[deliverable/linux.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 <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_timestamp(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) {
		atomic_inc(&sk->sk_drops);
		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>
	28	#include <net/sock.h>
f0166e5e	29	#include <net/af_ieee802154.h>
9ec76716 SL	30
	31	#include "af802154.h"
	32
	33	static HLIST_HEAD(raw_head);
	34	static DEFINE_RWLOCK(raw_lock);
	35
	36	static void raw_hash(struct sock *sk)
	37	{
	38	write_lock_bh(&raw_lock);
	39	sk_add_node(sk, &raw_head);
	40	sock_prot_inuse_add(sock_net(sk), sk->sk_prot, 1);
	41	write_unlock_bh(&raw_lock);
	42	}
	43
	44	static void raw_unhash(struct sock *sk)
	45	{
	46	write_lock_bh(&raw_lock);
	47	if (sk_del_node_init(sk))
	48	sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
	49	write_unlock_bh(&raw_lock);
	50	}
	51
	52	static void raw_close(struct sock *sk, long timeout)
	53	{
	54	sk_common_release(sk);
	55	}
	56
	57	static int raw_bind(struct sock sk, struct sockaddr uaddr, int len)
	58	{
	59	struct sockaddr_ieee802154 addr = (struct sockaddr_ieee802154 )uaddr;
	60	int err = 0;
	61	struct net_device *dev = NULL;
	62
	63	if (len < sizeof(*addr))
	64	return -EINVAL;
	65
	66	if (addr->family != AF_IEEE802154)
	67	return -EINVAL;
	68
	69	lock_sock(sk);
	70
	71	dev = ieee802154_get_dev(sock_net(sk), &addr->addr);
	72	if (!dev) {
	73	err = -ENODEV;
	74	goto out;
	75	}
	76
929122cd	77	if (dev->type != ARPHRD_IEEE802154) {
9ec76716 SL	78	err = -ENODEV;
	79	goto out_put;
	80	}
	81
	82	sk->sk_bound_dev_if = dev->ifindex;
	83	sk_dst_reset(sk);
	84
	85	out_put:
	86	dev_put(dev);
	87	out:
	88	release_sock(sk);
	89
	90	return err;
	91	}
	92
	93	static int raw_connect(struct sock sk, struct sockaddr uaddr,
	94	int addr_len)
	95	{
	96	return -ENOTSUPP;
	97	}
	98
	99	static int raw_disconnect(struct sock *sk, int flags)
	100	{
	101	return 0;
	102	}
	103
	104	static int raw_sendmsg(struct kiocb iocb, struct sock sk, struct msghdr *msg,
	105	size_t size)
	106	{
	107	struct net_device *dev;
	108	unsigned mtu;
	109	struct sk_buff *skb;
	110	int err;
	111
	112	if (msg->msg_flags & MSG_OOB) {
	113	pr_debug("msg->msg_flags = 0x%x\n", msg->msg_flags);
	114	return -EOPNOTSUPP;
	115	}
	116
	117	lock_sock(sk);
	118	if (!sk->sk_bound_dev_if)
	119	dev = dev_getfirstbyhwtype(sock_net(sk), ARPHRD_IEEE802154);
	120	else
	121	dev = dev_get_by_index(sock_net(sk), sk->sk_bound_dev_if);
	122	release_sock(sk);
	123
	124	if (!dev) {
	125	pr_debug("no dev\n");
	126	err = -ENXIO;
	127	goto out;
	128	}
	129
	130	mtu = dev->mtu;
	131	pr_debug("name = %s, mtu = %u\n", dev->name, mtu);
	132
	133	if (size > mtu) {
e5241c44	134	pr_debug("size = %Zu, mtu = %u\n", size, mtu);
9ec76716 SL	135	err = -EINVAL;
	136	goto out_dev;
	137	}
	138
	139	skb = sock_alloc_send_skb(sk, LL_ALLOCATED_SPACE(dev) + size,
	140	msg->msg_flags & MSG_DONTWAIT, &err);
	141	if (!skb)
	142	goto out_dev;
	143
	144	skb_reserve(skb, LL_RESERVED_SPACE(dev));
	145
	146	skb_reset_mac_header(skb);
	147	skb_reset_network_header(skb);
	148
	149	err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size);
	150	if (err < 0)
	151	goto out_skb;
	152
	153	skb->dev = dev;
	154	skb->sk = sk;
	155	skb->protocol = htons(ETH_P_IEEE802154);
	156
	157	dev_put(dev);
	158
	159	err = dev_queue_xmit(skb);
	160	if (err > 0)
	161	err = net_xmit_errno(err);
	162
	163	return err ?: size;
	164
	165	out_skb:
	166	kfree_skb(skb);
	167	out_dev:
	168	dev_put(dev);
	169	out:
	170	return err;
	171	}
	172
	173	static int raw_recvmsg(struct kiocb iocb, struct sock sk, struct msghdr *msg,
	174	size_t len, int noblock, int flags, int *addr_len)
	175	{
	176	size_t copied = 0;
	177	int err = -EOPNOTSUPP;
	178	struct sk_buff *skb;
	179
	180	skb = skb_recv_datagram(sk, flags, noblock, &err);
	181	if (!skb)
	182	goto out;
	183
	184	copied = skb->len;
	185	if (len < copied) {
	186	msg->msg_flags \|= MSG_TRUNC;
	187	copied = len;
	188	}
	189
	190	err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
	191	if (err)
	192	goto done;
	193
	194	sock_recv_timestamp(msg, sk, skb);
	195
	196	if (flags & MSG_TRUNC)
	197	copied = skb->len;
	198	done:
199	skb_free_datagram(sk, skb);
200	out:
201	if (err)
202	return err;
203	return copied;
204	}
205
206	static int raw_rcv_skb(struct sock sk, struct sk_buff skb)
207	{
208	if (sock_queue_rcv_skb(sk, skb) < 0) {
209	atomic_inc(&sk->sk_drops);
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