Tentative fixes for 2C lockups

os/hal/ports/RP/LLD/I2Cv1/hal_i2c_lld.c

@@ -113,43 +113,60 @@ static void setup_frequency(I2CDriver *i2cp) {
  */
 static void serve_interrupt(I2CDriver *i2cp) {
   I2C_TypeDef *dp = i2cp->i2c;
+  uint32_t intr   = dp->INTRSTAT;
+  uint32_t data;
 
   if (i2cp->state == I2C_ACTIVE_TX) {
     /* Transmission phase. */
-    if (dp->INTRSTAT & I2C_IC_INTR_STAT_R_TX_EMPTY_Msk) {
-      if (i2cp->txbytes) {
-        while (i2cp->txbytes && (dp->TXFLR & I2C_IC_TXFLR_TXFLR_Msk) < 16) {
-          dp->DATACMD = (uint32_t)i2cp->txbuf[i2cp->txindex] |
-                        (i2cp->txbytes == 1 ? I2C_IC_DATA_CMD_STOP : 0);
-          i2cp->txindex++;
-          i2cp->txbytes--;
+    if (intr & I2C_IC_INTR_STAT_R_TX_EMPTY) {
+      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--;
 
-        if (i2cp->txbytes == 0U) {
-          // Disable TX_EMPTY irq
-          dp->CLR.CON = I2C_IC_CON_TX_EMPTY_CTRL;
-          dp->CLR.INTRMASK = I2C_IC_INTR_MASK_M_TX_EMPTY;
+        /* Error during transmission  */
+        if(dp->INTRSTAT & I2C_IC_INTR_STAT_R_TX_ABRT) {
+          return;
         }
-        return;
       }
+
+      /* Everything is send, therefore disable TX FIFO empty IRQ. */
+      if (i2cp->txbytes == 0U) {
+        dp->CLR.INTRMASK = I2C_IC_INTR_MASK_M_TX_EMPTY;
+      }
+
+      return;
     }
   } else {
     /* Receive phase. */
-    if (dp->INTRSTAT & I2C_IC_INTR_STAT_R_RX_FULL_Msk) {
+    if (intr & I2C_IC_INTR_STAT_R_RX_FULL) {
       /* Read out received data. */
       i2cp->rxbuf[i2cp->rxindex] = (uint8_t)dp->DATACMD;
       i2cp->rxindex++;
       i2cp->rxbytes--;
-      if (i2cp->rxbytes > 0) {
-        // Set to master read
-        dp->DATACMD = I2C_IC_DATA_CMD_CMD |
-                      (i2cp->rxbytes == 1 ? I2C_IC_DATA_CMD_STOP : 0);
+
+      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;
         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) {
@@ -161,10 +178,15 @@ static void serve_interrupt(I2CDriver *i2cp) {
                        I2C_IC_INTR_MASK_M_TX_ABRT |
                        I2C_IC_INTR_MASK_M_RX_FULL;
 
+        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 = I2C_IC_DATA_CMD_CMD |
-                      I2C_IC_CON_IC_RESTART_EN |
-                      (i2cp->rxbytes == 1 ? I2C_IC_DATA_CMD_STOP : 0);
+        dp->DATACMD = data;
 
         /* State change to RX. */
         i2cp->state = I2C_ACTIVE_RX;
@@ -176,17 +198,21 @@ static void serve_interrupt(I2CDriver *i2cp) {
     }
   }
 
-  if (dp->INTRSTAT & I2C_IC_INTR_STAT_R_TX_ABRT_Msk) {
-    // Reason, dp->TXABRTSOURCE
+  /* Transmission error detected. */
+  if (intr & (I2C_IC_INTR_STAT_R_TX_ABRT |
+              I2C_IC_INTR_STAT_R_TX_OVER |
+              I2C_IC_INTR_STAT_R_RX_OVER |
+              I2C_IC_INTR_STAT_R_RX_UNDER)) {
     i2cp->errors = dp->TXABRTSOURCE;
-
     _i2c_wakeup_error_isr(i2cp);
+  } else {
+    /* Transmission complete */
+    _i2c_wakeup_isr(i2cp);
   }
 
-  /* Clear interrupt flags. */
+  /* Disable and clear interrupts. */
+  dp->INTRMASK = 0U;
   (void)dp->CLRINTR;
-
-  _i2c_wakeup_isr(i2cp);
 }
 
 /*===========================================================================*/