[PATCH 02/10] bsps/arm: Support recent bootloaders starting kernel in HYP mode
pisa at cmp.felk.cvut.cz
pisa at cmp.felk.cvut.cz
Mon Jul 4 00:07:06 UTC 2016
From: Pavel Pisa <pisa at cmp.felk.cvut.cz>
When HYP mode is detected at startup then setup HYP mode
vectors table (for future extensions) clean exceptions
switching to HYP mode and switch CPU to ARM SVC mode.
BSPs which want to use this support need to include next option
in their configure.ac
RTEMS_BSPOPTS_SET([BSP_START_IN_HYP_SUPPORT],[*],[1])
RTEMS_BSPOPTS_HELP([BSP_START_IN_HYP_SUPPORT], [Support start of BSP in ARM HYP mode])
AM_CONDITIONAL(BSP_START_IN_HYP_SUPPORT,test "$BSP_START_IN_HYP_SUPPORT" = "1")
and need to include next lines in corresponding Makefile.am
if BSP_START_IN_HYP_SUPPORT
libbsp_a_SOURCES += ../shared/startup/bsp-start-in-hyp-support.S
endif
---
.../lib/libbsp/arm/shared/include/linker-symbols.h | 4 ++
c/src/lib/libbsp/arm/shared/include/start.h | 13 +++-
c/src/lib/libbsp/arm/shared/start/start.S | 73 +++++++++++++++++++-
.../arm/shared/startup/bsp-start-in-hyp-support.S | 77 ++++++++++++++++++++++
c/src/lib/libbsp/arm/shared/startup/linkcmds.base | 9 ++-
5 files changed, 170 insertions(+), 6 deletions(-)
create mode 100644 c/src/lib/libbsp/arm/shared/startup/bsp-start-in-hyp-support.S
diff --git a/c/src/lib/libbsp/arm/shared/include/linker-symbols.h b/c/src/lib/libbsp/arm/shared/include/linker-symbols.h
index 7446259..1a996f5 100644
--- a/c/src/lib/libbsp/arm/shared/include/linker-symbols.h
+++ b/c/src/lib/libbsp/arm/shared/include/linker-symbols.h
@@ -59,6 +59,10 @@ LINKER_SYMBOL(bsp_stack_und_begin)
LINKER_SYMBOL(bsp_stack_und_end)
LINKER_SYMBOL(bsp_stack_und_size)
+LINKER_SYMBOL(bsp_stack_hyp_begin)
+LINKER_SYMBOL(bsp_stack_hyp_end)
+LINKER_SYMBOL(bsp_stack_hyp_size)
+
LINKER_SYMBOL(bsp_stack_svc_begin)
LINKER_SYMBOL(bsp_stack_svc_end)
LINKER_SYMBOL(bsp_stack_svc_size)
diff --git a/c/src/lib/libbsp/arm/shared/include/start.h b/c/src/lib/libbsp/arm/shared/include/start.h
index f61eed3..bf8eed4 100644
--- a/c/src/lib/libbsp/arm/shared/include/start.h
+++ b/c/src/lib/libbsp/arm/shared/include/start.h
@@ -45,6 +45,15 @@ extern "C" {
#define BSP_START_DATA_SECTION __attribute__((section(".bsp_start_data")))
+/*
+* Many ARM boot loaders pass arguments to loaded OS kernel
+*/
+#ifdef BSP_START_HOOKS_WITH_LOADER_ARGS
+#define BSP_START_HOOKS_LOADER_ARGS int saved_psr, int saved_machid, int saved_dtb_adr
+#else
+#define BSP_START_HOOKS_LOADER_ARGS void
+#endif
+
/**
* @brief System start entry.
*/
@@ -57,7 +66,7 @@ void _start(void);
* stack pointers are initialized but before the copying of the exception
* vectors.
*/
-void bsp_start_hook_0(void);
+void bsp_start_hook_0(BSP_START_HOOKS_LOADER_ARGS);
/**
* @brief Start entry hook 1.
@@ -65,7 +74,7 @@ void bsp_start_hook_0(void);
* This hook will be called from the start entry code after copying of the
* exception vectors but before the call to boot_card().
*/
-void bsp_start_hook_1(void);
+void bsp_start_hook_1(BSP_START_HOOKS_LOADER_ARGS);
/**
* @brief Similar to standard memcpy().
diff --git a/c/src/lib/libbsp/arm/shared/start/start.S b/c/src/lib/libbsp/arm/shared/start/start.S
index 4050deb..0848fff 100644
--- a/c/src/lib/libbsp/arm/shared/start/start.S
+++ b/c/src/lib/libbsp/arm/shared/start/start.S
@@ -54,6 +54,11 @@
.extern bsp_start_init_registers_vfp
#endif
+#ifdef BSP_START_IN_HYP_SUPPORT
+ .extern bsp_start_arm_drop_hyp_mode
+ .globl bsp_start_hyp_vector_table_begin
+#endif
+
/* Global symbols */
.globl _start
.globl bsp_start_vector_table_begin
@@ -124,16 +129,56 @@ handler_addr_fiq:
bsp_start_vector_table_end:
+#ifdef BSP_START_IN_HYP_SUPPORT
+bsp_start_hyp_vector_table_begin:
+ ldr pc, handler_addr_hyp_reset
+ ldr pc, handler_addr_hyp_undef
+ ldr pc, handler_addr_hyp_swi
+ ldr pc, handler_addr_hyp_prefetch
+ ldr pc, handler_addr_hyp_abort
+ ldr pc, handler_addr_hyp_hyp
+ ldr pc, handler_addr_hyp_irq
+ ldr pc, handler_addr_hyp_fiq
+
+handler_addr_hyp_reset:
+ .word _ARMV4_Exception_reserved_default
+
+handler_addr_hyp_undef:
+ .word _ARMV4_Exception_reserved_default
+
+handler_addr_hyp_swi:
+ .word _ARMV4_Exception_reserved_default
+
+handler_addr_hyp_prefetch:
+ .word _ARMV4_Exception_reserved_default
+
+handler_addr_hyp_abort:
+ .word _ARMV4_Exception_reserved_default
+
+handler_addr_hyp_hyp:
+ .word _ARMV4_Exception_reserved_default
+
+handler_addr_hyp_irq:
+ .word _ARMV4_Exception_reserved_default
+
+handler_addr_hyp_fiq:
+ .word _ARMV4_Exception_reserved_default
+
+bsp_start_hyp_vector_table_end:
+#endif
+
/* Start entry */
_start:
/*
* We do not save the context since we do not return to the boot
- * loader.
+ * loader but preserve r1 and r2 to allow access to bootloader parameters
*/
-
-#ifdef BSP_START_NEEDS_REGISTER_INITIALIZATION
+#ifndef BSP_START_NEEDS_REGISTER_INITIALIZATION
+ mov r5, r1 /* machine type number or ~0 for DT boot */
+ mov r6, r2 /* physical address of ATAGs or DTB */
+#else /* BSP_START_NEEDS_REGISTER_INITIALIZATION */
bl bsp_start_init_registers_core
#endif
@@ -147,6 +192,24 @@ _start:
mul r1, r0
#endif
+ mrs r4, cpsr /* save original procesor status value */
+#ifdef BSP_START_IN_HYP_SUPPORT
+ orr r0, r4, #(ARM_PSR_I | ARM_PSR_F)
+ msr cpsr, r4
+
+ and r0, r4, #ARM_PSR_M_MASK
+ cmp r0, #ARM_PSR_M_HYP
+ bne bsp_start_skip_hyp_svc_switch
+
+ /* Boot loader stats kernel in HYP mode, switch to SVC necessary */
+ ldr sp, =bsp_stack_hyp_end
+#ifdef RTEMS_SMP
+ add sp, r1
+#endif
+ bl bsp_start_arm_drop_hyp_mode
+
+bsp_start_skip_hyp_svc_switch:
+#endif
/*
* Set SVC mode, disable interrupts and enable ARM instructions.
*/
@@ -247,6 +310,10 @@ _start:
SWITCH_FROM_ARM_TO_THUMB r0
+ mov r0, r4 /* original cpsr value */
+ mov r1, r5 /* machine type number or ~0 for DT boot */
+ mov r2, r6 /* physical address of ATAGs or DTB */
+
b bsp_start_hook_0
bsp_start_hook_0_done:
diff --git a/c/src/lib/libbsp/arm/shared/startup/bsp-start-in-hyp-support.S b/c/src/lib/libbsp/arm/shared/startup/bsp-start-in-hyp-support.S
new file mode 100644
index 0000000..b608c59
--- /dev/null
+++ b/c/src/lib/libbsp/arm/shared/startup/bsp-start-in-hyp-support.S
@@ -0,0 +1,77 @@
+/**
+ * @file
+ *
+ * @brief Boot and system start code.
+ */
+
+ /*
+ * Copyright (c) 2016 Pavel Pisa <pisa at cmp.felk.cvut.cz>
+ *
+ * Czech Technical University in Prague
+ * Zikova 1903/4
+ * 166 36 Praha 6
+ * Czech Republic
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#include <rtems/asm.h>
+#include <rtems/system.h>
+#include <rtems/score/cpu.h>
+
+#include <bspopts.h>
+#include <bsp/irq.h>
+#include <bsp/linker-symbols.h>
+
+ .extern bsp_start_hyp_vector_table_begin
+ .globl bsp_start_arm_drop_hyp_mode
+ .globl bsp_arm_drop_hyp_mode_only
+
+.arm
+
+/*
+ * The routine is called from startup code and it should
+ * preserve all registers except r2 and r3. r0 can be used
+ * as pass though argument in some cases, a1 is used for
+ * CPU stack offset during startup and r4 to r6 to preserve
+ * booloader arguments
+ */
+
+bsp_start_arm_drop_hyp_mode:
+ ldr r2, bsp_start_hyp_vector_table_begin_addr
+ mcr p15, 4, r2, c12, c0, 0
+
+ mov r2, #0
+ mcr p15, 4, r2, c1, c1, 0
+ mcr p15, 4, r2, c1, c1, 2
+ mcr p15, 4, r2, c1, c1, 3
+/*
+ * HSCTLR.TE
+ * optional start of hypervisor handlers in Thumb mode
+ * orr r0, #(1 << 30)
+ */
+ mcr p15, 4, r2, c1, c0, 0 /* HSCTLR */
+ mrc p15, 4, r2, c1, c1, 1 /* HDCR */
+ and r2, #0x1f /* Preserve HPMN */
+ mcr p15, 4, r2, c1, c1, 1 /* HDCR */
+
+bsp_arm_drop_hyp_mode_only:
+ /* Prepare SVC mode for eret */
+ mrs r2, cpsr
+ bic r2, r2, #ARM_PSR_M_MASK
+ orr r2, r2, #ARM_PSR_M_SVC
+ msr spsr_cxsf, r2
+
+ adr r2, 1f
+ .inst 0xe12ef302 /* msr ELR_hyp, r2 */
+ mov r2, sp
+ mov r3, lr
+ .inst 0xe160006e /* eret */
+1: mov sp, r2
+ mov lr, r3
+ bx lr
+
+bsp_start_hyp_vector_table_begin_addr:
+ .word bsp_start_hyp_vector_table_begin
diff --git a/c/src/lib/libbsp/arm/shared/startup/linkcmds.base b/c/src/lib/libbsp/arm/shared/startup/linkcmds.base
index 83865fc..d7e60db 100644
--- a/c/src/lib/libbsp/arm/shared/startup/linkcmds.base
+++ b/c/src/lib/libbsp/arm/shared/startup/linkcmds.base
@@ -53,10 +53,13 @@ bsp_stack_svc_size = ALIGN (bsp_stack_svc_size, bsp_stack_align);
bsp_stack_und_size = DEFINED (bsp_stack_und_size) ? bsp_stack_und_size : 0;
bsp_stack_und_size = ALIGN (bsp_stack_und_size, bsp_stack_align);
+bsp_stack_hyp_size = DEFINED (bsp_stack_hyp_size) ? bsp_stack_hyp_size : 0;
+bsp_stack_hyp_size = ALIGN (bsp_stack_hyp_size, bsp_stack_align);
+
bsp_stack_main_size = DEFINED (bsp_stack_main_size) ? bsp_stack_main_size : 0;
bsp_stack_main_size = ALIGN (bsp_stack_main_size, bsp_stack_align);
-bsp_stack_all_size = bsp_stack_abt_size + bsp_stack_fiq_size + bsp_stack_irq_size + bsp_stack_svc_size + bsp_stack_und_size + bsp_stack_main_size;
+bsp_stack_all_size = bsp_stack_abt_size + bsp_stack_fiq_size + bsp_stack_irq_size + bsp_stack_svc_size + bsp_stack_und_size + bsp_stack_hyp_size + bsp_stack_main_size;
bsp_processor_count = DEFINED (bsp_processor_count) ? bsp_processor_count : 1;
@@ -312,6 +315,10 @@ SECTIONS {
. = . + bsp_stack_und_size;
bsp_stack_und_end = .;
+ bsp_stack_hyp_begin = .;
+ . = . + bsp_stack_hyp_size;
+ bsp_stack_hyp_end = .;
+
bsp_stack_abt_begin = .;
. = . + bsp_stack_abt_size;
bsp_stack_abt_end = .;
--
1.9.1
More information about the devel
mailing list