diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4307d22ebb9b8f788937234da5cbce6fb0a40d14..72516192f67d941830b02456351511511052db84 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -241,11 +241,17 @@ sbs_target(test-xbee-snd stm32f429zi_stm32f4discovery)
 add_executable(test-usart src/tests/drivers/usart/test-usart.cpp)
 sbs_target(test-usart stm32f407vg_stm32f4discovery)
 
-# add_executable(test-i2c-driver src/tests/drivers/i2c/test-i2c-driver.cpp)
-# sbs_target(test-i2c-driver stm32f429zi_stm32f4discovery)
+add_executable(test-i2c-driver-f4 src/tests/drivers/i2c/test-i2c-driver.cpp)
+sbs_target(test-i2c-driver-f4 stm32f429zi_stm32f4discovery)
 
-# add_executable(test-i2c src/tests/drivers/i2c/test-i2c.cpp)
-# sbs_target(test-i2c stm32f429zi_stm32f4discovery)
+add_executable(test-i2c-f4 src/tests/drivers/i2c/test-i2c.cpp)
+sbs_target(test-i2c-f4 stm32f429zi_stm32f4discovery)
+
+add_executable(test-i2c-driver-f7 src/tests/drivers/i2c/test-i2c-driver.cpp)
+sbs_target(test-i2c-driver-f7 stm32f767zi_nucleo)
+
+add_executable(test-i2c-f7 src/tests/drivers/i2c/test-i2c.cpp)
+sbs_target(test-i2c-f7 stm32f767zi_nucleo)
 
 add_executable(test-internal-temp src/tests/drivers/test-internal-temp.cpp)
 sbs_target(test-internal-temp stm32f407vg_stm32f4discovery)
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 2ddcb8dd831cae9f1db139f84484e882f42374ff..a3d1da7a95ea1553aeecedb6b9d4ca498c3838ef 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -57,7 +57,8 @@ foreach(OPT_BOARD ${BOARDS})
         ${SBS_BASE}/src/shared/drivers/runcam/Runcam.cpp
         ${SBS_BASE}/src/shared/drivers/spi/SPITransaction.cpp
         ${SBS_BASE}/src/shared/drivers/usart/USART.cpp
-        ${SBS_BASE}/src/shared/drivers/i2c/I2CDriver.cpp
+        ${SBS_BASE}/src/shared/drivers/i2c/I2CDriver-f4.cpp
+        ${SBS_BASE}/src/shared/drivers/i2c/I2CDriver-f7.cpp
         ${SBS_BASE}/src/shared/drivers/i2c/I2C.cpp
 
         # Events
diff --git a/src/shared/drivers/i2c/I2CDriver.cpp b/src/shared/drivers/i2c/I2CDriver-f4.cpp
similarity index 98%
rename from src/shared/drivers/i2c/I2CDriver.cpp
rename to src/shared/drivers/i2c/I2CDriver-f4.cpp
index 53a938783f83578ad2d4a70a63dcbcdd14e01163..79288dc17fddace034b1afe2a28e0dd0a9d2591c 100644
--- a/src/shared/drivers/i2c/I2CDriver.cpp
+++ b/src/shared/drivers/i2c/I2CDriver-f4.cpp
@@ -20,13 +20,15 @@
  * THE SOFTWARE.
  */
 
-#include "I2CDriver.h"
+#ifdef _ARCH_CORTEXM4_STM32F4
 
 #include <assert.h>
 #include <kernel/scheduler/scheduler.h>
 #include <utils/ClockUtils.h>
 #include <utils/Debug.h>
 
+#include "I2CDriver.h"
+
 namespace I2CConsts
 {
 static Boardcore::I2CDriver *ports[N_I2C_PORTS] =
@@ -359,7 +361,7 @@ void I2CDriver::setupPeripheral(const I2CSlaveConfig &slaveConfig)
         i2c->CCR =
             I2CConsts::f * 5;  // Setting the CCR bits (implicit Standard mode)
     }
-    else
+    else if (slaveConfig.speed == Speed::FAST)
     {
         // [WARNING] Hardcoded to use DUTY = 0
         i2c->CCR = I2C_CCR_FS |           // Selecting Fast mode
@@ -370,6 +372,10 @@ void I2CDriver::setupPeripheral(const I2CSlaveConfig &slaveConfig)
         //            I2C_CCR_DUTY |  // Selecting duty cycle of 9 - 16
         //            f * 2 / 5;      // Setting the CCR bits (f * 10 / 25)
     }
+    else
+    {
+        D(assert(false && "speed not supported!"));
+    }
 
     // Configuring the TRISE
     i2c->TRISE = (I2CConsts::f & I2C_CR2_FREQ) + 1;
