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

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