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