kernel/2.6/net/can/af_can.c

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