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