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 
9 /* Local echo of CAN messages
10  *
11  * CAN network devices *should* support a local echo functionality
12  * (see Documentation/networking/can.rst). To test the handling of CAN
13  * interfaces that do not support the local echo both driver types are
14  * implemented. In the case that the driver does not support the echo
15  * the IFF_ECHO remains clear in dev->flags. This causes the PF_CAN core
16  * to perform the echo as a fallback solution.
17  */
can_flush_echo_skb(struct net_device * dev)18 void can_flush_echo_skb(struct net_device *dev)
19 {
20 	struct can_priv *priv = netdev_priv(dev);
21 	struct net_device_stats *stats = &dev->stats;
22 	int i;
23 
24 	for (i = 0; i < priv->echo_skb_max; i++) {
25 		if (priv->echo_skb[i]) {
26 			kfree_skb(priv->echo_skb[i]);
27 			priv->echo_skb[i] = NULL;
28 			stats->tx_dropped++;
29 			stats->tx_aborted_errors++;
30 		}
31 	}
32 }
33 
34 /* Put the skb on the stack to be looped backed locally lateron
35  *
36  * The function is typically called in the start_xmit function
37  * of the device driver. The driver must protect access to
38  * priv->echo_skb, if necessary.
39  */
can_put_echo_skb(struct sk_buff * skb,struct net_device * dev,unsigned int idx,unsigned int frame_len)40 int can_put_echo_skb(struct sk_buff *skb, struct net_device *dev,
41 		     unsigned int idx, unsigned int frame_len)
42 {
43 	struct can_priv *priv = netdev_priv(dev);
44 
45 	BUG_ON(idx >= priv->echo_skb_max);
46 
47 	/* check flag whether this packet has to be looped back */
48 	if (!(dev->flags & IFF_ECHO) ||
49 	    (skb->protocol != htons(ETH_P_CAN) &&
50 	     skb->protocol != htons(ETH_P_CANFD))) {
51 		kfree_skb(skb);
52 		return 0;
53 	}
54 
55 	if (!priv->echo_skb[idx]) {
56 		skb = can_create_echo_skb(skb);
57 		if (!skb)
58 			return -ENOMEM;
59 
60 		/* make settings for echo to reduce code in irq context */
61 		skb->ip_summed = CHECKSUM_UNNECESSARY;
62 		skb->dev = dev;
63 
64 		/* save frame_len to reuse it when transmission is completed */
65 		can_skb_prv(skb)->frame_len = frame_len;
66 
67 		skb_tx_timestamp(skb);
68 
69 		/* save this skb for tx interrupt echo handling */
70 		priv->echo_skb[idx] = skb;
71 	} else {
72 		/* locking problem with netif_stop_queue() ?? */
73 		netdev_err(dev, "%s: BUG! echo_skb %d is occupied!\n", __func__, idx);
74 		kfree_skb(skb);
75 		return -EBUSY;
76 	}
77 
78 	return 0;
79 }
80 EXPORT_SYMBOL_GPL(can_put_echo_skb);
81 
82 struct sk_buff *
__can_get_echo_skb(struct net_device * dev,unsigned int idx,u8 * len_ptr,unsigned int * frame_len_ptr)83 __can_get_echo_skb(struct net_device *dev, unsigned int idx, u8 *len_ptr,
84 		   unsigned int *frame_len_ptr)
85 {
86 	struct can_priv *priv = netdev_priv(dev);
87 
88 	if (idx >= priv->echo_skb_max) {
89 		netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
90 			   __func__, idx, priv->echo_skb_max);
91 		return NULL;
92 	}
93 
94 	if (priv->echo_skb[idx]) {
95 		/* Using "struct canfd_frame::len" for the frame
96 		 * length is supported on both CAN and CANFD frames.
97 		 */
98 		struct sk_buff *skb = priv->echo_skb[idx];
99 		struct can_skb_priv *can_skb_priv = can_skb_prv(skb);
100 		struct canfd_frame *cf = (struct canfd_frame *)skb->data;
101 
102 		/* get the real payload length for netdev statistics */
103 		if (cf->can_id & CAN_RTR_FLAG)
104 			*len_ptr = 0;
105 		else
106 			*len_ptr = cf->len;
107 
108 		if (frame_len_ptr)
109 			*frame_len_ptr = can_skb_priv->frame_len;
110 
111 		priv->echo_skb[idx] = NULL;
112 
113 		if (skb->pkt_type == PACKET_LOOPBACK) {
114 			skb->pkt_type = PACKET_BROADCAST;
115 		} else {
116 			dev_consume_skb_any(skb);
117 			return NULL;
118 		}
119 
120 		return skb;
121 	}
122 
123 	return NULL;
124 }
125 
126 /* Get the skb from the stack and loop it back locally
127  *
128  * The function is typically called when the TX done interrupt
129  * is handled in the device driver. The driver must protect
130  * access to priv->echo_skb, if necessary.
131  */
can_get_echo_skb(struct net_device * dev,unsigned int idx,unsigned int * frame_len_ptr)132 unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx,
133 			      unsigned int *frame_len_ptr)
134 {
135 	struct sk_buff *skb;
136 	u8 len;
137 
138 	skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr);
139 	if (!skb)
140 		return 0;
141 
142 	skb_get(skb);
143 	if (netif_rx(skb) == NET_RX_SUCCESS)
144 		dev_consume_skb_any(skb);
145 	else
146 		dev_kfree_skb_any(skb);
147 
148 	return len;
149 }
150 EXPORT_SYMBOL_GPL(can_get_echo_skb);
151 
152 /* Remove the skb from the stack and free it.
153  *
154  * The function is typically called when TX failed.
155  */
can_free_echo_skb(struct net_device * dev,unsigned int idx,unsigned int * frame_len_ptr)156 void can_free_echo_skb(struct net_device *dev, unsigned int idx,
157 		       unsigned int *frame_len_ptr)
158 {
159 	struct can_priv *priv = netdev_priv(dev);
160 
161 	if (idx >= priv->echo_skb_max) {
162 		netdev_err(dev, "%s: BUG! Trying to access can_priv::echo_skb out of bounds (%u/max %u)\n",
163 			   __func__, idx, priv->echo_skb_max);
164 		return;
165 	}
166 
167 	if (priv->echo_skb[idx]) {
168 		struct sk_buff *skb = priv->echo_skb[idx];
169 		struct can_skb_priv *can_skb_priv = can_skb_prv(skb);
170 
171 		if (frame_len_ptr)
172 			*frame_len_ptr = can_skb_priv->frame_len;
173 
174 		dev_kfree_skb_any(skb);
175 		priv->echo_skb[idx] = NULL;
176 	}
177 }
178 EXPORT_SYMBOL_GPL(can_free_echo_skb);
179 
alloc_can_skb(struct net_device * dev,struct can_frame ** cf)180 struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)
181 {
182 	struct sk_buff *skb;
183 
184 	skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
185 			       sizeof(struct can_frame));
186 	if (unlikely(!skb)) {
187 		*cf = NULL;
188 
189 		return NULL;
190 	}
191 
192 	skb->protocol = htons(ETH_P_CAN);
193 	skb->pkt_type = PACKET_BROADCAST;
194 	skb->ip_summed = CHECKSUM_UNNECESSARY;
195 
196 	skb_reset_mac_header(skb);
197 	skb_reset_network_header(skb);
198 	skb_reset_transport_header(skb);
199 
200 	can_skb_reserve(skb);
201 	can_skb_prv(skb)->ifindex = dev->ifindex;
202 	can_skb_prv(skb)->skbcnt = 0;
203 
204 	*cf = skb_put_zero(skb, sizeof(struct can_frame));
205 
206 	return skb;
207 }
208 EXPORT_SYMBOL_GPL(alloc_can_skb);
209 
alloc_canfd_skb(struct net_device * dev,struct canfd_frame ** cfd)210 struct sk_buff *alloc_canfd_skb(struct net_device *dev,
211 				struct canfd_frame **cfd)
212 {
213 	struct sk_buff *skb;
214 
215 	skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
216 			       sizeof(struct canfd_frame));
217 	if (unlikely(!skb)) {
218 		*cfd = NULL;
219 
220 		return NULL;
221 	}
222 
223 	skb->protocol = htons(ETH_P_CANFD);
224 	skb->pkt_type = PACKET_BROADCAST;
225 	skb->ip_summed = CHECKSUM_UNNECESSARY;
226 
227 	skb_reset_mac_header(skb);
228 	skb_reset_network_header(skb);
229 	skb_reset_transport_header(skb);
230 
231 	can_skb_reserve(skb);
232 	can_skb_prv(skb)->ifindex = dev->ifindex;
233 	can_skb_prv(skb)->skbcnt = 0;
234 
235 	*cfd = skb_put_zero(skb, sizeof(struct canfd_frame));
236 
237 	return skb;
238 }
239 EXPORT_SYMBOL_GPL(alloc_canfd_skb);
240 
alloc_can_err_skb(struct net_device * dev,struct can_frame ** cf)241 struct sk_buff *alloc_can_err_skb(struct net_device *dev, struct can_frame **cf)
242 {
243 	struct sk_buff *skb;
244 
245 	skb = alloc_can_skb(dev, cf);
246 	if (unlikely(!skb))
247 		return NULL;
248 
249 	(*cf)->can_id = CAN_ERR_FLAG;
250 	(*cf)->len = CAN_ERR_DLC;
251 
252 	return skb;
253 }
254 EXPORT_SYMBOL_GPL(alloc_can_err_skb);
255