From c4486427c076c8c4e65c4b2a5a43bbee96d092c4 Mon Sep 17 00:00:00 2001
From: Ryan Kennedy <rkennedy9064@gmail.com>
Date: Sun, 22 Mar 2020 23:11:22 -0500
Subject: [PATCH] Faster set_pixel

---
 src/registers/graphics_controller.rs | 27 ++++++++++++++++++
 src/registers/mod.rs                 |  2 +-
 src/registers/sequencer.rs           |  6 +++-
 src/writers/graphics_640x480x16.rs   | 41 +++++++++-------------------
 4 files changed, 46 insertions(+), 30 deletions(-)

diff --git a/src/registers/graphics_controller.rs b/src/registers/graphics_controller.rs
index 5769ff9..057c3e7 100644
--- a/src/registers/graphics_controller.rs
+++ b/src/registers/graphics_controller.rs
@@ -73,6 +73,21 @@ impl From<GraphicsControllerIndex> for u8 {
     }
 }
 
+#[derive(Debug)]
+#[repr(u8)]
+pub enum WriteMode {
+    Mode0 = 0x0,
+    Mode1 = 0x1,
+    Mode2 = 0x2,
+    Mode3 = 0x3,
+}
+
+impl From<WriteMode> for u8 {
+    fn from(value: WriteMode) -> u8 {
+        value as u8
+    }
+}
+
 /// Represents the graphics controller registers on vga hardware.
 #[derive(Debug)]
 pub struct GraphicsControllerRegisters {
@@ -130,6 +145,18 @@ impl GraphicsControllerRegisters {
         );
     }
 
+    pub fn set_write_mode(&mut self, write_mode: WriteMode) {
+        let original_value = self.read(GraphicsControllerIndex::GraphicsMode) & 0xFC;
+        self.write(
+            GraphicsControllerIndex::GraphicsMode,
+            original_value | u8::from(write_mode),
+        );
+    }
+
+    pub fn set_bit_mask(&mut self, bit_mask: u8) {
+        self.write(GraphicsControllerIndex::BitMask, bit_mask);
+    }
+
     fn set_index(&mut self, index: GraphicsControllerIndex) {
         unsafe {
             self.grx_index.write(u8::from(index));
diff --git a/src/registers/mod.rs b/src/registers/mod.rs
index 32f0a80..c53850e 100644
--- a/src/registers/mod.rs
+++ b/src/registers/mod.rs
@@ -13,7 +13,7 @@ pub use attribute_controller::{AttributeControllerIndex, AttributeControllerRegi
 pub use color_palette::ColorPaletteRegisters;
 pub use crtc_controller::{CrtcControllerIndex, CrtcControllerRegisters};
 pub use general::GeneralRegisters;
-pub use graphics_controller::{GraphicsControllerIndex, GraphicsControllerRegisters};
+pub use graphics_controller::{GraphicsControllerIndex, GraphicsControllerRegisters, WriteMode};
 pub use sequencer::{PlaneMask, SequencerIndex, SequencerRegisters};
 
 const ST00_READ_ADDRESS: u16 = 0x3C2;
diff --git a/src/registers/sequencer.rs b/src/registers/sequencer.rs
index 1612dc9..a161929 100644
--- a/src/registers/sequencer.rs
+++ b/src/registers/sequencer.rs
@@ -96,7 +96,11 @@ impl SequencerRegisters {
 
     /// Sets the plane mask of the sequencer controller, as specified by `plane_mask`.
     pub fn set_plane_mask(&mut self, plane_mask: PlaneMask) {
-        self.write(SequencerIndex::PlaneMask, u8::from(plane_mask));
+        let original_value = self.read(SequencerIndex::PlaneMask);
+        self.write(
+            SequencerIndex::PlaneMask,
+            original_value | u8::from(plane_mask),
+        );
     }
 
     fn set_index(&mut self, index: SequencerIndex) {
diff --git a/src/writers/graphics_640x480x16.rs b/src/writers/graphics_640x480x16.rs
index ccb2514..faed7b8 100644
--- a/src/writers/graphics_640x480x16.rs
+++ b/src/writers/graphics_640x480x16.rs
@@ -1,9 +1,8 @@
 use crate::{
     colors::{Color16Bit, DEFAULT_PALETTE},
-    registers::PlaneMask,
+    registers::{PlaneMask, WriteMode},
     vga::{Vga, VideoMode, VGA},
 };
-use core::convert::TryInto;
 use spinning_top::SpinlockGuard;
 
 const WIDTH: usize = 640;
@@ -34,17 +33,16 @@ impl Graphics640x480x16 {
     }
 
     /// Clears the screen by setting all pixels to `Color16Bit::Black`.
-    pub fn clear_screen(&self) {
+    pub fn clear_screen(&self, color: Color16Bit) {
         let (mut vga, frame_buffer) = self.get_frame_buffer();
         vga.sequencer_registers
             .set_plane_mask(PlaneMask::ALL_PLANES);
+        vga.graphics_controller_registers.set_bit_mask(0xFF);
         vga.graphics_controller_registers
-            .write_enable_set_reset(PlaneMask::NONE);
+            .set_write_mode(WriteMode::Mode2);
         for offset in 0..ALL_PLANES_SCREEN_SIZE {
             unsafe {
-                frame_buffer
-                    .add(offset)
-                    .write_volatile(Color16Bit::Black as u8);
+                frame_buffer.add(offset).write_volatile(u8::from(color));
             }
         }
     }
@@ -53,27 +51,14 @@ impl Graphics640x480x16 {
     pub fn set_pixel(&self, x: usize, y: usize, color: Color16Bit) {
         let (mut vga, frame_buffer) = self.get_frame_buffer();
         let offset = x / 8 + (WIDTH / 8) * y;
-
-        // Store the current value for masking.
-        let x = x & 7;
-        let mask = 0x80 >> (x & 7);
-        let mut plane_mask = 0x01;
-
-        for plane in 0u8..4u8 {
-            vga.graphics_controller_registers
-                .write_read_plane(plane.try_into().unwrap());
-            vga.sequencer_registers
-                .set_plane_mask(plane.try_into().unwrap());
-            let current_value = unsafe { frame_buffer.add(offset).read_volatile() };
-            let new_value = if plane_mask & color as u8 != 0 {
-                current_value | mask
-            } else {
-                current_value & !mask
-            };
-            unsafe {
-                frame_buffer.add(offset).write_volatile(new_value);
-            }
-            plane_mask <<= 1;
+        let pixel_offset = x & 7;
+        vga.graphics_controller_registers
+            .set_write_mode(WriteMode::Mode2);
+        vga.graphics_controller_registers
+            .set_bit_mask(1 << pixel_offset);
+        unsafe {
+            frame_buffer.add(offset).read_volatile();
+            frame_buffer.add(offset).write_volatile(u8::from(color));
         }
     }