jurubas/crates/riscv-cpu/src/cpu.rs

use std::sync::{mpsc::Receiver, Arc, Mutex};

pub use super::mmu::Memory;
use super::mmu::{AddressingMode, Mmu};

const CSR_CAPACITY: usize = 4096;

const CSR_USTATUS_ADDRESS: u16 = 0x000;
const CSR_FFLAGS_ADDRESS: u16 = 0x001;
const CSR_FRM_ADDRESS: u16 = 0x002;
const CSR_FCSR_ADDRESS: u16 = 0x003;
const CSR_UIE_ADDRESS: u16 = 0x004;
const CSR_UTVEC_ADDRESS: u16 = 0x005;
const _CSR_USCRATCH_ADDRESS: u16 = 0x040;
const CSR_UEPC_ADDRESS: u16 = 0x041;
const CSR_UCAUSE_ADDRESS: u16 = 0x042;
const CSR_UTVAL_ADDRESS: u16 = 0x043;
const _CSR_UIP_ADDRESS: u16 = 0x044;
const CSR_SSTATUS_ADDRESS: u16 = 0x100;
const CSR_SEDELEG_ADDRESS: u16 = 0x102;
const CSR_SIDELEG_ADDRESS: u16 = 0x103;
const CSR_SIE_ADDRESS: u16 = 0x104;
const CSR_STVEC_ADDRESS: u16 = 0x105;
const _CSR_SSCRATCH_ADDRESS: u16 = 0x140;
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;
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;
const CSR_MIE_ADDRESS: u16 = 0x304;

const CSR_MTVEC_ADDRESS: u16 = 0x305;
const _CSR_MSCRATCH_ADDRESS: u16 = 0x340;
const CSR_MEPC_ADDRESS: u16 = 0x341;
const CSR_MCAUSE_ADDRESS: u16 = 0x342;
const CSR_MTVAL_ADDRESS: u16 = 0x343;
const CSR_MIP_ADDRESS: u16 = 0x344;
const _CSR_PMPCFG0_ADDRESS: u16 = 0x3a0;
const _CSR_PMPADDR0_ADDRESS: u16 = 0x3b0;
const _CSR_MCYCLE_ADDRESS: u16 = 0xb00;
const CSR_CYCLE_ADDRESS: u16 = 0xc00;
// const CSR_TIME_ADDRESS: u16 = 0xc01;
const _CSR_INSERT_ADDRESS: u16 = 0xc02;
const _CSR_MHARTID_ADDRESS: u16 = 0xf14;

const MIP_MEIP: u64 = 0x800;
pub const MIP_MTIP: u64 = 0x080;
pub const MIP_MSIP: u64 = 0x008;
pub const MIP_SEIP: u64 = 0x200;
const MIP_STIP: u64 = 0x020;
const MIP_SSIP: u64 = 0x002;

pub type ResponseData = ([i64; 8], Option<(Vec<u8>, u64)>);

pub enum TickResult {
    Ok,
    ExitThread(u64),
    PauseEmulation(Receiver<ResponseData>),
    CpuTrap(Trap),
}

/// Emulates a RISC-V CPU core
pub struct Cpu {
    clock: u64,
    xlen: Xlen,
    privilege_mode: PrivilegeMode,
    wfi: bool,
    // using only lower 32bits of x, pc, and csr registers
    // for 32-bit mode
    x: [i64; 32],
    f: [f64; 32],
    pc: u64,
    csr: [u64; CSR_CAPACITY],
    mmu: Mmu,
    memory: Arc<Mutex<dyn Memory + Send + Sync>>,
    reservation: Option<u64>, // @TODO: Should support multiple address reservations
    _dump_flag: bool,
    // decode_cache: DecodeCache,
    unsigned_data_mask: u64,
}

#[derive(Clone)]
pub enum Xlen {
    Bit32,
    Bit64, // @TODO: Support Bit128
}

#[derive(Clone, Copy, Debug)]
pub enum PrivilegeMode {
    User,
    Supervisor,
    Reserved,
    Machine,
}

#[derive(Debug)]
pub struct Trap {
    pub trap_type: TrapType,
    pub value: u64, // Trap type specific value
}

#[derive(Debug)]
pub enum TrapType {
    InstructionAddressMisaligned,
    InstructionAccessFault,
    IllegalInstruction,
    Breakpoint,
    LoadAddressMisaligned,
    LoadAccessFault,
    StoreAddressMisaligned,
    StoreAccessFault,
    EnvironmentCallFromUMode,
    EnvironmentCallFromSMode,
    EnvironmentCallFromMMode,
    InstructionPageFault,
    LoadPageFault,
    StorePageFault,
    UserSoftwareInterrupt,
    SupervisorSoftwareInterrupt,
    MachineSoftwareInterrupt,
    UserTimerInterrupt,
    SupervisorTimerInterrupt,
    MachineTimerInterrupt,
    UserExternalInterrupt,
    SupervisorExternalInterrupt,
    MachineExternalInterrupt,
    PauseEmulation(Receiver<ResponseData>),
}

fn _get_privilege_mode_name(mode: &PrivilegeMode) -> &'static str {
    match mode {
        PrivilegeMode::User => "User",
        PrivilegeMode::Supervisor => "Supervisor",
        PrivilegeMode::Reserved => "Reserved",
        PrivilegeMode::Machine => "Machine",
    }
}

// bigger number is higher privilege level
fn get_privilege_encoding(mode: &PrivilegeMode) -> u8 {
    match mode {
        PrivilegeMode::User => 0,
        PrivilegeMode::Supervisor => 1,
        PrivilegeMode::Reserved => panic!(),
        PrivilegeMode::Machine => 3,
    }
}

/// Returns `PrivilegeMode` from encoded privilege mode bits
pub fn decode_privilege_mode(encoding: u64) -> PrivilegeMode {
    match encoding {
        0 => PrivilegeMode::User,
        1 => PrivilegeMode::Supervisor,
        3 => PrivilegeMode::Machine,
        _ => panic!("Unknown privilege uncoding"),
    }
}

fn _get_trap_type_name(trap_type: &TrapType) -> &'static str {
    match trap_type {
        TrapType::InstructionAddressMisaligned => "InstructionAddressMisaligned",
        TrapType::InstructionAccessFault => "InstructionAccessFault",
        TrapType::IllegalInstruction => "IllegalInstruction",
        TrapType::Breakpoint => "Breakpoint",
        TrapType::LoadAddressMisaligned => "LoadAddressMisaligned",
        TrapType::LoadAccessFault => "LoadAccessFault",
        TrapType::StoreAddressMisaligned => "StoreAddressMisaligned",
        TrapType::StoreAccessFault => "StoreAccessFault",
        TrapType::EnvironmentCallFromUMode => "EnvironmentCallFromUMode",
        TrapType::EnvironmentCallFromSMode => "EnvironmentCallFromSMode",
        TrapType::EnvironmentCallFromMMode => "EnvironmentCallFromMMode",
        TrapType::InstructionPageFault => "InstructionPageFault",
        TrapType::LoadPageFault => "LoadPageFault",
        TrapType::StorePageFault => "StorePageFault",
        TrapType::UserSoftwareInterrupt => "UserSoftwareInterrupt",
        TrapType::SupervisorSoftwareInterrupt => "SupervisorSoftwareInterrupt",
        TrapType::MachineSoftwareInterrupt => "MachineSoftwareInterrupt",
        TrapType::UserTimerInterrupt => "UserTimerInterrupt",
        TrapType::SupervisorTimerInterrupt => "SupervisorTimerInterrupt",
        TrapType::MachineTimerInterrupt => "MachineTimerInterrupt",
        TrapType::UserExternalInterrupt => "UserExternalInterrupt",
        TrapType::SupervisorExternalInterrupt => "SupervisorExternalInterrupt",
        TrapType::MachineExternalInterrupt => "MachineExternalInterrupt",
        TrapType::PauseEmulation(_) => "PauseEmulation",
    }
}

fn get_trap_cause(trap: &Trap, xlen: &Xlen) -> u64 {
    let interrupt_bit = match xlen {
        Xlen::Bit32 => 0x80000000_u64,
        Xlen::Bit64 => 0x8000000000000000_u64,
    };
    match trap.trap_type {
        TrapType::InstructionAddressMisaligned => 0,
        TrapType::InstructionAccessFault => 1,
        TrapType::IllegalInstruction => 2,
        TrapType::Breakpoint => 3,
        TrapType::LoadAddressMisaligned => 4,
        TrapType::LoadAccessFault => 5,
        TrapType::StoreAddressMisaligned => 6,
        TrapType::StoreAccessFault => 7,
        TrapType::EnvironmentCallFromUMode => 8,
        TrapType::EnvironmentCallFromSMode => 9,
        TrapType::EnvironmentCallFromMMode => 11,
        TrapType::InstructionPageFault => 12,
        TrapType::LoadPageFault => 13,
        TrapType::StorePageFault => 15,
        TrapType::PauseEmulation(_) => 16,
        TrapType::UserSoftwareInterrupt => interrupt_bit,
        TrapType::SupervisorSoftwareInterrupt => interrupt_bit + 1,
        TrapType::MachineSoftwareInterrupt => interrupt_bit + 3,
        TrapType::UserTimerInterrupt => interrupt_bit + 4,
        TrapType::SupervisorTimerInterrupt => interrupt_bit + 5,
        TrapType::MachineTimerInterrupt => interrupt_bit + 7,
        TrapType::UserExternalInterrupt => interrupt_bit + 8,
        TrapType::SupervisorExternalInterrupt => interrupt_bit + 9,
        TrapType::MachineExternalInterrupt => interrupt_bit + 11,
    }
}

pub struct CpuBuilder {
    xlen: Xlen,
    pc: u64,
    sp: u64,
    memory: Arc<Mutex<dyn Memory + Send + Sync>>,
}

impl CpuBuilder {
    pub fn new(memory: Arc<Mutex<dyn Memory + Send + Sync>>) -> Self {
        CpuBuilder {
            xlen: Xlen::Bit64,
            memory,
            pc: 0,
            sp: 0,
        }
    }

    pub fn xlen(mut self, xlen: Xlen) -> Self {
        self.xlen = xlen;
        self
    }

    pub fn pc(mut self, pc: u64) -> Self {
        self.pc = pc;
        self
    }

    pub fn sp(mut self, sp: u64) -> Self {
        self.sp = sp;
        self
    }
    pub fn build(self) -> Cpu {
        let mut cpu = Cpu::new(self.memory);
        cpu.update_xlen(self.xlen.clone());
        cpu.update_pc(self.pc);
        cpu.write_register(2, self.sp as i64);
        cpu
    }
}

impl Cpu {
    /// Creates a new `Cpu`.
    ///
    /// # Arguments
    /// * `Terminal`
    pub fn new(memory: Arc<Mutex<dyn Memory + Send + Sync>>) -> Self {
        Cpu {
            clock: 0,
            xlen: Xlen::Bit64,
            privilege_mode: PrivilegeMode::Machine,
            wfi: false,
            x: [0; 32],
            f: [0.0; 32],
            pc: 0,
            csr: [0; CSR_CAPACITY],
            mmu: Mmu::new(Xlen::Bit64, memory.clone()),
            reservation: None,
            _dump_flag: false,
            // decode_cache: DecodeCache::new(),
            unsigned_data_mask: 0xffffffffffffffff,
            memory,
        }
        // let mut cpu = ;
        // cpu.x[0xb] = 0x1020; // I don't know why but Linux boot seems to require this initialization
        // cpu.write_csr_raw(CSR_MISA_ADDRESS, 0x800000008014312f);
        // cpu
    }

    /// Updates Program Counter content
    ///
    /// # Arguments
    /// * `value`
    pub fn update_pc(&mut self, value: u64) {
        self.pc = value;
    }

    /// Updates XLEN, 32-bit or 64-bit
    ///
    /// # Arguments
    /// * `xlen`
    pub fn update_xlen(&mut self, xlen: Xlen) {
        self.xlen = xlen.clone();
        self.unsigned_data_mask = match xlen {
            Xlen::Bit32 => 0xffffffff,
            Xlen::Bit64 => 0xffffffffffffffff,
        };
        self.mmu.update_xlen(xlen.clone());
    }

    /// Reads integer register content
    ///
    /// # Arguments
    /// * `reg` Register number. Must be 0-31
    pub fn read_register(&self, reg: u8) -> i64 {
        debug_assert!(reg <= 31, "reg must be 0-31. {}", reg);
        match reg {
            0 => 0, // 0th register is hardwired zero
            _ => self.x[reg as usize],
        }
    }

