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