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