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 | |
12 | MODULE_DESCRIPTION(MOD_DESC); |
13 | MODULE_LICENSE("GPL v2" ); |
14 | MODULE_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 | */ |
25 | void 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(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 | */ |
47 | int 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, format: "%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, format: "%s: BUG! echo_skb %d is occupied!\n" , __func__, idx); |
89 | kfree_skb(skb); |
90 | return -EBUSY; |
91 | } |
92 | |
93 | return 0; |
94 | } |
95 | EXPORT_SYMBOL_GPL(can_put_echo_skb); |
96 | |
97 | struct 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, format: "%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(orig_skb: skb, hwtstamps: 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 | */ |
146 | unsigned 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_ptr: &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 | } |
164 | EXPORT_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 | */ |
170 | void 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, format: "%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 | } |
192 | EXPORT_SYMBOL_GPL(can_free_echo_skb); |
193 | |
194 | /* fill common values for CAN sk_buffs */ |
195 | static 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 | |
208 | struct 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, length: 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, len: sizeof(struct can_frame)); |
225 | |
226 | return skb; |
227 | } |
228 | EXPORT_SYMBOL_GPL(alloc_can_skb); |
229 | |
230 | struct 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, length: 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, len: sizeof(struct canfd_frame)); |
248 | |
249 | /* set CAN FD flag by default */ |
250 | (*cfd)->flags = CANFD_FDF; |
251 | |
252 | return skb; |
253 | } |
254 | EXPORT_SYMBOL_GPL(alloc_canfd_skb); |
255 | |
256 | struct 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, length: 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 | |
282 | out_error: |
283 | *cxl = NULL; |
284 | |
285 | return NULL; |
286 | } |
287 | EXPORT_SYMBOL_GPL(alloc_canxl_skb); |
288 | |
289 | struct 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 | } |
302 | EXPORT_SYMBOL_GPL(alloc_can_err_skb); |
303 | |
304 | /* Check for outgoing skbs that have not been created by the CAN subsystem */ |
305 | static 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. */ |
342 | bool 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 | |
369 | inval_skb: |
370 | kfree_skb(skb); |
371 | dev->stats.tx_dropped++; |
372 | return true; |
373 | } |
374 | EXPORT_SYMBOL_GPL(can_dropped_invalid_skb); |
375 | |