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); */ 18FUNC get_core_pos_mpidr , : 19 /* 20 * Shift MPIDR value if it's not already shifted. 21 * Using logical shift ensures AFF0 to be filled with zeroes. 22 * This part is necessary even if CFG_CORE_THREAD_SHIFT is 0 because 23 * MT bit can be set on single threaded systems where all the AFF0 24 * values are zeroes. 25 */ 26 tst x0, #MPIDR_MT_MASK 27 lsl x3, x0, #MPIDR_AFFINITY_BITS 28 csel x3, x3, x0, eq 29 30 /* 31 * At this point the MPIDR layout is always shifted so it looks 32 * as follows AFF2 -> cluster, AFF1 -> core, AFF0 -> thread 33 */ 34#if CFG_CORE_THREAD_SHIFT == 0 35 /* Calculate CorePos = (ClusterId * (cores/cluster)) + CoreId */ 36 ubfx x0, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS 37 ubfx x1, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS 38 add x0, x0, x1, LSL #(CFG_CORE_CLUSTER_SHIFT) 39#else 40 /* 41 * Calculate CorePos = 42 * ((ClusterId * (cores/cluster)) + CoreId) * (threads/core) + ThreadId 43 */ 44 ubfx x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS 45 ubfx x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS 46 ubfx x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS 47 add x1, x1, x2, LSL #(CFG_CORE_CLUSTER_SHIFT) 48 add x0, x0, x1, LSL #(CFG_CORE_THREAD_SHIFT) 49#endif 50 51 ret 52END_FUNC get_core_pos_mpidr 53 54/* Let platforms override this if needed */ 55.weak get_core_pos_mpidr 56