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

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