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

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