1/* SPDX-License-Identifier: BSD-2-Clause */ 2/* 3 * Copyright (c) 2014, STMicroelectronics International N.V. 4 * All rights reserved. 5 * 6 * Redistribution and use in source and binary forms, with or without 7 * modification, are permitted provided that the following conditions are met: 8 * 9 * 1. Redistributions of source code must retain the above copyright notice, 10 * this list of conditions and the following disclaimer. 11 * 12 * 2. Redistributions in binary form must reproduce the above copyright notice, 13 * this list of conditions and the following disclaimer in the documentation 14 * and/or other materials provided with the distribution. 15 * 16 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 * POSSIBILITY OF SUCH DAMAGE. 27 */ 28 29#include <asm.S> 30#include <arm.h> 31#include <arm32_macros.S> 32#include <kernel/unwind.h> 33#include <platform_config.h> 34 35/* Let platforms override this if needed */ 36.weak get_core_pos_mpidr 37 38/* size_t get_core_pos(void); */ 39FUNC get_core_pos , : 40UNWIND( .fnstart) 41 read_mpidr r0 42 b get_core_pos_mpidr 43UNWIND( .fnend) 44END_FUNC get_core_pos 45 46/* size_t get_core_pos_mpidr(uint32_t mpidr); */ 47FUNC get_core_pos_mpidr , : 48UNWIND( .fnstart) 49 /* Calculate CorePos = (ClusterId * (cores/cluster)) + CoreId */ 50 and r1, r0, #MPIDR_CPU_MASK 51 and r0, r0, #MPIDR_CLUSTER_MASK 52 add r0, r1, r0, LSR #(MPIDR_CLUSTER_SHIFT - CFG_CORE_CLUSTER_SHIFT) 53 bx lr 54UNWIND( .fnend) 55END_FUNC get_core_pos_mpidr 56 57/* 58 * uint32_t temp_set_mode(int cpu_mode) 59 * returns cpsr to be set 60 */ 61LOCAL_FUNC temp_set_mode , : 62UNWIND( .fnstart) 63 mov r1, r0 64 cmp r1, #CPSR_MODE_USR /* update mode: usr -> sys */ 65 moveq r1, #CPSR_MODE_SYS 66 cpsid aif /* disable interrupts */ 67 mrs r0, cpsr /* get cpsr with disabled its*/ 68 bic r0, #CPSR_MODE_MASK /* clear mode */ 69 orr r0, r1 /* set expected mode */ 70 bx lr 71UNWIND( .fnend) 72END_FUNC temp_set_mode 73 74/* uint32_t read_mode_sp(int cpu_mode) */ 75FUNC read_mode_sp , : 76UNWIND( .fnstart) 77 push {r4, lr} 78UNWIND( .save {r4, lr}) 79 mrs r4, cpsr /* save cpsr */ 80 bl temp_set_mode 81 msr cpsr, r0 /* set the new mode */ 82 mov r0, sp /* get the function result */ 83 msr cpsr, r4 /* back to the old mode */ 84 pop {r4, pc} 85UNWIND( .fnend) 86END_FUNC read_mode_sp 87 88/* uint32_t read_mode_lr(int cpu_mode) */ 89FUNC read_mode_lr , : 90UNWIND( .fnstart) 91 push {r4, lr} 92UNWIND( .save {r4, lr}) 93 mrs r4, cpsr /* save cpsr */ 94 bl temp_set_mode 95 msr cpsr, r0 /* set the new mode */ 96 mov r0, lr /* get the function result */ 97 msr cpsr, r4 /* back to the old mode */ 98 pop {r4, pc} 99UNWIND( .fnend) 100END_FUNC read_mode_lr 101