Finish basic functions of MMU card

src/mmu.rs

@@ -4,7 +4,7 @@ use nullable_result::NullableResult;
 
 use crate::{
     backplane::{Backplane, CardAccessorBuilder, DMAHandler, MMUHandler},
-    card::{u16_get_be_byte, Card},
+    card::{u16_get_be_byte, u32_get_be_byte, u32_set_be_byte, Card},
     m68k::BusError,
     register,
 };
@@ -35,7 +35,10 @@ impl From<u32> for PagingEntry {
 pub struct MmuCard {
     enabled: bool,
     cache: [Option<PagingEntry>; 4096],
-    map_frames: [Option<u32>; 4],
+    map_frames: [u32; 4],
+    map_frames_enabled: [bool; 4],
+    tlb_clear_entry: u16,
+    print_debug: bool,
 }
 
 impl Card for MmuCard {
@@ -44,7 +47,10 @@ impl Card for MmuCard {
             Self {
                 enabled: false,
                 cache: [None; 4096],
-                map_frames: [None; 4],
+                map_frames: [0; 4],
+                map_frames_enabled: [false; 4],
+                tlb_clear_entry: 0,
+                print_debug: false,
             },
             None,
         ))
@@ -56,14 +62,78 @@ impl Card for MmuCard {
 
     fn read_byte_io(&mut self, address: u8) -> NullableResult<u8, BusError> {
         match address {
+            (0x0..=0xF) => {
+                let map_no = (address / 4) as usize;
+                let offset = address % 4;
+                match offset {
+                    (0..=2) => NullableResult::Ok(u32_get_be_byte(self.map_frames[map_no], offset)),
+                    3 => NullableResult::Ok(self.map_frames_enabled[map_no] as u8),
+                    _ => unreachable!(),
+                }
+            }
             (0xFE..=0xFF) => NullableResult::Ok(u16_get_be_byte(ID, address - 0xFE)),
             _ => NullableResult::Null,
         }
     }
 
+    fn write_byte_io(&mut self, address: u8, data: u8) -> NullableResult<(), BusError> {
+        match address {
+            (0x0..=0xF) => {
+                let map_no = (address / 4) as usize;
+                let offset = address % 4;
+                match offset {
+                    (0..=1) => {
+                        self.map_frames[map_no] =
+                            u32_set_be_byte(self.map_frames[map_no], offset, data);
+                    }
+                    2 => {
+                        self.map_frames[map_no] =
+                            u32_set_be_byte(self.map_frames[map_no], offset, data & 0xf0);
+                    }
+                    3 => {
+                        self.map_frames_enabled[map_no] = (data & 0x1) > 0;
+                        for i in map_no * 1024..(map_no + 1) * 1024 {
+                            self.cache[i] = None;
+                        }
+                    }
+                    _ => unreachable!(),
+                };
+            }
+            (0x10..=0x13) => {
+                let offset = (address - 0x10) % 4;
+                match offset {
+                    0 => (),
+                    1 => {
+                        self.tlb_clear_entry = (self.tlb_clear_entry & 0xF) | ((data as u16) << 4);
+                    }
+                    2 => {
+                        self.tlb_clear_entry =
+                            (self.tlb_clear_entry & 0xFF0) | (((data as u16) & 0xF0) >> 4);
+                    }
+                    3 => {
+                        if self.print_debug {
+                            println!(
+                                "Clearing TLB entry {:#x} ( page start {:#x} )",
+                                self.tlb_clear_entry,
+                                (self.tlb_clear_entry as u32) << 12
+                            );
+                        }
+                        self.cache[self.tlb_clear_entry as usize] = None;
+                    }
+                    _ => unreachable!(),
+                }
+            }
+            0x15 => {
+                self.enabled = (data & 0x1) > 0;
+            }
+            _ => (),
+        }
+        NullableResult::Ok(())
+    }
+
     fn cmd(&mut self, cmd: &[&str]) -> anyhow::Result<()> {
-        if cmd[0] == "enable" && cmd.len() >= 2 {
+        if cmd[0] == "debug" && cmd.len() >= 2 {
-            self.enabled = cmd[1].parse()?;
+            self.print_debug = cmd[1].parse()?;
         }
         Ok(())
     }
@@ -75,9 +145,33 @@ impl Card for MmuCard {
 
 impl Display for MmuCard {
     fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        let q1 = if self.map_frames_enabled[0] {
+            Some(self.map_frames[0])
+        } else {
+            None
+        };
+        let q2 = if self.map_frames_enabled[1] {
+            Some(self.map_frames[1])
+        } else {
+            None
+        };
+        let q3 = if self.map_frames_enabled[2] {
+            Some(self.map_frames[2])
+        } else {
+            None
+        };
+        let q4 = if self.map_frames_enabled[3] {
+            Some(self.map_frames[3])
+        } else {
+            None
+        };
         f.write_fmt(format_args!(
-            "MMU card, {}",
+            "MMU card, {} (Q1: {:?}, Q2: {:?}, Q3: {:?}, Q4: {:?})",
             if self.enabled { "enabled" } else { "disabled" },
+            q1,
+            q2,
+            q3,
+            q4
         ))
     }
 }
@@ -94,26 +188,59 @@ impl MMUHandler for Mmu {
         write: bool,
     ) -> NullableResult<u32, BusError> {
         let card_accessor = card_accessor.build::<MmuCard>();
-        if card_accessor.get().enabled {
+        let card = card_accessor.get();
+        let print_debug = card.print_debug;
+        if card.enabled {
             let page = address >> 12;
             let offset = address & 0xFFF;
-            let entry = if let Some(entry) = card_accessor.get().cache[page as usize] {
+            let entry = if let Some(entry) = card.cache[page as usize] {
+                if print_debug {
+                    println!("TLB hit");
+                }
                 entry
             } else {
-                let map_frame = card_accessor.get().map_frames[(page >> 10) as usize]?;
+                if print_debug {
-                let entry_address = (map_frame << 12) | ((page & 0x3FF) << 2);
+                    println!("TLB miss, fetching entry");
+                }
+                if card.map_frames_enabled[(page >> 10) as usize] == false {
+                    if print_debug {
+                        println!("No mapping frame for this quarter");
+                    }
+                    return NullableResult::Null;
+                }
+                let map_frame = card.map_frames[(page >> 10) as usize];
+                let entry_address = (map_frame) | ((page & 0x3FF) << 2);
+                if print_debug {
+                    println!("Entry is at {:#x}", entry_address);
+                }
+                drop(card);
                 let entry_hi = backplane.read_word_phys(entry_address)?;
                 let entry_lo = backplane.read_word_phys(entry_address + 2)?;
                 let entry = PagingEntry::from((entry_hi as u32) << 16 | entry_lo as u32);
                 card_accessor.get().cache[page as usize] = Some(entry);
+                if print_debug {
+                    println!("Fetched entry {:#?}", entry);
+                }
                 entry
             };
             if entry.present {
                 if write && !entry.writable {
+                    if print_debug {
+                        println!("Entry not writable");
+                    }
                     return NullableResult::Err(BusError);
                 }
+                if print_debug {
+                    println!(
+                        "Translation success, translated to {:#x}",
+                        (entry.frame << 12) | offset
+                    );
+                }
                 NullableResult::Ok((entry.frame << 12) | offset)
             } else {
+                if print_debug {
+                    println!("Entry not present");
+                }
                 NullableResult::Null
             }
         } else {