14ecca339SDan Handley/* 24ecca339SDan Handley * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. 34ecca339SDan Handley * 44ecca339SDan Handley * Redistribution and use in source and binary forms, with or without 54ecca339SDan Handley * modification, are permitted provided that the following conditions are met: 64ecca339SDan Handley * 74ecca339SDan Handley * Redistributions of source code must retain the above copyright notice, this 84ecca339SDan Handley * list of conditions and the following disclaimer. 94ecca339SDan Handley * 104ecca339SDan Handley * Redistributions in binary form must reproduce the above copyright notice, 114ecca339SDan Handley * this list of conditions and the following disclaimer in the documentation 124ecca339SDan Handley * and/or other materials provided with the distribution. 134ecca339SDan Handley * 144ecca339SDan Handley * Neither the name of ARM nor the names of its contributors may be used 154ecca339SDan Handley * to endorse or promote products derived from this software without specific 164ecca339SDan Handley * prior written permission. 174ecca339SDan Handley * 184ecca339SDan Handley * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 194ecca339SDan Handley * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 204ecca339SDan Handley * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 214ecca339SDan Handley * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 224ecca339SDan Handley * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 234ecca339SDan Handley * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 244ecca339SDan Handley * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 254ecca339SDan Handley * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 264ecca339SDan Handley * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 274ecca339SDan Handley * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 284ecca339SDan Handley * POSSIBILITY OF SUCH DAMAGE. 294ecca339SDan Handley */ 304ecca339SDan Handley 3197043ac9SDan Handley#include <arch.h> 324ecca339SDan Handley#include <asm_macros.S> 334ecca339SDan Handley 344ecca339SDan Handley .globl enable_irq 354ecca339SDan Handley .globl disable_irq 364ecca339SDan Handley 374ecca339SDan Handley .globl enable_fiq 384ecca339SDan Handley .globl disable_fiq 394ecca339SDan Handley 404ecca339SDan Handley .globl enable_serror 414ecca339SDan Handley .globl disable_serror 424ecca339SDan Handley 434ecca339SDan Handley .globl enable_debug_exceptions 444ecca339SDan Handley .globl disable_debug_exceptions 454ecca339SDan Handley 464ecca339SDan Handley .globl read_daif 474ecca339SDan Handley .globl write_daif 484ecca339SDan Handley 494ecca339SDan Handley .globl read_spsr 504ecca339SDan Handley .globl read_spsr_el1 514ecca339SDan Handley .globl read_spsr_el2 524ecca339SDan Handley .globl read_spsr_el3 534ecca339SDan Handley 544ecca339SDan Handley .globl write_spsr 554ecca339SDan Handley .globl write_spsr_el1 564ecca339SDan Handley .globl write_spsr_el2 574ecca339SDan Handley .globl write_spsr_el3 584ecca339SDan Handley 594ecca339SDan Handley .globl read_elr 604ecca339SDan Handley .globl read_elr_el1 614ecca339SDan Handley .globl read_elr_el2 624ecca339SDan Handley .globl read_elr_el3 634ecca339SDan Handley 644ecca339SDan Handley .globl write_elr 654ecca339SDan Handley .globl write_elr_el1 664ecca339SDan Handley .globl write_elr_el2 674ecca339SDan Handley .globl write_elr_el3 684ecca339SDan Handley 694ecca339SDan Handley .globl get_afflvl_shift 704ecca339SDan Handley .globl mpidr_mask_lower_afflvls 714ecca339SDan Handley .globl dsb 724ecca339SDan Handley .globl isb 734ecca339SDan Handley .globl sev 744ecca339SDan Handley .globl wfe 754ecca339SDan Handley .globl wfi 764ecca339SDan Handley .globl eret 774ecca339SDan Handley .globl smc 784ecca339SDan Handley 794ecca339SDan Handley .globl zeromem16 804ecca339SDan Handley .globl memcpy16 814ecca339SDan Handley 82*2f5dcfefSAndrew Thoelke .globl disable_mmu_el3 83*2f5dcfefSAndrew Thoelke .globl disable_mmu_icache_el3 84*2f5dcfefSAndrew Thoelke 854ecca339SDan Handley 864ecca339SDan Handleyfunc get_afflvl_shift 874ecca339SDan Handley cmp x0, #3 884ecca339SDan Handley cinc x0, x0, eq 894ecca339SDan Handley mov x1, #MPIDR_AFFLVL_SHIFT 904ecca339SDan Handley lsl x0, x0, x1 914ecca339SDan Handley ret 924ecca339SDan Handley 934ecca339SDan Handleyfunc mpidr_mask_lower_afflvls 944ecca339SDan Handley cmp x1, #3 954ecca339SDan Handley cinc x1, x1, eq 964ecca339SDan Handley mov x2, #MPIDR_AFFLVL_SHIFT 974ecca339SDan Handley lsl x2, x1, x2 984ecca339SDan Handley lsr x0, x0, x2 994ecca339SDan Handley lsl x0, x0, x2 1004ecca339SDan Handley ret 1014ecca339SDan Handley 1024ecca339SDan Handley /* ----------------------------------------------------- 1034ecca339SDan Handley * Asynchronous exception manipulation accessors 1044ecca339SDan Handley * ----------------------------------------------------- 1054ecca339SDan Handley */ 1064ecca339SDan Handleyfunc enable_irq 1074ecca339SDan Handley msr daifclr, #DAIF_IRQ_BIT 1084ecca339SDan Handley ret 1094ecca339SDan Handley 1104ecca339SDan Handley 1114ecca339SDan Handleyfunc enable_fiq 1124ecca339SDan Handley msr daifclr, #DAIF_FIQ_BIT 1134ecca339SDan Handley ret 1144ecca339SDan Handley 1154ecca339SDan Handley 1164ecca339SDan Handleyfunc enable_serror 1174ecca339SDan Handley msr daifclr, #DAIF_ABT_BIT 1184ecca339SDan Handley ret 1194ecca339SDan Handley 1204ecca339SDan Handley 1214ecca339SDan Handleyfunc enable_debug_exceptions 1224ecca339SDan Handley msr daifclr, #DAIF_DBG_BIT 1234ecca339SDan Handley ret 1244ecca339SDan Handley 1254ecca339SDan Handley 1264ecca339SDan Handleyfunc disable_irq 1274ecca339SDan Handley msr daifset, #DAIF_IRQ_BIT 1284ecca339SDan Handley ret 1294ecca339SDan Handley 1304ecca339SDan Handley 1314ecca339SDan Handleyfunc disable_fiq 1324ecca339SDan Handley msr daifset, #DAIF_FIQ_BIT 1334ecca339SDan Handley ret 1344ecca339SDan Handley 1354ecca339SDan Handley 1364ecca339SDan Handleyfunc disable_serror 1374ecca339SDan Handley msr daifset, #DAIF_ABT_BIT 1384ecca339SDan Handley ret 1394ecca339SDan Handley 1404ecca339SDan Handley 1414ecca339SDan Handleyfunc disable_debug_exceptions 1424ecca339SDan Handley msr daifset, #DAIF_DBG_BIT 1434ecca339SDan Handley ret 1444ecca339SDan Handley 1454ecca339SDan Handley 1464ecca339SDan Handleyfunc read_daif 1474ecca339SDan Handley mrs x0, daif 1484ecca339SDan Handley ret 1494ecca339SDan Handley 1504ecca339SDan Handley 1514ecca339SDan Handleyfunc write_daif 1524ecca339SDan Handley msr daif, x0 1534ecca339SDan Handley ret 1544ecca339SDan Handley 1554ecca339SDan Handley 1564ecca339SDan Handleyfunc read_spsr 1574ecca339SDan Handley mrs x0, CurrentEl 1584ecca339SDan Handley cmp x0, #(MODE_EL1 << MODE_EL_SHIFT) 1594ecca339SDan Handley b.eq read_spsr_el1 1604ecca339SDan Handley cmp x0, #(MODE_EL2 << MODE_EL_SHIFT) 1614ecca339SDan Handley b.eq read_spsr_el2 1624ecca339SDan Handley cmp x0, #(MODE_EL3 << MODE_EL_SHIFT) 1634ecca339SDan Handley b.eq read_spsr_el3 1644ecca339SDan Handley 1654ecca339SDan Handley 1664ecca339SDan Handleyfunc read_spsr_el1 1674ecca339SDan Handley mrs x0, spsr_el1 1684ecca339SDan Handley ret 1694ecca339SDan Handley 1704ecca339SDan Handley 1714ecca339SDan Handleyfunc read_spsr_el2 1724ecca339SDan Handley mrs x0, spsr_el2 1734ecca339SDan Handley ret 1744ecca339SDan Handley 1754ecca339SDan Handley 1764ecca339SDan Handleyfunc read_spsr_el3 1774ecca339SDan Handley mrs x0, spsr_el3 1784ecca339SDan Handley ret 1794ecca339SDan Handley 1804ecca339SDan Handley 1814ecca339SDan Handleyfunc write_spsr 1824ecca339SDan Handley mrs x1, CurrentEl 1834ecca339SDan Handley cmp x1, #(MODE_EL1 << MODE_EL_SHIFT) 1844ecca339SDan Handley b.eq write_spsr_el1 1854ecca339SDan Handley cmp x1, #(MODE_EL2 << MODE_EL_SHIFT) 1864ecca339SDan Handley b.eq write_spsr_el2 1874ecca339SDan Handley cmp x1, #(MODE_EL3 << MODE_EL_SHIFT) 1884ecca339SDan Handley b.eq write_spsr_el3 1894ecca339SDan Handley 1904ecca339SDan Handley 1914ecca339SDan Handleyfunc write_spsr_el1 1924ecca339SDan Handley msr spsr_el1, x0 1934ecca339SDan Handley ret 1944ecca339SDan Handley 1954ecca339SDan Handley 1964ecca339SDan Handleyfunc write_spsr_el2 1974ecca339SDan Handley msr spsr_el2, x0 1984ecca339SDan Handley ret 1994ecca339SDan Handley 2004ecca339SDan Handley 2014ecca339SDan Handleyfunc write_spsr_el3 2024ecca339SDan Handley msr spsr_el3, x0 2034ecca339SDan Handley ret 2044ecca339SDan Handley 2054ecca339SDan Handley 2064ecca339SDan Handleyfunc read_elr 2074ecca339SDan Handley mrs x0, CurrentEl 2084ecca339SDan Handley cmp x0, #(MODE_EL1 << MODE_EL_SHIFT) 2094ecca339SDan Handley b.eq read_elr_el1 2104ecca339SDan Handley cmp x0, #(MODE_EL2 << MODE_EL_SHIFT) 2114ecca339SDan Handley b.eq read_elr_el2 2124ecca339SDan Handley cmp x0, #(MODE_EL3 << MODE_EL_SHIFT) 2134ecca339SDan Handley b.eq read_elr_el3 2144ecca339SDan Handley 2154ecca339SDan Handley 2164ecca339SDan Handleyfunc read_elr_el1 2174ecca339SDan Handley mrs x0, elr_el1 2184ecca339SDan Handley ret 2194ecca339SDan Handley 2204ecca339SDan Handley 2214ecca339SDan Handleyfunc read_elr_el2 2224ecca339SDan Handley mrs x0, elr_el2 2234ecca339SDan Handley ret 2244ecca339SDan Handley 2254ecca339SDan Handley 2264ecca339SDan Handleyfunc read_elr_el3 2274ecca339SDan Handley mrs x0, elr_el3 2284ecca339SDan Handley ret 2294ecca339SDan Handley 2304ecca339SDan Handley 2314ecca339SDan Handleyfunc write_elr 2324ecca339SDan Handley mrs x1, CurrentEl 2334ecca339SDan Handley cmp x1, #(MODE_EL1 << MODE_EL_SHIFT) 2344ecca339SDan Handley b.eq write_elr_el1 2354ecca339SDan Handley cmp x1, #(MODE_EL2 << MODE_EL_SHIFT) 2364ecca339SDan Handley b.eq write_elr_el2 2374ecca339SDan Handley cmp x1, #(MODE_EL3 << MODE_EL_SHIFT) 2384ecca339SDan Handley b.eq write_elr_el3 2394ecca339SDan Handley 2404ecca339SDan Handley 2414ecca339SDan Handleyfunc write_elr_el1 2424ecca339SDan Handley msr elr_el1, x0 2434ecca339SDan Handley ret 2444ecca339SDan Handley 2454ecca339SDan Handley 2464ecca339SDan Handleyfunc write_elr_el2 2474ecca339SDan Handley msr elr_el2, x0 2484ecca339SDan Handley ret 2494ecca339SDan Handley 2504ecca339SDan Handley 2514ecca339SDan Handleyfunc write_elr_el3 2524ecca339SDan Handley msr elr_el3, x0 2534ecca339SDan Handley ret 2544ecca339SDan Handley 2554ecca339SDan Handley 2564ecca339SDan Handleyfunc dsb 2574ecca339SDan Handley dsb sy 2584ecca339SDan Handley ret 2594ecca339SDan Handley 2604ecca339SDan Handley 2614ecca339SDan Handleyfunc isb 2624ecca339SDan Handley isb 2634ecca339SDan Handley ret 2644ecca339SDan Handley 2654ecca339SDan Handley 2664ecca339SDan Handleyfunc sev 2674ecca339SDan Handley sev 2684ecca339SDan Handley ret 2694ecca339SDan Handley 2704ecca339SDan Handley 2714ecca339SDan Handleyfunc wfe 2724ecca339SDan Handley wfe 2734ecca339SDan Handley ret 2744ecca339SDan Handley 2754ecca339SDan Handley 2764ecca339SDan Handleyfunc wfi 2774ecca339SDan Handley wfi 2784ecca339SDan Handley ret 2794ecca339SDan Handley 2804ecca339SDan Handley 2814ecca339SDan Handleyfunc eret 2824ecca339SDan Handley eret 2834ecca339SDan Handley 2844ecca339SDan Handley 2854ecca339SDan Handleyfunc smc 2864ecca339SDan Handley smc #0 2874ecca339SDan Handley 2884ecca339SDan Handley/* ----------------------------------------------------------------------- 2894ecca339SDan Handley * void zeromem16(void *mem, unsigned int length); 2904ecca339SDan Handley * 2914ecca339SDan Handley * Initialise a memory region to 0. 2924ecca339SDan Handley * The memory address must be 16-byte aligned. 2934ecca339SDan Handley * ----------------------------------------------------------------------- 2944ecca339SDan Handley */ 2954ecca339SDan Handleyfunc zeromem16 2964ecca339SDan Handley add x2, x0, x1 2974ecca339SDan Handley/* zero 16 bytes at a time */ 2984ecca339SDan Handleyz_loop16: 2994ecca339SDan Handley sub x3, x2, x0 3004ecca339SDan Handley cmp x3, #16 3014ecca339SDan Handley b.lt z_loop1 3024ecca339SDan Handley stp xzr, xzr, [x0], #16 3034ecca339SDan Handley b z_loop16 3044ecca339SDan Handley/* zero byte per byte */ 3054ecca339SDan Handleyz_loop1: 3064ecca339SDan Handley cmp x0, x2 3074ecca339SDan Handley b.eq z_end 3084ecca339SDan Handley strb wzr, [x0], #1 3094ecca339SDan Handley b z_loop1 3104ecca339SDan Handleyz_end: ret 3114ecca339SDan Handley 3124ecca339SDan Handley 3134ecca339SDan Handley/* -------------------------------------------------------------------------- 3144ecca339SDan Handley * void memcpy16(void *dest, const void *src, unsigned int length) 3154ecca339SDan Handley * 3164ecca339SDan Handley * Copy length bytes from memory area src to memory area dest. 3174ecca339SDan Handley * The memory areas should not overlap. 3184ecca339SDan Handley * Destination and source addresses must be 16-byte aligned. 3194ecca339SDan Handley * -------------------------------------------------------------------------- 3204ecca339SDan Handley */ 3214ecca339SDan Handleyfunc memcpy16 3224ecca339SDan Handley/* copy 16 bytes at a time */ 3234ecca339SDan Handleym_loop16: 3244ecca339SDan Handley cmp x2, #16 3254ecca339SDan Handley b.lt m_loop1 3264ecca339SDan Handley ldp x3, x4, [x1], #16 3274ecca339SDan Handley stp x3, x4, [x0], #16 3284ecca339SDan Handley sub x2, x2, #16 3294ecca339SDan Handley b m_loop16 3304ecca339SDan Handley/* copy byte per byte */ 3314ecca339SDan Handleym_loop1: 3324ecca339SDan Handley cbz x2, m_end 3334ecca339SDan Handley ldrb w3, [x1], #1 3344ecca339SDan Handley strb w3, [x0], #1 3354ecca339SDan Handley subs x2, x2, #1 3364ecca339SDan Handley b.ne m_loop1 3374ecca339SDan Handleym_end: ret 338*2f5dcfefSAndrew Thoelke 339*2f5dcfefSAndrew Thoelke/* --------------------------------------------------------------------------- 340*2f5dcfefSAndrew Thoelke * Disable the MMU at EL3 341*2f5dcfefSAndrew Thoelke * This is implemented in assembler to ensure that the data cache is cleaned 342*2f5dcfefSAndrew Thoelke * and invalidated after the MMU is disabled without any intervening cacheable 343*2f5dcfefSAndrew Thoelke * data accesses 344*2f5dcfefSAndrew Thoelke * --------------------------------------------------------------------------- 345*2f5dcfefSAndrew Thoelke */ 346*2f5dcfefSAndrew Thoelke 347*2f5dcfefSAndrew Thoelkefunc disable_mmu_el3 348*2f5dcfefSAndrew Thoelke mov x1, #(SCTLR_M_BIT | SCTLR_C_BIT) 349*2f5dcfefSAndrew Thoelkedo_disable_mmu: 350*2f5dcfefSAndrew Thoelke mrs x0, sctlr_el3 351*2f5dcfefSAndrew Thoelke bic x0, x0, x1 352*2f5dcfefSAndrew Thoelke msr sctlr_el3, x0 353*2f5dcfefSAndrew Thoelke isb // ensure MMU is off 354*2f5dcfefSAndrew Thoelke mov x0, #DCCISW // DCache clean and invalidate 355*2f5dcfefSAndrew Thoelke b dcsw_op_all 356*2f5dcfefSAndrew Thoelke 357*2f5dcfefSAndrew Thoelke 358*2f5dcfefSAndrew Thoelkefunc disable_mmu_icache_el3 359*2f5dcfefSAndrew Thoelke mov x1, #(SCTLR_M_BIT | SCTLR_C_BIT | SCTLR_I_BIT) 360*2f5dcfefSAndrew Thoelke b do_disable_mmu 361*2f5dcfefSAndrew Thoelke 362