kernel/2.4/can/bcm.c

   1 /*
   2  * bcm.c
   3  *
   4  * Copyright (c) 2002-2005 Volkswagen Group Electronic Research
   5  * All rights reserved.
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  * 1. Redistributions of source code must retain the above copyright
  11  *    notice, this list of conditions, the following disclaimer and
  12  *    the referenced file 'COPYING'.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in the
  15  *    documentation and/or other materials provided with the distribution.
  16  * 3. Neither the name of Volkswagen nor the names of its contributors
  17  *    may be used to endorse or promote products derived from this software
  18  *    without specific prior written permission.
  19  *
  20  * Alternatively, provided that this notice is retained in full, this
  21  * software may be distributed under the terms of the GNU General
  22  * Public License ("GPL") version 2 as distributed in the 'COPYING'
  23  * file from the main directory of the linux kernel source.
  24  *
  25  * The provided data structures and external interfaces from this code
  26  * are not restricted to be used by modules with a GPL compatible license.
  27  *
  28  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  29  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  30  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  31  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  32  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  33  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  34  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  35  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  36  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  37  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  38  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  39  * DAMAGE.
  40  *
  41  * Send feedback to <llcf@volkswagen.de>
  42  *
  43  */
  44
  45 #include <linux/module.h>
  46 #include <linux/init.h>
  47 #include <linux/net.h>
  48 #include <linux/netdevice.h>
  49 #include <linux/proc_fs.h>
  50 #include <linux/poll.h>
  51 #include <net/sock.h>
  52
  53 #include "af_can.h"
  54 #include "version.h"
  55 #include "bcm.h"
  56
  57 RCSID("$Id$");
  58
  59 #ifdef DEBUG
  60 MODULE_PARM(debug, "1i");
  61 static int debug = 0;
  62 #define DBG(args...)       (debug & 1 ? \
  63                                (printk(KERN_DEBUG "BCM %s: ", __func__), \
  64                                 printk(args)) : 0)
  65 #define DBG_FRAME(args...) (debug & 2 ? can_debug_cframe(args) : 0)
  66 #define DBG_SKB(skb)       (debug & 4 ? can_debug_skb(skb) : 0)
  67 #else
  68 #define DBG(args...)
  69 #define DBG_FRAME(args...)
  70 #define DBG_SKB(skb)
  71 #endif
  72
  73 /* use of last_frames[index].can_dlc */
  74 #define RX_RECV    0x40 /* received data for this element */
  75 #define RX_THR     0x80 /* this element has not been sent due to throttle functionality */
  76 #define BCM_CAN_DLC_MASK 0x0F /* clean flags by masking with BCM_CAN_DLC_MASK */
  77
  78 #define NAME "Broadcast Manager (BCM) for LLCF"
  79 #define IDENT "bcm"
  80 static __initdata const char banner[] = BANNER(NAME);
  81
  82 MODULE_DESCRIPTION(NAME);
  83 MODULE_LICENSE("Dual BSD/GPL");
  84 MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
  85
  86
  87 #define GET_U64(p) (*(unsigned long long*)(p)->data)
  88
  89 struct bcm_op {
  90     struct bcm_op *next;
  91     canid_t can_id;
  92     int flags;
  93     unsigned long j_ival1, j_ival2, j_lastmsg;
  94     unsigned long frames_abs, frames_filtered;
  95     struct timeval ival1, ival2, stamp;
  96     struct timer_list timer, thrtimer;
  97     int count;
  98     int nframes;
  99     int currframe;
 100     struct can_frame *frames;
 101     struct can_frame *last_frames;
 102     struct sock *sk;
 103 };
 104
 105 struct bcm_user_data {
 106     struct bcm_op *rx_ops;
 107     struct bcm_op *tx_ops;
 108     struct proc_dir_entry *bcm_proc_read;
 109     char procname [9];
 110 };
 111
 112 #define bcm_sk(sk) ((struct bcm_user_data *)(sk)->user_data)
 113
 114 static struct proc_dir_entry *proc_dir = NULL;
 115 int bcm_read_proc(char *page, char **start, off_t off, int count, int *eof, void *data);
 116
 117 static void bcm_notifier(unsigned long msg, void *data);
 118 static int bcm_release(struct socket *sock);
 119 static int bcm_connect(struct socket *sock, struct sockaddr *uaddr, int len,
 120                        int flags);
 121 static unsigned int bcm_poll(struct file *file, struct socket *sock,
 122                              poll_table *wait);
 123 static int bcm_sendmsg(struct socket *sock, struct msghdr *msg, int size,
 124                        struct scm_cookie *scm);
 125 static int bcm_recvmsg(struct socket *sock, struct msghdr *msg, int size,
 126                        int flags, struct scm_cookie *scm);
 127
 128 static void bcm_tx_timeout_handler(unsigned long data);
 129 static void bcm_rx_handler(struct sk_buff *skb, void *op);
 130 static void bcm_rx_timeout_handler(unsigned long data);
 131 static void bcm_rx_thr_handler(unsigned long data);
 132 static struct bcm_op *bcm_find_op(struct bcm_op *ops, canid_t can_id);
 133 static void bcm_insert_op(struct bcm_op **ops, struct bcm_op *op);
 134 static void bcm_delete_tx_op(struct bcm_op **ops, canid_t can_id);
 135 static void bcm_delete_rx_op(struct bcm_op **ops, canid_t can_id);
 136 static void bcm_remove_op(struct bcm_op *op);
 137 static void bcm_can_tx(struct bcm_op *op);
 138 static void bcm_send_to_user(struct sock *sk, struct bcm_msg_head *head,
 139                              struct can_frame *frames, struct timeval *tv);
 140 static void bcm_rx_changed(struct bcm_op *op, struct can_frame *data);
 141 static void bcm_rx_starttimer(struct bcm_op *op);
 142 static void bcm_rx_update_and_send(struct bcm_op *op,
 143                                    struct can_frame *lastdata,
 144                                    struct can_frame *rxdata);
 145 static void bcm_rx_cmp_to_index(struct bcm_op *op, int index,
 146                                 struct can_frame *rxdata);
 147
 148 static struct proto_ops bcm_ops = {
 149     .family        = PF_CAN,
 150     .release       = bcm_release,
 151     .bind          = sock_no_bind,
 152     .connect       = bcm_connect,
 153     .socketpair    = sock_no_socketpair,
 154     .accept        = sock_no_accept,
 155     .getname       = sock_no_getname,
 156     .poll          = bcm_poll,
 157     .ioctl         = NULL,      /* use can_ioctl() from af_can.c */
 158     .listen        = sock_no_listen,
 159     .shutdown      = sock_no_shutdown,
 160     .setsockopt    = sock_no_setsockopt,
 161     .getsockopt    = sock_no_getsockopt,
 162     .sendmsg       = bcm_sendmsg,
 163     .recvmsg       = bcm_recvmsg,
 164     .mmap          = sock_no_mmap,
 165     .sendpage      = sock_no_sendpage,
 166 };
 167
 168 static int __init bcm_init(void)
 169 {
 170     printk(banner);
 171
 172     can_proto_register(CAN_BCM, &bcm_ops);
 173
 174     /* create /proc/can/bcm directory */
 175     proc_dir = proc_mkdir(CAN_PROC_DIR"/"IDENT, NULL);
 176
 177     if (proc_dir)
 178         proc_dir->owner = THIS_MODULE;
 179
 180     return 0;
 181 }
 182
 183 static void __exit bcm_exit(void)
 184 {
 185     can_proto_unregister(CAN_BCM);
 186
 187     if (proc_dir)
 188         remove_proc_entry(CAN_PROC_DIR"/"IDENT, NULL);
 189
 190 }
 191
 192 static void bcm_notifier(unsigned long msg, void *data)
 193 {
 194     struct sock *sk = (struct sock *)data;
 195
 196     DBG("called for sock %p\n", sk);
 197
 198     switch (msg)
 199     {
 200     case NETDEV_UNREGISTER:
 201         sk->bound_dev_if = 0;
 202     case NETDEV_DOWN:
 203         sk->err = ENETDOWN;
 204         sk->error_report(sk);
 205     }
 206 }
 207
 208 static int bcm_release(struct socket *sock)
 209 {
 210     struct sock *sk = sock->sk;
 211     struct bcm_user_data *ud = bcm_sk(sk);
 212     struct bcm_op *op,*next;
 213
 214     /* many things to do here:
 215        free all rx_ops and tx_ops, bcm_user_data structure,
 216        can_rx_unregister(dev, canid, raw_rcv) and can-data in ?x_ops */
 217
 218     DBG("socket %p, sk %p\n", sock, sk);
 219
 220     /* remove userdata, bcm_ops, timer, etc. */
 221
 222     if (ud) {
 223         for (op = ud->tx_ops; op ; op = next) {
 224             DBG("removing tx_op (%p) for can_id <%03X>\n", op, op->can_id);
 225             next = op->next;
 226             bcm_remove_op(op);
 227         }
 228
 229         for (op = ud->rx_ops; op ; op = next) {
 230             DBG("removing rx_op (%p) for can_id <%03X>\n", op, op->can_id);
 231             next = op->next;
 232
 233             if (sk->bound_dev_if) {
 234                 struct net_device *dev = dev_get_by_index(sk->bound_dev_if);
 235                 if (dev) {
 236                     can_rx_unregister(dev, op->can_id, 0xFFFFFFFFU, bcm_rx_handler, op);
 237                     dev_put(dev);
 238                 }
 239             } else
 240                 DBG("sock %p not bound for can_rx_unregister()\n", sk);
 241
 242             bcm_remove_op(op);
 243         }
 244
 245         if ((proc_dir) && (ud->bcm_proc_read)) {
 246             remove_proc_entry(ud->procname, proc_dir);
 247         }
 248
 249         kfree (ud);
 250     }
 251
 252     if (sk->bound_dev_if) {
 253         struct net_device *dev = dev_get_by_index(sk->bound_dev_if);
 254         if (dev) {
 255             can_dev_unregister(dev, bcm_notifier, sk);
 256             dev_put(dev);
 257         }
 258     } else
 259         DBG("sock %p not bound for can_dev_unregister()\n", sk);
 260
 261     sock_put(sk);
 262
 263     return 0;
 264 }
 265
 266 static int bcm_connect(struct socket *sock, struct sockaddr *uaddr, int len,
 267                        int flags)
 268 {
 269     struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
 270     struct sock *sk = sock->sk;
 271     struct net_device *dev;
 272     struct bcm_user_data *ud;
 273
 274     /* bind a device to this socket */
 275
 276     dev = dev_get_by_index(addr->can_ifindex);
 277     if (!dev) {
 278         DBG("could not find device %d\n", addr->can_ifindex);
 279         return -ENODEV;
 280     }
 281     sk->bound_dev_if = dev->ifindex;
 282     can_dev_register(dev, bcm_notifier, sk);
 283     dev_put(dev);
 284
 285     DBG("socket %p to device %s (idx %d)\n", sock, dev->name, dev->ifindex);
 286
 287     /* create struct for BCM-specific data for this socket */
 288
 289     if (!(ud = kmalloc(sizeof(struct bcm_user_data), GFP_KERNEL)))
 290         return -ENOMEM;
 291
 292     /* intitial BCM operations */
 293     ud->tx_ops = NULL;
 294     ud->rx_ops = NULL;
 295     ud->bcm_proc_read = NULL;
 296
 297     sk->user_data = ud;
 298
 299     if (proc_dir) {
 300         sprintf(ud->procname, "%p", ud);
 301         ud->bcm_proc_read = create_proc_read_entry(ud->procname, 0644,
 302                                                    proc_dir, bcm_read_proc, ud);
 303     }
 304
 305     return 0;
 306 }
 307
 308 int bcm_read_proc(char *page, char **start, off_t off, int count, int *eof, void *data)
 309 {
 310     int len = 0;
 311     struct bcm_user_data *ud = (struct bcm_user_data *) data;
 312     struct bcm_op *op;
 313     struct net_device *dev = NULL;
 314
 315     MOD_INC_USE_COUNT;
 316
 317     len += snprintf(page + len, PAGE_SIZE - len,">>> ud %p", ud);
 318
 319     if (ud->rx_ops) {
 320         if (ud->rx_ops->sk->bound_dev_if)
 321             dev = dev_get_by_index(ud->rx_ops->sk->bound_dev_if);
 322         len += snprintf(page + len, PAGE_SIZE - len,
 323                         " / sk %p / socket %p", ud->rx_ops->sk, ud->rx_ops->sk->socket);
 324     }
 325     else
 326         if (ud->tx_ops) {
 327             if (ud->tx_ops->sk->bound_dev_if)
 328                 dev = dev_get_by_index(ud->tx_ops->sk->bound_dev_if);
 329             len += snprintf(page + len, PAGE_SIZE - len,
 330                             " / sk %p / socket %p", ud->tx_ops->sk, ud->tx_ops->sk->socket);
 331     }
 332
 333     if (dev) {
 334         len += snprintf(page + len, PAGE_SIZE - len, " / %s", dev->name);
 335         dev_put(dev);
 336     }
 337
 338     len += snprintf(page + len, PAGE_SIZE - len, " <<<\n");
 339
 340     for (op = ud->rx_ops; op && (len < PAGE_SIZE - 100); op = op->next) {
 341
 342         unsigned long reduction;
 343
 344         /* print only active entries & prevent division by zero */
 345         if (!op->frames_abs)
 346             continue;
 347
 348         len += snprintf(page + len, PAGE_SIZE - len, "rx_op: %03X [%d]%c ",
 349                         op->can_id, op->nframes,(op->flags & RX_CHECK_DLC)?'d':' ');
 350         if (op->j_ival1)
 351             len += snprintf(page + len, PAGE_SIZE - len, "timeo=%ld ", op->j_ival1);
 352
 353         if (op->j_ival2)
 354             len += snprintf(page + len, PAGE_SIZE - len, "thr=%ld ", op->j_ival2);
 355
 356         len += snprintf(page + len, PAGE_SIZE - len, "# recv %ld (%ld) => reduction: ",
 357                         op->frames_filtered, op->frames_abs);
 358
 359         reduction = 100 - (op->frames_filtered * 100) / op->frames_abs;
 360
 361         len += snprintf(page + len, PAGE_SIZE - len, "%s%ld%%\n",
 362                         (reduction == 100)?"near ":"", reduction);
 363
 364         if (len > PAGE_SIZE - 100) /* 100 Bytes before end of buffer */
 365           len += snprintf(page + len, PAGE_SIZE - len, "(..)\n"); /* mark output cutted off */
 366     }
 367
 368     for (op = ud->tx_ops; op && (len < PAGE_SIZE - 100); op = op->next) {
 369
 370         len += snprintf(page + len, PAGE_SIZE - len, "tx_op: %03X [%d] ",
 371                         op->can_id, op->nframes);
 372         if (op->j_ival1)
 373             len += snprintf(page + len, PAGE_SIZE - len, "t1=%ld ", op->j_ival1);
 374
 375         if (op->j_ival2)
 376             len += snprintf(page + len, PAGE_SIZE - len, "t2=%ld ", op->j_ival2);
 377
 378         len += snprintf(page + len, PAGE_SIZE - len, "# sent %ld\n", op->frames_abs);
 379
 380         if (len > PAGE_SIZE - 100) /* 100 Bytes before end of buffer */
 381           len += snprintf(page + len, PAGE_SIZE - len, "(..)\n"); /* mark output cutted off */
 382     }
 383
 384     len += snprintf(page + len, PAGE_SIZE - len, "\n");
 385
 386     MOD_DEC_USE_COUNT;
 387
 388     *eof = 1;
 389     return len;
 390 }
 391
 392 static unsigned int bcm_poll(struct file *file, struct socket *sock,
 393                              poll_table *wait)
 394 {
 395     unsigned int mask = 0;
 396
 397     DBG("socket %p\n", sock);
 398
 399     mask = datagram_poll(file, sock, wait);
 400     return mask;
 401 }
 402
 403 static int bcm_sendmsg(struct socket *sock, struct msghdr *msg, int size,
 404                        struct scm_cookie *scm)
 405 {
 406     struct bcm_msg_head msg_head;
 407     int i;
 408     struct bcm_op *op;
 409     int err;
 410     struct sock *sk = sock->sk;
 411     struct bcm_user_data *ud = bcm_sk(sk);
 412     char c;
 413     int rbytes = 0; /* read bytes as return value */
 414
 415     /* read message head information */
 416
 417     if ((err = memcpy_fromiovec((unsigned char*)&msg_head, msg->msg_iov,
 418                                 sizeof(msg_head))) < 0)
 419         return err;
 420
 421     DBG("opcode %d for can_id <%03X>\n", msg_head.opcode, msg_head.can_id);
 422
 423     if (!sk->bound_dev_if) {
 424         DBG("sock %p not bound\n", sk); /* and therefore ud not initialized */
 425         return -ENOTCONN;
 426     }
 427
 428     switch (msg_head.opcode) {
 429
 430     case TX_SETUP:
 431
 432         if (msg_head.nframes < 1) /* we need at least one can_frame */
 433             return -EINVAL;
 434
 435         /* check the given can_id */
 436
 437         if (!(op = bcm_find_op(ud->tx_ops, msg_head.can_id))) {
 438
 439             /* insert new BCM operation for the given can_id */
 440
 441             if (!(op = kmalloc(sizeof(struct bcm_op), GFP_KERNEL)))
 442                 return -ENOMEM;
 443
 444             memset(op, 0, sizeof(struct bcm_op)); /* init to zero, e.g. for timers */
 445
 446             DBG("TX_SETUP: creating new tx_op (%p) for can_id <%03X>\n",
 447                 op, msg_head.can_id);
 448
 449             op->can_id    = msg_head.can_id;
 450
 451             /* create array for can_frames and copy the data */
 452             if (!(op->frames = kmalloc(msg_head.nframes * sizeof(struct can_frame), GFP_KERNEL))) {
 453                 kfree(op);
 454                 return -ENOMEM;
 455             }
 456
 457             for (i = 0; i < msg_head.nframes; i++) {
 458                 memcpy_fromiovec((unsigned char*)&op->frames[i], msg->msg_iov, sizeof(struct can_frame));
 459                 if (msg_head.flags & TX_CP_CAN_ID)
 460                     op->frames[i].can_id = msg_head.can_id; /* copy can_id into frame */
 461             }
 462
 463             op->last_frames = NULL; /* tx_ops never compare with previous received messages */
 464
 465             op->sk = sk; /* bcm_can_tx / bcm_tx_timeout_handler needs this */
 466
 467             init_timer(&op->timer); /* initialize uninitialized (kmalloc) structure */
 468             init_timer(&op->thrtimer); /* currently unused in tx_ops */
 469
 470             op->timer.function = bcm_tx_timeout_handler; /* handler for tx_ops */
 471             op->timer.data = (unsigned long)op; /* timer.data points to this op-structure */
 472
 473             /* add this bcm_op to the list of the tx_ops */
 474             bcm_insert_op(&ud->tx_ops, op);
 475
 476         }
 477         else {
 478             /* update existing BCM operation */
 479
 480             DBG("TX_SETUP: modifying existing tx_op (%p) for can_id <%03X>\n",
 481                 op, msg_head.can_id);
 482
 483             /* do we need more space for the can_frames? */
 484             if (msg_head.nframes > op->nframes) {
 485
 486                 /* yes => create new array */
 487
 488                 struct can_frame *p;
 489                 if (!(p = kmalloc(msg_head.nframes * sizeof(struct can_frame), GFP_KERNEL)))
 490                     return -ENOMEM;
 491
 492                 kfree (op->frames);
 493                 op->frames = p;
 494             }
 495
 496             /* update can_frames content */
 497             for (i = 0; i < msg_head.nframes; i++) {
 498                 memcpy_fromiovec((unsigned char*)&op->frames[i], msg->msg_iov, sizeof(struct can_frame));
 499                 if (msg_head.flags & TX_CP_CAN_ID)
 500                     op->frames[i].can_id = msg_head.can_id; /* copy can_id into frame */
 501             }
 502
 503         }
 504
 505         if (op->nframes != msg_head.nframes) {
 506             op->nframes   = msg_head.nframes;
 507             op->currframe = 0; /* start multiple frame transmission with index 0 */
 508         }
 509
 510         /* check flags */
 511
 512         op->flags = msg_head.flags;
 513
 514         if (op->flags & TX_RESET_MULTI_IDX)
 515             op->currframe = 0; /* start multiple frame transmission with index 0 */
 516
 517         if (op->flags & SETTIMER) {
 518
 519             /* set timer values */
 520
 521             op->count   = msg_head.count;
 522             op->ival1   = msg_head.ival1;
 523             op->ival2   = msg_head.ival2;
 524             op->j_ival1 = timeval2jiffies(&msg_head.ival1, 1);
 525             op->j_ival2 = timeval2jiffies(&msg_head.ival2, 1);
 526
 527             DBG("TX_SETUP: SETTIMER count=%d j_ival1=%ld j_ival2=%ld\n",
 528                 op->count, op->j_ival1, op->j_ival2);
 529
 530             /* disable an active timer due to zero values? */
 531             if (!op->j_ival1 && !op->j_ival2) {
 532                 del_timer(&op->timer);
 533                 DBG("TX_SETUP: SETTIMER disabled timer.\n");
 534             }
 535
 536         }
 537
 538         if ((op->flags & STARTTIMER) && ((op->j_ival1 && op->count) || op->j_ival2)) {
 539
 540             del_timer(&op->timer);
 541
 542             op->flags |= TX_ANNOUNCE; /* spec: send can_frame when starting timer */
 543             if (op->j_ival1 && (op->count > 0)){
 544                 op->timer.expires = jiffies + op->j_ival1;
 545                 /* op->count-- is done in bcm_tx_timeout_handler */
 546                 DBG("TX_SETUP: adding timer ival1. func=%p data=(%p) exp=0x%08X\n",
 547                     op->timer.function,
 548                     (char*) op->timer.data,
 549                     (unsigned int) op->timer.expires);
 550             }
 551             else{
 552                 op->timer.expires = jiffies + op->j_ival2;
 553                 DBG("TX_SETUP: adding timer ival2. func=%p data=(%p) exp=0x%08X\n",
 554                     op->timer.function,
 555                     (char*) op->timer.data,
 556                     (unsigned int) op->timer.expires);
 557             }
 558
 559             add_timer(&op->timer);
 560         }
 561
 562         if (op->flags & TX_ANNOUNCE)
 563             bcm_can_tx(op);
 564
 565         rbytes = msg_head.nframes * sizeof(struct can_frame) + sizeof(struct bcm_msg_head);
 566
 567         break; /* TX_SETUP */
 568
 569     case TX_DELETE:
 570
 571         bcm_delete_tx_op(&ud->tx_ops, msg_head.can_id);
 572
 573         rbytes = sizeof(struct bcm_msg_head);
 574
 575         break; /* TX_DELETE */
 576
 577     case TX_READ:
 578
 579         /* reuse msg_head for the reply */
 580         msg_head.opcode  = TX_STATUS; /* reply to TX_READ */
 581         op = bcm_find_op(ud->tx_ops, msg_head.can_id);
 582         c  = 'T'; /* for nice debug output ... */
 583
 584         goto TRX_READ;
 585
 586     case RX_READ:
 587
 588         /* reuse msg_head for the reply */
 589         msg_head.opcode  = RX_STATUS; /* reply to RX_READ */
 590         op = bcm_find_op(ud->rx_ops, msg_head.can_id);
 591         c  = 'R'; /* for nice debug output ... */
 592
 593     TRX_READ:
 594
 595         /* check the given can_id */
 596
 597         if (!op) {
 598             DBG("%cX_READ: did not find op for can_id <%03X>\n",
 599                 c, msg_head.can_id);
 600
 601             msg_head.flags   |= CMD_ERROR;
 602             msg_head.nframes  = 0;
 603             bcm_send_to_user(sk, &msg_head, NULL, NULL);
 604         }
 605         else {
 606             DBG("%cX_READ: sending status for can_id <%03X>\n",
 607                 c, msg_head.can_id);
 608
 609             /* put current values into msg_head */
 610             msg_head.flags   = op->flags;
 611             msg_head.count   = op->count;
 612             msg_head.ival1   = op->ival1;
 613             msg_head.ival2   = op->ival2;
 614             msg_head.nframes = op->nframes;
 615
 616             bcm_send_to_user(sk, &msg_head, op->frames, NULL);
 617         }
 618
 619         rbytes = sizeof(struct bcm_msg_head);
 620
 621         break; /* [T|R]X_READ */
 622
 623     case TX_SEND:
 624         {
 625             struct sk_buff *skb;
 626             struct net_device *dev;
 627
 628             /* just copy and send one can_frame */
 629
 630             if (msg_head.nframes < 1) /* we need at least one can_frame */
 631                 return -EINVAL;
 632
 633             skb = alloc_skb(sizeof(struct can_frame), GFP_KERNEL);
 634
 635             if (!skb)
 636                 break;
 637
 638             memcpy_fromiovec(skb_put(skb, sizeof(struct can_frame)), msg->msg_iov, sizeof(struct can_frame));
 639
 640             DBG_FRAME("BCM: TX_SEND: sending frame",
 641                       (struct can_frame *)skb->data);
 642             dev = dev_get_by_index(sk->bound_dev_if);
 643
 644             if (dev) {
 645                 skb->dev = dev;
 646                 can_send(skb);
 647                 dev_put(dev);
 648             }
 649
 650             rbytes = sizeof(struct can_frame) + sizeof(struct bcm_msg_head);
 651         }
 652         break;
 653
 654     case RX_SETUP:
 655
 656         if ((msg_head.flags & RX_FILTER_ID) || (!(msg_head.nframes))) {
 657             /* be robust against wrong usage ... */
 658             msg_head.flags |= RX_FILTER_ID;
 659             msg_head.nframes = 0; /* ignore trailing garbage */
 660         }
 661
 662         if ((msg_head.flags & RX_RTR_FRAME) &&
 663             ((msg_head.nframes != 1) || (!(msg_head.can_id & CAN_RTR_FLAG)))) {
 664
 665             DBG("RX_SETUP: bad RX_RTR_FRAME setup!\n");
 666
 667             msg_head.flags   |= CMD_ERROR; /* return msg_head back to sender */
 668             msg_head.nframes  = 0;
 669             bcm_send_to_user(sk, &msg_head, NULL, NULL);
 670
 671             rbytes = sizeof(struct bcm_msg_head);
 672
 673             break;
 674         }
 675
 676         /* check the given can_id */
 677
 678         if (!(op = bcm_find_op(ud->rx_ops, msg_head.can_id))) {
 679
 680             /* insert new BCM operation for the given can_id */
 681
 682             if (!(op = kmalloc(sizeof(struct bcm_op), GFP_KERNEL)))
 683                 return -ENOMEM;
 684
 685             memset(op, 0, sizeof(struct bcm_op)); /* init to zero, e.g. for timers */
 686
 687             DBG("RX_SETUP: creating new rx_op (%p) for can_id <%03X>\n",
 688                 op, msg_head.can_id);
 689
 690             op->can_id    = msg_head.can_id;
 691             op->nframes   = msg_head.nframes;
 692
 693             if (op->nframes) {
 694
 695                 /* create array for can_frames and copy the data */
 696                 if (!(op->frames = kmalloc(msg_head.nframes * sizeof(struct can_frame), GFP_KERNEL))) {
 697                     kfree(op);
 698                     return -ENOMEM;
 699                 }
 700
 701                 for (i = 0; i < msg_head.nframes; i++)
 702                     memcpy_fromiovec((unsigned char*)&op->frames[i], msg->msg_iov, sizeof(struct can_frame));
 703
 704                 /* create array for received can_frames */
 705                 if (!(op->last_frames = kmalloc(msg_head.nframes * sizeof(struct can_frame), GFP_KERNEL))) {
 706                     kfree(op->frames);
 707                     kfree(op);
 708                     return -ENOMEM;
 709                 }
 710
 711                 /* clear received can_frames to indicate 'nothing received' */
 712                 memset(op->last_frames, 0, msg_head.nframes * sizeof(struct can_frame));
 713             }
 714             else {
 715                 op->frames = NULL;
 716
 717                 /* even when we have the RX_FILTER_ID case, we need to store the last frame */
 718                 /* for the throttle functionality */
 719
 720                 /* create array for received can_frames */
 721                 if (!(op->last_frames = kmalloc(sizeof(struct can_frame), GFP_KERNEL)))
 722                     return -ENOMEM;
 723
 724                 /* clear received can_frames to indicate 'nothing received' */
 725                 memset(op->last_frames, 0, sizeof(struct can_frame));
 726             }
 727
 728             op->sk = sk; /* bcm_delete_rx_op() needs this */
 729
 730             init_timer(&op->timer); /* initialize uninitialized (kmalloc) structure */
 731             init_timer(&op->thrtimer); /* init throttle timer for RX_CHANGED */
 732
 733             op->timer.function = bcm_rx_timeout_handler; /* handler for rx timeouts */
 734             op->timer.data = (unsigned long)op; /* timer.data points to this op-structure */
 735
 736             op->thrtimer.function = bcm_rx_thr_handler; /* handler for RX_CHANGED throttle timeouts */
 737             op->thrtimer.data = (unsigned long)op; /* timer.data points to this op-structure */
 738             op->thrtimer.expires = 0; /* mark disabled timer */
 739
 740             /* add this bcm_op to the list of the tx_ops */
 741             bcm_insert_op(&ud->rx_ops, op);
 742
 743             c=1; /* call can_rx_register() at end of RX_SETUP */
 744
 745         }
 746         else {
 747             /* update existing BCM operation */
 748
 749             DBG("RX_SETUP: modifying existing rx_op (%p) for can_id <%03X>\n",
 750                 op, msg_head.can_id);
 751
 752             /* do we need more space for the can_frames? */
 753             if (msg_head.nframes > op->nframes) {
 754
 755                 /* yes => create new arrays */
 756
 757                 struct can_frame *p;
 758
 759                 if (!(p = kmalloc(msg_head.nframes * sizeof(struct can_frame), GFP_KERNEL)))
 760                     return -ENOMEM;
 761
 762                 if (op->frames)
 763                     kfree (op->frames);
 764                 op->frames = p;
 765
 766                 if (!(p = kmalloc(msg_head.nframes * sizeof(struct can_frame), GFP_KERNEL)))
 767                     return -ENOMEM;
 768                 if (op->last_frames)
 769                     kfree (op->last_frames);
 770                 op->last_frames = p;
 771             }
 772
 773             if (msg_head.nframes) {
 774                 /* update can_frames content */
 775                 for (i = 0; i < msg_head.nframes; i++)
 776                     memcpy_fromiovec((unsigned char*)&op->frames[i], msg->msg_iov, sizeof(struct can_frame));
 777
 778                 /* clear received can_frames to indicate 'nothing received' */
 779                 memset(op->last_frames, 0, msg_head.nframes * sizeof(struct can_frame));
 780             }
 781
 782             op->nframes = msg_head.nframes;
 783             c=0; /* do not call can_rx_register() at end of RX_SETUP */
 784
 785         } /* if (!bcm_find_op(ud->tx_ops, msg_head.can_id)) */
 786
 787
 788         /* check flags */
 789
 790         op->flags = msg_head.flags;
 791
 792         if (op->flags & RX_RTR_FRAME) {
 793
 794             /* no timers in RTR-mode */
 795             del_timer(&op->thrtimer);
 796             del_timer(&op->timer);
 797
 798             /* funny feature in RX(!)_SETUP only for RTR-mode: */
 799             /* copy can_id into frame BUT without RTR-flag to  */
 800             /* prevent a full-load-loopback-test ... ;-]       */
 801             if ((op->flags & TX_CP_CAN_ID) ||
 802                 (op->frames[0].can_id == op->can_id))
 803                 op->frames[0].can_id = op->can_id & ~CAN_RTR_FLAG;
 804
 805         }
 806         else {
 807             if (op->flags & SETTIMER) {
 808
 809                 /* set timer value */
 810
 811                 op->ival1   = msg_head.ival1;
 812                 op->j_ival1 = timeval2jiffies(&msg_head.ival1, 1);
 813                 op->ival2   = msg_head.ival2;
 814                 op->j_ival2 = timeval2jiffies(&msg_head.ival2, 1);
 815
 816                 DBG("RX_SETUP: SETTIMER j_ival1=%ld j_ival2=%ld\n",
 817                     op->j_ival1, op->j_ival2);
 818
 819                 /* disable an active timer due to zero value? */
 820                 if (!op->j_ival1) {
 821                     del_timer(&op->timer);
 822                     DBG("RX_SETUP: disabled timer for rx timeouts.\n");
 823                 }
 824
 825                 /* free currently blocked msgs ? */
 826                 if (op->thrtimer.expires) { /* running throttle timer? */
 827                     DBG("RX_SETUP: unblocking throttled msgs.\n");
 828                     del_timer(&op->thrtimer);
 829                     op->thrtimer.expires = jiffies + 2; /* send blocked msgs hereafter */
 830                     add_timer(&op->thrtimer);
 831                 }
 832                 /* if (op->j_ival2) is zero, no (new) throttling will happen */
 833                 /* see bcm_rx_update_and_send() and bcm_rx_thr_handler()     */
 834             }
 835
 836             if ((op->flags & STARTTIMER) && op->j_ival1) {
 837
 838                 del_timer(&op->timer);
 839
 840                 op->timer.expires = jiffies + op->j_ival1;
 841
 842                 DBG("RX_SETUP: adding timer ival1. func=%p data=(%p) exp=0x%08X\n",
 843                     (char *) op->timer.function,
 844                     (char *) op->timer.data,
 845                     (unsigned int) op->timer.expires);
 846
 847                 add_timer(&op->timer);
 848             }
 849         }
 850
 851         /* now we can register for can_ids, if we added a new bcm_op */
 852         if (c) {
 853             struct net_device *dev = dev_get_by_index(sk->bound_dev_if);
 854
 855             DBG("RX_SETUP: can_rx_register() for can_id <%03X>. rx_op is (%p)\n", op->can_id, op);
 856
 857             if (dev) {
 858                 can_rx_register(dev, op->can_id, 0xFFFFFFFFU, bcm_rx_handler, op, IDENT);
 859                 dev_put(dev);
 860             }
 861         }
 862
 863         rbytes = msg_head.nframes * sizeof(struct can_frame) + sizeof(struct bcm_msg_head);
 864
 865         break; /* RX_SETUP */
 866
 867     case RX_DELETE:
 868
 869         bcm_delete_rx_op(&ud->rx_ops, msg_head.can_id);
 870
 871         rbytes = sizeof(struct bcm_msg_head);
 872
 873         break; /* RX_DELETE */
 874
 875     default:
 876
 877         DBG("Unknown opcode %d\n", msg_head.opcode);
 878
 879         msg_head.flags   |= CMD_ERROR; /* return msg_head back to sender */
 880         msg_head.nframes  = 0;
 881         bcm_send_to_user(sk, &msg_head, NULL, NULL);
 882
 883         rbytes = sizeof(struct bcm_msg_head);
 884
 885         break;
 886     }
 887
 888     return rbytes;
 889 }
 890
 891 static int bcm_recvmsg(struct socket *sock, struct msghdr *msg, int size,
 892                        int flags, struct scm_cookie *scm)
 893 {
 894     struct sock *sk = sock->sk;
 895     struct sk_buff *skb;
 896     int error = 0;
 897     int noblock;
 898     int err;
 899
 900     DBG("socket %p, sk %p\n", sock, sk);
 901
 902     noblock =  flags & MSG_DONTWAIT;
 903     flags   &= ~MSG_DONTWAIT;
 904     if (!(skb = skb_recv_datagram(sk, flags, noblock, &error))) {
 905         return error;
 906     }
 907
 908     DBG("delivering skbuff %p\n", skb);
 909     DBG_SKB(skb);
 910
 911     if (skb->len < size)
 912         size = skb->len;
 913     if ((err = memcpy_toiovec(msg->msg_iov, skb->data, size)) < 0) {
 914         skb_free_datagram(sk, skb);
 915         return err;
 916     }
 917
 918     sock_recv_timestamp(msg, sk, skb);
 919
 920     DBG("freeing sock %p, skbuff %p\n", sk, skb);
 921     skb_free_datagram(sk, skb);
 922
 923     return size;
 924 }
 925
 926 static void bcm_tx_timeout_handler(unsigned long data)
 927 {
 928     struct bcm_op *op = (struct bcm_op*)data;
 929
 930     DBG("Called with bcm_op (%p)\n", op);
 931
 932     if (op->j_ival1 && (op->count > 0)) {
 933
 934         op->count--;
 935
 936         if (!op->count && (op->flags & TX_COUNTEVT)) { /* create notification to user? */
 937
 938             struct bcm_msg_head msg_head;
 939
 940             DBG("sending TX_EXPIRED for can_id <%03X>\n", op->can_id);
 941
 942             msg_head.opcode  = TX_EXPIRED;
 943             msg_head.flags   = op->flags;
 944             msg_head.count   = op->count;
 945             msg_head.ival1   = op->ival1;
 946             msg_head.ival2   = op->ival2;
 947             msg_head.can_id  = op->can_id;
 948             msg_head.nframes = 0;
 949
 950             bcm_send_to_user(op->sk, &msg_head, NULL, NULL);
 951         }
 952     }
 953
 954     DBG("count=%d j_ival1=%ld j_ival2=%ld\n",
 955         op->count, op->j_ival1, op->j_ival2);
 956
 957     if (op->j_ival1 && (op->count > 0)) {
 958
 959         op->timer.expires = jiffies + op->j_ival1;
 960         add_timer(&op->timer);
 961
 962         DBG("adding timer ival1. func=%p data=(%p) exp=0x%08X\n",
 963             op->timer.function,
 964             (char*) op->timer.data,
 965             (unsigned int) op->timer.expires);
 966
 967         bcm_can_tx(op); /* send (next) frame */
 968     }
 969     else {
 970         if (op->j_ival2) {
 971             op->timer.expires = jiffies + op->j_ival2;
 972             add_timer(&op->timer);
 973
 974         DBG("adding timer ival2. func=%p data=(%p) exp=0x%08X\n",
 975             op->timer.function,
 976             (char*) op->timer.data,
 977             (unsigned int) op->timer.expires);
 978
 979             bcm_can_tx(op); /* send (next) frame */
 980         }
 981         else
 982             DBG("no timer restart\n");
 983     }
 984
 985     return;
 986
 987 }
 988
 989 static void bcm_rx_handler(struct sk_buff *skb, void *data)
 990 {
 991     struct bcm_op *op = (struct bcm_op*)data;
 992     struct can_frame rxframe;
 993     int i;
 994
 995     del_timer(&op->timer); /* disable timeout */
 996
 997     DBG("Called with bcm_op (%p)\n", op);
 998
 999     if (skb->len == sizeof(rxframe)) {
1000         memcpy(&rxframe, skb->data, sizeof(rxframe));
1001         op->stamp = skb->stamp; /* save rx timestamp */
1002         op->frames_abs++; /* statistics */
1003         kfree_skb(skb);
1004         DBG("got can_frame with can_id <%03X>\n", rxframe.can_id);
1005     }
1006     else {
1007         DBG("Wrong skb->len = %d\n", skb->len);
1008         kfree_skb(skb);
1009         return;
1010     }
1011
1012     DBG_FRAME("BCM: bcm_rx_handler: CAN frame", &rxframe);
1013
1014     if (op->can_id != rxframe.can_id) {
1015         DBG("ERROR! Got wrong can_id <%03X>! Expected <%03X>.\n",
1016             rxframe.can_id, op->can_id);
1017         return;
1018     }
1019
1020     if (op->flags & RX_RTR_FRAME) { /* send reply for RTR-request */
1021         DBG("RTR-request\n");
1022         bcm_can_tx(op); /* send op->frames[0] to CAN device */
1023         return;
1024     }
1025
1026     if (op->flags & RX_FILTER_ID) { /* the easiest case */
1027         DBG("Easy does it with RX_FILTER_ID\n");
1028         bcm_rx_update_and_send(op, &op->last_frames[0], &rxframe);
1029         bcm_rx_starttimer(op);
1030         return;
1031     }
1032
1033     if (op->nframes == 1) { /* simple compare with index 0 */
1034         DBG("Simple compare\n");
1035         bcm_rx_cmp_to_index(op, 0, &rxframe);
1036         bcm_rx_starttimer(op);
1037         return;
1038     }
1039
1040     if (op->nframes > 1) { /* multiplex compare */
1041
1042         DBG("Multiplex compare\n");
1043         /* find the first multiplex mask that fits */
1044         /* MUX-mask is in index 0 */
1045
1046         for (i=1; i < op->nframes; i++) {
1047
1048             if ((GET_U64(&op->frames[0]) & GET_U64(&rxframe)) ==
1049                 (GET_U64(&op->frames[0]) & GET_U64(&op->frames[i]))) {
1050                 DBG("found MUX index %d\n", i);
1051                 bcm_rx_cmp_to_index(op, i, &rxframe);
1052                 break;
1053             }
1054         }
1055         bcm_rx_starttimer(op);
1056     }
1057 }
1058
1059 static void bcm_rx_cmp_to_index(struct bcm_op *op, int index,
1060                                 struct can_frame *rxdata)
1061 {
1062     /* no one uses the MSBs of can_dlc for comparation, */
1063     /* so we use it here to detect the first time of reception */
1064
1065     if (!(op->last_frames[index].can_dlc & RX_RECV)) { /* first time? */
1066         DBG("first time :)\n");
1067         bcm_rx_update_and_send(op, &op->last_frames[index], rxdata);
1068         return;
1069     }
1070
1071     /* do a real check in can_data */
1072
1073     DBG("op->frames[index].data = 0x%016llx\n", GET_U64(&op->frames[index]));
1074     DBG("op->last_frames[index].data = 0x%016llx\n",
1075         GET_U64(&op->last_frames[index]));
1076     DBG("rxdata->data = 0x%016llx\n", GET_U64(rxdata));
1077
1078     if ((GET_U64(&op->frames[index]) & GET_U64(rxdata)) !=
1079         (GET_U64(&op->frames[index]) & GET_U64(&op->last_frames[index]))) {
1080         DBG("relevant data change :)\n");
1081         bcm_rx_update_and_send(op, &op->last_frames[index], rxdata);
1082         return;
1083     }
1084
1085
1086     if (op->flags & RX_CHECK_DLC) {
1087
1088         /* do a real check in dlc */
1089
1090         if (rxdata->can_dlc != (op->last_frames[index].can_dlc & BCM_CAN_DLC_MASK)) {
1091             DBG("dlc change :)\n");
1092             bcm_rx_update_and_send(op, &op->last_frames[index], rxdata);
1093             return;
1094         }
1095     }
1096     DBG("no relevant change :(\n");
1097 }
1098
1099 static void bcm_rx_update_and_send(struct bcm_op *op,
1100                                    struct can_frame *lastdata,
1101                                    struct can_frame *rxdata)
1102 {
1103     unsigned long nexttx = op->j_lastmsg + op->j_ival2;
1104
1105     memcpy(lastdata, rxdata, sizeof(struct can_frame));
1106     lastdata->can_dlc |= RX_RECV; /* mark as used */
1107
1108     /* throttle bcm_rx_changed ? */
1109     if ((op->thrtimer.expires) || /* somebody else is already waiting OR */
1110         ((op->j_ival2) && (nexttx > jiffies))) {      /* we have to wait */
1111
1112         lastdata->can_dlc |= RX_THR; /* mark as 'throttled' */
1113
1114         if (!(op->thrtimer.expires)) { /* start only the first time */
1115             op->thrtimer.expires = nexttx;
1116             add_timer(&op->thrtimer);
1117
1118             DBG("adding thrtimer. func=%p data=(%p) exp=0x%08X\n",
1119                 op->thrtimer.function,
1120                 (char*) op->thrtimer.data,
1121                 (unsigned int) op->thrtimer.expires);
1122         }
1123     }
1124     else
1125         bcm_rx_changed(op, rxdata); /* send RX_CHANGED to the user */
1126 }
1127
1128 static void bcm_rx_starttimer(struct bcm_op *op)
1129 {
1130     if (op->flags & RX_NO_AUTOTIMER)
1131         return;
1132
1133     if (op->j_ival1) {
1134
1135         op->timer.expires = jiffies + op->j_ival1;
1136
1137         DBG("adding rx timeout timer ival1. func=%p data=(%p) exp=0x%08X\n",
1138             op->timer.function,
1139             (char*) op->timer.data,
1140             (unsigned int) op->timer.expires);
1141
1142         add_timer(&op->timer);
1143     }
1144 }
1145
1146
1147 static void bcm_rx_changed(struct bcm_op *op, struct can_frame *data)
1148 {
1149     struct bcm_msg_head head;
1150
1151     op->j_lastmsg = jiffies;
1152     op->frames_filtered++; /* statistics */
1153
1154     if (op->frames_filtered > ULONG_MAX/100)
1155         op->frames_filtered = op->frames_abs = 0; /* restart - spinlock ? */
1156
1157     DBG("setting j_lastmsg to 0x%08X for rx_op(%p)\n",
1158         (unsigned int) op->j_lastmsg, op);
1159     DBG("sending notification\n");
1160
1161     head.opcode  = RX_CHANGED;
1162     head.flags   = op->flags;
1163     head.count   = op->count;
1164     head.ival1   = op->ival1;
1165     head.ival2   = op->ival2;
1166     head.can_id  = op->can_id;
1167     head.nframes = 1;
1168
1169     bcm_send_to_user(op->sk, &head, data, &op->stamp);
1170 }
1171
1172
1173 static void bcm_rx_timeout_handler(unsigned long data)
1174 {
1175     struct bcm_op *op = (struct bcm_op*)data;
1176     struct bcm_msg_head msg_head;
1177
1178     DBG("sending RX_TIMEOUT for can_id <%03X>. op is (%p)\n", op->can_id, op);
1179
1180     msg_head.opcode  = RX_TIMEOUT;
1181     msg_head.flags   = op->flags;
1182     msg_head.count   = op->count;
1183     msg_head.ival1   = op->ival1;
1184     msg_head.ival2   = op->ival2;
1185     msg_head.can_id  = op->can_id;
1186     msg_head.nframes = 0;
1187
1188     bcm_send_to_user(op->sk, &msg_head, NULL, NULL);
1189
1190     /* no restart of the timer is done here! */
1191
1192     /* if the user wants to be informed, when cyclic CAN-Messages come back ... */
1193     if ((op->flags & RX_ANNOUNCE_RESUME) && op->last_frames) {
1194         /* clear received can_frames to indicate 'nothing received' */
1195         memset(op->last_frames, 0, op->nframes * sizeof(struct can_frame));
1196         DBG("RX_ANNOUNCE_RESTART\n");
1197     }
1198
1199 }
1200
1201 static void bcm_rx_thr_handler(unsigned long data)
1202 {
1203     struct bcm_op *op = (struct bcm_op*)data;
1204     int i = 0;
1205
1206     op->thrtimer.expires = 0; /* mark disabled / consumed timer */
1207
1208     if (op->nframes > 1){
1209
1210         DBG("sending MUX RX_CHANGED for can_id <%03X>. op is (%p)\n",
1211             op->can_id, op);
1212         /* for MUX filter we start at index 1 */
1213         for (i=1; i<op->nframes; i++){
1214             if ((op->last_frames) && (op->last_frames[i].can_dlc & RX_THR)){
1215                 op->last_frames[i].can_dlc &= ~RX_THR;
1216                 bcm_rx_changed(op, &op->last_frames[i]);
1217             }
1218         }
1219     }
1220     else{
1221
1222         DBG("sending simple RX_CHANGED for can_id <%03X>. op is (%p)\n",
1223             op->can_id, op);
1224         /* for RX_FILTER_ID and simple filter */
1225         if (op->last_frames && (op->last_frames[0].can_dlc & RX_THR)){
1226             op->last_frames[0].can_dlc &= ~RX_THR;
1227             bcm_rx_changed(op, &op->last_frames[0]);
1228         }
1229     }
1230 }
1231
1232 static void bcm_can_tx(struct bcm_op *op)
1233 {
1234     struct sk_buff *skb;
1235     struct net_device *dev;
1236     struct can_frame *cf = &op->frames[op->currframe];
1237
1238     DBG_FRAME("BCM: bcm_can_tx: sending frame", cf);
1239
1240     skb = alloc_skb(sizeof(struct can_frame), in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
1241
1242     if (!skb)
1243         return;
1244
1245     memcpy(skb_put(skb, sizeof(struct can_frame)), cf, sizeof(struct can_frame));
1246
1247     if (op->sk->bound_dev_if) {
1248         dev = dev_get_by_index(op->sk->bound_dev_if);
1249
1250         if (dev) {
1251             skb->dev = dev;
1252             can_send(skb);
1253             dev_put(dev);
1254         }
1255     }
1256
1257     op->currframe++;
1258     op->frames_abs++; /* statistics */
1259
1260     /* reached last frame? */
1261     if (op->currframe >= op->nframes)
1262         op->currframe = 0;
1263
1264 }
1265
1266 static void bcm_send_to_user(struct sock *sk, struct bcm_msg_head *head,
1267                              struct can_frame *frames, struct timeval *tv)
1268 {
1269     struct sk_buff *skb;
1270     struct can_frame *firstframe;
1271     int datalen = head->nframes * sizeof(struct can_frame);
1272     int err;
1273
1274     if (!sk) {
1275         DBG("no sk available\n");
1276         return;
1277     }
1278
1279     skb = alloc_skb(sizeof(*head) + datalen, in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
1280     memcpy(skb_put(skb, sizeof(*head)), head, sizeof(*head));
1281     firstframe = (struct can_frame *) skb->tail; /* can_frames starting here */
1282
1283     if (tv)
1284         skb->stamp = *tv;
1285
1286     if (head->nframes){
1287         memcpy(skb_put(skb, datalen), frames, datalen);
1288
1289         /* the BCM uses the can_dlc-element of the can_frame structure */
1290         /* for internal purposes. This is only relevant for updates that */
1291         /* are generated by the BCM, where nframes is 1                  */
1292         if (head->nframes == 1)
1293             firstframe->can_dlc &= BCM_CAN_DLC_MASK;
1294     }
1295     if ((err = sock_queue_rcv_skb(sk, skb)) < 0) {
1296         DBG("sock_queue_rcv_skb failed: %d\n", err);
1297         kfree_skb(skb);
1298     }
1299 }
1300
1301 static struct bcm_op *bcm_find_op(struct bcm_op *ops, canid_t can_id)
1302 {
1303     struct bcm_op *p;
1304
1305     for (p = ops; p; p = p->next)
1306         if (p->can_id == can_id)
1307             return p;
1308
1309     return NULL;
1310 }
1311
1312 static void bcm_delete_rx_op(struct bcm_op **ops, canid_t can_id)
1313 {
1314     struct bcm_op *p, **q;
1315
1316     for (q = ops; p = *q; q = &p->next)
1317         if (p->can_id == can_id) {
1318             *q = p->next;
1319             DBG("removing rx_op (%p) for can_id <%03X>\n", p, p->can_id);
1320
1321             if (p->sk->bound_dev_if) {
1322                 struct net_device *dev = dev_get_by_index(p->sk->bound_dev_if);
1323                 if (dev) {
1324                     can_rx_unregister(dev, p->can_id, 0xFFFFFFFFU, bcm_rx_handler, p);
1325                     dev_put(dev);
1326                 }
1327             } else
1328                 DBG("sock %p not bound for can_rx_unregister()\n", p->sk);
1329
1330             bcm_remove_op(p);
1331             return;
1332         }
1333 }
1334
1335 static void bcm_delete_tx_op(struct bcm_op **ops, canid_t can_id)
1336 {
1337     struct bcm_op *p, **q;
1338
1339     for (q = ops; p = *q; q = &p->next)
1340         if (p->can_id == can_id) {
1341             *q = p->next;
1342             DBG("removing rx_op (%p) for can_id <%03X>\n", p, p->can_id);
1343             bcm_remove_op(p);
1344             return;
1345         }
1346 }
1347
1348 static void bcm_remove_op(struct bcm_op *op)
1349 {
1350     del_timer(&op->timer);
1351     del_timer(&op->thrtimer);
1352     if (op->frames)
1353         kfree(op->frames);
1354     if (op->last_frames)
1355         kfree(op->last_frames);
1356     kfree(op);
1357
1358     return;
1359 }
1360
1361 static void bcm_insert_op(struct bcm_op **ops, struct bcm_op *op)
1362 {
1363     op->next = *ops;
1364     *ops = op;
1365 }
1366
1367 module_init(bcm_init);
1368 module_exit(bcm_exit);