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