1/* SPDX-License-Identifier: BSD-2-Clause */ 2/* 3 * Copyright (c) 2015, Linaro Limited 4 * Copyright (c) 2019, Arm Limited. All rights reserved. 5 */ 6 7#include <asm.S> 8#include <arm.h> 9#include <platform_config.h> 10 11/* size_t __get_core_pos(void); */ 12FUNC __get_core_pos , : , .identity_map 13 mrs x0, mpidr_el1 14 b get_core_pos_mpidr 15END_FUNC __get_core_pos 16 17/* size_t get_core_pos_mpidr(uint32_t mpidr); */ 18/* Let platforms override this if needed */ 19WEAK_FUNC get_core_pos_mpidr , : 20#if CFG_CORE_SEL2_SPMC 21 mov x1, #MPIDR_VCPU_MASK 22 and x0, x0, x1 23#else /* CFG_CORE_SEL2_SPMC */ 24 /* 25 * Shift MPIDR value if it's not already shifted. 26 * Using logical shift ensures AFF0 to be filled with zeroes. 27 * This part is necessary even if CFG_CORE_THREAD_SHIFT is 0 because 28 * MT bit can be set on single threaded systems where all the AFF0 29 * values are zeroes. 30 */ 31 tst x0, #MPIDR_MT_MASK 32 lsl x3, x0, #MPIDR_AFFINITY_BITS 33 csel x3, x3, x0, eq 34 35 /* 36 * At this point the MPIDR layout is always shifted so it looks 37 * as follows AFF2 -> cluster, AFF1 -> core, AFF0 -> thread 38 */ 39#if CFG_CORE_THREAD_SHIFT == 0 40 /* Calculate CorePos = (ClusterId * (cores/cluster)) + CoreId */ 41 ubfx x0, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS 42 ubfx x1, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS 43 add x0, x0, x1, LSL #(CFG_CORE_CLUSTER_SHIFT) 44#else 45 /* 46 * Calculate CorePos = 47 * ((ClusterId * (cores/cluster)) + CoreId) * (threads/core) + ThreadId 48 */ 49 ubfx x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS 50 ubfx x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS 51 ubfx x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS 52 add x1, x1, x2, LSL #(CFG_CORE_CLUSTER_SHIFT) 53 add x0, x0, x1, LSL #(CFG_CORE_THREAD_SHIFT) 54#endif 55#endif 56 57 ret 58END_FUNC get_core_pos_mpidr 59 60BTI(emit_aarch64_feature_1_and GNU_PROPERTY_AARCH64_FEATURE_1_BTI) 61