1/*
2 * Copyright (c) 2020, Arm Limited. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#include <arch.h>
8#include <asm_macros.S>
9#include <cpu_macros.S>
10#include <rainier.h>
11
12#include <platform_def.h>
13
14	.globl	plat_arm_calc_core_pos
15	.globl	plat_reset_handler
16
17	/* -----------------------------------------------------
18	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
19	 *
20	 * Helper function to calculate the core position.
21	 * ((ChipId * MORELLO_MAX_CLUSTERS_PER_CHIP + ClusterId) *
22	 * MORELLO_MAX_CPUS_PER_CLUSTER * MORELLO_MAX_PE_PER_CPU) +
23	 * (CPUId * MORELLO_MAX_PE_PER_CPU) + ThreadId
24	 *
25	 * which can be simplified as:
26	 *
27	 * (((ChipId * MORELLO_MAX_CLUSTERS_PER_CHIP + ClusterId) *
28	 * MORELLO_MAX_CPUS_PER_CLUSTER + CPUId) * MORELLO_MAX_PE_PER_CPU) +
29	 * ThreadId
30	 * ------------------------------------------------------
31	 */
32
33func plat_arm_calc_core_pos
34	mov	x4, x0
35
36	/*
37	 * The MT bit in MPIDR is always set for morello and the
38	 * affinity level 0 corresponds to thread affinity level.
39	 */
40
41	/* Extract individual affinity fields from MPIDR */
42	ubfx	x0, x4, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
43	ubfx	x1, x4, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
44	ubfx	x2, x4, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
45	ubfx	x3, x4, #MPIDR_AFF3_SHIFT, #MPIDR_AFFINITY_BITS
46
47	/* Compute linear position */
48	mov	x4, #MORELLO_MAX_CLUSTERS_PER_CHIP
49	madd	x2, x3, x4, x2
50	mov	x4, #MORELLO_MAX_CPUS_PER_CLUSTER
51	madd	x1, x2, x4, x1
52	mov	x4, #MORELLO_MAX_PE_PER_CPU
53	madd	x0, x1, x4, x0
54	ret
55endfunc plat_arm_calc_core_pos
56