can: add TX cancel mechanism to avoid priority inversion

Introduce a priority-based CAN TX cancel mechanism to
reorder pending frames and prevent priority inversion
during message transmission.

Signed-off-by: zhaohaiyang1 <zhaohaiyang1@xiaomi.com>

Author: OceanfromXiaomi

drivers/can/Kconfig

@@ -134,6 +134,22 @@ config CAN_TXPRIORITY
 	---help---
 		Prioritize sending based on canid.
 
+config CAN_TXCANCEL
+	bool "Implement tx cancel ability"
+	default n
+	depends on CAN_TXPRIORITY
+	---help---
+		Do not use cancel ability if lower driver enable H/W FIFO
+		to store msg.
+
+		CAN_TXCANCEL relies on the hardware buffer has the following
+		capability, Cancels the transmission of frames in the hardware
+		buffer.
+
+		Enabling this feature adds support for the can cancel ability.
+		this ability can cancel the msg with the largest msgID in the
+		mailbox and return true if success.
+
 choice
 	prompt "TX Ready Work Queue"
 	default CAN_TXREADY_HIPRI

drivers/can/can.c

@@ -595,6 +595,48 @@ static int can_xmit(FAR struct can_dev_s *dev)
         }
     }
 
+  /* When the hardware transmit buffer, not H/W FIFO, is full and
+   * there are frames in the tx_pending list.
+   *
+   * the cancel logic requires hardware transmit buffer must have ability
+   * of Canceling the transmission of frames.
+   *
+   * The can_txneed_cancel function checks whether the ID of the first
+   * frame in the tx_pending list is smaller than the minimum ID in the
+   * tx_sending list.
+   *
+   * If this condition is met, the can_cancel_mbmsg function is invoked
+   * to attempt to cancel the transmission of the frame with the largest
+   * ID in the tx_sending list, and this frame with the largest ID in the
+   * tx_sending list is then reinserted into the tx_pending list at a
+   * specified position, can_cancel_mbmsg return true if cancel succeed.
+   *
+   * Afterwards, dev_send is called to load the first frame(minimum ID)
+   * from the tx_pending list into the hardware transmit buffer. make
+   * this frame with the minimum ID appear on the bus in real time.
+   */
+
+#ifdef CONFIG_CAN_TXCANCEL
+  if (TX_PENDING(&dev->cd_sender) && can_txneed_cancel(&dev->cd_sender))
+    {
+      DEBUGASSERT(dev->cd_ops->co_cancel != NULL);
+
+      if (can_cancel_mbmsg(dev))
+        {
+          msg = can_get_msg(&dev->cd_sender);
+
+          /* Send the next message at the sender */
+
+          ret = dev_send(dev, msg);
+          if (ret < 0)
+            {
+              canerr("dev_send failed: %d\n", ret);
+              can_revert_msg(&dev->cd_sender, msg);
+            }
+        }
+    }
+#endif
+
   /* Make sure that TX interrupts are enabled */
 
   dev_txint(dev, true);
