arch: arm: cortex_m: implement functionality for ARCH core regs init

Implement the functionality for configuring the
architecture core registers to their warm reset
values upon system initialization. We enable the
support of the feature in the Cortex-M architecture.

Signed-off-by: Ioannis Glaropoulos <Ioannis.Glaropoulos@nordicsemi.no>

arch/arm/core/aarch32/Kconfig

@@ -19,6 +19,7 @@ config CPU_CORTEX_M
 	select SWAP_NONATOMIC
 	select ARCH_HAS_EXTRA_EXCEPTION_INFO
 	select ARCH_HAS_TIMING_FUNCTIONS if CPU_CORTEX_M_HAS_DWT
+	select ARCH_SUPPORTS_ARCH_HW_INIT
 	imply XIP
 	help
 	  This option signifies the use of a CPU of the Cortex-M family.

arch/arm/core/aarch32/cortex_m/mpu/arm_mpu.c

@@ -324,8 +324,13 @@ static int arm_mpu_init(const struct device *arg)
 	arm_core_mpu_disable();
 
 #if defined(CONFIG_NOCACHE_MEMORY)
+	/* Clean and invalidate data cache if
+	 * that was not already done at boot
+	 */
+#if !defined(CONFIG_INIT_ARCH_HW_AT_BOOT)
 	SCB_CleanInvalidateDCache();
 #endif
+#endif /* CONFIG_NOCACHE_MEMORY */
 
 	/* Architecture-specific configuration */
 	mpu_init();

arch/arm/core/aarch32/cortex_m/reset.S

@@ -24,6 +24,11 @@ GDATA(z_interrupt_stacks)
 #if defined(CONFIG_PLATFORM_SPECIFIC_INIT)
 GTEXT(z_platform_init)
 #endif
+#if defined(CONFIG_INIT_ARCH_HW_AT_BOOT)
+GTEXT(z_arm_init_arch_hw_at_boot)
+GDATA(z_main_stack)
+#endif
+
 
 /**
  *
@@ -59,10 +64,38 @@ SECTION_SUBSEC_FUNC(TEXT,_reset_section,z_arm_reset)
  */
 SECTION_SUBSEC_FUNC(TEXT,_reset_section,__start)
 
+#if defined(CONFIG_INIT_ARCH_HW_AT_BOOT)
+    /* Reset CONTROL register */
+    movs.n r0, #0
+    msr CONTROL, r0
+    isb
+#endif
+
 #if defined(CONFIG_PLATFORM_SPECIFIC_INIT)
     bl z_platform_init
 #endif
 
+#if defined(CONFIG_INIT_ARCH_HW_AT_BOOT)
+#if defined(CONFIG_CPU_HAS_ARM_MPU)
+    /* Disable MPU */
+    movs.n r0, #0
+    ldr r1, =_SCS_MPU_CTRL
+    str r0, [r1]
+    dsb
+#endif /* CONFIG_CPU_HAS_ARM_MPU */
+#if defined(CONFIG_CPU_CORTEX_M_HAS_SPLIM)
+    /* Clear SPLIM registers */
+    movs.n r0, #0
+	msr MSPLIM, r0
+    msr PSPLIM, r0
+#endif /* CONFIG_CPU_CORTEX_M_HAS_SPLIM */
+    ldr r0, =z_main_stack + CONFIG_MAIN_STACK_SIZE
+    msr msp, r0
+
+    /* Initialize core architecture registers and system blocks */
+    bl z_arm_init_arch_hw_at_boot
+#endif /* CONFIG_INIT_ARCH_HW_AT_BOOT */
+
     /* lock interrupts: will get unlocked when switch to main task */
 #if defined(CONFIG_ARMV6_M_ARMV8_M_BASELINE)
     cpsid i

arch/arm/core/aarch32/cortex_m/scb.c

@@ -18,7 +18,7 @@
 #include <arch/cpu.h>
 #include <sys/util.h>
 #include <arch/arm/aarch32/cortex_m/cmsis.h>
-
+#include <linker/linker-defs.h>
 /**
  *
  * @brief Reset the system
@@ -34,3 +34,75 @@ void __weak sys_arch_reboot(int type)
 
 	NVIC_SystemReset();
 }
+
+#if defined(CONFIG_CPU_HAS_ARM_MPU)
+/**
+ *
+ * @brief Clear all MPU region configuration
+ *
+ * This routine clears all ARM MPU region configuration.
+ *
+ * @return N/A
+ */
+void z_arm_clear_arm_mpu_config(void)
+{
+	int i;
+
+	int num_regions =
+		((MPU->TYPE & MPU_TYPE_DREGION_Msk) >> MPU_TYPE_DREGION_Pos);
+
+	for (i = 0; i < num_regions; i++) {
+		ARM_MPU_ClrRegion(i);
+	}
+}
+#endif /* CONFIG_CPU_HAS_ARM_MPU */
+
+#if defined(CONFIG_INIT_ARCH_HW_AT_BOOT)
+/**
+ *
+ * @brief Reset system control blocks and core registers
+ *
+ * This routine resets Cortex-M system control block
+ * components and core registers.
+ *
+ * @return N/A
+ */
+void z_arm_init_arch_hw_at_boot(void)
+{
+    /* Disable interrupts */
+	__disable_irq();
+
+#if defined(CONFIG_ARMV7_M_ARMV8_M_MAINLINE)
+	__set_FAULTMASK(0);
+#endif
+
+	/* Initialize System Control Block components */
+
+#if defined(CONFIG_CPU_HAS_ARM_MPU)
+	/* Clear MPU region configuration */
+	z_arm_clear_arm_mpu_config();
+#endif /* CONFIG_CPU_HAS_ARM_MPU */
+
+	/* Disable NVIC interrupts */
+	for (uint8_t i = 0; i < ARRAY_SIZE(NVIC->ICER); i++) {
+		NVIC->ICER[i] = 0xFFFFFFFF;
+	}
+	/* Clear pending NVIC interrupts */
+	for (uint8_t i = 0; i < ARRAY_SIZE(NVIC->ICPR); i++) {
+		NVIC->ICPR[i] = 0xFFFFFFFF;
+	}
+
+#if defined(CONFIG_CPU_CORTEX_M7)
+	/* Reset Cache settings */
+	SCB_CleanInvalidateDCache();
+	SCB_DisableDCache();
+	SCB_DisableICache();
+#endif /* CONFIG_CPU_CORTEX_M7 */
+
+	/* Restore Interrupts */
+	__enable_irq();
+
+	__DSB();
+	__ISB();
+}
+#endif /* CONFIG_INIT_ARCH_HW_AT_BOOT */

include/arch/arm/aarch32/cortex_m/cpu.h

@@ -15,6 +15,7 @@
 #define _SCS_ICSR_UNPENDSV (1 << 27)
 #define _SCS_ICSR_RETTOBASE (1 << 11)
 
+#define _SCS_MPU_CTRL (_SCS_BASE_ADDR + 0xd94)
 #endif
 
 #endif