[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.
 *
 * 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 <net/ieee802154_netdev.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 ieee802154_addr addr;
	struct sockaddr_ieee802154 *uaddr = (struct sockaddr_ieee802154 *)_uaddr;
	int err = 0;
	struct net_device *dev = NULL;

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

	uaddr = (struct sockaddr_ieee802154 *)_uaddr;
	if (uaddr->family != AF_IEEE802154)
		return -EINVAL;

	lock_sock(sk);

	ieee802154_addr_from_sa(&addr, &uaddr->addr);
	dev = ieee802154_get_dev(sock_net(sk), &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 int mtu;
	struct sk_buff *skb;
	int hlen, tlen;
	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;
	}

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

	skb_reserve(skb, hlen);

	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)
{
	skb = skb_share_check(skb, GFP_ATOMIC);
	if (!skb)
		return NET_RX_DROP;

	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;

	read_lock(&raw_lock);
	sk_for_each(sk, &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	*
9ec76716 SL	15	* Written by:
	16	* Sergey Lapin <slapin@ossfans.org>
	17	* Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
	18	*/
	19
	20	#include <linux/net.h>
	21	#include <linux/module.h>
	22	#include <linux/if_arp.h>
	23	#include <linux/list.h>
5a0e3ad6	24	#include <linux/slab.h>
9ec76716	25	#include <net/sock.h>
f0166e5e	26	#include <net/af_ieee802154.h>
e6278d92	27	#include <net/ieee802154_netdev.h>
9ec76716 SL	28
	29	#include "af802154.h"
	30
	31	static HLIST_HEAD(raw_head);
	32	static DEFINE_RWLOCK(raw_lock);
	33
	34	static void raw_hash(struct sock *sk)
	35	{
	36	write_lock_bh(&raw_lock);
	37	sk_add_node(sk, &raw_head);
	38	sock_prot_inuse_add(sock_net(sk), sk->sk_prot, 1);
	39	write_unlock_bh(&raw_lock);
	40	}
	41
	42	static void raw_unhash(struct sock *sk)
	43	{
	44	write_lock_bh(&raw_lock);
	45	if (sk_del_node_init(sk))
	46	sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
	47	write_unlock_bh(&raw_lock);
	48	}
	49
	50	static void raw_close(struct sock *sk, long timeout)
	51	{
	52	sk_common_release(sk);
	53	}
	54
e6278d92	55	static int raw_bind(struct sock sk, struct sockaddr _uaddr, int len)
9ec76716	56	{
e6278d92 PB	57	struct ieee802154_addr addr;
e6278d92 PB	58	struct sockaddr_ieee802154 uaddr = (struct sockaddr_ieee802154 )_uaddr;
9ec76716 SL	59	int err = 0;
	60	struct net_device *dev = NULL;
	61
e6278d92	62	if (len < sizeof(*uaddr))
9ec76716 SL	63	return -EINVAL;
9ec76716 SL	64
e6278d92 PB	65	uaddr = (struct sockaddr_ieee802154 *)_uaddr;
e6278d92 PB	66	if (uaddr->family != AF_IEEE802154)
9ec76716 SL	67	return -EINVAL;
	68
	69	lock_sock(sk);
	70
e6278d92 PB	71	ieee802154_addr_from_sa(&addr, &uaddr->addr);
e6278d92 PB	72	dev = ieee802154_get_dev(sock_net(sk), &addr);
9ec76716 SL	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,
4710d806	95	int addr_len)
9ec76716 SL	96	{
	97	return -ENOTSUPP;
	98	}
	99
	100	static int raw_disconnect(struct sock *sk, int flags)
	101	{
	102	return 0;
	103	}
	104
4710d806 VB	105	static int raw_sendmsg(struct kiocb iocb, struct sock sk,
4710d806 VB	106	struct msghdr *msg, size_t size)
9ec76716 SL	107	{
9ec76716 SL	108	struct net_device *dev;
95c96174	109	unsigned int mtu;
9ec76716	110	struct sk_buff *skb;
ae641949	111	int hlen, tlen;
9ec76716 SL	112	int err;
	113
	114	if (msg->msg_flags & MSG_OOB) {
	115	pr_debug("msg->msg_flags = 0x%x\n", msg->msg_flags);
	116	return -EOPNOTSUPP;
	117	}
	118
	119	lock_sock(sk);
	120	if (!sk->sk_bound_dev_if)
	121	dev = dev_getfirstbyhwtype(sock_net(sk), ARPHRD_IEEE802154);
	122	else
	123	dev = dev_get_by_index(sock_net(sk), sk->sk_bound_dev_if);
	124	release_sock(sk);
	125
	126	if (!dev) {
	127	pr_debug("no dev\n");
	128	err = -ENXIO;
	129	goto out;
	130	}
	131
	132	mtu = dev->mtu;
	133	pr_debug("name = %s, mtu = %u\n", dev->name, mtu);
	134
	135	if (size > mtu) {
e5241c44	136	pr_debug("size = %Zu, mtu = %u\n", size, mtu);
9ec76716 SL	137	err = -EINVAL;
	138	goto out_dev;
	139	}
	140
ae641949 HX	141	hlen = LL_RESERVED_SPACE(dev);
	142	tlen = dev->needed_tailroom;
	143	skb = sock_alloc_send_skb(sk, hlen + tlen + size,
4710d806	144	msg->msg_flags & MSG_DONTWAIT, &err);
9ec76716 SL	145	if (!skb)
	146	goto out_dev;
	147
ae641949	148	skb_reserve(skb, hlen);
9ec76716 SL	149
	150	skb_reset_mac_header(skb);
	151	skb_reset_network_header(skb);
	152
	153	err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size);
	154	if (err < 0)
	155	goto out_skb;
	156
	157	skb->dev = dev;
	158	skb->sk = sk;
	159	skb->protocol = htons(ETH_P_IEEE802154);
	160
	161	dev_put(dev);
	162
	163	err = dev_queue_xmit(skb);
	164	if (err > 0)
	165	err = net_xmit_errno(err);
	166
	167	return err ?: size;
	168
	169	out_skb:
	170	kfree_skb(skb);
	171	out_dev:
	172	dev_put(dev);
	173	out:
	174	return err;
	175	}
	176
	177	static int raw_recvmsg(struct kiocb iocb, struct sock sk, struct msghdr *msg,
	178	size_t len, int noblock, int flags, int *addr_len)
	179	{
	180	size_t copied = 0;
	181	int err = -EOPNOTSUPP;
	182	struct sk_buff *skb;
	183
	184	skb = skb_recv_datagram(sk, flags, noblock, &err);
	185	if (!skb)
	186	goto out;
	187
	188	copied = skb->len;
	189	if (len < copied) {
	190	msg->msg_flags \|= MSG_TRUNC;
	191	copied = len;
	192	}
	193
	194	err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
	195	if (err)
	196	goto done;
	197
3b885787	198	sock_recv_ts_and_drops(msg, sk, skb);
9ec76716 SL	199
	200	if (flags & MSG_TRUNC)
	201	copied = skb->len;
	202	done:
	203	skb_free_datagram(sk, skb);
	204	out:
	205	if (err)
	206	return err;
	207	return copied;
	208	}
	209
	210	static int raw_rcv_skb(struct sock sk, struct sk_buff skb)
	211	{
8cfad496 PB	212	skb = skb_share_check(skb, GFP_ATOMIC);
	213	if (!skb)
	214	return NET_RX_DROP;
	215
9ec76716	216	if (sock_queue_rcv_skb(sk, skb) < 0) {
9ec76716 SL	217	kfree_skb(skb);
	218	return NET_RX_DROP;
	219	}
	220
	221	return NET_RX_SUCCESS;
	222	}
	223
	224
	225	void ieee802154_raw_deliver(struct net_device dev, struct sk_buff skb)
	226	{
	227	struct sock *sk;
9ec76716 SL	228
9ec76716 SL	229	read_lock(&raw_lock);
b67bfe0d	230	sk_for_each(sk, &raw_head) {
9ec76716 SL	231	bh_lock_sock(sk);
	232	if (!sk->sk_bound_dev_if \|\|
	233	sk->sk_bound_dev_if == dev->ifindex) {
9ec76716 SL	234	struct sk_buff *clone;
	235
	236	clone = skb_clone(skb, GFP_ATOMIC);
	237	if (clone)
	238	raw_rcv_skb(sk, clone);
	239	}
	240	bh_unlock_sock(sk);
	241	}
	242	read_unlock(&raw_lock);
	243	}
	244
a9dfac33	245	static int raw_getsockopt(struct sock *sk, int level, int optname,
4710d806	246	char __user optval, int __user optlen)
a9dfac33 DES	247	{
	248	return -EOPNOTSUPP;
	249	}
	250
	251	static int raw_setsockopt(struct sock *sk, int level, int optname,
4710d806	252	char __user *optval, unsigned int optlen)
a9dfac33 DES	253	{
	254	return -EOPNOTSUPP;
	255	}
	256
9ec76716 SL	257	struct proto ieee802154_raw_prot = {
	258	.name = "IEEE-802.15.4-RAW",
	259	.owner = THIS_MODULE,
	260	.obj_size = sizeof(struct sock),
	261	.close = raw_close,
	262	.bind = raw_bind,
	263	.sendmsg = raw_sendmsg,
	264	.recvmsg = raw_recvmsg,
	265	.hash = raw_hash,
	266	.unhash = raw_unhash,
	267	.connect = raw_connect,
	268	.disconnect = raw_disconnect,
a9dfac33 DES	269	.getsockopt = raw_getsockopt,
a9dfac33 DES	270	.setsockopt = raw_setsockopt,
9ec76716	271	};