plat/arm/board/arm_fpga: Add PSCI implementation for FPGA images

This adds a basic PSCI implementation allow secondary CPUs to be
released from an initial state and continue through to the warm boot
entrypoint.

Each secondary CPU is kept in a holding pen, whereby it polls the value
representing its hold state, by reading this from an array that acts as
a table for all the PEs. The hold states are initially set to 0 for all
cores to indicate that the executing core should continue polling.
To prevent the secondary CPUs from interfering with the platform's
initialization, they are only updated by the primary CPU once the cold
boot sequence has completed and fpga_pwr_domain_on(mpidr) is called.
The polling target CPU will then read 1 (which indicates that it should
branch to the warm reset entrypoint) and then jump to that address
rather than continue polling.

In addition to the initial polling behaviour of the secondary CPUs
before their warm boot reset sequence, they are also placed in a
low-power wfe() state at the end of each poll; accordingly, the PSCI
fpga_pwr_domain_on(mpidr) function also signals an event to all cores
(after updating the target CPU's hold entry) to wake them from this
state, allowing any secondary CPUs that are still polling to check
their hold state again.
This method is in accordance with both the PSCI and Linux kernel
recommendations, as the lessened overhead reduces the energy
consumption associated with the busy-loop.

The table of hold entries is implemented by a global array as shared SRAM
(which is used by other platforms in similar implementations) is not
available on the FPGA images.

Signed-off-by: Oliver Swede <oli.swede@arm.com>
Change-Id: I65cfd1892f8be1dfcb285f0e1e94e7a9870cdf5a

plat/arm/board/arm_fpga/aarch64/fpga_helpers.S

@@ -20,8 +20,8 @@
 	.globl	plat_crash_console_flush
 
 /* -----------------------------------------------------------------------
- * unsigned long plat_get_my_entrypoint (void);
- * TODO: determine if warm boot should be supported for FPGA images
+ * Indicate a cold boot for every CPU - warm boot is unsupported for the
+ * holding pen PSCI implementation.
  * -----------------------------------------------------------------------
  */
 func plat_get_my_entrypoint
@@ -31,11 +31,26 @@ endfunc plat_get_my_entrypoint
 
 /* -----------------------------------------------------------------------
  * void plat_secondary_cold_boot_setup (void);
- * TODO: add placeholder PSCI implementation for FPGA images
  * -----------------------------------------------------------------------
  */
 func plat_secondary_cold_boot_setup
-	ret
+	/*
+	 * Poll the CPU's hold entry until it indicates to jump
+	 * to the entrypoint address.
+	 */
+	bl	plat_my_core_pos
+	lsl	x0, x0, #PLAT_FPGA_HOLD_ENTRY_SHIFT
+	ldr	x1, =hold_base
+	ldr	x2, =fpga_sec_entrypoint
+poll_hold_entry:
+	ldr	x3, [x1, x0]
+	cmp	x3, #PLAT_FPGA_HOLD_STATE_GO
+	b.ne	1f
+	ldr	x3, [x2]
+	br	x3
+1:
+	wfe
+	b	poll_hold_entry
 endfunc plat_secondary_cold_boot_setup
 
 /* -----------------------------------------------------------------------
@@ -64,11 +79,30 @@ endfunc plat_my_core_pos
 
 /* -----------------------------------------------------------------------
  * unsigned int plat_fpga_calc_core_pos(u_register_t mpidr)
- * TODO: add calculation of the core position for FPGA image CPUs
  * -----------------------------------------------------------------------
  */
 func plat_fpga_calc_core_pos
-	mov	x0, #0
+	/*
+	 * Check for MT bit in MPIDR, which may be either value for images
+	 * running on the FPGA.
+	 *
+	 * If not set, shift MPIDR to left to make it look as if in a
+	 * multi-threaded implementation.
+	 */
+	tst	x0, #MPIDR_MT_MASK
+	lsl	x3, x0, #MPIDR_AFFINITY_BITS
+	csel	x3, x3, x0, eq
+
+	/* Extract individual affinity fields from MPIDR */
+	ubfx	x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx	x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx	x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
+
+	/* Compute linear position */
+	mov	x4, #FPGA_MAX_CPUS_PER_CLUSTER
+	madd	x1, x2, x4, x1
+	mov	x5, #FPGA_MAX_PE_PER_CPU
+	madd	x0, x1, x5, x0
 	ret
 endfunc plat_fpga_calc_core_pos
 

plat/arm/board/arm_fpga/fpga_pm.c

@@ -4,12 +4,86 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
+
+#include <lib/psci/psci.h>
+#include <plat/arm/common/plat_arm.h>
 #include <plat/common/platform.h>
+#include <platform_def.h>
+
+/*
+ * This is a basic PSCI implementation that allows secondary CPUs to be
+ * released from their initial state and continue to the warm boot entrypoint.
+ *
+ * The secondary CPUs are placed in a holding pen and released by calls
+ * to fpga_pwr_domain_on(mpidr), which updates the hold entry for the CPU
+ * specified by the mpidr argument - the (polling) target CPU will then branch
+ * to the BL31 warm boot sequence at the entrypoint address.
+ *
+ * Additionally, the secondary CPUs are kept in a low-power wfe() state
+ * (placed there at the end of each poll) and woken when necessary through
+ * calls to sev() in fpga_pwr_domain_on(mpidr), once the hold state for the
+ * relevant CPU has been updated.
+ *
+ * Hotplug is currently implemented using a wfi-loop, which removes the
+ * dependencies on any power controllers or other mechanism that is specific
+ * to the running system as specified by the FPGA image.
+ */
+
+uint64_t hold_base[PLATFORM_CORE_COUNT];
+uintptr_t fpga_sec_entrypoint;
+
+/*
+ * Calls to the CPU specified by the mpidr will set its hold entry to a value
+ * indicating that it should stop polling and branch off to the warm entrypoint.
+ */
+static int fpga_pwr_domain_on(u_register_t mpidr)
+{
+	unsigned int pos = plat_core_pos_by_mpidr(mpidr);
+	unsigned long current_mpidr = read_mpidr_el1();
+
+	if (mpidr == current_mpidr) {
+		return PSCI_E_ALREADY_ON;
+	}
+	hold_base[pos] = PLAT_FPGA_HOLD_STATE_GO;
+	flush_dcache_range((uintptr_t)&hold_base[pos], sizeof(uint64_t));
+	sev(); /* Wake any CPUs from wfe */
+
+	return PSCI_E_SUCCESS;
+}
+
+static void fpga_pwr_domain_off(const psci_power_state_t *target_state)
+{
+	while (1) {
+		wfi();
+	}
+}
+
+static void fpga_cpu_standby(plat_local_state_t cpu_state)
+{
+	/*
+	 * Enter standby state
+	 * dsb is good practice before using wfi to enter low power states
+	 */
+	u_register_t scr = read_scr_el3();
+	write_scr_el3(scr|SCR_IRQ_BIT);
+	dsb();
+	wfi();
+	write_scr_el3(scr);
+}
 
-/* TODO: add PSCI implementation */
+plat_psci_ops_t plat_fpga_psci_pm_ops = {
+	.pwr_domain_on = fpga_pwr_domain_on,
+	.pwr_domain_off = fpga_pwr_domain_off,
+	.cpu_standby = fpga_cpu_standby
+};
 
 int plat_setup_psci_ops(uintptr_t sec_entrypoint,
 			const plat_psci_ops_t **psci_ops)
 {
+	fpga_sec_entrypoint = sec_entrypoint;
+	flush_dcache_range((uint64_t)&fpga_sec_entrypoint,
+			   sizeof(fpga_sec_entrypoint));
+	*psci_ops = &plat_fpga_psci_pm_ops;
 	return 0;
 }

plat/arm/board/arm_fpga/fpga_topology.c

@@ -9,17 +9,63 @@
 #include "fpga_private.h"
 #include <platform_def.h>
 
+static unsigned char fpga_power_domain_tree_desc[FPGA_MAX_CLUSTER_COUNT + 2];
+
 const unsigned char *plat_get_power_domain_tree_desc(void)
 {
-	/* TODO: add description of power domain topology and PSCI implementation */
-	return NULL;
+	int i;
+	/*
+	* The highest level is the system level. The next level is constituted
+	* by clusters and then cores in clusters.
+	*
+	* This description of the power domain topology is aligned with the CPU
+	* indices returned by the plat_core_pos_by_mpidr() and plat_my_core_pos()
+	* APIs.
+	*/
+	fpga_power_domain_tree_desc[0] = 1;
+	fpga_power_domain_tree_desc[1] = FPGA_MAX_CLUSTER_COUNT;
+
+	for (i = 0; i < FPGA_MAX_CLUSTER_COUNT; i++) {
+		fpga_power_domain_tree_desc[i + 2] = FPGA_MAX_CPUS_PER_CLUSTER;
+	}
+
+	return fpga_power_domain_tree_desc;
 }
 
 int plat_core_pos_by_mpidr(u_register_t mpidr)
 {
+	unsigned int cluster_id, cpu_id, thread_id;
+
+	mpidr &= MPIDR_AFFINITY_MASK;
+	if (mpidr & ~(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)) {
+		return -1;
+	}
+
+	if (mpidr & MPIDR_MT_MASK) {
+		thread_id = MPIDR_AFFLVL0_VAL(mpidr);
+	} else {
+		thread_id = 0;
+	}
+
+	cpu_id = MPIDR_AFFLVL1_VAL(mpidr);
+	cluster_id = MPIDR_AFFLVL2_VAL(mpidr);
+
+	if (cluster_id >= FPGA_MAX_CLUSTER_COUNT) {
+		return -1;
+	} else if (cpu_id >= FPGA_MAX_CPUS_PER_CLUSTER) {
+		return -1;
+	} else if (thread_id >= FPGA_MAX_PE_PER_CPU) {
+		return -1;
+	}
+
 	/*
-	 * TODO: calculate core position in a way that accounts for CPUs that
-	 *       potentially implement multithreading
+	 * The image running on the FPGA may or may not implement multithreading,
+	 * and it shouldn't be assumed this is consistent across all CPUs.
+	 * This ensures that any passed mpidr values reflect the status of the
+	 * primary CPU's MT bit.
 	 */
+	mpidr |= (read_mpidr_el1() & MPIDR_MT_MASK);
+
+	/* Calculate the correct core, catering for multi-threaded images */
 	return (int) plat_fpga_calc_core_pos(mpidr);
 }

plat/arm/board/arm_fpga/include/platform_def.h

@@ -33,6 +33,10 @@
 
 #define PLAT_MAX_PWR_LVL		MPIDR_AFFLVL2
 
+#define PLAT_FPGA_HOLD_ENTRY_SHIFT	3
+#define PLAT_FPGA_HOLD_STATE_WAIT	0
+#define PLAT_FPGA_HOLD_STATE_GO		1
+
 #define PLAT_FPGA_CONSOLE_BAUDRATE	38400
 
 #endif