1/*
2 * Copyright (c) 2019, 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 <platform_def.h>
10
11	.globl	plat_secondary_cold_boot_setup
12	.globl	plat_get_my_entrypoint
13	.globl	plat_is_my_cpu_primary
14
15	/* -----------------------------------------------------
16	 * void plat_secondary_cold_boot_setup (void);
17	 *
18	 * This function performs any platform specific actions
19	 * needed for a secondary cpu after a cold reset e.g
20	 * mark the cpu's presence, mechanism to place it in a
21	 * holding pen etc.
22	 * -----------------------------------------------------
23	 */
24func plat_secondary_cold_boot_setup
25	/* Calculate address of our hold entry */
26	bl	plat_my_core_pos
27	lsl	r0, r0, #A5DS_HOLD_ENTRY_SHIFT
28	mov_imm	r2, A5DS_HOLD_BASE
29	/* Clear the value stored in the hold address for the specific core */
30	mov_imm	r3, A5DS_HOLD_STATE_WAIT
31	str	r3, [r2, r0]
32	dmb	ish
33
34	/* Wait until we have a go */
35poll_mailbox:
36	ldr	r1, [r2, r0]
37	cmp	r1, #A5DS_HOLD_STATE_WAIT
38	beq	1f
39	mov_imm	r0, A5DS_TRUSTED_MAILBOX_BASE
40	ldr	r1, [r0]
41	bx	r1
421:
43	wfe
44	b	poll_mailbox
45endfunc plat_secondary_cold_boot_setup
46
47	/* ---------------------------------------------------------------------
48	 * unsigned long plat_get_my_entrypoint (void);
49	 *
50	 * Main job of this routine is to distinguish between a cold and warm
51	 * boot.
52	 * ---------------------------------------------------------------------
53	 */
54func plat_get_my_entrypoint
55	/* TODO support warm boot */
56	/* Cold reset */
57	mov	r0, #0
58	bx	lr
59
60endfunc plat_get_my_entrypoint
61
62	/* -----------------------------------------------------
63	 * unsigned int plat_is_my_cpu_primary (void);
64	 *
65	 * Find out whether the current cpu is the primary
66	 * cpu.
67	 * -----------------------------------------------------
68	 */
69func plat_is_my_cpu_primary
70	ldcopr	r0, MPIDR
71	ldr	r1, =MPIDR_AFFINITY_MASK
72	and	r0, r1
73	cmp	r0, #0
74	moveq	r0, #1
75	movne	r0, #0
76	bx	lr
77endfunc plat_is_my_cpu_primary
78
79	/* ---------------------------------------------------------------------
80	 * Loads MPIDR in r0 and calls plat_arm_calc_core_pos
81	 * ---------------------------------------------------------------------
82	 */
83func plat_my_core_pos
84	ldcopr	r0, MPIDR
85	b	plat_arm_calc_core_pos
86
87endfunc plat_my_core_pos
88
89	/* ---------------------------------------------------------------------
90	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
91	 *
92	 * Function to calculate the core position on A5DS.
93	 *
94	 * (ClusterId * A5DS_MAX_CPUS_PER_CLUSTER * A5DS_MAX_PE_PER_CPU) +
95	 * (CPUId * A5DS_MAX_PE_PER_CPU) +
96	 * ThreadId
97	 *
98	 * which can be simplified as:
99	 *
100	 * ((ClusterId * A5DS_MAX_CPUS_PER_CLUSTER + CPUId) * A5DS_MAX_PE_PER_CPU)
101	 * + ThreadId
102	 * ---------------------------------------------------------------------
103	 */
104func plat_arm_calc_core_pos
105	mov	r3, r0
106
107	/*
108	 * Check for MT bit in MPIDR. If not set, shift MPIDR to left to make it
109	 * look as if in a multi-threaded implementation
110	 */
111	tst	r0, #MPIDR_MT_MASK
112	lsleq	r3, r0, #MPIDR_AFFINITY_BITS
113
114	/* Extract individual affinity fields from MPIDR */
115	ubfx	r0, r3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
116	ubfx	r1, r3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
117	ubfx	r2, r3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
118
119	/* Compute linear position */
120	mov	r3, #A5DS_MAX_CPUS_PER_CLUSTER
121	mla	r1, r2, r3, r1
122	mov	r3, #A5DS_MAX_PE_PER_CPU
123	mla	r0, r1, r3, r0
124
125	bx	lr
126endfunc plat_arm_calc_core_pos
127