1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
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
|