From fdb1c6e893e69d283b4d650de42460926765876d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mario=20H=C3=BCttel?= <mario.huettel@gmx.net>
Date: Mon, 19 Jul 2021 22:29:13 +0200
Subject: [PATCH] Fix memory checking. The stack is now completely copied to
 CCMRAM before testing the main memory.

---
 stm-firmware/boot/startup_stm32f407vx.c | 48 +++++++++++++++++++++----
 1 file changed, 41 insertions(+), 7 deletions(-)

diff --git a/stm-firmware/boot/startup_stm32f407vx.c b/stm-firmware/boot/startup_stm32f407vx.c
index 544daf2..bbaa5bb 100644
--- a/stm-firmware/boot/startup_stm32f407vx.c
+++ b/stm-firmware/boot/startup_stm32f407vx.c
@@ -321,20 +321,54 @@ void __attribute__((noreturn)) Reset_Handler(void) {
 		while (1);
 	}
 
-	/* Move the Stack pointer to CCMRAM
+	/* Move the stack and the stack pointer to CCMRAM
 	 * This allows us to perform a RAM test on the main RAM.
-	 * Note: sp is not required to be inside the clobber list!
 	 */
-	__asm__ __volatile__ ("mov sp, %0\n\t" :: "r"(0x10000000UL + (64U * 1024UL)) :);
+	/* R2 holds the amount of bytes / words on the stack. */
+	__asm__ __volatile__ (
+			"mov r2, sp\n" /* Move stack pointer to register 2 */
+			"sub r2, %[stacktop], r2\n" /* Subtract stackpointer from top of ram => byte usage */
+			"mov r3, sp\n" /* Init r3 with first word address to copy (stack pointer) */
+			"sub r4, %[ccmtop], r2\n" /* Init r4 with first address to copy to! This will be the new stack pointer! */
+			"mov r5, r4\n" /* R5 will be the new stackpointer after we are finished copying */
+			"copyloop:\n"
+			"cmp r3, %[stacktop]\n" /* Check if we still have word to copy. If not => finish */
+			"beq finish\n"
+			"ldr.w r6, [r3, #0]\n" /* Load word from [r3] and store into [r4] */
+			"str.w r6, [r4, #0]\n"
+			"add r3, #4\n" /* Increment pointers */
+			"add r4, #4\n"
+			"b copyloop\n" /* go back to loop head */
+			"finish:\n"
+			"mov sp, r5\n" /* Set the new stack pointer to the beginning of the copied area */
+			:
+			: [stacktop]"r"(&__ld_top_of_stack), [ccmtop]"r"(0x10000000UL + (64U * 1024UL))
+			: "memory", "r2", "r3", "r4", "r5", "r6");
 
 	if (startup_test_perform_system_ram_check()) {
 		while (1);
 	}
 
-	/* Move the stack pointer back. Note: This only works if this function does not use the stack for variables.
-	 * Otherwise everything will be broken.
-	 */
-	__asm__ __volatile__ ("mov sp, %0\n\t" :: "r"(&__ld_top_of_stack) :);
+	/* Move the stack back to system ram */
+	__asm__ __volatile__ (
+			"mov r2, sp\n" /* Move stack pointer to register 2 */
+			"sub r2, %[ccmtop], r2\n" /* Subtract stackpointer from top of ccmram => byte usage */
+			"mov r3, sp\n" /* Init r3 with first word address to copy (stack pointer) */
+			"sub r4, %[stacktop], r2\n" /* Init r4 with first address to copy to! This will be the new stack pointer! */
+			"mov r5, r4\n" /* R5 will be the new stackpointer after we are finished copying */
+			"copyloop_2:\n"
+			"cmp r3, %[ccmtop]\n" /* Check if we still have word to copy. If not => finish */
+			"beq finish_2\n"
+			"ldr.w r6, [r3, #0]\n" /* Load word from [r3] and store into [r4] */
+			"str.w r6, [r4, #0]\n"
+			"add r3, #4\n" /* Increment pointers */
+			"add r4, #4\n"
+			"b copyloop_2\n" /* go back to loop head */
+			"finish_2:\n"
+			"mov sp, r5\n" /* Set the new stack pointer to the beginning of the copied area */
+			:
+			: [stacktop]"r"(&__ld_top_of_stack), [ccmtop]"r"(0x10000000UL + (64U * 1024UL))
+			: "memory", "r2", "r3", "r4", "r5", "r6");
 
 	/**
 	 * RAM tests destroyed our values. So we have to copy them again...