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