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 flash_bank *bank, target_addr_t addr,
 135                                    uint32_t count, const uint8_t *buffer)
 136 {
 137         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 138         if (cfi_info->write_mem) {
 139                 return cfi_info->write_mem(bank, addr, count, buffer);
 140         } else {
 141                 return target_write_memory(bank->target, addr, bank->bus_width,
 142                                            count, buffer);
 143         }
 144 }
 145
 146 static int cfi_target_read_memory(struct flash_bank *bank, target_addr_t addr,
 147                                   uint32_t count, uint8_t *buffer)
 148 {
 149         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 150         if (cfi_info->read_mem) {
 151                 return cfi_info->read_mem(bank, addr, count, buffer);
 152         } else {
 153                 return target_read_memory(bank->target, addr, bank->bus_width,
 154                                           count, buffer);
 155         }
 156 }
 157
 158 static void cfi_command(struct flash_bank *bank, uint8_t cmd, uint8_t *cmd_buf)
 159 {
 160         int i;
 161         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 162
 163         /* clear whole buffer, to ensure bits that exceed the bus_width
 164          * are set to zero
 165          */
 166         for (i = 0; i < CFI_MAX_BUS_WIDTH; i++)
 167                 cmd_buf[i] = 0;
 168
 169         if (cfi_info->endianness == TARGET_LITTLE_ENDIAN) {
 170                 for (i = bank->bus_width; i > 0; i--)
 171                         *cmd_buf++ = (i & (bank->chip_width - 1)) ? 0x0 : cmd;
 172         } else {
 173                 for (i = 1; i <= bank->bus_width; i++)
 174                         *cmd_buf++ = (i & (bank->chip_width - 1)) ? 0x0 : cmd;
 175         }
 176 }
 177
 178 static int cfi_send_command(struct flash_bank *bank, uint8_t cmd, uint32_t address)
 179 {
 180         uint8_t command[CFI_MAX_BUS_WIDTH];
 181
 182         cfi_command(bank, cmd, command);
 183         return cfi_target_write_memory(bank, address, 1, command);
 184 }
 185
 186 /* read unsigned 8-bit value from the bank
 187  * flash banks are expected to be made of similar chips
 188  * the query result should be the same for all
 189  */
 190 static int cfi_query_u8(struct flash_bank *bank, int sector, uint32_t offset, uint8_t *val)
 191 {
 192         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 193         uint8_t data[CFI_MAX_BUS_WIDTH];
 194
 195         int retval;
 196         retval = cfi_target_read_memory(bank, flash_address(bank, sector, offset),
 197                                         1, data);
 198         if (retval != ERROR_OK)
 199                 return retval;
 200
 201         if (cfi_info->endianness == TARGET_LITTLE_ENDIAN)
 202                 *val = data[0];
 203         else
 204                 *val = data[bank->bus_width - 1];
 205
 206         return ERROR_OK;
 207 }
 208
 209 /* read unsigned 8-bit value from the bank
 210  * in case of a bank made of multiple chips,
 211  * the individual values are ORed
 212  */
 213 static int cfi_get_u8(struct flash_bank *bank, int sector, uint32_t offset, uint8_t *val)
 214 {
 215         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 216         uint8_t data[CFI_MAX_BUS_WIDTH];
 217         int i;
 218
 219         int retval;
 220         retval = cfi_target_read_memory(bank, flash_address(bank, sector, offset),
 221                                         1, data);
 222         if (retval != ERROR_OK)
 223                 return retval;
 224
 225         if (cfi_info->endianness == TARGET_LITTLE_ENDIAN) {
 226                 for (i = 0; i < bank->bus_width / bank->chip_width; i++)
 227                         data[0] |= data[i];
 228
 229                 *val = data[0];
 230         } else {
 231                 uint8_t value = 0;
 232                 for (i = 0; i < bank->bus_width / bank->chip_width; i++)
 233                         value |= data[bank->bus_width - 1 - i];
 234
 235                 *val = value;
 236         }
 237         return ERROR_OK;
 238 }
 239
 240 static int cfi_query_u16(struct flash_bank *bank, int sector, uint32_t offset, uint16_t *val)
 241 {
 242         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 243         uint8_t data[CFI_MAX_BUS_WIDTH * 2];
 244         int retval;
 245
 246         if (cfi_info->x16_as_x8) {
 247                 uint8_t i;
 248                 for (i = 0; i < 2; i++) {
 249                         retval = cfi_target_read_memory(bank, flash_address(bank, sector, offset + i),
 250                                                         1, &data[i * bank->bus_width]);
 251                         if (retval != ERROR_OK)
 252                                 return retval;
 253                 }
 254         } else {
 255                 retval = cfi_target_read_memory(bank, flash_address(bank, sector, offset),
 256                                                 2, data);
 257                 if (retval != ERROR_OK)
 258                         return retval;
 259         }
 260
 261         if (cfi_info->endianness == TARGET_LITTLE_ENDIAN)
 262                 *val = data[0] | data[bank->bus_width] << 8;
 263         else
 264                 *val = data[bank->bus_width - 1] | data[(2 * bank->bus_width) - 1] << 8;
 265
 266         return ERROR_OK;
 267 }
 268
 269 static int cfi_query_u32(struct flash_bank *bank, int sector, uint32_t offset, uint32_t *val)
 270 {
 271         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 272         uint8_t data[CFI_MAX_BUS_WIDTH * 4];
 273         int retval;
 274
 275         if (cfi_info->x16_as_x8) {
 276                 uint8_t i;
 277                 for (i = 0; i < 4; i++) {
 278                         retval = cfi_target_read_memory(bank, flash_address(bank, sector, offset + i),
 279                                                         1, &data[i * bank->bus_width]);
 280                         if (retval != ERROR_OK)
 281                                 return retval;
 282                 }
 283         } else {
 284                 retval = cfi_target_read_memory(bank, flash_address(bank, sector, offset),
 285                                                 4, data);
 286                 if (retval != ERROR_OK)
 287                         return retval;
 288         }
 289
 290         if (cfi_info->endianness == TARGET_LITTLE_ENDIAN)
 291                 *val = data[0] | data[bank->bus_width] << 8 |
 292                         data[bank->bus_width * 2] << 16 | data[bank->bus_width * 3] << 24;
 293         else
 294                 *val = data[bank->bus_width - 1] | data[(2 * bank->bus_width) - 1] << 8 |
 295                         data[(3 * bank->bus_width) - 1] << 16 |
 296                         data[(4 * bank->bus_width) - 1] << 24;
 297
 298         return ERROR_OK;
 299 }
 300
 301 static int cfi_reset(struct flash_bank *bank)
 302 {
 303         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 304         int retval = ERROR_OK;
 305
 306         retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 307         if (retval != ERROR_OK)
 308                 return retval;
 309
 310         retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
 311         if (retval != ERROR_OK)
 312                 return retval;
 313
 314         if (cfi_info->manufacturer == 0x20 &&
 315                         (cfi_info->device_id == 0x227E || cfi_info->device_id == 0x7E)) {
 316                 /* Numonix M29W128G is cmd 0xFF intolerant - causes internal undefined state
 317                  * so we send an extra 0xF0 reset to fix the bug */
 318                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x00));
 319                 if (retval != ERROR_OK)
 320                         return retval;
 321         }
 322
 323         return retval;
 324 }
 325
 326 static void cfi_intel_clear_status_register(struct flash_bank *bank)
 327 {
 328         cfi_send_command(bank, 0x50, flash_address(bank, 0, 0x0));
 329 }
 330
 331 static int cfi_intel_wait_status_busy(struct flash_bank *bank, int timeout, uint8_t *val)
 332 {
 333         uint8_t status;
 334
 335         int retval = ERROR_OK;
 336
 337         for (;; ) {
 338                 if (timeout-- < 0) {
 339                         LOG_ERROR("timeout while waiting for WSM to become ready");
 340                         return ERROR_FAIL;
 341                 }
 342
 343                 retval = cfi_get_u8(bank, 0, 0x0, &status);
 344                 if (retval != ERROR_OK)
 345                         return retval;
 346
 347                 if (status & 0x80)
 348                         break;
 349
 350                 alive_sleep(1);
 351         }
 352
 353         /* mask out bit 0 (reserved) */
 354         status = status & 0xfe;
 355
 356         LOG_DEBUG("status: 0x%x", status);
 357
 358         if (status != 0x80) {
 359                 LOG_ERROR("status register: 0x%x", status);
 360                 if (status & 0x2)
 361                         LOG_ERROR("Block Lock-Bit Detected, Operation Abort");
 362                 if (status & 0x4)
 363                         LOG_ERROR("Program suspended");
 364                 if (status & 0x8)
 365                         LOG_ERROR("Low Programming Voltage Detected, Operation Aborted");
 366                 if (status & 0x10)
 367                         LOG_ERROR("Program Error / Error in Setting Lock-Bit");
 368                 if (status & 0x20)
 369                         LOG_ERROR("Error in Block Erasure or Clear Lock-Bits");
 370                 if (status & 0x40)
 371                         LOG_ERROR("Block Erase Suspended");
 372
 373                 cfi_intel_clear_status_register(bank);
 374
 375                 retval = ERROR_FAIL;
 376         }
 377
 378         *val = status;
 379         return retval;
 380 }
 381
 382 static int cfi_spansion_wait_status_busy(struct flash_bank *bank, int timeout)
 383 {
 384         uint8_t status, oldstatus;
 385         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 386         int retval;
 387
 388         retval = cfi_get_u8(bank, 0, 0x0, &oldstatus);
 389         if (retval != ERROR_OK)
 390                 return retval;
 391
 392         do {
 393                 retval = cfi_get_u8(bank, 0, 0x0, &status);
 394
 395                 if (retval != ERROR_OK)
 396                         return retval;
 397
 398                 if ((status ^ oldstatus) & 0x40) {
 399                         if (status & cfi_info->status_poll_mask & 0x20) {
 400                                 retval = cfi_get_u8(bank, 0, 0x0, &oldstatus);
 401                                 if (retval != ERROR_OK)
 402                                         return retval;
 403                                 retval = cfi_get_u8(bank, 0, 0x0, &status);
 404                                 if (retval != ERROR_OK)
 405                                         return retval;
 406                                 if ((status ^ oldstatus) & 0x40) {
 407                                         LOG_ERROR("dq5 timeout, status: 0x%x", status);
 408                                         return ERROR_FLASH_OPERATION_FAILED;
 409                                 } else {
 410                                         LOG_DEBUG("status: 0x%x", status);
 411                                         return ERROR_OK;
 412                                 }
 413                         }
 414                 } else {/* no toggle: finished, OK */
 415                         LOG_DEBUG("status: 0x%x", status);
 416                         return ERROR_OK;
 417                 }
 418
 419                 oldstatus = status;
 420                 alive_sleep(1);
 421         } while (timeout-- > 0);
 422
 423         LOG_ERROR("timeout, status: 0x%x", status);
 424
 425         return ERROR_FLASH_BUSY;
 426 }
 427
 428 static int cfi_read_intel_pri_ext(struct flash_bank *bank)
 429 {
 430         int retval;
 431         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 432         struct cfi_intel_pri_ext *pri_ext;
 433
 434         if (cfi_info->pri_ext)
 435                 free(cfi_info->pri_ext);
 436
 437         pri_ext = malloc(sizeof(struct cfi_intel_pri_ext));
 438         if (pri_ext == NULL) {
 439                 LOG_ERROR("Out of memory");
 440                 return ERROR_FAIL;
 441         }
 442         cfi_info->pri_ext = pri_ext;
 443
 444         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0, &pri_ext->pri[0]);
 445         if (retval != ERROR_OK)
 446                 return retval;
 447         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 1, &pri_ext->pri[1]);
 448         if (retval != ERROR_OK)
 449                 return retval;
 450         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 2, &pri_ext->pri[2]);
 451         if (retval != ERROR_OK)
 452                 return retval;
 453
 454         if ((pri_ext->pri[0] != 'P') || (pri_ext->pri[1] != 'R') || (pri_ext->pri[2] != 'I')) {
 455                 retval = cfi_reset(bank);
 456                 if (retval != ERROR_OK)
 457                         return retval;
 458                 LOG_ERROR("Could not read bank flash bank information");
 459                 return ERROR_FLASH_BANK_INVALID;
 460         }
 461
 462         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 3, &pri_ext->major_version);
 463         if (retval != ERROR_OK)
 464                 return retval;
 465         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 4, &pri_ext->minor_version);
 466         if (retval != ERROR_OK)
 467                 return retval;
 468
 469         LOG_DEBUG("pri: '%c%c%c', version: %c.%c", pri_ext->pri[0], pri_ext->pri[1],
 470                 pri_ext->pri[2], pri_ext->major_version, pri_ext->minor_version);
 471
 472         retval = cfi_query_u32(bank, 0, cfi_info->pri_addr + 5, &pri_ext->feature_support);
 473         if (retval != ERROR_OK)
 474                 return retval;
 475         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 9, &pri_ext->suspend_cmd_support);
 476         if (retval != ERROR_OK)
 477                 return retval;
 478         retval = cfi_query_u16(bank, 0, cfi_info->pri_addr + 0xa, &pri_ext->blk_status_reg_mask);
 479         if (retval != ERROR_OK)
 480                 return retval;
 481
 482         LOG_DEBUG("feature_support: 0x%" PRIx32 ", suspend_cmd_support: "
 483                 "0x%x, blk_status_reg_mask: 0x%x",
 484                 pri_ext->feature_support,
 485                 pri_ext->suspend_cmd_support,
 486                 pri_ext->blk_status_reg_mask);
 487
 488         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xc, &pri_ext->vcc_optimal);
 489         if (retval != ERROR_OK)
 490                 return retval;
 491         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xd, &pri_ext->vpp_optimal);
 492         if (retval != ERROR_OK)
 493                 return retval;
 494
 495         LOG_DEBUG("Vcc opt: %x.%x, Vpp opt: %u.%x",
 496                 (pri_ext->vcc_optimal & 0xf0) >> 4, pri_ext->vcc_optimal & 0x0f,
 497                 (pri_ext->vpp_optimal & 0xf0) >> 4, pri_ext->vpp_optimal & 0x0f);
 498
 499         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xe, &pri_ext->num_protection_fields);
 500         if (retval != ERROR_OK)
 501                 return retval;
 502         if (pri_ext->num_protection_fields != 1) {
 503                 LOG_WARNING("expected one protection register field, but found %i",
 504                         pri_ext->num_protection_fields);
 505         }
 506
 507         retval = cfi_query_u16(bank, 0, cfi_info->pri_addr + 0xf, &pri_ext->prot_reg_addr);
 508         if (retval != ERROR_OK)
 509                 return retval;
 510         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0x11, &pri_ext->fact_prot_reg_size);
 511         if (retval != ERROR_OK)
 512                 return retval;
 513         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0x12, &pri_ext->user_prot_reg_size);
 514         if (retval != ERROR_OK)
 515                 return retval;
 516
 517         LOG_DEBUG("protection_fields: %i, prot_reg_addr: 0x%x, "
 518                 "factory pre-programmed: %i, user programmable: %i",
 519                 pri_ext->num_protection_fields, pri_ext->prot_reg_addr,
 520                 1 << pri_ext->fact_prot_reg_size, 1 << pri_ext->user_prot_reg_size);
 521
 522         return ERROR_OK;
 523 }
 524
 525 static int cfi_read_spansion_pri_ext(struct flash_bank *bank)
 526 {
 527         int retval;
 528         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 529         struct cfi_spansion_pri_ext *pri_ext;
 530
 531         if (cfi_info->pri_ext)
 532                 free(cfi_info->pri_ext);
 533
 534         pri_ext = malloc(sizeof(struct cfi_spansion_pri_ext));
 535         if (pri_ext == NULL) {
 536                 LOG_ERROR("Out of memory");
 537                 return ERROR_FAIL;
 538         }
 539         cfi_info->pri_ext = pri_ext;
 540
 541         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0, &pri_ext->pri[0]);
 542         if (retval != ERROR_OK)
 543                 return retval;
 544         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 1, &pri_ext->pri[1]);
 545         if (retval != ERROR_OK)
 546                 return retval;
 547         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 2, &pri_ext->pri[2]);
 548         if (retval != ERROR_OK)
 549                 return retval;
 550
 551         /* default values for implementation specific workarounds */
 552         pri_ext->_unlock1 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock1;
 553         pri_ext->_unlock2 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock2;
 554         pri_ext->_reversed_geometry = 0;
 555
 556         if ((pri_ext->pri[0] != 'P') || (pri_ext->pri[1] != 'R') || (pri_ext->pri[2] != 'I')) {
 557                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 558                 if (retval != ERROR_OK)
 559                         return retval;
 560                 LOG_ERROR("Could not read spansion bank information");
 561                 return ERROR_FLASH_BANK_INVALID;
 562         }
 563
 564         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 3, &pri_ext->major_version);
 565         if (retval != ERROR_OK)
 566                 return retval;
 567         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 4, &pri_ext->minor_version);
 568         if (retval != ERROR_OK)
 569                 return retval;
 570
 571         LOG_DEBUG("pri: '%c%c%c', version: %c.%c", pri_ext->pri[0], pri_ext->pri[1],
 572                 pri_ext->pri[2], pri_ext->major_version, pri_ext->minor_version);
 573
 574         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 5, &pri_ext->SiliconRevision);
 575         if (retval != ERROR_OK)
 576                 return retval;
 577         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 6, &pri_ext->EraseSuspend);
 578         if (retval != ERROR_OK)
 579                 return retval;
 580         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 7, &pri_ext->BlkProt);
 581         if (retval != ERROR_OK)
 582                 return retval;
 583         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 8, &pri_ext->TmpBlkUnprotect);
 584         if (retval != ERROR_OK)
 585                 return retval;
 586         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 9, &pri_ext->BlkProtUnprot);
 587         if (retval != ERROR_OK)
 588                 return retval;
 589         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 10, &pri_ext->SimultaneousOps);
 590         if (retval != ERROR_OK)
 591                 return retval;
 592         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 11, &pri_ext->BurstMode);
 593         if (retval != ERROR_OK)
 594                 return retval;
 595         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 12, &pri_ext->PageMode);
 596         if (retval != ERROR_OK)
 597                 return retval;
 598         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 13, &pri_ext->VppMin);
 599         if (retval != ERROR_OK)
 600                 return retval;
 601         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 14, &pri_ext->VppMax);
 602         if (retval != ERROR_OK)
 603                 return retval;
 604         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 15, &pri_ext->TopBottom);
 605         if (retval != ERROR_OK)
 606                 return retval;
 607
 608         LOG_DEBUG("Silicon Revision: 0x%x, Erase Suspend: 0x%x, Block protect: 0x%x",
 609                 pri_ext->SiliconRevision, pri_ext->EraseSuspend, pri_ext->BlkProt);
 610
 611         LOG_DEBUG("Temporary Unprotect: 0x%x, Block Protect Scheme: 0x%x, "
 612                 "Simultaneous Ops: 0x%x", pri_ext->TmpBlkUnprotect,
 613                 pri_ext->BlkProtUnprot, pri_ext->SimultaneousOps);
 614
 615         LOG_DEBUG("Burst Mode: 0x%x, Page Mode: 0x%x, ", pri_ext->BurstMode, pri_ext->PageMode);
 616
 617
 618         LOG_DEBUG("Vpp min: %u.%x, Vpp max: %u.%x",
 619                 (pri_ext->VppMin & 0xf0) >> 4, pri_ext->VppMin & 0x0f,
 620                 (pri_ext->VppMax & 0xf0) >> 4, pri_ext->VppMax & 0x0f);
 621
 622         LOG_DEBUG("WP# protection 0x%x", pri_ext->TopBottom);
 623
 624         return ERROR_OK;
 625 }
 626
 627 static int cfi_read_atmel_pri_ext(struct flash_bank *bank)
 628 {
 629         int retval;
 630         struct cfi_atmel_pri_ext atmel_pri_ext;
 631         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 632         struct cfi_spansion_pri_ext *pri_ext;
 633
 634         if (cfi_info->pri_ext)
 635                 free(cfi_info->pri_ext);
 636
 637         pri_ext = malloc(sizeof(struct cfi_spansion_pri_ext));
 638         if (pri_ext == NULL) {
 639                 LOG_ERROR("Out of memory");
 640                 return ERROR_FAIL;
 641         }
 642
 643         /* ATMEL devices use the same CFI primary command set (0x2) as AMD/Spansion,
 644          * but a different primary extended query table.
 645          * We read the atmel table, and prepare a valid AMD/Spansion query table.
 646          */
 647
 648         memset(pri_ext, 0, sizeof(struct cfi_spansion_pri_ext));
 649
 650         cfi_info->pri_ext = pri_ext;
 651
 652         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0, &atmel_pri_ext.pri[0]);
 653         if (retval != ERROR_OK)
 654                 return retval;
 655         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 1, &atmel_pri_ext.pri[1]);
 656         if (retval != ERROR_OK)
 657                 return retval;
 658         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 2, &atmel_pri_ext.pri[2]);
 659         if (retval != ERROR_OK)
 660                 return retval;
 661
 662         if ((atmel_pri_ext.pri[0] != 'P') || (atmel_pri_ext.pri[1] != 'R')
 663                         || (atmel_pri_ext.pri[2] != 'I')) {
 664                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 665                 if (retval != ERROR_OK)
 666                         return retval;
 667                 LOG_ERROR("Could not read atmel bank information");
 668                 return ERROR_FLASH_BANK_INVALID;
 669         }
 670
 671         pri_ext->pri[0] = atmel_pri_ext.pri[0];
 672         pri_ext->pri[1] = atmel_pri_ext.pri[1];
 673         pri_ext->pri[2] = atmel_pri_ext.pri[2];
 674
 675         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 3, &atmel_pri_ext.major_version);
 676         if (retval != ERROR_OK)
 677                 return retval;
 678         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 4, &atmel_pri_ext.minor_version);
 679         if (retval != ERROR_OK)
 680                 return retval;
 681
 682         LOG_DEBUG("pri: '%c%c%c', version: %c.%c", atmel_pri_ext.pri[0],
 683                 atmel_pri_ext.pri[1], atmel_pri_ext.pri[2],
 684                 atmel_pri_ext.major_version, atmel_pri_ext.minor_version);
 685
 686         pri_ext->major_version = atmel_pri_ext.major_version;
 687         pri_ext->minor_version = atmel_pri_ext.minor_version;
 688
 689         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 5, &atmel_pri_ext.features);
 690         if (retval != ERROR_OK)
 691                 return retval;
 692         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 6, &atmel_pri_ext.bottom_boot);
 693         if (retval != ERROR_OK)
 694                 return retval;
 695         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 7, &atmel_pri_ext.burst_mode);
 696         if (retval != ERROR_OK)
 697                 return retval;
 698         retval = cfi_query_u8(bank, 0, cfi_info->pri_addr + 8, &atmel_pri_ext.page_mode);
 699         if (retval != ERROR_OK)
 700                 return retval;
 701
 702         LOG_DEBUG(
 703                 "features: 0x%2.2x, bottom_boot: 0x%2.2x, burst_mode: 0x%2.2x, page_mode: 0x%2.2x",
 704                 atmel_pri_ext.features,
 705                 atmel_pri_ext.bottom_boot,
 706                 atmel_pri_ext.burst_mode,
 707                 atmel_pri_ext.page_mode);
 708
 709         if (atmel_pri_ext.features & 0x02)
 710                 pri_ext->EraseSuspend = 2;
 711
 712         /* some chips got it backwards... */
 713         if (cfi_info->device_id == AT49BV6416 ||
 714                         cfi_info->device_id == AT49BV6416T) {
 715                 if (atmel_pri_ext.bottom_boot)
 716                         pri_ext->TopBottom = 3;
 717                 else
 718                         pri_ext->TopBottom = 2;
 719         } else {
 720                 if (atmel_pri_ext.bottom_boot)
 721                         pri_ext->TopBottom = 2;
 722                 else
 723                         pri_ext->TopBottom = 3;
 724         }
 725
 726         pri_ext->_unlock1 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock1;
 727         pri_ext->_unlock2 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock2;
 728
 729         return ERROR_OK;
 730 }
 731
 732 static int cfi_read_0002_pri_ext(struct flash_bank *bank)
 733 {
 734         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 735
 736         if (cfi_info->manufacturer == CFI_MFR_ATMEL)
 737                 return cfi_read_atmel_pri_ext(bank);
 738         else
 739                 return cfi_read_spansion_pri_ext(bank);
 740 }
 741
 742 static int cfi_spansion_info(struct flash_bank *bank, char *buf, int buf_size)
 743 {
 744         int printed;
 745         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 746         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 747
 748         printed = snprintf(buf, buf_size, "\nSpansion primary algorithm extend information:\n");
 749         buf += printed;
 750         buf_size -= printed;
 751
 752         printed = snprintf(buf, buf_size, "pri: '%c%c%c', version: %c.%c\n", pri_ext->pri[0],
 753                         pri_ext->pri[1], pri_ext->pri[2],
 754                         pri_ext->major_version, pri_ext->minor_version);
 755         buf += printed;
 756         buf_size -= printed;
 757
 758         printed = snprintf(buf, buf_size, "Silicon Rev.: 0x%x, Address Sensitive unlock: 0x%x\n",
 759                         (pri_ext->SiliconRevision) >> 2,
 760                         (pri_ext->SiliconRevision) & 0x03);
 761         buf += printed;
 762         buf_size -= printed;
 763
 764         printed = snprintf(buf, buf_size, "Erase Suspend: 0x%x, Sector Protect: 0x%x\n",
 765                         pri_ext->EraseSuspend,
 766                         pri_ext->BlkProt);
 767         buf += printed;
 768         buf_size -= printed;
 769
 770         snprintf(buf, buf_size, "VppMin: %u.%x, VppMax: %u.%x\n",
 771                 (pri_ext->VppMin & 0xf0) >> 4, pri_ext->VppMin & 0x0f,
 772                 (pri_ext->VppMax & 0xf0) >> 4, pri_ext->VppMax & 0x0f);
 773
 774         return ERROR_OK;
 775 }
 776
 777 static int cfi_intel_info(struct flash_bank *bank, char *buf, int buf_size)
 778 {
 779         int printed;
 780         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 781         struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
 782
 783         printed = snprintf(buf, buf_size, "\nintel primary algorithm extend information:\n");
 784         buf += printed;
 785         buf_size -= printed;
 786
 787         printed = snprintf(buf,
 788                         buf_size,
 789                         "pri: '%c%c%c', version: %c.%c\n",
 790                         pri_ext->pri[0],
 791                         pri_ext->pri[1],
 792                         pri_ext->pri[2],
 793                         pri_ext->major_version,
 794                         pri_ext->minor_version);
 795         buf += printed;
 796         buf_size -= printed;
 797
 798         printed = snprintf(buf,
 799                         buf_size,
 800                         "feature_support: 0x%" PRIx32 ", "
 801                         "suspend_cmd_support: 0x%x, blk_status_reg_mask: 0x%x\n",
 802                         pri_ext->feature_support,
 803                         pri_ext->suspend_cmd_support,
 804                         pri_ext->blk_status_reg_mask);
 805         buf += printed;
 806         buf_size -= printed;
 807
 808         printed = snprintf(buf, buf_size, "Vcc opt: %x.%x, Vpp opt: %u.%x\n",
 809                         (pri_ext->vcc_optimal & 0xf0) >> 4, pri_ext->vcc_optimal & 0x0f,
 810                         (pri_ext->vpp_optimal & 0xf0) >> 4, pri_ext->vpp_optimal & 0x0f);
 811         buf += printed;
 812         buf_size -= printed;
 813
 814         snprintf(buf, buf_size, "protection_fields: %i, prot_reg_addr: 0x%x, "
 815                 "factory pre-programmed: %i, user programmable: %i\n",
 816                 pri_ext->num_protection_fields, pri_ext->prot_reg_addr,
 817                 1 << pri_ext->fact_prot_reg_size, 1 << pri_ext->user_prot_reg_size);
 818
 819         return ERROR_OK;
 820 }
 821
 822 /* flash_bank cfi <base> <size> <chip_width> <bus_width> <target#> [options]
 823  */
 824 FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
 825 {
 826         struct cfi_flash_bank *cfi_info;
 827         int bus_swap = 0;
 828
 829         if (CMD_ARGC < 6)
 830                 return ERROR_COMMAND_SYNTAX_ERROR;
 831
 832         /* both widths must:
 833          * - not exceed max value;
 834          * - not be null;
 835          * - be equal to a power of 2.
 836          * bus must be wide enough to hold one chip */
 837         if ((bank->chip_width > CFI_MAX_CHIP_WIDTH)
 838                         || (bank->bus_width > CFI_MAX_BUS_WIDTH)
 839                         || (bank->chip_width == 0)
 840                         || (bank->bus_width == 0)
 841                         || (bank->chip_width & (bank->chip_width - 1))
 842                         || (bank->bus_width & (bank->bus_width - 1))
 843                         || (bank->chip_width > bank->bus_width)) {
 844                 LOG_ERROR("chip and bus width have to specified in bytes");
 845                 return ERROR_FLASH_BANK_INVALID;
 846         }
 847
 848         cfi_info = malloc(sizeof(struct cfi_flash_bank));
 849         cfi_info->probed = 0;
 850         cfi_info->erase_region_info = NULL;
 851         cfi_info->pri_ext = NULL;
 852         bank->driver_priv = cfi_info;
 853
 854         cfi_info->x16_as_x8 = 0;
 855         cfi_info->jedec_probe = 0;
 856         cfi_info->not_cfi = 0;
 857         cfi_info->data_swap = 0;
 858
 859         for (unsigned i = 6; i < CMD_ARGC; i++) {
 860                 if (strcmp(CMD_ARGV[i], "x16_as_x8") == 0)
 861                         cfi_info->x16_as_x8 = 1;
 862                 else if (strcmp(CMD_ARGV[i], "data_swap") == 0)
 863                         cfi_info->data_swap = 1;
 864                 else if (strcmp(CMD_ARGV[i], "bus_swap") == 0)
 865                         bus_swap = 1;
 866                 else if (strcmp(CMD_ARGV[i], "jedec_probe") == 0)
 867                         cfi_info->jedec_probe = 1;
 868         }
 869
 870         if (bus_swap)
 871                 cfi_info->endianness =
 872                         bank->target->endianness == TARGET_LITTLE_ENDIAN ?
 873                         TARGET_BIG_ENDIAN : TARGET_LITTLE_ENDIAN;
 874         else
 875                 cfi_info->endianness = bank->target->endianness;
 876
 877         /* bank wasn't probed yet */
 878         cfi_info->qry[0] = 0xff;
 879
 880         return ERROR_OK;
 881 }
 882
 883 static int cfi_intel_erase(struct flash_bank *bank, int first, int last)
 884 {
 885         int retval;
 886         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 887         int i;
 888
 889         cfi_intel_clear_status_register(bank);
 890
 891         for (i = first; i <= last; i++) {
 892                 retval = cfi_send_command(bank, 0x20, flash_address(bank, i, 0x0));
 893                 if (retval != ERROR_OK)
 894                         return retval;
 895
 896                 retval = cfi_send_command(bank, 0xd0, flash_address(bank, i, 0x0));
 897                 if (retval != ERROR_OK)
 898                         return retval;
 899
 900                 uint8_t status;
 901                 retval = cfi_intel_wait_status_busy(bank, cfi_info->block_erase_timeout, &status);
 902                 if (retval != ERROR_OK)
 903                         return retval;
 904
 905                 if (status == 0x80)
 906                         bank->sectors[i].is_erased = 1;
 907                 else {
 908                         retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
 909                         if (retval != ERROR_OK)
 910                                 return retval;
 911
 912                         LOG_ERROR("couldn't erase block %i of flash bank at base "
 913                                         TARGET_ADDR_FMT, i, bank->base);
 914                         return ERROR_FLASH_OPERATION_FAILED;
 915                 }
 916         }
 917
 918         return cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
 919 }
 920
 921 static int cfi_spansion_unlock_seq(struct flash_bank *bank)
 922 {
 923         int retval;
 924         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 925         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 926
 927         retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, pri_ext->_unlock1));
 928         if (retval != ERROR_OK)
 929                 return retval;
 930
 931         retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, pri_ext->_unlock2));
 932         if (retval != ERROR_OK)
 933                 return retval;
 934
 935         return ERROR_OK;
 936 }
 937
 938 static int cfi_spansion_erase(struct flash_bank *bank, int first, int last)
 939 {
 940         int retval;
 941         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 942         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 943         int i;
 944
 945         for (i = first; i <= last; i++) {
 946                 retval = cfi_spansion_unlock_seq(bank);
 947                 if (retval != ERROR_OK)
 948                         return retval;
 949
 950                 retval = cfi_send_command(bank, 0x80, flash_address(bank, 0, pri_ext->_unlock1));
 951                 if (retval != ERROR_OK)
 952                         return retval;
 953
 954                 retval = cfi_spansion_unlock_seq(bank);
 955                 if (retval != ERROR_OK)
 956                         return retval;
 957
 958                 retval = cfi_send_command(bank, 0x30, flash_address(bank, i, 0x0));
 959                 if (retval != ERROR_OK)
 960                         return retval;
 961
 962                 if (cfi_spansion_wait_status_busy(bank, cfi_info->block_erase_timeout) == ERROR_OK)
 963                         bank->sectors[i].is_erased = 1;
 964                 else {
 965                         retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 966                         if (retval != ERROR_OK)
 967                                 return retval;
 968
 969                         LOG_ERROR("couldn't erase block %i of flash bank at base "
 970                                 TARGET_ADDR_FMT, i, bank->base);
 971                         return ERROR_FLASH_OPERATION_FAILED;
 972                 }
 973         }
 974
 975         return cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
 976 }
 977
 978 static int cfi_erase(struct flash_bank *bank, int first, int last)
 979 {
 980         struct cfi_flash_bank *cfi_info = bank->driver_priv;
 981
 982         if (bank->target->state != TARGET_HALTED) {
 983                 LOG_ERROR("Target not halted");
 984                 return ERROR_TARGET_NOT_HALTED;
 985         }
 986
 987         if ((first < 0) || (last < first) || (last >= bank->num_sectors))
 988                 return ERROR_FLASH_SECTOR_INVALID;
 989
 990         if (cfi_info->qry[0] != 'Q')
 991                 return ERROR_FLASH_BANK_NOT_PROBED;
 992
 993         switch (cfi_info->pri_id) {
 994                 case 1:
 995                 case 3:
 996                         return cfi_intel_erase(bank, first, last);
 997                         break;
 998                 case 2:
 999                         return cfi_spansion_erase(bank, first, last);
1000                         break;
1001                 default:
1002                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
1003                         break;
1004         }
1005
1006         return ERROR_OK;
1007 }
1008
1009 static int cfi_intel_protect(struct flash_bank *bank, int set, int first, int last)
1010 {
1011         int retval;
1012         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1013         struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
1014         int retry = 0;
1015         int i;
1016
1017         /* if the device supports neither legacy lock/unlock (bit 3) nor
1018          * instant individual block locking (bit 5).
1019          */
1020         if (!(pri_ext->feature_support & 0x28)) {
1021                 LOG_ERROR("lock/unlock not supported on flash");
1022                 return ERROR_FLASH_OPERATION_FAILED;
1023         }
1024
1025         cfi_intel_clear_status_register(bank);
1026
1027         for (i = first; i <= last; i++) {
1028                 retval = cfi_send_command(bank, 0x60, flash_address(bank, i, 0x0));
1029                 if (retval != ERROR_OK)
1030                         return retval;
1031                 if (set) {
1032                         retval = cfi_send_command(bank, 0x01, flash_address(bank, i, 0x0));
1033                         if (retval != ERROR_OK)
1034                                 return retval;
1035                         bank->sectors[i].is_protected = 1;
1036                 } else {
1037                         retval = cfi_send_command(bank, 0xd0, flash_address(bank, i, 0x0));
1038                         if (retval != ERROR_OK)
1039                                 return retval;
1040                         bank->sectors[i].is_protected = 0;
1041                 }
1042
1043                 /* instant individual block locking doesn't require reading of the status register
1044                  **/
1045                 if (!(pri_ext->feature_support & 0x20)) {
1046                         /* Clear lock bits operation may take up to 1.4s */
1047                         uint8_t status;
1048                         retval = cfi_intel_wait_status_busy(bank, 1400, &status);
1049                         if (retval != ERROR_OK)
1050                                 return retval;
1051                 } else {
1052                         uint8_t block_status;
1053                         /* read block lock bit, to verify status */
1054                         retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, 0x55));
1055                         if (retval != ERROR_OK)
1056                                 return retval;
1057                         retval = cfi_get_u8(bank, i, 0x2, &block_status);
1058                         if (retval != ERROR_OK)
1059                                 return retval;
1060
1061                         if ((block_status & 0x1) != set) {
1062                                 LOG_ERROR(
1063                                         "couldn't change block lock status (set = %i, block_status = 0x%2.2x)",
1064                                         set, block_status);
1065                                 retval = cfi_send_command(bank, 0x70, flash_address(bank, 0, 0x55));
1066                                 if (retval != ERROR_OK)
1067                                         return retval;
1068                                 uint8_t status;
1069                                 retval = cfi_intel_wait_status_busy(bank, 10, &status);
1070                                 if (retval != ERROR_OK)
1071                                         return retval;
1072
1073                                 if (retry > 10)
1074                                         return ERROR_FLASH_OPERATION_FAILED;
1075                                 else {
1076                                         i--;
1077                                         retry++;
1078                                 }
1079                         }
1080                 }
1081         }
1082
1083         /* if the device doesn't support individual block lock bits set/clear,
1084          * all blocks have been unlocked in parallel, so we set those that should be protected
1085          */
1086         if ((!set) && (!(pri_ext->feature_support & 0x20))) {
1087                 /* FIX!!! this code path is broken!!!
1088                  *
1089                  * The correct approach is:
1090                  *
1091                  * 1. read out current protection status
1092                  *
1093                  * 2. override read out protection status w/unprotected.
1094                  *
1095                  * 3. re-protect what should be protected.
1096                  *
1097                  */
1098                 for (i = 0; i < bank->num_sectors; i++) {
1099                         if (bank->sectors[i].is_protected == 1) {
1100                                 cfi_intel_clear_status_register(bank);
1101
1102                                 retval = cfi_send_command(bank, 0x60, flash_address(bank, i, 0x0));
1103                                 if (retval != ERROR_OK)
1104                                         return retval;
1105
1106                                 retval = cfi_send_command(bank, 0x01, flash_address(bank, i, 0x0));
1107                                 if (retval != ERROR_OK)
1108                                         return retval;
1109
1110                                 uint8_t status;
1111                                 retval = cfi_intel_wait_status_busy(bank, 100, &status);
1112                                 if (retval != ERROR_OK)
1113                                         return retval;
1114                         }
1115                 }
1116         }
1117
1118         return cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
1119 }
1120
1121 static int cfi_protect(struct flash_bank *bank, int set, int first, int last)
1122 {
1123         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1124
1125         if (bank->target->state != TARGET_HALTED) {
1126                 LOG_ERROR("Target not halted");
1127                 return ERROR_TARGET_NOT_HALTED;
1128         }
1129
1130         if (cfi_info->qry[0] != 'Q')
1131                 return ERROR_FLASH_BANK_NOT_PROBED;
1132
1133         switch (cfi_info->pri_id) {
1134                 case 1:
1135                 case 3:
1136                         return cfi_intel_protect(bank, set, first, last);
1137                         break;
1138                 default:
1139                         LOG_WARNING("protect: cfi primary command set %i unsupported", cfi_info->pri_id);
1140                         return ERROR_OK;
1141         }
1142 }
1143
1144 static uint32_t cfi_command_val(struct flash_bank *bank, uint8_t cmd)
1145 {
1146         struct target *target = bank->target;
1147
1148         uint8_t buf[CFI_MAX_BUS_WIDTH];
1149         cfi_command(bank, cmd, buf);
1150         switch (bank->bus_width) {
1151                 case 1:
1152                         return buf[0];
1153                         break;
1154                 case 2:
1155                         return target_buffer_get_u16(target, buf);
1156                         break;
1157                 case 4:
1158                         return target_buffer_get_u32(target, buf);
1159                         break;
1160                 default:
1161                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1162                                         bank->bus_width);
1163                         return 0;
1164         }
1165 }
1166
1167 static int cfi_intel_write_block(struct flash_bank *bank, const uint8_t *buffer,
1168         uint32_t address, uint32_t count)
1169 {
1170         struct target *target = bank->target;
1171         struct reg_param reg_params[7];
1172         struct arm_algorithm arm_algo;
1173         struct working_area *write_algorithm;
1174         struct working_area *source = NULL;
1175         uint32_t buffer_size = 32768;
1176         uint32_t write_command_val, busy_pattern_val, error_pattern_val;
1177
1178         /* algorithm register usage:
1179          * r0: source address (in RAM)
1180          * r1: target address (in Flash)
1181          * r2: count
1182          * r3: flash write command
1183          * r4: status byte (returned to host)
1184          * r5: busy test pattern
1185          * r6: error test pattern
1186          */
1187
1188         /* see contib/loaders/flash/armv4_5_cfi_intel_32.s for src */
1189         static const uint32_t word_32_code[] = {
1190                 0xe4904004,     /* loop: ldr r4, [r0], #4 */
1191                 0xe5813000,     /*       str r3, [r1] */
1192                 0xe5814000,     /*       str r4, [r1] */
1193                 0xe5914000,     /* busy: ldr r4, [r1] */
1194                 0xe0047005,     /*        and r7, r4, r5 */
1195                 0xe1570005,     /*       cmp r7, r5 */
1196                 0x1afffffb,     /*       bne busy */
1197                 0xe1140006,     /*       tst r4, r6 */
1198                 0x1a000003,     /*       bne done */
1199                 0xe2522001,     /*       subs r2, r2, #1 */
1200                 0x0a000001,     /*       beq done */
1201                 0xe2811004,     /*       add r1, r1 #4 */
1202                 0xeafffff2,     /*       b loop */
1203                 0xeafffffe      /* done: b -2 */
1204         };
1205
1206         /* see contib/loaders/flash/armv4_5_cfi_intel_16.s for src */
1207         static const uint32_t word_16_code[] = {
1208                 0xe0d040b2,     /* loop: ldrh r4, [r0], #2 */
1209                 0xe1c130b0,     /*       strh r3, [r1] */
1210                 0xe1c140b0,     /*       strh r4, [r1] */
1211                 0xe1d140b0,     /* busy  ldrh r4, [r1] */
1212                 0xe0047005,     /*       and r7, r4, r5 */
1213                 0xe1570005,     /*       cmp r7, r5 */
1214                 0x1afffffb,     /*       bne busy */
1215                 0xe1140006,     /*       tst r4, r6 */
1216                 0x1a000003,     /*       bne done */
1217                 0xe2522001,     /*       subs r2, r2, #1 */
1218                 0x0a000001,     /*       beq done */
1219                 0xe2811002,     /*       add r1, r1 #2 */
1220                 0xeafffff2,     /*       b loop */
1221                 0xeafffffe      /* done:        b -2 */
1222         };
1223
1224         /* see contib/loaders/flash/armv4_5_cfi_intel_8.s for src */
1225         static const uint32_t word_8_code[] = {
1226                 0xe4d04001,     /* loop: ldrb r4, [r0], #1 */
1227                 0xe5c13000,     /*       strb r3, [r1] */
1228                 0xe5c14000,     /*       strb r4, [r1] */
1229                 0xe5d14000,     /* busy  ldrb r4, [r1] */
1230                 0xe0047005,     /*       and r7, r4, r5 */
1231                 0xe1570005,     /*       cmp r7, r5 */
1232                 0x1afffffb,     /*       bne busy */
1233                 0xe1140006,     /*       tst r4, r6 */
1234                 0x1a000003,     /*       bne done */
1235                 0xe2522001,     /*       subs r2, r2, #1 */
1236                 0x0a000001,     /*       beq done */
1237                 0xe2811001,     /*       add r1, r1 #1 */
1238                 0xeafffff2,     /*       b loop */
1239                 0xeafffffe      /* done: b -2 */
1240         };
1241         uint8_t target_code[4*CFI_MAX_INTEL_CODESIZE];
1242         const uint32_t *target_code_src;
1243         uint32_t target_code_size;
1244         int retval = ERROR_OK;
1245
1246         /* check we have a supported arch */
1247         if (is_arm(target_to_arm(target))) {
1248                 /* All other ARM CPUs have 32 bit instructions */
1249                 arm_algo.common_magic = ARM_COMMON_MAGIC;
1250                 arm_algo.core_mode = ARM_MODE_SVC;
1251                 arm_algo.core_state = ARM_STATE_ARM;
1252         } else {
1253                 LOG_ERROR("Unknown architecture");
1254                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1255         }
1256
1257         cfi_intel_clear_status_register(bank);
1258
1259         /* If we are setting up the write_algorith, we need target_code_src
1260          * if not we only need target_code_size. */
1261
1262         /* However, we don't want to create multiple code paths, so we
1263          * do the unnecessary evaluation of target_code_src, which the
1264          * compiler will probably nicely optimize away if not needed */
1265
1266         /* prepare algorithm code for target endian */
1267         switch (bank->bus_width) {
1268                 case 1:
1269                         target_code_src = word_8_code;
1270                         target_code_size = sizeof(word_8_code);
1271                         break;
1272                 case 2:
1273                         target_code_src = word_16_code;
1274                         target_code_size = sizeof(word_16_code);
1275                         break;
1276                 case 4:
1277                         target_code_src = word_32_code;
1278                         target_code_size = sizeof(word_32_code);
1279                         break;
1280                 default:
1281                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1282                                         bank->bus_width);
1283                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1284         }
1285
1286         /* flash write code */
1287         if (target_code_size > sizeof(target_code)) {
1288                 LOG_WARNING("Internal error - target code buffer to small. "
1289                                 "Increase CFI_MAX_INTEL_CODESIZE and recompile.");
1290                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1291         }
1292
1293         target_buffer_set_u32_array(target, target_code, target_code_size / 4, target_code_src);
1294
1295         /* Get memory for block write handler */
1296         retval = target_alloc_working_area(target,
1297                         target_code_size,
1298                         &write_algorithm);
1299         if (retval != ERROR_OK) {
1300                 LOG_WARNING("No working area available, can't do block memory writes");
1301                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1302         }
1303
1304         /* write algorithm code to working area */
1305         retval = target_write_buffer(target, write_algorithm->address,
1306                         target_code_size, target_code);
1307         if (retval != ERROR_OK) {
1308                 LOG_ERROR("Unable to write block write code to target");
1309                 goto cleanup;
1310         }
1311
1312         /* Get a workspace buffer for the data to flash starting with 32k size.
1313          * Half size until buffer would be smaller 256 Bytes then fail back */
1314         /* FIXME Why 256 bytes, why not 32 bytes (smallest flash write page */
1315         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
1316                 buffer_size /= 2;
1317                 if (buffer_size <= 256) {
1318                         LOG_WARNING(
1319                                 "no large enough working area available, can't do block memory writes");
1320                         retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1321                         goto cleanup;
1322                 }
1323         }
1324
1325         /* setup algo registers */
1326         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
1327         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
1328         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
1329         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
1330         init_reg_param(&reg_params[4], "r4", 32, PARAM_IN);
1331         init_reg_param(&reg_params[5], "r5", 32, PARAM_OUT);
1332         init_reg_param(&reg_params[6], "r6", 32, PARAM_OUT);
1333
1334         /* prepare command and status register patterns */
1335         write_command_val = cfi_command_val(bank, 0x40);
1336         busy_pattern_val  = cfi_command_val(bank, 0x80);
1337         error_pattern_val = cfi_command_val(bank, 0x7e);
1338
1339         LOG_DEBUG("Using target buffer at " TARGET_ADDR_FMT " and of size 0x%04" PRIx32,
1340                 source->address, buffer_size);
1341
1342         /* Programming main loop */
1343         while (count > 0) {
1344                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
1345                 uint32_t wsm_error;
1346
1347                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
1348                 if (retval != ERROR_OK)
1349                         goto cleanup;
1350
1351                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
1352                 buf_set_u32(reg_params[1].value, 0, 32, address);
1353                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
1354
1355                 buf_set_u32(reg_params[3].value, 0, 32, write_command_val);
1356                 buf_set_u32(reg_params[5].value, 0, 32, busy_pattern_val);
1357                 buf_set_u32(reg_params[6].value, 0, 32, error_pattern_val);
1358
1359                 LOG_DEBUG("Write 0x%04" PRIx32 " bytes to flash at 0x%08" PRIx32,
1360                         thisrun_count, address);
1361
1362                 /* Execute algorithm, assume breakpoint for last instruction */
1363                 retval = target_run_algorithm(target, 0, NULL, 7, reg_params,
1364                                 write_algorithm->address,
1365                                 write_algorithm->address + target_code_size -
1366                                 sizeof(uint32_t),
1367                                 10000,  /* 10s should be enough for max. 32k of data */
1368                                 &arm_algo);
1369
1370                 /* On failure try a fall back to direct word writes */
1371                 if (retval != ERROR_OK) {
1372                         cfi_intel_clear_status_register(bank);
1373                         LOG_ERROR(
1374                                 "Execution of flash algorythm failed. Can't fall back. Please report.");
1375                         retval = ERROR_FLASH_OPERATION_FAILED;
1376                         /* retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE; */
1377                         /* FIXME To allow fall back or recovery, we must save the actual status
1378                          * somewhere, so that a higher level code can start recovery. */
1379                         goto cleanup;
1380                 }
1381
1382                 /* Check return value from algo code */
1383                 wsm_error = buf_get_u32(reg_params[4].value, 0, 32) & error_pattern_val;
1384                 if (wsm_error) {
1385                         /* read status register (outputs debug information) */
1386                         uint8_t status;
1387                         cfi_intel_wait_status_busy(bank, 100, &status);
1388                         cfi_intel_clear_status_register(bank);
1389                         retval = ERROR_FLASH_OPERATION_FAILED;
1390                         goto cleanup;
1391                 }
1392
1393                 buffer += thisrun_count;
1394                 address += thisrun_count;
1395                 count -= thisrun_count;
1396
1397                 keep_alive();
1398         }
1399
1400         /* free up resources */
1401 cleanup:
1402         if (source)
1403                 target_free_working_area(target, source);
1404
1405         target_free_working_area(target, write_algorithm);
1406
1407         destroy_reg_param(&reg_params[0]);
1408         destroy_reg_param(&reg_params[1]);
1409         destroy_reg_param(&reg_params[2]);
1410         destroy_reg_param(&reg_params[3]);
1411         destroy_reg_param(&reg_params[4]);
1412         destroy_reg_param(&reg_params[5]);
1413         destroy_reg_param(&reg_params[6]);
1414
1415         return retval;
1416 }
1417
1418 static int cfi_spansion_write_block_mips(struct flash_bank *bank, const uint8_t *buffer,
1419         uint32_t address, uint32_t count)
1420 {
1421         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1422         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
1423         struct target *target = bank->target;
1424         struct reg_param reg_params[10];
1425         struct mips32_algorithm mips32_info;
1426         struct working_area *write_algorithm;
1427         struct working_area *source;
1428         uint32_t buffer_size = 32768;
1429         uint32_t status;
1430         int retval = ERROR_OK;
1431
1432         /* input parameters -
1433          *      4  A0 = source address
1434          *      5  A1 = destination address
1435          *      6  A2 = number of writes
1436          *      7  A3 = flash write command
1437          *      8  T0 = constant to mask DQ7 bits (also used for Dq5 with shift)
1438          * output parameters -
1439          *      9  T1 = 0x80 ok 0x00 bad
1440          * temp registers -
1441          *      10 T2 = value read from flash to test status
1442          *      11 T3 = holding register
1443          * unlock registers -
1444          *  12 T4 = unlock1_addr
1445          *  13 T5 = unlock1_cmd
1446          *  14 T6 = unlock2_addr
1447          *  15 T7 = unlock2_cmd */
1448
1449         static const uint32_t mips_word_16_code[] = {
1450                 /* start:       */
1451                 MIPS32_LHU(0, 9, 0, 4),         /* lhu $t1, ($a0)               ; out = &saddr */
1452                 MIPS32_ADDI(0, 4, 4, 2),        /* addi $a0, $a0, 2             ; saddr += 2 */
1453                 MIPS32_SH(0, 13, 0, 12),        /* sh $t5, ($t4)                ; *fl_unl_addr1 = fl_unl_cmd1 */
1454                 MIPS32_SH(0, 15, 0, 14),        /* sh $t7, ($t6)                ; *fl_unl_addr2 = fl_unl_cmd2 */
1455                 MIPS32_SH(0, 7, 0, 12),         /* sh $a3, ($t4)                ; *fl_unl_addr1 = fl_write_cmd */
1456                 MIPS32_SH(0, 9, 0, 5),          /* sh $t1, ($a1)                ; *daddr = out */
1457                 MIPS32_NOP,                                             /* nop */
1458                 /* busy:        */
1459                 MIPS32_LHU(0, 10, 0, 5),                /* lhu $t2, ($a1)               ; temp1 = *daddr */
1460                 MIPS32_XOR(0, 11, 9, 10),               /* xor $t3, $a0, $t2    ; temp2 = out ^ temp1; */
1461                 MIPS32_AND(0, 11, 8, 11),               /* and $t3, $t0, $t3    ; temp2 = temp2 & DQ7mask */
1462                 MIPS32_BNE(0, 11, 8, 13),               /* bne $t3, $t0, cont   ; if (temp2 != DQ7mask) goto cont */
1463                 MIPS32_NOP,                                             /* nop                                  */
1464
1465                 MIPS32_SRL(0, 10, 8, 2),                /* srl $t2,$t0,2                ; temp1 = DQ7mask >> 2 */
1466                 MIPS32_AND(0, 11, 10, 11),                      /* and $t3, $t2, $t3    ; temp2 = temp2 & temp1 */
1467                 MIPS32_BNE(0, 11, 10, NEG16(8)),        /* bne $t3, $t2, busy   ; if (temp2 != temp1) goto busy */
1468                 MIPS32_NOP,                                             /* nop                                  */
1469
1470                 MIPS32_LHU(0, 10, 0, 5),                /* lhu $t2, ($a1)               ; temp1 = *daddr */
1471                 MIPS32_XOR(0, 11, 9, 10),               /* xor $t3, $a0, $t2    ; temp2 = out ^ temp1; */
1472                 MIPS32_AND(0, 11, 8, 11),               /* and $t3, $t0, $t3    ; temp2 = temp2 & DQ7mask */
1473                 MIPS32_BNE(0, 11, 8, 4),                /* bne $t3, $t0, cont   ; if (temp2 != DQ7mask) goto cont */
1474                 MIPS32_NOP,                                             /* nop */
1475
1476                 MIPS32_XOR(0, 9, 9, 9),                 /* xor $t1, $t1, $t1    ; out = 0 */
1477                 MIPS32_BEQ(0, 9, 0, 11),                        /* beq $t1, $zero, done ; if (out == 0) goto done */
1478                 MIPS32_NOP,                                             /* nop */
1479                 /* cont:        */
1480                 MIPS32_ADDI(0, 6, 6, NEG16(1)), /* addi, $a2, $a2, -1   ; numwrites-- */
1481                 MIPS32_BNE(0, 6, 0, 5),         /* bne $a2, $zero, cont2        ; if (numwrite != 0) goto cont2 */
1482                 MIPS32_NOP,                                             /* nop */
1483
1484                 MIPS32_LUI(0, 9, 0),                            /* lui $t1, 0 */
1485                 MIPS32_ORI(0, 9, 9, 0x80),                      /* ori $t1, $t1, 0x80   ; out = 0x80 */
1486
1487                 MIPS32_B(0, 4),                                 /* b done                       ; goto done */
1488                 MIPS32_NOP,                                             /* nop */
1489                 /* cont2:       */
1490                 MIPS32_ADDI(0, 5, 5, 2),                        /* addi $a0, $a0, 2     ; daddr += 2 */
1491                 MIPS32_B(0, NEG16(33)),                 /* b start                      ; goto start */
1492                 MIPS32_NOP,                                             /* nop */
1493                 /* done: */
1494                 MIPS32_SDBBP(0),                                        /* sdbbp                        ; break(); */
1495         };
1496
1497         mips32_info.common_magic = MIPS32_COMMON_MAGIC;
1498         mips32_info.isa_mode = MIPS32_ISA_MIPS32;
1499
1500         int target_code_size = 0;
1501         const uint32_t *target_code_src = NULL;
1502
1503         switch (bank->bus_width) {
1504                 case 2:
1505                         /* Check for DQ5 support */
1506                         if (cfi_info->status_poll_mask & (1 << 5)) {
1507                                 target_code_src = mips_word_16_code;
1508                                 target_code_size = sizeof(mips_word_16_code);
1509                         } else {
1510                                 LOG_ERROR("Need DQ5 support");
1511                                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1512                                 /* target_code_src = mips_word_16_code_dq7only; */
1513                                 /* target_code_size = sizeof(mips_word_16_code_dq7only); */
1514                         }
1515                         break;
1516                 default:
1517                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1518                                         bank->bus_width);
1519                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1520         }
1521
1522         /* flash write code */
1523         uint8_t *target_code;
1524
1525         /* convert bus-width dependent algorithm code to correct endianness */
1526         target_code = malloc(target_code_size);
1527         if (target_code == NULL) {
1528                 LOG_ERROR("Out of memory");
1529                 return ERROR_FAIL;
1530         }
1531
1532         target_buffer_set_u32_array(target, target_code, target_code_size / 4, target_code_src);
1533
1534         /* allocate working area */
1535         retval = target_alloc_working_area(target, target_code_size,
1536                         &write_algorithm);
1537         if (retval != ERROR_OK) {
1538                 free(target_code);
1539                 return retval;
1540         }
1541
1542         /* write algorithm code to working area */
1543         retval = target_write_buffer(target, write_algorithm->address,
1544                         target_code_size, target_code);
1545         if (retval != ERROR_OK) {
1546                 free(target_code);
1547                 return retval;
1548         }
1549
1550         free(target_code);
1551
1552         /* the following code still assumes target code is fixed 24*4 bytes */
1553
1554         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
1555                 buffer_size /= 2;
1556                 if (buffer_size <= 256) {
1557                         /* we already allocated the writing code, but failed to get a
1558                          * buffer, free the algorithm */
1559                         target_free_working_area(target, write_algorithm);
1560
1561                         LOG_WARNING(
1562                                 "not enough working area available, can't do block memory writes");
1563                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1564                 }
1565         }
1566
1567         init_reg_param(&reg_params[0], "r4", 32, PARAM_OUT);
1568         init_reg_param(&reg_params[1], "r5", 32, PARAM_OUT);
1569         init_reg_param(&reg_params[2], "r6", 32, PARAM_OUT);
1570         init_reg_param(&reg_params[3], "r7", 32, PARAM_OUT);
1571         init_reg_param(&reg_params[4], "r8", 32, PARAM_OUT);
1572         init_reg_param(&reg_params[5], "r9", 32, PARAM_IN);
1573         init_reg_param(&reg_params[6], "r12", 32, PARAM_OUT);
1574         init_reg_param(&reg_params[7], "r13", 32, PARAM_OUT);
1575         init_reg_param(&reg_params[8], "r14", 32, PARAM_OUT);
1576         init_reg_param(&reg_params[9], "r15", 32, PARAM_OUT);
1577
1578         while (count > 0) {
1579                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
1580
1581                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
1582                 if (retval != ERROR_OK)
1583                         break;
1584
1585                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
1586                 buf_set_u32(reg_params[1].value, 0, 32, address);
1587                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
1588                 buf_set_u32(reg_params[3].value, 0, 32, cfi_command_val(bank, 0xA0));
1589                 buf_set_u32(reg_params[4].value, 0, 32, cfi_command_val(bank, 0x80));
1590                 buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
1591                 buf_set_u32(reg_params[7].value, 0, 32, 0xaaaaaaaa);
1592                 buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
1593                 buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
1594
1595                 retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
1596                                 write_algorithm->address,
1597                                 write_algorithm->address + ((target_code_size) - 4),
1598                                 10000, &mips32_info);
1599                 if (retval != ERROR_OK)
1600                         break;
1601
1602                 status = buf_get_u32(reg_params[5].value, 0, 32);
1603                 if (status != 0x80) {
1604                         LOG_ERROR("flash write block failed status: 0x%" PRIx32, status);
1605                         retval = ERROR_FLASH_OPERATION_FAILED;
1606                         break;
1607                 }
1608
1609                 buffer += thisrun_count;
1610                 address += thisrun_count;
1611                 count -= thisrun_count;
1612         }
1613
1614         target_free_all_working_areas(target);
1615
1616         destroy_reg_param(&reg_params[0]);
1617         destroy_reg_param(&reg_params[1]);
1618         destroy_reg_param(&reg_params[2]);
1619         destroy_reg_param(&reg_params[3]);
1620         destroy_reg_param(&reg_params[4]);
1621         destroy_reg_param(&reg_params[5]);
1622         destroy_reg_param(&reg_params[6]);
1623         destroy_reg_param(&reg_params[7]);
1624         destroy_reg_param(&reg_params[8]);
1625         destroy_reg_param(&reg_params[9]);
1626
1627         return retval;
1628 }
1629
1630 static int cfi_spansion_write_block(struct flash_bank *bank, const uint8_t *buffer,
1631         uint32_t address, uint32_t count)
1632 {
1633         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1634         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
1635         struct target *target = bank->target;
1636         struct reg_param reg_params[10];
1637         void *arm_algo;
1638         struct arm_algorithm armv4_5_algo;
1639         struct armv7m_algorithm armv7m_algo;
1640         struct working_area *write_algorithm;
1641         struct working_area *source;
1642         uint32_t buffer_size = 32768;
1643         uint32_t status;
1644         int retval = ERROR_OK;
1645
1646         /* input parameters -
1647          *      R0 = source address
1648          *      R1 = destination address
1649          *      R2 = number of writes
1650          *      R3 = flash write command
1651          *      R4 = constant to mask DQ7 bits (also used for Dq5 with shift)
1652          * output parameters -
1653          *      R5 = 0x80 ok 0x00 bad
1654          * temp registers -
1655          *      R6 = value read from flash to test status
1656          *      R7 = holding register
1657          * unlock registers -
1658          *  R8 = unlock1_addr
1659          *  R9 = unlock1_cmd
1660          *  R10 = unlock2_addr
1661          *  R11 = unlock2_cmd */
1662
1663         /* see contib/loaders/flash/armv4_5_cfi_span_32.s for src */
1664         static const uint32_t armv4_5_word_32_code[] = {
1665                 /* 00008100 <sp_32_code>:               */
1666                 0xe4905004,             /* ldr  r5, [r0], #4                    */
1667                 0xe5889000,             /* str  r9, [r8]                                */
1668                 0xe58ab000,             /* str  r11, [r10]                              */
1669                 0xe5883000,             /* str  r3, [r8]                                */
1670                 0xe5815000,             /* str  r5, [r1]                                */
1671                 0xe1a00000,             /* nop                                                  */
1672                 /* 00008110 <sp_32_busy>:               */
1673                 0xe5916000,             /* ldr  r6, [r1]                                */
1674                 0xe0257006,             /* eor  r7, r5, r6                              */
1675                 0xe0147007,             /* ands r7, r4, r7                              */
1676                 0x0a000007,             /* beq  8140 <sp_32_cont> ; b if DQ7 == Data7 */
1677                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1678                 0x0afffff9,             /* beq  8110 <sp_32_busy> ;     b if DQ5 low */
1679                 0xe5916000,             /* ldr  r6, [r1]                                */
1680                 0xe0257006,             /* eor  r7, r5, r6                              */
1681                 0xe0147007,             /* ands r7, r4, r7                              */
1682                 0x0a000001,             /* beq  8140 <sp_32_cont> ; b if DQ7 == Data7 */
1683                 0xe3a05000,             /* mov  r5, #0  ; 0x0 - return 0x00, error */
1684                 0x1a000004,             /* bne  8154 <sp_32_done>               */
1685                 /* 00008140 <sp_32_cont>:               */
1686                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1687                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1688                 0x0a000001,             /* beq  8154 <sp_32_done>               */
1689                 0xe2811004,             /* add  r1, r1, #4      ; 0x4           */
1690                 0xeaffffe8,             /* b    8100 <sp_32_code>               */
1691                 /* 00008154 <sp_32_done>:               */
1692                 0xeafffffe              /* b    8154 <sp_32_done>               */
1693         };
1694
1695         /* see contib/loaders/flash/armv4_5_cfi_span_16.s for src */
1696         static const uint32_t armv4_5_word_16_code[] = {
1697                 /* 00008158 <sp_16_code>:               */
1698                 0xe0d050b2,             /* ldrh r5, [r0], #2                    */
1699                 0xe1c890b0,             /* strh r9, [r8]                                */
1700                 0xe1cab0b0,             /* strh r11, [r10]                              */
1701                 0xe1c830b0,             /* strh r3, [r8]                                */
1702                 0xe1c150b0,             /* strh r5, [r1]                                */
1703                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1704                 /* 00008168 <sp_16_busy>:               */
1705                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1706                 0xe0257006,             /* eor  r7, r5, r6                              */
1707                 0xe0147007,             /* ands r7, r4, r7                              */
1708                 0x0a000007,             /* beq  8198 <sp_16_cont>               */
1709                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1710                 0x0afffff9,             /* beq  8168 <sp_16_busy>               */
1711                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1712                 0xe0257006,             /* eor  r7, r5, r6                              */
1713                 0xe0147007,             /* ands r7, r4, r7                              */
1714                 0x0a000001,             /* beq  8198 <sp_16_cont>               */
1715                 0xe3a05000,             /* mov  r5, #0  ; 0x0                   */
1716                 0x1a000004,             /* bne  81ac <sp_16_done>               */
1717                 /* 00008198 <sp_16_cont>:               */
1718                 0xe2522001,     /* subs r2, r2, #1      ; 0x1           */
1719                 0x03a05080,     /* moveq        r5, #128        ; 0x80  */
1720                 0x0a000001,     /* beq  81ac <sp_16_done>               */
1721                 0xe2811002,     /* add  r1, r1, #2      ; 0x2           */
1722                 0xeaffffe8,     /* b    8158 <sp_16_code>               */
1723                 /* 000081ac <sp_16_done>:               */
1724                 0xeafffffe              /* b    81ac <sp_16_done>               */
1725         };
1726
1727         /* see contrib/loaders/flash/armv7m_cfi_span_16.s for src */
1728         static const uint32_t armv7m_word_16_code[] = {
1729                 0x5B02F830,
1730                 0x9000F8A8,
1731                 0xB000F8AA,
1732                 0x3000F8A8,
1733                 0xBF00800D,
1734                 0xEA85880E,
1735                 0x40270706,
1736                 0xEA16D00A,
1737                 0xD0F70694,
1738                 0xEA85880E,
1739                 0x40270706,
1740                 0xF04FD002,
1741                 0xD1070500,
1742                 0xD0023A01,
1743                 0x0102F101,
1744                 0xF04FE7E0,
1745                 0xE7FF0580,
1746                 0x0000BE00
1747         };
1748
1749         /* see contrib/loaders/flash/armv7m_cfi_span_16_dq7.s for src */
1750         static const uint32_t armv7m_word_16_code_dq7only[] = {
1751                 /* 00000000 <code>: */
1752                 0x5B02F830,             /* ldrh.w       r5, [r0], #2    */
1753                 0x9000F8A8,             /* strh.w       r9, [r8]                */
1754                 0xB000F8AA,             /* strh.w       fp, [sl]                */
1755                 0x3000F8A8,             /* strh.w       r3, [r8]                */
1756                 0xBF00800D,             /* strh r5, [r1, #0]            */
1757                                                 /* nop                                          */
1758
1759                 /* 00000014 <busy>: */
1760                 0xEA85880E,             /* ldrh r6, [r1, #0]            */
1761                                                 /* eor.w        r7, r5, r6              */
1762                 0x40270706,             /* ands         r7, r4                  */
1763                 0x3A01D1FA,             /* bne.n        14 <busy>               */
1764                                                 /* subs r2, #1                          */
1765                 0xF101D002,             /* beq.n        28 <success>    */
1766                 0xE7EB0102,             /* add.w        r1, r1, #2              */
1767                                                 /* b.n  0 <code>                        */
1768
1769                 /* 00000028 <success>: */
1770                 0x0580F04F,             /* mov.w        r5, #128                */
1771                 0xBF00E7FF,             /* b.n  30 <done>                       */
1772                                                 /* nop (for alignment purposes) */
1773
1774                 /* 00000030 <done>: */
1775                 0x0000BE00              /* bkpt 0x0000                          */
1776         };
1777
1778         /* see contrib/loaders/flash/armv4_5_cfi_span_16_dq7.s for src */
1779         static const uint32_t armv4_5_word_16_code_dq7only[] = {
1780                 /* <sp_16_code>:                                */
1781                 0xe0d050b2,             /* ldrh r5, [r0], #2                    */
1782                 0xe1c890b0,             /* strh r9, [r8]                                */
1783                 0xe1cab0b0,             /* strh r11, [r10]                              */
1784                 0xe1c830b0,             /* strh r3, [r8]                                */
1785                 0xe1c150b0,             /* strh r5, [r1]                                */
1786                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1787                 /* <sp_16_busy>:                                */
1788                 0xe1d160b0,             /* ldrh r6, [r1]                                */
1789                 0xe0257006,             /* eor  r7, r5, r6                              */
1790                 0xe2177080,             /* ands r7, #0x80                               */
1791                 0x1afffffb,             /* bne  8168 <sp_16_busy>               */
1792                 /*                                                              */
1793                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1794                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1795                 0x0a000001,             /* beq  81ac <sp_16_done>               */
1796                 0xe2811002,             /* add  r1, r1, #2      ; 0x2           */
1797                 0xeafffff0,             /* b    8158 <sp_16_code>               */
1798                 /* 000081ac <sp_16_done>:               */
1799                 0xeafffffe              /* b    81ac <sp_16_done>               */
1800         };
1801
1802         /* see contrib/loaders/flash/armv4_5_cfi_span_8.s for src */
1803         static const uint32_t armv4_5_word_8_code[] = {
1804                 /* 000081b0 <sp_16_code_end>:   */
1805                 0xe4d05001,             /* ldrb r5, [r0], #1                    */
1806                 0xe5c89000,             /* strb r9, [r8]                                */
1807                 0xe5cab000,             /* strb r11, [r10]                              */
1808                 0xe5c83000,             /* strb r3, [r8]                                */
1809                 0xe5c15000,             /* strb r5, [r1]                                */
1810                 0xe1a00000,             /* nop                  (mov r0,r0)             */
1811                 /* 000081c0 <sp_8_busy>:                */
1812                 0xe5d16000,             /* ldrb r6, [r1]                                */
1813                 0xe0257006,             /* eor  r7, r5, r6                              */
1814                 0xe0147007,             /* ands r7, r4, r7                              */
1815                 0x0a000007,             /* beq  81f0 <sp_8_cont>                */
1816                 0xe0166124,             /* ands r6, r6, r4, lsr #2              */
1817                 0x0afffff9,             /* beq  81c0 <sp_8_busy>                */
1818                 0xe5d16000,             /* ldrb r6, [r1]                                */
1819                 0xe0257006,             /* eor  r7, r5, r6                              */
1820                 0xe0147007,             /* ands r7, r4, r7                              */
1821                 0x0a000001,             /* beq  81f0 <sp_8_cont>                */
1822                 0xe3a05000,             /* mov  r5, #0  ; 0x0                   */
1823                 0x1a000004,             /* bne  8204 <sp_8_done>                */
1824                 /* 000081f0 <sp_8_cont>:                */
1825                 0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
1826                 0x03a05080,             /* moveq        r5, #128        ; 0x80  */
1827                 0x0a000001,             /* beq  8204 <sp_8_done>                */
1828                 0xe2811001,             /* add  r1, r1, #1      ; 0x1           */
1829                 0xeaffffe8,             /* b    81b0 <sp_16_code_end>   */
1830                 /* 00008204 <sp_8_done>:                */
1831                 0xeafffffe              /* b    8204 <sp_8_done>                */
1832         };
1833
1834         if (strncmp(target_type_name(target), "mips_m4k", 8) == 0)
1835                 return cfi_spansion_write_block_mips(bank, buffer, address, count);
1836
1837         if (is_armv7m(target_to_armv7m(target))) {      /* armv7m target */
1838                 armv7m_algo.common_magic = ARMV7M_COMMON_MAGIC;
1839                 armv7m_algo.core_mode = ARM_MODE_THREAD;
1840                 arm_algo = &armv7m_algo;
1841         } else if (is_arm(target_to_arm(target))) {
1842                 /* All other ARM CPUs have 32 bit instructions */
1843                 armv4_5_algo.common_magic = ARM_COMMON_MAGIC;
1844                 armv4_5_algo.core_mode = ARM_MODE_SVC;
1845                 armv4_5_algo.core_state = ARM_STATE_ARM;
1846                 arm_algo = &armv4_5_algo;
1847         } else {
1848                 LOG_ERROR("Unknown architecture");
1849                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1850         }
1851
1852         int target_code_size = 0;
1853         const uint32_t *target_code_src = NULL;
1854
1855         switch (bank->bus_width) {
1856                 case 1:
1857                         if (is_armv7m(target_to_armv7m(target))) {
1858                                 LOG_ERROR("Unknown ARM architecture");
1859                                 return ERROR_FAIL;
1860                         }
1861                         target_code_src = armv4_5_word_8_code;
1862                         target_code_size = sizeof(armv4_5_word_8_code);
1863                         break;
1864                 case 2:
1865                         /* Check for DQ5 support */
1866                         if (cfi_info->status_poll_mask & (1 << 5)) {
1867                                 if (is_armv7m(target_to_armv7m(target))) {
1868                                         /* armv7m target */
1869                                         target_code_src = armv7m_word_16_code;
1870                                         target_code_size = sizeof(armv7m_word_16_code);
1871                                 } else { /* armv4_5 target */
1872                                         target_code_src = armv4_5_word_16_code;
1873                                         target_code_size = sizeof(armv4_5_word_16_code);
1874                                 }
1875                         } else {
1876                                 /* No DQ5 support. Use DQ7 DATA# polling only. */
1877                                 if (is_armv7m(target_to_armv7m(target))) {
1878                                         /* armv7m target */
1879                                         target_code_src = armv7m_word_16_code_dq7only;
1880                                         target_code_size = sizeof(armv7m_word_16_code_dq7only);
1881                                 } else { /* armv4_5 target */
1882                                         target_code_src = armv4_5_word_16_code_dq7only;
1883                                         target_code_size = sizeof(armv4_5_word_16_code_dq7only);
1884                                 }
1885                         }
1886                         break;
1887                 case 4:
1888                         if (is_armv7m(target_to_armv7m(target))) {
1889                                 LOG_ERROR("Unknown ARM architecture");
1890                                 return ERROR_FAIL;
1891                         }
1892                         target_code_src = armv4_5_word_32_code;
1893                         target_code_size = sizeof(armv4_5_word_32_code);
1894                         break;
1895                 default:
1896                         LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes",
1897                                         bank->bus_width);
1898                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1899         }
1900
1901         /* flash write code */
1902         uint8_t *target_code;
1903
1904         /* convert bus-width dependent algorithm code to correct endianness */
1905         target_code = malloc(target_code_size);
1906         if (target_code == NULL) {
1907                 LOG_ERROR("Out of memory");
1908                 return ERROR_FAIL;
1909         }
1910
1911         target_buffer_set_u32_array(target, target_code, target_code_size / 4, target_code_src);
1912
1913         /* allocate working area */
1914         retval = target_alloc_working_area(target, target_code_size,
1915                         &write_algorithm);
1916         if (retval != ERROR_OK) {
1917                 free(target_code);
1918                 return retval;
1919         }
1920
1921         /* write algorithm code to working area */
1922         retval = target_write_buffer(target, write_algorithm->address,
1923                         target_code_size, target_code);
1924         if (retval != ERROR_OK) {
1925                 free(target_code);
1926                 return retval;
1927         }
1928
1929         free(target_code);
1930
1931         /* the following code still assumes target code is fixed 24*4 bytes */
1932
1933         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
1934                 buffer_size /= 2;
1935                 if (buffer_size <= 256) {
1936                         /* we already allocated the writing code, but failed to get a
1937                          * buffer, free the algorithm */
1938                         target_free_working_area(target, write_algorithm);
1939
1940                         LOG_WARNING(
1941                                 "not enough working area available, can't do block memory writes");
1942                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1943                 }
1944         }
1945
1946         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
1947         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
1948         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
1949         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
1950         init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);
1951         init_reg_param(&reg_params[5], "r5", 32, PARAM_IN);
1952         init_reg_param(&reg_params[6], "r8", 32, PARAM_OUT);
1953         init_reg_param(&reg_params[7], "r9", 32, PARAM_OUT);
1954         init_reg_param(&reg_params[8], "r10", 32, PARAM_OUT);
1955         init_reg_param(&reg_params[9], "r11", 32, PARAM_OUT);
1956
1957         while (count > 0) {
1958                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
1959
1960                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
1961                 if (retval != ERROR_OK)
1962                         break;
1963
1964                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
1965                 buf_set_u32(reg_params[1].value, 0, 32, address);
1966                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
1967                 buf_set_u32(reg_params[3].value, 0, 32, cfi_command_val(bank, 0xA0));
1968                 buf_set_u32(reg_params[4].value, 0, 32, cfi_command_val(bank, 0x80));
1969                 buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
1970                 buf_set_u32(reg_params[7].value, 0, 32, 0xaaaaaaaa);
1971                 buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
1972                 buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
1973
1974                 retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
1975                                 write_algorithm->address,
1976                                 write_algorithm->address + ((target_code_size) - 4),
1977                                 10000, arm_algo);
1978                 if (retval != ERROR_OK)
1979                         break;
1980
1981                 status = buf_get_u32(reg_params[5].value, 0, 32);
1982                 if (status != 0x80) {
1983                         LOG_ERROR("flash write block failed status: 0x%" PRIx32, status);
1984                         retval = ERROR_FLASH_OPERATION_FAILED;
1985                         break;
1986                 }
1987
1988                 buffer += thisrun_count;
1989                 address += thisrun_count;
1990                 count -= thisrun_count;
1991         }
1992
1993         target_free_all_working_areas(target);
1994
1995         destroy_reg_param(&reg_params[0]);
1996         destroy_reg_param(&reg_params[1]);
1997         destroy_reg_param(&reg_params[2]);
1998         destroy_reg_param(&reg_params[3]);
1999         destroy_reg_param(&reg_params[4]);
2000         destroy_reg_param(&reg_params[5]);
2001         destroy_reg_param(&reg_params[6]);
2002         destroy_reg_param(&reg_params[7]);
2003         destroy_reg_param(&reg_params[8]);
2004         destroy_reg_param(&reg_params[9]);
2005
2006         return retval;
2007 }
2008
2009 static int cfi_intel_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2010 {
2011         int retval;
2012         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2013
2014         cfi_intel_clear_status_register(bank);
2015         retval = cfi_send_command(bank, 0x40, address);
2016         if (retval != ERROR_OK)
2017                 return retval;
2018
2019         retval = cfi_target_write_memory(bank, address, 1, word);
2020         if (retval != ERROR_OK)
2021                 return retval;
2022
2023         uint8_t status;
2024         retval = cfi_intel_wait_status_busy(bank, cfi_info->word_write_timeout, &status);
2025         if (retval != ERROR_OK)
2026                 return retval;
2027         if (status != 0x80) {
2028                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2029                 if (retval != ERROR_OK)
2030                         return retval;
2031
2032                 LOG_ERROR("couldn't write word at base " TARGET_ADDR_FMT
2033                                 ", address 0x%" PRIx32,
2034                                 bank->base, address);
2035                 return ERROR_FLASH_OPERATION_FAILED;
2036         }
2037
2038         return ERROR_OK;
2039 }
2040
2041 static int cfi_intel_write_words(struct flash_bank *bank, const uint8_t *word,
2042         uint32_t wordcount, uint32_t address)
2043 {
2044         int retval;
2045         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2046
2047         /* Calculate buffer size and boundary mask
2048          * buffersize is (buffer size per chip) * (number of chips)
2049          * bufferwsize is buffersize in words */
2050         uint32_t buffersize =
2051                 (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2052         uint32_t buffermask = buffersize-1;
2053         uint32_t bufferwsize = buffersize / bank->bus_width;
2054
2055         /* Check for valid range */
2056         if (address & buffermask) {
2057                 LOG_ERROR("Write address at base " TARGET_ADDR_FMT ", address 0x%"
2058                                 PRIx32 " not aligned to 2^%d boundary",
2059                                 bank->base, address, cfi_info->max_buf_write_size);
2060                 return ERROR_FLASH_OPERATION_FAILED;
2061         }
2062
2063         /* Check for valid size */
2064         if (wordcount > bufferwsize) {
2065                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %" PRId32,
2066                         wordcount, buffersize);
2067                 return ERROR_FLASH_OPERATION_FAILED;
2068         }
2069
2070         /* Write to flash buffer */
2071         cfi_intel_clear_status_register(bank);
2072
2073         /* Initiate buffer operation _*/
2074         retval = cfi_send_command(bank, 0xe8, address);
2075         if (retval != ERROR_OK)
2076                 return retval;
2077         uint8_t status;
2078         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
2079         if (retval != ERROR_OK)
2080                 return retval;
2081         if (status != 0x80) {
2082                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2083                 if (retval != ERROR_OK)
2084                         return retval;
2085
2086                 LOG_ERROR(
2087                         "couldn't start buffer write operation at base " TARGET_ADDR_FMT
2088                         ", address 0x%" PRIx32,
2089                         bank->base,
2090                         address);
2091                 return ERROR_FLASH_OPERATION_FAILED;
2092         }
2093
2094         /* Write buffer wordcount-1 and data words */
2095         retval = cfi_send_command(bank, bufferwsize-1, address);
2096         if (retval != ERROR_OK)
2097                 return retval;
2098
2099         retval = cfi_target_write_memory(bank, address, bufferwsize, word);
2100         if (retval != ERROR_OK)
2101                 return retval;
2102
2103         /* Commit write operation */
2104         retval = cfi_send_command(bank, 0xd0, address);
2105         if (retval != ERROR_OK)
2106                 return retval;
2107
2108         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
2109         if (retval != ERROR_OK)
2110                 return retval;
2111
2112         if (status != 0x80) {
2113                 retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2114                 if (retval != ERROR_OK)
2115                         return retval;
2116
2117                 LOG_ERROR("Buffer write at base " TARGET_ADDR_FMT
2118                         ", address 0x%" PRIx32 " failed.", bank->base, address);
2119                 return ERROR_FLASH_OPERATION_FAILED;
2120         }
2121
2122         return ERROR_OK;
2123 }
2124
2125 static int cfi_spansion_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2126 {
2127         int retval;
2128         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2129         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2130
2131         retval = cfi_spansion_unlock_seq(bank);
2132         if (retval != ERROR_OK)
2133                 return retval;
2134
2135         retval = cfi_send_command(bank, 0xa0, flash_address(bank, 0, pri_ext->_unlock1));
2136         if (retval != ERROR_OK)
2137                 return retval;
2138
2139         retval = cfi_target_write_memory(bank, address, 1, word);
2140         if (retval != ERROR_OK)
2141                 return retval;
2142
2143         if (cfi_spansion_wait_status_busy(bank, cfi_info->word_write_timeout) != ERROR_OK) {
2144                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2145                 if (retval != ERROR_OK)
2146                         return retval;
2147
2148                 LOG_ERROR("couldn't write word at base " TARGET_ADDR_FMT
2149                         ", address 0x%" PRIx32, bank->base, address);
2150                 return ERROR_FLASH_OPERATION_FAILED;
2151         }
2152
2153         return ERROR_OK;
2154 }
2155
2156 static int cfi_spansion_write_words(struct flash_bank *bank, const uint8_t *word,
2157         uint32_t wordcount, uint32_t address)
2158 {
2159         int retval;
2160         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2161
2162         /* Calculate buffer size and boundary mask
2163          * buffersize is (buffer size per chip) * (number of chips)
2164          * bufferwsize is buffersize in words */
2165         uint32_t buffersize =
2166                 (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2167         uint32_t buffermask = buffersize-1;
2168         uint32_t bufferwsize = buffersize / bank->bus_width;
2169
2170         /* Check for valid range */
2171         if (address & buffermask) {
2172                 LOG_ERROR("Write address at base " TARGET_ADDR_FMT
2173                         ", address 0x%" PRIx32 " not aligned to 2^%d boundary",
2174                         bank->base, address, cfi_info->max_buf_write_size);
2175                 return ERROR_FLASH_OPERATION_FAILED;
2176         }
2177
2178         /* Check for valid size */
2179         if (wordcount > bufferwsize) {
2180                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %"
2181                         PRId32, wordcount, buffersize);
2182                 return ERROR_FLASH_OPERATION_FAILED;
2183         }
2184
2185         /* Unlock */
2186         retval = cfi_spansion_unlock_seq(bank);
2187         if (retval != ERROR_OK)
2188                 return retval;
2189
2190         /* Buffer load command */
2191         retval = cfi_send_command(bank, 0x25, address);
2192         if (retval != ERROR_OK)
2193                 return retval;
2194
2195         /* Write buffer wordcount-1 and data words */
2196         retval = cfi_send_command(bank, bufferwsize-1, address);
2197         if (retval != ERROR_OK)
2198                 return retval;
2199
2200         retval = cfi_target_write_memory(bank, address, bufferwsize, word);
2201         if (retval != ERROR_OK)
2202                 return retval;
2203
2204         /* Commit write operation */
2205         retval = cfi_send_command(bank, 0x29, address);
2206         if (retval != ERROR_OK)
2207                 return retval;
2208
2209         if (cfi_spansion_wait_status_busy(bank, cfi_info->buf_write_timeout) != ERROR_OK) {
2210                 retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2211                 if (retval != ERROR_OK)
2212                         return retval;
2213
2214                 LOG_ERROR("couldn't write block at base " TARGET_ADDR_FMT
2215                         ", address 0x%" PRIx32 ", size 0x%" PRIx32, bank->base, address,
2216                         bufferwsize);
2217                 return ERROR_FLASH_OPERATION_FAILED;
2218         }
2219
2220         return ERROR_OK;
2221 }
2222
2223 static int cfi_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2224 {
2225         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2226
2227         switch (cfi_info->pri_id) {
2228                 case 1:
2229                 case 3:
2230                         return cfi_intel_write_word(bank, word, address);
2231                         break;
2232                 case 2:
2233                         return cfi_spansion_write_word(bank, word, address);
2234                         break;
2235                 default:
2236                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2237                         break;
2238         }
2239
2240         return ERROR_FLASH_OPERATION_FAILED;
2241 }
2242
2243 static int cfi_write_words(struct flash_bank *bank, const uint8_t *word,
2244         uint32_t wordcount, uint32_t address)
2245 {
2246         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2247
2248         if (cfi_info->buf_write_timeout_typ == 0) {
2249                 /* buffer writes are not supported */
2250                 LOG_DEBUG("Buffer Writes Not Supported");
2251                 return ERROR_FLASH_OPER_UNSUPPORTED;
2252         }
2253
2254         switch (cfi_info->pri_id) {
2255                 case 1:
2256                 case 3:
2257                         return cfi_intel_write_words(bank, word, wordcount, address);
2258                         break;
2259                 case 2:
2260                         return cfi_spansion_write_words(bank, word, wordcount, address);
2261                         break;
2262                 default:
2263                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2264                         break;
2265         }
2266
2267         return ERROR_FLASH_OPERATION_FAILED;
2268 }
2269
2270 static int cfi_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
2271 {
2272         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2273         uint32_t address = bank->base + offset;
2274         uint32_t read_p;
2275         int align;      /* number of unaligned bytes */
2276         uint8_t current_word[CFI_MAX_BUS_WIDTH];
2277         int i;
2278         int retval;
2279
2280         LOG_DEBUG("reading buffer of %i byte at 0x%8.8x",
2281                 (int)count, (unsigned)offset);
2282
2283         if (bank->target->state != TARGET_HALTED) {
2284                 LOG_ERROR("Target not halted");
2285                 return ERROR_TARGET_NOT_HALTED;
2286         }
2287
2288         if (offset + count > bank->size)
2289                 return ERROR_FLASH_DST_OUT_OF_BANK;
2290
2291         if (cfi_info->qry[0] != 'Q')
2292                 return ERROR_FLASH_BANK_NOT_PROBED;
2293
2294         /* start at the first byte of the first word (bus_width size) */
2295         read_p = address & ~(bank->bus_width - 1);
2296         align = address - read_p;
2297         if (align != 0) {
2298                 LOG_INFO("Fixup %d unaligned read head bytes", align);
2299
2300                 /* read a complete word from flash */
2301                 retval = cfi_target_read_memory(bank, read_p, 1, current_word);
2302                 if (retval != ERROR_OK)
2303                         return retval;
2304
2305                 /* take only bytes we need */
2306                 for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
2307                         *buffer++ = current_word[i];
2308
2309                 read_p += bank->bus_width;
2310         }
2311
2312         align = count / bank->bus_width;
2313         if (align) {
2314                 retval = cfi_target_read_memory(bank, read_p, align, buffer);
2315                 if (retval != ERROR_OK)
2316                         return retval;
2317
2318                 read_p += align * bank->bus_width;
2319                 buffer += align * bank->bus_width;
2320                 count -= align * bank->bus_width;
2321         }
2322
2323         if (count) {
2324                 LOG_INFO("Fixup %" PRIu32 " unaligned read tail bytes", count);
2325
2326                 /* read a complete word from flash */
2327                 retval = cfi_target_read_memory(bank, read_p, 1, current_word);
2328                 if (retval != ERROR_OK)
2329                         return retval;
2330
2331                 /* take only bytes we need */
2332                 for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
2333                         *buffer++ = current_word[i];
2334         }
2335
2336         return ERROR_OK;
2337 }
2338
2339 static int cfi_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t offset, uint32_t count)
2340 {
2341         struct cfi_flash_bank *cfi_info = bank->driver_priv;
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(bank, write_p, 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(bank, write_p, 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(bank, flash_address(bank, 0, 0x00),
2642                                         1, value_buf0);
2643         if (retval != ERROR_OK)
2644                 return retval;
2645         retval = cfi_target_read_memory(bank, flash_address(bank, 0, 0x01),
2646                                         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 };