Implement LDR/STR (not tested) and add cycle counting

Former-commit-id: ec9e6bfc2a94291e47d41ff7d839007879d3d694

src/arm7tdmi/arm/display.rs

@@ -1,6 +1,6 @@
 use std::fmt;
 
-use super::super::{reg_string, REG_PC};
+use crate::arm7tdmi::{Addr, reg_string, REG_PC};
 use super::{
     ArmCond, ArmHalfwordTransferType, ArmInstruction, ArmInstructionFormat, ArmOpCode,
     ArmRegisterShift, ArmShiftType, ArmShiftedValue,
@@ -108,7 +108,7 @@ impl ArmInstruction {
             "b{link}{cond}\t{ofs:#x}",
             link = if self.link_flag() { "l" } else { "" },
             cond = self.cond,
-            ofs = 8 + self.pc.wrapping_add(self.branch_offset() as u32) as u32
+            ofs = 8 + self.pc.wrapping_add(self.branch_offset() as Addr)
         )
     }
 

src/arm7tdmi/arm/exec.rs

@@ -1,11 +1,11 @@
 use crate::bit::BitIndex;
 
-use super::super::{
+use crate::arm7tdmi::cpu::{Core, CpuExecResult, CpuPipelineAction};
-    cpu::{Core, CpuExecResult, CpuPipelineAction},
+use crate::arm7tdmi::exception::Exception;
-    exception::Exception,
+use crate::arm7tdmi::bus::{Bus, MemoryAccess, MemoryAccessType::*, MemoryAccessWidth::*};
-    sysbus::SysBus,
+use crate::arm7tdmi::{Addr, CpuError, CpuInstruction, CpuResult, CpuState, REG_PC};
-    CpuError, CpuInstruction, CpuResult, CpuState, REG_PC,
+
-};
+use crate::sysbus::SysBus;
 
 use super::{
     ArmCond, ArmInstruction, ArmInstructionFormat, ArmOpCode, ArmRegisterShift, ArmShiftType,
@@ -15,6 +15,8 @@ use super::{
 impl Core {
     pub fn exec_arm(&mut self, sysbus: &mut SysBus, insn: ArmInstruction) -> CpuExecResult {
         if !self.check_arm_cond(insn.cond) {
+            self.add_cycles(self.pc + (self.word_size() as u32), sysbus, Seq + MemoryAccess32);
+            self.add_cycle();
             return Ok(CpuPipelineAction::IncPC);
         }
         match insn.fmt {
@@ -29,23 +31,32 @@ impl Core {
         }
     }
 
+    /// Cycles 2S+1N
     fn exec_b_bl(
         &mut self,
-        _sysbus: &mut SysBus,
+        sysbus: &mut SysBus,
         insn: ArmInstruction,
     ) -> CpuResult<CpuPipelineAction> {
-        if self.verbose && insn.cond != ArmCond::Always {
-        }
         if insn.link_flag() {
             self.set_reg(14, self.pc & !0b1);
         }
+        
+        // +1N
+        self.add_cycles(self.pc, sysbus, NonSeq + MemoryAccess32);
+
         self.pc = (self.pc as i32).wrapping_add(insn.branch_offset()) as u32;
+
+        // +2S
+        self.add_cycles(self.pc, sysbus, Seq + MemoryAccess32);
+        self.add_cycles(self.pc + (self.word_size() as u32), sysbus, Seq + MemoryAccess32);
+
         Ok(CpuPipelineAction::Flush)
     }
 
+    /// Cycles 2S+1N
     fn exec_bx(
         &mut self,
-        _sysbus: &mut SysBus,
+        sysbus: &mut SysBus,
         insn: ArmInstruction,
     ) -> CpuResult<CpuPipelineAction> {
         let rn = self.get_reg(insn.rn());
@@ -55,7 +66,15 @@ impl Core {
             self.cpsr.set_state(CpuState::ARM);
         }
 
+        // +1N
+        self.add_cycles(self.pc, sysbus, NonSeq + MemoryAccess32);
+        
         self.pc = rn & !1;
+
+        // +2S
+        self.add_cycles(self.pc, sysbus, Seq + MemoryAccess32);
+        self.add_cycles(self.pc + (self.word_size() as u32), sysbus, Seq + MemoryAccess32);
+
         Ok(CpuPipelineAction::Flush)
     }
 
@@ -139,16 +158,28 @@ impl Core {
         }
     }
 
+    /// Logical/Arithmetic ALU operations
+    /// 
+    /// Cycles: 1S+x+y (from GBATEK)
+    ///         Add x=1I cycles if Op2 shifted-by-register. Add y=1S+1N cycles if Rd=R15.
     fn exec_data_processing(
         &mut self,
-        _sysbus: &mut SysBus,
+        sysbus: &mut SysBus,
         insn: ArmInstruction,
     ) -> CpuResult<CpuPipelineAction> {
         // TODO handle carry flag
 
+        let mut pipeline_action = CpuPipelineAction::IncPC;
+
         let op1 = self.get_reg(insn.rn()) as i32;
         let op2 = insn.operand2()?;
 
+        let rd = insn.rd();
+        if rd == REG_PC {
+            // +1N
+            self.add_cycles(self.pc, sysbus, NonSeq + MemoryAccess32);
+        }
+
         let op2: i32 = match op2 {
             ArmShiftedValue::RotatedImmediate(immediate, rotate) => {
                 Ok(immediate.rotate_right(rotate) as i32)
@@ -157,26 +188,123 @@ impl Core {
                 reg,
                 shift,
                 added: _,
-            } => self.register_shift(reg, shift),
+            } => {
+                // +1I
+                self.add_cycle();
+                self.register_shift(reg, shift)
+            }
             _ => unreachable!(),
         }?;
 
         let opcode = insn.opcode().unwrap();
         let set_flags = opcode.is_setting_flags() || insn.set_cond_flag();
         if let Some(result) = self.alu(opcode, op1, op2, set_flags) {
-            self.set_reg(insn.rd(), result as u32)
+            self.set_reg(rd, result as u32);
+            if (rd == REG_PC) {
+                pipeline_action = CpuPipelineAction::Flush;
+                // +1S
+                self.add_cycles(self.pc, sysbus, Seq + MemoryAccess32);
+            }
         }
 
-        Ok(CpuPipelineAction::IncPC)
+        // +1S
+        self.add_cycles(self.pc + (self.word_size() as u32), sysbus, Seq + MemoryAccess32);
+        Ok(pipeline_action)
     }
 
+    fn get_rn_offset(&mut self, insn: &ArmInstruction) -> i32 {
+        // TODO decide if error handling or panic here
+        match insn.ldr_str_offset().unwrap() {
+            ArmShiftedValue::ImmediateValue(offset) => offset,
+            ArmShiftedValue::ShiftedRegister {
+                reg,
+                shift,
+                added: Some(added)
+            } => {
+                let abs = self.register_shift(reg, shift).unwrap();
+                if added { abs } else { -abs }
+            }
+            _ => panic!("bad barrel shift")
+        }
+    }
+
+    /// Memory Load/Store
+    /// Instruction                     |  Cycles       | Flags | Expl.
+    /// ------------------------------------------------------------------------------
+    /// LDR{cond}{B}{T} Rd,<Address>    | 1S+1N+1I+y    | ----  |  Rd=[Rn+/-<offset>]
+    /// STR{cond}{B}{T} Rd,<Address>    | 2N            | ----  |  [Rn+/-<offset>]=Rd
+    /// ------------------------------------------------------------------------------
+    /// For LDR, add y=1S+1N if Rd=R15.
     fn exec_ldr_str(
         &mut self,
-        _sysbus: &mut SysBus,
+        sysbus: &mut SysBus,
         insn: ArmInstruction,
     ) -> CpuResult<CpuPipelineAction> {
-        let rn = self.get_reg(insn.rn());
+
-        let rd = self.get_reg(insn.rd());
+        if insn.write_back_flag() && insn.rd() == insn.rn() {
+            return Err(CpuError::IllegalInstruction);
+        }
+
+        let mut pipeline_action = CpuPipelineAction::IncPC;
+
+        let mut addr = self.get_reg(insn.rn());
+        if insn.rn() == REG_PC {
+            addr += 8; // prefetching
+        }
+        let dest = self.get_reg(insn.rd());
+
+        let offset = self.get_rn_offset(&insn);
+
+        let effective_addr = (addr as i32).wrapping_add(offset) as Addr;
+        addr = if insn.pre_index_flag() {
+            effective_addr
+        }  else {
+            addr
+        };
+
+        if insn.load_flag() {
+            let data = if insn.transfer_size() == 1 {
+                // +1N
+                self.add_cycles(dest, sysbus, NonSeq + MemoryAccess8);
+                sysbus.read_8(addr) as u32
+            } else {
+                // +1N
+                self.add_cycles(dest, sysbus, NonSeq + MemoryAccess32);
+                sysbus.read_32(addr)
+            };
+            // +1S
+            self.add_cycles(self.pc + (self.word_size() as u32), sysbus, Seq + MemoryAccess32);
+            
+            self.set_reg(insn.rd(), data);
+
+            // +1I
+            self.add_cycle();
+            // +y
+            if insn.rd() == REG_PC {
+                // +1S
+                self.add_cycles(self.pc, sysbus, Seq + MemoryAccess32);
+                // +1N
+                self.add_cycles(self.pc + (self.word_size() as u32), sysbus, NonSeq + MemoryAccess32);
+                pipeline_action = CpuPipelineAction::Flush;
+            }
+        } else {
+            self.add_cycles(addr, sysbus, NonSeq + MemoryAccess32);
+            let value = self.get_reg(insn.rn());
+            if insn.transfer_size() == 1 {
+                // +1N
+                self.add_cycles(dest, sysbus, NonSeq + MemoryAccess8);
+                sysbus.write_8(addr, value as u8);
+            } else {
+                // +1N
+                self.add_cycles(dest, sysbus, NonSeq + MemoryAccess32);
+                sysbus.write_32(addr, value);
+            };
+        }
+
+        if insn.write_back_flag() {
+            self.set_reg(insn.rn(), effective_addr as u32)
+        }
+
         Ok(CpuPipelineAction::IncPC)
     }
 }

src/arm7tdmi/arm/mod.rs

@@ -1,6 +1,8 @@
 pub mod display;
 pub mod exec;
 
+use crate::arm7tdmi::Addr;
+
 use crate::bit::BitIndex;
 use crate::num_traits::FromPrimitive;
 use std::convert::TryFrom;
@@ -19,11 +21,11 @@ use ArmDecodeErrorKind::*;
 pub struct ArmDecodeError {
     pub kind: ArmDecodeErrorKind,
     pub insn: u32,
-    pub addr: u32,
+    pub addr: Addr,
 }
 
 impl ArmDecodeError {
-    fn new(kind: ArmDecodeErrorKind, insn: u32, addr: u32) -> ArmDecodeError {
+    fn new(kind: ArmDecodeErrorKind, insn: u32, addr: Addr) -> ArmDecodeError {
         ArmDecodeError {
             kind: kind,
             insn: insn,
@@ -125,13 +127,13 @@ pub struct ArmInstruction {
     pub cond: ArmCond,
     pub fmt: ArmInstructionFormat,
     pub raw: u32,
-    pub pc: u32,
+    pub pc: Addr,
 }
 
-impl TryFrom<(u32, u32)> for ArmInstruction {
+impl TryFrom<(u32, Addr)> for ArmInstruction {
     type Error = ArmDecodeError;
 
-    fn try_from(value: (u32, u32)) -> Result<Self, Self::Error> {
+    fn try_from(value: (u32, Addr)) -> Result<Self, Self::Error> {
         use ArmInstructionFormat::*;
         let (raw, addr) = value;
 

src/arm7tdmi/bus.rs

@@ -0,0 +1,38 @@
+use std::io;
+use std::ops::Add;
+use super::Addr;
+
+pub enum MemoryAccessType {
+    NonSeq,
+    Seq
+}
+
+pub enum MemoryAccessWidth {
+    MemoryAccess8,
+    MemoryAccess16,
+    MemoryAccess32
+}
+
+
+impl Add<MemoryAccessWidth> for MemoryAccessType {
+    type Output = MemoryAccess;
+
+    fn add(self, other: MemoryAccessWidth) -> Self::Output {
+        MemoryAccess(self, other)
+    }
+}
+
+pub struct MemoryAccess(MemoryAccessType, MemoryAccessWidth);
+
+pub trait Bus {
+    fn read_32(&self, addr: Addr) -> u32;
+    fn read_16(&self, addr: Addr) -> u16;
+    fn read_8(&self, addr: Addr) -> u8;
+    fn write_32(&mut self, addr: Addr, value: u32) -> Result<(), io::Error>;
+    fn write_16(&mut self, addr: Addr, value: u16) -> Result<(), io::Error>;
+    fn write_8(&mut self, addr: Addr, value: u8) -> Result<(), io::Error>;
+
+    fn get_bytes(&self, addr: Addr, size: usize) -> Option<&[u8]>;
+    /// returns the number of cycles needed for this memory access
+    fn get_cycles(&self, addr: Addr, access: MemoryAccess) -> usize;
+}

src/arm7tdmi/cpu.rs

@@ -1,16 +1,18 @@
+use std::ops::Add;
 use std::convert::TryFrom;
 use std::fmt;
 
 use ansi_term::{Colour, Style};
 
-use super::*;
+use crate::sysbus::SysBus;
 
 pub use super::exception::Exception;
-use super::psr::RegPSR;
+use super::{
-use super::reg_string;
+    CpuState, CpuMode, reg_string, CpuResult, Addr,
-use super::sysbus::SysBus;
+    psr::RegPSR,
-
+    bus::{Bus, MemoryAccess, MemoryAccessType::*, MemoryAccessWidth::*},
-type Addr = u32;
+    arm::*
+};
 
 #[derive(Debug, Default)]
 pub struct PipelineContext {
@@ -143,6 +145,18 @@ impl Core {
         self.pc = self.pc.wrapping_add(self.word_size() as u32)
     }
 
+    pub fn cycles(&self) -> usize {
+        self.cycles
+    }
+
+    pub fn add_cycle(&mut self) {
+        self.cycles += 1;
+    }
+
+    pub fn add_cycles(&mut self, addr: Addr, sysbus: &SysBus, access: MemoryAccess) {
+        self.cycles += sysbus.get_cycles(addr, access);
+    }
+
     pub fn check_arm_cond(&self, cond: ArmCond) -> bool {
         use ArmCond::*;
         match cond {
@@ -176,6 +190,7 @@ impl Core {
             Some((addr, i)) => Some(ArmInstruction::try_from((i, addr)).unwrap()),
             None => None,
         };
+
         // exec
         let result = match self.pipeline.decoded {
             Some(d) => {
@@ -183,7 +198,9 @@ impl Core {
                 let action = self.exec_arm(sysbus, d)?;
                 Ok((Some(d), action))
             }
-            None => Ok((None, CpuPipelineAction::IncPC)),
+            None => {
+                Ok((None, CpuPipelineAction::IncPC))
+            },
         };
 
         self.pipeline.fetched = Some((self.pc, new_fetched));
@@ -197,10 +214,6 @@ impl Core {
     /// Perform a pipeline step
     /// If an instruction was executed in this step, return it.
     pub fn step(&mut self, sysbus: &mut SysBus) -> CpuResult<Option<ArmInstruction>> {
-        if self.cycles > 0 {
-            self.cycles -= 1;
-            return Ok(None);
-        }
         let (executed_instruction, pipeline_action) = match self.cpsr.state() {
             CpuState::ARM => self.step_arm(sysbus),
             CpuState::THUMB => unimplemented!("thumb not implemented :("),
@@ -220,7 +233,7 @@ impl Core {
     /// Get's the address of the next instruction that is going to be executed
     pub fn get_next_pc(&self) -> Addr {
         if self.pipeline.is_flushed() {
-            self.pc
+            self.pc as Addr
         } else if self.pipeline.is_only_fetched() {
             self.pipeline.fetched.unwrap().0
         } else if self.pipeline.is_ready_to_execute() {
@@ -245,6 +258,7 @@ impl Core {
 impl fmt::Display for Core {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
         writeln!(f, "ARM7TDMI Core Status:")?;
+        writeln!(f, "\tCycles: {}", self.cycles)?;
         writeln!(f, "\tCPSR: {}", self.cpsr)?;
         writeln!(f, "\tGeneral Purpose Registers:")?;
         let reg_normal_style = Style::new().bold();

src/arm7tdmi/mod.rs

@@ -1,20 +1,20 @@
 use std::fmt;
 
-use crate::num_traits::FromPrimitive;
-
 pub mod arm;
-use arm::*;
+use arm::{ArmInstruction, ArmDecodeError};
 
 pub mod cpu;
+pub mod bus;
 mod exception;
 mod psr;
 
-pub use super::sysbus;
 
 pub const REG_PC: usize = 15;
 pub const REG_LR: usize = 14;
 pub const REG_SP: usize = 13;
 
+pub type Addr = u32;
+
 pub fn reg_string(reg: usize) -> &'static str {
     let reg_names = &[
         "r0", "r1", "r2", "r3", "r4", "r5", "r6", "r7", "r8", "r9", "r10", "fp", "ip", "sp", "lr",

src/debugger/command.rs

@@ -1,4 +1,5 @@
 use crate::arm7tdmi::{reg_string, REG_PC};
+use crate::arm7tdmi::bus::Bus;
 use crate::debugger::Debugger;
 use crate::disass::Disassembler;
 
@@ -56,7 +57,13 @@ impl Command {
                     break;
                 }
                 match debugger.cpu.step_debugger(&mut debugger.sysbus) {
-                    Ok(_) => (),
+                    Ok(insn) => {
+                        println!(
+                                "@0x{:08x}:\n\t{}",
+                                insn.pc,
+                                Colour::Yellow.italic().paint(format!("{} ", insn))
+                            );
+                    },
                     Err(e) => {
                         println!("{}: {}", "cpu encountered an error".red(), e);
                         println!("cpu: {:x?}", debugger.cpu);

src/sysbus.rs

@@ -1,3 +1,8 @@
+use std::io;
+
+use super::arm7tdmi::Addr;
+use super::arm7tdmi::bus::{Bus, MemoryAccess, MemoryAccessType, MemoryAccessWidth};
+
 use byteorder::{LittleEndian, ReadBytesExt, WriteBytesExt};
 
 const VIDEO_RAM_SIZE: usize = 128 * 1024;
@@ -9,35 +14,119 @@ const BIOS_SIZE: usize = 16 * 1024;
 const GAMEPAK_ROM_SIZE: usize = 32 * 1024 * 1024;
 
 #[derive(Debug)]
-pub struct SysBus {
+struct BiosROM(Vec<u8>);
-    bios_rom: Vec<u8>,
-}
 
-impl SysBus {
+impl Bus for BiosROM {
-    pub fn new(bios_rom: Vec<u8>) -> SysBus {
+    fn read_32(&self, addr: Addr) -> u32 {
-        SysBus { bios_rom: bios_rom }
-    }
-
-    pub fn read_32(&self, addr: u32) -> u32 {
         let addr = addr as usize;
-        (&self.bios_rom[addr..addr + 4])
+        (&self.0[addr..addr + 4])
             .read_u32::<LittleEndian>()
             .unwrap()
     }
 
-    pub fn read_16(&self, addr: u32) -> u16 {
+    fn read_16(&self, addr: Addr) -> u16 {
         let addr = addr as usize;
-        (&self.bios_rom[addr..addr + 4])
+        (&self.0[addr..addr + 4])
             .read_u16::<LittleEndian>()
             .unwrap()
     }
 
-    pub fn get_bytes(&self, addr: u32, size: usize) -> Option<&[u8]> {
+    fn read_8(&self, addr: Addr) -> u8 {
+        self.0[addr as usize]
+    }
+
+    fn write_32(&mut self, addr: Addr, value: u32) -> Result<(), io::Error> {
+        let mut wrt =  io::Cursor::new(&mut self.0);
+        wrt.set_position(addr as u64);
+        wrt.write_u32::<LittleEndian>(value)
+    }
+
+    fn write_16(&mut self, addr: Addr, value: u16) -> Result<(), io::Error> {
+        let mut wrt =  io::Cursor::new(&mut self.0);
+        wrt.set_position(addr as u64);
+        wrt.write_u16::<LittleEndian>(value)
+    }
+
+    fn write_8(&mut self, addr: Addr, value: u8) -> Result<(), io::Error> {
+        let mut wrt = io::Cursor::new(&mut self.0);
+        wrt.write_u8(value)
+    }
+
+    fn get_bytes(&self, addr: Addr, size: usize) -> Option<&[u8]> {
         let addr = addr as usize;
-        if addr + size > self.bios_rom.len() {
+        if addr + size > self.0.len() {
             None
         } else {
-            Some(&self.bios_rom[addr..addr + size])
+            Some(&self.0[addr..addr + size])
+        }
+    }
+    
+    fn get_cycles(&self, _addr: Addr, _access: MemoryAccess) -> usize {
+        1
+    }
+}
+
+#[derive(Debug)]
+enum SysBusDevice {
+    BiosROM(BiosROM)
+}
+
+#[derive(Debug)]
+pub struct SysBus {
+    bios: BiosROM
+}
+
+impl SysBus {
+    pub fn new(bios_rom: Vec<u8>) -> SysBus {
+        SysBus { bios: BiosROM(bios_rom) }
+    }
+
+    fn map(&self, addr: Addr) -> & impl Bus {
+        match addr as usize {
+            0...BIOS_SIZE => &self.bios,
+            _ => panic!("unmapped address")
+        }
+    }
+
+    fn map_mut(&mut self, addr: Addr) -> &mut impl Bus {
+        match addr as usize {
+            0...BIOS_SIZE => &mut self.bios,
+            _ => panic!("unmapped address")
         }
     }
 }
+
+impl Bus for SysBus {
+    fn read_32(&self, addr: Addr) -> u32 {
+        self.map(addr).read_32(addr)
+    }
+
+    fn read_16(&self, addr: Addr) -> u16 {
+        self.map(addr).read_16(addr)
+    }
+
+    fn read_8(&self, addr: Addr) -> u8 {
+        self.map(addr).read_8(addr)
+    }
+
+    fn write_32(&mut self, addr: Addr, value: u32) -> Result<(), io::Error> {
+        self.map_mut(addr).write_32(addr, value)
+    }
+
+    fn write_16(&mut self, addr: Addr, value: u16) -> Result<(), io::Error> {
+        self.map_mut(addr).write_16(addr, value)
+    }
+
+    fn write_8(&mut self, addr: Addr, value: u8) -> Result<(), io::Error> {
+        self.map_mut(addr).write_8(addr, value)
+    }
+
+
+    fn get_bytes(&self, addr: Addr, size: usize) -> Option<&[u8]> {
+        self.map(addr).get_bytes(addr, size)
+    }
+
+    fn get_cycles(&self, addr: Addr, access: MemoryAccess) -> usize {
+        self.map(addr).get_cycles(addr, access)
+    }
+}