| @@ -1,14 +1,14 @@ | | | @@ -1,14 +1,14 @@ |
1 | /* $NetBSD: a9_mpsubr.S,v 1.12 2014/01/24 05:14:11 matt Exp $ */ | | 1 | /* $NetBSD: a9_mpsubr.S,v 1.13 2014/02/21 22:22:48 matt Exp $ */ |
2 | /*- | | 2 | /*- |
3 | * Copyright (c) 2012 The NetBSD Foundation, Inc. | | 3 | * Copyright (c) 2012 The NetBSD Foundation, Inc. |
4 | * All rights reserved. | | 4 | * All rights reserved. |
5 | * | | 5 | * |
6 | * This code is derived from software contributed to The NetBSD Foundation | | 6 | * This code is derived from software contributed to The NetBSD Foundation |
7 | * by Matt Thomas of 3am Software Foundry. | | 7 | * by Matt Thomas of 3am Software Foundry. |
8 | * | | 8 | * |
9 | * Redistribution and use in source and binary forms, with or without | | 9 | * Redistribution and use in source and binary forms, with or without |
10 | * modification, are permitted provided that the following conditions | | 10 | * modification, are permitted provided that the following conditions |
11 | * are met: | | 11 | * are met: |
12 | * 1. Redistributions of source code must retain the above copyright | | 12 | * 1. Redistributions of source code must retain the above copyright |
13 | * notice, this list of conditions and the following disclaimer. | | 13 | * notice, this list of conditions and the following disclaimer. |
14 | * 2. Redistributions in binary form must reproduce the above copyright | | 14 | * 2. Redistributions in binary form must reproduce the above copyright |
| @@ -101,142 +101,177 @@ arm_boot_l1pt_init: | | | @@ -101,142 +101,177 @@ arm_boot_l1pt_init: |
101 | cmp n_sec, #0 | | 101 | cmp n_sec, #0 |
102 | bne 2b | | 102 | bne 2b |
103 | bx lr @ return | | 103 | bx lr @ return |
104 | | | 104 | |
105 | .unreq va | | 105 | .unreq va |
106 | .unreq pa | | 106 | .unreq pa |
107 | .unreq n_sec | | 107 | .unreq n_sec |
108 | .unreq attr | | 108 | .unreq attr |
109 | .unreq itable | | 109 | .unreq itable |
110 | .unreq l1table | | 110 | .unreq l1table |
111 | | | 111 | |
112 | #if defined(CPU_CORTEXA8) | | 112 | #if defined(CPU_CORTEXA8) |
113 | #undef CPU_CONTROL_SWP_ENABLE // not present on A8 | | 113 | #undef CPU_CONTROL_SWP_ENABLE // not present on A8 |
114 | #define CPU_CONTROL_SWP_ENABLE 0 | | 114 | #define CPU_CONTROL_SWP_ENABLE 0 |
115 | #endif | | 115 | #endif |
116 | #ifdef __ARMEL__ | | 116 | #ifdef __ARMEL__ |
117 | #undef CPU_CONTROL_EX_BEND // needs to clear on LE systems | | 117 | #define CPU_CONTROL_EX_BEND_SET 0 |
118 | #define CPU_CONTROL_EX_BEND 0 | | 118 | #else |
| | | 119 | #define CPU_CONTROL_EX_BEND_SET CPU_CONTROL_EX_BEND |
119 | #endif | | 120 | #endif |
120 | #ifdef ARM32_DISABLE_ALIGNMENT_FAULTS | | 121 | #ifdef ARM32_DISABLE_ALIGNMENT_FAULTS |
121 | #undef CPU_CONTROL_AFLT_ENABLE | | 122 | #define CPU_CONTROL_AFLT_ENABLE_CLR CPU_CONTROL_AFLT_ENABLE |
122 | #define CPU_CONTROL_AFLT_ENABLE 0 | | 123 | #define CPU_CONTROL_AFLT_ENABLE_SET 0 |
| | | 124 | #else |
| | | 125 | #deifne CPU_CONTROL_AFLT_ENABLE_CLR 0 |
| | | 126 | #define CPU_CONTROL_AFLT_ENABLE_SET CPU_CONTROL_AFLT_ENABLE |
123 | #endif | | 127 | #endif |
124 | | | 128 | |
125 | #define CPU_CONTROL_SET \ | | 129 | #define CPU_CONTROL_SET \ |
126 | (CPU_CONTROL_MMU_ENABLE | \ | | 130 | (CPU_CONTROL_MMU_ENABLE | \ |
127 | CPU_CONTROL_AFLT_ENABLE | \ | | 131 | CPU_CONTROL_AFLT_ENABLE_SET | \ |
128 | CPU_CONTROL_EX_BEND | \ | | | |
129 | CPU_CONTROL_DC_ENABLE | \ | | 132 | CPU_CONTROL_DC_ENABLE | \ |
130 | CPU_CONTROL_SWP_ENABLE | \ | | 133 | CPU_CONTROL_SWP_ENABLE | \ |
131 | CPU_CONTROL_BPRD_ENABLE | \ | | 134 | CPU_CONTROL_BPRD_ENABLE | \ |
132 | CPU_CONTROL_IC_ENABLE | \ | | 135 | CPU_CONTROL_IC_ENABLE | \ |
| | | 136 | CPU_CONTROL_EX_BEND_SET | \ |
133 | CPU_CONTROL_UNAL_ENABLE) | | 137 | CPU_CONTROL_UNAL_ENABLE) |
134 | | | 138 | |
| | | 139 | #define CPU_CONTROL_CLR \ |
| | | 140 | (CPU_CONTROL_AFLT_ENABLE_CLR) |
| | | 141 | |
135 | arm_cpuinit: | | 142 | arm_cpuinit: |
136 | /* | | 143 | /* |
137 | * In theory, because the MMU is off, we shouldn't need all of this, | | 144 | * In theory, because the MMU is off, we shouldn't need all of this, |
138 | * but let's not take any chances and do a typical sequence to set | | 145 | * but let's not take any chances and do a typical sequence to set |
139 | * the Translation Table Base. | | 146 | * the Translation Table Base. |
140 | */ | | 147 | */ |
141 | mov ip, lr | | 148 | mov ip, lr |
142 | mov r10, r0 | | 149 | mov r10, r0 |
| | | 150 | mov r1, #0 |
| | | 151 | |
| | | 152 | mcr p15, 0, r1, c7, c5, 0 // invalidate I cache |
143 | | | 153 | |
144 | mcr p15, 0, r10, c7, c5, 0 /* invalidate I cache */ | | 154 | mrc p15, 0, r2, c1, c0, 0 // read SCTRL |
| | | 155 | movw r1, #(CPU_CONTROL_DC_ENABLE|CPU_CONTROL_IC_ENABLE) |
| | | 156 | bic r2, r2, r1 // clear I+D cache enable |
145 | | | 157 | |
146 | mrc p15, 0, r2, c1, c0, 0 /* " " " */ | | 158 | #ifdef __ARMEB__ |
147 | bic r2, r2, #CPU_CONTROL_DC_ENABLE @ clear data cache enable | | 159 | /* |
148 | bic r2, r2, #CPU_CONTROL_IC_ENABLE @ clear instruction cache enable | | 160 | * SCTRL.EE determines the endianness of translation table lookups. |
149 | mcr p15, 0, r2, c1, c0, 0 /* " " " */ | | 161 | * So we need to make sure it's set before starting to use the new |
| | | 162 | * translation tables (which are big endian). |
| | | 163 | */ |
| | | 164 | orr r2, r2, #CPU_CONTROL_EX_BEND |
| | | 165 | bic r2, r2, #CPU_CONTROL_MMU_ENABLE |
| | | 166 | pli [pc, #32] /* preload the next few cachelines */ |
| | | 167 | pli [pc, #64] |
| | | 168 | pli [pc, #96] |
| | | 169 | pli [pc, #128] |
| | | 170 | #endif |
| | | 171 | |
| | | 172 | mcr p15, 0, r2, c1, c0, 0 /* write SCTRL */ |
150 | | | 173 | |
151 | XPUTC(#70) | | 174 | XPUTC(#70) |
152 | mov r1, #0 | | | |
153 | dsb /* Drain the write buffers. */ | | 175 | dsb /* Drain the write buffers. */ |
154 | | | 176 | 1: |
155 | XPUTC(#71) | | 177 | XPUTC(#71) |
156 | mrc p15, 0, r2, c0, c0, 5 /* get MPIDR */ | | 178 | mrc p15, 0, r1, c0, c0, 5 /* get MPIDR */ |
157 | cmp r2, #0 | | 179 | cmp r1, #0 |
158 | orrlt r10, r10, #0x5b /* MP, cachable (Normal WB) */ | | 180 | orrlt r10, r10, #0x5b /* MP, cachable (Normal WB) */ |
159 | orrge r10, r10, #0x1b /* Non-MP, cacheable, normal WB */ | | 181 | orrge r10, r10, #0x1b /* Non-MP, cacheable, normal WB */ |
160 | mcr p15, 0, r10, c2, c0, 0 /* Set Translation Table Base */ | | 182 | mcr p15, 0, r10, c2, c0, 0 /* Set Translation Table Base */ |
161 | | | 183 | |
162 | XPUTC(#49) | | 184 | XPUTC(#72) |
| | | 185 | mov r1, #0 |
163 | mcr p15, 0, r1, c2, c0, 2 /* Set Translation Table Control */ | | 186 | mcr p15, 0, r1, c2, c0, 2 /* Set Translation Table Control */ |
164 | | | 187 | |
165 | XPUTC(#72) | | 188 | XPUTC(#73) |
166 | mov r1, #0 | | 189 | mov r1, #0 |
167 | mcr p15, 0, r1, c8, c7, 0 /* Invalidate TLBs */ | | 190 | mcr p15, 0, r1, c8, c7, 0 /* Invalidate TLBs */ |
168 | | | 191 | |
169 | /* Set the Domain Access register. Very important! */ | | 192 | /* Set the Domain Access register. Very important! */ |
170 | XPUTC(#73) | | 193 | XPUTC(#74) |
171 | mov r1, #((DOMAIN_CLIENT << (PMAP_DOMAIN_KERNEL*2)) | DOMAIN_CLIENT) | | 194 | mov r1, #((DOMAIN_CLIENT << (PMAP_DOMAIN_KERNEL*2)) | DOMAIN_CLIENT) |
172 | mcr p15, 0, r1, c3, c0, 0 | | 195 | mcr p15, 0, r1, c3, c0, 0 |
173 | | | 196 | |
174 | /* | | 197 | /* |
175 | * Enable the MMU, etc. | | 198 | * Enable the MMU, etc. |
176 | */ | | 199 | */ |
177 | XPUTC(#74) | | 200 | XPUTC(#75) |
178 | mrc p15, 0, r0, c1, c0, 0 | | 201 | mrc p15, 0, r0, c1, c0, 0 |
179 | | | 202 | |
180 | movw r3, #:lower16:CPU_CONTROL_SET | | 203 | movw r3, #:lower16:CPU_CONTROL_SET |
181 | #if (CPU_CONTROL_SET & 0xffff0000) | | 204 | #if (CPU_CONTROL_SET & 0xffff0000) |
182 | movt r3, #:upper16:CPU_CONTROL_SET | | 205 | movt r3, #:upper16:CPU_CONTROL_SET |
183 | #endif | | 206 | #endif |
184 | orr r0, r0, r3 | | 207 | orr r0, r0, r3 |
| | | 208 | #if defined(CPU_CONTROL_CLR) && (CPU_CONTROL_CLR != 0) |
| | | 209 | bic r0, r0, #CPU_CONTROL_CLR |
| | | 210 | #endif |
| | | 211 | pli 1f |
185 | | | 212 | |
186 | dsb | | 213 | dsb |
187 | .align 5 | | | |
188 | @ turn mmu on! | | 214 | @ turn mmu on! |
189 | mov r0, r0 | | 215 | mov r0, r0 /* fetch instruction cacheline */ |
190 | mcr p15, 0, r0, c1, c0, 0 | | 216 | 1: mcr p15, 0, r0, c1, c0, 0 |
191 | | | 217 | |
192 | /* | | 218 | /* |
193 | * Ensure that the coprocessor has finished turning on the MMU. | | 219 | * Ensure that the coprocessor has finished turning on the MMU. |
194 | */ | | 220 | */ |
195 | mrc p15, 0, r0, c0, c0, 0 /* Read an arbitrary value. */ | | 221 | mrc p15, 0, r0, c0, c0, 0 /* Read an arbitrary value. */ |
196 | mov r0, r0 /* Stall until read completes. */ | | 222 | mov r0, r0 /* Stall until read completes. */ |
197 | XPUTC(#76) | | 223 | 1: XPUTC(#76) |
198 | | | 224 | |
199 | bx ip /* return */ | | 225 | bx ip /* return */ |
200 | | | 226 | |
201 | /* | | 227 | /* |
202 | * Coprocessor register initialization values | | 228 | * Coprocessor register initialization values |
203 | */ | | 229 | */ |
204 | | | 230 | |
205 | .p2align 2 | | 231 | .p2align 2 |
206 | | | 232 | |
207 | /* bits to set in the Control Register */ | | 233 | /* bits to set in the Control Register */ |
208 | | | 234 | |
209 | #if defined(VERBOSE_INIT_ARM) && XPUTC_COM | | 235 | #if defined(VERBOSE_INIT_ARM) && XPUTC_COM |
210 | #define TIMO 0x25000 | | 236 | #define TIMO 0x25000 |
211 | #ifndef COM_MULT | | 237 | #ifndef COM_MULT |
212 | #define COM_MULT 1 | | 238 | #define COM_MULT 1 |
213 | #endif | | 239 | #endif |
214 | xputc: | | 240 | xputc: |
215 | #ifdef MULTIPROCESSOR | | 241 | #ifdef MULTIPROCESSOR |
| | | 242 | adr r3, xputc |
| | | 243 | movw r2, #:lower16:comlock |
| | | 244 | movt r2, #:upper16:comlock |
| | | 245 | bfi r3, r2, #0, #28 |
216 | mov r2, #1 | | 246 | mov r2, #1 |
217 | ldr r3, .Lcomlock | | | |
218 | 10: | | 247 | 10: |
219 | ldrex r1, [r3] | | 248 | ldrex r1, [r3] |
220 | cmp r1, #0 | | 249 | cmp r1, #0 |
221 | bne 10b | | 250 | bne 10b |
222 | strex r1, r2, [r3] | | 251 | strex r1, r2, [r3] |
223 | cmp r1, #0 | | 252 | cmp r1, #0 |
224 | bne 10b | | 253 | bne 10b |
225 | dsb | | 254 | dsb |
226 | #endif | | 255 | #endif |
227 | | | 256 | |
228 | mov r2, #TIMO | | 257 | mov r2, #TIMO |
229 | ldr r3, .Luart0 | | 258 | #ifdef CONADDR |
| | | 259 | movw r3, #:lower16:CONADDR |
| | | 260 | movt r3, #:upper16:CONADDR |
| | | 261 | #elif defined(CONSADDR) |
| | | 262 | movw r3, #:lower16:CONSADDR |
| | | 263 | movt r3, #:upper16:CONSADDR |
| | | 264 | #endif |
230 | 1: | | 265 | 1: |
231 | #if COM_MULT == 1 | | 266 | #if COM_MULT == 1 |
232 | ldrb r1, [r3, #(COM_LSR*COM_MULT)] | | 267 | ldrb r1, [r3, #(COM_LSR*COM_MULT)] |
233 | #else | | 268 | #else |
234 | #if COM_MULT == 2 | | 269 | #if COM_MULT == 2 |
235 | ldrh r1, [r3, #(COM_LSR*COM_MULT)] | | 270 | ldrh r1, [r3, #(COM_LSR*COM_MULT)] |
236 | #elif COM_MULT == 4 | | 271 | #elif COM_MULT == 4 |
237 | ldr r1, [r3, #(COM_LSR*COM_MULT)] | | 272 | ldr r1, [r3, #(COM_LSR*COM_MULT)] |
238 | #endif | | 273 | #endif |
239 | #ifdef COM_BSWAP | | 274 | #ifdef COM_BSWAP |
240 | lsr r1, r1, #(COM_MULT-1)*8 | | 275 | lsr r1, r1, #(COM_MULT-1)*8 |
241 | #endif | | 276 | #endif |
242 | #endif | | 277 | #endif |
| @@ -268,83 +303,87 @@ xputc: | | | @@ -268,83 +303,87 @@ xputc: |
268 | #elif COM_MULT == 4 | | 303 | #elif COM_MULT == 4 |
269 | ldr r1, [r3, #(COM_LSR*COM_MULT)] | | 304 | ldr r1, [r3, #(COM_LSR*COM_MULT)] |
270 | #endif | | 305 | #endif |
271 | #ifdef COM_BSWAP | | 306 | #ifdef COM_BSWAP |
272 | lsr r1, r1, #(COM_MULT-1)*8 | | 307 | lsr r1, r1, #(COM_MULT-1)*8 |
273 | #endif | | 308 | #endif |
274 | #endif | | 309 | #endif |
275 | tst r1, #LSR_TSRE | | 310 | tst r1, #LSR_TSRE |
276 | bne 4f | | 311 | bne 4f |
277 | subs r2, r2, #1 | | 312 | subs r2, r2, #1 |
278 | bne 3b | | 313 | bne 3b |
279 | 4: | | 314 | 4: |
280 | #ifdef MULTIPROCESSOR | | 315 | #ifdef MULTIPROCESSOR |
281 | ldr r3, .Lcomlock | | 316 | adr r3, xputc |
| | | 317 | movw r2, #:lower16:comlock |
| | | 318 | movt r2, #:upper16:comlock |
| | | 319 | bfi r3, r2, #0, #28 |
282 | mov r0, #0 | | 320 | mov r0, #0 |
283 | str r0, [r3] | | 321 | str r0, [r3] |
284 | dsb | | 322 | dsb |
285 | #endif | | 323 | #endif |
286 | bx lr | | 324 | bx lr |
287 | | | 325 | |
288 | .Luart0: | | | |
289 | #ifdef CONADDR | | | |
290 | .word CONADDR | | | |
291 | #elif defined(CONSADDR) | | | |
292 | .word CONSADDR | | | |
293 | #endif | | | |
294 | | | | |
295 | #ifdef MULTIPROCESSOR | | 326 | #ifdef MULTIPROCESSOR |
296 | .Lcomlock: | | | |
297 | .word comlock | | | |
298 | | | | |
299 | .pushsection .data | | 327 | .pushsection .data |
300 | comlock: | | 328 | comlock: |
301 | .p2align 2 | | 329 | .p2align 4 |
302 | .word 0 @ not in bss | | 330 | .word 0 @ not in bss |
| | | 331 | .p2align 4 |
303 | | | 332 | |
304 | .popsection | | 333 | .popsection |
305 | #endif /* MULTIPROCESSOR */ | | 334 | #endif /* MULTIPROCESSOR */ |
306 | #endif /* VERBOSE_INIT_ARM */ | | 335 | #endif /* VERBOSE_INIT_ARM */ |
307 | | | 336 | |
308 | #ifdef CPU_CORTEXA9 | | 337 | cortex_init: |
309 | a9_start: | | | |
310 | mov r10, lr @ save lr | | 338 | mov r10, lr @ save lr |
311 | | | 339 | |
312 | cpsid if, #PSR_SVC32_MODE | | 340 | cpsid if, #PSR_SVC32_MODE |
313 | | | 341 | |
314 | XPUTC(#64) | | 342 | XPUTC(#64) |
315 | bl _C_LABEL(armv7_icache_inv_all) @ invalidate i-cache | | 343 | adr ip, cortex_init |
| | | 344 | movw r0, #:lower16:_C_LABEL(armv7_icache_inv_all) |
| | | 345 | movt r0, #:upper16:_C_LABEL(armv7_icache_inv_all) |
| | | 346 | bfi ip, r0, #0, #28 |
| | | 347 | blx ip @ toss i-cache |
316 | | | 348 | |
| | | 349 | #ifdef CPU_CORTEXA9 |
317 | /* | | 350 | /* |
318 | * Step 1a, invalidate the all cache tags in all ways on the SCU. | | 351 | * Step 1a, invalidate the all cache tags in all ways on the SCU. |
319 | */ | | 352 | */ |
320 | XPUTC(#65) | | 353 | XPUTC(#65) |
321 | mrc p15, 4, r3, c15, c0, 0 @ read cbar | | 354 | mrc p15, 4, r3, c15, c0, 0 @ read cbar |
322 | ldr r0, [r3, #SCU_CFG] @ read scu config | | 355 | ldr r0, [r3, #SCU_CFG] @ read scu config |
323 | and r0, r0, #7 @ get cpu max | | 356 | and r0, r0, #7 @ get cpu max |
324 | add r0, r0, #2 @ adjust to cpu num | | 357 | add r0, r0, #2 @ adjust to cpu num |
325 | mov r1, #0xf @ select all ways | | 358 | mov r1, #0xf @ select all ways |
326 | lsl r1, r1, r0 @ shift into place | | 359 | lsl r1, r1, r0 @ shift into place |
327 | str r1, [r3, #SCU_INV_ALL_REG] @ write scu invalidate all | | 360 | str r1, [r3, #SCU_INV_ALL_REG] @ write scu invalidate all |
328 | dsb | | 361 | dsb |
329 | isb | | 362 | isb |
| | | 363 | #endif |
330 | | | 364 | |
331 | /* | | 365 | /* |
332 | * Step 1b, invalidate the data cache | | 366 | * Step 1b, invalidate the data cache |
333 | */ | | 367 | */ |
334 | XPUTC(#66) | | 368 | XPUTC(#66) |
335 | bl _C_LABEL(armv7_dcache_wbinv_all) @ writeback/invalidate d-cache | | 369 | adr ip, cortex_init |
| | | 370 | movw r0, #:lower16:_C_LABEL(armv7_dcache_wbinv_all) |
| | | 371 | movt r0, #:upper16:_C_LABEL(armv7_dcache_wbinv_all) |
| | | 372 | bfi ip, r0, #0, #28 |
| | | 373 | blx ip @ writeback & toss d-cache |
336 | XPUTC(#67) | | 374 | XPUTC(#67) |
337 | | | 375 | |
| | | 376 | #ifdef CPU_CORTEXA9 |
338 | /* | | 377 | /* |
339 | * Step 2, disable the data cache | | 378 | * Step 2, disable the data cache |
340 | */ | | 379 | */ |
341 | mrc p15, 0, r2, c1, c0, 0 @ get system ctl register (save) | | 380 | mrc p15, 0, r2, c1, c0, 0 @ get system ctl register (save) |
342 | bic r1, r2, #CPU_CONTROL_DC_ENABLE @ clear data cache enable | | 381 | bic r1, r2, #CPU_CONTROL_DC_ENABLE @ clear data cache enable |
343 | mcr p15, 0, r1, c1, c0, 0 @ set system ctl register | | 382 | mcr p15, 0, r1, c1, c0, 0 @ set system ctl register |
344 | isb | | 383 | isb |
345 | XPUTC(#49) | | 384 | XPUTC(#49) |
346 | | | 385 | |
347 | /* | | 386 | /* |
348 | * Step 3, enable the SCU (and set SMP mode) | | 387 | * Step 3, enable the SCU (and set SMP mode) |
349 | */ | | 388 | */ |
350 | mrc p15, 4, r3, c15, c0, 0 @ read cbar | | 389 | mrc p15, 4, r3, c15, c0, 0 @ read cbar |
| @@ -352,153 +391,195 @@ a9_start: | | | @@ -352,153 +391,195 @@ a9_start: |
352 | orr r1, r1, #SCU_CTL_SCU_ENA @ set scu enable flag | | 391 | orr r1, r1, #SCU_CTL_SCU_ENA @ set scu enable flag |
353 | str r1, [r3, #SCU_CTL] @ write scu control | | 392 | str r1, [r3, #SCU_CTL] @ write scu control |
354 | dsb | | 393 | dsb |
355 | isb | | 394 | isb |
356 | XPUTC(#50) | | 395 | XPUTC(#50) |
357 | | | 396 | |
358 | /* | | 397 | /* |
359 | * Step 4a, enable the data cache | | 398 | * Step 4a, enable the data cache |
360 | */ | | 399 | */ |
361 | orr r2, r2, #CPU_CONTROL_DC_ENABLE @ set data cache enable | | 400 | orr r2, r2, #CPU_CONTROL_DC_ENABLE @ set data cache enable |
362 | mcr p15, 0, r2, c1, c0, 0 @ reenable caches | | 401 | mcr p15, 0, r2, c1, c0, 0 @ reenable caches |
363 | isb | | 402 | isb |
364 | XPUTC(#51) | | 403 | XPUTC(#51) |
| | | 404 | #endif |
365 | | | 405 | |
366 | #ifdef MULTIPROCESSOR | | 406 | #ifdef MULTIPROCESSOR |
367 | /* | | 407 | /* |
368 | * Step 4b, set ACTLR.SMP=1 (and ACTRL.FX=1) | | 408 | * Step 4b, set ACTLR.SMP=1 (and on A9, ACTRL.FX=1) |
369 | */ | | 409 | */ |
370 | mrc p15, 0, r0, c1, c0, 1 @ read aux ctl | | 410 | mrc p15, 0, r0, c1, c0, 1 @ read aux ctl |
371 | orr r0, r0, #CORTEXA9_AUXCTL_SMP @ enable SMP | | 411 | orr r0, r0, #CORTEXA9_AUXCTL_SMP @ enable SMP |
372 | mcr p15, 0, r0, c1, c0, 1 @ write aux ctl | | 412 | mcr p15, 0, r0, c1, c0, 1 @ write aux ctl |
373 | isb | | 413 | isb |
| | | 414 | #ifdef CPU_CORTEXA9 |
374 | orr r0, r0, #CORTEXA9_AUXCTL_FW @ enable cache/tlb/coherency | | 415 | orr r0, r0, #CORTEXA9_AUXCTL_FW @ enable cache/tlb/coherency |
375 | mcr p15, 0, r0, c1, c0, 1 @ write aux ctl | | 416 | mcr p15, 0, r0, c1, c0, 1 @ write aux ctl |
376 | isb | | 417 | isb |
377 | XPUTC(#52) | | | |
378 | #endif | | 418 | #endif |
| | | 419 | XPUTC(#52) |
| | | 420 | #endif /* MULTIPROCESSOR */ |
379 | | | 421 | |
380 | bx r10 | | 422 | bx r10 |
381 | ASEND(a9_start) | | 423 | ASEND(cortex_init) |
382 | | | 424 | |
383 | /* | | 425 | /* |
384 | * Secondary processors come here after exiting the SKU ROM. | | 426 | * Secondary processors come here after exiting the SKU ROM. |
| | | 427 | * Running native endian until we have SMP enabled. Since no data |
| | | 428 | * is accessed, that shouldn't be a problem. |
385 | */ | | 429 | */ |
386 | a9_mpstart: | | 430 | cortex_mpstart: |
387 | #ifdef MULTIPROCESSOR | | 431 | cpsid if, #PSR_SVC32_MODE @ make sure we are in SVC mode |
| | | 432 | mrs r0, cpsr @ fetch CPSR value |
| | | 433 | msr spsr_sxc, r0 @ set SPSR[23:8] to known value |
| | | 434 | |
| | | 435 | #ifndef MULTIPROCESSOR |
| | | 436 | /* |
| | | 437 | * If not MULTIPROCESSOR, drop CPU into power saving state. |
| | | 438 | */ |
| | | 439 | 3: wfe |
| | | 440 | b 3b |
| | | 441 | #else |
388 | /* | | 442 | /* |
389 | * Step 1, invalidate the caches | | 443 | * Step 1, invalidate the caches |
390 | */ | | 444 | */ |
391 | bl _C_LABEL(armv7_icache_inv_all) @ toss i-cache | | 445 | adr ip, cortex_mpstart |
392 | bl _C_LABEL(armv7_dcache_inv_all) @ toss d-cache | | 446 | movw r0, #:lower16:_C_LABEL(armv7_icache_inv_all) |
| | | 447 | movt r0, #:upper16:_C_LABEL(armv7_icache_inv_all) |
| | | 448 | bfi ip, r0, #0, #28 |
| | | 449 | blx ip @ toss i-cache |
| | | 450 | adr ip, cortex_mpstart |
| | | 451 | movw ip, #:lower16:_C_LABEL(armv7_dcache_inv_all) |
| | | 452 | movt ip, #:upper16:_C_LABEL(armv7_dcache_inv_all) |
| | | 453 | bfi ip, r0, #0, #28 |
| | | 454 | blx ip @ toss d-cache |
393 | | | 455 | |
| | | 456 | #if defined(CPU_CORTEXA9) |
394 | /* | | 457 | /* |
395 | * Step 2, wait for the SCU to be enabled | | 458 | * Step 2, wait for the SCU to be enabled |
396 | */ | | 459 | */ |
397 | mrc p15, 4, r3, c15, c0, 0 @ read cbar | | 460 | mrc p15, 4, r3, c15, c0, 0 @ read cbar |
398 | 1: ldr r0, [r3, #SCU_CTL] @ read scu control | | 461 | 1: ldr r0, [r3, #SCU_CTL] @ read scu control |
399 | tst r0, #SCU_CTL_SCU_ENA @ enable bit set yet? | | 462 | tst r0, #SCU_CTL_SCU_ENA @ enable bit set yet? |
400 | bne 1b @ try again | | 463 | bne 1b @ try again |
| | | 464 | #endif |
401 | | | 465 | |
402 | /* | | 466 | /* |
403 | * Step 3, set ACTLR.SMP=1 (and ACTRL.FX=1) | | 467 | * Step 3, set ACTLR.SMP=1 (and ACTRL.FX=1) |
404 | */ | | 468 | */ |
405 | mrc p15, 0, r0, c1, c0, 1 @ read aux ctl | | 469 | mrc p15, 0, r0, c1, c0, 1 @ read aux ctl |
406 | orr r0, #CORTEXA9_AUXCTL_SMP @ enable SMP | | 470 | orr r0, #CORTEXA9_AUXCTL_SMP @ enable SMP |
407 | mcr p15, 0, r0, c1, c0, 1 @ write aux ctl | | 471 | mcr p15, 0, r0, c1, c0, 1 @ write aux ctl |
408 | mov r0, r0 | | 472 | mov r0, r0 |
| | | 473 | #if defined(CPU_CORTEXA9) |
409 | orr r0, #CORTEXA9_AUXCTL_FW @ enable cache/tlb/coherency | | 474 | orr r0, #CORTEXA9_AUXCTL_FW @ enable cache/tlb/coherency |
410 | mcr p15, 0, r0, c1, c0, 1 @ write aux ctl | | 475 | mcr p15, 0, r0, c1, c0, 1 @ write aux ctl |
411 | mov r0, r0 | | 476 | mov r0, r0 |
| | | 477 | #endif |
412 | | | 478 | |
413 | /* | | 479 | /* |
414 | * We should be in SMP mode now. | | 480 | * We should be in SMP mode now. |
415 | */ | | 481 | */ |
416 | mrc p15, 0, r4, c0, c0, 5 @ get MPIDR | | 482 | mrc p15, 0, r4, c0, c0, 5 @ get MPIDR |
417 | and r4, r4, #7 @ get our cpu numder | | 483 | and r4, r4, #7 @ get our cpu numder |
418 | | | 484 | |
| | | 485 | #ifdef __ARMEB__ |
| | | 486 | setend be @ switch to BE now |
| | | 487 | #endif |
| | | 488 | |
419 | #if defined(VERBOSE_INIT_ARM) | | 489 | #if defined(VERBOSE_INIT_ARM) |
420 | add r0, r4, #48 | | 490 | add r0, r4, #48 |
421 | bl xputc | | 491 | bl xputc |
422 | #endif | | 492 | #endif |
423 | | | 493 | |
424 | ldr r0, .Lcpu_hatched @ now show we've hatched | | 494 | /* |
| | | 495 | * To access things are not in .start, we need to replace the upper |
| | | 496 | * 4 bits of the address with where we are current executing. |
| | | 497 | */ |
| | | 498 | adr r10, cortex_mpstart |
| | | 499 | lsr r10, r10, #28 |
| | | 500 | |
| | | 501 | movw r0, #:lower16:_C_LABEL(arm_cpu_hatched) |
| | | 502 | movt r0, #:upper16:_C_LABEL(arm_cpu_hatched) |
| | | 503 | bfi r0, r10, #28, #4 // replace top 4 bits |
| | | 504 | add r0, r0, r10 |
425 | mov r5, #1 | | 505 | mov r5, #1 |
426 | lsl r5, r5, r4 | | 506 | lsl r5, r5, r4 |
427 | mov r1, r5 | | 507 | /* |
428 | bl _C_LABEL(atomic_or_32) | | 508 | * We inline the atomic_or_32 call since we might be in a different |
| | | 509 | * area of memory. |
| | | 510 | */ |
| | | 511 | 2: ldrex r1, [r0] |
| | | 512 | orr r1, r1, r5 |
| | | 513 | strex r2, r1, [r0] |
| | | 514 | cmp r2, #0 |
| | | 515 | bne 2b |
429 | | | 516 | |
430 | XPUTC(#97) | | 517 | XPUTC(#97) |
431 | #endif | | | |
432 | | | | |
433 | cpsid if, #PSR_SVC32_MODE @ make sure we are in SVC mode | | | |
434 | | | 518 | |
435 | /* Now we will wait for someone tell this cpu to start running */ | | 519 | /* Now we will wait for someone tell this cpu to start running */ |
436 | #ifdef MULTIPROCESSOR | | 520 | movw r0, #:lower16:_C_LABEL(arm_cpu_mbox) |
437 | ldr r0, .Lcpu_mbox | | 521 | movt r0, #:upper16:_C_LABEL(arm_cpu_mbox) |
438 | #else | | 522 | bfi r0, r10, #28, #4 |
439 | cmp r0, r0 | | 523 | add r0, r0, r10 |
440 | #endif | | 524 | 3: dmb |
441 | 2: | | | |
442 | #ifdef MULTIPROCESSOR | | | |
443 | dmb | | | |
444 | ldr r2, [r0] | | 525 | ldr r2, [r0] |
445 | tst r2, r5 | | 526 | tst r2, r5 |
446 | #endif | | 527 | wfeeq |
447 | @wfeeq | | 528 | beq 3b |
448 | beq 2b | | | |
449 | | | 529 | |
450 | #ifdef MULTIPROCESSOR | | 530 | XPUTC(#98) |
451 | 3: XPUTC(#98) | | 531 | movw r0, #:lower16:_C_LABEL(arm_cpu_marker) |
452 | ldr r0, .Lcpu_marker | | 532 | movt r0, #:upper16:_C_LABEL(arm_cpu_marker) |
| | | 533 | bfi r0, r10, #28, #4 |
453 | str pc, [r0] | | 534 | str pc, [r0] |
454 | | | 535 | |
455 | ldr r0, .Lkernel_l1pt /* get address of l1pt pvaddr */ | | 536 | movw r0, #:lower16:_C_LABEL(kernel_l1pt) |
| | | 537 | movt r0, #:upper16:_C_LABEL(kernel_l1pt) |
| | | 538 | bfi r0, r10, #28, #4 /* get address of l1pt pvaddr */ |
456 | ldr r0, [r0, #PV_PA] /* Now get the phys addr */ | | 539 | ldr r0, [r0, #PV_PA] /* Now get the phys addr */ |
457 | bl cpu_init | | 540 | /* |
458 | | | 541 | * After we turn on the MMU, we will no longer in .start so setup |
459 | ldr r0, .Lcpu_marker | | 542 | * return to rest of MP startup code in .text. |
460 | str pc, [r0] | | 543 | */ |
| | | 544 | movw lr, #:lower16:cortex_mpcontinuation |
| | | 545 | movt lr, #:upper16:cortex_mpcontinuation |
| | | 546 | b arm_cpuinit |
| | | 547 | #endif /* MULTIPROCESSOR */ |
| | | 548 | ASEND(cortex_mpstart) |
461 | | | 549 | |
| | | 550 | #ifdef MULTIPROCESSOR |
| | | 551 | .pushsection .text |
| | | 552 | cortex_mpcontinuation: |
462 | /* MMU, L1, are now on. */ | | 553 | /* MMU, L1, are now on. */ |
463 | | | 554 | |
464 | ldr r0, .Lcpu_info /* get pointer to cpu_infos */ | | 555 | movw r0, #:lower16:_C_LABEL(arm_cpu_marker) |
| | | 556 | movt r0, #:upper16:_C_LABEL(arm_cpu_marker) |
| | | 557 | str pc, [r0] |
| | | 558 | |
| | | 559 | movw r0, #:lower16:cpu_info |
| | | 560 | movt r0, #:upper16:cpu_info /* get pointer to cpu_infos */ |
465 | ldr r5, [r0, r4, lsl #2] /* load our cpu_info */ | | 561 | ldr r5, [r0, r4, lsl #2] /* load our cpu_info */ |
466 | ldr r6, [r5, #CI_IDLELWP] /* get the idlelwp */ | | 562 | ldr r6, [r5, #CI_IDLELWP] /* get the idlelwp */ |
467 | ldr r7, [r6, #L_PCB] /* now get its pcb */ | | 563 | ldr r7, [r6, #L_PCB] /* now get its pcb */ |
468 | ldr sp, [r7, #PCB_SP] /* finally, we can load our SP */ | | 564 | ldr sp, [r7, #PCB_KSP] /* finally, we can load our SP */ |
469 | #ifdef TPIDRPRW_IS_CURCPU | | 565 | #ifdef TPIDRPRW_IS_CURCPU |
470 | mcr p15, 0, r5, c13, c0, 4 /* squirrel away curcpu() */ | | 566 | mcr p15, 0, r5, c13, c0, 4 /* squirrel away curcpu() */ |
471 | #elif defined(TPIDRPRW_IS_CURLWP) | | 567 | #elif defined(TPIDRPRW_IS_CURLWP) |
472 | mcr p15, 0, r6, c13, c0, 4 /* squirrel away curlwp() */ | | 568 | mcr p15, 0, r6, c13, c0, 4 /* squirrel away curlwp() */ |
473 | #else | | 569 | #else |
474 | #error either TPIDRPRW_IS_CURCPU or TPIDRPRW_IS_CURLWP must be defined | | 570 | #error either TPIDRPRW_IS_CURCPU or TPIDRPRW_IS_CURLWP must be defined |
475 | #endif | | 571 | #endif |
476 | str r6, [r5, #CI_CURLWP] /* and note we are running on it */ | | 572 | str r6, [r5, #CI_CURLWP] /* and note we are running on it */ |
477 | | | 573 | |
478 | ldr r0, .Lcpu_marker | | 574 | str pc, [r0] // r0 still have arm_cpu_marker |
479 | str pc, [r0] | | | |
480 | | | 575 | |
481 | mov r0, r5 /* pass cpu_info */ | | 576 | mov r0, r5 // pass cpu_info |
482 | mov r1, r4 /* pass cpu_id */ | | 577 | mov r1, r4 // pass cpu_id |
483 | ldr r2, .Lbcm53xx_cpu_hatch /* pass md_cpu_hatch */ | | 578 | movw r2, #:lower16:MD_CPU_HATCH // pass md_cpu_hatch |
| | | 579 | movt r2, #:upper16:MD_CPU_HATCH // pass md_cpu_hatch |
484 | bl _C_LABEL(cpu_hatch) | | 580 | bl _C_LABEL(cpu_hatch) |
485 | b _C_LABEL(idle_loop) | | 581 | b _C_LABEL(idle_loop) |
486 | ASEND(a9_mpstart) | | 582 | ASEND(cortex_mpcontinuation) |
487 | /* NOT REACHED */ | | 583 | /* NOT REACHED */ |
488 | | | 584 | .popsection |
489 | .Lkernel_l1pt: | | | |
490 | .word _C_LABEL(kernel_l1pt) | | | |
491 | .Lcpu_info: | | | |
492 | .word _C_LABEL(cpu_info) | | | |
493 | .Lcpu_max: | | | |
494 | .word _C_LABEL(arm_cpu_max) | | | |
495 | .Lcpu_hatched: | | | |
496 | .word _C_LABEL(arm_cpu_hatched) | | | |
497 | .Lcpu_mbox: | | | |
498 | .word _C_LABEL(arm_cpu_mbox) | | | |
499 | .Lcpu_marker: | | | |
500 | .word _C_LABEL(arm_cpu_marker) | | | |
501 | .Lbcm53xx_cpu_hatch: | | | |
502 | .word _C_LABEL(bcm53xx_cpu_hatch) | | | |
503 | #endif /* MULTIPROCESSOR */ | | 585 | #endif /* MULTIPROCESSOR */ |
504 | #endif /* CPU_CORTEXA9 */ | | | |