[linux-2.6-block.git] / net / can / raw.c

/*
 * raw.c - Raw sockets for protocol family CAN
 *
 * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of Volkswagen nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * Alternatively, provided that this notice is retained in full, this
 * software may be distributed under the terms of the GNU General
 * Public License ("GPL") version 2, in which case the provisions of the
 * GPL apply INSTEAD OF those given above.
 *
 * The provided data structures and external interfaces from this code
 * are not restricted to be used by modules with a GPL compatible license.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 * DAMAGE.
 *
 * Send feedback to <socketcan-users@lists.berlios.de>
 *
 */

#include <linux/module.h>
#include <linux/init.h>
#include <linux/uio.h>
#include <linux/net.h>
#include <linux/netdevice.h>
#include <linux/socket.h>
#include <linux/if_arp.h>
#include <linux/skbuff.h>
#include <linux/can.h>
#include <linux/can/core.h>
#include <linux/can/raw.h>
#include <net/sock.h>
#include <net/net_namespace.h>

#define CAN_RAW_VERSION CAN_VERSION
static __initdata const char banner[] =
	KERN_INFO "can: raw protocol (rev " CAN_RAW_VERSION ")\n";

MODULE_DESCRIPTION("PF_CAN raw protocol");
MODULE_LICENSE("Dual BSD/GPL");
MODULE_AUTHOR("Urs Thuermann <urs.thuermann@volkswagen.de>");

#define MASK_ALL 0

/*
 * A raw socket has a list of can_filters attached to it, each receiving
 * the CAN frames matching that filter.  If the filter list is empty,
 * no CAN frames will be received by the socket.  The default after
 * opening the socket, is to have one filter which receives all frames.
 * The filter list is allocated dynamically with the exception of the
 * list containing only one item.  This common case is optimized by
 * storing the single filter in dfilter, to avoid using dynamic memory.
 */

struct raw_sock {
	struct sock sk;
	int bound;
	int ifindex;
	struct notifier_block notifier;
	int loopback;
	int recv_own_msgs;
	int count;                 /* number of active filters */
	struct can_filter dfilter; /* default/single filter */
	struct can_filter *filter; /* pointer to filter(s) */
	can_err_mask_t err_mask;
};

static inline struct raw_sock *raw_sk(const struct sock *sk)
{
	return (struct raw_sock *)sk;
}

static void raw_rcv(struct sk_buff *skb, void *data)
{
	struct sock *sk = (struct sock *)data;
	struct raw_sock *ro = raw_sk(sk);
	struct sockaddr_can *addr;

	if (!ro->recv_own_msgs) {
		/* check the received tx sock reference */
		if (skb->sk == sk) {
			kfree_skb(skb);
			return;
		}
	}

	/*
	 *  Put the datagram to the queue so that raw_recvmsg() can
	 *  get it from there.  We need to pass the interface index to
	 *  raw_recvmsg().  We pass a whole struct sockaddr_can in skb->cb
	 *  containing the interface index.
	 */

	BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct sockaddr_can));
	addr = (struct sockaddr_can *)skb->cb;
	memset(addr, 0, sizeof(*addr));
	addr->can_family  = AF_CAN;
	addr->can_ifindex = skb->dev->ifindex;

	if (sock_queue_rcv_skb(sk, skb) < 0)
		kfree_skb(skb);
}

static int raw_enable_filters(struct net_device *dev, struct sock *sk,
			      struct can_filter *filter, int count)
{
	int err = 0;
	int i;

	for (i = 0; i < count; i++) {
		err = can_rx_register(dev, filter[i].can_id,
				      filter[i].can_mask,
				      raw_rcv, sk, "raw");
		if (err) {
			/* clean up successfully registered filters */
			while (--i >= 0)
				can_rx_unregister(dev, filter[i].can_id,
						  filter[i].can_mask,
						  raw_rcv, sk);
			break;
		}
	}

	return err;
}

static int raw_enable_errfilter(struct net_device *dev, struct sock *sk,
				can_err_mask_t err_mask)
{
	int err = 0;

	if (err_mask)
		err = can_rx_register(dev, 0, err_mask | CAN_ERR_FLAG,
				      raw_rcv, sk, "raw");

	return err;
}

static void raw_disable_filters(struct net_device *dev, struct sock *sk,
			      struct can_filter *filter, int count)
{
	int i;

	for (i = 0; i < count; i++)
		can_rx_unregister(dev, filter[i].can_id, filter[i].can_mask,
				  raw_rcv, sk);
}

static inline void raw_disable_errfilter(struct net_device *dev,
					 struct sock *sk,
					 can_err_mask_t err_mask)

{
	if (err_mask)
		can_rx_unregister(dev, 0, err_mask | CAN_ERR_FLAG,
				  raw_rcv, sk);
}

static inline void raw_disable_allfilters(struct net_device *dev,
					  struct sock *sk)
{
	struct raw_sock *ro = raw_sk(sk);

	raw_disable_filters(dev, sk, ro->filter, ro->count);
	raw_disable_errfilter(dev, sk, ro->err_mask);
}

static int raw_enable_allfilters(struct net_device *dev, struct sock *sk)
{
	struct raw_sock *ro = raw_sk(sk);
	int err;

	err = raw_enable_filters(dev, sk, ro->filter, ro->count);
	if (!err) {
		err = raw_enable_errfilter(dev, sk, ro->err_mask);
		if (err)
			raw_disable_filters(dev, sk, ro->filter, ro->count);
	}

	return err;
}

