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