← Back to branch summary

~ubuntu-branches/ubuntu/precise/linux-lowlatency/precise

« back to all changes in this revision

Viewing changes to arch/arm/boot/compressed/head.S

  • Committer: Package Import Robot
  • Author(s): Alessio Igor Bogani
  • Date: 2011-10-26 11:13:05 UTC
  • Revision ID: package-import@ubuntu.com-20111026111305-tz023xykf0i6eosh
Tags: upstream-3.2.0
Import upstream version 3.2.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
arch/arm/boot/compressed/head.S
 
1
/*
 
2
 *  linux/arch/arm/boot/compressed/head.S
 
3
 *
 
4
 *  Copyright (C) 1996-2002 Russell King
 
5
 *  Copyright (C) 2004 Hyok S. Choi (MPU support)
 
6
 *
 
7
 * This program is free software; you can redistribute it and/or modify
 
8
 * it under the terms of the GNU General Public License version 2 as
 
9
 * published by the Free Software Foundation.
 
10
 */
 
11
#include <linux/linkage.h>
 
12
 
 
13
/*
 
14
 * Debugging stuff
 
15
 *
 
16
 * Note that these macros must not contain any code which is not
 
17
 * 100% relocatable.  Any attempt to do so will result in a crash.
 
18
 * Please select one of the following when turning on debugging.
 
19
 */
 
20
#ifdef DEBUG
 
21
 
 
22
#if defined(CONFIG_DEBUG_ICEDCC)
 
23
 
 
24
#if defined(CONFIG_CPU_V6) || defined(CONFIG_CPU_V6K) || defined(CONFIG_CPU_V7)
 
25
                .macro  loadsp, rb, tmp
 
26
                .endm
 
27
                .macro  writeb, ch, rb
 
28
                mcr     p14, 0, \ch, c0, c5, 0
 
29
                .endm
 
30
#elif defined(CONFIG_CPU_XSCALE)
 
31
                .macro  loadsp, rb, tmp
 
32
                .endm
 
33
                .macro  writeb, ch, rb
 
34
                mcr     p14, 0, \ch, c8, c0, 0
 
35
                .endm
 
36
#else
 
37
                .macro  loadsp, rb, tmp
 
38
                .endm
 
39
                .macro  writeb, ch, rb
 
40
                mcr     p14, 0, \ch, c1, c0, 0
 
41
                .endm
 
42
#endif
 
43
 
 
44
#else
 
45
 
 
46
#include <mach/debug-macro.S>
 
47
 
 
48
                .macro  writeb, ch, rb
 
49
                senduart \ch, \rb
 
50
                .endm
 
51
 
 
52
#if defined(CONFIG_ARCH_SA1100)
 
53
                .macro  loadsp, rb, tmp
 
54
                mov     \rb, #0x80000000        @ physical base address
 
55
#ifdef CONFIG_DEBUG_LL_SER3
 
56
                add     \rb, \rb, #0x00050000   @ Ser3
 
57
#else
 
58
                add     \rb, \rb, #0x00010000   @ Ser1
 
59
#endif
 
60
                .endm
 
61
#elif defined(CONFIG_ARCH_S3C2410)
 
62
                .macro loadsp, rb, tmp
 
63
                mov     \rb, #0x50000000
 
64
                add     \rb, \rb, #0x4000 * CONFIG_S3C_LOWLEVEL_UART_PORT
 
65
                .endm
 
66
#else
 
67
                .macro  loadsp, rb, tmp
 
68
                addruart \rb, \tmp
 
69
                .endm
 
70
#endif
 
71
#endif
 
72
#endif
 
73
 
 
74
                .macro  kputc,val
 
75
                mov     r0, \val
 
76
                bl      putc
 
77
                .endm
 
78
 
 
79
                .macro  kphex,val,len
 
80
                mov     r0, \val
 
81
                mov     r1, #\len
 
82
                bl      phex
 
83
                .endm
 
84
 
 
85
                .macro  debug_reloc_start
 
86
#ifdef DEBUG
 
87
                kputc   #'\n'
 
88
                kphex   r6, 8           /* processor id */
 
89
                kputc   #':'
 
90
                kphex   r7, 8           /* architecture id */
 
91
#ifdef CONFIG_CPU_CP15
 
92
                kputc   #':'
 
93
                mrc     p15, 0, r0, c1, c0
 
94
                kphex   r0, 8           /* control reg */
 
95
#endif
 
96
                kputc   #'\n'
 
97
                kphex   r5, 8           /* decompressed kernel start */
 
98
                kputc   #'-'
 
99
                kphex   r9, 8           /* decompressed kernel end  */
 
100
                kputc   #'>'
 
101
                kphex   r4, 8           /* kernel execution address */
 
102
                kputc   #'\n'
 
103
#endif
 
104
                .endm
 
105
 
 
106
                .macro  debug_reloc_end
 
107
#ifdef DEBUG
 
108
                kphex   r5, 8           /* end of kernel */
 
109
                kputc   #'\n'
 
110
                mov     r0, r4
 
111
                bl      memdump         /* dump 256 bytes at start of kernel */
 
112
#endif
 
113
                .endm
 
114
 
 
115
                .section ".start", #alloc, #execinstr
 
116
/*
 
117
 * sort out different calling conventions
 
118
 */
 
119
                .align
 
120
                .arm                            @ Always enter in ARM state
 
121
start:
 
122
                .type   start,#function
 
123
                .rept   7
 
124
                mov     r0, r0
 
125
                .endr
 
126
   ARM(         mov     r0, r0          )
 
127
   ARM(         b       1f              )
 
128
 THUMB(         adr     r12, BSYM(1f)   )
 
129
 THUMB(         bx      r12             )
 
130
 
 
131
                .word   0x016f2818              @ Magic numbers to help the loader
 
132
                .word   start                   @ absolute load/run zImage address
 
133
                .word   _edata                  @ zImage end address
 
134
 THUMB(         .thumb                  )
 
135
1:              mov     r7, r1                  @ save architecture ID
 
136
                mov     r8, r2                  @ save atags pointer
 
137
 
 
138
#ifndef __ARM_ARCH_2__
 
139
                /*
 
140
                 * Booting from Angel - need to enter SVC mode and disable
 
141
                 * FIQs/IRQs (numeric definitions from angel arm.h source).
 
142
                 * We only do this if we were in user mode on entry.
 
143
                 */
 
144
                mrs     r2, cpsr                @ get current mode
 
145
                tst     r2, #3                  @ not user?
 
146
                bne     not_angel
 
147
                mov     r0, #0x17               @ angel_SWIreason_EnterSVC
 
148
 ARM(           swi     0x123456        )       @ angel_SWI_ARM
 
149
 THUMB(         svc     0xab            )       @ angel_SWI_THUMB
 
150
not_angel:
 
151
                mrs     r2, cpsr                @ turn off interrupts to
 
152
                orr     r2, r2, #0xc0           @ prevent angel from running
 
153
                msr     cpsr_c, r2
 
154
#else
 
155
                teqp    pc, #0x0c000003         @ turn off interrupts
 
156
#endif
 
157
 
 
158
                /*
 
159
                 * Note that some cache flushing and other stuff may
 
160
                 * be needed here - is there an Angel SWI call for this?
 
161
                 */
 
162
 
 
163
                /*
 
164
                 * some architecture specific code can be inserted
 
165
                 * by the linker here, but it should preserve r7, r8, and r9.
 
166
                 */
 
