git.sur5r.net Git - 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
1655         {
1656                 /* All other ARM CPUs have 32 bit instructions */
1657                 armv4_5_info.common_magic = ARM_COMMON_MAGIC;
1658                 armv4_5_info.core_mode = ARM_MODE_SVC;
1659                 armv4_5_info.core_state = ARM_STATE_ARM;
1660         }
1661
1662         int target_code_size = 0;
1663         const uint32_t *target_code_src = NULL;
1664
1665         switch (bank->bus_width)
1666         {
1667         case 1 :
1668                 if(armv4_5_info.common_magic == ARM_COMMON_MAGIC) /* armv4_5 target */
1669                 {
1670                         target_code_src = armv4_5_word_8_code;
1671                         target_code_size = sizeof(armv4_5_word_8_code);
1672                 }
1673                 break;
1674         case 2 :
1675                 /* Check for DQ5 support */
1676                 if( cfi_info->status_poll_mask & (1 << 5) )
1677                 {
1678                         if(armv4_5_info.common_magic == ARM_COMMON_MAGIC) /* armv4_5 target */
1679                         {
1680                                 target_code_src = armv4_5_word_16_code;
1681                                 target_code_size = sizeof(armv4_5_word_16_code);
1682                         }
1683                         else if (armv4_5_info.common_magic == ARMV7M_COMMON_MAGIC) /* cortex-m3 target */
1684                         {
1685                                 target_code_src = armv7m_word_16_code;
1686                                 target_code_size = sizeof(armv7m_word_16_code);
1687                         }
1688                 }
1689                 else
1690                 {
1691                         /* No DQ5 support. Use DQ7 DATA# polling only. */
1692                         if(armv4_5_info.common_magic == ARM_COMMON_MAGIC) // armv4_5 target
1693                         {
1694                                 target_code_src = armv4_5_word_16_code_dq7only;
1695                                 target_code_size = sizeof(armv4_5_word_16_code_dq7only);
1696                         }
1697                 }
1698                 break;
1699         case 4 :
1700                 if(armv4_5_info.common_magic == ARM_COMMON_MAGIC) // armv4_5 target
1701                 {
1702                         target_code_src = armv4_5_word_32_code;
1703                         target_code_size = sizeof(armv4_5_word_32_code);
1704                 }
1705                 break;
1706         default:
1707                 LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes", bank->bus_width);
1708                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1709         }
1710
1711         /* flash write code */
1712         if (!cfi_info->write_algorithm)
1713         {
1714                 uint8_t *target_code;
1715
1716                 /* convert bus-width dependent algorithm code to correct endiannes */
1717                 target_code = malloc(target_code_size);
1718                 if (target_code == NULL)
1719                 {
1720                         LOG_ERROR("Out of memory");
1721                         return ERROR_FAIL;
1722                 }
1723                 cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
1724
1725                 /* allocate working area */
1726                 retval = target_alloc_working_area(target, target_code_size,
1727                                 &cfi_info->write_algorithm);
1728                 if (retval != ERROR_OK)
1729                 {
1730                         free(target_code);
1731                         return retval;
1732                 }
1733
1734                 /* write algorithm code to working area */
1735                 if ((retval = target_write_buffer(target, cfi_info->write_algorithm->address,
1736                                 target_code_size, target_code)) != ERROR_OK)
1737                 {
1738                         free(target_code);
1739                         return retval;
1740                 }
1741
1742                 free(target_code);
1743         }
1744         /* the following code still assumes target code is fixed 24*4 bytes */
1745
1746         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK)
1747         {
1748                 buffer_size /= 2;
1749                 if (buffer_size <= 256)
1750                 {
1751                         /* if we already allocated the writing code, but failed to get a
1752                          * buffer, free the algorithm */
1753                         if (cfi_info->write_algorithm)
1754                                 target_free_working_area(target, cfi_info->write_algorithm);
1755
1756                         LOG_WARNING("not enough working area available, can't do block memory writes");
1757                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1758                 }
1759         };
1760
1761         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
1762         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
1763         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
1764         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
1765         init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);
1766         init_reg_param(&reg_params[5], "r5", 32, PARAM_IN);
1767         init_reg_param(&reg_params[6], "r8", 32, PARAM_OUT);
1768         init_reg_param(&reg_params[7], "r9", 32, PARAM_OUT);
1769         init_reg_param(&reg_params[8], "r10", 32, PARAM_OUT);
1770         init_reg_param(&reg_params[9], "r11", 32, PARAM_OUT);
1771
1772         while (count > 0)
1773         {
1774                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
1775
1776                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
1777                 if (retval != ERROR_OK)
1778                 {
1779                         break;
1780                 }
1781
1782                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
1783                 buf_set_u32(reg_params[1].value, 0, 32, address);
1784                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
1785                 buf_set_u32(reg_params[3].value, 0, 32, cfi_command_val(bank, 0xA0));
1786                 buf_set_u32(reg_params[4].value, 0, 32, cfi_command_val(bank, 0x80));
1787                 buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
1788                 buf_set_u32(reg_params[7].value, 0, 32, 0xaaaaaaaa);
1789                 buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
1790                 buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
1791
1792                 retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
1793                                 cfi_info->write_algorithm->address,
1794                                 cfi_info->write_algorithm->address + ((target_code_size) - 4),
1795                                 10000, &armv4_5_info);
1796                 if (retval != ERROR_OK)
1797                 {
1798                         break;
1799                 }
1800
1801                 status = buf_get_u32(reg_params[5].value, 0, 32);
1802                 if (status != 0x80)
1803                 {
1804                         LOG_ERROR("flash write block failed status: 0x%" PRIx32 , status);
1805                         retval = ERROR_FLASH_OPERATION_FAILED;
1806                         break;
1807                 }
1808
1809                 buffer += thisrun_count;
1810                 address += thisrun_count;
1811                 count -= thisrun_count;
1812         }
1813
1814         target_free_all_working_areas(target);
1815
1816         destroy_reg_param(&reg_params[0]);
1817         destroy_reg_param(&reg_params[1]);
1818         destroy_reg_param(&reg_params[2]);
1819         destroy_reg_param(&reg_params[3]);
1820         destroy_reg_param(&reg_params[4]);
1821         destroy_reg_param(&reg_params[5]);
1822         destroy_reg_param(&reg_params[6]);
1823         destroy_reg_param(&reg_params[7]);
1824         destroy_reg_param(&reg_params[8]);
1825         destroy_reg_param(&reg_params[9]);
1826
1827         return retval;
1828 }
1829
1830 static int cfi_intel_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
1831 {
1832         int retval;
1833         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1834         struct target *target = bank->target;
1835
1836         cfi_intel_clear_status_register(bank);
1837         if ((retval = cfi_send_command(bank, 0x40, address)) != ERROR_OK)
1838         {
1839                 return retval;
1840         }
1841
1842         if ((retval = target_write_memory(target, address, bank->bus_width, 1, word)) != ERROR_OK)
1843         {
1844                 return retval;
1845         }
1846
1847         uint8_t status;
1848         retval = cfi_intel_wait_status_busy(bank, cfi_info->word_write_timeout, &status);
1849         if (retval != 0x80)
1850         {
1851                 if ((retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0))) != ERROR_OK)
1852                 {
1853                         return retval;
1854                 }
1855
1856                 LOG_ERROR("couldn't write word at base 0x%" PRIx32 ", address 0x%" PRIx32,
1857                                 bank->base, address);
1858                 return ERROR_FLASH_OPERATION_FAILED;
1859         }
1860
1861         return ERROR_OK;
1862 }
1863
1864 static int cfi_intel_write_words(struct flash_bank *bank, uint8_t *word,
1865                 uint32_t wordcount, uint32_t address)
1866 {
1867         int retval;
1868         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1869         struct target *target = bank->target;
1870
1871         /* Calculate buffer size and boundary mask */
1872         /* buffersize is (buffer size per chip) * (number of chips) */
1873         /* bufferwsize is buffersize in words */
1874         uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
1875         uint32_t buffermask = buffersize-1;
1876         uint32_t bufferwsize = buffersize / bank->bus_width;
1877
1878         /* Check for valid range */
1879         if (address & buffermask)
1880         {
1881                 LOG_ERROR("Write address at base 0x%" PRIx32 ", address 0x%" PRIx32
1882                                 " not aligned to 2^%d boundary",
1883                                 bank->base, address, cfi_info->max_buf_write_size);
1884                 return ERROR_FLASH_OPERATION_FAILED;
1885         }
1886
1887         /* Check for valid size */
1888         if (wordcount > bufferwsize)
1889         {
1890                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %" PRId32,
1891                                 wordcount, buffersize);
1892                 return ERROR_FLASH_OPERATION_FAILED;
1893         }
1894
1895         /* Write to flash buffer */
1896         cfi_intel_clear_status_register(bank);
1897
1898         /* Initiate buffer operation _*/
1899         if ((retval = cfi_send_command(bank, 0xe8, address)) != ERROR_OK)
1900         {
1901                 return retval;
1902         }
1903         uint8_t status;
1904         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
1905         if (retval != ERROR_OK)
1906                 return retval;
1907         if (status != 0x80)
1908         {
1909                 if ((retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0))) != ERROR_OK)
1910                 {
1911                         return retval;
1912                 }
1913
1914                 LOG_ERROR("couldn't start buffer write operation at base 0x%" PRIx32 ", address 0x%" PRIx32,
1915                                 bank->base, address);
1916                 return ERROR_FLASH_OPERATION_FAILED;
1917         }
1918
1919         /* Write buffer wordcount-1 and data words */
1920         if ((retval = cfi_send_command(bank, bufferwsize-1, address)) != ERROR_OK)
1921         {
1922                 return retval;
1923         }
1924
1925         if ((retval = target_write_memory(target,
1926                         address, bank->bus_width, bufferwsize, word)) != ERROR_OK)
1927         {
1928                 return retval;
1929         }
1930
1931         /* Commit write operation */
1932         if ((retval = cfi_send_command(bank, 0xd0, address)) != ERROR_OK)
1933         {
1934                 return retval;
1935         }
1936
1937         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
1938         if (retval != ERROR_OK)
1939                 return retval;
1940
1941         if (status != 0x80)
1942         {
1943                 if ((retval = cfi_send_command(bank, 0xff,
1944                                 flash_address(bank, 0, 0x0))) != ERROR_OK)
1945                 {
1946                         return retval;
1947                 }
1948
1949                 LOG_ERROR("Buffer write at base 0x%" PRIx32
1950                                 ", address 0x%" PRIx32 " failed.", bank->base, address);
1951                 return ERROR_FLASH_OPERATION_FAILED;
1952         }
1953
1954         return ERROR_OK;
1955 }
1956
1957 static int cfi_spansion_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
1958 {
1959         int retval;
1960         struct cfi_flash_bank *cfi_info = bank->driver_priv;
1961         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
1962         struct target *target = bank->target;
1963
1964         if ((retval = cfi_send_command(bank, 0xaa,
1965                         flash_address(bank, 0, pri_ext->_unlock1))) != ERROR_OK)
1966         {
1967                 return retval;
1968         }
1969
1970         if ((retval = cfi_send_command(bank, 0x55,
1971                         flash_address(bank, 0, pri_ext->_unlock2))) != ERROR_OK)
1972         {
1973                 return retval;
1974         }
1975
1976         if ((retval = cfi_send_command(bank, 0xa0,
1977                         flash_address(bank, 0, pri_ext->_unlock1))) != ERROR_OK)
1978         {
1979                 return retval;
1980         }
1981
1982         if ((retval = target_write_memory(target,
1983                         address, bank->bus_width, 1, word)) != ERROR_OK)
1984         {
1985                 return retval;
1986         }
1987
1988         if (cfi_spansion_wait_status_busy(bank, cfi_info->word_write_timeout) != ERROR_OK)
1989         {
1990                 if ((retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0))) != ERROR_OK)
1991                 {
1992                         return retval;
1993                 }
1994
1995                 LOG_ERROR("couldn't write word at base 0x%" PRIx32
1996                                 ", address 0x%" PRIx32 , bank->base, address);
1997                 return ERROR_FLASH_OPERATION_FAILED;
1998         }
1999
2000         return ERROR_OK;
2001 }
2002
2003 static int cfi_spansion_write_words(struct flash_bank *bank, uint8_t *word,
2004                 uint32_t wordcount, uint32_t address)
2005 {
2006         int retval;
2007         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2008         struct target *target = bank->target;
2009         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2010
2011         /* Calculate buffer size and boundary mask */
2012         /* buffersize is (buffer size per chip) * (number of chips) */
2013         /* bufferwsize is buffersize in words */
2014         uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2015         uint32_t buffermask = buffersize-1;
2016         uint32_t bufferwsize = buffersize / bank->bus_width;
2017
2018         /* Check for valid range */
2019         if (address & buffermask)
2020         {
2021                 LOG_ERROR("Write address at base 0x%" PRIx32
2022                                 ", address 0x%" PRIx32 " not aligned to 2^%d boundary",
2023                                 bank->base, address, cfi_info->max_buf_write_size);
2024                 return ERROR_FLASH_OPERATION_FAILED;
2025         }
2026
2027         /* Check for valid size */
2028         if (wordcount > bufferwsize)
2029         {
2030                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %"
2031                                 PRId32, wordcount, buffersize);
2032                 return ERROR_FLASH_OPERATION_FAILED;
2033         }
2034
2035         /* Unlock */
2036         if ((retval = cfi_send_command(bank, 0xaa,
2037                         flash_address(bank, 0, pri_ext->_unlock1))) != ERROR_OK)
2038         {
2039                 return retval;
2040         }
2041
2042         if ((retval = cfi_send_command(bank, 0x55,
2043                         flash_address(bank, 0, pri_ext->_unlock2))) != ERROR_OK)
2044         {
2045                 return retval;
2046         }
2047
2048         /* Buffer load command */
2049         if ((retval = cfi_send_command(bank, 0x25, address)) != ERROR_OK)
2050         {
2051                 return retval;
2052         }
2053
2054         /* Write buffer wordcount-1 and data words */
2055         if ((retval = cfi_send_command(bank, bufferwsize-1, address)) != ERROR_OK)
2056         {
2057                 return retval;
2058         }
2059
2060         if ((retval = target_write_memory(target,
2061                         address, bank->bus_width, bufferwsize, word)) != ERROR_OK)
2062         {
2063                 return retval;
2064         }
2065
2066         /* Commit write operation */
2067         if ((retval = cfi_send_command(bank, 0x29, address)) != ERROR_OK)
2068         {
2069                 return retval;
2070         }
2071
2072         if (cfi_spansion_wait_status_busy(bank, cfi_info->buf_write_timeout) != ERROR_OK)
2073         {
2074                 if ((retval = cfi_send_command(bank, 0xf0,
2075                                 flash_address(bank, 0, 0x0))) != ERROR_OK)
2076                 {
2077                         return retval;
2078                 }
2079
2080                 LOG_ERROR("couldn't write block at base 0x%" PRIx32
2081                                 ", address 0x%" PRIx32 ", size 0x%" PRIx32, bank->base, address, bufferwsize);
2082                 return ERROR_FLASH_OPERATION_FAILED;
2083         }
2084
2085         return ERROR_OK;
2086 }
2087
2088 static int cfi_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2089 {
2090         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2091
2092         switch (cfi_info->pri_id)
2093         {
2094                 case 1:
2095                 case 3:
2096                         return cfi_intel_write_word(bank, word, address);
2097                         break;
2098                 case 2:
2099                         return cfi_spansion_write_word(bank, word, address);
2100                         break;
2101                 default:
2102                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2103                         break;
2104         }
2105
2106         return ERROR_FLASH_OPERATION_FAILED;
2107 }
2108
2109 static int cfi_write_words(struct flash_bank *bank, uint8_t *word,
2110                 uint32_t wordcount, uint32_t address)
2111 {
2112         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2113
2114         if (cfi_info->buf_write_timeout_typ == 0)
2115         {
2116                 /* buffer writes are not supported */
2117                 LOG_DEBUG("Buffer Writes Not Supported");
2118                 return ERROR_FLASH_OPER_UNSUPPORTED;
2119         }
2120
2121         switch (cfi_info->pri_id)
2122         {
2123                 case 1:
2124                 case 3:
2125                         return cfi_intel_write_words(bank, word, wordcount, address);
2126                         break;
2127                 case 2:
2128                         return cfi_spansion_write_words(bank, word, wordcount, address);
2129                         break;
2130                 default:
2131                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2132                         break;
2133         }
2134
2135         return ERROR_FLASH_OPERATION_FAILED;
2136 }
2137
2138 static int cfi_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
2139 {
2140         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2141         struct target *target = bank->target;
2142         uint32_t address = bank->base + offset;
2143         uint32_t read_p;
2144         int align;      /* number of unaligned bytes */
2145         uint8_t current_word[CFI_MAX_BUS_WIDTH];
2146         int i;
2147         int retval;
2148
2149         LOG_DEBUG("reading buffer of %i byte at 0x%8.8x",
2150                 (int)count, (unsigned)offset);
2151
2152         if (bank->target->state != TARGET_HALTED)
2153         {
2154                 LOG_ERROR("Target not halted");
2155                 return ERROR_TARGET_NOT_HALTED;
2156         }
2157
2158         if (offset + count > bank->size)
2159                 return ERROR_FLASH_DST_OUT_OF_BANK;
2160
2161         if (cfi_info->qry[0] != 'Q')
2162                 return ERROR_FLASH_BANK_NOT_PROBED;
2163
2164         /* start at the first byte of the first word (bus_width size) */
2165         read_p = address & ~(bank->bus_width - 1);
2166         if ((align = address - read_p) != 0)
2167         {
2168                 LOG_INFO("Fixup %d unaligned read head bytes", align);
2169
2170                 /* read a complete word from flash */
2171                 if ((retval = target_read_memory(target, read_p,
2172                                 bank->bus_width, 1, current_word)) != ERROR_OK)
2173                         return retval;
2174
2175                 /* take only bytes we need */
2176                 for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
2177                         *buffer++ = current_word[i];
2178
2179                 read_p += bank->bus_width;
2180         }
2181
2182         align = count / bank->bus_width;
2183         if (align)
2184         {
2185                 if ((retval = target_read_memory(target, read_p,
2186                                 bank->bus_width, align, buffer)) != ERROR_OK)
2187                         return retval;
2188
2189                 read_p += align * bank->bus_width;
2190                 buffer += align * bank->bus_width;
2191                 count -= align * bank->bus_width;
2192         }
2193
2194         if (count)
2195         {
2196                 LOG_INFO("Fixup %d unaligned read tail bytes", count);
2197
2198                 /* read a complete word from flash */
2199                 if ((retval = target_read_memory(target, read_p,
2200                                 bank->bus_width, 1, current_word)) != ERROR_OK)
2201                         return retval;
2202
2203                 /* take only bytes we need */
2204                 for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
2205                         *buffer++ = current_word[i];
2206         }
2207
2208         return ERROR_OK;
2209 }
2210
2211 static int cfi_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
2212 {
2213         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2214         struct target *target = bank->target;
2215         uint32_t address = bank->base + offset; /* address of first byte to be programmed */
2216         uint32_t write_p;
2217         int align;      /* number of unaligned bytes */
2218         int blk_count; /* number of bus_width bytes for block copy */
2219         uint8_t current_word[CFI_MAX_BUS_WIDTH * 4];    /* word (bus_width size) currently being programmed */
2220         int i;
2221         int retval;
2222
2223         if (bank->target->state != TARGET_HALTED)
2224         {
2225                 LOG_ERROR("Target not halted");
2226                 return ERROR_TARGET_NOT_HALTED;
2227         }
2228
2229         if (offset + count > bank->size)
2230                 return ERROR_FLASH_DST_OUT_OF_BANK;
2231
2232         if (cfi_info->qry[0] != 'Q')
2233                 return ERROR_FLASH_BANK_NOT_PROBED;
2234
2235         /* start at the first byte of the first word (bus_width size) */
2236         write_p = address & ~(bank->bus_width - 1);
2237         if ((align = address - write_p) != 0)
2238         {
2239                 LOG_INFO("Fixup %d unaligned head bytes", align);
2240
2241                 /* read a complete word from flash */
2242                 if ((retval = target_read_memory(target, write_p,
2243                                 bank->bus_width, 1, current_word)) != ERROR_OK)
2244                         return retval;
2245
2246                 /* replace only bytes that must be written */
2247                 for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
2248                         current_word[i] = *buffer++;
2249
2250                 retval = cfi_write_word(bank, current_word, write_p);
2251                 if (retval != ERROR_OK)
2252                         return retval;
2253                 write_p += bank->bus_width;
2254         }
2255
2256         /* handle blocks of bus_size aligned bytes */
2257         blk_count = count & ~(bank->bus_width - 1); /* round down, leave tail bytes */
2258         switch (cfi_info->pri_id)
2259         {
2260                 /* try block writes (fails without working area) */
2261                 case 1:
2262                 case 3:
2263                         retval = cfi_intel_write_block(bank, buffer, write_p, blk_count);
2264                         break;
2265                 case 2:
2266                         retval = cfi_spansion_write_block(bank, buffer, write_p, blk_count);
2267                         break;
2268                 default:
2269                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2270                         retval = ERROR_FLASH_OPERATION_FAILED;
2271                         break;
2272         }
2273         if (retval == ERROR_OK)
2274         {
2275                 /* Increment pointers and decrease count on succesful block write */
2276                 buffer += blk_count;
2277                 write_p += blk_count;
2278                 count -= blk_count;
2279         }
2280         else
2281         {
2282                 if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
2283                 {
2284                         /* Calculate buffer size and boundary mask */
2285                         /* buffersize is (buffer size per chip) * (number of chips) */
2286                         /* bufferwsize is buffersize in words */
2287                         uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2288                         uint32_t buffermask = buffersize-1;
2289                         uint32_t bufferwsize = buffersize / bank->bus_width;
2290
2291                         /* fall back to memory writes */
2292                         while (count >= (uint32_t)bank->bus_width)
2293                         {
2294                                 int fallback;
2295                                 if ((write_p & 0xff) == 0)
2296                                 {
2297                                         LOG_INFO("Programming at 0x%08" PRIx32 ", count 0x%08"
2298                                                         PRIx32 " bytes remaining", write_p, count);
2299                                 }
2300                                 fallback = 1;
2301                                 if ((bufferwsize > 0) && (count >= buffersize) && !(write_p & buffermask))
2302                                 {
2303                                         retval = cfi_write_words(bank, buffer, bufferwsize, write_p);
2304                                         if (retval == ERROR_OK)
2305                                         {
2306                                                 buffer += buffersize;
2307                                                 write_p += buffersize;
2308                                                 count -= buffersize;
2309                                                 fallback = 0;
2310                                         }
2311                                         else if (retval != ERROR_FLASH_OPER_UNSUPPORTED)
2312                                                 return retval;
2313                                 }
2314                                 /* try the slow way? */
2315                                 if (fallback)
2316                                 {
2317                                         for (i = 0; i < bank->bus_width; i++)
2318                                                 current_word[i] = *buffer++;
2319
2320                                         retval = cfi_write_word(bank, current_word, write_p);
2321                                         if (retval != ERROR_OK)
2322                                                 return retval;
2323
2324                                         write_p += bank->bus_width;
2325                                         count -= bank->bus_width;
2326                                 }
2327                         }
2328                 }
2329                 else
2330                         return retval;
2331         }
2332
2333         /* return to read array mode, so we can read from flash again for padding */
2334         if ((retval = cfi_reset(bank)) != ERROR_OK)
2335         {
2336                 return retval;
2337         }
2338
2339         /* handle unaligned tail bytes */
2340         if (count > 0)
2341         {
2342                 LOG_INFO("Fixup %" PRId32 " unaligned tail bytes", count);
2343
2344                 /* read a complete word from flash */
2345                 if ((retval = target_read_memory(target, write_p,
2346                                 bank->bus_width, 1, current_word)) != ERROR_OK)
2347                         return retval;
2348
2349                 /* replace only bytes that must be written */
2350                 for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
2351                         current_word[i] = *buffer++;
2352
2353                 retval = cfi_write_word(bank, current_word, write_p);
2354                 if (retval != ERROR_OK)
2355                         return retval;
2356         }
2357
2358         /* return to read array mode */
2359         return cfi_reset(bank);
2360 }
2361
2362 static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, void *param)
2363 {
2364         (void) param;
2365         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2366         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2367
2368         pri_ext->_reversed_geometry = 1;
2369 }
2370
2371 static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, void *param)
2372 {
2373         int i;
2374         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2375         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2376         (void) param;
2377
2378         if ((pri_ext->_reversed_geometry) || (pri_ext->TopBottom == 3))
2379         {
2380                 LOG_DEBUG("swapping reversed erase region information on cmdset 0002 device");
2381
2382                 for (i = 0; i < cfi_info->num_erase_regions / 2; i++)
2383                 {
2384                         int j = (cfi_info->num_erase_regions - 1) - i;
2385                         uint32_t swap;
2386
2387                         swap = cfi_info->erase_region_info[i];
2388                         cfi_info->erase_region_info[i] = cfi_info->erase_region_info[j];
2389                         cfi_info->erase_region_info[j] = swap;
2390                 }
2391         }
2392 }
2393
2394 static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, void *param)
2395 {
2396         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2397         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2398         struct cfi_unlock_addresses *unlock_addresses = param;
2399
2400         pri_ext->_unlock1 = unlock_addresses->unlock1;
2401         pri_ext->_unlock2 = unlock_addresses->unlock2;
2402 }
2403
2404
2405 static int cfi_query_string(struct flash_bank *bank, int address)
2406 {
2407         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2408         int retval;
2409
2410         if ((retval = cfi_send_command(bank, 0x98, flash_address(bank, 0, address))) != ERROR_OK)
2411         {
2412                 return retval;
2413         }
2414
2415         retval = cfi_query_u8(bank, 0, 0x10, &cfi_info->qry[0]);
2416         if (retval != ERROR_OK)
2417                 return retval;
2418         retval = cfi_query_u8(bank, 0, 0x11, &cfi_info->qry[1]);
2419         if (retval != ERROR_OK)
2420                 return retval;
2421         retval = cfi_query_u8(bank, 0, 0x12, &cfi_info->qry[2]);
2422         if (retval != ERROR_OK)
2423                 return retval;
2424
2425         LOG_DEBUG("CFI qry returned: 0x%2.2x 0x%2.2x 0x%2.2x",
2426                         cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2]);
2427
2428         if ((cfi_info->qry[0] != 'Q') || (cfi_info->qry[1] != 'R') || (cfi_info->qry[2] != 'Y'))
2429         {
2430                 if ((retval = cfi_reset(bank)) != ERROR_OK)
2431                 {
2432                         return retval;
2433                 }
2434                 LOG_ERROR("Could not probe bank: no QRY");
2435                 return ERROR_FLASH_BANK_INVALID;
2436         }
2437
2438         return ERROR_OK;
2439 }
2440
2441 static int cfi_probe(struct flash_bank *bank)
2442 {
2443         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2444         struct target *target = bank->target;
2445         int num_sectors = 0;
2446         int i;
2447         int sector = 0;
2448         uint32_t unlock1 = 0x555;
2449         uint32_t unlock2 = 0x2aa;
2450         int retval;
2451         uint8_t value_buf0[CFI_MAX_BUS_WIDTH], value_buf1[CFI_MAX_BUS_WIDTH];
2452
2453         if (bank->target->state != TARGET_HALTED)
2454         {
2455                 LOG_ERROR("Target not halted");
2456                 return ERROR_TARGET_NOT_HALTED;
2457         }
2458
2459         cfi_info->probed = 0;
2460         if (bank->sectors)
2461         {
2462                 free(bank->sectors);
2463                 bank->sectors = NULL;
2464         }
2465         if(cfi_info->erase_region_info)
2466         {
2467                 free(cfi_info->erase_region_info);
2468                 cfi_info->erase_region_info = NULL;
2469         }
2470
2471         /* JEDEC standard JESD21C uses 0x5555 and 0x2aaa as unlock addresses,
2472          * while CFI compatible AMD/Spansion flashes use 0x555 and 0x2aa
2473          */
2474         if (cfi_info->jedec_probe)
2475         {
2476                 unlock1 = 0x5555;
2477                 unlock2 = 0x2aaa;
2478         }
2479
2480         /* switch to read identifier codes mode ("AUTOSELECT") */
2481         if ((retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, unlock1))) != ERROR_OK)
2482         {
2483                 return retval;
2484         }
2485         if ((retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, unlock2))) != ERROR_OK)
2486         {
2487                 return retval;
2488         }
2489         if ((retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, unlock1))) != ERROR_OK)
2490         {
2491                 return retval;
2492         }
2493
2494         if ((retval = target_read_memory(target, flash_address(bank, 0, 0x00),
2495                         bank->bus_width, 1, value_buf0)) != ERROR_OK)
2496         {
2497                 return retval;
2498         }
2499         if ((retval = target_read_memory(target, flash_address(bank, 0, 0x01),
2500                         bank->bus_width, 1, value_buf1)) != ERROR_OK)
2501         {
2502                 return retval;
2503         }
2504         switch (bank->chip_width) {
2505                 case 1:
2506                         cfi_info->manufacturer = *value_buf0;
2507                         cfi_info->device_id = *value_buf1;
2508                         break;
2509                 case 2:
2510                         cfi_info->manufacturer = target_buffer_get_u16(target, value_buf0);
2511                         cfi_info->device_id = target_buffer_get_u16(target, value_buf1);
2512                         break;
2513                 case 4:
2514                         cfi_info->manufacturer = target_buffer_get_u32(target, value_buf0);
2515                         cfi_info->device_id = target_buffer_get_u32(target, value_buf1);
2516                         break;
2517                 default:
2518                         LOG_ERROR("Unsupported bank chipwidth %d, can't probe memory", bank->chip_width);
2519                         return ERROR_FLASH_OPERATION_FAILED;
2520         }
2521
2522         LOG_INFO("Flash Manufacturer/Device: 0x%04x 0x%04x",
2523                         cfi_info->manufacturer, cfi_info->device_id);
2524         /* switch back to read array mode */
2525         if ((retval = cfi_reset(bank)) != ERROR_OK)
2526         {
2527                 return retval;
2528         }
2529
2530         /* check device/manufacturer ID for known non-CFI flashes. */
2531         cfi_fixup_non_cfi(bank);
2532
2533         /* query only if this is a CFI compatible flash,
2534          * otherwise the relevant info has already been filled in
2535          */
2536         if (cfi_info->not_cfi == 0)
2537         {
2538                 /* enter CFI query mode
2539                  * according to JEDEC Standard No. 68.01,
2540                  * a single bus sequence with address = 0x55, data = 0x98 should put
2541                  * the device into CFI query mode.
2542                  *
2543                  * SST flashes clearly violate this, and we will consider them incompatbile for now
2544                  */
2545
2546                 retval = cfi_query_string(bank, 0x55);
2547                 if (retval != ERROR_OK)
2548                 {
2549                         /*
2550                          * Spansion S29WS-N CFI query fix is to try 0x555 if 0x55 fails. Should
2551                          * be harmless enough:
2552                          *
2553                          * http://www.infradead.org/pipermail/linux-mtd/2005-September/013618.html
2554                          */
2555                         LOG_USER("Try workaround w/0x555 instead of 0x55 to get QRY.");
2556                         retval = cfi_query_string(bank, 0x555);
2557                 }
2558                 if (retval != ERROR_OK)
2559                         return retval;
2560
2561                 retval = cfi_query_u16(bank, 0, 0x13, &cfi_info->pri_id);
2562                 if (retval != ERROR_OK)
2563                         return retval;
2564                 retval = cfi_query_u16(bank, 0, 0x15, &cfi_info->pri_addr);
2565                 if (retval != ERROR_OK)
2566                         return retval;
2567                 retval = cfi_query_u16(bank, 0, 0x17, &cfi_info->alt_id);
2568                 if (retval != ERROR_OK)
2569                         return retval;
2570                 retval = cfi_query_u16(bank, 0, 0x19, &cfi_info->alt_addr);
2571                 if (retval != ERROR_OK)
2572                         return retval;
2573
2574                 LOG_DEBUG("qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: 0x%4.4x, alt_id: "
2575                                 "0x%4.4x, alt_addr: 0x%4.4x", cfi_info->qry[0], cfi_info->qry[1],
2576                                 cfi_info->qry[2], cfi_info->pri_id, cfi_info->pri_addr,
2577                                 cfi_info->alt_id, cfi_info->alt_addr);
2578
2579                 retval = cfi_query_u8(bank, 0, 0x1b, &cfi_info->vcc_min);
2580                 if (retval != ERROR_OK)
2581                         return retval;
2582                 retval = cfi_query_u8(bank, 0, 0x1c, &cfi_info->vcc_max);
2583                 if (retval != ERROR_OK)
2584                         return retval;
2585                 retval = cfi_query_u8(bank, 0, 0x1d, &cfi_info->vpp_min);
2586                 if (retval != ERROR_OK)
2587                         return retval;
2588                 retval = cfi_query_u8(bank, 0, 0x1e, &cfi_info->vpp_max);
2589                 if (retval != ERROR_OK)
2590                         return retval;
2591
2592                 retval = cfi_query_u8(bank, 0, 0x1f, &cfi_info->word_write_timeout_typ);
2593                 if (retval != ERROR_OK)
2594                         return retval;
2595                 retval = cfi_query_u8(bank, 0, 0x20, &cfi_info->buf_write_timeout_typ);
2596                 if (retval != ERROR_OK)
2597                         return retval;
2598                 retval = cfi_query_u8(bank, 0, 0x21, &cfi_info->block_erase_timeout_typ);
2599                 if (retval != ERROR_OK)
2600                         return retval;
2601                 retval = cfi_query_u8(bank, 0, 0x22, &cfi_info->chip_erase_timeout_typ);
2602                 if (retval != ERROR_OK)
2603                         return retval;
2604                 retval = cfi_query_u8(bank, 0, 0x23, &cfi_info->word_write_timeout_max);
2605                 if (retval != ERROR_OK)
2606                         return retval;
2607                 retval = cfi_query_u8(bank, 0, 0x24, &cfi_info->buf_write_timeout_max);
2608                 if (retval != ERROR_OK)
2609                         return retval;
2610                 retval = cfi_query_u8(bank, 0, 0x25, &cfi_info->block_erase_timeout_max);
2611                 if (retval != ERROR_OK)
2612                         return retval;
2613                 retval = cfi_query_u8(bank, 0, 0x26, &cfi_info->chip_erase_timeout_max);
2614                 if (retval != ERROR_OK)
2615                         return retval;
2616
2617                 uint8_t data;
2618                 retval = cfi_query_u8(bank, 0, 0x27, &data);
2619                 if (retval != ERROR_OK)
2620                         return retval;
2621                 cfi_info->dev_size = 1 << data;
2622
2623                 retval = cfi_query_u16(bank, 0, 0x28, &cfi_info->interface_desc);
2624                 if (retval != ERROR_OK)
2625                         return retval;
2626                 retval = cfi_query_u16(bank, 0, 0x2a, &cfi_info->max_buf_write_size);
2627                 if (retval != ERROR_OK)
2628                         return retval;
2629                 retval = cfi_query_u8(bank, 0, 0x2c, &cfi_info->num_erase_regions);
2630                 if (retval != ERROR_OK)
2631                         return retval;
2632
2633                 LOG_DEBUG("size: 0x%" PRIx32 ", interface desc: %i, max buffer write size: 0x%x",
2634                                 cfi_info->dev_size, cfi_info->interface_desc, (1 << cfi_info->max_buf_write_size));
2635
2636                 if (cfi_info->num_erase_regions)
2637                 {
2638                         cfi_info->erase_region_info = malloc(sizeof(*cfi_info->erase_region_info)
2639                                         * cfi_info->num_erase_regions);
2640                         for (i = 0; i < cfi_info->num_erase_regions; i++)
2641                         {
2642                                 retval = cfi_query_u32(bank, 0, 0x2d + (4 * i), &cfi_info->erase_region_info[i]);
2643                                 if (retval != ERROR_OK)
2644                                         return retval;
2645                                 LOG_DEBUG("erase region[%i]: %" PRIu32 " blocks of size 0x%" PRIx32 "", i,
2646                                                 (cfi_info->erase_region_info[i] & 0xffff) + 1,
2647                                                 (cfi_info->erase_region_info[i] >> 16) * 256);
2648                         }
2649                 }
2650                 else
2651                 {
2652                         cfi_info->erase_region_info = NULL;
2653                 }
2654
2655                 /* We need to read the primary algorithm extended query table before calculating
2656                  * the sector layout to be able to apply fixups
2657                  */
2658                 switch (cfi_info->pri_id)
2659                 {
2660                         /* Intel command set (standard and extended) */
2661                         case 0x0001:
2662                         case 0x0003:
2663                                 cfi_read_intel_pri_ext(bank);
2664                                 break;
2665                         /* AMD/Spansion, Atmel, ... command set */
2666                         case 0x0002:
2667                                 cfi_info->status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7; /* default for all CFI flashs */
2668                                 cfi_read_0002_pri_ext(bank);
2669                                 break;
2670                         default:
2671                                 LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2672                                 break;
2673                 }
2674
2675                 /* return to read array mode
2676                  * we use both reset commands, as some Intel flashes fail to recognize the 0xF0 command
2677                  */
2678                 if ((retval = cfi_reset(bank)) != ERROR_OK)
2679                 {
2680                         return retval;
2681                 }
2682         } /* end CFI case */
2683
2684         LOG_DEBUG("Vcc min: %x.%x, Vcc max: %x.%x, Vpp min: %u.%x, Vpp max: %u.%x",
2685                 (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
2686                 (cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
2687                 (cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
2688                 (cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
2689
2690         LOG_DEBUG("typ. word write timeout: %u us, typ. buf write timeout: %u us, "
2691                         "typ. block erase timeout: %u ms, typ. chip erase timeout: %u ms",
2692                         1 << cfi_info->word_write_timeout_typ, 1 << cfi_info->buf_write_timeout_typ,
2693                         1 << cfi_info->block_erase_timeout_typ, 1 << cfi_info->chip_erase_timeout_typ);
2694
2695         LOG_DEBUG("max. word write timeout: %u us, max. buf write timeout: %u us, "
2696                         "max. block erase timeout: %u ms, max. chip erase timeout: %u ms",
2697                         (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
2698                         (1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
2699                         (1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
2700                         (1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
2701
2702         /* convert timeouts to real values in ms */
2703         cfi_info->word_write_timeout = DIV_ROUND_UP((1L << cfi_info->word_write_timeout_typ) *
2704                                 (1L << cfi_info->word_write_timeout_max), 1000);
2705         cfi_info->buf_write_timeout = DIV_ROUND_UP((1L << cfi_info->buf_write_timeout_typ) *
2706                                 (1L << cfi_info->buf_write_timeout_max), 1000);
2707         cfi_info->block_erase_timeout = (1L << cfi_info->block_erase_timeout_typ) *
2708                                 (1L << cfi_info->block_erase_timeout_max);
2709         cfi_info->chip_erase_timeout = (1L << cfi_info->chip_erase_timeout_typ) *
2710                                 (1L << cfi_info->chip_erase_timeout_max);
2711
2712         LOG_DEBUG("calculated word write timeout: %u ms, buf write timeout: %u ms, "
2713                         "block erase timeout: %u ms, chip erase timeout: %u ms",
2714                         cfi_info->word_write_timeout, cfi_info->buf_write_timeout,
2715                         cfi_info->block_erase_timeout, cfi_info->chip_erase_timeout);
2716
2717         /* apply fixups depending on the primary command set */
2718         switch (cfi_info->pri_id)
2719         {
2720                 /* Intel command set (standard and extended) */
2721                 case 0x0001:
2722                 case 0x0003:
2723                         cfi_fixup(bank, cfi_0001_fixups);
2724                         break;
2725                 /* AMD/Spansion, Atmel, ... command set */
2726                 case 0x0002:
2727                         cfi_fixup(bank, cfi_0002_fixups);
2728                         break;
2729                 default:
2730                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2731                         break;
2732         }
2733
2734         if ((cfi_info->dev_size * bank->bus_width / bank->chip_width) != bank->size)
2735         {
2736                 LOG_WARNING("configuration specifies 0x%" PRIx32 " size, but a 0x%" PRIx32
2737                                 " size flash was found", bank->size, cfi_info->dev_size);
2738         }
2739
2740         if (cfi_info->num_erase_regions == 0)
2741         {
2742                 /* a device might have only one erase block, spanning the whole device */
2743                 bank->num_sectors = 1;
2744                 bank->sectors = malloc(sizeof(struct flash_sector));
2745
2746                 bank->sectors[sector].offset = 0x0;
2747                 bank->sectors[sector].size = bank->size;
2748                 bank->sectors[sector].is_erased = -1;
2749                 bank->sectors[sector].is_protected = -1;
2750         }
2751         else
2752         {
2753                 uint32_t offset = 0;
2754
2755                 for (i = 0; i < cfi_info->num_erase_regions; i++)
2756                 {
2757                         num_sectors += (cfi_info->erase_region_info[i] & 0xffff) + 1;
2758                 }
2759
2760                 bank->num_sectors = num_sectors;
2761                 bank->sectors = malloc(sizeof(struct flash_sector) * num_sectors);
2762
2763                 for (i = 0; i < cfi_info->num_erase_regions; i++)
2764                 {
2765                         uint32_t j;
2766                         for (j = 0; j < (cfi_info->erase_region_info[i] & 0xffff) + 1; j++)
2767                         {
2768                                 bank->sectors[sector].offset = offset;
2769                                 bank->sectors[sector].size = ((cfi_info->erase_region_info[i] >> 16) * 256)
2770                                                 * bank->bus_width / bank->chip_width;
2771                                 offset += bank->sectors[sector].size;
2772                                 bank->sectors[sector].is_erased = -1;
2773                                 bank->sectors[sector].is_protected = -1;
2774                                 sector++;
2775                         }
2776                 }
2777                 if (offset != (cfi_info->dev_size * bank->bus_width / bank->chip_width))
2778                 {
2779                         LOG_WARNING("CFI size is 0x%" PRIx32 ", but total sector size is 0x%" PRIx32 "", \
2780                                 (cfi_info->dev_size * bank->bus_width / bank->chip_width), offset);
2781                 }
2782         }
2783
2784         cfi_info->probed = 1;
2785
2786         return ERROR_OK;
2787 }
2788
2789 static int cfi_auto_probe(struct flash_bank *bank)
2790 {
2791         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2792         if (cfi_info->probed)
2793                 return ERROR_OK;
2794         return cfi_probe(bank);
2795 }
2796
2797 static int cfi_intel_protect_check(struct flash_bank *bank)
2798 {
2799         int retval;
2800         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2801         struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
2802         int i;
2803
2804         /* check if block lock bits are supported on this device */
2805         if (!(pri_ext->blk_status_reg_mask & 0x1))
2806                 return ERROR_FLASH_OPERATION_FAILED;
2807
2808         if ((retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, 0x55))) != ERROR_OK)
2809         {
2810                 return retval;
2811         }
2812
2813         for (i = 0; i < bank->num_sectors; i++)
2814         {
2815                 uint8_t block_status;
2816                 retval = cfi_get_u8(bank, i, 0x2, &block_status);
2817                 if (retval != ERROR_OK)
2818                         return retval;
2819
2820                 if (block_status & 1)
2821                         bank->sectors[i].is_protected = 1;
2822                 else
2823                         bank->sectors[i].is_protected = 0;
2824         }
2825
2826         return cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
2827 }
2828
2829 static int cfi_spansion_protect_check(struct flash_bank *bank)
2830 {
2831         int retval;
2832         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2833         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2834         int i;
2835
2836         if ((retval = cfi_send_command(bank, 0xaa,
2837                         flash_address(bank, 0, pri_ext->_unlock1))) != ERROR_OK)
2838         {
2839                 return retval;
2840         }
2841
2842         if ((retval = cfi_send_command(bank, 0x55,
2843                         flash_address(bank, 0, pri_ext->_unlock2))) != ERROR_OK)
2844         {
2845                 return retval;
2846         }
2847
2848         if ((retval = cfi_send_command(bank, 0x90,
2849                         flash_address(bank, 0, pri_ext->_unlock1))) != ERROR_OK)
2850         {
2851                 return retval;
2852         }
2853
2854         for (i = 0; i < bank->num_sectors; i++)
2855         {
2856                 uint8_t block_status;
2857                 retval = cfi_get_u8(bank, i, 0x2, &block_status);
2858                 if (retval != ERROR_OK)
2859                         return retval;
2860
2861                 if (block_status & 1)
2862                         bank->sectors[i].is_protected = 1;
2863                 else
2864                         bank->sectors[i].is_protected = 0;
2865         }
2866
2867         return cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
2868 }
2869
2870 static int cfi_protect_check(struct flash_bank *bank)
2871 {
2872         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2873
2874         if (bank->target->state != TARGET_HALTED)
2875         {
2876                 LOG_ERROR("Target not halted");
2877                 return ERROR_TARGET_NOT_HALTED;
2878         }
2879
2880         if (cfi_info->qry[0] != 'Q')
2881                 return ERROR_FLASH_BANK_NOT_PROBED;
2882
2883         switch (cfi_info->pri_id)
2884         {
2885                 case 1:
2886                 case 3:
2887                         return cfi_intel_protect_check(bank);
2888                         break;
2889                 case 2:
2890                         return cfi_spansion_protect_check(bank);
2891                         break;
2892                 default:
2893                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2894                         break;
2895         }
2896
2897         return ERROR_OK;
2898 }
2899
2900 static int get_cfi_info(struct flash_bank *bank, char *buf, int buf_size)
2901 {
2902         int printed;
2903         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2904
2905         if (cfi_info->qry[0] == 0xff)
2906         {
2907                 printed = snprintf(buf, buf_size, "\ncfi flash bank not probed yet\n");
2908                 return ERROR_OK;
2909         }
2910
2911         if (cfi_info->not_cfi == 0)
2912                 printed = snprintf(buf, buf_size, "\nCFI flash: ");
2913         else
2914                 printed = snprintf(buf, buf_size, "\nnon-CFI flash: ");
2915         buf += printed;
2916         buf_size -= printed;
2917
2918         printed = snprintf(buf, buf_size, "mfr: 0x%4.4x, id:0x%4.4x\n\n",
2919                 cfi_info->manufacturer, cfi_info->device_id);
2920         buf += printed;
2921         buf_size -= printed;
2922
2923         printed = snprintf(buf, buf_size, "qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: "
2924                         "0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x\n",
2925                         cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2],
2926                         cfi_info->pri_id, cfi_info->pri_addr, cfi_info->alt_id, cfi_info->alt_addr);
2927         buf += printed;
2928         buf_size -= printed;
2929
2930         printed = snprintf(buf, buf_size, "Vcc min: %x.%x, Vcc max: %x.%x, "
2931                         "Vpp min: %u.%x, Vpp max: %u.%x\n",
2932                         (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
2933                         (cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
2934                         (cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
2935                         (cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
2936         buf += printed;
2937         buf_size -= printed;
2938
2939         printed = snprintf(buf, buf_size, "typ. word write timeout: %u us, "
2940                         "typ. buf write timeout: %u us, "
2941                         "typ. block erase timeout: %u ms, "
2942                         "typ. chip erase timeout: %u ms\n",
2943                         1 << cfi_info->word_write_timeout_typ,
2944                         1 << cfi_info->buf_write_timeout_typ,
2945                         1 << cfi_info->block_erase_timeout_typ,
2946                         1 << cfi_info->chip_erase_timeout_typ);
2947         buf += printed;
2948         buf_size -= printed;
2949
2950         printed = snprintf(buf, buf_size, "max. word write timeout: %u us, "
2951                         "max. buf write timeout: %u us, max. "
2952                         "block erase timeout: %u ms, max. chip erase timeout: %u ms\n",
2953                         (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
2954                         (1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
2955                         (1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
2956                         (1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
2957         buf += printed;
2958         buf_size -= printed;
2959
2960         printed = snprintf(buf, buf_size, "size: 0x%" PRIx32 ", interface desc: %i, "
2961                         "max buffer write size: 0x%x\n",
2962                         cfi_info->dev_size,
2963                         cfi_info->interface_desc,
2964                         1 << cfi_info->max_buf_write_size);
2965         buf += printed;
2966         buf_size -= printed;
2967
2968         switch (cfi_info->pri_id)
2969         {
2970                 case 1:
2971                 case 3:
2972                         cfi_intel_info(bank, buf, buf_size);
2973                         break;
2974                 case 2:
2975                         cfi_spansion_info(bank, buf, buf_size);
2976                         break;
2977                 default:
2978                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2979                         break;
2980         }
2981
2982         return ERROR_OK;
2983 }
2984
2985 static void cfi_fixup_0002_write_buffer(struct flash_bank *bank, void *param)
2986 {
2987         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2988
2989         /* disable write buffer for M29W128G */
2990         cfi_info->buf_write_timeout_typ = 0;
2991 }
2992
2993 struct flash_driver cfi_flash = {
2994         .name = "cfi",
2995         .flash_bank_command = cfi_flash_bank_command,
2996         .erase = cfi_erase,
2997         .protect = cfi_protect,
2998         .write = cfi_write,
2999         .read = cfi_read,
3000         .probe = cfi_probe,
3001         .auto_probe = cfi_auto_probe,
3002         /* FIXME: access flash at bus_width size */
3003         .erase_check = default_flash_blank_check,
3004         .protect_check = cfi_protect_check,
3005         .info = get_cfi_info,
3006 };