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_can_frame_header_equal(const struct canfd_frame * const cfd1,const struct canfd_frame * const cfd2,const bool is_canfd)11 static bool rkcanfd_can_frame_header_equal(const struct canfd_frame *const cfd1,
12 const struct canfd_frame *const cfd2,
13 const bool is_canfd)
14 {
15 const u8 mask_flags = CANFD_BRS | CANFD_ESI | CANFD_FDF;
16 canid_t mask = CAN_EFF_FLAG;
17
18 if (canfd_sanitize_len(cfd1->len) != canfd_sanitize_len(cfd2->len))
19 return false;
20
21 if (!is_canfd)
22 mask |= CAN_RTR_FLAG;
23
24 if (cfd1->can_id & CAN_EFF_FLAG)
25 mask |= CAN_EFF_MASK;
26 else
27 mask |= CAN_SFF_MASK;
28
29 if ((cfd1->can_id & mask) != (cfd2->can_id & mask))
30 return false;
31
32 if (is_canfd &&
33 (cfd1->flags & mask_flags) != (cfd2->flags & mask_flags))
34 return false;
35
36 return true;
37 }
38
rkcanfd_can_frame_data_equal(const struct canfd_frame * cfd1,const struct canfd_frame * cfd2,const bool is_canfd)39 static bool rkcanfd_can_frame_data_equal(const struct canfd_frame *cfd1,
40 const struct canfd_frame *cfd2,
41 const bool is_canfd)
42 {
43 u8 len;
44
45 if (!is_canfd && (cfd1->can_id & CAN_RTR_FLAG))
46 return true;
47
48 len = canfd_sanitize_len(cfd1->len);
49
50 return !memcmp(cfd1->data, cfd2->data, len);
51 }
52
53 static unsigned int
rkcanfd_fifo_header_to_cfd_header(const struct rkcanfd_priv * priv,const struct rkcanfd_fifo_header * header,struct canfd_frame * cfd)54 rkcanfd_fifo_header_to_cfd_header(const struct rkcanfd_priv *priv,
55 const struct rkcanfd_fifo_header *header,
56 struct canfd_frame *cfd)
57 {
58 unsigned int len = sizeof(*cfd) - sizeof(cfd->data);
59 u8 dlc;
60
61 if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FRAME_FORMAT)
62 cfd->can_id = FIELD_GET(RKCANFD_REG_FD_ID_EFF, header->id) |
63 CAN_EFF_FLAG;
64 else
65 cfd->can_id = FIELD_GET(RKCANFD_REG_FD_ID_SFF, header->id);
66
67 dlc = FIELD_GET(RKCANFD_REG_FD_FRAMEINFO_DATA_LENGTH,
68 header->frameinfo);
69
70 /* CAN-FD */
71 if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF) {
72 cfd->len = can_fd_dlc2len(dlc);
73
74 /* The cfd is not allocated by alloc_canfd_skb(), so
75 * set CANFD_FDF here.
76 */
77 cfd->flags |= CANFD_FDF;
78
79 if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_BRS)
80 cfd->flags |= CANFD_BRS;
81 } else {
82 cfd->len = can_cc_dlc2len(dlc);
83
84 if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_RTR) {
85 cfd->can_id |= CAN_RTR_FLAG;
86
87 return len;
88 }
89 }
90
91 return len + cfd->len;
92 }
93
rkcanfd_rxstx_filter(struct rkcanfd_priv * priv,const struct canfd_frame * cfd_rx,const u32 ts,bool * tx_done)94 static int rkcanfd_rxstx_filter(struct rkcanfd_priv *priv,
95 const struct canfd_frame *cfd_rx, const u32 ts,
96 bool *tx_done)
97 {
98 struct net_device_stats *stats = &priv->ndev->stats;
99 struct rkcanfd_stats *rkcanfd_stats = &priv->stats;
100 const struct canfd_frame *cfd_nominal;
101 const struct sk_buff *skb;
102 unsigned int tx_tail;
103
104 tx_tail = rkcanfd_get_tx_tail(priv);
105 skb = priv->can.echo_skb[tx_tail];
106 if (!skb) {
107 netdev_err(priv->ndev,
108 "%s: echo_skb[%u]=NULL tx_head=0x%08x tx_tail=0x%08x\n",
109 __func__, tx_tail,
110 priv->tx_head, priv->tx_tail);
111
112 return -ENOMSG;
113 }
114 cfd_nominal = (struct canfd_frame *)skb->data;
115
116 /* We RX'ed a frame identical to our pending TX frame. */
117 if (rkcanfd_can_frame_header_equal(cfd_rx, cfd_nominal,
118 cfd_rx->flags & CANFD_FDF) &&
119 rkcanfd_can_frame_data_equal(cfd_rx, cfd_nominal,
120 cfd_rx->flags & CANFD_FDF)) {
121 unsigned int frame_len;
122
123 rkcanfd_handle_tx_done_one(priv, ts, &frame_len);
124
125 WRITE_ONCE(priv->tx_tail, priv->tx_tail + 1);
126 netif_subqueue_completed_wake(priv->ndev, 0, 1, frame_len,
127 rkcanfd_get_effective_tx_free(priv),
128 RKCANFD_TX_START_THRESHOLD);
129
130 *tx_done = true;
131
132 return 0;
133 }
134
135 if (!(priv->devtype_data.quirks & RKCANFD_QUIRK_RK3568_ERRATUM_6))
136 return 0;
137
138 /* Erratum 6: Extended frames may be send as standard frames.
139 *
140 * Not affected if:
141 * - TX'ed a standard frame -or-
142 * - RX'ed an extended frame
143 */
144 if (!(cfd_nominal->can_id & CAN_EFF_FLAG) ||
145 (cfd_rx->can_id & CAN_EFF_FLAG))
146 return 0;
147
148 /* Not affected if:
149 * - standard part and RTR flag of the TX'ed frame
150 * is not equal the CAN-ID and RTR flag of the RX'ed frame.
151 */
152 if ((cfd_nominal->can_id & (CAN_RTR_FLAG | CAN_SFF_MASK)) !=
153 (cfd_rx->can_id & (CAN_RTR_FLAG | CAN_SFF_MASK)))
154 return 0;
155
156 /* Not affected if:
157 * - length is not the same
158 */
159 if (cfd_nominal->len != cfd_rx->len)
160 return 0;
161
162 /* Not affected if:
163 * - the data of non RTR frames is different
164 */
165 if (!(cfd_nominal->can_id & CAN_RTR_FLAG) &&
166 memcmp(cfd_nominal->data, cfd_rx->data, cfd_nominal->len))
167 return 0;
168
169 /* Affected by Erratum 6 */
170 u64_stats_update_begin(&rkcanfd_stats->syncp);
171 u64_stats_inc(&rkcanfd_stats->tx_extended_as_standard_errors);
172 u64_stats_update_end(&rkcanfd_stats->syncp);
173
174 /* Manual handling of CAN Bus Error counters. See
175 * rkcanfd_get_corrected_berr_counter() for detailed
176 * explanation.
177 */
178 if (priv->bec.txerr)
179 priv->bec.txerr--;
180
181 *tx_done = true;
182
183 stats->tx_packets++;
184 stats->tx_errors++;
185
186 rkcanfd_xmit_retry(priv);
187
188 return 0;
189 }
190
191 static inline bool
rkcanfd_fifo_header_empty(const struct rkcanfd_fifo_header * header)192 rkcanfd_fifo_header_empty(const struct rkcanfd_fifo_header *header)
193 {
194 /* Erratum 5: If the FIFO is empty, we read the same value for
195 * all elements.
196 */
197 return header->frameinfo == header->id &&
198 header->frameinfo == header->ts;
199 }
200
rkcanfd_handle_rx_int_one(struct rkcanfd_priv * priv)201 static int rkcanfd_handle_rx_int_one(struct rkcanfd_priv *priv)
202 {
203 struct net_device_stats *stats = &priv->ndev->stats;
204 struct canfd_frame cfd[1] = { }, *skb_cfd;
205 struct rkcanfd_fifo_header header[1] = { };
206 struct sk_buff *skb;
207 unsigned int len;
208 int err;
209
210 /* read header into separate struct and convert it later */
211 rkcanfd_read_rep(priv, RKCANFD_REG_RX_FIFO_RDATA,
212 header, sizeof(*header));
213 /* read data directly into cfd */
214 rkcanfd_read_rep(priv, RKCANFD_REG_RX_FIFO_RDATA,
215 cfd->data, sizeof(cfd->data));
216
217 /* Erratum 5: Counters for TXEFIFO and RXFIFO may be wrong */
218 if (rkcanfd_fifo_header_empty(header)) {
219 struct rkcanfd_stats *rkcanfd_stats = &priv->stats;
220
221 u64_stats_update_begin(&rkcanfd_stats->syncp);
222 u64_stats_inc(&rkcanfd_stats->rx_fifo_empty_errors);
223 u64_stats_update_end(&rkcanfd_stats->syncp);
224
225 return 0;
226 }
227
228 len = rkcanfd_fifo_header_to_cfd_header(priv, header, cfd);
229
230 /* Drop any received CAN-FD frames if CAN-FD mode is not
231 * requested.
232 */
233 if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF &&
234 !(priv->can.ctrlmode & CAN_CTRLMODE_FD)) {
235 stats->rx_dropped++;
236
237 return 0;
238 }
239
240 if (rkcanfd_get_tx_pending(priv)) {
241 bool tx_done = false;
242
243 err = rkcanfd_rxstx_filter(priv, cfd, header->ts, &tx_done);
244 if (err)
245 return err;
246 if (tx_done && !(priv->can.ctrlmode & CAN_CTRLMODE_LOOPBACK))
247 return 0;
248 }
249
250 /* Manual handling of CAN Bus Error counters. See
251 * rkcanfd_get_corrected_berr_counter() for detailed
252 * explanation.
253 */
254 if (priv->bec.rxerr)
255 priv->bec.rxerr = min(CAN_ERROR_PASSIVE_THRESHOLD,
256 priv->bec.rxerr) - 1;
257
258 if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF)
259 skb = alloc_canfd_skb(priv->ndev, &skb_cfd);
260 else
261 skb = alloc_can_skb(priv->ndev, (struct can_frame **)&skb_cfd);
262
263 if (!skb) {
264 stats->rx_dropped++;
265
266 return 0;
267 }
268
269 memcpy(skb_cfd, cfd, len);
270 rkcanfd_skb_set_timestamp(priv, skb, header->ts);
271
272 err = can_rx_offload_queue_timestamp(&priv->offload, skb, header->ts);
273 if (err)
274 stats->rx_fifo_errors++;
275
276 return 0;
277 }
278
279 static inline unsigned int
rkcanfd_rx_fifo_get_len(const struct rkcanfd_priv * priv)280 rkcanfd_rx_fifo_get_len(const struct rkcanfd_priv *priv)
281 {
282 const u32 reg = rkcanfd_read(priv, RKCANFD_REG_RX_FIFO_CTRL);
283
284 return FIELD_GET(RKCANFD_REG_RX_FIFO_CTRL_RX_FIFO_CNT, reg);
285 }
286
rkcanfd_handle_rx_int(struct rkcanfd_priv * priv)287 int rkcanfd_handle_rx_int(struct rkcanfd_priv *priv)
288 {
289 unsigned int len;
290 int err;
291
292 while ((len = rkcanfd_rx_fifo_get_len(priv))) {
293 err = rkcanfd_handle_rx_int_one(priv);
294 if (err)
295 return err;
296 }
297
298 return 0;
299 }
300