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') 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> 58 #include <linux/spinlock.h> 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 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 80 #define ISOTP_MIN_NAMELEN CAN_REQUIRED_SIZE(struct sockaddr_can, can_addr.tp) 81 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 set the static buffer to 89 * something about 8 kbyte to be able to test this new functionality. 90 */ 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) ")"); 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) 118 #define ISOTP_ALL_BC_FLAGS (CAN_ISOTP_SF_BROADCAST | CAN_ISOTP_CF_BROADCAST) 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 125 #define ISOTP_FC_TIMEOUT 1 /* 1 sec */ 126 #define ISOTP_ECHO_TIMEOUT 2 /* 2 secs */ 127 128 enum { 129 ISOTP_IDLE = 0, 130 ISOTP_WAIT_FIRST_FC, 131 ISOTP_WAIT_FC, 132 ISOTP_WAIT_DATA, 133 ISOTP_SENDING, 134 ISOTP_SHUTDOWN, 135 }; 136 137 struct tpcon { 138 u8 *buf; 139 unsigned int buflen; 140 unsigned int len; 141 unsigned int idx; 142 u32 state; 143 u8 bs; 144 u8 sn; 145 u8 ll_dl; 146 u8 sbuf[DEFAULT_MAX_PDU_SIZE]; 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; 157 struct hrtimer rxtimer, txtimer, txfrtimer; 158 struct can_isotp_options opt; 159 struct can_isotp_fc_options rxfc, txfc; 160 struct can_isotp_ll_options ll; 161 u32 frame_txtime; 162 u32 force_tx_stmin; 163 u32 force_rx_stmin; 164 u32 cfecho; /* consecutive frame echo tag */ 165 struct tpcon rx, tx; 166 struct list_head notifier; 167 wait_queue_head_t wait; 168 spinlock_t rx_lock; /* protect single thread state machine */ 169 }; 170 171 static LIST_HEAD(isotp_notifier_list); 172 static DEFINE_SPINLOCK(isotp_notifier_lock); 173 static struct isotp_sock *isotp_busy_notifier; 174 175 static inline struct isotp_sock *isotp_sk(const struct sock *sk) 176 { 177 return (struct isotp_sock *)sk; 178 } 179 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 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)) 203 sk_error_report(sk); 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; 237 skb_put_zero(nskb, so->ll.mtu); 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 256 ncf->flags = so->ll.tx_flags; 257 258 can_send_ret = can_send(nskb, 1); 259 if (can_send_ret) 260 pr_notice_once("can-isotp: %s: can_send_ret %pe\n", 261 __func__, ERR_PTR(can_send_ret)); 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 */ 272 hrtimer_start(&so->rxtimer, ktime_set(ISOTP_FC_TIMEOUT, 0), 273 HRTIMER_MODE_REL_SOFT); 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 { 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 }; 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 359 static void isotp_send_cframe(struct isotp_sock *so); 360 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)) 377 sk_error_report(sk); 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 */ 396 so->tx_gap = ktime_add_ns(so->tx_gap, so->frame_txtime); 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; 415 /* send CF frame and enable echo timeout handling */ 416 hrtimer_start(&so->txtimer, ktime_set(ISOTP_ECHO_TIMEOUT, 0), 417 HRTIMER_MODE_REL_SOFT); 418 isotp_send_cframe(so); 419 break; 420 421 case ISOTP_FC_WT: 422 /* start timer to wait for next FC frame */ 423 hrtimer_start(&so->txtimer, ktime_set(ISOTP_FC_TIMEOUT, 0), 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)) 431 sk_error_report(sk); 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)) 459 sk_error_report(sk); 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 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) { 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)) 584 sk_error_report(sk); 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)) 608 sk_error_report(sk); 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 625 /* perform blocksize handling, if enabled */ 626 if (!so->rxfc.bs || ++so->rx.bs < so->rxfc.bs) { 627 /* start rx timeout watchdog */ 628 hrtimer_start(&so->rxtimer, ktime_set(ISOTP_FC_TIMEOUT, 0), 629 HRTIMER_MODE_REL_SOFT); 630 return 0; 631 } 632 633 /* no creation of flow control frames */ 634 if (so->opt.flags & CAN_ISOTP_LISTEN_MODE) 635 return 0; 636 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 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 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)) 674 goto out_unlock; 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 { 697 if (can_is_canfd_skb(skb)) { 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 } 723 724 out_unlock: 725 spin_unlock(&so->rx_lock); 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 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 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 */ 828 if (so->tx.len > MAX_12BIT_PDU_SIZE) { 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; 849 } 850 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 857 /* only handle my own local echo CF/SF skb's (no FF!) */ 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; 877 hrtimer_start(&so->txtimer, ktime_set(ISOTP_FC_TIMEOUT, 0), 878 HRTIMER_MODE_REL_SOFT); 879 return; 880 } 881 882 /* no gap between data frames needed => use burst mode */ 883 if (!so->tx_gap) { 884 /* enable echo timeout handling */ 885 hrtimer_start(&so->txtimer, ktime_set(ISOTP_ECHO_TIMEOUT, 0), 886 HRTIMER_MODE_REL_SOFT); 887 isotp_send_cframe(so); 888 return; 889 } 890 891 /* start timer to send next consecutive frame with correct delay */ 892 hrtimer_start(&so->txfrtimer, so->tx_gap, HRTIMER_MODE_REL_SOFT); 893 } 894 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; 900 901 /* don't handle timeouts in IDLE or SHUTDOWN state */ 902 if (so->tx.state == ISOTP_IDLE || so->tx.state == ISOTP_SHUTDOWN) 903 return HRTIMER_NORESTART; 904 905 /* we did not get any flow control or echo frame in time */ 906 907 /* report 'communication error on send' */ 908 sk->sk_err = ECOMM; 909 if (!sock_flag(sk, SOCK_DEAD)) 910 sk_error_report(sk); 911 912 /* reset tx state */ 913 so->tx.state = ISOTP_IDLE; 914 wake_up_interruptible(&so->wait); 915 916 return HRTIMER_NORESTART; 917 } 918 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); 923 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); 927 928 /* cfecho should be consumed by isotp_rcv_echo() here */ 929 if (so->tx.state == ISOTP_SENDING && !so->cfecho) 930 isotp_send_cframe(so); 931 932 return HRTIMER_NORESTART; 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; 944 s64 hrtimer_sec = ISOTP_ECHO_TIMEOUT; 945 int off; 946 int err; 947 948 if (!so->bound || so->tx.state == ISOTP_SHUTDOWN) 949 return -EADDRNOTAVAIL; 950 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; 955 956 if (so->tx.state == ISOTP_SHUTDOWN) 957 return -EADDRNOTAVAIL; 958 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; 963 } 964 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) { 976 err = -EINVAL; 977 goto err_out_drop; 978 } 979 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? */ 984 if ((isotp_bc_flags(so) == CAN_ISOTP_SF_BROADCAST) && 985 (size > so->tx.ll_dl - SF_PCI_SZ4 - ae - off)) { 986 err = -EINVAL; 987 goto err_out_drop; 988 } 989 990 err = memcpy_from_msg(so->tx.buf, msg, size); 991 if (err < 0) 992 goto err_out_drop; 993 994 dev = dev_get_by_index(sock_net(sk), so->ifindex); 995 if (!dev) { 996 err = -ENXIO; 997 goto err_out_drop; 998 } 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); 1004 goto err_out_drop; 1005 } 1006 1007 can_skb_reserve(skb); 1008 can_skb_prv(skb)->ifindex = dev->ifindex; 1009 can_skb_prv(skb)->skbcnt = 0; 1010 1011 so->tx.len = size; 1012 so->tx.idx = 0; 1013 1014 cf = (struct canfd_frame *)skb->data; 1015 skb_put_zero(skb, so->ll.mtu); 1016 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 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 1046 /* set CF echo tag for isotp_rcv_echo() (SF-mode) */ 1047 so->cfecho = *(u32 *)cf->data; 1048 } else { 1049 /* send first frame */ 1050 1051 isotp_create_fframe(cf, so, ae); 1052 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 1063 /* set CF echo tag for isotp_rcv_echo() (CF-mode) */ 1064 so->cfecho = *(u32 *)cf->data; 1065 } else { 1066 /* standard flow control check */ 1067 so->tx.state = ISOTP_WAIT_FIRST_FC; 1068 1069 /* start timeout for FC */ 1070 hrtimer_sec = ISOTP_FC_TIMEOUT; 1071 1072 /* no CF echo tag for isotp_rcv_echo() (FF-mode) */ 1073 so->cfecho = 0; 1074 } 1075 } 1076 1077 hrtimer_start(&so->txtimer, ktime_set(hrtimer_sec, 0), 1078 HRTIMER_MODE_REL_SOFT); 1079 1080 /* send the first or only CAN frame */ 1081 cf->flags = so->ll.tx_flags; 1082 1083 skb->dev = dev; 1084 skb->sk = sk; 1085 err = can_send(skb, 1); 1086 dev_put(dev); 1087 if (err) { 1088 pr_notice_once("can-isotp: %s: can_send_ret %pe\n", 1089 __func__, ERR_PTR(err)); 1090 1091 /* no transmission -> no timeout monitoring */ 1092 hrtimer_cancel(&so->txtimer); 1093 1094 /* reset consecutive frame echo tag */ 1095 so->cfecho = 0; 1096 1097 goto err_out_drop; 1098 } 1099 1100 if (wait_tx_done) { 1101 /* wait for complete transmission of current pdu */ 1102 err = wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE); 1103 if (err) 1104 goto err_event_drop; 1105 1106 err = sock_error(sk); 1107 if (err) 1108 return err; 1109 } 1110 1111 return size; 1112 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); 1118 err_out_drop: 1119 /* drop this PDU and unlock a potential wait queue */ 1120 so->tx.state = ISOTP_IDLE; 1121 wake_up_interruptible(&so->wait); 1122 1123 return err; 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; 1131 struct isotp_sock *so = isotp_sk(sk); 1132 int ret = 0; 1133 1134 if (flags & ~(MSG_DONTWAIT | MSG_TRUNC | MSG_PEEK | MSG_CMSG_COMPAT)) 1135 return -EINVAL; 1136 1137 if (!so->bound) 1138 return -EADDRNOTAVAIL; 1139 1140 skb = skb_recv_datagram(sk, flags, &ret); 1141 if (!skb) 1142 return ret; 1143 1144 if (size < skb->len) 1145 msg->msg_flags |= MSG_TRUNC; 1146 else 1147 size = skb->len; 1148 1149 ret = memcpy_to_msg(msg, skb->data, size); 1150 if (ret < 0) 1151 goto out_err; 1152 1153 sock_recv_cmsgs(msg, sk, skb); 1154 1155 if (msg->msg_name) { 1156 __sockaddr_check_size(ISOTP_MIN_NAMELEN); 1157 msg->msg_namelen = ISOTP_MIN_NAMELEN; 1158 memcpy(msg->msg_name, skb->cb, msg->msg_namelen); 1159 } 1160 1161 /* set length of return value */ 1162 ret = (flags & MSG_TRUNC) ? skb->len : size; 1163 1164 out_err: 1165 skb_free_datagram(sk, skb); 1166 1167 return ret; 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 */ 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 ; 1186 1187 /* force state machines to be idle also when a signal occurred */ 1188 so->tx.state = ISOTP_SHUTDOWN; 1189 so->rx.state = ISOTP_IDLE; 1190 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); 1199 1200 lock_sock(sk); 1201 1202 /* remove current filters & unregister */ 1203 if (so->bound) { 1204 if (so->ifindex) { 1205 struct net_device *dev; 1206 1207 dev = dev_get_by_index(net, so->ifindex); 1208 if (dev) { 1209 if (isotp_register_rxid(so)) 1210 can_rx_unregister(net, dev, so->rxid, 1211 SINGLE_MASK(so->rxid), 1212 isotp_rcv, sk); 1213 1214 can_rx_unregister(net, dev, so->txid, 1215 SINGLE_MASK(so->txid), 1216 isotp_rcv_echo, sk); 1217 dev_put(dev); 1218 synchronize_rcu(); 1219 } 1220 } 1221 } 1222 1223 hrtimer_cancel(&so->txfrtimer); 1224 hrtimer_cancel(&so->txtimer); 1225 hrtimer_cancel(&so->rxtimer); 1226 1227 so->ifindex = 0; 1228 so->bound = 0; 1229 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 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; 1253 canid_t tx_id = addr->can_addr.tp.tx_id; 1254 canid_t rx_id = addr->can_addr.tp.rx_id; 1255 int err = 0; 1256 int notify_enetdown = 0; 1257 1258 if (len < ISOTP_MIN_NAMELEN) 1259 return -EINVAL; 1260 1261 if (addr->can_family != AF_CAN) 1262 return -EINVAL; 1263 1264 /* sanitize tx CAN identifier */ 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 1270 /* give feedback on wrong CAN-ID value */ 1271 if (tx_id != addr->can_addr.tp.tx_id) 1272 return -EINVAL; 1273 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 1286 if (!addr->can_ifindex) 1287 return -ENODEV; 1288 1289 lock_sock(sk); 1290 1291 if (so->bound) { 1292 err = -EINVAL; 1293 goto out; 1294 } 1295 1296 /* ensure different CAN IDs when the rx_id is to be registered */ 1297 if (isotp_register_rxid(so) && rx_id == tx_id) { 1298 err = -EADDRNOTAVAIL; 1299 goto out; 1300 } 1301 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 1322 if (isotp_register_rxid(so)) 1323 can_rx_register(net, dev, rx_id, SINGLE_MASK(rx_id), 1324 isotp_rcv, sk, "isotp", sk); 1325 1326 /* no consecutive frame echo skb in flight */ 1327 so->cfecho = 0; 1328 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); 1332 1333 dev_put(dev); 1334 1335 /* switch to new settings */ 1336 so->ifindex = ifindex; 1337 so->rxid = rx_id; 1338 so->txid = tx_id; 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)) 1347 sk_error_report(sk); 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 1362 memset(addr, 0, ISOTP_MIN_NAMELEN); 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 1368 return ISOTP_MIN_NAMELEN; 1369 } 1370 1371 static int isotp_setsockopt_locked(struct socket *sock, int level, int optname, 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 1378 if (so->bound) 1379 return -EISCONN; 1380 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; 1392 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 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 } 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 1449 if (ll.mtu == CAN_MTU && 1450 (ll.tx_dl > CAN_MAX_DLEN || ll.tx_flags != 0)) 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 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 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 1537 static void isotp_notify(struct isotp_sock *so, unsigned long msg, 1538 struct net_device *dev) 1539 { 1540 struct sock *sk = &so->sk; 1541 1542 if (!net_eq(dev_net(dev), sock_net(sk))) 1543 return; 1544 1545 if (so->ifindex != dev->ifindex) 1546 return; 1547 1548 switch (msg) { 1549 case NETDEV_UNREGISTER: 1550 lock_sock(sk); 1551 /* remove current filters & unregister */ 1552 if (so->bound) { 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 1558 can_rx_unregister(dev_net(dev), dev, so->txid, 1559 SINGLE_MASK(so->txid), 1560 isotp_rcv_echo, sk); 1561 } 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)) 1569 sk_error_report(sk); 1570 break; 1571 1572 case NETDEV_DOWN: 1573 sk->sk_err = ENETDOWN; 1574 if (!sock_flag(sk, SOCK_DEAD)) 1575 sk_error_report(sk); 1576 break; 1577 } 1578 } 1579 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); 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; 1616 so->frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME; 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 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 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; 1639 hrtimer_init(&so->txfrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT); 1640 so->txfrtimer.function = isotp_txfr_timer_handler; 1641 1642 init_waitqueue_head(&so->wait); 1643 spin_lock_init(&so->rx_lock); 1644 1645 spin_lock(&isotp_notifier_lock); 1646 list_add_tail(&so->notifier, &isotp_notifier_list); 1647 spin_unlock(&isotp_notifier_lock); 1648 1649 return 0; 1650 } 1651 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 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, 1682 .poll = isotp_poll, 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, 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 1708 static struct notifier_block canisotp_notifier = { 1709 .notifier_call = isotp_notifier 1710 }; 1711 1712 static __init int isotp_module_init(void) 1713 { 1714 int err; 1715 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); 1720 1721 err = can_proto_register(&isotp_can_proto); 1722 if (err < 0) 1723 pr_err("can: registration of isotp protocol failed %pe\n", ERR_PTR(err)); 1724 else 1725 register_netdevice_notifier(&canisotp_notifier); 1726 1727 return err; 1728 } 1729 1730 static __exit void isotp_module_exit(void) 1731 { 1732 can_proto_unregister(&isotp_can_proto); 1733 unregister_netdevice_notifier(&canisotp_notifier); 1734 } 1735 1736 module_init(isotp_module_init); 1737 module_exit(isotp_module_exit); 1738