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