167
 
 
168
                .text
 
169
 
 
170
#ifdef CONFIG_AUTO_ZRELADDR
 
171
                @ determine final kernel image address
 
172
                mov     r4, pc
 
173
                and     r4, r4, #0xf8000000
 
174
                add     r4, r4, #TEXT_OFFSET
 
175
#else
 
176
                ldr     r4, =zreladdr
 
177
#endif
 
178
 
 
179
                bl      cache_on
 
180
 
 
181
restart:        adr     r0, LC0
 
182
                ldmia   r0, {r1, r2, r3, r6, r10, r11, r12}
 
183
                ldr     sp, [r0, #28]
 
184
 
 
185
                /*
 
186
                 * We might be running at a different address.  We need
 
187
                 * to fix up various pointers.
 
188
                 */
 
189
                sub     r0, r0, r1              @ calculate the delta offset
 
190
                add     r6, r6, r0              @ _edata
 
191
                add     r10, r10, r0            @ inflated kernel size location
 
192
 
 
193
                /*
 
194
                 * The kernel build system appends the size of the
 
195
                 * decompressed kernel at the end of the compressed data
 
196
                 * in little-endian form.
 
197
                 */
 
198
                ldrb    r9, [r10, #0]
 
199
                ldrb    lr, [r10, #1]
 
200
                orr     r9, r9, lr, lsl #8
 
201
                ldrb    lr, [r10, #2]
 
202
                ldrb    r10, [r10, #3]
 
203
                orr     r9, r9, lr, lsl #16
 
204
                orr     r9, r9, r10, lsl #24
 
205
 
 
206
#ifndef CONFIG_ZBOOT_ROM
 
207
                /* malloc space is above the relocated stack (64k max) */
 
208
                add     sp, sp, r0
 
209
                add     r10, sp, #0x10000
 
210
#else
 
211
                /*
 
212
                 * With ZBOOT_ROM the bss/stack is non relocatable,
 
213
                 * but someone could still run this code from RAM,
 
214
                 * in which case our reference is _edata.
 
215
                 */
 
216
                mov     r10, r6
 
217
#endif
 
218
 
 
219
                mov     r5, #0                  @ init dtb size to 0
 
220
#ifdef CONFIG_ARM_APPENDED_DTB
 
221
/*
 
222
 *   r0  = delta
 
223
 *   r2  = BSS start
 
224
 *   r3  = BSS end
 
225
 *   r4  = final kernel address
 
226
 *   r5  = appended dtb size (still unknown)
 
227
 *   r6  = _edata
 
228
 *   r7  = architecture ID
 
229
 *   r8  = atags/device tree pointer
 
230
 *   r9  = size of decompressed image
 
231
 *   r10 = end of this image, including  bss/stack/malloc space if non XIP
 
232
 *   r11 = GOT start
 
233
 *   r12 = GOT end
 
234
 *   sp  = stack pointer
 
235
 *
 
236
 * if there are device trees (dtb) appended to zImage, advance r10 so that the
 
237
 * dtb data will get relocated along with the kernel if necessary.
 
238
 */
 
239
 
 
240
                ldr     lr, [r6, #0]
 
241
#ifndef __ARMEB__
 
242
                ldr     r1, =0xedfe0dd0         @ sig is 0xd00dfeed big endian
 
243
#else
 
244
                ldr     r1, =0xd00dfeed
 
245
#endif
 
246
                cmp     lr, r1
 
247
                bne     dtb_check_done          @ not found
 
248
 
 
249
#ifdef CONFIG_ARM_ATAG_DTB_COMPAT
 
250
                /*
 
251
                 * OK... Let's do some funky business here.
 
252
                 * If we do have a DTB appended to zImage, and we do have
 
253
                 * an ATAG list around, we want the later to be translated
 
254
                 * and folded into the former here.  To be on the safe side,
 
255
                 * let's temporarily move  the stack away into the malloc
 
256
                 * area.  No GOT fixup has occurred yet, but none of the
 
257
                 * code we're about to call uses any global variable.
 
258
                */
 
259
                add     sp, sp, #0x10000
 
260
                stmfd   sp!, {r0-r3, ip, lr}
 
261
                mov     r0, r8
 
262
                mov     r1, r6
 
263
                sub     r2, sp, r6
 
264
                bl      atags_to_fdt
 
265
 
 
266
                /*
 
267
                 * If returned value is 1, there is no ATAG at the location
 
268
                 * pointed by r8.  Try the typical 0x100 offset from start
 
269
                 * of RAM and hope for the best.
 
270
                 */
 
271
                cmp     r0, #1
 
272
                sub     r0, r4, #TEXT_OFFSET
 
273
                add     r0, r0, #0x100
 
274
                mov     r1, r6
 
275
                sub     r2, sp, r6
 
276
                blne    atags_to_fdt
 
277
 
 
278
                ldmfd   sp!, {r0-r3, ip, lr}
 
279
                sub     sp, sp, #0x10000
 
280
#endif
 
281
 
 
282
                mov     r8, r6                  @ use the appended device tree
 
283
 
 
284
                /*
 
285
                 * Make sure that the DTB doesn't end up in the final
 
286
                 * kernel's .bss area. To do so, we adjust the decompressed
 
287
                 * kernel size to compensate if that .bss size is larger
 
288
                 * than the relocated code.
 
289
                 */
 
290
                ldr     r5, =_kernel_bss_size
 
291
                adr     r1, wont_overwrite
 
292
                sub     r1, r6, r1
 
293
                subs    r1, r5, r1
 
294
                addhi   r9, r9, r1
 
295
 
 
296
                /* Get the dtb's size */
 
297
                ldr     r5, [r6, #4]
 
298
#ifndef __ARMEB__
 
299
                /* convert r5 (dtb size) to little endian */
 
300
                eor     r1, r5, r5, ror #16
 
301
                bic     r1, r1, #0x00ff0000
 
302
                mov     r5, r5, ror #8
 
303
                eor     r5, r5, r1, lsr #8
 
304
#endif
 
305
 
 
306
                /* preserve 64-bit alignment */
 
307
                add     r5, r5, #7
 
308
                bic     r5, r5, #7
 
309
 
 
310
                /* relocate some pointers past the appended dtb */
 
311
                add     r6, r6, r5
 
312
                add     r10, r10, r5
 
313
                add     sp, sp, r5
 
314
dtb_check_done:
 
315
#endif
 
316
 
 
317
/*
 
318
 * Check to see if we will overwrite ourselves.
 
319
 *   r4  = final kernel address
 
320
 *   r9  = size of decompressed image
 
321
 *   r10 = end of this image, including  bss/stack/malloc space if non XIP
 
322
 * We basically want:
 
323
 *   r4 - 16k page directory >= r10 -> OK
 
324
 *   r4 + image length <= address of wont_overwrite -> OK
 
325
 */
 
326
                add     r10, r10, #16384
 
327
                cmp     r4, r10
 
328
                bhs     wont_overwrite
 
329
                add     r10, r4, r9
 
330
                adr     r9, wont_overwrite
 
331
                cmp     r10, r9
 
332
                bls     wont_overwrite
 
333
 
 
334
/*
 
335
 * Relocate ourselves past the end of the decompressed kernel.
 
336
 *   r6  = _edata
 
337
 *   r10 = end of the decompressed kernel
 
338
 * Because we always copy ahead, we need to do it from the end and go
 
339
 * backward in case the source and destination overlap.
 
340
 */
 
341
                /*
 
342
                 * Bump to the next 256-byte boundary with the size of
 
343
                 * the relocation code added. This avoids overwriting
 
344
                 * ourself when the offset is small.
 
345
                 */
 
346
                add     r10, r10, #((reloc_code_end - restart + 256) & ~255)
 
347
                bic     r10, r10, #255
 
348
 
 
349
                /* Get start of code we want to copy and align it down. */
 
350
                adr     r5, restart
 
351
                bic     r5, r5, #31
 
352
 
 
353
                sub     r9, r6, r5              @ size to copy
 
354
                add     r9, r9, #31             @ rounded up to a multiple
 
355
                bic     r9, r9, #31             @ ... of 32 bytes
 
356
                add     r6, r9, r5
 
357
                add     r9, r9, r10
 
358
 
 
359
1:              ldmdb   r6!, {r0 - r3, r10 - r12, lr}
 
360
                cmp     r6, r5
 
361
                stmdb   r9!, {r0 - r3, r10 - r12, lr}
 
362
                bhi     1b
 
363
 
 
364
                /* Preserve offset to relocated code. */
 
365
                sub     r6, r9, r6
 
366
 
 
367
#ifndef CONFIG_ZBOOT_ROM
 
368
                /* cache_clean_flush may use the stack, so relocate it */
 
369
                add     sp, sp, r6
 
370
#endif
 
371
 
 
372
                bl      cache_clean_flush
 
373
 
 
374
                adr     r0, BSYM(restart)
 
375
                add     r0, r0, r6
 
376
                mov     pc, r0
 
377
 
 
378
wont_overwrite:
 
379
/*
 
380
 * If delta is zero, we are running at the address we were linked at.
 
381
 *   r0  = delta
 
382
 *   r2  = BSS start
 
383
 *   r3  = BSS end
 
384
 *   r4  = kernel execution address
 
385
 *   r5  = appended dtb size (0 if not present)
 
386
 *   r7  = architecture ID
 
387
 *   r8  = atags pointer
 
388
 *   r11 = GOT start
 
389
 *   r12 = GOT end
 
390
 *   sp  = stack pointer
 
391
 */
 
392
                orrs    r1, r0, r5
 
393
                beq     not_relocated
 
394
 
 
395
                add     r11, r11, r0
 
396
                add     r12, r12, r0
 
397
 
 
398
#ifndef CONFIG_ZBOOT_ROM
 
399
                /*
 
400
                 * If we're running fully PIC === CONFIG_ZBOOT_ROM = n,
 
401
                 * we need to fix up pointers into the BSS region.
 
402
                 * Note that the stack pointer has already been fixed up.
 
403
                 */
 
404
                add     r2, r2, r0
 
405
                add     r3, r3, r0
 
406
 
 
407
                /*
 
408
                 * Relocate all entries in the GOT table.
 
409
                 * Bump bss entries to _edata + dtb size
 
410
                 */
 
411
1:              ldr     r1, [r11, #0]           @ relocate entries in the GOT
 
412
                add     r1, r1, r0              @ This fixes up C references
 
413
                cmp     r1, r2                  @ if entry >= bss_start &&
 
414
                cmphs   r3, r1                  @       bss_end > entry
 
415
                addhi   r1, r1, r5              @    entry += dtb size
 
416
                str     r1, [r11], #4           @ next entry
 
417
                cmp     r11, r12
 
418
                blo     1b
 
419
 
 
420
                /* bump our bss pointers too */
 
421
                add     r2, r2, r5
 
422
                add     r3, r3, r5
 
423
 
 
424
#else
 
425
 
 
426
                /*
 
427
                 * Relocate entries in the GOT table.  We only relocate
 
428
                 * the entries that are outside the (relocated) BSS region.
 
429
                 */
 
430
1:              ldr     r1, [r11, #0]           @ relocate entries in the GOT
 
431
                cmp     r1, r2                  @ entry < bss_start ||
 
432
                cmphs   r3, r1                  @ _end < entry
 
433
                addlo   r1, r1, r0              @ table.  This fixes up the
 
434
                str     r1, [r11], #4           @ C references.
 
435
                cmp     r11, r12
 
436
                blo     1b
 
437
#endif
 
438
 
 
439
not_relocated:  mov     r0, #0
 
440
1:              str     r0, [r2], #4            @ clear bss
 
441
                str     r0, [r2], #4
 
442
                str     r0, [r2], #4
 
443
                str     r0, [r2], #4
 
444
                cmp     r2, r3
 
445
                blo     1b
 
446
 
 
447
/*
 
448
 * The C runtime environment should now be setup sufficiently.
 
449
 * Set up some pointers, and start decompressing.
 
450
 *   r4  = kernel execution address
 
451
 *   r7  = architecture ID
 
452
 *   r8  = atags pointer
 
453
 */
 
454
                mov     r0, r4
 
455
                mov     r1, sp                  @ malloc space above stack
 
456
                add     r2, sp, #0x10000        @ 64k max
 
457
                mov     r3, r7
 
458
                bl      decompress_kernel
 
459
                bl      cache_clean_flush
 
460
                bl      cache_off
 
461
                mov     r0, #0                  @ must be zero
 
462
                mov     r1, r7                  @ restore architecture number
 
463
                mov     r2, r8                  @ restore atags pointer
 
464
 ARM(           mov     pc, r4  )               @ call kernel
 
465
 THUMB(         bx      r4      )               @ entry point is always ARM
 
466
 
 
467
                .align  2
 
468
                .type   LC0, #object
 
469
LC0:            .word   LC0                     @ r1
 
470
                .word   __bss_start             @ r2
 
471
                .word   _end                    @ r3
 
472
                .word   _edata                  @ r6
 
473
                .word   input_data_end - 4      @ r10 (inflated size location)
 
474
                .word   _got_start              @ r11
 
475
                .word   _got_end                @ ip
 
476
                .word   .L_user_stack_end       @ sp
 
477
                .size   LC0, . - LC0
 
478
 
 
479
#ifdef CONFIG_ARCH_RPC
 
480
                .globl  params
 
481
params:         ldr     r0, =0x10000100         @ params_phys for RPC
 
482
                mov     pc, lr
 
483
                .ltorg
 
484
                .align
 
485
#endif
 
486
 
 
487
/*
 
488
 * Turn on the cache.  We need to setup some page tables so that we
 
489
 * can have both the I and D caches on.
 
490
 *
 
491
 * We place the page tables 16k down from the kernel execution address,
 
492
 * and we hope that nothing else is using it.  If we're using it, we
 
493
 * will go pop!
 
494
 *
 
495
 * On entry,
 
496
 *  r4 = kernel execution address
 
497
 *  r7 = architecture number
 
498
 *  r8 = atags pointer
 
499
 * On exit,
 
500
 *  r0, r1, r2, r3, r9, r10, r12 corrupted
 
501
 * This routine must preserve:
 
502
 *  r4, r7, r8
 
503
 */
 
504
                .align  5
 
505
cache_on:       mov     r3, #8                  @ cache_on function
 
506
                b       call_cache_fn
 
507
 
 
508
/*
 
509
 * Initialize the highest priority protection region, PR7
 
510
 * to cover all 32bit address and cacheable and bufferable.
 
511
 */
 
512
__armv4_mpu_cache_on:
 
513
                mov     r0, #0x3f               @ 4G, the whole
 
514
                mcr     p15, 0, r0, c6, c7, 0   @ PR7 Area Setting
 
515
                mcr     p15, 0, r0, c6, c7, 1
 
516
 
 
517
                mov     r0, #0x80               @ PR7
 
518
                mcr     p15, 0, r0, c2, c0, 0   @ D-cache on
 
519
                mcr     p15, 0, r0, c2, c0, 1   @ I-cache on
 
520
                mcr     p15, 0, r0, c3, c0, 0   @ write-buffer on
 
521
 
 
522
                mov     r0, #0xc000
 
523
                mcr     p15, 0, r0, c5, c0, 1   @ I-access permission
 
524
                mcr     p15, 0, r0, c5, c0, 0   @ D-access permission
 
525
 
 
526
                mov     r0, #0
 
527
                mcr     p15, 0, r0, c7, c10, 4  @ drain write buffer
 
528
                mcr     p15, 0, r0, c7, c5, 0   @ flush(inval) I-Cache
 
529
                mcr     p15, 0, r0, c7, c6, 0   @ flush(inval) D-Cache
 
530
                mrc     p15, 0, r0, c1, c0, 0   @ read control reg
 
531
                                                @ ...I .... ..D. WC.M
 
532
                orr     r0, r0, #0x002d         @ .... .... ..1. 11.1
 
533
                orr     r0, r0, #0x1000         @ ...1 .... .... ....
 
534
 
 
535
                mcr     p15, 0, r0, c1, c0, 0   @ write control reg
 
536
 
 
537
                mov     r0, #0
 
538
                mcr     p15, 0, r0, c7, c5, 0   @ flush(inval) I-Cache
 
539
                mcr     p15, 0, r0, c7, c6, 0   @ flush(inval) D-Cache
 
540
                mov     pc, lr
 
541
 
 
542
__armv3_mpu_cache_on:
 
543
                mov     r0, #0x3f               @ 4G, the whole
 
544
                mcr     p15, 0, r0, c6, c7, 0   @ PR7 Area Setting
 
545
 
 
546
                mov     r0, #0x80               @ PR7
 
547
                mcr     p15, 0, r0, c2, c0, 0   @ cache on
 
548
                mcr     p15, 0, r0, c3, c0, 0   @ write-buffer on
 
549
 
 
550
                mov     r0, #0xc000
 
551
                mcr     p15, 0, r0, c5, c0, 0   @ access permission
 
552
 
 
553
                mov     r0, #0
 
554
                mcr     p15, 0, r0, c7, c0, 0   @ invalidate whole cache v3
 
555
                /*
 
556
                 * ?? ARMv3 MMU does not allow reading the control register,
 
557
                 * does this really work on ARMv3 MPU?
 
558
                 */
 
559
                mrc     p15, 0, r0, c1, c0, 0   @ read control reg
 
560
                                                @ .... .... .... WC.M
 
561
                orr     r0, r0, #0x000d         @ .... .... .... 11.1
 
562
                /* ?? this overwrites the value constructed above? */
 
563
                mov     r0, #0
 
564
                mcr     p15, 0, r0, c1, c0, 0   @ write control reg
 
565
 
 
566
                /* ?? invalidate for the second time? */
 
567
                mcr     p15, 0, r0, c7, c0, 0   @ invalidate whole cache v3
 
568
                mov     pc, lr
 
569
 
 
570
__setup_mmu:    sub     r3, r4, #16384          @ Page directory size
 
571
                bic     r3, r3, #0xff           @ Align the pointer
 
572
                bic     r3, r3, #0x3f00
 
573
/*
 
574
 * Initialise the page tables, turning on the cacheable and bufferable
 
575
 * bits for the RAM area only.
 
576
 */
 
577
                mov     r0, r3
 
578
                mov     r9, r0, lsr #18
 
579
                mov     r9, r9, lsl #18         @ start of RAM
 
580
                add     r10, r9, #0x10000000    @ a reasonable RAM size
 
581
                mov     r1, #0x12
 
582
                orr     r1, r1, #3 << 10
 
583
                add     r2, r3, #16384
 
584
1:              cmp     r1, r9                  @ if virt > start of RAM
 
585
#ifdef CONFIG_CPU_DCACHE_WRITETHROUGH
 
586
                orrhs   r1, r1, #0x08           @ set cacheable
 
587
#else
 
588
                orrhs   r1, r1, #0x0c           @ set cacheable, bufferable
 
589
#endif
 
590
                cmp     r1, r10                 @ if virt > end of RAM
 
591
                bichs   r1, r1, #0x0c           @ clear cacheable, bufferable
 
592
                str     r1, [r0], #4            @ 1:1 mapping
 
593
                add     r1, r1, #1048576
 
594
                teq     r0, r2
 
595
                bne     1b
 
596
/*
 
597
 * If ever we are running from Flash, then we surely want the cache
 
598
 * to be enabled also for our execution instance...  We map 2MB of it
 
599
 * so there is no map overlap problem for up to 1 MB compressed kernel.
 
600
 * If the execution is in RAM then we would only be duplicating the above.
 
601
 */
 
602
                mov     r1, #0x1e
 
603
                orr     r1, r1, #3 << 10
 
604
                mov     r2, pc
 
605
                mov     r2, r2, lsr #20
 
606
                orr     r1, r1, r2, lsl #20
 
607
                add     r0, r3, r2, lsl #2
 
608
                str     r1, [r0], #4
 
609
                add     r1, r1, #1048576
 
610
                str     r1, [r0]
 
611
                mov     pc, lr
 
612
ENDPROC(__setup_mmu)
 
613
 
 
614
__arm926ejs_mmu_cache_on:
 
615
#ifdef CONFIG_CPU_DCACHE_WRITETHROUGH
 
616
                mov     r0, #4                  @ put dcache in WT mode
 
617
                mcr     p15, 7, r0, c15, c0, 0
 
618
#endif
 
619
 
 
620
__armv4_mmu_cache_on:
 
621
                mov     r12, lr
 
622
#ifdef CONFIG_MMU
 
623
                bl      __setup_mmu
 
624
                mov     r0, #0
 
625
                mcr     p15, 0, r0, c7, c10, 4  @ drain write buffer
 
626
                mcr     p15, 0, r0, c8, c7, 0   @ flush I,D TLBs
 
627
                mrc     p15, 0, r0, c1, c0, 0   @ read control reg
 
628
                orr     r0, r0, #0x5000         @ I-cache enable, RR cache replacement
 
629
                orr     r0, r0, #0x0030
 
630
#ifdef CONFIG_CPU_ENDIAN_BE8
 
631
                orr     r0, r0, #1 << 25        @ big-endian page tables
 
632
#endif
 
633
                bl      __common_mmu_cache_on
 
634
                mov     r0, #0
 
635
                mcr     p15, 0, r0, c8, c7, 0   @ flush I,D TLBs
 
636
#endif
 
637
                mov     pc, r12
 
638
 
 
639
__armv7_mmu_cache_on:
 
640
                mov     r12, lr
 
641
#ifdef CONFIG_MMU
 
642
                mrc     p15, 0, r11, c0, c1, 4  @ read ID_MMFR0
 
643
                tst     r11, #0xf               @ VMSA
 
644
                blne    __setup_mmu
 
645
                mov     r0, #0
 
646
                mcr     p15, 0, r0, c7, c10, 4  @ drain write buffer
 
647
                tst     r11, #0xf               @ VMSA
 
648
                mcrne   p15, 0, r0, c8, c7, 0   @ flush I,D TLBs
 
649
#endif
 
650
                mrc     p15, 0, r0, c1, c0, 0   @ read control reg
 
651
                orr     r0, r0, #0x5000         @ I-cache enable, RR cache replacement
 
652
                orr     r0, r0, #0x003c         @ write buffer
 
653
#ifdef CONFIG_MMU
 
654
#ifdef CONFIG_CPU_ENDIAN_BE8
 
655
                orr     r0, r0, #1 << 25        @ big-endian page tables
 
656
#endif
 
657
                orrne   r0, r0, #1              @ MMU enabled
 
658
                movne   r1, #-1
 
659
                mcrne   p15, 0, r3, c2, c0, 0   @ load page table pointer
 
660
                mcrne   p15, 0, r1, c3, c0, 0   @ load domain access control
 
661
#endif
 
662
                mcr     p15, 0, r0, c1, c0, 0   @ load control register
 
663
                mrc     p15, 0, r0, c1, c0, 0   @ and read it back
 
664
                mov     r0, #0
 
665
                mcr     p15, 0, r0, c7, c5, 4   @ ISB
 
666
                mov     pc, r12
 
667
 
 
668
__fa526_cache_on:
 
669
                mov     r12, lr
 
670
                bl      __setup_mmu
 
671
                mov     r0, #0
 
672
                mcr     p15, 0, r0, c7, c7, 0   @ Invalidate whole cache
 
673
                mcr     p15, 0, r0, c7, c10, 4  @ drain write buffer
 
674
                mcr     p15, 0, r0, c8, c7, 0   @ flush UTLB
 
675
                mrc     p15, 0, r0, c1, c0, 0   @ read control reg
 
676
                orr     r0, r0, #0x1000         @ I-cache enable
 
677
                bl      __common_mmu_cache_on
 
678
                mov     r0, #0
 
679
                mcr     p15, 0, r0, c8, c7, 0   @ flush UTLB
 
680
                mov     pc, r12
 
681
 
 
682
__arm6_mmu_cache_on:
 
683
                mov     r12, lr
 
684
                bl      __setup_mmu
 
685
                mov     r0, #0
 
686
                mcr     p15, 0, r0, c7, c0, 0   @ invalidate whole cache v3
 
687
                mcr     p15, 0, r0, c5, c0, 0   @ invalidate whole TLB v3
 
688
                mov     r0, #0x30
 
689
                bl      __common_mmu_cache_on
 
690
                mov     r0, #0
 
691
                mcr     p15, 0, r0, c5, c0, 0   @ invalidate whole TLB v3
 
692
                mov     pc, r12
 
693
 
 
694
__common_mmu_cache_on:
 
695
#ifndef CONFIG_THUMB2_KERNEL
 
696
#ifndef DEBUG
 
697
                orr     r0, r0, #0x000d         @ Write buffer, mmu
 
698
#endif
 
699
                mov     r1, #-1
 
700
                mcr     p15, 0, r3, c2, c0, 0   @ load page table pointer
 
701
                mcr     p15, 0, r1, c3, c0, 0   @ load domain access control
 
702
                b       1f
 
703
                .align  5                       @ cache line aligned
 
704
1:              mcr     p15, 0, r0, c1, c0, 0   @ load control register
 
705
                mrc     p15, 0, r0, c1, c0, 0   @ and read it back to
 
706
                sub     pc, lr, r0, lsr #32     @ properly flush pipeline
 
707
#endif
 
708
 
 
709
#define PROC_ENTRY_SIZE (4*5)
 
710
 
 
711
/*
 
712
 * Here follow the relocatable cache support functions for the
 
713
 * various processors.  This is a generic hook for locating an
 
714
 * entry and jumping to an instruction at the specified offset
 
715
 * from the start of the block.  Please note this is all position
 
716
 * independent code.
 
717
 *
 
718
 *  r1  = corrupted
 
719
 *  r2  = corrupted
 
720
 *  r3  = block offset
 
721
 *  r9  = corrupted
 
722
 *  r12 = corrupted
 
723
 */
 
724
 
 
725
call_cache_fn:  adr     r12, proc_types
 
726
#ifdef CONFIG_CPU_CP15
 
727
                mrc     p15, 0, r9, c0, c0      @ get processor ID
 
728
#else
 
729
                ldr     r9, =CONFIG_PROCESSOR_ID
 
730
#endif
 
731
1:              ldr     r1, [r12, #0]           @ get value
 
732
                ldr     r2, [r12, #4]           @ get mask
 
733
                eor     r1, r1, r9              @ (real ^ match)
 
734
                tst     r1, r2                  @       & mask
 
735
 ARM(           addeq   pc, r12, r3             ) @ call cache function
 
736
 THUMB(         addeq   r12, r3                 )
 
737
 THUMB(         moveq   pc, r12                 ) @ call cache function
 
738
                add     r12, r12, #PROC_ENTRY_SIZE
 
739
                b       1b
 
740
 
 
741
/*
 
742
 * Table for cache operations.  This is basically:
 
743
 *   - CPU ID match
 
744
 *   - CPU ID mask
 
745
 *   - 'cache on' method instruction
 
746
 *   - 'cache off' method instruction
 
747
 *   - 'cache flush' method instruction
 
748
 *
 
749
 * We match an entry using: ((real_id ^ match) & mask) == 0
 
750
 *
 
751
 * Writethrough caches generally only need 'on' and 'off'
 
752
 * methods.  Writeback caches _must_ have the flush method
 
753
 * defined.
 
754
 */
 
755
                .align  2
 
756
                .type   proc_types,#object
 
757
proc_types:
 
758
                .word   0x41560600              @ ARM6/610
 
759
                .word   0xffffffe0
 
760
                W(b)    __arm6_mmu_cache_off    @ works, but slow
 
761
                W(b)    __arm6_mmu_cache_off
 
762
                mov     pc, lr
 
763
 THUMB(         nop                             )
 
764
@               b       __arm6_mmu_cache_on             @ untested
 
765
@               b       __arm6_mmu_cache_off
 
766
@               b       __armv3_mmu_cache_flush
 
767
 
 
768
                .word   0x00000000              @ old ARM ID
 
769
                .word   0x0000f000
 
770
                mov     pc, lr
 
771
 THUMB(         nop                             )
 
772
                mov     pc, lr
 
773
 THUMB(         nop                             )
 
774
                mov     pc, lr
 
775
 THUMB(         nop                             )
 
776
 
 
777
                .word   0x41007000              @ ARM7/710
 
778
                .word   0xfff8fe00
 
779
                W(b)    __arm7_mmu_cache_off
 
780
                W(b)    __arm7_mmu_cache_off
 
781
                mov     pc, lr
 
782
 THUMB(         nop                             )
 
783
 
 
784
                .word   0x41807200              @ ARM720T (writethrough)
 
785
                .word   0xffffff00
 
786
                W(b)    __armv4_mmu_cache_on
 
787
                W(b)    __armv4_mmu_cache_off
 
788
                mov     pc, lr
 
789
 THUMB(         nop                             )
 
790
 
 
791
                .word   0x41007400              @ ARM74x
 
792
                .word   0xff00ff00
 
793
                W(b)    __armv3_mpu_cache_on
 
794
                W(b)    __armv3_mpu_cache_off
 
795
                W(b)    __armv3_mpu_cache_flush
 
796
                
 
797
                .word   0x41009400              @ ARM94x
 
798
                .word   0xff00ff00
 
799
                W(b)    __armv4_mpu_cache_on
 
800
                W(b)    __armv4_mpu_cache_off
 
801
                W(b)    __armv4_mpu_cache_flush
 
802
 
 
803
                .word   0x41069260              @ ARM926EJ-S (v5TEJ)
 
804
                .word   0xff0ffff0
 
805
                W(b)    __arm926ejs_mmu_cache_on
 
806
                W(b)    __armv4_mmu_cache_off
 
807
                W(b)    __armv5tej_mmu_cache_flush
 
808
 
 
809
                .word   0x00007000              @ ARM7 IDs
 
810
                .word   0x0000f000
 
811
                mov     pc, lr
 
812
 THUMB(         nop                             )
 
813
                mov     pc, lr
 
814
 THUMB(         nop                             )
 
815
                mov     pc, lr
 
816
 THUMB(         nop                             )
 
817
 
 
818
                @ Everything from here on will be the new ID system.
 
819
 
 
820
                .word   0x4401a100              @ sa110 / sa1100
 
821
                .word   0xffffffe0
 
822
                W(b)    __armv4_mmu_cache_on
 
823
                W(b)    __armv4_mmu_cache_off
 
824
                W(b)    __armv4_mmu_cache_flush
 
825
 
 
826
                .word   0x6901b110              @ sa1110
 
827
                .word   0xfffffff0
 
828
                W(b)    __armv4_mmu_cache_on
 
829
                W(b)    __armv4_mmu_cache_off
 
830
                W(b)    __armv4_mmu_cache_flush
 
831
 
 
832
                .word   0x56056900
 
833
                .word   0xffffff00              @ PXA9xx
 
834
                W(b)    __armv4_mmu_cache_on
 
835
                W(b)    __armv4_mmu_cache_off
 
836
                W(b)    __armv4_mmu_cache_flush
 
837
 
 
838
                .word   0x56158000              @ PXA168
 
839
                .word   0xfffff000
 
840
                W(b)    __armv4_mmu_cache_on
 
841
                W(b)    __armv4_mmu_cache_off
 
842
                W(b)    __armv5tej_mmu_cache_flush
 
843
 
 
844
                .word   0x56050000              @ Feroceon
 
845
                .word   0xff0f0000
 
846
                W(b)    __armv4_mmu_cache_on
 
847
                W(b)    __armv4_mmu_cache_off
 
848
                W(b)    __armv5tej_mmu_cache_flush
 
849
 
 
850
#ifdef CONFIG_CPU_FEROCEON_OLD_ID
 
851
                /* this conflicts with the standard ARMv5TE entry */
 
852
                .long   0x41009260              @ Old Feroceon
 
853
                .long   0xff00fff0
 
854
                b       __armv4_mmu_cache_on
 
855
                b       __armv4_mmu_cache_off
 
856
                b       __armv5tej_mmu_cache_flush
 
857
#endif
 
858
 
 
859
                .word   0x66015261              @ FA526
 
860
                .word   0xff01fff1
 
861
                W(b)    __fa526_cache_on
 
862
                W(b)    __armv4_mmu_cache_off
 
863
                W(b)    __fa526_cache_flush
 
864
 
 
865
                @ These match on the architecture ID
 
866
 
 
867
                .word   0x00020000              @ ARMv4T
 
868
                .word   0x000f0000
 
869
                W(b)    __armv4_mmu_cache_on
 
870
                W(b)    __armv4_mmu_cache_off
 
871
                W(b)    __armv4_mmu_cache_flush
 
872
 
 
873
                .word   0x00050000              @ ARMv5TE
 
874
                .word   0x000f0000
 
875
                W(b)    __armv4_mmu_cache_on
 
876
                W(b)    __armv4_mmu_cache_off
 
877
                W(b)    __armv4_mmu_cache_flush
 
878
 
 
879
                .word   0x00060000              @ ARMv5TEJ
 
880
                .word   0x000f0000
 
881
                W(b)    __armv4_mmu_cache_on
 
882
                W(b)    __armv4_mmu_cache_off
 
883
                W(b)    __armv5tej_mmu_cache_flush
 
884
 
 
885
                .word   0x0007b000              @ ARMv6
 
886
                .word   0x000ff000
 
887
                W(b)    __armv4_mmu_cache_on
 
888
                W(b)    __armv4_mmu_cache_off
 
889
                W(b)    __armv6_mmu_cache_flush
 
890
 
 
891
                .word   0x000f0000              @ new CPU Id
 
892
                .word   0x000f0000
 
893
                W(b)    __armv7_mmu_cache_on
 
894
                W(b)    __armv7_mmu_cache_off
 
895
                W(b)    __armv7_mmu_cache_flush
 
896
 
 
897
                .word   0                       @ unrecognised type
 
898
                .word   0
 
899
                mov     pc, lr
 
900
 THUMB(         nop                             )
 
901
                mov     pc, lr
 
902
 THUMB(         nop                             )
 
903
                mov     pc, lr
 
904
 THUMB(         nop                             )
 
905
 
 
906
                .size   proc_types, . - proc_types
 
907
 
 
908
                /*
 
909
                 * If you get a "non-constant expression in ".if" statement"
 
910
                 * error from the assembler on this line, check that you have
 
911
                 * not accidentally written a "b" instruction where you should
 
912
                 * have written W(b).
 
913
                 */
 
914
                .if (. - proc_types) % PROC_ENTRY_SIZE != 0
 
915
                .error "The size of one or more proc_types entries is wrong."
 
916
                .endif
 
917
 
 
918
/*
 
919
 * Turn off the Cache and MMU.  ARMv3 does not support
 
920
 * reading the control register, but ARMv4 does.
 
921
 *
 
922
 * On exit,
 
923
 *  r0, r1, r2, r3, r9, r12 corrupted
 
924
 * This routine must preserve:
 
925
 *  r4, r7, r8
 
926
 */
 
927
                .align  5
 
928
cache_off:      mov     r3, #12                 @ cache_off function
 
929
                b       call_cache_fn
 
930
 
 
931
__armv4_mpu_cache_off:
 
932
                mrc     p15, 0, r0, c1, c0
 
933
                bic     r0, r0, #0x000d
 
934
                mcr     p15, 0, r0, c1, c0      @ turn MPU and cache off
 
935
                mov     r0, #0
 
936
                mcr     p15, 0, r0, c7, c10, 4  @ drain write buffer
 
937
                mcr     p15, 0, r0, c7, c6, 0   @ flush D-Cache
 
938
                mcr     p15, 0, r0, c7, c5, 0   @ flush I-Cache
 
939
                mov     pc, lr
 
940
 
 
941
__armv3_mpu_cache_off:
 
942
                mrc     p15, 0, r0, c1, c0
 
943
                bic     r0, r0, #0x000d
 
944
                mcr     p15, 0, r0, c1, c0, 0   @ turn MPU and cache off
 
945
                mov     r0, #0
 
946
                mcr     p15, 0, r0, c7, c0, 0   @ invalidate whole cache v3
 
947
                mov     pc, lr
 
948
 
 
949
__armv4_mmu_cache_off:
 
950
#ifdef CONFIG_MMU
 
951
                mrc     p15, 0, r0, c1, c0
 
952
                bic     r0, r0, #0x000d
 
953
                mcr     p15, 0, r0, c1, c0      @ turn MMU and cache off
 
954
                mov     r0, #0
 
955
                mcr     p15, 0, r0, c7, c7      @ invalidate whole cache v4
 
956
                mcr     p15, 0, r0, c8, c7      @ invalidate whole TLB v4
 
957
#endif
 
958
                mov     pc, lr
 
959
 
 
960
__armv7_mmu_cache_off:
 
961
                mrc     p15, 0, r0, c1, c0
 
962
#ifdef CONFIG_MMU
 
963
                bic     r0, r0, #0x000d
 
964
#else
 
965
                bic     r0, r0, #0x000c
 
966
#endif
 
967
                mcr     p15, 0, r0, c1, c0      @ turn MMU and cache off
 
968
                mov     r12, lr
 
969
                bl      __armv7_mmu_cache_flush
 
970
                mov     r0, #0
 
971
#ifdef CONFIG_MMU
 
972
                mcr     p15, 0, r0, c8, c7, 0   @ invalidate whole TLB
 
973
#endif
 
974
                mcr     p15, 0, r0, c7, c5, 6   @ invalidate BTC
 
975
                mcr     p15, 0, r0, c7, c10, 4  @ DSB
 
976
                mcr     p15, 0, r0, c7, c5, 4   @ ISB
 
977
                mov     pc, r12
 
978
 
 
979
__arm6_mmu_cache_off:
 
980
                mov     r0, #0x00000030         @ ARM6 control reg.
 
981
                b       __armv3_mmu_cache_off
 
982
 
 
983
__arm7_mmu_cache_off:
 
984
                mov     r0, #0x00000070         @ ARM7 control reg.
 
985
                b       __armv3_mmu_cache_off
 
986
 
 
987
__armv3_mmu_cache_off:
 
988
                mcr     p15, 0, r0, c1, c0, 0   @ turn MMU and cache off
 
989
                mov     r0, #0
 
990
                mcr     p15, 0, r0, c7, c0, 0   @ invalidate whole cache v3
 
991
                mcr     p15, 0, r0, c5, c0, 0   @ invalidate whole TLB v3
 
992
                mov     pc, lr
 
993
 
 
994
/*
 
995
 * Clean and flush the cache to maintain consistency.
 
996
 *
 
997
 * On exit,
 
998
 *  r1, r2, r3, r9, r10, r11, r12 corrupted
 
999
 * This routine must preserve:
 
1000
 *  r4, r6, r7, r8
 
1001
 */
 
1002
                .align  5
 
1003
cache_clean_flush:
 
1004
                mov     r3, #16
 
1005
                b       call_cache_fn
 
1006
 
 
1007
__armv4_mpu_cache_flush:
 
1008
                mov     r2, #1
 
1009
                mov     r3, #0
 
1010
                mcr     p15, 0, ip, c7, c6, 0   @ invalidate D cache
 
1011
                mov     r1, #7 << 5             @ 8 segments
 
1012
1:              orr     r3, r1, #63 << 26       @ 64 entries
 
1013
2:              mcr     p15, 0, r3, c7, c14, 2  @ clean & invalidate D index
 
1014
                subs    r3, r3, #1 << 26
 
1015
                bcs     2b                      @ entries 63 to 0
 
1016
                subs    r1, r1, #1 << 5
 
1017
                bcs     1b                      @ segments 7 to 0
 
1018
 
 
1019
                teq     r2, #0
 
1020
                mcrne   p15, 0, ip, c7, c5, 0   @ invalidate I cache
 
1021
                mcr     p15, 0, ip, c7, c10, 4  @ drain WB
 
1022
                mov     pc, lr
 
1023
                
 
1024
__fa526_cache_flush:
 
1025
                mov     r1, #0
 
1026
                mcr     p15, 0, r1, c7, c14, 0  @ clean and invalidate D cache
 
1027
                mcr     p15, 0, r1, c7, c5, 0   @ flush I cache
 
1028
                mcr     p15, 0, r1, c7, c10, 4  @ drain WB
 
1029
                mov     pc, lr
 
1030
 
 
1031
__armv6_mmu_cache_flush:
 
1032
                mov     r1, #0
 
1033
                mcr     p15, 0, r1, c7, c14, 0  @ clean+invalidate D
 
1034
                mcr     p15, 0, r1, c7, c5, 0   @ invalidate I+BTB
 
1035
                mcr     p15, 0, r1, c7, c15, 0  @ clean+invalidate unified
 
1036
                mcr     p15, 0, r1, c7, c10, 4  @ drain WB
 
1037
                mov     pc, lr
 
1038
 
 
1039
__armv7_mmu_cache_flush:
 
1040
                mrc     p15, 0, r10, c0, c1, 5  @ read ID_MMFR1
 
1041
                tst     r10, #0xf << 16         @ hierarchical cache (ARMv7)
 
1042
                mov     r10, #0
 
1043
                beq     hierarchical
 
1044
                mcr     p15, 0, r10, c7, c14, 0 @ clean+invalidate D
 
1045
                b       iflush
 
1046
hierarchical:
 
1047
                mcr     p15, 0, r10, c7, c10, 5 @ DMB
 
1048
                stmfd   sp!, {r0-r7, r9-r11}
 
1049
                mrc     p15, 1, r0, c0, c0, 1   @ read clidr
 
1050
                ands    r3, r0, #0x7000000      @ extract loc from clidr
 
1051
                mov     r3, r3, lsr #23         @ left align loc bit field
 
1052
                beq     finished                @ if loc is 0, then no need to clean
 
1053
                mov     r10, #0                 @ start clean at cache level 0
 
1054
loop1:
 
1055
                add     r2, r10, r10, lsr #1    @ work out 3x current cache level
 
1056
                mov     r1, r0, lsr r2          @ extract cache type bits from clidr
 
1057
                and     r1, r1, #7              @ mask of the bits for current cache only
 
1058
                cmp     r1, #2                  @ see what cache we have at this level
 
1059
                blt     skip                    @ skip if no cache, or just i-cache
 
1060
                mcr     p15, 2, r10, c0, c0, 0  @ select current cache level in cssr
 
1061
                mcr     p15, 0, r10, c7, c5, 4  @ isb to sych the new cssr&csidr
 
1062
                mrc     p15, 1, r1, c0, c0, 0   @ read the new csidr
 
1063
                and     r2, r1, #7              @ extract the length of the cache lines
 
1064
                add     r2, r2, #4              @ add 4 (line length offset)
 
1065
                ldr     r4, =0x3ff
 
1066
                ands    r4, r4, r1, lsr #3      @ find maximum number on the way size
 
1067
                clz     r5, r4                  @ find bit position of way size increment
 
1068
                ldr     r7, =0x7fff
 
1069
                ands    r7, r7, r1, lsr #13     @ extract max number of the index size
 
1070
loop2:
 
1071
                mov     r9, r4                  @ create working copy of max way size
 
1072
loop3:
 
1073
 ARM(           orr     r11, r10, r9, lsl r5    ) @ factor way and cache number into r11
 
1074
 ARM(           orr     r11, r11, r7, lsl r2    ) @ factor index number into r11
 
1075
 THUMB(         lsl     r6, r9, r5              )
 
1076
 THUMB(         orr     r11, r10, r6            ) @ factor way and cache number into r11
 
1077
 THUMB(         lsl     r6, r7, r2              )
 
1078
 THUMB(         orr     r11, r11, r6            ) @ factor index number into r11
 
1079
                mcr     p15, 0, r11, c7, c14, 2 @ clean & invalidate by set/way
 
1080
                subs    r9, r9, #1              @ decrement the way
 
1081
                bge     loop3
 
1082
                subs    r7, r7, #1              @ decrement the index
 
1083
                bge     loop2
 
1084
skip:
 
1085
                add     r10, r10, #2            @ increment cache number
 
1086
                cmp     r3, r10
 
1087
                bgt     loop1
 
1088
finished:
 
1089
                ldmfd   sp!, {r0-r7, r9-r11}
 
1090
                mov     r10, #0                 @ swith back to cache level 0
 
1091
                mcr     p15, 2, r10, c0, c0, 0  @ select current cache level in cssr
 
1092
iflush:
 
1093
                mcr     p15, 0, r10, c7, c10, 4 @ DSB
 
1094
                mcr     p15, 0, r10, c7, c5, 0  @ invalidate I+BTB
 
1095
                mcr     p15, 0, r10, c7, c10, 4 @ DSB
 
1096
                mcr     p15, 0, r10, c7, c5, 4  @ ISB
 
1097
                mov     pc, lr
 
1098
 
 
1099
__armv5tej_mmu_cache_flush:
 
1100
1:              mrc     p15, 0, r15, c7, c14, 3 @ test,clean,invalidate D cache
 
1101
                bne     1b
 
1102
                mcr     p15, 0, r0, c7, c5, 0   @ flush I cache
 
1103
                mcr     p15, 0, r0, c7, c10, 4  @ drain WB
 
1104
                mov     pc, lr
 
1105
 
 
1106
__armv4_mmu_cache_flush:
 
1107
                mov     r2, #64*1024            @ default: 32K dcache size (*2)
 
1108
                mov     r11, #32                @ default: 32 byte line size
 
1109
                mrc     p15, 0, r3, c0, c0, 1   @ read cache type
 
1110
                teq     r3, r9                  @ cache ID register present?
 
1111
                beq     no_cache_id
 
1112
                mov     r1, r3, lsr #18
 
1113
                and     r1, r1, #7
 
1114
                mov     r2, #1024
 
1115
                mov     r2, r2, lsl r1          @ base dcache size *2
 
1116
                tst     r3, #1 << 14            @ test M bit
 
1117
                addne   r2, r2, r2, lsr #1      @ +1/2 size if M == 1
 
1118
                mov     r3, r3, lsr #12
 
1119
                and     r3, r3, #3
 
1120
                mov     r11, #8
 
1121
                mov     r11, r11, lsl r3        @ cache line size in bytes
 
1122
no_cache_id:
 
1123
                mov     r1, pc
 
1124
                bic     r1, r1, #63             @ align to longest cache line
 
1125
                add     r2, r1, r2
 
1126
1:
 
1127
 ARM(           ldr     r3, [r1], r11           ) @ s/w flush D cache
 
1128
 THUMB(         ldr     r3, [r1]                ) @ s/w flush D cache
 
1129
 THUMB(         add     r1, r1, r11             )
 
1130
                teq     r1, r2
 
1131
                bne     1b
 
1132
 
 
1133
                mcr     p15, 0, r1, c7, c5, 0   @ flush I cache
 
1134
                mcr     p15, 0, r1, c7, c6, 0   @ flush D cache
 
1135
                mcr     p15, 0, r1, c7, c10, 4  @ drain WB
 
1136
                mov     pc, lr
 
1137
 
 
1138
__armv3_mmu_cache_flush:
 
1139
__armv3_mpu_cache_flush:
 
1140
                mov     r1, #0
 
1141
                mcr     p15, 0, r1, c7, c0, 0   @ invalidate whole cache v3
 
1142
                mov     pc, lr
 
1143
 
 
1144
/*
 
1145
 * Various debugging routines for printing hex characters and
 
1146
 * memory, which again must be relocatable.
 
1147
 */
 
1148
#ifdef DEBUG
 
1149
                .align  2
 
1150
                .type   phexbuf,#object
 
1151
phexbuf:        .space  12
 
1152
                .size   phexbuf, . - phexbuf
 
1153
 
 
1154
@ phex corrupts {r0, r1, r2, r3}
 
1155
phex:           adr     r3, phexbuf
 
1156
                mov     r2, #0
 
1157
                strb    r2, [r3, r1]
 
1158
1:              subs    r1, r1, #1
 
1159
                movmi   r0, r3
 
1160
                bmi     puts
 
1161
                and     r2, r0, #15
 
1162
                mov     r0, r0, lsr #4
 
1163
                cmp     r2, #10
 
1164
                addge   r2, r2, #7
 
1165
                add     r2, r2, #'0'
 
1166
                strb    r2, [r3, r1]
 
1167
                b       1b
 
1168
 
 
1169
@ puts corrupts {r0, r1, r2, r3}
 
1170
puts:           loadsp  r3, r1
 
1171
1:              ldrb    r2, [r0], #1
 
1172
                teq     r2, #0
 
1173
                moveq   pc, lr
 
1174
2:              writeb  r2, r3
 
1175
                mov     r1, #0x00020000
 
1176
3:              subs    r1, r1, #1
 
1177
                bne     3b
 
1178
                teq     r2, #'\n'
 
1179
                moveq   r2, #'\r'
 
1180
                beq     2b
 
1181
                teq     r0, #0
 
1182
                bne     1b
 
1183
                mov     pc, lr
 
1184
@ putc corrupts {r0, r1, r2, r3}
 
1185
putc:
 
1186
                mov     r2, r0
 
1187
                mov     r0, #0
 
1188
                loadsp  r3, r1
 
1189
                b       2b
 
1190
 
 
1191
@ memdump corrupts {r0, r1, r2, r3, r10, r11, r12, lr}
 
1192
memdump:        mov     r12, r0
 
1193
                mov     r10, lr
 
1194
                mov     r11, #0
 
1195
2:              mov     r0, r11, lsl #2
 
1196
                add     r0, r0, r12
 
1197
                mov     r1, #8
 
1198
                bl      phex
 
1199
                mov     r0, #':'
 
1200
                bl      putc
 
1201
1:              mov     r0, #' '
 
1202
                bl      putc
 
1203
                ldr     r0, [r12, r11, lsl #2]
 
1204
                mov     r1, #8
 
1205
                bl      phex
 
1206
                and     r0, r11, #7
 
1207
                teq     r0, #3
 
1208
                moveq   r0, #' '
 
1209
                bleq    putc
 
1210
                and     r0, r11, #7
 
1211
                add     r11, r11, #1
 
1212
                teq     r0, #7
 
1213
                bne     1b
 
1214
                mov     r0, #'\n'
 
1215
                bl      putc
 
1216
                cmp     r11, #64
 
1217
                blt     2b
 
1218
                mov     pc, r10
 
1219
#endif
 
1220
 
 
1221
                .ltorg
 
1222
reloc_code_end:
 
1223
 
 
1224
                .align
 
1225
                .section ".stack", "aw", %nobits
 
1226
.L_user_stack:  .space  4096
 
1227
.L_user_stack_end: