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/core.h>
  66 #include "af_can.h"
  67
  68 #include <linux/can/version.h> /* for RCSID. Removed by mkpatch script */
  69 RCSID("$Id$");
  70
  71 #define IDENT "core"
  72 static __initdata const char banner[] =
  73         KERN_INFO "can: controller area network core # "
  74         CAN_VERSION_STRING "\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 MODULE_ALIAS_NETPROTO(PF_CAN);
  82
  83 int stats_timer = 1; /* default: on */
  84 module_param(stats_timer, int, S_IRUGO);
  85 MODULE_PARM_DESC(stats_timer, "enable timer for statistics (default:on)");
  86
  87 #ifdef CONFIG_CAN_DEBUG_CORE
  88 static int debug = 0;
  89 module_param(debug, int, S_IRUGO);
  90 MODULE_PARM_DESC(debug, "debug print mask: 1:debug, 2:frames, 4:skbs");
  91 #endif
  92
  93 struct notifier {
  94         struct list_head list;
  95         struct net_device *dev;
  96         void (*func)(unsigned long msg, void *data);
  97         void *data;
  98 };
  99
 100 static LIST_HEAD(notifier_list);
 101 static DEFINE_RWLOCK(notifier_lock);
 102
 103 HLIST_HEAD(rx_dev_list);
 104 static struct dev_rcv_lists rx_alldev_list;
 105 static DEFINE_SPINLOCK(rcv_lists_lock);
 106
 107 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
 108 static struct kmem_cache *rcv_cache __read_mostly;
 109 #else
 110 static kmem_cache_t *rcv_cache;
 111 #endif
 112
 113 /* table of registered CAN protocols */
 114 static struct can_proto *proto_tab[CAN_NPROTO];
 115
 116 struct timer_list stattimer; /* timer for statistics update */
 117 struct s_stats  stats;       /* packet statistics */
 118 struct s_pstats pstats;      /* receive list statistics */
 119
 120 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,14)
 121 static void *kzalloc(size_t size, unsigned int __nocast flags)
 122 {
 123         void *ret = kmalloc(size, flags);
 124         if (ret)
 125                 memset(ret, 0, size);
 126         return ret;
 127 }
 128 #endif
 129
 130 /*
 131  * af_can socket functions
 132  */
 133
 134 static int can_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
 135 {
 136         struct sock *sk = sock->sk;
 137
 138         switch (cmd) {
 139
 140         case SIOCGSTAMP:
 141                 return sock_get_timestamp(sk, (struct timeval __user *)arg);
 142
 143         default:
 144 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,16)
 145                 return -ENOIOCTLCMD;
 146 #else
 147                 return dev_ioctl(cmd, (void __user *)arg);
 148 #endif
 149         }
 150 }
 151
 152 static void can_sock_destruct(struct sock *sk)
 153 {
 154         DBG("called for sock %p\n", sk);
 155
 156         skb_queue_purge(&sk->sk_receive_queue);
 157         if (sk->sk_protinfo)
 158                 kfree(sk->sk_protinfo);
 159 }
 160
 161 static int can_create(struct socket *sock, int protocol)
 162 {
 163         struct sock *sk;
 164         struct can_proto *cp;
 165         char module_name[30];
 166         int ret = 0;
 167
 168         DBG("socket %p, type %d, proto %d\n", sock, sock->type, protocol);
 169
 170         sock->state = SS_UNCONNECTED;
 171
 172         if (protocol < 0 || protocol >= CAN_NPROTO)
 173                 return -EINVAL;
 174
 175         DBG("looking up proto %d in proto_tab[]\n", protocol);
 176
 177         /* try to load protocol module, when CONFIG_KMOD is defined */
 178         if (!proto_tab[protocol]) {
 179                 sprintf(module_name, "can-proto-%d", protocol);
 180                 if (request_module(module_name) == -ENOSYS)
 181                         printk(KERN_INFO "can: request_module(%s) not"
 182                                " implemented.\n", module_name);
 183         }
 184
 185         /* check for success and correct type */
 186         cp = proto_tab[protocol];
 187         if (!cp || cp->type != sock->type)
 188                 return -EPROTONOSUPPORT;
 189
 190         if (cp->capability >= 0 && !capable(cp->capability))
 191                 return -EPERM;
 192
 193         sock->ops = cp->ops;
 194
 195 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,12)
 196         sk = sk_alloc(PF_CAN, GFP_KERNEL, cp->prot, 1);
 197         if (!sk)
 198                 return -ENOMEM;
 199 #else
 200         sk = sk_alloc(PF_CAN, GFP_KERNEL, 1, 0);
 201         if (!sk)
 202                 return -ENOMEM;
 203
 204         if (cp->obj_size) {
 205                 sk->sk_protinfo = kmalloc(cp->obj_size, GFP_KERNEL);
 206                 if (!sk->sk_protinfo) {
 207                         sk_free(sk);
 208                         return -ENOMEM;
 209                 }
 210         }
 211         sk_set_owner(sk, proto_tab[protocol]->owner);
 212 #endif
 213
 214         sock_init_data(sock, sk);
 215         sk->sk_destruct = can_sock_destruct;
 216
 217         DBG("created sock: %p\n", sk);
 218
 219 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,12)
 220         if (sk->sk_prot->init)
 221                 ret = sk->sk_prot->init(sk);
 222 #else
 223         if (cp->init)
 224                 ret = cp->init(sk);
 225 #endif
 226
 227         if (ret) {
 228                 /* release sk on errors */
 229                 sock_orphan(sk);
 230                 sock_put(sk);
 231         }
 232
 233         return ret;
 234 }
 235
 236 /*
 237  * af_can tx path
 238  */
 239
 240 /**
 241  * can_send - transmit a CAN frame (optional with local loopback)
 242  * @skb: pointer to socket buffer with CAN frame in data section
 243  * @loop: loopback for listeners on local CAN sockets (recommended default!)
 244  *
 245  * Return:
 246  *  0 on success
 247  *  -ENETDOWN when the selected interface is down
 248  *  -ENOBUFS on full driver queue (see net_xmit_errno())
 249  */
 250 int can_send(struct sk_buff *skb, int loop)
 251 {
 252         int err;
 253
 254         if (loop) {
 255                 /* local loopback of sent CAN frames (default) */
 256
 257                 /* indication for the CAN driver: do loopback */
 258                 *(struct sock **)skb->cb = skb->sk;
 259
 260                 /*
 261                  * The reference to the originating sock may be also required
 262                  * by the receiving socket to indicate (and ignore) his own
 263                  * sent data. Example: can_raw sockopt CAN_RAW_RECV_OWN_MSGS
 264                  */
 265
 266                 /* interface not capabable to do the loopback itself? */
 267                 if (!(skb->dev->flags & IFF_LOOPBACK)) {
 268                         struct sk_buff *newskb = skb_clone(skb, GFP_ATOMIC);
 269
 270                         /* perform the local loopback here */
 271                         newskb->protocol  = htons(ETH_P_CAN);
 272                         newskb->ip_summed = CHECKSUM_UNNECESSARY;
 273                         netif_rx(newskb);
 274                 }
 275         } else {
 276                 /* indication for the CAN driver: no loopback required */
 277                 *(struct sock **)skb->cb = NULL;
 278         }
 279
 280         if (!(skb->dev->flags & IFF_UP))
 281                 return -ENETDOWN;
 282
 283         /* send to netdevice */
 284         err = dev_queue_xmit(skb);
 285         if (err > 0)
 286                 err = net_xmit_errno(err);
 287
 288         /* update statistics */
 289         stats.tx_frames++;
 290         stats.tx_frames_delta++;
 291
 292         return err;
 293 }
 294 EXPORT_SYMBOL_GPL(can_send);
 295
 296 /*
 297  * af_can rx path
 298  */
 299
 300 static struct dev_rcv_lists *find_dev_rcv_lists(struct net_device *dev)
 301 {
 302         struct dev_rcv_lists *d;
 303         struct hlist_node *n;
 304
 305         /*
 306          * find receive list for this device
 307          *
 308          * The hlist_for_each_entry*() macros curse through the list
 309          * using the pointer variable n and set d to the containing
 310          * struct in each list iteration.  Therefore, after list
 311          * iteration, d is unmodified when the list is empty, and it
 312          * points to last list element, when the list is non-empty
 313          * but no match in the loop body is found.  I.e. d is *not*
 314          * NULL when no match is found.  We can, however, use the
 315          * cursor variable n to decide if a match was found.
 316          */
 317
 318         hlist_for_each_entry(d, n, &rx_dev_list, list) {
 319                 if (d->dev == dev)
 320                         break;
 321         }
 322
 323         return n ? d : NULL;
 324 }
 325
 326 static struct hlist_head *find_rcv_list(canid_t *can_id, canid_t *mask,
 327                                         struct dev_rcv_lists *d)
 328 {
 329         canid_t inv = *can_id & CAN_INV_FILTER; /* save flag before masking */
 330
 331         if (*mask & CAN_ERR_FLAG) { /* filter error frames */
 332                 *mask &= CAN_ERR_MASK; /* clear CAN_ERR_FLAG in list entry */
 333                 return &d->rx_err;
 334         }
 335
 336         /* ensure valid values in can_mask */
 337         if (*mask & CAN_EFF_FLAG)
 338                 *mask &= (CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG);
 339         else
 340                 *mask &= (CAN_SFF_MASK | CAN_RTR_FLAG);
 341
 342         /* reduce condition testing at receive time */
 343         *can_id &= *mask;
 344
 345         /* inverse can_id/can_mask filter */
 346         if (inv)
 347                 return &d->rx_inv;
 348
 349         /* mask == 0 => no condition testing at receive time */
 350         if (!(*mask))
 351                 return &d->rx_all;
 352
 353         /* use extra filterset for the subscription of exactly *ONE* can_id */
 354         if (*can_id & CAN_EFF_FLAG) {
 355                 if (*mask == (CAN_EFF_MASK | CAN_EFF_FLAG)) {
 356                         /* RFC: a use-case for hash-tables in the future? */
 357                         return &d->rx_eff;
 358                 }
 359         } else {
 360                 if (*mask == CAN_SFF_MASK)
 361                         return &d->rx_sff[*can_id];
 362         }
 363
 364         /* default: filter via can_id/can_mask */
 365         return &d->rx_fil;
 366 }
 367
 368 /**
 369  * can_rx_register - subscribe CAN frames from a specific interface
 370  * @dev: pointer to netdevice (NULL => subcribe from 'all' CAN devices list)
 371  * @can_id: CAN identifier (see description)
 372  * @mask: CAN mask (see description)
 373  * @func: callback function on filter match
 374  * @data: returned parameter for callback function
 375  * @ident: string for calling module indentification
 376  *
 377  * Description:
 378  *  Invokes the callback function with the received sk_buff and the given
 379  *  parameter 'data' on a matching receive filter. A filter matches, when
 380  *
 381  *          <received_can_id> & mask == can_id & mask
 382  *
 383  *  The filter can be inverted (CAN_INV_FILTER bit set in can_id) or it can
 384  *  filter for error frames (CAN_ERR_FLAG bit set in mask).
 385  *
 386  * Return:
 387  *  0 on success
 388  *  -ENOMEM on missing cache mem to create subscription entry
 389  *  -ENODEV unknown device
 390  */
 391 int can_rx_register(struct net_device *dev, canid_t can_id, canid_t mask,
 392                     void (*func)(struct sk_buff *, void *), void *data,
 393                     char *ident)
 394 {
 395         struct receiver *r;
 396         struct hlist_head *rl;
 397         struct dev_rcv_lists *d;
 398         int ret = 0;
 399
 400         /* insert new receiver  (dev,canid,mask) -> (func,data) */
 401
 402         DBG("dev %p, id %03X, mask %03X, callback %p, data %p, ident %s\n",
 403             dev, can_id, mask, func, data, ident);
 404
 405         r = kmem_cache_alloc(rcv_cache, GFP_KERNEL);
 406         if (!r)
 407                 return -ENOMEM;
 408
 409         spin_lock_bh(&rcv_lists_lock);
 410
 411         d = find_dev_rcv_lists(dev);
 412         if (d) {
 413                 rl = find_rcv_list(&can_id, &mask, d);
 414
 415                 r->can_id  = can_id;
 416                 r->mask    = mask;
 417                 r->matches = 0;
 418                 r->func    = func;
 419                 r->data    = data;
 420                 r->ident   = ident;
 421
 422                 hlist_add_head_rcu(&r->list, rl);
 423                 d->entries++;
 424
 425                 pstats.rcv_entries++;
 426                 if (pstats.rcv_entries_max < pstats.rcv_entries)
 427                         pstats.rcv_entries_max = pstats.rcv_entries;
 428         } else {
 429                 DBG("receive list not found for dev %s, id %03X, mask %03X\n",
 430                     DNAME(dev), can_id, mask);
 431                 kmem_cache_free(rcv_cache, r);
 432                 ret = -ENODEV;
 433         }
 434
 435         spin_unlock_bh(&rcv_lists_lock);
 436
 437         return ret;
 438 }
 439 EXPORT_SYMBOL_GPL(can_rx_register);
 440
 441 static void can_rcv_lists_delete(struct rcu_head *rp)
 442 {
 443         struct dev_rcv_lists *d = container_of(rp, struct dev_rcv_lists, rcu);
 444         kfree(d);
 445 }
 446
 447 static void can_rx_delete(struct rcu_head *rp)
 448 {
 449         struct receiver *r = container_of(rp, struct receiver, rcu);
 450         kmem_cache_free(rcv_cache, r);
 451 }
 452
 453 static void can_rx_delete_all(struct hlist_head *rl)
 454 {
 455         struct receiver *r;
 456         struct hlist_node *n;
 457
 458         hlist_for_each_entry_rcu(r, n, rl, list) {
 459                 hlist_del_rcu(&r->list);
 460                 call_rcu(&r->rcu, can_rx_delete);
 461         }
 462 }
 463
 464 /**
 465  * can_rx_unregister - unsubscribe CAN frames from a specific interface
 466  * @dev: pointer to netdevice (NULL => unsubcribe from 'all' CAN devices list)
 467  * @can_id: CAN identifier
 468  * @mask: CAN mask
 469  * @func: callback function on filter match
 470  * @data: returned parameter for callback function
 471  *
 472  * Description:
 473  *  Removes subscription entry depending on given (subscription) values.
 474  *
 475  * Return:
 476  *  0 on success
 477  *  -EINVAL on missing subscription entry
 478  *  -ENODEV unknown device
 479  */
 480 int can_rx_unregister(struct net_device *dev, canid_t can_id, canid_t mask,
 481                       void (*func)(struct sk_buff *, void *), void *data)
 482 {
 483         struct receiver *r = NULL;
 484         struct hlist_head *rl;
 485         struct hlist_node *next;
 486         struct dev_rcv_lists *d;
 487         int ret = 0;
 488
 489         DBG("dev %p, id %03X, mask %03X, callback %p, data %p\n",
 490             dev, can_id, mask, func, data);
 491
 492         spin_lock_bh(&rcv_lists_lock);
 493
 494         d = find_dev_rcv_lists(dev);
 495         if (!d) {
 496                 DBG("receive list not found for dev %s, id %03X, mask %03X\n",
 497                     DNAME(dev), can_id, mask);
 498                 ret = -ENODEV;
 499                 goto out;
 500         }
 501
 502         rl = find_rcv_list(&can_id, &mask, d);
 503
 504         /*
 505          * Search the receiver list for the item to delete.  This should
 506          * exist, since no receiver may be unregistered that hasn't
 507          * been registered before.
 508          */
 509
 510         hlist_for_each_entry(r, next, rl, list) {
 511                 if (r->can_id == can_id && r->mask == mask
 512                     && r->func == func && r->data == data)
 513                         break;
 514         }
 515
 516         /*
 517          * Check for bug in CAN protocol implementations:
 518          * If no matching list item was found, the list cursor variable next
 519          * will be NULL, while r will point to the last item of the list.
 520          */
 521
 522         if (!next) {
 523                 DBG("receive list entry not found for "
 524                     "dev %s, id %03X, mask %03X\n", DNAME(dev), can_id, mask);
 525                 ret = -EINVAL;
 526                 r = NULL;
 527                 goto out;
 528         }
 529
 530         hlist_del_rcu(&r->list);
 531         d->entries--;
 532
 533         if (pstats.rcv_entries > 0)
 534                 pstats.rcv_entries--;
 535
 536  out:
 537         spin_unlock_bh(&rcv_lists_lock);
 538
 539         /* schedule the receiver item for deletion */
 540         if (r)
 541                 call_rcu(&r->rcu, can_rx_delete);
 542
 543         return ret;
 544 }
 545 EXPORT_SYMBOL_GPL(can_rx_unregister);
 546
 547 static inline void deliver(struct sk_buff *skb, struct receiver *r)
 548 {
 549         struct sk_buff *clone = skb_clone(skb, GFP_ATOMIC);
 550         DBG("skbuff %p cloned to %p\n", skb, clone);
 551         if (clone) {
 552                 r->func(clone, r->data);
 553                 r->matches++;
 554         }
 555 }
 556
 557 static int can_rcv_filter(struct dev_rcv_lists *d, struct sk_buff *skb)
 558 {
 559         struct receiver *r;
 560         struct hlist_node *n;
 561         int matches = 0;
 562         struct can_frame *cf = (struct can_frame*)skb->data;
 563         canid_t can_id = cf->can_id;
 564
 565         if (d->entries == 0)
 566                 return 0;
 567
 568         if (can_id & CAN_ERR_FLAG) {
 569                 /* check for error frame entries only */
 570                 hlist_for_each_entry_rcu(r, n, &d->rx_err, list) {
 571                         if (can_id & r->mask) {
 572                                 DBG("match on rx_err skbuff %p\n", skb);
 573                                 deliver(skb, r);
 574                                 matches++;
 575                         }
 576                 }
 577                 return matches;
 578         }
 579
 580         /* check for unfiltered entries */
 581         hlist_for_each_entry_rcu(r, n, &d->rx_all, list) {
 582                 DBG("match on rx_all skbuff %p\n", skb);
 583                 deliver(skb, r);
 584                 matches++;
 585         }
 586
 587         /* check for can_id/mask entries */
 588         hlist_for_each_entry_rcu(r, n, &d->rx_fil, list) {
 589                 if ((can_id & r->mask) == r->can_id) {
 590                         DBG("match on rx_fil skbuff %p\n", skb);
 591                         deliver(skb, r);
 592                         matches++;
 593                 }
 594         }
 595
 596         /* check for inverted can_id/mask entries */
 597         hlist_for_each_entry_rcu(r, n, &d->rx_inv, list) {
 598                 if ((can_id & r->mask) != r->can_id) {
 599                         DBG("match on rx_inv skbuff %p\n", skb);
 600                         deliver(skb, r);
 601                         matches++;
 602                 }
 603         }
 604
 605         /* check CAN_ID specific entries */
 606         if (can_id & CAN_EFF_FLAG) {
 607                 hlist_for_each_entry_rcu(r, n, &d->rx_eff, list) {
 608                         if (r->can_id == can_id) {
 609                                 DBG("match on rx_eff skbuff %p\n", skb);
 610                                 deliver(skb, r);
 611                                 matches++;
 612                         }
 613                 }
 614         } else {
 615                 can_id &= CAN_SFF_MASK;
 616                 hlist_for_each_entry_rcu(r, n, &d->rx_sff[can_id], list) {
 617                         DBG("match on rx_sff skbuff %p\n", skb);
 618                         deliver(skb, r);
 619                         matches++;
 620                 }
 621         }
 622
 623         return matches;
 624 }
 625
 626 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,14)
 627 static int can_rcv(struct sk_buff *skb, struct net_device *dev,
 628                    struct packet_type *pt, struct net_device *orig_dev)
 629 #else
 630 static int can_rcv(struct sk_buff *skb, struct net_device *dev,
 631                    struct packet_type *pt)
 632 #endif
 633 {
 634         struct dev_rcv_lists *d;
 635         int matches;
 636
 637         DBG("received skbuff on device %s, ptype %04x\n",
 638             dev->name, ntohs(pt->type));
 639         DBG_SKB(skb);
 640         DBG_FRAME("af_can: can_rcv: received CAN frame",
 641                   (struct can_frame *)skb->data);
 642
 643         /* update statistics */
 644         stats.rx_frames++;
 645         stats.rx_frames_delta++;
 646
 647         rcu_read_lock();
 648
 649         /* deliver the packet to sockets listening on all devices */
 650         matches = can_rcv_filter(&rx_alldev_list, skb);
 651
 652         /* find receive list for this device */
 653         d = find_dev_rcv_lists(dev);
 654         if (d)
 655                 matches += can_rcv_filter(d, skb);
 656
 657         rcu_read_unlock();
 658
 659         /* free the skbuff allocated by the netdevice driver */
 660         DBG("freeing skbuff %p\n", skb);
 661         kfree_skb(skb);
 662
 663         if (matches > 0) {
 664                 stats.matches++;
 665                 stats.matches_delta++;
 666         }
 667
 668         return 0;
 669 }
 670
 671 /*
 672  * af_can utility stuff
 673  */
 674
 675 /**
 676  * timeval2jiffies - calculate jiffies from timeval including optional round up
 677  * @tv: pointer to timeval
 678  * @round_up: return at least 1 jiffie
 679  *
 680  * Return:
 681  *  calculated jiffies (max: ULONG_MAX)
 682  */
 683 unsigned long timeval2jiffies(struct timeval *tv, int round_up)
 684 {
 685         unsigned long jif;
 686         unsigned long sec  = tv->tv_sec;
 687         unsigned long usec = tv->tv_usec;
 688
 689         /* check for overflow */
 690         if (sec > ULONG_MAX / HZ)
 691                 return ULONG_MAX;
 692
 693         /* any usec below one HZ? => pump it up */
 694         if (round_up)
 695                 usec += 1000000 / HZ - 1;
 696
 697         jif = usec / (1000000 / HZ);
 698
 699         /* check for overflow */
 700         if (sec * HZ > ULONG_MAX - jif)
 701                 return ULONG_MAX;
 702         else
 703                 return jif + sec * HZ;
 704 }
 705 EXPORT_SYMBOL_GPL(timeval2jiffies);
 706
 707 /*
 708  * af_can debugging stuff
 709  */
 710
 711 #ifdef CONFIG_CAN_DEBUG_CORE
 712
 713 #define DBG_BSIZE 1024
 714
 715 /**
 716  * can_debug_cframe - print CAN frame
 717  * @msg: pointer to message printed before the given CAN frame
 718  * @cf: pointer to CAN frame
 719  */
 720 void can_debug_cframe(const char *msg, struct can_frame *cf, ...)
 721 {
 722         va_list ap;
 723         int len;
 724         int dlc, i;
 725         char *buf;
 726
 727         buf = kmalloc(DBG_BSIZE, GFP_ATOMIC);
 728         if (!buf)
 729                 return;
 730
 731         len = sprintf(buf, KERN_DEBUG);
 732         va_start(ap, cf);
 733         len += snprintf(buf + len, DBG_BSIZE - 64, msg, ap);
 734         buf[len++] = ':';
 735         buf[len++] = ' ';
 736         va_end(ap);
 737
 738         dlc = cf->can_dlc;
 739         if (dlc > 8)
 740                 dlc = 8;
 741
 742         if (cf->can_id & CAN_EFF_FLAG)
 743                 len += sprintf(buf + len, "<%08X> [%X] ",
 744                                cf->can_id & CAN_EFF_MASK, dlc);
 745         else
 746                 len += sprintf(buf + len, "<%03X> [%X] ",
 747                                cf->can_id & CAN_SFF_MASK, dlc);
 748
 749         for (i = 0; i < dlc; i++)
 750                 len += sprintf(buf + len, "%02X ", cf->data[i]);
 751
 752         if (cf->can_id & CAN_RTR_FLAG)
 753                 len += sprintf(buf + len, "(RTR)");
 754
 755         buf[len++] = '\n';
 756         buf[len]   = '\0';
 757         printk(buf);
 758         kfree(buf);
 759 }
 760 EXPORT_SYMBOL_GPL(can_debug_cframe);
 761
 762 /**
 763  * can_debug_skb - print socket buffer content to kernel log
 764  * @skb: pointer to socket buffer
 765  */
 766 void can_debug_skb(struct sk_buff *skb)
 767 {
 768         int len, nbytes, i;
 769         char *buf;
 770
 771         buf = kmalloc(DBG_BSIZE, GFP_ATOMIC);
 772         if (!buf)
 773                 return;
 774
 775         len = sprintf(buf,
 776                       KERN_DEBUG "  skbuff at %p, dev: %d, proto: %04x\n"
 777                       KERN_DEBUG "  users: %d, dataref: %d, nr_frags: %d, "
 778                       "h,d,t,e,l: %p %+d %+d %+d, %d",
 779                       skb, skb->dev ? skb->dev->ifindex : -1,
 780                       ntohs(skb->protocol),
 781                       atomic_read(&skb->users),
 782                       atomic_read(&(skb_shinfo(skb)->dataref)),
 783                       skb_shinfo(skb)->nr_frags,
 784                       skb->head, skb->data - skb->head,
 785                       skb->tail - skb->head, skb->end - skb->head, skb->len);
 786         nbytes = skb->end - skb->head;
 787         for (i = 0; i < nbytes; i++) {
 788                 if (i % 16 == 0)
 789                         len += sprintf(buf + len, "\n" KERN_DEBUG "  ");
 790                 if (len < DBG_BSIZE - 16) {
 791                         len += sprintf(buf + len, " %02x", skb->head[i]);
 792                 } else {
 793                         len += sprintf(buf + len, "...");
 794                         break;
 795                 }
 796         }
 797         buf[len++] = '\n';
 798         buf[len]   = '\0';
 799         printk(buf);
 800         kfree(buf);
 801 }
 802 EXPORT_SYMBOL_GPL(can_debug_skb);
 803
 804 #endif
 805
 806 /*
 807  * af_can protocol functions
 808  */
 809
 810 /**
 811  * can_proto_register - register CAN transport protocol
 812  * @cp: pointer to CAN protocol structure
 813  *
 814  * Return:
 815  *  0 on success
 816  *  -EINVAL invalid (out of range) protocol number
 817  *  -EBUSY  protocol already in use
 818  *  -ENOBUF if proto_register() fails
 819  */
 820 int can_proto_register(struct can_proto *cp)
 821 {
 822         int proto = cp->protocol;
 823         int err = 0;
 824
 825         if (proto < 0 || proto >= CAN_NPROTO) {
 826                 printk(KERN_ERR "can: protocol number %d out "
 827                        "of range\n", proto);
 828                 return -EINVAL;
 829         }
 830         if (proto_tab[proto]) {
 831                 printk(KERN_ERR "can: protocol %d already "
 832                        "registered\n", proto);
 833                 return -EBUSY;
 834         }
 835
 836 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,12)
 837         err = proto_register(cp->prot, 0);
 838         if (err < 0)
 839                 return err;
 840 #endif
 841
 842         proto_tab[proto] = cp;
 843
 844         /* use generic ioctl function if the module doesn't bring its own */
 845         if (!cp->ops->ioctl)
 846                 cp->ops->ioctl = can_ioctl;
 847
 848         return err;
 849 }
 850 EXPORT_SYMBOL_GPL(can_proto_register);
 851
 852 /**
 853  * can_proto_unregister - unregister CAN transport protocol
 854  * @cp: pointer to CAN protocol structure
 855  *
 856  * Return:
 857  *  0 on success
 858  *  -ESRCH protocol number was not registered
 859  */
 860 int can_proto_unregister(struct can_proto *cp)
 861 {
 862         int proto = cp->protocol;
 863
 864         if (!proto_tab[proto]) {
 865                 printk(KERN_ERR "can: protocol %d is not registered\n", proto);
 866                 return -ESRCH;
 867         }
 868 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,12)
 869         proto_unregister(cp->prot);
 870 #endif
 871         proto_tab[proto] = NULL;
 872
 873         return 0;
 874 }
 875 EXPORT_SYMBOL_GPL(can_proto_unregister);
 876
 877 /**
 878  * can_dev_register - subscribe notifier for CAN device status changes
 879  * @dev: pointer to netdevice
 880  * @func: callback function on status change
 881  * @data: returned parameter for callback function
 882  *
 883  * Description:
 884  *  Invokes the callback function with the status 'msg' and the given
 885  *  parameter 'data' on a status change of the given CAN network device.
 886  *
 887  * Return:
 888  *  0 on success
 889  *  -ENOMEM on missing mem to create subscription entry
 890  *  -ENODEV unknown device
 891  */
 892 int can_dev_register(struct net_device *dev,
 893                      void (*func)(unsigned long msg, void *), void *data)
 894 {
 895         struct notifier *n;
 896
 897         DBG("called for %s\n", dev->name);
 898
 899         if (!dev || dev->type != ARPHRD_CAN)
 900                 return -ENODEV;
 901
 902         n = kmalloc(sizeof(*n), GFP_KERNEL);
 903         if (!n)
 904                 return -ENOMEM;
 905
 906         n->dev  = dev;
 907         n->func = func;
 908         n->data = data;
 909
 910         write_lock(&notifier_lock);
 911         list_add(&n->list, &notifier_list);
 912         write_unlock(&notifier_lock);
 913
 914         return 0;
 915 }
 916 EXPORT_SYMBOL_GPL(can_dev_register);
 917
 918 /**
 919  * can_dev_unregister - unsubscribe notifier for CAN device status changes
 920  * @dev: pointer to netdevice
 921  * @func: callback function on filter match
 922  * @data: returned parameter for callback function
 923  *
 924  * Description:
 925  *  Removes subscription entry depending on given (subscription) values.
 926  *
 927  * Return:
 928  *  0 on success
 929  *  -EINVAL on missing subscription entry
 930  */
 931 int can_dev_unregister(struct net_device *dev,
 932                        void (*func)(unsigned long msg, void *), void *data)
 933 {
 934         struct notifier *n, *next;
 935         int ret = -EINVAL;
 936
 937         DBG("called for %s\n", dev->name);
 938
 939         write_lock(&notifier_lock);
 940         list_for_each_entry_safe(n, next, &notifier_list, list) {
 941                 if (n->dev == dev && n->func == func && n->data == data) {
 942                         list_del(&n->list);
 943                         kfree(n);
 944                         ret = 0;
 945                         break;
 946                 }
 947         }
 948         write_unlock(&notifier_lock);
 949
 950         return ret;
 951 }
 952 EXPORT_SYMBOL_GPL(can_dev_unregister);
 953
 954 static int can_notifier(struct notifier_block *nb,
 955                         unsigned long msg, void *data)
 956 {
 957         struct net_device *dev = (struct net_device *)data;
 958         struct notifier *n;
 959         struct dev_rcv_lists *d;
 960         int i;
 961
 962         DBG("called for %s, msg = %lu\n", dev->name, msg);
 963
 964         if (dev->type != ARPHRD_CAN)
 965                 return NOTIFY_DONE;
 966
 967         switch (msg) {
 968
 969         case NETDEV_REGISTER:
 970
 971                 /*
 972                  * create new dev_rcv_lists for this device
 973                  *
 974                  * N.B. zeroing the struct is the correct initialization
 975                  * for the embedded hlist_head structs.
 976                  * Another list type, e.g. list_head, would require
 977                  * explicit initialization.
 978                  */
 979
 980                 DBG("creating new dev_rcv_lists for %s\n", dev->name);
 981
 982                 d = kzalloc(sizeof(*d),
 983                             in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
 984                 if (!d) {
 985                         printk(KERN_ERR "can: allocation of receive "
 986                                "list failed\n");
 987                         return NOTIFY_DONE;
 988                 }
 989                 d->dev = dev;
 990
 991                 spin_lock_bh(&rcv_lists_lock);
 992                 hlist_add_head_rcu(&d->list, &rx_dev_list);
 993                 spin_unlock_bh(&rcv_lists_lock);
 994
 995                 break;
 996
 997         case NETDEV_UNREGISTER:
 998                 spin_lock_bh(&rcv_lists_lock);
 999
1000                 d = find_dev_rcv_lists(dev);
1001                 if (d) {
1002                         hlist_del_rcu(&d->list);
1003
1004                         /* remove all receivers hooked at this netdevice */
1005                         can_rx_delete_all(&d->rx_err);
1006                         can_rx_delete_all(&d->rx_all);
1007                         can_rx_delete_all(&d->rx_fil);
1008                         can_rx_delete_all(&d->rx_inv);
1009                         can_rx_delete_all(&d->rx_eff);
1010                         for (i = 0; i < 2048; i++)
1011                                 can_rx_delete_all(&d->rx_sff[i]);
1012                 } else
1013                         printk(KERN_ERR "can: notifier: receive list not "
1014                                "found for dev %s\n", dev->name);
1015
1016                 spin_unlock_bh(&rcv_lists_lock);
1017
1018                 if (d)
1019                         call_rcu(&d->rcu, can_rcv_lists_delete);
1020
1021                 break;
1022         }
1023
1024         read_lock(&notifier_lock);
1025         list_for_each_entry(n, &notifier_list, list) {
1026                 if (n->dev == dev)
1027                         n->func(msg, n->data);
1028         }
1029         read_unlock(&notifier_lock);
1030
1031         return NOTIFY_DONE;
1032 }
1033
1034 /*
1035  * af_can module init/exit functions
1036  */
1037
1038 static struct packet_type can_packet = {
1039         .type = __constant_htons(ETH_P_CAN),
1040         .dev  = NULL,
1041         .func = can_rcv,
1042 };
1043
1044 static struct net_proto_family can_family_ops = {
1045         .family = PF_CAN,
1046         .create = can_create,
1047         .owner  = THIS_MODULE,
1048 };
1049
1050 /* notifier block for netdevice event */
1051 static struct notifier_block can_netdev_notifier = {
1052         .notifier_call = can_notifier,
1053 };
1054
1055 static __init int can_init(void)
1056 {
1057         printk(banner);
1058
1059         rcv_cache = kmem_cache_create("can_receiver", sizeof(struct receiver),
1060                                       0, 0, NULL, NULL);
1061         if (!rcv_cache)
1062                 return -ENOMEM;
1063
1064         /*
1065          * Insert struct dev_rcv_lists for reception on all devices.
1066          * This struct is zero initialized which is correct for the
1067          * embedded hlist heads, the dev pointer, and the entries counter.
1068          */
1069
1070         spin_lock_bh(&rcv_lists_lock);
1071         hlist_add_head_rcu(&rx_alldev_list.list, &rx_dev_list);
1072         spin_unlock_bh(&rcv_lists_lock);
1073
1074         if (stats_timer) {
1075                 /* the statistics are updated every second (timer triggered) */
1076                 init_timer(&stattimer);
1077                 stattimer.function = can_stat_update;
1078                 stattimer.data = 0;
1079                 /* update every second */
1080                 stattimer.expires = jiffies + HZ;
1081                 /* start statistics timer */
1082                 add_timer(&stattimer);
1083         }
1084
1085         /* procfs init */
1086         can_init_proc();
1087
1088         /* protocol register */
1089         sock_register(&can_family_ops);
1090         register_netdevice_notifier(&can_netdev_notifier);
1091         dev_add_pack(&can_packet);
1092
1093         return 0;
1094 }
1095
1096 static __exit void can_exit(void)
1097 {
1098         struct dev_rcv_lists *d;
1099         struct hlist_node *n, *next;
1100
1101         if (stats_timer)
1102                 del_timer(&stattimer);
1103
1104         /* procfs remove */
1105         can_remove_proc();
1106
1107         /* protocol unregister */
1108         dev_remove_pack(&can_packet);
1109         unregister_netdevice_notifier(&can_netdev_notifier);
1110         sock_unregister(PF_CAN);
1111
1112         /* remove rx_dev_list */
1113         spin_lock_bh(&rcv_lists_lock);
1114         hlist_del(&rx_alldev_list.list);
1115         hlist_for_each_entry_safe(d, n, next, &rx_dev_list, list) {
1116                 hlist_del(&d->list);
1117                 kfree(d);
1118         }
1119         spin_unlock_bh(&rcv_lists_lock);
1120
1121         kmem_cache_destroy(rcv_cache);
1122 }
1123
1124 module_init(can_init);
1125 module_exit(can_exit);