diff --git a/arch/arm64/src/imx9/Kconfig b/arch/arm64/src/imx9/Kconfig
index 4870d66de2af4..7bea8d670e40f 100644
--- a/arch/arm64/src/imx9/Kconfig
+++ b/arch/arm64/src/imx9/Kconfig
@@ -1094,7 +1094,7 @@ config IMX9_FLEXCAN1_BITRATE
 config IMX9_FLEXCAN1_SAMPLEP
 	int "CAN sample point"
 	depends on !NET_CAN_CANFD
-	default 80
+	default 75
 
 config IMX9_FLEXCAN1_ARBI_BITRATE
 	int "CAN FD Arbitration phase bitrate"
@@ -1104,7 +1104,7 @@ config IMX9_FLEXCAN1_ARBI_BITRATE
 config IMX9_FLEXCAN1_ARBI_SAMPLEP
 	int "CAN FD Arbitration phase sample point"
 	depends on NET_CAN_CANFD
-	default 80
+	default 75
 
 config IMX9_FLEXCAN1_DATA_BITRATE
 	int "CAN FD Data phase bitrate"
@@ -1114,7 +1114,7 @@ config IMX9_FLEXCAN1_DATA_BITRATE
 config IMX9_FLEXCAN1_DATA_SAMPLEP
 	int "CAN FD Data phase sample point"
 	depends on NET_CAN_CANFD
-	default 90
+	default 75
 
 endmenu # IMX9_FLEXCAN1
 
@@ -1129,7 +1129,7 @@ config IMX9_FLEXCAN2_BITRATE
 config IMX9_FLEXCAN2_SAMPLEP
 	int "CAN sample point"
 	depends on !NET_CAN_CANFD
-	default 80
+	default 75
 
 config IMX9_FLEXCAN2_ARBI_BITRATE
 	int "CAN FD Arbitration phase bitrate"
@@ -1139,7 +1139,7 @@ config IMX9_FLEXCAN2_ARBI_BITRATE
 config IMX9_FLEXCAN2_ARBI_SAMPLEP
 	int "CAN FD Arbitration phase sample point"
 	depends on NET_CAN_CANFD
-	default 80
+	default 75
 
 config IMX9_FLEXCAN2_DATA_BITRATE
 	int "CAN FD Data phase bitrate"
@@ -1149,7 +1149,7 @@ config IMX9_FLEXCAN2_DATA_BITRATE
 config IMX9_FLEXCAN2_DATA_SAMPLEP
 	int "CAN FD Data phase sample point"
 	depends on NET_CAN_CANFD
-	default 90
+	default 75
 
 endmenu # IMX9_FLEXCAN2
 
diff --git a/arch/arm64/src/imx9/imx9_flexcan.c b/arch/arm64/src/imx9/imx9_flexcan.c
index 683822c5b9f6b..fd86375cd86be 100644
--- a/arch/arm64/src/imx9/imx9_flexcan.c
+++ b/arch/arm64/src/imx9/imx9_flexcan.c
@@ -71,31 +71,30 @@
 #define IFLAG1_RX                   ((1 << RXMBCOUNT)-1)
 #define IFLAG1_TX                   (((1 << TXMBCOUNT)-1) << RXMBCOUNT)
 
-#define CAN_FIFO_NE                 (1 << 5)
-#define CAN_FIFO_OV                 (1 << 6)
-#define CAN_FIFO_WARN               (1 << 7)
-#define CAN_EFF_FLAG                0x80000000 /* EFF/SFF is set in the MSB */
-
 #define POOL_SIZE                   1
 
 #define MSG_DATA                    sizeof(struct timeval)
 
-/* CAN bit timing values  */
-#define PRESDIV_MAX                 256
-
-#define SEG_MAX                     8
-#define SEG_MIN                     1
 #define TSEG_MIN                    2
-#define TSEG1_MAX                   17
-#define TSEG2_MAX                   9
-#define NUMTQ_MAX                   26
 
-#define SEG_FD_MAX                  32
-#define SEG_FD_MIN                  1
-#define TSEG_FD_MIN                 2
-#define TSEG1_FD_MAX                39
-#define TSEG2_FD_MAX                9
-#define NUMTQ_FD_MAX                49
+/* Classical can CTRL1 bit timing */
+
+#define TSEG1_MAX                   16
+#define TSEG2_MAX                   8
+#define NUMTQ_MIN                   8
+#define NUMTQ_MAX                   25
+
+/* CAN FD CBT and FDCBT bit timing */
+
+#define TSEG1_FD_MAX                96
+#define TSEG2_FD_MAX                32
+#define NUMTQ_FD_MIN                8
+#define NUMTQ_FD_MAX                129
+
+#define TSEG1_FD_DATAMAX            39
+#define TSEG2_FD_DATAMAX            8
+#define NUMTQ_FD_DATAMIN            5
+#define NUMTQ_FD_DATAMAX            48
 
 #ifdef CONFIG_NET_CAN_RAW_TX_DEADLINE
 
@@ -110,11 +109,6 @@
 # error Only 21 MB are allowed to be used
 #endif
 
-/* Interrupt flags for RX fifo */
-#define IFLAG1_RXFIFO               (CAN_FIFO_NE | CAN_FIFO_WARN | CAN_FIFO_OV)
-
-static int peak_tx_mailbox_index_ = 0;
-
 /****************************************************************************
  * Private Types
  ****************************************************************************/
@@ -296,114 +290,196 @@ static inline uint32_t arm64_lsb(unsigned int value)
  *
  * Input Parameters:
  *   timeseg - structure to store bit timing
