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 if (sk->sk_err) 1116 return -sk->sk_err; 1117 } 1118 1119 return size; 1120 1121 err_event_drop: 1122 /* got signal: force tx state machine to be idle */ 1123 so->tx.state = ISOTP_IDLE; 1124 hrtimer_cancel(&so->txfrtimer); 1125 hrtimer_cancel(&so->txtimer); 1126 err_out_drop: 1127 /* drop this PDU and unlock a potential wait queue */ 1128 so->tx.state = ISOTP_IDLE; 1129 wake_up_interruptible(&so->wait); 1130 1131 return err; 1132 } 1133 1134 static int isotp_recvmsg(struct socket *sock, struct msghdr *msg, size_t size, 1135 int flags) 1136 { 1137 struct sock *sk = sock->sk; 1138 struct sk_buff *skb; 1139 struct isotp_sock *so = isotp_sk(sk); 1140 int ret = 0; 1141 1142 if (flags & ~(MSG_DONTWAIT | MSG_TRUNC | MSG_PEEK)) 1143 return -EINVAL; 1144 1145 if (!so->bound) 1146 return -EADDRNOTAVAIL; 1147 1148 skb = skb_recv_datagram(sk, flags, &ret); 1149 if (!skb) 1150 return ret; 1151 1152 if (size < skb->len) 1153 msg->msg_flags |= MSG_TRUNC; 1154 else 1155 size = skb->len; 1156 1157 ret = memcpy_to_msg(msg, skb->data, size); 1158 if (ret < 0) 1159 goto out_err; 1160 1161 sock_recv_cmsgs(msg, sk, skb); 1162 1163 if (msg->msg_name) { 1164 __sockaddr_check_size(ISOTP_MIN_NAMELEN); 1165 msg->msg_namelen = ISOTP_MIN_NAMELEN; 1166 memcpy(msg->msg_name, skb->cb, msg->msg_namelen); 1167 } 1168 1169 /* set length of return value */ 1170 ret = (flags & MSG_TRUNC) ? skb->len : size; 1171 1172 out_err: 1173 skb_free_datagram(sk, skb); 1174 1175 return ret; 1176 } 1177 1178 static int isotp_release(struct socket *sock) 1179 { 1180 struct sock *sk = sock->sk; 1181 struct isotp_sock *so; 1182 struct net *net; 1183 1184 if (!sk) 1185 return 0; 1186 1187 so = isotp_sk(sk); 1188 net = sock_net(sk); 1189 1190 /* wait for complete transmission of current pdu */ 1191 while (wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE) == 0 && 1192 cmpxchg(&so->tx.state, ISOTP_IDLE, ISOTP_SHUTDOWN) != ISOTP_IDLE) 1193 ; 1194 1195 /* force state machines to be idle also when a signal occurred */ 1196 so->tx.state = ISOTP_SHUTDOWN; 1197 so->rx.state = ISOTP_IDLE; 1198 1199 spin_lock(&isotp_notifier_lock); 1200 while (isotp_busy_notifier == so) { 1201 spin_unlock(&isotp_notifier_lock); 1202 schedule_timeout_uninterruptible(1); 1203 spin_lock(&isotp_notifier_lock); 1204 } 1205 list_del(&so->notifier); 1206 spin_unlock(&isotp_notifier_lock); 1207 1208 lock_sock(sk); 1209 1210 /* remove current filters & unregister */ 1211 if (so->bound && isotp_register_txecho(so)) { 1212 if (so->ifindex) { 1213 struct net_device *dev; 1214 1215 dev = dev_get_by_index(net, so->ifindex); 1216 if (dev) { 1217 if (isotp_register_rxid(so)) 1218 can_rx_unregister(net, dev, so->rxid, 1219 SINGLE_MASK(so->rxid), 1220 isotp_rcv, sk); 1221 1222 can_rx_unregister(net, dev, so->txid, 1223 SINGLE_MASK(so->txid), 1224 isotp_rcv_echo, sk); 1225 dev_put(dev); 1226 synchronize_rcu(); 1227 } 1228 } 1229 } 1230 1231 hrtimer_cancel(&so->txfrtimer); 1232 hrtimer_cancel(&so->txtimer); 1233 hrtimer_cancel(&so->rxtimer); 1234 1235 so->ifindex = 0; 1236 so->bound = 0; 1237 1238 if (so->rx.buf != so->rx.sbuf) 1239 kfree(so->rx.buf); 1240 1241 if (so->tx.buf != so->tx.sbuf) 1242 kfree(so->tx.buf); 1243 1244 sock_orphan(sk); 1245 sock->sk = NULL; 1246 1247 release_sock(sk); 1248 sock_put(sk); 1249 1250 return 0; 1251 } 1252 1253 static int isotp_bind(struct socket *sock, struct sockaddr *uaddr, int len) 1254 { 1255 struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; 1256 struct sock *sk = sock->sk; 1257 struct isotp_sock *so = isotp_sk(sk); 1258 struct net *net = sock_net(sk); 1259 int ifindex; 1260 struct net_device *dev; 1261 canid_t tx_id = addr->can_addr.tp.tx_id; 1262 canid_t rx_id = addr->can_addr.tp.rx_id; 1263 int err = 0; 1264 int notify_enetdown = 0; 1265 1266 if (len < ISOTP_MIN_NAMELEN) 1267 return -EINVAL; 1268 1269 if (addr->can_family != AF_CAN) 1270 return -EINVAL; 1271 1272 /* sanitize tx CAN identifier */ 1273 if (tx_id & CAN_EFF_FLAG) 1274 tx_id &= (CAN_EFF_FLAG | CAN_EFF_MASK); 1275 else 1276 tx_id &= CAN_SFF_MASK; 1277 1278 /* give feedback on wrong CAN-ID value */ 1279 if (tx_id != addr->can_addr.tp.tx_id) 1280 return -EINVAL; 1281 1282 /* sanitize rx CAN identifier (if needed) */ 1283 if (isotp_register_rxid(so)) { 1284 if (rx_id & CAN_EFF_FLAG) 1285 rx_id &= (CAN_EFF_FLAG | CAN_EFF_MASK); 1286 else 1287 rx_id &= CAN_SFF_MASK; 1288 1289 /* give feedback on wrong CAN-ID value */ 1290 if (rx_id != addr->can_addr.tp.rx_id) 1291 return -EINVAL; 1292 } 1293 1294 if (!addr->can_ifindex) 1295 return -ENODEV; 1296 1297 lock_sock(sk); 1298 1299 if (so->bound) { 1300 err = -EINVAL; 1301 goto out; 1302 } 1303 1304 /* ensure different CAN IDs when the rx_id is to be registered */ 1305 if (isotp_register_rxid(so) && rx_id == tx_id) { 1306 err = -EADDRNOTAVAIL; 1307 goto out; 1308 } 1309 1310 dev = dev_get_by_index(net, addr->can_ifindex); 1311 if (!dev) { 1312 err = -ENODEV; 1313 goto out; 1314 } 1315 if (dev->type != ARPHRD_CAN) { 1316 dev_put(dev); 1317 err = -ENODEV; 1318 goto out; 1319 } 1320 if (dev->mtu < so->ll.mtu) { 1321 dev_put(dev); 1322 err = -EINVAL; 1323 goto out; 1324 } 1325 if (!(dev->flags & IFF_UP)) 1326 notify_enetdown = 1; 1327 1328 ifindex = dev->ifindex; 1329 1330 if (isotp_register_rxid(so)) 1331 can_rx_register(net, dev, rx_id, SINGLE_MASK(rx_id), 1332 isotp_rcv, sk, "isotp", sk); 1333 1334 if (isotp_register_txecho(so)) { 1335 /* no consecutive frame echo skb in flight */ 1336 so->cfecho = 0; 1337 1338 /* register for echo skb's */ 1339 can_rx_register(net, dev, tx_id, SINGLE_MASK(tx_id), 1340 isotp_rcv_echo, sk, "isotpe", sk); 1341 } 1342 1343 dev_put(dev); 1344 1345 /* switch to new settings */ 1346 so->ifindex = ifindex; 1347 so->rxid = rx_id; 1348 so->txid = tx_id; 1349 so->bound = 1; 1350 1351 out: 1352 release_sock(sk); 1353 1354 if (notify_enetdown) { 1355 sk->sk_err = ENETDOWN; 1356 if (!sock_flag(sk, SOCK_DEAD)) 1357 sk_error_report(sk); 1358 } 1359 1360 return err; 1361 } 1362 1363 static int isotp_getname(struct socket *sock, struct sockaddr *uaddr, int peer) 1364 { 1365 struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; 1366 struct sock *sk = sock->sk; 1367 struct isotp_sock *so = isotp_sk(sk); 1368 1369 if (peer) 1370 return -EOPNOTSUPP; 1371 1372 memset(addr, 0, ISOTP_MIN_NAMELEN); 1373 addr->can_family = AF_CAN; 1374 addr->can_ifindex = so->ifindex; 1375 addr->can_addr.tp.rx_id = so->rxid; 1376 addr->can_addr.tp.tx_id = so->txid; 1377 1378 return ISOTP_MIN_NAMELEN; 1379 } 1380 1381 static int isotp_setsockopt_locked(struct socket *sock, int level, int optname, 1382 sockptr_t optval, unsigned int optlen) 1383 { 1384 struct sock *sk = sock->sk; 1385 struct isotp_sock *so = isotp_sk(sk); 1386 int ret = 0; 1387 1388 if (so->bound) 1389 return -EISCONN; 1390 1391 switch (optname) { 1392 case CAN_ISOTP_OPTS: 1393 if (optlen != sizeof(struct can_isotp_options)) 1394 return -EINVAL; 1395 1396 if (copy_from_sockptr(&so->opt, optval, optlen)) 1397 return -EFAULT; 1398 1399 /* no separate rx_ext_address is given => use ext_address */ 1400 if (!(so->opt.flags & CAN_ISOTP_RX_EXT_ADDR)) 1401 so->opt.rx_ext_address = so->opt.ext_address; 1402 1403 /* these broadcast flags are not allowed together */ 1404 if (isotp_bc_flags(so) == ISOTP_ALL_BC_FLAGS) { 1405 /* CAN_ISOTP_SF_BROADCAST is prioritized */ 1406 so->opt.flags &= ~CAN_ISOTP_CF_BROADCAST; 1407 1408 /* give user feedback on wrong config attempt */ 1409 ret = -EINVAL; 1410 } 1411 1412 /* check for frame_txtime changes (0 => no changes) */ 1413 if (so->opt.frame_txtime) { 1414 if (so->opt.frame_txtime == CAN_ISOTP_FRAME_TXTIME_ZERO) 1415 so->frame_txtime = 0; 1416 else 1417 so->frame_txtime = so->opt.frame_txtime; 1418 } 1419 break; 1420 1421 case CAN_ISOTP_RECV_FC: 1422 if (optlen != sizeof(struct can_isotp_fc_options)) 1423 return -EINVAL; 1424 1425 if (copy_from_sockptr(&so->rxfc, optval, optlen)) 1426 return -EFAULT; 1427 break; 1428 1429 case CAN_ISOTP_TX_STMIN: 1430 if (optlen != sizeof(u32)) 1431 return -EINVAL; 1432 1433 if (copy_from_sockptr(&so->force_tx_stmin, optval, optlen)) 1434 return -EFAULT; 1435 break; 1436 1437 case CAN_ISOTP_RX_STMIN: 1438 if (optlen != sizeof(u32)) 1439 return -EINVAL; 1440 1441 if (copy_from_sockptr(&so->force_rx_stmin, optval, optlen)) 1442 return -EFAULT; 1443 break; 1444 1445 case CAN_ISOTP_LL_OPTS: 1446 if (optlen == sizeof(struct can_isotp_ll_options)) { 1447 struct can_isotp_ll_options ll; 1448 1449 if (copy_from_sockptr(&ll, optval, optlen)) 1450 return -EFAULT; 1451 1452 /* check for correct ISO 11898-1 DLC data length */ 1453 if (ll.tx_dl != padlen(ll.tx_dl)) 1454 return -EINVAL; 1455 1456 if (ll.mtu != CAN_MTU && ll.mtu != CANFD_MTU) 1457 return -EINVAL; 1458 1459 if (ll.mtu == CAN_MTU && 1460 (ll.tx_dl > CAN_MAX_DLEN || ll.tx_flags != 0)) 1461 return -EINVAL; 1462 1463 memcpy(&so->ll, &ll, sizeof(ll)); 1464 1465 /* set ll_dl for tx path to similar place as for rx */ 1466 so->tx.ll_dl = ll.tx_dl; 1467 } else { 1468 return -EINVAL; 1469 } 1470 break; 1471 1472 default: 1473 ret = -ENOPROTOOPT; 1474 } 1475 1476 return ret; 1477 } 1478 1479 static int isotp_setsockopt(struct socket *sock, int level, int optname, 1480 sockptr_t optval, unsigned int optlen) 1481 1482 { 1483 struct sock *sk = sock->sk; 1484 int ret; 1485 1486 if (level != SOL_CAN_ISOTP) 1487 return -EINVAL; 1488 1489 lock_sock(sk); 1490 ret = isotp_setsockopt_locked(sock, level, optname, optval, optlen); 1491 release_sock(sk); 1492 return ret; 1493 } 1494 1495 static int isotp_getsockopt(struct socket *sock, int level, int optname, 1496 char __user *optval, int __user *optlen) 1497 { 1498 struct sock *sk = sock->sk; 1499 struct isotp_sock *so = isotp_sk(sk); 1500 int len; 1501 void *val; 1502 1503 if (level != SOL_CAN_ISOTP) 1504 return -EINVAL; 1505 if (get_user(len, optlen)) 1506 return -EFAULT; 1507 if (len < 0) 1508 return -EINVAL; 1509 1510 switch (optname) { 1511 case CAN_ISOTP_OPTS: 1512 len = min_t(int, len, sizeof(struct can_isotp_options)); 1513 val = &so->opt; 1514 break; 1515 1516 case CAN_ISOTP_RECV_FC: 1517 len = min_t(int, len, sizeof(struct can_isotp_fc_options)); 1518 val = &so->rxfc; 1519 break; 1520 1521 case CAN_ISOTP_TX_STMIN: 1522 len = min_t(int, len, sizeof(u32)); 1523 val = &so->force_tx_stmin; 1524 break; 1525 1526 case CAN_ISOTP_RX_STMIN: 1527 len = min_t(int, len, sizeof(u32)); 1528 val = &so->force_rx_stmin; 1529 break; 1530 1531 case CAN_ISOTP_LL_OPTS: 1532 len = min_t(int, len, sizeof(struct can_isotp_ll_options)); 1533 val = &so->ll; 1534 break; 1535 1536 default: 1537 return -ENOPROTOOPT; 1538 } 1539 1540 if (put_user(len, optlen)) 1541 return -EFAULT; 1542 if (copy_to_user(optval, val, len)) 1543 return -EFAULT; 1544 return 0; 1545 } 1546 1547 static void isotp_notify(struct isotp_sock *so, unsigned long msg, 1548 struct net_device *dev) 1549 { 1550 struct sock *sk = &so->sk; 1551 1552 if (!net_eq(dev_net(dev), sock_net(sk))) 1553 return; 1554 1555 if (so->ifindex != dev->ifindex) 1556 return; 1557 1558 switch (msg) { 1559 case NETDEV_UNREGISTER: 1560 lock_sock(sk); 1561 /* remove current filters & unregister */ 1562 if (so->bound && isotp_register_txecho(so)) { 1563 if (isotp_register_rxid(so)) 1564 can_rx_unregister(dev_net(dev), dev, so->rxid, 1565 SINGLE_MASK(so->rxid), 1566 isotp_rcv, sk); 1567 1568 can_rx_unregister(dev_net(dev), dev, so->txid, 1569 SINGLE_MASK(so->txid), 1570 isotp_rcv_echo, sk); 1571 } 1572 1573 so->ifindex = 0; 1574 so->bound = 0; 1575 release_sock(sk); 1576 1577 sk->sk_err = ENODEV; 1578 if (!sock_flag(sk, SOCK_DEAD)) 1579 sk_error_report(sk); 1580 break; 1581 1582 case NETDEV_DOWN: 1583 sk->sk_err = ENETDOWN; 1584 if (!sock_flag(sk, SOCK_DEAD)) 1585 sk_error_report(sk); 1586 break; 1587 } 1588 } 1589 1590 static int isotp_notifier(struct notifier_block *nb, unsigned long msg, 1591 void *ptr) 1592 { 1593 struct net_device *dev = netdev_notifier_info_to_dev(ptr); 1594 1595 if (dev->type != ARPHRD_CAN) 1596 return NOTIFY_DONE; 1597 if (msg != NETDEV_UNREGISTER && msg != NETDEV_DOWN) 1598 return NOTIFY_DONE; 1599 if (unlikely(isotp_busy_notifier)) /* Check for reentrant bug. */ 1600 return NOTIFY_DONE; 1601 1602 spin_lock(&isotp_notifier_lock); 1603 list_for_each_entry(isotp_busy_notifier, &isotp_notifier_list, notifier) { 1604 spin_unlock(&isotp_notifier_lock); 1605 isotp_notify(isotp_busy_notifier, msg, dev); 1606 spin_lock(&isotp_notifier_lock); 1607 } 1608 isotp_busy_notifier = NULL; 1609 spin_unlock(&isotp_notifier_lock); 1610 return NOTIFY_DONE; 1611 } 1612 1613 static int isotp_init(struct sock *sk) 1614 { 1615 struct isotp_sock *so = isotp_sk(sk); 1616 1617 so->ifindex = 0; 1618 so->bound = 0; 1619 1620 so->opt.flags = CAN_ISOTP_DEFAULT_FLAGS; 1621 so->opt.ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS; 1622 so->opt.rx_ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS; 1623 so->opt.rxpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT; 1624 so->opt.txpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT; 1625 so->opt.frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME; 1626 so->frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME; 1627 so->rxfc.bs = CAN_ISOTP_DEFAULT_RECV_BS; 1628 so->rxfc.stmin = CAN_ISOTP_DEFAULT_RECV_STMIN; 1629 so->rxfc.wftmax = CAN_ISOTP_DEFAULT_RECV_WFTMAX; 1630 so->ll.mtu = CAN_ISOTP_DEFAULT_LL_MTU; 1631 so->ll.tx_dl = CAN_ISOTP_DEFAULT_LL_TX_DL; 1632 so->ll.tx_flags = CAN_ISOTP_DEFAULT_LL_TX_FLAGS; 1633 1634 /* set ll_dl for tx path to similar place as for rx */ 1635 so->tx.ll_dl = so->ll.tx_dl; 1636 1637 so->rx.state = ISOTP_IDLE; 1638 so->tx.state = ISOTP_IDLE; 1639 1640 so->rx.buf = so->rx.sbuf; 1641 so->tx.buf = so->tx.sbuf; 1642 so->rx.buflen = ARRAY_SIZE(so->rx.sbuf); 1643 so->tx.buflen = ARRAY_SIZE(so->tx.sbuf); 1644 1645 hrtimer_init(&so->rxtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT); 1646 so->rxtimer.function = isotp_rx_timer_handler; 1647 hrtimer_init(&so->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT); 1648 so->txtimer.function = isotp_tx_timer_handler; 1649 hrtimer_init(&so->txfrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT); 1650 so->txfrtimer.function = isotp_txfr_timer_handler; 1651 1652 init_waitqueue_head(&so->wait); 1653 spin_lock_init(&so->rx_lock); 1654 1655 spin_lock(&isotp_notifier_lock); 1656 list_add_tail(&so->notifier, &isotp_notifier_list); 1657 spin_unlock(&isotp_notifier_lock); 1658 1659 return 0; 1660 } 1661 1662 static __poll_t isotp_poll(struct file *file, struct socket *sock, poll_table *wait) 1663 { 1664 struct sock *sk = sock->sk; 1665 struct isotp_sock *so = isotp_sk(sk); 1666 1667 __poll_t mask = datagram_poll(file, sock, wait); 1668 poll_wait(file, &so->wait, wait); 1669 1670 /* Check for false positives due to TX state */ 1671 if ((mask & EPOLLWRNORM) && (so->tx.state != ISOTP_IDLE)) 1672 mask &= ~(EPOLLOUT | EPOLLWRNORM); 1673 1674 return mask; 1675 } 1676 1677 static int isotp_sock_no_ioctlcmd(struct socket *sock, unsigned int cmd, 1678 unsigned long arg) 1679 { 1680 /* no ioctls for socket layer -> hand it down to NIC layer */ 1681 return -ENOIOCTLCMD; 1682 } 1683 1684 static const struct proto_ops isotp_ops = { 1685 .family = PF_CAN, 1686 .release = isotp_release, 1687 .bind = isotp_bind, 1688 .connect = sock_no_connect, 1689 .socketpair = sock_no_socketpair, 1690 .accept = sock_no_accept, 1691 .getname = isotp_getname, 1692 .poll = isotp_poll, 1693 .ioctl = isotp_sock_no_ioctlcmd, 1694 .gettstamp = sock_gettstamp, 1695 .listen = sock_no_listen, 1696 .shutdown = sock_no_shutdown, 1697 .setsockopt = isotp_setsockopt, 1698 .getsockopt = isotp_getsockopt, 1699 .sendmsg = isotp_sendmsg, 1700 .recvmsg = isotp_recvmsg, 1701 .mmap = sock_no_mmap, 1702 .sendpage = sock_no_sendpage, 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