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 31*97043ac9SDan 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 824ecca339SDan Handley 834ecca339SDan Handleyfunc get_afflvl_shift 844ecca339SDan Handley cmp x0, #3 854ecca339SDan Handley cinc x0, x0, eq 864ecca339SDan Handley mov x1, #MPIDR_AFFLVL_SHIFT 874ecca339SDan Handley lsl x0, x0, x1 884ecca339SDan Handley ret 894ecca339SDan Handley 904ecca339SDan Handleyfunc mpidr_mask_lower_afflvls 914ecca339SDan Handley cmp x1, #3 924ecca339SDan Handley cinc x1, x1, eq 934ecca339SDan Handley mov x2, #MPIDR_AFFLVL_SHIFT 944ecca339SDan Handley lsl x2, x1, x2 954ecca339SDan Handley lsr x0, x0, x2 964ecca339SDan Handley lsl x0, x0, x2 974ecca339SDan Handley ret 984ecca339SDan Handley 994ecca339SDan Handley /* ----------------------------------------------------- 1004ecca339SDan Handley * Asynchronous exception manipulation accessors 1014ecca339SDan Handley * ----------------------------------------------------- 1024ecca339SDan Handley */ 1034ecca339SDan Handleyfunc enable_irq 1044ecca339SDan Handley msr daifclr, #DAIF_IRQ_BIT 1054ecca339SDan Handley ret 1064ecca339SDan Handley 1074ecca339SDan Handley 1084ecca339SDan Handleyfunc enable_fiq 1094ecca339SDan Handley msr daifclr, #DAIF_FIQ_BIT 1104ecca339SDan Handley ret 1114ecca339SDan Handley 1124ecca339SDan Handley 1134ecca339SDan Handleyfunc enable_serror 1144ecca339SDan Handley msr daifclr, #DAIF_ABT_BIT 1154ecca339SDan Handley ret 1164ecca339SDan Handley 1174ecca339SDan Handley 1184ecca339SDan Handleyfunc enable_debug_exceptions 1194ecca339SDan Handley msr daifclr, #DAIF_DBG_BIT 1204ecca339SDan Handley ret 1214ecca339SDan Handley 1224ecca339SDan Handley 1234ecca339SDan Handleyfunc disable_irq 1244ecca339SDan Handley msr daifset, #DAIF_IRQ_BIT 1254ecca339SDan Handley ret 1264ecca339SDan Handley 1274ecca339SDan Handley 1284ecca339SDan Handleyfunc disable_fiq 1294ecca339SDan Handley msr daifset, #DAIF_FIQ_BIT 1304ecca339SDan Handley ret 1314ecca339SDan Handley 1324ecca339SDan Handley 1334ecca339SDan Handleyfunc disable_serror 1344ecca339SDan Handley msr daifset, #DAIF_ABT_BIT 1354ecca339SDan Handley ret 1364ecca339SDan Handley 1374ecca339SDan Handley 1384ecca339SDan Handleyfunc disable_debug_exceptions 1394ecca339SDan Handley msr daifset, #DAIF_DBG_BIT 1404ecca339SDan Handley ret 1414ecca339SDan Handley 1424ecca339SDan Handley 1434ecca339SDan Handleyfunc read_daif 1444ecca339SDan Handley mrs x0, daif 1454ecca339SDan Handley ret 1464ecca339SDan Handley 1474ecca339SDan Handley 1484ecca339SDan Handleyfunc write_daif 1494ecca339SDan Handley msr daif, x0 1504ecca339SDan Handley ret 1514ecca339SDan Handley 1524ecca339SDan Handley 1534ecca339SDan Handleyfunc read_spsr 1544ecca339SDan Handley mrs x0, CurrentEl 1554ecca339SDan Handley cmp x0, #(MODE_EL1 << MODE_EL_SHIFT) 1564ecca339SDan Handley b.eq read_spsr_el1 1574ecca339SDan Handley cmp x0, #(MODE_EL2 << MODE_EL_SHIFT) 1584ecca339SDan Handley b.eq read_spsr_el2 1594ecca339SDan Handley cmp x0, #(MODE_EL3 << MODE_EL_SHIFT) 1604ecca339SDan Handley b.eq read_spsr_el3 1614ecca339SDan Handley 1624ecca339SDan Handley 1634ecca339SDan Handleyfunc read_spsr_el1 1644ecca339SDan Handley mrs x0, spsr_el1 1654ecca339SDan Handley ret 1664ecca339SDan Handley 1674ecca339SDan Handley 1684ecca339SDan Handleyfunc read_spsr_el2 1694ecca339SDan Handley mrs x0, spsr_el2 1704ecca339SDan Handley ret 1714ecca339SDan Handley 1724ecca339SDan Handley 1734ecca339SDan Handleyfunc read_spsr_el3 1744ecca339SDan Handley mrs x0, spsr_el3 1754ecca339SDan Handley ret 1764ecca339SDan Handley 1774ecca339SDan Handley 1784ecca339SDan Handleyfunc write_spsr 1794ecca339SDan Handley mrs x1, CurrentEl 1804ecca339SDan Handley cmp x1, #(MODE_EL1 << MODE_EL_SHIFT) 1814ecca339SDan Handley b.eq write_spsr_el1 1824ecca339SDan Handley cmp x1, #(MODE_EL2 << MODE_EL_SHIFT) 1834ecca339SDan Handley b.eq write_spsr_el2 1844ecca339SDan Handley cmp x1, #(MODE_EL3 << MODE_EL_SHIFT) 1854ecca339SDan Handley b.eq write_spsr_el3 1864ecca339SDan Handley 1874ecca339SDan Handley 1884ecca339SDan Handleyfunc write_spsr_el1 1894ecca339SDan Handley msr spsr_el1, x0 1904ecca339SDan Handley isb 1914ecca339SDan Handley ret 1924ecca339SDan Handley 1934ecca339SDan Handley 1944ecca339SDan Handleyfunc write_spsr_el2 1954ecca339SDan Handley msr spsr_el2, x0 1964ecca339SDan Handley isb 1974ecca339SDan Handley ret 1984ecca339SDan Handley 1994ecca339SDan Handley 2004ecca339SDan Handleyfunc write_spsr_el3 2014ecca339SDan Handley msr spsr_el3, x0 2024ecca339SDan Handley isb 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 isb 2444ecca339SDan Handley ret 2454ecca339SDan Handley 2464ecca339SDan Handley 2474ecca339SDan Handleyfunc write_elr_el2 2484ecca339SDan Handley msr elr_el2, x0 2494ecca339SDan Handley isb 2504ecca339SDan Handley ret 2514ecca339SDan Handley 2524ecca339SDan Handley 2534ecca339SDan Handleyfunc write_elr_el3 2544ecca339SDan Handley msr elr_el3, x0 2554ecca339SDan Handley isb 2564ecca339SDan Handley ret 2574ecca339SDan Handley 2584ecca339SDan Handley 2594ecca339SDan Handleyfunc dsb 2604ecca339SDan Handley dsb sy 2614ecca339SDan Handley ret 2624ecca339SDan Handley 2634ecca339SDan Handley 2644ecca339SDan Handleyfunc isb 2654ecca339SDan Handley isb 2664ecca339SDan Handley ret 2674ecca339SDan Handley 2684ecca339SDan Handley 2694ecca339SDan Handleyfunc sev 2704ecca339SDan Handley sev 2714ecca339SDan Handley ret 2724ecca339SDan Handley 2734ecca339SDan Handley 2744ecca339SDan Handleyfunc wfe 2754ecca339SDan Handley wfe 2764ecca339SDan Handley ret 2774ecca339SDan Handley 2784ecca339SDan Handley 2794ecca339SDan Handleyfunc wfi 2804ecca339SDan Handley wfi 2814ecca339SDan Handley ret 2824ecca339SDan Handley 2834ecca339SDan Handley 2844ecca339SDan Handleyfunc eret 2854ecca339SDan Handley eret 2864ecca339SDan Handley 2874ecca339SDan Handley 2884ecca339SDan Handleyfunc smc 2894ecca339SDan Handley smc #0 2904ecca339SDan Handley 2914ecca339SDan Handley/* ----------------------------------------------------------------------- 2924ecca339SDan Handley * void zeromem16(void *mem, unsigned int length); 2934ecca339SDan Handley * 2944ecca339SDan Handley * Initialise a memory region to 0. 2954ecca339SDan Handley * The memory address must be 16-byte aligned. 2964ecca339SDan Handley * ----------------------------------------------------------------------- 2974ecca339SDan Handley */ 2984ecca339SDan Handleyfunc zeromem16 2994ecca339SDan Handley add x2, x0, x1 3004ecca339SDan Handley/* zero 16 bytes at a time */ 3014ecca339SDan Handleyz_loop16: 3024ecca339SDan Handley sub x3, x2, x0 3034ecca339SDan Handley cmp x3, #16 3044ecca339SDan Handley b.lt z_loop1 3054ecca339SDan Handley stp xzr, xzr, [x0], #16 3064ecca339SDan Handley b z_loop16 3074ecca339SDan Handley/* zero byte per byte */ 3084ecca339SDan Handleyz_loop1: 3094ecca339SDan Handley cmp x0, x2 3104ecca339SDan Handley b.eq z_end 3114ecca339SDan Handley strb wzr, [x0], #1 3124ecca339SDan Handley b z_loop1 3134ecca339SDan Handleyz_end: ret 3144ecca339SDan Handley 3154ecca339SDan Handley 3164ecca339SDan Handley/* -------------------------------------------------------------------------- 3174ecca339SDan Handley * void memcpy16(void *dest, const void *src, unsigned int length) 3184ecca339SDan Handley * 3194ecca339SDan Handley * Copy length bytes from memory area src to memory area dest. 3204ecca339SDan Handley * The memory areas should not overlap. 3214ecca339SDan Handley * Destination and source addresses must be 16-byte aligned. 3224ecca339SDan Handley * -------------------------------------------------------------------------- 3234ecca339SDan Handley */ 3244ecca339SDan Handleyfunc memcpy16 3254ecca339SDan Handley/* copy 16 bytes at a time */ 3264ecca339SDan Handleym_loop16: 3274ecca339SDan Handley cmp x2, #16 3284ecca339SDan Handley b.lt m_loop1 3294ecca339SDan Handley ldp x3, x4, [x1], #16 3304ecca339SDan Handley stp x3, x4, [x0], #16 3314ecca339SDan Handley sub x2, x2, #16 3324ecca339SDan Handley b m_loop16 3334ecca339SDan Handley/* copy byte per byte */ 3344ecca339SDan Handleym_loop1: 3354ecca339SDan Handley cbz x2, m_end 3364ecca339SDan Handley ldrb w3, [x1], #1 3374ecca339SDan Handley strb w3, [x0], #1 3384ecca339SDan Handley subs x2, x2, #1 3394ecca339SDan Handley b.ne m_loop1 3404ecca339SDan Handleym_end: ret 341