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