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