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