flash/stm32h7x: support STM32H7R/H7Sx

The STM32H7R/H7Sx has a flash size up to 64 Kb
Change-Id: I2e9d80758d1bc88defdd6bbd1787026373b39fa4
Signed-off-by: HAOUES Ahmed <ahmed.haoues@st.com>
Reviewed-on: https://review.openocd.org/c/openocd/+/8890
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
Tested-by: jenkins

contrib/loaders/flash/stm32/stm32h7rx.S

@@ -0,0 +1,105 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   Copyright (C) 2017 by STMicroelectronics                              *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m4
+	.thumb
+
+/*
+ * Code limitations:
+ * The workarea must have size multiple of 4 bytes, since R/W
+ * operations are all at 32 bits.
+ * The workarea must be big enough to contain rp, wp and data, thus the minimum
+ * workarea size is: min_wa_size = sizeof(rp, wp, data) = 4 + 4 + sizeof(data).
+ *  - for 0x450 devices: sizeof(data) = 32 bytes, thus min_wa_size = 40 bytes.
+ *  - for 0x480 devices: sizeof(data) = 16 bytes, thus min_wa_size = 24 bytes.
+ * To benefit from concurrent host write-to-buffer and target
+ * write-to-flash, the workarea must be way bigger than the minimum.
+ *
+ * To avoid confusions the write word size is got from .block_size member of
+ * struct stm32h7x_part_info defined in stm32h7x.c
+*/
+
+/*
+ * Params :
+ * r0 = workarea start, status (out)
+ * r1 = workarea end
+ * r2 = target address
+ * r3 = count (of write words)
+ * r4 = size of write word
+ * r5 = flash reg base
+ *
+ * Clobbered:
+ * r6 - rp
+ * r7 - wp, status, tmp
+ * r8 - loop index, tmp
+ */
+
+#define STM32_FLASH_CR_OFFSET	0x10	/* offset of CR register in FLASH struct */
+#define STM32_FLASH_SR_OFFSET	0x14	/* offset of SR register in FLASH struct */
+#define STM32_FLASH_ICR_OFFSET	0x28	/* offset of SR register in FLASH struct */
+#define STM32_CR_PROG			0x00000002	/* PG */
+#define STM32_SR_QW_MASK		0x00000004	/* QW */
+#define STM32_SR_ERROR_MASK		0x1F2E0000	/* DBECCERR | SNECCERR | RDSERR | RDPERR | OPERR
+											   | INCERR | STRBERR | PGSERR | WRPERR */
+
+	.thumb_func
+	.global _start
+_start:
+	ldr		r6, [r0, #4]		/* read rp */
+
+wait_fifo:
+	ldr		r7, [r0, #0]		/* read wp */
+	cbz		r7, exit			/* abort if wp == 0, status = 0 */
+	subs	r7, r7, r6			/* number of bytes available for read in r7 */
+	ittt	mi					/* if wrapped around */
+	addmi	r7, r1				/* add size of buffer */
+	submi	r7, r0
+	submi	r7, #8
+	cmp		r7, r4				/* wait until data buffer is full */
+	bcc		wait_fifo
+
+	mov		r7, #STM32_CR_PROG
+	str		r7, [r5, #STM32_FLASH_CR_OFFSET]
+
+	mov		r8, #4
+	udiv	r8, r4, r8			/* number of words is size of write word divided by 4*/
+write_flash:
+	dsb
+	ldr		r7, [r6], #0x04		/* read one word from src, increment ptr */
+	str		r7, [r2], #0x04		/* write one word to dst, increment ptr */
+	dsb
+	cmp		r6, r1				/* if rp >= end of buffer ... */
+	it		cs
+	addcs	r6, r0, #8			/* ... then wrap at buffer start */
+	subs	r8, r8, #1			/* decrement loop index */
+	bne		write_flash			/* loop if not done */
+
+busy:
+	ldr		r7, [r5, #STM32_FLASH_SR_OFFSET]
+	tst		r7, #STM32_SR_QW_MASK
+	bne		busy				/* operation in progress, wait ... */
+
+	ldr		r7, [r5, #STM32_FLASH_ICR_OFFSET]
+	ldr		r8, =STM32_SR_ERROR_MASK
+	tst		r7, r8
+	bne		error				/* fail... */
+
+	str		r6, [r0, #4]		/* store rp */
+	subs	r3, r3, #1			/* decrement count */
+	bne		wait_fifo			/* loop if not done */
+	b		exit
+
+error:
+	movs	r8, #0
+	str		r8, [r0, #4]		/* set rp = 0 on error */
+
+exit:
+	mov		r0, r7				/* return status in r0 */
+	bkpt	#0x00
+
+	.pool

contrib/loaders/flash/stm32/stm32h7rx.inc

@@ -0,0 +1,8 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x46,0x68,0x07,0x68,0x77,0xb3,0xbf,0x1b,0x42,0xbf,0x7f,0x18,0x3f,0x1a,0x08,0x3f,
+0xa7,0x42,0xf6,0xd3,0x4f,0xf0,0x02,0x07,0x2f,0x61,0x4f,0xf0,0x04,0x08,0xb4,0xfb,
+0xf8,0xf8,0xbf,0xf3,0x4f,0x8f,0x56,0xf8,0x04,0x7b,0x42,0xf8,0x04,0x7b,0xbf,0xf3,
+0x4f,0x8f,0x8e,0x42,0x28,0xbf,0x00,0xf1,0x08,0x06,0xb8,0xf1,0x01,0x08,0xf0,0xd1,
+0x6f,0x69,0x17,0xf0,0x04,0x0f,0xfb,0xd1,0xaf,0x6a,0xdf,0xf8,0x1c,0x80,0x17,0xea,
+0x08,0x0f,0x03,0xd1,0x46,0x60,0x01,0x3b,0xd3,0xd1,0x03,0xe0,0x5f,0xf0,0x00,0x08,
+0xc0,0xf8,0x04,0x80,0x38,0x46,0x00,0xbe,0x00,0x00,0x2e,0x1f,

src/flash/nor/stm32h7x.c

@@ -18,14 +18,21 @@
 #define FLASH_WRITE_TIMEOUT 5
 #define MASS_ERASE_TIMEOUT 30000
 
+static const uint8_t stm32h7_flash_write_code[] = {
+#include "../../../contrib/loaders/flash/stm32/stm32h7x.inc"
+};
+
+static const uint8_t stm32h7rs_flash_write_code[] = {
+#include "../../../contrib/loaders/flash/stm32/stm32h7rx.inc"
+};
+
 enum stm32h7_flash_reg_index {
 	STM32_FLASH_ACR_INDEX,
 	STM32_FLASH_KEYR_INDEX,
 	STM32_FLASH_OPTKEYR_INDEX,
 	STM32_FLASH_SR_INDEX,
 	STM32_FLASH_CR_INDEX,
-	STM32_FLASH_ICR_INDEX,
+	STM32_FLASH_ICR_CCR_INDEX,
-	STM32_FLASH_CCR_INDEX,
 	STM32_FLASH_OPTCR_INDEX,
 	STM32_FLASH_OPTSR_INDEX,
 	STM32_FLASH_OPTSR_CUR_INDEX,
@@ -46,7 +53,7 @@ static const uint32_t stm32h7_flash_regs[STM32_FLASH_REG_INDEX_NUM] = {
 	[STM32_FLASH_OPTKEYR_INDEX]		= 0x08,
 	[STM32_FLASH_SR_INDEX]			= 0x10,
 	[STM32_FLASH_CR_INDEX]			= 0x0C,
-	[STM32_FLASH_CCR_INDEX]			= 0x14,
+	[STM32_FLASH_ICR_CCR_INDEX]		= 0x14,
 	[STM32_FLASH_OPTCR_INDEX]		= 0x18,
 	[STM32_FLASH_OPTSR_CUR_INDEX]	= 0x1C,
 	[STM32_FLASH_OPTSR_PRG_INDEX]	= 0x20,
@@ -55,6 +62,18 @@ static const uint32_t stm32h7_flash_regs[STM32_FLASH_REG_INDEX_NUM] = {
 	[STM32_FLASH_WPSN_PRG_INDEX]	= 0x3C
 };
 
+static const uint32_t stm32h7rs_flash_regs[STM32_FLASH_REG_INDEX_NUM] = {
+	[STM32_FLASH_ACR_INDEX]			= 0x00,
+	[STM32_FLASH_KEYR_INDEX]		= 0x04,
+	[STM32_FLASH_OPTKEYR_INDEX]		= 0x100,
+	[STM32_FLASH_SR_INDEX]			= 0x14,
+	[STM32_FLASH_CR_INDEX]			= 0x10,
+	[STM32_FLASH_ICR_CCR_INDEX]		= 0x28,
+	[STM32_FLASH_OPTCR_INDEX]		= 0x104,
+	[STM32_FLASH_OPTSR_INDEX]		= 0x10C,
+	[STM32_FLASH_ISR_INDEX]			= 0x24,
+};
+
 /* FLASH_CR register bits */
 #define FLASH_LOCK     BIT(0)
 #define FLASH_PG       BIT(1)
@@ -67,6 +86,19 @@ static const uint32_t stm32h7_flash_regs[STM32_FLASH_REG_INDEX_NUM] = {
 #define FLASH_FW       BIT(6)
 #define FLASH_START    BIT(7)
 
+/* FLASH_ISR register bits for H7RS */
+#define FLASH_CRCRDERRF	BIT(28) /* CRC read error flag */
+#define FLASH_CRCENDF	BIT(27) /* CRC end flag */
+#define FLASH_DBECCERRF	BIT(26) /* ECC double error flag */
+#define FLASH_SNECCERRF	BIT(25) /* ECC single error flag */
+#define FLASH_RDSERRF	BIT(24) /* Read security error flag */
+#define FLASH_INCERRF	BIT(21) /* Inconsistency error flag */
+#define FLASH_OBLERRF	BIT(20) /* Option byte loading error flag */
+#define FLASH_STRBERRF	BIT(19) /* Strobe error flag */
+#define FLASH_PGSERRF	BIT(18) /* Programming sequence error flag */
+#define FLASH_WRPERRF	BIT(17) /* Write protection error flag */
+#define FLASH_EOPF		BIT(16) /* End-of-program flag */
+
 /* FLASH_SR register bits */
 #define FLASH_BSY      BIT(0)  /* Operation in progress */
 #define FLASH_QW       BIT(2)  /* Operation queue in progress */
@@ -82,6 +114,9 @@ static const uint32_t stm32h7_flash_regs[STM32_FLASH_REG_INDEX_NUM] = {
 
 #define FLASH_ERROR (FLASH_WRPERR | FLASH_PGSERR | FLASH_STRBERR | FLASH_INCERR | FLASH_OPERR | \
 					 FLASH_RDPERR | FLASH_RDSERR | FLASH_SNECCERR | FLASH_DBECCERR)
+/* Possible errors for H7RS */
+#define FLASH_ERROR_H7RS (FLASH_CRCRDERRF | FLASH_CRCENDF | FLASH_DBECCERRF | FLASH_SNECCERRF | FLASH_RDSERRF | \
+					 FLASH_INCERRF | FLASH_OBLERRF | FLASH_STRBERRF | FLASH_PGSERRF | FLASH_WRPERRF | FLASH_EOPF)
 
 /* FLASH_OPTCR register bits */
 #define OPT_LOCK       BIT(0)
@@ -114,6 +149,7 @@ static const uint32_t stm32h7_flash_regs[STM32_FLASH_REG_INDEX_NUM] = {
 #define DEVID_STM32H74_H75XX    0x450
 #define DEVID_STM32H7A_H7BXX    0x480
 #define DEVID_STM32H72_H73XX    0x483
+#define DEVID_STM32H7R_H7SXX	0x485
 
 struct stm32h7_rev {
 	uint16_t rev;
@@ -139,6 +175,9 @@ struct stm32h7_part_info {
 	/* function to compute flash_cr register values */
 	uint32_t (*compute_flash_cr)(uint32_t cmd, int snb);
 	int (*get_flash_error_status)(struct flash_bank *bank, uint32_t *status);
+	uint32_t flash_error;
+	const uint8_t *write_code;
+	size_t write_code_size;
 };
 
 struct stm32h7_flash_bank {
@@ -168,12 +207,16 @@ static const struct stm32h7_rev stm32h72_h73xx_revs[] = {
 	{ 0x1000, "A" }, { 0x1001, "Z" },
 };
 
+static const struct stm32h7_rev stm32h7r_h7sxx_revs[] = {
+	{ 0x1000, "A" }, { 0x2000, "B" },
+};
+
 static uint32_t stm32h74_h75xx_compute_flash_cr(uint32_t cmd, int snb)
 {
 	return cmd | (snb << 8);
 }
 
-static uint32_t stm32h7a_h7bxx_compute_flash_cr(uint32_t cmd, int snb)
+static uint32_t stm32h7a_h7b_h7r_h7sxx_compute_flash_cr(uint32_t cmd, int snb)
 {
 	/* save FW and START bits, to be right shifted by 2 bits later */
 	const uint32_t tmp = cmd & (FLASH_FW | FLASH_START);
@@ -185,6 +228,7 @@ static uint32_t stm32h7a_h7bxx_compute_flash_cr(uint32_t cmd, int snb)
 }
 
 static int stm32h7_get_flash_status(struct flash_bank *bank, uint32_t *status);
+static int stm32h7rs_get_flash_status(struct flash_bank *bank, uint32_t *status);
 
 static const struct stm32h7_part_info stm32h7_parts[] = {
 	{
@@ -202,6 +246,9 @@ static const struct stm32h7_part_info stm32h7_parts[] = {
 	.wps_mask			    = 0xFF,
 	.compute_flash_cr	    = stm32h74_h75xx_compute_flash_cr,
 	.get_flash_error_status = stm32h7_get_flash_status,
+	.flash_error			= FLASH_ERROR,
+	.write_code				= stm32h7_flash_write_code,
+	.write_code_size		= sizeof(stm32h7_flash_write_code),
 	},
 	{
 	.id					    = DEVID_STM32H7A_H7BXX,
@@ -216,8 +263,11 @@ static const struct stm32h7_part_info stm32h7_parts[] = {
 	.fsize_addr			    = 0x08FFF80C,
 	.wps_group_size		    = 4,
 	.wps_mask			    = 0xFFFFFFFF,
-	.compute_flash_cr	    = stm32h7a_h7bxx_compute_flash_cr,
+	.compute_flash_cr	    = stm32h7a_h7b_h7r_h7sxx_compute_flash_cr,
 	.get_flash_error_status = stm32h7_get_flash_status,
+	.flash_error			= FLASH_ERROR,
+	.write_code				= stm32h7_flash_write_code,
+	.write_code_size		= sizeof(stm32h7_flash_write_code),
 	},
 	{
 	.id					    = DEVID_STM32H72_H73XX,
@@ -234,6 +284,28 @@ static const struct stm32h7_part_info stm32h7_parts[] = {
 	.wps_mask			    = 0xFF,
 	.compute_flash_cr       = stm32h74_h75xx_compute_flash_cr,
 	.get_flash_error_status = stm32h7_get_flash_status,
+	.flash_error			= FLASH_ERROR,
+	.write_code				= stm32h7_flash_write_code,
+	.write_code_size		= sizeof(stm32h7_flash_write_code),
+	},
+	{
+	.id						= DEVID_STM32H7R_H7SXX,
+	.revs					= stm32h7r_h7sxx_revs,
+	.num_revs				= ARRAY_SIZE(stm32h7r_h7sxx_revs),
+	.device_str				= "STM32H7Rx/7Sx",
+	.page_size_kb			= 8,
+	.block_size				= 16,
+	.max_flash_size_kb		= 64,
+	.max_bank_size_kb		= 64,
+	.has_dual_bank			= false,
+	.fsize_addr				= 0x08FFF80C,
+	.wps_group_size			= 1,
+	.wps_mask				= 0xFF,
+	.compute_flash_cr		= stm32h7a_h7b_h7r_h7sxx_compute_flash_cr,
+	.get_flash_error_status	= stm32h7rs_get_flash_status,
+	.flash_error			= FLASH_ERROR_H7RS,
+	.write_code				= stm32h7rs_flash_write_code,
+	.write_code_size		= sizeof(stm32h7rs_flash_write_code),
 	},
 };
 
@@ -302,10 +374,16 @@ static int stm32h7_get_flash_status(struct flash_bank *bank, uint32_t *status)
 	return stm32h7_read_flash_reg_by_index(bank, STM32_FLASH_SR_INDEX, status);
 }
 
+static int stm32h7rs_get_flash_status(struct flash_bank *bank, uint32_t *status)
+{
+	return stm32h7_read_flash_reg_by_index(bank, STM32_FLASH_ISR_INDEX, status);
+}
+
 static int stm32h7_wait_flash_op_queue(struct flash_bank *bank, int timeout)
 {
 	uint32_t status;
 	int retval;
+	struct stm32h7_flash_bank *stm32h7_info = bank->driver_priv;
 
 	/* wait for flash operations completion */
 	for (;;) {
@@ -324,16 +402,16 @@ static int stm32h7_wait_flash_op_queue(struct flash_bank *bank, int timeout)
 	}
 
 	if (status & FLASH_WRPERR) {
-		LOG_ERROR("wait_flash_op_queue, WRPERR detected");
+		LOG_ERROR("wait_flash_op_queue, write protection error");
 		retval = ERROR_FAIL;
 	}
 
 	/* Clear error + EOP flags but report errors */
-	if (status & FLASH_ERROR) {
+	if (status & stm32h7_info->part_info->flash_error) {
 		if (retval == ERROR_OK)
 			retval = ERROR_FAIL;
 		/* If this operation fails, we ignore it and report the original retval */
-		stm32h7_write_flash_reg_by_index(bank, STM32_FLASH_CCR_INDEX, status);
+		stm32h7_write_flash_reg_by_index(bank, STM32_FLASH_ICR_CCR_INDEX, status);
 	}
 	return retval;
 }
@@ -611,24 +689,21 @@ static int stm32h7_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	struct armv7m_algorithm armv7m_info;
 	int retval = ERROR_OK;
 
-	static const uint8_t stm32h7_flash_write_code[] = {
+	if (target_alloc_working_area(target, stm32h7_info->part_info->write_code_size,
-#include "../../../contrib/loaders/flash/stm32/stm32h7x.inc"
-	};
-
-	if (target_alloc_working_area(target, sizeof(stm32h7_flash_write_code),
 			&write_algorithm) != ERROR_OK) {
 		LOG_WARNING("no working area available, can't do block memory writes");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
 
 	retval = target_write_buffer(target, write_algorithm->address,
-			sizeof(stm32h7_flash_write_code),
+			stm32h7_info->part_info->write_code_size,
-			stm32h7_flash_write_code);
+			stm32h7_info->part_info->write_code);
 	if (retval != ERROR_OK) {
 		target_free_working_area(target, write_algorithm);
 		return retval;
 	}
 
+
 	/* memory buffer */
 	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 		data_size /= 2;
@@ -680,10 +755,10 @@ static int stm32h7_write_block(struct flash_bank *bank, const uint8_t *buffer,
 		if (flash_sr & FLASH_WRPERR)
 			LOG_ERROR("flash memory write protected");
 
-		if ((flash_sr & FLASH_ERROR) != 0) {
+		if ((flash_sr & stm32h7_info->part_info->flash_error) != 0) {
-			LOG_ERROR("flash write failed, FLASH_SR = 0x%08" PRIx32, flash_sr);
+			LOG_ERROR("flash write failed, status = 0x%08" PRIx32, flash_sr);
 			/* Clear error + EOP flags but report errors */
-			stm32h7_write_flash_reg_by_index(bank, STM32_FLASH_CCR_INDEX, flash_sr);
+			stm32h7_write_flash_reg_by_index(bank, STM32_FLASH_ICR_CCR_INDEX, flash_sr);
 			retval = ERROR_FAIL;
 		}
 	}
@@ -810,8 +885,11 @@ static int stm32h7_probe(struct flash_bank *bank)
 	LOG_DEBUG("device id = 0x%08" PRIx32, stm32h7_info->idcode);
 
 	device_id = stm32h7_info->idcode & 0xfff;
-
+	if (device_id == DEVID_STM32H7R_H7SXX) {
-	stm32h7_info->flash_regs = stm32h7_flash_regs;
+		stm32h7_info->flash_regs = stm32h7rs_flash_regs;
+	} else {
+		stm32h7_info->flash_regs = stm32h7_flash_regs;
+	}
 
 	for (unsigned int n = 0; n < ARRAY_SIZE(stm32h7_parts); n++) {
 		if (device_id == stm32h7_parts[n].id)
@@ -872,6 +950,7 @@ static int stm32h7_probe(struct flash_bank *bank)
 			flash_size_in_kb /= 2;
 		break;
 	case DEVID_STM32H72_H73XX:
+	case DEVID_STM32H7R_H7SXX:
 		break;
 	default:
 		LOG_ERROR("unsupported device");

tcl/target/stm32h7rsx.cfg

@@ -0,0 +1,98 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# script for stm32h7rsx family
+
+#
+# stm32h7rs devices support both JTAG and SWD transports.
+#
+source [find target/swj-dp.tcl]
+source [find mem_helper.tcl]
+
+if { [info exists CHIPNAME] } {
+   set _CHIPNAME $CHIPNAME
+} else {
+   set _CHIPNAME stm32h7rsx
+}
+
+# Issue an error when hla is used
+if { [using_hla] } {
+	echo "Error : hla does not support STM32H7RS devices"
+}
+
+set _ENDIAN little
+
+# Work-area is a space in RAM used for flash programming
+# By default use 64kB
+if { [info exists WORKAREASIZE] } {
+   set _WORKAREASIZE $WORKAREASIZE
+} else {
+   set _WORKAREASIZE 0x10000
+}
+
+#jtag scan chain
+if { [info exists CPUTAPID] } {
+   set _CPUTAPID $CPUTAPID
+} else {
+   if { [using_jtag] } {
+	  set _CPUTAPID 0x6ba00477
+   } {
+      set _CPUTAPID 0x6ba02477
+   }
+}
+
+swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
+
+if {[using_jtag]} {
+   jtag newtap $_CHIPNAME bs -irlen 5
+}
+
+target create $_CHIPNAME.ap0 mem_ap -dap $_CHIPNAME.dap -ap-num 0
+target create $_CHIPNAME.cpu0 cortex_m -endian $_ENDIAN -dap $_CHIPNAME.dap -ap-num 1
+# test ram area
+$_CHIPNAME.cpu0 configure -work-area-phys 0x24000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+flash bank $_CHIPNAME.flash stm32h7x 0x08000000 0 0 0 $_CHIPNAME.cpu0
+
+# Make sure that cpu0 is selected
+targets $_CHIPNAME.cpu0
+
+# Clock after reset is HSI at 64 MHz, no need of PLL
+adapter speed 4000
+
+adapter srst delay 100
+if {[using_jtag]} {
+ jtag_ntrst_delay 100
+}
+
+# use hardware reset
+#
+# The STM32H7RS does not support connect_assert_srst mode because the AXI is
+# unavailable while SRST is asserted, and that is used to access the DBGMCU
+# component at 0x5C001000 in the examine-end event handler.
+#
+reset_config srst_gates_jtag
+
+if {![using_hla]} {
+   # if srst is not fitted use SYSRESETREQ to
+   # perform a soft reset
+	$_CHIPNAME.cpu0 cortex_m reset_config sysresetreq
+
+   # Set CSW[27], which according to ARM ADI v5 appendix E1.4 maps to AHB signal
+   # HPROT[3], which according to AMBA AHB/ASB/APB specification chapter 3.7.3
+   # makes the data access cacheable. This allows reading and writing data in the
+   # CPU cache from the debugger, which is far more useful than going straight to
+   # RAM when operating on typical variables, and is generally no worse when
+   # operating on special memory locations.
+   $_CHIPNAME.dap apcsw 0x08000000 0x08000000
+}
+
+$_CHIPNAME.cpu0 configure -event examine-end {
+	# Enable debug during low power modes (uses more power)
+	# DBGMCU_CR |= DBG_STANDBY | DBG_STOP | DBG_SLEEP
+	mmw 0x5C001004 0x00000007 0
+
+	# Enable clock for tracing
+	# DBGMCU_CR |= TRACECLKEN
+	mmw 0x5C001004 0x00100000 0
+}