kernel/2.6/net/can/isotp.c

   1 /*
   2  * isotp.c - ISO 15765-2 CAN transport protocol for protocol family CAN
   3  *
   4  * WARNING: This is ALPHA code for discussions and first tests that should
   5  *          not be used in production environments.
   6  *
   7  * In the discussion the Socket-API to the userspace or the ISO-TP socket
   8  * options or the return values we may change! Current behaviour:
   9  *
  10  * - no ISO-TP specific return values are provided to the userspace
  11  * - when a transfer (tx) is on the run the next write() blocks until it's done
  12  * - no support for sending wait frames to the data source in the rx path
  13  *
  14  * Copyright (c) 2008 Volkswagen Group Electronic Research
  15  * All rights reserved.
  16  *
  17  * Redistribution and use in source and binary forms, with or without
  18  * modification, are permitted provided that the following conditions
  19  * are met:
  20  * 1. Redistributions of source code must retain the above copyright
  21  *    notice, this list of conditions and the following disclaimer.
  22  * 2. Redistributions in binary form must reproduce the above copyright
  23  *    notice, this list of conditions and the following disclaimer in the
  24  *    documentation and/or other materials provided with the distribution.
  25  * 3. Neither the name of Volkswagen nor the names of its contributors
  26  *    may be used to endorse or promote products derived from this software
  27  *    without specific prior written permission.
  28  *
  29  * Alternatively, provided that this notice is retained in full, this
  30  * software may be distributed under the terms of the GNU General
  31  * Public License ("GPL") version 2, in which case the provisions of the
  32  * GPL apply INSTEAD OF those given above.
  33  *
  34  * The provided data structures and external interfaces from this code
  35  * are not restricted to be used by modules with a GPL compatible license.
  36  *
  37  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  38  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  39  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  40  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  41  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  42  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  43  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  44  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  45  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  46  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  47  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  48  * DAMAGE.
  49  *
  50  * Send feedback to <socketcan-users@lists.berlios.de>
  51  *
  52  */
  53
  54 #include <linux/module.h>
  55 #include <linux/version.h>
  56 #include <linux/init.h>
  57 #include <linux/interrupt.h>
  58 #include <linux/hrtimer.h>
  59 #include <linux/wait.h>
  60 #include <linux/uio.h>
  61 #include <linux/net.h>
  62 #include <linux/netdevice.h>
  63 #include <linux/socket.h>
  64 #include <linux/if_arp.h>
  65 #include <linux/skbuff.h>
  66 #include <socketcan/can.h>
  67 #include <socketcan/can/core.h>
  68 #include <socketcan/can/isotp.h>
  69 #include <net/sock.h>
  70 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
  71 #include <net/net_namespace.h>
  72 #endif
  73 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,25)
  74 #include "compat.h"
  75 #endif
  76
  77 #include <socketcan/can/version.h> /* for RCSID. Removed by mkpatch script */
  78 RCSID("$Id$");
  79
  80 #define CAN_ISOTP_VERSION CAN_VERSION
  81 static __initdata const char banner[] =
  82         KERN_INFO "can: isotp protocol (rev " CAN_ISOTP_VERSION " alpha)\n";
  83
  84 MODULE_DESCRIPTION("PF_CAN isotp 15765-2 protocol");
  85 MODULE_LICENSE("Dual BSD/GPL");
  86 MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
  87 MODULE_ALIAS("can-proto-6");
  88
  89 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
  90 #error This modules needs hrtimers (available since Kernel 2.6.22)
  91 #endif
  92
  93 #define DBG(fmt, args...) (printk( KERN_DEBUG "can-isotp: %s: " fmt, \
  94                                    __func__, ##args))
  95 #undef DBG
  96 #define DBG(fmt, args...)
  97
  98 #define SINGLE_MASK(id) ((id & CAN_EFF_FLAG) ? \
  99                          (CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG) : \
 100                          (CAN_SFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG))
 101
 102 /* N_PCI type values in bits 7-4 of N_PCI bytes */
 103 #define N_PCI_SF 0x00   /* single frame */
 104 #define N_PCI_FF 0x10   /* first frame */
 105 #define N_PCI_CF 0x20   /* consecutive frame */
 106 #define N_PCI_FC 0x30   /* flow control */
 107
 108 /* Flow Status given in FC frame */
 109 #define ISOTP_FC_CTS    0       /* clear to send */
 110 #define ISOTP_FC_WT     1       /* wait */
 111 #define ISOTP_FC_OVFLW  2       /* overflow */
 112
 113 enum {
 114         ISOTP_IDLE = 0,
 115         ISOTP_WAIT_FIRST_FC,
 116         ISOTP_WAIT_FC,
 117         ISOTP_WAIT_DATA,
 118         ISOTP_SENDING
 119 };
 120
 121 struct tpcon {
 122         int idx;
 123         int len;
 124         u8  state;
 125         u8  bs;
 126         u8  sn;
 127         u8  buf[4096];
 128 };
 129
 130 struct isotp_sock {
 131         struct sock sk;
 132         int bound;
 133         int ifindex;
 134         canid_t txid;
 135         canid_t rxid;
 136         ktime_t tx_gap;
 137         ktime_t lastrxcf_tstamp;
 138         struct hrtimer rxtimer, txtimer;
 139         struct tasklet_struct txtsklet;
 140         struct can_isotp_options opt;
 141         struct can_isotp_fc_options rxfc, txfc;
 142         __u32 force_tx_stmin;
 143         __u32 force_rx_stmin;
 144         struct tpcon rx, tx;
 145         struct notifier_block notifier;
 146         wait_queue_head_t wait;
 147 };
 148
 149 static inline struct isotp_sock *isotp_sk(const struct sock *sk)
 150 {
 151         return (struct isotp_sock *)sk;
 152 }
 153
 154 static enum hrtimer_restart isotp_rx_timer_handler(struct hrtimer *hrtimer)
 155 {
 156         struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
 157                                              rxtimer);
 158         if (so->rx.state == ISOTP_WAIT_DATA) {
 159 #if 0
 160                 struct sock *sk = &so->sk;
 161
 162                 /* report 'timeout' */
 163                 sk->sk_err = E?????;
 164                 if (!sock_flag(sk, SOCK_DEAD))
 165                         sk->sk_error_report(sk);
 166 #endif
 167                 DBG("we did not get new data frames in time.\n");
 168
 169                 /* reset tx state */
 170                 so->rx.state = ISOTP_IDLE;
 171         }
 172
 173         return HRTIMER_NORESTART;
 174 }
 175
 176 static int isotp_send_fc(struct sock *sk, int ae)
 177 {
 178         struct net_device *dev;
 179         struct sk_buff *nskb;
 180         struct can_frame *ncf;
 181         struct isotp_sock *so = isotp_sk(sk);
 182
 183         nskb = alloc_skb(sizeof(struct can_frame), gfp_any());
 184         if (!nskb)
 185                 return 1;
 186
 187         dev = dev_get_by_index(&init_net, so->ifindex);
 188         if (!dev) {
 189                 kfree_skb(nskb);
 190                 return 1;
 191         }
 192         nskb->dev = dev;
 193         nskb->sk = sk;
 194         ncf = (struct can_frame *) nskb->data;
 195         skb_put(nskb, sizeof(struct can_frame));
 196
 197         /* create & send flow control reply */
 198         ncf->can_id = so->txid;
 199
 200         if (so->opt.flags & CAN_ISOTP_RX_PADDING) {
 201                 memset(ncf->data, so->opt.rxpad_content, 8);
 202                 ncf->can_dlc = 8;
 203         } else
 204                 ncf->can_dlc = ae+3;
 205
 206         ncf->data[ae] = N_PCI_FC | ISOTP_FC_CTS;
 207         ncf->data[ae+1] = so->rxfc.bs;
 208         ncf->data[ae+2] = so->rxfc.stmin;
 209
 210         if (ae)
 211                 ncf->data[0] = so->opt.ext_address;
 212
 213         can_send(nskb, 1);
 214         dev_put(dev);
 215
 216         /* reset blocksize counter */
 217         so->rx.bs = 0;
 218
 219         /* reset last CF frame rx timestamp for rx stmin enforcement */
 220         so->lastrxcf_tstamp = ktime_set(0,0);
 221
 222         /* start rx timeout watchdog */
 223         hrtimer_start(&so->rxtimer, ktime_set(1,0), HRTIMER_MODE_REL);
 224         return 0;
 225 }
 226
 227 static void isotp_rcv_skb(struct sk_buff *skb, struct sock *sk)
 228 {
 229         struct sockaddr_can *addr = (struct sockaddr_can *)skb->cb;
 230
 231         BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct sockaddr_can));
 232
 233         skb->sk = sk;
 234
 235         memset(addr, 0, sizeof(*addr));
 236         addr->can_family  = AF_CAN;
 237         addr->can_ifindex = skb->dev->ifindex;
 238
 239         if (sock_queue_rcv_skb(sk, skb) < 0)
 240                 kfree_skb(skb);
 241 }
 242
 243 static int check_pad(struct isotp_sock *so, struct can_frame *cf,
 244                      int start_index, __u8 content)
 245 {
 246         int i;
 247
 248         /* check datalength code */
 249         if ((so->opt.flags & CAN_ISOTP_CHK_PAD_LEN) && cf->can_dlc != 8)
 250                         return 1;
 251
 252         /* check padding content */
 253         if (so->opt.flags & CAN_ISOTP_CHK_PAD_DATA) {
 254                 for (i = start_index; i < 8; i++)
 255                         if (cf->data[i] != content)
 256                                 return 1;
 257         }
 258         return 0;
 259 }
 260
 261 static int isotp_rcv_fc(struct isotp_sock *so, struct can_frame *cf, int ae)
 262 {
 263         if (so->tx.state != ISOTP_WAIT_FC &&
 264             so->tx.state != ISOTP_WAIT_FIRST_FC)
 265                 return 0;
 266
 267         hrtimer_cancel(&so->txtimer);
 268
 269         if ((so->opt.flags & CAN_ISOTP_TX_PADDING) &&
 270             check_pad(so, cf, ae+3, so->opt.txpad_content)) {
 271                 so->tx.state = ISOTP_IDLE;
 272                 wake_up_interruptible(&so->wait);
 273                 return 1;
 274         }
 275
 276         /* get communication parameters only from the first FC frame */
 277         if (so->tx.state == ISOTP_WAIT_FIRST_FC) {
 278
 279                 so->txfc.bs = cf->data[ae+1];
 280                 so->txfc.stmin = cf->data[ae+2];
 281
 282                 /* fix wrong STmin values according spec */
 283                 if ((so->txfc.stmin > 0x7F) &&
 284                     ((so->txfc.stmin < 0xF1) || (so->txfc.stmin > 0xF9)))
 285                         so->txfc.stmin = 0x7F;
 286
 287                 so->tx_gap = ktime_set(0,0);
 288                 /* add transmission time for CAN frame N_As */
 289                 so->tx_gap = ktime_add_ns(so->tx_gap, so->opt.frame_txtime);
 290                 /* add waiting time for consecutive frames N_Cs */
 291                 if (so->opt.flags & CAN_ISOTP_FORCE_TXSTMIN)
 292                         so->tx_gap = ktime_add_ns(so->tx_gap,
 293                                                   so->force_tx_stmin);
 294                 else if (so->txfc.stmin < 0x80)
 295                         so->tx_gap = ktime_add_ns(so->tx_gap,
 296                                                   so->txfc.stmin * 1000000);
 297                 else
 298                         so->tx_gap = ktime_add_ns(so->tx_gap,
 299                                                   (so->txfc.stmin - 0xF0)
 300                                                   * 100000);
 301                 so->tx.state = ISOTP_WAIT_FC;
 302         }
 303
 304         DBG("FC frame: FS %d, BS %d, STmin 0x%02X, tx_gap %lld\n",
 305             cf->data[ae] & 0x0F & 0x0F, so->txfc.bs, so->txfc.stmin,
 306             (long long)so->tx_gap.tv64);
 307
 308         switch (cf->data[ae] & 0x0F) {
 309
 310         case ISOTP_FC_CTS:
 311                 so->tx.bs = 0;
 312                 so->tx.state = ISOTP_SENDING;
 313                 DBG("starting txtimer for sending\n");
 314                 /* start cyclic timer for sending CF frame */
 315                 hrtimer_start(&so->txtimer, so->tx_gap,
 316                               HRTIMER_MODE_REL);
 317                 break;
 318
 319         case ISOTP_FC_WT:
 320                 DBG("starting waiting for next FC\n");
 321                 /* start timer to wait for next FC frame */
 322                 hrtimer_start(&so->txtimer, ktime_set(1,0),
 323                               HRTIMER_MODE_REL);
 324                 break;
 325
 326         case ISOTP_FC_OVFLW:
 327                 DBG("overflow in receiver side\n");
 328
 329         default:
 330                 /* stop this tx job. TODO: error reporting? */
 331                 so->tx.state = ISOTP_IDLE;
 332                 wake_up_interruptible(&so->wait);
 333         }
 334         return 0;
 335 }
 336
 337 static int isotp_rcv_sf(struct sock *sk, struct can_frame *cf, int ae,
 338                         struct sk_buff *skb)
 339 {
 340         struct isotp_sock *so = isotp_sk(sk);
 341         int len = cf->data[ae] & 0x0F;
 342         struct sk_buff *nskb;
 343
 344         hrtimer_cancel(&so->rxtimer);
 345         so->rx.state = ISOTP_IDLE;
 346
 347         if (!len || len > 7 || (ae && len > 6))
 348                 return 1;
 349
 350         if ((so->opt.flags & CAN_ISOTP_RX_PADDING) &&
 351             check_pad(so, cf, 1+ae+len, so->opt.rxpad_content))
 352                 return 1;
 353
 354         nskb = alloc_skb(len, gfp_any());
 355         if (!nskb)
 356                 return 1;
 357
 358         memcpy(skb_put(nskb, len), &cf->data[1+ae], len);
 359
 360         nskb->tstamp = skb->tstamp;
 361         nskb->dev = skb->dev;
 362         isotp_rcv_skb(nskb, sk);
 363         return 0;
 364 }
 365
 366 static int isotp_rcv_ff(struct sock *sk, struct can_frame *cf, int ae)
 367 {
 368         struct isotp_sock *so = isotp_sk(sk);
 369         int i;
 370
 371         hrtimer_cancel(&so->rxtimer);
 372         so->rx.state = ISOTP_IDLE;
 373
 374         if (cf->can_dlc != 8)
 375                 return 1;
 376
 377         so->rx.len = (cf->data[ae] & 0x0F) << 8;
 378         so->rx.len += cf->data[ae+1];
 379
 380         if (so->rx.len + ae < 8)
 381                 return 1;
 382
 383         /* copy the first received data bytes */
 384         so->rx.idx = 0;
 385         for (i = ae+2; i < 8; i++)
 386                 so->rx.buf[so->rx.idx++] = cf->data[i];
 387
 388         /* initial setup for this pdu receiption */
 389         so->rx.sn = 1;
 390         so->rx.state = ISOTP_WAIT_DATA;
 391
 392         /* no creation of flow control frames */
 393         if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
 394                 return 0;
 395
 396         /* send our first FC frame */
 397         isotp_send_fc(sk, ae);
 398         return 0;
 399 }
 400
 401 static int isotp_rcv_cf(struct sock *sk, struct can_frame *cf, int ae,
 402                         struct sk_buff *skb)
 403 {
 404         struct isotp_sock *so = isotp_sk(sk);
 405         struct sk_buff *nskb;
 406         int i;
 407
 408         if (so->rx.state != ISOTP_WAIT_DATA)
 409                 return 0;
 410
 411         /* drop if timestamp gap is less than force_rx_stmin nano secs */
 412         if (so->opt.flags & CAN_ISOTP_FORCE_RXSTMIN) {
 413
 414                 if (ktime_to_ns(ktime_sub(skb->tstamp, so->lastrxcf_tstamp)) <
 415                     so->force_rx_stmin)
 416                         return 0;
 417
 418                 so->lastrxcf_tstamp = skb->tstamp;
 419         }
 420
 421         hrtimer_cancel(&so->rxtimer);
 422
 423         if ((cf->data[ae] & 0x0F) != so->rx.sn) {
 424                 DBG("wrong sn %d. expected %d.\n",
 425                     cf->data[ae] & 0x0F, so->rx.sn);
 426                 /* some error reporting? */
 427                 so->rx.state = ISOTP_IDLE;
 428                 return 1;
 429         }
 430         so->rx.sn++;
 431         so->rx.sn %= 16;
 432
 433         for (i = ae+1; i < 8; i++) {
 434                 so->rx.buf[so->rx.idx++] = cf->data[i];
 435                 if (so->rx.idx >= so->rx.len)
 436                         break;
 437         }
 438
 439         if (so->rx.idx >= so->rx.len) {
 440
 441                 /* we are done */
 442                 so->rx.state = ISOTP_IDLE;
 443
 444                 if ((so->opt.flags & CAN_ISOTP_RX_PADDING) &&
 445                     check_pad(so, cf, i+1, so->opt.rxpad_content))
 446                         return 1;
 447
 448                 nskb = alloc_skb(so->rx.len, gfp_any());
 449                 if (!nskb)
 450                         return 1;
 451
 452                 memcpy(skb_put(nskb, so->rx.len), so->rx.buf,
 453                        so->rx.len);
 454
 455                 nskb->tstamp = skb->tstamp;
 456                 nskb->dev = skb->dev;
 457                 isotp_rcv_skb(nskb, sk);
 458                 return 0;
 459         }
 460
 461         /* no creation of flow control frames */
 462         if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
 463                 return 0;
 464
 465         /* perform blocksize handling, if enabled */
 466         if (!so->rxfc.bs || ++so->rx.bs < so->rxfc.bs) {
 467
 468                 /* start rx timeout watchdog */
 469                 hrtimer_start(&so->rxtimer, ktime_set(1,0),
 470                               HRTIMER_MODE_REL);
 471                 return 0;
 472         }
 473
 474         /* we reached the specified blocksize so->rxfc.bs */
 475         isotp_send_fc(sk, ae);
 476         return 0;
 477 }
 478
 479 static void isotp_rcv(struct sk_buff *skb, void *data)
 480 {
 481         struct sock *sk = (struct sock *)data;
 482         struct isotp_sock *so = isotp_sk(sk);
 483         struct can_frame *cf;
 484         int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR)? 1:0;
 485         u8 n_pci_type;
 486
 487         /* read CAN frame and free skbuff */
 488         BUG_ON(skb->len != sizeof(struct can_frame));
 489         cf = (struct can_frame *) skb->data;
 490
 491         /* if enabled: check receiption of my configured extended address */
 492         if (ae && cf->data[0] != so->opt.ext_address)
 493                 return;
 494
 495         n_pci_type = cf->data[ae] & 0xF0;
 496
 497         if (so->opt.flags & CAN_ISOTP_HALF_DUPLEX) {
 498                 /* check rx/tx path half duplex expectations */
 499                 if ((so->tx.state != ISOTP_IDLE && n_pci_type != N_PCI_FC) ||
 500                     (so->rx.state != ISOTP_IDLE && n_pci_type == N_PCI_FC))
 501                         return;
 502         }
 503
 504         switch (n_pci_type) {
 505         case N_PCI_FC:
 506                 /* tx path: flow control frame containing the FC parameters */
 507                 isotp_rcv_fc(so, cf, ae);
 508                 break;
 509
 510         case N_PCI_SF:
 511                 /* rx path: single frame */
 512                 isotp_rcv_sf(sk, cf, ae, skb);
 513                 break;
 514
 515         case N_PCI_FF:
 516                 /* rx path: first frame */
 517                 isotp_rcv_ff(sk, cf, ae);
 518                 break;
 519
 520         case N_PCI_CF:
 521                 /* rx path: consecutive frame */
 522                 isotp_rcv_cf(sk, cf, ae, skb);
 523                 break;
 524         }
 525 }
 526
 527 static void isotp_fill_dataframe(struct can_frame *cf, struct isotp_sock *so,
 528                                  int ae)
 529 {
 530         unsigned char space = 7 - ae;
 531         int num = min_t(int, so->tx.len - so->tx.idx, space);
 532         int i;
 533
 534         cf->can_id = so->txid;
 535
 536         if (so->opt.flags & CAN_ISOTP_TX_PADDING) {
 537                 if (num < space)
 538                         memset(cf->data, so->opt.txpad_content, 8);
 539
 540                 cf->can_dlc = 8;
 541         } else
 542                 cf->can_dlc = num + 1 + ae;
 543
 544
 545         for (i = 0; i < num; i++)
 546                 cf->data[i+ae+1] = so->tx.buf[so->tx.idx++];
 547
 548         if (ae)
 549                 cf->data[0] = so->opt.ext_address;
 550 }
 551
 552 static void isotp_create_fframe(struct can_frame *cf, struct isotp_sock *so,
 553                                 int ae)
 554 {
 555         int i;
 556
 557         cf->can_id = so->txid;
 558         cf->can_dlc = 8;
 559         if (ae)
 560                 cf->data[0] = so->opt.ext_address;
 561
 562         /* N_PCI bytes with FF_DL data length */
 563         cf->data[ae] = (u8) (so->tx.len>>8) | N_PCI_FF;
 564         cf->data[ae+1] = (u8) so->tx.len & 0xFFU;
 565
 566         /* add first 5 or 6 data bytes depending on ae */
 567         for (i = ae+2; i < 8; i++)
 568                 cf->data[i] = so->tx.buf[so->tx.idx++];
 569
 570         so->tx.sn = 1;
 571         so->tx.state = ISOTP_WAIT_FIRST_FC;
 572 }
 573
 574 static void isotp_tx_timer_tsklet(unsigned long data)
 575 {
 576         struct isotp_sock *so = (struct isotp_sock *)data;
 577         struct sock *sk = &so->sk;
 578         struct sk_buff *skb;
 579         struct net_device *dev;
 580         struct can_frame *cf;
 581         int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR)? 1:0;
 582
 583         switch (so->tx.state) {
 584
 585         case ISOTP_WAIT_FC:
 586         case ISOTP_WAIT_FIRST_FC:
 587
 588                 /* we did not get any flow control frame in time */
 589
 590                 DBG("we did not get FC frame in time.\n");
 591
 592 #if 0
 593                 /* report 'communication error on send' */
 594                 sk->sk_err = ECOMM;
 595                 if (!sock_flag(sk, SOCK_DEAD))
 596                         sk->sk_error_report(sk);
 597 #endif
 598                 /* reset tx state */
 599                 so->tx.state = ISOTP_IDLE;
 600                 wake_up_interruptible(&so->wait);
 601                 break;
 602
 603         case ISOTP_SENDING:
 604
 605                 /* push out the next segmented pdu */
 606
 607                 DBG("next pdu to send.\n");
 608
 609                 dev = dev_get_by_index(&init_net, so->ifindex);
 610                 if (!dev)
 611                         break;
 612
 613 isotp_tx_burst:
 614                 skb = alloc_skb(sizeof(*cf), gfp_any());
 615                 if (!skb) {
 616                         dev_put(dev);
 617                         break;
 618                 }
 619
 620                 cf = (struct can_frame *)skb->data;
 621                 skb_put(skb, sizeof(*cf));
 622
 623                 /* create consecutive frame */
 624                 isotp_fill_dataframe(cf, so, ae);
 625
 626                 /* place consecutive frame N_PCI in appropriate index */
 627                 cf->data[ae] = N_PCI_CF | so->tx.sn++;
 628                 so->tx.sn %= 16;
 629                 so->tx.bs++;
 630
 631                 skb->dev = dev;
 632                 skb->sk  = sk;
 633                 can_send(skb, 1);
 634
 635                 if (so->tx.idx >= so->tx.len) {
 636                         /* we are done */
 637                         DBG("we are done\n");
 638                         so->tx.state = ISOTP_IDLE;
 639                         dev_put(dev);
 640                         wake_up_interruptible(&so->wait);
 641                         break;
 642                 }
 643
 644                 if (so->txfc.bs && so->tx.bs >= so->txfc.bs) {
 645                         /* stop and wait for FC */
 646                         DBG("BS stop and wait for FC\n");
 647                         so->tx.state = ISOTP_WAIT_FC;
 648                         dev_put(dev);
 649                         hrtimer_start(&so->txtimer,
 650                                       ktime_add(ktime_get(), ktime_set(1,0)),
 651                                       HRTIMER_MODE_ABS);
 652                         break;
 653                 }
 654
 655                 /* no gap between data frames needed => use burst mode */
 656                 if (!so->tx_gap.tv64)
 657                         goto isotp_tx_burst;
 658
 659                 /* start timer to send next data frame with correct delay */
 660                 dev_put(dev);
 661                 hrtimer_start(&so->txtimer,
 662                               ktime_add(ktime_get(), so->tx_gap),
 663                               HRTIMER_MODE_ABS);
 664                 break;
 665
 666         default:
 667                 BUG_ON(1);
 668         }
 669 }
 670
 671 static enum hrtimer_restart isotp_tx_timer_handler(struct hrtimer *hrtimer)
 672 {
 673         struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
 674                                              txtimer);
 675         tasklet_schedule(&so->txtsklet);
 676
 677         return HRTIMER_NORESTART;
 678 }
 679
 680 static int isotp_sendmsg(struct kiocb *iocb, struct socket *sock,
 681                        struct msghdr *msg, size_t size)
 682 {
 683         struct sock *sk = sock->sk;
 684         struct isotp_sock *so = isotp_sk(sk);
 685         struct sk_buff *skb;
 686         struct net_device *dev;
 687         struct can_frame *cf;
 688         int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR)? 1:0;
 689         int err;
 690
 691         if (!so->bound)
 692                 return -EADDRNOTAVAIL;
 693
 694         /* we do not support multiple buffers - for now */
 695         if (so->tx.state != ISOTP_IDLE) {
 696                 if (msg->msg_flags & MSG_DONTWAIT)
 697                         return -EAGAIN;
 698
 699                 /* wait for complete transmission of current pdu */
 700                 wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
 701         }
 702
 703         if (!size || size > 4095)
 704                 return -EINVAL;
 705
 706         err = memcpy_fromiovec(so->tx.buf, msg->msg_iov, size);
 707         if (err < 0)
 708                 return err;
 709
 710         dev = dev_get_by_index(&init_net, so->ifindex);
 711         if (!dev)
 712                 return -ENXIO;
 713
 714         skb = sock_alloc_send_skb(sk, sizeof(*cf),
 715                                   msg->msg_flags & MSG_DONTWAIT, &err);
 716         if (!skb) {
 717                 dev_put(dev);
 718                 return err;
 719         }
 720
 721         so->tx.state = ISOTP_SENDING;
 722         so->tx.len = size;
 723         so->tx.idx = 0;
 724
 725         cf = (struct can_frame *)skb->data;
 726         skb_put(skb, sizeof(*cf));
 727
 728         /* check for single frame transmission */
 729         if (size <= 7 - ae) {
 730
 731                 isotp_fill_dataframe(cf, so, ae);
 732
 733                 /* place single frame N_PCI in appropriate index */
 734                 cf->data[ae] = size | N_PCI_SF;
 735
 736                 so->tx.state = ISOTP_IDLE;
 737                 wake_up_interruptible(&so->wait);
 738         } else {
 739                 /* send first frame and wait for FC */
 740
 741                 isotp_create_fframe(cf, so, ae);
 742
 743                 DBG("starting txtimer for fc\n");
 744                 /* start timeout for FC */
 745                 hrtimer_start(&so->txtimer, ktime_set(1,0), HRTIMER_MODE_REL);
 746         }
 747
 748         /* send the first or only CAN frame */
 749         skb->dev = dev;
 750         skb->sk  = sk;
 751         err = can_send(skb, 1);
 752         dev_put(dev);
 753         if (err)
 754                 return err;
 755
 756         return size;
 757 }
 758
 759 static int isotp_recvmsg(struct kiocb *iocb, struct socket *sock,
 760                          struct msghdr *msg, size_t size, int flags)
 761 {
 762         struct sock *sk = sock->sk;
 763         struct sk_buff *skb;
 764         int err = 0;
 765         int noblock;
 766
 767         noblock =  flags & MSG_DONTWAIT;
 768         flags   &= ~MSG_DONTWAIT;
 769
 770         skb = skb_recv_datagram(sk, flags, noblock, &err);
 771         if (!skb)
 772                 return err;
 773
 774         if (size < skb->len)
 775                 msg->msg_flags |= MSG_TRUNC;
 776         else
 777                 size = skb->len;
 778
 779         err = memcpy_toiovec(msg->msg_iov, skb->data, size);
 780         if (err < 0) {
 781                 skb_free_datagram(sk, skb);
 782                 return err;
 783         }
 784
 785         sock_recv_timestamp(msg, sk, skb);
 786
 787         if (msg->msg_name) {
 788                 msg->msg_namelen = sizeof(struct sockaddr_can);
 789                 memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
 790         }
 791
 792         skb_free_datagram(sk, skb);
 793
 794         return size;
 795 }
 796
 797 static int isotp_release(struct socket *sock)
 798 {
 799         struct sock *sk = sock->sk;
 800         struct isotp_sock *so;
 801
 802         if (!sk)
 803                 return 0;
 804
 805         so = isotp_sk(sk);
 806
 807         /* wait for complete transmission of current pdu */
 808         wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
 809
 810         unregister_netdevice_notifier(&so->notifier);
 811
 812         lock_sock(sk);
 813
 814         hrtimer_cancel(&so->txtimer);
 815         hrtimer_cancel(&so->rxtimer);
 816         tasklet_kill(&so->txtsklet);
 817
 818         /* remove current filters & unregister */
 819         if (so->bound) {
 820                 if (so->ifindex) {
 821                         struct net_device *dev;
 822
 823                         dev = dev_get_by_index(&init_net, so->ifindex);
 824                         if (dev) {
 825                                 can_rx_unregister(dev, so->rxid,
 826                                                   SINGLE_MASK(so->rxid),
 827                                                   isotp_rcv, sk);
 828                                 dev_put(dev);
 829                         }
 830                 }
 831         }
 832
 833         so->ifindex = 0;
 834         so->bound   = 0;
 835
 836         sock_orphan(sk);
 837         sock->sk = NULL;
 838
 839         release_sock(sk);
 840         sock_put(sk);
 841
 842         return 0;
 843 }
 844
 845 static int isotp_bind(struct socket *sock, struct sockaddr *uaddr, int len)
 846 {
 847         struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
 848         struct sock *sk = sock->sk;
 849         struct isotp_sock *so = isotp_sk(sk);
 850         int ifindex;
 851         struct net_device *dev;
 852         int err = 0;
 853         int notify_enetdown = 0;
 854
 855         if (len < sizeof(*addr))
 856                 return -EINVAL;
 857
 858         if (addr->can_addr.tp.rx_id == addr->can_addr.tp.tx_id)
 859                 return -EADDRNOTAVAIL;
 860
 861         if ((addr->can_addr.tp.rx_id | addr->can_addr.tp.tx_id) &
 862             (CAN_ERR_FLAG | CAN_RTR_FLAG))
 863                 return -EADDRNOTAVAIL;
 864
 865         if (!addr->can_ifindex)
 866                 return -ENODEV;
 867
 868         lock_sock(sk);
 869
 870         if (so->bound && addr->can_ifindex == so->ifindex &&
 871             addr->can_addr.tp.rx_id == so->rxid &&
 872             addr->can_addr.tp.tx_id == so->txid)
 873                 goto out;
 874
 875         dev = dev_get_by_index(&init_net, addr->can_ifindex);
 876         if (!dev) {
 877                 err = -ENODEV;
 878                 goto out;
 879         }
 880         if (dev->type != ARPHRD_CAN) {
 881                 dev_put(dev);
 882                 err = -ENODEV;
 883                 goto out;
 884         }
 885         if (!(dev->flags & IFF_UP))
 886                 notify_enetdown = 1;
 887
 888         ifindex = dev->ifindex;
 889
 890         can_rx_register(dev, addr->can_addr.tp.rx_id,
 891                         SINGLE_MASK(addr->can_addr.tp.rx_id),
 892                         isotp_rcv, sk, "isotp");
 893         dev_put(dev);
 894
 895         if (so->bound) {
 896                 /* unregister old filter */
 897                 if (so->ifindex) {
 898                         dev = dev_get_by_index(&init_net, so->ifindex);
 899                         if (dev) {
 900                                 can_rx_unregister(dev, so->rxid,
 901                                                   SINGLE_MASK(so->rxid),
 902                                                   isotp_rcv, sk);
 903                                 dev_put(dev);
 904                         }
 905                 }
 906         }
 907
 908         /* switch to new settings */
 909         so->ifindex = ifindex;
 910         so->rxid = addr->can_addr.tp.rx_id;
 911         so->txid = addr->can_addr.tp.tx_id;
 912         so->bound = 1;
 913
 914  out:
 915         release_sock(sk);
 916
 917         if (notify_enetdown) {
 918                 sk->sk_err = ENETDOWN;
 919                 if (!sock_flag(sk, SOCK_DEAD))
 920                         sk->sk_error_report(sk);
 921         }
 922
 923         return err;
 924 }
 925
 926 static int isotp_getname(struct socket *sock, struct sockaddr *uaddr,
 927                        int *len, int peer)
 928 {
 929         struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
 930         struct sock *sk = sock->sk;
 931         struct isotp_sock *so = isotp_sk(sk);
 932
 933         if (peer)
 934                 return -EOPNOTSUPP;
 935
 936         addr->can_family  = AF_CAN;
 937         addr->can_ifindex = so->ifindex;
 938
 939         *len = sizeof(*addr);
 940
 941         return 0;
 942 }
 943
 944 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
 945 static int isotp_setsockopt(struct socket *sock, int level, int optname,
 946                             char __user *optval, unsigned int optlen)
 947 #else
 948 static int isotp_setsockopt(struct socket *sock, int level, int optname,
 949                             char __user *optval, int optlen)
 950 #endif
 951 {
 952         struct sock *sk = sock->sk;
 953         struct isotp_sock *so = isotp_sk(sk);
 954         int ret = 0;
 955
 956         if (level != SOL_CAN_ISOTP)
 957                 return -EINVAL;
 958         if (optlen < 0)
 959                 return -EINVAL;
 960
 961         switch (optname) {
 962
 963         case CAN_ISOTP_OPTS:
 964                 if (optlen != sizeof(struct can_isotp_options))
 965                         return -EINVAL;
 966
 967                 if (copy_from_user(&so->opt, optval, optlen))
 968                         return -EFAULT;
 969                 break;
 970
 971         case CAN_ISOTP_RECV_FC:
 972                 if (optlen != sizeof(struct can_isotp_fc_options))
 973                         return -EINVAL;
 974
 975                 if (copy_from_user(&so->rxfc, optval, optlen))
 976                         return -EFAULT;
 977                 break;
 978
 979         case CAN_ISOTP_TX_STMIN:
 980                 if (optlen != sizeof(__u32))
 981                         return -EINVAL;
 982
 983                 if (copy_from_user(&so->force_tx_stmin, optval, optlen))
 984                         return -EFAULT;
 985                 break;
 986
 987         case CAN_ISOTP_RX_STMIN:
 988                 if (optlen != sizeof(__u32))
 989                         return -EINVAL;
 990
 991                 if (copy_from_user(&so->force_rx_stmin, optval, optlen))
 992                         return -EFAULT;
 993                 break;
 994
 995         default:
 996                 ret = -ENOPROTOOPT;
 997         }
 998
 999         return ret;
1000 }
1001
1002 static int isotp_getsockopt(struct socket *sock, int level, int optname,
1003                           char __user *optval, int __user *optlen)
1004 {
1005         struct sock *sk = sock->sk;
1006         struct isotp_sock *so = isotp_sk(sk);
1007         int len;
1008         void *val;
1009
1010         if (level != SOL_CAN_ISOTP)
1011                 return -EINVAL;
1012         if (get_user(len, optlen))
1013                 return -EFAULT;
1014         if (len < 0)
1015                 return -EINVAL;
1016
1017         switch (optname) {
1018
1019         case CAN_ISOTP_OPTS:
1020                 len = min_t(int, len, sizeof(struct can_isotp_options));
1021                 val = &so->opt;
1022                 break;
1023
1024         case CAN_ISOTP_RECV_FC:
1025                 len = min_t(int, len, sizeof(struct can_isotp_fc_options));
1026                 val = &so->rxfc;
1027                 break;
1028
1029         case CAN_ISOTP_TX_STMIN:
1030                 len = min_t(int, len, sizeof(__u32));
1031                 val = &so->force_tx_stmin;
1032                 break;
1033
1034         case CAN_ISOTP_RX_STMIN:
1035                 len = min_t(int, len, sizeof(__u32));
1036                 val = &so->force_rx_stmin;
1037                 break;
1038
1039         default:
1040                 return -ENOPROTOOPT;
1041         }
1042
1043         if (put_user(len, optlen))
1044                 return -EFAULT;
1045         if (copy_to_user(optval, val, len))
1046                 return -EFAULT;
1047         return 0;
1048 }
1049
1050
1051 static int isotp_notifier(struct notifier_block *nb,
1052                         unsigned long msg, void *data)
1053 {
1054         struct net_device *dev = (struct net_device *)data;
1055         struct isotp_sock *so = container_of(nb, struct isotp_sock, notifier);
1056         struct sock *sk = &so->sk;
1057
1058 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
1059         if (dev_net(dev) != &init_net)
1060                 return NOTIFY_DONE;
1061 #elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
1062         if (dev->nd_net != &init_net)
1063                 return NOTIFY_DONE;
1064 #endif
1065
1066         if (dev->type != ARPHRD_CAN)
1067                 return NOTIFY_DONE;
1068
1069         if (so->ifindex != dev->ifindex)
1070                 return NOTIFY_DONE;
1071
1072         switch (msg) {
1073
1074         case NETDEV_UNREGISTER:
1075                 lock_sock(sk);
1076                 /* remove current filters & unregister */
1077                 if (so->bound)
1078                         can_rx_unregister(dev, so->rxid, SINGLE_MASK(so->rxid),
1079                                           isotp_rcv, sk);
1080
1081                 so->ifindex = 0;
1082                 so->bound   = 0;
1083                 release_sock(sk);
1084
1085                 sk->sk_err = ENODEV;
1086                 if (!sock_flag(sk, SOCK_DEAD))
1087                         sk->sk_error_report(sk);
1088                 break;
1089
1090         case NETDEV_DOWN:
1091                 sk->sk_err = ENETDOWN;
1092                 if (!sock_flag(sk, SOCK_DEAD))
1093                         sk->sk_error_report(sk);
1094                 break;
1095         }
1096
1097         return NOTIFY_DONE;
1098 }
1099
1100
1101 static int isotp_init(struct sock *sk)
1102 {
1103         struct isotp_sock *so = isotp_sk(sk);
1104
1105         so->ifindex = 0;
1106         so->bound   = 0;
1107
1108         so->opt.flags           = CAN_ISOTP_DEFAULT_FLAGS;
1109         so->opt.ext_address     = CAN_ISOTP_DEFAULT_EXT_ADDRESS;
1110         so->opt.rxpad_content   = CAN_ISOTP_DEFAULT_RXPAD_CONTENT;
1111         so->opt.txpad_content   = CAN_ISOTP_DEFAULT_TXPAD_CONTENT;
1112         so->opt.frame_txtime    = CAN_ISOTP_DEFAULT_FRAME_TXTIME;
1113         so->rxfc.bs             = CAN_ISOTP_DEFAULT_RECV_BS;
1114         so->rxfc.stmin          = CAN_ISOTP_DEFAULT_RECV_STMIN;
1115         so->rxfc.wftmax         = CAN_ISOTP_DEFAULT_RECV_WFTMAX;
1116
1117         so->rx.state = ISOTP_IDLE;
1118         so->tx.state = ISOTP_IDLE;
1119
1120         hrtimer_init(&so->rxtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
1121         so->rxtimer.function = isotp_rx_timer_handler;
1122         hrtimer_init(&so->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
1123         so->txtimer.function = isotp_tx_timer_handler;
1124
1125         tasklet_init(&so->txtsklet, isotp_tx_timer_tsklet, (unsigned long)so);
1126
1127         init_waitqueue_head(&so->wait);
1128
1129         so->notifier.notifier_call = isotp_notifier;
1130         register_netdevice_notifier(&so->notifier);
1131
1132         return 0;
1133 }
1134
1135
1136 static const struct proto_ops isotp_ops = {
1137         .family        = PF_CAN,
1138         .release       = isotp_release,
1139         .bind          = isotp_bind,
1140         .connect       = sock_no_connect,
1141         .socketpair    = sock_no_socketpair,
1142         .accept        = sock_no_accept,
1143         .getname       = isotp_getname,
1144         .poll          = datagram_poll,
1145         .ioctl         = can_ioctl,     /* use can_ioctl() from af_can.c */
1146         .listen        = sock_no_listen,
1147         .shutdown      = sock_no_shutdown,
1148         .setsockopt    = isotp_setsockopt,
1149         .getsockopt    = isotp_getsockopt,
1150         .sendmsg       = isotp_sendmsg,
1151         .recvmsg       = isotp_recvmsg,
1152         .mmap          = sock_no_mmap,
1153         .sendpage      = sock_no_sendpage,
1154 };
1155
1156 static struct proto isotp_proto __read_mostly = {
1157         .name       = "CAN_ISOTP",
1158         .owner      = THIS_MODULE,
1159         .obj_size   = sizeof(struct isotp_sock),
1160         .init       = isotp_init,
1161 };
1162
1163 static const struct can_proto isotp_can_proto = {
1164         .type       = SOCK_DGRAM,
1165         .protocol   = CAN_ISOTP,
1166 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,33)
1167         .capability = -1,
1168 #endif
1169         .ops        = &isotp_ops,
1170         .prot       = &isotp_proto,
1171 };
1172
1173 static __init int isotp_module_init(void)
1174 {
1175         int err;
1176
1177         printk(banner);
1178
1179         err = can_proto_register(&isotp_can_proto);
1180         if (err < 0)
1181                 printk(KERN_ERR "can: registration of isotp protocol failed\n");
1182
1183         return err;
1184 }
1185
1186 static __exit void isotp_module_exit(void)
1187 {
1188         can_proto_unregister(&isotp_can_proto);
1189 }
1190
1191 module_init(isotp_module_init);
1192 module_exit(isotp_module_exit);