flash/stm32l4x: add support of STM32WL5x dual core
[fw/openocd] / src / flash / nor / stm32l4x.c
index db8d5e78d3175d5ecf010b5e06ce7acf37c594c0..8d463ac0593e6f5af11118115524c6fd9fe118c2 100644 (file)
@@ -27,7 +27,7 @@
 #include <helper/align.h>
 #include <helper/binarybuffer.h>
 #include <target/algorithm.h>
-#include <target/armv7m.h>
+#include <target/cortex_m.h>
 #include "bits.h"
 #include "stm32l4x.h"
 
@@ -80,6 +80,9 @@
  *
  * RM0461 (STM32WLEx)
  * http://www.st.com/resource/en/reference_manual/dm00530369.pdf
+ *
+ * RM0453 (STM32WL5x)
+ * http://www.st.com/resource/en/reference_manual/dm00451556.pdf
  */
 
 /* STM32G0xxx series for reference.
 /* Erase time can be as high as 25ms, 10x this and assume it's toast... */
 
 #define FLASH_ERASE_TIMEOUT 250
+#define FLASH_WRITE_TIMEOUT 50
 
 
 /* relevant STM32L4 flags ****************************************************/
@@ -138,6 +142,9 @@ enum stm32l4_flash_reg_index {
        STM32_FLASH_OPTKEYR_INDEX,
        STM32_FLASH_SR_INDEX,
        STM32_FLASH_CR_INDEX,
+       /* for some devices like STM32WL5x, the CPU2 have a dedicated C2CR register w/o LOCKs,
+        * so it uses the C2CR for flash operations and CR for checking locks and locking */
+       STM32_FLASH_CR_WLK_INDEX, /* FLASH_CR_WITH_LOCK */
        STM32_FLASH_OPTR_INDEX,
        STM32_FLASH_WRP1AR_INDEX,
        STM32_FLASH_WRP1BR_INDEX,
@@ -166,6 +173,18 @@ static const uint32_t stm32l4_flash_regs[STM32_FLASH_REG_INDEX_NUM] = {
        [STM32_FLASH_WRP2BR_INDEX]   = 0x050,
 };
 
+static const uint32_t stm32wl_cpu2_flash_regs[STM32_FLASH_REG_INDEX_NUM] = {
+       [STM32_FLASH_ACR_INDEX]      = 0x000,
+       [STM32_FLASH_KEYR_INDEX]     = 0x008,
+       [STM32_FLASH_OPTKEYR_INDEX]  = 0x010,
+       [STM32_FLASH_SR_INDEX]       = 0x060,
+       [STM32_FLASH_CR_INDEX]       = 0x064,
+       [STM32_FLASH_CR_WLK_INDEX]   = 0x014,
+       [STM32_FLASH_OPTR_INDEX]     = 0x020,
+       [STM32_FLASH_WRP1AR_INDEX]   = 0x02C,
+       [STM32_FLASH_WRP1BR_INDEX]   = 0x030,
+};
+
 static const uint32_t stm32l5_ns_flash_regs[STM32_FLASH_REG_INDEX_NUM] = {
        [STM32_FLASH_ACR_INDEX]      = 0x000,
        [STM32_FLASH_KEYR_INDEX]     = 0x008, /* NSKEYR */
@@ -205,7 +224,6 @@ struct stm32l4_part_info {
        const uint16_t max_flash_size_kb;
        const uint32_t flags; /* one bit per feature, see STM32L4 flags: macros F_XXX */
        const uint32_t flash_regs_base;
-       const uint32_t *default_flash_regs;
        const uint32_t fsize_addr;
        const uint32_t otp_base;
        const uint32_t otp_size;
@@ -218,11 +236,14 @@ struct stm32l4_flash_bank {
        bool dual_bank_mode;
        int hole_sectors;
        uint32_t user_bank_size;
+       uint32_t cr_bker_mask;
+       uint32_t sr_bsy_mask;
        uint32_t wrpxxr_mask;
        const struct stm32l4_part_info *part_info;
        uint32_t flash_regs_base;
        const uint32_t *flash_regs;
        bool otp_enabled;
+       bool use_flashloader;
        enum stm32l4_rdp rdp;
        bool tzen;
        uint32_t optr;
@@ -274,6 +295,10 @@ static const struct stm32l4_rev stm32_466_revs[] = {
        { 0x1000, "A" }, { 0x1001, "Z" }, { 0x2000, "B" },
 };
 
+static const struct stm32l4_rev stm32_467_revs[] = {
+       { 0x1000, "A" },
+};
+
 static const struct stm32l4_rev stm32_468_revs[] = {
        { 0x1000, "A" }, { 0x2000, "B" }, { 0x2001, "Z" },
 };
@@ -319,7 +344,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 1024,
          .flags                 = F_HAS_DUAL_BANK,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -332,7 +356,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 256,
          .flags                 = F_NONE,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -345,7 +368,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 128,
          .flags                 = F_NONE,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -358,7 +380,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 1024,
          .flags                 = F_HAS_DUAL_BANK,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -371,7 +392,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 512,
          .flags                 = F_NONE,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -384,7 +404,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 128,
          .flags                 = F_NONE,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -397,7 +416,18 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 64,
          .flags                 = F_NONE,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
+         .fsize_addr            = 0x1FFF75E0,
+         .otp_base              = 0x1FFF7000,
+         .otp_size              = 1024,
+       },
+       {
+         .id                    = 0x467,
+         .revs                  = stm32_467_revs,
+         .num_revs              = ARRAY_SIZE(stm32_467_revs),
+         .device_str            = "STM32G0Bx/G0Cx",
+         .max_flash_size_kb     = 512,
+         .flags                 = F_HAS_DUAL_BANK,
+         .flash_regs_base       = 0x40022000,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -410,7 +440,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 128,
          .flags                 = F_NONE,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -423,7 +452,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 512,
          .flags                 = F_HAS_DUAL_BANK | F_USE_ALL_WRPXX,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -436,7 +464,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 2048,
          .flags                 = F_HAS_DUAL_BANK | F_USE_ALL_WRPXX,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -449,7 +476,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 1024,
          .flags                 = F_HAS_DUAL_BANK | F_USE_ALL_WRPXX,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -462,7 +488,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 512,
          .flags                 = F_HAS_DUAL_BANK | F_USE_ALL_WRPXX | F_HAS_TZ | F_HAS_L5_FLASH_REGS,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l5_ns_flash_regs,
          .fsize_addr            = 0x0BFA05E0,
          .otp_base              = 0x0BFA0000,
          .otp_size              = 512,
@@ -475,7 +500,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 512,
          .flags                 = F_NONE,
          .flash_regs_base       = 0x40022000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -488,7 +512,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 1024,
          .flags                 = F_NONE,
          .flash_regs_base       = 0x58004000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -501,7 +524,6 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .max_flash_size_kb     = 512,
          .flags                 = F_NONE,
          .flash_regs_base       = 0x58004000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -510,11 +532,10 @@ static const struct stm32l4_part_info stm32l4_parts[] = {
          .id                    = 0x497,
          .revs                  = stm32_497_revs,
          .num_revs              = ARRAY_SIZE(stm32_497_revs),
-         .device_str            = "STM32WLEx",
+         .device_str            = "STM32WLEx/WL5x",
          .max_flash_size_kb     = 256,
          .flags                 = F_NONE,
          .flash_regs_base       = 0x58004000,
-         .default_flash_regs    = stm32l4_flash_regs,
          .fsize_addr            = 0x1FFF75E0,
          .otp_base              = 0x1FFF7000,
          .otp_size              = 1024,
@@ -545,6 +566,7 @@ FLASH_BANK_COMMAND_HANDLER(stm32l4_flash_bank_command)
        stm32l4_info->probed = false;
        stm32l4_info->otp_enabled = false;
        stm32l4_info->user_bank_size = bank->size;
+       stm32l4_info->use_flashloader = true;
 
        return ERROR_OK;
 }
@@ -705,6 +727,7 @@ static inline int stm32l4_write_flash_reg_by_index(struct flash_bank *bank,
 
 static int stm32l4_wait_status_busy(struct flash_bank *bank, int timeout)
 {
+       struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
        uint32_t status;
        int retval = ERROR_OK;
 
@@ -714,7 +737,7 @@ static int stm32l4_wait_status_busy(struct flash_bank *bank, int timeout)
                if (retval != ERROR_OK)
                        return retval;
                LOG_DEBUG("status: 0x%" PRIx32 "", status);
-               if ((status & FLASH_BSY) == 0)
+               if ((status & stm32l4_info->sr_bsy_mask) == 0)
                        break;
                if (timeout-- <= 0) {
                        LOG_ERROR("timed out waiting for flash");
@@ -784,14 +807,22 @@ static int stm32l4_set_secbb(struct flash_bank *bank, uint32_t value)
        return ERROR_OK;
 }
 
+static inline int stm32l4_get_flash_cr_with_lock_index(struct flash_bank *bank)
+{
+       struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
+       return (stm32l4_info->flash_regs[STM32_FLASH_CR_WLK_INDEX]) ?
+               STM32_FLASH_CR_WLK_INDEX : STM32_FLASH_CR_INDEX;
+}
+
 static int stm32l4_unlock_reg(struct flash_bank *bank)
 {
+       const uint32_t flash_cr_index = stm32l4_get_flash_cr_with_lock_index(bank);
        uint32_t ctrl;
 
        /* first check if not already unlocked
         * otherwise writing on STM32_FLASH_KEYR will fail
         */
-       int retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, &ctrl);
+       int retval = stm32l4_read_flash_reg_by_index(bank, flash_cr_index, &ctrl);
        if (retval != ERROR_OK)
                return retval;
 
@@ -807,7 +838,7 @@ static int stm32l4_unlock_reg(struct flash_bank *bank)
        if (retval != ERROR_OK)
                return retval;
 
-       retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, &ctrl);
+       retval = stm32l4_read_flash_reg_by_index(bank, flash_cr_index, &ctrl);
        if (retval != ERROR_OK)
                return retval;
 
@@ -821,9 +852,10 @@ static int stm32l4_unlock_reg(struct flash_bank *bank)
 
 static int stm32l4_unlock_option_reg(struct flash_bank *bank)
 {
+       const uint32_t flash_cr_index = stm32l4_get_flash_cr_with_lock_index(bank);
        uint32_t ctrl;
 
-       int retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, &ctrl);
+       int retval = stm32l4_read_flash_reg_by_index(bank, flash_cr_index, &ctrl);
        if (retval != ERROR_OK)
                return retval;
 
@@ -839,7 +871,7 @@ static int stm32l4_unlock_option_reg(struct flash_bank *bank)
        if (retval != ERROR_OK)
                return retval;
 
-       retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, &ctrl);
+       retval = stm32l4_read_flash_reg_by_index(bank, flash_cr_index, &ctrl);
        if (retval != ERROR_OK)
                return retval;
 
@@ -879,7 +911,8 @@ static int stm32l4_perform_obl_launch(struct flash_bank *bank)
        stm32l4_info->probed = false;
 
 err_lock:
-       retval2 = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_LOCK | FLASH_OPTLOCK);
+       retval2 = stm32l4_write_flash_reg_by_index(bank, stm32l4_get_flash_cr_with_lock_index(bank),
+                       FLASH_LOCK | FLASH_OPTLOCK);
 
        if (retval != ERROR_OK)
                return retval;
@@ -925,7 +958,8 @@ static int stm32l4_write_option(struct flash_bank *bank, uint32_t reg_offset,
        retval = stm32l4_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
 
 err_lock:
-       retval2 = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_LOCK | FLASH_OPTLOCK);
+       retval2 = stm32l4_write_flash_reg_by_index(bank, stm32l4_get_flash_cr_with_lock_index(bank),
+                       FLASH_LOCK | FLASH_OPTLOCK);
        stm32l4_info->flash_regs = saved_flash_regs;
 
        if (retval != ERROR_OK)
@@ -1106,7 +1140,7 @@ static int stm32l4_erase(struct flash_bank *bank, unsigned int first,
                if (i >= stm32l4_info->bank1_sectors) {
                        uint8_t snb;
                        snb = i - stm32l4_info->bank1_sectors;
-                       erase_flags |= snb << FLASH_PAGE_SHIFT | FLASH_CR_BKER;
+                       erase_flags |= snb << FLASH_PAGE_SHIFT | stm32l4_info->cr_bker_mask;
                } else
                        erase_flags |= i << FLASH_PAGE_SHIFT;
                retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, erase_flags);
@@ -1119,7 +1153,7 @@ static int stm32l4_erase(struct flash_bank *bank, unsigned int first,
        }
 
 err_lock:
-       retval2 = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_LOCK);
+       retval2 = stm32l4_write_flash_reg_by_index(bank, stm32l4_get_flash_cr_with_lock_index(bank), FLASH_LOCK);
 
        if (stm32l4_info->tzen && (stm32l4_info->rdp == RDP_LEVEL_0)) {
                /* restore all FLASH pages as non-secure */
@@ -1362,6 +1396,49 @@ static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
        return retval;
 }
 
+/* Count is in double-words */
+static int stm32l4_write_block_without_loader(struct flash_bank *bank, const uint8_t *buffer,
+                               uint32_t offset, uint32_t count)
+{
+       struct target *target = bank->target;
+       uint32_t address = bank->base + offset;
+       int retval = ERROR_OK;
+
+       /* wait for BSY bit */
+       retval = stm32l4_wait_status_busy(bank, FLASH_WRITE_TIMEOUT);
+       if (retval != ERROR_OK)
+               return retval;
+
+       /* set PG in FLASH_CR */
+       retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_PG);
+       if (retval != ERROR_OK)
+               return retval;
+
+
+       /* write directly to flash memory */
+       const uint8_t *src = buffer;
+       while (count--) {
+               retval = target_write_memory(target, address, 4, 2, src);
+               if (retval != ERROR_OK)
+                       return retval;
+
+               /* wait for BSY bit */
+               retval = stm32l4_wait_status_busy(bank, FLASH_WRITE_TIMEOUT);
+               if (retval != ERROR_OK)
+                       return retval;
+
+               src += 8;
+               address += 8;
+       }
+
+       /* reset PG in FLASH_CR */
+       retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, 0);
+       if (retval != ERROR_OK)
+               return retval;
+
+       return retval;
+}
+
 static int stm32l4_write(struct flash_bank *bank, const uint8_t *buffer,
        uint32_t offset, uint32_t count)
 {
@@ -1434,10 +1511,36 @@ static int stm32l4_write(struct flash_bank *bank, const uint8_t *buffer,
        if (retval != ERROR_OK)
                goto err_lock;
 
-       retval = stm32l4_write_block(bank, buffer, offset, count / 8);
+       /**
+        * FIXME update the flash loader to use a custom FLASH_SR_BSY mask
+        * Workaround for STM32G0Bx/G0Cx devices in dual bank mode,
+        * as the flash loader does not use the SR_BSY2
+        */
+       bool use_flashloader = stm32l4_info->use_flashloader;
+       if ((stm32l4_info->part_info->id == 0x467) && stm32l4_info->dual_bank_mode) {
+               LOG_INFO("Couldn't use the flash loader in dual-bank mode");
+               use_flashloader = false;
+       }
+
+       if (use_flashloader) {
+               /* For TrustZone enabled devices, when TZEN is set and RDP level is 0.5,
+                * the debug is possible only in non-secure state.
+                * Thus means the flashloader will run in non-secure mode,
+                * and the workarea need to be in non-secure RAM */
+               if (stm32l4_info->tzen && (stm32l4_info->rdp == RDP_LEVEL_0_5))
+                       LOG_INFO("RDP level is 0.5, the work-area should reside in non-secure RAM");
+
+               retval = stm32l4_write_block(bank, buffer, offset, count / 8);
+       }
+
+       if (!use_flashloader || retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
+               LOG_INFO("falling back to single memory accesses");
+               retval = stm32l4_write_block_without_loader(bank, buffer, offset, count / 8);
+       }
+
 
 err_lock:
-       retval2 = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_LOCK);
+       retval2 = stm32l4_write_flash_reg_by_index(bank, stm32l4_get_flash_cr_with_lock_index(bank), FLASH_LOCK);
 
        if (stm32l4_info->tzen && (stm32l4_info->rdp == RDP_LEVEL_0)) {
                /* restore all FLASH pages as non-secure */
@@ -1466,6 +1569,30 @@ static int stm32l4_read_idcode(struct flash_bank *bank, uint32_t *id)
                        return ERROR_OK;
        }
 
+       /* Workaround for STM32WL5x devices:
+        * DBGMCU_IDCODE cannot be read using CPU1 (Cortex-M0+) at AP1,
+        * to solve this read the UID64 (IEEE 64-bit unique device ID register) */
+
+       struct cortex_m_common *cortex_m = target_to_cm(bank->target);
+
+       if (cortex_m->core_info->partno == CORTEX_M0P_PARTNO && cortex_m->armv7m.debug_ap->ap_num == 1) {
+               uint32_t uid64_ids;
+
+               /* UID64 is contains
+                *  - Bits 63:32 : DEVNUM (unique device number, different for each individual device)
+                *  - Bits 31:08 : STID (company ID) = 0x0080E1
+                *  - Bits 07:00 : DEVID (device ID) = 0x15
+                *
+                *  read only the fixed values {STID,DEVID} from UID64_IDS to identify the device as STM32WLx
+                */
+               retval = target_read_u32(bank->target, UID64_IDS, &uid64_ids);
+               if (retval == ERROR_OK && uid64_ids == UID64_IDS_STM32WL) {
+                       /* force the DEV_ID to 0x497 and the REV_ID to unknown */
+                       *id = 0x00000497;
+                       return ERROR_OK;
+               }
+       }
+
        LOG_ERROR("can't get the device id");
        return (retval == ERROR_OK) ? ERROR_FAIL : retval;
 }
