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