diff --git a/Cargo.lock b/Cargo.lock index 6b92449..4dbd04d 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2,6 +2,47 @@ # It is not intended for manual editing. version = 3 +[[package]] +name = "goblin" +version = "0.7.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f27c1b4369c2cd341b5de549380158b105a04c331be5db9110eef7b6d2742134" +dependencies = [ + "log", + "plain", + "scroll", +] + +[[package]] +name = "log" +version = "0.4.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b5e6163cb8c49088c2c36f57875e58ccd8c87c7427f7fbd50ea6710b2f3f2e8f" + +[[package]] +name = "plain" +version = "0.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4596b6d070b27117e987119b4dac604f3c58cfb0b191112e24771b2faeac1a6" + +[[package]] +name = "proc-macro2" +version = "1.0.71" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "75cb1540fadbd5b8fbccc4dddad2734eba435053f725621c070711a14bb5f4b8" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.33" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5267fca4496028628a95160fc423a33e8b2e6af8a5302579e322e4b520293cae" +dependencies = [ + "proc-macro2", +] + [[package]] name = "riscv-cpu" version = "0.1.0" @@ -10,5 +51,43 @@ version = "0.1.0" name = "rouns" version = "0.1.0" dependencies = [ + "goblin", "riscv-cpu", ] + +[[package]] +name = "scroll" +version = "0.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "04c565b551bafbef4157586fa379538366e4385d42082f255bfd96e4fe8519da" +dependencies = [ + "scroll_derive", +] + +[[package]] +name = "scroll_derive" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1db149f81d46d2deba7cd3c50772474707729550221e69588478ebf9ada425ae" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "syn" +version = "2.0.43" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ee659fb5f3d355364e1f3e5bc10fb82068efbf824a1e9d1c9504244a6469ad53" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "unicode-ident" +version = "1.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" diff --git a/Cargo.toml b/Cargo.toml index 648c20a..b315773 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,3 +9,4 @@ edition = "2021" [dependencies] riscv-cpu = { path = "crates/riscv-cpu" } +goblin = { version = "0.7.1", features = [ "std", "elf32", "alloc" ]} diff --git a/crates/riscv-cpu/src/cpu.rs b/crates/riscv-cpu/src/cpu.rs index 95f50d0..48126f9 100644 --- a/crates/riscv-cpu/src/cpu.rs +++ b/crates/riscv-cpu/src/cpu.rs @@ -23,12 +23,12 @@ const CSR_SIDELEG_ADDRESS: u16 = 0x103; const CSR_SIE_ADDRESS: u16 = 0x104; const CSR_STVEC_ADDRESS: u16 = 0x105; const _CSR_SSCRATCH_ADDRESS: u16 = 0x140; -const CSR_SEPC_ADDRESS: u16 = 0x141; +pub const CSR_SEPC_ADDRESS: u16 = 0x141; const CSR_SCAUSE_ADDRESS: u16 = 0x142; const CSR_STVAL_ADDRESS: u16 = 0x143; const CSR_SIP_ADDRESS: u16 = 0x144; -const CSR_SATP_ADDRESS: u16 = 0x180; -const CSR_MSTATUS_ADDRESS: u16 = 0x300; +pub const CSR_SATP_ADDRESS: u16 = 0x180; +pub const CSR_MSTATUS_ADDRESS: u16 = 0x300; // const CSR_MISA_ADDRESS: u16 = 0x301; const CSR_MEDELEG_ADDRESS: u16 = 0x302; const CSR_MIDELEG_ADDRESS: u16 = 0x303; @@ -82,8 +82,7 @@ pub enum Xlen { Bit64, // @TODO: Support Bit128 } -#[derive(Clone)] -#[allow(dead_code)] +#[derive(Clone, Debug)] pub enum PrivilegeMode { User, Supervisor, @@ -91,12 +90,13 @@ pub enum PrivilegeMode { Machine, } +#[derive(Debug)] pub struct Trap { pub trap_type: TrapType, pub value: u64, // Trap type specific value } -#[allow(dead_code)] +#[derive(Debug)] pub enum TrapType { InstructionAddressMisaligned, InstructionAccessFault, @@ -319,6 +319,19 @@ impl Cpu { } } + /// Writes integer register content + /// + /// # Arguments + /// * `reg` Register number. Must be 0-31 + /// * `val` 64-bit value + pub fn write_register(&mut self, reg: u8, val: i64) { + debug_assert!(reg <= 31, "reg must be 0-31. {}", reg); + if reg == 0 { + return; + } + self.x[reg as usize] = val; + } + /// Reads Program counter content pub fn read_pc(&self) -> u64 { self.pc @@ -350,35 +363,37 @@ impl Cpu { return Ok(()); } - let original_word = match self.fetch() { - Ok(word) => word, - Err(e) => return Err(e), - }; + let original_word = self.fetch()?; let instruction_address = self.pc; - let word = match (original_word & 0x3) == 0x3 { - true => { - self.pc = self.pc.wrapping_add(4); // 32-bit length non-compressed instruction - original_word - } - false => { - self.pc = self.pc.wrapping_add(2); // 16-bit length compressed instruction - self.uncompress(original_word & 0xffff) - } + let word = if (original_word & 0x3) == 0x3 { + self.pc = self.pc.wrapping_add(4); // 32-bit length non-compressed instruction + original_word + } else { + self.pc = self.pc.wrapping_add(2); // 16-bit length compressed instruction + self.uncompress(original_word & 0xffff) }; - match self.decode(word) { - Ok(inst) => { - let result = (inst.operation)(self, word, instruction_address); - self.x[0] = 0; // hardwired zero - result - } - Err(()) => { - panic!( - "Unknown instruction PC:{:x} WORD:{:x}", - instruction_address, original_word - ); - } - } + let Ok(inst) = self.decode_raw(word) else { + panic!( + "Unknown instruction PC:{:x} WORD:{:x}", + instruction_address, original_word + ); + }; + + println!( + "pc @ 0x{:08x}: {:08x} {} {}", + instruction_address, + original_word, + inst.name, + (inst.disassemble)(self, original_word, self.pc, true) + ); + let result = (inst.operation)(self, word, instruction_address); + self.x[0] = 0; // hardwired zero + result + } + + pub fn execute_opcode(&mut self, op: u32) -> Result<(), Trap> { + (self.decode_raw(op)?.operation)(self, op, self.pc) } /// Decodes a word instruction data and returns a reference to @@ -386,27 +401,27 @@ impl Cpu { /// so if cache hits this method returns the result very quickly. /// The result will be stored to cache. fn decode(&mut self, word: u32) -> Result<&Instruction, ()> { - match self.decode_cache.get(word) { - Some(index) => Ok(&INSTRUCTIONS[index]), - None => match self.decode_and_get_instruction_index(word) { - Ok(index) => { - self.decode_cache.insert(word, index); - Ok(&INSTRUCTIONS[index]) - } - Err(()) => Err(()), - }, + if let Some(index) = self.decode_cache.get(word) { + return Ok(&INSTRUCTIONS[index]); } + let Ok(index) = self.decode_and_get_instruction_index(word) else { + return Err(()); + }; + self.decode_cache.insert(word, index); + Ok(&INSTRUCTIONS[index]) } /// Decodes a word instruction data and returns a reference to /// [`Instruction`](struct.Instruction.html). Not Using [`DecodeCache`](struct.DecodeCache.html) /// so if you don't want to pollute the cache you should use this method /// instead of `decode`. - fn decode_raw(&self, word: u32) -> Result<&Instruction, ()> { - match self.decode_and_get_instruction_index(word) { - Ok(index) => Ok(&INSTRUCTIONS[index]), - Err(()) => Err(()), - } + fn decode_raw(&self, word: u32) -> Result<&Instruction, Trap> { + self.decode_and_get_instruction_index(word) + .map(|index| &INSTRUCTIONS[index]) + .map_err(|_| Trap { + value: self.pc.wrapping_sub(4), + trap_type: TrapType::IllegalInstruction, + }) } /// Decodes a word instruction data and returns an index of @@ -744,14 +759,12 @@ impl Cpu { } fn fetch(&mut self) -> Result { - let word = match self.mmu.fetch_word(self.pc) { - Ok(word) => word, - Err(e) => { - self.pc = self.pc.wrapping_add(4); // @TODO: What if instruction is compressed? - return Err(e); - } - }; - Ok(word) + // println!("Fetching word from {:08x}...", self.pc); + self.mmu.fetch_word(self.pc).map_err(|e| { + self.pc = self.pc.wrapping_add(4); // @TODO: What if instruction is compressed? + println!("Fetch error: {:x?}", e); + e + }) } fn has_csr_access_privilege(&self, address: u16) -> bool { @@ -769,7 +782,7 @@ impl Cpu { } } - fn write_csr(&mut self, address: u16, value: u64) -> Result<(), Trap> { + pub fn write_csr(&mut self, address: u16, value: u64) -> Result<(), Trap> { match self.has_csr_access_privilege(address) { true => { /* @@ -1442,31 +1455,22 @@ impl Cpu { // for example updating page table entry or update peripheral hardware registers. // But ideally disassembling doesn't want to cause any side effect. // How can we avoid side effect? - let mut original_word = match self.mmu.fetch_word(self.pc) { - Ok(data) => data, - Err(_e) => { - return format!("PC:{:016x}, InstructionPageFault Trap!\n", self.pc); - } + let Ok(mut original_word) = self.mmu.fetch_word(self.pc) else { + return format!("PC:{:016x}, InstructionPageFault Trap!\n", self.pc); }; - let word = match (original_word & 0x3) == 0x3 { - true => original_word, - false => { - original_word &= 0xffff; - self.uncompress(original_word) - } + let word = if (original_word & 0x3) == 0x3 { + original_word + } else { + original_word &= 0xffff; + self.uncompress(original_word) }; - let inst = { - match self.decode_raw(word) { - Ok(inst) => inst, - Err(()) => { - return format!( - "Unknown instruction PC:{:x} WORD:{:x}", - self.pc, original_word - ); - } - } + let Ok(inst) = self.decode_raw(word) else { + return format!( + "Unknown instruction PC:{:x} WORD:{:x}", + self.pc, original_word + ); }; let mut s = format!("PC:{:016x} ", self.unsigned_data(self.pc as i64)); @@ -1484,6 +1488,18 @@ impl Cpu { pub fn memory_base(&self) -> u64 { self.memory_base } + + pub fn memory_size(&self) -> u64 { + self.mmu.memory_size() + } + + pub fn phys_read_u8(&mut self, address: u64) -> u8 { + self.mmu.load_raw(address) + } + + pub fn phys_write_u8(&mut self, address: u64, value: u8) { + self.mmu.store_raw(address, value) + } } struct Instruction { @@ -1491,7 +1507,7 @@ struct Instruction { data: u32, // @TODO: rename name: &'static str, operation: fn(cpu: &mut Cpu, word: u32, address: u64) -> Result<(), Trap>, - disassemble: fn(cpu: &mut Cpu, word: u32, address: u64, evaluate: bool) -> String, + disassemble: fn(cpu: &Cpu, word: u32, address: u64, evaluate: bool) -> String, } struct FormatB { @@ -1517,7 +1533,7 @@ fn parse_format_b(word: u32) -> FormatB { } } -fn dump_format_b(cpu: &mut Cpu, word: u32, address: u64, evaluate: bool) -> String { +fn dump_format_b(cpu: &Cpu, word: u32, address: u64, evaluate: bool) -> String { let f = parse_format_b(word); let mut s = String::new(); s += get_register_name(f.rs1); @@ -1546,7 +1562,7 @@ fn parse_format_csr(word: u32) -> FormatCSR { } } -fn dump_format_csr(cpu: &mut Cpu, word: u32, _address: u64, evaluate: bool) -> String { +fn dump_format_csr(cpu: &Cpu, word: u32, _address: u64, evaluate: bool) -> String { let f = parse_format_csr(word); let mut s = String::new(); s += get_register_name(f.rd); @@ -1586,7 +1602,7 @@ fn parse_format_i(word: u32) -> FormatI { } } -fn dump_format_i(cpu: &mut Cpu, word: u32, _address: u64, evaluate: bool) -> String { +fn dump_format_i(cpu: &Cpu, word: u32, _address: u64, evaluate: bool) -> String { let f = parse_format_i(word); let mut s = String::new(); s += get_register_name(f.rd); @@ -1601,7 +1617,7 @@ fn dump_format_i(cpu: &mut Cpu, word: u32, _address: u64, evaluate: bool) -> Str s } -fn dump_format_i_mem(cpu: &mut Cpu, word: u32, _address: u64, evaluate: bool) -> String { +fn dump_format_i_mem(cpu: &Cpu, word: u32, _address: u64, evaluate: bool) -> String { let f = parse_format_i(word); let mut s = String::new(); s += get_register_name(f.rd); @@ -1637,7 +1653,7 @@ fn parse_format_j(word: u32) -> FormatJ { } } -fn dump_format_j(cpu: &mut Cpu, word: u32, address: u64, evaluate: bool) -> String { +fn dump_format_j(cpu: &Cpu, word: u32, address: u64, evaluate: bool) -> String { let f = parse_format_j(word); let mut s = String::new(); s += get_register_name(f.rd); @@ -1662,7 +1678,7 @@ fn parse_format_r(word: u32) -> FormatR { } } -fn dump_format_r(cpu: &mut Cpu, word: u32, _address: u64, evaluate: bool) -> String { +fn dump_format_r(cpu: &Cpu, word: u32, _address: u64, evaluate: bool) -> String { let f = parse_format_r(word); let mut s = String::new(); s += get_register_name(f.rd); @@ -1697,7 +1713,7 @@ fn parse_format_r2(word: u32) -> FormatR2 { } } -fn dump_format_r2(cpu: &mut Cpu, word: u32, _address: u64, evaluate: bool) -> String { +fn dump_format_r2(cpu: &Cpu, word: u32, _address: u64, evaluate: bool) -> String { let f = parse_format_r2(word); let mut s = String::new(); s += get_register_name(f.rd); @@ -1741,7 +1757,7 @@ fn parse_format_s(word: u32) -> FormatS { } } -fn dump_format_s(cpu: &mut Cpu, word: u32, _address: u64, evaluate: bool) -> String { +fn dump_format_s(cpu: &Cpu, word: u32, _address: u64, evaluate: bool) -> String { let f = parse_format_s(word); let mut s = String::new(); s += get_register_name(f.rs2); @@ -1775,7 +1791,7 @@ fn parse_format_u(word: u32) -> FormatU { } } -fn dump_format_u(cpu: &mut Cpu, word: u32, _address: u64, evaluate: bool) -> String { +fn dump_format_u(cpu: &Cpu, word: u32, _address: u64, evaluate: bool) -> String { let f = parse_format_u(word); let mut s = String::new(); s += get_register_name(f.rd); @@ -1786,7 +1802,7 @@ fn dump_format_u(cpu: &mut Cpu, word: u32, _address: u64, evaluate: bool) -> Str s } -fn dump_empty(_cpu: &mut Cpu, _word: u32, _address: u64, _evaluate: bool) -> String { +fn dump_empty(_cpu: &Cpu, _word: u32, _address: u64, _evaluate: bool) -> String { String::new() } @@ -3424,6 +3440,7 @@ const INSTRUCTIONS: [Instruction; INSTRUCTION_NUM] = [ 1 => PrivilegeMode::Supervisor, _ => panic!(), // Shouldn't happen }; + println!("Updating privilege mode to {:?}", cpu.privilege_mode); cpu.mmu.update_privilege_mode(cpu.privilege_mode.clone()); Ok(()) }, diff --git a/crates/riscv-cpu/src/mmu.rs b/crates/riscv-cpu/src/mmu.rs index 3866f8a..9f89b8e 100644 --- a/crates/riscv-cpu/src/mmu.rs +++ b/crates/riscv-cpu/src/mmu.rs @@ -1,4 +1,4 @@ -use std::collections::HashMap; +use std::{collections::HashMap, num::NonZeroU64}; use crate::{ cpu::{get_privilege_mode, PrivilegeMode, Trap, TrapType, Xlen}, @@ -18,6 +18,9 @@ pub struct Mmu { privilege_mode: PrivilegeMode, memory: MemoryWrapper, + /// The size of main memory (if initialized) + memory_length: Option, + /// Address translation can be affected `mstatus` (MPRV, MPP in machine mode) /// then `Mmu` has copy of it. mstatus: u64, @@ -39,6 +42,7 @@ pub struct Mmu { store_page_cache: HashMap, } +#[derive(Debug)] pub enum AddressingMode { None, SV32, @@ -87,6 +91,7 @@ impl Mmu { fetch_page_cache: HashMap::default(), load_page_cache: HashMap::default(), store_page_cache: HashMap::default(), + memory_length: None, } } @@ -104,9 +109,15 @@ impl Mmu { /// # Arguments /// * `capacity` pub fn init_memory(&mut self, capacity: u64) { + assert!(self.memory_length.is_none()); + self.memory_length = Some(NonZeroU64::new(capacity).unwrap()); self.memory.init(capacity); } + pub fn memory_size(&self) -> u64 { + self.memory_length.unwrap().get() + } + /// Enables or disables page cache optimization. /// /// # Arguments @@ -190,29 +201,25 @@ impl Mmu { /// * `v_address` Virtual address pub fn fetch_word(&mut self, v_address: u64) -> Result { let width = 4; - match (v_address & 0xfff) <= (0x1000 - width) { - true => { - // Fast path. All bytes fetched are in the same page so - // translating an address only once. - let effective_address = self.get_effective_address(v_address); - match self.translate_address(effective_address, &MemoryAccessType::Execute) { - Ok(p_address) => Ok(self.load_word_raw(p_address)), - Err(()) => Err(Trap { - trap_type: TrapType::InstructionPageFault, - value: effective_address, - }), - } - } - false => { - let mut data = 0; - for i in 0..width { - match self.fetch(v_address.wrapping_add(i)) { - Ok(byte) => data |= (byte as u32) << (i * 8), - Err(e) => return Err(e), - }; - } - Ok(data) + if (v_address & 0xfff) <= (0x1000 - width) { + // Fast path. All bytes fetched are in the same page so + // translating an address only once. + let effective_address = self.get_effective_address(v_address); + self.translate_address(effective_address, &MemoryAccessType::Execute) + .map(|p_address| self.load_word_raw(p_address)) + .map_err(|()| Trap { + trap_type: TrapType::InstructionPageFault, + value: effective_address, + }) + } else { + let mut data = 0; + for i in 0..width { + match self.fetch(v_address.wrapping_add(i)) { + Ok(byte) => data |= (byte as u32) << (i * 8), + Err(e) => return Err(e), + }; } + Ok(data) } } @@ -396,7 +403,7 @@ impl Mmu { /// /// # Arguments /// * `p_address` Physical address - fn load_raw(&mut self, p_address: u64) -> u8 { + pub(crate) fn load_raw(&mut self, p_address: u64) -> u8 { self.memory.read_byte(self.get_effective_address(p_address)) } @@ -416,7 +423,9 @@ impl Mmu { /// # Arguments /// * `p_address` Physical address pub fn load_word_raw(&mut self, p_address: u64) -> u32 { - self.memory.read_word(self.get_effective_address(p_address)) + let val = self.memory.read_word(self.get_effective_address(p_address)); + // println!("Read value from {:08x}: {:08x}", p_address, val); + val } /// Loads eight bytes from main memory or peripheral devices depending on @@ -435,7 +444,7 @@ impl Mmu { /// # Arguments /// * `p_address` Physical address /// * `value` data written - pub fn store_raw(&mut self, p_address: u64, value: u8) { + pub(crate) fn store_raw(&mut self, p_address: u64, value: u8) { self.memory .write_byte(self.get_effective_address(p_address), value) } @@ -496,7 +505,7 @@ impl Mmu { ) -> Result { let address = self.get_effective_address(v_address); let v_page = address & !0xfff; - let cache = match self.page_cache_enabled { + if let Some(p_page) = match self.page_cache_enabled { true => match access_type { MemoryAccessType::Execute => self.fetch_page_cache.get(&v_page), MemoryAccessType::Read => self.load_page_cache.get(&v_page), @@ -504,106 +513,94 @@ impl Mmu { MemoryAccessType::DontCare => None, }, false => None, - }; - match cache { - Some(p_page) => Ok(p_page | (address & 0xfff)), - None => { - let p_address = match self.addressing_mode { - AddressingMode::None => Ok(address), - AddressingMode::SV32 => match self.privilege_mode { - // @TODO: Optimize - PrivilegeMode::Machine => match access_type { - MemoryAccessType::Execute => Ok(address), - // @TODO: Remove magic number - _ => match (self.mstatus >> 17) & 1 { - 0 => Ok(address), + } { + return Ok(p_page | (address & 0xfff)); + } + + let p_address = match self.addressing_mode { + AddressingMode::None => Ok(address), + AddressingMode::SV32 => match self.privilege_mode { + // @TODO: Optimize + PrivilegeMode::Machine => match access_type { + MemoryAccessType::Execute => Ok(address), + // @TODO: Remove magic number + _ => match (self.mstatus >> 17) & 1 { + 0 => Ok(address), + _ => { + let privilege_mode = get_privilege_mode((self.mstatus >> 9) & 3); + match privilege_mode { + PrivilegeMode::Machine => Ok(address), _ => { - let privilege_mode = - get_privilege_mode((self.mstatus >> 9) & 3); - match privilege_mode { - PrivilegeMode::Machine => Ok(address), - _ => { - let current_privilege_mode = - self.privilege_mode.clone(); - self.update_privilege_mode(privilege_mode); - let result = - self.translate_address(v_address, access_type); - self.update_privilege_mode(current_privilege_mode); - result - } - } + let current_privilege_mode = self.privilege_mode.clone(); + self.update_privilege_mode(privilege_mode); + let result = self.translate_address(v_address, access_type); + self.update_privilege_mode(current_privilege_mode); + result } - }, - }, - PrivilegeMode::User | PrivilegeMode::Supervisor => { - let vpns = [(address >> 12) & 0x3ff, (address >> 22) & 0x3ff]; - self.traverse_page(address, 2 - 1, self.ppn, &vpns, access_type) + } } - _ => Ok(address), }, - AddressingMode::SV39 => match self.privilege_mode { - // @TODO: Optimize - // @TODO: Remove duplicated code with SV32 - PrivilegeMode::Machine => match access_type { - MemoryAccessType::Execute => Ok(address), - // @TODO: Remove magic number - _ => match (self.mstatus >> 17) & 1 { - 0 => Ok(address), - _ => { - let privilege_mode = - get_privilege_mode((self.mstatus >> 9) & 3); - match privilege_mode { - PrivilegeMode::Machine => Ok(address), - _ => { - let current_privilege_mode = - self.privilege_mode.clone(); - self.update_privilege_mode(privilege_mode); - let result = - self.translate_address(v_address, access_type); - self.update_privilege_mode(current_privilege_mode); - result - } - } - } - }, - }, - PrivilegeMode::User | PrivilegeMode::Supervisor => { - let vpns = [ - (address >> 12) & 0x1ff, - (address >> 21) & 0x1ff, - (address >> 30) & 0x1ff, - ]; - self.traverse_page(address, 3 - 1, self.ppn, &vpns, access_type) - } - _ => Ok(address), - }, - AddressingMode::SV48 => { - panic!("AddressingMode SV48 is not supported yet."); - } - }; - match self.page_cache_enabled { - true => match p_address { - Ok(p_address) => { - let p_page = p_address & !0xfff; - match access_type { - MemoryAccessType::Execute => { - self.fetch_page_cache.insert(v_page, p_page) - } - MemoryAccessType::Read => { - self.load_page_cache.insert(v_page, p_page) - } - MemoryAccessType::Write => { - self.store_page_cache.insert(v_page, p_page) - } - MemoryAccessType::DontCare => None, - }; - Ok(p_address) - } - Err(()) => Err(()), - }, - false => p_address, + }, + PrivilegeMode::User | PrivilegeMode::Supervisor => { + let vpns = [(address >> 12) & 0x3ff, (address >> 22) & 0x3ff]; + self.traverse_page(address, 2 - 1, self.ppn, &vpns, access_type) } + _ => Ok(address), + }, + AddressingMode::SV39 => match self.privilege_mode { + // @TODO: Optimize + // @TODO: Remove duplicated code with SV32 + PrivilegeMode::Machine => match access_type { + MemoryAccessType::Execute => Ok(address), + // @TODO: Remove magic number + _ => match (self.mstatus >> 17) & 1 { + 0 => Ok(address), + _ => { + let privilege_mode = get_privilege_mode((self.mstatus >> 9) & 3); + match privilege_mode { + PrivilegeMode::Machine => Ok(address), + _ => { + let current_privilege_mode = self.privilege_mode.clone(); + self.update_privilege_mode(privilege_mode); + let result = self.translate_address(v_address, access_type); + self.update_privilege_mode(current_privilege_mode); + result + } + } + } + }, + }, + PrivilegeMode::User | PrivilegeMode::Supervisor => { + let vpns = [ + (address >> 12) & 0x1ff, + (address >> 21) & 0x1ff, + (address >> 30) & 0x1ff, + ]; + self.traverse_page(address, 3 - 1, self.ppn, &vpns, access_type) + } + _ => Ok(address), + }, + AddressingMode::SV48 => { + panic!("AddressingMode SV48 is not supported yet."); } + }; + + if self.page_cache_enabled { + match p_address { + Ok(p_address) => { + let p_page = p_address & !0xfff; + match access_type { + MemoryAccessType::Execute => self.fetch_page_cache.insert(v_page, p_page), + MemoryAccessType::Read => self.load_page_cache.insert(v_page, p_page), + MemoryAccessType::Write => self.store_page_cache.insert(v_page, p_page), + MemoryAccessType::DontCare => None, + }; + Ok(p_address) + } + Err(()) => Err(()), + } + } else { + p_address } }