From da13e2db2f2609f6c9a3a3d1f634fe1261a823fe Mon Sep 17 00:00:00 2001
From: Aleksander Kaminski <aleksander.kaminski@phoenix-rtos.com>
Date: Mon, 16 Dec 2024 15:11:18 +0100
Subject: [PATCH] MCXN94x: Allow running Phoenix-RTOS on CPU1

JIRA: RTOS-988
---
 hal/armv8m/mcx/hal.c        |  4 ++++
 hal/armv8m/mcx/n94x/_init.S | 42 ++++++++++++++++++++++++++++++++++++-
 hal/armv8m/mcx/n94x/n94x.c  |  1 -
 3 files changed, 45 insertions(+), 2 deletions(-)

diff --git a/hal/armv8m/mcx/hal.c b/hal/armv8m/mcx/hal.c
index 90e8f87e..2470229a 100644
--- a/hal/armv8m/mcx/hal.c
+++ b/hal/armv8m/mcx/hal.c
@@ -73,7 +73,11 @@ void hal_syspageSet(hal_syspage_t *hs)
 
 const char *hal_cpuInfo(void)
 {
+#ifdef MCX_USE_CPU1
 	return "Cortex-M33 MCXN94x";
+#else
+	return "Micro Cortex-M33 MCXN94x";
+#endif
 }
 
 
diff --git a/hal/armv8m/mcx/n94x/_init.S b/hal/armv8m/mcx/n94x/_init.S
index f135934c..d4edacc3 100644
--- a/hal/armv8m/mcx/n94x/_init.S
+++ b/hal/armv8m/mcx/n94x/_init.S
@@ -47,6 +47,16 @@ _init_vectors:
 .endr
 .size _init_vectors, .-_init_vectors
 
+.org _init_vectors + 0x300
+
+.globl _vtor_temp
+.type _vtor_temp, %object
+_vtor_temp:
+.word _stack
+.word _start_stage2
+.size _vtor_temp, .-_vtor_temp
+
+
 .thumb
 .thumb_func
 
@@ -54,6 +64,36 @@ _init_vectors:
 .type _start, %function
 _start:
 	cpsid if
+#ifdef MCX_USE_CPU1
+	/* Switch to CPU1 (micro M33) */
+	/* Set CPBOOT1 (CPU1 VTOR) */
+	ldr r0, =_vtor_temp
+	ldr r1, =0x40000800
+	str r0, [r1, #4]
+
+	/* Start CPU1 */
+	ldr r0, =0xc0c40008
+	str r0, [r1]
+
+	/* CPU0: Wait for something to do */
+	/* FIXME: might be not possible to
+	 * exit this situation, CPU0 reset might
+	 * cause global reset. How to park it
+	 * in a way, that allows to reuse the CPU? */
+1:	wfi
+	b 1b
+#else
+	b _start_stage2
+#endif
+
+.size _start, .-_start
+.ltorg
+
+
+.globl _start_stage2
+.type _start_stage2, %function
+_start_stage2:
+	cpsid if
 
 	/* Init vector table and stack pointer */
 	ldr r0, =0xe000ed08
@@ -67,7 +107,7 @@ _start:
 	msr msp, r0
 	ldr r8, =_startc
 	bx r8
-.size _start, .-_start
+.size _start_stage2, .-_start_stage2
 .ltorg
 
 
diff --git a/hal/armv8m/mcx/n94x/n94x.c b/hal/armv8m/mcx/n94x/n94x.c
index d0c312c9..3f62eeed 100644
--- a/hal/armv8m/mcx/n94x/n94x.c
+++ b/hal/armv8m/mcx/n94x/n94x.c
@@ -920,7 +920,6 @@ void _mcxn94x_init(void)
 	_mcxn94x_portPinConfig(pctl_pin_p5_1, 0, MCX_PIN_FAST | MCX_PIN_INPUT_BUFFER_ENABLE);
 
 	/* Enable 32 KHz oscillator */
-	/* TODO determine EXTAL_CAP_SEL, XTAL_CAP_SEL and COARSE_AMP_GAIN values */
 	_mcxn94x_clockConfigOsc32khz(ROSC_EXTALCAP_PF / 2, ROSC_CAP_PF / 2, ROSC_AMP_GAIN);
 
 	/* Configure 24 MHz oscillator pins */