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