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

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