soc/cavium: Add secondary CPU support

Change-Id: I07428161615bcd3d03a3eea0df2dd813e08c8f66
Signed-off-by: Patrick Rudolph <patrick.rudolph@9elements.com>
Reviewed-on: https://review.coreboot.org/25752
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: David Hendricks <david.hendricks@gmail.com>

7 files changed, 273 insertions, 8 deletions

diff --git a/src/soc/cavium/cn81xx/Makefile.inc b/src/soc/cavium/cn81xx/Makefile.inc
index 2e12b0137c..16aa4ff2ab 100644
--- a/src/soc/cavium/cn81xx/Makefile.inc
+++ b/src/soc/cavium/cn81xx/Makefile.inc
@@ -62,6 +62,7 @@ ramstage-$(CONFIG_DRIVERS_UART) += uart.c
 ramstage-y += sdram.c
 ramstage-y += soc.c
 ramstage-y += cpu.c
+ramstage-y += cpu_secondary.S
 
 # BDK coreboot interface
 ramstage-y += ../common/bdk-coreboot.c
diff --git a/src/soc/cavium/cn81xx/cpu.c b/src/soc/cavium/cn81xx/cpu.c
index c054aa8987..9504868dff 100644
--- a/src/soc/cavium/cn81xx/cpu.c
+++ b/src/soc/cavium/cn81xx/cpu.c
@@ -14,13 +14,122 @@
  */
 
 #include <types.h>
+#include <soc/addressmap.h>
 #include <arch/io.h>
 #include <soc/cpu.h>
 #include <bdk-coreboot.h>
+#include <console/console.h>
+#include <timer.h>
+#include <delay.h>
 
-/* Return the number of cores available in the chip */
-size_t cpu_get_num_cores(void)
+uint64_t cpu_get_available_core_mask(void)
 {
-	uint64_t available = read64((void *)0x87e006001738ll);
-	return bdk_dpop(available);
+	return read64((void *)RST_PP_AVAILABLE);
+}
+
+size_t cpu_get_num_available_cores(void)
+{
+	return bdk_dpop(cpu_get_available_core_mask());
+}
+
+static void (*secondary_c_entry)(size_t core_id);
+static size_t secondary_booted;
+
+void secondary_cpu_init(size_t core_id)
+{
+	write64(&secondary_booted, 1);
+	dmb();
+
+	if (secondary_c_entry)
+		secondary_c_entry(core_id);
+	else
+		asm("wfi");
+}
+
+size_t cpu_self_get_core_id(void)
+{
+	u32 mpidr_el1;
+	asm("mrs %0, MPIDR_EL1\n\t" : "=r" (mpidr_el1) :: "memory");
+
+	/* Core is 4 bits from AFF0 and rest from AFF1 */
+	size_t core_num;
+	core_num = mpidr_el1 & 0xf;
+	core_num |= (mpidr_el1 & 0xff00) >> 4;
+
+	return core_num;
+}
+
+uint64_t cpu_self_get_core_mask(void)
+{
+	return 1ULL << cpu_self_get_core_id();
+}
+
+size_t start_cpu(size_t cpu, void (*entry_64)(size_t core_id))
+{
+	const uint64_t coremask = 1ULL << cpu;
+	struct stopwatch sw;
+	uint64_t pending;
+
+	printk(BIOS_DEBUG, "CPU: Starting CPU%zu @ %p.\n", cpu, entry_64);
+
+	/* Core not available */
+	if (!(coremask & cpu_get_available_core_mask()))
+		return 1;
+
+	/* Only secondary CPUs are supported */
+	if (cpu == cpu_self_get_core_id())
+		return 1;
+
+	/* Check stack here, instead of in cpu_secondary.S */
+	if ((CONFIG_STACK_SIZE * cpu) > _stack_sec_size)
+		return 1;
+
+	/* Write the address of the main entry point */
+	write64((void *)MIO_BOOT_AP_JUMP, (uintptr_t)secondary_init);
+
+	/* Get coremask of cores in reset */
+	const uint64_t reset = read64((void *)RST_PP_RESET);
+	printk(BIOS_INFO, "CPU: Cores currently in reset: 0x%llx\n", reset);
+
+	/* Setup entry for secondary core */
+	write64(&secondary_c_entry, (uintptr_t)entry_64);
+	write64(&secondary_booted, 0);
+	dmb();
+
+	printk(BIOS_DEBUG, "CPU: Taking core %zu out of reset.\n", cpu);
+
+	/* Release core from reset */
+	write64((void *)RST_PP_RESET, reset & ~coremask);
+
+	/* Wait for cores to finish coming out of reset */
+	udelay(1);
+
+	stopwatch_init_usecs_expire(&sw, 1000000);
+	do {
+		pending = read64((void *)RST_PP_PENDING);
+	} while (!stopwatch_expired(&sw) && (pending & coremask));
+
+	if (stopwatch_expired(&sw)) {
+		printk(BIOS_ERR, "ERROR: Timeout waiting for reset "
+		       "pending to clear.");
+		return 1;
+	}
+
+	stopwatch_init_usecs_expire(&sw, 1000000);
+
+	printk(BIOS_DEBUG, "CPU: Wait up to 1s for the core to boot...\n");
+	while (!stopwatch_expired(&sw) && !read64(&secondary_booted))
+		;
+
+	/* Cleanup */
+	write64(&secondary_c_entry, 0);
+	dmb();
+
+	if (!read64(&secondary_booted)) {
+		printk(BIOS_ERR, "ERROR: Core %zu failed to start.\n", cpu);
+		return 1;
+	}
+
+	printk(BIOS_INFO, "CPU: Core %zu booted\n", cpu);
+	return 0;
 }
