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