[net-next-2.6.git] / net / ieee802154 / raw.c

/*
 * Raw IEEE 802.15.4 sockets
 *
 * Copyright 2007, 2008 Siemens AG
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2
 * as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Written by:
 * Sergey Lapin <slapin@ossfans.org>
 * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
 */

#include <linux/net.h>
#include <linux/module.h>
#include <linux/if_arp.h>
#include <linux/list.h>
#include <linux/slab.h>
#include <net/sock.h>
#include <net/af_ieee802154.h>

#include "af802154.h"

static HLIST_HEAD(raw_head);
static DEFINE_RWLOCK(raw_lock);

static void raw_hash(struct sock *sk)
{
	write_lock_bh(&raw_lock);
	sk_add_node(sk, &raw_head);
	sock_prot_inuse_add(sock_net(sk), sk->sk_prot, 1);
	write_unlock_bh(&raw_lock);
}

static void raw_unhash(struct sock *sk)
{
	write_lock_bh(&raw_lock);
	if (sk_del_node_init(sk))
		sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
	write_unlock_bh(&raw_lock);
}

static void raw_close(struct sock *sk, long timeout)
{
	sk_common_release(sk);
}

static int raw_bind(struct sock *sk, struct sockaddr *uaddr, int len)
{
	struct sockaddr_ieee802154 *addr = (struct sockaddr_ieee802154 *)uaddr;
	int err = 0;
	struct net_device *dev = NULL;

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

	if (addr->family != AF_IEEE802154)
		return -EINVAL;

	lock_sock(sk);

	dev = ieee802154_get_dev(sock_net(sk), &addr->addr);
	if (!dev) {
		err = -ENODEV;
		goto out;
	}

	if (dev->type != ARPHRD_IEEE802154) {
		err = -ENODEV;
		goto out_put;
	}

	sk->sk_bound_dev_if = dev->ifindex;
	sk_dst_reset(sk);

out_put:
	dev_put(dev);
out:
	release_sock(sk);

	return err;
}

static int raw_connect(struct sock *sk, struct sockaddr *uaddr,
			int addr_len)
{
	return -ENOTSUPP;
}

static int raw_disconnect(struct sock *sk, int flags)
{
	return 0;
}

static int raw_sendmsg(struct kiocb *iocb, struct sock *sk, struct msghdr *msg,
		       size_t size)
{
	struct net_device *dev;
	unsigned mtu;
	struct sk_buff *skb;
	int err;

	if (msg->msg_flags & MSG_OOB) {
		pr_debug("msg->msg_flags = 0x%x\n", msg->msg_flags);
		return -EOPNOTSUPP;
	}

	lock_sock(sk);
	if (!sk->sk_bound_dev_if)
		dev = dev_getfirstbyhwtype(sock_net(sk), ARPHRD_IEEE802154);
	else
		dev = dev_get_by_index(sock_net(sk), sk->sk_bound_dev_if);
	release_sock(sk);

	if (!dev) {
		pr_debug("no dev\n");
		err = -ENXIO;
		goto out;
	}

	mtu = dev->mtu;
	pr_debug("name = %s, mtu = %u\n", dev->name, mtu);

	if (size > mtu) {
		pr_debug("size = %Zu, mtu = %u\n", size, mtu);
		err = -EINVAL;
		goto out_dev;
	}

	skb = sock_alloc_send_skb(sk, LL_ALLOCATED_SPACE(dev) + size,
			msg->msg_flags & MSG_DONTWAIT, &err);
	if (!skb)
		goto out_dev;

	skb_reserve(skb, LL_RESERVED_SPACE(dev));

	skb_reset_mac_header(skb);
	skb_reset_network_header(skb);

	err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size);
	if (err < 0)
		goto out_skb;

	skb->dev = dev;
	skb->sk  = sk;
	skb->protocol = htons(ETH_P_IEEE802154);

	dev_put(dev);

	err = dev_queue_xmit(skb);
	if (err > 0)
		err = net_xmit_errno(err);

	return err ?: size;

