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