From 88b908f2a09fb8c3f9bdeac2e37bae2a7fde36f2 Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Tue, 12 Nov 2019 18:55:26 +0200
Subject: [PATCH] Implemenet GBA Halt control

Former-commit-id: 1b09cb881b4e68f410abe6e1f6a161cd55a4626a
---
 src/core/gba.rs   | 26 ++++++++++++--------------
 src/core/iodev.rs | 18 +++++++++++++++++-
 2 files changed, 29 insertions(+), 15 deletions(-)

diff --git a/src/core/gba.rs b/src/core/gba.rs
index 5fd0df9..d71552e 100644
--- a/src/core/gba.rs
+++ b/src/core/gba.rs
@@ -1,12 +1,11 @@
 /// Struct containing everything
-use super::arm7tdmi::{exception::Exception, Core, CpuState, DecodedInstruction};
+use super::arm7tdmi::Core;
 use super::cartridge::Cartridge;
 use super::gpu::*;
 use super::interrupt::*;
 use super::iodev::*;
 use super::sysbus::SysBus;
 
-use super::GBAResult;
 use super::SyncedIoDevice;
 use crate::backend::*;
 
@@ -45,14 +44,6 @@ impl GameBoyAdvance {
         self.sysbus.io.keyinput = self.backend.get_key_state();
     }
 
-    // TODO deprecate
-    pub fn step(&mut self) -> GBAResult<DecodedInstruction> {
-        let previous_cycles = self.cpu.cycles;
-        let executed_insn = self.cpu.step_one(&mut self.sysbus)?;
-        let cycles = self.cpu.cycles - previous_cycles;
-        Ok(executed_insn)
-    }
-
     pub fn step_new(&mut self) {
         let mut irqs = IrqBitmask(0);
         let previous_cycles = self.cpu.cycles;
@@ -63,14 +54,21 @@ impl GameBoyAdvance {
             &mut (*ptr).io as &mut IoDevices
         };
 
-        if !io.dmac.perform_work(&mut self.sysbus, &mut irqs) {
+        let cycles = if !io.dmac.perform_work(&mut self.sysbus, &mut irqs) {
             if io.intc.irq_pending() {
                 self.cpu.irq(&mut self.sysbus);
+                io.haltcnt = HaltState::Running;
             }
-            self.cpu.step(&mut self.sysbus).unwrap();
-        }
 
-        let cycles = self.cpu.cycles - previous_cycles;
+            if HaltState::Running == io.haltcnt {
+                self.cpu.step(&mut self.sysbus).unwrap();
+                self.cpu.cycles - previous_cycles
+            } else {
+                1
+            }
+        } else {
+            0
+        };
 
         io.timers.step(cycles, &mut self.sysbus, &mut irqs);
         if let Some(new_gpu_state) = io.gpu.step(cycles, &mut self.sysbus, &mut irqs) {
diff --git a/src/core/iodev.rs b/src/core/iodev.rs
index b20f4fd..8f7367b 100644
--- a/src/core/iodev.rs
+++ b/src/core/iodev.rs
@@ -9,6 +9,13 @@ use super::timer::Timers;
 
 use consts::*;
 
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub enum HaltState {
+    Running,
+    Halt, // In Halt mode, the CPU is paused as long as (IE AND IF)=0,
+    Stop, // In Stop mode, most of the hardware including sound and video are paused
+}
+
 #[derive(Debug)]
 pub struct IoDevices {
     pub intc: InterruptController,
@@ -18,6 +25,7 @@ pub struct IoDevices {
     pub keyinput: u16,
     pub post_boot_flag: bool,
     pub waitcnt: WaitControl, // TODO also implement 4000800
+    pub haltcnt: HaltState,
 
     mem: BoxedMemory,
 }
@@ -31,6 +39,7 @@ impl IoDevices {
             intc: InterruptController::new(),
             mem: BoxedMemory::new(vec![0; 0x800].into_boxed_slice()),
             post_boot_flag: false,
+            haltcnt: HaltState::Running,
             keyinput: keypad::KEYINPUT_ALL_RELEASED,
             waitcnt: WaitControl(0),
         }
@@ -223,7 +232,14 @@ impl Bus for IoDevices {
             REG_WAITCNT => io.waitcnt.0 = value,
 
             REG_POSTFLG => io.post_boot_flag = value != 0,
-            REG_HALTCNT => {}
+            REG_HALTCNT => {
+                if value & 0x80 != 0 {
+                    io.haltcnt = HaltState::Stop;
+                    panic!("Can't handle HaltCtrl == Stop yet");
+                } else {
+                    io.haltcnt = HaltState::Halt;
+                }
+            }
             _ => {
                 println!(
                     "Unimplemented write to {:x} {}",