git.gag.com Git - fw/openocd/blob - src/flash/nor/cfi.c

   1 /***************************************************************************
   2  *   Copyright (C) 2005, 2007 by Dominic Rath                              *
   3  *   Dominic.Rath@gmx.de                                                   *
   4  *   Copyright (C) 2009 Michael Schwingen                                  *
   5  *   michael@schwingen.org                                                 *
   6  *   Copyright (C) 2010 Øyvind Harboe <oyvind.harboe@zylin.com>            *
   7  *   Copyright (C) 2010 by Antonio Borneo <borneo.antonio@gmail.com>       *
   8  *                                                                         *
   9  *   This program is free software; you can redistribute it and/or modify  *
  10  *   it under the terms of the GNU General Public License as published by  *
  11  *   the Free Software Foundation; either version 2 of the License, or     *
  12  *   (at your option) any later version.                                   *
  13  *                                                                         *
  14  *   This program is distributed in the hope that it will be useful,       *
  15  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  16  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  17  *   GNU General Public License for more details.                          *
  18  *                                                                         *
  19  *   You should have received a copy of the GNU General Public License     *
  20  *   along with this program; if not, write to the                         *
  21  *   Free Software Foundation, Inc.,                                       *
  22  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  23  ***************************************************************************/
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include "config.h"
  27 #endif
  28
  29 #include "imp.h"
  30 #include "cfi.h"
  31 #include "non_cfi.h"
  32 #include <target/arm.h>
  33 #include <target/arm7_9_common.h>
  34 #include <target/armv7m.h>
  35 #include <target/mips32.h>
  36 #include <helper/binarybuffer.h>
  37 #include <target/algorithm.h>
  38
  39 #define CFI_MAX_BUS_WIDTH       4
  40 #define CFI_MAX_CHIP_WIDTH      4
  41
  42 /* defines internal maximum size for code fragment in cfi_intel_write_block() */
  43 #define CFI_MAX_INTEL_CODESIZE 256
  44
  45 /* some id-types with specific handling */
  46 #define AT49BV6416      0x00d6
  47 #define AT49BV6416T     0x00d2
  48
  49 static struct cfi_unlock_addresses cfi_unlock_addresses[] = {
  50         [CFI_UNLOCK_555_2AA] = { .unlock1 = 0x555, .unlock2 = 0x2aa },
  51         [CFI_UNLOCK_5555_2AAA] = { .unlock1 = 0x5555, .unlock2 = 0x2aaa },
  52 };
  53
  54 /* CFI fixups foward declarations */
  55 static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, void *param);
  56 static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, void *param);
  57 static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, void *param);
  58 static void cfi_fixup_0002_write_buffer(struct flash_bank *bank, void *param);
  59
  60 /* fixup after reading cmdset 0002 primary query table */
  61 static const struct cfi_fixup cfi_0002_fixups[] = {
  62         {CFI_MFR_SST, 0x00D4, cfi_fixup_0002_unlock_addresses,
  63          &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
  64         {CFI_MFR_SST, 0x00D5, cfi_fixup_0002_unlock_addresses,
  65          &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
  66         {CFI_MFR_SST, 0x00D6, cfi_fixup_0002_unlock_addresses,
  67          &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
  68         {CFI_MFR_SST, 0x00D7, cfi_fixup_0002_unlock_addresses,
  69          &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
  70         {CFI_MFR_SST, 0x2780, cfi_fixup_0002_unlock_addresses,
  71          &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
  72         {CFI_MFR_SST, 0x274b, cfi_fixup_0002_unlock_addresses,
  73          &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
  74         {CFI_MFR_SST, 0x236d, cfi_fixup_0002_unlock_addresses,
  75          &cfi_unlock_addresses[CFI_UNLOCK_555_2AA]},
  76         {CFI_MFR_ATMEL, 0x00C8, cfi_fixup_reversed_erase_regions, NULL},
  77         {CFI_MFR_ST, 0x22C4, cfi_fixup_reversed_erase_regions, NULL},   /* M29W160ET */
  78         {CFI_MFR_FUJITSU, 0x22ea, cfi_fixup_0002_unlock_addresses,
  79          &cfi_unlock_addresses[CFI_UNLOCK_555_2AA]},
  80         {CFI_MFR_FUJITSU, 0x226b, cfi_fixup_0002_unlock_addresses,
  81          &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
  82         {CFI_MFR_AMIC, 0xb31a, cfi_fixup_0002_unlock_addresses,
  83          &cfi_unlock_addresses[CFI_UNLOCK_555_2AA]},
  84         {CFI_MFR_MX, 0x225b, cfi_fixup_0002_unlock_addresses,
  85          &cfi_unlock_addresses[CFI_UNLOCK_555_2AA]},
  86         {CFI_MFR_EON, 0x225b, cfi_fixup_0002_unlock_addresses,
  87          &cfi_unlock_addresses[CFI_UNLOCK_555_2AA]},
  88         {CFI_MFR_AMD, 0x225b, cfi_fixup_0002_unlock_addresses,
  89          &cfi_unlock_addresses[CFI_UNLOCK_555_2AA]},
  90         {CFI_MFR_ANY, CFI_ID_ANY, cfi_fixup_0002_erase_regions, NULL},
  91         {CFI_MFR_ST, 0x227E, cfi_fixup_0002_write_buffer, NULL},/* M29W128G */
  92         {0, 0, NULL, NULL}
  93 };
  94
  95 /* fixup after reading cmdset 0001 primary query table */
  96 static const struct cfi_fixup cfi_0001_fixups[] = {
  97         {0, 0, NULL, NULL}
  98 };
  99
 100 static void cfi_fixup(struct flash_bank *bank, const struct cfi_fixup *fixups)
 101 {
 102         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 103         const struct cfi_fixup *f;
 104
 105         for (f = fixups; f->fixup; f++) {
 106                 if (((f->mfr == CFI_MFR_ANY) || (f->mfr == cfi_info->manufacturer)) &&
 107                                 ((f->id  == CFI_ID_ANY)  || (f->id  == cfi_info->device_id)))
 108                         f->fixup(bank, f->param);
 109         }
 110 }
 111
 112 /* inline uint32_t flash_address(struct flash_bank *bank, int sector, uint32_t offset) */
 113 static inline uint32_t flash_address(struct flash_bank *bank, int sector, uint32_t offset)
 114 {
 115         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 116
 117         if (cfi_info->x16_as_x8)
 118                 offset *= 2;
 119
 120         /* while the sector list isn't built, only accesses to sector 0 work */
 121         if (sector == 0)
 122                 return bank->base + offset * bank->bus_width;
 123         else {
 124                 if (!bank->sectors) {
 125                         LOG_ERROR("BUG: sector list not yet built");
 126                         exit(-1);
 127                 }
 128                 return bank->base + bank->sectors[sector].offset + offset * bank->bus_width;
 129         }
 130 }
 131
 132 static void cfi_command(struct flash_bank *bank, uint8_t cmd, uint8_t *cmd_buf)
 133 {
 134         int i;
 135
 136         /* clear whole buffer, to ensure bits that exceed the bus_width
 137          * are set to zero
 138          */
 139         for (i = 0; i < CFI_MAX_BUS_WIDTH; i++)
 140                 cmd_buf[i] = 0;
 141
 142         if (bank->target->endianness == TARGET_LITTLE_ENDIAN) {
 143                 for (i = bank->bus_width; i > 0; i--)
 144                         *cmd_buf++ = (i & (bank->chip_width - 1)) ? 0x0 : cmd;
 145         } else {
 146                 for (i = 1; i <= bank->bus_width; i++)
 147                         *cmd_buf++ = (i & (bank->chip_width - 1)) ? 0x0 : cmd;
 148         }
 149 }
 150
 151 static int cfi_send_command(struct flash_bank *bank, uint8_t cmd, uint32_t address)
 152 {
 153         uint8_t command[CFI_MAX_BUS_WIDTH];
 154
 155         cfi_command(bank, cmd, command);
 156         return target_write_memory(bank->target, address, bank->bus_width, 1, command);
 157 }
 158
 159 /* read unsigned 8-bit value from the bank
 160  * flash banks are expected to be made of similar chips
 161  * the query result should be the same for all
 162  */
 163 static int cfi_query_u8(struct flash_bank *bank, int sector, uint32_t offset, uint8_t *val)
 164 {
 165         struct target *target = bank->target;
 166         uint8_t data[CFI_MAX_BUS_WIDTH];
 167
 168         int retval;
 169         retval = target_read_memory(target, flash_address(bank, sector, offset),
 170                         bank->bus_width, 1, data);
 171         if (retval != ERROR_OK)
 172                 return retval;
 173
 174         if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
 175                 *val = data[0];
 176         else
 177                 *val = data[bank->bus_width - 1];
 178
 179         return ERROR_OK;
 180 }
 181
 182 /* read unsigned 8-bit value from the bank
 183  * in case of a bank made of multiple chips,
 184  * the individual values are ORed
 185  */
 186 static int cfi_get_u8(struct flash_bank *bank, int sector, uint32_t offset, uint8_t *val)
 187 {
 188         struct target *target = bank->target;
 189         uint8_t data[CFI_MAX_BUS_WIDTH];
 190         int i;
 191
 192         int retval;
 193         retval = target_read_memory(target, flash_address(bank, sector, offset),
 194                         bank->bus_width, 1, data);
 195         if (retval != ERROR_OK)
 196                 return retval;
 197
 198         if (bank->target->endianness == TARGET_LITTLE_ENDIAN) {
 199                 for (i = 0; i < bank->bus_width / bank->chip_width; i++)
 200                         data[0] |= data[i];
 201
 202                 *val = data[0];
 203         } else {
 204                 uint8_t value = 0;
 205                 for (i = 0; i < bank->bus_width / bank->chip_width; i++)
 206                         value |= data[bank->bus_width - 1 - i];
 207
 208                 *val = value;
 209         }
 210         return ERROR_OK;
 211 }
 212
 213 static int cfi_query_u16(struct flash_bank *bank, int sector, uint32_t offset, uint16_t *val)
 214 {
 215         struct target *target = bank->target;
 216         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 217         uint8_t data[CFI_MAX_BUS_WIDTH * 2];
 218         int retval;
 219
 220         if (cfi_info->x16_as_x8) {
 221                 uint8_t i;
 222                 for (i = 0; i < 2; i++) {
 223                         retval = target_read_memory(target, flash_address(bank, sector, offset + i),
 224                                         bank->bus_width, 1, &data[i * bank->bus_width]);
 225                         if (retval != ERROR_OK)
 226                                 return retval;
 227                 }
 228         } else {
 229                 retval = target_read_memory(target, flash_address(bank, sector, offset),
 230                                 bank->bus_width, 2, data);
 231                 if (retval != ERROR_OK)
 232                         return retval;
 233         }
 234
 235         if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
 236                 *val = data[0] | data[bank->bus_width] << 8;
 237         else
 238                 *val = data[bank->bus_width - 1] | data[(2 * bank->bus_width) - 1] << 8;
 239
 240         return ERROR_OK;
 241 }
 242
 243 static int cfi_query_u32(struct flash_bank *bank, int sector, uint32_t offset, uint32_t *val)
 244 {
 245         struct target *target = bank->target;
 246         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 247         uint8_t data[CFI_MAX_BUS_WIDTH * 4];
 248         int retval;
 249
 250         if (cfi_info->x16_as_x8) {
 251                 uint8_t i;
 252                 for (i = 0; i < 4; i++) {
 253                         retval = target_read_memory(target, flash_address(bank, sector, offset + i),
 254                                         bank->bus_width, 1, &data[i * bank->bus_width]);
 255                         if (retval != ERROR_OK)
 256                                 return retval;
 257                 }
 258         } else {
 259                 retval = target_read_memory(target, flash_address(bank, sector, offset),
 260                                 bank->bus_width, 4, data);
 261                 if (retval != ERROR_OK)
 262                         return retval;
 263         }
 264
 265         if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
 266                 *val = data[0] | data[bank->bus_width] << 8 |
 267                         data[bank->bus_width * 2] << 16 | data[bank->bus_width * 3] << 24;
 268         else
 269                 *val = data[bank->bus_width - 1] | data[(2 * bank->bus_width) - 1] << 8 |
 270                         data[(3 * bank->bus_width) - 1] << 16 |
 271                         data[(4 * bank->bus_width) - 1] << 24;
 272
 273         return ERROR_OK;
 274 }
 275
 276 static int cfi_reset(struct flash_bank *bank)
 277 {
 278         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 279         int retval = ERROR_OK;
 280
 281         retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 282         if (retval != ERROR_OK)
 283                 return retval;
 284
 285         retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
 286         if (retval != ERROR_OK)
 287                 return retval;
 288
 289         if (cfi_info->manufacturer == 0x20 &&
 290                         (cfi_info->device_id == 0x227E || cfi_info->device_id == 0x7E)) {
 291                 /* Numonix M29W128G is cmd 0xFF intolerant - causes internal undefined state
 292                  * so we send an extra 0xF0 reset to fix the bug */
 293                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x00));
 294                 if (retval != ERROR_OK)
 295                         return retval;
 296         }
 297
 298         return retval;
 299 }
 300
 301 static void cfi_intel_clear_status_register(struct flash_bank *bank)
 302 {
 303         cfi_send_command(bank, 0x50, flash_address(bank, 0, 0x0));
 304 }
 305
 306 static int cfi_intel_wait_status_busy(struct flash_bank *bank, int timeout, uint8_t *val)
 307 {
 308         uint8_t status;
 309
 310         int retval = ERROR_OK;
 311
 312         for (;; ) {
 313                 if (timeout-- < 0) {
 314                         LOG_ERROR("timeout while waiting for WSM to become ready");
 315                         return ERROR_FAIL;
 316                 }
 317
 318                 retval = cfi_get_u8(bank, 0, 0x0, &status);
 319                 if (retval != ERROR_OK)
 320                         return retval;
 321
 322                 if (status & 0x80)
 323                         break;
 324
 325                 alive_sleep(1);
 326         }
 327
 328         /* mask out bit 0 (reserved) */
 329         status = status & 0xfe;
 330
 331         LOG_DEBUG("status: 0x%x", status);
 332
 333         if (status != 0x80) {
 334                 LOG_ERROR("status register: 0x%x", status);
 335                 if (status & 0x2)
 336                         LOG_ERROR("Block Lock-Bit Detected, Operation Abort");
 337                 if (status & 0x4)
 338                         LOG_ERROR("Program suspended");
 339                 if (status & 0x8)
 340                         LOG_ERROR("Low Programming Voltage Detected, Operation Aborted");
 341                 if (status & 0x10)
 342                         LOG_ERROR("Program Error / Error in Setting Lock-Bit");
 343                 if (status & 0x20)
 344                         LOG_ERROR("Error in Block Erasure or Clear Lock-Bits");
 345                 if (status & 0x40)
 346                         LOG_ERROR("Block Erase Suspended");
 347
 348                 cfi_intel_clear_status_register(bank);
 349
 350                 retval = ERROR_FAIL;
 351         }
 352
 353         *val = status;
 354         return retval;
 355 }
 356
 357 static int cfi_spansion_wait_status_busy(struct flash_bank *bank, int timeout)
 358 {
 359         uint8_t status, oldstatus;
 360         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 361         int retval;
 362
 363         retval = cfi_get_u8(bank, 0, 0x0, &oldstatus);
 364         if (retval != ERROR_OK)
 365                 return retval;
 366
 367         do {
 368                 retval = cfi_get_u8(bank, 0, 0x0, &status);
 369
 370                 if (retval != ERROR_OK)
 371                         return retval;
 372
 373                 if ((status ^ oldstatus) & 0x40) {
 374                         if (status & cfi_info->status_poll_mask & 0x20) {
 375                                 retval = cfi_get_u8(bank, 0, 0x0, &oldstatus);
 376                                 if (retval != ERROR_OK)
 377                                         return retval;
 378                                 retval = cfi_get_u8(bank, 0, 0x0, &status);
 379                                 if (retval != ERROR_OK)
 380                                         return retval;
 381                                 if ((status ^ oldstatus) & 0x40) {
 382                                         LOG_ERROR("dq5 timeout, status: 0x%x", status);
 383                                         return ERROR_FLASH_OPERATION_FAILED;
 384                                 } else {
 385                                         LOG_DEBUG("status: 0x%x", status);
 386                                         return ERROR_OK;
 387                                 }
 388                         }
 389                 } else {/* no toggle: finished, OK */
 390                         LOG_DEBUG("status: 0x%x", status);
 391                         return ERROR_OK;
 392                 }
 393
 394                 oldstatus = status;
 395                 alive_sleep(1);
 396         } while (timeout-- > 0);
 397
 398         LOG_ERROR("timeout, status: 0x%x", status);
 399
 400         return ERROR_FLASH_BUSY;
 401 }
 402
 403 static int cfi_read_intel_pri_ext(struct flash_bank *bank)
 404 {
 405         int retval;
 406         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 407         struct cfi_intel_pri_ext *pri_ext;
 408
 409         if (cfi_info->pri_ext)
 410                 free(cfi_info->pri_ext);
 411
 412         pri_ext = malloc(sizeof(struct cfi_intel_pri_ext));
 413         if (pri_ext == NULL) {
 414                 LOG_ERROR("Out of memory");
 415                 return ERROR_FAIL;
 416         }
 417         cfi_info->pri_ext = pri_ext;
 418
 419         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0, &pri_ext->pri[0]);
 420         if (retval != ERROR_OK)
 421                 return retval;
 422         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 1, &pri_ext->pri[1]);
 423         if (retval != ERROR_OK)
 424                 return retval;
 425         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 2, &pri_ext->pri[2]);
 426         if (retval != ERROR_OK)
 427                 return retval;
 428
 429         if ((pri_ext->pri[0] != 'P') || (pri_ext->pri[1] != 'R') || (pri_ext->pri[2] != 'I')) {
 430                 retval = cfi_reset(bank);
 431                 if (retval != ERROR_OK)
 432                         return retval;
 433                 LOG_ERROR("Could not read bank flash bank information");
 434                 return ERROR_FLASH_BANK_INVALID;
 435         }
 436
 437         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 3, &pri_ext->major_version);
 438         if (retval != ERROR_OK)
 439                 return retval;
 440         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 4, &pri_ext->minor_version);
 441         if (retval != ERROR_OK)
 442                 return retval;
 443
 444         LOG_DEBUG("pri: '%c%c%c', version: %c.%c", pri_ext->pri[0], pri_ext->pri[1],
 445                 pri_ext->pri[2], pri_ext->major_version, pri_ext->minor_version);
 446
 447         retval = cfi_query_u32(bank, 0, cfi_info->pri_addr + 5, &pri_ext->feature_support);
 448         if (retval != ERROR_OK)
 449                 return retval;
 450         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 9, &pri_ext->suspend_cmd_support);
 451         if (retval != ERROR_OK)
 452                 return retval;
 453         retval = cfi_query_u16(bank, 0, cfi_info->pri_addr + 0xa, &pri_ext->blk_status_reg_mask);
 454         if (retval != ERROR_OK)
 455                 return retval;
 456
 457         LOG_DEBUG("feature_support: 0x%" PRIx32 ", suspend_cmd_support: "
 458                 "0x%x, blk_status_reg_mask: 0x%x",
 459                 pri_ext->feature_support,
 460                 pri_ext->suspend_cmd_support,
 461                 pri_ext->blk_status_reg_mask);
 462
 463         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xc, &pri_ext->vcc_optimal);
 464         if (retval != ERROR_OK)
 465                 return retval;
 466         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xd, &pri_ext->vpp_optimal);
 467         if (retval != ERROR_OK)
 468                 return retval;
 469
 470         LOG_DEBUG("Vcc opt: %x.%x, Vpp opt: %u.%x",
 471                 (pri_ext->vcc_optimal & 0xf0) >> 4, pri_ext->vcc_optimal & 0x0f,
 472                 (pri_ext->vpp_optimal & 0xf0) >> 4, pri_ext->vpp_optimal & 0x0f);
 473
 474         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xe, &pri_ext->num_protection_fields);
 475         if (retval != ERROR_OK)
 476                 return retval;
 477         if (pri_ext->num_protection_fields != 1) {
 478                 LOG_WARNING("expected one protection register field, but found %i",
 479                         pri_ext->num_protection_fields);
 480         }
 481
 482         retval = cfi_query_u16(bank, 0, cfi_info->pri_addr + 0xf, &pri_ext->prot_reg_addr);
 483         if (retval != ERROR_OK)
 484                 return retval;
 485         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0x11, &pri_ext->fact_prot_reg_size);
 486         if (retval != ERROR_OK)
 487                 return retval;
 488         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0x12, &pri_ext->user_prot_reg_size);
 489         if (retval != ERROR_OK)
 490                 return retval;
 491
 492         LOG_DEBUG("protection_fields: %i, prot_reg_addr: 0x%x, "
 493                 "factory pre-programmed: %i, user programmable: %i",
 494                 pri_ext->num_protection_fields, pri_ext->prot_reg_addr,
 495                 1 << pri_ext->fact_prot_reg_size, 1 << pri_ext->user_prot_reg_size);
 496
 497         return ERROR_OK;
 498 }
 499
 500 static int cfi_read_spansion_pri_ext(struct flash_bank *bank)
 501 {
 502         int retval;
 503         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 504         struct cfi_spansion_pri_ext *pri_ext;
 505
 506         if (cfi_info->pri_ext)
 507                 free(cfi_info->pri_ext);
 508
 509         pri_ext = malloc(sizeof(struct cfi_spansion_pri_ext));
 510         if (pri_ext == NULL) {
 511                 LOG_ERROR("Out of memory");
 512                 return ERROR_FAIL;
 513         }
 514         cfi_info->pri_ext = pri_ext;
 515
 516         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0, &pri_ext->pri[0]);
 517         if (retval != ERROR_OK)
 518                 return retval;
 519         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 1, &pri_ext->pri[1]);
 520         if (retval != ERROR_OK)
 521                 return retval;
 522         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 2, &pri_ext->pri[2]);
 523         if (retval != ERROR_OK)
 524                 return retval;
 525
 526         if ((pri_ext->pri[0] != 'P') || (pri_ext->pri[1] != 'R') || (pri_ext->pri[2] != 'I')) {
 527                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 528                 if (retval != ERROR_OK)
 529                         return retval;
 530                 LOG_ERROR("Could not read spansion bank information");
 531                 return ERROR_FLASH_BANK_INVALID;
 532         }
 533
 534         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 3, &pri_ext->major_version);
 535         if (retval != ERROR_OK)
 536                 return retval;
 537         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 4, &pri_ext->minor_version);
 538         if (retval != ERROR_OK)
 539                 return retval;
 540
 541         LOG_DEBUG("pri: '%c%c%c', version: %c.%c", pri_ext->pri[0], pri_ext->pri[1],
 542                 pri_ext->pri[2], pri_ext->major_version, pri_ext->minor_version);
 543
 544         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 5, &pri_ext->SiliconRevision);
 545         if (retval != ERROR_OK)
 546                 return retval;
 547         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 6, &pri_ext->EraseSuspend);
 548         if (retval != ERROR_OK)
 549                 return retval;
 550         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 7, &pri_ext->BlkProt);
 551         if (retval != ERROR_OK)
 552                 return retval;
 553         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 8, &pri_ext->TmpBlkUnprotect);
 554         if (retval != ERROR_OK)
 555                 return retval;
 556         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 9, &pri_ext->BlkProtUnprot);
 557         if (retval != ERROR_OK)
 558                 return retval;
 559         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 10, &pri_ext->SimultaneousOps);
 560         if (retval != ERROR_OK)
 561                 return retval;
 562         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 11, &pri_ext->BurstMode);
 563         if (retval != ERROR_OK)
 564                 return retval;
 565         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 12, &pri_ext->PageMode);
 566         if (retval != ERROR_OK)
 567                 return retval;
 568         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 13, &pri_ext->VppMin);
 569         if (retval != ERROR_OK)
 570                 return retval;
 571         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 14, &pri_ext->VppMax);
 572         if (retval != ERROR_OK)
 573                 return retval;
 574         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 15, &pri_ext->TopBottom);
 575         if (retval != ERROR_OK)
 576                 return retval;
 577
 578         LOG_DEBUG("Silicon Revision: 0x%x, Erase Suspend: 0x%x, Block protect: 0x%x",
 579                 pri_ext->SiliconRevision, pri_ext->EraseSuspend, pri_ext->BlkProt);
 580
 581         LOG_DEBUG("Temporary Unprotect: 0x%x, Block Protect Scheme: 0x%x, "
 582                 "Simultaneous Ops: 0x%x", pri_ext->TmpBlkUnprotect,
 583                 pri_ext->BlkProtUnprot, pri_ext->SimultaneousOps);
 584
 585         LOG_DEBUG("Burst Mode: 0x%x, Page Mode: 0x%x, ", pri_ext->BurstMode, pri_ext->PageMode);
 586
 587
 588         LOG_DEBUG("Vpp min: %u.%x, Vpp max: %u.%x",
 589                 (pri_ext->VppMin & 0xf0) >> 4, pri_ext->VppMin & 0x0f,
 590                 (pri_ext->VppMax & 0xf0) >> 4, pri_ext->VppMax & 0x0f);
 591
 592         LOG_DEBUG("WP# protection 0x%x", pri_ext->TopBottom);
 593
 594         /* default values for implementation specific workarounds */
 595         pri_ext->_unlock1 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock1;
 596         pri_ext->_unlock2 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock2;
 597         pri_ext->_reversed_geometry = 0;
 598
 599         return ERROR_OK;
 600 }
 601
 602 static int cfi_read_atmel_pri_ext(struct flash_bank *bank)
 603 {
 604         int retval;
 605         struct cfi_atmel_pri_ext atmel_pri_ext;
 606         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 607         struct cfi_spansion_pri_ext *pri_ext;
 608
 609         if (cfi_info->pri_ext)
 610                 free(cfi_info->pri_ext);
 611
 612         pri_ext = malloc(sizeof(struct cfi_spansion_pri_ext));
 613         if (pri_ext == NULL) {
 614                 LOG_ERROR("Out of memory");
 615                 return ERROR_FAIL;
 616         }
 617
 618         /* ATMEL devices use the same CFI primary command set (0x2) as AMD/Spansion,
 619          * but a different primary extended query table.
 620          * We read the atmel table, and prepare a valid AMD/Spansion query table.
 621          */
 622
 623         memset(pri_ext, 0, sizeof(struct cfi_spansion_pri_ext));
 624
 625         cfi_info->pri_ext = pri_ext;
 626
 627         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0, &atmel_pri_ext.pri[0]);
 628         if (retval != ERROR_OK)
 629                 return retval;
 630         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 1, &atmel_pri_ext.pri[1]);
 631         if (retval != ERROR_OK)
 632                 return retval;
 633         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 2, &atmel_pri_ext.pri[2]);
 634         if (retval != ERROR_OK)
 635                 return retval;
 636
 637         if ((atmel_pri_ext.pri[0] != 'P') || (atmel_pri_ext.pri[1] != 'R')
 638                         || (atmel_pri_ext.pri[2] != 'I')) {
 639                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 640                 if (retval != ERROR_OK)
 641                         return retval;
 642                 LOG_ERROR("Could not read atmel bank information");
 643                 return ERROR_FLASH_BANK_INVALID;
 644         }
 645
 646         pri_ext->pri[0] = atmel_pri_ext.pri[0];
 647         pri_ext->pri[1] = atmel_pri_ext.pri[1];
 648         pri_ext->pri[2] = atmel_pri_ext.pri[2];
 649
 650         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 3, &atmel_pri_ext.major_version);
 651         if (retval != ERROR_OK)
 652                 return retval;
 653         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 4, &atmel_pri_ext.minor_version);
 654         if (retval != ERROR_OK)
 655                 return retval;
 656
 657         LOG_DEBUG("pri: '%c%c%c', version: %c.%c", atmel_pri_ext.pri[0],
 658                 atmel_pri_ext.pri[1], atmel_pri_ext.pri[2],
 659                 atmel_pri_ext.major_version, atmel_pri_ext.minor_version);
 660
 661         pri_ext->major_version = atmel_pri_ext.major_version;
 662         pri_ext->minor_version = atmel_pri_ext.minor_version;
 663
 664         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 5, &atmel_pri_ext.features);
 665         if (retval != ERROR_OK)
 666                 return retval;
 667         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 6, &atmel_pri_ext.bottom_boot);
 668         if (retval != ERROR_OK)
 669                 return retval;
 670         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 7, &atmel_pri_ext.burst_mode);
 671         if (retval != ERROR_OK)
 672                 return retval;
 673         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 8, &atmel_pri_ext.page_mode);
 674         if (retval != ERROR_OK)
 675                 return retval;
 676
 677         LOG_DEBUG(
 678                 "features: 0x%2.2x, bottom_boot: 0x%2.2x, burst_mode: 0x%2.2x, page_mode: 0x%2.2x",
 679                 atmel_pri_ext.features,
 680                 atmel_pri_ext.bottom_boot,
 681                 atmel_pri_ext.burst_mode,
 682                 atmel_pri_ext.page_mode);
 683
 684         if (atmel_pri_ext.features & 0x02)
 685                 pri_ext->EraseSuspend = 2;
 686
 687         /* some chips got it backwards... */
 688         if (cfi_info->device_id == AT49BV6416 ||
 689                         cfi_info->device_id == AT49BV6416T) {
 690                 if (atmel_pri_ext.bottom_boot)
 691                         pri_ext->TopBottom = 3;
 692                 else
 693                         pri_ext->TopBottom = 2;
 694         } else {
 695                 if (atmel_pri_ext.bottom_boot)
 696                         pri_ext->TopBottom = 2;
 697                 else
 698                         pri_ext->TopBottom = 3;
 699         }
 700
 701         pri_ext->_unlock1 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock1;
 702         pri_ext->_unlock2 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock2;
 703
 704         return ERROR_OK;
 705 }
 706
 707 static int cfi_read_0002_pri_ext(struct flash_bank *bank)
 708 {
 709         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 710
 711         if (cfi_info->manufacturer == CFI_MFR_ATMEL)
 712                 return cfi_read_atmel_pri_ext(bank);
 713         else
 714                 return cfi_read_spansion_pri_ext(bank);
 715 }
 716
 717 static int cfi_spansion_info(struct flash_bank *bank, char *buf, int buf_size)
 718 {
 719         int printed;
 720         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 721         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 722
 723         printed = snprintf(buf, buf_size, "\nSpansion primary algorithm extend information:\n");
 724         buf += printed;
 725         buf_size -= printed;
 726
 727         printed = snprintf(buf, buf_size, "pri: '%c%c%c', version: %c.%c\n", pri_ext->pri[0],
 728                         pri_ext->pri[1], pri_ext->pri[2],
 729                         pri_ext->major_version, pri_ext->minor_version);
 730         buf += printed;
 731         buf_size -= printed;
 732
 733         printed = snprintf(buf, buf_size, "Silicon Rev.: 0x%x, Address Sensitive unlock: 0x%x\n",
 734                         (pri_ext->SiliconRevision) >> 2,
 735                         (pri_ext->SiliconRevision) & 0x03);
 736         buf += printed;
 737         buf_size -= printed;
 738
 739         printed = snprintf(buf, buf_size, "Erase Suspend: 0x%x, Sector Protect: 0x%x\n",
 740                         pri_ext->EraseSuspend,
 741                         pri_ext->BlkProt);
 742         buf += printed;
 743         buf_size -= printed;
 744
 745         snprintf(buf, buf_size, "VppMin: %u.%x, VppMax: %u.%x\n",
 746                 (pri_ext->VppMin & 0xf0) >> 4, pri_ext->VppMin & 0x0f,
 747                 (pri_ext->VppMax & 0xf0) >> 4, pri_ext->VppMax & 0x0f);
 748
 749         return ERROR_OK;
 750 }
 751
 752 static int cfi_intel_info(struct flash_bank *bank, char *buf, int buf_size)
 753 {
 754         int printed;
 755         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 756         struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
 757
 758         printed = snprintf(buf, buf_size, "\nintel primary algorithm extend information:\n");
 759         buf += printed;
 760         buf_size -= printed;
 761
 762         printed = snprintf(buf,
 763                         buf_size,
 764                         "pri: '%c%c%c', version: %c.%c\n",
 765                         pri_ext->pri[0],
 766                         pri_ext->pri[1],
 767                         pri_ext->pri[2],
 768                         pri_ext->major_version,
 769                         pri_ext->minor_version);
 770         buf += printed;
 771         buf_size -= printed;
 772
 773         printed = snprintf(buf,
 774                         buf_size,
 775                         "feature_support: 0x%" PRIx32 ", "
 776                         "suspend_cmd_support: 0x%x, blk_status_reg_mask: 0x%x\n",
 777                         pri_ext->feature_support,
 778                         pri_ext->suspend_cmd_support,
 779                         pri_ext->blk_status_reg_mask);
 780         buf += printed;
 781         buf_size -= printed;
 782
 783         printed = snprintf(buf, buf_size, "Vcc opt: %x.%x, Vpp opt: %u.%x\n",
 784                         (pri_ext->vcc_optimal & 0xf0) >> 4, pri_ext->vcc_optimal & 0x0f,
 785                         (pri_ext->vpp_optimal & 0xf0) >> 4, pri_ext->vpp_optimal & 0x0f);
 786         buf += printed;
 787         buf_size -= printed;
 788
 789         snprintf(buf, buf_size, "protection_fields: %i, prot_reg_addr: 0x%x, "
 790                 "factory pre-programmed: %i, user programmable: %i\n",
 791                 pri_ext->num_protection_fields, pri_ext->prot_reg_addr,
 792                 1 << pri_ext->fact_prot_reg_size, 1 << pri_ext->user_prot_reg_size);
 793
 794         return ERROR_OK;
 795 }
 796
 797 /* flash_bank cfi <base> <size> <chip_width> <bus_width> <target#> [options]
 798  */
 799 FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
 800 {
 801         struct cfi_flash_bank *cfi_info;
 802
 803         if (CMD_ARGC < 6)
 804                 return ERROR_COMMAND_SYNTAX_ERROR;
 805
 806         /* both widths must:
 807          * - not exceed max value;
 808          * - not be null;
 809          * - be equal to a power of 2.
 810          * bus must be wide enought to hold one chip */
 811         if ((bank->chip_width > CFI_MAX_CHIP_WIDTH)
 812                         || (bank->bus_width > CFI_MAX_BUS_WIDTH)
 813                         || (bank->chip_width == 0)
 814                         || (bank->bus_width == 0)
 815                         || (bank->chip_width & (bank->chip_width - 1))
 816                         || (bank->bus_width & (bank->bus_width - 1))
 817                         || (bank->chip_width > bank->bus_width)) {
 818                 LOG_ERROR("chip and bus width have to specified in bytes");
 819                 return ERROR_FLASH_BANK_INVALID;
 820         }
 821
 822         cfi_info = malloc(sizeof(struct cfi_flash_bank));
 823         cfi_info->probed = 0;
 824         cfi_info->erase_region_info = NULL;
 825         cfi_info->pri_ext = NULL;
 826         bank->driver_priv = cfi_info;
 827
 828         cfi_info->x16_as_x8 = 0;
 829         cfi_info->jedec_probe = 0;
 830         cfi_info->not_cfi = 0;
 831
 832         for (unsigned i = 6; i < CMD_ARGC; i++) {
 833                 if (strcmp(CMD_ARGV[i], "x16_as_x8") == 0)
 834                         cfi_info->x16_as_x8 = 1;
 835                 else if (strcmp(CMD_ARGV[i], "jedec_probe") == 0)
 836                         cfi_info->jedec_probe = 1;
 837         }
 838
 839         /* bank wasn't probed yet */
 840         cfi_info->qry[0] = 0xff;
 841
 842         return ERROR_OK;
 843 }
 844
 845 static int cfi_intel_erase(struct flash_bank *bank, int first, int last)
 846 {
 847         int retval;
 848         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 849         int i;
 850
 851         cfi_intel_clear_status_register(bank);
 852
 853         for (i = first; i <= last; i++) {
 854                 retval = cfi_send_command(bank, 0x20, flash_address(bank, i, 0x0));
 855                 if (retval != ERROR_OK)
 856                         return retval;
 857
 858                 retval = cfi_send_command(bank, 0xd0, flash_address(bank, i, 0x0));
 859                 if (retval != ERROR_OK)
 860                         return retval;
 861
 862                 uint8_t status;
 863                 retval = cfi_intel_wait_status_busy(bank, cfi_info->block_erase_timeout, &status);
 864                 if (retval != ERROR_OK)
 865                         return retval;
 866
 867                 if (status == 0x80)
 868                         bank->sectors[i].is_erased = 1;
 869                 else {
 870                         retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
 871                         if (retval != ERROR_OK)
 872                                 return retval;
 873
 874                         LOG_ERROR("couldn't erase block %i of flash bank at base 0x%" PRIx32,
 875                                 i,
 876                                 bank->base);
 877                         return ERROR_FLASH_OPERATION_FAILED;
 878                 }
 879         }
 880
 881         return cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
 882 }
 883
 884 static int cfi_spansion_erase(struct flash_bank *bank, int first, int last)
 885 {
 886         int retval;
 887         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 888         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 889         int i;
 890
 891         for (i = first; i <= last; i++) {
 892                 retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
 893                 if (retval != ERROR_OK)
 894                         return retval;
 895
 896                 retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
 897                 if (retval != ERROR_OK)
 898                         return retval;
 899
 900                 retval = cfi_send_command(bank, 0x80, flash_address(bank, 0, pri_ext->_unlock1));
 901                 if (retval != ERROR_OK)
 902                         return retval;
 903
 904                 retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
 905                 if (retval != ERROR_OK)
 906                         return retval;
 907
 908                 retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
 909                 if (retval != ERROR_OK)
 910                         return retval;
 911
 912                 retval = cfi_send_command(bank, 0x30, flash_address(bank, i, 0x0));
 913                 if (retval != ERROR_OK)
 914                         return retval;
 915
 916                 if (cfi_spansion_wait_status_busy(bank, cfi_info->block_erase_timeout) == ERROR_OK)
 917                         bank->sectors[i].is_erased = 1;
 918                 else {
 919                         retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 920                         if (retval != ERROR_OK)
 921                                 return retval;
 922
 923                         LOG_ERROR("couldn't erase block %i of flash bank at base 0x%"
 924                                 PRIx32, i, bank->base);
 925                         return ERROR_FLASH_OPERATION_FAILED;
 926                 }
 927         }
 928
 929         return cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 930 }
 931
 932 static int cfi_erase(struct flash_bank *bank, int first, int last)
 933 {
 934         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 935
 936         if (bank->target->state != TARGET_HALTED) {
 937                 LOG_ERROR("Target not halted");
 938                 return ERROR_TARGET_NOT_HALTED;
 939         }
 940
 941         if ((first < 0) || (last < first) || (last >= bank->num_sectors))
 942                 return ERROR_FLASH_SECTOR_INVALID;
 943
 944         if (cfi_info->qry[0] != 'Q')
 945                 return ERROR_FLASH_BANK_NOT_PROBED;
 946
 947         switch (cfi_info->pri_id) {
 948                 case 1:
 949                 case 3:
 950                         return cfi_intel_erase(bank, first, last);
 951                         break;
 952                 case 2:
 953                         return cfi_spansion_erase(bank, first, last);
 954                         break;
 955                 default:
 956                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
 957                         break;
 958         }
 959
 960         return ERROR_OK;
 961 }
 962
 963 static int cfi_intel_protect(struct flash_bank *bank, int set, int first, int last)
 964 {
 965         int retval;
 966         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 967         struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
 968         int retry = 0;
 969         int i;
 970
 971         /* if the device supports neither legacy lock/unlock (bit 3) nor
 972          * instant individual block locking (bit 5).
 973          */
 974         if (!(pri_ext->feature_support & 0x28)) {
 975                 LOG_ERROR("lock/unlock not supported on flash");
 976                 return ERROR_FLASH_OPERATION_FAILED;
 977         }
 978
 979         cfi_intel_clear_status_register(bank);
 980
 981         for (i = first; i <= last; i++) {
 982                 retval = cfi_send_command(bank, 0x60, flash_address(bank, i, 0x0));
 983                 if (retval != ERROR_OK)
 984                         return retval;
 985                 if (set) {
 986                         retval = cfi_send_command(bank, 0x01, flash_address(bank, i, 0x0));
 987                         if (retval != ERROR_OK)
 988                                 return retval;
 989                         bank->sectors[i].is_protected = 1;
 990                 } else {
 991                         retval = cfi_send_command(bank, 0xd0, flash_address(bank, i, 0x0));
 992                         if (retval != ERROR_OK)
 993                                 return retval;
 994                         bank->sectors[i].is_protected = 0;
 995                 }
 996
 997                 /* instant individual block locking doesn't require reading of the status register
 998                  **/
 999                 if (!(pri_ext->feature_support & 0x20)) {
1000                         /* Clear lock bits operation may take up to 1.4s */
1001                         uint8_t status;
1002                         retval = cfi_intel_wait_status_busy(bank, 1400, &status);
1003                         if (retval != ERROR_OK)
1004                                 return retval;
1005                 } else {
1006                         uint8_t block_status;
1007                         /* read block lock bit, to verify status */
1008                         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, 0x55));
1009                         if (retval != ERROR_OK)
1010                                 return retval;
1011                         retval = cfi_get_u8(bank, i, 0x2, &block_status);
1012                         if (retval != ERROR_OK)
1013                                 return retval;
1014
1015                         if ((block_status & 0x1) != set) {
1016                                 LOG_ERROR(
1017                                         "couldn't change block lock status (set = %i, block_status = 0x%2.2x)",
1018                                         set, block_status);
1019                                 retval = cfi_send_command(bank, 0x70, flash_address(bank, 0, 0x55));
1020                                 if (retval != ERROR_OK)
1021                                         return retval;
1022                                 uint8_t status;
1023                                 retval = cfi_intel_wait_status_busy(bank, 10, &status);
1024                                 if (retval != ERROR_OK)
1025                                         return retval;
1026
1027                                 if (retry > 10)
1028                                         return ERROR_FLASH_OPERATION_FAILED;
1029                                 else {
1030                                         i--;
1031                                         retry++;
1032                                 }
1033                         }
1034                 }
1035         }
1036
1037         /* if the device doesn't support individual block lock bits set/clear,
1038          * all blocks have been unlocked in parallel, so we set those that should be protected
1039          */
1040         if ((!set) && (!(pri_ext->feature_support & 0x20))) {
1041                 /* FIX!!! this code path is broken!!!
1042                  *
1043                  * The correct approach is:
1044                  *
1045                  * 1. read out current protection status
1046                  *
1047                  * 2. override read out protection status w/unprotected.
1048                  *
1049                  * 3. re-protect what should be protected.
1050                  *
1051                  */
1052                 for (i = 0; i < bank->num_sectors; i++) {
1053                         if (bank->sectors[i].is_protected == 1) {
1054                                 cfi_intel_clear_status_register(bank);
1055
1056                                 retval = cfi_send_command(bank, 0x60, flash_address(bank, i, 0x0));
1057                                 if (retval != ERROR_OK)
1058                                         return retval;
1059
1060                                 retval = cfi_send_command(bank, 0x01, flash_address(bank, i, 0x0));
1061                                 if (retval != ERROR_OK)
1062                                         return retval;
1063
1064                                 uint8_t status;
1065                                 retval = cfi_intel_wait_status_busy(bank, 100, &status);
1066                                 if (retval != ERROR_OK)
1067                                         return retval;
1068                         }
1069                 }
1070         }
1071
1072         return cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
1073 }
1074
1075 static int cfi_protect(struct flash_bank *bank, int set, int first, int last)
1076 {
1077         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1078
1079         if (bank->target->state != TARGET_HALTED) {
1080                 LOG_ERROR("Target not halted");
1081                 return ERROR_TARGET_NOT_HALTED;
1082         }
1083
1084         if ((first < 0) || (last < first) || (last >= bank->num_sectors)) {
1085                 LOG_ERROR("Invalid sector range");
1086                 return ERROR_FLASH_SECTOR_INVALID;
1087         }
1088
1089         if (cfi_info->qry[0] != 'Q')
1090                 return ERROR_FLASH_BANK_NOT_PROBED;
1091
1092         switch (cfi_info->pri_id) {
1093                 case 1:
1094                 case 3:
1095                         return cfi_intel_protect(bank, set, first, last);
1096                         break;
1097                 default:
1098                         LOG_WARNING("protect: cfi primary command set %i unsupported", cfi_info->pri_id);
1099                         return ERROR_OK;
1100         }
1101 }
1102
1103 /* Convert code image to target endian
1104  * FIXME create general block conversion fcts in target.c?) */
1105 static void cfi_fix_code_endian(struct target *target, uint8_t *dest,
1106         const uint32_t *src, uint32_t count)
1107 {
1108         uint32_t i;
1109         for (i = 0; i < count; i++) {
1110                 target_buffer_set_u32(target, dest, *src);
1111                 dest += 4;
1112                 src++;
1113         }
1114 }
1115
1116 static uint32_t cfi_command_val(struct flash_bank *bank, uint8_t cmd)
1117 {
1118         struct target *target = bank->target;
1119
1120         uint8_t buf[CFI_MAX_BUS_WIDTH];
1121         cfi_command(bank, cmd, buf);
1122         switch (bank->bus_width) {
1123                 case 1:
1124                         return buf[0];
1125                         break;
1126                 case 2:
1127                         return target_buffer_get_u16(target, buf);
1128                         break;
1129                 case 4:
1130                         return target_buffer_get_u32(target, buf);
1131                         break;
1132                 default:
1133                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1134                                         bank->bus_width);
1135                         return 0;
1136         }
1137 }
1138
1139 static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
1140         uint32_t address, uint32_t count)
1141 {
1142         struct target *target = bank->target;
1143         struct reg_param reg_params[7];
1144         struct arm_algorithm arm_algo;
1145         struct working_area *write_algorithm;
1146         struct working_area *source = NULL;
1147         uint32_t buffer_size = 32768;
1148         uint32_t write_command_val, busy_pattern_val, error_pattern_val;
1149
1150         /* algorithm register usage:
1151          * r0: source address (in RAM)
1152          * r1: target address (in Flash)
1153          * r2: count
1154          * r3: flash write command
1155          * r4: status byte (returned to host)
1156          * r5: busy test pattern
1157          * r6: error test pattern
1158          */
1159
1160         /* see contib/loaders/flash/armv4_5_cfi_intel_32.s for src */
1161         static const uint32_t word_32_code[] = {
1162                 0xe4904004,     /* loop:        ldr r4, [r0], #4 */
1163                 0xe5813000,     /*              str r3, [r1] */
1164                 0xe5814000,     /*              str r4, [r1] */
1165                 0xe5914000,     /* busy:  ldr r4, [r1] */
1166                 0xe0047005,     /*              and r7, r4, r5 */
1167                 0xe1570005,     /*              cmp r7, r5 */
1168                 0x1afffffb,     /*              bne busy */
1169                 0xe1140006,     /*              tst r4, r6 */
1170                 0x1a000003,     /*              bne done */
1171                 0xe2522001,     /*              subs r2, r2, #1 */
1172                 0x0a000001,     /*              beq done */
1173                 0xe2811004,     /*              add r1, r1 #4 */
1174                 0xeafffff2,     /*              b loop */
1175                 0xeafffffe      /* done:        b -2 */
1176         };
1177
1178         /* see contib/loaders/flash/armv4_5_cfi_intel_16.s for src */
1179         static const uint32_t word_16_code[] = {
1180                 0xe0d040b2,     /* loop:        ldrh r4, [r0], #2 */
1181                 0xe1c130b0,     /*              strh r3, [r1] */
1182                 0xe1c140b0,     /*              strh r4, [r1] */
1183                 0xe1d140b0,     /* busy ldrh r4, [r1] */
1184                 0xe0047005,     /*              and r7, r4, r5 */
1185                 0xe1570005,     /*              cmp r7, r5 */
1186                 0x1afffffb,     /*              bne busy */
1187                 0xe1140006,     /*              tst r4, r6 */
1188                 0x1a000003,     /*              bne done */
1189                 0xe2522001,     /*              subs r2, r2, #1 */
1190                 0x0a000001,     /*              beq done */
1191                 0xe2811002,     /*              add r1, r1 #2 */
1192                 0xeafffff2,     /*              b loop */
1193                 0xeafffffe      /* done:        b -2 */
1194         };
1195
1196         /* see contib/loaders/flash/armv4_5_cfi_intel_8.s for src */
1197         static const uint32_t word_8_code[] = {
1198                 0xe4d04001,     /* loop:        ldrb r4, [r0], #1 */
1199                 0xe5c13000,     /*              strb r3, [r1] */
1200                 0xe5c14000,     /*              strb r4, [r1] */
1201                 0xe5d14000,     /* busy ldrb r4, [r1] */
1202                 0xe0047005,     /*              and r7, r4, r5 */
1203                 0xe1570005,     /*              cmp r7, r5 */
1204                 0x1afffffb,     /*              bne busy */
1205                 0xe1140006,     /*              tst r4, r6 */
1206                 0x1a000003,     /*              bne done */
1207                 0xe2522001,     /*              subs r2, r2, #1 */
1208                 0x0a000001,     /*              beq done */
1209                 0xe2811001,     /*              add r1, r1 #1 */
1210                 0xeafffff2,     /*              b loop */
1211                 0xeafffffe      /* done:        b -2 */
1212         };
1213         uint8_t target_code[4*CFI_MAX_INTEL_CODESIZE];
1214         const uint32_t *target_code_src;
1215         uint32_t target_code_size;
1216         int retval = ERROR_OK;
1217
1218         /* check we have a supported arch */
1219         if (is_arm(target_to_arm(target))) {
1220                 /* All other ARM CPUs have 32 bit instructions */
1221                 arm_algo.common_magic = ARM_COMMON_MAGIC;
1222                 arm_algo.core_mode = ARM_MODE_SVC;
1223                 arm_algo.core_state = ARM_STATE_ARM;
1224         } else {
1225                 LOG_ERROR("Unknown architecture");
1226                 return ERROR_FAIL;
1227         }
1228
1229         cfi_intel_clear_status_register(bank);
1230
1231         /* If we are setting up the write_algorith, we need target_code_src
1232          * if not we only need target_code_size. */
1233
1234         /* However, we don't want to create multiple code paths, so we
1235          * do the unecessary evaluation of target_code_src, which the
1236          * compiler will probably nicely optimize away if not needed */
1237
1238         /* prepare algorithm code for target endian */
1239         switch (bank->bus_width) {
1240                 case 1:
1241                         target_code_src = word_8_code;
1242                         target_code_size = sizeof(word_8_code);
1243                         break;
1244                 case 2:
1245                         target_code_src = word_16_code;
1246                         target_code_size = sizeof(word_16_code);
1247                         break;
1248                 case 4:
1249                         target_code_src = word_32_code;
1250                         target_code_size = sizeof(word_32_code);
1251                         break;
1252                 default:
1253                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1254                                         bank->bus_width);
1255                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1256         }
1257
1258         /* flash write code */
1259         if (target_code_size > sizeof(target_code)) {
1260                 LOG_WARNING("Internal error - target code buffer to small. "
1261                                 "Increase CFI_MAX_INTEL_CODESIZE and recompile.");
1262                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1263         }
1264         cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
1265
1266         /* Get memory for block write handler */
1267         retval = target_alloc_working_area(target,
1268                         target_code_size,
1269                         &write_algorithm);
1270         if (retval != ERROR_OK) {
1271                 LOG_WARNING("No working area available, can't do block memory writes");
1272                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1273         }
1274         ;
1275
1276         /* write algorithm code to working area */
1277         retval = target_write_buffer(target, write_algorithm->address,
1278                         target_code_size, target_code);
1279         if (retval != ERROR_OK) {
1280                 LOG_ERROR("Unable to write block write code to target");
1281                 goto cleanup;
1282         }
1283
1284         /* Get a workspace buffer for the data to flash starting with 32k size.
1285            Half size until buffer would be smaller 256 Bytem then fail back */
1286         /* FIXME Why 256 bytes, why not 32 bytes (smallest flash write page */
1287         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
1288                 buffer_size /= 2;
1289                 if (buffer_size <= 256) {
1290                         LOG_WARNING(
1291                                 "no large enough working area available, can't do block memory writes");
1292                         retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1293                         goto cleanup;
1294                 }
1295         }
1296         ;
1297
1298         /* setup algo registers */
1299         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
1300         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
1301         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
1302         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
1303         init_reg_param(&reg_params[4], "r4", 32, PARAM_IN);
1304         init_reg_param(&reg_params[5], "r5", 32, PARAM_OUT);
1305         init_reg_param(&reg_params[6], "r6", 32, PARAM_OUT);
1306
1307         /* prepare command and status register patterns */
1308         write_command_val = cfi_command_val(bank, 0x40);
1309         busy_pattern_val  = cfi_command_val(bank, 0x80);
1310         error_pattern_val = cfi_command_val(bank, 0x7e);
1311
1312         LOG_DEBUG("Using target buffer at 0x%08" PRIx32 " and of size 0x%04" PRIx32,
1313                 source->address, buffer_size);
1314
1315         /* Programming main loop */
1316         while (count > 0) {
1317                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
1318                 uint32_t wsm_error;
1319
1320                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
1321                 if (retval != ERROR_OK)
1322                         goto cleanup;
1323
1324                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
1325                 buf_set_u32(reg_params[1].value, 0, 32, address);
1326                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
1327
1328                 buf_set_u32(reg_params[3].value, 0, 32, write_command_val);
1329                 buf_set_u32(reg_params[5].value, 0, 32, busy_pattern_val);
1330                 buf_set_u32(reg_params[6].value, 0, 32, error_pattern_val);
1331
1332                 LOG_DEBUG("Write 0x%04" PRIx32 " bytes to flash at 0x%08" PRIx32,
1333                         thisrun_count, address);
1334
1335                 /* Execute algorithm, assume breakpoint for last instruction */
1336                 retval = target_run_algorithm(target, 0, NULL, 7, reg_params,
1337                                 write_algorithm->address,
1338                                 write_algorithm->address + target_code_size -
1339                                 sizeof(uint32_t),
1340                                 10000,  /* 10s should be enough for max. 32k of data */
1341                                 &arm_algo);
1342
1343                 /* On failure try a fall back to direct word writes */
1344                 if (retval != ERROR_OK) {
1345                         cfi_intel_clear_status_register(bank);
1346                         LOG_ERROR(
1347                                 "Execution of flash algorythm failed. Can't fall back. Please report.");
1348                         retval = ERROR_FLASH_OPERATION_FAILED;
1349                         /* retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE; */
1350                         /* FIXME To allow fall back or recovery, we must save the actual status
1351                          * somewhere, so that a higher level code can start recovery. */
1352                         goto cleanup;
1353                 }
1354
1355                 /* Check return value from algo code */
1356                 wsm_error = buf_get_u32(reg_params[4].value, 0, 32) & error_pattern_val;
1357                 if (wsm_error) {
1358                         /* read status register (outputs debug inforation) */
1359                         uint8_t status;
1360                         cfi_intel_wait_status_busy(bank, 100, &status);
1361                         cfi_intel_clear_status_register(bank);
1362                         retval = ERROR_FLASH_OPERATION_FAILED;
1363                         goto cleanup;
1364                 }
1365
1366                 buffer += thisrun_count;
1367                 address += thisrun_count;
1368                 count -= thisrun_count;
1369
1370                 keep_alive();
1371         }
1372
1373         /* free up resources */
1374 cleanup:
1375         if (source)
1376                 target_free_working_area(target, source);
1377
1378         target_free_working_area(target, write_algorithm);
1379
1380         destroy_reg_param(&reg_params[0]);
1381         destroy_reg_param(&reg_params[1]);
1382         destroy_reg_param(&reg_params[2]);
1383         destroy_reg_param(&reg_params[3]);
1384         destroy_reg_param(&reg_params[4]);
1385         destroy_reg_param(&reg_params[5]);
1386         destroy_reg_param(&reg_params[6]);
1387
1388         return retval;
1389 }
1390
1391 static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffer,
1392         uint32_t address, uint32_t count)
1393 {
1394         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1395         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
1396         struct target *target = bank->target;
1397         struct reg_param reg_params[10];
1398         struct mips32_algorithm mips32_info;
1399         struct working_area *write_algorithm;
1400         struct working_area *source;
1401         uint32_t buffer_size = 32768;
1402         uint32_t status;
1403         int retval = ERROR_OK;
1404
1405         /* input parameters -
1406          *      4  A0 = source address
1407          *      5  A1 = destination address
1408          *      6  A2 = number of writes
1409          *      7  A3 = flash write command
1410          *      8  T0 = constant to mask DQ7 bits (also used for Dq5 with shift)
1411          * output parameters -
1412          *      9  T1 = 0x80 ok 0x00 bad
1413          * temp registers -
1414          *      10 T2 = value read from flash to test status
1415          *      11 T3 = holding register
1416          * unlock registers -
1417          *  12 T4 = unlock1_addr
1418          *  13 T5 = unlock1_cmd
1419          *  14 T6 = unlock2_addr
1420          *  15 T7 = unlock2_cmd */
1421
1422         static const uint32_t mips_word_16_code[] = {
1423                 /* start:       */
1424                 MIPS32_LHU(9, 0, 4),            /* lhu $t1, ($a0)               ; out = &saddr                          */
1425                 MIPS32_ADDI(4, 4, 2),           /* addi $a0, $a0, 2             ; saddr += 2                            */
1426                 MIPS32_SH(13, 0, 12),           /* sh $t5, ($t4)                ; *fl_unl_addr1 =
1427                                                  *fl_unl_cmd1           */
1428                 MIPS32_SH(15, 0, 14),           /* sh $t7, ($t6)                ; *fl_unl_addr2 =
1429                                                  *fl_unl_cmd2           */
1430                 MIPS32_SH(7, 0, 12),            /* sh $a3, ($t4)                ; *fl_unl_addr1 =
1431                                                  *fl_write_cmd          */
1432                 MIPS32_SH(9, 0, 5),             /* sh $t1, ($a1)                ; *daddr = out                          */
1433                 MIPS32_NOP,                     /* nop                                                                  */
1434                 /* busy:        */
1435                 MIPS32_LHU(10, 0, 5),           /* lhu $t2, ($a1)               ; temp1 = *daddr                        */
1436                 MIPS32_XOR(11, 9, 10),          /* xor $t3, $a0, $t2            ; temp2 = out ^
1437                                                  *temp1;                        */
1438                 MIPS32_AND(11, 8, 11),          /* and $t3, $t0, $t3            ; temp2 = temp2 &
1439                                                  *DQ7mask               */
1440                 MIPS32_BNE(11, 8, 13),          /* bne $t3, $t0, cont           ; if (temp2 !=
1441                                                  *DQ7mask) goto cont    */
1442                 MIPS32_NOP,                     /* nop                                                                  */
1443
1444                 MIPS32_SRL(10, 8, 2),           /* srl $t2,$t0,2                ; temp1 = DQ7mask >>
1445                                                  *2                     */
1446                 MIPS32_AND(11, 10, 11),         /* and $t3, $t2, $t3            ; temp2 = temp2 &
1447                                                  *temp1                 */
1448                 MIPS32_BNE(11, 10, NEG16(8)),   /* bne $t3, $t2, busy           ; if (temp2 !=
1449                                                  *temp1) goto busy              */
1450                 MIPS32_NOP,                     /* nop                                                                  */
1451
1452                 MIPS32_LHU(10, 0, 5),           /* lhu $t2, ($a1)               ; temp1 = *daddr                        */
1453                 MIPS32_XOR(11, 9, 10),          /* xor $t3, $a0, $t2            ; temp2 = out ^
1454                                                  *temp1;                        */
1455                 MIPS32_AND(11, 8, 11),          /* and $t3, $t0, $t3            ; temp2 = temp2 &
1456                                                  *DQ7mask               */
1457                 MIPS32_BNE(11, 8, 4),           /* bne $t3, $t0, cont           ; if (temp2 !=
1458                                                  *DQ7mask) goto cont    */
1459                 MIPS32_NOP,                     /* nop                                                                  */
1460
1461                 MIPS32_XOR(9, 9, 9),            /* xor $t1, $t1, $t1            ; out = 0                               */
1462                 MIPS32_BEQ(9, 0, 11),           /* beq $t1, $zero, done         ; if (out == 0) goto
1463                                                  *done          */
1464                 MIPS32_NOP,                     /* nop                                                                  */
1465                 /* cont:        */
1466                 MIPS32_ADDI(6, 6, NEG16(1)),    /* addi, $a2, $a2, -1           ; numwrites--                           */
1467                 MIPS32_BNE(6, 0, 5),            /* bne $a2, $zero, cont2        ; if (numwrite != 0)
1468                                                  *goto cont2            */
1469                 MIPS32_NOP,                     /* nop                                                                  */
1470
1471                 MIPS32_LUI(9, 0),               /* lui $t1, 0                                                           */
1472                 MIPS32_ORI(9, 9, 0x80),         /* ori $t1, $t1, 0x80           ; out = 0x80                            */
1473
1474                 MIPS32_B(4),                    /* b done                       ; goto done                             */
1475                 MIPS32_NOP,                     /* nop                                                                  */
1476                 /* cont2:       */
1477                 MIPS32_ADDI(5, 5, 2),           /* addi $a0, $a0, 2             ; daddr += 2                            */
1478                 MIPS32_B(NEG16(33)),            /* b start                      ; goto start                            */
1479                 MIPS32_NOP,                     /* nop                                                                  */
1480                 /* done:
1481                  *MIPS32_B(NEG16(1)),   */      /* b done                       ; goto done                             */
1482                 MIPS32_SDBBP,                   /* sdbbp                        ; break();                              */
1483                 /*MIPS32_B(NEG16(33)),  */      /* b start                      ; goto start
1484                  * MIPS32_NOP, */
1485         };
1486
1487         mips32_info.common_magic = MIPS32_COMMON_MAGIC;
1488         mips32_info.isa_mode = MIPS32_ISA_MIPS32;
1489
1490         int target_code_size = 0;
1491         const uint32_t *target_code_src = NULL;
1492
1493         switch (bank->bus_width) {
1494                 case 2:
1495                         /* Check for DQ5 support */
1496                         if (cfi_info->status_poll_mask & (1 << 5)) {
1497                                 target_code_src = mips_word_16_code;
1498                                 target_code_size = sizeof(mips_word_16_code);
1499                         } else {
1500                                 LOG_ERROR("Need DQ5 support");
1501                                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1502                                 /* target_code_src = mips_word_16_code_dq7only; */
1503                                 /* target_code_size = sizeof(mips_word_16_code_dq7only); */
1504                         }
1505                         break;
1506                 default:
1507                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1508                                         bank->bus_width);
1509                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1510         }
1511
1512         /* flash write code */
1513         uint8_t *target_code;
1514
1515         /* convert bus-width dependent algorithm code to correct endiannes */
1516         target_code = malloc(target_code_size);
1517         if (target_code == NULL) {
1518                 LOG_ERROR("Out of memory");
1519                 return ERROR_FAIL;
1520         }
1521         cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
1522
1523         /* allocate working area */
1524         retval = target_alloc_working_area(target, target_code_size,
1525                         &write_algorithm);
1526         if (retval != ERROR_OK) {
1527                 free(target_code);
1528                 return retval;
1529         }
1530
1531         /* write algorithm code to working area */
1532         retval = target_write_buffer(target, write_algorithm->address,
1533                         target_code_size, target_code);
1534         if (retval != ERROR_OK) {
1535                 free(target_code);
1536                 return retval;
1537         }
1538
1539         free(target_code);
1540
1541         /* the following code still assumes target code is fixed 24*4 bytes */
1542
1543         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
1544                 buffer_size /= 2;
1545                 if (buffer_size <= 256) {
1546                         /* we already allocated the writing code, but failed to get a
1547                          * buffer, free the algorithm */
1548                         target_free_working_area(target, write_algorithm);
1549
1550                         LOG_WARNING(
1551                                 "not enough working area available, can't do block memory writes");
1552                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1553                 }
1554         }
1555         ;
1556
1557         init_reg_param(&reg_params[0], "a0", 32, PARAM_OUT);
1558         init_reg_param(&reg_params[1], "a1", 32, PARAM_OUT);
1559         init_reg_param(&reg_params[2], "a2", 32, PARAM_OUT);
1560         init_reg_param(&reg_params[3], "a3", 32, PARAM_OUT);
1561         init_reg_param(&reg_params[4], "t0", 32, PARAM_OUT);
1562         init_reg_param(&reg_params[5], "t1", 32, PARAM_IN);
1563         init_reg_param(&reg_params[6], "t4", 32, PARAM_OUT);
1564         init_reg_param(&reg_params[7], "t5", 32, PARAM_OUT);
1565         init_reg_param(&reg_params[8], "t6", 32, PARAM_OUT);
1566         init_reg_param(&reg_params[9], "t7", 32, PARAM_OUT);
1567
1568         while (count > 0) {
1569                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
1570
1571                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
1572                 if (retval != ERROR_OK)
1573                         break;
1574
1575                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
1576                 buf_set_u32(reg_params[1].value, 0, 32, address);
1577                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
1578                 buf_set_u32(reg_params[3].value, 0, 32, cfi_command_val(bank, 0xA0));
1579                 buf_set_u32(reg_params[4].value, 0, 32, cfi_command_val(bank, 0x80));
1580                 buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
1581                 buf_set_u32(reg_params[7].value, 0, 32, 0xaaaaaaaa);
1582                 buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
1583                 buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
1584
1585                 retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
1586                                 write_algorithm->address,
1587                                 write_algorithm->address + ((target_code_size) - 4),
1588                                 10000, &mips32_info);
1589                 if (retval != ERROR_OK)
1590                         break;
1591
1592                 status = buf_get_u32(reg_params[5].value, 0, 32);
1593                 if (status != 0x80) {
1594                         LOG_ERROR("flash write block failed status: 0x%" PRIx32, status);
1595                         retval = ERROR_FLASH_OPERATION_FAILED;
1596                         break;
1597                 }
1598
1599                 buffer += thisrun_count;
1600                 address += thisrun_count;
1601                 count -= thisrun_count;
1602         }
1603
1604         target_free_all_working_areas(target);
1605
1606         destroy_reg_param(&reg_params[0]);
1607         destroy_reg_param(&reg_params[1]);
1608         destroy_reg_param(&reg_params[2]);
1609         destroy_reg_param(&reg_params[3]);
1610         destroy_reg_param(&reg_params[4]);
1611         destroy_reg_param(&reg_params[5]);
1612         destroy_reg_param(&reg_params[6]);
1613         destroy_reg_param(&reg_params[7]);
1614         destroy_reg_param(&reg_params[8]);
1615         destroy_reg_param(&reg_params[9]);
1616
1617         return retval;
1618 }
1619
1620 static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
1621         uint32_t address, uint32_t count)
1622 {
1623         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1624         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
1625         struct target *target = bank->target;
1626         struct reg_param reg_params[10];
1627         void *arm_algo;
1628         struct arm_algorithm armv4_5_algo;
1629         struct armv7m_algorithm armv7m_algo;
1630         struct working_area *write_algorithm;
1631         struct working_area *source;
1632         uint32_t buffer_size = 32768;
1633         uint32_t status;
1634         int retval = ERROR_OK;
1635
1636         /* input parameters -
1637          *      R0 = source address
1638          *      R1 = destination address
1639          *      R2 = number of writes
1640          *      R3 = flash write command
1641          *      R4 = constant to mask DQ7 bits (also used for Dq5 with shift)
1642          * output parameters -
1643          *      R5 = 0x80 ok 0x00 bad
1644          * temp registers -
1645          *      R6 = value read from flash to test status
1646          *      R7 = holding register
1647          * unlock registers -
1648          *  R8 = unlock1_addr
1649          *  R9 = unlock1_cmd
1650          *  R10 = unlock2_addr
1651          *  R11 = unlock2_cmd */
1652
1653         /* see contib/loaders/flash/armv4_5_cfi_span_32.s for src */
1654         static const uint32_t armv4_5_word_32_code[] = {
1655                 /* 00008100 <sp_32_code>:               */
1656                 0xe4905004,             /* ldr  r5, [r0], #4                    */
1657                 0xe5889000,             /* str  r9, [r8]                                */
1658                 0xe58ab000,             /* str  r11, [r10]                              */
1659                 0xe5883000,             /* str  r3, [r8]                                */
1660                 0xe5815000,             /* str  r5, [r1]                                */
1661                 0xe1a00000,             /* nop                                                  */
1662                 /*
1663                  * 00008110 <sp_32_busy>:               */
1664                 0xe5916000,             /* ldr  r6, [r1]                                */
1665                 0xe0257006,             /* eor  r7, r5, r6                              */
1666                 0xe0147007,             /* ands r7, r4, r7                              */
1667                 0x0a000007,             /* beq  8140 <sp_32_cont> ; b if DQ7 == Data7 */
1668                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1669                 0x0afffff9,             /* beq  8110 <sp_32_busy> ;     b if DQ5 low */
1670                 0xe5916000,             /* ldr  r6, [r1]                                */
1671                 0xe0257006,             /* eor  r7, r5, r6                              */
1672                 0xe0147007,             /* ands r7, r4, r7                              */
1673                 0x0a000001,             /* beq  8140 <sp_32_cont> ; b if DQ7 == Data7 */
1674                 0xe3a05000,             /* mov  r5, #0  ; 0x0 - return 0x00, error */
1675                 0x1a000004,             /* bne  8154 <sp_32_done>               */
1676                 /*
1677                  * 00008140 <sp_32_cont>:               */
1678                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1679                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1680                 0x0a000001,             /* beq  8154 <sp_32_done>               */
1681                 0xe2811004,             /* add  r1, r1, #4      ; 0x4           */
1682                 0xeaffffe8,             /* b    8100 <sp_32_code>               */
1683                 /*
1684                  * 00008154 <sp_32_done>:               */
1685                 0xeafffffe              /* b    8154 <sp_32_done>               */
1686         };
1687
1688         /* see contib/loaders/flash/armv4_5_cfi_span_16.s for src */
1689         static const uint32_t armv4_5_word_16_code[] = {
1690                 /* 00008158 <sp_16_code>:               */
1691                 0xe0d050b2,             /* ldrh r5, [r0], #2                    */
1692                 0xe1c890b0,             /* strh r9, [r8]                                */
1693                 0xe1cab0b0,             /* strh r11, [r10]                              */
1694                 0xe1c830b0,             /* strh r3, [r8]                                */
1695                 0xe1c150b0,             /* strh r5, [r1]                                */
1696                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1697                 /*
1698                  * 00008168 <sp_16_busy>:               */
1699                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1700                 0xe0257006,             /* eor  r7, r5, r6                              */
1701                 0xe0147007,             /* ands r7, r4, r7                              */
1702                 0x0a000007,             /* beq  8198 <sp_16_cont>               */
1703                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1704                 0x0afffff9,             /* beq  8168 <sp_16_busy>               */
1705                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1706                 0xe0257006,             /* eor  r7, r5, r6                              */
1707                 0xe0147007,             /* ands r7, r4, r7                              */
1708                 0x0a000001,             /* beq  8198 <sp_16_cont>               */
1709                 0xe3a05000,             /* mov  r5, #0  ; 0x0                   */
1710                 0x1a000004,             /* bne  81ac <sp_16_done>               */
1711                 /*
1712                  * 00008198 <sp_16_cont>:               */
1713                 0xe2522001,     /* subs r2, r2, #1      ; 0x1           */
1714                 0x03a05080,     /* moveq        r5, #128        ; 0x80  */
1715                 0x0a000001,     /* beq  81ac <sp_16_done>               */
1716                 0xe2811002,     /* add  r1, r1, #2      ; 0x2           */
1717                 0xeaffffe8,     /* b    8158 <sp_16_code>               */
1718                 /*
1719                  * 000081ac <sp_16_done>:               */
1720                 0xeafffffe              /* b    81ac <sp_16_done>               */
1721         };
1722
1723         /* see contib/loaders/flash/armv7m_cfi_span_16.s for src */
1724         static const uint32_t armv7m_word_16_code[] = {
1725                 0x5B02F830,
1726                 0x9000F8A8,
1727                 0xB000F8AA,
1728                 0x3000F8A8,
1729                 0xBF00800D,
1730                 0xEA85880E,
1731                 0x40270706,
1732                 0xEA16D00A,
1733                 0xD0F70694,
1734                 0xEA85880E,
1735                 0x40270706,
1736                 0xF04FD002,
1737                 0xD1070500,
1738                 0xD0023A01,
1739                 0x0102F101,
1740                 0xF04FE7E0,
1741                 0xE7FF0580,
1742                 0x0000BE00
1743         };
1744
1745         /* see contib/loaders/flash/armv4_5_cfi_span_16_dq7.s for src */
1746         static const uint32_t armv4_5_word_16_code_dq7only[] = {
1747                 /* <sp_16_code>:                                */
1748                 0xe0d050b2,             /* ldrh r5, [r0], #2                    */
1749                 0xe1c890b0,             /* strh r9, [r8]                                */
1750                 0xe1cab0b0,             /* strh r11, [r10]                              */
1751                 0xe1c830b0,             /* strh r3, [r8]                                */
1752                 0xe1c150b0,             /* strh r5, [r1]                                */
1753                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1754                 /*
1755                  * <sp_16_busy>:                                */
1756                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1757                 0xe0257006,             /* eor  r7, r5, r6                              */
1758                 0xe2177080,             /* ands r7, #0x80                               */
1759                 0x1afffffb,             /* bne  8168 <sp_16_busy>               */
1760                 /*                                                              */
1761                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1762                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1763                 0x0a000001,             /* beq  81ac <sp_16_done>               */
1764                 0xe2811002,             /* add  r1, r1, #2      ; 0x2           */
1765                 0xeafffff0,             /* b    8158 <sp_16_code>               */
1766                 /*
1767                  * 000081ac <sp_16_done>:               */
1768                 0xeafffffe              /* b    81ac <sp_16_done>               */
1769         };
1770
1771         /* see contib/loaders/flash/armv4_5_cfi_span_8.s for src */
1772         static const uint32_t armv4_5_word_8_code[] = {
1773                 /* 000081b0 <sp_16_code_end>:   */
1774                 0xe4d05001,             /* ldrb r5, [r0], #1                    */
1775                 0xe5c89000,             /* strb r9, [r8]                                */
1776                 0xe5cab000,             /* strb r11, [r10]                              */
1777                 0xe5c83000,             /* strb r3, [r8]                                */
1778                 0xe5c15000,             /* strb r5, [r1]                                */
1779                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1780                 /*
1781                  * 000081c0 <sp_8_busy>:                */
1782                 0xe5d16000,             /* ldrb r6, [r1]                                */
1783                 0xe0257006,             /* eor  r7, r5, r6                              */
1784                 0xe0147007,             /* ands r7, r4, r7                              */
1785                 0x0a000007,             /* beq  81f0 <sp_8_cont>                */
1786                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1787                 0x0afffff9,             /* beq  81c0 <sp_8_busy>                */
1788                 0xe5d16000,             /* ldrb r6, [r1]                                */
1789                 0xe0257006,             /* eor  r7, r5, r6                              */
1790                 0xe0147007,             /* ands r7, r4, r7                              */
1791                 0x0a000001,             /* beq  81f0 <sp_8_cont>                */
1792                 0xe3a05000,             /* mov  r5, #0  ; 0x0                   */
1793                 0x1a000004,             /* bne  8204 <sp_8_done>                */
1794                 /*
1795                  * 000081f0 <sp_8_cont>:                */
1796                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1797                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1798                 0x0a000001,             /* beq  8204 <sp_8_done>                */
1799                 0xe2811001,             /* add  r1, r1, #1      ; 0x1           */
1800                 0xeaffffe8,             /* b    81b0 <sp_16_code_end>   */
1801                 /*
1802                  * 00008204 <sp_8_done>:                */
1803                 0xeafffffe              /* b    8204 <sp_8_done>                */
1804         };
1805
1806         if (strncmp(target_type_name(target), "mips_m4k", 8) == 0)
1807                 return cfi_spansion_write_block_mips(bank, buffer, address, count);
1808
1809         if (is_armv7m(target_to_armv7m(target))) {      /* Cortex-M3 target */
1810                 armv7m_algo.common_magic = ARMV7M_COMMON_MAGIC;
1811                 armv7m_algo.core_mode = ARMV7M_MODE_HANDLER;
1812                 arm_algo = &armv7m_algo;
1813         } else if (is_arm(target_to_arm(target))) {
1814                 /* All other ARM CPUs have 32 bit instructions */
1815                 armv4_5_algo.common_magic = ARM_COMMON_MAGIC;
1816                 armv4_5_algo.core_mode = ARM_MODE_SVC;
1817                 armv4_5_algo.core_state = ARM_STATE_ARM;
1818                 arm_algo = &armv4_5_algo;
1819         } else {
1820                 LOG_ERROR("Unknown architecture");
1821                 return ERROR_FAIL;
1822         }
1823
1824         int target_code_size = 0;
1825         const uint32_t *target_code_src = NULL;
1826
1827         switch (bank->bus_width) {
1828                 case 1:
1829                         if (is_armv7m(target_to_armv7m(target))) {
1830                                 LOG_ERROR("Unknown ARM architecture");
1831                                 return ERROR_FAIL;
1832                         }
1833                         target_code_src = armv4_5_word_8_code;
1834                         target_code_size = sizeof(armv4_5_word_8_code);
1835                         break;
1836                 case 2:
1837                         /* Check for DQ5 support */
1838                         if (cfi_info->status_poll_mask & (1 << 5)) {
1839                                 if (is_armv7m(target_to_armv7m(target))) {      /*
1840                                                                                                  *cortex-m3
1841                                                                                                  *target
1842                                                                                                  **/
1843                                         target_code_src = armv7m_word_16_code;
1844                                         target_code_size = sizeof(armv7m_word_16_code);
1845                                 } else { /* armv4_5 target */
1846                                         target_code_src = armv4_5_word_16_code;
1847                                         target_code_size = sizeof(armv4_5_word_16_code);
1848                                 }
1849                         } else {
1850                                 /* No DQ5 support. Use DQ7 DATA# polling only. */
1851                                 if (is_armv7m(target_to_armv7m(target))) {
1852                                         LOG_ERROR("Unknown ARM architecture");
1853                                         return ERROR_FAIL;
1854                                 }
1855                                 target_code_src = armv4_5_word_16_code_dq7only;
1856                                 target_code_size = sizeof(armv4_5_word_16_code_dq7only);
1857                         }
1858                         break;
1859                 case 4:
1860                         if (is_armv7m(target_to_armv7m(target))) {
1861                                 LOG_ERROR("Unknown ARM architecture");
1862                                 return ERROR_FAIL;
1863                         }
1864                         target_code_src = armv4_5_word_32_code;
1865                         target_code_size = sizeof(armv4_5_word_32_code);
1866                         break;
1867                 default:
1868                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1869                                         bank->bus_width);
1870                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1871         }
1872
1873         /* flash write code */
1874         uint8_t *target_code;
1875
1876         /* convert bus-width dependent algorithm code to correct endiannes */
1877         target_code = malloc(target_code_size);
1878         if (target_code == NULL) {
1879                 LOG_ERROR("Out of memory");
1880                 return ERROR_FAIL;
1881         }
1882         cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
1883
1884         /* allocate working area */
1885         retval = target_alloc_working_area(target, target_code_size,
1886                         &write_algorithm);
1887         if (retval != ERROR_OK) {
1888                 free(target_code);
1889                 return retval;
1890         }
1891
1892         /* write algorithm code to working area */
1893         retval = target_write_buffer(target, write_algorithm->address,
1894                         target_code_size, target_code);
1895         if (retval != ERROR_OK) {
1896                 free(target_code);
1897                 return retval;
1898         }
1899
1900         free(target_code);
1901
1902         /* the following code still assumes target code is fixed 24*4 bytes */
1903
1904         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
1905                 buffer_size /= 2;
1906                 if (buffer_size <= 256) {
1907                         /* we already allocated the writing code, but failed to get a
1908                          * buffer, free the algorithm */
1909                         target_free_working_area(target, write_algorithm);
1910
1911                         LOG_WARNING(
1912                                 "not enough working area available, can't do block memory writes");
1913                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1914                 }
1915         }
1916         ;
1917
1918         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
1919         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
1920         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
1921         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
1922         init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);
1923         init_reg_param(&reg_params[5], "r5", 32, PARAM_IN);
1924         init_reg_param(&reg_params[6], "r8", 32, PARAM_OUT);
1925         init_reg_param(&reg_params[7], "r9", 32, PARAM_OUT);
1926         init_reg_param(&reg_params[8], "r10", 32, PARAM_OUT);
1927         init_reg_param(&reg_params[9], "r11", 32, PARAM_OUT);
1928
1929         while (count > 0) {
1930                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
1931
1932                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
1933                 if (retval != ERROR_OK)
1934                         break;
1935
1936                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
1937                 buf_set_u32(reg_params[1].value, 0, 32, address);
1938                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
1939                 buf_set_u32(reg_params[3].value, 0, 32, cfi_command_val(bank, 0xA0));
1940                 buf_set_u32(reg_params[4].value, 0, 32, cfi_command_val(bank, 0x80));
1941                 buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
1942                 buf_set_u32(reg_params[7].value, 0, 32, 0xaaaaaaaa);
1943                 buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
1944                 buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
1945
1946                 retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
1947                                 write_algorithm->address,
1948                                 write_algorithm->address + ((target_code_size) - 4),
1949                                 10000, arm_algo);
1950                 if (retval != ERROR_OK)
1951                         break;
1952
1953                 status = buf_get_u32(reg_params[5].value, 0, 32);
1954                 if (status != 0x80) {
1955                         LOG_ERROR("flash write block failed status: 0x%" PRIx32, status);
1956                         retval = ERROR_FLASH_OPERATION_FAILED;
1957                         break;
1958                 }
1959
1960                 buffer += thisrun_count;
1961                 address += thisrun_count;
1962                 count -= thisrun_count;
1963         }
1964
1965         target_free_all_working_areas(target);
1966
1967         destroy_reg_param(&reg_params[0]);
1968         destroy_reg_param(&reg_params[1]);
1969         destroy_reg_param(&reg_params[2]);
1970         destroy_reg_param(&reg_params[3]);
1971         destroy_reg_param(&reg_params[4]);
1972         destroy_reg_param(&reg_params[5]);
1973         destroy_reg_param(&reg_params[6]);
1974         destroy_reg_param(&reg_params[7]);
1975         destroy_reg_param(&reg_params[8]);
1976         destroy_reg_param(&reg_params[9]);
1977
1978         return retval;
1979 }
1980
1981 static int cfi_intel_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
1982 {
1983         int retval;
1984         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1985         struct target *target = bank->target;
1986
1987         cfi_intel_clear_status_register(bank);
1988         retval = cfi_send_command(bank, 0x40, address);
1989         if (retval != ERROR_OK)
1990                 return retval;
1991
1992         retval = target_write_memory(target, address, bank->bus_width, 1, word);
1993         if (retval != ERROR_OK)
1994                 return retval;
1995
1996         uint8_t status;
1997         retval = cfi_intel_wait_status_busy(bank, cfi_info->word_write_timeout, &status);
1998         if (retval != 0x80) {
1999                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2000                 if (retval != ERROR_OK)
2001                         return retval;
2002
2003                 LOG_ERROR("couldn't write word at base 0x%" PRIx32 ", address 0x%" PRIx32,
2004                         bank->base, address);
2005                 return ERROR_FLASH_OPERATION_FAILED;
2006         }
2007
2008         return ERROR_OK;
2009 }
2010
2011 static int cfi_intel_write_words(struct flash_bank *bank, uint8_t *word,
2012         uint32_t wordcount, uint32_t address)
2013 {
2014         int retval;
2015         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2016         struct target *target = bank->target;
2017
2018         /* Calculate buffer size and boundary mask
2019          * buffersize is (buffer size per chip) * (number of chips)
2020          * bufferwsize is buffersize in words */
2021         uint32_t buffersize =
2022                 (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2023         uint32_t buffermask = buffersize-1;
2024         uint32_t bufferwsize = buffersize / bank->bus_width;
2025
2026         /* Check for valid range */
2027         if (address & buffermask) {
2028                 LOG_ERROR("Write address at base 0x%" PRIx32 ", address 0x%" PRIx32
2029                         " not aligned to 2^%d boundary",
2030                         bank->base, address, cfi_info->max_buf_write_size);
2031                 return ERROR_FLASH_OPERATION_FAILED;
2032         }
2033
2034         /* Check for valid size */
2035         if (wordcount > bufferwsize) {
2036                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %" PRId32,
2037                         wordcount, buffersize);
2038                 return ERROR_FLASH_OPERATION_FAILED;
2039         }
2040
2041         /* Write to flash buffer */
2042         cfi_intel_clear_status_register(bank);
2043
2044         /* Initiate buffer operation _*/
2045         retval = cfi_send_command(bank, 0xe8, address);
2046         if (retval != ERROR_OK)
2047                 return retval;
2048         uint8_t status;
2049         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
2050         if (retval != ERROR_OK)
2051                 return retval;
2052         if (status != 0x80) {
2053                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2054                 if (retval != ERROR_OK)
2055                         return retval;
2056
2057                 LOG_ERROR(
2058                         "couldn't start buffer write operation at base 0x%" PRIx32 ", address 0x%" PRIx32,
2059                         bank->base,
2060                         address);
2061                 return ERROR_FLASH_OPERATION_FAILED;
2062         }
2063
2064         /* Write buffer wordcount-1 and data words */
2065         retval = cfi_send_command(bank, bufferwsize-1, address);
2066         if (retval != ERROR_OK)
2067                 return retval;
2068
2069         retval = target_write_memory(target, address, bank->bus_width, bufferwsize, word);
2070         if (retval != ERROR_OK)
2071                 return retval;
2072
2073         /* Commit write operation */
2074         retval = cfi_send_command(bank, 0xd0, address);
2075         if (retval != ERROR_OK)
2076                 return retval;
2077
2078         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
2079         if (retval != ERROR_OK)
2080                 return retval;
2081
2082         if (status != 0x80) {
2083                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2084                 if (retval != ERROR_OK)
2085                         return retval;
2086
2087                 LOG_ERROR("Buffer write at base 0x%" PRIx32
2088                         ", address 0x%" PRIx32 " failed.", bank->base, address);
2089                 return ERROR_FLASH_OPERATION_FAILED;
2090         }
2091
2092         return ERROR_OK;
2093 }
2094
2095 static int cfi_spansion_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2096 {
2097         int retval;
2098         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2099         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2100         struct target *target = bank->target;
2101
2102         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
2103         if (retval != ERROR_OK)
2104                 return retval;
2105
2106         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
2107         if (retval != ERROR_OK)
2108                 return retval;
2109
2110         retval = cfi_send_command(bank, 0xa0, flash_address(bank, 0, pri_ext->_unlock1));
2111         if (retval != ERROR_OK)
2112                 return retval;
2113
2114         retval = target_write_memory(target, address, bank->bus_width, 1, word);
2115         if (retval != ERROR_OK)
2116                 return retval;
2117
2118         if (cfi_spansion_wait_status_busy(bank, cfi_info->word_write_timeout) != ERROR_OK) {
2119                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2120                 if (retval != ERROR_OK)
2121                         return retval;
2122
2123                 LOG_ERROR("couldn't write word at base 0x%" PRIx32
2124                         ", address 0x%" PRIx32, bank->base, address);
2125                 return ERROR_FLASH_OPERATION_FAILED;
2126         }
2127
2128         return ERROR_OK;
2129 }
2130
2131 static int cfi_spansion_write_words(struct flash_bank *bank, uint8_t *word,
2132         uint32_t wordcount, uint32_t address)
2133 {
2134         int retval;
2135         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2136         struct target *target = bank->target;
2137         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2138
2139         /* Calculate buffer size and boundary mask
2140          * buffersize is (buffer size per chip) * (number of chips)
2141          * bufferwsize is buffersize in words */
2142         uint32_t buffersize =
2143                 (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2144         uint32_t buffermask = buffersize-1;
2145         uint32_t bufferwsize = buffersize / bank->bus_width;
2146
2147         /* Check for valid range */
2148         if (address & buffermask) {
2149                 LOG_ERROR("Write address at base 0x%" PRIx32
2150                         ", address 0x%" PRIx32 " not aligned to 2^%d boundary",
2151                         bank->base, address, cfi_info->max_buf_write_size);
2152                 return ERROR_FLASH_OPERATION_FAILED;
2153         }
2154
2155         /* Check for valid size */
2156         if (wordcount > bufferwsize) {
2157                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %"
2158                         PRId32, wordcount, buffersize);
2159                 return ERROR_FLASH_OPERATION_FAILED;
2160         }
2161
2162         /* Unlock */
2163         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
2164         if (retval != ERROR_OK)
2165                 return retval;
2166
2167         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
2168         if (retval != ERROR_OK)
2169                 return retval;
2170
2171         /* Buffer load command */
2172         retval = cfi_send_command(bank, 0x25, address);
2173         if (retval != ERROR_OK)
2174                 return retval;
2175
2176         /* Write buffer wordcount-1 and data words */
2177         retval = cfi_send_command(bank, bufferwsize-1, address);
2178         if (retval != ERROR_OK)
2179                 return retval;
2180
2181         retval = target_write_memory(target, address, bank->bus_width, bufferwsize, word);
2182         if (retval != ERROR_OK)
2183                 return retval;
2184
2185         /* Commit write operation */
2186         retval = cfi_send_command(bank, 0x29, address);
2187         if (retval != ERROR_OK)
2188                 return retval;
2189
2190         if (cfi_spansion_wait_status_busy(bank, cfi_info->buf_write_timeout) != ERROR_OK) {
2191                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2192                 if (retval != ERROR_OK)
2193                         return retval;
2194
2195                 LOG_ERROR("couldn't write block at base 0x%" PRIx32
2196                         ", address 0x%" PRIx32 ", size 0x%" PRIx32, bank->base, address,
2197                         bufferwsize);
2198                 return ERROR_FLASH_OPERATION_FAILED;
2199         }
2200
2201         return ERROR_OK;
2202 }
2203
2204 static int cfi_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2205 {
2206         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2207
2208         switch (cfi_info->pri_id) {
2209                 case 1:
2210                 case 3:
2211                         return cfi_intel_write_word(bank, word, address);
2212                         break;
2213                 case 2:
2214                         return cfi_spansion_write_word(bank, word, address);
2215                         break;
2216                 default:
2217                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2218                         break;
2219         }
2220
2221         return ERROR_FLASH_OPERATION_FAILED;
2222 }
2223
2224 static int cfi_write_words(struct flash_bank *bank, uint8_t *word,
2225         uint32_t wordcount, uint32_t address)
2226 {
2227         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2228
2229         if (cfi_info->buf_write_timeout_typ == 0) {
2230                 /* buffer writes are not supported */
2231                 LOG_DEBUG("Buffer Writes Not Supported");
2232                 return ERROR_FLASH_OPER_UNSUPPORTED;
2233         }
2234
2235         switch (cfi_info->pri_id) {
2236                 case 1:
2237                 case 3:
2238                         return cfi_intel_write_words(bank, word, wordcount, address);
2239                         break;
2240                 case 2:
2241                         return cfi_spansion_write_words(bank, word, wordcount, address);
2242                         break;
2243                 default:
2244                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2245                         break;
2246         }
2247
2248         return ERROR_FLASH_OPERATION_FAILED;
2249 }
2250
2251 static int cfi_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
2252 {
2253         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2254         struct target *target = bank->target;
2255         uint32_t address = bank->base + offset;
2256         uint32_t read_p;
2257         int align;      /* number of unaligned bytes */
2258         uint8_t current_word[CFI_MAX_BUS_WIDTH];
2259         int i;
2260         int retval;
2261
2262         LOG_DEBUG("reading buffer of %i byte at 0x%8.8x",
2263                 (int)count, (unsigned)offset);
2264
2265         if (bank->target->state != TARGET_HALTED) {
2266                 LOG_ERROR("Target not halted");
2267                 return ERROR_TARGET_NOT_HALTED;
2268         }
2269
2270         if (offset + count > bank->size)
2271                 return ERROR_FLASH_DST_OUT_OF_BANK;
2272
2273         if (cfi_info->qry[0] != 'Q')
2274                 return ERROR_FLASH_BANK_NOT_PROBED;
2275
2276         /* start at the first byte of the first word (bus_width size) */
2277         read_p = address & ~(bank->bus_width - 1);
2278         align = address - read_p;
2279         if (align != 0) {
2280                 LOG_INFO("Fixup %d unaligned read head bytes", align);
2281
2282                 /* read a complete word from flash */
2283                 retval = target_read_memory(target, read_p, bank->bus_width, 1, current_word);
2284                 if (retval != ERROR_OK)
2285                         return retval;
2286
2287                 /* take only bytes we need */
2288                 for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
2289                         *buffer++ = current_word[i];
2290
2291                 read_p += bank->bus_width;
2292         }
2293
2294         align = count / bank->bus_width;
2295         if (align) {
2296                 retval = target_read_memory(target, read_p, bank->bus_width, align, buffer);
2297                 if (retval != ERROR_OK)
2298                         return retval;
2299
2300                 read_p += align * bank->bus_width;
2301                 buffer += align * bank->bus_width;
2302                 count -= align * bank->bus_width;
2303         }
2304
2305         if (count) {
2306                 LOG_INFO("Fixup %d unaligned read tail bytes", count);
2307
2308                 /* read a complete word from flash */
2309                 retval = target_read_memory(target, read_p, bank->bus_width, 1, current_word);
2310                 if (retval != ERROR_OK)
2311                         return retval;
2312
2313                 /* take only bytes we need */
2314                 for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
2315                         *buffer++ = current_word[i];
2316         }
2317
2318         return ERROR_OK;
2319 }
2320
2321 static int cfi_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
2322 {
2323         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2324         struct target *target = bank->target;
2325         uint32_t address = bank->base + offset; /* address of first byte to be programmed */
2326         uint32_t write_p;
2327         int align;      /* number of unaligned bytes */
2328         int blk_count;  /* number of bus_width bytes for block copy */
2329         uint8_t current_word[CFI_MAX_BUS_WIDTH * 4];    /* word (bus_width size) currently being
2330                                                          *programmed */
2331         int i;
2332         int retval;
2333
2334         if (bank->target->state != TARGET_HALTED) {
2335                 LOG_ERROR("Target not halted");
2336                 return ERROR_TARGET_NOT_HALTED;
2337         }
2338
2339         if (offset + count > bank->size)
2340                 return ERROR_FLASH_DST_OUT_OF_BANK;
2341
2342         if (cfi_info->qry[0] != 'Q')
2343                 return ERROR_FLASH_BANK_NOT_PROBED;
2344
2345         /* start at the first byte of the first word (bus_width size) */
2346         write_p = address & ~(bank->bus_width - 1);
2347         align = address - write_p;
2348         if (align != 0) {
2349                 LOG_INFO("Fixup %d unaligned head bytes", align);
2350
2351                 /* read a complete word from flash */
2352                 retval = target_read_memory(target, write_p, bank->bus_width, 1, current_word);
2353                 if (retval != ERROR_OK)
2354                         return retval;
2355
2356                 /* replace only bytes that must be written */
2357                 for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
2358                         current_word[i] = *buffer++;
2359
2360                 retval = cfi_write_word(bank, current_word, write_p);
2361                 if (retval != ERROR_OK)
2362                         return retval;
2363                 write_p += bank->bus_width;
2364         }
2365
2366         /* handle blocks of bus_size aligned bytes */
2367         blk_count = count & ~(bank->bus_width - 1);     /* round down, leave tail bytes */
2368         switch (cfi_info->pri_id) {
2369                 /* try block writes (fails without working area) */
2370                 case 1:
2371                 case 3:
2372                         retval = cfi_intel_write_block(bank, buffer, write_p, blk_count);
2373                         break;
2374                 case 2:
2375                         retval = cfi_spansion_write_block(bank, buffer, write_p, blk_count);
2376                         break;
2377                 default:
2378                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2379                         retval = ERROR_FLASH_OPERATION_FAILED;
2380                         break;
2381         }
2382         if (retval == ERROR_OK) {
2383                 /* Increment pointers and decrease count on succesful block write */
2384                 buffer += blk_count;
2385                 write_p += blk_count;
2386                 count -= blk_count;
2387         } else {
2388                 if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
2389                         /* Calculate buffer size and boundary mask
2390                          * buffersize is (buffer size per chip) * (number of chips)
2391                          * bufferwsize is buffersize in words */
2392                         uint32_t buffersize =
2393                                 (1UL <<
2394                                  cfi_info->max_buf_write_size) *
2395                                 (bank->bus_width / bank->chip_width);
2396                         uint32_t buffermask = buffersize-1;
2397                         uint32_t bufferwsize = buffersize / bank->bus_width;
2398
2399                         /* fall back to memory writes */
2400                         while (count >= (uint32_t)bank->bus_width) {
2401                                 int fallback;
2402                                 if ((write_p & 0xff) == 0) {
2403                                         LOG_INFO("Programming at 0x%08" PRIx32 ", count 0x%08"
2404                                                 PRIx32 " bytes remaining", write_p, count);
2405                                 }
2406                                 fallback = 1;
2407                                 if ((bufferwsize > 0) && (count >= buffersize) &&
2408                                                 !(write_p & buffermask)) {
2409                                         retval = cfi_write_words(bank, buffer, bufferwsize, write_p);
2410                                         if (retval == ERROR_OK) {
2411                                                 buffer += buffersize;
2412                                                 write_p += buffersize;
2413                                                 count -= buffersize;
2414                                                 fallback = 0;
2415                                         } else if (retval != ERROR_FLASH_OPER_UNSUPPORTED)
2416                                                 return retval;
2417                                 }
2418                                 /* try the slow way? */
2419                                 if (fallback) {
2420                                         for (i = 0; i < bank->bus_width; i++)
2421                                                 current_word[i] = *buffer++;
2422
2423                                         retval = cfi_write_word(bank, current_word, write_p);
2424                                         if (retval != ERROR_OK)
2425                                                 return retval;
2426
2427                                         write_p += bank->bus_width;
2428                                         count -= bank->bus_width;
2429                                 }
2430                         }
2431                 } else
2432                         return retval;
2433         }
2434
2435         /* return to read array mode, so we can read from flash again for padding */
2436         retval = cfi_reset(bank);
2437         if (retval != ERROR_OK)
2438                 return retval;
2439
2440         /* handle unaligned tail bytes */
2441         if (count > 0) {
2442                 LOG_INFO("Fixup %" PRId32 " unaligned tail bytes", count);
2443
2444                 /* read a complete word from flash */
2445                 retval = target_read_memory(target, write_p, bank->bus_width, 1, current_word);
2446                 if (retval != ERROR_OK)
2447                         return retval;
2448
2449                 /* replace only bytes that must be written */
2450                 for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
2451                         current_word[i] = *buffer++;
2452
2453                 retval = cfi_write_word(bank, current_word, write_p);
2454                 if (retval != ERROR_OK)
2455                         return retval;
2456         }
2457
2458         /* return to read array mode */
2459         return cfi_reset(bank);
2460 }
2461
2462 static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, void *param)
2463 {
2464         (void) param;
2465         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2466         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2467
2468         pri_ext->_reversed_geometry = 1;
2469 }
2470
2471 static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, void *param)
2472 {
2473         int i;
2474         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2475         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2476         (void) param;
2477
2478         if ((pri_ext->_reversed_geometry) || (pri_ext->TopBottom == 3)) {
2479                 LOG_DEBUG("swapping reversed erase region information on cmdset 0002 device");
2480
2481                 for (i = 0; i < cfi_info->num_erase_regions / 2; i++) {
2482                         int j = (cfi_info->num_erase_regions - 1) - i;
2483                         uint32_t swap;
2484
2485                         swap = cfi_info->erase_region_info[i];
2486                         cfi_info->erase_region_info[i] = cfi_info->erase_region_info[j];
2487                         cfi_info->erase_region_info[j] = swap;
2488                 }
2489         }
2490 }
2491
2492 static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, void *param)
2493 {
2494         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2495         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2496         struct cfi_unlock_addresses *unlock_addresses = param;
2497
2498         pri_ext->_unlock1 = unlock_addresses->unlock1;
2499         pri_ext->_unlock2 = unlock_addresses->unlock2;
2500 }
2501
2502
2503 static int cfi_query_string(struct flash_bank *bank, int address)
2504 {
2505         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2506         int retval;
2507
2508         retval = cfi_send_command(bank, 0x98, flash_address(bank, 0, address));
2509         if (retval != ERROR_OK)
2510                 return retval;
2511
2512         retval = cfi_query_u8(bank, 0, 0x10, &cfi_info->qry[0]);
2513         if (retval != ERROR_OK)
2514                 return retval;
2515         retval = cfi_query_u8(bank, 0, 0x11, &cfi_info->qry[1]);
2516         if (retval != ERROR_OK)
2517                 return retval;
2518         retval = cfi_query_u8(bank, 0, 0x12, &cfi_info->qry[2]);
2519         if (retval != ERROR_OK)
2520                 return retval;
2521
2522         LOG_DEBUG("CFI qry returned: 0x%2.2x 0x%2.2x 0x%2.2x",
2523                 cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2]);
2524
2525         if ((cfi_info->qry[0] != 'Q') || (cfi_info->qry[1] != 'R') || (cfi_info->qry[2] != 'Y')) {
2526                 retval = cfi_reset(bank);
2527                 if (retval != ERROR_OK)
2528                         return retval;
2529                 LOG_ERROR("Could not probe bank: no QRY");
2530                 return ERROR_FLASH_BANK_INVALID;
2531         }
2532
2533         return ERROR_OK;
2534 }
2535
2536 static int cfi_probe(struct flash_bank *bank)
2537 {
2538         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2539         struct target *target = bank->target;
2540         int num_sectors = 0;
2541         int i;
2542         int sector = 0;
2543         uint32_t unlock1 = 0x555;
2544         uint32_t unlock2 = 0x2aa;
2545         int retval;
2546         uint8_t value_buf0[CFI_MAX_BUS_WIDTH], value_buf1[CFI_MAX_BUS_WIDTH];
2547
2548         if (bank->target->state != TARGET_HALTED) {
2549                 LOG_ERROR("Target not halted");
2550                 return ERROR_TARGET_NOT_HALTED;
2551         }
2552
2553         cfi_info->probed = 0;
2554         cfi_info->num_erase_regions = 0;
2555         if (bank->sectors) {
2556                 free(bank->sectors);
2557                 bank->sectors = NULL;
2558         }
2559         if (cfi_info->erase_region_info) {
2560                 free(cfi_info->erase_region_info);
2561                 cfi_info->erase_region_info = NULL;
2562         }
2563
2564         /* JEDEC standard JESD21C uses 0x5555 and 0x2aaa as unlock addresses,
2565          * while CFI compatible AMD/Spansion flashes use 0x555 and 0x2aa
2566          */
2567         if (cfi_info->jedec_probe) {
2568                 unlock1 = 0x5555;
2569                 unlock2 = 0x2aaa;
2570         }
2571
2572         /* switch to read identifier codes mode ("AUTOSELECT") */
2573         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, unlock1));
2574         if (retval != ERROR_OK)
2575                 return retval;
2576         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, unlock2));
2577         if (retval != ERROR_OK)
2578                 return retval;
2579         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, unlock1));
2580         if (retval != ERROR_OK)
2581                 return retval;
2582
2583         retval = target_read_memory(target, flash_address(bank, 0, 0x00),
2584                         bank->bus_width, 1, value_buf0);
2585         if (retval != ERROR_OK)
2586                 return retval;
2587         retval = target_read_memory(target, flash_address(bank, 0, 0x01),
2588                         bank->bus_width, 1, value_buf1);
2589         if (retval != ERROR_OK)
2590                 return retval;
2591         switch (bank->chip_width) {
2592                 case 1:
2593                         cfi_info->manufacturer = *value_buf0;
2594                         cfi_info->device_id = *value_buf1;
2595                         break;
2596                 case 2:
2597                         cfi_info->manufacturer = target_buffer_get_u16(target, value_buf0);
2598                         cfi_info->device_id = target_buffer_get_u16(target, value_buf1);
2599                         break;
2600                 case 4:
2601                         cfi_info->manufacturer = target_buffer_get_u32(target, value_buf0);
2602                         cfi_info->device_id = target_buffer_get_u32(target, value_buf1);
2603                         break;
2604                 default:
2605                         LOG_ERROR("Unsupported bank chipwidth %d, can't probe memory",
2606                                         bank->chip_width);
2607                         return ERROR_FLASH_OPERATION_FAILED;
2608         }
2609
2610         LOG_INFO("Flash Manufacturer/Device: 0x%04x 0x%04x",
2611                 cfi_info->manufacturer, cfi_info->device_id);
2612         /* switch back to read array mode */
2613         retval = cfi_reset(bank);
2614         if (retval != ERROR_OK)
2615                 return retval;
2616
2617         /* check device/manufacturer ID for known non-CFI flashes. */
2618         cfi_fixup_non_cfi(bank);
2619
2620         /* query only if this is a CFI compatible flash,
2621          * otherwise the relevant info has already been filled in
2622          */
2623         if (cfi_info->not_cfi == 0) {
2624                 /* enter CFI query mode
2625                  * according to JEDEC Standard No. 68.01,
2626                  * a single bus sequence with address = 0x55, data = 0x98 should put
2627                  * the device into CFI query mode.
2628                  *
2629                  * SST flashes clearly violate this, and we will consider them incompatbile for now
2630                  */
2631
2632                 retval = cfi_query_string(bank, 0x55);
2633                 if (retval != ERROR_OK) {
2634                         /*
2635                          * Spansion S29WS-N CFI query fix is to try 0x555 if 0x55 fails. Should
2636                          * be harmless enough:
2637                          *
2638                          * http://www.infradead.org/pipermail/linux-mtd/2005-September/013618.html
2639                          */
2640                         LOG_USER("Try workaround w/0x555 instead of 0x55 to get QRY.");
2641                         retval = cfi_query_string(bank, 0x555);
2642                 }
2643                 if (retval != ERROR_OK)
2644                         return retval;
2645
2646                 retval = cfi_query_u16(bank, 0, 0x13, &cfi_info->pri_id);
2647                 if (retval != ERROR_OK)
2648                         return retval;
2649                 retval = cfi_query_u16(bank, 0, 0x15, &cfi_info->pri_addr);
2650                 if (retval != ERROR_OK)
2651                         return retval;
2652                 retval = cfi_query_u16(bank, 0, 0x17, &cfi_info->alt_id);
2653                 if (retval != ERROR_OK)
2654                         return retval;
2655                 retval = cfi_query_u16(bank, 0, 0x19, &cfi_info->alt_addr);
2656                 if (retval != ERROR_OK)
2657                         return retval;
2658
2659                 LOG_DEBUG("qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: 0x%4.4x, alt_id: "
2660                         "0x%4.4x, alt_addr: 0x%4.4x", cfi_info->qry[0], cfi_info->qry[1],
2661                         cfi_info->qry[2], cfi_info->pri_id, cfi_info->pri_addr,
2662                         cfi_info->alt_id, cfi_info->alt_addr);
2663
2664                 retval = cfi_query_u8(bank, 0, 0x1b, &cfi_info->vcc_min);
2665                 if (retval != ERROR_OK)
2666                         return retval;
2667                 retval = cfi_query_u8(bank, 0, 0x1c, &cfi_info->vcc_max);
2668                 if (retval != ERROR_OK)
2669                         return retval;
2670                 retval = cfi_query_u8(bank, 0, 0x1d, &cfi_info->vpp_min);
2671                 if (retval != ERROR_OK)
2672                         return retval;
2673                 retval = cfi_query_u8(bank, 0, 0x1e, &cfi_info->vpp_max);
2674                 if (retval != ERROR_OK)
2675                         return retval;
2676
2677                 retval = cfi_query_u8(bank, 0, 0x1f, &cfi_info->word_write_timeout_typ);
2678                 if (retval != ERROR_OK)
2679                         return retval;
2680                 retval = cfi_query_u8(bank, 0, 0x20, &cfi_info->buf_write_timeout_typ);
2681                 if (retval != ERROR_OK)
2682                         return retval;
2683                 retval = cfi_query_u8(bank, 0, 0x21, &cfi_info->block_erase_timeout_typ);
2684                 if (retval != ERROR_OK)
2685                         return retval;
2686                 retval = cfi_query_u8(bank, 0, 0x22, &cfi_info->chip_erase_timeout_typ);
2687                 if (retval != ERROR_OK)
2688                         return retval;
2689                 retval = cfi_query_u8(bank, 0, 0x23, &cfi_info->word_write_timeout_max);
2690                 if (retval != ERROR_OK)
2691                         return retval;
2692                 retval = cfi_query_u8(bank, 0, 0x24, &cfi_info->buf_write_timeout_max);
2693                 if (retval != ERROR_OK)
2694                         return retval;
2695                 retval = cfi_query_u8(bank, 0, 0x25, &cfi_info->block_erase_timeout_max);
2696                 if (retval != ERROR_OK)
2697                         return retval;
2698                 retval = cfi_query_u8(bank, 0, 0x26, &cfi_info->chip_erase_timeout_max);
2699                 if (retval != ERROR_OK)
2700                         return retval;
2701
2702                 uint8_t data;
2703                 retval = cfi_query_u8(bank, 0, 0x27, &data);
2704                 if (retval != ERROR_OK)
2705                         return retval;
2706                 cfi_info->dev_size = 1 << data;
2707
2708                 retval = cfi_query_u16(bank, 0, 0x28, &cfi_info->interface_desc);
2709                 if (retval != ERROR_OK)
2710                         return retval;
2711                 retval = cfi_query_u16(bank, 0, 0x2a, &cfi_info->max_buf_write_size);
2712                 if (retval != ERROR_OK)
2713                         return retval;
2714                 retval = cfi_query_u8(bank, 0, 0x2c, &cfi_info->num_erase_regions);
2715                 if (retval != ERROR_OK)
2716                         return retval;
2717
2718                 LOG_DEBUG("size: 0x%" PRIx32 ", interface desc: %i, max buffer write size: 0x%x",
2719                         cfi_info->dev_size, cfi_info->interface_desc,
2720                         (1 << cfi_info->max_buf_write_size));
2721
2722                 if (cfi_info->num_erase_regions) {
2723                         cfi_info->erase_region_info = malloc(sizeof(*cfi_info->erase_region_info)
2724                                         * cfi_info->num_erase_regions);
2725                         for (i = 0; i < cfi_info->num_erase_regions; i++) {
2726                                 retval = cfi_query_u32(bank,
2727                                                 0,
2728                                                 0x2d + (4 * i),
2729                                                 &cfi_info->erase_region_info[i]);
2730                                 if (retval != ERROR_OK)
2731                                         return retval;
2732                                 LOG_DEBUG(
2733                                         "erase region[%i]: %" PRIu32 " blocks of size 0x%" PRIx32 "",
2734                                         i,
2735                                         (cfi_info->erase_region_info[i] & 0xffff) + 1,
2736                                         (cfi_info->erase_region_info[i] >> 16) * 256);
2737                         }
2738                 } else
2739                         cfi_info->erase_region_info = NULL;
2740
2741                 /* We need to read the primary algorithm extended query table before calculating
2742                  * the sector layout to be able to apply fixups
2743                  */
2744                 switch (cfi_info->pri_id) {
2745                         /* Intel command set (standard and extended) */
2746                         case 0x0001:
2747                         case 0x0003:
2748                                 cfi_read_intel_pri_ext(bank);
2749                                 break;
2750                         /* AMD/Spansion, Atmel, ... command set */
2751                         case 0x0002:
2752                                 cfi_info->status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7;  /*
2753                                                                                                  *default
2754                                                                                                  *for
2755                                                                                                  *all
2756                                                                                                  *CFI
2757                                                                                                  *flashs
2758                                                                                                  **/
2759                                 cfi_read_0002_pri_ext(bank);
2760                                 break;
2761                         default:
2762                                 LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2763                                 break;
2764                 }
2765
2766                 /* return to read array mode
2767                  * we use both reset commands, as some Intel flashes fail to recognize the 0xF0 command
2768                  */
2769                 retval = cfi_reset(bank);
2770                 if (retval != ERROR_OK)
2771                         return retval;
2772         }       /* end CFI case */
2773
2774         LOG_DEBUG("Vcc min: %x.%x, Vcc max: %x.%x, Vpp min: %u.%x, Vpp max: %u.%x",
2775                 (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
2776                 (cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
2777                 (cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
2778                 (cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
2779
2780         LOG_DEBUG("typ. word write timeout: %u us, typ. buf write timeout: %u us, "
2781                 "typ. block erase timeout: %u ms, typ. chip erase timeout: %u ms",
2782                 1 << cfi_info->word_write_timeout_typ, 1 << cfi_info->buf_write_timeout_typ,
2783                 1 << cfi_info->block_erase_timeout_typ, 1 << cfi_info->chip_erase_timeout_typ);
2784
2785         LOG_DEBUG("max. word write timeout: %u us, max. buf write timeout: %u us, "
2786                 "max. block erase timeout: %u ms, max. chip erase timeout: %u ms",
2787                 (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
2788                 (1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
2789                 (1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
2790                 (1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
2791
2792         /* convert timeouts to real values in ms */
2793         cfi_info->word_write_timeout = DIV_ROUND_UP((1L << cfi_info->word_write_timeout_typ) *
2794                         (1L << cfi_info->word_write_timeout_max), 1000);
2795         cfi_info->buf_write_timeout = DIV_ROUND_UP((1L << cfi_info->buf_write_timeout_typ) *
2796                         (1L << cfi_info->buf_write_timeout_max), 1000);
2797         cfi_info->block_erase_timeout = (1L << cfi_info->block_erase_timeout_typ) *
2798                 (1L << cfi_info->block_erase_timeout_max);
2799         cfi_info->chip_erase_timeout = (1L << cfi_info->chip_erase_timeout_typ) *
2800                 (1L << cfi_info->chip_erase_timeout_max);
2801
2802         LOG_DEBUG("calculated word write timeout: %u ms, buf write timeout: %u ms, "
2803                 "block erase timeout: %u ms, chip erase timeout: %u ms",
2804                 cfi_info->word_write_timeout, cfi_info->buf_write_timeout,
2805                 cfi_info->block_erase_timeout, cfi_info->chip_erase_timeout);
2806
2807         /* apply fixups depending on the primary command set */
2808         switch (cfi_info->pri_id) {
2809                 /* Intel command set (standard and extended) */
2810                 case 0x0001:
2811                 case 0x0003:
2812                         cfi_fixup(bank, cfi_0001_fixups);
2813                         break;
2814                 /* AMD/Spansion, Atmel, ... command set */
2815                 case 0x0002:
2816                         cfi_fixup(bank, cfi_0002_fixups);
2817                         break;
2818                 default:
2819                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2820                         break;
2821         }
2822
2823         if ((cfi_info->dev_size * bank->bus_width / bank->chip_width) != bank->size) {
2824                 LOG_WARNING("configuration specifies 0x%" PRIx32 " size, but a 0x%" PRIx32
2825                         " size flash was found", bank->size, cfi_info->dev_size);
2826         }
2827
2828         if (cfi_info->num_erase_regions == 0) {
2829                 /* a device might have only one erase block, spanning the whole device */
2830                 bank->num_sectors = 1;
2831                 bank->sectors = malloc(sizeof(struct flash_sector));
2832
2833                 bank->sectors[sector].offset = 0x0;
2834                 bank->sectors[sector].size = bank->size;
2835                 bank->sectors[sector].is_erased = -1;
2836                 bank->sectors[sector].is_protected = -1;
2837         } else {
2838                 uint32_t offset = 0;
2839
2840                 for (i = 0; i < cfi_info->num_erase_regions; i++)
2841                         num_sectors += (cfi_info->erase_region_info[i] & 0xffff) + 1;
2842
2843                 bank->num_sectors = num_sectors;
2844                 bank->sectors = malloc(sizeof(struct flash_sector) * num_sectors);
2845
2846                 for (i = 0; i < cfi_info->num_erase_regions; i++) {
2847                         uint32_t j;
2848                         for (j = 0; j < (cfi_info->erase_region_info[i] & 0xffff) + 1; j++) {
2849                                 bank->sectors[sector].offset = offset;
2850                                 bank->sectors[sector].size =
2851                                         ((cfi_info->erase_region_info[i] >> 16) * 256)
2852                                         * bank->bus_width / bank->chip_width;
2853                                 offset += bank->sectors[sector].size;
2854                                 bank->sectors[sector].is_erased = -1;
2855                                 bank->sectors[sector].is_protected = -1;
2856                                 sector++;
2857                         }
2858                 }
2859                 if (offset != (cfi_info->dev_size * bank->bus_width / bank->chip_width)) {
2860                         LOG_WARNING(
2861                                 "CFI size is 0x%" PRIx32 ", but total sector size is 0x%" PRIx32 "", \
2862                                 (cfi_info->dev_size * bank->bus_width / bank->chip_width),
2863                                 offset);
2864                 }
2865         }
2866
2867         cfi_info->probed = 1;
2868
2869         return ERROR_OK;
2870 }
2871
2872 static int cfi_auto_probe(struct flash_bank *bank)
2873 {
2874         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2875         if (cfi_info->probed)
2876                 return ERROR_OK;
2877         return cfi_probe(bank);
2878 }
2879
2880 static int cfi_intel_protect_check(struct flash_bank *bank)
2881 {
2882         int retval;
2883         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2884         struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
2885         int i;
2886
2887         /* check if block lock bits are supported on this device */
2888         if (!(pri_ext->blk_status_reg_mask & 0x1))
2889                 return ERROR_FLASH_OPERATION_FAILED;
2890
2891         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, 0x55));
2892         if (retval != ERROR_OK)
2893                 return retval;
2894
2895         for (i = 0; i < bank->num_sectors; i++) {
2896                 uint8_t block_status;
2897                 retval = cfi_get_u8(bank, i, 0x2, &block_status);
2898                 if (retval != ERROR_OK)
2899                         return retval;
2900
2901                 if (block_status & 1)
2902                         bank->sectors[i].is_protected = 1;
2903                 else
2904                         bank->sectors[i].is_protected = 0;
2905         }
2906
2907         return cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2908 }
2909
2910 static int cfi_spansion_protect_check(struct flash_bank *bank)
2911 {
2912         int retval;
2913         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2914         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2915         int i;
2916
2917         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
2918         if (retval != ERROR_OK)
2919                 return retval;
2920
2921         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
2922         if (retval != ERROR_OK)
2923                 return retval;
2924
2925         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, pri_ext->_unlock1));
2926         if (retval != ERROR_OK)
2927                 return retval;
2928
2929         for (i = 0; i < bank->num_sectors; i++) {
2930                 uint8_t block_status;
2931                 retval = cfi_get_u8(bank, i, 0x2, &block_status);
2932                 if (retval != ERROR_OK)
2933                         return retval;
2934
2935                 if (block_status & 1)
2936                         bank->sectors[i].is_protected = 1;
2937                 else
2938                         bank->sectors[i].is_protected = 0;
2939         }
2940
2941         return cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2942 }
2943
2944 static int cfi_protect_check(struct flash_bank *bank)
2945 {
2946         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2947
2948         if (bank->target->state != TARGET_HALTED) {
2949                 LOG_ERROR("Target not halted");
2950                 return ERROR_TARGET_NOT_HALTED;
2951         }
2952
2953         if (cfi_info->qry[0] != 'Q')
2954                 return ERROR_FLASH_BANK_NOT_PROBED;
2955
2956         switch (cfi_info->pri_id) {
2957                 case 1:
2958                 case 3:
2959                         return cfi_intel_protect_check(bank);
2960                         break;
2961                 case 2:
2962                         return cfi_spansion_protect_check(bank);
2963                         break;
2964                 default:
2965                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2966                         break;
2967         }
2968
2969         return ERROR_OK;
2970 }
2971
2972 static int get_cfi_info(struct flash_bank *bank, char *buf, int buf_size)
2973 {
2974         int printed;
2975         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2976
2977         if (cfi_info->qry[0] == 0xff) {
2978                 snprintf(buf, buf_size, "\ncfi flash bank not probed yet\n");
2979                 return ERROR_OK;
2980         }
2981
2982         if (cfi_info->not_cfi == 0)
2983                 printed = snprintf(buf, buf_size, "\nCFI flash: ");
2984         else
2985                 printed = snprintf(buf, buf_size, "\nnon-CFI flash: ");
2986         buf += printed;
2987         buf_size -= printed;
2988
2989         printed = snprintf(buf, buf_size, "mfr: 0x%4.4x, id:0x%4.4x\n\n",
2990                         cfi_info->manufacturer, cfi_info->device_id);
2991         buf += printed;
2992         buf_size -= printed;
2993
2994         printed = snprintf(buf, buf_size, "qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: "
2995                         "0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x\n",
2996                         cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2],
2997                         cfi_info->pri_id, cfi_info->pri_addr, cfi_info->alt_id, cfi_info->alt_addr);
2998         buf += printed;
2999         buf_size -= printed;
3000
3001         printed = snprintf(buf, buf_size, "Vcc min: %x.%x, Vcc max: %x.%x, "
3002                         "Vpp min: %u.%x, Vpp max: %u.%x\n",
3003                         (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
3004                         (cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
3005                         (cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
3006                         (cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
3007         buf += printed;
3008         buf_size -= printed;
3009
3010         printed = snprintf(buf, buf_size, "typ. word write timeout: %u us, "
3011                         "typ. buf write timeout: %u us, "
3012                         "typ. block erase timeout: %u ms, "
3013                         "typ. chip erase timeout: %u ms\n",
3014                         1 << cfi_info->word_write_timeout_typ,
3015                         1 << cfi_info->buf_write_timeout_typ,
3016                         1 << cfi_info->block_erase_timeout_typ,
3017                         1 << cfi_info->chip_erase_timeout_typ);
3018         buf += printed;
3019         buf_size -= printed;
3020
3021         printed = snprintf(buf,
3022                         buf_size,
3023                         "max. word write timeout: %u us, "
3024                         "max. buf write timeout: %u us, max. "
3025                         "block erase timeout: %u ms, max. chip erase timeout: %u ms\n",
3026                         (1 <<
3027                          cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
3028                         (1 <<
3029                          cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
3030                         (1 <<
3031                          cfi_info->block_erase_timeout_max) *
3032                         (1 << cfi_info->block_erase_timeout_typ),
3033                         (1 <<
3034                          cfi_info->chip_erase_timeout_max) *
3035                         (1 << cfi_info->chip_erase_timeout_typ));
3036         buf += printed;
3037         buf_size -= printed;
3038
3039         printed = snprintf(buf, buf_size, "size: 0x%" PRIx32 ", interface desc: %i, "
3040                         "max buffer write size: 0x%x\n",
3041                         cfi_info->dev_size,
3042                         cfi_info->interface_desc,
3043                         1 << cfi_info->max_buf_write_size);
3044         buf += printed;
3045         buf_size -= printed;
3046
3047         switch (cfi_info->pri_id) {
3048             case 1:
3049             case 3:
3050                     cfi_intel_info(bank, buf, buf_size);
3051                     break;
3052             case 2:
3053                     cfi_spansion_info(bank, buf, buf_size);
3054                     break;
3055             default:
3056                     LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
3057                     break;
3058         }
3059
3060         return ERROR_OK;
3061 }
3062
3063 static void cfi_fixup_0002_write_buffer(struct flash_bank *bank, void *param)
3064 {
3065         struct cfi_flash_bank *cfi_info = bank->driver_priv;
3066
3067         /* disable write buffer for M29W128G */
3068         cfi_info->buf_write_timeout_typ = 0;
3069 }
3070
3071 struct flash_driver cfi_flash = {
3072         .name = "cfi",
3073         .flash_bank_command = cfi_flash_bank_command,
3074         .erase = cfi_erase,
3075         .protect = cfi_protect,
3076         .write = cfi_write,
3077         .read = cfi_read,
3078         .probe = cfi_probe,
3079         .auto_probe = cfi_auto_probe,
3080         /* FIXME: access flash at bus_width size */
3081         .erase_check = default_flash_blank_check,
3082         .protect_check = cfi_protect_check,
3083         .info = get_cfi_info,
3084 };