static int raw_notifier(struct notifier_block *nb,
			unsigned long msg, void *data)
{
	struct net_device *dev = (struct net_device *)data;
	struct raw_sock *ro = container_of(nb, struct raw_sock, notifier);
	struct sock *sk = &ro->sk;

	if (!net_eq(dev_net(dev), &init_net))
		return NOTIFY_DONE;

	if (dev->type != ARPHRD_CAN)
		return NOTIFY_DONE;

	if (ro->ifindex != dev->ifindex)
		return NOTIFY_DONE;

	switch (msg) {

	case NETDEV_UNREGISTER:
		lock_sock(sk);
		/* remove current filters & unregister */
		if (ro->bound)
			raw_disable_allfilters(dev, sk);

		if (ro->count > 1)
			kfree(ro->filter);

		ro->ifindex = 0;
		ro->bound   = 0;
		ro->count   = 0;
		release_sock(sk);

		sk->sk_err = ENODEV;
		if (!sock_flag(sk, SOCK_DEAD))
			sk->sk_error_report(sk);
		break;

	case NETDEV_DOWN:
		sk->sk_err = ENETDOWN;
		if (!sock_flag(sk, SOCK_DEAD))
			sk->sk_error_report(sk);
		break;
	}

	return NOTIFY_DONE;
}

static int raw_init(struct sock *sk)
{
	struct raw_sock *ro = raw_sk(sk);

	ro->bound            = 0;
	ro->ifindex          = 0;

	/* set default filter to single entry dfilter */
	ro->dfilter.can_id   = 0;
	ro->dfilter.can_mask = MASK_ALL;
	ro->filter           = &ro->dfilter;
	ro->count            = 1;

	/* set default loopback behaviour */
	ro->loopback         = 1;
	ro->recv_own_msgs    = 0;

	/* set notifier */
	ro->notifier.notifier_call = raw_notifier;

	register_netdevice_notifier(&ro->notifier);

	return 0;
}

static int raw_release(struct socket *sock)
{
	struct sock *sk = sock->sk;
	struct raw_sock *ro = raw_sk(sk);

	unregister_netdevice_notifier(&ro->notifier);

	lock_sock(sk);

	/* remove current filters & unregister */
	if (ro->bound) {
		if (ro->ifindex) {
			struct net_device *dev;

			dev = dev_get_by_index(&init_net, ro->ifindex);
			if (dev) {
				raw_disable_allfilters(dev, sk);
				dev_put(dev);
			}
		} else
			raw_disable_allfilters(NULL, sk);
	}

	if (ro->count > 1)
		kfree(ro->filter);

	ro->ifindex = 0;
	ro->bound   = 0;
	ro->count   = 0;

	release_sock(sk);
	sock_put(sk);

	return 0;
}

static int raw_bind(struct socket *sock, struct sockaddr *uaddr, int len)
{
	struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
	struct sock *sk = sock->sk;
	struct raw_sock *ro = raw_sk(sk);
	int ifindex;
	int err = 0;
	int notify_enetdown = 0;

	if (len < sizeof(*addr))
		return -EINVAL;

	lock_sock(sk);

	if (ro->bound && addr->can_ifindex == ro->ifindex)
		goto out;

	if (addr->can_ifindex) {
		struct net_device *dev;

		dev = dev_get_by_index(&init_net, addr->can_ifindex);
		if (!dev) {
			err = -ENODEV;
			goto out;
		}
		if (dev->type != ARPHRD_CAN) {
			dev_put(dev);
			err = -ENODEV;
			goto out;
		}
		if (!(dev->flags & IFF_UP))
			notify_enetdown = 1;

		ifindex = dev->ifindex;

		/* filters set by default/setsockopt */
		err = raw_enable_allfilters(dev, sk);
		dev_put(dev);
	} else {
		ifindex = 0;

		/* filters set by default/setsockopt */
		err = raw_enable_allfilters(NULL, sk);
	}

	if (!err) {
		if (ro->bound) {
			/* unregister old filters */
			if (ro->ifindex) {
				struct net_device *dev;

				dev = dev_get_by_index(&init_net, ro->ifindex);
				if (dev) {
					raw_disable_allfilters(dev, sk);
					dev_put(dev);
				}
			} else
				raw_disable_allfilters(NULL, sk);
		}
		ro->ifindex = ifindex;
		ro->bound = 1;
	}

 out:
	release_sock(sk);

	if (notify_enetdown) {
		sk->sk_err = ENETDOWN;
		if (!sock_flag(sk, SOCK_DEAD))
			sk->sk_error_report(sk);
	}

	return err;
}

static int raw_getname(struct socket *sock, struct sockaddr *uaddr,
		       int *len, int peer)
{
	struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
	struct sock *sk = sock->sk;
	struct raw_sock *ro = raw_sk(sk);

	if (peer)
		return -EOPNOTSUPP;

	addr->can_family  = AF_CAN;
	addr->can_ifindex = ro->ifindex;

	*len = sizeof(*addr);

	return 0;
}