    /// 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
    }

    /// Runs program one cycle. Fetch, decode, and execution are completed in a cycle so far.
    pub fn tick(&mut self) -> TickResult {
        match self.tick_operate() {
            Ok(()) => {}
            Err(Trap {
                trap_type: TrapType::PauseEmulation(rx),
                ..
            }) => {
                return TickResult::PauseEmulation(rx);
            }
            Err(Trap {
                trap_type: TrapType::InstructionPageFault,
                value: 0xff803000,
            }) => {
                return TickResult::ExitThread(self.read_register(10) as u64);
            }
            Err(e) => return TickResult::CpuTrap(e),
        }
        self.mmu.tick(&mut self.csr[CSR_MIP_ADDRESS as usize]);
        self.handle_interrupt(self.pc);
        self.clock = self.clock.wrapping_add(1);

        // cpu core clock : mtime clock in clint = 8 : 1 is
        // just an arbiraty ratio.
        // @TODO: Implement more properly
        self.write_csr_raw(CSR_CYCLE_ADDRESS, self.clock * 8);

        TickResult::Ok
    }

    // @TODO: Rename?
    fn tick_operate(&mut self) -> Result<(), Trap> {
        if self.wfi {
            if (self.read_csr_raw(CSR_MIE_ADDRESS) & self.read_csr_raw(CSR_MIP_ADDRESS)) != 0 {
                self.wfi = false;
            }
            return Ok(());
        }

        let original_word = self.fetch()?;
        let instruction_address = self.pc;
        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)
        };

        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
    // /// [`Instruction`](struct.Instruction.html). Using [`DecodeCache`](struct.DecodeCache.html)
    // /// 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, ()> {
    //     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, 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
    /// [`INSTRUCTIONS`](constant.INSTRUCTIONS.html)
    ///
    /// # Arguments
    /// * `word` word instruction data decoded
    fn decode_and_get_instruction_index(&self, word: u32) -> Result<usize, ()> {
        for (idx, inst) in INSTRUCTIONS.iter().enumerate() {
            if (word & inst.mask) == inst.data {
                return Ok(idx);
            }
        }
        Err(())
    }

    fn handle_interrupt(&mut self, instruction_address: u64) {
        // @TODO: Optimize
        let minterrupt = self.read_csr_raw(CSR_MIP_ADDRESS) & self.read_csr_raw(CSR_MIE_ADDRESS);

        if (minterrupt & MIP_MEIP) != 0
            && self.handle_trap(
                Trap {
                    trap_type: TrapType::MachineExternalInterrupt,
                    value: self.pc, // dummy
                },
                instruction_address,
                true,
            )
        {
            // Who should clear mip bit?
            self.write_csr_raw(
                CSR_MIP_ADDRESS,
                self.read_csr_raw(CSR_MIP_ADDRESS) & !MIP_MEIP,
            );
            self.wfi = false;
            return;
        }
        if (minterrupt & MIP_MSIP) != 0
            && self.handle_trap(
                Trap {
                    trap_type: TrapType::MachineSoftwareInterrupt,
                    value: self.pc, // dummy
                },
                instruction_address,
                true,
            )
        {
            self.write_csr_raw(
                CSR_MIP_ADDRESS,
                self.read_csr_raw(CSR_MIP_ADDRESS) & !MIP_MSIP,
            );
            self.wfi = false;
            return;
        }
        if (minterrupt & MIP_MTIP) != 0
            && self.handle_trap(
                Trap {
                    trap_type: TrapType::MachineTimerInterrupt,
                    value: self.pc, // dummy
                },
                instruction_address,
                true,
            )
        {
            self.write_csr_raw(
                CSR_MIP_ADDRESS,
                self.read_csr_raw(CSR_MIP_ADDRESS) & !MIP_MTIP,
            );
            self.wfi = false;
            return;
        }
        if (minterrupt & MIP_SEIP) != 0
            && self.handle_trap(
                Trap {
                    trap_type: TrapType::SupervisorExternalInterrupt,
                    value: self.pc, // dummy
                },
                instruction_address,
                true,
            )
        {
            self.write_csr_raw(
                CSR_MIP_ADDRESS,
                self.read_csr_raw(CSR_MIP_ADDRESS) & !MIP_SEIP,
            );
            self.wfi = false;
            return;
        }
        if (minterrupt & MIP_SSIP) != 0
            && self.handle_trap(
                Trap {
                    trap_type: TrapType::SupervisorSoftwareInterrupt,
                    value: self.pc, // dummy
                },
                instruction_address,
                true,
            )
        {
            self.write_csr_raw(
                CSR_MIP_ADDRESS,
                self.read_csr_raw(CSR_MIP_ADDRESS) & !MIP_SSIP,
            );
            self.wfi = false;
            return;
        }
        if (minterrupt & MIP_STIP) != 0
            && self.handle_trap(
                Trap {
                    trap_type: TrapType::SupervisorTimerInterrupt,
                    value: self.pc, // dummy
                },
                instruction_address,
                true,
            )
        {
            self.write_csr_raw(
                CSR_MIP_ADDRESS,
                self.read_csr_raw(CSR_MIP_ADDRESS) & !MIP_STIP,
            );
            self.wfi = false;
        }
    }

    fn handle_trap(&mut self, trap: Trap, instruction_address: u64, is_interrupt: bool) -> bool {
        let current_privilege_encoding = get_privilege_encoding(&self.privilege_mode) as u64;
        let cause = get_trap_cause(&trap, &self.xlen);

        // First, determine which privilege mode should handle the trap.
        // @TODO: Check if this logic is correct
        let mdeleg = match is_interrupt {
            true => self.read_csr_raw(CSR_MIDELEG_ADDRESS),
            false => self.read_csr_raw(CSR_MEDELEG_ADDRESS),
        };
        let sdeleg = match is_interrupt {
            true => self.read_csr_raw(CSR_SIDELEG_ADDRESS),
            false => self.read_csr_raw(CSR_SEDELEG_ADDRESS),
        };
        let pos = cause & 0xffff;

        let new_privilege_mode = match ((mdeleg >> pos) & 1) == 0 {
            true => PrivilegeMode::Machine,
            false => match ((sdeleg >> pos) & 1) == 0 {
                true => PrivilegeMode::Supervisor,
                false => PrivilegeMode::User,
            },
        };
        let new_privilege_encoding = get_privilege_encoding(&new_privilege_mode) as u64;

        let current_status = match self.privilege_mode {
            PrivilegeMode::Machine => self.read_csr_raw(CSR_MSTATUS_ADDRESS),
            PrivilegeMode::Supervisor => self.read_csr_raw(CSR_SSTATUS_ADDRESS),
            PrivilegeMode::User => self.read_csr_raw(CSR_USTATUS_ADDRESS),
            PrivilegeMode::Reserved => panic!(),
        };

        // Second, ignore the interrupt if it's disabled by some conditions

        if is_interrupt {
            let ie = match new_privilege_mode {
                PrivilegeMode::Machine => self.read_csr_raw(CSR_MIE_ADDRESS),
                PrivilegeMode::Supervisor => self.read_csr_raw(CSR_SIE_ADDRESS),
                PrivilegeMode::User => self.read_csr_raw(CSR_UIE_ADDRESS),
                PrivilegeMode::Reserved => panic!(),
            };

            let current_mie = (current_status >> 3) & 1;
            let current_sie = (current_status >> 1) & 1;
            let current_uie = current_status & 1;

            let msie = (ie >> 3) & 1;
            let ssie = (ie >> 1) & 1;
            let usie = ie & 1;

            let mtie = (ie >> 7) & 1;
            let stie = (ie >> 5) & 1;
            let utie = (ie >> 4) & 1;

            let meie = (ie >> 11) & 1;
            let seie = (ie >> 9) & 1;
            let ueie = (ie >> 8) & 1;

            // 1. Interrupt is always enabled if new privilege level is higher
            // than current privilege level
            // 2. Interrupt is always disabled if new privilege level is lower
            // than current privilege level
            // 3. Interrupt is enabled if xIE in xstatus is 1 where x is privilege level
            // and new privilege level equals to current privilege level

            if new_privilege_encoding < current_privilege_encoding {
                return false;
            }

            if current_privilege_encoding == new_privilege_encoding {
                match self.privilege_mode {
                    PrivilegeMode::Machine => {
                        if current_mie == 0 {
                            return false;
                        }
                    }
                    PrivilegeMode::Supervisor => {
                        if current_sie == 0 {
                            return false;
                        }
                    }
                    PrivilegeMode::User => {
                        if current_uie == 0 {
                            return false;
                        }
                    }
                    PrivilegeMode::Reserved => panic!(),
                };
            }

            // Interrupt can be maskable by xie csr register
            // where x is a new privilege mode.

            match trap.trap_type {
                TrapType::UserSoftwareInterrupt => {
                    if usie == 0 {
                        return false;
                    }
                }
                TrapType::SupervisorSoftwareInterrupt => {
                    if ssie == 0 {
                        return false;
                    }
                }
                TrapType::MachineSoftwareInterrupt => {
                    if msie == 0 {
                        return false;
                    }
                }
                TrapType::UserTimerInterrupt => {
                    if utie == 0 {
                        return false;
                    }
                }
                TrapType::SupervisorTimerInterrupt => {
                    if stie == 0 {
                        return false;
                    }
                }
                TrapType::MachineTimerInterrupt => {
                    if mtie == 0 {
                        return false;
                    }
                }
                TrapType::UserExternalInterrupt => {
                    if ueie == 0 {
                        return false;
                    }
                }
                TrapType::SupervisorExternalInterrupt => {
                    if seie == 0 {
                        return false;
                    }
                }
                TrapType::MachineExternalInterrupt => {
                    if meie == 0 {
                        return false;
                    }
                }
                _ => {}
            };
        }

        // So, this trap should be taken

        self.privilege_mode = new_privilege_mode;
        self.mmu.update_privilege_mode(self.privilege_mode);
        let csr_epc_address = match self.privilege_mode {
            PrivilegeMode::Machine => CSR_MEPC_ADDRESS,
            PrivilegeMode::Supervisor => CSR_SEPC_ADDRESS,
            PrivilegeMode::User => CSR_UEPC_ADDRESS,
            PrivilegeMode::Reserved => panic!(),
        };
        let csr_cause_address = match self.privilege_mode {
            PrivilegeMode::Machine => CSR_MCAUSE_ADDRESS,
            PrivilegeMode::Supervisor => CSR_SCAUSE_ADDRESS,
            PrivilegeMode::User => CSR_UCAUSE_ADDRESS,
            PrivilegeMode::Reserved => panic!(),
        };
        let csr_tval_address = match self.privilege_mode {
            PrivilegeMode::Machine => CSR_MTVAL_ADDRESS,
            PrivilegeMode::Supervisor => CSR_STVAL_ADDRESS,
            PrivilegeMode::User => CSR_UTVAL_ADDRESS,
            PrivilegeMode::Reserved => panic!(),
        };
        let csr_tvec_address = match self.privilege_mode {
            PrivilegeMode::Machine => CSR_MTVEC_ADDRESS,
            PrivilegeMode::Supervisor => CSR_STVEC_ADDRESS,
            PrivilegeMode::User => CSR_UTVEC_ADDRESS,
            PrivilegeMode::Reserved => panic!(),
        };

        self.write_csr_raw(csr_epc_address, instruction_address);
        self.write_csr_raw(csr_cause_address, cause);
        self.write_csr_raw(csr_tval_address, trap.value);
        self.pc = self.read_csr_raw(csr_tvec_address);

        // Add 4 * cause if tvec has vector type address
        if (self.pc & 0x3) != 0 {
            self.pc = (self.pc & !0x3) + 4 * (cause & 0xffff);
        }

        match self.privilege_mode {
            PrivilegeMode::Machine => {
                let status = self.read_csr_raw(CSR_MSTATUS_ADDRESS);
                let mie = (status >> 3) & 1;
                // clear MIE[3], override MPIE[7] with MIE[3], override MPP[12:11] with current privilege encoding
                let new_status =
                    (status & !0x1888) | (mie << 7) | (current_privilege_encoding << 11);
                self.write_csr_raw(CSR_MSTATUS_ADDRESS, new_status);
            }
            PrivilegeMode::Supervisor => {
                let status = self.read_csr_raw(CSR_SSTATUS_ADDRESS);
                let sie = (status >> 1) & 1;
                // clear SIE[1], override SPIE[5] with SIE[1], override SPP[8] with current privilege encoding
                let new_status =
                    (status & !0x122) | (sie << 5) | ((current_privilege_encoding & 1) << 8);
                self.write_csr_raw(CSR_SSTATUS_ADDRESS, new_status);
            }
            PrivilegeMode::User => {
                panic!("Not implemented yet");
            }
            PrivilegeMode::Reserved => panic!(), // shouldn't happen
        };
        //println!("Trap! {:x} Clock:{:x}", cause, self.clock);
        true
    }

    fn fetch(&mut self) -> Result<u32, Trap> {
        // 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 {
        let privilege = (address >> 8) & 0x3; // the lowest privilege level that can access the CSR
        privilege as u8 <= get_privilege_encoding(&self.privilege_mode)
    }

    fn read_csr(&mut self, address: u16) -> Result<u64, Trap> {
        match self.has_csr_access_privilege(address) {
            true => Ok(self.read_csr_raw(address)),
            false => Err(Trap {
                trap_type: TrapType::IllegalInstruction,
                value: self.pc.wrapping_sub(4), // @TODO: Is this always correct?
            }),
        }
    }

    pub fn write_csr(&mut self, address: u16, value: u64) -> Result<(), Trap> {
        match self.has_csr_access_privilege(address) {
            true => {
                /*
                // Checking writability fails some tests so disabling so far
                let read_only = ((address >> 10) & 0x3) == 0x3;
                if read_only {
                    return Err(Exception::IllegalInstruction);
                }
                */
                self.write_csr_raw(address, value);
                if address == CSR_SATP_ADDRESS {
                    self.update_addressing_mode(value);
                }
                Ok(())
            }
            false => Err(Trap {
                trap_type: TrapType::IllegalInstruction,
                value: self.pc.wrapping_sub(4), // @TODO: Is this always correct?
            }),
        }
    }

    // SSTATUS, SIE, and SIP are subsets of MSTATUS, MIE, and MIP
    fn read_csr_raw(&self, address: u16) -> u64 {
        match address {
            // @TODO: Mask shuld consider of 32-bit mode
            CSR_FFLAGS_ADDRESS => self.csr[CSR_FCSR_ADDRESS as usize] & 0x1f,
            CSR_FRM_ADDRESS => (self.csr[CSR_FCSR_ADDRESS as usize] >> 5) & 0x7,
            CSR_SSTATUS_ADDRESS => self.csr[CSR_MSTATUS_ADDRESS as usize] & 0x80000003000de162,
            CSR_SIE_ADDRESS => self.csr[CSR_MIE_ADDRESS as usize] & 0x222,
            CSR_SIP_ADDRESS => self.csr[CSR_MIP_ADDRESS as usize] & 0x222,
            // CSR_TIME_ADDRESS => self.mmu.get_clint().read_mtime(),
            _ => self.csr[address as usize],
        }
    }

    fn write_csr_raw(&mut self, address: u16, value: u64) {
        match address {
            CSR_FFLAGS_ADDRESS => {
                self.csr[CSR_FCSR_ADDRESS as usize] &= !0x1f;
                self.csr[CSR_FCSR_ADDRESS as usize] |= value & 0x1f;
            }
            CSR_FRM_ADDRESS => {
                self.csr[CSR_FCSR_ADDRESS as usize] &= !0xe0;
                self.csr[CSR_FCSR_ADDRESS as usize] |= (value << 5) & 0xe0;
            }
            CSR_SSTATUS_ADDRESS => {
                self.csr[CSR_MSTATUS_ADDRESS as usize] &= !0x80000003000de162;
                self.csr[CSR_MSTATUS_ADDRESS as usize] |= value & 0x80000003000de162;
                self.mmu
                    .update_mstatus(self.read_csr_raw(CSR_MSTATUS_ADDRESS));
            }
            CSR_SIE_ADDRESS => {
                self.csr[CSR_MIE_ADDRESS as usize] &= !0x222;
                self.csr[CSR_MIE_ADDRESS as usize] |= value & 0x222;
            }
            CSR_SIP_ADDRESS => {
                self.csr[CSR_MIP_ADDRESS as usize] &= !0x222;
                self.csr[CSR_MIP_ADDRESS as usize] |= value & 0x222;
            }
            CSR_MIDELEG_ADDRESS => {
                self.csr[address as usize] = value & 0x666; // from qemu
            }
            CSR_MSTATUS_ADDRESS => {
                self.csr[address as usize] = value;
                self.mmu
                    .update_mstatus(self.read_csr_raw(CSR_MSTATUS_ADDRESS));
            }
            // CSR_TIME_ADDRESS => {
            //     self.mmu.get_mut_clint().write_mtime(value);
            // }
            _ => {
                self.csr[address as usize] = value;
            }
        };
    }

    fn _set_fcsr_nv(&mut self) {
        self.csr[CSR_FCSR_ADDRESS as usize] |= 0x10;
    }

    fn set_fcsr_dz(&mut self) {
        self.csr[CSR_FCSR_ADDRESS as usize] |= 0x8;
    }

    fn _set_fcsr_of(&mut self) {
        self.csr[CSR_FCSR_ADDRESS as usize] |= 0x4;
    }

    fn _set_fcsr_uf(&mut self) {
        self.csr[CSR_FCSR_ADDRESS as usize] |= 0x2;
    }

    fn _set_fcsr_nx(&mut self) {
        self.csr[CSR_FCSR_ADDRESS as usize] |= 0x1;
    }

    fn update_addressing_mode(&mut self, value: u64) {
        let addressing_mode = match self.xlen {
            Xlen::Bit32 => match value & 0x80000000 {
                0 => AddressingMode::None,
                _ => AddressingMode::SV32,
            },
            Xlen::Bit64 => match value >> 60 {
                0 => AddressingMode::None,
                8 => AddressingMode::SV39,
                9 => AddressingMode::SV48,
                _ => {
                    println!("Unknown addressing_mode {:x}", value >> 60);
                    panic!();
                }
            },
        };
        let ppn = match self.xlen {
            Xlen::Bit32 => value & 0x3fffff,
            Xlen::Bit64 => value & 0xfffffffffff,
        };
        self.mmu.update_addressing_mode(addressing_mode);
        self.mmu.update_ppn(ppn);
    }

    // @TODO: Rename to better name?
    fn sign_extend(&self, value: i64) -> i64 {
        match self.xlen {
            Xlen::Bit32 => value as i32 as i64,
            Xlen::Bit64 => value,
        }
    }

    // @TODO: Rename to better name?
    fn unsigned_data(&self, value: i64) -> u64 {
        (value as u64) & self.unsigned_data_mask
    }

    // @TODO: Rename to better name?
    fn most_negative(&self) -> i64 {
        match self.xlen {
            Xlen::Bit32 => std::i32::MIN as i64,
            Xlen::Bit64 => std::i64::MIN,
        }
    }

    // @TODO: Optimize
    fn uncompress(&self, halfword: u32) -> u32 {
        let op = halfword & 0x3; // [1:0]
        let funct3 = (halfword >> 13) & 0x7; // [15:13]

        match op {
            0 => match funct3 {
                0 => {
                    // C.ADDI4SPN
                    // addi rd+8, x2, nzuimm
                    let rd = (halfword >> 2) & 0x7; // [4:2]
                    let nzuimm = ((halfword >> 7) & 0x30) | // nzuimm[5:4] <= [12:11]
						((halfword >> 1) & 0x3c0) | // nzuimm{9:6] <= [10:7]
						((halfword >> 4) & 0x4) | // nzuimm[2] <= [6]
						((halfword >> 2) & 0x8); // nzuimm[3] <= [5]
                         // nzuimm == 0 is reserved instruction
                    if nzuimm != 0 {
                        return (nzuimm << 20) | (2 << 15) | ((rd + 8) << 7) | 0x13;
                    }
                }
                1 => {
                    // @TODO: Support C.LQ for 128-bit
                    // C.FLD for 32, 64-bit
                    // fld rd+8, offset(rs1+8)
                    let rd = (halfword >> 2) & 0x7; // [4:2]
                    let rs1 = (halfword >> 7) & 0x7; // [9:7]
                    let offset = ((halfword >> 7) & 0x38) | // offset[5:3] <= [12:10]
						((halfword << 1) & 0xc0); // offset[7:6] <= [6:5]
                    return (offset << 20) | ((rs1 + 8) << 15) | (3 << 12) | ((rd + 8) << 7) | 0x7;
                }
                2 => {
                    // C.LW
                    // lw rd+8, offset(rs1+8)
                    let rs1 = (halfword >> 7) & 0x7; // [9:7]
                    let rd = (halfword >> 2) & 0x7; // [4:2]
                    let offset = ((halfword >> 7) & 0x38) | // offset[5:3] <= [12:10]
						((halfword >> 4) & 0x4) | // offset[2] <= [6]
						((halfword << 1) & 0x40); // offset[6] <= [5]
                    return (offset << 20) | ((rs1 + 8) << 15) | (2 << 12) | ((rd + 8) << 7) | 0x3;
                }
                3 => {
                    // @TODO: Support C.FLW in 32-bit mode
                    // C.LD in 64-bit mode
                    // ld rd+8, offset(rs1+8)
                    let rs1 = (halfword >> 7) & 0x7; // [9:7]
                    let rd = (halfword >> 2) & 0x7; // [4:2]
                    let offset = ((halfword >> 7) & 0x38) | // offset[5:3] <= [12:10]
						((halfword << 1) & 0xc0); // offset[7:6] <= [6:5]
                    return (offset << 20) | ((rs1 + 8) << 15) | (3 << 12) | ((rd + 8) << 7) | 0x3;
                }
                4 => {
                    // Reserved
                }
                5 => {
                    // C.FSD
                    // fsd rs2+8, offset(rs1+8)
                    let rs1 = (halfword >> 7) & 0x7; // [9:7]
                    let rs2 = (halfword >> 2) & 0x7; // [4:2]
                    let offset = ((halfword >> 7) & 0x38) | // uimm[5:3] <= [12:10]
						((halfword << 1) & 0xc0); // uimm[7:6] <= [6:5]
                    let imm11_5 = (offset >> 5) & 0x7f;
                    let imm4_0 = offset & 0x1f;
                    return (imm11_5 << 25)
                        | ((rs2 + 8) << 20)
                        | ((rs1 + 8) << 15)
                        | (3 << 12)
                        | (imm4_0 << 7)
                        | 0x27;
                }
                6 => {
                    // C.SW
                    // sw rs2+8, offset(rs1+8)
                    let rs1 = (halfword >> 7) & 0x7; // [9:7]
                    let rs2 = (halfword >> 2) & 0x7; // [4:2]
                    let offset = ((halfword >> 7) & 0x38) | // offset[5:3] <= [12:10]
						((halfword << 1) & 0x40) | // offset[6] <= [5]
						((halfword >> 4) & 0x4); // offset[2] <= [6]
                    let imm11_5 = (offset >> 5) & 0x7f;
                    let imm4_0 = offset & 0x1f;
                    return (imm11_5 << 25)
                        | ((rs2 + 8) << 20)
                        | ((rs1 + 8) << 15)
                        | (2 << 12)
                        | (imm4_0 << 7)
                        | 0x23;
                }
                7 => {
                    // @TODO: Support C.FSW in 32-bit mode
                    // C.SD
                    // sd rs2+8, offset(rs1+8)
                    let rs1 = (halfword >> 7) & 0x7; // [9:7]
                    let rs2 = (halfword >> 2) & 0x7; // [4:2]
                    let offset = ((halfword >> 7) & 0x38) | // uimm[5:3] <= [12:10]
						((halfword << 1) & 0xc0); // uimm[7:6] <= [6:5]
                    let imm11_5 = (offset >> 5) & 0x7f;
                    let imm4_0 = offset & 0x1f;
                    return (imm11_5 << 25)
                        | ((rs2 + 8) << 20)
                        | ((rs1 + 8) << 15)
                        | (3 << 12)
                        | (imm4_0 << 7)
                        | 0x23;
                }
                _ => {} // Not happens
            },
            1 => {
                match funct3 {
                    0 => {
                        let r = (halfword >> 7) & 0x1f; // [11:7]
                        let imm = match halfword & 0x1000 {
							0x1000 => 0xffffffc0,
							_ => 0
						} | // imm[31:6] <= [12]
						((halfword >> 7) & 0x20) | // imm[5] <= [12]
						((halfword >> 2) & 0x1f); // imm[4:0] <= [6:2]
                        if r == 0 && imm == 0 {
                            // C.NOP
                            // addi x0, x0, 0
                            return 0x13;
                        } else if r != 0 {
                            // C.ADDI
                            // addi r, r, imm
                            return (imm << 20) | (r << 15) | (r << 7) | 0x13;
                        }
                        // @TODO: Support HINTs
                        // r == 0 and imm != 0 is HINTs
                    }
                    1 => {
                        // @TODO: Support C.JAL in 32-bit mode
                        // C.ADDIW
                        // addiw r, r, imm
                        let r = (halfword >> 7) & 0x1f;
                        let imm = match halfword & 0x1000 {
							0x1000 => 0xffffffc0,
							_ => 0
						} | // imm[31:6] <= [12]
						((halfword >> 7) & 0x20) | // imm[5] <= [12]
						((halfword >> 2) & 0x1f); // imm[4:0] <= [6:2]
                        if r != 0 {
                            return (imm << 20) | (r << 15) | (r << 7) | 0x1b;
                        }
                        // r == 0 is reserved instruction
                    }
                    2 => {
                        // C.LI
                        // addi rd, x0, imm
                        let r = (halfword >> 7) & 0x1f;
                        let imm = match halfword & 0x1000 {
							0x1000 => 0xffffffc0,
							_ => 0
						} | // imm[31:6] <= [12]
						((halfword >> 7) & 0x20) | // imm[5] <= [12]
						((halfword >> 2) & 0x1f); // imm[4:0] <= [6:2]
                        if r != 0 {
                            return (imm << 20) | (r << 7) | 0x13;
                        }
                        // @TODO: Support HINTs
                        // r == 0 is for HINTs
                    }
                    3 => {
                        let r = (halfword >> 7) & 0x1f; // [11:7]
                        if r == 2 {
                            // C.ADDI16SP
                            // addi r, r, nzimm
                            let imm = match halfword & 0x1000 {
								0x1000 => 0xfffffc00,
								_ => 0
							} | // imm[31:10] <= [12]
							((halfword >> 3) & 0x200) | // imm[9] <= [12]
							((halfword >> 2) & 0x10) | // imm[4] <= [6]
							((halfword << 1) & 0x40) | // imm[6] <= [5]
							((halfword << 4) & 0x180) | // imm[8:7] <= [4:3]
							((halfword << 3) & 0x20); // imm[5] <= [2]
                            if imm != 0 {
                                return (imm << 20) | (r << 15) | (r << 7) | 0x13;
                            }
                            // imm == 0 is for reserved instruction
                        }
                        if r != 0 && r != 2 {
                            // C.LUI
                            // lui r, nzimm
                            let nzimm = match halfword & 0x1000 {
								0x1000 => 0xfffc0000,
								_ => 0
							} | // nzimm[31:18] <= [12]
							((halfword << 5) & 0x20000) | // nzimm[17] <= [12]
							((halfword << 10) & 0x1f000); // nzimm[16:12] <= [6:2]
                            if nzimm != 0 {
                                return nzimm | (r << 7) | 0x37;
                            }
                            // nzimm == 0 is for reserved instruction
                        }
                    }
                    4 => {
                        let funct2 = (halfword >> 10) & 0x3; // [11:10]
                        match funct2 {
                            0 => {
                                // C.SRLI
                                // c.srli rs1+8, rs1+8, shamt
                                let shamt = ((halfword >> 7) & 0x20) | // shamt[5] <= [12]
									((halfword >> 2) & 0x1f); // shamt[4:0] <= [6:2]
                                let rs1 = (halfword >> 7) & 0x7; // [9:7]
                                return (shamt << 20)
                                    | ((rs1 + 8) << 15)
                                    | (5 << 12)
                                    | ((rs1 + 8) << 7)
                                    | 0x13;
                            }
                            1 => {
                                // C.SRAI
                                // srai rs1+8, rs1+8, shamt
                                let shamt = ((halfword >> 7) & 0x20) | // shamt[5] <= [12]
									((halfword >> 2) & 0x1f); // shamt[4:0] <= [6:2]
                                let rs1 = (halfword >> 7) & 0x7; // [9:7]
                                return (0x20 << 25)
                                    | (shamt << 20)
                                    | ((rs1 + 8) << 15)
                                    | (5 << 12)
                                    | ((rs1 + 8) << 7)
                                    | 0x13;
                            }
                            2 => {
                                // C.ANDI
                                // andi, r+8, r+8, imm
                                let r = (halfword >> 7) & 0x7; // [9:7]
                                let imm = match halfword & 0x1000 {
									0x1000 => 0xffffffc0,
									_ => 0
								} | // imm[31:6] <= [12]
								((halfword >> 7) & 0x20) | // imm[5] <= [12]
								((halfword >> 2) & 0x1f); // imm[4:0] <= [6:2]
                                return (imm << 20)
                                    | ((r + 8) << 15)
                                    | (7 << 12)
                                    | ((r + 8) << 7)
                                    | 0x13;
                            }
                            3 => {
                                let funct1 = (halfword >> 12) & 1; // [12]
                                let funct2_2 = (halfword >> 5) & 0x3; // [6:5]
                                let rs1 = (halfword >> 7) & 0x7;
                                let rs2 = (halfword >> 2) & 0x7;
                                match funct1 {
                                    0 => match funct2_2 {
                                        0 => {
                                            // C.SUB
                                            // sub rs1+8, rs1+8, rs2+8
                                            return (0x20 << 25)
                                                | ((rs2 + 8) << 20)
                                                | ((rs1 + 8) << 15)
                                                | ((rs1 + 8) << 7)
                                                | 0x33;
                                        }
                                        1 => {
                                            // C.XOR
                                            // xor rs1+8, rs1+8, rs2+8
                                            return ((rs2 + 8) << 20)
                                                | ((rs1 + 8) << 15)
                                                | (4 << 12)
                                                | ((rs1 + 8) << 7)
                                                | 0x33;
                                        }
                                        2 => {
                                            // C.OR
                                            // or rs1+8, rs1+8, rs2+8
                                            return ((rs2 + 8) << 20)
                                                | ((rs1 + 8) << 15)
                                                | (6 << 12)
                                                | ((rs1 + 8) << 7)
                                                | 0x33;
                                        }
                                        3 => {
                                            // C.AND
                                            // and rs1+8, rs1+8, rs2+8
                                            return ((rs2 + 8) << 20)
                                                | ((rs1 + 8) << 15)
                                                | (7 << 12)
                                                | ((rs1 + 8) << 7)
                                                | 0x33;
                                        }
                                        _ => {} // Not happens
                                    },
                                    1 => match funct2_2 {
                                        0 => {
                                            // C.SUBW
                                            // subw r1+8, r1+8, r2+8
                                            return (0x20 << 25)
                                                | ((rs2 + 8) << 20)
                                                | ((rs1 + 8) << 15)
                                                | ((rs1 + 8) << 7)
                                                | 0x3b;
                                        }
                                        1 => {
                                            // C.ADDW
                                            // addw r1+8, r1+8, r2+8
                                            return ((rs2 + 8) << 20)
                                                | ((rs1 + 8) << 15)
                                                | ((rs1 + 8) << 7)
                                                | 0x3b;
                                        }
                                        2 => {
                                            // Reserved
                                        }
                                        3 => {
                                            // Reserved
                                        }
                                        _ => {} // Not happens
                                    },
                                    _ => {} // No happens
                                };
                            }
                            _ => {} // not happens
                        };
                    }
                    5 => {
                        // C.J
                        // jal x0, imm
                        let offset = match halfword & 0x1000 {
								0x1000 => 0xfffff000,
								_ => 0
							} | // offset[31:12] <= [12]
							((halfword >> 1) & 0x800) | // offset[11] <= [12]
							((halfword >> 7) & 0x10) | // offset[4] <= [11]
							((halfword >> 1) & 0x300) | // offset[9:8] <= [10:9]
							((halfword << 2) & 0x400) | // offset[10] <= [8]
							((halfword >> 1) & 0x40) | // offset[6] <= [7]
							((halfword << 1) & 0x80) | // offset[7] <= [6]
							((halfword >> 2) & 0xe) | // offset[3:1] <= [5:3]
							((halfword << 3) & 0x20); // offset[5] <= [2]
                        let imm = ((offset >> 1) & 0x80000) | // imm[19] <= offset[20]
							((offset << 8) & 0x7fe00) | // imm[18:9] <= offset[10:1]
							((offset >> 3) & 0x100) | // imm[8] <= offset[11]
							((offset >> 12) & 0xff); // imm[7:0] <= offset[19:12]
                        return (imm << 12) | 0x6f;
                    }
                    6 => {
                        // C.BEQZ
                        // beq r+8, x0, offset
                        let r = (halfword >> 7) & 0x7;
                        let offset = match halfword & 0x1000 {
								0x1000 => 0xfffffe00,
								_ => 0
							} | // offset[31:9] <= [12]
							((halfword >> 4) & 0x100) | // offset[8] <= [12]
							((halfword >> 7) & 0x18) | // offset[4:3] <= [11:10]
							((halfword << 1) & 0xc0) | // offset[7:6] <= [6:5]
							((halfword >> 2) & 0x6) | // offset[2:1] <= [4:3]
							((halfword << 3) & 0x20); // offset[5] <= [2]
                        let imm2 = ((offset >> 6) & 0x40) | // imm2[6] <= [12]
							((offset >> 5) & 0x3f); // imm2[5:0] <= [10:5]
                        let imm1 = (offset & 0x1e) | // imm1[4:1] <= [4:1]
							((offset >> 11) & 0x1); // imm1[0] <= [11]
                        return (imm2 << 25) | ((r + 8) << 20) | (imm1 << 7) | 0x63;
                    }
                    7 => {
                        // C.BNEZ
                        // bne r+8, x0, offset
                        let r = (halfword >> 7) & 0x7;
                        let offset = match halfword & 0x1000 {
								0x1000 => 0xfffffe00,
								_ => 0
							} | // offset[31:9] <= [12]
							((halfword >> 4) & 0x100) | // offset[8] <= [12]
							((halfword >> 7) & 0x18) | // offset[4:3] <= [11:10]
							((halfword << 1) & 0xc0) | // offset[7:6] <= [6:5]
							((halfword >> 2) & 0x6) | // offset[2:1] <= [4:3]
							((halfword << 3) & 0x20); // offset[5] <= [2]
                        let imm2 = ((offset >> 6) & 0x40) | // imm2[6] <= [12]
							((offset >> 5) & 0x3f); // imm2[5:0] <= [10:5]
                        let imm1 = (offset & 0x1e) | // imm1[4:1] <= [4:1]
							((offset >> 11) & 0x1); // imm1[0] <= [11]
                        return (imm2 << 25) | ((r + 8) << 20) | (1 << 12) | (imm1 << 7) | 0x63;
                    }
                    _ => {} // No happens
                };
            }
            2 => {
                match funct3 {
                    0 => {
                        // C.SLLI
                        // slli r, r, shamt
                        let r = (halfword >> 7) & 0x1f;
                        let shamt = ((halfword >> 7) & 0x20) | // imm[5] <= [12]
							((halfword >> 2) & 0x1f); // imm[4:0] <= [6:2]
                        if r != 0 {
                            return (shamt << 20) | (r << 15) | (1 << 12) | (r << 7) | 0x13;
                        }
                        // r == 0 is reserved instruction?
                    }
                    1 => {
                        // C.FLDSP
                        // fld rd, offset(x2)
                        let rd = (halfword >> 7) & 0x1f;
                        let offset = ((halfword >> 7) & 0x20) | // offset[5] <= [12]
							((halfword >> 2) & 0x18) | // offset[4:3] <= [6:5]
							((halfword << 4) & 0x1c0); // offset[8:6] <= [4:2]
                        if rd != 0 {
                            return (offset << 20) | (2 << 15) | (3 << 12) | (rd << 7) | 0x7;
                        }
                        // rd == 0 is reseved instruction
                    }
                    2 => {
                        // C.LWSP
                        // lw r, offset(x2)
                        let r = (halfword >> 7) & 0x1f;
                        let offset = ((halfword >> 7) & 0x20) | // offset[5] <= [12]
							((halfword >> 2) & 0x1c) | // offset[4:2] <= [6:4]
							((halfword << 4) & 0xc0); // offset[7:6] <= [3:2]
                        if r != 0 {
                            return (offset << 20) | (2 << 15) | (2 << 12) | (r << 7) | 0x3;
                        }
                        // r == 0 is reseved instruction
                    }
                    3 => {
                        // @TODO: Support C.FLWSP in 32-bit mode
                        // C.LDSP
                        // ld rd, offset(x2)
                        let rd = (halfword >> 7) & 0x1f;
                        let offset = ((halfword >> 7) & 0x20) | // offset[5] <= [12]
							((halfword >> 2) & 0x18) | // offset[4:3] <= [6:5]
							((halfword << 4) & 0x1c0); // offset[8:6] <= [4:2]
                        if rd != 0 {
                            return (offset << 20) | (2 << 15) | (3 << 12) | (rd << 7) | 0x3;
                        }
                        // rd == 0 is reseved instruction
                    }
                    4 => {
                        let funct1 = (halfword >> 12) & 1; // [12]
                        let rs1 = (halfword >> 7) & 0x1f; // [11:7]
                        let rs2 = (halfword >> 2) & 0x1f; // [6:2]
                        match funct1 {
                            0 => {
                                if rs1 != 0 && rs2 == 0 {
                                    // C.JR
                                    // jalr x0, 0(rs1)
                                    return (rs1 << 15) | 0x67;
                                }
                                // rs1 == 0 is reserved instruction
                                if rs1 != 0 && rs2 != 0 {
                                    // C.MV
                                    // add rs1, x0, rs2
                                    // println!("C.MV RS1:{:x} RS2:{:x}", rs1, rs2);
                                    return (rs2 << 20) | (rs1 << 7) | 0x33;
                                }
                                // rs1 == 0 && rs2 != 0 is Hints
                                // @TODO: Support Hints
                            }
                            1 => {
                                if rs1 == 0 && rs2 == 0 {
                                    // C.EBREAK
                                    // ebreak
                                    return 0x00100073;
                                }
                                if rs1 != 0 && rs2 == 0 {
                                    // C.JALR
                                    // jalr x1, 0(rs1)
                                    return (rs1 << 15) | (1 << 7) | 0x67;
                                }
                                if rs1 != 0 && rs2 != 0 {
                                    // C.ADD
                                    // add rs1, rs1, rs2
                                    return (rs2 << 20) | (rs1 << 15) | (rs1 << 7) | 0x33;
                                }
                                // rs1 == 0 && rs2 != 0 is Hists
                                // @TODO: Supports Hinsts
                            }
                            _ => {} // Not happens
                        };
                    }
                    5 => {
                        // @TODO: Implement
                        // C.FSDSP
                        // fsd rs2, offset(x2)
                        let rs2 = (halfword >> 2) & 0x1f; // [6:2]
                        let offset = ((halfword >> 7) & 0x38) | // offset[5:3] <= [12:10]
							((halfword >> 1) & 0x1c0); // offset[8:6] <= [9:7]
                        let imm11_5 = (offset >> 5) & 0x3f;
                        let imm4_0 = offset & 0x1f;
                        return (imm11_5 << 25)
                            | (rs2 << 20)
                            | (2 << 15)
                            | (3 << 12)
                            | (imm4_0 << 7)
                            | 0x27;
                    }
                    6 => {
                        // C.SWSP
                        // sw rs2, offset(x2)
                        let rs2 = (halfword >> 2) & 0x1f; // [6:2]
                        let offset = ((halfword >> 7) & 0x3c) | // offset[5:2] <= [12:9]
							((halfword >> 1) & 0xc0); // offset[7:6] <= [8:7]
                        let imm11_5 = (offset >> 5) & 0x3f;
                        let imm4_0 = offset & 0x1f;
                        return (imm11_5 << 25)
                            | (rs2 << 20)
                            | (2 << 15)
                            | (2 << 12)
                            | (imm4_0 << 7)
                            | 0x23;
                    }
                    7 => {
                        // @TODO: Support C.FSWSP in 32-bit mode
                        // C.SDSP
                        // sd rs, offset(x2)
                        let rs2 = (halfword >> 2) & 0x1f; // [6:2]
                        let offset = ((halfword >> 7) & 0x38) | // offset[5:3] <= [12:10]
							((halfword >> 1) & 0x1c0); // offset[8:6] <= [9:7]
                        let imm11_5 = (offset >> 5) & 0x3f;
                        let imm4_0 = offset & 0x1f;
                        return (imm11_5 << 25)
                            | (rs2 << 20)
                            | (2 << 15)
                            | (3 << 12)
                            | (imm4_0 << 7)
                            | 0x23;
                    }
                    _ => {} // Not happens
                };
            }
            _ => {} // No happnes
        };
        0xffffffff // Return invalid value
    }

    /// Disassembles an instruction pointed by Program Counter.
    pub fn disassemble_next_instruction(&mut self) -> String {
        // @TODO: Fetching can make a side effect,
        // 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 Ok(mut original_word) = self.mmu.fetch_word(self.pc) else {
            return format!("PC:{:016x}, InstructionPageFault Trap!\n", self.pc);
        };

        let word = if (original_word & 0x3) == 0x3 {
            original_word
        } else {
            original_word &= 0xffff;
            self.uncompress(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));
        s += &format!("{:08x} ", original_word);
        s += &format!("{} ", inst.name);
        s += &(inst.disassemble)(self, word, self.pc, true).to_string();
        s
    }

    /// Returns mutable `Mmu`
    pub fn get_mut_mmu(&mut self) -> &mut Mmu {
        &mut self.mmu
    }

    pub fn phys_read_u32(&self, address: u64) -> u32 {
        self.mmu.load_word_raw(address)
    }

    pub fn phys_write_u32(&mut self, address: u64, value: u32) {
        self.mmu.store_word_raw(address, value)
    }

    pub fn phys_read_u8(&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 {
    mask: u32,
    data: u32, // @TODO: rename
    name: &'static str,
    operation: fn(cpu: &mut Cpu, word: u32, address: u64) -> Result<(), Trap>,
    disassemble: fn(cpu: &Cpu, word: u32, address: u64, evaluate: bool) -> String,
}

struct FormatB {
    rs1: usize,
    rs2: usize,
    imm: u64,
}

fn parse_format_b(word: u32) -> FormatB {
    FormatB {
        rs1: ((word >> 15) & 0x1f) as usize, // [19:15]
        rs2: ((word >> 20) & 0x1f) as usize, // [24:20]
        imm: (
            match word & 0x80000000 { // imm[31:12] = [31]
				0x80000000 => 0xfffff000,
				_ => 0
			} |
			((word << 4) & 0x00000800) | // imm[11] = [7]
			((word >> 20) & 0x000007e0) | // imm[10:5] = [30:25]
			((word >> 7) & 0x0000001e)
            // imm[4:1] = [11:8]
        ) as i32 as i64 as u64,
    }
}

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);
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs1]);
    }
    s += &format!(",{}", get_register_name(f.rs2));
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs2]);
    }
    s += &format!(",{:x}", address.wrapping_add(f.imm));
    s
}

