bsps/arm: Support recent bootloaders starting kernel in HYP mode

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

Updates #2783

2 files changed, 85 insertions, 1 deletions

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 0000000000..b608c59fa8
--- /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@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 8a653e9337..da2316a82f 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 = .;