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->write_algorithm = NULL;
 829
 830         cfi_info->x16_as_x8 = 0;
 831         cfi_info->jedec_probe = 0;
 832         cfi_info->not_cfi = 0;
 833
 834         for (unsigned i = 6; i < CMD_ARGC; i++) {
 835                 if (strcmp(CMD_ARGV[i], "x16_as_x8") == 0)
 836                         cfi_info->x16_as_x8 = 1;
 837                 else if (strcmp(CMD_ARGV[i], "jedec_probe") == 0)
 838                         cfi_info->jedec_probe = 1;
 839         }
 840
 841         cfi_info->write_algorithm = NULL;
 842
 843         /* bank wasn't probed yet */
 844         cfi_info->qry[0] = 0xff;
 845
 846         return ERROR_OK;
 847 }
 848
 849 static int cfi_intel_erase(struct flash_bank *bank, int first, int last)
 850 {
 851         int retval;
 852         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 853         int i;
 854
 855         cfi_intel_clear_status_register(bank);
 856
 857         for (i = first; i <= last; i++) {
 858                 retval = cfi_send_command(bank, 0x20, flash_address(bank, i, 0x0));
 859                 if (retval != ERROR_OK)
 860                         return retval;
 861
 862                 retval = cfi_send_command(bank, 0xd0, flash_address(bank, i, 0x0));
 863                 if (retval != ERROR_OK)
 864                         return retval;
 865
 866                 uint8_t status;
 867                 retval = cfi_intel_wait_status_busy(bank, cfi_info->block_erase_timeout, &status);
 868                 if (retval != ERROR_OK)
 869                         return retval;
 870
 871                 if (status == 0x80)
 872                         bank->sectors[i].is_erased = 1;
 873                 else {
 874                         retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
 875                         if (retval != ERROR_OK)
 876                                 return retval;
 877
 878                         LOG_ERROR("couldn't erase block %i of flash bank at base 0x%" PRIx32,
 879                                 i,
 880                                 bank->base);
 881                         return ERROR_FLASH_OPERATION_FAILED;
 882                 }
 883         }
 884
 885         return cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
 886 }
 887
 888 static int cfi_spansion_erase(struct flash_bank *bank, int first, int last)
 889 {
 890         int retval;
 891         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 892         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 893         int i;
 894
 895         for (i = first; i <= last; i++) {
 896                 retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
 897                 if (retval != ERROR_OK)
 898                         return retval;
 899
 900                 retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
 901                 if (retval != ERROR_OK)
 902                         return retval;
 903
 904                 retval = cfi_send_command(bank, 0x80, flash_address(bank, 0, pri_ext->_unlock1));
 905                 if (retval != ERROR_OK)
 906                         return retval;
 907
 908                 retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
 909                 if (retval != ERROR_OK)
 910                         return retval;
 911
 912                 retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
 913                 if (retval != ERROR_OK)
 914                         return retval;
 915
 916                 retval = cfi_send_command(bank, 0x30, flash_address(bank, i, 0x0));
 917                 if (retval != ERROR_OK)
 918                         return retval;
 919
 920                 if (cfi_spansion_wait_status_busy(bank, cfi_info->block_erase_timeout) == ERROR_OK)
 921                         bank->sectors[i].is_erased = 1;
 922                 else {
 923                         retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 924                         if (retval != ERROR_OK)
 925                                 return retval;
 926
 927                         LOG_ERROR("couldn't erase block %i of flash bank at base 0x%"
 928                                 PRIx32, i, bank->base);
 929                         return ERROR_FLASH_OPERATION_FAILED;
 930                 }
 931         }
 932
 933         return cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 934 }
 935
 936 static int cfi_erase(struct flash_bank *bank, int first, int last)
 937 {
 938         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 939
 940         if (bank->target->state != TARGET_HALTED) {
 941                 LOG_ERROR("Target not halted");
 942                 return ERROR_TARGET_NOT_HALTED;
 943         }
 944
 945         if ((first < 0) || (last < first) || (last >= bank->num_sectors))
 946                 return ERROR_FLASH_SECTOR_INVALID;
 947
 948         if (cfi_info->qry[0] != 'Q')
 949                 return ERROR_FLASH_BANK_NOT_PROBED;
 950
 951         switch (cfi_info->pri_id) {
 952                 case 1:
 953                 case 3:
 954                         return cfi_intel_erase(bank, first, last);
 955                         break;
 956                 case 2:
 957                         return cfi_spansion_erase(bank, first, last);
 958                         break;
 959                 default:
 960                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
 961                         break;
 962         }
 963
 964         return ERROR_OK;
 965 }
 966
 967 static int cfi_intel_protect(struct flash_bank *bank, int set, int first, int last)
 968 {
 969         int retval;
 970         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 971         struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
 972         int retry = 0;
 973         int i;
 974
 975         /* if the device supports neither legacy lock/unlock (bit 3) nor
 976          * instant individual block locking (bit 5).
 977          */
 978         if (!(pri_ext->feature_support & 0x28)) {
 979                 LOG_ERROR("lock/unlock not supported on flash");
 980                 return ERROR_FLASH_OPERATION_FAILED;
 981         }
 982
 983         cfi_intel_clear_status_register(bank);
 984
 985         for (i = first; i <= last; i++) {
 986                 retval = cfi_send_command(bank, 0x60, flash_address(bank, i, 0x0));
 987                 if (retval != ERROR_OK)
 988                         return retval;
 989                 if (set) {
 990                         retval = cfi_send_command(bank, 0x01, flash_address(bank, i, 0x0));
 991                         if (retval != ERROR_OK)
 992                                 return retval;
 993                         bank->sectors[i].is_protected = 1;
 994                 } else {
 995                         retval = cfi_send_command(bank, 0xd0, flash_address(bank, i, 0x0));
 996                         if (retval != ERROR_OK)
 997                                 return retval;
 998                         bank->sectors[i].is_protected = 0;
 999                 }
1000
1001                 /* instant individual block locking doesn't require reading of the status register
1002                  **/
1003                 if (!(pri_ext->feature_support & 0x20)) {
1004                         /* Clear lock bits operation may take up to 1.4s */
1005                         uint8_t status;
1006                         retval = cfi_intel_wait_status_busy(bank, 1400, &status);
1007                         if (retval != ERROR_OK)
1008                                 return retval;
1009                 } else {
1010                         uint8_t block_status;
1011                         /* read block lock bit, to verify status */
1012                         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, 0x55));
1013                         if (retval != ERROR_OK)
1014                                 return retval;
1015                         retval = cfi_get_u8(bank, i, 0x2, &block_status);
1016                         if (retval != ERROR_OK)
1017                                 return retval;
1018
1019                         if ((block_status & 0x1) != set) {
1020                                 LOG_ERROR(
1021                                         "couldn't change block lock status (set = %i, block_status = 0x%2.2x)",
1022                                         set, block_status);
1023                                 retval = cfi_send_command(bank, 0x70, flash_address(bank, 0, 0x55));
1024                                 if (retval != ERROR_OK)
1025                                         return retval;
1026                                 uint8_t status;
1027                                 retval = cfi_intel_wait_status_busy(bank, 10, &status);
1028                                 if (retval != ERROR_OK)
1029                                         return retval;
1030
1031                                 if (retry > 10)
1032                                         return ERROR_FLASH_OPERATION_FAILED;
1033                                 else {
1034                                         i--;
1035                                         retry++;
1036                                 }
1037                         }
1038                 }
1039         }
1040
1041         /* if the device doesn't support individual block lock bits set/clear,
1042          * all blocks have been unlocked in parallel, so we set those that should be protected
1043          */
1044         if ((!set) && (!(pri_ext->feature_support & 0x20))) {
1045                 /* FIX!!! this code path is broken!!!
1046                  *
1047                  * The correct approach is:
1048                  *
1049                  * 1. read out current protection status
1050                  *
1051                  * 2. override read out protection status w/unprotected.
1052                  *
1053                  * 3. re-protect what should be protected.
1054                  *
1055                  */
1056                 for (i = 0; i < bank->num_sectors; i++) {
1057                         if (bank->sectors[i].is_protected == 1) {
1058                                 cfi_intel_clear_status_register(bank);
1059
1060                                 retval = cfi_send_command(bank, 0x60, flash_address(bank, i, 0x0));
1061                                 if (retval != ERROR_OK)
1062                                         return retval;
1063
1064                                 retval = cfi_send_command(bank, 0x01, flash_address(bank, i, 0x0));
1065                                 if (retval != ERROR_OK)
1066                                         return retval;
1067
1068                                 uint8_t status;
1069                                 retval = cfi_intel_wait_status_busy(bank, 100, &status);
1070                                 if (retval != ERROR_OK)
1071                                         return retval;
1072                         }
1073                 }
1074         }
1075
1076         return cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
1077 }
1078
1079 static int cfi_protect(struct flash_bank *bank, int set, int first, int last)
1080 {
1081         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1082
1083         if (bank->target->state != TARGET_HALTED) {
1084                 LOG_ERROR("Target not halted");
1085                 return ERROR_TARGET_NOT_HALTED;
1086         }
1087
1088         if ((first < 0) || (last < first) || (last >= bank->num_sectors)) {
1089                 LOG_ERROR("Invalid sector range");
1090                 return ERROR_FLASH_SECTOR_INVALID;
1091         }
1092
1093         if (cfi_info->qry[0] != 'Q')
1094                 return ERROR_FLASH_BANK_NOT_PROBED;
1095
1096         switch (cfi_info->pri_id) {
1097                 case 1:
1098                 case 3:
1099                         return cfi_intel_protect(bank, set, first, last);
1100                         break;
1101                 default:
1102                         LOG_WARNING("protect: cfi primary command set %i unsupported", cfi_info->pri_id);
1103                         return ERROR_OK;
1104         }
1105 }
1106
1107 /* Convert code image to target endian
1108  * FIXME create general block conversion fcts in target.c?) */
1109 static void cfi_fix_code_endian(struct target *target, uint8_t *dest,
1110         const uint32_t *src, uint32_t count)
1111 {
1112         uint32_t i;
1113         for (i = 0; i < count; i++) {
1114                 target_buffer_set_u32(target, dest, *src);
1115                 dest += 4;
1116                 src++;
1117         }
1118 }
1119
1120 static uint32_t cfi_command_val(struct flash_bank *bank, uint8_t cmd)
1121 {
1122         struct target *target = bank->target;
1123
1124         uint8_t buf[CFI_MAX_BUS_WIDTH];
1125         cfi_command(bank, cmd, buf);
1126         switch (bank->bus_width) {
1127                 case 1:
1128                         return buf[0];
1129                         break;
1130                 case 2:
1131                         return target_buffer_get_u16(target, buf);
1132                         break;
1133                 case 4:
1134                         return target_buffer_get_u32(target, buf);
1135                         break;
1136                 default:
1137                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1138                                         bank->bus_width);
1139                         return 0;
1140         }
1141 }
1142
1143 static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
1144         uint32_t address, uint32_t count)
1145 {
1146         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1147         struct target *target = bank->target;
1148         struct reg_param reg_params[7];
1149         struct arm_algorithm arm_algo;
1150         struct working_area *source = NULL;
1151         uint32_t buffer_size = 32768;
1152         uint32_t write_command_val, busy_pattern_val, error_pattern_val;
1153
1154         /* algorithm register usage:
1155          * r0: source address (in RAM)
1156          * r1: target address (in Flash)
1157          * r2: count
1158          * r3: flash write command
1159          * r4: status byte (returned to host)
1160          * r5: busy test pattern
1161          * r6: error test pattern
1162          */
1163
1164         /* see contib/loaders/flash/armv4_5_cfi_intel_32.s for src */
1165         static const uint32_t word_32_code[] = {
1166                 0xe4904004,     /* loop:        ldr r4, [r0], #4 */
1167                 0xe5813000,     /*              str r3, [r1] */
1168                 0xe5814000,     /*              str r4, [r1] */
1169                 0xe5914000,     /* busy:  ldr r4, [r1] */
1170                 0xe0047005,     /*              and r7, r4, r5 */
1171                 0xe1570005,     /*              cmp r7, r5 */
1172                 0x1afffffb,     /*              bne busy */
1173                 0xe1140006,     /*              tst r4, r6 */
1174                 0x1a000003,     /*              bne done */
1175                 0xe2522001,     /*              subs r2, r2, #1 */
1176                 0x0a000001,     /*              beq done */
1177                 0xe2811004,     /*              add r1, r1 #4 */
1178                 0xeafffff2,     /*              b loop */
1179                 0xeafffffe      /* done:        b -2 */
1180         };
1181
1182         /* see contib/loaders/flash/armv4_5_cfi_intel_16.s for src */
1183         static const uint32_t word_16_code[] = {
1184                 0xe0d040b2,     /* loop:        ldrh r4, [r0], #2 */
1185                 0xe1c130b0,     /*              strh r3, [r1] */
1186                 0xe1c140b0,     /*              strh r4, [r1] */
1187                 0xe1d140b0,     /* busy ldrh r4, [r1] */
1188                 0xe0047005,     /*              and r7, r4, r5 */
1189                 0xe1570005,     /*              cmp r7, r5 */
1190                 0x1afffffb,     /*              bne busy */
1191                 0xe1140006,     /*              tst r4, r6 */
1192                 0x1a000003,     /*              bne done */
1193                 0xe2522001,     /*              subs r2, r2, #1 */
1194                 0x0a000001,     /*              beq done */
1195                 0xe2811002,     /*              add r1, r1 #2 */
1196                 0xeafffff2,     /*              b loop */
1197                 0xeafffffe      /* done:        b -2 */
1198         };
1199
1200         /* see contib/loaders/flash/armv4_5_cfi_intel_8.s for src */
1201         static const uint32_t word_8_code[] = {
1202                 0xe4d04001,     /* loop:        ldrb r4, [r0], #1 */
1203                 0xe5c13000,     /*              strb r3, [r1] */
1204                 0xe5c14000,     /*              strb r4, [r1] */
1205                 0xe5d14000,     /* busy ldrb r4, [r1] */
1206                 0xe0047005,     /*              and r7, r4, r5 */
1207                 0xe1570005,     /*              cmp r7, r5 */
1208                 0x1afffffb,     /*              bne busy */
1209                 0xe1140006,     /*              tst r4, r6 */
1210                 0x1a000003,     /*              bne done */
1211                 0xe2522001,     /*              subs r2, r2, #1 */
1212                 0x0a000001,     /*              beq done */
1213                 0xe2811001,     /*              add r1, r1 #1 */
1214                 0xeafffff2,     /*              b loop */
1215                 0xeafffffe      /* done:        b -2 */
1216         };
1217         uint8_t target_code[4*CFI_MAX_INTEL_CODESIZE];
1218         const uint32_t *target_code_src;
1219         uint32_t target_code_size;
1220         int retval = ERROR_OK;
1221
1222         /* check we have a supported arch */
1223         if (is_arm(target_to_arm(target))) {
1224                 /* All other ARM CPUs have 32 bit instructions */
1225                 arm_algo.common_magic = ARM_COMMON_MAGIC;
1226                 arm_algo.core_mode = ARM_MODE_SVC;
1227                 arm_algo.core_state = ARM_STATE_ARM;
1228         } else {
1229                 LOG_ERROR("Unknown architecture");
1230                 return ERROR_FAIL;
1231         }
1232
1233         cfi_intel_clear_status_register(bank);
1234
1235         /* If we are setting up the write_algorith, we need target_code_src
1236          * if not we only need target_code_size. */
1237
1238         /* However, we don't want to create multiple code paths, so we
1239          * do the unecessary evaluation of target_code_src, which the
1240          * compiler will probably nicely optimize away if not needed */
1241
1242         /* prepare algorithm code for target endian */
1243         switch (bank->bus_width) {
1244                 case 1:
1245                         target_code_src = word_8_code;
1246                         target_code_size = sizeof(word_8_code);
1247                         break;
1248                 case 2:
1249                         target_code_src = word_16_code;
1250                         target_code_size = sizeof(word_16_code);
1251                         break;
1252                 case 4:
1253                         target_code_src = word_32_code;
1254                         target_code_size = sizeof(word_32_code);
1255                         break;
1256                 default:
1257                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1258                                         bank->bus_width);
1259                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1260         }
1261
1262         /* flash write code */
1263         if (!cfi_info->write_algorithm) {
1264                 if (target_code_size > sizeof(target_code)) {
1265                         LOG_WARNING("Internal error - target code buffer to small. "
1266                                 "Increase CFI_MAX_INTEL_CODESIZE and recompile.");
1267                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1268                 }
1269                 cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
1270
1271                 /* Get memory for block write handler */
1272                 retval = target_alloc_working_area(target,
1273                                 target_code_size,
1274                                 &cfi_info->write_algorithm);
1275                 if (retval != ERROR_OK) {
1276                         LOG_WARNING("No working area available, can't do block memory writes");
1277                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1278                 }
1279                 ;
1280
1281                 /* write algorithm code to working area */
1282                 retval = target_write_buffer(target, cfi_info->write_algorithm->address,
1283                                 target_code_size, target_code);
1284                 if (retval != ERROR_OK) {
1285                         LOG_ERROR("Unable to write block write code to target");
1286                         goto cleanup;
1287                 }
1288         }
1289
1290         /* Get a workspace buffer for the data to flash starting with 32k size.
1291            Half size until buffer would be smaller 256 Bytem then fail back */
1292         /* FIXME Why 256 bytes, why not 32 bytes (smallest flash write page */
1293         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
1294                 buffer_size /= 2;
1295                 if (buffer_size <= 256) {
1296                         LOG_WARNING(
1297                                 "no large enough working area available, can't do block memory writes");
1298                         retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1299                         goto cleanup;
1300                 }
1301         }
1302         ;
1303
1304         /* setup algo registers */
1305         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
1306         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
1307         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
1308         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
1309         init_reg_param(&reg_params[4], "r4", 32, PARAM_IN);
1310         init_reg_param(&reg_params[5], "r5", 32, PARAM_OUT);
1311         init_reg_param(&reg_params[6], "r6", 32, PARAM_OUT);
1312
1313         /* prepare command and status register patterns */
1314         write_command_val = cfi_command_val(bank, 0x40);
1315         busy_pattern_val  = cfi_command_val(bank, 0x80);
1316         error_pattern_val = cfi_command_val(bank, 0x7e);
1317
1318         LOG_DEBUG("Using target buffer at 0x%08" PRIx32 " and of size 0x%04" PRIx32,
1319                 source->address, buffer_size);
1320
1321         /* Programming main loop */
1322         while (count > 0) {
1323                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
1324                 uint32_t wsm_error;
1325
1326                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
1327                 if (retval != ERROR_OK)
1328                         goto cleanup;
1329
1330                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
1331                 buf_set_u32(reg_params[1].value, 0, 32, address);
1332                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
1333
1334                 buf_set_u32(reg_params[3].value, 0, 32, write_command_val);
1335                 buf_set_u32(reg_params[5].value, 0, 32, busy_pattern_val);
1336                 buf_set_u32(reg_params[6].value, 0, 32, error_pattern_val);
1337
1338                 LOG_DEBUG("Write 0x%04" PRIx32 " bytes to flash at 0x%08" PRIx32,
1339                         thisrun_count, address);
1340
1341                 /* Execute algorithm, assume breakpoint for last instruction */
1342                 retval = target_run_algorithm(target, 0, NULL, 7, reg_params,
1343                                 cfi_info->write_algorithm->address,
1344                                 cfi_info->write_algorithm->address + target_code_size -
1345                                 sizeof(uint32_t),
1346                                 10000,  /* 10s should be enough for max. 32k of data */
1347                                 &arm_algo);
1348
1349                 /* On failure try a fall back to direct word writes */
1350                 if (retval != ERROR_OK) {
1351                         cfi_intel_clear_status_register(bank);
1352                         LOG_ERROR(
1353                                 "Execution of flash algorythm failed. Can't fall back. Please report.");
1354                         retval = ERROR_FLASH_OPERATION_FAILED;
1355                         /* retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE; */
1356                         /* FIXME To allow fall back or recovery, we must save the actual status
1357                          * somewhere, so that a higher level code can start recovery. */
1358                         goto cleanup;
1359                 }
1360
1361                 /* Check return value from algo code */
1362                 wsm_error = buf_get_u32(reg_params[4].value, 0, 32) & error_pattern_val;
1363                 if (wsm_error) {
1364                         /* read status register (outputs debug inforation) */
1365                         uint8_t status;
1366                         cfi_intel_wait_status_busy(bank, 100, &status);
1367                         cfi_intel_clear_status_register(bank);
1368                         retval = ERROR_FLASH_OPERATION_FAILED;
1369                         goto cleanup;
1370                 }
1371
1372                 buffer += thisrun_count;
1373                 address += thisrun_count;
1374                 count -= thisrun_count;
1375
1376                 keep_alive();
1377         }
1378
1379         /* free up resources */
1380 cleanup:
1381         if (source)
1382                 target_free_working_area(target, source);
1383
1384         if (cfi_info->write_algorithm) {
1385                 target_free_working_area(target, cfi_info->write_algorithm);
1386                 cfi_info->write_algorithm = NULL;
1387         }
1388
1389         destroy_reg_param(&reg_params[0]);
1390         destroy_reg_param(&reg_params[1]);
1391         destroy_reg_param(&reg_params[2]);
1392         destroy_reg_param(&reg_params[3]);
1393         destroy_reg_param(&reg_params[4]);
1394         destroy_reg_param(&reg_params[5]);
1395         destroy_reg_param(&reg_params[6]);
1396
1397         return retval;
1398 }
1399
1400 static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffer,
1401         uint32_t address, uint32_t count)
1402 {
1403         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1404         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
1405         struct target *target = bank->target;
1406         struct reg_param reg_params[10];
1407         struct mips32_algorithm mips32_info;
1408         struct working_area *source;
1409         uint32_t buffer_size = 32768;
1410         uint32_t status;
1411         int retval = ERROR_OK;
1412
1413         /* input parameters -
1414          *      4  A0 = source address
1415          *      5  A1 = destination address
1416          *      6  A2 = number of writes
1417          *      7  A3 = flash write command
1418          *      8  T0 = constant to mask DQ7 bits (also used for Dq5 with shift)
1419          * output parameters -
1420          *      9  T1 = 0x80 ok 0x00 bad
1421          * temp registers -
1422          *      10 T2 = value read from flash to test status
1423          *      11 T3 = holding register
1424          * unlock registers -
1425          *  12 T4 = unlock1_addr
1426          *  13 T5 = unlock1_cmd
1427          *  14 T6 = unlock2_addr
1428          *  15 T7 = unlock2_cmd */
1429
1430         static const uint32_t mips_word_16_code[] = {
1431                 /* start:       */
1432                 MIPS32_LHU(9, 0, 4),            /* lhu $t1, ($a0)               ; out = &saddr                          */
1433                 MIPS32_ADDI(4, 4, 2),           /* addi $a0, $a0, 2             ; saddr += 2                            */
1434                 MIPS32_SH(13, 0, 12),           /* sh $t5, ($t4)                ; *fl_unl_addr1 =
1435                                                  *fl_unl_cmd1           */
1436                 MIPS32_SH(15, 0, 14),           /* sh $t7, ($t6)                ; *fl_unl_addr2 =
1437                                                  *fl_unl_cmd2           */
1438                 MIPS32_SH(7, 0, 12),            /* sh $a3, ($t4)                ; *fl_unl_addr1 =
1439                                                  *fl_write_cmd          */
1440                 MIPS32_SH(9, 0, 5),             /* sh $t1, ($a1)                ; *daddr = out                          */
1441                 MIPS32_NOP,                     /* nop                                                                  */
1442                 /* busy:        */
1443                 MIPS32_LHU(10, 0, 5),           /* lhu $t2, ($a1)               ; temp1 = *daddr                        */
1444                 MIPS32_XOR(11, 9, 10),          /* xor $t3, $a0, $t2            ; temp2 = out ^
1445                                                  *temp1;                        */
1446                 MIPS32_AND(11, 8, 11),          /* and $t3, $t0, $t3            ; temp2 = temp2 &
1447                                                  *DQ7mask               */
1448                 MIPS32_BNE(11, 8, 13),          /* bne $t3, $t0, cont           ; if (temp2 !=
1449                                                  *DQ7mask) goto cont    */
1450                 MIPS32_NOP,                     /* nop                                                                  */
1451
1452                 MIPS32_SRL(10, 8, 2),           /* srl $t2,$t0,2                ; temp1 = DQ7mask >>
1453                                                  *2                     */
1454                 MIPS32_AND(11, 10, 11),         /* and $t3, $t2, $t3            ; temp2 = temp2 &
1455                                                  *temp1                 */
1456                 MIPS32_BNE(11, 10, NEG16(8)),   /* bne $t3, $t2, busy           ; if (temp2 !=
1457                                                  *temp1) goto busy              */
1458                 MIPS32_NOP,                     /* nop                                                                  */
1459
1460                 MIPS32_LHU(10, 0, 5),           /* lhu $t2, ($a1)               ; temp1 = *daddr                        */
1461                 MIPS32_XOR(11, 9, 10),          /* xor $t3, $a0, $t2            ; temp2 = out ^
1462                                                  *temp1;                        */
1463                 MIPS32_AND(11, 8, 11),          /* and $t3, $t0, $t3            ; temp2 = temp2 &
1464                                                  *DQ7mask               */
1465                 MIPS32_BNE(11, 8, 4),           /* bne $t3, $t0, cont           ; if (temp2 !=
1466                                                  *DQ7mask) goto cont    */
1467                 MIPS32_NOP,                     /* nop                                                                  */
1468
1469                 MIPS32_XOR(9, 9, 9),            /* xor $t1, $t1, $t1            ; out = 0                               */
1470                 MIPS32_BEQ(9, 0, 11),           /* beq $t1, $zero, done         ; if (out == 0) goto
1471                                                  *done          */
1472                 MIPS32_NOP,                     /* nop                                                                  */
1473                 /* cont:        */
1474                 MIPS32_ADDI(6, 6, NEG16(1)),    /* addi, $a2, $a2, -1           ; numwrites--                           */
1475                 MIPS32_BNE(6, 0, 5),            /* bne $a2, $zero, cont2        ; if (numwrite != 0)
1476                                                  *goto cont2            */
1477                 MIPS32_NOP,                     /* nop                                                                  */
1478
1479                 MIPS32_LUI(9, 0),               /* lui $t1, 0                                                           */
1480                 MIPS32_ORI(9, 9, 0x80),         /* ori $t1, $t1, 0x80           ; out = 0x80                            */
1481
1482                 MIPS32_B(4),                    /* b done                       ; goto done                             */
1483                 MIPS32_NOP,                     /* nop                                                                  */
1484                 /* cont2:       */
1485                 MIPS32_ADDI(5, 5, 2),           /* addi $a0, $a0, 2             ; daddr += 2                            */
1486                 MIPS32_B(NEG16(33)),            /* b start                      ; goto start                            */
1487                 MIPS32_NOP,                     /* nop                                                                  */
1488                 /* done:
1489                  *MIPS32_B(NEG16(1)),   */      /* b done                       ; goto done                             */
1490                 MIPS32_SDBBP,                   /* sdbbp                        ; break();                              */
1491                 /*MIPS32_B(NEG16(33)),  */      /* b start                      ; goto start
1492                  * MIPS32_NOP, */
1493         };
1494
1495         mips32_info.common_magic = MIPS32_COMMON_MAGIC;
1496         mips32_info.isa_mode = MIPS32_ISA_MIPS32;
1497
1498         int target_code_size = 0;
1499         const uint32_t *target_code_src = NULL;
1500
1501         switch (bank->bus_width) {
1502                 case 2:
1503                         /* Check for DQ5 support */
1504                         if (cfi_info->status_poll_mask & (1 << 5)) {
1505                                 target_code_src = mips_word_16_code;
1506                                 target_code_size = sizeof(mips_word_16_code);
1507                         } else {
1508                                 LOG_ERROR("Need DQ5 support");
1509                                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1510                                 /* target_code_src = mips_word_16_code_dq7only; */
1511                                 /* target_code_size = sizeof(mips_word_16_code_dq7only); */
1512                         }
1513                         break;
1514                 default:
1515                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1516                                         bank->bus_width);
1517                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1518         }
1519
1520         /* flash write code */
1521         if (!cfi_info->write_algorithm) {
1522                 uint8_t *target_code;
1523
1524                 /* convert bus-width dependent algorithm code to correct endiannes */
1525                 target_code = malloc(target_code_size);
1526                 if (target_code == NULL) {
1527                         LOG_ERROR("Out of memory");
1528                         return ERROR_FAIL;
1529                 }
1530                 cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
1531
1532                 /* allocate working area */
1533                 retval = target_alloc_working_area(target, target_code_size,
1534                                 &cfi_info->write_algorithm);
1535                 if (retval != ERROR_OK) {
1536                         free(target_code);
1537                         return retval;
1538                 }
1539
1540                 /* write algorithm code to working area */
1541                 retval = target_write_buffer(target, cfi_info->write_algorithm->address,
1542                                 target_code_size, target_code);
1543                 if (retval != ERROR_OK) {
1544                         free(target_code);
1545                         return retval;
1546                 }
1547
1548                 free(target_code);
1549         }
1550         /* the following code still assumes target code is fixed 24*4 bytes */
1551
1552         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
1553                 buffer_size /= 2;
1554                 if (buffer_size <= 256) {
1555                         /* if we already allocated the writing code, but failed to get a
1556                          * buffer, free the algorithm */
1557                         if (cfi_info->write_algorithm)
1558                                 target_free_working_area(target, cfi_info->write_algorithm);
1559
1560                         LOG_WARNING(
1561                                 "not enough working area available, can't do block memory writes");
1562                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1563                 }
1564         }
1565         ;
1566
1567         init_reg_param(&reg_params[0], "a0", 32, PARAM_OUT);
1568         init_reg_param(&reg_params[1], "a1", 32, PARAM_OUT);
1569         init_reg_param(&reg_params[2], "a2", 32, PARAM_OUT);
1570         init_reg_param(&reg_params[3], "a3", 32, PARAM_OUT);
1571         init_reg_param(&reg_params[4], "t0", 32, PARAM_OUT);
1572         init_reg_param(&reg_params[5], "t1", 32, PARAM_IN);
1573         init_reg_param(&reg_params[6], "t4", 32, PARAM_OUT);
1574         init_reg_param(&reg_params[7], "t5", 32, PARAM_OUT);
1575         init_reg_param(&reg_params[8], "t6", 32, PARAM_OUT);
1576         init_reg_param(&reg_params[9], "t7", 32, PARAM_OUT);
1577
1578         while (count > 0) {
1579                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
1580
1581                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
1582                 if (retval != ERROR_OK)
1583                         break;
1584
1585                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
1586                 buf_set_u32(reg_params[1].value, 0, 32, address);
1587                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
1588                 buf_set_u32(reg_params[3].value, 0, 32, cfi_command_val(bank, 0xA0));
1589                 buf_set_u32(reg_params[4].value, 0, 32, cfi_command_val(bank, 0x80));
1590                 buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
1591                 buf_set_u32(reg_params[7].value, 0, 32, 0xaaaaaaaa);
1592                 buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
1593                 buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
1594
1595                 retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
1596                                 cfi_info->write_algorithm->address,
1597                                 cfi_info->write_algorithm->address + ((target_code_size) - 4),
1598                                 10000, &mips32_info);
1599                 if (retval != ERROR_OK)
1600                         break;
1601
1602                 status = buf_get_u32(reg_params[5].value, 0, 32);
1603                 if (status != 0x80) {
1604                         LOG_ERROR("flash write block failed status: 0x%" PRIx32, status);
1605                         retval = ERROR_FLASH_OPERATION_FAILED;
1606                         break;
1607                 }
1608
1609                 buffer += thisrun_count;
1610                 address += thisrun_count;
1611                 count -= thisrun_count;
1612         }
1613
1614         target_free_all_working_areas(target);
1615
1616         destroy_reg_param(&reg_params[0]);
1617         destroy_reg_param(&reg_params[1]);
1618         destroy_reg_param(&reg_params[2]);
1619         destroy_reg_param(&reg_params[3]);
1620         destroy_reg_param(&reg_params[4]);
1621         destroy_reg_param(&reg_params[5]);
1622         destroy_reg_param(&reg_params[6]);
1623         destroy_reg_param(&reg_params[7]);
1624         destroy_reg_param(&reg_params[8]);
1625         destroy_reg_param(&reg_params[9]);
1626
1627         return retval;
1628 }
1629
1630 static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
1631         uint32_t address, uint32_t count)
1632 {
1633         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1634         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
1635         struct target *target = bank->target;
1636         struct reg_param reg_params[10];
1637         void *arm_algo;
1638         struct arm_algorithm armv4_5_algo;
1639         struct armv7m_algorithm armv7m_algo;
1640         struct working_area *source;
1641         uint32_t buffer_size = 32768;
1642         uint32_t status;
1643         int retval = ERROR_OK;
1644
1645         /* input parameters -
1646          *      R0 = source address
1647          *      R1 = destination address
1648          *      R2 = number of writes
1649          *      R3 = flash write command
1650          *      R4 = constant to mask DQ7 bits (also used for Dq5 with shift)
1651          * output parameters -
1652          *      R5 = 0x80 ok 0x00 bad
1653          * temp registers -
1654          *      R6 = value read from flash to test status
1655          *      R7 = holding register
1656          * unlock registers -
1657          *  R8 = unlock1_addr
1658          *  R9 = unlock1_cmd
1659          *  R10 = unlock2_addr
1660          *  R11 = unlock2_cmd */
1661
1662         /* see contib/loaders/flash/armv4_5_cfi_span_32.s for src */
1663         static const uint32_t armv4_5_word_32_code[] = {
1664                 /* 00008100 <sp_32_code>:               */
1665                 0xe4905004,             /* ldr  r5, [r0], #4                    */
1666                 0xe5889000,             /* str  r9, [r8]                                */
1667                 0xe58ab000,             /* str  r11, [r10]                              */
1668                 0xe5883000,             /* str  r3, [r8]                                */
1669                 0xe5815000,             /* str  r5, [r1]                                */
1670                 0xe1a00000,             /* nop                                                  */
1671                 /*
1672                  * 00008110 <sp_32_busy>:               */
1673                 0xe5916000,             /* ldr  r6, [r1]                                */
1674                 0xe0257006,             /* eor  r7, r5, r6                              */
1675                 0xe0147007,             /* ands r7, r4, r7                              */
1676                 0x0a000007,             /* beq  8140 <sp_32_cont> ; b if DQ7 == Data7 */
1677                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1678                 0x0afffff9,             /* beq  8110 <sp_32_busy> ;     b if DQ5 low */
1679                 0xe5916000,             /* ldr  r6, [r1]                                */
1680                 0xe0257006,             /* eor  r7, r5, r6                              */
1681                 0xe0147007,             /* ands r7, r4, r7                              */
1682                 0x0a000001,             /* beq  8140 <sp_32_cont> ; b if DQ7 == Data7 */
1683                 0xe3a05000,             /* mov  r5, #0  ; 0x0 - return 0x00, error */
1684                 0x1a000004,             /* bne  8154 <sp_32_done>               */
1685                 /*
1686                  * 00008140 <sp_32_cont>:               */
1687                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1688                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1689                 0x0a000001,             /* beq  8154 <sp_32_done>               */
1690                 0xe2811004,             /* add  r1, r1, #4      ; 0x4           */
1691                 0xeaffffe8,             /* b    8100 <sp_32_code>               */
1692                 /*
1693                  * 00008154 <sp_32_done>:               */
1694                 0xeafffffe              /* b    8154 <sp_32_done>               */
1695         };
1696
1697         /* see contib/loaders/flash/armv4_5_cfi_span_16.s for src */
1698         static const uint32_t armv4_5_word_16_code[] = {
1699                 /* 00008158 <sp_16_code>:               */
1700                 0xe0d050b2,             /* ldrh r5, [r0], #2                    */
1701                 0xe1c890b0,             /* strh r9, [r8]                                */
1702                 0xe1cab0b0,             /* strh r11, [r10]                              */
1703                 0xe1c830b0,             /* strh r3, [r8]                                */
1704                 0xe1c150b0,             /* strh r5, [r1]                                */
1705                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1706                 /*
1707                  * 00008168 <sp_16_busy>:               */
1708                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1709                 0xe0257006,             /* eor  r7, r5, r6                              */
1710                 0xe0147007,             /* ands r7, r4, r7                              */
1711                 0x0a000007,             /* beq  8198 <sp_16_cont>               */
1712                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1713                 0x0afffff9,             /* beq  8168 <sp_16_busy>               */
1714                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1715                 0xe0257006,             /* eor  r7, r5, r6                              */
1716                 0xe0147007,             /* ands r7, r4, r7                              */
1717                 0x0a000001,             /* beq  8198 <sp_16_cont>               */
1718                 0xe3a05000,             /* mov  r5, #0  ; 0x0                   */
1719                 0x1a000004,             /* bne  81ac <sp_16_done>               */
1720                 /*
1721                  * 00008198 <sp_16_cont>:               */
1722                 0xe2522001,     /* subs r2, r2, #1      ; 0x1           */
1723                 0x03a05080,     /* moveq        r5, #128        ; 0x80  */
1724                 0x0a000001,     /* beq  81ac <sp_16_done>               */
1725                 0xe2811002,     /* add  r1, r1, #2      ; 0x2           */
1726                 0xeaffffe8,     /* b    8158 <sp_16_code>               */
1727                 /*
1728                  * 000081ac <sp_16_done>:               */
1729                 0xeafffffe              /* b    81ac <sp_16_done>               */
1730         };
1731
1732         /* see contib/loaders/flash/armv7m_cfi_span_16.s for src */
1733         static const uint32_t armv7m_word_16_code[] = {
1734                 0x5B02F830,
1735                 0x9000F8A8,
1736                 0xB000F8AA,
1737                 0x3000F8A8,
1738                 0xBF00800D,
1739                 0xEA85880E,
1740                 0x40270706,
1741                 0xEA16D00A,
1742                 0xD0F70694,
1743                 0xEA85880E,
1744                 0x40270706,
1745                 0xF04FD002,
1746                 0xD1070500,
1747                 0xD0023A01,
1748                 0x0102F101,
1749                 0xF04FE7E0,
1750                 0xE7FF0580,
1751                 0x0000BE00
1752         };
1753
1754         /* see contib/loaders/flash/armv4_5_cfi_span_16_dq7.s for src */
1755         static const uint32_t armv4_5_word_16_code_dq7only[] = {
1756                 /* <sp_16_code>:                                */
1757                 0xe0d050b2,             /* ldrh r5, [r0], #2                    */
1758                 0xe1c890b0,             /* strh r9, [r8]                                */
1759                 0xe1cab0b0,             /* strh r11, [r10]                              */
1760                 0xe1c830b0,             /* strh r3, [r8]                                */
1761                 0xe1c150b0,             /* strh r5, [r1]                                */
1762                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1763                 /*
1764                  * <sp_16_busy>:                                */
1765                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1766                 0xe0257006,             /* eor  r7, r5, r6                              */
1767                 0xe2177080,             /* ands r7, #0x80                               */
1768                 0x1afffffb,             /* bne  8168 <sp_16_busy>               */
1769                 /*                                                              */
1770                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1771                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1772                 0x0a000001,             /* beq  81ac <sp_16_done>               */
1773                 0xe2811002,             /* add  r1, r1, #2      ; 0x2           */
1774                 0xeafffff0,             /* b    8158 <sp_16_code>               */
1775                 /*
1776                  * 000081ac <sp_16_done>:               */
1777                 0xeafffffe              /* b    81ac <sp_16_done>               */
1778         };
1779
1780         /* see contib/loaders/flash/armv4_5_cfi_span_8.s for src */
1781         static const uint32_t armv4_5_word_8_code[] = {
1782                 /* 000081b0 <sp_16_code_end>:   */
1783                 0xe4d05001,             /* ldrb r5, [r0], #1                    */
1784                 0xe5c89000,             /* strb r9, [r8]                                */
1785                 0xe5cab000,             /* strb r11, [r10]                              */
1786                 0xe5c83000,             /* strb r3, [r8]                                */
1787                 0xe5c15000,             /* strb r5, [r1]                                */
1788                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1789                 /*
1790                  * 000081c0 <sp_8_busy>:                */
1791                 0xe5d16000,             /* ldrb r6, [r1]                                */
1792                 0xe0257006,             /* eor  r7, r5, r6                              */
1793                 0xe0147007,             /* ands r7, r4, r7                              */
1794                 0x0a000007,             /* beq  81f0 <sp_8_cont>                */
1795                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1796                 0x0afffff9,             /* beq  81c0 <sp_8_busy>                */
1797                 0xe5d16000,             /* ldrb r6, [r1]                                */
1798                 0xe0257006,             /* eor  r7, r5, r6                              */
1799                 0xe0147007,             /* ands r7, r4, r7                              */
1800                 0x0a000001,             /* beq  81f0 <sp_8_cont>                */
1801                 0xe3a05000,             /* mov  r5, #0  ; 0x0                   */
1802                 0x1a000004,             /* bne  8204 <sp_8_done>                */
1803                 /*
1804                  * 000081f0 <sp_8_cont>:                */
1805                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1806                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1807                 0x0a000001,             /* beq  8204 <sp_8_done>                */
1808                 0xe2811001,             /* add  r1, r1, #1      ; 0x1           */
1809                 0xeaffffe8,             /* b    81b0 <sp_16_code_end>   */
1810                 /*
1811                  * 00008204 <sp_8_done>:                */
1812                 0xeafffffe              /* b    8204 <sp_8_done>                */
1813         };
1814
1815         if (strncmp(target_type_name(target), "mips_m4k", 8) == 0)
1816                 return cfi_spansion_write_block_mips(bank, buffer, address, count);
1817
1818         if (is_armv7m(target_to_armv7m(target))) {      /* Cortex-M3 target */
1819                 armv7m_algo.common_magic = ARMV7M_COMMON_MAGIC;
1820                 armv7m_algo.core_mode = ARMV7M_MODE_HANDLER;
1821                 arm_algo = &armv7m_algo;
1822         } else if (is_arm(target_to_arm(target))) {
1823                 /* All other ARM CPUs have 32 bit instructions */
1824                 armv4_5_algo.common_magic = ARM_COMMON_MAGIC;
1825                 armv4_5_algo.core_mode = ARM_MODE_SVC;
1826                 armv4_5_algo.core_state = ARM_STATE_ARM;
1827                 arm_algo = &armv4_5_algo;
1828         } else {
1829                 LOG_ERROR("Unknown architecture");
1830                 return ERROR_FAIL;
1831         }
1832
1833         int target_code_size = 0;
1834         const uint32_t *target_code_src = NULL;
1835
1836         switch (bank->bus_width) {
1837                 case 1:
1838                         if (is_armv7m(target_to_armv7m(target))) {
1839                                 LOG_ERROR("Unknown ARM architecture");
1840                                 return ERROR_FAIL;
1841                         }
1842                         target_code_src = armv4_5_word_8_code;
1843                         target_code_size = sizeof(armv4_5_word_8_code);
1844                         break;
1845                 case 2:
1846                         /* Check for DQ5 support */
1847                         if (cfi_info->status_poll_mask & (1 << 5)) {
1848                                 if (is_armv7m(target_to_armv7m(target))) {      /*
1849                                                                                                  *cortex-m3
1850                                                                                                  *target
1851                                                                                                  **/
1852                                         target_code_src = armv7m_word_16_code;
1853                                         target_code_size = sizeof(armv7m_word_16_code);
1854                                 } else { /* armv4_5 target */
1855                                         target_code_src = armv4_5_word_16_code;
1856                                         target_code_size = sizeof(armv4_5_word_16_code);
1857                                 }
1858                         } else {
1859                                 /* No DQ5 support. Use DQ7 DATA# polling only. */
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_16_code_dq7only;
1865                                 target_code_size = sizeof(armv4_5_word_16_code_dq7only);
1866                         }
1867                         break;
1868                 case 4:
1869                         if (is_armv7m(target_to_armv7m(target))) {
1870                                 LOG_ERROR("Unknown ARM architecture");
1871                                 return ERROR_FAIL;
1872                         }
1873                         target_code_src = armv4_5_word_32_code;
1874                         target_code_size = sizeof(armv4_5_word_32_code);
1875                         break;
1876                 default:
1877                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1878                                         bank->bus_width);
1879                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1880         }
1881
1882         /* flash write code */
1883         if (!cfi_info->write_algorithm) {
1884                 uint8_t *target_code;
1885
1886                 /* convert bus-width dependent algorithm code to correct endiannes */
1887                 target_code = malloc(target_code_size);
1888                 if (target_code == NULL) {
1889                         LOG_ERROR("Out of memory");
1890                         return ERROR_FAIL;
1891                 }
1892                 cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
1893
1894                 /* allocate working area */
1895                 retval = target_alloc_working_area(target, target_code_size,
1896                                 &cfi_info->write_algorithm);
1897                 if (retval != ERROR_OK) {
1898                         free(target_code);
1899                         return retval;
1900                 }
1901
1902                 /* write algorithm code to working area */
1903                 retval = target_write_buffer(target, cfi_info->write_algorithm->address,
1904                                 target_code_size, target_code);
1905                 if (retval != ERROR_OK) {
1906                         free(target_code);
1907                         return retval;
1908                 }
1909
1910                 free(target_code);
1911         }
1912         /* the following code still assumes target code is fixed 24*4 bytes */
1913
1914         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
1915                 buffer_size /= 2;
1916                 if (buffer_size <= 256) {
1917                         /* if we already allocated the writing code, but failed to get a
1918                          * buffer, free the algorithm */
1919                         if (cfi_info->write_algorithm)
1920                                 target_free_working_area(target, cfi_info->write_algorithm);
1921
1922                         LOG_WARNING(
1923                                 "not enough working area available, can't do block memory writes");
1924                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1925                 }
1926         }
1927         ;
1928
1929         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
1930         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
1931         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
1932         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
1933         init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);
1934         init_reg_param(&reg_params[5], "r5", 32, PARAM_IN);
1935         init_reg_param(&reg_params[6], "r8", 32, PARAM_OUT);
1936         init_reg_param(&reg_params[7], "r9", 32, PARAM_OUT);
1937         init_reg_param(&reg_params[8], "r10", 32, PARAM_OUT);
1938         init_reg_param(&reg_params[9], "r11", 32, PARAM_OUT);
1939
1940         while (count > 0) {
1941                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
1942
1943                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
1944                 if (retval != ERROR_OK)
1945                         break;
1946
1947                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
1948                 buf_set_u32(reg_params[1].value, 0, 32, address);
1949                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
1950                 buf_set_u32(reg_params[3].value, 0, 32, cfi_command_val(bank, 0xA0));
1951                 buf_set_u32(reg_params[4].value, 0, 32, cfi_command_val(bank, 0x80));
1952                 buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
1953                 buf_set_u32(reg_params[7].value, 0, 32, 0xaaaaaaaa);
1954                 buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
1955                 buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
1956
1957                 retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
1958                                 cfi_info->write_algorithm->address,
1959                                 cfi_info->write_algorithm->address + ((target_code_size) - 4),
1960                                 10000, arm_algo);
1961                 if (retval != ERROR_OK)
1962                         break;
1963
1964                 status = buf_get_u32(reg_params[5].value, 0, 32);
1965                 if (status != 0x80) {
1966                         LOG_ERROR("flash write block failed status: 0x%" PRIx32, status);
1967                         retval = ERROR_FLASH_OPERATION_FAILED;
1968                         break;
1969                 }
1970
1971                 buffer += thisrun_count;
1972                 address += thisrun_count;
1973                 count -= thisrun_count;
1974         }
1975
1976         target_free_all_working_areas(target);
1977
1978         destroy_reg_param(&reg_params[0]);
1979         destroy_reg_param(&reg_params[1]);
1980         destroy_reg_param(&reg_params[2]);
1981         destroy_reg_param(&reg_params[3]);
1982         destroy_reg_param(&reg_params[4]);
1983         destroy_reg_param(&reg_params[5]);
1984         destroy_reg_param(&reg_params[6]);
1985         destroy_reg_param(&reg_params[7]);
1986         destroy_reg_param(&reg_params[8]);
1987         destroy_reg_param(&reg_params[9]);
1988
1989         return retval;
1990 }
1991
1992 static int cfi_intel_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
1993 {
1994         int retval;
1995         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1996         struct target *target = bank->target;
1997
1998         cfi_intel_clear_status_register(bank);
1999         retval = cfi_send_command(bank, 0x40, address);
2000         if (retval != ERROR_OK)
2001                 return retval;
2002
2003         retval = target_write_memory(target, address, bank->bus_width, 1, word);
2004         if (retval != ERROR_OK)
2005                 return retval;
2006
2007         uint8_t status;
2008         retval = cfi_intel_wait_status_busy(bank, cfi_info->word_write_timeout, &status);
2009         if (retval != 0x80) {
2010                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2011                 if (retval != ERROR_OK)
2012                         return retval;
2013
2014                 LOG_ERROR("couldn't write word at base 0x%" PRIx32 ", address 0x%" PRIx32,
2015                         bank->base, address);
2016                 return ERROR_FLASH_OPERATION_FAILED;
2017         }
2018
2019         return ERROR_OK;
2020 }
2021
2022 static int cfi_intel_write_words(struct flash_bank *bank, uint8_t *word,
2023         uint32_t wordcount, uint32_t address)
2024 {
2025         int retval;
2026         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2027         struct target *target = bank->target;
2028
2029         /* Calculate buffer size and boundary mask
2030          * buffersize is (buffer size per chip) * (number of chips)
2031          * bufferwsize is buffersize in words */
2032         uint32_t buffersize =
2033                 (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2034         uint32_t buffermask = buffersize-1;
2035         uint32_t bufferwsize = buffersize / bank->bus_width;
2036
2037         /* Check for valid range */
2038         if (address & buffermask) {
2039                 LOG_ERROR("Write address at base 0x%" PRIx32 ", address 0x%" PRIx32
2040                         " not aligned to 2^%d boundary",
2041                         bank->base, address, cfi_info->max_buf_write_size);
2042                 return ERROR_FLASH_OPERATION_FAILED;
2043         }
2044
2045         /* Check for valid size */
2046         if (wordcount > bufferwsize) {
2047                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %" PRId32,
2048                         wordcount, buffersize);
2049                 return ERROR_FLASH_OPERATION_FAILED;
2050         }
2051
2052         /* Write to flash buffer */
2053         cfi_intel_clear_status_register(bank);
2054
2055         /* Initiate buffer operation _*/
2056         retval = cfi_send_command(bank, 0xe8, address);
2057         if (retval != ERROR_OK)
2058                 return retval;
2059         uint8_t status;
2060         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
2061         if (retval != ERROR_OK)
2062                 return retval;
2063         if (status != 0x80) {
2064                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2065                 if (retval != ERROR_OK)
2066                         return retval;
2067
2068                 LOG_ERROR(
2069                         "couldn't start buffer write operation at base 0x%" PRIx32 ", address 0x%" PRIx32,
2070                         bank->base,
2071                         address);
2072                 return ERROR_FLASH_OPERATION_FAILED;
2073         }
2074
2075         /* Write buffer wordcount-1 and data words */
2076         retval = cfi_send_command(bank, bufferwsize-1, address);
2077         if (retval != ERROR_OK)
2078                 return retval;
2079
2080         retval = target_write_memory(target, address, bank->bus_width, bufferwsize, word);
2081         if (retval != ERROR_OK)
2082                 return retval;
2083
2084         /* Commit write operation */
2085         retval = cfi_send_command(bank, 0xd0, address);
2086         if (retval != ERROR_OK)
2087                 return retval;
2088
2089         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
2090         if (retval != ERROR_OK)
2091                 return retval;
2092
2093         if (status != 0x80) {
2094                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2095                 if (retval != ERROR_OK)
2096                         return retval;
2097
2098                 LOG_ERROR("Buffer write at base 0x%" PRIx32
2099                         ", address 0x%" PRIx32 " failed.", bank->base, address);
2100                 return ERROR_FLASH_OPERATION_FAILED;
2101         }
2102
2103         return ERROR_OK;
2104 }
2105
2106 static int cfi_spansion_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2107 {
2108         int retval;
2109         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2110         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2111         struct target *target = bank->target;
2112
2113         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
2114         if (retval != ERROR_OK)
2115                 return retval;
2116
2117         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
2118         if (retval != ERROR_OK)
2119                 return retval;
2120
2121         retval = cfi_send_command(bank, 0xa0, flash_address(bank, 0, pri_ext->_unlock1));
2122         if (retval != ERROR_OK)
2123                 return retval;
2124
2125         retval = target_write_memory(target, address, bank->bus_width, 1, word);
2126         if (retval != ERROR_OK)
2127                 return retval;
2128
2129         if (cfi_spansion_wait_status_busy(bank, cfi_info->word_write_timeout) != ERROR_OK) {
2130                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2131                 if (retval != ERROR_OK)
2132                         return retval;
2133
2134                 LOG_ERROR("couldn't write word at base 0x%" PRIx32
2135                         ", address 0x%" PRIx32, bank->base, address);
2136                 return ERROR_FLASH_OPERATION_FAILED;
2137         }
2138
2139         return ERROR_OK;
2140 }
2141
2142 static int cfi_spansion_write_words(struct flash_bank *bank, uint8_t *word,
2143         uint32_t wordcount, uint32_t address)
2144 {
2145         int retval;
2146         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2147         struct target *target = bank->target;
2148         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2149
2150         /* Calculate buffer size and boundary mask
2151          * buffersize is (buffer size per chip) * (number of chips)
2152          * bufferwsize is buffersize in words */
2153         uint32_t buffersize =
2154                 (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2155         uint32_t buffermask = buffersize-1;
2156         uint32_t bufferwsize = buffersize / bank->bus_width;
2157
2158         /* Check for valid range */
2159         if (address & buffermask) {
2160                 LOG_ERROR("Write address at base 0x%" PRIx32
2161                         ", address 0x%" PRIx32 " not aligned to 2^%d boundary",
2162                         bank->base, address, cfi_info->max_buf_write_size);
2163                 return ERROR_FLASH_OPERATION_FAILED;
2164         }
2165
2166         /* Check for valid size */
2167         if (wordcount > bufferwsize) {
2168                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %"
2169                         PRId32, wordcount, buffersize);
2170                 return ERROR_FLASH_OPERATION_FAILED;
2171         }
2172
2173         /* Unlock */
2174         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
2175         if (retval != ERROR_OK)
2176                 return retval;
2177
2178         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
2179         if (retval != ERROR_OK)
2180                 return retval;
2181
2182         /* Buffer load command */
2183         retval = cfi_send_command(bank, 0x25, address);
2184         if (retval != ERROR_OK)
2185                 return retval;
2186
2187         /* Write buffer wordcount-1 and data words */
2188         retval = cfi_send_command(bank, bufferwsize-1, address);
2189         if (retval != ERROR_OK)
2190                 return retval;
2191
2192         retval = target_write_memory(target, address, bank->bus_width, bufferwsize, word);
2193         if (retval != ERROR_OK)
2194                 return retval;
2195
2196         /* Commit write operation */
2197         retval = cfi_send_command(bank, 0x29, address);
2198         if (retval != ERROR_OK)
2199                 return retval;
2200
2201         if (cfi_spansion_wait_status_busy(bank, cfi_info->buf_write_timeout) != ERROR_OK) {
2202                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2203                 if (retval != ERROR_OK)
2204                         return retval;
2205
2206                 LOG_ERROR("couldn't write block at base 0x%" PRIx32
2207                         ", address 0x%" PRIx32 ", size 0x%" PRIx32, bank->base, address,
2208                         bufferwsize);
2209                 return ERROR_FLASH_OPERATION_FAILED;
2210         }
2211
2212         return ERROR_OK;
2213 }
2214
2215 static int cfi_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2216 {
2217         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2218
2219         switch (cfi_info->pri_id) {
2220                 case 1:
2221                 case 3:
2222                         return cfi_intel_write_word(bank, word, address);
2223                         break;
2224                 case 2:
2225                         return cfi_spansion_write_word(bank, word, address);
2226                         break;
2227                 default:
2228                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2229                         break;
2230         }
2231
2232         return ERROR_FLASH_OPERATION_FAILED;
2233 }
2234
2235 static int cfi_write_words(struct flash_bank *bank, uint8_t *word,
2236         uint32_t wordcount, uint32_t address)
2237 {
2238         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2239
2240         if (cfi_info->buf_write_timeout_typ == 0) {
2241                 /* buffer writes are not supported */
2242                 LOG_DEBUG("Buffer Writes Not Supported");
2243                 return ERROR_FLASH_OPER_UNSUPPORTED;
2244         }
2245
2246         switch (cfi_info->pri_id) {
2247                 case 1:
2248                 case 3:
2249                         return cfi_intel_write_words(bank, word, wordcount, address);
2250                         break;
2251                 case 2:
2252                         return cfi_spansion_write_words(bank, word, wordcount, address);
2253                         break;
2254                 default:
2255                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2256                         break;
2257         }
2258
2259         return ERROR_FLASH_OPERATION_FAILED;
2260 }
2261
2262 static int cfi_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
2263 {
2264         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2265         struct target *target = bank->target;
2266         uint32_t address = bank->base + offset;
2267         uint32_t read_p;
2268         int align;      /* number of unaligned bytes */
2269         uint8_t current_word[CFI_MAX_BUS_WIDTH];
2270         int i;
2271         int retval;
2272
2273         LOG_DEBUG("reading buffer of %i byte at 0x%8.8x",
2274                 (int)count, (unsigned)offset);
2275
2276         if (bank->target->state != TARGET_HALTED) {
2277                 LOG_ERROR("Target not halted");
2278                 return ERROR_TARGET_NOT_HALTED;
2279         }
2280
2281         if (offset + count > bank->size)
2282                 return ERROR_FLASH_DST_OUT_OF_BANK;
2283
2284         if (cfi_info->qry[0] != 'Q')
2285                 return ERROR_FLASH_BANK_NOT_PROBED;
2286
2287         /* start at the first byte of the first word (bus_width size) */
2288         read_p = address & ~(bank->bus_width - 1);
2289         align = address - read_p;
2290         if (align != 0) {
2291                 LOG_INFO("Fixup %d unaligned read head bytes", align);
2292
2293                 /* read a complete word from flash */
2294                 retval = target_read_memory(target, read_p, bank->bus_width, 1, current_word);
2295                 if (retval != ERROR_OK)
2296                         return retval;
2297
2298                 /* take only bytes we need */
2299                 for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
2300                         *buffer++ = current_word[i];
2301
2302                 read_p += bank->bus_width;
2303         }
2304
2305         align = count / bank->bus_width;
2306         if (align) {
2307                 retval = target_read_memory(target, read_p, bank->bus_width, align, buffer);
2308                 if (retval != ERROR_OK)
2309                         return retval;
2310
2311                 read_p += align * bank->bus_width;
2312                 buffer += align * bank->bus_width;
2313                 count -= align * bank->bus_width;
2314         }
2315
2316         if (count) {
2317                 LOG_INFO("Fixup %d unaligned read tail bytes", count);
2318
2319                 /* read a complete word from flash */
2320                 retval = target_read_memory(target, read_p, bank->bus_width, 1, current_word);
2321                 if (retval != ERROR_OK)
2322                         return retval;
2323
2324                 /* take only bytes we need */
2325                 for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
2326                         *buffer++ = current_word[i];
2327         }
2328
2329         return ERROR_OK;
2330 }
2331
2332 static int cfi_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
2333 {
2334         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2335         struct target *target = bank->target;
2336         uint32_t address = bank->base + offset; /* address of first byte to be programmed */
2337         uint32_t write_p;
2338         int align;      /* number of unaligned bytes */
2339         int blk_count;  /* number of bus_width bytes for block copy */
2340         uint8_t current_word[CFI_MAX_BUS_WIDTH * 4];    /* word (bus_width size) currently being
2341                                                          *programmed */
2342         int i;
2343         int retval;
2344
2345         if (bank->target->state != TARGET_HALTED) {
2346                 LOG_ERROR("Target not halted");
2347                 return ERROR_TARGET_NOT_HALTED;
2348         }
2349
2350         if (offset + count > bank->size)
2351                 return ERROR_FLASH_DST_OUT_OF_BANK;
2352
2353         if (cfi_info->qry[0] != 'Q')
2354                 return ERROR_FLASH_BANK_NOT_PROBED;
2355
2356         /* start at the first byte of the first word (bus_width size) */
2357         write_p = address & ~(bank->bus_width - 1);
2358         align = address - write_p;
2359         if (align != 0) {
2360                 LOG_INFO("Fixup %d unaligned head bytes", align);
2361
2362                 /* read a complete word from flash */
2363                 retval = target_read_memory(target, write_p, bank->bus_width, 1, current_word);
2364                 if (retval != ERROR_OK)
2365                         return retval;
2366
2367                 /* replace only bytes that must be written */
2368                 for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
2369                         current_word[i] = *buffer++;
2370
2371                 retval = cfi_write_word(bank, current_word, write_p);
2372                 if (retval != ERROR_OK)
2373                         return retval;
2374                 write_p += bank->bus_width;
2375         }
2376
2377         /* handle blocks of bus_size aligned bytes */
2378         blk_count = count & ~(bank->bus_width - 1);     /* round down, leave tail bytes */
2379         switch (cfi_info->pri_id) {
2380                 /* try block writes (fails without working area) */
2381                 case 1:
2382                 case 3:
2383                         retval = cfi_intel_write_block(bank, buffer, write_p, blk_count);
2384                         break;
2385                 case 2:
2386                         retval = cfi_spansion_write_block(bank, buffer, write_p, blk_count);
2387                         break;
2388                 default:
2389                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2390                         retval = ERROR_FLASH_OPERATION_FAILED;
2391                         break;
2392         }
2393         if (retval == ERROR_OK) {
2394                 /* Increment pointers and decrease count on succesful block write */
2395                 buffer += blk_count;
2396                 write_p += blk_count;
2397                 count -= blk_count;
2398         } else {
2399                 if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
2400                         /* Calculate buffer size and boundary mask
2401                          * buffersize is (buffer size per chip) * (number of chips)
2402                          * bufferwsize is buffersize in words */
2403                         uint32_t buffersize =
2404                                 (1UL <<
2405                                  cfi_info->max_buf_write_size) *
2406                                 (bank->bus_width / bank->chip_width);
2407                         uint32_t buffermask = buffersize-1;
2408                         uint32_t bufferwsize = buffersize / bank->bus_width;
2409
2410                         /* fall back to memory writes */
2411                         while (count >= (uint32_t)bank->bus_width) {
2412                                 int fallback;
2413                                 if ((write_p & 0xff) == 0) {
2414                                         LOG_INFO("Programming at 0x%08" PRIx32 ", count 0x%08"
2415                                                 PRIx32 " bytes remaining", write_p, count);
2416                                 }
2417                                 fallback = 1;
2418                                 if ((bufferwsize > 0) && (count >= buffersize) &&
2419                                                 !(write_p & buffermask)) {
2420                                         retval = cfi_write_words(bank, buffer, bufferwsize, write_p);
2421                                         if (retval == ERROR_OK) {
2422                                                 buffer += buffersize;
2423                                                 write_p += buffersize;
2424                                                 count -= buffersize;
2425                                                 fallback = 0;
2426                                         } else if (retval != ERROR_FLASH_OPER_UNSUPPORTED)
2427                                                 return retval;
2428                                 }
2429                                 /* try the slow way? */
2430                                 if (fallback) {
2431                                         for (i = 0; i < bank->bus_width; i++)
2432                                                 current_word[i] = *buffer++;
2433
2434                                         retval = cfi_write_word(bank, current_word, write_p);
2435                                         if (retval != ERROR_OK)
2436                                                 return retval;
2437
2438                                         write_p += bank->bus_width;
2439                                         count -= bank->bus_width;
2440                                 }
2441                         }
2442                 } else
2443                         return retval;
2444         }
2445
2446         /* return to read array mode, so we can read from flash again for padding */
2447         retval = cfi_reset(bank);
2448         if (retval != ERROR_OK)
2449                 return retval;
2450
2451         /* handle unaligned tail bytes */
2452         if (count > 0) {
2453                 LOG_INFO("Fixup %" PRId32 " unaligned tail bytes", count);
2454
2455                 /* read a complete word from flash */
2456                 retval = target_read_memory(target, write_p, bank->bus_width, 1, current_word);
2457                 if (retval != ERROR_OK)
2458                         return retval;
2459
2460                 /* replace only bytes that must be written */
2461                 for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
2462                         current_word[i] = *buffer++;
2463
2464                 retval = cfi_write_word(bank, current_word, write_p);
2465                 if (retval != ERROR_OK)
2466                         return retval;
2467         }
2468
2469         /* return to read array mode */
2470         return cfi_reset(bank);
2471 }
2472
2473 static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, void *param)
2474 {
2475         (void) param;
2476         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2477         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2478
2479         pri_ext->_reversed_geometry = 1;
2480 }
2481
2482 static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, void *param)
2483 {
2484         int i;
2485         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2486         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2487         (void) param;
2488
2489         if ((pri_ext->_reversed_geometry) || (pri_ext->TopBottom == 3)) {
2490                 LOG_DEBUG("swapping reversed erase region information on cmdset 0002 device");
2491
2492                 for (i = 0; i < cfi_info->num_erase_regions / 2; i++) {
2493                         int j = (cfi_info->num_erase_regions - 1) - i;
2494                         uint32_t swap;
2495
2496                         swap = cfi_info->erase_region_info[i];
2497                         cfi_info->erase_region_info[i] = cfi_info->erase_region_info[j];
2498                         cfi_info->erase_region_info[j] = swap;
2499                 }
2500         }
2501 }
2502
2503 static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, void *param)
2504 {
2505         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2506         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2507         struct cfi_unlock_addresses *unlock_addresses = param;
2508
2509         pri_ext->_unlock1 = unlock_addresses->unlock1;
2510         pri_ext->_unlock2 = unlock_addresses->unlock2;
2511 }
2512
2513
2514 static int cfi_query_string(struct flash_bank *bank, int address)
2515 {
2516         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2517         int retval;
2518
2519         retval = cfi_send_command(bank, 0x98, flash_address(bank, 0, address));
2520         if (retval != ERROR_OK)
2521                 return retval;
2522
2523         retval = cfi_query_u8(bank, 0, 0x10, &cfi_info->qry[0]);
2524         if (retval != ERROR_OK)
2525                 return retval;
2526         retval = cfi_query_u8(bank, 0, 0x11, &cfi_info->qry[1]);
2527         if (retval != ERROR_OK)
2528                 return retval;
2529         retval = cfi_query_u8(bank, 0, 0x12, &cfi_info->qry[2]);
2530         if (retval != ERROR_OK)
2531                 return retval;
2532
2533         LOG_DEBUG("CFI qry returned: 0x%2.2x 0x%2.2x 0x%2.2x",
2534                 cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2]);
2535
2536         if ((cfi_info->qry[0] != 'Q') || (cfi_info->qry[1] != 'R') || (cfi_info->qry[2] != 'Y')) {
2537                 retval = cfi_reset(bank);
2538                 if (retval != ERROR_OK)
2539                         return retval;
2540                 LOG_ERROR("Could not probe bank: no QRY");
2541                 return ERROR_FLASH_BANK_INVALID;
2542         }
2543
2544         return ERROR_OK;
2545 }
2546
2547 static int cfi_probe(struct flash_bank *bank)
2548 {
2549         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2550         struct target *target = bank->target;
2551         int num_sectors = 0;
2552         int i;
2553         int sector = 0;
2554         uint32_t unlock1 = 0x555;
2555         uint32_t unlock2 = 0x2aa;
2556         int retval;
2557         uint8_t value_buf0[CFI_MAX_BUS_WIDTH], value_buf1[CFI_MAX_BUS_WIDTH];
2558
2559         if (bank->target->state != TARGET_HALTED) {
2560                 LOG_ERROR("Target not halted");
2561                 return ERROR_TARGET_NOT_HALTED;
2562         }
2563
2564         cfi_info->probed = 0;
2565         cfi_info->num_erase_regions = 0;
2566         if (bank->sectors) {
2567                 free(bank->sectors);
2568                 bank->sectors = NULL;
2569         }
2570         if (cfi_info->erase_region_info) {
2571                 free(cfi_info->erase_region_info);
2572                 cfi_info->erase_region_info = NULL;
2573         }
2574
2575         /* JEDEC standard JESD21C uses 0x5555 and 0x2aaa as unlock addresses,
2576          * while CFI compatible AMD/Spansion flashes use 0x555 and 0x2aa
2577          */
2578         if (cfi_info->jedec_probe) {
2579                 unlock1 = 0x5555;
2580                 unlock2 = 0x2aaa;
2581         }
2582
2583         /* switch to read identifier codes mode ("AUTOSELECT") */
2584         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, unlock1));
2585         if (retval != ERROR_OK)
2586                 return retval;
2587         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, unlock2));
2588         if (retval != ERROR_OK)
2589                 return retval;
2590         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, unlock1));
2591         if (retval != ERROR_OK)
2592                 return retval;
2593
2594         retval = target_read_memory(target, flash_address(bank, 0, 0x00),
2595                         bank->bus_width, 1, value_buf0);
2596         if (retval != ERROR_OK)
2597                 return retval;
2598         retval = target_read_memory(target, flash_address(bank, 0, 0x01),
2599                         bank->bus_width, 1, value_buf1);
2600         if (retval != ERROR_OK)
2601                 return retval;
2602         switch (bank->chip_width) {
2603                 case 1:
2604                         cfi_info->manufacturer = *value_buf0;
2605                         cfi_info->device_id = *value_buf1;
2606                         break;
2607                 case 2:
2608                         cfi_info->manufacturer = target_buffer_get_u16(target, value_buf0);
2609                         cfi_info->device_id = target_buffer_get_u16(target, value_buf1);
2610                         break;
2611                 case 4:
2612                         cfi_info->manufacturer = target_buffer_get_u32(target, value_buf0);
2613                         cfi_info->device_id = target_buffer_get_u32(target, value_buf1);
2614                         break;
2615                 default:
2616                         LOG_ERROR("Unsupported bank chipwidth %d, can't probe memory",
2617                                         bank->chip_width);
2618                         return ERROR_FLASH_OPERATION_FAILED;
2619         }
2620
2621         LOG_INFO("Flash Manufacturer/Device: 0x%04x 0x%04x",
2622                 cfi_info->manufacturer, cfi_info->device_id);
2623         /* switch back to read array mode */
2624         retval = cfi_reset(bank);
2625         if (retval != ERROR_OK)
2626                 return retval;
2627
2628         /* check device/manufacturer ID for known non-CFI flashes. */
2629         cfi_fixup_non_cfi(bank);
2630
2631         /* query only if this is a CFI compatible flash,
2632          * otherwise the relevant info has already been filled in
2633          */
2634         if (cfi_info->not_cfi == 0) {
2635                 /* enter CFI query mode
2636                  * according to JEDEC Standard No. 68.01,
2637                  * a single bus sequence with address = 0x55, data = 0x98 should put
2638                  * the device into CFI query mode.
2639                  *
2640                  * SST flashes clearly violate this, and we will consider them incompatbile for now
2641                  */
2642
2643                 retval = cfi_query_string(bank, 0x55);
2644                 if (retval != ERROR_OK) {
2645                         /*
2646                          * Spansion S29WS-N CFI query fix is to try 0x555 if 0x55 fails. Should
2647                          * be harmless enough:
2648                          *
2649                          * http://www.infradead.org/pipermail/linux-mtd/2005-September/013618.html
2650                          */
2651                         LOG_USER("Try workaround w/0x555 instead of 0x55 to get QRY.");
2652                         retval = cfi_query_string(bank, 0x555);
2653                 }
2654                 if (retval != ERROR_OK)
2655                         return retval;
2656
2657                 retval = cfi_query_u16(bank, 0, 0x13, &cfi_info->pri_id);
2658                 if (retval != ERROR_OK)
2659                         return retval;
2660                 retval = cfi_query_u16(bank, 0, 0x15, &cfi_info->pri_addr);
2661                 if (retval != ERROR_OK)
2662                         return retval;
2663                 retval = cfi_query_u16(bank, 0, 0x17, &cfi_info->alt_id);
2664                 if (retval != ERROR_OK)
2665                         return retval;
2666                 retval = cfi_query_u16(bank, 0, 0x19, &cfi_info->alt_addr);
2667                 if (retval != ERROR_OK)
2668                         return retval;
2669
2670                 LOG_DEBUG("qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: 0x%4.4x, alt_id: "
2671                         "0x%4.4x, alt_addr: 0x%4.4x", cfi_info->qry[0], cfi_info->qry[1],
2672                         cfi_info->qry[2], cfi_info->pri_id, cfi_info->pri_addr,
2673                         cfi_info->alt_id, cfi_info->alt_addr);
2674
2675                 retval = cfi_query_u8(bank, 0, 0x1b, &cfi_info->vcc_min);
2676                 if (retval != ERROR_OK)
2677                         return retval;
2678                 retval = cfi_query_u8(bank, 0, 0x1c, &cfi_info->vcc_max);
2679                 if (retval != ERROR_OK)
2680                         return retval;
2681                 retval = cfi_query_u8(bank, 0, 0x1d, &cfi_info->vpp_min);
2682                 if (retval != ERROR_OK)
2683                         return retval;
2684                 retval = cfi_query_u8(bank, 0, 0x1e, &cfi_info->vpp_max);
2685                 if (retval != ERROR_OK)
2686                         return retval;
2687
2688                 retval = cfi_query_u8(bank, 0, 0x1f, &cfi_info->word_write_timeout_typ);
2689                 if (retval != ERROR_OK)
2690                         return retval;
2691                 retval = cfi_query_u8(bank, 0, 0x20, &cfi_info->buf_write_timeout_typ);
2692                 if (retval != ERROR_OK)
2693                         return retval;
2694                 retval = cfi_query_u8(bank, 0, 0x21, &cfi_info->block_erase_timeout_typ);
2695                 if (retval != ERROR_OK)
2696                         return retval;
2697                 retval = cfi_query_u8(bank, 0, 0x22, &cfi_info->chip_erase_timeout_typ);
2698                 if (retval != ERROR_OK)
2699                         return retval;
2700                 retval = cfi_query_u8(bank, 0, 0x23, &cfi_info->word_write_timeout_max);
2701                 if (retval != ERROR_OK)
2702                         return retval;
2703                 retval = cfi_query_u8(bank, 0, 0x24, &cfi_info->buf_write_timeout_max);
2704                 if (retval != ERROR_OK)
2705                         return retval;
2706                 retval = cfi_query_u8(bank, 0, 0x25, &cfi_info->block_erase_timeout_max);
2707                 if (retval != ERROR_OK)
2708                         return retval;
2709                 retval = cfi_query_u8(bank, 0, 0x26, &cfi_info->chip_erase_timeout_max);
2710                 if (retval != ERROR_OK)
2711                         return retval;
2712
2713                 uint8_t data;
2714                 retval = cfi_query_u8(bank, 0, 0x27, &data);
2715                 if (retval != ERROR_OK)
2716                         return retval;
2717                 cfi_info->dev_size = 1 << data;
2718
2719                 retval = cfi_query_u16(bank, 0, 0x28, &cfi_info->interface_desc);
2720                 if (retval != ERROR_OK)
2721                         return retval;
2722                 retval = cfi_query_u16(bank, 0, 0x2a, &cfi_info->max_buf_write_size);
2723                 if (retval != ERROR_OK)
2724                         return retval;
2725                 retval = cfi_query_u8(bank, 0, 0x2c, &cfi_info->num_erase_regions);
2726                 if (retval != ERROR_OK)
2727                         return retval;
2728
2729                 LOG_DEBUG("size: 0x%" PRIx32 ", interface desc: %i, max buffer write size: 0x%x",
2730                         cfi_info->dev_size, cfi_info->interface_desc,
2731                         (1 << cfi_info->max_buf_write_size));
2732
2733                 if (cfi_info->num_erase_regions) {
2734                         cfi_info->erase_region_info = malloc(sizeof(*cfi_info->erase_region_info)
2735                                         * cfi_info->num_erase_regions);
2736                         for (i = 0; i < cfi_info->num_erase_regions; i++) {
2737                                 retval = cfi_query_u32(bank,
2738                                                 0,
2739                                                 0x2d + (4 * i),
2740                                                 &cfi_info->erase_region_info[i]);
2741                                 if (retval != ERROR_OK)
2742                                         return retval;
2743                                 LOG_DEBUG(
2744                                         "erase region[%i]: %" PRIu32 " blocks of size 0x%" PRIx32 "",
2745                                         i,
2746                                         (cfi_info->erase_region_info[i] & 0xffff) + 1,
2747                                         (cfi_info->erase_region_info[i] >> 16) * 256);
2748                         }
2749                 } else
2750                         cfi_info->erase_region_info = NULL;
2751
2752                 /* We need to read the primary algorithm extended query table before calculating
2753                  * the sector layout to be able to apply fixups
2754                  */
2755                 switch (cfi_info->pri_id) {
2756                         /* Intel command set (standard and extended) */
2757                         case 0x0001:
2758                         case 0x0003:
2759                                 cfi_read_intel_pri_ext(bank);
2760                                 break;
2761                         /* AMD/Spansion, Atmel, ... command set */
2762                         case 0x0002:
2763                                 cfi_info->status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7;  /*
2764                                                                                                  *default
2765                                                                                                  *for
2766                                                                                                  *all
2767                                                                                                  *CFI
2768                                                                                                  *flashs
2769                                                                                                  **/
2770                                 cfi_read_0002_pri_ext(bank);
2771                                 break;
2772                         default:
2773                                 LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2774                                 break;
2775                 }
2776
2777                 /* return to read array mode
2778                  * we use both reset commands, as some Intel flashes fail to recognize the 0xF0 command
2779                  */
2780                 retval = cfi_reset(bank);
2781                 if (retval != ERROR_OK)
2782                         return retval;
2783         }       /* end CFI case */
2784
2785         LOG_DEBUG("Vcc min: %x.%x, Vcc max: %x.%x, Vpp min: %u.%x, Vpp max: %u.%x",
2786                 (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
2787                 (cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
2788                 (cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
2789                 (cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
2790
2791         LOG_DEBUG("typ. word write timeout: %u us, typ. buf write timeout: %u us, "
2792                 "typ. block erase timeout: %u ms, typ. chip erase timeout: %u ms",
2793                 1 << cfi_info->word_write_timeout_typ, 1 << cfi_info->buf_write_timeout_typ,
2794                 1 << cfi_info->block_erase_timeout_typ, 1 << cfi_info->chip_erase_timeout_typ);
2795
2796         LOG_DEBUG("max. word write timeout: %u us, max. buf write timeout: %u us, "
2797                 "max. block erase timeout: %u ms, max. chip erase timeout: %u ms",
2798                 (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
2799                 (1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
2800                 (1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
2801                 (1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
2802
2803         /* convert timeouts to real values in ms */
2804         cfi_info->word_write_timeout = DIV_ROUND_UP((1L << cfi_info->word_write_timeout_typ) *
2805                         (1L << cfi_info->word_write_timeout_max), 1000);
2806         cfi_info->buf_write_timeout = DIV_ROUND_UP((1L << cfi_info->buf_write_timeout_typ) *
2807                         (1L << cfi_info->buf_write_timeout_max), 1000);
2808         cfi_info->block_erase_timeout = (1L << cfi_info->block_erase_timeout_typ) *
2809                 (1L << cfi_info->block_erase_timeout_max);
2810         cfi_info->chip_erase_timeout = (1L << cfi_info->chip_erase_timeout_typ) *
2811                 (1L << cfi_info->chip_erase_timeout_max);
2812
2813         LOG_DEBUG("calculated word write timeout: %u ms, buf write timeout: %u ms, "
2814                 "block erase timeout: %u ms, chip erase timeout: %u ms",
2815                 cfi_info->word_write_timeout, cfi_info->buf_write_timeout,
2816                 cfi_info->block_erase_timeout, cfi_info->chip_erase_timeout);
2817
2818         /* apply fixups depending on the primary command set */
2819         switch (cfi_info->pri_id) {
2820                 /* Intel command set (standard and extended) */
2821                 case 0x0001:
2822                 case 0x0003:
2823                         cfi_fixup(bank, cfi_0001_fixups);
2824                         break;
2825                 /* AMD/Spansion, Atmel, ... command set */
2826                 case 0x0002:
2827                         cfi_fixup(bank, cfi_0002_fixups);
2828                         break;
2829                 default:
2830                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2831                         break;
2832         }
2833
2834         if ((cfi_info->dev_size * bank->bus_width / bank->chip_width) != bank->size) {
2835                 LOG_WARNING("configuration specifies 0x%" PRIx32 " size, but a 0x%" PRIx32
2836                         " size flash was found", bank->size, cfi_info->dev_size);
2837         }
2838
2839         if (cfi_info->num_erase_regions == 0) {
2840                 /* a device might have only one erase block, spanning the whole device */
2841                 bank->num_sectors = 1;
2842                 bank->sectors = malloc(sizeof(struct flash_sector));
2843
2844                 bank->sectors[sector].offset = 0x0;
2845                 bank->sectors[sector].size = bank->size;
2846                 bank->sectors[sector].is_erased = -1;
2847                 bank->sectors[sector].is_protected = -1;
2848         } else {
2849                 uint32_t offset = 0;
2850
2851                 for (i = 0; i < cfi_info->num_erase_regions; i++)
2852                         num_sectors += (cfi_info->erase_region_info[i] & 0xffff) + 1;
2853
2854                 bank->num_sectors = num_sectors;
2855                 bank->sectors = malloc(sizeof(struct flash_sector) * num_sectors);
2856
2857                 for (i = 0; i < cfi_info->num_erase_regions; i++) {
2858                         uint32_t j;
2859                         for (j = 0; j < (cfi_info->erase_region_info[i] & 0xffff) + 1; j++) {
2860                                 bank->sectors[sector].offset = offset;
2861                                 bank->sectors[sector].size =
2862                                         ((cfi_info->erase_region_info[i] >> 16) * 256)
2863                                         * bank->bus_width / bank->chip_width;
2864                                 offset += bank->sectors[sector].size;
2865                                 bank->sectors[sector].is_erased = -1;
2866                                 bank->sectors[sector].is_protected = -1;
2867                                 sector++;
2868                         }
2869                 }
2870                 if (offset != (cfi_info->dev_size * bank->bus_width / bank->chip_width)) {
2871                         LOG_WARNING(
2872                                 "CFI size is 0x%" PRIx32 ", but total sector size is 0x%" PRIx32 "", \
2873                                 (cfi_info->dev_size * bank->bus_width / bank->chip_width),
2874                                 offset);
2875                 }
2876         }
2877
2878         cfi_info->probed = 1;
2879
2880         return ERROR_OK;
2881 }
2882
2883 static int cfi_auto_probe(struct flash_bank *bank)
2884 {
2885         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2886         if (cfi_info->probed)
2887                 return ERROR_OK;
2888         return cfi_probe(bank);
2889 }
2890
2891 static int cfi_intel_protect_check(struct flash_bank *bank)
2892 {
2893         int retval;
2894         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2895         struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
2896         int i;
2897
2898         /* check if block lock bits are supported on this device */
2899         if (!(pri_ext->blk_status_reg_mask & 0x1))
2900                 return ERROR_FLASH_OPERATION_FAILED;
2901
2902         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, 0x55));
2903         if (retval != ERROR_OK)
2904                 return retval;
2905
2906         for (i = 0; i < bank->num_sectors; i++) {
2907                 uint8_t block_status;
2908                 retval = cfi_get_u8(bank, i, 0x2, &block_status);
2909                 if (retval != ERROR_OK)
2910                         return retval;
2911
2912                 if (block_status & 1)
2913                         bank->sectors[i].is_protected = 1;
2914                 else
2915                         bank->sectors[i].is_protected = 0;
2916         }
2917
2918         return cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2919 }
2920
2921 static int cfi_spansion_protect_check(struct flash_bank *bank)
2922 {
2923         int retval;
2924         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2925         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2926         int i;
2927
2928         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
2929         if (retval != ERROR_OK)
2930                 return retval;
2931
2932         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
2933         if (retval != ERROR_OK)
2934                 return retval;
2935
2936         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, pri_ext->_unlock1));
2937         if (retval != ERROR_OK)
2938                 return retval;
2939
2940         for (i = 0; i < bank->num_sectors; i++) {
2941                 uint8_t block_status;
2942                 retval = cfi_get_u8(bank, i, 0x2, &block_status);
2943                 if (retval != ERROR_OK)
2944                         return retval;
2945
2946                 if (block_status & 1)
2947                         bank->sectors[i].is_protected = 1;
2948                 else
2949                         bank->sectors[i].is_protected = 0;
2950         }
2951
2952         return cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2953 }
2954
2955 static int cfi_protect_check(struct flash_bank *bank)
2956 {
2957         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2958
2959         if (bank->target->state != TARGET_HALTED) {
2960                 LOG_ERROR("Target not halted");
2961                 return ERROR_TARGET_NOT_HALTED;
2962         }
2963
2964         if (cfi_info->qry[0] != 'Q')
2965                 return ERROR_FLASH_BANK_NOT_PROBED;
2966
2967         switch (cfi_info->pri_id) {
2968                 case 1:
2969                 case 3:
2970                         return cfi_intel_protect_check(bank);
2971                         break;
2972                 case 2:
2973                         return cfi_spansion_protect_check(bank);
2974                         break;
2975                 default:
2976                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2977                         break;
2978         }
2979
2980         return ERROR_OK;
2981 }
2982
2983 static int get_cfi_info(struct flash_bank *bank, char *buf, int buf_size)
2984 {
2985         int printed;
2986         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2987
2988         if (cfi_info->qry[0] == 0xff) {
2989                 snprintf(buf, buf_size, "\ncfi flash bank not probed yet\n");
2990                 return ERROR_OK;
2991         }
2992
2993         if (cfi_info->not_cfi == 0)
2994                 printed = snprintf(buf, buf_size, "\nCFI flash: ");
2995         else
2996                 printed = snprintf(buf, buf_size, "\nnon-CFI flash: ");
2997         buf += printed;
2998         buf_size -= printed;
2999
3000         printed = snprintf(buf, buf_size, "mfr: 0x%4.4x, id:0x%4.4x\n\n",
3001                         cfi_info->manufacturer, cfi_info->device_id);
3002         buf += printed;
3003         buf_size -= printed;
3004
3005         printed = snprintf(buf, buf_size, "qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: "
3006                         "0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x\n",
3007                         cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2],
3008                         cfi_info->pri_id, cfi_info->pri_addr, cfi_info->alt_id, cfi_info->alt_addr);
3009         buf += printed;
3010         buf_size -= printed;
3011
3012         printed = snprintf(buf, buf_size, "Vcc min: %x.%x, Vcc max: %x.%x, "
3013                         "Vpp min: %u.%x, Vpp max: %u.%x\n",
3014                         (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
3015                         (cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
3016                         (cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
3017                         (cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
3018         buf += printed;
3019         buf_size -= printed;
3020
3021         printed = snprintf(buf, buf_size, "typ. word write timeout: %u us, "
3022                         "typ. buf write timeout: %u us, "
3023                         "typ. block erase timeout: %u ms, "
3024                         "typ. chip erase timeout: %u ms\n",
3025                         1 << cfi_info->word_write_timeout_typ,
3026                         1 << cfi_info->buf_write_timeout_typ,
3027                         1 << cfi_info->block_erase_timeout_typ,
3028                         1 << cfi_info->chip_erase_timeout_typ);
3029         buf += printed;
3030         buf_size -= printed;
3031
3032         printed = snprintf(buf,
3033                         buf_size,
3034                         "max. word write timeout: %u us, "
3035                         "max. buf write timeout: %u us, max. "
3036                         "block erase timeout: %u ms, max. chip erase timeout: %u ms\n",
3037                         (1 <<
3038                          cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
3039                         (1 <<
3040                          cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
3041                         (1 <<
3042                          cfi_info->block_erase_timeout_max) *
3043                         (1 << cfi_info->block_erase_timeout_typ),
3044                         (1 <<
3045                          cfi_info->chip_erase_timeout_max) *
3046                         (1 << cfi_info->chip_erase_timeout_typ));
3047         buf += printed;
3048         buf_size -= printed;
3049
3050         printed = snprintf(buf, buf_size, "size: 0x%" PRIx32 ", interface desc: %i, "
3051                         "max buffer write size: 0x%x\n",
3052                         cfi_info->dev_size,
3053                         cfi_info->interface_desc,
3054                         1 << cfi_info->max_buf_write_size);
3055         buf += printed;
3056         buf_size -= printed;
3057
3058         switch (cfi_info->pri_id) {
3059             case 1:
3060             case 3:
3061                     cfi_intel_info(bank, buf, buf_size);
3062                     break;
3063             case 2:
3064                     cfi_spansion_info(bank, buf, buf_size);
3065                     break;
3066             default:
3067                     LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
3068                     break;
3069         }
3070
3071         return ERROR_OK;
3072 }
3073
3074 static void cfi_fixup_0002_write_buffer(struct flash_bank *bank, void *param)
3075 {
3076         struct cfi_flash_bank *cfi_info = bank->driver_priv;
3077
3078         /* disable write buffer for M29W128G */
3079         cfi_info->buf_write_timeout_typ = 0;
3080 }
3081
3082 struct flash_driver cfi_flash = {
3083         .name = "cfi",
3084         .flash_bank_command = cfi_flash_bank_command,
3085         .erase = cfi_erase,
3086         .protect = cfi_protect,
3087         .write = cfi_write,
3088         .read = cfi_read,
3089         .probe = cfi_probe,
3090         .auto_probe = cfi_auto_probe,
3091         /* FIXME: access flash at bus_width size */
3092         .erase_check = default_flash_blank_check,
3093         .protect_check = cfi_protect_check,
3094         .info = get_cfi_info,
3095 };