diff options
Diffstat (limited to 'c/src/lib/libbsp/arm/shared/startup/bsp-start-in-hyp-support.S')
| -rw-r--r-- | c/src/lib/libbsp/arm/shared/startup/bsp-start-in-hyp-support.S | 77 |
1 files changed, 77 insertions, 0 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 |