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