net/can/isotp.c

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