1/*
2 * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#include <arch.h>
8#include <asm_macros.S>
9#include <neoverse_n1.h>
10#include <cpu_macros.S>
11#include <platform_def.h>
12
13	.globl	plat_arm_calc_core_pos
14	.globl	plat_reset_handler
15
16	/* -----------------------------------------------------
17	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
18	 *
19	 * Helper function to calculate the core position.
20	 * ((ChipId * N1SDP_MAX_CLUSTERS_PER_CHIP + ClusterId) *
21	 * N1SDP_MAX_CPUS_PER_CLUSTER * N1SDP_MAX_PE_PER_CPU) +
22	 * (CPUId * N1SDP_MAX_PE_PER_CPU) + ThreadId
23	 *
24	 * which can be simplified as:
25	 *
26	 * (((ChipId * N1SDP_MAX_CLUSTERS_PER_CHIP + ClusterId) *
27	 * N1SDP_MAX_CPUS_PER_CLUSTER + CPUId) * N1SDP_MAX_PE_PER_CPU) +
28	 * ThreadId
29	 * ------------------------------------------------------
30	 */
31
32func plat_arm_calc_core_pos
33	mov	x4, x0
34
35	/*
36	 * The MT bit in MPIDR is always set for n1sdp and the
37	 * affinity level 0 corresponds to thread affinity level.
38	 */
39
40	/* Extract individual affinity fields from MPIDR */
41	ubfx	x0, x4, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
42	ubfx	x1, x4, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
43	ubfx	x2, x4, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
44	ubfx	x3, x4, #MPIDR_AFF3_SHIFT, #MPIDR_AFFINITY_BITS
45
46	/* Compute linear position */
47	mov	x4, #N1SDP_MAX_CLUSTERS_PER_CHIP
48	madd	x2, x3, x4, x2
49	mov	x4, #N1SDP_MAX_CPUS_PER_CLUSTER
50	madd	x1, x2, x4, x1
51	mov	x4, #N1SDP_MAX_PE_PER_CPU
52	madd	x0, x1, x4, x0
53	ret
54endfunc plat_arm_calc_core_pos
55
56	/* -----------------------------------------------------
57	 * void plat_reset_handler(void);
58	 *
59	 * Determine the CPU MIDR and disable power down bit for
60	 * that CPU.
61	 * -----------------------------------------------------
62	 */
63
64func plat_reset_handler
65	jump_if_cpu_midr NEOVERSE_N1_MIDR, N1
66	ret
67
68	/* -----------------------------------------------------
69	 * Disable CPU power down bit in power control register
70	 * -----------------------------------------------------
71	 */
72N1:
73	mrs	x0, NEOVERSE_N1_CPUPWRCTLR_EL1
74	bic	x0, x0, #NEOVERSE_N1_CORE_PWRDN_EN_MASK
75	msr	NEOVERSE_N1_CPUPWRCTLR_EL1, x0
76	isb
77	ret
78endfunc plat_reset_handler
79