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>
diff --git a/plat/arm/board/a5ds/aarch32/a5ds_helpers.S b/plat/arm/board/a5ds/aarch32/a5ds_helpers.S
index 23a22d9..ed7ad9c 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 @@
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 db65c37..13c1934 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
