Merge commit '2b4000b2b44ca629d18d96a803413c71ab7b8e52' into sam-update

src/Makefile

@@ -48,6 +48,7 @@ SRC =			\
 	morse.c		\
 	msp432.c	\
 	nrf51.c		\
+	nxpke04.c	\
 	platform.c	\
 	sam3x.c		\
 	sam4l.c	\

src/target/cortexm.c

@@ -344,6 +344,7 @@ bool cortexm_probe(ADIv5_AP_t *ap, bool forced)
 	PROBE(kinetis_probe);
 	PROBE(efm32_probe);
 	PROBE(msp432_probe);
+	PROBE(ke04_probe);
 	PROBE(lpc17xx_probe);
 #undef PROBE
 

src/target/kinetis.c

@@ -335,9 +335,11 @@ static int kl_gen_flash_done(struct target_flash *f)
 #define KINETIS_MDM_IDR_KZ03 0x1c0020
 
 static bool kinetis_mdm_cmd_erase_mass(target *t);
+static bool kinetis_mdm_cmd_ke04_mode(target *t);
 
 const struct command_s kinetis_mdm_cmd_list[] = {
 	{"erase_mass", (cmd_handler)kinetis_mdm_cmd_erase_mass, "Erase entire flash memory"},
+	{"ke04_mode", (cmd_handler)kinetis_mdm_cmd_ke04_mode, "Allow erase for KE04"},
 	{NULL, NULL, NULL}
 };
 
