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