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