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