diff --git a/os/hal/ports/RP/LLD/I2Cv1/hal_i2c_lld.c b/os/hal/ports/RP/LLD/I2Cv1/hal_i2c_lld.c
index 38eca2ed0b..fbd6cfad91 100644
--- a/os/hal/ports/RP/LLD/I2Cv1/hal_i2c_lld.c
+++ b/os/hal/ports/RP/LLD/I2Cv1/hal_i2c_lld.c
@@ -61,8 +61,10 @@ I2CDriver I2CD2;
 /* Driver local functions.                                                   */
 /*===========================================================================*/
 
-
-sysinterval_t i2c_wait_time(I2CDriver *i2cp){
+/**
+ * @brief    Calculates the waiting time which is 10 times the SCL period duration.
+ */
+static sysinterval_t i2c_lld_get_wait_time(I2CDriver *i2cp){
   uint32_t baudrate = i2cp->config->baudrate;
   if (baudrate <= 100000U) {
     return OSAL_US2I(100);
@@ -73,13 +75,16 @@ sysinterval_t i2c_wait_time(I2CDriver *i2cp){
   }
 }
 
- msg_t i2c_disable(I2CDriver *i2cp) {
+/**
+ * @brief    Tries to disable the I2C peripheral.
+ */
+static msg_t i2c_lld_disable(I2CDriver *i2cp) {
   I2C_TypeDef *dp = i2cp->i2c;
   systime_t start, end;
 
   /* Calculating the time window for the timeout on the enable condition.*/
   start = osalOsGetSystemTimeX();
-  end = osalTimeAddX(start, i2c_wait_time(i2cp));
+  end = osalTimeAddX(start, i2c_lld_get_wait_time(i2cp));
 
   dp->ENABLE &= ~I2C_IC_ENABLE_ENABLE;
   while ((dp->ENABLESTATUS & I2C_IC_ENABLE_STATUS_IC_EN) != 0U) {
@@ -91,17 +96,22 @@ sysinterval_t i2c_wait_time(I2CDriver *i2cp){
   return MSG_OK;
 }
 
-msg_t i2c_abort(I2CDriver *i2cp) {
+/**
+ * @brief    Aborts any ongoing transmission of the I2C peripheral.
+ */
+static msg_t i2c_lld_abort_transmission(I2CDriver *i2cp) {
   I2C_TypeDef *dp = i2cp->i2c;
   systime_t start, end;
 
   /* Calculating the time window for the timeout on the abort condition.*/
   start = osalOsGetSystemTimeX();
-  end = osalTimeAddX(start, i2c_wait_time(i2cp));
+  end = osalTimeAddX(start, i2c_lld_get_wait_time(i2cp));
 
-  /* Issue abort. */
+  /* Disable all interrupts as a pre-caution. */
   dp->INTRMASK = 0U;
+  /* Enable peripheral to issue abort. */
   dp->ENABLE |= I2C_IC_ENABLE_ENABLE;
+  /* Issue abort. */
   dp->ENABLE |= I2C_IC_ENABLE_ABORT;
 
   /* Wait for transmission the be aborted */
@@ -115,23 +125,37 @@ msg_t i2c_abort(I2CDriver *i2cp) {
   return MSG_OK;
 }
 
-void i2c_handle_error(I2CDriver *i2cp) {
+/**
+ * @brief    Handles transmission errors by waking the sleeping thread
+ *           and setting the error reasons.
+ */
+void i2c_lld_handle_errors(I2CDriver *i2cp) {
     I2C_TypeDef *dp = i2cp->i2c;
 
-    i2cp->errors = dp->TXABRTSOURCE;
-    (void)dp->CLRTXABRT;
+    if (dp->TXABRTSOURCE & I2C_IC_TX_ABRT_SOURCE_ARB_LOST) {
+      i2cp->errors |= I2C_ARBITRATION_LOST;
+    }
+
+    const uint32_t overrun_mask =
+      (I2C_IC_INTR_STAT_M_RX_OVER |
+      I2C_IC_INTR_STAT_M_RX_UNDER |
+      I2C_IC_INTR_STAT_M_TX_OVER);
+
+    if (dp->RAWINTRSTAT & overrun_mask) {
+      i2cp->errors |= I2C_OVERRUN;
+    }
 
     /* Disable all interrupts. */
     dp->INTRMASK = 0U;
     (void)dp->CLRINTR;
-
+    (void)dp->CLRTXABRT;
     _i2c_wakeup_error_isr(i2cp);
 }
 
 /**
  * @brief    Calculates and set up clock settings.
  */
-static void i2c_setup_frequency(I2CDriver *i2cp) {
+static void i2c_lld_setup_frequency(I2CDriver *i2cp) {
   I2C_TypeDef *dp = i2cp->i2c;
   /*                [us]    SS   FS  FS+
    *  MIN_SCL_LOWtime_FS:  4.7  1.3  0.5
@@ -175,107 +199,130 @@ static void i2c_setup_frequency(I2CDriver *i2cp) {
 }
 
 /**
- * @brief   Interrupt processing.
+ * @brief    Requests data to be received, actual reception is done in the interrupt handler.
  */
-static void serve_interrupt(I2CDriver *i2cp) {
+static void i2c_lld_request_data(I2CDriver *i2cp) {
   I2C_TypeDef *dp = i2cp->i2c;
-  uint32_t data;
+  uint32_t data = I2C_IC_DATA_CMD_CMD;
+  uint32_t bytes_left = i2cp->rxbytes;
+  /* RP2040 Designware I2C peripheral has FIFO depth of 16 elements. */
+  uint32_t batch = MIN(16U - dp->TXFLR, bytes_left - i2cp->rxbytes_pending);
 
-  /* Transmission error detected. */
-  if ((dp->INTRSTAT & I2C_ERROR_INTERRUPTS) != 0U) {
-    return i2c_handle_error(i2cp);
+  if (i2cp->send_restart) {
+    data |= I2C_IC_DATA_CMD_RESTART;
   }
 
-  /* Transmission phase. */
-  if (dp->INTRSTAT & I2C_IC_INTR_STAT_R_TX_EMPTY && i2cp->state == I2C_ACTIVE_TX) {
-      while (i2cp->txbytes > 0U && dp->STATUS & I2C_IC_STATUS_TFNF) {
-        data = i2cp->txbuf[i2cp->txindex];
+  /* Setup RX FIFO trigger level to only trigger when the batch has been
+   * completely received. */
+  dp->RXTL = batch > 1U ? batch - 1U : 0U;
 
-        /* Send STOP after last byte */
-        if (i2cp->txbytes == 1U) {
-          data |= I2C_IC_DATA_CMD_STOP;
-        }
-        dp->DATACMD = data;
+  while (batch > 0U) {
+    /* Send STOP after last byte. */
+    if (bytes_left == 1U) {
+      data |= I2C_IC_DATA_CMD_STOP;
+    }
+    dp->DATACMD = data;
 
-        i2cp->txindex++;
-        i2cp->txbytes--;
+    batch--;
+    bytes_left--;
+    i2cp->rxbytes_pending++;
+    data = I2C_IC_DATA_CMD_CMD;
+  }
 
-        /* Error during transmission  */
-        if(dp->INTRSTAT & I2C_ERROR_INTERRUPTS) {
-          return i2c_handle_error(i2cp);
-        }
-      }
+  /* Clear TX FIFO empty interrupt, it will be re-activated when data has been
+   * received. */
+  dp->CLR.INTRMASK = I2C_IC_INTR_MASK_M_TX_EMPTY;
+  /* Enable RX FULL interrupt to process received data. */
+  dp->SET.INTRMASK = I2C_IC_INTR_STAT_R_RX_FULL;
+}
 
-    if (i2cp->txbytes == 0U) {
-      /* Everything is send, therefore disable TX FIFO empty IRQ. */
-      dp->CLR.INTRMASK = I2C_IC_INTR_MASK_M_TX_EMPTY;
+/**
+ * @brief    Fills TX FIFO with data to be send.
+ */
+static void i2c_lld_transmit_data(I2CDriver *i2cp) {
+  I2C_TypeDef *dp = i2cp->i2c;
+  uint32_t data;
+  while (i2cp->txbytes > 0U && dp->STATUS & I2C_IC_STATUS_TFNF) {
+    data = i2cp->txbuf[i2cp->txindex];
+
+    /* Send STOP after last byte */
+    if (i2cp->txbytes == 1U) {
+      data |= I2C_IC_DATA_CMD_STOP;
     }
+    dp->DATACMD = data;
+
+    i2cp->txindex++;
+    i2cp->txbytes--;
+  }
 
+  /* Nothing more to send, disable TX FIFO empty interrupt. */
+  if (i2cp->txbytes == 0U) {
+    dp->CLR.INTRMASK = I2C_IC_INTR_MASK_M_TX_EMPTY;
+  }
+}
+
+/**
+ * @brief   I2C shared ISR code.
+ *
+ * @param[in] i2cp      pointer to the @p I2CDriver object
+ */
+static void i2c_lld_serve_interrupt(I2CDriver *i2cp) {
+  I2C_TypeDef *dp = i2cp->i2c;
+  uint32_t intr = dp->INTRSTAT;
+
+  /* Transmission error detected. */
+  if (intr & I2C_ERROR_INTERRUPTS) {
+    return i2c_lld_handle_errors(i2cp);
+  }
+
+  /* If the TX FIFO is empty we can request or send more data. */
+  if (intr & I2C_IC_INTR_STAT_R_TX_EMPTY) {
+    if (i2cp->state == I2C_ACTIVE_TX && i2cp->txbytes > 0U) {
+      i2c_lld_transmit_data(i2cp);
+    } else if (i2cp->state == I2C_ACTIVE_RX && i2cp->rxbytes > 0U) {
+      i2c_lld_request_data(i2cp);
+    }
     return;
   }
 
-  if (dp->INTRSTAT & I2C_IC_INTR_STAT_R_RX_FULL && i2cp->state == I2C_ACTIVE_RX) {
-    while(i2cp->rxbytes > 0U && dp->STATUS & I2C_IC_STATUS_RFNE) {
+  if (i2cp->state == I2C_ACTIVE_RX && intr & I2C_IC_INTR_STAT_R_RX_FULL) {
+    while (i2cp->rxbytes > 0U && dp->STATUS & I2C_IC_STATUS_RFNE) {
       /* Read out received data. */
       i2cp->rxbuf[i2cp->rxindex] = (uint8_t)dp->DATACMD;
       i2cp->rxindex++;
       i2cp->rxbytes--;
-
-      if (i2cp->rxbytes > 0U) {
-        /* Set to master read */
-        data = I2C_IC_DATA_CMD_CMD;
-
-        /* Send STOP after last byte. */
-        if (i2cp->rxbytes == 1U) {
-          data |= I2C_IC_DATA_CMD_STOP;
-        }
-
-        dp->DATACMD = data;
-      }
-
-      /* Error during transmission  */
-      if(dp->INTRSTAT & I2C_ERROR_INTERRUPTS) {
-        return i2c_handle_error(i2cp);
-      }
+      i2cp->rxbytes_pending--;
     }
 
     if (i2cp->rxbytes == 0U) {
-      /* Everything is received, therefore disable RX FIFO full IRQ. */
-      dp->CLR.INTRMASK = I2C_IC_INTR_MASK_M_RX_FULL;
+      /* Everything is received, therefore disable all FIFO IRQs. */
+      dp->CLR.INTRMASK = I2C_IC_INTR_MASK_M_RX_FULL | I2C_IC_INTR_STAT_R_TX_EMPTY;
+    } else {
+      /* In case we didn't receive the full batch, reset RX fifo trigger
+       * to in flight bytes. */
+      dp->RXTL = i2cp->rxbytes_pending > 1U ? i2cp->rxbytes_pending - 1U : 0U;
+      /* Enable TX FIFO empty IRQ to request more data. */
+      dp->SET.INTRMASK = I2C_IC_INTR_STAT_R_TX_EMPTY;
     }
-
     return;
   }
 
-  if (dp->INTRSTAT & I2C_IC_INTR_STAT_R_STOP_DET) {
+  if (intr & I2C_IC_INTR_STAT_R_STOP_DET) {
     /* Clear irq flag. */
     (void)dp->CLRSTOPDET;
-    if (i2cp->state == I2C_ACTIVE_TX) {
-      /* TX end. */
-      if (i2cp->rxbytes > 0U) {
-        /* Setup RX. */
-        /* Set interrupt mask. */
-        dp->INTRMASK = I2C_IC_INTR_MASK_M_STOP_DET |
-                       I2C_IC_INTR_MASK_M_RX_FULL |
-                       I2C_ERROR_INTERRUPTS;
-
-        data = I2C_IC_DATA_CMD_CMD | I2C_IC_CON_IC_RESTART_EN;
-
-        /* Send STOP after last byte. */
-        if (i2cp->rxbytes == 1U) {
-          data |= I2C_IC_DATA_CMD_STOP;
-        }
-
-        /* Set read flag. */
-        dp->DATACMD = data;
-
-        /* State change to RX. */
-        i2cp->state = I2C_ACTIVE_RX;
-        return;
-      }
+    if (i2cp->state == I2C_ACTIVE_TX && i2cp->rxbytes > 0U) {
+      /* State change to RX. */
+      i2cp->state = I2C_ACTIVE_RX;
+      /* Restart communication. */
+      i2cp->send_restart = true;
+
+      /* Enable TX FIFO empty IRQ to request more data to be received. */
+      dp->SET.INTRMASK = I2C_IC_INTR_STAT_R_TX_EMPTY;
+      return;
     }
   }
 
+  /* Transmission complete, disable and clear all interrupts. */
   dp->INTRMASK = 0U;
   (void)dp->CLRINTR;
   _i2c_wakeup_isr(i2cp);
@@ -290,7 +337,7 @@ static void serve_interrupt(I2CDriver *i2cp) {
 OSAL_IRQ_HANDLER(RP_I2C0_IRQ_HANDLER) {
   OSAL_IRQ_PROLOGUE();
 
-  serve_interrupt(&I2CD1);
+  i2c_lld_serve_interrupt(&I2CD1);
 
   OSAL_IRQ_EPILOGUE();
 }
@@ -302,7 +349,7 @@ OSAL_IRQ_HANDLER(RP_I2C0_IRQ_HANDLER) {
 OSAL_IRQ_HANDLER(RP_I2C1_IRQ_HANDLER) {
   OSAL_IRQ_PROLOGUE();
 
-  serve_interrupt(&I2CD2);
+  i2c_lld_serve_interrupt(&I2CD2);
 
   OSAL_IRQ_EPILOGUE();
 }
@@ -369,7 +416,7 @@ void i2c_lld_start(I2CDriver *i2cp) {
   }
 
   /* Disable i2c peripheral for setup phase. */
-  if (i2c_disable(i2cp) != MSG_OK) {
+  if (i2c_lld_disable(i2cp) != MSG_OK) {
     return;
   }
 
@@ -380,12 +427,23 @@ void i2c_lld_start(I2CDriver *i2cp) {
 #endif
             (2U << I2C_IC_CON_SPEED_Pos) | // Always Fast Mode
             I2C_IC_CON_MASTER_MODE |
-            I2C_IC_CON_STOP_DET_IF_MASTER_ACTIVE;
+            I2C_IC_CON_STOP_DET_IF_MASTER_ACTIVE |
+            I2C_IC_CON_TX_EMPTY_CTRL;
+
 
   dp->RXTL = 0U;
   dp->TXTL = 0U;
 
-  i2c_setup_frequency(i2cp);
+  i2c_lld_setup_frequency(i2cp);
+
+  /* Clear interrupt mask. */
+  dp->INTRMASK = 0U;
+
+  /* Enable peripheral */
+  dp->ENABLE = I2C_IC_ENABLE_ENABLE;
+
+  /* Clear interrupts. */
+  (void)dp->CLRINTR;
 }
 
 /**
@@ -398,8 +456,8 @@ void i2c_lld_start(I2CDriver *i2cp) {
 void i2c_lld_stop(I2CDriver *i2cp) {
 
   if (i2cp->state != I2C_STOP) {
-      if (i2c_disable(i2cp) != MSG_OK) {
-        i2c_abort(i2cp);
+      if (i2c_lld_disable(i2cp) != MSG_OK) {
+        i2c_lld_abort_transmission(i2cp);
       }
 #if RP_I2C_USE_I2C0 == TRUE
     if (&I2CD1 == i2cp) {
@@ -443,11 +501,9 @@ void i2c_lld_stop(I2CDriver *i2cp) {
 msg_t i2c_lld_master_receive_timeout(I2CDriver *i2cp, i2caddr_t addr,
                                      uint8_t *rxbuf, size_t rxbytes,
                                      sysinterval_t timeout) {
-
   msg_t msg;
   systime_t start, end;
   I2C_TypeDef *dp = i2cp->i2c;
-  uint32_t data;
 
   /* Releases the lock from high level driver.*/
   osalSysUnlock();
@@ -477,13 +533,15 @@ msg_t i2c_lld_master_receive_timeout(I2CDriver *i2cp, i2caddr_t addr,
 
   i2cp->txbytes = 0U;
   i2cp->rxbytes = rxbytes;
+  i2cp->rxbytes_pending = 0U;
+  i2cp->send_restart = false;
   i2cp->txindex = 0U;
   i2cp->rxindex = 0U;
   i2cp->txbuf = NULL;
   i2cp->rxbuf = rxbuf;
 
   /* Disable I2C peripheral during setup phase. */
-  msg = i2c_disable(i2cp);
+  msg = i2c_lld_disable(i2cp);
   if (msg != MSG_OK) {
     return msg;
   }
@@ -502,23 +560,12 @@ msg_t i2c_lld_master_receive_timeout(I2CDriver *i2cp, i2caddr_t addr,
 
   /* Set interrupt mask. */
   dp->INTRMASK = I2C_IC_INTR_MASK_M_STOP_DET |
-                 I2C_IC_INTR_MASK_M_RX_FULL |
+                 I2C_IC_INTR_MASK_M_TX_EMPTY |
                  I2C_ERROR_INTERRUPTS;
 
-  /* Read flag. */
-  data = I2C_IC_DATA_CMD_CMD;
-  /* Send STOP after last byte. */
-  if (i2cp->rxbytes == 1U) {
-    data |= I2C_IC_DATA_CMD_STOP;
-  }
-
-  dp->DATACMD = data;
-
   /* Waits for the operation completion or a timeout.*/
   msg = osalThreadSuspendTimeoutS(&i2cp->thread, timeout);
 
-  // In case of a software timeout or error a STOP is sent as an extreme attempt
-  // to release the bus.
   if (msg == MSG_TIMEOUT) {
     /* Disable and clear interrupts. */
     dp->INTRMASK = 0U;
@@ -587,13 +634,15 @@ msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
 
   i2cp->txbytes = txbytes;
   i2cp->rxbytes = rxbytes;
+  i2cp->rxbytes_pending = 0U;
+  i2cp->send_restart = false;
   i2cp->txindex = 0U;
   i2cp->rxindex = 0U;
   i2cp->txbuf = txbuf;
   i2cp->rxbuf = rxbuf;
 
   /* disabel i2c peripheral during setup phase. */
-  msg = i2c_disable(i2cp);
+  msg = i2c_lld_disable(i2cp);
   if (msg != MSG_OK) {
     return msg;
   }
@@ -601,9 +650,6 @@ msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
   /* Set target address. */
   dp->TAR = addr & I2C_IC_TAR_IC_TAR;
 
-  /* TX_EMPTY irq active. */
-  dp->CON |= I2C_IC_CON_TX_EMPTY_CTRL;
-
   /* Clear interrupt mask. */
   dp->INTRMASK = 0U;
 
diff --git a/os/hal/ports/RP/LLD/I2Cv1/hal_i2c_lld.h b/os/hal/ports/RP/LLD/I2Cv1/hal_i2c_lld.h
index ff278adeef..065dbfaf23 100644
--- a/os/hal/ports/RP/LLD/I2Cv1/hal_i2c_lld.h
+++ b/os/hal/ports/RP/LLD/I2Cv1/hal_i2c_lld.h
@@ -139,6 +139,15 @@ struct I2CDriver {
    * @brief     Number of bytes in RX phase.
    */
   size_t                    rxbytes;
+  /**
+   * @brief     Number of bytes that have been requested,
+   *            but have not been received yet.
+   */
+  size_t                    rxbytes_pending;
+  /**
+   * @brief     Send restart on next transmission.
+   */
+  bool                      send_restart;
   /**
    * @brief     Next index of data in TX buffer.
    */