diff --git a/arch/arm/src/stm32h7/stm32_flash.c b/arch/arm/src/stm32h7/stm32_flash.c
index fb11ec88fbe..c251d854729 100644
--- a/arch/arm/src/stm32h7/stm32_flash.c
+++ b/arch/arm/src/stm32h7/stm32_flash.c
@@ -787,7 +787,8 @@ ssize_t up_progmem_eraseblock(size_t block)
 
   if (stm32h7_wait_for_last_operation(priv))
     {
-      return -EIO;
+      ret = -EIO;
+      goto exit_with_sem;
     }
 
   /* Get flash ready and begin erasing single block */
@@ -804,27 +805,34 @@ ssize_t up_progmem_eraseblock(size_t block)
 
   if (stm32h7_wait_for_last_operation(priv))
     {
-      return -EIO;
+      ret = -EIO;
+      goto exit_with_lock_sem;
     }
 
   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);
+  ret = 0;
 
+exit_with_lock_sem:
   stm32h7_lock_flash(priv);
 
+exit_with_sem:
   stm32h7_flash_sem_unlock(priv);
 
   /* Verify */
 
-  if (stm32h7_israngeerased(block_address, up_progmem_erasesize(block)) == 0)
+  if (ret == 0 &&
+      stm32h7_israngeerased(block_address, up_progmem_erasesize(block)) == 0)
     {
-      return up_progmem_erasesize(block); /* success */
+      ret = up_progmem_erasesize(block); /* success */
     }
   else
     {
-      return -EIO; /* failure */
+      ret = -EIO; /* failure */
     }
+
+  return ret;
 }
 
 ssize_t up_progmem_write(size_t addr, const void *buf, size_t count)
@@ -869,7 +877,8 @@ ssize_t up_progmem_write(size_t addr, const void *buf, size_t count)
 
   if (stm32h7_wait_for_last_operation(priv))
     {
-      return -EIO;
+      written = -EIO;
+      goto exit_with_sem;
     }
 
   /* Get flash ready and begin flashing */
@@ -911,7 +920,8 @@ ssize_t up_progmem_write(size_t addr, const void *buf, size_t count)
 
       if (stm32h7_wait_for_last_operation(priv))
         {
-          return -EIO;
+          written = -EIO;
+          goto exit_with_lock_sem;
         }
 
       sr = stm32h7_flash_getreg32(priv, STM32_FLASH_SR1_OFFSET);
@@ -923,9 +933,8 @@ ssize_t up_progmem_write(size_t addr, const void *buf, size_t count)
 
           stm32h7_flash_modifyreg32(priv, STM32_FLASH_CCR1_OFFSET,
                                     0, ~0);
-          stm32h7_lock_flash(priv);
-          stm32h7_flash_sem_unlock(priv);
-          return -EIO;
+          ret = -EIO;
+          goto exit_with_lock_sem;
         }
     }
 
@@ -933,41 +942,47 @@ ssize_t up_progmem_write(size_t addr, const void *buf, size_t count)
 
   stm32h7_flash_modifyreg32(priv, STM32_FLASH_CCR1_OFFSET,
                             0, ~0);
+exit_with_lock_sem:
   stm32h7_lock_flash(priv);
 
   /* Verify */
 
-  for (ll = (uint32_t *) buf, faddr = addr, pcount = count / pagesize;
-      pcount; pcount -= 1, ll += llperpage, faddr += pagesize)
+  if (written > 0)
     {
-      fp = (uint32_t *) faddr;
-      rp = ll;
+      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);
-      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);
+exit_with_sem:
   stm32h7_flash_sem_unlock(priv);
   return written;
 }