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