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