First version
[3rdparty/ote_partner/tlk.git] / lib / monitor / arm64 / monitor_mmu.S
1 /*
2  * Copyright (c) 2013-2014, NVIDIA CORPORATION. All rights reserved
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining
5  * a copy of this software and associated documentation files
6  * (the "Software"), to deal in the Software without restriction,
7  * including without limitation the rights to use, copy, modify, merge,
8  * publish, distribute, sublicense, and/or sell copies of the Software,
9  * and to permit persons to whom the Software is furnished to do so,
10  * subject to the following conditions:
11  *
12  * The above copyright notice and this permission notice shall be
13  * included in all copies or substantial portions of the Software.
14  *
15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
18  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
19  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
20  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
21  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22  */
23 #include <config.h>
24 #include <asm.h>
25 #include <arm64/asm.h>
26 #include <arm64/mmu_ldesc.h>
27
28 /* MMU params for monitor mode */
29 #define MMU_PAGE_GRANULE        MMU_PAGE_GRANULE_4K
30
31 /* number of first/second level entries */
32 #define NUM_FIRST_LEVEL         (1 << (MONBITS - MMU_L1_INDEX_LSB))
33 #define NUM_SECOND_LEVEL        (1 << (MONBITS - MMU_L2_INDEX_LSB))
34
35 /* indices into attr indirect regs */
36 #define MMU_MEMORY_STRONGLY_ORDERED                     0
37 #define MMU_MEMORY_WB_OUTER_NO_ALLOC_INNER_ALLOC        1
38
39 /* mmio (index 0) */
40 #define MMU_PTE_L2_BLOCK_MMIO_FLAGS     \
41         (MMU_MEMORY_SET_ATTR_IDX(0) | MMU_MEMORY_ACCESS_FLAG |  \
42          MMU_MEMORY_AP_P_RW_U_NA | 0x1)
43
44 /* mem (index 1) */
45 #define MMU_PTE_L2_BLOCK_MEM_FLAGS      \
46         (MMU_MEMORY_SET_ATTR_IDX(1) | MMU_MEMORY_ACCESS_FLAG |  \
47          MMU_MEMORY_AP_P_RW_U_NA | 0x1)
48
49 /* value for MAIR register:
50  *      idx0 = strongly-ordered,
51  *      idx1 = outer: writeback/no alloc, inner: writeback/alloc
52  */
53 #define MMU_MEMORY_ATTR_INDIR   0x0000EF00
54
55 #define MMU_TCR_FLAGS_EL3       \
56         (MMU_MEMORY_TCR_PS_40BIT | \
57          MMU_MEMORY_TCR_TG0_4K | \
58          MMU_MEMORY_TCR_OUTER_RGN0(MMU_MEMORY_RGN_WRITE_BACK_ALLOCATE) | \
59          MMU_MEMORY_TCR_INNER_RGN0(MMU_MEMORY_RGN_WRITE_BACK_ALLOCATE) | \
60          MMU_MEMORY_TCR_T0SZ(MONBITS))
61
62 .macro mmu_phys_align, base, size, tmp
63         mov     \tmp, #(MMU_L2_BLOCK_SIZE - 1)
64         cbz     \size, .        // size is zero
65         tst     \base, \tmp
66         b.ne    .               // base not block aligned
67         tst     \size, \tmp
68         b.ne    .               // size not block aligned
69 .endm
70
71 .macro mmu_load_first_level, level1, level2, count
72         orr     \level2, \level2, #0x3
73 1:
74         str     \level2, [\level1], #(1 << MMU_ENTRY_SHIFT)
75         add     \level2, \level2, #(1 << MMU_PAGE_GRANULE)
76         sub     \count, \count, #0x1
77         cbnz    \count, 1b
78 .endm
79
80
81 /* corrupts \vaddr, \pgt, \paddr, \length \tmp */
82 .macro mmu_map_virt_phys, vaddr, paddr, length, pgt, flags, tmp, tmp2
83         mov     \tmp, #(MMU_L2_BLOCK_SIZE - 1)
84         bic     \paddr, \paddr, \tmp            // make aligned
85         add     \paddr, \paddr, \flags
86         lsr     \vaddr, \vaddr, #MMU_L2_BLOCK_SHIFT
87         add     \pgt, \pgt, \vaddr, lsl #MMU_ENTRY_SHIFT
88         mov     \tmp2, #(1 << MMU_L2_BLOCK_SHIFT)
89 1:
90         ldr     \tmp, [\pgt]
91         cbnz    \tmp, .         /* verify vaddr is not mapped */
92
93         /* write entry and update */
94         str     \paddr, [\pgt], #(1 << MMU_ENTRY_SHIFT)
95         add     \paddr, \paddr, \tmp2
96         sub     \length, \length, \tmp2
97         cbnz    \length, 1b
98         dsb     sy
99 .endm
100
101 /* int mon_mmu_map_mmio(vaddr_t vaddr, paddr_t paddr, uint64_t length) */
102 FUNCTION(mon_mmu_map_mmio)
103         mov     x9, #(MMU_L2_BLOCK_SIZE - 1)
104         mov     x10, x0         // save x0
105
106         /* check vaddr/paddr/length are BLOCK size aligned */
107         tst     x0, x9
108         cneg    x0, xzr, ne
109         cbnz    x0, done
110         tst     x1, x9
111         cneg    x0, xzr, ne
112         cbnz    x0, done
113         tst     x2, x9
114         cneg    x0, xzr, ne
115         cbnz    x0, done
116
117         mov     x0, x10         // restore x0
118
119         /* phys address of mon_second_level */
120         adr     x3, mon_second_level    // virt addr
121         ldr     x4, __mon_phys_offset
122         sub     x3, x3, x4              // phys addr
123         ldr     x4, =MMU_PTE_L2_BLOCK_MMIO_FLAGS
124
125         /* create mapping (x5, x6 are scratch) */
126         mmu_map_virt_phys x0, x1, x2, x3, x4, x5, x6
127         dsb     sy
128         isb
129         tlbi    vmalle1is
130         dsb     sy
131         isb
132
133         mov     x0, xzr
134 done:
135         ret
136
137 /* uint64_t mon_virt_phys_el3(uint64_t vaddr) */
138 FUNCTION(mon_virt_phys_el3)
139         at      s1e3r, x0
140         mrs     x0, par_el1
141         ret
142
143 /* void mon_enable_mmu() */
144 FUNCTION(mon_enable_mmu)
145 spin_wait:
146         ldr     x1, mon_pagetable_done
147         cbz     x1, spin_wait           // wait for pagetable done
148
149         ldr     x0, __mon_phys_offset
150         add     lr, lr, x0              // convert phys LR to virt
151
152         ldr     x1, =MMU_MEMORY_ATTR_INDIR
153         msr     mair_el3, x1
154         ldr     x1, =MMU_TCR_FLAGS_EL3
155         msr     tcr_el3, x1
156
157         adr     x4, mon_first_level     // phys addr
158         msr     ttbr0_el3, x4
159
160         tlbi    vmalle1is
161         dsb     sy
162         isb
163
164         mrs     x1, sctlr_el3
165         orr     x1, x1, #1
166         msr     sctlr_el3, x1           // enable MMU
167         isb
168
169         ret
170
171 /* void mon_setup_pagetable(uint64_t pbase, uint64 poff, uint64 psize) */
172 FUNCTION(mon_setup_pagetable)
173         /* check base/size alignment of carveout */
174         mmu_phys_align  x0, x2, x11
175
176         /* phys address of mon_second_level */
177         ldr     x3, =mon_second_level   // virt addr
178         sub     x3, x3, x1              // phys addr
179
180         /* clear mon_second_level */
181         mov     x4, #NUM_SECOND_LEVEL
182         mov     x13, x3         // copy mon_second_level
183 1:
184         stp     xzr, xzr, [x13], #16
185         sub     x4, x4, #2      // 2 entries at a time
186         cbnz    x4, 1b
187
188         adr     x4, mon_first_level     // phys addr
189         mov     x5, #NUM_FIRST_LEVEL
190         mov     x13, x3                 // ptr to mon_second_level
191         mmu_load_first_level x4, x13, x5
192
193         /* map MONBASE -> carveout in mon_second_level */
194         ldr     x4, =MONBASE            // virt
195         ldr     x5, =MMU_PTE_L2_BLOCK_MEM_FLAGS
196         mov     x10, x0                 // phys (carveout base)
197         mov     x12, x2                 // size
198         mov     x13, x3                 // phys pgt
199         mmu_map_virt_phys x4, x10, x12, x13, x5, x6, x7
200
201         /* indentity map carveout in mon_second_level */
202         mov     x4, x0                  // virt
203         mov     x10, x0                 // phys (carveout base)
204         mov     x12, x2                 // size
205         mov     x13, x3                 // phys pgt
206         mmu_map_virt_phys x4, x10, x12, x13, x5, x6, x7
207         dsb     sy
208         isb
209
210         /* indicate pagetables are done */
211         adr     x14, mon_pagetable_done
212         mov     x15, 0x1
213         str     x15, [x14]
214         ret
215
216 /* uint64_t mon_virt_to_phys(uint64_t vaddr) */
217 FUNCTION(mon_virt_to_phys)
218         ldr     x1, __mon_phys_offset
219         sub     x0, x0, x1
220         ret
221
222 /* uint64_t mon_phys_to_virt(uint64_t paddr) */
223 FUNCTION(mon_phys_to_virt)
224         ldr     x1, __mon_phys_offset
225         add     x0, x0, x1
226         ret
227
228 .align MMU_PAGE_GRANULE
229 mon_second_level:
230         .rept   NUM_SECOND_LEVEL
231         .quad   0
232         .endr
233
234 .align MMU_PAGE_GRANULE
235 mon_first_level:
236         .rept   NUM_FIRST_LEVEL
237         .quad   0
238         .endr
239
240 mon_pagetable_done:
241         .quad   0
242
243 .align 3
244 .global __mon_phys_offset
245 __mon_phys_offset:
246         .quad 0