static int raw_setsockopt(struct socket *sock, int level, int optname,
			  char __user *optval, int optlen)
{
	struct sock *sk = sock->sk;
	struct raw_sock *ro = raw_sk(sk);
	struct can_filter *filter = NULL;  /* dyn. alloc'ed filters */
	struct can_filter sfilter;         /* single filter */
	struct net_device *dev = NULL;
	can_err_mask_t err_mask = 0;
	int count = 0;
	int err = 0;

	if (level != SOL_CAN_RAW)
		return -EINVAL;
	if (optlen < 0)
		return -EINVAL;

	switch (optname) {

	case CAN_RAW_FILTER:
		if (optlen % sizeof(struct can_filter) != 0)
			return -EINVAL;

		count = optlen / sizeof(struct can_filter);

		if (count > 1) {
			/* filter does not fit into dfilter => alloc space */
			filter = kmalloc(optlen, GFP_KERNEL);
			if (!filter)
				return -ENOMEM;

			if (copy_from_user(filter, optval, optlen)) {
				kfree(filter);
				return -EFAULT;
			}
		} else if (count == 1) {
			if (copy_from_user(&sfilter, optval, optlen))
				return -EFAULT;
		}

		lock_sock(sk);

		if (ro->bound && ro->ifindex)
			dev = dev_get_by_index(&init_net, ro->ifindex);

		if (ro->bound) {
			/* (try to) register the new filters */
			if (count == 1)
				err = raw_enable_filters(dev, sk, &sfilter, 1);
			else
				err = raw_enable_filters(dev, sk, filter,
							 count);
			if (err) {
				if (count > 1)
					kfree(filter);
				goto out_fil;
			}

			/* remove old filter registrations */
			raw_disable_filters(dev, sk, ro->filter, ro->count);
		}

		/* remove old filter space */
		if (ro->count > 1)
			kfree(ro->filter);

		/* link new filters to the socket */
		if (count == 1) {
			/* copy filter data for single filter */
			ro->dfilter = sfilter;
			filter = &ro->dfilter;
		}
		ro->filter = filter;
		ro->count  = count;

 out_fil:
		if (dev)
			dev_put(dev);

		release_sock(sk);

		break;

	case CAN_RAW_ERR_FILTER:
		if (optlen != sizeof(err_mask))
			return -EINVAL;

		if (copy_from_user(&err_mask, optval, optlen))
			return -EFAULT;

		err_mask &= CAN_ERR_MASK;

		lock_sock(sk);

		if (ro->bound && ro->ifindex)
			dev = dev_get_by_index(&init_net, ro->ifindex);

		/* remove current error mask */
		if (ro->bound) {
			/* (try to) register the new err_mask */
			err = raw_enable_errfilter(dev, sk, err_mask);

			if (err)
				goto out_err;

			/* remove old err_mask registration */
			raw_disable_errfilter(dev, sk, ro->err_mask);
		}

		/* link new err_mask to the socket */
		ro->err_mask = err_mask;

 out_err:
		if (dev)
			dev_put(dev);

		release_sock(sk);

		break;

	case CAN_RAW_LOOPBACK:
		if (optlen != sizeof(ro->loopback))
			return -EINVAL;

		if (copy_from_user(&ro->loopback, optval, optlen))
			return -EFAULT;

		break;

	case CAN_RAW_RECV_OWN_MSGS:
		if (optlen != sizeof(ro->recv_own_msgs))
			return -EINVAL;

		if (copy_from_user(&ro->recv_own_msgs, optval, optlen))
			return -EFAULT;

		break;

	default:
		return -ENOPROTOOPT;
	}
	return err;
}

static int raw_getsockopt(struct socket *sock, int level, int optname,
			  char __user *optval, int __user *optlen)
{
	struct sock *sk = sock->sk;
	struct raw_sock *ro = raw_sk(sk);
	int len;
	void *val;
	int err = 0;

	if (level != SOL_CAN_RAW)
		return -EINVAL;
	if (get_user(len, optlen))
		return -EFAULT;
	if (len < 0)
		return -EINVAL;

	switch (optname) {

	case CAN_RAW_FILTER:
		lock_sock(sk);
		if (ro->count > 0) {
			int fsize = ro->count * sizeof(struct can_filter);
			if (len > fsize)
				len = fsize;
			if (copy_to_user(optval, ro->filter, len))
				err = -EFAULT;
		} else
			len = 0;
		release_sock(sk);

		if (!err)
			err = put_user(len, optlen);
		return err;

	case CAN_RAW_ERR_FILTER:
		if (len > sizeof(can_err_mask_t))
			len = sizeof(can_err_mask_t);
		val = &ro->err_mask;
		break;

	case CAN_RAW_LOOPBACK:
		if (len > sizeof(int))
			len = sizeof(int);
		val = &ro->loopback;
		break;

	case CAN_RAW_RECV_OWN_MSGS:
		if (len > sizeof(int))
			len = sizeof(int);
		val = &ro->recv_own_msgs;
		break;

	default:
		return -ENOPROTOOPT;
	}

	if (put_user(len, optlen))
		return -EFAULT;
	if (copy_to_user(optval, val, len))
		return -EFAULT;
	return 0;
}

static int raw_sendmsg(struct kiocb *iocb, struct socket *sock,
		       struct msghdr *msg, size_t size)
{
	struct sock *sk = sock->sk;
	struct raw_sock *ro = raw_sk(sk);
	struct sk_buff *skb;
	struct net_device *dev;
	int ifindex;
	int err;

	if (msg->msg_name) {
		struct sockaddr_can *addr =
			(struct sockaddr_can *)msg->msg_name;

		if (addr->can_family != AF_CAN)
			return -EINVAL;

		ifindex = addr->can_ifindex;
	} else
		ifindex = ro->ifindex;

	if (size != sizeof(struct can_frame))
		return -EINVAL;

	dev = dev_get_by_index(&init_net, ifindex);
	if (!dev)
		return -ENXIO;

	skb = sock_alloc_send_skb(sk, size, msg->msg_flags & MSG_DONTWAIT,
				  &err);
	if (!skb) {
		dev_put(dev);
		return err;
	}

