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                 kfree(cfi->cmdset_priv);
 179                 return NULL;
 180         }
 181
 182         mtd->priv = map;
 183         mtd->type = MTD_NORFLASH;
 184         mtd->size = devsize * cfi->numchips;
 185
 186         mtd->numeraseregions = cfi->cfiq->NumEraseRegions * cfi->numchips;
 187         mtd->eraseregions = kmalloc_array(mtd->numeraseregions,
 188                                           sizeof(struct mtd_erase_region_info),
 189                                           GFP_KERNEL);
 190         if (!mtd->eraseregions) {
 191                 kfree(cfi->cmdset_priv);
 192                 kfree(mtd);
 193                 return NULL;
 194         }
 195
 196         for (i=0; i<cfi->cfiq->NumEraseRegions; i++) {
 197                 unsigned long ernum, ersize;
 198                 ersize = ((cfi->cfiq->EraseRegionInfo[i] >> 8) & ~0xff) * cfi->interleave;
 199                 ernum = (cfi->cfiq->EraseRegionInfo[i] & 0xffff) + 1;
 200
 201                 if (mtd->erasesize < ersize) {
 202                         mtd->erasesize = ersize;
 203                 }
 204                 for (j=0; j<cfi->numchips; j++) {
 205                         mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].offset = (j*devsize)+offset;
 206                         mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].erasesize = ersize;
 207                         mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].numblocks = ernum;
 208                 }
 209                 offset += (ersize * ernum);
 210         }
 211
 212         if (offset != devsize) {
 213                 /* Argh */
 214                 printk(KERN_WARNING "Sum of regions (%lx) != total size of set of interleaved chips (%lx)\n", offset, devsize);
 215                 kfree(mtd->eraseregions);
 216                 kfree(cfi->cmdset_priv);
 217                 kfree(mtd);
 218                 return NULL;
 219         }
 220
 221         for (i=0; i<mtd->numeraseregions;i++){
 222                 printk(KERN_DEBUG "%d: offset=0x%llx,size=0x%x,blocks=%d\n",
 223                        i, (unsigned long long)mtd->eraseregions[i].offset,
 224                        mtd->eraseregions[i].erasesize,
 225                        mtd->eraseregions[i].numblocks);
 226         }
 227
 228         /* Also select the correct geometry setup too */
 229         mtd->_erase = cfi_staa_erase_varsize;
 230         mtd->_read = cfi_staa_read;
 231         mtd->_write = cfi_staa_write_buffers;
 232         mtd->_writev = cfi_staa_writev;
 233         mtd->_sync = cfi_staa_sync;
 234         mtd->_lock = cfi_staa_lock;
 235         mtd->_unlock = cfi_staa_unlock;
 236         mtd->_suspend = cfi_staa_suspend;
 237         mtd->_resume = cfi_staa_resume;
 238         mtd->flags = MTD_CAP_NORFLASH & ~MTD_BIT_WRITEABLE;
 239         mtd->writesize = 8; /* FIXME: Should be 0 for STMicro flashes w/out ECC */
 240         mtd->writebufsize = cfi_interleave(cfi) << cfi->cfiq->MaxBufWriteSize;
 241         map->fldrv = &cfi_staa_chipdrv;
 242         __module_get(THIS_MODULE);
 243         mtd->name = map->name;
 244         return mtd;
 245 }
 246
 247
 248 static inline int do_read_onechip(struct map_info *map, struct flchip *chip, loff_t adr, size_t len, u_char *buf)
 249 {
 250         map_word status, status_OK;
 251         unsigned long timeo;
 252         DECLARE_WAITQUEUE(wait, current);
 253         int suspended = 0;
 254         unsigned long cmd_addr;
 255         struct cfi_private *cfi = map->fldrv_priv;
 256
 257         adr += chip->start;
 258
 259         /* Ensure cmd read/writes are aligned. */
 260         cmd_addr = adr & ~(map_bankwidth(map)-1);
 261
 262         /* Let's determine this according to the interleave only once */
 263         status_OK = CMD(0x80);
 264
 265         timeo = jiffies + HZ;
 266  retry:
 267         mutex_lock(&chip->mutex);
 268
 269         /* Check that the chip's ready to talk to us.
 270          * If it's in FL_ERASING state, suspend it and make it talk now.
 271          */
 272         switch (chip->state) {
 273         case FL_ERASING:
 274                 if (!(((struct cfi_pri_intelext *)cfi->cmdset_priv)->FeatureSupport & 2))
 275                         goto sleep; /* We don't support erase suspend */
 276
 277                 map_write (map, CMD(0xb0), cmd_addr);
 278                 /* If the flash has finished erasing, then 'erase suspend'
 279                  * appears to make some (28F320) flash devices switch to
 280                  * 'read' mode.  Make sure that we switch to 'read status'
 281                  * mode so we get the right data. --rmk
 282                  */
 283                 map_write(map, CMD(0x70), cmd_addr);
 284                 chip->oldstate = FL_ERASING;
 285                 chip->state = FL_ERASE_SUSPENDING;
 286                 //              printk("Erase suspending at 0x%lx\n", cmd_addr);
 287                 for (;;) {
 288                         status = map_read(map, cmd_addr);
 289                         if (map_word_andequal(map, status, status_OK, status_OK))
 290                                 break;
 291
 292                         if (time_after(jiffies, timeo)) {
 293                                 /* Urgh */
 294                                 map_write(map, CMD(0xd0), cmd_addr);
 295                                 /* make sure we're in 'read status' mode */
 296                                 map_write(map, CMD(0x70), cmd_addr);
 297                                 chip->state = FL_ERASING;
 298                                 wake_up(&chip->wq);
 299                                 mutex_unlock(&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                         mutex_unlock(&chip->mutex);
 306                         cfi_udelay(1);
 307                         mutex_lock(&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                 fallthrough;
 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                         mutex_unlock(&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                 mutex_unlock(&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                 mutex_unlock(&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         mutex_unlock(&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         while (len) {
 396                 unsigned long thislen;
 397
 398                 if (chipnum >= cfi->numchips)
 399                         break;
 400
 401                 if ((len + ofs -1) >> cfi->chipshift)
 402                         thislen = (1<<cfi->chipshift) - ofs;
 403                 else
 404                         thislen = len;
 405
 406                 ret = do_read_onechip(map, &cfi->chips[chipnum], ofs, thislen, buf);
 407                 if (ret)
 408                         break;
 409
 410                 *retlen += thislen;
 411                 len -= thislen;
 412                 buf += thislen;
 413
 414                 ofs = 0;
 415                 chipnum++;
 416         }
 417         return ret;
 418 }
 419
 420 static int do_write_buffer(struct map_info *map, struct flchip *chip,
 421                                   unsigned long adr, const u_char *buf, int len)
 422 {
 423         struct cfi_private *cfi = map->fldrv_priv;
 424         map_word status, status_OK;
 425         unsigned long cmd_adr, timeo;
 426         DECLARE_WAITQUEUE(wait, current);
 427         int wbufsize, z;
 428
 429         /* M58LW064A requires bus alignment for buffer wriets -- saw */
 430         if (adr & (map_bankwidth(map)-1))
 431             return -EINVAL;
 432
 433         wbufsize = cfi_interleave(cfi) << cfi->cfiq->MaxBufWriteSize;
 434         adr += chip->start;
 435         cmd_adr = adr & ~(wbufsize-1);
 436
 437         /* Let's determine this according to the interleave only once */
 438         status_OK = CMD(0x80);
 439
 440         timeo = jiffies + HZ;
 441  retry:
 442
 443 #ifdef DEBUG_CFI_FEATURES
 444        printk("%s: chip->state[%d]\n", __func__, chip->state);
 445 #endif
 446         mutex_lock(&chip->mutex);
 447
 448         /* Check that the chip's ready to talk to us.
 449          * Later, we can actually think about interrupting it
 450          * if it's in FL_ERASING state.
 451          * Not just yet, though.
 452          */
 453         switch (chip->state) {
 454         case FL_READY:
 455                 break;
 456
 457         case FL_CFI_QUERY:
 458         case FL_JEDEC_QUERY:
 459                 map_write(map, CMD(0x70), cmd_adr);
 460                 chip->state = FL_STATUS;
 461 #ifdef DEBUG_CFI_FEATURES
 462         printk("%s: 1 status[%x]\n", __func__, map_read(map, cmd_adr));
 463 #endif
 464                 fallthrough;
 465         case FL_STATUS:
 466                 status = map_read(map, cmd_adr);
 467                 if (map_word_andequal(map, status, status_OK, status_OK))
 468                         break;
 469                 /* Urgh. Chip not yet ready to talk to us. */
 470                 if (time_after(jiffies, timeo)) {
 471                         mutex_unlock(&chip->mutex);
 472                         printk(KERN_ERR "waiting for chip to be ready timed out in buffer write Xstatus = %lx, status = %lx\n",
 473                                status.x[0], map_read(map, cmd_adr).x[0]);
 474                         return -EIO;
 475                 }
 476
 477                 /* Latency issues. Drop the lock, wait a while and retry */
 478                 mutex_unlock(&chip->mutex);
 479                 cfi_udelay(1);
 480                 goto retry;
 481
 482         default:
 483                 /* Stick ourselves on a wait queue to be woken when
 484                    someone changes the status */
 485                 set_current_state(TASK_UNINTERRUPTIBLE);
 486                 add_wait_queue(&chip->wq, &wait);
 487                 mutex_unlock(&chip->mutex);
 488                 schedule();
 489                 remove_wait_queue(&chip->wq, &wait);
 490                 timeo = jiffies + HZ;
 491                 goto retry;
 492         }
 493
 494         ENABLE_VPP(map);
 495         map_write(map, CMD(0xe8), cmd_adr);
 496         chip->state = FL_WRITING_TO_BUFFER;
 497
 498         z = 0;
 499         for (;;) {
 500                 status = map_read(map, cmd_adr);
 501                 if (map_word_andequal(map, status, status_OK, status_OK))
 502                         break;
 503
 504                 mutex_unlock(&chip->mutex);
 505                 cfi_udelay(1);
 506                 mutex_lock(&chip->mutex);
 507
 508                 if (++z > 100) {
 509                         /* Argh. Not ready for write to buffer */
 510                         DISABLE_VPP(map);
 511                         map_write(map, CMD(0x70), cmd_adr);
 512                         chip->state = FL_STATUS;
 513                         mutex_unlock(&chip->mutex);
 514                         printk(KERN_ERR "Chip not ready for buffer write. Xstatus = %lx\n", status.x[0]);
 515                         return -EIO;
 516                 }
 517         }
 518
 519         /* Write length of data to come */
 520         map_write(map, CMD(len/map_bankwidth(map)-1), cmd_adr );
 521
 522         /* Write data */
 523         for (z = 0; z < len;
 524              z += map_bankwidth(map), buf += map_bankwidth(map)) {
 525                 map_word d;
 526                 d = map_word_load(map, buf);
 527                 map_write(map, d, adr+z);
 528         }
 529         /* GO GO GO */
 530         map_write(map, CMD(0xd0), cmd_adr);
 531         chip->state = FL_WRITING;
 532
 533         mutex_unlock(&chip->mutex);
 534         cfi_udelay(chip->buffer_write_time);
 535         mutex_lock(&chip->mutex);
 536
 537         timeo = jiffies + (HZ/2);
 538         z = 0;
 539         for (;;) {
 540                 if (chip->state != FL_WRITING) {
 541                         /* Someone's suspended the write. Sleep */
 542                         set_current_state(TASK_UNINTERRUPTIBLE);
 543                         add_wait_queue(&chip->wq, &wait);
 544                         mutex_unlock(&chip->mutex);
 545                         schedule();
 546                         remove_wait_queue(&chip->wq, &wait);
 547                         timeo = jiffies + (HZ / 2); /* FIXME */
 548                         mutex_lock(&chip->mutex);
 549                         continue;
 550                 }
 551
 552                 status = map_read(map, cmd_adr);
 553                 if (map_word_andequal(map, status, status_OK, status_OK))
 554                         break;
 555
 556                 /* OK Still waiting */
 557                 if (time_after(jiffies, timeo)) {
 558                         /* clear status */
 559                         map_write(map, CMD(0x50), cmd_adr);
 560                         /* put back into read status register mode */
 561                         map_write(map, CMD(0x70), adr);
 562                         chip->state = FL_STATUS;
 563                         DISABLE_VPP(map);
 564                         mutex_unlock(&chip->mutex);
 565                         printk(KERN_ERR "waiting for chip to be ready timed out in bufwrite\n");
 566                         return -EIO;
 567                 }
 568
 569                 /* Latency issues. Drop the lock, wait a while and retry */
 570                 mutex_unlock(&chip->mutex);
 571                 cfi_udelay(1);
 572                 z++;
 573                 mutex_lock(&chip->mutex);
 574         }
 575         if (!z) {
 576                 chip->buffer_write_time--;
 577                 if (!chip->buffer_write_time)
 578                         chip->buffer_write_time++;
 579         }
 580         if (z > 1)
 581                 chip->buffer_write_time++;
 582
 583         /* Done and happy. */
 584         DISABLE_VPP(map);
 585         chip->state = FL_STATUS;
 586
 587         /* check for errors: 'lock bit', 'VPP', 'dead cell'/'unerased cell' or 'incorrect cmd' -- saw */
 588         if (map_word_bitsset(map, status, CMD(0x3a))) {
 589 #ifdef DEBUG_CFI_FEATURES
 590                 printk("%s: 2 status[%lx]\n", __func__, status.x[0]);
 591 #endif
 592                 /* clear status */
 593                 map_write(map, CMD(0x50), cmd_adr);
 594                 /* put back into read status register mode */
 595                 map_write(map, CMD(0x70), adr);
 596                 wake_up(&chip->wq);
 597                 mutex_unlock(&chip->mutex);
 598                 return map_word_bitsset(map, status, CMD(0x02)) ? -EROFS : -EIO;
 599         }
 600         wake_up(&chip->wq);
 601         mutex_unlock(&chip->mutex);
 602
 603         return 0;
 604 }
 605
 606 static int cfi_staa_write_buffers (struct mtd_info *mtd, loff_t to,
 607                                        size_t len, size_t *retlen, const u_char *buf)
 608 {
 609         struct map_info *map = mtd->priv;
 610         struct cfi_private *cfi = map->fldrv_priv;
 611         int wbufsize = cfi_interleave(cfi) << cfi->cfiq->MaxBufWriteSize;
 612         int ret;
 613         int chipnum;
 614         unsigned long ofs;
 615
 616         chipnum = to >> cfi->chipshift;
 617         ofs = to  - (chipnum << cfi->chipshift);
 618
 619 #ifdef DEBUG_CFI_FEATURES
 620         printk("%s: map_bankwidth(map)[%x]\n", __func__, map_bankwidth(map));
 621         printk("%s: chipnum[%x] wbufsize[%x]\n", __func__, chipnum, wbufsize);
 622         printk("%s: ofs[%x] len[%x]\n", __func__, ofs, len);
 623 #endif
 624
 625         /* Write buffer is worth it only if more than one word to write... */
 626         while (len > 0) {
 627                 /* We must not cross write block boundaries */
 628                 int size = wbufsize - (ofs & (wbufsize-1));
 629
 630                 if (size > len)
 631                     size = len;
 632
 633                 ret = do_write_buffer(map, &cfi->chips[chipnum],
 634                                       ofs, buf, size);
 635                 if (ret)
 636                         return ret;
 637
 638                 ofs += size;
 639                 buf += size;
 640                 (*retlen) += size;
 641                 len -= size;
 642
 643                 if (ofs >> cfi->chipshift) {
 644                         chipnum ++;
 645                         ofs = 0;
 646                         if (chipnum == cfi->numchips)
 647                                 return 0;
 648                 }
 649         }
 650
 651         return 0;
 652 }
 653
 654 /*
 655  * Writev for ECC-Flashes is a little more complicated. We need to maintain
 656  * a small buffer for this.
 657  * XXX: If the buffer size is not a multiple of 2, this will break
 658  */
 659 #define ECCBUF_SIZE (mtd->writesize)
 660 #define ECCBUF_DIV(x) ((x) & ~(ECCBUF_SIZE - 1))
 661 #define ECCBUF_MOD(x) ((x) &  (ECCBUF_SIZE - 1))
 662 static int
 663 cfi_staa_writev(struct mtd_info *mtd, const struct kvec *vecs,
 664                 unsigned long count, loff_t to, size_t *retlen)
 665 {
 666         unsigned long i;
 667         size_t   totlen = 0, thislen;
 668         int      ret = 0;
 669         size_t   buflen = 0;
 670         char *buffer;
 671
 672         if (!ECCBUF_SIZE) {
 673                 /* We should fall back to a general writev implementation.
 674                  * Until that is written, just break.
 675                  */
 676                 return -EIO;
 677         }
 678         buffer = kmalloc(ECCBUF_SIZE, GFP_KERNEL);
 679         if (!buffer)
 680                 return -ENOMEM;
 681
 682         for (i=0; i<count; i++) {
 683                 size_t elem_len = vecs[i].iov_len;
 684                 void *elem_base = vecs[i].iov_base;
 685                 if (!elem_len) /* FIXME: Might be unnecessary. Check that */
 686                         continue;
 687                 if (buflen) { /* cut off head */
 688                         if (buflen + elem_len < ECCBUF_SIZE) { /* just accumulate */
 689                                 memcpy(buffer+buflen, elem_base, elem_len);
 690                                 buflen += elem_len;
 691                                 continue;
 692                         }
 693                         memcpy(buffer+buflen, elem_base, ECCBUF_SIZE-buflen);
 694                         ret = mtd_write(mtd, to, ECCBUF_SIZE, &thislen,
 695                                         buffer);
 696                         totlen += thislen;
 697                         if (ret || thislen != ECCBUF_SIZE)
 698                                 goto write_error;
 699                         elem_len -= thislen-buflen;
 700                         elem_base += thislen-buflen;
 701                         to += ECCBUF_SIZE;
 702                 }
 703                 if (ECCBUF_DIV(elem_len)) { /* write clean aligned data */
 704                         ret = mtd_write(mtd, to, ECCBUF_DIV(elem_len),
 705                                         &thislen, elem_base);
 706                         totlen += thislen;
 707                         if (ret || thislen != ECCBUF_DIV(elem_len))
 708                                 goto write_error;
 709                         to += thislen;
 710                 }
 711                 buflen = ECCBUF_MOD(elem_len); /* cut off tail */
 712                 if (buflen) {
 713                         memset(buffer, 0xff, ECCBUF_SIZE);
 714                         memcpy(buffer, elem_base + thislen, buflen);
 715                 }
 716         }
 717         if (buflen) { /* flush last page, even if not full */
 718                 /* This is sometimes intended behaviour, really */
 719                 ret = mtd_write(mtd, to, buflen, &thislen, buffer);
 720                 totlen += thislen;
 721                 if (ret || thislen != ECCBUF_SIZE)
 722                         goto write_error;
 723         }
 724 write_error:
 725         if (retlen)
 726                 *retlen = totlen;
 727         kfree(buffer);
 728         return ret;
 729 }
 730
 731
 732 static inline int do_erase_oneblock(struct map_info *map, struct flchip *chip, unsigned long adr)
 733 {
 734         struct cfi_private *cfi = map->fldrv_priv;
 735         map_word status, status_OK;
 736         unsigned long timeo;
 737         int retries = 3;
 738         DECLARE_WAITQUEUE(wait, current);
 739         int ret = 0;
 740
 741         adr += chip->start;
 742
 743         /* Let's determine this according to the interleave only once */
 744         status_OK = CMD(0x80);
 745
 746         timeo = jiffies + HZ;
 747 retry:
 748         mutex_lock(&chip->mutex);
 749
 750         /* Check that the chip's ready to talk to us. */
 751         switch (chip->state) {
 752         case FL_CFI_QUERY:
 753         case FL_JEDEC_QUERY:
 754         case FL_READY:
 755                 map_write(map, CMD(0x70), adr);
 756                 chip->state = FL_STATUS;
 757                 fallthrough;
 758         case FL_STATUS:
 759                 status = map_read(map, adr);
 760                 if (map_word_andequal(map, status, status_OK, status_OK))
 761                         break;
 762
 763                 /* Urgh. Chip not yet ready to talk to us. */
 764                 if (time_after(jiffies, timeo)) {
 765                         mutex_unlock(&chip->mutex);
 766                         printk(KERN_ERR "waiting for chip to be ready timed out in erase\n");
 767                         return -EIO;
 768                 }
 769
 770                 /* Latency issues. Drop the lock, wait a while and retry */
 771                 mutex_unlock(&chip->mutex);
 772                 cfi_udelay(1);
 773                 goto retry;
 774
 775         default:
 776                 /* Stick ourselves on a wait queue to be woken when
 777                    someone changes the status */
 778                 set_current_state(TASK_UNINTERRUPTIBLE);
 779                 add_wait_queue(&chip->wq, &wait);
 780                 mutex_unlock(&chip->mutex);
 781                 schedule();
 782                 remove_wait_queue(&chip->wq, &wait);
 783                 timeo = jiffies + HZ;
 784                 goto retry;
 785         }
 786
 787         ENABLE_VPP(map);
 788         /* Clear the status register first */
 789         map_write(map, CMD(0x50), adr);
 790
 791         /* Now erase */
 792         map_write(map, CMD(0x20), adr);
 793         map_write(map, CMD(0xD0), adr);
 794         chip->state = FL_ERASING;
 795
 796         mutex_unlock(&chip->mutex);
 797         msleep(1000);
 798         mutex_lock(&chip->mutex);
 799
 800         /* FIXME. Use a timer to check this, and return immediately. */
 801         /* Once the state machine's known to be working I'll do that */
 802
 803         timeo = jiffies + (HZ*20);
 804         for (;;) {
 805                 if (chip->state != FL_ERASING) {
 806                         /* Someone's suspended the erase. Sleep */
 807                         set_current_state(TASK_UNINTERRUPTIBLE);
 808                         add_wait_queue(&chip->wq, &wait);
 809                         mutex_unlock(&chip->mutex);
 810                         schedule();
 811                         remove_wait_queue(&chip->wq, &wait);
 812                         timeo = jiffies + (HZ*20); /* FIXME */
 813                         mutex_lock(&chip->mutex);
 814                         continue;
 815                 }
 816
 817                 status = map_read(map, adr);
 818                 if (map_word_andequal(map, status, status_OK, status_OK))
 819                         break;
 820
 821                 /* OK Still waiting */
 822                 if (time_after(jiffies, timeo)) {
 823                         map_write(map, CMD(0x70), adr);
 824                         chip->state = FL_STATUS;
 825                         printk(KERN_ERR "waiting for erase to complete timed out. Xstatus = %lx, status = %lx.\n", status.x[0], map_read(map, adr).x[0]);
 826                         DISABLE_VPP(map);
 827                         mutex_unlock(&chip->mutex);
 828                         return -EIO;
 829                 }
 830
 831                 /* Latency issues. Drop the lock, wait a while and retry */
 832                 mutex_unlock(&chip->mutex);
 833                 cfi_udelay(1);
 834                 mutex_lock(&chip->mutex);
 835         }
 836
 837         DISABLE_VPP(map);
 838         ret = 0;
 839
 840         /* We've broken this before. It doesn't hurt to be safe */
 841         map_write(map, CMD(0x70), adr);
 842         chip->state = FL_STATUS;
 843         status = map_read(map, adr);
 844
 845         /* check for lock bit */
 846         if (map_word_bitsset(map, status, CMD(0x3a))) {
 847                 unsigned char chipstatus = status.x[0];
 848                 if (!map_word_equal(map, status, CMD(chipstatus))) {
 849                         int i, w;
 850                         for (w=0; w<map_words(map); w++) {
 851                                 for (i = 0; i<cfi_interleave(cfi); i++) {
 852                                         chipstatus |= status.x[w] >> (cfi->device_type * 8);
 853                                 }
 854                         }
 855                         printk(KERN_WARNING "Status is not identical for all chips: 0x%lx. Merging to give 0x%02x\n",
 856                                status.x[0], chipstatus);
 857                 }
 858                 /* Reset the error bits */
 859                 map_write(map, CMD(0x50), adr);
 860                 map_write(map, CMD(0x70), adr);
 861
 862                 if ((chipstatus & 0x30) == 0x30) {
 863                         printk(KERN_NOTICE "Chip reports improper command sequence: status 0x%x\n", chipstatus);
 864                         ret = -EIO;
 865                 } else if (chipstatus & 0x02) {
 866                         /* Protection bit set */
 867                         ret = -EROFS;
 868                 } else if (chipstatus & 0x8) {
 869                         /* Voltage */
 870                         printk(KERN_WARNING "Chip reports voltage low on erase: status 0x%x\n", chipstatus);
 871                         ret = -EIO;
 872                 } else if (chipstatus & 0x20) {
 873                         if (retries--) {
 874                                 printk(KERN_DEBUG "Chip erase failed at 0x%08lx: status 0x%x. Retrying...\n", adr, chipstatus);
 875                                 timeo = jiffies + HZ;
 876                                 chip->state = FL_STATUS;
 877                                 mutex_unlock(&chip->mutex);
 878                                 goto retry;
 879                         }
 880                         printk(KERN_DEBUG "Chip erase failed at 0x%08lx: status 0x%x\n", adr, chipstatus);
 881                         ret = -EIO;
 882                 }
 883         }
 884
 885         wake_up(&chip->wq);
 886         mutex_unlock(&chip->mutex);
 887         return ret;
 888 }
 889
 890 static int cfi_staa_erase_varsize(struct mtd_info *mtd,
 891                                   struct erase_info *instr)
 892 {       struct map_info *map = mtd->priv;
 893         struct cfi_private *cfi = map->fldrv_priv;
 894         unsigned long adr, len;
 895         int chipnum, ret;
 896         int i, first;
 897         struct mtd_erase_region_info *regions = mtd->eraseregions;
 898
 899         /* Check that both start and end of the requested erase are
 900          * aligned with the erasesize at the appropriate addresses.
 901          */
 902
 903         i = 0;
 904
 905         /* Skip all erase regions which are ended before the start of
 906            the requested erase. Actually, to save on the calculations,
 907            we skip to the first erase region which starts after the
 908            start of the requested erase, and then go back one.
 909         */
 910
 911         while (i < mtd->numeraseregions && instr->addr >= regions[i].offset)
 912                i++;
 913         i--;
 914
 915         /* OK, now i is pointing at the erase region in which this
 916            erase request starts. Check the start of the requested
 917            erase range is aligned with the erase size which is in
 918            effect here.
 919         */
 920
 921         if (instr->addr & (regions[i].erasesize-1))
 922                 return -EINVAL;
 923
 924         /* Remember the erase region we start on */
 925         first = i;
 926
 927         /* Next, check that the end of the requested erase is aligned
 928          * with the erase region at that address.
 929          */
 930
 931         while (i<mtd->numeraseregions && (instr->addr + instr->len) >= regions[i].offset)
 932                 i++;
 933
 934         /* As before, drop back one to point at the region in which
 935            the address actually falls
 936         */
 937         i--;
 938
 939         if ((instr->addr + instr->len) & (regions[i].erasesize-1))
 940                 return -EINVAL;
 941
 942         chipnum = instr->addr >> cfi->chipshift;
 943         adr = instr->addr - (chipnum << cfi->chipshift);
 944         len = instr->len;
 945
 946         i=first;
 947
 948         while(len) {
 949                 ret = do_erase_oneblock(map, &cfi->chips[chipnum], adr);
 950
 951                 if (ret)
 952                         return ret;
 953
 954                 adr += regions[i].erasesize;
 955                 len -= regions[i].erasesize;
 956
 957                 if (adr % (1<< cfi->chipshift) == (((unsigned long)regions[i].offset + (regions[i].erasesize * regions[i].numblocks)) %( 1<< cfi->chipshift)))
 958                         i++;
 959
 960                 if (adr >> cfi->chipshift) {
 961                         adr = 0;
 962                         chipnum++;
 963
 964                         if (chipnum >= cfi->numchips)
 965                                 break;
 966                 }
 967         }
 968
 969         return 0;
 970 }
 971
 972 static void cfi_staa_sync (struct mtd_info *mtd)
 973 {
 974         struct map_info *map = mtd->priv;
 975         struct cfi_private *cfi = map->fldrv_priv;
 976         int i;
 977         struct flchip *chip;
 978         int ret = 0;
 979         DECLARE_WAITQUEUE(wait, current);
 980
 981         for (i=0; !ret && i<cfi->numchips; i++) {
 982                 chip = &cfi->chips[i];
 983
 984         retry:
 985                 mutex_lock(&chip->mutex);
 986
 987                 switch(chip->state) {
 988                 case FL_READY:
 989                 case FL_STATUS:
 990                 case FL_CFI_QUERY:
 991                 case FL_JEDEC_QUERY:
 992                         chip->oldstate = chip->state;
 993                         chip->state = FL_SYNCING;
 994                         /* No need to wake_up() on this state change -
 995                          * as the whole point is that nobody can do anything
 996                          * with the chip now anyway.
 997                          */
 998                         fallthrough;
 999                 case FL_SYNCING:
1000                         mutex_unlock(&chip->mutex);
1001                         break;
1002
1003                 default:
1004                         /* Not an idle state */
1005                         set_current_state(TASK_UNINTERRUPTIBLE);
1006                         add_wait_queue(&chip->wq, &wait);
1007
1008                         mutex_unlock(&chip->mutex);
1009                         schedule();
1010                         remove_wait_queue(&chip->wq, &wait);
1011
1012                         goto retry;
1013                 }
1014         }
1015
1016         /* Unlock the chips again */
1017
1018         for (i--; i >=0; i--) {
1019                 chip = &cfi->chips[i];
1020
1021                 mutex_lock(&chip->mutex);
1022
1023                 if (chip->state == FL_SYNCING) {
1024                         chip->state = chip->oldstate;
1025                         wake_up(&chip->wq);
1026                 }
1027                 mutex_unlock(&chip->mutex);
1028         }
1029 }
1030
1031 static inline int do_lock_oneblock(struct map_info *map, struct flchip *chip, unsigned long adr)
1032 {
1033         struct cfi_private *cfi = map->fldrv_priv;
1034         map_word status, status_OK;
1035         unsigned long timeo = jiffies + HZ;
1036         DECLARE_WAITQUEUE(wait, current);
1037
1038         adr += chip->start;
1039
1040         /* Let's determine this according to the interleave only once */
1041         status_OK = CMD(0x80);
1042
1043         timeo = jiffies + HZ;
1044 retry:
1045         mutex_lock(&chip->mutex);
1046
1047         /* Check that the chip's ready to talk to us. */
1048         switch (chip->state) {
1049         case FL_CFI_QUERY:
1050         case FL_JEDEC_QUERY:
1051         case FL_READY:
1052                 map_write(map, CMD(0x70), adr);
1053                 chip->state = FL_STATUS;
1054                 fallthrough;
1055         case FL_STATUS:
1056                 status = map_read(map, adr);
1057                 if (map_word_andequal(map, status, status_OK, status_OK))
1058                         break;
1059
1060                 /* Urgh. Chip not yet ready to talk to us. */
1061                 if (time_after(jiffies, timeo)) {
1062                         mutex_unlock(&chip->mutex);
1063                         printk(KERN_ERR "waiting for chip to be ready timed out in lock\n");
1064                         return -EIO;
1065                 }
1066
1067                 /* Latency issues. Drop the lock, wait a while and retry */
1068                 mutex_unlock(&chip->mutex);
1069                 cfi_udelay(1);
1070                 goto retry;
1071
1072         default:
1073                 /* Stick ourselves on a wait queue to be woken when
1074                    someone changes the status */
1075                 set_current_state(TASK_UNINTERRUPTIBLE);
1076                 add_wait_queue(&chip->wq, &wait);
1077                 mutex_unlock(&chip->mutex);
1078                 schedule();
1079                 remove_wait_queue(&chip->wq, &wait);
1080                 timeo = jiffies + HZ;
1081                 goto retry;
1082         }
1083
1084         ENABLE_VPP(map);
1085         map_write(map, CMD(0x60), adr);
1086         map_write(map, CMD(0x01), adr);
1087         chip->state = FL_LOCKING;
1088
1089         mutex_unlock(&chip->mutex);
1090         msleep(1000);
1091         mutex_lock(&chip->mutex);
1092
1093         /* FIXME. Use a timer to check this, and return immediately. */
1094         /* Once the state machine's known to be working I'll do that */
1095
1096         timeo = jiffies + (HZ*2);
1097         for (;;) {
1098
1099                 status = map_read(map, adr);
1100                 if (map_word_andequal(map, status, status_OK, status_OK))
1101                         break;
1102
1103                 /* OK Still waiting */
1104                 if (time_after(jiffies, timeo)) {
1105                         map_write(map, CMD(0x70), adr);
1106                         chip->state = FL_STATUS;
1107                         printk(KERN_ERR "waiting for lock to complete timed out. Xstatus = %lx, status = %lx.\n", status.x[0], map_read(map, adr).x[0]);
1108                         DISABLE_VPP(map);
1109                         mutex_unlock(&chip->mutex);
1110                         return -EIO;
1111                 }
1112
1113                 /* Latency issues. Drop the lock, wait a while and retry */
1114                 mutex_unlock(&chip->mutex);
1115                 cfi_udelay(1);
1116                 mutex_lock(&chip->mutex);
1117         }
1118
1119         /* Done and happy. */
1120         chip->state = FL_STATUS;
1121         DISABLE_VPP(map);
1122         wake_up(&chip->wq);
1123         mutex_unlock(&chip->mutex);
1124         return 0;
1125 }
1126 static int cfi_staa_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
1127 {
1128         struct map_info *map = mtd->priv;
1129         struct cfi_private *cfi = map->fldrv_priv;
1130         unsigned long adr;
1131         int chipnum, ret;
1132 #ifdef DEBUG_LOCK_BITS
1133         int ofs_factor = cfi->interleave * cfi->device_type;
1134 #endif
1135
1136         if (ofs & (mtd->erasesize - 1))
1137                 return -EINVAL;
1138
1139         if (len & (mtd->erasesize -1))
1140                 return -EINVAL;
1141
1142         chipnum = ofs >> cfi->chipshift;
1143         adr = ofs - (chipnum << cfi->chipshift);
1144
1145         while(len) {
1146
1147 #ifdef DEBUG_LOCK_BITS
1148                 cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1149                 printk("before lock: block status register is %x\n",cfi_read_query(map, adr+(2*ofs_factor)));
1150                 cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1151 #endif
1152
1153                 ret = do_lock_oneblock(map, &cfi->chips[chipnum], adr);
1154
1155 #ifdef DEBUG_LOCK_BITS
1156                 cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1157                 printk("after lock: block status register is %x\n",cfi_read_query(map, adr+(2*ofs_factor)));
1158                 cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1159 #endif
1160
1161                 if (ret)
1162                         return ret;
1163
1164                 adr += mtd->erasesize;
1165                 len -= mtd->erasesize;
1166
1167                 if (adr >> cfi->chipshift) {
1168                         adr = 0;
1169                         chipnum++;
1170
1171                         if (chipnum >= cfi->numchips)
1172                                 break;
1173                 }
1174         }
1175         return 0;
1176 }
1177 static inline int do_unlock_oneblock(struct map_info *map, struct flchip *chip, unsigned long adr)
1178 {
1179         struct cfi_private *cfi = map->fldrv_priv;
1180         map_word status, status_OK;
1181         unsigned long timeo = jiffies + HZ;
1182         DECLARE_WAITQUEUE(wait, current);
1183
1184         adr += chip->start;
1185
1186         /* Let's determine this according to the interleave only once */
1187         status_OK = CMD(0x80);
1188
1189         timeo = jiffies + HZ;
1190 retry:
1191         mutex_lock(&chip->mutex);
1192
1193         /* Check that the chip's ready to talk to us. */
1194         switch (chip->state) {
1195         case FL_CFI_QUERY:
1196         case FL_JEDEC_QUERY:
1197         case FL_READY:
1198                 map_write(map, CMD(0x70), adr);
1199                 chip->state = FL_STATUS;
1200                 fallthrough;
1201         case FL_STATUS:
1202                 status = map_read(map, adr);
1203                 if (map_word_andequal(map, status, status_OK, status_OK))
1204                         break;
1205
1206                 /* Urgh. Chip not yet ready to talk to us. */
1207                 if (time_after(jiffies, timeo)) {
1208                         mutex_unlock(&chip->mutex);
1209                         printk(KERN_ERR "waiting for chip to be ready timed out in unlock\n");
1210                         return -EIO;
1211                 }
1212
1213                 /* Latency issues. Drop the lock, wait a while and retry */
1214                 mutex_unlock(&chip->mutex);
1215                 cfi_udelay(1);
1216                 goto retry;
1217
1218         default:
1219                 /* Stick ourselves on a wait queue to be woken when
1220                    someone changes the status */
1221                 set_current_state(TASK_UNINTERRUPTIBLE);
1222                 add_wait_queue(&chip->wq, &wait);
1223                 mutex_unlock(&chip->mutex);
1224                 schedule();
1225                 remove_wait_queue(&chip->wq, &wait);
1226                 timeo = jiffies + HZ;
1227                 goto retry;
1228         }
1229
1230         ENABLE_VPP(map);
1231         map_write(map, CMD(0x60), adr);
1232         map_write(map, CMD(0xD0), adr);
1233         chip->state = FL_UNLOCKING;
1234
1235         mutex_unlock(&chip->mutex);
1236         msleep(1000);
1237         mutex_lock(&chip->mutex);
1238
1239         /* FIXME. Use a timer to check this, and return immediately. */
1240         /* Once the state machine's known to be working I'll do that */
1241
1242         timeo = jiffies + (HZ*2);
1243         for (;;) {
1244
1245                 status = map_read(map, adr);
1246                 if (map_word_andequal(map, status, status_OK, status_OK))
1247                         break;
1248
1249                 /* OK Still waiting */
1250                 if (time_after(jiffies, timeo)) {
1251                         map_write(map, CMD(0x70), adr);
1252                         chip->state = FL_STATUS;
1253                         printk(KERN_ERR "waiting for unlock to complete timed out. Xstatus = %lx, status = %lx.\n", status.x[0], map_read(map, adr).x[0]);
1254                         DISABLE_VPP(map);
1255                         mutex_unlock(&chip->mutex);
1256                         return -EIO;
1257                 }
1258
1259                 /* Latency issues. Drop the unlock, wait a while and retry */
1260                 mutex_unlock(&chip->mutex);
1261                 cfi_udelay(1);
1262                 mutex_lock(&chip->mutex);
1263         }
1264
1265         /* Done and happy. */
1266         chip->state = FL_STATUS;
1267         DISABLE_VPP(map);
1268         wake_up(&chip->wq);
1269         mutex_unlock(&chip->mutex);
1270         return 0;
1271 }
1272 static int cfi_staa_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
1273 {
1274         struct map_info *map = mtd->priv;
1275         struct cfi_private *cfi = map->fldrv_priv;
1276         unsigned long adr;
1277         int chipnum, ret;
1278 #ifdef DEBUG_LOCK_BITS
1279         int ofs_factor = cfi->interleave * cfi->device_type;
1280 #endif
1281
1282         chipnum = ofs >> cfi->chipshift;
1283         adr = ofs - (chipnum << cfi->chipshift);
1284
1285 #ifdef DEBUG_LOCK_BITS
1286         {
1287                 unsigned long temp_adr = adr;
1288                 unsigned long temp_len = len;
1289
1290                 cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1291                 while (temp_len) {
1292                         printk("before unlock %x: block status register is %x\n",temp_adr,cfi_read_query(map, temp_adr+(2*ofs_factor)));
1293                         temp_adr += mtd->erasesize;
1294                         temp_len -= mtd->erasesize;
1295                 }
1296                 cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1297         }
1298 #endif
1299
1300         ret = do_unlock_oneblock(map, &cfi->chips[chipnum], adr);
1301
1302 #ifdef DEBUG_LOCK_BITS
1303         cfi_send_gen_cmd(0x90, 0x55, 0, map, cfi, cfi->device_type, NULL);
1304         printk("after unlock: block status register is %x\n",cfi_read_query(map, adr+(2*ofs_factor)));
1305         cfi_send_gen_cmd(0xff, 0x55, 0, map, cfi, cfi->device_type, NULL);
1306 #endif
1307
1308         return ret;
1309 }
1310
1311 static int cfi_staa_suspend(struct mtd_info *mtd)
1312 {
1313         struct map_info *map = mtd->priv;
1314         struct cfi_private *cfi = map->fldrv_priv;
1315         int i;
1316         struct flchip *chip;
1317         int ret = 0;
1318
1319         for (i=0; !ret && i<cfi->numchips; i++) {
1320                 chip = &cfi->chips[i];
1321
1322                 mutex_lock(&chip->mutex);
1323
1324                 switch(chip->state) {
1325                 case FL_READY:
1326                 case FL_STATUS:
1327                 case FL_CFI_QUERY:
1328                 case FL_JEDEC_QUERY:
1329                         chip->oldstate = chip->state;
1330                         chip->state = FL_PM_SUSPENDED;
1331                         /* No need to wake_up() on this state change -
1332                          * as the whole point is that nobody can do anything
1333                          * with the chip now anyway.
1334                          */
1335                 case FL_PM_SUSPENDED:
1336                         break;
1337
1338                 default:
1339                         ret = -EAGAIN;
1340                         break;
1341                 }
1342                 mutex_unlock(&chip->mutex);
1343         }
1344
1345         /* Unlock the chips again */
1346
1347         if (ret) {
1348                 for (i--; i >=0; i--) {
1349                         chip = &cfi->chips[i];
1350
1351                         mutex_lock(&chip->mutex);
1352
1353                         if (chip->state == FL_PM_SUSPENDED) {
1354                                 /* No need to force it into a known state here,
1355                                    because we're returning failure, and it didn't
1356                                    get power cycled */
1357                                 chip->state = chip->oldstate;
1358                                 wake_up(&chip->wq);
1359                         }
1360                         mutex_unlock(&chip->mutex);
1361                 }
1362         }
1363
1364         return ret;
1365 }
1366
1367 static void cfi_staa_resume(struct mtd_info *mtd)
1368 {
1369         struct map_info *map = mtd->priv;
1370         struct cfi_private *cfi = map->fldrv_priv;
1371         int i;
1372         struct flchip *chip;
1373
1374         for (i=0; i<cfi->numchips; i++) {
1375
1376                 chip = &cfi->chips[i];
1377
1378                 mutex_lock(&chip->mutex);
1379
1380                 /* Go to known state. Chip may have been power cycled */
1381                 if (chip->state == FL_PM_SUSPENDED) {
1382                         map_write(map, CMD(0xFF), 0);
1383                         chip->state = FL_READY;
1384                         wake_up(&chip->wq);
1385                 }
1386
1387                 mutex_unlock(&chip->mutex);
1388         }
1389 }
1390
1391 static void cfi_staa_destroy(struct mtd_info *mtd)
1392 {
1393         struct map_info *map = mtd->priv;
1394         struct cfi_private *cfi = map->fldrv_priv;
1395         kfree(cfi->cmdset_priv);
1396         kfree(cfi);
1397 }
1398
1399 MODULE_LICENSE("GPL");