@@ -355,7 +357,7 @@ enum target_halt_reason mdm_halt_poll(target *t, target_addr *watch)
 void kinetis_mdm_probe(ADIv5_AP_t *ap)
 {
 	switch(ap->idr) {
-	case KINETIS_MDM_IDR_KZ03:
+	case KINETIS_MDM_IDR_KZ03: /* Also valid for KE04, no way to check! */
 	case KINETIS_MDM_IDR_K22F:
 		break;
 	default:
@@ -392,21 +394,40 @@ void kinetis_mdm_probe(ADIv5_AP_t *ap)
 #define MDM_STATUS_MASS_ERASE_ENABLED (1 << 5)
 
 #define MDM_CONTROL_MASS_ERASE (1 << 0)
+#define MDM_CONTROL_SYS_RESET  (1 << 3)
 
+/* This is needed as a separate command, as there's no way to  *
+ * tell a KE04 from other kinetis in kinetis_mdm_probe()       */
+static bool ke04_mode = false;
+static bool kinetis_mdm_cmd_ke04_mode(target *t)
+{
+	/* Set a flag to ignore part of the status and assert reset */
+	ke04_mode = true;
+	tc_printf(t, "Mass erase for KE04 now allowed\n");
+	return true;
+}
 static bool kinetis_mdm_cmd_erase_mass(target *t)
 {
 	ADIv5_AP_t *ap = t->priv;
 
+	/* Keep the MCU in reset as stated in KL25PxxM48SF0RM */
+	if(ke04_mode)
+		adiv5_ap_write(ap, MDM_CONTROL, MDM_CONTROL_SYS_RESET);
+
 	uint32_t status, control;
 	status = adiv5_ap_read(ap, MDM_STATUS);
 	control = adiv5_ap_read(ap, MDM_CONTROL);
 	tc_printf(t, "Requesting mass erase (status = 0x%"PRIx32")\n", status);
 
-	if (!(status & MDM_STATUS_MASS_ERASE_ENABLED)) {
+	/* This flag does not exist on KE04 */
+	if (!(status & MDM_STATUS_MASS_ERASE_ENABLED) && !ke04_mode) {
 		tc_printf(t, "ERROR: Mass erase disabled!\n");
 		return false;
 	}
 
+	/* Flag is not persistent */
+	ke04_mode = false;
+
 	if (!(status & MDM_STATUS_FLASH_READY)) {
 		tc_printf(t, "ERROR: Flash not ready!\n");
 		return false;

src/target/nxpke04.c

@@ -0,0 +1,375 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2015  Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ *
+ * Copyright (C) 2018  newbrain
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* This file implements KE04 target specific functions providing
+ * the XML memory map and Flash memory programming.
+ *
+ * An additional command to manually erase a single sector is also provided
+ *
+ * While very similar to other Kinetis, the differences in the Flash Module
+ * registers and the security byte warrant a separate set of routines.
+ *
+ * According to Freescale document MKE04P24M48SF0RM and MKE04P80M48SF0RM:
+ *    KE04 Sub-Family Reference Manual
+ *
+ * And document MKE04P24M48SF0 and MKE04P80M48SF0:
+ *    KE04 Sub-Family Data Sheet
+ */
+
+#include "general.h"
+#include "target.h"
+#include "target_internal.h"
+
+/* KE04 registers and constants */
+
+/* Memory base addresses */
+#define RAM_BASE_ADDR   0x20000000u
+#define FLASH_BASE_ADDR 0x00000000u
+
+/* ID register and related constants */
+#define SIM_SRSID         0x40048000u
+#define SRSID_KE04_MASK   0xFF00u
+#define SRSID_KE04_FAMILY 0x0400u
+#define SRSID_PIN_MASK    0x000Fu
+#define SRSID_PIN__8      0x0000u
+#define SRSID_PIN_16      0x0001u
+#define SRSID_PIN_20      0x0002u
+#define SRSID_PIN_24      0x0003u
+#define SRSID_PIN_32      0x0004u
+#define SRSID_PIN_44      0x0005u
+#define SRSID_PIN_48      0x0006u
+#define SRSID_PIN_64      0x0007u
+#define SRSID_PIN_80      0x0008u
+#define SRSID_PIN100      0x000Au
+
+/* Flash Memory Module registers */
+#define FTMRE_BASE  0x40020000u
+#define FTMRE_FCCOBIX (FTMRE_BASE + 0x01u)
+#define FTMRE_FSEC    (FTMRE_BASE + 0x02u)
+#define FTMRE_FCLKDIV (FTMRE_BASE + 0x03u)
+#define FTMRE_FSTAT   (FTMRE_BASE + 0x05u)
+#define FTMRE_FCNFG   (FTMRE_BASE + 0x07u)
+#define FTMRE_FCCOB   (FTMRE_BASE + 0x08u)
+#define FTMRE_FCCOBLO (FTMRE_BASE + 0x08u)
+#define FTMRE_FCCOBHI (FTMRE_BASE + 0x09u)
+#define FTMRE_FPROT   (FTMRE_BASE + 0x0Bu)
+#define FTMRE_FOPT    (FTMRE_BASE + 0x0Fu)
+
+/* FTMRE_FSTAT flags */
+#define FTMRE_FSTAT_CCIF    0x80u
+#define FTMRE_FSTAT_ACCERR  0x20u
+#define FTMRE_FSTAT_FPVIOL  0x10u
+#define FTMRE_FSTAT_MGBUSY  0x08u
+#define FTMRE_FSTAT_MGSTAT1 0x02u
+#define FTMRE_FSTAT_MGSTAT0 0x01u
+
+/* Flash Memory Module commands */
+#define CMD_PROGRAM_FLASH_32           0x00u /* Special placeholder */
+#define CMD_ERASE_VERIFY_ALL_BLOCKS    0x01u /* Unused */
+#define CMD_ERASE_VERIFY_BLOCK         0x02u /* Unused */
+#define CMD_ERASE_VERIFY_SECTION       0x03u /* Unused */
+#define CMD_READ_ONCE                  0x04u /* Unused */
+#define CMD_PROGRAM_FLASH              0x06u /* Used   */
+#define CMD_PROGRAM_ONCE               0x07u /* Unused */
+#define CMD_ERASE_ALL_BLOCKS           0x08u /* Unused */
+#define CMD_ERASE_FLASH_BLOCK          0x09u /* Unused */
+#define CMD_ERASE_FLASH_SECTOR         0x0Au /* Used   */
+#define CMD_UNSECURE_FLASH             0x0Bu /* Used   */
+#define CMD_VERIFY_BACKDOOR_ACCESS_KEY 0x0Cu /* Unused */
+#define CMD_SET_USER_MARGIN_LEVEL      0x0Du /* Unused */
+#define CMD_SET_FACTORY_MARGIN_LEVEL   0x0Eu /* Unused */
+
+/* Flash Memory Module write and erase sizes */
+#define KE04_WRITE_LEN   8
+#define KE04_SECTOR_SIZE 0x200u
+
+/* Security byte */
+#define FLASH_SECURITY_BYTE_ADDRESS   0x0000040Eu
+#define FLASH_SECURITY_BYTE_UNSECURED 0xFEu
+#define FLASH_SECURITY_WORD_ADDRESS   0x0000040Cu
+#define FLASH_SECURITY_WORD_UNSECURED 0xFFFEFFFFu
+
+
+/* Length in 16bit words of flash commands */
+static const uint8_t cmdLen[] = {
+    4, 1, 2, 3, 6, 0, 6, 6, 1, 2, 2, 1, 5, 3, 3};
+
+/* Flash routines */
+static bool ke04_command(target *t, uint8_t cmd, uint32_t addr, const uint8_t data[8]);
+static int ke04_flash_erase(struct target_flash *f, target_addr addr, size_t len);
+static int ke04_flash_write(struct target_flash *f,
+			    target_addr dest, const void *src, size_t len);
+
+static int ke04_flash_done(struct target_flash *f);
+
+/* Target specific commands */
+static bool kinetis_cmd_unsafe(target *t, int argc, char *argv[]);
+static bool ke04_cmd_sector_erase(target *t, int argc, char *argv[]);
+static bool ke04_cmd_mass_erase(target *t, int argc, char *argv[]);
+static bool unsafe_enabled;
+
+const struct command_s ke_cmd_list[] = {
+	{"unsafe", (cmd_handler)kinetis_cmd_unsafe, "Allow programming security byte (enable|disable)"},
+	{"sector_erase", (cmd_handler)ke04_cmd_sector_erase, "Erase sector containing given address"},
+	{"mass_erase", (cmd_handler)ke04_cmd_mass_erase, "Erase the whole flash"},
+	{NULL, NULL, NULL}
+};
+
+static bool ke04_cmd_sector_erase(target *t, int argc, char *argv[])
+{
+	if (argc < 2)
+		tc_printf(t, "usage: monitor sector_erase <addr>\n");
+
+	char *eos;
+	struct target_flash *f = t->flash;
+	uint32_t addr = strtoul(argv[1], &eos, 0);
+
+	/* check that addr is a valid number and inside the flash range */
+	if ((*eos != 0) || (addr < f->start) || (addr >= f->start+f->length)) {
+		/* Address not in range */
+		tc_printf(t, "Invalid sector address\n");
+		return false;
+	}
+
+	/* Erase and verify the given sector */
+	ke04_command(t, CMD_ERASE_FLASH_SECTOR, addr, NULL);
+	/* Adjust security byte if needed */
+	ke04_flash_done(f);
+	return true;
+}
+
+static bool ke04_cmd_mass_erase(target *t, int argc, char *argv[])
+{
+	(void)argc;
+	(void)argv;
+	/* Erase and verify the whole flash */
+	ke04_command(t, CMD_ERASE_ALL_BLOCKS, 0, NULL);
+	/* Adjust security byte if needed */
+	ke04_flash_done(t->flash);
+	return true;
+}
+
+static bool kinetis_cmd_unsafe(target *t, int argc, char *argv[])
+{
+	if (argc == 1)
+		tc_printf(t, "Allow programming security byte: %s\n",
+			  unsafe_enabled ? "enabled" : "disabled");
+	else
+		unsafe_enabled = argv[1][0] == 'e';
+	return true;
+}
+
+bool ke04_probe(target *t)
+{
+	uint32_t ramsize;
+	uint32_t flashsize;
+	volatile uint32_t dummy;
+
+	/* Read the higher 16bits of System Reset Status and ID Register */
+	uint32_t srsid = target_mem_read32(t, SIM_SRSID) >> 16;
+
+	/* Is this a Kinetis KE04 family MCU? */
+	if ((srsid & SRSID_KE04_MASK) != SRSID_KE04_FAMILY)
+		return false;
+
+	/* KE04Z8 only comes in 16, 20, and 24 pins */
+	switch (srsid & SRSID_PIN_MASK) {
+	case SRSID_PIN_16:
+	case SRSID_PIN_20:
+	case SRSID_PIN_24:
+		/* We have a KE04Z8 */
+		t->driver = "Kinetis KE04Z8Vxxxx";
+		flashsize = 0x2000u; /* 8 kilobytes */
+		ramsize   = 0x400u;  /* 1 kilobyte  */
+		break;
+
+	/* KE04Z64 and KE04Z128 only come in 44, 64, and 80 pins */
+	case SRSID_PIN_44:
+	case SRSID_PIN_64:
+	case SRSID_PIN_80:
+		/* We have either a KE04Z64 or 128 */
+		/* Try to read a flash address not available in a Z64 */
+		dummy = target_mem_read32(t, 0x00010000u);
+		(void)dummy;
+		if (target_check_error(t)) {
+			/* Read failed: we have a 64 */
+			t->driver = "Kinetis KE04Z64Vxxxx";
+			flashsize = 0x10000u; /* 64 kilobytes */
+			ramsize   = 0x2000u;  /*  8 kilobytes */
+		} else {
+			/* Read succeeded: we have a 128 */
+			t->driver = "Kinetis KE04Z128Vxxxx";
+			flashsize = 0x20000u; /* 128 kilobytes */
+			ramsize   = 0x4000u;  /*  16 kilobytes */
+		}
+		break;
+
+	/* Unknown number of pins, not a supported KE04 */
+	default:
+		return false;
+	}
+
+	/* Add low (1/4) and high (3/4) RAM */
+	ramsize /= 4;                       /* Amount before RAM_BASE_ADDR */
+	target_add_ram(t, RAM_BASE_ADDR - ramsize, ramsize); /* Lower RAM  */
+	ramsize *= 3;                       /* Amount after RAM_BASE_ADDR  */
+	target_add_ram(t, RAM_BASE_ADDR, ramsize);           /* Higher RAM */
+
+	/* Add flash, all KE04 have same write and erase size */
+	struct target_flash *f = calloc(1, sizeof(*f));
+	f->start     = FLASH_BASE_ADDR;
+	f->length    = flashsize;
+	f->blocksize = KE04_SECTOR_SIZE;
+	f->erase     = ke04_flash_erase;
+	f->write     = ke04_flash_write;
+	f->done      = ke04_flash_done;
+	f->erased    = 0xFFu;
+	target_add_flash(t, f);
+
+	/* Add target specific commands */
+	unsafe_enabled = false;
+	target_add_commands(t, ke_cmd_list, t->driver);
+
+	return true;
+}
+
+static bool
+ke04_command(target *t, uint8_t cmd, uint32_t addr, const uint8_t data[8])
+{
+	uint8_t fstat;
+
+	/* Set FCLKDIV to 0x17 for 24MHz (default at reset) */
+	uint8_t fclkdiv = target_mem_read8(t, FTMRE_FCLKDIV);
+	if( (fclkdiv & 0x1Fu) != 0x17u ) {
+		/* Wait for CCIF to be high */
+		do {
+			fstat = target_mem_read8(t, FTMRE_FSTAT);
+		} while (!(fstat & FTMRE_FSTAT_CCIF));
+		/* Write correct value */
+		target_mem_write8(t, FTMRE_FCLKDIV, 0x17u);
+	}
+
+	/* clear errors unconditionally, so we can start a new operation */
+	target_mem_write8(t,FTMRE_FSTAT,(FTMRE_FSTAT_ACCERR | FTMRE_FSTAT_FPVIOL));
+
+	/* Wait for CCIF to be high */
+	do {
+		fstat = target_mem_read8(t, FTMRE_FSTAT);
+	} while (!(fstat & FTMRE_FSTAT_CCIF));
+
+	/* Write the flash command and the needed parameters */
+	uint8_t fccobix = 0;
+	/* Trim address, probably not needed */
+	addr &= 0x00FFFFFF;
+	uint8_t cmdL = cmdLen[cmd];
+	/* Special case: single 32bits word flashing */
+	if(cmd == CMD_PROGRAM_FLASH_32)
+		cmd = CMD_PROGRAM_FLASH;
+	uint16_t cmdV = (cmd << 8) | (addr >> 16);
+	/* Write command to FCCOB array */
+	target_mem_write8(t, FTMRE_FCCOBIX, fccobix++);
+	target_mem_write16(t, FTMRE_FCCOB, cmdV);
+
+	/* Write first argument (low partof address) */
+	if (cmdL >= 1) {
+		target_mem_write8(t, FTMRE_FCCOBIX, fccobix++);
+		target_mem_write16(t, FTMRE_FCCOB, addr & 0xFFFFu);
+	}
+
+	/* Write one or two 32 bit words of data */
+	uint8_t dataix = 0;
+	for ( ; fccobix < cmdL; fccobix++) {
+		target_mem_write8(t, FTMRE_FCCOBIX, fccobix);
+		target_mem_write16(t, FTMRE_FCCOB, *(uint16_t *)&data[dataix]);
+		dataix += 2;
+	}
+
+	/* Enable execution by clearing CCIF */
+	target_mem_write8(t, FTMRE_FSTAT, FTMRE_FSTAT_CCIF);
+
+	/* Wait for execution to complete */
+	do {
+		fstat = target_mem_read8(t, FTMRE_FSTAT);
+		/* Check ACCERR and FPVIOL are zero in FSTAT */
+		if (fstat & (FTMRE_FSTAT_ACCERR | FTMRE_FSTAT_FPVIOL)) {
+			return false;
+		}
+	} while (!(fstat & FTMRE_FSTAT_CCIF));
+
+	return true;
+}
+
+static int ke04_flash_erase(struct target_flash *f, target_addr addr, size_t len)
+{
+	while (len) {
+		if (ke04_command(f->t, CMD_ERASE_FLASH_SECTOR, addr, NULL)) {
+			/* Different targets have different flash erase sizes */
+			len  -= f->blocksize;
+			addr += f->blocksize;
+		} else {
+			return 1;
+		}
+	}
+	return 0;
+}
+
+static int ke04_flash_write(struct target_flash *f,
+                              target_addr dest, const void *src, size_t len)
+{
+	/* Ensure we don't write something horrible over the security byte */
+	if (!unsafe_enabled &&
+	    (dest <= FLASH_SECURITY_BYTE_ADDRESS) &&
+	    ((dest + len) > FLASH_SECURITY_BYTE_ADDRESS)) {
+		((uint8_t*)src)[FLASH_SECURITY_BYTE_ADDRESS - dest] =
+		    FLASH_SECURITY_BYTE_UNSECURED;
+	}
+
+	while (len) {
+		if (ke04_command(f->t, CMD_PROGRAM_FLASH, dest, src)) {
+			len  -= KE04_WRITE_LEN;
+			dest += KE04_WRITE_LEN;
+			src  += KE04_WRITE_LEN;
+		} else {
+			return 1;
+		}
+	}
+	return 0;
+}
+
+static int ke04_flash_done(struct target_flash *f)
+{
+	if (unsafe_enabled)
+		return 0;
+
+	if (target_mem_read8(f->t, FLASH_SECURITY_BYTE_ADDRESS) ==
+	    FLASH_SECURITY_BYTE_UNSECURED)
+		return 0;
+
+	/* Load the security byte from its field */
+	/* Note: Cumulative programming is not allowed according to the RM */
+	uint32_t val = target_mem_read32(f->t, FLASH_SECURITY_WORD_ADDRESS);
+	val = (val & 0xff00ffff) | (FLASH_SECURITY_BYTE_UNSECURED << 16);
+	ke04_command(f->t, CMD_PROGRAM_FLASH_32,
+			 FLASH_SECURITY_WORD_ADDRESS, (uint8_t *)&val);
+
+	return 0;
+}

src/target/stm32f1.c

@@ -188,7 +188,6 @@ static int stm32f1_flash_erase(struct target_flash *f,
                                target_addr addr, size_t len)
 {
 	target *t = f->t;
-	uint16_t sr;
 
 	stm32f1_flash_unlock(t);
 
@@ -212,7 +211,7 @@ static int stm32f1_flash_erase(struct target_flash *f,
 	}
 
 	/* Check for error */
-	sr = target_mem_read32(t, FLASH_SR);
+	uint32_t sr = target_mem_read32(t, FLASH_SR);
 	if ((sr & SR_ERROR_MASK) || !(sr & SR_EOP)) {
 		DEBUG("stm32f1 flash erase error 0x%" PRIx32 "\n", sr);
 		return -1;

src/target/stm32f4.c

@@ -339,12 +339,17 @@ static bool stm32f4_attach(target *t)
 			remains = banksize - 0x20000; /* 128 k in small sectors.*/
 		if (is_f7) {
 			stm32f4_add_flash(t, ITCM_BASE, 0x10000,  0x4000,  0, split);
+			if (banksize > 0x10000) {
+				/* STM32F730 has only 64 kiB flash! */
 				stm32f4_add_flash(t, 0x0210000, 0x10000, 0x10000,  4, split);
 				stm32f4_add_flash(t, 0x0220000, remains, 0x20000,  5, split);
 			}
+		}
 		stm32f4_add_flash(t, 0x8000000, 0x10000,  0x4000,  0, split);
+		if (banksize > 0x10000) {
 			stm32f4_add_flash(t, 0x8010000, 0x10000, 0x10000,  4, split);
 			stm32f4_add_flash(t, 0x8020000, remains, 0x20000,  5, split);
+		}
 		if (use_dual_bank) {
 			if (is_f7) {
 				uint32_t bk1 = ITCM_BASE + banksize;

src/target/stm32h7.c

@@ -187,6 +187,14 @@ static bool stm32h7_attach(target *t)
 	uint32_t optsr = target_mem_read32(t, FPEC1_BASE + FLASH_OPTSR);
 	if (!(optsr & FLASH_OPTSR_IWDG1_SW))
 		tc_printf(t, "Hardware IWDG running. Expect failure. Set IWDG1_SW!");
+	uint32_t flashsize = target_mem_read32(t,  FLASH_SIZE_REG);
+	flashsize &= 0xffff;
+	if (flashsize == 128) { /* H750 has only 128 kByte!*/
+		stm32h7_add_flash(t, 0x8000000, FLASH_SECTOR_SIZE, FLASH_SECTOR_SIZE);
+	} else {
+		stm32h7_add_flash(t, 0x8000000, 0x100000, FLASH_SECTOR_SIZE);
+		stm32h7_add_flash(t, 0x8100000, 0x100000, FLASH_SECTOR_SIZE);
+	}
 	return true;
 }
 
@@ -213,8 +221,6 @@ bool stm32h7_probe(target *t)
 		target_add_ram(t, 0x32000000, 0x20000); /* AHB SRAM2, 128 k */
 		target_add_ram(t, 0x34000000, 0x08000); /* AHB SRAM3,  32 k */
 		target_add_ram(t, 0x38000000, 0x01000); /* AHB SRAM4,  32 k */
-		stm32h7_add_flash(t, 0x8000000, 0x100000, FLASH_SECTOR_SIZE);
-		stm32h7_add_flash(t, 0x8100000, 0x100000, FLASH_SECTOR_SIZE);
 		return true;
 	}
 	return false;

src/target/stm32l4.c

@@ -108,6 +108,11 @@ static int stm32l4_flash_write(struct target_flash *f,
 #define OR_DB1M 		(1 << 21)
 #define OR_DBANK 		(1 << 22)
 
+#define DBGMCU_CR(dbgmcureg)	(dbgmcureg + 0x04)
+#define DBGMCU_CR_DBG_SLEEP		(0x1U << 0U)
+#define DBGMCU_CR_DBG_STOP		(0x1U << 1U)
+#define DBGMCU_CR_DBG_STANDBY	(0x1U << 2U)
+
 enum {
         STM32G0_DBGMCU_IDCODE_PHYS = 0x40015800,
         STM32L4_DBGMCU_IDCODE_PHYS = 0xe0042000,
@@ -119,6 +124,108 @@ struct stm32l4_flash {
 	uint32_t bank1_start;
 };
 
+enum ID_STM32L4 {
+	ID_STM32L41  = 0x464u, /* RM0394, Rev.4 */
+	ID_STM32L43  = 0x435u, /* RM0394, Rev.4 */
+	ID_STM32L45  = 0x462u, /* RM0394, Rev.4 */
+	ID_STM32L47  = 0x415u, /* RM0351, Rev.5 */
+	ID_STM32L49  = 0x461u, /* RM0351, Rev.5 */
+	ID_STM32L4R  = 0x470u, /* RM0432, Rev.5 */
+	ID_STM32G07  = 0x460u, /* RM0444/454, Rev.1 */
+};
+
+enum FAM_STM32L4 {
+	FAM_STM32L4xx = 1,
+	FAM_STM32L4Rx = 2,
+	FAM_STM32G0x = 3,
+	FAM_STM32WBxx = 4,
+};
+
+#define DUAL_BANK	0x80u
+#define RAM_COUNT_MSK	0x07u
+
+struct stm32l4_info {
+	char designator[10];
+	uint16_t sram1;
+	uint16_t sram2;
+	uint16_t sram3;
+	enum ID_STM32L4 idcode;
+	enum FAM_STM32L4 family;
+	uint8_t flags;
+};
+
+struct stm32l4_info const L4info[] = {
+	{
+		.idcode = ID_STM32L41,
+		.family = FAM_STM32L4xx,
+		.designator = "STM32L41x",
+		.sram1 = 32,
+		.sram2 = 8,
+		.flags = 2,
+	},
+	{
+		.idcode = ID_STM32L43,
+		.family = FAM_STM32L4xx,
+		.designator = "STM32L43x",
+		.sram1 = 48,
+		.sram2 = 16,
+		.flags = 2,
+	},
+	{
+		.idcode = ID_STM32L45,
+		.family = FAM_STM32L4xx,
+		.designator = "STM32L45x",
+		.sram1 = 128,
+		.sram2 = 32,
+		.flags = 2,
+	},
+	{
+		.idcode = ID_STM32L47,
+		.family = FAM_STM32L4xx,
+		.designator = "STM32L47x",
+		.sram1 = 96,
+		.sram2 = 32,
+		.flags = 2 | DUAL_BANK,
+	},
+	{
+		.idcode = ID_STM32L49,
+		.family = FAM_STM32L4xx,
+		.designator = "STM32L49x",
+		.sram1 = 256,
+		.sram2 = 64,
+		.flags = 2 | DUAL_BANK,
+	},
+	{
+		.idcode = ID_STM32L4R,
+		.family = FAM_STM32L4Rx,
+		.designator = "STM32L4Rx",
+		.sram1 = 192,
+		.sram2 = 64,
+		.sram3 = 384,
+		.flags = 3 | DUAL_BANK,
+	},
+	{
+		.idcode = ID_STM32G07,
+		.family = FAM_STM32G0x,
+		.designator = "STM32G07",
+		.sram1 = 36,
+		.flags = 1,
+	},
+	{
+		/* Terminator */
+		.idcode = 0,
+	},
+};
+
+
+/* Retrieve chip basic information, just add to the vector to extend */
+static struct stm32l4_info const * stm32l4_get_chip_info(uint32_t idcode) {
+	struct stm32l4_info const *p = L4info;
+	while (p->idcode && (p->idcode != idcode))
+		p++;
+	return p;
+}
+
 static void stm32l4_add_flash(target *t,
                               uint32_t addr, size_t length, size_t blocksize,
                               uint32_t bank1_start)
@@ -136,96 +243,54 @@ static void stm32l4_add_flash(target *t,
 	target_add_flash(t, f);
 }
 
-enum ID_STM32L4 {
+static bool stm32l4_attach(target *t)
-	ID_STM32L43  = 0x435, /* RM0394, Rev.4 */
-	ID_STM32L45  = 0x462, /* RM0394, Rev.4 */
-	ID_STM32L41  = 0x464, /* RM0394, Rev.4 */
-	ID_STM32L47  = 0x415, /* RM0351, Rev.5 */
-	ID_STM32G07  = 0x460, /* RM0444/454, Rev.1 */
-	ID_STM32L49  = 0x461, /* RM0351, Rev.5 */
-	ID_STM32L4R  = 0x470, /* RM0432, Rev.5 */
-};
-
-bool stm32l4_probe(target *t)
 {
-	const char* designator = NULL;
+	if (!cortexm_attach(t))
-	bool dual_bank = false;
-	bool is_stm32g0 = false;
-	uint32_t size;
-	uint16_t sram1_size = 0;
-	uint16_t sram2_size = 0;
-	uint16_t sram3_size = 0;
-
-	uint32_t idcode_reg = STM32L4_DBGMCU_IDCODE_PHYS;
-	ADIv5_AP_t *ap = cortexm_ap(t);
-	if (ap->dp->idcode == 0x0BC11477)
-		idcode_reg = STM32G0_DBGMCU_IDCODE_PHYS;
-	uint32_t idcode = target_mem_read32(t, idcode_reg) & 0xfff;
-	switch(idcode) {
-	case ID_STM32G07:
-		designator = "STM32G07";
-		is_stm32g0 = true;
-		sram1_size =  36;
-		break;
-	case ID_STM32L41:
-		designator = "STM32L41x";
-		sram1_size =  32;
-		sram2_size =  8;
-		break;
-	case ID_STM32L43:
-		designator = "STM32L43x";
-		sram1_size =  48;
-		sram2_size =  16;
-		break;
-	case ID_STM32L45:
-		designator = "STM32L45x";
-		sram1_size = 128;
-		sram2_size =  32;
-		break;
-	case ID_STM32L47:
-		designator = "STM32L47x";
-		sram1_size =  96;
-		sram2_size =  32;
-		dual_bank = true;
-		break;
-	case ID_STM32L49:
-		designator = "STM32L49x";
-		sram1_size = 256;
-		sram2_size =  64;
-		dual_bank = true;
-		break;
-	case ID_STM32L4R:
-		designator = "STM32L4Rx";
-		sram1_size = 192;
-		sram2_size =  64;
-		sram3_size = 384;
-		/* 4 k block in dual bank, 8 k in single bank.*/
-		dual_bank = true;
-		break;
-	default:
 		return false;
-	}
+
-	t->driver = designator;
+	/* Retrive chip information, no need to check return */
-	if (is_stm32g0) {
+	struct stm32l4_info const *chip = stm32l4_get_chip_info(t->idcode);
-		target_add_ram(t, 0x20000000, sram1_size << 10);
+
+
+	uint32_t idcodereg = (chip->family == FAM_STM32G0x)
+				     ? STM32G0_DBGMCU_IDCODE_PHYS
+				     : STM32L4_DBGMCU_IDCODE_PHYS;
+
+
+	/* Save DBGMCU_CR to restore it when detaching*/
+	uint32_t dbgmcu_cr = target_mem_read32(t, DBGMCU_CR(idcodereg));
+	t->target_storage = dbgmcu_cr;
+
+	/* Enable debugging during all low power modes*/
+	target_mem_write32(t, DBGMCU_CR(idcodereg), DBGMCU_CR_DBG_SLEEP | DBGMCU_CR_DBG_STANDBY | DBGMCU_CR_DBG_STOP);
+
+
+	/* Free previously loaded memory map */
+	target_mem_map_free(t);
+
+	/* Add RAM to memory map */
+	if (chip->family == FAM_STM32G0x) {
+		target_add_ram(t, 0x20000000, chip->sram1 << 10);
 	} else {
-		target_add_ram(t, 0x10000000, sram2_size << 10);
+		target_add_ram(t, 0x10000000, chip->sram2 << 10);
 		/* All L4 beside L47 alias SRAM2 after SRAM1.*/
-		uint32_t ramsize = (idcode == ID_STM32L47)?
+		uint32_t ramsize = (t->idcode == ID_STM32L47)?
-			sram1_size : (sram1_size + sram2_size + sram3_size);
+			chip->sram1 : (chip->sram1 + chip->sram2 + chip->sram3);
 		target_add_ram(t, 0x20000000, ramsize << 10);
 	}
-	size = (target_mem_read32(t, FLASH_SIZE_REG) & 0xffff);
+
-	if (dual_bank) {
+	/* Add the flash to memory map. */
+	uint32_t size = target_mem_read16(t, FLASH_SIZE_REG);
 	uint32_t options =  target_mem_read32(t, FLASH_OPTR);
-		if (idcode == ID_STM32L4R) {
+
+	if (chip->family == FAM_STM32L4Rx) {
 		/* rm0432 Rev. 2 does not mention 1 MB devices or explain DB1M.*/
 		if (options & OR_DBANK) {
 			stm32l4_add_flash(t, 0x08000000, 0x00100000, 0x1000, 0x08100000);
 			stm32l4_add_flash(t, 0x08100000, 0x00100000, 0x1000, 0x08100000);
 		} else
 			stm32l4_add_flash(t, 0x08000000, 0x00200000, 0x2000, -1);
-		} else {
+	} else if (chip->flags & DUAL_BANK) {
 		if (options & OR_DUALBANK) {
 			uint32_t banksize = size << 9;
 			stm32l4_add_flash(t, 0x08000000           , banksize, 0x0800, 0x08000000 + banksize);
@@ -234,12 +299,43 @@ bool stm32l4_probe(target *t)
 			uint32_t banksize = size << 10;
 			stm32l4_add_flash(t, 0x08000000           , banksize, 0x0800, -1);
 		}
-		}
 	} else
 		stm32l4_add_flash(t, 0x08000000, size << 10, 0x800, -1);
-	target_add_commands(t, stm32l4_cmd_list, designator);
+
 	/* Clear all errors in the status register. */
 	target_mem_write32(t, FLASH_SR, target_mem_read32(t, FLASH_SR));
+
+	return true;
+}
+
+static void stm32l4_detach(target *t)
+{
+	/*reverse all changes to DBGMCU_CR*/
+	uint32_t idcodereg = STM32L4_DBGMCU_IDCODE_PHYS;
+	if (t->idcode == ID_STM32G07)
+		idcodereg = STM32G0_DBGMCU_IDCODE_PHYS;
+	target_mem_write32(t, DBGMCU_CR(idcodereg), t->target_storage);
+	cortexm_detach(t);
+}
+
+bool stm32l4_probe(target *t)
+{
+	uint32_t idcode_reg = STM32L4_DBGMCU_IDCODE_PHYS;
+	ADIv5_AP_t *ap = cortexm_ap(t);
+	if (ap->dp->idcode == 0x0BC11477)
+		idcode_reg = STM32G0_DBGMCU_IDCODE_PHYS;
+	uint32_t idcode = target_mem_read32(t, idcode_reg) & 0xfff;
+
+	struct stm32l4_info const *chip = stm32l4_get_chip_info(idcode);
+
+	if( !chip->idcode )	/* Not found */
+		return false;
+
+	t->idcode = idcode;
+	t->driver = chip->designator;
+	t->attach = stm32l4_attach;
+	t->detach = stm32l4_detach;
+	target_add_commands(t, stm32l4_cmd_list, chip->designator);
 	return true;
 }
 
@@ -370,7 +466,7 @@ static const uint8_t g0_i2offset[7] = {
 static bool stm32l4_option_write(
 	target *t,const uint32_t *values, int len, const uint8_t *i2offset)
 {
-	tc_printf(t, "Device will loose connection. Rescan!\n");
+	tc_printf(t, "Device will lose connection. Rescan!\n");
 	stm32l4_flash_unlock(t);
 	target_mem_write32(t, FLASH_OPTKEYR, OPTKEY1);
 	target_mem_write32(t, FLASH_OPTKEYR, OPTKEY2);
@@ -416,7 +512,7 @@ static bool stm32l4_cmd_option(target *t, int argc, char *argv[])
 	const uint8_t *i2offset = l4_i2offset;
 	if (t->idcode == 0x435) {/* L43x */
 		len = 5;
-	} else if (t->idcode == 0x460) {/* G07x */
+	} else if (t->idcode == ID_STM32G07) {/* G07x */
 		i2offset = g0_i2offset;
 		len = 7;
 	} else {

src/target/target_internal.h

@@ -178,5 +178,5 @@ bool samd_probe(target *t);
 bool kinetis_probe(target *t);
 bool efm32_probe(target *t);
 bool msp432_probe(target *t);
-
+bool ke04_probe(target *t);
 #endif