@@ -1496,6 +1623,7 @@ static const char *get_stm32l4_bank_type_str(struct flash_bank *bank)
 static int stm32l4_probe(struct flash_bank *bank)
 {
        struct target *target = bank->target;
+       struct armv7m_common *armv7m = target_to_armv7m(target);
        struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
        const struct stm32l4_part_info *part_info;
        uint16_t flash_size_kb = 0xffff;
@@ -1529,7 +1657,14 @@ static int stm32l4_probe(struct flash_bank *bank)
                        stm32l4_info->idcode, part_info->device_str, rev_str, rev_id);
 
        stm32l4_info->flash_regs_base = stm32l4_info->part_info->flash_regs_base;
-       stm32l4_info->flash_regs = stm32l4_info->part_info->default_flash_regs;
+       stm32l4_info->cr_bker_mask = FLASH_BKER;
+       stm32l4_info->sr_bsy_mask = FLASH_BSY;
+
+       /* initialise the flash registers layout */
+       if (part_info->flags & F_HAS_L5_FLASH_REGS)
+               stm32l4_info->flash_regs = stm32l5_ns_flash_regs;
+       else
+               stm32l4_info->flash_regs = stm32l4_flash_regs;
 
        /* read flash option register */
        retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_OPTR_INDEX, &stm32l4_info->optr);
