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

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