kernel/2.6/net/can/af_can.c

   1 /*
   2  * af_can.c
   3  *
   4  * Copyright (c) 2002-2005 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/version.h>
  48 #include <linux/slab.h>
  49 #include <linux/kmod.h>
  50 #include <linux/init.h>
  51 #include <linux/list.h>
  52 #include <linux/spinlock.h>
  53 #include <linux/rcupdate.h>
  54 #include <linux/socket.h>
  55 #include <linux/skbuff.h>
  56 #include <linux/net.h>
  57 #include <linux/netdevice.h>
  58 #include <net/sock.h>
  59 #include <asm/uaccess.h>
  60
  61 #include <linux/can.h>
  62 #include <linux/can/version.h>
  63
  64 #include "af_can.h"
  65
  66
  67 RCSID("$Id$");
  68
  69 #define NAME "Volkswagen AG - Low Level CAN Framework (LLCF)"
  70 #define IDENT "af_can"
  71 static __initdata const char banner[] = BANNER(NAME);
  72
  73 MODULE_DESCRIPTION(NAME);
  74 MODULE_LICENSE("Dual BSD/GPL");
  75 MODULE_AUTHOR("Urs Thuermann <urs.thuermann@volkswagen.de>, "
  76               "Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
  77
  78 int stats_timer = 1; /* default: on */
  79 module_param(stats_timer, int, S_IRUGO);
  80
  81 #ifdef CONFIG_CAN_DEBUG_CORE
  82 static int debug = 0;
  83 module_param(debug, int, S_IRUGO);
  84 #define DBG(args...)       (debug & 1 ? \
  85                                (printk(KERN_DEBUG "CAN %s: ", __func__), \
  86                                 printk(args)) : 0)
  87 #define DBG_FRAME(args...) (debug & 2 ? can_debug_cframe(args) : 0)
  88 #define DBG_SKB(skb)       (debug & 4 ? can_debug_skb(skb) : 0)
  89 #else
  90 #define DBG(args...)
  91 #define DBG_FRAME(args...)
  92 #define DBG_SKB(skb)
  93 #endif
  94
  95 static __init int  can_init(void);
  96 static __exit void can_exit(void);
  97
  98 static int can_create(struct socket *sock, int protocol);
  99 static int can_notifier(struct notifier_block *nb,
 100                         unsigned long msg, void *data);
 101 static int can_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg);
 102 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,14)
 103 static int can_rcv(struct sk_buff *skb, struct net_device *dev,
 104                    struct packet_type *pt, struct net_device *orig_dev);
 105 #else
 106 static int can_rcv(struct sk_buff *skb, struct net_device *dev,
 107                    struct packet_type *pt);
 108 #endif
 109 static int can_rcv_filter(struct dev_rcv_lists *d, struct sk_buff *skb);
 110 static struct dev_rcv_lists *find_dev_rcv_lists(struct net_device *dev);
 111 static struct hlist_head *find_rcv_list(canid_t *can_id, canid_t *mask,
 112                                         struct dev_rcv_lists *d);
 113 static void can_rcv_lists_delete(struct rcu_head *rp);
 114 static void can_rx_delete(struct rcu_head *rp);
 115 static void can_rx_delete_all(struct hlist_head *rl);
 116
 117
 118 struct notifier {
 119         struct list_head list;
 120         struct net_device *dev;
 121         void (*func)(unsigned long msg, void *data);
 122         void *data;
 123 };
 124
 125 static LIST_HEAD(notifier_list);
 126 static rwlock_t notifier_lock = RW_LOCK_UNLOCKED;
 127
 128 HLIST_HEAD(rx_dev_list);
 129 struct dev_rcv_lists rx_alldev_list;
 130 static spinlock_t rcv_lists_lock = SPIN_LOCK_UNLOCKED;
 131
 132 static kmem_cache_t *rcv_cache;
 133
 134 static struct packet_type can_packet = {
 135         .type = __constant_htons(ETH_P_CAN),
 136         .dev  = NULL,
 137         .func = can_rcv,
 138 };
 139
 140 static struct net_proto_family can_family_ops = {
 141         .family = PF_CAN,
 142         .create = can_create,
 143         .owner  = THIS_MODULE,
 144 };
 145
 146 /* notifier block for netdevice event */
 147 static struct notifier_block can_netdev_notifier = {
 148         .notifier_call = can_notifier,
 149 };
 150
 151 /* table of registered CAN protocols */
 152 static struct can_proto *proto_tab[CAN_NPROTO];
 153
 154 extern struct timer_list stattimer; /* timer for statistics update */
 155 extern struct s_stats  stats;       /* packet statistics */
 156 extern struct s_pstats pstats;      /* receive list statistics */
 157
 158 module_init(can_init);
 159 module_exit(can_exit);
 160
 161 /**************************************************/
 162 /* af_can module init/exit functions              */
 163 /**************************************************/
 164
 165 static __init int can_init(void)
 166 {
 167         printk(banner);
 168
 169         rcv_cache = kmem_cache_create("can_receiver", sizeof(struct receiver),
 170                                       0, 0, NULL, NULL);
 171         if (!rcv_cache)
 172                 return -ENOMEM;
 173
 174         if (stats_timer) {
 175                 /* statistics init */
 176                 init_timer(&stattimer);
 177         }
 178
 179         /* procfs init */
 180         can_init_proc();
 181
 182         /* protocol register */
 183         sock_register(&can_family_ops);
 184         register_netdevice_notifier(&can_netdev_notifier);
 185         dev_add_pack(&can_packet);
 186
 187         return 0;
 188 }
 189
 190 static __exit void can_exit(void)
 191 {
 192         if (stats_timer) {
 193                 /* stop statistics timer */
 194                 del_timer(&stattimer);
 195         }
 196
 197         /* procfs remove */
 198         can_remove_proc();
 199
 200         /* protocol unregister */
 201         dev_remove_pack(&can_packet);
 202         unregister_netdevice_notifier(&can_netdev_notifier);
 203         sock_unregister(PF_CAN);
 204
 205         kmem_cache_destroy(rcv_cache);
 206 }
 207
 208 /**************************************************/
 209 /* af_can protocol functions                      */
 210 /**************************************************/
 211
 212 void can_proto_register(struct can_proto *cp)
 213 {
 214         int proto = cp->protocol;
 215         if (proto < 0 || proto >= CAN_NPROTO) {
 216                 printk(KERN_ERR "CAN: protocol number %d out of range\n", proto);
 217                 return;
 218         }
 219         if (proto_tab[proto]) {
 220                 printk(KERN_ERR "CAN: protocol %d already registered\n", proto);
 221                 return;
 222         }
 223
 224 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,13)
 225         if (proto_register(cp->prot, 0) != 0) {
 226                 return;
 227         }
 228 #endif
 229         proto_tab[proto] = cp;
 230
 231         /* use our generic ioctl function if the module doesn't bring its own */
 232         if (!cp->ops->ioctl)
 233                 cp->ops->ioctl = can_ioctl;
 234 }
 235
 236 void can_proto_unregister(struct can_proto *cp)
 237 {
 238         int proto = cp->protocol;
 239         if (!proto_tab[proto]) {
 240                 printk(KERN_ERR "CAN: protocol %d is not registered\n", proto);
 241                 return;
 242         }
 243 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,13)
 244         proto_unregister(cp->prot);
 245 #endif
 246         proto_tab[proto] = NULL;
 247 }
 248
 249 void can_dev_register(struct net_device *dev,
 250                       void (*func)(unsigned long msg, void *), void *data)
 251 {
 252         struct notifier *n;
 253
 254         DBG("called for %s\n", dev->name);
 255
 256         if (!(n = kmalloc(sizeof(*n), GFP_KERNEL)))
 257                 return;
 258
 259         n->dev  = dev;
 260         n->func = func;
 261         n->data = data;
 262
 263         write_lock(&notifier_lock);
 264         list_add(&n->list, &notifier_list);
 265         write_unlock(&notifier_lock);
 266 }
 267
 268 void can_dev_unregister(struct net_device *dev,
 269                         void (*func)(unsigned long msg, void *), void *data)
 270 {
 271         struct notifier *n, *next;
 272
 273         DBG("called for %s\n", dev->name);
 274
 275         write_lock(&notifier_lock);
 276         list_for_each_entry_safe(n, next, &notifier_list, list) {
 277                 if (n->dev == dev && n->func == func && n->data == data) {
 278                         list_del(&n->list);
 279                         kfree(n);
 280                         break;
 281                 }
 282         }
 283         write_unlock(&notifier_lock);
 284 }
 285
 286 /**************************************************/
 287 /* af_can socket functions                        */
 288 /**************************************************/
 289
 290 static void can_sock_destruct(struct sock *sk)
 291 {
 292         DBG("called for sock %p\n", sk);
 293
 294         skb_queue_purge(&sk->sk_receive_queue);
 295         if (sk->sk_protinfo)
 296                 kfree(sk->sk_protinfo);
 297 }
 298
 299 static int can_create(struct socket *sock, int protocol)
 300 {
 301         struct sock *sk;
 302         struct can_proto *cp;
 303         int ret;
 304
 305         DBG("socket %p, type %d, proto %d\n", sock, sock->type, protocol);
 306
 307         sock->state = SS_UNCONNECTED;
 308
 309         if (protocol < 0 || protocol >= CAN_NPROTO)
 310                 return -EINVAL;
 311
 312         DBG("looking up proto %d in proto_tab[]\n", protocol);
 313
 314         /* try to load protocol module, when CONFIG_KMOD is defined */
 315         if (!proto_tab[protocol]) {
 316                 char module_name[30];
 317                 sprintf(module_name, "can-proto-%d", protocol);
 318                 if (request_module(module_name) == -ENOSYS)
 319                         printk(KERN_INFO "CAN: request_module(%s) not implemented.\n",
 320                                module_name);
 321         }
 322
 323         /* check for success and correct type */
 324         if (!(cp = proto_tab[protocol]) || cp->type != sock->type)
 325                 return -EPROTONOSUPPORT;
 326
 327         if (cp->capability >= 0 && !capable(cp->capability))
 328                 return -EPERM;
 329
 330         sock->ops = cp->ops;
 331
 332 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,13)
 333         sk = sk_alloc(PF_CAN, GFP_KERNEL, cp->prot, 1);
 334         if (!sk)
 335                 goto oom;
 336 #else
 337         sk = sk_alloc(PF_CAN, GFP_KERNEL, 1, 0);
 338         if (!sk)
 339                 goto oom;
 340         if (cp->obj_size &&
 341             !(sk->sk_protinfo = kmalloc(cp->obj_size, GFP_KERNEL))) {
 342                 sk_free(sk);
 343                 goto oom;
 344         }
 345         sk_set_owner(sk, proto_tab[protocol]->owner);
 346 #endif
 347         sock_init_data(sock, sk);
 348         sk->sk_destruct = can_sock_destruct;
 349
 350         DBG("created sock: %p\n", sk);
 351
 352         ret = 0;
 353 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,13)
 354         if (sk->sk_prot->init)
 355                 ret = sk->sk_prot->init(sk);
 356 #else
 357         if (cp->init)
 358                 ret = cp->init(sk);
 359 #endif
 360         if (ret) {
 361                 /* we must release sk */
 362                 sock_orphan(sk);
 363                 sock_put(sk);
 364                 return ret;
 365         }
 366
 367         return 0;
 368
 369  oom:
 370         return -ENOMEM;
 371 }
 372
 373 static int can_notifier(struct notifier_block *nb,
 374                         unsigned long msg, void *data)
 375 {
 376         struct net_device *dev = (struct net_device *)data;
 377         struct notifier *n;
 378
 379         DBG("called for %s, msg = %lu\n", dev->name, msg);
 380
 381 #if 0
 382         if (dev->type != ARPHRD_CAN)
 383                 return NOTIFY_DONE;
 384 #endif
 385
 386         switch (msg) {
 387                 struct dev_rcv_lists *d;
 388                 int i;
 389
 390         case NETDEV_REGISTER:
 391
 392                 /* create new dev_rcv_lists for this device */
 393
 394                 DBG("creating new dev_rcv_lists for %s\n", dev->name);
 395                 if (!(d = kmalloc(sizeof(*d), GFP_KERNEL))) {
 396                         printk(KERN_ERR "CAN: allocation of receive list failed\n");
 397                         return NOTIFY_DONE;
 398                 }
 399                 /* N.B. zeroing the struct is the correct initialization
 400                         for the embedded hlist_head structs.
 401                         Another list type, e.g. list_head, would require
 402                         explicit initialization. */
 403                 memset(d, 0, sizeof(*d));
 404                 d->dev = dev;
 405
 406                 spin_lock(&rcv_lists_lock);
 407                 hlist_add_head_rcu(&d->list, &rx_dev_list);
 408                 spin_unlock(&rcv_lists_lock);
 409
 410                 break;
 411
 412         case NETDEV_UNREGISTER:
 413                 spin_lock(&rcv_lists_lock);
 414
 415                 if (!(d = find_dev_rcv_lists(dev))) {
 416                         printk(KERN_ERR "CAN: notifier: receive list not "
 417                                "found for dev %s\n", dev->name);
 418                         goto unreg_out;
 419                 }
 420
 421                 hlist_del_rcu(&d->list);
 422
 423                 /* remove all receivers hooked at this netdevice */
 424                 can_rx_delete_all(&d->rx_err);
 425                 can_rx_delete_all(&d->rx_all);
 426                 can_rx_delete_all(&d->rx_fil);
 427                 can_rx_delete_all(&d->rx_inv);
 428                 can_rx_delete_all(&d->rx_eff);
 429                 for (i = 0; i < 2048; i++)
 430                         can_rx_delete_all(&d->rx_sff[i]);
 431
 432         unreg_out:
 433                 spin_unlock(&rcv_lists_lock);
 434
 435                 if (d)
 436                         call_rcu(&d->rcu, can_rcv_lists_delete);
 437
 438                 break;
 439         }
 440
 441         read_lock(&notifier_lock);
 442         list_for_each_entry(n, &notifier_list, list) {
 443                 if (n->dev == dev)
 444                         n->func(msg, n->data);
 445         }
 446         read_unlock(&notifier_lock);
 447
 448         return NOTIFY_DONE;
 449 }
 450
 451 static int can_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
 452 {
 453         struct sock *sk = sock->sk;
 454
 455         switch (cmd) {
 456         case SIOCGSTAMP:
 457                 return sock_get_timestamp(sk, (struct timeval __user *)arg);
 458         default:
 459 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,16)
 460                 return -ENOIOCTLCMD;
 461 #else
 462                 return dev_ioctl(cmd, (void __user *)arg);
 463 #endif
 464         }
 465         return 0;
 466 }
 467
 468 /**************************************************/
 469 /* af_can tx path                                 */
 470 /**************************************************/
 471
 472 int can_send(struct sk_buff *skb, int loop)
 473 {
 474         int err;
 475
 476         if (loop) { /* local loopback (default) */
 477                 *(struct sock **)skb->cb = skb->sk; /* tx sock reference */
 478
 479                 /* interface not capabable to do the loopback itself? */
 480                 if (!(skb->dev->flags & IFF_LOOPBACK)) {
 481                         struct sk_buff *newskb = skb_clone(skb, GFP_ATOMIC);
 482                         newskb->protocol  = htons(ETH_P_CAN);
 483                         newskb->ip_summed = CHECKSUM_UNNECESSARY;
 484                         netif_rx(newskb); /* perform local loopback here */
 485                 }
 486         } else
 487                 *(struct sock **)skb->cb = NULL; /* no loopback required */
 488
 489         if (!(skb->dev->flags & IFF_UP))
 490                 err = -ENETDOWN;
 491         else if ((err = dev_queue_xmit(skb)) > 0)  /* send to netdevice */
 492                 err = net_xmit_errno(err);
 493
 494         /* update statistics */
 495         stats.tx_frames++;
 496         stats.tx_frames_delta++;
 497
 498         return err;
 499 }
 500
 501 /**************************************************/
 502 /* af_can rx path                                 */
 503 /**************************************************/
 504
 505 int can_rx_register(struct net_device *dev, canid_t can_id, canid_t mask,
 506                     void (*func)(struct sk_buff *, void *), void *data,
 507                     char *ident)
 508 {
 509         struct receiver *r;
 510         struct hlist_head *rl;
 511         struct dev_rcv_lists *d;
 512         int ret = 0;
 513
 514         /* insert new receiver  (dev,canid,mask) -> (func,data) */
 515
 516         DBG("dev %p, id %03X, mask %03X, callback %p, data %p, ident %s\n",
 517             dev, can_id, mask, func, data, ident);
 518
 519         if (!(r = kmem_cache_alloc(rcv_cache, GFP_KERNEL))) {
 520                 ret = -ENOMEM;
 521                 goto out;
 522         }
 523
 524         spin_lock(&rcv_lists_lock);
 525
 526         if (!(d = find_dev_rcv_lists(dev))) {
 527                 DBG("receive list not found for dev %s, id %03X, mask %03X\n",
 528                     dev->name, can_id, mask);
 529                 kmem_cache_free(rcv_cache, r);
 530                 ret = -ENODEV;
 531                 goto out_unlock;
 532         }
 533
 534         rl = find_rcv_list(&can_id, &mask, d);
 535
 536         r->can_id  = can_id;
 537         r->mask    = mask;
 538         r->matches = 0;
 539         r->func    = func;
 540         r->data    = data;
 541         r->ident   = ident;
 542
 543         hlist_add_head_rcu(&r->list, rl);
 544         d->entries++;
 545
 546         pstats.rcv_entries++;
 547         if (pstats.rcv_entries_max < pstats.rcv_entries)
 548                 pstats.rcv_entries_max = pstats.rcv_entries;
 549
 550  out_unlock:
 551         spin_unlock(&rcv_lists_lock);
 552  out:
 553         return ret;
 554 }
 555
 556 static void can_rcv_lists_delete(struct rcu_head *rp)
 557 {
 558         struct dev_rcv_lists *d = container_of(rp, struct dev_rcv_lists, rcu);
 559         kfree(d);
 560 }
 561
 562 static void can_rx_delete(struct rcu_head *rp)
 563 {
 564         struct receiver *r = container_of(rp, struct receiver, rcu);
 565         kmem_cache_free(rcv_cache, r);
 566 }
 567
 568 static void can_rx_delete_all(struct hlist_head *rl)
 569 {
 570         struct receiver *r;
 571         struct hlist_node *n;
 572
 573         hlist_for_each_entry_rcu(r, n, rl, list) {
 574                 hlist_del_rcu(&r->list);
 575                 call_rcu(&r->rcu, can_rx_delete);
 576         }
 577 }
 578
 579 int can_rx_unregister(struct net_device *dev, canid_t can_id, canid_t mask,
 580                       void (*func)(struct sk_buff *, void *), void *data)
 581 {
 582         struct receiver *r;
 583         struct hlist_head *rl;
 584         struct hlist_node *next;
 585         struct dev_rcv_lists *d;
 586         int ret = 0;
 587
 588         DBG("dev %p, id %03X, mask %03X, callback %p, data %p\n",
 589             dev, can_id, mask, func, data);
 590
 591         r = NULL;
 592
 593         spin_lock(&rcv_lists_lock);
 594
 595         if (!(d = find_dev_rcv_lists(dev))) {
 596                 DBG("receive list not found for dev %s, id %03X, mask %03X\n",
 597                     dev->name, can_id, mask);
 598                 ret = -ENODEV;
 599                 goto out;
 600         }
 601
 602         rl = find_rcv_list(&can_id, &mask, d);
 603
 604         /*  Search the receiver list for the item to delete.  This should
 605          *  exist, since no receiver may be unregistered that hasn't
 606          *  been registered before.
 607          */
 608
 609         hlist_for_each_entry(r, next, rl, list) {
 610                 if (r->can_id == can_id && r->mask == mask
 611                     && r->func == func && r->data == data)
 612                         break;
 613         }
 614
 615         /*  Check for bug in CAN protocol implementations:
 616          *  If no matching list item was found, the list cursor variable next
 617          *  will be NULL, while r will point to the last item of the list.
 618          */
 619
 620         if (!next) {
 621                 DBG("receive list entry not found for "
 622                     "dev %s, id %03X, mask %03X\n", dev->name, can_id, mask);
 623                 ret = -EINVAL;
 624                 r = NULL;
 625                 goto out;
 626         }
 627
 628         hlist_del_rcu(&r->list);
 629         d->entries--;
 630
 631         if (pstats.rcv_entries > 0)
 632                 pstats.rcv_entries--;
 633
 634  out:
 635         spin_unlock(&rcv_lists_lock);
 636
 637         /* schedule the receiver item for deletion */
 638         if (r)
 639                 call_rcu(&r->rcu, can_rx_delete);
 640
 641         return ret;
 642 }
 643
 644 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,14)
 645 static int can_rcv(struct sk_buff *skb, struct net_device *dev,
 646                    struct packet_type *pt, struct net_device *orig_dev)
 647 #else
 648 static int can_rcv(struct sk_buff *skb, struct net_device *dev,
 649                    struct packet_type *pt)
 650 #endif
 651 {
 652         struct dev_rcv_lists *d;
 653         int matches;
 654
 655         DBG("received skbuff on device %s, ptype %04x\n",
 656             dev->name, ntohs(pt->type));
 657         DBG_SKB(skb);
 658         DBG_FRAME("af_can: can_rcv: received CAN frame",
 659                   (struct can_frame *)skb->data);
 660
 661         /* update statistics */
 662         stats.rx_frames++;
 663         stats.rx_frames_delta++;
 664
 665         rcu_read_lock();
 666
 667         /* deliver the packet to sockets listening on all devices */
 668         matches = can_rcv_filter(&rx_alldev_list, skb);
 669
 670         /* find receive list for this device */
 671         if ((d = find_dev_rcv_lists(dev)))
 672                 matches += can_rcv_filter(d, skb);
 673
 674         rcu_read_unlock();
 675
 676         /* free the skbuff allocated by the netdevice driver */
 677         DBG("freeing skbuff %p\n", skb);
 678         kfree_skb(skb);
 679
 680         if (matches > 0) {
 681                 stats.matches++;
 682                 stats.matches_delta++;
 683         }
 684
 685         return 0;
 686 }
 687
 688
 689 static inline void deliver(struct sk_buff *skb, struct receiver *r)
 690 {
 691         struct sk_buff *clone = skb_clone(skb, GFP_ATOMIC);
 692         DBG("skbuff %p cloned to %p\n", skb, clone);
 693         if (clone) {
 694                 r->func(clone, r->data);
 695                 r->matches++;    /* update specific statistics */
 696         }
 697 }
 698
 699 static int can_rcv_filter(struct dev_rcv_lists *d, struct sk_buff *skb)
 700 {
 701         struct receiver *r;
 702         struct hlist_node *n;
 703         int matches = 0;
 704         struct can_frame *cf = (struct can_frame*)skb->data;
 705         canid_t can_id = cf->can_id;
 706
 707         if (d->entries == 0)
 708                 return 0;
 709
 710         if (can_id & CAN_ERR_FLAG) {
 711                 /* check for error frame entries only */
 712                 hlist_for_each_entry_rcu(r, n, &d->rx_err, list) {
 713                         if (can_id & r->mask) {
 714                                 DBG("match on rx_err skbuff %p\n", skb);
 715                                 deliver(skb, r);
 716                                 matches++;
 717                         }
 718                 }
 719                 goto out;
 720         }
 721
 722         /* check for unfiltered entries */
 723         hlist_for_each_entry_rcu(r, n, &d->rx_all, list) {
 724                 DBG("match on rx_all skbuff %p\n", skb);
 725                 deliver(skb, r);
 726                 matches++;
 727         }
 728
 729         /* check for can_id/mask entries */
 730         hlist_for_each_entry_rcu(r, n, &d->rx_fil, list) {
 731                 if ((can_id & r->mask) == r->can_id) {
 732                         DBG("match on rx_fil skbuff %p\n", skb);
 733                         deliver(skb, r);
 734                         matches++;
 735                 }
 736         }
 737
 738         /* check for inverted can_id/mask entries */
 739         hlist_for_each_entry_rcu(r, n, &d->rx_inv, list) {
 740                 if ((can_id & r->mask) != r->can_id) {
 741                         DBG("match on rx_inv skbuff %p\n", skb);
 742                         deliver(skb, r);
 743                         matches++;
 744                 }
 745         }
 746
 747         /* check CAN_ID specific entries */
 748         if (can_id & CAN_EFF_FLAG) {
 749                 hlist_for_each_entry_rcu(r, n, &d->rx_eff, list) {
 750                         if (r->can_id == can_id) {
 751                                 DBG("match on rx_eff skbuff %p\n", skb);
 752                                 deliver(skb, r);
 753                                 matches++;
 754                         }
 755                 }
 756         } else {
 757                 can_id &= CAN_SFF_MASK;
 758                 hlist_for_each_entry_rcu(r, n, &d->rx_sff[can_id], list) {
 759                         DBG("match on rx_sff skbuff %p\n", skb);
 760                         deliver(skb, r);
 761                         matches++;
 762                 }
 763         }
 764
 765  out:
 766         return matches;
 767 }
 768
 769 static struct dev_rcv_lists *find_dev_rcv_lists(struct net_device *dev)
 770 {
 771         struct dev_rcv_lists *d;
 772         struct hlist_node *n;
 773
 774         /* find receive list for this device */
 775
 776         if (!dev)
 777                 return &rx_alldev_list;
 778
 779         /*  The hlist_for_each_entry*() macros curse through the list
 780          *  using the pointer variable n and set d to the containing
 781          *  struct in each list iteration.  Therefore, after list
 782          *  iteration, d is unmodified when the list is empty, and it
 783          *  points to last list element, when the list is non-empty
 784          *  but no match in the loop body is found.  I.e. d is *not*
 785          *  NULL when no match is found.  We can, however, use the
 786          *  cursor variable n to decide if a match was found.
 787          */
 788
 789         hlist_for_each_entry(d, n, &rx_dev_list, list)
 790                 if (d->dev == dev)
 791                         break;
 792
 793         return n ? d : NULL;
 794 }
 795
 796 static struct hlist_head *find_rcv_list(canid_t *can_id, canid_t *mask,
 797                                         struct dev_rcv_lists *d)
 798 {
 799         canid_t inv = *can_id & CAN_INV_FILTER; /* save flag before masking values */
 800         canid_t eff = *can_id & *mask & CAN_EFF_FLAG; /* correct EFF check? */
 801         canid_t rtr = *can_id & *mask & CAN_RTR_FLAG; /* correct RTR check? */
 802         canid_t err = *mask & CAN_ERR_FLAG; /* mask for error frames only */
 803
 804         /* make some paranoic operations */
 805         if (*can_id & CAN_EFF_FLAG)
 806                 *mask &= (CAN_EFF_MASK | eff | rtr);
 807         else
 808                 *mask &= (CAN_SFF_MASK | rtr);
 809
 810         *can_id &= *mask;
 811
 812         if (err) /* error frames */
 813                 return &d->rx_err;
 814
 815         if (inv) /* inverse can_id/can_mask filter and RTR */
 816                 return &d->rx_inv;
 817
 818         if (*can_id & CAN_RTR_FLAG) /* positive filter RTR */
 819                 return &d->rx_fil;
 820
 821         if (!(*mask)) /* mask == 0 => no filter */
 822                 return &d->rx_all;
 823
 824         if (*can_id & CAN_EFF_FLAG) {
 825                 if (*mask == CAN_EFF_MASK) /* filter exact EFF can_id */
 826                         return &d->rx_eff;
 827         } else {
 828                 if (*mask == CAN_SFF_MASK) /* filter exact SFF can_id */
 829                         return &d->rx_sff[*can_id];
 830         }
 831
 832         return &d->rx_fil;  /* filter via can_id/can_mask */
 833 }
 834
 835 /**************************************************/
 836 /* af_can utility stuff                           */
 837 /**************************************************/
 838
 839 unsigned long timeval2jiffies(struct timeval *tv, int round_up)
 840 {
 841         unsigned long jif;
 842         unsigned long sec  = tv->tv_sec;
 843         unsigned long usec = tv->tv_usec;
 844
 845         if (sec > ULONG_MAX / HZ)          /* check for overflow */
 846                 return ULONG_MAX;
 847
 848         if (round_up)                      /* any usec below one HZ? */
 849                 usec += 1000000 / HZ - 1;  /* pump it up */
 850
 851         jif = usec / (1000000 / HZ);
 852
 853         if (sec * HZ > ULONG_MAX - jif)    /* check for overflow */
 854                 return ULONG_MAX;
 855         else
 856                 return jif + sec * HZ;
 857 }
 858
 859
 860 /**************************************************/
 861 /* af_can debugging stuff                         */
 862 /**************************************************/
 863
 864 #ifdef CONFIG_CAN_DEBUG_CORE
 865
 866 void can_debug_cframe(const char *msg, struct can_frame *cf, ...)
 867 {
 868         va_list ap;
 869         int len;
 870         int dlc, i;
 871         char buf[1024];
 872
 873         len = sprintf(buf, KERN_DEBUG);
 874         va_start(ap, cf);
 875         len += snprintf(buf + len, sizeof(buf) - 64, msg, ap);
 876         buf[len++] = ':';
 877         buf[len++] = ' ';
 878         va_end(ap);
 879
 880         if ((dlc = cf->can_dlc) > 8)
 881                 dlc = 8;
 882
 883         if (cf->can_id & CAN_EFF_FLAG)
 884                 len += sprintf(buf + len, "<%08X> [%X] ",
 885                                cf->can_id & CAN_EFF_MASK, dlc);
 886         else
 887                 len += sprintf(buf + len, "<%03X> [%X] ",
 888                                cf->can_id & CAN_SFF_MASK, dlc);
 889
 890         for (i = 0; i < dlc; i++)
 891                 len += sprintf(buf + len, "%02X ", cf->data[i]);
 892
 893         if (cf->can_id & CAN_RTR_FLAG)
 894                 len += sprintf(buf + len, "(RTR)");
 895
 896         buf[len++] = '\n';
 897         buf[len]   = '\0';
 898         printk(buf);
 899 }
 900
 901 void can_debug_skb(struct sk_buff *skb)
 902 {
 903         int len, nbytes, i;
 904         char buf[1024];
 905
 906         len = sprintf(buf,
 907                       KERN_DEBUG "  skbuff at %p, dev: %d, proto: %04x\n"
 908                       KERN_DEBUG "  users: %d, dataref: %d, nr_frags: %d, "
 909                       "h,d,t,e,l: %p %+d %+d %+d, %d",
 910                       skb, skb->dev ? skb->dev->ifindex : -1,
 911                       ntohs(skb->protocol),
 912                       atomic_read(&skb->users),
 913                       atomic_read(&(skb_shinfo(skb)->dataref)),
 914                       skb_shinfo(skb)->nr_frags,
 915                       skb->head, skb->data - skb->head,
 916                       skb->tail - skb->head, skb->end - skb->head, skb->len);
 917         nbytes = skb->end - skb->head;
 918         for (i = 0; i < nbytes; i++) {
 919                 if (i % 16 == 0)
 920                         len += sprintf(buf + len, "\n" KERN_DEBUG "  ");
 921                 if (len < sizeof(buf) - 16) {
 922                         len += sprintf(buf + len, " %02x", skb->head[i]);
 923                 } else {
 924                         len += sprintf(buf + len, "...");
 925                         break;
 926                 }
 927         }
 928         buf[len++] = '\n';
 929         buf[len]   = '\0';
 930         printk(buf);
 931 }
 932
 933 EXPORT_SYMBOL(can_debug_cframe);
 934 EXPORT_SYMBOL(can_debug_skb);
 935
 936 #endif
 937
 938 /**************************************************/
 939 /* Exported symbols                               */
 940 /**************************************************/
 941 EXPORT_SYMBOL(can_proto_register);
 942 EXPORT_SYMBOL(can_proto_unregister);
 943 EXPORT_SYMBOL(can_rx_register);
 944 EXPORT_SYMBOL(can_rx_unregister);
 945 EXPORT_SYMBOL(can_dev_register);
 946 EXPORT_SYMBOL(can_dev_unregister);
 947 EXPORT_SYMBOL(can_send);
 948 EXPORT_SYMBOL(timeval2jiffies);