struct FormatCSR {
    csr: u16,
    rs: usize,
    rd: usize,
}

fn parse_format_csr(word: u32) -> FormatCSR {
    FormatCSR {
        csr: ((word >> 20) & 0xfff) as u16, // [31:20]
        rs: ((word >> 15) & 0x1f) as usize, // [19:15], also uimm
        rd: ((word >> 7) & 0x1f) as usize,  // [11:7]
    }
}

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);
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rd]);
    }
    // @TODO: Use CSR name
    s += &format!(",{:x}", f.csr);
    if evaluate {
        s += &format!(":{:x}", cpu.read_csr_raw(f.csr));
    }
    s += &format!(",{}", get_register_name(f.rs));
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs]);
    }
    s
}

struct FormatI {
    rd: usize,
    rs1: usize,
    imm: i64,
}

fn parse_format_i(word: u32) -> FormatI {
    FormatI {
        rd: ((word >> 7) & 0x1f) as usize,   // [11:7]
        rs1: ((word >> 15) & 0x1f) as usize, // [19:15]
        imm: (
            match word & 0x80000000 {
                // imm[31:11] = [31]
                0x80000000 => 0xfffff800,
                _ => 0,
            } | ((word >> 20) & 0x000007ff)
            // imm[10:0] = [30:20]
        ) as i32 as i64,
    }
}

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);
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rd]);
    }
    s += &format!(",{}", get_register_name(f.rs1));
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs1]);
    }
    s += &format!(",{:x}", f.imm);
    s
}

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);
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rd]);
    }
    s += &format!(",{:x}({}", f.imm, get_register_name(f.rs1));
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs1]);
    }
    s += ")";
    s
}

