1/* SPDX-License-Identifier: BSD-2-Clause */ 2/* 3 * Copyright (c) 2014, STMicroelectronics International N.V. 4 * Copyright (c) 2019, Arm Limited. All rights reserved. 5 */ 6 7#include <asm.S> 8#include <arm.h> 9#include <arm32_macros.S> 10#include <platform_config.h> 11 12 13/* size_t __get_core_pos(void); */ 14FUNC __get_core_pos , : , .identity_map 15 read_mpidr r0 16 b get_core_pos_mpidr 17END_FUNC __get_core_pos 18 19/* size_t get_core_pos_mpidr(uint32_t mpidr); */ 20FUNC get_core_pos_mpidr , : 21 mov r3, r0 22 23 /* 24 * Shift MPIDR value if it's not already shifted. 25 * Using logical shift ensures AFF0 to be filled with zeroes. 26 * This part is necessary even if CFG_CORE_THREAD_SHIFT is 0 because 27 * MT bit can be set on single threaded systems where all the AFF0 28 * values are zeroes. 29 */ 30 tst r0, #MPIDR_MT_MASK 31 lsleq r3, r0, #MPIDR_AFFINITY_BITS 32 33 /* 34 * At this point the MPIDR layout is always shifted so it looks 35 * as follows AFF2 -> cluster, AFF1 -> core, AFF0 -> thread 36 */ 37#if CFG_CORE_THREAD_SHIFT == 0 38 /* Calculate CorePos = (ClusterId * (cores/cluster)) + CoreId */ 39 ubfx r0, r3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS 40 ubfx r1, r3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS 41 add r0, r0, r1, LSL #(CFG_CORE_CLUSTER_SHIFT) 42#else 43 /* 44 * Calculate CorePos = 45 * ((ClusterId * (cores/cluster)) + CoreId) * (threads/core) + ThreadId 46 */ 47 ubfx r0, r3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS 48 ubfx r1, r3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS 49 ubfx r2, r3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS 50 add r1, r1, r2, LSL #(CFG_CORE_CLUSTER_SHIFT) 51 add r0, r0, r1, LSL #(CFG_CORE_THREAD_SHIFT) 52#endif 53 54 bx lr 55END_FUNC get_core_pos_mpidr 56 57/* Let platforms override this if needed */ 58.weak get_core_pos_mpidr 59 60/* 61 * uint32_t temp_set_mode(int cpu_mode) 62 * returns cpsr to be set 63 */ 64LOCAL_FUNC temp_set_mode , : 65 mov r1, r0 66 cmp r1, #CPSR_MODE_USR /* update mode: usr -> sys */ 67 moveq r1, #CPSR_MODE_SYS 68 cpsid aif /* disable interrupts */ 69 mrs r0, cpsr /* get cpsr with disabled its*/ 70 bic r0, #CPSR_MODE_MASK /* clear mode */ 71 orr r0, r1 /* set expected mode */ 72 bx lr 73END_FUNC temp_set_mode 74 75/* uint32_t read_mode_sp(int cpu_mode) */ 76FUNC read_mode_sp , : 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} 85END_FUNC read_mode_sp 86 87/* uint32_t read_mode_lr(int cpu_mode) */ 88FUNC read_mode_lr , : 89 push {r4, lr} 90UNWIND( .save {r4, lr}) 91 mrs r4, cpsr /* save cpsr */ 92 bl temp_set_mode 93 msr cpsr, r0 /* set the new mode */ 94 mov r0, lr /* get the function result */ 95 msr cpsr, r4 /* back to the old mode */ 96 pop {r4, pc} 97END_FUNC read_mode_lr 98