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

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