a5ds: add multicore support

Enable cores 1-3 using psci. On receiving the smc call from kernel,
core 0 will bring the secondary cores out pen and signal an event for
the cores. Currently on switching the cores is enabled i.e. it is not
possible to suspend, switch cores off, etc.

Change-Id: I6087e1d2ec650e1d587fd543efc1b08cbb50ae5f
Signed-off-by: Usama Arif <usama.arif@arm.com>

fdts/a5ds.dts

@@ -12,14 +12,40 @@
 	interrupt-parent = <&gic>;
 	#address-cells = <1>;
 	#size-cells = <1>;
+
+	psci {
+		compatible = "arm,psci-1.0", "arm,psci-0.2", "arm,psci";
+		method = "smc";
+		cpu_on = <0x84000003>;
+	};
+
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
 		cpu@0 {
 			device_type = "cpu";
 			compatible = "arm,cortex-a5";
+			enable-method = "psci";
 			reg = <0>;
 		};
+		cpu@1 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a5";
+			enable-method = "psci";
+			reg = <1>;
+		};
+		cpu@2 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a5";
+			enable-method = "psci";
+			reg = <2>;
+		};
+		cpu@3 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a5";
+			enable-method = "psci";
+			reg = <3>;
+		};
 	};
 
 	memory@80000000 {

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;

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