net: add IEEE 802.15.4 socket family implementation
[linux-2.6.git] / net / ieee802154 / dgram.c
1 /*
2  * ZigBee 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 <net/sock.h>
29 #include <net/ieee802154/af_ieee802154.h>
30 #include <net/ieee802154/mac_def.h>
31 #include <net/ieee802154/netdevice.h>
32
33 #include <asm/ioctls.h>
34
35 #include "af802154.h"
36
37 static HLIST_HEAD(dgram_head);
38 static DEFINE_RWLOCK(dgram_lock);
39
40 struct dgram_sock {
41         struct sock sk;
42
43         int bound;
44         struct ieee802154_addr src_addr;
45         struct ieee802154_addr dst_addr;
46 };
47
48 static inline struct dgram_sock *dgram_sk(const struct sock *sk)
49 {
50         return container_of(sk, struct dgram_sock, sk);
51 }
52
53
54 static void dgram_hash(struct sock *sk)
55 {
56         write_lock_bh(&dgram_lock);
57         sk_add_node(sk, &dgram_head);
58         sock_prot_inuse_add(sock_net(sk), sk->sk_prot, 1);
59         write_unlock_bh(&dgram_lock);
60 }
61
62 static void dgram_unhash(struct sock *sk)
63 {
64         write_lock_bh(&dgram_lock);
65         if (sk_del_node_init(sk))
66                 sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
67         write_unlock_bh(&dgram_lock);
68 }
69
70 static int dgram_init(struct sock *sk)
71 {
72         struct dgram_sock *ro = dgram_sk(sk);
73
74         ro->dst_addr.addr_type = IEEE802154_ADDR_LONG;
75         ro->dst_addr.pan_id = 0xffff;
76         memset(&ro->dst_addr.hwaddr, 0xff, sizeof(ro->dst_addr.hwaddr));
77         return 0;
78 }
79
80 static void dgram_close(struct sock *sk, long timeout)
81 {
82         sk_common_release(sk);
83 }
84
85 static int dgram_bind(struct sock *sk, struct sockaddr *uaddr, int len)
86 {
87         struct sockaddr_ieee802154 *addr = (struct sockaddr_ieee802154 *)uaddr;
88         struct dgram_sock *ro = dgram_sk(sk);
89         int err = 0;
90         struct net_device *dev;
91
92         ro->bound = 0;
93
94         if (len < sizeof(*addr))
95                 return -EINVAL;
96
97         if (addr->family != AF_IEEE802154)
98                 return -EINVAL;
99
100         lock_sock(sk);
101
102         dev = ieee802154_get_dev(sock_net(sk), &addr->addr);
103         if (!dev) {
104                 err = -ENODEV;
105                 goto out;
106         }
107
108         if (dev->type != ARPHRD_IEEE802154) {
109                 err = -ENODEV;
110                 goto out_put;
111         }
112
113         memcpy(&ro->src_addr, &addr->addr, sizeof(struct ieee802154_addr));
114
115         ro->bound = 1;
116 out_put:
117         dev_put(dev);
118 out:
119         release_sock(sk);
120
121         return err;
122 }
123
124 static int dgram_ioctl(struct sock *sk, int cmd, unsigned long arg)
125 {
126         switch (cmd) {
127         case SIOCOUTQ:
128         {
129                 int amount = atomic_read(&sk->sk_wmem_alloc);
130                 return put_user(amount, (int __user *)arg);
131         }
132
133         case SIOCINQ:
134         {
135                 struct sk_buff *skb;
136                 unsigned long amount;
137
138                 amount = 0;
139                 spin_lock_bh(&sk->sk_receive_queue.lock);
140                 skb = skb_peek(&sk->sk_receive_queue);
141                 if (skb != NULL) {
142                         /*
143                          * We will only return the amount
144                          * of this packet since that is all
145                          * that will be read.
146                          */
147                         /* FIXME: parse the header for more correct value */
148                         amount = skb->len - (3+8+8);
149                 }
150                 spin_unlock_bh(&sk->sk_receive_queue.lock);
151                 return put_user(amount, (int __user *)arg);
152         }
153
154         }
155         return -ENOIOCTLCMD;
156 }
157
158 /* FIXME: autobind */
159 static int dgram_connect(struct sock *sk, struct sockaddr *uaddr,
160                         int len)
161 {
162         struct sockaddr_ieee802154 *addr = (struct sockaddr_ieee802154 *)uaddr;
163         struct dgram_sock *ro = dgram_sk(sk);
164         int err = 0;
165
166         if (len < sizeof(*addr))
167                 return -EINVAL;
168
169         if (addr->family != AF_IEEE802154)
170                 return -EINVAL;
171
172         lock_sock(sk);
173
174         if (!ro->bound) {
175                 err = -ENETUNREACH;
176                 goto out;
177         }
178
179         memcpy(&ro->dst_addr, &addr->addr, sizeof(struct ieee802154_addr));
180
181 out:
182         release_sock(sk);
183         return err;
184 }
185
186 static int dgram_disconnect(struct sock *sk, int flags)
187 {
188         struct dgram_sock *ro = dgram_sk(sk);
189
190         lock_sock(sk);
191
192         ro->dst_addr.addr_type = IEEE802154_ADDR_LONG;
193         memset(&ro->dst_addr.hwaddr, 0xff, sizeof(ro->dst_addr.hwaddr));
194
195         release_sock(sk);
196
197         return 0;
198 }
199
200 static int dgram_sendmsg(struct kiocb *iocb, struct sock *sk,
201                 struct msghdr *msg, size_t size)
202 {
203         struct net_device *dev;
204         unsigned mtu;
205         struct sk_buff *skb;
206         struct dgram_sock *ro = dgram_sk(sk);
207         int err;
208
209         if (msg->msg_flags & MSG_OOB) {
210                 pr_debug("msg->msg_flags = 0x%x\n", msg->msg_flags);
211                 return -EOPNOTSUPP;
212         }
213
214         if (!ro->bound)
215                 dev = dev_getfirstbyhwtype(sock_net(sk), ARPHRD_IEEE802154);
216         else
217                 dev = ieee802154_get_dev(sock_net(sk), &ro->src_addr);
218
219         if (!dev) {
220                 pr_debug("no dev\n");
221                 err = -ENXIO;
222                 goto out;
223         }
224         mtu = dev->mtu;
225         pr_debug("name = %s, mtu = %u\n", dev->name, mtu);
226
227         skb = sock_alloc_send_skb(sk, LL_ALLOCATED_SPACE(dev) + size,
228                         msg->msg_flags & MSG_DONTWAIT,
229                         &err);
230         if (!skb)
231                 goto out_dev;
232
233         skb_reserve(skb, LL_RESERVED_SPACE(dev));
234
235         skb_reset_network_header(skb);
236
237         mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA | MAC_CB_FLAG_ACKREQ;
238         mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
239         err = dev_hard_header(skb, dev, ETH_P_IEEE802154, &ro->dst_addr,
240                         ro->bound ? &ro->src_addr : NULL, size);
241         if (err < 0)
242                 goto out_skb;
243
244         skb_reset_mac_header(skb);
245
246         err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size);
247         if (err < 0)
248                 goto out_skb;
249
250         if (size > mtu) {
251                 pr_debug("size = %u, mtu = %u\n", size, mtu);
252                 err = -EINVAL;
253                 goto out_skb;
254         }
255
256         skb->dev = dev;
257         skb->sk  = sk;
258         skb->protocol = htons(ETH_P_IEEE802154);
259
260         dev_put(dev);
261
262         err = dev_queue_xmit(skb);
263         if (err > 0)
264                 err = net_xmit_errno(err);
265
266         return err ?: size;
267
268 out_skb:
269         kfree_skb(skb);
270 out_dev:
271         dev_put(dev);
272 out:
273         return err;
274 }
275
276 static int dgram_recvmsg(struct kiocb *iocb, struct sock *sk,
277                 struct msghdr *msg, size_t len, int noblock, int flags,
278                 int *addr_len)
279 {
280         size_t copied = 0;
281         int err = -EOPNOTSUPP;
282         struct sk_buff *skb;
283
284         skb = skb_recv_datagram(sk, flags, noblock, &err);
285         if (!skb)
286                 goto out;
287
288         copied = skb->len;
289         if (len < copied) {
290                 msg->msg_flags |= MSG_TRUNC;
291                 copied = len;
292         }
293
294         /* FIXME: skip headers if necessary ?! */
295         err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
296         if (err)
297                 goto done;
298
299         sock_recv_timestamp(msg, sk, skb);
300
301         if (flags & MSG_TRUNC)
302                 copied = skb->len;
303 done:
304         skb_free_datagram(sk, skb);
305 out:
306         if (err)
307                 return err;
308         return copied;
309 }
310
311 static int dgram_rcv_skb(struct sock *sk, struct sk_buff *skb)
312 {
313         if (sock_queue_rcv_skb(sk, skb) < 0) {
314                 atomic_inc(&sk->sk_drops);
315                 kfree_skb(skb);
316                 return NET_RX_DROP;
317         }
318
319         return NET_RX_SUCCESS;
320 }
321
322 static inline int ieee802154_match_sock(u8 *hw_addr, u16 pan_id,
323                 u16 short_addr, struct dgram_sock *ro)
324 {
325         if (!ro->bound)
326                 return 1;
327
328         if (ro->src_addr.addr_type == IEEE802154_ADDR_LONG &&
329             !memcmp(ro->src_addr.hwaddr, hw_addr, IEEE802154_ADDR_LEN))
330                 return 1;
331
332         if (ro->src_addr.addr_type == IEEE802154_ADDR_SHORT &&
333                      pan_id == ro->src_addr.pan_id &&
334                      short_addr == ro->src_addr.short_addr)
335                 return 1;
336
337         return 0;
338 }
339
340 int ieee802154_dgram_deliver(struct net_device *dev, struct sk_buff *skb)
341 {
342         struct sock *sk, *prev = NULL;
343         struct hlist_node *node;
344         int ret = NET_RX_SUCCESS;
345         u16 pan_id, short_addr;
346
347         /* Data frame processing */
348         BUG_ON(dev->type != ARPHRD_IEEE802154);
349
350         pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
351         short_addr = ieee802154_mlme_ops(dev)->get_short_addr(dev);
352
353         read_lock(&dgram_lock);
354         sk_for_each(sk, node, &dgram_head) {
355                 if (ieee802154_match_sock(dev->dev_addr, pan_id, short_addr,
356                                         dgram_sk(sk))) {
357                         if (prev) {
358                                 struct sk_buff *clone;
359                                 clone = skb_clone(skb, GFP_ATOMIC);
360                                 if (clone)
361                                         dgram_rcv_skb(prev, clone);
362                         }
363
364                         prev = sk;
365                 }
366         }
367
368         if (prev)
369                 dgram_rcv_skb(prev, skb);
370         else {
371                 kfree_skb(skb);
372                 ret = NET_RX_DROP;
373         }
374         read_unlock(&dgram_lock);
375
376         return ret;
377 }
378
379 struct proto ieee802154_dgram_prot = {
380         .name           = "IEEE-802.15.4-MAC",
381         .owner          = THIS_MODULE,
382         .obj_size       = sizeof(struct dgram_sock),
383         .init           = dgram_init,
384         .close          = dgram_close,
385         .bind           = dgram_bind,
386         .sendmsg        = dgram_sendmsg,
387         .recvmsg        = dgram_recvmsg,
388         .hash           = dgram_hash,
389         .unhash         = dgram_unhash,
390         .connect        = dgram_connect,
391         .disconnect     = dgram_disconnect,
392         .ioctl          = dgram_ioctl,
393 };
394