drivers: flash: stm32h7 flash has a write_optb function to set rdp level

Move the stm32h7 flash register manipulation in a write_optb()
to be called by the flash_stm32_set_rdp_level

Signed-off-by: Francois Ramu <[email protected]>

Author: FRASTM

drivers/flash/flash_stm32h7x.c

@@ -62,44 +62,55 @@ struct flash_stm32_sector_t {
 	volatile uint32_t *sr;
 };
 
-#if defined(CONFIG_FLASH_STM32_READOUT_PROTECTION)
-uint8_t flash_stm32_get_rdp_level(const struct device *dev)
-{
-	FLASH_TypeDef *regs = FLASH_STM32_REGS(dev);
-
-	return (regs->OPTSR_CUR & FLASH_OPTSR_RDP_Msk) >> FLASH_OPTSR_RDP_Pos;
-}
-
-void flash_stm32_set_rdp_level(const struct device *dev, uint8_t level)
+static __unused int write_optb(const struct device *dev, uint32_t mask,
+			       uint32_t value)
 {
 	FLASH_TypeDef *regs = FLASH_STM32_REGS(dev);
-	uint32_t value = (uint32_t)level << FLASH_OPTSR_RDP_Pos;
+	int rc;
 
 	if (regs->OPTCR & FLASH_OPTCR_OPTLOCK) {
 		LOG_ERR("Option bytes locked");
-		return;
+		return -EIO;
 	}
 
-	if ((regs->OPTCR & FLASH_OPTSR_RDP_Msk) == value) {
+	if ((regs->OPTCR & mask) == value) {
 		/* Done already */
-		return;
+		return 0;
 	}
 
-	if (flash_stm32_wait_flash_idle(dev) != 0) {
+	rc = flash_stm32_wait_flash_idle(dev);
+	if (rc < 0) {
 		LOG_ERR("Err flash no idle");
-		return;
+		return rc;
 	}
 
-	regs->OPTCR = (regs->OPTCR & ~FLASH_OPTSR_RDP_Msk) | value;
+	regs->OPTCR = (regs->OPTCR & ~mask) | value;
 	regs->OPTCR |= FLASH_OPTCR_OPTSTART;
 
 	/* Make sure previous write is completed. */
 	barrier_dsync_fence_full();
 
-	if (flash_stm32_wait_flash_idle(dev) != 0) {
+	rc = flash_stm32_wait_flash_idle(dev);
+	if (rc < 0) {
 		LOG_ERR("Err flash no idle");
-		return;
+		return rc;
 	}
+
+	return 0;
+}
+
+#if defined(CONFIG_FLASH_STM32_READOUT_PROTECTION)
+uint8_t flash_stm32_get_rdp_level(const struct device *dev)
+{
+	FLASH_TypeDef *regs = FLASH_STM32_REGS(dev);
+
+	return (regs->OPTSR_CUR & FLASH_OPTSR_RDP_Msk) >> FLASH_OPTSR_RDP_Pos;
+}
+
+void flash_stm32_set_rdp_level(const struct device *dev, uint8_t level)
+{
+	write_optb(dev, FLASH_OPTSR_RDP_Msk,
+		(uint32_t)level << FLASH_OPTSR_RDP_Pos);
 }
 #endif /* CONFIG_FLASH_STM32_READOUT_PROTECTION */