Implement the same SMP API that Arm has but using an SBI HSM
call instead of PSCI. Unlike Arm, riscv needs to always set
cpu0_calls_idle, because the boot hart doesn't have to be the
first hart described in the DT, which means cpu0 may well be
a secondary. As usual, add a couple tests to selftest.c to
make sure everything works.
(The secondary boot process is also improved over Arm's a bit
by keeping boot data percpu, dropping the need for a lock. We
could create percpu data for Arm too, but that's left as future
work.)
Signed-off-by: Andrew Jones <andrew.jones@xxxxxxxxx>
---
lib/riscv/asm-offsets.c | 6 ++++
lib/riscv/asm/barrier.h | 2 ++
lib/riscv/asm/sbi.h | 22 +++++++++++++++
lib/riscv/asm/smp.h | 28 +++++++++++++++++++
lib/riscv/processor.c | 2 ++
lib/riscv/sbi.c | 5 ++++
lib/riscv/setup.c | 2 ++
lib/riscv/smp.c | 61 +++++++++++++++++++++++++++++++++++++++++
riscv/Makefile | 1 +
riscv/cstart.S | 25 +++++++++++++++++
riscv/selftest.c | 31 +++++++++++++++++++++
11 files changed, 185 insertions(+)
create mode 100644 lib/riscv/asm/smp.h
diff --git a/lib/riscv/asm-offsets.c b/lib/riscv/asm-offsets.c
index 7b88d16fd0e4..f5beeeb45e09 100644
--- a/lib/riscv/asm-offsets.c
+++ b/lib/riscv/asm-offsets.c
@@ -2,6 +2,7 @@
#include <kbuild.h>
#include <elf.h>
#include <asm/ptrace.h>
+#include <asm/smp.h>
int main(void)
{
@@ -51,5 +52,10 @@ int main(void)
OFFSET(PT_CAUSE, pt_regs, cause);
OFFSET(PT_ORIG_A0, pt_regs, orig_a0);
DEFINE(PT_SIZE, sizeof(struct pt_regs));
+
+ OFFSET(SECONDARY_STVEC, secondary_data, stvec);
+ OFFSET(SECONDARY_FUNC, secondary_data, func);
+ DEFINE(SECONDARY_DATA_SIZE, sizeof(struct secondary_data));
+
return 0;
}
diff --git a/lib/riscv/asm/barrier.h b/lib/riscv/asm/barrier.h
index 6036d66af76f..4fef120a0fe8 100644
--- a/lib/riscv/asm/barrier.h
+++ b/lib/riscv/asm/barrier.h
@@ -15,4 +15,6 @@
#define smp_rmb() RISCV_FENCE(r,r)
#define smp_wmb() RISCV_FENCE(w,w)
+#define cpu_relax() __asm__ __volatile__ ("pause")
+
#endif /* _ASMRISCV_BARRIER_H_ */
diff --git a/lib/riscv/asm/sbi.h b/lib/riscv/asm/sbi.h
index aeff07f6f1a8..d82a384da5ce 100644
--- a/lib/riscv/asm/sbi.h
+++ b/lib/riscv/asm/sbi.h
@@ -2,8 +2,21 @@
#ifndef _ASMRISCV_SBI_H_
#define _ASMRISCV_SBI_H_
+#define SBI_SUCCESS 0
+#define SBI_ERR_FAILURE -1
+#define SBI_ERR_NOT_SUPPORTED -2
+#define SBI_ERR_INVALID_PARAM -3
+#define SBI_ERR_DENIED -4
+#define SBI_ERR_INVALID_ADDRESS -5
+#define SBI_ERR_ALREADY_AVAILABLE -6
+#define SBI_ERR_ALREADY_STARTED -7
+#define SBI_ERR_ALREADY_STOPPED -8
+
+#ifndef __ASSEMBLY__
+
enum sbi_ext_id {
SBI_EXT_BASE = 0x10,
+ SBI_EXT_HSM = 0x48534d,
SBI_EXT_SRST = 0x53525354,
};
@@ -17,6 +30,13 @@ enum sbi_ext_base_fid {
SBI_EXT_BASE_GET_MIMPID,
};
+enum sbi_ext_hsm_fid {
+ SBI_EXT_HSM_HART_START = 0,
+ SBI_EXT_HSM_HART_STOP,
+ SBI_EXT_HSM_HART_STATUS,
+ SBI_EXT_HSM_HART_SUSPEND,
+};
+
struct sbiret {
long error;
long value;
@@ -28,5 +48,7 @@ struct sbiret sbi_ecall(int ext, int fid, unsigned long arg0,
unsigned long arg5);
void sbi_shutdown(void);
+struct sbiret sbi_hart_start(unsigned long hartid, unsigned long entry, unsigned long sp);
+#endif /* !__ASSEMBLY__ */
#endif /* _ASMRISCV_SBI_H_ */
diff --git a/lib/riscv/asm/smp.h b/lib/riscv/asm/smp.h
new file mode 100644
index 000000000000..931766dc3969
--- /dev/null
+++ b/lib/riscv/asm/smp.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+#ifndef _ASMRISCV_SMP_H_
+#define _ASMRISCV_SMP_H_
+#include <asm/barrier.h>
+#include <asm/processor.h>
+
+#define smp_wait_for_event() cpu_relax()
+#define smp_send_event() cpu_relax()
+
+static inline int smp_processor_id(void)
+{
+ return current_thread_info()->cpu;
+}
+
+typedef void (*secondary_func_t)(void);
+
+struct secondary_data {
+ unsigned long stvec;
+ secondary_func_t func;
+} __attribute__((aligned(16)));
+
+void secondary_entry(unsigned long hartid, unsigned long sp_phys);
+secondary_func_t secondary_cinit(struct secondary_data *data);
+
+void smp_boot_secondary(int cpu, void (*func)(void));
+void smp_boot_secondary_nofail(int cpu, void (*func)(void));
+
+#endif /* _ASMRISCV_SMP_H_ */
diff --git a/lib/riscv/processor.c b/lib/riscv/processor.c
index 3790349aadd1..7248cf4c5ca6 100644
--- a/lib/riscv/processor.c
+++ b/lib/riscv/processor.c
@@ -11,10 +11,12 @@ extern unsigned long _text;
static void show_regs(struct pt_regs *regs)
{
+ struct thread_info *info = current_thread_info();
uintptr_t text = (uintptr_t)&_text;
unsigned int w = __riscv_xlen / 4;
printf("Load address: %" PRIxPTR "\n", text);
+ printf("CPU%3d : hartid=%lx\n", info->cpu, info->hartid);
printf("status : %.*lx\n", w, regs->status);
printf("cause : %.*lx\n", w, regs->cause);
printf("badaddr: %.*lx\n", w, regs->badaddr);
diff --git a/lib/riscv/sbi.c b/lib/riscv/sbi.c
index fd758555b888..f39134c4d77e 100644
--- a/lib/riscv/sbi.c
+++ b/lib/riscv/sbi.c
@@ -33,3 +33,8 @@ void sbi_shutdown(void)
sbi_ecall(SBI_EXT_SRST, 0, 0, 0, 0, 0, 0, 0);
puts("SBI shutdown failed!\n");
}
+
+struct sbiret sbi_hart_start(unsigned long hartid, unsigned long entry, unsigned long sp)
+{
+ return sbi_ecall(SBI_EXT_HSM, SBI_EXT_HSM_HART_START, hartid, entry, sp, 0, 0, 0);
+}
diff --git a/lib/riscv/setup.c b/lib/riscv/setup.c
index 57eb4797f798..9ff446b5e171 100644
--- a/lib/riscv/setup.c
+++ b/lib/riscv/setup.c
@@ -10,6 +10,7 @@
#include <argv.h>
#include <cpumask.h>
#include <devicetree.h>
+#include <on-cpus.h>
#include <asm/csr.h>
#include <asm/page.h>
#include <asm/processor.h>
@@ -60,6 +61,7 @@ static void cpu_init(void)
}
set_cpu_online(hartid_to_cpu(csr_read(CSR_SSCRATCH)), true);
+ cpu0_calls_idle = true;
}
static void mem_init(phys_addr_t freemem_start)
diff --git a/lib/riscv/smp.c b/lib/riscv/smp.c
index a89b59d8dd03..ed7984e75608 100644
--- a/lib/riscv/smp.c
+++ b/lib/riscv/smp.c
@@ -1,6 +1,67 @@
// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Boot secondary CPUs
+ *
+ * Copyright (C) 2023, Ventana Micro Systems Inc., Andrew Jones <ajones@xxxxxxxxxxxxxxxx>
+ */
+#include <libcflat.h>
+#include <alloc.h>
#include <cpumask.h>
+#include <asm/csr.h>
+#include <asm/page.h>
+#include <asm/processor.h>
+#include <asm/sbi.h>
+#include <asm/smp.h>
cpumask_t cpu_present_mask;
cpumask_t cpu_online_mask;
cpumask_t cpu_idle_mask;
+
+static cpumask_t cpu_started;
+
+secondary_func_t secondary_cinit(struct secondary_data *data)
+{
+ struct thread_info *info;
+
+ thread_info_init();
+ info = current_thread_info();
+ set_cpu_online(info->cpu, true);
+ smp_send_event();
+
+ return data->func;
+}
+
+static void __smp_boot_secondary(int cpu, secondary_func_t func)
+{
+ struct secondary_data *sp = memalign(16, SZ_8K) + SZ_8K - 16;
+ struct sbiret ret;
+
+ sp -= sizeof(struct secondary_data);
+ sp->stvec = csr_read(CSR_STVEC);
+ sp->func = func;
+
+ ret = sbi_hart_start(cpus[cpu].hartid, (unsigned long)&secondary_entry, __pa(sp));
+ assert(ret.error == SBI_SUCCESS);
+}
+
+void smp_boot_secondary(int cpu, void (*func)(void))
+{
+ int ret = cpumask_test_and_set_cpu(cpu, &cpu_started);
+
+ assert_msg(!ret, "CPU%d already boot once", cpu);
+ __smp_boot_secondary(cpu, func);
+
+ while (!cpu_online(cpu))
+ smp_wait_for_event();
+}
+
+void smp_boot_secondary_nofail(int cpu, void (*func)(void))
+{
+ int ret = cpumask_test_and_set_cpu(cpu, &cpu_started);
+
+ if (!ret)
+ __smp_boot_secondary(cpu, func);
+
+ while (!cpu_online(cpu))
+ smp_wait_for_event();
+}
diff --git a/riscv/Makefile b/riscv/Makefile
index 697a3beb2703..932f3378264c 100644
--- a/riscv/Makefile
+++ b/riscv/Makefile
@@ -24,6 +24,7 @@ cstart.o = $(TEST_DIR)/cstart.o
cflatobjs += lib/alloc.o
cflatobjs += lib/alloc_phys.o
cflatobjs += lib/devicetree.o
+cflatobjs += lib/on-cpus.o
cflatobjs += lib/riscv/bitops.o
cflatobjs += lib/riscv/io.o
cflatobjs += lib/riscv/processor.o
diff --git a/riscv/cstart.S b/riscv/cstart.S
index 9bdf2e3b17dd..145ccaab6159 100644
--- a/riscv/cstart.S
+++ b/riscv/cstart.S
@@ -117,6 +117,31 @@ halt:
1: wfi
j 1b
+.balign 4
+.global secondary_entry
+secondary_entry:
+ /*
+ * From the "HSM Hart Start Register State" table of the SBI spec:
+ * satp 0
+ * sstatus.SIE 0
+ * a0 hartid
+ * a1 opaque parameter
+ *
+ * __smp_boot_secondary() sets the opaque parameter (a1) to the physical
+ * address of the stack and the stack contains the secondary data.
+ */
+ csrw CSR_SSCRATCH, a0
+ mv sp, a1
+ mv fp, zero
+ REG_L a0, SECONDARY_STVEC(sp)
+ csrw CSR_STVEC, a0
+ mv a0, sp
+ call secondary_cinit
+ addi sp, sp, SECONDARY_DATA_SIZE
+ jr a0
+ la a0, do_idle
+ jr a0
+
/*
* Save context to address in a0.
* For a0, sets PT_A0(a0) to the contents of PT_ORIG_A0(a0).
diff --git a/riscv/selftest.c b/riscv/selftest.c
index 219093489b62..da13c622dba7 100644
--- a/riscv/selftest.c
+++ b/riscv/selftest.c
@@ -6,8 +6,10 @@
*/
#include <libcflat.h>
#include <cpumask.h>
+#include <on-cpus.h>
#include <asm/processor.h>
#include <asm/setup.h>
+#include <asm/smp.h>
static void check_cpus(void)
{
@@ -33,6 +35,34 @@ static void check_exceptions(void)
report(exceptions_work, "exceptions");
}
+static cpumask_t cpus_alive;
+
+static void check_secondary(void *data)
+{
+ cpumask_set_cpu(smp_processor_id(), &cpus_alive);
+}
+
+static void check_smp(void)
+{
+ int cpu, me = smp_processor_id();
+ bool fail = false;
+
+ on_cpus(check_secondary, NULL);
+
+ report(cpumask_full(&cpu_online_mask), "Brought up all cpus");
+ report(cpumask_full(&cpus_alive), "check_secondary");
+
+ for_each_present_cpu(cpu) {
+ if (cpu == me)
+ continue;
+ if (!cpu_idle(cpu)) {
+ fail = true;
+ break;
+ }
+ }
+ report(!fail, "All secondaries are idle");
+}
+
int main(int argc, char **argv)
{
bool r;
@@ -64,6 +94,7 @@ int main(int argc, char **argv)
check_exceptions();
check_cpus();
+ check_smp();
return report_summary();
}
--
2.43.0
