diff --git a/arch/riscv/kernel/traps.c b/arch/riscv/kernel/traps.c
index 97f6d34cd..5b3bdc9e4 100644
--- a/arch/riscv/kernel/traps.c
+++ b/arch/riscv/kernel/traps.c
@@ -94,6 +94,90 @@ static void do_trap_error(struct pt_regs *regs, int signo, int code,
 	}
 }
 
+uint32_t get_register_value(struct pt_regs *regs, int x) {
+	if (x == 0)
+		return 0;
+	else
+		return *((uint32_t*)regs + x);
+}
+
+void set_register_value(struct pt_regs *regs, int x, uint32_t val) {
+	if (x != 0)
+		*((uint32_t*)regs + x) = val;
+}
+
+int maybe_handle_illegal(struct pt_regs *regs)
+{
+	uint32_t insn;
+	get_user(insn, (uint32_t*)regs->sepc);
+	//DBGMSG("trying to handle illegal instr %08x", insn);
+	if ((insn & 0x7F) == 0x2F) {
+		//DBGMSG("handling A instr %08x", insn);
+		// Atomic instructions - FIXME: make these actually atomic...
+		uint32_t rs1v = get_register_value(regs, (insn >> 15) & 0x1F);
+		uint32_t rs2v = get_register_value(regs, (insn >> 20) & 0x1F);
+		uint32_t rdv = 0;
+		uint32_t funct = (insn >> 27) & 0x1F;
+		if (funct == 0x2) {
+			// LR
+			get_user(rdv, (uint32_t*)rs1v);
+		} else if (funct == 0x3) {
+			// SC
+			put_user(rs2v, (uint32_t*)rs1v);
+			rdv = 0;
+		} else {
+			//AMO
+			uint32_t opres;
+			get_user(rdv, (uint32_t*)rs1v);
+			switch (funct) {
+				case 0x00: //AMOADD
+					opres = rdv + rs2v;
+					break;
+				case 0x01: // AMOSWAP
+					opres = rs2v;
+					break;
+				case 0x04: //AMOXOR
+					opres = rdv ^ rs2v;
+					break;
+				case 0x0C: //AMOAND
+					opres = rdv & rs2v;
+					break;
+				case 0x08: //AMOOR
+					opres = rdv | rs2v;
+					break;
+				case 0x10: //AMOMIN
+					opres = (uint32_t)(min((int)rdv, (int)rs2v));
+					break;
+				case 0x14: //AMOMAX
+					opres = (uint32_t)(max((int)rdv, (int)rs2v));
+					break;
+				case 0x18: //AMOMINU
+					opres = min(rdv, rs2v);
+					break;
+				case 0x1C:
+					opres = max(rdv, rs2v);
+					break;
+				default:
+					DBG();
+					return 0;
+			}
+			put_user(opres, (uint32_t*)rs1v);
+		}
+		set_register_value(regs, (insn >> 7) & 0x1F, rdv);
+		return 1;
+	}
+	return 0;
+}
+
+asmlinkage void do_trap_insn_illegal(struct pt_regs *regs) {
+	if (maybe_handle_illegal(regs)) {
+		regs->sepc += 4;
+		return;
+	} else {
+		do_trap_error(regs, SIGILL, ILL_ILLOPC, regs->sepc, "Oops - illegal instruction");
+	}
+}
+
 #define DO_ERROR_INFO(name, signo, code, str)				\
 asmlinkage void name(struct pt_regs *regs)				\
 {									\
@@ -106,8 +190,8 @@ DO_ERROR_INFO(do_trap_insn_misaligned,
 	SIGBUS, BUS_ADRALN, "instruction address misaligned");
 DO_ERROR_INFO(do_trap_insn_fault,
 	SIGSEGV, SEGV_ACCERR, "instruction access fault");
-DO_ERROR_INFO(do_trap_insn_illegal,
-	SIGILL, ILL_ILLOPC, "illegal instruction");
+//DO_ERROR_INFO(do_trap_insn_illegal,
+//	SIGILL, ILL_ILLOPC, "illegal instruction");
 DO_ERROR_INFO(do_trap_load_misaligned,
 	SIGBUS, BUS_ADRALN, "load address misaligned");
 DO_ERROR_INFO(do_trap_load_fault,