rockchip_canfd-rx.c

  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
 11static 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
 39static 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
 53static unsigned int
 54rkcanfd_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
 94static 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
191static inline bool
192rkcanfd_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
201static 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
279static inline unsigned int
280rkcanfd_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
287int 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}