diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml
index 000bb2c..2e30dd9 100644
--- a/.github/workflows/rust.yml
+++ b/.github/workflows/rust.yml
@@ -20,3 +20,12 @@ jobs:
       run: cargo build --verbose
     - name: Run tests
       run: cargo test --verbose
+
+  keep-in-sync:
+
+    runs-on: ubuntu-latest
+
+    steps:
+      - uses: actions/checkout@v4
+      - name: Check that and async versions are in sync
+        run: make check-async-sync
diff --git a/Cargo.toml b/Cargo.toml
index 07c5e60..cd0c969 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -14,3 +14,8 @@ exclude = ["examples/nrf52840/.cargo"]
 [dependencies]
 embedded-hal = "1.0.0"
 fixedvec = "0.2.4"
+embedded-hal-async = {version = "1.0.0", optional = true }
+
+
+[features]
+async = ["dep:embedded-hal-async"]
diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..13cd0a9
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,22 @@
+
+# When you've made changes to the async version, updating the sync version is trivial
+.PHONY: async-to-sync
+async-to-sync:
+	cat src/interface_async.rs | sed -e 's/[.]await//' -e 's/async //' -e 's/_async//' | grep -vF '#[allow(async_fn_in_trait)]' | rustfmt > src/interface.rs
+	cat src/bmi2_async.rs | sed -e 's/[.]await//' -e 's/async //' -e 's/_async//' | grep -vF '#[allow(async_fn_in_trait)]' | rustfmt > src/bmi2.rs
+
+# When you've made changes to the sync version, updating the async version is not always as clear
+# because there are many possible places to insert async and await, so the best we can do is check.
+.PHONY: check-async-sync
+check-async-sync:
+	# Everything must be formatted before we start, because the code might reflow differently in
+	# sync vs async versions. For local dev we do it automatically. For CI we just check it.
+	if [ "${CI}" = "" ]; then cargo fmt; fi
+	cargo fmt --check
+	# Strip out the async-related keywords, then format, then diff against the sync version.
+	cat src/interface_async.rs | sed -e 's/[.]await//' -e 's/async //' -e 's/_async//' | grep -vF '#[allow(async_fn_in_trait)]' | rustfmt > /tmp/stripped-interface_async.rs
+	diff -u /tmp/stripped-interface_async.rs src/interface.rs
+	cat src/bmi2_async.rs | sed -e 's/[.]await//' -e 's/async //' -e 's/_async//' | grep -vF '#[allow(async_fn_in_trait)]' | rustfmt > /tmp/stripped-bmi2_async.rs
+	diff -u /tmp/stripped-bmi2_async.rs src/bmi2.rs
+	# Make sure everything actually compiles once we're done.
+	cargo check --all-features
diff --git a/README.md b/README.md
index 2c4ea28..a606784 100644
--- a/README.md
+++ b/README.md
@@ -8,32 +8,39 @@ This is an [embedded-hal](https://github.com/rust-embedded/embedded-hal) driver
 
 ```rust
 // ...
-
 use bmi2::Bmi2;
 use bmi2::config;
 use bmi2::{types::Burst, I2cAddr, types::PwrCtrl};
+use embedded_hal::delay::DelayNs;
+
+// Specify your delay type, for example:
+let delay = MyDelay::new(); // Replace with your actual delay implementation
+
+// Choose a buffer size (e.g., 512 bytes), needs to be >= max data burst
+const BUFFER_SIZE: usize = 512;
 
 /// Create a new Bmi2 device using I2C with its alternative address (0x69).
 /// Configure the max data burst to 255 bytes:
 /// - used for the upload of the configuration during initialization.
-/// - This is a limitation from your device or its HAL. 
-let mut bmi = Bmi2::new_i2c(i2c, I2cAddr::Alternative, Burst::Other(255));
+/// - This is a limitation from your device or its HAL.
+let mut bmi = Bmi2::<_, _, BUFFER_SIZE>::new_i2c(
+    i2c, 
+    delay,
+    I2cAddr::Alternative, 
+    Burst::new(255),
+);
 
 /// Get the chip id. Should be 0x24 or 36 in decimal
 let chip_id = bmi.get_chip_id().unwrap();
-
 /// Initialize the senor.
 /// During this process a configuration of > 8kB is uploaded to the sensor.
 /// Alternatively, for the BMI260 call its dedicated config:
 /// bmi.init(&config::BMI260_CONFIG_FILE).unwrap();
 bmi.init(&config::BMI270_CONFIG_FILE).unwrap();
-
 /// Enable power for the accelerometer and the gyroscope.
 let pwr_ctrl = PwrCtrl { aux_en: false, gyr_en: true, acc_en: true, temp_en: false };
 bmi.set_pwr_ctrl(pwr_ctrl).unwrap();
-
 /// Read the raw data
 let data = bmi.get_data().unwrap();
-
 // ...
 ```
diff --git a/examples/nrf52840/basic_read/src/main.rs b/examples/nrf52840/basic_read/src/main.rs
index 47f7ada..6d403ca 100644
--- a/examples/nrf52840/basic_read/src/main.rs
+++ b/examples/nrf52840/basic_read/src/main.rs
@@ -11,6 +11,7 @@ use nrf52840_hal::{
     pac,
     timer::Timer,
     twim::{self, Twim},
+    Delay,
 };
 
 use defmt_rtt as _;
@@ -22,6 +23,7 @@ use bmi2::{types::Burst, types::PwrCtrl, I2cAddr};
 #[entry]
 fn main() -> ! {
     let p = pac::Peripherals::take().unwrap();
+    let core = pac::CorePeripherals::take().unwrap();
 
     let mut timer = Timer::new(p.TIMER0);
     let port0 = p0::Parts::new(p.P0);
@@ -33,7 +35,11 @@ fn main() -> ! {
 
     let i2c = Twim::new(p.TWIM0, twim_pins, twim::Frequency::K100);
 
-    let mut bmi = Bmi2::new_i2c(i2c, I2cAddr::Alternative, Burst::Other(255));
+    let delay = Delay::new(core.SYST);
+
+    const BUFFER_SIZE: usize = 256;
+
+    let mut bmi = Bmi2::<_, _, BUFFER_SIZE>::new_i2c(i2c, delay, I2cAddr::Alternative, Burst::new(255));
     let chip_id = bmi.get_chip_id().unwrap();
     defmt::info!("chip id: {}", chip_id);
 
diff --git a/src/bmi2.rs b/src/bmi2.rs
index ce4515b..a0a24a0 100644
--- a/src/bmi2.rs
+++ b/src/bmi2.rs
@@ -1,30 +1,37 @@
+use embedded_hal::delay::DelayNs;
 use fixedvec::FixedVec;
 
-use crate::interface::{I2cAddr, I2cInterface, ReadData, SpiInterface, WriteData};
 use crate::registers::Registers;
+use crate::{
+    interface::{I2cInterface, ReadData, SpiInterface, WriteData},
+    interface_common::I2cAddr,
+};
 
 use crate::types::{
     AccConf, AccOffsets, AccRange, AccSelfTest, AuxConf, AuxData, AuxIfConf, AxisData, Burst, Cmd,
     Data, Drv, Error, ErrorReg, ErrorRegMsk, Event, FifoConf, FifoDowns, GyrConf, GyrCrtConf,
     GyrOffsets, GyrRange, GyrSelfTest, IfConf, IntIoCtrl, IntLatch, IntMapData, IntMapFeat,
     InternalError, InternalStatus, InterruptStatus, NvConf, PullUpConf, PwrConf, PwrCtrl,
-    Saturation, Status, WristGestureActivity, FIFO_LENGTH_1_MASK,
+    Saturation, Status, WristGestureActivity, BMI160_CHIP_ID, BMI260_CHIP_ID, BMI270_CHIP_ID,
+    FIFO_LENGTH_1_MASK,
 };
 
-pub struct Bmi2<I> {
+pub struct Bmi2<I, D, const N: usize> {
     iface: I,
     max_burst: u16,
+    delay: D,
 }
 
-impl<I2C> Bmi2<I2cInterface<I2C>> {
+impl<I2C, D, const N: usize> Bmi2<I2cInterface<I2C>, D, N> {
     /// Create a new Bmi270 device with I2C communication.
-    pub fn new_i2c(i2c: I2C, address: I2cAddr, burst: Burst) -> Self {
+    pub fn new_i2c(i2c: I2C, delay: D, address: I2cAddr, burst: Burst) -> Self {
         Bmi2 {
             iface: I2cInterface {
                 i2c,
                 address: address.addr(),
             },
             max_burst: burst.val(),
+            delay,
         }
     }
 
@@ -34,12 +41,16 @@ impl<I2C> Bmi2<I2cInterface<I2C>> {
     }
 }
 
-impl<SPI> Bmi2<SpiInterface<SPI>> {
+impl<SPI, D, const N: usize> Bmi2<SpiInterface<SPI>, D, N>
+where
+    D: DelayNs,
+{
     /// Create a new Bmi270 device with SPI communication.
-    pub fn new_spi(spi: SPI, burst: Burst) -> Self {
+    pub fn new_spi(spi: SPI, delay: D, burst: Burst) -> Self {
         Bmi2 {
             iface: SpiInterface { spi },
             max_burst: burst.val(),
+            delay,
         }
     }
 
@@ -49,9 +60,10 @@ impl<SPI> Bmi2<SpiInterface<SPI>> {
     }
 }
 
-impl<I, CommE> Bmi2<I>
+impl<I, D, CommE, const N: usize> Bmi2<I, D, N>
 where
     I: ReadData<Error = Error<CommE>> + WriteData<Error = Error<CommE>>,
+    D: embedded_hal::delay::DelayNs, // Add constraint for D
 {
     /// Get the chip id.
     pub fn get_chip_id(&mut self) -> Result<u8, Error<CommE>> {
@@ -149,9 +161,7 @@ where
     }
 
     /// Get the detected wrist gesture and activity type.
-    pub fn get_wrist_gesture_activity(
-        &mut self,
-    ) -> Result<WristGestureActivity, Error<CommE>> {
+    pub fn get_wrist_gesture_activity(&mut self) -> Result<WristGestureActivity, Error<CommE>> {
         let wr_gest_acc = self.iface.read_reg(Registers::WR_GEST_ACT)?;
 
         Ok(WristGestureActivity::from_reg(wr_gest_acc))
@@ -426,10 +436,7 @@ where
     }
 
     /// Set interrupt 1 feature mapping.
-    pub fn set_int1_map_feat(
-        &mut self,
-        int1_map_feat: IntMapFeat,
-    ) -> Result<(), Error<CommE>> {
+    pub fn set_int1_map_feat(&mut self, int1_map_feat: IntMapFeat) -> Result<(), Error<CommE>> {
         let reg = int1_map_feat.to_reg();
         self.iface.write_reg(Registers::INT1_MAP_FEAT, reg)?;
         Ok(())
@@ -442,10 +449,7 @@ where
     }
 
     /// Set interrupt 2 feature mapping.
-    pub fn set_int2_map_feat(
-        &mut self,
-        int2_map_feat: IntMapFeat,
-    ) -> Result<(), Error<CommE>> {
+    pub fn set_int2_map_feat(&mut self, int2_map_feat: IntMapFeat) -> Result<(), Error<CommE>> {
         let reg = int2_map_feat.to_reg();
         self.iface.write_reg(Registers::INT2_MAP_FEAT, reg)?;
         Ok(())
@@ -581,10 +585,7 @@ where
     }
 
     /// Set the accelerometer self test configuration.
-    pub fn set_acc_self_test(
-        &mut self,
-        acc_self_test: AccSelfTest,
-    ) -> Result<(), Error<CommE>> {
+    pub fn set_acc_self_test(&mut self, acc_self_test: AccSelfTest) -> Result<(), Error<CommE>> {
         self.iface
             .write_reg(Registers::ACC_SELF_TEST, acc_self_test.to_reg())?;
         Ok(())
@@ -703,46 +704,107 @@ where
         Ok(())
     }
 
+    /// Disable power save mode.
+    pub fn disable_power_save(&mut self) -> Result<(), Error<CommE>> {
+        let mut pwr_conf = self.get_pwr_conf()?;
+        pwr_conf.power_save = false;
+        self.set_pwr_conf(pwr_conf)?;
+        // Critical delay after disabling power save
+        self.delay.delay_us(450);
+        Ok(())
+    }
+
+    /// Enable power save mode.
+    pub fn enable_power_save(&mut self) -> Result<(), Error<CommE>> {
+        let mut pwr_conf = self.get_pwr_conf()?;
+        pwr_conf.power_save = true;
+        self.set_pwr_conf(pwr_conf)?;
+        // Critical delay after enabling power save
+        self.delay.delay_us(450);
+        Ok(())
+    }
+
     /// Initialize sensor.
     pub fn init(&mut self, config_file: &[u8]) -> Result<(), Error<CommE>> {
+        // Verify chip ID first
+        let chip_id = self.iface.read_reg(Registers::CHIP_ID)?;
+        if !(chip_id == BMI160_CHIP_ID || chip_id == BMI260_CHIP_ID || chip_id == BMI270_CHIP_ID) {
+            return Err(Error::InvalidChipId);
+        }
+
+        // Reset Chip, mandatory per datasheet
+        self.send_cmd(Cmd::SoftReset)?;
+        self.delay.delay_us(2000);
 
         // Disable advanced power mode
-        let mut pwr_conf = self.get_pwr_conf()?;
-        pwr_conf.power_save = false;
-        self.set_pwr_conf(pwr_conf)?;
+        self.disable_power_save()?;
 
-        // TODO allow config of pre alloc
-        let mut preallocated_space = alloc_stack!([u8; 512]);
+        // Verify buffer size
+        if self.max_burst as usize > N {
+            return Err(Error::<CommE>::BufferTooSmall);
+        }
+
+        let mut preallocated_space = alloc_stack!([u8; N]);
         let mut vec = FixedVec::new(&mut preallocated_space);
 
+        // Offset and burst calculation
         let mut offset = 0u16;
         let max_len = config_file.len() as u16;
-        let burst = self.max_burst - 1; // Remove 1 for address byte
+        let burst = if self.max_burst % 2 == 0 {
+            self.max_burst - 1 // Address byte + even number of data bytes
+        } else {
+            self.max_burst - 2 // Make sure we have even data bytes
+        };
 
-        self.set_init_ctrl(0)?;
+        let init_ctrl = self.get_init_ctrl()?;
+        self.set_init_ctrl(init_ctrl & 0b1111_1110)?;
+        self.delay.delay_us(450);
 
         while offset < max_len {
-            self.set_init_addr(offset / 2)?;
-
-            let end = if (offset + burst) > max_len {
-                max_len
+            // INIT_ADDR should point to 16-bit words
+            self.set_init_addr(offset / 2)?; // needs to be divided by 2 because offset is in bytes
+
+            // Ensure we're writing complete 16-bit words
+            let mut chunk_size = burst;
+            if (chunk_size % 2) != 0 {
+                // If burst size would result in odd number of bytes, reduce by 1
+                chunk_size -= 1;
+            }
+
+            let end = if (offset + chunk_size) > max_len {
+                // For the last chunk, ensure we still write complete 16-bit words
+                let remaining = max_len - offset;
+                offset + (remaining - (remaining % 2))
             } else {
-                offset + burst
+                offset + chunk_size
             };
 
-            vec.push(Registers::INIT_DATA)
-                .map_err(|_| Error::<CommE>::Alloc)?;
+            vec.clear();
+            vec.push(Registers::INIT_DATA).map_err(|_| Error::Alloc)?;
 
             vec.push_all(&config_file[offset as usize..end as usize])
-                .map_err(|_| Error::<CommE>::Alloc)?;
+                .map_err(|_| Error::Alloc)?;
 
-            self.iface.write(&mut vec.as_mut_slice())?;
+            self.iface.write(vec.as_mut_slice())?;
 
-            offset += burst;
-            vec.clear();
+            offset += chunk_size;
+            self.delay.delay_us(2);
         }
 
+        // This operation must not be performed more than once after POR or soft reset.
         self.set_init_ctrl(1)?;
+        self.delay.delay_us(2);
+
+        self.enable_power_save()?;
+
+        // Initialization takes at most 20ms per datasheet
+        self.delay.delay_us(20_000);
+
+        let internal_status = self.iface.read_reg(Registers::INTERNAL_STATUS)?;
+
+        if internal_status & 0b0000_0001 != 1 {
+            return Err(Error::InitFailed);
+        }
 
         Ok(())
     }
diff --git a/src/bmi2_async.rs b/src/bmi2_async.rs
new file mode 100644
index 0000000..59b6540
--- /dev/null
+++ b/src/bmi2_async.rs
@@ -0,0 +1,857 @@
+use embedded_hal::delay::DelayNs;
+use fixedvec::FixedVec;
+
+use crate::registers::Registers;
+use crate::{
+    interface_async::{I2cInterface, ReadData, SpiInterface, WriteData},
+    interface_common::I2cAddr,
+};
+
+use crate::types::{
+    AccConf, AccOffsets, AccRange, AccSelfTest, AuxConf, AuxData, AuxIfConf, AxisData, Burst, Cmd,
+    Data, Drv, Error, ErrorReg, ErrorRegMsk, Event, FifoConf, FifoDowns, GyrConf, GyrCrtConf,
+    GyrOffsets, GyrRange, GyrSelfTest, IfConf, IntIoCtrl, IntLatch, IntMapData, IntMapFeat,
+    InternalError, InternalStatus, InterruptStatus, NvConf, PullUpConf, PwrConf, PwrCtrl,
+    Saturation, Status, WristGestureActivity, BMI160_CHIP_ID, BMI260_CHIP_ID, BMI270_CHIP_ID,
+    FIFO_LENGTH_1_MASK,
+};
+
+pub struct Bmi2<I, D, const N: usize> {
+    iface: I,
+    max_burst: u16,
+    delay: D,
+}
+
+impl<I2C, D, const N: usize> Bmi2<I2cInterface<I2C>, D, N> {
+    /// Create a new Bmi270 device with I2C communication.
+    pub fn new_i2c(i2c: I2C, delay: D, address: I2cAddr, burst: Burst) -> Self {
+        Bmi2 {
+            iface: I2cInterface {
+                i2c,
+                address: address.addr(),
+            },
+            max_burst: burst.val(),
+            delay,
+        }
+    }
+
+    /// Release I2C.
+    pub fn release(self) -> I2C {
+        self.iface.i2c
+    }
+}
+
+impl<SPI, D, const N: usize> Bmi2<SpiInterface<SPI>, D, N>
+where
+    D: DelayNs,
+{
+    /// Create a new Bmi270 device with SPI communication.
+    pub fn new_spi(spi: SPI, delay: D, burst: Burst) -> Self {
+        Bmi2 {
+            iface: SpiInterface { spi },
+            max_burst: burst.val(),
+            delay,
+        }
+    }
+
+    /// Release I2C and CS.
+    pub fn release(self) -> SPI {
+        self.iface.spi
+    }
+}
+
+impl<I, D, CommE, const N: usize> Bmi2<I, D, N>
+where
+    I: ReadData<Error = Error<CommE>> + WriteData<Error = Error<CommE>>,
+    D: embedded_hal::delay::DelayNs, // Add constraint for D
+{
+    /// Get the chip id.
+    pub async fn get_chip_id(&mut self) -> Result<u8, Error<CommE>> {
+        self.iface.read_reg(Registers::CHIP_ID).await
+    }
+
+    /// Get the errors from the error register.
+    pub async fn get_errors(&mut self) -> Result<ErrorReg, Error<CommE>> {
+        let errors = self.iface.read_reg(Registers::ERR_REG).await?;
+
+        Ok(ErrorReg::from_reg(errors))
+    }
+
+    /// Get the sensor status.
+    pub async fn get_status(&mut self) -> Result<Status, Error<CommE>> {
+        let status = self.iface.read_reg(Registers::STATUS).await?;
+
+        Ok(Status::from_reg(status))
+    }
+
+    /// Get the sensor auxiliary data.
+    pub async fn get_aux_data(&mut self) -> Result<AuxData, Error<CommE>> {
+        let mut payload = [0_u8; 9];
+        payload[0] = Registers::AUX_DATA_0;
+        self.iface.read(&mut payload).await?;
+
+        Ok(AuxData {
+            axis: payload_to_axis(&payload[1..7]),
+            r: (i16::from(payload[7]) | (i16::from(payload[8]) << 8)),
+        })
+    }
+
+    /// Get the sensor accelerometer data.
+    pub async fn get_acc_data(&mut self) -> Result<AxisData, Error<CommE>> {
+        let mut payload = [0_u8; 7];
+        payload[0] = Registers::ACC_DATA_0;
+        self.iface.read(&mut payload).await?;
+
+        Ok(payload_to_axis(&payload[1..]))
+    }
+
+    /// Get the sensor gyroscope data.
+    pub async fn get_gyr_data(&mut self) -> Result<AxisData, Error<CommE>> {
+        let mut payload = [0_u8; 7];
+        payload[0] = Registers::GYR_DATA_0;
+        self.iface.read(&mut payload).await?;
+
+        Ok(payload_to_axis(&payload[1..]))
+    }
+
+    /// Get the sensor time.
+    pub async fn get_sensor_time(&mut self) -> Result<u32, Error<CommE>> {
+        let mut payload = [Registers::SENSORTIME_0, 0, 0, 0];
+        self.iface.read(&mut payload).await?;
+
+        Ok(payload_to_sensortime(&payload[1..]))
+    }
+
+    /// Get all the sensor data (excluding auxiliary data).
+    pub async fn get_data(&mut self) -> Result<Data, Error<CommE>> {
+        let mut payload = [0_u8; 16];
+        payload[0] = Registers::ACC_DATA_0;
+        self.iface.read(&mut payload).await?;
+
+        Ok(Data {
+            acc: payload_to_axis(&payload[1..7]),
+            gyr: payload_to_axis(&payload[7..13]),
+            time: payload_to_sensortime(&payload[13..16]),
+        })
+    }
+
+    /// Get the event register.
+    pub async fn get_event(&mut self) -> Result<Event, Error<CommE>> {
+        let event = self.iface.read_reg(Registers::EVENT).await?;
+
+        Ok(Event::from_reg(event))
+    }
+
+    /// Get the interrupt/feature status.
+    pub async fn get_int_status(&mut self) -> Result<InterruptStatus, Error<CommE>> {
+        let int_stat_0 = self.iface.read_reg(Registers::INT_STATUS_0).await?;
+        let int_stat_1 = self.iface.read_reg(Registers::INT_STATUS_1).await?;
+
+        Ok(InterruptStatus::from_regs(int_stat_0, int_stat_1))
+    }
+
+    /// Get the step count.
+    pub async fn get_step_count(&mut self) -> Result<u16, Error<CommE>> {
+        let mut payload = [Registers::SC_OUT_0, 0, 0];
+        self.iface.read(&mut payload).await?;
+
+        let steps: u16 = u16::from(payload[1]) | u16::from(payload[2]) << 8;
+
+        Ok(steps)
+    }
+
+    /// Get the detected wrist gesture and activity type.
+    pub async fn get_wrist_gesture_activity(
+        &mut self,
+    ) -> Result<WristGestureActivity, Error<CommE>> {
+        let wr_gest_acc = self.iface.read_reg(Registers::WR_GEST_ACT).await?;
+
+        Ok(WristGestureActivity::from_reg(wr_gest_acc))
+    }
+
+    /// Get the sensor internal status.
+    pub async fn get_internal_status(&mut self) -> Result<InternalStatus, Error<CommE>> {
+        let internal_status = self.iface.read_reg(Registers::INTERNAL_STATUS).await?;
+
+        Ok(InternalStatus::from_reg(internal_status))
+    }
+
+    /// Get the sensor temperature.
+    pub async fn get_temperature(&mut self) -> Result<Option<f32>, Error<CommE>> {
+        let mut payload = [Registers::TEMPERATURE_0, 0, 0];
+        self.iface.read(&mut payload).await?;
+        let raw_temp = u16::from(payload[1]) | u16::from(payload[2]) << 8;
+
+        Ok(match raw_temp {
+            0x8000 => None,
+            _ => Some(f32::from(raw_temp as i16) * (1.0_f32 / 512.0_f32) + 23.0),
+        })
+    }
+
+    /// Get the current fill level of the FIFO buffer.
+    pub async fn get_fifo_len(&mut self) -> Result<i16, Error<CommE>> {
+        let mut payload = [Registers::FIFO_LENGTH_0, 0, 0];
+        self.iface.read(&mut payload).await?;
+        let len = i16::from(payload[1]) | i16::from(payload[2] & FIFO_LENGTH_1_MASK) << 8;
+
+        Ok(len)
+    }
+
+    pub async fn get_fifo_data(&mut self) -> Result<(), Error<CommE>> {
+        // TODO Fifo is 6KB, will need the max read info from user + fifo config
+        Ok(())
+    }
+
+    /// Get the accelerometer configuration.
+    pub async fn get_acc_conf(&mut self) -> Result<AccConf, Error<CommE>> {
+        let acc_conf = self.iface.read_reg(Registers::ACC_CONF).await?;
+        Ok(AccConf::from_reg(acc_conf))
+    }
+
+    /// Set the accelerometer configuration.
+    pub async fn set_acc_conf(&mut self, acc_conf: AccConf) -> Result<(), Error<CommE>> {
+        let reg = acc_conf.to_reg();
+        self.iface.write_reg(Registers::ACC_CONF, reg).await?;
+        Ok(())
+    }
+
+    /// Get the accelerometer range.
+    pub async fn get_acc_range(&mut self) -> Result<AccRange, Error<CommE>> {
+        let acc_range = self.iface.read_reg(Registers::ACC_RANGE).await?;
+        Ok(AccRange::from_reg(acc_range))
+    }
+
+    /// Set the accelerometer range.
+    pub async fn set_acc_range(&mut self, acc_range: AccRange) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::ACC_RANGE, acc_range as u8)
+            .await?;
+        Ok(())
+    }
+
+    /// Get the gyroscope configuration.
+    pub async fn get_gyr_conf(&mut self) -> Result<GyrConf, Error<CommE>> {
+        let gyr_conf = self.iface.read_reg(Registers::GYR_CONF).await?;
+        Ok(GyrConf::from_reg(gyr_conf))
+    }
+
+    /// Set the gyroscope configuration.
+    pub async fn set_gyr_conf(&mut self, gyr_conf: GyrConf) -> Result<(), Error<CommE>> {
+        let reg = gyr_conf.to_reg();
+        self.iface.write_reg(Registers::GYR_CONF, reg).await?;
+        Ok(())
+    }
+
+    /// Get the gyroscope range.
+    pub async fn get_gyr_range(&mut self) -> Result<GyrRange, Error<CommE>> {
+        let gyr_range = self.iface.read_reg(Registers::GYR_RANGE).await?;
+        Ok(GyrRange::from_reg(gyr_range))
+    }
+
+    /// Set the gyroscope configuration.
+    pub async fn set_gyr_range(&mut self, gyr_range: GyrRange) -> Result<(), Error<CommE>> {
+        let reg = gyr_range.to_reg();
+        self.iface.write_reg(Registers::GYR_RANGE, reg).await?;
+        Ok(())
+    }
+
+    /// Get the Auxiliary device configuration.
+    pub async fn get_aux_conf(&mut self) -> Result<AuxConf, Error<CommE>> {
+        let aux_conf = self.iface.read_reg(Registers::AUX_CONF).await?;
+        Ok(AuxConf::from_reg(aux_conf))
+    }
+
+    /// Set the Auxiliary device configuration.
+    pub async fn set_aux_conf(&mut self, aux_conf: AuxConf) -> Result<(), Error<CommE>> {
+        let reg = aux_conf.to_reg();
+        self.iface.write_reg(Registers::AUX_CONF, reg).await?;
+        Ok(())
+    }
+
+    /// Get the fifo downsampling configuration.
+    pub async fn get_fifo_downs(&mut self) -> Result<FifoDowns, Error<CommE>> {
+        let fifo_downs = self.iface.read_reg(Registers::FIFO_DOWNS).await?;
+        Ok(FifoDowns::from_reg(fifo_downs))
+    }
+
+    /// Set the fifo downsampling configuration.
+    pub async fn set_fifo_downs(&mut self, fifo_downs: FifoDowns) -> Result<(), Error<CommE>> {
+        let reg = fifo_downs.to_reg();
+        self.iface.write_reg(Registers::FIFO_DOWNS, reg).await?;
+        Ok(())
+    }
+
+    /// Get the fifo watermark level.
+    pub async fn get_fifo_wtm(&mut self) -> Result<u16, Error<CommE>> {
+        let mut payload = [Registers::FIFO_WTM_0, 0, 0];
+        self.iface.read(&mut payload).await?;
+        Ok(u16::from(payload[1]) | u16::from(payload[2]) << 8)
+    }
+
+    /// Set the fifo watermark level. Interrupt will trigger when the fifo reaches wtm * 256 bytes.
+    pub async fn set_fifo_wtm(&mut self, wtm: u16) -> Result<(), Error<CommE>> {
+        let reg_0 = wtm as u8;
+        let reg_1 = (wtm >> 8) as u8;
+        let mut payload = [Registers::FIFO_WTM_0, reg_0, reg_1];
+        self.iface.write(&mut payload).await?;
+        Ok(())
+    }
+
+    /// Get the fifo configuration.
+    pub async fn get_fifo_conf(&mut self) -> Result<FifoConf, Error<CommE>> {
+        let mut payload = [Registers::FIFO_CONFIG_0, 0, 0];
+        self.iface.read(&mut payload).await?;
+        Ok(FifoConf::from_regs(payload[1], payload[2]))
+    }
+
+    /// Set the fifo configuration.
+    pub async fn set_fifo_conf(&mut self, fifo_conf: FifoConf) -> Result<(), Error<CommE>> {
+        let (reg_0, reg_1) = fifo_conf.to_regs();
+        let mut payload = [Registers::FIFO_CONFIG_0, reg_0, reg_1];
+        self.iface.write(&mut payload).await?;
+        Ok(())
+    }
+
+    /// Get the current saturation.
+    pub async fn get_saturation(&mut self) -> Result<Saturation, Error<CommE>> {
+        let saturation = self.iface.read_reg(Registers::SATURATION).await?;
+        Ok(Saturation::from_reg(saturation))
+    }
+
+    /// Get the auxiliary device id.
+    pub async fn get_aux_dev_id(&mut self) -> Result<u8, Error<CommE>> {
+        let dev_id = self.iface.read_reg(Registers::AUX_DEV_ID).await?;
+        Ok(dev_id >> 1)
+    }
+
+    /// Set the auxiliary device id.
+    pub async fn set_aux_dev_id(&mut self, dev_id: u8) -> Result<(), Error<CommE>> {
+        let reg = dev_id << 1;
+        self.iface.write_reg(Registers::AUX_DEV_ID, reg).await?;
+        Ok(())
+    }
+
+    /// Get auxiliary device interface configuration.
+    pub async fn get_aux_if_conf(&mut self) -> Result<AuxIfConf, Error<CommE>> {
+        let aux_if_conf = self.iface.read_reg(Registers::AUX_IF_CONF).await?;
+        Ok(AuxIfConf::from_reg(aux_if_conf))
+    }
+
+    /// Set auxiliary device interface configuration.
+    pub async fn set_aux_if_conf(&mut self, aux_if_conf: AuxIfConf) -> Result<(), Error<CommE>> {
+        let reg = aux_if_conf.to_reg();
+        self.iface.write_reg(Registers::AUX_IF_CONF, reg).await?;
+        Ok(())
+    }
+
+    /// Get auxiliary device read address.
+    pub async fn get_aux_rd_addr(&mut self) -> Result<u8, Error<CommE>> {
+        let aux_rd_addr = self.iface.read_reg(Registers::AUX_RD_ADDR).await?;
+        Ok(aux_rd_addr)
+    }
+
+    /// Set auxiliary device read address.
+    pub async fn set_aux_rd_addr(&mut self, aux_rd_addr: u8) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::AUX_RD_ADDR, aux_rd_addr)
+            .await?;
+        Ok(())
+    }
+
+    /// Get auxiliary device write address.
+    pub async fn get_aux_wr_addr(&mut self) -> Result<u8, Error<CommE>> {
+        let aux_wr_addr = self.iface.read_reg(Registers::AUX_WR_ADDR).await?;
+        Ok(aux_wr_addr)
+    }
+
+    /// Set auxiliary device write address.
+    pub async fn set_aux_wr_addr(&mut self, aux_wr_addr: u8) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::AUX_WR_ADDR, aux_wr_addr)
+            .await?;
+        Ok(())
+    }
+
+    /// Get auxiliary device data to write.
+    pub async fn get_aux_wr_data(&mut self) -> Result<u8, Error<CommE>> {
+        let aux_wr_data = self.iface.read_reg(Registers::AUX_WR_DATA).await?;
+        Ok(aux_wr_data)
+    }
+
+    /// Set auxiliary device data to write.
+    pub async fn set_aux_wr_data(&mut self, aux_wr_data: u8) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::AUX_WR_DATA, aux_wr_data)
+            .await?;
+        Ok(())
+    }
+
+    /// Get error register mask.
+    pub async fn get_err_reg_msk(&mut self) -> Result<ErrorRegMsk, Error<CommE>> {
+        let err_reg_msk = self.iface.read_reg(Registers::ERR_REG_MSK).await?;
+        Ok(ErrorRegMsk::from_reg(err_reg_msk))
+    }
+
+    /// Get error register mask.
+    pub async fn set_err_reg_msk(&mut self, err_reg_msk: ErrorRegMsk) -> Result<(), Error<CommE>> {
+        let reg = err_reg_msk.to_reg();
+        self.iface.write_reg(Registers::ERR_REG_MSK, reg).await?;
+        Ok(())
+    }
+
+    /// Get interrupt 1 io control.
+    pub async fn get_int1_io_ctrl(&mut self) -> Result<IntIoCtrl, Error<CommE>> {
+        let int1_io_ctrl = self.iface.read_reg(Registers::INT1_IO_CTRL).await?;
+        Ok(IntIoCtrl::from_reg(int1_io_ctrl))
+    }
+
+    /// Set interrupt 1 io control.
+    pub async fn set_int1_io_ctrl(&mut self, int1_io_ctrl: IntIoCtrl) -> Result<(), Error<CommE>> {
+        let reg = int1_io_ctrl.to_reg();
+        self.iface.write_reg(Registers::INT1_IO_CTRL, reg).await?;
+        Ok(())
+    }
+
+    /// Get interrupt 2 io control.
+    pub async fn get_int2_io_ctrl(&mut self) -> Result<IntIoCtrl, Error<CommE>> {
+        let int2_io_ctrl = self.iface.read_reg(Registers::INT2_IO_CTRL).await?;
+        Ok(IntIoCtrl::from_reg(int2_io_ctrl))
+    }
+
+    /// Set interrupt 2 io control.
+    pub async fn set_int2_io_ctrl(&mut self, int2_io_ctrl: IntIoCtrl) -> Result<(), Error<CommE>> {
+        let reg = int2_io_ctrl.to_reg();
+        self.iface.write_reg(Registers::INT2_IO_CTRL, reg).await?;
+        Ok(())
+    }
+
+    /// Get interrupt latched mode.
+    pub async fn get_int_latch(&mut self) -> Result<IntLatch, Error<CommE>> {
+        let int_latch = self.iface.read_reg(Registers::INT_LATCH).await?;
+        Ok(IntLatch::from_reg(int_latch))
+    }
+
+    /// Set interrupt latched mode.
+    pub async fn set_int_latch(&mut self, int_latch: IntLatch) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::INT_LATCH, int_latch as u8)
+            .await?;
+        Ok(())
+    }
+
+    /// Get interrupt 1 feature mapping.
+    pub async fn get_int1_map_feat(&mut self) -> Result<IntMapFeat, Error<CommE>> {
+        let int1_map_feat = self.iface.read_reg(Registers::INT1_MAP_FEAT).await?;
+        Ok(IntMapFeat::from_reg(int1_map_feat))
+    }
+
+    /// Set interrupt 1 feature mapping.
+    pub async fn set_int1_map_feat(
+        &mut self,
+        int1_map_feat: IntMapFeat,
+    ) -> Result<(), Error<CommE>> {
+        let reg = int1_map_feat.to_reg();
+        self.iface.write_reg(Registers::INT1_MAP_FEAT, reg).await?;
+        Ok(())
+    }
+
+    /// Get interrupt 2 feature mapping.
+    pub async fn get_int2_map_feat(&mut self) -> Result<IntMapFeat, Error<CommE>> {
+        let int2_map_feat = self.iface.read_reg(Registers::INT2_MAP_FEAT).await?;
+        Ok(IntMapFeat::from_reg(int2_map_feat))
+    }
+
+    /// Set interrupt 2 feature mapping.
+    pub async fn set_int2_map_feat(
+        &mut self,
+        int2_map_feat: IntMapFeat,
+    ) -> Result<(), Error<CommE>> {
+        let reg = int2_map_feat.to_reg();
+        self.iface.write_reg(Registers::INT2_MAP_FEAT, reg).await?;
+        Ok(())
+    }
+
+    /// Get interrupt data map.
+    pub async fn get_int_map_data(&mut self) -> Result<IntMapData, Error<CommE>> {
+        let int_map_data = self.iface.read_reg(Registers::INT_MAP_DATA).await?;
+        Ok(IntMapData::from_reg(int_map_data))
+    }
+
+    /// Set interrupt data map.
+    pub async fn set_int_map_data(&mut self, int_map_data: IntMapData) -> Result<(), Error<CommE>> {
+        let reg = int_map_data.to_reg();
+        self.iface.write_reg(Registers::INT_MAP_DATA, reg).await?;
+        Ok(())
+    }
+
+    /// Get initialization control register.
+    pub async fn get_init_ctrl(&mut self) -> Result<u8, Error<CommE>> {
+        let init_ctrl = self.iface.read_reg(Registers::INIT_CTRL).await?;
+        Ok(init_ctrl)
+    }
+
+    /// Set initialization control register (start initialization).
+    pub async fn set_init_ctrl(&mut self, init_ctrl: u8) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::INIT_CTRL, init_ctrl)
+            .await?;
+        Ok(())
+    }
+
+    /// Get init address.
+    pub async fn get_init_addr(&mut self) -> Result<u16, Error<CommE>> {
+        let mut payload = [Registers::INIT_ADDR_0, 0, 0];
+        self.iface.read(&mut payload).await?;
+        Ok(u16::from(payload[1] & 0b0000_1111) | u16::from(payload[2]) << 4)
+    }
+
+    /// Set init address.
+    pub async fn set_init_addr(&mut self, init_addr: u16) -> Result<(), Error<CommE>> {
+        let reg_0 = init_addr as u8 & 0b0000_1111;
+        let reg_1 = (init_addr >> 4) as u8;
+        let mut payload = [Registers::INIT_ADDR_0, reg_0, reg_1];
+        self.iface.write(&mut payload).await?;
+        Ok(())
+    }
+
+    /// Get the initialization register.
+    pub async fn get_init_data(&mut self) -> Result<u8, Error<CommE>> {
+        let init_data = self.iface.read_reg(Registers::INIT_DATA).await?;
+        Ok(init_data)
+    }
+
+    /// Set the initialization register.
+    pub async fn set_init_data(&mut self, init_data: u8) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::INIT_DATA, init_data)
+            .await?;
+        Ok(())
+    }
+
+    /// Get the internal errors.
+    pub async fn get_internal_error(&mut self) -> Result<InternalError, Error<CommE>> {
+        let internal_error = self.iface.read_reg(Registers::INTERNAL_ERROR).await?;
+        Ok(InternalError::from_reg(internal_error))
+    }
+
+    /// Get ASDA pull up.
+    pub async fn get_asda_pullup(&mut self) -> Result<PullUpConf, Error<CommE>> {
+        let aux_if_trim = self.iface.read_reg(Registers::AUX_IF_TRIM).await?;
+        Ok(PullUpConf::from_reg(aux_if_trim))
+    }
+
+    /// Set ASDA pull up.
+    pub async fn set_asda_pullup(&mut self, pull_up_conf: PullUpConf) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::AUX_IF_TRIM, pull_up_conf.to_reg())
+            .await?;
+        Ok(())
+    }
+
+    /// Get gyroscope component retrimming register.
+    pub async fn get_gyr_crt_conf(&mut self) -> Result<GyrCrtConf, Error<CommE>> {
+        let gyr_crt_conf = self.iface.read_reg(Registers::GYR_CRT_CONF).await?;
+        Ok(GyrCrtConf::from_reg(gyr_crt_conf))
+    }
+
+    /// Set gyroscope component retrimming register.
+    /// GyrCrtConf.rdy_for_dl is read-only, only crt_running will be set.
+    pub async fn set_gyr_crt_conf(&mut self, gyr_crt_conf: GyrCrtConf) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::GYR_CRT_CONF, gyr_crt_conf.to_reg())
+            .await?;
+        Ok(())
+    }
+
+    /// Get NVM configuration.
+    pub async fn get_nvm_conf(&mut self) -> Result<bool, Error<CommE>> {
+        let nvm_conf = self.iface.read_reg(Registers::NVM_CONF).await?;
+        Ok((nvm_conf & 1 << 1) != 0)
+    }
+
+    /// Set NVM configuration.
+    pub async fn set_nvm_conf(&mut self, gyr_crt_conf: bool) -> Result<(), Error<CommE>> {
+        let value: u8 = if gyr_crt_conf { 0x01 } else { 0x00 };
+        self.iface
+            .write_reg(Registers::NVM_CONF, value << 1)
+            .await?;
+        Ok(())
+    }
+
+    /// Get the interface configuration.
+    pub async fn get_if_conf(&mut self) -> Result<IfConf, Error<CommE>> {
+        let if_conf = self.iface.read_reg(Registers::IF_CONF).await?;
+        Ok(IfConf::from_reg(if_conf))
+    }
+
+    /// Set the interface configuration.
+    pub async fn set_if_conf(&mut self, if_conf: IfConf) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::IF_CONF, if_conf.to_reg())
+            .await?;
+        Ok(())
+    }
+
+    /// Get the drive strength configuration.
+    pub async fn get_drv(&mut self) -> Result<Drv, Error<CommE>> {
+        let drv = self.iface.read_reg(Registers::DRV).await?;
+        Ok(Drv::from_reg(drv))
+    }
+
+    /// Set the drive strength configuration.
+    pub async fn set_drv(&mut self, drv: Drv) -> Result<(), Error<CommE>> {
+        self.iface.write_reg(Registers::DRV, drv.to_reg()).await?;
+        Ok(())
+    }
+
+    /// Get the accelerometer self test configuration.
+    pub async fn get_acc_self_test(&mut self) -> Result<AccSelfTest, Error<CommE>> {
+        let acc_self_test = self.iface.read_reg(Registers::ACC_SELF_TEST).await?;
+        Ok(AccSelfTest::from_reg(acc_self_test))
+    }
+
+    /// Set the accelerometer self test configuration.
+    pub async fn set_acc_self_test(
+        &mut self,
+        acc_self_test: AccSelfTest,
+    ) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::ACC_SELF_TEST, acc_self_test.to_reg())
+            .await?;
+        Ok(())
+    }
+
+    /// Get the gyroscope self test configuration.
+    pub async fn get_gyr_self_test(&mut self) -> Result<GyrSelfTest, Error<CommE>> {
+        let gyr_self_test = self.iface.read_reg(Registers::GYR_SELF_TEST).await?;
+        Ok(GyrSelfTest::from_reg(gyr_self_test))
+    }
+
+    /// Get NV configuration.
+    pub async fn get_nv_conf(&mut self) -> Result<NvConf, Error<CommE>> {
+        let nv_conf = self.iface.read_reg(Registers::NV_CONF).await?;
+        Ok(NvConf::from_reg(nv_conf))
+    }
+
+    /// Set NV configuration.
+    pub async fn set_nv_conf(&mut self, nv_conf: NvConf) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::NV_CONF, nv_conf.to_reg())
+            .await?;
+        Ok(())
+    }
+
+    /// Get accelerometer offsets.
+    pub async fn get_acc_offsets(&mut self) -> Result<AccOffsets, Error<CommE>> {
+        let mut payload = [Registers::OFFSET_0, 0, 0, 0];
+        self.iface.read(&mut payload).await?;
+        Ok(AccOffsets {
+            x: payload[1],
+            y: payload[2],
+            z: payload[3],
+        })
+    }
+
+    /// Set accelerometer offsets.
+    pub async fn set_acc_offsets(&mut self, acc_offsets: AccOffsets) -> Result<(), Error<CommE>> {
+        let mut payload = [
+            Registers::OFFSET_0,
+            acc_offsets.x,
+            acc_offsets.y,
+            acc_offsets.z,
+        ];
+        self.iface.write(&mut payload).await?;
+        Ok(())
+    }
+
+    /// Get gyroscope offsets.
+    pub async fn get_gyr_offsets(&mut self) -> Result<GyrOffsets, Error<CommE>> {
+        let mut payload = [0_u8; 5];
+        payload[0] = Registers::OFFSET_3;
+        self.iface.read(&mut payload).await?;
+
+        let x = u16::from(payload[1]) | u16::from(payload[4] & 0b0000_0011) << 8;
+        let y = u16::from(payload[2]) | u16::from(payload[4] & 0b0000_1100) << 6;
+        let z = u16::from(payload[3]) | u16::from(payload[4] & 0b0011_0000) << 4;
+        let offset_en = (payload[4] & 1 << 6) != 0;
+        let gain_en = (payload[4] & 1 << 7) != 0;
+
+        Ok(GyrOffsets {
+            x,
+            y,
+            z,
+            offset_en,
+            gain_en,
+        })
+    }
+
+    /// Set gyroscope offsets.
+    pub async fn set_gyr_offsets(&mut self, gyr_offsets: GyrOffsets) -> Result<(), Error<CommE>> {
+        let mut payload = [0_u8; 5];
+        payload[0] = Registers::OFFSET_3;
+
+        payload[1] = gyr_offsets.x as u8;
+        payload[2] = gyr_offsets.y as u8;
+        payload[3] = gyr_offsets.z as u8;
+
+        payload[4] |= (gyr_offsets.x >> 8 & 0b0000_0011) as u8;
+        payload[4] |= (gyr_offsets.y >> 6 & 0b0000_1100) as u8;
+        payload[4] |= (gyr_offsets.z >> 4 & 0b0011_0000) as u8;
+        payload[4] |= if gyr_offsets.offset_en { 0x01 } else { 0x00 } << 6;
+        payload[4] |= if gyr_offsets.gain_en { 0x01 } else { 0x00 } << 7;
+        self.iface.write(&mut payload).await?;
+
+        Ok(())
+    }
+
+    /// Get power configuration.
+    pub async fn get_pwr_conf(&mut self) -> Result<PwrConf, Error<CommE>> {
+        let pwr_conf = self.iface.read_reg(Registers::PWR_CONF).await?;
+        Ok(PwrConf::from_reg(pwr_conf))
+    }
+
+    /// Set power configuration.
+    pub async fn set_pwr_conf(&mut self, pwr_conf: PwrConf) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::PWR_CONF, pwr_conf.to_reg())
+            .await?;
+        Ok(())
+    }
+
+    /// Get power control.
+    pub async fn get_pwr_ctrl(&mut self) -> Result<PwrCtrl, Error<CommE>> {
+        let pwr_ctrl = self.iface.read_reg(Registers::PWR_CTRL).await?;
+        Ok(PwrCtrl::from_reg(pwr_ctrl))
+    }
+
+    /// Set power control.
+    pub async fn set_pwr_ctrl(&mut self, pwr_ctrl: PwrCtrl) -> Result<(), Error<CommE>> {
+        self.iface
+            .write_reg(Registers::PWR_CTRL, pwr_ctrl.to_reg())
+            .await?;
+        Ok(())
+    }
+
+    /// Send a command.
+    pub async fn send_cmd(&mut self, cmd: Cmd) -> Result<(), Error<CommE>> {
+        self.iface.write_reg(Registers::CMD, cmd as u8).await?;
+        Ok(())
+    }
+
+    /// Disable power save mode.
+    pub async fn disable_power_save(&mut self) -> Result<(), Error<CommE>> {
+        let mut pwr_conf = self.get_pwr_conf().await?;
+        pwr_conf.power_save = false;
+        self.set_pwr_conf(pwr_conf).await?;
+        // Critical delay after disabling power save
+        self.delay.delay_us(450);
+        Ok(())
+    }
+
+    /// Enable power save mode.
+    pub async fn enable_power_save(&mut self) -> Result<(), Error<CommE>> {
+        let mut pwr_conf = self.get_pwr_conf().await?;
+        pwr_conf.power_save = true;
+        self.set_pwr_conf(pwr_conf).await?;
+        // Critical delay after enabling power save
+        self.delay.delay_us(450);
+        Ok(())
+    }
+
+    /// Initialize sensor.
+    pub async fn init(&mut self, config_file: &[u8]) -> Result<(), Error<CommE>> {
+        // Verify chip ID first
+        let chip_id = self.iface.read_reg(Registers::CHIP_ID).await?;
+        if !(chip_id == BMI160_CHIP_ID || chip_id == BMI260_CHIP_ID || chip_id == BMI270_CHIP_ID) {
+            return Err(Error::InvalidChipId);
+        }
+
+        // Reset Chip, mandatory per datasheet
+        self.send_cmd(Cmd::SoftReset).await?;
+        self.delay.delay_us(2000);
+
+        // Disable advanced power mode
+        self.disable_power_save().await?;
+
+        // Verify buffer size
+        if self.max_burst as usize > N {
+            return Err(Error::<CommE>::BufferTooSmall);
+        }
+
+        let mut preallocated_space = alloc_stack!([u8; N]);
+        let mut vec = FixedVec::new(&mut preallocated_space);
+
+        // Offset and burst calculation
+        let mut offset = 0u16;
+        let max_len = config_file.len() as u16;
+        let burst = if self.max_burst % 2 == 0 {
+            self.max_burst - 1 // Address byte + even number of data bytes
+        } else {
+            self.max_burst - 2 // Make sure we have even data bytes
+        };
+
+        let init_ctrl = self.get_init_ctrl().await?;
+        self.set_init_ctrl(init_ctrl & 0b1111_1110).await?;
+        self.delay.delay_us(450);
+
+        while offset < max_len {
+            // INIT_ADDR should point to 16-bit words
+            self.set_init_addr(offset / 2).await?; // needs to be divided by 2 because offset is in bytes
+
+            // Ensure we're writing complete 16-bit words
+            let mut chunk_size = burst;
+            if (chunk_size % 2) != 0 {
+                // If burst size would result in odd number of bytes, reduce by 1
+                chunk_size -= 1;
+            }
+
+            let end = if (offset + chunk_size) > max_len {
+                // For the last chunk, ensure we still write complete 16-bit words
+                let remaining = max_len - offset;
+                offset + (remaining - (remaining % 2))
+            } else {
+                offset + chunk_size
+            };
+
+            vec.clear();
+            vec.push(Registers::INIT_DATA).map_err(|_| Error::Alloc)?;
+
+            vec.push_all(&config_file[offset as usize..end as usize])
+                .map_err(|_| Error::Alloc)?;
+
+            self.iface.write(vec.as_mut_slice()).await?;
+
+            offset += chunk_size;
+            self.delay.delay_us(2);
+        }
+
+        // This operation must not be performed more than once after POR or soft reset.
+        self.set_init_ctrl(1).await?;
+        self.delay.delay_us(2);
+
+        self.enable_power_save().await?;
+
+        // Initialization takes at most 20ms per datasheet
+        self.delay.delay_us(20_000);
+
+        let internal_status = self.iface.read_reg(Registers::INTERNAL_STATUS).await?;
+
+        if internal_status & 0b0000_0001 != 1 {
+            return Err(Error::InitFailed);
+        }
+
+        Ok(())
+    }
+}
+
+fn payload_to_axis(payload: &[u8]) -> AxisData {
+    AxisData {
+        x: (i16::from(payload[0]) | (i16::from(payload[1]) << 8)),
+        y: (i16::from(payload[2]) | (i16::from(payload[3]) << 8)),
+        z: (i16::from(payload[4]) | (i16::from(payload[5]) << 8)),
+    }
+}
+
+fn payload_to_sensortime(payload: &[u8]) -> u32 {
+    u32::from(payload[0]) | (u32::from(payload[1]) << 8) | (u32::from(payload[2]) << 16)
+}
diff --git a/src/config.rs b/src/config.rs
index 462d1e2..8a35884 100644
--- a/src/config.rs
+++ b/src/config.rs
@@ -1,3 +1,4 @@
+#[rustfmt::skip]
 pub static BMI270_CONFIG_FILE: [u8; 8192] = [
     0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x3d, 0xb1, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x91, 0x03, 0x80, 0x2e, 0xbc,
     0xb0, 0x80, 0x2e, 0xa3, 0x03, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5,
@@ -945,5 +946,5 @@ pub static BMI260_CONFIG_FILE: [u8; 8192] = [
     0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e,
     0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e,
     0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e,
-    0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d
-];
\ No newline at end of file
+    0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d,
+];
diff --git a/src/interface.rs b/src/interface.rs
index 677f390..0476c85 100644
--- a/src/interface.rs
+++ b/src/interface.rs
@@ -1,40 +1,9 @@
+pub use crate::interface_common::{I2cAddr, I2cInterface, SpiInterface};
+
 use crate::types::Error;
