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

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