alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 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 | |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 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: |
Phoebe Buckheister | b70ab2e | 2014-03-14 21:23:59 +0100 | [diff] [blame] | 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) { |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 57 | err = -EADDRNOTAVAIL; |
| 58 | break; |
| 59 | } |
| 60 | |
| 61 | sa->family = AF_IEEE802154; |
| 62 | sa->addr.addr_type = IEEE802154_ADDR_SHORT; |
Phoebe Buckheister | b70ab2e | 2014-03-14 21:23:59 +0100 | [diff] [blame] | 63 | sa->addr.pan_id = pan_id; |
| 64 | sa->addr.short_addr = short_addr; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 65 | |
| 66 | err = 0; |
| 67 | break; |
Phoebe Buckheister | b70ab2e | 2014-03-14 21:23:59 +0100 | [diff] [blame] | 68 | } |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 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 | |
Phoebe Buckheister | b70ab2e | 2014-03-14 21:23:59 +0100 | [diff] [blame] | 81 | priv->pan_id = cpu_to_le16(sa->addr.pan_id); |
| 82 | priv->short_addr = cpu_to_le16(sa->addr.short_addr); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 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 | |
Phoebe Buckheister | e462ded | 2014-03-31 21:37:46 +0200 | [diff] [blame] | 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 | |
Phoebe Buckheister | 6ef0023 | 2014-05-14 17:43:11 +0200 | [diff] [blame] | 127 | static int mac802154_wpan_open(struct net_device *dev) |
Phoebe Buckheister | e462ded | 2014-03-31 21:37:46 +0200 | [diff] [blame] | 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 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 186 | static int mac802154_set_header_security(struct mac802154_sub_if_data *priv, |
| 187 | struct ieee802154_hdr *hdr, |
| 188 | const struct ieee802154_mac_cb *cb) |
| 189 | { |
| 190 | struct ieee802154_llsec_params params; |
| 191 | u8 level; |
| 192 | |
| 193 | mac802154_llsec_get_params(&priv->sec, ¶ms); |
| 194 | |
| 195 | if (!params.enabled && cb->secen_override && cb->secen) |
| 196 | return -EINVAL; |
| 197 | if (!params.enabled || |
| 198 | (cb->secen_override && !cb->secen) || |
| 199 | !params.out_level) |
| 200 | return 0; |
| 201 | if (cb->seclevel_override && !cb->seclevel) |
| 202 | return -EINVAL; |
| 203 | |
| 204 | level = cb->seclevel_override ? cb->seclevel : params.out_level; |
| 205 | |
| 206 | hdr->fc.security_enabled = 1; |
| 207 | hdr->sec.level = level; |
| 208 | hdr->sec.key_id_mode = params.out_key.mode; |
| 209 | if (params.out_key.mode == IEEE802154_SCF_KEY_SHORT_INDEX) |
| 210 | hdr->sec.short_src = params.out_key.short_source; |
| 211 | else if (params.out_key.mode == IEEE802154_SCF_KEY_HW_INDEX) |
| 212 | hdr->sec.extended_src = params.out_key.extended_source; |
| 213 | hdr->sec.key_id = params.out_key.id; |
| 214 | |
| 215 | return 0; |
| 216 | } |
| 217 | |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 218 | static int mac802154_header_create(struct sk_buff *skb, |
| 219 | struct net_device *dev, |
| 220 | unsigned short type, |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 221 | const void *daddr, |
| 222 | const void *saddr, |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 223 | unsigned len) |
| 224 | { |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 225 | struct ieee802154_hdr hdr; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 226 | struct mac802154_sub_if_data *priv = netdev_priv(dev); |
Phoebe Buckheister | 32edc40 | 2014-05-14 17:43:08 +0200 | [diff] [blame] | 227 | struct ieee802154_mac_cb *cb = mac_cb(skb); |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 228 | int hlen; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 229 | |
| 230 | if (!daddr) |
| 231 | return -EINVAL; |
| 232 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 233 | memset(&hdr.fc, 0, sizeof(hdr.fc)); |
Phoebe Buckheister | 32edc40 | 2014-05-14 17:43:08 +0200 | [diff] [blame] | 234 | hdr.fc.type = cb->type; |
| 235 | hdr.fc.security_enabled = cb->secen; |
| 236 | hdr.fc.ack_request = cb->ackreq; |
| 237 | hdr.seq = ieee802154_mlme_ops(dev)->get_dsn(dev); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 238 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 239 | if (mac802154_set_header_security(priv, &hdr, cb) < 0) |
| 240 | return -EINVAL; |
| 241 | |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 242 | if (!saddr) { |
| 243 | spin_lock_bh(&priv->mib_lock); |
| 244 | |
Phoebe Buckheister | b70ab2e | 2014-03-14 21:23:59 +0100 | [diff] [blame] | 245 | if (priv->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST) || |
| 246 | priv->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) || |
| 247 | priv->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST)) { |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 248 | hdr.source.mode = IEEE802154_ADDR_LONG; |
| 249 | hdr.source.extended_addr = priv->extended_addr; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 250 | } else { |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 251 | hdr.source.mode = IEEE802154_ADDR_SHORT; |
| 252 | hdr.source.short_addr = priv->short_addr; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 253 | } |
| 254 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 255 | hdr.source.pan_id = priv->pan_id; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 256 | |
| 257 | spin_unlock_bh(&priv->mib_lock); |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 258 | } else { |
| 259 | hdr.source = *(const struct ieee802154_addr *)saddr; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 260 | } |
| 261 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 262 | hdr.dest = *(const struct ieee802154_addr *)daddr; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 263 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 264 | hlen = ieee802154_hdr_push(skb, &hdr); |
| 265 | if (hlen < 0) |
| 266 | return -EINVAL; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 267 | |
Alexander Aring | 3e69162 | 2013-10-30 09:18:22 +0100 | [diff] [blame] | 268 | skb_reset_mac_header(skb); |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 269 | skb->mac_len = hlen; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 270 | |
Phoebe Buckheister | 8c84296 | 2014-05-14 17:43:07 +0200 | [diff] [blame] | 271 | if (len > ieee802154_max_payload(&hdr)) |
Phoebe Buckheister | d1d7358 | 2014-03-14 21:24:04 +0100 | [diff] [blame] | 272 | return -EMSGSIZE; |
| 273 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 274 | return hlen; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 275 | } |
| 276 | |
| 277 | static int |
| 278 | mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr) |
| 279 | { |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 280 | struct ieee802154_hdr hdr; |
| 281 | struct ieee802154_addr *addr = (struct ieee802154_addr *)haddr; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 282 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 283 | if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0) { |
| 284 | pr_debug("malformed packet\n"); |
| 285 | return 0; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 286 | } |
| 287 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 288 | *addr = hdr.source; |
| 289 | return sizeof(*addr); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 290 | } |
| 291 | |
| 292 | static netdev_tx_t |
| 293 | mac802154_wpan_xmit(struct sk_buff *skb, struct net_device *dev) |
| 294 | { |
| 295 | struct mac802154_sub_if_data *priv; |
| 296 | u8 chan, page; |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 297 | int rc; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 298 | |
| 299 | priv = netdev_priv(dev); |
| 300 | |
| 301 | spin_lock_bh(&priv->mib_lock); |
| 302 | chan = priv->chan; |
| 303 | page = priv->page; |
| 304 | spin_unlock_bh(&priv->mib_lock); |
| 305 | |
| 306 | if (chan == MAC802154_CHAN_NONE || |
| 307 | page >= WPAN_NUM_PAGES || |
Alan Ott | fcefbe9 | 2012-11-29 18:25:10 +0000 | [diff] [blame] | 308 | chan >= WPAN_NUM_CHANNELS) { |
| 309 | kfree_skb(skb); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 310 | return NETDEV_TX_OK; |
Alan Ott | fcefbe9 | 2012-11-29 18:25:10 +0000 | [diff] [blame] | 311 | } |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 312 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 313 | rc = mac802154_llsec_encrypt(&priv->sec, skb); |
| 314 | if (rc) { |
| 315 | pr_warn("encryption failed: %i\n", rc); |
| 316 | kfree_skb(skb); |
| 317 | return NETDEV_TX_OK; |
| 318 | } |
| 319 | |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 320 | skb->skb_iif = dev->ifindex; |
| 321 | dev->stats.tx_packets++; |
| 322 | dev->stats.tx_bytes += skb->len; |
| 323 | |
| 324 | return mac802154_tx(priv->hw, skb, page, chan); |
| 325 | } |
| 326 | |
| 327 | static struct header_ops mac802154_header_ops = { |
| 328 | .create = mac802154_header_create, |
| 329 | .parse = mac802154_header_parse, |
| 330 | }; |
| 331 | |
| 332 | static const struct net_device_ops mac802154_wpan_ops = { |
Phoebe Buckheister | e462ded | 2014-03-31 21:37:46 +0200 | [diff] [blame] | 333 | .ndo_open = mac802154_wpan_open, |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 334 | .ndo_stop = mac802154_slave_close, |
| 335 | .ndo_start_xmit = mac802154_wpan_xmit, |
| 336 | .ndo_do_ioctl = mac802154_wpan_ioctl, |
| 337 | .ndo_set_mac_address = mac802154_wpan_mac_addr, |
| 338 | }; |
| 339 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 340 | static void mac802154_wpan_free(struct net_device *dev) |
| 341 | { |
| 342 | struct mac802154_sub_if_data *priv = netdev_priv(dev); |
| 343 | |
| 344 | mac802154_llsec_destroy(&priv->sec); |
| 345 | |
| 346 | free_netdev(dev); |
| 347 | } |
| 348 | |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 349 | void mac802154_wpan_setup(struct net_device *dev) |
| 350 | { |
| 351 | struct mac802154_sub_if_data *priv; |
| 352 | |
| 353 | dev->addr_len = IEEE802154_ADDR_LEN; |
| 354 | memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN); |
| 355 | |
| 356 | dev->hard_header_len = MAC802154_FRAME_HARD_HEADER_LEN; |
| 357 | dev->header_ops = &mac802154_header_ops; |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 358 | dev->needed_tailroom = 2 + 16; /* FCS + MIC */ |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 359 | dev->mtu = IEEE802154_MTU; |
Alan Ott | e937f583 | 2013-04-03 04:00:57 +0000 | [diff] [blame] | 360 | dev->tx_queue_len = 300; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 361 | dev->type = ARPHRD_IEEE802154; |
| 362 | dev->flags = IFF_NOARP | IFF_BROADCAST; |
| 363 | dev->watchdog_timeo = 0; |
| 364 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 365 | dev->destructor = mac802154_wpan_free; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 366 | dev->netdev_ops = &mac802154_wpan_ops; |
| 367 | dev->ml_priv = &mac802154_mlme_wpan; |
| 368 | |
| 369 | priv = netdev_priv(dev); |
| 370 | priv->type = IEEE802154_DEV_WPAN; |
| 371 | |
| 372 | priv->chan = MAC802154_CHAN_NONE; |
| 373 | priv->page = 0; |
| 374 | |
| 375 | spin_lock_init(&priv->mib_lock); |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 376 | mutex_init(&priv->sec_mtx); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 377 | |
| 378 | get_random_bytes(&priv->bsn, 1); |
| 379 | get_random_bytes(&priv->dsn, 1); |
| 380 | |
Phoebe Buckheister | e462ded | 2014-03-31 21:37:46 +0200 | [diff] [blame] | 381 | /* defaults per 802.15.4-2011 */ |
| 382 | priv->mac_params.min_be = 3; |
| 383 | priv->mac_params.max_be = 5; |
| 384 | priv->mac_params.csma_retries = 4; |
| 385 | priv->mac_params.frame_retries = -1; /* for compatibility, actual default is 3 */ |
| 386 | |
Phoebe Buckheister | b70ab2e | 2014-03-14 21:23:59 +0100 | [diff] [blame] | 387 | priv->pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST); |
| 388 | priv->short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST); |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 389 | |
| 390 | mac802154_llsec_init(&priv->sec); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 391 | } |
| 392 | |
| 393 | static int mac802154_process_data(struct net_device *dev, struct sk_buff *skb) |
| 394 | { |
Alexander Aring | 5ff3fec | 2013-01-02 01:01:10 +0000 | [diff] [blame] | 395 | return netif_rx_ni(skb); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 396 | } |
| 397 | |
| 398 | static int |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 399 | mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb, |
| 400 | const struct ieee802154_hdr *hdr) |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 401 | { |
Phoebe Buckheister | ae531b9 | 2014-03-14 21:24:02 +0100 | [diff] [blame] | 402 | __le16 span, sshort; |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 403 | int rc; |
Phoebe Buckheister | b70ab2e | 2014-03-14 21:23:59 +0100 | [diff] [blame] | 404 | |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 405 | pr_debug("getting packet via slave interface %s\n", sdata->dev->name); |
| 406 | |
| 407 | spin_lock_bh(&sdata->mib_lock); |
| 408 | |
Phoebe Buckheister | ae531b9 | 2014-03-14 21:24:02 +0100 | [diff] [blame] | 409 | span = sdata->pan_id; |
| 410 | sshort = sdata->short_addr; |
Phoebe Buckheister | b70ab2e | 2014-03-14 21:23:59 +0100 | [diff] [blame] | 411 | |
Phoebe Buckheister | ae531b9 | 2014-03-14 21:24:02 +0100 | [diff] [blame] | 412 | switch (mac_cb(skb)->dest.mode) { |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 413 | case IEEE802154_ADDR_NONE: |
Phoebe Buckheister | ae531b9 | 2014-03-14 21:24:02 +0100 | [diff] [blame] | 414 | if (mac_cb(skb)->dest.mode != IEEE802154_ADDR_NONE) |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 415 | /* FIXME: check if we are PAN coordinator */ |
| 416 | skb->pkt_type = PACKET_OTHERHOST; |
| 417 | else |
| 418 | /* ACK comes with both addresses empty */ |
| 419 | skb->pkt_type = PACKET_HOST; |
| 420 | break; |
| 421 | case IEEE802154_ADDR_LONG: |
Phoebe Buckheister | ae531b9 | 2014-03-14 21:24:02 +0100 | [diff] [blame] | 422 | if (mac_cb(skb)->dest.pan_id != span && |
| 423 | mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST)) |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 424 | skb->pkt_type = PACKET_OTHERHOST; |
Phoebe Buckheister | ae531b9 | 2014-03-14 21:24:02 +0100 | [diff] [blame] | 425 | else if (mac_cb(skb)->dest.extended_addr == sdata->extended_addr) |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 426 | skb->pkt_type = PACKET_HOST; |
| 427 | else |
| 428 | skb->pkt_type = PACKET_OTHERHOST; |
| 429 | break; |
| 430 | case IEEE802154_ADDR_SHORT: |
Phoebe Buckheister | ae531b9 | 2014-03-14 21:24:02 +0100 | [diff] [blame] | 431 | if (mac_cb(skb)->dest.pan_id != span && |
| 432 | mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST)) |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 433 | skb->pkt_type = PACKET_OTHERHOST; |
Phoebe Buckheister | ae531b9 | 2014-03-14 21:24:02 +0100 | [diff] [blame] | 434 | else if (mac_cb(skb)->dest.short_addr == sshort) |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 435 | skb->pkt_type = PACKET_HOST; |
Phoebe Buckheister | ae531b9 | 2014-03-14 21:24:02 +0100 | [diff] [blame] | 436 | else if (mac_cb(skb)->dest.short_addr == |
| 437 | cpu_to_le16(IEEE802154_ADDR_BROADCAST)) |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 438 | skb->pkt_type = PACKET_BROADCAST; |
| 439 | else |
| 440 | skb->pkt_type = PACKET_OTHERHOST; |
| 441 | break; |
| 442 | default: |
| 443 | break; |
| 444 | } |
| 445 | |
| 446 | spin_unlock_bh(&sdata->mib_lock); |
| 447 | |
| 448 | skb->dev = sdata->dev; |
| 449 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 450 | rc = mac802154_llsec_decrypt(&sdata->sec, skb); |
| 451 | if (rc) { |
| 452 | pr_debug("decryption failed: %i\n", rc); |
| 453 | return NET_RX_DROP; |
| 454 | } |
| 455 | |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 456 | sdata->dev->stats.rx_packets++; |
| 457 | sdata->dev->stats.rx_bytes += skb->len; |
| 458 | |
Phoebe Buckheister | 32edc40 | 2014-05-14 17:43:08 +0200 | [diff] [blame] | 459 | switch (mac_cb(skb)->type) { |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 460 | case IEEE802154_FC_TYPE_DATA: |
| 461 | return mac802154_process_data(sdata->dev, skb); |
| 462 | default: |
Weilong Chen | 2cc33c7 | 2013-12-19 09:22:26 +0800 | [diff] [blame] | 463 | pr_warn("ieee802154: bad frame received (type = %d)\n", |
Phoebe Buckheister | 32edc40 | 2014-05-14 17:43:08 +0200 | [diff] [blame] | 464 | mac_cb(skb)->type); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 465 | kfree_skb(skb); |
| 466 | return NET_RX_DROP; |
| 467 | } |
| 468 | } |
| 469 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 470 | static void mac802154_print_addr(const char *name, |
| 471 | const struct ieee802154_addr *addr) |
| 472 | { |
| 473 | if (addr->mode == IEEE802154_ADDR_NONE) |
| 474 | pr_debug("%s not present\n", name); |
| 475 | |
| 476 | pr_debug("%s PAN ID: %04x\n", name, le16_to_cpu(addr->pan_id)); |
| 477 | if (addr->mode == IEEE802154_ADDR_SHORT) { |
| 478 | pr_debug("%s is short: %04x\n", name, |
| 479 | le16_to_cpu(addr->short_addr)); |
| 480 | } else { |
| 481 | u64 hw = swab64((__force u64) addr->extended_addr); |
| 482 | |
| 483 | pr_debug("%s is hardware: %8phC\n", name, &hw); |
| 484 | } |
| 485 | } |
| 486 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 487 | static int mac802154_parse_frame_start(struct sk_buff *skb, |
| 488 | struct ieee802154_hdr *hdr) |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 489 | { |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 490 | int hlen; |
Phoebe Buckheister | 32edc40 | 2014-05-14 17:43:08 +0200 | [diff] [blame] | 491 | struct ieee802154_mac_cb *cb = mac_cb_init(skb); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 492 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 493 | hlen = ieee802154_hdr_pull(skb, hdr); |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 494 | if (hlen < 0) |
| 495 | return -EINVAL; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 496 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 497 | skb->mac_len = hlen; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 498 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 499 | pr_debug("fc: %04x dsn: %02x\n", le16_to_cpup((__le16 *)&hdr->fc), |
| 500 | hdr->seq); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 501 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 502 | cb->type = hdr->fc.type; |
| 503 | cb->ackreq = hdr->fc.ack_request; |
| 504 | cb->secen = hdr->fc.security_enabled; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 505 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 506 | mac802154_print_addr("destination", &hdr->dest); |
| 507 | mac802154_print_addr("source", &hdr->source); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 508 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 509 | cb->source = hdr->source; |
| 510 | cb->dest = hdr->dest; |
Phoebe Buckheister | ae531b9 | 2014-03-14 21:24:02 +0100 | [diff] [blame] | 511 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 512 | if (hdr->fc.security_enabled) { |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 513 | u64 key; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 514 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 515 | pr_debug("seclevel %i\n", hdr->sec.level); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 516 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 517 | switch (hdr->sec.key_id_mode) { |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 518 | case IEEE802154_SCF_KEY_IMPLICIT: |
| 519 | pr_debug("implicit key\n"); |
| 520 | break; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 521 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 522 | case IEEE802154_SCF_KEY_INDEX: |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 523 | pr_debug("key %02x\n", hdr->sec.key_id); |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 524 | break; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 525 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 526 | case IEEE802154_SCF_KEY_SHORT_INDEX: |
| 527 | pr_debug("key %04x:%04x %02x\n", |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 528 | le32_to_cpu(hdr->sec.short_src) >> 16, |
| 529 | le32_to_cpu(hdr->sec.short_src) & 0xffff, |
| 530 | hdr->sec.key_id); |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 531 | break; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 532 | |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 533 | case IEEE802154_SCF_KEY_HW_INDEX: |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 534 | key = swab64((__force u64) hdr->sec.extended_src); |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 535 | pr_debug("key source %8phC %02x\n", &key, |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 536 | hdr->sec.key_id); |
Phoebe Buckheister | e6278d9 | 2014-03-14 21:24:01 +0100 | [diff] [blame] | 537 | break; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 538 | } |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 539 | } |
| 540 | |
| 541 | return 0; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 542 | } |
| 543 | |
| 544 | void mac802154_wpans_rx(struct mac802154_priv *priv, struct sk_buff *skb) |
| 545 | { |
| 546 | int ret; |
| 547 | struct sk_buff *sskb; |
| 548 | struct mac802154_sub_if_data *sdata; |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 549 | struct ieee802154_hdr hdr; |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 550 | |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 551 | ret = mac802154_parse_frame_start(skb, &hdr); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 552 | if (ret) { |
| 553 | pr_debug("got invalid frame\n"); |
| 554 | return; |
| 555 | } |
| 556 | |
| 557 | rcu_read_lock(); |
| 558 | list_for_each_entry_rcu(sdata, &priv->slaves, list) { |
| 559 | if (sdata->type != IEEE802154_DEV_WPAN) |
| 560 | continue; |
| 561 | |
| 562 | sskb = skb_clone(skb, GFP_ATOMIC); |
| 563 | if (sskb) |
Phoebe Buckheister | f30be4d | 2014-05-16 17:46:40 +0200 | [diff] [blame] | 564 | mac802154_subif_frame(sdata, sskb, &hdr); |
alex.bluesman.smirnov@gmail.com | 32bad7e | 2012-06-25 23:24:48 +0000 | [diff] [blame] | 565 | } |
| 566 | rcu_read_unlock(); |
| 567 | } |