1/* 2 * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions are met: 6 * 7 * Redistributions of source code must retain the above copyright notice, this 8 * list of conditions and the following disclaimer. 9 * 10 * Redistributions in binary form must reproduce the above copyright notice, 11 * this list of conditions and the following disclaimer in the documentation 12 * and/or other materials provided with the distribution. 13 * 14 * Neither the name of ARM nor the names of its contributors may be used 15 * to endorse or promote products derived from this software without specific 16 * prior written permission. 17 * 18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 * POSSIBILITY OF SUCH DAMAGE. 29 */ 30 31#include <arch.h> 32#include <asm_macros.S> 33 34 .globl enable_irq 35 .globl disable_irq 36 37 .globl enable_fiq 38 .globl disable_fiq 39 40 .globl enable_serror 41 .globl disable_serror 42 43 .globl enable_debug_exceptions 44 .globl disable_debug_exceptions 45 46 .globl read_daif 47 .globl write_daif 48 49 .globl read_spsr 50 .globl read_spsr_el1 51 .globl read_spsr_el2 52 .globl read_spsr_el3 53 54 .globl write_spsr 55 .globl write_spsr_el1 56 .globl write_spsr_el2 57 .globl write_spsr_el3 58 59 .globl read_elr 60 .globl read_elr_el1 61 .globl read_elr_el2 62 .globl read_elr_el3 63 64 .globl write_elr 65 .globl write_elr_el1 66 .globl write_elr_el2 67 .globl write_elr_el3 68 69 .globl get_afflvl_shift 70 .globl mpidr_mask_lower_afflvls 71 .globl dsb 72 .globl isb 73 .globl sev 74 .globl wfe 75 .globl wfi 76 .globl eret 77 .globl smc 78 79 .globl zeromem16 80 .globl memcpy16 81 82 83func get_afflvl_shift 84 cmp x0, #3 85 cinc x0, x0, eq 86 mov x1, #MPIDR_AFFLVL_SHIFT 87 lsl x0, x0, x1 88 ret 89 90func mpidr_mask_lower_afflvls 91 cmp x1, #3 92 cinc x1, x1, eq 93 mov x2, #MPIDR_AFFLVL_SHIFT 94 lsl x2, x1, x2 95 lsr x0, x0, x2 96 lsl x0, x0, x2 97 ret 98 99 /* ----------------------------------------------------- 100 * Asynchronous exception manipulation accessors 101 * ----------------------------------------------------- 102 */ 103func enable_irq 104 msr daifclr, #DAIF_IRQ_BIT 105 ret 106 107 108func enable_fiq 109 msr daifclr, #DAIF_FIQ_BIT 110 ret 111 112 113func enable_serror 114 msr daifclr, #DAIF_ABT_BIT 115 ret 116 117 118func enable_debug_exceptions 119 msr daifclr, #DAIF_DBG_BIT 120 ret 121 122 123func disable_irq 124 msr daifset, #DAIF_IRQ_BIT 125 ret 126 127 128func disable_fiq 129 msr daifset, #DAIF_FIQ_BIT 130 ret 131 132 133func disable_serror 134 msr daifset, #DAIF_ABT_BIT 135 ret 136 137 138func disable_debug_exceptions 139 msr daifset, #DAIF_DBG_BIT 140 ret 141 142 143func read_daif 144 mrs x0, daif 145 ret 146 147 148func write_daif 149 msr daif, x0 150 ret 151 152 153func read_spsr 154 mrs x0, CurrentEl 155 cmp x0, #(MODE_EL1 << MODE_EL_SHIFT) 156 b.eq read_spsr_el1 157 cmp x0, #(MODE_EL2 << MODE_EL_SHIFT) 158 b.eq read_spsr_el2 159 cmp x0, #(MODE_EL3 << MODE_EL_SHIFT) 160 b.eq read_spsr_el3 161 162 163func read_spsr_el1 164 mrs x0, spsr_el1 165 ret 166 167 168func read_spsr_el2 169 mrs x0, spsr_el2 170 ret 171 172 173func read_spsr_el3 174 mrs x0, spsr_el3 175 ret 176 177 178func write_spsr 179 mrs x1, CurrentEl 180 cmp x1, #(MODE_EL1 << MODE_EL_SHIFT) 181 b.eq write_spsr_el1 182 cmp x1, #(MODE_EL2 << MODE_EL_SHIFT) 183 b.eq write_spsr_el2 184 cmp x1, #(MODE_EL3 << MODE_EL_SHIFT) 185 b.eq write_spsr_el3 186 187 188func write_spsr_el1 189 msr spsr_el1, x0 190 ret 191 192 193func write_spsr_el2 194 msr spsr_el2, x0 195 ret 196 197 198func write_spsr_el3 199 msr spsr_el3, x0 200 ret 201 202 203func read_elr 204 mrs x0, CurrentEl 205 cmp x0, #(MODE_EL1 << MODE_EL_SHIFT) 206 b.eq read_elr_el1 207 cmp x0, #(MODE_EL2 << MODE_EL_SHIFT) 208 b.eq read_elr_el2 209 cmp x0, #(MODE_EL3 << MODE_EL_SHIFT) 210 b.eq read_elr_el3 211 212 213func read_elr_el1 214 mrs x0, elr_el1 215 ret 216 217 218func read_elr_el2 219 mrs x0, elr_el2 220 ret 221 222 223func read_elr_el3 224 mrs x0, elr_el3 225 ret 226 227 228func write_elr 229 mrs x1, CurrentEl 230 cmp x1, #(MODE_EL1 << MODE_EL_SHIFT) 231 b.eq write_elr_el1 232 cmp x1, #(MODE_EL2 << MODE_EL_SHIFT) 233 b.eq write_elr_el2 234 cmp x1, #(MODE_EL3 << MODE_EL_SHIFT) 235 b.eq write_elr_el3 236 237 238func write_elr_el1 239 msr elr_el1, x0 240 ret 241 242 243func write_elr_el2 244 msr elr_el2, x0 245 ret 246 247 248func write_elr_el3 249 msr elr_el3, x0 250 ret 251 252 253func dsb 254 dsb sy 255 ret 256 257 258func isb 259 isb 260 ret 261 262 263func sev 264 sev 265 ret 266 267 268func wfe 269 wfe 270 ret 271 272 273func wfi 274 wfi 275 ret 276 277 278func eret 279 eret 280 281 282func smc 283 smc #0 284 285/* ----------------------------------------------------------------------- 286 * void zeromem16(void *mem, unsigned int length); 287 * 288 * Initialise a memory region to 0. 289 * The memory address must be 16-byte aligned. 290 * ----------------------------------------------------------------------- 291 */ 292func zeromem16 293 add x2, x0, x1 294/* zero 16 bytes at a time */ 295z_loop16: 296 sub x3, x2, x0 297 cmp x3, #16 298 b.lt z_loop1 299 stp xzr, xzr, [x0], #16 300 b z_loop16 301/* zero byte per byte */ 302z_loop1: 303 cmp x0, x2 304 b.eq z_end 305 strb wzr, [x0], #1 306 b z_loop1 307z_end: ret 308 309 310/* -------------------------------------------------------------------------- 311 * void memcpy16(void *dest, const void *src, unsigned int length) 312 * 313 * Copy length bytes from memory area src to memory area dest. 314 * The memory areas should not overlap. 315 * Destination and source addresses must be 16-byte aligned. 316 * -------------------------------------------------------------------------- 317 */ 318func memcpy16 319/* copy 16 bytes at a time */ 320m_loop16: 321 cmp x2, #16 322 b.lt m_loop1 323 ldp x3, x4, [x1], #16 324 stp x3, x4, [x0], #16 325 sub x2, x2, #16 326 b m_loop16 327/* copy byte per byte */ 328m_loop1: 329 cbz x2, m_end 330 ldrb w3, [x1], #1 331 strb w3, [x0], #1 332 subs x2, x2, #1 333 b.ne m_loop1 334m_end: ret 335