	err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size);
	if (err < 0) {
		kfree_skb(skb);
		dev_put(dev);
		return err;
	}
	skb->dev = dev;
	skb->sk  = sk;

	err = can_send(skb, ro->loopback);

	dev_put(dev);

	if (err)
		return err;

	return size;
}

static int raw_recvmsg(struct kiocb *iocb, struct socket *sock,
		       struct msghdr *msg, size_t size, int flags)
{
	struct sock *sk = sock->sk;
	struct sk_buff *skb;
	int err = 0;
	int noblock;

	noblock =  flags & MSG_DONTWAIT;
	flags   &= ~MSG_DONTWAIT;

	skb = skb_recv_datagram(sk, flags, noblock, &err);
	if (!skb)
		return err;

	if (size < skb->len)
		msg->msg_flags |= MSG_TRUNC;
	else
		size = skb->len;

	err = memcpy_toiovec(msg->msg_iov, skb->data, size);
	if (err < 0) {
		skb_free_datagram(sk, skb);
		return err;
	}

	sock_recv_timestamp(msg, sk, skb);

	if (msg->msg_name) {
		msg->msg_namelen = sizeof(struct sockaddr_can);
		memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
	}

	skb_free_datagram(sk, skb);

	return size;
}

static struct proto_ops raw_ops __read_mostly = {
	.family        = PF_CAN,
	.release       = raw_release,
	.bind          = raw_bind,
	.connect       = sock_no_connect,
	.socketpair    = sock_no_socketpair,
	.accept        = sock_no_accept,
	.getname       = raw_getname,
	.poll          = datagram_poll,
	.ioctl         = NULL,		/* use can_ioctl() from af_can.c */
	.listen        = sock_no_listen,
	.shutdown      = sock_no_shutdown,
	.setsockopt    = raw_setsockopt,
	.getsockopt    = raw_getsockopt,
	.sendmsg       = raw_sendmsg,
	.recvmsg       = raw_recvmsg,
	.mmap          = sock_no_mmap,
	.sendpage      = sock_no_sendpage,
};

static struct proto raw_proto __read_mostly = {
	.name       = "CAN_RAW",
	.owner      = THIS_MODULE,
	.obj_size   = sizeof(struct raw_sock),
	.init       = raw_init,
};

static struct can_proto raw_can_proto __read_mostly = {
	.type       = SOCK_RAW,
	.protocol   = CAN_RAW,
	.capability = -1,
	.ops        = &raw_ops,
	.prot       = &raw_proto,
};

static __init int raw_module_init(void)
{
	int err;

	printk(banner);

	err = can_proto_register(&raw_can_proto);
	if (err < 0)
		printk(KERN_ERR "can: registration of raw protocol failed\n");

	return err;
}

static __exit void raw_module_exit(void)
{
	can_proto_unregister(&raw_can_proto);
}

module_init(raw_module_init);
module_exit(raw_module_exit);
Commit	Line	Data
c18ce101 OH	1	/*
	2	* raw.c - Raw sockets for protocol family CAN
	3	*
	4	* Copyright (c) 2002-2007 Volkswagen Group Electronic Research
	5	* All rights reserved.
	6	*
	7	* Redistribution and use in source and binary forms, with or without
	8	* modification, are permitted provided that the following conditions
	9	* are met:
	10	* 1. Redistributions of source code must retain the above copyright
	11	* notice, this list of conditions and the following disclaimer.
	12	* 2. Redistributions in binary form must reproduce the above copyright
	13	* notice, this list of conditions and the following disclaimer in the
	14	* documentation and/or other materials provided with the distribution.
	15	* 3. Neither the name of Volkswagen nor the names of its contributors
	16	* may be used to endorse or promote products derived from this software
	17	* without specific prior written permission.
	18	*
	19	* Alternatively, provided that this notice is retained in full, this
	20	* software may be distributed under the terms of the GNU General
	21	* Public License ("GPL") version 2, in which case the provisions of the
	22	* GPL apply INSTEAD OF those given above.
	23	*
	24	* The provided data structures and external interfaces from this code
	25	* are not restricted to be used by modules with a GPL compatible license.
	26	*
	27	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
	28	* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
	29	* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
	30	* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
	31	* OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
	32	* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
	33	* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
	34	* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
	35	* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
	36	* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
	37	* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
	38	* DAMAGE.
	39	*
	40	* Send feedback to <socketcan-users@lists.berlios.de>
	41	*
	42	*/
	43
	44	#include <linux/module.h>
	45	#include <linux/init.h>
	46	#include <linux/uio.h>
	47	#include <linux/net.h>
	48	#include <linux/netdevice.h>
	49	#include <linux/socket.h>
	50	#include <linux/if_arp.h>
	51	#include <linux/skbuff.h>
	52	#include <linux/can.h>
	53	#include <linux/can/core.h>
	54	#include <linux/can/raw.h>
	55	#include <net/sock.h>
	56	#include <net/net_namespace.h>
	57
	58	#define CAN_RAW_VERSION CAN_VERSION
	59	static __initdata const char banner[] =
	60	KERN_INFO "can: raw protocol (rev " CAN_RAW_VERSION ")\n";
	61
	62	MODULE_DESCRIPTION("PF_CAN raw protocol");
	63	MODULE_LICENSE("Dual BSD/GPL");
	64	MODULE_AUTHOR("Urs Thuermann <urs.thuermann@volkswagen.de>");