diff --git a/src/soc/cavium/cn81xx/cpu_secondary.S b/src/soc/cavium/cn81xx/cpu_secondary.S
new file mode 100644
index 0000000000..d4b4d3cd9b
--- /dev/null
+++ b/src/soc/cavium/cn81xx/cpu_secondary.S
@@ -0,0 +1,91 @@
+/*
+ * Early initialization code for aarch64 (a.k.a. armv8)
+ *
+ * Copyright 2016          Cavium, Inc. <support@cavium.com>
+ * Copyright 2018-present  Facebook, Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; version 2 of
+ * the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <arch/asm.h>
+#include <soc/addressmap.h>
+
+// based on arm64_init_cpu
+ENTRY(secondary_init)
+	/* Initialize PSTATE (unmask all exceptions, select SP_EL0). */
+	msr	SPSel, #0
+	msr	DAIFClr, #0xf
+
+	/* TODO: This is where we'd put non-boot CPUs into WFI if needed. */
+
+	/* x22: SCTLR, return address: x23 (callee-saved by subroutine) */
+	mov	x23, x30
+	/* TODO: Assert that we always start running at EL3 */
+	mrs	x22, sctlr_el3
+
+	/* Activate ICache (12) already for speed during cache flush below. */
+	orr	x22, x22, #(1 << 12)
+	msr	sctlr_el3, x22
+	isb
+
+	/* Invalidate dcache */
+	bl	dcache_invalidate_all
+
+	/* Deactivate MMU (0), Alignment Check (1) and DCache (2) */
+	and	x22, x22, # ~(1 << 0) & ~(1 << 1) & ~(1 << 2)
+	/* Activate Stack Alignment (3) because why not */
+	orr	x22, x22, #(1 << 3)
+	/* Set to little-endian (25) */
+	and	x22, x22, # ~(1 << 25)
+	/* Deactivate write-xor-execute enforcement (19) */
+	and	x22, x22, # ~(1 << 19)
+	msr	sctlr_el3, x22
+
+	/* Invalidate icache and TLB for good measure */
+	ic	iallu
+	tlbi	alle3
+	dsb	sy
+	isb
+
+	/* Load core ID to x0 */
+	mrs	x0, MPIDR_EL1
+	and	x1, x0, # 0xf
+	lsr	x0, x0, 4
+	and	x0, x0, # 0xff0
+	orr	x0, x0, x1
+
+	/* Each core gets CONFIG_STACK_SIZE bytes of stack */
+	mov	x2, # CONFIG_STACK_SIZE
+	mul	x1, x0, x2
+	/* Backup core id */
+	mov	x22, x0
+	ldr	x0, =_stack_sec
+	add	x0, x1, x0 // x0 = CONFIG_STACK_SIZE * coreid + _stack_sec
+	add	x1, x0, # CONFIG_STACK_SIZE // x1 = x0 + CONFIG_STACK_SIZE
+
+	/* Initialize stack with sentinel value to later check overflow. */
+	ldr	x2, =0xdeadbeefdeadbeef
+
+1:
+	stp	x2, x2, [x0], #16
+	cmp	x0, x1
+	bne	1b
+
+	/* Leave a line of beef dead for easier visibility in stack dumps. */
+	sub	sp, x0, #16
+
+	/* Set arg0 to core id */
+	mov	x0, x22
+
+	/* Call C entry */
+	bl	secondary_cpu_init
+
+ENDPROC(secondary_init)
diff --git a/src/soc/cavium/cn81xx/include/soc/addressmap.h b/src/soc/cavium/cn81xx/include/soc/addressmap.h
index 8c993ad9d2..938dd3233d 100644
--- a/src/soc/cavium/cn81xx/include/soc/addressmap.h
+++ b/src/soc/cavium/cn81xx/include/soc/addressmap.h
@@ -58,9 +58,14 @@
 
 /* RST */
 #define RST_PF_BAR0		(0x87E006000000ULL + 0x1600)
