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

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