1/*
2 * Copyright (c) 2018-2020, 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 <platform_def.h>
10#include <cortex_a75.h>
11#include <neoverse_n1.h>
12#include <neoverse_v1.h>
13#include <neoverse_n2.h>
14#include <cpu_macros.S>
15
16	.globl	plat_arm_calc_core_pos
17	.globl	plat_reset_handler
18
19	/* -----------------------------------------------------
20	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
21	 *
22	 * Helper function to calculate the core position.
23	 * (ChipId * PLAT_ARM_CLUSTER_COUNT *
24	 *  CSS_SGI_MAX_CPUS_PER_CLUSTER * CSS_SGI_MAX_PE_PER_CPU) +
25	 * (ClusterId * CSS_SGI_MAX_CPUS_PER_CLUSTER * CSS_SGI_MAX_PE_PER_CPU) +
26	 * (CPUId * CSS_SGI_MAX_PE_PER_CPU) +
27	 * ThreadId
28	 *
29	 * which can be simplified as:
30	 *
31	 * ((((ChipId * PLAT_ARM_CLUSTER_COUNT) + ClusterId) *
32	 *   CSS_SGI_MAX_CPUS_PER_CLUSTER) + CPUId) * CSS_SGI_MAX_PE_PER_CPU +
33	 * ThreadId
34	 * ------------------------------------------------------
35	 */
36
37func plat_arm_calc_core_pos
38	mov	x4, x0
39
40	/*
41	 * The MT bit in MPIDR is always set for SGI platforms
42	 * and the affinity level 0 corresponds to thread affinity level.
43	 */
44
45	/* Extract individual affinity fields from MPIDR */
46	ubfx    x0, x4, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
47	ubfx    x1, x4, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
48	ubfx    x2, x4, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
49	ubfx    x3, x4, #MPIDR_AFF3_SHIFT, #MPIDR_AFFINITY_BITS
50
51	/* Compute linear position */
52	mov     x4, #PLAT_ARM_CLUSTER_COUNT
53	madd    x2, x3, x4, x2
54	mov     x4, #CSS_SGI_MAX_CPUS_PER_CLUSTER
55	madd    x1, x2, x4, x1
56	mov     x4, #CSS_SGI_MAX_PE_PER_CPU
57	madd    x0, x1, x4, x0
58	ret
59endfunc plat_arm_calc_core_pos
60
61	/* -----------------------------------------------------
62	 * void plat_reset_handler(void);
63	 *
64	 * Determine the CPU MIDR and disable power down bit for
65	 * that CPU.
66	 * -----------------------------------------------------
67	 */
68func plat_reset_handler
69	jump_if_cpu_midr CORTEX_A75_MIDR, A75
70	jump_if_cpu_midr NEOVERSE_N1_MIDR, N1
71	jump_if_cpu_midr NEOVERSE_V1_MIDR, V1
72	jump_if_cpu_midr NEOVERSE_N2_MIDR, N2
73	ret
74
75	/* -----------------------------------------------------
76	 * Disable CPU power down bit in power control register
77	 * -----------------------------------------------------
78	 */
79A75:
80	mrs	x0, CORTEX_A75_CPUPWRCTLR_EL1
81	bic	x0, x0, #CORTEX_A75_CORE_PWRDN_EN_MASK
82	msr	CORTEX_A75_CPUPWRCTLR_EL1, x0
83	isb
84	ret
85
86N1:
87	mrs	x0, NEOVERSE_N1_CPUPWRCTLR_EL1
88	bic	x0, x0, #NEOVERSE_N1_CORE_PWRDN_EN_MASK
89	msr	NEOVERSE_N1_CPUPWRCTLR_EL1, x0
90	isb
91	ret
92
93V1:
94	mrs	x0, NEOVERSE_V1_CPUPWRCTLR_EL1
95	bic	x0, x0, #NEOVERSE_V1_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
96	msr	NEOVERSE_V1_CPUPWRCTLR_EL1, x0
97	isb
98	ret
99
100N2:
101	mrs	x0, NEOVERSE_N2_CPUPWRCTLR_EL1
102	bic	x0, x0, #NEOVERSE_N2_CORE_PWRDN_EN_BIT
103	msr	NEOVERSE_N2_CPUPWRCTLR_EL1, x0
104	isb
105	ret
106endfunc plat_reset_handler
107