bcm.c

   1// SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
   2/*
   3 * bcm.c - Broadcast Manager to filter/send (cyclic) CAN content
   4 *
   5 * Copyright (c) 2002-2017 Volkswagen Group Electronic Research
   6 * All rights reserved.
   7 *
   8 * Redistribution and use in source and binary forms, with or without
   9 * modification, are permitted provided that the following conditions
  10 * are met:
  11 * 1. Redistributions of source code must retain the above copyright
  12 *    notice, this list of conditions and the following disclaimer.
  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, in which case the provisions of the
  23 * GPL apply INSTEAD OF those given above.
  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 */
  42
  43#include <linux/module.h>
  44#include <linux/init.h>
  45#include <linux/interrupt.h>
  46#include <linux/hrtimer.h>
  47#include <linux/list.h>
  48#include <linux/proc_fs.h>
  49#include <linux/seq_file.h>
  50#include <linux/uio.h>
  51#include <linux/net.h>
  52#include <linux/netdevice.h>
  53#include <linux/socket.h>
  54#include <linux/if_arp.h>
  55#include <linux/skbuff.h>
  56#include <linux/can.h>
  57#include <linux/can/core.h>
  58#include <linux/can/skb.h>
  59#include <linux/can/bcm.h>
  60#include <linux/slab.h>
  61#include <linux/spinlock.h>
  62#include <net/sock.h>
  63#include <net/net_namespace.h>
  64
  65/*
  66 * To send multiple CAN frame content within TX_SETUP or to filter
  67 * CAN messages with multiplex index within RX_SETUP, the number of
  68 * different filters is limited to 256 due to the one byte index value.
  69 */
  70#define MAX_NFRAMES 256
  71
  72/* limit timers to 400 days for sending/timeouts */
  73#define BCM_TIMER_SEC_MAX (400 * 24 * 60 * 60)
  74
  75/* use of last_frames[index].flags */
  76#define RX_LOCAL   0x10 /* frame was created on the local host */
  77#define RX_OWN     0x20 /* frame was sent via the socket it was received on */
  78#define RX_RECV    0x40 /* received data for this element */
  79#define RX_THR     0x80 /* element not been sent due to throttle feature */
  80#define BCM_CAN_FLAGS_MASK 0x0F /* to clean private flags after usage */
  81
  82/* get best masking value for can_rx_register() for a given single can_id */
  83#define REGMASK(id) ((id & CAN_EFF_FLAG) ? \
  84		     (CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG) : \
  85		     (CAN_SFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG))
  86
  87MODULE_DESCRIPTION("PF_CAN broadcast manager protocol");
  88MODULE_LICENSE("Dual BSD/GPL");
  89MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
  90MODULE_ALIAS("can-proto-2");
  91
  92#define BCM_MIN_NAMELEN CAN_REQUIRED_SIZE(struct sockaddr_can, can_ifindex)
  93
  94/*
  95 * easy access to the first 64 bit of can(fd)_frame payload. cp->data is
  96 * 64 bit aligned so the offset has to be multiples of 8 which is ensured
  97 * by the only callers in bcm_rx_cmp_to_index() bcm_rx_handler().
  98 */
  99static inline u64 get_u64(const struct canfd_frame *cp, int offset)
 100{
 101	return *(u64 *)(cp->data + offset);
 102}
 103
 104struct bcm_op {
 105	struct list_head list;
 106	struct rcu_head rcu;
 107	int ifindex;
 108	canid_t can_id;
 109	u32 flags;
 110	unsigned long frames_abs, frames_filtered;
 111	struct bcm_timeval ival1, ival2;
 112	struct hrtimer timer, thrtimer;
 113	ktime_t rx_stamp, kt_ival1, kt_ival2, kt_lastmsg;
 114	int rx_ifindex;
 115	int cfsiz;
 116	u32 count;
 117	u32 nframes;
 118	u32 currframe;
 119	/* void pointers to arrays of struct can[fd]_frame */
 120	void *frames;
 121	void *last_frames;
 122	struct canfd_frame sframe;
 123	struct canfd_frame last_sframe;
 124	struct sock *sk;
 125	struct net_device *rx_reg_dev;
 126	spinlock_t bcm_tx_lock; /* protect currframe/count in runtime updates */
 127};
 128
 129struct bcm_sock {
 130	struct sock sk;
 131	int bound;
 132	int ifindex;
 133	struct list_head notifier;
 134	struct list_head rx_ops;
 135	struct list_head tx_ops;
 136	unsigned long dropped_usr_msgs;
 137	struct proc_dir_entry *bcm_proc_read;
 138	char procname [32]; /* inode number in decimal with \0 */
 139};
 140
 141static LIST_HEAD(bcm_notifier_list);
 142static DEFINE_SPINLOCK(bcm_notifier_lock);
 143static struct bcm_sock *bcm_busy_notifier;
 144
 145/* Return pointer to store the extra msg flags for bcm_recvmsg().
 146 * We use the space of one unsigned int beyond the 'struct sockaddr_can'
 147 * in skb->cb.
 148 */
 149static inline unsigned int *bcm_flags(struct sk_buff *skb)
 150{
 151	/* return pointer after struct sockaddr_can */
 152	return (unsigned int *)(&((struct sockaddr_can *)skb->cb)[1]);
 153}
 154
 155static inline struct bcm_sock *bcm_sk(const struct sock *sk)
 156{
 157	return (struct bcm_sock *)sk;
 158}
 159
 160static inline ktime_t bcm_timeval_to_ktime(struct bcm_timeval tv)
 161{
 162	return ktime_set(tv.tv_sec, tv.tv_usec * NSEC_PER_USEC);
 163}
 164
 165/* check limitations for timeval provided by user */
 166static bool bcm_is_invalid_tv(struct bcm_msg_head *msg_head)
 167{
 168	if ((msg_head->ival1.tv_sec < 0) ||
 169	    (msg_head->ival1.tv_sec > BCM_TIMER_SEC_MAX) ||
 170	    (msg_head->ival1.tv_usec < 0) ||
 171	    (msg_head->ival1.tv_usec >= USEC_PER_SEC) ||
 172	    (msg_head->ival2.tv_sec < 0) ||
 173	    (msg_head->ival2.tv_sec > BCM_TIMER_SEC_MAX) ||
 174	    (msg_head->ival2.tv_usec < 0) ||
 175	    (msg_head->ival2.tv_usec >= USEC_PER_SEC))
 176		return true;
 177
 178	return false;
 179}
 180
 181#define CFSIZ(flags) ((flags & CAN_FD_FRAME) ? CANFD_MTU : CAN_MTU)
 182#define OPSIZ sizeof(struct bcm_op)
 183#define MHSIZ sizeof(struct bcm_msg_head)
 184
 185/*
 186 * procfs functions
 187 */
 188#if IS_ENABLED(CONFIG_PROC_FS)
 189static char *bcm_proc_getifname(struct net *net, char *result, int ifindex)
 190{
 191	struct net_device *dev;
 192
 193	if (!ifindex)
 194		return "any";
 195
 196	rcu_read_lock();
 197	dev = dev_get_by_index_rcu(net, ifindex);
 198	if (dev)
 199		strcpy(result, dev->name);
 200	else
 201		strcpy(result, "???");
 202	rcu_read_unlock();
 203
 204	return result;
 205}
 206
 207static int bcm_proc_show(struct seq_file *m, void *v)
 208{
 209	char ifname[IFNAMSIZ];
 210	struct net *net = m->private;
 211	struct sock *sk = (struct sock *)pde_data(m->file->f_inode);
 212	struct bcm_sock *bo = bcm_sk(sk);
 213	struct bcm_op *op;
 214
 215	seq_printf(m, ">>> socket %pK", sk->sk_socket);
 216	seq_printf(m, " / sk %pK", sk);
 217	seq_printf(m, " / bo %pK", bo);
 218	seq_printf(m, " / dropped %lu", bo->dropped_usr_msgs);
 219	seq_printf(m, " / bound %s", bcm_proc_getifname(net, ifname, bo->ifindex));
 220	seq_printf(m, " <<<\n");
 221
 222	rcu_read_lock();
 223
 224	list_for_each_entry_rcu(op, &bo->rx_ops, list) {
 225
 226		unsigned long reduction;
 227
 228		/* print only active entries & prevent division by zero */
 229		if (!op->frames_abs)
 230			continue;
 231
 232		seq_printf(m, "rx_op: %03X %-5s ", op->can_id,
 233			   bcm_proc_getifname(net, ifname, op->ifindex));
 234
 235		if (op->flags & CAN_FD_FRAME)
 236			seq_printf(m, "(%u)", op->nframes);
 237		else
 238			seq_printf(m, "[%u]", op->nframes);
 239
 240		seq_printf(m, "%c ", (op->flags & RX_CHECK_DLC) ? 'd' : ' ');
 241
 242		if (op->kt_ival1)
 243			seq_printf(m, "timeo=%lld ",
 244				   (long long)ktime_to_us(op->kt_ival1));
 245
 246		if (op->kt_ival2)
 247			seq_printf(m, "thr=%lld ",
 248				   (long long)ktime_to_us(op->kt_ival2));
 249
 250		seq_printf(m, "# recv %ld (%ld) => reduction: ",
 251			   op->frames_filtered, op->frames_abs);
 252
 253		reduction = 100 - (op->frames_filtered * 100) / op->frames_abs;
 254
 255		seq_printf(m, "%s%ld%%\n",
 256			   (reduction == 100) ? "near " : "", reduction);
 257	}
 258
 259	list_for_each_entry(op, &bo->tx_ops, list) {
 260
 261		seq_printf(m, "tx_op: %03X %s ", op->can_id,
 262			   bcm_proc_getifname(net, ifname, op->ifindex));
 263
 264		if (op->flags & CAN_FD_FRAME)
 265			seq_printf(m, "(%u) ", op->nframes);
 266		else
 267			seq_printf(m, "[%u] ", op->nframes);
 268
 269		if (op->kt_ival1)
 270			seq_printf(m, "t1=%lld ",
 271				   (long long)ktime_to_us(op->kt_ival1));
 272
 273		if (op->kt_ival2)
 274			seq_printf(m, "t2=%lld ",
 275				   (long long)ktime_to_us(op->kt_ival2));
 276
 277		seq_printf(m, "# sent %ld\n", op->frames_abs);
 278	}
 279	seq_putc(m, '\n');
 280
 281	rcu_read_unlock();
 282
 283	return 0;
 284}
 285#endif /* CONFIG_PROC_FS */
 286
 287/*
 288 * bcm_can_tx - send the (next) CAN frame to the appropriate CAN interface
 289 *              of the given bcm tx op
 290 */
 291static void bcm_can_tx(struct bcm_op *op)
 292{
 293	struct sk_buff *skb;
 294	struct net_device *dev;
 295	struct canfd_frame *cf;
 296	int err;
 297
 298	/* no target device? => exit */
 299	if (!op->ifindex)
 300		return;
 301
 302	/* read currframe under lock protection */
 303	spin_lock_bh(&op->bcm_tx_lock);
 304	cf = op->frames + op->cfsiz * op->currframe;
 305	spin_unlock_bh(&op->bcm_tx_lock);
 306
 307	dev = dev_get_by_index(sock_net(op->sk), op->ifindex);
 308	if (!dev) {
 309		/* RFC: should this bcm_op remove itself here? */
 310		return;
 311	}
 312
 313	skb = alloc_skb(op->cfsiz + sizeof(struct can_skb_priv), gfp_any());
 314	if (!skb)
 315		goto out;
 316
 317	can_skb_reserve(skb);
 318	can_skb_prv(skb)->ifindex = dev->ifindex;
 319	can_skb_prv(skb)->skbcnt = 0;
 320
 321	skb_put_data(skb, cf, op->cfsiz);
 322
 323	/* send with loopback */
 324	skb->dev = dev;
 325	can_skb_set_owner(skb, op->sk);
 326	err = can_send(skb, 1);
 327
 328	/* update currframe and count under lock protection */
 329	spin_lock_bh(&op->bcm_tx_lock);
 330
 331	if (!err)
 332		op->frames_abs++;
 333
 334	op->currframe++;
 335
 336	/* reached last frame? */
 337	if (op->currframe >= op->nframes)
 338		op->currframe = 0;
 339
 340	if (op->count > 0)
 341		op->count--;
 342
 343	spin_unlock_bh(&op->bcm_tx_lock);
 344out:
 345	dev_put(dev);
 346}
 347
 348/*
 349 * bcm_send_to_user - send a BCM message to the userspace
 350 *                    (consisting of bcm_msg_head + x CAN frames)
 351 */
 352static void bcm_send_to_user(struct bcm_op *op, struct bcm_msg_head *head,
 353			     struct canfd_frame *frames, int has_timestamp)
 354{
 355	struct sk_buff *skb;
 356	struct canfd_frame *firstframe;
 357	struct sockaddr_can *addr;
 358	struct sock *sk = op->sk;
 359	unsigned int datalen = head->nframes * op->cfsiz;
 360	int err;
 361	unsigned int *pflags;
 362	enum skb_drop_reason reason;
 363
 364	skb = alloc_skb(sizeof(*head) + datalen, gfp_any());
 365	if (!skb)
 366		return;
 367
 368	skb_put_data(skb, head, sizeof(*head));
 369
 370	/* ensure space for sockaddr_can and msg flags */
 371	sock_skb_cb_check_size(sizeof(struct sockaddr_can) +
 372			       sizeof(unsigned int));
 373
 374	/* initialize msg flags */
 375	pflags = bcm_flags(skb);
 376	*pflags = 0;
 377
 378	if (head->nframes) {
 379		/* CAN frames starting here */
 380		firstframe = (struct canfd_frame *)skb_tail_pointer(skb);
 381
 382		skb_put_data(skb, frames, datalen);
 383
 384		/*
 385		 * the BCM uses the flags-element of the canfd_frame
 386		 * structure for internal purposes. This is only
 387		 * relevant for updates that are generated by the
 388		 * BCM, where nframes is 1
 389		 */
 390		if (head->nframes == 1) {
 391			if (firstframe->flags & RX_LOCAL)
 392				*pflags |= MSG_DONTROUTE;
 393			if (firstframe->flags & RX_OWN)
 394				*pflags |= MSG_CONFIRM;
 395
 396			firstframe->flags &= BCM_CAN_FLAGS_MASK;
 397		}
 398	}
 399
 400	if (has_timestamp) {
 401		/* restore rx timestamp */
 402		skb->tstamp = op->rx_stamp;
 403	}
 404
 405	/*
 406	 *  Put the datagram to the queue so that bcm_recvmsg() can
 407	 *  get it from there.  We need to pass the interface index to
 408	 *  bcm_recvmsg().  We pass a whole struct sockaddr_can in skb->cb
 409	 *  containing the interface index.
 410	 */
 411
 412	addr = (struct sockaddr_can *)skb->cb;
 413	memset(addr, 0, sizeof(*addr));
 414	addr->can_family  = AF_CAN;
 415	addr->can_ifindex = op->rx_ifindex;
 416
 417	err = sock_queue_rcv_skb_reason(sk, skb, &reason);
 418	if (err < 0) {
 419		struct bcm_sock *bo = bcm_sk(sk);
 420
 421		sk_skb_reason_drop(sk, skb, reason);
 422		/* don't care about overflows in this statistic */
 423		bo->dropped_usr_msgs++;
 424	}
 425}
 426
 427static bool bcm_tx_set_expiry(struct bcm_op *op, struct hrtimer *hrt)
 428{
 429	ktime_t ival;
 430
 431	if (op->kt_ival1 && op->count)
 432		ival = op->kt_ival1;
 433	else if (op->kt_ival2)
 434		ival = op->kt_ival2;
 435	else
 436		return false;
 437
 438	hrtimer_set_expires(hrt, ktime_add(ktime_get(), ival));
 439	return true;
 440}
 441
 442static void bcm_tx_start_timer(struct bcm_op *op)
 443{
 444	if (bcm_tx_set_expiry(op, &op->timer))
 445		hrtimer_start_expires(&op->timer, HRTIMER_MODE_ABS_SOFT);
 446}
 447
 448/* bcm_tx_timeout_handler - performs cyclic CAN frame transmissions */
 449static enum hrtimer_restart bcm_tx_timeout_handler(struct hrtimer *hrtimer)
 450{
 451	struct bcm_op *op = container_of(hrtimer, struct bcm_op, timer);
 452	struct bcm_msg_head msg_head;
 453
 454	if (op->kt_ival1 && (op->count > 0)) {
 455		bcm_can_tx(op);
 456		if (!op->count && (op->flags & TX_COUNTEVT)) {
 457
 458			/* create notification to user */
 459			memset(&msg_head, 0, sizeof(msg_head));
 460			msg_head.opcode  = TX_EXPIRED;
 461			msg_head.flags   = op->flags;
 462			msg_head.count   = op->count;
 463			msg_head.ival1   = op->ival1;
 464			msg_head.ival2   = op->ival2;
 465			msg_head.can_id  = op->can_id;
 466			msg_head.nframes = 0;
 467
 468			bcm_send_to_user(op, &msg_head, NULL, 0);
 469		}
 470
 471	} else if (op->kt_ival2) {
 472		bcm_can_tx(op);
 473	}
 474
 475	return bcm_tx_set_expiry(op, &op->timer) ?
 476		HRTIMER_RESTART : HRTIMER_NORESTART;
 477}
 478
 479/*
 480 * bcm_rx_changed - create a RX_CHANGED notification due to changed content
 481 */
 482static void bcm_rx_changed(struct bcm_op *op, struct canfd_frame *data)
 483{
 484	struct bcm_msg_head head;
 485
 486	/* update statistics */
 487	op->frames_filtered++;
 488
 489	/* prevent statistics overflow */
 490	if (op->frames_filtered > ULONG_MAX/100)
 491		op->frames_filtered = op->frames_abs = 0;
 492
 493	/* this element is not throttled anymore */
 494	data->flags &= ~RX_THR;
 495
 496	memset(&head, 0, sizeof(head));
 497	head.opcode  = RX_CHANGED;
 498	head.flags   = op->flags;
 499	head.count   = op->count;
 500	head.ival1   = op->ival1;
 501	head.ival2   = op->ival2;
 502	head.can_id  = op->can_id;
 503	head.nframes = 1;
 504
 505	bcm_send_to_user(op, &head, data, 1);
 506}
 507
 508/*
 509 * bcm_rx_update_and_send - process a detected relevant receive content change
 510 *                          1. update the last received data
 511 *                          2. send a notification to the user (if possible)
 512 */
 513static void bcm_rx_update_and_send(struct bcm_op *op,
 514				   struct canfd_frame *lastdata,
 515				   const struct canfd_frame *rxdata,
 516				   unsigned char traffic_flags)
 517{
 518	memcpy(lastdata, rxdata, op->cfsiz);
 519
 520	/* mark as used and throttled by default */
 521	lastdata->flags |= (RX_RECV|RX_THR);
 522
 523	/* add own/local/remote traffic flags */
 524	lastdata->flags |= traffic_flags;
 525
 526	/* throttling mode inactive ? */
 527	if (!op->kt_ival2) {
 528		/* send RX_CHANGED to the user immediately */
 529		bcm_rx_changed(op, lastdata);
 530		return;
 531	}
 532
 533	/* with active throttling timer we are just done here */
 534	if (hrtimer_active(&op->thrtimer))
 535		return;
 536
 537	/* first reception with enabled throttling mode */
 538	if (!op->kt_lastmsg)
 539		goto rx_changed_settime;
 540
 541	/* got a second frame inside a potential throttle period? */
 542	if (ktime_us_delta(ktime_get(), op->kt_lastmsg) <
 543	    ktime_to_us(op->kt_ival2)) {
 544		/* do not send the saved data - only start throttle timer */
 545		hrtimer_start(&op->thrtimer,
 546			      ktime_add(op->kt_lastmsg, op->kt_ival2),
 547			      HRTIMER_MODE_ABS_SOFT);
 548		return;
 549	}
 550
 551	/* the gap was that big, that throttling was not needed here */
 552rx_changed_settime:
 553	bcm_rx_changed(op, lastdata);
 554	op->kt_lastmsg = ktime_get();
 555}
 556
 557/*
 558 * bcm_rx_cmp_to_index - (bit)compares the currently received data to formerly
 559 *                       received data stored in op->last_frames[]
 560 */
 561static void bcm_rx_cmp_to_index(struct bcm_op *op, unsigned int index,
 562				const struct canfd_frame *rxdata,
 563				unsigned char traffic_flags)
 564{
 565	struct canfd_frame *cf = op->frames + op->cfsiz * index;
 566	struct canfd_frame *lcf = op->last_frames + op->cfsiz * index;
 567	int i;
 568
 569	/*
 570	 * no one uses the MSBs of flags for comparison,
 571	 * so we use it here to detect the first time of reception
 572	 */
 573
 574	if (!(lcf->flags & RX_RECV)) {
 575		/* received data for the first time => send update to user */
 576		bcm_rx_update_and_send(op, lcf, rxdata, traffic_flags);
 577		return;
 578	}
 579
 580	/* do a real check in CAN frame data section */
 581	for (i = 0; i < rxdata->len; i += 8) {
 582		if ((get_u64(cf, i) & get_u64(rxdata, i)) !=
 583		    (get_u64(cf, i) & get_u64(lcf, i))) {
 584			bcm_rx_update_and_send(op, lcf, rxdata, traffic_flags);
 585			return;
 586		}
 587	}
 588
 589	if (op->flags & RX_CHECK_DLC) {
 590		/* do a real check in CAN frame length */
 591		if (rxdata->len != lcf->len) {
 592			bcm_rx_update_and_send(op, lcf, rxdata, traffic_flags);
 593			return;
 594		}
 595	}
 596}
 597
 598/*
 599 * bcm_rx_starttimer - enable timeout monitoring for CAN frame reception
 600 */
 601static void bcm_rx_starttimer(struct bcm_op *op)
 602{
 603	if (op->flags & RX_NO_AUTOTIMER)
 604		return;
 605
 606	if (op->kt_ival1)
 607		hrtimer_start(&op->timer, op->kt_ival1, HRTIMER_MODE_REL_SOFT);
 608}
 609
 610/* bcm_rx_timeout_handler - when the (cyclic) CAN frame reception timed out */
 611static enum hrtimer_restart bcm_rx_timeout_handler(struct hrtimer *hrtimer)
 612{
 613	struct bcm_op *op = container_of(hrtimer, struct bcm_op, timer);
 614	struct bcm_msg_head msg_head;
 615
 616	/* if user wants to be informed, when cyclic CAN-Messages come back */
 617	if ((op->flags & RX_ANNOUNCE_RESUME) && op->last_frames) {
 618		/* clear received CAN frames to indicate 'nothing received' */
 619		memset(op->last_frames, 0, op->nframes * op->cfsiz);
 620	}
 621
 622	/* create notification to user */
 623	memset(&msg_head, 0, sizeof(msg_head));
 624	msg_head.opcode  = RX_TIMEOUT;
 625	msg_head.flags   = op->flags;
 626	msg_head.count   = op->count;
 627	msg_head.ival1   = op->ival1;
 628	msg_head.ival2   = op->ival2;
 629	msg_head.can_id  = op->can_id;
 630	msg_head.nframes = 0;
 631
 632	bcm_send_to_user(op, &msg_head, NULL, 0);
 633
 634	return HRTIMER_NORESTART;
 635}
 636
 637/*
 638 * bcm_rx_do_flush - helper for bcm_rx_thr_flush
 639 */
 640static inline int bcm_rx_do_flush(struct bcm_op *op, unsigned int index)
 641{
 642	struct canfd_frame *lcf = op->last_frames + op->cfsiz * index;
 643
 644	if ((op->last_frames) && (lcf->flags & RX_THR)) {
 645		bcm_rx_changed(op, lcf);
 646		return 1;
 647	}
 648	return 0;
 649}
 650
 651/*
 652 * bcm_rx_thr_flush - Check for throttled data and send it to the userspace
 653 */
 654static int bcm_rx_thr_flush(struct bcm_op *op)
 655{
 656	int updated = 0;
 657
 658	if (op->nframes > 1) {
 659		unsigned int i;
 660
 661		/* for MUX filter we start at index 1 */
 662		for (i = 1; i < op->nframes; i++)
 663			updated += bcm_rx_do_flush(op, i);
 664
 665	} else {
 666		/* for RX_FILTER_ID and simple filter */
 667		updated += bcm_rx_do_flush(op, 0);
 668	}
 669
 670	return updated;
 671}
 672
 673/*
 674 * bcm_rx_thr_handler - the time for blocked content updates is over now:
 675 *                      Check for throttled data and send it to the userspace
 676 */
 677static enum hrtimer_restart bcm_rx_thr_handler(struct hrtimer *hrtimer)
 678{
 679	struct bcm_op *op = container_of(hrtimer, struct bcm_op, thrtimer);
 680
 681	if (bcm_rx_thr_flush(op)) {
 682		hrtimer_forward_now(hrtimer, op->kt_ival2);
 683		return HRTIMER_RESTART;
 684	} else {
 685		/* rearm throttle handling */
 686		op->kt_lastmsg = 0;
 687		return HRTIMER_NORESTART;
 688	}
 689}
 690
 691/*
 692 * bcm_rx_handler - handle a CAN frame reception
 693 */
 694static void bcm_rx_handler(struct sk_buff *skb, void *data)
 695{
 696	struct bcm_op *op = (struct bcm_op *)data;
 697	const struct canfd_frame *rxframe = (struct canfd_frame *)skb->data;
 698	unsigned int i;
 699	unsigned char traffic_flags;
 700
 701	if (op->can_id != rxframe->can_id)
 702		return;
 703
 704	/* make sure to handle the correct frame type (CAN / CAN FD) */
 705	if (op->flags & CAN_FD_FRAME) {
 706		if (!can_is_canfd_skb(skb))
 707			return;
 708	} else {
 709		if (!can_is_can_skb(skb))
 710			return;
 711	}
 712
 713	/* disable timeout */
 714	hrtimer_cancel(&op->timer);
 715
 716	/* save rx timestamp */
 717	op->rx_stamp = skb->tstamp;
 718	/* save originator for recvfrom() */
 719	op->rx_ifindex = skb->dev->ifindex;
 720	/* update statistics */
 721	op->frames_abs++;
 722
 723	if (op->flags & RX_RTR_FRAME) {
 724		/* send reply for RTR-request (placed in op->frames[0]) */
 725		bcm_can_tx(op);
 726		return;
 727	}
 728
 729	/* compute flags to distinguish between own/local/remote CAN traffic */
 730	traffic_flags = 0;
 731	if (skb->sk) {
 732		traffic_flags |= RX_LOCAL;
 733		if (skb->sk == op->sk)
 734			traffic_flags |= RX_OWN;
 735	}
 736
 737	if (op->flags & RX_FILTER_ID) {
 738		/* the easiest case */
 739		bcm_rx_update_and_send(op, op->last_frames, rxframe,
 740				       traffic_flags);
 741		goto rx_starttimer;
 742	}
 743
 744	if (op->nframes == 1) {
 745		/* simple compare with index 0 */
 746		bcm_rx_cmp_to_index(op, 0, rxframe, traffic_flags);
 747		goto rx_starttimer;
 748	}
 749
 750	if (op->nframes > 1) {
 751		/*
 752		 * multiplex compare
 753		 *
 754		 * find the first multiplex mask that fits.
 755		 * Remark: The MUX-mask is stored in index 0 - but only the
 756		 * first 64 bits of the frame data[] are relevant (CAN FD)
 757		 */
 758
 759		for (i = 1; i < op->nframes; i++) {
 760			if ((get_u64(op->frames, 0) & get_u64(rxframe, 0)) ==
 761			    (get_u64(op->frames, 0) &
 762			     get_u64(op->frames + op->cfsiz * i, 0))) {
 763				bcm_rx_cmp_to_index(op, i, rxframe,
 764						    traffic_flags);
 765				break;
 766			}
 767		}
 768	}
 769
 770rx_starttimer:
 771	bcm_rx_starttimer(op);
 772}
 773
 774/*
 775 * helpers for bcm_op handling: find & delete bcm [rx|tx] op elements
 776 */
 777static struct bcm_op *bcm_find_op(struct list_head *ops,
 778				  struct bcm_msg_head *mh, int ifindex)
 779{
 780	struct bcm_op *op;
 781
 782	list_for_each_entry(op, ops, list) {
 783		if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) &&
 784		    (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME))
 785			return op;
 786	}
 787
 788	return NULL;
 789}
 790
 791static void bcm_free_op_rcu(struct rcu_head *rcu_head)
 792{
 793	struct bcm_op *op = container_of(rcu_head, struct bcm_op, rcu);
 794
 795	if ((op->frames) && (op->frames != &op->sframe))
 796		kfree(op->frames);
 797
 798	if ((op->last_frames) && (op->last_frames != &op->last_sframe))
 799		kfree(op->last_frames);
 800
 801	kfree(op);
 802}
 803
 804static void bcm_remove_op(struct bcm_op *op)
 805{
 806	hrtimer_cancel(&op->timer);
 807	hrtimer_cancel(&op->thrtimer);
 808
 809	call_rcu(&op->rcu, bcm_free_op_rcu);
 810}
 811
 812static void bcm_rx_unreg(struct net_device *dev, struct bcm_op *op)
 813{
 814	if (op->rx_reg_dev == dev) {
 815		can_rx_unregister(dev_net(dev), dev, op->can_id,
 816				  REGMASK(op->can_id), bcm_rx_handler, op);
 817
 818		/* mark as removed subscription */
 819		op->rx_reg_dev = NULL;
 820	} else
 821		printk(KERN_ERR "can-bcm: bcm_rx_unreg: registered device "
 822		       "mismatch %p %p\n", op->rx_reg_dev, dev);
 823}
 824
 825/*
 826 * bcm_delete_rx_op - find and remove a rx op (returns number of removed ops)
 827 */
 828static int bcm_delete_rx_op(struct list_head *ops, struct bcm_msg_head *mh,
 829			    int ifindex)
 830{
 831	struct bcm_op *op, *n;
 832
 833	list_for_each_entry_safe(op, n, ops, list) {
 834		if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) &&
 835		    (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME)) {
 836
 837			/* disable automatic timer on frame reception */
 838			op->flags |= RX_NO_AUTOTIMER;
 839
 840			/*
 841			 * Don't care if we're bound or not (due to netdev
 842			 * problems) can_rx_unregister() is always a save
 843			 * thing to do here.
 844			 */
 845			if (op->ifindex) {
 846				/*
 847				 * Only remove subscriptions that had not
 848				 * been removed due to NETDEV_UNREGISTER
 849				 * in bcm_notifier()
 850				 */
 851				if (op->rx_reg_dev) {
 852					struct net_device *dev;
 853
 854					dev = dev_get_by_index(sock_net(op->sk),
 855							       op->ifindex);
 856					if (dev) {
 857						bcm_rx_unreg(dev, op);
 858						dev_put(dev);
 859					}
 860				}
 861			} else
 862				can_rx_unregister(sock_net(op->sk), NULL,
 863						  op->can_id,
 864						  REGMASK(op->can_id),
 865						  bcm_rx_handler, op);
 866
 867			list_del_rcu(&op->list);
 868			bcm_remove_op(op);
 869			return 1; /* done */
 870		}
 871	}
 872
 873	return 0; /* not found */
 874}
 875
 876/*
 877 * bcm_delete_tx_op - find and remove a tx op (returns number of removed ops)
 878 */
 879static int bcm_delete_tx_op(struct list_head *ops, struct bcm_msg_head *mh,
 880			    int ifindex)
 881{
 882	struct bcm_op *op, *n;
 883
 884	list_for_each_entry_safe(op, n, ops, list) {
 885		if ((op->can_id == mh->can_id) && (op->ifindex == ifindex) &&
 886		    (op->flags & CAN_FD_FRAME) == (mh->flags & CAN_FD_FRAME)) {
 887			list_del_rcu(&op->list);
 888			bcm_remove_op(op);
 889			return 1; /* done */
 890		}
 891	}
 892
 893	return 0; /* not found */
 894}
 895
 896/*
 897 * bcm_read_op - read out a bcm_op and send it to the user (for bcm_sendmsg)
 898 */
 899static int bcm_read_op(struct list_head *ops, struct bcm_msg_head *msg_head,
 900		       int ifindex)
 901{
 902	struct bcm_op *op = bcm_find_op(ops, msg_head, ifindex);
 903
 904	if (!op)
 905		return -EINVAL;
 906
 907	/* put current values into msg_head */
 908	msg_head->flags   = op->flags;
 909	msg_head->count   = op->count;
 910	msg_head->ival1   = op->ival1;
 911	msg_head->ival2   = op->ival2;
 912	msg_head->nframes = op->nframes;
 913
 914	bcm_send_to_user(op, msg_head, op->frames, 0);
 915
 916	return MHSIZ;
 917}
 918
 919/*
 920 * bcm_tx_setup - create or update a bcm tx op (for bcm_sendmsg)
 921 */
 922static int bcm_tx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg,
 923			int ifindex, struct sock *sk)
 924{
 925	struct bcm_sock *bo = bcm_sk(sk);
 926	struct bcm_op *op;
 927	struct canfd_frame *cf;
 928	unsigned int i;
 929	int err;
 930
 931	/* we need a real device to send frames */
 932	if (!ifindex)
 933		return -ENODEV;
 934
 935	/* check nframes boundaries - we need at least one CAN frame */
 936	if (msg_head->nframes < 1 || msg_head->nframes > MAX_NFRAMES)
 937		return -EINVAL;
 938
 939	/* check timeval limitations */
 940	if ((msg_head->flags & SETTIMER) && bcm_is_invalid_tv(msg_head))
 941		return -EINVAL;
 942
 943	/* check the given can_id */
 944	op = bcm_find_op(&bo->tx_ops, msg_head, ifindex);
 945	if (op) {
 946		/* update existing BCM operation */
 947
 948		/*
 949		 * Do we need more space for the CAN frames than currently
 950		 * allocated? -> This is a _really_ unusual use-case and
 951		 * therefore (complexity / locking) it is not supported.
 952		 */
 953		if (msg_head->nframes > op->nframes)
 954			return -E2BIG;
 955
 956		/* update CAN frames content */
 957		for (i = 0; i < msg_head->nframes; i++) {
 958
 959			cf = op->frames + op->cfsiz * i;
 960			err = memcpy_from_msg((u8 *)cf, msg, op->cfsiz);
 961
 962			if (op->flags & CAN_FD_FRAME) {
 963				if (cf->len > 64)
 964					err = -EINVAL;
 965			} else {
 966				if (cf->len > 8)
 967					err = -EINVAL;
 968			}
 969
 970			if (err < 0)
 971				return err;
 972
 973			if (msg_head->flags & TX_CP_CAN_ID) {
 974				/* copy can_id into frame */
 975				cf->can_id = msg_head->can_id;
 976			}
 977		}
 978		op->flags = msg_head->flags;
 979
 980		/* only lock for unlikely count/nframes/currframe changes */
 981		if (op->nframes != msg_head->nframes ||
 982		    op->flags & TX_RESET_MULTI_IDX ||
 983		    op->flags & SETTIMER) {
 984
 985			spin_lock_bh(&op->bcm_tx_lock);
 986
 987			if (op->nframes != msg_head->nframes ||
 988			    op->flags & TX_RESET_MULTI_IDX) {
 989				/* potentially update changed nframes */
 990				op->nframes = msg_head->nframes;
 991				/* restart multiple frame transmission */
 992				op->currframe = 0;
 993			}
 994
 995			if (op->flags & SETTIMER)
 996				op->count = msg_head->count;
 997
 998			spin_unlock_bh(&op->bcm_tx_lock);
 999		}
1000
1001	} else {
1002		/* insert new BCM operation for the given can_id */
1003
1004		op = kzalloc(OPSIZ, GFP_KERNEL);
1005		if (!op)
1006			return -ENOMEM;
1007
1008		spin_lock_init(&op->bcm_tx_lock);
1009		op->can_id = msg_head->can_id;
1010		op->cfsiz = CFSIZ(msg_head->flags);
1011		op->flags = msg_head->flags;
1012		op->nframes = msg_head->nframes;
1013
1014		if (op->flags & SETTIMER)
1015			op->count = msg_head->count;
1016
1017		/* create array for CAN frames and copy the data */
1018		if (msg_head->nframes > 1) {
1019			op->frames = kmalloc_array(msg_head->nframes,
1020						   op->cfsiz,
1021						   GFP_KERNEL);
1022			if (!op->frames) {
1023				kfree(op);
1024				return -ENOMEM;
1025			}
1026		} else
1027			op->frames = &op->sframe;
1028
1029		for (i = 0; i < msg_head->nframes; i++) {
1030
1031			cf = op->frames + op->cfsiz * i;
1032			err = memcpy_from_msg((u8 *)cf, msg, op->cfsiz);
1033			if (err < 0)
1034				goto free_op;
1035
1036			if (op->flags & CAN_FD_FRAME) {
1037				if (cf->len > 64)
1038					err = -EINVAL;
1039			} else {
1040				if (cf->len > 8)
1041					err = -EINVAL;
1042			}
1043
1044			if (err < 0)
1045				goto free_op;
1046
1047			if (msg_head->flags & TX_CP_CAN_ID) {
1048				/* copy can_id into frame */
1049				cf->can_id = msg_head->can_id;
1050			}
1051		}
1052
1053		/* tx_ops never compare with previous received messages */
1054		op->last_frames = NULL;
1055
1056		/* bcm_can_tx / bcm_tx_timeout_handler needs this */
1057		op->sk = sk;
1058		op->ifindex = ifindex;
1059
1060		/* initialize uninitialized (kzalloc) structure */
1061		hrtimer_setup(&op->timer, bcm_tx_timeout_handler, CLOCK_MONOTONIC,
1062			      HRTIMER_MODE_REL_SOFT);
1063
1064		/* currently unused in tx_ops */
1065		hrtimer_setup(&op->thrtimer, hrtimer_dummy_timeout, CLOCK_MONOTONIC,
1066			      HRTIMER_MODE_REL_SOFT);
1067
1068		/* add this bcm_op to the list of the tx_ops */
1069		list_add(&op->list, &bo->tx_ops);
1070
1071	} /* if ((op = bcm_find_op(&bo->tx_ops, msg_head->can_id, ifindex))) */
1072
1073	if (op->flags & SETTIMER) {
1074		/* set timer values */
1075		op->ival1 = msg_head->ival1;
1076		op->ival2 = msg_head->ival2;
1077		op->kt_ival1 = bcm_timeval_to_ktime(msg_head->ival1);
1078		op->kt_ival2 = bcm_timeval_to_ktime(msg_head->ival2);
1079
1080		/* disable an active timer due to zero values? */
1081		if (!op->kt_ival1 && !op->kt_ival2)
1082			hrtimer_cancel(&op->timer);
1083	}
1084
1085	if (op->flags & STARTTIMER) {
1086		hrtimer_cancel(&op->timer);
1087		/* spec: send CAN frame when starting timer */
1088		op->flags |= TX_ANNOUNCE;
1089	}
1090
1091	if (op->flags & TX_ANNOUNCE)
1092		bcm_can_tx(op);
1093
1094	if (op->flags & STARTTIMER)
1095		bcm_tx_start_timer(op);
1096
1097	return msg_head->nframes * op->cfsiz + MHSIZ;
1098
1099free_op:
1100	if (op->frames != &op->sframe)
1101		kfree(op->frames);
1102	kfree(op);
1103	return err;
1104}
1105
1106/*
1107 * bcm_rx_setup - create or update a bcm rx op (for bcm_sendmsg)
1108 */
1109static int bcm_rx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg,
1110			int ifindex, struct sock *sk)
1111{
1112	struct bcm_sock *bo = bcm_sk(sk);
1113	struct bcm_op *op;
1114	int do_rx_register;
1115	int err = 0;
1116
1117	if ((msg_head->flags & RX_FILTER_ID) || (!(msg_head->nframes))) {
1118		/* be robust against wrong usage ... */
1119		msg_head->flags |= RX_FILTER_ID;
1120		/* ignore trailing garbage */
1121		msg_head->nframes = 0;
1122	}
1123
1124	/* the first element contains the mux-mask => MAX_NFRAMES + 1  */
1125	if (msg_head->nframes > MAX_NFRAMES + 1)
1126		return -EINVAL;
1127
1128	if ((msg_head->flags & RX_RTR_FRAME) &&
1129	    ((msg_head->nframes != 1) ||
1130	     (!(msg_head->can_id & CAN_RTR_FLAG))))
1131		return -EINVAL;
1132
1133	/* check timeval limitations */
1134	if ((msg_head->flags & SETTIMER) && bcm_is_invalid_tv(msg_head))
1135		return -EINVAL;
1136
1137	/* check the given can_id */
1138	op = bcm_find_op(&bo->rx_ops, msg_head, ifindex);
1139	if (op) {
1140		/* update existing BCM operation */
1141
1142		/*
1143		 * Do we need more space for the CAN frames than currently
1144		 * allocated? -> This is a _really_ unusual use-case and
1145		 * therefore (complexity / locking) it is not supported.
1146		 */
1147		if (msg_head->nframes > op->nframes)
1148			return -E2BIG;
1149
1150		if (msg_head->nframes) {
1151			/* update CAN frames content */
1152			err = memcpy_from_msg(op->frames, msg,
1153					      msg_head->nframes * op->cfsiz);
1154			if (err < 0)
1155				return err;
1156
1157			/* clear last_frames to indicate 'nothing received' */
1158			memset(op->last_frames, 0, msg_head->nframes * op->cfsiz);
1159		}
1160
1161		op->nframes = msg_head->nframes;
1162		op->flags = msg_head->flags;
1163
1164		/* Only an update -> do not call can_rx_register() */
1165		do_rx_register = 0;
1166
1167	} else {
1168		/* insert new BCM operation for the given can_id */
1169		op = kzalloc(OPSIZ, GFP_KERNEL);
1170		if (!op)
1171			return -ENOMEM;
1172
1173		spin_lock_init(&op->bcm_tx_lock);
1174		op->can_id = msg_head->can_id;
1175		op->nframes = msg_head->nframes;
1176		op->cfsiz = CFSIZ(msg_head->flags);
1177		op->flags = msg_head->flags;
1178
1179		if (msg_head->nframes > 1) {
1180			/* create array for CAN frames and copy the data */
1181			op->frames = kmalloc_array(msg_head->nframes,
1182						   op->cfsiz,
1183						   GFP_KERNEL);
1184			if (!op->frames) {
1185				kfree(op);
1186				return -ENOMEM;
1187			}
1188
1189			/* create and init array for received CAN frames */
1190			op->last_frames = kcalloc(msg_head->nframes,
1191						  op->cfsiz,
1192						  GFP_KERNEL);
1193			if (!op->last_frames) {
1194				kfree(op->frames);
1195				kfree(op);
1196				return -ENOMEM;
1197			}
1198
1199		} else {
1200			op->frames = &op->sframe;
1201			op->last_frames = &op->last_sframe;
1202		}
1203
1204		if (msg_head->nframes) {
1205			err = memcpy_from_msg(op->frames, msg,
1206					      msg_head->nframes * op->cfsiz);
1207			if (err < 0) {
1208				if (op->frames != &op->sframe)
1209					kfree(op->frames);
1210				if (op->last_frames != &op->last_sframe)
1211					kfree(op->last_frames);
1212				kfree(op);
1213				return err;
1214			}
1215		}
1216
1217		/* bcm_can_tx / bcm_tx_timeout_handler needs this */
1218		op->sk = sk;
1219		op->ifindex = ifindex;
1220
1221		/* ifindex for timeout events w/o previous frame reception */
1222		op->rx_ifindex = ifindex;
1223
1224		/* initialize uninitialized (kzalloc) structure */
1225		hrtimer_setup(&op->timer, bcm_rx_timeout_handler, CLOCK_MONOTONIC,
1226			      HRTIMER_MODE_REL_SOFT);
1227		hrtimer_setup(&op->thrtimer, bcm_rx_thr_handler, CLOCK_MONOTONIC,
1228			      HRTIMER_MODE_REL_SOFT);
1229
1230		/* add this bcm_op to the list of the rx_ops */
1231		list_add(&op->list, &bo->rx_ops);
1232
1233		/* call can_rx_register() */
1234		do_rx_register = 1;
1235
1236	} /* if ((op = bcm_find_op(&bo->rx_ops, msg_head->can_id, ifindex))) */
1237
1238	/* check flags */
1239
1240	if (op->flags & RX_RTR_FRAME) {
1241		struct canfd_frame *frame0 = op->frames;
1242
1243		/* no timers in RTR-mode */
1244		hrtimer_cancel(&op->thrtimer);
1245		hrtimer_cancel(&op->timer);
1246
1247		/*
1248		 * funny feature in RX(!)_SETUP only for RTR-mode:
1249		 * copy can_id into frame BUT without RTR-flag to
1250		 * prevent a full-load-loopback-test ... ;-]
1251		 */
1252		if ((op->flags & TX_CP_CAN_ID) ||
1253		    (frame0->can_id == op->can_id))
1254			frame0->can_id = op->can_id & ~CAN_RTR_FLAG;
1255
1256	} else {
1257		if (op->flags & SETTIMER) {
1258
1259			/* set timer value */
1260			op->ival1 = msg_head->ival1;
1261			op->ival2 = msg_head->ival2;
1262			op->kt_ival1 = bcm_timeval_to_ktime(msg_head->ival1);
1263			op->kt_ival2 = bcm_timeval_to_ktime(msg_head->ival2);
1264
1265			/* disable an active timer due to zero value? */
1266			if (!op->kt_ival1)
1267				hrtimer_cancel(&op->timer);
1268
1269			/*
1270			 * In any case cancel the throttle timer, flush
1271			 * potentially blocked msgs and reset throttle handling
1272			 */
1273			op->kt_lastmsg = 0;
1274			hrtimer_cancel(&op->thrtimer);
1275			bcm_rx_thr_flush(op);
1276		}
1277
1278		if ((op->flags & STARTTIMER) && op->kt_ival1)
1279			hrtimer_start(&op->timer, op->kt_ival1,
1280				      HRTIMER_MODE_REL_SOFT);
1281	}
1282
1283	/* now we can register for can_ids, if we added a new bcm_op */
1284	if (do_rx_register) {
1285		if (ifindex) {
1286			struct net_device *dev;
1287
1288			dev = dev_get_by_index(sock_net(sk), ifindex);
1289			if (dev) {
1290				err = can_rx_register(sock_net(sk), dev,
1291						      op->can_id,
1292						      REGMASK(op->can_id),
1293						      bcm_rx_handler, op,
1294						      "bcm", sk);
1295
1296				op->rx_reg_dev = dev;
1297				dev_put(dev);
1298			}
1299
1300		} else
1301			err = can_rx_register(sock_net(sk), NULL, op->can_id,
1302					      REGMASK(op->can_id),
1303					      bcm_rx_handler, op, "bcm", sk);
1304		if (err) {
1305			/* this bcm rx op is broken -> remove it */
1306			list_del_rcu(&op->list);
1307			bcm_remove_op(op);
1308			return err;
1309		}
1310	}
1311
1312	return msg_head->nframes * op->cfsiz + MHSIZ;
1313}
1314
1315/*
1316 * bcm_tx_send - send a single CAN frame to the CAN interface (for bcm_sendmsg)
1317 */
1318static int bcm_tx_send(struct msghdr *msg, int ifindex, struct sock *sk,
1319		       int cfsiz)
1320{
1321	struct sk_buff *skb;
1322	struct net_device *dev;
1323	int err;
1324
1325	/* we need a real device to send frames */
1326	if (!ifindex)
1327		return -ENODEV;
1328
1329	skb = alloc_skb(cfsiz + sizeof(struct can_skb_priv), GFP_KERNEL);
1330	if (!skb)
1331		return -ENOMEM;
1332
1333	can_skb_reserve(skb);
1334
1335	err = memcpy_from_msg(skb_put(skb, cfsiz), msg, cfsiz);
1336	if (err < 0) {
1337		kfree_skb(skb);
1338		return err;
1339	}
1340
1341	dev = dev_get_by_index(sock_net(sk), ifindex);
1342	if (!dev) {
1343		kfree_skb(skb);
1344		return -ENODEV;
1345	}
1346
1347	can_skb_prv(skb)->ifindex = dev->ifindex;
1348	can_skb_prv(skb)->skbcnt = 0;
1349	skb->dev = dev;
1350	can_skb_set_owner(skb, sk);
1351	err = can_send(skb, 1); /* send with loopback */
1352	dev_put(dev);
1353
1354	if (err)
1355		return err;
1356
1357	return cfsiz + MHSIZ;
1358}
1359
1360/*
1361 * bcm_sendmsg - process BCM commands (opcodes) from the userspace
1362 */
1363static int bcm_sendmsg(struct socket *sock, struct msghdr *msg, size_t size)
1364{
1365	struct sock *sk = sock->sk;
1366	struct bcm_sock *bo = bcm_sk(sk);
1367	int ifindex = bo->ifindex; /* default ifindex for this bcm_op */
1368	struct bcm_msg_head msg_head;
1369	int cfsiz;
1370	int ret; /* read bytes or error codes as return value */
1371
1372	if (!bo->bound)
1373		return -ENOTCONN;
1374
1375	/* check for valid message length from userspace */
1376	if (size < MHSIZ)
1377		return -EINVAL;
1378
1379	/* read message head information */
1380	ret = memcpy_from_msg((u8 *)&msg_head, msg, MHSIZ);
1381	if (ret < 0)
1382		return ret;
1383
1384	cfsiz = CFSIZ(msg_head.flags);
1385	if ((size - MHSIZ) % cfsiz)
1386		return -EINVAL;
1387
1388	/* check for alternative ifindex for this bcm_op */
1389
1390	if (!ifindex && msg->msg_name) {
1391		/* no bound device as default => check msg_name */
1392		DECLARE_SOCKADDR(struct sockaddr_can *, addr, msg->msg_name);
1393
1394		if (msg->msg_namelen < BCM_MIN_NAMELEN)
1395			return -EINVAL;
1396
1397		if (addr->can_family != AF_CAN)
1398			return -EINVAL;
1399
1400		/* ifindex from sendto() */
1401		ifindex = addr->can_ifindex;
1402
1403		if (ifindex) {
1404			struct net_device *dev;
1405
1406			dev = dev_get_by_index(sock_net(sk), ifindex);
1407			if (!dev)
1408				return -ENODEV;
1409
1410			if (dev->type != ARPHRD_CAN) {
1411				dev_put(dev);
1412				return -ENODEV;
1413			}
1414
1415			dev_put(dev);
1416		}
1417	}
1418
1419	lock_sock(sk);
1420
1421	switch (msg_head.opcode) {
1422
1423	case TX_SETUP:
1424		ret = bcm_tx_setup(&msg_head, msg, ifindex, sk);
1425		break;
1426
1427	case RX_SETUP:
1428		ret = bcm_rx_setup(&msg_head, msg, ifindex, sk);
1429		break;
1430
1431	case TX_DELETE:
1432		if (bcm_delete_tx_op(&bo->tx_ops, &msg_head, ifindex))
1433			ret = MHSIZ;
1434		else
1435			ret = -EINVAL;
1436		break;
1437
1438	case RX_DELETE:
1439		if (bcm_delete_rx_op(&bo->rx_ops, &msg_head, ifindex))
1440			ret = MHSIZ;
1441		else
1442			ret = -EINVAL;
1443		break;
1444
1445	case TX_READ:
1446		/* reuse msg_head for the reply to TX_READ */
1447		msg_head.opcode  = TX_STATUS;
1448		ret = bcm_read_op(&bo->tx_ops, &msg_head, ifindex);
1449		break;
1450
1451	case RX_READ:
1452		/* reuse msg_head for the reply to RX_READ */
1453		msg_head.opcode  = RX_STATUS;
1454		ret = bcm_read_op(&bo->rx_ops, &msg_head, ifindex);
1455		break;
1456
1457	case TX_SEND:
1458		/* we need exactly one CAN frame behind the msg head */
1459		if ((msg_head.nframes != 1) || (size != cfsiz + MHSIZ))
1460			ret = -EINVAL;
1461		else
1462			ret = bcm_tx_send(msg, ifindex, sk, cfsiz);
1463		break;
1464
1465	default:
1466		ret = -EINVAL;
1467		break;
1468	}
1469
1470	release_sock(sk);
1471
1472	return ret;
1473}
1474
1475/*
1476 * notification handler for netdevice status changes
1477 */
1478static void bcm_notify(struct bcm_sock *bo, unsigned long msg,
1479		       struct net_device *dev)
1480{
1481	struct sock *sk = &bo->sk;
1482	struct bcm_op *op;
1483	int notify_enodev = 0;
1484
1485	if (!net_eq(dev_net(dev), sock_net(sk)))
1486		return;
1487
1488	switch (msg) {
1489
1490	case NETDEV_UNREGISTER:
1491		lock_sock(sk);
1492
1493		/* remove device specific receive entries */
1494		list_for_each_entry(op, &bo->rx_ops, list)
1495			if (op->rx_reg_dev == dev)
1496				bcm_rx_unreg(dev, op);
1497
1498		/* remove device reference, if this is our bound device */
1499		if (bo->bound && bo->ifindex == dev->ifindex) {
1500#if IS_ENABLED(CONFIG_PROC_FS)
1501			if (sock_net(sk)->can.bcmproc_dir && bo->bcm_proc_read) {
1502				remove_proc_entry(bo->procname, sock_net(sk)->can.bcmproc_dir);
1503				bo->bcm_proc_read = NULL;
1504			}
1505#endif
1506			bo->bound   = 0;
1507			bo->ifindex = 0;
1508			notify_enodev = 1;
1509		}
1510
1511		release_sock(sk);
1512
1513		if (notify_enodev) {
1514			sk->sk_err = ENODEV;
1515			if (!sock_flag(sk, SOCK_DEAD))
1516				sk_error_report(sk);
1517		}
1518		break;
1519
1520	case NETDEV_DOWN:
1521		if (bo->bound && bo->ifindex == dev->ifindex) {
1522			sk->sk_err = ENETDOWN;
1523			if (!sock_flag(sk, SOCK_DEAD))
1524				sk_error_report(sk);
1525		}
1526	}
1527}
1528
1529static int bcm_notifier(struct notifier_block *nb, unsigned long msg,
1530			void *ptr)
1531{
1532	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1533
1534	if (dev->type != ARPHRD_CAN)
1535		return NOTIFY_DONE;
1536	if (msg != NETDEV_UNREGISTER && msg != NETDEV_DOWN)
1537		return NOTIFY_DONE;
1538	if (unlikely(bcm_busy_notifier)) /* Check for reentrant bug. */
1539		return NOTIFY_DONE;
1540
1541	spin_lock(&bcm_notifier_lock);
1542	list_for_each_entry(bcm_busy_notifier, &bcm_notifier_list, notifier) {
1543		spin_unlock(&bcm_notifier_lock);
1544		bcm_notify(bcm_busy_notifier, msg, dev);
1545		spin_lock(&bcm_notifier_lock);
1546	}
1547	bcm_busy_notifier = NULL;
1548	spin_unlock(&bcm_notifier_lock);
1549	return NOTIFY_DONE;
1550}
1551
1552/*
1553 * initial settings for all BCM sockets to be set at socket creation time
1554 */
1555static int bcm_init(struct sock *sk)
1556{
1557	struct bcm_sock *bo = bcm_sk(sk);
1558
1559	bo->bound            = 0;
1560	bo->ifindex          = 0;
1561	bo->dropped_usr_msgs = 0;
1562	bo->bcm_proc_read    = NULL;
1563
1564	INIT_LIST_HEAD(&bo->tx_ops);
1565	INIT_LIST_HEAD(&bo->rx_ops);
1566
1567	/* set notifier */
1568	spin_lock(&bcm_notifier_lock);
1569	list_add_tail(&bo->notifier, &bcm_notifier_list);
1570	spin_unlock(&bcm_notifier_lock);
1571
1572	return 0;
1573}
1574
1575/*
1576 * standard socket functions
1577 */
1578static int bcm_release(struct socket *sock)
1579{
1580	struct sock *sk = sock->sk;
1581	struct net *net;
1582	struct bcm_sock *bo;
1583	struct bcm_op *op, *next;
1584
1585	if (!sk)
1586		return 0;
1587
1588	net = sock_net(sk);
1589	bo = bcm_sk(sk);
1590
1591	/* remove bcm_ops, timer, rx_unregister(), etc. */
1592
1593	spin_lock(&bcm_notifier_lock);
1594	while (bcm_busy_notifier == bo) {
1595		spin_unlock(&bcm_notifier_lock);
1596		schedule_timeout_uninterruptible(1);
1597		spin_lock(&bcm_notifier_lock);
1598	}
1599	list_del(&bo->notifier);
1600	spin_unlock(&bcm_notifier_lock);
1601
1602	lock_sock(sk);
1603
1604#if IS_ENABLED(CONFIG_PROC_FS)
1605	/* remove procfs entry */
1606	if (net->can.bcmproc_dir && bo->bcm_proc_read)
1607		remove_proc_entry(bo->procname, net->can.bcmproc_dir);
1608#endif /* CONFIG_PROC_FS */
1609
1610	list_for_each_entry_safe(op, next, &bo->tx_ops, list)
1611		bcm_remove_op(op);
1612
1613	list_for_each_entry_safe(op, next, &bo->rx_ops, list) {
1614		/*
1615		 * Don't care if we're bound or not (due to netdev problems)
1616		 * can_rx_unregister() is always a save thing to do here.
1617		 */
1618		if (op->ifindex) {
1619			/*
1620			 * Only remove subscriptions that had not
1621			 * been removed due to NETDEV_UNREGISTER
1622			 * in bcm_notifier()
1623			 */
1624			if (op->rx_reg_dev) {
1625				struct net_device *dev;
1626
1627				dev = dev_get_by_index(net, op->ifindex);
1628				if (dev) {
1629					bcm_rx_unreg(dev, op);
1630					dev_put(dev);
1631				}
1632			}
1633		} else
1634			can_rx_unregister(net, NULL, op->can_id,
1635					  REGMASK(op->can_id),
1636					  bcm_rx_handler, op);
1637
1638	}
1639
1640	synchronize_rcu();
1641
1642	list_for_each_entry_safe(op, next, &bo->rx_ops, list)
1643		bcm_remove_op(op);
1644
1645	/* remove device reference */
1646	if (bo->bound) {
1647		bo->bound   = 0;
1648		bo->ifindex = 0;
1649	}
1650
1651	sock_orphan(sk);
1652	sock->sk = NULL;
1653
1654	release_sock(sk);
1655	sock_prot_inuse_add(net, sk->sk_prot, -1);
1656	sock_put(sk);
1657
1658	return 0;
1659}
1660
1661static int bcm_connect(struct socket *sock, struct sockaddr_unsized *uaddr, int len,
1662		       int flags)
1663{
1664	struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
1665	struct sock *sk = sock->sk;
1666	struct bcm_sock *bo = bcm_sk(sk);
1667	struct net *net = sock_net(sk);
1668	int ret = 0;
1669
1670	if (len < BCM_MIN_NAMELEN)
1671		return -EINVAL;
1672
1673	lock_sock(sk);
1674
1675	if (bo->bound) {
1676		ret = -EISCONN;
1677		goto fail;
1678	}
1679
1680	/* bind a device to this socket */
1681	if (addr->can_ifindex) {
1682		struct net_device *dev;
1683
1684		dev = dev_get_by_index(net, addr->can_ifindex);
1685		if (!dev) {
1686			ret = -ENODEV;
1687			goto fail;
1688		}
1689		if (dev->type != ARPHRD_CAN) {
1690			dev_put(dev);
1691			ret = -ENODEV;
1692			goto fail;
1693		}
1694
1695		bo->ifindex = dev->ifindex;
1696		dev_put(dev);
1697
1698	} else {
1699		/* no interface reference for ifindex = 0 ('any' CAN device) */
1700		bo->ifindex = 0;
1701	}
1702
1703#if IS_ENABLED(CONFIG_PROC_FS)
1704	if (net->can.bcmproc_dir) {
1705		/* unique socket address as filename */
1706		sprintf(bo->procname, "%lu", sock_i_ino(sk));
1707		bo->bcm_proc_read = proc_create_net_single(bo->procname, 0644,
1708						     net->can.bcmproc_dir,
1709						     bcm_proc_show, sk);
1710		if (!bo->bcm_proc_read) {
1711			ret = -ENOMEM;
1712			goto fail;
1713		}
1714	}
1715#endif /* CONFIG_PROC_FS */
1716
1717	bo->bound = 1;
1718
1719fail:
1720	release_sock(sk);
1721
1722	return ret;
1723}
1724
1725static int bcm_recvmsg(struct socket *sock, struct msghdr *msg, size_t size,
1726		       int flags)
1727{
1728	struct sock *sk = sock->sk;
1729	struct sk_buff *skb;
1730	int error = 0;
1731	int err;
1732
1733	skb = skb_recv_datagram(sk, flags, &error);
1734	if (!skb)
1735		return error;
1736
1737	if (skb->len < size)
1738		size = skb->len;
1739
1740	err = memcpy_to_msg(msg, skb->data, size);
1741	if (err < 0) {
1742		skb_free_datagram(sk, skb);
1743		return err;
1744	}
1745
1746	sock_recv_cmsgs(msg, sk, skb);
1747
1748	if (msg->msg_name) {
1749		__sockaddr_check_size(BCM_MIN_NAMELEN);
1750		msg->msg_namelen = BCM_MIN_NAMELEN;
1751		memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
1752	}
1753
1754	/* assign the flags that have been recorded in bcm_send_to_user() */
1755	msg->msg_flags |= *(bcm_flags(skb));
1756
1757	skb_free_datagram(sk, skb);
1758
1759	return size;
1760}
1761
1762static int bcm_sock_no_ioctlcmd(struct socket *sock, unsigned int cmd,
1763				unsigned long arg)
1764{
1765	/* no ioctls for socket layer -> hand it down to NIC layer */
1766	return -ENOIOCTLCMD;
1767}
1768
1769static const struct proto_ops bcm_ops = {
1770	.family        = PF_CAN,
1771	.release       = bcm_release,
1772	.bind          = sock_no_bind,
1773	.connect       = bcm_connect,
1774	.socketpair    = sock_no_socketpair,
1775	.accept        = sock_no_accept,
1776	.getname       = sock_no_getname,
1777	.poll          = datagram_poll,
1778	.ioctl         = bcm_sock_no_ioctlcmd,
1779	.gettstamp     = sock_gettstamp,
1780	.listen        = sock_no_listen,
1781	.shutdown      = sock_no_shutdown,
1782	.sendmsg       = bcm_sendmsg,
1783	.recvmsg       = bcm_recvmsg,
1784	.mmap          = sock_no_mmap,
1785};
1786
1787static struct proto bcm_proto __read_mostly = {
1788	.name       = "CAN_BCM",
1789	.owner      = THIS_MODULE,
1790	.obj_size   = sizeof(struct bcm_sock),
1791	.init       = bcm_init,
1792};
1793
1794static const struct can_proto bcm_can_proto = {
1795	.type       = SOCK_DGRAM,
1796	.protocol   = CAN_BCM,
1797	.ops        = &bcm_ops,
1798	.prot       = &bcm_proto,
1799};
1800
1801static int canbcm_pernet_init(struct net *net)
1802{
1803#if IS_ENABLED(CONFIG_PROC_FS)
1804	/* create /proc/net/can-bcm directory */
1805	net->can.bcmproc_dir = proc_net_mkdir(net, "can-bcm", net->proc_net);
1806#endif /* CONFIG_PROC_FS */
1807
1808	return 0;
1809}
1810
1811static void canbcm_pernet_exit(struct net *net)
1812{
1813#if IS_ENABLED(CONFIG_PROC_FS)
1814	/* remove /proc/net/can-bcm directory */
1815	if (net->can.bcmproc_dir)
1816		remove_proc_entry("can-bcm", net->proc_net);
1817#endif /* CONFIG_PROC_FS */
1818}
1819
1820static struct pernet_operations canbcm_pernet_ops __read_mostly = {
1821	.init = canbcm_pernet_init,
1822	.exit = canbcm_pernet_exit,
1823};
1824
1825static struct notifier_block canbcm_notifier = {
1826	.notifier_call = bcm_notifier
1827};
1828
1829static int __init bcm_module_init(void)
1830{
1831	int err;
1832
1833	pr_info("can: broadcast manager protocol\n");
1834
1835	err = register_pernet_subsys(&canbcm_pernet_ops);
1836	if (err)
1837		return err;
1838
1839	err = register_netdevice_notifier(&canbcm_notifier);
1840	if (err)
1841		goto register_notifier_failed;
1842
1843	err = can_proto_register(&bcm_can_proto);
1844	if (err < 0) {
1845		printk(KERN_ERR "can: registration of bcm protocol failed\n");
1846		goto register_proto_failed;
1847	}
1848
1849	return 0;
1850
1851register_proto_failed:
1852	unregister_netdevice_notifier(&canbcm_notifier);
1853register_notifier_failed:
1854	unregister_pernet_subsys(&canbcm_pernet_ops);
1855	return err;
1856}
1857
1858static void __exit bcm_module_exit(void)
1859{
1860	can_proto_unregister(&bcm_can_proto);
1861	unregister_netdevice_notifier(&canbcm_notifier);
1862	unregister_pernet_subsys(&canbcm_pernet_ops);
1863}
1864
1865module_init(bcm_module_init);
1866module_exit(bcm_module_exit);