1/* SPDX-License-Identifier: BSD-2-Clause */ 2/* 3 * Copyright (c) 2014, STMicroelectronics International N.V. 4 */ 5 6#include <asm.S> 7#include <arm.h> 8#include <arm32_macros.S> 9#include <kernel/unwind.h> 10#include <platform_config.h> 11 12/* Let platforms override this if needed */ 13.weak get_core_pos_mpidr 14 15/* size_t __get_core_pos(void); */ 16FUNC __get_core_pos , : 17UNWIND( .fnstart) 18 read_mpidr r0 19 b get_core_pos_mpidr 20UNWIND( .fnend) 21END_FUNC __get_core_pos 22 23/* size_t get_core_pos_mpidr(uint32_t mpidr); */ 24FUNC get_core_pos_mpidr , : 25UNWIND( .fnstart) 26 /* Calculate CorePos = (ClusterId * (cores/cluster)) + CoreId */ 27 and r1, r0, #MPIDR_CPU_MASK 28 and r0, r0, #MPIDR_CLUSTER_MASK 29 add r0, r1, r0, LSR #(MPIDR_CLUSTER_SHIFT - CFG_CORE_CLUSTER_SHIFT) 30 bx lr 31UNWIND( .fnend) 32END_FUNC get_core_pos_mpidr 33 34/* 35 * uint32_t temp_set_mode(int cpu_mode) 36 * returns cpsr to be set 37 */ 38LOCAL_FUNC temp_set_mode , : 39UNWIND( .fnstart) 40 mov r1, r0 41 cmp r1, #CPSR_MODE_USR /* update mode: usr -> sys */ 42 moveq r1, #CPSR_MODE_SYS 43 cpsid aif /* disable interrupts */ 44 mrs r0, cpsr /* get cpsr with disabled its*/ 45 bic r0, #CPSR_MODE_MASK /* clear mode */ 46 orr r0, r1 /* set expected mode */ 47 bx lr 48UNWIND( .fnend) 49END_FUNC temp_set_mode 50 51/* uint32_t read_mode_sp(int cpu_mode) */ 52FUNC read_mode_sp , : 53UNWIND( .fnstart) 54 push {r4, lr} 55UNWIND( .save {r4, lr}) 56 mrs r4, cpsr /* save cpsr */ 57 bl temp_set_mode 58 msr cpsr, r0 /* set the new mode */ 59 mov r0, sp /* get the function result */ 60 msr cpsr, r4 /* back to the old mode */ 61 pop {r4, pc} 62UNWIND( .fnend) 63END_FUNC read_mode_sp 64 65/* uint32_t read_mode_lr(int cpu_mode) */ 66FUNC read_mode_lr , : 67UNWIND( .fnstart) 68 push {r4, lr} 69UNWIND( .save {r4, lr}) 70 mrs r4, cpsr /* save cpsr */ 71 bl temp_set_mode 72 msr cpsr, r0 /* set the new mode */ 73 mov r0, lr /* get the function result */ 74 msr cpsr, r4 /* back to the old mode */ 75 pop {r4, pc} 76UNWIND( .fnend) 77END_FUNC read_mode_lr 78