Fix sound once again :)

Oh man, that was way harder to track down that I want to admit.
Couple of hours wasted, yeeeah.

.envrc

@@ -0,0 +1 @@
+use_nix

Cargo.toml

@@ -4,6 +4,11 @@ version = "0.1.0"
 authors = ["Kevin Hamacher <kevin.hamacher@rub.de>"]
 edition = '2018'
 
+[features]
+default = ["audio"]
+audio = ["pulse-simple"]
+
 [dependencies]
 sdl2 = "*"
 libc = "*"
+pulse-simple = { version = "*", optional = true }

Readme.md

@@ -4,10 +4,15 @@ Awesome GB(C?) emulator.
 
 ## Current status
 
-All instructions are completely implemented.
-Display is able to render tiles + sprites, 8x16 sprites are implemented but untested.
-Sound isn't implemented at all.
+Pretty much all instructions are implemented.
+Display is able to render the background, the window and sprites.
+8x16 sprites are implemented but probably wrong.
 
-## Test suite
+Sound is completely missing as well was most of the GBC features.
 
-All CPU tests pass :)
+PKMN red works fine (except for minor graphic bugs).
+Tetris runs correctly.
+
+## Test suites
+
+CPU test suite passes.

shell.nix

@@ -0,0 +1,24 @@
+let
+  moz_overlay = import (builtins.fetchTarball https://github.com/mozilla/nixpkgs-mozilla/archive/master.tar.gz);
+  nixpkgs = import <nixpkgs> { overlays = [ moz_overlay ]; };
+  rustStableChannel = nixpkgs.latest.rustChannels.stable.rust.override {
+    extensions = [
+      "rust-src"
+      "rls-preview"
+      "clippy-preview"
+      "rustfmt-preview"
+    ];
+  };
+in
+  with nixpkgs;
+  stdenv.mkDerivation {
+    name = "moz_overlay_shell";
+    buildInputs = [
+      rustStableChannel
+      rls
+      rustup
+      pkg-config
+      SDL2
+      pulseaudio
+    ];
+  }

src/cartridge.rs

@@ -1,4 +1,4 @@
-use super::mbc::mbc::MBC;
+use crate::mbc;
 
 #[derive(Debug, PartialEq)]
 enum MemoryBankControllerType {
@@ -6,6 +6,7 @@ enum MemoryBankControllerType {
     MBC1,
     MBC2,
     MBC3,
+    MBC5,
     //HuC1,
 }
 
@@ -18,17 +19,18 @@ enum RamSize {
 }
 
 pub struct Cartridge {
-    mbc: Box<dyn super::mbc::mbc::MBC>,
+    mbc: Box<dyn mbc::MBC>,
     savefile: Option<String>,
 }
 
 impl Cartridge {
-    pub fn new(rom: Box<[u8]>, save_file: Option<String>) -> Cartridge {
+    pub fn new(rom: Box<[u8]>, savefile: Option<String>) -> Cartridge {
         let mbc_type: MemoryBankControllerType = match rom[0x0147] {
             0x00 | 0x08..=0x09 => MemoryBankControllerType::None,
             0x01..=0x03 => MemoryBankControllerType::MBC1,
             0x05..=0x06 => MemoryBankControllerType::MBC2,
             0x0F..=0x13 => MemoryBankControllerType::MBC3,
+            0x19..=0x1E => MemoryBankControllerType::MBC5,
             // 0xFF => MemoryBankControllerType::HuC1,
             _ => panic!("Unsupported MBC type: {:02X}", rom[0x0147]),
         };
@@ -55,23 +57,20 @@ impl Cartridge {
         println!("Rom size: {} banks", rom_banks);
         println!("Ram size: {:?}", ram_size);
 
-        let ram = Cartridge::load_savefile(&save_file, ram_size);
+        let ram = Cartridge::load_savefile(&savefile, ram_size);
 
-        let mbc: Box<dyn super::mbc::mbc::MBC> = match mbc_type {
-            MemoryBankControllerType::None => Box::new(super::mbc::mbc::NoMBC::new(rom, ram)),
-            MemoryBankControllerType::MBC1 => Box::new(super::mbc::mbc1::MBC1::new(rom, ram)),
-            MemoryBankControllerType::MBC2 => Box::new(super::mbc::mbc2::MBC2::new(rom, ram)),
-            MemoryBankControllerType::MBC3 => Box::new(super::mbc::mbc3::MBC3::new(rom, ram)),
+        let mbc: Box<dyn mbc::MBC> = match mbc_type {
+            MemoryBankControllerType::None => Box::new(mbc::NoMBC::new(rom, ram)),
+            MemoryBankControllerType::MBC1 => Box::new(mbc::MBC1::new(rom, ram)),
+            MemoryBankControllerType::MBC2 => Box::new(mbc::MBC2::new(rom, ram)),
+            MemoryBankControllerType::MBC3 => Box::new(mbc::MBC3::new(rom, ram)),
+            MemoryBankControllerType::MBC5 => Box::new(mbc::MBC5::new(rom, ram)),
         };
 
-        Cartridge {
-            mbc: mbc,
-            savefile: save_file,
-        }
+        Cartridge { mbc, savefile }
     }
 
-    fn load_savefile(save_file: &Option<String>, ram_size: RamSize) -> Box<[u8]> {
-        let old_data;
+    fn load_savefile(savefile: &Option<String>, ram_size: RamSize) -> Box<[u8]> {
         let size = match ram_size {
             RamSize::None => 0,
             RamSize::Ram2KB => 2048,
@@ -79,24 +78,27 @@ impl Cartridge {
             RamSize::Ram32KB => 16 * 2048,
         };
 
-        if let &Some(ref filename) = save_file {
+        if let Some(ref filename) = *savefile {
             let data = super::read_file(&filename);
-            if let Ok(success) = data {
-                old_data = success
+            if let Ok(data) = data {
+                if data.len() != size {
+                    panic!("Ram size does not match");
+                }
+                data
             } else {
-                old_data = vec![0u8; size].into_boxed_slice();
+                // Generate empty buffer
+                println!("Warning: Could not open save file");
+                vec![0u8; size].into_boxed_slice()
             }
         } else {
-            old_data = vec![0u8; size].into_boxed_slice();
+            vec![0u8; size].into_boxed_slice()
         }
-
-        old_data
     }
 
     pub fn save(&self) {
-        if let &Some(ref fil) = &self.savefile {
+        if let Some(ref fil) = self.savefile {
+            println!("Updating RAM dump");
             self.mbc.dump_ram(fil);
-            println!("Ram dump updated.")
         } else {
             println!("Did not update ram dump");
         }

src/cpu.rs

@@ -1,5 +1,6 @@
-use super::interconnect;
+use crate::interconnect;
 
+use std::convert::TryFrom;
 use std::thread::sleep;
 use std::time::{Duration, Instant};
 
@@ -14,13 +15,19 @@ const REG_N_L: usize = 5;
 const REG_N_HL: usize = 6;
 const REG_N_A: usize = 7;
 const REG_N_F: usize = 8;
-const REG_NAMES: [&'static str; 9] = ["B", "C", "D", "E", "H", "L", "(HL)", "A", "F"];
+const REG_NAMES: [&str; 9] = ["B", "C", "D", "E", "H", "L", "(HL)", "A", "F"];
 
 const FLAG_Z: u8 = 1 << 7;
 const FLAG_N: u8 = 1 << 6;
 const FLAG_H: u8 = 1 << 5;
 const FLAG_C: u8 = 1 << 4;
 
+use interconnect::TickResult;
+pub enum CpuTickResult {
+    Continue { cycles_executed: usize },
+    Shutdown { cycles_executed: usize },
+}
+
 pub struct CPU {
     // Registers: B, C, D, E, H, L, A
     regs: [u8; 7],
@@ -34,10 +41,35 @@ pub struct CPU {
     debug: bool,
 
     halted: bool,
+
+    trigger_once: bool,
 }
 
-fn to_u16(bytes: Box<[u8]>) -> u16 {
-    (bytes[1] as u16) << 8 | (bytes[0] as u16)
+enum Args {
+    Single(u8),
+    Double(u8, u8),
+}
+
+impl TryFrom<Args> for u8 {
+    type Error = ();
+
+    fn try_from(val: Args) -> Result<u8, Self::Error> {
+        match val {
+            Args::Single(x) => Ok(x),
+            _ => Err(()),
+        }
+    }
+}
+
+impl TryFrom<Args> for u16 {
+    type Error = ();
+
+    fn try_from(val: Args) -> Result<u16, Self::Error> {
+        match val {
+            Args::Double(a, b) => Ok((a as u16) | ((b as u16) << 8)),
+            _ => Err(()),
+        }
+    }
 }
 
 impl CPU {
@@ -47,29 +79,37 @@ impl CPU {
             regs: [0, 0, 0, 0, 0, 0, 0],
             ip: 0,
             sp: 0xFFFE,
-            interconnect: interconnect,
+            interconnect,
             ime: false,
             debug: false,
             halted: false,
+
+            trigger_once: false,
         }
     }
 
-    #[inline]
     fn read_byte(&self, addr: u16) -> u8 {
         self.interconnect.read_byte(addr)
     }
 
-    #[inline]
-    fn load_args(&mut self, num_args: u8) -> Box<[u8]> {
-        let mut args = Vec::new();
-        for i in 0..num_args {
-            args.push(self.read_byte(self.ip + i as u16));
+    fn load_args(&mut self, num_args: u8) -> Args {
+        match num_args {
+            1 => {
+                let val = self.read_byte(self.ip);
+                self.ip += 1;
+                Args::Single(val)
+            }
+            2 => {
+                let b1 = self.read_byte(self.ip);
+                self.ip += 1;
+                let b2 = self.read_byte(self.ip);
+                self.ip += 1;
+                Args::Double(b1, b2)
+            }
+            _ => panic!("load_args only supports two bytes"),
         }
-        self.ip += num_args as u16;
-        args.into_boxed_slice()
     }
 
-    #[inline]
     fn set_8bit_reg(&mut self, reg_id: usize, value: u8) {
         // Make sure that we skip the (HL) part.
         if reg_id == REG_N_A {
@@ -84,7 +124,6 @@ impl CPU {
         }
     }
 
-    #[inline]
     fn get_8bit_reg(&self, reg_id: usize) -> u8 {
         // Make sure that we skip the (HL) part.
         if reg_id == REG_N_A {
@@ -99,17 +138,10 @@ impl CPU {
         }
     }
 
-    #[inline]
     fn adc_r(&mut self, val: u8) {
-        // Passes the test
         let old: u8 = self.regs[REG_A];
         let mut new: u8 = old;
-        let c: u8;
-        if self.flags & FLAG_C == FLAG_C {
-            c = 1;
-        } else {
-            c = 0;
-        }
+        let c = if self.flags & FLAG_C == FLAG_C { 1 } else { 0 };
         new = new.wrapping_add(val);
         new = new.wrapping_add(c);
         self.regs[REG_A] = new;
@@ -120,9 +152,7 @@ impl CPU {
         self.set_clear_flag(FLAG_H, ((old & 0x0F) + (val & 0x0F) + c) > 0x0F);
     }
 
-    #[inline]
     fn add_r(&mut self, val: u8) {
-        // Passes the test
         let old: u8 = self.regs[REG_A];
         let new: u8 = old.wrapping_add(val);
         let carry: u16 = (old as u16) ^ (val as u16) ^ (new as u16);
@@ -134,17 +164,10 @@ impl CPU {
         self.set_clear_flag(FLAG_H, carry & 0x10 == 0x10);
     }
 
-    #[inline]
     fn sbc_r(&mut self, val: u8) {
-        // Passes the test
         let old: u8 = self.regs[REG_A];
         let mut new: u8 = old as u8;
-        let c: u8;
-        if self.flags & FLAG_C == FLAG_C {
-            c = 1;
-        } else {
-            c = 0;
-        }
+        let c = if self.flags & FLAG_C == FLAG_C { 1 } else { 0 };
         new = new.wrapping_sub(val);
         new = new.wrapping_sub(c);
 
@@ -159,9 +182,7 @@ impl CPU {
         );
     }
 
-    #[inline]
     fn sub_r(&mut self, val: u8) {
-        // Passes the test
         let old: u8 = self.regs[REG_A];
         let new: u8 = old.wrapping_sub(val);
         let carry: u16 = (old as u16) ^ (val as u16) ^ (new as u16);
@@ -173,9 +194,7 @@ impl CPU {
         self.set_clear_flag(FLAG_H, carry & 0x10 == 0x10);
     }
 
-    #[inline]
     fn cp_r(&mut self, val: u8) {
-        // Passes the test
         let old: u8 = self.regs[REG_A];
         let new: u8 = old.wrapping_sub(val);
         let carry: u16 = (old as u16) ^ (val as u16) ^ (new as u16);
@@ -186,7 +205,6 @@ impl CPU {
         self.set_clear_flag(FLAG_H, carry & 0x10 == 0x10);
     }
 
-    #[inline]
     fn run_prefix_instruction(&mut self) {
         let instruction = self.read_byte(self.ip);
         self.ip += 1;
@@ -199,12 +217,11 @@ impl CPU {
                     println!("RLC {}", REG_NAMES[reg_id]);
                 }
 
-                let nval: u8;
-                if val & 0x80 == 0x80 {
-                    nval = val << 1 | 1;
+                let nval = if val & 0x80 == 0x80 {
+                    val << 1 | 1
                 } else {
-                    nval = val << 1;
-                }
+                    val << 1
+                };
                 self.set_8bit_reg(reg_id, nval);
 
                 self.set_clear_flag(FLAG_C, val & 0x80 == 0x80);
@@ -238,12 +255,7 @@ impl CPU {
                 }
 
                 let carry = self.flags & FLAG_C > 0;
-                let nval: u8;
-                if !carry {
-                    nval = val << 1;
-                } else {
-                    nval = val << 1 | 1;
-                }
+                let nval = if !carry { val << 1 } else { val << 1 | 1 };
                 self.set_8bit_reg(reg_id, nval);
                 self.set_clear_flag(FLAG_C, val & 0x80 == 0x80);
                 self.set_clear_flag(FLAG_Z, nval == 0);
@@ -259,12 +271,7 @@ impl CPU {
                 }
 
                 let carry = self.flags & FLAG_C > 0;
-                let v: u8;
-                if !carry {
-                    v = val >> 1;
-                } else {
-                    v = (val >> 1) | 0x80;
-                }
+                let v = if !carry { val >> 1 } else { (val >> 1) | 0x80 };
                 self.set_8bit_reg(reg_id, v);
                 self.set_clear_flag(FLAG_C, val & 1 == 1);
                 self.set_clear_flag(FLAG_Z, v == 0);
@@ -554,24 +561,18 @@ impl CPU {
                 }
                 self.set_8bit_reg(reg_id, reg_content | (1 << 7));
             }
-            _ => {
-                panic!("Unsupported prefix instruction: {:x}", instruction);
-            }
         }
     }
 
-    #[inline]
     fn get_pair_value(&self, a: usize, b: usize) -> u16 {
         (self.get_8bit_reg(a) as u16) << 8 | self.get_8bit_reg(b) as u16
     }
 
-    #[inline]
     fn set_pair_value(&mut self, a: usize, b: usize, value: u16) {
         self.set_8bit_reg(a, (value >> 8) as u8);
         self.set_8bit_reg(b, value as u8);
     }
 
-    #[inline]
     fn call(&mut self, dst: u16) {
         let ip = self.ip;
         self.push(ip);
@@ -579,9 +580,8 @@ impl CPU {
         self.ip = dst;
     }
 
-    #[inline]
-    fn call_condition(&mut self, cond_str: String, cond: bool) -> u8 {
-        let dst = to_u16(self.load_args(2));
+    fn call_condition(&mut self, cond_str: &'static str, cond: bool) -> u8 {
+        let dst = u16::try_from(self.load_args(2)).unwrap();
         if self.debug {
             println!("CALL {} {:04X}", cond_str, dst);
         }
@@ -593,8 +593,7 @@ impl CPU {
         }
     }
 
-    #[inline]
-    fn ret_condition(&mut self, cond_str: String, cond: bool) -> u8 {
+    fn ret_condition(&mut self, cond_str: &'static str, cond: bool) -> u8 {
         if self.debug {
             println!("RET {}", cond_str);
         }
@@ -606,7 +605,6 @@ impl CPU {
         }
     }
 
-    #[inline]
     fn jmp_r(&mut self, addr: u8) {
         let off: i8 = addr as i8;
         if off < 0 {
@@ -616,9 +614,8 @@ impl CPU {
         }
     }
 
-    #[inline]
-    fn jmp_r_condition(&mut self, cond_str: String, cond: bool) -> u8 {
-        let t = self.load_args(1)[0];
+    fn jmp_r_condition(&mut self, cond_str: &'static str, cond: bool) -> u8 {
+        let t = u8::try_from(self.load_args(1)).unwrap();
         if self.debug {
             println!("JR {} {:02X}", cond_str, t);
         }
@@ -630,14 +627,12 @@ impl CPU {
         }
     }
 
-    #[inline]
     fn jmp_p(&mut self, addr: u16) {
         self.ip = addr;
     }
 
-    #[inline]
-    fn jmp_p_condition(&mut self, cond_str: String, cond: bool) -> u8 {
-        let t = to_u16(self.load_args(2));
+    fn jmp_p_condition(&mut self, cond_str: &'static str, cond: bool) -> u8 {
+        let t = u16::try_from(self.load_args(2)).unwrap();
         if self.debug {
             println!("JP {} {:04X}", cond_str, t);
         }
@@ -649,7 +644,6 @@ impl CPU {
         }
     }
 
-    #[inline]
     fn rst(&mut self, val: u8) -> u8 {
         // Make sure this is correct.
         if self.debug {
@@ -659,7 +653,6 @@ impl CPU {
         16
     }
 
-    #[inline]
     fn pop_rr(&mut self, r1: usize, r2: usize) -> u8 {
         if self.debug {
             println!("POP {}{}", REG_NAMES[r1], REG_NAMES[r2]);
@@ -669,7 +662,6 @@ impl CPU {
         12
     }
 
-    #[inline]
     fn push_rr(&mut self, r1: usize, r2: usize) -> u8 {
         if self.debug {
             println!("PUSH {}{}", REG_NAMES[r1], REG_NAMES[r2]);
@@ -679,7 +671,6 @@ impl CPU {
         16
     }
 
-    #[inline]
     fn dec_rr(&mut self, r1: usize, r2: usize) -> u8 {
         if self.debug {
             println!("DEC {}{}", REG_NAMES[r1], REG_NAMES[r2]);
@@ -690,7 +681,6 @@ impl CPU {
         8
     }
 
-    #[inline]
     fn inc_rr(&mut self, r1: usize, r2: usize) -> u8 {
         if self.debug {
             println!("INC {}{}", REG_NAMES[r1], REG_NAMES[r2]);
@@ -701,20 +691,17 @@ impl CPU {
         8
     }
 
-    #[inline]
     fn push(&mut self, val: u16) {
         self.interconnect.write_word(self.sp - 2, val);
         self.sp -= 2;
     }
 
-    #[inline]
     fn pop(&mut self) -> u16 {
         let v: u16 = self.interconnect.read_word(self.sp);
         self.sp += 2;
         v
     }
 
-    #[inline]
     fn ld_r_r(&mut self, reg_dst: usize, reg_src: usize) -> u8 {
         if self.debug {
             println!("LD {}, {}", REG_NAMES[reg_dst], REG_NAMES[reg_src])
@@ -728,9 +715,8 @@ impl CPU {
         }
     }
 
-    #[inline]
     fn ld_r_v(&mut self, r: usize) -> u8 {
-        let val: u8 = self.load_args(1)[0];
+        let val = u8::try_from(self.load_args(1)).unwrap();
         if self.debug {
             println!("LD {}, {:02X}", REG_NAMES[r], val);
         }
@@ -742,9 +728,8 @@ impl CPU {
         }
     }
 
-    #[inline]
     fn ld_rr_vv(&mut self, r1: usize, r2: usize) -> u8 {
-        let val: u16 = to_u16(self.load_args(2));
+        let val = u16::try_from(self.load_args(2)).unwrap();
         if self.debug {
             println!("LD {}{}, {:04X}", REG_NAMES[r1], REG_NAMES[r2], val);
         }
@@ -752,7 +737,6 @@ impl CPU {
         12
     }
 
-    #[inline]
     fn reg_inc(&mut self, reg_id: usize) -> u8 {
         if self.debug {
             println!("INC {}", REG_NAMES[reg_id]);
@@ -766,7 +750,6 @@ impl CPU {
         4
     }
 
-    #[inline]
     fn add_rr_rr(&mut self, r1: usize, r2: usize, r3: usize, r4: usize) -> u8 {
         if self.debug {
             println!(
@@ -784,12 +767,11 @@ impl CPU {
         self.clear_flag(FLAG_N);
 
         // Some magic formula
-        self.set_clear_flag(FLAG_C, val1 as usize + val2 as usize & 0x10000 == 0x10000);
+        self.set_clear_flag(FLAG_C, (val1 as usize + val2 as usize) & 0x10000 == 0x10000);
         self.set_clear_flag(FLAG_H, (val1 ^ val2 ^ res) & 0x1000 == 0x1000);
         8
     }
 
-    #[inline]
     fn reg_dec(&mut self, reg_id: usize) -> u8 {
         if self.debug {
             println!("DEC {}", REG_NAMES[reg_id]);
@@ -805,7 +787,6 @@ impl CPU {
         4
     }
 
-    #[inline]
     fn ld_dref_rr_a(&mut self, r1: usize, r2: usize) -> u8 {
         if self.debug {
             println!("LD ({}{}), A", REG_NAMES[r1], REG_NAMES[r2]);
@@ -816,17 +797,14 @@ impl CPU {
         8
     }
 
-    #[inline]
     fn set_flag(&mut self, flag: u8) {
         self.flags |= flag;
     }
 
-    #[inline]
     fn clear_flag(&mut self, flag: u8) {
         self.flags &= !flag;
     }
 
-    #[inline]
     fn set_clear_flag(&mut self, flag: u8, dep: bool) {
         if dep {
             self.set_flag(flag);
@@ -835,7 +813,6 @@ impl CPU {
         }
     }
 
-    #[inline]
     fn int_(&mut self, val: u8) {
         if self.debug {
             println!("INT {:02X}", val);
@@ -845,21 +822,18 @@ impl CPU {
         self.call(val as u16);
     }
 
-    #[inline]
     fn ret(&mut self) {
         let new_ip: u16 = self.pop();
         //self.dump_stack();
         self.ip = new_ip;
     }
 
-    #[inline]
     fn reti(&mut self) -> u8 {
         self.ret();
         self.ime = true;
         16
     }
 
-    #[inline]
     fn handle_interrupt(&mut self, offset: u8, flag: u8) {
         // Remove interrupt requested flag
         let new_flag = self.interconnect.read_byte(0xFF0F) & !flag;
@@ -870,23 +844,27 @@ impl CPU {
         self.halted = false;
     }
 
+    #[allow(unused_variables)]
     pub fn run(&mut self) {
+        let start = Instant::now();
+        let mut cycles_executed = 0u128;
         loop {
-            let mut cycles: i32 = 0;
-            let start = Instant::now();
-            for _ in 0..1000 {
-                cycles += self.run_instruction() as i32;
-            }
-
-            let gb_dur = cycles * 238;
-            let our_dur = start.elapsed().subsec_nanos() as i32;
-            let delta = gb_dur - our_dur;
-            if delta > (20 * 238) {
-                // We're at least 20 cycles faster.
-                sleep(Duration::new(0, delta as u32));
-            } else if delta < 0 {
-                print!("-");
+            // Calculate the amount of cycles we should've executed by now.
+            // The gameboy has a freq of 4.194304MHz
+            // This means it takes it 238.418569ns to execute a single
+            // cycle.
+            let expected_cycles = start.elapsed().as_nanos() / (238 / 2);
+            while cycles_executed < expected_cycles {
+                if let CpuTickResult::Continue {
+                    cycles_executed: x, ..
+                } = self.run_instruction()
+                {
+                    cycles_executed += x as u128;
+                } else {
+                    return;
+                }
             }
+            sleep(Duration::new(0, 10 * 238));
         }
     }
 
@@ -896,6 +874,8 @@ impl CPU {
         let enabled = self.interconnect.read_byte(0xFFFF);
         let e_pending = pending & enabled;
 
+        // Handling interrupts. Do only execute the one with the highest
+        // priority
         if e_pending & interconnect::INTERRUPT_DISPLAY_VBLANK > 0 {
             if execute {
                 self.handle_interrupt(0x40, interconnect::INTERRUPT_DISPLAY_VBLANK);
@@ -927,31 +907,58 @@ impl CPU {
         false
     }
 
-    pub fn run_instruction(&mut self) -> u8 {
+    pub fn run_instruction(&mut self) -> CpuTickResult {
         // self.debug = !self.interconnect.is_boot_rom();
+        if !self.trigger_once && !self.interconnect.is_boot_rom() {
+            self.trigger_once = true;
+            self.regs[REG_A] = 0x11;
+            println!("Patching reg");
+        }
+
+        /*
+        if self.ip >= 100 && self.ip < 120 {
+            self.debug = true;
+        } else {
+            self.debug = false;
+        }
+        */
+
+        // self.debug = true;
         // Check for interrupts.
         if self.ime {
             self.check_interrupts(true);
-        } else if self.halted {
-            if self.check_interrupts(false) {
-                self.halted = false;
-            }
+        } else if self.halted && self.check_interrupts(false) {
+            self.halted = false;
         }
 
-        let mut cycles: u8 = 1;
+        let mut cycles: u8 = 255;
         let instruction: u8;
         if !self.halted {
             // We need to double-check the flags
             instruction = self.read_byte(self.ip);
             if self.debug {
                 print!(
-                    "{:#06x}: [SP: {:#04X}] i={:02X}. ",
+                    "{:#06X}: [SP: {:#04X}] i={:02X}. ",
                     &self.ip, &self.sp, &instruction
                 );
-                for i in 0..6 {
-                    print!("{}: {:02X} ", REG_NAMES[i], self.get_8bit_reg(i));
+                /*
+                for (idx, reg) in REG_NAMES.iter().enumerate() {
+                    print!("{}: {:02X} ", reg, self.get_8bit_reg(idx));
                 }
                 print!("A: {:02X}  ", self.regs[REG_A]);
+                */
+
+                print!(
+                    "AF={:02X}{:02X} BC={:02X}{:02X} DE={:02X}{:02X} HL={:02X}{:02X}  ",
+                    self.get_8bit_reg(REG_N_A),
+                    self.get_8bit_reg(REG_N_F),
+                    self.get_8bit_reg(REG_N_B),
+                    self.get_8bit_reg(REG_N_C),
+                    self.get_8bit_reg(REG_N_D),
+                    self.get_8bit_reg(REG_N_E),
+                    self.get_8bit_reg(REG_N_H),
+                    self.get_8bit_reg(REG_N_L)
+                );
                 print!("I: {:02X}  ", self.interconnect.read_byte(0xFFFF));
 
                 // Flags
@@ -984,9 +991,10 @@ impl CPU {
                     let carry = val & 0x80 == 0x80;
                     self.set_clear_flag(FLAG_C, carry);
                     if !carry {
-                        self.regs[REG_A] = self.regs[REG_A] << 1;
+                        self.regs[REG_A] <<= 1;
                     } else {
-                        self.regs[REG_A] = self.regs[REG_A] << 1 | 1;
+                        self.regs[REG_A] <<= 1;
+                        self.regs[REG_A] |= 1;
                     }
                     self.clear_flag(FLAG_Z);
                     self.clear_flag(FLAG_N);
@@ -994,7 +1002,7 @@ impl CPU {
                     4
                 }
                 0x08 => {
-                    let a: u16 = to_u16(self.load_args(2));
+                    let a = u16::try_from(self.load_args(2)).unwrap();
                     if self.debug {
                         println!("LD ({:04X}), sp", a);
                     }
@@ -1022,9 +1030,10 @@ impl CPU {
                     let val = self.regs[REG_A];
                     self.set_clear_flag(FLAG_C, val & 1 == 1);
                     if val & 1 == 0 {
-                        self.regs[REG_A] = self.regs[REG_A] >> 1;
+                        self.regs[REG_A] >>= 1;
                     } else {
-                        self.regs[REG_A] = self.regs[REG_A] >> 1 | 0x80;
+                        self.regs[REG_A] >>= 1;
+                        self.regs[REG_A] |= 0x80;
                     }
                     self.clear_flag(FLAG_Z);
                     self.clear_flag(FLAG_N);
@@ -1033,7 +1042,10 @@ impl CPU {
                 }
 
                 0x10 => {
-                    println!("STOP 0 {:02X} not implemented.", self.load_args(1)[0]);
+                    println!(
+                        "STOP 0 {:02X} not implemented.",
+                        u8::try_from(self.load_args(1)).unwrap()
+                    );
                     4
                 }
                 0x11 => self.ld_rr_vv(REG_N_D, REG_N_E),
@@ -1050,9 +1062,10 @@ impl CPU {
                     let val = self.regs[REG_A];
                     self.set_clear_flag(FLAG_C, val & 0x80 == 0x80);
                     if !carry {
-                        self.regs[REG_A] = self.regs[REG_A] << 1;
+                        self.regs[REG_A] <<= 1;
                     } else {
-                        self.regs[REG_A] = self.regs[REG_A] << 1 | 1;
+                        self.regs[REG_A] <<= 1;
+                        self.regs[REG_A] |= 1;
                     }
                     self.clear_flag(FLAG_Z);
                     self.clear_flag(FLAG_N);
@@ -1060,8 +1073,11 @@ impl CPU {
                     4
                 }
                 0x18 => {
-                    let dst = self.load_args(1)[0];
+                    let dst = u8::try_from(self.load_args(1)).unwrap();
                     self.jmp_r(dst);
+                    if self.debug {
+                        println!("JMPR {:02X}", dst);
+                    }
                     12
                 }
                 0x19 => self.add_rr_rr(REG_N_H, REG_N_L, REG_N_D, REG_N_E),
@@ -1086,9 +1102,10 @@ impl CPU {
                     let val = self.regs[REG_A];
                     self.set_clear_flag(FLAG_C, val & 1 == 1);
                     if !carry {
-                        self.regs[REG_A] = self.regs[REG_A] >> 1;
+                        self.regs[REG_A] >>= 1;
                     } else {
-                        self.regs[REG_A] = self.regs[REG_A] >> 1 | 0x80;
+                        self.regs[REG_A] >>= 1;
+                        self.regs[REG_A] |= 0x80;
                     }
                     self.clear_flag(FLAG_Z);
                     self.clear_flag(FLAG_N);
@@ -1098,7 +1115,7 @@ impl CPU {
 
                 0x20 => {
                     let c = self.flags & FLAG_Z == 0;
-                    self.jmp_r_condition("NZ".to_owned(), c)
+                    self.jmp_r_condition("NZ", c)
                 }
                 0x21 => self.ld_rr_vv(REG_N_H, REG_N_L),
                 0x22 => {
@@ -1154,7 +1171,7 @@ impl CPU {
                 }
                 0x28 => {
                     let c = self.flags & FLAG_Z == FLAG_Z;
-                    self.jmp_r_condition("Z".to_owned(), c)
+                    self.jmp_r_condition("Z", c)
                 }
                 0x29 => self.add_rr_rr(REG_N_H, REG_N_L, REG_N_H, REG_N_L),
                 0x2A => {
@@ -1182,11 +1199,11 @@ impl CPU {
 
                 0x30 => {
                     let c = self.flags & FLAG_C == 0;
-                    self.jmp_r_condition("NC".to_owned(), c)
+                    self.jmp_r_condition("NC", c)
                 }
                 0x31 => {
                     let args = self.load_args(2);
-                    self.sp = to_u16(args);
+                    self.sp = u16::try_from(args).unwrap();
                     if self.debug {
                         println!("LD SP, {:04x}", self.sp);
                     }
@@ -1223,7 +1240,7 @@ impl CPU {
                 }
                 0x38 => {
                     let c = self.flags & FLAG_C == FLAG_C;
-                    self.jmp_r_condition("C".to_owned(), c)
+                    self.jmp_r_condition("C", c)
                 }
                 0x39 => {
                     if self.debug {
@@ -1395,15 +1412,15 @@ impl CPU {
 
                 0xC0 => {
                     let c = self.flags & FLAG_Z == 0;
-                    self.ret_condition("NZ".to_owned(), c)
+                    self.ret_condition("NZ", c)
                 }
                 0xC1 => self.pop_rr(REG_N_B, REG_N_C),
                 0xC2 => {
                     let c = self.flags & FLAG_Z == 0;
-                    self.jmp_p_condition("NZ".to_owned(), c)
+                    self.jmp_p_condition("NZ", c)
                 }
                 0xC3 => {
-                    let dst = to_u16(self.load_args(2));
+                    let dst = u16::try_from(self.load_args(2)).unwrap();
                     if self.debug {
                         println!("JMP {:04X}", dst);
                     }
@@ -1412,11 +1429,11 @@ impl CPU {
                 }
                 0xC4 => {
                     let c = self.flags & FLAG_Z == 0;
-                    self.call_condition("NZ".to_owned(), c)
+                    self.call_condition("NZ", c)
                 }
                 0xC5 => self.push_rr(REG_N_B, REG_N_C),
                 0xC6 => {
-                    let val = self.load_args(1)[0];
+                    let val = u8::try_from(self.load_args(1)).unwrap();
                     if self.debug {
                         println!("ADD A, {:02X}", val);
                     }
@@ -1428,7 +1445,7 @@ impl CPU {
                 0xC7 => self.rst(0x00),
                 0xC8 => {
                     let c = self.flags & FLAG_Z == FLAG_Z;
-                    self.ret_condition("Z".to_owned(), c)
+                    self.ret_condition("Z", c)
                 }
                 0xC9 => {
                     if self.debug {
@@ -1439,7 +1456,7 @@ impl CPU {
                 }
                 0xCA => {
                     let c = self.flags & FLAG_Z == FLAG_Z;
-                    self.jmp_p_condition("Z".to_owned(), c)
+                    self.jmp_p_condition("Z", c)
                 }
                 0xCB => {
                     self.run_prefix_instruction();
@@ -1447,11 +1464,11 @@ impl CPU {
                 }
                 0xCC => {
                     let c = self.flags & FLAG_Z == FLAG_Z;
-                    self.call_condition("Z".to_owned(), c)
+                    self.call_condition("Z", c)
                 }
-                0xCD => self.call_condition("".to_owned(), true),
+                0xCD => self.call_condition("", true),
                 0xCE => {
-                    let arg = self.load_args(1)[0];
+                    let arg = u8::try_from(self.load_args(1)).unwrap();
                     if self.debug {
                         println!("ADC A, {:02X}", arg);
                     }
@@ -1462,21 +1479,21 @@ impl CPU {
 
                 0xD0 => {
                     let c = self.flags & FLAG_C == 0;
-                    self.ret_condition("NC".to_owned(), c)
+                    self.ret_condition("NC", c)
                 }
                 0xD1 => self.pop_rr(REG_N_D, REG_N_E),
                 0xD2 => {
                     let c = self.flags & FLAG_C == 0;
-                    self.jmp_p_condition("NC".to_owned(), c)
+                    self.jmp_p_condition("NC", c)
                 }
                 0xD3 => panic!("NON-EXISTING OPCODE"),
                 0xD4 => {
                     let c = self.flags & FLAG_C == 0;
-                    self.call_condition("NC".to_owned(), c)
+                    self.call_condition("NC", c)
                 }
                 0xD5 => self.push_rr(REG_N_D, REG_N_E),
                 0xD6 => {
-                    let val = self.load_args(1)[0];
+                    let val = u8::try_from(self.load_args(1)).unwrap();
                     if self.debug {
                         println!("SUB {:02X}", val);
                     }
@@ -1487,7 +1504,7 @@ impl CPU {
                 0xD7 => self.rst(0x10),
                 0xD8 => {
                     let c = self.flags & FLAG_C == FLAG_C;
-                    self.ret_condition("C".to_owned(), c)
+                    self.ret_condition("C", c)
                 }
                 0xD9 => {
                     if self.debug {
@@ -1497,16 +1514,16 @@ impl CPU {
                 }
                 0xDA => {
                     let c = self.flags & FLAG_C == FLAG_C;
-                    self.jmp_p_condition("C".to_owned(), c)
+                    self.jmp_p_condition("C", c)
                 }
                 0xDB => panic!("NON-EXISTING OPCODE"),
                 0xDC => {
                     let c = self.flags & FLAG_C == FLAG_C;
-                    self.call_condition("C".to_owned(), c)
+                    self.call_condition("C", c)
                 }
                 0xDD => panic!("NON-EXISTING OPCODE"),
                 0xDE => {
-                    let arg = self.load_args(1)[0];
+                    let arg = u8::try_from(self.load_args(1)).unwrap();
                     if self.debug {
                         println!("SBC {:02X}", arg);
                     }
@@ -1516,12 +1533,12 @@ impl CPU {
                 0xDF => self.rst(0x18),
 
                 0xE0 => {
-                    let args = self.load_args(1);
+                    let arg = u8::try_from(self.load_args(1)).unwrap();
                     if self.debug {
-                        println!("LDH {:02X}, A", args[0]);
+                        println!("LDH {:02X}, A", arg);
                     }
                     self.interconnect
-                        .write_byte(0xFF00 + args[0] as u16, self.regs[REG_A]);
+                        .write_byte(0xFF00 + arg as u16, self.regs[REG_A]);
                     12
                 }
                 0xE1 => self.pop_rr(REG_N_H, REG_N_L),
@@ -1536,7 +1553,7 @@ impl CPU {
                 0xE3 | 0xE4 => panic!("NON-EXISTING OPCODE"),
                 0xE5 => self.push_rr(REG_N_H, REG_N_L),
                 0xE6 => {
-                    let val = self.load_args(1)[0];
+                    let val = u8::try_from(self.load_args(1)).unwrap();
                     if self.debug {
                         println!("AND {:02X}", val);
                     }
@@ -1551,16 +1568,15 @@ impl CPU {
                 }
                 0xE7 => self.rst(0x20),
                 0xE8 => {
-                    let arg = self.load_args(1)[0] as i8;
+                    let arg = u8::try_from(self.load_args(1)).unwrap() as i8;
                     if self.debug {
                         println!("ADD SP, {:02X}", arg);
                     }
-                    let t: u16;
-                    if arg > 0 {
-                        t = self.sp.wrapping_add(arg as u16);
+                    let t = if arg > 0 {
+                        self.sp.wrapping_add(arg as u16)
                     } else {
-                        t = self.sp.wrapping_sub((-arg) as u16);
-                    }
+                        self.sp.wrapping_sub((-arg) as u16)
+                    };
                     let sp = self.sp;
                     self.clear_flag(FLAG_N);
                     self.clear_flag(FLAG_Z);
@@ -1577,7 +1593,7 @@ impl CPU {
                     4
                 }
                 0xEA => {
-                    let addr = to_u16(self.load_args(2));
+                    let addr = u16::try_from(self.load_args(2)).unwrap();
                     if self.debug {
                         println!("LD ({:04X}), A", addr);
                     }
@@ -1586,7 +1602,7 @@ impl CPU {
                 }
                 0xEB..=0xED => panic!("NON-EXISTING OPCODE"),
                 0xEE => {
-                    let arg = self.load_args(1)[0];
+                    let arg = u8::try_from(self.load_args(1)).unwrap();
                     if self.debug {
                         println!("XOR {:02X}", arg);
                     }
@@ -1601,11 +1617,11 @@ impl CPU {
                 0xEF => self.rst(0x28),
 
                 0xF0 => {
-                    let args = self.load_args(1);
+                    let arg = u8::try_from(self.load_args(1)).unwrap();
                     if self.debug {
-                        println!("LDH A, {:02X}", args[0]);
+                        println!("LDH A, {:02X}", arg);
                     }
-                    self.regs[REG_A] = self.interconnect.read_byte(0xFF00 + args[0] as u16);
+                    self.regs[REG_A] = self.interconnect.read_byte(0xFF00 + arg as u16);
                     12
                 }
                 0xF1 => self.pop_rr(REG_N_A, REG_N_F),
@@ -1627,7 +1643,7 @@ impl CPU {
                 0xF4 => panic!("NON-EXISTING OPCODE"),
                 0xF5 => self.push_rr(REG_N_A, REG_N_F),
                 0xF6 => {
-                    let val = self.load_args(1)[0];
+                    let val = u8::try_from(self.load_args(1)).unwrap();
                     if self.debug {
                         println!("OR {:02X}", val);
                     }
@@ -1641,7 +1657,7 @@ impl CPU {
                 }
                 0xF7 => self.rst(0x30),
                 0xF8 => {
-                    let arg = self.load_args(1)[0] as i8;
+                    let arg = u8::try_from(self.load_args(1)).unwrap() as i8;
                     if self.debug {
                         println!("LD HL, SP+{:02X}", arg);
                     }
@@ -1671,7 +1687,7 @@ impl CPU {
                     8
                 }
                 0xFA => {
-                    let addr = to_u16(self.load_args(2));
+                    let addr = u16::try_from(self.load_args(2)).unwrap();
                     if self.debug {
                         println!("LD A, ({:04X})", addr);
                     }
@@ -1679,7 +1695,6 @@ impl CPU {
                     16
                 }
                 0xFB => {
-                    // Enable interrupts - TODO
                     if self.debug {
                         println!("EI");
                     }
@@ -1689,19 +1704,25 @@ impl CPU {
                 0xFC | 0xFD => panic!("NON-EXISTING OPCODE"),
 
                 0xFE => {
-                    let args = self.load_args(1);
+                    let arg = u8::try_from(self.load_args(1)).unwrap();
                     if self.debug {
-                        println!("CP {:02X}", args[0]);
+                        println!("CP {:02X}", arg);
                     }
 
-                    self.cp_r(args[0]);
+                    self.cp_r(arg);
                     8
                 }
                 0xFF => self.rst(0x38),
-                _ => panic!("Unknown instruction: {:02x}", instruction),
             };
         }
-        self.interconnect.tick(cycles);
-        cycles
+
+        if self.interconnect.tick(cycles) == TickResult::Shutdown {
+            return CpuTickResult::Shutdown {
+                cycles_executed: cycles as usize,
+            };
+        }
+        CpuTickResult::Continue {
+            cycles_executed: cycles as usize,
+        }
     }
 }

src/display/mod.rs

@@ -1,6 +1,13 @@
 extern crate libc;
 extern crate sdl2;
 
+mod palette;
+mod structs;
+use palette::{CgbPalette, DmgPalette};
+use structs::*;
+
+use std::io::Write;
+
 // Internal ram size
 const VRAM_SIZE: usize = 0x2000;
 
@@ -41,32 +48,8 @@ const STAT_MODE_HBLANK_INT: u8 = 1 << 3;
 const SPRITE_OBJ_BG_PRIORITY: u8 = 1 << 7;
 const SPRITE_Y_FLIP: u8 = 1 << 6;
 const SPRITE_X_FLIP: u8 = 1 << 5;
-const SPRITE_PALETTE_NO: u8 = 1 << 4; // NonCGB only
-                                      // const SPRITE_TILE_VRAM_BANK: u8 = 1 << 3; // CGB only
-
-// Display color
-/*
-const MONOCHROME_PALETTE: &'static [[f64; 3]; 4] = &[
-    [0.605, 0.734, 0.059],
-    [0.543, 0.672, 0.059],
-    [0.188, 0.383, 0.188],
-    [0.059, 0.219, 0.059],
-];
-*/
-const MONOCHROME_PALETTE: &'static [[u8; 3]; 4] = &[
-    [255, 255, 255],
-    [200, 200, 200],
-    [125, 125, 12],
-    [50, 50, 50],
-];
-
-#[derive(Debug, Copy, Clone)]
-enum PixelOrigin {
-    Empty,
-    Background,
-    Window,
-    Sprite,
-}
+//const SPRITE_PALETTE_NO: u8 = 1 << 4; // NonCGB only
+const SPRITE_TILE_VRAM_BANK: u8 = 1 << 3; // CGB only
 
 #[derive(Copy, Clone)]
 struct Pixel {
@@ -83,7 +66,7 @@ impl Default for Pixel {
     }
 }
 
-#[derive(Debug)]
+#[derive(Debug, PartialEq)]
 enum DisplayMode {
     ReadOAMMemory,
     ReadFullMemory,
@@ -97,45 +80,12 @@ impl Default for DisplayMode {
     }
 }
 
-struct Sprite {
-    x: u8,
-    y: u8,
-    tile: u8,
-    flags: u8,
-}
-
-impl Sprite {
-    fn is_hidden(&self) -> bool {
-        self.x == 0 || self.y == 0
-    }
-
-    fn is_foreground(&self) -> bool {
-        (self.flags & SPRITE_OBJ_BG_PRIORITY) == 0
-    }
-
-    fn is_x_flipped(&self) -> bool {
-        (self.flags & SPRITE_X_FLIP) == SPRITE_X_FLIP
-    }
-
-    fn is_y_flipped(&self) -> bool {
-        (self.flags & SPRITE_Y_FLIP) == SPRITE_Y_FLIP
-    }
-
-    fn palette(&self) -> u8 {
-        if (self.flags & SPRITE_PALETTE_NO) == 0 {
-            0
-        } else {
-            1
-        }
-    }
-}
-
 pub struct Display {
     control: u8,
     status: u8,
-    background_palette: u8,
-    object_palette_0: u8,
-    object_palette_1: u8,
+    background_palette: DmgPalette,
+    // Only 0 and 1 for GB
+    object_palette: [DmgPalette; 2],
     scrollx: u8,
     scrolly: u8,
     windowx: u8,
@@ -143,12 +93,14 @@ pub struct Display {
     curline: u8,
     lyc: u8,
 
-    vram: Box<[u8]>,
+    vram0: Box<[u8]>,
+    vram1: Box<[u8]>,
     oam: Box<[u8]>,
 
+    vram_bank: u8,
     current_ticks: u16,
     current_mode: DisplayMode,
-    // TODO
+
     renderer: sdl2::render::Canvas<sdl2::video::Window>,
 
     pub event_pump: sdl2::EventPump,
@@ -160,6 +112,15 @@ pub struct Display {
 
     frameskip: u8,
     frame_no: u8,
+
+    // GBC:
+    background_palette_autoinc: bool,
+    background_palette_index: u8,
+    background_palette_cgb: [CgbPalette; 0x40 / 8],
+
+    object_palette_autoinc: bool,
+    object_palette_index: u8,
+    object_palette_cgb: [CgbPalette; 0x40 / 8],
 }
 
 impl Display {
@@ -178,9 +139,8 @@ impl Display {
         Display {
             control: 0,
             status: 0,
-            background_palette: 0,
-            object_palette_0: 0,
-            object_palette_1: 0,
+            background_palette: Default::default(),
+            object_palette: Default::default(),
             scrollx: 0,
             scrolly: 0,
             windowx: 0,
@@ -190,28 +150,35 @@ impl Display {
 
             current_ticks: 0,
             current_mode: DisplayMode::default(),
-            vram: vec![0; VRAM_SIZE].into_boxed_slice(),
+            vram0: vec![0; VRAM_SIZE].into_boxed_slice(),
+            vram1: vec![0; VRAM_SIZE].into_boxed_slice(),
+            vram_bank: 0,
             oam: vec![0; OAM_SIZE].into_boxed_slice(),
-            renderer: renderer,
+            renderer,
 
-            event_pump: event_pump,
+            event_pump,
 
             vblank_interrupt: false,
             stat_interrupt: false,
-            pixels: pixels,
+            pixels,
             frameskip: 0,
             frame_no: 0,
+
+            background_palette_autoinc: false,
+            background_palette_index: 0,
+            background_palette_cgb: [CgbPalette([0u8; 2 * 4]); 0x40 / 8],
+            object_palette_autoinc: false,
+            object_palette_index: 0,
+            object_palette_cgb: [CgbPalette([0u8; 2 * 4]); 0x40 / 8],
         }
     }
 
-    #[inline]
     fn set_pixel(&mut self, x: u8, y: u8, color: sdl2::pixels::Color, origin: PixelOrigin) {
         let p = &mut self.pixels[(x as usize) + (y as usize) * GB_PIXELS_X];
         p.color = color;
         p.origin = origin;
     }
 
-    #[inline]
     fn get_pixel_origin(&self, x: u8, y: u8) -> PixelOrigin {
         self.pixels[(x as usize) + (y as usize) * GB_PIXELS_X].origin
     }
@@ -247,10 +214,9 @@ impl Display {
             .expect("Rendering failed");
     }
 
-    #[inline]
     pub fn vblank_interrupt(&mut self) -> bool {
         // Returns whether or not a vblank interrupt should be done
-        // Yes, this is polling, and yes, this sucks.\
+        // Yes, this is polling, and yes, this sucks.
         if self.vblank_interrupt {
             self.vblank_interrupt = false;
             true
@@ -259,7 +225,6 @@ impl Display {
         }
     }
 
-    #[inline]
     pub fn stat_interrupt(&mut self) -> bool {
         if self.stat_interrupt {
             self.stat_interrupt = false;
@@ -269,10 +234,15 @@ impl Display {
         }
     }
 
-    #[inline]
     pub fn write_byte(&mut self, addr: u16, val: u8) {
         match addr {
-            0x8000..=0x9FFF => self.vram[(addr - 0x8000) as usize] = val,
+            0x8000..=0x9FFF => {
+                if (self.vram_bank & 1) == 0 {
+                    self.vram0[(addr - 0x8000) as usize] = val
+                } else {
+                    self.vram1[(addr - 0x8000) as usize] = val
+                }
+            }
             0xFE00..=0xFE9F => self.oam[(addr - 0xFE00) as usize] = val,
             0xFF40 => self.control = val,
             0xFF41 => self.status = val,
@@ -280,9 +250,47 @@ impl Display {
             0xFF43 => self.scrollx = val,
             0xFF44 => self.curline = 0,
             0xFF45 => self.lyc = val,
-            0xFF47 => self.background_palette = val,
-            0xFF48 => self.object_palette_0 = val,
-            0xFF49 => self.object_palette_1 = val,
+            // GB classic
+            0xFF47 => self.background_palette.0 = val,
+            0xFF48 => self.object_palette[0].0 = val,
+            0xFF49 => self.object_palette[1].0 = val,
+            // GBC
+            0xFF68 => {
+                self.background_palette_index = val & 0b0111_1111;
+                self.background_palette_autoinc = (val >> 7) != 0;
+            }
+            0xFF69 => {
+                if self.current_mode == DisplayMode::ReadFullMemory {
+                    println!("Trying to write to palette memory while being accessed!");
+                }
+
+                let idx = self.background_palette_index as usize;
+                if idx < 64 {
+                    self.background_palette_cgb[idx / 8].0[idx % 8] = val;
+                } else {
+                    println!("OOB palette w/ autoinc");
+                    return;
+                }
+                if self.background_palette_autoinc {
+                    self.background_palette_index += 1;
+                }
+            }
+            0xFF6A => {
+                self.object_palette_index = val & 0b0111_1111;
+                self.object_palette_autoinc = (val >> 7) != 0;
+            }
+            0xFF6B => {
+                let idx = self.object_palette_index as usize;
+                if idx < 64 {
+                    self.object_palette_cgb[idx / 8].0[idx % 8] = val;
+                } else {
+                    println!("OOB obj palette w/ autoinc");
+                    return;
+                }
+                if self.object_palette_autoinc {
+                    self.object_palette_index += 1;
+                }
+            }
             0xFF4A => {
                 if self.windowy != val {
                     println!("WY set to {:02X}", val);
@@ -295,14 +303,20 @@ impl Display {
                 }
                 self.windowx = val;
             }
+            0xFF4F => self.vram_bank = val,
             _ => panic!("Display: Write {:02X} to {:04X} unsupported", val, addr),
         }
     }
 
-    #[inline]
     pub fn read_byte(&self, addr: u16) -> u8 {
         match addr {
-            0x8000..=0x9FFF => self.vram[(addr - 0x8000) as usize],
+            0x8000..=0x9FFF => {
+                if (self.vram_bank & 1) == 0 {
+                    self.vram0[(addr - 0x8000) as usize]
+                } else {
+                    self.vram1[(addr - 0x8000) as usize]
+                }
+            }
             0xFE00..=0xFE9F => self.oam[(addr - 0xFE00) as usize],
             0xFF40 => self.control,
             0xFF41 => self.status,
@@ -310,16 +324,16 @@ impl Display {
             0xFF43 => self.scrollx,
             0xFF44 => self.curline,
             0xFF45 => self.lyc,
-            0xFF47 => self.background_palette,
-            0xFF48 => self.object_palette_0,
-            0xFF49 => self.object_palette_1,
+            0xFF47 => self.background_palette.0,
+            0xFF48 => self.object_palette[0].0,
+            0xFF49 => self.object_palette[1].0,
             0xFF4A => self.windowy,
             0xFF4B => self.windowx,
+            0xFF4F => self.vram_bank | 0b1111_1110,
             _ => panic!("Display: Read from {:04X} unsupported", addr),
         }
     }
 
-    #[inline]
     pub fn tick(&mut self, ticks: u16) {
         self.status &= 0xFC;
         if self.control & CTRL_LCD_DISPLAY_ENABLE == 0 {
@@ -419,15 +433,14 @@ impl Display {
         }
     }
 
-    fn render_sprites(&mut self, sprites: &Vec<Sprite>) {
+    fn render_sprites(&mut self, sprites: &[Sprite]) {
         if self.control & CTRL_BG_SPRITE_ENABLE > 0 {
             let mut num_rendered: u8 = 0;
-            for i in 0..39 {
+            for sprite in sprites.iter().take(39) {
                 // Gameboy limitation
-                if num_rendered > 10 {
+                if num_rendered >= 10 {
                     break;
                 }
-                let sprite = &sprites[i];
 
                 // Skip hidden sprites
                 if sprite.is_hidden() {
@@ -437,36 +450,32 @@ impl Display {
                 // Calculate correct coords
                 let x: u8 = sprite.x.wrapping_sub(8);
                 let y: u8 = sprite.y.wrapping_sub(16);
-
                 let render_y = self.curline;
 
                 // Is this sprite on the current line?
-                if y.wrapping_add(8) >= render_y && y <= render_y {
+                let wide_mode = self.control & CTRL_BG_SPRITE_SIZE > 0;
+                let actual_h = if wide_mode { 16 } else { 8 };
+
+                if y.wrapping_add(actual_h) > render_y && y <= render_y {
                     num_rendered += 1;
-                    // Flip sprite, TODO: Validate
-                    let y_o: u8 = match sprite.is_y_flipped() {
-                        true => y ^ 7,
-                        false => y,
+                    let tile_offset_y = if sprite.is_y_flipped() {
+                        render_y as usize - (y as usize) ^ (actual_h as usize - 1)
+                    } else {
+                        render_y as usize - y as usize
                     };
-                    let tile_offset_y: usize = render_y as usize - y_o as usize;
-                    let tile_base_addr: usize = sprite.tile as usize * 16; // Should this be twice as wide in wide mode?
+
+                    let tile_base_addr: usize = sprite.tile as usize * 16;
 
                     let tile_addr = tile_base_addr + tile_offset_y * 2;
-                    let tile_line_1 = self.vram[tile_addr + 0];
-                    let tile_line_2 = self.vram[tile_addr + 1];
-                    let tile_line_3 = self.vram[tile_addr + 2];
-                    let tile_line_4 = self.vram[tile_addr + 3];
-
-                    // We need to draw this.
-                    let wide_mode = self.control & CTRL_BG_SPRITE_SIZE > 0;
-                    if wide_mode {
-                        // panic!("TODO");
-                    }
-                    let limit = match wide_mode {
-                        true => 16,
-                        false => 8,
+                    let vram = if sprite.vram_bank() == 0 {
+                        &*self.vram0
+                    } else {
+                        &*self.vram1
                     };
-                    for x_o in 0..limit {
+                    let tile_line_1 = vram[tile_addr + 0];
+                    let tile_line_2 = vram[tile_addr + 1];
+
+                    for x_o in 0..8 {
                         let b1: bool;
                         let b2: bool;
 
@@ -476,55 +485,37 @@ impl Display {
                         // Do not draw if the sprite should be drawn in the background
                         if !sprite.is_foreground() {
                             match pixel_origin {
-                                PixelOrigin::Background | PixelOrigin::Window => continue,
+                                PixelOrigin::Background(0) => {}
+                                PixelOrigin::Window | PixelOrigin::Background(_) => continue,
                                 _ => {}
                             }
                         }
 
-                        if wide_mode && x_o > 7 {
-                            let x_o2: u8 = match sprite.is_x_flipped() {
-                                true => (x_o - 8) ^ 7,
-                                false => (x_o - 8),
-                            };
-                            b1 = (tile_line_3 & 1 << (7 - x_o2)) > 0;
-                            b2 = (tile_line_4 & 1 << (7 - x_o2)) > 0;
-                        } else {
-                            let x_o2: u8 = match sprite.is_x_flipped() {
-                                true => x_o ^ 7,
-                                false => x_o,
-                            };
-                            b1 = (tile_line_1 & 1 << (7 - x_o2)) > 0;
-                            b2 = (tile_line_2 & 1 << (7 - x_o2)) > 0;
-                        }
-
-                        // Sprites may be transparent.
-                        if !b1 && !b2 {
+                        if pixel_origin == PixelOrigin::BackgroundPriority {
                             continue;
                         }
 
-                        let c = (b1 as u8) * 2 + b2 as u8;
-                        let lookup: [u8; 4] = match sprite.palette() {
-                            0 => [
-                                self.object_palette_0 & 3,
-                                (self.object_palette_0 >> 2) & 3,
-                                (self.object_palette_0 >> 4) & 3,
-                                (self.object_palette_0 >> 6) & 3,
-                            ],
-                            1 => [
-                                self.object_palette_1 & 3,
-                                (self.object_palette_1 >> 2) & 3,
-                                (self.object_palette_1 >> 4) & 3,
-                                (self.object_palette_1 >> 6) & 3,
-                            ],
-                            _ => unreachable!(),
-                        };
-                        let entry = MONOCHROME_PALETTE[lookup[c as usize] as usize];
+                        let x_maybe_flipped: u8 = if sprite.is_x_flipped() { x_o ^ 7 } else { x_o };
+                        b1 = (tile_line_1 & 1 << (7 - x_o)) > 0;
+                        b2 = (tile_line_2 & 1 << (7 - x_o)) > 0;
 
-                        // Draw stuff. We're currently only in monochrome mode
+                        // Sprites may be transparent.
+                        if !b1 && !b2 && sprite.is_foreground() {
+                            continue;
+                        }
+
+                        let c = ((b2 as u8) * 2 + b1 as u8) as usize;
+                        if c == 0 {
+                            continue;
+                        }
+                        // DMG:
+                        // let c = self.object_palette[sprite.palette() as usize].get_color(c);
+                        // GBC:
+                        let c = self.object_palette_cgb[sprite.palette() as usize].get_color(c);
                         self.set_pixel(
-                            x.wrapping_add(x_o),
+                            x.wrapping_add(x_maybe_flipped),
                             render_y,
-                            sdl2::pixels::Color::RGB(entry[0], entry[1], entry[2]),
+                            c,
                             PixelOrigin::Sprite,
                         );
                     }
@@ -533,7 +524,6 @@ impl Display {
         }
     }
 
-    #[inline]
     fn get_bg_window_tile_addr(&self, tile_id: u8) -> usize {
         if (self.control & CTRL_BG_WINDOW_TILE_DATA_SELECT) == CTRL_BG_WINDOW_TILE_DATA_SELECT {
             let base_addr = 0x0000;
@@ -545,7 +535,17 @@ impl Display {
         }
     }
 
-    #[inline]
+    pub fn dump_vram(&self) {
+        std::fs::File::create("vram0.dat")
+            .unwrap()
+            .write_all(&self.vram0)
+            .unwrap();
+        std::fs::File::create("vram1.dat")
+            .unwrap()
+            .write_all(&self.vram1)
+            .unwrap();
+    }
+
     fn renderscan(&mut self) {
         // Points to the background map offset to use.
         let background_map: usize;
@@ -571,132 +571,140 @@ impl Display {
         // Order sprites by priority
         let mut queue: Vec<Sprite> = Vec::new();
         for i in 0..39 {
-            queue.push(Sprite {
-                y: self.oam[i * 4 + 0],
-                x: self.oam[i * 4 + 1],
-                tile: self.oam[i * 4 + 2],
-                flags: self.oam[i * 4 + 3],
-            });
+            queue.push(Sprite::load(&self.oam[i * 4..(i + 1) * 4]));
         }
 
         // This is the non-CGB priority.
         // Smaller x coord = higher.
-        use std::cmp;
-        queue.sort_by(|x, y| {
-            if x.x > y.x {
-                cmp::Ordering::Greater
-            } else if x.x < y.x {
-                cmp::Ordering::Less
-            } else {
-                cmp::Ordering::Equal
-            }
-        });
+        queue.sort_by(|x, y| x.x.cmp(&y.x));
         queue.reverse();
 
         // Render background
         if (self.control & CTRL_BG_DISPLAY) == CTRL_BG_DISPLAY {
             // Render pixels (20 tiles)
+            let render_abs_y = map_offset_y.wrapping_add(render_y);
+            let tile_index_y: u8 = render_abs_y >> 3;
+            let tile_offset_y: u8 = render_abs_y & 7;
+
             for render_x in 0..160 {
                 // Absolute render coordinates
                 let render_abs_x = map_offset_x.wrapping_add(render_x);
-                let render_abs_y = map_offset_y.wrapping_add(render_y);
-
                 let tile_index_x: u8 = render_abs_x >> 3;
                 let tile_offset_x: u8 = render_abs_x & 7;
 
-                let tile_index_y: u8 = render_abs_y >> 3;
-                let tile_offset_y: u8 = render_abs_y & 7;
-
                 let vram_offset: usize =
                     background_map + ((tile_index_y as usize) * 32) + tile_index_x as usize;
 
                 // Obtain tile ID in this area
-                let tile_id = self.vram[vram_offset];
+                let tile_id = self.vram0[vram_offset];
+
+                // GBC stuff
+                // Get BG map attributes
+                let bg_attribs = BgMapAttributes(self.vram1[vram_offset]);
+                let vram = [&*self.vram0, &*self.vram1][bg_attribs.vram_bank_number()];
+
+                let tile_offset_y = if bg_attribs.vertical_flip() {
+                    7 ^ tile_offset_y
+                } else {
+                    tile_offset_y
+                };
 
                 // Obtain tile information
                 let tile_base_addr: usize = self.get_bg_window_tile_addr(tile_id);
-                let addr = tile_base_addr + (tile_offset_y as usize) * 2;
-                let tile_line_1 = self.vram[addr];
-                let tile_line_2 = self.vram[addr + 1];
+                let addr = tile_base_addr + (tile_offset_y as usize) * 2; // two bytes per row
+
+                let tile_line_1 = vram[addr];
+                let tile_line_2 = vram[addr + 1];
 
                 // Get the correct bit
-                let b1: bool = (tile_line_1 & 1 << (7 - tile_offset_x)) != 0;
-                let b2: bool = (tile_line_2 & 1 << (7 - tile_offset_x)) != 0;
+                let b1 = if bg_attribs.horizontal_flip() {
+                    7 ^ (1 << (7 - tile_offset_x))
+                } else {
+                    1 << (7 - tile_offset_x)
+                };
+                let b2 = if bg_attribs.horizontal_flip() {
+                    7 ^ (1 << (7 - tile_offset_x))
+                } else {
+                    1 << (7 - tile_offset_x)
+                };
+                let b1: bool = (tile_line_1 & b1) != 0;
+                let b2: bool = (tile_line_2 & b2) != 0;
 
                 // Lookup the color
-                let c = (b1 as u8) * 2 + b2 as u8;
-                let lookup: [u8; 4] = [
-                    self.background_palette & 3,
-                    (self.background_palette >> 2) & 3,
-                    (self.background_palette >> 4) & 3,
-                    (self.background_palette >> 6) & 3,
-                ];
-                let entry = MONOCHROME_PALETTE[lookup[c as usize] as usize];
-
-                let origin = match c {
-                    0 => PixelOrigin::Empty, // Hack so that objects will be in front of it.
-                    _ => PixelOrigin::Background,
-                };
-                self.set_pixel(
-                    render_x,
-                    render_y,
-                    sdl2::pixels::Color::RGB(entry[0], entry[1], entry[2]),
-                    origin,
-                );
+                let c = ((b2 as u8) * 2 + b1 as u8) as usize;
+                // let color = self.background_palette.get_color(c);
+                let color = self.background_palette_cgb[bg_attribs.palette_number()].get_color(c);
+                if bg_attribs.has_priority() && (self.control & 1) == 1 {
+                    self.set_pixel(render_x, render_y, color, PixelOrigin::BackgroundPriority);
+                } else {
+                    self.set_pixel(render_x, render_y, color, PixelOrigin::Background(c));
+                }
             }
         }
 
         if (self.control & CTRL_WND_DISPLAY_ENABLE) == CTRL_WND_DISPLAY_ENABLE {
             // Draw 'window' over the background.
             // Screen coordinates of the top left corner are WX-7, WY
+            // Quick check if the window is visible at all.
             if self.windowx < 167 && self.windowy < 144 {
-                //let rx = self.windowx.wrapping_add(7);
-                let rx = 7u8.wrapping_sub(self.windowx);
-                let ry = render_y.wrapping_add(self.windowy);
-                for r_x in 0..160u8 {
-                    let render_x = r_x.wrapping_add(rx);
-                    // Absolute render coordinates
-                    let tile_index_x: u8 = render_x >> 3;
-                    let tile_offset_x: u8 = render_x & 7;
-                    let tile_index_y: u8 = ry >> 3;
-                    let tile_offset_y: u8 = ry & 7;
+                let window_y_offset = render_y as i16 - self.windowy as i16;
+                if self.windowx < 7 {
+                    eprintln!("Window X position < 7, bailing out");
+                } else if window_y_offset >= 144 || window_y_offset < 0 {
+                    // Not visible
+                } else {
+                    let window_y_offset = window_y_offset as u8;
+                    let window_x_offset = self.windowx - 7;
 
-                    let vram_offset: usize =
-                        window_map + (tile_index_y as usize) * 32 + tile_index_x as usize;
+                    for r_x in 0..(160u8 - window_x_offset) {
+                        let render_x = r_x.wrapping_add(window_x_offset);
+                        // Absolute render coordinates
+                        let tile_index_x: u8 = render_x >> 3;
+                        let tile_index_y: u8 = window_y_offset >> 3;
+                        let tile_offset_x: u8 = render_x & 7;
+                        let tile_offset_y: u8 = window_y_offset & 7;
 
-                    // Obtain tile ID in this area
-                    let tile_id = self.vram[vram_offset];
+                        let vram_offset: usize =
+                            window_map + (tile_index_y as usize) * 32 + tile_index_x as usize;
 
-                    // Obtain tile information
-                    let tile_base_addr: usize = self.get_bg_window_tile_addr(tile_id);
-                    let tile_addr = tile_base_addr + (tile_offset_y as usize) * 2;
-                    let tile_line_1 = self.vram[tile_addr];
-                    let tile_line_2 = self.vram[tile_addr + 1];
+                        // Obtain tile ID in this area
+                        let tile_id = self.vram0[vram_offset];
+                        let bg_attribs = BgMapAttributes(self.vram1[vram_offset]);
 
-                    // Get the correct bit
-                    let b1: bool = (tile_line_1 & 1 << (7 - tile_offset_x)) > 0;
-                    let b2: bool = (tile_line_2 & 1 << (7 - tile_offset_x)) > 0;
+                        // Get BG map attributes
+                        let vram = if bg_attribs.vram_bank_number() == 0 {
+                            &*self.vram0
+                        } else {
+                            &*self.vram1
+                        };
 
-                    let c = (b1 as u8) * 2 + b2 as u8;
-                    let lookup: [u8; 4] = [
-                        self.background_palette & 3,
-                        (self.background_palette >> 2) & 3,
-                        (self.background_palette >> 4) & 3,
-                        (self.background_palette >> 6) & 3,
-                    ];
-                    let entry = MONOCHROME_PALETTE[lookup[c as usize] as usize];
+                        let tile_offset_x = if bg_attribs.horizontal_flip() {
+                            7 ^ tile_offset_x
+                        } else {
+                            tile_offset_x
+                        };
+                        let tile_offset_y = if bg_attribs.vertical_flip() {
+                            7 ^ tile_offset_y
+                        } else {
+                            tile_offset_y
+                        };
 
-                    // Draw stuff. We're currently only in monochrome mode
-                    let origin = match c {
-                        0 => PixelOrigin::Empty, // Hack so that objects will be in front of it.
-                        _ => PixelOrigin::Window,
-                    };
-                    self.set_pixel(
-                        render_x,
-                        render_y,
-                        sdl2::pixels::Color::RGB(entry[0], entry[1], entry[2]),
-                        origin,
-                    );
+                        // Obtain tile information
+                        let tile_base_addr: usize = self.get_bg_window_tile_addr(tile_id);
+                        let tile_addr = tile_base_addr + (tile_offset_y as usize) * 2;
+                        let tile_line_1 = vram[tile_addr];
+                        let tile_line_2 = vram[tile_addr + 1];
+
+                        // Get the correct bit
+                        let b1: bool = (tile_line_1 & 1 << (7 - tile_offset_x)) > 0;
+                        let b2: bool = (tile_line_2 & 1 << (7 - tile_offset_x)) > 0;
+
+                        let c = (b2 as u8) * 2 + b1 as u8;
+                        // let c = self.background_palette.get_color(c);
+                        let c = self.background_palette_cgb[bg_attribs.palette_number()]
+                            .get_color(c as usize);
+                        self.set_pixel(render_x, render_y, c, PixelOrigin::Window);
+                    }
                 }
             }
         }

src/display/palette.rs

@@ -0,0 +1,61 @@
+use super::sdl2;
+
+#[derive(Copy, Clone, Default)]
+pub struct DmgPalette(pub u8);
+impl DmgPalette {
+    pub fn get_color(self, n: usize) -> sdl2::pixels::Color {
+        const MONOCHROME_PALETTE: &[[u8; 3]; 4] = &[
+            [255, 255, 255],
+            [200, 200, 200],
+            [125, 125, 12],
+            [50, 50, 50],
+        ];
+        assert!(n < 4);
+        let c = self.0 >> (2 * n);
+        let n = c & 3;
+        let c = MONOCHROME_PALETTE[n as usize];
+        sdl2::pixels::Color::RGB(c[0], c[1], c[2])
+    }
+}
+
+#[derive(Copy, Clone)]
+pub struct CgbPalette(pub [u8; 8]);
+impl CgbPalette {
+    pub fn get_color(self, n: usize) -> sdl2::pixels::Color {
+        if n == 0 {
+            return sdl2::pixels::Color::RGB(255, 255, 255);
+        }
+        let v = ((self.0[2 * n + 1] as u16) << 8) | (self.0[2 * n] as u16);
+        let r = (v & 0b1_1111) as u8;
+        let g = ((v >> 5) & 0b1_1111) as u8;
+        let b = ((v >> 10) & 0b1_1111) as u8;
+
+        if false {
+            sdl2::pixels::Color::RGB(r << 3, g << 3, b << 3)
+        } else {
+            // According to some code:
+            // Real colors:
+            let r = r as u16;
+            let g = g as u16;
+            let b = b as u16;
+            let mapped_r = ((r * 13 + g * 2 + b) >> 1) as u8;
+            let mapped_g = ((g * 3 + b) << 1) as u8;
+            let mapped_b = ((r * 3 + g * 2 + b * 11) >> 1) as u8;
+            sdl2::pixels::Color::RGB(mapped_r, mapped_g, mapped_b)
+        }
+    }
+}
+
+impl std::fmt::Display for CgbPalette {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        write!(f, "Palette: ")?;
+        for n in 0..4 {
+            let v = ((self.0[2 * n + 1] as u16) << 8) | (self.0[2 * n] as u16);
+            let r = (v & 0b1_1111) as u8;
+            let g = ((v >> 5) & 0b1_1111) as u8;
+            let b = ((v >> 10) & 0b1_1111) as u8;
+            write!(f, "{:02X}{:02X}{:02X} ", r, g, b)?;
+        }
+        write!(f, "")
+    }
+}

src/display/structs.rs

@@ -0,0 +1,90 @@
+use super::*;
+// Display color
+pub struct BgMapAttributes(pub u8);
+impl BgMapAttributes {
+    pub fn palette_number(&self) -> usize {
+        (self.0 & 0b111) as usize
+    }
+
+    pub fn vram_bank_number(&self) -> usize {
+        ((self.0 >> 3) & 1) as usize
+    }
+
+    pub fn horizontal_flip(&self) -> bool {
+        ((self.0 >> 5) & 1) == 1
+    }
+
+    pub fn vertical_flip(&self) -> bool {
+        ((self.0 >> 6) & 1) == 1
+    }
+
+    pub fn has_priority(&self) -> bool {
+        (self.0 >> 7) != 0
+    }
+}
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub enum PixelOrigin {
+    Empty,
+    Background(usize),
+    BackgroundPriority,
+    Window,
+    Sprite,
+}
+
+pub struct Sprite {
+    pub x: u8,
+    pub y: u8,
+    pub tile: u8,
+    flags: u8,
+}
+
+impl Sprite {
+    pub fn load(buf: &[u8]) -> Self {
+        assert!(buf.len() >= 4);
+        Self {
+            x: buf[1],
+            y: buf[0],
+            tile: buf[2],
+            flags: buf[3],
+        }
+    }
+
+    pub fn is_hidden(&self) -> bool {
+        self.x == 0 || self.y == 0
+    }
+
+    pub fn is_foreground(&self) -> bool {
+        (self.flags & SPRITE_OBJ_BG_PRIORITY) == 0
+    }
+
+    pub fn is_x_flipped(&self) -> bool {
+        (self.flags & SPRITE_X_FLIP) == SPRITE_X_FLIP
+    }
+
+    pub fn is_y_flipped(&self) -> bool {
+        (self.flags & SPRITE_Y_FLIP) == SPRITE_Y_FLIP
+    }
+
+    pub fn palette(&self) -> u8 {
+        // GB
+        /*
+        if (self.flags & SPRITE_PALETTE_NO) == 0 {
+            0
+        } else {
+            1
+        }
+        */
+        // GBC
+        self.flags & 0b111
+    }
+
+    // GBC only
+    pub fn vram_bank(&self) -> u8 {
+        if (self.flags & SPRITE_TILE_VRAM_BANK) == 0 {
+            0
+        } else {
+            1
+        }
+    }
+}

src/interconnect.rs

@@ -32,6 +32,12 @@ extern crate sdl2;
 use self::sdl2::event::Event;
 use self::sdl2::keyboard::Keycode;
 
+#[derive(Debug, Copy, Clone, PartialEq)]
+pub enum TickResult {
+    Continue,
+    Shutdown,
+}
+
 #[derive(Debug)]
 enum Key {
     UP,
@@ -50,7 +56,7 @@ pub struct Interconnect {
     ram: Box<[u8]>,
     hiram: Box<[u8]>,
     wram_bank: u8,
-    sound: sound::Sound,
+    sound: sound::SoundManager,
     display: display::Display,
     interrupt: u8,
     interrupt_request_flags: u8,
@@ -76,12 +82,12 @@ pub struct Interconnect {
 impl Interconnect {
     pub fn new(bios: Box<[u8]>, rom: Box<[u8]>, save_file: Option<String>) -> Interconnect {
         Interconnect {
-            bios: bios,
+            bios,
             cartridge: cartridge::Cartridge::new(rom, save_file),
             ram: vec![0; WRAM_SIZE].into_boxed_slice(),
             hiram: vec![0; HIRAM_SIZE].into_boxed_slice(),
             wram_bank: 1,
-            sound: sound::Sound::new(),
+            sound: sound::SoundManager::new(),
             display: display::Display::new(),
             // Refactor those
             interrupt_request_flags: 0,
@@ -137,7 +143,7 @@ impl Interconnect {
     }
 
     // Somehow we need different timers for this.
-    pub fn tick(&mut self, cycles: u8) {
+    pub fn tick(&mut self, cycles: u8) -> TickResult {
         self.display.tick(cycles as u16);
         self.timer.tick(cycles as u16);
 
@@ -159,113 +165,66 @@ impl Interconnect {
 
         // Make sure the window is responsive:
         if self.cycles > 500 {
-            loop {
-                if let Some(event) = self.display.event_pump.poll_event() {
-                    match event {
-                        Event::Quit { .. }
-                        | Event::KeyDown {
-                            keycode: Some(Keycode::Escape),
-                            ..
-                        } => {
-                            self.cartridge.save();
-                            panic!("TODO: Proper shutdown");
-                        }
-                        Event::KeyDown {
-                            keycode: Some(Keycode::Left),
-                            ..
-                        } => self.press_key(Key::LEFT),
-                        Event::KeyDown {
-                            keycode: Some(Keycode::Down),
-                            ..
-                        } => self.press_key(Key::DOWN),
-                        Event::KeyDown {
-                            keycode: Some(Keycode::Up),
-                            ..
-                        } => self.press_key(Key::UP),
-                        Event::KeyDown {
-                            keycode: Some(Keycode::Right),
-                            ..
-                        } => self.press_key(Key::RIGHT),
-                        Event::KeyDown {
-                            keycode: Some(Keycode::A),
-                            ..
-                        } => self.press_key(Key::START),
-                        Event::KeyDown {
-                            keycode: Some(Keycode::S),
-                            ..
-                        } => self.press_key(Key::SELECT),
-                        Event::KeyDown {
-                            keycode: Some(Keycode::Z),
-                            ..
-                        } => self.press_key(Key::A),
-                        Event::KeyDown {
-                            keycode: Some(Keycode::X),
-                            ..
-                        } => self.press_key(Key::B),
-
-                        Event::KeyUp {
-                            keycode: Some(Keycode::Left),
-                            ..
-                        } => self.release_key(Key::LEFT),
-                        Event::KeyUp {
-                            keycode: Some(Keycode::Down),
-                            ..
-                        } => self.release_key(Key::DOWN),
-                        Event::KeyUp {
-                            keycode: Some(Keycode::Up),
-                            ..
-                        } => self.release_key(Key::UP),
-                        Event::KeyUp {
-                            keycode: Some(Keycode::Right),
-                            ..
-                        } => self.release_key(Key::RIGHT),
-                        Event::KeyUp {
-                            keycode: Some(Keycode::A),
-                            ..
-                        } => self.release_key(Key::START),
-                        Event::KeyUp {
-                            keycode: Some(Keycode::S),
-                            ..
-                        } => self.release_key(Key::SELECT),
-                        Event::KeyUp {
-                            keycode: Some(Keycode::Z),
-                            ..
-                        } => self.release_key(Key::A),
-                        Event::KeyUp {
-                            keycode: Some(Keycode::X),
-                            ..
-                        } => self.release_key(Key::B),
-                        _ => {}
+            while let Some(event) = self.display.event_pump.poll_event() {
+                match event {
+                    Event::Quit { .. }
+                    | Event::KeyDown {
+                        keycode: Some(Keycode::Escape),
+                        ..
+                    } => {
+                        self.cartridge.save();
+                        self.display.dump_vram();
+                        return TickResult::Shutdown;
                     }
-                } else {
-                    break;
+                    Event::KeyDown { keycode: k, .. } => match k {
+                        Some(Keycode::Left) => self.press_key(Key::LEFT),
+                        Some(Keycode::Down) => self.press_key(Key::DOWN),
+                        Some(Keycode::Up) => self.press_key(Key::UP),
+                        Some(Keycode::Right) => self.press_key(Key::RIGHT),
+                        Some(Keycode::A) => self.press_key(Key::START),
+                        Some(Keycode::S) => self.press_key(Key::SELECT),
+                        Some(Keycode::Z) => self.press_key(Key::A),
+                        Some(Keycode::X) => self.press_key(Key::B),
+                        _ => {}
+                    },
+                    Event::KeyUp { keycode: k, .. } => match k {
+                        Some(Keycode::Left) => self.release_key(Key::LEFT),
+                        Some(Keycode::Down) => self.release_key(Key::DOWN),
+                        Some(Keycode::Up) => self.release_key(Key::UP),
+                        Some(Keycode::Right) => self.release_key(Key::RIGHT),
+                        Some(Keycode::A) => self.release_key(Key::START),
+                        Some(Keycode::S) => self.release_key(Key::SELECT),
+                        Some(Keycode::Z) => self.release_key(Key::A),
+                        Some(Keycode::X) => self.release_key(Key::B),
+                        _ => {}
+                    },
+                    _ => {}
                 }
             }
             self.cycles = 0;
         } else {
             self.cycles += cycles as u16;
         }
+        TickResult::Continue
     }
 
-    #[inline]
     pub fn is_boot_rom(&self) -> bool {
         self.disable_bootrom == 0
     }
 
-    #[inline]
     pub fn read_byte(&self, addr: u16) -> u8 {
         // TODO: Make this more beautiful.
         // TODO: if some flag set, use bios, otherwise only use rom
         // For now, just use bios
         match addr {
-            0x0000..=0x100 => {
-                if self.disable_bootrom == 0 {
+            0x0000..=0x0FF | 0x200..=0x8FF => {
+                if self.disable_bootrom == 0 && self.bios.len() > addr as usize {
                     self.bios[addr as usize]
                 } else {
                     self.cartridge.read_byte(addr)
                 }
             }
-            0x100..=0x7FFF => self.cartridge.read_byte(addr),
+            0x100..=0x1FF | 0x900..=0x7FFF => self.cartridge.read_byte(addr),
             0x8000..=0x9FFF => self.display.read_byte(addr),
             0xA000..=0xBFFF => self.cartridge.read_byte(addr),
             0xC000..=0xCFFF => self.ram[(addr - 0xC000) as usize],
@@ -289,9 +248,10 @@ impl Interconnect {
                 // println!("Reading IF: {:02X}", self.interrupt_request_flags);
                 self.interrupt_request_flags
             }
-            0xFF10..=0xFF26 => self.sound.read_byte(addr),
-            0xFF30..=0xFF3F => self.sound.read_byte(addr),
+            0xFF10..=0xFF26 => self.sound.sound_object.lock().unwrap().read_byte(addr),
+            0xFF30..=0xFF3F => self.sound.sound_object.lock().unwrap().read_byte(addr),
             0xFF40..=0xFF4B => self.display.read_byte(addr),
+            0xFF4F => self.display.read_byte(addr),
             0xFF50 => self.disable_bootrom,
             0xFF51 => self.vram_dma_source_high,
             0xFF52 => self.vram_dma_source_low,
@@ -299,6 +259,7 @@ impl Interconnect {
             0xFF54 => self.vram_dma_destination_low,
             0xFF55 => self.vram_dma_length,
             0xFF56 => self.infrared_com_port,
+            0xFF6A | 0xFF6B => self.display.read_byte(addr),
             0xFF70 => self.wram_bank,
             0xFF80..=0xFFFE => self.hiram[(addr - 0xFF80) as usize],
             0xFFFF => self.interrupt,
@@ -309,7 +270,6 @@ impl Interconnect {
         }
     }
 
-    #[inline]
     pub fn write_byte(&mut self, addr: u16, val: u8) {
         // TODO: Make this more beautful
         /*
@@ -335,6 +295,9 @@ impl Interconnect {
             0xD000..=0xDFFF => {
                 self.ram[(addr - 0xD000) as usize + self.wram_bank as usize * 0x1000] = val;
             }
+            0xE000..=0xFCFF => {
+                self.ram[(addr - 0xE000) as usize] = val;
+            }
             0xFE00..=0xFE9F => self.display.write_byte(addr, val), // OAM
             0xFF00 => {
                 // Joystick select
@@ -346,9 +309,18 @@ impl Interconnect {
                 self.interrupt_request_flags = val;
             }
             0xFF10..=0xFF26 => {
-                self.sound.write_byte(addr, val);
+                self.sound
+                    .sound_object
+                    .lock()
+                    .expect("Sound-related crash")
+                    .write_byte(addr, val);
             }
-            0xFF30..=0xFF3F => self.sound.write_byte(addr, val),
+            0xFF30..=0xFF3F => self
+                .sound
+                .sound_object
+                .lock()
+                .expect("Sound related crash")
+                .write_byte(addr, val),
             // Exclude DMA transfer, we will do this below
             0xFF40..=0xFF45 | 0xFF47..=0xFF4B => {
                 self.display.write_byte(addr, val);
@@ -360,6 +332,7 @@ impl Interconnect {
                     self.write_byte(0xFE00 | x, dma_b);
                 }
             }
+            0xFF4F => self.display.write_byte(addr, val),
             0xFF50 => {
                 println!("Disabling boot rom.");
                 self.disable_bootrom = val;
@@ -374,28 +347,32 @@ impl Interconnect {
                 let mut dst: u16 = ((self.vram_dma_destination_high as u16) << 8)
                     | self.vram_dma_destination_low as u16;
 
+                dst &= 0b0001_1111_1111_0000;
                 dst += 0x8000;
                 println!(
                     "VRAM DMA transfer from {:04X} to {:04X}; {:02X}",
                     src, dst, val
                 );
-                let len: u16 = ((val & 0x7F) + 1) as u16 * 0x10 - 1;
+                let len: u16 = ((val & 0x7F) + 1) as u16 * 0x10;
+                let _mode = val & 0x80 != 0;
                 for i in 0..len {
                     let v = self.read_byte(src.wrapping_add(i));
                     self.write_byte(dst.wrapping_add(i), v);
                 }
 
                 // DMA done
-                self.vram_dma_length = val | 0x80;
+                self.vram_dma_length = 0xFF; // val | 0x80;
             }
             0xFF56 => {
                 self.infrared_com_port = val;
             }
             0xFF70 => {
                 if self.wram_bank != val {
-                    println!("Switching wram bank to {:02X}", val);
                     if val > 7 {
-                        panic!("R u sure this is correct?");
+                        panic!(
+                            "Trying to switch to wram bank {} which is non-existing",
+                            val
+                        );
                     }
                     if val == 0 {
                         self.wram_bank = 1;
@@ -404,6 +381,8 @@ impl Interconnect {
                     }
                 }
             }
+            0xFF68 | 0xFF69 => self.display.write_byte(addr, val),
+            0xFF6A | 0xFF6B => self.display.write_byte(addr, val),
             0xFF80..=0xFFFE => {
                 self.hiram[(addr - 0xFF80) as usize] = val;
             }
@@ -417,13 +396,11 @@ impl Interconnect {
         }
     }
 
-    #[inline]
     pub fn write_word(&mut self, addr: u16, val: u16) {
         self.write_byte(addr, val as u8);
         self.write_byte(addr + 1, (val >> 8) as u8);
     }
 
-    #[inline]
     pub fn read_word(&self, addr: u16) -> u16 {
         self.read_byte(addr) as u16 | (self.read_byte(addr + 1) as u16) << 8
         // (self.read_byte(addr) as u16) << 8 | (self.read_byte(addr + 1) as u16)

src/main.rs

@@ -1,5 +1,7 @@
 // let's try to write our own, awesome emulator.
 // gameboy (color?)
+// To make things more readable at points
+#![allow(clippy::identity_op)]
 
 use std::env;
 use std::fs;
@@ -24,10 +26,11 @@ fn main() {
     } else {
         let bios_path = &args[1];
         let rom_path = &args[2];
-        let mut save_file: Option<String> = None;
-        if args.len() == 4 {
-            save_file = Some(args[3].clone());
-        }
+        let save_file = if args.len() == 4 {
+            Some(args[3].clone())
+        } else {
+            None
+        };
 
         let bios = read_file(&bios_path).unwrap();
         let rom = read_file(&rom_path).unwrap();
@@ -36,21 +39,19 @@ fn main() {
         let interconnect = interconnect::Interconnect::new(bios, rom, save_file);
         let mut cpu = cpu::CPU::new(interconnect);
 
-        loop {
-            cpu.run();
-        }
+        cpu.run();
     }
 }
 
 pub fn read_file<P: AsRef<Path>>(rom_path: P) -> Result<Box<[u8]>, io::Error> {
-    let mut file = r#try!(fs::File::open(rom_path));
+    let mut file = fs::File::open(rom_path)?;
     let mut buf = Vec::new();
-    r#try!(file.read_to_end(&mut buf));
+    file.read_to_end(&mut buf)?;
     Ok(buf.into_boxed_slice())
 }
 
-pub fn write_file<P: AsRef<Path>>(path: P, data: &Box<[u8]>) -> Result<(), io::Error> {
-    let mut file = r#try!(fs::File::create(path));
-    r#try!(file.write(&data));
+pub fn write_file<P: AsRef<Path>>(path: P, data: &[u8]) -> Result<(), io::Error> {
+    let mut file = fs::File::create(path)?;
+    file.write_all(&data)?;
     Ok(())
 }

src/mbc/mbc.rs

@@ -1,53 +0,0 @@
-pub trait MBC {
-    fn read_byte(&self, addr: u16) -> u8;
-    fn write_byte(&mut self, addr: u16, val: u8);
-
-    fn dump_ram(&self, file: &String);
-}
-
-pub struct NoMBC {
-    rom: Box<[u8]>,
-    ram: Box<[u8]>,
-}
-
-impl NoMBC {
-    pub fn new(rom: Box<[u8]>, ram: Box<[u8]>) -> NoMBC {
-        NoMBC { rom: rom, ram: ram }
-    }
-}
-
-impl MBC for NoMBC {
-    fn dump_ram(&self, file: &String) {
-        super::super::write_file(&file, &self.ram).expect("Saving failed");
-    }
-
-    fn write_byte(&mut self, addr: u16, val: u8) {
-        println!(
-            "Writing not supported for cartridges without MBC. (Tried to set {:04X} to {:02X})",
-            addr, val
-        );
-    }
-
-    fn read_byte(&self, addr: u16) -> u8 {
-        match addr {
-            0x0000..=0x7FFF => self.rom[addr as usize],
-            0xA000..=0xBFFF => {
-                // TODO: Check for ram
-                let addr = (addr as usize) - 0xA000;
-
-                if addr >= self.ram.len() {
-                    println!(
-                        "Tried to access {:04X}, however the memory is not present.",
-                        addr + 0xA000
-                    );
-                    0
-                } else {
-                    self.ram[addr]
-                }
-            }
-            _ => {
-                panic!("Cartride: Unable to read from {:04X}", addr);
-            }
-        }
-    }
-}

src/mbc/mbc1.rs

@@ -1,4 +1,4 @@
-use super::mbc::MBC;
+use super::MBC;
 
 enum BankMode {
     RomBankMode,
@@ -17,8 +17,8 @@ pub struct MBC1 {
 impl MBC1 {
     pub fn new(rom: Box<[u8]>, ram: Box<[u8]>) -> MBC1 {
         MBC1 {
-            rom: rom,
-            ram: ram,
+            rom,
+            ram,
             rom_bank_no: 1,
             bank_mode: BankMode::RomBankMode,
             bank_no_high: 0,
@@ -43,7 +43,7 @@ impl MBC1 {
 }
 
 impl MBC for MBC1 {
-    fn dump_ram(&self, file: &String) {
+    fn dump_ram(&self, file: &str) {
         super::super::write_file(&file, &self.ram).expect("Saving failed");
     }
 
@@ -94,6 +94,11 @@ impl MBC for MBC1 {
                     _ => panic!("Invalid bank mode {:02X}", val),
                 }
             }
+            0xA000..=0xBFFF => {
+                let addr = addr - 0xA000;
+                println!("Access [{:02X}] {:04X}", self.active_ram_bank(), addr);
+                self.ram[self.active_ram_bank() as usize * 0x2000 + addr as usize] = val;
+            }
             _ => panic!("MBC1: Writing {:02X} to {:04X} not supported", val, addr),
         }
     }

src/mbc/mbc2.rs

@@ -1,4 +1,4 @@
-use super::mbc::MBC;
+use super::MBC;
 
 pub struct MBC2 {
     rom: Box<[u8]>,
@@ -10,8 +10,8 @@ pub struct MBC2 {
 impl MBC2 {
     pub fn new(rom: Box<[u8]>, ram: Box<[u8]>) -> MBC2 {
         MBC2 {
-            rom: rom,
-            ram: ram,
+            rom,
+            ram,
             rom_bank_no: 1,
             ram_enable: false,
         }
@@ -23,7 +23,7 @@ impl MBC2 {
 }
 
 impl MBC for MBC2 {
-    fn dump_ram(&self, file: &String) {
+    fn dump_ram(&self, file: &str) {
         super::super::write_file(&file, &self.ram).expect("Saving failed");
     }
 
@@ -61,7 +61,7 @@ impl MBC for MBC2 {
                 }
             }
             0x2000..=0x3FFF => {
-                if addr & 0x0100 == 1 {
+                if addr & 0x0100 == 0 {
                     self.rom_bank_no = val & 0x0F;
                     println!("MBC2: Selecting bank {:02X}", self.rom_bank_no);
                 } else {

src/mbc/mbc3.rs

@@ -1,4 +1,4 @@
-use super::mbc::MBC;
+use super::MBC;
 
 pub struct MBC3 {
     rom: Box<[u8]>,
@@ -11,8 +11,8 @@ pub struct MBC3 {
 impl MBC3 {
     pub fn new(rom: Box<[u8]>, ram: Box<[u8]>) -> MBC3 {
         MBC3 {
-            rom: rom,
-            ram: ram,
+            rom,
+            ram,
             rom_bank_no: 1,
             ram_bank_no: 0,
             ram_rtc_enabled: false,
@@ -28,7 +28,7 @@ impl MBC3 {
 }
 
 impl MBC for MBC3 {
-    fn dump_ram(&self, file: &String) {
+    fn dump_ram(&self, file: &str) {
         super::super::write_file(&file, &self.ram).expect("Saving failed");
     }
 

src/mbc/mbc5.rs

@@ -0,0 +1,115 @@
+use super::MBC;
+
+pub struct MBC5 {
+    rom: Box<[u8]>,
+    ram: Box<[u8]>,
+    rom_bank_no: u16,
+    ram_bank_no: u8,
+    ram_rtc_enabled: bool,
+}
+
+impl MBC5 {
+    pub fn new(rom: Box<[u8]>, ram: Box<[u8]>) -> Self {
+        MBC5 {
+            rom,
+            ram,
+            rom_bank_no: 1,
+            ram_bank_no: 0,
+            ram_rtc_enabled: false,
+        }
+    }
+    fn active_rom_bank(&self) -> u16 {
+        self.rom_bank_no
+    }
+
+    fn active_ram_bank(&self) -> u8 {
+        self.ram_bank_no
+    }
+}
+
+impl MBC for MBC5 {
+    fn dump_ram(&self, file: &str) {
+        use crate::write_file;
+        write_file(&file, &self.ram).expect("Saving failed");
+    }
+
+    fn read_byte(&self, addr: u16) -> u8 {
+        match addr {
+            0x0000..=0x3FFF => self.rom[addr as usize],
+            0x4000..=0x7FFF => {
+                let addr = addr - 0x4000;
+                let abs_addr: usize = addr as usize + self.active_rom_bank() as usize * 0x4000;
+                let val: u8 = self.rom[abs_addr];
+                val
+            }
+            0xA000..=0xBFFF => {
+                let addr = addr - 0xA000;
+                match self.active_ram_bank() {
+                    0x00..=0x03 => {
+                        self.ram[self.active_ram_bank() as usize * 0x2000 + addr as usize]
+                    }
+                    0x08..=0x0C => {
+                        // TODO
+                        println!("MBC5: Ignoring RTC read");
+                        0
+                    }
+                    _ => panic!(
+                        "MBC5: Accessing unknown RAM bank {:02X}",
+                        self.active_ram_bank()
+                    ),
+                }
+            }
+            _ => {
+                panic!("MBC5: Unable to read from {:04X}", addr);
+            }
+        }
+    }
+
+    fn write_byte(&mut self, addr: u16, val: u8) {
+        match addr {
+            0x0000..=0x1FFF => match val & 0x0F {
+                0x0A => self.ram_rtc_enabled = true,
+                0x00 => self.ram_rtc_enabled = false,
+                _ => {
+                    println!("MBC5: Unknown MBC value {:02X} for {:04X}", val, addr);
+                    self.ram_rtc_enabled = false;
+                }
+            },
+            // Lower part rom bank select
+            0x2000..=0x2FFF => self.rom_bank_no = (self.rom_bank_no & (!0xFF)) | u16::from(val),
+            0x3000..=0x3FFF => {
+                self.rom_bank_no = (self.rom_bank_no & (0xFF)) | ((u16::from(val) & 1) << 8)
+            }
+            0x4000..=0x5FFF => {
+                // RAM bank select
+                self.ram_bank_no = val & 0x0F;
+            }
+            0x6000..=0x7FFF => {
+                // Latch clock data
+                match val {
+                    0x00 => println!("latch = 0"),
+                    0x01 => println!("latch = 1"), // TODO: This should copy the current clock to the register
+                    _ => panic!("MBC5: Unknown latch value {:02X}", val),
+                }
+            }
+            0xA000..=0xBFFF => {
+                let addr = addr - 0xA000;
+                match self.active_ram_bank() {
+                    0x00..=0x03 => {
+                        let active_bank = self.active_ram_bank() as usize;
+                        self.ram[active_bank * 0x2000 + addr as usize] = val;
+                    }
+                    0x08..=0x0C => {
+                        // TODO
+                        println!("MBC5: Ignoring RTC write ({:02X})", val);
+                    }
+                    _ => panic!(
+                        "MBC5: Writing unknown RAM bank {:02X}",
+                        self.active_ram_bank()
+                    ),
+                }
+            }
+            _ => panic!("MBC5: Writing {:02X} to {:04X} not supported", val, addr),
+        }
+    }
+}

src/mbc/mod.rs

@@ -1,4 +1,63 @@
-pub mod mbc;
-pub mod mbc1;
-pub mod mbc2;
-pub mod mbc3;
+mod mbc1;
+mod mbc2;
+mod mbc3;
+mod mbc5;
+
+pub use mbc1::MBC1;
+pub use mbc2::MBC2;
+pub use mbc3::MBC3;
+pub use mbc5::MBC5;
+
+pub trait MBC {
+    fn read_byte(&self, addr: u16) -> u8;
+    fn write_byte(&mut self, addr: u16, val: u8);
+
+    fn dump_ram(&self, file: &str);
+}
+
+pub struct NoMBC {
+    rom: Box<[u8]>,
+    ram: Box<[u8]>,
+}
+
+impl NoMBC {
+    pub fn new(rom: Box<[u8]>, ram: Box<[u8]>) -> NoMBC {
+        NoMBC { rom, ram }
+    }
+}
+
+impl MBC for NoMBC {
+    fn dump_ram(&self, file: &str) {
+        super::write_file(&file, &self.ram).expect("Saving failed");
+    }
+
+    fn write_byte(&mut self, addr: u16, val: u8) {
+        println!(
+            "Writing not supported for cartridges without MBC. (Tried to set {:04X} to {:02X})",
+            addr, val
+        );
+    }
+
+    fn read_byte(&self, addr: u16) -> u8 {
+        match addr {
+            0x0000..=0x7FFF => self.rom[addr as usize],
+            0xA000..=0xBFFF => {
+                // TODO: Check for ram
+                let addr = (addr as usize) - 0xA000;
+
+                if addr >= self.ram.len() {
+                    println!(
+                        "Tried to access {:04X}, however the memory is not present.",
+                        addr + 0xA000
+                    );
+                    0
+                } else {
+                    self.ram[addr]
+                }
+            }
+            _ => {
+                panic!("Cartride: Unable to read from {:04X}", addr);
+            }
+        }
+    }
+}

src/serial.rs

@@ -12,7 +12,7 @@ impl Serial {
     pub fn write_byte(&mut self, addr: u16, val: u8) {
         match addr {
             0xFF01 => {
-                println!("Serial: Write {:02X}", val);
+                // println!("Serial: Write {:02X}", val);
                 self.data = val;
             }
             0xFF02 => self.control = val,

src/sound.rs

@@ -1,61 +0,0 @@
-#[derive(Debug, Default)]
-pub struct Sound {
-    enabled: u8,
-    sound_length_1: u8,
-    sound_control_1: u8,
-    sound_channel_volume_control: u8,
-    sound_output_terminal_selector: u8,
-    sound_freq_low: u8,
-    sound_freq_high: u8,
-
-    channel2_sound_length: u8,
-    channel2_volume: u8,
-    channel2_freq_lo: u8,
-    channel2_freq_hi: u8,
-}
-
-impl Sound {
-    pub fn new() -> Sound {
-        Sound::default()
-    }
-
-    pub fn write_byte(&mut self, addr: u16, val: u8) {
-        match addr {
-            0xFF11 => self.sound_length_1 = val,
-            0xFF12 => self.sound_control_1 = val,
-            0xFF13 => self.sound_freq_low = val,
-            0xFF14 => self.sound_freq_high = val,
-
-            0xFF16 => self.channel2_sound_length = val,
-            0xFF17 => self.channel2_volume = val,
-            0xFF18 => self.channel2_freq_lo = val,
-            0xFF19 => self.channel2_freq_hi = val,
-
-            0xFF24 => self.sound_channel_volume_control = val,
-            0xFF25 => self.sound_output_terminal_selector = val,
-            0xFF26 => self.enabled = val,
-            _ => {
-                // println!("Sound: Write {:02X} to {:04X} unsupported", val, addr);
-            }
-        }
-    }
-
-    pub fn read_byte(&self, addr: u16) -> u8 {
-        match addr {
-            0xFF11 => self.sound_length_1,
-            0xFF12 => self.sound_control_1,
-            0xFF13 => self.sound_freq_low,
-            0xFF14 => self.sound_freq_high,
-
-            0xFF16 => self.channel2_sound_length,
-            0xFF17 => self.channel2_volume,
-            0xFF18 => self.channel2_freq_lo,
-            0xFF19 => self.channel2_freq_hi,
-
-            0xFF24 => self.sound_channel_volume_control,
-            0xFF25 => self.sound_output_terminal_selector,
-            0xFF26 => self.enabled,
-            _ => panic!("Sound: Read from {:04X} unsupported", addr),
-        }
-    }
-}

src/sound/envelope.rs

@@ -0,0 +1,109 @@
+// Implements the envelopes
+use crate::sound::{AudioComponent, AudioModule};
+
+#[derive(Clone, Debug)]
+pub enum EnvelopeDirection {
+    Increase,
+    Decrease,
+}
+
+impl Default for EnvelopeDirection {
+    fn default() -> Self {
+        EnvelopeDirection::Decrease
+    }
+}
+
+/// Represents a volume envelope for the sound channels
+#[derive(Default, Debug)]
+pub struct VolumeEnvelope {
+    /// Accessible via the corresponding memory location, used as template
+    /// when the sound is enabled
+    register_value: u8,
+
+    /// Amount of clocks required for one step
+    clocks_per_step: u8,
+
+    /// Clocks occured since last step
+    clocks: u8,
+
+    /// Current volume
+    current_volume: u8,
+
+    /// Envelope direction
+    direction: EnvelopeDirection,
+}
+
+#[allow(dead_code)]
+impl VolumeEnvelope {
+    /// Returns the intiial volume of an envelope operation
+    pub fn initial_volume(&self) -> u8 {
+        self.register_value >> 4
+    }
+
+    /// Returns the envelope direction
+    pub fn envelope_direction(&self) -> EnvelopeDirection {
+        match self.register_value & (1 << 3) {
+            0 => EnvelopeDirection::Decrease,
+            _ => EnvelopeDirection::Increase,
+        }
+    }
+
+    /// Return the number of sweep steps
+    pub fn envelope_sweep_count(&self) -> u8 {
+        self.register_value & 7
+    }
+
+    pub fn start(&mut self) {
+        self.current_volume = self.initial_volume();
+        self.clocks_per_step = self.envelope_sweep_count();
+        self.direction = self.envelope_direction();
+        self.clocks = 0;
+    }
+
+    pub fn set_register(&mut self, register: u8) {
+        self.register_value = register; // & !(1 << 6); // TODO: Where does this come from?
+        if register & (1 << 6) != 0 {
+            self.start();
+        }
+    }
+}
+
+impl AudioModule<u8> for VolumeEnvelope {
+    fn transform(&self, sample: u8) -> u8 {
+        if sample > 0 {
+            128 + self.current_volume
+        } else {
+            128 - self.current_volume
+        }
+    }
+}
+
+impl AudioComponent for VolumeEnvelope {
+    fn clock(&mut self) {
+        if self.clocks_per_step > 0 {
+            self.clocks += 1;
+            if self.clocks == self.clocks_per_step {
+                self.clocks = 0;
+                self.current_volume = match self.direction {
+                    EnvelopeDirection::Decrease => {
+                        if self.current_volume > 0 {
+                            self.current_volume - 1
+                        } else {
+                            self.clocks_per_step = 0;
+                            0
+                        }
+                    }
+
+                    EnvelopeDirection::Increase => {
+                        if self.current_volume < 15 {
+                            self.current_volume + 1
+                        } else {
+                            self.clocks_per_step = 0;
+                            15
+                        }
+                    }
+                };
+            }
+        }
+    }
+}

src/sound/length.rs

@@ -0,0 +1,72 @@
+// Implement the length counter
+use crate::sound::{AudioComponent, AudioModule};
+use std::fmt::Debug;
+
+#[derive(Default, Debug)]
+pub struct LengthCounter<T>
+where
+    T: Default + Debug,
+{
+    // Do we need an additional enabled flag?
+    counter: T,
+    enabled: bool,
+}
+
+impl<T> LengthCounter<T>
+where
+    T: Default + Debug,
+{
+    pub fn set_length(&mut self, value: T) {
+        self.counter = value;
+    }
+
+    pub fn set_enabled(&mut self, val: u8) {
+        self.enabled = val > 0;
+    }
+}
+
+impl AudioModule<u8> for LengthCounter<u16> {
+    fn transform(&self, sample: u8) -> u8 {
+        if self.enabled {
+            if self.counter > 0 {
+                sample
+            } else {
+                0
+            }
+        } else {
+            sample
+        }
+    }
+}
+
+impl AudioComponent for LengthCounter<u16> {
+    fn clock(&mut self) {
+        if self.enabled && self.counter > 0 {
+            self.counter -= 1;
+        }
+    }
+}
+
+// TODO: Is the same as above, there is something we should be able to do about
+// this
+impl AudioModule<u8> for LengthCounter<u8> {
+    fn transform(&self, sample: u8) -> u8 {
+        if self.enabled {
+            if self.counter > 0 {
+                sample
+            } else {
+                0
+            }
+        } else {
+            sample
+        }
+    }
+}
+
+impl AudioComponent for LengthCounter<u8> {
+    fn clock(&mut self) {
+        if self.enabled && self.counter > 0 {
+            self.counter -= 1;
+        }
+    }
+}

src/sound/mod.rs

@@ -0,0 +1,547 @@
+#[cfg(feature = "audio")]
+extern crate pulse_simple;
+mod envelope;
+mod length;
+mod square;
+mod wave;
+
+#[cfg(feature = "audio")]
+use self::pulse_simple::Playback;
+use std::sync::{
+    atomic::{AtomicBool, Ordering},
+    Arc, Mutex,
+};
+#[cfg(feature = "audio")]
+use std::thread;
+
+use std::convert::TryFrom;
+
+const OUTPUT_SAMPLE_RATE: usize = 48100;
+//const OUTPUT_SAMPLE_RATE: usize = 32768;
+
+/// Represents an 'audio module', transforming an input sample to an output sample
+trait AudioModule<T> {
+    fn transform(&self, sample: T) -> T;
+}
+
+/// Clockable audio components
+trait AudioComponent {
+    fn clock(&mut self);
+}
+
+#[derive(Debug, Default)]
+struct Channel1 {
+    // Square 1: Sweep -> Timer -> Duty -> Length Counter -> Envelope -> Mixer
+    wave_gen: square::SquareWaveGenerator,
+    length_counter: length::LengthCounter<u8>,
+    envelope: envelope::VolumeEnvelope,
+
+    tick_state: u8,
+
+    stored_regs: [u8; 5],
+}
+
+impl Channel1 {
+    pub fn write_byte(&mut self, addr: u16, val: u8) {
+        match addr {
+            0xFF10 => {
+                self.wave_gen.set_sweep_reg(val);
+                self.stored_regs[0] = val;
+            }
+            0xFF11 => {
+                self.length_counter.set_length(val & 0x3F);
+                self.wave_gen.set_duty_cycle(val >> 6);
+                self.stored_regs[1] = val;
+            }
+            0xFF12 => {
+                self.envelope.set_register(val);
+                self.stored_regs[2] = val;
+            }
+            0xFF13 => {
+                // Set lower frequency
+                self.wave_gen.set_lower_freq(val);
+                self.stored_regs[3] = val;
+            }
+            0xFF14 => {
+                self.stored_regs[4] = val;
+                // Set higher
+                self.wave_gen.set_higher_freq(val & 0b111);
+
+                if val & (1 << 7) != 0 {
+                    self.wave_gen.reset();
+                    self.envelope.start();
+                }
+
+                self.length_counter.set_enabled(val & 1 << 6);
+            }
+            _ => panic!("Channel1: Write not supported: {:04X} = {:02X}", addr, val),
+        }
+    }
+
+    pub fn read_byte(&self, addr: u16) -> u8 {
+        match addr {
+            0xFF10..=0xFF14 => self.stored_regs[(addr - 0xFF10) as usize],
+            _ => {
+                println!("Channel1 does not support reading ({:04X})", addr);
+                0
+            }
+        }
+    }
+
+    pub fn sample_clock(&mut self) {
+        self.wave_gen.clock();
+    }
+
+    pub fn sample(&self) -> u8 {
+        let input = self.wave_gen.sample();
+        // Follow the chain
+        let input = self.length_counter.transform(input);
+        self.envelope.transform(input)
+    }
+
+    pub fn clock(&mut self) {
+        if self.tick_state % 2 == 0 {
+            self.length_counter.clock();
+        }
+
+        if self.tick_state == 2 || self.tick_state == 6 {
+            self.wave_gen.sweep_clock();
+        }
+
+        if self.tick_state == 7 {
+            self.envelope.clock();
+        }
+        // TODO: Sweep
+        // TODO: Sweep in adition mode + overflow? stop.
+        self.tick_state += 1;
+
+        if self.tick_state == 8 {
+            self.tick_state = 0;
+        }
+    }
+}
+
+#[derive(Debug, Default)]
+struct Channel2 {
+    // Square 2: Timer -> Duty -> Length Counter -> Envelope -> Mixer
+    wave_gen: square::SquareWaveGenerator,
+    length_counter: length::LengthCounter<u8>,
+    envelope: envelope::VolumeEnvelope,
+
+    tick_state: u8,
+}
+
+impl Channel2 {
+    pub fn write_byte(&mut self, addr: u16, val: u8) {
+        match addr {
+            0xFF16 => {
+                self.length_counter.set_length(val & 0x3F);
+                self.wave_gen.set_duty_cycle(val >> 6);
+            }
+            0xFF17 => {
+                self.envelope.set_register(val);
+            }
+            0xFF18 => {
+                // Set lower frequency
+                self.wave_gen.set_lower_freq(val);
+            }
+            0xFF19 => {
+                // Set higher
+                self.wave_gen.set_higher_freq(val & 0b111);
+
+                if val & (1 << 7) != 0 {
+                    self.wave_gen.reset();
+                    self.envelope.start();
+                }
+
+                self.length_counter.set_enabled(val & 1 << 6);
+            }
+            _ => panic!("Channel2: Write not supported: {:04X} = {:02X}", addr, val),
+        }
+    }
+
+    pub fn read_byte(&self, addr: u16) -> u8 {
+        use crate::sound::square::DutyCycle::*;
+        match addr {
+            0xFF16 => match self.wave_gen.duty_cycle {
+                Duty0 => 0,           // 12.5%
+                Duty1 => 0b0100_0000, // 25.0%
+                Duty2 => 0b1000_0000, // 50.0%
+                Duty3 => 0b1100_0000, // 75.0%
+            },
+            _ => {
+                println!("Channel2 does not support reading ({:04X})", addr);
+                0
+            }
+        }
+    }
+
+    pub fn sample_clock(&mut self) {
+        self.wave_gen.clock();
+    }
+
+    pub fn sample(&self) -> u8 {
+        let input = self.wave_gen.sample();
+        // Follow the chain
+        let input = self.length_counter.transform(input);
+        self.envelope.transform(input)
+    }
+
+    pub fn clock(&mut self) {
+        if self.tick_state % 2 == 0 {
+            self.length_counter.clock();
+        }
+
+        if self.tick_state == 2 || self.tick_state == 6 {
+            // TODO: Sweep
+        }
+
+        if self.tick_state == 7 {
+            self.envelope.clock();
+        }
+
+        self.tick_state += 1;
+
+        if self.tick_state == 8 {
+            self.tick_state = 0;
+        }
+    }
+}
+
+#[derive(Debug)]
+enum Channel3VolumeSetting {
+    Muted,
+    Original,
+    Half,
+    Quarter,
+}
+
+impl From<u8> for Channel3VolumeSetting {
+    fn from(o: u8) -> Self {
+        use Channel3VolumeSetting::*;
+        match o {
+            0 => Muted,
+            1 => Original,
+            2 => Half,
+            3 => Quarter,
+            _ => unreachable!("Invalid channel volume setting"),
+        }
+    }
+}
+
+impl Default for Channel3VolumeSetting {
+    fn default() -> Self {
+        Self::Muted
+    }
+}
+
+impl Channel3VolumeSetting {
+    fn transform(&self, sample: u8) -> u8 {
+        match self {
+            Self::Muted => 128,
+            // Actually making it louder as the wave src is quite silent.
+            Self::Original => ((sample - 120) << 1) + 112,
+            Self::Half => sample,
+            Self::Quarter => ((sample - 120) >> 1) + 124,
+        }
+    }
+}
+
+#[derive(Debug, Default)]
+struct Channel3 {
+    // Timer -> Wave -> Length Counter -> Volume -> Mixer
+    wave_gen: wave::WaveGenerator,
+    length_counter: length::LengthCounter<u8>,
+    volume_ctrl: Channel3VolumeSetting,
+    tick_state: u8,
+}
+
+impl Channel3 {
+    pub fn write_byte(&mut self, addr: u16, val: u8) {
+        // OFF flag -> "bit 7 of NR30"
+        match addr {
+            0xFF1A => {
+                self.wave_gen.enabled = (val & (1 << 7)) > 0;
+            }
+            0xFF1B => {
+                self.length_counter.set_length(val);
+            }
+            0xFF1C => self.volume_ctrl = ((val >> 5) & 0b11).into(),
+            0xFF1D => {
+                // Set lower frequency
+                self.wave_gen.set_lower_freq(val);
+            }
+            0xFF1E => {
+                // Set higher
+                self.wave_gen.set_higher_freq(val & 0b111);
+
+                if val & (1 << 7) != 0 {
+                    self.wave_gen.reset();
+                }
+
+                self.length_counter.set_enabled(val & (1 << 6));
+            }
+            0xFF30..=0xFF3F => {
+                self.wave_gen.set_sample_pair((addr - 0xFF30) as usize, val);
+            }
+            _ => panic!("Channel3: Write not supported: {:04X} = {:02X}", addr, val),
+        }
+    }
+
+    pub fn read_byte(&self, addr: u16) -> u8 {
+        match addr {
+            0xFF30..=0xFF3F => self.wave_gen.get_sample_pair((addr - 0xFF30) as usize),
+            _ => panic!("Channel3 does not support reading ({:04X})", addr),
+        }
+    }
+
+    pub fn sample_clock(&mut self) {
+        self.wave_gen.clock();
+    }
+
+    pub fn sample(&self) -> u8 {
+        let input = self.wave_gen.sample();
+        // TODO(?): Follow the chain
+        let input = self.length_counter.transform(input);
+        self.volume_ctrl.transform(input)
+    }
+
+    pub fn clock(&mut self) {
+        if self.tick_state % 2 == 0 {
+            self.length_counter.clock();
+        }
+
+        if self.tick_state == 2 || self.tick_state == 6 {
+            // TODO: Sweep
+        }
+
+        self.tick_state += 1;
+
+        if self.tick_state == 8 {
+            self.tick_state = 0;
+        }
+    }
+}
+
+#[derive(Default)]
+pub struct Sound {
+    channel1: Channel1,
+    channel2: Channel2,
+    channel3: Channel3,
+
+    sound_channel_volume_control: u8,
+    sound_output_terminal_selector: u8,
+    enabled: u8,
+}
+
+pub struct SoundManager {
+    pub sound_object: Arc<Mutex<Sound>>,
+    handle: Option<std::thread::JoinHandle<()>>,
+    do_exit: Arc<AtomicBool>,
+    sound_enabled: bool,
+}
+
+impl Drop for SoundManager {
+    fn drop(&mut self) {
+        self.do_exit.store(true, Ordering::Relaxed);
+        self.handle.take().and_then(|h| h.join().ok());
+    }
+}
+
+impl SoundManager {
+    pub fn new() -> Self {
+        let mut res = SoundManager {
+            sound_object: Arc::new(Mutex::new(Sound::new())),
+            handle: None,
+            do_exit: Arc::new(AtomicBool::new(false)),
+            sound_enabled: true,
+        };
+
+        res.launch_thread();
+        res
+    }
+
+    fn launch_thread(&mut self) {
+        let obj = self.sound_object.clone();
+        let do_exit = self.do_exit.clone();
+        #[cfg(feature = "audio")]
+        {
+            if self.sound_enabled {
+                self.handle = Some(
+                    thread::Builder::new()
+                        .name("Audio".into())
+                        .spawn(move || {
+                            // PulseAudio playback object
+                            let playback =
+                                Playback::new("GBC", "Output", None, (OUTPUT_SAMPLE_RATE) as _);
+
+                            // Counter, used for calling the 512 Hz timer
+                            let mut counter = 0;
+
+                            while !do_exit.load(Ordering::Relaxed) {
+                                const N_SAMPLES: usize = 1024;
+                                let mut sample_buffer = [[128u8; 2]; N_SAMPLES];
+
+                                for idx in 0..N_SAMPLES {
+                                    if let Some((s1, s2)) = {
+                                        let mut c_obj = obj.lock().unwrap();
+
+                                        // Check for 512 Hz timer
+                                        if counter >= (OUTPUT_SAMPLE_RATE / 512) {
+                                            c_obj.clock();
+                                            counter = 0;
+                                        } else {
+                                            counter += 1;
+                                        }
+
+                                        // Sample clock
+                                        c_obj.sample_clock();
+
+                                        // Get sample
+                                        c_obj.sample()
+                                    } {
+                                        sample_buffer[idx] = [s1, s2];
+                                    }
+                                }
+                                playback.write(&sample_buffer[..]);
+                            }
+                        })
+                        .unwrap(),
+                );
+            } else {
+                println!("Sound will be redirected to /dev/null ;)");
+                self.handle = None;
+            }
+        }
+    }
+}
+
+impl Sound {
+    pub fn new() -> Sound {
+        Sound::default()
+    }
+
+    pub fn clock(&mut self) {
+        self.channel1.clock();
+        self.channel2.clock();
+        self.channel3.clock();
+    }
+
+    pub fn sample_clock(&mut self) {
+        self.channel1.sample_clock();
+        self.channel2.sample_clock();
+        self.channel3.sample_clock();
+    }
+
+    pub fn sample(&self) -> Option<(u8, u8)> {
+        if self.enabled == 0 {
+            return None;
+        }
+
+        // Some basic mixing
+        // Output value
+        let mut s = (0, 0);
+        let s01_volume = self.sound_channel_volume_control & 7;
+        let s02_volume = (self.sound_channel_volume_control >> 4) & 7;
+
+        /*
+        if self.sound_channel_volume_control & 0b1000 != 0 {
+
+            VIN!
+        }
+
+        if self.sound_channel_volume_control & 0b1000_0000 != 0 {
+            VIN!
+        }
+        */
+
+        let c1_sample = self.channel1.sample();
+        let c2_sample = self.channel2.sample();
+        let c3_sample = self.channel3.sample();
+        let c4_sample = 128;
+
+        if self.sound_output_terminal_selector & 1 > 0 {
+            // Output Channel1
+            s.0 += i16::try_from(c1_sample).unwrap() - 128;
+        }
+        if self.sound_output_terminal_selector & 2 > 0 {
+            // Output Channel2
+            s.0 += i16::try_from(c2_sample).unwrap() - 128;
+        }
+        if self.sound_output_terminal_selector & 4 > 0 {
+            // Output Channel3
+            s.0 += i16::try_from(c3_sample).unwrap() - 128;
+        }
+        if self.sound_output_terminal_selector & 8 > 0 {
+            // Output Channel4
+            s.0 += i16::try_from(c4_sample).unwrap() - 128;
+        }
+
+        if self.sound_output_terminal_selector & 0x10 > 0 {
+            // Output Channel1
+            s.1 += i16::try_from(c1_sample).unwrap() - 128;
+        }
+        if self.sound_output_terminal_selector & 0x20 > 0 {
+            // Output Channel2
+            s.1 += i16::try_from(c2_sample).unwrap() - 128;
+        }
+        if self.sound_output_terminal_selector & 0x40 > 0 {
+            // Output Channel3
+            s.1 += i16::try_from(c3_sample).unwrap() - 128;
+        }
+        if self.sound_output_terminal_selector & 0x80 > 0 {
+            // Output Channel4
+            s.1 += i16::try_from(c4_sample).unwrap() - 128;
+        }
+
+        // TODO: This is too loud (overflowing u8) for some reason, so we're
+        //       backing off a little bit from this.
+        /*
+        if s.0 > 30 || s.1 > 30 {
+            println!("Would overflow! s={:?} s1vol={} s2vol={}", s, s01_volume, s02_volume);
+        }
+        */
+        //s.0 *= i16::try_from(s01_volume).unwrap(); // + 1;
+        //s.1 *= i16::try_from(s02_volume).unwrap(); // + 1;
+
+        let s = (128 + s.0, 128 + s.1);
+        let s = (u8::try_from(s.0).unwrap(), u8::try_from(s.1).unwrap());
+        Some(s)
+    }
+
+    pub fn write_byte(&mut self, addr: u16, val: u8) {
+        // Bit 7 of NR52 = 0 -> everything reset
+        // If all sound off no mode register can be set
+        // println!("Snd: {:04X} = {:02X} ({:08b})", addr, val, val);
+        match addr {
+            0xFF10..=0xFF14 => self.channel1.write_byte(addr, val),
+            0xFF16..=0xFF19 => self.channel2.write_byte(addr, val),
+            0xFF1A..=0xFF1E => self.channel3.write_byte(addr, val),
+            // TODO: Channel 4
+            0xFF24 => self.sound_channel_volume_control = val,
+            0xFF25 => self.sound_output_terminal_selector = val,
+            0xFF26 => self.enabled = val,
+            0xFF30..=0xFF3F => self.channel3.write_byte(addr, val),
+
+            _ => {
+                println!("Sound: Write {:02X} to {:04X} unsupported", val, addr);
+            }
+        }
+    }
+
+    pub fn read_byte(&self, addr: u16) -> u8 {
+        // println!("RD {:04X}", addr);
+        match addr {
+            0xFF10..=0xFF14 => self.channel1.read_byte(addr),
+            0xFF16..=0xFF19 => self.channel2.read_byte(addr),
+            0xFF1A..=0xFF1E => self.channel3.read_byte(addr),
+
+            0xFF24 => self.sound_channel_volume_control,
+            0xFF25 => self.sound_output_terminal_selector,
+            0xFF26 => self.enabled,
+            0xFF30..=0xFF3F => self.channel3.read_byte(addr),
+
+            _ => panic!("Sound: Read from {:04X} unsupported", addr),
+        }
+    }
+}

src/sound/square.rs

@@ -0,0 +1,138 @@
+// Square wave generator
+use crate::sound::AudioComponent;
+use crate::sound::OUTPUT_SAMPLE_RATE;
+
+#[derive(Debug)]
+pub enum DutyCycle {
+    Duty0, // 12.5%
+    Duty1, // 25.0%
+    Duty2, // 50.0%
+    Duty3, // 75.0%
+}
+
+impl Default for DutyCycle {
+    fn default() -> Self {
+        DutyCycle::Duty2
+    }
+}
+
+#[derive(Debug, Default)]
+pub struct SquareWaveGenerator {
+    frequency: f32,
+    time: f32,
+    pub duty_cycle: DutyCycle,
+
+    gb_reg_freq: u16,
+
+    // Sweep settings (if existing)
+    sweep_change_period: u8,
+    // selects the inc/dec mode of the sweep. Dec if set
+    sweep_dec: bool,
+    // Sweep speed (n/128th)
+    sweep_change: u8,
+
+    // Current sweep clock cycle
+    sweep_clock: u8,
+    // sweep_freq_shadow: f32,
+}
+
+impl SquareWaveGenerator {
+    fn update_frequency(&mut self) {
+        self.frequency = 131072f32 / ((2048 - self.gb_reg_freq) as f32);
+    }
+
+    pub fn set_lower_freq(&mut self, freq: u8) {
+        self.gb_reg_freq &= 0xFF00;
+        self.gb_reg_freq |= freq as u16;
+        self.update_frequency();
+    }
+
+    pub fn set_higher_freq(&mut self, freq: u8) {
+        self.gb_reg_freq &= 0x00FF;
+        self.gb_reg_freq |= ((freq & 7) as u16) << 8;
+        self.update_frequency();
+    }
+
+    pub fn set_sweep_reg(&mut self, reg: u8) {
+        //Bit 6-4 - Sweep Time
+        //Bit 3   - Sweep Increase/Decrease
+        //     0: Addition    (frequency increases)
+        //     1: Subtraction (frequency decreases)
+        //Bit 2-0 - Number of sweep shift (n: 0-7)
+        self.sweep_change_period = (reg >> 4) & 0b0111;
+        self.sweep_dec = reg & (1 << 3) > 0;
+        self.sweep_change = reg & 0b111;
+        self.sweep_clock = 0;
+    }
+
+    pub fn sweep_clock(&mut self) {
+        // This should be sampled at 128Hz
+        if self.sweep_change_period > 0  // enabled
+            && self.sweep_clock >= self.sweep_change_period
+        // sweep time hit
+        {
+            self.sweep_clock = 0;
+            let change = self.frequency / (2f32.powi(self.sweep_change as _));
+            self.frequency += if self.sweep_dec { -change } else { change };
+        }
+    }
+
+    pub fn reset(&mut self) {
+        self.time = 0f32;
+    }
+
+    pub fn sample(&self) -> u8 {
+        if self.frequency > 0f32 {
+            let mut temp = self.time * self.frequency;
+            temp -= temp.trunc();
+
+            match self.duty_cycle {
+                DutyCycle::Duty0 => {
+                    if temp < 0.125 {
+                        255
+                    } else {
+                        0
+                    }
+                }
+                DutyCycle::Duty1 => {
+                    if temp < 0.25 {
+                        255
+                    } else {
+                        0
+                    }
+                }
+                DutyCycle::Duty2 => {
+                    if temp < 0.5 {
+                        255
+                    } else {
+                        0
+                    }
+                }
+                DutyCycle::Duty3 => {
+                    if temp < 0.75 {
+                        255
+                    } else {
+                        0
+                    }
+                }
+            }
+        } else {
+            0
+        }
+    }
+
+    pub fn set_duty_cycle(&mut self, bits: u8) {
+        self.duty_cycle = match bits {
+            0 => DutyCycle::Duty0,
+            1 => DutyCycle::Duty1,
+            2 => DutyCycle::Duty2,
+            _ => DutyCycle::Duty3,
+        };
+    }
+}
+
+impl AudioComponent for SquareWaveGenerator {
+    fn clock(&mut self) {
+        self.time += 1f32 / (OUTPUT_SAMPLE_RATE as f32);
+    }
+}

src/sound/wave.rs

@@ -0,0 +1,90 @@
+// Square wave generator
+use crate::sound::AudioComponent;
+use crate::sound::OUTPUT_SAMPLE_RATE;
+
+#[derive(Debug, Default, Copy, Clone)]
+struct SamplePair(u8);
+
+impl SamplePair {
+    fn first(self) -> u8 {
+        self.0 >> 4
+    }
+
+    fn second(self) -> u8 {
+        self.0 & 0x0F
+    }
+}
+
+impl From<u8> for SamplePair {
+    fn from(val: u8) -> SamplePair {
+        SamplePair(val)
+    }
+}
+
+impl Into<u8> for SamplePair {
+    fn into(self: Self) -> u8 {
+        self.0
+    }
+}
+
+#[derive(Debug, Default)]
+pub struct WaveGenerator {
+    frequency: f32,
+    time: f32,
+    gb_reg_freq: u16,
+
+    pub enabled: bool,
+    samples: [SamplePair; 16],
+}
+
+impl WaveGenerator {
+    fn update_frequency(&mut self) {
+        self.frequency = ((2048 - self.gb_reg_freq) * 2) as _;
+        self.time = 0f32;
+    }
+
+    pub fn set_lower_freq(&mut self, freq: u8) {
+        self.gb_reg_freq &= 0xFF00;
+        self.gb_reg_freq |= freq as u16;
+        self.update_frequency();
+    }
+
+    pub fn set_higher_freq(&mut self, freq: u8) {
+        self.gb_reg_freq &= 0x00FF;
+        self.gb_reg_freq |= ((freq & 7) as u16) << 8;
+        self.update_frequency();
+    }
+
+    pub fn reset(&mut self) {
+        self.time = 0f32;
+    }
+
+    pub fn sample(&self) -> u8 {
+        if self.enabled && self.frequency > 0f32 {
+            let temp = self.time * self.frequency;
+            let position = temp.trunc() as usize % 32;
+
+            let sample = &self.samples[position / 2];
+            match position % 2 {
+                0 => sample.first() + 120,
+                _ => sample.second() + 120,
+            }
+        } else {
+            128
+        }
+    }
+
+    pub fn set_sample_pair(&mut self, nr: usize, val: u8) {
+        self.samples[nr] = val.into();
+    }
+
+    pub fn get_sample_pair(&self, nr: usize) -> u8 {
+        self.samples[nr].into()
+    }
+}
+
+impl AudioComponent for WaveGenerator {
+    fn clock(&mut self) {
+        self.time += 1f32 / (OUTPUT_SAMPLE_RATE as f32);
+    }
+}

src/timer.rs

@@ -11,43 +11,51 @@ pub struct Timer {
     gb_ticks: u16,
 }
 
+// Timer speed contains 4 different scales:
+// [0] = 64 = 4096   Hz
+// [1] =  1 = 262144 Hz
+// [2] =  4 = 65536  Hz
+// [3] = 16 = 16384  Hz
 const TIMER_SPEED: [u16; 4] = [64, 1, 4, 16];
-const TIMER_ENABLE: u8 = (1 << 2);
+const TIMER_ENABLE: u8 = 1 << 2;
 
 impl Timer {
     pub fn new() -> Timer {
         Timer::default()
     }
 
+    /// This clock is ticking at 262.144 Hz
     fn timer_clock_tick(&mut self) {
         // The div reg will always tick
         self.div_tick_counter += 1;
-        if self.div_tick_counter >= TIMER_SPEED[3] {
+
+        // 262144 Hz / 16 = 16384 Hz
+        if self.div_tick_counter >= 16 {
             self.div = self.div.wrapping_add(1);
-            self.div_tick_counter -= TIMER_SPEED[3];
+            self.div_tick_counter -= 16;
         }
 
-        if (self.tac & TIMER_ENABLE) == TIMER_ENABLE {
-            // Is timer enabled?
+        // Check if the timer is enabled
+        if (self.tac & TIMER_ENABLE) != 0 {
             self.tick_counter += 1;
             let req_ticks = TIMER_SPEED[(self.tac & 3) as usize];
 
-            if self.tick_counter >= req_ticks {
+            if self.tick_counter == req_ticks {
                 if self.tima == 0xFF {
                     self.timer_interrupt = true;
-                    self.tima = 0;
+                    self.tima = self.tma;
                 } else {
                     self.tima += 1;
                 }
-                self.tick_counter -= req_ticks;
+                self.tick_counter = 0;
             }
         }
     }
 
     pub fn tick(&mut self, ticks: u16) {
-        // One tick 1/4.194304MHz on regular speed => 16 gb ticks
+        // One tick 1/4.194304MHz on regular speed => 16 gb ticks = prescaler
         self.gb_ticks += ticks;
-        // If we're in GBC fast mode, we'd require 32 ticks instead of 16
+
         while self.gb_ticks >= 16 {
             self.timer_clock_tick();
             self.gb_ticks -= 16;
@@ -55,12 +63,13 @@ impl Timer {
     }
 
     pub fn write_byte(&mut self, addr: u16, val: u8) {
+        // println!("Timer WR: {:04X} = {:02X}", addr, val);
         match addr {
             0xFF04 => self.div = 0,
             0xFF05 => self.tima = val,
             0xFF06 => self.tma = val,
             0xFF07 => self.tac = val,
-            _ => println!("Timer: Write {:02X} to {:04X} unsupported", val, addr),
+            _ => unreachable!(),
         }
     }