STM32L4 option bytes write

[zx81a]

Hello. I'm new to github, so probably this is a wrong place to post my proposal patches.
Here is a simple support for L4 option bytes, strongly based od G0 version.

diff --git a/include/stm32.h b/include/stm32.h
index 8f8e7eb..1d49ca7 100644
--- a/include/stm32.h
+++ b/include/stm32.h
@@ -17,4 +17,6 @@
 #define STM32_G0_OPTION_BYTES_BASE ((uint32_t)0x1FFF7800)
 #define STM32_L0_CAT2_OPTION_BYTES_BASE ((uint32_t)0x1FF80000)
 #define STM32_F2_OPTION_BYTES_BASE ((uint32_t)0x1FFFC000)
+#define STM32_L496X_OPTION_BYTES_BASE ((uint32_t)0x1FFF7800)
+
 #endif /* STM32_H */
diff --git a/src/common.c b/src/common.c
index 99bd7a8..2daf39b 100644
--- a/src/common.c
+++ b/src/common.c
@@ -137,6 +137,7 @@
 
 //32L4 register base is at FLASH_REGS_ADDR (0x40022000)
 #define STM32L4_FLASH_KEYR      (FLASH_REGS_ADDR + 0x08)
+#define STM32L4_FLASH_OPTKEYR   (FLASH_REGS_ADDR + 0x0C)
 #define STM32L4_FLASH_SR        (FLASH_REGS_ADDR + 0x10)
 #define STM32L4_FLASH_CR        (FLASH_REGS_ADDR + 0x14)
 #define STM32L4_FLASH_OPTR      (FLASH_REGS_ADDR + 0x20)
@@ -145,13 +146,16 @@
 #define STM32L4_FLASH_SR_ERRMASK        0x3f8 /* SR [9:3] */
 
 #define STM32L4_FLASH_CR_LOCK   31      /* Lock control register */
+#define STM32L4_FLASH_CR_OPTLOCK 30	/* Lock option bytes */
 #define STM32L4_FLASH_CR_PG     0       /* Program */
 #define STM32L4_FLASH_CR_PER    1       /* Page erase */
 #define STM32L4_FLASH_CR_MER1   2       /* Bank 1 erase */
 #define STM32L4_FLASH_CR_MER2   15      /* Bank 2 erase */
 #define STM32L4_FLASH_CR_STRT   16      /* Start command */
+#define STM32L4_FLASH_CR_OPTSTRT 17	/* Start writing option bytes */
 #define STM32L4_FLASH_CR_BKER   11      /* Bank select for page erase */
 #define STM32L4_FLASH_CR_PNB    3       /* Page number (8 bits) */
