diff --git a/src/barometer.rs b/src/barometer.rs
index 41174bcaaceb43c7ace605a48e3bb90863d0b01f..3139707318db68c5f244673eb2d16270368c1bc0 100644
--- a/src/barometer.rs
+++ b/src/barometer.rs
@@ -3,7 +3,9 @@ use crate::once_cell::OnceCell;
 use crate::time::Instant;
 use crate::twi::TWI;
 use core::time::Duration;
-use embedded_hal::prelude::_embedded_hal_blocking_i2c_WriteRead;
+use embedded_hal::prelude::{
+    _embedded_hal_blocking_i2c_Write, _embedded_hal_blocking_i2c_WriteRead,
+};
 
 const MS5611_ADDR: u8 = 0b0111_0111;
 const REG_READ: u8 = 0x0;
diff --git a/src/mpu.rs b/src/mpu.rs
index c935ecb695a25f337d4a9164130d962199a96be5..93008d1b62c77cffe50a68257137fab129c5f714 100644
--- a/src/mpu.rs
+++ b/src/mpu.rs
@@ -3,10 +3,8 @@ use crate::mpu::error::Error;
 use crate::mpu::sensor::Mpu6050;
 use crate::mutex::Mutex;
 use crate::once_cell::OnceCell;
-use crate::twi::TWI;
+use crate::twi::{TwiWrapper, TWI};
 use nb::Error::WouldBlock;
-use nrf51_hal::Twi;
-use nrf51_pac::TWI0;
 use structs::{Accel, Gyro, Quaternion};
 
 #[allow(unused)]
@@ -27,7 +25,7 @@ pub const SAMPLE_RATE_DIVIDER_MPU: u8 = 0;
 pub const SAMPLE_RATE_DIVIDER_RAW: u8 = 0;
 
 struct Mpu {
-    mpu: Mpu6050<Twi<TWI0>>,
+    mpu: Mpu6050<TwiWrapper>,
     dmp_enabled: bool,
 }
 
