blob: 3c64d3caac7b6b549a2d35dbfe0b86f190707c48 [file] [log] [blame]
Achin Gupta4f6ad662013-10-25 09:08:21 +01001/*
Douglas Raillard21362a92016-12-02 13:51:54 +00002 * Copyright (c) 2013-2017, ARM Limited and Contributors. All rights reserved.
Achin Gupta4f6ad662013-10-25 09:08:21 +01003 *
dp-armfa3cf0b2017-05-03 09:38:09 +01004 * SPDX-License-Identifier: BSD-3-Clause
Achin Gupta4f6ad662013-10-25 09:08:21 +01005 */
6
Dan Handley2bd4ef22014-04-09 13:14:54 +01007#include <arch.h>
Andrew Thoelke38bde412014-03-18 13:46:55 +00008#include <asm_macros.S>
Soby Mathew041f62a2014-07-14 16:58:03 +01009#include <assert_macros.S>
Achin Gupta4a826dd2013-11-25 14:00:56 +000010
Achin Gupta4f6ad662013-10-25 09:08:21 +010011 .globl get_afflvl_shift
12 .globl mpidr_mask_lower_afflvls
Achin Gupta4f6ad662013-10-25 09:08:21 +010013 .globl eret
14 .globl smc
15
Douglas Raillard21362a92016-12-02 13:51:54 +000016 .globl zero_normalmem
17 .globl zeromem
Sandrine Bailleux65f546a2013-11-28 09:43:06 +000018 .globl zeromem16
19 .globl memcpy16
Achin Gupta4f6ad662013-10-25 09:08:21 +010020
Antonio Nino Diaz4613d5f2017-10-05 15:19:42 +010021 .globl disable_mmu_el1
Andrew Thoelke438c63a2014-04-28 12:06:18 +010022 .globl disable_mmu_el3
Antonio Nino Diaz4613d5f2017-10-05 15:19:42 +010023 .globl disable_mmu_icache_el1
Andrew Thoelke438c63a2014-04-28 12:06:18 +010024 .globl disable_mmu_icache_el3
25
Andrew Thoelke3f78dc32014-06-02 15:44:43 +010026#if SUPPORT_VFP
27 .globl enable_vfp
28#endif
29
Andrew Thoelke38bde412014-03-18 13:46:55 +000030func get_afflvl_shift
Achin Gupta4f6ad662013-10-25 09:08:21 +010031 cmp x0, #3
32 cinc x0, x0, eq
33 mov x1, #MPIDR_AFFLVL_SHIFT
34 lsl x0, x0, x1
35 ret
Kévin Petita877c252015-03-24 14:03:57 +000036endfunc get_afflvl_shift
Achin Gupta4f6ad662013-10-25 09:08:21 +010037
Andrew Thoelke38bde412014-03-18 13:46:55 +000038func mpidr_mask_lower_afflvls
Achin Gupta4f6ad662013-10-25 09:08:21 +010039 cmp x1, #3
40 cinc x1, x1, eq
41 mov x2, #MPIDR_AFFLVL_SHIFT
42 lsl x2, x1, x2
43 lsr x0, x0, x2
44 lsl x0, x0, x2
45 ret
Kévin Petita877c252015-03-24 14:03:57 +000046endfunc mpidr_mask_lower_afflvls
Achin Gupta4f6ad662013-10-25 09:08:21 +010047
Achin Gupta4f6ad662013-10-25 09:08:21 +010048
Andrew Thoelke38bde412014-03-18 13:46:55 +000049func eret
Achin Gupta4f6ad662013-10-25 09:08:21 +010050 eret
Kévin Petita877c252015-03-24 14:03:57 +000051endfunc eret
Achin Gupta4f6ad662013-10-25 09:08:21 +010052
53
Andrew Thoelke38bde412014-03-18 13:46:55 +000054func smc
Achin Gupta4f6ad662013-10-25 09:08:21 +010055 smc #0
Kévin Petita877c252015-03-24 14:03:57 +000056endfunc smc
Sandrine Bailleux65f546a2013-11-28 09:43:06 +000057
58/* -----------------------------------------------------------------------
59 * void zeromem16(void *mem, unsigned int length);
60 *
61 * Initialise a memory region to 0.
62 * The memory address must be 16-byte aligned.
Douglas Raillard21362a92016-12-02 13:51:54 +000063 * NOTE: This function is deprecated and zeromem should be used instead.
Sandrine Bailleux65f546a2013-11-28 09:43:06 +000064 * -----------------------------------------------------------------------
65 */
Douglas Raillard21362a92016-12-02 13:51:54 +000066.equ zeromem16, zeromem
67
68/* -----------------------------------------------------------------------
69 * void zero_normalmem(void *mem, unsigned int length);
70 *
71 * Initialise a region in normal memory to 0. This functions complies with the
72 * AAPCS and can be called from C code.
73 *
74 * NOTE: MMU must be enabled when using this function as it can only operate on
75 * normal memory. It is intended to be mainly used from C code when MMU
76 * is usually enabled.
77 * -----------------------------------------------------------------------
78 */
79.equ zero_normalmem, zeromem_dczva
80
81/* -----------------------------------------------------------------------
82 * void zeromem(void *mem, unsigned int length);
83 *
84 * Initialise a region of device memory to 0. This functions complies with the
85 * AAPCS and can be called from C code.
86 *
87 * NOTE: When data caches and MMU are enabled, zero_normalmem can usually be
88 * used instead for faster zeroing.
89 *
90 * -----------------------------------------------------------------------
91 */
92func zeromem
93 /* x2 is the address past the last zeroed address */
94 add x2, x0, x1
95 /*
96 * Uses the fallback path that does not use DC ZVA instruction and
97 * therefore does not need enabled MMU
98 */
99 b .Lzeromem_dczva_fallback_entry
100endfunc zeromem
101
102/* -----------------------------------------------------------------------
103 * void zeromem_dczva(void *mem, unsigned int length);
104 *
105 * Fill a region of normal memory of size "length" in bytes with null bytes.
106 * MMU must be enabled and the memory be of
107 * normal type. This is because this function internally uses the DC ZVA
108 * instruction, which generates an Alignment fault if used on any type of
109 * Device memory (see section D3.4.9 of the ARMv8 ARM, issue k). When the MMU
110 * is disabled, all memory behaves like Device-nGnRnE memory (see section
111 * D4.2.8), hence the requirement on the MMU being enabled.
112 * NOTE: The code assumes that the block size as defined in DCZID_EL0
113 * register is at least 16 bytes.
114 *
115 * -----------------------------------------------------------------------
116 */
117func zeromem_dczva
118
119 /*
120 * The function consists of a series of loops that zero memory one byte
121 * at a time, 16 bytes at a time or using the DC ZVA instruction to
122 * zero aligned block of bytes, which is assumed to be more than 16.
123 * In the case where the DC ZVA instruction cannot be used or if the
124 * first 16 bytes loop would overflow, there is fallback path that does
125 * not use DC ZVA.
126 * Note: The fallback path is also used by the zeromem function that
127 * branches to it directly.
128 *
129 * +---------+ zeromem_dczva
130 * | entry |
131 * +----+----+
132 * |
133 * v
134 * +---------+
135 * | checks |>o-------+ (If any check fails, fallback)
136 * +----+----+ |
137 * | |---------------+
138 * v | Fallback path |
139 * +------+------+ |---------------+
140 * | 1 byte loop | |
141 * +------+------+ .Lzeromem_dczva_initial_1byte_aligned_end
142 * | |
143 * v |
144 * +-------+-------+ |
145 * | 16 bytes loop | |
146 * +-------+-------+ |
147 * | |
148 * v |
149 * +------+------+ .Lzeromem_dczva_blocksize_aligned
150 * | DC ZVA loop | |
151 * +------+------+ |
152 * +--------+ | |
153 * | | | |
154 * | v v |
155 * | +-------+-------+ .Lzeromem_dczva_final_16bytes_aligned
156 * | | 16 bytes loop | |
157 * | +-------+-------+ |
158 * | | |
159 * | v |
160 * | +------+------+ .Lzeromem_dczva_final_1byte_aligned
161 * | | 1 byte loop | |
162 * | +-------------+ |
163 * | | |
164 * | v |
165 * | +---+--+ |
166 * | | exit | |
167 * | +------+ |
168 * | |
169 * | +--------------+ +------------------+ zeromem
170 * | | +----------------| zeromem function |
171 * | | | +------------------+
172 * | v v
173 * | +-------------+ .Lzeromem_dczva_fallback_entry
174 * | | 1 byte loop |
175 * | +------+------+
176 * | |
177 * +-----------+
178 */
179
180 /*
181 * Readable names for registers
182 *
183 * Registers x0, x1 and x2 are also set by zeromem which
184 * branches into the fallback path directly, so cursor, length and
185 * stop_address should not be retargeted to other registers.
186 */
187 cursor .req x0 /* Start address and then current address */
188 length .req x1 /* Length in bytes of the region to zero out */
189 /* Reusing x1 as length is never used after block_mask is set */
190 block_mask .req x1 /* Bitmask of the block size read in DCZID_EL0 */
191 stop_address .req x2 /* Address past the last zeroed byte */
192 block_size .req x3 /* Size of a block in bytes as read in DCZID_EL0 */
193 tmp1 .req x4
194 tmp2 .req x5
195
Antonio Nino Diaz7c65c1e2017-04-20 09:58:28 +0100196#if ENABLE_ASSERTIONS
Douglas Raillard21362a92016-12-02 13:51:54 +0000197 /*
198 * Check for M bit (MMU enabled) of the current SCTLR_EL(1|3)
199 * register value and panic if the MMU is disabled.
200 */
Masahiro Yamadaffe92542018-02-01 13:17:29 +0900201#if defined(IMAGE_BL1) || defined(IMAGE_BL31) || (defined(IMAGE_BL2) && BL2_AT_EL3)
Douglas Raillard21362a92016-12-02 13:51:54 +0000202 mrs tmp1, sctlr_el3
203#else
204 mrs tmp1, sctlr_el1
Soby Mathew041f62a2014-07-14 16:58:03 +0100205#endif
Douglas Raillard21362a92016-12-02 13:51:54 +0000206
207 tst tmp1, #SCTLR_M_BIT
208 ASM_ASSERT(ne)
Antonio Nino Diaz7c65c1e2017-04-20 09:58:28 +0100209#endif /* ENABLE_ASSERTIONS */
Douglas Raillard21362a92016-12-02 13:51:54 +0000210
211 /* stop_address is the address past the last to zero */
212 add stop_address, cursor, length
213
214 /*
215 * Get block_size = (log2(<block size>) >> 2) (see encoding of
216 * dczid_el0 reg)
217 */
218 mrs block_size, dczid_el0
219
220 /*
221 * Select the 4 lowest bits and convert the extracted log2(<block size
222 * in words>) to <block size in bytes>
223 */
224 ubfx block_size, block_size, #0, #4
225 mov tmp2, #(1 << 2)
226 lsl block_size, tmp2, block_size
227
Antonio Nino Diaz7c65c1e2017-04-20 09:58:28 +0100228#if ENABLE_ASSERTIONS
Douglas Raillard21362a92016-12-02 13:51:54 +0000229 /*
230 * Assumes block size is at least 16 bytes to avoid manual realignment
231 * of the cursor at the end of the DCZVA loop.
232 */
233 cmp block_size, #16
234 ASM_ASSERT(hs)
235#endif
236 /*
237 * Not worth doing all the setup for a region less than a block and
238 * protects against zeroing a whole block when the area to zero is
239 * smaller than that. Also, as it is assumed that the block size is at
240 * least 16 bytes, this also protects the initial aligning loops from
241 * trying to zero 16 bytes when length is less than 16.
242 */
243 cmp length, block_size
244 b.lo .Lzeromem_dczva_fallback_entry
245
246 /*
247 * Calculate the bitmask of the block alignment. It will never
248 * underflow as the block size is between 4 bytes and 2kB.
249 * block_mask = block_size - 1
250 */
251 sub block_mask, block_size, #1
252
253 /*
254 * length alias should not be used after this point unless it is
255 * defined as a register other than block_mask's.
256 */
257 .unreq length
258
259 /*
260 * If the start address is already aligned to zero block size, go
261 * straight to the cache zeroing loop. This is safe because at this
262 * point, the length cannot be smaller than a block size.
263 */
264 tst cursor, block_mask
265 b.eq .Lzeromem_dczva_blocksize_aligned
266
267 /*
268 * Calculate the first block-size-aligned address. It is assumed that
269 * the zero block size is at least 16 bytes. This address is the last
270 * address of this initial loop.
271 */
272 orr tmp1, cursor, block_mask
273 add tmp1, tmp1, #1
274
275 /*
276 * If the addition overflows, skip the cache zeroing loops. This is
277 * quite unlikely however.
278 */
279 cbz tmp1, .Lzeromem_dczva_fallback_entry
280
281 /*
282 * If the first block-size-aligned address is past the last address,
283 * fallback to the simpler code.
284 */
285 cmp tmp1, stop_address
286 b.hi .Lzeromem_dczva_fallback_entry
287
288 /*
289 * If the start address is already aligned to 16 bytes, skip this loop.
290 * It is safe to do this because tmp1 (the stop address of the initial
291 * 16 bytes loop) will never be greater than the final stop address.
292 */
293 tst cursor, #0xf
294 b.eq .Lzeromem_dczva_initial_1byte_aligned_end
295
296 /* Calculate the next address aligned to 16 bytes */
297 orr tmp2, cursor, #0xf
298 add tmp2, tmp2, #1
299 /* If it overflows, fallback to the simple path (unlikely) */
300 cbz tmp2, .Lzeromem_dczva_fallback_entry
301 /*
302 * Next aligned address cannot be after the stop address because the
303 * length cannot be smaller than 16 at this point.
304 */
305
306 /* First loop: zero byte per byte */
3071:
308 strb wzr, [cursor], #1
309 cmp cursor, tmp2
310 b.ne 1b
311.Lzeromem_dczva_initial_1byte_aligned_end:
312
313 /*
314 * Second loop: we need to zero 16 bytes at a time from cursor to tmp1
315 * before being able to use the code that deals with block-size-aligned
316 * addresses.
317 */
318 cmp cursor, tmp1
319 b.hs 2f
3201:
321 stp xzr, xzr, [cursor], #16
322 cmp cursor, tmp1
323 b.lo 1b
3242:
325
326 /*
327 * Third loop: zero a block at a time using DC ZVA cache block zeroing
328 * instruction.
329 */
330.Lzeromem_dczva_blocksize_aligned:
331 /*
332 * Calculate the last block-size-aligned address. If the result equals
333 * to the start address, the loop will exit immediately.
334 */
335 bic tmp1, stop_address, block_mask
336
337 cmp cursor, tmp1
338 b.hs 2f
3391:
340 /* Zero the block containing the cursor */
341 dc zva, cursor
342 /* Increment the cursor by the size of a block */
343 add cursor, cursor, block_size
344 cmp cursor, tmp1
345 b.lo 1b
3462:
347
348 /*
349 * Fourth loop: zero 16 bytes at a time and then byte per byte the
350 * remaining area
351 */
352.Lzeromem_dczva_final_16bytes_aligned:
353 /*
354 * Calculate the last 16 bytes aligned address. It is assumed that the
355 * block size will never be smaller than 16 bytes so that the current
356 * cursor is aligned to at least 16 bytes boundary.
357 */
358 bic tmp1, stop_address, #15
359
360 cmp cursor, tmp1
361 b.hs 2f
3621:
363 stp xzr, xzr, [cursor], #16
364 cmp cursor, tmp1
365 b.lo 1b
3662:
367
368 /* Fifth and final loop: zero byte per byte */
369.Lzeromem_dczva_final_1byte_aligned:
370 cmp cursor, stop_address
371 b.eq 2f
3721:
373 strb wzr, [cursor], #1
374 cmp cursor, stop_address
375 b.ne 1b
3762:
Kévin Petita877c252015-03-24 14:03:57 +0000377 ret
Douglas Raillard21362a92016-12-02 13:51:54 +0000378
379 /* Fallback for unaligned start addresses */
380.Lzeromem_dczva_fallback_entry:
381 /*
382 * If the start address is already aligned to 16 bytes, skip this loop.
383 */
384 tst cursor, #0xf
385 b.eq .Lzeromem_dczva_final_16bytes_aligned
386
387 /* Calculate the next address aligned to 16 bytes */
388 orr tmp1, cursor, #15
389 add tmp1, tmp1, #1
390 /* If it overflows, fallback to byte per byte zeroing */
391 cbz tmp1, .Lzeromem_dczva_final_1byte_aligned
392 /* If the next aligned address is after the stop address, fall back */
393 cmp tmp1, stop_address
394 b.hs .Lzeromem_dczva_final_1byte_aligned
395
396 /* Fallback entry loop: zero byte per byte */
3971:
398 strb wzr, [cursor], #1
399 cmp cursor, tmp1
400 b.ne 1b
401
402 b .Lzeromem_dczva_final_16bytes_aligned
Sandrine Bailleux65f546a2013-11-28 09:43:06 +0000403
Douglas Raillard21362a92016-12-02 13:51:54 +0000404 .unreq cursor
405 /*
406 * length is already unreq'ed to reuse the register for another
407 * variable.
408 */
409 .unreq stop_address
410 .unreq block_size
411 .unreq block_mask
412 .unreq tmp1
413 .unreq tmp2
414endfunc zeromem_dczva
Sandrine Bailleux65f546a2013-11-28 09:43:06 +0000415
416/* --------------------------------------------------------------------------
417 * void memcpy16(void *dest, const void *src, unsigned int length)
418 *
419 * Copy length bytes from memory area src to memory area dest.
420 * The memory areas should not overlap.
421 * Destination and source addresses must be 16-byte aligned.
422 * --------------------------------------------------------------------------
423 */
Andrew Thoelke38bde412014-03-18 13:46:55 +0000424func memcpy16
Antonio Nino Diaz7c65c1e2017-04-20 09:58:28 +0100425#if ENABLE_ASSERTIONS
Soby Mathew041f62a2014-07-14 16:58:03 +0100426 orr x3, x0, x1
427 tst x3, #0xf
428 ASM_ASSERT(eq)
429#endif
Sandrine Bailleux65f546a2013-11-28 09:43:06 +0000430/* copy 16 bytes at a time */
431m_loop16:
432 cmp x2, #16
Douglas Raillard861be8e2016-12-02 13:56:06 +0000433 b.lo m_loop1
Sandrine Bailleux65f546a2013-11-28 09:43:06 +0000434 ldp x3, x4, [x1], #16
435 stp x3, x4, [x0], #16
436 sub x2, x2, #16
437 b m_loop16
438/* copy byte per byte */
439m_loop1:
440 cbz x2, m_end
441 ldrb w3, [x1], #1
442 strb w3, [x0], #1
443 subs x2, x2, #1
444 b.ne m_loop1
Kévin Petita877c252015-03-24 14:03:57 +0000445m_end:
446 ret
447endfunc memcpy16
Andrew Thoelke438c63a2014-04-28 12:06:18 +0100448
449/* ---------------------------------------------------------------------------
450 * Disable the MMU at EL3
Andrew Thoelke438c63a2014-04-28 12:06:18 +0100451 * ---------------------------------------------------------------------------
452 */
453
454func disable_mmu_el3
455 mov x1, #(SCTLR_M_BIT | SCTLR_C_BIT)
Antonio Nino Diaz4613d5f2017-10-05 15:19:42 +0100456do_disable_mmu_el3:
Andrew Thoelke438c63a2014-04-28 12:06:18 +0100457 mrs x0, sctlr_el3
458 bic x0, x0, x1
459 msr sctlr_el3, x0
Antonio Nino Diaz4613d5f2017-10-05 15:19:42 +0100460 isb /* ensure MMU is off */
Achin Guptae9c4a642015-09-11 16:03:13 +0100461 dsb sy
462 ret
Kévin Petita877c252015-03-24 14:03:57 +0000463endfunc disable_mmu_el3
Andrew Thoelke438c63a2014-04-28 12:06:18 +0100464
465
466func disable_mmu_icache_el3
467 mov x1, #(SCTLR_M_BIT | SCTLR_C_BIT | SCTLR_I_BIT)
Antonio Nino Diaz4613d5f2017-10-05 15:19:42 +0100468 b do_disable_mmu_el3
Kévin Petita877c252015-03-24 14:03:57 +0000469endfunc disable_mmu_icache_el3
Andrew Thoelke438c63a2014-04-28 12:06:18 +0100470
Andrew Thoelke3f78dc32014-06-02 15:44:43 +0100471/* ---------------------------------------------------------------------------
Antonio Nino Diaz4613d5f2017-10-05 15:19:42 +0100472 * Disable the MMU at EL1
473 * ---------------------------------------------------------------------------
474 */
475
476func disable_mmu_el1
477 mov x1, #(SCTLR_M_BIT | SCTLR_C_BIT)
478do_disable_mmu_el1:
479 mrs x0, sctlr_el1
480 bic x0, x0, x1
481 msr sctlr_el1, x0
482 isb /* ensure MMU is off */
483 dsb sy
484 ret
485endfunc disable_mmu_el1
486
487
488func disable_mmu_icache_el1
489 mov x1, #(SCTLR_M_BIT | SCTLR_C_BIT | SCTLR_I_BIT)
490 b do_disable_mmu_el1
491endfunc disable_mmu_icache_el1
492
493/* ---------------------------------------------------------------------------
Andrew Thoelke3f78dc32014-06-02 15:44:43 +0100494 * Enable the use of VFP at EL3
495 * ---------------------------------------------------------------------------
496 */
497#if SUPPORT_VFP
498func enable_vfp
499 mrs x0, cpacr_el1
500 orr x0, x0, #CPACR_VFP_BITS
501 msr cpacr_el1, x0
502 mrs x0, cptr_el3
503 mov x1, #AARCH64_CPTR_TFP
504 bic x0, x0, x1
505 msr cptr_el3, x0
506 isb
507 ret
Kévin Petita877c252015-03-24 14:03:57 +0000508endfunc enable_vfp
Andrew Thoelke3f78dc32014-06-02 15:44:43 +0100509#endif