diff --git a/src/shared/drivers/i2c/I2C.cpp b/src/shared/drivers/i2c/I2C.cpp
index 299e0ff197ddbb6f1dca291e8a367a6bf296af7c..5242318f47e7bc42ead8e55366bbcfc179815d9f 100644
--- a/src/shared/drivers/i2c/I2C.cpp
+++ b/src/shared/drivers/i2c/I2C.cpp
@@ -30,21 +30,21 @@ I2C::I2C(I2C_TypeDef *i2c, miosix::GpioPin scl, miosix::GpioPin sda)
 {
 }
 
-bool I2C::read(I2CDriver::I2CSlaveConfig slaveConfig, void *buffer,
+bool I2C::read(const 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)
+bool I2C::write(const I2CDriver::I2CSlaveConfig &slaveConfig,
+                const void *buffer, size_t nBytes)
 {
     i2c.flushBus();
     return i2c.write(slaveConfig, buffer, nBytes);
 }
 
-bool I2C::readRegister(I2CDriver::I2CSlaveConfig slaveConfig,
+bool I2C::readRegister(const I2CDriver::I2CSlaveConfig &slaveConfig,
                        const uint8_t registerAddress, uint8_t &registerContent)
 {
     i2c.flushBus();
@@ -52,32 +52,34 @@ bool I2C::readRegister(I2CDriver::I2CSlaveConfig slaveConfig,
            i2c.read(slaveConfig, &registerContent, 1);
 }
 
-bool I2C::probe(I2CDriver::I2CSlaveConfig slaveConfig)
+bool I2C::probe(const I2CDriver::I2CSlaveConfig &slaveConfig)
 {
     i2c.flushBus();
     return i2c.write(slaveConfig, nullptr, 0);
 }
 
+uint16_t I2C::getLastError() { return i2c.getLastError(); }
+
 SyncedI2C::SyncedI2C(I2C_TypeDef *i2c, miosix::GpioPin scl, miosix::GpioPin sda)
     : I2C(i2c, scl, sda)
 {
 }
 
-bool SyncedI2C::read(I2CDriver::I2CSlaveConfig slaveConfig, void *buffer,
+bool SyncedI2C::read(const 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)
+bool SyncedI2C::write(const 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,
+bool SyncedI2C::readRegister(const I2CDriver::I2CSlaveConfig &slaveConfig,
                              const uint8_t registerAddress,
                              uint8_t registerContent)
 {
@@ -85,10 +87,16 @@ bool SyncedI2C::readRegister(I2CDriver::I2CSlaveConfig slaveConfig,
     return I2C::readRegister(slaveConfig, registerAddress, registerContent);
 }
 
-bool SyncedI2C::probe(I2CDriver::I2CSlaveConfig slaveConfig)
+bool SyncedI2C::probe(const I2CDriver::I2CSlaveConfig &slaveConfig)
 {
     miosix::Lock<miosix::FastMutex> lock(mutex);
     return I2C::probe(slaveConfig);
 }
 
+uint16_t SyncedI2C::getLastError()
+{
+    miosix::Lock<miosix::FastMutex> lock(mutex);
+    return i2c.getLastError();
+}
+
 }  // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/drivers/i2c/I2C.h b/src/shared/drivers/i2c/I2C.h
index b161f70cbdb203ed621357e913117daaa992b8c2..071925e8e2d3c4bbbef5600f0d54695535d7b44e 100644
--- a/src/shared/drivers/i2c/I2C.h
+++ b/src/shared/drivers/i2c/I2C.h
@@ -63,8 +63,8 @@ public:
      * @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);
+    [[nodiscard]] bool read(const I2CDriver::I2CSlaveConfig &slaveConfig,
+                            void *buffer, size_t nBytes);
 
     /**
      * @brief Non blocking write operation to write nBytes.
@@ -78,7 +78,7 @@ public:
      * @param nBytes Number of bytes to send.
      * @returns True if the write is successful, false otherwise.
      */
-    [[nodiscard]] bool write(I2CDriver::I2CSlaveConfig slaveConfig,
+    [[nodiscard]] bool write(const I2CDriver::I2CSlaveConfig &slaveConfig,
                              const void *buffer, size_t nBytes);
 
     /**
@@ -93,9 +93,9 @@ public:
      * @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);
+    [[nodiscard]] bool readRegister(
+        const I2CDriver::I2CSlaveConfig &slaveConfig,
+        const uint8_t registerAddress, uint8_t &registerContent);
 
     /**
      * @brief Non blocking operation to check if a slave is available.
@@ -104,7 +104,22 @@ public:
      * @param slaveConfig The configuration struct of the slave device.
      * @returns True if the device is available, false otherwise.
      */
-    [[nodiscard]] bool probe(I2CDriver::I2CSlaveConfig slaveConfig);
+    [[nodiscard]] bool probe(const I2CDriver::I2CSlaveConfig &slaveConfig);
+
+    /**
+     * @brief Returns the last errors happened in the communication.
+     *
+     * For checking if a specific error occurred in the last transaction you can
+     * do `if(getLastError() & Errors::<error-to-check>)`. Do not use `==` to
+     * check for errors because there could be more errors at once. To check if
+     * no errors occurred use `if(getLastError() == Errors::NO_ERROR)` or simply
+     * `if(!getLastError())`
+     *
+     * @return A bit sequence where the bits set correspond to the last errors
+     * occurred in the peripheral (see the `I2CDriver::Errors` enum to get the
+     * correspondence between bit position and error reported).
+     */
+    uint16_t getLastError();
 
 protected:
     I2CDriver i2c;  ///< Instance of I2C low-level driver
@@ -137,8 +152,8 @@ public:
      * @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);
+    [[nodiscard]] bool read(const I2CDriver::I2CSlaveConfig &slaveConfig,
+                            void *buffer, size_t nBytes);
 
     /**
      * @brief Write operation to write nBytes.
@@ -152,7 +167,7 @@ public:
      * @param nBytes Number of bytes to send.
      * @returns True if the write is successful, false otherwise.
      */
-    [[nodiscard]] bool write(I2CDriver::I2CSlaveConfig slaveConfig,
+    [[nodiscard]] bool write(const I2CDriver::I2CSlaveConfig &slaveConfig,
                              const void *buffer, size_t nBytes);
 
     /**
@@ -167,9 +182,9 @@ public:
      * @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);
+    [[nodiscard]] bool readRegister(
+        const I2CDriver::I2CSlaveConfig &slaveConfig,
+        const uint8_t registerAddress, uint8_t registerContent);
 
     /**
      * @brief Check if a slave is available.
@@ -180,7 +195,22 @@ public:
      * @param slaveConfig The configuration struct of the slave device.
      * @returns true if the device is available, false otherwise.
      */
-    [[nodiscard]] bool probe(I2CDriver::I2CSlaveConfig slaveConfig);
+    [[nodiscard]] bool probe(const I2CDriver::I2CSlaveConfig &slaveConfig);
+
+    /**
+     * @brief Returns the last errors happened in the communication.
+     *
+     * For checking if a specific error occurred in the last transaction you can
+     * do `if(getLastError() & Errors::<error-to-check>)`. Do not use `==` to
+     * check for errors because there could be more errors at once. To check if
+     * no errors occurred use `if(getLastError() == Errors::NO_ERROR)` or simply
+     * `if(!getLastError())`
+     *
+     * @return A bit sequence where the bits set correspond to the last errors
+     * occurred in the peripheral (see the `I2CDriver::Errors` enum to get the
+     * correspondence between bit position and error reported).
+     */
+    uint16_t getLastError();
 
 private:
     miosix::FastMutex mutex;  ///< Mutex for rx/tx
diff --git a/src/shared/drivers/i2c/I2CDriver.cpp b/src/shared/drivers/i2c/I2CDriver.cpp
index c03f30d7cc2b710c17e9a479cf410c8fedcfb36b..99a87992034d6a341598bd0c75d0aa73337ff218 100644
--- a/src/shared/drivers/i2c/I2CDriver.cpp
+++ b/src/shared/drivers/i2c/I2CDriver.cpp
@@ -36,8 +36,6 @@ 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
@@ -231,7 +229,7 @@ namespace Boardcore
 {
 
 I2CDriver::I2CDriver(I2C_TypeDef *i2c, miosix::GpioPin scl, miosix::GpioPin sda)
-    : i2c(i2c), scl(scl), sda(sda)
+    : i2c(i2c), scl(scl), sda(sda), transaction()
 {
     // Setting the id and irqn of the right i2c peripheral
     switch (reinterpret_cast<uint32_t>(i2c))
@@ -338,10 +336,10 @@ void I2CDriver::init()
     // 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
+               I2C_CR2_ITBUFEN;  // enabling interrupts for rx/tx byte
 }
 
-void I2CDriver::setupPeripheral(I2CSlaveConfig slaveConfig)
+void I2CDriver::setupPeripheral(const I2CSlaveConfig &slaveConfig)
 {
     // Frequency < 2MHz in standard mode or < 4MHz in fast mode not allowed by
     // the peripheral
@@ -383,117 +381,57 @@ void I2CDriver::setupPeripheral(I2CSlaveConfig slaveConfig)
     i2c->CR1 |= I2C_CR1_PE;
 }
 
-bool I2CDriver::read(I2CSlaveConfig slaveConfig, void *buffer, size_t nBytes)
+bool I2CDriver::read(const 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
+    // 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;
+
+    // Disabling the generation of the ACK if reading only 1 byte, otherwise
+    // enable it
+    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)
-    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;
+    return doOperation(slaveConfig);
 };
 
-bool I2CDriver::write(I2CSlaveConfig slaveConfig, const void *buffer,
+bool I2CDriver::write(const 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;
+    // 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;
+
+    // Sending doOperation when the channel isn't busy
+    return doOperation(slaveConfig);
 };
 
-bool I2CDriver::prologue(I2CSlaveConfig 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 (lockedState)
+    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
@@ -504,7 +442,7 @@ bool I2CDriver::prologue(I2CSlaveConfig slaveConfig)
         // Locked state detected after N polling cycles
         if (i == I2CConsts::MAX_N_POLLING)
         {
-            lockedState = true;
+            lastError = Errors::BUS_LOCKED;
             LOG_ERR(logger, fmt::format("I2C{} bus locked state detected", id));
             return false;
         }
@@ -516,8 +454,11 @@ bool I2CDriver::prologue(I2CSlaveConfig slaveConfig)
 
     reStarting = false;
 
+    // Starting the transaction with the START bit
+    // From the wait till the end of transaction it will all be executed in the
+    // event ISR
     {
-        miosix::FastInterruptDisableLock dLock;
+        miosix::PauseKernelLock dLock;
         uint32_t i{0};
 
         // Sending the start condition
@@ -538,111 +479,34 @@ bool I2CDriver::prologue(I2CSlaveConfig slaveConfig)
         // START condition not sent after N polling cycles
         if (i == I2CConsts::MAX_N_POLLING)
         {
-            miosix::FastInterruptEnableLock eLock(dLock);
+            lastError |= Errors::SB_NOT_SENT;
             LOG_ERR(logger,
-                    fmt::format("I2C{} bus didn't sent the start bit", id));
+                    fmt::format("I2C{} bus didn't send the start bit", id));
             return false;
         }
     }
 
-    // Sending (header + ) slave address
-    if (slaveConfig.addressing == Addressing::BIT7)
+    transaction.nBytesDone = 0;
+
+    // Sending slave address
     {
         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);
+        i2c->DR = ((slaveConfig.slaveAddress << 1) | transaction.operation);
 
-            // 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;
+        // 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);
     }
-
-    return true;
 }
 
 void I2CDriver::flushBus()
 {
     // If there isn't any locked state return immediately
-    if (!lockedState)
+    if (!(lastError & Errors::BUS_LOCKED))
     {
         return;
     }
@@ -680,16 +544,21 @@ void I2CDriver::flushBus()
 
     // 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;
+    lastError = Errors::NO_ERROR;
 
     LOG_WARN(logger, fmt::format("I2C{} Bus flushed", id));
 }
 
-inline bool I2CDriver::IRQwaitForRegisterChange(
+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();
 
+    // flag thread as waiting, enable interrupts in I2C peripheral and yield
+    // till an interrupt doesn't wake up the thread
     while (waiting)
     {
         waiting->IRQwait();
@@ -698,17 +567,36 @@ inline bool I2CDriver::IRQwaitForRegisterChange(
         waiting->yield();
     }
 
+    // If error occurred, parse it to notify the error(s); otherwise reset
+    // lastError parameter
     if (error)
     {
-        error = false;
+        lastError |= ((error & I2C_SR1_OVR) ? Errors::OVR : 0) |
+                     ((error & I2C_SR1_BERR) ? Errors::BERR : 0) |
+                     ((error & I2C_SR1_ARLO) ? Errors::ARLO : 0) |
+                     ((error & I2C_SR1_AF) ? Errors::AF : 0);
+        error = 0;
         return false;
     }
+    else
+    {
+        lastError = Errors::NO_ERROR;
+    }
 
     return true;
 }
 
 inline void I2CDriver::IRQwakeUpWaitingThread()
 {
+    // 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);
+
+    // invalidating the buffer pointers (avoiding to keep a pointer to an old
+    // memory location)
+    transaction.buffRead  = nullptr;
+    transaction.buffWrite = nullptr;
+
     if (waiting)
     {
         waiting->IRQwakeup();
@@ -719,30 +607,98 @@ inline void I2CDriver::IRQwakeUpWaitingThread()
             miosix::Scheduler::IRQfindNextThread();
         }
 
-        waiting = 0;
+        waiting = nullptr;
     }
 }
 
 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);
+    // Address sending
+    if ((i2c->SR1 & I2C_SR1_ADDR))
+    {
+        // Clearing ADDR flag
+        if (!(i2c->SR2 & I2C_SR2_BUSY) ||  // Channel should be busy
+            !(i2c->SR2 & I2C_SR2_MSL) ||   // Should be in Master mode
+            !((i2c->SR2 & I2C_SR2_TRA) != transaction.operation))
+        {
+            // "reserved" bit in the SR1 register, so we don't collide with
+            // other fields
+            error = 1 << 13;
+            lastError |= ADDR_ERROR;
+            IRQwakeUpWaitingThread();
+            return;
+        }
+    }
 
-    // waking up the waiting thread
-    IRQwakeUpWaitingThread();
+    // Performing the read/write
+    if (transaction.operation == Operation::READ)
+    {
+        // READ
+        if (i2c->SR1 & (I2C_SR1_BTF | I2C_SR1_RXNE))
+        {
+            // Clearing the ACK flag in order to send a NACK on the last byte
+            // that will be read
+            if (transaction.nBytesDone >= transaction.nBytes - 2)
+            {
+                i2c->CR1 &= ~I2C_CR1_ACK;
+            }
+
+            if (transaction.nBytesDone < transaction.nBytes)
+            {
+                transaction.buffRead[transaction.nBytesDone] = i2c->DR;
+                transaction.nBytesDone++;
+            }
+        }
+    }
+    else
+    {
+        // WRITE
+        if (i2c->SR1 & (I2C_SR1_BTF | I2C_SR1_TXE))
+        {
+            if (transaction.nBytesDone < transaction.nBytes)
+            {
+                i2c->DR = transaction.buffWrite[transaction.nBytesDone];
+                transaction.nBytesDone++;
+                return;
+            }
+        }
+    }
+
+    // Sending STOP condition and wake up thread
+    if (transaction.nBytesDone >= transaction.nBytes)
+    {
+        // If we are on the last byte, generate the stop condition if we have to
+        // end the communication
+        if (transaction.generateStop)
+        {
+            i2c->CR1 |= I2C_CR1_STOP;
+            reStarting = false;
+        }
+        else
+        {
+            reStarting = true;
+        }
+
+        // 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;
+    error = i2c->SR1;
 
     // Clearing all the errors in the register
     i2c->SR1 = 0;
 
+    // In case of arbitration lost, the hardware releases automatically the
+    // lines. Do not send STOP condition. In the other cases, the software must
+    // issue the STOP condition.
+    if (!(error & I2C_SR1_ARLO))
+    {
+        i2c->CR1 |= I2C_CR1_STOP;
+    }
+
     // Waking up the waiting thread
     IRQwakeUpWaitingThread();
 }
diff --git a/src/shared/drivers/i2c/I2CDriver.h b/src/shared/drivers/i2c/I2CDriver.h
index 1d4c1d52d0b8c3f495dccfbb21cc7e0635b3d6f0..d1301e3369b2fc0b532b48a5c44e29445eb98236 100644
--- a/src/shared/drivers/i2c/I2CDriver.h
+++ b/src/shared/drivers/i2c/I2CDriver.h
@@ -45,9 +45,9 @@ namespace Boardcore
  * This is NOT a thread safe driver. The features supported are:
  * - Only Master logic;
  * - Standard/Fast speed modes;
- * - 7bit and 10bit addressing;
+ * - 7bit addressing;
  * - Exposes basic read or write methods with the option for the write method to
- * not generate a stop condition;
+ * 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
@@ -56,6 +56,12 @@ namespace Boardcore
 class I2CDriver
 {
 public:
+    enum Operation : uint8_t
+    {
+        WRITE = 0,
+        READ  = 1
+    };
+
     enum Speed : uint8_t
     {
         STANDARD = 0,
@@ -68,6 +74,22 @@ public:
         BIT10 = 1
     };
 
+    /**
+     * @brief Error enums with a value that makes it possible to or them and
+     * report more than one error at once
+     */
+    enum Errors : uint16_t
+    {
+        NO_ERROR    = 0,       ///< The bus didn't have any error
+        BUS_LOCKED  = 1 << 0,  ///< Detected a locked state on the bus
+        BERR        = 1 << 1,  ///< External Start or stop condition detected
+        ARLO        = 1 << 2,  ///< Arbitration lost
+        AF          = 1 << 3,  ///< Acknowledge failure
+        OVR         = 1 << 4,  ///< Overrun/underrun error
+        SB_NOT_SENT = 1 << 5,  ///< Start bit not sent
+        ADDR_ERROR  = 1 << 6   ///< Address sent but peripheral in wrong state
+    };
+
     /**
      * @brief Configuration struct for a slave device. This will be used for
      * configuring the bus in order to communicate with the addressed device.
@@ -85,7 +107,9 @@ public:
     /**
      * @brief Constructor for the I2C low-level driver.
      *
-     * It also initializes internally the pins so that they are always set to
+     * It initializes the peripheral clock, the pins, calls the `init()` method
+     * and enables the IRQs in the NVIC.
+     * Pins are internally initialized 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
@@ -104,9 +128,8 @@ public:
     ~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.
+     * @brief 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.
@@ -114,13 +137,12 @@ public:
      * @param nBytes Number of bytes to read.
      * @return True if the read is successful, false otherwise.
      */
-    [[nodiscard]] bool read(I2CSlaveConfig slaveConfig, void *buffer,
+    [[nodiscard]] bool read(const 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.
+     * @brief 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.
@@ -129,24 +151,42 @@ public:
      * @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);
+    [[nodiscard]] bool write(const 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
+     * the clock pin) N_SCL_BITBANG clock cycles and re-initializing the
      * peripheral. It usually takes less than 200us for 16 clocks forced in
      * standard mode.
      */
     void flushBus();
 
+    /**
+     * @brief Returns the last errors happened in the communication.
+     *
+     * For checking if a specific error occurred in the last transaction you can
+     * do `if(getLastError() & Errors::<error-to-check>)`. Do not use `==` to
+     * check for errors because there could be more errors at once. To check if
+     * no errors occurred use `if(getLastError() == Errors::NO_ERROR)` or simply
+     * `if(!getLastError())`
+     *
+     * @return A bit sequence where the bits set correspond to the last errors
+     * occurred in the peripheral (see the `I2CDriver::Errors` enum to get the
+     * correspondence between bit position and error reported).
+     */
+    uint16_t getLastError();
+
     /**
      * @brief Handles the interrupt for events of the specific peripheral.
      *
-     * It just disables the interrupts of the peripheral and wakes the thread
-     * up.
+     * Wakes up the thread only if the operation is completed or an error is
+     * detected, otherwise all the phases of the read or write are handled in
+     * this ISR thanks to the changing of the peripheral flags.
+     *
      * @warning This function should only be called by interrupts. No user code
      * should call this method.
      */
@@ -157,6 +197,7 @@ public:
      *
      * 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.
      */
@@ -164,7 +205,24 @@ public:
 
 private:
     /**
-     * @brief Enables the peripheral clock and sets up various parameters.
+     * @brief Structure that stores all the info of the transaction, such as
+     * operation, buffers, number of bytes of the buffer, number of bytes
+     * already processed (read or written) and whether to generate the stop bit
+     * or not.
+     */
+    typedef struct
+    {
+        Operation operation;       ///< Operation to be performed (R/W)
+        uint8_t *buffRead;         ///< Buffer with the data to read
+        const uint8_t *buffWrite;  ///< Buffer with the data to write
+        size_t nBytes;             ///< Number of bytes of the buffer
+        size_t nBytesDone;         ///< Number of bytes already processed
+        bool generateStop;         ///< Whether to generate stop condition
+    } I2CTransaction;
+
+    /**
+     * @brief Resets the peripheral and sets up the internal clock parameter
+     * parameters.
      */
     void init();
 
@@ -173,19 +231,20 @@ private:
      * the speed and the addressing mode specified.
      * @param slaveConfig The configuration struct of the slave device.
      */
-    void setupPeripheral(I2CSlaveConfig slaveConfig);
+    void setupPeripheral(const I2CSlaveConfig &slaveConfig);
 
     /**
-     * @brief Prologue of any read/write operation in master mode.
+     * @brief Method to perform a read or write operation.
      *
-     * It also detects locked states; in this case sets the lockedState flag to
-     * true.
+     * This method waits for the bus to be clear, sets up the peripheral for the
+     * new communication, sends the START condition and the address. After this,
+     * it delegates the logic to the event ISR.
      *
      * @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.
+     * @return True if the operation succeeded, False otherwise.
      */
-    [[nodiscard]] bool prologue(I2CSlaveConfig slaveConfig);
+    [[nodiscard]] bool doOperation(const I2CSlaveConfig &slaveConfig);
 
     /**
      * @brief This waits until the thread isn't waken up by an I2C interrupt (EV
@@ -193,13 +252,14 @@ private:
      *
      * 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(
+    inline bool IRQwaitForOperationCompletion(
         miosix::FastInterruptDisableLock &dLock);
 
     /**
@@ -216,10 +276,11 @@ private:
     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
+    uint16_t lastError = NO_ERROR;  ///< Flag for the last error occurred
+    uint16_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
 
     PrintLogger logger = Logging::getLogger("i2c");
 };
diff --git a/src/tests/drivers/i2c/test-i2c-driver.cpp b/src/tests/drivers/i2c/test-i2c-driver.cpp
index b821003c7e50ad7c862fa193322343df112051e0..80f3e52953f4dd850b535c197d7b9b848d7fa898 100644
--- a/src/tests/drivers/i2c/test-i2c-driver.cpp
+++ b/src/tests/drivers/i2c/test-i2c-driver.cpp
@@ -68,8 +68,6 @@ typedef miosix::Gpio<GPIOH_BASE, 8> i3scl2;
  * one sensor (in order to provoke a locked state).
  */
 
-uint8_t buffer = 0;
-
 typedef struct
 {
     // cppcheck-suppress unusedStructMember
@@ -95,24 +93,45 @@ I2CDriver::I2CSlaveConfig OLEDConfig{
 bool i2cDriver(I2CDriver &i2c, I2CSensor sensor,
                I2CDriver::I2CSlaveConfig sensorConfig)
 {
-    buffer = 0;
+    uint8_t buffer[10]  = {0};
+    const uint8_t nRead = 10;
 
     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;
+    if (!(i2c.write(sensorConfig, sensor.softReset, 2) &&
+          i2c.write(sensorConfig, &sensor.whoamiRegister, 1, false) &&
+          i2c.read(sensorConfig, buffer, nRead) &&
+          (buffer[0] == sensor.whoamiContent)))
+    {
+        uint16_t lastError{i2c.getLastError()};
+        if (!(lastError &
+              (I2CDriver::Errors::AF | I2CDriver::Errors::BUS_LOCKED)))
+        {
+            printf("LastError: %d\n", lastError);
+        }
+        return false;
+    }
+
+    return true;
 }
 
 int main()
 {
     int nRepeat = 50;
 
-    I2CDriver i2c(I2C1, i1scl2::getPin(), i1sda2::getPin());
+    // thread that uses 100% CPU
+    std::thread t(
+        []()
+        {
+            while (1)
+                ;
+        });
 
     for (;;)
     {
+        I2CDriver i2c(I2C1, i1scl2::getPin(), i1sda2::getPin());
+
         // resetting status of read sensors
         bool statusOLED = true;
         bool statusBMP  = true;
diff --git a/src/tests/drivers/i2c/test-i2c.cpp b/src/tests/drivers/i2c/test-i2c.cpp
index ef36a877800a2ba68ff826d07fcc4a98d4f1674e..ee9ba2587b08b3af1100faaf129211169b398ab4 100644
--- a/src/tests/drivers/i2c/test-i2c.cpp
+++ b/src/tests/drivers/i2c/test-i2c.cpp
@@ -97,16 +97,34 @@ bool i2cDriver(I2C &i2c, I2CSensor sensor,
     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;
+    if (!(i2c.probe(sensorConfig) &&
+          i2c.write(sensorConfig, sensor.softReset, 2) &&
+          i2c.readRegister(sensorConfig, sensor.whoamiRegister, buffer) &&
+          buffer == sensor.whoamiContent))
+    {
+        uint16_t lastError{i2c.getLastError()};
+        if (!(lastError & I2CDriver::Errors::AF))
+        {
+            printf("LastError: %d\n", lastError);
+        }
+        return false;
+    }
+
+    return true;
 }
 
 int main()
 {
     int nRepeat = 50;
 
+    // thread that uses 100% CPU
+    std::thread t(
+        []()
+        {
+            while (1)
+                ;
+        });
+
     for (;;)
     {
         SyncedI2C i2c(I2C1, i1scl2::getPin(), i1sda2::getPin());