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