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>

1 files changed, 85 insertions, 0 deletions

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);
+}