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