source: src/linux/universal/linux-3.18/net/can/af_can.c @ 31869

Last change on this file since 31869 was 31869, checked in by brainslayer, 2 months ago

update

File size: 24.0 KB
Line 
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
67static __initconst 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/* receive filters subscribed for 'all' CAN devices */
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/* table of registered CAN protocols */
88static const struct can_proto *proto_tab[CAN_NPROTO] __read_mostly;
89static DEFINE_MUTEX(proto_tab_lock);
90
91struct timer_list can_stattimer;   /* timer for statistics update */
92struct s_stats    can_stats;       /* packet statistics */
93struct s_pstats   can_pstats;      /* receive list statistics */
94
95/*
96 * af_can socket functions
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                /* 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 */
228int 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
325inval_skb:
326        kfree_skb(skb);
327        return err;
328}
329EXPORT_SYMBOL(can_send);
330
331/*
332 * af_can rx path
333 */
334
335static 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 */
355static 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 */
393static 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 */
471int 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}
518EXPORT_SYMBOL(can_rx_register);
519
520/*
521 * can_rx_delete_receiver - rcu callback for single receiver entry removal
522 */
523static 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 */
544void 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}
611EXPORT_SYMBOL(can_rx_unregister);
612
613static inline void deliver(struct sk_buff *skb, struct receiver *r)
614{
615        r->func(skb, r->data);
616        r->matches++;
617}
618
619static 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
684static 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
714static 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 (WARN_ONCE(dev->type != ARPHRD_CAN ||
723                      skb->len != CAN_MTU ||
724                      cfd->len > CAN_MAX_DLEN,
725                      "PF_CAN: dropped non conform CAN skbuf: "
726                      "dev type %d, len %d, datalen %d\n",
727                      dev->type, skb->len, cfd->len))
728                goto drop;
729
730        can_receive(skb, dev);
731        return NET_RX_SUCCESS;
732
733drop:
734        kfree_skb(skb);
735        return NET_RX_DROP;
736}
737
738static int canfd_rcv(struct sk_buff *skb, struct net_device *dev,
739                   struct packet_type *pt, struct net_device *orig_dev)
740{
741        struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
742
743        if (unlikely(!net_eq(dev_net(dev), &init_net)))
744                goto drop;
745
746        if (WARN_ONCE(dev->type != ARPHRD_CAN ||
747                      skb->len != CANFD_MTU ||
748                      cfd->len > CANFD_MAX_DLEN,
749                      "PF_CAN: dropped non conform CAN FD skbuf: "
750                      "dev type %d, len %d, datalen %d\n",
751                      dev->type, skb->len, cfd->len))
752                goto drop;
753
754        can_receive(skb, dev);
755        return NET_RX_SUCCESS;
756
757drop:
758        kfree_skb(skb);
759        return NET_RX_DROP;
760}
761
762/*
763 * af_can protocol functions
764 */
765
766/**
767 * can_proto_register - register CAN transport protocol
768 * @cp: pointer to CAN protocol structure
769 *
770 * Return:
771 *  0 on success
772 *  -EINVAL invalid (out of range) protocol number
773 *  -EBUSY  protocol already in use
774 *  -ENOBUF if proto_register() fails
775 */
776int can_proto_register(const struct can_proto *cp)
777{
778        int proto = cp->protocol;
779        int err = 0;
780
781        if (proto < 0 || proto >= CAN_NPROTO) {
782                pr_err("can: protocol number %d out of range\n", proto);
783                return -EINVAL;
784        }
785
786        err = proto_register(cp->prot, 0);
787        if (err < 0)
788                return err;
789
790        mutex_lock(&proto_tab_lock);
791
792        if (proto_tab[proto]) {
793                pr_err("can: protocol %d already registered\n", proto);
794                err = -EBUSY;
795        } else
796                RCU_INIT_POINTER(proto_tab[proto], cp);
797
798        mutex_unlock(&proto_tab_lock);
799
800        if (err < 0)
801                proto_unregister(cp->prot);
802
803        return err;
804}
805EXPORT_SYMBOL(can_proto_register);
806
807/**
808 * can_proto_unregister - unregister CAN transport protocol
809 * @cp: pointer to CAN protocol structure
810 */
811void can_proto_unregister(const struct can_proto *cp)
812{
813        int proto = cp->protocol;
814
815        mutex_lock(&proto_tab_lock);
816        BUG_ON(proto_tab[proto] != cp);
817        RCU_INIT_POINTER(proto_tab[proto], NULL);
818        mutex_unlock(&proto_tab_lock);
819
820        synchronize_rcu();
821
822        proto_unregister(cp->prot);
823}
824EXPORT_SYMBOL(can_proto_unregister);
825
826/*
827 * af_can notifier to create/remove CAN netdevice specific structs
828 */
829static int can_notifier(struct notifier_block *nb, unsigned long msg,
830                        void *ptr)
831{
832        struct net_device *dev = netdev_notifier_info_to_dev(ptr);
833        struct dev_rcv_lists *d;
834
835        if (!net_eq(dev_net(dev), &init_net))
836                return NOTIFY_DONE;
837
838        if (dev->type != ARPHRD_CAN)
839                return NOTIFY_DONE;
840
841        switch (msg) {
842
843        case NETDEV_REGISTER:
844
845                /* create new dev_rcv_lists for this device */
846                d = kzalloc(sizeof(*d), GFP_KERNEL);
847                if (!d)
848                        return NOTIFY_DONE;
849                BUG_ON(dev->ml_priv);
850                dev->ml_priv = d;
851
852                break;
853
854        case NETDEV_UNREGISTER:
855                spin_lock(&can_rcvlists_lock);
856
857                d = dev->ml_priv;
858                if (d) {
859                        if (d->entries)
860                                d->remove_on_zero_entries = 1;
861                        else {
862                                kfree(d);
863                                dev->ml_priv = NULL;
864                        }
865                } else
866                        pr_err("can: notifier: receive list not found for dev "
867                               "%s\n", dev->name);
868
869                spin_unlock(&can_rcvlists_lock);
870
871                break;
872        }
873
874        return NOTIFY_DONE;
875}
876
877/*
878 * af_can module init/exit functions
879 */
880
881static struct packet_type can_packet __read_mostly = {
882        .type = cpu_to_be16(ETH_P_CAN),
883        .func = can_rcv,
884};
885
886static struct packet_type canfd_packet __read_mostly = {
887        .type = cpu_to_be16(ETH_P_CANFD),
888        .func = canfd_rcv,
889};
890
891static const struct net_proto_family can_family_ops = {
892        .family = PF_CAN,
893        .create = can_create,
894        .owner  = THIS_MODULE,
895};
896
897/* notifier block for netdevice event */
898static struct notifier_block can_netdev_notifier __read_mostly = {
899        .notifier_call = can_notifier,
900};
901
902static __init int can_init(void)
903{
904        /* check for correct padding to be able to use the structs similarly */
905        BUILD_BUG_ON(offsetof(struct can_frame, can_dlc) !=
906                     offsetof(struct canfd_frame, len) ||
907                     offsetof(struct can_frame, data) !=
908                     offsetof(struct canfd_frame, data));
909
910        printk(banner);
911
912        memset(&can_rx_alldev_list, 0, sizeof(can_rx_alldev_list));
913
914        rcv_cache = kmem_cache_create("can_receiver", sizeof(struct receiver),
915                                      0, 0, NULL);
916        if (!rcv_cache)
917                return -ENOMEM;
918
919        if (stats_timer) {
920                /* the statistics are updated every second (timer triggered) */
921                setup_timer(&can_stattimer, can_stat_update, 0);
922                mod_timer(&can_stattimer, round_jiffies(jiffies + HZ));
923        } else
924                can_stattimer.function = NULL;
925
926        can_init_proc();
927
928        /* protocol register */
929        sock_register(&can_family_ops);
930        register_netdevice_notifier(&can_netdev_notifier);
931        dev_add_pack(&can_packet);
932        dev_add_pack(&canfd_packet);
933
934        return 0;
935}
936
937static __exit void can_exit(void)
938{
939        struct net_device *dev;
940
941        if (stats_timer)
942                del_timer_sync(&can_stattimer);
943
944        can_remove_proc();
945
946        /* protocol unregister */
947        dev_remove_pack(&canfd_packet);
948        dev_remove_pack(&can_packet);
949        unregister_netdevice_notifier(&can_netdev_notifier);
950        sock_unregister(PF_CAN);
951
952        /* remove created dev_rcv_lists from still registered CAN devices */
953        rcu_read_lock();
954        for_each_netdev_rcu(&init_net, dev) {
955                if (dev->type == ARPHRD_CAN && dev->ml_priv) {
956
957                        struct dev_rcv_lists *d = dev->ml_priv;
958
959                        BUG_ON(d->entries);
960                        kfree(d);
961                        dev->ml_priv = NULL;
962                }
963        }
964        rcu_read_unlock();
965
966        rcu_barrier(); /* Wait for completion of call_rcu()'s */
967
968        kmem_cache_destroy(rcv_cache);
969}
970
971module_init(can_init);
972module_exit(can_exit);
Note: See TracBrowser for help on using the repository browser.