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