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         printed = 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         printed = 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;
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         }
1901         else
1902         {
1903                 /* All other ARM CPUs have 32 bit instructions */
1904                 armv4_5_info.common_magic = ARM_COMMON_MAGIC;
1905                 armv4_5_info.core_mode = ARM_MODE_SVC;
1906                 armv4_5_info.core_state = ARM_STATE_ARM;
1907         }
1908
1909         int target_code_size = 0;
1910         const uint32_t *target_code_src = NULL;
1911
1912         switch (bank->bus_width)
1913         {
1914         case 1 :
1915                 if(armv4_5_info.common_magic == ARM_COMMON_MAGIC) /* armv4_5 target */
1916                 {
1917                         target_code_src = armv4_5_word_8_code;
1918                         target_code_size = sizeof(armv4_5_word_8_code);
1919                 }
1920                 break;
1921         case 2 :
1922                 /* Check for DQ5 support */
1923                 if( cfi_info->status_poll_mask & (1 << 5) )
1924                 {
1925                         if(armv4_5_info.common_magic == ARM_COMMON_MAGIC) /* armv4_5 target */
1926                         {
1927                                 target_code_src = armv4_5_word_16_code;
1928                                 target_code_size = sizeof(armv4_5_word_16_code);
1929                         }
1930                         else if (armv4_5_info.common_magic == ARMV7M_COMMON_MAGIC) /* cortex-m3 target */
1931                         {
1932                                 target_code_src = armv7m_word_16_code;
1933                                 target_code_size = sizeof(armv7m_word_16_code);
1934                         }
1935                 }
1936                 else
1937                 {
1938                         /* No DQ5 support. Use DQ7 DATA# polling only. */
1939                         if(armv4_5_info.common_magic == ARM_COMMON_MAGIC) // armv4_5 target
1940                         {
1941                                 target_code_src = armv4_5_word_16_code_dq7only;
1942                                 target_code_size = sizeof(armv4_5_word_16_code_dq7only);
1943                         }
1944                 }
1945                 break;
1946         case 4 :
1947                 if(armv4_5_info.common_magic == ARM_COMMON_MAGIC) // armv4_5 target
1948                 {
1949                         target_code_src = armv4_5_word_32_code;
1950                         target_code_size = sizeof(armv4_5_word_32_code);
1951                 }
1952                 break;
1953         default:
1954                 LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes", bank->bus_width);
1955                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1956         }
1957
1958         /* flash write code */
1959         if (!cfi_info->write_algorithm)
1960         {
1961                 uint8_t *target_code;
1962
1963                 /* convert bus-width dependent algorithm code to correct endiannes */
1964                 target_code = malloc(target_code_size);
1965                 if (target_code == NULL)
1966                 {
1967                         LOG_ERROR("Out of memory");
1968                         return ERROR_FAIL;
1969                 }
1970                 cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
1971
1972                 /* allocate working area */
1973                 retval = target_alloc_working_area(target, target_code_size,
1974                                 &cfi_info->write_algorithm);
1975                 if (retval != ERROR_OK)
1976                 {
1977                         free(target_code);
1978                         return retval;
1979                 }
1980
1981                 /* write algorithm code to working area */
1982                 if ((retval = target_write_buffer(target, cfi_info->write_algorithm->address,
1983                                 target_code_size, target_code)) != ERROR_OK)
1984                 {
1985                         free(target_code);
1986                         return retval;
1987                 }
1988
1989                 free(target_code);
1990         }
1991         /* the following code still assumes target code is fixed 24*4 bytes */
1992
1993         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK)
1994         {
1995                 buffer_size /= 2;
1996                 if (buffer_size <= 256)
1997                 {
1998                         /* if we already allocated the writing code, but failed to get a
1999                          * buffer, free the algorithm */
2000                         if (cfi_info->write_algorithm)
2001                                 target_free_working_area(target, cfi_info->write_algorithm);
2002
2003                         LOG_WARNING("not enough working area available, can't do block memory writes");
2004                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2005                 }
2006         };
2007
2008         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
2009         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
2010         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
2011         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
2012         init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);
2013         init_reg_param(&reg_params[5], "r5", 32, PARAM_IN);
2014         init_reg_param(&reg_params[6], "r8", 32, PARAM_OUT);
2015         init_reg_param(&reg_params[7], "r9", 32, PARAM_OUT);
2016         init_reg_param(&reg_params[8], "r10", 32, PARAM_OUT);
2017         init_reg_param(&reg_params[9], "r11", 32, PARAM_OUT);
2018
2019         while (count > 0)
2020         {
2021                 uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
2022
2023                 retval = target_write_buffer(target, source->address, thisrun_count, buffer);
2024                 if (retval != ERROR_OK)
2025                 {
2026                         break;
2027                 }
2028
2029                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
2030                 buf_set_u32(reg_params[1].value, 0, 32, address);
2031                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
2032                 buf_set_u32(reg_params[3].value, 0, 32, cfi_command_val(bank, 0xA0));
2033                 buf_set_u32(reg_params[4].value, 0, 32, cfi_command_val(bank, 0x80));
2034                 buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
2035                 buf_set_u32(reg_params[7].value, 0, 32, 0xaaaaaaaa);
2036                 buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
2037                 buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
2038
2039                 retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
2040                                 cfi_info->write_algorithm->address,
2041                                 cfi_info->write_algorithm->address + ((target_code_size) - 4),
2042                                 10000, &armv4_5_info);
2043                 if (retval != ERROR_OK)
2044                 {
2045                         break;
2046                 }
2047
2048                 status = buf_get_u32(reg_params[5].value, 0, 32);
2049                 if (status != 0x80)
2050                 {
2051                         LOG_ERROR("flash write block failed status: 0x%" PRIx32 , status);
2052                         retval = ERROR_FLASH_OPERATION_FAILED;
2053                         break;
2054                 }
2055
2056                 buffer += thisrun_count;
2057                 address += thisrun_count;
2058                 count -= thisrun_count;
2059         }
2060
2061         target_free_all_working_areas(target);
2062
2063         destroy_reg_param(&reg_params[0]);
2064         destroy_reg_param(&reg_params[1]);
2065         destroy_reg_param(&reg_params[2]);
2066         destroy_reg_param(&reg_params[3]);
2067         destroy_reg_param(&reg_params[4]);
2068         destroy_reg_param(&reg_params[5]);
2069         destroy_reg_param(&reg_params[6]);
2070         destroy_reg_param(&reg_params[7]);
2071         destroy_reg_param(&reg_params[8]);
2072         destroy_reg_param(&reg_params[9]);
2073
2074         return retval;
2075 }
2076
2077 static int cfi_intel_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2078 {
2079         int retval;
2080         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2081         struct target *target = bank->target;
2082
2083         cfi_intel_clear_status_register(bank);
2084         if ((retval = cfi_send_command(bank, 0x40, address)) != ERROR_OK)
2085         {
2086                 return retval;
2087         }
2088
2089         if ((retval = target_write_memory(target, address, bank->bus_width, 1, word)) != ERROR_OK)
2090         {
2091                 return retval;
2092         }
2093
2094         uint8_t status;
2095         retval = cfi_intel_wait_status_busy(bank, cfi_info->word_write_timeout, &status);
2096         if (retval != 0x80)
2097         {
2098                 if ((retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0))) != ERROR_OK)
2099                 {
2100                         return retval;
2101                 }
2102
2103                 LOG_ERROR("couldn't write word at base 0x%" PRIx32 ", address 0x%" PRIx32,
2104                                 bank->base, address);
2105                 return ERROR_FLASH_OPERATION_FAILED;
2106         }
2107
2108         return ERROR_OK;
2109 }
2110
2111 static int cfi_intel_write_words(struct flash_bank *bank, uint8_t *word,
2112                 uint32_t wordcount, uint32_t address)
2113 {
2114         int retval;
2115         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2116         struct target *target = bank->target;
2117
2118         /* Calculate buffer size and boundary mask */
2119         /* buffersize is (buffer size per chip) * (number of chips) */
2120         /* bufferwsize is buffersize in words */
2121         uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2122         uint32_t buffermask = buffersize-1;
2123         uint32_t bufferwsize = buffersize / bank->bus_width;
2124
2125         /* Check for valid range */
2126         if (address & buffermask)
2127         {
2128                 LOG_ERROR("Write address at base 0x%" PRIx32 ", address 0x%" PRIx32
2129                                 " not aligned to 2^%d boundary",
2130                                 bank->base, address, cfi_info->max_buf_write_size);
2131                 return ERROR_FLASH_OPERATION_FAILED;
2132         }
2133
2134         /* Check for valid size */
2135         if (wordcount > bufferwsize)
2136         {
2137                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %" PRId32,
2138                                 wordcount, buffersize);
2139                 return ERROR_FLASH_OPERATION_FAILED;
2140         }
2141
2142         /* Write to flash buffer */
2143         cfi_intel_clear_status_register(bank);
2144
2145         /* Initiate buffer operation _*/
2146         if ((retval = cfi_send_command(bank, 0xe8, address)) != ERROR_OK)
2147         {
2148                 return retval;
2149         }
2150         uint8_t status;
2151         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
2152         if (retval != ERROR_OK)
2153                 return retval;
2154         if (status != 0x80)
2155         {
2156                 if ((retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0))) != ERROR_OK)
2157                 {
2158                         return retval;
2159                 }
2160
2161                 LOG_ERROR("couldn't start buffer write operation at base 0x%" PRIx32 ", address 0x%" PRIx32,
2162                                 bank->base, address);
2163                 return ERROR_FLASH_OPERATION_FAILED;
2164         }
2165
2166         /* Write buffer wordcount-1 and data words */
2167         if ((retval = cfi_send_command(bank, bufferwsize-1, address)) != ERROR_OK)
2168         {
2169                 return retval;
2170         }
2171
2172         if ((retval = target_write_memory(target,
2173                         address, bank->bus_width, bufferwsize, word)) != ERROR_OK)
2174         {
2175                 return retval;
2176         }
2177
2178         /* Commit write operation */
2179         if ((retval = cfi_send_command(bank, 0xd0, address)) != ERROR_OK)
2180         {
2181                 return retval;
2182         }
2183
2184         retval = cfi_intel_wait_status_busy(bank, cfi_info->buf_write_timeout, &status);
2185         if (retval != ERROR_OK)
2186                 return retval;
2187
2188         if (status != 0x80)
2189         {
2190                 if ((retval = cfi_send_command(bank, 0xff,
2191                                 flash_address(bank, 0, 0x0))) != ERROR_OK)
2192                 {
2193                         return retval;
2194                 }
2195
2196                 LOG_ERROR("Buffer write at base 0x%" PRIx32
2197                                 ", address 0x%" PRIx32 " failed.", bank->base, address);
2198                 return ERROR_FLASH_OPERATION_FAILED;
2199         }
2200
2201         return ERROR_OK;
2202 }
2203
2204 static int cfi_spansion_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2205 {
2206         int retval;
2207         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2208         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2209         struct target *target = bank->target;
2210
2211         if ((retval = cfi_send_command(bank, 0xaa,
2212                         flash_address(bank, 0, pri_ext->_unlock1))) != ERROR_OK)
2213         {
2214                 return retval;
2215         }
2216
2217         if ((retval = cfi_send_command(bank, 0x55,
2218                         flash_address(bank, 0, pri_ext->_unlock2))) != ERROR_OK)
2219         {
2220                 return retval;
2221         }
2222
2223         if ((retval = cfi_send_command(bank, 0xa0,
2224                         flash_address(bank, 0, pri_ext->_unlock1))) != ERROR_OK)
2225         {
2226                 return retval;
2227         }
2228
2229         if ((retval = target_write_memory(target,
2230                         address, bank->bus_width, 1, word)) != ERROR_OK)
2231         {
2232                 return retval;
2233         }
2234
2235         if (cfi_spansion_wait_status_busy(bank, cfi_info->word_write_timeout) != ERROR_OK)
2236         {
2237                 if ((retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0))) != ERROR_OK)
2238                 {
2239                         return retval;
2240                 }
2241
2242                 LOG_ERROR("couldn't write word at base 0x%" PRIx32
2243                                 ", address 0x%" PRIx32 , bank->base, address);
2244                 return ERROR_FLASH_OPERATION_FAILED;
2245         }
2246
2247         return ERROR_OK;
2248 }
2249
2250 static int cfi_spansion_write_words(struct flash_bank *bank, uint8_t *word,
2251                 uint32_t wordcount, uint32_t address)
2252 {
2253         int retval;
2254         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2255         struct target *target = bank->target;
2256         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2257
2258         /* Calculate buffer size and boundary mask */
2259         /* buffersize is (buffer size per chip) * (number of chips) */
2260         /* bufferwsize is buffersize in words */
2261         uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2262         uint32_t buffermask = buffersize-1;
2263         uint32_t bufferwsize = buffersize / bank->bus_width;
2264
2265         /* Check for valid range */
2266         if (address & buffermask)
2267         {
2268                 LOG_ERROR("Write address at base 0x%" PRIx32
2269                                 ", address 0x%" PRIx32 " not aligned to 2^%d boundary",
2270                                 bank->base, address, cfi_info->max_buf_write_size);
2271                 return ERROR_FLASH_OPERATION_FAILED;
2272         }
2273
2274         /* Check for valid size */
2275         if (wordcount > bufferwsize)
2276         {
2277                 LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %"
2278                                 PRId32, wordcount, buffersize);
2279                 return ERROR_FLASH_OPERATION_FAILED;
2280         }
2281
2282         /* Unlock */
2283         if ((retval = cfi_send_command(bank, 0xaa,
2284                         flash_address(bank, 0, pri_ext->_unlock1))) != ERROR_OK)
2285         {
2286                 return retval;
2287         }
2288
2289         if ((retval = cfi_send_command(bank, 0x55,
2290                         flash_address(bank, 0, pri_ext->_unlock2))) != ERROR_OK)
2291         {
2292                 return retval;
2293         }
2294
2295         /* Buffer load command */
2296         if ((retval = cfi_send_command(bank, 0x25, address)) != ERROR_OK)
2297         {
2298                 return retval;
2299         }
2300
2301         /* Write buffer wordcount-1 and data words */
2302         if ((retval = cfi_send_command(bank, bufferwsize-1, address)) != ERROR_OK)
2303         {
2304                 return retval;
2305         }
2306
2307         if ((retval = target_write_memory(target,
2308                         address, bank->bus_width, bufferwsize, word)) != ERROR_OK)
2309         {
2310                 return retval;
2311         }
2312
2313         /* Commit write operation */
2314         if ((retval = cfi_send_command(bank, 0x29, address)) != ERROR_OK)
2315         {
2316                 return retval;
2317         }
2318
2319         if (cfi_spansion_wait_status_busy(bank, cfi_info->buf_write_timeout) != ERROR_OK)
2320         {
2321                 if ((retval = cfi_send_command(bank, 0xf0,
2322                                 flash_address(bank, 0, 0x0))) != ERROR_OK)
2323                 {
2324                         return retval;
2325                 }
2326
2327                 LOG_ERROR("couldn't write block at base 0x%" PRIx32
2328                                 ", address 0x%" PRIx32 ", size 0x%" PRIx32, bank->base, address, bufferwsize);
2329                 return ERROR_FLASH_OPERATION_FAILED;
2330         }
2331
2332         return ERROR_OK;
2333 }
2334
2335 static int cfi_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
2336 {
2337         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2338
2339         switch (cfi_info->pri_id)
2340         {
2341                 case 1:
2342                 case 3:
2343                         return cfi_intel_write_word(bank, word, address);
2344                         break;
2345                 case 2:
2346                         return cfi_spansion_write_word(bank, word, address);
2347                         break;
2348                 default:
2349                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2350                         break;
2351         }
2352
2353         return ERROR_FLASH_OPERATION_FAILED;
2354 }
2355
2356 static int cfi_write_words(struct flash_bank *bank, uint8_t *word,
2357                 uint32_t wordcount, uint32_t address)
2358 {
2359         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2360
2361         if (cfi_info->buf_write_timeout_typ == 0)
2362         {
2363                 /* buffer writes are not supported */
2364                 LOG_DEBUG("Buffer Writes Not Supported");
2365                 return ERROR_FLASH_OPER_UNSUPPORTED;
2366         }
2367
2368         switch (cfi_info->pri_id)
2369         {
2370                 case 1:
2371                 case 3:
2372                         return cfi_intel_write_words(bank, word, wordcount, address);
2373                         break;
2374                 case 2:
2375                         return cfi_spansion_write_words(bank, word, wordcount, address);
2376                         break;
2377                 default:
2378                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2379                         break;
2380         }
2381
2382         return ERROR_FLASH_OPERATION_FAILED;
2383 }
2384
2385 static int cfi_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
2386 {
2387         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2388         struct target *target = bank->target;
2389         uint32_t address = bank->base + offset;
2390         uint32_t read_p;
2391         int align;      /* number of unaligned bytes */
2392         uint8_t current_word[CFI_MAX_BUS_WIDTH];
2393         int i;
2394         int retval;
2395
2396         LOG_DEBUG("reading buffer of %i byte at 0x%8.8x",
2397                 (int)count, (unsigned)offset);
2398
2399         if (bank->target->state != TARGET_HALTED)
2400         {
2401                 LOG_ERROR("Target not halted");
2402                 return ERROR_TARGET_NOT_HALTED;
2403         }
2404
2405         if (offset + count > bank->size)
2406                 return ERROR_FLASH_DST_OUT_OF_BANK;
2407
2408         if (cfi_info->qry[0] != 'Q')
2409                 return ERROR_FLASH_BANK_NOT_PROBED;
2410
2411         /* start at the first byte of the first word (bus_width size) */
2412         read_p = address & ~(bank->bus_width - 1);
2413         if ((align = address - read_p) != 0)
2414         {
2415                 LOG_INFO("Fixup %d unaligned read head bytes", align);
2416
2417                 /* read a complete word from flash */
2418                 if ((retval = target_read_memory(target, read_p,
2419                                 bank->bus_width, 1, current_word)) != ERROR_OK)
2420                         return retval;
2421
2422                 /* take only bytes we need */
2423                 for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
2424                         *buffer++ = current_word[i];
2425
2426                 read_p += bank->bus_width;
2427         }
2428
2429         align = count / bank->bus_width;
2430         if (align)
2431         {
2432                 if ((retval = target_read_memory(target, read_p,
2433                                 bank->bus_width, align, buffer)) != ERROR_OK)
2434                         return retval;
2435
2436                 read_p += align * bank->bus_width;
2437                 buffer += align * bank->bus_width;
2438                 count -= align * bank->bus_width;
2439         }
2440
2441         if (count)
2442         {
2443                 LOG_INFO("Fixup %d unaligned read tail bytes", count);
2444
2445                 /* read a complete word from flash */
2446                 if ((retval = target_read_memory(target, read_p,
2447                                 bank->bus_width, 1, current_word)) != ERROR_OK)
2448                         return retval;
2449
2450                 /* take only bytes we need */
2451                 for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
2452                         *buffer++ = current_word[i];
2453         }
2454
2455         return ERROR_OK;
2456 }
2457
2458 static int cfi_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
2459 {
2460         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2461         struct target *target = bank->target;
2462         uint32_t address = bank->base + offset; /* address of first byte to be programmed */
2463         uint32_t write_p;
2464         int align;      /* number of unaligned bytes */
2465         int blk_count; /* number of bus_width bytes for block copy */
2466         uint8_t current_word[CFI_MAX_BUS_WIDTH * 4];    /* word (bus_width size) currently being programmed */
2467         int i;
2468         int retval;
2469
2470         if (bank->target->state != TARGET_HALTED)
2471         {
2472                 LOG_ERROR("Target not halted");
2473                 return ERROR_TARGET_NOT_HALTED;
2474         }
2475
2476         if (offset + count > bank->size)
2477                 return ERROR_FLASH_DST_OUT_OF_BANK;
2478
2479         if (cfi_info->qry[0] != 'Q')
2480                 return ERROR_FLASH_BANK_NOT_PROBED;
2481
2482         /* start at the first byte of the first word (bus_width size) */
2483         write_p = address & ~(bank->bus_width - 1);
2484         if ((align = address - write_p) != 0)
2485         {
2486                 LOG_INFO("Fixup %d unaligned head bytes", align);
2487
2488                 /* read a complete word from flash */
2489                 if ((retval = target_read_memory(target, write_p,
2490                                 bank->bus_width, 1, current_word)) != ERROR_OK)
2491                         return retval;
2492
2493                 /* replace only bytes that must be written */
2494                 for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
2495                         current_word[i] = *buffer++;
2496
2497                 retval = cfi_write_word(bank, current_word, write_p);
2498                 if (retval != ERROR_OK)
2499                         return retval;
2500                 write_p += bank->bus_width;
2501         }
2502
2503         /* handle blocks of bus_size aligned bytes */
2504         blk_count = count & ~(bank->bus_width - 1); /* round down, leave tail bytes */
2505         switch (cfi_info->pri_id)
2506         {
2507                 /* try block writes (fails without working area) */
2508                 case 1:
2509                 case 3:
2510                         retval = cfi_intel_write_block(bank, buffer, write_p, blk_count);
2511                         break;
2512                 case 2:
2513                         retval = cfi_spansion_write_block(bank, buffer, write_p, blk_count);
2514                         break;
2515                 default:
2516                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2517                         retval = ERROR_FLASH_OPERATION_FAILED;
2518                         break;
2519         }
2520         if (retval == ERROR_OK)
2521         {
2522                 /* Increment pointers and decrease count on succesful block write */
2523                 buffer += blk_count;
2524                 write_p += blk_count;
2525                 count -= blk_count;
2526         }
2527         else
2528         {
2529                 if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
2530                 {
2531                         /* Calculate buffer size and boundary mask */
2532                         /* buffersize is (buffer size per chip) * (number of chips) */
2533                         /* bufferwsize is buffersize in words */
2534                         uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
2535                         uint32_t buffermask = buffersize-1;
2536                         uint32_t bufferwsize = buffersize / bank->bus_width;
2537
2538                         /* fall back to memory writes */
2539                         while (count >= (uint32_t)bank->bus_width)
2540                         {
2541                                 int fallback;
2542                                 if ((write_p & 0xff) == 0)
2543                                 {
2544                                         LOG_INFO("Programming at 0x%08" PRIx32 ", count 0x%08"
2545                                                         PRIx32 " bytes remaining", write_p, count);
2546                                 }
2547                                 fallback = 1;
2548                                 if ((bufferwsize > 0) && (count >= buffersize) && !(write_p & buffermask))
2549                                 {
2550                                         retval = cfi_write_words(bank, buffer, bufferwsize, write_p);
2551                                         if (retval == ERROR_OK)
2552                                         {
2553                                                 buffer += buffersize;
2554                                                 write_p += buffersize;
2555                                                 count -= buffersize;
2556                                                 fallback = 0;
2557                                         }
2558                                         else if (retval != ERROR_FLASH_OPER_UNSUPPORTED)
2559                                                 return retval;
2560                                 }
2561                                 /* try the slow way? */
2562                                 if (fallback)
2563                                 {
2564                                         for (i = 0; i < bank->bus_width; i++)
2565                                                 current_word[i] = *buffer++;
2566
2567                                         retval = cfi_write_word(bank, current_word, write_p);
2568                                         if (retval != ERROR_OK)
2569                                                 return retval;
2570
2571                                         write_p += bank->bus_width;
2572                                         count -= bank->bus_width;
2573                                 }
2574                         }
2575                 }
2576                 else
2577                         return retval;
2578         }
2579
2580         /* return to read array mode, so we can read from flash again for padding */
2581         if ((retval = cfi_reset(bank)) != ERROR_OK)
2582         {
2583                 return retval;
2584         }
2585
2586         /* handle unaligned tail bytes */
2587         if (count > 0)
2588         {
2589                 LOG_INFO("Fixup %" PRId32 " unaligned tail bytes", count);
2590
2591                 /* read a complete word from flash */
2592                 if ((retval = target_read_memory(target, write_p,
2593                                 bank->bus_width, 1, current_word)) != ERROR_OK)
2594                         return retval;
2595
2596                 /* replace only bytes that must be written */
2597                 for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
2598                         current_word[i] = *buffer++;
2599
2600                 retval = cfi_write_word(bank, current_word, write_p);
2601                 if (retval != ERROR_OK)
2602                         return retval;
2603         }
2604
2605         /* return to read array mode */
2606         return cfi_reset(bank);
2607 }
2608
2609 static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, void *param)
2610 {
2611         (void) param;
2612         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2613         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2614
2615         pri_ext->_reversed_geometry = 1;
2616 }
2617
2618 static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, void *param)
2619 {
2620         int i;
2621         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2622         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2623         (void) param;
2624
2625         if ((pri_ext->_reversed_geometry) || (pri_ext->TopBottom == 3))
2626         {
2627                 LOG_DEBUG("swapping reversed erase region information on cmdset 0002 device");
2628
2629                 for (i = 0; i < cfi_info->num_erase_regions / 2; i++)
2630                 {
2631                         int j = (cfi_info->num_erase_regions - 1) - i;
2632                         uint32_t swap;
2633
2634                         swap = cfi_info->erase_region_info[i];
2635                         cfi_info->erase_region_info[i] = cfi_info->erase_region_info[j];
2636                         cfi_info->erase_region_info[j] = swap;
2637                 }
2638         }
2639 }
2640
2641 static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, void *param)
2642 {
2643         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2644         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
2645         struct cfi_unlock_addresses *unlock_addresses = param;
2646
2647         pri_ext->_unlock1 = unlock_addresses->unlock1;
2648         pri_ext->_unlock2 = unlock_addresses->unlock2;
2649 }
2650
2651
2652 static int cfi_query_string(struct flash_bank *bank, int address)
2653 {
2654         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2655         int retval;
2656
2657         if ((retval = cfi_send_command(bank, 0x98, flash_address(bank, 0, address))) != ERROR_OK)
2658         {
2659                 return retval;
2660         }
2661
2662         retval = cfi_query_u8(bank, 0, 0x10, &cfi_info->qry[0]);
2663         if (retval != ERROR_OK)
2664                 return retval;
2665         retval = cfi_query_u8(bank, 0, 0x11, &cfi_info->qry[1]);
2666         if (retval != ERROR_OK)
2667                 return retval;
2668         retval = cfi_query_u8(bank, 0, 0x12, &cfi_info->qry[2]);
2669         if (retval != ERROR_OK)
2670                 return retval;
2671
2672         LOG_DEBUG("CFI qry returned: 0x%2.2x 0x%2.2x 0x%2.2x",
2673                         cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2]);
2674
2675         if ((cfi_info->qry[0] != 'Q') || (cfi_info->qry[1] != 'R') || (cfi_info->qry[2] != 'Y'))
2676         {
2677                 if ((retval = cfi_reset(bank)) != ERROR_OK)
2678                 {
2679                         return retval;
2680                 }
2681                 LOG_ERROR("Could not probe bank: no QRY");
2682                 return ERROR_FLASH_BANK_INVALID;
2683         }
2684
2685         return ERROR_OK;
2686 }
2687
2688 static int cfi_probe(struct flash_bank *bank)
2689 {
2690         struct cfi_flash_bank *cfi_info = bank->driver_priv;
2691         struct target *target = bank->target;
2692         int num_sectors = 0;
2693         int i;
2694         int sector = 0;
2695         uint32_t unlock1 = 0x555;
2696         uint32_t unlock2 = 0x2aa;
2697         int retval;
2698         uint8_t value_buf0[CFI_MAX_BUS_WIDTH], value_buf1[CFI_MAX_BUS_WIDTH];
2699
2700         if (bank->target->state != TARGET_HALTED)
2701         {
2702                 LOG_ERROR("Target not halted");
2703                 return ERROR_TARGET_NOT_HALTED;
2704         }
2705
2706         cfi_info->probed = 0;
2707         if (bank->sectors)
2708         {
2709                 free(bank->sectors);
2710                 bank->sectors = NULL;
2711         }
2712         if(cfi_info->erase_region_info)
2713         {
2714                 free(cfi_info->erase_region_info);
2715                 cfi_info->erase_region_info = NULL;
2716         }
2717
2718         /* JEDEC standard JESD21C uses 0x5555 and 0x2aaa as unlock addresses,
2719          * while CFI compatible AMD/Spansion flashes use 0x555 and 0x2aa
2720          */
2721         if (cfi_info->jedec_probe)
2722         {
2723                 unlock1 = 0x5555;
2724                 unlock2 = 0x2aaa;
2725         }
2726
2727         /* switch to read identifier codes mode ("AUTOSELECT") */
2728         if ((retval = cfi_send_command(bank, 0xaa, flash_address(bank, 0, unlock1))) != ERROR_OK)
2729         {
2730                 return retval;
2731         }
2732         if ((retval = cfi_send_command(bank, 0x55, flash_address(bank, 0, unlock2))) != ERROR_OK)
2733         {
2734                 return retval;
2735         }
2736         if ((retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, unlock1))) != ERROR_OK)
2737         {
2738                 return retval;
2739         }
2740
2741         if ((retval = target_read_memory(target, flash_address(bank, 0, 0x00),
2742                         bank->bus_width, 1, value_buf0)) != ERROR_OK)
2743         {
2744                 return retval;
2745         }
2746         if ((retval = target_read_memory(target, flash_address(bank, 0, 0x01),
2747                         bank->bus_width, 1, value_buf1)) != ERROR_OK)
2748         {
2749                 return retval;
2750         }
2751         switch (bank->chip_width) {
2752                 case 1:
2753                         cfi_info->manufacturer = *value_buf0;
2754                         cfi_info->device_id = *value_buf1;
2755                         break;
2756                 case 2:
2757                         cfi_info->manufacturer = target_buffer_get_u16(target, value_buf0);
2758                         cfi_info->device_id = target_buffer_get_u16(target, value_buf1);
2759                         break;
2760                 case 4:
2761                         cfi_info->manufacturer = target_buffer_get_u32(target, value_buf0);
2762                         cfi_info->device_id = target_buffer_get_u32(target, value_buf1);
2763                         break;
2764                 default:
2765                         LOG_ERROR("Unsupported bank chipwidth %d, can't probe memory", bank->chip_width);
2766                         return ERROR_FLASH_OPERATION_FAILED;
2767         }
2768
2769         LOG_INFO("Flash Manufacturer/Device: 0x%04x 0x%04x",
2770                         cfi_info->manufacturer, cfi_info->device_id);
2771         /* switch back to read array mode */
2772         if ((retval = cfi_reset(bank)) != ERROR_OK)
2773         {
2774                 return retval;
2775         }
2776
2777         /* check device/manufacturer ID for known non-CFI flashes. */
2778         cfi_fixup_non_cfi(bank);
2779
2780         /* query only if this is a CFI compatible flash,
2781          * otherwise the relevant info has already been filled in
2782          */
2783         if (cfi_info->not_cfi == 0)
2784         {
2785                 /* enter CFI query mode
2786                  * according to JEDEC Standard No. 68.01,
2787                  * a single bus sequence with address = 0x55, data = 0x98 should put
2788                  * the device into CFI query mode.
2789                  *
2790                  * SST flashes clearly violate this, and we will consider them incompatbile for now
2791                  */
2792
2793                 retval = cfi_query_string(bank, 0x55);
2794                 if (retval != ERROR_OK)
2795                 {
2796                         /*
2797                          * Spansion S29WS-N CFI query fix is to try 0x555 if 0x55 fails. Should
2798                          * be harmless enough:
2799                          *
2800                          * http://www.infradead.org/pipermail/linux-mtd/2005-September/013618.html
2801                          */
2802                         LOG_USER("Try workaround w/0x555 instead of 0x55 to get QRY.");
2803                         retval = cfi_query_string(bank, 0x555);
2804                 }
2805                 if (retval != ERROR_OK)
2806                         return retval;
2807
2808                 retval = cfi_query_u16(bank, 0, 0x13, &cfi_info->pri_id);
2809                 if (retval != ERROR_OK)
2810                         return retval;
2811                 retval = cfi_query_u16(bank, 0, 0x15, &cfi_info->pri_addr);
2812                 if (retval != ERROR_OK)
2813                         return retval;
2814                 retval = cfi_query_u16(bank, 0, 0x17, &cfi_info->alt_id);
2815                 if (retval != ERROR_OK)
2816                         return retval;
2817                 retval = cfi_query_u16(bank, 0, 0x19, &cfi_info->alt_addr);
2818                 if (retval != ERROR_OK)
2819                         return retval;
2820
2821                 LOG_DEBUG("qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: 0x%4.4x, alt_id: "
2822                                 "0x%4.4x, alt_addr: 0x%4.4x", cfi_info->qry[0], cfi_info->qry[1],
2823                                 cfi_info->qry[2], cfi_info->pri_id, cfi_info->pri_addr,
2824                                 cfi_info->alt_id, cfi_info->alt_addr);
2825
2826                 retval = cfi_query_u8(bank, 0, 0x1b, &cfi_info->vcc_min);
2827                 if (retval != ERROR_OK)
2828                         return retval;
2829                 retval = cfi_query_u8(bank, 0, 0x1c, &cfi_info->vcc_max);
2830                 if (retval != ERROR_OK)
2831                         return retval;
2832                 retval = cfi_query_u8(bank, 0, 0x1d, &cfi_info->vpp_min);
2833                 if (retval != ERROR_OK)
2834                         return retval;
2835                 retval = cfi_query_u8(bank, 0, 0x1e, &cfi_info->vpp_max);
2836                 if (retval != ERROR_OK)
2837                         return retval;
2838
2839                 retval = cfi_query_u8(bank, 0, 0x1f, &cfi_info->word_write_timeout_typ);
2840                 if (retval != ERROR_OK)
2841                         return retval;
2842                 retval = cfi_query_u8(bank, 0, 0x20, &cfi_info->buf_write_timeout_typ);
2843                 if (retval != ERROR_OK)
2844                         return retval;
2845                 retval = cfi_query_u8(bank, 0, 0x21, &cfi_info->block_erase_timeout_typ);
2846                 if (retval != ERROR_OK)
2847                         return retval;
2848                 retval = cfi_query_u8(bank, 0, 0x22, &cfi_info->chip_erase_timeout_typ);
2849                 if (retval != ERROR_OK)
2850                         return retval;
2851                 retval = cfi_query_u8(bank, 0, 0x23, &cfi_info->word_write_timeout_max);
2852                 if (retval != ERROR_OK)
2853                         return retval;
2854                 retval = cfi_query_u8(bank, 0, 0x24, &cfi_info->buf_write_timeout_max);
2855                 if (retval != ERROR_OK)
2856                         return retval;
2857                 retval = cfi_query_u8(bank, 0, 0x25, &cfi_info->block_erase_timeout_max);
2858                 if (retval != ERROR_OK)
2859                         return retval;
2860                 retval = cfi_query_u8(bank, 0, 0x26, &cfi_info->chip_erase_timeout_max);
2861                 if (retval != ERROR_OK)
2862                         return retval;
2863
2864                 uint8_t data;
2865                 retval = cfi_query_u8(bank, 0, 0x27, &data);
2866                 if (retval != ERROR_OK)
2867                         return retval;
2868                 cfi_info->dev_size = 1 << data;
2869
2870                 retval = cfi_query_u16(bank, 0, 0x28, &cfi_info->interface_desc);
2871                 if (retval != ERROR_OK)
2872                         return retval;
2873                 retval = cfi_query_u16(bank, 0, 0x2a, &cfi_info->max_buf_write_size);
2874                 if (retval != ERROR_OK)
2875                         return retval;
2876                 retval = cfi_query_u8(bank, 0, 0x2c, &cfi_info->num_erase_regions);
2877                 if (retval != ERROR_OK)
2878                         return retval;
2879
2880                 LOG_DEBUG("size: 0x%" PRIx32 ", interface desc: %i, max buffer write size: 0x%x",
2881                                 cfi_info->dev_size, cfi_info->interface_desc, (1 << cfi_info->max_buf_write_size));
2882
2883                 if (cfi_info->num_erase_regions)
2884                 {
2885                         cfi_info->erase_region_info = malloc(sizeof(*cfi_info->erase_region_info)
2886                                         * cfi_info->num_erase_regions);
2887                         for (i = 0; i < cfi_info->num_erase_regions; i++)
2888                         {
2889                                 retval = cfi_query_u32(bank, 0, 0x2d + (4 * i), &cfi_info->erase_region_info[i]);
2890                                 if (retval != ERROR_OK)
2891                                         return retval;
2892                                 LOG_DEBUG("erase region[%i]: %" PRIu32 " blocks of size 0x%" PRIx32 "", i,
2893                                                 (cfi_info->erase_region_info[i] & 0xffff) + 1,
2894                                                 (cfi_info->erase_region_info[i] >> 16) * 256);
2895                         }
2896                 }
2897                 else
2898                 {
2899                         cfi_info->erase_region_info = NULL;
2900                 }
2901
2902                 /* We need to read the primary algorithm extended query table before calculating
2903                  * the sector layout to be able to apply fixups
2904                  */
2905                 switch (cfi_info->pri_id)
2906                 {
2907                         /* Intel command set (standard and extended) */
2908                         case 0x0001:
2909                         case 0x0003:
2910                                 cfi_read_intel_pri_ext(bank);
2911                                 break;
2912                         /* AMD/Spansion, Atmel, ... command set */
2913                         case 0x0002:
2914                                 cfi_info->status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7; /* default for all CFI flashs */
2915                                 cfi_read_0002_pri_ext(bank);
2916                                 break;
2917                         default:
2918                                 LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2919                                 break;
2920                 }
2921
2922                 /* return to read array mode
2923                  * we use both reset commands, as some Intel flashes fail to recognize the 0xF0 command
2924                  */
2925                 if ((retval = cfi_reset(bank)) != ERROR_OK)
2926                 {
2927                         return retval;
2928                 }
2929         } /* end CFI case */
2930
2931         LOG_DEBUG("Vcc min: %x.%x, Vcc max: %x.%x, Vpp min: %u.%x, Vpp max: %u.%x",
2932                 (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
2933                 (cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
2934                 (cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
2935                 (cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
2936
2937         LOG_DEBUG("typ. word write timeout: %u us, typ. buf write timeout: %u us, "
2938                         "typ. block erase timeout: %u ms, typ. chip erase timeout: %u ms",
2939                         1 << cfi_info->word_write_timeout_typ, 1 << cfi_info->buf_write_timeout_typ,
2940                         1 << cfi_info->block_erase_timeout_typ, 1 << cfi_info->chip_erase_timeout_typ);
2941
2942         LOG_DEBUG("max. word write timeout: %u us, max. buf write timeout: %u us, "
2943                         "max. block erase timeout: %u ms, max. chip erase timeout: %u ms",
2944                         (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
2945                         (1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
2946                         (1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
2947                         (1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
2948
2949         /* convert timeouts to real values in ms */
2950         cfi_info->word_write_timeout = DIV_ROUND_UP((1L << cfi_info->word_write_timeout_typ) *
2951                                 (1L << cfi_info->word_write_timeout_max), 1000);
2952         cfi_info->buf_write_timeout = DIV_ROUND_UP((1L << cfi_info->buf_write_timeout_typ) *
2953                                 (1L << cfi_info->buf_write_timeout_max), 1000);
2954         cfi_info->block_erase_timeout = (1L << cfi_info->block_erase_timeout_typ) *
2955                                 (1L << cfi_info->block_erase_timeout_max);
2956         cfi_info->chip_erase_timeout = (1L << cfi_info->chip_erase_timeout_typ) *
2957                                 (1L << cfi_info->chip_erase_timeout_max);
2958
2959         LOG_DEBUG("calculated word write timeout: %u ms, buf write timeout: %u ms, "
2960                         "block erase timeout: %u ms, chip erase timeout: %u ms",
2961                         cfi_info->word_write_timeout, cfi_info->buf_write_timeout,
2962                         cfi_info->block_erase_timeout, cfi_info->chip_erase_timeout);
2963
2964         /* apply fixups depending on the primary command set */
2965         switch (cfi_info->pri_id)
2966         {
2967                 /* Intel command set (standard and extended) */
2968                 case 0x0001:
2969                 case 0x0003:
2970                         cfi_fixup(bank, cfi_0001_fixups);
2971                         break;
2972                 /* AMD/Spansion, Atmel, ... command set */
2973                 case 0x0002:
2974                         cfi_fixup(bank, cfi_0002_fixups);
2975                         break;
2976                 default:
2977                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
2978                         break;
2979         }
2980
2981         if ((cfi_info->dev_size * bank->bus_width / bank->chip_width) != bank->size)
2982         {
2983                 LOG_WARNING("configuration specifies 0x%" PRIx32 " size, but a 0x%" PRIx32
2984                                 " size flash was found", bank->size, cfi_info->dev_size);
2985         }
2986
2987         if (cfi_info->num_erase_regions == 0)
2988         {
2989                 /* a device might have only one erase block, spanning the whole device */
2990                 bank->num_sectors = 1;
2991                 bank->sectors = malloc(sizeof(struct flash_sector));
2992
2993                 bank->sectors[sector].offset = 0x0;
2994                 bank->sectors[sector].size = bank->size;
2995                 bank->sectors[sector].is_erased = -1;
2996                 bank->sectors[sector].is_protected = -1;
2997         }
2998         else
2999         {
3000                 uint32_t offset = 0;
3001
3002                 for (i = 0; i < cfi_info->num_erase_regions; i++)
3003                 {
3004                         num_sectors += (cfi_info->erase_region_info[i] & 0xffff) + 1;
3005                 }
3006
3007                 bank->num_sectors = num_sectors;
3008                 bank->sectors = malloc(sizeof(struct flash_sector) * num_sectors);
3009
3010                 for (i = 0; i < cfi_info->num_erase_regions; i++)
3011                 {
3012                         uint32_t j;
3013                         for (j = 0; j < (cfi_info->erase_region_info[i] & 0xffff) + 1; j++)
3014                         {
3015                                 bank->sectors[sector].offset = offset;
3016                                 bank->sectors[sector].size = ((cfi_info->erase_region_info[i] >> 16) * 256)
3017                                                 * bank->bus_width / bank->chip_width;
3018                                 offset += bank->sectors[sector].size;
3019                                 bank->sectors[sector].is_erased = -1;
3020                                 bank->sectors[sector].is_protected = -1;
3021                                 sector++;
3022                         }
3023                 }
3024                 if (offset != (cfi_info->dev_size * bank->bus_width / bank->chip_width))
3025                 {
3026                         LOG_WARNING("CFI size is 0x%" PRIx32 ", but total sector size is 0x%" PRIx32 "", \
3027                                 (cfi_info->dev_size * bank->bus_width / bank->chip_width), offset);
3028                 }
3029         }
3030
3031         cfi_info->probed = 1;
3032
3033         return ERROR_OK;
3034 }
3035
3036 static int cfi_auto_probe(struct flash_bank *bank)
3037 {
3038         struct cfi_flash_bank *cfi_info = bank->driver_priv;
3039         if (cfi_info->probed)
3040                 return ERROR_OK;
3041         return cfi_probe(bank);
3042 }
3043
3044 static int cfi_intel_protect_check(struct flash_bank *bank)
3045 {
3046         int retval;
3047         struct cfi_flash_bank *cfi_info = bank->driver_priv;
3048         struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
3049         int i;
3050
3051         /* check if block lock bits are supported on this device */
3052         if (!(pri_ext->blk_status_reg_mask & 0x1))
3053                 return ERROR_FLASH_OPERATION_FAILED;
3054
3055         if ((retval = cfi_send_command(bank, 0x90, flash_address(bank, 0, 0x55))) != ERROR_OK)
3056         {
3057                 return retval;
3058         }
3059
3060         for (i = 0; i < bank->num_sectors; i++)
3061         {
3062                 uint8_t block_status;
3063                 retval = cfi_get_u8(bank, i, 0x2, &block_status);
3064                 if (retval != ERROR_OK)
3065                         return retval;
3066
3067                 if (block_status & 1)
3068                         bank->sectors[i].is_protected = 1;
3069                 else
3070                         bank->sectors[i].is_protected = 0;
3071         }
3072
3073         return cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
3074 }
3075
3076 static int cfi_spansion_protect_check(struct flash_bank *bank)
3077 {
3078         int retval;
3079         struct cfi_flash_bank *cfi_info = bank->driver_priv;
3080         struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
3081         int i;
3082
3083         if ((retval = cfi_send_command(bank, 0xaa,
3084                         flash_address(bank, 0, pri_ext->_unlock1))) != ERROR_OK)
3085         {
3086                 return retval;
3087         }
3088
3089         if ((retval = cfi_send_command(bank, 0x55,
3090                         flash_address(bank, 0, pri_ext->_unlock2))) != ERROR_OK)
3091         {
3092                 return retval;
3093         }
3094
3095         if ((retval = cfi_send_command(bank, 0x90,
3096                         flash_address(bank, 0, pri_ext->_unlock1))) != ERROR_OK)
3097         {
3098                 return retval;
3099         }
3100
3101         for (i = 0; i < bank->num_sectors; i++)
3102         {
3103                 uint8_t block_status;
3104                 retval = cfi_get_u8(bank, i, 0x2, &block_status);
3105                 if (retval != ERROR_OK)
3106                         return retval;
3107
3108                 if (block_status & 1)
3109                         bank->sectors[i].is_protected = 1;
3110                 else
3111                         bank->sectors[i].is_protected = 0;
3112         }
3113
3114         return cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
3115 }
3116
3117 static int cfi_protect_check(struct flash_bank *bank)
3118 {
3119         struct cfi_flash_bank *cfi_info = bank->driver_priv;
3120
3121         if (bank->target->state != TARGET_HALTED)
3122         {
3123                 LOG_ERROR("Target not halted");
3124                 return ERROR_TARGET_NOT_HALTED;
3125         }
3126
3127         if (cfi_info->qry[0] != 'Q')
3128                 return ERROR_FLASH_BANK_NOT_PROBED;
3129
3130         switch (cfi_info->pri_id)
3131         {
3132                 case 1:
3133                 case 3:
3134                         return cfi_intel_protect_check(bank);
3135                         break;
3136                 case 2:
3137                         return cfi_spansion_protect_check(bank);
3138                         break;
3139                 default:
3140                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
3141                         break;
3142         }
3143
3144         return ERROR_OK;
3145 }
3146
3147 static int get_cfi_info(struct flash_bank *bank, char *buf, int buf_size)
3148 {
3149         int printed;
3150         struct cfi_flash_bank *cfi_info = bank->driver_priv;
3151
3152         if (cfi_info->qry[0] == 0xff)
3153         {
3154                 printed = snprintf(buf, buf_size, "\ncfi flash bank not probed yet\n");
3155                 return ERROR_OK;
3156         }
3157
3158         if (cfi_info->not_cfi == 0)
3159                 printed = snprintf(buf, buf_size, "\nCFI flash: ");
3160         else
3161                 printed = snprintf(buf, buf_size, "\nnon-CFI flash: ");
3162         buf += printed;
3163         buf_size -= printed;
3164
3165         printed = snprintf(buf, buf_size, "mfr: 0x%4.4x, id:0x%4.4x\n\n",
3166                 cfi_info->manufacturer, cfi_info->device_id);
3167         buf += printed;
3168         buf_size -= printed;
3169
3170         printed = snprintf(buf, buf_size, "qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: "
3171                         "0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x\n",
3172                         cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2],
3173                         cfi_info->pri_id, cfi_info->pri_addr, cfi_info->alt_id, cfi_info->alt_addr);
3174         buf += printed;
3175         buf_size -= printed;
3176
3177         printed = snprintf(buf, buf_size, "Vcc min: %x.%x, Vcc max: %x.%x, "
3178                         "Vpp min: %u.%x, Vpp max: %u.%x\n",
3179                         (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
3180                         (cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
3181                         (cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
3182                         (cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
3183         buf += printed;
3184         buf_size -= printed;
3185
3186         printed = snprintf(buf, buf_size, "typ. word write timeout: %u us, "
3187                         "typ. buf write timeout: %u us, "
3188                         "typ. block erase timeout: %u ms, "
3189                         "typ. chip erase timeout: %u ms\n",
3190                         1 << cfi_info->word_write_timeout_typ,
3191                         1 << cfi_info->buf_write_timeout_typ,
3192                         1 << cfi_info->block_erase_timeout_typ,
3193                         1 << cfi_info->chip_erase_timeout_typ);
3194         buf += printed;
3195         buf_size -= printed;
3196
3197         printed = snprintf(buf, buf_size, "max. word write timeout: %u us, "
3198                         "max. buf write timeout: %u us, max. "
3199                         "block erase timeout: %u ms, max. chip erase timeout: %u ms\n",
3200                         (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
3201                         (1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
3202                         (1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
3203                         (1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
3204         buf += printed;
3205         buf_size -= printed;
3206
3207         printed = snprintf(buf, buf_size, "size: 0x%" PRIx32 ", interface desc: %i, "
3208                         "max buffer write size: 0x%x\n",
3209                         cfi_info->dev_size,
3210                         cfi_info->interface_desc,
3211                         1 << cfi_info->max_buf_write_size);
3212         buf += printed;
3213         buf_size -= printed;
3214
3215         switch (cfi_info->pri_id)
3216         {
3217                 case 1:
3218                 case 3:
3219                         cfi_intel_info(bank, buf, buf_size);
3220                         break;
3221                 case 2:
3222                         cfi_spansion_info(bank, buf, buf_size);
3223                         break;
3224                 default:
3225                         LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
3226                         break;
3227         }
3228
3229         return ERROR_OK;
3230 }
3231
3232 static void cfi_fixup_0002_write_buffer(struct flash_bank *bank, void *param)
3233 {
3234         struct cfi_flash_bank *cfi_info = bank->driver_priv;
3235
3236         /* disable write buffer for M29W128G */
3237         cfi_info->buf_write_timeout_typ = 0;
3238 }
3239
3240 struct flash_driver cfi_flash = {
3241         .name = "cfi",
3242         .flash_bank_command = cfi_flash_bank_command,
3243         .erase = cfi_erase,
3244         .protect = cfi_protect,
3245         .write = cfi_write,
3246         .read = cfi_read,
3247         .probe = cfi_probe,
3248         .auto_probe = cfi_auto_probe,
3249         /* FIXME: access flash at bus_width size */
3250         .erase_check = default_flash_blank_check,
3251         .protect_check = cfi_protect_check,
3252         .info = get_cfi_info,
3253 };