diff --git a/hal/stm32h7.c b/hal/stm32h7.c
index 3c1b92dff..01d7a47da 100644
--- a/hal/stm32h7.c
+++ b/hal/stm32h7.c
@@ -571,11 +571,9 @@ int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length
         /* Program an OTP word (16 bits) */
         *(volatile uint16_t*)flashAddress = *(const uint16_t*)data;
 
-#if 0
         /* Read it back */
         tmp = *(volatile uint16_t*)flashAddress;
         (void)tmp; /* avoid unused warnings */
-#endif
 
         /* Wait for last operation to be completed */
         flash_otp_wait();
@@ -584,7 +582,7 @@ int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length
         FLASH_OPTCR &= ~FLASH_OPTCR_PG_OTP;
 
         flashAddress += sizeof(uint16_t);
-        data++;
+        (uintptr_t)data++;
         idx += sizeof(uint16_t);
     }