-use embedded_hal::i2c::{I2c, SevenBitAddress};
+use embedded_hal::i2c::I2c;
 use embedded_hal::spi::SpiDevice;
 
-/// Default I2C address of BMI270
-const BMI270_I2C_ADDR: u8 = 0x68;
-/// Alternative I2C address when SDO is pulled high
-const BMI270_I2C_ADDR_ALT: u8 = 0x69;
-
-pub struct I2cInterface<I2C> {
-    pub i2c: I2C,
-    pub address: u8,
-}
-
-pub struct SpiInterface<SPI> {
-    pub spi: SPI,
-}
-
-/// I2c address.
-#[derive(Debug, Default, Clone, Copy)]
-pub enum I2cAddr {
-    /// Use the default i2c address, 0x68.
-    #[default]
-    Default,
-    /// Use alternative 0x69 as the i2c address (selected when SDO is pulled high).
-    Alternative,
-}
-
-impl I2cAddr {
-    pub fn addr(self) -> SevenBitAddress {
-        match self {
-            I2cAddr::Default => BMI270_I2C_ADDR,
-            I2cAddr::Alternative => BMI270_I2C_ADDR_ALT,
-        }
-    }
-}
-
 pub trait WriteData {
     type Error;
     fn write(&mut self, payload: &mut [u8]) -> Result<(), Self::Error>;
@@ -53,16 +22,12 @@ where
 {
     type Error = Error<I2C::Error>;
     fn write(&mut self, payload: &mut [u8]) -> Result<(), Self::Error> {
-        self.i2c
-            .write(self.address, payload)
-            .map_err(Error::Comm)
+        self.i2c.write(self.address, payload).map_err(Error::Comm)
     }
 
     fn write_reg(&mut self, register: u8, data: u8) -> Result<(), Self::Error> {
         let payload: [u8; 2] = [register, data];
-        self.i2c
-            .write(self.address, &payload)
-            .map_err(Error::Comm)
+        self.i2c.write(self.address, &payload).map_err(Error::Comm)
     }
 }
 
@@ -75,18 +40,16 @@ where
         payload[0] += 0x80;
 
         // `write` asserts and deasserts CS for us. No need to do it manually!
-        let res = self.spi.write(&payload).map_err(Error::Comm);
 
-        res
+        self.spi.write(payload).map_err(Error::Comm)
     }
 
     fn write_reg(&mut self, register: u8, data: u8) -> Result<(), Self::Error> {
         let payload: [u8; 2] = [register + 0x80, data];
 
         // `write` asserts and deasserts CS for us. No need to do it manually!
-        let res = self.spi.write(&payload).map_err(Error::Comm);
 
-        res
+        self.spi.write(&payload).map_err(Error::Comm)
     }
 }
 
