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