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         struct arm_algorithm arm_algo;
1638         struct working_area *source;
1639         uint32_t buffer_size = 32768;
1640         uint32_t status;
1641         int retval = ERROR_OK;
1642
1643         /* input parameters -
1644          *      R0 = source address
1645          *      R1 = destination address
1646          *      R2 = number of writes
1647          *      R3 = flash write command
1648          *      R4 = constant to mask DQ7 bits (also used for Dq5 with shift)
1649          * output parameters -
1650          *      R5 = 0x80 ok 0x00 bad
1651          * temp registers -
1652          *      R6 = value read from flash to test status
1653          *      R7 = holding register
1654          * unlock registers -
1655          *  R8 = unlock1_addr
1656          *  R9 = unlock1_cmd
1657          *  R10 = unlock2_addr
1658          *  R11 = unlock2_cmd */
1659
1660         /* see contib/loaders/flash/armv4_5_cfi_span_32.s for src */
1661         static const uint32_t armv4_5_word_32_code[] = {
1662                 /* 00008100 <sp_32_code>:               */
1663                 0xe4905004,             /* ldr  r5, [r0], #4                    */
1664                 0xe5889000,             /* str  r9, [r8]                                */
1665                 0xe58ab000,             /* str  r11, [r10]                              */
1666                 0xe5883000,             /* str  r3, [r8]                                */
1667                 0xe5815000,             /* str  r5, [r1]                                */
1668                 0xe1a00000,             /* nop                                                  */
1669                 /*
1670                  * 00008110 <sp_32_busy>:               */
1671                 0xe5916000,             /* ldr  r6, [r1]                                */
1672                 0xe0257006,             /* eor  r7, r5, r6                              */
1673                 0xe0147007,             /* ands r7, r4, r7                              */
1674                 0x0a000007,             /* beq  8140 <sp_32_cont> ; b if DQ7 == Data7 */
1675                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1676                 0x0afffff9,             /* beq  8110 <sp_32_busy> ;     b if DQ5 low */
1677                 0xe5916000,             /* ldr  r6, [r1]                                */
1678                 0xe0257006,             /* eor  r7, r5, r6                              */
1679                 0xe0147007,             /* ands r7, r4, r7                              */
1680                 0x0a000001,             /* beq  8140 <sp_32_cont> ; b if DQ7 == Data7 */
1681                 0xe3a05000,             /* mov  r5, #0  ; 0x0 - return 0x00, error */
1682                 0x1a000004,             /* bne  8154 <sp_32_done>               */
1683                 /*
1684                  * 00008140 <sp_32_cont>:               */
1685                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1686                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1687                 0x0a000001,             /* beq  8154 <sp_32_done>               */
1688                 0xe2811004,             /* add  r1, r1, #4      ; 0x4           */
1689                 0xeaffffe8,             /* b    8100 <sp_32_code>               */
1690                 /*
1691                  * 00008154 <sp_32_done>:               */
1692                 0xeafffffe              /* b    8154 <sp_32_done>               */
1693         };
1694
1695         /* see contib/loaders/flash/armv4_5_cfi_span_16.s for src */
1696         static const uint32_t armv4_5_word_16_code[] = {
1697                 /* 00008158 <sp_16_code>:               */
1698                 0xe0d050b2,             /* ldrh r5, [r0], #2                    */
1699                 0xe1c890b0,             /* strh r9, [r8]                                */
1700                 0xe1cab0b0,             /* strh r11, [r10]                              */
1701                 0xe1c830b0,             /* strh r3, [r8]                                */
1702                 0xe1c150b0,             /* strh r5, [r1]                                */
1703                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1704                 /*
1705                  * 00008168 <sp_16_busy>:               */
1706                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1707                 0xe0257006,             /* eor  r7, r5, r6                              */
1708                 0xe0147007,             /* ands r7, r4, r7                              */
1709                 0x0a000007,             /* beq  8198 <sp_16_cont>               */
1710                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1711                 0x0afffff9,             /* beq  8168 <sp_16_busy>               */
1712                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1713                 0xe0257006,             /* eor  r7, r5, r6                              */
1714                 0xe0147007,             /* ands r7, r4, r7                              */
1715                 0x0a000001,             /* beq  8198 <sp_16_cont>               */
1716                 0xe3a05000,             /* mov  r5, #0  ; 0x0                   */
1717                 0x1a000004,             /* bne  81ac <sp_16_done>               */
1718                 /*
1719                  * 00008198 <sp_16_cont>:               */
1720                 0xe2522001,     /* subs r2, r2, #1      ; 0x1           */
1721                 0x03a05080,     /* moveq        r5, #128        ; 0x80  */
1722                 0x0a000001,     /* beq  81ac <sp_16_done>               */
1723                 0xe2811002,     /* add  r1, r1, #2      ; 0x2           */
1724                 0xeaffffe8,     /* b    8158 <sp_16_code>               */
1725                 /*
1726                  * 000081ac <sp_16_done>:               */
1727                 0xeafffffe              /* b    81ac <sp_16_done>               */
1728         };
1729
1730         /* see contib/loaders/flash/armv7m_cfi_span_16.s for src */
1731         static const uint32_t armv7m_word_16_code[] = {
1732                 0x5B02F830,
1733                 0x9000F8A8,
1734                 0xB000F8AA,
1735                 0x3000F8A8,
1736                 0xBF00800D,
1737                 0xEA85880E,
1738                 0x40270706,
1739                 0xEA16D00A,
1740                 0xD0F70694,
1741                 0xEA85880E,
1742                 0x40270706,
1743                 0xF04FD002,
1744                 0xD1070500,
1745                 0xD0023A01,
1746                 0x0102F101,
1747                 0xF04FE7E0,
1748                 0xE7FF0580,
1749                 0x0000BE00
1750         };
1751
1752         /* see contib/loaders/flash/armv4_5_cfi_span_16_dq7.s for src */
1753         static const uint32_t armv4_5_word_16_code_dq7only[] = {
1754                 /* <sp_16_code>:                                */
1755                 0xe0d050b2,             /* ldrh r5, [r0], #2                    */
1756                 0xe1c890b0,             /* strh r9, [r8]                                */
1757                 0xe1cab0b0,             /* strh r11, [r10]                              */
1758                 0xe1c830b0,             /* strh r3, [r8]                                */
1759                 0xe1c150b0,             /* strh r5, [r1]                                */
1760                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1761                 /*
1762                  * <sp_16_busy>:                                */
1763                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1764                 0xe0257006,             /* eor  r7, r5, r6                              */
1765                 0xe2177080,             /* ands r7, #0x80                               */
1766                 0x1afffffb,             /* bne  8168 <sp_16_busy>               */
1767                 /*                                                              */
1768                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1769                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1770                 0x0a000001,             /* beq  81ac <sp_16_done>               */
1771                 0xe2811002,             /* add  r1, r1, #2      ; 0x2           */
1772                 0xeafffff0,             /* b    8158 <sp_16_code>               */
1773                 /*
1774                  * 000081ac <sp_16_done>:               */
1775                 0xeafffffe              /* b    81ac <sp_16_done>               */
1776         };
1777
1778         /* see contib/loaders/flash/armv4_5_cfi_span_8.s for src */
1779         static const uint32_t armv4_5_word_8_code[] = {
1780                 /* 000081b0 <sp_16_code_end>:   */
1781                 0xe4d05001,             /* ldrb r5, [r0], #1                    */
1782                 0xe5c89000,             /* strb r9, [r8]                                */
1783                 0xe5cab000,             /* strb r11, [r10]                              */
1784                 0xe5c83000,             /* strb r3, [r8]                                */
1785                 0xe5c15000,             /* strb r5, [r1]                                */
1786                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1787                 /*
1788                  * 000081c0 <sp_8_busy>:                */
1789                 0xe5d16000,             /* ldrb r6, [r1]                                */
1790                 0xe0257006,             /* eor  r7, r5, r6                              */
1791                 0xe0147007,             /* ands r7, r4, r7                              */
1792                 0x0a000007,             /* beq  81f0 <sp_8_cont>                */
1793                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1794                 0x0afffff9,             /* beq  81c0 <sp_8_busy>                */
1795                 0xe5d16000,             /* ldrb r6, [r1]                                */
1796                 0xe0257006,             /* eor  r7, r5, r6                              */
1797                 0xe0147007,             /* ands r7, r4, r7                              */
1798                 0x0a000001,             /* beq  81f0 <sp_8_cont>                */
1799                 0xe3a05000,             /* mov  r5, #0  ; 0x0                   */
1800                 0x1a000004,             /* bne  8204 <sp_8_done>                */
1801                 /*
1802                  * 000081f0 <sp_8_cont>:                */
1803                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1804                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1805                 0x0a000001,             /* beq  8204 <sp_8_done>                */
1806                 0xe2811001,             /* add  r1, r1, #1      ; 0x1           */
1807                 0xeaffffe8,             /* b    81b0 <sp_16_code_end>   */
1808                 /*
1809                  * 00008204 <sp_8_done>:                */
1810                 0xeafffffe              /* b    8204 <sp_8_done>                */
1811         };
1812
1813         if (strncmp(target_type_name(target), "mips_m4k", 8) == 0)
1814                 return cfi_spansion_write_block_mips(bank, buffer, address, count);
1815
1816         if (is_armv7m(target_to_armv7m(target))) {      /* Cortex-M3 target */
1817                 arm_algo.common_magic = ARMV7M_COMMON_MAGIC;
1818                 arm_algo.core_mode = ARMV7M_MODE_HANDLER;
1819                 arm_algo.core_state = ARM_STATE_ARM;
1820         } else if (is_arm7_9(target_to_arm7_9(target))) {
1821                 /* All other ARM CPUs have 32 bit instructions */
1822                 arm_algo.common_magic = ARM_COMMON_MAGIC;
1823                 arm_algo.core_mode = ARM_MODE_SVC;
1824                 arm_algo.core_state = ARM_STATE_ARM;
1825         } else {
1826                 LOG_ERROR("Unknown architecture");
1827                 return ERROR_FAIL;
1828         }
1829
1830         int target_code_size = 0;
1831         const uint32_t *target_code_src = NULL;
1832
1833         switch (bank->bus_width) {
1834                 case 1:
1835                         if (arm_algo.common_magic != ARM_COMMON_MAGIC) {
1836                                 LOG_ERROR("Unknown ARM architecture");
1837                                 return ERROR_FAIL;
1838                         }
1839                         target_code_src = armv4_5_word_8_code;
1840                         target_code_size = sizeof(armv4_5_word_8_code);
1841                         break;
1842                 case 2:
1843                         /* Check for DQ5 support */
1844                         if (cfi_info->status_poll_mask & (1 << 5)) {
1845                                 if (arm_algo.common_magic == ARM_COMMON_MAGIC) {/* armv4_5 target */
1846                                         target_code_src = armv4_5_word_16_code;
1847                                         target_code_size = sizeof(armv4_5_word_16_code);
1848                                 } else if (arm_algo.common_magic == ARMV7M_COMMON_MAGIC) {      /*
1849                                                                                                  *cortex-m3
1850                                                                                                  *target
1851                                                                                                  **/
1852                                         target_code_src = armv7m_word_16_code;
1853                                         target_code_size = sizeof(armv7m_word_16_code);
1854                                 }
1855                         } else {
1856                                 /* No DQ5 support. Use DQ7 DATA# polling only. */
1857                                 if (arm_algo.common_magic != ARM_COMMON_MAGIC) {
1858                                         LOG_ERROR("Unknown ARM architecture");
1859                                         return ERROR_FAIL;
1860                                 }
1861                                 target_code_src = armv4_5_word_16_code_dq7only;
1862                                 target_code_size = sizeof(armv4_5_word_16_code_dq7only);
1863                         }
1864                         break;
1865                 case 4:
1866                         if (arm_algo.common_magic != ARM_COMMON_MAGIC) {
1867                                 LOG_ERROR("Unknown ARM architecture");
1868                                 return ERROR_FAIL;
1869                         }
1870                         target_code_src = armv4_5_word_32_code;
1871                         target_code_size = sizeof(armv4_5_word_32_code);
1872                         break;
1873                 default:
1874                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1875                                         bank->bus_width);
1876                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1877         }
1878
1879         /* flash write code */
1880         if (!cfi_info->write_algorithm) {
1881                 uint8_t *target_code;
1882
1883                 /* convert bus-width dependent algorithm code to correct endiannes */
1884                 target_code = malloc(target_code_size);
1885                 if (target_code == NULL) {
1886                         LOG_ERROR("Out of memory");
1887                         return ERROR_FAIL;
1888                 }
1889                 cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
1890
1891                 /* allocate working area */
1892                 retval = target_alloc_working_area(target, target_code_size,
1893                                 &cfi_info->write_algorithm);
1894                 if (retval != ERROR_OK) {
1895                         free(target_code);
1896                         return retval;
1897                 }
1898
1899                 /* write algorithm code to working area */
1900                 retval = target_write_buffer(target, cfi_info->write_algorithm->address,
1901                                 target_code_size, target_code);
1902                 if (retval != ERROR_OK) {
1903                         free(target_code);
1904                         return retval;
1905                 }
1906
1907                 free(target_code);
1908         }
1909         /* the following code still assumes target code is fixed 24*4 bytes */
1910
1911         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
1912                 buffer_size /= 2;
1913                 if (buffer_size <= 256) {
1914                         /* if we already allocated the writing code, but failed to get a
1915                          * buffer, free the algorithm */
1916                         if (cfi_info->write_algorithm)
1917                                 target_free_working_area(target, cfi_info->write_algorithm);
1918
1919                         LOG_WARNING(
1920                                 "not enough working area available, can't do block memory writes");
1921                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1922                 }
1923         }
1924         ;
1925
1926         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
1927         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
1928         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
1929         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
1930         init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);
1931         init_reg_param(&reg_params[5], "r5", 32, PARAM_IN);
1932         init_reg_param(&reg_params[6], "r8", 32, PARAM_OUT);
1933         init_reg_param(&reg_params[7], "r9", 32, PARAM_OUT);
1934         init_reg_param(&reg_params[8], "r10", 32, PARAM_OUT);
1935         init_reg_param(&reg_params[9], "r11", 32, PARAM_OUT);
1936
1937         while (count > 0) {
1938                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
1939
1940                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
1941                 if (retval != ERROR_OK)
1942                         break;
1943
1944                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
1945                 buf_set_u32(reg_params[1].value, 0, 32, address);
1946                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
1947                 buf_set_u32(reg_params[3].value, 0, 32, cfi_command_val(bank, 0xA0));
1948                 buf_set_u32(reg_params[4].value, 0, 32, cfi_command_val(bank, 0x80));
1949                 buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
1950                 buf_set_u32(reg_params[7].value, 0, 32, 0xaaaaaaaa);
1951                 buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
1952                 buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
1953
1954                 retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
1955                                 cfi_info->write_algorithm->address,
1956                                 cfi_info->write_algorithm->address + ((target_code_size) - 4),
1957                                 10000, &arm_algo);
1958                 if (retval != ERROR_OK)
1959                         break;
1960
1961                 status = buf_get_u32(reg_params[5].value, 0, 32);
1962                 if (status != 0x80) {
1963                         LOG_ERROR("flash write block failed status: 0x%" PRIx32, status);
1964                         retval = ERROR_FLASH_OPERATION_FAILED;
1965                         break;
1966                 }
1967
1968                 buffer += thisrun_count;
1969                 address += thisrun_count;
1970                 count -= thisrun_count;
1971         }
1972
1973         target_free_all_working_areas(target);
1974
1975         destroy_reg_param(&reg_params[0]);
1976         destroy_reg_param(&reg_params[1]);
1977         destroy_reg_param(&reg_params[2]);
1978         destroy_reg_param(&reg_params[3]);
1979         destroy_reg_param(&reg_params[4]);
1980         destroy_reg_param(&reg_params[5]);
1981         destroy_reg_param(&reg_params[6]);
1982         destroy_reg_param(&reg_params[7]);
1983         destroy_reg_param(&reg_params[8]);
1984         destroy_reg_param(&reg_params[9]);
1985
1986         return retval;
1987 }
1988
1989 static int cfi_intel_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
1990 {
1991         int retval;
1992         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1993         struct target *target = bank->target;
1994
1995         cfi_intel_clear_status_register(bank);
1996         retval = cfi_send_command(bank, 0x40, address);
1997         if (retval != ERROR_OK)
1998                 return retval;
1999
2000         retval = target_write_memory(target, address, bank->bus_width, 1, word);
2001         if (retval != ERROR_OK)
2002                 return retval;
2003
2004         uint8_t status;
2005         retval = cfi_intel_wait_status_busy(bank, cfi_info->word_write_timeout, &status);
2006         if (retval != 0x80) {
2007                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2008                 if (retval != ERROR_OK)
2009                         return retval;
2010
2011                 LOG_ERROR("couldn't write word at base 0x%" PRIx32 ", address 0x%" PRIx32,
2012                         bank->base, address);
2013                 return ERROR_FLASH_OPERATION_FAILED;
2014         }
2015
2016         return ERROR_OK;
2017 }
2018
2019 static int cfi_intel_write_words(struct flash_bank *bank, uint8_t *word,
2020         uint32_t wordcount, uint32_t address)
2021 {
2022         int retval;
2023         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2024         struct target *target = bank->target;
2025
2026         /* Calculate buffer size and boundary mask
2027          * buffersize is (buffer size per chip) * (number of chips)
2028          * bufferwsize is buffersize in words */
2029         uint32_t buffersize =
2030                 (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2031         uint32_t buffermask = buffersize-1;
2032         uint32_t bufferwsize = buffersize / bank->bus_width;
2033
2034         /* Check for valid range */
2035         if (address & buffermask) {
2036                 LOG_ERROR("Write address at base 0x%" PRIx32 ", address 0x%" PRIx32
2037                         " not aligned to 2^%d boundary",
2038                         bank->base, address, cfi_info->max_buf_write_size);
2039                 return ERROR_FLASH_OPERATION_FAILED;
2040         }
2041
2042         /* Check for valid size */
2043         if (wordcount > bufferwsize) {
2044                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %" PRId32,
2045                         wordcount, buffersize);
2046                 return ERROR_FLASH_OPERATION_FAILED;
2047         }
2048
2049         /* Write to flash buffer */
2050         cfi_intel_clear_status_register(bank);
2051
2052         /* Initiate buffer operation _*/
2053         retval = cfi_send_command(bank, 0xe8, address);
2054         if (retval != ERROR_OK)
2055                 return retval;
2056         uint8_t status;
2057         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
2058         if (retval != ERROR_OK)
2059                 return retval;
2060         if (status != 0x80) {
2061                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2062                 if (retval != ERROR_OK)
2063                         return retval;
2064
2065                 LOG_ERROR(
2066                         "couldn't start buffer write operation at base 0x%" PRIx32 ", address 0x%" PRIx32,
2067                         bank->base,
2068                         address);
2069                 return ERROR_FLASH_OPERATION_FAILED;
2070         }
2071
2072         /* Write buffer wordcount-1 and data words */
2073         retval = cfi_send_command(bank, bufferwsize-1, address);
2074         if (retval != ERROR_OK)
2075                 return retval;
2076
2077         retval = target_write_memory(target, address, bank->bus_width, bufferwsize, word);
2078         if (retval != ERROR_OK)
2079                 return retval;
2080
2081         /* Commit write operation */
2082         retval = cfi_send_command(bank, 0xd0, address);
2083         if (retval != ERROR_OK)
2084                 return retval;
2085
2086         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
2087         if (retval != ERROR_OK)
2088                 return retval;
2089
2090         if (status != 0x80) {
2091                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2092                 if (retval != ERROR_OK)
2093                         return retval;
2094
2095                 LOG_ERROR("Buffer write at base 0x%" PRIx32
2096                         ", address 0x%" PRIx32 " failed.", bank->base, address);
2097                 return ERROR_FLASH_OPERATION_FAILED;
2098         }
2099
2100         return ERROR_OK;
2101 }
2102
2103 static int cfi_spansion_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2104 {
2105         int retval;
2106         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2107         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2108         struct target *target = bank->target;
2109
2110         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
2111         if (retval != ERROR_OK)
2112                 return retval;
2113
2114         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
2115         if (retval != ERROR_OK)
2116                 return retval;
2117
2118         retval = cfi_send_command(bank, 0xa0, flash_address(bank, 0, pri_ext->_unlock1));
2119         if (retval != ERROR_OK)
2120                 return retval;
2121
2122         retval = target_write_memory(target, address, bank->bus_width, 1, word);
2123         if (retval != ERROR_OK)
2124                 return retval;
2125
2126         if (cfi_spansion_wait_status_busy(bank, cfi_info->word_write_timeout) != ERROR_OK) {
2127                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2128                 if (retval != ERROR_OK)
2129                         return retval;
2130
2131                 LOG_ERROR("couldn't write word at base 0x%" PRIx32
2132                         ", address 0x%" PRIx32, bank->base, address);
2133                 return ERROR_FLASH_OPERATION_FAILED;
2134         }
2135
2136         return ERROR_OK;
2137 }
2138
2139 static int cfi_spansion_write_words(struct flash_bank *bank, uint8_t *word,
2140         uint32_t wordcount, uint32_t address)
2141 {
2142         int retval;
2143         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2144         struct target *target = bank->target;
2145         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2146
2147         /* Calculate buffer size and boundary mask
2148          * buffersize is (buffer size per chip) * (number of chips)
2149          * bufferwsize is buffersize in words */
2150         uint32_t buffersize =
2151                 (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2152         uint32_t buffermask = buffersize-1;
2153         uint32_t bufferwsize = buffersize / bank->bus_width;
2154
2155         /* Check for valid range */
2156         if (address & buffermask) {
2157                 LOG_ERROR("Write address at base 0x%" PRIx32
2158                         ", address 0x%" PRIx32 " not aligned to 2^%d boundary",
2159                         bank->base, address, cfi_info->max_buf_write_size);
2160                 return ERROR_FLASH_OPERATION_FAILED;
2161         }
2162
2163         /* Check for valid size */
2164         if (wordcount > bufferwsize) {
2165                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %"
2166                         PRId32, wordcount, buffersize);
2167                 return ERROR_FLASH_OPERATION_FAILED;
2168         }
2169
2170         /* Unlock */
2171         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
2172         if (retval != ERROR_OK)
2173                 return retval;
2174
2175         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
2176         if (retval != ERROR_OK)
2177                 return retval;
2178
2179         /* Buffer load command */
2180         retval = cfi_send_command(bank, 0x25, address);
2181         if (retval != ERROR_OK)
2182                 return retval;
2183
2184         /* Write buffer wordcount-1 and data words */
2185         retval = cfi_send_command(bank, bufferwsize-1, address);
2186         if (retval != ERROR_OK)
2187                 return retval;
2188
2189         retval = target_write_memory(target, address, bank->bus_width, bufferwsize, word);
2190         if (retval != ERROR_OK)
2191                 return retval;
2192
2193         /* Commit write operation */
2194         retval = cfi_send_command(bank, 0x29, address);
2195         if (retval != ERROR_OK)
2196                 return retval;
2197
2198         if (cfi_spansion_wait_status_busy(bank, cfi_info->buf_write_timeout) != ERROR_OK) {
2199                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2200                 if (retval != ERROR_OK)
2201                         return retval;
2202
2203                 LOG_ERROR("couldn't write block at base 0x%" PRIx32
2204                         ", address 0x%" PRIx32 ", size 0x%" PRIx32, bank->base, address,
2205                         bufferwsize);
2206                 return ERROR_FLASH_OPERATION_FAILED;
2207         }
2208
2209         return ERROR_OK;
2210 }
2211
2212 static int cfi_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2213 {
2214         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2215
2216         switch (cfi_info->pri_id) {
2217                 case 1:
2218                 case 3:
2219                         return cfi_intel_write_word(bank, word, address);
2220                         break;
2221                 case 2:
2222                         return cfi_spansion_write_word(bank, word, address);
2223                         break;
2224                 default:
2225                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2226                         break;
2227         }
2228
2229         return ERROR_FLASH_OPERATION_FAILED;
2230 }
2231
2232 static int cfi_write_words(struct flash_bank *bank, uint8_t *word,
2233         uint32_t wordcount, uint32_t address)
2234 {
2235         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2236
2237         if (cfi_info->buf_write_timeout_typ == 0) {
2238                 /* buffer writes are not supported */
2239                 LOG_DEBUG("Buffer Writes Not Supported");
2240                 return ERROR_FLASH_OPER_UNSUPPORTED;
2241         }
2242
2243         switch (cfi_info->pri_id) {
2244                 case 1:
2245                 case 3:
2246                         return cfi_intel_write_words(bank, word, wordcount, address);
2247                         break;
2248                 case 2:
2249                         return cfi_spansion_write_words(bank, word, wordcount, address);
2250                         break;
2251                 default:
2252                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2253                         break;
2254         }
2255
2256         return ERROR_FLASH_OPERATION_FAILED;
2257 }
2258
2259 static int cfi_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
2260 {
2261         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2262         struct target *target = bank->target;
2263         uint32_t address = bank->base + offset;
2264         uint32_t read_p;
2265         int align;      /* number of unaligned bytes */
2266         uint8_t current_word[CFI_MAX_BUS_WIDTH];
2267         int i;
2268         int retval;
2269
2270         LOG_DEBUG("reading buffer of %i byte at 0x%8.8x",
2271                 (int)count, (unsigned)offset);
2272
2273         if (bank->target->state != TARGET_HALTED) {
2274                 LOG_ERROR("Target not halted");
2275                 return ERROR_TARGET_NOT_HALTED;
2276         }
2277
2278         if (offset + count > bank->size)
2279                 return ERROR_FLASH_DST_OUT_OF_BANK;
2280
2281         if (cfi_info->qry[0] != 'Q')
2282                 return ERROR_FLASH_BANK_NOT_PROBED;
2283
2284         /* start at the first byte of the first word (bus_width size) */
2285         read_p = address & ~(bank->bus_width - 1);
2286         align = address - read_p;
2287         if (align != 0) {
2288                 LOG_INFO("Fixup %d unaligned read head bytes", align);
2289
2290                 /* read a complete word from flash */
2291                 retval = target_read_memory(target, read_p, bank->bus_width, 1, current_word);
2292                 if (retval != ERROR_OK)
2293                         return retval;
2294
2295                 /* take only bytes we need */
2296                 for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
2297                         *buffer++ = current_word[i];
2298
2299                 read_p += bank->bus_width;
2300         }
2301
2302         align = count / bank->bus_width;
2303         if (align) {
2304                 retval = target_read_memory(target, read_p, bank->bus_width, align, buffer);
2305                 if (retval != ERROR_OK)
2306                         return retval;
2307
2308                 read_p += align * bank->bus_width;
2309                 buffer += align * bank->bus_width;
2310                 count -= align * bank->bus_width;
2311         }
2312
2313         if (count) {
2314                 LOG_INFO("Fixup %d unaligned read tail bytes", count);
2315
2316                 /* read a complete word from flash */
2317                 retval = target_read_memory(target, read_p, bank->bus_width, 1, current_word);
2318                 if (retval != ERROR_OK)
2319                         return retval;
2320
2321                 /* take only bytes we need */
2322                 for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
2323                         *buffer++ = current_word[i];
2324         }
2325
2326         return ERROR_OK;
2327 }
2328
2329 static int cfi_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
2330 {
2331         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2332         struct target *target = bank->target;
2333         uint32_t address = bank->base + offset; /* address of first byte to be programmed */
2334         uint32_t write_p;
2335         int align;      /* number of unaligned bytes */
2336         int blk_count;  /* number of bus_width bytes for block copy */
2337         uint8_t current_word[CFI_MAX_BUS_WIDTH * 4];    /* word (bus_width size) currently being
2338                                                          *programmed */
2339         int i;
2340         int retval;
2341
2342         if (bank->target->state != TARGET_HALTED) {
2343                 LOG_ERROR("Target not halted");
2344                 return ERROR_TARGET_NOT_HALTED;
2345         }
2346
2347         if (offset + count > bank->size)
2348                 return ERROR_FLASH_DST_OUT_OF_BANK;
2349
2350         if (cfi_info->qry[0] != 'Q')
2351                 return ERROR_FLASH_BANK_NOT_PROBED;
2352
2353         /* start at the first byte of the first word (bus_width size) */
2354         write_p = address & ~(bank->bus_width - 1);
2355         align = address - write_p;
2356         if (align != 0) {
2357                 LOG_INFO("Fixup %d unaligned head bytes", align);
2358
2359                 /* read a complete word from flash */
2360                 retval = target_read_memory(target, write_p, bank->bus_width, 1, current_word);
2361                 if (retval != ERROR_OK)
2362                         return retval;
2363
2364                 /* replace only bytes that must be written */
2365                 for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
2366                         current_word[i] = *buffer++;
2367
2368                 retval = cfi_write_word(bank, current_word, write_p);
2369                 if (retval != ERROR_OK)
2370                         return retval;
2371                 write_p += bank->bus_width;
2372         }
2373
2374         /* handle blocks of bus_size aligned bytes */
2375         blk_count = count & ~(bank->bus_width - 1);     /* round down, leave tail bytes */
2376         switch (cfi_info->pri_id) {
2377                 /* try block writes (fails without working area) */
2378                 case 1:
2379                 case 3:
2380                         retval = cfi_intel_write_block(bank, buffer, write_p, blk_count);
2381                         break;
2382                 case 2:
2383                         retval = cfi_spansion_write_block(bank, buffer, write_p, blk_count);
2384                         break;
2385                 default:
2386                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2387                         retval = ERROR_FLASH_OPERATION_FAILED;
2388                         break;
2389         }
2390         if (retval == ERROR_OK) {
2391                 /* Increment pointers and decrease count on succesful block write */
2392                 buffer += blk_count;
2393                 write_p += blk_count;
2394                 count -= blk_count;
2395         } else {
2396                 if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
2397                         /* Calculate buffer size and boundary mask
2398                          * buffersize is (buffer size per chip) * (number of chips)
2399                          * bufferwsize is buffersize in words */
2400                         uint32_t buffersize =
2401                                 (1UL <<
2402                                  cfi_info->max_buf_write_size) *
2403                                 (bank->bus_width / bank->chip_width);
2404                         uint32_t buffermask = buffersize-1;
2405                         uint32_t bufferwsize = buffersize / bank->bus_width;
2406
2407                         /* fall back to memory writes */
2408                         while (count >= (uint32_t)bank->bus_width) {
2409                                 int fallback;
2410                                 if ((write_p & 0xff) == 0) {
2411                                         LOG_INFO("Programming at 0x%08" PRIx32 ", count 0x%08"
2412                                                 PRIx32 " bytes remaining", write_p, count);
2413                                 }
2414                                 fallback = 1;
2415                                 if ((bufferwsize > 0) && (count >= buffersize) &&
2416                                                 !(write_p & buffermask)) {
2417                                         retval = cfi_write_words(bank, buffer, bufferwsize, write_p);
2418                                         if (retval == ERROR_OK) {
2419                                                 buffer += buffersize;
2420                                                 write_p += buffersize;
2421                                                 count -= buffersize;
2422                                                 fallback = 0;
2423                                         } else if (retval != ERROR_FLASH_OPER_UNSUPPORTED)
2424                                                 return retval;
2425                                 }
2426                                 /* try the slow way? */
2427                                 if (fallback) {
2428                                         for (i = 0; i < bank->bus_width; i++)
2429                                                 current_word[i] = *buffer++;
2430
2431                                         retval = cfi_write_word(bank, current_word, write_p);
2432                                         if (retval != ERROR_OK)
2433                                                 return retval;
2434
2435                                         write_p += bank->bus_width;
2436                                         count -= bank->bus_width;
2437                                 }
2438                         }
2439                 } else
2440                         return retval;
2441         }
2442
2443         /* return to read array mode, so we can read from flash again for padding */
2444         retval = cfi_reset(bank);
2445         if (retval != ERROR_OK)
2446                 return retval;
2447
2448         /* handle unaligned tail bytes */
2449         if (count > 0) {
2450                 LOG_INFO("Fixup %" PRId32 " unaligned tail bytes", count);
2451
2452                 /* read a complete word from flash */
2453                 retval = target_read_memory(target, write_p, bank->bus_width, 1, current_word);
2454                 if (retval != ERROR_OK)
2455                         return retval;
2456
2457                 /* replace only bytes that must be written */
2458                 for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
2459                         current_word[i] = *buffer++;
2460
2461                 retval = cfi_write_word(bank, current_word, write_p);
2462                 if (retval != ERROR_OK)
2463                         return retval;
2464         }
2465
2466         /* return to read array mode */
2467         return cfi_reset(bank);
2468 }
2469
2470 static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, void *param)
2471 {
2472         (void) param;
2473         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2474         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2475
2476         pri_ext->_reversed_geometry = 1;
2477 }
2478
2479 static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, void *param)
2480 {
2481         int i;
2482         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2483         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2484         (void) param;
2485
2486         if ((pri_ext->_reversed_geometry) || (pri_ext->TopBottom == 3)) {
2487                 LOG_DEBUG("swapping reversed erase region information on cmdset 0002 device");
2488
2489                 for (i = 0; i < cfi_info->num_erase_regions / 2; i++) {
2490                         int j = (cfi_info->num_erase_regions - 1) - i;
2491                         uint32_t swap;
2492
2493                         swap = cfi_info->erase_region_info[i];
2494                         cfi_info->erase_region_info[i] = cfi_info->erase_region_info[j];
2495                         cfi_info->erase_region_info[j] = swap;
2496                 }
2497         }
2498 }
2499
2500 static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, void *param)
2501 {
2502         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2503         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2504         struct cfi_unlock_addresses *unlock_addresses = param;
2505
2506         pri_ext->_unlock1 = unlock_addresses->unlock1;
2507         pri_ext->_unlock2 = unlock_addresses->unlock2;
2508 }
2509
2510
2511 static int cfi_query_string(struct flash_bank *bank, int address)
2512 {
2513         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2514         int retval;
2515
2516         retval = cfi_send_command(bank, 0x98, flash_address(bank, 0, address));
2517         if (retval != ERROR_OK)
2518                 return retval;
2519
2520         retval = cfi_query_u8(bank, 0, 0x10, &cfi_info->qry[0]);
2521         if (retval != ERROR_OK)
2522                 return retval;
2523         retval = cfi_query_u8(bank, 0, 0x11, &cfi_info->qry[1]);
2524         if (retval != ERROR_OK)
2525                 return retval;
2526         retval = cfi_query_u8(bank, 0, 0x12, &cfi_info->qry[2]);
2527         if (retval != ERROR_OK)
2528                 return retval;
2529
2530         LOG_DEBUG("CFI qry returned: 0x%2.2x 0x%2.2x 0x%2.2x",
2531                 cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2]);
2532
2533         if ((cfi_info->qry[0] != 'Q') || (cfi_info->qry[1] != 'R') || (cfi_info->qry[2] != 'Y')) {
2534                 retval = cfi_reset(bank);
2535                 if (retval != ERROR_OK)
2536                         return retval;
2537                 LOG_ERROR("Could not probe bank: no QRY");
2538                 return ERROR_FLASH_BANK_INVALID;
2539         }
2540
2541         return ERROR_OK;
2542 }
2543
2544 static int cfi_probe(struct flash_bank *bank)
2545 {
2546         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2547         struct target *target = bank->target;
2548         int num_sectors = 0;
2549         int i;
2550         int sector = 0;
2551         uint32_t unlock1 = 0x555;
2552         uint32_t unlock2 = 0x2aa;
2553         int retval;
2554         uint8_t value_buf0[CFI_MAX_BUS_WIDTH], value_buf1[CFI_MAX_BUS_WIDTH];
2555
2556         if (bank->target->state != TARGET_HALTED) {
2557                 LOG_ERROR("Target not halted");
2558                 return ERROR_TARGET_NOT_HALTED;
2559         }
2560
2561         cfi_info->probed = 0;
2562         cfi_info->num_erase_regions = 0;
2563         if (bank->sectors) {
2564                 free(bank->sectors);
2565                 bank->sectors = NULL;
2566         }
2567         if (cfi_info->erase_region_info) {
2568                 free(cfi_info->erase_region_info);
2569                 cfi_info->erase_region_info = NULL;
2570         }
2571
2572         /* JEDEC standard JESD21C uses 0x5555 and 0x2aaa as unlock addresses,
2573          * while CFI compatible AMD/Spansion flashes use 0x555 and 0x2aa
2574          */
2575         if (cfi_info->jedec_probe) {
2576                 unlock1 = 0x5555;
2577                 unlock2 = 0x2aaa;
2578         }
2579
2580         /* switch to read identifier codes mode ("AUTOSELECT") */
2581         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, unlock1));
2582         if (retval != ERROR_OK)
2583                 return retval;
2584         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, unlock2));
2585         if (retval != ERROR_OK)
2586                 return retval;
2587         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, unlock1));
2588         if (retval != ERROR_OK)
2589                 return retval;
2590
2591         retval = target_read_memory(target, flash_address(bank, 0, 0x00),
2592                         bank->bus_width, 1, value_buf0);
2593         if (retval != ERROR_OK)
2594                 return retval;
2595         retval = target_read_memory(target, flash_address(bank, 0, 0x01),
2596                         bank->bus_width, 1, value_buf1);
2597         if (retval != ERROR_OK)
2598                 return retval;
2599         switch (bank->chip_width) {
2600                 case 1:
2601                         cfi_info->manufacturer = *value_buf0;
2602                         cfi_info->device_id = *value_buf1;
2603                         break;
2604                 case 2:
2605                         cfi_info->manufacturer = target_buffer_get_u16(target, value_buf0);
2606                         cfi_info->device_id = target_buffer_get_u16(target, value_buf1);
2607                         break;
2608                 case 4:
2609                         cfi_info->manufacturer = target_buffer_get_u32(target, value_buf0);
2610                         cfi_info->device_id = target_buffer_get_u32(target, value_buf1);
2611                         break;
2612                 default:
2613                         LOG_ERROR("Unsupported bank chipwidth %d, can't probe memory",
2614                                         bank->chip_width);
2615                         return ERROR_FLASH_OPERATION_FAILED;
2616         }
2617
2618         LOG_INFO("Flash Manufacturer/Device: 0x%04x 0x%04x",
2619                 cfi_info->manufacturer, cfi_info->device_id);
2620         /* switch back to read array mode */
2621         retval = cfi_reset(bank);
2622         if (retval != ERROR_OK)
2623                 return retval;
2624
2625         /* check device/manufacturer ID for known non-CFI flashes. */
2626         cfi_fixup_non_cfi(bank);
2627
2628         /* query only if this is a CFI compatible flash,
2629          * otherwise the relevant info has already been filled in
2630          */
2631         if (cfi_info->not_cfi == 0) {
2632                 /* enter CFI query mode
2633                  * according to JEDEC Standard No. 68.01,
2634                  * a single bus sequence with address = 0x55, data = 0x98 should put
2635                  * the device into CFI query mode.
2636                  *
2637                  * SST flashes clearly violate this, and we will consider them incompatbile for now
2638                  */
2639
2640                 retval = cfi_query_string(bank, 0x55);
2641                 if (retval != ERROR_OK) {
2642                         /*
2643                          * Spansion S29WS-N CFI query fix is to try 0x555 if 0x55 fails. Should
2644                          * be harmless enough:
2645                          *
2646                          * http://www.infradead.org/pipermail/linux-mtd/2005-September/013618.html
2647                          */
2648                         LOG_USER("Try workaround w/0x555 instead of 0x55 to get QRY.");
2649                         retval = cfi_query_string(bank, 0x555);
2650                 }
2651                 if (retval != ERROR_OK)
2652                         return retval;
2653
2654                 retval = cfi_query_u16(bank, 0, 0x13, &cfi_info->pri_id);
2655                 if (retval != ERROR_OK)
2656                         return retval;
2657                 retval = cfi_query_u16(bank, 0, 0x15, &cfi_info->pri_addr);
2658                 if (retval != ERROR_OK)
2659                         return retval;
2660                 retval = cfi_query_u16(bank, 0, 0x17, &cfi_info->alt_id);
2661                 if (retval != ERROR_OK)
2662                         return retval;
2663                 retval = cfi_query_u16(bank, 0, 0x19, &cfi_info->alt_addr);
2664                 if (retval != ERROR_OK)
2665                         return retval;
2666
2667                 LOG_DEBUG("qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: 0x%4.4x, alt_id: "
2668                         "0x%4.4x, alt_addr: 0x%4.4x", cfi_info->qry[0], cfi_info->qry[1],
2669                         cfi_info->qry[2], cfi_info->pri_id, cfi_info->pri_addr,
2670                         cfi_info->alt_id, cfi_info->alt_addr);
2671
2672                 retval = cfi_query_u8(bank, 0, 0x1b, &cfi_info->vcc_min);
2673                 if (retval != ERROR_OK)
2674                         return retval;
2675                 retval = cfi_query_u8(bank, 0, 0x1c, &cfi_info->vcc_max);
2676                 if (retval != ERROR_OK)
2677                         return retval;
2678                 retval = cfi_query_u8(bank, 0, 0x1d, &cfi_info->vpp_min);
2679                 if (retval != ERROR_OK)
2680                         return retval;
2681                 retval = cfi_query_u8(bank, 0, 0x1e, &cfi_info->vpp_max);
2682                 if (retval != ERROR_OK)
2683                         return retval;
2684
2685                 retval = cfi_query_u8(bank, 0, 0x1f, &cfi_info->word_write_timeout_typ);
2686                 if (retval != ERROR_OK)
2687                         return retval;
2688                 retval = cfi_query_u8(bank, 0, 0x20, &cfi_info->buf_write_timeout_typ);
2689                 if (retval != ERROR_OK)
2690                         return retval;
2691                 retval = cfi_query_u8(bank, 0, 0x21, &cfi_info->block_erase_timeout_typ);
2692                 if (retval != ERROR_OK)
2693                         return retval;
2694                 retval = cfi_query_u8(bank, 0, 0x22, &cfi_info->chip_erase_timeout_typ);
2695                 if (retval != ERROR_OK)
2696                         return retval;
2697                 retval = cfi_query_u8(bank, 0, 0x23, &cfi_info->word_write_timeout_max);
2698                 if (retval != ERROR_OK)
2699                         return retval;
2700                 retval = cfi_query_u8(bank, 0, 0x24, &cfi_info->buf_write_timeout_max);
2701                 if (retval != ERROR_OK)
2702                         return retval;
2703                 retval = cfi_query_u8(bank, 0, 0x25, &cfi_info->block_erase_timeout_max);
2704                 if (retval != ERROR_OK)
2705                         return retval;
2706                 retval = cfi_query_u8(bank, 0, 0x26, &cfi_info->chip_erase_timeout_max);
2707                 if (retval != ERROR_OK)
2708                         return retval;
2709
2710                 uint8_t data;
2711                 retval = cfi_query_u8(bank, 0, 0x27, &data);
2712                 if (retval != ERROR_OK)
2713                         return retval;
2714                 cfi_info->dev_size = 1 << data;
2715
2716                 retval = cfi_query_u16(bank, 0, 0x28, &cfi_info->interface_desc);
2717                 if (retval != ERROR_OK)
2718                         return retval;
2719                 retval = cfi_query_u16(bank, 0, 0x2a, &cfi_info->max_buf_write_size);
2720                 if (retval != ERROR_OK)
2721                         return retval;
2722                 retval = cfi_query_u8(bank, 0, 0x2c, &cfi_info->num_erase_regions);
2723                 if (retval != ERROR_OK)
2724                         return retval;
2725
2726                 LOG_DEBUG("size: 0x%" PRIx32 ", interface desc: %i, max buffer write size: 0x%x",
2727                         cfi_info->dev_size, cfi_info->interface_desc,
2728                         (1 << cfi_info->max_buf_write_size));
2729
2730                 if (cfi_info->num_erase_regions) {
2731                         cfi_info->erase_region_info = malloc(sizeof(*cfi_info->erase_region_info)
2732                                         * cfi_info->num_erase_regions);
2733                         for (i = 0; i < cfi_info->num_erase_regions; i++) {
2734                                 retval = cfi_query_u32(bank,
2735                                                 0,
2736                                                 0x2d + (4 * i),
2737                                                 &cfi_info->erase_region_info[i]);
2738                                 if (retval != ERROR_OK)
2739                                         return retval;
2740                                 LOG_DEBUG(
2741                                         "erase region[%i]: %" PRIu32 " blocks of size 0x%" PRIx32 "",
2742                                         i,
2743                                         (cfi_info->erase_region_info[i] & 0xffff) + 1,
2744                                         (cfi_info->erase_region_info[i] >> 16) * 256);
2745                         }
2746                 } else
2747                         cfi_info->erase_region_info = NULL;
2748
2749                 /* We need to read the primary algorithm extended query table before calculating
2750                  * the sector layout to be able to apply fixups
2751                  */
2752                 switch (cfi_info->pri_id) {
2753                         /* Intel command set (standard and extended) */
2754                         case 0x0001:
2755                         case 0x0003:
2756                                 cfi_read_intel_pri_ext(bank);
2757                                 break;
2758                         /* AMD/Spansion, Atmel, ... command set */
2759                         case 0x0002:
2760                                 cfi_info->status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7;  /*
2761                                                                                                  *default
2762                                                                                                  *for
2763                                                                                                  *all
2764                                                                                                  *CFI
2765                                                                                                  *flashs
2766                                                                                                  **/
2767                                 cfi_read_0002_pri_ext(bank);
2768                                 break;
2769                         default:
2770                                 LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2771                                 break;
2772                 }
2773
2774                 /* return to read array mode
2775                  * we use both reset commands, as some Intel flashes fail to recognize the 0xF0 command
2776                  */
2777                 retval = cfi_reset(bank);
2778                 if (retval != ERROR_OK)
2779                         return retval;
2780         }       /* end CFI case */
2781
2782         LOG_DEBUG("Vcc min: %x.%x, Vcc max: %x.%x, Vpp min: %u.%x, Vpp max: %u.%x",
2783                 (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
2784                 (cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
2785                 (cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
2786                 (cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
2787
2788         LOG_DEBUG("typ. word write timeout: %u us, typ. buf write timeout: %u us, "
2789                 "typ. block erase timeout: %u ms, typ. chip erase timeout: %u ms",
2790                 1 << cfi_info->word_write_timeout_typ, 1 << cfi_info->buf_write_timeout_typ,
2791                 1 << cfi_info->block_erase_timeout_typ, 1 << cfi_info->chip_erase_timeout_typ);
2792
2793         LOG_DEBUG("max. word write timeout: %u us, max. buf write timeout: %u us, "
2794                 "max. block erase timeout: %u ms, max. chip erase timeout: %u ms",
2795                 (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
2796                 (1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
2797                 (1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
2798                 (1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
2799
2800         /* convert timeouts to real values in ms */
2801         cfi_info->word_write_timeout = DIV_ROUND_UP((1L << cfi_info->word_write_timeout_typ) *
2802                         (1L << cfi_info->word_write_timeout_max), 1000);
2803         cfi_info->buf_write_timeout = DIV_ROUND_UP((1L << cfi_info->buf_write_timeout_typ) *
2804                         (1L << cfi_info->buf_write_timeout_max), 1000);
2805         cfi_info->block_erase_timeout = (1L << cfi_info->block_erase_timeout_typ) *
2806                 (1L << cfi_info->block_erase_timeout_max);
2807         cfi_info->chip_erase_timeout = (1L << cfi_info->chip_erase_timeout_typ) *
2808                 (1L << cfi_info->chip_erase_timeout_max);
2809
2810         LOG_DEBUG("calculated word write timeout: %u ms, buf write timeout: %u ms, "
2811                 "block erase timeout: %u ms, chip erase timeout: %u ms",
2812                 cfi_info->word_write_timeout, cfi_info->buf_write_timeout,
2813                 cfi_info->block_erase_timeout, cfi_info->chip_erase_timeout);
2814
2815         /* apply fixups depending on the primary command set */
2816         switch (cfi_info->pri_id) {
2817                 /* Intel command set (standard and extended) */
2818                 case 0x0001:
2819                 case 0x0003:
2820                         cfi_fixup(bank, cfi_0001_fixups);
2821                         break;
2822                 /* AMD/Spansion, Atmel, ... command set */
2823                 case 0x0002:
2824                         cfi_fixup(bank, cfi_0002_fixups);
2825                         break;
2826                 default:
2827                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2828                         break;
2829         }
2830
2831         if ((cfi_info->dev_size * bank->bus_width / bank->chip_width) != bank->size) {
2832                 LOG_WARNING("configuration specifies 0x%" PRIx32 " size, but a 0x%" PRIx32
2833                         " size flash was found", bank->size, cfi_info->dev_size);
2834         }
2835
2836         if (cfi_info->num_erase_regions == 0) {
2837                 /* a device might have only one erase block, spanning the whole device */
2838                 bank->num_sectors = 1;
2839                 bank->sectors = malloc(sizeof(struct flash_sector));
2840
2841                 bank->sectors[sector].offset = 0x0;
2842                 bank->sectors[sector].size = bank->size;
2843                 bank->sectors[sector].is_erased = -1;
2844                 bank->sectors[sector].is_protected = -1;
2845         } else {
2846                 uint32_t offset = 0;
2847
2848                 for (i = 0; i < cfi_info->num_erase_regions; i++)
2849                         num_sectors += (cfi_info->erase_region_info[i] & 0xffff) + 1;
2850
2851                 bank->num_sectors = num_sectors;
2852                 bank->sectors = malloc(sizeof(struct flash_sector) * num_sectors);
2853
2854                 for (i = 0; i < cfi_info->num_erase_regions; i++) {
2855                         uint32_t j;
2856                         for (j = 0; j < (cfi_info->erase_region_info[i] & 0xffff) + 1; j++) {
2857                                 bank->sectors[sector].offset = offset;
2858                                 bank->sectors[sector].size =
2859                                         ((cfi_info->erase_region_info[i] >> 16) * 256)
2860                                         * bank->bus_width / bank->chip_width;
2861                                 offset += bank->sectors[sector].size;
2862                                 bank->sectors[sector].is_erased = -1;
2863                                 bank->sectors[sector].is_protected = -1;
2864                                 sector++;
2865                         }
2866                 }
2867                 if (offset != (cfi_info->dev_size * bank->bus_width / bank->chip_width)) {
2868                         LOG_WARNING(
2869                                 "CFI size is 0x%" PRIx32 ", but total sector size is 0x%" PRIx32 "", \
2870                                 (cfi_info->dev_size * bank->bus_width / bank->chip_width),
2871                                 offset);
2872                 }
2873         }
2874
2875         cfi_info->probed = 1;
2876
2877         return ERROR_OK;
2878 }
2879
2880 static int cfi_auto_probe(struct flash_bank *bank)
2881 {
2882         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2883         if (cfi_info->probed)
2884                 return ERROR_OK;
2885         return cfi_probe(bank);
2886 }
2887
2888 static int cfi_intel_protect_check(struct flash_bank *bank)
2889 {
2890         int retval;
2891         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2892         struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
2893         int i;
2894
2895         /* check if block lock bits are supported on this device */
2896         if (!(pri_ext->blk_status_reg_mask & 0x1))
2897                 return ERROR_FLASH_OPERATION_FAILED;
2898
2899         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, 0x55));
2900         if (retval != ERROR_OK)
2901                 return retval;
2902
2903         for (i = 0; i < bank->num_sectors; i++) {
2904                 uint8_t block_status;
2905                 retval = cfi_get_u8(bank, i, 0x2, &block_status);
2906                 if (retval != ERROR_OK)
2907                         return retval;
2908
2909                 if (block_status & 1)
2910                         bank->sectors[i].is_protected = 1;
2911                 else
2912                         bank->sectors[i].is_protected = 0;
2913         }
2914
2915         return cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2916 }
2917
2918 static int cfi_spansion_protect_check(struct flash_bank *bank)
2919 {
2920         int retval;
2921         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2922         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2923         int i;
2924
2925         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
2926         if (retval != ERROR_OK)
2927                 return retval;
2928
2929         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
2930         if (retval != ERROR_OK)
2931                 return retval;
2932
2933         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, pri_ext->_unlock1));
2934         if (retval != ERROR_OK)
2935                 return retval;
2936
2937         for (i = 0; i < bank->num_sectors; i++) {
2938                 uint8_t block_status;
2939                 retval = cfi_get_u8(bank, i, 0x2, &block_status);
2940                 if (retval != ERROR_OK)
2941                         return retval;
2942
2943                 if (block_status & 1)
2944                         bank->sectors[i].is_protected = 1;
2945                 else
2946                         bank->sectors[i].is_protected = 0;
2947         }
2948
2949         return cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2950 }
2951
2952 static int cfi_protect_check(struct flash_bank *bank)
2953 {
2954         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2955
2956         if (bank->target->state != TARGET_HALTED) {
2957                 LOG_ERROR("Target not halted");
2958                 return ERROR_TARGET_NOT_HALTED;
2959         }
2960
2961         if (cfi_info->qry[0] != 'Q')
2962                 return ERROR_FLASH_BANK_NOT_PROBED;
2963
2964         switch (cfi_info->pri_id) {
2965                 case 1:
2966                 case 3:
2967                         return cfi_intel_protect_check(bank);
2968                         break;
2969                 case 2:
2970                         return cfi_spansion_protect_check(bank);
2971                         break;
2972                 default:
2973                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2974                         break;
2975         }
2976
2977         return ERROR_OK;
2978 }
2979
2980 static int get_cfi_info(struct flash_bank *bank, char *buf, int buf_size)
2981 {
2982         int printed;
2983         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2984
2985         if (cfi_info->qry[0] == 0xff) {
2986                 snprintf(buf, buf_size, "\ncfi flash bank not probed yet\n");
2987                 return ERROR_OK;
2988         }
2989
2990         if (cfi_info->not_cfi == 0)
2991                 printed = snprintf(buf, buf_size, "\nCFI flash: ");
2992         else
2993                 printed = snprintf(buf, buf_size, "\nnon-CFI flash: ");
2994         buf += printed;
2995         buf_size -= printed;
2996
2997         printed = snprintf(buf, buf_size, "mfr: 0x%4.4x, id:0x%4.4x\n\n",
2998                         cfi_info->manufacturer, cfi_info->device_id);
2999         buf += printed;
3000         buf_size -= printed;
3001
3002         printed = snprintf(buf, buf_size, "qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: "
3003                         "0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x\n",
3004                         cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2],
3005                         cfi_info->pri_id, cfi_info->pri_addr, cfi_info->alt_id, cfi_info->alt_addr);
3006         buf += printed;
3007         buf_size -= printed;
3008
3009         printed = snprintf(buf, buf_size, "Vcc min: %x.%x, Vcc max: %x.%x, "
3010                         "Vpp min: %u.%x, Vpp max: %u.%x\n",
3011                         (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
3012                         (cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
3013                         (cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
3014                         (cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
3015         buf += printed;
3016         buf_size -= printed;
3017
3018         printed = snprintf(buf, buf_size, "typ. word write timeout: %u us, "
3019                         "typ. buf write timeout: %u us, "
3020                         "typ. block erase timeout: %u ms, "
3021                         "typ. chip erase timeout: %u ms\n",
3022                         1 << cfi_info->word_write_timeout_typ,
3023                         1 << cfi_info->buf_write_timeout_typ,
3024                         1 << cfi_info->block_erase_timeout_typ,
3025                         1 << cfi_info->chip_erase_timeout_typ);
3026         buf += printed;
3027         buf_size -= printed;
3028
3029         printed = snprintf(buf,
3030                         buf_size,
3031                         "max. word write timeout: %u us, "
3032                         "max. buf write timeout: %u us, max. "
3033                         "block erase timeout: %u ms, max. chip erase timeout: %u ms\n",
3034                         (1 <<
3035                          cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
3036                         (1 <<
3037                          cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
3038                         (1 <<
3039                          cfi_info->block_erase_timeout_max) *
3040                         (1 << cfi_info->block_erase_timeout_typ),
3041                         (1 <<
3042                          cfi_info->chip_erase_timeout_max) *
3043                         (1 << cfi_info->chip_erase_timeout_typ));
3044         buf += printed;
3045         buf_size -= printed;
3046
3047         printed = snprintf(buf, buf_size, "size: 0x%" PRIx32 ", interface desc: %i, "
3048                         "max buffer write size: 0x%x\n",
3049                         cfi_info->dev_size,
3050                         cfi_info->interface_desc,
3051                         1 << cfi_info->max_buf_write_size);
3052         buf += printed;
3053         buf_size -= printed;
3054
3055         switch (cfi_info->pri_id) {
3056             case 1:
3057             case 3:
3058                     cfi_intel_info(bank, buf, buf_size);
3059                     break;
3060             case 2:
3061                     cfi_spansion_info(bank, buf, buf_size);
3062                     break;
3063             default:
3064                     LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
3065                     break;
3066         }
3067
3068         return ERROR_OK;
3069 }
3070
3071 static void cfi_fixup_0002_write_buffer(struct flash_bank *bank, void *param)
3072 {
3073         struct cfi_flash_bank *cfi_info = bank->driver_priv;
3074
3075         /* disable write buffer for M29W128G */
3076         cfi_info->buf_write_timeout_typ = 0;
3077 }
3078
3079 struct flash_driver cfi_flash = {
3080         .name = "cfi",
3081         .flash_bank_command = cfi_flash_bank_command,
3082         .erase = cfi_erase,
3083         .protect = cfi_protect,
3084         .write = cfi_write,
3085         .read = cfi_read,
3086         .probe = cfi_probe,
3087         .auto_probe = cfi_auto_probe,
3088         /* FIXME: access flash at bus_width size */
3089         .erase_check = default_flash_blank_check,
3090         .protect_check = cfi_protect_check,
3091         .info = get_cfi_info,
3092 };