@@ -130,8 +93,8 @@ where
         let res = self.spi.read(&mut payload).map_err(Error::Comm);
 
         match res {
-                Ok(_) => Ok(payload[1]),
-                Err(e) => Err(e)
-        }   
+            Ok(_) => Ok(payload[1]),
+            Err(e) => Err(e),
+        }
     }
 }
diff --git a/src/interface_async.rs b/src/interface_async.rs
new file mode 100644
index 0000000..fd2afeb
--- /dev/null
+++ b/src/interface_async.rs
@@ -0,0 +1,110 @@
+pub use crate::interface_common::{I2cAddr, I2cInterface, SpiInterface};
+
+use crate::types::Error;
+use embedded_hal_async::i2c::I2c;
+use embedded_hal_async::spi::SpiDevice;
+
+#[allow(async_fn_in_trait)]
+pub trait WriteData {
+    type Error;
+    async fn write(&mut self, payload: &mut [u8]) -> Result<(), Self::Error>;
+    async fn write_reg(&mut self, register: u8, data: u8) -> Result<(), Self::Error>;
+}
+
+#[allow(async_fn_in_trait)]
+pub trait ReadData {
+    type Error;
+    async fn read(&mut self, payload: &mut [u8]) -> Result<(), Self::Error>;
+    async fn read_reg(&mut self, register: u8) -> Result<u8, Self::Error>;
+}
+
+impl<I2C, E> WriteData for I2cInterface<I2C>
+where
+    I2C: I2c<Error = E>,
+{
+    type Error = Error<I2C::Error>;
+    async fn write(&mut self, payload: &mut [u8]) -> Result<(), Self::Error> {
+        self.i2c
+            .write(self.address, payload)
+            .await
+            .map_err(Error::Comm)
+    }
+
+    async fn write_reg(&mut self, register: u8, data: u8) -> Result<(), Self::Error> {
+        let payload: [u8; 2] = [register, data];
+        self.i2c
+            .write(self.address, &payload)
+            .await
+            .map_err(Error::Comm)
+    }
+}
+
+impl<SPI, CommE> WriteData for SpiInterface<SPI>
+where
+    SPI: SpiDevice<Error = CommE>,
+{
+    type Error = Error<CommE>;
+    async fn write(&mut self, payload: &mut [u8]) -> Result<(), Self::Error> {
+        payload[0] += 0x80;
+
+        // `write` asserts and deasserts CS for us. No need to do it manually!
+
+        self.spi.write(payload).await.map_err(Error::Comm)
+    }
+
+    async fn write_reg(&mut self, register: u8, data: u8) -> Result<(), Self::Error> {
+        let payload: [u8; 2] = [register + 0x80, data];
+
+        // `write` asserts and deasserts CS for us. No need to do it manually!
+
+        self.spi.write(&payload).await.map_err(Error::Comm)
+    }
+}
+
+impl<I2C, E> ReadData for I2cInterface<I2C>
+where
+    I2C: I2c<Error = E>,
+{
+    type Error = Error<I2C::Error>;
+    async fn read(&mut self, payload: &mut [u8]) -> Result<(), Self::Error> {
+        self.i2c
+            .write_read(self.address, &[payload[0]], &mut payload[1..])
+            .await
+            .map_err(Error::Comm)
+    }
+
+    async fn read_reg(&mut self, register: u8) -> Result<u8, Self::Error> {
+        let mut data = [0];
+        self.i2c
+            .write_read(self.address, &[register], &mut data)
+            .await
+            .map_err(Error::Comm)
+            .and(Ok(data[0]))
+    }
+}
+
+impl<SPI, CommE> ReadData for SpiInterface<SPI>
+where
+    SPI: SpiDevice<Error = CommE>,
+{
+    type Error = Error<CommE>;
+    async fn read(&mut self, payload: &mut [u8]) -> Result<(), Self::Error> {
+        // `read` asserts and deasserts CS for us. No need to do it manually!
+        let res = self.spi.read(payload).await.map_err(Error::Comm);
+
+        res?;
+        Ok(())
+    }
+
+    async fn read_reg(&mut self, register: u8) -> Result<u8, Self::Error> {
+        let mut payload = [register, 0];
+
+        // `read` asserts and deasserts CS for us. No need to do it manually!
+        let res = self.spi.read(&mut payload).await.map_err(Error::Comm);
+
+        match res {
+            Ok(_) => Ok(payload[1]),
+            Err(e) => Err(e),
+        }
+    }
+}
diff --git a/src/interface_common.rs b/src/interface_common.rs
new file mode 100644
index 0000000..02ef0a6
--- /dev/null
+++ b/src/interface_common.rs
@@ -0,0 +1,34 @@
+use embedded_hal::i2c::SevenBitAddress;
+
+/// Default I2C address of BMI270
+const BMI270_I2C_ADDR: u8 = 0x68;
+/// Alternative I2C address when SDO is pulled high
+const BMI270_I2C_ADDR_ALT: u8 = 0x69;
+
+pub struct I2cInterface<I2C> {
+    pub i2c: I2C,
+    pub address: u8,
+}
+
+pub struct SpiInterface<SPI> {
+    pub spi: SPI,
+}
+
+/// I2c address.
+#[derive(Debug, Default, Clone, Copy)]
+pub enum I2cAddr {
+    /// Use the default i2c address, 0x68.
+    #[default]
+    Default,
+    /// Use alternative 0x69 as the i2c address (selected when SDO is pulled high).
+    Alternative,
+}
+
+impl I2cAddr {
+    pub fn addr(self) -> SevenBitAddress {
+        match self {
+            I2cAddr::Default => BMI270_I2C_ADDR,
+            I2cAddr::Alternative => BMI270_I2C_ADDR_ALT,
+        }
+    }
+}
diff --git a/src/lib.rs b/src/lib.rs
index 20a7168..c1b4aed 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -3,12 +3,19 @@
 #[macro_use]
 extern crate fixedvec;
 
