@@ -229,6 +229,7 @@ static void rkcanfd_chip_start(struct rkcanfd_priv *priv)
* - CAN_FD: enable CAN-FD
* - AUTO_RETX_MODE: auto retransmission on TX error
* - COVER_MODE: RX-FIFO overwrite mode, do not send OVERLOAD frames
+ * - RXSTX_MODE: Receive Self Transmit data mode
* - WORK_MODE: transition from reset to working mode
*/
reg = rkcanfd_read(priv, RKCANFD_REG_MODE);
@@ -236,17 +237,20 @@ static void rkcanfd_chip_start(struct rkcanfd_priv *priv)
RKCANFD_REG_MODE_CAN_FD_MODE_ENABLE |
RKCANFD_REG_MODE_AUTO_RETX_MODE |
RKCANFD_REG_MODE_COVER_MODE |
+ RKCANFD_REG_MODE_RXSTX_MODE |
RKCANFD_REG_MODE_WORK_MODE;
/* mask, i.e. ignore:
* - TIMESTAMP_COUNTER_OVERFLOW_INT - timestamp counter overflow interrupt
* - TX_ARBIT_FAIL_INT - TX arbitration fail interrupt
* - OVERLOAD_INT - CAN bus overload interrupt
+ * - TX_FINISH_INT - Transmit finish interrupt
*/
priv->reg_int_mask_default =
RKCANFD_REG_INT_TIMESTAMP_COUNTER_OVERFLOW_INT |
RKCANFD_REG_INT_TX_ARBIT_FAIL_INT |
- RKCANFD_REG_INT_OVERLOAD_INT;
+ RKCANFD_REG_INT_OVERLOAD_INT |
+ RKCANFD_REG_INT_TX_FINISH_INT;
rkcanfd_chip_fifo_setup(priv);
rkcanfd_timestamp_init(priv);
@@ -4,8 +4,52 @@
// Marc Kleine-Budde <kernel@pengutronix.de>
//
+#include <net/netdev_queues.h>
+
#include "rockchip_canfd.h"
+static bool rkcanfd_can_frame_header_equal(const struct canfd_frame *const cfd1,
+ const struct canfd_frame *const cfd2,
+ const bool is_canfd)
+{
+ const u8 mask_flags = CANFD_BRS | CANFD_ESI | CANFD_FDF;
+ canid_t mask = CAN_EFF_FLAG;
+
+ if (canfd_sanitize_len(cfd1->len) != canfd_sanitize_len(cfd2->len))
+ return false;
+
+ if (!is_canfd)
+ mask |= CAN_RTR_FLAG;
+
+ if (cfd1->can_id & CAN_EFF_FLAG)
+ mask |= CAN_EFF_MASK;
+ else
+ mask |= CAN_SFF_MASK;
+
+ if ((cfd1->can_id & mask) != (cfd2->can_id & mask))
+ return false;
+
+ if (is_canfd &&
+ (cfd1->flags & mask_flags) != (cfd2->flags & mask_flags))
+ return false;
+
+ return true;
+}
+
+static bool rkcanfd_can_frame_data_equal(const struct canfd_frame *cfd1,
+ const struct canfd_frame *cfd2,
+ const bool is_canfd)
+{
+ u8 len;
+
+ if (!is_canfd && (cfd1->can_id & CAN_RTR_FLAG))
+ return true;
+
+ len = canfd_sanitize_len(cfd1->len);
+
+ return !memcmp(cfd1->data, cfd2->data, len);
+}
+
static unsigned int
rkcanfd_fifo_header_to_cfd_header(const struct rkcanfd_priv *priv,
const struct rkcanfd_fifo_header *header,
@@ -47,6 +91,48 @@ rkcanfd_fifo_header_to_cfd_header(const struct rkcanfd_priv *priv,
return len + cfd->len;
}
+static int rkcanfd_rxstx_filter(struct rkcanfd_priv *priv,
+ const struct canfd_frame *cfd_rx, const u32 ts,
+ bool *tx_done)
+{
+ const struct canfd_frame *cfd_nominal;
+ const struct sk_buff *skb;
+ unsigned int tx_tail;
+
+ tx_tail = rkcanfd_get_tx_tail(priv);
+ skb = priv->can.echo_skb[tx_tail];
+ if (!skb) {
+ netdev_err(priv->ndev,
+ "%s: echo_skb[%u]=NULL tx_head=0x%08x tx_tail=0x%08x\n",
+ __func__, tx_tail,
+ priv->tx_head, priv->tx_tail);
+
+ return -ENOMSG;
+ }
+ cfd_nominal = (struct canfd_frame *)skb->data;
+
+ /* We RX'ed a frame identical to our pending TX frame. */
+ if (rkcanfd_can_frame_header_equal(cfd_rx, cfd_nominal,
+ cfd_rx->flags & CANFD_FDF) &&
+ rkcanfd_can_frame_data_equal(cfd_rx, cfd_nominal,
+ cfd_rx->flags & CANFD_FDF)) {
+ unsigned int frame_len;
+
+ rkcanfd_handle_tx_done_one(priv, ts, &frame_len);
+
+ WRITE_ONCE(priv->tx_tail, priv->tx_tail + 1);
+ netif_subqueue_completed_wake(priv->ndev, 0, 1, frame_len,
+ rkcanfd_get_tx_free(priv),
+ RKCANFD_TX_START_THRESHOLD);
+
+ *tx_done = true;
+
+ return 0;
+ }
+
+ return 0;
+}
+
static inline bool
rkcanfd_fifo_header_empty(const struct rkcanfd_fifo_header *header)
{
@@ -89,6 +175,16 @@ static int rkcanfd_handle_rx_int_one(struct rkcanfd_priv *priv)
return 0;
}
+ if (rkcanfd_get_tx_pending(priv)) {
+ bool tx_done = false;
+
+ err = rkcanfd_rxstx_filter(priv, cfd, header->ts, &tx_done);
+ if (err)
+ return err;
+ if (tx_done)
+ return 0;
+ }
+
if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF)
skb = alloc_canfd_skb(priv->ndev, &skb_cfd);
else
@@ -4,9 +4,103 @@
// Marc Kleine-Budde <kernel@pengutronix.de>
//
+#include <net/netdev_queues.h>
+
#include "rockchip_canfd.h"
+static void rkcanfd_start_xmit_write_cmd(const struct rkcanfd_priv *priv,
+ const u32 reg_cmd)
+{
+ rkcanfd_write(priv, RKCANFD_REG_CMD, reg_cmd);
+}
+
int rkcanfd_start_xmit(struct sk_buff *skb, struct net_device *ndev)
{
+ struct rkcanfd_priv *priv = netdev_priv(ndev);
+ u32 reg_frameinfo, reg_id, reg_cmd;
+ unsigned int tx_head, frame_len;
+ const struct canfd_frame *cfd;
+ int err;
+ u8 i;
+
+ if (can_dropped_invalid_skb(ndev, skb))
+ return NETDEV_TX_OK;
+
+ if (!netif_subqueue_maybe_stop(priv->ndev, 0,
+ rkcanfd_get_tx_free(priv),
+ RKCANFD_TX_STOP_THRESHOLD,
+ RKCANFD_TX_START_THRESHOLD)) {
+ if (net_ratelimit())
+ netdev_info(priv->ndev,
+ "Stopping tx-queue (tx_head=0x%08x, tx_tail=0x%08x, tx_pending=%d)\n",
+ priv->tx_head, priv->tx_tail,
+ rkcanfd_get_tx_pending(priv));
+
+ return NETDEV_TX_BUSY;
+ }
+
+ cfd = (struct canfd_frame *)skb->data;
+
+ if (cfd->can_id & CAN_EFF_FLAG) {
+ reg_frameinfo = RKCANFD_REG_FD_FRAMEINFO_FRAME_FORMAT;
+ reg_id = FIELD_PREP(RKCANFD_REG_FD_ID_EFF, cfd->can_id);
+ } else {
+ reg_frameinfo = 0;
+ reg_id = FIELD_PREP(RKCANFD_REG_FD_ID_SFF, cfd->can_id);
+ }
+
+ if (cfd->can_id & CAN_RTR_FLAG)
+ reg_frameinfo |= RKCANFD_REG_FD_FRAMEINFO_RTR;
+
+ if (can_is_canfd_skb(skb)) {
+ reg_frameinfo |= RKCANFD_REG_FD_FRAMEINFO_FDF;
+
+ if (cfd->flags & CANFD_BRS)
+ reg_frameinfo |= RKCANFD_REG_FD_FRAMEINFO_BRS;
+
+ reg_frameinfo |= FIELD_PREP(RKCANFD_REG_FD_FRAMEINFO_DATA_LENGTH,
+ can_fd_len2dlc(cfd->len));
+ } else {
+ reg_frameinfo |= FIELD_PREP(RKCANFD_REG_FD_FRAMEINFO_DATA_LENGTH,
+ cfd->len);
+ }
+
+ tx_head = rkcanfd_get_tx_head(priv);
+ reg_cmd = RKCANFD_REG_CMD_TX_REQ(tx_head);
+
+ rkcanfd_write(priv, RKCANFD_REG_FD_TXFRAMEINFO, reg_frameinfo);
+ rkcanfd_write(priv, RKCANFD_REG_FD_TXID, reg_id);
+ for (i = 0; i < cfd->len; i += 4)
+ rkcanfd_write(priv, RKCANFD_REG_FD_TXDATA0 + i,
+ *(u32 *)(cfd->data + i));
+
+ frame_len = can_skb_get_frame_len(skb);
+ err = can_put_echo_skb(skb, ndev, tx_head, frame_len);
+ if (!err)
+ netdev_sent_queue(priv->ndev, frame_len);
+
+ WRITE_ONCE(priv->tx_head, priv->tx_head + 1);
+
+ rkcanfd_start_xmit_write_cmd(priv, reg_cmd);
+
+ netif_subqueue_maybe_stop(priv->ndev, 0,
+ rkcanfd_get_tx_free(priv),
+ RKCANFD_TX_STOP_THRESHOLD,
+ RKCANFD_TX_START_THRESHOLD);
+
return NETDEV_TX_OK;
}
+
+void rkcanfd_handle_tx_done_one(struct rkcanfd_priv *priv, const u32 ts,
+ unsigned int *frame_len_p)
+{
+ struct net_device_stats *stats = &priv->ndev->stats;
+ unsigned int tx_tail;
+
+ tx_tail = rkcanfd_get_tx_tail(priv);
+ stats->tx_bytes +=
+ can_rx_offload_get_echo_skb_queue_timestamp(&priv->offload,
+ tx_tail, ts,
+ frame_len_p);
+ stats->tx_packets++;
+}
@@ -471,10 +471,36 @@ rkcanfd_get_timestamp(const struct rkcanfd_priv *priv)
return rkcanfd_read(priv, RKCANFD_REG_TIMESTAMP);
}
+static inline unsigned int
+rkcanfd_get_tx_head(const struct rkcanfd_priv *priv)
+{
+ return READ_ONCE(priv->tx_head) & (RKCANFD_TXFIFO_DEPTH - 1);
+}
+
+static inline unsigned int
+rkcanfd_get_tx_tail(const struct rkcanfd_priv *priv)
+{
+ return READ_ONCE(priv->tx_tail) & (RKCANFD_TXFIFO_DEPTH - 1);
+}
+
+static inline unsigned int
+rkcanfd_get_tx_pending(const struct rkcanfd_priv *priv)
+{
+ return READ_ONCE(priv->tx_head) - READ_ONCE(priv->tx_tail);
+}
+
+static inline unsigned int
+rkcanfd_get_tx_free(const struct rkcanfd_priv *priv)
+{
+ return RKCANFD_TXFIFO_DEPTH - rkcanfd_get_tx_pending(priv);
+}
+
int rkcanfd_handle_rx_int(struct rkcanfd_priv *priv);
void rkcanfd_timestamp_init(struct rkcanfd_priv *priv);
int rkcanfd_start_xmit(struct sk_buff *skb, struct net_device *ndev);
+void rkcanfd_handle_tx_done_one(struct rkcanfd_priv *priv, const u32 ts,
+ unsigned int *frame_len_p);
#endif