Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
0002 /*
0003  * bcm.c - Broadcast Manager to filter/send (cyclic) CAN content
0004  *
0005  * Copyright (c) 2002-2017 Volkswagen Group Electronic Research
0006  * All rights reserved.
0007  *
0008  * Redistribution and use in source and binary forms, with or without
0009  * modification, are permitted provided that the following conditions
0010  * are met:
0011  * 1. Redistributions of source code must retain the above copyright
0012  *    notice, this list of conditions and the following disclaimer.
0013  * 2. Redistributions in binary form must reproduce the above copyright
0014  *    notice, this list of conditions and the following disclaimer in the
0015  *    documentation and/or other materials provided with the distribution.
0016  * 3. Neither the name of Volkswagen nor the names of its contributors
0017  *    may be used to endorse or promote products derived from this software
0018  *    without specific prior written permission.
0019  *
0020  * Alternatively, provided that this notice is retained in full, this
0021  * software may be distributed under the terms of the GNU General
0022  * Public License ("GPL") version 2, in which case the provisions of the
0023  * GPL apply INSTEAD OF those given above.
0024  *
0025  * The provided data structures and external interfaces from this code
0026  * are not restricted to be used by modules with a GPL compatible license.
0027  *
0028  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
0029  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
0030  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
0031  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
0032  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
0033  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
0034  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
0035  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
0036  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
0037  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
0038  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
0039  * DAMAGE.
0040  *
0041  */
0042 
0043 #include <linux/module.h>
0044 #include <linux/init.h>
0045 #include <linux/interrupt.h>
0046 #include <linux/hrtimer.h>
0047 #include <linux/list.h>
0048 #include <linux/proc_fs.h>
0049 #include <linux/seq_file.h>
0050 #include <linux/uio.h>
0051 #include <linux/net.h>
0052 #include <linux/netdevice.h>
0053 #include <linux/socket.h>
0054 #include <linux/if_arp.h>
0055 #include <linux/skbuff.h>
0056 #include <linux/can.h>
0057 #include <linux/can/core.h>
0058 #include <linux/can/skb.h>
0059 #include <linux/can/bcm.h>
0060 #include <linux/slab.h>
0061 #include <net/sock.h>
0062 #include <net/net_namespace.h>
0063 
0064 /*
0065  * To send multiple CAN frame content within TX_SETUP or to filter
0066  * CAN messages with multiplex index within RX_SETUP, the number of
0067  * different filters is limited to 256 due to the one byte index value.
0068  */
0069 #define MAX_NFRAMES 256
0070 
0071 /* limit timers to 400 days for sending/timeouts */
0072 #define BCM_TIMER_SEC_MAX (400 * 24 * 60 * 60)
0073 
0074 /* use of last_frames[index].flags */
0075 #define RX_RECV    0x40 /* received data for this element */
0076 #define RX_THR     0x80 /* element not been sent due to throttle feature */
0077 #define BCM_CAN_FLAGS_MASK 0x3F /* to clean private flags after usage */
0078 
0079 /* get best masking value for can_rx_register() for a given single can_id */
0080 #define REGMASK(id) ((id & CAN_EFF_FLAG) ? \
0081              (CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG) : \
0082              (CAN_SFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG))
0083 
0084 MODULE_DESCRIPTION("PF_CAN broadcast manager protocol");
0085 MODULE_LICENSE("Dual BSD/GPL");
0086 MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
0087 MODULE_ALIAS("can-proto-2");
0088 
0089 #define BCM_MIN_NAMELEN CAN_REQUIRED_SIZE(struct sockaddr_can, can_ifindex)
0090 
0091 /*
0092  * easy access to the first 64 bit of can(fd)_frame payload. cp->data is
0093  * 64 bit aligned so the offset has to be multiples of 8 which is ensured
0094  * by the only callers in bcm_rx_cmp_to_index() bcm_rx_handler().
0095  */
0096 static inline u64 get_u64(const struct canfd_frame *cp, int offset)
0097 {
0098     return *(u64 *)(cp->data + offset);
0099 }
0100 
0101 struct bcm_op {
0102     struct list_head list;
0103     struct rcu_head rcu;
0104     int ifindex;
0105     canid_t can_id;
0106     u32 flags;
0107     unsigned long frames_abs, frames_filtered;
0108     struct bcm_timeval ival1, ival2;
0109     struct hrtimer timer, thrtimer;
0110     ktime_t rx_stamp, kt_ival1, kt_ival2, kt_lastmsg;
0111     int rx_ifindex;
0112     int cfsiz;
0113     u32 count;
0114     u32 nframes;
0115     u32 currframe;
0116     /* void pointers to arrays of struct can[fd]_frame */
0117     void *frames;
0118     void *last_frames;
0119     struct canfd_frame sframe;
0120     struct canfd_frame last_sframe;
0121     struct sock *sk;
0122     struct net_device *rx_reg_dev;
0123 };
0124 
0125 struct bcm_sock {
0126     struct sock sk;
0127     int bound;
0128     int ifindex;
0129     struct list_head notifier;
0130     struct list_head rx_ops;
0131     struct list_head tx_ops;
0132     unsigned long dropped_usr_msgs;
0133     struct proc_dir_entry *bcm_proc_read;
0134     char procname [32]; /* inode number in decimal with \0 */
0135 };
0136 
0137 static LIST_HEAD(bcm_notifier_list);
0138 static DEFINE_SPINLOCK(bcm_notifier_lock);
0139 static struct bcm_sock *bcm_busy_notifier;
0140 
0141 static inline struct bcm_sock *bcm_sk(const struct sock *sk)
0142 {
0143     return (struct bcm_sock *)sk;
0144 }
0145 
0146 static inline ktime_t bcm_timeval_to_ktime(struct bcm_timeval tv)
0147 {
0148     return ktime_set(tv.tv_sec, tv.tv_usec * NSEC_PER_USEC);
0149 }
0150 
0151 /* check limitations for timeval provided by user */
0152 static bool bcm_is_invalid_tv(struct bcm_msg_head *msg_head)
0153 {
0154     if ((msg_head->ival1.tv_sec < 0) ||
0155         (msg_head->ival1.tv_sec > BCM_TIMER_SEC_MAX) ||
0156         (msg_head->ival1.tv_usec < 0) ||
0157         (msg_head->ival1.tv_usec >= USEC_PER_SEC) ||
0158         (msg_head->ival2.tv_sec < 0) ||
0159         (msg_head->ival2.tv_sec > BCM_TIMER_SEC_MAX) ||
0160         (msg_head->ival2.tv_usec < 0) ||
0161         (msg_head->ival2.tv_usec >= USEC_PER_SEC))
0162         return true;
0163 
0164     return false;
0165 }
0166 
0167 #define CFSIZ(flags) ((flags & CAN_FD_FRAME) ? CANFD_MTU : CAN_MTU)
0168 #define OPSIZ sizeof(struct bcm_op)
0169 #define MHSIZ sizeof(struct bcm_msg_head)
0170 
0171 /*
0172  * procfs functions
0173  */
0174 #if IS_ENABLED(CONFIG_PROC_FS)
0175 static char *bcm_proc_getifname(struct net *net, char *result, int ifindex)
0176 {
0177     struct net_device *dev;
0178 
0179     if (!ifindex)
0180         return "any";
0181 
0182     rcu_read_lock();
0183     dev = dev_get_by_index_rcu(net, ifindex);
0184     if (dev)
0185         strcpy(result, dev->name);
0186     else
0187         strcpy(result, "???");
0188     rcu_read_unlock();
0189 
0190     return result;
0191 }
0192 
0193 static int bcm_proc_show(struct seq_file *m, void *v)
0194 {
0195     char ifname[IFNAMSIZ];
0196     struct net *net = m->private;
0197     struct sock *sk = (struct sock *)pde_data(m->file->f_inode);
0198     struct bcm_sock *bo = bcm_sk(sk);
0199     struct bcm_op *op;
0200 
0201     seq_printf(m, ">>> socket %pK", sk->sk_socket);
0202     seq_printf(m, " / sk %pK", sk);
0203     seq_printf(m, " / bo %pK", bo);
0204     seq_printf(m, " / dropped %lu", bo->dropped_usr_msgs);
0205     seq_printf(m, " / bound %s", bcm_proc_getifname(net, ifname, bo->ifindex));
0206     seq_printf(m, " <<<\n");
0207 
0208     list_for_each_entry(op, &bo->rx_ops, list) {
0209 
0210         unsigned long reduction;
0211 
0212         /* print only active entries & prevent division by zero */
0213         if (!op->frames_abs)
0214             continue;
0215 
0216         seq_printf(m, "rx_op: %03X %-5s ", op->can_id,
0217                bcm_proc_getifname(net, ifname, op->ifindex));
0218 
0219         if (op->flags & CAN_FD_FRAME)
0220             seq_printf(m, "(%u)", op->nframes);
0221         else
0222             seq_printf(m, "[%u]", op->nframes);
0223 
0224         seq_printf(m, "%c ", (op->flags & RX_CHECK_DLC) ? 'd' : ' ');
0225 
0226         if (op->kt_ival1)
0227             seq_printf(m, "timeo=%lld ",
0228                    (long long)ktime_to_us(op->kt_ival1));
0229 
0230         if (op->kt_ival2)
0231             seq_printf(m, "thr=%lld ",
0232                    (long long)ktime_to_us(op->kt_ival2));
0233 
0234         seq_printf(m, "# recv %ld (%ld) => reduction: ",
0235                op->frames_filtered, op->frames_abs);
0236 
0237         reduction = 100 - (op->frames_filtered * 100) / op->frames_abs;
0238 
0239         seq_printf(m, "%s%ld%%\n",
0240                (reduction == 100) ? "near " : "", reduction);
0241     }
0242 
0243     list_for_each_entry(op, &bo->tx_ops, list) {
0244 
0245         seq_printf(m, "tx_op: %03X %s ", op->can_id,
0246                bcm_proc_getifname(net, ifname, op->ifindex));
0247 
0248         if (op->flags & CAN_FD_FRAME)
0249             seq_printf(m, "(%u) ", op->nframes);
0250         else
0251             seq_printf(m, "[%u] ", op->nframes);
0252 
0253         if (op->kt_ival1)
0254             seq_printf(m, "t1=%lld ",
0255                    (long long)ktime_to_us(op->kt_ival1));
0256 
0257         if (op->kt_ival2)
0258             seq_printf(m, "t2=%lld ",
0259                    (long long)ktime_to_us(op->kt_ival2));
0260 
0261         seq_printf(m, "# sent %ld\n", op->frames_abs);
0262     }
0263     seq_putc(m, '\n');
0264     return 0;
0265 }
0266 #endif /* CONFIG_PROC_FS */
0267 
0268 /*
0269  * bcm_can_tx - send the (next) CAN frame to the appropriate CAN interface
0270  *              of the given bcm tx op
0271  */
0272 static void bcm_can_tx(struct bcm_op *op)
0273 {
0274     struct sk_buff *skb;
0275     struct net_device *dev;
0276     struct canfd_frame *cf = op->frames + op->cfsiz * op->currframe;
0277 
0278     /* no target device? => exit */
0279     if (!op->ifindex)
0280         return;
0281 
0282     dev = dev_get_by_index(sock_net(op->sk), op->ifindex);
0283     if (!dev) {
0284         /* RFC: should this bcm_op remove itself here? */
0285         return;
0286     }
0287 
0288     skb = alloc_skb(op->cfsiz + sizeof(struct can_skb_priv), gfp_any());
0289     if (!skb)
0290         goto out;
0291 
0292     can_skb_reserve(skb);
0293     can_skb_prv(skb)->ifindex = dev->ifindex;
0294     can_skb_prv(skb)->skbcnt = 0;
0295 
0296     skb_put_data(skb, cf, op->cfsiz);
0297 
0298     /* send with loopback */
0299     skb->dev = dev;
0300     can_skb_set_owner(skb, op->sk);
0301     can_send(skb, 1);
0302 
0303     /* update statistics */
0304     op->currframe++;
0305     op->frames_abs++;
0306 
0307     /* reached last frame? */
0308     if (op->currframe >= op->nframes)
0309         op->currframe = 0;
0310 out:
0311     dev_put(dev);
0312 }
0313 
0314 /*
0315  * bcm_send_to_user - send a BCM message to the userspace
0316  *                    (consisting of bcm_msg_head + x CAN frames)
0317  */
0318 static void bcm_send_to_user(struct bcm_op *op, struct bcm_msg_head *head,
0319                  struct canfd_frame *frames, int has_timestamp)
0320 {
0321     struct sk_buff *skb;
0322     struct canfd_frame *firstframe;
0323     struct sockaddr_can *addr;
0324     struct sock *sk = op->sk;
0325     unsigned int datalen = head->nframes * op->cfsiz;
0326     int err;
0327 
0328     skb = alloc_skb(sizeof(*head) + datalen, gfp_any());
0329     if (!skb)
0330         return;
0331 
0332     skb_put_data(skb, head, sizeof(*head));
0333 
0334     if (head->nframes) {
0335         /* CAN frames starting here */
0336         firstframe = (struct canfd_frame *)skb_tail_pointer(skb);
0337 
0338         skb_put_data(skb, frames, datalen);
0339 
0340         /*
0341          * the BCM uses the flags-element of the canfd_frame
0342          * structure for internal purposes. This is only
0343          * relevant for updates that are generated by the
0344          * BCM, where nframes is 1
0345          */
0346         if (head->nframes == 1)
0347             firstframe->flags &= BCM_CAN_FLAGS_MASK;
0348     }
0349 
0350     if (has_timestamp) {
0351         /* restore rx timestamp */
0352         skb->tstamp = op->rx_stamp;
0353     }
0354 
0355     /*
0356      *  Put the datagram to the queue so that bcm_recvmsg() can
0357      *  get it from there.  We need to pass the interface index to
0358      *  bcm_recvmsg().  We pass a whole struct sockaddr_can in skb->cb
0359      *  containing the interface index.
0360      */
0361 
0362     sock_skb_cb_check_size(sizeof(struct sockaddr_can));
0363     addr = (struct sockaddr_can *)skb->cb;
0364     memset(addr, 0, sizeof(*addr));
0365     addr->can_family  = AF_CAN;
0366     addr->can_ifindex = op->rx_ifindex;
0367 
0368     err = sock_queue_rcv_skb(sk, skb);
0369     if (err < 0) {
0370         struct bcm_sock *bo = bcm_sk(sk);
0371 
0372         kfree_skb(skb);
0373         /* don't care about overflows in this statistic */
0374         bo->dropped_usr_msgs++;
0375     }
0376 }
0377 
0378 static bool bcm_tx_set_expiry(struct bcm_op *op, struct hrtimer *hrt)
0379 {
0380     ktime_t ival;
0381 
0382     if (op->kt_ival1 && op->count)
0383         ival = op->kt_ival1;
0384     else if (op->kt_ival2)
0385         ival = op->kt_ival2;
0386     else
0387         return false;
0388 
0389     hrtimer_set_expires(hrt, ktime_add(ktime_get(), ival));
0390     return true;
0391 }
0392 
0393 static void bcm_tx_start_timer(struct bcm_op *op)
0394 {
0395     if (bcm_tx_set_expiry(op, &op->timer))
0396         hrtimer_start_expires(&op->timer, HRTIMER_MODE_ABS_SOFT);
0397 }
0398 
0399 /* bcm_tx_timeout_handler - performs cyclic CAN frame transmissions */
0400 static enum hrtimer_restart bcm_tx_timeout_handler(struct hrtimer *hrtimer)
0401 {
0402     struct bcm_op *op = container_of(hrtimer, struct bcm_op, timer);
0403     struct bcm_msg_head msg_head;
0404 
0405     if (op->kt_ival1 && (op->count > 0)) {
0406         op->count--;
0407         if (!op->count && (op->flags & TX_COUNTEVT)) {
0408 
0409             /* create notification to user */
0410             memset(&msg_head, 0, sizeof(msg_head));
0411             msg_head.opcode  = TX_EXPIRED;
0412             msg_head.flags   = op->flags;
0413             msg_head.count   = op->count;
0414             msg_head.ival1   = op->ival1;
0415             msg_head.ival2   = op->ival2;
0416             msg_head.can_id  = op->can_id;
0417             msg_head.nframes = 0;
0418 
0419             bcm_send_to_user(op, &msg_head, NULL, 0);
0420         }
0421         bcm_can_tx(op);
0422 
0423     } else if (op->kt_ival2) {
0424         bcm_can_tx(op);
0425     }
0426 
0427     return bcm_tx_set_expiry(op, &op->timer) ?
0428         HRTIMER_RESTART : HRTIMER_NORESTART;
0429 }
0430 
0431 /*
0432  * bcm_rx_changed - create a RX_CHANGED notification due to changed content
0433  */
0434 static void bcm_rx_changed(struct bcm_op *op, struct canfd_frame *data)
0435 {
0436     struct bcm_msg_head head;
0437 
0438     /* update statistics */
0439     op->frames_filtered++;
0440 
0441     /* prevent statistics overflow */
0442     if (op->frames_filtered > ULONG_MAX/100)
0443         op->frames_filtered = op->frames_abs = 0;
0444 
0445     /* this element is not throttled anymore */
0446     data->flags &= (BCM_CAN_FLAGS_MASK|RX_RECV);
0447 
0448     memset(&head, 0, sizeof(head));
0449     head.opcode  = RX_CHANGED;
0450     head.flags   = op->flags;
0451     head.count   = op->count;
0452     head.ival1   = op->ival1;
0453     head.ival2   = op->ival2;
0454     head.can_id  = op->can_id;
0455     head.nframes = 1;
0456 
0457     bcm_send_to_user(op, &head, data, 1);
0458 }
0459 
0460 /*
0461  * bcm_rx_update_and_send - process a detected relevant receive content change
0462  *                          1. update the last received data
0463  *                          2. send a notification to the user (if possible)
0464  */
0465 static void bcm_rx_update_and_send(struct bcm_op *op,
0466                    struct canfd_frame *lastdata,
0467                    const struct canfd_frame *rxdata)
0468 {
0469     memcpy(lastdata, rxdata, op->cfsiz);
0470 
0471     /* mark as used and throttled by default */
0472     lastdata->flags |= (RX_RECV|RX_THR);
0473 
0474     /* throttling mode inactive ? */
0475     if (!op->kt_ival2) {
0476         /* send RX_CHANGED to the user immediately */
0477         bcm_rx_changed(op, lastdata);
0478         return;
0479     }
0480 
0481     /* with active throttling timer we are just done here */
0482     if (hrtimer_active(&op->thrtimer))
0483         return;
0484 
0485     /* first reception with enabled throttling mode */
0486     if (!op->kt_lastmsg)
0487         goto rx_changed_settime;
0488 
0489     /* got a second frame inside a potential throttle period? */
0490     if (ktime_us_delta(ktime_get(), op->kt_lastmsg) <
0491         ktime_to_us(op->kt_ival2)) {
0492         /* do not send the saved data - only start throttle timer */
0493         hrtimer_start(&op->thrtimer,
0494                   ktime_add(op->kt_lastmsg, op->kt_ival2),
0495                   HRTIMER_MODE_ABS_SOFT);
0496         return;
0497     }
0498 
0499     /* the gap was that big, that throttling was not needed here */
0500 rx_changed_settime:
0501     bcm_rx_changed(op, lastdata);
0502     op->kt_lastmsg = ktime_get();
0503 }
0504 
0505 /*
0506  * bcm_rx_cmp_to_index - (bit)compares the currently received data to formerly
0507  *                       received data stored in op->last_frames[]
0508  */
0509 static void bcm_rx_cmp_to_index(struct bcm_op *op, unsigned int index,
0510                 const struct canfd_frame *rxdata)
0511 {
0512     struct canfd_frame *cf = op->frames + op->cfsiz * index;
0513     struct canfd_frame *lcf = op->last_frames + op->cfsiz * index;
0514     int i;
0515 
0516     /*
0517      * no one uses the MSBs of flags for comparison,
0518      * so we use it here to detect the first time of reception
0519      */
0520 
0521     if (!(lcf->flags & RX_RECV)) {
0522         /* received data for the first time => send update to user */
0523         bcm_rx_update_and_send(op, lcf, rxdata);
0524         return;
0525     }
0526 
0527     /* do a real check in CAN frame data section */
0528     for (i = 0; i < rxdata->len; i += 8) {
0529         if ((get_u64(cf, i) & get_u64(rxdata, i)) !=
0530             (get_u64(cf, i) & get_u64(lcf, i))) {
0531             bcm_rx_update_and_send(op, lcf, rxdata);
0532             return;
0533         }
0534     }
0535 
0536     if (op->flags & RX_CHECK_DLC) {
0537         /* do a real check in CAN frame length */
0538         if (rxdata->len != lcf->len) {
0539             bcm_rx_update_and_send(op, lcf, rxdata);
0540             return;
0541         }
0542     }
0543 }
0544 
0545 /*
0546  * bcm_rx_starttimer - enable timeout monitoring for CAN frame reception
0547  */
0548 static void bcm_rx_starttimer(struct bcm_op *op)
0549 {
0550     if (op->flags & RX_NO_AUTOTIMER)
0551         return;
0552 
0553     if (op->kt_ival1)
0554         hrtimer_start(&op->timer, op->kt_ival1, HRTIMER_MODE_REL_SOFT);
0555 }
0556 
0557 /* bcm_rx_timeout_handler - when the (cyclic) CAN frame reception timed out */
0558 static enum hrtimer_restart bcm_rx_timeout_handler(struct hrtimer *hrtimer)
0559 {
0560     struct bcm_op *op = container_of(hrtimer, struct bcm_op, timer);
0561     struct bcm_msg_head msg_head;
0562 
0563     /* if user wants to be informed, when cyclic CAN-Messages come back */
0564     if ((op->flags & RX_ANNOUNCE_RESUME) && op->last_frames) {
0565         /* clear received CAN frames to indicate 'nothing received' */
0566         memset(op->last_frames, 0, op->nframes * op->cfsiz);
0567     }
0568 
0569     /* create notification to user */
0570     memset(&msg_head, 0, sizeof(msg_head));
0571     msg_head.opcode  = RX_TIMEOUT;
0572     msg_head.flags   = op->flags;
0573     msg_head.count   = op->count;
0574     msg_head.ival1   = op->ival1;
0575     msg_head.ival2   = op->ival2;
0576     msg_head.can_id  = op->can_id;
0577     msg_head.nframes = 0;
0578 
0579     bcm_send_to_user(op, &msg_head, NULL, 0);
0580 
0581     return HRTIMER_NORESTART;
0582 }
0583 
0584 /*
0585  * bcm_rx_do_flush - helper for bcm_rx_thr_flush
0586  */
0587 static inline int bcm_rx_do_flush(struct bcm_op *op, unsigned int index)
0588 {
0589     struct canfd_frame *lcf = op->last_frames + op->cfsiz * index;
0590 
0591     if ((op->last_frames) && (lcf->flags & RX_THR)) {
0592         bcm_rx_changed(op, lcf);
0593         return 1;
0594     }
0595     return 0;
0596 }
0597 
0598 /*
0599  * bcm_rx_thr_flush - Check for throttled data and send it to the userspace
0600  */
0601 static int bcm_rx_thr_flush(struct bcm_op *op)
0602 {
0603     int updated = 0;
0604 
0605     if (op->nframes > 1) {
0606         unsigned int i;
0607 
0608         /* for MUX filter we start at index 1 */
0609         for (i = 1; i < op->nframes; i++)
0610             updated += bcm_rx_do_flush(op, i);
0611 
0612     } else {
0613         /* for RX_FILTER_ID and simple filter */
0614         updated += bcm_rx_do_flush(op, 0);
0615     }
0616 
0617     return updated;
0618 }
0619 
0620 /*
0621  * bcm_rx_thr_handler - the time for blocked content updates is over now:
0622  *                      Check for throttled data and send it to the userspace
0623  */
0624 static enum hrtimer_restart bcm_rx_thr_handler(struct hrtimer *hrtimer)
0625 {
0626     struct bcm_op *op = container_of(hrtimer, struct bcm_op, thrtimer);
0627 
0628     if (bcm_rx_thr_flush(op)) {
0629         hrtimer_forward_now(hrtimer, op->kt_ival2);
0630         return HRTIMER_RESTART;
0631     } else {
0632         /* rearm throttle handling */
0633         op->kt_lastmsg = 0;
0634         return HRTIMER_NORESTART;
0635     }
0636 }
0637 
0638 /*
0639  * bcm_rx_handler - handle a CAN frame reception
0640  */
0641 static void bcm_rx_handler(struct sk_buff *skb, void *data)
0642 {
0643     struct bcm_op *op = (struct bcm_op *)data;
0644     const struct canfd_frame *rxframe = (struct canfd_frame *)skb->data;
0645     unsigned int i;
0646 
0647     if (op->can_id != rxframe->can_id)
0648         return;
0649 
0650     /* make sure to handle the correct frame type (CAN / CAN FD) */
0651     if (skb->len != op->cfsiz)
0652         return;
0653 
0654     /* disable timeout */
0655     hrtimer_cancel(&op->timer);
0656 
0657     /* save rx timestamp */
0658     op->rx_stamp = skb->tstamp;
0659     /* save originator for recvfrom() */
0660     op->rx_ifindex = skb->dev->ifindex;
0661     /* update statistics */
0662     op->frames_abs++;
0663 
0664     if (op->flags & RX_RTR_FRAME) {
0665         /* send reply for RTR-request (placed in op->frames[0]) */
0666         bcm_can_tx(op);
0667         return;
0668     }
0669 
0670     if (op->flags & RX_FILTER_ID) {
0671         /* the easiest case */
0672         bcm_rx_update_and_send(op, op->last_frames, rxframe);
0673         goto rx_starttimer;
0674     }
0675 
0676     if (op->nframes == 1) {
0677         /* simple compare with index 0 */
0678         bcm_rx_cmp_to_index(op, 0, rxframe);
0679         goto rx_starttimer;
0680     }
0681 
0682     if (op->nframes > 1) {
0683         /*
0684          * multiplex compare
0685          *
0686          * find the first multiplex mask that fits.
0687          * Remark: The MUX-mask is stored in index 0 - but only the
0688          * first 64 bits of the frame data[] are relevant (CAN FD)
0689          */
0690 
0691         for (i = 1; i < op->nframes; i++) {
0692             if ((get_u64(op->frames, 0) & get_u64(rxframe, 0)) ==
0693                 (get_u64(op->frames, 0) &
0694                  get_u64(op->frames + op->cfsiz * i, 0))) {
0695                 bcm_rx_cmp_to_index(op, i, rxframe);
0696                 break;
0697             }
0698         }
0699     }
0700 
0701 rx_starttimer:
0702     bcm_rx_starttimer(op);
0703 }
0704 
0705 /*
0706  * helpers for bcm_op handling: find & delete bcm [rx|tx] op elements
0707  */
0708 static struct bcm_op *bcm_find_op(struct list_head *ops,
0709                   struct bcm_msg_head *mh, int ifindex)
0710 {
0711     struct bcm_op *op;
0712 
0713     list_for_each_entry(op, ops, list) {
0714         if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) &&
0715             (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME))
0716             return op;
0717     }
0718 
0719     return NULL;
0720 }
0721 
0722 static void bcm_free_op_rcu(struct rcu_head *rcu_head)
0723 {
0724     struct bcm_op *op = container_of(rcu_head, struct bcm_op, rcu);
0725 
0726     if ((op->frames) && (op->frames != &op->sframe))
0727         kfree(op->frames);
0728 
0729     if ((op->last_frames) && (op->last_frames != &op->last_sframe))
0730         kfree(op->last_frames);
0731 
0732     kfree(op);
0733 }
0734 
0735 static void bcm_remove_op(struct bcm_op *op)
0736 {
0737     hrtimer_cancel(&op->timer);
0738     hrtimer_cancel(&op->thrtimer);
0739 
0740     call_rcu(&op->rcu, bcm_free_op_rcu);
0741 }
0742 
0743 static void bcm_rx_unreg(struct net_device *dev, struct bcm_op *op)
0744 {
0745     if (op->rx_reg_dev == dev) {
0746         can_rx_unregister(dev_net(dev), dev, op->can_id,
0747                   REGMASK(op->can_id), bcm_rx_handler, op);
0748 
0749         /* mark as removed subscription */
0750         op->rx_reg_dev = NULL;
0751     } else
0752         printk(KERN_ERR "can-bcm: bcm_rx_unreg: registered device "
0753                "mismatch %p %p\n", op->rx_reg_dev, dev);
0754 }
0755 
0756 /*
0757  * bcm_delete_rx_op - find and remove a rx op (returns number of removed ops)
0758  */
0759 static int bcm_delete_rx_op(struct list_head *ops, struct bcm_msg_head *mh,
0760                 int ifindex)
0761 {
0762     struct bcm_op *op, *n;
0763 
0764     list_for_each_entry_safe(op, n, ops, list) {
0765         if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) &&
0766             (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME)) {
0767 
0768             /* disable automatic timer on frame reception */
0769             op->flags |= RX_NO_AUTOTIMER;
0770 
0771             /*
0772              * Don't care if we're bound or not (due to netdev
0773              * problems) can_rx_unregister() is always a save
0774              * thing to do here.
0775              */
0776             if (op->ifindex) {
0777                 /*
0778                  * Only remove subscriptions that had not
0779                  * been removed due to NETDEV_UNREGISTER
0780                  * in bcm_notifier()
0781                  */
0782                 if (op->rx_reg_dev) {
0783                     struct net_device *dev;
0784 
0785                     dev = dev_get_by_index(sock_net(op->sk),
0786                                    op->ifindex);
0787                     if (dev) {
0788                         bcm_rx_unreg(dev, op);
0789                         dev_put(dev);
0790                     }
0791                 }
0792             } else
0793                 can_rx_unregister(sock_net(op->sk), NULL,
0794                           op->can_id,
0795                           REGMASK(op->can_id),
0796                           bcm_rx_handler, op);
0797 
0798             list_del(&op->list);
0799             bcm_remove_op(op);
0800             return 1; /* done */
0801         }
0802     }
0803 
0804     return 0; /* not found */
0805 }
0806 
0807 /*
0808  * bcm_delete_tx_op - find and remove a tx op (returns number of removed ops)
0809  */
0810 static int bcm_delete_tx_op(struct list_head *ops, struct bcm_msg_head *mh,
0811                 int ifindex)
0812 {
0813     struct bcm_op *op, *n;
0814 
0815     list_for_each_entry_safe(op, n, ops, list) {
0816         if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) &&
0817             (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME)) {
0818             list_del(&op->list);
0819             bcm_remove_op(op);
0820             return 1; /* done */
0821         }
0822     }
0823 
0824     return 0; /* not found */
0825 }
0826 
0827 /*
0828  * bcm_read_op - read out a bcm_op and send it to the user (for bcm_sendmsg)
0829  */
0830 static int bcm_read_op(struct list_head *ops, struct bcm_msg_head *msg_head,
0831                int ifindex)
0832 {
0833     struct bcm_op *op = bcm_find_op(ops, msg_head, ifindex);
0834 
0835     if (!op)
0836         return -EINVAL;
0837 
0838     /* put current values into msg_head */
0839     msg_head->flags   = op->flags;
0840     msg_head->count   = op->count;
0841     msg_head->ival1   = op->ival1;
0842     msg_head->ival2   = op->ival2;
0843     msg_head->nframes = op->nframes;
0844 
0845     bcm_send_to_user(op, msg_head, op->frames, 0);
0846 
0847     return MHSIZ;
0848 }
0849 
0850 /*
0851  * bcm_tx_setup - create or update a bcm tx op (for bcm_sendmsg)
0852  */
0853 static int bcm_tx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg,
0854             int ifindex, struct sock *sk)
0855 {
0856     struct bcm_sock *bo = bcm_sk(sk);
0857     struct bcm_op *op;
0858     struct canfd_frame *cf;
0859     unsigned int i;
0860     int err;
0861 
0862     /* we need a real device to send frames */
0863     if (!ifindex)
0864         return -ENODEV;
0865 
0866     /* check nframes boundaries - we need at least one CAN frame */
0867     if (msg_head->nframes < 1 || msg_head->nframes > MAX_NFRAMES)
0868         return -EINVAL;
0869 
0870     /* check timeval limitations */
0871     if ((msg_head->flags & SETTIMER) && bcm_is_invalid_tv(msg_head))
0872         return -EINVAL;
0873 
0874     /* check the given can_id */
0875     op = bcm_find_op(&bo->tx_ops, msg_head, ifindex);
0876     if (op) {
0877         /* update existing BCM operation */
0878 
0879         /*
0880          * Do we need more space for the CAN frames than currently
0881          * allocated? -> This is a _really_ unusual use-case and
0882          * therefore (complexity / locking) it is not supported.
0883          */
0884         if (msg_head->nframes > op->nframes)
0885             return -E2BIG;
0886 
0887         /* update CAN frames content */
0888         for (i = 0; i < msg_head->nframes; i++) {
0889 
0890             cf = op->frames + op->cfsiz * i;
0891             err = memcpy_from_msg((u8 *)cf, msg, op->cfsiz);
0892 
0893             if (op->flags & CAN_FD_FRAME) {
0894                 if (cf->len > 64)
0895                     err = -EINVAL;
0896             } else {
0897                 if (cf->len > 8)
0898                     err = -EINVAL;
0899             }
0900 
0901             if (err < 0)
0902                 return err;
0903 
0904             if (msg_head->flags & TX_CP_CAN_ID) {
0905                 /* copy can_id into frame */
0906                 cf->can_id = msg_head->can_id;
0907             }
0908         }
0909         op->flags = msg_head->flags;
0910 
0911     } else {
0912         /* insert new BCM operation for the given can_id */
0913 
0914         op = kzalloc(OPSIZ, GFP_KERNEL);
0915         if (!op)
0916             return -ENOMEM;
0917 
0918         op->can_id = msg_head->can_id;
0919         op->cfsiz = CFSIZ(msg_head->flags);
0920         op->flags = msg_head->flags;
0921 
0922         /* create array for CAN frames and copy the data */
0923         if (msg_head->nframes > 1) {
0924             op->frames = kmalloc_array(msg_head->nframes,
0925                            op->cfsiz,
0926                            GFP_KERNEL);
0927             if (!op->frames) {
0928                 kfree(op);
0929                 return -ENOMEM;
0930             }
0931         } else
0932             op->frames = &op->sframe;
0933 
0934         for (i = 0; i < msg_head->nframes; i++) {
0935 
0936             cf = op->frames + op->cfsiz * i;
0937             err = memcpy_from_msg((u8 *)cf, msg, op->cfsiz);
0938 
0939             if (op->flags & CAN_FD_FRAME) {
0940                 if (cf->len > 64)
0941                     err = -EINVAL;
0942             } else {
0943                 if (cf->len > 8)
0944                     err = -EINVAL;
0945             }
0946 
0947             if (err < 0) {
0948                 if (op->frames != &op->sframe)
0949                     kfree(op->frames);
0950                 kfree(op);
0951                 return err;
0952             }
0953 
0954             if (msg_head->flags & TX_CP_CAN_ID) {
0955                 /* copy can_id into frame */
0956                 cf->can_id = msg_head->can_id;
0957             }
0958         }
0959 
0960         /* tx_ops never compare with previous received messages */
0961         op->last_frames = NULL;
0962 
0963         /* bcm_can_tx / bcm_tx_timeout_handler needs this */
0964         op->sk = sk;
0965         op->ifindex = ifindex;
0966 
0967         /* initialize uninitialized (kzalloc) structure */
0968         hrtimer_init(&op->timer, CLOCK_MONOTONIC,
0969                  HRTIMER_MODE_REL_SOFT);
0970         op->timer.function = bcm_tx_timeout_handler;
0971 
0972         /* currently unused in tx_ops */
0973         hrtimer_init(&op->thrtimer, CLOCK_MONOTONIC,
0974                  HRTIMER_MODE_REL_SOFT);
0975 
0976         /* add this bcm_op to the list of the tx_ops */
0977         list_add(&op->list, &bo->tx_ops);
0978 
0979     } /* if ((op = bcm_find_op(&bo->tx_ops, msg_head->can_id, ifindex))) */
0980 
0981     if (op->nframes != msg_head->nframes) {
0982         op->nframes   = msg_head->nframes;
0983         /* start multiple frame transmission with index 0 */
0984         op->currframe = 0;
0985     }
0986 
0987     /* check flags */
0988 
0989     if (op->flags & TX_RESET_MULTI_IDX) {
0990         /* start multiple frame transmission with index 0 */
0991         op->currframe = 0;
0992     }
0993 
0994     if (op->flags & SETTIMER) {
0995         /* set timer values */
0996         op->count = msg_head->count;
0997         op->ival1 = msg_head->ival1;
0998         op->ival2 = msg_head->ival2;
0999         op->kt_ival1 = bcm_timeval_to_ktime(msg_head->ival1);
1000         op->kt_ival2 = bcm_timeval_to_ktime(msg_head->ival2);
1001 
1002         /* disable an active timer due to zero values? */
1003         if (!op->kt_ival1 && !op->kt_ival2)
1004             hrtimer_cancel(&op->timer);
1005     }
1006 
1007     if (op->flags & STARTTIMER) {
1008         hrtimer_cancel(&op->timer);
1009         /* spec: send CAN frame when starting timer */
1010         op->flags |= TX_ANNOUNCE;
1011     }
1012 
1013     if (op->flags & TX_ANNOUNCE) {
1014         bcm_can_tx(op);
1015         if (op->count)
1016             op->count--;
1017     }
1018 
1019     if (op->flags & STARTTIMER)
1020         bcm_tx_start_timer(op);
1021 
1022     return msg_head->nframes * op->cfsiz + MHSIZ;
1023 }
1024 
1025 /*
1026  * bcm_rx_setup - create or update a bcm rx op (for bcm_sendmsg)
1027  */
1028 static int bcm_rx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg,
1029             int ifindex, struct sock *sk)
1030 {
1031     struct bcm_sock *bo = bcm_sk(sk);
1032     struct bcm_op *op;
1033     int do_rx_register;
1034     int err = 0;
1035 
1036     if ((msg_head->flags & RX_FILTER_ID) || (!(msg_head->nframes))) {
1037         /* be robust against wrong usage ... */
1038         msg_head->flags |= RX_FILTER_ID;
1039         /* ignore trailing garbage */
1040         msg_head->nframes = 0;
1041     }
1042 
1043     /* the first element contains the mux-mask => MAX_NFRAMES + 1  */
1044     if (msg_head->nframes > MAX_NFRAMES + 1)
1045         return -EINVAL;
1046 
1047     if ((msg_head->flags & RX_RTR_FRAME) &&
1048         ((msg_head->nframes != 1) ||
1049          (!(msg_head->can_id & CAN_RTR_FLAG))))
1050         return -EINVAL;
1051 
1052     /* check timeval limitations */
1053     if ((msg_head->flags & SETTIMER) && bcm_is_invalid_tv(msg_head))
1054         return -EINVAL;
1055 
1056     /* check the given can_id */
1057     op = bcm_find_op(&bo->rx_ops, msg_head, ifindex);
1058     if (op) {
1059         /* update existing BCM operation */
1060 
1061         /*
1062          * Do we need more space for the CAN frames than currently
1063          * allocated? -> This is a _really_ unusual use-case and
1064          * therefore (complexity / locking) it is not supported.
1065          */
1066         if (msg_head->nframes > op->nframes)
1067             return -E2BIG;
1068 
1069         if (msg_head->nframes) {
1070             /* update CAN frames content */
1071             err = memcpy_from_msg(op->frames, msg,
1072                           msg_head->nframes * op->cfsiz);
1073             if (err < 0)
1074                 return err;
1075 
1076             /* clear last_frames to indicate 'nothing received' */
1077             memset(op->last_frames, 0, msg_head->nframes * op->cfsiz);
1078         }
1079 
1080         op->nframes = msg_head->nframes;
1081         op->flags = msg_head->flags;
1082 
1083         /* Only an update -> do not call can_rx_register() */
1084         do_rx_register = 0;
1085 
1086     } else {
1087         /* insert new BCM operation for the given can_id */
1088         op = kzalloc(OPSIZ, GFP_KERNEL);
1089         if (!op)
1090             return -ENOMEM;
1091 
1092         op->can_id = msg_head->can_id;
1093         op->nframes = msg_head->nframes;
1094         op->cfsiz = CFSIZ(msg_head->flags);
1095         op->flags = msg_head->flags;
1096 
1097         if (msg_head->nframes > 1) {
1098             /* create array for CAN frames and copy the data */
1099             op->frames = kmalloc_array(msg_head->nframes,
1100                            op->cfsiz,
1101                            GFP_KERNEL);
1102             if (!op->frames) {
1103                 kfree(op);
1104                 return -ENOMEM;
1105             }
1106 
1107             /* create and init array for received CAN frames */
1108             op->last_frames = kcalloc(msg_head->nframes,
1109                           op->cfsiz,
1110                           GFP_KERNEL);
1111             if (!op->last_frames) {
1112                 kfree(op->frames);
1113                 kfree(op);
1114                 return -ENOMEM;
1115             }
1116 
1117         } else {
1118             op->frames = &op->sframe;
1119             op->last_frames = &op->last_sframe;
1120         }
1121 
1122         if (msg_head->nframes) {
1123             err = memcpy_from_msg(op->frames, msg,
1124                           msg_head->nframes * op->cfsiz);
1125             if (err < 0) {
1126                 if (op->frames != &op->sframe)
1127                     kfree(op->frames);
1128                 if (op->last_frames != &op->last_sframe)
1129                     kfree(op->last_frames);
1130                 kfree(op);
1131                 return err;
1132             }
1133         }
1134 
1135         /* bcm_can_tx / bcm_tx_timeout_handler needs this */
1136         op->sk = sk;
1137         op->ifindex = ifindex;
1138 
1139         /* ifindex for timeout events w/o previous frame reception */
1140         op->rx_ifindex = ifindex;
1141 
1142         /* initialize uninitialized (kzalloc) structure */
1143         hrtimer_init(&op->timer, CLOCK_MONOTONIC,
1144                  HRTIMER_MODE_REL_SOFT);
1145         op->timer.function = bcm_rx_timeout_handler;
1146 
1147         hrtimer_init(&op->thrtimer, CLOCK_MONOTONIC,
1148                  HRTIMER_MODE_REL_SOFT);
1149         op->thrtimer.function = bcm_rx_thr_handler;
1150 
1151         /* add this bcm_op to the list of the rx_ops */
1152         list_add(&op->list, &bo->rx_ops);
1153 
1154         /* call can_rx_register() */
1155         do_rx_register = 1;
1156 
1157     } /* if ((op = bcm_find_op(&bo->rx_ops, msg_head->can_id, ifindex))) */
1158 
1159     /* check flags */
1160 
1161     if (op->flags & RX_RTR_FRAME) {
1162         struct canfd_frame *frame0 = op->frames;
1163 
1164         /* no timers in RTR-mode */
1165         hrtimer_cancel(&op->thrtimer);
1166         hrtimer_cancel(&op->timer);
1167 
1168         /*
1169          * funny feature in RX(!)_SETUP only for RTR-mode:
1170          * copy can_id into frame BUT without RTR-flag to
1171          * prevent a full-load-loopback-test ... ;-]
1172          */
1173         if ((op->flags & TX_CP_CAN_ID) ||
1174             (frame0->can_id == op->can_id))
1175             frame0->can_id = op->can_id & ~CAN_RTR_FLAG;
1176 
1177     } else {
1178         if (op->flags & SETTIMER) {
1179 
1180             /* set timer value */
1181             op->ival1 = msg_head->ival1;
1182             op->ival2 = msg_head->ival2;
1183             op->kt_ival1 = bcm_timeval_to_ktime(msg_head->ival1);
1184             op->kt_ival2 = bcm_timeval_to_ktime(msg_head->ival2);
1185 
1186             /* disable an active timer due to zero value? */
1187             if (!op->kt_ival1)
1188                 hrtimer_cancel(&op->timer);
1189 
1190             /*
1191              * In any case cancel the throttle timer, flush
1192              * potentially blocked msgs and reset throttle handling
1193              */
1194             op->kt_lastmsg = 0;
1195             hrtimer_cancel(&op->thrtimer);
1196             bcm_rx_thr_flush(op);
1197         }
1198 
1199         if ((op->flags & STARTTIMER) && op->kt_ival1)
1200             hrtimer_start(&op->timer, op->kt_ival1,
1201                       HRTIMER_MODE_REL_SOFT);
1202     }
1203 
1204     /* now we can register for can_ids, if we added a new bcm_op */
1205     if (do_rx_register) {
1206         if (ifindex) {
1207             struct net_device *dev;
1208 
1209             dev = dev_get_by_index(sock_net(sk), ifindex);
1210             if (dev) {
1211                 err = can_rx_register(sock_net(sk), dev,
1212                               op->can_id,
1213                               REGMASK(op->can_id),
1214                               bcm_rx_handler, op,
1215                               "bcm", sk);
1216 
1217                 op->rx_reg_dev = dev;
1218                 dev_put(dev);
1219             }
1220 
1221         } else
1222             err = can_rx_register(sock_net(sk), NULL, op->can_id,
1223                           REGMASK(op->can_id),
1224                           bcm_rx_handler, op, "bcm", sk);
1225         if (err) {
1226             /* this bcm rx op is broken -> remove it */
1227             list_del(&op->list);
1228             bcm_remove_op(op);
1229             return err;
1230         }
1231     }
1232 
1233     return msg_head->nframes * op->cfsiz + MHSIZ;
1234 }
1235 
1236 /*
1237  * bcm_tx_send - send a single CAN frame to the CAN interface (for bcm_sendmsg)
1238  */
1239 static int bcm_tx_send(struct msghdr *msg, int ifindex, struct sock *sk,
1240                int cfsiz)
1241 {
1242     struct sk_buff *skb;
1243     struct net_device *dev;
1244     int err;
1245 
1246     /* we need a real device to send frames */
1247     if (!ifindex)
1248         return -ENODEV;
1249 
1250     skb = alloc_skb(cfsiz + sizeof(struct can_skb_priv), GFP_KERNEL);
1251     if (!skb)
1252         return -ENOMEM;
1253 
1254     can_skb_reserve(skb);
1255 
1256     err = memcpy_from_msg(skb_put(skb, cfsiz), msg, cfsiz);
1257     if (err < 0) {
1258         kfree_skb(skb);
1259         return err;
1260     }
1261 
1262     dev = dev_get_by_index(sock_net(sk), ifindex);
1263     if (!dev) {
1264         kfree_skb(skb);
1265         return -ENODEV;
1266     }
1267 
1268     can_skb_prv(skb)->ifindex = dev->ifindex;
1269     can_skb_prv(skb)->skbcnt = 0;
1270     skb->dev = dev;
1271     can_skb_set_owner(skb, sk);
1272     err = can_send(skb, 1); /* send with loopback */
1273     dev_put(dev);
1274 
1275     if (err)
1276         return err;
1277 
1278     return cfsiz + MHSIZ;
1279 }
1280 
1281 /*
1282  * bcm_sendmsg - process BCM commands (opcodes) from the userspace
1283  */
1284 static int bcm_sendmsg(struct socket *sock, struct msghdr *msg, size_t size)
1285 {
1286     struct sock *sk = sock->sk;
1287     struct bcm_sock *bo = bcm_sk(sk);
1288     int ifindex = bo->ifindex; /* default ifindex for this bcm_op */
1289     struct bcm_msg_head msg_head;
1290     int cfsiz;
1291     int ret; /* read bytes or error codes as return value */
1292 
1293     if (!bo->bound)
1294         return -ENOTCONN;
1295 
1296     /* check for valid message length from userspace */
1297     if (size < MHSIZ)
1298         return -EINVAL;
1299 
1300     /* read message head information */
1301     ret = memcpy_from_msg((u8 *)&msg_head, msg, MHSIZ);
1302     if (ret < 0)
1303         return ret;
1304 
1305     cfsiz = CFSIZ(msg_head.flags);
1306     if ((size - MHSIZ) % cfsiz)
1307         return -EINVAL;
1308 
1309     /* check for alternative ifindex for this bcm_op */
1310 
1311     if (!ifindex && msg->msg_name) {
1312         /* no bound device as default => check msg_name */
1313         DECLARE_SOCKADDR(struct sockaddr_can *, addr, msg->msg_name);
1314 
1315         if (msg->msg_namelen < BCM_MIN_NAMELEN)
1316             return -EINVAL;
1317 
1318         if (addr->can_family != AF_CAN)
1319             return -EINVAL;
1320 
1321         /* ifindex from sendto() */
1322         ifindex = addr->can_ifindex;
1323 
1324         if (ifindex) {
1325             struct net_device *dev;
1326 
1327             dev = dev_get_by_index(sock_net(sk), ifindex);
1328             if (!dev)
1329                 return -ENODEV;
1330 
1331             if (dev->type != ARPHRD_CAN) {
1332                 dev_put(dev);
1333                 return -ENODEV;
1334             }
1335 
1336             dev_put(dev);
1337         }
1338     }
1339 
1340     lock_sock(sk);
1341 
1342     switch (msg_head.opcode) {
1343 
1344     case TX_SETUP:
1345         ret = bcm_tx_setup(&msg_head, msg, ifindex, sk);
1346         break;
1347 
1348     case RX_SETUP:
1349         ret = bcm_rx_setup(&msg_head, msg, ifindex, sk);
1350         break;
1351 
1352     case TX_DELETE:
1353         if (bcm_delete_tx_op(&bo->tx_ops, &msg_head, ifindex))
1354             ret = MHSIZ;
1355         else
1356             ret = -EINVAL;
1357         break;
1358 
1359     case RX_DELETE:
1360         if (bcm_delete_rx_op(&bo->rx_ops, &msg_head, ifindex))
1361             ret = MHSIZ;
1362         else
1363             ret = -EINVAL;
1364         break;
1365 
1366     case TX_READ:
1367         /* reuse msg_head for the reply to TX_READ */
1368         msg_head.opcode  = TX_STATUS;
1369         ret = bcm_read_op(&bo->tx_ops, &msg_head, ifindex);
1370         break;
1371 
1372     case RX_READ:
1373         /* reuse msg_head for the reply to RX_READ */
1374         msg_head.opcode  = RX_STATUS;
1375         ret = bcm_read_op(&bo->rx_ops, &msg_head, ifindex);
1376         break;
1377 
1378     case TX_SEND:
1379         /* we need exactly one CAN frame behind the msg head */
1380         if ((msg_head.nframes != 1) || (size != cfsiz + MHSIZ))
1381             ret = -EINVAL;
1382         else
1383             ret = bcm_tx_send(msg, ifindex, sk, cfsiz);
1384         break;
1385 
1386     default:
1387         ret = -EINVAL;
1388         break;
1389     }
1390 
1391     release_sock(sk);
1392 
1393     return ret;
1394 }
1395 
1396 /*
1397  * notification handler for netdevice status changes
1398  */
1399 static void bcm_notify(struct bcm_sock *bo, unsigned long msg,
1400                struct net_device *dev)
1401 {
1402     struct sock *sk = &bo->sk;
1403     struct bcm_op *op;
1404     int notify_enodev = 0;
1405 
1406     if (!net_eq(dev_net(dev), sock_net(sk)))
1407         return;
1408 
1409     switch (msg) {
1410 
1411     case NETDEV_UNREGISTER:
1412         lock_sock(sk);
1413 
1414         /* remove device specific receive entries */
1415         list_for_each_entry(op, &bo->rx_ops, list)
1416             if (op->rx_reg_dev == dev)
1417                 bcm_rx_unreg(dev, op);
1418 
1419         /* remove device reference, if this is our bound device */
1420         if (bo->bound && bo->ifindex == dev->ifindex) {
1421             bo->bound   = 0;
1422             bo->ifindex = 0;
1423             notify_enodev = 1;
1424         }
1425 
1426         release_sock(sk);
1427 
1428         if (notify_enodev) {
1429             sk->sk_err = ENODEV;
1430             if (!sock_flag(sk, SOCK_DEAD))
1431                 sk_error_report(sk);
1432         }
1433         break;
1434 
1435     case NETDEV_DOWN:
1436         if (bo->bound && bo->ifindex == dev->ifindex) {
1437             sk->sk_err = ENETDOWN;
1438             if (!sock_flag(sk, SOCK_DEAD))
1439                 sk_error_report(sk);
1440         }
1441     }
1442 }
1443 
1444 static int bcm_notifier(struct notifier_block *nb, unsigned long msg,
1445             void *ptr)
1446 {
1447     struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1448 
1449     if (dev->type != ARPHRD_CAN)
1450         return NOTIFY_DONE;
1451     if (msg != NETDEV_UNREGISTER && msg != NETDEV_DOWN)
1452         return NOTIFY_DONE;
1453     if (unlikely(bcm_busy_notifier)) /* Check for reentrant bug. */
1454         return NOTIFY_DONE;
1455 
1456     spin_lock(&bcm_notifier_lock);
1457     list_for_each_entry(bcm_busy_notifier, &bcm_notifier_list, notifier) {
1458         spin_unlock(&bcm_notifier_lock);
1459         bcm_notify(bcm_busy_notifier, msg, dev);
1460         spin_lock(&bcm_notifier_lock);
1461     }
1462     bcm_busy_notifier = NULL;
1463     spin_unlock(&bcm_notifier_lock);
1464     return NOTIFY_DONE;
1465 }
1466 
1467 /*
1468  * initial settings for all BCM sockets to be set at socket creation time
1469  */
1470 static int bcm_init(struct sock *sk)
1471 {
1472     struct bcm_sock *bo = bcm_sk(sk);
1473 
1474     bo->bound            = 0;
1475     bo->ifindex          = 0;
1476     bo->dropped_usr_msgs = 0;
1477     bo->bcm_proc_read    = NULL;
1478 
1479     INIT_LIST_HEAD(&bo->tx_ops);
1480     INIT_LIST_HEAD(&bo->rx_ops);
1481 
1482     /* set notifier */
1483     spin_lock(&bcm_notifier_lock);
1484     list_add_tail(&bo->notifier, &bcm_notifier_list);
1485     spin_unlock(&bcm_notifier_lock);
1486 
1487     return 0;
1488 }
1489 
1490 /*
1491  * standard socket functions
1492  */
1493 static int bcm_release(struct socket *sock)
1494 {
1495     struct sock *sk = sock->sk;
1496     struct net *net;
1497     struct bcm_sock *bo;
1498     struct bcm_op *op, *next;
1499 
1500     if (!sk)
1501         return 0;
1502 
1503     net = sock_net(sk);
1504     bo = bcm_sk(sk);
1505 
1506     /* remove bcm_ops, timer, rx_unregister(), etc. */
1507 
1508     spin_lock(&bcm_notifier_lock);
1509     while (bcm_busy_notifier == bo) {
1510         spin_unlock(&bcm_notifier_lock);
1511         schedule_timeout_uninterruptible(1);
1512         spin_lock(&bcm_notifier_lock);
1513     }
1514     list_del(&bo->notifier);
1515     spin_unlock(&bcm_notifier_lock);
1516 
1517     lock_sock(sk);
1518 
1519     list_for_each_entry_safe(op, next, &bo->tx_ops, list)
1520         bcm_remove_op(op);
1521 
1522     list_for_each_entry_safe(op, next, &bo->rx_ops, list) {
1523         /*
1524          * Don't care if we're bound or not (due to netdev problems)
1525          * can_rx_unregister() is always a save thing to do here.
1526          */
1527         if (op->ifindex) {
1528             /*
1529              * Only remove subscriptions that had not
1530              * been removed due to NETDEV_UNREGISTER
1531              * in bcm_notifier()
1532              */
1533             if (op->rx_reg_dev) {
1534                 struct net_device *dev;
1535 
1536                 dev = dev_get_by_index(net, op->ifindex);
1537                 if (dev) {
1538                     bcm_rx_unreg(dev, op);
1539                     dev_put(dev);
1540                 }
1541             }
1542         } else
1543             can_rx_unregister(net, NULL, op->can_id,
1544                       REGMASK(op->can_id),
1545                       bcm_rx_handler, op);
1546 
1547     }
1548 
1549     synchronize_rcu();
1550 
1551     list_for_each_entry_safe(op, next, &bo->rx_ops, list)
1552         bcm_remove_op(op);
1553 
1554 #if IS_ENABLED(CONFIG_PROC_FS)
1555     /* remove procfs entry */
1556     if (net->can.bcmproc_dir && bo->bcm_proc_read)
1557         remove_proc_entry(bo->procname, net->can.bcmproc_dir);
1558 #endif /* CONFIG_PROC_FS */
1559 
1560     /* remove device reference */
1561     if (bo->bound) {
1562         bo->bound   = 0;
1563         bo->ifindex = 0;
1564     }
1565 
1566     sock_orphan(sk);
1567     sock->sk = NULL;
1568 
1569     release_sock(sk);
1570     sock_put(sk);
1571 
1572     return 0;
1573 }
1574 
1575 static int bcm_connect(struct socket *sock, struct sockaddr *uaddr, int len,
1576                int flags)
1577 {
1578     struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
1579     struct sock *sk = sock->sk;
1580     struct bcm_sock *bo = bcm_sk(sk);
1581     struct net *net = sock_net(sk);
1582     int ret = 0;
1583 
1584     if (len < BCM_MIN_NAMELEN)
1585         return -EINVAL;
1586 
1587     lock_sock(sk);
1588 
1589     if (bo->bound) {
1590         ret = -EISCONN;
1591         goto fail;
1592     }
1593 
1594     /* bind a device to this socket */
1595     if (addr->can_ifindex) {
1596         struct net_device *dev;
1597 
1598         dev = dev_get_by_index(net, addr->can_ifindex);
1599         if (!dev) {
1600             ret = -ENODEV;
1601             goto fail;
1602         }
1603         if (dev->type != ARPHRD_CAN) {
1604             dev_put(dev);
1605             ret = -ENODEV;
1606             goto fail;
1607         }
1608 
1609         bo->ifindex = dev->ifindex;
1610         dev_put(dev);
1611 
1612     } else {
1613         /* no interface reference for ifindex = 0 ('any' CAN device) */
1614         bo->ifindex = 0;
1615     }
1616 
1617 #if IS_ENABLED(CONFIG_PROC_FS)
1618     if (net->can.bcmproc_dir) {
1619         /* unique socket address as filename */
1620         sprintf(bo->procname, "%lu", sock_i_ino(sk));
1621         bo->bcm_proc_read = proc_create_net_single(bo->procname, 0644,
1622                              net->can.bcmproc_dir,
1623                              bcm_proc_show, sk);
1624         if (!bo->bcm_proc_read) {
1625             ret = -ENOMEM;
1626             goto fail;
1627         }
1628     }
1629 #endif /* CONFIG_PROC_FS */
1630 
1631     bo->bound = 1;
1632 
1633 fail:
1634     release_sock(sk);
1635 
1636     return ret;
1637 }
1638 
1639 static int bcm_recvmsg(struct socket *sock, struct msghdr *msg, size_t size,
1640                int flags)
1641 {
1642     struct sock *sk = sock->sk;
1643     struct sk_buff *skb;
1644     int error = 0;
1645     int err;
1646 
1647     skb = skb_recv_datagram(sk, flags, &error);
1648     if (!skb)
1649         return error;
1650 
1651     if (skb->len < size)
1652         size = skb->len;
1653 
1654     err = memcpy_to_msg(msg, skb->data, size);
1655     if (err < 0) {
1656         skb_free_datagram(sk, skb);
1657         return err;
1658     }
1659 
1660     sock_recv_cmsgs(msg, sk, skb);
1661 
1662     if (msg->msg_name) {
1663         __sockaddr_check_size(BCM_MIN_NAMELEN);
1664         msg->msg_namelen = BCM_MIN_NAMELEN;
1665         memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
1666     }
1667 
1668     skb_free_datagram(sk, skb);
1669 
1670     return size;
1671 }
1672 
1673 static int bcm_sock_no_ioctlcmd(struct socket *sock, unsigned int cmd,
1674                 unsigned long arg)
1675 {
1676     /* no ioctls for socket layer -> hand it down to NIC layer */
1677     return -ENOIOCTLCMD;
1678 }
1679 
1680 static const struct proto_ops bcm_ops = {
1681     .family        = PF_CAN,
1682     .release       = bcm_release,
1683     .bind          = sock_no_bind,
1684     .connect       = bcm_connect,
1685     .socketpair    = sock_no_socketpair,
1686     .accept        = sock_no_accept,
1687     .getname       = sock_no_getname,
1688     .poll          = datagram_poll,
1689     .ioctl         = bcm_sock_no_ioctlcmd,
1690     .gettstamp     = sock_gettstamp,
1691     .listen        = sock_no_listen,
1692     .shutdown      = sock_no_shutdown,
1693     .sendmsg       = bcm_sendmsg,
1694     .recvmsg       = bcm_recvmsg,
1695     .mmap          = sock_no_mmap,
1696     .sendpage      = sock_no_sendpage,
1697 };
1698 
1699 static struct proto bcm_proto __read_mostly = {
1700     .name       = "CAN_BCM",
1701     .owner      = THIS_MODULE,
1702     .obj_size   = sizeof(struct bcm_sock),
1703     .init       = bcm_init,
1704 };
1705 
1706 static const struct can_proto bcm_can_proto = {
1707     .type       = SOCK_DGRAM,
1708     .protocol   = CAN_BCM,
1709     .ops        = &bcm_ops,
1710     .prot       = &bcm_proto,
1711 };
1712 
1713 static int canbcm_pernet_init(struct net *net)
1714 {
1715 #if IS_ENABLED(CONFIG_PROC_FS)
1716     /* create /proc/net/can-bcm directory */
1717     net->can.bcmproc_dir = proc_net_mkdir(net, "can-bcm", net->proc_net);
1718 #endif /* CONFIG_PROC_FS */
1719 
1720     return 0;
1721 }
1722 
1723 static void canbcm_pernet_exit(struct net *net)
1724 {
1725 #if IS_ENABLED(CONFIG_PROC_FS)
1726     /* remove /proc/net/can-bcm directory */
1727     if (net->can.bcmproc_dir)
1728         remove_proc_entry("can-bcm", net->proc_net);
1729 #endif /* CONFIG_PROC_FS */
1730 }
1731 
1732 static struct pernet_operations canbcm_pernet_ops __read_mostly = {
1733     .init = canbcm_pernet_init,
1734     .exit = canbcm_pernet_exit,
1735 };
1736 
1737 static struct notifier_block canbcm_notifier = {
1738     .notifier_call = bcm_notifier
1739 };
1740 
1741 static int __init bcm_module_init(void)
1742 {
1743     int err;
1744 
1745     pr_info("can: broadcast manager protocol\n");
1746 
1747     err = can_proto_register(&bcm_can_proto);
1748     if (err < 0) {
1749         printk(KERN_ERR "can: registration of bcm protocol failed\n");
1750         return err;
1751     }
1752 
1753     register_pernet_subsys(&canbcm_pernet_ops);
1754     register_netdevice_notifier(&canbcm_notifier);
1755     return 0;
1756 }
1757 
1758 static void __exit bcm_module_exit(void)
1759 {
1760     can_proto_unregister(&bcm_can_proto);
1761     unregister_netdevice_notifier(&canbcm_notifier);
1762     unregister_pernet_subsys(&canbcm_pernet_ops);
1763 }
1764 
1765 module_init(bcm_module_init);
1766 module_exit(bcm_module_exit);
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!