@@ -1538,6 +1673,17 @@ static int stm32l4_probe(struct flash_bank *bank)
 
        stm32l4_sync_rdp_tzen(bank);
 
+       /* for devices with trustzone, use flash secure registers when TZEN=1 and RDP is LEVEL_0 */
+       if (stm32l4_info->tzen && (stm32l4_info->rdp == RDP_LEVEL_0)) {
+               if (part_info->flags & F_HAS_L5_FLASH_REGS) {
+                       stm32l4_info->flash_regs_base |= STM32L5_REGS_SEC_OFFSET;
+                       stm32l4_info->flash_regs = stm32l5_s_flash_regs;
+               } else {
+                       LOG_ERROR("BUG: device supported incomplete");
+                       return ERROR_NOT_IMPLEMENTED;
+               }
+       }
+
        if (part_info->flags & F_HAS_TZ)
                LOG_INFO("TZEN = %d : TrustZone %s by option bytes",
                                stm32l4_info->tzen,
@@ -1630,12 +1776,25 @@ static int stm32l4_probe(struct flash_bank *bank)
        case 0x466: /* STM32G03/G04xx */
        case 0x468: /* STM32G43/G44xx */
        case 0x479: /* STM32G49/G4Axx */
-       case 0x497: /* STM32WLEx */
                /* single bank flash */
                page_size_kb = 2;
                num_pages = flash_size_kb / page_size_kb;
                stm32l4_info->bank1_sectors = num_pages;
                break;
+       case 0x467: /* STM32G0B/G0Cxx */
+               /* single/dual bank depending on bit(21) */
+               page_size_kb = 2;
+               num_pages = flash_size_kb / page_size_kb;
+               stm32l4_info->bank1_sectors = num_pages;
+               stm32l4_info->cr_bker_mask = FLASH_BKER_G0;
+
+               /* check DUAL_BANK bit */
+               if (stm32l4_info->optr & BIT(21)) {
+                       stm32l4_info->sr_bsy_mask = FLASH_BSY | FLASH_BSY2;
+                       stm32l4_info->dual_bank_mode = true;
+                       stm32l4_info->bank1_sectors = num_pages / 2;
+               }
+               break;
        case 0x469: /* STM32G47/G48xx */
                /* STM32G47/8 can be single/dual bank:
                 *   if DUAL_BANK = 0 -> single bank
@@ -1692,15 +1851,6 @@ static int stm32l4_probe(struct flash_bank *bank)
                        num_pages = flash_size_kb / page_size_kb;
                        stm32l4_info->bank1_sectors = num_pages / 2;
                }
-
-               /**
-                * by default use the non-secure registers,
-                * switch secure registers if TZ is enabled and RDP is LEVEL_0
-                */
-               if (stm32l4_info->tzen && (stm32l4_info->rdp == RDP_LEVEL_0)) {
-                       stm32l4_info->flash_regs_base |= 0x10000000;
-                       stm32l4_info->flash_regs = stm32l5_s_flash_regs;
-               }
                break;
        case 0x495: /* STM32WB5x */
        case 0x496: /* STM32WB3x */
@@ -1709,6 +1859,14 @@ static int stm32l4_probe(struct flash_bank *bank)
                num_pages = flash_size_kb / page_size_kb;
                stm32l4_info->bank1_sectors = num_pages;
                break;
+       case 0x497: /* STM32WLEx/WL5x */
+               /* single bank flash */
+               page_size_kb = 2;
+               num_pages = flash_size_kb / page_size_kb;
+               stm32l4_info->bank1_sectors = num_pages;
+               if (armv7m->debug_ap->ap_num == 1)
+                       stm32l4_info->flash_regs = stm32wl_cpu2_flash_regs;
+               break;
        default:
                LOG_ERROR("unsupported device");
                return ERROR_FAIL;
@@ -1856,7 +2014,7 @@ static int stm32l4_mass_erase(struct flash_bank *bank)
        retval = stm32l4_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
 
 err_lock:
-       retval2 = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_LOCK);
+       retval2 = stm32l4_write_flash_reg_by_index(bank, stm32l4_get_flash_cr_with_lock_index(bank), FLASH_LOCK);
 
        if (stm32l4_info->tzen && (stm32l4_info->rdp == RDP_LEVEL_0)) {
                /* restore all FLASH pages as non-secure */
@@ -2010,6 +2168,26 @@ COMMAND_HANDLER(stm32l4_handle_trustzone_command)
        return stm32l4_perform_obl_launch(bank);
 }
 
