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