/* SPDX-License-Identifier: BSD-2-Clause */
/*
 * Copyright (c) 2015, Linaro Limited
 * Copyright (c) 2019, Arm Limited. All rights reserved.
 */

#include <asm.S>
#include <arm.h>
#include <platform_config.h>

/* size_t __get_core_pos(void); */
FUNC __get_core_pos , : , .identity_map
	mrs	x0, mpidr_el1
	b get_core_pos_mpidr
END_FUNC __get_core_pos

/* size_t get_core_pos_mpidr(uint32_t mpidr); */
FUNC get_core_pos_mpidr , :
	/*
	 * Shift MPIDR value if it's not already shifted.
	 * Using logical shift ensures AFF0 to be filled with zeroes.
	 * This part is necessary even if CFG_CORE_THREAD_SHIFT is 0 because
	 * MT bit can be set on single threaded systems where all the AFF0
	 * values are zeroes.
	 */
	tst	x0, #MPIDR_MT_MASK
	lsl	x3, x0, #MPIDR_AFFINITY_BITS
	csel	x3, x3, x0, eq

	/*
	 * At this point the MPIDR layout is always shifted so it looks
	 * as follows AFF2 -> cluster, AFF1 -> core, AFF0 -> thread
	 */
#if CFG_CORE_THREAD_SHIFT == 0
	/* Calculate CorePos = (ClusterId * (cores/cluster)) + CoreId */
	ubfx	x0, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
	ubfx	x1, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
	add	x0, x0, x1, LSL #(CFG_CORE_CLUSTER_SHIFT)
#else
	/*
	 * Calculate CorePos =
	 * ((ClusterId * (cores/cluster)) + CoreId) * (threads/core) + ThreadId
	 */
	ubfx	x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
	ubfx	x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
	ubfx	x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
	add	x1, x1, x2, LSL #(CFG_CORE_CLUSTER_SHIFT)
	add	x0, x0, x1, LSL #(CFG_CORE_THREAD_SHIFT)
#endif

	ret
END_FUNC get_core_pos_mpidr

/* Let platforms override this if needed */
.weak get_core_pos_mpidr