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->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 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");