diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5cd9dd4d171df80edbf0fb5ccd951a6041775814..0082bcd90b01663b5fc29e92dedfa58b36b6aa2c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -242,6 +242,12 @@ 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 src/tests/drivers/i2c/test-i2c.cpp)
+sbs_target(test-i2c stm32f429zi_stm32f4discovery)
+
 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 2f473be9fd915bc365be4eb26081eaee0a935119..5632efa3cea92276884ecff0f5f77a7a3cb428ae 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -51,13 +51,14 @@ foreach(OPT_BOARD ${BOARDS})
         ${SBS_BASE}/src/shared/drivers/canbus/CanDriver/CanDriver.cpp
         ${SBS_BASE}/src/shared/drivers/canbus/CanDriver/CanInterrupt.cpp
         ${SBS_BASE}/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
-        ${SBS_BASE}/src/shared/drivers/i2c/stm32f2_f4_i2c.cpp
         ${SBS_BASE}/src/shared/drivers/interrupt/external_interrupts.cpp
         ${SBS_BASE}/src/shared/drivers/timer/PWM.cpp
         ${SBS_BASE}/src/shared/drivers/timer/TimestampTimer.cpp
         ${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/I2C.cpp
 
         # Events
         ${SBS_BASE}/src/shared/events/EventBroker.cpp
diff --git a/src/shared/drivers/i2c/I2C.cpp b/src/shared/drivers/i2c/I2C.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..299e0ff197ddbb6f1dca291e8a367a6bf296af7c
--- /dev/null
+++ b/src/shared/drivers/i2c/I2C.cpp
@@ -0,0 +1,94 @@
+/* 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.
+ */
+
+#include "I2C.h"
+
+namespace Boardcore
+{
+
+I2C::I2C(I2C_TypeDef *i2c, miosix::GpioPin scl, miosix::GpioPin sda)
+    : i2c(i2c, scl, sda)
+{
+}
+
+bool I2C::read(I2CDriver::I2CSlaveConfig slaveConfig, void *buffer,
+               size_t nBytes)
+{
+    i2c.flushBus();
+    return i2c.read(slaveConfig, buffer, nBytes);
+}
+
+bool I2C::write(I2CDriver::I2CSlaveConfig slaveConfig, const void *buffer,
+                size_t nBytes)
+{
+    i2c.flushBus();
+    return i2c.write(slaveConfig, buffer, nBytes);
+}
+
+bool I2C::readRegister(I2CDriver::I2CSlaveConfig slaveConfig,
+                       const uint8_t registerAddress, uint8_t &registerContent)
+{
+    i2c.flushBus();
+    return i2c.write(slaveConfig, &registerAddress, 1, false) &&
+           i2c.read(slaveConfig, &registerContent, 1);
+}
+
+bool I2C::probe(I2CDriver::I2CSlaveConfig slaveConfig)
+{
+    i2c.flushBus();
+    return i2c.write(slaveConfig, nullptr, 0);
+}
+
+SyncedI2C::SyncedI2C(I2C_TypeDef *i2c, miosix::GpioPin scl, miosix::GpioPin sda)
+    : I2C(i2c, scl, sda)
+{
+}
+
+bool SyncedI2C::read(I2CDriver::I2CSlaveConfig slaveConfig, void *buffer,
+                     size_t nBytes)
+{
+    miosix::Lock<miosix::FastMutex> lock(mutex);
+    return I2C::read(slaveConfig, buffer, nBytes);
+}
+
+bool SyncedI2C::write(I2CDriver::I2CSlaveConfig slaveConfig, const void *buffer,
+                      size_t nBytes)
+{
+    miosix::Lock<miosix::FastMutex> lock(mutex);
+    return I2C::write(slaveConfig, buffer, nBytes);
+}
+
+bool SyncedI2C::readRegister(I2CDriver::I2CSlaveConfig slaveConfig,
+                             const uint8_t registerAddress,
+                             uint8_t registerContent)
+{
+    miosix::Lock<miosix::FastMutex> lock(mutex);
+    return I2C::readRegister(slaveConfig, registerAddress, registerContent);
+}
+
+bool SyncedI2C::probe(I2CDriver::I2CSlaveConfig slaveConfig)
+{
+    miosix::Lock<miosix::FastMutex> lock(mutex);
+    return I2C::probe(slaveConfig);
+}
+
+}  // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/drivers/i2c/I2C.h b/src/shared/drivers/i2c/I2C.h
new file mode 100644
index 0000000000000000000000000000000000000000..b161f70cbdb203ed621357e913117daaa992b8c2
--- /dev/null
+++ b/src/shared/drivers/i2c/I2C.h
@@ -0,0 +1,189 @@
+/* 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.
+ */
+
+#include "I2CDriver.h"
+namespace Boardcore
+{
+
+/**
+ * @brief High level driver for the I2C peripherals.
+ *
+ * This driver is NOT thread safe. It implements high level functionalities such
+ * as:
+ * - Automatic bus recovery before each operation;
+ * - Method in order to read a one-byte register without issuing a stop
+ * condition (unique transaction).
+ */
+class I2C
+{
+public:
+    /**
+     * @brief Constructor for the I2C high-level driver.
+     *
+     * It uses the low-level I2C constructor that internally initializes the
+     * pins so that they are always set to ALTERNATE_OD mode with Alternate
+     * Function 4 (the usual AF of I2C pins). Thanks to this we avoid the
+     * possibility of short circuits between master and slaves when they both
+     * drive the same bus on two different logical values.
+     *
+     * @param i2c Structure that represents the I2C peripheral.
+     * @param scl Serial clock GpioPin of the relative I2C peripheral.
+     * @param sda Serial data GpioPin of the relative I2C peripheral.
+     */
+    I2C(I2C_TypeDef *i2c, miosix::GpioPin scl, miosix::GpioPin sda);
+
+    /**
+     * @brief Non blocking read operation to read nBytes.
+     *
+     * This method, if necessary, flushes the bus before the read operation is
+     * performed. In case of an error during the communication, this method
+     * returns false immediately.
+     * @warning Check always if the operation succeeded or not!
+     * @param slaveConfig The configuration struct of the slave device.
+     * @param buffer Data buffer where to store the data read.
+     * @param nBytes Number of bytes to read.
+     * @returns True if the read is successful, false otherwise.
+     */
+    [[nodiscard]] bool read(I2CDriver::I2CSlaveConfig slaveConfig, void *buffer,
+                            size_t nBytes);
+
+    /**
+     * @brief Non blocking write operation to write nBytes.
+     *
+     * This method, if necessary, flushes the bus before the read operation is
+     * performed. In case of an error during the communication, this method
+     * returns false immediately.
+     * @warning Check always if the operation succeeded or not!
+     * @param slaveConfig The configuration struct of the slave device.
+     * @param buffer Data buffer where to read the data to send.
+     * @param nBytes Number of bytes to send.
+     * @returns True if the write is successful, false otherwise.
+     */
+    [[nodiscard]] bool write(I2CDriver::I2CSlaveConfig slaveConfig,
+                             const void *buffer, size_t nBytes);
+
+    /**
+     * @brief Non blocking operation to read a 1-byte register from a slave.
+     *
+     * This method, if necessary, flushes the bus before the read operation is
+     * performed. In case of an error during the communication, this method
+     * returns false immediately.
+     * @warning Check always if the operation succeeded or not!
+     * @param slaveConfig The configuration struct of the slave device.
+     * @param registerAddress Byte that represents the address of the register.
+     * @param registerContent Where to store the content of the register.
+     * @returns True if the write is successful, false otherwise.
+     */
+    [[nodiscard]] bool readRegister(I2CDriver::I2CSlaveConfig slaveConfig,
+                                    const uint8_t registerAddress,
+                                    uint8_t &registerContent);
+
+    /**
+     * @brief Non blocking operation to check if a slave is available.
+     *
+     * @warning Check always if the operation succeeded or not!
+     * @param slaveConfig The configuration struct of the slave device.
+     * @returns True if the device is available, false otherwise.
+     */
+    [[nodiscard]] bool probe(I2CDriver::I2CSlaveConfig slaveConfig);
+
+protected:
+    I2CDriver i2c;  ///< Instance of I2C low-level driver
+};
+
+/**
+ * @brief Thread safe version of the I2C high-level driver.
+ */
+class SyncedI2C : public I2C
+{
+public:
+    /**
+     * @brief Constructor for the synced I2C high-level driver.
+     *
+     * @param i2c Structure that represents the I2C peripheral.
+     * @param scl Serial clock GpioPin of the relative I2C peripheral.
+     * @param sda Serial data GpioPin of the relative I2C peripheral.
+     */
+    SyncedI2C(I2C_TypeDef *i2c, miosix::GpioPin scl, miosix::GpioPin sda);
+
+    /**
+     * @brief Read operation to read nBytes.
+     *
+     * This method could have to wait that no other thread is trying to do some
+     * operation on the bus. In case of an error during the communication, this
+     * method returns false immediately.
+     * @warning Check always if the operation succeeded or not!
+     * @param slaveConfig The configuration struct of the slave device.
+     * @param buffer Data buffer where to store the data read.
+     * @param nBytes Number of bytes to read.
+     * @returns True if the read is successful, false otherwise.
+     */
+    [[nodiscard]] bool read(I2CDriver::I2CSlaveConfig slaveConfig, void *buffer,
+                            size_t nBytes);
+
+    /**
+     * @brief Write operation to write nBytes.
+     *
+     * This method could have to wait that no other thread is trying to do some
+     * operation on the bus. In case of an error during the communication, this
+     * method returns false immediately.
+     * @warning Check always if the operation succeeded or not!
+     * @param slaveConfig The configuration struct of the slave device.
+     * @param buffer Data buffer where to read the data to send.
+     * @param nBytes Number of bytes to send.
+     * @returns True if the write is successful, false otherwise.
+     */
+    [[nodiscard]] bool write(I2CDriver::I2CSlaveConfig slaveConfig,
+                             const void *buffer, size_t nBytes);
+
+    /**
+     * @brief Read a one-byte register from the device.
+     *
+     * This method could have to wait that no other thread is trying to do some
+     * operation on the bus. In case of an error during the communication, this
+     * method returns false immediately.
+     * @warning Check always if the operation succeeded or not!
+     * @param slaveConfig The configuration struct of the slave device.
+     * @param registerAddress Byte that represents the address of the register.
+     * @param registerContent Where to store the content of the register.
+     * @returns True if the write is successful, false otherwise.
+     */
+    [[nodiscard]] bool readRegister(I2CDriver::I2CSlaveConfig slaveConfig,
+                                    const uint8_t registerAddress,
+                                    uint8_t registerContent);
+
+    /**
+     * @brief Check if a slave is available.
+     *
+     * This method could have to wait that no other thread is trying to do some
+     * operation on the bus.
+     * @warning Check always if the operation succeeded or not!
+     * @param slaveConfig The configuration struct of the slave device.
+     * @returns true if the device is available, false otherwise.
+     */
+    [[nodiscard]] bool probe(I2CDriver::I2CSlaveConfig slaveConfig);
+
+private:
+    miosix::FastMutex mutex;  ///< Mutex for rx/tx
+};
+
+}  // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/drivers/i2c/I2CDriver.cpp b/src/shared/drivers/i2c/I2CDriver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c03f30d7cc2b710c17e9a479cf410c8fedcfb36b
--- /dev/null
+++ b/src/shared/drivers/i2c/I2CDriver.cpp
@@ -0,0 +1,750 @@
+/* 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.
+ */
+
+#include "I2CDriver.h"
+
+#include <assert.h>
+#include <kernel/scheduler/scheduler.h>
+#include <utils/ClockUtils.h>
+#include <utils/Debug.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_ADDRESS_READ  = 0x1;  ///< LSB of address to read
+static const uint8_t I2C_ADDRESS_WRITE = 0x0;  ///< LSB of address to write
+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)
+{
+    // 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
+    i2c->CR1 = I2C_CR1_SWRST;
+    i2c->CR1 = 0;  // cppcheck-suppress redundantAssignment
+
+    // Retrieving the frequency of the APB relative to the I2C peripheral
+    // [MHz] (I2C peripherals are always connected to APB1, Low speed bus)
+    I2CConsts::f =
+        ClockUtils::getAPBPeripheralsClock(ClockUtils::APB::APB1) / 1000000;
+
+    // Frequency higher than 50MHz not allowed by I2C peripheral
+    D(assert(I2CConsts::f <= 50));
+
+    // Programming the input clock in order to generate correct timings +
+    // enabling generation of all interrupts
+    i2c->CR2 = (I2CConsts::f & I2C_CR2_FREQ) |  // setting FREQ bits
+               I2C_CR2_ITBUFEN;  // enabling interupts for rx/tx byte
+}
+
+void I2CDriver::setupPeripheral(I2CSlaveConfig slaveConfig)
+{
+    // Frequency < 2MHz in standard mode or < 4MHz in fast mode not allowed by
+    // the peripheral
+    D(assert((slaveConfig.speed == STANDARD && I2CConsts::f >= 2) ||
+             (slaveConfig.speed == FAST && I2CConsts::f >= 4)));
+
+    // Disabling the I2C peripheral before setting the registers
+    i2c->CR1 &= ~I2C_CR1_PE;
+
+    // Configuring the Clock Control Register
+    if (slaveConfig.speed == Speed::STANDARD)
+    {
+        // If STANDARD mode, this is the divider to the peripheral clock to
+        // reach the wanted frequency. It's divided by 2 because in reality
+        // it uses this value to calculate the time that the clock needs to
+        // be in the "set" state. [* 1000 KHz / (100 KHz * 2) = *5]
+        i2c->CCR =
+            I2CConsts::f * 5;  // Setting the CCR bits (implicit Standard mode)
+    }
+    else
+    {
+        // [WARNING] Hardcoded to use DUTY = 0
+        i2c->CCR = I2C_CCR_FS |           // Selecting Fast mode
+                   I2CConsts::f * 5 / 6;  // Setting the CCR bits
+
+        // For DUTY = 1
+        // i2c->CCR = I2C_CCR_FS |    // Selecting Fast mode
+        //            I2C_CCR_DUTY |  // Selecting duty cycle of 9 - 16
+        //            f * 2 / 5;      // Setting the CCR bits (f * 10 / 25)
+    }
+
+    // Configuring the TRISE
+    i2c->TRISE = (I2CConsts::f & I2C_CR2_FREQ) + 1;
+
+    // Setting the addressing mode
+    i2c->OAR1 = (slaveConfig.addressing << 15);
+
+    // Finally enabling the peripheral
+    i2c->CR1 |= I2C_CR1_PE;
+}
+
+bool I2CDriver::read(I2CSlaveConfig slaveConfig, void *buffer, size_t nBytes)
+{
+    auto *buff = static_cast<uint8_t *>(buffer);
+
+    // Enabling option to generate ACK
+    i2c->CR1 |= I2C_CR1_ACK;
+
+    // Modifying the slave address to be ready to be sent
+    slaveConfig.slaveAddress =
+        (slaveConfig.slaveAddress << 1 | I2CConsts::I2C_ADDRESS_READ);
+
+    // Sending prologue when the channel isn't busy (LSB set to signal this
+    // is a read)
+    if (!prologue(slaveConfig))
+    {
+        return false;
+    }
+
+    // Disabling the generation of the ACK if reading only 1 byte
+    if (nBytes <= 1)
+    {
+        i2c->CR1 &= ~I2C_CR1_ACK;
+    }
+
+    // Reading the nBytes
+    for (size_t i = 0; i < nBytes; i++)
+    {
+        {
+            miosix::FastInterruptDisableLock dLock;
+
+            // Waiting for the reception of another byte
+            if (!IRQwaitForRegisterChange(dLock) || !(i2c->SR1 & I2C_SR1_RXNE))
+            {
+                i2c->CR1 |= I2C_CR1_STOP;
+                return false;
+            }
+        }
+
+        // Checking if a byte has been lost
+        if (i2c->SR1 & I2C_SR1_BTF)
+            ;
+
+        buff[i] = i2c->DR;
+
+        // Clearing the ACK flag in order to send a NACK on the last byte that
+        // will be read
+        if (i == nBytes - 2)
+        {
+            i2c->CR1 &= ~I2C_CR1_ACK;
+        }
+    }
+
+    // Generate the stop condition after the read transaction
+    i2c->CR1 |= I2C_CR1_STOP;
+
+    return true;
+};
+
+bool I2CDriver::write(I2CSlaveConfig slaveConfig, const void *buffer,
+                      size_t nBytes, bool generateStop)
+{
+    auto *buff = static_cast<const uint8_t *>(buffer);
+
+    // Modifying the slave address to be ready to be sent
+    slaveConfig.slaveAddress =
+        (slaveConfig.slaveAddress << 1 | I2CConsts::I2C_ADDRESS_WRITE);
+
+    // Sending prologue when the channel isn't busy
+    if (!prologue(slaveConfig))
+    {
+        return false;
+    }
+
+    // Sending the nBytes
+    for (size_t i = 0; i < nBytes; i++)
+    {
+        miosix::FastInterruptDisableLock dLock;
+        i2c->DR = buff[i];
+
+        // Waiting for the sending of the byte
+        if (!IRQwaitForRegisterChange(dLock) || !(i2c->SR1 & I2C_SR1_TXE))
+        {
+            i2c->CR1 |= I2C_CR1_STOP;
+            return false;
+        }
+    }
+
+    // If we are on the last byte, generate the stop condition if we have to end
+    // the communication
+    if (generateStop)
+    {
+        i2c->CR1 |= I2C_CR1_STOP;
+        reStarting = false;
+    }
+    else
+    {
+        reStarting = true;
+    }
+
+    return true;
+};
+
+bool I2CDriver::prologue(I2CSlaveConfig slaveConfig)
+{
+    // If already detected a locked state return directly without loosing time
+    if (lockedState)
+    {
+        reStarting = false;
+        return false;
+    }
+
+    if (!reStarting)
+    {
+        // Waiting for the bus to be clear
+        uint32_t i{0};
+        for (; (i < I2CConsts::MAX_N_POLLING) && (i2c->SR2 & I2C_SR2_BUSY); ++i)
+            ;
+
+        // Locked state detected after N polling cycles
+        if (i == I2CConsts::MAX_N_POLLING)
+        {
+            lockedState = true;
+            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;
+
+    {
+        miosix::FastInterruptDisableLock dLock;
+        uint32_t i{0};
+
+        // Sending the start condition
+        // We are waiting on the Start Bit using polling because using
+        // interrupts would have lead to a messy and less reliable code. The
+        // only downside regards the usage of polling instead of interrupts; the
+        // maximum number of cycles used in the tests for waiting for a start
+        // condition were more or less 950. Anyway this is preferred to the risk
+        // of having a deadlock.
+        i2c->CR1 |= I2C_CR1_START;
+
+        // Waiting for START condition to be sent
+        for (; (i < I2CConsts::MAX_N_POLLING) &&
+               (!(i2c->SR1 & I2C_SR1_SB) || !(i2c->SR2 & I2C_SR2_MSL));
+             ++i)
+            ;
+
+        // START condition not sent after N polling cycles
+        if (i == I2CConsts::MAX_N_POLLING)
+        {
+            miosix::FastInterruptEnableLock eLock(dLock);
+            LOG_ERR(logger,
+                    fmt::format("I2C{} bus didn't sent the start bit", id));
+            return false;
+        }
+    }
+
+    // Sending (header + ) slave address
+    if (slaveConfig.addressing == Addressing::BIT7)
+    {
+        miosix::FastInterruptDisableLock dLock;
+
+        // Setting the LSB if we want to enter receiver mode
+        i2c->DR = slaveConfig.slaveAddress;
+
+        // Checking if a slave matched his address
+        if (!IRQwaitForRegisterChange(dLock) || !(i2c->SR1 & I2C_SR1_ADDR))
+        {
+            i2c->CR1 |= I2C_CR1_STOP;
+            return false;
+        }
+    }
+    else  // addressing == Addressing::BIT10
+    {
+        // Header generated (composed of 11110xx0 bits with xx as the 9th
+        // and 8th bits of the address)
+        const uint8_t header =
+            0b11110000 | ((slaveConfig.slaveAddress >> 7) & 0b110);
+
+        {
+            miosix::FastInterruptDisableLock dLock;
+
+            // Sending header
+            i2c->DR = header;
+
+            // Checking if the header has been sent
+            if (!IRQwaitForRegisterChange(dLock) || !(i2c->SR1 & I2C_SR1_ADD10))
+            {
+                i2c->CR1 |= I2C_CR1_STOP;
+                return false;
+            }
+        }
+
+        {
+            miosix::FastInterruptDisableLock dLock;
+
+            // Sending address ((1 << 8) - 1) = 0xff
+            i2c->DR = (slaveConfig.slaveAddress & 0xff);
+
+            // Checking if a slave matched his address
+            if (!IRQwaitForRegisterChange(dLock) || !(i2c->SR1 & I2C_SR1_ADDR))
+            {
+                i2c->CR1 |= I2C_CR1_STOP;
+                return false;
+            }
+        }
+
+        // If we want to enter in receiver mode
+        if (slaveConfig.slaveAddress & I2CConsts::I2C_ADDRESS_READ)
+        {
+            // Checking if the peripheral is in Master mode (clearing ADDR
+            // flag with a read on SR2 register)
+            if (!(i2c->SR2 & I2C_SR2_MSL))
+            {
+                i2c->CR1 |= I2C_CR1_STOP;
+                return false;
+            }
+
+            {
+                miosix::FastInterruptDisableLock dLock;
+                // Repeated start
+                i2c->CR1 |= I2C_CR1_START;
+
+                // Waiting for reception of start signal
+                if (!IRQwaitForRegisterChange(dLock) ||
+                    !(i2c->SR1 & I2C_SR1_SB))
+                {
+                    i2c->CR1 |= I2C_CR1_STOP;
+                    return false;
+                }
+            }
+
+            // Sending modified header
+            i2c->DR = header | I2CConsts::I2C_ADDRESS_READ;
+        }
+    }
+
+    // Clearing ADDR flag
+    if (!(i2c->SR2 & I2C_SR2_BUSY) ||  // Channel should be busy
+        !(i2c->SR2 & I2C_SR2_MSL) ||  // The peripheral should be in master mode
+        !((i2c->SR2 & I2C_SR2_TRA) !=
+          (slaveConfig.slaveAddress &
+           I2CConsts::I2C_ADDRESS_READ)))  // Tx or Rx mode
+    {
+        i2c->CR1 |= I2C_CR1_STOP;
+        return false;
+    }
+
+    return true;
+}
+
+void I2CDriver::flushBus()
+{
+    // If there isn't any locked state return immediately
+    if (!lockedState)
+    {
+        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
+    lockedState = false;
+
+    LOG_WARN(logger, fmt::format("I2C{} Bus flushed", id));
+}
+
+inline bool I2CDriver::IRQwaitForRegisterChange(
+    miosix::FastInterruptDisableLock &dLock)
+{
+    waiting = miosix::Thread::IRQgetCurrentThread();
+
+    while (waiting)
+    {
+        waiting->IRQwait();
+        i2c->CR2 |= I2C_CR2_ITEVTEN | I2C_CR2_ITERREN;
+        miosix::FastInterruptEnableLock eLock(dLock);
+        waiting->yield();
+    }
+
+    if (error)
+    {
+        error = false;
+        return false;
+    }
+
+    return true;
+}
+
+inline void I2CDriver::IRQwakeUpWaitingThread()
+{
+    if (waiting)
+    {
+        waiting->IRQwakeup();
+
+        if (waiting->IRQgetPriority() >
+            miosix::Thread::IRQgetCurrentThread()->IRQgetPriority())
+        {
+            miosix::Scheduler::IRQfindNextThread();
+        }
+
+        waiting = 0;
+    }
+}
+
+void I2CDriver::IRQhandleInterrupt()
+{
+    // Disabling the regeneration of the interrupt; if we don't disable the
+    // interrupts we will enter in an infinite loop of interrupts
+    i2c->CR2 &= ~(I2C_CR2_ITEVTEN | I2C_CR2_ITERREN);
+
+    // waking up the waiting thread
+    IRQwakeUpWaitingThread();
+}
+
+void I2CDriver::IRQhandleErrInterrupt()
+{
+    // Disabling the regeneration of the interrupt; if we don't disable
+    // the interrupts we will enter in an infinite loop of interrupts
+    i2c->CR2 &= ~(I2C_CR2_ITEVTEN | I2C_CR2_ITERREN);
+    error = true;
+
+    // Clearing all the errors in the register
+    i2c->SR1 = 0;
+
+    // Waking up the waiting thread
+    IRQwakeUpWaitingThread();
+}
+
+}  // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/drivers/i2c/I2CDriver.h b/src/shared/drivers/i2c/I2CDriver.h
new file mode 100644
index 0000000000000000000000000000000000000000..1d4c1d52d0b8c3f495dccfbb21cc7e0635b3d6f0
--- /dev/null
+++ b/src/shared/drivers/i2c/I2CDriver.h
@@ -0,0 +1,227 @@
+/* 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.
+ */
+
+#include <diagnostic/PrintLogger.h>
+
+#include "miosix.h"
+
+#if defined(I2C4)
+#define N_I2C_PORTS 4
+#elif defined(I2C3)
+#define N_I2C_PORTS 3
+#elif defined(I2C2)
+#define N_I2C_PORTS 2
+#elif defined(I2C1)
+#define N_I2C_PORTS 1
+#else
+#error "Your architecture doesn't support I2C"
+#endif
+
+namespace Boardcore
+{
+
+/**
+ * @brief Low level driver for I2C peripherals.
+ *
+ * This is NOT a thread safe driver. The features supported are:
+ * - Only Master logic;
+ * - Standard/Fast speed modes;
+ * - 7bit and 10bit addressing;
+ * - Exposes basic read or write methods with the option for the write method to
+ * not generate a stop condition;
+ * - There is a method 'flushBus' in order to check and possibly recover from a
+ * locked state on the bus;
+ * - Dynamic setting of clock parameters in order to change speed or addressing
+ * mode before interacting with a device;
+ */
+class I2CDriver
+{
+public:
+    enum Speed : uint8_t
+    {
+        STANDARD = 0,
+        FAST     = 1
+    };
+
+    enum Addressing : uint8_t
+    {
+        BIT7  = 0,
+        BIT10 = 1
+    };
+
+    /**
+     * @brief Configuration struct for a slave device. This will be used for
+     * configuring the bus in order to communicate with the addressed device.
+     */
+    typedef struct
+    {
+        uint16_t
+            slaveAddress;  ///< Slave address without shifts (in BIT7 addressing
+                           ///< |9-bit unused|7-bit address|; in BIT10
+                           ///< addressing |6-bit unused|10-bit address|).
+        I2CDriver::Addressing addressing;  ///< Addressing mode of the device.
+        I2CDriver::Speed speed;            ///< Speed mode of the communication.
+    } I2CSlaveConfig;
+
+    /**
+     * @brief Constructor for the I2C low-level driver.
+     *
+     * It also initializes internally the pins so that they are always set to
+     * ALTERNATE_OD mode with Alternate Function 4 (the usual AF of I2C pins).
+     * Thanks to this we avoid the possibility of short circuits between master
+     * and slaves when they both drive the same bus on two different logical
+     * values.
+     *
+     * @param i2c Structure that represents the I2C peripheral.
+     * @param scl Serial clock GpioPin of the relative I2C peripheral.
+     * @param sda Serial data GpioPin of the relative I2C peripheral.
+     */
+    I2CDriver(I2C_TypeDef *i2c, miosix::GpioPin scl, miosix::GpioPin sda);
+
+    /**
+     * @brief Disables the peripheral, the interrupts in the NVIC and the
+     * peripheral's clock.
+     */
+    ~I2CDriver();
+
+    /**
+     * @brief Non blocking read operation to read nBytes. In case of an error
+     * during the communication, this method returns false with no further
+     * attempts.
+     *
+     * @warning Check always if the operation succeeded or not!
+     * @param slaveConfig The configuration struct of the slave device.
+     * @param buffer Data buffer where to store the data read from the bus.
+     * @param nBytes Number of bytes to read.
+     * @return True if the read is successful, false otherwise.
+     */
+    [[nodiscard]] bool read(I2CSlaveConfig slaveConfig, void *buffer,
+                            size_t nBytes);
+
+    /**
+     * @brief Non blocking write operation to write nBytes. In case of an error
+     * during the communication, this method returns false with no further
+     * attempts.
+     *
+     * @warning Check always if the operation succeeded or not!
+     * @param slaveConfig The configuration struct of the slave device.
+     * @param buffer Data buffer where to read the data to send.
+     * @param nBytes Number of bytes to send.
+     * @param generateStop Flag for the stop condition generation.
+     * @return True if the write is successful, false otherwise.
+     */
+    [[nodiscard]] bool write(I2CSlaveConfig slaveConfig, const void *buffer,
+                             size_t nBytes, bool generateStop = true);
+
+    /**
+     * @brief Performs the recovery from the locked state if necessary.
+     *
+     * It tries to recover from the locked state forcing (changing the mode of
+     * the clock pin) N_SCL_BITBANG clock cycles and reinitializing the
+     * peripheral. It usually takes less than 200us for 16 clocks forced in
+     * standard mode.
+     */
+    void flushBus();
+
+    /**
+     * @brief Handles the interrupt for events of the specific peripheral.
+     *
+     * It just disables the interrupts of the peripheral and wakes the thread
+     * up.
+     * @warning This function should only be called by interrupts. No user code
+     * should call this method.
+     */
+    void IRQhandleInterrupt();
+
+    /**
+     * @brief Handles the interrupt for the errors in the specific peripheral.
+     *
+     * It disables the interrupts of the peripheral, wakes the thread up, sets
+     * the "error" software flag and resets the error flags in the register.
+     * @warning This function should only be called by interrupts. No user code
+     * should call this method.
+     */
+    void IRQhandleErrInterrupt();
+
+private:
+    /**
+     * @brief Enables the peripheral clock and sets up various parameters.
+     */
+    void init();
+
+    /**
+     * @brief Sets up the I2C peripheral registers in order to communicate with
+     * the speed and the addressing mode specified.
+     * @param slaveConfig The configuration struct of the slave device.
+     */
+    void setupPeripheral(I2CSlaveConfig slaveConfig);
+
+    /**
+     * @brief Prologue of any read/write operation in master mode.
+     *
+     * It also detects locked states; in this case sets the lockedState flag to
+     * true.
+     *
+     * @warning Check always if the operation succeeded or not!
+     * @param slaveConfig The configuration struct of the slave device.
+     * @return True if prologue didn't have any error; False otherwise.
+     */
+    [[nodiscard]] bool prologue(I2CSlaveConfig slaveConfig);
+
+    /**
+     * @brief This waits until the thread isn't waken up by an I2C interrupt (EV
+     * or ERR).
+     *
+     * It handles the waiting and yielding and the management of the flags for
+     * the interrupts.
+     * @warning This method should be called in a block where interrupts are
+     * disabled.
+     * @param dLock Reference to the InterruptDisableLock object active in the
+     * scope.
+     * @return True if waken up by an event, false if an error occurred.
+     */
+    inline bool IRQwaitForRegisterChange(
+        miosix::FastInterruptDisableLock &dLock);
+
+    /**
+     * @brief This function has the logic to wake up and reschedule the thread
+     * if it has a higher priority with relation to the one in current
+     * execution.
+     */
+    inline void IRQwakeUpWaitingThread();
+
+    I2C_TypeDef *i2c;
+    uint8_t id;
+    IRQn_Type irqnEv;
+    IRQn_Type irqnErr;
+    miosix::GpioPin scl;  ///< GpioPin of the serial clock pin
+    miosix::GpioPin sda;  ///< GpioPin of the serial data pin
+
+    bool error       = false;  ///< Flag that tells if an error occurred
+    bool lockedState = false;  ///< Flag for locked state detection
+    bool reStarting  = false;  ///< Flag true if not generated a STOP condition
+    miosix::Thread *waiting = 0;  ///< Pointer to the waiting for event thread
+
+    PrintLogger logger = Logging::getLogger("i2c");
+};
+
+}  // namespace Boardcore
\ No newline at end of file
diff --git a/src/tests/drivers/i2c/test-i2c-driver.cpp b/src/tests/drivers/i2c/test-i2c-driver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b821003c7e50ad7c862fa193322343df112051e0
--- /dev/null
+++ b/src/tests/drivers/i2c/test-i2c-driver.cpp
@@ -0,0 +1,129 @@
+/* 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.
+ */
+
+#include <iostream>
+
+#include "drivers/i2c/I2CDriver.h"
+#include "miosix.h"
+#include "string"
+#include "string.h"
+#include "thread"
+
+using namespace std;
+using namespace miosix;
+using namespace Boardcore;
+
+bool generateStop = false;
+
+// I2C1
+typedef miosix::Gpio<GPIOB_BASE, 6> i1scl1;
+typedef miosix::Gpio<GPIOB_BASE, 7> i1sda1;
+typedef miosix::Gpio<GPIOB_BASE, 8> i1scl2;
+typedef miosix::Gpio<GPIOB_BASE, 9> i1sda2;
+
+// I2C2
+typedef miosix::Gpio<GPIOB_BASE, 9> i2sda1;
+typedef miosix::Gpio<GPIOB_BASE, 10> i2scl1;
+typedef miosix::Gpio<GPIOB_BASE, 11> i2sda2;
+typedef miosix::Gpio<GPIOB_BASE, 12> i2scl2;
+#ifdef GPIOH
+typedef miosix::Gpio<GPIOF_BASE, 0> i2sda3;
+typedef miosix::Gpio<GPIOF_BASE, 1> i2scl3;
+typedef miosix::Gpio<GPIOH_BASE, 4> i2sda4;
+typedef miosix::Gpio<GPIOH_BASE, 5> i2scl4;
+#endif
+
+// I2C3
+typedef miosix::Gpio<GPIOC_BASE, 9> i3sda1;
+typedef miosix::Gpio<GPIOA_BASE, 8> i3scl1;
+#ifdef GPIOH
+typedef miosix::Gpio<GPIOH_BASE, 7> i3sda2;
+typedef miosix::Gpio<GPIOH_BASE, 8> i3scl2;
+#endif
+
+/**
+ * SETUP: Connect to the I2C1 port a pullup circuit and than sensors of your
+ * choice (in this test there are the data for some random sensors). The test
+ * just tries to write and read from these sensors. In order to test the
+ * flushBus method try to disconnect and reconnect rapidly the SCL connection of
+ * one sensor (in order to provoke a locked state).
+ */
+
+uint8_t buffer = 0;
+
+typedef struct
+{
+    // cppcheck-suppress unusedStructMember
+    const uint8_t addressSensor;
+    const uint8_t whoamiRegister;
+    const uint8_t whoamiContent;
+    const uint8_t softReset[2];
+} I2CSensor;
+
+I2CSensor BMP180{0b1110111, 0xD0, 0x55, {0xE0, 0xB6}};
+I2CSensor BME280{0b1110110, 0xD0, 0x60, {0xE0, 0xB6}};
+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{
+    OLED.addressSensor, I2CDriver::Addressing::BIT7, I2CDriver::Speed::FAST};
+
+bool i2cDriver(I2CDriver &i2c, I2CSensor sensor,
+               I2CDriver::I2CSlaveConfig sensorConfig)
+{
+    buffer = 0;
+
+    i2c.flushBus();
+
+    // reset the sensor and then read the whoami
+    return i2c.write(sensorConfig, sensor.softReset, 2) &&
+           i2c.write(sensorConfig, &sensor.whoamiRegister, 1, false) &&
+           i2c.read(sensorConfig, &buffer, 1) && buffer == sensor.whoamiContent;
+}
+
+int main()
+{
+    int nRepeat = 50;
+
+    I2CDriver i2c(I2C1, i1scl2::getPin(), i1sda2::getPin());
+
+    for (;;)
+    {
+        // resetting status of read sensors
+        bool statusOLED = true;
+        bool statusBMP  = true;
+
+        for (int i = 0; i < nRepeat; i++)
+        {
+            statusOLED &= i2cDriver(i2c, OLED, OLEDConfig);
+            statusBMP &= i2cDriver(i2c, BMP180, BMP180Config);
+        }
+
+        printf("OLED:%d\tBMP:%d\n", statusOLED, statusBMP);
+    }
+    return 0;
+}
diff --git a/src/tests/drivers/i2c/test-i2c.cpp b/src/tests/drivers/i2c/test-i2c.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ef36a877800a2ba68ff826d07fcc4a98d4f1674e
--- /dev/null
+++ b/src/tests/drivers/i2c/test-i2c.cpp
@@ -0,0 +1,127 @@
+/* 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.
+ */
+
+#include <iostream>
+
+#include "drivers/i2c/I2C.h"
+#include "miosix.h"
+#include "scheduler/TaskScheduler.h"
+#include "string"
+#include "string.h"
+#include "thread"
+
+using namespace std;
+using namespace miosix;
+using namespace Boardcore;
+
+// I2C1
+typedef miosix::Gpio<GPIOB_BASE, 6> i1scl1;
+typedef miosix::Gpio<GPIOB_BASE, 7> i1sda1;
+typedef miosix::Gpio<GPIOB_BASE, 8> i1scl2;
+typedef miosix::Gpio<GPIOB_BASE, 9> i1sda2;
+
+// I2C2
+typedef miosix::Gpio<GPIOB_BASE, 9> i2sda1;
+typedef miosix::Gpio<GPIOB_BASE, 10> i2scl1;
+typedef miosix::Gpio<GPIOB_BASE, 11> i2sda2;
+typedef miosix::Gpio<GPIOB_BASE, 12> i2scl2;
+#ifdef GPIOH
+typedef miosix::Gpio<GPIOF_BASE, 0> i2sda3;
+typedef miosix::Gpio<GPIOF_BASE, 1> i2scl3;
+typedef miosix::Gpio<GPIOH_BASE, 4> i2sda4;
+typedef miosix::Gpio<GPIOH_BASE, 5> i2scl4;
+#endif
+
+// I2C3
+typedef miosix::Gpio<GPIOC_BASE, 9> i3sda1;
+typedef miosix::Gpio<GPIOA_BASE, 8> i3scl1;
+#ifdef GPIOH
+typedef miosix::Gpio<GPIOH_BASE, 7> i3sda2;
+typedef miosix::Gpio<GPIOH_BASE, 8> i3scl2;
+#endif
+
+/**
+ * SETUP: Connect to the I2C1 port a pullup circuit and than sensors of your
+ * choice (in this test there are the data for some random sensors). The test
+ * just tries to write and read from these sensors. In order to test the
+ * flushBus method try to disconnect and reconnect rapidly the SCL connection of
+ * one sensor (in order to provoke a locked state).
+ */
+
+uint8_t buffer = 0;
+
+typedef struct
+{
+    // cppcheck-suppress unusedStructMember
+    const uint8_t addressSensor;
+    const uint8_t whoamiRegister;
+    const uint8_t whoamiContent;
+    const uint8_t softReset[2];
+} I2CSensor;
+
+I2CSensor BMP180{0b1110111, 0xD0, 0x55, {0xE0, 0xB6}};
+I2CSensor BME280{0b1110110, 0xD0, 0x60, {0xE0, 0xB6}};
+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{
+    OLED.addressSensor, I2CDriver::Addressing::BIT7, I2CDriver::Speed::FAST};
+
+bool i2cDriver(I2C &i2c, I2CSensor sensor,
+               I2CDriver::I2CSlaveConfig sensorConfig)
+{
+    buffer = 0;
+
+    // reset the sensor and then read the whoami
+    return i2c.probe(sensorConfig) &&
+           i2c.write(sensorConfig, sensor.softReset, 2) &&
+           i2c.readRegister(sensorConfig, sensor.whoamiRegister, buffer) &&
+           buffer == sensor.whoamiContent;
+}
+
+int main()
+{
+    int nRepeat = 50;
+
+    for (;;)
+    {
+        SyncedI2C i2c(I2C1, i1scl2::getPin(), i1sda2::getPin());
+
+        // resetting status of read sensors
+        bool statusOLED = true;
+        bool statusBMP  = true;
+
+        for (int i = 0; i < nRepeat; i++)
+        {
+            statusOLED &= i2cDriver(i2c, OLED, OLEDConfig);
+            statusBMP &= i2cDriver(i2c, BMP180, BMP180Config);
+        }
+
+        printf("OLED:%d\tBMP:%d\n", statusOLED, statusBMP);
+    }
+    return 0;
+}