kernel/2.6/net/can/isotp.c

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