- *   sp_tolerance - allowed difference in sample point from calculated
  *                  bit timings (recommended value: 1)
- *   can_fd - if set to calculate CAN FD bit timings, otherwise calculate
- *            classical can timings
+ *   can_fd_data - if set to calculate CAN FD data bit timings,
+ *                  otherwise calculate classical or arbitration can
+ *                  timings
  *
  * Returned Value:
  *   return OK on success, negated error number on failure
  *
  ****************************************************************************/
 
-uint32_t imx9_bitratetotimeseg(struct flexcan_timeseg *timeseg,
-                               int32_t sp_tolerance,
-                               uint32_t can_fd,
-                               uint32_t clk_freq)
+uint32_t imx9_bitratetotimeseg(struct imx9_driver_s *priv,
+                               struct flexcan_timeseg *timeseg,
+                               bool can_fd_data)
 {
+#if defined(CONFIG_NET_CAN_CANFD)
+  /* Max SEG1 & SEG2 values in TQ */
+
+  const int32_t TSEG1MAX = can_fd_data ? TSEG1_FD_DATAMAX : TSEG1_FD_MAX;
+  const int32_t TSEG2MAX = can_fd_data ? TSEG2_FD_DATAMAX : TSEG2_FD_MAX;
+
+  /* Min and max bit length in TQ */
+
+  const int32_t NUMTQMIN = can_fd_data ? NUMTQ_FD_DATAMIN : NUMTQ_FD_MIN;
+  const int32_t NUMTQMAX = can_fd_data ? NUMTQ_FD_DATAMAX : NUMTQ_FD_MAX;
+
+  /* Max register field values */
+
+  /* Max register field value for presdiv */
+
+  const uint32_t PRESDIVMAX = can_fd_data ?
+    CAN_FDCBT_FPRESDIV_MASK >> CAN_FDCBT_FPRESDIV_SHIFT :
+    CAN_CBT_EPRESDIV_MASK >> CAN_CBT_EPRESDIV_SHIFT;
+
+  /* Max register field values from PSEG and PROPSEG */
+
+  const int32_t PSEGMAX = can_fd_data ?
+    CAN_FDCBT_FPSEG1_MASK >> CAN_FDCBT_FPSEG1_SHIFT :
+    CAN_CBT_EPSEG1_MASK >> CAN_CBT_EPSEG1_SHIFT;
+  const int32_t PROPSEGMAX = can_fd_data ?
+    (CAN_FDCBT_FPROPSEG_MASK >> CAN_FDCBT_FPROPSEG_SHIFT) - 1 :
+    CAN_CBT_EPROPSEG_MASK >> CAN_CBT_EPROPSEG_SHIFT;
+#else
+  /* Max SEG1 & SEG2 values in TQ */
+
+  const int32_t TSEG1MAX = TSEG1_MAX;
+  const int32_t TSEG2MAX = TSEG2_MAX;
+
+  /* Min and max bit length in TQ */
+
+  const int32_t NUMTQMIN = NUMTQ_MIN;
+  const int32_t NUMTQMAX = NUMTQ_MAX;
+
+  /* Max register field values */
+
+  /* Max register field value for presdiv */
+
+  const uint32_t PRESDIVMAX =
+    CAN_CTRL1_PRESDIV_MASK >> CAN_CTRL1_PRESDIV_SHIFT;
+
+  /* Max register field values from PSEG and PROPSEG */
+
+  const int32_t PSEGMAX = CAN_CTRL1_PSEG1_MASK >> CAN_CTRL1_PSEG1_SHIFT;
+  const int32_t PROPSEGMAX =
+    CAN_CTRL1_PROPSEG_MASK >> CAN_CTRL1_PROPSEG_SHIFT;
+#endif
+
+  int32_t presdiv = PRESDIVMAX;
   int32_t tmppresdiv;
   int32_t numtq;
+  int32_t tmpnumtq;
   int32_t tmpsample;
   int32_t tseg1;
   int32_t tseg2;
   int32_t tmppseg1;
   int32_t tmppseg2;
   int32_t tmppropseg;
+  int32_t bitrate = 0;
 
-  const int32_t TSEG1MAX = (can_fd ? TSEG1_FD_MAX : TSEG1_MAX);
-  const int32_t TSEG2MAX = (can_fd ? TSEG2_FD_MAX : TSEG2_MAX);
-  const int32_t SEGMAX = (can_fd ? SEG_FD_MAX : SEG_MAX);
-  const int32_t NUMTQMAX = (can_fd ? NUMTQ_FD_MAX : NUMTQ_MAX);
+  int32_t bitrate_tmp = 0;
+  int32_t bitrate_err = INT32_MAX;
 
-  for (tmppresdiv = 0; tmppresdiv < PRESDIV_MAX; tmppresdiv++)
+  for (tmppresdiv = 0; tmppresdiv < PRESDIVMAX; tmppresdiv++)
     {
-      numtq = (clk_freq / ((tmppresdiv + 1) * timeseg->bitrate));
+      tmpnumtq = (priv->clk_freq / ((tmppresdiv + 1) * timeseg->bitrate));
+
+      /* if number of time quanta per bit is too high, continue */
 
-      if (numtq == 0)
+      if (tmpnumtq > NUMTQMAX)
         {
           continue;
         }
 
-      /* The number of time quanta in 1 bit time must be
-       * lower than the one supported
-       */
+      /* if number of time quanta per bit is too small, break out */
 
-      if ((clk_freq / ((tmppresdiv + 1) * numtq) == timeseg->bitrate)
-          && (numtq >= 8) && (numtq < NUMTQMAX))
+      if (tmpnumtq < NUMTQMIN)
         {
-          /* Compute time segments based on the value of the sampling point */
+          break;
+        }
 
-          tseg1 = (numtq * timeseg->samplep / 100) - 1;
-          tseg2 = numtq - 1 - tseg1;
+      bitrate_tmp = priv->clk_freq / ((tmppresdiv + 1) * tmpnumtq);
+      if (abs(bitrate - bitrate_tmp) < bitrate_err)
+        {
+          bitrate_err = abs(bitrate - bitrate_tmp);
+          bitrate = bitrate_tmp;
+          numtq = tmpnumtq;
+          presdiv = tmppresdiv;
+        }
+    }
 
-          /* Adjust time segment 1 and time segment 2 */
+  if (bitrate != timeseg->bitrate)
+    {
+      canwarn("bitrate set to %" PRId32 " instead of %" PRId32 "\n",
+              bitrate, timeseg->bitrate);
+    }
 
-          while (tseg1 >= TSEG1MAX || tseg2 < TSEG_MIN)
-            {
-              tseg2++;
-              tseg1--;
-            }
+  /* Compute time segments based on the value of the sampling point */
 
-          tmppseg2 = tseg2 - 1;
+  tseg1 = (numtq * timeseg->samplep / 100) - 1;
+  tseg2 = numtq - 1 - tseg1;
 
-          /* Start from pseg1 = pseg2 and adjust until propseg is valid */
+  /* Adjust time segment 1 and time segment 2 */
 
-          tmppseg1 = tmppseg2;
-          tmppropseg = tseg1 - tmppseg1 - 2;
+  while (tseg1 >= TSEG1MAX || tseg2 < TSEG_MIN)
+    {
+      tseg2++;
+      tseg1--;
+    }
 
-          while (tmppropseg <= 0)
-            {
-              tmppropseg++;
-              tmppseg1--;
-            }
+  if (tseg1 > TSEG1MAX || tseg2 > TSEG2MAX ||
+      tseg2 < TSEG_MIN || tseg1 < TSEG_MIN)
+    {
+      canerr("tseg1 %" PRId32 ", max %" PRId32 "\n", tseg1, TSEG1MAX);
+      canerr("tseg2 %" PRId32 ", max %" PRId32 "\n", tseg2, TSEG2MAX);
+      return -EINVAL;
+    }
 
-          while (tmppropseg >= SEGMAX)
-            {
-              tmppropseg--;
-              tmppseg1++;
-            }
+  DEBUGASSERT(1 + tseg1 + tseg2 == numtq);
 
-          if (((tseg1 >= TSEG1MAX) || (tseg2 >= TSEG2MAX) ||
-              (tseg2 < TSEG_MIN) || (tseg1 < TSEG_MIN)) ||
-              ((tmppropseg >= SEGMAX) || (tmppseg1 >= SEGMAX) ||
-                  (tmppseg2 < SEG_MIN) || (tmppseg2 >= SEGMAX)))
-            {
-              continue;
-            }
+  tmppseg2 = tseg2 - 1;
 
-          tmpsample = ((tseg1 + 1) * 100) / numtq;
+  /* Start from pseg1 = pseg2 and adjust until propseg is valid */
 
-          if ((tmpsample - timeseg->samplep) <= sp_tolerance &&
-              (timeseg->samplep - tmpsample) <= sp_tolerance)
-            {
-              if (can_fd == 1)
-                {
-                  timeseg->propseg = tmppropseg + 1;
-                }
-              else
-                {
-                  timeseg->propseg = tmppropseg;
-                }
-
-              timeseg->pseg1 = tmppseg1;
-              timeseg->pseg2 = tmppseg2;
-              timeseg->presdiv = tmppresdiv;
-              timeseg->samplep = tmpsample;
-              return OK;
-            }
+  tmppseg1 = tmppseg2;
+  tmppropseg = tseg1 - tmppseg1 - 2;
+
+  while (tmppropseg <= 0)
+    {
+      tmppropseg++;
+      tmppseg1--;
+    }
+
+  while (tmppropseg >= PROPSEGMAX)
+    {
+      tmppropseg--;
+      tmppseg1++;
+    }
+
+  if (tmppseg1 > PSEGMAX || tmppseg2 > PSEGMAX)
+    {
+      canerr("tmppseg1 %" PRId32 ", max %" PRId32 "\n", tmppseg1, PSEGMAX);
+      canerr("tmppseg2 %" PRId32 ", max %" PRId32 "\n", tmppseg2, PSEGMAX);
+      return -EINVAL;
+    }
+
+  tmpsample = (1 + tseg1) * 100 / numtq;
+
+  /* Allow 5% tolerance in sample point */
+
+  if (abs(tmpsample - timeseg->samplep) <= 5)
+    {
+      if (can_fd_data)
+        {
+          timeseg->propseg = tmppropseg + 1;
         }
+      else
+        {
+          timeseg->propseg = tmppropseg;
+        }
+
+      timeseg->pseg1 = tmppseg1;
+      timeseg->pseg2 = tmppseg2;
+      timeseg->presdiv = presdiv;
+      timeseg->samplep = tmpsample;
+
+      return OK;
     }
 
+  canerr("sample point %" PRId32 ", configured %" PRId32 "\n",
+         tmpsample, timeseg->samplep);
+
   return -EINVAL;
 }
 
