From e231f3a5f46f3897368521e0b61fec8cad91b35e Mon Sep 17 00:00:00 2001
From: Usama Arif <usama.arif@arm.com>
Date: Thu, 19 Sep 2019 10:54:16 +0100
Subject: [PATCH] a5ds: Hold the secondary cpus in pen rather than panic

For the secondary CPUs, hold the cpu in wfe rather then panic.
This will be needed when multicore support is added to a5ds as
the smc call will write to the hold base and signal an event to
power on the secondary CPUs.

Change-Id: I0ffc2059e9ef894c21375ca5c94def859bfa6599
Signed-off-by: Usama Arif <usama.arif@arm.com>
---
 plat/arm/board/a5ds/aarch32/a5ds_helpers.S | 82 ++++++++++++++++++++--
 plat/arm/board/a5ds/include/platform_def.h |  8 +++
 2 files changed, 83 insertions(+), 7 deletions(-)

diff --git a/plat/arm/board/a5ds/aarch32/a5ds_helpers.S b/plat/arm/board/a5ds/aarch32/a5ds_helpers.S
index 23a22d9c..ed7ad9c8 100644
--- a/plat/arm/board/a5ds/aarch32/a5ds_helpers.S
+++ b/plat/arm/board/a5ds/aarch32/a5ds_helpers.S
@@ -12,17 +12,36 @@
 	.globl	plat_get_my_entrypoint
 	.globl	plat_is_my_cpu_primary
 
-	/* --------------------------------------------------------------------
+	/* -----------------------------------------------------
 	 * void plat_secondary_cold_boot_setup (void);
 	 *
-	 * For AArch32, cold-booting secondary CPUs is not yet
-	 * implemented and they panic.
-	 * --------------------------------------------------------------------
+	 * This function performs any platform specific actions
+	 * needed for a secondary cpu after a cold reset e.g
+	 * mark the cpu's presence, mechanism to place it in a
+	 * holding pen etc.
+	 * -----------------------------------------------------
 	 */
 func plat_secondary_cold_boot_setup
-cb_panic:
-	wfi
-	b	cb_panic
+	/* Calculate address of our hold entry */
+	bl	plat_my_core_pos
+	lsl	r0, r0, #A5DS_HOLD_ENTRY_SHIFT
+	mov_imm	r2, A5DS_HOLD_BASE
+	/* Clear the value stored in the hold address for the specific core */
+	mov_imm	r3, A5DS_HOLD_STATE_WAIT
+	str	r3, [r2, r0]
+	dmb	ish
+
+	/* Wait until we have a go */
+poll_mailbox:
+	ldr	r1, [r2, r0]
+	cmp	r1, #A5DS_HOLD_STATE_WAIT
+	beq	1f
+	mov_imm	r0, A5DS_TRUSTED_MAILBOX_BASE
+	ldr	r1, [r0]
+	bx	r1
+1:
+	wfe
+	b	poll_mailbox
 endfunc plat_secondary_cold_boot_setup
 
 	/* ---------------------------------------------------------------------
@@ -56,3 +75,52 @@ func plat_is_my_cpu_primary
 	movne	r0, #0
 	bx	lr
 endfunc plat_is_my_cpu_primary
+
+	/* ---------------------------------------------------------------------
+	 * Loads MPIDR in r0 and calls plat_arm_calc_core_pos
+	 * ---------------------------------------------------------------------
+	 */
+func plat_my_core_pos
+	ldcopr	r0, MPIDR
+	b	plat_arm_calc_core_pos
+
+endfunc plat_my_core_pos
+
+	/* ---------------------------------------------------------------------
+	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
+	 *
+	 * Function to calculate the core position on A5DS.
+	 *
+	 * (ClusterId * A5DS_MAX_CPUS_PER_CLUSTER * A5DS_MAX_PE_PER_CPU) +
+	 * (CPUId * A5DS_MAX_PE_PER_CPU) +
+	 * ThreadId
+	 *
+	 * which can be simplified as:
+	 *
+	 * ((ClusterId * A5DS_MAX_CPUS_PER_CLUSTER + CPUId) * A5DS_MAX_PE_PER_CPU)
+	 * + ThreadId
+	 * ---------------------------------------------------------------------
+	 */
+func plat_arm_calc_core_pos
+	mov	r3, r0
+
+	/*
+	 * Check for MT bit in MPIDR. If not set, shift MPIDR to left to make it
+	 * look as if in a multi-threaded implementation
+	 */
+	tst	r0, #MPIDR_MT_MASK
+	lsleq	r3, r0, #MPIDR_AFFINITY_BITS
+
+	/* Extract individual affinity fields from MPIDR */
+	ubfx	r0, r3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx	r1, r3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx	r2, r3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
+
+	/* Compute linear position */
+	mov	r3, #A5DS_MAX_CPUS_PER_CLUSTER
+	mla	r1, r2, r3, r1
+	mov	r3, #A5DS_MAX_PE_PER_CPU
+	mla	r0, r1, r3, r0
+
+	bx	lr
+endfunc plat_arm_calc_core_pos
diff --git a/plat/arm/board/a5ds/include/platform_def.h b/plat/arm/board/a5ds/include/platform_def.h
index db65c377..13c19343 100644
--- a/plat/arm/board/a5ds/include/platform_def.h
+++ b/plat/arm/board/a5ds/include/platform_def.h
@@ -325,6 +325,14 @@
 
 /* Mailbox base address */
 #define A5DS_TRUSTED_MAILBOX_BASE	A5DS_SHARED_RAM_BASE
+#define A5DS_TRUSTED_MAILBOX_SIZE	(8 + A5DS_HOLD_SIZE)
+#define A5DS_HOLD_BASE		(A5DS_TRUSTED_MAILBOX_BASE + 8)
+#define A5DS_HOLD_SIZE		(PLATFORM_CORE_COUNT * \
+					 A5DS_HOLD_ENTRY_SIZE)
+#define A5DS_HOLD_ENTRY_SHIFT	3
+#define A5DS_HOLD_ENTRY_SIZE	(1 << A5DS_HOLD_ENTRY_SHIFT)
+#define A5DS_HOLD_STATE_WAIT	0
+#define A5DS_HOLD_STATE_GO	1
 
 /*
  * GIC related constants to cater for GICv2
-- 
2.30.2