ieee802154/socket.c

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