xref: /openbmc/linux/net/can/bcm.c (revision 22246614)
1 /*
2  * bcm.c - Broadcast Manager to filter/send (cyclic) CAN content
3  *
4  * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  * 1. Redistributions of source code must retain the above copyright
11  *    notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  *    notice, this list of conditions and the following disclaimer in the
14  *    documentation and/or other materials provided with the distribution.
15  * 3. Neither the name of Volkswagen nor the names of its contributors
16  *    may be used to endorse or promote products derived from this software
17  *    without specific prior written permission.
18  *
19  * Alternatively, provided that this notice is retained in full, this
20  * software may be distributed under the terms of the GNU General
21  * Public License ("GPL") version 2, in which case the provisions of the
22  * GPL apply INSTEAD OF those given above.
23  *
24  * The provided data structures and external interfaces from this code
25  * are not restricted to be used by modules with a GPL compatible license.
26  *
27  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
28  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
29  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
30  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
31  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
32  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
33  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
34  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
35  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
36  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
37  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
38  * DAMAGE.
39  *
40  * Send feedback to <socketcan-users@lists.berlios.de>
41  *
42  */
43 
44 #include <linux/module.h>
45 #include <linux/init.h>
46 #include <linux/hrtimer.h>
47 #include <linux/list.h>
48 #include <linux/proc_fs.h>
49 #include <linux/uio.h>
50 #include <linux/net.h>
51 #include <linux/netdevice.h>
52 #include <linux/socket.h>
53 #include <linux/if_arp.h>
54 #include <linux/skbuff.h>
55 #include <linux/can.h>
56 #include <linux/can/core.h>
57 #include <linux/can/bcm.h>
58 #include <net/sock.h>
59 #include <net/net_namespace.h>
60 
61 /* use of last_frames[index].can_dlc */
62 #define RX_RECV    0x40 /* received data for this element */
63 #define RX_THR     0x80 /* element not been sent due to throttle feature */
64 #define BCM_CAN_DLC_MASK 0x0F /* clean private flags in can_dlc by masking */
65 
66 /* get best masking value for can_rx_register() for a given single can_id */
67 #define REGMASK(id) ((id & CAN_RTR_FLAG) | ((id & CAN_EFF_FLAG) ? \
68 			(CAN_EFF_MASK | CAN_EFF_FLAG) : CAN_SFF_MASK))
69 
70 #define CAN_BCM_VERSION "20080415"
71 static __initdata const char banner[] = KERN_INFO
72 	"can: broadcast manager protocol (rev " CAN_BCM_VERSION ")\n";
73 
74 MODULE_DESCRIPTION("PF_CAN broadcast manager protocol");
75 MODULE_LICENSE("Dual BSD/GPL");
76 MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
77 
78 /* easy access to can_frame payload */
79 static inline u64 GET_U64(const struct can_frame *cp)
80 {
81 	return *(u64 *)cp->data;
82 }
83 
84 struct bcm_op {
85 	struct list_head list;
86 	int ifindex;
87 	canid_t can_id;
88 	int flags;
89 	unsigned long frames_abs, frames_filtered;
90 	struct timeval ival1, ival2;
91 	struct hrtimer timer, thrtimer;
92 	ktime_t rx_stamp, kt_ival1, kt_ival2, kt_lastmsg;
93 	int rx_ifindex;
94 	int count;
95 	int nframes;
96 	int currframe;
97 	struct can_frame *frames;
98 	struct can_frame *last_frames;
99 	struct can_frame sframe;
100 	struct can_frame last_sframe;
101 	struct sock *sk;
102 	struct net_device *rx_reg_dev;
103 };
104 
105 static struct proc_dir_entry *proc_dir;
106 
107 struct bcm_sock {
108 	struct sock sk;
109 	int bound;
110 	int ifindex;
111 	struct notifier_block notifier;
112 	struct list_head rx_ops;
113 	struct list_head tx_ops;
114 	unsigned long dropped_usr_msgs;
115 	struct proc_dir_entry *bcm_proc_read;
116 	char procname [9]; /* pointer printed in ASCII with \0 */
117 };
118 
119 static inline struct bcm_sock *bcm_sk(const struct sock *sk)
120 {
121 	return (struct bcm_sock *)sk;
122 }
123 
124 #define CFSIZ sizeof(struct can_frame)
125 #define OPSIZ sizeof(struct bcm_op)
126 #define MHSIZ sizeof(struct bcm_msg_head)
127 
128 /*
129  * procfs functions
130  */
131 static char *bcm_proc_getifname(int ifindex)
132 {
133 	struct net_device *dev;
134 
135 	if (!ifindex)
136 		return "any";
137 
138 	/* no usage counting */
139 	dev = __dev_get_by_index(&init_net, ifindex);
140 	if (dev)
141 		return dev->name;
142 
143 	return "???";
144 }
145 
146 static int bcm_read_proc(char *page, char **start, off_t off,
147 			 int count, int *eof, void *data)
148 {
149 	int len = 0;
150 	struct sock *sk = (struct sock *)data;
151 	struct bcm_sock *bo = bcm_sk(sk);
152 	struct bcm_op *op;
153 
154 	len += snprintf(page + len, PAGE_SIZE - len, ">>> socket %p",
155 			sk->sk_socket);
156 	len += snprintf(page + len, PAGE_SIZE - len, " / sk %p", sk);
157 	len += snprintf(page + len, PAGE_SIZE - len, " / bo %p", bo);
158 	len += snprintf(page + len, PAGE_SIZE - len, " / dropped %lu",
159 			bo->dropped_usr_msgs);
160 	len += snprintf(page + len, PAGE_SIZE - len, " / bound %s",
161 			bcm_proc_getifname(bo->ifindex));
162 	len += snprintf(page + len, PAGE_SIZE - len, " <<<\n");
163 
164 	list_for_each_entry(op, &bo->rx_ops, list) {
165 
166 		unsigned long reduction;
167 
168 		/* print only active entries & prevent division by zero */
169 		if (!op->frames_abs)
170 			continue;
171 
172 		len += snprintf(page + len, PAGE_SIZE - len,
173 				"rx_op: %03X %-5s ",
174 				op->can_id, bcm_proc_getifname(op->ifindex));
175 		len += snprintf(page + len, PAGE_SIZE - len, "[%d]%c ",
176 				op->nframes,
177 				(op->flags & RX_CHECK_DLC)?'d':' ');
178 		if (op->kt_ival1.tv64)
179 			len += snprintf(page + len, PAGE_SIZE - len,
180 					"timeo=%lld ",
181 					(long long)
182 					ktime_to_us(op->kt_ival1));
183 
184 		if (op->kt_ival2.tv64)
185 			len += snprintf(page + len, PAGE_SIZE - len,
186 					"thr=%lld ",
187 					(long long)
188 					ktime_to_us(op->kt_ival2));
189 
190 		len += snprintf(page + len, PAGE_SIZE - len,
191 				"# recv %ld (%ld) => reduction: ",
192 				op->frames_filtered, op->frames_abs);
193 
194 		reduction = 100 - (op->frames_filtered * 100) / op->frames_abs;
195 
196 		len += snprintf(page + len, PAGE_SIZE - len, "%s%ld%%\n",
197 				(reduction == 100)?"near ":"", reduction);
198 
199 		if (len > PAGE_SIZE - 200) {
200 			/* mark output cut off */
201 			len += snprintf(page + len, PAGE_SIZE - len, "(..)\n");
202 			break;
203 		}
204 	}
205 
206 	list_for_each_entry(op, &bo->tx_ops, list) {
207 
208 		len += snprintf(page + len, PAGE_SIZE - len,
209 				"tx_op: %03X %s [%d] ",
210 				op->can_id, bcm_proc_getifname(op->ifindex),
211 				op->nframes);
212 
213 		if (op->kt_ival1.tv64)
214 			len += snprintf(page + len, PAGE_SIZE - len, "t1=%lld ",
215 					(long long) ktime_to_us(op->kt_ival1));
216 
217 		if (op->kt_ival2.tv64)
218 			len += snprintf(page + len, PAGE_SIZE - len, "t2=%lld ",
219 					(long long) ktime_to_us(op->kt_ival2));
220 
221 		len += snprintf(page + len, PAGE_SIZE - len, "# sent %ld\n",
222 				op->frames_abs);
223 
224 		if (len > PAGE_SIZE - 100) {
225 			/* mark output cut off */
226 			len += snprintf(page + len, PAGE_SIZE - len, "(..)\n");
227 			break;
228 		}
229 	}
230 
231 	len += snprintf(page + len, PAGE_SIZE - len, "\n");
232 
233 	*eof = 1;
234 	return len;
235 }
236 
237 /*
238  * bcm_can_tx - send the (next) CAN frame to the appropriate CAN interface
239  *              of the given bcm tx op
240  */
241 static void bcm_can_tx(struct bcm_op *op)
242 {
243 	struct sk_buff *skb;
244 	struct net_device *dev;
245 	struct can_frame *cf = &op->frames[op->currframe];
246 
247 	/* no target device? => exit */
248 	if (!op->ifindex)
249 		return;
250 
251 	dev = dev_get_by_index(&init_net, op->ifindex);
252 	if (!dev) {
253 		/* RFC: should this bcm_op remove itself here? */
254 		return;
255 	}
256 
257 	skb = alloc_skb(CFSIZ, gfp_any());
258 	if (!skb)
259 		goto out;
260 
261 	memcpy(skb_put(skb, CFSIZ), cf, CFSIZ);
262 
263 	/* send with loopback */
264 	skb->dev = dev;
265 	skb->sk = op->sk;
266 	can_send(skb, 1);
267 
268 	/* update statistics */
269 	op->currframe++;
270 	op->frames_abs++;
271 
272 	/* reached last frame? */
273 	if (op->currframe >= op->nframes)
274 		op->currframe = 0;
275  out:
276 	dev_put(dev);
277 }
278 
279 /*
280  * bcm_send_to_user - send a BCM message to the userspace
281  *                    (consisting of bcm_msg_head + x CAN frames)
282  */
283 static void bcm_send_to_user(struct bcm_op *op, struct bcm_msg_head *head,
284 			     struct can_frame *frames, int has_timestamp)
285 {
286 	struct sk_buff *skb;
287 	struct can_frame *firstframe;
288 	struct sockaddr_can *addr;
289 	struct sock *sk = op->sk;
290 	int datalen = head->nframes * CFSIZ;
291 	int err;
292 
293 	skb = alloc_skb(sizeof(*head) + datalen, gfp_any());
294 	if (!skb)
295 		return;
296 
297 	memcpy(skb_put(skb, sizeof(*head)), head, sizeof(*head));
298 
299 	if (head->nframes) {
300 		/* can_frames starting here */
301 		firstframe = (struct can_frame *) skb_tail_pointer(skb);
302 
303 		memcpy(skb_put(skb, datalen), frames, datalen);
304 
305 		/*
306 		 * the BCM uses the can_dlc-element of the can_frame
307 		 * structure for internal purposes. This is only
308 		 * relevant for updates that are generated by the
309 		 * BCM, where nframes is 1
310 		 */
311 		if (head->nframes == 1)
312 			firstframe->can_dlc &= BCM_CAN_DLC_MASK;
313 	}
314 
315 	if (has_timestamp) {
316 		/* restore rx timestamp */
317 		skb->tstamp = op->rx_stamp;
318 	}
319 
320 	/*
321 	 *  Put the datagram to the queue so that bcm_recvmsg() can
322 	 *  get it from there.  We need to pass the interface index to
323 	 *  bcm_recvmsg().  We pass a whole struct sockaddr_can in skb->cb
324 	 *  containing the interface index.
325 	 */
326 
327 	BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct sockaddr_can));
328 	addr = (struct sockaddr_can *)skb->cb;
329 	memset(addr, 0, sizeof(*addr));
330 	addr->can_family  = AF_CAN;
331 	addr->can_ifindex = op->rx_ifindex;
332 
333 	err = sock_queue_rcv_skb(sk, skb);
334 	if (err < 0) {
335 		struct bcm_sock *bo = bcm_sk(sk);
336 
337 		kfree_skb(skb);
338 		/* don't care about overflows in this statistic */
339 		bo->dropped_usr_msgs++;
340 	}
341 }
342 
343 /*
344  * bcm_tx_timeout_handler - performes cyclic CAN frame transmissions
345  */
346 static enum hrtimer_restart bcm_tx_timeout_handler(struct hrtimer *hrtimer)
347 {
348 	struct bcm_op *op = container_of(hrtimer, struct bcm_op, timer);
349 	enum hrtimer_restart ret = HRTIMER_NORESTART;
350 
351 	if (op->kt_ival1.tv64 && (op->count > 0)) {
352 
353 		op->count--;
354 		if (!op->count && (op->flags & TX_COUNTEVT)) {
355 			struct bcm_msg_head msg_head;
356 
357 			/* create notification to user */
358 			msg_head.opcode  = TX_EXPIRED;
359 			msg_head.flags   = op->flags;
360 			msg_head.count   = op->count;
361 			msg_head.ival1   = op->ival1;
362 			msg_head.ival2   = op->ival2;
363 			msg_head.can_id  = op->can_id;
364 			msg_head.nframes = 0;
365 
366 			bcm_send_to_user(op, &msg_head, NULL, 0);
367 		}
368 	}
369 
370 	if (op->kt_ival1.tv64 && (op->count > 0)) {
371 
372 		/* send (next) frame */
373 		bcm_can_tx(op);
374 		hrtimer_forward(hrtimer, ktime_get(), op->kt_ival1);
375 		ret = HRTIMER_RESTART;
376 
377 	} else {
378 		if (op->kt_ival2.tv64) {
379 
380 			/* send (next) frame */
381 			bcm_can_tx(op);
382 			hrtimer_forward(hrtimer, ktime_get(), op->kt_ival2);
383 			ret = HRTIMER_RESTART;
384 		}
385 	}
386 
387 	return ret;
388 }
389 
390 /*
391  * bcm_rx_changed - create a RX_CHANGED notification due to changed content
392  */
393 static void bcm_rx_changed(struct bcm_op *op, struct can_frame *data)
394 {
395 	struct bcm_msg_head head;
396 
397 	/* update statistics */
398 	op->frames_filtered++;
399 
400 	/* prevent statistics overflow */
401 	if (op->frames_filtered > ULONG_MAX/100)
402 		op->frames_filtered = op->frames_abs = 0;
403 
404 	head.opcode  = RX_CHANGED;
405 	head.flags   = op->flags;
406 	head.count   = op->count;
407 	head.ival1   = op->ival1;
408 	head.ival2   = op->ival2;
409 	head.can_id  = op->can_id;
410 	head.nframes = 1;
411 
412 	bcm_send_to_user(op, &head, data, 1);
413 }
414 
415 /*
416  * bcm_rx_update_and_send - process a detected relevant receive content change
417  *                          1. update the last received data
418  *                          2. send a notification to the user (if possible)
419  */
420 static void bcm_rx_update_and_send(struct bcm_op *op,
421 				   struct can_frame *lastdata,
422 				   struct can_frame *rxdata)
423 {
424 	memcpy(lastdata, rxdata, CFSIZ);
425 
426 	/* mark as used */
427 	lastdata->can_dlc |= RX_RECV;
428 
429 	/* throtteling mode inactive OR data update already on the run ? */
430 	if (!op->kt_ival2.tv64 || hrtimer_callback_running(&op->thrtimer)) {
431 		/* send RX_CHANGED to the user immediately */
432 		bcm_rx_changed(op, rxdata);
433 		return;
434 	}
435 
436 	if (hrtimer_active(&op->thrtimer)) {
437 		/* mark as 'throttled' */
438 		lastdata->can_dlc |= RX_THR;
439 		return;
440 	}
441 
442 	if (!op->kt_lastmsg.tv64) {
443 		/* send first RX_CHANGED to the user immediately */
444 		bcm_rx_changed(op, rxdata);
445 		op->kt_lastmsg = ktime_get();
446 		return;
447 	}
448 
449 	if (ktime_us_delta(ktime_get(), op->kt_lastmsg) <
450 	    ktime_to_us(op->kt_ival2)) {
451 		/* mark as 'throttled' and start timer */
452 		lastdata->can_dlc |= RX_THR;
453 		hrtimer_start(&op->thrtimer,
454 			      ktime_add(op->kt_lastmsg, op->kt_ival2),
455 			      HRTIMER_MODE_ABS);
456 		return;
457 	}
458 
459 	/* the gap was that big, that throttling was not needed here */
460 	bcm_rx_changed(op, rxdata);
461 	op->kt_lastmsg = ktime_get();
462 }
463 
464 /*
465  * bcm_rx_cmp_to_index - (bit)compares the currently received data to formerly
466  *                       received data stored in op->last_frames[]
467  */
468 static void bcm_rx_cmp_to_index(struct bcm_op *op, int index,
469 				struct can_frame *rxdata)
470 {
471 	/*
472 	 * no one uses the MSBs of can_dlc for comparation,
473 	 * so we use it here to detect the first time of reception
474 	 */
475 
476 	if (!(op->last_frames[index].can_dlc & RX_RECV)) {
477 		/* received data for the first time => send update to user */
478 		bcm_rx_update_and_send(op, &op->last_frames[index], rxdata);
479 		return;
480 	}
481 
482 	/* do a real check in can_frame data section */
483 
484 	if ((GET_U64(&op->frames[index]) & GET_U64(rxdata)) !=
485 	    (GET_U64(&op->frames[index]) & GET_U64(&op->last_frames[index]))) {
486 		bcm_rx_update_and_send(op, &op->last_frames[index], rxdata);
487 		return;
488 	}
489 
490 	if (op->flags & RX_CHECK_DLC) {
491 		/* do a real check in can_frame dlc */
492 		if (rxdata->can_dlc != (op->last_frames[index].can_dlc &
493 					BCM_CAN_DLC_MASK)) {
494 			bcm_rx_update_and_send(op, &op->last_frames[index],
495 					       rxdata);
496 			return;
497 		}
498 	}
499 }
500 
501 /*
502  * bcm_rx_starttimer - enable timeout monitoring for CAN frame receiption
503  */
504 static void bcm_rx_starttimer(struct bcm_op *op)
505 {
506 	if (op->flags & RX_NO_AUTOTIMER)
507 		return;
508 
509 	if (op->kt_ival1.tv64)
510 		hrtimer_start(&op->timer, op->kt_ival1, HRTIMER_MODE_REL);
511 }
512 
513 /*
514  * bcm_rx_timeout_handler - when the (cyclic) CAN frame receiption timed out
515  */
516 static enum hrtimer_restart bcm_rx_timeout_handler(struct hrtimer *hrtimer)
517 {
518 	struct bcm_op *op = container_of(hrtimer, struct bcm_op, timer);
519 	struct bcm_msg_head msg_head;
520 
521 	msg_head.opcode  = RX_TIMEOUT;
522 	msg_head.flags   = op->flags;
523 	msg_head.count   = op->count;
524 	msg_head.ival1   = op->ival1;
525 	msg_head.ival2   = op->ival2;
526 	msg_head.can_id  = op->can_id;
527 	msg_head.nframes = 0;
528 
529 	bcm_send_to_user(op, &msg_head, NULL, 0);
530 
531 	/* no restart of the timer is done here! */
532 
533 	/* if user wants to be informed, when cyclic CAN-Messages come back */
534 	if ((op->flags & RX_ANNOUNCE_RESUME) && op->last_frames) {
535 		/* clear received can_frames to indicate 'nothing received' */
536 		memset(op->last_frames, 0, op->nframes * CFSIZ);
537 	}
538 
539 	return HRTIMER_NORESTART;
540 }
541 
542 /*
543  * bcm_rx_thr_flush - Check for throttled data and send it to the userspace
544  */
545 static int bcm_rx_thr_flush(struct bcm_op *op)
546 {
547 	int updated = 0;
548 
549 	if (op->nframes > 1) {
550 		int i;
551 
552 		/* for MUX filter we start at index 1 */
553 		for (i = 1; i < op->nframes; i++) {
554 			if ((op->last_frames) &&
555 			    (op->last_frames[i].can_dlc & RX_THR)) {
556 				op->last_frames[i].can_dlc &= ~RX_THR;
557 				bcm_rx_changed(op, &op->last_frames[i]);
558 				updated++;
559 			}
560 		}
561 
562 	} else {
563 		/* for RX_FILTER_ID and simple filter */
564 		if (op->last_frames && (op->last_frames[0].can_dlc & RX_THR)) {
565 			op->last_frames[0].can_dlc &= ~RX_THR;
566 			bcm_rx_changed(op, &op->last_frames[0]);
567 			updated++;
568 		}
569 	}
570 
571 	return updated;
572 }
573 
574 /*
575  * bcm_rx_thr_handler - the time for blocked content updates is over now:
576  *                      Check for throttled data and send it to the userspace
577  */
578 static enum hrtimer_restart bcm_rx_thr_handler(struct hrtimer *hrtimer)
579 {
580 	struct bcm_op *op = container_of(hrtimer, struct bcm_op, thrtimer);
581 
582 	if (bcm_rx_thr_flush(op)) {
583 		hrtimer_forward(hrtimer, ktime_get(), op->kt_ival2);
584 		return HRTIMER_RESTART;
585 	} else {
586 		/* rearm throttle handling */
587 		op->kt_lastmsg = ktime_set(0, 0);
588 		return HRTIMER_NORESTART;
589 	}
590 }
591 
592 /*
593  * bcm_rx_handler - handle a CAN frame receiption
594  */
595 static void bcm_rx_handler(struct sk_buff *skb, void *data)
596 {
597 	struct bcm_op *op = (struct bcm_op *)data;
598 	struct can_frame rxframe;
599 	int i;
600 
601 	/* disable timeout */
602 	hrtimer_cancel(&op->timer);
603 
604 	if (skb->len == sizeof(rxframe)) {
605 		memcpy(&rxframe, skb->data, sizeof(rxframe));
606 		/* save rx timestamp */
607 		op->rx_stamp = skb->tstamp;
608 		/* save originator for recvfrom() */
609 		op->rx_ifindex = skb->dev->ifindex;
610 		/* update statistics */
611 		op->frames_abs++;
612 		kfree_skb(skb);
613 
614 	} else {
615 		kfree_skb(skb);
616 		return;
617 	}
618 
619 	if (op->can_id != rxframe.can_id)
620 		return;
621 
622 	if (op->flags & RX_RTR_FRAME) {
623 		/* send reply for RTR-request (placed in op->frames[0]) */
624 		bcm_can_tx(op);
625 		return;
626 	}
627 
628 	if (op->flags & RX_FILTER_ID) {
629 		/* the easiest case */
630 		bcm_rx_update_and_send(op, &op->last_frames[0], &rxframe);
631 		bcm_rx_starttimer(op);
632 		return;
633 	}
634 
635 	if (op->nframes == 1) {
636 		/* simple compare with index 0 */
637 		bcm_rx_cmp_to_index(op, 0, &rxframe);
638 		bcm_rx_starttimer(op);
639 		return;
640 	}
641 
642 	if (op->nframes > 1) {
643 		/*
644 		 * multiplex compare
645 		 *
646 		 * find the first multiplex mask that fits.
647 		 * Remark: The MUX-mask is stored in index 0
648 		 */
649 
650 		for (i = 1; i < op->nframes; i++) {
651 			if ((GET_U64(&op->frames[0]) & GET_U64(&rxframe)) ==
652 			    (GET_U64(&op->frames[0]) &
653 			     GET_U64(&op->frames[i]))) {
654 				bcm_rx_cmp_to_index(op, i, &rxframe);
655 				break;
656 			}
657 		}
658 		bcm_rx_starttimer(op);
659 	}
660 }
661 
662 /*
663  * helpers for bcm_op handling: find & delete bcm [rx|tx] op elements
664  */
665 static struct bcm_op *bcm_find_op(struct list_head *ops, canid_t can_id,
666 				  int ifindex)
667 {
668 	struct bcm_op *op;
669 
670 	list_for_each_entry(op, ops, list) {
671 		if ((op->can_id == can_id) && (op->ifindex == ifindex))
672 			return op;
673 	}
674 
675 	return NULL;
676 }
677 
678 static void bcm_remove_op(struct bcm_op *op)
679 {
680 	hrtimer_cancel(&op->timer);
681 	hrtimer_cancel(&op->thrtimer);
682 
683 	if ((op->frames) && (op->frames != &op->sframe))
684 		kfree(op->frames);
685 
686 	if ((op->last_frames) && (op->last_frames != &op->last_sframe))
687 		kfree(op->last_frames);
688 
689 	kfree(op);
690 
691 	return;
692 }
693 
694 static void bcm_rx_unreg(struct net_device *dev, struct bcm_op *op)
695 {
696 	if (op->rx_reg_dev == dev) {
697 		can_rx_unregister(dev, op->can_id, REGMASK(op->can_id),
698 				  bcm_rx_handler, op);
699 
700 		/* mark as removed subscription */
701 		op->rx_reg_dev = NULL;
702 	} else
703 		printk(KERN_ERR "can-bcm: bcm_rx_unreg: registered device "
704 		       "mismatch %p %p\n", op->rx_reg_dev, dev);
705 }
706 
707 /*
708  * bcm_delete_rx_op - find and remove a rx op (returns number of removed ops)
709  */
710 static int bcm_delete_rx_op(struct list_head *ops, canid_t can_id, int ifindex)
711 {
712 	struct bcm_op *op, *n;
713 
714 	list_for_each_entry_safe(op, n, ops, list) {
715 		if ((op->can_id == can_id) && (op->ifindex == ifindex)) {
716 
717 			/*
718 			 * Don't care if we're bound or not (due to netdev
719 			 * problems) can_rx_unregister() is always a save
720 			 * thing to do here.
721 			 */
722 			if (op->ifindex) {
723 				/*
724 				 * Only remove subscriptions that had not
725 				 * been removed due to NETDEV_UNREGISTER
726 				 * in bcm_notifier()
727 				 */
728 				if (op->rx_reg_dev) {
729 					struct net_device *dev;
730 
731 					dev = dev_get_by_index(&init_net,
732 							       op->ifindex);
733 					if (dev) {
734 						bcm_rx_unreg(dev, op);
735 						dev_put(dev);
736 					}
737 				}
738 			} else
739 				can_rx_unregister(NULL, op->can_id,
740 						  REGMASK(op->can_id),
741 						  bcm_rx_handler, op);
742 
743 			list_del(&op->list);
744 			bcm_remove_op(op);
745 			return 1; /* done */
746 		}
747 	}
748 
749 	return 0; /* not found */
750 }
751 
752 /*
753  * bcm_delete_tx_op - find and remove a tx op (returns number of removed ops)
754  */
755 static int bcm_delete_tx_op(struct list_head *ops, canid_t can_id, int ifindex)
756 {
757 	struct bcm_op *op, *n;
758 
759 	list_for_each_entry_safe(op, n, ops, list) {
760 		if ((op->can_id == can_id) && (op->ifindex == ifindex)) {
761 			list_del(&op->list);
762 			bcm_remove_op(op);
763 			return 1; /* done */
764 		}
765 	}
766 
767 	return 0; /* not found */
768 }
769 
770 /*
771  * bcm_read_op - read out a bcm_op and send it to the user (for bcm_sendmsg)
772  */
773 static int bcm_read_op(struct list_head *ops, struct bcm_msg_head *msg_head,
774 		       int ifindex)
775 {
776 	struct bcm_op *op = bcm_find_op(ops, msg_head->can_id, ifindex);
777 
778 	if (!op)
779 		return -EINVAL;
780 
781 	/* put current values into msg_head */
782 	msg_head->flags   = op->flags;
783 	msg_head->count   = op->count;
784 	msg_head->ival1   = op->ival1;
785 	msg_head->ival2   = op->ival2;
786 	msg_head->nframes = op->nframes;
787 
788 	bcm_send_to_user(op, msg_head, op->frames, 0);
789 
790 	return MHSIZ;
791 }
792 
793 /*
794  * bcm_tx_setup - create or update a bcm tx op (for bcm_sendmsg)
795  */
796 static int bcm_tx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg,
797 			int ifindex, struct sock *sk)
798 {
799 	struct bcm_sock *bo = bcm_sk(sk);
800 	struct bcm_op *op;
801 	int i, err;
802 
803 	/* we need a real device to send frames */
804 	if (!ifindex)
805 		return -ENODEV;
806 
807 	/* we need at least one can_frame */
808 	if (msg_head->nframes < 1)
809 		return -EINVAL;
810 
811 	/* check the given can_id */
812 	op = bcm_find_op(&bo->tx_ops, msg_head->can_id, ifindex);
813 
814 	if (op) {
815 		/* update existing BCM operation */
816 
817 		/*
818 		 * Do we need more space for the can_frames than currently
819 		 * allocated? -> This is a _really_ unusual use-case and
820 		 * therefore (complexity / locking) it is not supported.
821 		 */
822 		if (msg_head->nframes > op->nframes)
823 			return -E2BIG;
824 
825 		/* update can_frames content */
826 		for (i = 0; i < msg_head->nframes; i++) {
827 			err = memcpy_fromiovec((u8 *)&op->frames[i],
828 					       msg->msg_iov, CFSIZ);
829 			if (err < 0)
830 				return err;
831 
832 			if (msg_head->flags & TX_CP_CAN_ID) {
833 				/* copy can_id into frame */
834 				op->frames[i].can_id = msg_head->can_id;
835 			}
836 		}
837 
838 	} else {
839 		/* insert new BCM operation for the given can_id */
840 
841 		op = kzalloc(OPSIZ, GFP_KERNEL);
842 		if (!op)
843 			return -ENOMEM;
844 
845 		op->can_id    = msg_head->can_id;
846 
847 		/* create array for can_frames and copy the data */
848 		if (msg_head->nframes > 1) {
849 			op->frames = kmalloc(msg_head->nframes * CFSIZ,
850 					     GFP_KERNEL);
851 			if (!op->frames) {
852 				kfree(op);
853 				return -ENOMEM;
854 			}
855 		} else
856 			op->frames = &op->sframe;
857 
858 		for (i = 0; i < msg_head->nframes; i++) {
859 			err = memcpy_fromiovec((u8 *)&op->frames[i],
860 					       msg->msg_iov, CFSIZ);
861 			if (err < 0) {
862 				if (op->frames != &op->sframe)
863 					kfree(op->frames);
864 				kfree(op);
865 				return err;
866 			}
867 
868 			if (msg_head->flags & TX_CP_CAN_ID) {
869 				/* copy can_id into frame */
870 				op->frames[i].can_id = msg_head->can_id;
871 			}
872 		}
873 
874 		/* tx_ops never compare with previous received messages */
875 		op->last_frames = NULL;
876 
877 		/* bcm_can_tx / bcm_tx_timeout_handler needs this */
878 		op->sk = sk;
879 		op->ifindex = ifindex;
880 
881 		/* initialize uninitialized (kzalloc) structure */
882 		hrtimer_init(&op->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
883 		op->timer.function = bcm_tx_timeout_handler;
884 
885 		/* currently unused in tx_ops */
886 		hrtimer_init(&op->thrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
887 
888 		/* add this bcm_op to the list of the tx_ops */
889 		list_add(&op->list, &bo->tx_ops);
890 
891 	} /* if ((op = bcm_find_op(&bo->tx_ops, msg_head->can_id, ifindex))) */
892 
893 	if (op->nframes != msg_head->nframes) {
894 		op->nframes   = msg_head->nframes;
895 		/* start multiple frame transmission with index 0 */
896 		op->currframe = 0;
897 	}
898 
899 	/* check flags */
900 
901 	op->flags = msg_head->flags;
902 
903 	if (op->flags & TX_RESET_MULTI_IDX) {
904 		/* start multiple frame transmission with index 0 */
905 		op->currframe = 0;
906 	}
907 
908 	if (op->flags & SETTIMER) {
909 		/* set timer values */
910 		op->count = msg_head->count;
911 		op->ival1 = msg_head->ival1;
912 		op->ival2 = msg_head->ival2;
913 		op->kt_ival1 = timeval_to_ktime(msg_head->ival1);
914 		op->kt_ival2 = timeval_to_ktime(msg_head->ival2);
915 
916 		/* disable an active timer due to zero values? */
917 		if (!op->kt_ival1.tv64 && !op->kt_ival2.tv64)
918 			hrtimer_cancel(&op->timer);
919 	}
920 
921 	if ((op->flags & STARTTIMER) &&
922 	    ((op->kt_ival1.tv64 && op->count) || op->kt_ival2.tv64)) {
923 
924 		/* spec: send can_frame when starting timer */
925 		op->flags |= TX_ANNOUNCE;
926 
927 		if (op->kt_ival1.tv64 && (op->count > 0)) {
928 			/* op->count-- is done in bcm_tx_timeout_handler */
929 			hrtimer_start(&op->timer, op->kt_ival1,
930 				      HRTIMER_MODE_REL);
931 		} else
932 			hrtimer_start(&op->timer, op->kt_ival2,
933 				      HRTIMER_MODE_REL);
934 	}
935 
936 	if (op->flags & TX_ANNOUNCE)
937 		bcm_can_tx(op);
938 
939 	return msg_head->nframes * CFSIZ + MHSIZ;
940 }
941 
942 /*
943  * bcm_rx_setup - create or update a bcm rx op (for bcm_sendmsg)
944  */
945 static int bcm_rx_setup(struct bcm_msg_head *msg_head, struct msghdr *msg,
946 			int ifindex, struct sock *sk)
947 {
948 	struct bcm_sock *bo = bcm_sk(sk);
949 	struct bcm_op *op;
950 	int do_rx_register;
951 	int err = 0;
952 
953 	if ((msg_head->flags & RX_FILTER_ID) || (!(msg_head->nframes))) {
954 		/* be robust against wrong usage ... */
955 		msg_head->flags |= RX_FILTER_ID;
956 		/* ignore trailing garbage */
957 		msg_head->nframes = 0;
958 	}
959 
960 	if ((msg_head->flags & RX_RTR_FRAME) &&
961 	    ((msg_head->nframes != 1) ||
962 	     (!(msg_head->can_id & CAN_RTR_FLAG))))
963 		return -EINVAL;
964 
965 	/* check the given can_id */
966 	op = bcm_find_op(&bo->rx_ops, msg_head->can_id, ifindex);
967 	if (op) {
968 		/* update existing BCM operation */
969 
970 		/*
971 		 * Do we need more space for the can_frames than currently
972 		 * allocated? -> This is a _really_ unusual use-case and
973 		 * therefore (complexity / locking) it is not supported.
974 		 */
975 		if (msg_head->nframes > op->nframes)
976 			return -E2BIG;
977 
978 		if (msg_head->nframes) {
979 			/* update can_frames content */
980 			err = memcpy_fromiovec((u8 *)op->frames,
981 					       msg->msg_iov,
982 					       msg_head->nframes * CFSIZ);
983 			if (err < 0)
984 				return err;
985 
986 			/* clear last_frames to indicate 'nothing received' */
987 			memset(op->last_frames, 0, msg_head->nframes * CFSIZ);
988 		}
989 
990 		op->nframes = msg_head->nframes;
991 
992 		/* Only an update -> do not call can_rx_register() */
993 		do_rx_register = 0;
994 
995 	} else {
996 		/* insert new BCM operation for the given can_id */
997 		op = kzalloc(OPSIZ, GFP_KERNEL);
998 		if (!op)
999 			return -ENOMEM;
1000 
1001 		op->can_id    = msg_head->can_id;
1002 		op->nframes   = msg_head->nframes;
1003 
1004 		if (msg_head->nframes > 1) {
1005 			/* create array for can_frames and copy the data */
1006 			op->frames = kmalloc(msg_head->nframes * CFSIZ,
1007 					     GFP_KERNEL);
1008 			if (!op->frames) {
1009 				kfree(op);
1010 				return -ENOMEM;
1011 			}
1012 
1013 			/* create and init array for received can_frames */
1014 			op->last_frames = kzalloc(msg_head->nframes * CFSIZ,
1015 						  GFP_KERNEL);
1016 			if (!op->last_frames) {
1017 				kfree(op->frames);
1018 				kfree(op);
1019 				return -ENOMEM;
1020 			}
1021 
1022 		} else {
1023 			op->frames = &op->sframe;
1024 			op->last_frames = &op->last_sframe;
1025 		}
1026 
1027 		if (msg_head->nframes) {
1028 			err = memcpy_fromiovec((u8 *)op->frames, msg->msg_iov,
1029 					       msg_head->nframes * CFSIZ);
1030 			if (err < 0) {
1031 				if (op->frames != &op->sframe)
1032 					kfree(op->frames);
1033 				if (op->last_frames != &op->last_sframe)
1034 					kfree(op->last_frames);
1035 				kfree(op);
1036 				return err;
1037 			}
1038 		}
1039 
1040 		/* bcm_can_tx / bcm_tx_timeout_handler needs this */
1041 		op->sk = sk;
1042 		op->ifindex = ifindex;
1043 
1044 		/* initialize uninitialized (kzalloc) structure */
1045 		hrtimer_init(&op->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
1046 		op->timer.function = bcm_rx_timeout_handler;
1047 
1048 		hrtimer_init(&op->thrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
1049 		op->thrtimer.function = bcm_rx_thr_handler;
1050 
1051 		/* add this bcm_op to the list of the rx_ops */
1052 		list_add(&op->list, &bo->rx_ops);
1053 
1054 		/* call can_rx_register() */
1055 		do_rx_register = 1;
1056 
1057 	} /* if ((op = bcm_find_op(&bo->rx_ops, msg_head->can_id, ifindex))) */
1058 
1059 	/* check flags */
1060 	op->flags = msg_head->flags;
1061 
1062 	if (op->flags & RX_RTR_FRAME) {
1063 
1064 		/* no timers in RTR-mode */
1065 		hrtimer_cancel(&op->thrtimer);
1066 		hrtimer_cancel(&op->timer);
1067 
1068 		/*
1069 		 * funny feature in RX(!)_SETUP only for RTR-mode:
1070 		 * copy can_id into frame BUT without RTR-flag to
1071 		 * prevent a full-load-loopback-test ... ;-]
1072 		 */
1073 		if ((op->flags & TX_CP_CAN_ID) ||
1074 		    (op->frames[0].can_id == op->can_id))
1075 			op->frames[0].can_id = op->can_id & ~CAN_RTR_FLAG;
1076 
1077 	} else {
1078 		if (op->flags & SETTIMER) {
1079 
1080 			/* set timer value */
1081 			op->ival1 = msg_head->ival1;
1082 			op->ival2 = msg_head->ival2;
1083 			op->kt_ival1 = timeval_to_ktime(msg_head->ival1);
1084 			op->kt_ival2 = timeval_to_ktime(msg_head->ival2);
1085 
1086 			/* disable an active timer due to zero value? */
1087 			if (!op->kt_ival1.tv64)
1088 				hrtimer_cancel(&op->timer);
1089 
1090 			/*
1091 			 * In any case cancel the throttle timer, flush
1092 			 * potentially blocked msgs and reset throttle handling
1093 			 */
1094 			op->kt_lastmsg = ktime_set(0, 0);
1095 			hrtimer_cancel(&op->thrtimer);
1096 			bcm_rx_thr_flush(op);
1097 		}
1098 
1099 		if ((op->flags & STARTTIMER) && op->kt_ival1.tv64)
1100 			hrtimer_start(&op->timer, op->kt_ival1,
1101 				      HRTIMER_MODE_REL);
1102 	}
1103 
1104 	/* now we can register for can_ids, if we added a new bcm_op */
1105 	if (do_rx_register) {
1106 		if (ifindex) {
1107 			struct net_device *dev;
1108 
1109 			dev = dev_get_by_index(&init_net, ifindex);
1110 			if (dev) {
1111 				err = can_rx_register(dev, op->can_id,
1112 						      REGMASK(op->can_id),
1113 						      bcm_rx_handler, op,
1114 						      "bcm");
1115 
1116 				op->rx_reg_dev = dev;
1117 				dev_put(dev);
1118 			}
1119 
1120 		} else
1121 			err = can_rx_register(NULL, op->can_id,
1122 					      REGMASK(op->can_id),
1123 					      bcm_rx_handler, op, "bcm");
1124 		if (err) {
1125 			/* this bcm rx op is broken -> remove it */
1126 			list_del(&op->list);
1127 			bcm_remove_op(op);
1128 			return err;
1129 		}
1130 	}
1131 
1132 	return msg_head->nframes * CFSIZ + MHSIZ;
1133 }
1134 
1135 /*
1136  * bcm_tx_send - send a single CAN frame to the CAN interface (for bcm_sendmsg)
1137  */
1138 static int bcm_tx_send(struct msghdr *msg, int ifindex, struct sock *sk)
1139 {
1140 	struct sk_buff *skb;
1141 	struct net_device *dev;
1142 	int err;
1143 
1144 	/* we need a real device to send frames */
1145 	if (!ifindex)
1146 		return -ENODEV;
1147 
1148 	skb = alloc_skb(CFSIZ, GFP_KERNEL);
1149 
1150 	if (!skb)
1151 		return -ENOMEM;
1152 
1153 	err = memcpy_fromiovec(skb_put(skb, CFSIZ), msg->msg_iov, CFSIZ);
1154 	if (err < 0) {
1155 		kfree_skb(skb);
1156 		return err;
1157 	}
1158 
1159 	dev = dev_get_by_index(&init_net, ifindex);
1160 	if (!dev) {
1161 		kfree_skb(skb);
1162 		return -ENODEV;
1163 	}
1164 
1165 	skb->dev = dev;
1166 	skb->sk  = sk;
1167 	can_send(skb, 1); /* send with loopback */
1168 	dev_put(dev);
1169 
1170 	return CFSIZ + MHSIZ;
1171 }
1172 
1173 /*
1174  * bcm_sendmsg - process BCM commands (opcodes) from the userspace
1175  */
1176 static int bcm_sendmsg(struct kiocb *iocb, struct socket *sock,
1177 		       struct msghdr *msg, size_t size)
1178 {
1179 	struct sock *sk = sock->sk;
1180 	struct bcm_sock *bo = bcm_sk(sk);
1181 	int ifindex = bo->ifindex; /* default ifindex for this bcm_op */
1182 	struct bcm_msg_head msg_head;
1183 	int ret; /* read bytes or error codes as return value */
1184 
1185 	if (!bo->bound)
1186 		return -ENOTCONN;
1187 
1188 	/* check for alternative ifindex for this bcm_op */
1189 
1190 	if (!ifindex && msg->msg_name) {
1191 		/* no bound device as default => check msg_name */
1192 		struct sockaddr_can *addr =
1193 			(struct sockaddr_can *)msg->msg_name;
1194 
1195 		if (addr->can_family != AF_CAN)
1196 			return -EINVAL;
1197 
1198 		/* ifindex from sendto() */
1199 		ifindex = addr->can_ifindex;
1200 
1201 		if (ifindex) {
1202 			struct net_device *dev;
1203 
1204 			dev = dev_get_by_index(&init_net, ifindex);
1205 			if (!dev)
1206 				return -ENODEV;
1207 
1208 			if (dev->type != ARPHRD_CAN) {
1209 				dev_put(dev);
1210 				return -ENODEV;
1211 			}
1212 
1213 			dev_put(dev);
1214 		}
1215 	}
1216 
1217 	/* read message head information */
1218 
1219 	ret = memcpy_fromiovec((u8 *)&msg_head, msg->msg_iov, MHSIZ);
1220 	if (ret < 0)
1221 		return ret;
1222 
1223 	lock_sock(sk);
1224 
1225 	switch (msg_head.opcode) {
1226 
1227 	case TX_SETUP:
1228 		ret = bcm_tx_setup(&msg_head, msg, ifindex, sk);
1229 		break;
1230 
1231 	case RX_SETUP:
1232 		ret = bcm_rx_setup(&msg_head, msg, ifindex, sk);
1233 		break;
1234 
1235 	case TX_DELETE:
1236 		if (bcm_delete_tx_op(&bo->tx_ops, msg_head.can_id, ifindex))
1237 			ret = MHSIZ;
1238 		else
1239 			ret = -EINVAL;
1240 		break;
1241 
1242 	case RX_DELETE:
1243 		if (bcm_delete_rx_op(&bo->rx_ops, msg_head.can_id, ifindex))
1244 			ret = MHSIZ;
1245 		else
1246 			ret = -EINVAL;
1247 		break;
1248 
1249 	case TX_READ:
1250 		/* reuse msg_head for the reply to TX_READ */
1251 		msg_head.opcode  = TX_STATUS;
1252 		ret = bcm_read_op(&bo->tx_ops, &msg_head, ifindex);
1253 		break;
1254 
1255 	case RX_READ:
1256 		/* reuse msg_head for the reply to RX_READ */
1257 		msg_head.opcode  = RX_STATUS;
1258 		ret = bcm_read_op(&bo->rx_ops, &msg_head, ifindex);
1259 		break;
1260 
1261 	case TX_SEND:
1262 		/* we need at least one can_frame */
1263 		if (msg_head.nframes < 1)
1264 			ret = -EINVAL;
1265 		else
1266 			ret = bcm_tx_send(msg, ifindex, sk);
1267 		break;
1268 
1269 	default:
1270 		ret = -EINVAL;
1271 		break;
1272 	}
1273 
1274 	release_sock(sk);
1275 
1276 	return ret;
1277 }
1278 
1279 /*
1280  * notification handler for netdevice status changes
1281  */
1282 static int bcm_notifier(struct notifier_block *nb, unsigned long msg,
1283 			void *data)
1284 {
1285 	struct net_device *dev = (struct net_device *)data;
1286 	struct bcm_sock *bo = container_of(nb, struct bcm_sock, notifier);
1287 	struct sock *sk = &bo->sk;
1288 	struct bcm_op *op;
1289 	int notify_enodev = 0;
1290 
1291 	if (dev_net(dev) != &init_net)
1292 		return NOTIFY_DONE;
1293 
1294 	if (dev->type != ARPHRD_CAN)
1295 		return NOTIFY_DONE;
1296 
1297 	switch (msg) {
1298 
1299 	case NETDEV_UNREGISTER:
1300 		lock_sock(sk);
1301 
1302 		/* remove device specific receive entries */
1303 		list_for_each_entry(op, &bo->rx_ops, list)
1304 			if (op->rx_reg_dev == dev)
1305 				bcm_rx_unreg(dev, op);
1306 
1307 		/* remove device reference, if this is our bound device */
1308 		if (bo->bound && bo->ifindex == dev->ifindex) {
1309 			bo->bound   = 0;
1310 			bo->ifindex = 0;
1311 			notify_enodev = 1;
1312 		}
1313 
1314 		release_sock(sk);
1315 
1316 		if (notify_enodev) {
1317 			sk->sk_err = ENODEV;
1318 			if (!sock_flag(sk, SOCK_DEAD))
1319 				sk->sk_error_report(sk);
1320 		}
1321 		break;
1322 
1323 	case NETDEV_DOWN:
1324 		if (bo->bound && bo->ifindex == dev->ifindex) {
1325 			sk->sk_err = ENETDOWN;
1326 			if (!sock_flag(sk, SOCK_DEAD))
1327 				sk->sk_error_report(sk);
1328 		}
1329 	}
1330 
1331 	return NOTIFY_DONE;
1332 }
1333 
1334 /*
1335  * initial settings for all BCM sockets to be set at socket creation time
1336  */
1337 static int bcm_init(struct sock *sk)
1338 {
1339 	struct bcm_sock *bo = bcm_sk(sk);
1340 
1341 	bo->bound            = 0;
1342 	bo->ifindex          = 0;
1343 	bo->dropped_usr_msgs = 0;
1344 	bo->bcm_proc_read    = NULL;
1345 
1346 	INIT_LIST_HEAD(&bo->tx_ops);
1347 	INIT_LIST_HEAD(&bo->rx_ops);
1348 
1349 	/* set notifier */
1350 	bo->notifier.notifier_call = bcm_notifier;
1351 
1352 	register_netdevice_notifier(&bo->notifier);
1353 
1354 	return 0;
1355 }
1356 
1357 /*
1358  * standard socket functions
1359  */
1360 static int bcm_release(struct socket *sock)
1361 {
1362 	struct sock *sk = sock->sk;
1363 	struct bcm_sock *bo = bcm_sk(sk);
1364 	struct bcm_op *op, *next;
1365 
1366 	/* remove bcm_ops, timer, rx_unregister(), etc. */
1367 
1368 	unregister_netdevice_notifier(&bo->notifier);
1369 
1370 	lock_sock(sk);
1371 
1372 	list_for_each_entry_safe(op, next, &bo->tx_ops, list)
1373 		bcm_remove_op(op);
1374 
1375 	list_for_each_entry_safe(op, next, &bo->rx_ops, list) {
1376 		/*
1377 		 * Don't care if we're bound or not (due to netdev problems)
1378 		 * can_rx_unregister() is always a save thing to do here.
1379 		 */
1380 		if (op->ifindex) {
1381 			/*
1382 			 * Only remove subscriptions that had not
1383 			 * been removed due to NETDEV_UNREGISTER
1384 			 * in bcm_notifier()
1385 			 */
1386 			if (op->rx_reg_dev) {
1387 				struct net_device *dev;
1388 
1389 				dev = dev_get_by_index(&init_net, op->ifindex);
1390 				if (dev) {
1391 					bcm_rx_unreg(dev, op);
1392 					dev_put(dev);
1393 				}
1394 			}
1395 		} else
1396 			can_rx_unregister(NULL, op->can_id,
1397 					  REGMASK(op->can_id),
1398 					  bcm_rx_handler, op);
1399 
1400 		bcm_remove_op(op);
1401 	}
1402 
1403 	/* remove procfs entry */
1404 	if (proc_dir && bo->bcm_proc_read)
1405 		remove_proc_entry(bo->procname, proc_dir);
1406 
1407 	/* remove device reference */
1408 	if (bo->bound) {
1409 		bo->bound   = 0;
1410 		bo->ifindex = 0;
1411 	}
1412 
1413 	release_sock(sk);
1414 	sock_put(sk);
1415 
1416 	return 0;
1417 }
1418 
1419 static int bcm_connect(struct socket *sock, struct sockaddr *uaddr, int len,
1420 		       int flags)
1421 {
1422 	struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
1423 	struct sock *sk = sock->sk;
1424 	struct bcm_sock *bo = bcm_sk(sk);
1425 
1426 	if (bo->bound)
1427 		return -EISCONN;
1428 
1429 	/* bind a device to this socket */
1430 	if (addr->can_ifindex) {
1431 		struct net_device *dev;
1432 
1433 		dev = dev_get_by_index(&init_net, addr->can_ifindex);
1434 		if (!dev)
1435 			return -ENODEV;
1436 
1437 		if (dev->type != ARPHRD_CAN) {
1438 			dev_put(dev);
1439 			return -ENODEV;
1440 		}
1441 
1442 		bo->ifindex = dev->ifindex;
1443 		dev_put(dev);
1444 
1445 	} else {
1446 		/* no interface reference for ifindex = 0 ('any' CAN device) */
1447 		bo->ifindex = 0;
1448 	}
1449 
1450 	bo->bound = 1;
1451 
1452 	if (proc_dir) {
1453 		/* unique socket address as filename */
1454 		sprintf(bo->procname, "%p", sock);
1455 		bo->bcm_proc_read = create_proc_read_entry(bo->procname, 0644,
1456 							   proc_dir,
1457 							   bcm_read_proc, sk);
1458 	}
1459 
1460 	return 0;
1461 }
1462 
1463 static int bcm_recvmsg(struct kiocb *iocb, struct socket *sock,
1464 		       struct msghdr *msg, size_t size, int flags)
1465 {
1466 	struct sock *sk = sock->sk;
1467 	struct sk_buff *skb;
1468 	int error = 0;
1469 	int noblock;
1470 	int err;
1471 
1472 	noblock =  flags & MSG_DONTWAIT;
1473 	flags   &= ~MSG_DONTWAIT;
1474 	skb = skb_recv_datagram(sk, flags, noblock, &error);
1475 	if (!skb)
1476 		return error;
1477 
1478 	if (skb->len < size)
1479 		size = skb->len;
1480 
1481 	err = memcpy_toiovec(msg->msg_iov, skb->data, size);
1482 	if (err < 0) {
1483 		skb_free_datagram(sk, skb);
1484 		return err;
1485 	}
1486 
1487 	sock_recv_timestamp(msg, sk, skb);
1488 
1489 	if (msg->msg_name) {
1490 		msg->msg_namelen = sizeof(struct sockaddr_can);
1491 		memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
1492 	}
1493 
1494 	skb_free_datagram(sk, skb);
1495 
1496 	return size;
1497 }
1498 
1499 static struct proto_ops bcm_ops __read_mostly = {
1500 	.family        = PF_CAN,
1501 	.release       = bcm_release,
1502 	.bind          = sock_no_bind,
1503 	.connect       = bcm_connect,
1504 	.socketpair    = sock_no_socketpair,
1505 	.accept        = sock_no_accept,
1506 	.getname       = sock_no_getname,
1507 	.poll          = datagram_poll,
1508 	.ioctl         = NULL,		/* use can_ioctl() from af_can.c */
1509 	.listen        = sock_no_listen,
1510 	.shutdown      = sock_no_shutdown,
1511 	.setsockopt    = sock_no_setsockopt,
1512 	.getsockopt    = sock_no_getsockopt,
1513 	.sendmsg       = bcm_sendmsg,
1514 	.recvmsg       = bcm_recvmsg,
1515 	.mmap          = sock_no_mmap,
1516 	.sendpage      = sock_no_sendpage,
1517 };
1518 
1519 static struct proto bcm_proto __read_mostly = {
1520 	.name       = "CAN_BCM",
1521 	.owner      = THIS_MODULE,
1522 	.obj_size   = sizeof(struct bcm_sock),
1523 	.init       = bcm_init,
1524 };
1525 
1526 static struct can_proto bcm_can_proto __read_mostly = {
1527 	.type       = SOCK_DGRAM,
1528 	.protocol   = CAN_BCM,
1529 	.capability = -1,
1530 	.ops        = &bcm_ops,
1531 	.prot       = &bcm_proto,
1532 };
1533 
1534 static int __init bcm_module_init(void)
1535 {
1536 	int err;
1537 
1538 	printk(banner);
1539 
1540 	err = can_proto_register(&bcm_can_proto);
1541 	if (err < 0) {
1542 		printk(KERN_ERR "can: registration of bcm protocol failed\n");
1543 		return err;
1544 	}
1545 
1546 	/* create /proc/net/can-bcm directory */
1547 	proc_dir = proc_mkdir("can-bcm", init_net.proc_net);
1548 
1549 	if (proc_dir)
1550 		proc_dir->owner = THIS_MODULE;
1551 
1552 	return 0;
1553 }
1554 
1555 static void __exit bcm_module_exit(void)
1556 {
1557 	can_proto_unregister(&bcm_can_proto);
1558 
1559 	if (proc_dir)
1560 		proc_net_remove(&init_net, "can-bcm");
1561 }
1562 
1563 module_init(bcm_module_init);
1564 module_exit(bcm_module_exit);
1565