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