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