Commit | Line | Data |
---|---|---|
e057dd3f OH |
1 | // SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause) |
2 | /* isotp.c - ISO 15765-2 CAN transport protocol for protocol family CAN | |
3 | * | |
4 | * This implementation does not provide ISO-TP specific return values to the | |
5 | * userspace. | |
6 | * | |
7 | * - RX path timeout of data reception leads to -ETIMEDOUT | |
8 | * - RX path SN mismatch leads to -EILSEQ | |
9 | * - RX path data reception with wrong padding leads to -EBADMSG | |
10 | * - TX path flowcontrol reception timeout leads to -ECOMM | |
11 | * - TX path flowcontrol reception overflow leads to -EMSGSIZE | |
12 | * - TX path flowcontrol reception with wrong layout/padding leads to -EBADMSG | |
13 | * - when a transfer (tx) is on the run the next write() blocks until it's done | |
14 | * - use CAN_ISOTP_WAIT_TX_DONE flag to block the caller until the PDU is sent | |
15 | * - as we have static buffers the check whether the PDU fits into the buffer | |
16 | * is done at FF reception time (no support for sending 'wait frames') | |
e057dd3f OH |
17 | * |
18 | * Copyright (c) 2020 Volkswagen Group Electronic Research | |
19 | * All rights reserved. | |
20 | * | |
21 | * Redistribution and use in source and binary forms, with or without | |
22 | * modification, are permitted provided that the following conditions | |
23 | * are met: | |
24 | * 1. Redistributions of source code must retain the above copyright | |
25 | * notice, this list of conditions and the following disclaimer. | |
26 | * 2. Redistributions in binary form must reproduce the above copyright | |
27 | * notice, this list of conditions and the following disclaimer in the | |
28 | * documentation and/or other materials provided with the distribution. | |
29 | * 3. Neither the name of Volkswagen nor the names of its contributors | |
30 | * may be used to endorse or promote products derived from this software | |
31 | * without specific prior written permission. | |
32 | * | |
33 | * Alternatively, provided that this notice is retained in full, this | |
34 | * software may be distributed under the terms of the GNU General | |
35 | * Public License ("GPL") version 2, in which case the provisions of the | |
36 | * GPL apply INSTEAD OF those given above. | |
37 | * | |
38 | * The provided data structures and external interfaces from this code | |
39 | * are not restricted to be used by modules with a GPL compatible license. | |
40 | * | |
41 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | |
42 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | |
43 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | |
44 | * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | |
45 | * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | |
46 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | |
47 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | |
48 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | |
49 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | |
50 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | |
51 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | |
52 | * DAMAGE. | |
53 | */ | |
54 | ||
55 | #include <linux/module.h> | |
56 | #include <linux/init.h> | |
57 | #include <linux/interrupt.h> | |
7c759040 | 58 | #include <linux/spinlock.h> |
e057dd3f OH |
59 | #include <linux/hrtimer.h> |
60 | #include <linux/wait.h> | |
61 | #include <linux/uio.h> | |
62 | #include <linux/net.h> | |
63 | #include <linux/netdevice.h> | |
64 | #include <linux/socket.h> | |
65 | #include <linux/if_arp.h> | |
66 | #include <linux/skbuff.h> | |
67 | #include <linux/can.h> | |
68 | #include <linux/can/core.h> | |
69 | #include <linux/can/skb.h> | |
70 | #include <linux/can/isotp.h> | |
71 | #include <linux/slab.h> | |
72 | #include <net/sock.h> | |
73 | #include <net/net_namespace.h> | |
74 | ||
e057dd3f OH |
75 | MODULE_DESCRIPTION("PF_CAN isotp 15765-2:2016 protocol"); |
76 | MODULE_LICENSE("Dual BSD/GPL"); | |
77 | MODULE_AUTHOR("Oliver Hartkopp <socketcan@hartkopp.net>"); | |
78 | MODULE_ALIAS("can-proto-6"); | |
79 | ||
f522d955 OH |
80 | #define ISOTP_MIN_NAMELEN CAN_REQUIRED_SIZE(struct sockaddr_can, can_addr.tp) |
81 | ||
e057dd3f OH |
82 | #define SINGLE_MASK(id) (((id) & CAN_EFF_FLAG) ? \ |
83 | (CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG) : \ | |
84 | (CAN_SFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG)) | |
85 | ||
86 | /* ISO 15765-2:2016 supports more than 4095 byte per ISO PDU as the FF_DL can | |
87 | * take full 32 bit values (4 Gbyte). We would need some good concept to handle | |
88 | * this between user space and kernel space. For now increase the static buffer | |
9c0c191d | 89 | * to something about 64 kbyte to be able to test this new functionality. |
e057dd3f | 90 | */ |
9c0c191d | 91 | #define MAX_MSG_LENGTH 66000 |
e057dd3f OH |
92 | |
93 | /* N_PCI type values in bits 7-4 of N_PCI bytes */ | |
94 | #define N_PCI_SF 0x00 /* single frame */ | |
95 | #define N_PCI_FF 0x10 /* first frame */ | |
96 | #define N_PCI_CF 0x20 /* consecutive frame */ | |
97 | #define N_PCI_FC 0x30 /* flow control */ | |
98 | ||
99 | #define N_PCI_SZ 1 /* size of the PCI byte #1 */ | |
100 | #define SF_PCI_SZ4 1 /* size of SingleFrame PCI including 4 bit SF_DL */ | |
101 | #define SF_PCI_SZ8 2 /* size of SingleFrame PCI including 8 bit SF_DL */ | |
102 | #define FF_PCI_SZ12 2 /* size of FirstFrame PCI including 12 bit FF_DL */ | |
103 | #define FF_PCI_SZ32 6 /* size of FirstFrame PCI including 32 bit FF_DL */ | |
104 | #define FC_CONTENT_SZ 3 /* flow control content size in byte (FS/BS/STmin) */ | |
105 | ||
106 | #define ISOTP_CHECK_PADDING (CAN_ISOTP_CHK_PAD_LEN | CAN_ISOTP_CHK_PAD_DATA) | |
9f39d365 | 107 | #define ISOTP_ALL_BC_FLAGS (CAN_ISOTP_SF_BROADCAST | CAN_ISOTP_CF_BROADCAST) |
e057dd3f OH |
108 | |
109 | /* Flow Status given in FC frame */ | |
110 | #define ISOTP_FC_CTS 0 /* clear to send */ | |
111 | #define ISOTP_FC_WT 1 /* wait */ | |
112 | #define ISOTP_FC_OVFLW 2 /* overflow */ | |
113 | ||
86633786 OH |
114 | #define ISOTP_FC_TIMEOUT 1 /* 1 sec */ |
115 | #define ISOTP_ECHO_TIMEOUT 2 /* 2 secs */ | |
116 | ||
e057dd3f OH |
117 | enum { |
118 | ISOTP_IDLE = 0, | |
119 | ISOTP_WAIT_FIRST_FC, | |
120 | ISOTP_WAIT_FC, | |
121 | ISOTP_WAIT_DATA, | |
122 | ISOTP_SENDING | |
123 | }; | |
124 | ||
125 | struct tpcon { | |
5f33a09e MKB |
126 | unsigned int idx; |
127 | unsigned int len; | |
43a08c3b | 128 | u32 state; |
e057dd3f OH |
129 | u8 bs; |
130 | u8 sn; | |
131 | u8 ll_dl; | |
132 | u8 buf[MAX_MSG_LENGTH + 1]; | |
133 | }; | |
134 | ||
135 | struct isotp_sock { | |
136 | struct sock sk; | |
137 | int bound; | |
138 | int ifindex; | |
139 | canid_t txid; | |
140 | canid_t rxid; | |
141 | ktime_t tx_gap; | |
142 | ktime_t lastrxcf_tstamp; | |
4f027cba | 143 | struct hrtimer rxtimer, txtimer, txfrtimer; |
e057dd3f OH |
144 | struct can_isotp_options opt; |
145 | struct can_isotp_fc_options rxfc, txfc; | |
146 | struct can_isotp_ll_options ll; | |
530e0d46 | 147 | u32 frame_txtime; |
e057dd3f OH |
148 | u32 force_tx_stmin; |
149 | u32 force_rx_stmin; | |
4b7fe92c | 150 | u32 cfecho; /* consecutive frame echo tag */ |
e057dd3f | 151 | struct tpcon rx, tx; |
8d0caedb | 152 | struct list_head notifier; |
e057dd3f | 153 | wait_queue_head_t wait; |
7c759040 | 154 | spinlock_t rx_lock; /* protect single thread state machine */ |
e057dd3f OH |
155 | }; |
156 | ||
8d0caedb TH |
157 | static LIST_HEAD(isotp_notifier_list); |
158 | static DEFINE_SPINLOCK(isotp_notifier_lock); | |
159 | static struct isotp_sock *isotp_busy_notifier; | |
160 | ||
e057dd3f OH |
161 | static inline struct isotp_sock *isotp_sk(const struct sock *sk) |
162 | { | |
163 | return (struct isotp_sock *)sk; | |
164 | } | |
165 | ||
9f39d365 OH |
166 | static u32 isotp_bc_flags(struct isotp_sock *so) |
167 | { | |
168 | return so->opt.flags & ISOTP_ALL_BC_FLAGS; | |
169 | } | |
170 | ||
171 | static bool isotp_register_rxid(struct isotp_sock *so) | |
172 | { | |
173 | /* no broadcast modes => register rx_id for FC frame reception */ | |
174 | return (isotp_bc_flags(so) == 0); | |
175 | } | |
176 | ||
177 | static bool isotp_register_txecho(struct isotp_sock *so) | |
178 | { | |
179 | /* all modes but SF_BROADCAST register for tx echo skbs */ | |
180 | return (isotp_bc_flags(so) != CAN_ISOTP_SF_BROADCAST); | |
181 | } | |
182 | ||
e057dd3f OH |
183 | static enum hrtimer_restart isotp_rx_timer_handler(struct hrtimer *hrtimer) |
184 | { | |
185 | struct isotp_sock *so = container_of(hrtimer, struct isotp_sock, | |
186 | rxtimer); | |
187 | struct sock *sk = &so->sk; | |
188 | ||
189 | if (so->rx.state == ISOTP_WAIT_DATA) { | |
190 | /* we did not get new data frames in time */ | |
191 | ||
192 | /* report 'connection timed out' */ | |
193 | sk->sk_err = ETIMEDOUT; | |
194 | if (!sock_flag(sk, SOCK_DEAD)) | |
e3ae2365 | 195 | sk_error_report(sk); |
e057dd3f OH |
196 | |
197 | /* reset rx state */ | |
198 | so->rx.state = ISOTP_IDLE; | |
199 | } | |
200 | ||
201 | return HRTIMER_NORESTART; | |
202 | } | |
203 | ||
204 | static int isotp_send_fc(struct sock *sk, int ae, u8 flowstatus) | |
205 | { | |
206 | struct net_device *dev; | |
207 | struct sk_buff *nskb; | |
208 | struct canfd_frame *ncf; | |
209 | struct isotp_sock *so = isotp_sk(sk); | |
210 | int can_send_ret; | |
211 | ||
212 | nskb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv), gfp_any()); | |
213 | if (!nskb) | |
214 | return 1; | |
215 | ||
216 | dev = dev_get_by_index(sock_net(sk), so->ifindex); | |
217 | if (!dev) { | |
218 | kfree_skb(nskb); | |
219 | return 1; | |
220 | } | |
221 | ||
222 | can_skb_reserve(nskb); | |
223 | can_skb_prv(nskb)->ifindex = dev->ifindex; | |
224 | can_skb_prv(nskb)->skbcnt = 0; | |
225 | ||
226 | nskb->dev = dev; | |
227 | can_skb_set_owner(nskb, sk); | |
228 | ncf = (struct canfd_frame *)nskb->data; | |
b5f020f8 | 229 | skb_put_zero(nskb, so->ll.mtu); |
e057dd3f OH |
230 | |
231 | /* create & send flow control reply */ | |
232 | ncf->can_id = so->txid; | |
233 | ||
234 | if (so->opt.flags & CAN_ISOTP_TX_PADDING) { | |
235 | memset(ncf->data, so->opt.txpad_content, CAN_MAX_DLEN); | |
236 | ncf->len = CAN_MAX_DLEN; | |
237 | } else { | |
238 | ncf->len = ae + FC_CONTENT_SZ; | |
239 | } | |
240 | ||
241 | ncf->data[ae] = N_PCI_FC | flowstatus; | |
242 | ncf->data[ae + 1] = so->rxfc.bs; | |
243 | ncf->data[ae + 2] = so->rxfc.stmin; | |
244 | ||
245 | if (ae) | |
246 | ncf->data[0] = so->opt.ext_address; | |
247 | ||
d4eb538e | 248 | ncf->flags = so->ll.tx_flags; |
e057dd3f OH |
249 | |
250 | can_send_ret = can_send(nskb, 1); | |
251 | if (can_send_ret) | |
46d8657a PM |
252 | pr_notice_once("can-isotp: %s: can_send_ret %pe\n", |
253 | __func__, ERR_PTR(can_send_ret)); | |
e057dd3f OH |
254 | |
255 | dev_put(dev); | |
256 | ||
257 | /* reset blocksize counter */ | |
258 | so->rx.bs = 0; | |
259 | ||
260 | /* reset last CF frame rx timestamp for rx stmin enforcement */ | |
261 | so->lastrxcf_tstamp = ktime_set(0, 0); | |
262 | ||
263 | /* start rx timeout watchdog */ | |
86633786 OH |
264 | hrtimer_start(&so->rxtimer, ktime_set(ISOTP_FC_TIMEOUT, 0), |
265 | HRTIMER_MODE_REL_SOFT); | |
e057dd3f OH |
266 | return 0; |
267 | } | |
268 | ||
269 | static void isotp_rcv_skb(struct sk_buff *skb, struct sock *sk) | |
270 | { | |
271 | struct sockaddr_can *addr = (struct sockaddr_can *)skb->cb; | |
272 | ||
273 | BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct sockaddr_can)); | |
274 | ||
275 | memset(addr, 0, sizeof(*addr)); | |
276 | addr->can_family = AF_CAN; | |
277 | addr->can_ifindex = skb->dev->ifindex; | |
278 | ||
279 | if (sock_queue_rcv_skb(sk, skb) < 0) | |
280 | kfree_skb(skb); | |
281 | } | |
282 | ||
283 | static u8 padlen(u8 datalen) | |
284 | { | |
c3ddac4b CIK |
285 | static const u8 plen[] = { |
286 | 8, 8, 8, 8, 8, 8, 8, 8, 8, /* 0 - 8 */ | |
287 | 12, 12, 12, 12, /* 9 - 12 */ | |
288 | 16, 16, 16, 16, /* 13 - 16 */ | |
289 | 20, 20, 20, 20, /* 17 - 20 */ | |
290 | 24, 24, 24, 24, /* 21 - 24 */ | |
291 | 32, 32, 32, 32, 32, 32, 32, 32, /* 25 - 32 */ | |
292 | 48, 48, 48, 48, 48, 48, 48, 48, /* 33 - 40 */ | |
293 | 48, 48, 48, 48, 48, 48, 48, 48 /* 41 - 48 */ | |
294 | }; | |
e057dd3f OH |
295 | |
296 | if (datalen > 48) | |
297 | return 64; | |
298 | ||
299 | return plen[datalen]; | |
300 | } | |
301 | ||
302 | /* check for length optimization and return 1/true when the check fails */ | |
303 | static int check_optimized(struct canfd_frame *cf, int start_index) | |
304 | { | |
305 | /* for CAN_DL <= 8 the start_index is equal to the CAN_DL as the | |
306 | * padding would start at this point. E.g. if the padding would | |
307 | * start at cf.data[7] cf->len has to be 7 to be optimal. | |
308 | * Note: The data[] index starts with zero. | |
309 | */ | |
310 | if (cf->len <= CAN_MAX_DLEN) | |
311 | return (cf->len != start_index); | |
312 | ||
313 | /* This relation is also valid in the non-linear DLC range, where | |
314 | * we need to take care of the minimal next possible CAN_DL. | |
315 | * The correct check would be (padlen(cf->len) != padlen(start_index)). | |
316 | * But as cf->len can only take discrete values from 12, .., 64 at this | |
317 | * point the padlen(cf->len) is always equal to cf->len. | |
318 | */ | |
319 | return (cf->len != padlen(start_index)); | |
320 | } | |
321 | ||
322 | /* check padding and return 1/true when the check fails */ | |
323 | static int check_pad(struct isotp_sock *so, struct canfd_frame *cf, | |
324 | int start_index, u8 content) | |
325 | { | |
326 | int i; | |
327 | ||
328 | /* no RX_PADDING value => check length of optimized frame length */ | |
329 | if (!(so->opt.flags & CAN_ISOTP_RX_PADDING)) { | |
330 | if (so->opt.flags & CAN_ISOTP_CHK_PAD_LEN) | |
331 | return check_optimized(cf, start_index); | |
332 | ||
333 | /* no valid test against empty value => ignore frame */ | |
334 | return 1; | |
335 | } | |
336 | ||
337 | /* check datalength of correctly padded CAN frame */ | |
338 | if ((so->opt.flags & CAN_ISOTP_CHK_PAD_LEN) && | |
339 | cf->len != padlen(cf->len)) | |
340 | return 1; | |
341 | ||
342 | /* check padding content */ | |
343 | if (so->opt.flags & CAN_ISOTP_CHK_PAD_DATA) { | |
344 | for (i = start_index; i < cf->len; i++) | |
345 | if (cf->data[i] != content) | |
346 | return 1; | |
347 | } | |
348 | return 0; | |
349 | } | |
350 | ||
86633786 OH |
351 | static void isotp_send_cframe(struct isotp_sock *so); |
352 | ||
e057dd3f OH |
353 | static int isotp_rcv_fc(struct isotp_sock *so, struct canfd_frame *cf, int ae) |
354 | { | |
355 | struct sock *sk = &so->sk; | |
356 | ||
357 | if (so->tx.state != ISOTP_WAIT_FC && | |
358 | so->tx.state != ISOTP_WAIT_FIRST_FC) | |
359 | return 0; | |
360 | ||
361 | hrtimer_cancel(&so->txtimer); | |
362 | ||
363 | if ((cf->len < ae + FC_CONTENT_SZ) || | |
364 | ((so->opt.flags & ISOTP_CHECK_PADDING) && | |
365 | check_pad(so, cf, ae + FC_CONTENT_SZ, so->opt.rxpad_content))) { | |
366 | /* malformed PDU - report 'not a data message' */ | |
367 | sk->sk_err = EBADMSG; | |
368 | if (!sock_flag(sk, SOCK_DEAD)) | |
e3ae2365 | 369 | sk_error_report(sk); |
e057dd3f OH |
370 | |
371 | so->tx.state = ISOTP_IDLE; | |
372 | wake_up_interruptible(&so->wait); | |
373 | return 1; | |
374 | } | |
375 | ||
376 | /* get communication parameters only from the first FC frame */ | |
377 | if (so->tx.state == ISOTP_WAIT_FIRST_FC) { | |
378 | so->txfc.bs = cf->data[ae + 1]; | |
379 | so->txfc.stmin = cf->data[ae + 2]; | |
380 | ||
381 | /* fix wrong STmin values according spec */ | |
382 | if (so->txfc.stmin > 0x7F && | |
383 | (so->txfc.stmin < 0xF1 || so->txfc.stmin > 0xF9)) | |
384 | so->txfc.stmin = 0x7F; | |
385 | ||
386 | so->tx_gap = ktime_set(0, 0); | |
387 | /* add transmission time for CAN frame N_As */ | |
530e0d46 | 388 | so->tx_gap = ktime_add_ns(so->tx_gap, so->frame_txtime); |
e057dd3f OH |
389 | /* add waiting time for consecutive frames N_Cs */ |
390 | if (so->opt.flags & CAN_ISOTP_FORCE_TXSTMIN) | |
391 | so->tx_gap = ktime_add_ns(so->tx_gap, | |
392 | so->force_tx_stmin); | |
393 | else if (so->txfc.stmin < 0x80) | |
394 | so->tx_gap = ktime_add_ns(so->tx_gap, | |
395 | so->txfc.stmin * 1000000); | |
396 | else | |
397 | so->tx_gap = ktime_add_ns(so->tx_gap, | |
398 | (so->txfc.stmin - 0xF0) | |
399 | * 100000); | |
400 | so->tx.state = ISOTP_WAIT_FC; | |
401 | } | |
402 | ||
403 | switch (cf->data[ae] & 0x0F) { | |
404 | case ISOTP_FC_CTS: | |
405 | so->tx.bs = 0; | |
406 | so->tx.state = ISOTP_SENDING; | |
86633786 OH |
407 | /* send CF frame and enable echo timeout handling */ |
408 | hrtimer_start(&so->txtimer, ktime_set(ISOTP_ECHO_TIMEOUT, 0), | |
e057dd3f | 409 | HRTIMER_MODE_REL_SOFT); |
86633786 | 410 | isotp_send_cframe(so); |
e057dd3f OH |
411 | break; |
412 | ||
413 | case ISOTP_FC_WT: | |
414 | /* start timer to wait for next FC frame */ | |
86633786 | 415 | hrtimer_start(&so->txtimer, ktime_set(ISOTP_FC_TIMEOUT, 0), |
e057dd3f OH |
416 | HRTIMER_MODE_REL_SOFT); |
417 | break; | |
418 | ||
419 | case ISOTP_FC_OVFLW: | |
420 | /* overflow on receiver side - report 'message too long' */ | |
421 | sk->sk_err = EMSGSIZE; | |
422 | if (!sock_flag(sk, SOCK_DEAD)) | |
e3ae2365 | 423 | sk_error_report(sk); |
e057dd3f OH |
424 | fallthrough; |
425 | ||
426 | default: | |
427 | /* stop this tx job */ | |
428 | so->tx.state = ISOTP_IDLE; | |
429 | wake_up_interruptible(&so->wait); | |
430 | } | |
431 | return 0; | |
432 | } | |
433 | ||
434 | static int isotp_rcv_sf(struct sock *sk, struct canfd_frame *cf, int pcilen, | |
435 | struct sk_buff *skb, int len) | |
436 | { | |
437 | struct isotp_sock *so = isotp_sk(sk); | |
438 | struct sk_buff *nskb; | |
439 | ||
440 | hrtimer_cancel(&so->rxtimer); | |
441 | so->rx.state = ISOTP_IDLE; | |
442 | ||
443 | if (!len || len > cf->len - pcilen) | |
444 | return 1; | |
445 | ||
446 | if ((so->opt.flags & ISOTP_CHECK_PADDING) && | |
447 | check_pad(so, cf, pcilen + len, so->opt.rxpad_content)) { | |
448 | /* malformed PDU - report 'not a data message' */ | |
449 | sk->sk_err = EBADMSG; | |
450 | if (!sock_flag(sk, SOCK_DEAD)) | |
e3ae2365 | 451 | sk_error_report(sk); |
e057dd3f OH |
452 | return 1; |
453 | } | |
454 | ||
455 | nskb = alloc_skb(len, gfp_any()); | |
456 | if (!nskb) | |
457 | return 1; | |
458 | ||
459 | memcpy(skb_put(nskb, len), &cf->data[pcilen], len); | |
460 | ||
461 | nskb->tstamp = skb->tstamp; | |
462 | nskb->dev = skb->dev; | |
463 | isotp_rcv_skb(nskb, sk); | |
464 | return 0; | |
465 | } | |
466 | ||
467 | static int isotp_rcv_ff(struct sock *sk, struct canfd_frame *cf, int ae) | |
468 | { | |
469 | struct isotp_sock *so = isotp_sk(sk); | |
470 | int i; | |
471 | int off; | |
472 | int ff_pci_sz; | |
473 | ||
474 | hrtimer_cancel(&so->rxtimer); | |
475 | so->rx.state = ISOTP_IDLE; | |
476 | ||
477 | /* get the used sender LL_DL from the (first) CAN frame data length */ | |
478 | so->rx.ll_dl = padlen(cf->len); | |
479 | ||
480 | /* the first frame has to use the entire frame up to LL_DL length */ | |
481 | if (cf->len != so->rx.ll_dl) | |
482 | return 1; | |
483 | ||
484 | /* get the FF_DL */ | |
485 | so->rx.len = (cf->data[ae] & 0x0F) << 8; | |
486 | so->rx.len += cf->data[ae + 1]; | |
487 | ||
488 | /* Check for FF_DL escape sequence supporting 32 bit PDU length */ | |
489 | if (so->rx.len) { | |
490 | ff_pci_sz = FF_PCI_SZ12; | |
491 | } else { | |
492 | /* FF_DL = 0 => get real length from next 4 bytes */ | |
493 | so->rx.len = cf->data[ae + 2] << 24; | |
494 | so->rx.len += cf->data[ae + 3] << 16; | |
495 | so->rx.len += cf->data[ae + 4] << 8; | |
496 | so->rx.len += cf->data[ae + 5]; | |
497 | ff_pci_sz = FF_PCI_SZ32; | |
498 | } | |
499 | ||
500 | /* take care of a potential SF_DL ESC offset for TX_DL > 8 */ | |
501 | off = (so->rx.ll_dl > CAN_MAX_DLEN) ? 1 : 0; | |
502 | ||
503 | if (so->rx.len + ae + off + ff_pci_sz < so->rx.ll_dl) | |
504 | return 1; | |
505 | ||
506 | if (so->rx.len > MAX_MSG_LENGTH) { | |
507 | /* send FC frame with overflow status */ | |
508 | isotp_send_fc(sk, ae, ISOTP_FC_OVFLW); | |
509 | return 1; | |
510 | } | |
511 | ||
512 | /* copy the first received data bytes */ | |
513 | so->rx.idx = 0; | |
514 | for (i = ae + ff_pci_sz; i < so->rx.ll_dl; i++) | |
515 | so->rx.buf[so->rx.idx++] = cf->data[i]; | |
516 | ||
517 | /* initial setup for this pdu reception */ | |
518 | so->rx.sn = 1; | |
519 | so->rx.state = ISOTP_WAIT_DATA; | |
520 | ||
521 | /* no creation of flow control frames */ | |
522 | if (so->opt.flags & CAN_ISOTP_LISTEN_MODE) | |
523 | return 0; | |
524 | ||
525 | /* send our first FC frame */ | |
526 | isotp_send_fc(sk, ae, ISOTP_FC_CTS); | |
527 | return 0; | |
528 | } | |
529 | ||
530 | static int isotp_rcv_cf(struct sock *sk, struct canfd_frame *cf, int ae, | |
531 | struct sk_buff *skb) | |
532 | { | |
533 | struct isotp_sock *so = isotp_sk(sk); | |
534 | struct sk_buff *nskb; | |
535 | int i; | |
536 | ||
537 | if (so->rx.state != ISOTP_WAIT_DATA) | |
538 | return 0; | |
539 | ||
540 | /* drop if timestamp gap is less than force_rx_stmin nano secs */ | |
541 | if (so->opt.flags & CAN_ISOTP_FORCE_RXSTMIN) { | |
542 | if (ktime_to_ns(ktime_sub(skb->tstamp, so->lastrxcf_tstamp)) < | |
543 | so->force_rx_stmin) | |
544 | return 0; | |
545 | ||
546 | so->lastrxcf_tstamp = skb->tstamp; | |
547 | } | |
548 | ||
549 | hrtimer_cancel(&so->rxtimer); | |
550 | ||
551 | /* CFs are never longer than the FF */ | |
552 | if (cf->len > so->rx.ll_dl) | |
553 | return 1; | |
554 | ||
555 | /* CFs have usually the LL_DL length */ | |
556 | if (cf->len < so->rx.ll_dl) { | |
557 | /* this is only allowed for the last CF */ | |
558 | if (so->rx.len - so->rx.idx > so->rx.ll_dl - ae - N_PCI_SZ) | |
559 | return 1; | |
560 | } | |
561 | ||
562 | if ((cf->data[ae] & 0x0F) != so->rx.sn) { | |
563 | /* wrong sn detected - report 'illegal byte sequence' */ | |
564 | sk->sk_err = EILSEQ; | |
565 | if (!sock_flag(sk, SOCK_DEAD)) | |
e3ae2365 | 566 | sk_error_report(sk); |
e057dd3f OH |
567 | |
568 | /* reset rx state */ | |
569 | so->rx.state = ISOTP_IDLE; | |
570 | return 1; | |
571 | } | |
572 | so->rx.sn++; | |
573 | so->rx.sn %= 16; | |
574 | ||
575 | for (i = ae + N_PCI_SZ; i < cf->len; i++) { | |
576 | so->rx.buf[so->rx.idx++] = cf->data[i]; | |
577 | if (so->rx.idx >= so->rx.len) | |
578 | break; | |
579 | } | |
580 | ||
581 | if (so->rx.idx >= so->rx.len) { | |
582 | /* we are done */ | |
583 | so->rx.state = ISOTP_IDLE; | |
584 | ||
585 | if ((so->opt.flags & ISOTP_CHECK_PADDING) && | |
586 | check_pad(so, cf, i + 1, so->opt.rxpad_content)) { | |
587 | /* malformed PDU - report 'not a data message' */ | |
588 | sk->sk_err = EBADMSG; | |
589 | if (!sock_flag(sk, SOCK_DEAD)) | |
e3ae2365 | 590 | sk_error_report(sk); |
e057dd3f OH |
591 | return 1; |
592 | } | |
593 | ||
594 | nskb = alloc_skb(so->rx.len, gfp_any()); | |
595 | if (!nskb) | |
596 | return 1; | |
597 | ||
598 | memcpy(skb_put(nskb, so->rx.len), so->rx.buf, | |
599 | so->rx.len); | |
600 | ||
601 | nskb->tstamp = skb->tstamp; | |
602 | nskb->dev = skb->dev; | |
603 | isotp_rcv_skb(nskb, sk); | |
604 | return 0; | |
605 | } | |
606 | ||
e057dd3f OH |
607 | /* perform blocksize handling, if enabled */ |
608 | if (!so->rxfc.bs || ++so->rx.bs < so->rxfc.bs) { | |
609 | /* start rx timeout watchdog */ | |
86633786 | 610 | hrtimer_start(&so->rxtimer, ktime_set(ISOTP_FC_TIMEOUT, 0), |
e057dd3f OH |
611 | HRTIMER_MODE_REL_SOFT); |
612 | return 0; | |
613 | } | |
614 | ||
78656ea2 OH |
615 | /* no creation of flow control frames */ |
616 | if (so->opt.flags & CAN_ISOTP_LISTEN_MODE) | |
617 | return 0; | |
618 | ||
e057dd3f OH |
619 | /* we reached the specified blocksize so->rxfc.bs */ |
620 | isotp_send_fc(sk, ae, ISOTP_FC_CTS); | |
621 | return 0; | |
622 | } | |
623 | ||
624 | static void isotp_rcv(struct sk_buff *skb, void *data) | |
625 | { | |
626 | struct sock *sk = (struct sock *)data; | |
627 | struct isotp_sock *so = isotp_sk(sk); | |
628 | struct canfd_frame *cf; | |
629 | int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0; | |
630 | u8 n_pci_type, sf_dl; | |
631 | ||
632 | /* Strictly receive only frames with the configured MTU size | |
633 | * => clear separation of CAN2.0 / CAN FD transport channels | |
634 | */ | |
635 | if (skb->len != so->ll.mtu) | |
636 | return; | |
637 | ||
638 | cf = (struct canfd_frame *)skb->data; | |
639 | ||
640 | /* if enabled: check reception of my configured extended address */ | |
641 | if (ae && cf->data[0] != so->opt.rx_ext_address) | |
642 | return; | |
643 | ||
644 | n_pci_type = cf->data[ae] & 0xF0; | |
645 | ||
7c759040 OH |
646 | /* Make sure the state changes and data structures stay consistent at |
647 | * CAN frame reception time. This locking is not needed in real world | |
648 | * use cases but the inconsistency can be triggered with syzkaller. | |
649 | */ | |
650 | spin_lock(&so->rx_lock); | |
651 | ||
e057dd3f OH |
652 | if (so->opt.flags & CAN_ISOTP_HALF_DUPLEX) { |
653 | /* check rx/tx path half duplex expectations */ | |
654 | if ((so->tx.state != ISOTP_IDLE && n_pci_type != N_PCI_FC) || | |
655 | (so->rx.state != ISOTP_IDLE && n_pci_type == N_PCI_FC)) | |
7c759040 | 656 | goto out_unlock; |
e057dd3f OH |
657 | } |
658 | ||
659 | switch (n_pci_type) { | |
660 | case N_PCI_FC: | |
661 | /* tx path: flow control frame containing the FC parameters */ | |
662 | isotp_rcv_fc(so, cf, ae); | |
663 | break; | |
664 | ||
665 | case N_PCI_SF: | |
666 | /* rx path: single frame | |
667 | * | |
668 | * As we do not have a rx.ll_dl configuration, we can only test | |
669 | * if the CAN frames payload length matches the LL_DL == 8 | |
670 | * requirements - no matter if it's CAN 2.0 or CAN FD | |
671 | */ | |
672 | ||
673 | /* get the SF_DL from the N_PCI byte */ | |
674 | sf_dl = cf->data[ae] & 0x0F; | |
675 | ||
676 | if (cf->len <= CAN_MAX_DLEN) { | |
677 | isotp_rcv_sf(sk, cf, SF_PCI_SZ4 + ae, skb, sf_dl); | |
678 | } else { | |
96a7457a | 679 | if (can_is_canfd_skb(skb)) { |
e057dd3f OH |
680 | /* We have a CAN FD frame and CAN_DL is greater than 8: |
681 | * Only frames with the SF_DL == 0 ESC value are valid. | |
682 | * | |
683 | * If so take care of the increased SF PCI size | |
684 | * (SF_PCI_SZ8) to point to the message content behind | |
685 | * the extended SF PCI info and get the real SF_DL | |
686 | * length value from the formerly first data byte. | |
687 | */ | |
688 | if (sf_dl == 0) | |
689 | isotp_rcv_sf(sk, cf, SF_PCI_SZ8 + ae, skb, | |
690 | cf->data[SF_PCI_SZ4 + ae]); | |
691 | } | |
692 | } | |
693 | break; | |
694 | ||
695 | case N_PCI_FF: | |
696 | /* rx path: first frame */ | |
697 | isotp_rcv_ff(sk, cf, ae); | |
698 | break; | |
699 | ||
700 | case N_PCI_CF: | |
701 | /* rx path: consecutive frame */ | |
702 | isotp_rcv_cf(sk, cf, ae, skb); | |
703 | break; | |
704 | } | |
7c759040 OH |
705 | |
706 | out_unlock: | |
707 | spin_unlock(&so->rx_lock); | |
e057dd3f OH |
708 | } |
709 | ||
710 | static void isotp_fill_dataframe(struct canfd_frame *cf, struct isotp_sock *so, | |
711 | int ae, int off) | |
712 | { | |
713 | int pcilen = N_PCI_SZ + ae + off; | |
714 | int space = so->tx.ll_dl - pcilen; | |
715 | int num = min_t(int, so->tx.len - so->tx.idx, space); | |
716 | int i; | |
717 | ||
718 | cf->can_id = so->txid; | |
719 | cf->len = num + pcilen; | |
720 | ||
721 | if (num < space) { | |
722 | if (so->opt.flags & CAN_ISOTP_TX_PADDING) { | |
723 | /* user requested padding */ | |
724 | cf->len = padlen(cf->len); | |
725 | memset(cf->data, so->opt.txpad_content, cf->len); | |
726 | } else if (cf->len > CAN_MAX_DLEN) { | |
727 | /* mandatory padding for CAN FD frames */ | |
728 | cf->len = padlen(cf->len); | |
729 | memset(cf->data, CAN_ISOTP_DEFAULT_PAD_CONTENT, | |
730 | cf->len); | |
731 | } | |
732 | } | |
733 | ||
734 | for (i = 0; i < num; i++) | |
735 | cf->data[pcilen + i] = so->tx.buf[so->tx.idx++]; | |
736 | ||
737 | if (ae) | |
738 | cf->data[0] = so->opt.ext_address; | |
739 | } | |
740 | ||
4b7fe92c OH |
741 | static void isotp_send_cframe(struct isotp_sock *so) |
742 | { | |
743 | struct sock *sk = &so->sk; | |
744 | struct sk_buff *skb; | |
745 | struct net_device *dev; | |
746 | struct canfd_frame *cf; | |
747 | int can_send_ret; | |
748 | int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0; | |
749 | ||
750 | dev = dev_get_by_index(sock_net(sk), so->ifindex); | |
751 | if (!dev) | |
752 | return; | |
753 | ||
754 | skb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv), GFP_ATOMIC); | |
755 | if (!skb) { | |
756 | dev_put(dev); | |
757 | return; | |
758 | } | |
759 | ||
760 | can_skb_reserve(skb); | |
761 | can_skb_prv(skb)->ifindex = dev->ifindex; | |
762 | can_skb_prv(skb)->skbcnt = 0; | |
763 | ||
764 | cf = (struct canfd_frame *)skb->data; | |
765 | skb_put_zero(skb, so->ll.mtu); | |
766 | ||
767 | /* create consecutive frame */ | |
768 | isotp_fill_dataframe(cf, so, ae, 0); | |
769 | ||
770 | /* place consecutive frame N_PCI in appropriate index */ | |
771 | cf->data[ae] = N_PCI_CF | so->tx.sn++; | |
772 | so->tx.sn %= 16; | |
773 | so->tx.bs++; | |
774 | ||
775 | cf->flags = so->ll.tx_flags; | |
776 | ||
777 | skb->dev = dev; | |
778 | can_skb_set_owner(skb, sk); | |
779 | ||
780 | /* cfecho should have been zero'ed by init/isotp_rcv_echo() */ | |
781 | if (so->cfecho) | |
782 | pr_notice_once("can-isotp: cfecho is %08X != 0\n", so->cfecho); | |
783 | ||
784 | /* set consecutive frame echo tag */ | |
785 | so->cfecho = *(u32 *)cf->data; | |
786 | ||
787 | /* send frame with local echo enabled */ | |
788 | can_send_ret = can_send(skb, 1); | |
789 | if (can_send_ret) { | |
790 | pr_notice_once("can-isotp: %s: can_send_ret %pe\n", | |
791 | __func__, ERR_PTR(can_send_ret)); | |
792 | if (can_send_ret == -ENOBUFS) | |
793 | pr_notice_once("can-isotp: tx queue is full\n"); | |
794 | } | |
795 | dev_put(dev); | |
796 | } | |
797 | ||
e057dd3f OH |
798 | static void isotp_create_fframe(struct canfd_frame *cf, struct isotp_sock *so, |
799 | int ae) | |
800 | { | |
801 | int i; | |
802 | int ff_pci_sz; | |
803 | ||
804 | cf->can_id = so->txid; | |
805 | cf->len = so->tx.ll_dl; | |
806 | if (ae) | |
807 | cf->data[0] = so->opt.ext_address; | |
808 | ||
809 | /* create N_PCI bytes with 12/32 bit FF_DL data length */ | |
810 | if (so->tx.len > 4095) { | |
811 | /* use 32 bit FF_DL notation */ | |
812 | cf->data[ae] = N_PCI_FF; | |
813 | cf->data[ae + 1] = 0; | |
814 | cf->data[ae + 2] = (u8)(so->tx.len >> 24) & 0xFFU; | |
815 | cf->data[ae + 3] = (u8)(so->tx.len >> 16) & 0xFFU; | |
816 | cf->data[ae + 4] = (u8)(so->tx.len >> 8) & 0xFFU; | |
817 | cf->data[ae + 5] = (u8)so->tx.len & 0xFFU; | |
818 | ff_pci_sz = FF_PCI_SZ32; | |
819 | } else { | |
820 | /* use 12 bit FF_DL notation */ | |
821 | cf->data[ae] = (u8)(so->tx.len >> 8) | N_PCI_FF; | |
822 | cf->data[ae + 1] = (u8)so->tx.len & 0xFFU; | |
823 | ff_pci_sz = FF_PCI_SZ12; | |
824 | } | |
825 | ||
826 | /* add first data bytes depending on ae */ | |
827 | for (i = ae + ff_pci_sz; i < so->tx.ll_dl; i++) | |
828 | cf->data[i] = so->tx.buf[so->tx.idx++]; | |
829 | ||
830 | so->tx.sn = 1; | |
e057dd3f OH |
831 | } |
832 | ||
4b7fe92c OH |
833 | static void isotp_rcv_echo(struct sk_buff *skb, void *data) |
834 | { | |
835 | struct sock *sk = (struct sock *)data; | |
836 | struct isotp_sock *so = isotp_sk(sk); | |
837 | struct canfd_frame *cf = (struct canfd_frame *)skb->data; | |
838 | ||
86633786 | 839 | /* only handle my own local echo CF/SF skb's (no FF!) */ |
4b7fe92c OH |
840 | if (skb->sk != sk || so->cfecho != *(u32 *)cf->data) |
841 | return; | |
842 | ||
843 | /* cancel local echo timeout */ | |
844 | hrtimer_cancel(&so->txtimer); | |
845 | ||
846 | /* local echo skb with consecutive frame has been consumed */ | |
847 | so->cfecho = 0; | |
848 | ||
849 | if (so->tx.idx >= so->tx.len) { | |
850 | /* we are done */ | |
851 | so->tx.state = ISOTP_IDLE; | |
852 | wake_up_interruptible(&so->wait); | |
853 | return; | |
854 | } | |
855 | ||
856 | if (so->txfc.bs && so->tx.bs >= so->txfc.bs) { | |
857 | /* stop and wait for FC with timeout */ | |
858 | so->tx.state = ISOTP_WAIT_FC; | |
86633786 | 859 | hrtimer_start(&so->txtimer, ktime_set(ISOTP_FC_TIMEOUT, 0), |
4b7fe92c OH |
860 | HRTIMER_MODE_REL_SOFT); |
861 | return; | |
862 | } | |
863 | ||
864 | /* no gap between data frames needed => use burst mode */ | |
865 | if (!so->tx_gap) { | |
86633786 OH |
866 | /* enable echo timeout handling */ |
867 | hrtimer_start(&so->txtimer, ktime_set(ISOTP_ECHO_TIMEOUT, 0), | |
868 | HRTIMER_MODE_REL_SOFT); | |
4b7fe92c OH |
869 | isotp_send_cframe(so); |
870 | return; | |
871 | } | |
872 | ||
873 | /* start timer to send next consecutive frame with correct delay */ | |
4f027cba | 874 | hrtimer_start(&so->txfrtimer, so->tx_gap, HRTIMER_MODE_REL_SOFT); |
4b7fe92c OH |
875 | } |
876 | ||
e057dd3f OH |
877 | static enum hrtimer_restart isotp_tx_timer_handler(struct hrtimer *hrtimer) |
878 | { | |
879 | struct isotp_sock *so = container_of(hrtimer, struct isotp_sock, | |
880 | txtimer); | |
881 | struct sock *sk = &so->sk; | |
e057dd3f | 882 | |
4f027cba OH |
883 | /* don't handle timeouts in IDLE state */ |
884 | if (so->tx.state == ISOTP_IDLE) | |
885 | return HRTIMER_NORESTART; | |
4b7fe92c | 886 | |
4f027cba | 887 | /* we did not get any flow control or echo frame in time */ |
4b7fe92c | 888 | |
4f027cba OH |
889 | /* report 'communication error on send' */ |
890 | sk->sk_err = ECOMM; | |
891 | if (!sock_flag(sk, SOCK_DEAD)) | |
892 | sk_error_report(sk); | |
4b7fe92c | 893 | |
4f027cba OH |
894 | /* reset tx state */ |
895 | so->tx.state = ISOTP_IDLE; | |
896 | wake_up_interruptible(&so->wait); | |
e057dd3f | 897 | |
4f027cba OH |
898 | return HRTIMER_NORESTART; |
899 | } | |
e057dd3f | 900 | |
4f027cba OH |
901 | static enum hrtimer_restart isotp_txfr_timer_handler(struct hrtimer *hrtimer) |
902 | { | |
903 | struct isotp_sock *so = container_of(hrtimer, struct isotp_sock, | |
904 | txfrtimer); | |
e057dd3f | 905 | |
4f027cba OH |
906 | /* start echo timeout handling and cover below protocol error */ |
907 | hrtimer_start(&so->txtimer, ktime_set(ISOTP_ECHO_TIMEOUT, 0), | |
908 | HRTIMER_MODE_REL_SOFT); | |
e057dd3f | 909 | |
4f027cba OH |
910 | /* cfecho should be consumed by isotp_rcv_echo() here */ |
911 | if (so->tx.state == ISOTP_SENDING && !so->cfecho) | |
912 | isotp_send_cframe(so); | |
e057dd3f | 913 | |
4f027cba | 914 | return HRTIMER_NORESTART; |
e057dd3f OH |
915 | } |
916 | ||
917 | static int isotp_sendmsg(struct socket *sock, struct msghdr *msg, size_t size) | |
918 | { | |
919 | struct sock *sk = sock->sk; | |
920 | struct isotp_sock *so = isotp_sk(sk); | |
43a08c3b | 921 | u32 old_state = so->tx.state; |
e057dd3f OH |
922 | struct sk_buff *skb; |
923 | struct net_device *dev; | |
924 | struct canfd_frame *cf; | |
925 | int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0; | |
926 | int wait_tx_done = (so->opt.flags & CAN_ISOTP_WAIT_TX_DONE) ? 1 : 0; | |
86633786 | 927 | s64 hrtimer_sec = ISOTP_ECHO_TIMEOUT; |
e057dd3f OH |
928 | int off; |
929 | int err; | |
930 | ||
931 | if (!so->bound) | |
932 | return -EADDRNOTAVAIL; | |
933 | ||
934 | /* we do not support multiple buffers - for now */ | |
43a08c3b ZX |
935 | if (cmpxchg(&so->tx.state, ISOTP_IDLE, ISOTP_SENDING) != ISOTP_IDLE || |
936 | wq_has_sleeper(&so->wait)) { | |
937 | if (msg->msg_flags & MSG_DONTWAIT) { | |
938 | err = -EAGAIN; | |
939 | goto err_out; | |
940 | } | |
e057dd3f OH |
941 | |
942 | /* wait for complete transmission of current pdu */ | |
9acf6362 ZX |
943 | err = wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE); |
944 | if (err) | |
43a08c3b | 945 | goto err_out; |
86633786 OH |
946 | |
947 | so->tx.state = ISOTP_SENDING; | |
e057dd3f OH |
948 | } |
949 | ||
43a08c3b ZX |
950 | if (!size || size > MAX_MSG_LENGTH) { |
951 | err = -EINVAL; | |
8375dfac | 952 | goto err_out_drop; |
43a08c3b | 953 | } |
e057dd3f | 954 | |
921ca574 OH |
955 | /* take care of a potential SF_DL ESC offset for TX_DL > 8 */ |
956 | off = (so->tx.ll_dl > CAN_MAX_DLEN) ? 1 : 0; | |
957 | ||
958 | /* does the given data fit into a single frame for SF_BROADCAST? */ | |
9f39d365 | 959 | if ((isotp_bc_flags(so) == CAN_ISOTP_SF_BROADCAST) && |
43a08c3b ZX |
960 | (size > so->tx.ll_dl - SF_PCI_SZ4 - ae - off)) { |
961 | err = -EINVAL; | |
8375dfac | 962 | goto err_out_drop; |
43a08c3b | 963 | } |
921ca574 | 964 | |
e057dd3f OH |
965 | err = memcpy_from_msg(so->tx.buf, msg, size); |
966 | if (err < 0) | |
8375dfac | 967 | goto err_out_drop; |
e057dd3f OH |
968 | |
969 | dev = dev_get_by_index(sock_net(sk), so->ifindex); | |
43a08c3b ZX |
970 | if (!dev) { |
971 | err = -ENXIO; | |
8375dfac | 972 | goto err_out_drop; |
43a08c3b | 973 | } |
e057dd3f OH |
974 | |
975 | skb = sock_alloc_send_skb(sk, so->ll.mtu + sizeof(struct can_skb_priv), | |
976 | msg->msg_flags & MSG_DONTWAIT, &err); | |
977 | if (!skb) { | |
978 | dev_put(dev); | |
8375dfac | 979 | goto err_out_drop; |
e057dd3f OH |
980 | } |
981 | ||
982 | can_skb_reserve(skb); | |
983 | can_skb_prv(skb)->ifindex = dev->ifindex; | |
984 | can_skb_prv(skb)->skbcnt = 0; | |
985 | ||
e057dd3f OH |
986 | so->tx.len = size; |
987 | so->tx.idx = 0; | |
988 | ||
989 | cf = (struct canfd_frame *)skb->data; | |
b5f020f8 | 990 | skb_put_zero(skb, so->ll.mtu); |
e057dd3f | 991 | |
86633786 OH |
992 | /* cfecho should have been zero'ed by init / former isotp_rcv_echo() */ |
993 | if (so->cfecho) | |
994 | pr_notice_once("can-isotp: uninit cfecho %08X\n", so->cfecho); | |
995 | ||
e057dd3f OH |
996 | /* check for single frame transmission depending on TX_DL */ |
997 | if (size <= so->tx.ll_dl - SF_PCI_SZ4 - ae - off) { | |
998 | /* The message size generally fits into a SingleFrame - good. | |
999 | * | |
1000 | * SF_DL ESC offset optimization: | |
1001 | * | |
1002 | * When TX_DL is greater 8 but the message would still fit | |
1003 | * into a 8 byte CAN frame, we can omit the offset. | |
1004 | * This prevents a protocol caused length extension from | |
1005 | * CAN_DL = 8 to CAN_DL = 12 due to the SF_SL ESC handling. | |
1006 | */ | |
1007 | if (size <= CAN_MAX_DLEN - SF_PCI_SZ4 - ae) | |
1008 | off = 0; | |
1009 | ||
1010 | isotp_fill_dataframe(cf, so, ae, off); | |
1011 | ||
1012 | /* place single frame N_PCI w/o length in appropriate index */ | |
1013 | cf->data[ae] = N_PCI_SF; | |
1014 | ||
1015 | /* place SF_DL size value depending on the SF_DL ESC offset */ | |
1016 | if (off) | |
1017 | cf->data[SF_PCI_SZ4 + ae] = size; | |
1018 | else | |
1019 | cf->data[ae] |= size; | |
1020 | ||
86633786 OH |
1021 | /* set CF echo tag for isotp_rcv_echo() (SF-mode) */ |
1022 | so->cfecho = *(u32 *)cf->data; | |
e057dd3f | 1023 | } else { |
9f39d365 | 1024 | /* send first frame */ |
e057dd3f OH |
1025 | |
1026 | isotp_create_fframe(cf, so, ae); | |
1027 | ||
9f39d365 OH |
1028 | if (isotp_bc_flags(so) == CAN_ISOTP_CF_BROADCAST) { |
1029 | /* set timer for FC-less operation (STmin = 0) */ | |
1030 | if (so->opt.flags & CAN_ISOTP_FORCE_TXSTMIN) | |
1031 | so->tx_gap = ktime_set(0, so->force_tx_stmin); | |
1032 | else | |
1033 | so->tx_gap = ktime_set(0, so->frame_txtime); | |
1034 | ||
1035 | /* disable wait for FCs due to activated block size */ | |
1036 | so->txfc.bs = 0; | |
1037 | ||
86633786 | 1038 | /* set CF echo tag for isotp_rcv_echo() (CF-mode) */ |
9f39d365 | 1039 | so->cfecho = *(u32 *)cf->data; |
9f39d365 OH |
1040 | } else { |
1041 | /* standard flow control check */ | |
1042 | so->tx.state = ISOTP_WAIT_FIRST_FC; | |
1043 | ||
1044 | /* start timeout for FC */ | |
86633786 | 1045 | hrtimer_sec = ISOTP_FC_TIMEOUT; |
9f39d365 | 1046 | |
86633786 OH |
1047 | /* no CF echo tag for isotp_rcv_echo() (FF-mode) */ |
1048 | so->cfecho = 0; | |
1049 | } | |
e057dd3f OH |
1050 | } |
1051 | ||
86633786 OH |
1052 | hrtimer_start(&so->txtimer, ktime_set(hrtimer_sec, 0), |
1053 | HRTIMER_MODE_REL_SOFT); | |
1054 | ||
e057dd3f | 1055 | /* send the first or only CAN frame */ |
d4eb538e | 1056 | cf->flags = so->ll.tx_flags; |
e057dd3f OH |
1057 | |
1058 | skb->dev = dev; | |
1059 | skb->sk = sk; | |
1060 | err = can_send(skb, 1); | |
1061 | dev_put(dev); | |
1062 | if (err) { | |
46d8657a PM |
1063 | pr_notice_once("can-isotp: %s: can_send_ret %pe\n", |
1064 | __func__, ERR_PTR(err)); | |
d7349708 OH |
1065 | |
1066 | /* no transmission -> no timeout monitoring */ | |
86633786 | 1067 | hrtimer_cancel(&so->txtimer); |
d7349708 | 1068 | |
9f39d365 OH |
1069 | /* reset consecutive frame echo tag */ |
1070 | so->cfecho = 0; | |
1071 | ||
8375dfac | 1072 | goto err_out_drop; |
e057dd3f OH |
1073 | } |
1074 | ||
1075 | if (wait_tx_done) { | |
1076 | /* wait for complete transmission of current pdu */ | |
1077 | wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE); | |
d674a8f1 MKB |
1078 | |
1079 | if (sk->sk_err) | |
1080 | return -sk->sk_err; | |
e057dd3f OH |
1081 | } |
1082 | ||
1083 | return size; | |
43a08c3b | 1084 | |
8375dfac OH |
1085 | err_out_drop: |
1086 | /* drop this PDU and unlock a potential wait queue */ | |
1087 | old_state = ISOTP_IDLE; | |
43a08c3b ZX |
1088 | err_out: |
1089 | so->tx.state = old_state; | |
1090 | if (so->tx.state == ISOTP_IDLE) | |
1091 | wake_up_interruptible(&so->wait); | |
1092 | ||
1093 | return err; | |
e057dd3f OH |
1094 | } |
1095 | ||
1096 | static int isotp_recvmsg(struct socket *sock, struct msghdr *msg, size_t size, | |
1097 | int flags) | |
1098 | { | |
1099 | struct sock *sk = sock->sk; | |
1100 | struct sk_buff *skb; | |
30ffd533 | 1101 | struct isotp_sock *so = isotp_sk(sk); |
42bf50a1 | 1102 | int ret = 0; |
e057dd3f | 1103 | |
e382fea8 | 1104 | if (flags & ~(MSG_DONTWAIT | MSG_TRUNC | MSG_PEEK)) |
42bf50a1 | 1105 | return -EINVAL; |
e057dd3f | 1106 | |
30ffd533 OH |
1107 | if (!so->bound) |
1108 | return -EADDRNOTAVAIL; | |
1109 | ||
f4b41f06 | 1110 | skb = skb_recv_datagram(sk, flags, &ret); |
e057dd3f | 1111 | if (!skb) |
42bf50a1 | 1112 | return ret; |
e057dd3f OH |
1113 | |
1114 | if (size < skb->len) | |
1115 | msg->msg_flags |= MSG_TRUNC; | |
1116 | else | |
1117 | size = skb->len; | |
1118 | ||
42bf50a1 OH |
1119 | ret = memcpy_to_msg(msg, skb->data, size); |
1120 | if (ret < 0) | |
1121 | goto out_err; | |
e057dd3f OH |
1122 | |
1123 | sock_recv_timestamp(msg, sk, skb); | |
1124 | ||
1125 | if (msg->msg_name) { | |
f522d955 OH |
1126 | __sockaddr_check_size(ISOTP_MIN_NAMELEN); |
1127 | msg->msg_namelen = ISOTP_MIN_NAMELEN; | |
e057dd3f OH |
1128 | memcpy(msg->msg_name, skb->cb, msg->msg_namelen); |
1129 | } | |
1130 | ||
42bf50a1 OH |
1131 | /* set length of return value */ |
1132 | ret = (flags & MSG_TRUNC) ? skb->len : size; | |
1133 | ||
1134 | out_err: | |
e057dd3f OH |
1135 | skb_free_datagram(sk, skb); |
1136 | ||
42bf50a1 | 1137 | return ret; |
e057dd3f OH |
1138 | } |
1139 | ||
1140 | static int isotp_release(struct socket *sock) | |
1141 | { | |
1142 | struct sock *sk = sock->sk; | |
1143 | struct isotp_sock *so; | |
1144 | struct net *net; | |
1145 | ||
1146 | if (!sk) | |
1147 | return 0; | |
1148 | ||
1149 | so = isotp_sk(sk); | |
1150 | net = sock_net(sk); | |
1151 | ||
1152 | /* wait for complete transmission of current pdu */ | |
1153 | wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE); | |
1154 | ||
823b2e42 OH |
1155 | /* force state machines to be idle also when a signal occurred */ |
1156 | so->tx.state = ISOTP_IDLE; | |
1157 | so->rx.state = ISOTP_IDLE; | |
1158 | ||
8d0caedb TH |
1159 | spin_lock(&isotp_notifier_lock); |
1160 | while (isotp_busy_notifier == so) { | |
1161 | spin_unlock(&isotp_notifier_lock); | |
1162 | schedule_timeout_uninterruptible(1); | |
1163 | spin_lock(&isotp_notifier_lock); | |
1164 | } | |
1165 | list_del(&so->notifier); | |
1166 | spin_unlock(&isotp_notifier_lock); | |
e057dd3f OH |
1167 | |
1168 | lock_sock(sk); | |
1169 | ||
e057dd3f | 1170 | /* remove current filters & unregister */ |
9f39d365 | 1171 | if (so->bound && isotp_register_txecho(so)) { |
e057dd3f OH |
1172 | if (so->ifindex) { |
1173 | struct net_device *dev; | |
1174 | ||
1175 | dev = dev_get_by_index(net, so->ifindex); | |
1176 | if (dev) { | |
9f39d365 OH |
1177 | if (isotp_register_rxid(so)) |
1178 | can_rx_unregister(net, dev, so->rxid, | |
1179 | SINGLE_MASK(so->rxid), | |
1180 | isotp_rcv, sk); | |
1181 | ||
4b7fe92c OH |
1182 | can_rx_unregister(net, dev, so->txid, |
1183 | SINGLE_MASK(so->txid), | |
1184 | isotp_rcv_echo, sk); | |
e057dd3f | 1185 | dev_put(dev); |
14a4696b | 1186 | synchronize_rcu(); |
e057dd3f OH |
1187 | } |
1188 | } | |
1189 | } | |
1190 | ||
4f027cba | 1191 | hrtimer_cancel(&so->txfrtimer); |
14a4696b OH |
1192 | hrtimer_cancel(&so->txtimer); |
1193 | hrtimer_cancel(&so->rxtimer); | |
1194 | ||
e057dd3f OH |
1195 | so->ifindex = 0; |
1196 | so->bound = 0; | |
1197 | ||
1198 | sock_orphan(sk); | |
1199 | sock->sk = NULL; | |
1200 | ||
1201 | release_sock(sk); | |
1202 | sock_put(sk); | |
1203 | ||
1204 | return 0; | |
1205 | } | |
1206 | ||
1207 | static int isotp_bind(struct socket *sock, struct sockaddr *uaddr, int len) | |
1208 | { | |
1209 | struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; | |
1210 | struct sock *sk = sock->sk; | |
1211 | struct isotp_sock *so = isotp_sk(sk); | |
1212 | struct net *net = sock_net(sk); | |
1213 | int ifindex; | |
1214 | struct net_device *dev; | |
b76b163f OH |
1215 | canid_t tx_id = addr->can_addr.tp.tx_id; |
1216 | canid_t rx_id = addr->can_addr.tp.rx_id; | |
e057dd3f OH |
1217 | int err = 0; |
1218 | int notify_enetdown = 0; | |
1219 | ||
f522d955 | 1220 | if (len < ISOTP_MIN_NAMELEN) |
e057dd3f OH |
1221 | return -EINVAL; |
1222 | ||
b76b163f | 1223 | /* sanitize tx CAN identifier */ |
3ea56642 OH |
1224 | if (tx_id & CAN_EFF_FLAG) |
1225 | tx_id &= (CAN_EFF_FLAG | CAN_EFF_MASK); | |
1226 | else | |
1227 | tx_id &= CAN_SFF_MASK; | |
1228 | ||
b76b163f OH |
1229 | /* give feedback on wrong CAN-ID value */ |
1230 | if (tx_id != addr->can_addr.tp.tx_id) | |
2aa39889 OH |
1231 | return -EINVAL; |
1232 | ||
b76b163f OH |
1233 | /* sanitize rx CAN identifier (if needed) */ |
1234 | if (isotp_register_rxid(so)) { | |
1235 | if (rx_id & CAN_EFF_FLAG) | |
1236 | rx_id &= (CAN_EFF_FLAG | CAN_EFF_MASK); | |
1237 | else | |
1238 | rx_id &= CAN_SFF_MASK; | |
1239 | ||
1240 | /* give feedback on wrong CAN-ID value */ | |
1241 | if (rx_id != addr->can_addr.tp.rx_id) | |
1242 | return -EINVAL; | |
1243 | } | |
1244 | ||
2b17c400 NS |
1245 | if (!addr->can_ifindex) |
1246 | return -ENODEV; | |
1247 | ||
1248 | lock_sock(sk); | |
1249 | ||
72ed3ee9 OH |
1250 | if (so->bound) { |
1251 | err = -EINVAL; | |
1252 | goto out; | |
1253 | } | |
1254 | ||
9f39d365 OH |
1255 | /* ensure different CAN IDs when the rx_id is to be registered */ |
1256 | if (isotp_register_rxid(so) && rx_id == tx_id) { | |
3ea56642 OH |
1257 | err = -EADDRNOTAVAIL; |
1258 | goto out; | |
921ca574 | 1259 | } |
e057dd3f | 1260 | |
e057dd3f OH |
1261 | dev = dev_get_by_index(net, addr->can_ifindex); |
1262 | if (!dev) { | |
1263 | err = -ENODEV; | |
1264 | goto out; | |
1265 | } | |
1266 | if (dev->type != ARPHRD_CAN) { | |
1267 | dev_put(dev); | |
1268 | err = -ENODEV; | |
1269 | goto out; | |
1270 | } | |
1271 | if (dev->mtu < so->ll.mtu) { | |
1272 | dev_put(dev); | |
1273 | err = -EINVAL; | |
1274 | goto out; | |
1275 | } | |
1276 | if (!(dev->flags & IFF_UP)) | |
1277 | notify_enetdown = 1; | |
1278 | ||
1279 | ifindex = dev->ifindex; | |
1280 | ||
9f39d365 | 1281 | if (isotp_register_rxid(so)) |
3ea56642 | 1282 | can_rx_register(net, dev, rx_id, SINGLE_MASK(rx_id), |
921ca574 | 1283 | isotp_rcv, sk, "isotp", sk); |
e057dd3f | 1284 | |
9f39d365 | 1285 | if (isotp_register_txecho(so)) { |
4b7fe92c OH |
1286 | /* no consecutive frame echo skb in flight */ |
1287 | so->cfecho = 0; | |
1288 | ||
1289 | /* register for echo skb's */ | |
3ea56642 | 1290 | can_rx_register(net, dev, tx_id, SINGLE_MASK(tx_id), |
4b7fe92c OH |
1291 | isotp_rcv_echo, sk, "isotpe", sk); |
1292 | } | |
1293 | ||
e057dd3f OH |
1294 | dev_put(dev); |
1295 | ||
e057dd3f OH |
1296 | /* switch to new settings */ |
1297 | so->ifindex = ifindex; | |
3ea56642 OH |
1298 | so->rxid = rx_id; |
1299 | so->txid = tx_id; | |
e057dd3f OH |
1300 | so->bound = 1; |
1301 | ||
1302 | out: | |
1303 | release_sock(sk); | |
1304 | ||
1305 | if (notify_enetdown) { | |
1306 | sk->sk_err = ENETDOWN; | |
1307 | if (!sock_flag(sk, SOCK_DEAD)) | |
e3ae2365 | 1308 | sk_error_report(sk); |
e057dd3f OH |
1309 | } |
1310 | ||
1311 | return err; | |
1312 | } | |
1313 | ||
1314 | static int isotp_getname(struct socket *sock, struct sockaddr *uaddr, int peer) | |
1315 | { | |
1316 | struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; | |
1317 | struct sock *sk = sock->sk; | |
1318 | struct isotp_sock *so = isotp_sk(sk); | |
1319 | ||
1320 | if (peer) | |
1321 | return -EOPNOTSUPP; | |
1322 | ||
f522d955 | 1323 | memset(addr, 0, ISOTP_MIN_NAMELEN); |
e057dd3f OH |
1324 | addr->can_family = AF_CAN; |
1325 | addr->can_ifindex = so->ifindex; | |
1326 | addr->can_addr.tp.rx_id = so->rxid; | |
1327 | addr->can_addr.tp.tx_id = so->txid; | |
1328 | ||
f522d955 | 1329 | return ISOTP_MIN_NAMELEN; |
e057dd3f OH |
1330 | } |
1331 | ||
2b17c400 | 1332 | static int isotp_setsockopt_locked(struct socket *sock, int level, int optname, |
e057dd3f OH |
1333 | sockptr_t optval, unsigned int optlen) |
1334 | { | |
1335 | struct sock *sk = sock->sk; | |
1336 | struct isotp_sock *so = isotp_sk(sk); | |
1337 | int ret = 0; | |
1338 | ||
323a391a OH |
1339 | if (so->bound) |
1340 | return -EISCONN; | |
1341 | ||
e057dd3f OH |
1342 | switch (optname) { |
1343 | case CAN_ISOTP_OPTS: | |
1344 | if (optlen != sizeof(struct can_isotp_options)) | |
1345 | return -EINVAL; | |
1346 | ||
1347 | if (copy_from_sockptr(&so->opt, optval, optlen)) | |
1348 | return -EFAULT; | |
1349 | ||
1350 | /* no separate rx_ext_address is given => use ext_address */ | |
1351 | if (!(so->opt.flags & CAN_ISOTP_RX_EXT_ADDR)) | |
1352 | so->opt.rx_ext_address = so->opt.ext_address; | |
530e0d46 | 1353 | |
9f39d365 OH |
1354 | /* these broadcast flags are not allowed together */ |
1355 | if (isotp_bc_flags(so) == ISOTP_ALL_BC_FLAGS) { | |
1356 | /* CAN_ISOTP_SF_BROADCAST is prioritized */ | |
1357 | so->opt.flags &= ~CAN_ISOTP_CF_BROADCAST; | |
1358 | ||
1359 | /* give user feedback on wrong config attempt */ | |
1360 | ret = -EINVAL; | |
1361 | } | |
1362 | ||
530e0d46 OH |
1363 | /* check for frame_txtime changes (0 => no changes) */ |
1364 | if (so->opt.frame_txtime) { | |
1365 | if (so->opt.frame_txtime == CAN_ISOTP_FRAME_TXTIME_ZERO) | |
1366 | so->frame_txtime = 0; | |
1367 | else | |
1368 | so->frame_txtime = so->opt.frame_txtime; | |
1369 | } | |
e057dd3f OH |
1370 | break; |
1371 | ||
1372 | case CAN_ISOTP_RECV_FC: | |
1373 | if (optlen != sizeof(struct can_isotp_fc_options)) | |
1374 | return -EINVAL; | |
1375 | ||
1376 | if (copy_from_sockptr(&so->rxfc, optval, optlen)) | |
1377 | return -EFAULT; | |
1378 | break; | |
1379 | ||
1380 | case CAN_ISOTP_TX_STMIN: | |
1381 | if (optlen != sizeof(u32)) | |
1382 | return -EINVAL; | |
1383 | ||
1384 | if (copy_from_sockptr(&so->force_tx_stmin, optval, optlen)) | |
1385 | return -EFAULT; | |
1386 | break; | |
1387 | ||
1388 | case CAN_ISOTP_RX_STMIN: | |
1389 | if (optlen != sizeof(u32)) | |
1390 | return -EINVAL; | |
1391 | ||
1392 | if (copy_from_sockptr(&so->force_rx_stmin, optval, optlen)) | |
1393 | return -EFAULT; | |
1394 | break; | |
1395 | ||
1396 | case CAN_ISOTP_LL_OPTS: | |
1397 | if (optlen == sizeof(struct can_isotp_ll_options)) { | |
1398 | struct can_isotp_ll_options ll; | |
1399 | ||
1400 | if (copy_from_sockptr(&ll, optval, optlen)) | |
1401 | return -EFAULT; | |
1402 | ||
1403 | /* check for correct ISO 11898-1 DLC data length */ | |
1404 | if (ll.tx_dl != padlen(ll.tx_dl)) | |
1405 | return -EINVAL; | |
1406 | ||
1407 | if (ll.mtu != CAN_MTU && ll.mtu != CANFD_MTU) | |
1408 | return -EINVAL; | |
1409 | ||
e4912459 MKB |
1410 | if (ll.mtu == CAN_MTU && |
1411 | (ll.tx_dl > CAN_MAX_DLEN || ll.tx_flags != 0)) | |
e057dd3f OH |
1412 | return -EINVAL; |
1413 | ||
1414 | memcpy(&so->ll, &ll, sizeof(ll)); | |
1415 | ||
1416 | /* set ll_dl for tx path to similar place as for rx */ | |
1417 | so->tx.ll_dl = ll.tx_dl; | |
1418 | } else { | |
1419 | return -EINVAL; | |
1420 | } | |
1421 | break; | |
1422 | ||
1423 | default: | |
1424 | ret = -ENOPROTOOPT; | |
1425 | } | |
1426 | ||
1427 | return ret; | |
1428 | } | |
1429 | ||
2b17c400 NS |
1430 | static int isotp_setsockopt(struct socket *sock, int level, int optname, |
1431 | sockptr_t optval, unsigned int optlen) | |
1432 | ||
1433 | { | |
1434 | struct sock *sk = sock->sk; | |
1435 | int ret; | |
1436 | ||
1437 | if (level != SOL_CAN_ISOTP) | |
1438 | return -EINVAL; | |
1439 | ||
1440 | lock_sock(sk); | |
1441 | ret = isotp_setsockopt_locked(sock, level, optname, optval, optlen); | |
1442 | release_sock(sk); | |
1443 | return ret; | |
1444 | } | |
1445 | ||
e057dd3f OH |
1446 | static int isotp_getsockopt(struct socket *sock, int level, int optname, |
1447 | char __user *optval, int __user *optlen) | |
1448 | { | |
1449 | struct sock *sk = sock->sk; | |
1450 | struct isotp_sock *so = isotp_sk(sk); | |
1451 | int len; | |
1452 | void *val; | |
1453 | ||
1454 | if (level != SOL_CAN_ISOTP) | |
1455 | return -EINVAL; | |
1456 | if (get_user(len, optlen)) | |
1457 | return -EFAULT; | |
1458 | if (len < 0) | |
1459 | return -EINVAL; | |
1460 | ||
1461 | switch (optname) { | |
1462 | case CAN_ISOTP_OPTS: | |
1463 | len = min_t(int, len, sizeof(struct can_isotp_options)); | |
1464 | val = &so->opt; | |
1465 | break; | |
1466 | ||
1467 | case CAN_ISOTP_RECV_FC: | |
1468 | len = min_t(int, len, sizeof(struct can_isotp_fc_options)); | |
1469 | val = &so->rxfc; | |
1470 | break; | |
1471 | ||
1472 | case CAN_ISOTP_TX_STMIN: | |
1473 | len = min_t(int, len, sizeof(u32)); | |
1474 | val = &so->force_tx_stmin; | |
1475 | break; | |
1476 | ||
1477 | case CAN_ISOTP_RX_STMIN: | |
1478 | len = min_t(int, len, sizeof(u32)); | |
1479 | val = &so->force_rx_stmin; | |
1480 | break; | |
1481 | ||
1482 | case CAN_ISOTP_LL_OPTS: | |
1483 | len = min_t(int, len, sizeof(struct can_isotp_ll_options)); | |
1484 | val = &so->ll; | |
1485 | break; | |
1486 | ||
1487 | default: | |
1488 | return -ENOPROTOOPT; | |
1489 | } | |
1490 | ||
1491 | if (put_user(len, optlen)) | |
1492 | return -EFAULT; | |
1493 | if (copy_to_user(optval, val, len)) | |
1494 | return -EFAULT; | |
1495 | return 0; | |
1496 | } | |
1497 | ||
8d0caedb TH |
1498 | static void isotp_notify(struct isotp_sock *so, unsigned long msg, |
1499 | struct net_device *dev) | |
e057dd3f | 1500 | { |
e057dd3f OH |
1501 | struct sock *sk = &so->sk; |
1502 | ||
1503 | if (!net_eq(dev_net(dev), sock_net(sk))) | |
8d0caedb | 1504 | return; |
e057dd3f OH |
1505 | |
1506 | if (so->ifindex != dev->ifindex) | |
8d0caedb | 1507 | return; |
e057dd3f OH |
1508 | |
1509 | switch (msg) { | |
1510 | case NETDEV_UNREGISTER: | |
1511 | lock_sock(sk); | |
1512 | /* remove current filters & unregister */ | |
9f39d365 OH |
1513 | if (so->bound && isotp_register_txecho(so)) { |
1514 | if (isotp_register_rxid(so)) | |
1515 | can_rx_unregister(dev_net(dev), dev, so->rxid, | |
1516 | SINGLE_MASK(so->rxid), | |
1517 | isotp_rcv, sk); | |
1518 | ||
4b7fe92c OH |
1519 | can_rx_unregister(dev_net(dev), dev, so->txid, |
1520 | SINGLE_MASK(so->txid), | |
1521 | isotp_rcv_echo, sk); | |
1522 | } | |
e057dd3f OH |
1523 | |
1524 | so->ifindex = 0; | |
1525 | so->bound = 0; | |
1526 | release_sock(sk); | |
1527 | ||
1528 | sk->sk_err = ENODEV; | |
1529 | if (!sock_flag(sk, SOCK_DEAD)) | |
e3ae2365 | 1530 | sk_error_report(sk); |
e057dd3f OH |
1531 | break; |
1532 | ||
1533 | case NETDEV_DOWN: | |
1534 | sk->sk_err = ENETDOWN; | |
1535 | if (!sock_flag(sk, SOCK_DEAD)) | |
e3ae2365 | 1536 | sk_error_report(sk); |
e057dd3f OH |
1537 | break; |
1538 | } | |
8d0caedb | 1539 | } |
e057dd3f | 1540 | |
8d0caedb TH |
1541 | static int isotp_notifier(struct notifier_block *nb, unsigned long msg, |
1542 | void *ptr) | |
1543 | { | |
1544 | struct net_device *dev = netdev_notifier_info_to_dev(ptr); | |
1545 | ||
1546 | if (dev->type != ARPHRD_CAN) | |
1547 | return NOTIFY_DONE; | |
1548 | if (msg != NETDEV_UNREGISTER && msg != NETDEV_DOWN) | |
1549 | return NOTIFY_DONE; | |
1550 | if (unlikely(isotp_busy_notifier)) /* Check for reentrant bug. */ | |
1551 | return NOTIFY_DONE; | |
1552 | ||
1553 | spin_lock(&isotp_notifier_lock); | |
1554 | list_for_each_entry(isotp_busy_notifier, &isotp_notifier_list, notifier) { | |
1555 | spin_unlock(&isotp_notifier_lock); | |
1556 | isotp_notify(isotp_busy_notifier, msg, dev); | |
1557 | spin_lock(&isotp_notifier_lock); | |
1558 | } | |
1559 | isotp_busy_notifier = NULL; | |
1560 | spin_unlock(&isotp_notifier_lock); | |
e057dd3f OH |
1561 | return NOTIFY_DONE; |
1562 | } | |
1563 | ||
1564 | static int isotp_init(struct sock *sk) | |
1565 | { | |
1566 | struct isotp_sock *so = isotp_sk(sk); | |
1567 | ||
1568 | so->ifindex = 0; | |
1569 | so->bound = 0; | |
1570 | ||
1571 | so->opt.flags = CAN_ISOTP_DEFAULT_FLAGS; | |
1572 | so->opt.ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS; | |
1573 | so->opt.rx_ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS; | |
1574 | so->opt.rxpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT; | |
1575 | so->opt.txpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT; | |
1576 | so->opt.frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME; | |
530e0d46 | 1577 | so->frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME; |
e057dd3f OH |
1578 | so->rxfc.bs = CAN_ISOTP_DEFAULT_RECV_BS; |
1579 | so->rxfc.stmin = CAN_ISOTP_DEFAULT_RECV_STMIN; | |
1580 | so->rxfc.wftmax = CAN_ISOTP_DEFAULT_RECV_WFTMAX; | |
1581 | so->ll.mtu = CAN_ISOTP_DEFAULT_LL_MTU; | |
1582 | so->ll.tx_dl = CAN_ISOTP_DEFAULT_LL_TX_DL; | |
1583 | so->ll.tx_flags = CAN_ISOTP_DEFAULT_LL_TX_FLAGS; | |
1584 | ||
1585 | /* set ll_dl for tx path to similar place as for rx */ | |
1586 | so->tx.ll_dl = so->ll.tx_dl; | |
1587 | ||
1588 | so->rx.state = ISOTP_IDLE; | |
1589 | so->tx.state = ISOTP_IDLE; | |
1590 | ||
1591 | hrtimer_init(&so->rxtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT); | |
1592 | so->rxtimer.function = isotp_rx_timer_handler; | |
1593 | hrtimer_init(&so->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT); | |
1594 | so->txtimer.function = isotp_tx_timer_handler; | |
4f027cba OH |
1595 | hrtimer_init(&so->txfrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT); |
1596 | so->txfrtimer.function = isotp_txfr_timer_handler; | |
e057dd3f OH |
1597 | |
1598 | init_waitqueue_head(&so->wait); | |
7c759040 | 1599 | spin_lock_init(&so->rx_lock); |
e057dd3f | 1600 | |
8d0caedb TH |
1601 | spin_lock(&isotp_notifier_lock); |
1602 | list_add_tail(&so->notifier, &isotp_notifier_list); | |
1603 | spin_unlock(&isotp_notifier_lock); | |
e057dd3f OH |
1604 | |
1605 | return 0; | |
1606 | } | |
1607 | ||
1608 | static int isotp_sock_no_ioctlcmd(struct socket *sock, unsigned int cmd, | |
1609 | unsigned long arg) | |
1610 | { | |
1611 | /* no ioctls for socket layer -> hand it down to NIC layer */ | |
1612 | return -ENOIOCTLCMD; | |
1613 | } | |
1614 | ||
1615 | static const struct proto_ops isotp_ops = { | |
1616 | .family = PF_CAN, | |
1617 | .release = isotp_release, | |
1618 | .bind = isotp_bind, | |
1619 | .connect = sock_no_connect, | |
1620 | .socketpair = sock_no_socketpair, | |
1621 | .accept = sock_no_accept, | |
1622 | .getname = isotp_getname, | |
1623 | .poll = datagram_poll, | |
1624 | .ioctl = isotp_sock_no_ioctlcmd, | |
1625 | .gettstamp = sock_gettstamp, | |
1626 | .listen = sock_no_listen, | |
1627 | .shutdown = sock_no_shutdown, | |
1628 | .setsockopt = isotp_setsockopt, | |
1629 | .getsockopt = isotp_getsockopt, | |
1630 | .sendmsg = isotp_sendmsg, | |
1631 | .recvmsg = isotp_recvmsg, | |
1632 | .mmap = sock_no_mmap, | |
1633 | .sendpage = sock_no_sendpage, | |
1634 | }; | |
1635 | ||
1636 | static struct proto isotp_proto __read_mostly = { | |
1637 | .name = "CAN_ISOTP", | |
1638 | .owner = THIS_MODULE, | |
1639 | .obj_size = sizeof(struct isotp_sock), | |
1640 | .init = isotp_init, | |
1641 | }; | |
1642 | ||
1643 | static const struct can_proto isotp_can_proto = { | |
1644 | .type = SOCK_DGRAM, | |
1645 | .protocol = CAN_ISOTP, | |
1646 | .ops = &isotp_ops, | |
1647 | .prot = &isotp_proto, | |
1648 | }; | |
1649 | ||
8d0caedb TH |
1650 | static struct notifier_block canisotp_notifier = { |
1651 | .notifier_call = isotp_notifier | |
1652 | }; | |
1653 | ||
e057dd3f OH |
1654 | static __init int isotp_module_init(void) |
1655 | { | |
1656 | int err; | |
1657 | ||
f726f3d3 | 1658 | pr_info("can: isotp protocol\n"); |
e057dd3f OH |
1659 | |
1660 | err = can_proto_register(&isotp_can_proto); | |
1661 | if (err < 0) | |
6a5ddae5 | 1662 | pr_err("can: registration of isotp protocol failed %pe\n", ERR_PTR(err)); |
8d0caedb TH |
1663 | else |
1664 | register_netdevice_notifier(&canisotp_notifier); | |
e057dd3f OH |
1665 | |
1666 | return err; | |
1667 | } | |
1668 | ||
1669 | static __exit void isotp_module_exit(void) | |
1670 | { | |
1671 | can_proto_unregister(&isotp_can_proto); | |
8d0caedb | 1672 | unregister_netdevice_notifier(&canisotp_notifier); |
e057dd3f OH |
1673 | } |
1674 | ||
1675 | module_init(isotp_module_init); | |
1676 | module_exit(isotp_module_exit); |