+#define RST_PP_AVAILABLE	(RST_PF_BAR0 + 0x138ULL)
+#define RST_PP_RESET		(RST_PF_BAR0 + 0x140ULL)
+#define RST_PP_PENDING		(RST_PF_BAR0 + 0x148ULL)
+
 #define FUSF_PF_BAR0		0x87E004000000ULL
 #define MIO_FUS_PF_BAR0		0x87E003000000ULL
 #define MIO_BOOT_PF_BAR0	0x87E000000000ULL
+#define MIO_BOOT_AP_JUMP	(MIO_BOOT_PF_BAR0 + 0xD0ULL)
 
 /* PTP */
 #define MIO_PTP_PF_BAR0		0x807000000000ULL
diff --git a/src/soc/cavium/cn81xx/include/soc/cpu.h b/src/soc/cavium/cn81xx/include/soc/cpu.h
index df3c06955a..b2472d78e3 100644
--- a/src/soc/cavium/cn81xx/include/soc/cpu.h
+++ b/src/soc/cavium/cn81xx/include/soc/cpu.h
@@ -17,6 +17,59 @@
 #ifndef __SOC_CAVIUM_CN81XX_CPU_H__
 #define __SOC_CAVIUM_CN81XX_CPU_H__
 
-size_t cpu_get_num_cores(void);
+/**
+ * Number of the Core on which the program is currently running.
+ *
+ * @return Number of cores
+ */
+size_t cpu_self_get_core_id(void);
+
+/**
+ * Return a mask representing this core in a 64bit bitmask
+ *
+ * @return The mask of active core.
+ */
+uint64_t cpu_self_get_core_mask(void);
+
+/**
+ * Return the mask of available cores.
+ *
+ * @return Mask of available cores
+ */
+uint64_t cpu_get_available_core_mask(void);
+
+/**
+ * Return the number of cores available in the chip.
+ *
+ * @return The number of available cores.
+ */
+size_t cpu_get_num_available_cores(void);
+
+/**
+ * Init secondary core and call the provided entry for given core.
+ * A stack of size CONFIG_STACK_SIZE is set up for each core in REGION
+ * stack_sec. The unique core id is passed to the entry point functions.
+ *
+ * @return zero on success
+ */
+size_t start_cpu(size_t cpu, void (*entry_64)(size_t core_id));
+
+/**
+ * Secondary ASM CPU entry point.
+ * For internal use only.
+ */
+void secondary_init(void);
+
+/**
+ * Secondary CPU C entry point.
+ * For internal use only.
+ */
+void secondary_cpu_init(size_t core_id);
+
+/* Symbols in memlayout.ld */
+
+extern u8 _stack_sec[];
+extern u8 _estack_sec[];
+#define _stack_sec_size (_estack_sec - _stack_sec)
 
 #endif	/* __SOC_CAVIUM_CN81XX_CPU_H__ */
diff --git a/src/soc/cavium/cn81xx/include/soc/memlayout.ld b/src/soc/cavium/cn81xx/include/soc/memlayout.ld
index d7ee5766a0..0400d2985e 100644
--- a/src/soc/cavium/cn81xx/include/soc/memlayout.ld
+++ b/src/soc/cavium/cn81xx/include/soc/memlayout.ld
@@ -35,6 +35,9 @@ SECTIONS
 	SRAM_END(BOOTROM_OFFSET + 0x80000)
 	TTB(BOOTROM_OFFSET + 0x80000, 512K)
 	RAMSTAGE(BOOTROM_OFFSET + 0x100000, 512K)
+	/* Stack for secondary CPUs */
+	REGION(stack_sec, BOOTROM_OFFSET + 0x180000,
+	       CONFIG_MAX_CPUS * CONFIG_STACK_SIZE, 0x1000)
 
 	/* Leave some space for the payload */
 	POSTRAM_CBFS_CACHE(0x2000000, 16M)
diff --git a/src/soc/cavium/cn81xx/soc.c b/src/soc/cavium/cn81xx/soc.c
index 03f9122404..6c68bb2fe5 100644
--- a/src/soc/cavium/cn81xx/soc.c
+++ b/src/soc/cavium/cn81xx/soc.c
@@ -49,9 +49,12 @@ static void soc_final(device_t dev)
 }
 
 static struct device_operations soc_ops = {
-	.read_resources = soc_read_resources,
-	.init = soc_init,
-	.final = soc_final,
+	.read_resources   = soc_read_resources,
+	.set_resources    = DEVICE_NOOP,
+	.enable_resources = DEVICE_NOOP,
+	.init             = soc_init,
+	.final            = soc_final,
+	.scan_bus         = NULL,
 };
 
 static void enable_soc_dev(device_t dev)