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