diff options
Diffstat (limited to 'c/src/lib')
| -rw-r--r-- | c/src/lib/libbsp/arm/shared/include/linker-symbols.h | 4 | ||||
| -rw-r--r-- | c/src/lib/libbsp/arm/shared/include/start.h | 13 | ||||
| -rw-r--r-- | c/src/lib/libbsp/arm/shared/start/start.S | 79 | ||||
| -rw-r--r-- | c/src/lib/libbsp/arm/shared/startup/bsp-start-in-hyp-support.S | 77 | ||||
| -rw-r--r-- | c/src/lib/libbsp/arm/shared/startup/linkcmds.base | 9 |
5 files changed, 175 insertions, 7 deletions
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 8bfa5d172d..4fe70de8fa 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 9df6df4f3f..d5e69c318c 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 4050deb930..30501be1e8 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: @@ -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,7 +338,9 @@ bsp_start_hook_0_done: bsp_vector_table_copy_done: - SWITCH_FROM_ARM_TO_THUMB r0 + ldmia sp!, {r0, r1, r2} + + SWITCH_FROM_ARM_TO_THUMB r3 /* Branch to start hook 1 */ bl bsp_start_hook_1 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 = .; |