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