Merge changes from topic "a5ds-multicore" into integration
* changes:
a5ds: add multicore support
a5ds: Hold the secondary cpus in pen rather than panic
diff --git a/fdts/a5ds.dts b/fdts/a5ds.dts
index 8bc4adf..91212e8 100644
--- a/fdts/a5ds.dts
+++ b/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 {
diff --git a/plat/arm/board/a5ds/a5ds_pm.c b/plat/arm/board/a5ds/a5ds_pm.c
index 5fd443b..98de77d 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 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..e9e4b9a 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
