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