From 587c2cefe5d8fe777be8ad371064193121194cec Mon Sep 17 00:00:00 2001
From: Sofia <sofia@teascade.net>
Date: Fri, 29 May 2026 17:23:08 +0300
Subject: [PATCH] Set RegMisc in reset

---
 src/main.rs   | 39 +++++++++-----------------
 src/sx1509.rs | 77 +++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 90 insertions(+), 26 deletions(-)
 create mode 100644 src/sx1509.rs

diff --git a/src/main.rs b/src/main.rs
index 5e15a61..79e9192 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -34,6 +34,7 @@ use crate::{
     font::FontRenderer,
     state::StateManager,
     states::InitATState,
+    sx1509::{Register, Sx1509},
 };
 
 extern crate alloc;
@@ -44,6 +45,7 @@ mod display;
 mod font;
 mod state;
 mod states;
+mod sx1509;
 
 // This creates a default app-descriptor required by the esp-idf bootloader.
 // For more information see: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
@@ -127,33 +129,18 @@ fn main() -> ! {
     .with_rx(peripherals.GPIO7)
     .with_tx(peripherals.GPIO8);
 
-    let mut i2c = I2c::new(
-        peripherals.I2C0,
-        i2c::master::Config::default().with_frequency(Rate::from_khz(400)),
-    )
-    .unwrap()
-    .with_scl(peripherals.GPIO20)
-    .with_sda(peripherals.GPIO22);
+    let mut sx1509 = Sx1509::from(
+        I2c::new(
+            peripherals.I2C0,
+            i2c::master::Config::default().with_frequency(Rate::from_khz(400)),
+        )
+        .unwrap()
+        .with_scl(peripherals.GPIO20)
+        .with_sda(peripherals.GPIO22),
+        Output::new(peripherals.GPIO15, Level::High, OutputConfig::default()),
+    );
 
-    let mut i2c_rst = Output::new(peripherals.GPIO15, Level::High, OutputConfig::default());
-
-    let test_delay = Delay::new();
-
-    i2c_rst.set_low();
-    test_delay.delay_millis(1000);
-    i2c_rst.set_high();
-
-    log::info!("I2C reset");
-
-    let REG_INTERRUPT_MASK_A = 0x13;
-    let default_address = 0x3e;
-
-    let write_buf = [REG_INTERRUPT_MASK_A];
-    let mut read_buf = [0, 0];
-    i2c.write_read(default_address, &write_buf, &mut read_buf)
-        .unwrap();
-
-    log::info!("Read: {:?}", read_buf);
+    sx1509.init();
 
     let mut at_commands = ATCommands::new(sim_rst, sim_pwr_key, uart);
 
diff --git a/src/sx1509.rs b/src/sx1509.rs
new file mode 100644
index 0000000..342cb52
--- /dev/null
+++ b/src/sx1509.rs
@@ -0,0 +1,77 @@
+use esp_hal::{Blocking, delay::Delay, gpio::Output, i2c::master::I2c};
+
+pub enum Register {
+    RegInterruptMaskA = 0x13,
+
+    RegMisc = 0x1F,
+}
+
+pub struct Sx1509<'d> {
+    i2c: I2c<'d, Blocking>,
+    rst: Output<'d>,
+    delay: Delay,
+    address: u8,
+}
+
+impl<'d> Sx1509<'d> {
+    pub fn from(i2c: I2c<'d, Blocking>, rst: Output<'d>) -> Sx1509<'d> {
+        Sx1509 {
+            i2c,
+            rst,
+            delay: Delay::new(),
+            address: 0x3e,
+        }
+    }
+
+    pub fn reset(&mut self) {
+        let mut reg_misc = self.read_8(Register::RegMisc);
+        if (reg_misc & (1 << 2)) != 0 {
+            reg_misc &= !(1 << 2);
+            self.write_8(Register::RegMisc, reg_misc);
+        }
+
+        self.rst.set_low();
+        self.delay.delay_millis(1000);
+        self.rst.set_high();
+        log::info!("SX1509 reset");
+    }
+
+    pub fn init(&mut self) -> Result<(), ()> {
+        self.reset();
+        let int = self.read_16(Register::RegInterruptMaskA);
+        let res = if int == 0xFF00 { Ok(()) } else { Err(()) };
+
+        log::info!("SX1509 initialized");
+
+        res
+    }
+
+    pub fn write_8(&mut self, register: Register, value: u8) {
+        self.i2c
+            .write(self.address, &[register as u8, value])
+            .unwrap();
+    }
+
+    pub fn write_16(&mut self, register: Register, value: u16) {
+        let [byte1, byte2] = value.to_be_bytes();
+        self.i2c
+            .write(self.address, &[register as u8, byte1, byte2])
+            .unwrap();
+    }
+
+    pub fn read_8(&mut self, register: Register) -> u8 {
+        let mut buffer = [0u8; 1];
+        self.i2c
+            .write_read(self.address, &[register as u8], &mut buffer)
+            .unwrap();
+        buffer[0]
+    }
+
+    pub fn read_16(&mut self, register: Register) -> u16 {
+        let mut buffer = [0u8; 2];
+        self.i2c
+            .write_read(self.address, &[register as u8], &mut buffer)
+            .unwrap();
+        ((buffer[0] as u16) << 8) | (buffer[1] as u16)
+    }
+}