dgram.c revision 54af36e7136b5e111734ca5b06c6b4390d663cac 
1/*
2 * IEEE 802.15.4 dgram socket interface
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 <linux/slab.h>
29#include <net/sock.h>
30#include <net/af_ieee802154.h>
31#include <net/ieee802154.h>
32#include <net/ieee802154_netdev.h>
33
34#include <asm/ioctls.h>
35
36#include "af802154.h"
37
38static HLIST_HEAD(dgram_head);
39static DEFINE_RWLOCK(dgram_lock);
40
41struct dgram_sock {
42	struct sock sk;
43
44	struct ieee802154_addr src_addr;
45	struct ieee802154_addr dst_addr;
46
47	unsigned int bound:1;
48	unsigned int want_ack:1;
49};
50
51static inline struct dgram_sock *dgram_sk(const struct sock *sk)
52{
53	return container_of(sk, struct dgram_sock, sk);
54}
55
56static void dgram_hash(struct sock *sk)
57{
58	write_lock_bh(&dgram_lock);
59	sk_add_node(sk, &dgram_head);
60	sock_prot_inuse_add(sock_net(sk), sk->sk_prot, 1);
61	write_unlock_bh(&dgram_lock);
62}
63
64static void dgram_unhash(struct sock *sk)
65{
66	write_lock_bh(&dgram_lock);
67	if (sk_del_node_init(sk))
68		sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
69	write_unlock_bh(&dgram_lock);
70}
71
72static int dgram_init(struct sock *sk)
73{
74	struct dgram_sock *ro = dgram_sk(sk);
75
76	ro->dst_addr.mode = IEEE802154_ADDR_LONG;
77	ro->dst_addr.pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
78	ro->want_ack = 1;
79	memset(&ro->dst_addr.extended_addr, 0xff, IEEE802154_ADDR_LEN);
80	return 0;
81}
82
83static void dgram_close(struct sock *sk, long timeout)
84{
85	sk_common_release(sk);
86}
87
88static int dgram_bind(struct sock *sk, struct sockaddr *uaddr, int len)
89{
90	struct sockaddr_ieee802154 *addr = (struct sockaddr_ieee802154 *)uaddr;
91	struct ieee802154_addr haddr;
92	struct dgram_sock *ro = dgram_sk(sk);
93	int err = -EINVAL;
94	struct net_device *dev;
95
96	lock_sock(sk);
97
98	ro->bound = 0;
99
100	if (len < sizeof(*addr))
101		goto out;
102
103	if (addr->family != AF_IEEE802154)
104		goto out;
105
106	ieee802154_addr_from_sa(&haddr, &addr->addr);
107	dev = ieee802154_get_dev(sock_net(sk), &haddr);
108	if (!dev) {
109		err = -ENODEV;
110		goto out;
111	}
112
113	if (dev->type != ARPHRD_IEEE802154) {
114		err = -ENODEV;
115		goto out_put;
116	}
117
118	ro->src_addr = haddr;
119
120	ro->bound = 1;
121	err = 0;
122out_put:
123	dev_put(dev);
124out:
125	release_sock(sk);
126
127	return err;
128}
129
130static int dgram_ioctl(struct sock *sk, int cmd, unsigned long arg)
131{
132	switch (cmd) {
133	case SIOCOUTQ:
134	{
135		int amount = sk_wmem_alloc_get(sk);
136
137		return put_user(amount, (int __user *)arg);
138	}
139
140	case SIOCINQ:
141	{
142		struct sk_buff *skb;
143		unsigned long amount;
144
145		amount = 0;
146		spin_lock_bh(&sk->sk_receive_queue.lock);
147		skb = skb_peek(&sk->sk_receive_queue);
148		if (skb != NULL) {
149			/*
150			 * We will only return the amount
151			 * of this packet since that is all
152			 * that will be read.
153			 */
154			amount = skb->len - ieee802154_hdr_length(skb);
155		}
156		spin_unlock_bh(&sk->sk_receive_queue.lock);
157		return put_user(amount, (int __user *)arg);
158	}
159
160	}
161	return -ENOIOCTLCMD;
162}
163
164/* FIXME: autobind */
165static int dgram_connect(struct sock *sk, struct sockaddr *uaddr,
166			int len)
167{
168	struct sockaddr_ieee802154 *addr = (struct sockaddr_ieee802154 *)uaddr;
169	struct dgram_sock *ro = dgram_sk(sk);
170	int err = 0;
171
172	if (len < sizeof(*addr))
173		return -EINVAL;
174
175	if (addr->family != AF_IEEE802154)
176		return -EINVAL;
177
178	lock_sock(sk);
179
180	if (!ro->bound) {
181		err = -ENETUNREACH;
182		goto out;
183	}
184
185	ieee802154_addr_from_sa(&ro->dst_addr, &addr->addr);
186
187out:
188	release_sock(sk);
189	return err;
190}
191
192static int dgram_disconnect(struct sock *sk, int flags)
193{
194	struct dgram_sock *ro = dgram_sk(sk);
195
196	lock_sock(sk);
197
198	ro->dst_addr.mode = IEEE802154_ADDR_LONG;
199	memset(&ro->dst_addr.extended_addr, 0xff, IEEE802154_ADDR_LEN);
200
201	release_sock(sk);
202
203	return 0;
204}
205
206static int dgram_sendmsg(struct kiocb *iocb, struct sock *sk,
207		struct msghdr *msg, size_t size)
208{
209	struct net_device *dev;
210	unsigned int mtu;
211	struct sk_buff *skb;
212	struct dgram_sock *ro = dgram_sk(sk);
213	int hlen, tlen;
214	int err;
215
216	if (msg->msg_flags & MSG_OOB) {
217		pr_debug("msg->msg_flags = 0x%x\n", msg->msg_flags);
218		return -EOPNOTSUPP;
219	}
220
221	if (!ro->bound)
222		dev = dev_getfirstbyhwtype(sock_net(sk), ARPHRD_IEEE802154);
223	else
224		dev = ieee802154_get_dev(sock_net(sk), &ro->src_addr);
225
226	if (!dev) {
227		pr_debug("no dev\n");
228		err = -ENXIO;
229		goto out;
230	}
231	mtu = dev->mtu;
232	pr_debug("name = %s, mtu = %u\n", dev->name, mtu);
233
234	if (size > mtu) {
235		pr_debug("size = %Zu, mtu = %u\n", size, mtu);
236		err = -EMSGSIZE;
237		goto out_dev;
238	}
239
240	hlen = LL_RESERVED_SPACE(dev);
241	tlen = dev->needed_tailroom;
242	skb = sock_alloc_send_skb(sk, hlen + tlen + size,
243			msg->msg_flags & MSG_DONTWAIT,
244			&err);
245	if (!skb)
246		goto out_dev;
247
248	skb_reserve(skb, hlen);
249
250	skb_reset_network_header(skb);
251
252	mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA;
253	if (ro->want_ack)
254		mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
255
256	mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
257	err = dev_hard_header(skb, dev, ETH_P_IEEE802154, &ro->dst_addr,
258			ro->bound ? &ro->src_addr : NULL, size);
259	if (err < 0)
260		goto out_skb;
261
262	skb_reset_mac_header(skb);
263
264	err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size);
265	if (err < 0)
266		goto out_skb;
267
268	skb->dev = dev;
269	skb->sk  = sk;
270	skb->protocol = htons(ETH_P_IEEE802154);
271
272	dev_put(dev);
273
274	err = dev_queue_xmit(skb);
275	if (err > 0)
276		err = net_xmit_errno(err);
277
278	return err ?: size;
279
280out_skb:
281	kfree_skb(skb);
282out_dev:
283	dev_put(dev);
284out:
285	return err;
286}
287
288static int dgram_recvmsg(struct kiocb *iocb, struct sock *sk,
289		struct msghdr *msg, size_t len, int noblock, int flags,
290		int *addr_len)
291{
292	size_t copied = 0;
293	int err = -EOPNOTSUPP;
294	struct sk_buff *skb;
295	DECLARE_SOCKADDR(struct sockaddr_ieee802154 *, saddr, msg->msg_name);
296
297	skb = skb_recv_datagram(sk, flags, noblock, &err);
298	if (!skb)
299		goto out;
300
301	copied = skb->len;
302	if (len < copied) {
303		msg->msg_flags |= MSG_TRUNC;
304		copied = len;
305	}
306
307	/* FIXME: skip headers if necessary ?! */
308	err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
309	if (err)
310		goto done;
311
312	sock_recv_ts_and_drops(msg, sk, skb);
313
314	if (saddr) {
315		saddr->family = AF_IEEE802154;
316		ieee802154_addr_to_sa(&saddr->addr, &mac_cb(skb)->source);
317		*addr_len = sizeof(*saddr);
318	}
319
320	if (flags & MSG_TRUNC)
321		copied = skb->len;
322done:
323	skb_free_datagram(sk, skb);
324out:
325	if (err)
326		return err;
327	return copied;
328}
329
330static int dgram_rcv_skb(struct sock *sk, struct sk_buff *skb)
331{
332	skb = skb_share_check(skb, GFP_ATOMIC);
333	if (!skb)
334		return NET_RX_DROP;
335
336	if (sock_queue_rcv_skb(sk, skb) < 0) {
337		kfree_skb(skb);
338		return NET_RX_DROP;
339	}
340
341	return NET_RX_SUCCESS;
342}
343
344static inline bool
345ieee802154_match_sock(__le64 hw_addr, __le16 pan_id, __le16 short_addr,
346		      struct dgram_sock *ro)
347{
348	if (!ro->bound)
349		return true;
350
351	if (ro->src_addr.mode == IEEE802154_ADDR_LONG &&
352	    hw_addr == ro->src_addr.extended_addr)
353		return true;
354
355	if (ro->src_addr.mode == IEEE802154_ADDR_SHORT &&
356	    pan_id == ro->src_addr.pan_id &&
357	    short_addr == ro->src_addr.short_addr)
358		return true;
359
360	return false;
361}
362
363int ieee802154_dgram_deliver(struct net_device *dev, struct sk_buff *skb)
364{
365	struct sock *sk, *prev = NULL;
366	int ret = NET_RX_SUCCESS;
367	__le16 pan_id, short_addr;
368	__le64 hw_addr;
369
370	/* Data frame processing */
371	BUG_ON(dev->type != ARPHRD_IEEE802154);
372
373	pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
374	short_addr = ieee802154_mlme_ops(dev)->get_short_addr(dev);
375	hw_addr = ieee802154_devaddr_from_raw(dev->dev_addr);
376
377	read_lock(&dgram_lock);
378	sk_for_each(sk, &dgram_head) {
379		if (ieee802154_match_sock(hw_addr, pan_id, short_addr,
380					  dgram_sk(sk))) {
381			if (prev) {
382				struct sk_buff *clone;
383				clone = skb_clone(skb, GFP_ATOMIC);
384				if (clone)
385					dgram_rcv_skb(prev, clone);
386			}
387
388			prev = sk;
389		}
390	}
391
392	if (prev)
393		dgram_rcv_skb(prev, skb);
394	else {
395		kfree_skb(skb);
396		ret = NET_RX_DROP;
397	}
398	read_unlock(&dgram_lock);
399
400	return ret;
401}
402
403static int dgram_getsockopt(struct sock *sk, int level, int optname,
404		    char __user *optval, int __user *optlen)
405{
406	struct dgram_sock *ro = dgram_sk(sk);
407
408	int val, len;
409
410	if (level != SOL_IEEE802154)
411		return -EOPNOTSUPP;
412
413	if (get_user(len, optlen))
414		return -EFAULT;
415
416	len = min_t(unsigned int, len, sizeof(int));
417
418	switch (optname) {
419	case WPAN_WANTACK:
420		val = ro->want_ack;
421		break;
422	default:
423		return -ENOPROTOOPT;
424	}
425
426	if (put_user(len, optlen))
427		return -EFAULT;
428	if (copy_to_user(optval, &val, len))
429		return -EFAULT;
430	return 0;
431}
432
433static int dgram_setsockopt(struct sock *sk, int level, int optname,
434		    char __user *optval, unsigned int optlen)
435{
436	struct dgram_sock *ro = dgram_sk(sk);
437	int val;
438	int err = 0;
439
440	if (optlen < sizeof(int))
441		return -EINVAL;
442
443	if (get_user(val, (int __user *)optval))
444		return -EFAULT;
445
446	lock_sock(sk);
447
448	switch (optname) {
449	case WPAN_WANTACK:
450		ro->want_ack = !!val;
451		break;
452	default:
453		err = -ENOPROTOOPT;
454		break;
455	}
456
457	release_sock(sk);
458	return err;
459}
460
461struct proto ieee802154_dgram_prot = {
462	.name		= "IEEE-802.15.4-MAC",
463	.owner		= THIS_MODULE,
464	.obj_size	= sizeof(struct dgram_sock),
465	.init		= dgram_init,
466	.close		= dgram_close,
467	.bind		= dgram_bind,
468	.sendmsg	= dgram_sendmsg,
469	.recvmsg	= dgram_recvmsg,
470	.hash		= dgram_hash,
471	.unhash		= dgram_unhash,
472	.connect	= dgram_connect,
473	.disconnect	= dgram_disconnect,
474	.ioctl		= dgram_ioctl,
475	.getsockopt	= dgram_getsockopt,
476	.setsockopt	= dgram_setsockopt,
477};
478
479