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

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)

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,15 +66,15 @@ 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.
 *
 * 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().

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:
@@ -258,6 +325,8 @@ bsp_start_hook_0_done:
 	 * vectors and the pointers to the default exception handlers.
 	 */
 
+	stmdb	sp!, {r4, r5, r6}
+
 	ldr	r0, =bsp_vector_table_begin
 	adr	r1, bsp_start_vector_table_begin
 	cmp	r0, r1
@@ -269,6 +338,8 @@ bsp_start_hook_0_done:
 
 bsp_vector_table_copy_done:
 
+	ldmia	sp!, {r0, r1, r2}
+
 	SWITCH_FROM_ARM_TO_THUMB	r0
 
 	/* Branch to start hook 1 */

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

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 = .;