From d59da1009a4a4b3ecaec67a725aed4cc38cf4db8 Mon Sep 17 00:00:00 2001
From: Simon Gellis <simongellis@gmail.com>
Date: Sat, 18 Jan 2025 18:51:25 -0500
Subject: [PATCH] Fix watchpoint reporting

---
 src/emulator/address_set.rs     | 43 +++++++++++++++++++++++++++++++--
 src/emulator/shrooms_vb_core.rs |  8 +++---
 2 files changed, 45 insertions(+), 6 deletions(-)

diff --git a/src/emulator/address_set.rs b/src/emulator/address_set.rs
index fe8e5be..4e8ee17 100644
--- a/src/emulator/address_set.rs
+++ b/src/emulator/address_set.rs
@@ -1,11 +1,11 @@
 use std::{
-    collections::{BTreeMap, HashSet},
+    collections::{BTreeMap, BTreeSet},
     ops::Bound,
 };
 
 #[derive(Debug, Default)]
 pub struct AddressSet {
-    ranges: HashSet<(u32, usize)>,
+    ranges: BTreeSet<(u32, usize)>,
     bounds: BTreeMap<u32, usize>,
 }
 
@@ -109,6 +109,22 @@ impl AddressSet {
             .next_back()
             .is_some_and(|(_, &val)| val > 0)
     }
+
+    pub fn start_of_range_containing(&self, address: u32) -> Option<u32> {
+        if !self.contains(address) {
+            return None;
+        }
+        self.ranges
+            .range(..=(address, usize::MAX))
+            .rev()
+            .find_map(|&(start, length)| {
+                let contains = start <= address
+                    && (start as usize)
+                        .checked_add(length)
+                        .is_none_or(|end| end > address as usize);
+                contains.then_some(start)
+            })
+    }
 }
 
 #[cfg(test)]
@@ -120,6 +136,7 @@ mod tests {
         let set = AddressSet::new();
         assert!(set.is_empty());
         assert!(!set.contains(0x13374200));
+        assert_eq!(set.start_of_range_containing(0x13374200), None);
     }
 
     #[test]
@@ -127,6 +144,7 @@ mod tests {
         let mut set = AddressSet::new();
         set.add(0x00000000, 0x100000000);
         assert!(set.contains(0x13374200));
+        assert_eq!(set.start_of_range_containing(0x13374200), Some(0x00000000));
     }
 
     #[test]
@@ -228,4 +246,25 @@ mod tests {
             assert!(!set.contains(address));
         }
     }
+
+    #[test]
+    fn should_find_start_of_range() {
+        let mut set = AddressSet::new();
+        set.add(0x13374200, 4);
+        assert_eq!(set.start_of_range_containing(0x133741ff), None);
+        for address in 0x13374200..0x13374204 {
+            assert_eq!(set.start_of_range_containing(address), Some(0x13374200));
+        }
+        assert_eq!(set.start_of_range_containing(0x13374204), None);
+    }
+
+    #[test]
+    fn should_ignore_ranges_not_containing_address() {
+        let mut set = AddressSet::new();
+        set.add(0x10000000, 1024);
+        set.add(0x30000000, 1024);
+
+        assert!(!set.contains(0x20000000));
+        assert_eq!(set.start_of_range_containing(0x20000000), None);
+    }
 }
diff --git a/src/emulator/shrooms_vb_core.rs b/src/emulator/shrooms_vb_core.rs
index 53c99de..e15ca2c 100644
--- a/src/emulator/shrooms_vb_core.rs
+++ b/src/emulator/shrooms_vb_core.rs
@@ -215,13 +215,13 @@ extern "C" fn on_read(
     // There is no way for the userdata to be null or otherwise invalid.
     let data: &mut VBState = unsafe { &mut *vb_get_user_data(sim).cast() };
 
-    if data.read_watchpoints.contains(address) {
+    if let Some(start) = data.read_watchpoints.start_of_range_containing(address) {
         let watch = if data.write_watchpoints.contains(address) {
             VBWatchpointType::Access
         } else {
             VBWatchpointType::Read
         };
-        data.stop_reason = Some(StopReason::Watchpoint(watch, address));
+        data.stop_reason = Some(StopReason::Watchpoint(watch, start));
     }
 
     // Don't stop here, the debugger expects us to break after the memory access.
@@ -242,13 +242,13 @@ extern "C" fn on_write(
     // There is no way for the userdata to be null or otherwise invalid.
     let data: &mut VBState = unsafe { &mut *vb_get_user_data(sim).cast() };
 
-    if data.write_watchpoints.contains(address) {
+    if let Some(start) = data.write_watchpoints.start_of_range_containing(address) {
         let watch = if data.read_watchpoints.contains(address) {
             VBWatchpointType::Access
         } else {
             VBWatchpointType::Write
         };
-        data.stop_reason = Some(StopReason::Watchpoint(watch, address));
+        data.stop_reason = Some(StopReason::Watchpoint(watch, start));
     }
 
     // Don't stop here, the debugger expects us to break after the memory access.