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

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