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

Archive Download this file



interactive