65
66	#define MASK_ALL 0
67
68	/*
69	* A raw socket has a list of can_filters attached to it, each receiving
70	* the CAN frames matching that filter. If the filter list is empty,
71	* no CAN frames will be received by the socket. The default after
72	* opening the socket, is to have one filter which receives all frames.
73	* The filter list is allocated dynamically with the exception of the
74	* list containing only one item. This common case is optimized by
75	* storing the single filter in dfilter, to avoid using dynamic memory.
76	*/
77
78	struct raw_sock {
79	struct sock sk;
80	int bound;
81	int ifindex;
82	struct notifier_block notifier;
83	int loopback;
84	int recv_own_msgs;
85	int count; /* number of active filters */
86	struct can_filter dfilter; /* default/single filter */
87	struct can_filter filter; / pointer to filter(s) */
88	can_err_mask_t err_mask;
89	};
90
91	static inline struct raw_sock raw_sk(const struct sock sk)
92	{
93	return (struct raw_sock *)sk;
94	}
95
96	static void raw_rcv(struct sk_buff skb, void data)
97	{
98	struct sock sk = (struct sock )data;
99	struct raw_sock *ro = raw_sk(sk);
100	struct sockaddr_can *addr;
c18ce101 OH	101
	102	if (!ro->recv_own_msgs) {
	103	/* check the received tx sock reference */
	104	if (skb->sk == sk) {
	105	kfree_skb(skb);
	106	return;
	107	}
	108	}
	109
	110	/*
	111	* Put the datagram to the queue so that raw_recvmsg() can
	112	* get it from there. We need to pass the interface index to
	113	* raw_recvmsg(). We pass a whole struct sockaddr_can in skb->cb
	114	* containing the interface index.
	115	*/
	116
	117	BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct sockaddr_can));
	118	addr = (struct sockaddr_can *)skb->cb;
	119	memset(addr, 0, sizeof(*addr));
	120	addr->can_family = AF_CAN;
	121	addr->can_ifindex = skb->dev->ifindex;
	122
a219994b	123	if (sock_queue_rcv_skb(sk, skb) < 0)
c18ce101 OH	124	kfree_skb(skb);
	125	}
	126
	127	static int raw_enable_filters(struct net_device dev, struct sock sk,
a219994b	128	struct can_filter *filter, int count)
c18ce101 OH	129	{
	130	int err = 0;
	131	int i;
	132
	133	for (i = 0; i < count; i++) {
	134	err = can_rx_register(dev, filter[i].can_id,
	135	filter[i].can_mask,
	136	raw_rcv, sk, "raw");
	137	if (err) {
	138	/* clean up successfully registered filters */
	139	while (--i >= 0)
	140	can_rx_unregister(dev, filter[i].can_id,
	141	filter[i].can_mask,
	142	raw_rcv, sk);
	143	break;
	144	}
	145	}
	146
	147	return err;
	148	}
	149
	150	static int raw_enable_errfilter(struct net_device dev, struct sock sk,
	151	can_err_mask_t err_mask)
	152	{
	153	int err = 0;
	154
	155	if (err_mask)
	156	err = can_rx_register(dev, 0, err_mask \| CAN_ERR_FLAG,
	157	raw_rcv, sk, "raw");
	158
	159	return err;
	160	}
	161
	162	static void raw_disable_filters(struct net_device dev, struct sock sk,
a219994b	163	struct can_filter *filter, int count)
c18ce101 OH	164	{
	165	int i;
	166
	167	for (i = 0; i < count; i++)
	168	can_rx_unregister(dev, filter[i].can_id, filter[i].can_mask,
	169	raw_rcv, sk);
	170	}
	171
	172	static inline void raw_disable_errfilter(struct net_device *dev,
	173	struct sock *sk,
	174	can_err_mask_t err_mask)
	175
	176	{
	177	if (err_mask)
	178	can_rx_unregister(dev, 0, err_mask \| CAN_ERR_FLAG,
	179	raw_rcv, sk);
	180	}
	181
	182	static inline void raw_disable_allfilters(struct net_device *dev,
	183	struct sock *sk)
	184	{
	185	struct raw_sock *ro = raw_sk(sk);
	186
	187	raw_disable_filters(dev, sk, ro->filter, ro->count);
	188	raw_disable_errfilter(dev, sk, ro->err_mask);
	189	}
	190
	191	static int raw_enable_allfilters(struct net_device dev, struct sock sk)
	192	{
	193	struct raw_sock *ro = raw_sk(sk);
	194	int err;
	195
	196	err = raw_enable_filters(dev, sk, ro->filter, ro->count);
	197	if (!err) {
	198	err = raw_enable_errfilter(dev, sk, ro->err_mask);
	199	if (err)
	200	raw_disable_filters(dev, sk, ro->filter, ro->count);
	201	}
	202
	203	return err;
	204	}
	205
	206	static int raw_notifier(struct notifier_block *nb,
	207	unsigned long msg, void *data)
	208	{
	209	struct net_device dev = (struct net_device )data;
	210	struct raw_sock *ro = container_of(nb, struct raw_sock, notifier);
	211	struct sock *sk = &ro->sk;
	212
721499e8	213	if (!net_eq(dev_net(dev), &init_net))
c18ce101 OH	214	return NOTIFY_DONE;
	215
	216	if (dev->type != ARPHRD_CAN)
	217	return NOTIFY_DONE;
	218
	219	if (ro->ifindex != dev->ifindex)
	220	return NOTIFY_DONE;
	221
	222	switch (msg) {
	223
	224	case NETDEV_UNREGISTER:
	225	lock_sock(sk);
	226	/* remove current filters & unregister */
	227	if (ro->bound)
	228	raw_disable_allfilters(dev, sk);
	229
	230	if (ro->count > 1)
	231	kfree(ro->filter);
	232
	233	ro->ifindex = 0;
	234	ro->bound = 0;
	235	ro->count = 0;
	236	release_sock(sk);
	237
	238	sk->sk_err = ENODEV;
	239	if (!sock_flag(sk, SOCK_DEAD))
	240	sk->sk_error_report(sk);
	241	break;
	242
	243	case NETDEV_DOWN:
	244	sk->sk_err = ENETDOWN;
	245	if (!sock_flag(sk, SOCK_DEAD))
	246	sk->sk_error_report(sk);
	247	break;
	248	}
	249
	250	return NOTIFY_DONE;
	251	}
	252
	253	static int raw_init(struct sock *sk)
	254	{
	255	struct raw_sock *ro = raw_sk(sk);
	256
	257	ro->bound = 0;
	258	ro->ifindex = 0;
	259
	260	/* set default filter to single entry dfilter */
	261	ro->dfilter.can_id = 0;
	262	ro->dfilter.can_mask = MASK_ALL;
	263	ro->filter = &ro->dfilter;
	264	ro->count = 1;
	265
	266	/* set default loopback behaviour */
	267	ro->loopback = 1;
	268	ro->recv_own_msgs = 0;
	269
	270	/* set notifier */
	271	ro->notifier.notifier_call = raw_notifier;
	272
	273	register_netdevice_notifier(&ro->notifier);
	274
	275	return 0;
	276	}
	277