struct FormatJ {
    rd: usize,
    imm: u64,
}

fn parse_format_j(word: u32) -> FormatJ {
    FormatJ {
        rd: ((word >> 7) & 0x1f) as usize, // [11:7]
        imm: (
            match word & 0x80000000 { // imm[31:20] = [31]
				0x80000000 => 0xfff00000,
				_ => 0
			} |
			(word & 0x000ff000) | // imm[19:12] = [19:12]
			((word & 0x00100000) >> 9) | // imm[11] = [20]
			((word & 0x7fe00000) >> 20)
            // imm[10:1] = [30:21]
        ) as i32 as i64 as u64,
    }
}

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);
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rd]);
    }
    s += &format!(",{:x}", address.wrapping_add(f.imm));
    s
}

struct FormatR {
    rd: usize,
    rs1: usize,
    rs2: usize,
}

fn parse_format_r(word: u32) -> FormatR {
    FormatR {
        rd: ((word >> 7) & 0x1f) as usize,   // [11:7]
        rs1: ((word >> 15) & 0x1f) as usize, // [19:15]
        rs2: ((word >> 20) & 0x1f) as usize, // [24:20]
    }
}

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);
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rd]);
    }
    s += &format!(",{}", get_register_name(f.rs1));
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs1]);
    }
    s += &format!(",{}", get_register_name(f.rs2));
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs2]);
    }
    s
}

