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