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