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