// has rs3
struct FormatR2 {
    rd: usize,
    rs1: usize,
    rs2: usize,
    rs3: usize,
}

fn parse_format_r2(word: u32) -> FormatR2 {
    FormatR2 {
        rd: ((word >> 7) & 0x1f) as usize,   // [11:7]
        rs1: ((word >> 15) & 0x1f) as usize, // [19:15]
        rs2: ((word >> 20) & 0x1f) as usize, // [24:20]
        rs3: ((word >> 27) & 0x1f) as usize, // [31:27]
    }
}

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);
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rd]);
    }
    s += &format!(",{}", get_register_name(f.rs1));
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs1]);
    }
    s += &format!(",{}", get_register_name(f.rs2));
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs2]);
    }
    s += &format!(",{}", get_register_name(f.rs3));
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs3]);
    }
    s
}

struct FormatS {
    rs1: usize,
    rs2: usize,
    imm: i64,
}

fn parse_format_s(word: u32) -> FormatS {
    FormatS {
        rs1: ((word >> 15) & 0x1f) as usize, // [19:15]
        rs2: ((word >> 20) & 0x1f) as usize, // [24:20]
        imm: (
            match word & 0x80000000 {
				0x80000000 => 0xfffff000,
				_ => 0
			} | // imm[31:12] = [31]
			((word >> 20) & 0xfe0) | // imm[11:5] = [31:25]
			((word >> 7) & 0x1f)
            // imm[4:0] = [11:7]
        ) as i32 as i64,
    }
}

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);
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs2]);
    }
    s += &format!(",{:x}({}", f.imm, get_register_name(f.rs1));
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rs1]);
    }
    s += ")";
    s
}

struct FormatU {
    rd: usize,
    imm: u64,
}

fn parse_format_u(word: u32) -> FormatU {
    FormatU {
        rd: ((word >> 7) & 0x1f) as usize, // [11:7]
        imm: (
            match word & 0x80000000 {
				0x80000000 => 0xffffffff00000000,
				_ => 0
			} | // imm[63:32] = [31]
			((word as u64) & 0xfffff000)
            // imm[31:12] = [31:12]
        ),
    }
}

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);
    if evaluate {
        s += &format!(":{:x}", cpu.x[f.rd]);
    }
    s += &format!(",{:x}", f.imm);
    s
}

fn dump_empty(_cpu: &Cpu, _word: u32, _address: u64, _evaluate: bool) -> String {
    String::new()
}

fn get_register_name(num: usize) -> &'static str {
    match num {
        0 => "zero",
        1 => "ra",
        2 => "sp",
        3 => "gp",
        4 => "tp",
        5 => "t0",
        6 => "t1",
        7 => "t2",
        8 => "s0",
        9 => "s1",
        10 => "a0",
        11 => "a1",
        12 => "a2",
        13 => "a3",
        14 => "a4",
        15 => "a5",
        16 => "a6",
        17 => "a7",
        18 => "s2",
        19 => "s3",
        20 => "s4",
        21 => "s5",
        22 => "s6",
        23 => "s7",
        24 => "s8",
        25 => "s9",
        26 => "s10",
        27 => "s11",
        28 => "t3",
        29 => "t4",
        30 => "t5",
        31 => "t6",
        _ => panic!("Unknown register num {}", num),
    }
}

const INSTRUCTION_NUM: usize = 116;

