1 // SPDX-License-Identifier: GPL-2.0
2 //
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
rkcanfd_tx_tail_is_eff(const struct rkcanfd_priv * priv)11 static 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
rkcanfd_get_effective_tx_free(const struct rkcanfd_priv * priv)36 unsigned 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
rkcanfd_start_xmit_write_cmd(const struct rkcanfd_priv * priv,const u32 reg_cmd)45 static 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
rkcanfd_xmit_retry(struct rkcanfd_priv * priv)58 void 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
rkcanfd_start_xmit(struct sk_buff * skb,struct net_device * ndev)66 netdev_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
rkcanfd_handle_tx_done_one(struct rkcanfd_priv * priv,const u32 ts,unsigned int * frame_len_p)143 void 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. See
154 * rkcanfd_get_corrected_berr_counter() for detailed
155 * 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