1// SPDX-License-Identifier: GPL-2.0-only
2/* Copyright (C) 2005 Marc Kleine-Budde, Pengutronix
3 * Copyright (C) 2006 Andrey Volkov, Varma Electronics
4 * Copyright (C) 2008-2009 Wolfgang Grandegger <wg@grandegger.com>
5 */
6
7#include <linux/can/dev.h>
8#include <linux/module.h>
9
10#define MOD_DESC "CAN device driver interface"
11
12MODULE_DESCRIPTION(MOD_DESC);
13MODULE_LICENSE("GPL v2");
14MODULE_AUTHOR("Wolfgang Grandegger <wg@grandegger.com>");
15
16/* Local echo of CAN messages
17 *
18 * CAN network devices *should* support a local echo functionality
19 * (see Documentation/networking/can.rst). To test the handling of CAN
20 * interfaces that do not support the local echo both driver types are
21 * implemented. In the case that the driver does not support the echo
22 * the IFF_ECHO remains clear in dev->flags. This causes the PF_CAN core
23 * to perform the echo as a fallback solution.
24 */
25void can_flush_echo_skb(struct net_device *dev)
26{
27	struct can_priv *priv = netdev_priv(dev);
28	struct net_device_stats *stats = &dev->stats;
29	int i;
30
31	for (i = 0; i < priv->echo_skb_max; i++) {
32		if (priv->echo_skb[i]) {
33			kfree_skb(priv->echo_skb[i]);
34			priv->echo_skb[i] = NULL;
35			stats->tx_dropped++;
36			stats->tx_aborted_errors++;
37		}
38	}
39}
40
41/* Put the skb on the stack to be looped backed locally lateron
42 *
43 * The function is typically called in the start_xmit function
44 * of the device driver. The driver must protect access to
45 * priv->echo_skb, if necessary.
46 */
47int can_put_echo_skb(struct sk_buff *skb, struct net_device *dev,
48		     unsigned int idx, unsigned int frame_len)
49{
50	struct can_priv *priv = netdev_priv(dev);
51
52	if (idx >= priv->echo_skb_max) {
53		netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
54			   __func__, idx, priv->echo_skb_max);
55		return -EINVAL;
56	}
57
58	/* check flag whether this packet has to be looped back */
59	if (!(dev->flags & IFF_ECHO) ||
60	    (skb->protocol != htons(ETH_P_CAN) &&
61	     skb->protocol != htons(ETH_P_CANFD) &&
62	     skb->protocol != htons(ETH_P_CANXL))) {
63		kfree_skb(skb);
64		return 0;
65	}
66
67	if (!priv->echo_skb[idx]) {
68		skb = can_create_echo_skb(skb);
69		if (!skb)
70			return -ENOMEM;
71
72		/* make settings for echo to reduce code in irq context */
73		skb->ip_summed = CHECKSUM_UNNECESSARY;
74		skb->dev = dev;
75
76		/* save frame_len to reuse it when transmission is completed */
77		can_skb_prv(skb)->frame_len = frame_len;
78
79		if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)
80			skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
81
82		skb_tx_timestamp(skb);
83
84		/* save this skb for tx interrupt echo handling */
85		priv->echo_skb[idx] = skb;
86	} else {
87		/* locking problem with netif_stop_queue() ?? */
88		netdev_err(dev, "%s: BUG! echo_skb %d is occupied!\n", __func__, idx);
89		kfree_skb(skb);
90		return -EBUSY;
91	}
92
93	return 0;
94}
95EXPORT_SYMBOL_GPL(can_put_echo_skb);
96
97struct sk_buff *
98__can_get_echo_skb(struct net_device *dev, unsigned int idx,
99		   unsigned int *len_ptr, unsigned int *frame_len_ptr)
100{
101	struct can_priv *priv = netdev_priv(dev);
102
103	if (idx >= priv->echo_skb_max) {
104		netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
105			   __func__, idx, priv->echo_skb_max);
106		return NULL;
107	}
108
109	if (priv->echo_skb[idx]) {
110		/* Using "struct canfd_frame::len" for the frame
111		 * length is supported on both CAN and CANFD frames.
112		 */
113		struct sk_buff *skb = priv->echo_skb[idx];
114		struct can_skb_priv *can_skb_priv = can_skb_prv(skb);
115
116		if (skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)
117			skb_tstamp_tx(skb, skb_hwtstamps(skb));
118
119		/* get the real payload length for netdev statistics */
120		*len_ptr = can_skb_get_data_len(skb);
121
122		if (frame_len_ptr)
123			*frame_len_ptr = can_skb_priv->frame_len;
124
125		priv->echo_skb[idx] = NULL;
126
127		if (skb->pkt_type == PACKET_LOOPBACK) {
128			skb->pkt_type = PACKET_BROADCAST;
129		} else {
130			dev_consume_skb_any(skb);
131			return NULL;
132		}
133
134		return skb;
135	}
136
137	return NULL;
138}
139
140/* Get the skb from the stack and loop it back locally
141 *
142 * The function is typically called when the TX done interrupt
143 * is handled in the device driver. The driver must protect
144 * access to priv->echo_skb, if necessary.
145 */
146unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx,
147			      unsigned int *frame_len_ptr)
148{
149	struct sk_buff *skb;
150	unsigned int len;
151
152	skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr);
153	if (!skb)
154		return 0;
155
156	skb_get(skb);
157	if (netif_rx(skb) == NET_RX_SUCCESS)
158		dev_consume_skb_any(skb);
159	else
160		dev_kfree_skb_any(skb);
161
162	return len;
163}
164EXPORT_SYMBOL_GPL(can_get_echo_skb);
165
166/* Remove the skb from the stack and free it.
167 *
168 * The function is typically called when TX failed.
169 */
170void can_free_echo_skb(struct net_device *dev, unsigned int idx,
171		       unsigned int *frame_len_ptr)
172{
173	struct can_priv *priv = netdev_priv(dev);
174
175	if (idx >= priv->echo_skb_max) {
176		netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
177			   __func__, idx, priv->echo_skb_max);
178		return;
179	}
180
181	if (priv->echo_skb[idx]) {
182		struct sk_buff *skb = priv->echo_skb[idx];
183		struct can_skb_priv *can_skb_priv = can_skb_prv(skb);
184
185		if (frame_len_ptr)
186			*frame_len_ptr = can_skb_priv->frame_len;
187
188		dev_kfree_skb_any(skb);
189		priv->echo_skb[idx] = NULL;
190	}
191}
192EXPORT_SYMBOL_GPL(can_free_echo_skb);
193
194/* fill common values for CAN sk_buffs */
195static void init_can_skb_reserve(struct sk_buff *skb)
196{
197	skb->pkt_type = PACKET_BROADCAST;
198	skb->ip_summed = CHECKSUM_UNNECESSARY;
199
200	skb_reset_mac_header(skb);
201	skb_reset_network_header(skb);
202	skb_reset_transport_header(skb);
203
204	can_skb_reserve(skb);
205	can_skb_prv(skb)->skbcnt = 0;
206}
207
208struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)
209{
210	struct sk_buff *skb;
211
212	skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
213			       sizeof(struct can_frame));
214	if (unlikely(!skb)) {
215		*cf = NULL;
216
217		return NULL;
218	}
219
220	skb->protocol = htons(ETH_P_CAN);
221	init_can_skb_reserve(skb);
222	can_skb_prv(skb)->ifindex = dev->ifindex;
223
224	*cf = skb_put_zero(skb, sizeof(struct can_frame));
225
226	return skb;
227}
228EXPORT_SYMBOL_GPL(alloc_can_skb);
229
230struct sk_buff *alloc_canfd_skb(struct net_device *dev,
231				struct canfd_frame **cfd)
232{
233	struct sk_buff *skb;
234
235	skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
236			       sizeof(struct canfd_frame));
237	if (unlikely(!skb)) {
238		*cfd = NULL;
239
240		return NULL;
241	}
242
243	skb->protocol = htons(ETH_P_CANFD);
244	init_can_skb_reserve(skb);
245	can_skb_prv(skb)->ifindex = dev->ifindex;
246
247	*cfd = skb_put_zero(skb, sizeof(struct canfd_frame));
248
249	/* set CAN FD flag by default */
250	(*cfd)->flags = CANFD_FDF;
251
252	return skb;
253}
254EXPORT_SYMBOL_GPL(alloc_canfd_skb);
255
256struct sk_buff *alloc_canxl_skb(struct net_device *dev,
257				struct canxl_frame **cxl,
258				unsigned int data_len)
259{
260	struct sk_buff *skb;
261
262	if (data_len < CANXL_MIN_DLEN || data_len > CANXL_MAX_DLEN)
263		goto out_error;
264
265	skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
266			       CANXL_HDR_SIZE + data_len);
267	if (unlikely(!skb))
268		goto out_error;
269
270	skb->protocol = htons(ETH_P_CANXL);
271	init_can_skb_reserve(skb);
272	can_skb_prv(skb)->ifindex = dev->ifindex;
273
274	*cxl = skb_put_zero(skb, CANXL_HDR_SIZE + data_len);
275
276	/* set CAN XL flag and length information by default */
277	(*cxl)->flags = CANXL_XLF;
278	(*cxl)->len = data_len;
279
280	return skb;
281
282out_error:
283	*cxl = NULL;
284
285	return NULL;
286}
287EXPORT_SYMBOL_GPL(alloc_canxl_skb);
288
289struct sk_buff *alloc_can_err_skb(struct net_device *dev, struct can_frame **cf)
290{
291	struct sk_buff *skb;
292
293	skb = alloc_can_skb(dev, cf);
294	if (unlikely(!skb))
295		return NULL;
296
297	(*cf)->can_id = CAN_ERR_FLAG;
298	(*cf)->len = CAN_ERR_DLC;
299
300	return skb;
301}
302EXPORT_SYMBOL_GPL(alloc_can_err_skb);
303
304/* Check for outgoing skbs that have not been created by the CAN subsystem */
305static bool can_skb_headroom_valid(struct net_device *dev, struct sk_buff *skb)
306{
307	/* af_packet creates a headroom of HH_DATA_MOD bytes which is fine */
308	if (WARN_ON_ONCE(skb_headroom(skb) < sizeof(struct can_skb_priv)))
309		return false;
310
311	/* af_packet does not apply CAN skb specific settings */
312	if (skb->ip_summed == CHECKSUM_NONE) {
313		/* init headroom */
314		can_skb_prv(skb)->ifindex = dev->ifindex;
315		can_skb_prv(skb)->skbcnt = 0;
316
317		skb->ip_summed = CHECKSUM_UNNECESSARY;
318
319		/* perform proper loopback on capable devices */
320		if (dev->flags & IFF_ECHO)
321			skb->pkt_type = PACKET_LOOPBACK;
322		else
323			skb->pkt_type = PACKET_HOST;
324
325		skb_reset_mac_header(skb);
326		skb_reset_network_header(skb);
327		skb_reset_transport_header(skb);
328
329		/* set CANFD_FDF flag for CAN FD frames */
330		if (can_is_canfd_skb(skb)) {
331			struct canfd_frame *cfd;
332
333			cfd = (struct canfd_frame *)skb->data;
334			cfd->flags |= CANFD_FDF;
335		}
336	}
337
338	return true;
339}
340
341/* Drop a given socketbuffer if it does not contain a valid CAN frame. */
342bool can_dropped_invalid_skb(struct net_device *dev, struct sk_buff *skb)
343{
344	switch (ntohs(skb->protocol)) {
345	case ETH_P_CAN:
346		if (!can_is_can_skb(skb))
347			goto inval_skb;
348		break;
349
350	case ETH_P_CANFD:
351		if (!can_is_canfd_skb(skb))
352			goto inval_skb;
353		break;
354
355	case ETH_P_CANXL:
356		if (!can_is_canxl_skb(skb))
357			goto inval_skb;
358		break;
359
360	default:
361		goto inval_skb;
362	}
363
364	if (!can_skb_headroom_valid(dev, skb))
365		goto inval_skb;
366
367	return false;
368
369inval_skb:
370	kfree_skb(skb);
371	dev->stats.tx_dropped++;
372	return true;
373}
374EXPORT_SYMBOL_GPL(can_dropped_invalid_skb);
375