diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock
index f10167e..d418b3b 100644
--- a/firmware/Cargo.lock
+++ b/firmware/Cargo.lock
@@ -30,6 +30,11 @@ dependencies = [
  "rustc_version 0.2.3 (registry+https://github.com/rust-lang/crates.io-index)",
 ]
 
+[[package]]
+name = "bit_field"
+version = "0.10.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+
 [[package]]
 name = "bitflags"
 version = "1.1.0"
@@ -123,6 +128,8 @@ dependencies = [
 name = "ionpak-firmware"
 version = "1.0.0"
 dependencies = [
+ "bit_field 0.10.0 (registry+https://github.com/rust-lang/crates.io-index)",
+ "byteorder 1.3.2 (registry+https://github.com/rust-lang/crates.io-index)",
  "cortex-m 0.5.10 (registry+https://github.com/rust-lang/crates.io-index)",
  "cortex-m-rt 0.6.10 (registry+https://github.com/rust-lang/crates.io-index)",
  "cortex-m-semihosting 0.3.5 (registry+https://github.com/rust-lang/crates.io-index)",
@@ -320,6 +327,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 "checksum aligned 0.3.1 (registry+https://github.com/rust-lang/crates.io-index)" = "d3a316c7ea8e1e9ece54862c992def5a7ac14de9f5832b69d71760680efeeefa"
 "checksum as-slice 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)" = "293dac66b274fab06f95e7efb05ec439a6b70136081ea522d270bc351ae5bb27"
 "checksum bare-metal 0.2.5 (registry+https://github.com/rust-lang/crates.io-index)" = "5deb64efa5bd81e31fcd1938615a6d98c82eafcbcd787162b6f63b91d6bac5b3"
+"checksum bit_field 0.10.0 (registry+https://github.com/rust-lang/crates.io-index)" = "a165d606cf084741d4ac3a28fb6e9b1eb0bd31f6cd999098cfddb0b2ab381dc0"
 "checksum bitflags 1.1.0 (registry+https://github.com/rust-lang/crates.io-index)" = "3d155346769a6855b86399e9bc3814ab343cd3d62c7e985113d46a0ec3c281fd"
 "checksum build_const 0.2.1 (registry+https://github.com/rust-lang/crates.io-index)" = "39092a32794787acd8525ee150305ff051b0aa6cc2abaf193924f5ab05425f39"
 "checksum byteorder 1.3.2 (registry+https://github.com/rust-lang/crates.io-index)" = "a7c3dd8985a7111efc5c80b44e23ecdd8c007de8ade3b96595387e812b957cf5"
diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml
index c76b4a9..b541278 100644
--- a/firmware/Cargo.toml
+++ b/firmware/Cargo.toml
@@ -16,6 +16,8 @@ tm4c129x = { version = "0.8", features = ["rt"] }
 embedded-hal = { version = "0.2", features = ["unproven"] }
 nb = "0.1"
 cortex-m-semihosting = "0.3"
+byteorder = { version = "1.3", default-features = false }
+bit_field = "0.10"
 
 [dependencies.smoltcp]
 git = "https://github.com/m-labs/smoltcp"
diff --git a/firmware/src/ad7172.rs b/firmware/src/ad7172.rs
index cd107e7..f9f7d8d 100644
--- a/firmware/src/ad7172.rs
+++ b/firmware/src/ad7172.rs
@@ -1,37 +1,167 @@
 use embedded_hal::digital::v2::OutputPin;
 use embedded_hal::blocking::spi::Transfer;
+use byteorder::{BigEndian, ByteOrder};
+use bit_field::BitField;
 
-#[allow(unused)]
-#[derive(Clone, Copy)]
+trait Register {
+    type Data: RegisterData;
+    fn address(&self) -> u8;
+}
+trait RegisterData {
+    fn empty() -> Self;
+    fn as_mut(&mut self) -> &mut [u8];
+}
+
+macro_rules! def_reg {
+    ($Reg: ident, $reg: ident, $addr: expr, $size: expr) => (
+        struct $Reg;
+        impl Register for $Reg {
+            type Data = $reg::Data;
+            fn address(&self) -> u8 {
+                $addr
+            }
+        }
+        mod $reg {
+            pub struct Data(pub [u8; $size]);
+            impl super::RegisterData for Data {
+                fn empty() -> Self {
+                    Data([0; $size])
+                }
+                fn as_mut(&mut self) -> &mut [u8] {
+                    &mut self.0
+                }
+            }
+        }
+    )
+}
+
+def_reg!(Status, status, 0x00, 1);
+impl status::Data {
+    /// Is there new data to read?
+    fn ready(&self) -> bool {
+        ! self.0[0].get_bit(7)
+    }
+
+    /// Channel for which data is ready
+    fn channel(&self) -> u8 {
+        self.0[0].get_bits(0..=1)
+    }
+
+    fn adc_error(&self) -> bool {
+        self.0[0].get_bit(6)
+    }
+
+    fn crc_error(&self) -> bool {
+        self.0[0].get_bit(5)
+    }
+
+    fn reg_error(&self) -> bool {
+        self.0[0].get_bit(4)
+    }
+}
+
+def_reg!(IfMode, if_mode, 0x02, 2);
+impl if_mode::Data {
+    fn set_crc(&mut self, mode: ChecksumMode) {
+        self.0[1].set_bits(2..=3, mode as u8);
+    }
+}
+
+def_reg!(Data, data, 0x04, 3);
+impl data::Data {
+    fn data(&self) -> u32 {
+        (u32::from(self.0[0]) << 16) |
+        (u32::from(self.0[1]) << 8) |
+        u32::from(self.0[2])
+    }
+}
+
+def_reg!(Id, id, 0x07, 2);
+impl id::Data {
+    fn id(&self) -> u16 {
+        BigEndian::read_u16(&self.0)
+    }
+}
+
+// #[allow(unused)]
+// #[derive(Clone, Copy)]
+// #[repr(u8)]
+// pub enum Register {
+//     Status = 0x00,
+//     AdcMode = 0x01,
+//     IfMode = 0x02,
+//     RegCheck = 0x03,
+//     Data = 0x04,
+//     GpioCon = 0x06,
+//     Id = 0x07,
+//     Ch0 = 0x10,
+//     Ch1 = 0x11,
+//     Ch2 = 0x12,
+//     Ch3 = 0x13,
+//     SetupCon0 = 0x20,
+//     SetupCon1 = 0x21,
+//     SetupCon2 = 0x22,
+//     SetupCon3 = 0x23,
+//     FiltCon0 = 0x28,
+//     FiltCon1 = 0x29,
+//     FiltCon2 = 0x2A,
+//     FiltCon3 = 0x2B,
+//     Offset0 = 0x30,
+//     Offset1 = 0x31,
+//     Offset2 = 0x32,
+//     Offset3 = 0x33,
+//     Gain0 = 0x38,
+//     Gain1 = 0x39,
+//     Gain2 = 0x3A,
+//     Gain3 = 0x3B,
+// }
+
+#[derive(Clone, Debug, PartialEq)]
+pub enum AdcError<SPI> {
+    SPI(SPI),
+    ChecksumMismatch(Option<u8>, Option<u8>),
+}
+
+impl<SPI> From<SPI> for AdcError<SPI> {
+    fn from(e: SPI) -> Self {
+        AdcError::SPI(e)
+    }
+}
+
+#[derive(Clone, Copy, PartialEq)]
 #[repr(u8)]
-pub enum Register {
-    Status = 0x00,
-    AdcMode = 0x01,
-    IfMode = 0x02,
-    RegCheck = 0x03,
-    Data = 0x04,
-    GpioCon = 0x06,
-    Id = 0x07,
-    Ch0 = 0x10,
-    Ch1 = 0x11,
-    Ch2 = 0x12,
-    Ch3 = 0x13,
-    SetupCon0 = 0x20,
-    SetupCon1 = 0x21,
-    SetupCon2 = 0x22,
-    SetupCon3 = 0x23,
-    FiltCon0 = 0x28,
-    FiltCon1 = 0x29,
-    FiltCon2 = 0x2A,
-    FiltCon3 = 0x2B,
-    Offset0 = 0x30,
-    Offset1 = 0x31,
-    Offset2 = 0x32,
-    Offset3 = 0x33,
-    Gain0 = 0x38,
-    Gain1 = 0x39,
-    Gain2 = 0x3A,
-    Gain3 = 0x3B,
+pub enum ChecksumMode {
+    Off = 0b00,
+    Xor = 0b01,
+    /// Not implemented
+    #[allow(unused)]
+    Crc = 0b10,
+}
+
+struct Checksum {
+    mode: ChecksumMode,
+    state: u8,
+}
+
+impl Checksum {
+    pub fn new(mode: ChecksumMode) -> Self {
+        Checksum { mode, state: 0 }
+    }
+    pub fn feed(&mut self, input: u8) {
+        match self.mode {
+            ChecksumMode::Off => {},
+            ChecksumMode::Xor => self.state ^= input,
+            ChecksumMode::Crc => {
+                // TODO
+            }
+        }
+    }
+    pub fn result(&self) -> Option<u8> {
+        match self.mode {
+            ChecksumMode::Off => None,
+            _ => Some(self.state)
+        }
+    }
 }
 
 /// AD7172-2 implementation
@@ -40,79 +170,141 @@ pub enum Register {
 pub struct Adc<SPI: Transfer<u8>, NSS: OutputPin> {
     spi: SPI,
     nss: NSS,
+    checksum_mode: ChecksumMode,
 }
 
 impl<SPI: Transfer<u8>, NSS: OutputPin> Adc<SPI, NSS> {
     pub fn new(spi: SPI, mut nss: NSS) -> Result<Self, SPI::Error> {
         let _ = nss.set_high();
-        let mut adc = Adc { spi, nss};
+        let mut adc = Adc {
+            spi, nss,
+            checksum_mode: ChecksumMode::Off,
+        };
         adc.reset()?;
 
-        let mut buf = [0, 0, 0];
-        adc.write_reg(Register::AdcMode, &mut buf)?;
-        let mut buf = [0, 1, 0];
-        adc.write_reg(Register::IfMode, &mut buf)?;
-        let mut buf = [0, 0, 0];
-        adc.write_reg(Register::GpioCon, &mut buf)?;
-
         Ok(adc)
     }
 
     /// `0x00DX` for AD7271-2
-    pub fn identify(&mut self) -> Option<u16> {
-        let mut buf = [0u8; 3];
-        self.read_reg(Register::Id, &mut buf)
-            .ok()
-            .map(|()| (u16::from(buf[1]) << 8) | u16::from(buf[2]))
+    pub fn identify(&mut self) -> Result<u16, AdcError<SPI::Error>> {
+        self.read_reg(&Id)
+            .map(|id| id.id())
     }
 
+    pub fn set_checksum_mode(&mut self, mode: ChecksumMode) -> Result<(), AdcError<SPI::Error>> {
+        let mut ifmode = self.read_reg(&IfMode)?;
+        ifmode.set_crc(mode);
+        self.checksum_mode = mode;
+        self.write_reg(&IfMode, &mut ifmode)?;
+        Ok(())
+    }
+
+    // pub fn setup(&mut self) -> Result<(), SPI::Error> {
+    //     let mut buf = [0, 0, 0];
+    //     adc.write_reg(Register::AdcMode, &mut buf)?;
+    //     let mut buf = [0, 1, 0];
+    //     adc.write_reg(Register::IfMode, &mut buf)?;
+    //     let mut buf = [0, 0, 0];
+    //     adc.write_reg(Register::GpioCon, &mut buf)?;
+
+    //     Ok(())
+    // }
+
     /// Returns the channel the data is from
-    pub fn data_ready(&mut self) -> Option<u8> {
-        let mut buf = [0u8; 2];
-        self.read_reg(Register::Status, &mut buf)
-            .ok()
-            .and_then(|()| {
-                if buf[1] & 0x80 == 0 {
-                    None
+    pub fn data_ready(&mut self) -> Result<Option<u8>, AdcError<SPI::Error>> {
+        self.read_reg(&Status)
+            .map(|status| {
+                if status.ready() {
+                    Some(status.channel())
                 } else {
-                    Some(buf[1] & 0x3)
+                    None
                 }
             })
     }
 
     /// Get data
-    pub fn read_data(&mut self) -> Result<u32, SPI::Error> {
-        let mut buf = [0u8; 4];
-        self.read_reg(Register::Data, &mut buf)?;
-        let result =
-            (u32::from(buf[1]) << 16) |
-            (u32::from(buf[2]) << 8) |
-            u32::from(buf[3]);
+    pub fn read_data(&mut self) -> Result<u32, AdcError<SPI::Error>> {
+        self.read_reg(&Data)
+            .map(|data| data.data())
+    }
+
+    fn read_reg<R: Register>(&mut self, reg: &R) -> Result<R::Data, AdcError<SPI::Error>> {
+        let mut reg_data = R::Data::empty();
+        let address = 0x40 | reg.address();
+        let mut checksum = Checksum::new(self.checksum_mode);
+        checksum.feed(address);
+        let checksum_out = checksum.result();
+        let checksum_in = self.transfer(address, reg_data.as_mut(), checksum_out)?;
+        for &mut b in reg_data.as_mut() {
+            checksum.feed(b);
+        }
+        let checksum_expected = checksum.result();
+        if checksum_expected != checksum_in {
+            return Err(AdcError::ChecksumMismatch(checksum_expected, checksum_in));
+        }
+        Ok(reg_data)
+    }
+
+    fn write_reg<R: Register>(&mut self, reg: &R, reg_data: &mut R::Data) -> Result<(), AdcError<SPI::Error>> {
+        let address = reg.address();
+        let checksum_out = match self.checksum_mode {
+            ChecksumMode::Off => None,
+            ChecksumMode::Xor => {
+                let mut xor = address;
+                for b in reg_data.as_mut() {
+                    xor ^= *b;
+                }
+                Some(xor)
+            }
+            ChecksumMode::Crc => panic!("Not implemented"),
+        };
+        self.transfer(address, reg_data.as_mut(), checksum_out)?;
+        Ok(())
+    }
+
+    fn update_reg<R, F, A>(&mut self, reg: &R, f: F) -> Result<A, AdcError<SPI::Error>>
+    where
+        R: Register,
+        F: FnOnce(&mut R::Data) -> A,
+    {
+        let mut reg_data = self.read_reg(reg)?;
+        let result = f(&mut reg_data);
+        self.write_reg(reg, &mut reg_data)?;
         Ok(result)
     }
 
-    fn read_reg(&mut self, reg: Register, buffer: &'_ mut [u8]) -> Result<(), SPI::Error> {
-        buffer[0] = 0x40 | (reg as u8);
-        self.transfer(buffer)?;
-        Ok(())
-    }
-
-    fn write_reg(&mut self, reg: Register, buffer: &'_ mut [u8]) -> Result<(), SPI::Error> {
-        buffer[0] = reg as u8;
-        self.transfer(buffer)?;
-        Ok(())
-    }
-
     pub fn reset(&mut self) -> Result<(), SPI::Error> {
         let mut buf = [0xFFu8; 8];
-        self.transfer(&mut buf)?;
+        let _ = self.nss.set_low();
+        let result = self.spi.transfer(&mut buf);
+        let _ = self.nss.set_high();
+        result?;
         Ok(())
     }
 
-    fn transfer<'w>(&mut self, words: &'w mut [u8]) -> Result<&'w [u8], SPI::Error> {
+    fn transfer<'w>(&mut self, addr: u8, reg_data: &'w mut [u8], checksum: Option<u8>) -> Result<Option<u8>, SPI::Error> {
+        let mut addr_buf = [addr];
+
         let _ = self.nss.set_low();
-        let result = self.spi.transfer(words);
+        let result = match self.spi.transfer(&mut addr_buf) {
+            Ok(_) => self.spi.transfer(reg_data),
+            Err(e) => Err(e),
+        };
+        let result = match (result, checksum) {
+            (Ok(_),None) =>
+                Ok(None),
+            (Ok(_), Some(checksum_out)) => {
+                let mut checksum_buf = [checksum_out; 1];
+                match self.spi.transfer(&mut checksum_buf) {
+                    Ok(_) => Ok(Some(checksum_buf[0])),
+                    Err(e) => Err(e),
+                }
+            }
+            (Err(e), _) =>
+                Err(e),
+        };
         let _ = self.nss.set_high();
+
         result
     }
 }
diff --git a/firmware/src/main.rs b/firmware/src/main.rs
index 95f6d50..6415e85 100644
--- a/firmware/src/main.rs
+++ b/firmware/src/main.rs
@@ -106,13 +106,28 @@ fn main() -> ! {
     loop {
         let r = adc.identify();
         match r {
-            None =>
-                writeln!(stdout, "Cannot identify ADC!").unwrap(),
-            Some(id) if id & 0xFFF0 == 0x00D0 => {
+            Err(e) =>
+                writeln!(stdout, "Cannot identify ADC: {:?}", e).unwrap(),
+            Ok(id) if id & 0xFFF0 == 0x00D0 => {
                 writeln!(stdout, "ADC id: {:04X}", id).unwrap();
                 break;
             }
-            Some(id) =>
+            Ok(id) =>
+                writeln!(stdout, "Corrupt ADC id: {:04X}", id).unwrap(),
+        };
+    }
+    writeln!(stdout, "AD7172: setting checksum mode to XOR").unwrap();
+    adc.set_checksum_mode(ad7172::ChecksumMode::Xor).unwrap();
+    loop {
+        let r = adc.identify();
+        match r {
+            Err(e) =>
+                writeln!(stdout, "Cannot identify ADC: {:?}", e).unwrap(),
+            Ok(id) if id & 0xFFF0 == 0x00D0 => {
+                writeln!(stdout, "ADC id: {:04X}", id).unwrap();
+                break;
+            }
+            Ok(id) =>
                 writeln!(stdout, "Corrupt ADC id: {:04X}", id).unwrap(),
         };
     }
@@ -165,21 +180,27 @@ fn main() -> ! {
     ];
 
     let mut time = 0i64;
-    let mut data = 0;
+    let mut data = None;
+    // if a socket has sent the latest data
     let mut socket_pending = [false; 8];
     loop {
         adc.data_ready()
-            .map(|channel| {
-                adc.read_data()
-                    .map(|new_data| {
-                        writeln!(stdout, "adc data: {:?}", new_data).unwrap();
-                        data = new_data;
-                        if channel == 0 {
-                            for p in socket_pending.iter_mut() {
-                                *p = true;
-                            }
+            .and_then(|channel|
+                channel.map(|channel|
+                    adc.read_data().map(|new_data| {
+                        data = Some(Ok((channel, new_data)));
+                        for p in socket_pending.iter_mut() {
+                            *p = true;
                         }
                     })
+                ).unwrap_or(Ok(()))
+            )
+            .map_err(|e| {
+                data = Some(Err(e));
+                for p in socket_pending.iter_mut() {
+                    *p = true;
+                }
+
             });
         for (&tcp_handle, pending) in handles.iter().zip(socket_pending.iter_mut()) {
             let socket = &mut *sockets.get::<TcpSocket>(tcp_handle);
@@ -188,7 +209,18 @@ fn main() -> ! {
             }
 
             if socket.may_send() && *pending {
-                let _ = writeln!(socket, "{}\r", data);
+                match &data {
+                    Some(Ok((channel, input))) => {
+                        let _ = writeln!(socket, "channel={} input={}\r", channel, input);
+                    }
+                    Some(Err(ad7172::AdcError::ChecksumMismatch(Some(expected), Some(input)))) => {
+                        let _ = writeln!(socket, "checksum_expected={:02X} checksum_input={:02X}\r", expected, input);
+                    }
+                    Some(Err(e)) => {
+                        let _ = writeln!(socket, "adc_error={:?}\r", e);
+                    }
+                    None => {}
+                }
                 *pending = false;
             }
         }