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