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