net/can/af_can.c

0001 // SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
0002 /* af_can.c - Protocol family CAN core module
0003  *            (used by different CAN protocol modules)
0004  *
0005  * Copyright (c) 2002-2017 Volkswagen Group Electronic Research
0006  * All rights reserved.
0007  *
0008  * Redistribution and use in source and binary forms, with or without
0009  * modification, are permitted provided that the following conditions
0010  * are met:
0011  * 1. Redistributions of source code must retain the above copyright
0012  *    notice, this list of conditions and the following disclaimer.
0013  * 2. Redistributions in binary form must reproduce the above copyright
0014  *    notice, this list of conditions and the following disclaimer in the
0015  *    documentation and/or other materials provided with the distribution.
0016  * 3. Neither the name of Volkswagen nor the names of its contributors
0017  *    may be used to endorse or promote products derived from this software
0018  *    without specific prior written permission.
0019  *
0020  * Alternatively, provided that this notice is retained in full, this
0021  * software may be distributed under the terms of the GNU General
0022  * Public License ("GPL") version 2, in which case the provisions of the
0023  * GPL apply INSTEAD OF those given above.
0024  *
0025  * The provided data structures and external interfaces from this code
0026  * are not restricted to be used by modules with a GPL compatible license.
0027  *
0028  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
0029  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
0030  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
0031  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
0032  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
0033  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
0034  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
0035  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
0036  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
0037  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
0038  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
0039  * DAMAGE.
0040  *
0041  */
0042
0043 #include <linux/module.h>
0044 #include <linux/stddef.h>
0045 #include <linux/init.h>
0046 #include <linux/kmod.h>
0047 #include <linux/slab.h>
0048 #include <linux/list.h>
0049 #include <linux/spinlock.h>
0050 #include <linux/rcupdate.h>
0051 #include <linux/uaccess.h>
0052 #include <linux/net.h>
0053 #include <linux/netdevice.h>
0054 #include <linux/socket.h>
0055 #include <linux/if_ether.h>
0056 #include <linux/if_arp.h>
0057 #include <linux/skbuff.h>
0058 #include <linux/can.h>
0059 #include <linux/can/core.h>
0060 #include <linux/can/skb.h>
0061 #include <linux/can/can-ml.h>
0062 #include <linux/ratelimit.h>
0063 #include <net/net_namespace.h>
0064 #include <net/sock.h>
0065
0066 #include "af_can.h"
0067
0068 MODULE_DESCRIPTION("Controller Area Network PF_CAN core");
0069 MODULE_LICENSE("Dual BSD/GPL");
0070 MODULE_AUTHOR("Urs Thuermann <urs.thuermann@volkswagen.de>, "
0071           "Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
0072
0073 MODULE_ALIAS_NETPROTO(PF_CAN);
0074
0075 static int stats_timer __read_mostly = 1;
0076 module_param(stats_timer, int, 0444);
0077 MODULE_PARM_DESC(stats_timer, "enable timer for statistics (default:on)");
0078
0079 static struct kmem_cache *rcv_cache __read_mostly;
0080
0081 /* table of registered CAN protocols */
0082 static const struct can_proto __rcu *proto_tab[CAN_NPROTO] __read_mostly;
0083 static DEFINE_MUTEX(proto_tab_lock);
0084
0085 static atomic_t skbcounter = ATOMIC_INIT(0);
0086
0087 /* af_can socket functions */
0088
0089 void can_sock_destruct(struct sock *sk)
0090 {
0091     skb_queue_purge(&sk->sk_receive_queue);
0092     skb_queue_purge(&sk->sk_error_queue);
0093 }
0094 EXPORT_SYMBOL(can_sock_destruct);
0095
0096 static const struct can_proto *can_get_proto(int protocol)
0097 {
0098     const struct can_proto *cp;
0099
0100     rcu_read_lock();
0101     cp = rcu_dereference(proto_tab[protocol]);
0102     if (cp && !try_module_get(cp->prot->owner))
0103         cp = NULL;
0104     rcu_read_unlock();
0105
0106     return cp;
0107 }
0108
0109 static inline void can_put_proto(const struct can_proto *cp)
0110 {
0111     module_put(cp->prot->owner);
0112 }
0113
0114 static int can_create(struct net *net, struct socket *sock, int protocol,
0115               int kern)
0116 {
0117     struct sock *sk;
0118     const struct can_proto *cp;
0119     int err = 0;
0120
0121     sock->state = SS_UNCONNECTED;
0122
0123     if (protocol < 0 || protocol >= CAN_NPROTO)
0124         return -EINVAL;
0125
0126     cp = can_get_proto(protocol);
0127
0128 #ifdef CONFIG_MODULES
0129     if (!cp) {
0130         /* try to load protocol module if kernel is modular */
0131
0132         err = request_module("can-proto-%d", protocol);
0133
0134         /* In case of error we only print a message but don't
0135          * return the error code immediately.  Below we will
0136          * return -EPROTONOSUPPORT
0137          */
0138         if (err)
0139             pr_err_ratelimited("can: request_module (can-proto-%d) failed.\n",
0140                        protocol);
0141
0142         cp = can_get_proto(protocol);
0143     }
0144 #endif
0145
0146     /* check for available protocol and correct usage */
0147
0148     if (!cp)
0149         return -EPROTONOSUPPORT;
0150
0151     if (cp->type != sock->type) {
0152         err = -EPROTOTYPE;
0153         goto errout;
0154     }
0155
0156     sock->ops = cp->ops;
0157
0158     sk = sk_alloc(net, PF_CAN, GFP_KERNEL, cp->prot, kern);
0159     if (!sk) {
0160         err = -ENOMEM;
0161         goto errout;
0162     }
0163
0164     sock_init_data(sock, sk);
0165     sk->sk_destruct = can_sock_destruct;
0166
0167     if (sk->sk_prot->init)
0168         err = sk->sk_prot->init(sk);
0169
0170     if (err) {
0171         /* release sk on errors */
0172         sock_orphan(sk);
0173         sock_put(sk);
0174     }
0175
0176  errout:
0177     can_put_proto(cp);
0178     return err;
0179 }
0180
0181 /* af_can tx path */
0182
0183 /**
0184  * can_send - transmit a CAN frame (optional with local loopback)
0185  * @skb: pointer to socket buffer with CAN frame in data section
0186  * @loop: loopback for listeners on local CAN sockets (recommended default!)
0187  *
0188  * Due to the loopback this routine must not be called from hardirq context.
0189  *
0190  * Return:
0191  *  0 on success
0192  *  -ENETDOWN when the selected interface is down
0193  *  -ENOBUFS on full driver queue (see net_xmit_errno())
0194  *  -ENOMEM when local loopback failed at calling skb_clone()
0195  *  -EPERM when trying to send on a non-CAN interface
0196  *  -EMSGSIZE CAN frame size is bigger than CAN interface MTU
0197  *  -EINVAL when the skb->data does not contain a valid CAN frame
0198  */
0199 int can_send(struct sk_buff *skb, int loop)
0200 {
0201     struct sk_buff *newskb = NULL;
0202     struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
0203     struct can_pkg_stats *pkg_stats = dev_net(skb->dev)->can.pkg_stats;
0204     int err = -EINVAL;
0205
0206     if (skb->len == CAN_MTU) {
0207         skb->protocol = htons(ETH_P_CAN);
0208         if (unlikely(cfd->len > CAN_MAX_DLEN))
0209             goto inval_skb;
0210     } else if (skb->len == CANFD_MTU) {
0211         skb->protocol = htons(ETH_P_CANFD);
0212         if (unlikely(cfd->len > CANFD_MAX_DLEN))
0213             goto inval_skb;
0214     } else {
0215         goto inval_skb;
0216     }
0217
0218     /* Make sure the CAN frame can pass the selected CAN netdevice.
0219      * As structs can_frame and canfd_frame are similar, we can provide
0220      * CAN FD frames to legacy CAN drivers as long as the length is <= 8
0221      */
0222     if (unlikely(skb->len > skb->dev->mtu && cfd->len > CAN_MAX_DLEN)) {
0223         err = -EMSGSIZE;
0224         goto inval_skb;
0225     }
0226
0227     if (unlikely(skb->dev->type != ARPHRD_CAN)) {
0228         err = -EPERM;
0229         goto inval_skb;
0230     }
0231
0232     if (unlikely(!(skb->dev->flags & IFF_UP))) {
0233         err = -ENETDOWN;
0234         goto inval_skb;
0235     }
0236
0237     skb->ip_summed = CHECKSUM_UNNECESSARY;
0238
0239     skb_reset_mac_header(skb);
0240     skb_reset_network_header(skb);
0241     skb_reset_transport_header(skb);
0242
0243     if (loop) {
0244         /* local loopback of sent CAN frames */
0245
0246         /* indication for the CAN driver: do loopback */
0247         skb->pkt_type = PACKET_LOOPBACK;
0248
0249         /* The reference to the originating sock may be required
0250          * by the receiving socket to check whether the frame is
0251          * its own. Example: can_raw sockopt CAN_RAW_RECV_OWN_MSGS
0252          * Therefore we have to ensure that skb->sk remains the
0253          * reference to the originating sock by restoring skb->sk
0254          * after each skb_clone() or skb_orphan() usage.
0255          */
0256
0257         if (!(skb->dev->flags & IFF_ECHO)) {
0258             /* If the interface is not capable to do loopback
0259              * itself, we do it here.
0260              */
0261             newskb = skb_clone(skb, GFP_ATOMIC);
0262             if (!newskb) {
0263                 kfree_skb(skb);
0264                 return -ENOMEM;
0265             }
0266
0267             can_skb_set_owner(newskb, skb->sk);
0268             newskb->ip_summed = CHECKSUM_UNNECESSARY;
0269             newskb->pkt_type = PACKET_BROADCAST;
0270         }
0271     } else {
0272         /* indication for the CAN driver: no loopback required */
0273         skb->pkt_type = PACKET_HOST;
0274     }
0275
0276     /* send to netdevice */
0277     err = dev_queue_xmit(skb);
0278     if (err > 0)
0279         err = net_xmit_errno(err);
0280
0281     if (err) {
0282         kfree_skb(newskb);
0283         return err;
0284     }
0285
0286     if (newskb)
0287         netif_rx(newskb);
0288
0289     /* update statistics */
0290     pkg_stats->tx_frames++;
0291     pkg_stats->tx_frames_delta++;
0292
0293     return 0;
0294
0295 inval_skb:
0296     kfree_skb(skb);
0297     return err;
0298 }
0299 EXPORT_SYMBOL(can_send);
0300
0301 /* af_can rx path */
0302
0303 static struct can_dev_rcv_lists *can_dev_rcv_lists_find(struct net *net,
0304                             struct net_device *dev)
0305 {
0306     if (dev) {
0307         struct can_ml_priv *can_ml = can_get_ml_priv(dev);
0308         return &can_ml->dev_rcv_lists;
0309     } else {
0310         return net->can.rx_alldev_list;
0311     }
0312 }
0313
0314 /**
0315  * effhash - hash function for 29 bit CAN identifier reduction
0316  * @can_id: 29 bit CAN identifier
0317  *
0318  * Description:
0319  *  To reduce the linear traversal in one linked list of _single_ EFF CAN
0320  *  frame subscriptions the 29 bit identifier is mapped to 10 bits.
0321  *  (see CAN_EFF_RCV_HASH_BITS definition)
0322  *
0323  * Return:
0324  *  Hash value from 0x000 - 0x3FF ( enforced by CAN_EFF_RCV_HASH_BITS mask )
0325  */
0326 static unsigned int effhash(canid_t can_id)
0327 {
0328     unsigned int hash;
0329
0330     hash = can_id;
0331     hash ^= can_id >> CAN_EFF_RCV_HASH_BITS;
0332     hash ^= can_id >> (2 * CAN_EFF_RCV_HASH_BITS);
0333
0334     return hash & ((1 << CAN_EFF_RCV_HASH_BITS) - 1);
0335 }
0336
0337 /**
0338  * can_rcv_list_find - determine optimal filterlist inside device filter struct
0339  * @can_id: pointer to CAN identifier of a given can_filter
0340  * @mask: pointer to CAN mask of a given can_filter
0341  * @dev_rcv_lists: pointer to the device filter struct
0342  *
0343  * Description:
0344  *  Returns the optimal filterlist to reduce the filter handling in the
0345  *  receive path. This function is called by service functions that need
0346  *  to register or unregister a can_filter in the filter lists.
0347  *
0348  *  A filter matches in general, when
0349  *
0350  *          <received_can_id> & mask == can_id & mask
0351  *
0352  *  so every bit set in the mask (even CAN_EFF_FLAG, CAN_RTR_FLAG) describe
0353  *  relevant bits for the filter.
0354  *
0355  *  The filter can be inverted (CAN_INV_FILTER bit set in can_id) or it can
0356  *  filter for error messages (CAN_ERR_FLAG bit set in mask). For error msg
0357  *  frames there is a special filterlist and a special rx path filter handling.
0358  *
0359  * Return:
0360  *  Pointer to optimal filterlist for the given can_id/mask pair.
0361  *  Consistency checked mask.
0362  *  Reduced can_id to have a preprocessed filter compare value.
0363  */
0364 static struct hlist_head *can_rcv_list_find(canid_t *can_id, canid_t *mask,
0365                         struct can_dev_rcv_lists *dev_rcv_lists)
0366 {
0367     canid_t inv = *can_id & CAN_INV_FILTER; /* save flag before masking */
0368
0369     /* filter for error message frames in extra filterlist */
0370     if (*mask & CAN_ERR_FLAG) {
0371         /* clear CAN_ERR_FLAG in filter entry */
0372         *mask &= CAN_ERR_MASK;
0373         return &dev_rcv_lists->rx[RX_ERR];
0374     }
0375
0376     /* with cleared CAN_ERR_FLAG we have a simple mask/value filterpair */
0377
0378 #define CAN_EFF_RTR_FLAGS (CAN_EFF_FLAG | CAN_RTR_FLAG)
0379
0380     /* ensure valid values in can_mask for 'SFF only' frame filtering */
0381     if ((*mask & CAN_EFF_FLAG) && !(*can_id & CAN_EFF_FLAG))
0382         *mask &= (CAN_SFF_MASK | CAN_EFF_RTR_FLAGS);
0383
0384     /* reduce condition testing at receive time */
0385     *can_id &= *mask;
0386
0387     /* inverse can_id/can_mask filter */
0388     if (inv)
0389         return &dev_rcv_lists->rx[RX_INV];
0390
0391     /* mask == 0 => no condition testing at receive time */
0392     if (!(*mask))
0393         return &dev_rcv_lists->rx[RX_ALL];
0394
0395     /* extra filterlists for the subscription of a single non-RTR can_id */
0396     if (((*mask & CAN_EFF_RTR_FLAGS) == CAN_EFF_RTR_FLAGS) &&
0397         !(*can_id & CAN_RTR_FLAG)) {
0398         if (*can_id & CAN_EFF_FLAG) {
0399             if (*mask == (CAN_EFF_MASK | CAN_EFF_RTR_FLAGS))
0400                 return &dev_rcv_lists->rx_eff[effhash(*can_id)];
0401         } else {
0402             if (*mask == (CAN_SFF_MASK | CAN_EFF_RTR_FLAGS))
0403                 return &dev_rcv_lists->rx_sff[*can_id];
0404         }
0405     }
0406
0407     /* default: filter via can_id/can_mask */
0408     return &dev_rcv_lists->rx[RX_FIL];
0409 }
0410
0411 /**
0412  * can_rx_register - subscribe CAN frames from a specific interface
0413  * @net: the applicable net namespace
0414  * @dev: pointer to netdevice (NULL => subscribe from 'all' CAN devices list)
0415  * @can_id: CAN identifier (see description)
0416  * @mask: CAN mask (see description)
0417  * @func: callback function on filter match
0418  * @data: returned parameter for callback function
0419  * @ident: string for calling module identification
0420  * @sk: socket pointer (might be NULL)
0421  *
0422  * Description:
0423  *  Invokes the callback function with the received sk_buff and the given
0424  *  parameter 'data' on a matching receive filter. A filter matches, when
0425  *
0426  *          <received_can_id> & mask == can_id & mask
0427  *
0428  *  The filter can be inverted (CAN_INV_FILTER bit set in can_id) or it can
0429  *  filter for error message frames (CAN_ERR_FLAG bit set in mask).
0430  *
0431  *  The provided pointer to the sk_buff is guaranteed to be valid as long as
0432  *  the callback function is running. The callback function must *not* free
0433  *  the given sk_buff while processing it's task. When the given sk_buff is
0434  *  needed after the end of the callback function it must be cloned inside
0435  *  the callback function with skb_clone().
0436  *
0437  * Return:
0438  *  0 on success
0439  *  -ENOMEM on missing cache mem to create subscription entry
0440  *  -ENODEV unknown device
0441  */
0442 int can_rx_register(struct net *net, struct net_device *dev, canid_t can_id,
0443             canid_t mask, void (*func)(struct sk_buff *, void *),
0444             void *data, char *ident, struct sock *sk)
0445 {
0446     struct receiver *rcv;
0447     struct hlist_head *rcv_list;
0448     struct can_dev_rcv_lists *dev_rcv_lists;
0449     struct can_rcv_lists_stats *rcv_lists_stats = net->can.rcv_lists_stats;
0450     int err = 0;
0451
0452     /* insert new receiver  (dev,canid,mask) -> (func,data) */
0453
0454     if (dev && dev->type != ARPHRD_CAN)
0455         return -ENODEV;
0456
0457     if (dev && !net_eq(net, dev_net(dev)))
0458         return -ENODEV;
0459
0460     rcv = kmem_cache_alloc(rcv_cache, GFP_KERNEL);
0461     if (!rcv)
0462         return -ENOMEM;
0463
0464     spin_lock_bh(&net->can.rcvlists_lock);
0465
0466     dev_rcv_lists = can_dev_rcv_lists_find(net, dev);
0467     rcv_list = can_rcv_list_find(&can_id, &mask, dev_rcv_lists);
0468
0469     rcv->can_id = can_id;
0470     rcv->mask = mask;
0471     rcv->matches = 0;
0472     rcv->func = func;
0473     rcv->data = data;
0474     rcv->ident = ident;
0475     rcv->sk = sk;
0476
0477     hlist_add_head_rcu(&rcv->list, rcv_list);
0478     dev_rcv_lists->entries++;
0479
0480     rcv_lists_stats->rcv_entries++;
0481     rcv_lists_stats->rcv_entries_max = max(rcv_lists_stats->rcv_entries_max,
0482                            rcv_lists_stats->rcv_entries);
0483     spin_unlock_bh(&net->can.rcvlists_lock);
0484
0485     return err;
0486 }
0487 EXPORT_SYMBOL(can_rx_register);
0488
0489 /* can_rx_delete_receiver - rcu callback for single receiver entry removal */
0490 static void can_rx_delete_receiver(struct rcu_head *rp)
0491 {
0492     struct receiver *rcv = container_of(rp, struct receiver, rcu);
0493     struct sock *sk = rcv->sk;
0494
0495     kmem_cache_free(rcv_cache, rcv);
0496     if (sk)
0497         sock_put(sk);
0498 }
0499
0500 /**
0501  * can_rx_unregister - unsubscribe CAN frames from a specific interface
0502  * @net: the applicable net namespace
0503  * @dev: pointer to netdevice (NULL => unsubscribe from 'all' CAN devices list)
0504  * @can_id: CAN identifier
0505  * @mask: CAN mask
0506  * @func: callback function on filter match
0507  * @data: returned parameter for callback function
0508  *
0509  * Description:
0510  *  Removes subscription entry depending on given (subscription) values.
0511  */
0512 void can_rx_unregister(struct net *net, struct net_device *dev, canid_t can_id,
0513                canid_t mask, void (*func)(struct sk_buff *, void *),
0514                void *data)
0515 {
0516     struct receiver *rcv = NULL;
0517     struct hlist_head *rcv_list;
0518     struct can_rcv_lists_stats *rcv_lists_stats = net->can.rcv_lists_stats;
0519     struct can_dev_rcv_lists *dev_rcv_lists;
0520
0521     if (dev && dev->type != ARPHRD_CAN)
0522         return;
0523
0524     if (dev && !net_eq(net, dev_net(dev)))
0525         return;
0526
0527     spin_lock_bh(&net->can.rcvlists_lock);
0528
0529     dev_rcv_lists = can_dev_rcv_lists_find(net, dev);
0530     rcv_list = can_rcv_list_find(&can_id, &mask, dev_rcv_lists);
0531
0532     /* Search the receiver list for the item to delete.  This should
0533      * exist, since no receiver may be unregistered that hasn't
0534      * been registered before.
0535      */
0536     hlist_for_each_entry_rcu(rcv, rcv_list, list) {
0537         if (rcv->can_id == can_id && rcv->mask == mask &&
0538             rcv->func == func && rcv->data == data)
0539             break;
0540     }
0541
0542     /* Check for bugs in CAN protocol implementations using af_can.c:
0543      * 'rcv' will be NULL if no matching list item was found for removal.
0544      * As this case may potentially happen when closing a socket while
0545      * the notifier for removing the CAN netdev is running we just print
0546      * a warning here.
0547      */
0548     if (!rcv) {
0549         pr_warn("can: receive list entry not found for dev %s, id %03X, mask %03X\n",
0550             DNAME(dev), can_id, mask);
0551         goto out;
0552     }
0553
0554     hlist_del_rcu(&rcv->list);
0555     dev_rcv_lists->entries--;
0556
0557     if (rcv_lists_stats->rcv_entries > 0)
0558         rcv_lists_stats->rcv_entries--;
0559
0560  out:
0561     spin_unlock_bh(&net->can.rcvlists_lock);
0562
0563     /* schedule the receiver item for deletion */
0564     if (rcv) {
0565         if (rcv->sk)
0566             sock_hold(rcv->sk);
0567         call_rcu(&rcv->rcu, can_rx_delete_receiver);
0568     }
0569 }
0570 EXPORT_SYMBOL(can_rx_unregister);
0571
0572 static inline void deliver(struct sk_buff *skb, struct receiver *rcv)
0573 {
0574     rcv->func(skb, rcv->data);
0575     rcv->matches++;
0576 }
0577
0578 static int can_rcv_filter(struct can_dev_rcv_lists *dev_rcv_lists, struct sk_buff *skb)
0579 {
0580     struct receiver *rcv;
0581     int matches = 0;
0582     struct can_frame *cf = (struct can_frame *)skb->data;
0583     canid_t can_id = cf->can_id;
0584
0585     if (dev_rcv_lists->entries == 0)
0586         return 0;
0587
0588     if (can_id & CAN_ERR_FLAG) {
0589         /* check for error message frame entries only */
0590         hlist_for_each_entry_rcu(rcv, &dev_rcv_lists->rx[RX_ERR], list) {
0591             if (can_id & rcv->mask) {
0592                 deliver(skb, rcv);
0593                 matches++;
0594             }
0595         }
0596         return matches;
0597     }
0598
0599     /* check for unfiltered entries */
0600     hlist_for_each_entry_rcu(rcv, &dev_rcv_lists->rx[RX_ALL], list) {
0601         deliver(skb, rcv);
0602         matches++;
0603     }
0604
0605     /* check for can_id/mask entries */
0606     hlist_for_each_entry_rcu(rcv, &dev_rcv_lists->rx[RX_FIL], list) {
0607         if ((can_id & rcv->mask) == rcv->can_id) {
0608             deliver(skb, rcv);
0609             matches++;
0610         }
0611     }
0612
0613     /* check for inverted can_id/mask entries */
0614     hlist_for_each_entry_rcu(rcv, &dev_rcv_lists->rx[RX_INV], list) {
0615         if ((can_id & rcv->mask) != rcv->can_id) {
0616             deliver(skb, rcv);
0617             matches++;
0618         }
0619     }
0620
0621     /* check filterlists for single non-RTR can_ids */
0622     if (can_id & CAN_RTR_FLAG)
0623         return matches;
0624
0625     if (can_id & CAN_EFF_FLAG) {
0626         hlist_for_each_entry_rcu(rcv, &dev_rcv_lists->rx_eff[effhash(can_id)], list) {
0627             if (rcv->can_id == can_id) {
0628                 deliver(skb, rcv);
0629                 matches++;
0630             }
0631         }
0632     } else {
0633         can_id &= CAN_SFF_MASK;
0634         hlist_for_each_entry_rcu(rcv, &dev_rcv_lists->rx_sff[can_id], list) {
0635             deliver(skb, rcv);
0636             matches++;
0637         }
0638     }
0639
0640     return matches;
0641 }
0642
0643 static void can_receive(struct sk_buff *skb, struct net_device *dev)
0644 {
0645     struct can_dev_rcv_lists *dev_rcv_lists;
0646     struct net *net = dev_net(dev);
0647     struct can_pkg_stats *pkg_stats = net->can.pkg_stats;
0648     int matches;
0649
0650     /* update statistics */
0651     pkg_stats->rx_frames++;
0652     pkg_stats->rx_frames_delta++;
0653
0654     /* create non-zero unique skb identifier together with *skb */
0655     while (!(can_skb_prv(skb)->skbcnt))
0656         can_skb_prv(skb)->skbcnt = atomic_inc_return(&skbcounter);
0657
0658     rcu_read_lock();
0659
0660     /* deliver the packet to sockets listening on all devices */
0661     matches = can_rcv_filter(net->can.rx_alldev_list, skb);
0662
0663     /* find receive list for this device */
0664     dev_rcv_lists = can_dev_rcv_lists_find(net, dev);
0665     matches += can_rcv_filter(dev_rcv_lists, skb);
0666
0667     rcu_read_unlock();
0668
0669     /* consume the skbuff allocated by the netdevice driver */
0670     consume_skb(skb);
0671
0672     if (matches > 0) {
0673         pkg_stats->matches++;
0674         pkg_stats->matches_delta++;
0675     }
0676 }
0677
0678 static int can_rcv(struct sk_buff *skb, struct net_device *dev,
0679            struct packet_type *pt, struct net_device *orig_dev)
0680 {
0681     struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
0682
0683     if (unlikely(dev->type != ARPHRD_CAN || skb->len != CAN_MTU)) {
0684         pr_warn_once("PF_CAN: dropped non conform CAN skbuff: dev type %d, len %d\n",
0685                  dev->type, skb->len);
0686         goto free_skb;
0687     }
0688
0689     /* This check is made separately since cfd->len would be uninitialized if skb->len = 0. */
0690     if (unlikely(cfd->len > CAN_MAX_DLEN)) {
0691         pr_warn_once("PF_CAN: dropped non conform CAN skbuff: dev type %d, len %d, datalen %d\n",
0692                  dev->type, skb->len, cfd->len);
0693         goto free_skb;
0694     }
0695
0696     can_receive(skb, dev);
0697     return NET_RX_SUCCESS;
0698
0699 free_skb:
0700     kfree_skb(skb);
0701     return NET_RX_DROP;
0702 }
0703
0704 static int canfd_rcv(struct sk_buff *skb, struct net_device *dev,
0705              struct packet_type *pt, struct net_device *orig_dev)
0706 {
0707     struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
0708
0709     if (unlikely(dev->type != ARPHRD_CAN || skb->len != CANFD_MTU)) {
0710         pr_warn_once("PF_CAN: dropped non conform CAN FD skbuff: dev type %d, len %d\n",
0711                  dev->type, skb->len);
0712         goto free_skb;
0713     }
0714
0715     /* This check is made separately since cfd->len would be uninitialized if skb->len = 0. */
0716     if (unlikely(cfd->len > CANFD_MAX_DLEN)) {
0717         pr_warn_once("PF_CAN: dropped non conform CAN FD skbuff: dev type %d, len %d, datalen %d\n",
0718                  dev->type, skb->len, cfd->len);
0719         goto free_skb;
0720     }
0721
0722     can_receive(skb, dev);
0723     return NET_RX_SUCCESS;
0724
0725 free_skb:
0726     kfree_skb(skb);
0727     return NET_RX_DROP;
0728 }
0729
0730 /* af_can protocol functions */
0731
0732 /**
0733  * can_proto_register - register CAN transport protocol
0734  * @cp: pointer to CAN protocol structure
0735  *
0736  * Return:
0737  *  0 on success
0738  *  -EINVAL invalid (out of range) protocol number
0739  *  -EBUSY  protocol already in use
0740  *  -ENOBUF if proto_register() fails
0741  */
0742 int can_proto_register(const struct can_proto *cp)
0743 {
0744     int proto = cp->protocol;
0745     int err = 0;
0746
0747     if (proto < 0 || proto >= CAN_NPROTO) {
0748         pr_err("can: protocol number %d out of range\n", proto);
0749         return -EINVAL;
0750     }
0751
0752     err = proto_register(cp->prot, 0);
0753     if (err < 0)
0754         return err;
0755
0756     mutex_lock(&proto_tab_lock);
0757
0758     if (rcu_access_pointer(proto_tab[proto])) {
0759         pr_err("can: protocol %d already registered\n", proto);
0760         err = -EBUSY;
0761     } else {
0762         RCU_INIT_POINTER(proto_tab[proto], cp);
0763     }
0764
0765     mutex_unlock(&proto_tab_lock);
0766
0767     if (err < 0)
0768         proto_unregister(cp->prot);
0769
0770     return err;
0771 }
0772 EXPORT_SYMBOL(can_proto_register);
0773
0774 /**
0775  * can_proto_unregister - unregister CAN transport protocol
0776  * @cp: pointer to CAN protocol structure
0777  */
0778 void can_proto_unregister(const struct can_proto *cp)
0779 {
0780     int proto = cp->protocol;
0781
0782     mutex_lock(&proto_tab_lock);
0783     BUG_ON(rcu_access_pointer(proto_tab[proto]) != cp);
0784     RCU_INIT_POINTER(proto_tab[proto], NULL);
0785     mutex_unlock(&proto_tab_lock);
0786
0787     synchronize_rcu();
0788
0789     proto_unregister(cp->prot);
0790 }
0791 EXPORT_SYMBOL(can_proto_unregister);
0792
0793 static int can_pernet_init(struct net *net)
0794 {
0795     spin_lock_init(&net->can.rcvlists_lock);
0796     net->can.rx_alldev_list =
0797         kzalloc(sizeof(*net->can.rx_alldev_list), GFP_KERNEL);
0798     if (!net->can.rx_alldev_list)
0799         goto out;
0800     net->can.pkg_stats = kzalloc(sizeof(*net->can.pkg_stats), GFP_KERNEL);
0801     if (!net->can.pkg_stats)
0802         goto out_free_rx_alldev_list;
0803     net->can.rcv_lists_stats = kzalloc(sizeof(*net->can.rcv_lists_stats), GFP_KERNEL);
0804     if (!net->can.rcv_lists_stats)
0805         goto out_free_pkg_stats;
0806
0807     if (IS_ENABLED(CONFIG_PROC_FS)) {
0808         /* the statistics are updated every second (timer triggered) */
0809         if (stats_timer) {
0810             timer_setup(&net->can.stattimer, can_stat_update,
0811                     0);
0812             mod_timer(&net->can.stattimer,
0813                   round_jiffies(jiffies + HZ));
0814         }
0815         net->can.pkg_stats->jiffies_init = jiffies;
0816         can_init_proc(net);
0817     }
0818
0819     return 0;
0820
0821  out_free_pkg_stats:
0822     kfree(net->can.pkg_stats);
0823  out_free_rx_alldev_list:
0824     kfree(net->can.rx_alldev_list);
0825  out:
0826     return -ENOMEM;
0827 }
0828
0829 static void can_pernet_exit(struct net *net)
0830 {
0831     if (IS_ENABLED(CONFIG_PROC_FS)) {
0832         can_remove_proc(net);
0833         if (stats_timer)
0834             del_timer_sync(&net->can.stattimer);
0835     }
0836
0837     kfree(net->can.rx_alldev_list);
0838     kfree(net->can.pkg_stats);
0839     kfree(net->can.rcv_lists_stats);
0840 }
0841
0842 /* af_can module init/exit functions */
0843
0844 static struct packet_type can_packet __read_mostly = {
0845     .type = cpu_to_be16(ETH_P_CAN),
0846     .func = can_rcv,
0847 };
0848
0849 static struct packet_type canfd_packet __read_mostly = {
0850     .type = cpu_to_be16(ETH_P_CANFD),
0851     .func = canfd_rcv,
0852 };
0853
0854 static const struct net_proto_family can_family_ops = {
0855     .family = PF_CAN,
0856     .create = can_create,
0857     .owner  = THIS_MODULE,
0858 };
0859
0860 static struct pernet_operations can_pernet_ops __read_mostly = {
0861     .init = can_pernet_init,
0862     .exit = can_pernet_exit,
0863 };
0864
0865 static __init int can_init(void)
0866 {
0867     int err;
0868
0869     /* check for correct padding to be able to use the structs similarly */
0870     BUILD_BUG_ON(offsetof(struct can_frame, len) !=
0871              offsetof(struct canfd_frame, len) ||
0872              offsetof(struct can_frame, data) !=
0873              offsetof(struct canfd_frame, data));
0874
0875     pr_info("can: controller area network core\n");
0876
0877     rcv_cache = kmem_cache_create("can_receiver", sizeof(struct receiver),
0878                       0, 0, NULL);
0879     if (!rcv_cache)
0880         return -ENOMEM;
0881
0882     err = register_pernet_subsys(&can_pernet_ops);
0883     if (err)
0884         goto out_pernet;
0885
0886     /* protocol register */
0887     err = sock_register(&can_family_ops);
0888     if (err)
0889         goto out_sock;
0890
0891     dev_add_pack(&can_packet);
0892     dev_add_pack(&canfd_packet);
0893
0894     return 0;
0895
0896 out_sock:
0897     unregister_pernet_subsys(&can_pernet_ops);
0898 out_pernet:
0899     kmem_cache_destroy(rcv_cache);
0900
0901     return err;
0902 }
0903
0904 static __exit void can_exit(void)
0905 {
0906     /* protocol unregister */
0907     dev_remove_pack(&canfd_packet);
0908     dev_remove_pack(&can_packet);
0909     sock_unregister(PF_CAN);
0910
0911     unregister_pernet_subsys(&can_pernet_ops);
0912
0913     rcu_barrier(); /* Wait for completion of call_rcu()'s */
0914
0915     kmem_cache_destroy(rcv_cache);
0916 }
0917
0918 module_init(can_init);
0919 module_exit(can_exit);