168 lines · c
1// SPDX-License-Identifier: GPL-2.02//3// Copyright (c) 2023, 2024 Pengutronix,4// Marc Kleine-Budde <kernel@pengutronix.de>5//6 7#include <net/netdev_queues.h>8 9#include "rockchip_canfd.h"10 11static bool rkcanfd_tx_tail_is_eff(const struct rkcanfd_priv *priv)12{13 const struct canfd_frame *cfd;14 const struct sk_buff *skb;15 unsigned int tx_tail;16 17 if (!rkcanfd_get_tx_pending(priv))18 return false;19 20 tx_tail = rkcanfd_get_tx_tail(priv);21 skb = priv->can.echo_skb[tx_tail];22 if (!skb) {23 netdev_err(priv->ndev,24 "%s: echo_skb[%u]=NULL tx_head=0x%08x tx_tail=0x%08x\n",25 __func__, tx_tail,26 priv->tx_head, priv->tx_tail);27 28 return false;29 }30 31 cfd = (struct canfd_frame *)skb->data;32 33 return cfd->can_id & CAN_EFF_FLAG;34}35 36unsigned int rkcanfd_get_effective_tx_free(const struct rkcanfd_priv *priv)37{38 if (priv->devtype_data.quirks & RKCANFD_QUIRK_RK3568_ERRATUM_6 &&39 rkcanfd_tx_tail_is_eff(priv))40 return 0;41 42 return rkcanfd_get_tx_free(priv);43}44 45static void rkcanfd_start_xmit_write_cmd(const struct rkcanfd_priv *priv,46 const u32 reg_cmd)47{48 if (priv->devtype_data.quirks & RKCANFD_QUIRK_RK3568_ERRATUM_12)49 rkcanfd_write(priv, RKCANFD_REG_MODE, priv->reg_mode_default |50 RKCANFD_REG_MODE_SPACE_RX_MODE);51 52 rkcanfd_write(priv, RKCANFD_REG_CMD, reg_cmd);53 54 if (priv->devtype_data.quirks & RKCANFD_QUIRK_RK3568_ERRATUM_12)55 rkcanfd_write(priv, RKCANFD_REG_MODE, priv->reg_mode_default);56}57 58void rkcanfd_xmit_retry(struct rkcanfd_priv *priv)59{60 const unsigned int tx_head = rkcanfd_get_tx_head(priv);61 const u32 reg_cmd = RKCANFD_REG_CMD_TX_REQ(tx_head);62 63 rkcanfd_start_xmit_write_cmd(priv, reg_cmd);64}65 66netdev_tx_t rkcanfd_start_xmit(struct sk_buff *skb, struct net_device *ndev)67{68 struct rkcanfd_priv *priv = netdev_priv(ndev);69 u32 reg_frameinfo, reg_id, reg_cmd;70 unsigned int tx_head, frame_len;71 const struct canfd_frame *cfd;72 int err;73 u8 i;74 75 if (can_dropped_invalid_skb(ndev, skb))76 return NETDEV_TX_OK;77 78 if (!netif_subqueue_maybe_stop(priv->ndev, 0,79 rkcanfd_get_effective_tx_free(priv),80 RKCANFD_TX_STOP_THRESHOLD,81 RKCANFD_TX_START_THRESHOLD)) {82 if (net_ratelimit())83 netdev_info(priv->ndev,84 "Stopping tx-queue (tx_head=0x%08x, tx_tail=0x%08x, tx_pending=%d)\n",85 priv->tx_head, priv->tx_tail,86 rkcanfd_get_tx_pending(priv));87 88 return NETDEV_TX_BUSY;89 }90 91 cfd = (struct canfd_frame *)skb->data;92 93 if (cfd->can_id & CAN_EFF_FLAG) {94 reg_frameinfo = RKCANFD_REG_FD_FRAMEINFO_FRAME_FORMAT;95 reg_id = FIELD_PREP(RKCANFD_REG_FD_ID_EFF, cfd->can_id);96 } else {97 reg_frameinfo = 0;98 reg_id = FIELD_PREP(RKCANFD_REG_FD_ID_SFF, cfd->can_id);99 }100 101 if (cfd->can_id & CAN_RTR_FLAG)102 reg_frameinfo |= RKCANFD_REG_FD_FRAMEINFO_RTR;103 104 if (can_is_canfd_skb(skb)) {105 reg_frameinfo |= RKCANFD_REG_FD_FRAMEINFO_FDF;106 107 if (cfd->flags & CANFD_BRS)108 reg_frameinfo |= RKCANFD_REG_FD_FRAMEINFO_BRS;109 110 reg_frameinfo |= FIELD_PREP(RKCANFD_REG_FD_FRAMEINFO_DATA_LENGTH,111 can_fd_len2dlc(cfd->len));112 } else {113 reg_frameinfo |= FIELD_PREP(RKCANFD_REG_FD_FRAMEINFO_DATA_LENGTH,114 cfd->len);115 }116 117 tx_head = rkcanfd_get_tx_head(priv);118 reg_cmd = RKCANFD_REG_CMD_TX_REQ(tx_head);119 120 rkcanfd_write(priv, RKCANFD_REG_FD_TXFRAMEINFO, reg_frameinfo);121 rkcanfd_write(priv, RKCANFD_REG_FD_TXID, reg_id);122 for (i = 0; i < cfd->len; i += 4)123 rkcanfd_write(priv, RKCANFD_REG_FD_TXDATA0 + i,124 *(u32 *)(cfd->data + i));125 126 frame_len = can_skb_get_frame_len(skb);127 err = can_put_echo_skb(skb, ndev, tx_head, frame_len);128 if (!err)129 netdev_sent_queue(priv->ndev, frame_len);130 131 WRITE_ONCE(priv->tx_head, priv->tx_head + 1);132 133 rkcanfd_start_xmit_write_cmd(priv, reg_cmd);134 135 netif_subqueue_maybe_stop(priv->ndev, 0,136 rkcanfd_get_effective_tx_free(priv),137 RKCANFD_TX_STOP_THRESHOLD,138 RKCANFD_TX_START_THRESHOLD);139 140 return NETDEV_TX_OK;141}142 143void rkcanfd_handle_tx_done_one(struct rkcanfd_priv *priv, const u32 ts,144 unsigned int *frame_len_p)145{146 struct net_device_stats *stats = &priv->ndev->stats;147 unsigned int tx_tail;148 struct sk_buff *skb;149 150 tx_tail = rkcanfd_get_tx_tail(priv);151 skb = priv->can.echo_skb[tx_tail];152 153 /* Manual handling of CAN Bus Error counters. See154 * rkcanfd_get_corrected_berr_counter() for detailed155 * explanation.156 */157 if (priv->bec.txerr)158 priv->bec.txerr--;159 160 if (skb)161 rkcanfd_skb_set_timestamp(priv, skb, ts);162 stats->tx_bytes +=163 can_rx_offload_get_echo_skb_queue_timestamp(&priv->offload,164 tx_tail, ts,165 frame_len_p);166 stats->tx_packets++;167}168