net/mac802154/wpan.c

   1 /*
   2  * Copyright 2007-2012 Siemens AG
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License version 2
   6  * as published by the Free Software Foundation.
   7  *
   8  * This program is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  16  *
  17  * Written by:
  18  * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
  19  * Sergey Lapin <slapin@ossfans.org>
  20  * Maxim Gorbachyov <maxim.gorbachev@siemens.com>
  21  * Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
  22  */
  23
  24 #include <linux/netdevice.h>
  25 #include <linux/module.h>
  26 #include <linux/if_arp.h>
  27
  28 #include <net/rtnetlink.h>
  29 #include <linux/nl802154.h>
  30 #include <net/af_ieee802154.h>
  31 #include <net/mac802154.h>
  32 #include <net/ieee802154_netdev.h>
  33 #include <net/ieee802154.h>
  34 #include <net/wpan-phy.h>
  35
  36 #include "mac802154.h"
  37
  38 static inline int mac802154_fetch_skb_u8(struct sk_buff *skb, u8 *val)
  39 {
  40         if (unlikely(!pskb_may_pull(skb, 1)))
  41                 return -EINVAL;
  42
  43         *val = skb->data[0];
  44         skb_pull(skb, 1);
  45
  46         return 0;
  47 }
  48
  49 static inline int mac802154_fetch_skb_u16(struct sk_buff *skb, u16 *val)
  50 {
  51         if (unlikely(!pskb_may_pull(skb, 2)))
  52                 return -EINVAL;
  53
  54         *val = skb->data[0] | (skb->data[1] << 8);
  55         skb_pull(skb, 2);
  56
  57         return 0;
  58 }
  59
  60 static inline void mac802154_haddr_copy_swap(u8 *dest, const u8 *src)
  61 {
  62         int i;
  63         for (i = 0; i < IEEE802154_ADDR_LEN; i++)
  64                 dest[IEEE802154_ADDR_LEN - i - 1] = src[i];
  65 }
  66
  67 static int
  68 mac802154_wpan_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
  69 {
  70         struct mac802154_sub_if_data *priv = netdev_priv(dev);
  71         struct sockaddr_ieee802154 *sa =
  72                 (struct sockaddr_ieee802154 *)&ifr->ifr_addr;
  73         int err = -ENOIOCTLCMD;
  74
  75         spin_lock_bh(&priv->mib_lock);
  76
  77         switch (cmd) {
  78         case SIOCGIFADDR:
  79                 if (priv->pan_id == IEEE802154_PANID_BROADCAST ||
  80                     priv->short_addr == IEEE802154_ADDR_BROADCAST) {
  81                         err = -EADDRNOTAVAIL;
  82                         break;
  83                 }
  84
  85                 sa->family = AF_IEEE802154;
  86                 sa->addr.addr_type = IEEE802154_ADDR_SHORT;
  87                 sa->addr.pan_id = priv->pan_id;
  88                 sa->addr.short_addr = priv->short_addr;
  89
  90                 err = 0;
  91                 break;
  92         case SIOCSIFADDR:
  93                 dev_warn(&dev->dev,
  94                          "Using DEBUGing ioctl SIOCSIFADDR isn't recommened!\n");
  95                 if (sa->family != AF_IEEE802154 ||
  96                     sa->addr.addr_type != IEEE802154_ADDR_SHORT ||
  97                     sa->addr.pan_id == IEEE802154_PANID_BROADCAST ||
  98                     sa->addr.short_addr == IEEE802154_ADDR_BROADCAST ||
  99                     sa->addr.short_addr == IEEE802154_ADDR_UNDEF) {
 100                         err = -EINVAL;
 101                         break;
 102                 }
 103
 104                 priv->pan_id = sa->addr.pan_id;
 105                 priv->short_addr = sa->addr.short_addr;
 106
 107                 err = 0;
 108                 break;
 109         }
 110
 111         spin_unlock_bh(&priv->mib_lock);
 112         return err;
 113 }
 114
 115 static int mac802154_wpan_mac_addr(struct net_device *dev, void *p)
 116 {
 117         struct sockaddr *addr = p;
 118
 119         if (netif_running(dev))
 120                 return -EBUSY;
 121
 122         /* FIXME: validate addr */
 123         memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
 124         mac802154_dev_set_ieee_addr(dev);
 125         return 0;
 126 }
 127
 128 static int mac802154_header_create(struct sk_buff *skb,
 129                                    struct net_device *dev,
 130                                    unsigned short type,
 131                                    const void *_daddr,
 132                                    const void *_saddr,
 133                                    unsigned len)
 134 {
 135         const struct ieee802154_addr *saddr = _saddr;
 136         const struct ieee802154_addr *daddr = _daddr;
 137         struct ieee802154_addr dev_addr;
 138         struct mac802154_sub_if_data *priv = netdev_priv(dev);
 139         int pos = 2;
 140         u8 head[MAC802154_FRAME_HARD_HEADER_LEN];
 141         u16 fc;
 142
 143         if (!daddr)
 144                 return -EINVAL;
 145
 146         head[pos++] = mac_cb(skb)->seq; /* DSN/BSN */
 147         fc = mac_cb_type(skb);
 148         if (mac_cb_is_ackreq(skb))
 149                 fc |= IEEE802154_FC_ACK_REQ;
 150
 151         if (!saddr) {
 152                 spin_lock_bh(&priv->mib_lock);
 153
 154                 if (priv->short_addr == IEEE802154_ADDR_BROADCAST ||
 155                     priv->short_addr == IEEE802154_ADDR_UNDEF ||
 156                     priv->pan_id == IEEE802154_PANID_BROADCAST) {
 157                         dev_addr.addr_type = IEEE802154_ADDR_LONG;
 158                         memcpy(dev_addr.hwaddr, dev->dev_addr,
 159                                IEEE802154_ADDR_LEN);
 160                 } else {
 161                         dev_addr.addr_type = IEEE802154_ADDR_SHORT;
 162                         dev_addr.short_addr = priv->short_addr;
 163                 }
 164
 165                 dev_addr.pan_id = priv->pan_id;
 166                 saddr = &dev_addr;
 167
 168                 spin_unlock_bh(&priv->mib_lock);
 169         }
 170
 171         if (daddr->addr_type != IEEE802154_ADDR_NONE) {
 172                 fc |= (daddr->addr_type << IEEE802154_FC_DAMODE_SHIFT);
 173
 174                 head[pos++] = daddr->pan_id & 0xff;
 175                 head[pos++] = daddr->pan_id >> 8;
 176
 177                 if (daddr->addr_type == IEEE802154_ADDR_SHORT) {
 178                         head[pos++] = daddr->short_addr & 0xff;
 179                         head[pos++] = daddr->short_addr >> 8;
 180                 } else {
 181                         mac802154_haddr_copy_swap(head + pos, daddr->hwaddr);
 182                         pos += IEEE802154_ADDR_LEN;
 183                 }
 184         }
 185
 186         if (saddr->addr_type != IEEE802154_ADDR_NONE) {
 187                 fc |= (saddr->addr_type << IEEE802154_FC_SAMODE_SHIFT);
 188
 189                 if ((saddr->pan_id == daddr->pan_id) &&
 190                     (saddr->pan_id != IEEE802154_PANID_BROADCAST)) {
 191                         /* PANID compression/intra PAN */
 192                         fc |= IEEE802154_FC_INTRA_PAN;
 193                 } else {
 194                         head[pos++] = saddr->pan_id & 0xff;
 195                         head[pos++] = saddr->pan_id >> 8;
 196                 }
 197
 198                 if (saddr->addr_type == IEEE802154_ADDR_SHORT) {
 199                         head[pos++] = saddr->short_addr & 0xff;
 200                         head[pos++] = saddr->short_addr >> 8;
 201                 } else {
 202                         mac802154_haddr_copy_swap(head + pos, saddr->hwaddr);
 203                         pos += IEEE802154_ADDR_LEN;
 204                 }
 205         }
 206
 207         head[0] = fc;
 208         head[1] = fc >> 8;
 209
 210         memcpy(skb_push(skb, pos), head, pos);
 211         skb_reset_mac_header(skb);
 212         skb->mac_len = pos;
 213
 214         return pos;
 215 }
 216
 217 static int
 218 mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr)
 219 {
 220         const u8 *hdr = skb_mac_header(skb);
 221         const u8 *tail = skb_tail_pointer(skb);
 222         struct ieee802154_addr *addr = (struct ieee802154_addr *)haddr;
 223         u16 fc;
 224         int da_type;
 225
 226         if (hdr + 3 > tail)
 227                 goto malformed;
 228
 229         fc = hdr[0] | (hdr[1] << 8);
 230
 231         hdr += 3;
 232
 233         da_type = IEEE802154_FC_DAMODE(fc);
 234         addr->addr_type = IEEE802154_FC_SAMODE(fc);
 235
 236         switch (da_type) {
 237         case IEEE802154_ADDR_NONE:
 238                 if (fc & IEEE802154_FC_INTRA_PAN)
 239                         goto malformed;
 240                 break;
 241         case IEEE802154_ADDR_LONG:
 242                 if (fc & IEEE802154_FC_INTRA_PAN) {
 243                         if (hdr + 2 > tail)
 244                                 goto malformed;
 245                         addr->pan_id = hdr[0] | (hdr[1] << 8);
 246                         hdr += 2;
 247                 }
 248
 249                 if (hdr + IEEE802154_ADDR_LEN > tail)
 250                         goto malformed;
 251
 252                 hdr += IEEE802154_ADDR_LEN;
 253                 break;
 254         case IEEE802154_ADDR_SHORT:
 255                 if (fc & IEEE802154_FC_INTRA_PAN) {
 256                         if (hdr + 2 > tail)
 257                                 goto malformed;
 258                         addr->pan_id = hdr[0] | (hdr[1] << 8);
 259                         hdr += 2;
 260                 }
 261
 262                 if (hdr + 2 > tail)
 263                         goto malformed;
 264
 265                 hdr += 2;
 266                 break;
 267         default:
 268                 goto malformed;
 269
 270         }
 271
 272         switch (addr->addr_type) {
 273         case IEEE802154_ADDR_NONE:
 274                 break;
 275         case IEEE802154_ADDR_LONG:
 276                 if (!(fc & IEEE802154_FC_INTRA_PAN)) {
 277                         if (hdr + 2 > tail)
 278                                 goto malformed;
 279                         addr->pan_id = hdr[0] | (hdr[1] << 8);
 280                         hdr += 2;
 281                 }
 282
 283                 if (hdr + IEEE802154_ADDR_LEN > tail)
 284                         goto malformed;
 285
 286                 mac802154_haddr_copy_swap(addr->hwaddr, hdr);
 287                 hdr += IEEE802154_ADDR_LEN;
 288                 break;
 289         case IEEE802154_ADDR_SHORT:
 290                 if (!(fc & IEEE802154_FC_INTRA_PAN)) {
 291                         if (hdr + 2 > tail)
 292                                 goto malformed;
 293                         addr->pan_id = hdr[0] | (hdr[1] << 8);
 294                         hdr += 2;
 295                 }
 296
 297                 if (hdr + 2 > tail)
 298                         goto malformed;
 299
 300                 addr->short_addr = hdr[0] | (hdr[1] << 8);
 301                 hdr += 2;
 302                 break;
 303         default:
 304                 goto malformed;
 305         }
 306
 307         return sizeof(struct ieee802154_addr);
 308
 309 malformed:
 310         pr_debug("malformed packet\n");
 311         return 0;
 312 }
 313
 314 static netdev_tx_t
 315 mac802154_wpan_xmit(struct sk_buff *skb, struct net_device *dev)
 316 {
 317         struct mac802154_sub_if_data *priv;
 318         u8 chan, page;
 319
 320         priv = netdev_priv(dev);
 321
 322         spin_lock_bh(&priv->mib_lock);
 323         chan = priv->chan;
 324         page = priv->page;
 325         spin_unlock_bh(&priv->mib_lock);
 326
 327         if (chan == MAC802154_CHAN_NONE ||
 328             page >= WPAN_NUM_PAGES ||
 329             chan >= WPAN_NUM_CHANNELS) {
 330                 kfree_skb(skb);
 331                 return NETDEV_TX_OK;
 332         }
 333
 334         skb->skb_iif = dev->ifindex;
 335         dev->stats.tx_packets++;
 336         dev->stats.tx_bytes += skb->len;
 337
 338         return mac802154_tx(priv->hw, skb, page, chan);
 339 }
 340
 341 static struct header_ops mac802154_header_ops = {
 342         .create         = mac802154_header_create,
 343         .parse          = mac802154_header_parse,
 344 };
 345
 346 static const struct net_device_ops mac802154_wpan_ops = {
 347         .ndo_open               = mac802154_slave_open,
 348         .ndo_stop               = mac802154_slave_close,
 349         .ndo_start_xmit         = mac802154_wpan_xmit,
 350         .ndo_do_ioctl           = mac802154_wpan_ioctl,
 351         .ndo_set_mac_address    = mac802154_wpan_mac_addr,
 352 };
 353
 354 void mac802154_wpan_setup(struct net_device *dev)
 355 {
 356         struct mac802154_sub_if_data *priv;
 357
 358         dev->addr_len           = IEEE802154_ADDR_LEN;
 359         memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
 360
 361         dev->hard_header_len    = MAC802154_FRAME_HARD_HEADER_LEN;
 362         dev->header_ops         = &mac802154_header_ops;
 363         dev->needed_tailroom    = 2; /* FCS */
 364         dev->mtu                = IEEE802154_MTU;
 365         dev->tx_queue_len       = 300;
 366         dev->type               = ARPHRD_IEEE802154;
 367         dev->flags              = IFF_NOARP | IFF_BROADCAST;
 368         dev->watchdog_timeo     = 0;
 369
 370         dev->destructor         = free_netdev;
 371         dev->netdev_ops         = &mac802154_wpan_ops;
 372         dev->ml_priv            = &mac802154_mlme_wpan;
 373
 374         priv = netdev_priv(dev);
 375         priv->type = IEEE802154_DEV_WPAN;
 376
 377         priv->chan = MAC802154_CHAN_NONE;
 378         priv->page = 0;
 379
 380         spin_lock_init(&priv->mib_lock);
 381
 382         get_random_bytes(&priv->bsn, 1);
 383         get_random_bytes(&priv->dsn, 1);
 384
 385         priv->pan_id = IEEE802154_PANID_BROADCAST;
 386         priv->short_addr = IEEE802154_ADDR_BROADCAST;
 387 }
 388
 389 static int mac802154_process_data(struct net_device *dev, struct sk_buff *skb)
 390 {
 391         return netif_rx_ni(skb);
 392 }
 393
 394 static int
 395 mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb)
 396 {
 397         pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
 398
 399         spin_lock_bh(&sdata->mib_lock);
 400
 401         switch (mac_cb(skb)->da.addr_type) {
 402         case IEEE802154_ADDR_NONE:
 403                 if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE)
 404                         /* FIXME: check if we are PAN coordinator */
 405                         skb->pkt_type = PACKET_OTHERHOST;
 406                 else
 407                         /* ACK comes with both addresses empty */
 408                         skb->pkt_type = PACKET_HOST;
 409                 break;
 410         case IEEE802154_ADDR_LONG:
 411                 if (mac_cb(skb)->da.pan_id != sdata->pan_id &&
 412                     mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
 413                         skb->pkt_type = PACKET_OTHERHOST;
 414                 else if (!memcmp(mac_cb(skb)->da.hwaddr, sdata->dev->dev_addr,
 415                                  IEEE802154_ADDR_LEN))
 416                         skb->pkt_type = PACKET_HOST;
 417                 else
 418                         skb->pkt_type = PACKET_OTHERHOST;
 419                 break;
 420         case IEEE802154_ADDR_SHORT:
 421                 if (mac_cb(skb)->da.pan_id != sdata->pan_id &&
 422                     mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
 423                         skb->pkt_type = PACKET_OTHERHOST;
 424                 else if (mac_cb(skb)->da.short_addr == sdata->short_addr)
 425                         skb->pkt_type = PACKET_HOST;
 426                 else if (mac_cb(skb)->da.short_addr ==
 427                                         IEEE802154_ADDR_BROADCAST)
 428                         skb->pkt_type = PACKET_BROADCAST;
 429                 else
 430                         skb->pkt_type = PACKET_OTHERHOST;
 431                 break;
 432         default:
 433                 break;
 434         }
 435
 436         spin_unlock_bh(&sdata->mib_lock);
 437
 438         skb->dev = sdata->dev;
 439
 440         sdata->dev->stats.rx_packets++;
 441         sdata->dev->stats.rx_bytes += skb->len;
 442
 443         switch (mac_cb_type(skb)) {
 444         case IEEE802154_FC_TYPE_DATA:
 445                 return mac802154_process_data(sdata->dev, skb);
 446         default:
 447                 pr_warn("ieee802154: bad frame received (type = %d)\n",
 448                         mac_cb_type(skb));
 449                 kfree_skb(skb);
 450                 return NET_RX_DROP;
 451         }
 452 }
 453
 454 static int mac802154_parse_frame_start(struct sk_buff *skb)
 455 {
 456         u8 *head = skb->data;
 457         u16 fc;
 458
 459         if (mac802154_fetch_skb_u16(skb, &fc) ||
 460             mac802154_fetch_skb_u8(skb, &(mac_cb(skb)->seq)))
 461                 goto err;
 462
 463         pr_debug("fc: %04x dsn: %02x\n", fc, head[2]);
 464
 465         mac_cb(skb)->flags = IEEE802154_FC_TYPE(fc);
 466         mac_cb(skb)->sa.addr_type = IEEE802154_FC_SAMODE(fc);
 467         mac_cb(skb)->da.addr_type = IEEE802154_FC_DAMODE(fc);
 468
 469         if (fc & IEEE802154_FC_INTRA_PAN)
 470                 mac_cb(skb)->flags |= MAC_CB_FLAG_INTRAPAN;
 471
 472         if (mac_cb(skb)->da.addr_type != IEEE802154_ADDR_NONE) {
 473                 if (mac802154_fetch_skb_u16(skb, &(mac_cb(skb)->da.pan_id)))
 474                         goto err;
 475
 476                 /* source PAN id compression */
 477                 if (mac_cb_is_intrapan(skb))
 478                         mac_cb(skb)->sa.pan_id = mac_cb(skb)->da.pan_id;
 479
 480                 pr_debug("dest PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
 481
 482                 if (mac_cb(skb)->da.addr_type == IEEE802154_ADDR_SHORT) {
 483                         u16 *da = &(mac_cb(skb)->da.short_addr);
 484
 485                         if (mac802154_fetch_skb_u16(skb, da))
 486                                 goto err;
 487
 488                         pr_debug("destination address is short: %04x\n",
 489                                  mac_cb(skb)->da.short_addr);
 490                 } else {
 491                         if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
 492                                 goto err;
 493
 494                         mac802154_haddr_copy_swap(mac_cb(skb)->da.hwaddr,
 495                                                   skb->data);
 496                         skb_pull(skb, IEEE802154_ADDR_LEN);
 497
 498                         pr_debug("destination address is hardware\n");
 499                 }
 500         }
 501
 502         if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE) {
 503                 /* non PAN-compression, fetch source address id */
 504                 if (!(mac_cb_is_intrapan(skb))) {
 505                         u16 *sa_pan = &(mac_cb(skb)->sa.pan_id);
 506
 507                         if (mac802154_fetch_skb_u16(skb, sa_pan))
 508                                 goto err;
 509                 }
 510
 511                 pr_debug("source PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
 512
 513                 if (mac_cb(skb)->sa.addr_type == IEEE802154_ADDR_SHORT) {
 514                         u16 *sa = &(mac_cb(skb)->sa.short_addr);
 515
 516                         if (mac802154_fetch_skb_u16(skb, sa))
 517                                 goto err;
 518
 519                         pr_debug("source address is short: %04x\n",
 520                                  mac_cb(skb)->sa.short_addr);
 521                 } else {
 522                         if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
 523                                 goto err;
 524
 525                         mac802154_haddr_copy_swap(mac_cb(skb)->sa.hwaddr,
 526                                                   skb->data);
 527                         skb_pull(skb, IEEE802154_ADDR_LEN);
 528
 529                         pr_debug("source address is hardware\n");
 530                 }
 531         }
 532
 533         return 0;
 534 err:
 535         return -EINVAL;
 536 }
 537
 538 void mac802154_wpans_rx(struct mac802154_priv *priv, struct sk_buff *skb)
 539 {
 540         int ret;
 541         struct sk_buff *sskb;
 542         struct mac802154_sub_if_data *sdata;
 543
 544         ret = mac802154_parse_frame_start(skb);
 545         if (ret) {
 546                 pr_debug("got invalid frame\n");
 547                 return;
 548         }
 549
 550         rcu_read_lock();
 551         list_for_each_entry_rcu(sdata, &priv->slaves, list) {
 552                 if (sdata->type != IEEE802154_DEV_WPAN)
 553                         continue;
 554
 555                 sskb = skb_clone(skb, GFP_ATOMIC);
 556                 if (sskb)
 557                         mac802154_subif_frame(sdata, sskb);
 558         }
 559         rcu_read_unlock();
 560 }