Fix color bug

src/bus.rs

@@ -10,12 +10,17 @@ pub struct Bus<A, B> {
     start: Word,
     end: Word,
     device: A,
-    rest: B
+    rest: B,
 }
 
 impl<A, B> Bus<A, B> {
     fn new(start: u32, end: u32, device: A, rest: B) -> Self {
-        Self { start: start.into(), end: end.into(), device, rest }
+        Self {
+            start: start.into(),
+            end: end.into(),
+            device,
+            rest,
+        }
     }
 
     fn at(addr: u32, device: A, rest: B) -> Self {
@@ -59,14 +64,22 @@ mod tests {
 
     struct TestDevice(i32);
     impl Device for TestDevice {
-        fn tick(&mut self) { self.0 += 1 }
+        fn tick(&mut self) {
-        fn reset(&mut self) { self.0 = 10 }
+            self.0 += 1
+        }
+        fn reset(&mut self) {
+            self.0 = 10
+        }
     }
 
     struct ArrayDevice([u8; 10]);
     impl PeekPoke for ArrayDevice {
-        fn peek(&self, addr: Word) -> u8 { self.0[usize::from(addr)] }
+        fn peek(&self, addr: Word) -> u8 {
-        fn poke(&mut self, addr: Word, val: u8) { self.0[usize::from(addr)] = val }
+            self.0[usize::from(addr)]
+        }
+        fn poke(&mut self, addr: Word, val: u8) {
+            self.0[usize::from(addr)] = val
+        }
     }
 
     #[test]
@@ -76,7 +89,9 @@ mod tests {
         let device3 = TestDevice(7);
         let mut bus = Bus::at(5, device1, Bus::at(6, device2, device3));
 
-        for _ in 0..5 { bus.tick() }
+        for _ in 0..5 {
+            bus.tick()
+        }
         assert_eq!(bus.device.0, 10);
         assert_eq!(bus.rest.device.0, 11);
         assert_eq!(bus.rest.rest.0, 12);

src/cpu.rs

@@ -1,31 +1,35 @@
-use crate::opcodes::Opcode;
-use crate::opcodes::InvalidOpcode;
 use crate::memory::Memory;
-use crate::word::Word;
 use crate::memory::PeekPoke;
+use crate::opcodes::InvalidOpcode;
+use crate::opcodes::Opcode;
+use crate::word::Word;
 use std::convert::TryFrom;
 
 #[allow(clippy::upper_case_acronyms)]
 pub struct CPU {
-    memory: Memory, // Main memory, all of it
+    memory: Memory,    // Main memory, all of it
-    pc: Word, // program counter, address of the low byte of the instruction
+    pc: Word,          // program counter, address of the low byte of the instruction
-    dp: Word, // data pointer, address of the low byte of one cell above the data stack
+    dp: Word,          // data pointer, address of the low byte of one cell above the data stack
-    sp: Word, // stack pointer, address of the low byte of the return stack
+    sp: Word,          // stack pointer, address of the low byte of the return stack
-    iv: Word, // interrupt vector
+    iv: Word,          // interrupt vector
     int_enabled: bool, // interrupt enable bit
-    halted: bool, // Whether the CPU is halted
+    halted: bool,      // Whether the CPU is halted
 }
 
 #[derive(Copy, Clone, Eq, PartialEq, Debug)]
 struct Instruction {
     opcode: Opcode,
     arg: Option<u32>,
-    length: u8
+    length: u8,
 }
 
 impl PeekPoke for CPU {
-    fn peek(&self, addr: Word) -> u8 { self.memory.peek(addr) }
+    fn peek(&self, addr: Word) -> u8 {
-    fn poke(&mut self, addr: Word, val: u8) { self.memory.poke(addr, val) }
+        self.memory.peek(addr)
+    }
+    fn poke(&mut self, addr: Word, val: u8) {
+        self.memory.poke(addr, val)
+    }
 }
 
 impl CPU {
@@ -88,7 +92,7 @@ impl CPU {
                     Ok(Instruction {
                         opcode,
                         arg: None,
-                        length: 1
+                        length: 1,
                     })
                 } else {
                     let mut arg = 0u32;
@@ -100,11 +104,11 @@ impl CPU {
                     Ok(Instruction {
                         opcode,
                         arg: Some(arg),
-                        length: arg_length + 1
+                        length: arg_length + 1,
                     })
                 }
-            },
+            }
-            Err(e) => Err(e)
+            Err(e) => Err(e),
         }
     }
 
@@ -118,20 +122,20 @@ impl CPU {
             let y = self.pop_data();
 
             match instruction.opcode {
-                Opcode::Add => { self.push_data(x + y) }
+                Opcode::Add => self.push_data(x + y),
-                Opcode::Sub => { self.push_data(y - x) }
+                Opcode::Sub => self.push_data(y - x),
-                Opcode::Mul => { self.push_data(y * x) }
+                Opcode::Mul => self.push_data(y * x),
-                Opcode::Div => { self.push_data(y / x) }
+                Opcode::Div => self.push_data(y / x),
-                Opcode::Mod => { self.push_data(y % x) }
+                Opcode::Mod => self.push_data(y % x),
-                Opcode::And => { self.push_data(y & x) }
+                Opcode::And => self.push_data(y & x),
-                Opcode::Or => { self.push_data(y | x) }
+                Opcode::Or => self.push_data(y | x),
-                Opcode::Xor => { self.push_data(y ^ x) }
+                Opcode::Xor => self.push_data(y ^ x),
-                Opcode::Gt => { self.push_data(bool_as_word(y > x)) }
+                Opcode::Gt => self.push_data(bool_as_word(y > x)),
-                Opcode::Lt => { self.push_data(bool_as_word(y < x)) }
+                Opcode::Lt => self.push_data(bool_as_word(y < x)),
-                Opcode::Agt => { self.push_data(bool_as_word(word_as_signed(y) > word_as_signed(x))) }
+                Opcode::Agt => self.push_data(bool_as_word(word_as_signed(y) > word_as_signed(x))),
-                Opcode::Alt => { self.push_data(bool_as_word(word_as_signed(y) < word_as_signed(x))) }
+                Opcode::Alt => self.push_data(bool_as_word(word_as_signed(y) < word_as_signed(x))),
-                Opcode::Lshift => { self.push_data(y << x) }
+                Opcode::Lshift => self.push_data(y << x),
-                Opcode::Rshift => { self.push_data(y >> x) }
+                Opcode::Rshift => self.push_data(y >> x),
                 Opcode::Arshift => {
                     if y & 0x800000 != 0 {
                         let mut shifted = y;
@@ -147,14 +151,22 @@ impl CPU {
                     self.push_data(x);
                     self.push_data(y)
                 }
-                Opcode::Store => { self.memory.poke(x.into(), y as u8) }
+                Opcode::Store => self.memory.poke(x.into(), y as u8),
-                Opcode::Storew => { self.memory.poke24(x.into(), y) }
+                Opcode::Storew => self.memory.poke24(x.into(), y),
                 Opcode::Setsdp => {
                     self.dp = x.into();
                     self.sp = y.into()
                 }
-                Opcode::Brz => { if y == 0 { return self.pc + word_as_signed(x) } }
+                Opcode::Brz => {
-                Opcode::Brnz => { if y != 0 { return self.pc + word_as_signed(x) } }
+                    if y == 0 {
+                        return self.pc + word_as_signed(x);
+                    }
+                }
+                Opcode::Brnz => {
+                    if y != 0 {
+                        return self.pc + word_as_signed(x);
+                    }
+                }
                 _ => {} // This can never happen
             }
             self.pc + instruction.length as i32
@@ -166,8 +178,10 @@ impl CPU {
                     let x = self.pop_data();
                     self.push_data(bool_as_word(x == 0))
                 }
-                Opcode::Pop => { self.pop_data(); }
+                Opcode::Pop => {
-                Opcode::Dup => { self.push_data(self.peek_data()) }
+                    self.pop_data();
+                }
+                Opcode::Dup => self.push_data(self.peek_data()),
                 Opcode::Pick => {
                     let index = self.pop_data();
                     let val = self.memory.peek24(self.dp - (index as i32 + 1) * 3);
@@ -181,18 +195,18 @@ impl CPU {
                     self.push_data(x);
                     self.push_data(z)
                 }
-                Opcode::Jmp => { return self.pop_data().into() }
+                Opcode::Jmp => return self.pop_data().into(),
                 Opcode::Jmpr => {
                     let x = word_as_signed(self.pop_data());
-                    return self.pc + x
+                    return self.pc + x;
                 }
                 Opcode::Call => {
                     let x = self.pop_data();
                     self.push_call(self.pc + instruction.length as i32);
-                    return x.into()
+                    return x.into();
                 }
-                Opcode::Ret => { return self.pop_call().into() }
+                Opcode::Ret => return self.pop_call().into(),
-                Opcode::Hlt => { self.halted = true }
+                Opcode::Hlt => self.halted = true,
                 Opcode::Load => {
                     let x = self.pop_data();
                     self.push_data(self.memory.peek(x.into()) as u32)
@@ -201,9 +215,9 @@ impl CPU {
                     let x = self.pop_data();
                     self.push_data(self.memory.peek24(x.into()))
                 }
-                Opcode::Inton => { self.int_enabled = true }
+                Opcode::Inton => self.int_enabled = true,
-                Opcode::Intoff => { self.int_enabled = false }
+                Opcode::Intoff => self.int_enabled = false,
-                Opcode::Setiv => { self.iv = self.pop_data().into() }
+                Opcode::Setiv => self.iv = self.pop_data().into(),
                 Opcode::Sdp => {
                     self.push_data(self.sp);
                     self.push_data(self.dp + 3) // The +3 accounts for the word we're about to push
@@ -231,11 +245,28 @@ impl CPU {
 impl Opcode {
     fn is_binary(self) -> bool {
         use Opcode::*;
-        self != Nop && self != Not && self != Rand && self != Pop && self != Dup && self != Pick &&
+        self != Nop
-            self != Rot && self != Jmp && self != Jmpr && self != Call && self != Ret &&
+            && self != Not
-            self != Hlt && self != Load && self != Loadw && self != Inton && self != Intoff &&
+            && self != Rand
-            self != Setiv && self != Sdp && self != Pushr && self != Popr && self != Peekr &&
+            && self != Pop
-            self != Debug
+            && self != Dup
+            && self != Pick
+            && self != Rot
+            && self != Jmp
+            && self != Jmpr
+            && self != Call
+            && self != Ret
+            && self != Hlt
+            && self != Load
+            && self != Loadw
+            && self != Inton
+            && self != Intoff
+            && self != Setiv
+            && self != Sdp
+            && self != Pushr
+            && self != Popr
+            && self != Peekr
+            && self != Debug
     }
 }
 
@@ -247,7 +278,13 @@ fn word_as_signed(word: u32) -> i32 {
     }
 }
 
-fn bool_as_word(flag: bool) -> u32 { if flag { 1 } else { 0 } }
+fn bool_as_word(flag: bool) -> u32 {
+    if flag {
+        1
+    } else {
+        0
+    }
+}
 
 #[cfg(test)]
 mod tests {
@@ -276,59 +313,102 @@ mod tests {
         }
     }
 
-    fn predicate_opcode_test<P, Q>(opcode: Opcode, given: P, pred: Q) where P: FnOnce(&mut CPU), Q: FnOnce(&CPU)
+    fn predicate_opcode_test<P, Q>(opcode: Opcode, given: P, pred: Q)
+    where
+        P: FnOnce(&mut CPU),
+        Q: FnOnce(&CPU),
     {
         let mut cpu = CPU::new(Memory::default());
         given(&mut cpu);
-        let new_pc = cpu.execute(Instruction{ opcode: opcode, arg: None, length: 1 });
+        let new_pc = cpu.execute(Instruction {
+            opcode: opcode,
+            arg: None,
+            length: 1,
+        });
         cpu.pc = new_pc;
         pred(&mut cpu)
     }
 
     fn simple_opcode_test(given: Vec<u32>, opcode: Opcode, expected: Vec<u32>) {
-        predicate_opcode_test(opcode, |cpu| {
+        predicate_opcode_test(
-            for i in given.into_iter() { cpu.push_data(i) }
+            opcode,
-        }, |cpu| {
+            |cpu| {
-            assert_eq!(cpu.get_stack(), expected)
+                for i in given.into_iter() {
-        })
+                    cpu.push_data(i)
+                }
+            },
+            |cpu| assert_eq!(cpu.get_stack(), expected),
+        )
     }
 
-    fn call_stack_opcode_test(given: Vec<u32>, given_r: Vec<u32>, opcode: Opcode, expected: Vec<u32>, expected_r: Vec<u32>, pc: Word) {
+    fn call_stack_opcode_test(
-        predicate_opcode_test(opcode, |cpu| {
+        given: Vec<u32>,
-            for i in given.into_iter() { cpu.push_data(i) }
+        given_r: Vec<u32>,
-            for i in given_r.into_iter() { cpu.push_call(i) }
+        opcode: Opcode,
-        }, |cpu| {
+        expected: Vec<u32>,
-            assert_eq!(cpu.get_stack(), expected);
+        expected_r: Vec<u32>,
-            assert_eq!(cpu.get_call(), expected_r);
+        pc: Word,
-            assert_eq!(pc, cpu.pc)
+    ) {
-        })
+        predicate_opcode_test(
+            opcode,
+            |cpu| {
+                for i in given.into_iter() {
+                    cpu.push_data(i)
+                }
+                for i in given_r.into_iter() {
+                    cpu.push_call(i)
+                }
+            },
+            |cpu| {
+                assert_eq!(cpu.get_stack(), expected);
+                assert_eq!(cpu.get_call(), expected_r);
+                assert_eq!(pc, cpu.pc)
+            },
+        )
     }
 
-    fn control_flow_opcode_test<A>(given: Vec<u32>, opcode: Opcode, expected_pc: A) where A: Into<Word> {
+    fn control_flow_opcode_test<A>(given: Vec<u32>, opcode: Opcode, expected_pc: A)
-        predicate_opcode_test(opcode, |cpu| {
+    where
-            for i in given.into_iter() { cpu.push_data(i) }
+        A: Into<Word>,
-        }, |cpu| {
+    {
-            assert_eq!(cpu.pc, expected_pc.into())
+        predicate_opcode_test(
-        })
+            opcode,
+            |cpu| {
+                for i in given.into_iter() {
+                    cpu.push_data(i)
+                }
+            },
+            |cpu| assert_eq!(cpu.pc, expected_pc.into()),
+        )
     }
 
-    fn memory_opcode_test(given: Vec<u32>, given_memory: Vec<u8>, opcode: Opcode, expected: Vec<u32>, expected_memory: Option<Vec<u8>>) {
+    fn memory_opcode_test(
-        predicate_opcode_test(opcode,
+        given: Vec<u32>,
-                              |cpu| {
+        given_memory: Vec<u8>,
-                                  for i in given.into_iter() { cpu.push_data(i) }
+        opcode: Opcode,
-                                  for (offset, byte) in given_memory.into_iter().enumerate() {
+        expected: Vec<u32>,
-                                      cpu.memory.poke(Word::from(2048 + offset as u32), byte)
+        expected_memory: Option<Vec<u8>>,
-                                  }
+    ) {
-                              },
+        predicate_opcode_test(
-                              |cpu| {
+            opcode,
-                                  if let Some(expected_memory) = expected_memory {
+            |cpu| {
-                                      for (offset, byte) in expected_memory.into_iter().enumerate() {
+                for i in given.into_iter() {
-                                          let actual = cpu.memory.peek(Word::from(2048 + offset as u32));
+                    cpu.push_data(i)
-                                          assert_eq!(byte, actual, "At address 2048 + {}", offset)
+                }
-                                      }
+                for (offset, byte) in given_memory.into_iter().enumerate() {
-                                      assert_eq!(cpu.get_stack(), expected)
+                    cpu.memory.poke(Word::from(2048 + offset as u32), byte)
-                                  }
+                }
-                              })
+            },
+            |cpu| {
+                if let Some(expected_memory) = expected_memory {
+                    for (offset, byte) in expected_memory.into_iter().enumerate() {
+                        let actual = cpu.memory.peek(Word::from(2048 + offset as u32));
+                        assert_eq!(byte, actual, "At address 2048 + {}", offset)
+                    }
+                    assert_eq!(cpu.get_stack(), expected)
+                }
+            },
+        )
     }
 
     fn to_word(val: i32) -> u32 {
@@ -361,7 +441,7 @@ mod tests {
     fn test_basic_ops() {
         control_flow_opcode_test(vec![], Nop, 1025);
         simple_opcode_test(vec![2], Nop, vec![2]);
-        predicate_opcode_test(Hlt, |_| { }, |cpu| { assert!(cpu.halted) })
+        predicate_opcode_test(Hlt, |_| {}, |cpu| assert!(cpu.halted))
     }
 
     #[test]
@@ -378,9 +458,27 @@ mod tests {
     #[test]
     fn test_memory() {
         memory_opcode_test(vec![2048], vec![123], Load, vec![123], None);
-        memory_opcode_test(vec![2048], vec![0x12, 0x34, 0x56], Loadw, vec![0x123456], None);
+        memory_opcode_test(
-        memory_opcode_test(vec![100, 2048], vec![0x12, 0x34, 0x56], Store, vec![], Some(vec![100, 0x34, 0x56]));
+            vec![2048],
-        memory_opcode_test(vec![0x112233, 2048], vec![0x12, 0x34, 0x56], Storew, vec![], Some(vec![0x33, 0x22, 0x11]));
+            vec![0x12, 0x34, 0x56],
+            Loadw,
+            vec![0x123456],
+            None,
+        );
+        memory_opcode_test(
+            vec![100, 2048],
+            vec![0x12, 0x34, 0x56],
+            Store,
+            vec![],
+            Some(vec![100, 0x34, 0x56]),
+        );
+        memory_opcode_test(
+            vec![0x112233, 2048],
+            vec![0x12, 0x34, 0x56],
+            Storew,
+            vec![],
+            Some(vec![0x33, 0x22, 0x11]),
+        );
     }
 
     #[test]
@@ -407,16 +505,25 @@ mod tests {
     fn test_cpu_call_stack() {
         call_stack_opcode_test(vec![5000], vec![], Call, vec![], vec![1025], 5000.into());
         call_stack_opcode_test(vec![], vec![5000], Ret, vec![], vec![], 5000.into());
-        call_stack_opcode_test(vec![], vec![], Sdp, vec![1024, 256 + 6], vec![], 1025.into());
+        call_stack_opcode_test(
-        predicate_opcode_test(Setsdp,
+            vec![],
-                              |cpu| {
+            vec![],
-                                  cpu.push_data(1000u32);
+            Sdp,
-                                  cpu.push_data(2000u32)
+            vec![1024, 256 + 6],
-                              },
+            vec![],
-                              |cpu| {
+            1025.into(),
-                                  assert_eq!(cpu.sp, 1000.into());
+        );
-                                  assert_eq!(cpu.dp, 2000.into())
+        predicate_opcode_test(
-                              });
+            Setsdp,
+            |cpu| {
+                cpu.push_data(1000u32);
+                cpu.push_data(2000u32)
+            },
+            |cpu| {
+                assert_eq!(cpu.sp, 1000.into());
+                assert_eq!(cpu.dp, 2000.into())
+            },
+        );
         call_stack_opcode_test(vec![123], vec![], Pushr, vec![], vec![123], 1025.into());
         call_stack_opcode_test(vec![], vec![123], Popr, vec![123], vec![], 1025.into());
         call_stack_opcode_test(vec![], vec![123], Peekr, vec![123], vec![123], 1025.into());
@@ -471,13 +578,34 @@ mod tests {
         cpu.memory.poke_u32(0x406, 29 << 2); // hlt
         cpu.memory.poke_u32(0x407, 0xfc); // gibberish
 
-        assert_eq!(cpu.fetch(), Ok(Instruction { opcode: Opcode::Nop, arg: Some(2), length: 2 }));
+        assert_eq!(
+            cpu.fetch(),
+            Ok(Instruction {
+                opcode: Opcode::Nop,
+                arg: Some(2),
+                length: 2
+            })
+        );
 
         cpu.pc = 0x402.into();
-        assert_eq!(cpu.fetch(), Ok(Instruction { opcode: Opcode::Add, arg: Some(0x123456), length: 4 }));
+        assert_eq!(
+            cpu.fetch(),
+            Ok(Instruction {
+                opcode: Opcode::Add,
+                arg: Some(0x123456),
+                length: 4
+            })
+        );
 
         cpu.pc = 0x406.into();
-        assert_eq!(cpu.fetch(), Ok(Instruction { opcode: Opcode::Hlt, arg: None, length: 1 }));
+        assert_eq!(
+            cpu.fetch(),
+            Ok(Instruction {
+                opcode: Opcode::Hlt,
+                arg: None,
+                length: 1
+            })
+        );
 
         cpu.pc = 0x407.into();
         assert_eq!(cpu.fetch(), Err(InvalidOpcode(0x3f)));

src/display.rs

@@ -1,5 +1,6 @@
-use crate::Word;
 use crate::memory::PeekPoke;
+use crate::Word;
+use std::convert::TryFrom;
 
 #[derive(Copy, Clone, Debug, Eq, PartialEq)]
 struct DisplayRegisters {
@@ -10,20 +11,20 @@ struct DisplayRegisters {
     height: u32,
     width: u32,
     row_offset: u32,
-    col_offset: u32
+    col_offset: u32,
 }
 
 impl Default for DisplayRegisters {
     fn default() -> Self {
         Self {
-            mode: 5,
+            mode: 4,
             screen: 0x10000,
             palette: 0x20000 - 0x100,
             font: 0x20000 - 0x100 - 0x2000,
             height: 128,
             width: 128,
             row_offset: 0,
-            col_offset: 0
+            col_offset: 0,
         }
     }
 }
@@ -37,7 +38,7 @@ fn read_display_registers<P: PeekPoke>(machine: &P, start: Word) -> DisplayRegis
         height: machine.peek24(start + 10),
         width: machine.peek24(start + 13),
         row_offset: machine.peek24(start + 16),
-        col_offset: machine.peek24(start + 19)
+        col_offset: machine.peek24(start + 19),
     }
 }
 
@@ -62,7 +63,7 @@ fn init_font<P: PeekPoke>(machine: &mut P) {
     }
 }
 
-pub fn draw<P: PeekPoke>(machine: &P, frame: &mut[u8]) {
+pub fn draw<P: PeekPoke>(machine: &P, frame: &mut [u8]) {
     let reg = read_display_registers(machine, 16.into());
     let (gfx, highres, paletted) = (reg.mode & 1 > 0, reg.mode & 2 > 0, reg.mode & 4 > 0);
 
@@ -75,7 +76,6 @@ pub fn draw<P: PeekPoke>(machine: &P, frame: &mut[u8]) {
         (true, false, false) => draw_paletted_low_text(machine, reg, frame),
         (false, false, true) => draw_direct_low_gfx(machine, reg, frame),
         (true, false, true) => draw_paletted_low_gfx(machine, reg, frame),
-        (_, _, _) => {panic!("Unimplemented mode {}", reg.mode)}
     }
 }
 
@@ -88,8 +88,8 @@ pub fn reset<P: PeekPoke>(machine: &mut P) {
 fn init_palette<P: PeekPoke>(machine: &mut P) {
     let palette_addr = read_display_registers(machine, Word::from(16)).palette;
     let default_palette = [
-        0x00, 0x05, 0x65, 0x11, 0xa8, 0x49, 0xeb, 0xff,
+        0x00, 0x05, 0x65, 0x11, 0xa8, 0x49, 0xeb, 0xff, 0xe1, 0xf4, 0xfc, 0x1c, 0x37, 0x8e, 0xee,
-        0xe1, 0xf4, 0xfc, 0x1c, 0x37, 0x8e, 0xee, 0xfa
+        0xfa,
     ];
     for (i, b) in default_palette.into_iter().enumerate() {
         machine.poke(Word::from(palette_addr + i as u32), b);
@@ -107,12 +107,7 @@ fn draw_direct_high_gfx<P: PeekPoke>(machine: &P, reg: DisplayRegisters, frame:
         let (vulcan_row, vulcan_col) = ((display_row >> 2) as u32, (display_col >> 2) as u32);
 
         let vb = machine.peek(Word::from(to_byte_address((vulcan_col, vulcan_row), reg)));
-        let (red, green, blue) = (vb >> 5, (vb >> 2) & 7, (vb & 3) << 1);
+        paint_pixel(<&mut [u8; 4]>::try_from(pixel).unwrap(), vb)
-
-        pixel[0] = red << 5;
-        pixel[1] = green << 5;
-        pixel[2] = blue << 5;
-        pixel[3] = 0xff;
     }
 }
 
@@ -126,10 +121,7 @@ fn draw_paletted_high_gfx<P: PeekPoke>(machine: &P, reg: DisplayRegisters, frame
         let color = machine.peek(Word::from(reg.palette + color_idx as u32));
         let (red, green, blue) = (color >> 5, (color >> 2) & 7, (color & 3) << 1);
 
-        pixel[0] = red << 5;
+        paint_pixel(<&mut [u8; 4]>::try_from(pixel).unwrap(), color)
-        pixel[1] = green << 5;
-        pixel[2] = blue << 5;
-        pixel[3] = 0xff;
     }
 }
 
@@ -139,9 +131,11 @@ fn draw_paletted_high_text<P: PeekPoke>(machine: &P, reg: DisplayRegisters, fram
         let (vulcan_row, vulcan_col) = ((display_row >> 3) as u32, (display_col >> 3) as u32);
 
         let addr = to_byte_address((vulcan_col, vulcan_row), reg);
-        let char_idx= machine.peek(Word::from(addr));
+        let char_idx = machine.peek(Word::from(addr));
         let (char_row, char_col) = (display_row % 8, display_col % 8);
-        let char_byte = machine.peek(Word::from(reg.font + ((char_idx as u32) << 3) + char_row as u32));
+        let char_byte = machine.peek(Word::from(
+            reg.font + ((char_idx as u32) << 3) + char_row as u32,
+        ));
 
         let color_addr = addr + (reg.width * reg.height);
         let color_byte = machine.peek(Word::from(color_addr));
@@ -150,19 +144,21 @@ fn draw_paletted_high_text<P: PeekPoke>(machine: &P, reg: DisplayRegisters, fram
         let fg_color = machine.peek(Word::from(reg.palette + fg_color_idx as u32));
         let bg_color = machine.peek(Word::from(reg.palette + bg_color_idx as u32));
 
-        let (fg_red, fg_green, fg_blue) = (fg_color >> 5, (fg_color >> 2) & 7, (fg_color & 3) << 1);
+        draw_text_pixel(
-        let (bg_red, bg_green, bg_blue) = (bg_color >> 5, (bg_color >> 2) & 7, (bg_color & 3) << 1);
+            <&mut [u8; 4]>::try_from(pixel).unwrap(),
+            char_col as u8,
+            char_byte,
+            fg_color,
+            bg_color,
+        )
+    }
+}
 
-        if char_byte & (1 << (7 - char_col)) != 0 {
+fn draw_text_pixel(pixel: &mut [u8; 4], char_col: u8, char_byte: u8, fg_color: u8, bg_color: u8) {
-            pixel[0] = fg_red << 5;
+    if char_byte & (1 << (7 - char_col)) != 0 {
-            pixel[1] = fg_green << 5;
+        paint_pixel(pixel, fg_color)
-            pixel[2] = fg_blue << 5;
+    } else {
-        } else {
+        paint_pixel(pixel, bg_color)
-            pixel[0] = bg_red << 5;
-            pixel[1] = bg_green << 5;
-            pixel[2] = bg_blue << 5;
-        }
-        pixel[3] = 0xff;
     }
 }
 
@@ -172,25 +168,22 @@ fn draw_direct_high_text<P: PeekPoke>(machine: &P, reg: DisplayRegisters, frame:
         let (vulcan_row, vulcan_col) = ((display_row >> 3) as u32, (display_col >> 3) as u32);
 
         let addr = to_byte_address((vulcan_col, vulcan_row), reg);
-        let char_idx= machine.peek(Word::from(addr));
+        let char_idx = machine.peek(Word::from(addr));
         let (char_row, char_col) = (display_row % 8, display_col % 8);
-        let char_byte = machine.peek(Word::from(reg.font + ((char_idx as u32) << 3) + char_row as u32));
+        let char_byte = machine.peek(Word::from(
+            reg.font + ((char_idx as u32) << 3) + char_row as u32,
+        ));
 
         let color_addr = addr + (reg.width * reg.height);
         let color = machine.peek(Word::from(color_addr));
 
-        let (red, green, blue) = (color >> 5, (color >> 2) & 7, (color & 3) << 1);
+        draw_text_pixel(
-
+            <&mut [u8; 4]>::try_from(pixel).unwrap(),
-        if char_byte & (1 << (7 - char_col)) != 0 {
+            char_col as u8,
-            pixel[0] = red << 5;
+            char_byte,
-            pixel[1] = green << 5;
+            color,
-            pixel[2] = blue << 5;
+            0,
-        } else {
+        )
-            pixel[0] = 0;
-            pixel[1] = 0;
-            pixel[2] = 0;
-        }
-        pixel[3] = 0xff;
     }
 }
 
@@ -200,25 +193,22 @@ fn draw_direct_low_text<P: PeekPoke>(machine: &P, reg: DisplayRegisters, frame:
         let (vulcan_row, vulcan_col) = ((display_row >> 4) as u32, (display_col >> 4) as u32);
 
         let addr = to_byte_address((vulcan_col, vulcan_row), reg);
-        let char_idx= machine.peek(Word::from(addr));
+        let char_idx = machine.peek(Word::from(addr));
         let (char_row, char_col) = ((display_row / 2) % 8, (display_col / 2) % 8);
-        let char_byte = machine.peek(Word::from(reg.font + ((char_idx as u32) << 3) + char_row as u32));
+        let char_byte = machine.peek(Word::from(
+            reg.font + ((char_idx as u32) << 3) + char_row as u32,
+        ));
 
         let color_addr = addr + (reg.width * reg.height);
         let color = machine.peek(Word::from(color_addr));
 
-        let (red, green, blue) = (color >> 5, (color >> 2) & 7, (color & 3) << 1);
+        draw_text_pixel(
-
+            <&mut [u8; 4]>::try_from(pixel).unwrap(),
-        if char_byte & (1 << (7 - char_col)) != 0 {
+            char_col as u8,
-            pixel[0] = red << 5;
+            char_byte,
-            pixel[1] = green << 5;
+            color,
-            pixel[2] = blue << 5;
+            0,
-        } else {
+        )
-            pixel[0] = 0;
-            pixel[1] = 0;
-            pixel[2] = 0;
-        }
-        pixel[3] = 0xff;
     }
 }
 
@@ -228,9 +218,11 @@ fn draw_paletted_low_text<P: PeekPoke>(machine: &P, reg: DisplayRegisters, frame
         let (vulcan_row, vulcan_col) = ((display_row >> 4) as u32, (display_col >> 4) as u32);
 
         let addr = to_byte_address((vulcan_col, vulcan_row), reg);
-        let char_idx= machine.peek(Word::from(addr));
+        let char_idx = machine.peek(Word::from(addr));
         let (char_row, char_col) = ((display_row >> 1) % 8, (display_col >> 1) % 8);
-        let char_byte = machine.peek(Word::from(reg.font + ((char_idx as u32) << 3) + char_row as u32));
+        let char_byte = machine.peek(Word::from(
+            reg.font + ((char_idx as u32) << 3) + char_row as u32,
+        ));
 
         let color_addr = addr + (reg.width * reg.height);
         let color_byte = machine.peek(Word::from(color_addr));
@@ -239,19 +231,13 @@ fn draw_paletted_low_text<P: PeekPoke>(machine: &P, reg: DisplayRegisters, frame
         let fg_color = machine.peek(Word::from(reg.palette + fg_color_idx as u32));
         let bg_color = machine.peek(Word::from(reg.palette + bg_color_idx as u32));
 
-        let (fg_red, fg_green, fg_blue) = (fg_color >> 5, (fg_color >> 2) & 7, (fg_color & 3) << 1);
+        draw_text_pixel(
-        let (bg_red, bg_green, bg_blue) = (bg_color >> 5, (bg_color >> 2) & 7, (bg_color & 3) << 1);
+            <&mut [u8; 4]>::try_from(pixel).unwrap(),
-
+            char_col as u8,
-        if char_byte & (1 << (7 - char_col)) != 0 {
+            char_byte,
-            pixel[0] = fg_red << 5;
+            fg_color,
-            pixel[1] = fg_green << 5;
+            bg_color,
-            pixel[2] = fg_blue << 5;
+        )
-        } else {
-            pixel[0] = bg_red << 5;
-            pixel[1] = bg_green << 5;
-            pixel[2] = bg_blue << 5;
-        }
-        pixel[3] = 0xff;
     }
 }
 
@@ -259,23 +245,21 @@ fn draw_direct_low_gfx<P: PeekPoke>(machine: &P, reg: DisplayRegisters, frame: &
     for (i, pixel) in frame.chunks_exact_mut(4).enumerate() {
         let (display_row, display_col) = (i / 640, i % 640);
 
-        if display_row >= (240 - 64 * 3) && display_row < (240 + 64 * 3) &&
+        if display_row >= (240 - 64 * 3)
-            display_col >= (320 - 64 * 3) && display_col < (320 + 64 * 3) {
+            && display_row < (240 + 64 * 3)
-            let (vulcan_row, vulcan_col) = (((display_row - (240 - 64 * 3)) / 3) as u32, ((display_col - (320 - 64 * 3)) / 3) as u32);
+            && display_col >= (320 - 64 * 3)
+            && display_col < (320 + 64 * 3)
+        {
+            let (vulcan_row, vulcan_col) = (
+                ((display_row - (240 - 64 * 3)) / 3) as u32,
+                ((display_col - (320 - 64 * 3)) / 3) as u32,
+            );
 
             let vb = machine.peek(Word::from(to_byte_address((vulcan_col, vulcan_row), reg)));
-            let (red, green, blue) = (vb >> 5, (vb >> 2) & 7, (vb & 3) << 1);
+            paint_pixel(<&mut [u8; 4]>::try_from(pixel).unwrap(), vb)
-
-            pixel[0] = red << 5;
-            pixel[1] = green << 5;
-            pixel[2] = blue << 5;
         } else {
-            pixel[0] = 0;
+            paint_pixel(<&mut [u8; 4]>::try_from(pixel).unwrap(), 0)
-            pixel[1] = 0;
-            pixel[2] = 0;
         }
-
-        pixel[3] = 0xff;
     }
 }
 
@@ -283,23 +267,59 @@ fn draw_paletted_low_gfx<P: PeekPoke>(machine: &P, reg: DisplayRegisters, frame:
     for (i, pixel) in frame.chunks_exact_mut(4).enumerate() {
         let (display_row, display_col) = (i / 640, i % 640);
 
-        if display_row >= (240 - 64 * 3) && display_row < (240 + 64 * 3) &&
+        if display_row >= (240 - 64 * 3)
-            display_col >= (320 - 64 * 3) && display_col < (320 + 64 * 3) {
+            && display_row < (240 + 64 * 3)
-            let (vulcan_row, vulcan_col) = (((display_row - (240 - 64 * 3)) / 3) as u32, ((display_col - (320 - 64 * 3)) / 3) as u32);
+            && display_col >= (320 - 64 * 3)
+            && display_col < (320 + 64 * 3)
+        {
+            let (vulcan_row, vulcan_col) = (
+                ((display_row - (240 - 64 * 3)) / 3) as u32,
+                ((display_col - (320 - 64 * 3)) / 3) as u32,
+            );
 
-            let color_idx = machine.peek(Word::from(to_byte_address((vulcan_col, vulcan_row), reg)));
+            let color_idx =
+                machine.peek(Word::from(to_byte_address((vulcan_col, vulcan_row), reg)));
             let vb = machine.peek(Word::from(reg.palette + color_idx as u32));
-            let (red, green, blue) = (vb >> 5, (vb >> 2) & 7, (vb & 3) << 1);
+            paint_pixel(<&mut [u8; 4]>::try_from(pixel).unwrap(), vb);
-
-            pixel[0] = red << 5;
-            pixel[1] = green << 5;
-            pixel[2] = blue << 5;
         } else {
-            pixel[0] = 0;
+            paint_pixel(<&mut [u8; 4]>::try_from(pixel).unwrap(), 0);
-            pixel[1] = 0;
-            pixel[2] = 0;
         }
-
-        pixel[3] = 0xff;
     }
 }
+
+fn paint_pixel(pixel: &mut [u8; 4], vb: u8) {
+    let (red, green, blue) = (vb >> 5, (vb >> 2) & 7, vb & 3);
+
+    fn scale(b: u8) -> u8 {
+        let tmp = (b << 3) | b;
+        (tmp << 2) | (b & 3)
+    }
+
+    let blue_shifted2 = (blue << 2) | blue;
+    let blue_shifted = (blue_shifted2 << 4) | blue_shifted2;
+
+    pixel[0] = scale(red);
+    pixel[1] = scale(green);
+    pixel[2] = blue_shifted;
+    pixel[3] = 0xff;
+}
+
+#[test]
+fn test_paint_pixel() {
+    let mut p: [u8; 4] = [0, 0, 0, 0];
+
+    paint_pixel(&mut p, 0b11100000);
+    assert_eq!(p, [0xff, 0, 0, 255]);
+
+    paint_pixel(&mut p, 0b00011100);
+    assert_eq!(p, [0, 0xff, 0, 255]);
+
+    paint_pixel(&mut p, 0b00000011);
+    assert_eq!(p, [0, 0, 0xff, 255]);
+
+    paint_pixel(&mut p, 0xff);
+    assert_eq!(p, [0xff, 0xff, 0xff, 255]);
+
+    paint_pixel(&mut p, 0);
+    assert_eq!(p, [0, 0, 0, 255]);
+}

src/main.rs

@@ -1,23 +1,23 @@
-mod memory;
-mod word;
-mod opcodes;
-mod cpu;
 mod bus;
+mod cpu;
 mod display;
+mod memory;
+mod opcodes;
+mod word;
 
 use winit::{
-    event::{ Event, WindowEvent },
+    dpi::LogicalSize,
-    event_loop::{ EventLoop, ControlFlow },
+    event::{Event, WindowEvent},
+    event_loop::{ControlFlow, EventLoop},
     window::WindowBuilder,
-    dpi::LogicalSize
 };
 
+use crate::cpu::CPU;
+use crate::memory::{Memory, PeekPoke};
+use crate::word::Word;
 use pixels::{Pixels, SurfaceTexture};
 use std::time::Instant;
 use winit::window::Window;
-use crate::cpu::CPU;
-use crate::memory::{ Memory, PeekPoke };
-use crate::word::Word;
 
 fn main() {
     let event_loop = EventLoop::new();
@@ -47,11 +47,6 @@ fn main() {
         cpu.poke(Word::from(0x10000 + n), 0xff);
         cpu.poke(Word::from(0x10000 + 128 * n + 127), 0b00000011);
         cpu.poke(Word::from(0x10000 + 128 * 127 + n), 0b00011100);
-
-        cpu.poke(Word::from(0x10000 + 128 * n), 1);
-        cpu.poke(Word::from(0x10000 + n), 2);
-        cpu.poke(Word::from(0x10000 + 128 * n + 127), 3);
-        cpu.poke(Word::from(0x10000 + 128 * 127 + n), 4);
     }
     window_loop(event_loop, window, pixels, cpu)
 }
@@ -63,17 +58,19 @@ fn window_loop(event_loop: EventLoop<()>, window: Window, mut pixels: Pixels, mu
         match event {
             Event::WindowEvent {
                 event: WindowEvent::CloseRequested,
-                window_id
+                window_id,
-            } if window_id == window.id() => {
+            } if window_id == window.id() => *control_flow = ControlFlow::Exit,
-                *control_flow = ControlFlow::Exit
-            }
             Event::MainEventsCleared => {
                 let start = Instant::now();
                 draw(pixels.get_frame(), &mut cpu);
                 let draw_time = Instant::now() - start;
                 pixels.render().expect("Problem displaying framebuffer");
                 let total_time = Instant::now() - start;
-                println!("Tick took {} total, {} to draw", total_time.as_micros(), draw_time.as_micros());
+                println!(
+                    "Tick took {} total, {} to draw",
+                    total_time.as_micros(),
+                    draw_time.as_micros()
+                );
             }
             _ => {}
         }

src/memory.rs

@@ -1,17 +1,19 @@
-use rand::Rng;
 use crate::word::Word;
 use crate::word::MEM_SIZE;
+use rand::Rng;
 
 pub struct Memory([u8; MEM_SIZE as usize]);
 
 impl Default for Memory {
-    fn default() -> Self { Self([0u8; MEM_SIZE as usize]) }
+    fn default() -> Self {
+        Self([0u8; MEM_SIZE as usize])
+    }
 }
 
 impl From<Word> for usize {
     fn from(w: Word) -> Self {
         let w: u32 = w.into();
-        (w & (MEM_SIZE-1)) as usize
+        (w & (MEM_SIZE - 1)) as usize
     }
 }
 
@@ -54,15 +56,27 @@ pub trait PeekPoke {
         self.poke(addr + 2, (val >> 16) as u8);
     }
 
-    fn peek_u32(&self, addr: u32) -> u8 { self.peek(addr.into()) }
+    fn peek_u32(&self, addr: u32) -> u8 {
-    fn poke_u32(&mut self, addr: u32, val: u8) { self.poke(addr.into(), val) }
+        self.peek(addr.into())
-    fn peek24_u32(&mut self, addr: u32) -> u32 { self.peek24(addr.into()) }
+    }
-    fn poke24_u32(&mut self, addr: u32, val: u32) { self.poke24(addr.into(), val) }
+    fn poke_u32(&mut self, addr: u32, val: u8) {
+        self.poke(addr.into(), val)
+    }
+    fn peek24_u32(&mut self, addr: u32) -> u32 {
+        self.peek24(addr.into())
+    }
+    fn poke24_u32(&mut self, addr: u32, val: u32) {
+        self.poke24(addr.into(), val)
+    }
 }
 
 impl PeekPoke for Memory {
-    fn peek(&self, addr: Word) -> u8 { self[addr] }
+    fn peek(&self, addr: Word) -> u8 {
-    fn poke(&mut self, addr: Word, val: u8) { self[addr] = val; }
+        self[addr]
+    }
+    fn poke(&mut self, addr: Word, val: u8) {
+        self[addr] = val;
+    }
 }
 
 #[cfg(test)]

src/opcodes.rs

@@ -108,7 +108,7 @@ impl TryFrom<u8> for Opcode {
             40 => Popr,
             41 => Peekr,
             42 => Debug,
-            other => return Err(InvalidOpcode(other))
+            other => return Err(InvalidOpcode(other)),
         })
     }
 }

src/word.rs

@@ -5,11 +5,15 @@ pub const MEM_SIZE: u32 = 128 * 1024;
 pub struct Word(u32);
 
 impl From<u32> for Word {
-    fn from(a: u32) -> Self { Self(a & 0xffffff) }
+    fn from(a: u32) -> Self {
+        Self(a & 0xffffff)
+    }
 }
 
 impl From<Word> for u32 {
-    fn from(word: Word) -> Self { word.0 }
+    fn from(word: Word) -> Self {
+        word.0
+    }
 }
 
 impl std::ops::Add<i32> for Word {
@@ -21,20 +25,28 @@ impl std::ops::Add<i32> for Word {
 
 impl std::ops::Sub<i32> for Word {
     type Output = Word;
-    fn sub(self, rhs: i32) -> Self::Output { self + -rhs }
+    fn sub(self, rhs: i32) -> Self::Output {
+        self + -rhs
+    }
 }
 
 impl std::ops::Sub<Word> for Word {
     type Output = Word;
-    fn sub(self, rhs: Word) -> Self::Output { Word(self.0 - rhs.0) }
+    fn sub(self, rhs: Word) -> Self::Output {
+        Word(self.0 - rhs.0)
+    }
 }
 
 impl std::ops::SubAssign<i32> for Word {
-    fn sub_assign(&mut self, rhs: i32) { *self = *self - rhs; }
+    fn sub_assign(&mut self, rhs: i32) {
+        *self = *self - rhs;
+    }
 }
 
 impl std::ops::AddAssign<i32> for Word {
-    fn add_assign(&mut self, rhs: i32) { *self = *self + rhs; }
+    fn add_assign(&mut self, rhs: i32) {
+        *self = *self + rhs;
+    }
 }
 
 #[test]