Pileus Git - ~andy/linux/blob - drivers/net/wireless/mac80211_hwsim.c

   1 /*
   2  * mac80211_hwsim - software simulator of 802.11 radio(s) for mac80211
   3  * Copyright (c) 2008, Jouni Malinen <j@w1.fi>
   4  * Copyright (c) 2011, Javier Lopez <jlopex@gmail.com>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   9  */
  10
  11 /*
  12  * TODO:
  13  * - Add TSF sync and fix IBSS beacon transmission by adding
  14  *   competition for "air time" at TBTT
  15  * - RX filtering based on filter configuration (data->rx_filter)
  16  */
  17
  18 #include <linux/list.h>
  19 #include <linux/slab.h>
  20 #include <linux/spinlock.h>
  21 #include <net/dst.h>
  22 #include <net/xfrm.h>
  23 #include <net/mac80211.h>
  24 #include <net/ieee80211_radiotap.h>
  25 #include <linux/if_arp.h>
  26 #include <linux/rtnetlink.h>
  27 #include <linux/etherdevice.h>
  28 #include <linux/platform_device.h>
  29 #include <linux/debugfs.h>
  30 #include <linux/module.h>
  31 #include <linux/ktime.h>
  32 #include <net/genetlink.h>
  33 #include "mac80211_hwsim.h"
  34
  35 #define WARN_QUEUE 100
  36 #define MAX_QUEUE 200
  37
  38 MODULE_AUTHOR("Jouni Malinen");
  39 MODULE_DESCRIPTION("Software simulator of 802.11 radio(s) for mac80211");
  40 MODULE_LICENSE("GPL");
  41
  42 static u32 wmediumd_portid;
  43
  44 static int radios = 2;
  45 module_param(radios, int, 0444);
  46 MODULE_PARM_DESC(radios, "Number of simulated radios");
  47
  48 static int channels = 1;
  49 module_param(channels, int, 0444);
  50 MODULE_PARM_DESC(channels, "Number of concurrent channels");
  51
  52 static bool paged_rx = false;
  53 module_param(paged_rx, bool, 0644);
  54 MODULE_PARM_DESC(paged_rx, "Use paged SKBs for RX instead of linear ones");
  55
  56 static bool rctbl = false;
  57 module_param(rctbl, bool, 0444);
  58 MODULE_PARM_DESC(rctbl, "Handle rate control table");
  59
  60 struct hwsim_vif_priv {
  61         u32 magic;
  62         u8 bssid[ETH_ALEN];
  63         bool assoc;
  64         bool bcn_en;
  65         u16 aid;
  66 };
  67
  68 #define HWSIM_VIF_MAGIC 0x69537748
  69
  70 static inline void hwsim_check_magic(struct ieee80211_vif *vif)
  71 {
  72         struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
  73         WARN(vp->magic != HWSIM_VIF_MAGIC,
  74              "Invalid VIF (%p) magic %#x, %pM, %d/%d\n",
  75              vif, vp->magic, vif->addr, vif->type, vif->p2p);
  76 }
  77
  78 static inline void hwsim_set_magic(struct ieee80211_vif *vif)
  79 {
  80         struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
  81         vp->magic = HWSIM_VIF_MAGIC;
  82 }
  83
  84 static inline void hwsim_clear_magic(struct ieee80211_vif *vif)
  85 {
  86         struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
  87         vp->magic = 0;
  88 }
  89
  90 struct hwsim_sta_priv {
  91         u32 magic;
  92 };
  93
  94 #define HWSIM_STA_MAGIC 0x6d537749
  95
  96 static inline void hwsim_check_sta_magic(struct ieee80211_sta *sta)
  97 {
  98         struct hwsim_sta_priv *sp = (void *)sta->drv_priv;
  99         WARN_ON(sp->magic != HWSIM_STA_MAGIC);
 100 }
 101
 102 static inline void hwsim_set_sta_magic(struct ieee80211_sta *sta)
 103 {
 104         struct hwsim_sta_priv *sp = (void *)sta->drv_priv;
 105         sp->magic = HWSIM_STA_MAGIC;
 106 }
 107
 108 static inline void hwsim_clear_sta_magic(struct ieee80211_sta *sta)
 109 {
 110         struct hwsim_sta_priv *sp = (void *)sta->drv_priv;
 111         sp->magic = 0;
 112 }
 113
 114 struct hwsim_chanctx_priv {
 115         u32 magic;
 116 };
 117
 118 #define HWSIM_CHANCTX_MAGIC 0x6d53774a
 119
 120 static inline void hwsim_check_chanctx_magic(struct ieee80211_chanctx_conf *c)
 121 {
 122         struct hwsim_chanctx_priv *cp = (void *)c->drv_priv;
 123         WARN_ON(cp->magic != HWSIM_CHANCTX_MAGIC);
 124 }
 125
 126 static inline void hwsim_set_chanctx_magic(struct ieee80211_chanctx_conf *c)
 127 {
 128         struct hwsim_chanctx_priv *cp = (void *)c->drv_priv;
 129         cp->magic = HWSIM_CHANCTX_MAGIC;
 130 }
 131
 132 static inline void hwsim_clear_chanctx_magic(struct ieee80211_chanctx_conf *c)
 133 {
 134         struct hwsim_chanctx_priv *cp = (void *)c->drv_priv;
 135         cp->magic = 0;
 136 }
 137
 138 static struct class *hwsim_class;
 139
 140 static struct net_device *hwsim_mon; /* global monitor netdev */
 141
 142 #define CHAN2G(_freq)  { \
 143         .band = IEEE80211_BAND_2GHZ, \
 144         .center_freq = (_freq), \
 145         .hw_value = (_freq), \
 146         .max_power = 20, \
 147 }
 148
 149 #define CHAN5G(_freq) { \
 150         .band = IEEE80211_BAND_5GHZ, \
 151         .center_freq = (_freq), \
 152         .hw_value = (_freq), \
 153         .max_power = 20, \
 154 }
 155
 156 static const struct ieee80211_channel hwsim_channels_2ghz[] = {
 157         CHAN2G(2412), /* Channel 1 */
 158         CHAN2G(2417), /* Channel 2 */
 159         CHAN2G(2422), /* Channel 3 */
 160         CHAN2G(2427), /* Channel 4 */
 161         CHAN2G(2432), /* Channel 5 */
 162         CHAN2G(2437), /* Channel 6 */
 163         CHAN2G(2442), /* Channel 7 */
 164         CHAN2G(2447), /* Channel 8 */
 165         CHAN2G(2452), /* Channel 9 */
 166         CHAN2G(2457), /* Channel 10 */
 167         CHAN2G(2462), /* Channel 11 */
 168         CHAN2G(2467), /* Channel 12 */
 169         CHAN2G(2472), /* Channel 13 */
 170         CHAN2G(2484), /* Channel 14 */
 171 };
 172
 173 static const struct ieee80211_channel hwsim_channels_5ghz[] = {
 174         CHAN5G(5180), /* Channel 36 */
 175         CHAN5G(5200), /* Channel 40 */
 176         CHAN5G(5220), /* Channel 44 */
 177         CHAN5G(5240), /* Channel 48 */
 178
 179         CHAN5G(5260), /* Channel 52 */
 180         CHAN5G(5280), /* Channel 56 */
 181         CHAN5G(5300), /* Channel 60 */
 182         CHAN5G(5320), /* Channel 64 */
 183
 184         CHAN5G(5500), /* Channel 100 */
 185         CHAN5G(5520), /* Channel 104 */
 186         CHAN5G(5540), /* Channel 108 */
 187         CHAN5G(5560), /* Channel 112 */
 188         CHAN5G(5580), /* Channel 116 */
 189         CHAN5G(5600), /* Channel 120 */
 190         CHAN5G(5620), /* Channel 124 */
 191         CHAN5G(5640), /* Channel 128 */
 192         CHAN5G(5660), /* Channel 132 */
 193         CHAN5G(5680), /* Channel 136 */
 194         CHAN5G(5700), /* Channel 140 */
 195
 196         CHAN5G(5745), /* Channel 149 */
 197         CHAN5G(5765), /* Channel 153 */
 198         CHAN5G(5785), /* Channel 157 */
 199         CHAN5G(5805), /* Channel 161 */
 200         CHAN5G(5825), /* Channel 165 */
 201 };
 202
 203 static const struct ieee80211_rate hwsim_rates[] = {
 204         { .bitrate = 10 },
 205         { .bitrate = 20, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
 206         { .bitrate = 55, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
 207         { .bitrate = 110, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
 208         { .bitrate = 60 },
 209         { .bitrate = 90 },
 210         { .bitrate = 120 },
 211         { .bitrate = 180 },
 212         { .bitrate = 240 },
 213         { .bitrate = 360 },
 214         { .bitrate = 480 },
 215         { .bitrate = 540 }
 216 };
 217
 218 static const struct ieee80211_iface_limit hwsim_if_limits[] = {
 219         { .max = 1, .types = BIT(NL80211_IFTYPE_ADHOC) },
 220         { .max = 2048,  .types = BIT(NL80211_IFTYPE_STATION) |
 221                                  BIT(NL80211_IFTYPE_P2P_CLIENT) |
 222 #ifdef CONFIG_MAC80211_MESH
 223                                  BIT(NL80211_IFTYPE_MESH_POINT) |
 224 #endif
 225                                  BIT(NL80211_IFTYPE_AP) |
 226                                  BIT(NL80211_IFTYPE_P2P_GO) },
 227         { .max = 1, .types = BIT(NL80211_IFTYPE_P2P_DEVICE) },
 228 };
 229
 230 static const struct ieee80211_iface_limit hwsim_if_dfs_limits[] = {
 231         { .max = 8, .types = BIT(NL80211_IFTYPE_AP) },
 232 };
 233
 234 static const struct ieee80211_iface_combination hwsim_if_comb[] = {
 235         {
 236                 .limits = hwsim_if_limits,
 237                 .n_limits = ARRAY_SIZE(hwsim_if_limits),
 238                 .max_interfaces = 2048,
 239                 .num_different_channels = 1,
 240         },
 241         {
 242                 .limits = hwsim_if_dfs_limits,
 243                 .n_limits = ARRAY_SIZE(hwsim_if_dfs_limits),
 244                 .max_interfaces = 8,
 245                 .num_different_channels = 1,
 246                 .radar_detect_widths = BIT(NL80211_CHAN_WIDTH_20_NOHT) |
 247                                        BIT(NL80211_CHAN_WIDTH_20) |
 248                                        BIT(NL80211_CHAN_WIDTH_40) |
 249                                        BIT(NL80211_CHAN_WIDTH_80) |
 250                                        BIT(NL80211_CHAN_WIDTH_160),
 251         }
 252 };
 253
 254 static spinlock_t hwsim_radio_lock;
 255 static struct list_head hwsim_radios;
 256 static int hwsim_radio_idx;
 257
 258 static struct platform_driver mac80211_hwsim_driver = {
 259         .driver = {
 260                 .name = "mac80211_hwsim",
 261                 .owner = THIS_MODULE,
 262         },
 263 };
 264
 265 struct mac80211_hwsim_data {
 266         struct list_head list;
 267         struct ieee80211_hw *hw;
 268         struct device *dev;
 269         struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
 270         struct ieee80211_channel channels_2ghz[ARRAY_SIZE(hwsim_channels_2ghz)];
 271         struct ieee80211_channel channels_5ghz[ARRAY_SIZE(hwsim_channels_5ghz)];
 272         struct ieee80211_rate rates[ARRAY_SIZE(hwsim_rates)];
 273         struct ieee80211_iface_combination if_combination;
 274
 275         struct mac_address addresses[2];
 276         int channels, idx;
 277
 278         struct ieee80211_channel *tmp_chan;
 279         struct delayed_work roc_done;
 280         struct delayed_work hw_scan;
 281         struct cfg80211_scan_request *hw_scan_request;
 282         struct ieee80211_vif *hw_scan_vif;
 283         int scan_chan_idx;
 284
 285         struct ieee80211_channel *channel;
 286         u64 beacon_int  /* beacon interval in us */;
 287         unsigned int rx_filter;
 288         bool started, idle, scanning;
 289         struct mutex mutex;
 290         struct tasklet_hrtimer beacon_timer;
 291         enum ps_mode {
 292                 PS_DISABLED, PS_ENABLED, PS_AUTO_POLL, PS_MANUAL_POLL
 293         } ps;
 294         bool ps_poll_pending;
 295         struct dentry *debugfs;
 296
 297         struct sk_buff_head pending;    /* packets pending */
 298         /*
 299          * Only radios in the same group can communicate together (the
 300          * channel has to match too). Each bit represents a group. A
 301          * radio can be in more then one group.
 302          */
 303         u64 group;
 304
 305         int power_level;
 306
 307         /* difference between this hw's clock and the real clock, in usecs */
 308         s64 tsf_offset;
 309         s64 bcn_delta;
 310         /* absolute beacon transmission time. Used to cover up "tx" delay. */
 311         u64 abs_bcn_ts;
 312 };
 313
 314
 315 struct hwsim_radiotap_hdr {
 316         struct ieee80211_radiotap_header hdr;
 317         __le64 rt_tsft;
 318         u8 rt_flags;
 319         u8 rt_rate;
 320         __le16 rt_channel;
 321         __le16 rt_chbitmask;
 322 } __packed;
 323
 324 struct hwsim_radiotap_ack_hdr {
 325         struct ieee80211_radiotap_header hdr;
 326         u8 rt_flags;
 327         u8 pad;
 328         __le16 rt_channel;
 329         __le16 rt_chbitmask;
 330 } __packed;
 331
 332 /* MAC80211_HWSIM netlinf family */
 333 static struct genl_family hwsim_genl_family = {
 334         .id = GENL_ID_GENERATE,
 335         .hdrsize = 0,
 336         .name = "MAC80211_HWSIM",
 337         .version = 1,
 338         .maxattr = HWSIM_ATTR_MAX,
 339 };
 340
 341 /* MAC80211_HWSIM netlink policy */
 342
 343 static struct nla_policy hwsim_genl_policy[HWSIM_ATTR_MAX + 1] = {
 344         [HWSIM_ATTR_ADDR_RECEIVER] = { .type = NLA_UNSPEC, .len = ETH_ALEN },
 345         [HWSIM_ATTR_ADDR_TRANSMITTER] = { .type = NLA_UNSPEC, .len = ETH_ALEN },
 346         [HWSIM_ATTR_FRAME] = { .type = NLA_BINARY,
 347                                .len = IEEE80211_MAX_DATA_LEN },
 348         [HWSIM_ATTR_FLAGS] = { .type = NLA_U32 },
 349         [HWSIM_ATTR_RX_RATE] = { .type = NLA_U32 },
 350         [HWSIM_ATTR_SIGNAL] = { .type = NLA_U32 },
 351         [HWSIM_ATTR_TX_INFO] = { .type = NLA_UNSPEC,
 352                                  .len = IEEE80211_TX_MAX_RATES *
 353                                         sizeof(struct hwsim_tx_rate)},
 354         [HWSIM_ATTR_COOKIE] = { .type = NLA_U64 },
 355         [HWSIM_ATTR_CHANNELS] = { .type = NLA_U32 },
 356         [HWSIM_ATTR_RADIO_ID] = { .type = NLA_U32 },
 357 };
 358
 359 static void mac80211_hwsim_tx_frame(struct ieee80211_hw *hw,
 360                                     struct sk_buff *skb,
 361                                     struct ieee80211_channel *chan);
 362
 363 /* sysfs attributes */
 364 static void hwsim_send_ps_poll(void *dat, u8 *mac, struct ieee80211_vif *vif)
 365 {
 366         struct mac80211_hwsim_data *data = dat;
 367         struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
 368         struct sk_buff *skb;
 369         struct ieee80211_pspoll *pspoll;
 370
 371         if (!vp->assoc)
 372                 return;
 373
 374         wiphy_debug(data->hw->wiphy,
 375                     "%s: send PS-Poll to %pM for aid %d\n",
 376                     __func__, vp->bssid, vp->aid);
 377
 378         skb = dev_alloc_skb(sizeof(*pspoll));
 379         if (!skb)
 380                 return;
 381         pspoll = (void *) skb_put(skb, sizeof(*pspoll));
 382         pspoll->frame_control = cpu_to_le16(IEEE80211_FTYPE_CTL |
 383                                             IEEE80211_STYPE_PSPOLL |
 384                                             IEEE80211_FCTL_PM);
 385         pspoll->aid = cpu_to_le16(0xc000 | vp->aid);
 386         memcpy(pspoll->bssid, vp->bssid, ETH_ALEN);
 387         memcpy(pspoll->ta, mac, ETH_ALEN);
 388
 389         rcu_read_lock();
 390         mac80211_hwsim_tx_frame(data->hw, skb,
 391                                 rcu_dereference(vif->chanctx_conf)->def.chan);
 392         rcu_read_unlock();
 393 }
 394
 395 static void hwsim_send_nullfunc(struct mac80211_hwsim_data *data, u8 *mac,
 396                                 struct ieee80211_vif *vif, int ps)
 397 {
 398         struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
 399         struct sk_buff *skb;
 400         struct ieee80211_hdr *hdr;
 401
 402         if (!vp->assoc)
 403                 return;
 404
 405         wiphy_debug(data->hw->wiphy,
 406                     "%s: send data::nullfunc to %pM ps=%d\n",
 407                     __func__, vp->bssid, ps);
 408
 409         skb = dev_alloc_skb(sizeof(*hdr));
 410         if (!skb)
 411                 return;
 412         hdr = (void *) skb_put(skb, sizeof(*hdr) - ETH_ALEN);
 413         hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
 414                                          IEEE80211_STYPE_NULLFUNC |
 415                                          (ps ? IEEE80211_FCTL_PM : 0));
 416         hdr->duration_id = cpu_to_le16(0);
 417         memcpy(hdr->addr1, vp->bssid, ETH_ALEN);
 418         memcpy(hdr->addr2, mac, ETH_ALEN);
 419         memcpy(hdr->addr3, vp->bssid, ETH_ALEN);
 420
 421         rcu_read_lock();
 422         mac80211_hwsim_tx_frame(data->hw, skb,
 423                                 rcu_dereference(vif->chanctx_conf)->def.chan);
 424         rcu_read_unlock();
 425 }
 426
 427
 428 static void hwsim_send_nullfunc_ps(void *dat, u8 *mac,
 429                                    struct ieee80211_vif *vif)
 430 {
 431         struct mac80211_hwsim_data *data = dat;
 432         hwsim_send_nullfunc(data, mac, vif, 1);
 433 }
 434
 435 static void hwsim_send_nullfunc_no_ps(void *dat, u8 *mac,
 436                                       struct ieee80211_vif *vif)
 437 {
 438         struct mac80211_hwsim_data *data = dat;
 439         hwsim_send_nullfunc(data, mac, vif, 0);
 440 }
 441
 442 static int hwsim_fops_ps_read(void *dat, u64 *val)
 443 {
 444         struct mac80211_hwsim_data *data = dat;
 445         *val = data->ps;
 446         return 0;
 447 }
 448
 449 static int hwsim_fops_ps_write(void *dat, u64 val)
 450 {
 451         struct mac80211_hwsim_data *data = dat;
 452         enum ps_mode old_ps;
 453
 454         if (val != PS_DISABLED && val != PS_ENABLED && val != PS_AUTO_POLL &&
 455             val != PS_MANUAL_POLL)
 456                 return -EINVAL;
 457
 458         old_ps = data->ps;
 459         data->ps = val;
 460
 461         if (val == PS_MANUAL_POLL) {
 462                 ieee80211_iterate_active_interfaces(data->hw,
 463                                                     IEEE80211_IFACE_ITER_NORMAL,
 464                                                     hwsim_send_ps_poll, data);
 465                 data->ps_poll_pending = true;
 466         } else if (old_ps == PS_DISABLED && val != PS_DISABLED) {
 467                 ieee80211_iterate_active_interfaces(data->hw,
 468                                                     IEEE80211_IFACE_ITER_NORMAL,
 469                                                     hwsim_send_nullfunc_ps,
 470                                                     data);
 471         } else if (old_ps != PS_DISABLED && val == PS_DISABLED) {
 472                 ieee80211_iterate_active_interfaces(data->hw,
 473                                                     IEEE80211_IFACE_ITER_NORMAL,
 474                                                     hwsim_send_nullfunc_no_ps,
 475                                                     data);
 476         }
 477
 478         return 0;
 479 }
 480
 481 DEFINE_SIMPLE_ATTRIBUTE(hwsim_fops_ps, hwsim_fops_ps_read, hwsim_fops_ps_write,
 482                         "%llu\n");
 483
 484 static int hwsim_write_simulate_radar(void *dat, u64 val)
 485 {
 486         struct mac80211_hwsim_data *data = dat;
 487
 488         ieee80211_radar_detected(data->hw);
 489
 490         return 0;
 491 }
 492
 493 DEFINE_SIMPLE_ATTRIBUTE(hwsim_simulate_radar, NULL,
 494                         hwsim_write_simulate_radar, "%llu\n");
 495
 496 static int hwsim_fops_group_read(void *dat, u64 *val)
 497 {
 498         struct mac80211_hwsim_data *data = dat;
 499         *val = data->group;
 500         return 0;
 501 }
 502
 503 static int hwsim_fops_group_write(void *dat, u64 val)
 504 {
 505         struct mac80211_hwsim_data *data = dat;
 506         data->group = val;
 507         return 0;
 508 }
 509
 510 DEFINE_SIMPLE_ATTRIBUTE(hwsim_fops_group,
 511                         hwsim_fops_group_read, hwsim_fops_group_write,
 512                         "%llx\n");
 513
 514 static netdev_tx_t hwsim_mon_xmit(struct sk_buff *skb,
 515                                         struct net_device *dev)
 516 {
 517         /* TODO: allow packet injection */
 518         dev_kfree_skb(skb);
 519         return NETDEV_TX_OK;
 520 }
 521
 522 static inline u64 mac80211_hwsim_get_tsf_raw(void)
 523 {
 524         return ktime_to_us(ktime_get_real());
 525 }
 526
 527 static __le64 __mac80211_hwsim_get_tsf(struct mac80211_hwsim_data *data)
 528 {
 529         u64 now = mac80211_hwsim_get_tsf_raw();
 530         return cpu_to_le64(now + data->tsf_offset);
 531 }
 532
 533 static u64 mac80211_hwsim_get_tsf(struct ieee80211_hw *hw,
 534                                   struct ieee80211_vif *vif)
 535 {
 536         struct mac80211_hwsim_data *data = hw->priv;
 537         return le64_to_cpu(__mac80211_hwsim_get_tsf(data));
 538 }
 539
 540 static void mac80211_hwsim_set_tsf(struct ieee80211_hw *hw,
 541                 struct ieee80211_vif *vif, u64 tsf)
 542 {
 543         struct mac80211_hwsim_data *data = hw->priv;
 544         u64 now = mac80211_hwsim_get_tsf(hw, vif);
 545         u32 bcn_int = data->beacon_int;
 546         s64 delta = tsf - now;
 547
 548         data->tsf_offset += delta;
 549         /* adjust after beaconing with new timestamp at old TBTT */
 550         data->bcn_delta = do_div(delta, bcn_int);
 551 }
 552
 553 static void mac80211_hwsim_monitor_rx(struct ieee80211_hw *hw,
 554                                       struct sk_buff *tx_skb,
 555                                       struct ieee80211_channel *chan)
 556 {
 557         struct mac80211_hwsim_data *data = hw->priv;
 558         struct sk_buff *skb;
 559         struct hwsim_radiotap_hdr *hdr;
 560         u16 flags;
 561         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_skb);
 562         struct ieee80211_rate *txrate = ieee80211_get_tx_rate(hw, info);
 563
 564         if (!netif_running(hwsim_mon))
 565                 return;
 566
 567         skb = skb_copy_expand(tx_skb, sizeof(*hdr), 0, GFP_ATOMIC);
 568         if (skb == NULL)
 569                 return;
 570
 571         hdr = (struct hwsim_radiotap_hdr *) skb_push(skb, sizeof(*hdr));
 572         hdr->hdr.it_version = PKTHDR_RADIOTAP_VERSION;
 573         hdr->hdr.it_pad = 0;
 574         hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
 575         hdr->hdr.it_present = cpu_to_le32((1 << IEEE80211_RADIOTAP_FLAGS) |
 576                                           (1 << IEEE80211_RADIOTAP_RATE) |
 577                                           (1 << IEEE80211_RADIOTAP_TSFT) |
 578                                           (1 << IEEE80211_RADIOTAP_CHANNEL));
 579         hdr->rt_tsft = __mac80211_hwsim_get_tsf(data);
 580         hdr->rt_flags = 0;
 581         hdr->rt_rate = txrate->bitrate / 5;
 582         hdr->rt_channel = cpu_to_le16(chan->center_freq);
 583         flags = IEEE80211_CHAN_2GHZ;
 584         if (txrate->flags & IEEE80211_RATE_ERP_G)
 585                 flags |= IEEE80211_CHAN_OFDM;
 586         else
 587                 flags |= IEEE80211_CHAN_CCK;
 588         hdr->rt_chbitmask = cpu_to_le16(flags);
 589
 590         skb->dev = hwsim_mon;
 591         skb_set_mac_header(skb, 0);
 592         skb->ip_summed = CHECKSUM_UNNECESSARY;
 593         skb->pkt_type = PACKET_OTHERHOST;
 594         skb->protocol = htons(ETH_P_802_2);
 595         memset(skb->cb, 0, sizeof(skb->cb));
 596         netif_rx(skb);
 597 }
 598
 599
 600 static void mac80211_hwsim_monitor_ack(struct ieee80211_channel *chan,
 601                                        const u8 *addr)
 602 {
 603         struct sk_buff *skb;
 604         struct hwsim_radiotap_ack_hdr *hdr;
 605         u16 flags;
 606         struct ieee80211_hdr *hdr11;
 607
 608         if (!netif_running(hwsim_mon))
 609                 return;
 610
 611         skb = dev_alloc_skb(100);
 612         if (skb == NULL)
 613                 return;
 614
 615         hdr = (struct hwsim_radiotap_ack_hdr *) skb_put(skb, sizeof(*hdr));
 616         hdr->hdr.it_version = PKTHDR_RADIOTAP_VERSION;
 617         hdr->hdr.it_pad = 0;
 618         hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
 619         hdr->hdr.it_present = cpu_to_le32((1 << IEEE80211_RADIOTAP_FLAGS) |
 620                                           (1 << IEEE80211_RADIOTAP_CHANNEL));
 621         hdr->rt_flags = 0;
 622         hdr->pad = 0;
 623         hdr->rt_channel = cpu_to_le16(chan->center_freq);
 624         flags = IEEE80211_CHAN_2GHZ;
 625         hdr->rt_chbitmask = cpu_to_le16(flags);
 626
 627         hdr11 = (struct ieee80211_hdr *) skb_put(skb, 10);
 628         hdr11->frame_control = cpu_to_le16(IEEE80211_FTYPE_CTL |
 629                                            IEEE80211_STYPE_ACK);
 630         hdr11->duration_id = cpu_to_le16(0);
 631         memcpy(hdr11->addr1, addr, ETH_ALEN);
 632
 633         skb->dev = hwsim_mon;
 634         skb_set_mac_header(skb, 0);
 635         skb->ip_summed = CHECKSUM_UNNECESSARY;
 636         skb->pkt_type = PACKET_OTHERHOST;
 637         skb->protocol = htons(ETH_P_802_2);
 638         memset(skb->cb, 0, sizeof(skb->cb));
 639         netif_rx(skb);
 640 }
 641
 642
 643 static bool hwsim_ps_rx_ok(struct mac80211_hwsim_data *data,
 644                            struct sk_buff *skb)
 645 {
 646         switch (data->ps) {
 647         case PS_DISABLED:
 648                 return true;
 649         case PS_ENABLED:
 650                 return false;
 651         case PS_AUTO_POLL:
 652                 /* TODO: accept (some) Beacons by default and other frames only
 653                  * if pending PS-Poll has been sent */
 654                 return true;
 655         case PS_MANUAL_POLL:
 656                 /* Allow unicast frames to own address if there is a pending
 657                  * PS-Poll */
 658                 if (data->ps_poll_pending &&
 659                     memcmp(data->hw->wiphy->perm_addr, skb->data + 4,
 660                            ETH_ALEN) == 0) {
 661                         data->ps_poll_pending = false;
 662                         return true;
 663                 }
 664                 return false;
 665         }
 666
 667         return true;
 668 }
 669
 670
 671 struct mac80211_hwsim_addr_match_data {
 672         bool ret;
 673         const u8 *addr;
 674 };
 675
 676 static void mac80211_hwsim_addr_iter(void *data, u8 *mac,
 677                                      struct ieee80211_vif *vif)
 678 {
 679         struct mac80211_hwsim_addr_match_data *md = data;
 680         if (memcmp(mac, md->addr, ETH_ALEN) == 0)
 681                 md->ret = true;
 682 }
 683
 684
 685 static bool mac80211_hwsim_addr_match(struct mac80211_hwsim_data *data,
 686                                       const u8 *addr)
 687 {
 688         struct mac80211_hwsim_addr_match_data md;
 689
 690         if (memcmp(addr, data->hw->wiphy->perm_addr, ETH_ALEN) == 0)
 691                 return true;
 692
 693         md.ret = false;
 694         md.addr = addr;
 695         ieee80211_iterate_active_interfaces_atomic(data->hw,
 696                                                    IEEE80211_IFACE_ITER_NORMAL,
 697                                                    mac80211_hwsim_addr_iter,
 698                                                    &md);
 699
 700         return md.ret;
 701 }
 702
 703 static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
 704                                        struct sk_buff *my_skb,
 705                                        int dst_portid)
 706 {
 707         struct sk_buff *skb;
 708         struct mac80211_hwsim_data *data = hw->priv;
 709         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) my_skb->data;
 710         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(my_skb);
 711         void *msg_head;
 712         unsigned int hwsim_flags = 0;
 713         int i;
 714         struct hwsim_tx_rate tx_attempts[IEEE80211_TX_MAX_RATES];
 715
 716         if (data->ps != PS_DISABLED)
 717                 hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
 718         /* If the queue contains MAX_QUEUE skb's drop some */
 719         if (skb_queue_len(&data->pending) >= MAX_QUEUE) {
 720                 /* Droping until WARN_QUEUE level */
 721                 while (skb_queue_len(&data->pending) >= WARN_QUEUE)
 722                         skb_dequeue(&data->pending);
 723         }
 724
 725         skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_ATOMIC);
 726         if (skb == NULL)
 727                 goto nla_put_failure;
 728
 729         msg_head = genlmsg_put(skb, 0, 0, &hwsim_genl_family, 0,
 730                                HWSIM_CMD_FRAME);
 731         if (msg_head == NULL) {
 732                 printk(KERN_DEBUG "mac80211_hwsim: problem with msg_head\n");
 733                 goto nla_put_failure;
 734         }
 735
 736         if (nla_put(skb, HWSIM_ATTR_ADDR_TRANSMITTER,
 737                     ETH_ALEN, data->addresses[1].addr))
 738                 goto nla_put_failure;
 739
 740         /* We get the skb->data */
 741         if (nla_put(skb, HWSIM_ATTR_FRAME, my_skb->len, my_skb->data))
 742                 goto nla_put_failure;
 743
 744         /* We get the flags for this transmission, and we translate them to
 745            wmediumd flags  */
 746
 747         if (info->flags & IEEE80211_TX_CTL_REQ_TX_STATUS)
 748                 hwsim_flags |= HWSIM_TX_CTL_REQ_TX_STATUS;
 749
 750         if (info->flags & IEEE80211_TX_CTL_NO_ACK)
 751                 hwsim_flags |= HWSIM_TX_CTL_NO_ACK;
 752
 753         if (nla_put_u32(skb, HWSIM_ATTR_FLAGS, hwsim_flags))
 754                 goto nla_put_failure;
 755
 756         /* We get the tx control (rate and retries) info*/
 757
 758         for (i = 0; i < IEEE80211_TX_MAX_RATES; i++) {
 759                 tx_attempts[i].idx = info->status.rates[i].idx;
 760                 tx_attempts[i].count = info->status.rates[i].count;
 761         }
 762
 763         if (nla_put(skb, HWSIM_ATTR_TX_INFO,
 764                     sizeof(struct hwsim_tx_rate)*IEEE80211_TX_MAX_RATES,
 765                     tx_attempts))
 766                 goto nla_put_failure;
 767
 768         /* We create a cookie to identify this skb */
 769         if (nla_put_u64(skb, HWSIM_ATTR_COOKIE, (unsigned long) my_skb))
 770                 goto nla_put_failure;
 771
 772         genlmsg_end(skb, msg_head);
 773         genlmsg_unicast(&init_net, skb, dst_portid);
 774
 775         /* Enqueue the packet */
 776         skb_queue_tail(&data->pending, my_skb);
 777         return;
 778
 779 nla_put_failure:
 780         printk(KERN_DEBUG "mac80211_hwsim: error occurred in %s\n", __func__);
 781 }
 782
 783 static bool hwsim_chans_compat(struct ieee80211_channel *c1,
 784                                struct ieee80211_channel *c2)
 785 {
 786         if (!c1 || !c2)
 787                 return false;
 788
 789         return c1->center_freq == c2->center_freq;
 790 }
 791
 792 struct tx_iter_data {
 793         struct ieee80211_channel *channel;
 794         bool receive;
 795 };
 796
 797 static void mac80211_hwsim_tx_iter(void *_data, u8 *addr,
 798                                    struct ieee80211_vif *vif)
 799 {
 800         struct tx_iter_data *data = _data;
 801
 802         if (!vif->chanctx_conf)
 803                 return;
 804
 805         if (!hwsim_chans_compat(data->channel,
 806                                 rcu_dereference(vif->chanctx_conf)->def.chan))
 807                 return;
 808
 809         data->receive = true;
 810 }
 811
 812 static bool mac80211_hwsim_tx_frame_no_nl(struct ieee80211_hw *hw,
 813                                           struct sk_buff *skb,
 814                                           struct ieee80211_channel *chan)
 815 {
 816         struct mac80211_hwsim_data *data = hw->priv, *data2;
 817         bool ack = false;
 818         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 819         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 820         struct ieee80211_rx_status rx_status;
 821         u64 now;
 822
 823         memset(&rx_status, 0, sizeof(rx_status));
 824         rx_status.flag |= RX_FLAG_MACTIME_START;
 825         rx_status.freq = chan->center_freq;
 826         rx_status.band = chan->band;
 827         if (info->control.rates[0].flags & IEEE80211_TX_RC_VHT_MCS) {
 828                 rx_status.rate_idx =
 829                         ieee80211_rate_get_vht_mcs(&info->control.rates[0]);
 830                 rx_status.vht_nss =
 831                         ieee80211_rate_get_vht_nss(&info->control.rates[0]);
 832                 rx_status.flag |= RX_FLAG_VHT;
 833         } else {
 834                 rx_status.rate_idx = info->control.rates[0].idx;
 835                 if (info->control.rates[0].flags & IEEE80211_TX_RC_MCS)
 836                         rx_status.flag |= RX_FLAG_HT;
 837         }
 838         if (info->control.rates[0].flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
 839                 rx_status.flag |= RX_FLAG_40MHZ;
 840         if (info->control.rates[0].flags & IEEE80211_TX_RC_SHORT_GI)
 841                 rx_status.flag |= RX_FLAG_SHORT_GI;
 842         /* TODO: simulate real signal strength (and optional packet loss) */
 843         rx_status.signal = data->power_level - 50;
 844
 845         if (data->ps != PS_DISABLED)
 846                 hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
 847
 848         /* release the skb's source info */
 849         skb_orphan(skb);
 850         skb_dst_drop(skb);
 851         skb->mark = 0;
 852         secpath_reset(skb);
 853         nf_reset(skb);
 854
 855         /*
 856          * Get absolute mactime here so all HWs RX at the "same time", and
 857          * absolute TX time for beacon mactime so the timestamp matches.
 858          * Giving beacons a different mactime than non-beacons looks messy, but
 859          * it helps the Toffset be exact and a ~10us mactime discrepancy
 860          * probably doesn't really matter.
 861          */
 862         if (ieee80211_is_beacon(hdr->frame_control) ||
 863             ieee80211_is_probe_resp(hdr->frame_control))
 864                 now = data->abs_bcn_ts;
 865         else
 866                 now = mac80211_hwsim_get_tsf_raw();
 867
 868         /* Copy skb to all enabled radios that are on the current frequency */
 869         spin_lock(&hwsim_radio_lock);
 870         list_for_each_entry(data2, &hwsim_radios, list) {
 871                 struct sk_buff *nskb;
 872                 struct tx_iter_data tx_iter_data = {
 873                         .receive = false,
 874                         .channel = chan,
 875                 };
 876
 877                 if (data == data2)
 878                         continue;
 879
 880                 if (!data2->started || (data2->idle && !data2->tmp_chan) ||
 881                     !hwsim_ps_rx_ok(data2, skb))
 882                         continue;
 883
 884                 if (!(data->group & data2->group))
 885                         continue;
 886
 887                 if (!hwsim_chans_compat(chan, data2->tmp_chan) &&
 888                     !hwsim_chans_compat(chan, data2->channel)) {
 889                         ieee80211_iterate_active_interfaces_atomic(
 890                                 data2->hw, IEEE80211_IFACE_ITER_NORMAL,
 891                                 mac80211_hwsim_tx_iter, &tx_iter_data);
 892                         if (!tx_iter_data.receive)
 893                                 continue;
 894                 }
 895
 896                 /*
 897                  * reserve some space for our vendor and the normal
 898                  * radiotap header, since we're copying anyway
 899                  */
 900                 if (skb->len < PAGE_SIZE && paged_rx) {
 901                         struct page *page = alloc_page(GFP_ATOMIC);
 902
 903                         if (!page)
 904                                 continue;
 905
 906                         nskb = dev_alloc_skb(128);
 907                         if (!nskb) {
 908                                 __free_page(page);
 909                                 continue;
 910                         }
 911
 912                         memcpy(page_address(page), skb->data, skb->len);
 913                         skb_add_rx_frag(nskb, 0, page, 0, skb->len, skb->len);
 914                 } else {
 915                         nskb = skb_copy(skb, GFP_ATOMIC);
 916                         if (!nskb)
 917                                 continue;
 918                 }
 919
 920                 if (mac80211_hwsim_addr_match(data2, hdr->addr1))
 921                         ack = true;
 922
 923                 rx_status.mactime = now + data2->tsf_offset;
 924 #if 0
 925                 /*
 926                  * Don't enable this code by default as the OUI 00:00:00
 927                  * is registered to Xerox so we shouldn't use it here, it
 928                  * might find its way into pcap files.
 929                  * Note that this code requires the headroom in the SKB
 930                  * that was allocated earlier.
 931                  */
 932                 rx_status.vendor_radiotap_oui[0] = 0x00;
 933                 rx_status.vendor_radiotap_oui[1] = 0x00;
 934                 rx_status.vendor_radiotap_oui[2] = 0x00;
 935                 rx_status.vendor_radiotap_subns = 127;
 936                 /*
 937                  * Radiotap vendor namespaces can (and should) also be
 938                  * split into fields by using the standard radiotap
 939                  * presence bitmap mechanism. Use just BIT(0) here for
 940                  * the presence bitmap.
 941                  */
 942                 rx_status.vendor_radiotap_bitmap = BIT(0);
 943                 /* We have 8 bytes of (dummy) data */
 944                 rx_status.vendor_radiotap_len = 8;
 945                 /* For testing, also require it to be aligned */
 946                 rx_status.vendor_radiotap_align = 8;
 947                 /* push the data */
 948                 memcpy(skb_push(nskb, 8), "ABCDEFGH", 8);
 949 #endif
 950
 951                 memcpy(IEEE80211_SKB_RXCB(nskb), &rx_status, sizeof(rx_status));
 952                 ieee80211_rx_irqsafe(data2->hw, nskb);
 953         }
 954         spin_unlock(&hwsim_radio_lock);
 955
 956         return ack;
 957 }
 958
 959 static void mac80211_hwsim_tx(struct ieee80211_hw *hw,
 960                               struct ieee80211_tx_control *control,
 961                               struct sk_buff *skb)
 962 {
 963         struct mac80211_hwsim_data *data = hw->priv;
 964         struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(skb);
 965         struct ieee80211_chanctx_conf *chanctx_conf;
 966         struct ieee80211_channel *channel;
 967         bool ack;
 968         u32 _portid;
 969
 970         if (WARN_ON(skb->len < 10)) {
 971                 /* Should not happen; just a sanity check for addr1 use */
 972                 ieee80211_free_txskb(hw, skb);
 973                 return;
 974         }
 975
 976         if (data->channels == 1) {
 977                 channel = data->channel;
 978         } else if (txi->hw_queue == 4) {
 979                 channel = data->tmp_chan;
 980         } else {
 981                 chanctx_conf = rcu_dereference(txi->control.vif->chanctx_conf);
 982                 if (chanctx_conf)
 983                         channel = chanctx_conf->def.chan;
 984                 else
 985                         channel = NULL;
 986         }
 987
 988         if (WARN(!channel, "TX w/o channel - queue = %d\n", txi->hw_queue)) {
 989                 ieee80211_free_txskb(hw, skb);
 990                 return;
 991         }
 992
 993         if (data->idle && !data->tmp_chan) {
 994                 wiphy_debug(hw->wiphy, "Trying to TX when idle - reject\n");
 995                 ieee80211_free_txskb(hw, skb);
 996                 return;
 997         }
 998
 999         if (txi->control.vif)
1000                 hwsim_check_magic(txi->control.vif);
1001         if (control->sta)
1002                 hwsim_check_sta_magic(control->sta);
1003
1004         if (hw->flags & IEEE80211_HW_SUPPORTS_RC_TABLE)
1005                 ieee80211_get_tx_rates(txi->control.vif, control->sta, skb,
1006                                        txi->control.rates,
1007                                        ARRAY_SIZE(txi->control.rates));
1008
1009         txi->rate_driver_data[0] = channel;
1010         mac80211_hwsim_monitor_rx(hw, skb, channel);
1011
1012         /* wmediumd mode check */
1013         _portid = ACCESS_ONCE(wmediumd_portid);
1014
1015         if (_portid)
1016                 return mac80211_hwsim_tx_frame_nl(hw, skb, _portid);
1017
1018         /* NO wmediumd detected, perfect medium simulation */
1019         ack = mac80211_hwsim_tx_frame_no_nl(hw, skb, channel);
1020
1021         if (ack && skb->len >= 16) {
1022                 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
1023                 mac80211_hwsim_monitor_ack(channel, hdr->addr2);
1024         }
1025
1026         ieee80211_tx_info_clear_status(txi);
1027
1028         /* frame was transmitted at most favorable rate at first attempt */
1029         txi->control.rates[0].count = 1;
1030         txi->control.rates[1].idx = -1;
1031
1032         if (!(txi->flags & IEEE80211_TX_CTL_NO_ACK) && ack)
1033                 txi->flags |= IEEE80211_TX_STAT_ACK;
1034         ieee80211_tx_status_irqsafe(hw, skb);
1035 }
1036
1037
1038 static int mac80211_hwsim_start(struct ieee80211_hw *hw)
1039 {
1040         struct mac80211_hwsim_data *data = hw->priv;
1041         wiphy_debug(hw->wiphy, "%s\n", __func__);
1042         data->started = true;
1043         return 0;
1044 }
1045
1046
1047 static void mac80211_hwsim_stop(struct ieee80211_hw *hw)
1048 {
1049         struct mac80211_hwsim_data *data = hw->priv;
1050         data->started = false;
1051         tasklet_hrtimer_cancel(&data->beacon_timer);
1052         wiphy_debug(hw->wiphy, "%s\n", __func__);
1053 }
1054
1055
1056 static int mac80211_hwsim_add_interface(struct ieee80211_hw *hw,
1057                                         struct ieee80211_vif *vif)
1058 {
1059         wiphy_debug(hw->wiphy, "%s (type=%d mac_addr=%pM)\n",
1060                     __func__, ieee80211_vif_type_p2p(vif),
1061                     vif->addr);
1062         hwsim_set_magic(vif);
1063
1064         vif->cab_queue = 0;
1065         vif->hw_queue[IEEE80211_AC_VO] = 0;
1066         vif->hw_queue[IEEE80211_AC_VI] = 1;
1067         vif->hw_queue[IEEE80211_AC_BE] = 2;
1068         vif->hw_queue[IEEE80211_AC_BK] = 3;
1069
1070         return 0;
1071 }
1072
1073
1074 static int mac80211_hwsim_change_interface(struct ieee80211_hw *hw,
1075                                            struct ieee80211_vif *vif,
1076                                            enum nl80211_iftype newtype,
1077                                            bool newp2p)
1078 {
1079         newtype = ieee80211_iftype_p2p(newtype, newp2p);
1080         wiphy_debug(hw->wiphy,
1081                     "%s (old type=%d, new type=%d, mac_addr=%pM)\n",
1082                     __func__, ieee80211_vif_type_p2p(vif),
1083                     newtype, vif->addr);
1084         hwsim_check_magic(vif);
1085
1086         /*
1087          * interface may change from non-AP to AP in
1088          * which case this needs to be set up again
1089          */
1090         vif->cab_queue = 0;
1091
1092         return 0;
1093 }
1094
1095 static void mac80211_hwsim_remove_interface(
1096         struct ieee80211_hw *hw, struct ieee80211_vif *vif)
1097 {
1098         wiphy_debug(hw->wiphy, "%s (type=%d mac_addr=%pM)\n",
1099                     __func__, ieee80211_vif_type_p2p(vif),
1100                     vif->addr);
1101         hwsim_check_magic(vif);
1102         hwsim_clear_magic(vif);
1103 }
1104
1105 static void mac80211_hwsim_tx_frame(struct ieee80211_hw *hw,
1106                                     struct sk_buff *skb,
1107                                     struct ieee80211_channel *chan)
1108 {
1109         u32 _pid = ACCESS_ONCE(wmediumd_portid);
1110
1111         if (hw->flags & IEEE80211_HW_SUPPORTS_RC_TABLE) {
1112                 struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(skb);
1113                 ieee80211_get_tx_rates(txi->control.vif, NULL, skb,
1114                                        txi->control.rates,
1115                                        ARRAY_SIZE(txi->control.rates));
1116         }
1117
1118         mac80211_hwsim_monitor_rx(hw, skb, chan);
1119
1120         if (_pid)
1121                 return mac80211_hwsim_tx_frame_nl(hw, skb, _pid);
1122
1123         mac80211_hwsim_tx_frame_no_nl(hw, skb, chan);
1124         dev_kfree_skb(skb);
1125 }
1126
1127 static void mac80211_hwsim_beacon_tx(void *arg, u8 *mac,
1128                                      struct ieee80211_vif *vif)
1129 {
1130         struct mac80211_hwsim_data *data = arg;
1131         struct ieee80211_hw *hw = data->hw;
1132         struct ieee80211_tx_info *info;
1133         struct ieee80211_rate *txrate;
1134         struct ieee80211_mgmt *mgmt;
1135         struct sk_buff *skb;
1136
1137         hwsim_check_magic(vif);
1138
1139         if (vif->type != NL80211_IFTYPE_AP &&
1140             vif->type != NL80211_IFTYPE_MESH_POINT &&
1141             vif->type != NL80211_IFTYPE_ADHOC)
1142                 return;
1143
1144         skb = ieee80211_beacon_get(hw, vif);
1145         if (skb == NULL)
1146                 return;
1147         info = IEEE80211_SKB_CB(skb);
1148         if (hw->flags & IEEE80211_HW_SUPPORTS_RC_TABLE)
1149                 ieee80211_get_tx_rates(vif, NULL, skb,
1150                                        info->control.rates,
1151                                        ARRAY_SIZE(info->control.rates));
1152
1153         txrate = ieee80211_get_tx_rate(hw, info);
1154
1155         mgmt = (struct ieee80211_mgmt *) skb->data;
1156         /* fake header transmission time */
1157         data->abs_bcn_ts = mac80211_hwsim_get_tsf_raw();
1158         mgmt->u.beacon.timestamp = cpu_to_le64(data->abs_bcn_ts +
1159                                                data->tsf_offset +
1160                                                24 * 8 * 10 / txrate->bitrate);
1161
1162         mac80211_hwsim_tx_frame(hw, skb,
1163                                 rcu_dereference(vif->chanctx_conf)->def.chan);
1164 }
1165
1166 static enum hrtimer_restart
1167 mac80211_hwsim_beacon(struct hrtimer *timer)
1168 {
1169         struct mac80211_hwsim_data *data =
1170                 container_of(timer, struct mac80211_hwsim_data,
1171                              beacon_timer.timer);
1172         struct ieee80211_hw *hw = data->hw;
1173         u64 bcn_int = data->beacon_int;
1174         ktime_t next_bcn;
1175
1176         if (!data->started)
1177                 goto out;
1178
1179         ieee80211_iterate_active_interfaces_atomic(
1180                 hw, IEEE80211_IFACE_ITER_NORMAL,
1181                 mac80211_hwsim_beacon_tx, data);
1182
1183         /* beacon at new TBTT + beacon interval */
1184         if (data->bcn_delta) {
1185                 bcn_int -= data->bcn_delta;
1186                 data->bcn_delta = 0;
1187         }
1188
1189         next_bcn = ktime_add(hrtimer_get_expires(timer),
1190                              ns_to_ktime(bcn_int * 1000));
1191         tasklet_hrtimer_start(&data->beacon_timer, next_bcn, HRTIMER_MODE_ABS);
1192 out:
1193         return HRTIMER_NORESTART;
1194 }
1195
1196 static const char * const hwsim_chanwidths[] = {
1197         [NL80211_CHAN_WIDTH_20_NOHT] = "noht",
1198         [NL80211_CHAN_WIDTH_20] = "ht20",
1199         [NL80211_CHAN_WIDTH_40] = "ht40",
1200         [NL80211_CHAN_WIDTH_80] = "vht80",
1201         [NL80211_CHAN_WIDTH_80P80] = "vht80p80",
1202         [NL80211_CHAN_WIDTH_160] = "vht160",
1203 };
1204
1205 static int mac80211_hwsim_config(struct ieee80211_hw *hw, u32 changed)
1206 {
1207         struct mac80211_hwsim_data *data = hw->priv;
1208         struct ieee80211_conf *conf = &hw->conf;
1209         static const char *smps_modes[IEEE80211_SMPS_NUM_MODES] = {
1210                 [IEEE80211_SMPS_AUTOMATIC] = "auto",
1211                 [IEEE80211_SMPS_OFF] = "off",
1212                 [IEEE80211_SMPS_STATIC] = "static",
1213                 [IEEE80211_SMPS_DYNAMIC] = "dynamic",
1214         };
1215
1216         if (conf->chandef.chan)
1217                 wiphy_debug(hw->wiphy,
1218                             "%s (freq=%d(%d - %d)/%s idle=%d ps=%d smps=%s)\n",
1219                             __func__,
1220                             conf->chandef.chan->center_freq,
1221                             conf->chandef.center_freq1,
1222                             conf->chandef.center_freq2,
1223                             hwsim_chanwidths[conf->chandef.width],
1224                             !!(conf->flags & IEEE80211_CONF_IDLE),
1225                             !!(conf->flags & IEEE80211_CONF_PS),
1226                             smps_modes[conf->smps_mode]);
1227         else
1228                 wiphy_debug(hw->wiphy,
1229                             "%s (freq=0 idle=%d ps=%d smps=%s)\n",
1230                             __func__,
1231                             !!(conf->flags & IEEE80211_CONF_IDLE),
1232                             !!(conf->flags & IEEE80211_CONF_PS),
1233                             smps_modes[conf->smps_mode]);
1234
1235         data->idle = !!(conf->flags & IEEE80211_CONF_IDLE);
1236
1237         data->channel = conf->chandef.chan;
1238
1239         WARN_ON(data->channel && data->channels > 1);
1240
1241         data->power_level = conf->power_level;
1242         if (!data->started || !data->beacon_int)
1243                 tasklet_hrtimer_cancel(&data->beacon_timer);
1244         else if (!hrtimer_is_queued(&data->beacon_timer.timer)) {
1245                 u64 tsf = mac80211_hwsim_get_tsf(hw, NULL);
1246                 u32 bcn_int = data->beacon_int;
1247                 u64 until_tbtt = bcn_int - do_div(tsf, bcn_int);
1248
1249                 tasklet_hrtimer_start(&data->beacon_timer,
1250                                       ns_to_ktime(until_tbtt * 1000),
1251                                       HRTIMER_MODE_REL);
1252         }
1253
1254         return 0;
1255 }
1256
1257
1258 static void mac80211_hwsim_configure_filter(struct ieee80211_hw *hw,
1259                                             unsigned int changed_flags,
1260                                             unsigned int *total_flags,u64 multicast)
1261 {
1262         struct mac80211_hwsim_data *data = hw->priv;
1263
1264         wiphy_debug(hw->wiphy, "%s\n", __func__);
1265
1266         data->rx_filter = 0;
1267         if (*total_flags & FIF_PROMISC_IN_BSS)
1268                 data->rx_filter |= FIF_PROMISC_IN_BSS;
1269         if (*total_flags & FIF_ALLMULTI)
1270                 data->rx_filter |= FIF_ALLMULTI;
1271
1272         *total_flags = data->rx_filter;
1273 }
1274
1275 static void mac80211_hwsim_bcn_en_iter(void *data, u8 *mac,
1276                                        struct ieee80211_vif *vif)
1277 {
1278         unsigned int *count = data;
1279         struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
1280
1281         if (vp->bcn_en)
1282                 (*count)++;
1283 }
1284
1285 static void mac80211_hwsim_bss_info_changed(struct ieee80211_hw *hw,
1286                                             struct ieee80211_vif *vif,
1287                                             struct ieee80211_bss_conf *info,
1288                                             u32 changed)
1289 {
1290         struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
1291         struct mac80211_hwsim_data *data = hw->priv;
1292
1293         hwsim_check_magic(vif);
1294
1295         wiphy_debug(hw->wiphy, "%s(changed=0x%x vif->addr=%pM)\n",
1296                     __func__, changed, vif->addr);
1297
1298         if (changed & BSS_CHANGED_BSSID) {
1299                 wiphy_debug(hw->wiphy, "%s: BSSID changed: %pM\n",
1300                             __func__, info->bssid);
1301                 memcpy(vp->bssid, info->bssid, ETH_ALEN);
1302         }
1303
1304         if (changed & BSS_CHANGED_ASSOC) {
1305                 wiphy_debug(hw->wiphy, "  ASSOC: assoc=%d aid=%d\n",
1306                             info->assoc, info->aid);
1307                 vp->assoc = info->assoc;
1308                 vp->aid = info->aid;
1309         }
1310
1311         if (changed & BSS_CHANGED_BEACON_INT) {
1312                 wiphy_debug(hw->wiphy, "  BCNINT: %d\n", info->beacon_int);
1313                 data->beacon_int = info->beacon_int * 1024;
1314         }
1315
1316         if (changed & BSS_CHANGED_BEACON_ENABLED) {
1317                 wiphy_debug(hw->wiphy, "  BCN EN: %d\n", info->enable_beacon);
1318                 vp->bcn_en = info->enable_beacon;
1319                 if (data->started &&
1320                     !hrtimer_is_queued(&data->beacon_timer.timer) &&
1321                     info->enable_beacon) {
1322                         u64 tsf, until_tbtt;
1323                         u32 bcn_int;
1324                         if (WARN_ON(!data->beacon_int))
1325                                 data->beacon_int = 1000 * 1024;
1326                         tsf = mac80211_hwsim_get_tsf(hw, vif);
1327                         bcn_int = data->beacon_int;
1328                         until_tbtt = bcn_int - do_div(tsf, bcn_int);
1329                         tasklet_hrtimer_start(&data->beacon_timer,
1330                                               ns_to_ktime(until_tbtt * 1000),
1331                                               HRTIMER_MODE_REL);
1332                 } else if (!info->enable_beacon) {
1333                         unsigned int count = 0;
1334                         ieee80211_iterate_active_interfaces_atomic(
1335                                 data->hw, IEEE80211_IFACE_ITER_NORMAL,
1336                                 mac80211_hwsim_bcn_en_iter, &count);
1337                         wiphy_debug(hw->wiphy, "  beaconing vifs remaining: %u",
1338                                     count);
1339                         if (count == 0)
1340                                 tasklet_hrtimer_cancel(&data->beacon_timer);
1341                 }
1342         }
1343
1344         if (changed & BSS_CHANGED_ERP_CTS_PROT) {
1345                 wiphy_debug(hw->wiphy, "  ERP_CTS_PROT: %d\n",
1346                             info->use_cts_prot);
1347         }
1348
1349         if (changed & BSS_CHANGED_ERP_PREAMBLE) {
1350                 wiphy_debug(hw->wiphy, "  ERP_PREAMBLE: %d\n",
1351                             info->use_short_preamble);
1352         }
1353
1354         if (changed & BSS_CHANGED_ERP_SLOT) {
1355                 wiphy_debug(hw->wiphy, "  ERP_SLOT: %d\n", info->use_short_slot);
1356         }
1357
1358         if (changed & BSS_CHANGED_HT) {
1359                 wiphy_debug(hw->wiphy, "  HT: op_mode=0x%x\n",
1360                             info->ht_operation_mode);
1361         }
1362
1363         if (changed & BSS_CHANGED_BASIC_RATES) {
1364                 wiphy_debug(hw->wiphy, "  BASIC_RATES: 0x%llx\n",
1365                             (unsigned long long) info->basic_rates);
1366         }
1367
1368         if (changed & BSS_CHANGED_TXPOWER)
1369                 wiphy_debug(hw->wiphy, "  TX Power: %d dBm\n", info->txpower);
1370 }
1371
1372 static int mac80211_hwsim_sta_add(struct ieee80211_hw *hw,
1373                                   struct ieee80211_vif *vif,
1374                                   struct ieee80211_sta *sta)
1375 {
1376         hwsim_check_magic(vif);
1377         hwsim_set_sta_magic(sta);
1378
1379         return 0;
1380 }
1381
1382 static int mac80211_hwsim_sta_remove(struct ieee80211_hw *hw,
1383                                      struct ieee80211_vif *vif,
1384                                      struct ieee80211_sta *sta)
1385 {
1386         hwsim_check_magic(vif);
1387         hwsim_clear_sta_magic(sta);
1388
1389         return 0;
1390 }
1391
1392 static void mac80211_hwsim_sta_notify(struct ieee80211_hw *hw,
1393                                       struct ieee80211_vif *vif,
1394                                       enum sta_notify_cmd cmd,
1395                                       struct ieee80211_sta *sta)
1396 {
1397         hwsim_check_magic(vif);
1398
1399         switch (cmd) {
1400         case STA_NOTIFY_SLEEP:
1401         case STA_NOTIFY_AWAKE:
1402                 /* TODO: make good use of these flags */
1403                 break;
1404         default:
1405                 WARN(1, "Invalid sta notify: %d\n", cmd);
1406                 break;
1407         }
1408 }
1409
1410 static int mac80211_hwsim_set_tim(struct ieee80211_hw *hw,
1411                                   struct ieee80211_sta *sta,
1412                                   bool set)
1413 {
1414         hwsim_check_sta_magic(sta);
1415         return 0;
1416 }
1417
1418 static int mac80211_hwsim_conf_tx(
1419         struct ieee80211_hw *hw,
1420         struct ieee80211_vif *vif, u16 queue,
1421         const struct ieee80211_tx_queue_params *params)
1422 {
1423         wiphy_debug(hw->wiphy,
1424                     "%s (queue=%d txop=%d cw_min=%d cw_max=%d aifs=%d)\n",
1425                     __func__, queue,
1426                     params->txop, params->cw_min,
1427                     params->cw_max, params->aifs);
1428         return 0;
1429 }
1430
1431 static int mac80211_hwsim_get_survey(
1432         struct ieee80211_hw *hw, int idx,
1433         struct survey_info *survey)
1434 {
1435         struct ieee80211_conf *conf = &hw->conf;
1436
1437         wiphy_debug(hw->wiphy, "%s (idx=%d)\n", __func__, idx);
1438
1439         if (idx != 0)
1440                 return -ENOENT;
1441
1442         /* Current channel */
1443         survey->channel = conf->chandef.chan;
1444
1445         /*
1446          * Magically conjured noise level --- this is only ok for simulated hardware.
1447          *
1448          * A real driver which cannot determine the real channel noise MUST NOT
1449          * report any noise, especially not a magically conjured one :-)
1450          */
1451         survey->filled = SURVEY_INFO_NOISE_DBM;
1452         survey->noise = -92;
1453
1454         return 0;
1455 }
1456
1457 #ifdef CONFIG_NL80211_TESTMODE
1458 /*
1459  * This section contains example code for using netlink
1460  * attributes with the testmode command in nl80211.
1461  */
1462
1463 /* These enums need to be kept in sync with userspace */
1464 enum hwsim_testmode_attr {
1465         __HWSIM_TM_ATTR_INVALID = 0,
1466         HWSIM_TM_ATTR_CMD       = 1,
1467         HWSIM_TM_ATTR_PS        = 2,
1468
1469         /* keep last */
1470         __HWSIM_TM_ATTR_AFTER_LAST,
1471         HWSIM_TM_ATTR_MAX       = __HWSIM_TM_ATTR_AFTER_LAST - 1
1472 };
1473
1474 enum hwsim_testmode_cmd {
1475         HWSIM_TM_CMD_SET_PS             = 0,
1476         HWSIM_TM_CMD_GET_PS             = 1,
1477         HWSIM_TM_CMD_STOP_QUEUES        = 2,
1478         HWSIM_TM_CMD_WAKE_QUEUES        = 3,
1479 };
1480
1481 static const struct nla_policy hwsim_testmode_policy[HWSIM_TM_ATTR_MAX + 1] = {
1482         [HWSIM_TM_ATTR_CMD] = { .type = NLA_U32 },
1483         [HWSIM_TM_ATTR_PS] = { .type = NLA_U32 },
1484 };
1485
1486 static int mac80211_hwsim_testmode_cmd(struct ieee80211_hw *hw,
1487                                        struct ieee80211_vif *vif,
1488                                        void *data, int len)
1489 {
1490         struct mac80211_hwsim_data *hwsim = hw->priv;
1491         struct nlattr *tb[HWSIM_TM_ATTR_MAX + 1];
1492         struct sk_buff *skb;
1493         int err, ps;
1494
1495         err = nla_parse(tb, HWSIM_TM_ATTR_MAX, data, len,
1496                         hwsim_testmode_policy);
1497         if (err)
1498                 return err;
1499
1500         if (!tb[HWSIM_TM_ATTR_CMD])
1501                 return -EINVAL;
1502
1503         switch (nla_get_u32(tb[HWSIM_TM_ATTR_CMD])) {
1504         case HWSIM_TM_CMD_SET_PS:
1505                 if (!tb[HWSIM_TM_ATTR_PS])
1506                         return -EINVAL;
1507                 ps = nla_get_u32(tb[HWSIM_TM_ATTR_PS]);
1508                 return hwsim_fops_ps_write(hwsim, ps);
1509         case HWSIM_TM_CMD_GET_PS:
1510                 skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
1511                                                 nla_total_size(sizeof(u32)));
1512                 if (!skb)
1513                         return -ENOMEM;
1514                 if (nla_put_u32(skb, HWSIM_TM_ATTR_PS, hwsim->ps))
1515                         goto nla_put_failure;
1516                 return cfg80211_testmode_reply(skb);
1517         case HWSIM_TM_CMD_STOP_QUEUES:
1518                 ieee80211_stop_queues(hw);
1519                 return 0;
1520         case HWSIM_TM_CMD_WAKE_QUEUES:
1521                 ieee80211_wake_queues(hw);
1522                 return 0;
1523         default:
1524                 return -EOPNOTSUPP;
1525         }
1526
1527  nla_put_failure:
1528         kfree_skb(skb);
1529         return -ENOBUFS;
1530 }
1531 #endif
1532
1533 static int mac80211_hwsim_ampdu_action(struct ieee80211_hw *hw,
1534                                        struct ieee80211_vif *vif,
1535                                        enum ieee80211_ampdu_mlme_action action,
1536                                        struct ieee80211_sta *sta, u16 tid, u16 *ssn,
1537                                        u8 buf_size)
1538 {
1539         switch (action) {
1540         case IEEE80211_AMPDU_TX_START:
1541                 ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
1542                 break;
1543         case IEEE80211_AMPDU_TX_STOP_CONT:
1544         case IEEE80211_AMPDU_TX_STOP_FLUSH:
1545         case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
1546                 ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
1547                 break;
1548         case IEEE80211_AMPDU_TX_OPERATIONAL:
1549                 break;
1550         case IEEE80211_AMPDU_RX_START:
1551         case IEEE80211_AMPDU_RX_STOP:
1552                 break;
1553         default:
1554                 return -EOPNOTSUPP;
1555         }
1556
1557         return 0;
1558 }
1559
1560 static void mac80211_hwsim_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
1561 {
1562         /* Not implemented, queues only on kernel side */
1563 }
1564
1565 static void hw_scan_work(struct work_struct *work)
1566 {
1567         struct mac80211_hwsim_data *hwsim =
1568                 container_of(work, struct mac80211_hwsim_data, hw_scan.work);
1569         struct cfg80211_scan_request *req = hwsim->hw_scan_request;
1570         int dwell, i;
1571
1572         mutex_lock(&hwsim->mutex);
1573         if (hwsim->scan_chan_idx >= req->n_channels) {
1574                 wiphy_debug(hwsim->hw->wiphy, "hw scan complete\n");
1575                 ieee80211_scan_completed(hwsim->hw, false);
1576                 hwsim->hw_scan_request = NULL;
1577                 hwsim->hw_scan_vif = NULL;
1578                 hwsim->tmp_chan = NULL;
1579                 mutex_unlock(&hwsim->mutex);
1580                 return;
1581         }
1582
1583         wiphy_debug(hwsim->hw->wiphy, "hw scan %d MHz\n",
1584                     req->channels[hwsim->scan_chan_idx]->center_freq);
1585
1586         hwsim->tmp_chan = req->channels[hwsim->scan_chan_idx];
1587         if (hwsim->tmp_chan->flags & IEEE80211_CHAN_NO_IR ||
1588             !req->n_ssids) {
1589                 dwell = 120;
1590         } else {
1591                 dwell = 30;
1592                 /* send probes */
1593                 for (i = 0; i < req->n_ssids; i++) {
1594                         struct sk_buff *probe;
1595
1596                         probe = ieee80211_probereq_get(hwsim->hw,
1597                                                        hwsim->hw_scan_vif,
1598                                                        req->ssids[i].ssid,
1599                                                        req->ssids[i].ssid_len,
1600                                                        req->ie_len);
1601                         if (!probe)
1602                                 continue;
1603
1604                         if (req->ie_len)
1605                                 memcpy(skb_put(probe, req->ie_len), req->ie,
1606                                        req->ie_len);
1607
1608                         local_bh_disable();
1609                         mac80211_hwsim_tx_frame(hwsim->hw, probe,
1610                                                 hwsim->tmp_chan);
1611                         local_bh_enable();
1612                 }
1613         }
1614         ieee80211_queue_delayed_work(hwsim->hw, &hwsim->hw_scan,
1615                                      msecs_to_jiffies(dwell));
1616         hwsim->scan_chan_idx++;
1617         mutex_unlock(&hwsim->mutex);
1618 }
1619
1620 static int mac80211_hwsim_hw_scan(struct ieee80211_hw *hw,
1621                                   struct ieee80211_vif *vif,
1622                                   struct cfg80211_scan_request *req)
1623 {
1624         struct mac80211_hwsim_data *hwsim = hw->priv;
1625
1626         mutex_lock(&hwsim->mutex);
1627         if (WARN_ON(hwsim->tmp_chan || hwsim->hw_scan_request)) {
1628                 mutex_unlock(&hwsim->mutex);
1629                 return -EBUSY;
1630         }
1631         hwsim->hw_scan_request = req;
1632         hwsim->hw_scan_vif = vif;
1633         hwsim->scan_chan_idx = 0;
1634         mutex_unlock(&hwsim->mutex);
1635
1636         wiphy_debug(hw->wiphy, "hwsim hw_scan request\n");
1637
1638         ieee80211_queue_delayed_work(hwsim->hw, &hwsim->hw_scan, 0);
1639
1640         return 0;
1641 }
1642
1643 static void mac80211_hwsim_cancel_hw_scan(struct ieee80211_hw *hw,
1644                                           struct ieee80211_vif *vif)
1645 {
1646         struct mac80211_hwsim_data *hwsim = hw->priv;
1647
1648         wiphy_debug(hw->wiphy, "hwsim cancel_hw_scan\n");
1649
1650         cancel_delayed_work_sync(&hwsim->hw_scan);
1651
1652         mutex_lock(&hwsim->mutex);
1653         ieee80211_scan_completed(hwsim->hw, true);
1654         hwsim->tmp_chan = NULL;
1655         hwsim->hw_scan_request = NULL;
1656         hwsim->hw_scan_vif = NULL;
1657         mutex_unlock(&hwsim->mutex);
1658 }
1659
1660 static void mac80211_hwsim_sw_scan(struct ieee80211_hw *hw)
1661 {
1662         struct mac80211_hwsim_data *hwsim = hw->priv;
1663
1664         mutex_lock(&hwsim->mutex);
1665
1666         if (hwsim->scanning) {
1667                 printk(KERN_DEBUG "two hwsim sw_scans detected!\n");
1668                 goto out;
1669         }
1670
1671         printk(KERN_DEBUG "hwsim sw_scan request, prepping stuff\n");
1672         hwsim->scanning = true;
1673
1674 out:
1675         mutex_unlock(&hwsim->mutex);
1676 }
1677
1678 static void mac80211_hwsim_sw_scan_complete(struct ieee80211_hw *hw)
1679 {
1680         struct mac80211_hwsim_data *hwsim = hw->priv;
1681
1682         mutex_lock(&hwsim->mutex);
1683
1684         printk(KERN_DEBUG "hwsim sw_scan_complete\n");
1685         hwsim->scanning = false;
1686
1687         mutex_unlock(&hwsim->mutex);
1688 }
1689
1690 static void hw_roc_done(struct work_struct *work)
1691 {
1692         struct mac80211_hwsim_data *hwsim =
1693                 container_of(work, struct mac80211_hwsim_data, roc_done.work);
1694
1695         mutex_lock(&hwsim->mutex);
1696         ieee80211_remain_on_channel_expired(hwsim->hw);
1697         hwsim->tmp_chan = NULL;
1698         mutex_unlock(&hwsim->mutex);
1699
1700         wiphy_debug(hwsim->hw->wiphy, "hwsim ROC expired\n");
1701 }
1702
1703 static int mac80211_hwsim_roc(struct ieee80211_hw *hw,
1704                               struct ieee80211_vif *vif,
1705                               struct ieee80211_channel *chan,
1706                               int duration,
1707                               enum ieee80211_roc_type type)
1708 {
1709         struct mac80211_hwsim_data *hwsim = hw->priv;
1710
1711         mutex_lock(&hwsim->mutex);
1712         if (WARN_ON(hwsim->tmp_chan || hwsim->hw_scan_request)) {
1713                 mutex_unlock(&hwsim->mutex);
1714                 return -EBUSY;
1715         }
1716
1717         hwsim->tmp_chan = chan;
1718         mutex_unlock(&hwsim->mutex);
1719
1720         wiphy_debug(hw->wiphy, "hwsim ROC (%d MHz, %d ms)\n",
1721                     chan->center_freq, duration);
1722
1723         ieee80211_ready_on_channel(hw);
1724
1725         ieee80211_queue_delayed_work(hw, &hwsim->roc_done,
1726                                      msecs_to_jiffies(duration));
1727         return 0;
1728 }
1729
1730 static int mac80211_hwsim_croc(struct ieee80211_hw *hw)
1731 {
1732         struct mac80211_hwsim_data *hwsim = hw->priv;
1733
1734         cancel_delayed_work_sync(&hwsim->roc_done);
1735
1736         mutex_lock(&hwsim->mutex);
1737         hwsim->tmp_chan = NULL;
1738         mutex_unlock(&hwsim->mutex);
1739
1740         wiphy_debug(hw->wiphy, "hwsim ROC canceled\n");
1741
1742         return 0;
1743 }
1744
1745 static int mac80211_hwsim_add_chanctx(struct ieee80211_hw *hw,
1746                                       struct ieee80211_chanctx_conf *ctx)
1747 {
1748         hwsim_set_chanctx_magic(ctx);
1749         wiphy_debug(hw->wiphy,
1750                     "add channel context control: %d MHz/width: %d/cfreqs:%d/%d MHz\n",
1751                     ctx->def.chan->center_freq, ctx->def.width,
1752                     ctx->def.center_freq1, ctx->def.center_freq2);
1753         return 0;
1754 }
1755
1756 static void mac80211_hwsim_remove_chanctx(struct ieee80211_hw *hw,
1757                                           struct ieee80211_chanctx_conf *ctx)
1758 {
1759         wiphy_debug(hw->wiphy,
1760                     "remove channel context control: %d MHz/width: %d/cfreqs:%d/%d MHz\n",
1761                     ctx->def.chan->center_freq, ctx->def.width,
1762                     ctx->def.center_freq1, ctx->def.center_freq2);
1763         hwsim_check_chanctx_magic(ctx);
1764         hwsim_clear_chanctx_magic(ctx);
1765 }
1766
1767 static void mac80211_hwsim_change_chanctx(struct ieee80211_hw *hw,
1768                                           struct ieee80211_chanctx_conf *ctx,
1769                                           u32 changed)
1770 {
1771         hwsim_check_chanctx_magic(ctx);
1772         wiphy_debug(hw->wiphy,
1773                     "change channel context control: %d MHz/width: %d/cfreqs:%d/%d MHz\n",
1774                     ctx->def.chan->center_freq, ctx->def.width,
1775                     ctx->def.center_freq1, ctx->def.center_freq2);
1776 }
1777
1778 static int mac80211_hwsim_assign_vif_chanctx(struct ieee80211_hw *hw,
1779                                              struct ieee80211_vif *vif,
1780                                              struct ieee80211_chanctx_conf *ctx)
1781 {
1782         hwsim_check_magic(vif);
1783         hwsim_check_chanctx_magic(ctx);
1784
1785         return 0;
1786 }
1787
1788 static void mac80211_hwsim_unassign_vif_chanctx(struct ieee80211_hw *hw,
1789                                                 struct ieee80211_vif *vif,
1790                                                 struct ieee80211_chanctx_conf *ctx)
1791 {
1792         hwsim_check_magic(vif);
1793         hwsim_check_chanctx_magic(ctx);
1794 }
1795
1796 static const struct ieee80211_ops mac80211_hwsim_ops = {
1797         .tx = mac80211_hwsim_tx,
1798         .start = mac80211_hwsim_start,
1799         .stop = mac80211_hwsim_stop,
1800         .add_interface = mac80211_hwsim_add_interface,
1801         .change_interface = mac80211_hwsim_change_interface,
1802         .remove_interface = mac80211_hwsim_remove_interface,
1803         .config = mac80211_hwsim_config,
1804         .configure_filter = mac80211_hwsim_configure_filter,
1805         .bss_info_changed = mac80211_hwsim_bss_info_changed,
1806         .sta_add = mac80211_hwsim_sta_add,
1807         .sta_remove = mac80211_hwsim_sta_remove,
1808         .sta_notify = mac80211_hwsim_sta_notify,
1809         .set_tim = mac80211_hwsim_set_tim,
1810         .conf_tx = mac80211_hwsim_conf_tx,
1811         .get_survey = mac80211_hwsim_get_survey,
1812         CFG80211_TESTMODE_CMD(mac80211_hwsim_testmode_cmd)
1813         .ampdu_action = mac80211_hwsim_ampdu_action,
1814         .sw_scan_start = mac80211_hwsim_sw_scan,
1815         .sw_scan_complete = mac80211_hwsim_sw_scan_complete,
1816         .flush = mac80211_hwsim_flush,
1817         .get_tsf = mac80211_hwsim_get_tsf,
1818         .set_tsf = mac80211_hwsim_set_tsf,
1819 };
1820
1821 static struct ieee80211_ops mac80211_hwsim_mchan_ops;
1822
1823 static int mac80211_hwsim_create_radio(int channels)
1824 {
1825         int err;
1826         u8 addr[ETH_ALEN];
1827         struct mac80211_hwsim_data *data;
1828         struct ieee80211_hw *hw;
1829         enum ieee80211_band band;
1830         const struct ieee80211_ops *ops = &mac80211_hwsim_ops;
1831         int idx;
1832
1833         spin_lock_bh(&hwsim_radio_lock);
1834         idx = hwsim_radio_idx++;
1835         spin_unlock_bh(&hwsim_radio_lock);
1836
1837         if (channels > 1)
1838                 ops = &mac80211_hwsim_mchan_ops;
1839         hw = ieee80211_alloc_hw(sizeof(*data), ops);
1840         if (!hw) {
1841                 printk(KERN_DEBUG "mac80211_hwsim: ieee80211_alloc_hw failed\n");
1842                 err = -ENOMEM;
1843                 goto failed;
1844         }
1845         data = hw->priv;
1846         data->hw = hw;
1847
1848         data->dev = device_create(hwsim_class, NULL, 0, hw, "hwsim%d", idx);
1849         if (IS_ERR(data->dev)) {
1850                 printk(KERN_DEBUG
1851                        "mac80211_hwsim: device_create failed (%ld)\n",
1852                        PTR_ERR(data->dev));
1853                 err = -ENOMEM;
1854                 goto failed_drvdata;
1855         }
1856         data->dev->driver = &mac80211_hwsim_driver.driver;
1857         err = device_bind_driver(data->dev);
1858         if (err != 0) {
1859                 printk(KERN_DEBUG "mac80211_hwsim: device_bind_driver failed (%d)\n",
1860                        err);
1861                 goto failed_hw;
1862         }
1863
1864         skb_queue_head_init(&data->pending);
1865
1866         SET_IEEE80211_DEV(hw, data->dev);
1867         memset(addr, 0, ETH_ALEN);
1868         addr[0] = 0x02;
1869         addr[3] = idx >> 8;
1870         addr[4] = idx;
1871         memcpy(data->addresses[0].addr, addr, ETH_ALEN);
1872         memcpy(data->addresses[1].addr, addr, ETH_ALEN);
1873         data->addresses[1].addr[0] |= 0x40;
1874         hw->wiphy->n_addresses = 2;
1875         hw->wiphy->addresses = data->addresses;
1876
1877         data->channels = channels;
1878         data->idx = idx;
1879
1880         if (data->channels > 1) {
1881                 hw->wiphy->max_scan_ssids = 255;
1882                 hw->wiphy->max_scan_ie_len = IEEE80211_MAX_DATA_LEN;
1883                 hw->wiphy->max_remain_on_channel_duration = 1000;
1884                 /* For channels > 1 DFS is not allowed */
1885                 hw->wiphy->n_iface_combinations = 1;
1886                 hw->wiphy->iface_combinations = &data->if_combination;
1887                 data->if_combination = hwsim_if_comb[0];
1888                 data->if_combination.num_different_channels = data->channels;
1889         } else {
1890                 hw->wiphy->iface_combinations = hwsim_if_comb;
1891                 hw->wiphy->n_iface_combinations = ARRAY_SIZE(hwsim_if_comb);
1892         }
1893
1894         INIT_DELAYED_WORK(&data->roc_done, hw_roc_done);
1895         INIT_DELAYED_WORK(&data->hw_scan, hw_scan_work);
1896
1897         hw->queues = 5;
1898         hw->offchannel_tx_hw_queue = 4;
1899         hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
1900                                      BIT(NL80211_IFTYPE_AP) |
1901                                      BIT(NL80211_IFTYPE_P2P_CLIENT) |
1902                                      BIT(NL80211_IFTYPE_P2P_GO) |
1903                                      BIT(NL80211_IFTYPE_ADHOC) |
1904                                      BIT(NL80211_IFTYPE_MESH_POINT) |
1905                                      BIT(NL80211_IFTYPE_P2P_DEVICE);
1906
1907         hw->flags = IEEE80211_HW_MFP_CAPABLE |
1908                     IEEE80211_HW_SIGNAL_DBM |
1909                     IEEE80211_HW_SUPPORTS_STATIC_SMPS |
1910                     IEEE80211_HW_SUPPORTS_DYNAMIC_SMPS |
1911                     IEEE80211_HW_AMPDU_AGGREGATION |
1912                     IEEE80211_HW_WANT_MONITOR_VIF |
1913                     IEEE80211_HW_QUEUE_CONTROL |
1914                     IEEE80211_HW_SUPPORTS_HT_CCK_RATES;
1915         if (rctbl)
1916                 hw->flags |= IEEE80211_HW_SUPPORTS_RC_TABLE;
1917
1918         hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS |
1919                             WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL |
1920                             WIPHY_FLAG_AP_UAPSD;
1921         hw->wiphy->features |= NL80211_FEATURE_ACTIVE_MONITOR;
1922
1923         /* ask mac80211 to reserve space for magic */
1924         hw->vif_data_size = sizeof(struct hwsim_vif_priv);
1925         hw->sta_data_size = sizeof(struct hwsim_sta_priv);
1926         hw->chanctx_data_size = sizeof(struct hwsim_chanctx_priv);
1927
1928         memcpy(data->channels_2ghz, hwsim_channels_2ghz,
1929                 sizeof(hwsim_channels_2ghz));
1930         memcpy(data->channels_5ghz, hwsim_channels_5ghz,
1931                 sizeof(hwsim_channels_5ghz));
1932         memcpy(data->rates, hwsim_rates, sizeof(hwsim_rates));
1933
1934         for (band = IEEE80211_BAND_2GHZ; band < IEEE80211_NUM_BANDS; band++) {
1935                 struct ieee80211_supported_band *sband = &data->bands[band];
1936                 switch (band) {
1937                 case IEEE80211_BAND_2GHZ:
1938                         sband->channels = data->channels_2ghz;
1939                         sband->n_channels = ARRAY_SIZE(hwsim_channels_2ghz);
1940                         sband->bitrates = data->rates;
1941                         sband->n_bitrates = ARRAY_SIZE(hwsim_rates);
1942                         break;
1943                 case IEEE80211_BAND_5GHZ:
1944                         sband->channels = data->channels_5ghz;
1945                         sband->n_channels = ARRAY_SIZE(hwsim_channels_5ghz);
1946                         sband->bitrates = data->rates + 4;
1947                         sband->n_bitrates = ARRAY_SIZE(hwsim_rates) - 4;
1948                         break;
1949                 default:
1950                         continue;
1951                 }
1952
1953                 sband->ht_cap.ht_supported = true;
1954                 sband->ht_cap.cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
1955                                     IEEE80211_HT_CAP_GRN_FLD |
1956                                     IEEE80211_HT_CAP_SGI_40 |
1957                                     IEEE80211_HT_CAP_DSSSCCK40;
1958                 sband->ht_cap.ampdu_factor = 0x3;
1959                 sband->ht_cap.ampdu_density = 0x6;
1960                 memset(&sband->ht_cap.mcs, 0,
1961                        sizeof(sband->ht_cap.mcs));
1962                 sband->ht_cap.mcs.rx_mask[0] = 0xff;
1963                 sband->ht_cap.mcs.rx_mask[1] = 0xff;
1964                 sband->ht_cap.mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
1965
1966                 hw->wiphy->bands[band] = sband;
1967
1968                 sband->vht_cap.vht_supported = true;
1969                 sband->vht_cap.cap =
1970                         IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454 |
1971                         IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160_80PLUS80MHZ |
1972                         IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160MHZ |
1973                         IEEE80211_VHT_CAP_RXLDPC |
1974                         IEEE80211_VHT_CAP_SHORT_GI_80 |
1975                         IEEE80211_VHT_CAP_SHORT_GI_160 |
1976                         IEEE80211_VHT_CAP_TXSTBC |
1977                         IEEE80211_VHT_CAP_RXSTBC_1 |
1978                         IEEE80211_VHT_CAP_RXSTBC_2 |
1979                         IEEE80211_VHT_CAP_RXSTBC_3 |
1980                         IEEE80211_VHT_CAP_RXSTBC_4 |
1981                         IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK;
1982                 sband->vht_cap.vht_mcs.rx_mcs_map =
1983                         cpu_to_le16(IEEE80211_VHT_MCS_SUPPORT_0_8 << 0 |
1984                                     IEEE80211_VHT_MCS_SUPPORT_0_8 << 2 |
1985                                     IEEE80211_VHT_MCS_SUPPORT_0_9 << 4 |
1986                                     IEEE80211_VHT_MCS_SUPPORT_0_8 << 6 |
1987                                     IEEE80211_VHT_MCS_SUPPORT_0_8 << 8 |
1988                                     IEEE80211_VHT_MCS_SUPPORT_0_9 << 10 |
1989                                     IEEE80211_VHT_MCS_SUPPORT_0_9 << 12 |
1990                                     IEEE80211_VHT_MCS_SUPPORT_0_8 << 14);
1991                 sband->vht_cap.vht_mcs.tx_mcs_map =
1992                         sband->vht_cap.vht_mcs.rx_mcs_map;
1993         }
1994
1995         /* By default all radios belong to the first group */
1996         data->group = 1;
1997         mutex_init(&data->mutex);
1998
1999         /* Enable frame retransmissions for lossy channels */
2000         hw->max_rates = 4;
2001         hw->max_rate_tries = 11;
2002
2003         err = ieee80211_register_hw(hw);
2004         if (err < 0) {
2005                 printk(KERN_DEBUG "mac80211_hwsim: ieee80211_register_hw failed (%d)\n",
2006                        err);
2007                 goto failed_hw;
2008         }
2009
2010         wiphy_debug(hw->wiphy, "hwaddr %pM registered\n", hw->wiphy->perm_addr);
2011
2012         data->debugfs = debugfs_create_dir("hwsim", hw->wiphy->debugfsdir);
2013         debugfs_create_file("ps", 0666, data->debugfs, data, &hwsim_fops_ps);
2014         debugfs_create_file("group", 0666, data->debugfs, data,
2015                             &hwsim_fops_group);
2016         if (data->channels == 1)
2017                 debugfs_create_file("dfs_simulate_radar", 0222,
2018                                     data->debugfs,
2019                                     data, &hwsim_simulate_radar);
2020
2021         tasklet_hrtimer_init(&data->beacon_timer,
2022                              mac80211_hwsim_beacon,
2023                              CLOCK_MONOTONIC_RAW, HRTIMER_MODE_ABS);
2024
2025         spin_lock_bh(&hwsim_radio_lock);
2026         list_add_tail(&data->list, &hwsim_radios);
2027         spin_unlock_bh(&hwsim_radio_lock);
2028
2029         return idx;
2030
2031 failed_hw:
2032         device_unregister(data->dev);
2033 failed_drvdata:
2034         ieee80211_free_hw(hw);
2035 failed:
2036         return err;
2037 }
2038
2039 static void mac80211_hwsim_destroy_radio(struct mac80211_hwsim_data *data)
2040 {
2041         debugfs_remove_recursive(data->debugfs);
2042         ieee80211_unregister_hw(data->hw);
2043         device_release_driver(data->dev);
2044         device_unregister(data->dev);
2045         ieee80211_free_hw(data->hw);
2046 }
2047
2048 static void mac80211_hwsim_free(void)
2049 {
2050         struct mac80211_hwsim_data *data;
2051
2052         spin_lock_bh(&hwsim_radio_lock);
2053         while ((data = list_first_entry_or_null(&hwsim_radios,
2054                                                 struct mac80211_hwsim_data,
2055                                                 list))) {
2056                 list_del(&data->list);
2057                 spin_unlock_bh(&hwsim_radio_lock);
2058                 mac80211_hwsim_destroy_radio(data);
2059                 spin_lock_bh(&hwsim_radio_lock);
2060         }
2061         spin_unlock_bh(&hwsim_radio_lock);
2062         class_destroy(hwsim_class);
2063 }
2064
2065 static const struct net_device_ops hwsim_netdev_ops = {
2066         .ndo_start_xmit         = hwsim_mon_xmit,
2067         .ndo_change_mtu         = eth_change_mtu,
2068         .ndo_set_mac_address    = eth_mac_addr,
2069         .ndo_validate_addr      = eth_validate_addr,
2070 };
2071
2072 static void hwsim_mon_setup(struct net_device *dev)
2073 {
2074         dev->netdev_ops = &hwsim_netdev_ops;
2075         dev->destructor = free_netdev;
2076         ether_setup(dev);
2077         dev->tx_queue_len = 0;
2078         dev->type = ARPHRD_IEEE80211_RADIOTAP;
2079         memset(dev->dev_addr, 0, ETH_ALEN);
2080         dev->dev_addr[0] = 0x12;
2081 }
2082
2083 static struct mac80211_hwsim_data *get_hwsim_data_ref_from_addr(const u8 *addr)
2084 {
2085         struct mac80211_hwsim_data *data;
2086         bool _found = false;
2087
2088         spin_lock_bh(&hwsim_radio_lock);
2089         list_for_each_entry(data, &hwsim_radios, list) {
2090                 if (memcmp(data->addresses[1].addr, addr, ETH_ALEN) == 0) {
2091                         _found = true;
2092                         break;
2093                 }
2094         }
2095         spin_unlock_bh(&hwsim_radio_lock);
2096
2097         if (!_found)
2098                 return NULL;
2099
2100         return data;
2101 }
2102
2103 static int hwsim_tx_info_frame_received_nl(struct sk_buff *skb_2,
2104                                            struct genl_info *info)
2105 {
2106
2107         struct ieee80211_hdr *hdr;
2108         struct mac80211_hwsim_data *data2;
2109         struct ieee80211_tx_info *txi;
2110         struct hwsim_tx_rate *tx_attempts;
2111         unsigned long ret_skb_ptr;
2112         struct sk_buff *skb, *tmp;
2113         const u8 *src;
2114         unsigned int hwsim_flags;
2115         int i;
2116         bool found = false;
2117
2118         if (info->snd_portid != wmediumd_portid)
2119                 return -EINVAL;
2120
2121         if (!info->attrs[HWSIM_ATTR_ADDR_TRANSMITTER] ||
2122             !info->attrs[HWSIM_ATTR_FLAGS] ||
2123             !info->attrs[HWSIM_ATTR_COOKIE] ||
2124             !info->attrs[HWSIM_ATTR_TX_INFO])
2125                 goto out;
2126
2127         src = (void *)nla_data(info->attrs[HWSIM_ATTR_ADDR_TRANSMITTER]);
2128         hwsim_flags = nla_get_u32(info->attrs[HWSIM_ATTR_FLAGS]);
2129         ret_skb_ptr = nla_get_u64(info->attrs[HWSIM_ATTR_COOKIE]);
2130
2131         data2 = get_hwsim_data_ref_from_addr(src);
2132         if (!data2)
2133                 goto out;
2134
2135         /* look for the skb matching the cookie passed back from user */
2136         skb_queue_walk_safe(&data2->pending, skb, tmp) {
2137                 if ((unsigned long)skb == ret_skb_ptr) {
2138                         skb_unlink(skb, &data2->pending);
2139                         found = true;
2140                         break;
2141                 }
2142         }
2143
2144         /* not found */
2145         if (!found)
2146                 goto out;
2147
2148         /* Tx info received because the frame was broadcasted on user space,
2149          so we get all the necessary info: tx attempts and skb control buff */
2150
2151         tx_attempts = (struct hwsim_tx_rate *)nla_data(
2152                        info->attrs[HWSIM_ATTR_TX_INFO]);
2153
2154         /* now send back TX status */
2155         txi = IEEE80211_SKB_CB(skb);
2156
2157         ieee80211_tx_info_clear_status(txi);
2158
2159         for (i = 0; i < IEEE80211_TX_MAX_RATES; i++) {
2160                 txi->status.rates[i].idx = tx_attempts[i].idx;
2161                 txi->status.rates[i].count = tx_attempts[i].count;
2162                 /*txi->status.rates[i].flags = 0;*/
2163         }
2164
2165         txi->status.ack_signal = nla_get_u32(info->attrs[HWSIM_ATTR_SIGNAL]);
2166
2167         if (!(hwsim_flags & HWSIM_TX_CTL_NO_ACK) &&
2168            (hwsim_flags & HWSIM_TX_STAT_ACK)) {
2169                 if (skb->len >= 16) {
2170                         hdr = (struct ieee80211_hdr *) skb->data;
2171                         mac80211_hwsim_monitor_ack(txi->rate_driver_data[0],
2172                                                    hdr->addr2);
2173                 }
2174                 txi->flags |= IEEE80211_TX_STAT_ACK;
2175         }
2176         ieee80211_tx_status_irqsafe(data2->hw, skb);
2177         return 0;
2178 out:
2179         return -EINVAL;
2180
2181 }
2182
2183 static int hwsim_cloned_frame_received_nl(struct sk_buff *skb_2,
2184                                           struct genl_info *info)
2185 {
2186
2187         struct mac80211_hwsim_data *data2;
2188         struct ieee80211_rx_status rx_status;
2189         const u8 *dst;
2190         int frame_data_len;
2191         void *frame_data;
2192         struct sk_buff *skb = NULL;
2193
2194         if (info->snd_portid != wmediumd_portid)
2195                 return -EINVAL;
2196
2197         if (!info->attrs[HWSIM_ATTR_ADDR_RECEIVER] ||
2198             !info->attrs[HWSIM_ATTR_FRAME] ||
2199             !info->attrs[HWSIM_ATTR_RX_RATE] ||
2200             !info->attrs[HWSIM_ATTR_SIGNAL])
2201                 goto out;
2202
2203         dst = (void *)nla_data(info->attrs[HWSIM_ATTR_ADDR_RECEIVER]);
2204         frame_data_len = nla_len(info->attrs[HWSIM_ATTR_FRAME]);
2205         frame_data = (void *)nla_data(info->attrs[HWSIM_ATTR_FRAME]);
2206
2207         /* Allocate new skb here */
2208         skb = alloc_skb(frame_data_len, GFP_KERNEL);
2209         if (skb == NULL)
2210                 goto err;
2211
2212         if (frame_data_len > IEEE80211_MAX_DATA_LEN)
2213                 goto err;
2214
2215         /* Copy the data */
2216         memcpy(skb_put(skb, frame_data_len), frame_data, frame_data_len);
2217
2218         data2 = get_hwsim_data_ref_from_addr(dst);
2219         if (!data2)
2220                 goto out;
2221
2222         /* check if radio is configured properly */
2223
2224         if (data2->idle || !data2->started)
2225                 goto out;
2226
2227         /* A frame is received from user space */
2228         memset(&rx_status, 0, sizeof(rx_status));
2229         rx_status.freq = data2->channel->center_freq;
2230         rx_status.band = data2->channel->band;
2231         rx_status.rate_idx = nla_get_u32(info->attrs[HWSIM_ATTR_RX_RATE]);
2232         rx_status.signal = nla_get_u32(info->attrs[HWSIM_ATTR_SIGNAL]);
2233
2234         memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
2235         ieee80211_rx_irqsafe(data2->hw, skb);
2236
2237         return 0;
2238 err:
2239         printk(KERN_DEBUG "mac80211_hwsim: error occurred in %s\n", __func__);
2240         goto out;
2241 out:
2242         dev_kfree_skb(skb);
2243         return -EINVAL;
2244 }
2245
2246 static int hwsim_register_received_nl(struct sk_buff *skb_2,
2247                                       struct genl_info *info)
2248 {
2249         struct mac80211_hwsim_data *data;
2250         int chans = 1;
2251
2252         spin_lock_bh(&hwsim_radio_lock);
2253         list_for_each_entry(data, &hwsim_radios, list)
2254                 chans = max(chans, data->channels);
2255         spin_unlock_bh(&hwsim_radio_lock);
2256
2257         /* In the future we should revise the userspace API and allow it
2258          * to set a flag that it does support multi-channel, then we can
2259          * let this pass conditionally on the flag.
2260          * For current userspace, prohibit it since it won't work right.
2261          */
2262         if (chans > 1)
2263                 return -EOPNOTSUPP;
2264
2265         if (wmediumd_portid)
2266                 return -EBUSY;
2267
2268         wmediumd_portid = info->snd_portid;
2269
2270         printk(KERN_DEBUG "mac80211_hwsim: received a REGISTER, "
2271                "switching to wmediumd mode with pid %d\n", info->snd_portid);
2272
2273         return 0;
2274 }
2275
2276 static int hwsim_create_radio_nl(struct sk_buff *msg, struct genl_info *info)
2277 {
2278         unsigned int chans = channels;
2279
2280         if (info->attrs[HWSIM_ATTR_CHANNELS])
2281                 chans = nla_get_u32(info->attrs[HWSIM_ATTR_CHANNELS]);
2282
2283         return mac80211_hwsim_create_radio(chans);
2284 }
2285
2286 static int hwsim_destroy_radio_nl(struct sk_buff *msg, struct genl_info *info)
2287 {
2288         struct mac80211_hwsim_data *data;
2289         int idx;
2290
2291         if (!info->attrs[HWSIM_ATTR_RADIO_ID])
2292                 return -EINVAL;
2293         idx = nla_get_u32(info->attrs[HWSIM_ATTR_RADIO_ID]);
2294
2295         spin_lock_bh(&hwsim_radio_lock);
2296         list_for_each_entry(data, &hwsim_radios, list) {
2297                 if (data->idx != idx)
2298                         continue;
2299                 list_del(&data->list);
2300                 spin_unlock_bh(&hwsim_radio_lock);
2301                 mac80211_hwsim_destroy_radio(data);
2302                 return 0;
2303         }
2304         spin_unlock_bh(&hwsim_radio_lock);
2305
2306         return -ENODEV;
2307 }
2308
2309 /* Generic Netlink operations array */
2310 static const struct genl_ops hwsim_ops[] = {
2311         {
2312                 .cmd = HWSIM_CMD_REGISTER,
2313                 .policy = hwsim_genl_policy,
2314                 .doit = hwsim_register_received_nl,
2315                 .flags = GENL_ADMIN_PERM,
2316         },
2317         {
2318                 .cmd = HWSIM_CMD_FRAME,
2319                 .policy = hwsim_genl_policy,
2320                 .doit = hwsim_cloned_frame_received_nl,
2321         },
2322         {
2323                 .cmd = HWSIM_CMD_TX_INFO_FRAME,
2324                 .policy = hwsim_genl_policy,
2325                 .doit = hwsim_tx_info_frame_received_nl,
2326         },
2327         {
2328                 .cmd = HWSIM_CMD_CREATE_RADIO,
2329                 .policy = hwsim_genl_policy,
2330                 .doit = hwsim_create_radio_nl,
2331                 .flags = GENL_ADMIN_PERM,
2332         },
2333         {
2334                 .cmd = HWSIM_CMD_DESTROY_RADIO,
2335                 .policy = hwsim_genl_policy,
2336                 .doit = hwsim_destroy_radio_nl,
2337                 .flags = GENL_ADMIN_PERM,
2338         },
2339 };
2340
2341 static int mac80211_hwsim_netlink_notify(struct notifier_block *nb,
2342                                          unsigned long state,
2343                                          void *_notify)
2344 {
2345         struct netlink_notify *notify = _notify;
2346
2347         if (state != NETLINK_URELEASE)
2348                 return NOTIFY_DONE;
2349
2350         if (notify->portid == wmediumd_portid) {
2351                 printk(KERN_INFO "mac80211_hwsim: wmediumd released netlink"
2352                        " socket, switching to perfect channel medium\n");
2353                 wmediumd_portid = 0;
2354         }
2355         return NOTIFY_DONE;
2356
2357 }
2358
2359 static struct notifier_block hwsim_netlink_notifier = {
2360         .notifier_call = mac80211_hwsim_netlink_notify,
2361 };
2362
2363 static int hwsim_init_netlink(void)
2364 {
2365         int rc;
2366
2367         printk(KERN_INFO "mac80211_hwsim: initializing netlink\n");
2368
2369         rc = genl_register_family_with_ops(&hwsim_genl_family, hwsim_ops);
2370         if (rc)
2371                 goto failure;
2372
2373         rc = netlink_register_notifier(&hwsim_netlink_notifier);
2374         if (rc)
2375                 goto failure;
2376
2377         return 0;
2378
2379 failure:
2380         printk(KERN_DEBUG "mac80211_hwsim: error occurred in %s\n", __func__);
2381         return -EINVAL;
2382 }
2383
2384 static void hwsim_exit_netlink(void)
2385 {
2386         /* unregister the notifier */
2387         netlink_unregister_notifier(&hwsim_netlink_notifier);
2388         /* unregister the family */
2389         genl_unregister_family(&hwsim_genl_family);
2390 }
2391
2392 static int __init init_mac80211_hwsim(void)
2393 {
2394         int i, err;
2395
2396         if (radios < 0 || radios > 100)
2397                 return -EINVAL;
2398
2399         if (channels < 1)
2400                 return -EINVAL;
2401
2402         mac80211_hwsim_mchan_ops = mac80211_hwsim_ops;
2403         mac80211_hwsim_mchan_ops.hw_scan = mac80211_hwsim_hw_scan;
2404         mac80211_hwsim_mchan_ops.cancel_hw_scan = mac80211_hwsim_cancel_hw_scan;
2405         mac80211_hwsim_mchan_ops.sw_scan_start = NULL;
2406         mac80211_hwsim_mchan_ops.sw_scan_complete = NULL;
2407         mac80211_hwsim_mchan_ops.remain_on_channel = mac80211_hwsim_roc;
2408         mac80211_hwsim_mchan_ops.cancel_remain_on_channel = mac80211_hwsim_croc;
2409         mac80211_hwsim_mchan_ops.add_chanctx = mac80211_hwsim_add_chanctx;
2410         mac80211_hwsim_mchan_ops.remove_chanctx = mac80211_hwsim_remove_chanctx;
2411         mac80211_hwsim_mchan_ops.change_chanctx = mac80211_hwsim_change_chanctx;
2412         mac80211_hwsim_mchan_ops.assign_vif_chanctx =
2413                 mac80211_hwsim_assign_vif_chanctx;
2414         mac80211_hwsim_mchan_ops.unassign_vif_chanctx =
2415                 mac80211_hwsim_unassign_vif_chanctx;
2416
2417         spin_lock_init(&hwsim_radio_lock);
2418         INIT_LIST_HEAD(&hwsim_radios);
2419
2420         err = platform_driver_register(&mac80211_hwsim_driver);
2421         if (err)
2422                 return err;
2423
2424         hwsim_class = class_create(THIS_MODULE, "mac80211_hwsim");
2425         if (IS_ERR(hwsim_class)) {
2426                 err = PTR_ERR(hwsim_class);
2427                 goto out_unregister_driver;
2428         }
2429
2430         for (i = 0; i < radios; i++) {
2431                 err = mac80211_hwsim_create_radio(channels);
2432                 if (err < 0)
2433                         goto out_free_radios;
2434         }
2435
2436         hwsim_mon = alloc_netdev(0, "hwsim%d", hwsim_mon_setup);
2437         if (hwsim_mon == NULL) {
2438                 err = -ENOMEM;
2439                 goto out_free_radios;
2440         }
2441
2442         rtnl_lock();
2443         err = dev_alloc_name(hwsim_mon, hwsim_mon->name);
2444         if (err < 0) {
2445                 rtnl_unlock();
2446                 goto out_free_radios;
2447         }
2448
2449         err = register_netdevice(hwsim_mon);
2450         if (err < 0) {
2451                 rtnl_unlock();
2452                 goto out_free_mon;
2453         }
2454         rtnl_unlock();
2455
2456         err = hwsim_init_netlink();
2457         if (err < 0)
2458                 goto out_free_mon;
2459
2460         return 0;
2461
2462 out_free_mon:
2463         free_netdev(hwsim_mon);
2464 out_free_radios:
2465         mac80211_hwsim_free();
2466 out_unregister_driver:
2467         platform_driver_unregister(&mac80211_hwsim_driver);
2468         return err;
2469 }
2470 module_init(init_mac80211_hwsim);
2471
2472 static void __exit exit_mac80211_hwsim(void)
2473 {
2474         printk(KERN_DEBUG "mac80211_hwsim: unregister radios\n");
2475
2476         hwsim_exit_netlink();
2477
2478         mac80211_hwsim_free();
2479         unregister_netdev(hwsim_mon);
2480         platform_driver_unregister(&mac80211_hwsim_driver);
2481 }
2482 module_exit(exit_mac80211_hwsim);