Set mstatus (todo: remove debug println)

platform/k210/src/main.rs

@@ -339,7 +339,9 @@ extern "C" fn start_trap_rust(trap_frame: &mut TrapFrame) {
         }
         Trap::Exception(Exception::IllegalInstruction) => {
             let vaddr = mepc::read();
+            println!("vaddr: {:016X}", vaddr);
             let ins = unsafe { get_vaddr_u32(vaddr) };
+            println!("ins: {:08X}", ins);
             if ins & 0xFFFFF07F == 0xC0102073 { // rdtime instruction
                 // rdtime is actually a csrrw instruction
                 let rd = ((ins >> 7) & 0b1_1111) as u8;
@@ -348,6 +350,7 @@ extern "C" fn start_trap_rust(trap_frame: &mut TrapFrame) {
                 set_rd(trap_frame, rd, time_usize);
                 mepc::write(mepc::read().wrapping_add(4)); // skip current instruction 
             } else if ins & 0xFE007FFF == 0x12000073 { // sfence.vma instruction
+                println!("sfence.vma instruction");
                 // sfence.vma: | 31..25 funct7=SFENCE.VMA(0001001) | 24..20 rs2/asid | 19..15 rs1/vaddr | 
                 //               14..12 funct3=PRIV(000) | 11..7 rd, =0 | 6..0 opcode=SYSTEM(1110011) |
                 // sfence.vm(1.9):  | 31..=20 SFENCE.VM(000100000100) | 19..15 rs1/vaddr |
@@ -359,14 +362,24 @@ extern "C" fn start_trap_rust(trap_frame: &mut TrapFrame) {
                 let paging_mode = satp_bits >> 60; // 63..60 MODE WARL
                 let asid = (satp_bits >> 44) & 0xFFFF; // 59..44 ASID WARL
                 let ppn = satp_bits & 0xFFF_FFFF_FFFF; // 43..0 PPN WARL
+                println!("satp bits: {:016X}", satp_bits);
                 // write to sptbr
                 let sptbr_bits = (asid << 38) | (ppn & 0x3F_FFFF_FFFF);
+                println!("sptbr bits: {:016X}", satp_bits);
                 unsafe { llvm_asm!("csrw 0x180, $0"::"r"(sptbr_bits)) }; // write to sptbr
                 // enable paging (in v1.9.1, mstatus: | 28..24 VM[4:0] WARL | ... )
-                let mstatus_vm_mask: usize = 0x1F00_0000;
-                unsafe { llvm_asm!("csrc mstatus, $0"::"r"(mstatus_vm_mask)) };
-                let mstatus_bits: usize = paging_mode << 24;
-                unsafe { llvm_asm!("csrs mstatus, $0"::"r"(mstatus_bits)) };
+                let mut mstatus_bits: usize; 
+                unsafe { llvm_asm!("csrr $0, mstatus":"=r"(mstatus_bits)) };
+                mstatus_bits &= !0x1F00_0000;
+                mstatus_bits |= 9 << 24 ; //paging_mode << 24;
+                println!(" bits: {:016X}", mstatus_bits);
+                unsafe { llvm_asm!("csrw mstatus, $0"::"r"(mstatus_bits)) };
+                println!("mstatus paging mode updated {:016X}", 
+                    unsafe { 
+                        let ans: usize;
+                        llvm_asm!("csrr $0, mstatus":"=r"(ans));
+                        ans
+                    });
                 // emulate with sfence.vm (declared in privileged spec v1.9)
                 unsafe { llvm_asm!(".word 0x10400073") }; // sfence.vm x0
                 // ::"r"(rs1_vaddr)