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