// @TODO: Reorder in often used order as
const INSTRUCTIONS: [Instruction; INSTRUCTION_NUM] = [
    Instruction {
        mask: 0xfe00707f,
        data: 0x00000033,
        name: "ADD",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1].wrapping_add(cpu.x[f.rs2]));
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00000013,
        name: "ADDI",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1].wrapping_add(f.imm));
            Ok(())
        },
        disassemble: dump_format_i,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x0000001b,
        name: "ADDIW",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = cpu.x[f.rs1].wrapping_add(f.imm) as i32 as i64;
            Ok(())
        },
        disassemble: dump_format_i,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x0000003b,
        name: "ADDW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.x[f.rs1].wrapping_add(cpu.x[f.rs2]) as i32 as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0x0000302f,
        name: "AMOADD.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let tmp = match cpu.mmu.load_doubleword(cpu.x[f.rs1] as u64) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            match cpu
                .mmu
                .store_doubleword(cpu.x[f.rs1] as u64, cpu.x[f.rs2].wrapping_add(tmp) as u64)
            {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = tmp;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0x0000202f,
        name: "AMOADD.W",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let tmp = match cpu.mmu.load_word(cpu.x[f.rs1] as u64) {
                Ok(data) => data as i32 as i64,
                Err(e) => return Err(e),
            };
            match cpu
                .mmu
                .store_word(cpu.x[f.rs1] as u64, cpu.x[f.rs2].wrapping_add(tmp) as u32)
            {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = tmp;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0x6000302f,
        name: "AMOAND.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let tmp = match cpu.mmu.load_doubleword(cpu.x[f.rs1] as u64) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            match cpu
                .mmu
                .store_doubleword(cpu.x[f.rs1] as u64, (cpu.x[f.rs2] & tmp) as u64)
            {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = tmp;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0x6000202f,
        name: "AMOAND.W",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let tmp = match cpu.mmu.load_word(cpu.x[f.rs1] as u64) {
                Ok(data) => data as i32 as i64,
                Err(e) => return Err(e),
            };
            match cpu
                .mmu
                .store_word(cpu.x[f.rs1] as u64, (cpu.x[f.rs2] & tmp) as u32)
            {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = tmp;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0xe000302f,
        name: "AMOMAXU.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let tmp = match cpu.mmu.load_doubleword(cpu.x[f.rs1] as u64) {
                Ok(data) => data,
                Err(e) => return Err(e),
            };
            let max = match cpu.x[f.rs2] as u64 >= tmp {
                true => cpu.x[f.rs2] as u64,
                false => tmp,
            };
            match cpu.mmu.store_doubleword(cpu.x[f.rs1] as u64, max) {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = tmp as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0xe000202f,
        name: "AMOMAXU.W",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let tmp = match cpu.mmu.load_word(cpu.x[f.rs1] as u64) {
                Ok(data) => data,
                Err(e) => return Err(e),
            };
            let max = match cpu.x[f.rs2] as u32 >= tmp {
                true => cpu.x[f.rs2] as u32,
                false => tmp,
            };
            match cpu.mmu.store_word(cpu.x[f.rs1] as u64, max) {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = tmp as i32 as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0x4000302f,
        name: "AMOOR.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let tmp = match cpu.mmu.load_doubleword(cpu.x[f.rs1] as u64) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            match cpu
                .mmu
                .store_doubleword(cpu.x[f.rs1] as u64, (cpu.x[f.rs2] | tmp) as u64)
            {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = tmp;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0x4000202f,
        name: "AMOOR.W",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let tmp = match cpu.mmu.load_word(cpu.x[f.rs1] as u64) {
                Ok(data) => data as i32 as i64,
                Err(e) => return Err(e),
            };
            match cpu
                .mmu
                .store_word(cpu.x[f.rs1] as u64, (cpu.x[f.rs2] | tmp) as u32)
            {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = tmp;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0x0800302f,
        name: "AMOSWAP.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let tmp = match cpu.mmu.load_doubleword(cpu.x[f.rs1] as u64) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            match cpu
                .mmu
                .store_doubleword(cpu.x[f.rs1] as u64, cpu.x[f.rs2] as u64)
            {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = tmp;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0x0800202f,
        name: "AMOSWAP.W",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let tmp = match cpu.mmu.load_word(cpu.x[f.rs1] as u64) {
                Ok(data) => data as i32 as i64,
                Err(e) => return Err(e),
            };
            match cpu.mmu.store_word(cpu.x[f.rs1] as u64, cpu.x[f.rs2] as u32) {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = tmp;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x00007033,
        name: "AND",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1] & cpu.x[f.rs2]);
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00007013,
        name: "ANDI",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1] & f.imm);
            Ok(())
        },
        disassemble: dump_format_i,
    },
    Instruction {
        mask: 0x0000007f,
        data: 0x00000017,
        name: "AUIPC",
        operation: |cpu, word, address| {
            let f = parse_format_u(word);
            cpu.x[f.rd] = cpu.sign_extend(address.wrapping_add(f.imm) as i64);
            Ok(())
        },
        disassemble: dump_format_u,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00000063,
        name: "BEQ",
        operation: |cpu, word, address| {
            let f = parse_format_b(word);
            if cpu.sign_extend(cpu.x[f.rs1]) == cpu.sign_extend(cpu.x[f.rs2]) {
                cpu.pc = address.wrapping_add(f.imm);
            }
            Ok(())
        },
        disassemble: dump_format_b,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00005063,
        name: "BGE",
        operation: |cpu, word, address| {
            let f = parse_format_b(word);
            if cpu.sign_extend(cpu.x[f.rs1]) >= cpu.sign_extend(cpu.x[f.rs2]) {
                cpu.pc = address.wrapping_add(f.imm);
            }
            Ok(())
        },
        disassemble: dump_format_b,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00007063,
        name: "BGEU",
        operation: |cpu, word, address| {
            let f = parse_format_b(word);
            if cpu.unsigned_data(cpu.x[f.rs1]) >= cpu.unsigned_data(cpu.x[f.rs2]) {
                cpu.pc = address.wrapping_add(f.imm);
            }
            Ok(())
        },
        disassemble: dump_format_b,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00004063,
        name: "BLT",
        operation: |cpu, word, address| {
            let f = parse_format_b(word);
            if cpu.sign_extend(cpu.x[f.rs1]) < cpu.sign_extend(cpu.x[f.rs2]) {
                cpu.pc = address.wrapping_add(f.imm);
            }
            Ok(())
        },
        disassemble: dump_format_b,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00006063,
        name: "BLTU",
        operation: |cpu, word, address| {
            let f = parse_format_b(word);
            if cpu.unsigned_data(cpu.x[f.rs1]) < cpu.unsigned_data(cpu.x[f.rs2]) {
                cpu.pc = address.wrapping_add(f.imm);
            }
            Ok(())
        },
        disassemble: dump_format_b,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00001063,
        name: "BNE",
        operation: |cpu, word, address| {
            let f = parse_format_b(word);
            if cpu.sign_extend(cpu.x[f.rs1]) != cpu.sign_extend(cpu.x[f.rs2]) {
                cpu.pc = address.wrapping_add(f.imm);
            }
            Ok(())
        },
        disassemble: dump_format_b,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00003073,
        name: "CSRRC",
        operation: |cpu, word, _address| {
            let f = parse_format_csr(word);
            let data = match cpu.read_csr(f.csr) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            let tmp = cpu.x[f.rs];
            cpu.x[f.rd] = cpu.sign_extend(data);
            match cpu.write_csr(f.csr, (cpu.x[f.rd] & !tmp) as u64) {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_csr,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00007073,
        name: "CSRRCI",
        operation: |cpu, word, _address| {
            let f = parse_format_csr(word);
            let data = match cpu.read_csr(f.csr) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = cpu.sign_extend(data);
            match cpu.write_csr(f.csr, (cpu.x[f.rd] & !(f.rs as i64)) as u64) {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_csr,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00002073,
        name: "CSRRS",
        operation: |cpu, word, _address| {
            let f = parse_format_csr(word);
            let data = match cpu.read_csr(f.csr) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            let tmp = cpu.x[f.rs];
            cpu.x[f.rd] = cpu.sign_extend(data);
            match cpu.write_csr(f.csr, cpu.unsigned_data(cpu.x[f.rd] | tmp)) {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_csr,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00006073,
        name: "CSRRSI",
        operation: |cpu, word, _address| {
            let f = parse_format_csr(word);
            let data = match cpu.read_csr(f.csr) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = cpu.sign_extend(data);
            match cpu.write_csr(f.csr, cpu.unsigned_data(cpu.x[f.rd] | (f.rs as i64))) {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_csr,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00001073,
        name: "CSRRW",
        operation: |cpu, word, _address| {
            let f = parse_format_csr(word);
            let data = match cpu.read_csr(f.csr) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            let tmp = cpu.x[f.rs];
            cpu.x[f.rd] = cpu.sign_extend(data);
            match cpu.write_csr(f.csr, cpu.unsigned_data(tmp)) {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_csr,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00005073,
        name: "CSRRWI",
        operation: |cpu, word, _address| {
            let f = parse_format_csr(word);
            let data = match cpu.read_csr(f.csr) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            cpu.x[f.rd] = cpu.sign_extend(data);
            match cpu.write_csr(f.csr, f.rs as u64) {
                Ok(()) => {}
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_csr,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x02004033,
        name: "DIV",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let dividend = cpu.x[f.rs1];
            let divisor = cpu.x[f.rs2];
            if divisor == 0 {
                cpu.x[f.rd] = -1;
            } else if dividend == cpu.most_negative() && divisor == -1 {
                cpu.x[f.rd] = dividend;
            } else {
                cpu.x[f.rd] = cpu.sign_extend(dividend.wrapping_div(divisor))
            }
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x02005033,
        name: "DIVU",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let dividend = cpu.unsigned_data(cpu.x[f.rs1]);
            let divisor = cpu.unsigned_data(cpu.x[f.rs2]);
            if divisor == 0 {
                cpu.x[f.rd] = -1;
            } else {
                cpu.x[f.rd] = cpu.sign_extend(dividend.wrapping_div(divisor) as i64)
            }
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x0200503b,
        name: "DIVUW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let dividend = cpu.unsigned_data(cpu.x[f.rs1]) as u32;
            let divisor = cpu.unsigned_data(cpu.x[f.rs2]) as u32;
            if divisor == 0 {
                cpu.x[f.rd] = -1;
            } else {
                cpu.x[f.rd] = dividend.wrapping_div(divisor) as i32 as i64
            }
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x0200403b,
        name: "DIVW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let dividend = cpu.x[f.rs1] as i32;
            let divisor = cpu.x[f.rs2] as i32;
            if divisor == 0 {
                cpu.x[f.rd] = -1;
            } else if dividend == std::i32::MIN && divisor == -1 {
                cpu.x[f.rd] = dividend as i32 as i64;
            } else {
                cpu.x[f.rd] = dividend.wrapping_div(divisor) as i32 as i64
            }
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xffffffff,
        data: 0x00100073,
        name: "EBREAK",
        operation: |_cpu, _word, _address| {
            // @TODO: Implement
            Ok(())
        },
        disassemble: dump_empty,
    },
    Instruction {
        mask: 0xffffffff,
        data: 0x00000073,
        name: "ECALL",
        operation: |cpu, _word, address| {
            let mut args = [0i64; 8];
            for (src, dest) in cpu.x[10..].iter().zip(args.iter_mut()) {
                *dest = *src;
            }
            match cpu.memory.lock().unwrap().syscall(args) {
                super::mmu::SyscallResult::Ok(result) => {
                    for (src, dest) in result.iter().zip(cpu.x[10..].iter_mut()) {
                        *dest = *src;
                    }
                    Ok(())
                }
                super::mmu::SyscallResult::Defer(receiver) => Err(Trap {
                    trap_type: TrapType::PauseEmulation(receiver),
                    value: address,
                }),
            }

            // let exception_type = match cpu.privilege_mode {
            //     PrivilegeMode::User => TrapType::EnvironmentCallFromUMode,
            //     PrivilegeMode::Supervisor => TrapType::EnvironmentCallFromSMode,
            //     PrivilegeMode::Machine => TrapType::EnvironmentCallFromMMode,
            //     PrivilegeMode::Reserved => panic!("Unknown Privilege mode"),
            // };
            // Err(Trap {
            //     trap_type: exception_type,
            //     value: address,
            // })
        },
        disassemble: dump_empty,
    },
    Instruction {
        mask: 0xfe00007f,
        data: 0x02000053,
        name: "FADD.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.f[f.rd] = cpu.f[f.rs1] + cpu.f[f.rs2];
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfff0007f,
        data: 0xd2200053,
        name: "FCVT.D.L",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.f[f.rd] = cpu.x[f.rs1] as f64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfff0007f,
        data: 0x42000053,
        name: "FCVT.D.S",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            // Is this implementation correct?
            cpu.f[f.rd] = f32::from_bits(cpu.f[f.rs1].to_bits() as u32) as f64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfff0007f,
        data: 0xd2000053,
        name: "FCVT.D.W",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.f[f.rd] = cpu.x[f.rs1] as i32 as f64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfff0007f,
        data: 0xd2100053,
        name: "FCVT.D.WU",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.f[f.rd] = cpu.x[f.rs1] as u32 as f64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfff0007f,
        data: 0x40100053,
        name: "FCVT.S.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            // Is this implementation correct?
            cpu.f[f.rd] = cpu.f[f.rs1] as f32 as f64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfff0007f,
        data: 0xc2000053,
        name: "FCVT.W.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            // Is this implementation correct?
            cpu.x[f.rd] = cpu.f[f.rs1] as u32 as i32 as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00007f,
        data: 0x1a000053,
        name: "FDIV.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let dividend = cpu.f[f.rs1];
            let divisor = cpu.f[f.rs2];
            // Is this implementation correct?
            if divisor == 0.0 {
                cpu.f[f.rd] = std::f64::INFINITY;
                cpu.set_fcsr_dz();
            } else if divisor == -0.0 {
                cpu.f[f.rd] = std::f64::NEG_INFINITY;
                cpu.set_fcsr_dz();
            } else {
                cpu.f[f.rd] = dividend / divisor;
            }
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x0000000f,
        name: "FENCE",
        operation: |_cpu, _word, _address| {
            // Do nothing?
            Ok(())
        },
        disassemble: dump_empty,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x0000100f,
        name: "FENCE.I",
        operation: |_cpu, _word, _address| {
            // Do nothing?
            Ok(())
        },
        disassemble: dump_empty,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0xa2002053,
        name: "FEQ.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = match cpu.f[f.rs1] == cpu.f[f.rs2] {
                true => 1,
                false => 0,
            };
            Ok(())
        },
        disassemble: dump_empty,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00003007,
        name: "FLD",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.f[f.rd] = match cpu
                .mmu
                .load_doubleword(cpu.x[f.rs1].wrapping_add(f.imm) as u64)
            {
                Ok(data) => f64::from_bits(data),
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_i,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0xa2000053,
        name: "FLE.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = match cpu.f[f.rs1] <= cpu.f[f.rs2] {
                true => 1,
                false => 0,
            };
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0xa2001053,
        name: "FLT.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = match cpu.f[f.rs1] < cpu.f[f.rs2] {
                true => 1,
                false => 0,
            };
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00002007,
        name: "FLW",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.f[f.rd] = match cpu.mmu.load_word(cpu.x[f.rs1].wrapping_add(f.imm) as u64) {
                Ok(data) => f64::from_bits(data as i32 as i64 as u64),
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_i_mem,
    },
    Instruction {
        mask: 0x0600007f,
        data: 0x02000043,
        name: "FMADD.D",
        operation: |cpu, word, _address| {
            // @TODO: Update fcsr if needed?
            let f = parse_format_r2(word);
            cpu.f[f.rd] = cpu.f[f.rs1] * cpu.f[f.rs2] + cpu.f[f.rs3];
            Ok(())
        },
        disassemble: dump_format_r2,
    },
    Instruction {
        mask: 0xfe00007f,
        data: 0x12000053,
        name: "FMUL.D",
        operation: |cpu, word, _address| {
            // @TODO: Update fcsr if needed?
            let f = parse_format_r(word);
            cpu.f[f.rd] = cpu.f[f.rs1] * cpu.f[f.rs2];
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfff0707f,
        data: 0xf2000053,
        name: "FMV.D.X",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.f[f.rd] = f64::from_bits(cpu.x[f.rs1] as u64);
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfff0707f,
        data: 0xe2000053,
        name: "FMV.X.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.f[f.rs1].to_bits() as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfff0707f,
        data: 0xe0000053,
        name: "FMV.X.W",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.f[f.rs1].to_bits() as i32 as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfff0707f,
        data: 0xf0000053,
        name: "FMV.W.X",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.f[f.rd] = f64::from_bits(cpu.x[f.rs1] as u32 as u64);
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0600007f,
        data: 0x0200004b,
        name: "FNMSUB.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r2(word);
            cpu.f[f.rd] = -(cpu.f[f.rs1] * cpu.f[f.rs2]) + cpu.f[f.rs3];
            Ok(())
        },
        disassemble: dump_format_r2,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00003027,
        name: "FSD",
        operation: |cpu, word, _address| {
            let f = parse_format_s(word);
            cpu.mmu.store_doubleword(
                cpu.x[f.rs1].wrapping_add(f.imm) as u64,
                cpu.f[f.rs2].to_bits(),
            )
        },
        disassemble: dump_format_s,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x22000053,
        name: "FSGNJ.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let rs1_bits = cpu.f[f.rs1].to_bits();
            let rs2_bits = cpu.f[f.rs2].to_bits();
            let sign_bit = rs2_bits & 0x8000000000000000;
            cpu.f[f.rd] = f64::from_bits(sign_bit | (rs1_bits & 0x7fffffffffffffff));
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x22002053,
        name: "FSGNJX.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let rs1_bits = cpu.f[f.rs1].to_bits();
            let rs2_bits = cpu.f[f.rs2].to_bits();
            let sign_bit = (rs1_bits ^ rs2_bits) & 0x8000000000000000;
            cpu.f[f.rd] = f64::from_bits(sign_bit | (rs1_bits & 0x7fffffffffffffff));
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00007f,
        data: 0x0a000053,
        name: "FSUB.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            // @TODO: Update fcsr if needed?
            cpu.f[f.rd] = cpu.f[f.rs1] - cpu.f[f.rs2];
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00002027,
        name: "FSW",
        operation: |cpu, word, _address| {
            let f = parse_format_s(word);
            cpu.mmu.store_word(
                cpu.x[f.rs1].wrapping_add(f.imm) as u64,
                cpu.f[f.rs2].to_bits() as u32,
            )
        },
        disassemble: dump_format_s,
    },
    Instruction {
        mask: 0x0000007f,
        data: 0x0000006f,
        name: "JAL",
        operation: |cpu, word, address| {
            let f = parse_format_j(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.pc as i64);
            cpu.pc = address.wrapping_add(f.imm);
            Ok(())
        },
        disassemble: dump_format_j,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00000067,
        name: "JALR",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            let tmp = cpu.sign_extend(cpu.pc as i64);
            cpu.pc = (cpu.x[f.rs1] as u64).wrapping_add(f.imm as u64);
            cpu.x[f.rd] = tmp;
            Ok(())
        },
        disassemble: |cpu, word, _address, evaluate| {
            let f = parse_format_i(word);
            let mut s = String::new();
            s += get_register_name(f.rd);
            if evaluate {
                s += &format!(":{:x}", cpu.x[f.rd]);
            }
            s += &format!(",{:x}({}", f.imm, get_register_name(f.rs1));
            if evaluate {
                s += &format!(":{:x}", cpu.x[f.rs1]);
            }
            s += ")";
            s
        },
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00000003,
        name: "LB",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = match cpu.mmu.load(cpu.x[f.rs1].wrapping_add(f.imm) as u64) {
                Ok(data) => data as i8 as i64,
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_i_mem,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00004003,
        name: "LBU",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = match cpu.mmu.load(cpu.x[f.rs1].wrapping_add(f.imm) as u64) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_i_mem,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00003003,
        name: "LD",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = match cpu
                .mmu
                .load_doubleword(cpu.x[f.rs1].wrapping_add(f.imm) as u64)
            {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_i_mem,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00001003,
        name: "LH",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = match cpu
                .mmu
                .load_halfword(cpu.x[f.rs1].wrapping_add(f.imm) as u64)
            {
                Ok(data) => data as i16 as i64,
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_i_mem,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00005003,
        name: "LHU",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = match cpu
                .mmu
                .load_halfword(cpu.x[f.rs1].wrapping_add(f.imm) as u64)
            {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_i_mem,
    },
    Instruction {
        mask: 0xf9f0707f,
        data: 0x1000302f,
        name: "LR.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            // @TODO: Implement properly
            let address = cpu.x[f.rs1] as u64;
            cpu.x[f.rd] = cpu.mmu.load_doubleword(address)? as i64;
            if cpu.mmu.reserve(address) {
                cpu.reservation = Some(address);
            }
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf9f0707f,
        data: 0x1000202f,
        name: "LR.W",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            // @TODO: Implement properly
            let address = cpu.x[f.rs1] as u64;
            cpu.x[f.rd] = cpu.mmu.load_word(address)? as i64;
            if cpu.mmu.reserve(address) {
                cpu.reservation = Some(address);
            }
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000007f,
        data: 0x00000037,
        name: "LUI",
        operation: |cpu, word, _address| {
            let f = parse_format_u(word);
            cpu.x[f.rd] = f.imm as i64;
            Ok(())
        },
        disassemble: dump_format_u,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00002003,
        name: "LW",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = match cpu.mmu.load_word(cpu.x[f.rs1].wrapping_add(f.imm) as u64) {
                Ok(data) => data as i32 as i64,
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_i_mem,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00006003,
        name: "LWU",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = match cpu.mmu.load_word(cpu.x[f.rs1].wrapping_add(f.imm) as u64) {
                Ok(data) => data as i64,
                Err(e) => return Err(e),
            };
            Ok(())
        },
        disassemble: dump_format_i_mem,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x02000033,
        name: "MUL",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1].wrapping_mul(cpu.x[f.rs2]));
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x02001033,
        name: "MULH",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = match cpu.xlen {
                Xlen::Bit32 => cpu.sign_extend((cpu.x[f.rs1] * cpu.x[f.rs2]) >> 32),
                Xlen::Bit64 => (((cpu.x[f.rs1] as i128) * (cpu.x[f.rs2] as i128)) >> 64) as i64,
            };
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x02003033,
        name: "MULHU",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = match cpu.xlen {
                Xlen::Bit32 => cpu.sign_extend(
                    (((cpu.x[f.rs1] as u32 as u64) * (cpu.x[f.rs2] as u32 as u64)) >> 32) as i64,
                ),
                Xlen::Bit64 => {
                    ((cpu.x[f.rs1] as u64 as u128).wrapping_mul(cpu.x[f.rs2] as u64 as u128) >> 64)
                        as i64
                }
            };
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x02002033,
        name: "MULHSU",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = match cpu.xlen {
                Xlen::Bit32 => cpu.sign_extend(
                    ((cpu.x[f.rs1] as i64).wrapping_mul(cpu.x[f.rs2] as u32 as i64) >> 32) as i64,
                ),
                Xlen::Bit64 => {
                    ((cpu.x[f.rs1] as u128).wrapping_mul(cpu.x[f.rs2] as u64 as u128) >> 64) as i64
                }
            };
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x0200003b,
        name: "MULW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] =
                cpu.sign_extend((cpu.x[f.rs1] as i32).wrapping_mul(cpu.x[f.rs2] as i32) as i64);
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xffffffff,
        data: 0x30200073,
        name: "MRET",
        operation: |cpu, _word, _address| {
            cpu.pc = match cpu.read_csr(CSR_MEPC_ADDRESS) {
                Ok(data) => data,
                Err(e) => return Err(e),
            };
            let status = cpu.read_csr_raw(CSR_MSTATUS_ADDRESS);
            let mpie = (status >> 7) & 1;
            let mpp = (status >> 11) & 0x3;
            let mprv = match decode_privilege_mode(mpp) {
                PrivilegeMode::Machine => (status >> 17) & 1,
                _ => 0,
            };
            // Override MIE[3] with MPIE[7], set MPIE[7] to 1, set MPP[12:11] to 0
            // and override MPRV[17]
            let new_status = (status & !0x21888) | (mprv << 17) | (mpie << 3) | (1 << 7);
            cpu.write_csr_raw(CSR_MSTATUS_ADDRESS, new_status);
            cpu.privilege_mode = match mpp {
                0 => PrivilegeMode::User,
                1 => PrivilegeMode::Supervisor,
                3 => PrivilegeMode::Machine,
                _ => panic!(), // Shouldn't happen
            };
            cpu.mmu.update_privilege_mode(cpu.privilege_mode);
            Ok(())
        },
        disassemble: dump_empty,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x00006033,
        name: "OR",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1] | cpu.x[f.rs2]);
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00006013,
        name: "ORI",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1] | f.imm);
            Ok(())
        },
        disassemble: dump_format_i,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x02006033,
        name: "REM",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let dividend = cpu.x[f.rs1];
            let divisor = cpu.x[f.rs2];
            if divisor == 0 {
                cpu.x[f.rd] = dividend;
            } else if dividend == cpu.most_negative() && divisor == -1 {
                cpu.x[f.rd] = 0;
            } else {
                cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1].wrapping_rem(cpu.x[f.rs2]));
            }
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x02007033,
        name: "REMU",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let dividend = cpu.unsigned_data(cpu.x[f.rs1]);
            let divisor = cpu.unsigned_data(cpu.x[f.rs2]);
            cpu.x[f.rd] = match divisor {
                0 => cpu.sign_extend(dividend as i64),
                _ => cpu.sign_extend(dividend.wrapping_rem(divisor) as i64),
            };
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x0200703b,
        name: "REMUW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let dividend = cpu.x[f.rs1] as u32;
            let divisor = cpu.x[f.rs2] as u32;
            cpu.x[f.rd] = match divisor {
                0 => dividend as i32 as i64,
                _ => dividend.wrapping_rem(divisor) as i32 as i64,
            };
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x0200603b,
        name: "REMW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let dividend = cpu.x[f.rs1] as i32;
            let divisor = cpu.x[f.rs2] as i32;
            if divisor == 0 {
                cpu.x[f.rd] = dividend as i64;
            } else if dividend == std::i32::MIN && divisor == -1 {
                cpu.x[f.rd] = 0;
            } else {
                cpu.x[f.rd] = dividend.wrapping_rem(divisor) as i64;
            }
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00000023,
        name: "SB",
        operation: |cpu, word, _address| {
            let f = parse_format_s(word);
            cpu.mmu
                .store(cpu.x[f.rs1].wrapping_add(f.imm) as u64, cpu.x[f.rs2] as u8)
        },
        disassemble: dump_format_s,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0x1800302f,
        name: "SC.D",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            // @TODO: Implement properly
            let address = cpu.x[f.rs1] as u64;
            if Some(address) == cpu.reservation.take() {
                cpu.mmu.store_doubleword(address, cpu.x[f.rs2] as u64)?;
                cpu.mmu.clear_reservation(address);
                cpu.x[f.rd] = 0;
                return Ok(());
            }
            cpu.x[f.rd] = 1;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xf800707f,
        data: 0x1800202f,
        name: "SC.W",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            // @TODO: Implement properly
            let address = cpu.x[f.rs1] as u64;
            if Some(address) == cpu.reservation.take() {
                cpu.mmu.clear_reservation(address);
                cpu.mmu.store_word(address, cpu.x[f.rs2] as u32)?;
                cpu.x[f.rd] = 0;
                return Ok(());
            }
            cpu.x[f.rd] = 1;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00003023,
        name: "SD",
        operation: |cpu, word, _address| {
            let f = parse_format_s(word);
            cpu.mmu
                .store_doubleword(cpu.x[f.rs1].wrapping_add(f.imm) as u64, cpu.x[f.rs2] as u64)
        },
        disassemble: dump_format_s,
    },
    Instruction {
        mask: 0xfe007fff,
        data: 0x12000073,
        name: "SFENCE.VMA",
        operation: |_cpu, _word, _address| {
            // Do nothing?
            Ok(())
        },
        disassemble: dump_empty,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00001023,
        name: "SH",
        operation: |cpu, word, _address| {
            let f = parse_format_s(word);
            cpu.mmu
                .store_halfword(cpu.x[f.rs1].wrapping_add(f.imm) as u64, cpu.x[f.rs2] as u16)
        },
        disassemble: dump_format_s,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x00001033,
        name: "SLL",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1].wrapping_shl(cpu.x[f.rs2] as u32));
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfc00707f,
        data: 0x00001013,
        name: "SLLI",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let mask = match cpu.xlen {
                Xlen::Bit32 => 0x1f,
                Xlen::Bit64 => 0x3f,
            };
            let shamt = (word >> 20) & mask;
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1] << shamt);
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x0000101b,
        name: "SLLIW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let shamt = f.rs2 as u32;
            cpu.x[f.rd] = (cpu.x[f.rs1] << shamt) as i32 as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x0000103b,
        name: "SLLW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = (cpu.x[f.rs1] as u32).wrapping_shl(cpu.x[f.rs2] as u32) as i32 as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x00002033,
        name: "SLT",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = match cpu.x[f.rs1] < cpu.x[f.rs2] {
                true => 1,
                false => 0,
            };
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00002013,
        name: "SLTI",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = match cpu.x[f.rs1] < f.imm {
                true => 1,
                false => 0,
            };
            Ok(())
        },
        disassemble: dump_format_i,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00003013,
        name: "SLTIU",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = match cpu.unsigned_data(cpu.x[f.rs1]) < cpu.unsigned_data(f.imm) {
                true => 1,
                false => 0,
            };
            Ok(())
        },
        disassemble: dump_format_i,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x00003033,
        name: "SLTU",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = match cpu.unsigned_data(cpu.x[f.rs1]) < cpu.unsigned_data(cpu.x[f.rs2]) {
                true => 1,
                false => 0,
            };
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x40005033,
        name: "SRA",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1].wrapping_shr(cpu.x[f.rs2] as u32));
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfc00707f,
        data: 0x40005013,
        name: "SRAI",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let mask = match cpu.xlen {
                Xlen::Bit32 => 0x1f,
                Xlen::Bit64 => 0x3f,
            };
            let shamt = (word >> 20) & mask;
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1] >> shamt);
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfc00707f,
        data: 0x4000501b,
        name: "SRAIW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let shamt = (word >> 20) & 0x1f;
            cpu.x[f.rd] = ((cpu.x[f.rs1] as i32) >> shamt) as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x4000503b,
        name: "SRAW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = (cpu.x[f.rs1] as i32).wrapping_shr(cpu.x[f.rs2] as u32) as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xffffffff,
        data: 0x10200073,
        name: "SRET",
        operation: |cpu, _word, _address| {
            // @TODO: Throw error if higher privilege return instruction is executed
            cpu.pc = match cpu.read_csr(CSR_SEPC_ADDRESS) {
                Ok(data) => data,
                Err(e) => return Err(e),
            };
            let status = cpu.read_csr_raw(CSR_SSTATUS_ADDRESS);
            let spie = (status >> 5) & 1;
            let spp = (status >> 8) & 1;
            let mprv = match decode_privilege_mode(spp) {
                PrivilegeMode::Machine => (status >> 17) & 1,
                _ => 0,
            };
            // Override SIE[1] with SPIE[5], set SPIE[5] to 1, set SPP[8] to 0,
            // and override MPRV[17]
            let new_status = (status & !0x20122) | (mprv << 17) | (spie << 1) | (1 << 5);
            cpu.write_csr_raw(CSR_SSTATUS_ADDRESS, new_status);
            cpu.privilege_mode = match spp {
                0 => PrivilegeMode::User,
                1 => PrivilegeMode::Supervisor,
                _ => panic!(), // Shouldn't happen
            };
            // println!("Updating privilege mode to {:?}", cpu.privilege_mode);
            cpu.mmu.update_privilege_mode(cpu.privilege_mode);
            Ok(())
        },
        disassemble: dump_empty,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x00005033,
        name: "SRL",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.sign_extend(
                cpu.unsigned_data(cpu.x[f.rs1])
                    .wrapping_shr(cpu.x[f.rs2] as u32) as i64,
            );
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfc00707f,
        data: 0x00005013,
        name: "SRLI",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let mask = match cpu.xlen {
                Xlen::Bit32 => 0x1f,
                Xlen::Bit64 => 0x3f,
            };
            let shamt = (word >> 20) & mask;
            cpu.x[f.rd] = cpu.sign_extend((cpu.unsigned_data(cpu.x[f.rs1]) >> shamt) as i64);
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfc00707f,
        data: 0x0000501b,
        name: "SRLIW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            let mask = match cpu.xlen {
                Xlen::Bit32 => 0x1f,
                Xlen::Bit64 => 0x3f,
            };
            let shamt = (word >> 20) & mask;
            cpu.x[f.rd] = ((cpu.x[f.rs1] as u32) >> shamt) as i32 as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x0000503b,
        name: "SRLW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = (cpu.x[f.rs1] as u32).wrapping_shr(cpu.x[f.rs2] as u32) as i32 as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x40000033,
        name: "SUB",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1].wrapping_sub(cpu.x[f.rs2]));
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x4000003b,
        name: "SUBW",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.x[f.rs1].wrapping_sub(cpu.x[f.rs2]) as i32 as i64;
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00002023,
        name: "SW",
        operation: |cpu, word, _address| {
            let f = parse_format_s(word);
            cpu.mmu
                .store_word(cpu.x[f.rs1].wrapping_add(f.imm) as u64, cpu.x[f.rs2] as u32)
        },
        disassemble: dump_format_s,
    },
    Instruction {
        mask: 0xffffffff,
        data: 0x00200073,
        name: "URET",
        operation: |_cpu, _word, _address| {
            // @TODO: Implement
            panic!("URET instruction is not implemented yet.");
        },
        disassemble: dump_empty,
    },
    Instruction {
        mask: 0xffffffff,
        data: 0x10500073,
        name: "WFI",
        operation: |cpu, _word, _address| {
            cpu.wfi = true;
            Ok(())
        },
        disassemble: dump_empty,
    },
    Instruction {
        mask: 0xfe00707f,
        data: 0x00004033,
        name: "XOR",
        operation: |cpu, word, _address| {
            let f = parse_format_r(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1] ^ cpu.x[f.rs2]);
            Ok(())
        },
        disassemble: dump_format_r,
    },
    Instruction {
        mask: 0x0000707f,
        data: 0x00004013,
        name: "XORI",
        operation: |cpu, word, _address| {
            let f = parse_format_i(word);
            cpu.x[f.rd] = cpu.sign_extend(cpu.x[f.rs1] ^ f.imm);
            Ok(())
        },
        disassemble: dump_format_i,
    },
];