out_skb:
	kfree_skb(skb);
out_dev:
	dev_put(dev);
out:
	return err;
}

static int raw_recvmsg(struct kiocb *iocb, struct sock *sk, struct msghdr *msg,
		       size_t len, int noblock, int flags, int *addr_len)
{
	size_t copied = 0;
	int err = -EOPNOTSUPP;
	struct sk_buff *skb;

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

	copied = skb->len;
	if (len < copied) {
		msg->msg_flags |= MSG_TRUNC;
		copied = len;
	}

	err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
	if (err)
		goto done;

	sock_recv_ts_and_drops(msg, sk, skb);

	if (flags & MSG_TRUNC)
		copied = skb->len;
done:
	skb_free_datagram(sk, skb);
out:
	if (err)
		return err;
	return copied;
}

static int raw_rcv_skb(struct sock *sk, struct sk_buff *skb)
{
	if (sock_queue_rcv_skb(sk, skb) < 0) {
		kfree_skb(skb);
		return NET_RX_DROP;
	}

	return NET_RX_SUCCESS;
}


void ieee802154_raw_deliver(struct net_device *dev, struct sk_buff *skb)
{
	struct sock *sk;
	struct hlist_node *node;

	read_lock(&raw_lock);
	sk_for_each(sk, node, &raw_head) {
		bh_lock_sock(sk);
		if (!sk->sk_bound_dev_if ||
		    sk->sk_bound_dev_if == dev->ifindex) {

			struct sk_buff *clone;

			clone = skb_clone(skb, GFP_ATOMIC);
			if (clone)
				raw_rcv_skb(sk, clone);
		}
		bh_unlock_sock(sk);
	}
	read_unlock(&raw_lock);
}

static int raw_getsockopt(struct sock *sk, int level, int optname,
		    char __user *optval, int __user *optlen)
{
	return -EOPNOTSUPP;
}

static int raw_setsockopt(struct sock *sk, int level, int optname,
		    char __user *optval, unsigned int optlen)
{
	return -EOPNOTSUPP;
}

struct proto ieee802154_raw_prot = {
	.name		= "IEEE-802.15.4-RAW",
	.owner		= THIS_MODULE,
	.obj_size	= sizeof(struct sock),
	.close		= raw_close,
	.bind		= raw_bind,
	.sendmsg	= raw_sendmsg,
	.recvmsg	= raw_recvmsg,
	.hash		= raw_hash,
	.unhash		= raw_unhash,
	.connect	= raw_connect,
	.disconnect	= raw_disconnect,
	.getsockopt	= raw_getsockopt,
	.setsockopt	= raw_setsockopt,
};
Commit	Line	Data
9ec76716 SL	1	/*
	2	* Raw IEEE 802.15.4 sockets
	3	*
	4	* Copyright 2007, 2008 Siemens AG
	5	*
	6	* This program is free software; you can redistribute it and/or modify
	7	* it under the terms of the GNU General Public License version 2
	8	* as published by the Free Software Foundation.
	9	*
	10	* This program is distributed in the hope that it will be useful,
	11	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	12	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	13	* GNU General Public License for more details.
	14	*
	15	* You should have received a copy of the GNU General Public License along
	16	* with this program; if not, write to the Free Software Foundation, Inc.,
	17	* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
	18	*
	19	* Written by:
	20	* Sergey Lapin <slapin@ossfans.org>
	21	* Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
	22	*/
	23
	24	#include <linux/net.h>
	25	#include <linux/module.h>
	26	#include <linux/if_arp.h>
	27	#include <linux/list.h>
