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
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
|
/*
<:copyright-BRCM:2019:DUAL/GPL:standard
Copyright (c) 2019 Broadcom
All Rights Reserved
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License, version 2, as published by
the Free Software Foundation (the "GPL").
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
A copy of the GPL is available at http://www.broadcom.com/licenses/GPLv2.php, or by
writing to the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
Boston, MA 02111-1307, USA.
:>
*/
#include <asm_macros.S>
#include <platform_def.h>
#include <cpu_macros.S>
.globl a9_mmu_set_ttbr
.globl a9_mmu_invalidate_tlb
.globl a9_mmu_set_scn
.globl a9_mmu_enable
.globl a9_l1cache_enable_d
.globl a9_l1cache_enable_i
.globl a9_gic_secure_init
.globl a9_l1cache_inval_d
func a9_mmu_set_ttbr
push {r0-r12,r14}
// Set the access control to client so AP is checked with tlb entry
ldr r1, =0x55555555
mcr p15, 0, r1, c3, c0, 0 // Write Domain Access Control Register
// Set Translation Table base address. r0 must be preserved
mcr p15, 0, r0, c2, c0, 0 // Write Translation Table Base Register 0
isb
dsb
pop {r0-r12,r14}
mov pc,lr
endfunc a9_mmu_set_ttbr
func a9_mmu_invalidate_tlb
push {r0-r12,r14}
mov r0,#0
mcr p15,0,r0,c8,c7,0 /* Invalidate TLB */
isb
dsb
pop {r0-r12,r14}
mov pc,lr
endfunc a9_mmu_invalidate_tlb
/*
map_set_scn - sets number (SCN_RNG) of L1 table entries. Operates on 1 MB sections
r0 - L1 table paddr
r1 - virtual address start
r2 - address range in Megabytes
r3 - Sections' attributes
r4 - mapped physical start address
Table max size 16K (4K*4)
*/
#define WORD_SIZE 4 /* bytes */
#define WORD_BITLEN 32 /*bitlen or shift*/
#define WORD_ALGN 2 /* word alignment shift*/
#define SCN_BITLEN 20 /*bitlen or shift */
#define L1TBL_PA r0 /* */
#define SCN_VA r1
#define SCN_RNG r2
#define SCN_ATTR r3
#define SCN_PA r4
#define SCN_ENTRY r5
#define L1TBL_LEN r6
/* NN (not nested) - meaning there are no calls to subroutine. LR is not preserved
registers r0-r6 are not preserved and must not be relied upon
*/
func a9_mmu_set_scn
push {r0-r12,r14}
mov SCN_ENTRY,#0
/* adjust virt/phys addresses for the loop increment */
lsr SCN_VA, SCN_VA, #(SCN_BITLEN)
lsr SCN_PA, SCN_PA, #(SCN_BITLEN)
/* clear 12 msb of the section attribute */
bfc SCN_ATTR, #SCN_BITLEN, #(WORD_BITLEN-SCN_BITLEN)
/* set max range not exceeding 0xfff */
add SCN_RNG, SCN_VA, SCN_RNG
cmp SCN_RNG, L1TBL_LEN
blt 1f
mov SCN_RNG, L1TBL_LEN
1:
orr SCN_ENTRY, SCN_ATTR, SCN_PA, lsl #(SCN_BITLEN)
str SCN_ENTRY, [L1TBL_PA, SCN_VA, lsl #(WORD_ALGN)]
add SCN_PA, SCN_PA, #1
add SCN_VA, SCN_VA, #1
cmp SCN_VA, SCN_RNG
bne 1b
pop {r0-r12,r14}
mov pc,lr
endfunc a9_mmu_set_scn
/*
* CR1 bits (CP#15 CR1)
*/
#define CR_M (1 << 0) /* MMU enable */
#define CR_C (1 << 2) /* Dcache enable */
#define CR_Z (1 << 11) /* Program Flow Prediction */
#define CR_I (1 << 12) /* Icache enable */
func a9_mmu_enable
push {r0-r12,r14}
mrc p15, 0, r0, c1, c0, 0 // Read CP15 System Control register
orr r0, r0, #CR_M // Set M bit 0 to enable MMU
mcr p15, 0, r0, c1, c0, 0 // Write CP15 System Control register
isb
pop {r0-r12,r14}
mov pc, lr
endfunc a9_mmu_enable
func a9_l1cache_enable_i
push {r0-r12,r14}
mrc p15, 0, r0, c1, c0, 0 // Read Control Register configuration data
orr r0, r0, #CR_I // Enable I Cache
orr r0, r0, #CR_Z // Enable Prediction
mcr p15, 0, r0, c1, c0, 0 // Write Control Register configuration data
isb
pop {r0-r12,r14}
mov pc, lr
endfunc a9_l1cache_enable_i
func a9_l1cache_enable_d
push {r0-r12,r14}
mrc p15, 0, r0, c1, c0, 0 // Read Control Register configuration data
orr r0, r0, #CR_C // Enable D Cache
mcr p15, 0, r0, c1, c0, 0 // Write Control Register configuration data
isb
pop {r0-r12,r14}
mov pc, lr
endfunc a9_l1cache_enable_d
func a9_gic_secure_init
push {r0-r12,r14}
/* setup the GIC for non secure world. group 0 secure state, group 1 non secure state */
/* enable interrupt for both groups */
ldr r0, =GICD_BASE
ldr r1, =0x3
str r1, [r0, #GICD_CTLR_OFFSET]
/* assign all the interrupt to group 1 */
ldr r2, [r0, #GICD_TYPER_OFFSET]
and r2, r2, #0x1f
add r2, r2, #1
ldr r1, =0xffffffff
add r0, r0, #GICD_IGROUPR0_OFFSET
setgroup:
str r1, [r0]
add r0, #4
subs r2, r2, #1
bne setgroup
/* enable both group interrupt in the cpu interface */
ldr r0, =GICC_BASE
ldr r1, [r0, #GICC_CTLR_OFFSET]
orr r1, #0x3
str r1, [r0]
/* set cpu priority mask view for nonsecure */
mov r1, #0x80
str r1, [r0, #GICC_PMR_OFFSET]
pop {r0-r12,r14}
mov pc, lr
endfunc a9_gic_secure_init
func a9_l1cache_inval_d
push {r0-r12,r14}
mrc p15, 1, r0, c0, c0, 1 @ read clidr
ands r3, r0, #0x7000000 @ extract loc from clidr
mov r3, r3, lsr #23 @ left align loc bit field
beq invfinished @ if loc is 0, then no need to clean
mov r10, #0 @ start clean at cache level 0
invloop1:
add r2, r10, r10, lsr #1 @ work out 3x current cache level
mov r1, r0, lsr r2 @ extract cache type bits from clidr
and r1, r1, #7 @ mask of the bits for current cache only
cmp r1, #2 @ see what cache we have at this level
blt invskip @ skip if no cache, or just i-cache
mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr
isb
@ but we compile with armv5
mrc p15, 1, r1, c0, c0, 0 @ read the new csidr
and r2, r1, #7 @ extract the length of the cache lines
add r2, r2, #4 @ add 4 (line length offset)
ldr r4, =0x3ff
ands r4, r4, r1, lsr #3 @ find maximum number on the way size
clz r5, r4 @ find bit position of way size increment
ldr r7, =0x7fff
ands r7, r7, r1, lsr #13 @ extract max number of the index size
invloop2:
mov r9, r4 @ create working copy of max way size
invloop3:
orr r6, r10, r9, lsl r5 @ factor way and cache number into r6
orr r6, r6, r7, lsl r2 @ factor index number into r6
mcr p15, 0, r6, c7, c6, 2 @ invalidate by set/way
subs r9, r9, #1 @ decrement the way
bge invloop3
subs r7, r7, #1 @ decrement the index
bge invloop2
invskip:
add r10, r10, #2 @ increment cache number
cmp r3, r10
bgt invloop1
invfinished:
mov r10, #0 @ swith back to cache level 0
mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr
isb
pop {r0-r12,r14}
mov pc, lr
endfunc a9_l1cache_inval_d
|