-mod registers;
 pub mod config;
+mod registers;
 
 pub mod interface;
+pub mod interface_common;
 pub use interface::I2cAddr;
 pub mod types;
 
 pub mod bmi2;
 pub use bmi2::Bmi2;
+
+#[cfg(feature = "async")]
+pub mod bmi2_async;
+
+#[cfg(feature = "async")]
+pub mod interface_async;
diff --git a/src/registers.rs b/src/registers.rs
index b7cf67b..e779cf1 100644
--- a/src/registers.rs
+++ b/src/registers.rs
@@ -15,6 +15,7 @@ impl Registers {
     pub const INTERNAL_STATUS: u8 = 0x21;
     pub const TEMPERATURE_0: u8 = 0x22;
     pub const FIFO_LENGTH_0: u8 = 0x24;
+    #[allow(dead_code)]
     pub const FIFO_DATA: u8 = 0x26;
     pub const ACC_CONF: u8 = 0x40;
     pub const ACC_RANGE: u8 = 0x41;
diff --git a/src/types.rs b/src/types.rs
index 7857382..93f9032 100644
--- a/src/types.rs
+++ b/src/types.rs
@@ -1,3 +1,7 @@
+pub const BMI160_CHIP_ID: u8 = 0xD1;
+pub const BMI260_CHIP_ID: u8 = 0x27;
+pub const BMI270_CHIP_ID: u8 = 0x24;
+
 /// The possible errors that could be encountered.
 #[derive(Debug)]
 pub enum Error<CommE> {
@@ -5,29 +9,58 @@ pub enum Error<CommE> {
     Comm(CommE),
     /// Memory allocation error during initialization.
     Alloc,
+    /// Invalid Chip Id.
+    InvalidChipId,
+    /// Buffer too small.
+    BufferTooSmall,
+    /// Initialization Failed.
+    InitFailed,
 }
 
 /// Data burst.
-pub enum Burst {
-    /// Burst of 512 bytes.
+pub struct Burst {
+    max: u16,
+    kind: BurstKind,
+}
+
+enum BurstKind {
     Max,
-    /// An other burst amount under 512 bytes.
     Other(u16),
 }
 
-impl Default for Burst {
+impl Default for BurstKind {
     fn default() -> Self {
-        Burst::Max
+        Self::Max
     }
 }
 
 impl Burst {
-    pub fn val(self) -> u16 {
-        match self {
-            Burst::Max => 512,
-            Burst::Other(v) => v % 512,
+    pub fn new(max: u16) -> Self {
+        Self {
+            max,
+            kind: BurstKind::default(),
         }
     }
+
+    pub fn custom(max: u16, value: u16) -> Self {
+        Self {
+            max,
+            kind: BurstKind::Other(value),
+        }
+    }
+
+    pub fn val(&self) -> u16 {
+        match self.kind {
+            BurstKind::Max => self.max,
+            BurstKind::Other(v) => v.min(self.max),
+        }
+    }
+}
+
+impl Default for Burst {
+    fn default() -> Self {
+        Self::new(512)
+    }
 }
 
 pub struct ErrRegMask;