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