+COMMAND_HANDLER(stm32l4_handle_flashloader_command)
+{
+       if (CMD_ARGC < 1 || CMD_ARGC > 2)
+               return ERROR_COMMAND_SYNTAX_ERROR;
+
+       struct flash_bank *bank;
+       int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+       if (retval != ERROR_OK)
+               return retval;
+
+       struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
+
+       if (CMD_ARGC == 2)
+               COMMAND_PARSE_ENABLE(CMD_ARGV[1], stm32l4_info->use_flashloader);
+
+       command_print(CMD, "FlashLoader usage is %s", stm32l4_info->use_flashloader ? "enabled" : "disabled");
+
+       return ERROR_OK;
+}
+
 COMMAND_HANDLER(stm32l4_handle_option_load_command)
 {
        if (CMD_ARGC != 1)
@@ -2215,6 +2393,13 @@ static const struct command_registration stm32l4_exec_command_handlers[] = {
                .usage = "bank_id",
                .help = "Unlock entire protected flash device.",
        },
+       {
+               .name = "flashloader",
+               .handler = stm32l4_handle_flashloader_command,
+               .mode = COMMAND_EXEC,
+               .usage = "<bank_id> [enable|disable]",
+               .help = "Configure the flashloader usage",
+       },
        {
                .name = "mass_erase",
                .handler = stm32l4_handle_mass_erase_command,