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
44
45#include <linux/module.h>
46#include <linux/init.h>
47#include <linux/kmod.h>
48#include <linux/slab.h>
49#include <linux/list.h>
50#include <linux/spinlock.h>
51#include <linux/rcupdate.h>
52#include <linux/uaccess.h>
53#include <linux/net.h>
54#include <linux/netdevice.h>
55#include <linux/socket.h>
56#include <linux/if_ether.h>
57#include <linux/if_arp.h>
58#include <linux/skbuff.h>
59#include <linux/can.h>
60#include <linux/can/core.h>
61#include <linux/ratelimit.h>
62#include <net/net_namespace.h>
63#include <net/sock.h>
64
65#include "af_can.h"
66
67static __initdata const char banner[] = KERN_INFO
68 "can: controller area network core (" CAN_VERSION_STRING ")\n";
69
70MODULE_DESCRIPTION("Controller Area Network PF_CAN core");
71MODULE_LICENSE("Dual BSD/GPL");
72MODULE_AUTHOR("Urs Thuermann <urs.thuermann@volkswagen.de>, "
73 "Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
74
75MODULE_ALIAS_NETPROTO(PF_CAN);
76
77static int stats_timer __read_mostly = 1;
78module_param(stats_timer, int, S_IRUGO);
79MODULE_PARM_DESC(stats_timer, "enable timer for statistics (default:on)");
80
81
82struct dev_rcv_lists can_rx_alldev_list;
83static DEFINE_SPINLOCK(can_rcvlists_lock);
84
85static struct kmem_cache *rcv_cache __read_mostly;
86
87
88static const struct can_proto *proto_tab[CAN_NPROTO] __read_mostly;
89static DEFINE_MUTEX(proto_tab_lock);
90
91struct timer_list can_stattimer;
92struct s_stats can_stats;
93struct s_pstats can_pstats;
94
95
96
97
98
99int 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}
112EXPORT_SYMBOL(can_ioctl);
113
114static void can_sock_destruct(struct sock *sk)
115{
116 skb_queue_purge(&sk->sk_receive_queue);
117}
118
119static 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
132static inline void can_put_proto(const struct can_proto *cp)
133{
134 module_put(cp->prot->owner);
135}
136
137static 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
157
158 err = request_module("can-proto-%d", protocol);
159
160
161
162
163
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
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
199 sock_orphan(sk);
200 sock_put(sk);
201 }
202
203 errout:
204 can_put_proto(cp);
205 return err;
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 can_frame *cf = (struct can_frame *)skb->data;
231 int err;
232
233 if (skb->len != sizeof(struct can_frame) || cf->can_dlc > 8) {
234 kfree_skb(skb);
235 return -EINVAL;
236 }
237
238 if (skb->dev->type != ARPHRD_CAN) {
239 kfree_skb(skb);
240 return -EPERM;
241 }
242
243 if (!(skb->dev->flags & IFF_UP)) {
244 kfree_skb(skb);
245 return -ENETDOWN;
246 }
247
248 skb->protocol = htons(ETH_P_CAN);
249 skb_reset_network_header(skb);
250 skb_reset_transport_header(skb);
251
252 if (loop) {
253
254
255
256 skb->pkt_type = PACKET_LOOPBACK;
257
258
259
260
261
262
263
264
265
266
267 if (!(skb->dev->flags & IFF_ECHO)) {
268
269
270
271
272 newskb = skb_clone(skb, GFP_ATOMIC);
273 if (!newskb) {
274 kfree_skb(skb);
275 return -ENOMEM;
276 }
277
278 newskb->sk = skb->sk;
279 newskb->ip_summed = CHECKSUM_UNNECESSARY;
280 newskb->pkt_type = PACKET_BROADCAST;
281 }
282 } else {
283
284 skb->pkt_type = PACKET_HOST;
285 }
286
287
288 err = dev_queue_xmit(skb);
289 if (err > 0)
290 err = net_xmit_errno(err);
291
292 if (err) {
293 kfree_skb(newskb);
294 return err;
295 }
296
297 if (newskb)
298 netif_rx_ni(newskb);
299
300
301 can_stats.tx_frames++;
302 can_stats.tx_frames_delta++;
303
304 return 0;
305}
306EXPORT_SYMBOL(can_send);
307
308
309
310
311
312static struct dev_rcv_lists *find_dev_rcv_lists(struct net_device *dev)
313{
314 if (!dev)
315 return &can_rx_alldev_list;
316 else
317 return (struct dev_rcv_lists *)dev->ml_priv;
318}
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347static struct hlist_head *find_rcv_list(canid_t *can_id, canid_t *mask,
348 struct dev_rcv_lists *d)
349{
350 canid_t inv = *can_id & CAN_INV_FILTER;
351
352
353 if (*mask & CAN_ERR_FLAG) {
354
355 *mask &= CAN_ERR_MASK;
356 return &d->rx[RX_ERR];
357 }
358
359
360
361#define CAN_EFF_RTR_FLAGS (CAN_EFF_FLAG | CAN_RTR_FLAG)
362
363
364 if ((*mask & CAN_EFF_FLAG) && !(*can_id & CAN_EFF_FLAG))
365 *mask &= (CAN_SFF_MASK | CAN_EFF_RTR_FLAGS);
366
367
368 *can_id &= *mask;
369
370
371 if (inv)
372 return &d->rx[RX_INV];
373
374
375 if (!(*mask))
376 return &d->rx[RX_ALL];
377
378
379 if (((*mask & CAN_EFF_RTR_FLAGS) == CAN_EFF_RTR_FLAGS) &&
380 !(*can_id & CAN_RTR_FLAG)) {
381
382 if (*can_id & CAN_EFF_FLAG) {
383 if (*mask == (CAN_EFF_MASK | CAN_EFF_RTR_FLAGS)) {
384
385 return &d->rx[RX_EFF];
386 }
387 } else {
388 if (*mask == (CAN_SFF_MASK | CAN_EFF_RTR_FLAGS))
389 return &d->rx_sff[*can_id];
390 }
391 }
392
393
394 return &d->rx[RX_FIL];
395}
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426int can_rx_register(struct net_device *dev, canid_t can_id, canid_t mask,
427 void (*func)(struct sk_buff *, void *), void *data,
428 char *ident)
429{
430 struct receiver *r;
431 struct hlist_head *rl;
432 struct dev_rcv_lists *d;
433 int err = 0;
434
435
436
437 if (dev && dev->type != ARPHRD_CAN)
438 return -ENODEV;
439
440 r = kmem_cache_alloc(rcv_cache, GFP_KERNEL);
441 if (!r)
442 return -ENOMEM;
443
444 spin_lock(&can_rcvlists_lock);
445
446 d = find_dev_rcv_lists(dev);
447 if (d) {
448 rl = find_rcv_list(&can_id, &mask, d);
449
450 r->can_id = can_id;
451 r->mask = mask;
452 r->matches = 0;
453 r->func = func;
454 r->data = data;
455 r->ident = ident;
456
457 hlist_add_head_rcu(&r->list, rl);
458 d->entries++;
459
460 can_pstats.rcv_entries++;
461 if (can_pstats.rcv_entries_max < can_pstats.rcv_entries)
462 can_pstats.rcv_entries_max = can_pstats.rcv_entries;
463 } else {
464 kmem_cache_free(rcv_cache, r);
465 err = -ENODEV;
466 }
467
468 spin_unlock(&can_rcvlists_lock);
469
470 return err;
471}
472EXPORT_SYMBOL(can_rx_register);
473
474
475
476
477static void can_rx_delete_receiver(struct rcu_head *rp)
478{
479 struct receiver *r = container_of(rp, struct receiver, rcu);
480
481 kmem_cache_free(rcv_cache, r);
482}
483
484
485
486
487
488
489
490
491
492
493
494
495void can_rx_unregister(struct net_device *dev, canid_t can_id, canid_t mask,
496 void (*func)(struct sk_buff *, void *), void *data)
497{
498 struct receiver *r = NULL;
499 struct hlist_head *rl;
500 struct hlist_node *next;
501 struct dev_rcv_lists *d;
502
503 if (dev && dev->type != ARPHRD_CAN)
504 return;
505
506 spin_lock(&can_rcvlists_lock);
507
508 d = find_dev_rcv_lists(dev);
509 if (!d) {
510 printk(KERN_ERR "BUG: receive list not found for "
511 "dev %s, id %03X, mask %03X\n",
512 DNAME(dev), can_id, mask);
513 goto out;
514 }
515
516 rl = find_rcv_list(&can_id, &mask, d);
517
518
519
520
521
522
523
524 hlist_for_each_entry_rcu(r, next, rl, list) {
525 if (r->can_id == can_id && r->mask == mask &&
526 r->func == func && r->data == data)
527 break;
528 }
529
530
531
532
533
534
535
536 if (!next) {
537 printk(KERN_ERR "BUG: receive list entry not found for "
538 "dev %s, id %03X, mask %03X\n",
539 DNAME(dev), can_id, mask);
540 r = NULL;
541 goto out;
542 }
543
544 hlist_del_rcu(&r->list);
545 d->entries--;
546
547 if (can_pstats.rcv_entries > 0)
548 can_pstats.rcv_entries--;
549
550
551 if (d->remove_on_zero_entries && !d->entries) {
552 kfree(d);
553 dev->ml_priv = NULL;
554 }
555
556 out:
557 spin_unlock(&can_rcvlists_lock);
558
559
560 if (r)
561 call_rcu(&r->rcu, can_rx_delete_receiver);
562}
563EXPORT_SYMBOL(can_rx_unregister);
564
565static inline void deliver(struct sk_buff *skb, struct receiver *r)
566{
567 r->func(skb, r->data);
568 r->matches++;
569}
570
571static int can_rcv_filter(struct dev_rcv_lists *d, struct sk_buff *skb)
572{
573 struct receiver *r;
574 struct hlist_node *n;
575 int matches = 0;
576 struct can_frame *cf = (struct can_frame *)skb->data;
577 canid_t can_id = cf->can_id;
578
579 if (d->entries == 0)
580 return 0;
581
582 if (can_id & CAN_ERR_FLAG) {
583
584 hlist_for_each_entry_rcu(r, n, &d->rx[RX_ERR], list) {
585 if (can_id & r->mask) {
586 deliver(skb, r);
587 matches++;
588 }
589 }
590 return matches;
591 }
592
593
594 hlist_for_each_entry_rcu(r, n, &d->rx[RX_ALL], list) {
595 deliver(skb, r);
596 matches++;
597 }
598
599
600 hlist_for_each_entry_rcu(r, n, &d->rx[RX_FIL], list) {
601 if ((can_id & r->mask) == r->can_id) {
602 deliver(skb, r);
603 matches++;
604 }
605 }
606
607
608 hlist_for_each_entry_rcu(r, n, &d->rx[RX_INV], list) {
609 if ((can_id & r->mask) != r->can_id) {
610 deliver(skb, r);
611 matches++;
612 }
613 }
614
615
616 if (can_id & CAN_RTR_FLAG)
617 return matches;
618
619 if (can_id & CAN_EFF_FLAG) {
620 hlist_for_each_entry_rcu(r, n, &d->rx[RX_EFF], list) {
621 if (r->can_id == can_id) {
622 deliver(skb, r);
623 matches++;
624 }
625 }
626 } else {
627 can_id &= CAN_SFF_MASK;
628 hlist_for_each_entry_rcu(r, n, &d->rx_sff[can_id], list) {
629 deliver(skb, r);
630 matches++;
631 }
632 }
633
634 return matches;
635}
636
637static int can_rcv(struct sk_buff *skb, struct net_device *dev,
638 struct packet_type *pt, struct net_device *orig_dev)
639{
640 struct dev_rcv_lists *d;
641 struct can_frame *cf = (struct can_frame *)skb->data;
642 int matches;
643
644 if (!net_eq(dev_net(dev), &init_net))
645 goto drop;
646
647 if (WARN_ONCE(dev->type != ARPHRD_CAN ||
648 skb->len != sizeof(struct can_frame) ||
649 cf->can_dlc > 8,
650 "PF_CAN: dropped non conform skbuf: "
651 "dev type %d, len %d, can_dlc %d\n",
652 dev->type, skb->len, cf->can_dlc))
653 goto drop;
654
655
656 can_stats.rx_frames++;
657 can_stats.rx_frames_delta++;
658
659 rcu_read_lock();
660
661
662 matches = can_rcv_filter(&can_rx_alldev_list, skb);
663
664
665 d = find_dev_rcv_lists(dev);
666 if (d)
667 matches += can_rcv_filter(d, skb);
668
669 rcu_read_unlock();
670
671
672 consume_skb(skb);
673
674 if (matches > 0) {
675 can_stats.matches++;
676 can_stats.matches_delta++;
677 }
678
679 return NET_RX_SUCCESS;
680
681drop:
682 kfree_skb(skb);
683 return NET_RX_DROP;
684}
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700int can_proto_register(const struct can_proto *cp)
701{
702 int proto = cp->protocol;
703 int err = 0;
704
705 if (proto < 0 || proto >= CAN_NPROTO) {
706 printk(KERN_ERR "can: protocol number %d out of range\n",
707 proto);
708 return -EINVAL;
709 }
710
711 err = proto_register(cp->prot, 0);
712 if (err < 0)
713 return err;
714
715 mutex_lock(&proto_tab_lock);
716
717 if (proto_tab[proto]) {
718 printk(KERN_ERR "can: protocol %d already registered\n",
719 proto);
720 err = -EBUSY;
721 } else
722 rcu_assign_pointer(proto_tab[proto], cp);
723
724 mutex_unlock(&proto_tab_lock);
725
726 if (err < 0)
727 proto_unregister(cp->prot);
728
729 return err;
730}
731EXPORT_SYMBOL(can_proto_register);
732
733
734
735
736
737void can_proto_unregister(const struct can_proto *cp)
738{
739 int proto = cp->protocol;
740
741 mutex_lock(&proto_tab_lock);
742 BUG_ON(proto_tab[proto] != cp);
743 rcu_assign_pointer(proto_tab[proto], NULL);
744 mutex_unlock(&proto_tab_lock);
745
746 synchronize_rcu();
747
748 proto_unregister(cp->prot);
749}
750EXPORT_SYMBOL(can_proto_unregister);
751
752
753
754
755static int can_notifier(struct notifier_block *nb, unsigned long msg,
756 void *data)
757{
758 struct net_device *dev = (struct net_device *)data;
759 struct dev_rcv_lists *d;
760
761 if (!net_eq(dev_net(dev), &init_net))
762 return NOTIFY_DONE;
763
764 if (dev->type != ARPHRD_CAN)
765 return NOTIFY_DONE;
766
767 switch (msg) {
768
769 case NETDEV_REGISTER:
770
771
772 d = kzalloc(sizeof(*d), GFP_KERNEL);
773 if (!d) {
774 printk(KERN_ERR
775 "can: allocation of receive list failed\n");
776 return NOTIFY_DONE;
777 }
778 BUG_ON(dev->ml_priv);
779 dev->ml_priv = d;
780
781 break;
782
783 case NETDEV_UNREGISTER:
784 spin_lock(&can_rcvlists_lock);
785
786 d = dev->ml_priv;
787 if (d) {
788 if (d->entries)
789 d->remove_on_zero_entries = 1;
790 else {
791 kfree(d);
792 dev->ml_priv = NULL;
793 }
794 } else
795 printk(KERN_ERR "can: notifier: receive list not "
796 "found for dev %s\n", dev->name);
797
798 spin_unlock(&can_rcvlists_lock);
799
800 break;
801 }
802
803 return NOTIFY_DONE;
804}
805
806
807
808
809
810static struct packet_type can_packet __read_mostly = {
811 .type = cpu_to_be16(ETH_P_CAN),
812 .dev = NULL,
813 .func = can_rcv,
814};
815
816static const struct net_proto_family can_family_ops = {
817 .family = PF_CAN,
818 .create = can_create,
819 .owner = THIS_MODULE,
820};
821
822
823static struct notifier_block can_netdev_notifier __read_mostly = {
824 .notifier_call = can_notifier,
825};
826
827static __init int can_init(void)
828{
829 printk(banner);
830
831 memset(&can_rx_alldev_list, 0, sizeof(can_rx_alldev_list));
832
833 rcv_cache = kmem_cache_create("can_receiver", sizeof(struct receiver),
834 0, 0, NULL);
835 if (!rcv_cache)
836 return -ENOMEM;
837
838 if (stats_timer) {
839
840 setup_timer(&can_stattimer, can_stat_update, 0);
841 mod_timer(&can_stattimer, round_jiffies(jiffies + HZ));
842 } else
843 can_stattimer.function = NULL;
844
845 can_init_proc();
846
847
848 sock_register(&can_family_ops);
849 register_netdevice_notifier(&can_netdev_notifier);
850 dev_add_pack(&can_packet);
851
852 return 0;
853}
854
855static __exit void can_exit(void)
856{
857 struct net_device *dev;
858
859 if (stats_timer)
860 del_timer_sync(&can_stattimer);
861
862 can_remove_proc();
863
864
865 dev_remove_pack(&can_packet);
866 unregister_netdevice_notifier(&can_netdev_notifier);
867 sock_unregister(PF_CAN);
868
869
870 rcu_read_lock();
871 for_each_netdev_rcu(&init_net, dev) {
872 if (dev->type == ARPHRD_CAN && dev->ml_priv){
873
874 struct dev_rcv_lists *d = dev->ml_priv;
875
876 BUG_ON(d->entries);
877 kfree(d);
878 dev->ml_priv = NULL;
879 }
880 }
881 rcu_read_unlock();
882
883 rcu_barrier();
884
885 kmem_cache_destroy(rcv_cache);
886}
887
888module_init(can_init);
889module_exit(can_exit);
890