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