Fix rdhwr simulation

kernel/src/arch/aarch64/interrupt/consts.rs

@@ -40,3 +40,7 @@ pub fn is_intr(trap: usize) -> bool {
 pub fn is_timer_intr(trap: usize) -> bool {
     trap == Timer
 }
+
+pub fn is_reserved_inst(trap: usize) -> bool {
+    false
+}

kernel/src/arch/aarch64/interrupt/mod.rs

@@ -74,3 +74,7 @@ pub fn wait_for_interrupt() {
 pub fn handle_user_page_fault(thread: &Arc<Thread>, addr: usize) -> bool {
     thread.vm.lock().handle_page_fault(addr)
 }
+
+pub fn handle_reserved_inst(tf: &mut UserContext) -> bool {
+    false
+}

kernel/src/arch/mipsel/interrupt/consts.rs

@@ -37,3 +37,12 @@ pub fn is_timer_intr(trap: usize) -> bool {
         _ => false,
     }
 }
+
+pub fn is_reserved_inst(trap: usize) -> bool {
+    use cp0::cause::Exception as E;
+    let cause = cp0::cause::Cause { bits: trap as u32 };
+    match cause.cause() {
+        E::ReservedInstruction => true,
+        _ => false,
+    }
+}

kernel/src/arch/mipsel/interrupt/mod.rs

@@ -71,16 +71,6 @@ pub extern "C" fn trap_handler(tf: &mut TrapFrame) {
         E::TLBModification => page_fault(tf),
         E::TLBLoadMiss => page_fault(tf),
         E::TLBStoreMiss => page_fault(tf),
-        E::ReservedInstruction => {
-            if !reserved_inst(tf) {
-                error!("Unhandled Exception @ CPU{}: {:?} ", 0, cause.cause());
-            } else {
-                tf.epc = tf.epc + 4;
-            }
-        }
-        E::CoprocessorUnusable => {
-            tf.epc = tf.epc + 4;
-        }
         _ => {
             error!("Unhandled Exception @ CPU{}: {:?} ", 0, cause.cause());
         }
@@ -163,7 +153,7 @@ fn syscall(tf: &mut TrapFrame) {
     }
 }
 
-fn set_trapframe_register(rt: usize, val: usize, tf: &mut TrapFrame) {
+fn set_trapframe_register(rt: usize, val: usize, tf: &mut UserContext) {
     match rt {
         1 => tf.general.at = val,
         2 => tf.general.v0 = val,
@@ -203,7 +193,7 @@ fn set_trapframe_register(rt: usize, val: usize, tf: &mut TrapFrame) {
     }
 }
 
-fn reserved_inst(tf: &mut TrapFrame) -> bool {
+pub fn handle_reserved_inst(tf: &mut UserContext) -> bool {
     let inst = unsafe { *(tf.epc as *const usize) };
 
     let opcode = inst >> 26;
@@ -214,20 +204,18 @@ fn reserved_inst(tf: &mut TrapFrame) -> bool {
 
     if inst == 0x42000020 {
         // ignore WAIT
+        tf.epc = tf.epc + 4;
         return true;
     }
 
     if opcode == 0b011111 && format == 0b111011 {
-        // RDHWR
+        // RDHWR UserLocal
         if rd == 29 && sel == 0 {
-            extern "C" {
-                fn _cur_tls();
-            }
-
-            let tls = unsafe { *(_cur_tls as *const usize) };
+            let tls = tf.tls;
 
             set_trapframe_register(rt, tls, tf);
-            debug!("Read TLS by rdhdr {:x} to register {:?}", tls, rt);
+            debug!("Read TLS by rdhwr {:x} to register {:?}", tls, rt);
+            tf.epc = tf.epc + 4;
             return true;
         } else {
             return false;

kernel/src/arch/riscv/interrupt/consts.rs

@@ -25,3 +25,7 @@ pub fn is_intr(trap: usize) -> bool {
 pub fn is_timer_intr(trap: usize) -> bool {
     trap == Timer
 }
+
+pub fn is_reserved_inst(trap: usize) -> bool {
+    false
+}

kernel/src/arch/riscv/interrupt/mod.rs

@@ -119,3 +119,7 @@ pub fn wait_for_interrupt() {
 pub fn handle_user_page_fault(thread: &Arc<Thread>, addr: usize) -> bool {
     thread.vm.lock().handle_page_fault(addr)
 }
+
+pub fn handle_reserved_inst(tf: &mut UserContext) -> bool {
+    false
+}

kernel/src/arch/x86_64/interrupt/consts.rs

@@ -61,3 +61,7 @@ pub fn is_intr(trap: usize) -> bool {
 pub fn is_timer_intr(trap: usize) -> bool {
     trap == Timer
 }
+
+pub fn is_reserved_inst(trap: usize) -> bool {
+    false
+}

kernel/src/arch/x86_64/interrupt/mod.rs

@@ -66,3 +66,7 @@ pub fn wait_for_interrupt() {
 pub fn handle_user_page_fault(thread: &Arc<Thread>, addr: usize) -> bool {
     thread.vm.lock().handle_page_fault(addr)
 }
+
+pub fn handle_reserved_inst(tf: &mut UserContext) -> bool {
+    false
+}

kernel/src/process/thread.rs

@@ -2,8 +2,10 @@ use super::{
     abi::{self, ProcInitInfo},
     add_to_process_table, Pid, Process, PROCESSORS,
 };
-use crate::arch::interrupt::consts::{is_intr, is_page_fault, is_syscall, is_timer_intr};
-use crate::arch::interrupt::{get_trap_num, handle_user_page_fault};
+use crate::arch::interrupt::consts::{
+    is_intr, is_page_fault, is_reserved_inst, is_syscall, is_timer_intr,
+};
+use crate::arch::interrupt::{get_trap_num, handle_reserved_inst, handle_user_page_fault};
 use crate::arch::{
     cpu,
     fp::FpState,
@@ -525,6 +527,14 @@ pub fn spawn(thread: Arc<Thread>) {
                     }
                     IRQ_MANAGER.read().try_handle_interrupt(Some(trap_num));
                 }
+                _ if is_reserved_inst(trap_num) => {
+                    if !handle_reserved_inst(cx) {
+                        panic!(
+                            "unhandled reserved intr in thread {} trap {:#x} {:x?}",
+                            thread.tid, trap_num, cx
+                        );
+                    }
+                }
                 _ => {
                     panic!(
                         "unhandled trap in thread {} trap {:#x} {:x?}",