|
@@ -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)
|