// /// The number of results [`DecodeCache`](struct.DecodeCache.html) holds.
// /// You need to carefully choose the number. Too small number causes
// /// bad cache hit ratio. Too large number causes memory consumption
// /// and host hardware CPU cache memory miss.
// const DECODE_CACHE_ENTRY_NUM: usize = 0x1000;

// const INVALID_CACHE_ENTRY: usize = INSTRUCTION_NUM;
// const NULL_ENTRY: usize = DECODE_CACHE_ENTRY_NUM;

// /// `DecodeCache` provides a cache system for instruction decoding.
// /// It holds the recent [`DECODE_CACHE_ENTRY_NUM`](constant.DECODE_CACHE_ENTRY_NUM.html)
// /// instruction decode results. If it has a cache (called "hit") for passed
// /// word data, it returns decoding result very quickly. Decoding is one of the
// /// slowest parts in CPU. This cache system improves the CPU processing speed
// /// by skipping decoding. Especially it should work well for loop. It is said
// /// that some loops in a program consume the majority of time then this cache
// /// system is expected to reduce the decoding time very well.
// ///
// /// This cache system is based on LRU algorithm, and consists of a hash map and
// /// a linked list. Linked list is for LRU, front means recently used and back
// /// means least recently used. A content in hash map points to an entry in the
// /// linked list. This is the key to achieve computing in O(1).
// ///
// // @TODO: Write performance benchmark test to confirm this cache actually
// //        improves the speed.
// struct DecodeCache {
//     /// Holds mappings from word instruction data to an index of `entries`
//     /// pointing to the entry having the decoding result. Containing the word
//     /// means cache hit.
//     // hash_map: FnvHashMap::<u32, usize>,
//     hash_map: HashMap<u32, usize>,

//     /// Holds the entries [`DecodeCacheEntry`](struct.DecodeCacheEntry.html)
//     /// forming linked list.
//     entries: Vec<DecodeCacheEntry>,

//     /// An index of `entries` pointing to the head entry in the linked list
//     front_index: usize,

//     /// An index of `entries` pointing to the tail entry in the linked list
//     back_index: usize,

//     /// Cache hit count for debugging purpose
//     hit_count: u64,

//     /// Cache miss count for debugging purpose
//     miss_count: u64,
// }

// impl DecodeCache {
//     /// Creates a new `DecodeCache`.
//     fn new() -> Self {
//         // Initialize linked list
//         let mut entries = Vec::new();
//         for i in 0..DECODE_CACHE_ENTRY_NUM {
//             let next_index = match i == DECODE_CACHE_ENTRY_NUM - 1 {
//                 true => NULL_ENTRY,
//                 false => i + 1,
//             };
//             let prev_index = match i == 0 {
//                 true => NULL_ENTRY,
//                 false => i - 1,
//             };
//             entries.push(DecodeCacheEntry::new(next_index, prev_index));
//         }

//         DecodeCache {
//             // hash_map: FnvHashMap::default(),
//             hash_map: HashMap::default(),
//             entries,
//             front_index: 0,
//             back_index: DECODE_CACHE_ENTRY_NUM - 1,
//             hit_count: 0,
//             miss_count: 0,
//         }
//     }

//     /// Gets the cached decoding result. If hits this method moves the
//     /// cache entry to front of the linked list and returns an index of
//     /// [`INSTRUCTIONS`](constant.INSTRUCTIONS.html).
//     /// Otherwise returns `None`. This operation should compute in O(1) time.
//     ///
//     /// # Arguments
//     /// * `word` word instruction data
//     fn get(&mut self, word: u32) -> Option<usize> {
//         let result = match self.hash_map.get(&word) {
//             Some(index) => {
//                 self.hit_count += 1;
//                 // Move the entry to front of the list unless it is at front.
//                 if self.front_index != *index {
//                     let next_index = self.entries[*index].next_index;
//                     let prev_index = self.entries[*index].prev_index;

//                     // Remove the entry from the list
//                     if self.back_index == *index {
//                         self.back_index = prev_index;
//                     } else {
//                         self.entries[next_index].prev_index = prev_index;
//                     }
//                     self.entries[prev_index].next_index = next_index;

//                     // Push the entry to front
//                     self.entries[*index].prev_index = NULL_ENTRY;
//                     self.entries[*index].next_index = self.front_index;
//                     self.entries[self.front_index].prev_index = *index;
//                     self.front_index = *index;
//                 }
//                 Some(self.entries[*index].instruction_index)
//             }
//             None => {
//                 self.miss_count += 1;
//                 None
//             }
//         };
//         //println!("Hit:{:X}, Miss:{:X}, Ratio:{}", self.hit_count, self.miss_count,
//         //	(self.hit_count as f64) / (self.hit_count + self.miss_count) as f64);
//         result
//     }

