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

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2
   3 /***************************************************************************
   4  *   Copyright (C) 2016 - 2019 by Andreas Bolsch                           *
   5  *   andreas.bolsch@mni.thm.de                                             *
   6  *                                                                         *
   7  *   Copyright (C) 2010 by Antonio Borneo                                  *
   8  *   borneo.antonio@gmail.com                                              *
   9  ***************************************************************************/
  10
  11 /* STM QuadSPI (QSPI) and OctoSPI (OCTOSPI) controller are SPI bus controllers
  12  * specifically designed for SPI memories.
  13  * Two working modes are available:
  14  * - indirect mode: the SPI is controlled by SW. Any custom commands can be sent
  15  *   on the bus.
  16  * - memory mapped mode: the SPI is under QSPI/OCTOSPI control. Memory content
  17  *   is directly accessible in CPU memory space. CPU can read and execute from
  18  *   memory (but not write to) */
  19
  20 /* ATTENTION:
  21  * To have flash mapped in CPU memory space, the QSPI/OCTOSPI controller
  22  * has to be in "memory mapped mode". This requires following constraints:
  23  * 1) The command "reset init" has to initialize QSPI/OCTOSPI controller and put
  24  *    it in memory mapped mode;
  25  * 2) every command in this file has to return to prompt in memory mapped mode. */
  26
  27 #ifdef HAVE_CONFIG_H
  28 #include "config.h"
  29 #endif
  30
  31 #include "imp.h"
  32 #include <helper/binarybuffer.h>
  33 #include <helper/bits.h>
  34 #include <helper/time_support.h>
  35 #include <target/algorithm.h>
  36 #include <target/armv7m.h>
  37 #include <target/image.h>
  38 #include "stmqspi.h"
  39 #include "sfdp.h"
  40
  41 /* deprecated */
  42 #undef SPIFLASH_READ
  43 #undef SPIFLASH_PAGE_PROGRAM
  44
  45 /* saved mode settings */
  46 #define QSPI_MODE (stmqspi_info->saved_ccr & \
  47         (0xF0000000U | QSPI_DCYC_MASK | QSPI_4LINE_MODE | QSPI_ALTB_MODE | QSPI_ADDR4))
  48
  49 /* saved read mode settings but indirect read instead of memory mapped
  50  * in particular, use the dummy cycle setting from this saved setting */
  51 #define QSPI_CCR_READ (QSPI_READ_MODE | (stmqspi_info->saved_ccr & \
  52         (0xF0000000U | QSPI_DCYC_MASK | QSPI_4LINE_MODE | QSPI_ALTB_MODE | QSPI_ADDR4 | 0xFF)))
  53
  54 /* QSPI_CCR for various other commands, these never use dummy cycles nor alternate bytes */
  55 #define QSPI_CCR_READ_STATUS \
  56         ((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB) | \
  57         (QSPI_READ_MODE | SPIFLASH_READ_STATUS))
  58
  59 #define QSPI_CCR_READ_ID \
  60         ((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB) | \
  61         (QSPI_READ_MODE | SPIFLASH_READ_ID))
  62
  63 #define QSPI_CCR_READ_MID \
  64         ((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB) | \
  65         (QSPI_READ_MODE | SPIFLASH_READ_MID))
  66
  67 /* always use 3-byte addresses for read SFDP */
  68 #define QSPI_CCR_READ_SFDP \
  69         ((QSPI_MODE & ~QSPI_DCYC_MASK & ~QSPI_ADDR4 & QSPI_NO_ALTB) | \
  70         (QSPI_READ_MODE | QSPI_ADDR3 | SPIFLASH_READ_SFDP))
  71
  72 #define QSPI_CCR_WRITE_ENABLE \
  73         ((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB & QSPI_NO_DATA) | \
  74         (QSPI_WRITE_MODE | SPIFLASH_WRITE_ENABLE))
  75
  76 #define QSPI_CCR_SECTOR_ERASE \
  77         ((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB & QSPI_NO_DATA) | \
  78         (QSPI_WRITE_MODE | stmqspi_info->dev.erase_cmd))
  79
  80 #define QSPI_CCR_MASS_ERASE \
  81         ((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB & QSPI_NO_DATA) | \
  82         (QSPI_WRITE_MODE | stmqspi_info->dev.chip_erase_cmd))
  83
  84 #define QSPI_CCR_PAGE_PROG \
  85         ((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB) | \
  86         (QSPI_WRITE_MODE | stmqspi_info->dev.pprog_cmd))
  87
  88 /* saved mode settings */
  89 #define OCTOSPI_MODE (stmqspi_info->saved_cr & 0xCFFFFFFF)
  90
  91 #define OPI_MODE ((stmqspi_info->saved_ccr & OCTOSPI_ISIZE_MASK) != 0)
  92
  93 #define OCTOSPI_MODE_CCR (stmqspi_info->saved_ccr & \
  94         (0xF0000000U | OCTOSPI_8LINE_MODE | OCTOSPI_ALTB_MODE | OCTOSPI_ADDR4))
  95
  96 /* use saved ccr for read */
  97 #define OCTOSPI_CCR_READ OCTOSPI_MODE_CCR
  98
  99 /* OCTOSPI_CCR for various other commands, these never use alternate bytes      *
 100  * for READ_STATUS and READ_ID, 4-byte address 0                                                        *
 101  * 4 dummy cycles must sent in OPI mode when DQS is disabled. However, when     *
 102  * DQS is enabled, some STM32 devices need at least 6 dummy cycles for          *
 103  * proper operation, but otherwise the actual number has no effect!                     *
 104  * E.g. RM0432 Rev. 7 is incorrect regarding this: L4R9 works well with 4       *
 105  * dummy clocks whereas L4P5 not at all.                                                                        *
 106  */
 107 #define OPI_DUMMY \
 108         ((stmqspi_info->saved_ccr & OCTOSPI_DQSEN) ? 6U : 4U)
 109
 110 #define OCTOSPI_CCR_READ_STATUS \
 111         ((OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & \
 112         (OPI_MODE ? ~0U : OCTOSPI_NO_ADDR) & OCTOSPI_NO_ALTB))
 113
 114 #define OCTOSPI_CCR_READ_ID \
 115         ((OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & \
 116         (OPI_MODE ? ~0U : OCTOSPI_NO_ADDR) & OCTOSPI_NO_ALTB))
 117
 118 #define OCTOSPI_CCR_READ_MID OCTOSPI_CCR_READ_ID
 119
 120 /* 4-byte address in octo mode, else 3-byte address for read SFDP */
 121 #define OCTOSPI_CCR_READ_SFDP(len) \
 122         ((OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & ~OCTOSPI_ADDR4 & OCTOSPI_NO_ALTB) | \
 123         (((len) < 4) ? OCTOSPI_ADDR3 : OCTOSPI_ADDR4))
 124
 125 #define OCTOSPI_CCR_WRITE_ENABLE \
 126         ((OCTOSPI_MODE_CCR & OCTOSPI_NO_ADDR & OCTOSPI_NO_ALTB & OCTOSPI_NO_DATA))
 127
 128 #define OCTOSPI_CCR_SECTOR_ERASE \
 129         ((OCTOSPI_MODE_CCR & OCTOSPI_NO_ALTB & OCTOSPI_NO_DATA))
 130
 131 #define OCTOSPI_CCR_MASS_ERASE \
 132         ((OCTOSPI_MODE_CCR & OCTOSPI_NO_ADDR & OCTOSPI_NO_ALTB & OCTOSPI_NO_DATA))
 133
 134 #define OCTOSPI_CCR_PAGE_PROG \
 135         ((OCTOSPI_MODE_CCR & QSPI_NO_ALTB))
 136
 137 #define SPI_ADSIZE (((stmqspi_info->saved_ccr >> SPI_ADSIZE_POS) & 0x3) + 1)
 138
 139 #define OPI_CMD(cmd) ((OPI_MODE ? ((((uint16_t)(cmd)) << 8) | (~(cmd) & 0xFFU)) : (cmd)))
 140
 141 /* convert uint32_t into 4 uint8_t in little endian byte order */
 142 static inline uint32_t h_to_le_32(uint32_t val)
 143 {
 144         uint32_t result;
 145
 146         h_u32_to_le((uint8_t *)&result, val);
 147         return result;
 148 }
 149
 150 /* Timeout in ms */
 151 #define SPI_CMD_TIMEOUT                 (100)
 152 #define SPI_PROBE_TIMEOUT               (100)
 153 #define SPI_MAX_TIMEOUT                 (2000)
 154 #define SPI_MASS_ERASE_TIMEOUT  (400000)
 155
 156 struct sector_info {
 157         uint32_t offset;
 158         uint32_t size;
 159         uint32_t result;
 160 };
 161
 162 struct stmqspi_flash_bank {
 163         bool probed;
 164         char devname[32];
 165         bool octo;
 166         struct flash_device dev;
 167         uint32_t io_base;
 168         uint32_t saved_cr;      /* in particular FSEL, DFM bit mask in QUADSPI_CR *AND* OCTOSPI_CR */
 169         uint32_t saved_ccr; /* different meaning for QUADSPI and OCTOSPI */
 170         uint32_t saved_tcr;     /* only for OCTOSPI */
 171         uint32_t saved_ir;      /* only for OCTOSPI */
 172         unsigned int sfdp_dummy1;       /* number of dummy bytes for SFDP read for flash1 and octo */
 173         unsigned int sfdp_dummy2;       /* number of dummy bytes for SFDP read for flash2 */
 174 };
 175
 176 static inline int octospi_cmd(struct flash_bank *bank, uint32_t mode,
 177                 uint32_t ccr, uint32_t ir)
 178 {
 179         struct target *target = bank->target;
 180         const struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
 181         const uint32_t io_base = stmqspi_info->io_base;
 182
 183         int retval = target_write_u32(target, io_base + OCTOSPI_CR,
 184                 OCTOSPI_MODE | mode);
 185
 186         if (retval != ERROR_OK)
 187                 return retval;
 188
 189         retval = target_write_u32(target, io_base + OCTOSPI_TCR,
 190                 (stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK) |
 191                 ((OPI_MODE && (mode == OCTOSPI_READ_MODE)) ?
 192                 (OPI_DUMMY << OCTOSPI_DCYC_POS) : 0));
 193
 194         if (retval != ERROR_OK)
 195                 return retval;
 196
 197         retval = target_write_u32(target, io_base + OCTOSPI_CCR, ccr);
 198
 199         if (retval != ERROR_OK)
 200                 return retval;
 201
 202         return target_write_u32(target, io_base + OCTOSPI_IR, OPI_CMD(ir));
 203 }
 204
 205 FLASH_BANK_COMMAND_HANDLER(stmqspi_flash_bank_command)
 206 {
 207         struct stmqspi_flash_bank *stmqspi_info;
 208         uint32_t io_base;
 209
 210         LOG_DEBUG("%s", __func__);
 211
 212         if (CMD_ARGC < 7)
 213                 return ERROR_COMMAND_SYNTAX_ERROR;
 214
 215         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[6], io_base);
 216
 217         stmqspi_info = malloc(sizeof(struct stmqspi_flash_bank));
 218         if (!stmqspi_info) {
 219                 LOG_ERROR("not enough memory");
 220                 return ERROR_FAIL;
 221         }
 222
 223         bank->driver_priv = stmqspi_info;
 224         stmqspi_info->sfdp_dummy1 = 0;
 225         stmqspi_info->sfdp_dummy2 = 0;
 226         stmqspi_info->probed = false;
 227         stmqspi_info->io_base = io_base;
 228
 229         return ERROR_OK;
 230 }
 231
 232 /* Poll busy flag */
 233 /* timeout in ms */
 234 static int poll_busy(struct flash_bank *bank, int timeout)
 235 {
 236         struct target *target = bank->target;
 237         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
 238         uint32_t io_base = stmqspi_info->io_base;
 239         long long endtime;
 240
 241         endtime = timeval_ms() + timeout;
 242         do {
 243                 uint32_t spi_sr;
 244                 int retval = target_read_u32(target, io_base + SPI_SR, &spi_sr);
 245
 246                 if (retval != ERROR_OK)
 247                         return retval;
 248
 249                 if ((spi_sr & BIT(SPI_BUSY)) == 0) {
 250                         /* Clear transmit finished flag */
 251                         return target_write_u32(target, io_base + SPI_FCR, BIT(SPI_TCF));
 252                 } else
 253                         LOG_DEBUG("busy: 0x%08X", spi_sr);
 254                 alive_sleep(1);
 255         } while (timeval_ms() < endtime);
 256
 257         LOG_ERROR("Timeout while polling BUSY");
 258         return ERROR_FLASH_OPERATION_FAILED;
 259 }
 260
 261 static int stmqspi_abort(struct flash_bank *bank)
 262 {
 263         struct target *target = bank->target;
 264         const struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
 265         const uint32_t io_base = stmqspi_info->io_base;
 266         uint32_t cr;
 267
 268         int retval = target_read_u32(target, io_base + SPI_CR, &cr);
 269
 270         if (retval != ERROR_OK)
 271                 cr = 0;
 272
 273         return target_write_u32(target, io_base + SPI_CR, cr | BIT(SPI_ABORT));
 274 }
 275
 276 /* Set to memory-mapped mode, e.g. after an error */
 277 static int set_mm_mode(struct flash_bank *bank)
 278 {
 279         struct target *target = bank->target;
 280         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
 281         uint32_t io_base = stmqspi_info->io_base;
 282         int retval;
 283
 284         /* Reset Address register bits 0 and 1, see various errata sheets */
 285         retval = target_write_u32(target, io_base + SPI_AR, 0x0);
 286         if (retval != ERROR_OK)
 287                 return retval;
 288
 289         /* Abort any previous operation */
 290         retval = stmqspi_abort(bank);
 291         if (retval != ERROR_OK)
 292                 return retval;
 293
 294         /* Wait for busy to be cleared */
 295         retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
 296         if (retval != ERROR_OK)
 297                 return retval;
 298
 299         /* Finally switch to memory mapped mode */
 300         if (IS_OCTOSPI) {
 301                 retval = target_write_u32(target, io_base + OCTOSPI_CR,
 302                         OCTOSPI_MODE | OCTOSPI_MM_MODE);
 303                 if (retval == ERROR_OK)
 304                         retval = target_write_u32(target, io_base + OCTOSPI_CCR,
 305                                 stmqspi_info->saved_ccr);
 306                 if (retval == ERROR_OK)
 307                         retval = target_write_u32(target, io_base + OCTOSPI_TCR,
 308                                 stmqspi_info->saved_tcr);
 309                 if (retval == ERROR_OK)
 310                         retval = target_write_u32(target, io_base + OCTOSPI_IR,
 311                                 stmqspi_info->saved_ir);
 312         } else {
 313                 retval = target_write_u32(target, io_base + QSPI_CR,
 314                         stmqspi_info->saved_cr);
 315                 if (retval == ERROR_OK)
 316                         retval = target_write_u32(target, io_base + QSPI_CCR,
 317                                 stmqspi_info->saved_ccr);
 318         }
 319         return retval;
 320 }
 321
 322 /* Read the status register of the external SPI flash chip(s). */
 323 static int read_status_reg(struct flash_bank *bank, uint16_t *status)
 324 {
 325         struct target *target = bank->target;
 326         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
 327         uint32_t io_base = stmqspi_info->io_base;
 328         uint8_t data;
 329         int count, retval;
 330
 331         /* Abort any previous operation */
 332         retval = stmqspi_abort(bank);
 333         if (retval != ERROR_OK)
 334                 return retval;
 335
 336         /* Wait for busy to be cleared */
 337         retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
 338         if (retval != ERROR_OK)
 339                 goto err;
 340
 341         /* Read always two (for DTR mode) bytes per chip */
 342         count = 2;
 343         retval = target_write_u32(target, io_base + SPI_DLR,
 344                 ((stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 2 * count : count) - 1);
 345         if (retval != ERROR_OK)
 346                 goto err;
 347
 348         /* Read status */
 349         if (IS_OCTOSPI) {
 350                 retval = octospi_cmd(bank, OCTOSPI_READ_MODE, OCTOSPI_CCR_READ_STATUS,
 351                         SPIFLASH_READ_STATUS);
 352                 if (OPI_MODE) {
 353                         /* Dummy address 0, only required for 8-line mode */
 354                         retval = target_write_u32(target, io_base + SPI_AR, 0);
 355                         if (retval != ERROR_OK)
 356                                 goto err;
 357                 }
 358         } else
 359                 retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_STATUS);
 360         if (retval != ERROR_OK)
 361                 goto err;
 362
 363         *status = 0;
 364
 365         /* for debugging only */
 366         uint32_t dummy;
 367         (void)target_read_u32(target, io_base + SPI_SR, &dummy);
 368
 369         for ( ; count > 0; --count) {
 370                 if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH)))
 371                         != BIT(SPI_FSEL_FLASH)) {
 372                         /* get status of flash 1 in dual mode or flash 1 only mode */
 373                         retval = target_read_u8(target, io_base + SPI_DR, &data);
 374                         if (retval != ERROR_OK)
 375                                 goto err;
 376                         *status |= data;
 377                 }
 378
 379                 if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH))) != 0) {
 380                         /* get status of flash 2 in dual mode or flash 2 only mode */
 381                         retval = target_read_u8(target, io_base + SPI_DR, &data);
 382                         if (retval != ERROR_OK)
 383                                 goto err;
 384                         *status |= ((uint16_t)data) << 8;
 385                 }
 386         }
 387
 388         LOG_DEBUG("flash status regs: 0x%04" PRIx16, *status);
 389
 390 err:
 391         return retval;
 392 }
 393
 394 /* check for WIP (write in progress) bit(s) in status register(s) */
 395 /* timeout in ms */
 396 static int wait_till_ready(struct flash_bank *bank, int timeout)
 397 {
 398         uint16_t status;
 399         int retval;
 400         long long endtime;
 401
 402         endtime = timeval_ms() + timeout;
 403         do {
 404                 /* Read flash status register(s) */
 405                 retval = read_status_reg(bank, &status);
 406                 if (retval != ERROR_OK)
 407                         return retval;
 408
 409                 if ((status & ((SPIFLASH_BSY_BIT << 8) | SPIFLASH_BSY_BIT)) == 0)
 410                         return retval;
 411                 alive_sleep(25);
 412         } while (timeval_ms() < endtime);
 413
 414         LOG_ERROR("timeout");
 415         return ERROR_FLASH_OPERATION_FAILED;
 416 }
 417
 418 /* Send "write enable" command to SPI flash chip(s). */
 419 static int qspi_write_enable(struct flash_bank *bank)
 420 {
 421         struct target *target = bank->target;
 422         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
 423         uint32_t io_base = stmqspi_info->io_base;
 424         uint16_t status;
 425         int retval;
 426
 427         /* Abort any previous operation */
 428         retval = stmqspi_abort(bank);
 429         if (retval != ERROR_OK)
 430                 return retval;
 431
 432         /* Wait for busy to be cleared */
 433         retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
 434         if (retval != ERROR_OK)
 435                 goto err;
 436
 437         /* Send write enable command */
 438         if (IS_OCTOSPI) {
 439                 retval = octospi_cmd(bank, OCTOSPI_WRITE_MODE, OCTOSPI_CCR_WRITE_ENABLE,
 440                         SPIFLASH_WRITE_ENABLE);
 441                 if (OPI_MODE) {
 442                         /* Dummy address 0, only required for 8-line mode */
 443                         retval = target_write_u32(target, io_base + SPI_AR, 0);
 444                         if (retval != ERROR_OK)
 445                                 goto err;
 446                 }
 447         } else
 448                 retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_WRITE_ENABLE);
 449         if (retval != ERROR_OK)
 450                 goto err;
 451
 452
 453         /* Wait for transmit of command completed */
 454         poll_busy(bank, SPI_CMD_TIMEOUT);
 455         if (retval != ERROR_OK)
 456                 goto err;
 457
 458         /* Read flash status register */
 459         retval = read_status_reg(bank, &status);
 460         if (retval != ERROR_OK)
 461                 goto err;
 462
 463         /* Check write enabled for flash 1 */
 464         if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH)))
 465                 != BIT(SPI_FSEL_FLASH))
 466                 if ((status & (SPIFLASH_WE_BIT | SPIFLASH_BSY_BIT)) != SPIFLASH_WE_BIT) {
 467                         LOG_ERROR("Cannot write enable flash1. Status=0x%02x",
 468                                 status & 0xFFU);
 469                         return ERROR_FLASH_OPERATION_FAILED;
 470                 }
 471
 472         /* Check write enabled for flash 2 */
 473         status >>= 8;
 474         if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH))) != 0)
 475                 if ((status & (SPIFLASH_WE_BIT | SPIFLASH_BSY_BIT)) != SPIFLASH_WE_BIT) {
 476                         LOG_ERROR("Cannot write enable flash2. Status=0x%02x",
 477                                 status & 0xFFU);
 478                         return ERROR_FLASH_OPERATION_FAILED;
 479                 }
 480
 481 err:
 482         return retval;
 483 }
 484
 485 COMMAND_HANDLER(stmqspi_handle_mass_erase_command)
 486 {
 487         struct target *target = NULL;
 488         struct flash_bank *bank;
 489         struct stmqspi_flash_bank *stmqspi_info;
 490         struct duration bench;
 491         uint32_t io_base;
 492         uint16_t status;
 493         unsigned int sector;
 494         int retval;
 495
 496         LOG_DEBUG("%s", __func__);
 497
 498         if (CMD_ARGC != 1)
 499                 return ERROR_COMMAND_SYNTAX_ERROR;
 500
 501         retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 502         if (retval != ERROR_OK)
 503                 return retval;
 504
 505         stmqspi_info = bank->driver_priv;
 506         target = bank->target;
 507
 508         if (target->state != TARGET_HALTED) {
 509                 LOG_ERROR("Target not halted");
 510                 return ERROR_TARGET_NOT_HALTED;
 511         }
 512
 513         if (!(stmqspi_info->probed)) {
 514                 LOG_ERROR("Flash bank not probed");
 515                 return ERROR_FLASH_BANK_NOT_PROBED;
 516         }
 517
 518         if (stmqspi_info->dev.chip_erase_cmd == 0x00) {
 519                 LOG_ERROR("Mass erase not available for this device");
 520                 return ERROR_FLASH_OPER_UNSUPPORTED;
 521         }
 522
 523         for (sector = 0; sector < bank->num_sectors; sector++) {
 524                 if (bank->sectors[sector].is_protected) {
 525                         LOG_ERROR("Flash sector %u protected", sector);
 526                         return ERROR_FLASH_PROTECTED;
 527                 }
 528         }
 529
 530         io_base = stmqspi_info->io_base;
 531         duration_start(&bench);
 532
 533         retval = qspi_write_enable(bank);
 534         if (retval != ERROR_OK)
 535                 goto err;
 536
 537         /* Send Mass Erase command */
 538         if (IS_OCTOSPI)
 539                 retval = octospi_cmd(bank, OCTOSPI_WRITE_MODE, OCTOSPI_CCR_MASS_ERASE,
 540                         stmqspi_info->dev.chip_erase_cmd);
 541         else
 542                 retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_MASS_ERASE);
 543         if (retval != ERROR_OK)
 544                 goto err;
 545
 546         /* Wait for transmit of command completed */
 547         poll_busy(bank, SPI_CMD_TIMEOUT);
 548         if (retval != ERROR_OK)
 549                 goto err;
 550
 551         /* Read flash status register(s) */
 552         retval = read_status_reg(bank, &status);
 553         if (retval != ERROR_OK)
 554                 goto err;
 555
 556         /* Check for command in progress for flash 1 */
 557         if (((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH)))
 558                 != BIT(SPI_FSEL_FLASH)) && ((status & SPIFLASH_BSY_BIT) == 0) &&
 559                 ((status & SPIFLASH_WE_BIT) != 0)) {
 560                 LOG_ERROR("Mass erase command not accepted by flash1. Status=0x%02x",
 561                         status & 0xFFU);
 562                 retval = ERROR_FLASH_OPERATION_FAILED;
 563                 goto err;
 564         }
 565
 566         /* Check for command in progress for flash 2 */
 567         status >>= 8;
 568         if (((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH))) != 0) &&
 569                 ((status & SPIFLASH_BSY_BIT) == 0) &&
 570                 ((status & SPIFLASH_WE_BIT) != 0)) {
 571                 LOG_ERROR("Mass erase command not accepted by flash2. Status=0x%02x",
 572                         status & 0xFFU);
 573                 retval = ERROR_FLASH_OPERATION_FAILED;
 574                 goto err;
 575         }
 576
 577         /* Poll WIP for end of self timed Sector Erase cycle */
 578         retval = wait_till_ready(bank, SPI_MASS_ERASE_TIMEOUT);
 579
 580         duration_measure(&bench);
 581         if (retval == ERROR_OK)
 582                 command_print(CMD, "stmqspi mass erase completed in %fs (%0.3f KiB/s)",
 583                         duration_elapsed(&bench),
 584                         duration_kbps(&bench, bank->size));
 585         else
 586                 command_print(CMD, "stmqspi mass erase not completed even after %fs",
 587                         duration_elapsed(&bench));
 588
 589 err:
 590         /* Switch to memory mapped mode before return to prompt */
 591         set_mm_mode(bank);
 592
 593         return retval;
 594 }
 595
 596 static int log2u(uint32_t word)
 597 {
 598         int result;
 599
 600         for (result = 0; (unsigned int) result < sizeof(uint32_t) * CHAR_BIT; result++)
 601                 if (word == BIT(result))
 602                         return result;
 603
 604         return -1;
 605 }
 606
 607 COMMAND_HANDLER(stmqspi_handle_set)
 608 {
 609         struct flash_bank *bank = NULL;
 610         struct target *target = NULL;
 611         struct stmqspi_flash_bank *stmqspi_info = NULL;
 612         struct flash_sector *sectors = NULL;
 613         uint32_t io_base;
 614         unsigned int index = 0, dual, fsize;
 615         int retval;
 616
 617         LOG_DEBUG("%s", __func__);
 618
 619         dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
 620
 621         /* chip_erase_cmd, sectorsize and erase_cmd are optional */
 622         if ((CMD_ARGC < 7) || (CMD_ARGC > 10))
 623                 return ERROR_COMMAND_SYNTAX_ERROR;
 624
 625         retval = CALL_COMMAND_HANDLER(flash_command_get_bank, index++, &bank);
 626         if (retval != ERROR_OK)
 627                 return retval;
 628
 629         target = bank->target;
 630         stmqspi_info = bank->driver_priv;
 631
 632         /* invalidate all old info */
 633         if (stmqspi_info->probed)
 634                 free(bank->sectors);
 635         bank->size = 0;
 636         bank->num_sectors = 0;
 637         bank->sectors = NULL;
 638         stmqspi_info->sfdp_dummy1 = 0;
 639         stmqspi_info->sfdp_dummy2 = 0;
 640         stmqspi_info->probed = false;
 641         memset(&stmqspi_info->dev, 0, sizeof(stmqspi_info->dev));
 642         stmqspi_info->dev.name = "unknown";
 643
 644         strncpy(stmqspi_info->devname, CMD_ARGV[index++], sizeof(stmqspi_info->devname) - 1);
 645         stmqspi_info->devname[sizeof(stmqspi_info->devname) - 1] = '\0';
 646
 647         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[index++], stmqspi_info->dev.size_in_bytes);
 648         if (log2u(stmqspi_info->dev.size_in_bytes) < 8) {
 649                 command_print(CMD, "stmqspi: device size must be 2^n with n >= 8");
 650                 return ERROR_COMMAND_SYNTAX_ERROR;
 651         }
 652
 653         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[index++], stmqspi_info->dev.pagesize);
 654         if (stmqspi_info->dev.pagesize > stmqspi_info->dev.size_in_bytes ||
 655                 (log2u(stmqspi_info->dev.pagesize) < 0)) {
 656                 command_print(CMD, "stmqspi: page size must be 2^n and <= device size");
 657                 return ERROR_COMMAND_SYNTAX_ERROR;
 658         }
 659
 660         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.read_cmd);
 661         if ((stmqspi_info->dev.read_cmd != 0x03) &&
 662                 (stmqspi_info->dev.read_cmd != 0x13)) {
 663                 command_print(CMD, "stmqspi: only 0x03/0x13 READ cmd allowed");
 664                 return ERROR_COMMAND_SYNTAX_ERROR;
 665         }
 666
 667         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.qread_cmd);
 668         if ((stmqspi_info->dev.qread_cmd != 0x00) &&
 669                 (stmqspi_info->dev.qread_cmd != 0x0B) &&
 670                 (stmqspi_info->dev.qread_cmd != 0x0C) &&
 671                 (stmqspi_info->dev.qread_cmd != 0x3B) &&
 672                 (stmqspi_info->dev.qread_cmd != 0x3C) &&
 673                 (stmqspi_info->dev.qread_cmd != 0x6B) &&
 674                 (stmqspi_info->dev.qread_cmd != 0x6C) &&
 675                 (stmqspi_info->dev.qread_cmd != 0xBB) &&
 676                 (stmqspi_info->dev.qread_cmd != 0xBC) &&
 677                 (stmqspi_info->dev.qread_cmd != 0xEB) &&
 678                 (stmqspi_info->dev.qread_cmd != 0xEC) &&
 679                 (stmqspi_info->dev.qread_cmd != 0xEE)) {
 680                 command_print(CMD, "stmqspi: only 0x0B/0x0C/0x3B/0x3C/"
 681                         "0x6B/0x6C/0xBB/0xBC/0xEB/0xEC/0xEE QREAD allowed");
 682                 return ERROR_COMMAND_SYNTAX_ERROR;
 683         }
 684
 685         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.pprog_cmd);
 686         if ((stmqspi_info->dev.pprog_cmd != 0x02) &&
 687                 (stmqspi_info->dev.pprog_cmd != 0x12) &&
 688                 (stmqspi_info->dev.pprog_cmd != 0x32)) {
 689                 command_print(CMD, "stmqspi: only 0x02/0x12/0x32 PPRG cmd allowed");
 690                 return ERROR_COMMAND_SYNTAX_ERROR;
 691         }
 692
 693         if (index < CMD_ARGC)
 694                 COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.chip_erase_cmd);
 695         else
 696                 stmqspi_info->dev.chip_erase_cmd = 0x00;
 697
 698         if (index < CMD_ARGC) {
 699                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[index++], stmqspi_info->dev.sectorsize);
 700                 if ((stmqspi_info->dev.sectorsize > stmqspi_info->dev.size_in_bytes) ||
 701                         (stmqspi_info->dev.sectorsize < stmqspi_info->dev.pagesize) ||
 702                         (log2u(stmqspi_info->dev.sectorsize) < 0)) {
 703                         command_print(CMD, "stmqspi: sector size must be 2^n and <= device size");
 704                         return ERROR_COMMAND_SYNTAX_ERROR;
 705                 }
 706
 707                 if (index < CMD_ARGC)
 708                         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.erase_cmd);
 709                 else
 710                         return ERROR_COMMAND_SYNTAX_ERROR;
 711         } else {
 712                 /* no sector size / sector erase cmd given, treat whole bank as a single sector */
 713                 stmqspi_info->dev.erase_cmd = 0x00;
 714                 stmqspi_info->dev.sectorsize = stmqspi_info->dev.size_in_bytes;
 715         }
 716
 717         /* set correct size value */
 718         bank->size = stmqspi_info->dev.size_in_bytes << dual;
 719
 720         io_base = stmqspi_info->io_base;
 721
 722         uint32_t dcr;
 723         retval = target_read_u32(target, io_base + SPI_DCR, &dcr);
 724
 725         if (retval != ERROR_OK)
 726                 return retval;
 727
 728         fsize = (dcr >> SPI_FSIZE_POS) & (BIT(SPI_FSIZE_LEN) - 1);
 729
 730         LOG_DEBUG("FSIZE = 0x%04x", fsize);
 731         if (bank->size == BIT(fsize + 1))
 732                 LOG_DEBUG("FSIZE in DCR(1) matches actual capacity. Beware of silicon bug in H7, L4+, MP1.");
 733         else if (bank->size == BIT(fsize + 0))
 734                 LOG_DEBUG("FSIZE in DCR(1) is off by one regarding actual capacity. Fix for silicon bug?");
 735         else
 736                 LOG_ERROR("FSIZE in DCR(1) doesn't match actual capacity.");
 737
 738         /* create and fill sectors array */
 739         bank->num_sectors =
 740                 stmqspi_info->dev.size_in_bytes / stmqspi_info->dev.sectorsize;
 741         sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
 742         if (!sectors) {
 743                 LOG_ERROR("not enough memory");
 744                 return ERROR_FAIL;
 745         }
 746
 747         for (unsigned int sector = 0; sector < bank->num_sectors; sector++) {
 748                 sectors[sector].offset = sector * (stmqspi_info->dev.sectorsize << dual);
 749                 sectors[sector].size = (stmqspi_info->dev.sectorsize << dual);
 750                 sectors[sector].is_erased = -1;
 751                 sectors[sector].is_protected = 0;
 752         }
 753
 754         bank->sectors = sectors;
 755         stmqspi_info->dev.name = stmqspi_info->devname;
 756         if (stmqspi_info->dev.size_in_bytes / 4096)
 757                 LOG_INFO("flash \'%s\' id = unknown\nchip size = %" PRIu32 " KiB,"
 758                         " bank size = %" PRIu32 " KiB", stmqspi_info->dev.name,
 759                         stmqspi_info->dev.size_in_bytes / 1024,
 760                         (stmqspi_info->dev.size_in_bytes / 1024) << dual);
 761         else
 762                 LOG_INFO("flash \'%s\' id = unknown\nchip size = %" PRIu32 " B,"
 763                         " bank size = %" PRIu32 " B", stmqspi_info->dev.name,
 764                         stmqspi_info->dev.size_in_bytes,
 765                         stmqspi_info->dev.size_in_bytes << dual);
 766
 767         stmqspi_info->probed = true;
 768
 769         return ERROR_OK;
 770 }
 771
 772 COMMAND_HANDLER(stmqspi_handle_cmd)
 773 {
 774         struct target *target = NULL;
 775         struct flash_bank *bank;
 776         struct stmqspi_flash_bank *stmqspi_info = NULL;
 777         uint32_t io_base, addr;
 778         uint8_t num_write, num_read, cmd_byte, data;
 779         unsigned int count;
 780         const int max = 21;
 781         char temp[4], output[(2 + max + 256) * 3 + 8];
 782         int retval;
 783
 784         LOG_DEBUG("%s", __func__);
 785
 786         if (CMD_ARGC < 3)
 787                 return ERROR_COMMAND_SYNTAX_ERROR;
 788
 789         num_write = CMD_ARGC - 2;
 790         if (num_write > max) {
 791                 LOG_ERROR("at most %d bytes may be sent", max);
 792                 return ERROR_COMMAND_SYNTAX_ERROR;
 793         }
 794
 795         retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 796         if (retval != ERROR_OK)
 797                 return retval;
 798
 799         target = bank->target;
 800         stmqspi_info = bank->driver_priv;
 801         io_base = stmqspi_info->io_base;
 802
 803         if (target->state != TARGET_HALTED) {
 804                 LOG_ERROR("Target not halted");
 805                 return ERROR_TARGET_NOT_HALTED;
 806         }
 807
 808         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[1], num_read);
 809         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[2], cmd_byte);
 810
 811         if (num_read == 0) {
 812                 /* nothing to read, then one command byte and for dual flash
 813                  * an *even* number of data bytes to follow */
 814                 if (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) {
 815                         if ((num_write & 1) == 0) {
 816                                 LOG_ERROR("number of data bytes to write must be even in dual mode");
 817                                 return ERROR_COMMAND_SYNTAX_ERROR;
 818                         }
 819                 }
 820         } else {
 821                 /* read mode, one command byte and up to four following address bytes */
 822                 if (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) {
 823                         if ((num_read & 1) != 0) {
 824                                 LOG_ERROR("number of bytes to read must be even in dual mode");
 825                                 return ERROR_COMMAND_SYNTAX_ERROR;
 826                         }
 827                 }
 828                 if ((num_write < 1) || (num_write > 5)) {
 829                         LOG_ERROR("one cmd and up to four addr bytes must be send when reading");
 830                         return ERROR_COMMAND_SYNTAX_ERROR;
 831                 }
 832         }
 833
 834         /* Abort any previous operation */
 835         retval = stmqspi_abort(bank);
 836         if (retval != ERROR_OK)
 837                 return retval;
 838
 839         /* Wait for busy to be cleared */
 840         retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
 841         if (retval != ERROR_OK)
 842                 return retval;
 843
 844         /* send command byte */
 845         snprintf(output, sizeof(output), "spi: %02x ", cmd_byte);
 846         if (num_read == 0) {
 847                 /* write, send cmd byte */
 848                 retval = target_write_u32(target, io_base + SPI_DLR, ((uint32_t)num_write) - 2);
 849                 if (retval != ERROR_OK)
 850                         goto err;
 851
 852                 if (IS_OCTOSPI)
 853                         retval = octospi_cmd(bank, OCTOSPI_WRITE_MODE,
 854                                 (OCTOSPI_MODE_CCR & OCTOSPI_NO_ALTB & OCTOSPI_NO_ADDR &
 855                                 ((num_write == 1) ? OCTOSPI_NO_DATA : ~0U)), cmd_byte);
 856                 else
 857                         retval = target_write_u32(target, io_base + QSPI_CCR,
 858                                 (QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB & QSPI_NO_ADDR &
 859                                 ((num_write == 1) ? QSPI_NO_DATA : ~0U)) |
 860                                 (QSPI_WRITE_MODE | cmd_byte));
 861                 if (retval != ERROR_OK)
 862                         goto err;
 863
 864                 /* send additional data bytes */
 865                 for (count = 3; count < CMD_ARGC; count++) {
 866                         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[count], data);
 867                         snprintf(temp, sizeof(temp), "%02" PRIx8 " ", data);
 868                         retval = target_write_u8(target, io_base + SPI_DR, data);
 869                         if (retval != ERROR_OK)
 870                                 goto err;
 871                         strncat(output, temp, sizeof(output) - strlen(output) - 1);
 872                 }
 873                 strncat(output, "-> ", sizeof(output) - strlen(output) - 1);
 874         } else {
 875                 /* read, pack additional bytes into address */
 876                 addr = 0;
 877                 for (count = 3; count < CMD_ARGC; count++) {
 878                         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[count], data);
 879                         snprintf(temp, sizeof(temp), "%02" PRIx8 " ", data);
 880                         addr = (addr << 8) | data;
 881                         strncat(output, temp, sizeof(output) - strlen(output) - 1);
 882                 }
 883                 strncat(output, "-> ", sizeof(output) - strlen(output) - 1);
 884
 885                 /* send cmd byte, if ADMODE indicates no address, this already triggers command */
 886                 retval = target_write_u32(target, io_base + SPI_DLR, ((uint32_t)num_read) - 1);
 887                 if (retval != ERROR_OK)
 888                         goto err;
 889                 if (IS_OCTOSPI)
 890                         retval = octospi_cmd(bank, OCTOSPI_READ_MODE,
 891                                 (OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & OCTOSPI_NO_ALTB & ~OCTOSPI_ADDR4 &
 892                                         ((num_write == 1) ? OCTOSPI_NO_ADDR : ~0U)) |
 893                                 (((num_write - 2) & 0x3U) << SPI_ADSIZE_POS), cmd_byte);
 894                 else
 895                         retval = target_write_u32(target, io_base + QSPI_CCR,
 896                                 (QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB & ~QSPI_ADDR4 &
 897                                         ((num_write == 1) ? QSPI_NO_ADDR : ~0U)) |
 898                                 ((QSPI_READ_MODE | (((num_write - 2) & 0x3U) << SPI_ADSIZE_POS) | cmd_byte)));
 899                 if (retval != ERROR_OK)
 900                         goto err;
 901
 902                 if (num_write > 1) {
 903                         /* if ADMODE indicates address required, only the write to AR triggers command */
 904                         retval = target_write_u32(target, io_base + SPI_AR, addr);
 905                         if (retval != ERROR_OK)
 906                                 goto err;
 907                 }
 908
 909                 /* read response bytes */
 910                 for ( ; num_read > 0; num_read--) {
 911                         retval = target_read_u8(target, io_base + SPI_DR, &data);
 912                         if (retval != ERROR_OK)
 913                                 goto err;
 914                         snprintf(temp, sizeof(temp), "%02" PRIx8 " ", data);
 915                         strncat(output, temp, sizeof(output) - strlen(output) - 1);
 916                 }
 917         }
 918         command_print(CMD, "%s", output);
 919
 920 err:
 921         /* Switch to memory mapped mode before return to prompt */
 922         set_mm_mode(bank);
 923
 924         return retval;
 925 }
 926
 927 static int qspi_erase_sector(struct flash_bank *bank, unsigned int sector)
 928 {
 929         struct target *target = bank->target;
 930         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
 931         uint32_t io_base = stmqspi_info->io_base;
 932         uint16_t status;
 933         int retval;
 934
 935         retval = qspi_write_enable(bank);
 936         if (retval != ERROR_OK)
 937                 goto err;
 938
 939         /* Send Sector Erase command */
 940         if (IS_OCTOSPI)
 941                 retval = octospi_cmd(bank, OCTOSPI_WRITE_MODE, OCTOSPI_CCR_SECTOR_ERASE,
 942                         stmqspi_info->dev.erase_cmd);
 943         else
 944                 retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_SECTOR_ERASE);
 945         if (retval != ERROR_OK)
 946                 goto err;
 947
 948         /* Address is sector offset, this write initiates command transmission */
 949         retval = target_write_u32(target, io_base + SPI_AR, bank->sectors[sector].offset);
 950         if (retval != ERROR_OK)
 951                 goto err;
 952
 953         /* Wait for transmit of command completed */
 954         poll_busy(bank, SPI_CMD_TIMEOUT);
 955         if (retval != ERROR_OK)
 956                 goto err;
 957
 958         /* Read flash status register(s) */
 959         retval = read_status_reg(bank, &status);
 960         if (retval != ERROR_OK)
 961                 goto err;
 962
 963         LOG_DEBUG("erase status regs: 0x%04" PRIx16, status);
 964
 965         /* Check for command in progress for flash 1 */
 966         /* If BSY and WE are already cleared the erase did probably complete already */
 967         if (((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH)))
 968                 != BIT(SPI_FSEL_FLASH)) && ((status & SPIFLASH_BSY_BIT) == 0) &&
 969                 ((status & SPIFLASH_WE_BIT) != 0)) {
 970                 LOG_ERROR("Sector erase command not accepted by flash1. Status=0x%02x",
 971                         status & 0xFFU);
 972                 retval = ERROR_FLASH_OPERATION_FAILED;
 973                 goto err;
 974         }
 975
 976         /* Check for command in progress for flash 2 */
 977         /* If BSY and WE are already cleared the erase did probably complete already */
 978         status >>= 8;
 979         if (((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH))) != 0) &&
 980                 ((status & SPIFLASH_BSY_BIT) == 0) &&
 981                 ((status & SPIFLASH_WE_BIT) != 0)) {
 982                 LOG_ERROR("Sector erase command not accepted by flash2. Status=0x%02x",
 983                         status & 0xFFU);
 984                 retval = ERROR_FLASH_OPERATION_FAILED;
 985                 goto err;
 986         }
 987
 988         /* Erase takes a long time, so some sort of progress message is a good idea */
 989         LOG_DEBUG("erasing sector %4u", sector);
 990
 991         /* Poll WIP for end of self timed Sector Erase cycle */
 992         retval = wait_till_ready(bank, SPI_MAX_TIMEOUT);
 993
 994 err:
 995         return retval;
 996 }
 997
 998 static int stmqspi_erase(struct flash_bank *bank, unsigned int first, unsigned int last)
 999 {
1000         struct target *target = bank->target;
1001         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
1002         unsigned int sector;
1003         int retval = ERROR_OK;
1004
1005         LOG_DEBUG("%s: from sector %u to sector %u", __func__, first, last);
1006
1007         if (target->state != TARGET_HALTED) {
1008                 LOG_ERROR("Target not halted");
1009                 return ERROR_TARGET_NOT_HALTED;
1010         }
1011
1012         if (!(stmqspi_info->probed)) {
1013                 LOG_ERROR("Flash bank not probed");
1014                 return ERROR_FLASH_BANK_NOT_PROBED;
1015         }
1016
1017         if (stmqspi_info->dev.erase_cmd == 0x00) {
1018                 LOG_ERROR("Sector erase not available for this device");
1019                 return ERROR_FLASH_OPER_UNSUPPORTED;
1020         }
1021
1022         if ((last < first) || (last >= bank->num_sectors)) {
1023                 LOG_ERROR("Flash sector invalid");
1024                 return ERROR_FLASH_SECTOR_INVALID;
1025         }
1026
1027         for (sector = first; sector <= last; sector++) {
1028                 if (bank->sectors[sector].is_protected) {
1029                         LOG_ERROR("Flash sector %u protected", sector);
1030                         return ERROR_FLASH_PROTECTED;
1031                 }
1032         }
1033
1034         for (sector = first; sector <= last; sector++) {
1035                 retval = qspi_erase_sector(bank, sector);
1036                 if (retval != ERROR_OK)
1037                         break;
1038                 alive_sleep(10);
1039                 keep_alive();
1040         }
1041
1042         if (retval != ERROR_OK)
1043                 LOG_ERROR("Flash sector_erase failed on sector %u", sector);
1044
1045         /* Switch to memory mapped mode before return to prompt */
1046         set_mm_mode(bank);
1047
1048         return retval;
1049 }
1050
1051 static int stmqspi_protect(struct flash_bank *bank, int set,
1052         unsigned int first, unsigned int last)
1053 {
1054         unsigned int sector;
1055
1056         for (sector = first; sector <= last; sector++)
1057                 bank->sectors[sector].is_protected = set;
1058
1059         if (set)
1060                 LOG_WARNING("setting soft protection only, not related to flash's hardware write protection");
1061
1062         return ERROR_OK;
1063 }
1064
1065 /* Check whether flash is blank */
1066 static int stmqspi_blank_check(struct flash_bank *bank)
1067 {
1068         struct target *target = bank->target;
1069         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
1070         struct duration bench;
1071         struct reg_param reg_params[2];
1072         struct armv7m_algorithm armv7m_info;
1073         struct working_area *algorithm;
1074         const uint8_t *code;
1075         struct sector_info erase_check_info;
1076         uint32_t codesize, maxsize, result, exit_point;
1077         unsigned int count, index, num_sectors, sector;
1078         int retval;
1079         const uint32_t erased = 0x00FF;
1080
1081         if (target->state != TARGET_HALTED) {
1082                 LOG_ERROR("Target not halted");
1083                 return ERROR_TARGET_NOT_HALTED;
1084         }
1085
1086         if (!(stmqspi_info->probed)) {
1087                 LOG_ERROR("Flash bank not probed");
1088                 return ERROR_FLASH_BANK_NOT_PROBED;
1089         }
1090
1091         /* Abort any previous operation */
1092         retval = stmqspi_abort(bank);
1093         if (retval != ERROR_OK)
1094                 return retval;
1095
1096         /* Wait for busy to be cleared */
1097         retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
1098         if (retval != ERROR_OK)
1099                 return retval;
1100
1101         /* see contrib/loaders/flash/stmqspi/stmqspi_erase_check.S for src */
1102         static const uint8_t stmqspi_erase_check_code[] = {
1103                 #include "../../../contrib/loaders/flash/stmqspi/stmqspi_erase_check.inc"
1104         };
1105
1106         /* see contrib/loaders/flash/stmqspi/stmoctospi_erase_check.S for src */
1107         static const uint8_t stmoctospi_erase_check_code[] = {
1108                 #include "../../../contrib/loaders/flash/stmqspi/stmoctospi_erase_check.inc"
1109         };
1110
1111         if (IS_OCTOSPI) {
1112                 code = stmoctospi_erase_check_code;
1113                 codesize = sizeof(stmoctospi_erase_check_code);
1114         } else {
1115                 code = stmqspi_erase_check_code;
1116                 codesize = sizeof(stmqspi_erase_check_code);
1117         }
1118
1119         /* This will overlay the last 4 words of stmqspi/stmoctospi_erase_check_code in target */
1120         /* for read use the saved settings (memory mapped mode) but indirect read mode */
1121         uint32_t ccr_buffer[][4] = {
1122                 /* cr  (not used for QSPI)                      *
1123                  * ccr (for both QSPI and OCTOSPI)      *
1124                  * tcr (not used for QSPI)                      *
1125                  * ir  (not used for QSPI)                      */
1126                 {
1127                         h_to_le_32(OCTOSPI_MODE | OCTOSPI_READ_MODE),
1128                         h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_READ : QSPI_CCR_READ),
1129                         h_to_le_32(stmqspi_info->saved_tcr),
1130                         h_to_le_32(stmqspi_info->saved_ir),
1131                 },
1132         };
1133
1134         maxsize = target_get_working_area_avail(target);
1135         if (maxsize < codesize + sizeof(erase_check_info)) {
1136                 LOG_ERROR("Not enough working area, can't do QSPI blank check");
1137                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1138         }
1139
1140         num_sectors = (maxsize - codesize) / sizeof(erase_check_info);
1141         num_sectors = (bank->num_sectors < num_sectors) ? bank->num_sectors : num_sectors;
1142
1143         if (target_alloc_working_area_try(target,
1144                         codesize + num_sectors * sizeof(erase_check_info), &algorithm) != ERROR_OK) {
1145                 LOG_ERROR("allocating working area failed");
1146                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1147         };
1148
1149         /* prepare blank check code, excluding ccr_buffer */
1150         retval = target_write_buffer(target, algorithm->address,
1151                 codesize - sizeof(ccr_buffer), code);
1152         if (retval != ERROR_OK)
1153                 goto err;
1154
1155         /* prepare QSPI/OCTOSPI_CCR register values */
1156         retval = target_write_buffer(target, algorithm->address
1157                 + codesize - sizeof(ccr_buffer),
1158                 sizeof(ccr_buffer), (uint8_t *)ccr_buffer);
1159         if (retval != ERROR_OK)
1160                 goto err;
1161
1162         duration_start(&bench);
1163
1164         /* after breakpoint instruction (halfword), one nop (halfword) and
1165          * port_buffer till end of code */
1166         exit_point = algorithm->address + codesize - sizeof(uint32_t) - sizeof(ccr_buffer);
1167
1168         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);    /* sector count */
1169         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);    /* QSPI/OCTOSPI io_base */
1170
1171         sector = 0;
1172         while (sector < bank->num_sectors) {
1173                 /* at most num_sectors sectors to handle in one run */
1174                 count = bank->num_sectors - sector;
1175                 if (count > num_sectors)
1176                         count = num_sectors;
1177
1178                 for (index = 0; index < count; index++) {
1179                         erase_check_info.offset = h_to_le_32(bank->sectors[sector + index].offset);
1180                         erase_check_info.size = h_to_le_32(bank->sectors[sector + index].size);
1181                         erase_check_info.result = h_to_le_32(erased);
1182
1183                         retval = target_write_buffer(target, algorithm->address
1184                                 + codesize + index * sizeof(erase_check_info),
1185                                         sizeof(erase_check_info), (uint8_t *)&erase_check_info);
1186                         if (retval != ERROR_OK)
1187                                 goto err;
1188                 }
1189
1190                 buf_set_u32(reg_params[0].value, 0, 32, count);
1191                 buf_set_u32(reg_params[1].value, 0, 32, stmqspi_info->io_base);
1192
1193                 armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
1194                 armv7m_info.core_mode = ARM_MODE_THREAD;
1195
1196                 LOG_DEBUG("checking sectors %u to %u", sector, sector + count - 1);
1197                 /* check a block of sectors */
1198                 retval = target_run_algorithm(target,
1199                         0, NULL,
1200                         ARRAY_SIZE(reg_params), reg_params,
1201                         algorithm->address, exit_point,
1202                         count * ((bank->sectors[sector].size >> 6) + 1) + 1000,
1203                         &armv7m_info);
1204                 if (retval != ERROR_OK)
1205                         break;
1206
1207                 for (index = 0; index < count; index++) {
1208                         retval = target_read_buffer(target, algorithm->address
1209                                 + codesize + index * sizeof(erase_check_info),
1210                                         sizeof(erase_check_info), (uint8_t *)&erase_check_info);
1211                         if (retval != ERROR_OK)
1212                                 goto err;
1213
1214                         if ((erase_check_info.offset != h_to_le_32(bank->sectors[sector + index].offset)) ||
1215                                 (erase_check_info.size != 0)) {
1216                                 LOG_ERROR("corrupted blank check info");
1217                                 goto err;
1218                         }
1219
1220                         /* we need le_32_to_h, but that's the same as h_to_le_32 */
1221                         result = h_to_le_32(erase_check_info.result);
1222                         bank->sectors[sector + index].is_erased = ((result & 0xFF) == 0xFF);
1223                         LOG_DEBUG("Flash sector %u checked: 0x%04x", sector + index, result & 0xFFFFU);
1224                 }
1225                 keep_alive();
1226                 sector += count;
1227         }
1228
1229         destroy_reg_param(&reg_params[0]);
1230         destroy_reg_param(&reg_params[1]);
1231
1232         duration_measure(&bench);
1233         LOG_INFO("stmqspi blank checked in %fs (%0.3f KiB/s)", duration_elapsed(&bench),
1234                 duration_kbps(&bench, bank->size));
1235
1236 err:
1237         target_free_working_area(target, algorithm);
1238
1239         /* Switch to memory mapped mode before return to prompt */
1240         set_mm_mode(bank);
1241
1242         return retval;
1243 }
1244
1245 /* Verify checksum */
1246 static int qspi_verify(struct flash_bank *bank, uint8_t *buffer,
1247         uint32_t offset, uint32_t count)
1248 {
1249         struct target *target = bank->target;
1250         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
1251         struct reg_param reg_params[4];
1252         struct armv7m_algorithm armv7m_info;
1253         struct working_area *algorithm;
1254         const uint8_t *code;
1255         uint32_t pagesize, codesize, crc32, result, exit_point;
1256         int retval;
1257
1258         /* see contrib/loaders/flash/stmqspi/stmqspi_crc32.S for src */
1259         static const uint8_t stmqspi_crc32_code[] = {
1260                 #include "../../../contrib/loaders/flash/stmqspi/stmqspi_crc32.inc"
1261         };
1262
1263         /* see contrib/loaders/flash/stmqspi/stmoctospi_crc32.S for src */
1264         static const uint8_t stmoctospi_crc32_code[] = {
1265                 #include "../../../contrib/loaders/flash/stmqspi/stmoctospi_crc32.inc"
1266         };
1267
1268         if (IS_OCTOSPI) {
1269                 code = stmoctospi_crc32_code;
1270                 codesize = sizeof(stmoctospi_crc32_code);
1271         } else {
1272                 code = stmqspi_crc32_code;
1273                 codesize = sizeof(stmqspi_crc32_code);
1274         }
1275
1276         /* block size doesn't matter that much here */
1277         pagesize = stmqspi_info->dev.sectorsize;
1278         if (pagesize == 0)
1279                 pagesize = stmqspi_info->dev.pagesize;
1280         if (pagesize == 0)
1281                 pagesize = SPIFLASH_DEF_PAGESIZE;
1282
1283         /* adjust size according to dual flash mode */
1284         pagesize = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? pagesize << 1 : pagesize;
1285
1286         /* This will overlay the last 4 words of stmqspi/stmoctospi_crc32_code in target */
1287         /* for read use the saved settings (memory mapped mode) but indirect read mode */
1288         uint32_t ccr_buffer[][4] = {
1289                 /* cr  (not used for QSPI)                      *
1290                  * ccr (for both QSPI and OCTOSPI)      *
1291                  * tcr (not used for QSPI)                      *
1292                  * ir  (not used for QSPI)                      */
1293                 {
1294                         h_to_le_32(OCTOSPI_MODE | OCTOSPI_READ_MODE),
1295                         h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_READ : QSPI_CCR_READ),
1296                         h_to_le_32(stmqspi_info->saved_tcr),
1297                         h_to_le_32(stmqspi_info->saved_ir),
1298                 },
1299         };
1300
1301         if (target_alloc_working_area_try(target, codesize, &algorithm) != ERROR_OK) {
1302                 LOG_ERROR("Not enough working area, can't do QSPI verify");
1303                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1304         };
1305
1306         /* prepare verify code, excluding ccr_buffer */
1307         retval = target_write_buffer(target, algorithm->address,
1308                 codesize - sizeof(ccr_buffer), code);
1309         if (retval != ERROR_OK)
1310                 goto err;
1311
1312         /* prepare QSPI/OCTOSPI_CCR register values */
1313         retval = target_write_buffer(target, algorithm->address
1314                 + codesize - sizeof(ccr_buffer),
1315                 sizeof(ccr_buffer), (uint8_t *)ccr_buffer);
1316         if (retval != ERROR_OK)
1317                 goto err;
1318
1319         /* after breakpoint instruction (halfword), one nop (halfword) and
1320          * port_buffer till end of code */
1321         exit_point = algorithm->address + codesize - sizeof(uint32_t) - sizeof(ccr_buffer);
1322
1323         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT); /* count (in), crc32 (out) */
1324         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);    /* pagesize */
1325         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);    /* offset into flash address */
1326         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);    /* QSPI/OCTOSPI io_base */
1327
1328         buf_set_u32(reg_params[0].value, 0, 32, count);
1329         buf_set_u32(reg_params[1].value, 0, 32, pagesize);
1330         buf_set_u32(reg_params[2].value, 0, 32, offset);
1331         buf_set_u32(reg_params[3].value, 0, 32, stmqspi_info->io_base);
1332
1333
1334         armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
1335         armv7m_info.core_mode = ARM_MODE_THREAD;
1336
1337         retval = target_run_algorithm(target,
1338                 0, NULL,
1339                 ARRAY_SIZE(reg_params), reg_params,
1340                 algorithm->address, exit_point,
1341                 (count >> 5) + 1000,
1342                 &armv7m_info);
1343         keep_alive();
1344
1345         image_calculate_checksum(buffer, count, &crc32);
1346
1347         if (retval == ERROR_OK) {
1348                 result = buf_get_u32(reg_params[0].value, 0, 32);
1349                 LOG_DEBUG("addr " TARGET_ADDR_FMT ", len 0x%08" PRIx32 ", crc 0x%08" PRIx32 " 0x%08" PRIx32,
1350                         offset + bank->base, count, ~crc32, result);
1351                 if (~crc32 != result)
1352                         retval = ERROR_FAIL;
1353         }
1354
1355         destroy_reg_param(&reg_params[0]);
1356         destroy_reg_param(&reg_params[1]);
1357         destroy_reg_param(&reg_params[2]);
1358         destroy_reg_param(&reg_params[3]);
1359
1360 err:
1361         target_free_working_area(target, algorithm);
1362
1363         /* Switch to memory mapped mode before return to prompt */
1364         set_mm_mode(bank);
1365
1366         return retval;
1367 }
1368
1369 static int qspi_read_write_block(struct flash_bank *bank, uint8_t *buffer,
1370         uint32_t offset, uint32_t count, bool write)
1371 {
1372         struct target *target = bank->target;
1373         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
1374         uint32_t io_base = stmqspi_info->io_base;
1375         struct reg_param reg_params[6];
1376         struct armv7m_algorithm armv7m_info;
1377         struct working_area *algorithm;
1378         uint32_t pagesize, fifo_start, fifosize, remaining;
1379         uint32_t maxsize, codesize, exit_point;
1380         const uint8_t *code = NULL;
1381         unsigned int dual;
1382         int retval;
1383
1384         LOG_DEBUG("%s: offset=0x%08" PRIx32 " len=0x%08" PRIx32,
1385                 __func__, offset, count);
1386
1387         dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
1388
1389         /* see contrib/loaders/flash/stmqspi/stmqspi_read.S for src */
1390         static const uint8_t stmqspi_read_code[] = {
1391 #include "../../../contrib/loaders/flash/stmqspi/stmqspi_read.inc"
1392         };
1393
1394         /* see contrib/loaders/flash/stmqspi/stmoctospi_read.S for src */
1395         static const uint8_t stmoctospi_read_code[] = {
1396 #include "../../../contrib/loaders/flash/stmqspi/stmoctospi_read.inc"
1397         };
1398
1399         /* see contrib/loaders/flash/stmqspi/stmqspi_write.S for src */
1400         static const uint8_t stmqspi_write_code[] = {
1401 #include "../../../contrib/loaders/flash/stmqspi/stmqspi_write.inc"
1402         };
1403
1404         /* see contrib/loaders/flash/stmqspi/stmoctospi_write.S for src */
1405         static const uint8_t stmoctospi_write_code[] = {
1406 #include "../../../contrib/loaders/flash/stmqspi/stmoctospi_write.inc"
1407         };
1408
1409         /* This will overlay the last 12 words of stmqspi/stmoctospi_read/write_code in target */
1410         /* for read use the saved settings (memory mapped mode) but indirect read mode */
1411         uint32_t ccr_buffer[][4] = {
1412                 /* cr  (not used for QSPI)                      *
1413                  * ccr (for both QSPI and OCTOSPI)      *
1414                  * tcr (not used for QSPI)                      *
1415                  * ir  (not used for QSPI)                      */
1416                 {
1417                         h_to_le_32(OCTOSPI_MODE | OCTOSPI_READ_MODE),
1418                         h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_READ_STATUS : QSPI_CCR_READ_STATUS),
1419                         h_to_le_32((stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK) |
1420                                                 (OPI_MODE ? (OPI_DUMMY << OCTOSPI_DCYC_POS) : 0)),
1421                         h_to_le_32(OPI_CMD(SPIFLASH_READ_STATUS)),
1422                 },
1423                 {
1424                         h_to_le_32(OCTOSPI_MODE | OCTOSPI_WRITE_MODE),
1425                         h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_WRITE_ENABLE : QSPI_CCR_WRITE_ENABLE),
1426                         h_to_le_32(stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK),
1427                         h_to_le_32(OPI_CMD(SPIFLASH_WRITE_ENABLE)),
1428                 },
1429                 {
1430                         h_to_le_32(OCTOSPI_MODE | (write ? OCTOSPI_WRITE_MODE : OCTOSPI_READ_MODE)),
1431                         h_to_le_32(write ? (IS_OCTOSPI ? OCTOSPI_CCR_PAGE_PROG : QSPI_CCR_PAGE_PROG) :
1432                                 (IS_OCTOSPI ? OCTOSPI_CCR_READ : QSPI_CCR_READ)),
1433                         h_to_le_32(write ? (stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK) :
1434                                 stmqspi_info->saved_tcr),
1435                         h_to_le_32(write ? OPI_CMD(stmqspi_info->dev.pprog_cmd) : stmqspi_info->saved_ir),
1436                 },
1437         };
1438
1439         /* force reasonable defaults */
1440         fifosize = stmqspi_info->dev.sectorsize ?
1441                 stmqspi_info->dev.sectorsize : stmqspi_info->dev.size_in_bytes;
1442
1443         if (write) {
1444                 if (IS_OCTOSPI) {
1445                         code = stmoctospi_write_code;
1446                         codesize = sizeof(stmoctospi_write_code);
1447                 } else {
1448                         code = stmqspi_write_code;
1449                         codesize = sizeof(stmqspi_write_code);
1450                 }
1451         } else {
1452                 if (IS_OCTOSPI) {
1453                         code = stmoctospi_read_code;
1454                         codesize = sizeof(stmoctospi_read_code);
1455                 } else {
1456                         code = stmqspi_read_code;
1457                         codesize = sizeof(stmqspi_read_code);
1458                 }
1459         }
1460
1461         /* for write, pagesize must be taken into account */
1462         /* for read, the page size doesn't matter that much */
1463         pagesize = stmqspi_info->dev.pagesize;
1464         if (pagesize == 0)
1465                 pagesize = (fifosize <= SPIFLASH_DEF_PAGESIZE) ?
1466                         fifosize : SPIFLASH_DEF_PAGESIZE;
1467
1468         /* adjust sizes according to dual flash mode */
1469         pagesize <<= dual;
1470         fifosize <<= dual;
1471
1472         /* memory buffer, we assume sectorsize to be a power of 2 times pagesize */
1473         maxsize = target_get_working_area_avail(target);
1474         if (maxsize < codesize + 2 * sizeof(uint32_t) + pagesize) {
1475                 LOG_ERROR("not enough working area, can't do QSPI page reads/writes");
1476                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1477         }
1478
1479         /* fifo size at most sector size, and multiple of page size */
1480         maxsize -= (codesize + 2 * sizeof(uint32_t));
1481         fifosize = ((maxsize < fifosize) ? maxsize : fifosize) & ~(pagesize - 1);
1482
1483         if (target_alloc_working_area_try(target,
1484                 codesize + 2 * sizeof(uint32_t) + fifosize, &algorithm) != ERROR_OK) {
1485                 LOG_ERROR("allocating working area failed");
1486                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1487         };
1488
1489         /* prepare flash write code, excluding ccr_buffer */
1490         retval = target_write_buffer(target, algorithm->address,
1491                 codesize - sizeof(ccr_buffer), code);
1492         if (retval != ERROR_OK)
1493                 goto err;
1494
1495         /* prepare QSPI/OCTOSPI_CCR register values */
1496         retval = target_write_buffer(target, algorithm->address
1497                 + codesize - sizeof(ccr_buffer),
1498                 sizeof(ccr_buffer), (uint8_t *)ccr_buffer);
1499         if (retval != ERROR_OK)
1500                 goto err;
1501
1502         /* target buffer starts right after flash_write_code, i.e.
1503          * wp and rp are implicitly included in buffer!!! */
1504         fifo_start = algorithm->address + codesize + 2 * sizeof(uint32_t);
1505
1506         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT); /* count (in), status (out) */
1507         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);    /* pagesize */
1508         init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT); /* offset into flash address */
1509         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);    /* QSPI/OCTOSPI io_base */
1510         init_reg_param(&reg_params[4], "r8", 32, PARAM_OUT);    /* fifo start */
1511         init_reg_param(&reg_params[5], "r9", 32, PARAM_OUT);    /* fifo end + 1 */
1512
1513         buf_set_u32(reg_params[0].value, 0, 32, count);
1514         buf_set_u32(reg_params[1].value, 0, 32, pagesize);
1515         buf_set_u32(reg_params[2].value, 0, 32, offset);
1516         buf_set_u32(reg_params[3].value, 0, 32, io_base);
1517         buf_set_u32(reg_params[4].value, 0, 32, fifo_start);
1518         buf_set_u32(reg_params[5].value, 0, 32, fifo_start + fifosize);
1519
1520         armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
1521         armv7m_info.core_mode = ARM_MODE_THREAD;
1522
1523         /* after breakpoint instruction (halfword), one nop (halfword) and
1524          * ccr_buffer follow till end of code */
1525         exit_point = algorithm->address + codesize
1526                 - (sizeof(ccr_buffer) + sizeof(uint32_t));
1527
1528         if (write) {
1529                 retval = target_run_flash_async_algorithm(target, buffer, count, 1,
1530                                 0, NULL,
1531                                 ARRAY_SIZE(reg_params), reg_params,
1532                                 algorithm->address + codesize,
1533                                 fifosize + 2 * sizeof(uint32_t),
1534                                 algorithm->address, exit_point,
1535                                 &armv7m_info);
1536         } else {
1537                 retval = target_run_read_async_algorithm(target, buffer, count, 1,
1538                                 0, NULL,
1539                                 ARRAY_SIZE(reg_params), reg_params,
1540                                 algorithm->address + codesize,
1541                                 fifosize + 2 * sizeof(uint32_t),
1542                                 algorithm->address, exit_point,
1543                                 &armv7m_info);
1544         }
1545
1546         remaining = buf_get_u32(reg_params[0].value, 0, 32);
1547         if ((retval == ERROR_OK) && remaining)
1548                 retval = ERROR_FLASH_OPERATION_FAILED;
1549
1550         if (retval != ERROR_OK) {
1551                 offset = buf_get_u32(reg_params[2].value, 0, 32);
1552                 LOG_ERROR("flash %s failed at address 0x%" PRIx32 ", remaining 0x%" PRIx32,
1553                         write ? "write" : "read", offset, remaining);
1554         }
1555
1556         destroy_reg_param(&reg_params[0]);
1557         destroy_reg_param(&reg_params[1]);
1558         destroy_reg_param(&reg_params[2]);
1559         destroy_reg_param(&reg_params[3]);
1560         destroy_reg_param(&reg_params[4]);
1561         destroy_reg_param(&reg_params[5]);
1562
1563 err:
1564         target_free_working_area(target, algorithm);
1565
1566         /* Switch to memory mapped mode before return to prompt */
1567         set_mm_mode(bank);
1568
1569         return retval;
1570 }
1571
1572 static int stmqspi_read(struct flash_bank *bank, uint8_t *buffer,
1573         uint32_t offset, uint32_t count)
1574 {
1575         struct target *target = bank->target;
1576         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
1577         int retval;
1578
1579         LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
1580                 __func__, offset, count);
1581
1582         if (target->state != TARGET_HALTED) {
1583                 LOG_ERROR("Target not halted");
1584                 return ERROR_TARGET_NOT_HALTED;
1585         }
1586
1587         if (!(stmqspi_info->probed)) {
1588                 LOG_ERROR("Flash bank not probed");
1589                 return ERROR_FLASH_BANK_NOT_PROBED;
1590         }
1591
1592         if (offset + count > bank->size) {
1593                 LOG_WARNING("Read beyond end of flash. Extra data to be ignored.");
1594                 count = bank->size - offset;
1595         }
1596
1597         /* Abort any previous operation */
1598         retval = stmqspi_abort(bank);
1599         if (retval != ERROR_OK)
1600                 return retval;
1601
1602         /* Wait for busy to be cleared */
1603         retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
1604         if (retval != ERROR_OK)
1605                 return retval;
1606
1607         return qspi_read_write_block(bank, buffer, offset, count, false);
1608 }
1609
1610 static int stmqspi_write(struct flash_bank *bank, const uint8_t *buffer,
1611         uint32_t offset, uint32_t count)
1612 {
1613         struct target *target = bank->target;
1614         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
1615         unsigned int dual, sector;
1616         bool octal_dtr;
1617         int retval;
1618
1619         LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
1620                 __func__, offset, count);
1621
1622         dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
1623         octal_dtr = IS_OCTOSPI && (stmqspi_info->saved_ccr & BIT(OCTOSPI_DDTR));
1624
1625         if (target->state != TARGET_HALTED) {
1626                 LOG_ERROR("Target not halted");
1627                 return ERROR_TARGET_NOT_HALTED;
1628         }
1629
1630         if (!(stmqspi_info->probed)) {
1631                 LOG_ERROR("Flash bank not probed");
1632                 return ERROR_FLASH_BANK_NOT_PROBED;
1633         }
1634
1635         if (offset + count > bank->size) {
1636                 LOG_WARNING("Write beyond end of flash. Extra data discarded.");
1637                 count = bank->size - offset;
1638         }
1639
1640         /* Check sector protection */
1641         for (sector = 0; sector < bank->num_sectors; sector++) {
1642                 /* Start offset in or before this sector? */
1643                 /* End offset in or behind this sector? */
1644                 if ((offset < (bank->sectors[sector].offset + bank->sectors[sector].size)) &&
1645                         ((offset + count - 1) >= bank->sectors[sector].offset) &&
1646                         bank->sectors[sector].is_protected) {
1647                         LOG_ERROR("Flash sector %u protected", sector);
1648                         return ERROR_FLASH_PROTECTED;
1649                 }
1650         }
1651
1652         if ((dual || octal_dtr) && ((offset & 1) != 0 || (count & 1) != 0)) {
1653                 LOG_ERROR("In dual-QSPI and octal-DTR modes writes must be two byte aligned: "
1654                         "%s: address=0x%08" PRIx32 " len=0x%08" PRIx32, __func__, offset, count);
1655                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
1656         }
1657
1658         /* Abort any previous operation */
1659         retval = stmqspi_abort(bank);
1660         if (retval != ERROR_OK)
1661                 return retval;
1662
1663         /* Wait for busy to be cleared */
1664         retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
1665         if (retval != ERROR_OK)
1666                 return retval;
1667
1668         return qspi_read_write_block(bank, (uint8_t *)buffer, offset, count, true);
1669 }
1670
1671 static int stmqspi_verify(struct flash_bank *bank, const uint8_t *buffer,
1672         uint32_t offset, uint32_t count)
1673 {
1674         struct target *target = bank->target;
1675         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
1676         unsigned int dual;
1677         bool octal_dtr;
1678         int retval;
1679
1680         LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
1681                 __func__, offset, count);
1682
1683         dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
1684         octal_dtr = IS_OCTOSPI && (stmqspi_info->saved_ccr & BIT(OCTOSPI_DDTR));
1685
1686         if (target->state != TARGET_HALTED) {
1687                 LOG_ERROR("Target not halted");
1688                 return ERROR_TARGET_NOT_HALTED;
1689         }
1690
1691         if (!(stmqspi_info->probed)) {
1692                 LOG_ERROR("Flash bank not probed");
1693                 return ERROR_FLASH_BANK_NOT_PROBED;
1694         }
1695
1696         if (offset + count > bank->size) {
1697                 LOG_WARNING("Verify beyond end of flash. Extra data ignored.");
1698                 count = bank->size - offset;
1699         }
1700
1701         if ((dual || octal_dtr) && ((offset & 1) != 0 || (count & 1) != 0)) {
1702                 LOG_ERROR("In dual-QSPI and octal-DTR modes reads must be two byte aligned: "
1703                         "%s: address=0x%08" PRIx32 " len=0x%08" PRIx32, __func__, offset, count);
1704                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
1705         }
1706
1707         /* Abort any previous operation */
1708         retval = stmqspi_abort(bank);
1709         if (retval != ERROR_OK)
1710                 return retval;
1711
1712         /* Wait for busy to be cleared */
1713         retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
1714         if (retval != ERROR_OK)
1715                 return retval;
1716
1717         return qspi_verify(bank, (uint8_t *)buffer, offset, count);
1718 }
1719
1720 /* Find appropriate dummy setting, in particular octo mode */
1721 static int find_sfdp_dummy(struct flash_bank *bank, int len)
1722 {
1723         struct target *target = bank->target;
1724         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
1725         uint32_t io_base = stmqspi_info->io_base;
1726         uint8_t data;
1727         unsigned int dual, count;
1728         bool flash1 = !(stmqspi_info->saved_cr & BIT(SPI_FSEL_FLASH));
1729         int retval;
1730         const unsigned int max_bytes = 64;
1731
1732         dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
1733
1734         LOG_DEBUG("%s: len=%d, dual=%u, flash1=%d",
1735                 __func__, len, dual, flash1);
1736
1737         /* Abort any previous operation */
1738         retval = target_write_u32(target, io_base + SPI_CR,
1739                 stmqspi_info->saved_cr | BIT(SPI_ABORT));
1740         if (retval != ERROR_OK)
1741                 goto err;
1742
1743         /* Wait for busy to be cleared */
1744         retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
1745         if (retval != ERROR_OK)
1746                 goto err;
1747
1748         /* Switch to saved_cr (had to be set accordingly before this call) */
1749         retval = target_write_u32(target, io_base + SPI_CR, stmqspi_info->saved_cr);
1750         if (retval != ERROR_OK)
1751                 goto err;
1752
1753         /* Read at most that many bytes */
1754         retval = target_write_u32(target, io_base + SPI_DLR, (max_bytes << dual) - 1);
1755         if (retval != ERROR_OK)
1756                 return retval;
1757
1758         /* Read SFDP block */
1759         if (IS_OCTOSPI)
1760                 retval = octospi_cmd(bank, OCTOSPI_READ_MODE,
1761                         OCTOSPI_CCR_READ_SFDP(len), SPIFLASH_READ_SFDP);
1762         else
1763                 retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_SFDP);
1764         if (retval != ERROR_OK)
1765                 goto err;
1766
1767         /* Read from start of sfdp block */
1768         retval = target_write_u32(target, io_base + SPI_AR, 0);
1769         if (retval != ERROR_OK)
1770                 goto err;
1771
1772         for (count = 0 ; count < max_bytes; count++) {
1773                 if ((dual != 0) && !flash1) {
1774                         /* discard even byte in dual flash-mode if flash2 */
1775                         retval = target_read_u8(target, io_base + SPI_DR, &data);
1776                         if (retval != ERROR_OK)
1777                                 goto err;
1778                 }
1779
1780                 retval = target_read_u8(target, io_base + SPI_DR, &data);
1781                 if (retval != ERROR_OK)
1782                         goto err;
1783
1784                 if (data == 0x53) {
1785                         LOG_DEBUG("start of SFDP header for flash%c after %u dummy bytes",
1786                                 flash1 ? '1' : '2', count);
1787                         if (flash1)
1788                                 stmqspi_info->sfdp_dummy1 = count;
1789                         else
1790                                 stmqspi_info->sfdp_dummy2 = count;
1791                         return ERROR_OK;
1792                 }
1793
1794                 if ((dual != 0) && flash1) {
1795                         /* discard odd byte in dual flash-mode if flash1 */
1796                         retval = target_read_u8(target, io_base + SPI_DR, &data);
1797                         if (retval != ERROR_OK)
1798                                 goto err;
1799                 }
1800         }
1801
1802         retval = ERROR_FAIL;
1803         LOG_DEBUG("no start of SFDP header even after %u dummy bytes", count);
1804
1805 err:
1806         /* Abort operation */
1807         retval = stmqspi_abort(bank);
1808
1809         return retval;
1810 }
1811
1812 /* Read SFDP parameter block */
1813 static int read_sfdp_block(struct flash_bank *bank, uint32_t addr,
1814         uint32_t words, uint32_t *buffer)
1815 {
1816         struct target *target = bank->target;
1817         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
1818         uint32_t io_base = stmqspi_info->io_base;
1819         bool flash1 = !(stmqspi_info->saved_cr & BIT(SPI_FSEL_FLASH));
1820         unsigned int dual, count, len, *dummy;
1821         int retval;
1822
1823         dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
1824
1825         if (IS_OCTOSPI && (((stmqspi_info->saved_ccr >> SPI_DMODE_POS) & 0x7) > 3)) {
1826                 /* in OCTO mode 4-byte address and (yet) unknown number of dummy clocks */
1827                 len = 4;
1828
1829                 /* in octo mode, use sfdp_dummy1 only */
1830                 dummy = &stmqspi_info->sfdp_dummy1;
1831                 if (*dummy == 0) {
1832                         retval = find_sfdp_dummy(bank, len);
1833                         if (retval != ERROR_OK)
1834                                 return retval;
1835                 }
1836         } else {
1837                 /* in all other modes 3-byte-address and 8(?) dummy clocks */
1838                 len = 3;
1839
1840                 /* use sfdp_dummy1/2 according to currently selected flash */
1841                 dummy = (stmqspi_info->saved_cr & BIT(SPI_FSEL_FLASH)) ?
1842                         &stmqspi_info->sfdp_dummy2 : &stmqspi_info->sfdp_dummy1;
1843
1844                 /* according to SFDP standard, there should always be 8 dummy *CLOCKS*
1845                  * giving 1, 2 or 4 dummy *BYTES*, however, this is apparently not
1846                  * always implemented correctly, so determine the number of dummy bytes
1847                  * dynamically */
1848                 if (*dummy == 0) {
1849                         retval = find_sfdp_dummy(bank, len);
1850                         if (retval != ERROR_OK)
1851                                 return retval;
1852                 }
1853         }
1854
1855         LOG_DEBUG("%s: addr=0x%08" PRIx32 " words=0x%08" PRIx32 " dummy=%u",
1856                 __func__, addr, words, *dummy);
1857
1858         /* Abort any previous operation */
1859         retval = target_write_u32(target, io_base + SPI_CR,
1860                 stmqspi_info->saved_cr | BIT(SPI_ABORT));
1861         if (retval != ERROR_OK)
1862                 goto err;
1863
1864         /* Wait for busy to be cleared */
1865         retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
1866         if (retval != ERROR_OK)
1867                 goto err;
1868
1869         /* Switch to one flash only */
1870         retval = target_write_u32(target, io_base + SPI_CR, stmqspi_info->saved_cr);
1871         if (retval != ERROR_OK)
1872                 goto err;
1873
1874         /* Read that many words plus dummy bytes */
1875         retval = target_write_u32(target, io_base + SPI_DLR,
1876                 ((*dummy + words * sizeof(uint32_t)) << dual) - 1);
1877         if (retval != ERROR_OK)
1878                 goto err;
1879
1880         /* Read SFDP block */
1881         if (IS_OCTOSPI)
1882                 retval = octospi_cmd(bank, OCTOSPI_READ_MODE,
1883                         OCTOSPI_CCR_READ_SFDP(len), SPIFLASH_READ_SFDP);
1884         else
1885                 retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_SFDP);
1886         if (retval != ERROR_OK)
1887                 goto err;
1888
1889         retval = target_write_u32(target, io_base + SPI_AR, addr << dual);
1890         if (retval != ERROR_OK)
1891                 goto err;
1892
1893         /* dummy clocks */
1894         for (count = *dummy << dual; count > 0; --count) {
1895                 retval = target_read_u8(target, io_base + SPI_DR, (uint8_t *)buffer);
1896                 if (retval != ERROR_OK)
1897                         goto err;
1898         }
1899
1900         for ( ; words > 0; words--) {
1901                 if (dual != 0) {
1902                         uint32_t word1, word2;
1903
1904                         retval = target_read_u32(target, io_base + SPI_DR, &word1);
1905                         if (retval != ERROR_OK)
1906                                 goto err;
1907                         retval = target_read_u32(target, io_base + SPI_DR, &word2);
1908                         if (retval != ERROR_OK)
1909                                 goto err;
1910
1911                         if (!flash1) {
1912                                 /* shift odd numbered bytes into even numbered ones */
1913                                 word1 >>= 8;
1914                                 word2 >>= 8;
1915                         }
1916
1917                         /* pack even numbered bytes into one word */
1918                         *buffer = (word1 & 0xFFU) | ((word1 & 0xFF0000U) >> 8) |
1919                                 ((word2 & 0xFFU) << 16) | ((word2 & 0xFF0000U) << 8);
1920
1921
1922                 } else {
1923                         retval = target_read_u32(target, io_base + SPI_DR, buffer);
1924                         if (retval != ERROR_OK)
1925                                 goto err;
1926                 }
1927                 LOG_DEBUG("raw SFDP data 0x%08" PRIx32, *buffer);
1928
1929                 /* endian correction, sfdp data is always le uint32_t based */
1930                 *buffer = le_to_h_u32((uint8_t *)buffer);
1931                 buffer++;
1932         }
1933
1934 err:
1935         return retval;
1936 }
1937
1938 /* Return ID of flash device(s) */
1939 /* On exit, indirect mode is kept */
1940 static int read_flash_id(struct flash_bank *bank, uint32_t *id1, uint32_t *id2)
1941 {
1942         struct target *target = bank->target;
1943         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
1944         uint32_t io_base = stmqspi_info->io_base;
1945         uint8_t byte;
1946         unsigned int type, count, len1, len2;
1947         int retval = ERROR_OK;
1948
1949         /* invalidate both ids */
1950         *id1 = 0;
1951         *id2 = 0;
1952
1953         if (target->state != TARGET_HALTED) {
1954                 LOG_ERROR("Target not halted");
1955                 return ERROR_TARGET_NOT_HALTED;
1956         }
1957
1958         /* SPIFLASH_READ_MID causes device in octal mode to go berserk, so don't use in this case */
1959         for (type = (IS_OCTOSPI && OPI_MODE) ? 1 : 0; type < 2 ; type++) {
1960                 /* Abort any previous operation */
1961                 retval = stmqspi_abort(bank);
1962                 if (retval != ERROR_OK)
1963                         goto err;
1964
1965                 /* Poll WIP */
1966                 retval = wait_till_ready(bank, SPI_PROBE_TIMEOUT);
1967                 if (retval != ERROR_OK)
1968                         goto err;
1969
1970                 /* Wait for busy to be cleared */
1971                 retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
1972                 if (retval != ERROR_OK)
1973                         goto err;
1974
1975                 /* Read at most 16 bytes per chip */
1976                 count = 16;
1977                 retval = target_write_u32(target, io_base + SPI_DLR,
1978                         (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH) ? count * 2 : count) - 1);
1979                 if (retval != ERROR_OK)
1980                         goto err;
1981
1982                 /* Read id: one particular flash chip (N25Q128) switches back to SPI mode when receiving
1983                  * SPI_FLASH_READ_ID in QPI mode, hence try SPIFLASH_READ_MID first */
1984                 switch (type) {
1985                         case 0:
1986                                 if (IS_OCTOSPI)
1987                                         retval = octospi_cmd(bank, OCTOSPI_READ_MODE,
1988                                                 OCTOSPI_CCR_READ_MID, SPIFLASH_READ_MID);
1989                                 else
1990                                         retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_MID);
1991                                 break;
1992
1993                         case 1:
1994                                 if (IS_OCTOSPI)
1995                                         retval = octospi_cmd(bank, OCTOSPI_READ_MODE,
1996                                                 OCTOSPI_CCR_READ_ID, SPIFLASH_READ_ID);
1997                                 else
1998                                         retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_ID);
1999                                 break;
2000
2001                         default:
2002                                 return ERROR_FAIL;
2003                 }
2004
2005                 if (retval != ERROR_OK)
2006                         goto err;
2007
2008                 /* Dummy address 0, only required for 8-line mode */
2009                 if (IS_OCTOSPI && OPI_MODE) {
2010                         retval = target_write_u32(target, io_base + SPI_AR, 0);
2011                         if (retval != ERROR_OK)
2012                                 goto err;
2013                 }
2014
2015                 /* for debugging only */
2016                 uint32_t dummy;
2017                 (void)target_read_u32(target, io_base + SPI_SR, &dummy);
2018
2019                 /* Read ID from Data Register */
2020                 for (len1 = 0, len2 = 0; count > 0; --count) {
2021                         if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) |
2022                                 BIT(SPI_FSEL_FLASH))) != BIT(SPI_FSEL_FLASH)) {
2023                                 retval = target_read_u8(target, io_base + SPI_DR, &byte);
2024                                 if (retval != ERROR_OK)
2025                                         goto err;
2026                                 /* collect 3 bytes without continuation codes */
2027                                 if ((byte != 0x7F) && (len1 < 3)) {
2028                                         *id1 = (*id1 >> 8) | ((uint32_t)byte) << 16;
2029                                         len1++;
2030                                 }
2031                         }
2032                         if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) |
2033                                 BIT(SPI_FSEL_FLASH))) != 0) {
2034                                 retval = target_read_u8(target, io_base + SPI_DR, &byte);
2035                                 if (retval != ERROR_OK)
2036                                         goto err;
2037                                 /* collect 3 bytes without continuation codes */
2038                                 if ((byte != 0x7F) && (len2 < 3)) {
2039                                         *id2 = (*id2 >> 8) | ((uint32_t)byte) << 16;
2040                                         len2++;
2041                                 }
2042                         }
2043                 }
2044
2045                 if (((*id1 != 0x000000) && (*id1 != 0xFFFFFF)) ||
2046                         ((*id2 != 0x000000) && (*id2 != 0xFFFFFF)))
2047                         break;
2048         }
2049
2050         if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) |
2051                 BIT(SPI_FSEL_FLASH))) != BIT(SPI_FSEL_FLASH)) {
2052                 if ((*id1 == 0x000000) || (*id1 == 0xFFFFFF)) {
2053                         /* no id retrieved, so id must be set manually */
2054                         LOG_INFO("No id from flash1");
2055                         retval = ERROR_FLASH_BANK_NOT_PROBED;
2056                 }
2057         }
2058
2059         if ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) | BIT(SPI_FSEL_FLASH))) != 0) {
2060                 if ((*id2 == 0x000000) || (*id2 == 0xFFFFFF)) {
2061                         /* no id retrieved, so id must be set manually */
2062                         LOG_INFO("No id from flash2");
2063                         retval = ERROR_FLASH_BANK_NOT_PROBED;
2064                 }
2065         }
2066
2067 err:
2068         return retval;
2069 }
2070
2071 static int stmqspi_probe(struct flash_bank *bank)
2072 {
2073         struct target *target = bank->target;
2074         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
2075         struct flash_sector *sectors = NULL;
2076         uint32_t io_base = stmqspi_info->io_base;
2077         uint32_t id1 = 0, id2 = 0, data = 0;
2078         const struct flash_device *p;
2079         const uint32_t magic = 0xAEF1510E;
2080         unsigned int dual, fsize;
2081         bool octal_dtr;
2082         int retval;
2083
2084         if (stmqspi_info->probed) {
2085                 bank->size = 0;
2086                 bank->num_sectors = 0;
2087                 free(bank->sectors);
2088                 bank->sectors = NULL;
2089                 memset(&stmqspi_info->dev, 0, sizeof(stmqspi_info->dev));
2090                 stmqspi_info->sfdp_dummy1 = 0;
2091                 stmqspi_info->sfdp_dummy2 = 0;
2092                 stmqspi_info->probed = false;
2093         }
2094
2095         /* Abort any previous operation */
2096         retval = stmqspi_abort(bank);
2097         if (retval != ERROR_OK)
2098                 return retval;
2099
2100         /* Wait for busy to be cleared */
2101         retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
2102         if (retval != ERROR_OK)
2103                 return retval;
2104
2105         /* check whether QSPI_ABR is writeable and readback returns the value written */
2106         retval = target_write_u32(target, io_base + QSPI_ABR, magic);
2107         if (retval == ERROR_OK) {
2108                 retval = target_read_u32(target, io_base + QSPI_ABR, &data);
2109                 retval = target_write_u32(target, io_base + QSPI_ABR, 0);
2110         }
2111
2112         if (data == magic) {
2113                 LOG_DEBUG("QSPI_ABR register present");
2114                 stmqspi_info->octo = false;
2115         } else {
2116                 uint32_t magic_id;
2117
2118                 retval = target_read_u32(target, io_base + OCTOSPI_MAGIC, &magic_id);
2119
2120                 if (retval == ERROR_OK && magic_id == OCTO_MAGIC_ID) {
2121                         LOG_DEBUG("OCTOSPI_MAGIC present");
2122                         stmqspi_info->octo = true;
2123                 } else {
2124                         LOG_ERROR("No QSPI, no OCTOSPI at 0x%08" PRIx32, io_base);
2125                         stmqspi_info->probed = false;
2126                         stmqspi_info->dev.name = "none";
2127                         return ERROR_FAIL;
2128                 }
2129         }
2130
2131         /* save current FSEL and DFM bits in QSPI/OCTOSPI_CR, current QSPI/OCTOSPI_CCR value */
2132         retval = target_read_u32(target, io_base + SPI_CR, &stmqspi_info->saved_cr);
2133         if (retval == ERROR_OK)
2134                 retval = target_read_u32(target, io_base + SPI_CCR, &stmqspi_info->saved_ccr);
2135
2136         if (IS_OCTOSPI) {
2137                 uint32_t dcr1;
2138
2139                 retval = target_read_u32(target, io_base + OCTOSPI_DCR1, &dcr1);
2140
2141                 if (retval == ERROR_OK)
2142                         retval = target_read_u32(target, io_base + OCTOSPI_TCR,
2143                                 &stmqspi_info->saved_tcr);
2144
2145                 if (retval == ERROR_OK)
2146                         retval = target_read_u32(target, io_base + OCTOSPI_IR,
2147                                 &stmqspi_info->saved_ir);
2148
2149                 if (retval != ERROR_OK) {
2150                         LOG_ERROR("No OCTOSPI at io_base 0x%08" PRIx32, io_base);
2151                         stmqspi_info->probed = false;
2152                         stmqspi_info->dev.name = "none";
2153                         return ERROR_FAIL;
2154                 }
2155
2156                 const uint32_t mtyp = (dcr1 & OCTOSPI_MTYP_MASK) >> OCTOSPI_MTYP_POS;
2157
2158                 if ((mtyp != 0x0) && (mtyp != 0x1)) {
2159                         LOG_ERROR("Only regular SPI protocol supported in OCTOSPI");
2160                         stmqspi_info->probed = false;
2161                         stmqspi_info->dev.name = "none";
2162                         return ERROR_FAIL;
2163                 }
2164
2165                 LOG_DEBUG("OCTOSPI at 0x%08" PRIx64 ", io_base at 0x%08" PRIx32 ", OCTOSPI_CR 0x%08"
2166                         PRIx32 ", OCTOSPI_CCR 0x%08" PRIx32 ", %d-byte addr", bank->base, io_base,
2167                         stmqspi_info->saved_cr, stmqspi_info->saved_ccr, SPI_ADSIZE);
2168         } else {
2169                 if (retval == ERROR_OK) {
2170                         LOG_DEBUG("QSPI at 0x%08" PRIx64 ", io_base at 0x%08" PRIx32 ", QSPI_CR 0x%08"
2171                                 PRIx32 ", QSPI_CCR 0x%08" PRIx32 ", %d-byte addr", bank->base, io_base,
2172                                 stmqspi_info->saved_cr, stmqspi_info->saved_ccr, SPI_ADSIZE);
2173                                 if (stmqspi_info->saved_ccr & (1U << QSPI_DDRM))
2174                                         LOG_WARNING("DDR mode is untested and suffers from some silicon bugs");
2175                 } else {
2176                         LOG_ERROR("No QSPI at io_base 0x%08" PRIx32, io_base);
2177                         stmqspi_info->probed = false;
2178                         stmqspi_info->dev.name = "none";
2179                         return ERROR_FAIL;
2180                 }
2181         }
2182
2183         dual = (stmqspi_info->saved_cr & BIT(SPI_DUAL_FLASH)) ? 1 : 0;
2184         octal_dtr = IS_OCTOSPI && (stmqspi_info->saved_ccr & BIT(OCTOSPI_DDTR));
2185         if (dual || octal_dtr)
2186                 bank->write_start_alignment = bank->write_end_alignment = 2;
2187         else
2188                 bank->write_start_alignment = bank->write_end_alignment = 1;
2189
2190         /* read and decode flash ID; returns in indirect mode */
2191         retval = read_flash_id(bank, &id1, &id2);
2192         LOG_DEBUG("id1 0x%06" PRIx32 ", id2 0x%06" PRIx32, id1, id2);
2193         if (retval == ERROR_FLASH_BANK_NOT_PROBED) {
2194                 /* no id retrieved, so id must be set manually */
2195                 LOG_INFO("No id - set flash parameters manually");
2196                 retval = ERROR_OK;
2197                 goto err;
2198         }
2199
2200         if (retval != ERROR_OK)
2201                 goto err;
2202
2203         /* identify flash1 */
2204         for (p = flash_devices; id1 && p->name ; p++) {
2205                 if (p->device_id == id1) {
2206                         memcpy(&stmqspi_info->dev, p, sizeof(stmqspi_info->dev));
2207                         if (p->size_in_bytes / 4096)
2208                                 LOG_INFO("flash1 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
2209                                         " KiB", p->name, id1, p->size_in_bytes / 1024);
2210                         else
2211                                 LOG_INFO("flash1 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
2212                                         " B", p->name, id1, p->size_in_bytes);
2213                         break;
2214                 }
2215         }
2216
2217         if (id1 && !p->name) {
2218                 /* chip not been identified by id, then try SFDP */
2219                 struct flash_device temp;
2220                 uint32_t saved_cr = stmqspi_info->saved_cr;
2221
2222                 /* select flash1 */
2223                 stmqspi_info->saved_cr = stmqspi_info->saved_cr & ~BIT(SPI_FSEL_FLASH);
2224                 retval = spi_sfdp(bank, &temp, &read_sfdp_block);
2225
2226                 /* restore saved_cr */
2227                 stmqspi_info->saved_cr = saved_cr;
2228
2229                 if (retval == ERROR_OK) {
2230                         LOG_INFO("flash1 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
2231                                 " KiB", temp.name, id1, temp.size_in_bytes / 1024);
2232                         /* save info and retrieved *good* id as spi_sfdp clears all info */
2233                         memcpy(&stmqspi_info->dev, &temp, sizeof(stmqspi_info->dev));
2234                         stmqspi_info->dev.device_id = id1;
2235                 } else {
2236                         /* even not identified by SFDP, then give up */
2237                         LOG_WARNING("Unknown flash1 device id = 0x%06" PRIx32
2238                                 " - set flash parameters manually", id1);
2239                         retval = ERROR_OK;
2240                         goto err;
2241                 }
2242         }
2243
2244         /* identify flash2 */
2245         for (p = flash_devices; id2 && p->name ; p++) {
2246                 if (p->device_id == id2) {
2247                         if (p->size_in_bytes / 4096)
2248                                 LOG_INFO("flash2 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
2249                                         " KiB", p->name, id2, p->size_in_bytes / 1024);
2250                         else
2251                                 LOG_INFO("flash2 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
2252                                         " B", p->name, id2, p->size_in_bytes);
2253
2254                         if (!id1)
2255                                 memcpy(&stmqspi_info->dev, p, sizeof(stmqspi_info->dev));
2256                         else {
2257                                 if ((stmqspi_info->dev.read_cmd != p->read_cmd) ||
2258                                         (stmqspi_info->dev.qread_cmd != p->qread_cmd) ||
2259                                         (stmqspi_info->dev.pprog_cmd != p->pprog_cmd) ||
2260                                         (stmqspi_info->dev.erase_cmd != p->erase_cmd) ||
2261                                         (stmqspi_info->dev.chip_erase_cmd != p->chip_erase_cmd) ||
2262                                         (stmqspi_info->dev.sectorsize != p->sectorsize) ||
2263                                         (stmqspi_info->dev.size_in_bytes != p->size_in_bytes)) {
2264                                         LOG_ERROR("Incompatible flash1/flash2 devices");
2265                                         goto err;
2266                                 }
2267                                 /* page size is optional in SFDP, so accept smallest value */
2268                                 if (p->pagesize < stmqspi_info->dev.pagesize)
2269                                         stmqspi_info->dev.pagesize = p->pagesize;
2270                         }
2271                         break;
2272                 }
2273         }
2274
2275         if (id2 && !p->name) {
2276                 /* chip not been identified by id, then try SFDP */
2277                 struct flash_device temp;
2278                 uint32_t saved_cr = stmqspi_info->saved_cr;
2279
2280                 /* select flash2 */
2281                 stmqspi_info->saved_cr = stmqspi_info->saved_cr | BIT(SPI_FSEL_FLASH);
2282                 retval = spi_sfdp(bank, &temp, &read_sfdp_block);
2283
2284                 /* restore saved_cr */
2285                 stmqspi_info->saved_cr = saved_cr;
2286
2287                 if (retval == ERROR_OK)
2288                         LOG_INFO("flash2 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
2289                                 " KiB", temp.name, id2, temp.size_in_bytes / 1024);
2290                 else {
2291                         /* even not identified by SFDP, then give up */
2292                         LOG_WARNING("Unknown flash2 device id = 0x%06" PRIx32
2293                                 " - set flash parameters manually", id2);
2294                         retval = ERROR_OK;
2295                         goto err;
2296                 }
2297
2298                 if (!id1)
2299                         memcpy(&stmqspi_info->dev, &temp, sizeof(stmqspi_info->dev));
2300                 else {
2301                         if ((stmqspi_info->dev.read_cmd != temp.read_cmd) ||
2302                                 (stmqspi_info->dev.qread_cmd != temp.qread_cmd) ||
2303                                 (stmqspi_info->dev.pprog_cmd != temp.pprog_cmd) ||
2304                                 (stmqspi_info->dev.erase_cmd != temp.erase_cmd) ||
2305                                 (stmqspi_info->dev.chip_erase_cmd != temp.chip_erase_cmd) ||
2306                                 (stmqspi_info->dev.sectorsize != temp.sectorsize) ||
2307                                 (stmqspi_info->dev.size_in_bytes != temp.size_in_bytes)) {
2308                                 LOG_ERROR("Incompatible flash1/flash2 devices");
2309                                 goto err;
2310                         }
2311                         /* page size is optional in SFDP, so accept smallest value */
2312                         if (temp.pagesize < stmqspi_info->dev.pagesize)
2313                                 stmqspi_info->dev.pagesize = temp.pagesize;
2314                 }
2315         }
2316
2317         /* Set correct size value */
2318         bank->size = stmqspi_info->dev.size_in_bytes << dual;
2319
2320         uint32_t dcr;
2321         retval = target_read_u32(target, io_base + SPI_DCR, &dcr);
2322
2323         if (retval != ERROR_OK)
2324                 goto err;
2325
2326         fsize = (dcr >> SPI_FSIZE_POS) & (BIT(SPI_FSIZE_LEN) - 1);
2327
2328         LOG_DEBUG("FSIZE = 0x%04x", fsize);
2329         if (bank->size == BIT((fsize + 1)))
2330                 LOG_DEBUG("FSIZE in DCR(1) matches actual capacity. Beware of silicon bug in H7, L4+, MP1.");
2331         else if (bank->size == BIT((fsize + 0)))
2332                 LOG_DEBUG("FSIZE in DCR(1) is off by one regarding actual capacity. Fix for silicon bug?");
2333         else
2334                 LOG_ERROR("FSIZE in DCR(1) doesn't match actual capacity.");
2335
2336         /* if no sectors, then treat whole flash as single sector */
2337         if (stmqspi_info->dev.sectorsize == 0)
2338                 stmqspi_info->dev.sectorsize = stmqspi_info->dev.size_in_bytes;
2339         /* if no page_size, then use sectorsize as page_size */
2340         if (stmqspi_info->dev.pagesize == 0)
2341                 stmqspi_info->dev.pagesize = stmqspi_info->dev.sectorsize;
2342
2343         /* create and fill sectors array */
2344         bank->num_sectors = stmqspi_info->dev.size_in_bytes / stmqspi_info->dev.sectorsize;
2345         sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
2346         if (!sectors) {
2347                 LOG_ERROR("not enough memory");
2348                 retval = ERROR_FAIL;
2349                 goto err;
2350         }
2351
2352         for (unsigned int sector = 0; sector < bank->num_sectors; sector++) {
2353                 sectors[sector].offset = sector * (stmqspi_info->dev.sectorsize << dual);
2354                 sectors[sector].size = (stmqspi_info->dev.sectorsize << dual);
2355                 sectors[sector].is_erased = -1;
2356                 sectors[sector].is_protected = 0;
2357         }
2358
2359         bank->sectors = sectors;
2360         stmqspi_info->probed = true;
2361
2362 err:
2363         /* Switch to memory mapped mode before return to prompt */
2364         set_mm_mode(bank);
2365
2366         return retval;
2367 }
2368
2369 static int stmqspi_auto_probe(struct flash_bank *bank)
2370 {
2371         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
2372
2373         if (stmqspi_info->probed)
2374                 return ERROR_OK;
2375         stmqspi_probe(bank);
2376         return ERROR_OK;
2377 }
2378
2379 static int stmqspi_protect_check(struct flash_bank *bank)
2380 {
2381         /* Nothing to do. Protection is only handled in SW. */
2382         return ERROR_OK;
2383 }
2384
2385 static int get_stmqspi_info(struct flash_bank *bank, struct command_invocation *cmd)
2386 {
2387         struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
2388
2389         if (!(stmqspi_info->probed)) {
2390                 command_print_sameline(cmd, "\nQSPI flash bank not probed yet\n");
2391                 return ERROR_FLASH_BANK_NOT_PROBED;
2392         }
2393
2394         command_print_sameline(cmd, "flash%s%s \'%s\', device id = 0x%06" PRIx32
2395                         ", flash size = %" PRIu32 "%s B\n(page size = %" PRIu32
2396                         ", read = 0x%02" PRIx8 ", qread = 0x%02" PRIx8
2397                         ", pprog = 0x%02" PRIx8 ", mass_erase = 0x%02" PRIx8
2398                         ", sector size = %" PRIu32 " %sB, sector_erase = 0x%02" PRIx8 ")",
2399                         ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) |
2400                         BIT(SPI_FSEL_FLASH))) != BIT(SPI_FSEL_FLASH)) ? "1" : "",
2401                         ((stmqspi_info->saved_cr & (BIT(SPI_DUAL_FLASH) |
2402                         BIT(SPI_FSEL_FLASH))) != 0) ? "2" : "",
2403                         stmqspi_info->dev.name, stmqspi_info->dev.device_id,
2404                         bank->size / 4096 ? bank->size / 1024 : bank->size,
2405                         bank->size / 4096 ? "Ki" : "", stmqspi_info->dev.pagesize,
2406                         stmqspi_info->dev.read_cmd, stmqspi_info->dev.qread_cmd,
2407                         stmqspi_info->dev.pprog_cmd, stmqspi_info->dev.chip_erase_cmd,
2408                         stmqspi_info->dev.sectorsize / 4096 ?
2409                                 stmqspi_info->dev.sectorsize / 1024 : stmqspi_info->dev.sectorsize,
2410                         stmqspi_info->dev.sectorsize / 4096 ? "Ki" : "",
2411                         stmqspi_info->dev.erase_cmd);
2412
2413         return ERROR_OK;
2414 }
2415
2416 static const struct command_registration stmqspi_exec_command_handlers[] = {
2417         {
2418                 .name = "mass_erase",
2419                 .handler = stmqspi_handle_mass_erase_command,
2420                 .mode = COMMAND_EXEC,
2421                 .usage = "bank_id",
2422                 .help = "Mass erase entire flash device.",
2423         },
2424         {
2425                 .name = "set",
2426                 .handler = stmqspi_handle_set,
2427                 .mode = COMMAND_EXEC,
2428                 .usage = "bank_id name chip_size page_size read_cmd qread_cmd pprg_cmd "
2429                         "[ mass_erase_cmd ] [ sector_size sector_erase_cmd ]",
2430                 .help = "Set params of single flash chip",
2431         },
2432         {
2433                 .name = "cmd",
2434                 .handler = stmqspi_handle_cmd,
2435                 .mode = COMMAND_EXEC,
2436                 .usage = "bank_id num_resp cmd_byte ...",
2437                 .help = "Send low-level command cmd_byte and following bytes or read num_resp.",
2438         },
2439         COMMAND_REGISTRATION_DONE
2440 };
2441
2442 static const struct command_registration stmqspi_command_handlers[] = {
2443         {
2444                 .name = "stmqspi",
2445                 .mode = COMMAND_ANY,
2446                 .help = "stmqspi flash command group",
2447                 .usage = "",
2448                 .chain = stmqspi_exec_command_handlers,
2449         },
2450         COMMAND_REGISTRATION_DONE
2451 };
2452
2453 struct flash_driver stmqspi_flash = {
2454         .name = "stmqspi",
2455         .commands = stmqspi_command_handlers,
2456         .flash_bank_command = stmqspi_flash_bank_command,
2457         .erase = stmqspi_erase,
2458         .protect = stmqspi_protect,
2459         .write = stmqspi_write,
2460         .read = stmqspi_read,
2461         .verify = stmqspi_verify,
2462         .probe = stmqspi_probe,
2463         .auto_probe = stmqspi_auto_probe,
2464         .erase_check = stmqspi_blank_check,
2465         .protect_check = stmqspi_protect_check,
2466         .info = get_stmqspi_info,
2467         .free_driver_priv = default_flash_free_driver_priv,
2468 };