278	static int raw_release(struct socket *sock)
279	{
280	struct sock *sk = sock->sk;
281	struct raw_sock *ro = raw_sk(sk);
282
283	unregister_netdevice_notifier(&ro->notifier);
284
285	lock_sock(sk);
286
287	/* remove current filters & unregister */
288	if (ro->bound) {
289	if (ro->ifindex) {
290	struct net_device *dev;
291
292	dev = dev_get_by_index(&init_net, ro->ifindex);
293	if (dev) {
294	raw_disable_allfilters(dev, sk);
295	dev_put(dev);
296	}
297	} else
298	raw_disable_allfilters(NULL, sk);
299	}
300
301	if (ro->count > 1)
302	kfree(ro->filter);
303
304	ro->ifindex = 0;
305	ro->bound = 0;
306	ro->count = 0;
307
308	release_sock(sk);
309	sock_put(sk);
310
311	return 0;
312	}
313
314	static int raw_bind(struct socket sock, struct sockaddr uaddr, int len)
315	{
316	struct sockaddr_can addr = (struct sockaddr_can )uaddr;
317	struct sock *sk = sock->sk;
318	struct raw_sock *ro = raw_sk(sk);
319	int ifindex;
320	int err = 0;
321	int notify_enetdown = 0;
322
323	if (len < sizeof(*addr))
324	return -EINVAL;
325
326	lock_sock(sk);
327
328	if (ro->bound && addr->can_ifindex == ro->ifindex)
329	goto out;
330
331	if (addr->can_ifindex) {
332	struct net_device *dev;
333
334	dev = dev_get_by_index(&init_net, addr->can_ifindex);
335	if (!dev) {
336	err = -ENODEV;
337	goto out;
338	}
339	if (dev->type != ARPHRD_CAN) {
340	dev_put(dev);
341	err = -ENODEV;
342	goto out;
343	}
344	if (!(dev->flags & IFF_UP))
345	notify_enetdown = 1;
346
347	ifindex = dev->ifindex;
348
349	/* filters set by default/setsockopt */
350	err = raw_enable_allfilters(dev, sk);
351	dev_put(dev);
c18ce101 OH	352	} else {
	353	ifindex = 0;
	354
	355	/* filters set by default/setsockopt */
	356	err = raw_enable_allfilters(NULL, sk);
	357	}
	358
	359	if (!err) {
	360	if (ro->bound) {
	361	/* unregister old filters */
	362	if (ro->ifindex) {
	363	struct net_device *dev;
	364
	365	dev = dev_get_by_index(&init_net, ro->ifindex);
	366	if (dev) {
	367	raw_disable_allfilters(dev, sk);
	368	dev_put(dev);
	369	}
	370	} else
	371	raw_disable_allfilters(NULL, sk);
	372	}
	373	ro->ifindex = ifindex;
	374	ro->bound = 1;
	375	}
	376
	377	out:
	378	release_sock(sk);
	379
	380	if (notify_enetdown) {
	381	sk->sk_err = ENETDOWN;
	382	if (!sock_flag(sk, SOCK_DEAD))
	383	sk->sk_error_report(sk);
	384	}
	385
	386	return err;
	387	}
	388
	389	static int raw_getname(struct socket sock, struct sockaddr uaddr,
	390	int *len, int peer)
	391	{
	392	struct sockaddr_can addr = (struct sockaddr_can )uaddr;
	393	struct sock *sk = sock->sk;
	394	struct raw_sock *ro = raw_sk(sk);
	395
	396	if (peer)
	397	return -EOPNOTSUPP;
	398
	399	addr->can_family = AF_CAN;
	400	addr->can_ifindex = ro->ifindex;
	401
	402	len = sizeof(addr);
	403
	404	return 0;
	405	}
	406
	407	static int raw_setsockopt(struct socket *sock, int level, int optname,
	408	char __user *optval, int optlen)
	409	{
	410	struct sock *sk = sock->sk;
	411	struct raw_sock *ro = raw_sk(sk);
	412	struct can_filter filter = NULL; / dyn. alloc'ed filters */
	413	struct can_filter sfilter; /* single filter */
	414	struct net_device *dev = NULL;
	415	can_err_mask_t err_mask = 0;
416	int count = 0;
417	int err = 0;
418
419	if (level != SOL_CAN_RAW)
420	return -EINVAL;
421	if (optlen < 0)
422	return -EINVAL;
423
424	switch (optname) {
425
426	case CAN_RAW_FILTER:
427	if (optlen % sizeof(struct can_filter) != 0)
428	return -EINVAL;
429
430	count = optlen / sizeof(struct can_filter);
431
432	if (count > 1) {
433	/* filter does not fit into dfilter => alloc space */
434	filter = kmalloc(optlen, GFP_KERNEL);
435	if (!filter)
436	return -ENOMEM;
437
3f91bd42	438	if (copy_from_user(filter, optval, optlen)) {
c18ce101	439	kfree(filter);
3f91bd42	440	return -EFAULT;
c18ce101 OH	441	}
c18ce101 OH	442	} else if (count == 1) {
3f91bd42 SR	443	if (copy_from_user(&sfilter, optval, optlen))
3f91bd42 SR	444	return -EFAULT;
c18ce101 OH	445	}
	446
	447	lock_sock(sk);
	448
	449	if (ro->bound && ro->ifindex)
	450	dev = dev_get_by_index(&init_net, ro->ifindex);
	451
	452	if (ro->bound) {
	453	/* (try to) register the new filters */
	454	if (count == 1)
	455	err = raw_enable_filters(dev, sk, &sfilter, 1);
	456	else
	457	err = raw_enable_filters(dev, sk, filter,
	458	count);
	459	if (err) {
	460	if (count > 1)
	461	kfree(filter);
c18ce101 OH	462	goto out_fil;
	463	}
	464
	465	/* remove old filter registrations */
	466	raw_disable_filters(dev, sk, ro->filter, ro->count);
	467	}
	468
	469	/* remove old filter space */
	470	if (ro->count > 1)
	471	kfree(ro->filter);
	472
	473	/* link new filters to the socket */
	474	if (count == 1) {
	475	/* copy filter data for single filter */
	476	ro->dfilter = sfilter;
	477	filter = &ro->dfilter;
	478	}
	479	ro->filter = filter;
	480	ro->count = count;
	481
	482	out_fil:
	483	if (dev)
	484	dev_put(dev);
	485
	486	release_sock(sk);
	487
	488	break;
	489
	490	case CAN_RAW_ERR_FILTER:
	491	if (optlen != sizeof(err_mask))
	492	return -EINVAL;
	493
3f91bd42 SR	494	if (copy_from_user(&err_mask, optval, optlen))
3f91bd42 SR	495	return -EFAULT;
c18ce101 OH	496
	497	err_mask &= CAN_ERR_MASK;
	498
	499	lock_sock(sk);
	500
	501	if (ro->bound && ro->ifindex)
	502	dev = dev_get_by_index(&init_net, ro->ifindex);
	503
	504	/* remove current error mask */
	505	if (ro->bound) {
	506	/* (try to) register the new err_mask */
	507	err = raw_enable_errfilter(dev, sk, err_mask);
	508
	509	if (err)
	510	goto out_err;
	511
	512	/* remove old err_mask registration */
	513	raw_disable_errfilter(dev, sk, ro->err_mask);
	514	}
	515
	516	/* link new err_mask to the socket */
	517	ro->err_mask = err_mask;
	518
	519	out_err:
	520	if (dev)
	521	dev_put(dev);
	522
	523	release_sock(sk);
	524
	525	break;
	526
	527	case CAN_RAW_LOOPBACK:
	528	if (optlen != sizeof(ro->loopback))
	529	return -EINVAL;
	530
3f91bd42 SR	531	if (copy_from_user(&ro->loopback, optval, optlen))
3f91bd42 SR	532	return -EFAULT;
c18ce101 OH	533
	534	break;
	535
	536	case CAN_RAW_RECV_OWN_MSGS:
	537	if (optlen != sizeof(ro->recv_own_msgs))
	538	return -EINVAL;
	539
3f91bd42 SR	540	if (copy_from_user(&ro->recv_own_msgs, optval, optlen))
3f91bd42 SR	541	return -EFAULT;
c18ce101 OH	542
	543	break;
	544
	545	default:
	546	return -ENOPROTOOPT;
	547	}
	548	return err;
	549	}
	550
	551	static int raw_getsockopt(struct socket *sock, int level, int optname,
	552	char __user optval, int __user optlen)
	553	{
	554	struct sock *sk = sock->sk;
	555	struct raw_sock *ro = raw_sk(sk);
	556	int len;
	557	void *val;
	558	int err = 0;
	559
	560	if (level != SOL_CAN_RAW)
	561	return -EINVAL;
	562	if (get_user(len, optlen))
	563	return -EFAULT;
	564	if (len < 0)
	565	return -EINVAL;
	566
	567	switch (optname) {
	568
	569	case CAN_RAW_FILTER:
	570	lock_sock(sk);
	571	if (ro->count > 0) {
	572	int fsize = ro->count * sizeof(struct can_filter);
	573	if (len > fsize)
	574	len = fsize;
653252c2 PE	575	if (copy_to_user(optval, ro->filter, len))
653252c2 PE	576	err = -EFAULT;
c18ce101 OH	577	} else
	578	len = 0;
	579	release_sock(sk);
	580
	581	if (!err)
	582	err = put_user(len, optlen);
	583	return err;
	584
	585	case CAN_RAW_ERR_FILTER:
	586	if (len > sizeof(can_err_mask_t))
	587	len = sizeof(can_err_mask_t);
	588	val = &ro->err_mask;
	589	break;
	590
	591	case CAN_RAW_LOOPBACK:
	592	if (len > sizeof(int))
	593	len = sizeof(int);
	594	val = &ro->loopback;
	595	break;
	596
	597	case CAN_RAW_RECV_OWN_MSGS:
	598	if (len > sizeof(int))
	599	len = sizeof(int);
	600	val = &ro->recv_own_msgs;
	601	break;
	602
	603	default:
	604	return -ENOPROTOOPT;
	605	}
	606
	607	if (put_user(len, optlen))
	608	return -EFAULT;
	609	if (copy_to_user(optval, val, len))
	610	return -EFAULT;
	611	return 0;
	612	}
	613
	614	static int raw_sendmsg(struct kiocb iocb, struct socket sock,
	615	struct msghdr *msg, size_t size)
	616	{
	617	struct sock *sk = sock->sk;
	618	struct raw_sock *ro = raw_sk(sk);
	619	struct sk_buff *skb;
	620	struct net_device *dev;
	621	int ifindex;
	622	int err;
	623
	624	if (msg->msg_name) {
	625	struct sockaddr_can *addr =
	626	(struct sockaddr_can *)msg->msg_name;
	627
	628	if (addr->can_family != AF_CAN)
	629	return -EINVAL;
	630
	631	ifindex = addr->can_ifindex;
	632	} else
	633	ifindex = ro->ifindex;
	634
7f2d38eb OH	635	if (size != sizeof(struct can_frame))
	636	return -EINVAL;
	637
c18ce101 OH	638	dev = dev_get_by_index(&init_net, ifindex);
	639	if (!dev)
	640	return -ENXIO;
	641
	642	skb = sock_alloc_send_skb(sk, size, msg->msg_flags & MSG_DONTWAIT,
	643	&err);
	644	if (!skb) {
	645	dev_put(dev);
	646	return err;
	647	}
	648
	649	err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size);
	650	if (err < 0) {
	651	kfree_skb(skb);
	652	dev_put(dev);
	653	return err;
	654	}
	655	skb->dev = dev;
	656	skb->sk = sk;
	657
	658	err = can_send(skb, ro->loopback);
	659
	660	dev_put(dev);
	661
	662	if (err)
	663	return err;
	664
	665	return size;
	666	}
	667
	668	static int raw_recvmsg(struct kiocb iocb, struct socket sock,
	669	struct msghdr *msg, size_t size, int flags)
	670	{
	671	struct sock *sk = sock->sk;
	672	struct sk_buff *skb;
a219994b	673	int err = 0;
c18ce101 OH	674	int noblock;
	675
	676	noblock = flags & MSG_DONTWAIT;
	677	flags &= ~MSG_DONTWAIT;
	678
a219994b	679	skb = skb_recv_datagram(sk, flags, noblock, &err);
c18ce101	680	if (!skb)
a219994b	681	return err;
c18ce101 OH	682
	683	if (size < skb->len)
	684	msg->msg_flags \|= MSG_TRUNC;
	685	else
	686	size = skb->len;
	687
a219994b UT	688	err = memcpy_toiovec(msg->msg_iov, skb->data, size);
a219994b UT	689	if (err < 0) {
c18ce101	690	skb_free_datagram(sk, skb);
a219994b	691	return err;
c18ce101 OH	692	}
	693
	694	sock_recv_timestamp(msg, sk, skb);
	695
	696	if (msg->msg_name) {
	697	msg->msg_namelen = sizeof(struct sockaddr_can);
	698	memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
	699	}
	700
	701	skb_free_datagram(sk, skb);
	702
	703	return size;
	704	}
	705
	706	static struct proto_ops raw_ops __read_mostly = {
	707	.family = PF_CAN,
	708	.release = raw_release,
	709	.bind = raw_bind,
	710	.connect = sock_no_connect,
	711	.socketpair = sock_no_socketpair,
	712	.accept = sock_no_accept,
	713	.getname = raw_getname,
	714	.poll = datagram_poll,
	715	.ioctl = NULL, /* use can_ioctl() from af_can.c */
	716	.listen = sock_no_listen,
	717	.shutdown = sock_no_shutdown,
	718	.setsockopt = raw_setsockopt,
	719	.getsockopt = raw_getsockopt,
	720	.sendmsg = raw_sendmsg,
	721	.recvmsg = raw_recvmsg,
	722	.mmap = sock_no_mmap,
	723	.sendpage = sock_no_sendpage,
	724	};
	725
	726	static struct proto raw_proto __read_mostly = {
	727	.name = "CAN_RAW",
	728	.owner = THIS_MODULE,
	729	.obj_size = sizeof(struct raw_sock),
	730	.init = raw_init,
	731	};
	732
	733	static struct can_proto raw_can_proto __read_mostly = {
	734	.type = SOCK_RAW,
	735	.protocol = CAN_RAW,
	736	.capability = -1,
	737	.ops = &raw_ops,
	738	.prot = &raw_proto,
	739	};
	740
	741	static __init int raw_module_init(void)
	742	{
	743	int err;
	744
	745	printk(banner);
	746
	747	err = can_proto_register(&raw_can_proto);
	748	if (err < 0)
	749	printk(KERN_ERR "can: registration of raw protocol failed\n");
	750
	751	return err;
	752	}
	753
	754	static __exit void raw_module_exit(void)
	755	{
756	can_proto_unregister(&raw_can_proto);
757	}
758
759	module_init(raw_module_init);
760	module_exit(raw_module_exit);