[PATCH 3/3] riscv: add trap and emulation for RDCYCLE

[Ben Dooks]

Add a trap for RDCYCLE and emulate it as RDTIME instruciton.

This is an initial PoC and should probably be made more generic
way of trapping and dealing with bad instructions

Signed-off-by: Ben Dooks <ben.dooks at codethink.co.uk>
---
 arch/riscv/kernel/traps.c | 31 +++++++++++++++++++++++++++++++
 1 file changed, 31 insertions(+)

diff --git a/arch/riscv/kernel/traps.c b/arch/riscv/kernel/traps.c
index 1c3fab272fd1..51ea28ebf54d 100644
--- a/arch/riscv/kernel/traps.c
+++ b/arch/riscv/kernel/traps.c
@@ -167,6 +167,35 @@ DO_ERROR_INFO(do_trap_insn_misaligned,
 DO_ERROR_INFO(do_trap_insn_fault,
 	SIGSEGV, SEGV_ACCERR, "instruction access fault");
 
+#define is_system(__i) (((__i) & 0x7f) == RVG_OPCODE_SYSTEM)
+
+static bool riscv_try_csr_fixup_user(struct pt_regs *regs, u32 insn)
+{
+	/* expecting a 4 byte CSR instruction (*/
+	if (unlikely(GET_INSN_LENGTH(insn) != 4))
+		return false;
+
+	if (is_system(insn)) {
+		u32 csr = insn >> RVG_SYSTEM_CSR_OFF;
+		u32 rd = (insn >> RVG_RD_OPOFF) & RVG_RD_MASK;
+		u32 rs = (insn >> RVG_RS1_OPOFF) & RVG_RS1_MASK;
+		u32 funct3 = (insn >> RV_INSN_FUNCT3_OPOFF) & 0x7;
+
+		if (rs == 0 && funct3 == 2 && csr == CSR_CYCLE) {
+			u64 val = csr_read(CSR_TIME);
+			/* we've got a RDCCLYE, emulated it with CSR_TIME */
+
+			printk_ratelimited("PID %d: process using RDCYCLE, emulating with RDTIME\n", current->pid);
+
+			regs_set_register(regs, rd*sizeof(unsigned long), val);
+			regs->epc += 4;
+			return true;
+		}
+	}
+
+	return false;
+}
+
 asmlinkage __visible __trap_section void do_trap_insn_illegal(struct pt_regs *regs)
 {
 	bool handled;
@@ -186,6 +215,8 @@ asmlinkage __visible __trap_section void do_trap_insn_illegal(struct pt_regs *re
 		}
 
 		handled = riscv_v_first_use_handler(regs, insn);
+		if (!handle)
+			handled = riscv_try_csr_fixup_user(regs, insn);
 
 		local_irq_disable();
 
-- 
2.37.2.352.g3c44437643