[PATCH v9 4/5] xen/riscv: enable GENERIC_BUG_FRAME

[Oleksii Kurochko <oleksii.kurochko@xxxxxxxxx>]

Signed-off-by: Oleksii Kurochko <oleksii.kurochko@xxxxxxxxx>
---
 xen/arch/riscv/Kconfig |  1 +
 xen/arch/riscv/traps.c | 31 +++++++++++++++++++++++++++++++
 xen/common/bug.c       |  1 +
 3 files changed, 33 insertions(+)

diff --git a/xen/arch/riscv/Kconfig b/xen/arch/riscv/Kconfig
index b4b354a778..74ad019fe7 100644
--- a/xen/arch/riscv/Kconfig
+++ b/xen/arch/riscv/Kconfig
@@ -5,6 +5,7 @@ config RISCV
 config RISCV_64
        def_bool y
        select 64BIT
+       select GENERIC_BUG_FRAME
 
 config ARCH_DEFCONFIG
        string
diff --git a/xen/arch/riscv/traps.c b/xen/arch/riscv/traps.c
index cb18b30ff2..7ba16252fc 100644
--- a/xen/arch/riscv/traps.c
+++ b/xen/arch/riscv/traps.c
@@ -5,6 +5,7 @@
  * RISC-V Trap handlers
  */
 
+#include <xen/bug.h>
 #include <xen/lib.h>
 #include <xen/sched.h>
 
@@ -101,8 +102,38 @@ static void do_unexpected_trap(const struct cpu_user_regs 
*regs)
     die();
 }
 
+static bool is_valid_bug_insn(uint32_t insn)
+{
+    return insn == BUG_INSN_32 ||
+           (insn & COMPRESSED_INSN_MASK) == BUG_INSN_16;
+}
+
+/* Should be used only on Xen code */
+static uint32_t read_instr(unsigned long pc)
+{
+    uint16_t instr16 = *(uint16_t *)pc;
+
+    ASSERT(is_kernel_text(pc + 1) || is_kernel_inittext(pc + 1));
+
+    if ( GET_INSN_LENGTH(instr16) == 2 )
+        return instr16;
+
+    ASSERT(is_kernel_text(pc + 3) || is_kernel_inittext(pc + 3));
+
+    return *(uint32_t *)pc;
+}
+
 void do_trap(struct cpu_user_regs *cpu_regs)
 {
+    register_t pc = cpu_regs->sepc;
+    uint32_t instr = read_instr(pc);
+
+    if ( ( is_valid_bug_insn(instr) ) && ( do_bug_frame(cpu_regs, pc) >= 0 ) )
+    {
+        cpu_regs->sepc += GET_INSN_LENGTH(instr);
+        return;
+    }
+
     do_unexpected_trap(cpu_regs);
 }
 
diff --git a/xen/common/bug.c b/xen/common/bug.c
index b7c5d8fd4d..75cb35fcfa 100644
--- a/xen/common/bug.c
+++ b/xen/common/bug.c
@@ -1,6 +1,7 @@
 #include <xen/bug.h>
 #include <xen/errno.h>
 #include <xen/kernel.h>
+#include <xen/lib.h>
 #include <xen/livepatch.h>
 #include <xen/string.h>
 #include <xen/types.h>
-- 
2.45.2