1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
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
67MODULE_DESCRIPTION("Controller Area Network PF_CAN core");
68MODULE_LICENSE("Dual BSD/GPL");
69MODULE_AUTHOR("Urs Thuermann <urs.thuermann@volkswagen.de>, "
70 "Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
71
72MODULE_ALIAS_NETPROTO(PF_CAN);
73
74static int stats_timer __read_mostly = 1;
75module_param(stats_timer, int, S_IRUGO);
76MODULE_PARM_DESC(stats_timer, "enable timer for statistics (default:on)");
77
78
79struct dev_rcv_lists can_rx_alldev_list;
80static DEFINE_SPINLOCK(can_rcvlists_lock);
81
82static struct kmem_cache *rcv_cache __read_mostly;
83
84
85static const struct can_proto *proto_tab[CAN_NPROTO] __read_mostly;
86static DEFINE_MUTEX(proto_tab_lock);
87
88struct timer_list can_stattimer;
89struct s_stats can_stats;
90struct s_pstats can_pstats;
91
92
93
94
95
96int can_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
97{
98 struct sock *sk = sock->sk;
99
100 switch (cmd) {
101
102 case SIOCGSTAMP:
103 return sock_get_timestamp(sk, (struct timeval __user *)arg);
104
105 default:
106 return -ENOIOCTLCMD;
107 }
108}
109EXPORT_SYMBOL(can_ioctl);
110
111static void can_sock_destruct(struct sock *sk)
112{
113 skb_queue_purge(&sk->sk_receive_queue);
114}
115
116static const struct can_proto *can_get_proto(int protocol)
117{
118 const struct can_proto *cp;
119
120 rcu_read_lock();
121 cp = rcu_dereference(proto_tab[protocol]);
122 if (cp && !try_module_get(cp->prot->owner))
123 cp = NULL;
124 rcu_read_unlock();
125
126 return cp;
127}
128
129static inline void can_put_proto(const struct can_proto *cp)
130{
131 module_put(cp->prot->owner);
132}
133
134static int can_create(struct net *net, struct socket *sock, int protocol,
135 int kern)
136{
137 struct sock *sk;
138 const struct can_proto *cp;
139 int err = 0;
140
141 sock->state = SS_UNCONNECTED;
142
143 if (protocol < 0 || protocol >= CAN_NPROTO)
144 return -EINVAL;
145
146 if (!net_eq(net, &init_net))
147 return -EAFNOSUPPORT;
148
149 cp = can_get_proto(protocol);
150
151#ifdef CONFIG_MODULES
152 if (!cp) {
153
154
155 err = request_module("can-proto-%d", protocol);
156
157
158
159
160
161
162 if (err)
163 printk_ratelimited(KERN_ERR "can: request_module "
164 "(can-proto-%d) failed.\n", protocol);
165
166 cp = can_get_proto(protocol);
167 }
168#endif
169
170
171
172 if (!cp)
173 return -EPROTONOSUPPORT;
174
175 if (cp->type != sock->type) {
176 err = -EPROTOTYPE;
177 goto errout;
178 }
179
180 sock->ops = cp->ops;
181
182 sk = sk_alloc(net, PF_CAN, GFP_KERNEL, cp->prot);
183 if (!sk) {
184 err = -ENOMEM;
185 goto errout;
186 }
187
188 sock_init_data(sock, sk);
189 sk->sk_destruct = can_sock_destruct;
190
191 if (sk->sk_prot->init)
192 err = sk->sk_prot->init(sk);
193
194 if (err) {
195
196 sock_orphan(sk);
197 sock_put(sk);
198 }
199
200 errout:
201 can_put_proto(cp);
202 return err;
203}
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225int can_send(struct sk_buff *skb, int loop)
226{
227 struct sk_buff *newskb = NULL;
228 struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
229 int err = -EINVAL;
230
231 if (skb->len == CAN_MTU) {
232 skb->protocol = htons(ETH_P_CAN);
233 if (unlikely(cfd->len > CAN_MAX_DLEN))
234 goto inval_skb;
235 } else if (skb->len == CANFD_MTU) {
236 skb->protocol = htons(ETH_P_CANFD);
237 if (unlikely(cfd->len > CANFD_MAX_DLEN))
238 goto inval_skb;
239 } else
240 goto inval_skb;
241
242
243
244
245
246
247 if (unlikely(skb->len > skb->dev->mtu && cfd->len > CAN_MAX_DLEN)) {
248 err = -EMSGSIZE;
249 goto inval_skb;
250 }
251
252 if (unlikely(skb->dev->type != ARPHRD_CAN)) {
253 err = -EPERM;
254 goto inval_skb;
255 }
256
257 if (unlikely(!(skb->dev->flags & IFF_UP))) {
258 err = -ENETDOWN;
259 goto inval_skb;
260 }
261
262 skb->ip_summed = CHECKSUM_UNNECESSARY;
263
264 skb_reset_mac_header(skb);
265 skb_reset_network_header(skb);
266 skb_reset_transport_header(skb);
267
268 if (loop) {
269
270
271
272 skb->pkt_type = PACKET_LOOPBACK;
273
274
275
276
277
278
279
280
281
282
283 if (!(skb->dev->flags & IFF_ECHO)) {
284
285
286
287
288 newskb = skb_clone(skb, GFP_ATOMIC);
289 if (!newskb) {
290 kfree_skb(skb);
291 return -ENOMEM;
292 }
293
294 can_skb_set_owner(newskb, skb->sk);
295 newskb->ip_summed = CHECKSUM_UNNECESSARY;
296 newskb->pkt_type = PACKET_BROADCAST;
297 }
298 } else {
299
300 skb->pkt_type = PACKET_HOST;
301 }
302
303
304 err = dev_queue_xmit(skb);
305 if (err > 0)
306 err = net_xmit_errno(err);
307
308 if (err) {
309 kfree_skb(newskb);
310 return err;
311 }
312
313 if (newskb)
314 netif_rx_ni(newskb);
315
316
317 can_stats.tx_frames++;
318 can_stats.tx_frames_delta++;
319
320 return 0;
321
322inval_skb:
323 kfree_skb(skb);
324 return err;
325}
326EXPORT_SYMBOL(can_send);
327
328
329
330
331
332static struct dev_rcv_lists *find_dev_rcv_lists(struct net_device *dev)
333{
334 if (!dev)
335 return &can_rx_alldev_list;
336 else
337 return (struct dev_rcv_lists *)dev->ml_priv;
338}
339
340
341
342
343
344
345
346
347
348
349
350
351
352static unsigned int effhash(canid_t can_id)
353{
354 unsigned int hash;
355
356 hash = can_id;
357 hash ^= can_id >> CAN_EFF_RCV_HASH_BITS;
358 hash ^= can_id >> (2 * CAN_EFF_RCV_HASH_BITS);
359
360 return hash & ((1 << CAN_EFF_RCV_HASH_BITS) - 1);
361}
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390static struct hlist_head *find_rcv_list(canid_t *can_id, canid_t *mask,
391 struct dev_rcv_lists *d)
392{
393 canid_t inv = *can_id & CAN_INV_FILTER;
394
395
396 if (*mask & CAN_ERR_FLAG) {
397
398 *mask &= CAN_ERR_MASK;
399 return &d->rx[RX_ERR];
400 }
401
402
403
404#define CAN_EFF_RTR_FLAGS (CAN_EFF_FLAG | CAN_RTR_FLAG)
405
406
407 if ((*mask & CAN_EFF_FLAG) && !(*can_id & CAN_EFF_FLAG))
408 *mask &= (CAN_SFF_MASK | CAN_EFF_RTR_FLAGS);
409
410
411 *can_id &= *mask;
412
413
414 if (inv)
415 return &d->rx[RX_INV];
416
417
418 if (!(*mask))
419 return &d->rx[RX_ALL];
420
421
422 if (((*mask & CAN_EFF_RTR_FLAGS) == CAN_EFF_RTR_FLAGS) &&
423 !(*can_id & CAN_RTR_FLAG)) {
424
425 if (*can_id & CAN_EFF_FLAG) {
426 if (*mask == (CAN_EFF_MASK | CAN_EFF_RTR_FLAGS))
427 return &d->rx_eff[effhash(*can_id)];
428 } else {
429 if (*mask == (CAN_SFF_MASK | CAN_EFF_RTR_FLAGS))
430 return &d->rx_sff[*can_id];
431 }
432 }
433
434
435 return &d->rx[RX_FIL];
436}
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467int can_rx_register(struct net_device *dev, canid_t can_id, canid_t mask,
468 void (*func)(struct sk_buff *, void *), void *data,
469 char *ident)
470{
471 struct receiver *r;
472 struct hlist_head *rl;
473 struct dev_rcv_lists *d;
474 int err = 0;
475
476
477
478 if (dev && dev->type != ARPHRD_CAN)
479 return -ENODEV;
480
481 r = kmem_cache_alloc(rcv_cache, GFP_KERNEL);
482 if (!r)
483 return -ENOMEM;
484
485 spin_lock(&can_rcvlists_lock);
486
487 d = find_dev_rcv_lists(dev);
488 if (d) {
489 rl = find_rcv_list(&can_id, &mask, d);
490
491 r->can_id = can_id;
492 r->mask = mask;
493 r->matches = 0;
494 r->func = func;
495 r->data = data;
496 r->ident = ident;
497
498 hlist_add_head_rcu(&r->list, rl);
499 d->entries++;
500
501 can_pstats.rcv_entries++;
502 if (can_pstats.rcv_entries_max < can_pstats.rcv_entries)
503 can_pstats.rcv_entries_max = can_pstats.rcv_entries;
504 } else {
505 kmem_cache_free(rcv_cache, r);
506 err = -ENODEV;
507 }
508
509 spin_unlock(&can_rcvlists_lock);
510
511 return err;
512}
513EXPORT_SYMBOL(can_rx_register);
514
515
516
517
518static void can_rx_delete_receiver(struct rcu_head *rp)
519{
520 struct receiver *r = container_of(rp, struct receiver, rcu);
521
522 kmem_cache_free(rcv_cache, r);
523}
524
525
526
527
528
529
530
531
532
533
534
535
536void can_rx_unregister(struct net_device *dev, canid_t can_id, canid_t mask,
537 void (*func)(struct sk_buff *, void *), void *data)
538{
539 struct receiver *r = NULL;
540 struct hlist_head *rl;
541 struct dev_rcv_lists *d;
542
543 if (dev && dev->type != ARPHRD_CAN)
544 return;
545
546 spin_lock(&can_rcvlists_lock);
547
548 d = find_dev_rcv_lists(dev);
549 if (!d) {
550 pr_err("BUG: receive list not found for "
551 "dev %s, id %03X, mask %03X\n",
552 DNAME(dev), can_id, mask);
553 goto out;
554 }
555
556 rl = find_rcv_list(&can_id, &mask, d);
557
558
559
560
561
562
563
564 hlist_for_each_entry_rcu(r, rl, list) {
565 if (r->can_id == can_id && r->mask == mask &&
566 r->func == func && r->data == data)
567 break;
568 }
569
570
571
572
573
574
575 if (!r) {
576 WARN(1, "BUG: receive list entry not found for dev %s, "
577 "id %03X, mask %03X\n", DNAME(dev), can_id, mask);
578 goto out;
579 }
580
581 hlist_del_rcu(&r->list);
582 d->entries--;
583
584 if (can_pstats.rcv_entries > 0)
585 can_pstats.rcv_entries--;
586
587
588 if (d->remove_on_zero_entries && !d->entries) {
589 kfree(d);
590 dev->ml_priv = NULL;
591 }
592
593 out:
594 spin_unlock(&can_rcvlists_lock);
595
596
597 if (r)
598 call_rcu(&r->rcu, can_rx_delete_receiver);
599}
600EXPORT_SYMBOL(can_rx_unregister);
601
602static inline void deliver(struct sk_buff *skb, struct receiver *r)
603{
604 r->func(skb, r->data);
605 r->matches++;
606}
607
608static int can_rcv_filter(struct dev_rcv_lists *d, struct sk_buff *skb)
609{
610 struct receiver *r;
611 int matches = 0;
612 struct can_frame *cf = (struct can_frame *)skb->data;
613 canid_t can_id = cf->can_id;
614
615 if (d->entries == 0)
616 return 0;
617
618 if (can_id & CAN_ERR_FLAG) {
619
620 hlist_for_each_entry_rcu(r, &d->rx[RX_ERR], list) {
621 if (can_id & r->mask) {
622 deliver(skb, r);
623 matches++;
624 }
625 }
626 return matches;
627 }
628
629
630 hlist_for_each_entry_rcu(r, &d->rx[RX_ALL], list) {
631 deliver(skb, r);
632 matches++;
633 }
634
635
636 hlist_for_each_entry_rcu(r, &d->rx[RX_FIL], list) {
637 if ((can_id & r->mask) == r->can_id) {
638 deliver(skb, r);
639 matches++;
640 }
641 }
642
643
644 hlist_for_each_entry_rcu(r, &d->rx[RX_INV], list) {
645 if ((can_id & r->mask) != r->can_id) {
646 deliver(skb, r);
647 matches++;
648 }
649 }
650
651
652 if (can_id & CAN_RTR_FLAG)
653 return matches;
654
655 if (can_id & CAN_EFF_FLAG) {
656 hlist_for_each_entry_rcu(r, &d->rx_eff[effhash(can_id)], list) {
657 if (r->can_id == can_id) {
658 deliver(skb, r);
659 matches++;
660 }
661 }
662 } else {
663 can_id &= CAN_SFF_MASK;
664 hlist_for_each_entry_rcu(r, &d->rx_sff[can_id], list) {
665 deliver(skb, r);
666 matches++;
667 }
668 }
669
670 return matches;
671}
672
673static void can_receive(struct sk_buff *skb, struct net_device *dev)
674{
675 struct dev_rcv_lists *d;
676 int matches;
677
678
679 can_stats.rx_frames++;
680 can_stats.rx_frames_delta++;
681
682 rcu_read_lock();
683
684
685 matches = can_rcv_filter(&can_rx_alldev_list, skb);
686
687
688 d = find_dev_rcv_lists(dev);
689 if (d)
690 matches += can_rcv_filter(d, skb);
691
692 rcu_read_unlock();
693
694
695 consume_skb(skb);
696
697 if (matches > 0) {
698 can_stats.matches++;
699 can_stats.matches_delta++;
700 }
701}
702
703static int can_rcv(struct sk_buff *skb, struct net_device *dev,
704 struct packet_type *pt, struct net_device *orig_dev)
705{
706 struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
707
708 if (unlikely(!net_eq(dev_net(dev), &init_net)))
709 goto drop;
710
711 if (WARN_ONCE(dev->type != ARPHRD_CAN ||
712 skb->len != CAN_MTU ||
713 cfd->len > CAN_MAX_DLEN,
714 "PF_CAN: dropped non conform CAN skbuf: "
715 "dev type %d, len %d, datalen %d\n",
716 dev->type, skb->len, cfd->len))
717 goto drop;
718
719 can_receive(skb, dev);
720 return NET_RX_SUCCESS;
721
722drop:
723 kfree_skb(skb);
724 return NET_RX_DROP;
725}
726
727static int canfd_rcv(struct sk_buff *skb, struct net_device *dev,
728 struct packet_type *pt, struct net_device *orig_dev)
729{
730 struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
731
732 if (unlikely(!net_eq(dev_net(dev), &init_net)))
733 goto drop;
734
735 if (WARN_ONCE(dev->type != ARPHRD_CAN ||
736 skb->len != CANFD_MTU ||
737 cfd->len > CANFD_MAX_DLEN,
738 "PF_CAN: dropped non conform CAN FD skbuf: "
739 "dev type %d, len %d, datalen %d\n",
740 dev->type, skb->len, cfd->len))
741 goto drop;
742
743 can_receive(skb, dev);
744 return NET_RX_SUCCESS;
745
746drop:
747 kfree_skb(skb);
748 return NET_RX_DROP;
749}
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765int can_proto_register(const struct can_proto *cp)
766{
767 int proto = cp->protocol;
768 int err = 0;
769
770 if (proto < 0 || proto >= CAN_NPROTO) {
771 pr_err("can: protocol number %d out of range\n", proto);
772 return -EINVAL;
773 }
774
775 err = proto_register(cp->prot, 0);
776 if (err < 0)
777 return err;
778
779 mutex_lock(&proto_tab_lock);
780
781 if (proto_tab[proto]) {
782 pr_err("can: protocol %d already registered\n", proto);
783 err = -EBUSY;
784 } else
785 RCU_INIT_POINTER(proto_tab[proto], cp);
786
787 mutex_unlock(&proto_tab_lock);
788
789 if (err < 0)
790 proto_unregister(cp->prot);
791
792 return err;
793}
794EXPORT_SYMBOL(can_proto_register);
795
796
797
798
799
800void can_proto_unregister(const struct can_proto *cp)
801{
802 int proto = cp->protocol;
803
804 mutex_lock(&proto_tab_lock);
805 BUG_ON(proto_tab[proto] != cp);
806 RCU_INIT_POINTER(proto_tab[proto], NULL);
807 mutex_unlock(&proto_tab_lock);
808
809 synchronize_rcu();
810
811 proto_unregister(cp->prot);
812}
813EXPORT_SYMBOL(can_proto_unregister);
814
815
816
817
818static int can_notifier(struct notifier_block *nb, unsigned long msg,
819 void *ptr)
820{
821 struct net_device *dev = netdev_notifier_info_to_dev(ptr);
822 struct dev_rcv_lists *d;
823
824 if (!net_eq(dev_net(dev), &init_net))
825 return NOTIFY_DONE;
826
827 if (dev->type != ARPHRD_CAN)
828 return NOTIFY_DONE;
829
830 switch (msg) {
831
832 case NETDEV_REGISTER:
833
834
835 d = kzalloc(sizeof(*d), GFP_KERNEL);
836 if (!d)
837 return NOTIFY_DONE;
838 BUG_ON(dev->ml_priv);
839 dev->ml_priv = d;
840
841 break;
842
843 case NETDEV_UNREGISTER:
844 spin_lock(&can_rcvlists_lock);
845
846 d = dev->ml_priv;
847 if (d) {
848 if (d->entries)
849 d->remove_on_zero_entries = 1;
850 else {
851 kfree(d);
852 dev->ml_priv = NULL;
853 }
854 } else
855 pr_err("can: notifier: receive list not found for dev "
856 "%s\n", dev->name);
857
858 spin_unlock(&can_rcvlists_lock);
859
860 break;
861 }
862
863 return NOTIFY_DONE;
864}
865
866
867
868
869
870static struct packet_type can_packet __read_mostly = {
871 .type = cpu_to_be16(ETH_P_CAN),
872 .func = can_rcv,
873};
874
875static struct packet_type canfd_packet __read_mostly = {
876 .type = cpu_to_be16(ETH_P_CANFD),
877 .func = canfd_rcv,
878};
879
880static const struct net_proto_family can_family_ops = {
881 .family = PF_CAN,
882 .create = can_create,
883 .owner = THIS_MODULE,
884};
885
886
887static struct notifier_block can_netdev_notifier __read_mostly = {
888 .notifier_call = can_notifier,
889};
890
891static __init int can_init(void)
892{
893
894 BUILD_BUG_ON(offsetof(struct can_frame, can_dlc) !=
895 offsetof(struct canfd_frame, len) ||
896 offsetof(struct can_frame, data) !=
897 offsetof(struct canfd_frame, data));
898
899 pr_info("can: controller area network core (" CAN_VERSION_STRING ")\n");
900
901 memset(&can_rx_alldev_list, 0, sizeof(can_rx_alldev_list));
902
903 rcv_cache = kmem_cache_create("can_receiver", sizeof(struct receiver),
904 0, 0, NULL);
905 if (!rcv_cache)
906 return -ENOMEM;
907
908 if (stats_timer) {
909
910 setup_timer(&can_stattimer, can_stat_update, 0);
911 mod_timer(&can_stattimer, round_jiffies(jiffies + HZ));
912 } else
913 can_stattimer.function = NULL;
914
915 can_init_proc();
916
917
918 sock_register(&can_family_ops);
919 register_netdevice_notifier(&can_netdev_notifier);
920 dev_add_pack(&can_packet);
921 dev_add_pack(&canfd_packet);
922
923 return 0;
924}
925
926static __exit void can_exit(void)
927{
928 struct net_device *dev;
929
930 if (stats_timer)
931 del_timer_sync(&can_stattimer);
932
933 can_remove_proc();
934
935
936 dev_remove_pack(&canfd_packet);
937 dev_remove_pack(&can_packet);
938 unregister_netdevice_notifier(&can_netdev_notifier);
939 sock_unregister(PF_CAN);
940
941
942 rcu_read_lock();
943 for_each_netdev_rcu(&init_net, dev) {
944 if (dev->type == ARPHRD_CAN && dev->ml_priv) {
945
946 struct dev_rcv_lists *d = dev->ml_priv;
947
948 BUG_ON(d->entries);
949 kfree(d);
950 dev->ml_priv = NULL;
951 }
952 }
953 rcu_read_unlock();
954
955 rcu_barrier();
956
957 kmem_cache_destroy(rcv_cache);
958}
959
960module_init(can_init);
961module_exit(can_exit);
962