From 55b9b842a9783ed51013b3e98ff7aff408a6ab4d Mon Sep 17 00:00:00 2001
From: Yang Hau <yuanyanghau@gmail.com>
Date: Thu, 22 Feb 2024 01:23:17 +0900
Subject: [PATCH] feat: Add SB, SH, SW

---
 src/cpu.rs        | 21 ++++++++++++++++++---
 tests/cpu_test.rs | 42 +++++++++++++++++++++++++++++++++++++++---
 tests/helper.rs   | 14 +++++++++++++-
 3 files changed, 70 insertions(+), 7 deletions(-)

diff --git a/src/cpu.rs b/src/cpu.rs
index edfbba8..677fa83 100644
--- a/src/cpu.rs
+++ b/src/cpu.rs
@@ -208,9 +208,24 @@ pub fn exec_lwu(cpu: &mut CPU, instr: u32) {
         .bus
         .load(cpu.xregs.regs[rs1(instr) as usize].wrapping_add(imm), 32);
 }
-pub fn exec_sb(cpu: &mut CPU, instr: u32) {}
-pub fn exec_sh(cpu: &mut CPU, instr: u32) {}
-pub fn exec_sw(cpu: &mut CPU, instr: u32) {}
+pub fn exec_sb(cpu: &mut CPU, instr: u32) {
+    let imm = imm_s(instr) as i32;
+    let addr = (cpu.xregs.regs[rs1(instr) as usize] as i32).wrapping_add(imm) as u32;
+    let val = cpu.xregs.regs[rs2(instr) as usize] & std::u8::MAX as u32;
+    cpu.bus.store(addr, 8, val);
+}
+pub fn exec_sh(cpu: &mut CPU, instr: u32) {
+    let imm = imm_s(instr) as i32;
+    let addr = (cpu.xregs.regs[rs1(instr) as usize] as i32).wrapping_add(imm) as u32;
+    let val = cpu.xregs.regs[rs2(instr) as usize] & std::u16::MAX as u32;
+    cpu.bus.store(addr, 16, val);
+}
+pub fn exec_sw(cpu: &mut CPU, instr: u32) {
+    let imm = imm_s(instr) as i32;
+    let addr = (cpu.xregs.regs[rs1(instr) as usize] as i32).wrapping_add(imm) as u32;
+    let val = cpu.xregs.regs[rs2(instr) as usize] & std::u32::MAX as u32;
+    cpu.bus.store(addr, 32, val);
+}
 pub fn exec_addi(cpu: &mut CPU, instr: u32) {
     let imm = imm_i(instr);
     cpu.xregs.regs[rd(instr) as usize] = cpu.xregs.regs[rs1(instr) as usize] + imm as u32;
diff --git a/tests/cpu_test.rs b/tests/cpu_test.rs
index 6ec78d0..55a012a 100644
--- a/tests/cpu_test.rs
+++ b/tests/cpu_test.rs
@@ -286,11 +286,47 @@ mod tests {
         assert_eq!(cpu_test.xregs.regs[31], val & std::u32::MAX);
     }
     #[test]
-    fn test_exec_sb() {}
+    fn test_exec_sb() {
+        let mut cpu_test = cpu::CPU::new();
+        let offset = 3;
+        let val = (-2 as i32) as u32;
+        let rd = 5 + MEM_BASE;
+        helper::set_register_val(&mut cpu_test, 29, rd as i32);
+        helper::set_register_val(&mut cpu_test, 30, val as i32);
+        // sb x30, x29, 3
+        let instr: u32 = helper::set_s_type_instruction(offset as i16, 30, 29, SB as u8);
+        cpu::exec_sb(&mut cpu_test, instr);
+        assert_eq!(cpu_test.bus.load(rd + offset, 8), val & std::u8::MAX as u32);
+    }
     #[test]
-    fn test_exec_sh() {}
+    fn test_exec_sh() {
+        let mut cpu_test = cpu::CPU::new();
+        let offset = 3;
+        let val = (-2 as i32) as u32;
+        let rd = 5 + MEM_BASE;
+        helper::set_register_val(&mut cpu_test, 29, rd as i32);
+        helper::set_register_val(&mut cpu_test, 30, val as i32);
+        // sh x30, x29, 3
+        let instr: u32 = helper::set_s_type_instruction(offset as i16, 30, 29, SH as u8);
+        cpu::exec_sh(&mut cpu_test, instr);
+        assert_eq!(
+            cpu_test.bus.load(rd + offset, 16),
+            val & std::u16::MAX as u32
+        );
+    }
     #[test]
-    fn test_exec_sw() {}
+    fn test_exec_sw() {
+        let mut cpu_test = cpu::CPU::new();
+        let offset = 3;
+        let val = (-2 as i32) as u32;
+        let rd = 5 + MEM_BASE;
+        helper::set_register_val(&mut cpu_test, 29, rd as i32);
+        helper::set_register_val(&mut cpu_test, 30, val as i32);
+        // sw x30, x29, 3
+        let instr: u32 = helper::set_s_type_instruction(offset as i16, 30, 29, SW as u8);
+        cpu::exec_sw(&mut cpu_test, instr);
+        assert_eq!(cpu_test.bus.load(rd + offset, 32), val);
+    }
     #[test]
     fn test_exec_addi() {
         let mut cpu_test = cpu::CPU::new();
diff --git a/tests/helper.rs b/tests/helper.rs
index cb79330..3e4c260 100644
--- a/tests/helper.rs
+++ b/tests/helper.rs
@@ -1,6 +1,6 @@
 use emurv::{
     cpu,
-    opcode::{ADDI, B_TYPE, I_TYPE, LOAD, LUI, R_TYPE},
+    opcode::{ADDI, B_TYPE, I_TYPE, LOAD, LUI, R_TYPE, S_TYPE},
 };
 
 pub fn set_r_type_instruction(rs2: u8, rs1: u8, funct3: u8, rd: u8) -> u32 {
@@ -21,6 +21,18 @@ pub fn set_i_type_instruction(imm: i16, rs1: u8, funct3: u8, rd: u8) -> u32 {
         | ((I_TYPE as u32) & 0x7f);
 }
 
+pub fn set_s_type_instruction(imm: i16, rs2: u8, rs1: u8, funct3: u8) -> u32 {
+    let imm11_5 = (imm & 0x7f0) as u32;
+    let imm4_0 = (imm & 0x1f) as u32;
+
+    return (imm11_5 << 20)
+        | ((rs2 as u32 & 0x1f) << 20)
+        | ((rs1 as u32 & 0x1f) << 15)
+        | ((funct3 as u32 & 0x7) << 12)
+        | (imm4_0 << 7)
+        | ((S_TYPE as u32) & 0x7f);
+}
+
 pub fn set_load_type_instruction(imm: i16, rs1: u8, funct3: u8, rd: u8) -> u32 {
     // |31-20|19-15|14-12|11-7|6-0|
     return ((imm as u32 & 0xfff) << 20)