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

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