target/linux/linux-2.4/patches/generic/005-mtd_flashtypes.patch

   1 diff -Nur linux-2.4.30/drivers/mtd/chips/Config.in linux-2.4.30.openwrt/drivers/mtd/chips/Config.in
   2 --- linux-2.4.30/drivers/mtd/chips/Config.in    2003-06-13 16:51:34.000000000 +0200
   3 +++ linux-2.4.30.openwrt/drivers/mtd/chips/Config.in    2005-06-28 11:26:26.000000000 +0200
   4 @@ -45,6 +45,7 @@
   5  dep_tristate '  Support for Intel/Sharp flash chips' CONFIG_MTD_CFI_INTELEXT $CONFIG_MTD_GEN_PROBE
   6  dep_tristate '  Support for AMD/Fujitsu flash chips' CONFIG_MTD_CFI_AMDSTD $CONFIG_MTD_GEN_PROBE
   7  dep_tristate '  Support for ST (Advanced Architecture) flash chips' CONFIG_MTD_CFI_STAA $CONFIG_MTD_GEN_PROBE
   8 +dep_tristate '  Support for SST flash chips' CONFIG_MTD_CFI_SSTSTD $CONFIG_MTD_GEN_PROBE
   9
  10  dep_tristate '  Support for RAM chips in bus mapping' CONFIG_MTD_RAM $CONFIG_MTD
  11  dep_tristate '  Support for ROM chips in bus mapping' CONFIG_MTD_ROM $CONFIG_MTD
  12 diff -Nur linux-2.4.30/drivers/mtd/chips/Makefile linux-2.4.30.openwrt/drivers/mtd/chips/Makefile
  13 --- linux-2.4.30/drivers/mtd/chips/Makefile     2003-06-13 16:51:34.000000000 +0200
  14 +++ linux-2.4.30.openwrt/drivers/mtd/chips/Makefile     2005-06-28 11:26:26.000000000 +0200
  15 @@ -18,6 +18,7 @@
  16  obj-$(CONFIG_MTD_AMDSTD)       += amd_flash.o
  17  obj-$(CONFIG_MTD_CFI)          += cfi_probe.o
  18  obj-$(CONFIG_MTD_CFI_STAA)     += cfi_cmdset_0020.o
  19 +obj-$(CONFIG_MTD_CFI_SSTSTD)   += cfi_cmdset_0701.o
  20  obj-$(CONFIG_MTD_CFI_AMDSTD)   += cfi_cmdset_0002.o
  21  obj-$(CONFIG_MTD_CFI_INTELEXT) += cfi_cmdset_0001.o
  22  obj-$(CONFIG_MTD_GEN_PROBE)    += gen_probe.o
  23 diff -Nur linux-2.4.30/drivers/mtd/chips/cfi_cmdset_0701.c linux-2.4.30.openwrt/drivers/mtd/chips/cfi_cmdset_0701.c
  24 --- linux-2.4.30/drivers/mtd/chips/cfi_cmdset_0701.c    1970-01-01 01:00:00.000000000 +0100
  25 +++ linux-2.4.30.openwrt/drivers/mtd/chips/cfi_cmdset_0701.c    2005-06-28 11:26:26.000000000 +0200
  26 @@ -0,0 +1,855 @@
  27 +/*
  28 + * Common Flash Interface support:
  29 + *   SST Standard Vendor Command Set (ID 0x0701)
  30 + *
  31 + * Copyright (C) 2000 Crossnet Co. <info@crossnet.co.jp>
  32 + *
  33 + * 2_by_8 routines added by Simon Munton
  34 + *
  35 + * This code is GPL
  36 + *
  37 + * $Id: cfi_cmdset_0701.c,v 1.1 2005/03/16 13:50:00 wbx Exp $
  38 + *
  39 + */
  40 +
  41 +#include <linux/module.h>
  42 +#include <linux/types.h>
  43 +#include <linux/kernel.h>
  44 +#include <linux/sched.h>
  45 +#include <asm/io.h>
  46 +#include <asm/byteorder.h>
  47 +
  48 +#include <linux/errno.h>
  49 +#include <linux/slab.h>
  50 +#include <linux/delay.h>
  51 +#include <linux/interrupt.h>
  52 +#include <linux/mtd/map.h>
  53 +#include <linux/mtd/cfi.h>
  54 +
  55 +static int cfi_sststd_read (struct mtd_info *, loff_t, size_t, size_t *, u_char *);
  56 +static int cfi_sststd_write(struct mtd_info *, loff_t, size_t, size_t *, const u_char *);
  57 +static int cfi_sststd_erase_onesize(struct mtd_info *, struct erase_info *);
  58 +static int cfi_sststd_erase_varsize(struct mtd_info *, struct erase_info *);
  59 +static void cfi_sststd_sync (struct mtd_info *);
  60 +static int cfi_sststd_suspend (struct mtd_info *);
  61 +static void cfi_sststd_resume (struct mtd_info *);
  62 +
  63 +static void cfi_sststd_destroy(struct mtd_info *);
  64 +
  65 +struct mtd_info *cfi_cmdset_0701(struct map_info *, int);
  66 +static struct mtd_info *cfi_sststd_setup (struct map_info *);
  67 +
  68 +
  69 +static struct mtd_chip_driver cfi_sststd_chipdrv = {
  70 +       probe: NULL, /* Not usable directly */
  71 +       destroy: cfi_sststd_destroy,
  72 +       name: "cfi_cmdset_0701",
  73 +       module: THIS_MODULE
  74 +};
  75 +
  76 +struct mtd_info *cfi_cmdset_0701(struct map_info *map, int primary)
  77 +{
  78 +       struct cfi_private *cfi = map->fldrv_priv;
  79 +       int ofs_factor = cfi->interleave * cfi->device_type;
  80 +       int i;
  81 +       __u8 major, minor;
  82 +       __u32 base = cfi->chips[0].start;
  83 +
  84 +       if (cfi->cfi_mode==1){
  85 +               __u16 adr = primary?cfi->cfiq->P_ADR:cfi->cfiq->A_ADR;
  86 +
  87 +               cfi_send_gen_cmd(0xAA, 0x5555, base, map, cfi, cfi->device_type, NULL);
  88 +               cfi_send_gen_cmd(0x55, 0x2AAA, base, map, cfi, cfi->device_type, NULL);
  89 +               cfi_send_gen_cmd(0x98, 0x5555, base, map, cfi, cfi->device_type, NULL);
  90 +
  91 +               major = cfi_read_query(map, base + (adr+3)*ofs_factor);
  92 +               minor = cfi_read_query(map, base + (adr+4)*ofs_factor);
  93 +
  94 +               printk(" SST Query Table v%c.%c at 0x%4.4X\n",
  95 +                      major, minor, adr);
  96 +               cfi_send_gen_cmd(0xf0, 0x5555, base, map, cfi, cfi->device_type, NULL);
  97 +
  98 +               cfi_send_gen_cmd(0xAA, 0x5555, base, map, cfi, cfi->device_type, NULL);
  99 +               cfi_send_gen_cmd(0x55, 0x2AAA, base, map, cfi, cfi->device_type, NULL);
 100 +               cfi_send_gen_cmd(0x90, 0x5555, base, map, cfi, cfi->device_type, NULL);
 101 +               cfi->mfr = cfi_read_query(map, base);
 102 +               cfi->id = cfi_read_query(map, base + ofs_factor);
 103 +
 104 +               cfi_send_gen_cmd(0xAA, 0x5555, base, map, cfi, cfi->device_type, NULL);
 105 +               cfi_send_gen_cmd(0x55, 0x2AAA, base, map, cfi, cfi->device_type, NULL);
 106 +               cfi_send_gen_cmd(0x98, 0x5555, base, map, cfi, cfi->device_type, NULL);
 107 +
 108 +               switch (cfi->device_type) {
 109 +               case CFI_DEVICETYPE_X16:
 110 +                       cfi->addr_unlock1 = 0x5555;
 111 +                       cfi->addr_unlock2 = 0x2AAA;
 112 +                       break;
 113 +               default:
 114 +                       printk(KERN_NOTICE "Eep. Unknown cfi_cmdset_0701 device type %d\n", cfi->device_type);
 115 +                       return NULL;
 116 +               }
 117 +       } /* CFI mode */
 118 +
 119 +       for (i=0; i< cfi->numchips; i++) {
 120 +               cfi->chips[i].word_write_time = 1<<cfi->cfiq->WordWriteTimeoutTyp;
 121 +               cfi->chips[i].buffer_write_time = 1<<cfi->cfiq->BufWriteTimeoutTyp;
 122 +               cfi->chips[i].erase_time = 1<<cfi->cfiq->BlockEraseTimeoutTyp;
 123 +       }
 124 +
 125 +       map->fldrv = &cfi_sststd_chipdrv;
 126 +       MOD_INC_USE_COUNT;
 127 +
 128 +       cfi_send_gen_cmd(0xf0, 0x5555, base, map, cfi, cfi->device_type, NULL);
 129 +       return cfi_sststd_setup(map);
 130 +}
 131 +
 132 +static struct mtd_info *cfi_sststd_setup(struct map_info *map)
 133 +{
 134 +       struct cfi_private *cfi = map->fldrv_priv;
 135 +       struct mtd_info *mtd;
 136 +       unsigned long devsize = (1<<cfi->cfiq->DevSize) * cfi->interleave;
 137 +
 138 +       mtd = kmalloc(sizeof(*mtd), GFP_KERNEL);
 139 +       printk("number of %s chips: %d\n", (cfi->cfi_mode)?"JEDEC":"CFI",cfi->numchips);
 140 +
 141 +       if (!mtd) {
 142 +         printk("Failed to allocate memory for MTD device\n");
 143 +         kfree(cfi->cmdset_priv);
 144 +         return NULL;
 145 +       }
 146 +
 147 +       memset(mtd, 0, sizeof(*mtd));
 148 +       mtd->priv = map;
 149 +       mtd->type = MTD_NORFLASH;
 150 +       /* Also select the correct geometry setup too */
 151 +       mtd->size = devsize * cfi->numchips;
 152 +
 153 +       if (cfi->cfiq->NumEraseRegions == 1) {
 154 +               /* No need to muck about with multiple erase sizes */
 155 +               mtd->erasesize = ((cfi->cfiq->EraseRegionInfo[0] >> 8) & ~0xff) * cfi->interleave;
 156 +       } else {
 157 +               unsigned long offset = 0;
 158 +               int i,j;
 159 +
 160 +               mtd->numeraseregions = cfi->cfiq->NumEraseRegions * cfi->numchips;
 161 +               mtd->eraseregions = kmalloc(sizeof(struct mtd_erase_region_info) * mtd->numeraseregions, GFP_KERNEL);
 162 +               if (!mtd->eraseregions) {
 163 +                       printk("Failed to allocate memory for MTD erase region info\n");
 164 +                       kfree(cfi->cmdset_priv);
 165 +                       return NULL;
 166 +               }
 167 +
 168 +               for (i=0; i<cfi->cfiq->NumEraseRegions; i++) {
 169 +                       unsigned long ernum, ersize;
 170 +                       ersize = ((cfi->cfiq->EraseRegionInfo[i] >> 8) & ~0xff) * cfi->interleave;
 171 +                       ernum = (cfi->cfiq->EraseRegionInfo[i] & 0xffff) + 1;
 172 +
 173 +                       if (mtd->erasesize < ersize) {
 174 +                               mtd->erasesize = ersize;
 175 +                       }
 176 +                       for (j=0; j<cfi->numchips; j++) {
 177 +                               mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].offset = (j*devsize)+offset;
 178 +                               mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].erasesize = ersize;
 179 +                               mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].numblocks = ernum;
 180 +                       }
 181 +                       offset += (ersize * ernum);
 182 +               }
 183 +
 184 +               // debug
 185 +               for (i=0; i<mtd->numeraseregions;i++){
 186 +                       printk("%d: offset=0x%x,size=0x%x,blocks=%d\n",
 187 +                              i,mtd->eraseregions[i].offset,
 188 +                              mtd->eraseregions[i].erasesize,
 189 +                              mtd->eraseregions[i].numblocks);
 190 +               }
 191 +       }
 192 +
 193 +       switch (CFIDEV_BUSWIDTH)
 194 +       {
 195 +       case 1:
 196 +       case 2:
 197 +       case 4:
 198 +               if (mtd->numeraseregions > 1)
 199 +                       mtd->erase = cfi_sststd_erase_varsize;
 200 +               else
 201 +                       mtd->erase = cfi_sststd_erase_onesize;
 202 +               mtd->read = cfi_sststd_read;
 203 +               mtd->write = cfi_sststd_write;
 204 +               break;
 205 +
 206 +       default:
 207 +               printk("Unsupported buswidth\n");
 208 +               kfree(mtd);
 209 +               kfree(cfi->cmdset_priv);
 210 +               return NULL;
 211 +               break;
 212 +       }
 213 +       mtd->sync = cfi_sststd_sync;
 214 +       mtd->suspend = cfi_sststd_suspend;
 215 +       mtd->resume = cfi_sststd_resume;
 216 +       mtd->flags = MTD_CAP_NORFLASH;
 217 +       map->fldrv = &cfi_sststd_chipdrv;
 218 +       mtd->name = map->name;
 219 +       MOD_INC_USE_COUNT;
 220 +       return mtd;
 221 +}
 222 +
 223 +static inline int do_read_onechip(struct map_info *map, struct flchip *chip, loff_t adr, size_t len, u_char *buf)
 224 +{
 225 +       DECLARE_WAITQUEUE(wait, current);
 226 +       unsigned long timeo = jiffies + HZ;
 227 +
 228 + retry:
 229 +       cfi_spin_lock(chip->mutex);
 230 +
 231 +       if (chip->state != FL_READY){
 232 +               printk("Waiting for chip to read, status = %d\n", chip->state);
 233 +               set_current_state(TASK_UNINTERRUPTIBLE);
 234 +               add_wait_queue(&chip->wq, &wait);
 235 +
 236 +               cfi_spin_unlock(chip->mutex);
 237 +
 238 +               schedule();
 239 +               remove_wait_queue(&chip->wq, &wait);
 240 +               timeo = jiffies + HZ;
 241 +
 242 +               goto retry;
 243 +       }
 244 +
 245 +       adr += chip->start;
 246 +
 247 +       chip->state = FL_READY;
 248 +
 249 +       map->copy_from(map, buf, adr, len);
 250 +
 251 +       wake_up(&chip->wq);
 252 +       cfi_spin_unlock(chip->mutex);
 253 +
 254 +       return 0;
 255 +}
 256 +
 257 +static int cfi_sststd_read (struct mtd_info *mtd, loff_t from, size_t len, size_t *retlen, u_char *buf)
 258 +{
 259 +       struct map_info *map = mtd->priv;
 260 +       struct cfi_private *cfi = map->fldrv_priv;
 261 +       unsigned long ofs;
 262 +       int chipnum;
 263 +       int ret = 0;
 264 +
 265 +       /* ofs: offset within the first chip that the first read should start */
 266 +
 267 +       chipnum = (from >> cfi->chipshift);
 268 +       ofs = from - (chipnum <<  cfi->chipshift);
 269 +
 270 +
 271 +       *retlen = 0;
 272 +
 273 +       while (len) {
 274 +               unsigned long thislen;
 275 +
 276 +               if (chipnum >= cfi->numchips)
 277 +                       break;
 278 +
 279 +               if ((len + ofs -1) >> cfi->chipshift)
 280 +                       thislen = (1<<cfi->chipshift) - ofs;
 281 +               else
 282 +                       thislen = len;
 283 +
 284 +               ret = do_read_onechip(map, &cfi->chips[chipnum], ofs, thislen, buf);
 285 +               if (ret)
 286 +                       break;
 287 +
 288 +               *retlen += thislen;
 289 +               len -= thislen;
 290 +               buf += thislen;
 291 +
 292 +               ofs = 0;
 293 +               chipnum++;
 294 +       }
 295 +       return ret;
 296 +}
 297 +
 298 +static int do_write_oneword(struct map_info *map, struct flchip *chip, unsigned long adr, __u32 datum, int fast)
 299 +{
 300 +       unsigned long timeo = jiffies + HZ;
 301 +       unsigned int Last[4];
 302 +       unsigned long Count = 0;
 303 +       struct cfi_private *cfi = map->fldrv_priv;
 304 +       DECLARE_WAITQUEUE(wait, current);
 305 +       int ret = 0;
 306 +
 307 + retry:
 308 +       cfi_spin_lock(chip->mutex);
 309 +
 310 +       if (chip->state != FL_READY){
 311 +               printk("Waiting for chip to write, status = %d\n", chip->state);
 312 +               set_current_state(TASK_UNINTERRUPTIBLE);
 313 +               add_wait_queue(&chip->wq, &wait);
 314 +
 315 +               cfi_spin_unlock(chip->mutex);
 316 +
 317 +               schedule();
 318 +               remove_wait_queue(&chip->wq, &wait);
 319 +               printk("Wake up to write:\n");
 320 +               timeo = jiffies + HZ;
 321 +
 322 +               goto retry;
 323 +       }
 324 +
 325 +       chip->state = FL_WRITING;
 326 +
 327 +       adr += chip->start;
 328 +       ENABLE_VPP(map);
 329 +    cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map, cfi, CFI_DEVICETYPE_X16, NULL);
 330 +    cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chip->start, map, cfi, CFI_DEVICETYPE_X16, NULL);
 331 +    cfi_send_gen_cmd(0xA0, cfi->addr_unlock1, chip->start, map, cfi, CFI_DEVICETYPE_X16, NULL);
 332 +
 333 +       cfi_write(map, datum, adr);
 334 +
 335 +       cfi_spin_unlock(chip->mutex);
 336 +       cfi_udelay(chip->word_write_time);
 337 +       cfi_spin_lock(chip->mutex);
 338 +
 339 +       Last[0] = cfi_read(map, adr);
 340 +       //      printk("Last[0] is %x\n", Last[0]);
 341 +       Last[1] = cfi_read(map, adr);
 342 +       //      printk("Last[1] is %x\n", Last[1]);
 343 +       Last[2] = cfi_read(map, adr);
 344 +       //      printk("Last[2] is %x\n", Last[2]);
 345 +
 346 +       for (Count = 3; Last[(Count - 1) % 4] != Last[(Count - 2) % 4] && Count < 10000; Count++){
 347 +               cfi_spin_unlock(chip->mutex);
 348 +               cfi_udelay(10);
 349 +               cfi_spin_lock(chip->mutex);
 350 +
 351 +               Last[Count % 4] = cfi_read(map, adr);
 352 +               //              printk("Last[%d%%4] is %x\n", Count, Last[Count%4]);
 353 +       }
 354 +
 355 +       if (Last[(Count - 1) % 4] != datum){
 356 +               printk("Last[%ld] is %x, datum is %x\n",(Count - 1) % 4,Last[(Count - 1) % 4],datum);
 357 +               cfi_send_gen_cmd(0xF0, 0, chip->start, map, cfi, cfi->device_type, NULL);
 358 +               DISABLE_VPP(map);
 359 +               ret = -EIO;
 360 +       }
 361 +       DISABLE_VPP(map);
 362 +       chip->state = FL_READY;
 363 +       wake_up(&chip->wq);
 364 +       cfi_spin_unlock(chip->mutex);
 365 +
 366 +       return ret;
 367 +}
 368 +
 369 +static int cfi_sststd_write (struct mtd_info *mtd, loff_t to , size_t len, size_t *retlen, const u_char *buf)
 370 +{
 371 +       struct map_info *map = mtd->priv;
 372 +       struct cfi_private *cfi = map->fldrv_priv;
 373 +       int ret = 0;
 374 +       int chipnum;
 375 +       unsigned long ofs, chipstart;
 376 +
 377 +       *retlen = 0;
 378 +       if (!len)
 379 +               return 0;
 380 +
 381 +       chipnum = to >> cfi->chipshift;
 382 +       ofs = to  - (chipnum << cfi->chipshift);
 383 +       chipstart = cfi->chips[chipnum].start;
 384 +
 385 +       /* If it's not bus-aligned, do the first byte write */
 386 +       if (ofs & (CFIDEV_BUSWIDTH-1)) {
 387 +               unsigned long bus_ofs = ofs & ~(CFIDEV_BUSWIDTH-1);
 388 +               int i = ofs - bus_ofs;
 389 +               int n = 0;
 390 +               u_char tmp_buf[4];
 391 +               __u32 datum;
 392 +
 393 +               map->copy_from(map, tmp_buf, bus_ofs + cfi->chips[chipnum].start, CFIDEV_BUSWIDTH);
 394 +               while (len && i < CFIDEV_BUSWIDTH)
 395 +                       tmp_buf[i++] = buf[n++], len--;
 396 +
 397 +               if (cfi_buswidth_is_2()) {
 398 +                       datum = *(__u16*)tmp_buf;
 399 +               } else if (cfi_buswidth_is_4()) {
 400 +                       datum = *(__u32*)tmp_buf;
 401 +               } else {
 402 +                       return -EINVAL;  /* should never happen, but be safe */
 403 +               }
 404 +
 405 +               ret = do_write_oneword(map, &cfi->chips[chipnum],
 406 +                               bus_ofs, datum, 0);
 407 +               if (ret)
 408 +                       return ret;
 409 +
 410 +               ofs += n;
 411 +               buf += n;
 412 +               (*retlen) += n;
 413 +
 414 +               if (ofs >> cfi->chipshift) {
 415 +                       chipnum ++;
 416 +                       ofs = 0;
 417 +                       if (chipnum == cfi->numchips)
 418 +                               return 0;
 419 +               }
 420 +       }
 421 +
 422 +       /* We are now aligned, write as much as possible */
 423 +       while(len >= CFIDEV_BUSWIDTH) {
 424 +               __u32 datum;
 425 +
 426 +               if (cfi_buswidth_is_1()) {
 427 +                       datum = *(__u8*)buf;
 428 +               } else if (cfi_buswidth_is_2()) {
 429 +                       datum = *(__u16*)buf;
 430 +               } else if (cfi_buswidth_is_4()) {
 431 +                       datum = *(__u32*)buf;
 432 +               } else {
 433 +                       return -EINVAL;
 434 +               }
 435 +               ret = do_write_oneword(map, &cfi->chips[chipnum],
 436 +                                      ofs, datum, cfi->fast_prog);
 437 +               if (ret) {
 438 +                       return ret;
 439 +               }
 440 +
 441 +               ofs += CFIDEV_BUSWIDTH;
 442 +               buf += CFIDEV_BUSWIDTH;
 443 +               (*retlen) += CFIDEV_BUSWIDTH;
 444 +               len -= CFIDEV_BUSWIDTH;
 445 +
 446 +               if (ofs >> cfi->chipshift) {
 447 +                       chipnum ++;
 448 +                       ofs = 0;
 449 +                       if (chipnum == cfi->numchips)
 450 +                               return 0;
 451 +                       chipstart = cfi->chips[chipnum].start;
 452 +               }
 453 +       }
 454 +
 455 +       if (len & (CFIDEV_BUSWIDTH-1)) {
 456 +               int i = 0, n = 0;
 457 +               u_char tmp_buf[4];
 458 +               __u32 datum;
 459 +
 460 +               map->copy_from(map, tmp_buf, ofs + cfi->chips[chipnum].start, CFIDEV_BUSWIDTH);
 461 +               while (len--)
 462 +                       tmp_buf[i++] = buf[n++];
 463 +
 464 +               if (cfi_buswidth_is_2()) {
 465 +                       datum = *(__u16*)tmp_buf;
 466 +               } else if (cfi_buswidth_is_4()) {
 467 +                       datum = *(__u32*)tmp_buf;
 468 +               } else {
 469 +                       return -EINVAL;  /* should never happen, but be safe */
 470 +               }
 471 +
 472 +               ret = do_write_oneword(map, &cfi->chips[chipnum],
 473 +                               ofs, datum, 0);
 474 +               if (ret)
 475 +                       return ret;
 476 +
 477 +               (*retlen) += n;
 478 +       }
 479 +
 480 +       return 0;
 481 +}
 482 +
 483 +static inline int do_erase_oneblock(struct map_info *map, struct flchip *chip, unsigned long adr)
 484 +{
 485 +       unsigned int status;
 486 +       unsigned long timeo = jiffies + HZ;
 487 +       struct cfi_private *cfi = map->fldrv_priv;
 488 +       unsigned int rdy_mask;
 489 +       DECLARE_WAITQUEUE(wait, current);
 490 +
 491 + retry:
 492 +       cfi_spin_lock(chip->mutex);
 493 +
 494 +       if (chip->state != FL_READY){
 495 +               set_current_state(TASK_UNINTERRUPTIBLE);
 496 +               add_wait_queue(&chip->wq, &wait);
 497 +
 498 +               cfi_spin_unlock(chip->mutex);
 499 +
 500 +               schedule();
 501 +               remove_wait_queue(&chip->wq, &wait);
 502 +               timeo = jiffies + HZ;
 503 +
 504 +               goto retry;
 505 +       }
 506 +
 507 +       chip->state = FL_ERASING;
 508 +
 509 +       adr += chip->start;
 510 +       ENABLE_VPP(map);
 511 +       cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map, cfi, CFI_DEVICETYPE_X16, NULL);
 512 +       cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chip->start, map, cfi, CFI_DEVICETYPE_X16, NULL);
 513 +       cfi_send_gen_cmd(0x80, cfi->addr_unlock1, chip->start, map, cfi, CFI_DEVICETYPE_X16, NULL);
 514 +       cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map, cfi, CFI_DEVICETYPE_X16, NULL);
 515 +       cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chip->start, map, cfi, CFI_DEVICETYPE_X16, NULL);
 516 +       cfi_write(map, CMD(0x30), adr);
 517 +
 518 +       timeo = jiffies + (HZ*20);
 519 +
 520 +       cfi_spin_unlock(chip->mutex);
 521 +       schedule_timeout(HZ);
 522 +       cfi_spin_lock(chip->mutex);
 523 +
 524 +       rdy_mask = CMD(0x80);
 525 +
 526 +       /* Once the state machine's known to be working I'll do that */
 527 +
 528 +       while ( ( (status = cfi_read(map,adr)) & rdy_mask ) != rdy_mask ) {
 529 +               static int z=0;
 530 +
 531 +               if (chip->state != FL_ERASING) {
 532 +                       /* Someone's suspended the erase. Sleep */
 533 +                       set_current_state(TASK_UNINTERRUPTIBLE);
 534 +                       add_wait_queue(&chip->wq, &wait);
 535 +
 536 +                       cfi_spin_unlock(chip->mutex);
 537 +                       printk("erase suspended. Sleeping\n");
 538 +
 539 +                       schedule();
 540 +                       remove_wait_queue(&chip->wq, &wait);
 541 +                       timeo = jiffies + (HZ*2);
 542 +                       cfi_spin_lock(chip->mutex);
 543 +                       continue;
 544 +               }
 545 +
 546 +               /* OK Still waiting */
 547 +               if (time_after(jiffies, timeo)) {
 548 +                       chip->state = FL_READY;
 549 +                       cfi_spin_unlock(chip->mutex);
 550 +                       printk("waiting for erase to complete timed out.");
 551 +                       DISABLE_VPP(map);
 552 +                       return -EIO;
 553 +               }
 554 +
 555 +               /* Latency issues. Drop the lock, wait a while and retry */
 556 +               cfi_spin_unlock(chip->mutex);
 557 +
 558 +               z++;
 559 +               if ( 0 && !(z % 100 ))
 560 +                       printk("chip not ready yet after erase. looping\n");
 561 +
 562 +               cfi_udelay(1);
 563 +
 564 +               cfi_spin_lock(chip->mutex);
 565 +               continue;
 566 +       }
 567 +
 568 +       /* Done and happy. */
 569 +       DISABLE_VPP(map);
 570 +       chip->state = FL_READY;
 571 +       wake_up(&chip->wq);
 572 +       cfi_spin_unlock(chip->mutex);
 573 +       return 0;
 574 +}
 575 +
 576 +static int cfi_sststd_erase_varsize(struct mtd_info *mtd, struct erase_info *instr)
 577 +{
 578 +       struct map_info *map = mtd->priv;
 579 +       struct cfi_private *cfi = map->fldrv_priv;
 580 +       unsigned long adr, len;
 581 +       int chipnum, ret = 0;
 582 +       int i, first;
 583 +       struct mtd_erase_region_info *regions = mtd->eraseregions;
 584 +
 585 +       if (instr->addr > mtd->size)
 586 +               return -EINVAL;
 587 +
 588 +       if ((instr->len + instr->addr) > mtd->size)
 589 +               return -EINVAL;
 590 +
 591 +       /* Check that both start and end of the requested erase are
 592 +        * aligned with the erasesize at the appropriate addresses.
 593 +        */
 594 +
 595 +       i = 0;
 596 +
 597 +       /* Skip all erase regions which are ended before the start of
 598 +          the requested erase. Actually, to save on the calculations,
 599 +          we skip to the first erase region which starts after the
 600 +          start of the requested erase, and then go back one.
 601 +       */
 602 +
 603 +       while (i < mtd->numeraseregions && instr->addr >= regions[i].offset)
 604 +              i++;
 605 +       i--;
 606 +
 607 +       /* OK, now i is pointing at the erase region in which this
 608 +          erase request starts. Check the start of the requested
 609 +          erase range is aligned with the erase size which is in
 610 +          effect here.
 611 +       */
 612 +
 613 +       if (instr->addr & (regions[i].erasesize-1))
 614 +               return -EINVAL;
 615 +
 616 +       /* Remember the erase region we start on */
 617 +       first = i;
 618 +
 619 +       /* Next, check that the end of the requested erase is aligned
 620 +        * with the erase region at that address.
 621 +        */
 622 +
 623 +       while (i<mtd->numeraseregions && (instr->addr + instr->len) >= regions[i].offset)
 624 +               i++;
 625 +
 626 +       /* As before, drop back one to point at the region in which
 627 +          the address actually falls
 628 +       */
 629 +       i--;
 630 +
 631 +       if ((instr->addr + instr->len) & (regions[i].erasesize-1))
 632 +               return -EINVAL;
 633 +
 634 +       chipnum = instr->addr >> cfi->chipshift;
 635 +       adr = instr->addr - (chipnum << cfi->chipshift);
 636 +       len = instr->len;
 637 +
 638 +       i=first;
 639 +
 640 +       while(len) {
 641 +               ret = do_erase_oneblock(map, &cfi->chips[chipnum], adr);
 642 +
 643 +               if (ret)
 644 +                       return ret;
 645 +
 646 +               adr += regions[i].erasesize;
 647 +               len -= regions[i].erasesize;
 648 +
 649 +               if (adr % (1<< cfi->chipshift) == ((regions[i].offset + (regions[i].erasesize * regions[i].numblocks)) %( 1<< cfi->chipshift)))
 650 +                       i++;
 651 +
 652 +               if (adr >> cfi->chipshift) {
 653 +                       adr = 0;
 654 +                       chipnum++;
 655 +
 656 +                       if (chipnum >= cfi->numchips)
 657 +                       break;
 658 +               }
 659 +       }
 660 +
 661 +       instr->state = MTD_ERASE_DONE;
 662 +       if (instr->callback)
 663 +               instr->callback(instr);
 664 +
 665 +       return 0;
 666 +}
 667 +
 668 +static int cfi_sststd_erase_onesize(struct mtd_info *mtd, struct erase_info *instr)
 669 +{
 670 +       struct map_info *map = mtd->priv;
 671 +       struct cfi_private *cfi = map->fldrv_priv;
 672 +       unsigned long adr, len;
 673 +       int chipnum, ret = 0;
 674 +
 675 +       if (instr->addr & (mtd->erasesize - 1))
 676 +               return -EINVAL;
 677 +
 678 +       if (instr->len & (mtd->erasesize -1))
 679 +               return -EINVAL;
 680 +
 681 +       if ((instr->len + instr->addr) > mtd->size)
 682 +               return -EINVAL;
 683 +
 684 +       chipnum = instr->addr >> cfi->chipshift;
 685 +       adr = instr->addr - (chipnum << cfi->chipshift);
 686 +       len = instr->len;
 687 +
 688 +       while(len) {
 689 +               ret = do_erase_oneblock(map, &cfi->chips[chipnum], adr);
 690 +
 691 +               if (ret)
 692 +                       return ret;
 693 +
 694 +               adr += mtd->erasesize;
 695 +               len -= mtd->erasesize;
 696 +
 697 +               if (adr >> cfi->chipshift) {
 698 +                       adr = 0;
 699 +                       chipnum++;
 700 +
 701 +                       if (chipnum >= cfi->numchips)
 702 +                       break;
 703 +               }
 704 +       }
 705 +
 706 +       instr->state = MTD_ERASE_DONE;
 707 +       if (instr->callback)
 708 +               instr->callback(instr);
 709 +
 710 +       return 0;
 711 +}
 712 +
 713 +static void cfi_sststd_sync (struct mtd_info *mtd)
 714 +{
 715 +       struct map_info *map = mtd->priv;
 716 +       struct cfi_private *cfi = map->fldrv_priv;
 717 +       int i;
 718 +       struct flchip *chip;
 719 +       int ret = 0;
 720 +       DECLARE_WAITQUEUE(wait, current);
 721 +
 722 +       for (i=0; !ret && i<cfi->numchips; i++) {
 723 +               chip = &cfi->chips[i];
 724 +
 725 +       retry:
 726 +               cfi_spin_lock(chip->mutex);
 727 +
 728 +               switch(chip->state) {
 729 +               case FL_READY:
 730 +               case FL_STATUS:
 731 +               case FL_CFI_QUERY:
 732 +               case FL_JEDEC_QUERY:
 733 +                       chip->oldstate = chip->state;
 734 +                       chip->state = FL_SYNCING;
 735 +                       /* No need to wake_up() on this state change -
 736 +                        * as the whole point is that nobody can do anything
 737 +                        * with the chip now anyway.
 738 +                        */
 739 +               case FL_SYNCING:
 740 +                       cfi_spin_unlock(chip->mutex);
 741 +                       break;
 742 +
 743 +               default:
 744 +                       /* Not an idle state */
 745 +                       add_wait_queue(&chip->wq, &wait);
 746 +
 747 +                       cfi_spin_unlock(chip->mutex);
 748 +
 749 +                       schedule();
 750 +
 751 +                       remove_wait_queue(&chip->wq, &wait);
 752 +
 753 +                       goto retry;
 754 +               }
 755 +       }
 756 +
 757 +       /* Unlock the chips again */
 758 +
 759 +       for (i--; i >=0; i--) {
 760 +               chip = &cfi->chips[i];
 761 +
 762 +               cfi_spin_lock(chip->mutex);
 763 +
 764 +               if (chip->state == FL_SYNCING) {
 765 +                       chip->state = chip->oldstate;
 766 +                       wake_up(&chip->wq);
 767 +               }
 768 +               cfi_spin_unlock(chip->mutex);
 769 +       }
 770 +}
 771 +
 772 +
 773 +static int cfi_sststd_suspend(struct mtd_info *mtd)
 774 +{
 775 +       struct map_info *map = mtd->priv;
 776 +       struct cfi_private *cfi = map->fldrv_priv;
 777 +       int i;
 778 +       struct flchip *chip;
 779 +       int ret = 0;
 780 +//printk("suspend\n");
 781 +
 782 +       for (i=0; !ret && i<cfi->numchips; i++) {
 783 +               chip = &cfi->chips[i];
 784 +
 785 +               cfi_spin_lock(chip->mutex);
 786 +
 787 +               switch(chip->state) {
 788 +               case FL_READY:
 789 +               case FL_STATUS:
 790 +               case FL_CFI_QUERY:
 791 +               case FL_JEDEC_QUERY:
 792 +                       chip->oldstate = chip->state;
 793 +                       chip->state = FL_PM_SUSPENDED;
 794 +                       /* No need to wake_up() on this state change -
 795 +                        * as the whole point is that nobody can do anything
 796 +                        * with the chip now anyway.
 797 +                        */
 798 +               case FL_PM_SUSPENDED:
 799 +                       break;
 800 +
 801 +               default:
 802 +                       ret = -EAGAIN;
 803 +                       break;
 804 +               }
 805 +               cfi_spin_unlock(chip->mutex);
 806 +       }
 807 +
 808 +       /* Unlock the chips again */
 809 +
 810 +       if (ret) {
 811 +               for (i--; i >=0; i--) {
 812 +                       chip = &cfi->chips[i];
 813 +
 814 +                       cfi_spin_lock(chip->mutex);
 815 +
 816 +                       if (chip->state == FL_PM_SUSPENDED) {
 817 +                               chip->state = chip->oldstate;
 818 +                               wake_up(&chip->wq);
 819 +                       }
 820 +                       cfi_spin_unlock(chip->mutex);
 821 +               }
 822 +       }
 823 +
 824 +       return ret;
 825 +}
 826 +
 827 +static void cfi_sststd_resume(struct mtd_info *mtd)
 828 +{
 829 +       struct map_info *map = mtd->priv;
 830 +       struct cfi_private *cfi = map->fldrv_priv;
 831 +       int i;
 832 +       struct flchip *chip;
 833 +//printk("resume\n");
 834 +
 835 +       for (i=0; i<cfi->numchips; i++) {
 836 +
 837 +               chip = &cfi->chips[i];
 838 +
 839 +               cfi_spin_lock(chip->mutex);
 840 +
 841 +               if (chip->state == FL_PM_SUSPENDED) {
 842 +                       chip->state = FL_READY;
 843 +                       cfi_write(map, CMD(0xF0), chip->start);
 844 +                       wake_up(&chip->wq);
 845 +               }
 846 +               else
 847 +                       printk("Argh. Chip not in PM_SUSPENDED state upon resume()\n");
 848 +
 849 +               cfi_spin_unlock(chip->mutex);
 850 +       }
 851 +}
 852 +
 853 +static void cfi_sststd_destroy(struct mtd_info *mtd)
 854 +{
 855 +       struct map_info *map = mtd->priv;
 856 +       struct cfi_private *cfi = map->fldrv_priv;
 857 +       kfree(cfi->cmdset_priv);
 858 +       kfree(cfi);
 859 +}
 860 +
 861 +#if LINUX_VERSION_CODE < 0x20212 && defined(MODULE)
 862 +#define cfi_sststd_init init_module
 863 +#define cfi_sststd_exit cleanup_module
 864 +#endif
 865 +
 866 +static char im_name[]="cfi_cmdset_0701";
 867 +
 868 +mod_init_t cfi_sststd_init(void)
 869 +{
 870 +       inter_module_register(im_name, THIS_MODULE, &cfi_cmdset_0701);
 871 +       return 0;
 872 +}
 873 +
 874 +mod_exit_t cfi_sststd_exit(void)
 875 +{
 876 +       inter_module_unregister(im_name);
 877 +}
 878 +
 879 +module_init(cfi_sststd_init);
 880 +module_exit(cfi_sststd_exit);
 881 +
 882 diff -Nur linux-2.4.30/drivers/mtd/chips/cfi_probe.c linux-2.4.30.openwrt/drivers/mtd/chips/cfi_probe.c
 883 --- linux-2.4.30/drivers/mtd/chips/cfi_probe.c  2003-06-13 16:51:34.000000000 +0200
 884 +++ linux-2.4.30.openwrt/drivers/mtd/chips/cfi_probe.c  2005-06-28 11:26:26.000000000 +0200
 885 @@ -67,8 +67,15 @@
 886         cfi_send_gen_cmd(0xF0, 0, base, map, cfi, cfi->device_type, NULL);
 887         cfi_send_gen_cmd(0x98, 0x55, base, map, cfi, cfi->device_type, NULL);
 888
 889 -       if (!qry_present(map,base,cfi))
 890 -               return 0;
 891 +       if (!qry_present(map,base,cfi)) {
 892 +               /* rather broken SST cfi probe (requires SST unlock) */
 893 +               cfi_send_gen_cmd(0xF0, 0, base, map, cfi, cfi->device_type, NULL);
 894 +               cfi_send_gen_cmd(0xAA, 0x5555, base, map, cfi, cfi->device_type, NULL);
 895 +               cfi_send_gen_cmd(0x55, 0x2AAA, base, map, cfi, cfi->device_type, NULL);
 896 +               cfi_send_gen_cmd(0x98, 0x5555, base, map, cfi, cfi->device_type, NULL);
 897 +               if (!qry_present(map,base,cfi))
 898 +                       return 0;
 899 +       }
 900
 901         if (!cfi->numchips) {
 902                 /* This is the first time we're called. Set up the CFI
 903 diff -Nur linux-2.4.30/drivers/mtd/chips/gen_probe.c linux-2.4.30.openwrt/drivers/mtd/chips/gen_probe.c
 904 --- linux-2.4.30/drivers/mtd/chips/gen_probe.c  2003-08-25 13:44:42.000000000 +0200
 905 +++ linux-2.4.30.openwrt/drivers/mtd/chips/gen_probe.c  2005-06-28 11:29:23.000000000 +0200
 906 @@ -328,13 +328,18 @@
 907                 return cfi_cmdset_0001(map, primary);
 908  #endif
 909  #ifdef CONFIG_MTD_CFI_AMDSTD
 910 +       case 0x0006:
 911         case 0x0002:
 912                 return cfi_cmdset_0002(map, primary);
 913  #endif
 914  #ifdef CONFIG_MTD_CFI_STAA
 915 -        case 0x0020:
 916 +       case 0x0020:
 917                 return cfi_cmdset_0020(map, primary);
 918  #endif
 919 +#ifdef CONFIG_MTD_CFI_SSTSTD
 920 +       case 0x0701:
 921 +               return cfi_cmdset_0701(map, primary);
 922 +#endif
 923         }
 924
 925         return cfi_cmdset_unknown(map, primary);