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