@@ -35,7 +33,7 @@ static MPU: Mutex<OnceCell<Mpu>> = Mutex::new(OnceCell::uninitialized());
 
 pub(crate) fn initialize() {
     TWI.modify(|twi| {
-        let mut mpu: Mpu6050<Twi<TWI0>> = Mpu6050::new(&mut **twi).unwrap();
+        let mut mpu: Mpu6050<TwiWrapper> = Mpu6050::new(&mut **twi).unwrap();
 
         mpu.initialize_dmp(twi).unwrap();
 
@@ -79,7 +77,7 @@ pub fn disable_dmp() {
 ///
 /// # Errors
 /// when the global constant `SAMPLE_RATE_DIVIDER_MPU` is wrong (i.e. will not panic under normal conditions)
-pub fn enable_dmp() -> Result<(), Error<Twi<TWI0>>> {
+pub fn enable_dmp() -> Result<(), Error<TwiWrapper>> {
     // Safety: The TWI and MPU mutexes are not accessed in an interrupt
     let twi = unsafe { TWI.no_critical_section_lock_mut() };
     let mpu = unsafe { MPU.no_critical_section_lock_mut() };
@@ -102,7 +100,7 @@ pub fn enable_dmp() -> Result<(), Error<Twi<TWI0>>> {
 ///
 /// # Errors
 /// when a TWI(I2C) operation failed
-pub fn read_dmp_bytes() -> nb::Result<Quaternion, Error<Twi<TWI0>>> {
+pub fn read_dmp_bytes() -> nb::Result<Quaternion, Error<TwiWrapper>> {
     // Safety: The TWI and MPU mutexes are not accessed in an interrupt
     let twi = unsafe { TWI.no_critical_section_lock_mut() };
     let mpu = unsafe { MPU.no_critical_section_lock_mut() };
@@ -131,7 +129,7 @@ pub fn read_dmp_bytes() -> nb::Result<Quaternion, Error<Twi<TWI0>>> {
 ///
 /// # Errors
 /// when a TWI operation failed
-pub fn read_raw() -> Result<(Accel, Gyro), Error<Twi<TWI0>>> {
+pub fn read_raw() -> Result<(Accel, Gyro), Error<TwiWrapper>> {
     // Safety: The TWI and MPU mutexes are not accessed in an interrupt
     let twi = unsafe { TWI.no_critical_section_lock_mut() };
     let mpu = unsafe { MPU.no_critical_section_lock_mut() };
diff --git a/src/mpu/sensor.rs b/src/mpu/sensor.rs
index 6df53a8db336950de24a18fa4ea4f98eaa5dab6f..3fcf9cdd4048ef6c93ae78707cfce83dd5f3ef8e 100644
--- a/src/mpu/sensor.rs
+++ b/src/mpu/sensor.rs
@@ -67,7 +67,8 @@ where
         bytes: &[u8],
         response: &mut [u8],
     ) -> Result<(), Error<I2c>> {
-        free(|_| i2c.write_read(MPU6050_ADDRESS, bytes, response)).map_err(|e| Error::WriteRead(e))
+        i2c.write_read(MPU6050_ADDRESS, bytes, response)
+            .map_err(|e| Error::WriteRead(e))
     }
 
     pub(crate) fn write(&mut self, i2c: &mut I2c, bytes: &[u8]) -> Result<(), Error<I2c>> {
diff --git a/src/twi.rs b/src/twi.rs
index f8ec8c3023f4eb471c406563740ad536af7681de..f8f56e63e42d89b405aba5e73ca6a95f36a542c4 100644
--- a/src/twi.rs
+++ b/src/twi.rs
@@ -2,25 +2,60 @@ use crate::mutex::Mutex;
 use crate::once_cell::OnceCell;
 use nrf51_hal::gpio::p0::{P0_02, P0_04};
 use nrf51_hal::gpio::{Disconnected, Pin};
-use nrf51_hal::twi::Pins;
+use nrf51_hal::twi::{Error, Pins};
 use nrf51_hal::Twi;
 use nrf51_pac::twi0::frequency::FREQUENCY_A;
 use nrf51_pac::TWI0;
 
-pub(crate) static TWI: Mutex<OnceCell<Twi<TWI0>>> = Mutex::new(OnceCell::uninitialized());
+pub(crate) static TWI: Mutex<OnceCell<TwiWrapper>> = Mutex::new(OnceCell::uninitialized());
+
+pub struct TwiWrapper {
+    pub(crate) twi: Twi<TWI0>,
+}
+
+impl embedded_hal::blocking::i2c::Write for TwiWrapper {
+    type Error = Error;
+
+    fn write<'w>(&mut self, addr: u8, bytes: &'w [u8]) -> Result<(), Error> {
+        cortex_m::interrupt::free(|_| self.twi.write(addr, bytes))
+    }
+}
+
+impl embedded_hal::blocking::i2c::Read for TwiWrapper {
+    type Error = Error;
+
+    fn read<'w>(&mut self, addr: u8, bytes: &'w mut [u8]) -> Result<(), Error> {
+        cortex_m::interrupt::free(|_| self.twi.read(addr, bytes))
+    }
+}
+
+impl embedded_hal::blocking::i2c::WriteRead for TwiWrapper {
+    type Error = Error;
+
+    fn write_read<'w>(
+        &mut self,
+        addr: u8,
+        bytes: &'w [u8],
+        buffer: &'w mut [u8],
+    ) -> Result<(), Error> {
+        cortex_m::interrupt::free(|_| self.twi.write_then_read(addr, bytes, buffer))
+    }
+}
 
 pub(crate) fn initialize(twi: TWI0, scl_pin: P0_04<Disconnected>, sda_pin: P0_02<Disconnected>) {
     let scl_pin = scl_pin.into_floating_input();
     let sda_pin = sda_pin.into_floating_input();
 
     TWI.modify(|t| {
-        t.initialize(Twi::new(
-            twi,
-            Pins {
-                scl: Pin::from(scl_pin),
-                sda: Pin::from(sda_pin),
-            },
-            FREQUENCY_A::K400,
-        ))
+        t.initialize(TwiWrapper {
+            twi: Twi::new(
+                twi,
+                Pins {
+                    scl: Pin::from(scl_pin),
+                    sda: Pin::from(sda_pin),
+                },
+                FREQUENCY_A::K400,
+            ),
+        })
     });
 }