diff --git a/src/writers/graphics_640x480x16.rs b/src/writers/graphics_640x480x16.rs index e2f57ec..e471cd4 100644 --- a/src/writers/graphics_640x480x16.rs +++ b/src/writers/graphics_640x480x16.rs @@ -2,6 +2,7 @@ use crate::{ colors::{Color16Bit, DEFAULT_PALETTE}, vga::{PlaneMask, Vga, VideoMode, VGA}, }; +use core::convert::TryInto; use spinning_top::SpinlockGuard; const WIDTH: usize = 640; @@ -50,20 +51,24 @@ impl Graphics640x480x16 { let (mut vga, frame_buffer) = self.get_frame_buffer(); let offset = x / 8 + (WIDTH / 8) * y; - // Write to all 4 planes - vga.set_plane_mask(PlaneMask::ALL_PLANES); + // Store the current value for masking. + let x = x & 7; + let mask = 0x80 >> (x & 7); + let mut plane_mask = 0x01; - // Set the bits we want set/reset to the color - vga.set_graphics_set_reset(color); - - // Enable set/reset for all planes - vga.set_graphics_enable_set_reset(PlaneMask::ALL_PLANES); - unsafe { - // In write mode 0, when enable set/reset is turned on, cpu data - // is replaced with the data from the set/reset register. Since - // we're using set/reset for all 4 planes, it doesn't matter what value - // we write to the memory address. - frame_buffer.add(offset).write(0x0); + for plane in 0u8..4u8 { + vga.set_read_plane(plane.try_into().unwrap()); + vga.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; } }