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