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 int
  39 mac802154_wpan_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
  40 {
  41         struct mac802154_sub_if_data *priv = netdev_priv(dev);
  42         struct sockaddr_ieee802154 *sa =
  43                 (struct sockaddr_ieee802154 *)&ifr->ifr_addr;
  44         int err = -ENOIOCTLCMD;
  45
  46         spin_lock_bh(&priv->mib_lock);
  47
  48         switch (cmd) {
  49         case SIOCGIFADDR:
  50         {
  51                 u16 pan_id, short_addr;
  52
  53                 pan_id = le16_to_cpu(priv->pan_id);
  54                 short_addr = le16_to_cpu(priv->short_addr);
  55                 if (pan_id == IEEE802154_PANID_BROADCAST ||
  56                     short_addr == IEEE802154_ADDR_BROADCAST) {
  57                         err = -EADDRNOTAVAIL;
  58                         break;
  59                 }
  60
  61                 sa->family = AF_IEEE802154;
  62                 sa->addr.addr_type = IEEE802154_ADDR_SHORT;
  63                 sa->addr.pan_id = pan_id;
  64                 sa->addr.short_addr = short_addr;
  65
  66                 err = 0;
  67                 break;
  68         }
  69         case SIOCSIFADDR:
  70                 dev_warn(&dev->dev,
  71                          "Using DEBUGing ioctl SIOCSIFADDR isn't recommened!\n");
  72                 if (sa->family != AF_IEEE802154 ||
  73                     sa->addr.addr_type != IEEE802154_ADDR_SHORT ||
  74                     sa->addr.pan_id == IEEE802154_PANID_BROADCAST ||
  75                     sa->addr.short_addr == IEEE802154_ADDR_BROADCAST ||
  76                     sa->addr.short_addr == IEEE802154_ADDR_UNDEF) {
  77                         err = -EINVAL;
  78                         break;
  79                 }
  80
  81                 priv->pan_id = cpu_to_le16(sa->addr.pan_id);
  82                 priv->short_addr = cpu_to_le16(sa->addr.short_addr);
  83
  84                 err = 0;
  85                 break;
  86         }
  87
  88         spin_unlock_bh(&priv->mib_lock);
  89         return err;
  90 }
  91
  92 static int mac802154_wpan_mac_addr(struct net_device *dev, void *p)
  93 {
  94         struct sockaddr *addr = p;
  95
  96         if (netif_running(dev))
  97                 return -EBUSY;
  98
  99         /* FIXME: validate addr */
 100         memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
 101         mac802154_dev_set_ieee_addr(dev);
 102         return 0;
 103 }
 104
 105 int mac802154_set_mac_params(struct net_device *dev,
 106                              const struct ieee802154_mac_params *params)
 107 {
 108         struct mac802154_sub_if_data *priv = netdev_priv(dev);
 109
 110         mutex_lock(&priv->hw->slaves_mtx);
 111         priv->mac_params = *params;
 112         mutex_unlock(&priv->hw->slaves_mtx);
 113
 114         return 0;
 115 }
 116
 117 void mac802154_get_mac_params(struct net_device *dev,
 118                               struct ieee802154_mac_params *params)
 119 {
 120         struct mac802154_sub_if_data *priv = netdev_priv(dev);
 121
 122         mutex_lock(&priv->hw->slaves_mtx);
 123         *params = priv->mac_params;
 124         mutex_unlock(&priv->hw->slaves_mtx);
 125 }
 126
 127 int mac802154_wpan_open(struct net_device *dev)
 128 {
 129         int rc;
 130         struct mac802154_sub_if_data *priv = netdev_priv(dev);
 131         struct wpan_phy *phy = priv->hw->phy;
 132
 133         rc = mac802154_slave_open(dev);
 134         if (rc < 0)
 135                 return rc;
 136
 137         mutex_lock(&phy->pib_lock);
 138
 139         if (phy->set_txpower) {
 140                 rc = phy->set_txpower(phy, priv->mac_params.transmit_power);
 141                 if (rc < 0)
 142                         goto out;
 143         }
 144
 145         if (phy->set_lbt) {
 146                 rc = phy->set_lbt(phy, priv->mac_params.lbt);
 147                 if (rc < 0)
 148                         goto out;
 149         }
 150
 151         if (phy->set_cca_mode) {
 152                 rc = phy->set_cca_mode(phy, priv->mac_params.cca_mode);
 153                 if (rc < 0)
 154                         goto out;
 155         }
 156
 157         if (phy->set_cca_ed_level) {
 158                 rc = phy->set_cca_ed_level(phy, priv->mac_params.cca_ed_level);
 159                 if (rc < 0)
 160                         goto out;
 161         }
 162
 163         if (phy->set_csma_params) {
 164                 rc = phy->set_csma_params(phy, priv->mac_params.min_be,
 165                                           priv->mac_params.max_be,
 166                                           priv->mac_params.csma_retries);
 167                 if (rc < 0)
 168                         goto out;
 169         }
 170
 171         if (phy->set_frame_retries) {
 172                 rc = phy->set_frame_retries(phy,
 173                                             priv->mac_params.frame_retries);
 174                 if (rc < 0)
 175                         goto out;
 176         }
 177
 178         mutex_unlock(&phy->pib_lock);
 179         return 0;
 180
 181 out:
 182         mutex_unlock(&phy->pib_lock);
 183         return rc;
 184 }
 185
 186 static int mac802154_header_create(struct sk_buff *skb,
 187                                    struct net_device *dev,
 188                                    unsigned short type,
 189                                    const void *daddr,
 190                                    const void *saddr,
 191                                    unsigned len)
 192 {
 193         struct ieee802154_hdr hdr;
 194         struct mac802154_sub_if_data *priv = netdev_priv(dev);
 195         int hlen;
 196
 197         if (!daddr)
 198                 return -EINVAL;
 199
 200         memset(&hdr.fc, 0, sizeof(hdr.fc));
 201         hdr.fc.type = mac_cb_type(skb);
 202         hdr.fc.security_enabled = mac_cb_is_secen(skb);
 203         hdr.fc.ack_request = mac_cb_is_ackreq(skb);
 204
 205         if (!saddr) {
 206                 spin_lock_bh(&priv->mib_lock);
 207
 208                 if (priv->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST) ||
 209                     priv->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) ||
 210                     priv->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST)) {
 211                         hdr.source.mode = IEEE802154_ADDR_LONG;
 212                         hdr.source.extended_addr = priv->extended_addr;
 213                 } else {
 214                         hdr.source.mode = IEEE802154_ADDR_SHORT;
 215                         hdr.source.short_addr = priv->short_addr;
 216                 }
 217
 218                 hdr.source.pan_id = priv->pan_id;
 219
 220                 spin_unlock_bh(&priv->mib_lock);
 221         } else {
 222                 hdr.source = *(const struct ieee802154_addr *)saddr;
 223         }
 224
 225         hdr.dest = *(const struct ieee802154_addr *)daddr;
 226
 227         hlen = ieee802154_hdr_push(skb, &hdr);
 228         if (hlen < 0)
 229                 return -EINVAL;
 230
 231         skb_reset_mac_header(skb);
 232         skb->mac_len = hlen;
 233
 234         if (hlen + len + 2 > dev->mtu)
 235                 return -EMSGSIZE;
 236
 237         return hlen;
 238 }
 239
 240 static int
 241 mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr)
 242 {
 243         struct ieee802154_hdr hdr;
 244         struct ieee802154_addr *addr = (struct ieee802154_addr *)haddr;
 245
 246         if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0) {
 247                 pr_debug("malformed packet\n");
 248                 return 0;
 249         }
 250
 251         *addr = hdr.source;
 252         return sizeof(*addr);
 253 }
 254
 255 static netdev_tx_t
 256 mac802154_wpan_xmit(struct sk_buff *skb, struct net_device *dev)
 257 {
 258         struct mac802154_sub_if_data *priv;
 259         u8 chan, page;
 260
 261         priv = netdev_priv(dev);
 262
 263         spin_lock_bh(&priv->mib_lock);
 264         chan = priv->chan;
 265         page = priv->page;
 266         spin_unlock_bh(&priv->mib_lock);
 267
 268         if (chan == MAC802154_CHAN_NONE ||
 269             page >= WPAN_NUM_PAGES ||
 270             chan >= WPAN_NUM_CHANNELS) {
 271                 kfree_skb(skb);
 272                 return NETDEV_TX_OK;
 273         }
 274
 275         skb->skb_iif = dev->ifindex;
 276         dev->stats.tx_packets++;
 277         dev->stats.tx_bytes += skb->len;
 278
 279         return mac802154_tx(priv->hw, skb, page, chan);
 280 }
 281
 282 static struct header_ops mac802154_header_ops = {
 283         .create         = mac802154_header_create,
 284         .parse          = mac802154_header_parse,
 285 };
 286
 287 static const struct net_device_ops mac802154_wpan_ops = {
 288         .ndo_open               = mac802154_wpan_open,
 289         .ndo_stop               = mac802154_slave_close,
 290         .ndo_start_xmit         = mac802154_wpan_xmit,
 291         .ndo_do_ioctl           = mac802154_wpan_ioctl,
 292         .ndo_set_mac_address    = mac802154_wpan_mac_addr,
 293 };
 294
 295 void mac802154_wpan_setup(struct net_device *dev)
 296 {
 297         struct mac802154_sub_if_data *priv;
 298
 299         dev->addr_len           = IEEE802154_ADDR_LEN;
 300         memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
 301
 302         dev->hard_header_len    = MAC802154_FRAME_HARD_HEADER_LEN;
 303         dev->header_ops         = &mac802154_header_ops;
 304         dev->needed_tailroom    = 2; /* FCS */
 305         dev->mtu                = IEEE802154_MTU;
 306         dev->tx_queue_len       = 300;
 307         dev->type               = ARPHRD_IEEE802154;
 308         dev->flags              = IFF_NOARP | IFF_BROADCAST;
 309         dev->watchdog_timeo     = 0;
 310
 311         dev->destructor         = free_netdev;
 312         dev->netdev_ops         = &mac802154_wpan_ops;
 313         dev->ml_priv            = &mac802154_mlme_wpan;
 314
 315         priv = netdev_priv(dev);
 316         priv->type = IEEE802154_DEV_WPAN;
 317
 318         priv->chan = MAC802154_CHAN_NONE;
 319         priv->page = 0;
 320
 321         spin_lock_init(&priv->mib_lock);
 322
 323         get_random_bytes(&priv->bsn, 1);
 324         get_random_bytes(&priv->dsn, 1);
 325
 326         /* defaults per 802.15.4-2011 */
 327         priv->mac_params.min_be = 3;
 328         priv->mac_params.max_be = 5;
 329         priv->mac_params.csma_retries = 4;
 330         priv->mac_params.frame_retries = -1; /* for compatibility, actual default is 3 */
 331
 332         priv->pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
 333         priv->short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
 334 }
 335
 336 static int mac802154_process_data(struct net_device *dev, struct sk_buff *skb)
 337 {
 338         return netif_rx_ni(skb);
 339 }
 340
 341 static int
 342 mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb)
 343 {
 344         __le16 span, sshort;
 345
 346         pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
 347
 348         spin_lock_bh(&sdata->mib_lock);
 349
 350         span = sdata->pan_id;
 351         sshort = sdata->short_addr;
 352
 353         switch (mac_cb(skb)->dest.mode) {
 354         case IEEE802154_ADDR_NONE:
 355                 if (mac_cb(skb)->dest.mode != IEEE802154_ADDR_NONE)
 356                         /* FIXME: check if we are PAN coordinator */
 357                         skb->pkt_type = PACKET_OTHERHOST;
 358                 else
 359                         /* ACK comes with both addresses empty */
 360                         skb->pkt_type = PACKET_HOST;
 361                 break;
 362         case IEEE802154_ADDR_LONG:
 363                 if (mac_cb(skb)->dest.pan_id != span &&
 364                     mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
 365                         skb->pkt_type = PACKET_OTHERHOST;
 366                 else if (mac_cb(skb)->dest.extended_addr == sdata->extended_addr)
 367                         skb->pkt_type = PACKET_HOST;
 368                 else
 369                         skb->pkt_type = PACKET_OTHERHOST;
 370                 break;
 371         case IEEE802154_ADDR_SHORT:
 372                 if (mac_cb(skb)->dest.pan_id != span &&
 373                     mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
 374                         skb->pkt_type = PACKET_OTHERHOST;
 375                 else if (mac_cb(skb)->dest.short_addr == sshort)
 376                         skb->pkt_type = PACKET_HOST;
 377                 else if (mac_cb(skb)->dest.short_addr ==
 378                           cpu_to_le16(IEEE802154_ADDR_BROADCAST))
 379                         skb->pkt_type = PACKET_BROADCAST;
 380                 else
 381                         skb->pkt_type = PACKET_OTHERHOST;
 382                 break;
 383         default:
 384                 break;
 385         }
 386
 387         spin_unlock_bh(&sdata->mib_lock);
 388
 389         skb->dev = sdata->dev;
 390
 391         sdata->dev->stats.rx_packets++;
 392         sdata->dev->stats.rx_bytes += skb->len;
 393
 394         switch (mac_cb_type(skb)) {
 395         case IEEE802154_FC_TYPE_DATA:
 396                 return mac802154_process_data(sdata->dev, skb);
 397         default:
 398                 pr_warn("ieee802154: bad frame received (type = %d)\n",
 399                         mac_cb_type(skb));
 400                 kfree_skb(skb);
 401                 return NET_RX_DROP;
 402         }
 403 }
 404
 405 static void mac802154_print_addr(const char *name,
 406                                  const struct ieee802154_addr *addr)
 407 {
 408         if (addr->mode == IEEE802154_ADDR_NONE)
 409                 pr_debug("%s not present\n", name);
 410
 411         pr_debug("%s PAN ID: %04x\n", name, le16_to_cpu(addr->pan_id));
 412         if (addr->mode == IEEE802154_ADDR_SHORT) {
 413                 pr_debug("%s is short: %04x\n", name,
 414                          le16_to_cpu(addr->short_addr));
 415         } else {
 416                 u64 hw = swab64((__force u64) addr->extended_addr);
 417
 418                 pr_debug("%s is hardware: %8phC\n", name, &hw);
 419         }
 420 }
 421
 422 static int mac802154_parse_frame_start(struct sk_buff *skb)
 423 {
 424         int hlen;
 425         struct ieee802154_hdr hdr;
 426
 427         hlen = ieee802154_hdr_pull(skb, &hdr);
 428         if (hlen < 0)
 429                 return -EINVAL;
 430
 431         skb->mac_len = hlen;
 432
 433         pr_debug("fc: %04x dsn: %02x\n", le16_to_cpup((__le16 *)&hdr.fc),
 434                  hdr.seq);
 435
 436         mac_cb(skb)->flags = hdr.fc.type;
 437
 438         if (hdr.fc.ack_request)
 439                 mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
 440         if (hdr.fc.security_enabled)
 441                 mac_cb(skb)->flags |= MAC_CB_FLAG_SECEN;
 442
 443         mac802154_print_addr("destination", &hdr.dest);
 444         mac802154_print_addr("source", &hdr.source);
 445
 446         mac_cb(skb)->source = hdr.source;
 447         mac_cb(skb)->dest = hdr.dest;
 448
 449         if (hdr.fc.security_enabled) {
 450                 u64 key;
 451
 452                 pr_debug("seclevel %i\n", hdr.sec.level);
 453
 454                 switch (hdr.sec.key_id_mode) {
 455                 case IEEE802154_SCF_KEY_IMPLICIT:
 456                         pr_debug("implicit key\n");
 457                         break;
 458
 459                 case IEEE802154_SCF_KEY_INDEX:
 460                         pr_debug("key %02x\n", hdr.sec.key_id);
 461                         break;
 462
 463                 case IEEE802154_SCF_KEY_SHORT_INDEX:
 464                         pr_debug("key %04x:%04x %02x\n",
 465                                  le32_to_cpu(hdr.sec.short_src) >> 16,
 466                                  le32_to_cpu(hdr.sec.short_src) & 0xffff,
 467                                  hdr.sec.key_id);
 468                         break;
 469
 470                 case IEEE802154_SCF_KEY_HW_INDEX:
 471                         key = swab64((__force u64) hdr.sec.extended_src);
 472                         pr_debug("key source %8phC %02x\n", &key,
 473                                  hdr.sec.key_id);
 474                         break;
 475                 }
 476
 477                 return -EINVAL;
 478         }
 479
 480         return 0;
 481 }
 482
 483 void mac802154_wpans_rx(struct mac802154_priv *priv, struct sk_buff *skb)
 484 {
 485         int ret;
 486         struct sk_buff *sskb;
 487         struct mac802154_sub_if_data *sdata;
 488
 489         ret = mac802154_parse_frame_start(skb);
 490         if (ret) {
 491                 pr_debug("got invalid frame\n");
 492                 return;
 493         }
 494
 495         rcu_read_lock();
 496         list_for_each_entry_rcu(sdata, &priv->slaves, list) {
 497                 if (sdata->type != IEEE802154_DEV_WPAN)
 498                         continue;
 499
 500                 sskb = skb_clone(skb, GFP_ATOMIC);
 501                 if (sskb)
 502                         mac802154_subif_frame(sdata, sskb);
 503         }
 504         rcu_read_unlock();
 505 }