Merge changes from topic "a5ds-multicore" into integration

* changes:
  a5ds: add multicore support
  a5ds: Hold the secondary cpus in pen rather than panic

3 files changed, 125 insertions, 12 deletions

diff --git a/plat/arm/board/a5ds/a5ds_pm.c b/plat/arm/board/a5ds/a5ds_pm.c
index 5fd443b1..98de77d1 100644
--- a/plat/arm/board/a5ds/a5ds_pm.c
+++ b/plat/arm/board/a5ds/a5ds_pm.c
@@ -6,6 +6,38 @@
 
 #include <lib/psci/psci.h>
 #include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+#include <drivers/arm/gicv2.h>
+
+/*******************************************************************************
+ * Platform handler called when a power domain is about to be turned on. The
+ * mpidr determines the CPU to be turned on.
+ ******************************************************************************/
+static int a5ds_pwr_domain_on(u_register_t mpidr)
+{
+	unsigned int pos = plat_core_pos_by_mpidr(mpidr);
+	uint64_t *hold_base = (uint64_t *)A5DS_HOLD_BASE;
+
+	hold_base[pos] = A5DS_HOLD_STATE_GO;
+	dsbish();
+	sev();
+
+	return PSCI_E_SUCCESS;
+}
+
+/*******************************************************************************
+ * Platform handler called when a power domain has just been powered on after
+ * being turned off earlier. The target_state encodes the low power state that
+ * each level has woken up from.
+ ******************************************************************************/
+void a5ds_pwr_domain_on_finish(const psci_power_state_t *target_state)
+{
+	/* TODO: This setup is needed only after a cold boot*/
+	gicv2_pcpu_distif_init();
+
+	/* Enable the gic cpu interface */
+	gicv2_cpuif_enable();
+}
 
 /*******************************************************************************
  * Export the platform handlers via a5ds_psci_pm_ops. The ARM Standard
@@ -14,11 +46,16 @@
 plat_psci_ops_t a5ds_psci_pm_ops = {
 	/* dummy struct */
 	.validate_ns_entrypoint = NULL,
+	.pwr_domain_on = a5ds_pwr_domain_on,
+	.pwr_domain_on_finish = a5ds_pwr_domain_on_finish
 };
 
 int __init plat_setup_psci_ops(uintptr_t sec_entrypoint,
 				const plat_psci_ops_t **psci_ops)
 {
+	uintptr_t *mailbox = (void *)A5DS_TRUSTED_MAILBOX_BASE;
+	*mailbox = sec_entrypoint;
+
 	*psci_ops = &a5ds_psci_pm_ops;
 
 	return 0;
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..e9e4b9ae 100644
--- a/plat/arm/board/a5ds/include/platform_def.h
+++ b/plat/arm/board/a5ds/include/platform_def.h
@@ -97,9 +97,9 @@
 /* Default number of threads per CPU on A5DS */
 #define A5DS_MAX_PE_PER_CPU	1
 
-#define A5DS_CORE_COUNT 1
+#define A5DS_CORE_COUNT		4
 
-#define A5DS_PRIMARY_CPU			0x0
+#define A5DS_PRIMARY_CPU	0x0
 
 #define FLASH1_BASE			UL(0x8000000)
 #define FLASH1_SIZE			UL(0x2800000)
@@ -229,11 +229,11 @@
 #define BL32_LIMIT			(ARM_BL_RAM_BASE + ARM_BL_RAM_SIZE)
 
 /* Required platform porting definitions */
-#define PLATFORM_CORE_COUNT 1
-#define PLAT_NUM_PWR_DOMAINS		(A5DS_CLUSTER_COUNT + \
+#define PLATFORM_CORE_COUNT	A5DS_CORE_COUNT
+#define PLAT_NUM_PWR_DOMAINS	(A5DS_CLUSTER_COUNT + \
 					PLATFORM_CORE_COUNT) + 1
 
-#define PLAT_MAX_PWR_LVL		2
+#define PLAT_MAX_PWR_LVL	2
 
 /*
  * Other platform porting definitions are provided by included headers
@@ -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