Steal more code from Linux to implement exception handling, but with
the same kvm-unit-tests API that Arm has. Also introduce struct
thread_info like Arm has in order to hold the handler pointers.
Finally, as usual, extend the selftest to make sure it all works.
Signed-off-by: Andrew Jones <andrew.jones@xxxxxxxxx>
---
lib/riscv/asm-offsets.c | 38 +++++++++++++
lib/riscv/asm/bug.h | 20 +++++++
lib/riscv/asm/csr.h | 12 ++++
lib/riscv/asm/processor.h | 26 +++++++++
lib/riscv/asm/ptrace.h | 46 +++++++++++++++
lib/riscv/asm/setup.h | 3 +-
lib/riscv/processor.c | 60 ++++++++++++++++++++
lib/riscv/setup.c | 9 ++-
riscv/Makefile | 1 +
riscv/cstart.S | 116 +++++++++++++++++++++++++++++++++++++-
riscv/selftest.c | 20 ++++++-
11 files changed, 343 insertions(+), 8 deletions(-)
create mode 100644 lib/riscv/asm/bug.h
create mode 100644 lib/riscv/asm/processor.h
create mode 100644 lib/riscv/asm/ptrace.h
create mode 100644 lib/riscv/processor.c
diff --git a/lib/riscv/asm-offsets.c b/lib/riscv/asm-offsets.c
index eb337b7547b8..7b88d16fd0e4 100644
--- a/lib/riscv/asm-offsets.c
+++ b/lib/riscv/asm-offsets.c
@@ -1,6 +1,7 @@
// SPDX-License-Identifier: GPL-2.0-only
#include <kbuild.h>
#include <elf.h>
+#include <asm/ptrace.h>
int main(void)
{
@@ -13,5 +14,42 @@ int main(void)
OFFSET(ELF_RELA_ADDEND, elf64_rela, r_addend);
DEFINE(ELF_RELA_SIZE, sizeof(struct elf64_rela));
#endif
+ OFFSET(PT_EPC, pt_regs, epc);
+ OFFSET(PT_RA, pt_regs, ra);
+ OFFSET(PT_SP, pt_regs, sp);
+ OFFSET(PT_GP, pt_regs, gp);
+ OFFSET(PT_TP, pt_regs, tp);
+ OFFSET(PT_T0, pt_regs, t0);
+ OFFSET(PT_T1, pt_regs, t1);
+ OFFSET(PT_T2, pt_regs, t2);
+ OFFSET(PT_S0, pt_regs, s0);
+ OFFSET(PT_S1, pt_regs, s1);
+ OFFSET(PT_A0, pt_regs, a0);
+ OFFSET(PT_A1, pt_regs, a1);
+ OFFSET(PT_A2, pt_regs, a2);
+ OFFSET(PT_A3, pt_regs, a3);
+ OFFSET(PT_A4, pt_regs, a4);
+ OFFSET(PT_A5, pt_regs, a5);
+ OFFSET(PT_A6, pt_regs, a6);
+ OFFSET(PT_A7, pt_regs, a7);
+ OFFSET(PT_S2, pt_regs, s2);
+ OFFSET(PT_S3, pt_regs, s3);
+ OFFSET(PT_S4, pt_regs, s4);
+ OFFSET(PT_S5, pt_regs, s5);
+ OFFSET(PT_S6, pt_regs, s6);
+ OFFSET(PT_S7, pt_regs, s7);
+ OFFSET(PT_S8, pt_regs, s8);
+ OFFSET(PT_S9, pt_regs, s9);
+ OFFSET(PT_S10, pt_regs, s10);
+ OFFSET(PT_S11, pt_regs, s11);
+ OFFSET(PT_T3, pt_regs, t3);
+ OFFSET(PT_T4, pt_regs, t4);
+ OFFSET(PT_T5, pt_regs, t5);
+ OFFSET(PT_T6, pt_regs, t6);
+ OFFSET(PT_STATUS, pt_regs, status);
+ OFFSET(PT_BADADDR, pt_regs, badaddr);
+ OFFSET(PT_CAUSE, pt_regs, cause);
+ OFFSET(PT_ORIG_A0, pt_regs, orig_a0);
+ DEFINE(PT_SIZE, sizeof(struct pt_regs));
return 0;
}
diff --git a/lib/riscv/asm/bug.h b/lib/riscv/asm/bug.h
new file mode 100644
index 000000000000..a6f4136ba1b6
--- /dev/null
+++ b/lib/riscv/asm/bug.h
@@ -0,0 +1,20 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+#ifndef _ASMRISCV_BUG_H_
+#define _ASMRISCV_BUG_H_
+
+#ifndef __ASSEMBLY__
+
+static inline void bug(void)
+{
+ asm volatile("ebreak");
+}
+
+#else
+
+.macro bug
+ ebreak
+.endm
+
+#endif
+
+#endif /* _ASMRISCV_BUG_H_ */
diff --git a/lib/riscv/asm/csr.h b/lib/riscv/asm/csr.h
index 356ae054bfff..eeddd1fb448a 100644
--- a/lib/riscv/asm/csr.h
+++ b/lib/riscv/asm/csr.h
@@ -3,7 +3,19 @@
#define _ASMRISCV_CSR_H_
#include <linux/const.h>
+#define CSR_SSTATUS 0x100
+#define CSR_STVEC 0x105
#define CSR_SSCRATCH 0x140
+#define CSR_SEPC 0x141
+#define CSR_SCAUSE 0x142
+#define CSR_STVAL 0x143
+
+/* Exception cause high bit - is an interrupt if set */
+#define CAUSE_IRQ_FLAG (_AC(1, UL) << (__riscv_xlen - 1))
+
+/* Exception causes */
+#define EXC_INST_ILLEGAL 2
+#define EXC_BREAKPOINT 3
#ifndef __ASSEMBLY__
diff --git a/lib/riscv/asm/processor.h b/lib/riscv/asm/processor.h
new file mode 100644
index 000000000000..d8b7018c9102
--- /dev/null
+++ b/lib/riscv/asm/processor.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+#ifndef _ASMRISCV_PROCESSOR_H_
+#define _ASMRISCV_PROCESSOR_H_
+#include <asm/csr.h>
+#include <asm/ptrace.h>
+
+#define EXCEPTION_CAUSE_MAX 16
+
+typedef void (*exception_fn)(struct pt_regs *);
+
+struct thread_info {
+ int cpu;
+ unsigned long hartid;
+ exception_fn exception_handlers[EXCEPTION_CAUSE_MAX];
+};
+
+static inline struct thread_info *current_thread_info(void)
+{
+ return (struct thread_info *)csr_read(CSR_SSCRATCH);
+}
+
+void install_exception_handler(unsigned long cause, void (*handler)(struct pt_regs *));
+void do_handle_exception(struct pt_regs *regs);
+void thread_info_init(void);
+
+#endif /* _ASMRISCV_PROCESSOR_H_ */
diff --git a/lib/riscv/asm/ptrace.h b/lib/riscv/asm/ptrace.h
new file mode 100644
index 000000000000..0873a8ae749f
--- /dev/null
+++ b/lib/riscv/asm/ptrace.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+#ifndef _ASMRISCV_PTRACE_H_
+#define _ASMRISCV_PTRACE_H_
+
+struct pt_regs {
+ unsigned long epc;
+ unsigned long ra;
+ unsigned long sp;
+ unsigned long gp;
+ unsigned long tp;
+ unsigned long t0;
+ unsigned long t1;
+ unsigned long t2;
+ unsigned long s0;
+ unsigned long s1;
+ unsigned long a0;
+ unsigned long a1;
+ unsigned long a2;
+ unsigned long a3;
+ unsigned long a4;
+ unsigned long a5;
+ unsigned long a6;
+ unsigned long a7;
+ unsigned long s2;
+ unsigned long s3;
+ unsigned long s4;
+ unsigned long s5;
+ unsigned long s6;
+ unsigned long s7;
+ unsigned long s8;
+ unsigned long s9;
+ unsigned long s10;
+ unsigned long s11;
+ unsigned long t3;
+ unsigned long t4;
+ unsigned long t5;
+ unsigned long t6;
+ /* Supervisor/Machine CSRs */
+ unsigned long status;
+ unsigned long badaddr;
+ unsigned long cause;
+ /* a0 value before the syscall */
+ unsigned long orig_a0;
+};
+
+#endif /* _ASMRISCV_PTRACE_H_ */
diff --git a/lib/riscv/asm/setup.h b/lib/riscv/asm/setup.h
index c8cfebb4f2c1..e58dd53071ae 100644
--- a/lib/riscv/asm/setup.h
+++ b/lib/riscv/asm/setup.h
@@ -2,9 +2,10 @@
#ifndef _ASMRISCV_SETUP_H_
#define _ASMRISCV_SETUP_H_
#include <libcflat.h>
+#include <asm/processor.h>
#define NR_CPUS 16
-extern unsigned long cpus[NR_CPUS]; /* per-cpu IDs (hartids) */
+extern struct thread_info cpus[NR_CPUS];
extern int nr_cpus;
int hartid_to_cpu(unsigned long hartid);
diff --git a/lib/riscv/processor.c b/lib/riscv/processor.c
new file mode 100644
index 000000000000..3790349aadd1
--- /dev/null
+++ b/lib/riscv/processor.c
@@ -0,0 +1,60 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2023, Ventana Micro Systems Inc., Andrew Jones <ajones@xxxxxxxxxxxxxxxx>
+ */
+#include <libcflat.h>
+#include <asm/csr.h>
+#include <asm/processor.h>
+#include <asm/setup.h>
+
+extern unsigned long _text;
+
+static void show_regs(struct pt_regs *regs)
+{
+ uintptr_t text = (uintptr_t)&_text;
+ unsigned int w = __riscv_xlen / 4;
+
+ printf("Load address: %" PRIxPTR "\n", text);
+ printf("status : %.*lx\n", w, regs->status);
+ printf("cause : %.*lx\n", w, regs->cause);
+ printf("badaddr: %.*lx\n", w, regs->badaddr);
+ printf("pc: %.*lx ra: %.*lx\n", w, regs->epc, w, regs->ra);
+ printf("sp: %.*lx gp: %.*lx tp : %.*lx\n", w, regs->sp, w, regs->gp, w, regs->tp);
+ printf("a0: %.*lx a1: %.*lx a2 : %.*lx a3 : %.*lx\n", w, regs->a0, w, regs->a1, w, regs->a2, w, regs->a3);
+ printf("a4: %.*lx a5: %.*lx a6 : %.*lx a7 : %.*lx\n", w, regs->a4, w, regs->a5, w, regs->a6, w, regs->a7);
+ printf("t0: %.*lx t1: %.*lx t2 : %.*lx t3 : %.*lx\n", w, regs->t0, w, regs->t1, w, regs->t2, w, regs->t3);
+ printf("t4: %.*lx t5: %.*lx t6 : %.*lx\n", w, regs->t4, w, regs->t5, w, regs->t6);
+ printf("s0: %.*lx s1: %.*lx s2 : %.*lx s3 : %.*lx\n", w, regs->s0, w, regs->s1, w, regs->s2, w, regs->s3);
+ printf("s4: %.*lx s5: %.*lx s6 : %.*lx s7 : %.*lx\n", w, regs->s4, w, regs->s5, w, regs->s6, w, regs->s7);
+ printf("s8: %.*lx s9: %.*lx s10: %.*lx s11: %.*lx\n", w, regs->s8, w, regs->s9, w, regs->s10, w, regs->s11);
+}
+
+void do_handle_exception(struct pt_regs *regs)
+{
+ struct thread_info *info = current_thread_info();
+
+ assert(regs->cause < EXCEPTION_CAUSE_MAX);
+ if (info->exception_handlers[regs->cause]) {
+ info->exception_handlers[regs->cause](regs);
+ return;
+ }
+
+ show_regs(regs);
+ assert(0);
+}
+
+void install_exception_handler(unsigned long cause, void (*handler)(struct pt_regs *))
+{
+ struct thread_info *info = current_thread_info();
+
+ assert(cause < EXCEPTION_CAUSE_MAX);
+ info->exception_handlers[cause] = handler;
+}
+
+void thread_info_init(void)
+{
+ unsigned long hartid = csr_read(CSR_SSCRATCH);
+ int cpu = hartid_to_cpu(hartid);
+
+ csr_write(CSR_SSCRATCH, &cpus[cpu]);
+}
diff --git a/lib/riscv/setup.c b/lib/riscv/setup.c
index 44c26b125a27..57eb4797f798 100644
--- a/lib/riscv/setup.c
+++ b/lib/riscv/setup.c
@@ -12,12 +12,13 @@
#include <devicetree.h>
#include <asm/csr.h>
#include <asm/page.h>
+#include <asm/processor.h>
#include <asm/setup.h>
char *initrd;
u32 initrd_size;
-unsigned long cpus[NR_CPUS] = { [0 ... NR_CPUS - 1] = ~0UL };
+struct thread_info cpus[NR_CPUS];
int nr_cpus;
int hartid_to_cpu(unsigned long hartid)
@@ -25,7 +26,7 @@ int hartid_to_cpu(unsigned long hartid)
int cpu;
for_each_present_cpu(cpu)
- if (cpus[cpu] == hartid)
+ if (cpus[cpu].hartid == hartid)
return cpu;
return -1;
}
@@ -36,7 +37,8 @@ static void cpu_set_fdt(int fdtnode __unused, u64 regval, void *info __unused)
assert_msg(cpu < NR_CPUS, "Number cpus exceeds maximum supported (%d).", NR_CPUS);
- cpus[cpu] = regval;
+ cpus[cpu].cpu = cpu;
+ cpus[cpu].hartid = regval;
set_cpu_present(cpu, true);
}
@@ -104,6 +106,7 @@ void setup(const void *fdt, phys_addr_t freemem_start)
mem_init(PAGE_ALIGN((unsigned long)freemem));
cpu_init();
+ thread_info_init();
io_init();
ret = dt_get_bootargs(&bootargs);
diff --git a/riscv/Makefile b/riscv/Makefile
index fb97e678a456..1243be125c00 100644
--- a/riscv/Makefile
+++ b/riscv/Makefile
@@ -26,6 +26,7 @@ cflatobjs += lib/alloc_phys.o
cflatobjs += lib/devicetree.o
cflatobjs += lib/riscv/bitops.o
cflatobjs += lib/riscv/io.o
+cflatobjs += lib/riscv/processor.o
cflatobjs += lib/riscv/sbi.o
cflatobjs += lib/riscv/setup.o
cflatobjs += lib/riscv/smp.o
diff --git a/riscv/cstart.S b/riscv/cstart.S
index 6ec2231e5812..ac7858ef398f 100644
--- a/riscv/cstart.S
+++ b/riscv/cstart.S
@@ -37,8 +37,9 @@
.global start
start:
/*
- * Stash the hartid in scratch and shift the dtb
- * address into a0
+ * Stash the hartid in scratch and shift the dtb address into a0.
+ * thread_info_init() will later promote scratch to point at thread
+ * local storage.
*/
csrw CSR_SSCRATCH, a0
mv a0, a1
@@ -74,7 +75,8 @@ start:
zero_range a1, sp
/* set up exception handling */
- //TODO
+ la a1, exception_vectors
+ csrw CSR_STVEC, a1
/* complete setup */
la a1, stacktop // a1 is the base of free memory
@@ -97,3 +99,111 @@ start:
halt:
1: wfi
j 1b
+
+/*
+ * Save context to address in a0.
+ * For a0, sets PT_A0(a0) to the contents of PT_ORIG_A0(a0).
+ * Clobbers a1.
+ */
+.macro save_context
+ REG_S ra, PT_RA(a0) // x1
+ REG_S sp, PT_SP(a0) // x2
+ REG_S gp, PT_GP(a0) // x3
+ REG_S tp, PT_TP(a0) // x4
+ REG_S t0, PT_T0(a0) // x5
+ REG_S t1, PT_T1(a0) // x6
+ REG_S t2, PT_T2(a0) // x7
+ REG_S s0, PT_S0(a0) // x8 / fp
+ REG_S s1, PT_S1(a0) // x9
+ /* a0 */ // x10
+ REG_S a1, PT_A1(a0) // x11
+ REG_S a2, PT_A2(a0) // x12
+ REG_S a3, PT_A3(a0) // x13
+ REG_S a4, PT_A4(a0) // x14
+ REG_S a5, PT_A5(a0) // x15
+ REG_S a6, PT_A6(a0) // x16
+ REG_S a7, PT_A7(a0) // x17
+ REG_S s2, PT_S2(a0) // x18
+ REG_S s3, PT_S3(a0) // x19
+ REG_S s4, PT_S4(a0) // x20
+ REG_S s5, PT_S5(a0) // x21
+ REG_S s6, PT_S6(a0) // x22
+ REG_S s7, PT_S7(a0) // x23
+ REG_S s8, PT_S8(a0) // x24
+ REG_S s9, PT_S9(a0) // x25
+ REG_S s10, PT_S10(a0) // x26
+ REG_S s11, PT_S11(a0) // x27
+ REG_S t3, PT_T3(a0) // x28
+ REG_S t4, PT_T4(a0) // x29
+ REG_S t5, PT_T5(a0) // x30
+ REG_S t6, PT_T6(a0) // x31
+ csrr a1, CSR_SEPC
+ REG_S a1, PT_EPC(a0)
+ csrr a1, CSR_SSTATUS
+ REG_S a1, PT_STATUS(a0)
+ csrr a1, CSR_STVAL
+ REG_S a1, PT_BADADDR(a0)
+ csrr a1, CSR_SCAUSE
+ REG_S a1, PT_CAUSE(a0)
+ REG_L a1, PT_ORIG_A0(a0)
+ REG_S a1, PT_A0(a0)
+.endm
+
+/*
+ * Restore context from address in a0.
+ * Also restores a0.
+ */
+.macro restore_context
+ REG_L ra, PT_RA(a0) // x1
+ REG_L sp, PT_SP(a0) // x2
+ REG_L gp, PT_GP(a0) // x3
+ REG_L tp, PT_TP(a0) // x4
+ REG_L t0, PT_T0(a0) // x5
+ REG_L t1, PT_T1(a0) // x6
+ REG_L t2, PT_T2(a0) // x7
+ REG_L s0, PT_S0(a0) // x8 / fp
+ REG_L s1, PT_S1(a0) // x9
+ /* a0 */ // x10
+ /* a1 */ // x11
+ REG_L a2, PT_A2(a0) // x12
+ REG_L a3, PT_A3(a0) // x13
+ REG_L a4, PT_A4(a0) // x14
+ REG_L a5, PT_A5(a0) // x15
+ REG_L a6, PT_A6(a0) // x16
+ REG_L a7, PT_A7(a0) // x17
+ REG_L s2, PT_S2(a0) // x18
+ REG_L s3, PT_S3(a0) // x19
+ REG_L s4, PT_S4(a0) // x20
+ REG_L s5, PT_S5(a0) // x21
+ REG_L s6, PT_S6(a0) // x22
+ REG_L s7, PT_S7(a0) // x23
+ REG_L s8, PT_S8(a0) // x24
+ REG_L s9, PT_S9(a0) // x25
+ REG_L s10, PT_S10(a0) // x26
+ REG_L s11, PT_S11(a0) // x27
+ REG_L t3, PT_T3(a0) // x28
+ REG_L t4, PT_T4(a0) // x29
+ REG_L t5, PT_T5(a0) // x30
+ REG_L t6, PT_T6(a0) // x31
+ REG_L a1, PT_EPC(a0)
+ csrw CSR_SEPC, a1
+ REG_L a1, PT_STATUS(a0)
+ csrw CSR_SSTATUS, a1
+ REG_L a1, PT_BADADDR(a0)
+ csrw CSR_STVAL, a1
+ REG_L a1, PT_CAUSE(a0)
+ csrw CSR_SCAUSE, a1
+ REG_L a1, PT_A1(a0)
+ REG_L a0, PT_A0(a0)
+.endm
+
+.balign 4
+.global exception_vectors
+exception_vectors:
+ REG_S a0, (-PT_SIZE + PT_ORIG_A0)(sp)
+ addi a0, sp, -PT_SIZE
+ save_context
+ mv sp, a0
+ call do_handle_exception
+ restore_context
+ sret
diff --git a/riscv/selftest.c b/riscv/selftest.c
index d3b269cf6255..219093489b62 100644
--- a/riscv/selftest.c
+++ b/riscv/selftest.c
@@ -6,6 +6,7 @@
*/
#include <libcflat.h>
#include <cpumask.h>
+#include <asm/processor.h>
#include <asm/setup.h>
static void check_cpus(void)
@@ -13,7 +14,23 @@ static void check_cpus(void)
int cpu;
for_each_present_cpu(cpu)
- report_info("CPU%3d: hartid=%08lx", cpu, cpus[cpu]);
+ report_info("CPU%3d: hartid=%08lx", cpu, cpus[cpu].hartid);
+}
+
+static bool exceptions_work;
+
+static void handler(struct pt_regs *regs)
+{
+ exceptions_work = true;
+ regs->epc += 2;
+}
+
+static void check_exceptions(void)
+{
+ install_exception_handler(EXC_INST_ILLEGAL, handler);
+ asm volatile(".4byte 0");
+ install_exception_handler(EXC_INST_ILLEGAL, NULL);
+ report(exceptions_work, "exceptions");
}
int main(int argc, char **argv)
@@ -45,6 +62,7 @@ int main(int argc, char **argv)
report_skip("environ parsing");
}
+ check_exceptions();
check_cpus();
return report_summary();
--
2.43.0