@@ -627,13 +669,18 @@ static ssize_t can_write(FAR struct file *filep, FAR const char *buffer,
 
   flags = enter_critical_section();
 
-  /* Check if the H/W TX is inactive when we started. In certain race
+  /* if CONFIG_CAN_TXCANCEL is enable, inactive will be always true, else
+   * Check if the H/W TX is inactive when we started. In certain race
    * conditions, there may be a pending interrupt to kick things back off,
    * but we will be sure here that there is not.  That the hardware is IDLE
    * and will need to be kick-started.
    */
 
-  inactive = dev_txempty(dev);
+#ifdef CONFIG_CAN_TXCANCEL
+  inactive = true;
+#else
+  inactive = dev_txready(dev);
+#endif
 
   /* Add the messages to the sender.  Ignore any trailing messages that are
    * shorter than the minimum.
@@ -701,7 +748,11 @@ static ssize_t can_write(FAR struct file *filep, FAR const char *buffer,
 
           /* Re-check the H/W sender state */
 
-          inactive = dev_txempty(dev);
+#ifdef CONFIG_CAN_TXCANCEL
+          inactive = true;
+#else
+          inactive = dev_txready(dev);
+#endif
         }
 
       /* We get here if there is space in sender.  Add the new

drivers/can/can_sender.c

@@ -245,3 +245,87 @@ void can_send_done(FAR struct can_txcache_s *cd_sender)
     }
 #endif
 }
+
+/****************************************************************************
+ * Name: can_txneed_cancel
+ *
+ * Description:
+ *   Compare the msgID between tx_sending and tx_pending's head when
+ *   dev_txready return false and tx_pending is not empty, preserve the node
+ *   with largest msgID in tx_sending into *callbackmsg_node. return true if
+ *   the msgID in tx_pending's head < the smallest msgID in tx_sending.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_CAN_TXCANCEL
+bool can_txneed_cancel(FAR struct can_txcache_s *cd_sender)
+{
+  FAR struct can_msg_node_s *msg_node;
+  FAR struct can_msg_node_s *tmp_node;
+
+  /* acquire min msgID from tx_sending and compare it with masgID
+   * in tx_pending list's head.
+   */
+
+  if (SENDING_COUNT(cd_sender) == 0)
+    {
+      return false;
+    }
+
+  msg_node = list_first_entry(&cd_sender->tx_pending,
+                              struct can_msg_node_s, list);
+  tmp_node = list_first_entry(&cd_sender->tx_sending,
+                              struct can_msg_node_s, list);
+  if (msg_node->msg.cm_hdr.ch_id < tmp_node->msg.cm_hdr.ch_id)
+    {
+      return true;
+    }
+
+  return false;
+}
+#endif
+
+/****************************************************************************
+ * Name: can_cancel_mbmsg
+ *
+ * Description:
+ *   cancel the msg with the largest msgID in the mailbox and
+ *   return true if success.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_CAN_TXCANCEL
+bool can_cancel_mbmsg(FAR struct can_dev_s *dev)
+{
+  FAR struct can_msg_node_s *tmp_node;
+  FAR struct can_msg_node_s *callbackmsg_node;
+  FAR struct can_txcache_s *cd_sender = &dev->cd_sender;
+
+  callbackmsg_node = list_last_entry(&cd_sender->tx_sending,
+                                     struct can_msg_node_s, list);
+  if (dev_cancel(dev, &callbackmsg_node->msg))
+    {
+      /* take tx_sending's specific msg back into tx_pending at a
+       * specified position.
+       */
+
+      list_delete(&callbackmsg_node->list);
+
+      list_for_every_entry(&cd_sender->tx_pending, tmp_node,
+                           struct can_msg_node_s, list)
+        {
+          if (tmp_node->msg.cm_hdr.ch_id >=
+              callbackmsg_node->msg.cm_hdr.ch_id)
+            {
+              break;
+            }
+        }
+
+      list_add_before(&tmp_node->list, &callbackmsg_node->list);
+
+      return true;
+    }
+
+  return false;
+}
+#endif

include/nuttx/can/can.h

@@ -396,6 +396,7 @@
 #define dev_send(dev,m)           (dev)->cd_ops->co_send(dev,m)
 #define dev_txready(dev)          (dev)->cd_ops->co_txready(dev)
 #define dev_txempty(dev)          (dev)->cd_ops->co_txempty(dev)
+#define dev_cancel(dev,m)         (dev)->cd_ops->co_cancel(dev,m)
 
 /* CAN message support ******************************************************/
 
@@ -815,6 +816,9 @@ struct can_ops_s
    */
 
   CODE bool (*co_txempty)(FAR struct can_dev_s *dev);
+
+  CODE bool (*co_cancel)(FAR struct can_dev_s *dev,
+                         FAR struct can_msg_s *msg);
 };
 
 /* This is the device structure used by the driver.  The caller of

include/nuttx/can/can_sender.h

@@ -235,5 +235,33 @@ void can_revert_msg(FAR struct can_txcache_s *cd_sender,
 
 void can_send_done(FAR struct can_txcache_s *cd_sender);
 
+/****************************************************************************
+ * Name: can_txneed_cancel
+ *
+ * Description:
+ *   Compare the msgID between tx_sending and tx_pending's head when
+ *   dev_txready return false and tx_pending is not empty, preserve the node
+ *   with largest msgID in tx_sending into *callbackmsg_node. return true if
+ *   the msgID in tx_pending's head < the smallest msgID in tx_sending.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_CAN_TXCANCEL
+bool can_txneed_cancel(FAR struct can_txcache_s *cd_sender);
+#endif
+
+/****************************************************************************
+ * Name: can_cancel_mbmsg
+ *
+ * Description:
+ *   cancel the msg with the largest msgID in the mailbox and
+ *   return true if success.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_CAN_TXCANCEL
+bool can_cancel_mbmsg(FAR struct can_dev_s *dev);
+#endif
+
 #endif /* CONFIG_CAN */
 #endif /* __INCLUDE_NUTTX_CAN_SENDER_H */