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