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

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