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