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

   1 /***************************************************************************
   2  *   Copyright (C) 2010 by Antonio Borneo <borneo.antonio@gmail.com>       *
   3  *   Modified by Megan Wachs <megan@sifive.com> from the original stmsmi.c *
   4  *                                                                         *
   5  *   This program is free software; you can redistribute it and/or modify  *
   6  *   it under the terms of the GNU General Public License as published by  *
   7  *   the Free Software Foundation; either version 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   This program is distributed in the hope that it will be useful,       *
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  13  *   GNU General Public License for more details.                          *
  14  *                                                                         *
  15  *   You should have received a copy of the GNU General Public License     *
  16  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  17  ***************************************************************************/
  18
  19 /* The Freedom E SPI controller is a SPI bus controller
  20  * specifically designed for SPI Flash Memories on Freedom E platforms.
  21  *
  22  * Two working modes are available:
  23  * - SW mode: the SPI is controlled by SW. Any custom commands can be sent
  24  *   on the bus. Writes are only possible in this mode.
  25  * - HW mode: Memory content is directly
  26  *   accessible in CPU memory space. CPU can read and execute memory content.
  27  */
  28
  29 /* ATTENTION:
  30  * To have flash memory mapped in CPU memory space, the controller
  31  * must have "HW mode" enabled.
  32  * 1) The command "reset init" has to initialize the controller and put
  33  *    it in HW mode (this is actually the default out of reset for Freedom E systems).
  34  * 2) every command in this file have to return to prompt in HW mode. */
  35
  36 #ifdef HAVE_CONFIG_H
  37 #include "config.h"
  38 #endif
  39
  40 #include "imp.h"
  41 #include "spi.h"
  42 #include <jtag/jtag.h>
  43 #include <helper/time_support.h>
  44 #include <target/algorithm.h>
  45 #include "target/riscv/riscv.h"
  46
  47 /* Register offsets */
  48
  49 #define FESPI_REG_SCKDIV          0x00
  50 #define FESPI_REG_SCKMODE         0x04
  51 #define FESPI_REG_CSID            0x10
  52 #define FESPI_REG_CSDEF           0x14
  53 #define FESPI_REG_CSMODE          0x18
  54
  55 #define FESPI_REG_DCSSCK          0x28
  56 #define FESPI_REG_DSCKCS          0x2a
  57 #define FESPI_REG_DINTERCS        0x2c
  58 #define FESPI_REG_DINTERXFR       0x2e
  59
  60 #define FESPI_REG_FMT             0x40
  61 #define FESPI_REG_TXFIFO          0x48
  62 #define FESPI_REG_RXFIFO          0x4c
  63 #define FESPI_REG_TXCTRL          0x50
  64 #define FESPI_REG_RXCTRL          0x54
  65
  66 #define FESPI_REG_FCTRL           0x60
  67 #define FESPI_REG_FFMT            0x64
  68
  69 #define FESPI_REG_IE              0x70
  70 #define FESPI_REG_IP              0x74
  71
  72 /* Fields */
  73
  74 #define FESPI_SCK_POL             0x1
  75 #define FESPI_SCK_PHA             0x2
  76
  77 #define FESPI_FMT_PROTO(x)        ((x) & 0x3)
  78 #define FESPI_FMT_ENDIAN(x)       (((x) & 0x1) << 2)
  79 #define FESPI_FMT_DIR(x)          (((x) & 0x1) << 3)
  80 #define FESPI_FMT_LEN(x)          (((x) & 0xf) << 16)
  81
  82 /* TXCTRL register */
  83 #define FESPI_TXWM(x)             ((x) & 0xffff)
  84 /* RXCTRL register */
  85 #define FESPI_RXWM(x)             ((x) & 0xffff)
  86
  87 #define FESPI_IP_TXWM             0x1
  88 #define FESPI_IP_RXWM             0x2
  89
  90 #define FESPI_FCTRL_EN            0x1
  91
  92 #define FESPI_INSN_CMD_EN         0x1
  93 #define FESPI_INSN_ADDR_LEN(x)    (((x) & 0x7) << 1)
  94 #define FESPI_INSN_PAD_CNT(x)     (((x) & 0xf) << 4)
  95 #define FESPI_INSN_CMD_PROTO(x)   (((x) & 0x3) << 8)
  96 #define FESPI_INSN_ADDR_PROTO(x)  (((x) & 0x3) << 10)
  97 #define FESPI_INSN_DATA_PROTO(x)  (((x) & 0x3) << 12)
  98 #define FESPI_INSN_CMD_CODE(x)    (((x) & 0xff) << 16)
  99 #define FESPI_INSN_PAD_CODE(x)    (((x) & 0xff) << 24)
 100
 101 /* Values */
 102
 103 #define FESPI_CSMODE_AUTO         0
 104 #define FESPI_CSMODE_HOLD         2
 105 #define FESPI_CSMODE_OFF          3
 106
 107 #define FESPI_DIR_RX              0
 108 #define FESPI_DIR_TX              1
 109
 110 #define FESPI_PROTO_S             0
 111 #define FESPI_PROTO_D             1
 112 #define FESPI_PROTO_Q             2
 113
 114 #define FESPI_ENDIAN_MSB          0
 115 #define FESPI_ENDIAN_LSB          1
 116
 117
 118 /* Timeout in ms */
 119 #define FESPI_CMD_TIMEOUT   (100)
 120 #define FESPI_PROBE_TIMEOUT (100)
 121 #define FESPI_MAX_TIMEOUT  (3000)
 122
 123
 124 struct fespi_flash_bank {
 125         bool probed;
 126         target_addr_t ctrl_base;
 127         const struct flash_device *dev;
 128 };
 129
 130 struct fespi_target {
 131         char *name;
 132         uint32_t tap_idcode;
 133         uint32_t ctrl_base;
 134 };
 135
 136 /* TODO !!! What is the right naming convention here? */
 137 static const struct fespi_target target_devices[] = {
 138         /* name,   tap_idcode, ctrl_base */
 139         { "Freedom E310-G000 SPI Flash", 0x10e31913, 0x10014000 },
 140         { "Freedom E310-G002 SPI Flash", 0x20000913, 0x10014000 },
 141         { NULL, 0, 0 }
 142 };
 143
 144 FLASH_BANK_COMMAND_HANDLER(fespi_flash_bank_command)
 145 {
 146         struct fespi_flash_bank *fespi_info;
 147
 148         LOG_DEBUG("%s", __func__);
 149
 150         if (CMD_ARGC < 6)
 151                 return ERROR_COMMAND_SYNTAX_ERROR;
 152
 153         fespi_info = malloc(sizeof(struct fespi_flash_bank));
 154         if (!fespi_info) {
 155                 LOG_ERROR("not enough memory");
 156                 return ERROR_FAIL;
 157         }
 158
 159         bank->driver_priv = fespi_info;
 160         fespi_info->probed = false;
 161         fespi_info->ctrl_base = 0;
 162         if (CMD_ARGC >= 7) {
 163                 COMMAND_PARSE_ADDRESS(CMD_ARGV[6], fespi_info->ctrl_base);
 164                 LOG_DEBUG("ASSUMING FESPI device at ctrl_base = " TARGET_ADDR_FMT,
 165                                 fespi_info->ctrl_base);
 166         }
 167
 168         return ERROR_OK;
 169 }
 170
 171 static int fespi_read_reg(struct flash_bank *bank, uint32_t *value, target_addr_t address)
 172 {
 173         struct target *target = bank->target;
 174         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 175
 176         int result = target_read_u32(target, fespi_info->ctrl_base + address, value);
 177         if (result != ERROR_OK) {
 178                 LOG_ERROR("fespi_read_reg() error at " TARGET_ADDR_FMT,
 179                                 fespi_info->ctrl_base + address);
 180                 return result;
 181         }
 182         return ERROR_OK;
 183 }
 184
 185 static int fespi_write_reg(struct flash_bank *bank, target_addr_t address, uint32_t value)
 186 {
 187         struct target *target = bank->target;
 188         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 189
 190         int result = target_write_u32(target, fespi_info->ctrl_base + address, value);
 191         if (result != ERROR_OK) {
 192                 LOG_ERROR("fespi_write_reg() error writing 0x%" PRIx32 " to " TARGET_ADDR_FMT,
 193                                 value, fespi_info->ctrl_base + address);
 194                 return result;
 195         }
 196         return ERROR_OK;
 197 }
 198
 199 static int fespi_disable_hw_mode(struct flash_bank *bank)
 200 {
 201         uint32_t fctrl;
 202         if (fespi_read_reg(bank, &fctrl, FESPI_REG_FCTRL) != ERROR_OK)
 203                 return ERROR_FAIL;
 204         return fespi_write_reg(bank, FESPI_REG_FCTRL, fctrl & ~FESPI_FCTRL_EN);
 205 }
 206
 207 static int fespi_enable_hw_mode(struct flash_bank *bank)
 208 {
 209         uint32_t fctrl;
 210         if (fespi_read_reg(bank, &fctrl, FESPI_REG_FCTRL) != ERROR_OK)
 211                 return ERROR_FAIL;
 212         return fespi_write_reg(bank, FESPI_REG_FCTRL, fctrl | FESPI_FCTRL_EN);
 213 }
 214
 215 static int fespi_set_dir(struct flash_bank *bank, bool dir)
 216 {
 217         uint32_t fmt;
 218         if (fespi_read_reg(bank, &fmt, FESPI_REG_FMT) != ERROR_OK)
 219                 return ERROR_FAIL;
 220
 221         return fespi_write_reg(bank, FESPI_REG_FMT,
 222                         (fmt & ~(FESPI_FMT_DIR(0xFFFFFFFF))) | FESPI_FMT_DIR(dir));
 223 }
 224
 225 static int fespi_txwm_wait(struct flash_bank *bank)
 226 {
 227         int64_t start = timeval_ms();
 228
 229         while (1) {
 230                 uint32_t ip;
 231                 if (fespi_read_reg(bank, &ip, FESPI_REG_IP) != ERROR_OK)
 232                         return ERROR_FAIL;
 233                 if (ip & FESPI_IP_TXWM)
 234                         break;
 235                 int64_t now = timeval_ms();
 236                 if (now - start > 1000) {
 237                         LOG_ERROR("ip.txwm didn't get set.");
 238                         return ERROR_TARGET_TIMEOUT;
 239                 }
 240         }
 241
 242         return ERROR_OK;
 243 }
 244
 245 static int fespi_tx(struct flash_bank *bank, uint8_t in)
 246 {
 247         int64_t start = timeval_ms();
 248
 249         while (1) {
 250                 uint32_t txfifo;
 251                 if (fespi_read_reg(bank, &txfifo, FESPI_REG_TXFIFO) != ERROR_OK)
 252                         return ERROR_FAIL;
 253                 if (!(txfifo >> 31))
 254                         break;
 255                 int64_t now = timeval_ms();
 256                 if (now - start > 1000) {
 257                         LOG_ERROR("txfifo stayed negative.");
 258                         return ERROR_TARGET_TIMEOUT;
 259                 }
 260         }
 261
 262         return fespi_write_reg(bank, FESPI_REG_TXFIFO, in);
 263 }
 264
 265 static int fespi_rx(struct flash_bank *bank, uint8_t *out)
 266 {
 267         int64_t start = timeval_ms();
 268         uint32_t value;
 269
 270         while (1) {
 271                 if (fespi_read_reg(bank, &value, FESPI_REG_RXFIFO) != ERROR_OK)
 272                         return ERROR_FAIL;
 273                 if (!(value >> 31))
 274                         break;
 275                 int64_t now = timeval_ms();
 276                 if (now - start > 1000) {
 277                         LOG_ERROR("rxfifo didn't go positive (value=0x%" PRIx32 ").", value);
 278                         return ERROR_TARGET_TIMEOUT;
 279                 }
 280         }
 281
 282         if (out)
 283                 *out = value & 0xff;
 284
 285         return ERROR_OK;
 286 }
 287
 288 /* TODO!!! Why don't we need to call this after writing? */
 289 static int fespi_wip(struct flash_bank *bank, int timeout)
 290 {
 291         int64_t endtime;
 292
 293         fespi_set_dir(bank, FESPI_DIR_RX);
 294
 295         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD) != ERROR_OK)
 296                 return ERROR_FAIL;
 297         endtime = timeval_ms() + timeout;
 298
 299         fespi_tx(bank, SPIFLASH_READ_STATUS);
 300         if (fespi_rx(bank, NULL) != ERROR_OK)
 301                 return ERROR_FAIL;
 302
 303         do {
 304                 alive_sleep(1);
 305
 306                 fespi_tx(bank, 0);
 307                 uint8_t rx;
 308                 if (fespi_rx(bank, &rx) != ERROR_OK)
 309                         return ERROR_FAIL;
 310                 if ((rx & SPIFLASH_BSY_BIT) == 0) {
 311                         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO) != ERROR_OK)
 312                                 return ERROR_FAIL;
 313                         fespi_set_dir(bank, FESPI_DIR_TX);
 314                         return ERROR_OK;
 315                 }
 316         } while (timeval_ms() < endtime);
 317
 318         LOG_ERROR("timeout");
 319         return ERROR_FAIL;
 320 }
 321
 322 static int fespi_erase_sector(struct flash_bank *bank, int sector)
 323 {
 324         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 325         int retval;
 326
 327         retval = fespi_tx(bank, SPIFLASH_WRITE_ENABLE);
 328         if (retval != ERROR_OK)
 329                 return retval;
 330         retval = fespi_txwm_wait(bank);
 331         if (retval != ERROR_OK)
 332                 return retval;
 333
 334         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD) != ERROR_OK)
 335                 return ERROR_FAIL;
 336         retval = fespi_tx(bank, fespi_info->dev->erase_cmd);
 337         if (retval != ERROR_OK)
 338                 return retval;
 339         sector = bank->sectors[sector].offset;
 340         if (bank->size > 0x1000000) {
 341                 retval = fespi_tx(bank, sector >> 24);
 342                 if (retval != ERROR_OK)
 343                         return retval;
 344         }
 345         retval = fespi_tx(bank, sector >> 16);
 346         if (retval != ERROR_OK)
 347                 return retval;
 348         retval = fespi_tx(bank, sector >> 8);
 349         if (retval != ERROR_OK)
 350                 return retval;
 351         retval = fespi_tx(bank, sector);
 352         if (retval != ERROR_OK)
 353                 return retval;
 354         retval = fespi_txwm_wait(bank);
 355         if (retval != ERROR_OK)
 356                 return retval;
 357         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO) != ERROR_OK)
 358                 return ERROR_FAIL;
 359
 360         retval = fespi_wip(bank, FESPI_MAX_TIMEOUT);
 361         if (retval != ERROR_OK)
 362                 return retval;
 363
 364         return ERROR_OK;
 365 }
 366
 367 static int fespi_erase(struct flash_bank *bank, unsigned int first,
 368                 unsigned int last)
 369 {
 370         struct target *target = bank->target;
 371         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 372         int retval = ERROR_OK;
 373
 374         LOG_DEBUG("%s: from sector %u to sector %u", __func__, first, last);
 375
 376         if (target->state != TARGET_HALTED) {
 377                 LOG_ERROR("Target not halted");
 378                 return ERROR_TARGET_NOT_HALTED;
 379         }
 380
 381         if ((last < first) || (last >= bank->num_sectors)) {
 382                 LOG_ERROR("Flash sector invalid");
 383                 return ERROR_FLASH_SECTOR_INVALID;
 384         }
 385
 386         if (!(fespi_info->probed)) {
 387                 LOG_ERROR("Flash bank not probed");
 388                 return ERROR_FLASH_BANK_NOT_PROBED;
 389         }
 390
 391         for (unsigned int sector = first; sector <= last; sector++) {
 392                 if (bank->sectors[sector].is_protected) {
 393                         LOG_ERROR("Flash sector %u protected", sector);
 394                         return ERROR_FAIL;
 395                 }
 396         }
 397
 398         if (fespi_info->dev->erase_cmd == 0x00)
 399                 return ERROR_FLASH_OPER_UNSUPPORTED;
 400
 401         if (fespi_write_reg(bank, FESPI_REG_TXCTRL, FESPI_TXWM(1)) != ERROR_OK)
 402                 return ERROR_FAIL;
 403         retval = fespi_txwm_wait(bank);
 404         if (retval != ERROR_OK) {
 405                 LOG_ERROR("WM Didn't go high before attempting.");
 406                 return retval;
 407         }
 408
 409         /* Disable Hardware accesses*/
 410         if (fespi_disable_hw_mode(bank) != ERROR_OK)
 411                 return ERROR_FAIL;
 412
 413         /* poll WIP */
 414         retval = fespi_wip(bank, FESPI_PROBE_TIMEOUT);
 415         if (retval != ERROR_OK)
 416                 goto done;
 417
 418         for (unsigned int sector = first; sector <= last; sector++) {
 419                 retval = fespi_erase_sector(bank, sector);
 420                 if (retval != ERROR_OK)
 421                         goto done;
 422                 keep_alive();
 423         }
 424
 425         /* Switch to HW mode before return to prompt */
 426 done:
 427         if (fespi_enable_hw_mode(bank) != ERROR_OK)
 428                 return ERROR_FAIL;
 429         return retval;
 430 }
 431
 432 static int fespi_protect(struct flash_bank *bank, int set,
 433                 unsigned int first, unsigned int last)
 434 {
 435         for (unsigned int sector = first; sector <= last; sector++)
 436                 bank->sectors[sector].is_protected = set;
 437         return ERROR_OK;
 438 }
 439
 440 static int slow_fespi_write_buffer(struct flash_bank *bank,
 441                 const uint8_t *buffer, uint32_t offset, uint32_t len)
 442 {
 443         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 444         uint32_t ii;
 445
 446         /* TODO!!! assert that len < page size */
 447
 448         if (fespi_tx(bank, SPIFLASH_WRITE_ENABLE) != ERROR_OK)
 449                 return ERROR_FAIL;
 450         if (fespi_txwm_wait(bank) != ERROR_OK)
 451                 return ERROR_FAIL;
 452
 453         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD) != ERROR_OK)
 454                 return ERROR_FAIL;
 455
 456         if (fespi_tx(bank, fespi_info->dev->pprog_cmd) != ERROR_OK)
 457                 return ERROR_FAIL;
 458
 459         if (bank->size > 0x1000000 && fespi_tx(bank, offset >> 24) != ERROR_OK)
 460                 return ERROR_FAIL;
 461         if (fespi_tx(bank, offset >> 16) != ERROR_OK)
 462                 return ERROR_FAIL;
 463         if (fespi_tx(bank, offset >> 8) != ERROR_OK)
 464                 return ERROR_FAIL;
 465         if (fespi_tx(bank, offset) != ERROR_OK)
 466                 return ERROR_FAIL;
 467
 468         for (ii = 0; ii < len; ii++) {
 469                 if (fespi_tx(bank, buffer[ii]) != ERROR_OK)
 470                         return ERROR_FAIL;
 471         }
 472
 473         if (fespi_txwm_wait(bank) != ERROR_OK)
 474                 return ERROR_FAIL;
 475
 476         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO) != ERROR_OK)
 477                 return ERROR_FAIL;
 478
 479         keep_alive();
 480
 481         return ERROR_OK;
 482 }
 483
 484 static const uint8_t riscv32_bin[] = {
 485 #include "../../../contrib/loaders/flash/fespi/riscv32_fespi.inc"
 486 };
 487
 488 static const uint8_t riscv64_bin[] = {
 489 #include "../../../contrib/loaders/flash/fespi/riscv64_fespi.inc"
 490 };
 491
 492 static int fespi_write(struct flash_bank *bank, const uint8_t *buffer,
 493                 uint32_t offset, uint32_t count)
 494 {
 495         struct target *target = bank->target;
 496         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 497         uint32_t cur_count, page_size;
 498         int retval = ERROR_OK;
 499
 500         LOG_DEBUG("bank->size=0x%x offset=0x%08" PRIx32 " count=0x%08" PRIx32,
 501                         bank->size, offset, count);
 502
 503         if (target->state != TARGET_HALTED) {
 504                 LOG_ERROR("Target not halted");
 505                 return ERROR_TARGET_NOT_HALTED;
 506         }
 507
 508         if (offset + count > fespi_info->dev->size_in_bytes) {
 509                 LOG_WARNING("Write past end of flash. Extra data discarded.");
 510                 count = fespi_info->dev->size_in_bytes - offset;
 511         }
 512
 513         /* Check sector protection */
 514         for (unsigned int sector = 0; sector < bank->num_sectors; sector++) {
 515                 /* Start offset in or before this sector? */
 516                 /* End offset in or behind this sector? */
 517                 if ((offset <
 518                                         (bank->sectors[sector].offset + bank->sectors[sector].size))
 519                                 && ((offset + count - 1) >= bank->sectors[sector].offset)
 520                                 && bank->sectors[sector].is_protected) {
 521                         LOG_ERROR("Flash sector %u protected", sector);
 522                         return ERROR_FAIL;
 523                 }
 524         }
 525
 526         unsigned int xlen = riscv_xlen(target);
 527         struct working_area *algorithm_wa = NULL;
 528         struct working_area *data_wa = NULL;
 529         const uint8_t *bin;
 530         size_t bin_size;
 531         if (xlen == 32) {
 532                 bin = riscv32_bin;
 533                 bin_size = sizeof(riscv32_bin);
 534         } else {
 535                 bin = riscv64_bin;
 536                 bin_size = sizeof(riscv64_bin);
 537         }
 538
 539         unsigned data_wa_size = 0;
 540         if (target_alloc_working_area(target, bin_size, &algorithm_wa) == ERROR_OK) {
 541                 retval = target_write_buffer(target, algorithm_wa->address,
 542                                 bin_size, bin);
 543                 if (retval != ERROR_OK) {
 544                         LOG_ERROR("Failed to write code to " TARGET_ADDR_FMT ": %d",
 545                                         algorithm_wa->address, retval);
 546                         target_free_working_area(target, algorithm_wa);
 547                         algorithm_wa = NULL;
 548
 549                 } else {
 550                         data_wa_size = MIN(target_get_working_area_avail(target), count);
 551                         if (data_wa_size < 128) {
 552                                 LOG_WARNING("Couldn't allocate data working area.");
 553                                 target_free_working_area(target, algorithm_wa);
 554                                 algorithm_wa = NULL;
 555                         } else if (target_alloc_working_area(target, data_wa_size, &data_wa) != ERROR_OK) {
 556                                 target_free_working_area(target, algorithm_wa);
 557                                 algorithm_wa = NULL;
 558                         }
 559                 }
 560         } else {
 561                 LOG_WARNING("Couldn't allocate %zd-byte working area.", bin_size);
 562                 algorithm_wa = NULL;
 563         }
 564
 565         /* If no valid page_size, use reasonable default. */
 566         page_size = fespi_info->dev->pagesize ?
 567                 fespi_info->dev->pagesize : SPIFLASH_DEF_PAGESIZE;
 568
 569         if (algorithm_wa) {
 570                 struct reg_param reg_params[6];
 571                 init_reg_param(&reg_params[0], "a0", xlen, PARAM_IN_OUT);
 572                 init_reg_param(&reg_params[1], "a1", xlen, PARAM_OUT);
 573                 init_reg_param(&reg_params[2], "a2", xlen, PARAM_OUT);
 574                 init_reg_param(&reg_params[3], "a3", xlen, PARAM_OUT);
 575                 init_reg_param(&reg_params[4], "a4", xlen, PARAM_OUT);
 576                 init_reg_param(&reg_params[5], "a5", xlen, PARAM_OUT);
 577
 578                 while (count > 0) {
 579                         cur_count = MIN(count, data_wa_size);
 580                         buf_set_u64(reg_params[0].value, 0, xlen, fespi_info->ctrl_base);
 581                         buf_set_u64(reg_params[1].value, 0, xlen, page_size);
 582                         buf_set_u64(reg_params[2].value, 0, xlen, data_wa->address);
 583                         buf_set_u64(reg_params[3].value, 0, xlen, offset);
 584                         buf_set_u64(reg_params[4].value, 0, xlen, cur_count);
 585                         buf_set_u64(reg_params[5].value, 0, xlen,
 586                                         fespi_info->dev->pprog_cmd | (bank->size > 0x1000000 ? 0x100 : 0));
 587
 588                         retval = target_write_buffer(target, data_wa->address, cur_count,
 589                                         buffer);
 590                         if (retval != ERROR_OK) {
 591                                 LOG_DEBUG("Failed to write %d bytes to " TARGET_ADDR_FMT ": %d",
 592                                                 cur_count, data_wa->address, retval);
 593                                 goto err;
 594                         }
 595
 596                         LOG_DEBUG("write(ctrl_base=0x%" TARGET_PRIxADDR ", page_size=0x%x, "
 597                                         "address=0x%" TARGET_PRIxADDR ", offset=0x%" PRIx32
 598                                         ", count=0x%" PRIx32 "), buffer=%02x %02x %02x %02x %02x %02x ..." PRIx32,
 599                                         fespi_info->ctrl_base, page_size, data_wa->address, offset, cur_count,
 600                                         buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
 601                         retval = target_run_algorithm(target, 0, NULL,
 602                                         ARRAY_SIZE(reg_params), reg_params,
 603                                         algorithm_wa->address, 0, cur_count * 2, NULL);
 604                         if (retval != ERROR_OK) {
 605                                 LOG_ERROR("Failed to execute algorithm at " TARGET_ADDR_FMT ": %d",
 606                                                 algorithm_wa->address, retval);
 607                                 goto err;
 608                         }
 609
 610                         uint64_t algorithm_result = buf_get_u64(reg_params[0].value, 0, xlen);
 611                         if (algorithm_result != 0) {
 612                                 LOG_ERROR("Algorithm returned error %" PRId64, algorithm_result);
 613                                 retval = ERROR_FAIL;
 614                                 goto err;
 615                         }
 616
 617                         buffer += cur_count;
 618                         offset += cur_count;
 619                         count -= cur_count;
 620                 }
 621
 622                 target_free_working_area(target, data_wa);
 623                 target_free_working_area(target, algorithm_wa);
 624
 625         } else {
 626                 fespi_txwm_wait(bank);
 627
 628                 /* Disable Hardware accesses*/
 629                 if (fespi_disable_hw_mode(bank) != ERROR_OK)
 630                         return ERROR_FAIL;
 631
 632                 /* poll WIP */
 633                 retval = fespi_wip(bank, FESPI_PROBE_TIMEOUT);
 634                 if (retval != ERROR_OK)
 635                         goto err;
 636
 637                 uint32_t page_offset = offset % page_size;
 638                 /* central part, aligned words */
 639                 while (count > 0) {
 640                         /* clip block at page boundary */
 641                         if (page_offset + count > page_size)
 642                                 cur_count = page_size - page_offset;
 643                         else
 644                                 cur_count = count;
 645
 646                         retval = slow_fespi_write_buffer(bank, buffer, offset, cur_count);
 647                         if (retval != ERROR_OK)
 648                                 goto err;
 649
 650                         page_offset = 0;
 651                         buffer += cur_count;
 652                         offset += cur_count;
 653                         count -= cur_count;
 654                 }
 655
 656                 /* Switch to HW mode before return to prompt */
 657                 if (fespi_enable_hw_mode(bank) != ERROR_OK)
 658                         return ERROR_FAIL;
 659         }
 660
 661         return ERROR_OK;
 662
 663 err:
 664         target_free_working_area(target, data_wa);
 665         target_free_working_area(target, algorithm_wa);
 666
 667         /* Switch to HW mode before return to prompt */
 668         if (fespi_enable_hw_mode(bank) != ERROR_OK)
 669                 return ERROR_FAIL;
 670
 671         return retval;
 672 }
 673
 674 /* Return ID of flash device */
 675 /* On exit, SW mode is kept */
 676 static int fespi_read_flash_id(struct flash_bank *bank, uint32_t *id)
 677 {
 678         struct target *target = bank->target;
 679         int retval;
 680
 681         if (target->state != TARGET_HALTED) {
 682                 LOG_ERROR("Target not halted");
 683                 return ERROR_TARGET_NOT_HALTED;
 684         }
 685
 686         fespi_txwm_wait(bank);
 687
 688         /* poll WIP */
 689         retval = fespi_wip(bank, FESPI_PROBE_TIMEOUT);
 690         if (retval != ERROR_OK)
 691                 return retval;
 692
 693         fespi_set_dir(bank, FESPI_DIR_RX);
 694
 695         /* Send SPI command "read ID" */
 696         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD) != ERROR_OK)
 697                 return ERROR_FAIL;
 698
 699         fespi_tx(bank, SPIFLASH_READ_ID);
 700         /* Send dummy bytes to actually read the ID.*/
 701         fespi_tx(bank, 0);
 702         fespi_tx(bank, 0);
 703         fespi_tx(bank, 0);
 704
 705         /* read ID from Receive Register */
 706         *id = 0;
 707         if (fespi_rx(bank, NULL) != ERROR_OK)
 708                 return ERROR_FAIL;
 709         uint8_t rx;
 710         if (fespi_rx(bank, &rx) != ERROR_OK)
 711                 return ERROR_FAIL;
 712         *id = rx;
 713         if (fespi_rx(bank, &rx) != ERROR_OK)
 714                 return ERROR_FAIL;
 715         *id |= (rx << 8);
 716         if (fespi_rx(bank, &rx) != ERROR_OK)
 717                 return ERROR_FAIL;
 718         *id |= (rx << 16);
 719
 720         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO) != ERROR_OK)
 721                 return ERROR_FAIL;
 722
 723         fespi_set_dir(bank, FESPI_DIR_TX);
 724
 725         return ERROR_OK;
 726 }
 727
 728 static int fespi_probe(struct flash_bank *bank)
 729 {
 730         struct target *target = bank->target;
 731         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 732         struct flash_sector *sectors;
 733         uint32_t id = 0; /* silence uninitialized warning */
 734         const struct fespi_target *target_device;
 735         int retval;
 736         uint32_t sectorsize;
 737
 738         if (fespi_info->probed)
 739                 free(bank->sectors);
 740         fespi_info->probed = false;
 741
 742         if (fespi_info->ctrl_base == 0) {
 743                 for (target_device = target_devices ; target_device->name ; ++target_device)
 744                         if (target_device->tap_idcode == target->tap->idcode)
 745                                 break;
 746
 747                 if (!target_device->name) {
 748                         LOG_ERROR("Device ID 0x%" PRIx32 " is not known as FESPI capable",
 749                                         target->tap->idcode);
 750                         return ERROR_FAIL;
 751                 }
 752
 753                 fespi_info->ctrl_base = target_device->ctrl_base;
 754
 755                 LOG_DEBUG("Valid FESPI on device %s at address " TARGET_ADDR_FMT,
 756                                 target_device->name, bank->base);
 757
 758         } else {
 759           LOG_DEBUG("Assuming FESPI as specified at address " TARGET_ADDR_FMT
 760                           " with ctrl at " TARGET_ADDR_FMT, fespi_info->ctrl_base,
 761                           bank->base);
 762         }
 763
 764         /* read and decode flash ID; returns in SW mode */
 765         if (fespi_write_reg(bank, FESPI_REG_TXCTRL, FESPI_TXWM(1)) != ERROR_OK)
 766                 return ERROR_FAIL;
 767         fespi_set_dir(bank, FESPI_DIR_TX);
 768
 769         /* Disable Hardware accesses*/
 770         if (fespi_disable_hw_mode(bank) != ERROR_OK)
 771                 return ERROR_FAIL;
 772
 773         retval = fespi_read_flash_id(bank, &id);
 774
 775         if (fespi_enable_hw_mode(bank) != ERROR_OK)
 776                 return ERROR_FAIL;
 777         if (retval != ERROR_OK)
 778                 return retval;
 779
 780         fespi_info->dev = NULL;
 781         for (const struct flash_device *p = flash_devices; p->name ; p++)
 782                 if (p->device_id == id) {
 783                         fespi_info->dev = p;
 784                         break;
 785                 }
 786
 787         if (!fespi_info->dev) {
 788                 LOG_ERROR("Unknown flash device (ID 0x%08" PRIx32 ")", id);
 789                 return ERROR_FAIL;
 790         }
 791
 792         LOG_INFO("Found flash device \'%s\' (ID 0x%08" PRIx32 ")",
 793                         fespi_info->dev->name, fespi_info->dev->device_id);
 794
 795         /* Set correct size value */
 796         bank->size = fespi_info->dev->size_in_bytes;
 797
 798         if (bank->size <= (1UL << 16))
 799                 LOG_WARNING("device needs 2-byte addresses - not implemented");
 800
 801         /* if no sectors, treat whole bank as single sector */
 802         sectorsize = fespi_info->dev->sectorsize ?
 803                 fespi_info->dev->sectorsize : fespi_info->dev->size_in_bytes;
 804
 805         /* create and fill sectors array */
 806         bank->num_sectors = fespi_info->dev->size_in_bytes / sectorsize;
 807         sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
 808         if (!sectors) {
 809                 LOG_ERROR("not enough memory");
 810                 return ERROR_FAIL;
 811         }
 812
 813         for (unsigned int sector = 0; sector < bank->num_sectors; sector++) {
 814                 sectors[sector].offset = sector * sectorsize;
 815                 sectors[sector].size = sectorsize;
 816                 sectors[sector].is_erased = -1;
 817                 sectors[sector].is_protected = 0;
 818         }
 819
 820         bank->sectors = sectors;
 821         fespi_info->probed = true;
 822         return ERROR_OK;
 823 }
 824
 825 static int fespi_auto_probe(struct flash_bank *bank)
 826 {
 827         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 828         if (fespi_info->probed)
 829                 return ERROR_OK;
 830         return fespi_probe(bank);
 831 }
 832
 833 static int fespi_protect_check(struct flash_bank *bank)
 834 {
 835         /* Nothing to do. Protection is only handled in SW. */
 836         return ERROR_OK;
 837 }
 838
 839 static int get_fespi_info(struct flash_bank *bank, struct command_invocation *cmd)
 840 {
 841         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 842
 843         if (!(fespi_info->probed)) {
 844                 command_print_sameline(cmd, "\nFESPI flash bank not probed yet\n");
 845                 return ERROR_OK;
 846         }
 847
 848         command_print_sameline(cmd, "\nFESPI flash information:\n"
 849                         "  Device \'%s\' (ID 0x%08" PRIx32 ")\n",
 850                         fespi_info->dev->name, fespi_info->dev->device_id);
 851
 852         return ERROR_OK;
 853 }
 854
 855 const struct flash_driver fespi_flash = {
 856         .name = "fespi",
 857         .flash_bank_command = fespi_flash_bank_command,
 858         .erase = fespi_erase,
 859         .protect = fespi_protect,
 860         .write = fespi_write,
 861         .read = default_flash_read,
 862         .probe = fespi_probe,
 863         .auto_probe = fespi_auto_probe,
 864         .erase_check = default_flash_blank_check,
 865         .protect_check = fespi_protect_check,
 866         .info = get_fespi_info,
 867         .free_driver_priv = default_flash_free_driver_priv
 868 };