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