patch-series/net-2.6.24/03-can-raw-proto.diff

   1 DESC
   2 CAN: Add raw protocol
   3 EDESC
   4 This patch adds the CAN raw protocol.
   5
   6 Signed-off-by: Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
   7 Signed-off-by: Urs Thuermann <urs.thuermann@volkswagen.de>
   8
   9 ---
  10  include/linux/can/raw.h |   31 +
  11  net/can/Kconfig         |   11
  12  net/can/Makefile        |    3
  13  net/can/raw.c           |  821 ++++++++++++++++++++++++++++++++++++++++++++++++
  14  4 files changed, 866 insertions(+)
  15
  16 Index: net-2.6.24/include/linux/can/raw.h
  17 ===================================================================
  18 --- /dev/null   1970-01-01 00:00:00.000000000 +0000
  19 +++ net-2.6.24/include/linux/can/raw.h  2007-10-02 08:33:39.000000000 +0200
  20 @@ -0,0 +1,31 @@
  21 +/*
  22 + * linux/can/raw.h
  23 + *
  24 + * Definitions for raw CAN sockets
  25 + *
  26 + * Authors: Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
  27 + *          Urs Thuermann   <urs.thuermann@volkswagen.de>
  28 + * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
  29 + * All rights reserved.
  30 + *
  31 + * Send feedback to <socketcan-users@lists.berlios.de>
  32 + *
  33 + */
  34 +
  35 +#ifndef CAN_RAW_H
  36 +#define CAN_RAW_H
  37 +
  38 +#include <linux/can.h>
  39 +
  40 +#define SOL_CAN_RAW (SOL_CAN_BASE + CAN_RAW)
  41 +
  42 +/* for socket options affecting the socket (not the global system) */
  43 +
  44 +enum {
  45 +       CAN_RAW_FILTER = 1,     /* set 0 .. n can_filter(s)          */
  46 +       CAN_RAW_ERR_FILTER,     /* set filter for error frames       */
  47 +       CAN_RAW_LOOPBACK,       /* local loopback (default:on)       */
  48 +       CAN_RAW_RECV_OWN_MSGS   /* receive my own msgs (default:off) */
  49 +};
  50 +
  51 +#endif
  52 Index: net-2.6.24/net/can/Kconfig
  53 ===================================================================
  54 --- net-2.6.24.orig/net/can/Kconfig     2007-10-02 06:18:29.000000000 +0200
  55 +++ net-2.6.24/net/can/Kconfig  2007-10-02 08:35:31.000000000 +0200
  56 @@ -16,6 +16,17 @@
  57           If you want CAN support, you should say Y here and also to the
  58           specific driver for your controller(s) below.
  59
  60 +config CAN_RAW
  61 +       tristate "Raw CAN Protocol (raw access with CAN-ID filtering)"
  62 +       depends on CAN
  63 +       default N
  64 +       ---help---
  65 +         The Raw CAN protocol option offers access to the CAN bus via
  66 +         the BSD socket API. You probably want to use the raw socket in
  67 +         most cases where no higher level protocol is being used. The raw
  68 +         socket has several filter options e.g. ID-Masking / Errorframes.
  69 +         To receive/send raw CAN messages, use AF_CAN with protocol CAN_RAW.
  70 +
  71  config CAN_DEBUG_CORE
  72         bool "CAN Core debugging messages"
  73         depends on CAN
  74 Index: net-2.6.24/net/can/Makefile
  75 ===================================================================
  76 --- net-2.6.24.orig/net/can/Makefile    2007-10-02 06:18:29.000000000 +0200
  77 +++ net-2.6.24/net/can/Makefile 2007-10-02 08:35:31.000000000 +0200
  78 @@ -4,3 +4,6 @@
  79
  80  obj-$(CONFIG_CAN)      += can.o
  81  can-objs               := af_can.o proc.o
  82 +
  83 +obj-$(CONFIG_CAN_RAW)  += can-raw.o
  84 +can-raw-objs           := raw.o
  85 Index: net-2.6.24/net/can/raw.c
  86 ===================================================================
  87 --- /dev/null   1970-01-01 00:00:00.000000000 +0000
  88 +++ net-2.6.24/net/can/raw.c    2007-10-02 08:35:48.000000000 +0200
  89 @@ -0,0 +1,821 @@
  90 +/*
  91 + * raw.c - Raw sockets for protocol family CAN
  92 + *
  93 + * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
  94 + * All rights reserved.
  95 + *
  96 + * Redistribution and use in source and binary forms, with or without
  97 + * modification, are permitted provided that the following conditions
  98 + * are met:
  99 + * 1. Redistributions of source code must retain the above copyright
 100 + *    notice, this list of conditions and the following disclaimer.
 101 + * 2. Redistributions in binary form must reproduce the above copyright
 102 + *    notice, this list of conditions and the following disclaimer in the
 103 + *    documentation and/or other materials provided with the distribution.
 104 + * 3. Neither the name of Volkswagen nor the names of its contributors
 105 + *    may be used to endorse or promote products derived from this software
 106 + *    without specific prior written permission.
 107 + *
 108 + * Alternatively, provided that this notice is retained in full, this
 109 + * software may be distributed under the terms of the GNU General
 110 + * Public License ("GPL") version 2, in which case the provisions of the
 111 + * GPL apply INSTEAD OF those given above.
 112 + *
 113 + * The provided data structures and external interfaces from this code
 114 + * are not restricted to be used by modules with a GPL compatible license.
 115 + *
 116 + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 117 + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 118 + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 119 + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 120 + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 121 + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 122 + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 123 + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 124 + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 125 + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 126 + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 127 + * DAMAGE.
 128 + *
 129 + * Send feedback to <socketcan-users@lists.berlios.de>
 130 + *
 131 + */
 132 +
 133 +#include <linux/module.h>
 134 +#include <linux/init.h>
 135 +#include <linux/uio.h>
 136 +#include <linux/poll.h>
 137 +#include <linux/net.h>
 138 +#include <linux/netdevice.h>
 139 +#include <linux/socket.h>
 140 +#include <linux/if_arp.h>
 141 +#include <linux/skbuff.h>
 142 +#include <linux/can.h>
 143 +#include <linux/can/core.h>
 144 +#include <linux/can/raw.h>
 145 +#include <net/sock.h>
 146 +#include <net/net_namespace.h>
 147 +
 148 +#define IDENT "raw"
 149 +#define CAN_RAW_VERSION CAN_VERSION
 150 +static __initdata const char banner[] =
 151 +       KERN_INFO "can: raw protocol (rev " CAN_RAW_VERSION ")\n";
 152 +
 153 +MODULE_DESCRIPTION("PF_CAN raw protocol");
 154 +MODULE_LICENSE("Dual BSD/GPL");
 155 +MODULE_AUTHOR("Urs Thuermann <urs.thuermann@volkswagen.de>");
 156 +
 157 +#ifdef CONFIG_CAN_DEBUG_CORE
 158 +static int debug;
 159 +module_param(debug, int, S_IRUGO);
 160 +MODULE_PARM_DESC(debug, "debug print mask: 1:debug, 2:frames, 4:skbs");
 161 +#endif
 162 +
 163 +#define MASK_ALL 0
 164 +
 165 +/*
 166 + * A raw socket has a list of can_filters attached to it, each receiving
 167 + * the CAN frames matching that filter.  If the filter list is empty,
 168 + * no CAN frames will be received by the socket.  The default after
 169 + * opening the socket, is to have one filter which receives all frames.
 170 + * The filter list is allocated dynamically with the exception of the
 171 + * list containing only one item.  This common case is optimized by
 172 + * storing the single filter in dfilter, to avoid using dynamic memory.
 173 + */
 174 +
 175 +struct raw_sock {
 176 +       struct sock sk;
 177 +       int bound;
 178 +       int ifindex;
 179 +       struct notifier_block notifier;
 180 +       int loopback;
 181 +       int recv_own_msgs;
 182 +       int count;                 /* number of active filters */
 183 +       struct can_filter dfilter; /* default/single filter */
 184 +       struct can_filter *filter; /* pointer to filter(s) */
 185 +       can_err_mask_t err_mask;
 186 +};
 187 +
 188 +static inline struct raw_sock *raw_sk(const struct sock *sk)
 189 +{
 190 +       return (struct raw_sock *)sk;
 191 +}
 192 +
 193 +static void raw_rcv(struct sk_buff *skb, void *data)
 194 +{
 195 +       struct sock *sk = (struct sock *)data;
 196 +       struct raw_sock *ro = raw_sk(sk);
 197 +       struct sockaddr_can *addr;
 198 +       int error;
 199 +
 200 +       DBG("received skbuff %p, sk %p\n", skb, sk);
 201 +       DBG_SKB(skb);
 202 +
 203 +       if (!ro->recv_own_msgs) {
 204 +               /* check the received tx sock reference */
 205 +               if (skb->sk == sk) {
 206 +                       DBG("trashed own tx msg\n");
 207 +                       kfree_skb(skb);
 208 +                       return;
 209 +               }
 210 +       }
 211 +
 212 +       /*
 213 +        *  Put the datagram to the queue so that raw_recvmsg() can
 214 +        *  get it from there.  We need to pass the interface index to
 215 +        *  raw_recvmsg().  We pass a whole struct sockaddr_can in skb->cb
 216 +        *  containing the interface index.
 217 +        */
 218 +
 219 +       BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct sockaddr_can));
 220 +       addr = (struct sockaddr_can *)skb->cb;
 221 +       memset(addr, 0, sizeof(*addr));
 222 +       addr->can_family  = AF_CAN;
 223 +       addr->can_ifindex = skb->dev->ifindex;
 224 +
 225 +       error = sock_queue_rcv_skb(sk, skb);
 226 +       if (error < 0) {
 227 +               DBG("sock_queue_rcv_skb failed: %d\n", error);
 228 +               DBG("freeing skbuff %p\n", skb);
 229 +               kfree_skb(skb);
 230 +       }
 231 +}
 232 +
 233 +static int raw_enable_filters(struct net_device *dev, struct sock *sk,
 234 +                             struct can_filter *filter,
 235 +                             int count)
 236 +{
 237 +       int err = 0;
 238 +       int i;
 239 +
 240 +       for (i = 0; i < count; i++) {
 241 +               DBG("filter can_id %08X, can_mask %08X%s, sk %p\n",
 242 +                   filter[i].can_id, filter[i].can_mask,
 243 +                   filter[i].can_id & CAN_INV_FILTER ? " (inv)" : "", sk);
 244 +
 245 +               err = can_rx_register(dev, filter[i].can_id,
 246 +                                     filter[i].can_mask,
 247 +                                     raw_rcv, sk, IDENT);
 248 +
 249 +               if (err) {
 250 +                       /* clean up successfully registered filters */
 251 +                       while (--i >= 0)
 252 +                               can_rx_unregister(dev, filter[i].can_id,
 253 +                                                 filter[i].can_mask,
 254 +                                                 raw_rcv, sk);
 255 +                       break;
 256 +               }
 257 +       }
 258 +
 259 +       return err;
 260 +}
 261 +
 262 +static int raw_enable_errfilter(struct net_device *dev, struct sock *sk,
 263 +                               can_err_mask_t err_mask)
 264 +{
 265 +       int err = 0;
 266 +
 267 +       if (err_mask)
 268 +               err = can_rx_register(dev, 0, err_mask | CAN_ERR_FLAG,
 269 +                                     raw_rcv, sk, IDENT);
 270 +
 271 +       return err;
 272 +}
 273 +
 274 +static void raw_disable_filters(struct net_device *dev, struct sock *sk,
 275 +                             struct can_filter *filter,
 276 +                             int count)
 277 +{
 278 +       int i;
 279 +
 280 +       for (i = 0; i < count; i++) {
 281 +               DBG("filter can_id %08X, can_mask %08X%s, sk %p\n",
 282 +                   filter[i].can_id, filter[i].can_mask,
 283 +                   filter[i].can_id & CAN_INV_FILTER ? " (inv)" : "", sk);
 284 +
 285 +               can_rx_unregister(dev, filter[i].can_id, filter[i].can_mask,
 286 +                                 raw_rcv, sk);
 287 +       }
 288 +}
 289 +
 290 +static inline void raw_disable_errfilter(struct net_device *dev,
 291 +                                        struct sock *sk,
 292 +                                        can_err_mask_t err_mask)
 293 +
 294 +{
 295 +       if (err_mask)
 296 +               can_rx_unregister(dev, 0, err_mask | CAN_ERR_FLAG,
 297 +                                 raw_rcv, sk);
 298 +}
 299 +
 300 +static inline void raw_disable_allfilters(struct net_device *dev,
 301 +                                         struct sock *sk)
 302 +{
 303 +       struct raw_sock *ro = raw_sk(sk);
 304 +
 305 +       raw_disable_filters(dev, sk, ro->filter, ro->count);
 306 +       raw_disable_errfilter(dev, sk, ro->err_mask);
 307 +}
 308 +
 309 +static int raw_enable_allfilters(struct net_device *dev, struct sock *sk)
 310 +{
 311 +       struct raw_sock *ro = raw_sk(sk);
 312 +       int err;
 313 +
 314 +       err = raw_enable_filters(dev, sk, ro->filter, ro->count);
 315 +       if (!err) {
 316 +               err = raw_enable_errfilter(dev, sk, ro->err_mask);
 317 +               if (err)
 318 +                       raw_disable_filters(dev, sk, ro->filter, ro->count);
 319 +       }
 320 +
 321 +       return err;
 322 +}
 323 +
 324 +static int raw_notifier(struct notifier_block *nb,
 325 +                       unsigned long msg, void *data)
 326 +{
 327 +       struct net_device *dev = (struct net_device *)data;
 328 +       struct raw_sock *ro = container_of(nb, struct raw_sock, notifier);
 329 +       struct sock *sk = &ro->sk;
 330 +
 331 +       DBG("msg %ld for dev %p (%s idx %d) sk %p ro->ifindex %d\n",
 332 +           msg, dev, dev->name, dev->ifindex, sk, ro->ifindex);
 333 +
 334 +       if (dev->nd_net != &init_net)
 335 +               return NOTIFY_DONE;
 336 +
 337 +       if (dev->type != ARPHRD_CAN)
 338 +               return NOTIFY_DONE;
 339 +
 340 +       if (ro->ifindex != dev->ifindex)
 341 +               return NOTIFY_DONE;
 342 +
 343 +       switch (msg) {
 344 +
 345 +       case NETDEV_UNREGISTER:
 346 +               lock_sock(sk);
 347 +               /* remove current filters & unregister */
 348 +               if (ro->bound)
 349 +                       raw_disable_allfilters(dev, sk);
 350 +
 351 +               if (ro->count > 1)
 352 +                       kfree(ro->filter);
 353 +
 354 +               ro->ifindex = 0;
 355 +               ro->bound   = 0;
 356 +               ro->count   = 0;
 357 +               release_sock(sk);
 358 +
 359 +               sk->sk_err = ENODEV;
 360 +               if (!sock_flag(sk, SOCK_DEAD))
 361 +                       sk->sk_error_report(sk);
 362 +               break;
 363 +
 364 +       case NETDEV_DOWN:
 365 +               sk->sk_err = ENETDOWN;
 366 +               if (!sock_flag(sk, SOCK_DEAD))
 367 +                       sk->sk_error_report(sk);
 368 +               break;
 369 +       }
 370 +
 371 +       return NOTIFY_DONE;
 372 +}
 373 +
 374 +static int raw_init(struct sock *sk)
 375 +{
 376 +       struct raw_sock *ro = raw_sk(sk);
 377 +
 378 +       ro->bound            = 0;
 379 +       ro->ifindex          = 0;
 380 +
 381 +       /* set default filter to single entry dfilter */
 382 +       ro->dfilter.can_id   = 0;
 383 +       ro->dfilter.can_mask = MASK_ALL;
 384 +       ro->filter           = &ro->dfilter;
 385 +       ro->count            = 1;
 386 +
 387 +       /* set default loopback behaviour */
 388 +       ro->loopback         = 1;
 389 +       ro->recv_own_msgs    = 0;
 390 +
 391 +       /* set notifier */
 392 +       ro->notifier.notifier_call = raw_notifier;
 393 +
 394 +       register_netdevice_notifier(&ro->notifier);
 395 +
 396 +       return 0;
 397 +}
 398 +
 399 +static int raw_release(struct socket *sock)
 400 +{
 401 +       struct sock *sk = sock->sk;
 402 +       struct raw_sock *ro = raw_sk(sk);
 403 +
 404 +       DBG("socket %p, sk %p, refcnt %d\n", sock, sk,
 405 +           atomic_read(&sk->sk_refcnt));
 406 +
 407 +       unregister_netdevice_notifier(&ro->notifier);
 408 +
 409 +       lock_sock(sk);
 410 +
 411 +       /* remove current filters & unregister */
 412 +       if (ro->bound) {
 413 +               if (ro->ifindex) {
 414 +                       struct net_device *dev;
 415 +
 416 +                       dev = dev_get_by_index(&init_net, ro->ifindex);
 417 +                       if (dev) {
 418 +                               raw_disable_allfilters(dev, sk);
 419 +                               dev_put(dev);
 420 +                       }
 421 +               } else
 422 +                       raw_disable_allfilters(NULL, sk);
 423 +       }
 424 +
 425 +       if (ro->count > 1)
 426 +               kfree(ro->filter);
 427 +
 428 +       ro->ifindex = 0;
 429 +       ro->bound   = 0;
 430 +       ro->count   = 0;
 431 +
 432 +       release_sock(sk);
 433 +       sock_put(sk);
 434 +
 435 +       return 0;
 436 +}
 437 +
 438 +static int raw_bind(struct socket *sock, struct sockaddr *uaddr, int len)
 439 +{
 440 +       struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
 441 +       struct sock *sk = sock->sk;
 442 +       struct raw_sock *ro = raw_sk(sk);
 443 +       int ifindex;
 444 +       int err = 0;
 445 +       int notify_enetdown = 0;
 446 +
 447 +       DBG("socket %p to device %d\n", sock, addr->can_ifindex);
 448 +
 449 +       if (len < sizeof(*addr))
 450 +               return -EINVAL;
 451 +
 452 +       lock_sock(sk);
 453 +
 454 +       if (ro->bound && addr->can_ifindex == ro->ifindex)
 455 +               goto out;
 456 +
 457 +       if (addr->can_ifindex) {
 458 +               struct net_device *dev;
 459 +
 460 +               dev = dev_get_by_index(&init_net, addr->can_ifindex);
 461 +               if (!dev) {
 462 +                       DBG("could not find device %d\n", addr->can_ifindex);
 463 +                       err = -ENODEV;
 464 +                       goto out;
 465 +               }
 466 +               if (dev->type != ARPHRD_CAN) {
 467 +                       DBG("device %d no CAN device\n", addr->can_ifindex);
 468 +                       dev_put(dev);
 469 +                       err = -ENODEV;
 470 +                       goto out;
 471 +               }
 472 +               if (!(dev->flags & IFF_UP))
 473 +                       notify_enetdown = 1;
 474 +
 475 +               ifindex = dev->ifindex;
 476 +
 477 +               /* filters set by default/setsockopt */
 478 +               err = raw_enable_allfilters(dev, sk);
 479 +               dev_put(dev);
 480 +
 481 +       } else {
 482 +               ifindex = 0;
 483 +
 484 +               /* filters set by default/setsockopt */
 485 +               err = raw_enable_allfilters(NULL, sk);
 486 +       }
 487 +
 488 +       if (!err) {
 489 +               if (ro->bound) {
 490 +                       /* unregister old filters */
 491 +                       if (ro->ifindex) {
 492 +                               struct net_device *dev;
 493 +
 494 +                               dev = dev_get_by_index(&init_net, ro->ifindex);
 495 +                               if (dev) {
 496 +                                       raw_disable_allfilters(dev, sk);
 497 +                                       dev_put(dev);
 498 +                               }
 499 +                       } else
 500 +                               raw_disable_allfilters(NULL, sk);
 501 +               }
 502 +               ro->ifindex = ifindex;
 503 +               ro->bound = 1;
 504 +       }
 505 +
 506 + out:
 507 +       release_sock(sk);
 508 +
 509 +       if (notify_enetdown) {
 510 +               sk->sk_err = ENETDOWN;
 511 +               if (!sock_flag(sk, SOCK_DEAD))
 512 +                       sk->sk_error_report(sk);
 513 +       }
 514 +
 515 +       return err;
 516 +}
 517 +
 518 +static int raw_getname(struct socket *sock, struct sockaddr *uaddr,
 519 +                      int *len, int peer)
 520 +{
 521 +       struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
 522 +       struct sock *sk = sock->sk;
 523 +       struct raw_sock *ro = raw_sk(sk);
 524 +
 525 +       if (peer)
 526 +               return -EOPNOTSUPP;
 527 +
 528 +       addr->can_family  = AF_CAN;
 529 +       addr->can_ifindex = ro->ifindex;
 530 +
 531 +       *len = sizeof(*addr);
 532 +
 533 +       return 0;
 534 +}
 535 +
 536 +static unsigned int raw_poll(struct file *file, struct socket *sock,
 537 +                            poll_table *wait)
 538 +{
 539 +       unsigned int mask = 0;
 540 +
 541 +       DBG("socket %p\n", sock);
 542 +
 543 +       mask = datagram_poll(file, sock, wait);
 544 +       return mask;
 545 +}
 546 +
 547 +static int raw_setsockopt(struct socket *sock, int level, int optname,
 548 +                         char __user *optval, int optlen)
 549 +{
 550 +       struct sock *sk = sock->sk;
 551 +       struct raw_sock *ro = raw_sk(sk);
 552 +       struct can_filter *filter = NULL;  /* dyn. alloc'ed filters */
 553 +       struct can_filter sfilter;         /* single filter */
 554 +       struct net_device *dev = NULL;
 555 +       can_err_mask_t err_mask = 0;
 556 +       int count = 0;
 557 +       int err = 0;
 558 +
 559 +       if (level != SOL_CAN_RAW)
 560 +               return -EINVAL;
 561 +       if (optlen < 0)
 562 +               return -EINVAL;
 563 +
 564 +       switch (optname) {
 565 +
 566 +       case CAN_RAW_FILTER:
 567 +               if (optlen % sizeof(struct can_filter) != 0)
 568 +                       return -EINVAL;
 569 +
 570 +               count = optlen / sizeof(struct can_filter);
 571 +
 572 +               if (count > 1) {
 573 +                       /* filter does not fit into dfilter => alloc space */
 574 +                       filter = kmalloc(optlen, GFP_KERNEL);
 575 +                       if (!filter)
 576 +                               return -ENOMEM;
 577 +
 578 +                       err = copy_from_user(filter, optval, optlen);
 579 +                       if (err) {
 580 +                               kfree(filter);
 581 +                               return err;
 582 +                       }
 583 +               } else if (count == 1) {
 584 +                       err = copy_from_user(&sfilter, optval, optlen);
 585 +                       if (err)
 586 +                               return err;
 587 +               }
 588 +
 589 +               lock_sock(sk);
 590 +
 591 +               if (ro->bound && ro->ifindex)
 592 +                       dev = dev_get_by_index(&init_net, ro->ifindex);
 593 +
 594 +               if (ro->bound) {
 595 +                       /* (try to) register the new filters */
 596 +                       if (count == 1)
 597 +                               err = raw_enable_filters(dev, sk, &sfilter, 1);
 598 +                       else
 599 +                               err = raw_enable_filters(dev, sk, filter,
 600 +                                                        count);
 601 +                       if (err) {
 602 +                               if (count > 1)
 603 +                                       kfree(filter);
 604 +
 605 +                               goto out_fil;
 606 +                       }
 607 +
 608 +                       /* remove old filter registrations */
 609 +                       raw_disable_filters(dev, sk, ro->filter, ro->count);
 610 +               }
 611 +
 612 +               /* remove old filter space */
 613 +               if (ro->count > 1)
 614 +                       kfree(ro->filter);
 615 +
 616 +               /* link new filters to the socket */
 617 +               if (count == 1) {
 618 +                       /* copy filter data for single filter */
 619 +                       ro->dfilter = sfilter;
 620 +                       filter = &ro->dfilter;
 621 +               }
 622 +               ro->filter = filter;
 623 +               ro->count  = count;
 624 +
 625 + out_fil:
 626 +               if (dev)
 627 +                       dev_put(dev);
 628 +
 629 +               release_sock(sk);
 630 +
 631 +               break;
 632 +
 633 +       case CAN_RAW_ERR_FILTER:
 634 +               if (optlen != sizeof(err_mask))
 635 +                       return -EINVAL;
 636 +
 637 +               err = copy_from_user(&err_mask, optval, optlen);
 638 +               if (err)
 639 +                       return err;
 640 +
 641 +               err_mask &= CAN_ERR_MASK;
 642 +
 643 +               lock_sock(sk);
 644 +
 645 +               if (ro->bound && ro->ifindex)
 646 +                       dev = dev_get_by_index(&init_net, ro->ifindex);
 647 +
 648 +               /* remove current error mask */
 649 +               if (ro->bound) {
 650 +                       /* (try to) register the new err_mask */
 651 +                       err = raw_enable_errfilter(dev, sk, err_mask);
 652 +
 653 +                       if (err)
 654 +                               goto out_err;
 655 +
 656 +                       /* remove old err_mask registration */
 657 +                       raw_disable_errfilter(dev, sk, ro->err_mask);
 658 +               }
 659 +
 660 +               /* link new err_mask to the socket */
 661 +               ro->err_mask = err_mask;
 662 +
 663 + out_err:
 664 +               if (dev)
 665 +                       dev_put(dev);
 666 +
 667 +               release_sock(sk);
 668 +
 669 +               break;
 670 +
 671 +       case CAN_RAW_LOOPBACK:
 672 +               if (optlen != sizeof(ro->loopback))
 673 +                       return -EINVAL;
 674 +
 675 +               err = copy_from_user(&ro->loopback, optval, optlen);
 676 +
 677 +               break;
 678 +
 679 +       case CAN_RAW_RECV_OWN_MSGS:
 680 +               if (optlen != sizeof(ro->recv_own_msgs))
 681 +                       return -EINVAL;
 682 +
 683 +               err = copy_from_user(&ro->recv_own_msgs, optval, optlen);
 684 +
 685 +               break;
 686 +
 687 +       default:
 688 +               return -ENOPROTOOPT;
 689 +       }
 690 +       return err;
 691 +}
 692 +
 693 +static int raw_getsockopt(struct socket *sock, int level, int optname,
 694 +                         char __user *optval, int __user *optlen)
 695 +{
 696 +       struct sock *sk = sock->sk;
 697 +       struct raw_sock *ro = raw_sk(sk);
 698 +       int len;
 699 +       void *val;
 700 +       int err = 0;
 701 +
 702 +       if (level != SOL_CAN_RAW)
 703 +               return -EINVAL;
 704 +       if (get_user(len, optlen))
 705 +               return -EFAULT;
 706 +       if (len < 0)
 707 +               return -EINVAL;
 708 +
 709 +       switch (optname) {
 710 +
 711 +       case CAN_RAW_FILTER:
 712 +               lock_sock(sk);
 713 +               if (ro->count > 0) {
 714 +                       int fsize = ro->count * sizeof(struct can_filter);
 715 +                       if (len > fsize)
 716 +                               len = fsize;
 717 +                       err = copy_to_user(optval, ro->filter, len);
 718 +               } else
 719 +                       len = 0;
 720 +               release_sock(sk);
 721 +
 722 +               if (!err)
 723 +                       err = put_user(len, optlen);
 724 +               return err;
 725 +
 726 +       case CAN_RAW_ERR_FILTER:
 727 +               if (len > sizeof(can_err_mask_t))
 728 +                       len = sizeof(can_err_mask_t);
 729 +               val = &ro->err_mask;
 730 +               break;
 731 +
 732 +       case CAN_RAW_LOOPBACK:
 733 +               if (len > sizeof(int))
 734 +                       len = sizeof(int);
 735 +               val = &ro->loopback;
 736 +               break;
 737 +
 738 +       case CAN_RAW_RECV_OWN_MSGS:
 739 +               if (len > sizeof(int))
 740 +                       len = sizeof(int);
 741 +               val = &ro->recv_own_msgs;
 742 +               break;
 743 +
 744 +       default:
 745 +               return -ENOPROTOOPT;
 746 +       }
 747 +
 748 +       if (put_user(len, optlen))
 749 +               return -EFAULT;
 750 +       if (copy_to_user(optval, val, len))
 751 +               return -EFAULT;
 752 +       return 0;
 753 +}
 754 +
 755 +static int raw_sendmsg(struct kiocb *iocb, struct socket *sock,
 756 +                      struct msghdr *msg, size_t size)
 757 +{
 758 +       struct sock *sk = sock->sk;
 759 +       struct raw_sock *ro = raw_sk(sk);
 760 +       struct sk_buff *skb;
 761 +       struct net_device *dev;
 762 +       int ifindex;
 763 +       int err;
 764 +
 765 +       DBG("socket %p, sk %p\n", sock, sk);
 766 +
 767 +       if (msg->msg_name) {
 768 +               struct sockaddr_can *addr =
 769 +                       (struct sockaddr_can *)msg->msg_name;
 770 +
 771 +               if (addr->can_family != AF_CAN)
 772 +                       return -EINVAL;
 773 +
 774 +               ifindex = addr->can_ifindex;
 775 +       } else
 776 +               ifindex = ro->ifindex;
 777 +
 778 +       dev = dev_get_by_index(&init_net, ifindex);
 779 +       if (!dev) {
 780 +               DBG("device %d not found\n", ifindex);
 781 +               return -ENXIO;
 782 +       }
 783 +
 784 +       skb = alloc_skb(size, GFP_KERNEL);
 785 +       if (!skb) {
 786 +               dev_put(dev);
 787 +               return -ENOMEM;
 788 +       }
 789 +
 790 +       err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size);
 791 +       if (err < 0) {
 792 +               kfree_skb(skb);
 793 +               dev_put(dev);
 794 +               return err;
 795 +       }
 796 +       skb->dev = dev;
 797 +       skb->sk  = sk;
 798 +
 799 +       DBG("sending skbuff to interface %d\n", ifindex);
 800 +       DBG_SKB(skb);
 801 +
 802 +       err = can_send(skb, ro->loopback);
 803 +
 804 +       dev_put(dev);
 805 +
 806 +       if (err)
 807 +               return err;
 808 +
 809 +       return size;
 810 +}
 811 +
 812 +static int raw_recvmsg(struct kiocb *iocb, struct socket *sock,
 813 +                      struct msghdr *msg, size_t size, int flags)
 814 +{
 815 +       struct sock *sk = sock->sk;
 816 +       struct sk_buff *skb;
 817 +       int error = 0;
 818 +       int noblock;
 819 +
 820 +       DBG("socket %p, sk %p\n", sock, sk);
 821 +
 822 +       noblock =  flags & MSG_DONTWAIT;
 823 +       flags   &= ~MSG_DONTWAIT;
 824 +
 825 +       skb = skb_recv_datagram(sk, flags, noblock, &error);
 826 +       if (!skb)
 827 +               return error;
 828 +
 829 +       DBG("delivering skbuff %p\n", skb);
 830 +       DBG_SKB(skb);
 831 +
 832 +       if (size < skb->len)
 833 +               msg->msg_flags |= MSG_TRUNC;
 834 +       else
 835 +               size = skb->len;
 836 +
 837 +       error = memcpy_toiovec(msg->msg_iov, skb->data, size);
 838 +       if (error < 0) {
 839 +               skb_free_datagram(sk, skb);
 840 +               return error;
 841 +       }
 842 +
 843 +       sock_recv_timestamp(msg, sk, skb);
 844 +
 845 +       if (msg->msg_name) {
 846 +               msg->msg_namelen = sizeof(struct sockaddr_can);
 847 +               memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
 848 +       }
 849 +
 850 +       DBG("freeing sock %p, skbuff %p\n", sk, skb);
 851 +       skb_free_datagram(sk, skb);
 852 +
 853 +       return size;
 854 +}
 855 +
 856 +static struct proto_ops raw_ops __read_mostly = {
 857 +       .family        = PF_CAN,
 858 +       .release       = raw_release,
 859 +       .bind          = raw_bind,
 860 +       .connect       = sock_no_connect,
 861 +       .socketpair    = sock_no_socketpair,
 862 +       .accept        = sock_no_accept,
 863 +       .getname       = raw_getname,
 864 +       .poll          = raw_poll,
 865 +       .ioctl         = NULL,          /* use can_ioctl() from af_can.c */
 866 +       .listen        = sock_no_listen,
 867 +       .shutdown      = sock_no_shutdown,
 868 +       .setsockopt    = raw_setsockopt,
 869 +       .getsockopt    = raw_getsockopt,
 870 +       .sendmsg       = raw_sendmsg,
 871 +       .recvmsg       = raw_recvmsg,
 872 +       .mmap          = sock_no_mmap,
 873 +       .sendpage      = sock_no_sendpage,
 874 +};
 875 +
 876 +static struct proto raw_proto __read_mostly = {
 877 +       .name       = "CAN_RAW",
 878 +       .owner      = THIS_MODULE,
 879 +       .obj_size   = sizeof(struct raw_sock),
 880 +       .init       = raw_init,
 881 +};
 882 +
 883 +static struct can_proto raw_can_proto __read_mostly = {
 884 +       .type       = SOCK_RAW,
 885 +       .protocol   = CAN_RAW,
 886 +       .capability = -1,
 887 +       .ops        = &raw_ops,
 888 +       .prot       = &raw_proto,
 889 +};
 890 +
 891 +static __init int raw_module_init(void)
 892 +{
 893 +       int err;
 894 +
 895 +       printk(banner);
 896 +
 897 +       err = can_proto_register(&raw_can_proto);
 898 +       if (err < 0)
 899 +               printk(KERN_ERR "can: registration of raw protocol failed\n");
 900 +
 901 +       return err;
 902 +}
 903 +
 904 +static __exit void raw_module_exit(void)
 905 +{
 906 +       can_proto_unregister(&raw_can_proto);
 907 +}
 908 +
 909 +module_init(raw_module_init);
 910 +module_exit(raw_module_exit);