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