HAL_ChibiOS: fixed H7 flash storage

this fixes the flash re-init problem when flash storage fills on
H7. It was caused by rejecting writes where one or more of the 32 byte
chunks was not all 0xff but was equal to the current data. That
happens when writing to the sector header in AP_FlashStorage

it also moves the interrupt disable inside the loop to allow for
other interrupts to run between blocks

libraries/AP_HAL_ChibiOS/hwdef/common/flash.c

@@ -546,28 +546,37 @@ static bool stm32_flash_write_h7(uint32_t addr, const void *buf, uint32_t count)
         return false;
     }
 
-    // check for erasure
-    if (!stm32h7_check_all_ones(addr, count >> 2)) {
-        return false;
-    }
+    stm32_flash_unlock();
+    bool success = true;
+
+    while (count >= 32) {
+        const uint8_t *b2 = (const uint8_t *)addr;
+        // if the bytes already match then skip this chunk
+        if (memcmp(b, b2, 32) != 0) {
+            // check for erasure
+            if (!stm32h7_check_all_ones(addr, 8)) {
+                return false;
+            }
 
 #if STM32_FLASH_DISABLE_ISR
-    syssts_t sts = chSysGetStatusAndLockX();
+            syssts_t sts = chSysGetStatusAndLockX();
 #endif
 
-    stm32_flash_unlock();
-    bool success = true;
+            bool ok = stm32h7_flash_write32(addr, b);
 
-    while (count >= 32) {
-        if (!stm32h7_flash_write32(addr, b)) {
-            success = false;
-            goto failed;
-        }
+#if STM32_FLASH_DISABLE_ISR
+            chSysRestoreStatusX(sts);
+#endif
 
-        // check contents
-        if (memcmp((void*)addr, b, 32) != 0) {
-            success = false;
-            goto failed;
+            if (!ok) {
+                success = false;
+                goto failed;
+            }
+            // check contents
+            if (memcmp((void*)addr, b, 32) != 0) {
+                success = false;
+                goto failed;
+            }
         }
 
         addr += 32;
@@ -577,9 +586,6 @@ static bool stm32_flash_write_h7(uint32_t addr, const void *buf, uint32_t count)
 
 failed:
     stm32_flash_lock();
-#if STM32_FLASH_DISABLE_ISR
-    chSysRestoreStatusX(sts);
-#endif
     return success;
 }