[MTD] replace kmalloc+memset with kzalloc
[linux-2.6.git] / drivers / mtd / chips / cfi_cmdset_0020.c
1 /*
2  * Common Flash Interface support:
3  *   ST Advanced Architecture Command Set (ID 0x0020)
4  *
5  * (C) 2000 Red Hat. GPL'd
6  *
7  * $Id: cfi_cmdset_0020.c,v 1.22 2005/11/07 11:14:22 gleixner Exp $
8  *
9  * 10/10/2000   Nicolas Pitre <nico@cam.org>
10  *      - completely revamped method functions so they are aware and
11  *        independent of the flash geometry (buswidth, interleave, etc.)
12  *      - scalability vs code size is completely set at compile-time
13  *        (see include/linux/mtd/cfi.h for selection)
14  *      - optimized write buffer method
15  * 06/21/2002   Joern Engel <joern@wh.fh-wedel.de> and others
16  *      - modified Intel Command Set 0x0001 to support ST Advanced Architecture
17  *        (command set 0x0020)
18  *      - added a writev function
19  * 07/13/2005   Joern Engel <joern@wh.fh-wedel.de>
20  *      - Plugged memory leak in cfi_staa_writev().
21  */
22
23 #include <linux/module.h>
24 #include <linux/types.h>
25 #include <linux/kernel.h>
26 #include <linux/sched.h>
27 #include <linux/init.h>
28 #include <asm/io.h>
29 #include <asm/byteorder.h>
30
31 #include <linux/errno.h>
32 #include <linux/slab.h>
33 #include <linux/delay.h>
34 #include <linux/interrupt.h>
35 #include <linux/mtd/map.h>
36 #include <linux/mtd/cfi.h>
37 #include <linux/mtd/mtd.h>
38 #include <linux/mtd/compatmac.h>
39
40
41 static int cfi_staa_read(struct mtd_info *, loff_t, size_t, size_t *, u_char *);
42 static int cfi_staa_write_buffers(struct mtd_info *, loff_t, size_t, size_t *, const u_char *);
43 static int cfi_staa_writev(struct mtd_info *mtd, const struct kvec *vecs,
44                 unsigned long count, loff_t to, size_t *retlen);
45 static int cfi_staa_erase_varsize(struct mtd_info *, struct erase_info *);
46 static void cfi_staa_sync (struct mtd_info *);
47 static int cfi_staa_lock(struct mtd_info *mtd, loff_t ofs, size_t len);
48 static int cfi_staa_unlock(struct mtd_info *mtd, loff_t ofs, size_t len);
49 static int cfi_staa_suspend (struct mtd_info *);
50 static void cfi_staa_resume (struct mtd_info *);
51
52 static void cfi_staa_destroy(struct mtd_info *);
53
54 struct mtd_info *cfi_cmdset_0020(struct map_info *, int);
55
56 static struct mtd_info *cfi_staa_setup (struct map_info *);
57
58 static struct mtd_chip_driver cfi_staa_chipdrv = {
59         .probe          = NULL, /* Not usable directly */
60         .destroy        = cfi_staa_destroy,
61         .name           = "cfi_cmdset_0020",
62         .module         = THIS_MODULE
63 };
64
65 /* #define DEBUG_LOCK_BITS */
66 //#define DEBUG_CFI_FEATURES
67
68 #ifdef DEBUG_CFI_FEATURES
69 static void cfi_tell_features(struct cfi_pri_intelext *extp)
70 {
71         int i;
72         printk("  Feature/Command Support: %4.4X\n", extp->FeatureSupport);
73         printk("     - Chip Erase:         %s\n", extp->FeatureSupport&1?"supported":"unsupported");
74         printk("     - Suspend Erase:      %s\n", extp->FeatureSupport&2?"supported":"unsupported");
75         printk("     - Suspend Program:    %s\n", extp->FeatureSupport&4?"supported":"unsupported");
76         printk("     - Legacy Lock/Unlock: %s\n", extp->FeatureSupport&8?"supported":"unsupported");
77         printk("     - Queued Erase:       %s\n", extp->FeatureSupport&16?"supported":"unsupported");
78         printk("     - Instant block lock: %s\n", extp->FeatureSupport&32?"supported":"unsupported");
79         printk("     - Protection Bits:    %s\n", extp->FeatureSupport&64?"supported":"unsupported");
80         printk("     - Page-mode read:     %s\n", extp->FeatureSupport&128?"supported":"unsupported");
81         printk("     - Synchronous read:   %s\n", extp->FeatureSupport&256?"supported":"unsupported");
82         for (i=9; i<32; i++) {
83                 if (extp->FeatureSupport & (1<<i))
84                         printk("     - Unknown Bit %X:      supported\n", i);
85         }
86
87         printk("  Supported functions after Suspend: %2.2X\n", extp->SuspendCmdSupport);
88         printk("     - Program after Erase Suspend: %s\n", extp->SuspendCmdSupport&1?"supported":"unsupported");
89         for (i=1; i<8; i++) {
90                 if (extp->SuspendCmdSupport & (1<<i))
91                         printk("     - Unknown Bit %X:               supported\n", i);
92         }
93
94         printk("  Block Status Register Mask: %4.4X\n", extp->BlkStatusRegMask);
95         printk("     - Lock Bit Active:      %s\n", extp->BlkStatusRegMask&1?"yes":"no");
96         printk("     - Valid Bit Active:     %s\n", extp->BlkStatusRegMask&2?"yes":"no");
97         for (i=2; i<16; i++) {
98                 if (extp->BlkStatusRegMask & (1<<i))
99                         printk("     - Unknown Bit %X Active: yes\n",i);
100         }
101
102         printk("  Vcc Logic Supply Optimum Program/Erase Voltage: %d.%d V\n",
103                extp->VccOptimal >> 8, extp->VccOptimal & 0xf);
104         if (extp->VppOptimal)
105                 printk("  Vpp Programming Supply Optimum Program/Erase Voltage: %d.%d V\n",
106                        extp->VppOptimal >> 8, extp->VppOptimal & 0xf);
107 }
108 #endif
109
110 /* This routine is made available to other mtd code via
111  * inter_module_register.  It must only be accessed through
112  * inter_module_get which will bump the use count of this module.  The
113  * addresses passed back in cfi are valid as long as the use count of
114  * this module is non-zero, i.e. between inter_module_get and
115  * inter_module_put.  Keith Owens <kaos@ocs.com.au> 29 Oct 2000.
116  */
117 struct mtd_info *cfi_cmdset_0020(struct map_info *map, int primary)
118 {
119         struct cfi_private *cfi = map->fldrv_priv;
120         int i;
121
122         if (cfi->cfi_mode) {
123                 /*
124                  * It's a real CFI chip, not one for which the probe
125                  * routine faked a CFI structure. So we read the feature
126                  * table from it.
127                  */
128                 __u16 adr = primary?cfi->cfiq->P_ADR:cfi->cfiq->A_ADR;
129                 struct cfi_pri_intelext *extp;
130
131                 extp = (struct cfi_pri_intelext*)cfi_read_pri(map, adr, sizeof(*extp), "ST Microelectronics");
132                 if (!extp)
133                         return NULL;
134
135                 if (extp->MajorVersion != '1' ||
136                     (extp->MinorVersion < '0' || extp->MinorVersion > '3')) {
137                         printk(KERN_ERR "  Unknown ST Microelectronics"
138                                " Extended Query version %c.%c.\n",
139                                extp->MajorVersion, extp->MinorVersion);
140                         kfree(extp);
141                         return NULL;
142                 }
143
144                 /* Do some byteswapping if necessary */
145                 extp->FeatureSupport = cfi32_to_cpu(extp->FeatureSupport);
146                 extp->BlkStatusRegMask = cfi32_to_cpu(extp->BlkStatusRegMask);
147
148 #ifdef DEBUG_CFI_FEATURES
149                 /* Tell the user about it in lots of lovely detail */
150                 cfi_tell_features(extp);
151 #endif
152
153                 /* Install our own private info structure */
154                 cfi->cmdset_priv = extp;
155         }
156
157         for (i=0; i< cfi->numchips; i++) {
158                 cfi->chips[i].word_write_time = 128;
159                 cfi->chips[i].buffer_write_time = 128;
160                 cfi->chips[i].erase_time = 1024;
161         }
162
163         return cfi_staa_setup(map);
164 }
165 EXPORT_SYMBOL_GPL(cfi_cmdset_0020);
166
167 static struct mtd_info *cfi_staa_setup(struct map_info *map)
168 {
169         struct cfi_private *cfi = map->fldrv_priv;
170         struct mtd_info *mtd;
171         unsigned long offset = 0;
172         int i,j;
173         unsigned long devsize = (1<<cfi->cfiq->DevSize) * cfi->interleave;
174
175         mtd = kzalloc(sizeof(*mtd), GFP_KERNEL);
176         //printk(KERN_DEBUG "number of CFI chips: %d\n", cfi->numchips);
177
178         if (!mtd) {
179                 printk(KERN_ERR "Failed to allocate memory for MTD device\n");
180                 kfree(cfi->cmdset_priv);
181                 return NULL;
182         }
183
184         mtd->priv = map;
185         mtd->type = MTD_NORFLASH;
186         mtd->size = devsize * cfi->numchips;
187
188         mtd->numeraseregions = cfi->cfiq->NumEraseRegions * cfi->numchips;
189         mtd->eraseregions = kmalloc(sizeof(struct mtd_erase_region_info)
190                         * mtd->numeraseregions, GFP_KERNEL);
191         if (!mtd->eraseregions) {
192                 printk(KERN_ERR "Failed to allocate memory for MTD erase region info\n");
193                 kfree(cfi->cmdset_priv);
194                 kfree(mtd);
195                 return NULL;
196         }
197
198         for (i=0; i<cfi->cfiq->NumEraseRegions; i++) {
199                 unsigned long ernum, ersize;
200                 ersize = ((cfi->cfiq->EraseRegionInfo[i] >> 8) & ~0xff) * cfi->interleave;
201                 ernum = (cfi->cfiq->EraseRegionInfo[i] & 0xffff) + 1;
202
203                 if (mtd->erasesize < ersize) {
204                         mtd->erasesize = ersize;
205                 }
206                 for (j=0; j<cfi->numchips; j++) {
207                         mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].offset = (j*devsize)+offset;
208                         mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].erasesize = ersize;
209                         mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].numblocks = ernum;
210                 }
211                 offset += (ersize * ernum);
212                 }
213
214                 if (offset != devsize) {
215                         /* Argh */
216                         printk(KERN_WARNING "Sum of regions (%lx) != total size of set of interleaved chips (%lx)\n", offset, devsize);
217                         kfree(mtd->eraseregions);
218                         kfree(cfi->cmdset_priv);
219                         kfree(mtd);
220                         return NULL;
221                 }
222
223                 for (i=0; i<mtd->numeraseregions;i++){
224                         printk(KERN_DEBUG "%d: offset=0x%x,size=0x%x,blocks=%d\n",
225                                i,mtd->eraseregions[i].offset,
226                                mtd->eraseregions[i].erasesize,
227                                mtd->eraseregions[i].numblocks);
228                 }
229
230         /* Also select the correct geometry setup too */
231         mtd->erase = cfi_staa_erase_varsize;
232         mtd->read = cfi_staa_read;
233         mtd->write = cfi_staa_write_buffers;
234         mtd->writev = cfi_staa_writev;
235         mtd->sync = cfi_staa_sync;
236         mtd->lock = cfi_staa_lock;
237         mtd->unlock = cfi_staa_unlock;
238         mtd->suspend = cfi_staa_suspend;
239         mtd->resume = cfi_staa_resume;
240         mtd->flags = MTD_CAP_NORFLASH & ~MTD_BIT_WRITEABLE;
241         mtd->writesize = 8; /* FIXME: Should be 0 for STMicro flashes w/out ECC */
242         map->fldrv = &cfi_staa_chipdrv;
243         __module_get(THIS_MODULE);
244         mtd->name = map->name;
245         return mtd;
246 }
247
248
249 static inline int do_read_onechip(struct map_info *map, struct flchip *chip, loff_t adr, size_t len, u_char *buf)
250 {
251         map_word status, status_OK;
252         unsigned long timeo;
253         DECLARE_WAITQUEUE(wait, current);
254         int suspended = 0;
255         unsigned long cmd_addr;
256         struct cfi_private *cfi = map->fldrv_priv;
257
258         adr += chip->start;
259
260         /* Ensure cmd read/writes are aligned. */
261         cmd_addr = adr & ~(map_bankwidth(map)-1);
262
263         /* Let's determine this according to the interleave only once */
264         status_OK = CMD(0x80);
265
266         timeo = jiffies + HZ;
267  retry:
268         spin_lock_bh(chip->mutex);
269
270         /* Check that the chip's ready to talk to us.
271          * If it's in FL_ERASING state, suspend it and make it talk now.
272          */
273         switch (chip->state) {
274         case FL_ERASING:
275                 if (!(((struct cfi_pri_intelext *)cfi->cmdset_priv)->FeatureSupport & 2))
276                         goto sleep; /* We don't support erase suspend */
277
278                 map_write (map, CMD(0xb0), cmd_addr);
279                 /* If the flash has finished erasing, then 'erase suspend'
280                  * appears to make some (28F320) flash devices switch to
281                  * 'read' mode.  Make sure that we switch to 'read status'
282                  * mode so we get the right data. --rmk
283                  */
284                 map_write(map, CMD(0x70), cmd_addr);
285                 chip->oldstate = FL_ERASING;
286                 chip->state = FL_ERASE_SUSPENDING;
287                 //              printk("Erase suspending at 0x%lx\n", cmd_addr);
288                 for (;;) {
289                         status = map_read(map, cmd_addr);
290                         if (map_word_andequal(map, status, status_OK, status_OK))
291                                 break;
292
293                         if (time_after(jiffies, timeo)) {
294                                 /* Urgh */
295                                 map_write(map, CMD(0xd0), cmd_addr);
296                                 /* make sure we're in 'read status' mode */
297                                 map_write(map, CMD(0x70), cmd_addr);
298                                 chip->state = FL_ERASING;
299                                 spin_unlock_bh(chip->mutex);
300                                 printk(KERN_ERR "Chip not ready after erase "
301                                        "suspended: status = 0x%lx\n", status.x[0]);
302                                 return -EIO;
303                         }
304
305                         spin_unlock_bh(chip->mutex);
306                         cfi_udelay(1);
307                         spin_lock_bh(chip->mutex);
308                 }
309
310                 suspended = 1;
311                 map_write(map, CMD(0xff), cmd_addr);
312                 chip->state = FL_READY;
313                 break;
314
315 #if 0
316         case FL_WRITING:
317                 /* Not quite yet */
318 #endif
319
320         case FL_READY:
321                 break;
322
323         case FL_CFI_QUERY:
324         case FL_JEDEC_QUERY:
325                 map_write(map, CMD(0x70), cmd_addr);
326                 chip->state = FL_STATUS;
327
328         case FL_STATUS:
329                 status = map_read(map, cmd_addr);
330                 if (map_word_andequal(map, status, status_OK, status_OK)) {
331                         map_write(map, CMD(0xff), cmd_addr);
332                         chip->state = FL_READY;
333                         break;
334                 }
335
336                 /* Urgh. Chip not yet ready to talk to us. */
337                 if (time_after(jiffies, timeo)) {
338                         spin_unlock_bh(chip->mutex);
339                         printk(KERN_ERR "waiting for chip to be ready timed out in read. WSM status = %lx\n", status.x[0]);
340                         return -EIO;
341                 }
342
343                 /* Latency issues. Drop the lock, wait a while and retry */
344                 spin_unlock_bh(chip->mutex);
345                 cfi_udelay(1);
346                 goto retry;
347
348         default:
349         sleep:
350                 /* Stick ourselves on a wait queue to be woken when
351                    someone changes the status */
352                 set_current_state(TASK_UNINTERRUPTIBLE);
353                 add_wait_queue(&chip->wq, &wait);
354                 spin_unlock_bh(chip->mutex);
355                 schedule();
356                 remove_wait_queue(&chip->wq, &wait);
357                 timeo = jiffies + HZ;
358                 goto retry;
359         }
360
361         map_copy_from(map, buf, adr, len);
362
363         if (suspended) {
364                 chip->state = chip->oldstate;
365                 /* What if one interleaved chip has finished and the
366                    other hasn't? The old code would leave the finished
367                    one in READY mode. That's bad, and caused -EROFS
368                    errors to be returned from do_erase_oneblock because
369                    that's the only bit it checked for at the time.
370                    As the state machine appears to explicitly allow
371                    sending the 0x70 (Read Status) command to an erasing
372                    chip and expecting it to be ignored, that's what we
373                    do. */
374                 map_write(map, CMD(0xd0), cmd_addr);
375                 map_write(map, CMD(0x70), cmd_addr);
376         }
377
378         wake_up(&chip->wq);
379         spin_unlock_bh(chip->mutex);
380         return 0;
381 }
382
383 static int cfi_staa_read (struct mtd_info *mtd, loff_t from, size_t len, size_t *retlen, u_char *buf)
384 {
385         struct map_info *map = mtd->priv;
386         struct cfi_private *cfi = map->fldrv_priv;
387         unsigned long ofs;
388         int chipnum;
389         int ret = 0;
390
391         /* ofs: offset within the first chip that the first read should start */
392         chipnum = (from >> cfi->chipshift);
393         ofs = from - (chipnum <<  cfi->chipshift);
394
395         *retlen = 0;
396
397         while (len) {
398                 unsigned long thislen;
399
400                 if (chipnum >= cfi->numchips)
401                         break;
402
403                 if ((len + ofs -1) >> cfi->chipshift)
404                         thislen = (1<<cfi->chipshift) - ofs;
405                 else
406                         thislen = len;
407
408                 ret = do_read_onechip(map, &cfi->chips[chipnum], ofs, thislen, buf);
409                 if (ret)
410                         break;
411
412                 *retlen += thislen;
413                 len -= thislen;
414                 buf += thislen;
415
416                 ofs = 0;
417                 chipnum++;
418         }
419         return ret;
420 }
421
422 static inline int do_write_buffer(struct map_info *map, struct flchip *chip,
423                                   unsigned long adr, const u_char *buf, int len)
424 {
425         struct cfi_private *cfi = map->fldrv_priv;
426         map_word status, status_OK;
427         unsigned long cmd_adr, timeo;
428         DECLARE_WAITQUEUE(wait, current);
429         int wbufsize, z;
430
431         /* M58LW064A requires bus alignment for buffer wriets -- saw */
432         if (adr & (map_bankwidth(map)-1))
433             return -EINVAL;
434
435         wbufsize = cfi_interleave(cfi) << cfi->cfiq->MaxBufWriteSize;
436         adr += chip->start;
437         cmd_adr = adr & ~(wbufsize-1);
438
439         /* Let's determine this according to the interleave only once */
440         status_OK = CMD(0x80);
441
442         timeo = jiffies + HZ;
443  retry:
444
445 #ifdef DEBUG_CFI_FEATURES
446        printk("%s: chip->state[%d]\n", __FUNCTION__, chip->state);
447 #endif
448         spin_lock_bh(chip->mutex);
449
450         /* Check that the chip's ready to talk to us.
451          * Later, we can actually think about interrupting it
452          * if it's in FL_ERASING state.
453          * Not just yet, though.
454          */
455         switch (chip->state) {
456         case FL_READY:
457                 break;
458
459         case FL_CFI_QUERY:
460         case FL_JEDEC_QUERY:
461                 map_write(map, CMD(0x70), cmd_adr);
462                 chip->state = FL_STATUS;
463 #ifdef DEBUG_CFI_FEATURES
464         printk("%s: 1 status[%x]\n", __FUNCTION__, map_read(map, cmd_adr));
465 #endif
466
467         case FL_STATUS:
468                 status = map_read(map, cmd_adr);
469                 if (map_word_andequal(map, status, status_OK, status_OK))
470                         break;
471                 /* Urgh. Chip not yet ready to talk to us. */
472                 if (time_after(jiffies, timeo)) {
473                         spin_unlock_bh(chip->mutex);
474                         printk(KERN_ERR "waiting for chip to be ready timed out in buffer write Xstatus = %lx, status = %lx\n",
475                                status.x[0], map_read(map, cmd_adr).x[0]);
476                         return -EIO;
477                 }
478
479                 /* Latency issues. Drop the lock, wait a while and retry */
480                 spin_unlock_bh(chip->mutex);
481                 cfi_udelay(1);
482                 goto retry;
483
484         default:
485                 /* Stick ourselves on a wait queue to be woken when
486                    someone changes the status */
487                 set_current_state(TASK_UNINTERRUPTIBLE);
488                 add_wait_queue(&chip->wq, &wait);
489                 spin_unlock_bh(chip->mutex);
490                 schedule();
491                 remove_wait_queue(&chip->wq, &wait);
492                 timeo = jiffies + HZ;
493                 goto retry;
494         }
495
496         ENABLE_VPP(map);
497         map_write(map, CMD(0xe8), cmd_adr);
498         chip->state = FL_WRITING_TO_BUFFER;
499
500         z = 0;
501         for (;;) {
502                 status = map_read(map, cmd_adr);
503                 if (map_word_andequal(map, status, status_OK, status_OK))
504                         break;
505
506                 spin_unlock_bh(chip->mutex);
507                 cfi_udelay(1);
508                 spin_lock_bh(chip->mutex);
509
510                 if (++z > 100) {
511                         /* Argh. Not ready for write to buffer */
512                         DISABLE_VPP(map);
513                         map_write(map, CMD(0x70), cmd_adr);
514                         chip->state = FL_STATUS;
515                         spin_unlock_bh(chip->mutex);
516                         printk(KERN_ERR "Chip not ready for buffer write. Xstatus = %lx\n", status.x[0]);
517                         return -EIO;
518                 }
519         }
520
521         /* Write length of data to come */
522         map_write(map, CMD(len/map_bankwidth(map)-1), cmd_adr );
523
524         /* Write data */
525         for (z = 0; z < len;
526              z += map_bankwidth(map), buf += map_bankwidth(map)) {
527                 map_word d;
528                 d = map_word_load(map, buf);
529                 map_write(map, d, adr+z);
530         }
531         /* GO GO GO */
532         map_write(map, CMD(0xd0), cmd_adr);
533         chip->state = FL_WRITING;
534
535         spin_unlock_bh(chip->mutex);
536         cfi_udelay(chip->buffer_write_time);
537         spin_lock_bh(chip->mutex);
538
539         timeo = jiffies + (HZ/2);
540         z = 0;
541         for (;;) {
542                 if (chip->state != FL_WRITING) {
543                         /* Someone's suspended the write. Sleep */
544                         set_current_state(TASK_UNINTERRUPTIBLE);
545                         add_wait_queue(&chip->wq, &wait);
546                         spin_unlock_bh(chip->mutex);
547                         schedule();
548                         remove_wait_queue(&chip->wq, &wait);
549                         timeo = jiffies + (HZ / 2); /* FIXME */
550                         spin_lock_bh(chip->mutex);
551                         continue;
552                 }
553
554                 status = map_read(map, cmd_adr);
555                 if (map_word_andequal(map, status, status_OK, status_OK))
556                         break;
557
558                 /* OK Still waiting */
559                 if (time_after(jiffies, timeo)) {
560                         /* clear status */
561                         map_write(map, CMD(0x50), cmd_adr);
562                         /* put back into read status register mode */
563                         map_write(map, CMD(0x70), adr);
564                         chip->state = FL_STATUS;
565                         DISABLE_VPP(map);
566                         spin_unlock_bh(chip->mutex);
567                         printk(KERN_ERR "waiting for chip to be ready timed out in bufwrite\n");
568                         return -EIO;
569                 }
570
571                 /* Latency issues. Drop the lock, wait a while and retry */
572                 spin_unlock_bh(chip->mutex);
573                 cfi_udelay(1);
574                 z++;
575                 spin_lock_bh(chip->mutex);
576         }
577         if (!z) {
578                 chip->buffer_write_time--;
579                 if (!chip->buffer_write_time)
580                         chip->buffer_write_time++;
581         }
582         if (z > 1)
583                 chip->buffer_write_time++;
584
585         /* Done and happy. */
586         DISABLE_VPP(map);
587         chip->state = FL_STATUS;
588
589         /* check for errors: 'lock bit', 'VPP', 'dead cell'/'unerased cell' or 'incorrect cmd' -- saw */
590         if (map_word_bitsset(map, status, CMD(0x3a))) {
591 #ifdef DEBUG_CFI_FEATURES
592                 printk("%s: 2 status[%lx]\n", __FUNCTION__, status.x[0]);
593 #endif
594                 /* clear status */
595                 map_write(map, CMD(0x50), cmd_adr);
596                 /* put back into read status register mode */
597                 map_write(map, CMD(0x70), adr);
598                 wake_up(&chip->wq);
599                 spin_unlock_bh(chip->mutex);
600                 return map_word_bitsset(map, status, CMD(0x02)) ? -EROFS : -EIO;
601         }
602         wake_up(&chip->wq);
603         spin_unlock_bh(chip->mutex);
604
605         return 0;
606 }
607
608 static int cfi_staa_write_buffers (struct mtd_info *mtd, loff_t to,
609                                        size_t len, size_t *retlen, const u_char *buf)
610 {
611         struct map_info *map = mtd->priv;
612         struct cfi_private *cfi = map->fldrv_priv;
613         int wbufsize = cfi_interleave(cfi) << cfi->cfiq->MaxBufWriteSize;
614         int ret = 0;
615         int chipnum;
616         unsigned long ofs;
617
618         *retlen = 0;
619         if (!len)
620                 return 0;
621
622         chipnum = to >> cfi->chipshift;
623         ofs = to  - (chipnum << cfi->chipshift);
624
625 #ifdef DEBUG_CFI_FEATURES
626         printk("%s: map_bankwidth(map)[%x]\n", __FUNCTION__, map_bankwidth(map));
627         printk("%s: chipnum[%x] wbufsize[%x]\n", __FUNCTION__, chipnum, wbufsize);
628         printk("%s: ofs[%x] len[%x]\n", __FUNCTION__, ofs, len);
629 #endif
630
631         /* Write buffer is worth it only if more than one word to write... */
632         while (len > 0) {
633                 /* We must not cross write block boundaries */
634                 int size = wbufsize - (ofs & (wbufsize-1));
635
636                 if (size > len)
637                     size = len;
638
639                 ret = do_write_buffer(map, &cfi->chips[chipnum],
640                                       ofs, buf, size);
641                 if (ret)
642                         return ret;
643
644                 ofs += size;
645                 buf += size;
646                 (*retlen) += size;
647                 len -= size;
648
649                 if (ofs >> cfi->chipshift) {
650                         chipnum ++;
651                         ofs = 0;
652                         if (chipnum == cfi->numchips)
653                                 return 0;
654                 }
655         }
656
657         return 0;
658 }
659
660 /*
661  * Writev for ECC-Flashes is a little more complicated. We need to maintain
662  * a small buffer for this.
663  * XXX: If the buffer size is not a multiple of 2, this will break
664  */
665 #define ECCBUF_SIZE (mtd->eccsize)
666 #define ECCBUF_DIV(x) ((x) & ~(ECCBUF_SIZE - 1))
667 #define ECCBUF_MOD(x) ((x) &  (ECCBUF_SIZE - 1))
668 static int
669 cfi_staa_writev(struct mtd_info *mtd, const struct kvec *vecs,
670                 unsigned long count, loff_t to, size_t *retlen)
671 {
672         unsigned long i;
673         size_t   totlen = 0, thislen;
674         int      ret = 0;
675         size_t   buflen = 0;
676         static char *buffer;
677
678         if (!ECCBUF_SIZE) {
679                 /* We should fall back to a general writev implementation.
680                  * Until that is written, just break.
681                  */
682                 return -EIO;
683         }
684         buffer = kmalloc(ECCBUF_SIZE, GFP_KERNEL);
685         if (!buffer)
686                 return -ENOMEM;
687
688         for (i=0; i<count; i++) {
689                 size_t elem_len = vecs[i].iov_len;
690                 void *elem_base = vecs[i].iov_base;
691                 if (!elem_len) /* FIXME: Might be unnecessary. Check that */
692                         continue;
693                 if (buflen) { /* cut off head */
694                         if (buflen + elem_len < ECCBUF_SIZE) { /* just accumulate */
695                                 memcpy(buffer+buflen, elem_base, elem_len);
696                                 buflen += elem_len;
697                                 continue;
698                         }
699                         memcpy(buffer+buflen, elem_base, ECCBUF_SIZE-buflen);
700                         ret = mtd->write(mtd, to, ECCBUF_SIZE, &thislen, buffer);
701                         totlen += thislen;
702                         if (ret || thislen != ECCBUF_SIZE)
703                                 goto write_error;
704                         elem_len -= thislen-buflen;
705                         elem_base += thislen-buflen;
706                         to += ECCBUF_SIZE;
707                 }
708                 if (ECCBUF_DIV(elem_len)) { /* write clean aligned data */
709                         ret = mtd->write(mtd, to, ECCBUF_DIV(elem_len), &thislen, elem_base);
710                         totlen += thislen;
711                         if (ret || thislen != ECCBUF_DIV(elem_len))
712                                 goto write_error;
713                         to += thislen;
714                 }
715                 buflen = ECCBUF_MOD(elem_len); /* cut off tail */
716                 if (buflen) {
717                         memset(buffer, 0xff, ECCBUF_SIZE);
718                         memcpy(buffer, elem_base + thislen, buflen);
719                 }
720         }
721         if (buflen) { /* flush last page, even if not full */
722                 /* This is sometimes intended behaviour, really */
723                 ret = mtd->write(mtd, to, buflen, &thislen, buffer);
724                 totlen += thislen;
725                 if (ret || thislen != ECCBUF_SIZE)
726                         goto write_error;
727         }
728 write_error:
729         if (retlen)
730                 *retlen = totlen;
731         kfree(buffer);
732         return ret;
733 }
734
735
736 static inline int do_erase_oneblock(struct map_info *map, struct flchip *chip, unsigned long adr)
737 {
738         struct cfi_private *cfi = map->fldrv_priv;
739         map_word status, status_OK;
740         unsigned long timeo;
741         int retries = 3;
742         DECLARE_WAITQUEUE(wait, current);
743         int ret = 0;
744
745         adr += chip->start;
746
747         /* Let's determine this according to the interleave only once */
748         status_OK = CMD(0x80);
749
750         timeo = jiffies + HZ;
751 retry:
752         spin_lock_bh(chip->mutex);
753
754         /* Check that the chip's ready to talk to us. */
755         switch (chip->state) {
756         case FL_CFI_QUERY:
757         case FL_JEDEC_QUERY:
758         case FL_READY:
759                 map_write(map, CMD(0x70), adr);
760                 chip->state = FL_STATUS;
761
762         case FL_STATUS:
763                 status = map_read(map, adr);
764                 if (map_word_andequal(map, status, status_OK, status_OK))
765                         break;
766
767                 /* Urgh. Chip not yet ready to talk to us. */
768                 if (time_after(jiffies, timeo)) {
769                         spin_unlock_bh(chip->mutex);
770                         printk(KERN_ERR "waiting for chip to be ready timed out in erase\n");
771                         return -EIO;
772                 }
773
774                 /* Latency issues. Drop the lock, wait a while and retry */
775                 spin_unlock_bh(chip->mutex);
776                 cfi_udelay(1);
777                 goto retry;
778
779         default:
780                 /* Stick ourselves on a wait queue to be woken when
781                    someone changes the status */
782                 set_current_state(TASK_UNINTERRUPTIBLE);
783                 add_wait_queue(&chip->wq, &wait);
784                 spin_unlock_bh(chip->mutex);
785                 schedule();
786                 remove_wait_queue(&chip->wq, &wait);
787                 timeo = jiffies + HZ;
788                 goto retry;
789         }
790
791         ENABLE_VPP(map);
792         /* Clear the status register first */
793         map_write(map, CMD(0x50), adr);
794
795         /* Now erase */
796         map_write(map, CMD(0x20), adr);
797         map_write(map, CMD(0xD0), adr);
798         chip->state = FL_ERASING;
799
800         spin_unlock_bh(chip->mutex);
801         msleep(1000);
802         spin_lock_bh(chip->mutex);
803
804         /* FIXME. Use a timer to check this, and return immediately. */
805         /* Once the state machine's known to be working I'll do that */
806
807         timeo = jiffies + (HZ*20);
808         for (;;) {
809                 if (chip->state != FL_ERASING) {
810                         /* Someone's suspended the erase. Sleep */
811                         set_current_state(TASK_UNINTERRUPTIBLE);
812                         add_wait_queue(&chip->wq, &wait);
813                         spin_unlock_bh(chip->mutex);
814                         schedule();
815                         remove_wait_queue(&chip->wq, &wait);
816                         timeo = jiffies + (HZ*20); /* FIXME */
817                         spin_lock_bh(chip->mutex);
818                         continue;
819                 }
820
821                 status = map_read(map, adr);
822                 if (map_word_andequal(map, status, status_OK, status_OK))
823                         break;
824
825                 /* OK Still waiting */
826                 if (time_after(jiffies, timeo)) {
827                         map_write(map, CMD(0x70), adr);
828                         chip->state = FL_STATUS;
829                         printk(KERN_ERR "waiting for erase to complete timed out. Xstatus = %lx, status = %lx.\n", status.x[0], map_read(map, adr).x[0]);
830                         DISABLE_VPP(map);
831                         spin_unlock_bh(chip->mutex);
832                         return -EIO;
833                 }
834
835                 /* Latency issues. Drop the lock, wait a while and retry */
836                 spin_unlock_bh(chip->mutex);
837                 cfi_udelay(1);
838                 spin_lock_bh(chip->mutex);
839         }
840
841         DISABLE_VPP(map);
842         ret = 0;
843
844         /* We've broken this before. It doesn't hurt to be safe */
845         map_write(map, CMD(0x70), adr);
846         chip->state = FL_STATUS;
847         status = map_read(map, adr);
848
849         /* check for lock bit */
850         if (map_word_bitsset(map, status, CMD(0x3a))) {
851                 unsigned char chipstatus = status.x[0];
852                 if (!map_word_equal(map, status, CMD(chipstatus))) {
853                         int i, w;
854                         for (w=0; w<map_words(map); w++) {
855                                 for (i = 0; i<cfi_interleave(cfi); i++) {
856                                         chipstatus |= status.x[w] >> (cfi->device_type * 8);
857                                 }
858                         }
859                         printk(KERN_WARNING "Status is not identical for all chips: 0x%lx. Merging to give 0x%02x\n",
860                                status.x[0], chipstatus);
861                 }
862                 /* Reset the error bits */
863                 map_write(map, CMD(0x50), adr);
864                 map_write(map, CMD(0x70), adr);
865
866                 if ((chipstatus & 0x30) == 0x30) {
867                         printk(KERN_NOTICE "Chip reports improper command sequence: status 0x%x\n", chipstatus);
868                         ret = -EIO;
869                 } else if (chipstatus & 0x02) {
870                         /* Protection bit set */
871                         ret = -EROFS;
872                 } else if (chipstatus & 0x8) {
873                         /* Voltage */
874                         printk(KERN_WARNING "Chip reports voltage low on erase: status 0x%x\n", chipstatus);
875                         ret = -EIO;
876                 } else if (chipstatus & 0x20) {
877                         if (retries--) {
878                                 printk(KERN_DEBUG "Chip erase failed at 0x%08lx: status 0x%x. Retrying...\n", adr, chipstatus);
879                                 timeo = jiffies + HZ;
880                                 chip->state = FL_STATUS;
881                                 spin_unlock_bh(chip->mutex);
882                                 goto retry;
883                         }
884                         printk(KERN_DEBUG "Chip erase failed at 0x%08lx: status 0x%x\n", adr, chipstatus);
885                         ret = -EIO;
886                 }
887         }
888
889         wake_up(&chip->wq);
890         spin_unlock_bh(chip->mutex);
891         return ret;
892 }
893
894 int cfi_staa_erase_varsize(struct mtd_info *mtd, struct erase_info *instr)
895 {       struct map_info *map = mtd->priv;
896         struct cfi_private *cfi = map->fldrv_priv;
897         unsigned long adr, len;
898         int chipnum, ret = 0;
899         int i, first;
900         struct mtd_erase_region_info *regions = mtd->eraseregions;
901
902         if (instr->addr > mtd->size)
903                 return -EINVAL;
904
905         if ((instr->len + instr->addr) > mtd->size)
906                 return -EINVAL;
907
908         /* Check that both start and end of the requested erase are
909          * aligned with the erasesize at the appropriate addresses.
910          */
911
912         i = 0;
913
914         /* Skip all erase regions which are ended before the start of
915            the requested erase. Actually, to save on the calculations,
916            we skip to the first erase region which starts after the
917            start of the requested erase, and then go back one.
918         */
919
920         while (i < mtd->numeraseregions && instr->addr >= regions[i].offset)
921                i++;
922         i--;
923
924         /* OK, now i is pointing at the erase region in which this
925            erase request starts. Check the start of the requested
926            erase range is aligned with the erase size which is in
927            effect here.
928         */
929
930         if (instr->addr & (regions[i].erasesize-1))
931                 return -EINVAL;
932
933         /* Remember the erase region we start on */
934         first = i;
935
936         /* Next, check that the end of the requested erase is aligned
937          * with the erase region at that address.
938          */
939
940         while (i<mtd->numeraseregions && (instr->addr + instr->len) >= regions[i].offset)
941                 i++;
942
943         /* As before, drop back one to point at the region in which
944            the address actually falls
945         */
946         i--;
947
948         if ((instr->addr + instr->len) & (regions[i].erasesize-1))
949                 return -EINVAL;
950
951         chipnum = instr->addr >> cfi->chipshift;
952         adr = instr->addr - (chipnum << cfi->chipshift);
953         len = instr->len;
954
955         i=first;
956
957         while(len) {
958                 ret = do_erase_oneblock(map, &cfi->chips[chipnum], adr);
959
960                 if (ret)
961                         return ret;
962
963                 adr += regions[i].erasesize;
964                 len -= regions[i].erasesize;
965
966                 if (adr % (1<< cfi->chipshift) == ((regions[i].offset + (regions[i].erasesize * regions[i].numblocks)) %( 1<< cfi->chipshift)))
967                         i++;
968
969                 if (adr >> cfi->chipshift) {
970                         adr = 0;
971                         chipnum++;
972
973                         if (chipnum >= cfi->numchips)
974                         break;
975                 }
976         }
977
978         instr->state = MTD_ERASE_DONE;
979         mtd_erase_callback(instr);
980
981         return 0;
982 }
983
984 static void cfi_staa_sync (struct mtd_info *mtd)
985 {
986         struct map_info *map = mtd->priv;
987         struct cfi_private *cfi = map->fldrv_priv;
988         int i;
989         struct flchip *chip;
990         int ret = 0;
991         DECLARE_WAITQUEUE(wait, current);
992
993         for (i=0; !ret && i<cfi->numchips; i++) {
994                 chip = &cfi->chips[i];
995
996         retry:
997                 spin_lock_bh(chip->mutex);
998
999                 switch(chip->state) {
1000                 case FL_READY:
1001                 case FL_STATUS:
1002                 case FL_CFI_QUERY:
1003                 case FL_JEDEC_QUERY:
1004                         chip->oldstate = chip->state;
1005                         chip->state = FL_SYNCING;
1006                         /* No need to wake_up() on this state change -
1007                          * as the whole point is that nobody can do anything
1008                          * with the chip now anyway.
1009                          */
1010                 case FL_SYNCING:
1011                         spin_unlock_bh(chip->mutex);
1012                         break;
1013
1014                 default:
1015                         /* Not an idle state */
1016                         add_wait_queue(&chip->wq, &wait);
1017
1018                         spin_unlock_bh(chip->mutex);
1019                         schedule();
1020                         remove_wait_queue(&chip->wq, &wait);
1021
1022                         goto retry;
1023                 }
1024         }
1025
1026         /* Unlock the chips again */
1027
1028         for (i--; i >=0; i--) {
1029                 chip = &cfi->chips[i];
1030
1031                 spin_lock_bh(chip->mutex);
1032
1033                 if (chip->state == FL_SYNCING) {
1034                         chip->state = chip->oldstate;
1035                         wake_up(&chip->wq);
1036                 }
1037                 spin_unlock_bh(chip->mutex);
1038         }
1039 }
1040
1041 static inline int do_lock_oneblock(struct map_info *map, struct flchip *chip, unsigned long adr)
1042 {
1043         struct cfi_private *cfi = map->fldrv_priv;
1044         map_word status, status_OK;
1045         unsigned long timeo = jiffies + HZ;
1046         DECLARE_WAITQUEUE(wait, current);
1047
1048         adr += chip->start;
1049
1050         /* Let's determine this according to the interleave only once */
1051         status_OK = CMD(0x80);
1052
1053         timeo = jiffies + HZ;
1054 retry:
1055         spin_lock_bh(chip->mutex);
1056
1057         /* Check that the chip's ready to talk to us. */
1058         switch (chip->state) {
1059         case FL_CFI_QUERY:
1060         case FL_JEDEC_QUERY:
1061         case FL_READY:
1062                 map_write(map, CMD(0x70), adr);
1063                 chip->state = FL_STATUS;
1064
1065         case FL_STATUS:
1066                 status = map_read(map, adr);
1067                 if (map_word_andequal(map, status, status_OK, status_OK))
1068                         break;
1069
1070                 /* Urgh. Chip not yet ready to talk to us. */
1071                 if (time_after(jiffies, timeo)) {
1072                         spin_unlock_bh(chip->mutex);
1073                         printk(KERN_ERR "waiting for chip to be ready timed out in lock\n");
1074                         return -EIO;
1075                 }
1076
1077                 /* Latency issues. Drop the lock, wait a while and retry */
1078                 spin_unlock_bh(chip->mutex);
1079                 cfi_udelay(1);
1080                 goto retry;
1081
1082         default:
1083                 /* Stick ourselves on a wait queue to be woken when
1084                    someone changes the status */
1085                 set_current_state(TASK_UNINTERRUPTIBLE);
1086                 add_wait_queue(&chip->wq, &wait);
1087                 spin_unlock_bh(chip->mutex);
1088                 schedule();
1089                 remove_wait_queue(&chip->wq, &wait);
1090                 timeo = jiffies + HZ;
1091                 goto retry;
1092         }
1093
1094         ENABLE_VPP(map);
1095         map_write(map, CMD(0x60), adr);
1096         map_write(map, CMD(0x01), adr);
1097         chip->state = FL_LOCKING;
1098
1099         spin_unlock_bh(chip->mutex);
1100         msleep(1000);
1101         spin_lock_bh(chip->mutex);
1102
1103         /* FIXME. Use a timer to check this, and return immediately. */
1104         /* Once the state machine's known to be working I'll do that */
1105
1106         timeo = jiffies + (HZ*2);
1107         for (;;) {
1108
1109                 status = map_read(map, adr);
1110                 if (map_word_andequal(map, status, status_OK, status_OK))
1111                         break;
1112
1113                 /* OK Still waiting */
1114                 if (time_after(jiffies, timeo)) {
1115                         map_write(map, CMD(0x70), adr);
1116                         chip->state = FL_STATUS;
1117                         printk(KERN_ERR "waiting for lock to complete timed out. Xstatus = %lx, status = %lx.\n", status.x[0], map_read(map, adr).x[0]);
1118                         DISABLE_VPP(map);
1119                         spin_unlock_bh(chip->mutex);
1120                         return -EIO;
1121                 }
1122
1123                 /* Latency issues. Drop the lock, wait a while and retry */
1124                 spin_unlock_bh(chip->mutex);
1125                 cfi_udelay(1);
1126                 spin_lock_bh(chip->mutex);
1127         }
1128
1129         /* Done and happy. */
1130         chip->state = FL_STATUS;
1131         DISABLE_VPP(map);
1132         wake_up(&chip->wq);
1133         spin_unlock_bh(chip->mutex);
1134         return 0;
1135 }
1136 static int cfi_staa_lock(struct mtd_info *mtd, loff_t ofs, size_t len)
1137 {
1138         struct map_info *map = mtd->priv;
1139         struct cfi_private *cfi = map->fldrv_priv;
1140         unsigned long adr;
1141         int chipnum, ret = 0;
1142 #ifdef DEBUG_LOCK_BITS
1143         int ofs_factor = cfi->interleave * cfi->device_type;
1144 #endif
1145
1146         if (ofs & (mtd->erasesize - 1))
1147                 return -EINVAL;
1148
1149         if (len & (mtd->erasesize -1))
1150                 return -EINVAL;
1151
1152         if ((len + ofs) > mtd->size)
1153                 return -EINVAL;
1154
1155         chipnum = ofs >> cfi->chipshift;
1156         adr = ofs - (chipnum << cfi->chipshift);
1157
1158         while(len) {
1159
1160 #ifdef DEBUG_LOCK_BITS
1161                 cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1162                 printk("before lock: block status register is %x\n",cfi_read_query(map, adr+(2*ofs_factor)));
1163                 cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1164 #endif
1165
1166                 ret = do_lock_oneblock(map, &cfi->chips[chipnum], adr);
1167
1168 #ifdef DEBUG_LOCK_BITS
1169                 cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1170                 printk("after lock: block status register is %x\n",cfi_read_query(map, adr+(2*ofs_factor)));
1171                 cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1172 #endif
1173
1174                 if (ret)
1175                         return ret;
1176
1177                 adr += mtd->erasesize;
1178                 len -= mtd->erasesize;
1179
1180                 if (adr >> cfi->chipshift) {
1181                         adr = 0;
1182                         chipnum++;
1183
1184                         if (chipnum >= cfi->numchips)
1185                         break;
1186                 }
1187         }
1188         return 0;
1189 }
1190 static inline int do_unlock_oneblock(struct map_info *map, struct flchip *chip, unsigned long adr)
1191 {
1192         struct cfi_private *cfi = map->fldrv_priv;
1193         map_word status, status_OK;
1194         unsigned long timeo = jiffies + HZ;
1195         DECLARE_WAITQUEUE(wait, current);
1196
1197         adr += chip->start;
1198
1199         /* Let's determine this according to the interleave only once */
1200         status_OK = CMD(0x80);
1201
1202         timeo = jiffies + HZ;
1203 retry:
1204         spin_lock_bh(chip->mutex);
1205
1206         /* Check that the chip's ready to talk to us. */
1207         switch (chip->state) {
1208         case FL_CFI_QUERY:
1209         case FL_JEDEC_QUERY:
1210         case FL_READY:
1211                 map_write(map, CMD(0x70), adr);
1212                 chip->state = FL_STATUS;
1213
1214         case FL_STATUS:
1215                 status = map_read(map, adr);
1216                 if (map_word_andequal(map, status, status_OK, status_OK))
1217                         break;
1218
1219                 /* Urgh. Chip not yet ready to talk to us. */
1220                 if (time_after(jiffies, timeo)) {
1221                         spin_unlock_bh(chip->mutex);
1222                         printk(KERN_ERR "waiting for chip to be ready timed out in unlock\n");
1223                         return -EIO;
1224                 }
1225
1226                 /* Latency issues. Drop the lock, wait a while and retry */
1227                 spin_unlock_bh(chip->mutex);
1228                 cfi_udelay(1);
1229                 goto retry;
1230
1231         default:
1232                 /* Stick ourselves on a wait queue to be woken when
1233                    someone changes the status */
1234                 set_current_state(TASK_UNINTERRUPTIBLE);
1235                 add_wait_queue(&chip->wq, &wait);
1236                 spin_unlock_bh(chip->mutex);
1237                 schedule();
1238                 remove_wait_queue(&chip->wq, &wait);
1239                 timeo = jiffies + HZ;
1240                 goto retry;
1241         }
1242
1243         ENABLE_VPP(map);
1244         map_write(map, CMD(0x60), adr);
1245         map_write(map, CMD(0xD0), adr);
1246         chip->state = FL_UNLOCKING;
1247
1248         spin_unlock_bh(chip->mutex);
1249         msleep(1000);
1250         spin_lock_bh(chip->mutex);
1251
1252         /* FIXME. Use a timer to check this, and return immediately. */
1253         /* Once the state machine's known to be working I'll do that */
1254
1255         timeo = jiffies + (HZ*2);
1256         for (;;) {
1257
1258                 status = map_read(map, adr);
1259                 if (map_word_andequal(map, status, status_OK, status_OK))
1260                         break;
1261
1262                 /* OK Still waiting */
1263                 if (time_after(jiffies, timeo)) {
1264                         map_write(map, CMD(0x70), adr);
1265                         chip->state = FL_STATUS;
1266                         printk(KERN_ERR "waiting for unlock to complete timed out. Xstatus = %lx, status = %lx.\n", status.x[0], map_read(map, adr).x[0]);
1267                         DISABLE_VPP(map);
1268                         spin_unlock_bh(chip->mutex);
1269                         return -EIO;
1270                 }
1271
1272                 /* Latency issues. Drop the unlock, wait a while and retry */
1273                 spin_unlock_bh(chip->mutex);
1274                 cfi_udelay(1);
1275                 spin_lock_bh(chip->mutex);
1276         }
1277
1278         /* Done and happy. */
1279         chip->state = FL_STATUS;
1280         DISABLE_VPP(map);
1281         wake_up(&chip->wq);
1282         spin_unlock_bh(chip->mutex);
1283         return 0;
1284 }
1285 static int cfi_staa_unlock(struct mtd_info *mtd, loff_t ofs, size_t len)
1286 {
1287         struct map_info *map = mtd->priv;
1288         struct cfi_private *cfi = map->fldrv_priv;
1289         unsigned long adr;
1290         int chipnum, ret = 0;
1291 #ifdef DEBUG_LOCK_BITS
1292         int ofs_factor = cfi->interleave * cfi->device_type;
1293 #endif
1294
1295         chipnum = ofs >> cfi->chipshift;
1296         adr = ofs - (chipnum << cfi->chipshift);
1297
1298 #ifdef DEBUG_LOCK_BITS
1299         {
1300                 unsigned long temp_adr = adr;
1301                 unsigned long temp_len = len;
1302
1303                 cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1304                 while (temp_len) {
1305                         printk("before unlock %x: block status register is %x\n",temp_adr,cfi_read_query(map, temp_adr+(2*ofs_factor)));
1306                         temp_adr += mtd->erasesize;
1307                         temp_len -= mtd->erasesize;
1308                 }
1309                 cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1310         }
1311 #endif
1312
1313         ret = do_unlock_oneblock(map, &cfi->chips[chipnum], adr);
1314
1315 #ifdef DEBUG_LOCK_BITS
1316         cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1317         printk("after unlock: block status register is %x\n",cfi_read_query(map, adr+(2*ofs_factor)));
1318         cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1319 #endif
1320
1321         return ret;
1322 }
1323
1324 static int cfi_staa_suspend(struct mtd_info *mtd)
1325 {
1326         struct map_info *map = mtd->priv;
1327         struct cfi_private *cfi = map->fldrv_priv;
1328         int i;
1329         struct flchip *chip;
1330         int ret = 0;
1331
1332         for (i=0; !ret && i<cfi->numchips; i++) {
1333                 chip = &cfi->chips[i];
1334
1335                 spin_lock_bh(chip->mutex);
1336
1337                 switch(chip->state) {
1338                 case FL_READY:
1339                 case FL_STATUS:
1340                 case FL_CFI_QUERY:
1341                 case FL_JEDEC_QUERY:
1342                         chip->oldstate = chip->state;
1343                         chip->state = FL_PM_SUSPENDED;
1344                         /* No need to wake_up() on this state change -
1345                          * as the whole point is that nobody can do anything
1346                          * with the chip now anyway.
1347                          */
1348                 case FL_PM_SUSPENDED:
1349                         break;
1350
1351                 default:
1352                         ret = -EAGAIN;
1353                         break;
1354                 }
1355                 spin_unlock_bh(chip->mutex);
1356         }
1357
1358         /* Unlock the chips again */
1359
1360         if (ret) {
1361                 for (i--; i >=0; i--) {
1362                         chip = &cfi->chips[i];
1363
1364                         spin_lock_bh(chip->mutex);
1365
1366                         if (chip->state == FL_PM_SUSPENDED) {
1367                                 /* No need to force it into a known state here,
1368                                    because we're returning failure, and it didn't
1369                                    get power cycled */
1370                                 chip->state = chip->oldstate;
1371                                 wake_up(&chip->wq);
1372                         }
1373                         spin_unlock_bh(chip->mutex);
1374                 }
1375         }
1376
1377         return ret;
1378 }
1379
1380 static void cfi_staa_resume(struct mtd_info *mtd)
1381 {
1382         struct map_info *map = mtd->priv;
1383         struct cfi_private *cfi = map->fldrv_priv;
1384         int i;
1385         struct flchip *chip;
1386
1387         for (i=0; i<cfi->numchips; i++) {
1388
1389                 chip = &cfi->chips[i];
1390
1391                 spin_lock_bh(chip->mutex);
1392
1393                 /* Go to known state. Chip may have been power cycled */
1394                 if (chip->state == FL_PM_SUSPENDED) {
1395                         map_write(map, CMD(0xFF), 0);
1396                         chip->state = FL_READY;
1397                         wake_up(&chip->wq);
1398                 }
1399
1400                 spin_unlock_bh(chip->mutex);
1401         }
1402 }
1403
1404 static void cfi_staa_destroy(struct mtd_info *mtd)
1405 {
1406         struct map_info *map = mtd->priv;
1407         struct cfi_private *cfi = map->fldrv_priv;
1408         kfree(cfi->cmdset_priv);
1409         kfree(cfi);
1410 }
1411
1412 MODULE_LICENSE("GPL");