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