text: /****************************************************************************
   * arch/arm/src/stm32h7/stm32_flash.c
   *
   *   Copyright (C) 2019 Gregory Nutt. All rights reserved.
   *   Authors: Gregory Nutt <gnutt@nuttx.org>
   *            David Sidrane <david.sidrane@nscdg.com>
   *
   * Ported from stm32f7_flash.c, this is the original license:
   *
   *   Copyright (C) 2018 Wolpike LLC. All rights reserved.
   *   Author: Evgeniy Bobkov <evgen@wolpike.com>
   *
   * Ported from stm32f20xxf40xx_flash.c, this is the original license:
   *
   *   Copyright (C) 2011 Uros Platise. All rights reserved.
   *   Author: Uros Platise <uros.platise@isotel.eu>
   *
   * Redistribution and use in source and binary forms, with or without
   * modification, are permitted provided that the following conditions
   * are met:
   *
   * 1. Redistributions of source code must retain the above copyright
   *    notice, this list of conditions and the following disclaimer.
   * 2. Redistributions in binary form must reproduce the above copyright
   *    notice, this list of conditions and the following disclaimer in
   *    the documentation and/or other materials provided with the
   *    distribution.
   * 3. Neither the name NuttX nor the names of its contributors may be
   *    used to endorse or promote products derived from this software
   *    without specific prior written permission.
   *
   * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
   * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
   * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
   * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
   * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
   * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
   * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
   * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   * POSSIBILITY OF SUCH DAMAGE.
   *
   ****************************************************************************/
  
  /* Provides standard flash access functions, to be used by the  flash mtd
   * driver.  The interface is defined in the include/nuttx/progmem.h
   *
   * Requirements during write/erase operations on FLASH:
   *  - HSI must be ON.
   *  - Low Power Modes are not permitted during write/erase
   */
  
  /****************************************************************************
   * Included Files
   ****************************************************************************/
  
  #include <nuttx/config.h>
  #include <nuttx/arch.h>
  #include <nuttx/semaphore.h>
  
  #include <stdbool.h>
  #include <assert.h>
  #include <errno.h>
  
  #include "barriers.h"
  
  #include "hardware/stm32_flash.h"
  #include "arm_arch.h"
  
  /****************************************************************************
   * Pre-processor Definitions
   ****************************************************************************/
  
  /* Flash size is known from the chip selection:
   *
   *   When CONFIG_STM32H7_FLASH_OVERRIDE_DEFAULT is set the
   *   CONFIG_STM32H7_FLASH_CONFIG_x selects the default FLASH size based on
   *   the chip part number. This value can be overridden with
   *   CONFIG_STM32H7_FLASH_OVERRIDE_x
   *
   *   Parts STM32H74xxE have 512Kb of FLASH
   *   Parts STM32H74xxG have 1024Kb of FLASH
   *   Parts STM32H74xxI have 2048Kb of FLASH
   *
   *   N.B. Only Single bank mode is supported
   */
  
  #define _K(x) ((x)*1024)
  #define FLASH_SECTOR_SIZE  _K(128)
  #define FLASH_PAGE_SIZE        32
  
  #if !defined(CONFIG_STM32H7_FLASH_OVERRIDE_DEFAULT) && \
      !defined(CONFIG_STM32H7_FLASH_OVERRIDE_B) && \
      !defined(CONFIG_STM32H7_FLASH_OVERRIDE_G) && \
      !defined(CONFIG_STM32H7_FLASH_OVERRIDE_I) && \
      !defined(CONFIG_STM32H7_FLASH_CONFIG_B) && \
      !defined(CONFIG_STM32H7_FLASH_CONFIG_G) && \
      !defined(CONFIG_STM32H7_FLASH_CONFIG_I)
  #  define CONFIG_STM32H7_FLASH_OVERRIDE_B
  #  warning "Flash size not defined defaulting to 128KiB (B)"
  #endif
  
  #if !defined(CONFIG_STM32H7_FLASH_OVERRIDE_DEFAULT)
  
  #  undef CONFIG_STM32H7_FLASH_CONFIG_B
  #  undef CONFIG_STM32H7_FLASH_CONFIG_G
  #  undef CONFIG_STM32H7_FLASH_CONFIG_I
  
  #  if defined(CONFIG_STM32H7_FLASH_OVERRIDE_B)
  
  #    define CONFIG_STM32H7_FLASH_CONFIG_B
  
  #  elif defined(CONFIG_STM32H7_FLASH_OVERRIDE_G)
  
  #    define CONFIG_STM32H7_FLASH_CONFIG_G
  
  #  elif defined(CONFIG_STM32H7_FLASH_OVERRIDE_I)
  
  #    define CONFIG_STM32H7_FLASH_CONFIG_I
  
  #  endif
  #endif
  
  #if defined(CONFIG_STM32H7_FLASH_CONFIG_B)
  
  #  define STM32_FLASH_NBLOCKS      1
  #  define STM32_FLASH_SIZE        _K(1 * 128)
  
  #elif defined(CONFIG_STM32H7_FLASH_CONFIG_G)
  
  #  define STM32_FLASH_NBLOCKS      8
  #  define STM32_FLASH_SIZE        _K(8 * 128)
  
  #elif defined(CONFIG_STM32H7_FLASH_CONFIG_I)
  
  #  define STM32_FLASH_NBLOCKS      16
  #  define STM32_FLASH_SIZE        _K(16 * 128)
  
  #endif
  
  #ifndef CONFIG_STM32H7_FLASH_CR_PSIZE
  #define FLASH_CR_PSIZE FLASH_CR_PSIZE_X64
  #else
  #define FLASH_CR_PSIZE (CONFIG_STM32H7_FLASH_CR_PSIZE << FLASH_CR_PSIZE_SHIFT)
  #endif
  
  #define FLASH_KEY1           0x45670123
  #define FLASH_KEY2           0xcdef89ab
  #define FLASH_OPTKEY1        0x08192a3b
  #define FLASH_OPTKEY2        0x4c5d6e7f
  #define FLASH_ERASEDVALUE    0xff
  #define FLASH_ERASEDVALUE_DW 0xffffffff
  #define PROGMEM_NBLOCKS STM32_FLASH_NBLOCKS
  #define FLASH_NPAGES (STM32_FLASH_SIZE / FLASH_PAGE_SIZE)
  
  #define FLASH_TIMEOUT_VALUE 500000  /* 5s */
  
  /****************************************************************************
   * Private Types
   ****************************************************************************/
  
  struct stm32h7_flash_priv_s
  {
    sem_t    sem;     /* Bank exclusive */
    uint32_t ifbase;  /* FLASHIF interface base address */
    uint32_t base;    /* FLASH base address */
    uint32_t stblock; /* The first Block Number */
    uint32_t stpage;  /* The first Page Number */
  };
  
  /****************************************************************************
   * Private Data
   ****************************************************************************/
  
  static struct stm32h7_flash_priv_s stm32h7_flash_bank1_priv =
  {
    .sem     = SEM_INITIALIZER(1),
    .ifbase  = STM32_FLASHIF_BASE + STM32_FLASH_BANK1_OFFSET,
    .base    = STM32_FLASH_BANK1,
    .stblock = 0,
    .stpage  = 0,
  };
  #if STM32_FLASH_NBLOCKS > 1
  static struct stm32h7_flash_priv_s stm32h7_flash_bank2_priv =
  {
    .sem = SEM_INITIALIZER(1),
    .ifbase = STM32_FLASHIF_BASE + STM32_FLASH_BANK2_OFFSET,
    .base   = STM32_FLASH_BANK2,
    .stblock = PROGMEM_NBLOCKS / 2,
    .stpage = FLASH_NPAGES / 2,
  };
  #endif
  
  /****************************************************************************
   * Private Functions
   ****************************************************************************/
  
  /****************************************************************************
   * Name: stm32h7_flash_getreg32
   *
   * Description:
   *   Get a 32-bit register value by offset
   *
   ****************************************************************************/
  
  static inline uint32_t stm32h7_flash_getreg32(FAR struct stm32h7_flash_priv_s
                                              *priv, uint32_t offset)
  {
    return getreg32(priv->ifbase + offset);
  }
  
  /****************************************************************************
   * Name: stm32h7_flash_putreg32
   *
   * Description:
   *  Put a 32-bit register value by offset
   *
   ****************************************************************************/
  
  static inline void stm32h7_flash_putreg32(FAR struct stm32h7_flash_priv_s
                                            *priv, uint32_t offset,
                                            uint32_t value)
  {
    putreg32(value, priv->ifbase + offset);
  }
  
  /****************************************************************************
   * Name: stm32h7_flash_modifyreg32
   *
   * Description:
   *   Modify a 32-bit register value by offset
   *
   ****************************************************************************/
  
  static inline void stm32h7_flash_modifyreg32(FAR struct stm32h7_flash_priv_s
                                               *priv, uint32_t offset,
                                               uint32_t clearbits,
                                               uint32_t setbits)
  {
    modifyreg32(priv->ifbase + offset, clearbits, setbits);
  }
  
  /****************************************************************************
   * Name: stm32h7_flash_sem_lock
   *
   * Description:
   *   Take the Bank exclusive access semaphore
   *
   ****************************************************************************/
  
  static int stm32h7_flash_sem_lock(FAR struct stm32h7_flash_priv_s *priv)
  {
    return nxsem_wait_uninterruptible(&priv->sem);
  }
  
  /****************************************************************************
   * Name: stm32h7_flash_sem_unlock
   *
   * Description:
   *   Release the Bank exclusive access semaphore
   *
   ****************************************************************************/
  
  static inline void stm32h7_flash_sem_unlock(FAR struct stm32h7_flash_priv_s
                                              *priv)
  {
    nxsem_post(&priv->sem);
  }
  
  /****************************************************************************
   * Name: stm32h7_unlock_flash
   *
   * Description:
   *   Unlock the Bank
   *
   ****************************************************************************/
  
  static void stm32h7_unlock_flash(FAR struct stm32h7_flash_priv_s *priv)
  {
    while (stm32h7_flash_getreg32(priv, STM32_FLASH_SR1_OFFSET) & FLASH_SR_BSY)
      {
      }
  
    if (stm32h7_flash_getreg32(priv, STM32_FLASH_CR1_OFFSET) & FLASH_CR_LOCK)
      {
        /* Unlock sequence */
  
        stm32h7_flash_putreg32(priv, STM32_FLASH_KEYR1_OFFSET, FLASH_KEY1);
        stm32h7_flash_putreg32(priv, STM32_FLASH_KEYR1_OFFSET, FLASH_KEY2);
      }
  }
  
  /****************************************************************************
   * Name: stm32h7_lock_flash
   *
   * Description:
   *   Lock the Bank
   *
   ****************************************************************************/
  
  static void stm32h7_lock_flash(FAR struct stm32h7_flash_priv_s *priv)
  {
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_CR1_OFFSET, 0, FLASH_CR_LOCK);
  }
  
  /****************************************************************************
   * Name: stm32h7_flash_size
   *
   * Description:
   *   Returns the size in bytes of FLASH
   *
   ****************************************************************************/
  
  static inline uint32_t stm32h7_flash_size(
      FAR struct stm32h7_flash_priv_s *priv)
  {
    return FLASH_SECTOR_SIZE * PROGMEM_NBLOCKS;
  }
  
  /****************************************************************************
   * Name: stm32h7_flash_bank
   *
   * Description:
   *   Returns the priv pointer to the correct bank
   *
   ****************************************************************************/
  
  static inline
  FAR struct stm32h7_flash_priv_s * stm32h7_flash_bank(size_t address)
  {
    struct stm32h7_flash_priv_s *priv = &stm32h7_flash_bank1_priv;
    if (address < priv->base || address >=
        priv->base + stm32h7_flash_size(priv))
      {
        return NULL;
      }
  
  #if STM32_FLASH_NBLOCKS > 1
    if (address >= stm32h7_flash_bank2_priv.base)
      {
        priv = &stm32h7_flash_bank2_priv;
      }
  #endif
  
    return priv;
  }
  
  /****************************************************************************
   * Name: stm32h7_israngeerased
   *
   * Description:
   *   Returns count of non-erased words
   *
   ****************************************************************************/
  
  static int stm32h7_israngeerased(size_t startaddress, size_t size)
  {
    uint32_t *addr;
    uint8_t *baddr;
    size_t count = 0;
    size_t bwritten = 0;
  
    if (!stm32h7_flash_bank(startaddress) ||
        !stm32h7_flash_bank(startaddress + size))
      {
        return -EIO;
      }
  
    addr = (uint32_t *)startaddress;
    while (count + 4 <= size)
      {
        if (getreg32(addr) != FLASH_ERASEDVALUE_DW)
          {
            bwritten++;
          }
  
        addr++;
        count += 4;
      }
  
    baddr = (uint8_t *)addr;
    while (count < size)
      {
        if (getreg8(baddr) != FLASH_ERASEDVALUE)
          {
            bwritten++;
          }
  
        count++;
      }
  
    return bwritten;
  }
  
  /****************************************************************************
   * Name: stm32h7_wait_for_last_operation()
   *
   * Description:
   *   Wait for last write/erase operation to finish
   *   Return error in case of timeout
   *
   * Input Parameters:
   *   priv  - Flash bank based config
   *
   * Returned Value:
   *     Zero or error value
   *
   *     ETIME:  Timeout while waiting for previous write/erase operation to
   *             complete
   *
   ****************************************************************************/
  
  static int stm32h7_wait_for_last_operation(FAR struct stm32h7_flash_priv_s
                                             *priv)
  {
    int i;
    bool timeout = true;
  
    ARM_DSB();
  
    for (i = 0; i < FLASH_TIMEOUT_VALUE; i++)
      {
        if (!(stm32h7_flash_getreg32(priv, STM32_FLASH_SR1_OFFSET) &
              (FLASH_SR_QW | FLASH_SR_BSY | FLASH_SR_WBNE)))
          {
            timeout = false;
            break;
          }
  
        usleep(1000);
      }
  
    if (timeout)
      {
        return -EBUSY;
      }
  
    return 0;
  }
  
  /****************************************************************************
   * Name: stm32h7_unlock_flashopt
   *
   * Description:
   *   Unlock the flash option bytes
   *   Returns true if the flash was locked before, false otherwise
   *
   ****************************************************************************/
  
  static bool stm32h7_unlock_flashopt(FAR struct stm32h7_flash_priv_s *priv)
  {
    bool ret = false;
  
    while (stm32h7_flash_getreg32(priv, STM32_FLASH_SR1_OFFSET) & FLASH_SR_BSY)
      {
      }
  
    if (stm32h7_flash_getreg32(priv, STM32_FLASH_OPTCR_OFFSET) &
                               FLASH_OPTCR_OPTLOCK)
      {
        /* Unlock sequence */
  
        stm32h7_flash_putreg32(priv, STM32_FLASH_OPTKEYR_OFFSET,
                               FLASH_OPTKEY1);
        stm32h7_flash_putreg32(priv, STM32_FLASH_OPTKEYR_OFFSET,
                               FLASH_OPTKEY2);
  
        /* Was locked before and now unlocked */
  
        ret = true;
      }
  
    return ret;
  }
  
  /****************************************************************************
   * Name: stm32h7_lock_flashopt
   *
   * Description:
   *   Lock the flash option bytes
   *
   ****************************************************************************/
  
  static void stm32h7_lock_flashopt(FAR struct stm32h7_flash_priv_s *priv)
  {
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_OPTCR_OFFSET, 0,
                              FLASH_OPTCR_OPTLOCK);
  }
  
  /****************************************************************************
   * Name: stm32h7_save_flashopt
   *
   * Description:
   *   Save the flash option bytes to non-volatile storage.
   *
   ****************************************************************************/
  
  static void stm32h7_save_flashopt(FAR struct stm32h7_flash_priv_s *priv)
  {
    while (stm32h7_flash_getreg32(priv, STM32_FLASH_SR1_OFFSET) &
          (FLASH_SR_BSY | FLASH_SR_CRCBUSY))
      {
      }
    while (stm32h7_flash_getreg32(priv, STM32_FLASH_SR2_OFFSET) &
          (FLASH_SR_BSY | FLASH_SR_CRCBUSY))
      {
      }
  
    /* Can only write flash options if the option control reg is unlocked */
  
    if (!(stm32h7_flash_getreg32(priv, STM32_FLASH_OPTCR_OFFSET) &
                                 FLASH_OPTCR_OPTLOCK))
      {
        stm32h7_flash_modifyreg32(priv, STM32_FLASH_OPTCR_OFFSET, 0,
                                  FLASH_OPTCR_OPTSTRT);
      }
  
    /* Wait for the update to complete */
  
    while (stm32h7_flash_getreg32(priv, STM32_FLASH_OPTSR_CUR_OFFSET) &
                                        FLASH_OPTSR_BUSYV)
      {
      }
  }
  
  /****************************************************************************
   * Public Functions
   ****************************************************************************/
  
  /****************************************************************************
   * Name: stm32h7_flash_unlock
   *
   * Description:
   *   Unlocks a bank
   *
   ****************************************************************************/
  
  int stm32h7_flash_unlock(size_t addr)
  {
    int ret = -ENODEV;
    struct stm32h7_flash_priv_s *priv = stm32h7_flash_bank(addr);
  
    if (priv)
      {
        ret = stm32h7_flash_sem_lock(priv);
        if (ret < 0)
          {
            return ret;
          }
  
        stm32h7_unlock_flash(priv);
        stm32h7_flash_sem_unlock(priv);
      }
  
    return ret;
  }
  
  /****************************************************************************
   * Name: stm32h7_flash_lock
   *
   * Description:
   *   Locks a bank
   *
   ****************************************************************************/
  
  int stm32h7_flash_lock(size_t addr)
  {
    int ret = -ENODEV;
    struct stm32h7_flash_priv_s *priv = stm32h7_flash_bank(addr);
  
    if (priv)
      {
        ret = stm32h7_flash_sem_lock(priv);
        if (ret < 0)
          {
            return ret;
          }
  
        stm32h7_lock_flash(priv);
        stm32h7_flash_sem_unlock(priv);
      }
  
    return ret;
  }
  
  /****************************************************************************
   * Name: stm32h7_flash_writeprotect
   *
   * Description:
   *   Enable or disable the write protection of a flash sector.
   *
   ****************************************************************************/
  
  int stm32h7_flash_writeprotect(size_t block, bool enabled)
  {
    struct stm32h7_flash_priv_s *priv;
    uint32_t setbits   = 0;
    uint32_t clearbits = 0;
    int      rv        = -ENODEV;
  
    if (block >= PROGMEM_NBLOCKS)
      {
        return -EFAULT;
      }
  
    priv = stm32h7_flash_bank(STM32_FLASH_BANK1 + (block * FLASH_SECTOR_SIZE));
  
    if (priv)
      {
        if (enabled)
          {
            clearbits = 1 << block % (STM32_FLASH_NBLOCKS / 2);
          }
        else
          {
            setbits = 1 << block % (STM32_FLASH_NBLOCKS / 2);
          }
  
        stm32h7_flash_modifyreg32(priv, STM32_FLASH_WPSN_PRG1R_OFFSET,
                                  clearbits, setbits);
        rv  = OK;
      }
  
    return rv;
  }
  
  /****************************************************************************
   * Name: stm32h7_flash_getopt
   *
   * Description:
   *   Returns the current flash option bytes from the FLASH_OPTSR_CR register.
   *
   ****************************************************************************/
  
  uint32_t stm32h7_flash_getopt(void)
  {
    struct stm32h7_flash_priv_s *priv;
    priv = stm32h7_flash_bank(STM32_FLASH_BANK1);
    if (priv)
      {
        return stm32h7_flash_getreg32(priv, STM32_FLASH_OPTSR_CUR_OFFSET);
      }
  
    return 0;
  }
  
  /****************************************************************************
   * Name: stm32h7_flash_optmodify
   *
   * Description:
   *   Modifies the current flash option bytes, given bits to set and clear.
   *
   ****************************************************************************/
  
  void stm32h7_flash_optmodify(uint32_t clear, uint32_t set)
  {
    struct stm32h7_flash_priv_s *priv;
    bool was_locked;
  
    priv = stm32h7_flash_bank(STM32_FLASH_BANK1);
    if (priv)
      {
        was_locked = stm32h7_unlock_flashopt(priv);
        stm32h7_flash_modifyreg32(priv, STM32_FLASH_OPTSR_PRG_OFFSET,
                                  clear, set);
        stm32h7_save_flashopt(priv);
        if (was_locked)
          {
            stm32h7_lock_flashopt(priv);
          }
      }
  }
  
  /****************************************************************************
   * Name: stm32h7_flash_swapbanks
   *
   * Description:
   *   Swaps banks 1 and 2 in the processor's memory map.  Takes effect
   *   the next time the system is reset.
   *
   ****************************************************************************/
  
  void stm32h7_flash_swapbanks(void)
  {
    uint32_t opts = stm32h7_flash_getopt();
    if (opts & FLASH_OPTCR_SWAPBANK)
      {
        stm32h7_flash_optmodify(FLASH_OPTCR_SWAPBANK, 0);
      }
    else
      {
        stm32h7_flash_optmodify(0, FLASH_OPTCR_SWAPBANK);
      }
  }
  
  size_t up_progmem_pagesize(size_t page)
  {
    return FLASH_PAGE_SIZE;
  }
  
  ssize_t up_progmem_getpage(size_t addr)
  {
    struct stm32h7_flash_priv_s *priv;
  
    priv = stm32h7_flash_bank(addr);
  
    if (priv == NULL)
      {
        return -EFAULT;
      }
  
    return  priv->stpage + ((addr - priv->base) / FLASH_PAGE_SIZE);
  }
  
  size_t up_progmem_getaddress(size_t page)
  {
    struct stm32h7_flash_priv_s *priv;
    if (page >= FLASH_NPAGES)
      {
        return SIZE_MAX;
      }
  
    priv = stm32h7_flash_bank(STM32_FLASH_BANK1 + (page * FLASH_PAGE_SIZE));
    return priv->base + (page - priv->stpage) * FLASH_PAGE_SIZE;
  }
  
  size_t up_progmem_neraseblocks(void)
  {
    return PROGMEM_NBLOCKS;
  }
  
  bool up_progmem_isuniform(void)
  {
    return true;
  }
  
  ssize_t up_progmem_ispageerased(size_t page)
  {
    size_t addr;
    size_t count;
    size_t bwritten = 0;
  
    if (page >= FLASH_NPAGES)
      {
        return -EFAULT;
      }
  
    /* Verify */
  
    for (addr = up_progmem_getaddress(page), count = up_progmem_pagesize(page);
         count; count--, addr++)
      {
        if (getreg8(addr) != FLASH_ERASEDVALUE)
          {
            bwritten++;
          }
      }
  
    return bwritten;
  }
  
  size_t up_progmem_erasesize(size_t block)
  {
    return FLASH_SECTOR_SIZE;
  }
  
  ssize_t up_progmem_eraseblock(size_t block)
  {
    struct stm32h7_flash_priv_s *priv;
    int ret;
    size_t block_address = STM32_FLASH_BANK1 + (block * FLASH_SECTOR_SIZE);
  
    if (block >= PROGMEM_NBLOCKS)
      {
        return -EFAULT;
      }
  
    priv = stm32h7_flash_bank(block_address);
  
    ret = stm32h7_flash_sem_lock(priv);
    if (ret < 0)
      {
        return (ssize_t)ret;
      }
  
    if (stm32h7_wait_for_last_operation(priv))
      {
        return -EIO;
      }
  
    /* Get flash ready and begin erasing single block */
  
    stm32h7_unlock_flash(priv);
  
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_CR1_OFFSET, 0, FLASH_CR_SER);
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_CR1_OFFSET, FLASH_CR_SNB_MASK,
                              FLASH_CR_SNB(block - priv->stblock));
  
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_CR1_OFFSET, 0, FLASH_CR_START);
  
    /* Wait for erase operation to complete */
  
    if (stm32h7_wait_for_last_operation(priv))
      {
        return -EIO;
      }
  
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_CR1_OFFSET, FLASH_CR_SER, 0);
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_CR1_OFFSET, FLASH_CR_SNB_MASK,
                              0);
  
    stm32h7_lock_flash(priv);
  
    stm32h7_flash_sem_unlock(priv);
  
    /* Verify */
  
    if (stm32h7_israngeerased(block_address, up_progmem_erasesize(block)) == 0)
      {
        return up_progmem_erasesize(block); /* success */
      }
    else
      {
        return -EIO; /* failure */
      }
  }
  
  ssize_t up_progmem_write(size_t addr, const void *buf, size_t count)
  {
    struct stm32h7_flash_priv_s *priv;
    uint32_t     *fp;
    uint32_t     *rp;
    uint32_t     *ll        = (uint32_t *) buf;
    size_t       faddr;
    size_t       written    = count;
    int ret;
    const size_t pagesize   = up_progmem_pagesize(0); /* 256 bit, 32 bytes per page */
    const size_t llperpage  = pagesize / sizeof(uint32_t);
    size_t       pcount     = count / pagesize;
    uint32_t sr;
  
    priv = stm32h7_flash_bank(addr);
  
    if (priv == NULL)
      {
        return -EFAULT;
      }
  
    /* Check for valid address range */
  
    if (addr < priv->base ||
        addr + count > priv->base + (STM32_FLASH_SIZE / 2))
      {
        return -EFAULT;
      }
  
    ret = stm32h7_flash_sem_lock(priv);
    if (ret < 0)
      {
        return (ssize_t)ret;
      }
  
    /* Check address and count alignment */
  
    DEBUGASSERT(!(addr % pagesize));
    DEBUGASSERT(!(count % pagesize));
  
    if (stm32h7_wait_for_last_operation(priv))
      {
        return -EIO;
      }
  
    /* Get flash ready and begin flashing */
  
    stm32h7_unlock_flash(priv);
  
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_CR1_OFFSET,
                              FLASH_CR_PSIZE_MASK, FLASH_CR_PSIZE);
  
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_CR1_OFFSET, 0, FLASH_CR_PG);
  
    for (ll = (uint32_t *) buf, faddr = addr; pcount;
        pcount -= 1, ll += llperpage, faddr += pagesize)
      {
        fp = (uint32_t *) faddr;
        rp = ll;
  
        ARM_DSB();
        ARM_ISB();
  
        /* Write 8 32 bit word and wait to complete */
  
        *fp++ = *rp++;
        *fp++ = *rp++;
        *fp++ = *rp++;
        *fp++ = *rp++;
        *fp++ = *rp++;
        *fp++ = *rp++;
        *fp++ = *rp++;
        *fp++ = *rp++;
  
        /* Data synchronous Barrier (DSB) just after the write operation. This
         * will force the CPU to respect the sequence of instruction (no
         * optimization).
         */
  
        ARM_DSB();
        ARM_ISB();
  
        if (stm32h7_wait_for_last_operation(priv))
          {
            return -EIO;
          }
  
        sr = stm32h7_flash_getreg32(priv, STM32_FLASH_SR1_OFFSET);
        if (sr & (FLASH_SR_SNECCERR | FLASH_SR_DBECCERR))
          {
            stm32h7_flash_modifyreg32(priv, STM32_FLASH_CR1_OFFSET,
                                      FLASH_CR_PG,
                                      0);
  
            stm32h7_flash_modifyreg32(priv, STM32_FLASH_CCR1_OFFSET,
                                      0, ~0);
            stm32h7_lock_flash(priv);
            stm32h7_flash_sem_unlock(priv);
            return -EIO;
          }
      }
  
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_CR1_OFFSET, FLASH_CR_PG, 0);
  
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_CCR1_OFFSET,
                              0, ~0);
    stm32h7_lock_flash(priv);
  
    /* Verify */
  
    for (ll = (uint32_t *) buf, faddr = addr, pcount = count / pagesize;
        pcount; pcount -= 1, ll += llperpage, faddr += pagesize)
      {
        fp = (uint32_t *) faddr;
        rp = ll;
  
        stm32h7_flash_modifyreg32(priv, STM32_FLASH_CCR1_OFFSET,
                                  0, ~0);
        if ((*fp++ != *rp++) ||
            (*fp++ != *rp++) ||
            (*fp++ != *rp++) ||
            (*fp++ != *rp++) ||
            (*fp++ != *rp++) ||
            (*fp++ != *rp++) ||
            (*fp++ != *rp++) ||
            (*fp++ != *rp++))
          {
            written = -EIO;
            break;
          }
  
        sr = stm32h7_flash_getreg32(priv, STM32_FLASH_SR1_OFFSET);
        if (sr & (FLASH_SR_SNECCERR | FLASH_SR_DBECCERR))
          {
            written = -EIO;
            break;
          }
      }
  
    stm32h7_flash_modifyreg32(priv, STM32_FLASH_CCR1_OFFSET,
                              0, ~0);
    stm32h7_flash_sem_unlock(priv);
    return written;
  }
  