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