riscv: add support smp_pause / smp_resume

See https://doc.coreboot.org/arch/riscv/ we know that we need to execute
smp_pause at the start of each stage and smp_resume at the end of each
stage.

Change-Id: I6f8159637bfb15f54f0abeb335de2ba6e9cf82fb
Signed-off-by: Xiang Wang <wxjstz@126.com>
Reviewed-on: https://review.coreboot.org/29023
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Jonathan Neuschäfer <j.neuschaefer@gmx.net>
Reviewed-by: Philipp Hug <philipp@hug.cx>

11 files changed, 169 insertions, 12 deletions

diff --git a/src/arch/riscv/Kconfig b/src/arch/riscv/Kconfig
index 2d53f422c1..ae83be855b 100644
--- a/src/arch/riscv/Kconfig
+++ b/src/arch/riscv/Kconfig
@@ -32,3 +32,9 @@ config ARCH_RAMSTAGE_RISCV
 config RISCV_USE_ARCH_TIMER
 	bool
 	default n
+
+config RISCV_HART_NUM
+	int
+
+config RISCV_WORKING_HARTID
+	int
diff --git a/src/arch/riscv/Makefile.inc b/src/arch/riscv/Makefile.inc
index d1354e1249..eacf32acfd 100644
--- a/src/arch/riscv/Makefile.inc
+++ b/src/arch/riscv/Makefile.inc
@@ -53,6 +53,7 @@ bootblock-y += misaligned.c
 bootblock-y += mcall.c
 bootblock-y += virtual_memory.c
 bootblock-y += boot.c
+bootblock-y += smp.c
 bootblock-y += misc.c
 bootblock-y += pmp.c
 bootblock-y += \
@@ -84,6 +85,7 @@ romstage-y += boot.c
 romstage-y += stages.c
 romstage-y += misc.c
 romstage-y += pmp.c
+romstage-y += smp.c
 romstage-y += \
 	$(top)/src/lib/memchr.c \
 	$(top)/src/lib/memcmp.c \
@@ -119,6 +121,7 @@ ramstage-y += misaligned.c
 ramstage-y += virtual_memory.c
 ramstage-y += stages.c
 ramstage-y += misc.c
+ramstage-y += smp.c
 ramstage-y += boot.c
 ramstage-y += tables.c
 ramstage-y += payload.S
diff --git a/src/arch/riscv/include/arch/smp/smp.h b/src/arch/riscv/include/arch/smp/smp.h
new file mode 100644
index 0000000000..e996404476
--- /dev/null
+++ b/src/arch/riscv/include/arch/smp/smp.h
@@ -0,0 +1,33 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2018 HardenedLinux.
+ *
+ * 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.
+ */
+
+#ifndef _RISCV_SMP_H
+#define _RISCV_SMP_H
+
+/*
+ * This function is used to pause smp. Only the hart with hartid equal
+ * to working_hartid can be returned from smp_pause, other harts will
+ * enter halt and wait for wakeup
+ */
+void smp_pause(int working_hartid);
+
+/*
+ * This function is used to wake up the harts that are halted by the
+ * smp_pause function. And this function will not return, all hart will
+ * jump to fn execution, and arg is the argument of the function fn.
+ */
+void smp_resume(void (*fn)(void *), void *arg);
+
+#endif
diff --git a/src/arch/riscv/include/mcall.h b/src/arch/riscv/include/mcall.h
index d1e414a42f..29df736d1b 100644
--- a/src/arch/riscv/include/mcall.h
+++ b/src/arch/riscv/include/mcall.h
@@ -18,7 +18,7 @@
 
 // NOTE: this is the size of hls_t below. A static_assert would be
 // nice to have.
-#define HLS_SIZE 64
+#define HLS_SIZE 88
 
 /* We save 37 registers, currently. */
 #define MENTRY_FRAME_SIZE (HLS_SIZE + 37 * 8)
