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                 /* Clear the implicit padding in struct sockaddr_ieee802154
 743                  * (16 bits between 'family' and 'addr') and in struct
 744                  * ieee802154_addr_sa (16 bits at the end of the structure).
 745                  */
 746                 memset(saddr, 0, sizeof(*saddr));
 747
 748                 saddr->family = AF_IEEE802154;
 749                 ieee802154_addr_to_sa(&saddr->addr, &mac_cb(skb)->source);
 750                 *addr_len = sizeof(*saddr);
 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
 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
 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
 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 = ieee802154_mlme_ops(dev)->get_pan_id(dev);
 807         short_addr = ieee802154_mlme_ops(dev)->get_short_addr(dev);
 808         hw_addr = ieee802154_devaddr_from_raw(dev->dev_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
 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_SECURITY:
 857                 if (!ro->secen_override)
 858                         val = WPAN_SECURITY_DEFAULT;
 859                 else if (ro->secen)
 860                         val = WPAN_SECURITY_ON;
 861                 else
 862                         val = WPAN_SECURITY_OFF;
 863                 break;
 864         case WPAN_SECURITY_LEVEL:
 865                 if (!ro->seclevel_override)
 866                         val = WPAN_SECURITY_LEVEL_DEFAULT;
 867                 else
 868                         val = ro->seclevel;
 869                 break;
 870         default:
 871                 return -ENOPROTOOPT;
 872         }
 873
 874         if (put_user(len, optlen))
 875                 return -EFAULT;
 876         if (copy_to_user(optval, &val, len))
 877                 return -EFAULT;
 878         return 0;
 879 }
 880
 881 static int dgram_setsockopt(struct sock *sk, int level, int optname,
 882                             char __user *optval, unsigned int optlen)
 883 {
 884         struct dgram_sock *ro = dgram_sk(sk);
 885         struct net *net = sock_net(sk);
 886         int val;
 887         int err = 0;
 888
 889         if (optlen < sizeof(int))
 890                 return -EINVAL;
 891
 892         if (get_user(val, (int __user *)optval))
 893                 return -EFAULT;
 894
 895         lock_sock(sk);
 896
 897         switch (optname) {
 898         case WPAN_WANTACK:
 899                 ro->want_ack = !!val;
 900                 break;
 901         case WPAN_SECURITY:
 902                 if (!ns_capable(net->user_ns, CAP_NET_ADMIN) &&
 903                     !ns_capable(net->user_ns, CAP_NET_RAW)) {
 904                         err = -EPERM;
 905                         break;
 906                 }
 907
 908                 switch (val) {
 909                 case WPAN_SECURITY_DEFAULT:
 910                         ro->secen_override = 0;
 911                         break;
 912                 case WPAN_SECURITY_ON:
 913                         ro->secen_override = 1;
 914                         ro->secen = 1;
 915                         break;
 916                 case WPAN_SECURITY_OFF:
 917                         ro->secen_override = 1;
 918                         ro->secen = 0;
 919                         break;
 920                 default:
 921                         err = -EINVAL;
 922                         break;
 923                 }
 924                 break;
 925         case WPAN_SECURITY_LEVEL:
 926                 if (!ns_capable(net->user_ns, CAP_NET_ADMIN) &&
 927                     !ns_capable(net->user_ns, CAP_NET_RAW)) {
 928                         err = -EPERM;
 929                         break;
 930                 }
 931
 932                 if (val < WPAN_SECURITY_LEVEL_DEFAULT ||
 933                     val > IEEE802154_SCF_SECLEVEL_ENC_MIC128) {
 934                         err = -EINVAL;
 935                 } else if (val == WPAN_SECURITY_LEVEL_DEFAULT) {
 936                         ro->seclevel_override = 0;
 937                 } else {
 938                         ro->seclevel_override = 1;
 939                         ro->seclevel = val;
 940                 }
 941                 break;
 942         default:
 943                 err = -ENOPROTOOPT;
 944                 break;
 945         }
 946
 947         release_sock(sk);
 948         return err;
 949 }
 950
 951 static struct proto ieee802154_dgram_prot = {
 952         .name           = "IEEE-802.15.4-MAC",
 953         .owner          = THIS_MODULE,
 954         .obj_size       = sizeof(struct dgram_sock),
 955         .init           = dgram_init,
 956         .close          = dgram_close,
 957         .bind           = dgram_bind,
 958         .sendmsg        = dgram_sendmsg,
 959         .recvmsg        = dgram_recvmsg,
 960         .hash           = dgram_hash,
 961         .unhash         = dgram_unhash,
 962         .connect        = dgram_connect,
 963         .disconnect     = dgram_disconnect,
 964         .ioctl          = dgram_ioctl,
 965         .getsockopt     = dgram_getsockopt,
 966         .setsockopt     = dgram_setsockopt,
 967 };
 968
 969 static const struct proto_ops ieee802154_dgram_ops = {
 970         .family            = PF_IEEE802154,
 971         .owner             = THIS_MODULE,
 972         .release           = ieee802154_sock_release,
 973         .bind              = ieee802154_sock_bind,
 974         .connect           = ieee802154_sock_connect,
 975         .socketpair        = sock_no_socketpair,
 976         .accept            = sock_no_accept,
 977         .getname           = sock_no_getname,
 978         .poll              = datagram_poll,
 979         .ioctl             = ieee802154_sock_ioctl,
 980         .listen            = sock_no_listen,
 981         .shutdown          = sock_no_shutdown,
 982         .setsockopt        = sock_common_setsockopt,
 983         .getsockopt        = sock_common_getsockopt,
 984         .sendmsg           = ieee802154_sock_sendmsg,
 985         .recvmsg           = sock_common_recvmsg,
 986         .mmap              = sock_no_mmap,
 987         .sendpage          = sock_no_sendpage,
 988 #ifdef CONFIG_COMPAT
 989         .compat_setsockopt = compat_sock_common_setsockopt,
 990         .compat_getsockopt = compat_sock_common_getsockopt,
 991 #endif
 992 };
 993
 994 /* Create a socket. Initialise the socket, blank the addresses
 995  * set the state.
 996  */
 997 static int ieee802154_create(struct net *net, struct socket *sock,
 998                              int protocol, int kern)
 999 {
1000         struct sock *sk;
1001         int rc;
1002         struct proto *proto;
1003         const struct proto_ops *ops;
1004
1005         if (!net_eq(net, &init_net))
1006                 return -EAFNOSUPPORT;
1007
1008         switch (sock->type) {
1009         case SOCK_RAW:
1010                 proto = &ieee802154_raw_prot;
1011                 ops = &ieee802154_raw_ops;
1012                 break;
1013         case SOCK_DGRAM:
1014                 proto = &ieee802154_dgram_prot;
1015                 ops = &ieee802154_dgram_ops;
1016                 break;
1017         default:
1018                 rc = -ESOCKTNOSUPPORT;
1019                 goto out;
1020         }
1021
1022         rc = -ENOMEM;
1023         sk = sk_alloc(net, PF_IEEE802154, GFP_KERNEL, proto);
1024         if (!sk)
1025                 goto out;
1026         rc = 0;
1027
1028         sock->ops = ops;
1029
1030         sock_init_data(sock, sk);
1031         /* FIXME: sk->sk_destruct */
1032         sk->sk_family = PF_IEEE802154;
1033
1034         /* Checksums on by default */
1035         sock_set_flag(sk, SOCK_ZAPPED);
1036
1037         if (sk->sk_prot->hash)
1038                 sk->sk_prot->hash(sk);
1039
1040         if (sk->sk_prot->init) {
1041                 rc = sk->sk_prot->init(sk);
1042                 if (rc)
1043                         sk_common_release(sk);
1044         }
1045 out:
1046         return rc;
1047 }
1048
1049 static const struct net_proto_family ieee802154_family_ops = {
1050         .family         = PF_IEEE802154,
1051         .create         = ieee802154_create,
1052         .owner          = THIS_MODULE,
1053 };
1054
1055 static int ieee802154_rcv(struct sk_buff *skb, struct net_device *dev,
1056                           struct packet_type *pt, struct net_device *orig_dev)
1057 {
1058         if (!netif_running(dev))
1059                 goto drop;
1060         pr_debug("got frame, type %d, dev %p\n", dev->type, dev);
1061 #ifdef DEBUG
1062         print_hex_dump_bytes("ieee802154_rcv ",
1063                              DUMP_PREFIX_NONE, skb->data, skb->len);
1064 #endif
1065
1066         if (!net_eq(dev_net(dev), &init_net))
1067                 goto drop;
1068
1069         ieee802154_raw_deliver(dev, skb);
1070
1071         if (dev->type != ARPHRD_IEEE802154)
1072                 goto drop;
1073
1074         if (skb->pkt_type != PACKET_OTHERHOST)
1075                 return ieee802154_dgram_deliver(dev, skb);
1076
1077 drop:
1078         kfree_skb(skb);
1079         return NET_RX_DROP;
1080 }
1081
1082 static struct packet_type ieee802154_packet_type = {
1083         .type = htons(ETH_P_IEEE802154),
1084         .func = ieee802154_rcv,
1085 };
1086
1087 static int __init af_ieee802154_init(void)
1088 {
1089         int rc = -EINVAL;
1090
1091         rc = proto_register(&ieee802154_raw_prot, 1);
1092         if (rc)
1093                 goto out;
1094
1095         rc = proto_register(&ieee802154_dgram_prot, 1);
1096         if (rc)
1097                 goto err_dgram;
1098
1099         /* Tell SOCKET that we are alive */
1100         rc = sock_register(&ieee802154_family_ops);
1101         if (rc)
1102                 goto err_sock;
1103         dev_add_pack(&ieee802154_packet_type);
1104
1105         rc = 0;
1106         goto out;
1107
1108 err_sock:
1109         proto_unregister(&ieee802154_dgram_prot);
1110 err_dgram:
1111         proto_unregister(&ieee802154_raw_prot);
1112 out:
1113         return rc;
1114 }
1115
1116 static void __exit af_ieee802154_remove(void)
1117 {
1118         dev_remove_pack(&ieee802154_packet_type);
1119         sock_unregister(PF_IEEE802154);
1120         proto_unregister(&ieee802154_dgram_prot);
1121         proto_unregister(&ieee802154_raw_prot);
1122 }
1123
1124 module_init(af_ieee802154_init);
1125 module_exit(af_ieee802154_remove);
1126
1127 MODULE_LICENSE("GPL");
1128 MODULE_ALIAS_NETPROTO(PF_IEEE802154);