@@ -397,8 +403,7 @@ bool I2CDriver::read(const I2CSlaveConfig &slaveConfig, void *buffer,
     i2c->CR1 =
         ((nBytes <= 1) ? (i2c->CR1 & ~I2C_CR1_ACK) : (i2c->CR1 | I2C_CR1_ACK));
 
-    // Sending doOperation when the channel isn't busy (LSB set to signal this
-    // is a read)
+    // Sending doOperation when the channel isn't busy
     return doOperation(slaveConfig);
 };
 
@@ -703,4 +708,6 @@ void I2CDriver::IRQhandleErrInterrupt()
     IRQwakeUpWaitingThread();
 }
 
-}  // namespace Boardcore
\ No newline at end of file
+}  // namespace Boardcore
+
+#endif  // _ARCH_CORTEXM4_STM32F4
\ No newline at end of file
diff --git a/src/shared/drivers/i2c/I2CDriver-f7.cpp b/src/shared/drivers/i2c/I2CDriver-f7.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..606e3835ad37592b200dbc0bfa43f5ea9e414775
--- /dev/null
+++ b/src/shared/drivers/i2c/I2CDriver-f7.cpp
@@ -0,0 +1,720 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifdef _ARCH_CORTEXM7_STM32F7
+
+#include <assert.h>
+#include <kernel/scheduler/scheduler.h>
+#include <utils/ClockUtils.h>
+#include <utils/Debug.h>
+
+#include "I2CDriver.h"
+
+namespace I2CConsts
+{
+static Boardcore::I2CDriver *ports[N_I2C_PORTS] =
+    {};  ///< Pointer to serial port classes to
+         ///< let interrupts access the classes
+static const int MAX_N_POLLING =
+    2000;  ///< Maximum number of cycles for polling
+static const int N_SCL_BITBANG =
+    16;  ///< Number of clocks created for slave locked bus recovery
+static const uint8_t I2C_PIN_ALTERNATE_FUNCTION =
+    4;             ///< Alternate Function number of the I2C peripheral pins
+static uint8_t f;  ///< APB peripheral clock frequency
+}  // namespace I2CConsts
+
+#ifdef I2C1
+/**
+ * I2C1 event interrupt
+ */
+void __attribute__((naked)) I2C1_EV_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z15I2C1HandlerImplv");
+    restoreContext();
+}
+
+/**
+ * I2C1 event interrupt actual implementation
+ */
+void __attribute__((used)) I2C1HandlerImpl()
+{
+    auto *port = I2CConsts::ports[0];
+    if (port)
+    {
+        port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * I2C1 error interrupt
+ */
+void __attribute__((naked)) I2C1_ER_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z18I2C1errHandlerImplv");
+    restoreContext();
+}
+
+/**
+ * I2C1 error interrupt actual implementation
+ */
+void __attribute__((used)) I2C1errHandlerImpl()
+{
+    auto *port = I2CConsts::ports[0];
+    if (port)
+    {
+        port->IRQhandleErrInterrupt();
+    }
+}
+#endif
+
+#ifdef I2C2
+/**
+ * I2C2 event interrupt
+ */
+void __attribute__((naked)) I2C2_EV_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z15I2C2HandlerImplv");
+    restoreContext();
+}
+
+/**
+ * I2C2 event interrupt actual implementation
+ */
+void __attribute__((used)) I2C2HandlerImpl()
+{
+    auto *port = I2CConsts::ports[1];
+    if (port)
+    {
+        port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * I2C2 error interrupt
+ */
+void __attribute__((naked)) I2C2_ER_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z18I2C2errHandlerImplv");
+    restoreContext();
+}
+
+/**
+ * I2C2 error interrupt actual implementation
+ */
+void __attribute__((used)) I2C2errHandlerImpl()
+{
+    auto *port = I2CConsts::ports[1];
+    if (port)
+    {
+        port->IRQhandleErrInterrupt();
+    }
+}
+#endif
+
+#ifdef I2C3
+/**
+ * I2C3 event interrupt
+ */
+void __attribute__((naked)) I2C3_EV_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z15I2C3HandlerImplv");
+    restoreContext();
+}
+
+/**
+ * I2C3 event interrupt actual implementation
+ */
+void __attribute__((used)) I2C3HandlerImpl()
+{
+    auto *port = I2CConsts::ports[2];
+    if (port)
+    {
+        port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * I2C3 error interrupt
+ */
+void __attribute__((naked)) I2C3_ER_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z18I2C3errHandlerImplv");
+    restoreContext();
+}
+
+/**
+ * I2C3 error interrupt actual implementation
+ */
+void __attribute__((used)) I2C3errHandlerImpl()
+{
+    auto *port = I2CConsts::ports[2];
+    if (port)
+    {
+        port->IRQhandleErrInterrupt();
+    }
+}
+#endif
+
+#ifdef I2C4
+/**
+ * I2C4 event interrupt
+ */
+void __attribute__((naked)) I2C4_EV_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z15I2C4HandlerImplv");
+    restoreContext();
+}
+
+/**
+ * I2C4 event interrupt actual implementation
+ */
+void __attribute__((used)) I2C4HandlerImpl()
+{
+    auto *port = I2CConsts::ports[3];
+    if (port)
+    {
+        port->IRQhandleInterrupt();
+    }
+}
+
+/**
+ * I2C4 error interrupt
+ */
+void __attribute__((naked)) I2C4_ER_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z18I2C4errHandlerImplv");
+    restoreContext();
+}
+
+/**
+ * I2C4 error interrupt actual implementation
+ */
+void __attribute__((used)) I2C4errHandlerImpl()
+{
+    auto *port = I2CConsts::ports[3];
+    if (port)
+    {
+        port->IRQhandleErrInterrupt();
+    }
+}
+#endif
+
+namespace Boardcore
+{
+
+I2CDriver::I2CDriver(I2C_TypeDef *i2c, miosix::GpioPin scl, miosix::GpioPin sda)
+    : i2c(i2c), scl(scl), sda(sda), transaction()
+{
+    // Setting the id and irqn of the right i2c peripheral
+    switch (reinterpret_cast<uint32_t>(i2c))
+    {
+#ifdef I2C1
+        case I2C1_BASE:
+            this->id = 1;
+            irqnEv   = I2C1_EV_IRQn;
+            irqnErr  = I2C1_ER_IRQn;
+            break;
+#endif
+#ifdef I2C2
+        case I2C2_BASE:
+            this->id = 2;
+            irqnEv   = I2C2_EV_IRQn;
+            irqnErr  = I2C2_ER_IRQn;
+            break;
+#endif
+#ifdef I2C3
+        case I2C3_BASE:
+            this->id = 3;
+            irqnEv   = I2C3_EV_IRQn;
+            irqnErr  = I2C3_ER_IRQn;
+            break;
+#endif
+#ifdef I2C4
+        case I2C4_BASE:
+            this->id = 4;
+            irqnEv   = I2C4_EV_IRQn;
+            irqnErr  = I2C4_ER_IRQn;
+            break;
+#endif
+        default:
+            // Checking that the peripheral is present in this architecture
+            D(assert(false &&
+                     "I2C peripheral not present in this architecture"));
+            break;
+    }
+
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Initializing the alternate function and mode of the pins so we won't
+        // forget the open-drain mode, avoiding eventual short-circuits between
+        // master and slaves when they both drive the same bus on two different
+        // logical values.
+        scl.alternateFunction(I2CConsts::I2C_PIN_ALTERNATE_FUNCTION);
+        sda.alternateFunction(I2CConsts::I2C_PIN_ALTERNATE_FUNCTION);
+        scl.mode(miosix::Mode::ALTERNATE_OD);
+        sda.mode(miosix::Mode::ALTERNATE_OD);
+    }
+
+    // Checking that this particular I2C port hasn't been already instantiated
+    D(assert(id > 0));
+    D(assert(I2CConsts::ports[id - 1] == nullptr));
+
+    // Enabling the peripheral's clock
+    ClockUtils::enablePeripheralClock(i2c);
+
+    init();
+
+    // Add to the array of i2c peripherals so that the interrupts can see it
+    I2CConsts::ports[id - 1] = this;
+
+    // Enabling the interrupts (Ev and Err) in the NVIC
+    NVIC_SetPriority(irqnEv, 15);
+    NVIC_ClearPendingIRQ(irqnEv);
+    NVIC_EnableIRQ(irqnEv);
+    NVIC_SetPriority(irqnErr, 15);
+    NVIC_ClearPendingIRQ(irqnErr);
+    NVIC_EnableIRQ(irqnErr);
+}
+
+I2CDriver::~I2CDriver()
+{
+    // Removing the relative i2c port from the array
+    I2CConsts::ports[id - 1] = nullptr;
+
+    // Disabling the interrupts (Ev and Err) in the NVIC
+    NVIC_DisableIRQ(irqnEv);
+    NVIC_DisableIRQ(irqnErr);
+
+    // Disabling the peripheral
+    i2c->CR1 &= ~I2C_CR1_PE;
+
+    // Disabling the peripheral on the bus
+    ClockUtils::disablePeripheralClock(i2c);
+}
+
+void I2CDriver::init()
+{
+    // Resetting the I2C peripheral before setting the registers (resetting PE
+    // bit resets the peripheral)
+    i2c->CR1 = 0;
+
+    // Retrieving the frequency of the APB relative to the I2C peripheral
+    // [MHz] (I2C peripherals are always connected to APB1, Low speed bus). In
+    // fact by default the I2C peripheral is clocked by the APB1 bus; anyway HSI
+    // and SYSCLK can be chosen.
+    I2CConsts::f =
+        ClockUtils::getAPBPeripheralsClock(ClockUtils::APB::APB1) / 1000000;
+
+    // I2CCLK < (t_low - t_filters) / 4
+    // I2CCLK < t_high
+    // I2CCLK < 4/3 * t_SCL
+}
+
+bool I2CDriver::read(const I2CSlaveConfig &slaveConfig, void *buffer,
+                     size_t nBytes)
+{
+    // Setting up the read transaction
+    transaction.operation    = Operation::READ;
+    transaction.buffWrite    = nullptr;
+    transaction.buffRead     = static_cast<uint8_t *>(buffer);
+    transaction.nBytes       = nBytes;
+    transaction.nBytesDone   = 0;
+    transaction.generateStop = true;
+
+    // enabling only the RX interrupts
+    i2c->CR1 &= ~I2C_CR1_TXIE;
+    i2c->CR1 |= I2C_CR1_RXIE;
+
+    // Sending doOperation when the channel isn't busy
+    return doOperation(slaveConfig);
+};
+
+bool I2CDriver::write(const I2CSlaveConfig &slaveConfig, const void *buffer,
+                      size_t nBytes, bool generateStop)
+{
+    // Setting up the write transaction
+    transaction.operation    = Operation::WRITE;
+    transaction.buffWrite    = static_cast<const uint8_t *>(buffer);
+    transaction.buffRead     = nullptr;
+    transaction.nBytes       = nBytes;
+    transaction.nBytesDone   = 0;
+    transaction.generateStop = generateStop;
+
+    // enabling only the TX interrupts
+    i2c->CR1 &= ~I2C_CR1_RXIE;
+    i2c->CR1 |= I2C_CR1_TXIE;
+
+    // Sending doOperation when the channel isn't busy
+    return doOperation(slaveConfig);
+};
+
+bool I2CDriver::doOperation(const I2CSlaveConfig &slaveConfig)
+{
+    // Not yet supported
+    D(assert(slaveConfig.addressing == Addressing::BIT7 &&
+             "Only 7-bit addressing supported!"));
+
+    // If already detected a locked state return directly without loosing time
+    if (lastError & Errors::BUS_LOCKED)
+    {
+        reStarting = false;
+        return false;
+    }
+
+    // If starting a new transaction (so STOP bit sent in previous transaction),
+    // wait for the bus to be clear
+    if (!reStarting)
+    {
+        // Waiting for the bus to be clear
+        uint32_t i{0};
+        for (; (i < I2CConsts::MAX_N_POLLING) && (i2c->ISR & I2C_ISR_BUSY); ++i)
+            ;
+
+        // Locked state detected after N polling cycles
+        if (i == I2CConsts::MAX_N_POLLING)
+        {
+            lastError = Errors::BUS_LOCKED;
+            LOG_ERR(logger, fmt::format("I2C{} bus locked state detected", id));
+            return false;
+        }
+
+        // Setting up the peripheral when the bus is clear in order to
+        // communicate in the mode wanted by the slave device
+        setupPeripheral(slaveConfig);
+    }
+
+    reStarting = false;
+
+    // Setting up transaction
+    setupTransaction();
+
+    // Starting the transaction with the START bit
+    // From the wait till the end of transaction it will all be executed in the
+    // event Interrupt Service Routine
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Sending the start condition
+        i2c->CR2 |= I2C_CR2_START;
+
+        // Making the thread wait for the operation completion. The next steps
+        // will be performed in the ISR while the thread stays in waiting state.
+        // The only way the thread will be waken up are the completion of the
+        // operation or an error during the transaction.
+        return IRQwaitForOperationCompletion(dLock);
+    }
+}
+
+void I2CDriver::setupPeripheral(const I2CSlaveConfig &slaveConfig)
+{
+    // Disabling the I2C peripheral before setting the registers
+    i2c->CR1 &= ~I2C_CR1_PE;
+
+    // setting the SCLH and SCLL bits in I2C_TIMINGR register to generate
+    // correct timings for each speed modes
+    if (slaveConfig.speed == Speed::STANDARD)
+    {
+        // PRESC = 0xb
+        // SCLL = 0x13
+        // SCLH = 0xf
+        // SDADEL = 0x2
+        // SCLDEL = 0x4
+        i2c->TIMINGR = (0xb << I2C_TIMINGR_PRESC_Pos) |   // PRESC
+                       (0x13 << I2C_TIMINGR_SCLL_Pos) |   // SCLL
+                       (0xf << I2C_TIMINGR_SCLH_Pos) |    // SCLH
+                       (0x2 << I2C_TIMINGR_SDADEL_Pos) |  // SDADEL
+                       (0x4 << I2C_TIMINGR_SCLDEL_Pos);   // SCLDEL
+    }
+    else if (slaveConfig.speed == Speed::FAST)
+    {
+        // PRESC = 0x5
+        // SCLL = 0x9
+        // SCLH = 0x3
+        // SDADEL = 0x3
+        // SCLDEL = 0x3
+        i2c->TIMINGR = (0x5 << I2C_TIMINGR_PRESC_Pos) |   // PRESC
+                       (0x9 << I2C_TIMINGR_SCLL_Pos) |    // SCLL
+                       (0x3 << I2C_TIMINGR_SCLH_Pos) |    // SCLH
+                       (0x3 << I2C_TIMINGR_SDADEL_Pos) |  // SDADEL
+                       (0x3 << I2C_TIMINGR_SCLDEL_Pos);   // SCLDEL
+    }
+    else if (slaveConfig.speed == Speed::FAST_PLUS)
+    {
+        // PRESC = 0x5
+        // SCLL = 0x3
+        // SCLH = 0x1
+        // SDADEL = 0x0
+        // SCLDEL = 0x1
+        i2c->TIMINGR = (0x5 << I2C_TIMINGR_PRESC_Pos) |   // PRESC
+                       (0x3 << I2C_TIMINGR_SCLL_Pos) |    // SCLL
+                       (0x1 << I2C_TIMINGR_SCLH_Pos) |    // SCLH
+                       (0x0 << I2C_TIMINGR_SDADEL_Pos) |  // SDADEL
+                       (0x1 << I2C_TIMINGR_SCLDEL_Pos);   // SCLDEL
+    }
+    else
+    {
+        D(assert(false && "speed not supported"));
+    }
+
+    // setting addressing mode, read or write mode and slave address
+    i2c->CR2 =
+        (slaveConfig.addressing == Addressing::BIT10 ? I2C_CR2_ADD10 : 0) |
+        (slaveConfig.slaveAddress << (I2C_CR2_SADD_Pos + 1));
+
+    // Finally enabling the peripheral
+    i2c->CR1 |= I2C_CR1_PE;
+}
+
+void I2CDriver::setupTransaction()
+{
+    // Setting the direction of the transaction
+    if (transaction.operation == Operation::READ)
+    {
+        i2c->CR2 |= I2C_CR2_RD_WRN;
+    }
+    else
+    {
+        i2c->CR2 &= ~I2C_CR2_RD_WRN;
+    }
+
+    // setting automatic stop generation if we won't generate a reStart
+    if (transaction.generateStop)
+    {
+        i2c->CR2 |= I2C_CR2_AUTOEND;
+    }
+    else
+    {
+        i2c->CR2 &= ~I2C_CR2_AUTOEND;
+    }
+
+    // setting registers for the remaining bytes
+    setupReload();
+}
+
+void I2CDriver::setupReload()
+{
+    if (transaction.nBytes - transaction.nBytesDone <= 0xffu)
+    {
+        i2c->CR2 &= ~I2C_CR2_RELOAD;
+        i2c->CR2 |= (transaction.nBytes - transaction.nBytesDone)
+                    << I2C_CR2_NBYTES_Pos;
+    }
+    else
+    {
+        i2c->CR2 |=
+            I2C_CR2_RELOAD |             // There must be a reload
+            0xff << I2C_CR2_NBYTES_Pos;  // maximum bytes that can be sent
+    }
+}
+
+void I2CDriver::flushBus()
+{
+    // If there isn't any locked state return immediately
+    if (!(lastError & Errors::BUS_LOCKED))
+    {
+        return;
+    }
+
+    // Set the period of the bit-banged clock (Default to standard mode)
+    uint8_t toggleDelay = 5;
+
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Recovery from the locked state due to a stuck Slave.
+        // We bit-bang 16 clocks on the scl line in order to restore pending
+        // packets of the slaves.
+        scl.mode(miosix::Mode::OPEN_DRAIN);
+    }
+
+    for (size_t c = 0; c < I2CConsts::N_SCL_BITBANG; c++)
+    {
+        scl.low();
+        miosix::delayUs(toggleDelay);
+        scl.high();
+        miosix::delayUs(toggleDelay);
+    }
+
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // We set again the scl pin to the correct Alternate function
+        scl.mode(miosix::Mode::ALTERNATE_OD);
+        scl.alternateFunction(I2CConsts::I2C_PIN_ALTERNATE_FUNCTION);
+    }
+
+    // Re-initializing the peripheral in order to avoid inconsistent state
+    init();
+
+    // Assuming the locked state is solved. If it is not the case, only when
+    // it will be the case it will be detected again
+    lastError = Errors::NO_ERROR;
+
+    LOG_WARN(logger, fmt::format("I2C{} Bus flushed", id));
+}
+
+uint16_t I2CDriver::getLastError() { return lastError; }
+
+inline bool I2CDriver::IRQwaitForOperationCompletion(
+    miosix::FastInterruptDisableLock &dLock)
+{
+    // Saving the current thread in order to be waken up by interrupts
+    waiting = miosix::Thread::IRQgetCurrentThread();
+
+    // enabling interrupts for errors
+    i2c->CR1 |= I2C_CR1_ERRIE |   // interrupt for errors
+                I2C_CR1_NACKIE |  // interrupt for NACKs
+                I2C_CR1_TCIE |    // interrupt for TC and TCR
+                I2C_CR1_STOPIE;   // interrupt for STOP detected
+
+    // flag thread as waiting, enable interrupts in I2C peripheral and yield
+    // till an interrupt doesn't wake up the thread
+    while (waiting)
+    {
+        waiting->IRQwait();
+        miosix::FastInterruptEnableLock eLock(dLock);
+        waiting->yield();
+    }
+
+    // If error occurred, parse it to notify the error(s); otherwise reset
+    // lastError parameter
+    if (error)
+    {
+        lastError |= ((error & I2C_ISR_OVR) ? Errors::OVR : 0) |
+                     ((error & I2C_ISR_BERR) ? Errors::BERR : 0) |
+                     ((error & I2C_ISR_ARLO) ? Errors::ARLO : 0);
+        error = 0;
+
+        return false;
+    }
+
+    lastError = Errors::NO_ERROR;
+    return true;
+}
+
+inline void I2CDriver::IRQwakeUpWaitingThread()
+{
+    // invalidating the buffer pointers (avoiding to keep a pointer to an old
+    // memory location)
+    transaction.buffRead  = nullptr;
+    transaction.buffWrite = nullptr;
+
+    // disabling interrupts for errors
+    i2c->CR1 &= ~(I2C_CR1_ERRIE |   // interrupt for errors
+                  I2C_CR1_NACKIE |  // interrupt for NACKs
+                  I2C_CR1_TCIE |    // interrupt for TC and TCR
+                  I2C_CR1_STOPIE);  // interrupt for STOP detected
+
+    if (waiting)
+    {
+        waiting->IRQwakeup();
+
+        if (waiting->IRQgetPriority() >
+            miosix::Thread::IRQgetCurrentThread()->IRQgetPriority())
+        {
+            miosix::Scheduler::IRQfindNextThread();
+        }
+
+        waiting = nullptr;
+    }
+}
+
+void I2CDriver::IRQhandleInterrupt()
+{
+    // Transfer complete reload: setting registers for the remaining bytes
+    if (i2c->ISR & I2C_ISR_TCR)
+    {
+        setupReload();
+    }
+
+    // If NACK reception, return error
+    if (i2c->ISR & I2C_ISR_NACKF)
+    {
+        // "reserved" bit in the ISR register, so we don't collide with
+        // other fields
+        error = 1 << 14;
+        lastError |= Errors::AF;
+        i2c->ICR |= I2C_ICR_NACKCF;
+        IRQwakeUpWaitingThread();
+        return;
+    }
+
+    if (transaction.nBytesDone < transaction.nBytes)
+    {
+        // Performing the read/write
+        if (i2c->ISR & I2C_ISR_RXNE)
+        {
+            // READ
+            transaction.buffRead[transaction.nBytesDone] = i2c->RXDR;
+            transaction.nBytesDone++;
+        }
+        else if (i2c->ISR & I2C_ISR_TXIS)
+        {
+            // WRITE
+            i2c->TXDR = transaction.buffWrite[transaction.nBytesDone];
+            transaction.nBytesDone++;
+        }
+    }
+    else
+    {
+        // Sending STOP condition and wake up thread
+        if (!transaction.generateStop)
+        {
+            reStarting = true;
+
+            // waking up the waiting thread
+            IRQwakeUpWaitingThread();
+        }
+    }
+
+    if (i2c->ISR & I2C_ISR_STOPF)
+    {
+        // waking up the waiting thread after stop condition generated
+        IRQwakeUpWaitingThread();
+    }
+}
+
+void I2CDriver::IRQhandleErrInterrupt()
+{
+    error = i2c->ISR | (1 << 24);
+
+    // Clearing all the errors in the register
+    i2c->ICR |= I2C_ICR_ADDRCF | I2C_ICR_ARLOCF | I2C_ICR_BERRCF |
+                I2C_ICR_OVRCF | I2C_ICR_STOPCF;
+
+    // Waking up the waiting thread
+    IRQwakeUpWaitingThread();
+}
+
+}  // namespace Boardcore
+
+#endif  // _ARCH_CORTEXM7_STM32F7
\ No newline at end of file
diff --git a/src/shared/drivers/i2c/I2CDriver.h b/src/shared/drivers/i2c/I2CDriver.h
index cf205d404a5383da87419678700e2dc8d421f5a8..9d639636f496188ab786134e8db81909acef545f 100644
--- a/src/shared/drivers/i2c/I2CDriver.h
+++ b/src/shared/drivers/i2c/I2CDriver.h
@@ -62,10 +62,12 @@ public:
         READ  = 1
     };
 
