From fe071bf1ecac4ec75e93027b70b16ddacbdfcd94 Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Wed, 7 Aug 2019 09:44:47 +0300
Subject: [PATCH] Fix timer bugs

Former-commit-id: 9839368895dca306b66a095ab0b9386c77bd2c12
---
 src/core/ioregs.rs |  6 +++---
 src/core/timer.rs  | 34 +++++++++++++++++++++-------------
 2 files changed, 24 insertions(+), 16 deletions(-)

diff --git a/src/core/ioregs.rs b/src/core/ioregs.rs
index b3a77d8..fdebd83 100644
--- a/src/core/ioregs.rs
+++ b/src/core/ioregs.rs
@@ -237,19 +237,19 @@ impl Bus for IoRegs {
 
             REG_TM1CNT_L => {
                 io.timers[1].timer_data = value;
-                io.timers[0].initial_data = value;
+                io.timers[1].initial_data = value;
             }
             REG_TM1CNT_H => io.timers[1].timer_ctl.0 = value,
 
             REG_TM2CNT_L => {
                 io.timers[2].timer_data = value;
-                io.timers[0].initial_data = value;
+                io.timers[2].initial_data = value;
             }
             REG_TM2CNT_H => io.timers[2].timer_ctl.0 = value,
 
             REG_TM3CNT_L => {
                 io.timers[3].timer_data = value;
-                io.timers[0].initial_data = value;
+                io.timers[3].initial_data = value;
             }
             REG_TM3CNT_H => io.timers[3].timer_ctl.0 = value,
 
diff --git a/src/core/timer.rs b/src/core/timer.rs
index c25c3eb..7dfb6a2 100644
--- a/src/core/timer.rs
+++ b/src/core/timer.rs
@@ -20,7 +20,7 @@ pub struct Timer {
 }
 
 pub enum TimerAction {
-    Overflow,
+    Overflow(usize),
     Increment,
 }
 
@@ -50,6 +50,7 @@ impl Timer {
     }
 
     pub fn add_cycles(&mut self, cycles: usize, irqs: &mut IrqBitmask) -> TimerAction {
+        let mut num_overflows = 0;
         self.cycles += cycles;
 
         let frequency = self.frequency();
@@ -60,12 +61,15 @@ impl Timer {
                 if self.timer_ctl.irq_enabled() {
                     irqs.add_irq(self.get_irq());
                 }
-                println!("timer{} overflow", self.timer_id);
-                return TimerAction::Overflow;
+                self.timer_data = self.initial_data;
+                num_overflows += 1;
             }
         }
-
-        return TimerAction::Increment;
+        if num_overflows > 0 {
+            return TimerAction::Overflow(num_overflows);
+        } else {
+            return TimerAction::Increment;
+        }
     }
 }
 
@@ -96,16 +100,20 @@ impl SyncedIoDevice for Timers {
         for i in 0..4 {
             if self[i].timer_ctl.enabled() && !self[i].timer_ctl.cascade() {
                 match self[i].add_cycles(cycles, irqs) {
-                    TimerAction::Overflow => match i {
-                        3 => {}
-                        _ => {
-                            let next_i = i +1;
-                            if self[next_i].timer_ctl.cascade() {
-                                println!("{:?} is cascade!", self[next_i]);
-                                self[next_i].add_cycles(1, irqs);
+                    TimerAction::Overflow(num_overflows) => {
+                        println!("timer{} overflowed {} times", i, num_overflows);
+                        match i {
+                            3 => {}
+                            _ => {
+                                let next_i = i + 1;
+                                if self[next_i].timer_ctl.cascade() {
+                                    println!("cascading into timer{}", next_i);
+                                    self[next_i].add_cycles(num_overflows, irqs);
+                                }
                             }
                         }
-                    },
+                    }
+                    TimerAction::Increment => {}
                     _ => {}
                 }
             }