release/src-rt-6.x.4708/cfe/cfe/dev/dev_flashop_engine.c

   1 /*  *********************************************************************
   2     *  Broadcom Common Firmware Environment (CFE)
   3     *
   4     *  Flash engine     File: dev_flashop_engine.c
   5     *
   6     *  Author:  Mitch Lichtenberg (mpl@broadcom.com)
   7     *
   8     *********************************************************************
   9     *
  10     *  Copyright 2000,2001,2002,2003
  11     *  Broadcom Corporation. All rights reserved.
  12     *
  13     *  This software is furnished under license and may be used and
  14     *  copied only in accordance with the following terms and
  15     *  conditions.  Subject to these conditions, you may download,
  16     *  copy, install, use, modify and distribute modified or unmodified
  17     *  copies of this software in source and/or binary form.  No title
  18     *  or ownership is transferred hereby.
  19     *
  20     *  1) Any source code used, modified or distributed must reproduce
  21     *     and retain this copyright notice and list of conditions
  22     *     as they appear in the source file.
  23     *
  24     *  2) No right is granted to use any trade name, trademark, or
  25     *     logo of Broadcom Corporation.  The "Broadcom Corporation"
  26     *     name may not be used to endorse or promote products derived
  27     *     from this software without the prior written permission of
  28     *     Broadcom Corporation.
  29     *
  30     *  3) THIS SOFTWARE IS PROVIDED "AS-IS" AND ANY EXPRESS OR
  31     *     IMPLIED WARRANTIES, INCLUDING BUT NOT LIMITED TO, ANY IMPLIED
  32     *     WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
  33     *     PURPOSE, OR NON-INFRINGEMENT ARE DISCLAIMED. IN NO EVENT
  34     *     SHALL BROADCOM BE LIABLE FOR ANY DAMAGES WHATSOEVER, AND IN
  35     *     PARTICULAR, BROADCOM SHALL NOT BE LIABLE FOR DIRECT, INDIRECT,
  36     *     INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  37     *     (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
  38     *     GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
  39     *     BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
  40     *     OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
  41     *     TORT (INCLUDING NEGLIGENCE OR OTHERWISE), EVEN IF ADVISED OF
  42     *     THE POSSIBILITY OF SUCH DAMAGE.
  43     ********************************************************************* */
  44
  45 #include "lib_types.h"
  46 #include "lib_malloc.h"
  47 #include "lib_printf.h"
  48 #include "lib_string.h"
  49 #include "lib_physio.h"
  50 #include "addrspace.h"
  51 #include "cfe_iocb.h"
  52 #include "cfe_device.h"
  53 #include "cfe_ioctl.h"
  54 #include "cfe_error.h"
  55
  56 #include "bsp_config.h"
  57 #include "dev_newflash.h"
  58
  59 int flashop_engine(flashinstr_t *prog);
  60
  61 /* Not relocatable */
  62 int flashop_engine_len = 0;
  63 void *flashop_engine_ptr = (void *) flashop_engine;
  64
  65 /*  *********************************************************************
  66     *  Macros
  67     ********************************************************************* */
  68
  69 #define min(a,b) ((a) < (b) ? (a) : (b))
  70 #define max(a,b) ((a) > (b) ? (a) : (b))
  71 #define roundup(a,b) (((a)+((b)-1))/(b)*(b))
  72
  73 #if defined(__MIPSEB) && defined(_MIPSEB_DATA_INVARIANT_)
  74 /*
  75  * When the order of address bits matters (such as when writing
  76  * commands or reading magic registers), we want to use the address
  77  * fixup mechanism in lib_physio. When the order of data bits matters
  78  * (such as when copying data from host memory to flash or
  79  * vice-versa), we do not want to use the address fixup mechanism in
  80  * lib_physio. When both matter (such as when using a write buffer
  81  * mechanism that requires that consecutive addresses be presented to
  82  * the flash), macros can't help us (see intel_pgm8 and intel_pgm16).
  83  */
  84 #ifdef  BCMHND74K
  85 #define data_read8(a) phys_read8((a)^7)
  86 #define data_read16(a) phys_read16((a)^6)
  87 #define data_write8(a,v) phys_write8((a)^7,v)
  88 #define data_write16(a,v) phys_write16((a)^6,v)
  89 #else   /* Not 74K, bcm33xx */
  90 #define data_read8(a) phys_read8((a)^3)
  91 #define data_read16(a) phys_read16((a)^2)
  92 #define data_write8(a,v) phys_write8((a)^3,v)
  93 #define data_write16(a,v) phys_write16((a)^2,v)
  94 #endif  /* BCMHND74K */
  95 #else
  96 #define data_read8(a) phys_read8(a)
  97 #define data_read16(a) phys_read16(a)
  98 #define data_write8(a,v) phys_write8(a,v)
  99 #define data_write16(a,v) phys_write16(a,v)
 100 #endif
 101
 102 /* Read, 8-bit mode */
 103 static void flash_read8(long base, uint8_t *buf, int offset, int blen)
 104 {
 105     physaddr_t src = base + offset;
 106
 107     while (blen > 0) {
 108         *buf++ = data_read8(src++);
 109         blen--;
 110     }
 111 }
 112
 113 /* Read, 16-bit mode */
 114 static void flash_read16(long base, uint8_t *buf, int offset, int blen)
 115 {
 116     physaddr_t src = base + offset;
 117     uint16_t val;
 118     int n;
 119
 120     while (blen > 0) {
 121         offset = src & 1;
 122         val = data_read16(src - offset);
 123         n = min(blen, 2 - offset);
 124         memcpy(buf, (uint8_t *) &val + offset, n);
 125         buf += n;
 126         src = src - offset + 2;
 127         blen -= n;
 128     }
 129 }
 130
 131 /* CFI Query 8-bit */
 132 static void _cfi_query8(long base, uint8_t *buf, int offset, int blen, int buswidth)
 133 {
 134     physaddr_t src;
 135
 136     phys_write8(base + FLASH_CFI_QUERY_ADDR*buswidth, FLASH_CFI_QUERY_MODE);
 137     for (src = base + offset*buswidth; blen; src += buswidth, buf++, blen--)
 138         *buf = phys_read8(src);
 139     phys_write8(base + FLASH_CFI_QUERY_ADDR*buswidth, FLASH_CFI_QUERY_EXIT);
 140 }
 141
 142 /* CFI Query 8-bit */
 143 static void cfi_query8(long base, uint8_t *buf, int offset, int blen)
 144 {
 145     _cfi_query8(base, buf, offset, blen, 1);
 146 }
 147
 148 /* CFI Query 16-bit in byte mode */
 149 static void cfi_query16b(long base, uint8_t *buf, int offset, int blen)
 150 {
 151     _cfi_query8(base, buf, offset, blen, 2);
 152 }
 153
 154 /* CFI Query 16-bit in word mode */
 155 static void cfi_query16(long base, uint8_t *buf, int offset, int blen)
 156 {
 157     physaddr_t src;
 158
 159     phys_write16(base + FLASH_CFI_QUERY_ADDR*2, FLASH_CFI_QUERY_MODE);
 160     for (src = base + offset*2; blen; src += 2, buf++, blen--)
 161         *buf = (uint8_t) phys_read16(src);
 162     phys_write16(base + FLASH_CFI_QUERY_ADDR*2, FLASH_CFI_QUERY_EXIT);
 163 }
 164
 165 #if (FLASH_DRIVERS & FLASH_DRIVER_AMD)
 166
 167 /* AMD erase (8-bit) */
 168 static int _amd_erase8(long base, int offset, int buswidth)
 169 {
 170     physaddr_t dst = base + offset;
 171
 172     /* Do an "unlock write" sequence  (cycles 1-2) */
 173     phys_write8(base + AMD_FLASH_MAGIC_ADDR_1*buswidth, AMD_FLASH_MAGIC_1);
 174     phys_write8(base + AMD_FLASH_MAGIC_ADDR_2*buswidth, AMD_FLASH_MAGIC_2);
 175
 176     /* send the erase command (cycle 3) */
 177     phys_write8(base + AMD_FLASH_MAGIC_ADDR_1*buswidth, AMD_FLASH_ERASE_3);
 178
 179     /* Do an "unlock write" sequence (cycles 4-5) */
 180     phys_write8(base + AMD_FLASH_MAGIC_ADDR_1*buswidth, AMD_FLASH_MAGIC_1);
 181     phys_write8(base + AMD_FLASH_MAGIC_ADDR_2*buswidth, AMD_FLASH_MAGIC_2);
 182
 183     /* Send the "erase sector" qualifier (cycle 6) */
 184     phys_write8(dst, AMD_FLASH_ERASE_SEC_6);
 185
 186     /* Wait for the erase to complete */
 187     while (phys_read8(dst) != 0xff);
 188
 189     return 0;
 190 }
 191
 192 /* AMD erase (8-bit) */
 193 static int amd_erase8(long base, int offset)
 194 {
 195     return _amd_erase8(base, offset, 1);
 196 }
 197
 198 /* AMD erase (16-bit in byte mode) */
 199 static int amd_erase16b(long base, int offset)
 200 {
 201     return _amd_erase8(base, offset, 2);
 202 }
 203
 204 /* AMD erase (16-bit in word mode) */
 205 static int amd_erase16(long base, int offset)
 206 {
 207     physaddr_t dst = base + offset;
 208
 209     /* Do an "unlock write" sequence  (cycles 1-2) */
 210     phys_write16(base + AMD_FLASH_MAGIC_ADDR_1*2, AMD_FLASH_MAGIC_1);
 211     phys_write16(base + AMD_FLASH_MAGIC_ADDR_2*2, AMD_FLASH_MAGIC_2);
 212
 213     /* send the erase command (cycle 3) */
 214     phys_write16(base + AMD_FLASH_MAGIC_ADDR_1*2, AMD_FLASH_ERASE_3);
 215
 216     /* Do an "unlock write" sequence (cycles 4-5) */
 217     phys_write16(base + AMD_FLASH_MAGIC_ADDR_1*2, AMD_FLASH_MAGIC_1);
 218     phys_write16(base + AMD_FLASH_MAGIC_ADDR_2*2, AMD_FLASH_MAGIC_2);
 219
 220     /* Send the "erase sector" qualifier (cycle 6) */
 221     phys_write16(dst, AMD_FLASH_ERASE_SEC_6);
 222
 223     /* Wait for the erase to complete */
 224     while ((phys_read16(dst) & 0xff) != 0xff);
 225
 226     return 0;
 227 }
 228
 229 /* AMD 8-bit program */
 230 static int _amd_pgm8(long base, uint8_t *buf, int offset, int blen, int buswidth)
 231 {
 232     physaddr_t dst = base + offset;
 233     uint8_t *ptr = buf;
 234     uint8_t tempVal;
 235
 236     while (blen > 0) {
 237         /* Do an "unlock write" sequence  (cycles 1-2) */
 238         phys_write8(base + AMD_FLASH_MAGIC_ADDR_1*buswidth, AMD_FLASH_MAGIC_1);
 239         phys_write8(base + AMD_FLASH_MAGIC_ADDR_2*buswidth, AMD_FLASH_MAGIC_2);
 240
 241         /* Send a program command (cycle 3) */
 242         phys_write8(base + AMD_FLASH_MAGIC_ADDR_1*buswidth, AMD_FLASH_PROGRAM);
 243
 244         /* Write a byte (cycle 4) */
 245         data_write8(dst, *ptr);
 246
 247         /* Wait for write to complete or timeout */
 248         while (((tempVal = data_read8(dst)) & 0x80) != (*ptr & 0x80)) {
 249                 if (tempVal & 0x20) {
 250                         /* Bit DQ5 is set */
 251                         tempVal = data_read8(dst);
 252                         if ((tempVal & 0x80) != (*ptr & 0x80)) {
 253                                 xprintf("\nERROR. Flash device write error at address %x\n", dst);
 254                                 return CFE_ERR_IOERR;
 255                         }
 256                 }
 257         }
 258
 259         ptr++;
 260         dst++;
 261         blen--;
 262     }
 263
 264     return 0;
 265 }
 266
 267 /* AMD 8-bit program */
 268 static int amd_pgm8(long base, uint8_t *buf, int offset, int blen)
 269 {
 270     return _amd_pgm8(base, buf, offset, blen, 1);
 271 }
 272
 273 /* AMD 16-bit pgm in 8-bit mode */
 274 static int amd_pgm16b(long base, uint8_t *buf, int offset, int blen)
 275 {
 276     return _amd_pgm8(base, buf, offset, blen, 2);
 277 }
 278
 279 /* AMD 16-bit program */
 280 static int amd_pgm16(long base, uint8_t *buf, int offset, int blen)
 281 {
 282     physaddr_t dst = base + offset;
 283     uint16_t *ptr = (uint16_t *) buf;
 284     uint16_t *src;
 285     uint16_t tempVal;
 286     uint8_t tempData[2];
 287
 288     while (blen > 0) {
 289                 /* Do an "unlock write" sequence  (cycles 1-2) */
 290                 phys_write16(base + AMD_FLASH_MAGIC_ADDR_1*2, AMD_FLASH_MAGIC_1);
 291                 phys_write16(base + AMD_FLASH_MAGIC_ADDR_2*2, AMD_FLASH_MAGIC_2);
 292
 293                 /* Send a program command (cycle 3) */
 294                 phys_write16(base + AMD_FLASH_MAGIC_ADDR_1*2, AMD_FLASH_PROGRAM);
 295
 296                 /* If ptr is not 2-byte aligned, copy the data before writing */
 297                 if ((int)ptr & 1) {
 298                         if(blen == 1) {
 299                                 /* For the last byte, read the next byte to write first */
 300                                 flash_read16(dst, tempData, 0, 2);
 301                         } else {
 302                                 tempData[1] = *((uint8_t *)ptr + 1);
 303                         }
 304                         tempData[0] = *(uint8_t *)ptr;
 305                         src = (uint16_t *)tempData;
 306                 } else {
 307                         src = ptr;
 308                 }
 309
 310                 /* Write a byte (cycle 4) */
 311                 data_write16(dst, *src);
 312
 313                 /* Wait for write to complete or timeout */
 314                 while (((tempVal = data_read16(dst)) & 0x80) != (*src & 0x80)) {
 315                         if (tempVal & 0x20) {
 316                                 /* Bit DQ5 is set */
 317                                 tempVal = data_read16(dst);
 318                                 if ((tempVal & 0x80) != (*src & 0x80)) {
 319                                         xprintf("\nERROR. Flash device write error at address %x\n", dst);
 320                                         return CFE_ERR_IOERR;
 321                                 }
 322                         }
 323                 }
 324
 325                 ptr++;
 326                 dst += 2;
 327                 blen -= 2;
 328     }
 329
 330     return 0;
 331 }
 332
 333 static void _amd_autosel8(long base, uint8_t *buf, int offset, int buswidth)
 334 {
 335     /* Do an "unlock write" sequence  (cycles 1-2) */
 336     phys_write8(base + AMD_FLASH_MAGIC_ADDR_1*buswidth, AMD_FLASH_MAGIC_1);
 337     phys_write8(base + AMD_FLASH_MAGIC_ADDR_2*buswidth, AMD_FLASH_MAGIC_2);
 338
 339     /* Send an autoselect command (cycle 3) */
 340     phys_write8(base + AMD_FLASH_MAGIC_ADDR_1*buswidth, AMD_FLASH_AUTOSEL);
 341
 342     /* Read code (cycle 4) */
 343     *buf = phys_read8(base+offset*buswidth);
 344
 345     /* Reset */
 346     phys_write8(base, AMD_FLASH_RESET);
 347 }
 348
 349 static void amd_manid8(long base, uint8_t *buf)
 350 {
 351     _amd_autosel8(base, buf, 0, 1);
 352 }
 353
 354 static void amd_manid16b(long base, uint8_t *buf)
 355 {
 356     _amd_autosel8(base, buf, 0, 2);
 357 }
 358
 359 static void amd_devcode8(long base, uint8_t *buf)
 360 {
 361     _amd_autosel8(base, buf, 1, 1);
 362 }
 363
 364 static void amd_devcode16b(long base, uint8_t *buf)
 365 {
 366     _amd_autosel8(base, buf, 1, 2);
 367 }
 368
 369 static void amd_autosel16(long base, uint8_t *buf, int offset)
 370 {
 371     /* Do an "unlock write" sequence  (cycles 1-2) */
 372     phys_write16(base + AMD_FLASH_MAGIC_ADDR_1*2, AMD_FLASH_MAGIC_1);
 373     phys_write16(base + AMD_FLASH_MAGIC_ADDR_2*2, AMD_FLASH_MAGIC_2);
 374
 375     /* Send an autoselect command (cycle 3) */
 376     phys_write16(base + AMD_FLASH_MAGIC_ADDR_1*2, AMD_FLASH_AUTOSEL);
 377
 378     /* Read code (cycle 4) */
 379     *buf = (uint8_t) phys_read16(base+offset*2);
 380
 381     /* Reset */
 382     phys_write16(base, AMD_FLASH_RESET);
 383 }
 384
 385 static void amd_manid16(long base, uint8_t *buf)
 386 {
 387     amd_autosel16(base, buf, 0);
 388 }
 389
 390 static void amd_devcode16(long base, uint8_t *buf)
 391 {
 392     amd_autosel16(base, buf, 1);
 393 }
 394
 395 #endif
 396
 397 #if (FLASH_DRIVERS & FLASH_DRIVER_INTEL)
 398
 399 /* Intel extended erase 8-bit */
 400 static int intelext_erase8(long base, int offset)
 401 {
 402     physaddr_t dst = base + offset;
 403     int res;
 404
 405     phys_write8(dst, INTEL_FLASH_ERASE_BLOCK);
 406     phys_write8(dst, INTEL_FLASH_ERASE_CONFIRM);
 407
 408     /* poll for SR.7 */
 409     while (!((res = phys_read8(dst)) & 0x80));
 410
 411     /* check for SR.4 or SR.5 */
 412     return (res & 0x30);
 413 }
 414
 415 /* Intel erase 8-bit */
 416 static int intel_erase8(long base, int offset)
 417 {
 418     physaddr_t dst = base + offset;
 419
 420     /* unlock block */
 421     phys_write16(dst, INTEL_FLASH_CONFIG_SETUP);
 422     phys_write16(dst, INTEL_FLASH_UNLOCK);
 423
 424     return intelext_erase8(base, offset);
 425 }
 426
 427 /* Intel extended erase 16-bit */
 428 static int intelext_erase16(long base, int offset)
 429 {
 430     physaddr_t dst = base + offset;
 431     int res;
 432
 433     phys_write16(dst, INTEL_FLASH_ERASE_BLOCK);
 434     phys_write16(dst, INTEL_FLASH_ERASE_CONFIRM);
 435
 436     /* poll for SR.7 */
 437     while (!((res = phys_read16(dst)) & 0x80));
 438
 439     /* check for SR.4 or SR.5 */
 440     return (res & 0x30);
 441 }
 442
 443 /* Intel erase 16-bit */
 444 static int intel_erase16(long base, int offset)
 445 {
 446     physaddr_t dst = base + offset;
 447
 448     /* unlock block */
 449     phys_write16(dst, INTEL_FLASH_CONFIG_SETUP);
 450     phys_write16(dst, INTEL_FLASH_UNLOCK);
 451
 452     return intelext_erase16(base, offset);
 453 }
 454
 455 /* Intel 8-bit prog */
 456 static int _intel_pgm8(long base, uint8_t *buf, int offset, int blen, int cmdset)
 457 {
 458     physaddr_t dst = base + offset;
 459     uint8_t *ptr = buf;
 460     int n, res = 0;
 461
 462     while (blen > 0) {
 463         if (cmdset == FLASH_CFI_CMDSET_INTEL_ECS &&
 464             (dst & (INTEL_FLASH_WRITE_BUFFER_SIZE-1)) == 0) {
 465             data_write8(dst, INTEL_FLASH_WRITE_BUFFER);
 466
 467             /* poll for XSR.7 */
 468             while (!(data_read8(dst) & 0x80));
 469
 470 #if defined(__MIPSEB) && defined(_MIPSEB_DATA_INVARIANT_)
 471             /*
 472              * This write buffer mechanism appears to be
 473              * sensitive to the order of the addresses hence we need
 474              * to unscramble them. We may also need to pad the source
 475              * with up to three bytes of 0xffff in case an unaligned
 476              * number of bytes are presented.
 477              */
 478
 479             n = roundup(min(blen, INTEL_FLASH_WRITE_BUFFER_SIZE), 4);
 480
 481             /* write (nwords-1) */
 482             data_write8(dst, n-1);
 483
 484             /* write data (plus pad if necessary) */
 485             while (n) {
 486                 data_write8(dst+3, blen < 4 ? 0xff : *(ptr+3));
 487                 data_write8(dst+2, blen < 3 ? 0xff : *(ptr+2));
 488                 data_write8(dst+1, blen < 2 ? 0xff : *(ptr+1));
 489                 data_write8(dst+0, *ptr);
 490                 ptr += 4;
 491                 dst += 4;
 492                 blen -= 4;
 493                 n -= 2;
 494             }
 495 #else
 496             n = min(blen, INTEL_FLASH_WRITE_BUFFER_SIZE);
 497
 498             /* write (nbytes-1) */
 499             data_write8(dst, n-1);
 500
 501             /* write data */
 502             while (n--) {
 503                 data_write8(dst, *ptr);
 504                 ptr++;
 505                 dst++;
 506                 blen--;
 507             }
 508 #endif
 509
 510             /* write confirm */
 511             phys_write8(base, INTEL_FLASH_WRITE_CONFIRM);
 512         } else {
 513             data_write8(dst, INTEL_FLASH_PROGRAM);
 514             data_write8(dst, *ptr);
 515             ptr++;
 516             dst++;
 517             blen--;
 518         }
 519
 520         /* poll for SR.7 */
 521         while (!((res = phys_read8(base)) & 0x80));
 522
 523         /* check for SR.4 or SR.5 */
 524         if (res & 0x30)
 525             break;
 526     }
 527
 528     /* reset */
 529     phys_write8(base, INTEL_FLASH_READ_MODE);
 530
 531     /* check for SR.4 or SR.5 */
 532     return (res & 0x30);
 533 }
 534
 535 /* Intel 8-bit prog */
 536 static int intel_pgm8(long base, uint8_t *buf, int offset, int blen)
 537 {
 538     return _intel_pgm8(base, buf, offset, blen, FLASH_CFI_CMDSET_INTEL_STD);
 539 }
 540
 541 /* Intel extended 8-bit prog */
 542 static int intelext_pgm8(long base, uint8_t *buf, int offset, int blen)
 543 {
 544     return _intel_pgm8(base, buf, offset, blen, FLASH_CFI_CMDSET_INTEL_ECS);
 545 }
 546
 547 /* Intel 16-bit prog */
 548 static int _intel_pgm16(long base, uint8_t *buf, int offset, int blen, int cmdset)
 549 {
 550     physaddr_t dst = base + offset;
 551     uint16_t *ptr = (uint16_t *) buf;
 552     int n, res = 0;
 553
 554     while (blen > 0) {
 555         if (cmdset == FLASH_CFI_CMDSET_INTEL_ECS &&
 556             (dst & (INTEL_FLASH_WRITE_BUFFER_SIZE-1)) == 0) {
 557             data_write16(dst, INTEL_FLASH_WRITE_BUFFER);
 558
 559             /* poll for XSR.7 */
 560             while (!(data_read16(dst) & 0x80));
 561
 562 #if defined(__MIPSEB) && defined(_MIPSEB_DATA_INVARIANT_)
 563             /*
 564              * This write buffer mechanism appears to be
 565              * sensitive to the order of the addresses hence we need
 566              * to unscramble them. We may also need to pad the source
 567              * with up to three bytes of 0xffff in case an unaligned
 568              * number of bytes are presented.
 569              */
 570
 571             n = roundup(min(blen, INTEL_FLASH_WRITE_BUFFER_SIZE), 4)/2;
 572
 573             /* write (nwords-1) */
 574             data_write16(dst, n-1);
 575
 576             /* write data (plus pad if necessary) */
 577             while (n > 0) {
 578                 data_write16(dst+2, blen < 4 ? 0xffff : *(ptr+1));
 579                 data_write16(dst, *ptr);
 580                 ptr += 2;
 581                 dst += 4;
 582                 blen -= 4;
 583                 n -= 2;
 584             }
 585 #else
 586             n = min(blen, INTEL_FLASH_WRITE_BUFFER_SIZE)/2;
 587
 588             /* write (nwords-1) */
 589             data_write16(dst, n-1);
 590
 591             /* write data */
 592             while (n > 0) {
 593                 data_write16(dst, *ptr);
 594                 ptr++;
 595                 dst += 2;
 596                 blen -= 2;
 597                 n--;
 598             }
 599 #endif
 600
 601             /* write confirm */
 602             phys_write16(base, INTEL_FLASH_WRITE_CONFIRM);
 603         } else {
 604             data_write16(dst, INTEL_FLASH_PROGRAM);
 605             data_write16(dst, *ptr);
 606             ptr++;
 607             dst += 2;
 608             blen -= 2;
 609         }
 610
 611         /* poll for SR.7 */
 612         while (!((res = phys_read16(base)) & 0x80));
 613
 614         /* check for SR.4 or SR.5 */
 615         if (res & 0x30)
 616             break;
 617     }
 618
 619     /* reset */
 620     phys_write16(base, INTEL_FLASH_READ_MODE);
 621
 622     /* check for SR.4 or SR.5 */
 623     return (res & 0x30);
 624 }
 625
 626 /* Intel 16-bit prog */
 627 static int intel_pgm16(long base, uint8_t *buf, int offset, int blen)
 628 {
 629     return _intel_pgm16(base, buf, offset, blen, FLASH_CFI_CMDSET_INTEL_STD);
 630 }
 631
 632 /* Intel extended 16-bit prog */
 633 static int intelext_pgm16(long base, uint8_t *buf, int offset, int blen)
 634 {
 635     return _intel_pgm16(base, buf, offset, blen, FLASH_CFI_CMDSET_INTEL_ECS);
 636 }
 637
 638 #endif
 639
 640 #if (FLASH_DRIVERS & FLASH_DRIVER_SST)
 641
 642 /* SST CFI Query 16-bit in word mode */
 643 static void sst_cfi_query16(long base, uint8_t *buf, int offset, int blen)
 644 {
 645     physaddr_t src;
 646
 647     phys_write16(base + SST_FLASH_MAGIC_ADDR_1*2, SST_FLASH_MAGIC_1);
 648     phys_write16(base + SST_FLASH_MAGIC_ADDR_2*2, SST_FLASH_MAGIC_2);
 649
 650     phys_write16(base + SST_FLASH_MAGIC_ADDR_1*2, FLASH_CFI_QUERY_MODE);
 651     for (src = base + offset*2; blen; src += 2, buf++, blen--)
 652         *buf = (uint8_t) phys_read16(src);
 653     phys_write16(base + SST_FLASH_MAGIC_ADDR_1*2, SST_FLASH_RESET);
 654 }
 655
 656 /* SST erase (16-bit in word mode) */
 657 static int sst_erase16(long base, int offset)
 658 {
 659     physaddr_t dst = base + offset;
 660
 661     /* Do an "unlock write" sequence  (cycles 1-2) */
 662     phys_write16(base + SST_FLASH_MAGIC_ADDR_1*2, SST_FLASH_MAGIC_1);
 663     phys_write16(base + SST_FLASH_MAGIC_ADDR_2*2, SST_FLASH_MAGIC_2);
 664
 665     /* send the erase command (cycle 3) */
 666     phys_write16(base + SST_FLASH_MAGIC_ADDR_1*2, SST_FLASH_ERASE_3);
 667
 668     /* Do an "unlock write" sequence (cycles 4-5) */
 669     phys_write16(base + SST_FLASH_MAGIC_ADDR_1*2, SST_FLASH_MAGIC_1);
 670     phys_write16(base + SST_FLASH_MAGIC_ADDR_2*2, SST_FLASH_MAGIC_2);
 671
 672     /* Send the "erase sector" qualifier (cycle 6) */
 673     phys_write16(dst, SST_FLASH_ERASE_SEC_6);
 674
 675     /* Wait for the erase to complete */
 676     while ((phys_read16(dst) & 0xff) != 0xff);
 677
 678     return 0;
 679 }
 680
 681 /* SST 16-bit program */
 682 static int sst_pgm16(long base, uint8_t *buf, int offset, int blen)
 683 {
 684     physaddr_t dst = base + offset;
 685     uint16_t *ptr = (uint16_t *) buf;
 686
 687     while (blen > 0) {
 688         /* Do an "unlock write" sequence  (cycles 1-2) */
 689         phys_write16(base + SST_FLASH_MAGIC_ADDR_1*2, SST_FLASH_MAGIC_1);
 690         phys_write16(base + SST_FLASH_MAGIC_ADDR_2*2, SST_FLASH_MAGIC_2);
 691
 692         /* Send a program command (cycle 3) */
 693         phys_write16(base + SST_FLASH_MAGIC_ADDR_1*2, SST_FLASH_PROGRAM);
 694
 695         /* Write a byte (cycle 4) */
 696         data_write16(dst, *ptr);
 697
 698         /* Wait for write to complete */
 699         while ((data_read16(dst) & 0x80) != (*ptr & 0x80));
 700
 701         ptr++;
 702         dst += 2;
 703         blen -= 2;
 704     }
 705
 706     return 0;
 707 }
 708
 709 #endif
 710
 711 int flashop_engine(flashinstr_t *prog)
 712 {
 713         int ret = 0;
 714
 715     for (;; prog++) {
 716                 if (ret) {
 717                         break;
 718                 }
 719         switch (prog->fi_op) {
 720             case FEOP_RETURN:
 721                 return 0;
 722             case FEOP_REBOOT:
 723                 /* Jump to boot vector */
 724                 ((void (*)(void)) 0xbfc00000)();
 725                 break;
 726             case FEOP_READ8:
 727                 flash_read8(prog->fi_base, (uint8_t *) prog->fi_dest, prog->fi_src, prog->fi_cnt);
 728                 break;
 729             case FEOP_READ16:
 730                 flash_read16(prog->fi_base, (uint8_t *) prog->fi_dest, prog->fi_src, prog->fi_cnt);
 731                 break;
 732             case FEOP_CFIQUERY8:
 733                 cfi_query8(prog->fi_base, (uint8_t *) prog->fi_dest, prog->fi_src, prog->fi_cnt);
 734                 break;
 735             case FEOP_CFIQUERY16:
 736                 cfi_query16(prog->fi_base, (uint8_t *) prog->fi_dest, prog->fi_src, prog->fi_cnt);
 737                 break;
 738             case FEOP_CFIQUERY16B:
 739                 cfi_query16b(prog->fi_base, (uint8_t *) prog->fi_dest, prog->fi_src, prog->fi_cnt);
 740                 break;
 741             case FEOP_MEMCPY:
 742                 memcpy((uint8_t *) prog->fi_dest, (uint8_t *) prog->fi_src, prog->fi_cnt);
 743                 break;
 744 #if (FLASH_DRIVERS & FLASH_DRIVER_AMD)
 745             case FEOP_AMD_ERASE8:
 746                 amd_erase8(prog->fi_base, prog->fi_dest);
 747                 break;
 748             case FEOP_AMD_ERASE16:
 749                 amd_erase16(prog->fi_base, prog->fi_dest);
 750                 break;
 751             case FEOP_AMD_ERASE16B:
 752                 amd_erase16b(prog->fi_base, prog->fi_dest);
 753                 break;
 754             case FEOP_AMD_PGM8:
 755                 ret |= amd_pgm8(prog->fi_base, (uint8_t *) prog->fi_src, prog->fi_dest, prog->fi_cnt);
 756                 break;
 757             case FEOP_AMD_PGM16:
 758                 ret |= amd_pgm16(prog->fi_base, (uint8_t *) prog->fi_src, prog->fi_dest, prog->fi_cnt);
 759                 break;
 760             case FEOP_AMD_PGM16B:
 761                 ret |= amd_pgm16b(prog->fi_base, (uint8_t *) prog->fi_src, prog->fi_dest, prog->fi_cnt);
 762                 break;
 763             case FEOP_AMD_MANID8:
 764                 amd_manid8(prog->fi_base, (uint8_t *) prog->fi_dest);
 765                 break;
 766             case FEOP_AMD_MANID16:
 767                 amd_manid16(prog->fi_base, (uint8_t *) prog->fi_dest);
 768                 break;
 769             case FEOP_AMD_MANID16B:
 770                 amd_manid16b(prog->fi_base, (uint8_t *) prog->fi_dest);
 771                 break;
 772             case FEOP_AMD_DEVCODE8:
 773                 amd_devcode8(prog->fi_base, (uint8_t *) prog->fi_dest);
 774                 break;
 775             case FEOP_AMD_DEVCODE16:
 776                 amd_devcode16(prog->fi_base, (uint8_t *) prog->fi_dest);
 777                 break;
 778             case FEOP_AMD_DEVCODE16B:
 779                 amd_devcode16b(prog->fi_base, (uint8_t *) prog->fi_dest);
 780                 break;
 781 #endif
 782 #if (FLASH_DRIVERS & FLASH_DRIVER_INTEL)
 783             case FEOP_INTEL_ERASE8:
 784                 intel_erase8(prog->fi_base, prog->fi_dest);
 785                 break;
 786             case FEOP_INTEL_ERASE16:
 787                 intel_erase16(prog->fi_base, prog->fi_dest);
 788                 break;
 789             case FEOP_INTEL_PGM8:
 790                 intel_pgm8(prog->fi_base, (uint8_t *) prog->fi_src, prog->fi_dest, prog->fi_cnt);
 791                 break;
 792             case FEOP_INTEL_PGM16:
 793                 intel_pgm16(prog->fi_base, (uint8_t *) prog->fi_src, prog->fi_dest, prog->fi_cnt);
 794                 break;
 795             case FEOP_INTELEXT_ERASE8:
 796                 intelext_erase8(prog->fi_base, prog->fi_dest);
 797                 break;
 798             case FEOP_INTELEXT_ERASE16:
 799                 intelext_erase16(prog->fi_base, prog->fi_dest);
 800                 break;
 801             case FEOP_INTELEXT_PGM8:
 802                 intelext_pgm8(prog->fi_base, (uint8_t *) prog->fi_src, prog->fi_dest, prog->fi_cnt);
 803                 break;
 804             case FEOP_INTELEXT_PGM16:
 805                 intelext_pgm16(prog->fi_base, (uint8_t *) prog->fi_src, prog->fi_dest, prog->fi_cnt);
 806                 break;
 807 #endif
 808 #if (FLASH_DRIVERS & FLASH_DRIVER_SST)
 809             case FEOP_SST_CFIQUERY16:
 810                 sst_cfi_query16(prog->fi_base, (uint8_t *) prog->fi_dest, prog->fi_src, prog->fi_cnt);
 811                 break;
 812             case FEOP_SST_ERASE16:
 813                 sst_erase16(prog->fi_base, prog->fi_dest);
 814                 break;
 815             case FEOP_SST_PGM16:
 816                 sst_pgm16(prog->fi_base, (uint8_t *) prog->fi_src, prog->fi_dest, prog->fi_cnt);
 817                 break;
 818 #endif
 819             default:
 820                 return -1;
 821         }
 822     }
 823
 824     return ret;
 825 }