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 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 == NULL) {
 155                 LOG_ERROR("not enough memory");
 156                 return ERROR_FAIL;
 157         }
 158
 159         bank->driver_priv = fespi_info;
 160         fespi_info->probed = 0;
 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%x 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%x).", 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         retval = fespi_tx(bank, sector >> 16);
 341         if (retval != ERROR_OK)
 342                 return retval;
 343         retval = fespi_tx(bank, sector >> 8);
 344         if (retval != ERROR_OK)
 345                 return retval;
 346         retval = fespi_tx(bank, sector);
 347         if (retval != ERROR_OK)
 348                 return retval;
 349         retval = fespi_txwm_wait(bank);
 350         if (retval != ERROR_OK)
 351                 return retval;
 352         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO) != ERROR_OK)
 353                 return ERROR_FAIL;
 354
 355         retval = fespi_wip(bank, FESPI_MAX_TIMEOUT);
 356         if (retval != ERROR_OK)
 357                 return retval;
 358
 359         return ERROR_OK;
 360 }
 361
 362 static int fespi_erase(struct flash_bank *bank, int first, int last)
 363 {
 364         struct target *target = bank->target;
 365         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 366         int retval = ERROR_OK;
 367         int sector;
 368
 369         LOG_DEBUG("%s: from sector %d to sector %d", __func__, first, last);
 370
 371         if (target->state != TARGET_HALTED) {
 372                 LOG_ERROR("Target not halted");
 373                 return ERROR_TARGET_NOT_HALTED;
 374         }
 375
 376         if ((first < 0) || (last < first) || (last >= bank->num_sectors)) {
 377                 LOG_ERROR("Flash sector invalid");
 378                 return ERROR_FLASH_SECTOR_INVALID;
 379         }
 380
 381         if (!(fespi_info->probed)) {
 382                 LOG_ERROR("Flash bank not probed");
 383                 return ERROR_FLASH_BANK_NOT_PROBED;
 384         }
 385
 386         for (sector = first; sector <= last; sector++) {
 387                 if (bank->sectors[sector].is_protected) {
 388                         LOG_ERROR("Flash sector %d protected", sector);
 389                         return ERROR_FAIL;
 390                 }
 391         }
 392
 393         if (fespi_info->dev->erase_cmd == 0x00)
 394                 return ERROR_FLASH_OPER_UNSUPPORTED;
 395
 396         if (fespi_write_reg(bank, FESPI_REG_TXCTRL, FESPI_TXWM(1)) != ERROR_OK)
 397                 return ERROR_FAIL;
 398         retval = fespi_txwm_wait(bank);
 399         if (retval != ERROR_OK) {
 400                 LOG_ERROR("WM Didn't go high before attempting.");
 401                 return retval;
 402         }
 403
 404         /* Disable Hardware accesses*/
 405         if (fespi_disable_hw_mode(bank) != ERROR_OK)
 406                 return ERROR_FAIL;
 407
 408         /* poll WIP */
 409         retval = fespi_wip(bank, FESPI_PROBE_TIMEOUT);
 410         if (retval != ERROR_OK)
 411                 goto done;
 412
 413         for (sector = first; sector <= last; sector++) {
 414                 retval = fespi_erase_sector(bank, sector);
 415                 if (retval != ERROR_OK)
 416                         goto done;
 417                 keep_alive();
 418         }
 419
 420         /* Switch to HW mode before return to prompt */
 421 done:
 422         if (fespi_enable_hw_mode(bank) != ERROR_OK)
 423                 return ERROR_FAIL;
 424         return retval;
 425 }
 426
 427 static int fespi_protect(struct flash_bank *bank, int set,
 428                 int first, int last)
 429 {
 430         int sector;
 431
 432         for (sector = first; sector <= last; sector++)
 433                 bank->sectors[sector].is_protected = set;
 434         return ERROR_OK;
 435 }
 436
 437 static int slow_fespi_write_buffer(struct flash_bank *bank,
 438                 const uint8_t *buffer, uint32_t offset, uint32_t len)
 439 {
 440         uint32_t ii;
 441
 442         if (offset & 0xFF000000) {
 443                 LOG_ERROR("FESPI interface does not support greater than 3B addressing, can't write to offset 0x%x",
 444                                 offset);
 445                 return ERROR_FAIL;
 446         }
 447
 448         /* TODO!!! assert that len < page size */
 449
 450         fespi_tx(bank, SPIFLASH_WRITE_ENABLE);
 451         fespi_txwm_wait(bank);
 452
 453         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD) != ERROR_OK)
 454                 return ERROR_FAIL;
 455
 456         fespi_tx(bank, SPIFLASH_PAGE_PROGRAM);
 457
 458         fespi_tx(bank, offset >> 16);
 459         fespi_tx(bank, offset >> 8);
 460         fespi_tx(bank, offset);
 461
 462         for (ii = 0; ii < len; ii++)
 463                 fespi_tx(bank, buffer[ii]);
 464
 465         fespi_txwm_wait(bank);
 466
 467         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO) != ERROR_OK)
 468                 return ERROR_FAIL;
 469
 470         keep_alive();
 471
 472         return ERROR_OK;
 473 }
 474
 475 static const uint8_t algorithm_bin[] = {
 476 #include "../../../contrib/loaders/flash/fespi/fespi.inc"
 477 };
 478 #define STEP_EXIT                       4
 479 #define STEP_TX                         8
 480 #define STEP_TXWM_WAIT          12
 481 #define STEP_WRITE_REG          16
 482 #define STEP_WIP_WAIT           20
 483 #define STEP_SET_DIR            24
 484 #define STEP_NOP                        0xff
 485
 486 struct algorithm_steps {
 487         unsigned size;
 488         unsigned used;
 489         uint8_t **steps;
 490 };
 491
 492 static struct algorithm_steps *as_new(void)
 493 {
 494         struct algorithm_steps *as = calloc(1, sizeof(struct algorithm_steps));
 495         as->size = 8;
 496         as->steps = malloc(as->size * sizeof(as->steps[0]));
 497         return as;
 498 }
 499
 500 static struct algorithm_steps *as_delete(struct algorithm_steps *as)
 501 {
 502         for (unsigned step = 0; step < as->used; step++) {
 503                 free(as->steps[step]);
 504                 as->steps[step] = NULL;
 505         }
 506         free(as->steps);
 507         free(as);
 508         return NULL;
 509 }
 510
 511 static int as_empty(struct algorithm_steps *as)
 512 {
 513         for (unsigned s = 0; s < as->used; s++) {
 514                 if (as->steps[s][0] != STEP_NOP)
 515                         return 0;
 516         }
 517         return 1;
 518 }
 519
 520 /* Return size of compiled program. */
 521 static unsigned as_compile(struct algorithm_steps *as, uint8_t *target,
 522                 unsigned target_size)
 523 {
 524         unsigned offset = 0;
 525         bool finish_early = false;
 526         for (unsigned s = 0; s < as->used && !finish_early; s++) {
 527                 unsigned bytes_left = target_size - offset;
 528                 switch (as->steps[s][0]) {
 529                         case STEP_NOP:
 530                                 break;
 531                         case STEP_TX:
 532                                 {
 533                                         unsigned size = as->steps[s][1];
 534                                         if (size + 3 > bytes_left) {
 535                                                 finish_early = true;
 536                                                 break;
 537                                         }
 538                                         memcpy(target + offset, as->steps[s], size + 2);
 539                                         offset += size + 2;
 540                                         break;
 541                                 }
 542                         case STEP_WRITE_REG:
 543                                 if (4 > bytes_left) {
 544                                         finish_early = true;
 545                                         break;
 546                                 }
 547                                 memcpy(target + offset, as->steps[s], 3);
 548                                 offset += 3;
 549                                 break;
 550                         case STEP_SET_DIR:
 551                                 if (3 > bytes_left) {
 552                                         finish_early = true;
 553                                         break;
 554                                 }
 555                                 memcpy(target + offset, as->steps[s], 2);
 556                                 offset += 2;
 557                                 break;
 558                         case STEP_TXWM_WAIT:
 559                         case STEP_WIP_WAIT:
 560                                 if (2 > bytes_left) {
 561                                         finish_early = true;
 562                                         break;
 563                                 }
 564                                 memcpy(target + offset, as->steps[s], 1);
 565                                 offset += 1;
 566                                 break;
 567                         default:
 568                                 assert(0);
 569                 }
 570                 if (!finish_early)
 571                         as->steps[s][0] = STEP_NOP;
 572         }
 573         assert(offset + 1 <= target_size);
 574         target[offset++] = STEP_EXIT;
 575
 576         LOG_DEBUG("%d-byte program:", offset);
 577         for (unsigned i = 0; i < offset;) {
 578                 char buf[80];
 579                 for (unsigned x = 0; i < offset && x < 16; x++, i++)
 580                         sprintf(buf + x*3, "%02x ", target[i]);
 581                 LOG_DEBUG("%s", buf);
 582         }
 583
 584         return offset;
 585 }
 586
 587 static void as_add_step(struct algorithm_steps *as, uint8_t *step)
 588 {
 589         if (as->used == as->size) {
 590                 as->size *= 2;
 591                 as->steps = realloc(as->steps, sizeof(as->steps[0]) * as->size);
 592                 LOG_DEBUG("Increased size to 0x%x", as->size);
 593         }
 594         as->steps[as->used] = step;
 595         as->used++;
 596 }
 597
 598 static void as_add_tx(struct algorithm_steps *as, unsigned count, const uint8_t *data)
 599 {
 600         LOG_DEBUG("count=%d", count);
 601         while (count > 0) {
 602                 unsigned step_count = MIN(count, 255);
 603                 uint8_t *step = malloc(step_count + 2);
 604                 step[0] = STEP_TX;
 605                 step[1] = step_count;
 606                 memcpy(step + 2, data, step_count);
 607                 as_add_step(as, step);
 608                 data += step_count;
 609                 count -= step_count;
 610         }
 611 }
 612
 613 static void as_add_tx1(struct algorithm_steps *as, uint8_t byte)
 614 {
 615         uint8_t data[1];
 616         data[0] = byte;
 617         as_add_tx(as, 1, data);
 618 }
 619
 620 static void as_add_write_reg(struct algorithm_steps *as, uint8_t offset, uint8_t data)
 621 {
 622         uint8_t *step = malloc(3);
 623         step[0] = STEP_WRITE_REG;
 624         step[1] = offset;
 625         step[2] = data;
 626         as_add_step(as, step);
 627 }
 628
 629 static void as_add_txwm_wait(struct algorithm_steps *as)
 630 {
 631         uint8_t *step = malloc(1);
 632         step[0] = STEP_TXWM_WAIT;
 633         as_add_step(as, step);
 634 }
 635
 636 static void as_add_wip_wait(struct algorithm_steps *as)
 637 {
 638         uint8_t *step = malloc(1);
 639         step[0] = STEP_WIP_WAIT;
 640         as_add_step(as, step);
 641 }
 642
 643 static void as_add_set_dir(struct algorithm_steps *as, bool dir)
 644 {
 645         uint8_t *step = malloc(2);
 646         step[0] = STEP_SET_DIR;
 647         step[1] = FESPI_FMT_DIR(dir);
 648         as_add_step(as, step);
 649 }
 650
 651 /* This should write something less than or equal to a page.*/
 652 static int steps_add_buffer_write(struct algorithm_steps *as,
 653                 const uint8_t *buffer, uint32_t chip_offset, uint32_t len)
 654 {
 655         if (chip_offset & 0xFF000000) {
 656                 LOG_ERROR("FESPI interface does not support greater than 3B addressing, can't write to offset 0x%x",
 657                                 chip_offset);
 658                 return ERROR_FAIL;
 659         }
 660
 661         as_add_tx1(as, SPIFLASH_WRITE_ENABLE);
 662         as_add_txwm_wait(as);
 663         as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD);
 664
 665         uint8_t setup[] = {
 666                 SPIFLASH_PAGE_PROGRAM,
 667                 chip_offset >> 16,
 668                 chip_offset >> 8,
 669                 chip_offset,
 670         };
 671         as_add_tx(as, sizeof(setup), setup);
 672
 673         as_add_tx(as, len, buffer);
 674         as_add_txwm_wait(as);
 675         as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO);
 676
 677         /* fespi_wip() */
 678         as_add_set_dir(as, FESPI_DIR_RX);
 679         as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD);
 680         as_add_wip_wait(as);
 681         as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO);
 682         as_add_set_dir(as, FESPI_DIR_TX);
 683
 684         return ERROR_OK;
 685 }
 686
 687 static int steps_execute(struct algorithm_steps *as,
 688                 struct flash_bank *bank, struct working_area *algorithm_wa,
 689                 struct working_area *data_wa)
 690 {
 691         struct target *target = bank->target;
 692         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 693         uint32_t ctrl_base = fespi_info->ctrl_base;
 694         int xlen = riscv_xlen(target);
 695
 696         struct reg_param reg_params[2];
 697         init_reg_param(&reg_params[0], "a0", xlen, PARAM_OUT);
 698         init_reg_param(&reg_params[1], "a1", xlen, PARAM_OUT);
 699         buf_set_u64(reg_params[0].value, 0, xlen, ctrl_base);
 700         buf_set_u64(reg_params[1].value, 0, xlen, data_wa->address);
 701
 702         int retval = ERROR_OK;
 703         while (!as_empty(as)) {
 704                 keep_alive();
 705                 uint8_t *data_buf = malloc(data_wa->size);
 706                 unsigned bytes = as_compile(as, data_buf, data_wa->size);
 707                 retval = target_write_buffer(target, data_wa->address, bytes,
 708                                 data_buf);
 709                 free(data_buf);
 710                 if (retval != ERROR_OK) {
 711                         LOG_ERROR("Failed to write data to " TARGET_ADDR_FMT ": %d",
 712                                         data_wa->address, retval);
 713                         goto exit;
 714                 }
 715
 716                 retval = target_run_algorithm(target, 0, NULL, 2, reg_params,
 717                                 algorithm_wa->address, algorithm_wa->address + 4,
 718                                 10000, NULL);
 719                 if (retval != ERROR_OK) {
 720                         LOG_ERROR("Failed to execute algorithm at " TARGET_ADDR_FMT ": %d",
 721                                         algorithm_wa->address, retval);
 722                         goto exit;
 723                 }
 724         }
 725
 726 exit:
 727         destroy_reg_param(&reg_params[1]);
 728         destroy_reg_param(&reg_params[0]);
 729         return retval;
 730 }
 731
 732 static int fespi_write(struct flash_bank *bank, const uint8_t *buffer,
 733                 uint32_t offset, uint32_t count)
 734 {
 735         struct target *target = bank->target;
 736         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 737         uint32_t cur_count, page_size, page_offset;
 738         int sector;
 739         int retval = ERROR_OK;
 740
 741         LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
 742                         __func__, offset, count);
 743
 744         if (target->state != TARGET_HALTED) {
 745                 LOG_ERROR("Target not halted");
 746                 return ERROR_TARGET_NOT_HALTED;
 747         }
 748
 749         if (offset + count > fespi_info->dev->size_in_bytes) {
 750                 LOG_WARNING("Write past end of flash. Extra data discarded.");
 751                 count = fespi_info->dev->size_in_bytes - offset;
 752         }
 753
 754         /* Check sector protection */
 755         for (sector = 0; sector < bank->num_sectors; sector++) {
 756                 /* Start offset in or before this sector? */
 757                 /* End offset in or behind this sector? */
 758                 if ((offset <
 759                                         (bank->sectors[sector].offset + bank->sectors[sector].size))
 760                                 && ((offset + count - 1) >= bank->sectors[sector].offset)
 761                                 && bank->sectors[sector].is_protected) {
 762                         LOG_ERROR("Flash sector %d protected", sector);
 763                         return ERROR_FAIL;
 764                 }
 765         }
 766
 767         struct working_area *algorithm_wa;
 768         if (target_alloc_working_area(target, sizeof(algorithm_bin),
 769                                 &algorithm_wa) != ERROR_OK) {
 770                 LOG_WARNING("Couldn't allocate %zd-byte working area.",
 771                                 sizeof(algorithm_bin));
 772                 algorithm_wa = NULL;
 773         } else {
 774                 retval = target_write_buffer(target, algorithm_wa->address,
 775                                 sizeof(algorithm_bin), algorithm_bin);
 776                 if (retval != ERROR_OK) {
 777                         LOG_ERROR("Failed to write code to " TARGET_ADDR_FMT ": %d",
 778                                         algorithm_wa->address, retval);
 779                         target_free_working_area(target, algorithm_wa);
 780                         algorithm_wa = NULL;
 781                 }
 782         }
 783
 784         struct working_area *data_wa = NULL;
 785         unsigned data_wa_size = 2 * count;
 786         while (1) {
 787                 if (data_wa_size < 128) {
 788                         LOG_WARNING("Couldn't allocate data working area.");
 789                         target_free_working_area(target, algorithm_wa);
 790                         algorithm_wa = NULL;
 791                 }
 792                 if (target_alloc_working_area_try(target, data_wa_size, &data_wa) ==
 793                                 ERROR_OK) {
 794                         break;
 795                 }
 796
 797                 data_wa_size /= 2;
 798         }
 799
 800         /* If no valid page_size, use reasonable default. */
 801         page_size = fespi_info->dev->pagesize ?
 802                 fespi_info->dev->pagesize : SPIFLASH_DEF_PAGESIZE;
 803
 804         fespi_txwm_wait(bank);
 805
 806         /* Disable Hardware accesses*/
 807         if (fespi_disable_hw_mode(bank) != ERROR_OK)
 808                 return ERROR_FAIL;
 809
 810         struct algorithm_steps *as = as_new();
 811
 812         /* poll WIP */
 813         retval = fespi_wip(bank, FESPI_PROBE_TIMEOUT);
 814         if (retval != ERROR_OK)
 815                 goto err;
 816
 817         page_offset = offset % page_size;
 818         /* central part, aligned words */
 819         while (count > 0) {
 820                 /* clip block at page boundary */
 821                 if (page_offset + count > page_size)
 822                         cur_count = page_size - page_offset;
 823                 else
 824                         cur_count = count;
 825
 826                 if (algorithm_wa)
 827                         retval = steps_add_buffer_write(as, buffer, offset, cur_count);
 828                 else
 829                         retval = slow_fespi_write_buffer(bank, buffer, offset, cur_count);
 830                 if (retval != ERROR_OK)
 831                         goto err;
 832
 833                 page_offset = 0;
 834                 buffer += cur_count;
 835                 offset += cur_count;
 836                 count -= cur_count;
 837         }
 838
 839         if (algorithm_wa)
 840                 retval = steps_execute(as, bank, algorithm_wa, data_wa);
 841
 842 err:
 843         if (algorithm_wa) {
 844                 target_free_working_area(target, data_wa);
 845                 target_free_working_area(target, algorithm_wa);
 846         }
 847
 848         as_delete(as);
 849
 850         /* Switch to HW mode before return to prompt */
 851         if (fespi_enable_hw_mode(bank) != ERROR_OK)
 852                 return ERROR_FAIL;
 853         return retval;
 854 }
 855
 856 /* Return ID of flash device */
 857 /* On exit, SW mode is kept */
 858 static int fespi_read_flash_id(struct flash_bank *bank, uint32_t *id)
 859 {
 860         struct target *target = bank->target;
 861         int retval;
 862
 863         if (target->state != TARGET_HALTED) {
 864                 LOG_ERROR("Target not halted");
 865                 return ERROR_TARGET_NOT_HALTED;
 866         }
 867
 868         fespi_txwm_wait(bank);
 869
 870         /* poll WIP */
 871         retval = fespi_wip(bank, FESPI_PROBE_TIMEOUT);
 872         if (retval != ERROR_OK)
 873                 return retval;
 874
 875         fespi_set_dir(bank, FESPI_DIR_RX);
 876
 877         /* Send SPI command "read ID" */
 878         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD) != ERROR_OK)
 879                 return ERROR_FAIL;
 880
 881         fespi_tx(bank, SPIFLASH_READ_ID);
 882         /* Send dummy bytes to actually read the ID.*/
 883         fespi_tx(bank, 0);
 884         fespi_tx(bank, 0);
 885         fespi_tx(bank, 0);
 886
 887         /* read ID from Receive Register */
 888         *id = 0;
 889         if (fespi_rx(bank, NULL) != ERROR_OK)
 890                 return ERROR_FAIL;
 891         uint8_t rx;
 892         if (fespi_rx(bank, &rx) != ERROR_OK)
 893                 return ERROR_FAIL;
 894         *id = rx;
 895         if (fespi_rx(bank, &rx) != ERROR_OK)
 896                 return ERROR_FAIL;
 897         *id |= (rx << 8);
 898         if (fespi_rx(bank, &rx) != ERROR_OK)
 899                 return ERROR_FAIL;
 900         *id |= (rx << 16);
 901
 902         if (fespi_write_reg(bank, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO) != ERROR_OK)
 903                 return ERROR_FAIL;
 904
 905         fespi_set_dir(bank, FESPI_DIR_TX);
 906
 907         return ERROR_OK;
 908 }
 909
 910 static int fespi_probe(struct flash_bank *bank)
 911 {
 912         struct target *target = bank->target;
 913         struct fespi_flash_bank *fespi_info = bank->driver_priv;
 914         struct flash_sector *sectors;
 915         uint32_t id = 0; /* silence uninitialized warning */
 916         const struct fespi_target *target_device;
 917         int retval;
 918         uint32_t sectorsize;
 919
 920         if (fespi_info->probed)
 921                 free(bank->sectors);
 922         fespi_info->probed = 0;
 923
 924         if (fespi_info->ctrl_base == 0) {
 925                 for (target_device = target_devices ; target_device->name ; ++target_device)
 926                         if (target_device->tap_idcode == target->tap->idcode)
 927                                 break;
 928
 929                 if (!target_device->name) {
 930                         LOG_ERROR("Device ID 0x%" PRIx32 " is not known as FESPI capable",
 931                                         target->tap->idcode);
 932                         return ERROR_FAIL;
 933                 }
 934
 935                 fespi_info->ctrl_base = target_device->ctrl_base;
 936
 937                 LOG_DEBUG("Valid FESPI on device %s at address " TARGET_ADDR_FMT,
 938                                 target_device->name, bank->base);
 939
 940         } else {
 941           LOG_DEBUG("Assuming FESPI as specified at address " TARGET_ADDR_FMT
 942                           " with ctrl at " TARGET_ADDR_FMT, fespi_info->ctrl_base,
 943                           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 const 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 };