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