From acbddcb08fd310c88c6d99c09f25a6fd4ce11c4f Mon Sep 17 00:00:00 2001
From: Alex Janka <alex@alexjanka.com>
Date: Mon, 6 Feb 2023 21:00:56 +1100
Subject: [PATCH] back to functional

---
 src/processor/memory.rs | 51 +++++++++++++++++++++++++----------------
 1 file changed, 31 insertions(+), 20 deletions(-)

diff --git a/src/processor/memory.rs b/src/processor/memory.rs
index 91d4230..2adc5a5 100644
--- a/src/processor/memory.rs
+++ b/src/processor/memory.rs
@@ -68,12 +68,7 @@ impl Memory {
             0xFEA0..0xFF00 => {
                 return 0x0;
             }
-            0xFF00..0xFF4C => {
-                if address == 0xFF00 {
-                    return 0xFF;
-                }
-                return self.io[(address - 0xFF00) as usize];
-            }
+            0xFF00..0xFF4C => self.get_io(address),
             0xFF4C..0xFF80 => {
                 // println!("empty space 2 read");
                 return 0xFF;
@@ -114,22 +109,10 @@ impl Memory {
             0xFEA0..0xFF00 => {
                 // println!("empty space write: {:#X} to addr {:#X}", data, address);
             }
-            0xFF04 => {
-                if self.user_mode {
-                    self.io[0xFF04 - 0xFF00] = 0;
-                } else {
-                    self.io[0xFF04 - 0xFF00] = data;
-                }
-            }
-            0xFF00..0xFF04 | 0xFF05..0xFF4C => {
+            0xFF00..0xFF4C => {
+                self.set_io(address, data);
                 // verbose_print!("writing to addr {:#X}\r", address);
                 stdout().flush().unwrap();
-
-                if address == 0xFF02 && data == 0x81 {
-                    print!("{}", self.get(0xFF01) as char);
-                    stdout().flush().unwrap();
-                }
-                self.io[(address - 0xFF00) as usize] = data;
             }
             0xFF50 => {
                 self.bootrom_enabled = false;
@@ -147,4 +130,32 @@ impl Memory {
             }
         }
     }
+
+    fn get_io(&self, address: Address) -> u8 {
+        if address == 0xFF00 {
+            return 0xFF;
+        }
+        return self.io[(address - 0xFF00) as usize];
+    }
+
+    fn set_io(&mut self, address: Address, data: u8) {
+        let addr_l = (address - 0xFF00) as usize;
+        if !self.user_mode {
+            self.io[addr_l] = data;
+        } else {
+            match address {
+                0xFF02 => {
+                    if data == 0x81 {
+                        print!("{}", self.get(0xFF01) as char);
+                        stdout().flush().unwrap();
+                    }
+                }
+                0xFF04 => self.io[addr_l] = 0,
+                _ => {
+                    self.io[addr_l] = data;
+                    // panic!("passed non-io address to io handler!");
+                }
+            }
+        }
+    }
 }