kernel/2.6/net/can/bcm.c

   1 /*
   2  * bcm.c - Broadcast Manager to filter/send (cyclic) CAN content
   3  *
   4  * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
   5  * All rights reserved.
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  * 1. Redistributions of source code must retain the above copyright
  11  *    notice, this list of conditions, the following disclaimer and
  12  *    the referenced file 'COPYING'.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in the
  15  *    documentation and/or other materials provided with the distribution.
  16  * 3. Neither the name of Volkswagen nor the names of its contributors
  17  *    may be used to endorse or promote products derived from this software
  18  *    without specific prior written permission.
  19  *
  20  * Alternatively, provided that this notice is retained in full, this
  21  * software may be distributed under the terms of the GNU General
  22  * Public License ("GPL") version 2 as distributed in the 'COPYING'
  23  * file from the main directory of the linux kernel source.
  24  *
  25  * The provided data structures and external interfaces from this code
  26  * are not restricted to be used by modules with a GPL compatible license.
  27  *
  28  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  29  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  30  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  31  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  32  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  33  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  34  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  35  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  36  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  37  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  38  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  39  * DAMAGE.
  40  *
  41  * Send feedback to <socketcan-users@lists.berlios.de>
  42  *
  43  */
  44
  45 #include <linux/autoconf.h>
  46 #include <linux/module.h>
  47 #include <linux/init.h>
  48 #include <linux/net.h>
  49 #include <linux/netdevice.h>
  50 #include <linux/proc_fs.h>
  51 #include <linux/poll.h>
  52 #include <net/sock.h>
  53
  54 #include <linux/can.h>
  55 #include <linux/can/core.h>
  56 #include <linux/can/bcm.h>
  57 #include <linux/can/version.h>
  58
  59 RCSID("$Id$");
  60
  61 #ifdef CONFIG_CAN_DEBUG_CORE
  62 static int debug = 0;
  63 module_param(debug, int, S_IRUGO);
  64 #define DBG(args...)       (debug & 1 ? \
  65                                (printk(KERN_DEBUG "BCM %s: ", __func__), \
  66                                 printk(args)) : 0)
  67 #define DBG_FRAME(args...) (debug & 2 ? can_debug_cframe(args) : 0)
  68 #define DBG_SKB(skb)       (debug & 4 ? can_debug_skb(skb) : 0)
  69 #else
  70 #define DBG(args...)
  71 #define DBG_FRAME(args...)
  72 #define DBG_SKB(skb)
  73 #endif
  74
  75 /* use of last_frames[index].can_dlc */
  76 #define RX_RECV    0x40 /* received data for this element */
  77 #define RX_THR     0x80 /* element not been sent due to throttle feature */
  78 #define BCM_CAN_DLC_MASK 0x0F /* clean private flags in can_dlc by masking */
  79
  80 /* get best masking value for can_rx_register() for a given single can_id */
  81 #define REGMASK(id) ((id & CAN_RTR_FLAG) | ((id & CAN_EFF_FLAG) ? \
  82                         (CAN_EFF_MASK | CAN_EFF_FLAG) : CAN_SFF_MASK))
  83
  84 #define IDENT "bcm"
  85 static __initdata const char banner[] = KERN_INFO
  86         "CAN: broadcast manager (bcm) socket protocol " VERSION "\n";
  87
  88 MODULE_DESCRIPTION("PF_CAN bcm sockets");
  89 MODULE_LICENSE("Dual BSD/GPL");
  90 MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
  91
  92 #define GET_U64(p) (*(u64*)(p)->data) /* easy access */
  93
  94 struct bcm_op {
  95         struct list_head list;
  96         int ifindex;
  97         canid_t can_id;
  98         int flags;
  99         unsigned long j_ival1, j_ival2, j_lastmsg;
 100         unsigned long frames_abs, frames_filtered;
 101         struct timer_list timer, thrtimer;
 102         struct timeval ival1, ival2;
 103         struct timeval rx_stamp;
 104         int rx_ifindex;
 105         int count;
 106         int nframes;
 107         int currframe;
 108         struct can_frame *frames;
 109         struct can_frame *last_frames;
 110         struct sock *sk;
 111 };
 112
 113 struct bcm_opt {
 114         int bound;
 115         int ifindex;
 116         struct list_head rx_ops;
 117         struct list_head tx_ops;
 118         unsigned long dropped_usr_msgs;
 119         struct proc_dir_entry *bcm_proc_read;
 120         char procname [9]; /* pointer printed in ASCII with \0 */
 121 };
 122
 123 static struct proc_dir_entry *proc_dir = NULL;
 124
 125 static int  bcm_init(struct sock *sk);
 126 static void bcm_notifier(unsigned long msg, void *data);
 127 static int  bcm_release(struct socket *sock);
 128 static int  bcm_connect(struct socket *sock, struct sockaddr *uaddr, int len,
 129                         int flags);
 130 static int  bcm_sendmsg(struct kiocb *iocb, struct socket *sock,
 131                         struct msghdr *msg, size_t size);
 132 static int  bcm_recvmsg(struct kiocb *iocb, struct socket *sock,
 133                         struct msghdr *msg, size_t size, int flags);
 134 static unsigned int bcm_poll(struct file *file, struct socket *sock,
 135                              poll_table *wait);
 136
 137 static int  bcm_read_proc(char *page, char **start, off_t off,
 138                           int count, int *eof, void *data);
 139
 140 static void bcm_tx_timeout_handler(unsigned long data);
 141 static int  bcm_tx_send(struct msghdr *msg, int ifindex, struct sock *sk);
 142 static int  bcm_tx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg,
 143                          int ifindex, struct sock *sk);
 144 static void bcm_can_tx(struct bcm_op *op);
 145
 146 static int  bcm_rx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg,
 147                          int ifindex, struct sock *sk);
 148 static void bcm_rx_handler(struct sk_buff *skb, void *op);
 149 static void bcm_rx_timeout_handler(unsigned long data);
 150 static void bcm_rx_thr_handler(unsigned long data);
 151 static void bcm_rx_cmp_to_index(struct bcm_op *op, int index,
 152                                 struct can_frame *rxdata);
 153 static void bcm_rx_changed(struct bcm_op *op, struct can_frame *data);
 154 static void bcm_rx_starttimer(struct bcm_op *op);
 155 static void bcm_rx_update_and_send(struct bcm_op *op,
 156                                    struct can_frame *lastdata,
 157                                    struct can_frame *rxdata);
 158 static void bcm_send_to_user(struct bcm_op *op, struct bcm_msg_head *head,
 159                              struct can_frame *frames, struct timeval *tv);
 160
 161 static int  bcm_delete_tx_op(struct list_head *ops, canid_t can_id,
 162                              int ifindex);
 163 static int  bcm_delete_rx_op(struct list_head *ops, canid_t can_id,
 164                              int ifindex);
 165 static void bcm_remove_op(struct bcm_op *op);
 166 static int  bcm_read_op(struct list_head *ops, struct bcm_msg_head *msg_head,
 167                         int ifindex);
 168 static struct bcm_op *bcm_find_op(struct list_head *ops, canid_t can_id,
 169                                   int ifindex);
 170
 171 static struct proto_ops bcm_ops = {
 172         .family        = PF_CAN,
 173         .release       = bcm_release,
 174         .bind          = sock_no_bind,
 175         .connect       = bcm_connect,
 176         .socketpair    = sock_no_socketpair,
 177         .accept        = sock_no_accept,
 178         .getname       = sock_no_getname,
 179         .poll          = bcm_poll,
 180         .ioctl         = NULL,          /* use can_ioctl() from af_can.c */
 181         .listen        = sock_no_listen,
 182         .shutdown      = sock_no_shutdown,
 183         .setsockopt    = sock_no_setsockopt,
 184         .getsockopt    = sock_no_getsockopt,
 185         .sendmsg       = bcm_sendmsg,
 186         .recvmsg       = bcm_recvmsg,
 187         .mmap          = sock_no_mmap,
 188         .sendpage      = sock_no_sendpage,
 189 };
 190
 191 #ifdef CONFIG_CAN_BCM_USER
 192 #define BCM_CAP (-1)
 193 #else
 194 #define BCM_CAP CAP_NET_RAW
 195 #endif
 196
 197 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,13)
 198 struct bcm_sock {
 199         struct sock    sk;
 200         struct bcm_opt opt;
 201 };
 202
 203 #define bcm_sk(sk) (&((struct bcm_sock *)(sk))->opt)
 204
 205 static struct proto bcm_proto = {
 206         .name       = "CAN_BCM",
 207         .owner      = THIS_MODULE,
 208         .obj_size   = sizeof(struct bcm_sock),
 209         .init       = bcm_init,
 210 };
 211
 212 static struct can_proto bcm_can_proto = {
 213         .type       = SOCK_DGRAM,
 214         .protocol   = CAN_BCM,
 215         .capability = BCM_CAP,
 216         .ops        = &bcm_ops,
 217         .prot       = &bcm_proto,
 218 };
 219 #else
 220 #define bcm_sk(sk) ((struct bcm_opt *)(sk)->sk_protinfo)
 221
 222 static struct can_proto bcm_can_proto = {
 223         .type       = SOCK_DGRAM,
 224         .protocol   = CAN_BCM,
 225         .capability = BCM_CAP,
 226         .ops        = &bcm_ops,
 227         .owner      = THIS_MODULE,
 228         .obj_size   = sizeof(struct bcm_opt),
 229         .init       = bcm_init,
 230 };
 231 #endif
 232
 233 #define CFSIZ sizeof(struct can_frame)
 234 #define OPSIZ sizeof(struct bcm_op)
 235 #define MHSIZ sizeof(struct bcm_msg_head)
 236
 237 static int __init bcm_module_init(void)
 238 {
 239         printk(banner);
 240
 241         can_proto_register(&bcm_can_proto);
 242
 243         /* create /proc/net/can/bcm directory */
 244         proc_dir = proc_mkdir(CAN_PROC_DIR"/"IDENT, NULL);
 245
 246         if (proc_dir)
 247                 proc_dir->owner = THIS_MODULE;
 248
 249         return 0;
 250 }
 251
 252 static void __exit bcm_module_exit(void)
 253 {
 254         can_proto_unregister(&bcm_can_proto);
 255
 256         if (proc_dir)
 257                 remove_proc_entry(CAN_PROC_DIR"/"IDENT, NULL);
 258
 259 }
 260
 261 /* initial settings at socket creation time */
 262
 263 static int bcm_init(struct sock *sk)
 264 {
 265         struct bcm_opt *bo = bcm_sk(sk);
 266
 267         bo->bound            = 0;
 268         bo->ifindex          = 0;
 269         bo->dropped_usr_msgs = 0;
 270         bo->bcm_proc_read    = NULL;
 271
 272         INIT_LIST_HEAD(&bo->tx_ops);
 273         INIT_LIST_HEAD(&bo->rx_ops);
 274
 275         return 0;
 276 }
 277
 278 /* handling of netdevice problems */
 279
 280 static void bcm_notifier(unsigned long msg, void *data)
 281 {
 282         struct sock *sk = (struct sock *)data;
 283         struct bcm_opt *bo = bcm_sk(sk);
 284
 285         DBG("called for sock %p\n", sk);
 286
 287         switch (msg) {
 288         case NETDEV_UNREGISTER:
 289                 bo->bound   = 0;
 290                 bo->ifindex = 0;
 291                 /* fallthrough */
 292         case NETDEV_DOWN:
 293                 sk->sk_err = ENETDOWN;
 294                 if (!sock_flag(sk, SOCK_DEAD))
 295                         sk->sk_error_report(sk);
 296         }
 297 }
 298
 299 /* standard socket functions */
 300
 301 static int bcm_release(struct socket *sock)
 302 {
 303         struct sock *sk = sock->sk;
 304         struct bcm_opt *bo = bcm_sk(sk);
 305         struct bcm_op *op, *next;
 306
 307         DBG("socket %p, sk %p\n", sock, sk);
 308
 309         /* remove bcm_ops, timer, rx_unregister(), etc. */
 310
 311         list_for_each_entry_safe(op, next, &bo->tx_ops, list) {
 312                 DBG("removing tx_op %p for can_id %03X\n", op, op->can_id);
 313                 bcm_remove_op(op);
 314         }
 315
 316         list_for_each_entry_safe(op, next, &bo->rx_ops, list) {
 317                 DBG("removing rx_op %p for can_id %03X\n", op, op->can_id);
 318
 319                 /*
 320                  * Don't care if we're bound or not (due to netdev problems)
 321                  * can_rx_unregister() is always a save thing to do here.
 322                  */
 323                 if (op->ifindex) {
 324                         struct net_device *dev = dev_get_by_index(op->ifindex);
 325
 326                         if (dev) {
 327                                 can_rx_unregister(dev, op->can_id,
 328                                                   REGMASK(op->can_id),
 329                                                   bcm_rx_handler, op);
 330                                 dev_put(dev);
 331                         }
 332                 } else
 333                         can_rx_unregister(NULL, op->can_id,
 334                                           REGMASK(op->can_id),
 335                                           bcm_rx_handler, op);
 336
 337                 bcm_remove_op(op);
 338         }
 339
 340         /* remove procfs entry */
 341         if ((proc_dir) && (bo->bcm_proc_read)) {
 342                 remove_proc_entry(bo->procname, proc_dir);
 343         }
 344
 345         /* remove device notifier */
 346         if (bo->ifindex) {
 347                 struct net_device *dev = dev_get_by_index(bo->ifindex);
 348
 349                 if (dev) {
 350                         can_dev_unregister(dev, bcm_notifier, sk);
 351                         dev_put(dev);
 352                 }
 353         }
 354
 355         sock_put(sk);
 356
 357         return 0;
 358 }
 359
 360 static int bcm_connect(struct socket *sock, struct sockaddr *uaddr, int len,
 361                        int flags)
 362 {
 363         struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
 364         struct sock *sk = sock->sk;
 365         struct bcm_opt *bo = bcm_sk(sk);
 366
 367         if (bo->bound)
 368                 return -EISCONN;
 369
 370         /* bind a device to this socket */
 371         if (addr->can_ifindex) {
 372                 struct net_device *dev = dev_get_by_index(addr->can_ifindex);
 373
 374                 if (!dev) {
 375                         DBG("could not find device index %d\n",
 376                             addr->can_ifindex);
 377                         return -ENODEV;
 378                 }
 379                 bo->ifindex = dev->ifindex;
 380                 can_dev_register(dev, bcm_notifier, sk); /* register notif. */
 381                 dev_put(dev);
 382
 383                 DBG("socket %p bound to device %s (idx %d)\n",
 384                     sock, dev->name, dev->ifindex);
 385         } else {
 386                 /* no notifier for ifindex = 0 ('any' CAN device) */
 387                 bo->ifindex = 0;
 388         }
 389
 390         bo->bound = 1;
 391
 392         if (proc_dir) {
 393                 /* unique socket address as filename */
 394                 sprintf(bo->procname, "%p", sock);
 395                 bo->bcm_proc_read = create_proc_read_entry(bo->procname, 0644,
 396                                                            proc_dir,
 397                                                            bcm_read_proc, sk);
 398         }
 399
 400         return 0;
 401 }
 402
 403 static int bcm_sendmsg(struct kiocb *iocb, struct socket *sock,
 404                        struct msghdr *msg, size_t size)
 405 {
 406         struct sock *sk = sock->sk;
 407         struct bcm_opt *bo = bcm_sk(sk);
 408         int ifindex = bo->ifindex; /* default ifindex for this bcm_op */
 409         struct bcm_msg_head msg_head;
 410         int ret; /* read bytes or error codes as return value */
 411
 412         if (!bo->bound) {
 413                 DBG("sock %p not bound\n", sk);
 414                 return -ENOTCONN;
 415         }
 416
 417         /* check for alternative ifindex for this bcm_op */
 418
 419         if (!ifindex && msg->msg_name) {
 420                 /* no bound device as default => check msg_name */
 421                 struct sockaddr_can *addr =
 422                         (struct sockaddr_can *)msg->msg_name;
 423
 424                 if (addr->can_family != AF_CAN)
 425                         return -EINVAL;
 426
 427                 ifindex = addr->can_ifindex; /* ifindex from sendto() */
 428
 429                 if (ifindex && !dev_get_by_index(ifindex)) {
 430                         DBG("device %d not found\n", ifindex);
 431                         return -ENODEV;
 432                 }
 433         }
 434
 435         /* read message head information */
 436
 437         ret = memcpy_fromiovec((u8*)&msg_head, msg->msg_iov, MHSIZ);
 438         if (ret < 0)
 439                 return ret;
 440
 441         DBG("opcode %d for can_id %03X\n", msg_head.opcode, msg_head.can_id);
 442
 443         switch (msg_head.opcode) {
 444
 445         case TX_SETUP:
 446
 447                 ret = bcm_tx_setup(&msg_head, msg, ifindex, sk);
 448                 break;
 449
 450         case RX_SETUP:
 451
 452                 ret = bcm_rx_setup(&msg_head, msg, ifindex, sk);
 453                 break;
 454
 455         case TX_DELETE:
 456
 457                 if (bcm_delete_tx_op(&bo->tx_ops, msg_head.can_id, ifindex))
 458                         ret = MHSIZ;
 459                 else
 460                         ret = -EINVAL;
 461                 break;
 462
 463         case RX_DELETE:
 464
 465                 if (bcm_delete_rx_op(&bo->rx_ops, msg_head.can_id, ifindex))
 466                         ret = MHSIZ;
 467                 else
 468                         ret = -EINVAL;
 469                 break;
 470
 471         case TX_READ:
 472
 473                 /* reuse msg_head for the reply to TX_READ */
 474                 msg_head.opcode  = TX_STATUS;
 475                 ret = bcm_read_op(&bo->tx_ops, &msg_head, ifindex);
 476                 break;
 477
 478         case RX_READ:
 479
 480                 /* reuse msg_head for the reply to RX_READ */
 481                 msg_head.opcode  = RX_STATUS;
 482                 ret = bcm_read_op(&bo->rx_ops, &msg_head, ifindex);
 483                 break;
 484
 485         case TX_SEND:
 486
 487                 /* we need at least one can_frame */
 488                 if (msg_head.nframes < 1)
 489                         return -EINVAL;
 490
 491                 ret = bcm_tx_send(msg, ifindex, sk);
 492                 break;
 493
 494         default:
 495
 496                 DBG("Unknown opcode %d\n", msg_head.opcode);
 497                 ret = -EINVAL;
 498                 break;
 499         }
 500
 501         return ret;
 502 }
 503
 504 static int bcm_recvmsg(struct kiocb *iocb, struct socket *sock,
 505                        struct msghdr *msg, size_t size, int flags)
 506 {
 507         struct sock *sk = sock->sk;
 508         struct sk_buff *skb;
 509         int error = 0;
 510         int noblock;
 511         int err;
 512
 513         DBG("socket %p, sk %p\n", sock, sk);
 514
 515         noblock =  flags & MSG_DONTWAIT;
 516         flags   &= ~MSG_DONTWAIT;
 517         skb = skb_recv_datagram(sk, flags, noblock, &error);
 518         if (!skb)
 519                 return error;
 520
 521         DBG("delivering skbuff %p\n", skb);
 522         DBG_SKB(skb);
 523
 524         if (skb->len < size)
 525                 size = skb->len;
 526
 527         err = memcpy_toiovec(msg->msg_iov, skb->data, size);
 528         if (err < 0) {
 529                 skb_free_datagram(sk, skb);
 530                 return err;
 531         }
 532
 533         sock_recv_timestamp(msg, sk, skb);
 534
 535         if (msg->msg_name) {
 536                 msg->msg_namelen = sizeof(struct sockaddr_can);
 537                 memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
 538         }
 539
 540         DBG("freeing sock %p, skbuff %p\n", sk, skb);
 541         skb_free_datagram(sk, skb);
 542
 543         return size;
 544 }
 545
 546 static unsigned int bcm_poll(struct file *file, struct socket *sock,
 547                              poll_table *wait)
 548 {
 549         unsigned int mask = 0;
 550
 551         DBG("socket %p\n", sock);
 552
 553         mask = datagram_poll(file, sock, wait);
 554         return mask;
 555 }
 556
 557 /* helper functions for bcm_sendmsg() */
 558
 559 static int bcm_tx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg,
 560                         int ifindex, struct sock *sk)
 561 {
 562         struct bcm_opt *bo = bcm_sk(sk);
 563         struct bcm_op *op;
 564         int i, err;
 565
 566         /* we need a real device to send frames */
 567         if (!ifindex)
 568                 return -ENODEV;
 569
 570         /* we need at least one can_frame */
 571         if (msg_head->nframes < 1)
 572                 return -EINVAL;
 573
 574         /* check the given can_id */
 575
 576         op = bcm_find_op(&bo->tx_ops, msg_head->can_id, ifindex);
 577         if (op) {
 578
 579                 /* update existing BCM operation */
 580
 581                 DBG("TX_SETUP: modifying existing tx_op %p for can_id %03X\n",
 582                     op, msg_head->can_id);
 583
 584                 /*
 585                  * Do we need more space for the can_frames than currently
 586                  * allocated? -> This is a _really_ unusual use-case and
 587                  * therefore (complexity / locking) it is not supported.
 588                  */
 589                 if (msg_head->nframes > op->nframes)
 590                         return -E2BIG;
 591
 592                 /* update can_frames content */
 593                 for (i = 0; i < msg_head->nframes; i++) {
 594                         err = memcpy_fromiovec((u8*)&op->frames[i],
 595                                                msg->msg_iov, CFSIZ);
 596                         if (err < 0)
 597                                 return err;
 598
 599                         if (msg_head->flags & TX_CP_CAN_ID) {
 600                                 /* copy can_id into frame */
 601                                 op->frames[i].can_id = msg_head->can_id;
 602                         }
 603                 }
 604
 605         } else {
 606                 /* insert new BCM operation for the given can_id */
 607
 608                 op = kzalloc(OPSIZ, GFP_KERNEL);
 609                 if (!op)
 610                         return -ENOMEM;
 611
 612                 DBG("TX_SETUP: creating new tx_op %p for can_id %03X\n",
 613                     op, msg_head->can_id);
 614
 615                 op->can_id    = msg_head->can_id;
 616
 617                 /* create array for can_frames and copy the data */
 618                 op->frames = kmalloc(msg_head->nframes * CFSIZ, GFP_KERNEL);
 619                 if (!op->frames) {
 620                         kfree(op);
 621                         return -ENOMEM;
 622                 }
 623
 624                 for (i = 0; i < msg_head->nframes; i++) {
 625                         err = memcpy_fromiovec((u8*)&op->frames[i],
 626                                                msg->msg_iov, CFSIZ);
 627                         if (err < 0) {
 628                                 kfree(op->frames);
 629                                 kfree(op);
 630                                 return err;
 631                         }
 632
 633                         if (msg_head->flags & TX_CP_CAN_ID) {
 634                                 /* copy can_id into frame */
 635                                 op->frames[i].can_id = msg_head->can_id;
 636                         }
 637                 }
 638
 639                 /* tx_ops never compare with previous received messages */
 640                 op->last_frames = NULL;
 641
 642                 /* bcm_can_tx / bcm_tx_timeout_handler needs this */
 643                 op->sk = sk;
 644
 645                 op->ifindex = ifindex;
 646
 647                 /* initialize uninitialized (kmalloc) structure */
 648                 init_timer(&op->timer);
 649
 650                 /* currently unused in tx_ops */
 651                 init_timer(&op->thrtimer);
 652
 653                 /* handler for tx_ops */
 654                 op->timer.function = bcm_tx_timeout_handler;
 655
 656                 /* timer.data points to this op-structure */
 657                 op->timer.data = (unsigned long)op;
 658
 659                 /* add this bcm_op to the list of the tx_ops */
 660                 list_add(&op->list, &bo->tx_ops);
 661
 662         } /* if ((op = bcm_find_op(&bo->tx_ops, msg_head->can_id, ifindex))) */
 663
 664         if (op->nframes != msg_head->nframes) {
 665                 op->nframes   = msg_head->nframes;
 666                 /* start multiple frame transmission with index 0 */
 667                 op->currframe = 0;
 668         }
 669
 670         /* check flags */
 671
 672         op->flags = msg_head->flags;
 673
 674         if (op->flags & TX_RESET_MULTI_IDX) {
 675                 /* start multiple frame transmission with index 0 */
 676                 op->currframe = 0;
 677         }
 678
 679         if (op->flags & SETTIMER) {
 680
 681                 /* set timer values */
 682
 683                 op->count = msg_head->count;
 684                 op->ival1 = msg_head->ival1;
 685                 op->ival2 = msg_head->ival2;
 686                 op->j_ival1 = timeval2jiffies(&msg_head->ival1, 1);
 687                 op->j_ival2 = timeval2jiffies(&msg_head->ival2, 1);
 688
 689                 DBG("TX_SETUP: SETTIMER count=%d j_ival1=%ld j_ival2=%ld\n",
 690                     op->count, op->j_ival1, op->j_ival2);
 691
 692                 /* disable an active timer due to zero values? */
 693                 if (!op->j_ival1 && !op->j_ival2) {
 694                         del_timer(&op->timer);
 695                         DBG("TX_SETUP: SETTIMER disabled timer.\n");
 696                 }
 697         }
 698
 699         if ((op->flags & STARTTIMER) &&
 700             ((op->j_ival1 && op->count) || op->j_ival2)) {
 701
 702                 del_timer(&op->timer);
 703
 704                 /* spec: send can_frame when starting timer */
 705                 op->flags |= TX_ANNOUNCE;
 706
 707                 if (op->j_ival1 && (op->count > 0)){
 708                         op->timer.expires = jiffies + op->j_ival1;
 709                         /* op->count-- is done in bcm_tx_timeout_handler */
 710                         DBG("TX_SETUP: adding timer ival1. func=%p data=%p "
 711                             "exp=0x%08X\n",
 712                             op->timer.function,
 713                             (char*) op->timer.data,
 714                             (unsigned int) op->timer.expires);
 715                 } else{
 716                         op->timer.expires = jiffies + op->j_ival2;
 717                         DBG("TX_SETUP: adding timer ival2. func=%p data=%p "
 718                             "exp=0x%08X\n",
 719                             op->timer.function,
 720                             (char*) op->timer.data,
 721                             (unsigned int) op->timer.expires);
 722                 }
 723
 724                 add_timer(&op->timer);
 725         }
 726
 727         if (op->flags & TX_ANNOUNCE)
 728                 bcm_can_tx(op);
 729
 730         return msg_head->nframes * CFSIZ + MHSIZ;
 731 }
 732
 733 static int bcm_rx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg,
 734                         int ifindex, struct sock *sk)
 735 {
 736         struct bcm_opt *bo = bcm_sk(sk);
 737         struct bcm_op *op;
 738         int do_rx_register;
 739         int err;
 740
 741         if ((msg_head->flags & RX_FILTER_ID) || (!(msg_head->nframes))) {
 742                 /* be robust against wrong usage ... */
 743                 msg_head->flags |= RX_FILTER_ID;
 744                 msg_head->nframes = 0; /* ignore trailing garbage */
 745         }
 746
 747         if ((msg_head->flags & RX_RTR_FRAME) &&
 748             ((msg_head->nframes != 1) ||
 749              (!(msg_head->can_id & CAN_RTR_FLAG)))) {
 750
 751                 DBG("RX_SETUP: bad RX_RTR_FRAME setup!\n");
 752                 return -EINVAL;
 753         }
 754
 755         /* check the given can_id */
 756         op = bcm_find_op(&bo->rx_ops, msg_head->can_id, ifindex);
 757         if (op) {
 758
 759                 /* update existing BCM operation */
 760
 761                 DBG("RX_SETUP: modifying existing rx_op %p for can_id %03X\n",
 762                     op, msg_head->can_id);
 763
 764                 /*
 765                  * Do we need more space for the can_frames than currently
 766                  * allocated? -> This is a _really_ unusual use-case and
 767                  * therefore (complexity / locking) it is not supported.
 768                  */
 769                 if (msg_head->nframes > op->nframes)
 770                         return -E2BIG;
 771
 772                 if (msg_head->nframes) {
 773                         /* update can_frames content */
 774                         err = memcpy_fromiovec((u8*)op->frames,
 775                                                msg->msg_iov,
 776                                                msg_head->nframes * CFSIZ);
 777                         if (err < 0)
 778                                 return err;
 779
 780                         /* clear last_frames to indicate 'nothing received' */
 781                         memset(op->last_frames, 0, msg_head->nframes * CFSIZ);
 782                 }
 783
 784                 op->nframes = msg_head->nframes;
 785                 /* Only an update -> do not call can_rx_register() */
 786                 do_rx_register = 0;
 787
 788         } else {
 789                 /* insert new BCM operation for the given can_id */
 790
 791                 op = kzalloc(OPSIZ, GFP_KERNEL);
 792                 if (!op)
 793                         return -ENOMEM;
 794
 795                 DBG("RX_SETUP: creating new rx_op %p for can_id %03X\n",
 796                     op, msg_head->can_id);
 797
 798                 op->can_id    = msg_head->can_id;
 799                 op->nframes   = msg_head->nframes;
 800
 801                 if (msg_head->nframes) {
 802
 803                         /* create array for can_frames and copy the data */
 804                         op->frames = kmalloc(msg_head->nframes * CFSIZ,
 805                                              GFP_KERNEL);
 806                         if (!op->frames) {
 807                                 kfree(op);
 808                                 return -ENOMEM;
 809                         }
 810
 811                         err = memcpy_fromiovec((u8*)op->frames, msg->msg_iov,
 812                                                msg_head->nframes * CFSIZ);
 813                         if (err < 0) {
 814                                 kfree(op->frames);
 815                                 kfree(op);
 816                                 return err;
 817                         }
 818
 819                         /* create and init array for received can_frames */
 820                         op->last_frames = kzalloc(msg_head->nframes * CFSIZ,
 821                                                   GFP_KERNEL);
 822                         if (!op->last_frames) {
 823                                 kfree(op->frames);
 824                                 kfree(op);
 825                                 return -ENOMEM;
 826                         }
 827                 } else {
 828                         /* op->frames = NULL due to memset in kzalloc() */
 829
 830                         /*
 831                          * even when we have the RX_FILTER_ID case, we need
 832                          * to store the last frame for the throttle feature
 833                          */
 834
 835                         /* create and init array for received can_frames */
 836                         op->last_frames = kzalloc(CFSIZ, GFP_KERNEL);
 837                         if (!op->last_frames) {
 838                                 kfree(op);
 839                                 return -ENOMEM;
 840                         }
 841                 }
 842
 843                 op->sk = sk; /* bcm_delete_rx_op() needs this */
 844                 op->ifindex = ifindex;
 845
 846                 /* initialize uninitialized (kmalloc) structure */
 847                 init_timer(&op->timer);
 848
 849                 /* init throttle timer for RX_CHANGED */
 850                 init_timer(&op->thrtimer);
 851
 852                 /* handler for rx timeouts */
 853                 op->timer.function = bcm_rx_timeout_handler;
 854
 855                 /* timer.data points to this op-structure */
 856                 op->timer.data = (unsigned long)op;
 857
 858                 /* handler for RX_CHANGED throttle timeouts */
 859                 op->thrtimer.function = bcm_rx_thr_handler;
 860
 861                 /* timer.data points to this op-structure */
 862                 op->thrtimer.data = (unsigned long)op;
 863
 864                 op->thrtimer.expires = 0; /* mark disabled timer */
 865
 866                 /* add this bcm_op to the list of the tx_ops */
 867                 list_add(&op->list, &bo->rx_ops);
 868
 869                 do_rx_register = 1; /* call can_rx_register() */
 870
 871         } /* if ((op = bcm_find_op(&bo->rx_ops, msg_head->can_id, ifindex))) */
 872
 873
 874         /* check flags */
 875
 876         op->flags = msg_head->flags;
 877
 878         if (op->flags & RX_RTR_FRAME) {
 879
 880                 /* no timers in RTR-mode */
 881                 del_timer(&op->thrtimer);
 882                 del_timer(&op->timer);
 883
 884                 /*
 885                  * funny feature in RX(!)_SETUP only for RTR-mode:
 886                  * copy can_id into frame BUT without RTR-flag to
 887                  * prevent a full-load-loopback-test ... ;-]
 888                  */
 889                 if ((op->flags & TX_CP_CAN_ID) ||
 890                     (op->frames[0].can_id == op->can_id))
 891                         op->frames[0].can_id = op->can_id & ~CAN_RTR_FLAG;
 892
 893         } else {
 894                 if (op->flags & SETTIMER) {
 895
 896                         /* set timer value */
 897
 898                         op->ival1 = msg_head->ival1;
 899                         op->ival2 = msg_head->ival2;
 900                         op->j_ival1 = timeval2jiffies(&msg_head->ival1, 1);
 901                         op->j_ival2 = timeval2jiffies(&msg_head->ival2, 1);
 902
 903                         DBG("RX_SETUP: SETTIMER j_ival1=%ld j_ival2=%ld\n",
 904                             op->j_ival1, op->j_ival2);
 905
 906                         /* disable an active timer due to zero value? */
 907                         if (!op->j_ival1) {
 908                                 del_timer(&op->timer);
 909                                 DBG("RX_SETUP: disabled timer rx timeouts.\n");
 910                         }
 911
 912                         /* free currently blocked msgs ? */
 913                         if (op->thrtimer.expires) {
 914                                 DBG("RX_SETUP: unblocking throttled msgs.\n");
 915                                 del_timer(&op->thrtimer);
 916                                 /* send blocked msgs hereafter */
 917                                 op->thrtimer.expires = jiffies + 2;
 918                                 add_timer(&op->thrtimer);
 919                         }
 920                         /*
 921                          * if (op->j_ival2) is zero, no (new) throttling
 922                          * will happen. For details see functions
 923                          * bcm_rx_update_and_send() and bcm_rx_thr_handler()
 924                          */
 925                 }
 926
 927                 if ((op->flags & STARTTIMER) && op->j_ival1) {
 928
 929                         del_timer(&op->timer);
 930
 931                         op->timer.expires = jiffies + op->j_ival1;
 932
 933                         DBG("RX_SETUP: adding timer ival1. func=%p data=%p"
 934                             " exp=0x%08X\n",
 935                             (char *) op->timer.function,
 936                             (char *) op->timer.data,
 937                             (unsigned int) op->timer.expires);
 938
 939                         add_timer(&op->timer);
 940                 }
 941         }
 942
 943         /* now we can register for can_ids, if we added a new bcm_op */
 944         if (do_rx_register) {
 945                 DBG("RX_SETUP: can_rx_register() for can_id %03X. "
 946                     "rx_op is %p\n", op->can_id, op);
 947
 948                 if (ifindex) {
 949                         struct net_device *dev = dev_get_by_index(ifindex);
 950
 951                         if (dev) {
 952                                 can_rx_register(dev, op->can_id,
 953                                                 REGMASK(op->can_id),
 954                                                 bcm_rx_handler, op, IDENT);
 955                                 dev_put(dev);
 956                         }
 957                 } else
 958                         can_rx_register(NULL, op->can_id, REGMASK(op->can_id),
 959                                         bcm_rx_handler, op, IDENT);
 960         }
 961
 962         return msg_head->nframes * CFSIZ + MHSIZ;
 963 }
 964
 965 static int bcm_tx_send(struct msghdr *msg, int ifindex, struct sock *sk)
 966 {
 967         struct sk_buff *skb;
 968         struct net_device *dev;
 969         int err;
 970
 971         /* just copy and send one can_frame */
 972
 973         if (!ifindex) /* we need a real device to send frames */
 974                 return -ENODEV;
 975
 976         skb = alloc_skb(CFSIZ, GFP_KERNEL);
 977
 978         if (!skb)
 979                 return -ENOMEM;
 980
 981         err = memcpy_fromiovec(skb_put(skb, CFSIZ), msg->msg_iov, CFSIZ);
 982         if (err < 0) {
 983                 kfree_skb(skb);
 984                 return err;
 985         }
 986
 987         DBG_FRAME("BCM: TX_SEND: sending frame",
 988                   (struct can_frame *)skb->data);
 989
 990         dev = dev_get_by_index(ifindex);
 991         if (!dev) {
 992                 kfree_skb(skb);
 993                 return -ENODEV;
 994         }
 995
 996         skb->dev = dev;
 997         skb->sk  = sk;
 998         can_send(skb, 1); /* send with loopback */
 999         dev_put(dev);
1000
1001         return CFSIZ + MHSIZ;
1002 }
1003
1004 static int bcm_read_op(struct list_head *ops, struct bcm_msg_head *msg_head,
1005                        int ifindex)
1006 {
1007         struct bcm_op *op;
1008         int ret;
1009
1010         op = bcm_find_op(ops, msg_head->can_id, ifindex);
1011         if (op) {
1012
1013                 DBG("TRX_READ: sending status for can_id %03X\n",
1014                     msg_head->can_id);
1015                 /* put current values into msg_head */
1016                 msg_head->flags   = op->flags;
1017                 msg_head->count   = op->count;
1018                 msg_head->ival1   = op->ival1;
1019                 msg_head->ival2   = op->ival2;
1020                 msg_head->nframes = op->nframes;
1021
1022                 bcm_send_to_user(op, msg_head, op->frames, NULL);
1023
1024                 ret = MHSIZ;
1025
1026         } else {
1027
1028                 DBG("TRX_READ: did not find op for can_id %03X\n",
1029                     msg_head->can_id);
1030                 ret = -EINVAL;
1031         }
1032
1033         return ret;
1034 }
1035
1036 /* procfs functions */
1037
1038 static char *bcm_proc_getifname(int ifindex)
1039 {
1040         struct net_device *dev;
1041
1042         if (!ifindex)
1043                 return "any";
1044
1045         dev = __dev_get_by_index(ifindex); /* no usage counting */
1046         if (dev)
1047                 return dev->name;
1048
1049         return "???";
1050 }
1051
1052 static int bcm_read_proc(char *page, char **start, off_t off,
1053                          int count, int *eof, void *data)
1054 {
1055         int len = 0;
1056         struct sock *sk = (struct sock *)data;
1057         struct bcm_opt *bo = bcm_sk(sk);
1058         struct bcm_op *op;
1059
1060         len += snprintf(page + len, PAGE_SIZE - len, ">>> socket %p",
1061                         sk->sk_socket);
1062         len += snprintf(page + len, PAGE_SIZE - len, " / sk %p", sk);
1063         len += snprintf(page + len, PAGE_SIZE - len, " / bo %p", bo);
1064         len += snprintf(page + len, PAGE_SIZE - len, " / dropped %lu",
1065                         bo->dropped_usr_msgs);
1066         len += snprintf(page + len, PAGE_SIZE - len, " / bound %s",
1067                         bcm_proc_getifname(bo->ifindex));
1068         len += snprintf(page + len, PAGE_SIZE - len, " <<<\n");
1069
1070         list_for_each_entry(op, &bo->rx_ops, list) {
1071
1072                 unsigned long reduction;
1073
1074                 /* print only active entries & prevent division by zero */
1075                 if (!op->frames_abs)
1076                         continue;
1077
1078                 len += snprintf(page + len, PAGE_SIZE - len,
1079                                 "rx_op: %03X %-5s ",
1080                                 op->can_id, bcm_proc_getifname(op->ifindex));
1081                 len += snprintf(page + len, PAGE_SIZE - len, "[%d]%c ",
1082                                 op->nframes,
1083                                 (op->flags & RX_CHECK_DLC)?'d':' ');
1084                 if (op->j_ival1)
1085                         len += snprintf(page + len, PAGE_SIZE - len,
1086                                         "timeo=%ld ", op->j_ival1);
1087
1088                 if (op->j_ival2)
1089                         len += snprintf(page + len, PAGE_SIZE - len,
1090                                         "thr=%ld ", op->j_ival2);
1091
1092                 len += snprintf(page + len, PAGE_SIZE - len,
1093                                 "# recv %ld (%ld) => reduction: ",
1094                                 op->frames_filtered, op->frames_abs);
1095
1096                 reduction = 100 - (op->frames_filtered * 100) / op->frames_abs;
1097
1098                 len += snprintf(page + len, PAGE_SIZE - len, "%s%ld%%\n",
1099                                 (reduction == 100)?"near ":"", reduction);
1100
1101                 if (len > PAGE_SIZE - 200) {
1102                         /* mark output cut off */
1103                         len += snprintf(page + len, PAGE_SIZE - len, "(..)\n");
1104                         break;
1105                 }
1106         }
1107
1108         list_for_each_entry(op, &bo->tx_ops, list) {
1109
1110                 len += snprintf(page + len, PAGE_SIZE - len,
1111                                 "tx_op: %03X %s [%d] ",
1112                                 op->can_id, bcm_proc_getifname(op->ifindex),
1113                                 op->nframes);
1114                 if (op->j_ival1)
1115                         len += snprintf(page + len, PAGE_SIZE - len, "t1=%ld ",
1116                                         op->j_ival1);
1117
1118                 if (op->j_ival2)
1119                         len += snprintf(page + len, PAGE_SIZE - len, "t2=%ld ",
1120                                         op->j_ival2);
1121
1122                 len += snprintf(page + len, PAGE_SIZE - len, "# sent %ld\n",
1123                                 op->frames_abs);
1124
1125                 if (len > PAGE_SIZE - 100) {
1126                         /* mark output cut off */
1127                         len += snprintf(page + len, PAGE_SIZE - len, "(..)\n");
1128                         break;
1129                 }
1130         }
1131
1132         len += snprintf(page + len, PAGE_SIZE - len, "\n");
1133
1134         *eof = 1;
1135         return len;
1136 }
1137
1138 /* bcm_op handling tx path */
1139
1140 static void bcm_can_tx(struct bcm_op *op)
1141 {
1142         struct sk_buff *skb;
1143         struct net_device *dev;
1144         struct can_frame *cf = &op->frames[op->currframe];
1145
1146         DBG_FRAME("BCM: bcm_can_tx: sending frame", cf);
1147
1148         /* no target device? => exit */
1149         if (!op->ifindex)
1150                 return;
1151
1152         dev = dev_get_by_index(op->ifindex);
1153
1154         if (!dev) {
1155                 /* RFC: should this bcm_op remove itself here? */
1156                 return;
1157         }
1158
1159         skb = alloc_skb(CFSIZ,
1160                         in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
1161
1162         if (!skb)
1163                 goto out;
1164
1165         memcpy(skb_put(skb, CFSIZ), cf, CFSIZ);
1166
1167         /* send with loopback */
1168         skb->dev = dev;
1169         skb->sk = op->sk;
1170         can_send(skb, 1);
1171
1172         /* update statistics */
1173         op->currframe++;
1174         op->frames_abs++;
1175
1176         /* reached last frame? */
1177         if (op->currframe >= op->nframes)
1178                 op->currframe = 0;
1179  out:
1180         dev_put(dev);
1181 }
1182
1183 static void bcm_tx_timeout_handler(unsigned long data)
1184 {
1185         struct bcm_op *op = (struct bcm_op*)data;
1186
1187         DBG("Called with bcm_op %p\n", op);
1188
1189         if (op->j_ival1 && (op->count > 0)) {
1190
1191                 op->count--;
1192
1193                 if (!op->count && (op->flags & TX_COUNTEVT)) {
1194                         /* create notification to user */
1195
1196                         struct bcm_msg_head msg_head;
1197
1198                         DBG("sending TX_EXPIRED for can_id %03X\n",
1199                             op->can_id);
1200
1201                         msg_head.opcode  = TX_EXPIRED;
1202                         msg_head.flags   = op->flags;
1203                         msg_head.count   = op->count;
1204                         msg_head.ival1   = op->ival1;
1205                         msg_head.ival2   = op->ival2;
1206                         msg_head.can_id  = op->can_id;
1207                         msg_head.nframes = 0;
1208
1209                         bcm_send_to_user(op, &msg_head, NULL, NULL);
1210                 }
1211         }
1212
1213         DBG("count=%d j_ival1=%ld j_ival2=%ld\n",
1214             op->count, op->j_ival1, op->j_ival2);
1215
1216         if (op->j_ival1 && (op->count > 0)) {
1217
1218                 op->timer.expires = jiffies + op->j_ival1;
1219                 add_timer(&op->timer);
1220
1221                 DBG("adding timer ival1. func=%p data=%p exp=0x%08X\n",
1222                     op->timer.function,
1223                     (char*) op->timer.data,
1224                     (unsigned int) op->timer.expires);
1225
1226                 /* send (next) frame */
1227                 bcm_can_tx(op);
1228         } else {
1229                 if (op->j_ival2) {
1230                         op->timer.expires = jiffies + op->j_ival2;
1231                         add_timer(&op->timer);
1232
1233                         DBG("adding timer ival2. func=%p data=%p exp=0x%08X\n",
1234                             op->timer.function,
1235                             (char*) op->timer.data,
1236                             (unsigned int) op->timer.expires);
1237
1238                         /* send (next) frame */
1239                         bcm_can_tx(op);
1240                 } else
1241                         DBG("no timer restart\n");
1242         }
1243
1244         return;
1245
1246 }
1247
1248 /* bcm_op handling rx path */
1249
1250 static void bcm_rx_handler(struct sk_buff *skb, void *data)
1251 {
1252         struct bcm_op *op = (struct bcm_op*)data;
1253         struct can_frame rxframe;
1254         int i;
1255
1256         /* disable timeout */
1257         del_timer(&op->timer);
1258
1259         DBG("Called with bcm_op %p\n", op);
1260
1261         if (skb->len == sizeof(rxframe)) {
1262                 memcpy(&rxframe, skb->data, sizeof(rxframe));
1263                 /* save rx timestamp */
1264                 skb_get_timestamp(skb, &op->rx_stamp);
1265                 /* save originator for recvfrom() */
1266                 op->rx_ifindex = skb->dev->ifindex;
1267                 /* update statistics */
1268                 op->frames_abs++;
1269                 kfree_skb(skb);
1270                 DBG("got can_frame with can_id %03X\n", rxframe.can_id);
1271         } else {
1272                 DBG("Wrong skb->len = %d\n", skb->len);
1273                 kfree_skb(skb);
1274                 return;
1275         }
1276
1277         DBG_FRAME("BCM: bcm_rx_handler: CAN frame", &rxframe);
1278
1279         if (op->can_id != rxframe.can_id) {
1280                 DBG("ERROR! Got wrong can_id %03X! Expected %03X.\n",
1281                     rxframe.can_id, op->can_id);
1282                 return;
1283         }
1284
1285         if (op->flags & RX_RTR_FRAME) {
1286                 /* send reply for RTR-request */
1287                 DBG("RTR-request\n");
1288
1289                 /* send op->frames[0] to CAN device */
1290                 bcm_can_tx(op);
1291                 return;
1292         }
1293
1294         if (op->flags & RX_FILTER_ID) {
1295                 /* the easiest case */
1296                 DBG("Easy does it with RX_FILTER_ID\n");
1297
1298                 bcm_rx_update_and_send(op, &op->last_frames[0], &rxframe);
1299                 bcm_rx_starttimer(op);
1300                 return;
1301         }
1302
1303         if (op->nframes == 1) {
1304                 /* simple compare with index 0 */
1305                 DBG("Simple compare\n");
1306
1307                 bcm_rx_cmp_to_index(op, 0, &rxframe);
1308                 bcm_rx_starttimer(op);
1309                 return;
1310         }
1311
1312         if (op->nframes > 1) {
1313                 /* multiplex compare */
1314                 DBG("Multiplex compare\n");
1315
1316                 /*
1317                  * find the first multiplex mask that fits.
1318                  * Remark: The MUX-mask is stored in index 0
1319                  */
1320
1321                 for (i=1; i < op->nframes; i++) {
1322
1323                         if ((GET_U64(&op->frames[0]) & GET_U64(&rxframe)) ==
1324                             (GET_U64(&op->frames[0]) &
1325                              GET_U64(&op->frames[i]))) {
1326                                 DBG("found MUX index %d\n", i);
1327                                 bcm_rx_cmp_to_index(op, i, &rxframe);
1328                                 break;
1329                         }
1330                 }
1331                 bcm_rx_starttimer(op);
1332         }
1333 }
1334
1335 static void bcm_rx_cmp_to_index(struct bcm_op *op, int index,
1336                                 struct can_frame *rxdata)
1337 {
1338         /*
1339          * no one uses the MSBs of can_dlc for comparation,
1340          * so we use it here to detect the first time of reception
1341          */
1342
1343         if (!(op->last_frames[index].can_dlc & RX_RECV)) {
1344                 /* received data for the first time => send update to user */
1345                 DBG("first time :)\n");
1346                 bcm_rx_update_and_send(op, &op->last_frames[index], rxdata);
1347                 return;
1348         }
1349
1350         /* do a real check in can_data */
1351
1352         DBG("op->frames[index].data = 0x%016llx\n",
1353             GET_U64(&op->frames[index]));
1354         DBG("op->last_frames[index].data = 0x%016llx\n",
1355             GET_U64(&op->last_frames[index]));
1356         DBG("rxdata->data = 0x%016llx\n", GET_U64(rxdata));
1357
1358         if ((GET_U64(&op->frames[index]) & GET_U64(rxdata)) !=
1359             (GET_U64(&op->frames[index]) & GET_U64(&op->last_frames[index]))) {
1360                 DBG("relevant data change :)\n");
1361                 bcm_rx_update_and_send(op, &op->last_frames[index], rxdata);
1362                 return;
1363         }
1364
1365
1366         if (op->flags & RX_CHECK_DLC) {
1367
1368                 /* do a real check in dlc */
1369
1370                 if (rxdata->can_dlc != (op->last_frames[index].can_dlc &
1371                                         BCM_CAN_DLC_MASK)) {
1372                         DBG("dlc change :)\n");
1373                         bcm_rx_update_and_send(op, &op->last_frames[index],
1374                                                rxdata);
1375                         return;
1376                 }
1377         }
1378         DBG("no relevant change :(\n");
1379 }
1380
1381 static void bcm_rx_update_and_send(struct bcm_op *op,
1382                                    struct can_frame *lastdata,
1383                                    struct can_frame *rxdata)
1384 {
1385         unsigned long nexttx = op->j_lastmsg + op->j_ival2;
1386
1387         memcpy(lastdata, rxdata, CFSIZ);
1388
1389         /* mark as used */
1390         lastdata->can_dlc |= RX_RECV;
1391
1392         /* throttle bcm_rx_changed ? */
1393         if ((op->thrtimer.expires) ||
1394             ((op->j_ival2) && (nexttx > jiffies))) {
1395                 /* we are already waiting OR we have to start waiting */
1396
1397                 /* mark as 'throttled' */
1398                 lastdata->can_dlc |= RX_THR;
1399
1400                 if (!(op->thrtimer.expires)) {
1401                         /* start the timer only the first time */
1402                         op->thrtimer.expires = nexttx;
1403                         add_timer(&op->thrtimer);
1404
1405                         DBG("adding thrtimer. func=%p data=%p exp=0x%08X\n",
1406                             op->thrtimer.function,
1407                             (char*) op->thrtimer.data,
1408                             (unsigned int) op->thrtimer.expires);
1409                 }
1410         } else {
1411                 /* send RX_CHANGED to the user */
1412                 bcm_rx_changed(op, rxdata);
1413         }
1414 }
1415
1416 static void bcm_rx_starttimer(struct bcm_op *op)
1417 {
1418         if (op->flags & RX_NO_AUTOTIMER)
1419                 return;
1420
1421         if (op->j_ival1) {
1422
1423                 op->timer.expires = jiffies + op->j_ival1;
1424
1425                 DBG("adding rx timeout timer ival1. func=%p data=%p "
1426                     "exp=0x%08X\n",
1427                     op->timer.function,
1428                     (char*) op->timer.data,
1429                     (unsigned int) op->timer.expires);
1430
1431                 add_timer(&op->timer);
1432         }
1433 }
1434
1435
1436 static void bcm_rx_changed(struct bcm_op *op, struct can_frame *data)
1437 {
1438         struct bcm_msg_head head;
1439
1440         op->j_lastmsg = jiffies;
1441
1442         /* update statistics */
1443         op->frames_filtered++;
1444
1445         /* prevent statistics overflow */
1446         if (op->frames_filtered > ULONG_MAX/100)
1447                 op->frames_filtered = op->frames_abs = 0;
1448
1449         DBG("setting j_lastmsg to 0x%08X for rx_op %p\n",
1450             (unsigned int) op->j_lastmsg, op);
1451         DBG("sending notification\n");
1452
1453         head.opcode  = RX_CHANGED;
1454         head.flags   = op->flags;
1455         head.count   = op->count;
1456         head.ival1   = op->ival1;
1457         head.ival2   = op->ival2;
1458         head.can_id  = op->can_id;
1459         head.nframes = 1;
1460
1461         bcm_send_to_user(op, &head, data, &op->rx_stamp);
1462 }
1463
1464
1465 static void bcm_rx_timeout_handler(unsigned long data)
1466 {
1467         struct bcm_op *op = (struct bcm_op*)data;
1468         struct bcm_msg_head msg_head;
1469
1470         DBG("sending RX_TIMEOUT for can_id %03X. op is %p\n", op->can_id, op);
1471
1472         msg_head.opcode  = RX_TIMEOUT;
1473         msg_head.flags   = op->flags;
1474         msg_head.count   = op->count;
1475         msg_head.ival1   = op->ival1;
1476         msg_head.ival2   = op->ival2;
1477         msg_head.can_id  = op->can_id;
1478         msg_head.nframes = 0;
1479
1480         bcm_send_to_user(op, &msg_head, NULL, NULL);
1481
1482         /* no restart of the timer is done here! */
1483
1484         /* if user wants to be informed, when cyclic CAN-Messages come back */
1485         if ((op->flags & RX_ANNOUNCE_RESUME) && op->last_frames) {
1486                 /* clear received can_frames to indicate 'nothing received' */
1487                 memset(op->last_frames, 0, op->nframes * CFSIZ);
1488                 DBG("RX_ANNOUNCE_RESTART\n");
1489         }
1490
1491 }
1492
1493 static void bcm_rx_thr_handler(unsigned long data)
1494 {
1495         struct bcm_op *op = (struct bcm_op*)data;
1496         int i = 0;
1497
1498         /* mark disabled / consumed timer */
1499         op->thrtimer.expires = 0;
1500
1501         if (op->nframes > 1){
1502
1503                 DBG("sending MUX RX_CHANGED for can_id %03X. op is %p\n",
1504                     op->can_id, op);
1505                 /* for MUX filter we start at index 1 */
1506                 for (i=1; i<op->nframes; i++){
1507                         if ((op->last_frames) &&
1508                             (op->last_frames[i].can_dlc & RX_THR)){
1509                                 op->last_frames[i].can_dlc &= ~RX_THR;
1510                                 bcm_rx_changed(op, &op->last_frames[i]);
1511                         }
1512                 }
1513         } else {
1514
1515                 DBG("sending simple RX_CHANGED for can_id %03X. op is %p\n",
1516                     op->can_id, op);
1517                 /* for RX_FILTER_ID and simple filter */
1518                 if (op->last_frames && (op->last_frames[0].can_dlc & RX_THR)){
1519                         op->last_frames[0].can_dlc &= ~RX_THR;
1520                         bcm_rx_changed(op, &op->last_frames[0]);
1521                 }
1522         }
1523 }
1524
1525 static void bcm_send_to_user(struct bcm_op *op, struct bcm_msg_head *head,
1526                              struct can_frame *frames, struct timeval *tv)
1527 {
1528         struct sk_buff *skb;
1529         struct can_frame *firstframe;
1530         struct sock *sk = op->sk;
1531         int datalen = head->nframes * CFSIZ;
1532         struct sockaddr_can *addr;
1533         int err;
1534
1535         skb = alloc_skb(sizeof(*head) + datalen,
1536                         in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
1537         if (!skb)
1538                 return;
1539
1540         memcpy(skb_put(skb, sizeof(*head)), head, sizeof(*head));
1541
1542         /* can_frames starting here */
1543         firstframe = (struct can_frame *) skb->tail;
1544
1545         if (tv)
1546                 skb_set_timestamp(skb, tv); /* restore timestamp */
1547
1548         addr = (struct sockaddr_can *)skb->cb;
1549         memset(addr, 0, sizeof(*addr));
1550         addr->can_family  = AF_CAN;
1551         /* restore originator for recvfrom() */
1552         addr->can_ifindex = op->rx_ifindex;
1553
1554         if (head->nframes){
1555                 memcpy(skb_put(skb, datalen), frames, datalen);
1556
1557                 /*
1558                  * the BCM uses the can_dlc-element of the can_frame
1559                  * structure for internal purposes. This is only
1560                  * relevant for updates that are generated by the
1561                  * BCM, where nframes is 1
1562                  */
1563                 if (head->nframes == 1)
1564                         firstframe->can_dlc &= BCM_CAN_DLC_MASK;
1565         }
1566
1567         err = sock_queue_rcv_skb(sk, skb);
1568         if (err < 0) {
1569                 struct bcm_opt *bo = bcm_sk(sk);
1570
1571                 DBG("sock_queue_rcv_skb failed: %d\n", err);
1572                 kfree_skb(skb);
1573                 /* don't care about overflows in this statistic */
1574                 bo->dropped_usr_msgs++;
1575         }
1576 }
1577
1578 /* bcm_op handling: find & delete bcm_op elements */
1579
1580 static struct bcm_op *bcm_find_op(struct list_head *ops, canid_t can_id,
1581                                   int ifindex)
1582 {
1583         struct bcm_op *op;
1584
1585         list_for_each_entry(op, ops, list) {
1586                 if ((op->can_id == can_id) && (op->ifindex == ifindex))
1587                         return op;
1588         }
1589
1590         return NULL;
1591 }
1592
1593 static int bcm_delete_rx_op(struct list_head *ops, canid_t can_id, int ifindex)
1594 {
1595         struct bcm_op *op, *n;
1596
1597         list_for_each_entry_safe(op, n, ops, list) {
1598                 if ((op->can_id == can_id) && (op->ifindex == ifindex)) {
1599                         DBG("removing rx_op %p for can_id %03X\n",
1600                             op, op->can_id);
1601
1602                         /*
1603                          * Don't care if we're bound or not (due to netdev
1604                          * problems) can_rx_unregister() is always a save
1605                          * thing to do here.
1606                          */
1607                         if (op->ifindex) {
1608                                 struct net_device *dev =
1609                                         dev_get_by_index(op->ifindex);
1610
1611                                 if (dev) {
1612                                         can_rx_unregister(dev, op->can_id,
1613                                                           REGMASK(op->can_id),
1614                                                           bcm_rx_handler, op);
1615                                         dev_put(dev);
1616                                 }
1617                         } else
1618                                 can_rx_unregister(NULL, op->can_id,
1619                                                   REGMASK(op->can_id),
1620                                                   bcm_rx_handler, op);
1621
1622                         list_del(&op->list);
1623                         bcm_remove_op(op);
1624                         return 1; /* done */
1625                 }
1626         }
1627
1628         return 0; /* not found */
1629 }
1630
1631 static int bcm_delete_tx_op(struct list_head *ops, canid_t can_id, int ifindex)
1632 {
1633         struct bcm_op *op, *n;
1634
1635         list_for_each_entry_safe(op, n, ops, list) {
1636                 if ((op->can_id == can_id) && (op->ifindex == ifindex)) {
1637                         DBG("removing rx_op %p for can_id %03X\n",
1638                             op, op->can_id);
1639                         list_del(&op->list);
1640                         bcm_remove_op(op);
1641                         return 1; /* done */
1642                 }
1643         }
1644
1645         return 0; /* not found */
1646 }
1647
1648 static void bcm_remove_op(struct bcm_op *op)
1649 {
1650         del_timer(&op->timer);
1651         del_timer(&op->thrtimer);
1652         if (op->frames)
1653                 kfree(op->frames);
1654         if (op->last_frames)
1655                 kfree(op->last_frames);
1656         kfree(op);
1657
1658         return;
1659 }
1660
1661 module_init(bcm_module_init);
1662 module_exit(bcm_module_exit);