Thumb: Implement MultipleLoadStore

Signed-off-by: Federico Guerinoni <guerinoni.federico@gmail.com>
RIP-Comm · May 16, 2023 · 318cedd · 318cedd
1 parent fbda12d
commit 318cedd
Show file tree

Hide file tree

Showing 3 changed files with 123 additions and 5 deletions.
diff --git a/emu/src/cpu/arm7tdmi.rs b/emu/src/cpu/arm7tdmi.rs
@@ -281,7 +281,11 @@ impl Arm7tdmi {
                 pc_lr,
                 register_list,
             } => self.push_pop_register(load_store, pc_lr, register_list),
-            MultipleLoadStore => unimplemented!(),
+            MultipleLoadStore {
+                load_store,
+                base_register,
+                register_list,
+            } => self.multiple_load_store(load_store, base_register, register_list),
             CondBranch {
                 condition,
                 immediate_offset,
@@ -1094,6 +1098,35 @@ impl Arm7tdmi {
         }
     }
 
+    pub(crate) fn multiple_load_store(
+        &mut self,
+        load_store: LoadStoreKind,
+        base_register: u16,
+        register_list: u16,
+    ) -> Option<u32> {
+        match load_store {
+            LoadStoreKind::Store => unimplemented!("multiple store"),
+            LoadStoreKind::Load => {
+                let base_address = self.registers.register_at(base_register as usize);
+                let mut register_one = 0;
+                for r in 0..=7 {
+                    if register_list.get_bit(r) {
+                        let address = base_address + (register_one as u32 * 4);
+                        let value = self.memory.lock().unwrap().read_word(address as usize);
+                        self.registers.set_register_at(r as usize, value);
+                        register_one += 1;
+                    }
+                }
+
+                let new_base_address = base_address + (register_list.count_ones() * 4);
+                self.registers
+                    .set_register_at(base_register as usize, new_base_address);
+            }
+        }
+
+        Some(SIZE_OF_THUMB_INSTRUCTION)
+    }
+
     fn add_offset_sp(&mut self, s: bool, word7: u16) -> Option<u32> {
         let value = (word7 as i32).mul(if s { -1 } else { 1 });
         let old_sp = self.registers.register_at(REG_SP) as i32;
@@ -2615,4 +2648,44 @@ mod tests {
             (*case.check_fn)(cpu);
         }
     }
+
+    #[test]
+    fn check_multiple_load_store() {
+        struct Test {
+            opcode: u16,
+            expected_decode: ThumbModeInstruction,
+            prepare_fn: Box<dyn Fn(&mut Arm7tdmi)>,
+            check_fn: Box<dyn Fn(Arm7tdmi)>,
+        }
+
+        for case in [Test {
+            opcode: 0b1100_1_001_10100000,
+            expected_decode: ThumbModeInstruction::MultipleLoadStore {
+                load_store: LoadStoreKind::Load,
+                base_register: 1,
+                register_list: 160,
+            },
+            prepare_fn: Box::new(|cpu| {
+                cpu.registers.set_register_at(1, 100);
+                cpu.memory.lock().unwrap().write_at(100, 0xFF);
+                cpu.memory.lock().unwrap().write_at(104, 0xFF);
+            }),
+            check_fn: Box::new(|cpu| {
+                assert_eq!(cpu.registers.register_at(5), 0xFF);
+                assert_eq!(cpu.registers.register_at(7), 0xFF);
+                assert_eq!(cpu.registers.register_at(1), 108);
+            }),
+        }] {
+            let mut cpu = Arm7tdmi::default();
+            let op_code = case.opcode;
+            let op_code: ThumbModeOpcode = cpu.decode(op_code);
+            assert_eq!(op_code.instruction, case.expected_decode);
+
+            (*case.prepare_fn)(&mut cpu);
+
+            cpu.execute_thumb(op_code);
+
+            (*case.check_fn)(cpu);
+        }
+    }
 }
diff --git a/emu/src/cpu/instruction.rs b/emu/src/cpu/instruction.rs
@@ -495,7 +495,11 @@ pub enum ThumbModeInstruction {
         pc_lr: bool,
         register_list: u16,
     },
-    MultipleLoadStore,
+    MultipleLoadStore {
+        load_store: LoadStoreKind,
+        base_register: u16,
+        register_list: u16,
+    },
     CondBranch {
         condition: Condition,
         immediate_offset: i32,
@@ -689,7 +693,25 @@ impl ThumbModeInstruction {
 
                 format!("{instr} {{{registers}}}")
             }
-            Self::MultipleLoadStore => "".to_string(),
+            Self::MultipleLoadStore {
+                load_store,
+                base_register,
+                register_list,
+            } => {
+                let instr = match load_store {
+                    LoadStoreKind::Load => "LDMIA",
+                    LoadStoreKind::Store => "STMIA",
+                };
+
+                let mut registers = String::new();
+                for i in 0..=7 {
+                    if register_list.get_bit(i) {
+                        registers.push_str(&format!("R{}, ", i));
+                    }
+                }
+
+                format!("{instr} R{base_register}!, {{{registers}}}")
+            }
             Self::CondBranch {
                 condition,
                 immediate_offset,
@@ -832,7 +854,14 @@ impl From<u16> for ThumbModeInstruction {
                 offset: (op_code.get_bits(0..=7) as u32) << 2,
             }
         } else if op_code.get_bits(12..=15) == 0b1100 {
-            MultipleLoadStore
+            let load_store = op_code.get_bit(11).into();
+            let base_register = op_code.get_bits(8..=10);
+            let register_list = op_code.get_bits(0..=7);
+            MultipleLoadStore {
+                load_store,
+                base_register,
+                register_list,
+            }
         } else if op_code.get_bits(12..=15) == 0b1101 {
             let condition = op_code.get_bits(8..=11) as u8;
             // 9 bits signed offset (assembler puts `label` >> 1 in this field so we should <<1)
@@ -995,4 +1024,18 @@ mod tests {
             output
         );
     }
+
+    #[test]
+    fn decode_multiple_load_store() {
+        let cpu = Arm7tdmi::default();
+        let output: ThumbModeInstruction = cpu.decode(0b1100_1001_1010_0000);
+        assert_eq!(
+            ThumbModeInstruction::MultipleLoadStore {
+                load_store: LoadStoreKind::Load,
+                base_register: 1,
+                register_list: 160,
+            },
+            output
+        );
+    }
 }
diff --git a/emu/src/cpu/opcode.rs b/emu/src/cpu/opcode.rs
@@ -146,7 +146,9 @@ impl Display for ThumbModeOpcode {
             ThumbModeInstruction::LoadAddress { .. } => "FMT: |1_0_0_1|S|_Rd__|_____Word8_____|",
             ThumbModeInstruction::AddOffsetSP { .. } => "FMT: |1_0_1_1_0_0_0_0|S|____Word7____|",
             ThumbModeInstruction::PushPopReg { .. } => "FMT: |1_0_1_1|L|1_0|R|_____Rlist_____|",
-            ThumbModeInstruction::MultipleLoadStore => todo!(),
+            ThumbModeInstruction::MultipleLoadStore { .. } => {
+                "FMT: |1_1_0_0|L|_Rb__|_____Rlist_____|"
+            }
             ThumbModeInstruction::CondBranch { .. } => "FMT: |1_1_0_1|_Cond__|_____Offset____|",
             ThumbModeInstruction::Swi => todo!(),
             ThumbModeInstruction::UncondBranch { .. } => "FMT: |1_1_1_0_0|________Offset11_____|",