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

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