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