+    // [TODO] limit speed possibilities at compile time with ifdefs?
     enum Speed : uint8_t
     {
-        STANDARD = 0,
-        FAST     = 1
+        STANDARD  = 0,
+        FAST      = 1,
+        FAST_PLUS = 2
     };
 
     enum Addressing : uint8_t
@@ -239,6 +241,12 @@ private:
      */
     void setupPeripheral(const I2CSlaveConfig &slaveConfig);
 
+#ifdef _ARCH_CORTEXM7_STM32F7
+    inline void setupTransaction();
+
+    inline void setupReload();
+#endif  // _ARCH_CORTEXM7_STM32F7
+
     /**
      * @brief Method to perform a read or write operation.
      *
@@ -283,7 +291,7 @@ private:
     miosix::GpioPin sda;  ///< GpioPin of the serial data pin
 
     uint16_t lastError = NO_ERROR;  ///< Flag for the last error occurred
-    uint16_t error     = 0;         ///< Flag that tells if an error occurred
+    uint32_t error     = 0;         ///< Flag that tells if an error occurred
     bool reStarting = false;    ///< Flag true if not generated a STOP condition
     miosix::Thread *waiting{};  ///< Pointer to the waiting for event thread
     I2CTransaction transaction;  ///< Struct storing the transaction info
diff --git a/src/tests/drivers/i2c/test-i2c-driver.cpp b/src/tests/drivers/i2c/test-i2c-driver.cpp
index 80f3e52953f4dd850b535c197d7b9b848d7fa898..ce9d8ceebe2d091c282167427e8e191441e02bab 100644
--- a/src/tests/drivers/i2c/test-i2c-driver.cpp
+++ b/src/tests/drivers/i2c/test-i2c-driver.cpp
@@ -84,25 +84,32 @@ I2CSensor OLED{0b0111100, 0xD0, 0x43, {}};
 I2CDriver::I2CSlaveConfig BMP180Config{BMP180.addressSensor,
                                        I2CDriver::Addressing::BIT7,
                                        I2CDriver::Speed::STANDARD};
+
 I2CDriver::I2CSlaveConfig BME280Config{BME280.addressSensor,
                                        I2CDriver::Addressing::BIT7,
                                        I2CDriver::Speed::STANDARD};
-I2CDriver::I2CSlaveConfig OLEDConfig{
+
+I2CDriver::I2CSlaveConfig OLEDConfig_F{
     OLED.addressSensor, I2CDriver::Addressing::BIT7, I2CDriver::Speed::FAST};
 
+#ifdef _ARCH_CORTEXM7_STM32F7
+I2CDriver::I2CSlaveConfig OLEDConfig_FP{OLED.addressSensor,
+                                        I2CDriver::Addressing::BIT7,
+                                        I2CDriver::Speed::FAST_PLUS};
+#endif  // _ARCH_CORTEXM7_STM32F7
+
 bool i2cDriver(I2CDriver &i2c, I2CSensor sensor,
                I2CDriver::I2CSlaveConfig sensorConfig)
 {
     uint8_t buffer[10]  = {0};
-    const uint8_t nRead = 10;
+    const uint8_t nRead = 1;
 
     i2c.flushBus();
 
     // reset the sensor and then read the whoami
     if (!(i2c.write(sensorConfig, sensor.softReset, 2) &&
           i2c.write(sensorConfig, &sensor.whoamiRegister, 1, false) &&
-          i2c.read(sensorConfig, buffer, nRead) &&
-          (buffer[0] == sensor.whoamiContent)))
+          i2c.read(sensorConfig, buffer, nRead)))
     {
         uint16_t lastError{i2c.getLastError()};
         if (!(lastError &
@@ -113,6 +120,13 @@ bool i2cDriver(I2CDriver &i2c, I2CSensor sensor,
         return false;
     }
 
+    if (buffer[0] != sensor.whoamiContent)
+    {
+        printf("whoami expected %d, received %d\n", sensor.whoamiContent,
+               buffer[0]);
+        return false;
+    }
+
     return true;
 }
 
@@ -138,8 +152,11 @@ int main()
 
         for (int i = 0; i < nRepeat; i++)
         {
-            statusOLED &= i2cDriver(i2c, OLED, OLEDConfig);
             statusBMP &= i2cDriver(i2c, BMP180, BMP180Config);
+            statusOLED &= i2cDriver(i2c, OLED, OLEDConfig_F);
+#ifdef _ARCH_CORTEXM7_STM32F7
+            statusOLED &= i2cDriver(i2c, OLED, OLEDConfig_FP);
+#endif  // _ARCH_CORTEXM7_STM32F7
         }
 
         printf("OLED:%d\tBMP:%d\n", statusOLED, statusBMP);
diff --git a/src/tests/drivers/i2c/test-i2c.cpp b/src/tests/drivers/i2c/test-i2c.cpp
index 38d1554ef8b2f5a9fff7dbdc2c47f376584b08cd..ac292392e681fbcf4b81a18f4f81553544240805 100644
--- a/src/tests/drivers/i2c/test-i2c.cpp
+++ b/src/tests/drivers/i2c/test-i2c.cpp
@@ -83,13 +83,21 @@ I2CSensor OLED{0b0111100, 0xD0, 0x43, {}};
 I2CDriver::I2CSlaveConfig BMP180Config{BMP180.addressSensor,
                                        I2CDriver::Addressing::BIT7,
                                        I2CDriver::Speed::STANDARD};
+
 I2CDriver::I2CSlaveConfig BME280Config{BME280.addressSensor,
                                        I2CDriver::Addressing::BIT7,
                                        I2CDriver::Speed::STANDARD};
-I2CDriver::I2CSlaveConfig OLEDConfig{
+
+I2CDriver::I2CSlaveConfig OLEDConfig_F{
     OLED.addressSensor, I2CDriver::Addressing::BIT7, I2CDriver::Speed::FAST};
 
-bool i2cDriver(SyncedI2C &i2c, I2CSensor sensor,
+#ifdef _ARCH_CORTEXM7_STM32F7
+I2CDriver::I2CSlaveConfig OLEDConfig_FP{OLED.addressSensor,
+                                        I2CDriver::Addressing::BIT7,
+                                        I2CDriver::Speed::FAST_PLUS};
+#endif  // _ARCH_CORTEXM7_STM32F7
+
+bool i2cDriver(I2C &i2c, I2CSensor sensor,
                I2CDriver::I2CSlaveConfig sensorConfig)
 {
     uint8_t whoamiContent = 0;
@@ -98,32 +106,22 @@ bool i2cDriver(SyncedI2C &i2c, I2CSensor sensor,
 
     // reset the sensor and then read the whoami
     if (!(i2c.probe(sensorConfig) &&
-          i2c.writeRegister(sensorConfig, sensor.softReset[0],
-                            sensor.softReset[1]) &&
-          i2c.readRegister(sensorConfig, sensor.whoamiRegister,
-                           whoamiContent) &&
-          i2c.readFromRegister(sensorConfig, sensor.whoamiRegister, buffer,
-                               nRead)))
+          i2c.write(sensorConfig, sensor.softReset, 2) &&
+          i2c.readRegister(sensorConfig, sensor.whoamiRegister, buffer)))
     {
         uint16_t lastError{i2c.getLastError()};
-        if (!(lastError & I2CDriver::Errors::AF))
+        if (!(lastError &
+              (I2CDriver::Errors::AF | I2CDriver::Errors::BUS_LOCKED)))
         {
             printf("LastError: %d\n", lastError);
         }
         return false;
     }
 
-    if (whoamiContent != sensor.whoamiContent)
+    if (buffer != sensor.whoamiContent)
     {
         printf("whoami expected %d, received %d\n", sensor.whoamiContent,
-               whoamiContent);
-        return false;
-    }
-
-    if (buffer[0] != sensor.whoamiContent)
-    {
-        printf("buffer[0] expected %d, received %d\n", sensor.whoamiContent,
-               buffer[0]);
+               buffer);
         return false;
     }
 
@@ -152,8 +150,11 @@ int main()
 
         for (int i = 0; i < nRepeat; i++)
         {
-            statusOLED &= i2cDriver(i2c, OLED, OLEDConfig);
             statusBMP &= i2cDriver(i2c, BMP180, BMP180Config);
+            statusOLED &= i2cDriver(i2c, OLED, OLEDConfig_F);
+#ifdef _ARCH_CORTEXM7_STM32F7
+            statusOLED &= i2cDriver(i2c, OLED, OLEDConfig_FP);
+#endif  // _ARCH_CORTEXM7_STM32F7
         }
 
         printf("OLED:%d\tBMP:%d\n", statusOLED, statusBMP);