@@ -425,14 +501,13 @@ static struct mb_s *flexcan_get_mb(struct imx9_driver_s *priv,
 
 /* Interrupt handling */
 
-static void imx9_receive(struct imx9_driver_s *priv,
-                         uint32_t flags);
+static void imx9_rxdone_work(void *arg);
+static void imx9_rxdone(struct imx9_driver_s *priv);
 static void imx9_txdone_work(void *arg);
 static void imx9_txdone(struct imx9_driver_s *priv);
 
 static int  imx9_flexcan_interrupt(int irq, void *context,
                                    void *arg);
-static void imx9_flexcan_interrupt_work(void *arg);
 
 /* Watchdog timer expirations */
 #ifdef TX_TIMEOUT_WQ
@@ -479,18 +554,11 @@ static void imx9_reset(struct imx9_driver_s *priv);
 
 static bool imx9_txringfull(struct imx9_driver_s *priv)
 {
-  uint32_t mbi = RXMBCOUNT;
-  struct mb_s *mb;
-
-  while (mbi < TOTALMBCOUNT)
+  uint32_t flags = getreg32(priv->base + IMX9_CAN_IFLAG1_OFFSET);
+  
+  if ((flags & IFLAG1_TX) != IFLAG1_TX)
     {
-      mb = flexcan_get_mb(priv, mbi);
-      if (CAN_MB_CS_CODE(mb->cs) != CAN_TXMB_DATAORREMOTE)
-        {
-          return 0;
-        }
-
-      mbi++;
+      return 0;
     }
 
   return 1;
@@ -529,15 +597,15 @@ static int imx9_transmit(struct imx9_driver_s *priv)
   int32_t timeout;
   uint32_t txmb = 0;
 #endif
-
+  struct mb_s *mb;
+  _alert("1 ERRSR: %x\n", getreg32(priv->base + IMX9_CAN_ERRSR_OFFSET));
   for (mbi = RXMBCOUNT; mbi < TOTALMBCOUNT; mbi++)
     {
       /* Check whether message buffer is not currently transmitting */
 
-      struct mb_s *mb = flexcan_get_mb(priv, mbi);
+      mb = flexcan_get_mb(priv, mbi);
       _alert("mbi %d cs %x\n", mbi, CAN_MB_CS_CODE(mb->cs));
       _alert("MCR: %x\n", getreg32(priv->base + IMX9_CAN_MCR_OFFSET));
-      _alert("ESR1: %x\n", getreg32(priv->base + IMX9_CAN_ESR1_OFFSET));
       if (CAN_MB_CS_CODE(mb->cs) != CAN_TXMB_DATAORREMOTE)
         {
           break;
@@ -548,6 +616,7 @@ static int imx9_transmit(struct imx9_driver_s *priv)
 #endif
     }
 
+  _alert("2 ERRSR: %x\n", getreg32(priv->base + IMX9_CAN_ERRSR_OFFSET));
   if (mbi >= TOTALMBCOUNT)
     {
       nwarn("No TX MB available mbi %" PRIi32 "\n", mbi);
@@ -557,8 +626,6 @@ static int imx9_transmit(struct imx9_driver_s *priv)
 
   _alert("Got MBI %d\n", mbi);
 
-  modifyreg32(priv->base + IMX9_CAN_IFLAG1_OFFSET, 1 << mbi, 0);
-
 #ifdef CONFIG_NET_CAN_RAW_TX_DEADLINE
   struct timespec ts;
   clock_systime_timespec(&ts);
@@ -597,68 +664,57 @@ static int imx9_transmit(struct imx9_driver_s *priv)
     }
 #endif
 
-  peak_tx_mailbox_index_ =
-    (peak_tx_mailbox_index_ > mbi ? peak_tx_mailbox_index_ : mbi);
+  _alert("IFLAG1 %x\n", getreg32(priv->base + IMX9_CAN_IFLAG1_OFFSET));
 
-  struct mb_s *mb = flexcan_get_mb(priv, mbi);
   uint32_t cs = 0;
-  mb->id = 0;
+  canid_t can_id;
+  uint8_t can_dlc;
+  uint8_t len;
 
   if (priv->dev.d_len == sizeof(struct can_frame))
     {
       struct can_frame *frame = (struct can_frame *)priv->dev.d_buf;
-
-      if (frame->can_id & CAN_EFF_FLAG)
-        {
-          cs |= CAN_MB_CS_IDE;
-          mb->id |= (frame->can_id & CAN_MB_ID_ID_MASK);
-        }
-      else
-        {
-          mb->id |= (frame->can_id & CAN_MB_ID_ID_STD_MASK);
-        }
-
-      cs |= (frame->can_id & CAN_RTR_FLAG) ? CAN_MB_CS_RTR : 0;
-      cs |= ((frame->can_dlc << CAN_MB_CS_DLC_SHIFT) & CAN_MB_CS_DLC_MASK);
-      mb->data[0] = __builtin_bswap32(*(uint32_t *)&frame->data[0]);
-      mb->data[1] = __builtin_bswap32(*(uint32_t *)&frame->data[4]);
+      can_id = frame->can_id;
+      len = 8;
+      can_dlc = frame->can_dlc;
+      frame_data_word = (uint32_t *)&frame->data[0];
     }
 #ifdef CONFIG_NET_CAN_CANFD
-  else /* CAN FD frame */
+  else
     {
       struct canfd_frame *frame = (struct canfd_frame *)priv->dev.d_buf;
-
-      cs |= CAN_MB_CS_EDL; /* CAN FD Frame */
-
-      if (frame->can_id & CAN_EFF_FLAG)
-        {
-          cs |= CAN_MB_CS_IDE;
-          mb->id |= ((frame->can_id << CAN_MB_ID_ID_SHIFT) &
-                     CAN_MB_ID_ID_MASK);
-        }
-      else
-        {
-          mb->id |= ((frame->can_id << CAN_MB_ID_ID_STD_SHIFT) &
-                     CAN_MB_ID_ID_STD_MASK);
-        }
-
-      cs |= (frame->can_id & CAN_RTR_FLAG) ? CAN_MB_CS_RTR : 0;
-      cs |= ((((uint32_t)g_len_to_can_dlc[frame->len]) << CAN_MB_CS_DLC_SHIFT) &
-             CAN_MB_CS_DLC_MASK);
-
+      cs |= CAN_MB_CS_EDL;
+      cs |= frame->flags & CANFD_BRS ? CAN_MB_CS_BRS : 0;
+      can_id = frame->can_id;
+      len = frame->len;
+      can_dlc = g_len_to_can_dlc[len];
       frame_data_word = (uint32_t *)&frame->data[0];
-      for (i = 0; i < (frame->len + 4 - 1) / 4; i++)
-        {
-          mb->data[i] = __builtin_bswap32(frame_data_word[i]);
-        }
+    }
+#endif
 
+  if (can_id & CAN_EFF_FLAG)
+    {
+      cs |= CAN_MB_CS_IDE;
+      mb->id = (can_id << CAN_MB_ID_ID_SHIFT) & CAN_MB_ID_ID_MASK;
+    }
+  else
+    {
+      mb->id = (can_id << CAN_MB_ID_ID_STD_SHIFT) & CAN_MB_ID_ID_STD_MASK;
+    }
+
+  cs |= (can_id & CAN_RTR_FLAG) ? CAN_MB_CS_RTR : 0;
+  cs |= (((uint32_t)can_dlc) << CAN_MB_CS_DLC_SHIFT) & CAN_MB_CS_DLC_MASK;
 
-      _alert("payload sz %d\n", frame->len);
-      _alert("GO: %x\n", (cs | (CAN_TXMB_DATAORREMOTE << CAN_MB_CS_CODE_SHIFT)));      
+  for (i = 0; i < (len + 4 - 1) / 4; i++)
+    {
+      mb->data[i] = __builtin_bswap32(frame_data_word[i]);
     }
-#endif
-  
+
+  _alert("payload sz %d\n", len);
+  _alert("GO: %x\n", (cs | (CAN_TXMB_DATAORREMOTE << CAN_MB_CS_CODE_SHIFT)));
+  _alert("3 ERRSR: %x\n", getreg32(priv->base + IMX9_CAN_ERRSR_OFFSET));
   mb->cs = (cs | (CAN_TXMB_DATAORREMOTE << CAN_MB_CS_CODE_SHIFT)); /* Go */
+  _alert("4 ERRSR: %x\n", getreg32(priv->base + IMX9_CAN_ERRSR_OFFSET));
 
   /* Enable interrupt */
 
@@ -747,7 +803,7 @@ static int imx9_txpoll(struct net_driver_s *dev)
 }
 
 /****************************************************************************
- * Function: imx9_receive
+ * Function: imx9_rxdone
  *
  * Description:
  *   An interrupt was received indicating the availability of a new RX packet
@@ -763,8 +819,7 @@ static int imx9_txpoll(struct net_driver_s *dev)
  *
  ****************************************************************************/
 
-static void imx9_receive(struct imx9_driver_s *priv,
-                            uint32_t flags)
+static void imx9_rxdone(struct imx9_driver_s *priv)
 {
   uint32_t mbi;
   uint32_t mbj;
@@ -775,6 +830,9 @@ static void imx9_receive(struct imx9_driver_s *priv,
 #endif
   uint32_t f;
 
+  uint32_t flags = getreg32(priv->base + IMX9_CAN_IFLAG1_OFFSET);
+
+  flags &= IFLAG1_RX;
   while ((f = flags) != 0)
     {
       mbj = mbi = arm64_lsb(f);
@@ -820,6 +878,14 @@ static void imx9_receive(struct imx9_driver_s *priv,
               frame->can_id |= CAN_RTR_FLAG;
             }
 
+          /* Enable bitrate switch by default if frame is CANFD */
+
+          frame->flags = CANFD_BRS;
+          if (rf->cs & CAN_MB_CS_ESI)
+            {
+              frame->flags |= CANFD_ESI;
+            }
+
           frame->len = g_can_dlc_to_len[CAN_MB_CS_DLC(rf->cs)];
 
           frame_data_word = (uint32_t *)&frame->data[0];
@@ -829,11 +895,6 @@ static void imx9_receive(struct imx9_driver_s *priv,
               frame_data_word[i] = __builtin_bswap32(rf->data[i]);
             }
 
-          /* Clear MB interrupt flag */
-
-          putreg32(1 << mbi,
-                   priv->base + IMX9_CAN_IFLAG1_OFFSET);
-
           /* Copy the buffer pointer to priv->dev..  Set amount of data
            * in priv->dev.d_len
            */
@@ -870,11 +931,6 @@ static void imx9_receive(struct imx9_driver_s *priv,
           *(uint32_t *)&frame->data[0] = __builtin_bswap32(rf->data[0]);
           *(uint32_t *)&frame->data[4] = __builtin_bswap32(rf->data[1]);
 
-          /* Clear MB interrupt flag */
-
-          putreg32(1 << mbi,
-                   priv->base + IMX9_CAN_IFLAG1_OFFSET);
-
           /* Copy the buffer pointer to priv->dev..  Set amount of data
            * in priv->dev.d_len
            */
@@ -883,6 +939,14 @@ static void imx9_receive(struct imx9_driver_s *priv,
           priv->dev.d_buf = (uint8_t *)frame;
         }
 
+      /* Clear MB interrupt flag */
+
+      putreg32(1 << mbi, priv->base + IMX9_CAN_IFLAG1_OFFSET);
+
+      /* Re-enable interrupt */
+
+      modifyreg32(priv->base + IMX9_CAN_IMASK1_OFFSET, 0, 1 << mbi);
+
       /* Send to socket interface */
 
       NETDEV_RXPACKETS(&priv->dev);
@@ -906,14 +970,6 @@ static void imx9_receive(struct imx9_driver_s *priv,
         }
 
       flags &= ~(1 << mbi);
-
-      /* Reread interrupt flags and process them in this loop */
-
-      if (flags == 0)
-        {
-          flags  = getreg32(priv->base + IMX9_CAN_IFLAG1_OFFSET);
-          flags &= IFLAG1_RX;
-        }
     }
 }
 
@@ -941,22 +997,18 @@ static void imx9_txdone(struct imx9_driver_s *priv)
 #endif
 
   flags  = getreg32(priv->base + IMX9_CAN_IFLAG1_OFFSET);
-  flags &= IFLAG1_TX;
 
   /* Process TX completions */
 
-  mbi = RXMBCOUNT;
-  mb_bit = 1 << mbi;
 #ifdef TX_TIMEOUT_WQ
   txmb = 0;
 #endif
 
-  while (mbi < TOTALMBCOUNT)
+  for (mbi = RXMBCOUNT; mbi < TOTALMBCOUNT; mbi++)
     {
+      mb_bit = 1 << mbi;
       if (flags & mb_bit)
         {
-          putreg32(mb_bit, priv->base + IMX9_CAN_IFLAG1_OFFSET);
-          flags &= ~mb_bit;
           NETDEV_TXDONE(&priv->dev);
 #ifdef TX_TIMEOUT_WQ
           /* We are here because a transmission completed, so the
@@ -965,13 +1017,10 @@ static void imx9_txdone(struct imx9_driver_s *priv)
            */
 
           wd_cancel(&priv->txtimeout[txmb]);
-          struct mb_s *mb = flexcan_get_mb(priv, mbi);
-          mb->cs = CAN_TXMB_INACTIVE << CAN_MB_CS_CODE_SHIFT;
+          putreg32(mb_bit, priv->base + IMX9_CAN_IFLAG1_OFFSET);
 #endif
         }
 
-      mb_bit <<= 1;
-      mbi++;
 #ifdef TX_TIMEOUT_WQ
       txmb++;
 #endif
@@ -1012,7 +1061,7 @@ static void imx9_txdone_work(void *arg)
 }
 
 /****************************************************************************
- * Function: imx9_flexcan_interrupt_work
+ * Function: imx9_rxdone_work
  *
  * Description:
  *   Three interrupt sources will vector this this function:
@@ -1031,21 +1080,13 @@ static void imx9_txdone_work(void *arg)
  *
  ****************************************************************************/
 
-static void imx9_flexcan_interrupt_work(void *arg)
+static void imx9_rxdone_work(void *arg)
 {
   struct imx9_driver_s *priv = (struct imx9_driver_s *)arg;
 
-  uint32_t flags;
-  flags  = getreg32(priv->base + IMX9_CAN_IFLAG1_OFFSET);
-  flags &= IFLAG1_RX;
-
   net_lock();
-  imx9_receive(priv, flags);
+  imx9_rxdone(priv);
   net_unlock();
-
-  /* Mask MB again */
-
-  modifyreg32(priv->base + IMX9_CAN_IMASK1_OFFSET, 0, IFLAG1_RX);
 }
 
 /****************************************************************************
@@ -1072,7 +1113,20 @@ static int imx9_flexcan_interrupt(int irq, void *context,
                                      void *arg)
 {
   struct imx9_driver_s *priv = (struct imx9_driver_s *)arg;
-  _alert("IRQ\n");
+
+  uint32_t esr1 = getreg32(priv->base + IMX9_CAN_ESR1_OFFSET);
+  _alert("ESR1: %x\n", esr1);
+  _alert("ESR2: %x\n", getreg32(priv->base + IMX9_CAN_ESR2_OFFSET));
+  _alert("IFLAG1: %x\n", getreg32(priv->base + IMX9_CAN_IFLAG1_OFFSET));
+  _alert("ECR: %x\n", getreg32(priv->base + IMX9_CAN_ECR_OFFSET));
+
+  /* ERRSR[HANCEIF], ERRSR[FANCEIF], and ERRSR[CEIF] */
+
+  _alert("ERRSR: %x\n", getreg32(priv->base + IMX9_CAN_ERRSR_OFFSET));
+
+  putreg32(esr1, priv->base + IMX9_CAN_ESR1_OFFSET);
+  putreg32(0, priv->base + IMX9_CAN_ECR_OFFSET);
+
   if (irq == priv->irq)
     {
       uint32_t flags;
@@ -1080,14 +1134,11 @@ static int imx9_flexcan_interrupt(int irq, void *context,
 
       if (flags & IFLAG1_RX)
         {
-          modifyreg32(priv->base + IMX9_CAN_IMASK1_OFFSET, IFLAG1_RX, 0);
-          work_queue(CANRCVWORK, &priv->rcvwork,
-                     imx9_flexcan_interrupt_work, priv, 0);
+          work_queue(CANRCVWORK, &priv->rcvwork, imx9_rxdone_work, priv, 0);
         }
 
       if (flags & IFLAG1_TX)
         {
-          modifyreg32(priv->base + IMX9_CAN_IMASK1_OFFSET, IFLAG1_TX, 0);
           work_queue(CANWORK, &priv->irqwork, imx9_txdone_work, priv, 0);
         }
     }
@@ -1128,7 +1179,7 @@ static void imx9_txtimeout_work(void *arg)
    * transmit function transmitted a new frame
    */
 
-  flags  = getreg32(priv->base + IMX9_CAN_IFLAG1_OFFSET);
+  flags = getreg32(priv->base + IMX9_CAN_IFLAG1_OFFSET);
 
   for (mbi = 0; mbi < TXMBCOUNT; mbi++)
     {
@@ -1257,10 +1308,10 @@ static struct mb_s *flexcan_get_mb(struct imx9_driver_s *priv,
   size_t mb_bytes = sizeof(struct mb_s) + data_bytes;
   int mbs_per_block = 512 / mb_bytes; /* n of buffers in one ramblock */
   int ramblock = mbi / mbs_per_block; /* ramblock in which the mb resides */
-  int mb_off = mbi % mbs_per_block;   /* idx of the mb within ramblock */
+  int mb_off = mbi - ramblock * mbs_per_block;   /* idx of the mb within ramblock */
 
   mb_offset = IMX9_CAN_MB_OFFSET + (ramblock * 512) + mb_off * mb_bytes;
-
+    
   DEBUGASSERT(mb_offset < IMX9_CAN_MB_END);
 
   return (struct mb_s *)(priv->base + mb_offset);
@@ -1310,6 +1361,7 @@ static int imx9_ifup(struct net_driver_s *dev)
   /* Set interrupts */
 
   up_enable_irq(priv->irq);
+  up_enable_irq(priv->irq + 1);
 
   return OK;
 }
@@ -1334,6 +1386,11 @@ static int imx9_ifdown(struct net_driver_s *dev)
 {
   struct imx9_driver_s *priv = (struct imx9_driver_s *)dev;
 
+  /* Disable interrupts */
+
+  up_disable_irq(priv->irq);
+  up_disable_irq(priv->irq + 1);
+
   imx9_reset(priv);
 
   priv->bifup = false;
@@ -1480,13 +1537,12 @@ static int imx9_ioctl(struct net_driver_s *dev, int cmd,
 
           arbi_timing.bitrate = req->arbi_bitrate * 1000;
           arbi_timing.samplep = req->arbi_samplep;
-          ret = imx9_bitratetotimeseg(&arbi_timing, 10, 0, priv->clk_freq);
+          ret = imx9_bitratetotimeseg(priv, &arbi_timing, false);
           if (ret == OK && priv->canfd_capable)
             {
               data_timing.bitrate = req->data_bitrate * 1000;
               data_timing.samplep = req->data_samplep;
-              ret = imx9_bitratetotimeseg(&data_timing, 10, 1,
-                                          priv->clk_freq);
+              ret = imx9_bitratetotimeseg(priv, &data_timing, true);
             }
 
           if (ret == OK)
@@ -1545,7 +1601,7 @@ static int imx9_init_mbram(struct imx9_driver_s *priv)
   mb = flexcan_get_mb(priv, mbi);
   for (mbi = 0; mbi < RXMBCOUNT; mbi++)
     {
-      mb->cs = CAN_RXMB_INACTIVE << CAN_MB_CS_CODE_SHIFT;
+      mb->cs = (CAN_RXMB_EMPTY << CAN_MB_CS_CODE_SHIFT) | CAN_MB_CS_IDE;
 
       /* ID + Data fields until next mb */
 
@@ -1600,6 +1656,7 @@ static int imx9_init_mbram(struct imx9_driver_s *priv)
 static int imx9_initialize(struct imx9_driver_s *priv)
 {
   uint32_t i;
+  uint32_t tdcoff;
 
   /* Enable module */
 
@@ -1627,14 +1684,14 @@ static int imx9_initialize(struct imx9_driver_s *priv)
 
   /* Configure MCR */
 
-  modifyreg32(priv->base + IMX9_CAN_MCR_OFFSET, CAN_MCR_MAXMB_MASK,
+  modifyreg32(priv->base + IMX9_CAN_MCR_OFFSET, CAN_MCR_MAXMB_MASK | CAN_MCR_WAKSRC,
               CAN_MCR_SLFWAK | CAN_MCR_WRNEN | CAN_MCR_SRXDIS |
-              CAN_MCR_IRMQ | CAN_MCR_AEN |
+              CAN_MCR_IRMQ | /*CAN_MCR_AEN |*/
               ((TOTALMBCOUNT << CAN_MCR_MAXMB_SHIFT) & CAN_MCR_MAXMB_MASK));
 
-  /* Exit supervisor mode */
+  /* Configure MECR */
 
-  modifyreg32(priv->base + IMX9_CAN_MCR_OFFSET, CAN_MCR_SUPV, 0);
+  //modifyreg32(priv->base + IMX9_CAN_MECR_OFFSET, 0, 0);
 
   if (!priv->canfd_capable)
     {
@@ -1650,6 +1707,9 @@ static int imx9_initialize(struct imx9_driver_s *priv)
     }
   else
     {
+      //modifyreg32(priv->base + IMX9_CAN_CTRL1_OFFSET, 0,
+      //                  CAN_CTRL1_LPB);
+
       modifyreg32(priv->base + IMX9_CAN_CBT_OFFSET,
                   CAN_CBT_EPRESDIV_MASK | CAN_CBT_EPROPSEG_MASK |
                   CAN_CBT_EPSEG1_MASK | CAN_CBT_EPSEG2_MASK |
@@ -1680,41 +1740,56 @@ static int imx9_initialize(struct imx9_driver_s *priv)
 
       /* Additional CAN-FD configurations */
 
+      tdcoff = (priv->data_timing.pseg1 + priv->data_timing.pseg2 + 2) *
+        (priv->data_timing.presdiv + 1);
+
       modifyreg32(priv->base + IMX9_CAN_FDCTRL_OFFSET, 0,
-                  CAN_FDCTRL_FDRATE |     /* Enable bit rate switch in data phase of frame */
-                  CAN_FDCTRL_TDCEN |      /* Enable transceiver delay compensation */
-                  CAN_FDCTRL_TDCOFF(5) |  /* Setup 5 cycles for data phase sampling delay */
-                  CAN_FDCTRL_MSBSR0(3) |  /* Setup 64 bytes per MB 0-6 */
-                  CAN_FDCTRL_MSBSR1(3) |  /* Setup 64 bytes per MB 7-13 */
-                  CAN_FDCTRL_MSBSR2(3));  /* Setup 64 bytes per MB 14-20 */
+                  CAN_FDCTRL_FDRATE |          /* Enable bit rate switch in data phase of frame */
+                  CAN_FDCTRL_TDCEN |           /* Enable transceiver delay compensation */
+                  CAN_FDCTRL_TDCOFF(tdcoff) |  /* Setup 5 cycles for data phase sampling delay */
+                  CAN_FDCTRL_MSBSR0(3) |       /* Setup 64 bytes per MB 0-6 */
+                  CAN_FDCTRL_MSBSR1(3) |       /* Setup 64 bytes per MB 7-13 */
+                  CAN_FDCTRL_MSBSR2(3));       /* Setup 64 bytes per MB 14-20 */
 
       modifyreg32(priv->base + IMX9_CAN_CTRL2_OFFSET, 0,
                   CAN_CTRL2_ISOCANFDEN);
     }
 
+  /* Exit supervisor mode */
 
-  /* Filtering catchall */
-
-  putreg32(0x3fffffff, priv->base + IMX9_CAN_RX14MASK_OFFSET);
-  putreg32(0x3fffffff, priv->base + IMX9_CAN_RX15MASK_OFFSET);
-  putreg32(0x3fffffff, priv->base + IMX9_CAN_RXMGMASK_OFFSET);
-  putreg32(0x0, priv->base + IMX9_CAN_RXFGMASK_OFFSET);
+  modifyreg32(priv->base + IMX9_CAN_MCR_OFFSET, CAN_MCR_SUPV, 0);
 
   for (i = 0; i < TOTALMBCOUNT; i++)
     {
       putreg32(0, priv->base + IMX9_CAN_RXIMR_OFFSET(i));
     }
 
+  /* Configure RX*MASKs */
+
+  putreg32(0x3fffffff, priv->base + IMX9_CAN_RX14MASK_OFFSET);
+  putreg32(0x3fffffff, priv->base + IMX9_CAN_RX15MASK_OFFSET);
+  putreg32(0x3fffffff, priv->base + IMX9_CAN_RXMGMASK_OFFSET);
+  putreg32(0x0, priv->base + IMX9_CAN_RXFGMASK_OFFSET);
+
+#if 0
   for (i = 0; i < RXMBCOUNT; i++)
     {
       struct mb_s *rx = flexcan_get_mb(priv, i);
       ninfo("Set MB%" PRIi32 " to receive %p\n", i, rx);
       rx->cs = (CAN_RXMB_EMPTY << CAN_MB_CS_CODE_SHIFT) | CAN_MB_CS_IDE;
     }
+#endif
+
+  /* Clear and enable IFLAG1 interrupts (MB 0-20) */
 
   putreg32(IFLAG1_RX, priv->base + IMX9_CAN_IFLAG1_OFFSET);
   putreg32(IFLAG1_RX, priv->base + IMX9_CAN_IMASK1_OFFSET);
 
+  /* Disable IFLAG2 & 3 interrupts */
+
+  putreg32(0, priv->base + IMX9_CAN_IMASK2_OFFSET);
+  putreg32(0, priv->base + IMX9_CAN_IMASK3_OFFSET);
+
   /* Exit freeze mode */
 
   if (!imx9_setfreeze(priv->base, false))
@@ -1838,7 +1913,7 @@ int imx9_caninitialize(int intf)
 
   imx9_get_rootclock(CCM_CR_CAN1, &priv->clk_freq);
 
-  if (imx9_bitratetotimeseg(&priv->arbi_timing, 1, 0, priv->clk_freq) != OK)
+  if (imx9_bitratetotimeseg(priv, &priv->arbi_timing, false) != OK)
     {
       nerr("ERROR: Invalid CAN timings please try another sample point "
            "or refer to the reference manual\n");
@@ -1847,8 +1922,7 @@ int imx9_caninitialize(int intf)
 
   if (priv->canfd_capable)
     {
-      if (imx9_bitratetotimeseg(&priv->data_timing, 1, 1,
-                                priv->clk_freq) != OK)
+      if (imx9_bitratetotimeseg(priv, &priv->data_timing, true) != OK)
         {
           nerr("ERROR: Invalid CAN data phase timings please try another "
                "sample point or refer to the reference manual\n");