Merge branch 'linus' into core/generic-dma-coherent
[linux-2.6] / 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  * 10/10/2000   Nicolas Pitre <nico@cam.org>
8  *      - completely revamped method functions so they are aware and
9  *        independent of the flash geometry (buswidth, interleave, etc.)
10  *      - scalability vs code size is completely set at compile-time
11  *        (see include/linux/mtd/cfi.h for selection)
12  *      - optimized write buffer method
13  * 06/21/2002   Joern Engel <joern@wh.fh-wedel.de> and others
14  *      - modified Intel Command Set 0x0001 to support ST Advanced Architecture
15  *        (command set 0x0020)
16  *      - added a writev function
17  * 07/13/2005   Joern Engel <joern@wh.fh-wedel.de>
18  *      - Plugged memory leak in cfi_staa_writev().
19  */
20
21 #include <linux/module.h>
22 #include <linux/types.h>
23 #include <linux/kernel.h>
24 #include <linux/sched.h>
25 #include <linux/init.h>
26 #include <asm/io.h>
27 #include <asm/byteorder.h>
28
29 #include <linux/errno.h>
30 #include <linux/slab.h>
31 #include <linux/delay.h>
32 #include <linux/interrupt.h>
33 #include <linux/mtd/map.h>
34 #include <linux/mtd/cfi.h>
35 #include <linux/mtd/mtd.h>
36 #include <linux/mtd/compatmac.h>
37
38
39 static int cfi_staa_read(struct mtd_info *, loff_t, size_t, size_t *, u_char *);
40 static int cfi_staa_write_buffers(struct mtd_info *, loff_t, size_t, size_t *, const u_char *);
41 static int cfi_staa_writev(struct mtd_info *mtd, const struct kvec *vecs,
42                 unsigned long count, loff_t to, size_t *retlen);
43 static int cfi_staa_erase_varsize(struct mtd_info *, struct erase_info *);
44 static void cfi_staa_sync (struct mtd_info *);
45 static int cfi_staa_lock(struct mtd_info *mtd, loff_t ofs, size_t len);
46 static int cfi_staa_unlock(struct mtd_info *mtd, loff_t ofs, size_t len);
47 static int cfi_staa_suspend (struct mtd_info *);
48 static void cfi_staa_resume (struct mtd_info *);
49
50 static void cfi_staa_destroy(struct mtd_info *);
51
52 struct mtd_info *cfi_cmdset_0020(struct map_info *, int);
53
54 static struct mtd_info *cfi_staa_setup (struct map_info *);
55
56 static struct mtd_chip_driver cfi_staa_chipdrv = {
57         .probe          = NULL, /* Not usable directly */
58         .destroy        = cfi_staa_destroy,
59         .name           = "cfi_cmdset_0020",
60         .module         = THIS_MODULE
61 };
62
63 /* #define DEBUG_LOCK_BITS */
64 //#define DEBUG_CFI_FEATURES
65
66 #ifdef DEBUG_CFI_FEATURES
67 static void cfi_tell_features(struct cfi_pri_intelext *extp)
68 {
69         int i;
70         printk("  Feature/Command Support: %4.4X\n", extp->FeatureSupport);
71         printk("     - Chip Erase:         %s\n", extp->FeatureSupport&1?"supported":"unsupported");
72         printk("     - Suspend Erase:      %s\n", extp->FeatureSupport&2?"supported":"unsupported");
73         printk("     - Suspend Program:    %s\n", extp->FeatureSupport&4?"supported":"unsupported");
74         printk("     - Legacy Lock/Unlock: %s\n", extp->FeatureSupport&8?"supported":"unsupported");
75         printk("     - Queued Erase:       %s\n", extp->FeatureSupport&16?"supported":"unsupported");
76         printk("     - Instant block lock: %s\n", extp->FeatureSupport&32?"supported":"unsupported");
77         printk("     - Protection Bits:    %s\n", extp->FeatureSupport&64?"supported":"unsupported");
78         printk("     - Page-mode read:     %s\n", extp->FeatureSupport&128?"supported":"unsupported");
79         printk("     - Synchronous read:   %s\n", extp->FeatureSupport&256?"supported":"unsupported");
80         for (i=9; i<32; i++) {
81                 if (extp->FeatureSupport & (1<<i))
82                         printk("     - Unknown Bit %X:      supported\n", i);
83         }
84
85         printk("  Supported functions after Suspend: %2.2X\n", extp->SuspendCmdSupport);
86         printk("     - Program after Erase Suspend: %s\n", extp->SuspendCmdSupport&1?"supported":"unsupported");
87         for (i=1; i<8; i++) {
88                 if (extp->SuspendCmdSupport & (1<<i))
89                         printk("     - Unknown Bit %X:               supported\n", i);
90         }
91
92         printk("  Block Status Register Mask: %4.4X\n", extp->BlkStatusRegMask);
93         printk("     - Lock Bit Active:      %s\n", extp->BlkStatusRegMask&1?"yes":"no");
94         printk("     - Valid Bit Active:     %s\n", extp->BlkStatusRegMask&2?"yes":"no");
95         for (i=2; i<16; i++) {
96                 if (extp->BlkStatusRegMask & (1<<i))
97                         printk("     - Unknown Bit %X Active: yes\n",i);
98         }
99
100         printk("  Vcc Logic Supply Optimum Program/Erase Voltage: %d.%d V\n",
101                extp->VccOptimal >> 8, extp->VccOptimal & 0xf);
102         if (extp->VppOptimal)
103                 printk("  Vpp Programming Supply Optimum Program/Erase Voltage: %d.%d V\n",
104                        extp->VppOptimal >> 8, extp->VppOptimal & 0xf);
105 }
106 #endif
107
108 /* This routine is made available to other mtd code via
109  * inter_module_register.  It must only be accessed through
110  * inter_module_get which will bump the use count of this module.  The
111  * addresses passed back in cfi are valid as long as the use count of
112  * this module is non-zero, i.e. between inter_module_get and
113  * inter_module_put.  Keith Owens <kaos@ocs.com.au> 29 Oct 2000.
114  */
115 struct mtd_info *cfi_cmdset_0020(struct map_info *map, int primary)
116 {
117         struct cfi_private *cfi = map->fldrv_priv;
118         int i;
119
120         if (cfi->cfi_mode) {
121                 /*
122                  * It's a real CFI chip, not one for which the probe
123                  * routine faked a CFI structure. So we read the feature
124                  * table from it.
125                  */
126                 __u16 adr = primary?cfi->cfiq->P_ADR:cfi->cfiq->A_ADR;
127                 struct cfi_pri_intelext *extp;
128
129                 extp = (struct cfi_pri_intelext*)cfi_read_pri(map, adr, sizeof(*extp), "ST Microelectronics");
130                 if (!extp)
131                         return NULL;
132
133                 if (extp->MajorVersion != '1' ||
134                     (extp->MinorVersion < '0' || extp->MinorVersion > '3')) {
135                         printk(KERN_ERR "  Unknown ST Microelectronics"
136                                " Extended Query version %c.%c.\n",
137                                extp->MajorVersion, extp->MinorVersion);
138                         kfree(extp);
139                         return NULL;
140                 }
141
142                 /* Do some byteswapping if necessary */
143                 extp->FeatureSupport = cfi32_to_cpu(extp->FeatureSupport);
144                 extp->BlkStatusRegMask = cfi32_to_cpu(extp->BlkStatusRegMask);
145
146 #ifdef DEBUG_CFI_FEATURES
147                 /* Tell the user about it in lots of lovely detail */
148                 cfi_tell_features(extp);
149 #endif
150
151                 /* Install our own private info structure */
152                 cfi->cmdset_priv = extp;
153         }
154
155         for (i=0; i< cfi->numchips; i++) {
156                 cfi->chips[i].word_write_time = 128;
157                 cfi->chips[i].buffer_write_time = 128;
158                 cfi->chips[i].erase_time = 1024;
159                 cfi->chips[i].ref_point_counter = 0;
160                 init_waitqueue_head(&(cfi->chips[i].wq));
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", __func__, 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", __func__, 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", __func__, 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", __func__, map_bankwidth(map));
627         printk("%s: chipnum[%x] wbufsize[%x]\n", __func__, chipnum, wbufsize);
628         printk("%s: ofs[%x] len[%x]\n", __func__, 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->writesize)
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 static int cfi_staa_erase_varsize(struct mtd_info *mtd,
895                                   struct erase_info *instr)
896 {       struct map_info *map = mtd->priv;
897         struct cfi_private *cfi = map->fldrv_priv;
898         unsigned long adr, len;
899         int chipnum, ret = 0;
900         int i, first;
901         struct mtd_erase_region_info *regions = mtd->eraseregions;
902
903         if (instr->addr > mtd->size)
904                 return -EINVAL;
905
906         if ((instr->len + instr->addr) > mtd->size)
907                 return -EINVAL;
908
909         /* Check that both start and end of the requested erase are
910          * aligned with the erasesize at the appropriate addresses.
911          */
912
913         i = 0;
914
915         /* Skip all erase regions which are ended before the start of
916            the requested erase. Actually, to save on the calculations,
917            we skip to the first erase region which starts after the
918            start of the requested erase, and then go back one.
919         */
920
921         while (i < mtd->numeraseregions && instr->addr >= regions[i].offset)
922                i++;
923         i--;
924
925         /* OK, now i is pointing at the erase region in which this
926            erase request starts. Check the start of the requested
927            erase range is aligned with the erase size which is in
928            effect here.
929         */
930
931         if (instr->addr & (regions[i].erasesize-1))
932                 return -EINVAL;
933
934         /* Remember the erase region we start on */
935         first = i;
936
937         /* Next, check that the end of the requested erase is aligned
938          * with the erase region at that address.
939          */
940
941         while (i<mtd->numeraseregions && (instr->addr + instr->len) >= regions[i].offset)
942                 i++;
943
944         /* As before, drop back one to point at the region in which
945            the address actually falls
946         */
947         i--;
948
949         if ((instr->addr + instr->len) & (regions[i].erasesize-1))
950                 return -EINVAL;
951
952         chipnum = instr->addr >> cfi->chipshift;
953         adr = instr->addr - (chipnum << cfi->chipshift);
954         len = instr->len;
955
956         i=first;
957
958         while(len) {
959                 ret = do_erase_oneblock(map, &cfi->chips[chipnum], adr);
960
961                 if (ret)
962                         return ret;
963
964                 adr += regions[i].erasesize;
965                 len -= regions[i].erasesize;
966
967                 if (adr % (1<< cfi->chipshift) == ((regions[i].offset + (regions[i].erasesize * regions[i].numblocks)) %( 1<< cfi->chipshift)))
968                         i++;
969
970                 if (adr >> cfi->chipshift) {
971                         adr = 0;
972                         chipnum++;
973
974                         if (chipnum >= cfi->numchips)
975                         break;
976                 }
977         }
978
979         instr->state = MTD_ERASE_DONE;
980         mtd_erase_callback(instr);
981
982         return 0;
983 }
984
985 static void cfi_staa_sync (struct mtd_info *mtd)
986 {
987         struct map_info *map = mtd->priv;
988         struct cfi_private *cfi = map->fldrv_priv;
989         int i;
990         struct flchip *chip;
991         int ret = 0;
992         DECLARE_WAITQUEUE(wait, current);
993
994         for (i=0; !ret && i<cfi->numchips; i++) {
995                 chip = &cfi->chips[i];
996
997         retry:
998                 spin_lock_bh(chip->mutex);
999
1000                 switch(chip->state) {
1001                 case FL_READY:
1002                 case FL_STATUS:
1003                 case FL_CFI_QUERY:
1004                 case FL_JEDEC_QUERY:
1005                         chip->oldstate = chip->state;
1006                         chip->state = FL_SYNCING;
1007                         /* No need to wake_up() on this state change -
1008                          * as the whole point is that nobody can do anything
1009                          * with the chip now anyway.
1010                          */
1011                 case FL_SYNCING:
1012                         spin_unlock_bh(chip->mutex);
1013                         break;
1014
1015                 default:
1016                         /* Not an idle state */
1017                         set_current_state(TASK_UNINTERRUPTIBLE);
1018                         add_wait_queue(&chip->wq, &wait);
1019
1020                         spin_unlock_bh(chip->mutex);
1021                         schedule();
1022                         remove_wait_queue(&chip->wq, &wait);
1023
1024                         goto retry;
1025                 }
1026         }
1027
1028         /* Unlock the chips again */
1029
1030         for (i--; i >=0; i--) {
1031                 chip = &cfi->chips[i];
1032
1033                 spin_lock_bh(chip->mutex);
1034
1035                 if (chip->state == FL_SYNCING) {
1036                         chip->state = chip->oldstate;
1037                         wake_up(&chip->wq);
1038                 }
1039                 spin_unlock_bh(chip->mutex);
1040         }
1041 }
1042
1043 static inline int do_lock_oneblock(struct map_info *map, struct flchip *chip, unsigned long adr)
1044 {
1045         struct cfi_private *cfi = map->fldrv_priv;
1046         map_word status, status_OK;
1047         unsigned long timeo = jiffies + HZ;
1048         DECLARE_WAITQUEUE(wait, current);
1049
1050         adr += chip->start;
1051
1052         /* Let's determine this according to the interleave only once */
1053         status_OK = CMD(0x80);
1054
1055         timeo = jiffies + HZ;
1056 retry:
1057         spin_lock_bh(chip->mutex);
1058
1059         /* Check that the chip's ready to talk to us. */
1060         switch (chip->state) {
1061         case FL_CFI_QUERY:
1062         case FL_JEDEC_QUERY:
1063         case FL_READY:
1064                 map_write(map, CMD(0x70), adr);
1065                 chip->state = FL_STATUS;
1066
1067         case FL_STATUS:
1068                 status = map_read(map, adr);
1069                 if (map_word_andequal(map, status, status_OK, status_OK))
1070                         break;
1071
1072                 /* Urgh. Chip not yet ready to talk to us. */
1073                 if (time_after(jiffies, timeo)) {
1074                         spin_unlock_bh(chip->mutex);
1075                         printk(KERN_ERR "waiting for chip to be ready timed out in lock\n");
1076                         return -EIO;
1077                 }
1078
1079                 /* Latency issues. Drop the lock, wait a while and retry */
1080                 spin_unlock_bh(chip->mutex);
1081                 cfi_udelay(1);
1082                 goto retry;
1083
1084         default:
1085                 /* Stick ourselves on a wait queue to be woken when
1086                    someone changes the status */
1087                 set_current_state(TASK_UNINTERRUPTIBLE);
1088                 add_wait_queue(&chip->wq, &wait);
1089                 spin_unlock_bh(chip->mutex);
1090                 schedule();
1091                 remove_wait_queue(&chip->wq, &wait);
1092                 timeo = jiffies + HZ;
1093                 goto retry;
1094         }
1095
1096         ENABLE_VPP(map);
1097         map_write(map, CMD(0x60), adr);
1098         map_write(map, CMD(0x01), adr);
1099         chip->state = FL_LOCKING;
1100
1101         spin_unlock_bh(chip->mutex);
1102         msleep(1000);
1103         spin_lock_bh(chip->mutex);
1104
1105         /* FIXME. Use a timer to check this, and return immediately. */
1106         /* Once the state machine's known to be working I'll do that */
1107
1108         timeo = jiffies + (HZ*2);
1109         for (;;) {
1110
1111                 status = map_read(map, adr);
1112                 if (map_word_andequal(map, status, status_OK, status_OK))
1113                         break;
1114
1115                 /* OK Still waiting */
1116                 if (time_after(jiffies, timeo)) {
1117                         map_write(map, CMD(0x70), adr);
1118                         chip->state = FL_STATUS;
1119                         printk(KERN_ERR "waiting for lock to complete timed out. Xstatus = %lx, status = %lx.\n", status.x[0], map_read(map, adr).x[0]);
1120                         DISABLE_VPP(map);
1121                         spin_unlock_bh(chip->mutex);
1122                         return -EIO;
1123                 }
1124
1125                 /* Latency issues. Drop the lock, wait a while and retry */
1126                 spin_unlock_bh(chip->mutex);
1127                 cfi_udelay(1);
1128                 spin_lock_bh(chip->mutex);
1129         }
1130
1131         /* Done and happy. */
1132         chip->state = FL_STATUS;
1133         DISABLE_VPP(map);
1134         wake_up(&chip->wq);
1135         spin_unlock_bh(chip->mutex);
1136         return 0;
1137 }
1138 static int cfi_staa_lock(struct mtd_info *mtd, loff_t ofs, size_t len)
1139 {
1140         struct map_info *map = mtd->priv;
1141         struct cfi_private *cfi = map->fldrv_priv;
1142         unsigned long adr;
1143         int chipnum, ret = 0;
1144 #ifdef DEBUG_LOCK_BITS
1145         int ofs_factor = cfi->interleave * cfi->device_type;
1146 #endif
1147
1148         if (ofs & (mtd->erasesize - 1))
1149                 return -EINVAL;
1150
1151         if (len & (mtd->erasesize -1))
1152                 return -EINVAL;
1153
1154         if ((len + ofs) > mtd->size)
1155                 return -EINVAL;
1156
1157         chipnum = ofs >> cfi->chipshift;
1158         adr = ofs - (chipnum << cfi->chipshift);
1159
1160         while(len) {
1161
1162 #ifdef DEBUG_LOCK_BITS
1163                 cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1164                 printk("before lock: block status register is %x\n",cfi_read_query(map, adr+(2*ofs_factor)));
1165                 cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1166 #endif
1167
1168                 ret = do_lock_oneblock(map, &cfi->chips[chipnum], adr);
1169
1170 #ifdef DEBUG_LOCK_BITS
1171                 cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1172                 printk("after lock: block status register is %x\n",cfi_read_query(map, adr+(2*ofs_factor)));
1173                 cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1174 #endif
1175
1176                 if (ret)
1177                         return ret;
1178
1179                 adr += mtd->erasesize;
1180                 len -= mtd->erasesize;
1181
1182                 if (adr >> cfi->chipshift) {
1183                         adr = 0;
1184                         chipnum++;
1185
1186                         if (chipnum >= cfi->numchips)
1187                         break;
1188                 }
1189         }
1190         return 0;
1191 }
1192 static inline int do_unlock_oneblock(struct map_info *map, struct flchip *chip, unsigned long adr)
1193 {
1194         struct cfi_private *cfi = map->fldrv_priv;
1195         map_word status, status_OK;
1196         unsigned long timeo = jiffies + HZ;
1197         DECLARE_WAITQUEUE(wait, current);
1198
1199         adr += chip->start;
1200
1201         /* Let's determine this according to the interleave only once */
1202         status_OK = CMD(0x80);
1203
1204         timeo = jiffies + HZ;
1205 retry:
1206         spin_lock_bh(chip->mutex);
1207
1208         /* Check that the chip's ready to talk to us. */
1209         switch (chip->state) {
1210         case FL_CFI_QUERY:
1211         case FL_JEDEC_QUERY:
1212         case FL_READY:
1213                 map_write(map, CMD(0x70), adr);
1214                 chip->state = FL_STATUS;
1215
1216         case FL_STATUS:
1217                 status = map_read(map, adr);
1218                 if (map_word_andequal(map, status, status_OK, status_OK))
1219                         break;
1220
1221                 /* Urgh. Chip not yet ready to talk to us. */
1222                 if (time_after(jiffies, timeo)) {
1223                         spin_unlock_bh(chip->mutex);
1224                         printk(KERN_ERR "waiting for chip to be ready timed out in unlock\n");
1225                         return -EIO;
1226                 }
1227
1228                 /* Latency issues. Drop the lock, wait a while and retry */
1229                 spin_unlock_bh(chip->mutex);
1230                 cfi_udelay(1);
1231                 goto retry;
1232
1233         default:
1234                 /* Stick ourselves on a wait queue to be woken when
1235                    someone changes the status */
1236                 set_current_state(TASK_UNINTERRUPTIBLE);
1237                 add_wait_queue(&chip->wq, &wait);
1238                 spin_unlock_bh(chip->mutex);
1239                 schedule();
1240                 remove_wait_queue(&chip->wq, &wait);
1241                 timeo = jiffies + HZ;
1242                 goto retry;
1243         }
1244
1245         ENABLE_VPP(map);
1246         map_write(map, CMD(0x60), adr);
1247         map_write(map, CMD(0xD0), adr);
1248         chip->state = FL_UNLOCKING;
1249
1250         spin_unlock_bh(chip->mutex);
1251         msleep(1000);
1252         spin_lock_bh(chip->mutex);
1253
1254         /* FIXME. Use a timer to check this, and return immediately. */
1255         /* Once the state machine's known to be working I'll do that */
1256
1257         timeo = jiffies + (HZ*2);
1258         for (;;) {
1259
1260                 status = map_read(map, adr);
1261                 if (map_word_andequal(map, status, status_OK, status_OK))
1262                         break;
1263
1264                 /* OK Still waiting */
1265                 if (time_after(jiffies, timeo)) {
1266                         map_write(map, CMD(0x70), adr);
1267                         chip->state = FL_STATUS;
1268                         printk(KERN_ERR "waiting for unlock to complete timed out. Xstatus = %lx, status = %lx.\n", status.x[0], map_read(map, adr).x[0]);
1269                         DISABLE_VPP(map);
1270                         spin_unlock_bh(chip->mutex);
1271                         return -EIO;
1272                 }
1273
1274                 /* Latency issues. Drop the unlock, wait a while and retry */
1275                 spin_unlock_bh(chip->mutex);
1276                 cfi_udelay(1);
1277                 spin_lock_bh(chip->mutex);
1278         }
1279
1280         /* Done and happy. */
1281         chip->state = FL_STATUS;
1282         DISABLE_VPP(map);
1283         wake_up(&chip->wq);
1284         spin_unlock_bh(chip->mutex);
1285         return 0;
1286 }
1287 static int cfi_staa_unlock(struct mtd_info *mtd, loff_t ofs, size_t len)
1288 {
1289         struct map_info *map = mtd->priv;
1290         struct cfi_private *cfi = map->fldrv_priv;
1291         unsigned long adr;
1292         int chipnum, ret = 0;
1293 #ifdef DEBUG_LOCK_BITS
1294         int ofs_factor = cfi->interleave * cfi->device_type;
1295 #endif
1296
1297         chipnum = ofs >> cfi->chipshift;
1298         adr = ofs - (chipnum << cfi->chipshift);
1299
1300 #ifdef DEBUG_LOCK_BITS
1301         {
1302                 unsigned long temp_adr = adr;
1303                 unsigned long temp_len = len;
1304
1305                 cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1306                 while (temp_len) {
1307                         printk("before unlock %x: block status register is %x\n",temp_adr,cfi_read_query(map, temp_adr+(2*ofs_factor)));
1308                         temp_adr += mtd->erasesize;
1309                         temp_len -= mtd->erasesize;
1310                 }
1311                 cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1312         }
1313 #endif
1314
1315         ret = do_unlock_oneblock(map, &cfi->chips[chipnum], adr);
1316
1317 #ifdef DEBUG_LOCK_BITS
1318         cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1319         printk("after unlock: block status register is %x\n",cfi_read_query(map, adr+(2*ofs_factor)));
1320         cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1321 #endif
1322
1323         return ret;
1324 }
1325
1326 static int cfi_staa_suspend(struct mtd_info *mtd)
1327 {
1328         struct map_info *map = mtd->priv;
1329         struct cfi_private *cfi = map->fldrv_priv;
1330         int i;
1331         struct flchip *chip;
1332         int ret = 0;
1333
1334         for (i=0; !ret && i<cfi->numchips; i++) {
1335                 chip = &cfi->chips[i];
1336
1337                 spin_lock_bh(chip->mutex);
1338
1339                 switch(chip->state) {
1340                 case FL_READY:
1341                 case FL_STATUS:
1342                 case FL_CFI_QUERY:
1343                 case FL_JEDEC_QUERY:
1344                         chip->oldstate = chip->state;
1345                         chip->state = FL_PM_SUSPENDED;
1346                         /* No need to wake_up() on this state change -
1347                          * as the whole point is that nobody can do anything
1348                          * with the chip now anyway.
1349                          */
1350                 case FL_PM_SUSPENDED:
1351                         break;
1352
1353                 default:
1354                         ret = -EAGAIN;
1355                         break;
1356                 }
1357                 spin_unlock_bh(chip->mutex);
1358         }
1359
1360         /* Unlock the chips again */
1361
1362         if (ret) {
1363                 for (i--; i >=0; i--) {
1364                         chip = &cfi->chips[i];
1365
1366                         spin_lock_bh(chip->mutex);
1367
1368                         if (chip->state == FL_PM_SUSPENDED) {
1369                                 /* No need to force it into a known state here,
1370                                    because we're returning failure, and it didn't
1371                                    get power cycled */
1372                                 chip->state = chip->oldstate;
1373                                 wake_up(&chip->wq);
1374                         }
1375                         spin_unlock_bh(chip->mutex);
1376                 }
1377         }
1378
1379         return ret;
1380 }
1381
1382 static void cfi_staa_resume(struct mtd_info *mtd)
1383 {
1384         struct map_info *map = mtd->priv;
1385         struct cfi_private *cfi = map->fldrv_priv;
1386         int i;
1387         struct flchip *chip;
1388
1389         for (i=0; i<cfi->numchips; i++) {
1390
1391                 chip = &cfi->chips[i];
1392
1393                 spin_lock_bh(chip->mutex);
1394
1395                 /* Go to known state. Chip may have been power cycled */
1396                 if (chip->state == FL_PM_SUSPENDED) {
1397                         map_write(map, CMD(0xFF), 0);
1398                         chip->state = FL_READY;
1399                         wake_up(&chip->wq);
1400                 }
1401
1402                 spin_unlock_bh(chip->mutex);
1403         }
1404 }
1405
1406 static void cfi_staa_destroy(struct mtd_info *mtd)
1407 {
1408         struct map_info *map = mtd->priv;
1409         struct cfi_private *cfi = map->fldrv_priv;
1410         kfree(cfi->cmdset_priv);
1411         kfree(cfi);
1412 }
1413
1414 MODULE_LICENSE("GPL");