diff --git a/src/ad5940.rs b/src/ad5940.rs
new file mode 100644
index 0000000..646a21b
--- /dev/null
+++ b/src/ad5940.rs
@@ -0,0 +1,51 @@
+use embassy_stm32::{gpio::Output, mode::Blocking, spi::Spi};
+
+pub struct AD5940 {
+    spi: Spi<'static, Blocking>,
+    cs: Output<'static>,
+}
+
+impl AD5940 {
+    pub fn new(spi: Spi<'static, Blocking>, cs: Output<'static>) -> Self {
+        AD5940 { spi, cs }
+    }
+
+    fn read_reg_16(&mut self, address: u16) -> Result<u16, embassy_stm32::spi::Error> {
+        let addr_bytes = address.to_be_bytes();
+
+        // Write address command
+        self.cs.set_low();
+        self.spi.blocking_write(&[Command::SPICMD_SETADDR as u8])?;
+        self.spi.blocking_write(&addr_bytes)?;
+        self.cs.set_high();
+
+        // Read command
+        self.cs.set_low();
+        self.spi.blocking_write(&[Command::SPICMD_READREG as u8])?;
+
+        let mut data = [0u8; 3]; // First byte dummy, then two data bytes
+        self.spi.blocking_read(&mut data)?;
+        self.cs.set_high();
+
+        Ok(u16::from_be_bytes([data[1], data[2]]))
+    }
+
+    pub fn get_chipid(&mut self) -> Result<u16, embassy_stm32::spi::Error> {
+        self.read_reg_16(Register::CHIPID as u16)
+    }
+}
+
+#[allow(dead_code)]
+#[allow(non_camel_case_types)]
+enum Command {
+    SPICMD_SETADDR = 0x20,
+    SPICMD_READREG = 0x6D,
+    SPICMD_WRITEREG = 0x2D,
+    SPICMD_READFIFO = 0x5F,
+}
+
+#[allow(non_camel_case_types)]
+#[repr(u16)]
+enum Register {
+    CHIPID = 0x0000_0404, // Changed from u32 to u16, as expected by SPI write
+}
diff --git a/src/main.rs b/src/main.rs
index 4113308..6c5b5d0 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -4,23 +4,57 @@
 use defmt::*;
 use embassy_executor::Spawner;
 use embassy_stm32::gpio::{Level, Output, Speed};
+use embassy_stm32::{spi, Config};
 use embassy_time::Timer;
 use {defmt_rtt as _, panic_probe as _};
 
+mod ad5940;
+use ad5940::AD5940;
+
 #[embassy_executor::main]
 async fn main(_spawner: Spawner) {
-    let p = embassy_stm32::init(Default::default());
+
+    let mut config = Config::default();
+    {
+        use embassy_stm32::rcc::*;
+        config.rcc.pll1 = Some(Pll {
+            source: PllSource::HSI,
+            prediv: PllPreDiv::DIV4,
+            mul: PllMul::MUL8,
+            divp: Some(PllDiv::DIV2),
+            divq: Some(PllDiv::DIV2),
+            divr: Some(PllDiv::DIV2),
+        })
+    }
+
+    let p = embassy_stm32::init(config);
     info!("Hello World!");
 
-    let mut led = Output::new(p.PA5, Level::High, Speed::Low);
+    // let mut led = Output::new(p.PA5, Level::High, Speed::Low);
+
+    let cs = Output::new(p.PC9, Level::High, Speed::Low);
+
+    let spi = spi::Spi::new_blocking(
+        p.SPI1,
+        p.PA5, // SCK
+        p.PA7, // MOSI
+        p.PA6, // MISO
+        spi::Config::default()
+    );
+
+    let mut ad5940 = AD5940::new(spi, cs);
 
     loop {
-        info!("high");
-        led.set_high();
+        // Read chip id
+        let chip_id = ad5940.get_chipid().unwrap();
+        info!("Chip ID: 0x{:04X}", chip_id);
+
+        // info!("high");
+        // led.set_high();
         Timer::after_millis(500).await;
 
-        info!("low");
-        led.set_low();
+        // info!("low");
+        // led.set_low();
         Timer::after_millis(500).await;
     }
 }
\ No newline at end of file