linux/net/mac802154/wpan.c
<<
>>
Prefs
   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
  38static 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
  49static 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
  60static 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
  67static int
  68mac802154_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
 115static 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
 128static 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;
 141        u16 fc;
 142
 143        if (!daddr)
 144                return -EINVAL;
 145
 146        head = kzalloc(MAC802154_FRAME_HARD_HEADER_LEN, GFP_KERNEL);
 147        if (head == NULL)
 148                return -ENOMEM;
 149
 150        head[pos++] = mac_cb(skb)->seq; /* DSN/BSN */
 151        fc = mac_cb_type(skb);
 152
 153        if (!saddr) {
 154                spin_lock_bh(&priv->mib_lock);
 155
 156                if (priv->short_addr == IEEE802154_ADDR_BROADCAST ||
 157                    priv->short_addr == IEEE802154_ADDR_UNDEF ||
 158                    priv->pan_id == IEEE802154_PANID_BROADCAST) {
 159                        dev_addr.addr_type = IEEE802154_ADDR_LONG;
 160                        memcpy(dev_addr.hwaddr, dev->dev_addr,
 161                               IEEE802154_ADDR_LEN);
 162                } else {
 163                        dev_addr.addr_type = IEEE802154_ADDR_SHORT;
 164                        dev_addr.short_addr = priv->short_addr;
 165                }
 166
 167                dev_addr.pan_id = priv->pan_id;
 168                saddr = &dev_addr;
 169
 170                spin_unlock_bh(&priv->mib_lock);
 171        }
 172
 173        if (daddr->addr_type != IEEE802154_ADDR_NONE) {
 174                fc |= (daddr->addr_type << IEEE802154_FC_DAMODE_SHIFT);
 175
 176                head[pos++] = daddr->pan_id & 0xff;
 177                head[pos++] = daddr->pan_id >> 8;
 178
 179                if (daddr->addr_type == IEEE802154_ADDR_SHORT) {
 180                        head[pos++] = daddr->short_addr & 0xff;
 181                        head[pos++] = daddr->short_addr >> 8;
 182                } else {
 183                        mac802154_haddr_copy_swap(head + pos, daddr->hwaddr);
 184                        pos += IEEE802154_ADDR_LEN;
 185                }
 186        }
 187
 188        if (saddr->addr_type != IEEE802154_ADDR_NONE) {
 189                fc |= (saddr->addr_type << IEEE802154_FC_SAMODE_SHIFT);
 190
 191                if ((saddr->pan_id == daddr->pan_id) &&
 192                    (saddr->pan_id != IEEE802154_PANID_BROADCAST)) {
 193                        /* PANID compression/intra PAN */
 194                        fc |= IEEE802154_FC_INTRA_PAN;
 195                } else {
 196                        head[pos++] = saddr->pan_id & 0xff;
 197                        head[pos++] = saddr->pan_id >> 8;
 198                }
 199
 200                if (saddr->addr_type == IEEE802154_ADDR_SHORT) {
 201                        head[pos++] = saddr->short_addr & 0xff;
 202                        head[pos++] = saddr->short_addr >> 8;
 203                } else {
 204                        mac802154_haddr_copy_swap(head + pos, saddr->hwaddr);
 205                        pos += IEEE802154_ADDR_LEN;
 206                }
 207        }
 208
 209        head[0] = fc;
 210        head[1] = fc >> 8;
 211
 212        memcpy(skb_push(skb, pos), head, pos);
 213        kfree(head);
 214
 215        return pos;
 216}
 217
 218static int
 219mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr)
 220{
 221        const u8 *hdr = skb_mac_header(skb);
 222        const u8 *tail = skb_tail_pointer(skb);
 223        struct ieee802154_addr *addr = (struct ieee802154_addr *)haddr;
 224        u16 fc;
 225        int da_type;
 226
 227        if (hdr + 3 > tail)
 228                goto malformed;
 229
 230        fc = hdr[0] | (hdr[1] << 8);
 231
 232        hdr += 3;
 233
 234        da_type = IEEE802154_FC_DAMODE(fc);
 235        addr->addr_type = IEEE802154_FC_SAMODE(fc);
 236
 237        switch (da_type) {
 238        case IEEE802154_ADDR_NONE:
 239                if (fc & IEEE802154_FC_INTRA_PAN)
 240                        goto malformed;
 241                break;
 242        case IEEE802154_ADDR_LONG:
 243                if (fc & IEEE802154_FC_INTRA_PAN) {
 244                        if (hdr + 2 > tail)
 245                                goto malformed;
 246                        addr->pan_id = hdr[0] | (hdr[1] << 8);
 247                        hdr += 2;
 248                }
 249
 250                if (hdr + IEEE802154_ADDR_LEN > tail)
 251                        goto malformed;
 252
 253                hdr += IEEE802154_ADDR_LEN;
 254                break;
 255        case IEEE802154_ADDR_SHORT:
 256                if (fc & IEEE802154_FC_INTRA_PAN) {
 257                        if (hdr + 2 > tail)
 258                                goto malformed;
 259                        addr->pan_id = hdr[0] | (hdr[1] << 8);
 260                        hdr += 2;
 261                }
 262
 263                if (hdr + 2 > tail)
 264                        goto malformed;
 265
 266                hdr += 2;
 267                break;
 268        default:
 269                goto malformed;
 270
 271        }
 272
 273        switch (addr->addr_type) {
 274        case IEEE802154_ADDR_NONE:
 275                break;
 276        case IEEE802154_ADDR_LONG:
 277                if (!(fc & IEEE802154_FC_INTRA_PAN)) {
 278                        if (hdr + 2 > tail)
 279                                goto malformed;
 280                        addr->pan_id = hdr[0] | (hdr[1] << 8);
 281                        hdr += 2;
 282                }
 283
 284                if (hdr + IEEE802154_ADDR_LEN > tail)
 285                        goto malformed;
 286
 287                mac802154_haddr_copy_swap(addr->hwaddr, hdr);
 288                hdr += IEEE802154_ADDR_LEN;
 289                break;
 290        case IEEE802154_ADDR_SHORT:
 291                if (!(fc & IEEE802154_FC_INTRA_PAN)) {
 292                        if (hdr + 2 > tail)
 293                                goto malformed;
 294                        addr->pan_id = hdr[0] | (hdr[1] << 8);
 295                        hdr += 2;
 296                }
 297
 298                if (hdr + 2 > tail)
 299                        goto malformed;
 300
 301                addr->short_addr = hdr[0] | (hdr[1] << 8);
 302                hdr += 2;
 303                break;
 304        default:
 305                goto malformed;
 306        }
 307
 308        return sizeof(struct ieee802154_addr);
 309
 310malformed:
 311        pr_debug("malformed packet\n");
 312        return 0;
 313}
 314
 315static netdev_tx_t
 316mac802154_wpan_xmit(struct sk_buff *skb, struct net_device *dev)
 317{
 318        struct mac802154_sub_if_data *priv;
 319        u8 chan, page;
 320
 321        priv = netdev_priv(dev);
 322
 323        spin_lock_bh(&priv->mib_lock);
 324        chan = priv->chan;
 325        page = priv->page;
 326        spin_unlock_bh(&priv->mib_lock);
 327
 328        if (chan == MAC802154_CHAN_NONE ||
 329            page >= WPAN_NUM_PAGES ||
 330            chan >= WPAN_NUM_CHANNELS) {
 331                kfree_skb(skb);
 332                return NETDEV_TX_OK;
 333        }
 334
 335        skb->skb_iif = dev->ifindex;
 336        dev->stats.tx_packets++;
 337        dev->stats.tx_bytes += skb->len;
 338
 339        return mac802154_tx(priv->hw, skb, page, chan);
 340}
 341
 342static struct header_ops mac802154_header_ops = {
 343        .create         = mac802154_header_create,
 344        .parse          = mac802154_header_parse,
 345};
 346
 347static const struct net_device_ops mac802154_wpan_ops = {
 348        .ndo_open               = mac802154_slave_open,
 349        .ndo_stop               = mac802154_slave_close,
 350        .ndo_start_xmit         = mac802154_wpan_xmit,
 351        .ndo_do_ioctl           = mac802154_wpan_ioctl,
 352        .ndo_set_mac_address    = mac802154_wpan_mac_addr,
 353};
 354
 355void mac802154_wpan_setup(struct net_device *dev)
 356{
 357        struct mac802154_sub_if_data *priv;
 358
 359        dev->addr_len           = IEEE802154_ADDR_LEN;
 360        memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
 361
 362        dev->hard_header_len    = MAC802154_FRAME_HARD_HEADER_LEN;
 363        dev->header_ops         = &mac802154_header_ops;
 364        dev->needed_tailroom    = 2; /* FCS */
 365        dev->mtu                = IEEE802154_MTU;
 366        dev->tx_queue_len       = 10;
 367        dev->type               = ARPHRD_IEEE802154;
 368        dev->flags              = IFF_NOARP | IFF_BROADCAST;
 369        dev->watchdog_timeo     = 0;
 370
 371        dev->destructor         = free_netdev;
 372        dev->netdev_ops         = &mac802154_wpan_ops;
 373        dev->ml_priv            = &mac802154_mlme_wpan;
 374
 375        priv = netdev_priv(dev);
 376        priv->type = IEEE802154_DEV_WPAN;
 377
 378        priv->chan = MAC802154_CHAN_NONE;
 379        priv->page = 0;
 380
 381        spin_lock_init(&priv->mib_lock);
 382
 383        get_random_bytes(&priv->bsn, 1);
 384        get_random_bytes(&priv->dsn, 1);
 385
 386        priv->pan_id = IEEE802154_PANID_BROADCAST;
 387        priv->short_addr = IEEE802154_ADDR_BROADCAST;
 388}
 389
 390static int mac802154_process_data(struct net_device *dev, struct sk_buff *skb)
 391{
 392        return netif_rx_ni(skb);
 393}
 394
 395static int
 396mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb)
 397{
 398        pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
 399
 400        spin_lock_bh(&sdata->mib_lock);
 401
 402        switch (mac_cb(skb)->da.addr_type) {
 403        case IEEE802154_ADDR_NONE:
 404                if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE)
 405                        /* FIXME: check if we are PAN coordinator */
 406                        skb->pkt_type = PACKET_OTHERHOST;
 407                else
 408                        /* ACK comes with both addresses empty */
 409                        skb->pkt_type = PACKET_HOST;
 410                break;
 411        case IEEE802154_ADDR_LONG:
 412                if (mac_cb(skb)->da.pan_id != sdata->pan_id &&
 413                    mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
 414                        skb->pkt_type = PACKET_OTHERHOST;
 415                else if (!memcmp(mac_cb(skb)->da.hwaddr, sdata->dev->dev_addr,
 416                                 IEEE802154_ADDR_LEN))
 417                        skb->pkt_type = PACKET_HOST;
 418                else
 419                        skb->pkt_type = PACKET_OTHERHOST;
 420                break;
 421        case IEEE802154_ADDR_SHORT:
 422                if (mac_cb(skb)->da.pan_id != sdata->pan_id &&
 423                    mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
 424                        skb->pkt_type = PACKET_OTHERHOST;
 425                else if (mac_cb(skb)->da.short_addr == sdata->short_addr)
 426                        skb->pkt_type = PACKET_HOST;
 427                else if (mac_cb(skb)->da.short_addr ==
 428                                        IEEE802154_ADDR_BROADCAST)
 429                        skb->pkt_type = PACKET_BROADCAST;
 430                else
 431                        skb->pkt_type = PACKET_OTHERHOST;
 432                break;
 433        default:
 434                break;
 435        }
 436
 437        spin_unlock_bh(&sdata->mib_lock);
 438
 439        skb->dev = sdata->dev;
 440
 441        sdata->dev->stats.rx_packets++;
 442        sdata->dev->stats.rx_bytes += skb->len;
 443
 444        switch (mac_cb_type(skb)) {
 445        case IEEE802154_FC_TYPE_DATA:
 446                return mac802154_process_data(sdata->dev, skb);
 447        default:
 448                pr_warning("ieee802154: bad frame received (type = %d)\n",
 449                           mac_cb_type(skb));
 450                kfree_skb(skb);
 451                return NET_RX_DROP;
 452        }
 453}
 454
 455static int mac802154_parse_frame_start(struct sk_buff *skb)
 456{
 457        u8 *head = skb->data;
 458        u16 fc;
 459
 460        if (mac802154_fetch_skb_u16(skb, &fc) ||
 461            mac802154_fetch_skb_u8(skb, &(mac_cb(skb)->seq)))
 462                goto err;
 463
 464        pr_debug("fc: %04x dsn: %02x\n", fc, head[2]);
 465
 466        mac_cb(skb)->flags = IEEE802154_FC_TYPE(fc);
 467        mac_cb(skb)->sa.addr_type = IEEE802154_FC_SAMODE(fc);
 468        mac_cb(skb)->da.addr_type = IEEE802154_FC_DAMODE(fc);
 469
 470        if (fc & IEEE802154_FC_INTRA_PAN)
 471                mac_cb(skb)->flags |= MAC_CB_FLAG_INTRAPAN;
 472
 473        if (mac_cb(skb)->da.addr_type != IEEE802154_ADDR_NONE) {
 474                if (mac802154_fetch_skb_u16(skb, &(mac_cb(skb)->da.pan_id)))
 475                        goto err;
 476
 477                /* source PAN id compression */
 478                if (mac_cb_is_intrapan(skb))
 479                        mac_cb(skb)->sa.pan_id = mac_cb(skb)->da.pan_id;
 480
 481                pr_debug("dest PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
 482
 483                if (mac_cb(skb)->da.addr_type == IEEE802154_ADDR_SHORT) {
 484                        u16 *da = &(mac_cb(skb)->da.short_addr);
 485
 486                        if (mac802154_fetch_skb_u16(skb, da))
 487                                goto err;
 488
 489                        pr_debug("destination address is short: %04x\n",
 490                                 mac_cb(skb)->da.short_addr);
 491                } else {
 492                        if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
 493                                goto err;
 494
 495                        mac802154_haddr_copy_swap(mac_cb(skb)->da.hwaddr,
 496                                                  skb->data);
 497                        skb_pull(skb, IEEE802154_ADDR_LEN);
 498
 499                        pr_debug("destination address is hardware\n");
 500                }
 501        }
 502
 503        if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE) {
 504                /* non PAN-compression, fetch source address id */
 505                if (!(mac_cb_is_intrapan(skb))) {
 506                        u16 *sa_pan = &(mac_cb(skb)->sa.pan_id);
 507
 508                        if (mac802154_fetch_skb_u16(skb, sa_pan))
 509                                goto err;
 510                }
 511
 512                pr_debug("source PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
 513
 514                if (mac_cb(skb)->sa.addr_type == IEEE802154_ADDR_SHORT) {
 515                        u16 *sa = &(mac_cb(skb)->sa.short_addr);
 516
 517                        if (mac802154_fetch_skb_u16(skb, sa))
 518                                goto err;
 519
 520                        pr_debug("source address is short: %04x\n",
 521                                 mac_cb(skb)->sa.short_addr);
 522                } else {
 523                        if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
 524                                goto err;
 525
 526                        mac802154_haddr_copy_swap(mac_cb(skb)->sa.hwaddr,
 527                                                  skb->data);
 528                        skb_pull(skb, IEEE802154_ADDR_LEN);
 529
 530                        pr_debug("source address is hardware\n");
 531                }
 532        }
 533
 534        return 0;
 535err:
 536        return -EINVAL;
 537}
 538
 539void mac802154_wpans_rx(struct mac802154_priv *priv, struct sk_buff *skb)
 540{
 541        int ret;
 542        struct sk_buff *sskb;
 543        struct mac802154_sub_if_data *sdata;
 544
 545        ret = mac802154_parse_frame_start(skb);
 546        if (ret) {
 547                pr_debug("got invalid frame\n");
 548                return;
 549        }
 550
 551        rcu_read_lock();
 552        list_for_each_entry_rcu(sdata, &priv->slaves, list) {
 553                if (sdata->type != IEEE802154_DEV_WPAN)
 554                        continue;
 555
 556                sskb = skb_clone(skb, GFP_ATOMIC);
 557                if (sskb)
 558                        mac802154_subif_frame(sdata, sskb);
 559        }
 560        rcu_read_unlock();
 561}
 562
lxr.linux.no kindly hosted by Redpill Linpro AS, provider of Linux consulting and operations services since 1995.