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

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