//     /// Inserts a new decode result to front of the linked list while removing
//     /// the least recently used result from the list. This operation should
//     /// compute in O(1) time.
//     ///
//     /// # Arguments
//     /// * `word`
//     /// * `instruction_index`
//     fn insert(&mut self, word: u32, instruction_index: usize) {
//         let index = self.back_index;

//         // Remove the least recently used entry. The entry resource
//         // is reused as new entry.
//         if self.entries[index].instruction_index != INVALID_CACHE_ENTRY {
//             self.hash_map.remove(&self.entries[index].word);
//         }
//         self.back_index = self.entries[index].prev_index;
//         self.entries[self.back_index].next_index = NULL_ENTRY;

//         // Push the new entry to front of the linked list
//         self.hash_map.insert(word, index);
//         self.entries[index].prev_index = NULL_ENTRY;
//         self.entries[index].next_index = self.front_index;
//         self.entries[index].word = word;
//         self.entries[index].instruction_index = instruction_index;
//         self.entries[self.front_index].prev_index = index;
//         self.front_index = index;
//     }
// }

// /// An entry of linked list managed by [`DecodeCache`](struct.DecodeCache.html).
// /// An entry consists of a mapping from word instruction data to an index of
// /// [`INSTRUCTIONS`](constant.INSTRUCTIONS.html) and next/previous entry index
// /// in the linked list.
// struct DecodeCacheEntry {
//     /// Instruction word data
//     word: u32,

//     /// The result of decoding `word`. An index of [`INSTRUCTIONS`](constant.INSTRUCTIONS.html).
//     instruction_index: usize,

//     /// Next entry index in the linked list. [`NULL_ENTRY`](constant.NULL_ENTRY.html)
//     /// represents no next entry, meaning the entry is at tail.
//     next_index: usize,

//     /// Previous entry index in the linked list. [`NULL_ENTRY`](constant.NULL_ENTRY.html)
//     /// represents no previous entry, meaning the entry is at head.
//     prev_index: usize,
// }

// impl DecodeCacheEntry {
//     /// Creates a new entry. Initial `instruction_index` is
//     /// `INVALID_CACHE_ENTRY` meaning the entry is invalid.
//     ///
//     /// # Arguments
//     /// * `next_index`
//     /// * `prev_index`
//     fn new(next_index: usize, prev_index: usize) -> Self {
//         DecodeCacheEntry {
//             word: 0,
//             instruction_index: INVALID_CACHE_ENTRY,
//             next_index,
//             prev_index,
//         }
//     }
// }

#[cfg(test)]
mod test_cpu {
    use super::*;

    fn create_cpu() -> Cpu {
        Cpu::default()
    }

    #[test]
    fn initialize() {
        let _cpu = create_cpu();
    }

    #[test]
    fn update_pc() {
        let mut cpu = create_cpu();
        assert_eq!(0, cpu.read_pc());
        cpu.update_pc(1);
        assert_eq!(1, cpu.read_pc());
        cpu.update_pc(0xffffffffffffffff);
        assert_eq!(0xffffffffffffffff, cpu.read_pc());
    }

    #[test]
    fn update_xlen() {
        let mut cpu = create_cpu();
        assert!(matches!(cpu.xlen, Xlen::Bit64));
        cpu.update_xlen(Xlen::Bit32);
        assert!(matches!(cpu.xlen, Xlen::Bit32));
        cpu.update_xlen(Xlen::Bit64);
        assert!(matches!(cpu.xlen, Xlen::Bit64));
        // Note: cpu.update_xlen() updates cpu.mmu.xlen, too.
        // The test for mmu.xlen should be in Mmu?
    }

    #[test]
    fn read_register() {
        let mut cpu = create_cpu();
        // Initial register values are 0 other than 0xb th register.
        // Initial value of 0xb th register is temporal for Linux boot and
        // I'm not sure if the value is correct. Then skipping so far.
        for i in 0..31 {
            if i != 0xb {
                assert_eq!(0, cpu.read_register(i));
            }
        }

        for i in 0..31 {
            cpu.x[i] = i as i64 + 1;
        }

        for i in 0..31 {
            match i {
                // 0th register is hardwired zero
                0 => assert_eq!(0, cpu.read_register(i)),
                _ => assert_eq!(i as i64 + 1, cpu.read_register(i)),
            }
        }

        for i in 0..31 {
            cpu.x[i] = (0xffffffffffffffff - i) as i64;
        }

        for i in 0..31 {
            match i {
                // 0th register is hardwired zero
                0 => assert_eq!(0, cpu.read_register(i)),
                _ => assert_eq!(-(i as i64 + 1), cpu.read_register(i)),
            }
        }

        // @TODO: Should I test the case where the argument equals to or is
        // greater than 32?
    }

    #[test]
    fn tick() {
        let mut cpu = create_cpu();
        let memory_base = cpu.memory_base();
        cpu.get_mut_mmu().init_memory(4);
        cpu.update_pc(memory_base);

        // Write non-compressed "addi x1, x1, 1" instruction
        match cpu.get_mut_mmu().store_word(memory_base, 0x00108093) {
            Ok(()) => {}
            Err(_e) => panic!("Failed to store"),
        };
        // Write compressed "addi x8, x0, 8" instruction
        match cpu.get_mut_mmu().store_word(memory_base + 4, 0x20) {
            Ok(()) => {}
            Err(_e) => panic!("Failed to store"),
        };

        cpu.tick();

        assert_eq!(memory_base + 4, cpu.read_pc());
        assert_eq!(1, cpu.read_register(1));

        cpu.tick();

        assert_eq!(memory_base + 6, cpu.read_pc());
        assert_eq!(8, cpu.read_register(8));
    }

    #[test]
    fn tick_operate() {
        let mut cpu = create_cpu();
        let memory_base = cpu.memory_base();
        cpu.get_mut_mmu().init_memory(4);
        cpu.update_pc(memory_base);
        // write non-compressed "addi a0, a0, 12" instruction
        match cpu.get_mut_mmu().store_word(memory_base, 0xc50513) {
            Ok(()) => {}
            Err(_e) => panic!("Failed to store"),
        };
        assert_eq!(memory_base, cpu.read_pc());
        assert_eq!(0, cpu.read_register(10));
        match cpu.tick_operate() {
            Ok(()) => {}
            Err(_e) => panic!("tick_operate() unexpectedly did panic"),
        };
        // .tick_operate() increments the program counter by 4 for
        // non-compressed instruction.
        assert_eq!(memory_base + 4, cpu.read_pc());
        // "addi a0, a0, a12" instruction writes 12 to a0 register.
        assert_eq!(12, cpu.read_register(10));
        // @TODO: Test compressed instruction operation
    }

    #[test]
    fn fetch() {
        // .fetch() reads four bytes from the memory
        // at the address the program counter points to.
        // .fetch() doesn't increment the program counter.
        // .tick_operate() does.
        let mut cpu = create_cpu();
        let memory_base = cpu.memory_base();
        cpu.get_mut_mmu().init_memory(4);
        cpu.update_pc(memory_base);
        match cpu.get_mut_mmu().store_word(memory_base, 0xaaaaaaaa) {
            Ok(()) => {}
            Err(_e) => panic!("Failed to store"),
        };
        match cpu.fetch() {
            Ok(data) => assert_eq!(0xaaaaaaaa, data),
            Err(_e) => panic!("Failed to fetch"),
        };
        match cpu.get_mut_mmu().store_word(memory_base, 0x55555555) {
            Ok(()) => {}
            Err(_e) => panic!("Failed to store"),
        };
        match cpu.fetch() {
            Ok(data) => assert_eq!(0x55555555, data),
            Err(_e) => panic!("Failed to fetch"),
        };
        // @TODO: Write test cases where Trap happens
    }

    #[test]
    fn decode() {
        let mut cpu = create_cpu();
        // 0x13 is addi instruction
        match cpu.decode(0x13) {
            Ok(inst) => assert_eq!(inst.name, "ADDI"),
            Err(_e) => panic!("Failed to decode"),
        };
        // .decode() returns error for invalid word data.
        match cpu.decode(0x0) {
            Ok(_inst) => panic!("Unexpectedly succeeded in decoding"),
            Err(()) => assert!(true),
        };
        // @TODO: Should I test all instructions?
    }

    #[test]
    fn uncompress() {
        let mut cpu = create_cpu();
        // .uncompress() doesn't directly return an instruction but
        // it returns uncompressed word. Then you need to call .decode().
        match cpu.decode(cpu.uncompress(0x20)) {
            Ok(inst) => assert_eq!(inst.name, "ADDI"),
            Err(_e) => panic!("Failed to decode"),
        };
        // @TODO: Should I test all compressed instructions?
    }

    #[test]
    fn wfi() {
        let wfi_instruction = 0x10500073;
        let mut cpu = create_cpu();
        let memory_base = cpu.memory_base();
        // Just in case
        match cpu.decode(wfi_instruction) {
            Ok(inst) => assert_eq!(inst.name, "WFI"),
            Err(_e) => panic!("Failed to decode"),
        };
        cpu.get_mut_mmu().init_memory(4);
        cpu.update_pc(memory_base);
        // write WFI instruction
        match cpu.get_mut_mmu().store_word(memory_base, wfi_instruction) {
            Ok(()) => {}
            Err(_e) => panic!("Failed to store"),
        };
        cpu.tick();
        assert_eq!(memory_base + 4, cpu.read_pc());
        for _i in 0..10 {
            // Until interrupt happens, .tick() does nothing
            // @TODO: Check accurately that the state is unchanged
            cpu.tick();
            assert_eq!(memory_base + 4, cpu.read_pc());
        }
        // Machine timer interrupt
        cpu.write_csr_raw(CSR_MIE_ADDRESS, MIP_MTIP);
        cpu.write_csr_raw(CSR_MIP_ADDRESS, MIP_MTIP);
        cpu.write_csr_raw(CSR_MSTATUS_ADDRESS, 0x8);
        cpu.write_csr_raw(CSR_MTVEC_ADDRESS, 0x0);
        cpu.tick();
        // Interrupt happened and moved to handler
        assert_eq!(0, cpu.read_pc());
    }

    #[test]
    fn interrupt() {
        let handler_vector = 0x10000000;
        let mut cpu = create_cpu();
        let memory_base = cpu.memory_base();
        cpu.get_mut_mmu().init_memory(4);
        // Write non-compressed "addi x0, x0, 1" instruction
        match cpu.get_mut_mmu().store_word(memory_base, 0x00100013) {
            Ok(()) => {}
            Err(_e) => panic!("Failed to store"),
        };
        cpu.update_pc(memory_base);

        // Machine timer interrupt but mie in mstatus is not enabled yet
        cpu.write_csr_raw(CSR_MIE_ADDRESS, MIP_MTIP);
        cpu.write_csr_raw(CSR_MIP_ADDRESS, MIP_MTIP);
        cpu.write_csr_raw(CSR_MTVEC_ADDRESS, handler_vector);

        cpu.tick();

        // Interrupt isn't caught because mie is disabled
        assert_eq!(memory_base + 4, cpu.read_pc());

        cpu.update_pc(memory_base);
        // Enable mie in mstatus
        cpu.write_csr_raw(CSR_MSTATUS_ADDRESS, 0x8);

        cpu.tick();

        // Interrupt happened and moved to handler
        assert_eq!(handler_vector, cpu.read_pc());

        // CSR Cause register holds the reason what caused the interrupt
        assert_eq!(0x8000000000000007, cpu.read_csr_raw(CSR_MCAUSE_ADDRESS));

        // @TODO: Test post CSR status register
        // @TODO: Test xIE bit in CSR status register
        // @TODO: Test privilege levels
        // @TODO: Test delegation
        // @TODO: Test vector type handlers
    }

    #[test]
    fn exception() {
        let handler_vector = 0x10000000;
        let mut cpu = create_cpu();
        let memory_base = cpu.memory_base();
        cpu.get_mut_mmu().init_memory(4);
        // Write ECALL instruction
        match cpu.get_mut_mmu().store_word(memory_base, 0x00000073) {
            Ok(()) => {}
            Err(_e) => panic!("Failed to store"),
        };
        cpu.write_csr_raw(CSR_MTVEC_ADDRESS, handler_vector);
        cpu.update_pc(memory_base);

        cpu.tick();

        // Interrupt happened and moved to handler
        assert_eq!(handler_vector, cpu.read_pc());

        // CSR Cause register holds the reason what caused the trap
        assert_eq!(0xb, cpu.read_csr_raw(CSR_MCAUSE_ADDRESS));

        // @TODO: Test post CSR status register
        // @TODO: Test privilege levels
        // @TODO: Test delegation
        // @TODO: Test vector type handlers
    }

    #[test]
    fn hardocded_zero() {
        let mut cpu = create_cpu();
        let memory_base = cpu.memory_base();
        cpu.get_mut_mmu().init_memory(8);
        cpu.update_pc(memory_base);

        // Write non-compressed "addi x0, x0, 1" instruction
        match cpu.get_mut_mmu().store_word(memory_base, 0x00100013) {
            Ok(()) => {}
            Err(_e) => panic!("Failed to store"),
        };
        // Write non-compressed "addi x1, x1, 1" instruction
        match cpu.get_mut_mmu().store_word(memory_base + 4, 0x00108093) {
            Ok(()) => {}
            Err(_e) => panic!("Failed to store"),
        };

        // Test x0
        assert_eq!(0, cpu.read_register(0));
        cpu.tick(); // Execute  "addi x0, x0, 1"
                    // x0 is still zero because it's hardcoded zero
        assert_eq!(0, cpu.read_register(0));

        // Test x1
        assert_eq!(0, cpu.read_register(1));
        cpu.tick(); // Execute  "addi x1, x1, 1"
                    // x1 is not hardcoded zero
        assert_eq!(1, cpu.read_register(1));
    }

    #[test]
    fn disassemble_next_instruction() {
        let mut cpu = create_cpu();
        let memory_base = cpu.memory_base();
        cpu.get_mut_mmu().init_memory(4);
        cpu.update_pc(memory_base);

        // Write non-compressed "addi x0, x0, 1" instruction
        match cpu.get_mut_mmu().store_word(memory_base, 0x00100013) {
            Ok(()) => {}
            Err(_e) => panic!("Failed to store"),
        };

        assert_eq!(
            "PC:0000000080000000 00100013 ADDI zero:0,zero:0,1",
            cpu.disassemble_next_instruction()
        );

        // No effect to PC
        assert_eq!(memory_base, cpu.read_pc());
    }
}