| Russell King | d8dc7fb | 2015-04-04 16:58:38 +0100 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (C) 2015 Russell King |
| 3 | * |
| 4 | * This program is free software; you can redistribute it and/or modify |
| 5 | * it under the terms of the GNU General Public License version 2 as |
| 6 | * published by the Free Software Foundation. |
| 7 | * |
| 8 | * This assembly is required to safely remap the physical address space |
| 9 | * for Keystone 2 |
| 10 | */ |
| 11 | #include <linux/linkage.h> |
| 12 | #include <asm/asm-offsets.h> |
| 13 | #include <asm/cp15.h> |
| 14 | #include <asm/memory.h> |
| 15 | #include <asm/pgtable.h> |
| 16 | |
| 17 | .section ".idmap.text", "ax" |
| 18 | |
| 19 | #define L1_ORDER 3 |
| 20 | #define L2_ORDER 3 |
| 21 | |
| 22 | ENTRY(lpae_pgtables_remap_asm) |
| 23 | stmfd sp!, {r4-r8, lr} |
| 24 | |
| 25 | mrc p15, 0, r8, c1, c0, 0 @ read control reg |
| 26 | bic ip, r8, #CR_M @ disable caches and MMU |
| 27 | mcr p15, 0, ip, c1, c0, 0 |
| 28 | dsb |
| 29 | isb |
| 30 | |
| 31 | /* Update level 2 entries covering the kernel */ |
| 32 | ldr r6, =(_end - 1) |
| 33 | add r7, r2, #0x1000 |
| 34 | add r6, r7, r6, lsr #SECTION_SHIFT - L2_ORDER |
| 35 | add r7, r7, #PAGE_OFFSET >> (SECTION_SHIFT - L2_ORDER) |
| 36 | 1: ldrd r4, [r7] |
| 37 | adds r4, r4, r0 |
| 38 | adc r5, r5, r1 |
| 39 | strd r4, [r7], #1 << L2_ORDER |
| 40 | cmp r7, r6 |
| 41 | bls 1b |
| 42 | |
| 43 | /* Update level 2 entries for the boot data */ |
| 44 | add r7, r2, #0x1000 |
| 45 | add r7, r7, r3, lsr #SECTION_SHIFT - L2_ORDER |
| 46 | bic r7, r7, #(1 << L2_ORDER) - 1 |
| 47 | ldrd r4, [r7] |
| 48 | adds r4, r4, r0 |
| 49 | adc r5, r5, r1 |
| 50 | strd r4, [r7], #1 << L2_ORDER |
| 51 | ldrd r4, [r7] |
| 52 | adds r4, r4, r0 |
| 53 | adc r5, r5, r1 |
| 54 | strd r4, [r7] |
| 55 | |
| 56 | /* Update level 1 entries */ |
| 57 | mov r6, #4 |
| 58 | mov r7, r2 |
| 59 | 2: ldrd r4, [r7] |
| 60 | adds r4, r4, r0 |
| 61 | adc r5, r5, r1 |
| 62 | strd r4, [r7], #1 << L1_ORDER |
| 63 | subs r6, r6, #1 |
| 64 | bne 2b |
| 65 | |
| 66 | mrrc p15, 0, r4, r5, c2 @ read TTBR0 |
| 67 | adds r4, r4, r0 @ update physical address |
| 68 | adc r5, r5, r1 |
| 69 | mcrr p15, 0, r4, r5, c2 @ write back TTBR0 |
| 70 | mrrc p15, 1, r4, r5, c2 @ read TTBR1 |
| 71 | adds r4, r4, r0 @ update physical address |
| 72 | adc r5, r5, r1 |
| 73 | mcrr p15, 1, r4, r5, c2 @ write back TTBR1 |
| 74 | |
| 75 | dsb |
| 76 | |
| 77 | mov ip, #0 |
| 78 | mcr p15, 0, ip, c7, c5, 0 @ I+BTB cache invalidate |
| 79 | mcr p15, 0, ip, c8, c7, 0 @ local_flush_tlb_all() |
| 80 | dsb |
| 81 | isb |
| 82 | |
| 83 | mcr p15, 0, r8, c1, c0, 0 @ re-enable MMU |
| 84 | dsb |
| 85 | isb |
| 86 | |
| 87 | ldmfd sp!, {r4-r8, pc} |
| 88 | ENDPROC(lpae_pgtables_remap_asm) |