diff options
| -rw-r--r-- | src/instructions.rs | 1 | ||||
| -rw-r--r-- | src/instructions/rvi/mem.rs | 20 |
2 files changed, 21 insertions, 0 deletions
diff --git a/src/instructions.rs b/src/instructions.rs index 2297994..8c9c910 100644 --- a/src/instructions.rs +++ b/src/instructions.rs @@ -36,6 +36,7 @@ pub(crate) fn find_and_exec(instr: Instruction, core: &mut Core) -> Option<Instr 0b01000 => match instr.funct3() { // STORE 0b000 => Some(rvi::sb(core, instr)), + 0b001 => Some(rvi::sh(core, instr)), 0b010 => Some(rvi::sw(core, instr)), 0b011 => Some(rvi::sd(core, instr)), _ => None, diff --git a/src/instructions/rvi/mem.rs b/src/instructions/rvi/mem.rs index d065e8d..47eb01d 100644 --- a/src/instructions/rvi/mem.rs +++ b/src/instructions/rvi/mem.rs @@ -86,6 +86,26 @@ pub fn lw(core: &mut Core, instr: Instruction) -> InstructionResult { } } +pub fn sh(core: &mut Core, instr: Instruction) -> InstructionResult { + let addr = core.reg_read(instr.rs1()).wrapping_add(instr.imm_s()); + + if !addr.is_multiple_of(std::mem::size_of::<HWord>() as Addr) { + return InstructionResult::Exception(()); + } + + let page = (addr / 4096) as PageNum; + let offset = (addr / 2 & ((4096 / 2 as Addr) - 1)) as u16; + let value = core.reg_read(instr.rs2()) as HWord; + + match core.mem.write_hword(page, offset, value) { + Ok(_) => { + core.advance_pc(); + InstructionResult::Normal + } + Err(_) => InstructionResult::Exception(()), + } +} + pub fn lh(core: &mut Core, instr: Instruction) -> InstructionResult { let addr = core.reg_read(instr.rs1()).wrapping_add(instr.imm_i()); |