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
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
|
#define __ASSEMBLY__
#include "memory.h"
#include "register.h"
#include "vmsa.h"
#include "platform.h"
#undef __ASSEMBLY__
.arch_extension sec
#define INIT_STACK EL3_INIT_STACK
.section .init
monitor_init:
cps #CPSR_M_MON
init_uart:
/* UART initialisation (38400 8N1) */
ldr r0, =UART0_BASE // UART base (Versatile Express)
mov r1, #0x10 // ibrd
str r1, [r0, #0x24]
mov r1, #0xc300
orr r1, #0x0001 // cr
str r1, [r0, #0x30]
/* Disable interrupts for now */
mrs r10, cpsr
orr r10, r10, #0xc0 /* Mask IRQ and FIQ */
msr cpsr, r10
/* Set-up the monitor vbar
* During startup, before the MMU is enabled we'll set-up the vecs in low
* mem.
*/
ldr r10, =el3_vectors
mcr p15, 0, r10, c12, c0, 1 /* MVBAR */
/* The stack still needs to be allocated and mapped so we set up a
* temporary stack for the time being.
*/
ldr r10, =INIT_STACK
mov sp, r10
/* Use the top of the stack to track our PA pool pointer */
ldr r11, =EL3_PA_POOL_BASE
ldr r10, =EL3_PTE_POOL_BASE
push {r10, r11}
monitor_mmu_init:
/* Disable data and instruction caches */
mrc p15, 0, r10, c1, c0, 0
bic r10, r10, #0x0004
bic r10, r10, #0x1000
bic r10, r10, #0x20000000 /* Enable the simplified access model AFE=1 */
mcr p15, 0, r10, c1, c0, 0
/* Set domain 0 & 1 for client access */
mov r10, #0x5
mcr p15, 0, r10, c3, c0, 0
/* Set TTBR0 to the initialized address plus enable shareable write-back
* write-allocate.
*/
ldr r10, =EL3_PGTBL_BASE
orr r10, r10, #0xB
mcr p15, 0, r10, c2, c0, 0
/* Set-up the table base control to split between TTBR0/1 (N = 0) */
mov r10, #0x0
mcr p15, 0, r10, c2, c0, 2
/* Clear the page table pages */
ldr r10, =EL3_PGTBL_BASE
ldr r11, =EL3_PGTBL_SIZE
mov r0, #0
monitor_pt_clear:
cmp r11, #0
beq monitor_map_init
str r0, [r10], #4
sub r11, r11, #4
b monitor_pt_clear
monitor_map_init:
/* Direct map the init code */
ldr r0, =_EL3_INIT_BASE
movw r1, #PTE_PRIV_RO
ldr r2, =_EL3_INIT_SIZE
bl map_pa_range
monitor_map_flash:
/* Direct map the EL3 flash sections so we can copy from them once
* the MMU has been enabled.
*/
ldr r0, =_EL3_FLASH_TEXT
mov r1, #PTE_PRIV_RO
ldr r2, =_EL3_TEXT_SIZE
bl map_pa_range
ldr r0, =_EL3_FLASH_DATA
mov r1, #PTE_PRIV_RO
ldr r2, =_EL3_DATA_SIZE
bl map_pa_range
monitor_map_text:
/* Map the EL3 text address range. Initially this needs to be RW so we can
* copy in the text from flash.
*/
ldr r0, =_EL3_TEXT_BASE
mov r1, #PTE_PRIV_RW
ldr r2, =_EL3_TEXT_SIZE
bl map_va_range
monitor_map_data:
/* Map the EL3 data address range. */
ldr r0, =_EL3_DATA_BASE
mov r1, #PTE_PRIV_RW
ldr r2, =_EL3_DATA_SIZE
bl map_va_range
monitor_map_stack:
/* Map the first page of the stack so we can get off the ground */
ldr r0, =0x7FFEF000 // EL3_STACK_BASE-0x1000
mov r1, #PTE_PRIV_RW
bl map_va
/* Map the PT page pool so we can access the PT after the MMU is enabled.
* We only allcoate EL3_PGTBL_SIZE pages for PT use which consists of the
* 2^(2-N) TTBR pages and the rest are PT page pool
*/
monitor_map_pt:
/* Direct map the page table pool */
ldr r0, =EL3_PGTBL_BASE
mov r1, #PTE_PRIV_RW
ldr r2, =0x4000
bl map_pa_range
ldr r0, =EL3_PTE_POOL_BASE
mov r1, #PTE_PRIV_RW
ldr r2, =0x3C000
bl map_pa_range
monitor_map_uart:
ldr r0, =UART0_BASE
mov r1, #PTE_USER_RW
bl map_pa
monitor_map_sysreg:
ldr r0, =SYSREG_BASE;
mov r1, #PTE_PRIV_RW
bl map_pa
/* We need to map the first page of secure EL1 as we will be doing an exception
* return to it in order to initialize it. We only map the first page as this
* is enough to get it off the ground.
*/
monitor_map_el1:
/* Direct map the init code */
ldr r0, =EL1_S_FLASH_BASE
mov r1, #PTE_PRIV_RO
bl map_pa
save_last_pa:
pop {r8, r9}
monitor_mmu_enable:
/* Enable the mmu */
mcr p15, 0, r10, c8, c7, 0
mrc p15, 0, r10, c1, c0, 0
orr r10, r10, #0x1
mcr p15, 0, r10, c1, c0, 0
isb
dsb
monitor_init_stack:
/* Set-up the monitor SVC stack */
ldr sp, =EL3_STACK_BASE
/* Migrate the next PA to the new stack we should not use it again until
* after we transition to start.
*/
push {r8, r9}
/* The EL3 address space is set-up and the MMU is started so it is safe to copy
* the text and data sections in.
*/
monitor_copy_text:
ldr r0, =_EL3_TEXT_BASE
ldr r1, =_EL3_FLASH_TEXT
ldr r2, =_EL3_TEXT_SIZE
bl memcpy
monitor_copy_data:
ldr r0, =_EL3_DATA_BASE
ldr r1, =_EL3_FLASH_DATA
ldr r2, =_EL3_DATA_SIZE
bl memcpy
monitor_init_start:
pop {r8, r9}
ldr r10, =mem_next_pa
str r9, [r10]
ldr r10, =mem_next_l1_page
str r8, [r10]
ldr r0, =_EL3_INIT_BASE
ldr r1, =_EL3_INIT_SIZE
ldr r2, =el3_start
blx r2
/* We should never get here */
monitor_init_end:
b monitor_init_end
#include "init_util.S"
|