@@ -35,6 +35,12 @@ typedef struct {
 	unsigned long sbi_private_data;
 } sbi_device_message;
 
+struct blocker {
+	void *arg;
+	void (*fn)(void *arg);
+	uint32_t sync_a;
+	uint32_t sync_b;
+};
 
 typedef struct {
 	sbi_device_message *device_request_queue_head;
@@ -46,6 +52,7 @@ typedef struct {
 	int ipi_pending;
 	uint64_t *timecmp;
 	uint64_t *time;
+	struct blocker entry;
 } hls_t;
 
 #define MACHINE_STACK_TOP() ({ \
@@ -64,6 +71,15 @@ void hls_init(uint32_t hart_id); // need to call this before launching linux
 /* This function is used to initialize HLS()->time/HLS()->timecmp  */
 void mtime_init(void);
 
+/*
+ * This function needs be implement by SoC code.
+ * Although the privileged instruction set defines that MSIP will be
+ * memory-mapped, but does not define how to map. SoC can be implemented as
+ * a bit, a byte, a word, and so on.
+ * So we can't provide code that is related to implementation.
+ */
+void set_msip(int hartid, int val);
+
 #endif // __ASSEMBLER__
 
 #endif
diff --git a/src/arch/riscv/smp.c b/src/arch/riscv/smp.c
new file mode 100644
index 0000000000..8d07d39ded
--- /dev/null
+++ b/src/arch/riscv/smp.c
@@ -0,0 +1,85 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2018 HardenedLinux.
+ *
+ * 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 <stddef.h>
+#include <arch/encoding.h>
+#include <arch/smp/smp.h>
+#include <arch/smp/spinlock.h>
+#include <mcall.h>
+#include <commonlib/compiler.h>
+#include <console/console.h>
+
+void smp_pause(int working_hartid)
+{
+#define SYNCA (OTHER_HLS(working_hartid)->entry.sync_a)
+#define SYNCB (OTHER_HLS(working_hartid)->entry.sync_b)
+
+	int hartid = read_csr(mhartid);
+
+	if (hartid != working_hartid) {
+		/* waiting for work hart */
+		do {
+			barrier();
+		} while (SYNCA != 0x01234567);
+
+		clear_csr(mstatus, MSTATUS_MIE);
+		write_csr(mie, MIP_MSIP);
+
+		/* count how many cores enter the halt */
+		__sync_fetch_and_add(&SYNCB, 1);
+
+		do {
+			barrier();
+			__asm__ volatile ("wfi");
+		} while ((read_csr(mip) & MIP_MSIP) == 0);
+		set_msip(hartid, 0);
+		HLS()->entry.fn(HLS()->entry.arg);
+	} else {
+		/* Initialize the counter and
+		 * mark the work hart into smp_pause */
+		SYNCB = 0;
+		SYNCA = 0x01234567;
+
+		/* waiting for other Hart to enter the halt */
+		do {
+			barrier();
+		} while (SYNCB + 1 < CONFIG_RISCV_HART_NUM);
+
+		/* initialize for the next call */
+		SYNCA = 0;
+		SYNCB = 0;
+	}
+#undef SYNCA
+#undef SYNCB
+}
+
+void smp_resume(void (*fn)(void *), void *arg)
+{
+	int hartid = read_csr(mhartid);
+
+	if (fn == NULL)
+		die("must pass a non-null function pointer\n");
+
+	for (int i = 0; i < CONFIG_RISCV_HART_NUM; i++) {
+		OTHER_HLS(i)->entry.fn = fn;
+		OTHER_HLS(i)->entry.arg = arg;
+	}
+
+	for (int i = 0; i < CONFIG_RISCV_HART_NUM; i++)
+		if (i != hartid)
+			set_msip(i, 1);
+
+	HLS()->entry.fn(HLS()->entry.arg);
+}
diff --git a/src/soc/sifive/fu540/Kconfig b/src/soc/sifive/fu540/Kconfig
index bee8292199..72305d67a9 100644
--- a/src/soc/sifive/fu540/Kconfig
+++ b/src/soc/sifive/fu540/Kconfig
@@ -39,4 +39,12 @@ config RISCV_CODEMODEL
 	string
 	default "medany"
 
+config RISCV_HART_NUM
+	int
+	default 5
+
+config RISCV_WORKING_HARTID
+	int
+	default 0
+
 endif
diff --git a/src/soc/sifive/fu540/Makefile.inc b/src/soc/sifive/fu540/Makefile.inc
index e2bdd03a00..fef859d9e4 100644
--- a/src/soc/sifive/fu540/Makefile.inc
+++ b/src/soc/sifive/fu540/Makefile.inc
@@ -19,6 +19,7 @@ bootblock-y += media.c
 bootblock-y += bootblock.c
 
 romstage-y += uart.c
+romstage-y += clint.c
 romstage-y += media.c
 romstage-y += sdram.c
 romstage-y += cbmem.c
diff --git a/src/soc/sifive/fu540/clint.c b/src/soc/sifive/fu540/clint.c
index 00aec1ab37..fb4b97fbb3 100644
--- a/src/soc/sifive/fu540/clint.c
+++ b/src/soc/sifive/fu540/clint.c
@@ -14,9 +14,9 @@
  */
 
 #include <mcall.h>
+#include <stdint.h>
 #include <arch/io.h>
 #include <soc/addressmap.h>
-#include <soc/clint.h>
 
 void mtime_init(void)
 {
@@ -27,6 +27,5 @@ void mtime_init(void)
 
 void set_msip(int hartid, int val)
 {
-	long hart_id = read_csr(mhartid);
-	write32((void *)(FU540_CLINT + 4 * hart_id), !!val);
+	write32((void *)(FU540_CLINT + 4 * (uintptr_t)hartid), !!val);
 }
diff --git a/src/soc/ucb/riscv/Kconfig b/src/soc/ucb/riscv/Kconfig
index 26adb56bfe..99b049068e 100644
--- a/src/soc/ucb/riscv/Kconfig
+++ b/src/soc/ucb/riscv/Kconfig
@@ -25,4 +25,8 @@ config RISCV_CODEMODEL
 	string
 	default "medany"
 
+config RISCV_WORKING_HARTID
+	int
+	default 0
+
 endif
diff --git a/src/soc/ucb/riscv/Makefile.inc b/src/soc/ucb/riscv/Makefile.inc
index a10f3aae4c..c96e3637b7 100644
--- a/src/soc/ucb/riscv/Makefile.inc
+++ b/src/soc/ucb/riscv/Makefile.inc
@@ -1,8 +1,13 @@
 ifeq ($(CONFIG_SOC_UCB_RISCV),y)
 
 bootblock-y += mtime.c
+bootblock-y += ipi.c
+
 romstage-y += cbmem.c
+romstage-y += ipi.c
+
 ramstage-y += cbmem.c
 ramstage-y += mtime.c
+ramstage-y += ipi.c
 
 endif
diff --git a/src/soc/sifive/fu540/include/soc/clint.h b/src/soc/ucb/riscv/ipi.c
index d2399c220a..80307a8d2a 100644
--- a/src/soc/sifive/fu540/include/soc/clint.h
+++ b/src/soc/ucb/riscv/ipi.c
@@ -13,12 +13,9 @@
  * GNU General Public License for more details.
  */
 
-#ifndef __SOC_SIFIVE_FU540_CLINT_H
-#define __SOC_SIFIVE_FU540_CLINT_H
+#include <mcall.h>
 
-/* This function is used to set MSIP.
- * It can be used to send an IPI (inter-processor interrupt) to
- * another hart*/
-void set_msip(int hartid, int val);
-
-#endif
+/* TODO: Please implement this function */
+void set_msip(int hartid, int val)
+{
+}