+#define STM32L4_FLASH_CR_OBL_LAUNCH 27  /* Option bytes reload */
 // Bits requesting flash operations (useful when we want to clear them)
 #define STM32L4_FLASH_CR_OPBITS                                     \
     ((1lu<<STM32L4_FLASH_CR_PG) | (1lu<<STM32L4_FLASH_CR_PER)       \
@@ -2665,6 +2669,88 @@ static int stlink_write_option_bytes_l0_cat2(stlink_t *sl, uint8_t* base, uint32
 /**
  * Write option bytes
  * @param sl
+ * @param addr of the memory mapped option bytes
+ * @param base option bytes to write
+ * @return 0 on success, -ve on failure.
+ */
+static int stlink_write_option_bytes_l496x(stlink_t *sl, uint8_t* base, uint32_t len) {
+
+    uint32_t val;
+
+    if(len != 4) {
+        ELOG("Wrong length for writting option bytes, must be 4 is %d\n", len);
+        return -1;
+    }
+
+    /* Unlock flash if necessary  */
+    stlink_read_debug32(sl, STM32L4_FLASH_CR, &val);
+    if ((val & (1u << STM32L4_FLASH_CR_LOCK))) {
+
+        /* disable flash write protection. */
+        stlink_write_debug32(sl, STM32L4_FLASH_KEYR, 0x45670123);
+        stlink_write_debug32(sl, STM32L4_FLASH_KEYR, 0xCDEF89AB);
+
+        // check that the lock is no longer set.
+        stlink_read_debug32(sl, STM32L4_FLASH_CR, &val);
+        if ((val & (1u << STM32L4_FLASH_CR_LOCK))) {
+            ELOG("Flash unlock failed! System reset required to be able to unlock it again!\n");
+            return -1;
+        }
+    }
+
+    /* Unlock option bytes if necessary (ref manuel page 61) */
+    stlink_read_debug32(sl, STM32L4_FLASH_CR, &val);
+    if ((val & (1 << STM32L4_FLASH_CR_OPTLOCK))) {
+
+        /* disable option byte write protection. */
+        stlink_write_debug32(sl, STM32L4_FLASH_OPTKEYR, 0x08192A3B);
+        stlink_write_debug32(sl, STM32L4_FLASH_OPTKEYR, 0x4C5D6E7F);
+
+        /* check that the lock is no longer set. */
+        stlink_read_debug32(sl, STM32L4_FLASH_CR, &val);
+        if ((val & (1 << STM32L4_FLASH_CR_OPTLOCK))) {
+            ELOG("Options bytes unlock failed! System reset required to be able to unlock it again!\n");
+            return -1;
+        }
+    }
+
+    /* Write options bytes */
+    uint32_t data;
+    write_uint32((unsigned char*) &data, *(uint32_t*) (base));
+    WLOG("Writing option bytes 0x%04x\n", data);
+    stlink_write_debug32(sl, STM32L4_FLASH_OPTR, data);
+
+    /* Set Options Start bit */
+    stlink_read_debug32(sl, STM32L4_FLASH_CR, &val);
+    val |= (1 << STM32L4_FLASH_CR_OPTSTRT);
+    stlink_write_debug32(sl, STM32L4_FLASH_CR, val);
+
+    /* Wait for 'busy' bit in FLASH_SR to clear. */
+    do {
+        stlink_read_debug32(sl, STM32L4_FLASH_SR, &val);
+    } while ((val & (1 << 16)) != 0);
+
+    /* apply options bytes immediate */
+    stlink_read_debug32(sl, STM32L4_FLASH_CR, &val);
+    val |= (1 << STM32L4_FLASH_CR_OBL_LAUNCH);
+    stlink_write_debug32(sl, STM32L4_FLASH_CR, val);
+
+    /* Re-lock option bytes */
+    stlink_read_debug32(sl, STM32L4_FLASH_CR, &val);
+    val |= (1u << STM32L4_FLASH_CR_OPTLOCK);
+    stlink_write_debug32(sl, STM32L4_FLASH_CR, val);
+    /* Re-lock flash. */
+    stlink_read_debug32(sl, STM32L4_FLASH_CR, &val);
+    val |= (1u << STM32L4_FLASH_CR_LOCK);
+    stlink_write_debug32(sl, STM32L4_FLASH_CR, val);
+
+    return 0;
+}
+
+
+/**
+ * Write option bytes
+ * @param sl
  * @param option_byte value to write
  * @return 0 on success, -ve on failure.
  */
@@ -2840,6 +2926,8 @@ int stlink_write_option_bytes(stlink_t *sl, stm32_addr_t addr, uint8_t* base, ui
     // Make sure we've loaded the context with the chip details
     stlink_core_id(sl);
 
+    WLOG("Option bytes write chip_id 0x%08x addr 0x%08x\n",sl->chip_id,addr);
+
     /* Check if chip is supported and for correct address */
     if((sl->chip_id == STLINK_CHIPID_STM32_G0X1) && (addr == STM32_G0_OPTION_BYTES_BASE)) {
         return stlink_write_option_bytes_g0x1(sl, base, len);
@@ -2847,8 +2935,11 @@ int stlink_write_option_bytes(stlink_t *sl, stm32_addr_t addr, uint8_t* base, ui
     else if((sl->chip_id == STLINK_CHIPID_STM32_L0_CAT2) && (addr == STM32_L0_CAT2_OPTION_BYTES_BASE)) {
         return stlink_write_option_bytes_l0_cat2(sl, base, len);
     }
+    else if((sl->chip_id == STLINK_CHIPID_STM32_L496X) && (addr == STM32_L496X_OPTION_BYTES_BASE)) {
+        return stlink_write_option_bytes_l496x(sl, base, len);
+    }
     else {
-        ELOG("Option bytes writing is currently only supported for the STM32F2, STM32G0 and STM32L0\n");
+        ELOG("Option bytes writing is currently only supported for the STM32F2, STM32G0, L496x/L4A6x and STM32L0\n");
         return -1;
     }