diff options
Diffstat (limited to 'drivers/net/can/rockchip/rockchip_canfd-rx.c')
| -rw-r--r-- | drivers/net/can/rockchip/rockchip_canfd-rx.c | 96 |
1 files changed, 96 insertions, 0 deletions
diff --git a/drivers/net/can/rockchip/rockchip_canfd-rx.c b/drivers/net/can/rockchip/rockchip_canfd-rx.c index 609282359bca..650dfd41e0a0 100644 --- a/drivers/net/can/rockchip/rockchip_canfd-rx.c +++ b/drivers/net/can/rockchip/rockchip_canfd-rx.c @@ -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 |