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