From 5a394621f9ff5da52d4aa3ee75fb00745f0b4db9 Mon Sep 17 00:00:00 2001
From: Daniel Schaefer <git@danielschaefer.me>
Date: Mon, 18 Sep 2023 20:23:25 +0800
Subject: [PATCH] Make sure sw_enablement doesn't overwrite

Signed-off-by: Daniel Schaefer <git@danielschaefer.me>
---
 src/devices.rs |  4 ++++
 src/lib.rs     | 20 +++++++++++++++++++-
 2 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/src/devices.rs b/src/devices.rs
index 301f19f..0af91ef 100644
--- a/src/devices.rs
+++ b/src/devices.rs
@@ -5,6 +5,7 @@ use crate::{Error, IS31FL3741};
 use core::convert::TryFrom;
 #[allow(unused_imports)]
 use embedded_hal::blocking::delay::DelayMs;
+use embedded_hal::blocking::i2c::Read;
 #[allow(unused_imports)]
 use embedded_hal::blocking::i2c::Write;
 
@@ -20,6 +21,7 @@ use embedded_graphics_core::{pixelcolor::Rgb888, prelude::*, primitives::Rectang
 impl<I2C, I2cError> Dimensions for AdafruitRGB13x9<I2C>
 where
     I2C: Write<Error = I2cError>,
+    I2C: Read<Error = I2cError>,
 {
     fn bounding_box(&self) -> Rectangle {
         Rectangle::new(Point::zero(), Size::new(13, 9))
@@ -30,6 +32,7 @@ where
 impl<I2C, I2cError> DrawTarget for AdafruitRGB13x9<I2C>
 where
     I2C: Write<Error = I2cError>,
+    I2C: Read<Error = I2cError>,
     I2cError:,
 {
     type Color = Rgb888;
@@ -57,6 +60,7 @@ where
 impl<I2C, I2cError> AdafruitRGB13x9<I2C>
 where
     I2C: Write<Error = I2cError>,
+    I2C: Read<Error = I2cError>,
 {
     pub fn unwrap(self) -> I2C {
         self.device.i2c
diff --git a/src/lib.rs b/src/lib.rs
index 4ab7ce8..2421849 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -4,6 +4,7 @@
 pub mod devices;
 
 use embedded_hal::blocking::delay::DelayMs;
+use embedded_hal::blocking::i2c::Read;
 use embedded_hal::blocking::i2c::Write;
 
 /// A struct to integrate with a new IS31FL3741 powered device.
@@ -25,6 +26,7 @@ pub struct IS31FL3741<I2C> {
 impl<I2C, I2cError> IS31FL3741<I2C>
 where
     I2C: Write<Error = I2cError>,
+    I2C: Read<Error = I2cError>,
 {
     /// Fill all pixels of the display at once. The brightness should range from 0 to 255.
     pub fn fill_matrix(&mut self, brightnesses: &[u8]) -> Result<(), I2cError> {
@@ -125,7 +127,10 @@ where
 
     /// How many SW rows to enable
     pub fn sw_enablement(&mut self, setting: SwSetting) -> Result<(), I2cError> {
-        let config_register = 1; // Shutdown disable
+        let config_register = self.read_register(
+            Page::Config,
+            addresses::CONFIG_REGISTER,
+        )?;
 
         let new_val = (config_register & 0x0F) | (setting as u8) << 4;
         self.write_register(Page::Config, addresses::CONFIG_REGISTER, new_val)?;
@@ -147,6 +152,19 @@ where
         Ok(())
     }
 
+    fn read_u8(&mut self, register: u8) -> Result<u8, I2cError> {
+        let mut buf = [0x00];
+        self.i2c.write(self.address, &[register])?;
+        self.i2c.read(self.address, &mut buf)?;
+        Ok(buf[0])
+    }
+
+    fn read_register(&mut self, bank: Page, register: u8) -> Result<u8, I2cError> {
+        self.bank(bank)?;
+        let value = self.read_u8(register)?;
+        Ok(value)
+    }
+
     fn bank(&mut self, bank: Page) -> Result<(), I2cError> {
         self.unlock()?;
         self.write(&[addresses::PAGE_SELECT_REGISTER, bank as u8])?;