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