GNU Linux-libre 6.9.2-gnu
[releases.git] / net / ieee802154 / socket.c
1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * IEEE802154.4 socket interface
4  *
5  * Copyright 2007, 2008 Siemens AG
6  *
7  * Written by:
8  * Sergey Lapin <slapin@ossfans.org>
9  * Maxim Gorbachyov <maxim.gorbachev@siemens.com>
10  */
11
12 #include <linux/net.h>
13 #include <linux/capability.h>
14 #include <linux/module.h>
15 #include <linux/if_arp.h>
16 #include <linux/if.h>
17 #include <linux/termios.h>      /* For TIOCOUTQ/INQ */
18 #include <linux/list.h>
19 #include <linux/slab.h>
20 #include <linux/socket.h>
21 #include <net/datalink.h>
22 #include <net/psnap.h>
23 #include <net/sock.h>
24 #include <net/tcp_states.h>
25 #include <net/route.h>
26
27 #include <net/af_ieee802154.h>
28 #include <net/ieee802154_netdev.h>
29
30 /* Utility function for families */
31 static struct net_device*
32 ieee802154_get_dev(struct net *net, const struct ieee802154_addr *addr)
33 {
34         struct net_device *dev = NULL;
35         struct net_device *tmp;
36         __le16 pan_id, short_addr;
37         u8 hwaddr[IEEE802154_ADDR_LEN];
38
39         switch (addr->mode) {
40         case IEEE802154_ADDR_LONG:
41                 ieee802154_devaddr_to_raw(hwaddr, addr->extended_addr);
42                 rcu_read_lock();
43                 dev = dev_getbyhwaddr_rcu(net, ARPHRD_IEEE802154, hwaddr);
44                 dev_hold(dev);
45                 rcu_read_unlock();
46                 break;
47         case IEEE802154_ADDR_SHORT:
48                 if (addr->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST) ||
49                     addr->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) ||
50                     addr->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST))
51                         break;
52
53                 rtnl_lock();
54
55                 for_each_netdev(net, tmp) {
56                         if (tmp->type != ARPHRD_IEEE802154)
57                                 continue;
58
59                         pan_id = tmp->ieee802154_ptr->pan_id;
60                         short_addr = tmp->ieee802154_ptr->short_addr;
61                         if (pan_id == addr->pan_id &&
62                             short_addr == addr->short_addr) {
63                                 dev = tmp;
64                                 dev_hold(dev);
65                                 break;
66                         }
67                 }
68
69                 rtnl_unlock();
70                 break;
71         default:
72                 pr_warn("Unsupported ieee802154 address type: %d\n",
73                         addr->mode);
74                 break;
75         }
76
77         return dev;
78 }
79
80 static int ieee802154_sock_release(struct socket *sock)
81 {
82         struct sock *sk = sock->sk;
83
84         if (sk) {
85                 sock->sk = NULL;
86                 sk->sk_prot->close(sk, 0);
87         }
88         return 0;
89 }
90
91 static int ieee802154_sock_sendmsg(struct socket *sock, struct msghdr *msg,
92                                    size_t len)
93 {
94         struct sock *sk = sock->sk;
95
96         return sk->sk_prot->sendmsg(sk, msg, len);
97 }
98
99 static int ieee802154_sock_bind(struct socket *sock, struct sockaddr *uaddr,
100                                 int addr_len)
101 {
102         struct sock *sk = sock->sk;
103
104         if (sk->sk_prot->bind)
105                 return sk->sk_prot->bind(sk, uaddr, addr_len);
106
107         return sock_no_bind(sock, uaddr, addr_len);
108 }
109
110 static int ieee802154_sock_connect(struct socket *sock, struct sockaddr *uaddr,
111                                    int addr_len, int flags)
112 {
113         struct sock *sk = sock->sk;
114
115         if (addr_len < sizeof(uaddr->sa_family))
116                 return -EINVAL;
117
118         if (uaddr->sa_family == AF_UNSPEC)
119                 return sk->sk_prot->disconnect(sk, flags);
120
121         return sk->sk_prot->connect(sk, uaddr, addr_len);
122 }
123
124 static int ieee802154_dev_ioctl(struct sock *sk, struct ifreq __user *arg,
125                                 unsigned int cmd)
126 {
127         struct ifreq ifr;
128         int ret = -ENOIOCTLCMD;
129         struct net_device *dev;
130
131         if (get_user_ifreq(&ifr, NULL, arg))
132                 return -EFAULT;
133
134         ifr.ifr_name[IFNAMSIZ-1] = 0;
135
136         dev_load(sock_net(sk), ifr.ifr_name);
137         dev = dev_get_by_name(sock_net(sk), ifr.ifr_name);
138
139         if (!dev)
140                 return -ENODEV;
141
142         if (dev->type == ARPHRD_IEEE802154 && dev->netdev_ops->ndo_do_ioctl)
143                 ret = dev->netdev_ops->ndo_do_ioctl(dev, &ifr, cmd);
144
145         if (!ret && put_user_ifreq(&ifr, arg))
146                 ret = -EFAULT;
147         dev_put(dev);
148
149         return ret;
150 }
151
152 static int ieee802154_sock_ioctl(struct socket *sock, unsigned int cmd,
153                                  unsigned long arg)
154 {
155         struct sock *sk = sock->sk;
156
157         switch (cmd) {
158         case SIOCGIFADDR:
159         case SIOCSIFADDR:
160                 return ieee802154_dev_ioctl(sk, (struct ifreq __user *)arg,
161                                 cmd);
162         default:
163                 if (!sk->sk_prot->ioctl)
164                         return -ENOIOCTLCMD;
165                 return sk_ioctl(sk, cmd, (void __user *)arg);
166         }
167 }
168
169 /* RAW Sockets (802.15.4 created in userspace) */
170 static HLIST_HEAD(raw_head);
171 static DEFINE_RWLOCK(raw_lock);
172
173 static int raw_hash(struct sock *sk)
174 {
175         write_lock_bh(&raw_lock);
176         sk_add_node(sk, &raw_head);
177         write_unlock_bh(&raw_lock);
178         sock_prot_inuse_add(sock_net(sk), sk->sk_prot, 1);
179
180         return 0;
181 }
182
183 static void raw_unhash(struct sock *sk)
184 {
185         write_lock_bh(&raw_lock);
186         if (sk_del_node_init(sk))
187                 sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
188         write_unlock_bh(&raw_lock);
189 }
190
191 static void raw_close(struct sock *sk, long timeout)
192 {
193         sk_common_release(sk);
194 }
195
196 static int raw_bind(struct sock *sk, struct sockaddr *_uaddr, int len)
197 {
198         struct ieee802154_addr addr;
199         struct sockaddr_ieee802154 *uaddr = (struct sockaddr_ieee802154 *)_uaddr;
200         int err = 0;
201         struct net_device *dev = NULL;
202
203         err = ieee802154_sockaddr_check_size(uaddr, len);
204         if (err < 0)
205                 return err;
206
207         uaddr = (struct sockaddr_ieee802154 *)_uaddr;
208         if (uaddr->family != AF_IEEE802154)
209                 return -EINVAL;
210
211         lock_sock(sk);
212
213         ieee802154_addr_from_sa(&addr, &uaddr->addr);
214         dev = ieee802154_get_dev(sock_net(sk), &addr);
215         if (!dev) {
216                 err = -ENODEV;
217                 goto out;
218         }
219
220         sk->sk_bound_dev_if = dev->ifindex;
221         sk_dst_reset(sk);
222
223         dev_put(dev);
224 out:
225         release_sock(sk);
226
227         return err;
228 }
229
230 static int raw_connect(struct sock *sk, struct sockaddr *uaddr,
231                        int addr_len)
232 {
233         return -ENOTSUPP;
234 }
235
236 static int raw_disconnect(struct sock *sk, int flags)
237 {
238         return 0;
239 }
240
241 static int raw_sendmsg(struct sock *sk, struct msghdr *msg, size_t size)
242 {
243         struct net_device *dev;
244         unsigned int mtu;
245         struct sk_buff *skb;
246         int hlen, tlen;
247         int err;
248
249         if (msg->msg_flags & MSG_OOB) {
250                 pr_debug("msg->msg_flags = 0x%x\n", msg->msg_flags);
251                 return -EOPNOTSUPP;
252         }
253
254         lock_sock(sk);
255         if (!sk->sk_bound_dev_if)
256                 dev = dev_getfirstbyhwtype(sock_net(sk), ARPHRD_IEEE802154);
257         else
258                 dev = dev_get_by_index(sock_net(sk), sk->sk_bound_dev_if);
259         release_sock(sk);
260
261         if (!dev) {
262                 pr_debug("no dev\n");
263                 err = -ENXIO;
264                 goto out;
265         }
266
267         mtu = IEEE802154_MTU;
268         pr_debug("name = %s, mtu = %u\n", dev->name, mtu);
269
270         if (size > mtu) {
271                 pr_debug("size = %zu, mtu = %u\n", size, mtu);
272                 err = -EMSGSIZE;
273                 goto out_dev;
274         }
275         if (!size) {
276                 err = 0;
277                 goto out_dev;
278         }
279
280         hlen = LL_RESERVED_SPACE(dev);
281         tlen = dev->needed_tailroom;
282         skb = sock_alloc_send_skb(sk, hlen + tlen + size,
283                                   msg->msg_flags & MSG_DONTWAIT, &err);
284         if (!skb)
285                 goto out_dev;
286
287         skb_reserve(skb, hlen);
288
289         skb_reset_mac_header(skb);
290         skb_reset_network_header(skb);
291
292         err = memcpy_from_msg(skb_put(skb, size), msg, size);
293         if (err < 0)
294                 goto out_skb;
295
296         skb->dev = dev;
297         skb->protocol = htons(ETH_P_IEEE802154);
298
299         err = dev_queue_xmit(skb);
300         if (err > 0)
301                 err = net_xmit_errno(err);
302
303         dev_put(dev);
304
305         return err ?: size;
306
307 out_skb:
308         kfree_skb(skb);
309 out_dev:
310         dev_put(dev);
311 out:
312         return err;
313 }
314
315 static int raw_recvmsg(struct sock *sk, struct msghdr *msg, size_t len,
316                        int flags, int *addr_len)
317 {
318         size_t copied = 0;
319         int err = -EOPNOTSUPP;
320         struct sk_buff *skb;
321
322         skb = skb_recv_datagram(sk, flags, &err);
323         if (!skb)
324                 goto out;
325
326         copied = skb->len;
327         if (len < copied) {
328                 msg->msg_flags |= MSG_TRUNC;
329                 copied = len;
330         }
331
332         err = skb_copy_datagram_msg(skb, 0, msg, copied);
333         if (err)
334                 goto done;
335
336         sock_recv_cmsgs(msg, sk, skb);
337
338         if (flags & MSG_TRUNC)
339                 copied = skb->len;
340 done:
341         skb_free_datagram(sk, skb);
342 out:
343         if (err)
344                 return err;
345         return copied;
346 }
347
348 static int raw_rcv_skb(struct sock *sk, struct sk_buff *skb)
349 {
350         skb = skb_share_check(skb, GFP_ATOMIC);
351         if (!skb)
352                 return NET_RX_DROP;
353
354         if (sock_queue_rcv_skb(sk, skb) < 0) {
355                 kfree_skb(skb);
356                 return NET_RX_DROP;
357         }
358
359         return NET_RX_SUCCESS;
360 }
361
362 static void ieee802154_raw_deliver(struct net_device *dev, struct sk_buff *skb)
363 {
364         struct sock *sk;
365
366         read_lock(&raw_lock);
367         sk_for_each(sk, &raw_head) {
368                 bh_lock_sock(sk);
369                 if (!sk->sk_bound_dev_if ||
370                     sk->sk_bound_dev_if == dev->ifindex) {
371                         struct sk_buff *clone;
372
373                         clone = skb_clone(skb, GFP_ATOMIC);
374                         if (clone)
375                                 raw_rcv_skb(sk, clone);
376                 }
377                 bh_unlock_sock(sk);
378         }
379         read_unlock(&raw_lock);
380 }
381
382 static int raw_getsockopt(struct sock *sk, int level, int optname,
383                           char __user *optval, int __user *optlen)
384 {
385         return -EOPNOTSUPP;
386 }
387
388 static int raw_setsockopt(struct sock *sk, int level, int optname,
389                           sockptr_t optval, unsigned int optlen)
390 {
391         return -EOPNOTSUPP;
392 }
393
394 static struct proto ieee802154_raw_prot = {
395         .name           = "IEEE-802.15.4-RAW",
396         .owner          = THIS_MODULE,
397         .obj_size       = sizeof(struct sock),
398         .close          = raw_close,
399         .bind           = raw_bind,
400         .sendmsg        = raw_sendmsg,
401         .recvmsg        = raw_recvmsg,
402         .hash           = raw_hash,
403         .unhash         = raw_unhash,
404         .connect        = raw_connect,
405         .disconnect     = raw_disconnect,
406         .getsockopt     = raw_getsockopt,
407         .setsockopt     = raw_setsockopt,
408 };
409
410 static const struct proto_ops ieee802154_raw_ops = {
411         .family            = PF_IEEE802154,
412         .owner             = THIS_MODULE,
413         .release           = ieee802154_sock_release,
414         .bind              = ieee802154_sock_bind,
415         .connect           = ieee802154_sock_connect,
416         .socketpair        = sock_no_socketpair,
417         .accept            = sock_no_accept,
418         .getname           = sock_no_getname,
419         .poll              = datagram_poll,
420         .ioctl             = ieee802154_sock_ioctl,
421         .gettstamp         = sock_gettstamp,
422         .listen            = sock_no_listen,
423         .shutdown          = sock_no_shutdown,
424         .setsockopt        = sock_common_setsockopt,
425         .getsockopt        = sock_common_getsockopt,
426         .sendmsg           = ieee802154_sock_sendmsg,
427         .recvmsg           = sock_common_recvmsg,
428         .mmap              = sock_no_mmap,
429 };
430
431 /* DGRAM Sockets (802.15.4 dataframes) */
432 static HLIST_HEAD(dgram_head);
433 static DEFINE_RWLOCK(dgram_lock);
434
435 struct dgram_sock {
436         struct sock sk;
437
438         struct ieee802154_addr src_addr;
439         struct ieee802154_addr dst_addr;
440
441         unsigned int bound:1;
442         unsigned int connected:1;
443         unsigned int want_ack:1;
444         unsigned int want_lqi:1;
445         unsigned int secen:1;
446         unsigned int secen_override:1;
447         unsigned int seclevel:3;
448         unsigned int seclevel_override:1;
449 };
450
451 static inline struct dgram_sock *dgram_sk(const struct sock *sk)
452 {
453         return container_of(sk, struct dgram_sock, sk);
454 }
455
456 static int dgram_hash(struct sock *sk)
457 {
458         write_lock_bh(&dgram_lock);
459         sk_add_node(sk, &dgram_head);
460         write_unlock_bh(&dgram_lock);
461         sock_prot_inuse_add(sock_net(sk), sk->sk_prot, 1);
462
463         return 0;
464 }
465
466 static void dgram_unhash(struct sock *sk)
467 {
468         write_lock_bh(&dgram_lock);
469         if (sk_del_node_init(sk))
470                 sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
471         write_unlock_bh(&dgram_lock);
472 }
473
474 static int dgram_init(struct sock *sk)
475 {
476         struct dgram_sock *ro = dgram_sk(sk);
477
478         ro->want_ack = 1;
479         ro->want_lqi = 0;
480         return 0;
481 }
482
483 static void dgram_close(struct sock *sk, long timeout)
484 {
485         sk_common_release(sk);
486 }
487
488 static int dgram_bind(struct sock *sk, struct sockaddr *uaddr, int len)
489 {
490         struct sockaddr_ieee802154 *addr = (struct sockaddr_ieee802154 *)uaddr;
491         struct ieee802154_addr haddr;
492         struct dgram_sock *ro = dgram_sk(sk);
493         int err = -EINVAL;
494         struct net_device *dev;
495
496         lock_sock(sk);
497
498         ro->bound = 0;
499
500         err = ieee802154_sockaddr_check_size(addr, len);
501         if (err < 0)
502                 goto out;
503
504         if (addr->family != AF_IEEE802154) {
505                 err = -EINVAL;
506                 goto out;
507         }
508
509         ieee802154_addr_from_sa(&haddr, &addr->addr);
510         dev = ieee802154_get_dev(sock_net(sk), &haddr);
511         if (!dev) {
512                 err = -ENODEV;
513                 goto out;
514         }
515
516         if (dev->type != ARPHRD_IEEE802154) {
517                 err = -ENODEV;
518                 goto out_put;
519         }
520
521         ro->src_addr = haddr;
522
523         ro->bound = 1;
524         err = 0;
525 out_put:
526         dev_put(dev);
527 out:
528         release_sock(sk);
529
530         return err;
531 }
532
533 static int dgram_ioctl(struct sock *sk, int cmd, int *karg)
534 {
535         switch (cmd) {
536         case SIOCOUTQ:
537         {
538                 *karg = sk_wmem_alloc_get(sk);
539
540                 return 0;
541         }
542
543         case SIOCINQ:
544         {
545                 struct sk_buff *skb;
546
547                 *karg = 0;
548                 spin_lock_bh(&sk->sk_receive_queue.lock);
549                 skb = skb_peek(&sk->sk_receive_queue);
550                 if (skb) {
551                         /* We will only return the amount
552                          * of this packet since that is all
553                          * that will be read.
554                          */
555                         *karg = skb->len - ieee802154_hdr_length(skb);
556                 }
557                 spin_unlock_bh(&sk->sk_receive_queue.lock);
558                 return 0;
559         }
560         }
561
562         return -ENOIOCTLCMD;
563 }
564
565 /* FIXME: autobind */
566 static int dgram_connect(struct sock *sk, struct sockaddr *uaddr,
567                          int len)
568 {
569         struct sockaddr_ieee802154 *addr = (struct sockaddr_ieee802154 *)uaddr;
570         struct dgram_sock *ro = dgram_sk(sk);
571         int err = 0;
572
573         err = ieee802154_sockaddr_check_size(addr, len);
574         if (err < 0)
575                 return err;
576
577         if (addr->family != AF_IEEE802154)
578                 return -EINVAL;
579
580         lock_sock(sk);
581
582         if (!ro->bound) {
583                 err = -ENETUNREACH;
584                 goto out;
585         }
586
587         ieee802154_addr_from_sa(&ro->dst_addr, &addr->addr);
588         ro->connected = 1;
589
590 out:
591         release_sock(sk);
592         return err;
593 }
594
595 static int dgram_disconnect(struct sock *sk, int flags)
596 {
597         struct dgram_sock *ro = dgram_sk(sk);
598
599         lock_sock(sk);
600         ro->connected = 0;
601         release_sock(sk);
602
603         return 0;
604 }
605
606 static int dgram_sendmsg(struct sock *sk, struct msghdr *msg, size_t size)
607 {
608         struct net_device *dev;
609         unsigned int mtu;
610         struct sk_buff *skb;
611         struct ieee802154_mac_cb *cb;
612         struct dgram_sock *ro = dgram_sk(sk);
613         struct ieee802154_addr dst_addr;
614         DECLARE_SOCKADDR(struct sockaddr_ieee802154*, daddr, msg->msg_name);
615         int hlen, tlen;
616         int err;
617
618         if (msg->msg_flags & MSG_OOB) {
619                 pr_debug("msg->msg_flags = 0x%x\n", msg->msg_flags);
620                 return -EOPNOTSUPP;
621         }
622
623         if (msg->msg_name) {
624                 if (ro->connected)
625                         return -EISCONN;
626                 if (msg->msg_namelen < IEEE802154_MIN_NAMELEN)
627                         return -EINVAL;
628                 err = ieee802154_sockaddr_check_size(daddr, msg->msg_namelen);
629                 if (err < 0)
630                         return err;
631                 ieee802154_addr_from_sa(&dst_addr, &daddr->addr);
632         } else {
633                 if (!ro->connected)
634                         return -EDESTADDRREQ;
635                 dst_addr = ro->dst_addr;
636         }
637
638         if (!ro->bound)
639                 dev = dev_getfirstbyhwtype(sock_net(sk), ARPHRD_IEEE802154);
640         else
641                 dev = ieee802154_get_dev(sock_net(sk), &ro->src_addr);
642
643         if (!dev) {
644                 pr_debug("no dev\n");
645                 err = -ENXIO;
646                 goto out;
647         }
648         mtu = IEEE802154_MTU;
649         pr_debug("name = %s, mtu = %u\n", dev->name, mtu);
650
651         if (size > mtu) {
652                 pr_debug("size = %zu, mtu = %u\n", size, mtu);
653                 err = -EMSGSIZE;
654                 goto out_dev;
655         }
656
657         hlen = LL_RESERVED_SPACE(dev);
658         tlen = dev->needed_tailroom;
659         skb = sock_alloc_send_skb(sk, hlen + tlen + size,
660                                   msg->msg_flags & MSG_DONTWAIT,
661                                   &err);
662         if (!skb)
663                 goto out_dev;
664
665         skb_reserve(skb, hlen);
666
667         skb_reset_network_header(skb);
668
669         cb = mac_cb_init(skb);
670         cb->type = IEEE802154_FC_TYPE_DATA;
671         cb->ackreq = ro->want_ack;
672         cb->secen = ro->secen;
673         cb->secen_override = ro->secen_override;
674         cb->seclevel = ro->seclevel;
675         cb->seclevel_override = ro->seclevel_override;
676
677         err = wpan_dev_hard_header(skb, dev, &dst_addr,
678                                    ro->bound ? &ro->src_addr : NULL, size);
679         if (err < 0)
680                 goto out_skb;
681
682         err = memcpy_from_msg(skb_put(skb, size), msg, size);
683         if (err < 0)
684                 goto out_skb;
685
686         skb->dev = dev;
687         skb->protocol = htons(ETH_P_IEEE802154);
688
689         err = dev_queue_xmit(skb);
690         if (err > 0)
691                 err = net_xmit_errno(err);
692
693         dev_put(dev);
694
695         return err ?: size;
696
697 out_skb:
698         kfree_skb(skb);
699 out_dev:
700         dev_put(dev);
701 out:
702         return err;
703 }
704
705 static int dgram_recvmsg(struct sock *sk, struct msghdr *msg, size_t len,
706                          int flags, int *addr_len)
707 {
708         size_t copied = 0;
709         int err = -EOPNOTSUPP;
710         struct sk_buff *skb;
711         struct dgram_sock *ro = dgram_sk(sk);
712         DECLARE_SOCKADDR(struct sockaddr_ieee802154 *, saddr, msg->msg_name);
713
714         skb = skb_recv_datagram(sk, flags, &err);
715         if (!skb)
716                 goto out;
717
718         copied = skb->len;
719         if (len < copied) {
720                 msg->msg_flags |= MSG_TRUNC;
721                 copied = len;
722         }
723
724         /* FIXME: skip headers if necessary ?! */
725         err = skb_copy_datagram_msg(skb, 0, msg, copied);
726         if (err)
727                 goto done;
728
729         sock_recv_cmsgs(msg, sk, skb);
730
731         if (saddr) {
732                 /* Clear the implicit padding in struct sockaddr_ieee802154
733                  * (16 bits between 'family' and 'addr') and in struct
734                  * ieee802154_addr_sa (16 bits at the end of the structure).
735                  */
736                 memset(saddr, 0, sizeof(*saddr));
737
738                 saddr->family = AF_IEEE802154;
739                 ieee802154_addr_to_sa(&saddr->addr, &mac_cb(skb)->source);
740                 *addr_len = sizeof(*saddr);
741         }
742
743         if (ro->want_lqi) {
744                 err = put_cmsg(msg, SOL_IEEE802154, WPAN_WANTLQI,
745                                sizeof(uint8_t), &(mac_cb(skb)->lqi));
746                 if (err)
747                         goto done;
748         }
749
750         if (flags & MSG_TRUNC)
751                 copied = skb->len;
752 done:
753         skb_free_datagram(sk, skb);
754 out:
755         if (err)
756                 return err;
757         return copied;
758 }
759
760 static int dgram_rcv_skb(struct sock *sk, struct sk_buff *skb)
761 {
762         skb = skb_share_check(skb, GFP_ATOMIC);
763         if (!skb)
764                 return NET_RX_DROP;
765
766         if (sock_queue_rcv_skb(sk, skb) < 0) {
767                 kfree_skb(skb);
768                 return NET_RX_DROP;
769         }
770
771         return NET_RX_SUCCESS;
772 }
773
774 static inline bool
775 ieee802154_match_sock(__le64 hw_addr, __le16 pan_id, __le16 short_addr,
776                       struct dgram_sock *ro)
777 {
778         if (!ro->bound)
779                 return true;
780
781         if (ro->src_addr.mode == IEEE802154_ADDR_LONG &&
782             hw_addr == ro->src_addr.extended_addr)
783                 return true;
784
785         if (ro->src_addr.mode == IEEE802154_ADDR_SHORT &&
786             pan_id == ro->src_addr.pan_id &&
787             short_addr == ro->src_addr.short_addr)
788                 return true;
789
790         return false;
791 }
792
793 static int ieee802154_dgram_deliver(struct net_device *dev, struct sk_buff *skb)
794 {
795         struct sock *sk, *prev = NULL;
796         int ret = NET_RX_SUCCESS;
797         __le16 pan_id, short_addr;
798         __le64 hw_addr;
799
800         /* Data frame processing */
801         BUG_ON(dev->type != ARPHRD_IEEE802154);
802
803         pan_id = dev->ieee802154_ptr->pan_id;
804         short_addr = dev->ieee802154_ptr->short_addr;
805         hw_addr = dev->ieee802154_ptr->extended_addr;
806
807         read_lock(&dgram_lock);
808         sk_for_each(sk, &dgram_head) {
809                 if (ieee802154_match_sock(hw_addr, pan_id, short_addr,
810                                           dgram_sk(sk))) {
811                         if (prev) {
812                                 struct sk_buff *clone;
813
814                                 clone = skb_clone(skb, GFP_ATOMIC);
815                                 if (clone)
816                                         dgram_rcv_skb(prev, clone);
817                         }
818
819                         prev = sk;
820                 }
821         }
822
823         if (prev) {
824                 dgram_rcv_skb(prev, skb);
825         } else {
826                 kfree_skb(skb);
827                 ret = NET_RX_DROP;
828         }
829         read_unlock(&dgram_lock);
830
831         return ret;
832 }
833
834 static int dgram_getsockopt(struct sock *sk, int level, int optname,
835                             char __user *optval, int __user *optlen)
836 {
837         struct dgram_sock *ro = dgram_sk(sk);
838
839         int val, len;
840
841         if (level != SOL_IEEE802154)
842                 return -EOPNOTSUPP;
843
844         if (get_user(len, optlen))
845                 return -EFAULT;
846
847         len = min_t(unsigned int, len, sizeof(int));
848
849         switch (optname) {
850         case WPAN_WANTACK:
851                 val = ro->want_ack;
852                 break;
853         case WPAN_WANTLQI:
854                 val = ro->want_lqi;
855                 break;
856         case WPAN_SECURITY:
857                 if (!ro->secen_override)
858                         val = WPAN_SECURITY_DEFAULT;
859                 else if (ro->secen)
860                         val = WPAN_SECURITY_ON;
861                 else
862                         val = WPAN_SECURITY_OFF;
863                 break;
864         case WPAN_SECURITY_LEVEL:
865                 if (!ro->seclevel_override)
866                         val = WPAN_SECURITY_LEVEL_DEFAULT;
867                 else
868                         val = ro->seclevel;
869                 break;
870         default:
871                 return -ENOPROTOOPT;
872         }
873
874         if (put_user(len, optlen))
875                 return -EFAULT;
876         if (copy_to_user(optval, &val, len))
877                 return -EFAULT;
878         return 0;
879 }
880
881 static int dgram_setsockopt(struct sock *sk, int level, int optname,
882                             sockptr_t optval, unsigned int optlen)
883 {
884         struct dgram_sock *ro = dgram_sk(sk);
885         struct net *net = sock_net(sk);
886         int val;
887         int err = 0;
888
889         if (optlen < sizeof(int))
890                 return -EINVAL;
891
892         if (copy_from_sockptr(&val, optval, sizeof(int)))
893                 return -EFAULT;
894
895         lock_sock(sk);
896
897         switch (optname) {
898         case WPAN_WANTACK:
899                 ro->want_ack = !!val;
900                 break;
901         case WPAN_WANTLQI:
902                 ro->want_lqi = !!val;
903                 break;
904         case WPAN_SECURITY:
905                 if (!ns_capable(net->user_ns, CAP_NET_ADMIN) &&
906                     !ns_capable(net->user_ns, CAP_NET_RAW)) {
907                         err = -EPERM;
908                         break;
909                 }
910
911                 switch (val) {
912                 case WPAN_SECURITY_DEFAULT:
913                         ro->secen_override = 0;
914                         break;
915                 case WPAN_SECURITY_ON:
916                         ro->secen_override = 1;
917                         ro->secen = 1;
918                         break;
919                 case WPAN_SECURITY_OFF:
920                         ro->secen_override = 1;
921                         ro->secen = 0;
922                         break;
923                 default:
924                         err = -EINVAL;
925                         break;
926                 }
927                 break;
928         case WPAN_SECURITY_LEVEL:
929                 if (!ns_capable(net->user_ns, CAP_NET_ADMIN) &&
930                     !ns_capable(net->user_ns, CAP_NET_RAW)) {
931                         err = -EPERM;
932                         break;
933                 }
934
935                 if (val < WPAN_SECURITY_LEVEL_DEFAULT ||
936                     val > IEEE802154_SCF_SECLEVEL_ENC_MIC128) {
937                         err = -EINVAL;
938                 } else if (val == WPAN_SECURITY_LEVEL_DEFAULT) {
939                         ro->seclevel_override = 0;
940                 } else {
941                         ro->seclevel_override = 1;
942                         ro->seclevel = val;
943                 }
944                 break;
945         default:
946                 err = -ENOPROTOOPT;
947                 break;
948         }
949
950         release_sock(sk);
951         return err;
952 }
953
954 static struct proto ieee802154_dgram_prot = {
955         .name           = "IEEE-802.15.4-MAC",
956         .owner          = THIS_MODULE,
957         .obj_size       = sizeof(struct dgram_sock),
958         .init           = dgram_init,
959         .close          = dgram_close,
960         .bind           = dgram_bind,
961         .sendmsg        = dgram_sendmsg,
962         .recvmsg        = dgram_recvmsg,
963         .hash           = dgram_hash,
964         .unhash         = dgram_unhash,
965         .connect        = dgram_connect,
966         .disconnect     = dgram_disconnect,
967         .ioctl          = dgram_ioctl,
968         .getsockopt     = dgram_getsockopt,
969         .setsockopt     = dgram_setsockopt,
970 };
971
972 static const struct proto_ops ieee802154_dgram_ops = {
973         .family            = PF_IEEE802154,
974         .owner             = THIS_MODULE,
975         .release           = ieee802154_sock_release,
976         .bind              = ieee802154_sock_bind,
977         .connect           = ieee802154_sock_connect,
978         .socketpair        = sock_no_socketpair,
979         .accept            = sock_no_accept,
980         .getname           = sock_no_getname,
981         .poll              = datagram_poll,
982         .ioctl             = ieee802154_sock_ioctl,
983         .gettstamp         = sock_gettstamp,
984         .listen            = sock_no_listen,
985         .shutdown          = sock_no_shutdown,
986         .setsockopt        = sock_common_setsockopt,
987         .getsockopt        = sock_common_getsockopt,
988         .sendmsg           = ieee802154_sock_sendmsg,
989         .recvmsg           = sock_common_recvmsg,
990         .mmap              = sock_no_mmap,
991 };
992
993 static void ieee802154_sock_destruct(struct sock *sk)
994 {
995         skb_queue_purge(&sk->sk_receive_queue);
996 }
997
998 /* Create a socket. Initialise the socket, blank the addresses
999  * set the state.
1000  */
1001 static int ieee802154_create(struct net *net, struct socket *sock,
1002                              int protocol, int kern)
1003 {
1004         struct sock *sk;
1005         int rc;
1006         struct proto *proto;
1007         const struct proto_ops *ops;
1008
1009         if (!net_eq(net, &init_net))
1010                 return -EAFNOSUPPORT;
1011
1012         switch (sock->type) {
1013         case SOCK_RAW:
1014                 rc = -EPERM;
1015                 if (!capable(CAP_NET_RAW))
1016                         goto out;
1017                 proto = &ieee802154_raw_prot;
1018                 ops = &ieee802154_raw_ops;
1019                 break;
1020         case SOCK_DGRAM:
1021                 proto = &ieee802154_dgram_prot;
1022                 ops = &ieee802154_dgram_ops;
1023                 break;
1024         default:
1025                 rc = -ESOCKTNOSUPPORT;
1026                 goto out;
1027         }
1028
1029         rc = -ENOMEM;
1030         sk = sk_alloc(net, PF_IEEE802154, GFP_KERNEL, proto, kern);
1031         if (!sk)
1032                 goto out;
1033         rc = 0;
1034
1035         sock->ops = ops;
1036
1037         sock_init_data(sock, sk);
1038         sk->sk_destruct = ieee802154_sock_destruct;
1039         sk->sk_family = PF_IEEE802154;
1040
1041         /* Checksums on by default */
1042         sock_set_flag(sk, SOCK_ZAPPED);
1043
1044         if (sk->sk_prot->hash) {
1045                 rc = sk->sk_prot->hash(sk);
1046                 if (rc) {
1047                         sk_common_release(sk);
1048                         goto out;
1049                 }
1050         }
1051
1052         if (sk->sk_prot->init) {
1053                 rc = sk->sk_prot->init(sk);
1054                 if (rc)
1055                         sk_common_release(sk);
1056         }
1057 out:
1058         return rc;
1059 }
1060
1061 static const struct net_proto_family ieee802154_family_ops = {
1062         .family         = PF_IEEE802154,
1063         .create         = ieee802154_create,
1064         .owner          = THIS_MODULE,
1065 };
1066
1067 static int ieee802154_rcv(struct sk_buff *skb, struct net_device *dev,
1068                           struct packet_type *pt, struct net_device *orig_dev)
1069 {
1070         if (!netif_running(dev))
1071                 goto drop;
1072         pr_debug("got frame, type %d, dev %p\n", dev->type, dev);
1073 #ifdef DEBUG
1074         print_hex_dump_bytes("ieee802154_rcv ",
1075                              DUMP_PREFIX_NONE, skb->data, skb->len);
1076 #endif
1077
1078         if (!net_eq(dev_net(dev), &init_net))
1079                 goto drop;
1080
1081         ieee802154_raw_deliver(dev, skb);
1082
1083         if (dev->type != ARPHRD_IEEE802154)
1084                 goto drop;
1085
1086         if (skb->pkt_type != PACKET_OTHERHOST)
1087                 return ieee802154_dgram_deliver(dev, skb);
1088
1089 drop:
1090         kfree_skb(skb);
1091         return NET_RX_DROP;
1092 }
1093
1094 static struct packet_type ieee802154_packet_type = {
1095         .type = htons(ETH_P_IEEE802154),
1096         .func = ieee802154_rcv,
1097 };
1098
1099 static int __init af_ieee802154_init(void)
1100 {
1101         int rc;
1102
1103         rc = proto_register(&ieee802154_raw_prot, 1);
1104         if (rc)
1105                 goto out;
1106
1107         rc = proto_register(&ieee802154_dgram_prot, 1);
1108         if (rc)
1109                 goto err_dgram;
1110
1111         /* Tell SOCKET that we are alive */
1112         rc = sock_register(&ieee802154_family_ops);
1113         if (rc)
1114                 goto err_sock;
1115         dev_add_pack(&ieee802154_packet_type);
1116
1117         rc = 0;
1118         goto out;
1119
1120 err_sock:
1121         proto_unregister(&ieee802154_dgram_prot);
1122 err_dgram:
1123         proto_unregister(&ieee802154_raw_prot);
1124 out:
1125         return rc;
1126 }
1127
1128 static void __exit af_ieee802154_remove(void)
1129 {
1130         dev_remove_pack(&ieee802154_packet_type);
1131         sock_unregister(PF_IEEE802154);
1132         proto_unregister(&ieee802154_dgram_prot);
1133         proto_unregister(&ieee802154_raw_prot);
1134 }
1135
1136 module_init(af_ieee802154_init);
1137 module_exit(af_ieee802154_remove);
1138
1139 MODULE_LICENSE("GPL");
1140 MODULE_DESCRIPTION("IEEE 802.15.4 socket interface");
1141 MODULE_ALIAS_NETPROTO(PF_IEEE802154);