1 /*
2 * af_can.c - Protocol family CAN core module
3 * (used by different CAN protocol modules)
4 *
5 * Copyright (c) 2002-2007 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/stddef.h>
45 #include <linux/init.h>
46 #include <linux/kmod.h>
47 #include <linux/slab.h>
48 #include <linux/list.h>
49 #include <linux/spinlock.h>
50 #include <linux/rcupdate.h>
51 #include <linux/uaccess.h>
52 #include <linux/net.h>
53 #include <linux/netdevice.h>
54 #include <linux/socket.h>
55 #include <linux/if_ether.h>
56 #include <linux/if_arp.h>
57 #include <linux/skbuff.h>
58 #include <linux/can.h>
59 #include <linux/can/core.h>
60 #include <linux/can/skb.h>
61 #include <linux/ratelimit.h>
62 #include <net/net_namespace.h>
63 #include <net/sock.h>
64
65 #include "af_can.h"
66
67 static __initconst const char banner[] = KERN_INFO
68 "can: controller area network core (" CAN_VERSION_STRING ")\n";
69
70 MODULE_DESCRIPTION("Controller Area Network PF_CAN core");
71 MODULE_LICENSE("Dual BSD/GPL");
72 MODULE_AUTHOR("Urs Thuermann <urs.thuermann@volkswagen.de>, "
73 "Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
74
75 MODULE_ALIAS_NETPROTO(PF_CAN);
76
77 static int stats_timer __read_mostly = 1;
78 module_param(stats_timer, int, S_IRUGO);
79 MODULE_PARM_DESC(stats_timer, "enable timer for statistics (default:on)");
80
81 /* receive filters subscribed for 'all' CAN devices */
82 struct dev_rcv_lists can_rx_alldev_list;
83 static DEFINE_SPINLOCK(can_rcvlists_lock);
84
85 static struct kmem_cache *rcv_cache __read_mostly;
86
87 /* table of registered CAN protocols */
88 static const struct can_proto *proto_tab[CAN_NPROTO] __read_mostly;
89 static DEFINE_MUTEX(proto_tab_lock);
90
91 struct timer_list can_stattimer; /* timer for statistics update */
92 struct s_stats can_stats; /* packet statistics */
93 struct s_pstats can_pstats; /* receive list statistics */
94
95 /*
96 * af_can socket functions
97 */
98
can_ioctl(struct socket * sock,unsigned int cmd,unsigned long arg)99 int can_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
100 {
101 struct sock *sk = sock->sk;
102
103 switch (cmd) {
104
105 case SIOCGSTAMP:
106 return sock_get_timestamp(sk, (struct timeval __user *)arg);
107
108 default:
109 return -ENOIOCTLCMD;
110 }
111 }
112 EXPORT_SYMBOL(can_ioctl);
113
can_sock_destruct(struct sock * sk)114 static void can_sock_destruct(struct sock *sk)
115 {
116 skb_queue_purge(&sk->sk_receive_queue);
117 }
118
can_get_proto(int protocol)119 static const struct can_proto *can_get_proto(int protocol)
120 {
121 const struct can_proto *cp;
122
123 rcu_read_lock();
124 cp = rcu_dereference(proto_tab[protocol]);
125 if (cp && !try_module_get(cp->prot->owner))
126 cp = NULL;
127 rcu_read_unlock();
128
129 return cp;
130 }
131
can_put_proto(const struct can_proto * cp)132 static inline void can_put_proto(const struct can_proto *cp)
133 {
134 module_put(cp->prot->owner);
135 }
136
can_create(struct net * net,struct socket * sock,int protocol,int kern)137 static int can_create(struct net *net, struct socket *sock, int protocol,
138 int kern)
139 {
140 struct sock *sk;
141 const struct can_proto *cp;
142 int err = 0;
143
144 sock->state = SS_UNCONNECTED;
145
146 if (protocol < 0 || protocol >= CAN_NPROTO)
147 return -EINVAL;
148
149 if (!net_eq(net, &init_net))
150 return -EAFNOSUPPORT;
151
152 cp = can_get_proto(protocol);
153
154 #ifdef CONFIG_MODULES
155 if (!cp) {
156 /* try to load protocol module if kernel is modular */
157
158 err = request_module("can-proto-%d", protocol);
159
160 /*
161 * In case of error we only print a message but don't
162 * return the error code immediately. Below we will
163 * return -EPROTONOSUPPORT
164 */
165 if (err)
166 printk_ratelimited(KERN_ERR "can: request_module "
167 "(can-proto-%d) failed.\n", protocol);
168
169 cp = can_get_proto(protocol);
170 }
171 #endif
172
173 /* check for available protocol and correct usage */
174
175 if (!cp)
176 return -EPROTONOSUPPORT;
177
178 if (cp->type != sock->type) {
179 err = -EPROTOTYPE;
180 goto errout;
181 }
182
183 sock->ops = cp->ops;
184
185 sk = sk_alloc(net, PF_CAN, GFP_KERNEL, cp->prot);
186 if (!sk) {
187 err = -ENOMEM;
188 goto errout;
189 }
190
191 sock_init_data(sock, sk);
192 sk->sk_destruct = can_sock_destruct;
193
194 if (sk->sk_prot->init)
195 err = sk->sk_prot->init(sk);
196
197 if (err) {
198 /* release sk on errors */
199 sock_orphan(sk);
200 sock_put(sk);
201 }
202
203 errout:
204 can_put_proto(cp);
205 return err;
206 }
207
208 /*
209 * af_can tx path
210 */
211
212 /**
213 * can_send - transmit a CAN frame (optional with local loopback)
214 * @skb: pointer to socket buffer with CAN frame in data section
215 * @loop: loopback for listeners on local CAN sockets (recommended default!)
216 *
217 * Due to the loopback this routine must not be called from hardirq context.
218 *
219 * Return:
220 * 0 on success
221 * -ENETDOWN when the selected interface is down
222 * -ENOBUFS on full driver queue (see net_xmit_errno())
223 * -ENOMEM when local loopback failed at calling skb_clone()
224 * -EPERM when trying to send on a non-CAN interface
225 * -EMSGSIZE CAN frame size is bigger than CAN interface MTU
226 * -EINVAL when the skb->data does not contain a valid CAN frame
227 */
can_send(struct sk_buff * skb,int loop)228 int can_send(struct sk_buff *skb, int loop)
229 {
230 struct sk_buff *newskb = NULL;
231 struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
232 int err = -EINVAL;
233
234 if (skb->len == CAN_MTU) {
235 skb->protocol = htons(ETH_P_CAN);
236 if (unlikely(cfd->len > CAN_MAX_DLEN))
237 goto inval_skb;
238 } else if (skb->len == CANFD_MTU) {
239 skb->protocol = htons(ETH_P_CANFD);
240 if (unlikely(cfd->len > CANFD_MAX_DLEN))
241 goto inval_skb;
242 } else
243 goto inval_skb;
244
245 /*
246 * Make sure the CAN frame can pass the selected CAN netdevice.
247 * As structs can_frame and canfd_frame are similar, we can provide
248 * CAN FD frames to legacy CAN drivers as long as the length is <= 8
249 */
250 if (unlikely(skb->len > skb->dev->mtu && cfd->len > CAN_MAX_DLEN)) {
251 err = -EMSGSIZE;
252 goto inval_skb;
253 }
254
255 if (unlikely(skb->dev->type != ARPHRD_CAN)) {
256 err = -EPERM;
257 goto inval_skb;
258 }
259
260 if (unlikely(!(skb->dev->flags & IFF_UP))) {
261 err = -ENETDOWN;
262 goto inval_skb;
263 }
264
265 skb->ip_summed = CHECKSUM_UNNECESSARY;
266
267 skb_reset_mac_header(skb);
268 skb_reset_network_header(skb);
269 skb_reset_transport_header(skb);
270
271 if (loop) {
272 /* local loopback of sent CAN frames */
273
274 /* indication for the CAN driver: do loopback */
275 skb->pkt_type = PACKET_LOOPBACK;
276
277 /*
278 * The reference to the originating sock may be required
279 * by the receiving socket to check whether the frame is
280 * its own. Example: can_raw sockopt CAN_RAW_RECV_OWN_MSGS
281 * Therefore we have to ensure that skb->sk remains the
282 * reference to the originating sock by restoring skb->sk
283 * after each skb_clone() or skb_orphan() usage.
284 */
285
286 if (!(skb->dev->flags & IFF_ECHO)) {
287 /*
288 * If the interface is not capable to do loopback
289 * itself, we do it here.
290 */
291 newskb = skb_clone(skb, GFP_ATOMIC);
292 if (!newskb) {
293 kfree_skb(skb);
294 return -ENOMEM;
295 }
296
297 can_skb_set_owner(newskb, skb->sk);
298 newskb->ip_summed = CHECKSUM_UNNECESSARY;
299 newskb->pkt_type = PACKET_BROADCAST;
300 }
301 } else {
302 /* indication for the CAN driver: no loopback required */
303 skb->pkt_type = PACKET_HOST;
304 }
305
306 /* send to netdevice */
307 err = dev_queue_xmit(skb);
308 if (err > 0)
309 err = net_xmit_errno(err);
310
311 if (err) {
312 kfree_skb(newskb);
313 return err;
314 }
315
316 if (newskb)
317 netif_rx_ni(newskb);
318
319 /* update statistics */
320 can_stats.tx_frames++;
321 can_stats.tx_frames_delta++;
322
323 return 0;
324
325 inval_skb:
326 kfree_skb(skb);
327 return err;
328 }
329 EXPORT_SYMBOL(can_send);
330
331 /*
332 * af_can rx path
333 */
334
find_dev_rcv_lists(struct net_device * dev)335 static struct dev_rcv_lists *find_dev_rcv_lists(struct net_device *dev)
336 {
337 if (!dev)
338 return &can_rx_alldev_list;
339 else
340 return (struct dev_rcv_lists *)dev->ml_priv;
341 }
342
343 /**
344 * effhash - hash function for 29 bit CAN identifier reduction
345 * @can_id: 29 bit CAN identifier
346 *
347 * Description:
348 * To reduce the linear traversal in one linked list of _single_ EFF CAN
349 * frame subscriptions the 29 bit identifier is mapped to 10 bits.
350 * (see CAN_EFF_RCV_HASH_BITS definition)
351 *
352 * Return:
353 * Hash value from 0x000 - 0x3FF ( enforced by CAN_EFF_RCV_HASH_BITS mask )
354 */
effhash(canid_t can_id)355 static unsigned int effhash(canid_t can_id)
356 {
357 unsigned int hash;
358
359 hash = can_id;
360 hash ^= can_id >> CAN_EFF_RCV_HASH_BITS;
361 hash ^= can_id >> (2 * CAN_EFF_RCV_HASH_BITS);
362
363 return hash & ((1 << CAN_EFF_RCV_HASH_BITS) - 1);
364 }
365
366 /**
367 * find_rcv_list - determine optimal filterlist inside device filter struct
368 * @can_id: pointer to CAN identifier of a given can_filter
369 * @mask: pointer to CAN mask of a given can_filter
370 * @d: pointer to the device filter struct
371 *
372 * Description:
373 * Returns the optimal filterlist to reduce the filter handling in the
374 * receive path. This function is called by service functions that need
375 * to register or unregister a can_filter in the filter lists.
376 *
377 * A filter matches in general, when
378 *
379 * <received_can_id> & mask == can_id & mask
380 *
381 * so every bit set in the mask (even CAN_EFF_FLAG, CAN_RTR_FLAG) describe
382 * relevant bits for the filter.
383 *
384 * The filter can be inverted (CAN_INV_FILTER bit set in can_id) or it can
385 * filter for error messages (CAN_ERR_FLAG bit set in mask). For error msg
386 * frames there is a special filterlist and a special rx path filter handling.
387 *
388 * Return:
389 * Pointer to optimal filterlist for the given can_id/mask pair.
390 * Constistency checked mask.
391 * Reduced can_id to have a preprocessed filter compare value.
392 */
find_rcv_list(canid_t * can_id,canid_t * mask,struct dev_rcv_lists * d)393 static struct hlist_head *find_rcv_list(canid_t *can_id, canid_t *mask,
394 struct dev_rcv_lists *d)
395 {
396 canid_t inv = *can_id & CAN_INV_FILTER; /* save flag before masking */
397
398 /* filter for error message frames in extra filterlist */
399 if (*mask & CAN_ERR_FLAG) {
400 /* clear CAN_ERR_FLAG in filter entry */
401 *mask &= CAN_ERR_MASK;
402 return &d->rx[RX_ERR];
403 }
404
405 /* with cleared CAN_ERR_FLAG we have a simple mask/value filterpair */
406
407 #define CAN_EFF_RTR_FLAGS (CAN_EFF_FLAG | CAN_RTR_FLAG)
408
409 /* ensure valid values in can_mask for 'SFF only' frame filtering */
410 if ((*mask & CAN_EFF_FLAG) && !(*can_id & CAN_EFF_FLAG))
411 *mask &= (CAN_SFF_MASK | CAN_EFF_RTR_FLAGS);
412
413 /* reduce condition testing at receive time */
414 *can_id &= *mask;
415
416 /* inverse can_id/can_mask filter */
417 if (inv)
418 return &d->rx[RX_INV];
419
420 /* mask == 0 => no condition testing at receive time */
421 if (!(*mask))
422 return &d->rx[RX_ALL];
423
424 /* extra filterlists for the subscription of a single non-RTR can_id */
425 if (((*mask & CAN_EFF_RTR_FLAGS) == CAN_EFF_RTR_FLAGS) &&
426 !(*can_id & CAN_RTR_FLAG)) {
427
428 if (*can_id & CAN_EFF_FLAG) {
429 if (*mask == (CAN_EFF_MASK | CAN_EFF_RTR_FLAGS))
430 return &d->rx_eff[effhash(*can_id)];
431 } else {
432 if (*mask == (CAN_SFF_MASK | CAN_EFF_RTR_FLAGS))
433 return &d->rx_sff[*can_id];
434 }
435 }
436
437 /* default: filter via can_id/can_mask */
438 return &d->rx[RX_FIL];
439 }
440
441 /**
442 * can_rx_register - subscribe CAN frames from a specific interface
443 * @dev: pointer to netdevice (NULL => subcribe from 'all' CAN devices list)
444 * @can_id: CAN identifier (see description)
445 * @mask: CAN mask (see description)
446 * @func: callback function on filter match
447 * @data: returned parameter for callback function
448 * @ident: string for calling module identification
449 * @sk: socket pointer (might be NULL)
450 *
451 * Description:
452 * Invokes the callback function with the received sk_buff and the given
453 * parameter 'data' on a matching receive filter. A filter matches, when
454 *
455 * <received_can_id> & mask == can_id & mask
456 *
457 * The filter can be inverted (CAN_INV_FILTER bit set in can_id) or it can
458 * filter for error message frames (CAN_ERR_FLAG bit set in mask).
459 *
460 * The provided pointer to the sk_buff is guaranteed to be valid as long as
461 * the callback function is running. The callback function must *not* free
462 * the given sk_buff while processing it's task. When the given sk_buff is
463 * needed after the end of the callback function it must be cloned inside
464 * the callback function with skb_clone().
465 *
466 * Return:
467 * 0 on success
468 * -ENOMEM on missing cache mem to create subscription entry
469 * -ENODEV unknown device
470 */
can_rx_register(struct net_device * dev,canid_t can_id,canid_t mask,void (* func)(struct sk_buff *,void *),void * data,char * ident,struct sock * sk)471 int can_rx_register(struct net_device *dev, canid_t can_id, canid_t mask,
472 void (*func)(struct sk_buff *, void *), void *data,
473 char *ident, struct sock *sk)
474 {
475 struct receiver *r;
476 struct hlist_head *rl;
477 struct dev_rcv_lists *d;
478 int err = 0;
479
480 /* insert new receiver (dev,canid,mask) -> (func,data) */
481
482 if (dev && dev->type != ARPHRD_CAN)
483 return -ENODEV;
484
485 r = kmem_cache_alloc(rcv_cache, GFP_KERNEL);
486 if (!r)
487 return -ENOMEM;
488
489 spin_lock(&can_rcvlists_lock);
490
491 d = find_dev_rcv_lists(dev);
492 if (d) {
493 rl = find_rcv_list(&can_id, &mask, d);
494
495 r->can_id = can_id;
496 r->mask = mask;
497 r->matches = 0;
498 r->func = func;
499 r->data = data;
500 r->ident = ident;
501 r->sk = sk;
502
503 hlist_add_head_rcu(&r->list, rl);
504 d->entries++;
505
506 can_pstats.rcv_entries++;
507 if (can_pstats.rcv_entries_max < can_pstats.rcv_entries)
508 can_pstats.rcv_entries_max = can_pstats.rcv_entries;
509 } else {
510 kmem_cache_free(rcv_cache, r);
511 err = -ENODEV;
512 }
513
514 spin_unlock(&can_rcvlists_lock);
515
516 return err;
517 }
518 EXPORT_SYMBOL(can_rx_register);
519
520 /*
521 * can_rx_delete_receiver - rcu callback for single receiver entry removal
522 */
can_rx_delete_receiver(struct rcu_head * rp)523 static void can_rx_delete_receiver(struct rcu_head *rp)
524 {
525 struct receiver *r = container_of(rp, struct receiver, rcu);
526 struct sock *sk = r->sk;
527
528 kmem_cache_free(rcv_cache, r);
529 if (sk)
530 sock_put(sk);
531 }
532
533 /**
534 * can_rx_unregister - unsubscribe CAN frames from a specific interface
535 * @dev: pointer to netdevice (NULL => unsubcribe from 'all' CAN devices list)
536 * @can_id: CAN identifier
537 * @mask: CAN mask
538 * @func: callback function on filter match
539 * @data: returned parameter for callback function
540 *
541 * Description:
542 * Removes subscription entry depending on given (subscription) values.
543 */
can_rx_unregister(struct net_device * dev,canid_t can_id,canid_t mask,void (* func)(struct sk_buff *,void *),void * data)544 void can_rx_unregister(struct net_device *dev, canid_t can_id, canid_t mask,
545 void (*func)(struct sk_buff *, void *), void *data)
546 {
547 struct receiver *r = NULL;
548 struct hlist_head *rl;
549 struct dev_rcv_lists *d;
550
551 if (dev && dev->type != ARPHRD_CAN)
552 return;
553
554 spin_lock(&can_rcvlists_lock);
555
556 d = find_dev_rcv_lists(dev);
557 if (!d) {
558 pr_err("BUG: receive list not found for "
559 "dev %s, id %03X, mask %03X\n",
560 DNAME(dev), can_id, mask);
561 goto out;
562 }
563
564 rl = find_rcv_list(&can_id, &mask, d);
565
566 /*
567 * Search the receiver list for the item to delete. This should
568 * exist, since no receiver may be unregistered that hasn't
569 * been registered before.
570 */
571
572 hlist_for_each_entry_rcu(r, rl, list) {
573 if (r->can_id == can_id && r->mask == mask &&
574 r->func == func && r->data == data)
575 break;
576 }
577
578 /*
579 * Check for bugs in CAN protocol implementations using af_can.c:
580 * 'r' will be NULL if no matching list item was found for removal.
581 */
582
583 if (!r) {
584 WARN(1, "BUG: receive list entry not found for dev %s, "
585 "id %03X, mask %03X\n", DNAME(dev), can_id, mask);
586 goto out;
587 }
588
589 hlist_del_rcu(&r->list);
590 d->entries--;
591
592 if (can_pstats.rcv_entries > 0)
593 can_pstats.rcv_entries--;
594
595 /* remove device structure requested by NETDEV_UNREGISTER */
596 if (d->remove_on_zero_entries && !d->entries) {
597 kfree(d);
598 dev->ml_priv = NULL;
599 }
600
601 out:
602 spin_unlock(&can_rcvlists_lock);
603
604 /* schedule the receiver item for deletion */
605 if (r) {
606 if (r->sk)
607 sock_hold(r->sk);
608 call_rcu(&r->rcu, can_rx_delete_receiver);
609 }
610 }
611 EXPORT_SYMBOL(can_rx_unregister);
612
deliver(struct sk_buff * skb,struct receiver * r)613 static inline void deliver(struct sk_buff *skb, struct receiver *r)
614 {
615 r->func(skb, r->data);
616 r->matches++;
617 }
618
can_rcv_filter(struct dev_rcv_lists * d,struct sk_buff * skb)619 static int can_rcv_filter(struct dev_rcv_lists *d, struct sk_buff *skb)
620 {
621 struct receiver *r;
622 int matches = 0;
623 struct can_frame *cf = (struct can_frame *)skb->data;
624 canid_t can_id = cf->can_id;
625
626 if (d->entries == 0)
627 return 0;
628
629 if (can_id & CAN_ERR_FLAG) {
630 /* check for error message frame entries only */
631 hlist_for_each_entry_rcu(r, &d->rx[RX_ERR], list) {
632 if (can_id & r->mask) {
633 deliver(skb, r);
634 matches++;
635 }
636 }
637 return matches;
638 }
639
640 /* check for unfiltered entries */
641 hlist_for_each_entry_rcu(r, &d->rx[RX_ALL], list) {
642 deliver(skb, r);
643 matches++;
644 }
645
646 /* check for can_id/mask entries */
647 hlist_for_each_entry_rcu(r, &d->rx[RX_FIL], list) {
648 if ((can_id & r->mask) == r->can_id) {
649 deliver(skb, r);
650 matches++;
651 }
652 }
653
654 /* check for inverted can_id/mask entries */
655 hlist_for_each_entry_rcu(r, &d->rx[RX_INV], list) {
656 if ((can_id & r->mask) != r->can_id) {
657 deliver(skb, r);
658 matches++;
659 }
660 }
661
662 /* check filterlists for single non-RTR can_ids */
663 if (can_id & CAN_RTR_FLAG)
664 return matches;
665
666 if (can_id & CAN_EFF_FLAG) {
667 hlist_for_each_entry_rcu(r, &d->rx_eff[effhash(can_id)], list) {
668 if (r->can_id == can_id) {
669 deliver(skb, r);
670 matches++;
671 }
672 }
673 } else {
674 can_id &= CAN_SFF_MASK;
675 hlist_for_each_entry_rcu(r, &d->rx_sff[can_id], list) {
676 deliver(skb, r);
677 matches++;
678 }
679 }
680
681 return matches;
682 }
683
can_receive(struct sk_buff * skb,struct net_device * dev)684 static void can_receive(struct sk_buff *skb, struct net_device *dev)
685 {
686 struct dev_rcv_lists *d;
687 int matches;
688
689 /* update statistics */
690 can_stats.rx_frames++;
691 can_stats.rx_frames_delta++;
692
693 rcu_read_lock();
694
695 /* deliver the packet to sockets listening on all devices */
696 matches = can_rcv_filter(&can_rx_alldev_list, skb);
697
698 /* find receive list for this device */
699 d = find_dev_rcv_lists(dev);
700 if (d)
701 matches += can_rcv_filter(d, skb);
702
703 rcu_read_unlock();
704
705 /* consume the skbuff allocated by the netdevice driver */
706 consume_skb(skb);
707
708 if (matches > 0) {
709 can_stats.matches++;
710 can_stats.matches_delta++;
711 }
712 }
713
can_rcv(struct sk_buff * skb,struct net_device * dev,struct packet_type * pt,struct net_device * orig_dev)714 static int can_rcv(struct sk_buff *skb, struct net_device *dev,
715 struct packet_type *pt, struct net_device *orig_dev)
716 {
717 struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
718
719 if (unlikely(!net_eq(dev_net(dev), &init_net)))
720 goto drop;
721
722 if (unlikely(dev->type != ARPHRD_CAN || skb->len != CAN_MTU ||
723 cfd->len > CAN_MAX_DLEN)) {
724 pr_warn_once("PF_CAN: dropped non conform CAN skbuf: dev type %d, len %d, datalen %d\n",
725 dev->type, skb->len, cfd->len);
726 goto drop;
727 }
728
729 can_receive(skb, dev);
730 return NET_RX_SUCCESS;
731
732 drop:
733 kfree_skb(skb);
734 return NET_RX_DROP;
735 }
736
canfd_rcv(struct sk_buff * skb,struct net_device * dev,struct packet_type * pt,struct net_device * orig_dev)737 static int canfd_rcv(struct sk_buff *skb, struct net_device *dev,
738 struct packet_type *pt, struct net_device *orig_dev)
739 {
740 struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
741
742 if (unlikely(!net_eq(dev_net(dev), &init_net)))
743 goto drop;
744
745 if (unlikely(dev->type != ARPHRD_CAN || skb->len != CANFD_MTU ||
746 cfd->len > CANFD_MAX_DLEN)) {
747 pr_warn_once("PF_CAN: dropped non conform CAN FD skbuf: dev type %d, len %d, datalen %d\n",
748 dev->type, skb->len, cfd->len);
749 goto drop;
750 }
751
752 can_receive(skb, dev);
753 return NET_RX_SUCCESS;
754
755 drop:
756 kfree_skb(skb);
757 return NET_RX_DROP;
758 }
759
760 /*
761 * af_can protocol functions
762 */
763
764 /**
765 * can_proto_register - register CAN transport protocol
766 * @cp: pointer to CAN protocol structure
767 *
768 * Return:
769 * 0 on success
770 * -EINVAL invalid (out of range) protocol number
771 * -EBUSY protocol already in use
772 * -ENOBUF if proto_register() fails
773 */
can_proto_register(const struct can_proto * cp)774 int can_proto_register(const struct can_proto *cp)
775 {
776 int proto = cp->protocol;
777 int err = 0;
778
779 if (proto < 0 || proto >= CAN_NPROTO) {
780 pr_err("can: protocol number %d out of range\n", proto);
781 return -EINVAL;
782 }
783
784 err = proto_register(cp->prot, 0);
785 if (err < 0)
786 return err;
787
788 mutex_lock(&proto_tab_lock);
789
790 if (proto_tab[proto]) {
791 pr_err("can: protocol %d already registered\n", proto);
792 err = -EBUSY;
793 } else
794 RCU_INIT_POINTER(proto_tab[proto], cp);
795
796 mutex_unlock(&proto_tab_lock);
797
798 if (err < 0)
799 proto_unregister(cp->prot);
800
801 return err;
802 }
803 EXPORT_SYMBOL(can_proto_register);
804
805 /**
806 * can_proto_unregister - unregister CAN transport protocol
807 * @cp: pointer to CAN protocol structure
808 */
can_proto_unregister(const struct can_proto * cp)809 void can_proto_unregister(const struct can_proto *cp)
810 {
811 int proto = cp->protocol;
812
813 mutex_lock(&proto_tab_lock);
814 BUG_ON(proto_tab[proto] != cp);
815 RCU_INIT_POINTER(proto_tab[proto], NULL);
816 mutex_unlock(&proto_tab_lock);
817
818 synchronize_rcu();
819
820 proto_unregister(cp->prot);
821 }
822 EXPORT_SYMBOL(can_proto_unregister);
823
824 /*
825 * af_can notifier to create/remove CAN netdevice specific structs
826 */
can_notifier(struct notifier_block * nb,unsigned long msg,void * ptr)827 static int can_notifier(struct notifier_block *nb, unsigned long msg,
828 void *ptr)
829 {
830 struct net_device *dev = netdev_notifier_info_to_dev(ptr);
831 struct dev_rcv_lists *d;
832
833 if (!net_eq(dev_net(dev), &init_net))
834 return NOTIFY_DONE;
835
836 if (dev->type != ARPHRD_CAN)
837 return NOTIFY_DONE;
838
839 switch (msg) {
840
841 case NETDEV_REGISTER:
842
843 /* create new dev_rcv_lists for this device */
844 d = kzalloc(sizeof(*d), GFP_KERNEL);
845 if (!d)
846 return NOTIFY_DONE;
847 BUG_ON(dev->ml_priv);
848 dev->ml_priv = d;
849
850 break;
851
852 case NETDEV_UNREGISTER:
853 spin_lock(&can_rcvlists_lock);
854
855 d = dev->ml_priv;
856 if (d) {
857 if (d->entries)
858 d->remove_on_zero_entries = 1;
859 else {
860 kfree(d);
861 dev->ml_priv = NULL;
862 }
863 } else
864 pr_err("can: notifier: receive list not found for dev "
865 "%s\n", dev->name);
866
867 spin_unlock(&can_rcvlists_lock);
868
869 break;
870 }
871
872 return NOTIFY_DONE;
873 }
874
875 /*
876 * af_can module init/exit functions
877 */
878
879 static struct packet_type can_packet __read_mostly = {
880 .type = cpu_to_be16(ETH_P_CAN),
881 .func = can_rcv,
882 };
883
884 static struct packet_type canfd_packet __read_mostly = {
885 .type = cpu_to_be16(ETH_P_CANFD),
886 .func = canfd_rcv,
887 };
888
889 static const struct net_proto_family can_family_ops = {
890 .family = PF_CAN,
891 .create = can_create,
892 .owner = THIS_MODULE,
893 };
894
895 /* notifier block for netdevice event */
896 static struct notifier_block can_netdev_notifier __read_mostly = {
897 .notifier_call = can_notifier,
898 };
899
can_init(void)900 static __init int can_init(void)
901 {
902 /* check for correct padding to be able to use the structs similarly */
903 BUILD_BUG_ON(offsetof(struct can_frame, can_dlc) !=
904 offsetof(struct canfd_frame, len) ||
905 offsetof(struct can_frame, data) !=
906 offsetof(struct canfd_frame, data));
907
908 printk(banner);
909
910 memset(&can_rx_alldev_list, 0, sizeof(can_rx_alldev_list));
911
912 rcv_cache = kmem_cache_create("can_receiver", sizeof(struct receiver),
913 0, 0, NULL);
914 if (!rcv_cache)
915 return -ENOMEM;
916
917 if (stats_timer) {
918 /* the statistics are updated every second (timer triggered) */
919 setup_timer(&can_stattimer, can_stat_update, 0);
920 mod_timer(&can_stattimer, round_jiffies(jiffies + HZ));
921 } else
922 can_stattimer.function = NULL;
923
924 can_init_proc();
925
926 /* protocol register */
927 sock_register(&can_family_ops);
928 register_netdevice_notifier(&can_netdev_notifier);
929 dev_add_pack(&can_packet);
930 dev_add_pack(&canfd_packet);
931
932 return 0;
933 }
934
can_exit(void)935 static __exit void can_exit(void)
936 {
937 struct net_device *dev;
938
939 if (stats_timer)
940 del_timer_sync(&can_stattimer);
941
942 can_remove_proc();
943
944 /* protocol unregister */
945 dev_remove_pack(&canfd_packet);
946 dev_remove_pack(&can_packet);
947 unregister_netdevice_notifier(&can_netdev_notifier);
948 sock_unregister(PF_CAN);
949
950 /* remove created dev_rcv_lists from still registered CAN devices */
951 rcu_read_lock();
952 for_each_netdev_rcu(&init_net, dev) {
953 if (dev->type == ARPHRD_CAN && dev->ml_priv) {
954
955 struct dev_rcv_lists *d = dev->ml_priv;
956
957 BUG_ON(d->entries);
958 kfree(d);
959 dev->ml_priv = NULL;
960 }
961 }
962 rcu_read_unlock();
963
964 rcu_barrier(); /* Wait for completion of call_rcu()'s */
965
966 kmem_cache_destroy(rcv_cache);
967 }
968
969 module_init(can_init);
970 module_exit(can_exit);
971