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         int 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 E300 SPI Flash",  0x10e31913 , 0x10014000 },
 140         { NULL,    0,           0          }
 141 };
 142
 143 FLASH_BANK_COMMAND_HANDLER(fespi_flash_bank_command)
 144 {
 145         struct fespi_flash_bank *fespi_info;
 146
 147         LOG_DEBUG("%s", __func__);
 148
 149         if (CMD_ARGC < 6)
 150                 return ERROR_COMMAND_SYNTAX_ERROR;
 151
 152         fespi_info = malloc(sizeof(struct fespi_flash_bank));
 153         if (fespi_info == NULL) {
 154                 LOG_ERROR("not enough memory");
 155                 return ERROR_FAIL;
 156         }
 157
 158         bank->driver_priv = fespi_info;
 159         fespi_info->probed = 0;
 160         fespi_info->ctrl_base = 0;
 161         if (CMD_ARGC >= 7) {
 162                 int temp;
 163                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[6], temp);
 164                 fespi_info->ctrl_base = (uint32_t) temp;
 165                 LOG_DEBUG("ASSUMING FESPI device at ctrl_base = 0x%" TARGET_PRIxADDR,
 166                                 fespi_info->ctrl_base);
 167         }
 168
 169         return ERROR_OK;
 170 }
 171
 172 static int fespi_read_reg(struct flash_bank *bank, uint32_t *value, target_addr_t address)
 173 {
 174         struct target *target = bank->target;
 175         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 176
 177         int result = target_read_u32(target, fespi_info->ctrl_base + address, value);
 178         if (result != ERROR_OK) {
 179                 LOG_ERROR("fespi_read_reg() error at 0x%" TARGET_PRIxADDR,
 180                                 fespi_info->ctrl_base + address);
 181                 return result;
 182         }
 183         return ERROR_OK;
 184 }
 185
 186 static int fespi_write_reg(struct flash_bank *bank, target_addr_t address, uint32_t value)
 187 {                                                               \
 188         struct target *target = bank->target;
 189         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 190
 191         int result = target_write_u32(target, fespi_info->ctrl_base + address, value);
 192         if (result != ERROR_OK) {
 193                 LOG_ERROR("fespi_write_reg() error writing 0x%x to 0x%" TARGET_PRIxADDR,
 194                                 value, fespi_info->ctrl_base + address);
 195                 return result;
 196         }
 197         return ERROR_OK;
 198 }
 199
 200 static int fespi_disable_hw_mode(struct flash_bank *bank)
 201 {
 202         uint32_t fctrl;
 203         if (fespi_read_reg(bank, &fctrl, FESPI_REG_FCTRL) != ERROR_OK)
 204                 return ERROR_FAIL;
 205         return fespi_write_reg(bank, FESPI_REG_FCTRL, fctrl & ~FESPI_FCTRL_EN);
 206 }
 207
 208 static int fespi_enable_hw_mode(struct flash_bank *bank)
 209 {
 210         uint32_t fctrl;
 211         if (fespi_read_reg(bank, &fctrl, FESPI_REG_FCTRL) != ERROR_OK)
 212                 return ERROR_FAIL;
 213         return fespi_write_reg(bank, FESPI_REG_FCTRL, fctrl | FESPI_FCTRL_EN);
 214 }
 215
 216 static int fespi_set_dir(struct flash_bank *bank, bool dir)
 217 {
 218         uint32_t fmt;
 219         if (fespi_read_reg(bank, &fmt, FESPI_REG_FMT) != ERROR_OK)
 220                 return ERROR_FAIL;
 221
 222         return fespi_write_reg(bank, FESPI_REG_FMT,
 223                         (fmt & ~(FESPI_FMT_DIR(0xFFFFFFFF))) | FESPI_FMT_DIR(dir));
 224 }
 225
 226 static int fespi_txwm_wait(struct flash_bank *bank)
 227 {
 228         int64_t start = timeval_ms();
 229
 230         while (1) {
 231                 uint32_t ip;
 232                 if (fespi_read_reg(bank, &ip, FESPI_REG_IP) != ERROR_OK)
 233                         return ERROR_FAIL;
 234                 if (ip & FESPI_IP_TXWM)
 235                         break;
 236                 int64_t now = timeval_ms();
 237                 if (now - start > 1000) {
 238                         LOG_ERROR("ip.txwm didn't get set.");
 239                         return ERROR_TARGET_TIMEOUT;
 240                 }
 241         }
 242
 243         return ERROR_OK;
 244 }
 245
 246 static int fespi_tx(struct flash_bank *bank, uint8_t in)
 247 {
 248         int64_t start = timeval_ms();
 249
 250         while (1) {
 251                 uint32_t txfifo;
 252                 if (fespi_read_reg(bank, &txfifo, FESPI_REG_TXFIFO) != ERROR_OK)
 253                         return ERROR_FAIL;
 254                 if (!(txfifo >> 31))
 255                         break;
 256                 int64_t now = timeval_ms();
 257                 if (now - start > 1000) {
 258                         LOG_ERROR("txfifo stayed negative.");
 259                         return ERROR_TARGET_TIMEOUT;
 260                 }
 261         }
 262
 263         return fespi_write_reg(bank, FESPI_REG_TXFIFO, in);
 264 }
 265
 266 static int fespi_rx(struct flash_bank *bank, uint8_t *out)
 267 {
 268         int64_t start = timeval_ms();
 269         uint32_t value;
 270
 271         while (1) {
 272                 if (fespi_read_reg(bank, &value, FESPI_REG_RXFIFO) != ERROR_OK)
 273                         return ERROR_FAIL;
 274                 if (!(value >> 31))
 275                         break;
 276                 int64_t now = timeval_ms();
 277                 if (now - start > 1000) {
 278                         LOG_ERROR("rxfifo didn't go positive (value=0x%x).", value);
 279                         return ERROR_TARGET_TIMEOUT;
 280                 }
 281         }
 282
 283         if (out)
 284                 *out = value & 0xff;
 285
 286         return ERROR_OK;
 287 }
 288
 289 /* TODO!!! Why don't we need to call this after writing? */
 290 static int fespi_wip(struct flash_bank *bank, int timeout)
 291 {
 292         int64_t endtime;
 293
 294         fespi_set_dir(bank, FESPI_DIR_RX);
 295
 296         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD) != ERROR_OK)
 297                 return ERROR_FAIL;
 298         endtime = timeval_ms() + timeout;
 299
 300         fespi_tx(bank, SPIFLASH_READ_STATUS);
 301         if (fespi_rx(bank, NULL) != ERROR_OK)
 302                 return ERROR_FAIL;
 303
 304         do {
 305                 alive_sleep(1);
 306
 307                 fespi_tx(bank, 0);
 308                 uint8_t rx;
 309                 if (fespi_rx(bank, &rx) != ERROR_OK)
 310                         return ERROR_FAIL;
 311                 if ((rx & SPIFLASH_BSY_BIT) == 0) {
 312                         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO) != ERROR_OK)
 313                                 return ERROR_FAIL;
 314                         fespi_set_dir(bank, FESPI_DIR_TX);
 315                         return ERROR_OK;
 316                 }
 317         } while (timeval_ms() < endtime);
 318
 319         LOG_ERROR("timeout");
 320         return ERROR_FAIL;
 321 }
 322
 323 static int fespi_erase_sector(struct flash_bank *bank, int sector)
 324 {
 325         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 326         int retval;
 327
 328         retval = fespi_tx(bank, SPIFLASH_WRITE_ENABLE);
 329         if (retval != ERROR_OK)
 330                 return retval;
 331         retval = fespi_txwm_wait(bank);
 332         if (retval != ERROR_OK)
 333                 return retval;
 334
 335         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD) != ERROR_OK)
 336                 return ERROR_FAIL;
 337         retval = fespi_tx(bank, fespi_info->dev->erase_cmd);
 338         if (retval != ERROR_OK)
 339                 return retval;
 340         sector = bank->sectors[sector].offset;
 341         retval = fespi_tx(bank, sector >> 16);
 342         if (retval != ERROR_OK)
 343                 return retval;
 344         retval = fespi_tx(bank, sector >> 8);
 345         if (retval != ERROR_OK)
 346                 return retval;
 347         retval = fespi_tx(bank, sector);
 348         if (retval != ERROR_OK)
 349                 return retval;
 350         retval = fespi_txwm_wait(bank);
 351         if (retval != ERROR_OK)
 352                 return retval;
 353         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO) != ERROR_OK)
 354                 return ERROR_FAIL;
 355
 356         retval = fespi_wip(bank, FESPI_MAX_TIMEOUT);
 357         if (retval != ERROR_OK)
 358                 return retval;
 359
 360         return ERROR_OK;
 361 }
 362
 363 static int fespi_erase(struct flash_bank *bank, int first, int last)
 364 {
 365         struct target *target = bank->target;
 366         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 367         int retval = ERROR_OK;
 368         int sector;
 369
 370         LOG_DEBUG("%s: from sector %d to sector %d", __func__, first, last);
 371
 372         if (target->state != TARGET_HALTED) {
 373                 LOG_ERROR("Target not halted");
 374                 return ERROR_TARGET_NOT_HALTED;
 375         }
 376
 377         if ((first < 0) || (last < first) || (last >= bank->num_sectors)) {
 378                 LOG_ERROR("Flash sector invalid");
 379                 return ERROR_FLASH_SECTOR_INVALID;
 380         }
 381
 382         if (!(fespi_info->probed)) {
 383                 LOG_ERROR("Flash bank not probed");
 384                 return ERROR_FLASH_BANK_NOT_PROBED;
 385         }
 386
 387         for (sector = first; sector <= last; sector++) {
 388                 if (bank->sectors[sector].is_protected) {
 389                         LOG_ERROR("Flash sector %d protected", sector);
 390                         return ERROR_FAIL;
 391                 }
 392         }
 393
 394         if (fespi_info->dev->erase_cmd == 0x00)
 395                 return ERROR_FLASH_OPER_UNSUPPORTED;
 396
 397         if (fespi_write_reg(bank, FESPI_REG_TXCTRL, FESPI_TXWM(1)) != ERROR_OK)
 398                 return ERROR_FAIL;
 399         retval = fespi_txwm_wait(bank);
 400         if (retval != ERROR_OK) {
 401                 LOG_ERROR("WM Didn't go high before attempting.");
 402                 return retval;
 403         }
 404
 405         /* Disable Hardware accesses*/
 406         if (fespi_disable_hw_mode(bank) != ERROR_OK)
 407                 return ERROR_FAIL;
 408
 409         /* poll WIP */
 410         retval = fespi_wip(bank, FESPI_PROBE_TIMEOUT);
 411         if (retval != ERROR_OK)
 412                 goto done;
 413
 414         for (sector = first; sector <= last; sector++) {
 415                 retval = fespi_erase_sector(bank, sector);
 416                 if (retval != ERROR_OK)
 417                         goto done;
 418                 keep_alive();
 419         }
 420
 421         /* Switch to HW mode before return to prompt */
 422 done:
 423         if (fespi_enable_hw_mode(bank) != ERROR_OK)
 424                 return ERROR_FAIL;
 425         return retval;
 426 }
 427
 428 static int fespi_protect(struct flash_bank *bank, int set,
 429                 int first, int last)
 430 {
 431         int sector;
 432
 433         for (sector = first; sector <= last; sector++)
 434                 bank->sectors[sector].is_protected = set;
 435         return ERROR_OK;
 436 }
 437
 438 static int slow_fespi_write_buffer(struct flash_bank *bank,
 439                 const uint8_t *buffer, uint32_t offset, uint32_t len)
 440 {
 441         uint32_t ii;
 442
 443         if (offset & 0xFF000000) {
 444                 LOG_ERROR("FESPI interface does not support greater than 3B addressing, can't write to offset 0x%x",
 445                                 offset);
 446                 return ERROR_FAIL;
 447         }
 448
 449         /* TODO!!! assert that len < page size */
 450
 451         fespi_tx(bank, SPIFLASH_WRITE_ENABLE);
 452         fespi_txwm_wait(bank);
 453
 454         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD) != ERROR_OK)
 455                 return ERROR_FAIL;
 456
 457         fespi_tx(bank, SPIFLASH_PAGE_PROGRAM);
 458
 459         fespi_tx(bank, offset >> 16);
 460         fespi_tx(bank, offset >> 8);
 461         fespi_tx(bank, offset);
 462
 463         for (ii = 0; ii < len; ii++)
 464                 fespi_tx(bank, buffer[ii]);
 465
 466         fespi_txwm_wait(bank);
 467
 468         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO) != ERROR_OK)
 469                 return ERROR_FAIL;
 470
 471         keep_alive();
 472
 473         return ERROR_OK;
 474 }
 475
 476 static const uint8_t algorithm_bin[] = {
 477 #include "../../../contrib/loaders/flash/fespi/fespi.inc"
 478 };
 479 #define STEP_EXIT                       4
 480 #define STEP_TX                         8
 481 #define STEP_TXWM_WAIT          12
 482 #define STEP_WRITE_REG          16
 483 #define STEP_WIP_WAIT           20
 484 #define STEP_SET_DIR            24
 485 #define STEP_NOP                        0xff
 486
 487 struct algorithm_steps {
 488         unsigned size;
 489         unsigned used;
 490         uint8_t **steps;
 491 };
 492
 493 static struct algorithm_steps *as_new(void)
 494 {
 495         struct algorithm_steps *as = calloc(1, sizeof(struct algorithm_steps));
 496         as->size = 8;
 497         as->steps = malloc(as->size * sizeof(as->steps[0]));
 498         return as;
 499 }
 500
 501 static struct algorithm_steps *as_delete(struct algorithm_steps *as)
 502 {
 503         for (unsigned step = 0; step < as->used; step++) {
 504                 free(as->steps[step]);
 505                 as->steps[step] = NULL;
 506         }
 507         free(as->steps);
 508         free(as);
 509         return NULL;
 510 }
 511
 512 static int as_empty(struct algorithm_steps *as)
 513 {
 514         for (unsigned s = 0; s < as->used; s++) {
 515                 if (as->steps[s][0] != STEP_NOP)
 516                         return 0;
 517         }
 518         return 1;
 519 }
 520
 521 /* Return size of compiled program. */
 522 static unsigned as_compile(struct algorithm_steps *as, uint8_t *target,
 523                 unsigned target_size)
 524 {
 525         unsigned offset = 0;
 526         bool finish_early = false;
 527         for (unsigned s = 0; s < as->used && !finish_early; s++) {
 528                 unsigned bytes_left = target_size - offset;
 529                 switch (as->steps[s][0]) {
 530                         case STEP_NOP:
 531                                 break;
 532                         case STEP_TX:
 533                                 {
 534                                         unsigned size = as->steps[s][1];
 535                                         if (size + 3 > bytes_left) {
 536                                                 finish_early = true;
 537                                                 break;
 538                                         }
 539                                         memcpy(target + offset, as->steps[s], size + 2);
 540                                         offset += size + 2;
 541                                         break;
 542                                 }
 543                         case STEP_WRITE_REG:
 544                                 if (4 > bytes_left) {
 545                                         finish_early = true;
 546                                         break;
 547                                 }
 548                                 memcpy(target + offset, as->steps[s], 3);
 549                                 offset += 3;
 550                                 break;
 551                         case STEP_SET_DIR:
 552                                 if (3 > bytes_left) {
 553                                         finish_early = true;
 554                                         break;
 555                                 }
 556                                 memcpy(target + offset, as->steps[s], 2);
 557                                 offset += 2;
 558                                 break;
 559                         case STEP_TXWM_WAIT:
 560                         case STEP_WIP_WAIT:
 561                                 if (2 > bytes_left) {
 562                                         finish_early = true;
 563                                         break;
 564                                 }
 565                                 memcpy(target + offset, as->steps[s], 1);
 566                                 offset += 1;
 567                                 break;
 568                         default:
 569                                 assert(0);
 570                 }
 571                 if (!finish_early)
 572                         as->steps[s][0] = STEP_NOP;
 573         }
 574         assert(offset + 1 <= target_size);
 575         target[offset++] = STEP_EXIT;
 576
 577         LOG_DEBUG("%d-byte program:", offset);
 578         for (unsigned i = 0; i < offset;) {
 579                 char buf[80];
 580                 for (unsigned x = 0; i < offset && x < 16; x++, i++)
 581                         sprintf(buf + x*3, "%02x ", target[i]);
 582                 LOG_DEBUG("%s", buf);
 583         }
 584
 585         return offset;
 586 }
 587
 588 static void as_add_step(struct algorithm_steps *as, uint8_t *step)
 589 {
 590         if (as->used == as->size) {
 591                 as->size *= 2;
 592                 as->steps = realloc(as->steps, sizeof(as->steps[0]) * as->size);
 593                 LOG_DEBUG("Increased size to 0x%x", as->size);
 594         }
 595         as->steps[as->used] = step;
 596         as->used++;
 597 }
 598
 599 static void as_add_tx(struct algorithm_steps *as, unsigned count, const uint8_t *data)
 600 {
 601         LOG_DEBUG("count=%d", count);
 602         while (count > 0) {
 603                 unsigned step_count = MIN(count, 255);
 604                 uint8_t *step = malloc(step_count + 2);
 605                 step[0] = STEP_TX;
 606                 step[1] = step_count;
 607                 memcpy(step + 2, data, step_count);
 608                 as_add_step(as, step);
 609                 data += step_count;
 610                 count -= step_count;
 611         }
 612 }
 613
 614 static void as_add_tx1(struct algorithm_steps *as, uint8_t byte)
 615 {
 616         uint8_t data[1];
 617         data[0] = byte;
 618         as_add_tx(as, 1, data);
 619 }
 620
 621 static void as_add_write_reg(struct algorithm_steps *as, uint8_t offset, uint8_t data)
 622 {
 623         uint8_t *step = malloc(3);
 624         step[0] = STEP_WRITE_REG;
 625         step[1] = offset;
 626         step[2] = data;
 627         as_add_step(as, step);
 628 }
 629
 630 static void as_add_txwm_wait(struct algorithm_steps *as)
 631 {
 632         uint8_t *step = malloc(1);
 633         step[0] = STEP_TXWM_WAIT;
 634         as_add_step(as, step);
 635 }
 636
 637 static void as_add_wip_wait(struct algorithm_steps *as)
 638 {
 639         uint8_t *step = malloc(1);
 640         step[0] = STEP_WIP_WAIT;
 641         as_add_step(as, step);
 642 }
 643
 644 static void as_add_set_dir(struct algorithm_steps *as, bool dir)
 645 {
 646         uint8_t *step = malloc(2);
 647         step[0] = STEP_SET_DIR;
 648         step[1] = FESPI_FMT_DIR(dir);
 649         as_add_step(as, step);
 650 }
 651
 652 /* This should write something less than or equal to a page.*/
 653 static int steps_add_buffer_write(struct algorithm_steps *as,
 654                 const uint8_t *buffer, uint32_t chip_offset, uint32_t len)
 655 {
 656         if (chip_offset & 0xFF000000) {
 657                 LOG_ERROR("FESPI interface does not support greater than 3B addressing, can't write to offset 0x%x",
 658                                 chip_offset);
 659                 return ERROR_FAIL;
 660         }
 661
 662         as_add_tx1(as, SPIFLASH_WRITE_ENABLE);
 663         as_add_txwm_wait(as);
 664         as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD);
 665
 666         uint8_t setup[] = {
 667                 SPIFLASH_PAGE_PROGRAM,
 668                 chip_offset >> 16,
 669                 chip_offset >> 8,
 670                 chip_offset,
 671         };
 672         as_add_tx(as, sizeof(setup), setup);
 673
 674         as_add_tx(as, len, buffer);
 675         as_add_txwm_wait(as);
 676         as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO);
 677
 678         /* fespi_wip() */
 679         as_add_set_dir(as, FESPI_DIR_RX);
 680         as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD);
 681         as_add_wip_wait(as);
 682         as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO);
 683         as_add_set_dir(as, FESPI_DIR_TX);
 684
 685         return ERROR_OK;
 686 }
 687
 688 static int steps_execute(struct algorithm_steps *as,
 689                 struct flash_bank *bank, struct working_area *algorithm_wa,
 690                 struct working_area *data_wa)
 691 {
 692         struct target *target = bank->target;
 693         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 694         uint32_t ctrl_base = fespi_info->ctrl_base;
 695         int xlen = riscv_xlen(target);
 696
 697         struct reg_param reg_params[2];
 698         init_reg_param(&reg_params[0], "a0", xlen, PARAM_OUT);
 699         init_reg_param(&reg_params[1], "a1", xlen, PARAM_OUT);
 700         buf_set_u64(reg_params[0].value, 0, xlen, ctrl_base);
 701         buf_set_u64(reg_params[1].value, 0, xlen, data_wa->address);
 702
 703         int retval = ERROR_OK;
 704         while (!as_empty(as)) {
 705                 keep_alive();
 706                 uint8_t *data_buf = malloc(data_wa->size);
 707                 unsigned bytes = as_compile(as, data_buf, data_wa->size);
 708                 retval = target_write_buffer(target, data_wa->address, bytes,
 709                                 data_buf);
 710                 free(data_buf);
 711                 if (retval != ERROR_OK) {
 712                         LOG_ERROR("Failed to write data to 0x%" TARGET_PRIxADDR ": %d",
 713                                         data_wa->address, retval);
 714                         goto exit;
 715                 }
 716
 717                 retval = target_run_algorithm(target, 0, NULL, 2, reg_params,
 718                                 algorithm_wa->address, algorithm_wa->address + 4,
 719                                 10000, NULL);
 720                 if (retval != ERROR_OK) {
 721                         LOG_ERROR("Failed to execute algorithm at 0x%" TARGET_PRIxADDR ": %d",
 722                                         algorithm_wa->address, retval);
 723                         goto exit;
 724                 }
 725         }
 726
 727 exit:
 728         destroy_reg_param(&reg_params[1]);
 729         destroy_reg_param(&reg_params[0]);
 730         return retval;
 731 }
 732
 733 static int fespi_write(struct flash_bank *bank, const uint8_t *buffer,
 734                 uint32_t offset, uint32_t count)
 735 {
 736         struct target *target = bank->target;
 737         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 738         uint32_t cur_count, page_size, page_offset;
 739         int sector;
 740         int retval = ERROR_OK;
 741
 742         LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
 743                         __func__, offset, count);
 744
 745         if (target->state != TARGET_HALTED) {
 746                 LOG_ERROR("Target not halted");
 747                 return ERROR_TARGET_NOT_HALTED;
 748         }
 749
 750         if (offset + count > fespi_info->dev->size_in_bytes) {
 751                 LOG_WARNING("Write past end of flash. Extra data discarded.");
 752                 count = fespi_info->dev->size_in_bytes - offset;
 753         }
 754
 755         /* Check sector protection */
 756         for (sector = 0; sector < bank->num_sectors; sector++) {
 757                 /* Start offset in or before this sector? */
 758                 /* End offset in or behind this sector? */
 759                 if ((offset <
 760                                         (bank->sectors[sector].offset + bank->sectors[sector].size))
 761                                 && ((offset + count - 1) >= bank->sectors[sector].offset)
 762                                 && bank->sectors[sector].is_protected) {
 763                         LOG_ERROR("Flash sector %d protected", sector);
 764                         return ERROR_FAIL;
 765                 }
 766         }
 767
 768         struct working_area *algorithm_wa;
 769         if (target_alloc_working_area(target, sizeof(algorithm_bin),
 770                                 &algorithm_wa) != ERROR_OK) {
 771                 LOG_WARNING("Couldn't allocate %zd-byte working area.",
 772                                 sizeof(algorithm_bin));
 773                 algorithm_wa = NULL;
 774         } else {
 775                 retval = target_write_buffer(target, algorithm_wa->address,
 776                                 sizeof(algorithm_bin), algorithm_bin);
 777                 if (retval != ERROR_OK) {
 778                         LOG_ERROR("Failed to write code to 0x%" TARGET_PRIxADDR ": %d",
 779                                         algorithm_wa->address, retval);
 780                         target_free_working_area(target, algorithm_wa);
 781                         algorithm_wa = NULL;
 782                 }
 783         }
 784
 785         struct working_area *data_wa = NULL;
 786         unsigned data_wa_size = 2 * count;
 787         while (1) {
 788                 if (data_wa_size < 128) {
 789                         LOG_WARNING("Couldn't allocate data working area.");
 790                         target_free_working_area(target, algorithm_wa);
 791                         algorithm_wa = NULL;
 792                 }
 793                 if (target_alloc_working_area_try(target, data_wa_size, &data_wa) ==
 794                                 ERROR_OK) {
 795                         break;
 796                 }
 797
 798                 data_wa_size /= 2;
 799         }
 800
 801         /* If no valid page_size, use reasonable default. */
 802         page_size = fespi_info->dev->pagesize ?
 803                 fespi_info->dev->pagesize : SPIFLASH_DEF_PAGESIZE;
 804
 805         fespi_txwm_wait(bank);
 806
 807         /* Disable Hardware accesses*/
 808         if (fespi_disable_hw_mode(bank) != ERROR_OK)
 809                 return ERROR_FAIL;
 810
 811         struct algorithm_steps *as = as_new();
 812
 813         /* poll WIP */
 814         retval = fespi_wip(bank, FESPI_PROBE_TIMEOUT);
 815         if (retval != ERROR_OK)
 816                 goto err;
 817
 818         page_offset = offset % page_size;
 819         /* central part, aligned words */
 820         while (count > 0) {
 821                 /* clip block at page boundary */
 822                 if (page_offset + count > page_size)
 823                         cur_count = page_size - page_offset;
 824                 else
 825                         cur_count = count;
 826
 827                 if (algorithm_wa)
 828                         retval = steps_add_buffer_write(as, buffer, offset, cur_count);
 829                 else
 830                         retval = slow_fespi_write_buffer(bank, buffer, offset, cur_count);
 831                 if (retval != ERROR_OK)
 832                         goto err;
 833
 834                 page_offset = 0;
 835                 buffer += cur_count;
 836                 offset += cur_count;
 837                 count -= cur_count;
 838         }
 839
 840         if (algorithm_wa)
 841                 retval = steps_execute(as, bank, algorithm_wa, data_wa);
 842
 843 err:
 844         if (algorithm_wa) {
 845                 target_free_working_area(target, data_wa);
 846                 target_free_working_area(target, algorithm_wa);
 847         }
 848
 849         as_delete(as);
 850
 851         /* Switch to HW mode before return to prompt */
 852         if (fespi_enable_hw_mode(bank) != ERROR_OK)
 853                 return ERROR_FAIL;
 854         return retval;
 855 }
 856
 857 /* Return ID of flash device */
 858 /* On exit, SW mode is kept */
 859 static int fespi_read_flash_id(struct flash_bank *bank, uint32_t *id)
 860 {
 861         struct target *target = bank->target;
 862         int retval;
 863
 864         if (target->state != TARGET_HALTED) {
 865                 LOG_ERROR("Target not halted");
 866                 return ERROR_TARGET_NOT_HALTED;
 867         }
 868
 869         fespi_txwm_wait(bank);
 870
 871         /* poll WIP */
 872         retval = fespi_wip(bank, FESPI_PROBE_TIMEOUT);
 873         if (retval != ERROR_OK)
 874                 return retval;
 875
 876         fespi_set_dir(bank, FESPI_DIR_RX);
 877
 878         /* Send SPI command "read ID" */
 879         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD) != ERROR_OK)
 880                 return ERROR_FAIL;
 881
 882         fespi_tx(bank, SPIFLASH_READ_ID);
 883         /* Send dummy bytes to actually read the ID.*/
 884         fespi_tx(bank, 0);
 885         fespi_tx(bank, 0);
 886         fespi_tx(bank, 0);
 887
 888         /* read ID from Receive Register */
 889         *id = 0;
 890         if (fespi_rx(bank, NULL) != ERROR_OK)
 891                 return ERROR_FAIL;
 892         uint8_t rx;
 893         if (fespi_rx(bank, &rx) != ERROR_OK)
 894                 return ERROR_FAIL;
 895         *id = rx;
 896         if (fespi_rx(bank, &rx) != ERROR_OK)
 897                 return ERROR_FAIL;
 898         *id |= (rx << 8);
 899         if (fespi_rx(bank, &rx) != ERROR_OK)
 900                 return ERROR_FAIL;
 901         *id |= (rx << 16);
 902
 903         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO) != ERROR_OK)
 904                 return ERROR_FAIL;
 905
 906         fespi_set_dir(bank, FESPI_DIR_TX);
 907
 908         return ERROR_OK;
 909 }
 910
 911 static int fespi_probe(struct flash_bank *bank)
 912 {
 913         struct target *target = bank->target;
 914         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 915         struct flash_sector *sectors;
 916         uint32_t id = 0; /* silence uninitialized warning */
 917         const struct fespi_target *target_device;
 918         int retval;
 919         uint32_t sectorsize;
 920
 921         if (fespi_info->probed)
 922                 free(bank->sectors);
 923         fespi_info->probed = 0;
 924
 925         if (fespi_info->ctrl_base == 0) {
 926                 for (target_device = target_devices ; target_device->name ; ++target_device)
 927                         if (target_device->tap_idcode == target->tap->idcode)
 928                                 break;
 929
 930                 if (!target_device->name) {
 931                         LOG_ERROR("Device ID 0x%" PRIx32 " is not known as FESPI capable",
 932                                         target->tap->idcode);
 933                         return ERROR_FAIL;
 934                 }
 935
 936                 fespi_info->ctrl_base = target_device->ctrl_base;
 937
 938                 LOG_DEBUG("Valid FESPI on device %s at address 0x%" PRIx32,
 939                                 target_device->name, bank->base);
 940
 941         } else {
 942           LOG_DEBUG("Assuming FESPI as specified at address 0x%" TARGET_PRIxADDR
 943                           " with ctrl at 0x%x", fespi_info->ctrl_base, bank->base);
 944         }
 945
 946         /* read and decode flash ID; returns in SW mode */
 947         if (fespi_write_reg(bank, FESPI_REG_TXCTRL, FESPI_TXWM(1)) != ERROR_OK)
 948                 return ERROR_FAIL;
 949         fespi_set_dir(bank, FESPI_DIR_TX);
 950
 951         /* Disable Hardware accesses*/
 952         if (fespi_disable_hw_mode(bank) != ERROR_OK)
 953                 return ERROR_FAIL;
 954
 955         retval = fespi_read_flash_id(bank, &id);
 956
 957         if (fespi_enable_hw_mode(bank) != ERROR_OK)
 958                 return ERROR_FAIL;
 959         if (retval != ERROR_OK)
 960                 return retval;
 961
 962         fespi_info->dev = NULL;
 963         for (const struct flash_device *p = flash_devices; p->name ; p++)
 964                 if (p->device_id == id) {
 965                         fespi_info->dev = p;
 966                         break;
 967                 }
 968
 969         if (!fespi_info->dev) {
 970                 LOG_ERROR("Unknown flash device (ID 0x%08" PRIx32 ")", id);
 971                 return ERROR_FAIL;
 972         }
 973
 974         LOG_INFO("Found flash device \'%s\' (ID 0x%08" PRIx32 ")",
 975                         fespi_info->dev->name, fespi_info->dev->device_id);
 976
 977         /* Set correct size value */
 978         bank->size = fespi_info->dev->size_in_bytes;
 979
 980         if (bank->size <= (1UL << 16))
 981                 LOG_WARNING("device needs 2-byte addresses - not implemented");
 982         if (bank->size > (1UL << 24))
 983                 LOG_WARNING("device needs paging or 4-byte addresses - not implemented");
 984
 985         /* if no sectors, treat whole bank as single sector */
 986         sectorsize = fespi_info->dev->sectorsize ?
 987                 fespi_info->dev->sectorsize : fespi_info->dev->size_in_bytes;
 988
 989         /* create and fill sectors array */
 990         bank->num_sectors = fespi_info->dev->size_in_bytes / sectorsize;
 991         sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
 992         if (sectors == NULL) {
 993                 LOG_ERROR("not enough memory");
 994                 return ERROR_FAIL;
 995         }
 996
 997         for (int sector = 0; sector < bank->num_sectors; sector++) {
 998                 sectors[sector].offset = sector * sectorsize;
 999                 sectors[sector].size = sectorsize;
1000                 sectors[sector].is_erased = -1;
1001                 sectors[sector].is_protected = 0;
1002         }
1003
1004         bank->sectors = sectors;
1005         fespi_info->probed = 1;
1006         return ERROR_OK;
1007 }
1008
1009 static int fespi_auto_probe(struct flash_bank *bank)
1010 {
1011         struct fespi_flash_bank *fespi_info = bank->driver_priv;
1012         if (fespi_info->probed)
1013                 return ERROR_OK;
1014         return fespi_probe(bank);
1015 }
1016
1017 static int fespi_protect_check(struct flash_bank *bank)
1018 {
1019         /* Nothing to do. Protection is only handled in SW. */
1020         return ERROR_OK;
1021 }
1022
1023 static int get_fespi_info(struct flash_bank *bank, char *buf, int buf_size)
1024 {
1025         struct fespi_flash_bank *fespi_info = bank->driver_priv;
1026
1027         if (!(fespi_info->probed)) {
1028                 snprintf(buf, buf_size,
1029                                 "\nFESPI flash bank not probed yet\n");
1030                 return ERROR_OK;
1031         }
1032
1033         snprintf(buf, buf_size, "\nFESPI flash information:\n"
1034                         "  Device \'%s\' (ID 0x%08" PRIx32 ")\n",
1035                         fespi_info->dev->name, fespi_info->dev->device_id);
1036
1037         return ERROR_OK;
1038 }
1039
1040 struct flash_driver fespi_flash = {
1041         .name = "fespi",
1042         .flash_bank_command = fespi_flash_bank_command,
1043         .erase = fespi_erase,
1044         .protect = fespi_protect,
1045         .write = fespi_write,
1046         .read = default_flash_read,
1047         .probe = fespi_probe,
1048         .auto_probe = fespi_auto_probe,
1049         .erase_check = default_flash_blank_check,
1050         .protect_check = fespi_protect_check,
1051         .info = get_fespi_info,
1052         .free_driver_priv = default_flash_free_driver_priv
1053 };