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