5a0e3ad6	28	#include <linux/slab.h>
9ec76716	29	#include <net/sock.h>
f0166e5e	30	#include <net/af_ieee802154.h>
9ec76716 SL	31
	32	#include "af802154.h"
	33
	34	static HLIST_HEAD(raw_head);
	35	static DEFINE_RWLOCK(raw_lock);
	36
	37	static void raw_hash(struct sock *sk)
	38	{
	39	write_lock_bh(&raw_lock);
	40	sk_add_node(sk, &raw_head);
	41	sock_prot_inuse_add(sock_net(sk), sk->sk_prot, 1);
	42	write_unlock_bh(&raw_lock);
	43	}
	44
	45	static void raw_unhash(struct sock *sk)
	46	{
	47	write_lock_bh(&raw_lock);
	48	if (sk_del_node_init(sk))
	49	sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
	50	write_unlock_bh(&raw_lock);
	51	}
	52
	53	static void raw_close(struct sock *sk, long timeout)
	54	{
	55	sk_common_release(sk);
	56	}
	57
	58	static int raw_bind(struct sock sk, struct sockaddr uaddr, int len)
	59	{
	60	struct sockaddr_ieee802154 addr = (struct sockaddr_ieee802154 )uaddr;
	61	int err = 0;
	62	struct net_device *dev = NULL;
	63
	64	if (len < sizeof(*addr))
	65	return -EINVAL;
	66
	67	if (addr->family != AF_IEEE802154)
	68	return -EINVAL;
	69
	70	lock_sock(sk);
	71
	72	dev = ieee802154_get_dev(sock_net(sk), &addr->addr);
	73	if (!dev) {
	74	err = -ENODEV;
	75	goto out;
	76	}
	77
929122cd	78	if (dev->type != ARPHRD_IEEE802154) {
9ec76716 SL	79	err = -ENODEV;
	80	goto out_put;
	81	}
	82
	83	sk->sk_bound_dev_if = dev->ifindex;
	84	sk_dst_reset(sk);
	85
	86	out_put:
	87	dev_put(dev);
	88	out:
	89	release_sock(sk);
	90
	91	return err;
	92	}
	93
	94	static int raw_connect(struct sock sk, struct sockaddr uaddr,
	95	int addr_len)
	96	{
	97	return -ENOTSUPP;
	98	}
	99
	100	static int raw_disconnect(struct sock *sk, int flags)
	101	{
	102	return 0;
	103	}
	104
	105	static int raw_sendmsg(struct kiocb iocb, struct sock sk, struct msghdr *msg,
	106	size_t size)
	107	{
	108	struct net_device *dev;
	109	unsigned mtu;
	110	struct sk_buff *skb;
	111	int err;
	112
	113	if (msg->msg_flags & MSG_OOB) {
	114	pr_debug("msg->msg_flags = 0x%x\n", msg->msg_flags);
	115	return -EOPNOTSUPP;
	116	}
	117
	118	lock_sock(sk);
	119	if (!sk->sk_bound_dev_if)
	120	dev = dev_getfirstbyhwtype(sock_net(sk), ARPHRD_IEEE802154);
	121	else
	122	dev = dev_get_by_index(sock_net(sk), sk->sk_bound_dev_if);
	123	release_sock(sk);
	124
	125	if (!dev) {
	126	pr_debug("no dev\n");
	127	err = -ENXIO;
	128	goto out;
	129	}
	130
	131	mtu = dev->mtu;
	132	pr_debug("name = %s, mtu = %u\n", dev->name, mtu);
	133
	134	if (size > mtu) {
e5241c44	135	pr_debug("size = %Zu, mtu = %u\n", size, mtu);
9ec76716 SL	136	err = -EINVAL;
	137	goto out_dev;
	138	}
	139
	140	skb = sock_alloc_send_skb(sk, LL_ALLOCATED_SPACE(dev) + size,
	141	msg->msg_flags & MSG_DONTWAIT, &err);
	142	if (!skb)
	143	goto out_dev;
	144
	145	skb_reserve(skb, LL_RESERVED_SPACE(dev));
	146
	147	skb_reset_mac_header(skb);
	148	skb_reset_network_header(skb);
	149
	150	err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size);
	151	if (err < 0)
	152	goto out_skb;
	153
	154	skb->dev = dev;
	155	skb->sk = sk;
	156	skb->protocol = htons(ETH_P_IEEE802154);
	157
	158	dev_put(dev);
	159
	160	err = dev_queue_xmit(skb);
	161	if (err > 0)
	162	err = net_xmit_errno(err);
	163
	164	return err ?: size;
	165
	166	out_skb:
	167	kfree_skb(skb);
	168	out_dev:
	169	dev_put(dev);
	170	out:
	171	return err;
	172	}
	173
	174	static int raw_recvmsg(struct kiocb iocb, struct sock sk, struct msghdr *msg,
	175	size_t len, int noblock, int flags, int *addr_len)
	176	{
	177	size_t copied = 0;
	178	int err = -EOPNOTSUPP;
	179	struct sk_buff *skb;
	180
	181	skb = skb_recv_datagram(sk, flags, noblock, &err);
	182	if (!skb)
	183	goto out;
	184
	185	copied = skb->len;
	186	if (len < copied) {
	187	msg->msg_flags \|= MSG_TRUNC;
	188	copied = len;
	189	}
	190
	191	err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
	192	if (err)
	193	goto done;
	194
3b885787	195	sock_recv_ts_and_drops(msg, sk, skb);
9ec76716 SL	196
	197	if (flags & MSG_TRUNC)
	198	copied = skb->len;
	199	done:
	200	skb_free_datagram(sk, skb);
	201	out:
	202	if (err)
	203	return err;
	204	return copied;
	205	}
	206
	207	static int raw_rcv_skb(struct sock sk, struct sk_buff skb)
	208	{
	209	if (sock_queue_rcv_skb(sk, skb) < 0) {
9ec76716 SL	210	kfree_skb(skb);
	211	return NET_RX_DROP;
	212	}
	213
	214	return NET_RX_SUCCESS;
	215	}
	216
	217
	218	void ieee802154_raw_deliver(struct net_device dev, struct sk_buff skb)
	219	{
	220	struct sock *sk;
	221	struct hlist_node *node;
	222
	223	read_lock(&raw_lock);
	224	sk_for_each(sk, node, &raw_head) {
	225	bh_lock_sock(sk);
	226	if (!sk->sk_bound_dev_if \|\|
	227	sk->sk_bound_dev_if == dev->ifindex) {
	228
	229	struct sk_buff *clone;
	230
	231	clone = skb_clone(skb, GFP_ATOMIC);
	232	if (clone)
	233	raw_rcv_skb(sk, clone);
	234	}
	235	bh_unlock_sock(sk);
	236	}
	237	read_unlock(&raw_lock);
	238	}
	239
a9dfac33 DES	240	static int raw_getsockopt(struct sock *sk, int level, int optname,
	241	char __user optval, int __user optlen)
	242	{
	243	return -EOPNOTSUPP;
	244	}
	245
	246	static int raw_setsockopt(struct sock *sk, int level, int optname,
b7058842	247	char __user *optval, unsigned int optlen)
a9dfac33 DES	248	{
	249	return -EOPNOTSUPP;
	250	}
	251
9ec76716 SL	252	struct proto ieee802154_raw_prot = {
	253	.name = "IEEE-802.15.4-RAW",
	254	.owner = THIS_MODULE,
	255	.obj_size = sizeof(struct sock),
	256	.close = raw_close,
	257	.bind = raw_bind,
	258	.sendmsg = raw_sendmsg,
	259	.recvmsg = raw_recvmsg,
	260	.hash = raw_hash,
	261	.unhash = raw_unhash,
	262	.connect = raw_connect,
	263	.disconnect = raw_disconnect,
a9dfac33 DES	264	.getsockopt = raw_getsockopt,
a9dfac33 DES	265	.setsockopt = raw_setsockopt,
9ec76716 SL	266	};
9ec76716 SL	267