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 static int mac802154_header_create(struct sk_buff *skb,
 106                                    struct net_device *dev,
 107                                    unsigned short type,
 108                                    const void *daddr,
 109                                    const void *saddr,
 110                                    unsigned len)
 111 {
 112         struct ieee802154_hdr hdr;
 113         struct mac802154_sub_if_data *priv = netdev_priv(dev);
 114         int hlen;
 115
 116         if (!daddr)
 117                 return -EINVAL;
 118
 119         memset(&hdr.fc, 0, sizeof(hdr.fc));
 120         hdr.fc.type = mac_cb_type(skb);
 121         hdr.fc.security_enabled = mac_cb_is_secen(skb);
 122         hdr.fc.ack_request = mac_cb_is_ackreq(skb);
 123
 124         if (!saddr) {
 125                 spin_lock_bh(&priv->mib_lock);
 126
 127                 if (priv->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST) ||
 128                     priv->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) ||
 129                     priv->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST)) {
 130                         hdr.source.mode = IEEE802154_ADDR_LONG;
 131                         hdr.source.extended_addr = priv->extended_addr;
 132                 } else {
 133                         hdr.source.mode = IEEE802154_ADDR_SHORT;
 134                         hdr.source.short_addr = priv->short_addr;
 135                 }
 136
 137                 hdr.source.pan_id = priv->pan_id;
 138
 139                 spin_unlock_bh(&priv->mib_lock);
 140         } else {
 141                 hdr.source = *(const struct ieee802154_addr *)saddr;
 142         }
 143
 144         hdr.dest = *(const struct ieee802154_addr *)daddr;
 145
 146         hlen = ieee802154_hdr_push(skb, &hdr);
 147         if (hlen < 0)
 148                 return -EINVAL;
 149
 150         skb_reset_mac_header(skb);
 151         skb->mac_len = hlen;
 152
 153         if (hlen + len + 2 > dev->mtu)
 154                 return -EMSGSIZE;
 155
 156         return hlen;
 157 }
 158
 159 static int
 160 mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr)
 161 {
 162         struct ieee802154_hdr hdr;
 163         struct ieee802154_addr *addr = (struct ieee802154_addr *)haddr;
 164
 165         if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0) {
 166                 pr_debug("malformed packet\n");
 167                 return 0;
 168         }
 169
 170         *addr = hdr.source;
 171         return sizeof(*addr);
 172 }
 173
 174 static netdev_tx_t
 175 mac802154_wpan_xmit(struct sk_buff *skb, struct net_device *dev)
 176 {
 177         struct mac802154_sub_if_data *priv;
 178         u8 chan, page;
 179
 180         priv = netdev_priv(dev);
 181
 182         spin_lock_bh(&priv->mib_lock);
 183         chan = priv->chan;
 184         page = priv->page;
 185         spin_unlock_bh(&priv->mib_lock);
 186
 187         if (chan == MAC802154_CHAN_NONE ||
 188             page >= WPAN_NUM_PAGES ||
 189             chan >= WPAN_NUM_CHANNELS) {
 190                 kfree_skb(skb);
 191                 return NETDEV_TX_OK;
 192         }
 193
 194         skb->skb_iif = dev->ifindex;
 195         dev->stats.tx_packets++;
 196         dev->stats.tx_bytes += skb->len;
 197
 198         return mac802154_tx(priv->hw, skb, page, chan);
 199 }
 200
 201 static struct header_ops mac802154_header_ops = {
 202         .create         = mac802154_header_create,
 203         .parse          = mac802154_header_parse,
 204 };
 205
 206 static const struct net_device_ops mac802154_wpan_ops = {
 207         .ndo_open               = mac802154_slave_open,
 208         .ndo_stop               = mac802154_slave_close,
 209         .ndo_start_xmit         = mac802154_wpan_xmit,
 210         .ndo_do_ioctl           = mac802154_wpan_ioctl,
 211         .ndo_set_mac_address    = mac802154_wpan_mac_addr,
 212 };
 213
 214 void mac802154_wpan_setup(struct net_device *dev)
 215 {
 216         struct mac802154_sub_if_data *priv;
 217
 218         dev->addr_len           = IEEE802154_ADDR_LEN;
 219         memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
 220
 221         dev->hard_header_len    = MAC802154_FRAME_HARD_HEADER_LEN;
 222         dev->header_ops         = &mac802154_header_ops;
 223         dev->needed_tailroom    = 2; /* FCS */
 224         dev->mtu                = IEEE802154_MTU;
 225         dev->tx_queue_len       = 300;
 226         dev->type               = ARPHRD_IEEE802154;
 227         dev->flags              = IFF_NOARP | IFF_BROADCAST;
 228         dev->watchdog_timeo     = 0;
 229
 230         dev->destructor         = free_netdev;
 231         dev->netdev_ops         = &mac802154_wpan_ops;
 232         dev->ml_priv            = &mac802154_mlme_wpan;
 233
 234         priv = netdev_priv(dev);
 235         priv->type = IEEE802154_DEV_WPAN;
 236
 237         priv->chan = MAC802154_CHAN_NONE;
 238         priv->page = 0;
 239
 240         spin_lock_init(&priv->mib_lock);
 241
 242         get_random_bytes(&priv->bsn, 1);
 243         get_random_bytes(&priv->dsn, 1);
 244
 245         priv->pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
 246         priv->short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
 247 }
 248
 249 static int mac802154_process_data(struct net_device *dev, struct sk_buff *skb)
 250 {
 251         return netif_rx_ni(skb);
 252 }
 253
 254 static int
 255 mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb)
 256 {
 257         __le16 span, sshort;
 258
 259         pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
 260
 261         spin_lock_bh(&sdata->mib_lock);
 262
 263         span = sdata->pan_id;
 264         sshort = sdata->short_addr;
 265
 266         switch (mac_cb(skb)->dest.mode) {
 267         case IEEE802154_ADDR_NONE:
 268                 if (mac_cb(skb)->dest.mode != IEEE802154_ADDR_NONE)
 269                         /* FIXME: check if we are PAN coordinator */
 270                         skb->pkt_type = PACKET_OTHERHOST;
 271                 else
 272                         /* ACK comes with both addresses empty */
 273                         skb->pkt_type = PACKET_HOST;
 274                 break;
 275         case IEEE802154_ADDR_LONG:
 276                 if (mac_cb(skb)->dest.pan_id != span &&
 277                     mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
 278                         skb->pkt_type = PACKET_OTHERHOST;
 279                 else if (mac_cb(skb)->dest.extended_addr == sdata->extended_addr)
 280                         skb->pkt_type = PACKET_HOST;
 281                 else
 282                         skb->pkt_type = PACKET_OTHERHOST;
 283                 break;
 284         case IEEE802154_ADDR_SHORT:
 285                 if (mac_cb(skb)->dest.pan_id != span &&
 286                     mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST))
 287                         skb->pkt_type = PACKET_OTHERHOST;
 288                 else if (mac_cb(skb)->dest.short_addr == sshort)
 289                         skb->pkt_type = PACKET_HOST;
 290                 else if (mac_cb(skb)->dest.short_addr ==
 291                           cpu_to_le16(IEEE802154_ADDR_BROADCAST))
 292                         skb->pkt_type = PACKET_BROADCAST;
 293                 else
 294                         skb->pkt_type = PACKET_OTHERHOST;
 295                 break;
 296         default:
 297                 break;
 298         }
 299
 300         spin_unlock_bh(&sdata->mib_lock);
 301
 302         skb->dev = sdata->dev;
 303
 304         sdata->dev->stats.rx_packets++;
 305         sdata->dev->stats.rx_bytes += skb->len;
 306
 307         switch (mac_cb_type(skb)) {
 308         case IEEE802154_FC_TYPE_DATA:
 309                 return mac802154_process_data(sdata->dev, skb);
 310         default:
 311                 pr_warn("ieee802154: bad frame received (type = %d)\n",
 312                         mac_cb_type(skb));
 313                 kfree_skb(skb);
 314                 return NET_RX_DROP;
 315         }
 316 }
 317
 318 static void mac802154_print_addr(const char *name,
 319                                  const struct ieee802154_addr *addr)
 320 {
 321         if (addr->mode == IEEE802154_ADDR_NONE)
 322                 pr_debug("%s not present\n", name);
 323
 324         pr_debug("%s PAN ID: %04x\n", name, le16_to_cpu(addr->pan_id));
 325         if (addr->mode == IEEE802154_ADDR_SHORT) {
 326                 pr_debug("%s is short: %04x\n", name,
 327                          le16_to_cpu(addr->short_addr));
 328         } else {
 329                 u64 hw = swab64((__force u64) addr->extended_addr);
 330
 331                 pr_debug("%s is hardware: %8phC\n", name, &hw);
 332         }
 333 }
 334
 335 static int mac802154_parse_frame_start(struct sk_buff *skb)
 336 {
 337         int hlen;
 338         struct ieee802154_hdr hdr;
 339
 340         hlen = ieee802154_hdr_pull(skb, &hdr);
 341         if (hlen < 0)
 342                 return -EINVAL;
 343
 344         skb->mac_len = hlen;
 345
 346         pr_debug("fc: %04x dsn: %02x\n", le16_to_cpup((__le16 *)&hdr.fc),
 347                  hdr.seq);
 348
 349         mac_cb(skb)->flags = hdr.fc.type;
 350
 351         if (hdr.fc.ack_request)
 352                 mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
 353         if (hdr.fc.security_enabled)
 354                 mac_cb(skb)->flags |= MAC_CB_FLAG_SECEN;
 355
 356         mac802154_print_addr("destination", &hdr.dest);
 357         mac802154_print_addr("source", &hdr.source);
 358
 359         mac_cb(skb)->source = hdr.source;
 360         mac_cb(skb)->dest = hdr.dest;
 361
 362         if (hdr.fc.security_enabled) {
 363                 u64 key;
 364
 365                 pr_debug("seclevel %i\n", hdr.sec.level);
 366
 367                 switch (hdr.sec.key_id_mode) {
 368                 case IEEE802154_SCF_KEY_IMPLICIT:
 369                         pr_debug("implicit key\n");
 370                         break;
 371
 372                 case IEEE802154_SCF_KEY_INDEX:
 373                         pr_debug("key %02x\n", hdr.sec.key_id);
 374                         break;
 375
 376                 case IEEE802154_SCF_KEY_SHORT_INDEX:
 377                         pr_debug("key %04x:%04x %02x\n",
 378                                  le32_to_cpu(hdr.sec.short_src) >> 16,
 379                                  le32_to_cpu(hdr.sec.short_src) & 0xffff,
 380                                  hdr.sec.key_id);
 381                         break;
 382
 383                 case IEEE802154_SCF_KEY_HW_INDEX:
 384                         key = swab64((__force u64) hdr.sec.extended_src);
 385                         pr_debug("key source %8phC %02x\n", &key,
 386                                  hdr.sec.key_id);
 387                         break;
 388                 }
 389
 390                 return -EINVAL;
 391         }
 392
 393         return 0;
 394 }
 395
 396 void mac802154_wpans_rx(struct mac802154_priv *priv, struct sk_buff *skb)
 397 {
 398         int ret;
 399         struct sk_buff *sskb;
 400         struct mac802154_sub_if_data *sdata;
 401
 402         ret = mac802154_parse_frame_start(skb);
 403         if (ret) {
 404                 pr_debug("got invalid frame\n");
 405                 return;
 406         }
 407
 408         rcu_read_lock();
 409         list_for_each_entry_rcu(sdata, &priv->slaves, list) {
 410                 if (sdata->type != IEEE802154_DEV_WPAN)
 411                         continue;
 412
 413                 sskb = skb_clone(skb, GFP_ATOMIC);
 414                 if (sskb)
 415                         mac802154_subif_frame(sdata, sskb);
 416         }
 417         rcu_read_unlock();
 418 }