net/wireless/mac80211_hwsim.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * mac80211_hwsim - software simulator of 802.11 radio(s) for mac80211
0004  * Copyright (c) 2008, Jouni Malinen <j@w1.fi>
0005  * Copyright (c) 2011, Javier Lopez <jlopex@gmail.com>
0006  * Copyright (c) 2016 - 2017 Intel Deutschland GmbH
0007  * Copyright (C) 2018 - 2022 Intel Corporation
0008  */
0009
0010 /*
0011  * TODO:
0012  * - Add TSF sync and fix IBSS beacon transmission by adding
0013  *   competition for "air time" at TBTT
0014  * - RX filtering based on filter configuration (data->rx_filter)
0015  */
0016
0017 #include <linux/list.h>
0018 #include <linux/slab.h>
0019 #include <linux/spinlock.h>
0020 #include <net/dst.h>
0021 #include <net/xfrm.h>
0022 #include <net/mac80211.h>
0023 #include <net/ieee80211_radiotap.h>
0024 #include <linux/if_arp.h>
0025 #include <linux/rtnetlink.h>
0026 #include <linux/etherdevice.h>
0027 #include <linux/platform_device.h>
0028 #include <linux/debugfs.h>
0029 #include <linux/module.h>
0030 #include <linux/ktime.h>
0031 #include <net/genetlink.h>
0032 #include <net/net_namespace.h>
0033 #include <net/netns/generic.h>
0034 #include <linux/rhashtable.h>
0035 #include <linux/nospec.h>
0036 #include <linux/virtio.h>
0037 #include <linux/virtio_ids.h>
0038 #include <linux/virtio_config.h>
0039 #include "mac80211_hwsim.h"
0040
0041 #define WARN_QUEUE 100
0042 #define MAX_QUEUE 200
0043
0044 MODULE_AUTHOR("Jouni Malinen");
0045 MODULE_DESCRIPTION("Software simulator of 802.11 radio(s) for mac80211");
0046 MODULE_LICENSE("GPL");
0047
0048 static int radios = 2;
0049 module_param(radios, int, 0444);
0050 MODULE_PARM_DESC(radios, "Number of simulated radios");
0051
0052 static int channels = 1;
0053 module_param(channels, int, 0444);
0054 MODULE_PARM_DESC(channels, "Number of concurrent channels");
0055
0056 static bool paged_rx = false;
0057 module_param(paged_rx, bool, 0644);
0058 MODULE_PARM_DESC(paged_rx, "Use paged SKBs for RX instead of linear ones");
0059
0060 static bool rctbl = false;
0061 module_param(rctbl, bool, 0444);
0062 MODULE_PARM_DESC(rctbl, "Handle rate control table");
0063
0064 static bool support_p2p_device = true;
0065 module_param(support_p2p_device, bool, 0444);
0066 MODULE_PARM_DESC(support_p2p_device, "Support P2P-Device interface type");
0067
0068 static bool mlo;
0069 module_param(mlo, bool, 0444);
0070 MODULE_PARM_DESC(mlo, "Support MLO");
0071
0072 /**
0073  * enum hwsim_regtest - the type of regulatory tests we offer
0074  *
0075  * These are the different values you can use for the regtest
0076  * module parameter. This is useful to help test world roaming
0077  * and the driver regulatory_hint() call and combinations of these.
0078  * If you want to do specific alpha2 regulatory domain tests simply
0079  * use the userspace regulatory request as that will be respected as
0080  * well without the need of this module parameter. This is designed
0081  * only for testing the driver regulatory request, world roaming
0082  * and all possible combinations.
0083  *
0084  * @HWSIM_REGTEST_DISABLED: No regulatory tests are performed,
0085  *  this is the default value.
0086  * @HWSIM_REGTEST_DRIVER_REG_FOLLOW: Used for testing the driver regulatory
0087  *  hint, only one driver regulatory hint will be sent as such the
0088  *  secondary radios are expected to follow.
0089  * @HWSIM_REGTEST_DRIVER_REG_ALL: Used for testing the driver regulatory
0090  *  request with all radios reporting the same regulatory domain.
0091  * @HWSIM_REGTEST_DIFF_COUNTRY: Used for testing the drivers calling
0092  *  different regulatory domains requests. Expected behaviour is for
0093  *  an intersection to occur but each device will still use their
0094  *  respective regulatory requested domains. Subsequent radios will
0095  *  use the resulting intersection.
0096  * @HWSIM_REGTEST_WORLD_ROAM: Used for testing the world roaming. We accomplish
0097  *  this by using a custom beacon-capable regulatory domain for the first
0098  *  radio. All other device world roam.
0099  * @HWSIM_REGTEST_CUSTOM_WORLD: Used for testing the custom world regulatory
0100  *  domain requests. All radios will adhere to this custom world regulatory
0101  *  domain.
0102  * @HWSIM_REGTEST_CUSTOM_WORLD_2: Used for testing 2 custom world regulatory
0103  *  domain requests. The first radio will adhere to the first custom world
0104  *  regulatory domain, the second one to the second custom world regulatory
0105  *  domain. All other devices will world roam.
0106  * @HWSIM_REGTEST_STRICT_FOLLOW: Used for testing strict regulatory domain
0107  *  settings, only the first radio will send a regulatory domain request
0108  *  and use strict settings. The rest of the radios are expected to follow.
0109  * @HWSIM_REGTEST_STRICT_ALL: Used for testing strict regulatory domain
0110  *  settings. All radios will adhere to this.
0111  * @HWSIM_REGTEST_STRICT_AND_DRIVER_REG: Used for testing strict regulatory
0112  *  domain settings, combined with secondary driver regulatory domain
0113  *  settings. The first radio will get a strict regulatory domain setting
0114  *  using the first driver regulatory request and the second radio will use
0115  *  non-strict settings using the second driver regulatory request. All
0116  *  other devices should follow the intersection created between the
0117  *  first two.
0118  * @HWSIM_REGTEST_ALL: Used for testing every possible mix. You will need
0119  *  at least 6 radios for a complete test. We will test in this order:
0120  *  1 - driver custom world regulatory domain
0121  *  2 - second custom world regulatory domain
0122  *  3 - first driver regulatory domain request
0123  *  4 - second driver regulatory domain request
0124  *  5 - strict regulatory domain settings using the third driver regulatory
0125  *      domain request
0126  *  6 and on - should follow the intersection of the 3rd, 4rth and 5th radio
0127  *             regulatory requests.
0128  */
0129 enum hwsim_regtest {
0130     HWSIM_REGTEST_DISABLED = 0,
0131     HWSIM_REGTEST_DRIVER_REG_FOLLOW = 1,
0132     HWSIM_REGTEST_DRIVER_REG_ALL = 2,
0133     HWSIM_REGTEST_DIFF_COUNTRY = 3,
0134     HWSIM_REGTEST_WORLD_ROAM = 4,
0135     HWSIM_REGTEST_CUSTOM_WORLD = 5,
0136     HWSIM_REGTEST_CUSTOM_WORLD_2 = 6,
0137     HWSIM_REGTEST_STRICT_FOLLOW = 7,
0138     HWSIM_REGTEST_STRICT_ALL = 8,
0139     HWSIM_REGTEST_STRICT_AND_DRIVER_REG = 9,
0140     HWSIM_REGTEST_ALL = 10,
0141 };
0142
0143 /* Set to one of the HWSIM_REGTEST_* values above */
0144 static int regtest = HWSIM_REGTEST_DISABLED;
0145 module_param(regtest, int, 0444);
0146 MODULE_PARM_DESC(regtest, "The type of regulatory test we want to run");
0147
0148 static const char *hwsim_alpha2s[] = {
0149     "FI",
0150     "AL",
0151     "US",
0152     "DE",
0153     "JP",
0154     "AL",
0155 };
0156
0157 static const struct ieee80211_regdomain hwsim_world_regdom_custom_01 = {
0158     .n_reg_rules = 5,
0159     .alpha2 =  "99",
0160     .reg_rules = {
0161         REG_RULE(2412-10, 2462+10, 40, 0, 20, 0),
0162         REG_RULE(2484-10, 2484+10, 40, 0, 20, 0),
0163         REG_RULE(5150-10, 5240+10, 40, 0, 30, 0),
0164         REG_RULE(5745-10, 5825+10, 40, 0, 30, 0),
0165         REG_RULE(5855-10, 5925+10, 40, 0, 33, 0),
0166     }
0167 };
0168
0169 static const struct ieee80211_regdomain hwsim_world_regdom_custom_02 = {
0170     .n_reg_rules = 3,
0171     .alpha2 =  "99",
0172     .reg_rules = {
0173         REG_RULE(2412-10, 2462+10, 40, 0, 20, 0),
0174         REG_RULE(5725-10, 5850+10, 40, 0, 30,
0175              NL80211_RRF_NO_IR),
0176         REG_RULE(5855-10, 5925+10, 40, 0, 33, 0),
0177     }
0178 };
0179
0180 static const struct ieee80211_regdomain hwsim_world_regdom_custom_03 = {
0181     .n_reg_rules = 6,
0182     .alpha2 =  "99",
0183     .reg_rules = {
0184         REG_RULE(2412 - 10, 2462 + 10, 40, 0, 20, 0),
0185         REG_RULE(2484 - 10, 2484 + 10, 40, 0, 20, 0),
0186         REG_RULE(5150 - 10, 5240 + 10, 40, 0, 30, 0),
0187         REG_RULE(5745 - 10, 5825 + 10, 40, 0, 30, 0),
0188         REG_RULE(5855 - 10, 5925 + 10, 40, 0, 33, 0),
0189         REG_RULE(5955 - 10, 7125 + 10, 320, 0, 33, 0),
0190     }
0191 };
0192
0193 static const struct ieee80211_regdomain *hwsim_world_regdom_custom[] = {
0194     &hwsim_world_regdom_custom_01,
0195     &hwsim_world_regdom_custom_02,
0196     &hwsim_world_regdom_custom_03,
0197 };
0198
0199 struct hwsim_vif_priv {
0200     u32 magic;
0201     u8 bssid[ETH_ALEN];
0202     bool assoc;
0203     bool bcn_en;
0204     u16 aid;
0205 };
0206
0207 #define HWSIM_VIF_MAGIC 0x69537748
0208
0209 static inline void hwsim_check_magic(struct ieee80211_vif *vif)
0210 {
0211     struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
0212     WARN(vp->magic != HWSIM_VIF_MAGIC,
0213          "Invalid VIF (%p) magic %#x, %pM, %d/%d\n",
0214          vif, vp->magic, vif->addr, vif->type, vif->p2p);
0215 }
0216
0217 static inline void hwsim_set_magic(struct ieee80211_vif *vif)
0218 {
0219     struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
0220     vp->magic = HWSIM_VIF_MAGIC;
0221 }
0222
0223 static inline void hwsim_clear_magic(struct ieee80211_vif *vif)
0224 {
0225     struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
0226     vp->magic = 0;
0227 }
0228
0229 struct hwsim_sta_priv {
0230     u32 magic;
0231     unsigned int last_link;
0232 };
0233
0234 #define HWSIM_STA_MAGIC 0x6d537749
0235
0236 static inline void hwsim_check_sta_magic(struct ieee80211_sta *sta)
0237 {
0238     struct hwsim_sta_priv *sp = (void *)sta->drv_priv;
0239     WARN_ON(sp->magic != HWSIM_STA_MAGIC);
0240 }
0241
0242 static inline void hwsim_set_sta_magic(struct ieee80211_sta *sta)
0243 {
0244     struct hwsim_sta_priv *sp = (void *)sta->drv_priv;
0245     sp->magic = HWSIM_STA_MAGIC;
0246 }
0247
0248 static inline void hwsim_clear_sta_magic(struct ieee80211_sta *sta)
0249 {
0250     struct hwsim_sta_priv *sp = (void *)sta->drv_priv;
0251     sp->magic = 0;
0252 }
0253
0254 struct hwsim_chanctx_priv {
0255     u32 magic;
0256 };
0257
0258 #define HWSIM_CHANCTX_MAGIC 0x6d53774a
0259
0260 static inline void hwsim_check_chanctx_magic(struct ieee80211_chanctx_conf *c)
0261 {
0262     struct hwsim_chanctx_priv *cp = (void *)c->drv_priv;
0263     WARN_ON(cp->magic != HWSIM_CHANCTX_MAGIC);
0264 }
0265
0266 static inline void hwsim_set_chanctx_magic(struct ieee80211_chanctx_conf *c)
0267 {
0268     struct hwsim_chanctx_priv *cp = (void *)c->drv_priv;
0269     cp->magic = HWSIM_CHANCTX_MAGIC;
0270 }
0271
0272 static inline void hwsim_clear_chanctx_magic(struct ieee80211_chanctx_conf *c)
0273 {
0274     struct hwsim_chanctx_priv *cp = (void *)c->drv_priv;
0275     cp->magic = 0;
0276 }
0277
0278 static unsigned int hwsim_net_id;
0279
0280 static DEFINE_IDA(hwsim_netgroup_ida);
0281
0282 struct hwsim_net {
0283     int netgroup;
0284     u32 wmediumd;
0285 };
0286
0287 static inline int hwsim_net_get_netgroup(struct net *net)
0288 {
0289     struct hwsim_net *hwsim_net = net_generic(net, hwsim_net_id);
0290
0291     return hwsim_net->netgroup;
0292 }
0293
0294 static inline int hwsim_net_set_netgroup(struct net *net)
0295 {
0296     struct hwsim_net *hwsim_net = net_generic(net, hwsim_net_id);
0297
0298     hwsim_net->netgroup = ida_alloc(&hwsim_netgroup_ida, GFP_KERNEL);
0299     return hwsim_net->netgroup >= 0 ? 0 : -ENOMEM;
0300 }
0301
0302 static inline u32 hwsim_net_get_wmediumd(struct net *net)
0303 {
0304     struct hwsim_net *hwsim_net = net_generic(net, hwsim_net_id);
0305
0306     return hwsim_net->wmediumd;
0307 }
0308
0309 static inline void hwsim_net_set_wmediumd(struct net *net, u32 portid)
0310 {
0311     struct hwsim_net *hwsim_net = net_generic(net, hwsim_net_id);
0312
0313     hwsim_net->wmediumd = portid;
0314 }
0315
0316 static struct class *hwsim_class;
0317
0318 static struct net_device *hwsim_mon; /* global monitor netdev */
0319
0320 #define CHAN2G(_freq)  { \
0321     .band = NL80211_BAND_2GHZ, \
0322     .center_freq = (_freq), \
0323     .hw_value = (_freq), \
0324 }
0325
0326 #define CHAN5G(_freq) { \
0327     .band = NL80211_BAND_5GHZ, \
0328     .center_freq = (_freq), \
0329     .hw_value = (_freq), \
0330 }
0331
0332 #define CHAN6G(_freq) { \
0333     .band = NL80211_BAND_6GHZ, \
0334     .center_freq = (_freq), \
0335     .hw_value = (_freq), \
0336 }
0337
0338 static const struct ieee80211_channel hwsim_channels_2ghz[] = {
0339     CHAN2G(2412), /* Channel 1 */
0340     CHAN2G(2417), /* Channel 2 */
0341     CHAN2G(2422), /* Channel 3 */
0342     CHAN2G(2427), /* Channel 4 */
0343     CHAN2G(2432), /* Channel 5 */
0344     CHAN2G(2437), /* Channel 6 */
0345     CHAN2G(2442), /* Channel 7 */
0346     CHAN2G(2447), /* Channel 8 */
0347     CHAN2G(2452), /* Channel 9 */
0348     CHAN2G(2457), /* Channel 10 */
0349     CHAN2G(2462), /* Channel 11 */
0350     CHAN2G(2467), /* Channel 12 */
0351     CHAN2G(2472), /* Channel 13 */
0352     CHAN2G(2484), /* Channel 14 */
0353 };
0354
0355 static const struct ieee80211_channel hwsim_channels_5ghz[] = {
0356     CHAN5G(5180), /* Channel 36 */
0357     CHAN5G(5200), /* Channel 40 */
0358     CHAN5G(5220), /* Channel 44 */
0359     CHAN5G(5240), /* Channel 48 */
0360
0361     CHAN5G(5260), /* Channel 52 */
0362     CHAN5G(5280), /* Channel 56 */
0363     CHAN5G(5300), /* Channel 60 */
0364     CHAN5G(5320), /* Channel 64 */
0365
0366     CHAN5G(5500), /* Channel 100 */
0367     CHAN5G(5520), /* Channel 104 */
0368     CHAN5G(5540), /* Channel 108 */
0369     CHAN5G(5560), /* Channel 112 */
0370     CHAN5G(5580), /* Channel 116 */
0371     CHAN5G(5600), /* Channel 120 */
0372     CHAN5G(5620), /* Channel 124 */
0373     CHAN5G(5640), /* Channel 128 */
0374     CHAN5G(5660), /* Channel 132 */
0375     CHAN5G(5680), /* Channel 136 */
0376     CHAN5G(5700), /* Channel 140 */
0377
0378     CHAN5G(5745), /* Channel 149 */
0379     CHAN5G(5765), /* Channel 153 */
0380     CHAN5G(5785), /* Channel 157 */
0381     CHAN5G(5805), /* Channel 161 */
0382     CHAN5G(5825), /* Channel 165 */
0383     CHAN5G(5845), /* Channel 169 */
0384
0385     CHAN5G(5855), /* Channel 171 */
0386     CHAN5G(5860), /* Channel 172 */
0387     CHAN5G(5865), /* Channel 173 */
0388     CHAN5G(5870), /* Channel 174 */
0389
0390     CHAN5G(5875), /* Channel 175 */
0391     CHAN5G(5880), /* Channel 176 */
0392     CHAN5G(5885), /* Channel 177 */
0393     CHAN5G(5890), /* Channel 178 */
0394     CHAN5G(5895), /* Channel 179 */
0395     CHAN5G(5900), /* Channel 180 */
0396     CHAN5G(5905), /* Channel 181 */
0397
0398     CHAN5G(5910), /* Channel 182 */
0399     CHAN5G(5915), /* Channel 183 */
0400     CHAN5G(5920), /* Channel 184 */
0401     CHAN5G(5925), /* Channel 185 */
0402 };
0403
0404 static const struct ieee80211_channel hwsim_channels_6ghz[] = {
0405     CHAN6G(5955), /* Channel 1 */
0406     CHAN6G(5975), /* Channel 5 */
0407     CHAN6G(5995), /* Channel 9 */
0408     CHAN6G(6015), /* Channel 13 */
0409     CHAN6G(6035), /* Channel 17 */
0410     CHAN6G(6055), /* Channel 21 */
0411     CHAN6G(6075), /* Channel 25 */
0412     CHAN6G(6095), /* Channel 29 */
0413     CHAN6G(6115), /* Channel 33 */
0414     CHAN6G(6135), /* Channel 37 */
0415     CHAN6G(6155), /* Channel 41 */
0416     CHAN6G(6175), /* Channel 45 */
0417     CHAN6G(6195), /* Channel 49 */
0418     CHAN6G(6215), /* Channel 53 */
0419     CHAN6G(6235), /* Channel 57 */
0420     CHAN6G(6255), /* Channel 61 */
0421     CHAN6G(6275), /* Channel 65 */
0422     CHAN6G(6295), /* Channel 69 */
0423     CHAN6G(6315), /* Channel 73 */
0424     CHAN6G(6335), /* Channel 77 */
0425     CHAN6G(6355), /* Channel 81 */
0426     CHAN6G(6375), /* Channel 85 */
0427     CHAN6G(6395), /* Channel 89 */
0428     CHAN6G(6415), /* Channel 93 */
0429     CHAN6G(6435), /* Channel 97 */
0430     CHAN6G(6455), /* Channel 181 */
0431     CHAN6G(6475), /* Channel 105 */
0432     CHAN6G(6495), /* Channel 109 */
0433     CHAN6G(6515), /* Channel 113 */
0434     CHAN6G(6535), /* Channel 117 */
0435     CHAN6G(6555), /* Channel 121 */
0436     CHAN6G(6575), /* Channel 125 */
0437     CHAN6G(6595), /* Channel 129 */
0438     CHAN6G(6615), /* Channel 133 */
0439     CHAN6G(6635), /* Channel 137 */
0440     CHAN6G(6655), /* Channel 141 */
0441     CHAN6G(6675), /* Channel 145 */
0442     CHAN6G(6695), /* Channel 149 */
0443     CHAN6G(6715), /* Channel 153 */
0444     CHAN6G(6735), /* Channel 157 */
0445     CHAN6G(6755), /* Channel 161 */
0446     CHAN6G(6775), /* Channel 165 */
0447     CHAN6G(6795), /* Channel 169 */
0448     CHAN6G(6815), /* Channel 173 */
0449     CHAN6G(6835), /* Channel 177 */
0450     CHAN6G(6855), /* Channel 181 */
0451     CHAN6G(6875), /* Channel 185 */
0452     CHAN6G(6895), /* Channel 189 */
0453     CHAN6G(6915), /* Channel 193 */
0454     CHAN6G(6935), /* Channel 197 */
0455     CHAN6G(6955), /* Channel 201 */
0456     CHAN6G(6975), /* Channel 205 */
0457     CHAN6G(6995), /* Channel 209 */
0458     CHAN6G(7015), /* Channel 213 */
0459     CHAN6G(7035), /* Channel 217 */
0460     CHAN6G(7055), /* Channel 221 */
0461     CHAN6G(7075), /* Channel 225 */
0462     CHAN6G(7095), /* Channel 229 */
0463     CHAN6G(7115), /* Channel 233 */
0464 };
0465
0466 #define NUM_S1G_CHANS_US 51
0467 static struct ieee80211_channel hwsim_channels_s1g[NUM_S1G_CHANS_US];
0468
0469 static const struct ieee80211_sta_s1g_cap hwsim_s1g_cap = {
0470     .s1g = true,
0471     .cap = { S1G_CAP0_SGI_1MHZ | S1G_CAP0_SGI_2MHZ,
0472          0,
0473          0,
0474          S1G_CAP3_MAX_MPDU_LEN,
0475          0,
0476          S1G_CAP5_AMPDU,
0477          0,
0478          S1G_CAP7_DUP_1MHZ,
0479          S1G_CAP8_TWT_RESPOND | S1G_CAP8_TWT_REQUEST,
0480          0},
0481     .nss_mcs = { 0xfc | 1, /* MCS 7 for 1 SS */
0482     /* RX Highest Supported Long GI Data Rate 0:7 */
0483              0,
0484     /* RX Highest Supported Long GI Data Rate 0:7 */
0485     /* TX S1G MCS Map 0:6 */
0486              0xfa,
0487     /* TX S1G MCS Map :7 */
0488     /* TX Highest Supported Long GI Data Rate 0:6 */
0489              0x80,
0490     /* TX Highest Supported Long GI Data Rate 7:8 */
0491     /* Rx Single spatial stream and S1G-MCS Map for 1MHz */
0492     /* Tx Single spatial stream and S1G-MCS Map for 1MHz */
0493              0 },
0494 };
0495
0496 static void hwsim_init_s1g_channels(struct ieee80211_channel *chans)
0497 {
0498     int ch, freq;
0499
0500     for (ch = 0; ch < NUM_S1G_CHANS_US; ch++) {
0501         freq = 902000 + (ch + 1) * 500;
0502         chans[ch].band = NL80211_BAND_S1GHZ;
0503         chans[ch].center_freq = KHZ_TO_MHZ(freq);
0504         chans[ch].freq_offset = freq % 1000;
0505         chans[ch].hw_value = ch + 1;
0506     }
0507 }
0508
0509 static const struct ieee80211_rate hwsim_rates[] = {
0510     { .bitrate = 10 },
0511     { .bitrate = 20, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
0512     { .bitrate = 55, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
0513     { .bitrate = 110, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
0514     { .bitrate = 60 },
0515     { .bitrate = 90 },
0516     { .bitrate = 120 },
0517     { .bitrate = 180 },
0518     { .bitrate = 240 },
0519     { .bitrate = 360 },
0520     { .bitrate = 480 },
0521     { .bitrate = 540 }
0522 };
0523
0524 #define DEFAULT_RX_RSSI -50
0525
0526 static const u32 hwsim_ciphers[] = {
0527     WLAN_CIPHER_SUITE_WEP40,
0528     WLAN_CIPHER_SUITE_WEP104,
0529     WLAN_CIPHER_SUITE_TKIP,
0530     WLAN_CIPHER_SUITE_CCMP,
0531     WLAN_CIPHER_SUITE_CCMP_256,
0532     WLAN_CIPHER_SUITE_GCMP,
0533     WLAN_CIPHER_SUITE_GCMP_256,
0534     WLAN_CIPHER_SUITE_AES_CMAC,
0535     WLAN_CIPHER_SUITE_BIP_CMAC_256,
0536     WLAN_CIPHER_SUITE_BIP_GMAC_128,
0537     WLAN_CIPHER_SUITE_BIP_GMAC_256,
0538 };
0539
0540 #define OUI_QCA 0x001374
0541 #define QCA_NL80211_SUBCMD_TEST 1
0542 enum qca_nl80211_vendor_subcmds {
0543     QCA_WLAN_VENDOR_ATTR_TEST = 8,
0544     QCA_WLAN_VENDOR_ATTR_MAX = QCA_WLAN_VENDOR_ATTR_TEST
0545 };
0546
0547 static const struct nla_policy
0548 hwsim_vendor_test_policy[QCA_WLAN_VENDOR_ATTR_MAX + 1] = {
0549     [QCA_WLAN_VENDOR_ATTR_MAX] = { .type = NLA_U32 },
0550 };
0551
0552 static int mac80211_hwsim_vendor_cmd_test(struct wiphy *wiphy,
0553                       struct wireless_dev *wdev,
0554                       const void *data, int data_len)
0555 {
0556     struct sk_buff *skb;
0557     struct nlattr *tb[QCA_WLAN_VENDOR_ATTR_MAX + 1];
0558     int err;
0559     u32 val;
0560
0561     err = nla_parse_deprecated(tb, QCA_WLAN_VENDOR_ATTR_MAX, data,
0562                    data_len, hwsim_vendor_test_policy, NULL);
0563     if (err)
0564         return err;
0565     if (!tb[QCA_WLAN_VENDOR_ATTR_TEST])
0566         return -EINVAL;
0567     val = nla_get_u32(tb[QCA_WLAN_VENDOR_ATTR_TEST]);
0568     wiphy_dbg(wiphy, "%s: test=%u\n", __func__, val);
0569
0570     /* Send a vendor event as a test. Note that this would not normally be
0571      * done within a command handler, but rather, based on some other
0572      * trigger. For simplicity, this command is used to trigger the event
0573      * here.
0574      *
0575      * event_idx = 0 (index in mac80211_hwsim_vendor_commands)
0576      */
0577     skb = cfg80211_vendor_event_alloc(wiphy, wdev, 100, 0, GFP_KERNEL);
0578     if (skb) {
0579         /* skb_put() or nla_put() will fill up data within
0580          * NL80211_ATTR_VENDOR_DATA.
0581          */
0582
0583         /* Add vendor data */
0584         nla_put_u32(skb, QCA_WLAN_VENDOR_ATTR_TEST, val + 1);
0585
0586         /* Send the event - this will call nla_nest_end() */
0587         cfg80211_vendor_event(skb, GFP_KERNEL);
0588     }
0589
0590     /* Send a response to the command */
0591     skb = cfg80211_vendor_cmd_alloc_reply_skb(wiphy, 10);
0592     if (!skb)
0593         return -ENOMEM;
0594
0595     /* skb_put() or nla_put() will fill up data within
0596      * NL80211_ATTR_VENDOR_DATA
0597      */
0598     nla_put_u32(skb, QCA_WLAN_VENDOR_ATTR_TEST, val + 2);
0599
0600     return cfg80211_vendor_cmd_reply(skb);
0601 }
0602
0603 static struct wiphy_vendor_command mac80211_hwsim_vendor_commands[] = {
0604     {
0605         .info = { .vendor_id = OUI_QCA,
0606               .subcmd = QCA_NL80211_SUBCMD_TEST },
0607         .flags = WIPHY_VENDOR_CMD_NEED_NETDEV,
0608         .doit = mac80211_hwsim_vendor_cmd_test,
0609         .policy = hwsim_vendor_test_policy,
0610         .maxattr = QCA_WLAN_VENDOR_ATTR_MAX,
0611     }
0612 };
0613
0614 /* Advertise support vendor specific events */
0615 static const struct nl80211_vendor_cmd_info mac80211_hwsim_vendor_events[] = {
0616     { .vendor_id = OUI_QCA, .subcmd = 1 },
0617 };
0618
0619 static DEFINE_SPINLOCK(hwsim_radio_lock);
0620 static LIST_HEAD(hwsim_radios);
0621 static struct rhashtable hwsim_radios_rht;
0622 static int hwsim_radio_idx;
0623 static int hwsim_radios_generation = 1;
0624
0625 static struct platform_driver mac80211_hwsim_driver = {
0626     .driver = {
0627         .name = "mac80211_hwsim",
0628     },
0629 };
0630
0631 struct mac80211_hwsim_link_data {
0632     u32 link_id;
0633     u64 beacon_int  /* beacon interval in us */;
0634     struct hrtimer beacon_timer;
0635 };
0636
0637 struct mac80211_hwsim_data {
0638     struct list_head list;
0639     struct rhash_head rht;
0640     struct ieee80211_hw *hw;
0641     struct device *dev;
0642     struct ieee80211_supported_band bands[NUM_NL80211_BANDS];
0643     struct ieee80211_channel channels_2ghz[ARRAY_SIZE(hwsim_channels_2ghz)];
0644     struct ieee80211_channel channels_5ghz[ARRAY_SIZE(hwsim_channels_5ghz)];
0645     struct ieee80211_channel channels_6ghz[ARRAY_SIZE(hwsim_channels_6ghz)];
0646     struct ieee80211_channel channels_s1g[ARRAY_SIZE(hwsim_channels_s1g)];
0647     struct ieee80211_rate rates[ARRAY_SIZE(hwsim_rates)];
0648     struct ieee80211_iface_combination if_combination;
0649     struct ieee80211_iface_limit if_limits[3];
0650     int n_if_limits;
0651
0652     u32 ciphers[ARRAY_SIZE(hwsim_ciphers)];
0653
0654     struct mac_address addresses[2];
0655     struct ieee80211_chanctx_conf *chanctx;
0656     int channels, idx;
0657     bool use_chanctx;
0658     bool destroy_on_close;
0659     u32 portid;
0660     char alpha2[2];
0661     const struct ieee80211_regdomain *regd;
0662
0663     struct ieee80211_channel *tmp_chan;
0664     struct ieee80211_channel *roc_chan;
0665     u32 roc_duration;
0666     struct delayed_work roc_start;
0667     struct delayed_work roc_done;
0668     struct delayed_work hw_scan;
0669     struct cfg80211_scan_request *hw_scan_request;
0670     struct ieee80211_vif *hw_scan_vif;
0671     int scan_chan_idx;
0672     u8 scan_addr[ETH_ALEN];
0673     struct {
0674         struct ieee80211_channel *channel;
0675         unsigned long next_start, start, end;
0676     } survey_data[ARRAY_SIZE(hwsim_channels_2ghz) +
0677               ARRAY_SIZE(hwsim_channels_5ghz) +
0678               ARRAY_SIZE(hwsim_channels_6ghz)];
0679
0680     struct ieee80211_channel *channel;
0681     enum nl80211_chan_width bw;
0682     unsigned int rx_filter;
0683     bool started, idle, scanning;
0684     struct mutex mutex;
0685     enum ps_mode {
0686         PS_DISABLED, PS_ENABLED, PS_AUTO_POLL, PS_MANUAL_POLL
0687     } ps;
0688     bool ps_poll_pending;
0689     struct dentry *debugfs;
0690
0691     atomic_t pending_cookie;
0692     struct sk_buff_head pending;    /* packets pending */
0693     /*
0694      * Only radios in the same group can communicate together (the
0695      * channel has to match too). Each bit represents a group. A
0696      * radio can be in more than one group.
0697      */
0698     u64 group;
0699
0700     /* group shared by radios created in the same netns */
0701     int netgroup;
0702     /* wmediumd portid responsible for netgroup of this radio */
0703     u32 wmediumd;
0704
0705     /* difference between this hw's clock and the real clock, in usecs */
0706     s64 tsf_offset;
0707     s64 bcn_delta;
0708     /* absolute beacon transmission time. Used to cover up "tx" delay. */
0709     u64 abs_bcn_ts;
0710
0711     /* Stats */
0712     u64 tx_pkts;
0713     u64 rx_pkts;
0714     u64 tx_bytes;
0715     u64 rx_bytes;
0716     u64 tx_dropped;
0717     u64 tx_failed;
0718
0719     /* RSSI in rx status of the receiver */
0720     int rx_rssi;
0721
0722     struct mac80211_hwsim_link_data link_data[IEEE80211_MLD_MAX_NUM_LINKS];
0723 };
0724
0725 static const struct rhashtable_params hwsim_rht_params = {
0726     .nelem_hint = 2,
0727     .automatic_shrinking = true,
0728     .key_len = ETH_ALEN,
0729     .key_offset = offsetof(struct mac80211_hwsim_data, addresses[1]),
0730     .head_offset = offsetof(struct mac80211_hwsim_data, rht),
0731 };
0732
0733 struct hwsim_radiotap_hdr {
0734     struct ieee80211_radiotap_header hdr;
0735     __le64 rt_tsft;
0736     u8 rt_flags;
0737     u8 rt_rate;
0738     __le16 rt_channel;
0739     __le16 rt_chbitmask;
0740 } __packed;
0741
0742 struct hwsim_radiotap_ack_hdr {
0743     struct ieee80211_radiotap_header hdr;
0744     u8 rt_flags;
0745     u8 pad;
0746     __le16 rt_channel;
0747     __le16 rt_chbitmask;
0748 } __packed;
0749
0750 /* MAC80211_HWSIM netlink family */
0751 static struct genl_family hwsim_genl_family;
0752
0753 enum hwsim_multicast_groups {
0754     HWSIM_MCGRP_CONFIG,
0755 };
0756
0757 static const struct genl_multicast_group hwsim_mcgrps[] = {
0758     [HWSIM_MCGRP_CONFIG] = { .name = "config", },
0759 };
0760
0761 /* MAC80211_HWSIM netlink policy */
0762
0763 static const struct nla_policy hwsim_genl_policy[HWSIM_ATTR_MAX + 1] = {
0764     [HWSIM_ATTR_ADDR_RECEIVER] = NLA_POLICY_ETH_ADDR_COMPAT,
0765     [HWSIM_ATTR_ADDR_TRANSMITTER] = NLA_POLICY_ETH_ADDR_COMPAT,
0766     [HWSIM_ATTR_FRAME] = { .type = NLA_BINARY,
0767                    .len = IEEE80211_MAX_DATA_LEN },
0768     [HWSIM_ATTR_FLAGS] = { .type = NLA_U32 },
0769     [HWSIM_ATTR_RX_RATE] = { .type = NLA_U32 },
0770     [HWSIM_ATTR_SIGNAL] = { .type = NLA_U32 },
0771     [HWSIM_ATTR_TX_INFO] = { .type = NLA_BINARY,
0772                  .len = IEEE80211_TX_MAX_RATES *
0773                     sizeof(struct hwsim_tx_rate)},
0774     [HWSIM_ATTR_COOKIE] = { .type = NLA_U64 },
0775     [HWSIM_ATTR_CHANNELS] = { .type = NLA_U32 },
0776     [HWSIM_ATTR_RADIO_ID] = { .type = NLA_U32 },
0777     [HWSIM_ATTR_REG_HINT_ALPHA2] = { .type = NLA_STRING, .len = 2 },
0778     [HWSIM_ATTR_REG_CUSTOM_REG] = { .type = NLA_U32 },
0779     [HWSIM_ATTR_REG_STRICT_REG] = { .type = NLA_FLAG },
0780     [HWSIM_ATTR_SUPPORT_P2P_DEVICE] = { .type = NLA_FLAG },
0781     [HWSIM_ATTR_USE_CHANCTX] = { .type = NLA_FLAG },
0782     [HWSIM_ATTR_DESTROY_RADIO_ON_CLOSE] = { .type = NLA_FLAG },
0783     [HWSIM_ATTR_RADIO_NAME] = { .type = NLA_STRING },
0784     [HWSIM_ATTR_NO_VIF] = { .type = NLA_FLAG },
0785     [HWSIM_ATTR_FREQ] = { .type = NLA_U32 },
0786     [HWSIM_ATTR_TX_INFO_FLAGS] = { .type = NLA_BINARY },
0787     [HWSIM_ATTR_PERM_ADDR] = NLA_POLICY_ETH_ADDR_COMPAT,
0788     [HWSIM_ATTR_IFTYPE_SUPPORT] = { .type = NLA_U32 },
0789     [HWSIM_ATTR_CIPHER_SUPPORT] = { .type = NLA_BINARY },
0790     [HWSIM_ATTR_MLO_SUPPORT] = { .type = NLA_FLAG },
0791 };
0792
0793 #if IS_REACHABLE(CONFIG_VIRTIO)
0794
0795 /* MAC80211_HWSIM virtio queues */
0796 static struct virtqueue *hwsim_vqs[HWSIM_NUM_VQS];
0797 static bool hwsim_virtio_enabled;
0798 static DEFINE_SPINLOCK(hwsim_virtio_lock);
0799
0800 static void hwsim_virtio_rx_work(struct work_struct *work);
0801 static DECLARE_WORK(hwsim_virtio_rx, hwsim_virtio_rx_work);
0802
0803 static int hwsim_tx_virtio(struct mac80211_hwsim_data *data,
0804                struct sk_buff *skb)
0805 {
0806     struct scatterlist sg[1];
0807     unsigned long flags;
0808     int err;
0809
0810     spin_lock_irqsave(&hwsim_virtio_lock, flags);
0811     if (!hwsim_virtio_enabled) {
0812         err = -ENODEV;
0813         goto out_free;
0814     }
0815
0816     sg_init_one(sg, skb->head, skb_end_offset(skb));
0817     err = virtqueue_add_outbuf(hwsim_vqs[HWSIM_VQ_TX], sg, 1, skb,
0818                    GFP_ATOMIC);
0819     if (err)
0820         goto out_free;
0821     virtqueue_kick(hwsim_vqs[HWSIM_VQ_TX]);
0822     spin_unlock_irqrestore(&hwsim_virtio_lock, flags);
0823     return 0;
0824
0825 out_free:
0826     spin_unlock_irqrestore(&hwsim_virtio_lock, flags);
0827     nlmsg_free(skb);
0828     return err;
0829 }
0830 #else
0831 /* cause a linker error if this ends up being needed */
0832 extern int hwsim_tx_virtio(struct mac80211_hwsim_data *data,
0833                struct sk_buff *skb);
0834 #define hwsim_virtio_enabled false
0835 #endif
0836
0837 static int hwsim_get_chanwidth(enum nl80211_chan_width bw)
0838 {
0839     switch (bw) {
0840     case NL80211_CHAN_WIDTH_20_NOHT:
0841     case NL80211_CHAN_WIDTH_20:
0842         return 20;
0843     case NL80211_CHAN_WIDTH_40:
0844         return 40;
0845     case NL80211_CHAN_WIDTH_80:
0846         return 80;
0847     case NL80211_CHAN_WIDTH_80P80:
0848     case NL80211_CHAN_WIDTH_160:
0849         return 160;
0850     case NL80211_CHAN_WIDTH_320:
0851         return 320;
0852     case NL80211_CHAN_WIDTH_5:
0853         return 5;
0854     case NL80211_CHAN_WIDTH_10:
0855         return 10;
0856     case NL80211_CHAN_WIDTH_1:
0857         return 1;
0858     case NL80211_CHAN_WIDTH_2:
0859         return 2;
0860     case NL80211_CHAN_WIDTH_4:
0861         return 4;
0862     case NL80211_CHAN_WIDTH_8:
0863         return 8;
0864     case NL80211_CHAN_WIDTH_16:
0865         return 16;
0866     }
0867
0868     return INT_MAX;
0869 }
0870
0871 static void mac80211_hwsim_tx_frame(struct ieee80211_hw *hw,
0872                     struct sk_buff *skb,
0873                     struct ieee80211_channel *chan);
0874
0875 /* sysfs attributes */
0876 static void hwsim_send_ps_poll(void *dat, u8 *mac, struct ieee80211_vif *vif)
0877 {
0878     struct mac80211_hwsim_data *data = dat;
0879     struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
0880     struct sk_buff *skb;
0881     struct ieee80211_pspoll *pspoll;
0882
0883     if (!vp->assoc)
0884         return;
0885
0886     wiphy_dbg(data->hw->wiphy,
0887           "%s: send PS-Poll to %pM for aid %d\n",
0888           __func__, vp->bssid, vp->aid);
0889
0890     skb = dev_alloc_skb(sizeof(*pspoll));
0891     if (!skb)
0892         return;
0893     pspoll = skb_put(skb, sizeof(*pspoll));
0894     pspoll->frame_control = cpu_to_le16(IEEE80211_FTYPE_CTL |
0895                         IEEE80211_STYPE_PSPOLL |
0896                         IEEE80211_FCTL_PM);
0897     pspoll->aid = cpu_to_le16(0xc000 | vp->aid);
0898     memcpy(pspoll->bssid, vp->bssid, ETH_ALEN);
0899     memcpy(pspoll->ta, mac, ETH_ALEN);
0900
0901     rcu_read_lock();
0902     mac80211_hwsim_tx_frame(data->hw, skb,
0903                 rcu_dereference(vif->bss_conf.chanctx_conf)->def.chan);
0904     rcu_read_unlock();
0905 }
0906
0907 static void hwsim_send_nullfunc(struct mac80211_hwsim_data *data, u8 *mac,
0908                 struct ieee80211_vif *vif, int ps)
0909 {
0910     struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
0911     struct sk_buff *skb;
0912     struct ieee80211_hdr *hdr;
0913
0914     if (!vp->assoc)
0915         return;
0916
0917     wiphy_dbg(data->hw->wiphy,
0918           "%s: send data::nullfunc to %pM ps=%d\n",
0919           __func__, vp->bssid, ps);
0920
0921     skb = dev_alloc_skb(sizeof(*hdr));
0922     if (!skb)
0923         return;
0924     hdr = skb_put(skb, sizeof(*hdr) - ETH_ALEN);
0925     hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
0926                      IEEE80211_STYPE_NULLFUNC |
0927                      IEEE80211_FCTL_TODS |
0928                      (ps ? IEEE80211_FCTL_PM : 0));
0929     hdr->duration_id = cpu_to_le16(0);
0930     memcpy(hdr->addr1, vp->bssid, ETH_ALEN);
0931     memcpy(hdr->addr2, mac, ETH_ALEN);
0932     memcpy(hdr->addr3, vp->bssid, ETH_ALEN);
0933
0934     rcu_read_lock();
0935     mac80211_hwsim_tx_frame(data->hw, skb,
0936                 rcu_dereference(vif->bss_conf.chanctx_conf)->def.chan);
0937     rcu_read_unlock();
0938 }
0939
0940
0941 static void hwsim_send_nullfunc_ps(void *dat, u8 *mac,
0942                    struct ieee80211_vif *vif)
0943 {
0944     struct mac80211_hwsim_data *data = dat;
0945     hwsim_send_nullfunc(data, mac, vif, 1);
0946 }
0947
0948 static void hwsim_send_nullfunc_no_ps(void *dat, u8 *mac,
0949                       struct ieee80211_vif *vif)
0950 {
0951     struct mac80211_hwsim_data *data = dat;
0952     hwsim_send_nullfunc(data, mac, vif, 0);
0953 }
0954
0955 static int hwsim_fops_ps_read(void *dat, u64 *val)
0956 {
0957     struct mac80211_hwsim_data *data = dat;
0958     *val = data->ps;
0959     return 0;
0960 }
0961
0962 static int hwsim_fops_ps_write(void *dat, u64 val)
0963 {
0964     struct mac80211_hwsim_data *data = dat;
0965     enum ps_mode old_ps;
0966
0967     if (val != PS_DISABLED && val != PS_ENABLED && val != PS_AUTO_POLL &&
0968         val != PS_MANUAL_POLL)
0969         return -EINVAL;
0970
0971     if (val == PS_MANUAL_POLL) {
0972         if (data->ps != PS_ENABLED)
0973             return -EINVAL;
0974         local_bh_disable();
0975         ieee80211_iterate_active_interfaces_atomic(
0976             data->hw, IEEE80211_IFACE_ITER_NORMAL,
0977             hwsim_send_ps_poll, data);
0978         local_bh_enable();
0979         return 0;
0980     }
0981     old_ps = data->ps;
0982     data->ps = val;
0983
0984     local_bh_disable();
0985     if (old_ps == PS_DISABLED && val != PS_DISABLED) {
0986         ieee80211_iterate_active_interfaces_atomic(
0987             data->hw, IEEE80211_IFACE_ITER_NORMAL,
0988             hwsim_send_nullfunc_ps, data);
0989     } else if (old_ps != PS_DISABLED && val == PS_DISABLED) {
0990         ieee80211_iterate_active_interfaces_atomic(
0991             data->hw, IEEE80211_IFACE_ITER_NORMAL,
0992             hwsim_send_nullfunc_no_ps, data);
0993     }
0994     local_bh_enable();
0995
0996     return 0;
0997 }
0998
0999 DEFINE_DEBUGFS_ATTRIBUTE(hwsim_fops_ps, hwsim_fops_ps_read, hwsim_fops_ps_write,
1000              "%llu\n");
1001
1002 static int hwsim_write_simulate_radar(void *dat, u64 val)
1003 {
1004     struct mac80211_hwsim_data *data = dat;
1005
1006     ieee80211_radar_detected(data->hw);
1007
1008     return 0;
1009 }
1010
1011 DEFINE_DEBUGFS_ATTRIBUTE(hwsim_simulate_radar, NULL,
1012              hwsim_write_simulate_radar, "%llu\n");
1013
1014 static int hwsim_fops_group_read(void *dat, u64 *val)
1015 {
1016     struct mac80211_hwsim_data *data = dat;
1017     *val = data->group;
1018     return 0;
1019 }
1020
1021 static int hwsim_fops_group_write(void *dat, u64 val)
1022 {
1023     struct mac80211_hwsim_data *data = dat;
1024     data->group = val;
1025     return 0;
1026 }
1027
1028 DEFINE_DEBUGFS_ATTRIBUTE(hwsim_fops_group,
1029              hwsim_fops_group_read, hwsim_fops_group_write,
1030              "%llx\n");
1031
1032 static int hwsim_fops_rx_rssi_read(void *dat, u64 *val)
1033 {
1034     struct mac80211_hwsim_data *data = dat;
1035     *val = data->rx_rssi;
1036     return 0;
1037 }
1038
1039 static int hwsim_fops_rx_rssi_write(void *dat, u64 val)
1040 {
1041     struct mac80211_hwsim_data *data = dat;
1042     int rssi = (int)val;
1043
1044     if (rssi >= 0 || rssi < -100)
1045         return -EINVAL;
1046
1047     data->rx_rssi = rssi;
1048     return 0;
1049 }
1050
1051 DEFINE_DEBUGFS_ATTRIBUTE(hwsim_fops_rx_rssi,
1052              hwsim_fops_rx_rssi_read, hwsim_fops_rx_rssi_write,
1053              "%lld\n");
1054
1055 static netdev_tx_t hwsim_mon_xmit(struct sk_buff *skb,
1056                     struct net_device *dev)
1057 {
1058     /* TODO: allow packet injection */
1059     dev_kfree_skb(skb);
1060     return NETDEV_TX_OK;
1061 }
1062
1063 static inline u64 mac80211_hwsim_get_tsf_raw(void)
1064 {
1065     return ktime_to_us(ktime_get_real());
1066 }
1067
1068 static __le64 __mac80211_hwsim_get_tsf(struct mac80211_hwsim_data *data)
1069 {
1070     u64 now = mac80211_hwsim_get_tsf_raw();
1071     return cpu_to_le64(now + data->tsf_offset);
1072 }
1073
1074 static u64 mac80211_hwsim_get_tsf(struct ieee80211_hw *hw,
1075                   struct ieee80211_vif *vif)
1076 {
1077     struct mac80211_hwsim_data *data = hw->priv;
1078     return le64_to_cpu(__mac80211_hwsim_get_tsf(data));
1079 }
1080
1081 static void mac80211_hwsim_set_tsf(struct ieee80211_hw *hw,
1082         struct ieee80211_vif *vif, u64 tsf)
1083 {
1084     struct mac80211_hwsim_data *data = hw->priv;
1085     u64 now = mac80211_hwsim_get_tsf(hw, vif);
1086     /* MLD not supported here */
1087     u32 bcn_int = data->link_data[0].beacon_int;
1088     u64 delta = abs(tsf - now);
1089
1090     /* adjust after beaconing with new timestamp at old TBTT */
1091     if (tsf > now) {
1092         data->tsf_offset += delta;
1093         data->bcn_delta = do_div(delta, bcn_int);
1094     } else {
1095         data->tsf_offset -= delta;
1096         data->bcn_delta = -(s64)do_div(delta, bcn_int);
1097     }
1098 }
1099
1100 static void mac80211_hwsim_monitor_rx(struct ieee80211_hw *hw,
1101                       struct sk_buff *tx_skb,
1102                       struct ieee80211_channel *chan)
1103 {
1104     struct mac80211_hwsim_data *data = hw->priv;
1105     struct sk_buff *skb;
1106     struct hwsim_radiotap_hdr *hdr;
1107     u16 flags, bitrate;
1108     struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_skb);
1109     struct ieee80211_rate *txrate = ieee80211_get_tx_rate(hw, info);
1110
1111     if (!txrate)
1112         bitrate = 0;
1113     else
1114         bitrate = txrate->bitrate;
1115
1116     if (!netif_running(hwsim_mon))
1117         return;
1118
1119     skb = skb_copy_expand(tx_skb, sizeof(*hdr), 0, GFP_ATOMIC);
1120     if (skb == NULL)
1121         return;
1122
1123     hdr = skb_push(skb, sizeof(*hdr));
1124     hdr->hdr.it_version = PKTHDR_RADIOTAP_VERSION;
1125     hdr->hdr.it_pad = 0;
1126     hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
1127     hdr->hdr.it_present = cpu_to_le32((1 << IEEE80211_RADIOTAP_FLAGS) |
1128                       (1 << IEEE80211_RADIOTAP_RATE) |
1129                       (1 << IEEE80211_RADIOTAP_TSFT) |
1130                       (1 << IEEE80211_RADIOTAP_CHANNEL));
1131     hdr->rt_tsft = __mac80211_hwsim_get_tsf(data);
1132     hdr->rt_flags = 0;
1133     hdr->rt_rate = bitrate / 5;
1134     hdr->rt_channel = cpu_to_le16(chan->center_freq);
1135     flags = IEEE80211_CHAN_2GHZ;
1136     if (txrate && txrate->flags & IEEE80211_RATE_ERP_G)
1137         flags |= IEEE80211_CHAN_OFDM;
1138     else
1139         flags |= IEEE80211_CHAN_CCK;
1140     hdr->rt_chbitmask = cpu_to_le16(flags);
1141
1142     skb->dev = hwsim_mon;
1143     skb_reset_mac_header(skb);
1144     skb->ip_summed = CHECKSUM_UNNECESSARY;
1145     skb->pkt_type = PACKET_OTHERHOST;
1146     skb->protocol = htons(ETH_P_802_2);
1147     memset(skb->cb, 0, sizeof(skb->cb));
1148     netif_rx(skb);
1149 }
1150
1151
1152 static void mac80211_hwsim_monitor_ack(struct ieee80211_channel *chan,
1153                        const u8 *addr)
1154 {
1155     struct sk_buff *skb;
1156     struct hwsim_radiotap_ack_hdr *hdr;
1157     u16 flags;
1158     struct ieee80211_hdr *hdr11;
1159
1160     if (!netif_running(hwsim_mon))
1161         return;
1162
1163     skb = dev_alloc_skb(100);
1164     if (skb == NULL)
1165         return;
1166
1167     hdr = skb_put(skb, sizeof(*hdr));
1168     hdr->hdr.it_version = PKTHDR_RADIOTAP_VERSION;
1169     hdr->hdr.it_pad = 0;
1170     hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
1171     hdr->hdr.it_present = cpu_to_le32((1 << IEEE80211_RADIOTAP_FLAGS) |
1172                       (1 << IEEE80211_RADIOTAP_CHANNEL));
1173     hdr->rt_flags = 0;
1174     hdr->pad = 0;
1175     hdr->rt_channel = cpu_to_le16(chan->center_freq);
1176     flags = IEEE80211_CHAN_2GHZ;
1177     hdr->rt_chbitmask = cpu_to_le16(flags);
1178
1179     hdr11 = skb_put(skb, 10);
1180     hdr11->frame_control = cpu_to_le16(IEEE80211_FTYPE_CTL |
1181                        IEEE80211_STYPE_ACK);
1182     hdr11->duration_id = cpu_to_le16(0);
1183     memcpy(hdr11->addr1, addr, ETH_ALEN);
1184
1185     skb->dev = hwsim_mon;
1186     skb_reset_mac_header(skb);
1187     skb->ip_summed = CHECKSUM_UNNECESSARY;
1188     skb->pkt_type = PACKET_OTHERHOST;
1189     skb->protocol = htons(ETH_P_802_2);
1190     memset(skb->cb, 0, sizeof(skb->cb));
1191     netif_rx(skb);
1192 }
1193
1194 struct mac80211_hwsim_addr_match_data {
1195     u8 addr[ETH_ALEN];
1196     bool ret;
1197 };
1198
1199 static void mac80211_hwsim_addr_iter(void *data, u8 *mac,
1200                      struct ieee80211_vif *vif)
1201 {
1202     int i;
1203     struct mac80211_hwsim_addr_match_data *md = data;
1204
1205     if (memcmp(mac, md->addr, ETH_ALEN) == 0) {
1206         md->ret = true;
1207         return;
1208     }
1209
1210     /* Match the link address */
1211     for (i = 0; i < ARRAY_SIZE(vif->link_conf); i++) {
1212         struct ieee80211_bss_conf *conf;
1213
1214         conf = rcu_dereference(vif->link_conf[i]);
1215         if (!conf)
1216             continue;
1217
1218         if (memcmp(conf->addr, md->addr, ETH_ALEN) == 0) {
1219             md->ret = true;
1220             return;
1221         }
1222     }
1223 }
1224
1225 static bool mac80211_hwsim_addr_match(struct mac80211_hwsim_data *data,
1226                       const u8 *addr)
1227 {
1228     struct mac80211_hwsim_addr_match_data md = {
1229         .ret = false,
1230     };
1231
1232     if (data->scanning && memcmp(addr, data->scan_addr, ETH_ALEN) == 0)
1233         return true;
1234
1235     memcpy(md.addr, addr, ETH_ALEN);
1236
1237     ieee80211_iterate_active_interfaces_atomic(data->hw,
1238                            IEEE80211_IFACE_ITER_NORMAL,
1239                            mac80211_hwsim_addr_iter,
1240                            &md);
1241
1242     return md.ret;
1243 }
1244
1245 static bool hwsim_ps_rx_ok(struct mac80211_hwsim_data *data,
1246                struct sk_buff *skb)
1247 {
1248     switch (data->ps) {
1249     case PS_DISABLED:
1250         return true;
1251     case PS_ENABLED:
1252         return false;
1253     case PS_AUTO_POLL:
1254         /* TODO: accept (some) Beacons by default and other frames only
1255          * if pending PS-Poll has been sent */
1256         return true;
1257     case PS_MANUAL_POLL:
1258         /* Allow unicast frames to own address if there is a pending
1259          * PS-Poll */
1260         if (data->ps_poll_pending &&
1261             mac80211_hwsim_addr_match(data, skb->data + 4)) {
1262             data->ps_poll_pending = false;
1263             return true;
1264         }
1265         return false;
1266     }
1267
1268     return true;
1269 }
1270
1271 static int hwsim_unicast_netgroup(struct mac80211_hwsim_data *data,
1272                   struct sk_buff *skb, int portid)
1273 {
1274     struct net *net;
1275     bool found = false;
1276     int res = -ENOENT;
1277
1278     rcu_read_lock();
1279     for_each_net_rcu(net) {
1280         if (data->netgroup == hwsim_net_get_netgroup(net)) {
1281             res = genlmsg_unicast(net, skb, portid);
1282             found = true;
1283             break;
1284         }
1285     }
1286     rcu_read_unlock();
1287
1288     if (!found)
1289         nlmsg_free(skb);
1290
1291     return res;
1292 }
1293
1294 static void mac80211_hwsim_config_mac_nl(struct ieee80211_hw *hw,
1295                      const u8 *addr, bool add)
1296 {
1297     struct mac80211_hwsim_data *data = hw->priv;
1298     u32 _portid = READ_ONCE(data->wmediumd);
1299     struct sk_buff *skb;
1300     void *msg_head;
1301
1302     if (!_portid && !hwsim_virtio_enabled)
1303         return;
1304
1305     skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_ATOMIC);
1306     if (!skb)
1307         return;
1308
1309     msg_head = genlmsg_put(skb, 0, 0, &hwsim_genl_family, 0,
1310                    add ? HWSIM_CMD_ADD_MAC_ADDR :
1311                      HWSIM_CMD_DEL_MAC_ADDR);
1312     if (!msg_head) {
1313         pr_debug("mac80211_hwsim: problem with msg_head\n");
1314         goto nla_put_failure;
1315     }
1316
1317     if (nla_put(skb, HWSIM_ATTR_ADDR_TRANSMITTER,
1318             ETH_ALEN, data->addresses[1].addr))
1319         goto nla_put_failure;
1320
1321     if (nla_put(skb, HWSIM_ATTR_ADDR_RECEIVER, ETH_ALEN, addr))
1322         goto nla_put_failure;
1323
1324     genlmsg_end(skb, msg_head);
1325
1326     if (hwsim_virtio_enabled)
1327         hwsim_tx_virtio(data, skb);
1328     else
1329         hwsim_unicast_netgroup(data, skb, _portid);
1330     return;
1331 nla_put_failure:
1332     nlmsg_free(skb);
1333 }
1334
1335 static inline u16 trans_tx_rate_flags_ieee2hwsim(struct ieee80211_tx_rate *rate)
1336 {
1337     u16 result = 0;
1338
1339     if (rate->flags & IEEE80211_TX_RC_USE_RTS_CTS)
1340         result |= MAC80211_HWSIM_TX_RC_USE_RTS_CTS;
1341     if (rate->flags & IEEE80211_TX_RC_USE_CTS_PROTECT)
1342         result |= MAC80211_HWSIM_TX_RC_USE_CTS_PROTECT;
1343     if (rate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
1344         result |= MAC80211_HWSIM_TX_RC_USE_SHORT_PREAMBLE;
1345     if (rate->flags & IEEE80211_TX_RC_MCS)
1346         result |= MAC80211_HWSIM_TX_RC_MCS;
1347     if (rate->flags & IEEE80211_TX_RC_GREEN_FIELD)
1348         result |= MAC80211_HWSIM_TX_RC_GREEN_FIELD;
1349     if (rate->flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
1350         result |= MAC80211_HWSIM_TX_RC_40_MHZ_WIDTH;
1351     if (rate->flags & IEEE80211_TX_RC_DUP_DATA)
1352         result |= MAC80211_HWSIM_TX_RC_DUP_DATA;
1353     if (rate->flags & IEEE80211_TX_RC_SHORT_GI)
1354         result |= MAC80211_HWSIM_TX_RC_SHORT_GI;
1355     if (rate->flags & IEEE80211_TX_RC_VHT_MCS)
1356         result |= MAC80211_HWSIM_TX_RC_VHT_MCS;
1357     if (rate->flags & IEEE80211_TX_RC_80_MHZ_WIDTH)
1358         result |= MAC80211_HWSIM_TX_RC_80_MHZ_WIDTH;
1359     if (rate->flags & IEEE80211_TX_RC_160_MHZ_WIDTH)
1360         result |= MAC80211_HWSIM_TX_RC_160_MHZ_WIDTH;
1361
1362     return result;
1363 }
1364
1365 static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
1366                        struct sk_buff *my_skb,
1367                        int dst_portid,
1368                        struct ieee80211_channel *channel)
1369 {
1370     struct sk_buff *skb;
1371     struct mac80211_hwsim_data *data = hw->priv;
1372     struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) my_skb->data;
1373     struct ieee80211_tx_info *info = IEEE80211_SKB_CB(my_skb);
1374     void *msg_head;
1375     unsigned int hwsim_flags = 0;
1376     int i;
1377     struct hwsim_tx_rate tx_attempts[IEEE80211_TX_MAX_RATES];
1378     struct hwsim_tx_rate_flag tx_attempts_flags[IEEE80211_TX_MAX_RATES];
1379     uintptr_t cookie;
1380
1381     if (data->ps != PS_DISABLED)
1382         hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
1383     /* If the queue contains MAX_QUEUE skb's drop some */
1384     if (skb_queue_len(&data->pending) >= MAX_QUEUE) {
1385         /* Dropping until WARN_QUEUE level */
1386         while (skb_queue_len(&data->pending) >= WARN_QUEUE) {
1387             ieee80211_free_txskb(hw, skb_dequeue(&data->pending));
1388             data->tx_dropped++;
1389         }
1390     }
1391
1392     skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_ATOMIC);
1393     if (skb == NULL)
1394         goto nla_put_failure;
1395
1396     msg_head = genlmsg_put(skb, 0, 0, &hwsim_genl_family, 0,
1397                    HWSIM_CMD_FRAME);
1398     if (msg_head == NULL) {
1399         pr_debug("mac80211_hwsim: problem with msg_head\n");
1400         goto nla_put_failure;
1401     }
1402
1403     if (nla_put(skb, HWSIM_ATTR_ADDR_TRANSMITTER,
1404             ETH_ALEN, data->addresses[1].addr))
1405         goto nla_put_failure;
1406
1407     /* We get the skb->data */
1408     if (nla_put(skb, HWSIM_ATTR_FRAME, my_skb->len, my_skb->data))
1409         goto nla_put_failure;
1410
1411     /* We get the flags for this transmission, and we translate them to
1412        wmediumd flags  */
1413
1414     if (info->flags & IEEE80211_TX_CTL_REQ_TX_STATUS)
1415         hwsim_flags |= HWSIM_TX_CTL_REQ_TX_STATUS;
1416
1417     if (info->flags & IEEE80211_TX_CTL_NO_ACK)
1418         hwsim_flags |= HWSIM_TX_CTL_NO_ACK;
1419
1420     if (nla_put_u32(skb, HWSIM_ATTR_FLAGS, hwsim_flags))
1421         goto nla_put_failure;
1422
1423     if (nla_put_u32(skb, HWSIM_ATTR_FREQ, channel->center_freq))
1424         goto nla_put_failure;
1425
1426     /* We get the tx control (rate and retries) info*/
1427
1428     for (i = 0; i < IEEE80211_TX_MAX_RATES; i++) {
1429         tx_attempts[i].idx = info->status.rates[i].idx;
1430         tx_attempts_flags[i].idx = info->status.rates[i].idx;
1431         tx_attempts[i].count = info->status.rates[i].count;
1432         tx_attempts_flags[i].flags =
1433                 trans_tx_rate_flags_ieee2hwsim(
1434                         &info->status.rates[i]);
1435     }
1436
1437     if (nla_put(skb, HWSIM_ATTR_TX_INFO,
1438             sizeof(struct hwsim_tx_rate)*IEEE80211_TX_MAX_RATES,
1439             tx_attempts))
1440         goto nla_put_failure;
1441
1442     if (nla_put(skb, HWSIM_ATTR_TX_INFO_FLAGS,
1443             sizeof(struct hwsim_tx_rate_flag) * IEEE80211_TX_MAX_RATES,
1444             tx_attempts_flags))
1445         goto nla_put_failure;
1446
1447     /* We create a cookie to identify this skb */
1448     cookie = atomic_inc_return(&data->pending_cookie);
1449     info->rate_driver_data[0] = (void *)cookie;
1450     if (nla_put_u64_64bit(skb, HWSIM_ATTR_COOKIE, cookie, HWSIM_ATTR_PAD))
1451         goto nla_put_failure;
1452
1453     genlmsg_end(skb, msg_head);
1454
1455     if (hwsim_virtio_enabled) {
1456         if (hwsim_tx_virtio(data, skb))
1457             goto err_free_txskb;
1458     } else {
1459         if (hwsim_unicast_netgroup(data, skb, dst_portid))
1460             goto err_free_txskb;
1461     }
1462
1463     /* Enqueue the packet */
1464     skb_queue_tail(&data->pending, my_skb);
1465     data->tx_pkts++;
1466     data->tx_bytes += my_skb->len;
1467     return;
1468
1469 nla_put_failure:
1470     nlmsg_free(skb);
1471 err_free_txskb:
1472     pr_debug("mac80211_hwsim: error occurred in %s\n", __func__);
1473     ieee80211_free_txskb(hw, my_skb);
1474     data->tx_failed++;
1475 }
1476
1477 static bool hwsim_chans_compat(struct ieee80211_channel *c1,
1478                    struct ieee80211_channel *c2)
1479 {
1480     if (!c1 || !c2)
1481         return false;
1482
1483     return c1->center_freq == c2->center_freq;
1484 }
1485
1486 struct tx_iter_data {
1487     struct ieee80211_channel *channel;
1488     bool receive;
1489 };
1490
1491 static void mac80211_hwsim_tx_iter(void *_data, u8 *addr,
1492                    struct ieee80211_vif *vif)
1493 {
1494     struct tx_iter_data *data = _data;
1495     int i;
1496
1497     for (i = 0; i < ARRAY_SIZE(vif->link_conf); i++) {
1498         struct ieee80211_bss_conf *conf;
1499         struct ieee80211_chanctx_conf *chanctx;
1500
1501         conf = rcu_dereference(vif->link_conf[i]);
1502         if (!conf)
1503             continue;
1504
1505         chanctx = rcu_dereference(conf->chanctx_conf);
1506         if (!chanctx)
1507             continue;
1508
1509         if (!hwsim_chans_compat(data->channel, chanctx->def.chan))
1510             continue;
1511
1512         data->receive = true;
1513         return;
1514     }
1515 }
1516
1517 static void mac80211_hwsim_add_vendor_rtap(struct sk_buff *skb)
1518 {
1519     /*
1520      * To enable this code, #define the HWSIM_RADIOTAP_OUI,
1521      * e.g. like this:
1522      * #define HWSIM_RADIOTAP_OUI "\x02\x00\x00"
1523      * (but you should use a valid OUI, not that)
1524      *
1525      * If anyone wants to 'donate' a radiotap OUI/subns code
1526      * please send a patch removing this #ifdef and changing
1527      * the values accordingly.
1528      */
1529 #ifdef HWSIM_RADIOTAP_OUI
1530     struct ieee80211_vendor_radiotap *rtap;
1531
1532     /*
1533      * Note that this code requires the headroom in the SKB
1534      * that was allocated earlier.
1535      */
1536     rtap = skb_push(skb, sizeof(*rtap) + 8 + 4);
1537     rtap->oui[0] = HWSIM_RADIOTAP_OUI[0];
1538     rtap->oui[1] = HWSIM_RADIOTAP_OUI[1];
1539     rtap->oui[2] = HWSIM_RADIOTAP_OUI[2];
1540     rtap->subns = 127;
1541
1542     /*
1543      * Radiotap vendor namespaces can (and should) also be
1544      * split into fields by using the standard radiotap
1545      * presence bitmap mechanism. Use just BIT(0) here for
1546      * the presence bitmap.
1547      */
1548     rtap->present = BIT(0);
1549     /* We have 8 bytes of (dummy) data */
1550     rtap->len = 8;
1551     /* For testing, also require it to be aligned */
1552     rtap->align = 8;
1553     /* And also test that padding works, 4 bytes */
1554     rtap->pad = 4;
1555     /* push the data */
1556     memcpy(rtap->data, "ABCDEFGH", 8);
1557     /* make sure to clear padding, mac80211 doesn't */
1558     memset(rtap->data + 8, 0, 4);
1559
1560     IEEE80211_SKB_RXCB(skb)->flag |= RX_FLAG_RADIOTAP_VENDOR_DATA;
1561 #endif
1562 }
1563
1564 static bool mac80211_hwsim_tx_frame_no_nl(struct ieee80211_hw *hw,
1565                       struct sk_buff *skb,
1566                       struct ieee80211_channel *chan)
1567 {
1568     struct mac80211_hwsim_data *data = hw->priv, *data2;
1569     bool ack = false;
1570     struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
1571     struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1572     struct ieee80211_rx_status rx_status;
1573     u64 now;
1574
1575     memset(&rx_status, 0, sizeof(rx_status));
1576     rx_status.flag |= RX_FLAG_MACTIME_START;
1577     rx_status.freq = chan->center_freq;
1578     rx_status.freq_offset = chan->freq_offset ? 1 : 0;
1579     rx_status.band = chan->band;
1580     if (info->control.rates[0].flags & IEEE80211_TX_RC_VHT_MCS) {
1581         rx_status.rate_idx =
1582             ieee80211_rate_get_vht_mcs(&info->control.rates[0]);
1583         rx_status.nss =
1584             ieee80211_rate_get_vht_nss(&info->control.rates[0]);
1585         rx_status.encoding = RX_ENC_VHT;
1586     } else {
1587         rx_status.rate_idx = info->control.rates[0].idx;
1588         if (info->control.rates[0].flags & IEEE80211_TX_RC_MCS)
1589             rx_status.encoding = RX_ENC_HT;
1590     }
1591     if (info->control.rates[0].flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
1592         rx_status.bw = RATE_INFO_BW_40;
1593     else if (info->control.rates[0].flags & IEEE80211_TX_RC_80_MHZ_WIDTH)
1594         rx_status.bw = RATE_INFO_BW_80;
1595     else if (info->control.rates[0].flags & IEEE80211_TX_RC_160_MHZ_WIDTH)
1596         rx_status.bw = RATE_INFO_BW_160;
1597     else
1598         rx_status.bw = RATE_INFO_BW_20;
1599     if (info->control.rates[0].flags & IEEE80211_TX_RC_SHORT_GI)
1600         rx_status.enc_flags |= RX_ENC_FLAG_SHORT_GI;
1601     /* TODO: simulate optional packet loss */
1602     rx_status.signal = data->rx_rssi;
1603     if (info->control.vif)
1604         rx_status.signal += info->control.vif->bss_conf.txpower;
1605
1606     if (data->ps != PS_DISABLED)
1607         hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
1608
1609     /* release the skb's source info */
1610     skb_orphan(skb);
1611     skb_dst_drop(skb);
1612     skb->mark = 0;
1613     skb_ext_reset(skb);
1614     nf_reset_ct(skb);
1615
1616     /*
1617      * Get absolute mactime here so all HWs RX at the "same time", and
1618      * absolute TX time for beacon mactime so the timestamp matches.
1619      * Giving beacons a different mactime than non-beacons looks messy, but
1620      * it helps the Toffset be exact and a ~10us mactime discrepancy
1621      * probably doesn't really matter.
1622      */
1623     if (ieee80211_is_beacon(hdr->frame_control) ||
1624         ieee80211_is_probe_resp(hdr->frame_control)) {
1625         rx_status.boottime_ns = ktime_get_boottime_ns();
1626         now = data->abs_bcn_ts;
1627     } else {
1628         now = mac80211_hwsim_get_tsf_raw();
1629     }
1630
1631     /* Copy skb to all enabled radios that are on the current frequency */
1632     spin_lock(&hwsim_radio_lock);
1633     list_for_each_entry(data2, &hwsim_radios, list) {
1634         struct sk_buff *nskb;
1635         struct tx_iter_data tx_iter_data = {
1636             .receive = false,
1637             .channel = chan,
1638         };
1639
1640         if (data == data2)
1641             continue;
1642
1643         if (!data2->started || (data2->idle && !data2->tmp_chan) ||
1644             !hwsim_ps_rx_ok(data2, skb))
1645             continue;
1646
1647         if (!(data->group & data2->group))
1648             continue;
1649
1650         if (data->netgroup != data2->netgroup)
1651             continue;
1652
1653         if (!hwsim_chans_compat(chan, data2->tmp_chan) &&
1654             !hwsim_chans_compat(chan, data2->channel)) {
1655             ieee80211_iterate_active_interfaces_atomic(
1656                 data2->hw, IEEE80211_IFACE_ITER_NORMAL,
1657                 mac80211_hwsim_tx_iter, &tx_iter_data);
1658             if (!tx_iter_data.receive)
1659                 continue;
1660         }
1661
1662         /*
1663          * reserve some space for our vendor and the normal
1664          * radiotap header, since we're copying anyway
1665          */
1666         if (skb->len < PAGE_SIZE && paged_rx) {
1667             struct page *page = alloc_page(GFP_ATOMIC);
1668
1669             if (!page)
1670                 continue;
1671
1672             nskb = dev_alloc_skb(128);
1673             if (!nskb) {
1674                 __free_page(page);
1675                 continue;
1676             }
1677
1678             memcpy(page_address(page), skb->data, skb->len);
1679             skb_add_rx_frag(nskb, 0, page, 0, skb->len, skb->len);
1680         } else {
1681             nskb = skb_copy(skb, GFP_ATOMIC);
1682             if (!nskb)
1683                 continue;
1684         }
1685
1686         if (mac80211_hwsim_addr_match(data2, hdr->addr1))
1687             ack = true;
1688
1689         rx_status.mactime = now + data2->tsf_offset;
1690
1691         memcpy(IEEE80211_SKB_RXCB(nskb), &rx_status, sizeof(rx_status));
1692
1693         mac80211_hwsim_add_vendor_rtap(nskb);
1694
1695         data2->rx_pkts++;
1696         data2->rx_bytes += nskb->len;
1697         ieee80211_rx_irqsafe(data2->hw, nskb);
1698     }
1699     spin_unlock(&hwsim_radio_lock);
1700
1701     return ack;
1702 }
1703
1704 static struct ieee80211_bss_conf *
1705 mac80211_hwsim_select_tx_link(struct mac80211_hwsim_data *data,
1706                   struct ieee80211_vif *vif,
1707                   struct ieee80211_sta *sta,
1708                   struct ieee80211_hdr *hdr,
1709                   struct ieee80211_link_sta **link_sta)
1710 {
1711     struct hwsim_sta_priv *sp = (void *)sta->drv_priv;
1712     int i;
1713
1714     if (!vif->valid_links)
1715         return &vif->bss_conf;
1716
1717     /* FIXME: handle multicast TX properly */
1718     if (is_multicast_ether_addr(hdr->addr1) || WARN_ON_ONCE(!sta)) {
1719         unsigned int first_link = ffs(vif->valid_links) - 1;
1720
1721         return rcu_dereference(vif->link_conf[first_link]);
1722     }
1723
1724     if (WARN_ON_ONCE(!sta->valid_links))
1725         return &vif->bss_conf;
1726
1727     for (i = 0; i < ARRAY_SIZE(vif->link_conf); i++) {
1728         struct ieee80211_bss_conf *bss_conf;
1729         unsigned int link_id;
1730
1731         /* round-robin the available link IDs */
1732         link_id = (sp->last_link + i + 1) % ARRAY_SIZE(vif->link_conf);
1733
1734         *link_sta = rcu_dereference(sta->link[link_id]);
1735         if (!*link_sta)
1736             continue;
1737
1738         bss_conf = rcu_dereference(vif->link_conf[link_id]);
1739         if (WARN_ON_ONCE(!bss_conf))
1740             continue;
1741
1742         sp->last_link = link_id;
1743         return bss_conf;
1744     }
1745
1746     return NULL;
1747 }
1748
1749 static void mac80211_hwsim_tx(struct ieee80211_hw *hw,
1750                   struct ieee80211_tx_control *control,
1751                   struct sk_buff *skb)
1752 {
1753     struct mac80211_hwsim_data *data = hw->priv;
1754     struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(skb);
1755     struct ieee80211_hdr *hdr = (void *)skb->data;
1756     struct ieee80211_chanctx_conf *chanctx_conf;
1757     struct ieee80211_channel *channel;
1758     bool ack;
1759     enum nl80211_chan_width confbw = NL80211_CHAN_WIDTH_20_NOHT;
1760     u32 _portid, i;
1761
1762     if (WARN_ON(skb->len < 10)) {
1763         /* Should not happen; just a sanity check for addr1 use */
1764         ieee80211_free_txskb(hw, skb);
1765         return;
1766     }
1767
1768     if (!data->use_chanctx) {
1769         channel = data->channel;
1770         confbw = data->bw;
1771     } else if (txi->hw_queue == 4) {
1772         channel = data->tmp_chan;
1773     } else {
1774         u8 link = u32_get_bits(IEEE80211_SKB_CB(skb)->control.flags,
1775                        IEEE80211_TX_CTRL_MLO_LINK);
1776         struct ieee80211_vif *vif = txi->control.vif;
1777         struct ieee80211_link_sta *link_sta = NULL;
1778         struct ieee80211_sta *sta = control->sta;
1779         struct ieee80211_bss_conf *bss_conf;
1780
1781         if (link != IEEE80211_LINK_UNSPECIFIED) {
1782             bss_conf = rcu_dereference(txi->control.vif->link_conf[link]);
1783             if (sta)
1784                 link_sta = rcu_dereference(sta->link[link]);
1785         } else {
1786             bss_conf = mac80211_hwsim_select_tx_link(data, vif, sta,
1787                                  hdr, &link_sta);
1788         }
1789
1790         if (WARN_ON(!bss_conf)) {
1791             ieee80211_free_txskb(hw, skb);
1792             return;
1793         }
1794
1795         if (sta && sta->mlo) {
1796             if (WARN_ON(!link_sta)) {
1797                 ieee80211_free_txskb(hw, skb);
1798                 return;
1799             }
1800             /* address translation to link addresses on TX */
1801             ether_addr_copy(hdr->addr1, link_sta->addr);
1802             ether_addr_copy(hdr->addr2, bss_conf->addr);
1803             /* translate A3 only if it's the BSSID */
1804             if (!ieee80211_has_tods(hdr->frame_control) &&
1805                 !ieee80211_has_fromds(hdr->frame_control)) {
1806                 if (ether_addr_equal(hdr->addr3, sta->addr))
1807                     ether_addr_copy(hdr->addr3, link_sta->addr);
1808                 else if (ether_addr_equal(hdr->addr3, vif->addr))
1809                     ether_addr_copy(hdr->addr3, bss_conf->addr);
1810             }
1811             /* no need to look at A4, if present it's SA */
1812         }
1813
1814         chanctx_conf = rcu_dereference(bss_conf->chanctx_conf);
1815         if (chanctx_conf) {
1816             channel = chanctx_conf->def.chan;
1817             confbw = chanctx_conf->def.width;
1818         } else {
1819             channel = NULL;
1820         }
1821     }
1822
1823     if (WARN(!channel, "TX w/o channel - queue = %d\n", txi->hw_queue)) {
1824         ieee80211_free_txskb(hw, skb);
1825         return;
1826     }
1827
1828     if (data->idle && !data->tmp_chan) {
1829         wiphy_dbg(hw->wiphy, "Trying to TX when idle - reject\n");
1830         ieee80211_free_txskb(hw, skb);
1831         return;
1832     }
1833
1834     if (txi->control.vif)
1835         hwsim_check_magic(txi->control.vif);
1836     if (control->sta)
1837         hwsim_check_sta_magic(control->sta);
1838
1839     if (ieee80211_hw_check(hw, SUPPORTS_RC_TABLE))
1840         ieee80211_get_tx_rates(txi->control.vif, control->sta, skb,
1841                        txi->control.rates,
1842                        ARRAY_SIZE(txi->control.rates));
1843
1844     for (i = 0; i < ARRAY_SIZE(txi->control.rates); i++) {
1845         u16 rflags = txi->control.rates[i].flags;
1846         /* initialize to data->bw for 5/10 MHz handling */
1847         enum nl80211_chan_width bw = data->bw;
1848
1849         if (txi->control.rates[i].idx == -1)
1850             break;
1851
1852         if (rflags & IEEE80211_TX_RC_40_MHZ_WIDTH)
1853             bw = NL80211_CHAN_WIDTH_40;
1854         else if (rflags & IEEE80211_TX_RC_80_MHZ_WIDTH)
1855             bw = NL80211_CHAN_WIDTH_80;
1856         else if (rflags & IEEE80211_TX_RC_160_MHZ_WIDTH)
1857             bw = NL80211_CHAN_WIDTH_160;
1858
1859         if (WARN_ON(hwsim_get_chanwidth(bw) > hwsim_get_chanwidth(confbw)))
1860             return;
1861     }
1862
1863     if (skb->len >= 24 + 8 &&
1864         ieee80211_is_probe_resp(hdr->frame_control)) {
1865         /* fake header transmission time */
1866         struct ieee80211_mgmt *mgmt;
1867         struct ieee80211_rate *txrate;
1868         /* TODO: get MCS */
1869         int bitrate = 100;
1870         u64 ts;
1871
1872         mgmt = (struct ieee80211_mgmt *)skb->data;
1873         txrate = ieee80211_get_tx_rate(hw, txi);
1874         if (txrate)
1875             bitrate = txrate->bitrate;
1876         ts = mac80211_hwsim_get_tsf_raw();
1877         mgmt->u.probe_resp.timestamp =
1878             cpu_to_le64(ts + data->tsf_offset +
1879                     24 * 8 * 10 / bitrate);
1880     }
1881
1882     mac80211_hwsim_monitor_rx(hw, skb, channel);
1883
1884     /* wmediumd mode check */
1885     _portid = READ_ONCE(data->wmediumd);
1886
1887     if (_portid || hwsim_virtio_enabled)
1888         return mac80211_hwsim_tx_frame_nl(hw, skb, _portid, channel);
1889
1890     /* NO wmediumd detected, perfect medium simulation */
1891     data->tx_pkts++;
1892     data->tx_bytes += skb->len;
1893     ack = mac80211_hwsim_tx_frame_no_nl(hw, skb, channel);
1894
1895     if (ack && skb->len >= 16)
1896         mac80211_hwsim_monitor_ack(channel, hdr->addr2);
1897
1898     ieee80211_tx_info_clear_status(txi);
1899
1900     /* frame was transmitted at most favorable rate at first attempt */
1901     txi->control.rates[0].count = 1;
1902     txi->control.rates[1].idx = -1;
1903
1904     if (!(txi->flags & IEEE80211_TX_CTL_NO_ACK) && ack)
1905         txi->flags |= IEEE80211_TX_STAT_ACK;
1906     ieee80211_tx_status_irqsafe(hw, skb);
1907 }
1908
1909
1910 static int mac80211_hwsim_start(struct ieee80211_hw *hw)
1911 {
1912     struct mac80211_hwsim_data *data = hw->priv;
1913     wiphy_dbg(hw->wiphy, "%s\n", __func__);
1914     data->started = true;
1915     return 0;
1916 }
1917
1918
1919 static void mac80211_hwsim_stop(struct ieee80211_hw *hw)
1920 {
1921     struct mac80211_hwsim_data *data = hw->priv;
1922     int i;
1923
1924     data->started = false;
1925
1926     for (i = 0; i < ARRAY_SIZE(data->link_data); i++)
1927         hrtimer_cancel(&data->link_data[i].beacon_timer);
1928
1929     while (!skb_queue_empty(&data->pending))
1930         ieee80211_free_txskb(hw, skb_dequeue(&data->pending));
1931
1932     wiphy_dbg(hw->wiphy, "%s\n", __func__);
1933 }
1934
1935
1936 static int mac80211_hwsim_add_interface(struct ieee80211_hw *hw,
1937                     struct ieee80211_vif *vif)
1938 {
1939     wiphy_dbg(hw->wiphy, "%s (type=%d mac_addr=%pM)\n",
1940           __func__, ieee80211_vif_type_p2p(vif),
1941           vif->addr);
1942     hwsim_set_magic(vif);
1943
1944     if (vif->type != NL80211_IFTYPE_MONITOR)
1945         mac80211_hwsim_config_mac_nl(hw, vif->addr, true);
1946
1947     vif->cab_queue = 0;
1948     vif->hw_queue[IEEE80211_AC_VO] = 0;
1949     vif->hw_queue[IEEE80211_AC_VI] = 1;
1950     vif->hw_queue[IEEE80211_AC_BE] = 2;
1951     vif->hw_queue[IEEE80211_AC_BK] = 3;
1952
1953     return 0;
1954 }
1955
1956
1957 static int mac80211_hwsim_change_interface(struct ieee80211_hw *hw,
1958                        struct ieee80211_vif *vif,
1959                        enum nl80211_iftype newtype,
1960                        bool newp2p)
1961 {
1962     newtype = ieee80211_iftype_p2p(newtype, newp2p);
1963     wiphy_dbg(hw->wiphy,
1964           "%s (old type=%d, new type=%d, mac_addr=%pM)\n",
1965           __func__, ieee80211_vif_type_p2p(vif),
1966             newtype, vif->addr);
1967     hwsim_check_magic(vif);
1968
1969     /*
1970      * interface may change from non-AP to AP in
1971      * which case this needs to be set up again
1972      */
1973     vif->cab_queue = 0;
1974
1975     return 0;
1976 }
1977
1978 static void mac80211_hwsim_remove_interface(
1979     struct ieee80211_hw *hw, struct ieee80211_vif *vif)
1980 {
1981     wiphy_dbg(hw->wiphy, "%s (type=%d mac_addr=%pM)\n",
1982           __func__, ieee80211_vif_type_p2p(vif),
1983           vif->addr);
1984     hwsim_check_magic(vif);
1985     hwsim_clear_magic(vif);
1986     if (vif->type != NL80211_IFTYPE_MONITOR)
1987         mac80211_hwsim_config_mac_nl(hw, vif->addr, false);
1988 }
1989
1990 static void mac80211_hwsim_tx_frame(struct ieee80211_hw *hw,
1991                     struct sk_buff *skb,
1992                     struct ieee80211_channel *chan)
1993 {
1994     struct mac80211_hwsim_data *data = hw->priv;
1995     u32 _pid = READ_ONCE(data->wmediumd);
1996
1997     if (ieee80211_hw_check(hw, SUPPORTS_RC_TABLE)) {
1998         struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(skb);
1999         ieee80211_get_tx_rates(txi->control.vif, NULL, skb,
2000                        txi->control.rates,
2001                        ARRAY_SIZE(txi->control.rates));
2002     }
2003
2004     mac80211_hwsim_monitor_rx(hw, skb, chan);
2005
2006     if (_pid || hwsim_virtio_enabled)
2007         return mac80211_hwsim_tx_frame_nl(hw, skb, _pid, chan);
2008
2009     data->tx_pkts++;
2010     data->tx_bytes += skb->len;
2011     mac80211_hwsim_tx_frame_no_nl(hw, skb, chan);
2012     dev_kfree_skb(skb);
2013 }
2014
2015 static void mac80211_hwsim_beacon_tx(void *arg, u8 *mac,
2016                      struct ieee80211_vif *vif)
2017 {
2018     struct mac80211_hwsim_link_data *link_data = arg;
2019     u32 link_id = link_data->link_id;
2020     struct ieee80211_bss_conf *link_conf;
2021     struct mac80211_hwsim_data *data =
2022         container_of(link_data, struct mac80211_hwsim_data,
2023                  link_data[link_id]);
2024     struct ieee80211_hw *hw = data->hw;
2025     struct ieee80211_tx_info *info;
2026     struct ieee80211_rate *txrate;
2027     struct ieee80211_mgmt *mgmt;
2028     struct sk_buff *skb;
2029     /* TODO: get MCS */
2030     int bitrate = 100;
2031
2032     hwsim_check_magic(vif);
2033
2034     link_conf = rcu_dereference(vif->link_conf[link_id]);
2035     if (!link_conf)
2036         return;
2037
2038     if (vif->type != NL80211_IFTYPE_AP &&
2039         vif->type != NL80211_IFTYPE_MESH_POINT &&
2040         vif->type != NL80211_IFTYPE_ADHOC &&
2041         vif->type != NL80211_IFTYPE_OCB)
2042         return;
2043
2044     skb = ieee80211_beacon_get(hw, vif, link_data->link_id);
2045     if (skb == NULL)
2046         return;
2047     info = IEEE80211_SKB_CB(skb);
2048     if (ieee80211_hw_check(hw, SUPPORTS_RC_TABLE))
2049         ieee80211_get_tx_rates(vif, NULL, skb,
2050                        info->control.rates,
2051                        ARRAY_SIZE(info->control.rates));
2052
2053     txrate = ieee80211_get_tx_rate(hw, info);
2054     if (txrate)
2055         bitrate = txrate->bitrate;
2056
2057     mgmt = (struct ieee80211_mgmt *) skb->data;
2058     /* fake header transmission time */
2059     data->abs_bcn_ts = mac80211_hwsim_get_tsf_raw();
2060     if (ieee80211_is_s1g_beacon(mgmt->frame_control)) {
2061         struct ieee80211_ext *ext = (void *) mgmt;
2062
2063         ext->u.s1g_beacon.timestamp = cpu_to_le32(data->abs_bcn_ts +
2064                               data->tsf_offset +
2065                               10 * 8 * 10 /
2066                               bitrate);
2067     } else {
2068         mgmt->u.beacon.timestamp = cpu_to_le64(data->abs_bcn_ts +
2069                                data->tsf_offset +
2070                                24 * 8 * 10 /
2071                                bitrate);
2072     }
2073
2074     mac80211_hwsim_tx_frame(hw, skb,
2075             rcu_dereference(link_conf->chanctx_conf)->def.chan);
2076
2077     while ((skb = ieee80211_get_buffered_bc(hw, vif)) != NULL) {
2078         mac80211_hwsim_tx_frame(hw, skb,
2079             rcu_dereference(link_conf->chanctx_conf)->def.chan);
2080     }
2081
2082     if (link_conf->csa_active && ieee80211_beacon_cntdwn_is_complete(vif))
2083         ieee80211_csa_finish(vif);
2084 }
2085
2086 static enum hrtimer_restart
2087 mac80211_hwsim_beacon(struct hrtimer *timer)
2088 {
2089     struct mac80211_hwsim_link_data *link_data =
2090         container_of(timer, struct mac80211_hwsim_link_data, beacon_timer);
2091     struct mac80211_hwsim_data *data =
2092         container_of(link_data, struct mac80211_hwsim_data,
2093                  link_data[link_data->link_id]);
2094     struct ieee80211_hw *hw = data->hw;
2095     u64 bcn_int = link_data->beacon_int;
2096
2097     if (!data->started)
2098         return HRTIMER_NORESTART;
2099
2100     ieee80211_iterate_active_interfaces_atomic(
2101         hw, IEEE80211_IFACE_ITER_NORMAL,
2102         mac80211_hwsim_beacon_tx, link_data);
2103
2104     /* beacon at new TBTT + beacon interval */
2105     if (data->bcn_delta) {
2106         bcn_int -= data->bcn_delta;
2107         data->bcn_delta = 0;
2108     }
2109     hrtimer_forward_now(&link_data->beacon_timer,
2110                 ns_to_ktime(bcn_int * NSEC_PER_USEC));
2111     return HRTIMER_RESTART;
2112 }
2113
2114 static const char * const hwsim_chanwidths[] = {
2115     [NL80211_CHAN_WIDTH_5] = "ht5",
2116     [NL80211_CHAN_WIDTH_10] = "ht10",
2117     [NL80211_CHAN_WIDTH_20_NOHT] = "noht",
2118     [NL80211_CHAN_WIDTH_20] = "ht20",
2119     [NL80211_CHAN_WIDTH_40] = "ht40",
2120     [NL80211_CHAN_WIDTH_80] = "vht80",
2121     [NL80211_CHAN_WIDTH_80P80] = "vht80p80",
2122     [NL80211_CHAN_WIDTH_160] = "vht160",
2123     [NL80211_CHAN_WIDTH_1] = "1MHz",
2124     [NL80211_CHAN_WIDTH_2] = "2MHz",
2125     [NL80211_CHAN_WIDTH_4] = "4MHz",
2126     [NL80211_CHAN_WIDTH_8] = "8MHz",
2127     [NL80211_CHAN_WIDTH_16] = "16MHz",
2128 };
2129
2130 static int mac80211_hwsim_config(struct ieee80211_hw *hw, u32 changed)
2131 {
2132     struct mac80211_hwsim_data *data = hw->priv;
2133     struct ieee80211_conf *conf = &hw->conf;
2134     static const char *smps_modes[IEEE80211_SMPS_NUM_MODES] = {
2135         [IEEE80211_SMPS_AUTOMATIC] = "auto",
2136         [IEEE80211_SMPS_OFF] = "off",
2137         [IEEE80211_SMPS_STATIC] = "static",
2138         [IEEE80211_SMPS_DYNAMIC] = "dynamic",
2139     };
2140     int idx;
2141
2142     if (conf->chandef.chan)
2143         wiphy_dbg(hw->wiphy,
2144               "%s (freq=%d(%d - %d)/%s idle=%d ps=%d smps=%s)\n",
2145               __func__,
2146               conf->chandef.chan->center_freq,
2147               conf->chandef.center_freq1,
2148               conf->chandef.center_freq2,
2149               hwsim_chanwidths[conf->chandef.width],
2150               !!(conf->flags & IEEE80211_CONF_IDLE),
2151               !!(conf->flags & IEEE80211_CONF_PS),
2152               smps_modes[conf->smps_mode]);
2153     else
2154         wiphy_dbg(hw->wiphy,
2155               "%s (freq=0 idle=%d ps=%d smps=%s)\n",
2156               __func__,
2157               !!(conf->flags & IEEE80211_CONF_IDLE),
2158               !!(conf->flags & IEEE80211_CONF_PS),
2159               smps_modes[conf->smps_mode]);
2160
2161     data->idle = !!(conf->flags & IEEE80211_CONF_IDLE);
2162
2163     WARN_ON(conf->chandef.chan && data->use_chanctx);
2164
2165     mutex_lock(&data->mutex);
2166     if (data->scanning && conf->chandef.chan) {
2167         for (idx = 0; idx < ARRAY_SIZE(data->survey_data); idx++) {
2168             if (data->survey_data[idx].channel == data->channel) {
2169                 data->survey_data[idx].start =
2170                     data->survey_data[idx].next_start;
2171                 data->survey_data[idx].end = jiffies;
2172                 break;
2173             }
2174         }
2175
2176         data->channel = conf->chandef.chan;
2177         data->bw = conf->chandef.width;
2178
2179         for (idx = 0; idx < ARRAY_SIZE(data->survey_data); idx++) {
2180             if (data->survey_data[idx].channel &&
2181                 data->survey_data[idx].channel != data->channel)
2182                 continue;
2183             data->survey_data[idx].channel = data->channel;
2184             data->survey_data[idx].next_start = jiffies;
2185             break;
2186         }
2187     } else {
2188         data->channel = conf->chandef.chan;
2189         data->bw = conf->chandef.width;
2190     }
2191     mutex_unlock(&data->mutex);
2192
2193     for (idx = 0; idx < ARRAY_SIZE(data->link_data); idx++) {
2194         struct mac80211_hwsim_link_data *link_data =
2195             &data->link_data[idx];
2196
2197         if (!data->started || !link_data->beacon_int) {
2198             hrtimer_cancel(&link_data->beacon_timer);
2199         } else if (!hrtimer_is_queued(&link_data->beacon_timer)) {
2200             u64 tsf = mac80211_hwsim_get_tsf(hw, NULL);
2201             u32 bcn_int = link_data->beacon_int;
2202             u64 until_tbtt = bcn_int - do_div(tsf, bcn_int);
2203
2204             hrtimer_start(&link_data->beacon_timer,
2205                       ns_to_ktime(until_tbtt * NSEC_PER_USEC),
2206                       HRTIMER_MODE_REL_SOFT);
2207         }
2208     }
2209
2210     return 0;
2211 }
2212
2213
2214 static void mac80211_hwsim_configure_filter(struct ieee80211_hw *hw,
2215                         unsigned int changed_flags,
2216                         unsigned int *total_flags,u64 multicast)
2217 {
2218     struct mac80211_hwsim_data *data = hw->priv;
2219
2220     wiphy_dbg(hw->wiphy, "%s\n", __func__);
2221
2222     data->rx_filter = 0;
2223     if (*total_flags & FIF_ALLMULTI)
2224         data->rx_filter |= FIF_ALLMULTI;
2225     if (*total_flags & FIF_MCAST_ACTION)
2226         data->rx_filter |= FIF_MCAST_ACTION;
2227
2228     *total_flags = data->rx_filter;
2229 }
2230
2231 static void mac80211_hwsim_bcn_en_iter(void *data, u8 *mac,
2232                        struct ieee80211_vif *vif)
2233 {
2234     unsigned int *count = data;
2235     struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
2236
2237     if (vp->bcn_en)
2238         (*count)++;
2239 }
2240
2241 static void mac80211_hwsim_vif_info_changed(struct ieee80211_hw *hw,
2242                         struct ieee80211_vif *vif,
2243                         u64 changed)
2244 {
2245     struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
2246
2247     hwsim_check_magic(vif);
2248
2249     wiphy_dbg(hw->wiphy, "%s(changed=0x%llx vif->addr=%pM)\n",
2250           __func__, changed, vif->addr);
2251
2252     if (changed & BSS_CHANGED_ASSOC) {
2253         wiphy_dbg(hw->wiphy, "  ASSOC: assoc=%d aid=%d\n",
2254               vif->cfg.assoc, vif->cfg.aid);
2255         vp->assoc = vif->cfg.assoc;
2256         vp->aid = vif->cfg.aid;
2257     }
2258 }
2259
2260 static void mac80211_hwsim_link_info_changed(struct ieee80211_hw *hw,
2261                          struct ieee80211_vif *vif,
2262                          struct ieee80211_bss_conf *info,
2263                          u64 changed)
2264 {
2265     struct hwsim_vif_priv *vp = (void *)vif->drv_priv;
2266     struct mac80211_hwsim_data *data = hw->priv;
2267     unsigned int link_id = info->link_id;
2268     struct mac80211_hwsim_link_data *link_data = &data->link_data[link_id];
2269
2270     hwsim_check_magic(vif);
2271
2272     wiphy_dbg(hw->wiphy, "%s(changed=0x%llx vif->addr=%pM, link id %u)\n",
2273           __func__, (unsigned long long)changed, vif->addr, link_id);
2274
2275     if (changed & BSS_CHANGED_BSSID) {
2276         wiphy_dbg(hw->wiphy, "%s: BSSID changed: %pM\n",
2277               __func__, info->bssid);
2278         memcpy(vp->bssid, info->bssid, ETH_ALEN);
2279     }
2280
2281     if (changed & BSS_CHANGED_BEACON_ENABLED) {
2282         wiphy_dbg(hw->wiphy, "  BCN EN: %d (BI=%u)\n",
2283               info->enable_beacon, info->beacon_int);
2284         vp->bcn_en = info->enable_beacon;
2285         if (data->started &&
2286             !hrtimer_is_queued(&link_data->beacon_timer) &&
2287             info->enable_beacon) {
2288             u64 tsf, until_tbtt;
2289             u32 bcn_int;
2290             link_data->beacon_int = info->beacon_int * 1024;
2291             tsf = mac80211_hwsim_get_tsf(hw, vif);
2292             bcn_int = link_data->beacon_int;
2293             until_tbtt = bcn_int - do_div(tsf, bcn_int);
2294
2295             hrtimer_start(&link_data->beacon_timer,
2296                       ns_to_ktime(until_tbtt * NSEC_PER_USEC),
2297                       HRTIMER_MODE_REL_SOFT);
2298         } else if (!info->enable_beacon) {
2299             unsigned int count = 0;
2300             ieee80211_iterate_active_interfaces_atomic(
2301                 data->hw, IEEE80211_IFACE_ITER_NORMAL,
2302                 mac80211_hwsim_bcn_en_iter, &count);
2303             wiphy_dbg(hw->wiphy, "  beaconing vifs remaining: %u",
2304                   count);
2305             if (count == 0) {
2306                 hrtimer_cancel(&link_data->beacon_timer);
2307                 link_data->beacon_int = 0;
2308             }
2309         }
2310     }
2311
2312     if (changed & BSS_CHANGED_ERP_CTS_PROT) {
2313         wiphy_dbg(hw->wiphy, "  ERP_CTS_PROT: %d\n",
2314               info->use_cts_prot);
2315     }
2316
2317     if (changed & BSS_CHANGED_ERP_PREAMBLE) {
2318         wiphy_dbg(hw->wiphy, "  ERP_PREAMBLE: %d\n",
2319               info->use_short_preamble);
2320     }
2321
2322     if (changed & BSS_CHANGED_ERP_SLOT) {
2323         wiphy_dbg(hw->wiphy, "  ERP_SLOT: %d\n", info->use_short_slot);
2324     }
2325
2326     if (changed & BSS_CHANGED_HT) {
2327         wiphy_dbg(hw->wiphy, "  HT: op_mode=0x%x\n",
2328               info->ht_operation_mode);
2329     }
2330
2331     if (changed & BSS_CHANGED_BASIC_RATES) {
2332         wiphy_dbg(hw->wiphy, "  BASIC_RATES: 0x%llx\n",
2333               (unsigned long long) info->basic_rates);
2334     }
2335
2336     if (changed & BSS_CHANGED_TXPOWER)
2337         wiphy_dbg(hw->wiphy, "  TX Power: %d dBm\n", info->txpower);
2338 }
2339
2340 static void
2341 mac80211_hwsim_sta_rc_update(struct ieee80211_hw *hw,
2342                  struct ieee80211_vif *vif,
2343                  struct ieee80211_sta *sta,
2344                  u32 changed)
2345 {
2346     struct mac80211_hwsim_data *data = hw->priv;
2347     u32 bw = U32_MAX;
2348     int link_id;
2349
2350     rcu_read_lock();
2351     for (link_id = 0;
2352          link_id < ARRAY_SIZE(vif->link_conf);
2353          link_id++) {
2354         enum nl80211_chan_width confbw = NL80211_CHAN_WIDTH_20_NOHT;
2355         struct ieee80211_bss_conf *vif_conf;
2356         struct ieee80211_link_sta *link_sta;
2357
2358         link_sta = rcu_dereference(sta->link[link_id]);
2359
2360         if (!link_sta)
2361             continue;
2362
2363         switch (link_sta->bandwidth) {
2364 #define C(_bw) case IEEE80211_STA_RX_BW_##_bw: bw = _bw; break
2365         C(20);
2366         C(40);
2367         C(80);
2368         C(160);
2369         C(320);
2370 #undef C
2371         }
2372
2373         if (!data->use_chanctx) {
2374             confbw = data->bw;
2375         } else {
2376             struct ieee80211_chanctx_conf *chanctx_conf;
2377
2378             vif_conf = rcu_dereference(vif->link_conf[link_id]);
2379             if (WARN_ON(!vif_conf))
2380                 continue;
2381
2382             chanctx_conf = rcu_dereference(vif_conf->chanctx_conf);
2383
2384             if (!WARN_ON(!chanctx_conf))
2385                 confbw = chanctx_conf->def.width;
2386         }
2387
2388         WARN(bw > hwsim_get_chanwidth(confbw),
2389              "intf %pM [link=%d]: bad STA %pM bandwidth %d MHz (%d) > channel config %d MHz (%d)\n",
2390              vif->addr, link_id, sta->addr, bw, sta->deflink.bandwidth,
2391              hwsim_get_chanwidth(data->bw), data->bw);
2392
2393
2394     }
2395     rcu_read_unlock();
2396
2397
2398 }
2399
2400 static int mac80211_hwsim_sta_add(struct ieee80211_hw *hw,
2401                   struct ieee80211_vif *vif,
2402                   struct ieee80211_sta *sta)
2403 {
2404     hwsim_check_magic(vif);
2405     hwsim_set_sta_magic(sta);
2406     mac80211_hwsim_sta_rc_update(hw, vif, sta, 0);
2407
2408     return 0;
2409 }
2410
2411 static int mac80211_hwsim_sta_remove(struct ieee80211_hw *hw,
2412                      struct ieee80211_vif *vif,
2413                      struct ieee80211_sta *sta)
2414 {
2415     hwsim_check_magic(vif);
2416     hwsim_clear_sta_magic(sta);
2417
2418     return 0;
2419 }
2420
2421 static int mac80211_hwsim_sta_state(struct ieee80211_hw *hw,
2422                     struct ieee80211_vif *vif,
2423                     struct ieee80211_sta *sta,
2424                     enum ieee80211_sta_state old_state,
2425                     enum ieee80211_sta_state new_state)
2426 {
2427     if (new_state == IEEE80211_STA_NOTEXIST)
2428         return mac80211_hwsim_sta_remove(hw, vif, sta);
2429
2430     if (old_state == IEEE80211_STA_NOTEXIST)
2431         return mac80211_hwsim_sta_add(hw, vif, sta);
2432
2433     return 0;
2434 }
2435
2436 static void mac80211_hwsim_sta_notify(struct ieee80211_hw *hw,
2437                       struct ieee80211_vif *vif,
2438                       enum sta_notify_cmd cmd,
2439                       struct ieee80211_sta *sta)
2440 {
2441     hwsim_check_magic(vif);
2442
2443     switch (cmd) {
2444     case STA_NOTIFY_SLEEP:
2445     case STA_NOTIFY_AWAKE:
2446         /* TODO: make good use of these flags */
2447         break;
2448     default:
2449         WARN(1, "Invalid sta notify: %d\n", cmd);
2450         break;
2451     }
2452 }
2453
2454 static int mac80211_hwsim_set_tim(struct ieee80211_hw *hw,
2455                   struct ieee80211_sta *sta,
2456                   bool set)
2457 {
2458     hwsim_check_sta_magic(sta);
2459     return 0;
2460 }
2461
2462 static int mac80211_hwsim_conf_tx(struct ieee80211_hw *hw,
2463                   struct ieee80211_vif *vif,
2464                   unsigned int link_id, u16 queue,
2465                   const struct ieee80211_tx_queue_params *params)
2466 {
2467     wiphy_dbg(hw->wiphy,
2468           "%s (queue=%d txop=%d cw_min=%d cw_max=%d aifs=%d)\n",
2469           __func__, queue,
2470           params->txop, params->cw_min,
2471           params->cw_max, params->aifs);
2472     return 0;
2473 }
2474
2475 static int mac80211_hwsim_get_survey(struct ieee80211_hw *hw, int idx,
2476                      struct survey_info *survey)
2477 {
2478     struct mac80211_hwsim_data *hwsim = hw->priv;
2479
2480     if (idx < 0 || idx >= ARRAY_SIZE(hwsim->survey_data))
2481         return -ENOENT;
2482
2483     mutex_lock(&hwsim->mutex);
2484     survey->channel = hwsim->survey_data[idx].channel;
2485     if (!survey->channel) {
2486         mutex_unlock(&hwsim->mutex);
2487         return -ENOENT;
2488     }
2489
2490     /*
2491      * Magically conjured dummy values --- this is only ok for simulated hardware.
2492      *
2493      * A real driver which cannot determine real values noise MUST NOT
2494      * report any, especially not a magically conjured ones :-)
2495      */
2496     survey->filled = SURVEY_INFO_NOISE_DBM |
2497              SURVEY_INFO_TIME |
2498              SURVEY_INFO_TIME_BUSY;
2499     survey->noise = -92;
2500     survey->time =
2501         jiffies_to_msecs(hwsim->survey_data[idx].end -
2502                  hwsim->survey_data[idx].start);
2503     /* report 12.5% of channel time is used */
2504     survey->time_busy = survey->time/8;
2505     mutex_unlock(&hwsim->mutex);
2506
2507     return 0;
2508 }
2509
2510 #ifdef CONFIG_NL80211_TESTMODE
2511 /*
2512  * This section contains example code for using netlink
2513  * attributes with the testmode command in nl80211.
2514  */
2515
2516 /* These enums need to be kept in sync with userspace */
2517 enum hwsim_testmode_attr {
2518     __HWSIM_TM_ATTR_INVALID = 0,
2519     HWSIM_TM_ATTR_CMD   = 1,
2520     HWSIM_TM_ATTR_PS    = 2,
2521
2522     /* keep last */
2523     __HWSIM_TM_ATTR_AFTER_LAST,
2524     HWSIM_TM_ATTR_MAX   = __HWSIM_TM_ATTR_AFTER_LAST - 1
2525 };
2526
2527 enum hwsim_testmode_cmd {
2528     HWSIM_TM_CMD_SET_PS     = 0,
2529     HWSIM_TM_CMD_GET_PS     = 1,
2530     HWSIM_TM_CMD_STOP_QUEUES    = 2,
2531     HWSIM_TM_CMD_WAKE_QUEUES    = 3,
2532 };
2533
2534 static const struct nla_policy hwsim_testmode_policy[HWSIM_TM_ATTR_MAX + 1] = {
2535     [HWSIM_TM_ATTR_CMD] = { .type = NLA_U32 },
2536     [HWSIM_TM_ATTR_PS] = { .type = NLA_U32 },
2537 };
2538
2539 static int mac80211_hwsim_testmode_cmd(struct ieee80211_hw *hw,
2540                        struct ieee80211_vif *vif,
2541                        void *data, int len)
2542 {
2543     struct mac80211_hwsim_data *hwsim = hw->priv;
2544     struct nlattr *tb[HWSIM_TM_ATTR_MAX + 1];
2545     struct sk_buff *skb;
2546     int err, ps;
2547
2548     err = nla_parse_deprecated(tb, HWSIM_TM_ATTR_MAX, data, len,
2549                    hwsim_testmode_policy, NULL);
2550     if (err)
2551         return err;
2552
2553     if (!tb[HWSIM_TM_ATTR_CMD])
2554         return -EINVAL;
2555
2556     switch (nla_get_u32(tb[HWSIM_TM_ATTR_CMD])) {
2557     case HWSIM_TM_CMD_SET_PS:
2558         if (!tb[HWSIM_TM_ATTR_PS])
2559             return -EINVAL;
2560         ps = nla_get_u32(tb[HWSIM_TM_ATTR_PS]);
2561         return hwsim_fops_ps_write(hwsim, ps);
2562     case HWSIM_TM_CMD_GET_PS:
2563         skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
2564                         nla_total_size(sizeof(u32)));
2565         if (!skb)
2566             return -ENOMEM;
2567         if (nla_put_u32(skb, HWSIM_TM_ATTR_PS, hwsim->ps))
2568             goto nla_put_failure;
2569         return cfg80211_testmode_reply(skb);
2570     case HWSIM_TM_CMD_STOP_QUEUES:
2571         ieee80211_stop_queues(hw);
2572         return 0;
2573     case HWSIM_TM_CMD_WAKE_QUEUES:
2574         ieee80211_wake_queues(hw);
2575         return 0;
2576     default:
2577         return -EOPNOTSUPP;
2578     }
2579
2580  nla_put_failure:
2581     kfree_skb(skb);
2582     return -ENOBUFS;
2583 }
2584 #endif
2585
2586 static int mac80211_hwsim_ampdu_action(struct ieee80211_hw *hw,
2587                        struct ieee80211_vif *vif,
2588                        struct ieee80211_ampdu_params *params)
2589 {
2590     struct ieee80211_sta *sta = params->sta;
2591     enum ieee80211_ampdu_mlme_action action = params->action;
2592     u16 tid = params->tid;
2593
2594     switch (action) {
2595     case IEEE80211_AMPDU_TX_START:
2596         return IEEE80211_AMPDU_TX_START_IMMEDIATE;
2597     case IEEE80211_AMPDU_TX_STOP_CONT:
2598     case IEEE80211_AMPDU_TX_STOP_FLUSH:
2599     case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
2600         ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
2601         break;
2602     case IEEE80211_AMPDU_TX_OPERATIONAL:
2603         break;
2604     case IEEE80211_AMPDU_RX_START:
2605     case IEEE80211_AMPDU_RX_STOP:
2606         break;
2607     default:
2608         return -EOPNOTSUPP;
2609     }
2610
2611     return 0;
2612 }
2613
2614 static void mac80211_hwsim_flush(struct ieee80211_hw *hw,
2615                  struct ieee80211_vif *vif,
2616                  u32 queues, bool drop)
2617 {
2618     /* Not implemented, queues only on kernel side */
2619 }
2620
2621 static void hw_scan_work(struct work_struct *work)
2622 {
2623     struct mac80211_hwsim_data *hwsim =
2624         container_of(work, struct mac80211_hwsim_data, hw_scan.work);
2625     struct cfg80211_scan_request *req = hwsim->hw_scan_request;
2626     int dwell, i;
2627
2628     mutex_lock(&hwsim->mutex);
2629     if (hwsim->scan_chan_idx >= req->n_channels) {
2630         struct cfg80211_scan_info info = {
2631             .aborted = false,
2632         };
2633
2634         wiphy_dbg(hwsim->hw->wiphy, "hw scan complete\n");
2635         ieee80211_scan_completed(hwsim->hw, &info);
2636         hwsim->hw_scan_request = NULL;
2637         hwsim->hw_scan_vif = NULL;
2638         hwsim->tmp_chan = NULL;
2639         mutex_unlock(&hwsim->mutex);
2640         mac80211_hwsim_config_mac_nl(hwsim->hw, hwsim->scan_addr,
2641                          false);
2642         return;
2643     }
2644
2645     wiphy_dbg(hwsim->hw->wiphy, "hw scan %d MHz\n",
2646           req->channels[hwsim->scan_chan_idx]->center_freq);
2647
2648     hwsim->tmp_chan = req->channels[hwsim->scan_chan_idx];
2649     if (hwsim->tmp_chan->flags & (IEEE80211_CHAN_NO_IR |
2650                       IEEE80211_CHAN_RADAR) ||
2651         !req->n_ssids) {
2652         dwell = 120;
2653     } else {
2654         dwell = 30;
2655         /* send probes */
2656         for (i = 0; i < req->n_ssids; i++) {
2657             struct sk_buff *probe;
2658             struct ieee80211_mgmt *mgmt;
2659
2660             probe = ieee80211_probereq_get(hwsim->hw,
2661                                hwsim->scan_addr,
2662                                req->ssids[i].ssid,
2663                                req->ssids[i].ssid_len,
2664                                req->ie_len);
2665             if (!probe)
2666                 continue;
2667
2668             mgmt = (struct ieee80211_mgmt *) probe->data;
2669             memcpy(mgmt->da, req->bssid, ETH_ALEN);
2670             memcpy(mgmt->bssid, req->bssid, ETH_ALEN);
2671
2672             if (req->ie_len)
2673                 skb_put_data(probe, req->ie, req->ie_len);
2674
2675             rcu_read_lock();
2676             if (!ieee80211_tx_prepare_skb(hwsim->hw,
2677                               hwsim->hw_scan_vif,
2678                               probe,
2679                               hwsim->tmp_chan->band,
2680                               NULL)) {
2681                 rcu_read_unlock();
2682                 kfree_skb(probe);
2683                 continue;
2684             }
2685
2686             local_bh_disable();
2687             mac80211_hwsim_tx_frame(hwsim->hw, probe,
2688                         hwsim->tmp_chan);
2689             rcu_read_unlock();
2690             local_bh_enable();
2691         }
2692     }
2693     ieee80211_queue_delayed_work(hwsim->hw, &hwsim->hw_scan,
2694                      msecs_to_jiffies(dwell));
2695     hwsim->survey_data[hwsim->scan_chan_idx].channel = hwsim->tmp_chan;
2696     hwsim->survey_data[hwsim->scan_chan_idx].start = jiffies;
2697     hwsim->survey_data[hwsim->scan_chan_idx].end =
2698         jiffies + msecs_to_jiffies(dwell);
2699     hwsim->scan_chan_idx++;
2700     mutex_unlock(&hwsim->mutex);
2701 }
2702
2703 static int mac80211_hwsim_hw_scan(struct ieee80211_hw *hw,
2704                   struct ieee80211_vif *vif,
2705                   struct ieee80211_scan_request *hw_req)
2706 {
2707     struct mac80211_hwsim_data *hwsim = hw->priv;
2708     struct cfg80211_scan_request *req = &hw_req->req;
2709
2710     mutex_lock(&hwsim->mutex);
2711     if (WARN_ON(hwsim->tmp_chan || hwsim->hw_scan_request)) {
2712         mutex_unlock(&hwsim->mutex);
2713         return -EBUSY;
2714     }
2715     hwsim->hw_scan_request = req;
2716     hwsim->hw_scan_vif = vif;
2717     hwsim->scan_chan_idx = 0;
2718     if (req->flags & NL80211_SCAN_FLAG_RANDOM_ADDR)
2719         get_random_mask_addr(hwsim->scan_addr,
2720                      hw_req->req.mac_addr,
2721                      hw_req->req.mac_addr_mask);
2722     else
2723         memcpy(hwsim->scan_addr, vif->addr, ETH_ALEN);
2724     memset(hwsim->survey_data, 0, sizeof(hwsim->survey_data));
2725     mutex_unlock(&hwsim->mutex);
2726
2727     mac80211_hwsim_config_mac_nl(hw, hwsim->scan_addr, true);
2728     wiphy_dbg(hw->wiphy, "hwsim hw_scan request\n");
2729
2730     ieee80211_queue_delayed_work(hwsim->hw, &hwsim->hw_scan, 0);
2731
2732     return 0;
2733 }
2734
2735 static void mac80211_hwsim_cancel_hw_scan(struct ieee80211_hw *hw,
2736                       struct ieee80211_vif *vif)
2737 {
2738     struct mac80211_hwsim_data *hwsim = hw->priv;
2739     struct cfg80211_scan_info info = {
2740         .aborted = true,
2741     };
2742
2743     wiphy_dbg(hw->wiphy, "hwsim cancel_hw_scan\n");
2744
2745     cancel_delayed_work_sync(&hwsim->hw_scan);
2746
2747     mutex_lock(&hwsim->mutex);
2748     ieee80211_scan_completed(hwsim->hw, &info);
2749     hwsim->tmp_chan = NULL;
2750     hwsim->hw_scan_request = NULL;
2751     hwsim->hw_scan_vif = NULL;
2752     mutex_unlock(&hwsim->mutex);
2753 }
2754
2755 static void mac80211_hwsim_sw_scan(struct ieee80211_hw *hw,
2756                    struct ieee80211_vif *vif,
2757                    const u8 *mac_addr)
2758 {
2759     struct mac80211_hwsim_data *hwsim = hw->priv;
2760
2761     mutex_lock(&hwsim->mutex);
2762
2763     if (hwsim->scanning) {
2764         pr_debug("two hwsim sw_scans detected!\n");
2765         goto out;
2766     }
2767
2768     pr_debug("hwsim sw_scan request, prepping stuff\n");
2769
2770     memcpy(hwsim->scan_addr, mac_addr, ETH_ALEN);
2771     mac80211_hwsim_config_mac_nl(hw, hwsim->scan_addr, true);
2772     hwsim->scanning = true;
2773     memset(hwsim->survey_data, 0, sizeof(hwsim->survey_data));
2774
2775 out:
2776     mutex_unlock(&hwsim->mutex);
2777 }
2778
2779 static void mac80211_hwsim_sw_scan_complete(struct ieee80211_hw *hw,
2780                         struct ieee80211_vif *vif)
2781 {
2782     struct mac80211_hwsim_data *hwsim = hw->priv;
2783
2784     mutex_lock(&hwsim->mutex);
2785
2786     pr_debug("hwsim sw_scan_complete\n");
2787     hwsim->scanning = false;
2788     mac80211_hwsim_config_mac_nl(hw, hwsim->scan_addr, false);
2789     eth_zero_addr(hwsim->scan_addr);
2790
2791     mutex_unlock(&hwsim->mutex);
2792 }
2793
2794 static void hw_roc_start(struct work_struct *work)
2795 {
2796     struct mac80211_hwsim_data *hwsim =
2797         container_of(work, struct mac80211_hwsim_data, roc_start.work);
2798
2799     mutex_lock(&hwsim->mutex);
2800
2801     wiphy_dbg(hwsim->hw->wiphy, "hwsim ROC begins\n");
2802     hwsim->tmp_chan = hwsim->roc_chan;
2803     ieee80211_ready_on_channel(hwsim->hw);
2804
2805     ieee80211_queue_delayed_work(hwsim->hw, &hwsim->roc_done,
2806                      msecs_to_jiffies(hwsim->roc_duration));
2807
2808     mutex_unlock(&hwsim->mutex);
2809 }
2810
2811 static void hw_roc_done(struct work_struct *work)
2812 {
2813     struct mac80211_hwsim_data *hwsim =
2814         container_of(work, struct mac80211_hwsim_data, roc_done.work);
2815
2816     mutex_lock(&hwsim->mutex);
2817     ieee80211_remain_on_channel_expired(hwsim->hw);
2818     hwsim->tmp_chan = NULL;
2819     mutex_unlock(&hwsim->mutex);
2820
2821     wiphy_dbg(hwsim->hw->wiphy, "hwsim ROC expired\n");
2822 }
2823
2824 static int mac80211_hwsim_roc(struct ieee80211_hw *hw,
2825                   struct ieee80211_vif *vif,
2826                   struct ieee80211_channel *chan,
2827                   int duration,
2828                   enum ieee80211_roc_type type)
2829 {
2830     struct mac80211_hwsim_data *hwsim = hw->priv;
2831
2832     mutex_lock(&hwsim->mutex);
2833     if (WARN_ON(hwsim->tmp_chan || hwsim->hw_scan_request)) {
2834         mutex_unlock(&hwsim->mutex);
2835         return -EBUSY;
2836     }
2837
2838     hwsim->roc_chan = chan;
2839     hwsim->roc_duration = duration;
2840     mutex_unlock(&hwsim->mutex);
2841
2842     wiphy_dbg(hw->wiphy, "hwsim ROC (%d MHz, %d ms)\n",
2843           chan->center_freq, duration);
2844     ieee80211_queue_delayed_work(hw, &hwsim->roc_start, HZ/50);
2845
2846     return 0;
2847 }
2848
2849 static int mac80211_hwsim_croc(struct ieee80211_hw *hw,
2850                    struct ieee80211_vif *vif)
2851 {
2852     struct mac80211_hwsim_data *hwsim = hw->priv;
2853
2854     cancel_delayed_work_sync(&hwsim->roc_start);
2855     cancel_delayed_work_sync(&hwsim->roc_done);
2856
2857     mutex_lock(&hwsim->mutex);
2858     hwsim->tmp_chan = NULL;
2859     mutex_unlock(&hwsim->mutex);
2860
2861     wiphy_dbg(hw->wiphy, "hwsim ROC canceled\n");
2862
2863     return 0;
2864 }
2865
2866 static int mac80211_hwsim_add_chanctx(struct ieee80211_hw *hw,
2867                       struct ieee80211_chanctx_conf *ctx)
2868 {
2869     struct mac80211_hwsim_data *hwsim = hw->priv;
2870
2871     mutex_lock(&hwsim->mutex);
2872     hwsim->chanctx = ctx;
2873     mutex_unlock(&hwsim->mutex);
2874     hwsim_set_chanctx_magic(ctx);
2875     wiphy_dbg(hw->wiphy,
2876           "add channel context control: %d MHz/width: %d/cfreqs:%d/%d MHz\n",
2877           ctx->def.chan->center_freq, ctx->def.width,
2878           ctx->def.center_freq1, ctx->def.center_freq2);
2879     return 0;
2880 }
2881
2882 static void mac80211_hwsim_remove_chanctx(struct ieee80211_hw *hw,
2883                       struct ieee80211_chanctx_conf *ctx)
2884 {
2885     struct mac80211_hwsim_data *hwsim = hw->priv;
2886
2887     mutex_lock(&hwsim->mutex);
2888     hwsim->chanctx = NULL;
2889     mutex_unlock(&hwsim->mutex);
2890     wiphy_dbg(hw->wiphy,
2891           "remove channel context control: %d MHz/width: %d/cfreqs:%d/%d MHz\n",
2892           ctx->def.chan->center_freq, ctx->def.width,
2893           ctx->def.center_freq1, ctx->def.center_freq2);
2894     hwsim_check_chanctx_magic(ctx);
2895     hwsim_clear_chanctx_magic(ctx);
2896 }
2897
2898 static void mac80211_hwsim_change_chanctx(struct ieee80211_hw *hw,
2899                       struct ieee80211_chanctx_conf *ctx,
2900                       u32 changed)
2901 {
2902     struct mac80211_hwsim_data *hwsim = hw->priv;
2903
2904     mutex_lock(&hwsim->mutex);
2905     hwsim->chanctx = ctx;
2906     mutex_unlock(&hwsim->mutex);
2907     hwsim_check_chanctx_magic(ctx);
2908     wiphy_dbg(hw->wiphy,
2909           "change channel context control: %d MHz/width: %d/cfreqs:%d/%d MHz\n",
2910           ctx->def.chan->center_freq, ctx->def.width,
2911           ctx->def.center_freq1, ctx->def.center_freq2);
2912 }
2913
2914 static int mac80211_hwsim_assign_vif_chanctx(struct ieee80211_hw *hw,
2915                          struct ieee80211_vif *vif,
2916                          struct ieee80211_bss_conf *link_conf,
2917                          struct ieee80211_chanctx_conf *ctx)
2918 {
2919     hwsim_check_magic(vif);
2920     hwsim_check_chanctx_magic(ctx);
2921
2922     return 0;
2923 }
2924
2925 static void mac80211_hwsim_unassign_vif_chanctx(struct ieee80211_hw *hw,
2926                         struct ieee80211_vif *vif,
2927                         struct ieee80211_bss_conf *link_conf,
2928                         struct ieee80211_chanctx_conf *ctx)
2929 {
2930     hwsim_check_magic(vif);
2931     hwsim_check_chanctx_magic(ctx);
2932 }
2933
2934 static const char mac80211_hwsim_gstrings_stats[][ETH_GSTRING_LEN] = {
2935     "tx_pkts_nic",
2936     "tx_bytes_nic",
2937     "rx_pkts_nic",
2938     "rx_bytes_nic",
2939     "d_tx_dropped",
2940     "d_tx_failed",
2941     "d_ps_mode",
2942     "d_group",
2943 };
2944
2945 #define MAC80211_HWSIM_SSTATS_LEN ARRAY_SIZE(mac80211_hwsim_gstrings_stats)
2946
2947 static void mac80211_hwsim_get_et_strings(struct ieee80211_hw *hw,
2948                       struct ieee80211_vif *vif,
2949                       u32 sset, u8 *data)
2950 {
2951     if (sset == ETH_SS_STATS)
2952         memcpy(data, *mac80211_hwsim_gstrings_stats,
2953                sizeof(mac80211_hwsim_gstrings_stats));
2954 }
2955
2956 static int mac80211_hwsim_get_et_sset_count(struct ieee80211_hw *hw,
2957                         struct ieee80211_vif *vif, int sset)
2958 {
2959     if (sset == ETH_SS_STATS)
2960         return MAC80211_HWSIM_SSTATS_LEN;
2961     return 0;
2962 }
2963
2964 static void mac80211_hwsim_get_et_stats(struct ieee80211_hw *hw,
2965                     struct ieee80211_vif *vif,
2966                     struct ethtool_stats *stats, u64 *data)
2967 {
2968     struct mac80211_hwsim_data *ar = hw->priv;
2969     int i = 0;
2970
2971     data[i++] = ar->tx_pkts;
2972     data[i++] = ar->tx_bytes;
2973     data[i++] = ar->rx_pkts;
2974     data[i++] = ar->rx_bytes;
2975     data[i++] = ar->tx_dropped;
2976     data[i++] = ar->tx_failed;
2977     data[i++] = ar->ps;
2978     data[i++] = ar->group;
2979
2980     WARN_ON(i != MAC80211_HWSIM_SSTATS_LEN);
2981 }
2982
2983 static int mac80211_hwsim_tx_last_beacon(struct ieee80211_hw *hw)
2984 {
2985     return 1;
2986 }
2987
2988 static int mac80211_hwsim_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
2989 {
2990     return -EOPNOTSUPP;
2991 }
2992
2993 static int mac80211_hwsim_change_vif_links(struct ieee80211_hw *hw,
2994                        struct ieee80211_vif *vif,
2995                        u16 old_links, u16 new_links,
2996                        struct ieee80211_bss_conf *old[IEEE80211_MLD_MAX_NUM_LINKS])
2997 {
2998     unsigned long rem = old_links & ~new_links ?: BIT(0);
2999     unsigned long add = new_links & ~old_links;
3000     int i;
3001
3002     for_each_set_bit(i, &rem, IEEE80211_MLD_MAX_NUM_LINKS)
3003         mac80211_hwsim_config_mac_nl(hw, old[i]->addr, false);
3004
3005     for_each_set_bit(i, &add, IEEE80211_MLD_MAX_NUM_LINKS) {
3006         struct ieee80211_bss_conf *link_conf;
3007
3008         /* FIXME: figure out how to get the locking here */
3009         link_conf = rcu_dereference_protected(vif->link_conf[i], 1);
3010         if (WARN_ON(!link_conf))
3011             continue;
3012
3013         mac80211_hwsim_config_mac_nl(hw, link_conf->addr, true);
3014     }
3015
3016     return 0;
3017 }
3018
3019 static int mac80211_hwsim_change_sta_links(struct ieee80211_hw *hw,
3020                        struct ieee80211_vif *vif,
3021                        struct ieee80211_sta *sta,
3022                        u16 old_links, u16 new_links)
3023 {
3024     return 0;
3025 }
3026
3027 #define HWSIM_COMMON_OPS                    \
3028     .tx = mac80211_hwsim_tx,                \
3029     .start = mac80211_hwsim_start,              \
3030     .stop = mac80211_hwsim_stop,                \
3031     .add_interface = mac80211_hwsim_add_interface,      \
3032     .change_interface = mac80211_hwsim_change_interface,    \
3033     .remove_interface = mac80211_hwsim_remove_interface,    \
3034     .config = mac80211_hwsim_config,            \
3035     .configure_filter = mac80211_hwsim_configure_filter,    \
3036     .vif_cfg_changed = mac80211_hwsim_vif_info_changed, \
3037     .link_info_changed = mac80211_hwsim_link_info_changed,  \
3038     .tx_last_beacon = mac80211_hwsim_tx_last_beacon,    \
3039     .sta_notify = mac80211_hwsim_sta_notify,        \
3040     .sta_rc_update = mac80211_hwsim_sta_rc_update,      \
3041     .conf_tx = mac80211_hwsim_conf_tx,          \
3042     .get_survey = mac80211_hwsim_get_survey,        \
3043     CFG80211_TESTMODE_CMD(mac80211_hwsim_testmode_cmd)  \
3044     .ampdu_action = mac80211_hwsim_ampdu_action,        \
3045     .flush = mac80211_hwsim_flush,              \
3046     .get_et_sset_count = mac80211_hwsim_get_et_sset_count,  \
3047     .get_et_stats = mac80211_hwsim_get_et_stats,        \
3048     .get_et_strings = mac80211_hwsim_get_et_strings,
3049
3050 #define HWSIM_NON_MLO_OPS                   \
3051     .sta_add = mac80211_hwsim_sta_add,          \
3052     .sta_remove = mac80211_hwsim_sta_remove,        \
3053     .set_tim = mac80211_hwsim_set_tim,          \
3054     .get_tsf = mac80211_hwsim_get_tsf,          \
3055     .set_tsf = mac80211_hwsim_set_tsf,
3056
3057 static const struct ieee80211_ops mac80211_hwsim_ops = {
3058     HWSIM_COMMON_OPS
3059     HWSIM_NON_MLO_OPS
3060     .sw_scan_start = mac80211_hwsim_sw_scan,
3061     .sw_scan_complete = mac80211_hwsim_sw_scan_complete,
3062 };
3063
3064 #define HWSIM_CHANCTX_OPS                   \
3065     .hw_scan = mac80211_hwsim_hw_scan,          \
3066     .cancel_hw_scan = mac80211_hwsim_cancel_hw_scan,    \
3067     .remain_on_channel = mac80211_hwsim_roc,        \
3068     .cancel_remain_on_channel = mac80211_hwsim_croc,    \
3069     .add_chanctx = mac80211_hwsim_add_chanctx,      \
3070     .remove_chanctx = mac80211_hwsim_remove_chanctx,    \
3071     .change_chanctx = mac80211_hwsim_change_chanctx,    \
3072     .assign_vif_chanctx = mac80211_hwsim_assign_vif_chanctx,\
3073     .unassign_vif_chanctx = mac80211_hwsim_unassign_vif_chanctx,
3074
3075 static const struct ieee80211_ops mac80211_hwsim_mchan_ops = {
3076     HWSIM_COMMON_OPS
3077     HWSIM_NON_MLO_OPS
3078     HWSIM_CHANCTX_OPS
3079 };
3080
3081 static const struct ieee80211_ops mac80211_hwsim_mlo_ops = {
3082     HWSIM_COMMON_OPS
3083     HWSIM_CHANCTX_OPS
3084     .set_rts_threshold = mac80211_hwsim_set_rts_threshold,
3085     .change_vif_links = mac80211_hwsim_change_vif_links,
3086     .change_sta_links = mac80211_hwsim_change_sta_links,
3087     .sta_state = mac80211_hwsim_sta_state,
3088 };
3089
3090 struct hwsim_new_radio_params {
3091     unsigned int channels;
3092     const char *reg_alpha2;
3093     const struct ieee80211_regdomain *regd;
3094     bool reg_strict;
3095     bool p2p_device;
3096     bool use_chanctx;
3097     bool destroy_on_close;
3098     const char *hwname;
3099     bool no_vif;
3100     const u8 *perm_addr;
3101     u32 iftypes;
3102     u32 *ciphers;
3103     u8 n_ciphers;
3104     bool mlo;
3105 };
3106
3107 static void hwsim_mcast_config_msg(struct sk_buff *mcast_skb,
3108                    struct genl_info *info)
3109 {
3110     if (info)
3111         genl_notify(&hwsim_genl_family, mcast_skb, info,
3112                 HWSIM_MCGRP_CONFIG, GFP_KERNEL);
3113     else
3114         genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0,
3115                   HWSIM_MCGRP_CONFIG, GFP_KERNEL);
3116 }
3117
3118 static int append_radio_msg(struct sk_buff *skb, int id,
3119                 struct hwsim_new_radio_params *param)
3120 {
3121     int ret;
3122
3123     ret = nla_put_u32(skb, HWSIM_ATTR_RADIO_ID, id);
3124     if (ret < 0)
3125         return ret;
3126
3127     if (param->channels) {
3128         ret = nla_put_u32(skb, HWSIM_ATTR_CHANNELS, param->channels);
3129         if (ret < 0)
3130             return ret;
3131     }
3132
3133     if (param->reg_alpha2) {
3134         ret = nla_put(skb, HWSIM_ATTR_REG_HINT_ALPHA2, 2,
3135                   param->reg_alpha2);
3136         if (ret < 0)
3137             return ret;
3138     }
3139
3140     if (param->regd) {
3141         int i;
3142
3143         for (i = 0; i < ARRAY_SIZE(hwsim_world_regdom_custom); i++) {
3144             if (hwsim_world_regdom_custom[i] != param->regd)
3145                 continue;
3146
3147             ret = nla_put_u32(skb, HWSIM_ATTR_REG_CUSTOM_REG, i);
3148             if (ret < 0)
3149                 return ret;
3150             break;
3151         }
3152     }
3153
3154     if (param->reg_strict) {
3155         ret = nla_put_flag(skb, HWSIM_ATTR_REG_STRICT_REG);
3156         if (ret < 0)
3157             return ret;
3158     }
3159
3160     if (param->p2p_device) {
3161         ret = nla_put_flag(skb, HWSIM_ATTR_SUPPORT_P2P_DEVICE);
3162         if (ret < 0)
3163             return ret;
3164     }
3165
3166     if (param->use_chanctx) {
3167         ret = nla_put_flag(skb, HWSIM_ATTR_USE_CHANCTX);
3168         if (ret < 0)
3169             return ret;
3170     }
3171
3172     if (param->hwname) {
3173         ret = nla_put(skb, HWSIM_ATTR_RADIO_NAME,
3174                   strlen(param->hwname), param->hwname);
3175         if (ret < 0)
3176             return ret;
3177     }
3178
3179     return 0;
3180 }
3181
3182 static void hwsim_mcast_new_radio(int id, struct genl_info *info,
3183                   struct hwsim_new_radio_params *param)
3184 {
3185     struct sk_buff *mcast_skb;
3186     void *data;
3187
3188     mcast_skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
3189     if (!mcast_skb)
3190         return;
3191
3192     data = genlmsg_put(mcast_skb, 0, 0, &hwsim_genl_family, 0,
3193                HWSIM_CMD_NEW_RADIO);
3194     if (!data)
3195         goto out_err;
3196
3197     if (append_radio_msg(mcast_skb, id, param) < 0)
3198         goto out_err;
3199
3200     genlmsg_end(mcast_skb, data);
3201
3202     hwsim_mcast_config_msg(mcast_skb, info);
3203     return;
3204
3205 out_err:
3206     nlmsg_free(mcast_skb);
3207 }
3208
3209 static const struct ieee80211_sband_iftype_data sband_capa_2ghz[] = {
3210     {
3211         .types_mask = BIT(NL80211_IFTYPE_STATION) |
3212                   BIT(NL80211_IFTYPE_AP),
3213         .he_cap = {
3214             .has_he = true,
3215             .he_cap_elem = {
3216                 .mac_cap_info[0] =
3217                     IEEE80211_HE_MAC_CAP0_HTC_HE,
3218                 .mac_cap_info[1] =
3219                     IEEE80211_HE_MAC_CAP1_TF_MAC_PAD_DUR_16US |
3220                     IEEE80211_HE_MAC_CAP1_MULTI_TID_AGG_RX_QOS_8,
3221                 .mac_cap_info[2] =
3222                     IEEE80211_HE_MAC_CAP2_BSR |
3223                     IEEE80211_HE_MAC_CAP2_MU_CASCADING |
3224                     IEEE80211_HE_MAC_CAP2_ACK_EN,
3225                 .mac_cap_info[3] =
3226                     IEEE80211_HE_MAC_CAP3_OMI_CONTROL |
3227                     IEEE80211_HE_MAC_CAP3_MAX_AMPDU_LEN_EXP_EXT_3,
3228                 .mac_cap_info[4] = IEEE80211_HE_MAC_CAP4_AMSDU_IN_AMPDU,
3229                 .phy_cap_info[1] =
3230                     IEEE80211_HE_PHY_CAP1_PREAMBLE_PUNC_RX_MASK |
3231                     IEEE80211_HE_PHY_CAP1_DEVICE_CLASS_A |
3232                     IEEE80211_HE_PHY_CAP1_LDPC_CODING_IN_PAYLOAD |
3233                     IEEE80211_HE_PHY_CAP1_MIDAMBLE_RX_TX_MAX_NSTS,
3234                 .phy_cap_info[2] =
3235                     IEEE80211_HE_PHY_CAP2_NDP_4x_LTF_AND_3_2US |
3236                     IEEE80211_HE_PHY_CAP2_STBC_TX_UNDER_80MHZ |
3237                     IEEE80211_HE_PHY_CAP2_STBC_RX_UNDER_80MHZ |
3238                     IEEE80211_HE_PHY_CAP2_UL_MU_FULL_MU_MIMO |
3239                     IEEE80211_HE_PHY_CAP2_UL_MU_PARTIAL_MU_MIMO,
3240
3241                 /* Leave all the other PHY capability bytes
3242                  * unset, as DCM, beam forming, RU and PPE
3243                  * threshold information are not supported
3244                  */
3245             },
3246             .he_mcs_nss_supp = {
3247                 .rx_mcs_80 = cpu_to_le16(0xfffa),
3248                 .tx_mcs_80 = cpu_to_le16(0xfffa),
3249                 .rx_mcs_160 = cpu_to_le16(0xffff),
3250                 .tx_mcs_160 = cpu_to_le16(0xffff),
3251                 .rx_mcs_80p80 = cpu_to_le16(0xffff),
3252                 .tx_mcs_80p80 = cpu_to_le16(0xffff),
3253             },
3254         },
3255         .eht_cap = {
3256             .has_eht = true,
3257             .eht_cap_elem = {
3258                 .mac_cap_info[0] =
3259                     IEEE80211_EHT_MAC_CAP0_EPCS_PRIO_ACCESS |
3260                     IEEE80211_EHT_MAC_CAP0_OM_CONTROL |
3261                     IEEE80211_EHT_MAC_CAP0_TRIG_TXOP_SHARING_MODE1,
3262                 .phy_cap_info[0] =
3263                     IEEE80211_EHT_PHY_CAP0_242_TONE_RU_GT20MHZ |
3264                     IEEE80211_EHT_PHY_CAP0_NDP_4_EHT_LFT_32_GI |
3265                     IEEE80211_EHT_PHY_CAP0_PARTIAL_BW_UL_MU_MIMO |
3266                     IEEE80211_EHT_PHY_CAP0_SU_BEAMFORMER |
3267                     IEEE80211_EHT_PHY_CAP0_SU_BEAMFORMEE,
3268                 .phy_cap_info[3] =
3269                     IEEE80211_EHT_PHY_CAP3_NG_16_SU_FEEDBACK |
3270                     IEEE80211_EHT_PHY_CAP3_NG_16_MU_FEEDBACK |
3271                     IEEE80211_EHT_PHY_CAP3_CODEBOOK_4_2_SU_FDBK |
3272                     IEEE80211_EHT_PHY_CAP3_CODEBOOK_7_5_MU_FDBK |
3273                     IEEE80211_EHT_PHY_CAP3_TRIG_SU_BF_FDBK |
3274                     IEEE80211_EHT_PHY_CAP3_TRIG_MU_BF_PART_BW_FDBK |
3275                     IEEE80211_EHT_PHY_CAP3_TRIG_CQI_FDBK,
3276                 .phy_cap_info[4] =
3277                     IEEE80211_EHT_PHY_CAP4_PART_BW_DL_MU_MIMO |
3278                     IEEE80211_EHT_PHY_CAP4_PSR_SR_SUPP |
3279                     IEEE80211_EHT_PHY_CAP4_POWER_BOOST_FACT_SUPP |
3280                     IEEE80211_EHT_PHY_CAP4_EHT_MU_PPDU_4_EHT_LTF_08_GI |
3281                     IEEE80211_EHT_PHY_CAP4_MAX_NC_MASK,
3282                 .phy_cap_info[5] =
3283                     IEEE80211_EHT_PHY_CAP5_NON_TRIG_CQI_FEEDBACK |
3284                     IEEE80211_EHT_PHY_CAP5_TX_LESS_242_TONE_RU_SUPP |
3285                     IEEE80211_EHT_PHY_CAP5_RX_LESS_242_TONE_RU_SUPP |
3286                     IEEE80211_EHT_PHY_CAP5_PPE_THRESHOLD_PRESENT |
3287                     IEEE80211_EHT_PHY_CAP5_COMMON_NOMINAL_PKT_PAD_MASK |
3288                     IEEE80211_EHT_PHY_CAP5_MAX_NUM_SUPP_EHT_LTF_MASK,
3289                 .phy_cap_info[6] =
3290                     IEEE80211_EHT_PHY_CAP6_MAX_NUM_SUPP_EHT_LTF_MASK |
3291                     IEEE80211_EHT_PHY_CAP6_MCS15_SUPP_MASK,
3292                 .phy_cap_info[7] =
3293                     IEEE80211_EHT_PHY_CAP7_20MHZ_STA_RX_NDP_WIDER_BW,
3294             },
3295
3296             /* For all MCS and bandwidth, set 8 NSS for both Tx and
3297              * Rx
3298              */
3299             .eht_mcs_nss_supp = {
3300                 /*
3301                  * Since B0, B1, B2 and B3 are not set in
3302                  * the supported channel width set field in the
3303                  * HE PHY capabilities information field the
3304                  * device is a 20MHz only device on 2.4GHz band.
3305                  */
3306                 .only_20mhz = {
3307                     .rx_tx_mcs7_max_nss = 0x88,
3308                     .rx_tx_mcs9_max_nss = 0x88,
3309                     .rx_tx_mcs11_max_nss = 0x88,
3310                     .rx_tx_mcs13_max_nss = 0x88,
3311                 },
3312             },
3313             /* PPE threshold information is not supported */
3314         },
3315     },
3316 #ifdef CONFIG_MAC80211_MESH
3317     {
3318         .types_mask = BIT(NL80211_IFTYPE_MESH_POINT),
3319         .he_cap = {
3320             .has_he = true,
3321             .he_cap_elem = {
3322                 .mac_cap_info[0] =
3323                     IEEE80211_HE_MAC_CAP0_HTC_HE,
3324                 .mac_cap_info[1] =
3325                     IEEE80211_HE_MAC_CAP1_MULTI_TID_AGG_RX_QOS_8,
3326                 .mac_cap_info[2] =
3327                     IEEE80211_HE_MAC_CAP2_ACK_EN,
3328                 .mac_cap_info[3] =
3329                     IEEE80211_HE_MAC_CAP3_OMI_CONTROL |
3330                     IEEE80211_HE_MAC_CAP3_MAX_AMPDU_LEN_EXP_EXT_3,
3331                 .mac_cap_info[4] = IEEE80211_HE_MAC_CAP4_AMSDU_IN_AMPDU,
3332                 .phy_cap_info[1] =
3333                     IEEE80211_HE_PHY_CAP1_PREAMBLE_PUNC_RX_MASK |
3334                     IEEE80211_HE_PHY_CAP1_DEVICE_CLASS_A |
3335                     IEEE80211_HE_PHY_CAP1_LDPC_CODING_IN_PAYLOAD |
3336                     IEEE80211_HE_PHY_CAP1_MIDAMBLE_RX_TX_MAX_NSTS,
3337                 .phy_cap_info[2] = 0,
3338
3339                 /* Leave all the other PHY capability bytes
3340                  * unset, as DCM, beam forming, RU and PPE
3341                  * threshold information are not supported
3342                  */
3343             },
3344             .he_mcs_nss_supp = {
3345                 .rx_mcs_80 = cpu_to_le16(0xfffa),
3346                 .tx_mcs_80 = cpu_to_le16(0xfffa),
3347                 .rx_mcs_160 = cpu_to_le16(0xffff),
3348                 .tx_mcs_160 = cpu_to_le16(0xffff),
3349                 .rx_mcs_80p80 = cpu_to_le16(0xffff),
3350                 .tx_mcs_80p80 = cpu_to_le16(0xffff),
3351             },
3352         },
3353     },
3354 #endif
3355 };
3356
3357 static const struct ieee80211_sband_iftype_data sband_capa_5ghz[] = {
3358     {
3359         /* TODO: should we support other types, e.g., P2P?*/
3360         .types_mask = BIT(NL80211_IFTYPE_STATION) |
3361                   BIT(NL80211_IFTYPE_AP),
3362         .he_cap = {
3363             .has_he = true,
3364             .he_cap_elem = {
3365                 .mac_cap_info[0] =
3366                     IEEE80211_HE_MAC_CAP0_HTC_HE,
3367                 .mac_cap_info[1] =
3368                     IEEE80211_HE_MAC_CAP1_TF_MAC_PAD_DUR_16US |
3369                     IEEE80211_HE_MAC_CAP1_MULTI_TID_AGG_RX_QOS_8,
3370                 .mac_cap_info[2] =
3371                     IEEE80211_HE_MAC_CAP2_BSR |
3372                     IEEE80211_HE_MAC_CAP2_MU_CASCADING |
3373                     IEEE80211_HE_MAC_CAP2_ACK_EN,
3374                 .mac_cap_info[3] =
3375                     IEEE80211_HE_MAC_CAP3_OMI_CONTROL |
3376                     IEEE80211_HE_MAC_CAP3_MAX_AMPDU_LEN_EXP_EXT_3,
3377                 .mac_cap_info[4] = IEEE80211_HE_MAC_CAP4_AMSDU_IN_AMPDU,
3378                 .phy_cap_info[0] =
3379                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_40MHZ_80MHZ_IN_5G |
3380                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_160MHZ_IN_5G |
3381                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_80PLUS80_MHZ_IN_5G,
3382                 .phy_cap_info[1] =
3383                     IEEE80211_HE_PHY_CAP1_PREAMBLE_PUNC_RX_MASK |
3384                     IEEE80211_HE_PHY_CAP1_DEVICE_CLASS_A |
3385                     IEEE80211_HE_PHY_CAP1_LDPC_CODING_IN_PAYLOAD |
3386                     IEEE80211_HE_PHY_CAP1_MIDAMBLE_RX_TX_MAX_NSTS,
3387                 .phy_cap_info[2] =
3388                     IEEE80211_HE_PHY_CAP2_NDP_4x_LTF_AND_3_2US |
3389                     IEEE80211_HE_PHY_CAP2_STBC_TX_UNDER_80MHZ |
3390                     IEEE80211_HE_PHY_CAP2_STBC_RX_UNDER_80MHZ |
3391                     IEEE80211_HE_PHY_CAP2_UL_MU_FULL_MU_MIMO |
3392                     IEEE80211_HE_PHY_CAP2_UL_MU_PARTIAL_MU_MIMO,
3393
3394                 /* Leave all the other PHY capability bytes
3395                  * unset, as DCM, beam forming, RU and PPE
3396                  * threshold information are not supported
3397                  */
3398             },
3399             .he_mcs_nss_supp = {
3400                 .rx_mcs_80 = cpu_to_le16(0xfffa),
3401                 .tx_mcs_80 = cpu_to_le16(0xfffa),
3402                 .rx_mcs_160 = cpu_to_le16(0xfffa),
3403                 .tx_mcs_160 = cpu_to_le16(0xfffa),
3404                 .rx_mcs_80p80 = cpu_to_le16(0xfffa),
3405                 .tx_mcs_80p80 = cpu_to_le16(0xfffa),
3406             },
3407         },
3408         .eht_cap = {
3409             .has_eht = true,
3410             .eht_cap_elem = {
3411                 .mac_cap_info[0] =
3412                     IEEE80211_EHT_MAC_CAP0_EPCS_PRIO_ACCESS |
3413                     IEEE80211_EHT_MAC_CAP0_OM_CONTROL |
3414                     IEEE80211_EHT_MAC_CAP0_TRIG_TXOP_SHARING_MODE1,
3415                 .phy_cap_info[0] =
3416                     IEEE80211_EHT_PHY_CAP0_242_TONE_RU_GT20MHZ |
3417                     IEEE80211_EHT_PHY_CAP0_NDP_4_EHT_LFT_32_GI |
3418                     IEEE80211_EHT_PHY_CAP0_PARTIAL_BW_UL_MU_MIMO |
3419                     IEEE80211_EHT_PHY_CAP0_SU_BEAMFORMER |
3420                     IEEE80211_EHT_PHY_CAP0_SU_BEAMFORMEE |
3421                     IEEE80211_EHT_PHY_CAP0_BEAMFORMEE_SS_80MHZ_MASK,
3422                 .phy_cap_info[1] =
3423                     IEEE80211_EHT_PHY_CAP1_BEAMFORMEE_SS_80MHZ_MASK |
3424                     IEEE80211_EHT_PHY_CAP1_BEAMFORMEE_SS_160MHZ_MASK,
3425                 .phy_cap_info[2] =
3426                     IEEE80211_EHT_PHY_CAP2_SOUNDING_DIM_80MHZ_MASK |
3427                     IEEE80211_EHT_PHY_CAP2_SOUNDING_DIM_160MHZ_MASK,
3428                 .phy_cap_info[3] =
3429                     IEEE80211_EHT_PHY_CAP3_NG_16_SU_FEEDBACK |
3430                     IEEE80211_EHT_PHY_CAP3_NG_16_MU_FEEDBACK |
3431                     IEEE80211_EHT_PHY_CAP3_CODEBOOK_4_2_SU_FDBK |
3432                     IEEE80211_EHT_PHY_CAP3_CODEBOOK_7_5_MU_FDBK |
3433                     IEEE80211_EHT_PHY_CAP3_TRIG_SU_BF_FDBK |
3434                     IEEE80211_EHT_PHY_CAP3_TRIG_MU_BF_PART_BW_FDBK |
3435                     IEEE80211_EHT_PHY_CAP3_TRIG_CQI_FDBK,
3436                 .phy_cap_info[4] =
3437                     IEEE80211_EHT_PHY_CAP4_PART_BW_DL_MU_MIMO |
3438                     IEEE80211_EHT_PHY_CAP4_PSR_SR_SUPP |
3439                     IEEE80211_EHT_PHY_CAP4_POWER_BOOST_FACT_SUPP |
3440                     IEEE80211_EHT_PHY_CAP4_EHT_MU_PPDU_4_EHT_LTF_08_GI |
3441                     IEEE80211_EHT_PHY_CAP4_MAX_NC_MASK,
3442                 .phy_cap_info[5] =
3443                     IEEE80211_EHT_PHY_CAP5_NON_TRIG_CQI_FEEDBACK |
3444                     IEEE80211_EHT_PHY_CAP5_TX_LESS_242_TONE_RU_SUPP |
3445                     IEEE80211_EHT_PHY_CAP5_RX_LESS_242_TONE_RU_SUPP |
3446                     IEEE80211_EHT_PHY_CAP5_PPE_THRESHOLD_PRESENT |
3447                     IEEE80211_EHT_PHY_CAP5_COMMON_NOMINAL_PKT_PAD_MASK |
3448                     IEEE80211_EHT_PHY_CAP5_MAX_NUM_SUPP_EHT_LTF_MASK,
3449                 .phy_cap_info[6] =
3450                     IEEE80211_EHT_PHY_CAP6_MAX_NUM_SUPP_EHT_LTF_MASK |
3451                     IEEE80211_EHT_PHY_CAP6_MCS15_SUPP_MASK,
3452                 .phy_cap_info[7] =
3453                     IEEE80211_EHT_PHY_CAP7_20MHZ_STA_RX_NDP_WIDER_BW |
3454                     IEEE80211_EHT_PHY_CAP7_NON_OFDMA_UL_MU_MIMO_80MHZ |
3455                     IEEE80211_EHT_PHY_CAP7_NON_OFDMA_UL_MU_MIMO_160MHZ |
3456                     IEEE80211_EHT_PHY_CAP7_MU_BEAMFORMER_80MHZ |
3457                     IEEE80211_EHT_PHY_CAP7_MU_BEAMFORMER_160MHZ,
3458             },
3459
3460             /* For all MCS and bandwidth, set 8 NSS for both Tx and
3461              * Rx
3462              */
3463             .eht_mcs_nss_supp = {
3464                 /*
3465                  * As B1 and B2 are set in the supported
3466                  * channel width set field in the HE PHY
3467                  * capabilities information field include all
3468                  * the following MCS/NSS.
3469                  */
3470                 .bw._80 = {
3471                     .rx_tx_mcs9_max_nss = 0x88,
3472                     .rx_tx_mcs11_max_nss = 0x88,
3473                     .rx_tx_mcs13_max_nss = 0x88,
3474                 },
3475                 .bw._160 = {
3476                     .rx_tx_mcs9_max_nss = 0x88,
3477                     .rx_tx_mcs11_max_nss = 0x88,
3478                     .rx_tx_mcs13_max_nss = 0x88,
3479                 },
3480             },
3481             /* PPE threshold information is not supported */
3482         },
3483     },
3484 #ifdef CONFIG_MAC80211_MESH
3485     {
3486         /* TODO: should we support other types, e.g., IBSS?*/
3487         .types_mask = BIT(NL80211_IFTYPE_MESH_POINT),
3488         .he_cap = {
3489             .has_he = true,
3490             .he_cap_elem = {
3491                 .mac_cap_info[0] =
3492                     IEEE80211_HE_MAC_CAP0_HTC_HE,
3493                 .mac_cap_info[1] =
3494                     IEEE80211_HE_MAC_CAP1_MULTI_TID_AGG_RX_QOS_8,
3495                 .mac_cap_info[2] =
3496                     IEEE80211_HE_MAC_CAP2_ACK_EN,
3497                 .mac_cap_info[3] =
3498                     IEEE80211_HE_MAC_CAP3_OMI_CONTROL |
3499                     IEEE80211_HE_MAC_CAP3_MAX_AMPDU_LEN_EXP_EXT_3,
3500                 .mac_cap_info[4] = IEEE80211_HE_MAC_CAP4_AMSDU_IN_AMPDU,
3501                 .phy_cap_info[0] =
3502                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_40MHZ_80MHZ_IN_5G |
3503                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_160MHZ_IN_5G |
3504                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_80PLUS80_MHZ_IN_5G,
3505                 .phy_cap_info[1] =
3506                     IEEE80211_HE_PHY_CAP1_PREAMBLE_PUNC_RX_MASK |
3507                     IEEE80211_HE_PHY_CAP1_DEVICE_CLASS_A |
3508                     IEEE80211_HE_PHY_CAP1_LDPC_CODING_IN_PAYLOAD |
3509                     IEEE80211_HE_PHY_CAP1_MIDAMBLE_RX_TX_MAX_NSTS,
3510                 .phy_cap_info[2] = 0,
3511
3512                 /* Leave all the other PHY capability bytes
3513                  * unset, as DCM, beam forming, RU and PPE
3514                  * threshold information are not supported
3515                  */
3516             },
3517             .he_mcs_nss_supp = {
3518                 .rx_mcs_80 = cpu_to_le16(0xfffa),
3519                 .tx_mcs_80 = cpu_to_le16(0xfffa),
3520                 .rx_mcs_160 = cpu_to_le16(0xfffa),
3521                 .tx_mcs_160 = cpu_to_le16(0xfffa),
3522                 .rx_mcs_80p80 = cpu_to_le16(0xfffa),
3523                 .tx_mcs_80p80 = cpu_to_le16(0xfffa),
3524             },
3525         },
3526     },
3527 #endif
3528 };
3529
3530 static const struct ieee80211_sband_iftype_data sband_capa_6ghz[] = {
3531     {
3532         /* TODO: should we support other types, e.g., P2P?*/
3533         .types_mask = BIT(NL80211_IFTYPE_STATION) |
3534                   BIT(NL80211_IFTYPE_AP),
3535         .he_6ghz_capa = {
3536             .capa = cpu_to_le16(IEEE80211_HE_6GHZ_CAP_MIN_MPDU_START |
3537                         IEEE80211_HE_6GHZ_CAP_MAX_AMPDU_LEN_EXP |
3538                         IEEE80211_HE_6GHZ_CAP_MAX_MPDU_LEN |
3539                         IEEE80211_HE_6GHZ_CAP_SM_PS |
3540                         IEEE80211_HE_6GHZ_CAP_RD_RESPONDER |
3541                         IEEE80211_HE_6GHZ_CAP_TX_ANTPAT_CONS |
3542                         IEEE80211_HE_6GHZ_CAP_RX_ANTPAT_CONS),
3543         },
3544         .he_cap = {
3545             .has_he = true,
3546             .he_cap_elem = {
3547                 .mac_cap_info[0] =
3548                     IEEE80211_HE_MAC_CAP0_HTC_HE,
3549                 .mac_cap_info[1] =
3550                     IEEE80211_HE_MAC_CAP1_TF_MAC_PAD_DUR_16US |
3551                     IEEE80211_HE_MAC_CAP1_MULTI_TID_AGG_RX_QOS_8,
3552                 .mac_cap_info[2] =
3553                     IEEE80211_HE_MAC_CAP2_BSR |
3554                     IEEE80211_HE_MAC_CAP2_MU_CASCADING |
3555                     IEEE80211_HE_MAC_CAP2_ACK_EN,
3556                 .mac_cap_info[3] =
3557                     IEEE80211_HE_MAC_CAP3_OMI_CONTROL |
3558                     IEEE80211_HE_MAC_CAP3_MAX_AMPDU_LEN_EXP_EXT_3,
3559                 .mac_cap_info[4] = IEEE80211_HE_MAC_CAP4_AMSDU_IN_AMPDU,
3560                 .phy_cap_info[0] =
3561                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_40MHZ_80MHZ_IN_5G |
3562                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_160MHZ_IN_5G |
3563                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_80PLUS80_MHZ_IN_5G,
3564                 .phy_cap_info[1] =
3565                     IEEE80211_HE_PHY_CAP1_PREAMBLE_PUNC_RX_MASK |
3566                     IEEE80211_HE_PHY_CAP1_DEVICE_CLASS_A |
3567                     IEEE80211_HE_PHY_CAP1_LDPC_CODING_IN_PAYLOAD |
3568                     IEEE80211_HE_PHY_CAP1_MIDAMBLE_RX_TX_MAX_NSTS,
3569                 .phy_cap_info[2] =
3570                     IEEE80211_HE_PHY_CAP2_NDP_4x_LTF_AND_3_2US |
3571                     IEEE80211_HE_PHY_CAP2_STBC_TX_UNDER_80MHZ |
3572                     IEEE80211_HE_PHY_CAP2_STBC_RX_UNDER_80MHZ |
3573                     IEEE80211_HE_PHY_CAP2_UL_MU_FULL_MU_MIMO |
3574                     IEEE80211_HE_PHY_CAP2_UL_MU_PARTIAL_MU_MIMO,
3575
3576                 /* Leave all the other PHY capability bytes
3577                  * unset, as DCM, beam forming, RU and PPE
3578                  * threshold information are not supported
3579                  */
3580             },
3581             .he_mcs_nss_supp = {
3582                 .rx_mcs_80 = cpu_to_le16(0xfffa),
3583                 .tx_mcs_80 = cpu_to_le16(0xfffa),
3584                 .rx_mcs_160 = cpu_to_le16(0xfffa),
3585                 .tx_mcs_160 = cpu_to_le16(0xfffa),
3586                 .rx_mcs_80p80 = cpu_to_le16(0xfffa),
3587                 .tx_mcs_80p80 = cpu_to_le16(0xfffa),
3588             },
3589         },
3590         .eht_cap = {
3591             .has_eht = true,
3592             .eht_cap_elem = {
3593                 .mac_cap_info[0] =
3594                     IEEE80211_EHT_MAC_CAP0_EPCS_PRIO_ACCESS |
3595                     IEEE80211_EHT_MAC_CAP0_OM_CONTROL |
3596                     IEEE80211_EHT_MAC_CAP0_TRIG_TXOP_SHARING_MODE1,
3597                 .phy_cap_info[0] =
3598                     IEEE80211_EHT_PHY_CAP0_320MHZ_IN_6GHZ |
3599                     IEEE80211_EHT_PHY_CAP0_242_TONE_RU_GT20MHZ |
3600                     IEEE80211_EHT_PHY_CAP0_NDP_4_EHT_LFT_32_GI |
3601                     IEEE80211_EHT_PHY_CAP0_PARTIAL_BW_UL_MU_MIMO |
3602                     IEEE80211_EHT_PHY_CAP0_SU_BEAMFORMER |
3603                     IEEE80211_EHT_PHY_CAP0_SU_BEAMFORMEE |
3604                     IEEE80211_EHT_PHY_CAP0_BEAMFORMEE_SS_80MHZ_MASK,
3605                 .phy_cap_info[1] =
3606                     IEEE80211_EHT_PHY_CAP1_BEAMFORMEE_SS_80MHZ_MASK |
3607                     IEEE80211_EHT_PHY_CAP1_BEAMFORMEE_SS_160MHZ_MASK |
3608                     IEEE80211_EHT_PHY_CAP1_BEAMFORMEE_SS_320MHZ_MASK,
3609                 .phy_cap_info[2] =
3610                     IEEE80211_EHT_PHY_CAP2_SOUNDING_DIM_80MHZ_MASK |
3611                     IEEE80211_EHT_PHY_CAP2_SOUNDING_DIM_160MHZ_MASK |
3612                     IEEE80211_EHT_PHY_CAP2_SOUNDING_DIM_320MHZ_MASK,
3613                 .phy_cap_info[3] =
3614                     IEEE80211_EHT_PHY_CAP3_NG_16_SU_FEEDBACK |
3615                     IEEE80211_EHT_PHY_CAP3_NG_16_MU_FEEDBACK |
3616                     IEEE80211_EHT_PHY_CAP3_CODEBOOK_4_2_SU_FDBK |
3617                     IEEE80211_EHT_PHY_CAP3_CODEBOOK_7_5_MU_FDBK |
3618                     IEEE80211_EHT_PHY_CAP3_TRIG_SU_BF_FDBK |
3619                     IEEE80211_EHT_PHY_CAP3_TRIG_MU_BF_PART_BW_FDBK |
3620                     IEEE80211_EHT_PHY_CAP3_TRIG_CQI_FDBK,
3621                 .phy_cap_info[4] =
3622                     IEEE80211_EHT_PHY_CAP4_PART_BW_DL_MU_MIMO |
3623                     IEEE80211_EHT_PHY_CAP4_PSR_SR_SUPP |
3624                     IEEE80211_EHT_PHY_CAP4_POWER_BOOST_FACT_SUPP |
3625                     IEEE80211_EHT_PHY_CAP4_EHT_MU_PPDU_4_EHT_LTF_08_GI |
3626                     IEEE80211_EHT_PHY_CAP4_MAX_NC_MASK,
3627                 .phy_cap_info[5] =
3628                     IEEE80211_EHT_PHY_CAP5_NON_TRIG_CQI_FEEDBACK |
3629                     IEEE80211_EHT_PHY_CAP5_TX_LESS_242_TONE_RU_SUPP |
3630                     IEEE80211_EHT_PHY_CAP5_RX_LESS_242_TONE_RU_SUPP |
3631                     IEEE80211_EHT_PHY_CAP5_PPE_THRESHOLD_PRESENT |
3632                     IEEE80211_EHT_PHY_CAP5_COMMON_NOMINAL_PKT_PAD_MASK |
3633                     IEEE80211_EHT_PHY_CAP5_MAX_NUM_SUPP_EHT_LTF_MASK,
3634                 .phy_cap_info[6] =
3635                     IEEE80211_EHT_PHY_CAP6_MAX_NUM_SUPP_EHT_LTF_MASK |
3636                     IEEE80211_EHT_PHY_CAP6_MCS15_SUPP_MASK |
3637                     IEEE80211_EHT_PHY_CAP6_EHT_DUP_6GHZ_SUPP,
3638                 .phy_cap_info[7] =
3639                     IEEE80211_EHT_PHY_CAP7_20MHZ_STA_RX_NDP_WIDER_BW |
3640                     IEEE80211_EHT_PHY_CAP7_NON_OFDMA_UL_MU_MIMO_80MHZ |
3641                     IEEE80211_EHT_PHY_CAP7_NON_OFDMA_UL_MU_MIMO_160MHZ |
3642                     IEEE80211_EHT_PHY_CAP7_NON_OFDMA_UL_MU_MIMO_320MHZ |
3643                     IEEE80211_EHT_PHY_CAP7_MU_BEAMFORMER_80MHZ |
3644                     IEEE80211_EHT_PHY_CAP7_MU_BEAMFORMER_160MHZ |
3645                     IEEE80211_EHT_PHY_CAP7_MU_BEAMFORMER_320MHZ,
3646             },
3647
3648             /* For all MCS and bandwidth, set 8 NSS for both Tx and
3649              * Rx
3650              */
3651             .eht_mcs_nss_supp = {
3652                 /*
3653                  * As B1 and B2 are set in the supported
3654                  * channel width set field in the HE PHY
3655                  * capabilities information field and 320MHz in
3656                  * 6GHz is supported include all the following
3657                  * MCS/NSS.
3658                  */
3659                 .bw._80 = {
3660                     .rx_tx_mcs9_max_nss = 0x88,
3661                     .rx_tx_mcs11_max_nss = 0x88,
3662                     .rx_tx_mcs13_max_nss = 0x88,
3663                 },
3664                 .bw._160 = {
3665                     .rx_tx_mcs9_max_nss = 0x88,
3666                     .rx_tx_mcs11_max_nss = 0x88,
3667                     .rx_tx_mcs13_max_nss = 0x88,
3668                 },
3669                 .bw._320 = {
3670                     .rx_tx_mcs9_max_nss = 0x88,
3671                     .rx_tx_mcs11_max_nss = 0x88,
3672                     .rx_tx_mcs13_max_nss = 0x88,
3673                 },
3674             },
3675             /* PPE threshold information is not supported */
3676         },
3677     },
3678 #ifdef CONFIG_MAC80211_MESH
3679     {
3680         /* TODO: should we support other types, e.g., IBSS?*/
3681         .types_mask = BIT(NL80211_IFTYPE_MESH_POINT),
3682         .he_6ghz_capa = {
3683             .capa = cpu_to_le16(IEEE80211_HE_6GHZ_CAP_MIN_MPDU_START |
3684                         IEEE80211_HE_6GHZ_CAP_MAX_AMPDU_LEN_EXP |
3685                         IEEE80211_HE_6GHZ_CAP_MAX_MPDU_LEN |
3686                         IEEE80211_HE_6GHZ_CAP_SM_PS |
3687                         IEEE80211_HE_6GHZ_CAP_RD_RESPONDER |
3688                         IEEE80211_HE_6GHZ_CAP_TX_ANTPAT_CONS |
3689                         IEEE80211_HE_6GHZ_CAP_RX_ANTPAT_CONS),
3690         },
3691         .he_cap = {
3692             .has_he = true,
3693             .he_cap_elem = {
3694                 .mac_cap_info[0] =
3695                     IEEE80211_HE_MAC_CAP0_HTC_HE,
3696                 .mac_cap_info[1] =
3697                     IEEE80211_HE_MAC_CAP1_MULTI_TID_AGG_RX_QOS_8,
3698                 .mac_cap_info[2] =
3699                     IEEE80211_HE_MAC_CAP2_ACK_EN,
3700                 .mac_cap_info[3] =
3701                     IEEE80211_HE_MAC_CAP3_OMI_CONTROL |
3702                     IEEE80211_HE_MAC_CAP3_MAX_AMPDU_LEN_EXP_EXT_3,
3703                 .mac_cap_info[4] = IEEE80211_HE_MAC_CAP4_AMSDU_IN_AMPDU,
3704                 .phy_cap_info[0] =
3705                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_40MHZ_80MHZ_IN_5G |
3706                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_160MHZ_IN_5G |
3707                     IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_80PLUS80_MHZ_IN_5G,
3708                 .phy_cap_info[1] =
3709                     IEEE80211_HE_PHY_CAP1_PREAMBLE_PUNC_RX_MASK |
3710                     IEEE80211_HE_PHY_CAP1_DEVICE_CLASS_A |
3711                     IEEE80211_HE_PHY_CAP1_LDPC_CODING_IN_PAYLOAD |
3712                     IEEE80211_HE_PHY_CAP1_MIDAMBLE_RX_TX_MAX_NSTS,
3713                 .phy_cap_info[2] = 0,
3714
3715                 /* Leave all the other PHY capability bytes
3716                  * unset, as DCM, beam forming, RU and PPE
3717                  * threshold information are not supported
3718                  */
3719             },
3720             .he_mcs_nss_supp = {
3721                 .rx_mcs_80 = cpu_to_le16(0xfffa),
3722                 .tx_mcs_80 = cpu_to_le16(0xfffa),
3723                 .rx_mcs_160 = cpu_to_le16(0xfffa),
3724                 .tx_mcs_160 = cpu_to_le16(0xfffa),
3725                 .rx_mcs_80p80 = cpu_to_le16(0xfffa),
3726                 .tx_mcs_80p80 = cpu_to_le16(0xfffa),
3727             },
3728         },
3729     },
3730 #endif
3731 };
3732
3733 static void mac80211_hwsim_sband_capab(struct ieee80211_supported_band *sband)
3734 {
3735     u16 n_iftype_data;
3736
3737     if (sband->band == NL80211_BAND_2GHZ) {
3738         n_iftype_data = ARRAY_SIZE(sband_capa_2ghz);
3739         sband->iftype_data =
3740             (struct ieee80211_sband_iftype_data *)sband_capa_2ghz;
3741     } else if (sband->band == NL80211_BAND_5GHZ) {
3742         n_iftype_data = ARRAY_SIZE(sband_capa_5ghz);
3743         sband->iftype_data =
3744             (struct ieee80211_sband_iftype_data *)sband_capa_5ghz;
3745     } else if (sband->band == NL80211_BAND_6GHZ) {
3746         n_iftype_data = ARRAY_SIZE(sband_capa_6ghz);
3747         sband->iftype_data =
3748             (struct ieee80211_sband_iftype_data *)sband_capa_6ghz;
3749     } else {
3750         return;
3751     }
3752
3753     sband->n_iftype_data = n_iftype_data;
3754 }
3755
3756 #ifdef CONFIG_MAC80211_MESH
3757 #define HWSIM_MESH_BIT BIT(NL80211_IFTYPE_MESH_POINT)
3758 #else
3759 #define HWSIM_MESH_BIT 0
3760 #endif
3761
3762 #define HWSIM_DEFAULT_IF_LIMIT \
3763     (BIT(NL80211_IFTYPE_STATION) | \
3764      BIT(NL80211_IFTYPE_P2P_CLIENT) | \
3765      BIT(NL80211_IFTYPE_AP) | \
3766      BIT(NL80211_IFTYPE_P2P_GO) | \
3767      HWSIM_MESH_BIT)
3768
3769 #define HWSIM_IFTYPE_SUPPORT_MASK \
3770     (BIT(NL80211_IFTYPE_STATION) | \
3771      BIT(NL80211_IFTYPE_AP) | \
3772      BIT(NL80211_IFTYPE_P2P_CLIENT) | \
3773      BIT(NL80211_IFTYPE_P2P_GO) | \
3774      BIT(NL80211_IFTYPE_ADHOC) | \
3775      BIT(NL80211_IFTYPE_MESH_POINT) | \
3776      BIT(NL80211_IFTYPE_OCB))
3777
3778 static int mac80211_hwsim_new_radio(struct genl_info *info,
3779                     struct hwsim_new_radio_params *param)
3780 {
3781     int err;
3782     u8 addr[ETH_ALEN];
3783     struct mac80211_hwsim_data *data;
3784     struct ieee80211_hw *hw;
3785     enum nl80211_band band;
3786     const struct ieee80211_ops *ops = &mac80211_hwsim_ops;
3787     struct net *net;
3788     int idx, i;
3789     int n_limits = 0;
3790
3791     if (WARN_ON(param->channels > 1 && !param->use_chanctx))
3792         return -EINVAL;
3793
3794     spin_lock_bh(&hwsim_radio_lock);
3795     idx = hwsim_radio_idx++;
3796     spin_unlock_bh(&hwsim_radio_lock);
3797
3798     if (param->mlo)
3799         ops = &mac80211_hwsim_mlo_ops;
3800     else if (param->use_chanctx)
3801         ops = &mac80211_hwsim_mchan_ops;
3802     hw = ieee80211_alloc_hw_nm(sizeof(*data), ops, param->hwname);
3803     if (!hw) {
3804         pr_debug("mac80211_hwsim: ieee80211_alloc_hw failed\n");
3805         err = -ENOMEM;
3806         goto failed;
3807     }
3808
3809     /* ieee80211_alloc_hw_nm may have used a default name */
3810     param->hwname = wiphy_name(hw->wiphy);
3811
3812     if (info)
3813         net = genl_info_net(info);
3814     else
3815         net = &init_net;
3816     wiphy_net_set(hw->wiphy, net);
3817
3818     data = hw->priv;
3819     data->hw = hw;
3820
3821     data->dev = device_create(hwsim_class, NULL, 0, hw, "hwsim%d", idx);
3822     if (IS_ERR(data->dev)) {
3823         printk(KERN_DEBUG
3824                "mac80211_hwsim: device_create failed (%ld)\n",
3825                PTR_ERR(data->dev));
3826         err = -ENOMEM;
3827         goto failed_drvdata;
3828     }
3829     data->dev->driver = &mac80211_hwsim_driver.driver;
3830     err = device_bind_driver(data->dev);
3831     if (err != 0) {
3832         pr_debug("mac80211_hwsim: device_bind_driver failed (%d)\n",
3833                err);
3834         goto failed_bind;
3835     }
3836
3837     skb_queue_head_init(&data->pending);
3838
3839     SET_IEEE80211_DEV(hw, data->dev);
3840     if (!param->perm_addr) {
3841         eth_zero_addr(addr);
3842         addr[0] = 0x02;
3843         addr[3] = idx >> 8;
3844         addr[4] = idx;
3845         memcpy(data->addresses[0].addr, addr, ETH_ALEN);
3846         /* Why need here second address ? */
3847         memcpy(data->addresses[1].addr, addr, ETH_ALEN);
3848         data->addresses[1].addr[0] |= 0x40;
3849         hw->wiphy->n_addresses = 2;
3850         hw->wiphy->addresses = data->addresses;
3851         /* possible address clash is checked at hash table insertion */
3852     } else {
3853         memcpy(data->addresses[0].addr, param->perm_addr, ETH_ALEN);
3854         /* compatibility with automatically generated mac addr */
3855         memcpy(data->addresses[1].addr, param->perm_addr, ETH_ALEN);
3856         hw->wiphy->n_addresses = 2;
3857         hw->wiphy->addresses = data->addresses;
3858     }
3859
3860     data->channels = param->channels;
3861     data->use_chanctx = param->use_chanctx;
3862     data->idx = idx;
3863     data->destroy_on_close = param->destroy_on_close;
3864     if (info)
3865         data->portid = info->snd_portid;
3866
3867     /* setup interface limits, only on interface types we support */
3868     if (param->iftypes & BIT(NL80211_IFTYPE_ADHOC)) {
3869         data->if_limits[n_limits].max = 1;
3870         data->if_limits[n_limits].types = BIT(NL80211_IFTYPE_ADHOC);
3871         n_limits++;
3872     }
3873
3874     if (param->iftypes & HWSIM_DEFAULT_IF_LIMIT) {
3875         data->if_limits[n_limits].max = 2048;
3876         /*
3877          * For this case, we may only support a subset of
3878          * HWSIM_DEFAULT_IF_LIMIT, therefore we only want to add the
3879          * bits that both param->iftype & HWSIM_DEFAULT_IF_LIMIT have.
3880          */
3881         data->if_limits[n_limits].types =
3882                     HWSIM_DEFAULT_IF_LIMIT & param->iftypes;
3883         n_limits++;
3884     }
3885
3886     if (param->iftypes & BIT(NL80211_IFTYPE_P2P_DEVICE)) {
3887         data->if_limits[n_limits].max = 1;
3888         data->if_limits[n_limits].types =
3889                         BIT(NL80211_IFTYPE_P2P_DEVICE);
3890         n_limits++;
3891     }
3892
3893     if (data->use_chanctx) {
3894         hw->wiphy->max_scan_ssids = 255;
3895         hw->wiphy->max_scan_ie_len = IEEE80211_MAX_DATA_LEN;
3896         hw->wiphy->max_remain_on_channel_duration = 1000;
3897         data->if_combination.radar_detect_widths = 0;
3898         data->if_combination.num_different_channels = data->channels;
3899         data->chanctx = NULL;
3900     } else {
3901         data->if_combination.num_different_channels = 1;
3902         data->if_combination.radar_detect_widths =
3903                     BIT(NL80211_CHAN_WIDTH_5) |
3904                     BIT(NL80211_CHAN_WIDTH_10) |
3905                     BIT(NL80211_CHAN_WIDTH_20_NOHT) |
3906                     BIT(NL80211_CHAN_WIDTH_20) |
3907                     BIT(NL80211_CHAN_WIDTH_40) |
3908                     BIT(NL80211_CHAN_WIDTH_80) |
3909                     BIT(NL80211_CHAN_WIDTH_160);
3910     }
3911
3912     if (!n_limits) {
3913         err = -EINVAL;
3914         goto failed_hw;
3915     }
3916
3917     data->if_combination.max_interfaces = 0;
3918     for (i = 0; i < n_limits; i++)
3919         data->if_combination.max_interfaces +=
3920             data->if_limits[i].max;
3921
3922     data->if_combination.n_limits = n_limits;
3923     data->if_combination.limits = data->if_limits;
3924
3925     /*
3926      * If we actually were asked to support combinations,
3927      * advertise them - if there's only a single thing like
3928      * only IBSS then don't advertise it as combinations.
3929      */
3930     if (data->if_combination.max_interfaces > 1) {
3931         hw->wiphy->iface_combinations = &data->if_combination;
3932         hw->wiphy->n_iface_combinations = 1;
3933     }
3934
3935     if (param->ciphers) {
3936         memcpy(data->ciphers, param->ciphers,
3937                param->n_ciphers * sizeof(u32));
3938         hw->wiphy->cipher_suites = data->ciphers;
3939         hw->wiphy->n_cipher_suites = param->n_ciphers;
3940     }
3941
3942     data->rx_rssi = DEFAULT_RX_RSSI;
3943
3944     INIT_DELAYED_WORK(&data->roc_start, hw_roc_start);
3945     INIT_DELAYED_WORK(&data->roc_done, hw_roc_done);
3946     INIT_DELAYED_WORK(&data->hw_scan, hw_scan_work);
3947
3948     hw->queues = 5;
3949     hw->offchannel_tx_hw_queue = 4;
3950
3951     ieee80211_hw_set(hw, SUPPORT_FAST_XMIT);
3952     ieee80211_hw_set(hw, CHANCTX_STA_CSA);
3953     ieee80211_hw_set(hw, SUPPORTS_HT_CCK_RATES);
3954     ieee80211_hw_set(hw, QUEUE_CONTROL);
3955     ieee80211_hw_set(hw, WANT_MONITOR_VIF);
3956     ieee80211_hw_set(hw, AMPDU_AGGREGATION);
3957     ieee80211_hw_set(hw, MFP_CAPABLE);
3958     ieee80211_hw_set(hw, SIGNAL_DBM);
3959     ieee80211_hw_set(hw, SUPPORTS_PS);
3960     ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
3961     ieee80211_hw_set(hw, TDLS_WIDER_BW);
3962     ieee80211_hw_set(hw, SUPPORTS_MULTI_BSSID);
3963
3964     if (param->mlo) {
3965         hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_MLO;
3966         ieee80211_hw_set(hw, HAS_RATE_CONTROL);
3967         ieee80211_hw_set(hw, SUPPORTS_DYNAMIC_PS);
3968         ieee80211_hw_set(hw, CONNECTION_MONITOR);
3969         ieee80211_hw_set(hw, AP_LINK_PS);
3970     } else {
3971         ieee80211_hw_set(hw, HOST_BROADCAST_PS_BUFFERING);
3972         ieee80211_hw_set(hw, PS_NULLFUNC_STACK);
3973         if (rctbl)
3974             ieee80211_hw_set(hw, SUPPORTS_RC_TABLE);
3975     }
3976
3977     hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
3978     hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS |
3979                 WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL |
3980                 WIPHY_FLAG_AP_UAPSD |
3981                 WIPHY_FLAG_SUPPORTS_5_10_MHZ |
3982                 WIPHY_FLAG_HAS_CHANNEL_SWITCH;
3983     hw->wiphy->features |= NL80211_FEATURE_ACTIVE_MONITOR |
3984                    NL80211_FEATURE_AP_MODE_CHAN_WIDTH_CHANGE |
3985                    NL80211_FEATURE_STATIC_SMPS |
3986                    NL80211_FEATURE_DYNAMIC_SMPS |
3987                    NL80211_FEATURE_SCAN_RANDOM_MAC_ADDR;
3988     wiphy_ext_feature_set(hw->wiphy, NL80211_EXT_FEATURE_VHT_IBSS);
3989     wiphy_ext_feature_set(hw->wiphy, NL80211_EXT_FEATURE_BEACON_PROTECTION);
3990     wiphy_ext_feature_set(hw->wiphy,
3991                   NL80211_EXT_FEATURE_MULTICAST_REGISTRATIONS);
3992     wiphy_ext_feature_set(hw->wiphy,
3993                   NL80211_EXT_FEATURE_BEACON_RATE_LEGACY);
3994
3995     hw->wiphy->interface_modes = param->iftypes;
3996
3997     /* ask mac80211 to reserve space for magic */
3998     hw->vif_data_size = sizeof(struct hwsim_vif_priv);
3999     hw->sta_data_size = sizeof(struct hwsim_sta_priv);
4000     hw->chanctx_data_size = sizeof(struct hwsim_chanctx_priv);
4001
4002     memcpy(data->channels_2ghz, hwsim_channels_2ghz,
4003         sizeof(hwsim_channels_2ghz));
4004     memcpy(data->channels_5ghz, hwsim_channels_5ghz,
4005         sizeof(hwsim_channels_5ghz));
4006     memcpy(data->channels_6ghz, hwsim_channels_6ghz,
4007         sizeof(hwsim_channels_6ghz));
4008     memcpy(data->channels_s1g, hwsim_channels_s1g,
4009            sizeof(hwsim_channels_s1g));
4010     memcpy(data->rates, hwsim_rates, sizeof(hwsim_rates));
4011
4012     for (band = NL80211_BAND_2GHZ; band < NUM_NL80211_BANDS; band++) {
4013         struct ieee80211_supported_band *sband = &data->bands[band];
4014
4015         sband->band = band;
4016
4017         switch (band) {
4018         case NL80211_BAND_2GHZ:
4019             sband->channels = data->channels_2ghz;
4020             sband->n_channels = ARRAY_SIZE(hwsim_channels_2ghz);
4021             sband->bitrates = data->rates;
4022             sband->n_bitrates = ARRAY_SIZE(hwsim_rates);
4023             break;
4024         case NL80211_BAND_5GHZ:
4025             sband->channels = data->channels_5ghz;
4026             sband->n_channels = ARRAY_SIZE(hwsim_channels_5ghz);
4027             sband->bitrates = data->rates + 4;
4028             sband->n_bitrates = ARRAY_SIZE(hwsim_rates) - 4;
4029
4030             sband->vht_cap.vht_supported = true;
4031             sband->vht_cap.cap =
4032                 IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454 |
4033                 IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160_80PLUS80MHZ |
4034                 IEEE80211_VHT_CAP_RXLDPC |
4035                 IEEE80211_VHT_CAP_SHORT_GI_80 |
4036                 IEEE80211_VHT_CAP_SHORT_GI_160 |
4037                 IEEE80211_VHT_CAP_TXSTBC |
4038                 IEEE80211_VHT_CAP_RXSTBC_4 |
4039                 IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK;
4040             sband->vht_cap.vht_mcs.rx_mcs_map =
4041                 cpu_to_le16(IEEE80211_VHT_MCS_SUPPORT_0_9 << 0 |
4042                         IEEE80211_VHT_MCS_SUPPORT_0_9 << 2 |
4043                         IEEE80211_VHT_MCS_SUPPORT_0_9 << 4 |
4044                         IEEE80211_VHT_MCS_SUPPORT_0_9 << 6 |
4045                         IEEE80211_VHT_MCS_SUPPORT_0_9 << 8 |
4046                         IEEE80211_VHT_MCS_SUPPORT_0_9 << 10 |
4047                         IEEE80211_VHT_MCS_SUPPORT_0_9 << 12 |
4048                         IEEE80211_VHT_MCS_SUPPORT_0_9 << 14);
4049             sband->vht_cap.vht_mcs.tx_mcs_map =
4050                 sband->vht_cap.vht_mcs.rx_mcs_map;
4051             break;
4052         case NL80211_BAND_6GHZ:
4053             sband->channels = data->channels_6ghz;
4054             sband->n_channels = ARRAY_SIZE(hwsim_channels_6ghz);
4055             sband->bitrates = data->rates + 4;
4056             sband->n_bitrates = ARRAY_SIZE(hwsim_rates) - 4;
4057             break;
4058         case NL80211_BAND_S1GHZ:
4059             memcpy(&sband->s1g_cap, &hwsim_s1g_cap,
4060                    sizeof(sband->s1g_cap));
4061             sband->channels = data->channels_s1g;
4062             sband->n_channels = ARRAY_SIZE(hwsim_channels_s1g);
4063             break;
4064         default:
4065             continue;
4066         }
4067
4068         if (band != NL80211_BAND_6GHZ){
4069             sband->ht_cap.ht_supported = true;
4070             sband->ht_cap.cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
4071                         IEEE80211_HT_CAP_GRN_FLD |
4072                         IEEE80211_HT_CAP_SGI_20 |
4073                         IEEE80211_HT_CAP_SGI_40 |
4074                         IEEE80211_HT_CAP_DSSSCCK40;
4075             sband->ht_cap.ampdu_factor = 0x3;
4076             sband->ht_cap.ampdu_density = 0x6;
4077             memset(&sband->ht_cap.mcs, 0,
4078                    sizeof(sband->ht_cap.mcs));
4079             sband->ht_cap.mcs.rx_mask[0] = 0xff;
4080             sband->ht_cap.mcs.rx_mask[1] = 0xff;
4081             sband->ht_cap.mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
4082         }
4083
4084         mac80211_hwsim_sband_capab(sband);
4085
4086         hw->wiphy->bands[band] = sband;
4087     }
4088
4089     /* By default all radios belong to the first group */
4090     data->group = 1;
4091     mutex_init(&data->mutex);
4092
4093     data->netgroup = hwsim_net_get_netgroup(net);
4094     data->wmediumd = hwsim_net_get_wmediumd(net);
4095
4096     /* Enable frame retransmissions for lossy channels */
4097     hw->max_rates = 4;
4098     hw->max_rate_tries = 11;
4099
4100     hw->wiphy->vendor_commands = mac80211_hwsim_vendor_commands;
4101     hw->wiphy->n_vendor_commands =
4102         ARRAY_SIZE(mac80211_hwsim_vendor_commands);
4103     hw->wiphy->vendor_events = mac80211_hwsim_vendor_events;
4104     hw->wiphy->n_vendor_events = ARRAY_SIZE(mac80211_hwsim_vendor_events);
4105
4106     if (param->reg_strict)
4107         hw->wiphy->regulatory_flags |= REGULATORY_STRICT_REG;
4108     if (param->regd) {
4109         data->regd = param->regd;
4110         hw->wiphy->regulatory_flags |= REGULATORY_CUSTOM_REG;
4111         wiphy_apply_custom_regulatory(hw->wiphy, param->regd);
4112         /* give the regulatory workqueue a chance to run */
4113         schedule_timeout_interruptible(1);
4114     }
4115
4116     if (param->no_vif)
4117         ieee80211_hw_set(hw, NO_AUTO_VIF);
4118
4119     wiphy_ext_feature_set(hw->wiphy, NL80211_EXT_FEATURE_CQM_RSSI_LIST);
4120
4121     for (i = 0; i < ARRAY_SIZE(data->link_data); i++) {
4122         hrtimer_init(&data->link_data[i].beacon_timer, CLOCK_MONOTONIC,
4123                  HRTIMER_MODE_ABS_SOFT);
4124         data->link_data[i].beacon_timer.function =
4125             mac80211_hwsim_beacon;
4126         data->link_data[i].link_id = i;
4127     }
4128
4129     err = ieee80211_register_hw(hw);
4130     if (err < 0) {
4131         pr_debug("mac80211_hwsim: ieee80211_register_hw failed (%d)\n",
4132                err);
4133         goto failed_hw;
4134     }
4135
4136     wiphy_dbg(hw->wiphy, "hwaddr %pM registered\n", hw->wiphy->perm_addr);
4137
4138     if (param->reg_alpha2) {
4139         data->alpha2[0] = param->reg_alpha2[0];
4140         data->alpha2[1] = param->reg_alpha2[1];
4141         regulatory_hint(hw->wiphy, param->reg_alpha2);
4142     }
4143
4144     data->debugfs = debugfs_create_dir("hwsim", hw->wiphy->debugfsdir);
4145     debugfs_create_file("ps", 0666, data->debugfs, data, &hwsim_fops_ps);
4146     debugfs_create_file("group", 0666, data->debugfs, data,
4147                 &hwsim_fops_group);
4148     debugfs_create_file("rx_rssi", 0666, data->debugfs, data,
4149                 &hwsim_fops_rx_rssi);
4150     if (!data->use_chanctx)
4151         debugfs_create_file("dfs_simulate_radar", 0222,
4152                     data->debugfs,
4153                     data, &hwsim_simulate_radar);
4154
4155     spin_lock_bh(&hwsim_radio_lock);
4156     err = rhashtable_insert_fast(&hwsim_radios_rht, &data->rht,
4157                      hwsim_rht_params);
4158     if (err < 0) {
4159         if (info) {
4160             GENL_SET_ERR_MSG(info, "perm addr already present");
4161             NL_SET_BAD_ATTR(info->extack,
4162                     info->attrs[HWSIM_ATTR_PERM_ADDR]);
4163         }
4164         spin_unlock_bh(&hwsim_radio_lock);
4165         goto failed_final_insert;
4166     }
4167
4168     list_add_tail(&data->list, &hwsim_radios);
4169     hwsim_radios_generation++;
4170     spin_unlock_bh(&hwsim_radio_lock);
4171
4172     hwsim_mcast_new_radio(idx, info, param);
4173
4174     return idx;
4175
4176 failed_final_insert:
4177     debugfs_remove_recursive(data->debugfs);
4178     ieee80211_unregister_hw(data->hw);
4179 failed_hw:
4180     device_release_driver(data->dev);
4181 failed_bind:
4182     device_unregister(data->dev);
4183 failed_drvdata:
4184     ieee80211_free_hw(hw);
4185 failed:
4186     return err;
4187 }
4188
4189 static void hwsim_mcast_del_radio(int id, const char *hwname,
4190                   struct genl_info *info)
4191 {
4192     struct sk_buff *skb;
4193     void *data;
4194     int ret;
4195
4196     skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
4197     if (!skb)
4198         return;
4199
4200     data = genlmsg_put(skb, 0, 0, &hwsim_genl_family, 0,
4201                HWSIM_CMD_DEL_RADIO);
4202     if (!data)
4203         goto error;
4204
4205     ret = nla_put_u32(skb, HWSIM_ATTR_RADIO_ID, id);
4206     if (ret < 0)
4207         goto error;
4208
4209     ret = nla_put(skb, HWSIM_ATTR_RADIO_NAME, strlen(hwname),
4210               hwname);
4211     if (ret < 0)
4212         goto error;
4213
4214     genlmsg_end(skb, data);
4215
4216     hwsim_mcast_config_msg(skb, info);
4217
4218     return;
4219
4220 error:
4221     nlmsg_free(skb);
4222 }
4223
4224 static void mac80211_hwsim_del_radio(struct mac80211_hwsim_data *data,
4225                      const char *hwname,
4226                      struct genl_info *info)
4227 {
4228     hwsim_mcast_del_radio(data->idx, hwname, info);
4229     debugfs_remove_recursive(data->debugfs);
4230     ieee80211_unregister_hw(data->hw);
4231     device_release_driver(data->dev);
4232     device_unregister(data->dev);
4233     ieee80211_free_hw(data->hw);
4234 }
4235
4236 static int mac80211_hwsim_get_radio(struct sk_buff *skb,
4237                     struct mac80211_hwsim_data *data,
4238                     u32 portid, u32 seq,
4239                     struct netlink_callback *cb, int flags)
4240 {
4241     void *hdr;
4242     struct hwsim_new_radio_params param = { };
4243     int res = -EMSGSIZE;
4244
4245     hdr = genlmsg_put(skb, portid, seq, &hwsim_genl_family, flags,
4246               HWSIM_CMD_GET_RADIO);
4247     if (!hdr)
4248         return -EMSGSIZE;
4249
4250     if (cb)
4251         genl_dump_check_consistent(cb, hdr);
4252
4253     if (data->alpha2[0] && data->alpha2[1])
4254         param.reg_alpha2 = data->alpha2;
4255
4256     param.reg_strict = !!(data->hw->wiphy->regulatory_flags &
4257                     REGULATORY_STRICT_REG);
4258     param.p2p_device = !!(data->hw->wiphy->interface_modes &
4259                     BIT(NL80211_IFTYPE_P2P_DEVICE));
4260     param.use_chanctx = data->use_chanctx;
4261     param.regd = data->regd;
4262     param.channels = data->channels;
4263     param.hwname = wiphy_name(data->hw->wiphy);
4264
4265     res = append_radio_msg(skb, data->idx, &param);
4266     if (res < 0)
4267         goto out_err;
4268
4269     genlmsg_end(skb, hdr);
4270     return 0;
4271
4272 out_err:
4273     genlmsg_cancel(skb, hdr);
4274     return res;
4275 }
4276
4277 static void mac80211_hwsim_free(void)
4278 {
4279     struct mac80211_hwsim_data *data;
4280
4281     spin_lock_bh(&hwsim_radio_lock);
4282     while ((data = list_first_entry_or_null(&hwsim_radios,
4283                         struct mac80211_hwsim_data,
4284                         list))) {
4285         list_del(&data->list);
4286         spin_unlock_bh(&hwsim_radio_lock);
4287         mac80211_hwsim_del_radio(data, wiphy_name(data->hw->wiphy),
4288                      NULL);
4289         spin_lock_bh(&hwsim_radio_lock);
4290     }
4291     spin_unlock_bh(&hwsim_radio_lock);
4292     class_destroy(hwsim_class);
4293 }
4294
4295 static const struct net_device_ops hwsim_netdev_ops = {
4296     .ndo_start_xmit     = hwsim_mon_xmit,
4297     .ndo_set_mac_address    = eth_mac_addr,
4298     .ndo_validate_addr  = eth_validate_addr,
4299 };
4300
4301 static void hwsim_mon_setup(struct net_device *dev)
4302 {
4303     u8 addr[ETH_ALEN];
4304
4305     dev->netdev_ops = &hwsim_netdev_ops;
4306     dev->needs_free_netdev = true;
4307     ether_setup(dev);
4308     dev->priv_flags |= IFF_NO_QUEUE;
4309     dev->type = ARPHRD_IEEE80211_RADIOTAP;
4310     eth_zero_addr(addr);
4311     addr[0] = 0x12;
4312     eth_hw_addr_set(dev, addr);
4313 }
4314
4315 static struct mac80211_hwsim_data *get_hwsim_data_ref_from_addr(const u8 *addr)
4316 {
4317     return rhashtable_lookup_fast(&hwsim_radios_rht,
4318                       addr,
4319                       hwsim_rht_params);
4320 }
4321
4322 static void hwsim_register_wmediumd(struct net *net, u32 portid)
4323 {
4324     struct mac80211_hwsim_data *data;
4325
4326     hwsim_net_set_wmediumd(net, portid);
4327
4328     spin_lock_bh(&hwsim_radio_lock);
4329     list_for_each_entry(data, &hwsim_radios, list) {
4330         if (data->netgroup == hwsim_net_get_netgroup(net))
4331             data->wmediumd = portid;
4332     }
4333     spin_unlock_bh(&hwsim_radio_lock);
4334 }
4335
4336 static int hwsim_tx_info_frame_received_nl(struct sk_buff *skb_2,
4337                        struct genl_info *info)
4338 {
4339
4340     struct ieee80211_hdr *hdr;
4341     struct mac80211_hwsim_data *data2;
4342     struct ieee80211_tx_info *txi;
4343     struct hwsim_tx_rate *tx_attempts;
4344     u64 ret_skb_cookie;
4345     struct sk_buff *skb, *tmp;
4346     const u8 *src;
4347     unsigned int hwsim_flags;
4348     int i;
4349     unsigned long flags;
4350     bool found = false;
4351
4352     if (!info->attrs[HWSIM_ATTR_ADDR_TRANSMITTER] ||
4353         !info->attrs[HWSIM_ATTR_FLAGS] ||
4354         !info->attrs[HWSIM_ATTR_COOKIE] ||
4355         !info->attrs[HWSIM_ATTR_SIGNAL] ||
4356         !info->attrs[HWSIM_ATTR_TX_INFO])
4357         goto out;
4358
4359     src = (void *)nla_data(info->attrs[HWSIM_ATTR_ADDR_TRANSMITTER]);
4360     hwsim_flags = nla_get_u32(info->attrs[HWSIM_ATTR_FLAGS]);
4361     ret_skb_cookie = nla_get_u64(info->attrs[HWSIM_ATTR_COOKIE]);
4362
4363     data2 = get_hwsim_data_ref_from_addr(src);
4364     if (!data2)
4365         goto out;
4366
4367     if (!hwsim_virtio_enabled) {
4368         if (hwsim_net_get_netgroup(genl_info_net(info)) !=
4369             data2->netgroup)
4370             goto out;
4371
4372         if (info->snd_portid != data2->wmediumd)
4373             goto out;
4374     }
4375
4376     /* look for the skb matching the cookie passed back from user */
4377     spin_lock_irqsave(&data2->pending.lock, flags);
4378     skb_queue_walk_safe(&data2->pending, skb, tmp) {
4379         uintptr_t skb_cookie;
4380
4381         txi = IEEE80211_SKB_CB(skb);
4382         skb_cookie = (uintptr_t)txi->rate_driver_data[0];
4383
4384         if (skb_cookie == ret_skb_cookie) {
4385             __skb_unlink(skb, &data2->pending);
4386             found = true;
4387             break;
4388         }
4389     }
4390     spin_unlock_irqrestore(&data2->pending.lock, flags);
4391
4392     /* not found */
4393     if (!found)
4394         goto out;
4395
4396     /* Tx info received because the frame was broadcasted on user space,
4397      so we get all the necessary info: tx attempts and skb control buff */
4398
4399     tx_attempts = (struct hwsim_tx_rate *)nla_data(
4400                info->attrs[HWSIM_ATTR_TX_INFO]);
4401
4402     /* now send back TX status */
4403     txi = IEEE80211_SKB_CB(skb);
4404
4405     ieee80211_tx_info_clear_status(txi);
4406
4407     for (i = 0; i < IEEE80211_TX_MAX_RATES; i++) {
4408         txi->status.rates[i].idx = tx_attempts[i].idx;
4409         txi->status.rates[i].count = tx_attempts[i].count;
4410     }
4411
4412     txi->status.ack_signal = nla_get_u32(info->attrs[HWSIM_ATTR_SIGNAL]);
4413
4414     if (!(hwsim_flags & HWSIM_TX_CTL_NO_ACK) &&
4415        (hwsim_flags & HWSIM_TX_STAT_ACK)) {
4416         if (skb->len >= 16) {
4417             hdr = (struct ieee80211_hdr *) skb->data;
4418             mac80211_hwsim_monitor_ack(data2->channel,
4419                            hdr->addr2);
4420         }
4421         txi->flags |= IEEE80211_TX_STAT_ACK;
4422     }
4423
4424     if (hwsim_flags & HWSIM_TX_CTL_NO_ACK)
4425         txi->flags |= IEEE80211_TX_STAT_NOACK_TRANSMITTED;
4426
4427     ieee80211_tx_status_irqsafe(data2->hw, skb);
4428     return 0;
4429 out:
4430     return -EINVAL;
4431
4432 }
4433
4434 static int hwsim_cloned_frame_received_nl(struct sk_buff *skb_2,
4435                       struct genl_info *info)
4436 {
4437     struct mac80211_hwsim_data *data2;
4438     struct ieee80211_rx_status rx_status;
4439     struct ieee80211_hdr *hdr;
4440     const u8 *dst;
4441     int frame_data_len;
4442     void *frame_data;
4443     struct sk_buff *skb = NULL;
4444     struct ieee80211_channel *channel = NULL;
4445
4446     if (!info->attrs[HWSIM_ATTR_ADDR_RECEIVER] ||
4447         !info->attrs[HWSIM_ATTR_FRAME] ||
4448         !info->attrs[HWSIM_ATTR_RX_RATE] ||
4449         !info->attrs[HWSIM_ATTR_SIGNAL])
4450         goto out;
4451
4452     dst = (void *)nla_data(info->attrs[HWSIM_ATTR_ADDR_RECEIVER]);
4453     frame_data_len = nla_len(info->attrs[HWSIM_ATTR_FRAME]);
4454     frame_data = (void *)nla_data(info->attrs[HWSIM_ATTR_FRAME]);
4455
4456     /* Allocate new skb here */
4457     skb = alloc_skb(frame_data_len, GFP_KERNEL);
4458     if (skb == NULL)
4459         goto err;
4460
4461     if (frame_data_len > IEEE80211_MAX_DATA_LEN)
4462         goto err;
4463
4464     /* Copy the data */
4465     skb_put_data(skb, frame_data, frame_data_len);
4466
4467     data2 = get_hwsim_data_ref_from_addr(dst);
4468     if (!data2)
4469         goto out;
4470
4471     if (data2->use_chanctx) {
4472         if (data2->tmp_chan)
4473             channel = data2->tmp_chan;
4474         else if (data2->chanctx)
4475             channel = data2->chanctx->def.chan;
4476     } else {
4477         channel = data2->channel;
4478     }
4479     if (!channel)
4480         goto out;
4481
4482     if (!hwsim_virtio_enabled) {
4483         if (hwsim_net_get_netgroup(genl_info_net(info)) !=
4484             data2->netgroup)
4485             goto out;
4486
4487         if (info->snd_portid != data2->wmediumd)
4488             goto out;
4489     }
4490
4491     /* check if radio is configured properly */
4492
4493     if ((data2->idle && !data2->tmp_chan) || !data2->started)
4494         goto out;
4495
4496     /* A frame is received from user space */
4497     memset(&rx_status, 0, sizeof(rx_status));
4498     if (info->attrs[HWSIM_ATTR_FREQ]) {
4499         struct tx_iter_data iter_data = {};
4500
4501         /* throw away off-channel packets, but allow both the temporary
4502          * ("hw" scan/remain-on-channel), regular channels and links,
4503          * since the internal datapath also allows this
4504          */
4505         rx_status.freq = nla_get_u32(info->attrs[HWSIM_ATTR_FREQ]);
4506
4507         iter_data.channel = ieee80211_get_channel(data2->hw->wiphy,
4508                               rx_status.freq);
4509         if (!iter_data.channel)
4510             goto out;
4511
4512         mutex_lock(&data2->mutex);
4513         if (!hwsim_chans_compat(iter_data.channel, channel)) {
4514             ieee80211_iterate_active_interfaces_atomic(
4515                 data2->hw, IEEE80211_IFACE_ITER_NORMAL,
4516                 mac80211_hwsim_tx_iter, &iter_data);
4517             if (!iter_data.receive) {
4518                 mutex_unlock(&data2->mutex);
4519                 goto out;
4520             }
4521         }
4522         mutex_unlock(&data2->mutex);
4523     } else {
4524         rx_status.freq = channel->center_freq;
4525     }
4526
4527     rx_status.band = channel->band;
4528     rx_status.rate_idx = nla_get_u32(info->attrs[HWSIM_ATTR_RX_RATE]);
4529     rx_status.signal = nla_get_u32(info->attrs[HWSIM_ATTR_SIGNAL]);
4530
4531     hdr = (void *)skb->data;
4532
4533     if (ieee80211_is_beacon(hdr->frame_control) ||
4534         ieee80211_is_probe_resp(hdr->frame_control))
4535         rx_status.boottime_ns = ktime_get_boottime_ns();
4536
4537     memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
4538     data2->rx_pkts++;
4539     data2->rx_bytes += skb->len;
4540     ieee80211_rx_irqsafe(data2->hw, skb);
4541
4542     return 0;
4543 err:
4544     pr_debug("mac80211_hwsim: error occurred in %s\n", __func__);
4545 out:
4546     dev_kfree_skb(skb);
4547     return -EINVAL;
4548 }
4549
4550 static int hwsim_register_received_nl(struct sk_buff *skb_2,
4551                       struct genl_info *info)
4552 {
4553     struct net *net = genl_info_net(info);
4554     struct mac80211_hwsim_data *data;
4555     int chans = 1;
4556
4557     spin_lock_bh(&hwsim_radio_lock);
4558     list_for_each_entry(data, &hwsim_radios, list)
4559         chans = max(chans, data->channels);
4560     spin_unlock_bh(&hwsim_radio_lock);
4561
4562     /* In the future we should revise the userspace API and allow it
4563      * to set a flag that it does support multi-channel, then we can
4564      * let this pass conditionally on the flag.
4565      * For current userspace, prohibit it since it won't work right.
4566      */
4567     if (chans > 1)
4568         return -EOPNOTSUPP;
4569
4570     if (hwsim_net_get_wmediumd(net))
4571         return -EBUSY;
4572
4573     hwsim_register_wmediumd(net, info->snd_portid);
4574
4575     pr_debug("mac80211_hwsim: received a REGISTER, "
4576            "switching to wmediumd mode with pid %d\n", info->snd_portid);
4577
4578     return 0;
4579 }
4580
4581 /* ensures ciphers only include ciphers listed in 'hwsim_ciphers' array */
4582 static bool hwsim_known_ciphers(const u32 *ciphers, int n_ciphers)
4583 {
4584     int i;
4585
4586     for (i = 0; i < n_ciphers; i++) {
4587         int j;
4588         int found = 0;
4589
4590         for (j = 0; j < ARRAY_SIZE(hwsim_ciphers); j++) {
4591             if (ciphers[i] == hwsim_ciphers[j]) {
4592                 found = 1;
4593                 break;
4594             }
4595         }
4596
4597         if (!found)
4598             return false;
4599     }
4600
4601     return true;
4602 }
4603
4604 static int hwsim_new_radio_nl(struct sk_buff *msg, struct genl_info *info)
4605 {
4606     struct hwsim_new_radio_params param = { 0 };
4607     const char *hwname = NULL;
4608     int ret;
4609
4610     param.reg_strict = info->attrs[HWSIM_ATTR_REG_STRICT_REG];
4611     param.p2p_device = info->attrs[HWSIM_ATTR_SUPPORT_P2P_DEVICE];
4612     param.channels = channels;
4613     param.destroy_on_close =
4614         info->attrs[HWSIM_ATTR_DESTROY_RADIO_ON_CLOSE];
4615
4616     if (info->attrs[HWSIM_ATTR_CHANNELS])
4617         param.channels = nla_get_u32(info->attrs[HWSIM_ATTR_CHANNELS]);
4618
4619     if (param.channels < 1) {
4620         GENL_SET_ERR_MSG(info, "must have at least one channel");
4621         return -EINVAL;
4622     }
4623
4624     if (info->attrs[HWSIM_ATTR_NO_VIF])
4625         param.no_vif = true;
4626
4627     if (info->attrs[HWSIM_ATTR_USE_CHANCTX])
4628         param.use_chanctx = true;
4629     else
4630         param.use_chanctx = (param.channels > 1);
4631
4632     if (info->attrs[HWSIM_ATTR_REG_HINT_ALPHA2])
4633         param.reg_alpha2 =
4634             nla_data(info->attrs[HWSIM_ATTR_REG_HINT_ALPHA2]);
4635
4636     if (info->attrs[HWSIM_ATTR_REG_CUSTOM_REG]) {
4637         u32 idx = nla_get_u32(info->attrs[HWSIM_ATTR_REG_CUSTOM_REG]);
4638
4639         if (idx >= ARRAY_SIZE(hwsim_world_regdom_custom))
4640             return -EINVAL;
4641
4642         idx = array_index_nospec(idx,
4643                      ARRAY_SIZE(hwsim_world_regdom_custom));
4644         param.regd = hwsim_world_regdom_custom[idx];
4645     }
4646
4647     if (info->attrs[HWSIM_ATTR_PERM_ADDR]) {
4648         if (!is_valid_ether_addr(
4649                 nla_data(info->attrs[HWSIM_ATTR_PERM_ADDR]))) {
4650             GENL_SET_ERR_MSG(info,"MAC is no valid source addr");
4651             NL_SET_BAD_ATTR(info->extack,
4652                     info->attrs[HWSIM_ATTR_PERM_ADDR]);
4653             return -EINVAL;
4654         }
4655
4656         param.perm_addr = nla_data(info->attrs[HWSIM_ATTR_PERM_ADDR]);
4657     }
4658
4659     if (info->attrs[HWSIM_ATTR_IFTYPE_SUPPORT]) {
4660         param.iftypes =
4661             nla_get_u32(info->attrs[HWSIM_ATTR_IFTYPE_SUPPORT]);
4662
4663         if (param.iftypes & ~HWSIM_IFTYPE_SUPPORT_MASK) {
4664             NL_SET_ERR_MSG_ATTR(info->extack,
4665                         info->attrs[HWSIM_ATTR_IFTYPE_SUPPORT],
4666                         "cannot support more iftypes than kernel");
4667             return -EINVAL;
4668         }
4669     } else {
4670         param.iftypes = HWSIM_IFTYPE_SUPPORT_MASK;
4671     }
4672
4673     /* ensure both flag and iftype support is honored */
4674     if (param.p2p_device ||
4675         param.iftypes & BIT(NL80211_IFTYPE_P2P_DEVICE)) {
4676         param.iftypes |= BIT(NL80211_IFTYPE_P2P_DEVICE);
4677         param.p2p_device = true;
4678     }
4679
4680     if (info->attrs[HWSIM_ATTR_CIPHER_SUPPORT]) {
4681         u32 len = nla_len(info->attrs[HWSIM_ATTR_CIPHER_SUPPORT]);
4682
4683         param.ciphers =
4684             nla_data(info->attrs[HWSIM_ATTR_CIPHER_SUPPORT]);
4685
4686         if (len % sizeof(u32)) {
4687             NL_SET_ERR_MSG_ATTR(info->extack,
4688                         info->attrs[HWSIM_ATTR_CIPHER_SUPPORT],
4689                         "bad cipher list length");
4690             return -EINVAL;
4691         }
4692
4693         param.n_ciphers = len / sizeof(u32);
4694
4695         if (param.n_ciphers > ARRAY_SIZE(hwsim_ciphers)) {
4696             NL_SET_ERR_MSG_ATTR(info->extack,
4697                         info->attrs[HWSIM_ATTR_CIPHER_SUPPORT],
4698                         "too many ciphers specified");
4699             return -EINVAL;
4700         }
4701
4702         if (!hwsim_known_ciphers(param.ciphers, param.n_ciphers)) {
4703             NL_SET_ERR_MSG_ATTR(info->extack,
4704                         info->attrs[HWSIM_ATTR_CIPHER_SUPPORT],
4705                         "unsupported ciphers specified");
4706             return -EINVAL;
4707         }
4708     }
4709
4710     param.mlo = info->attrs[HWSIM_ATTR_MLO_SUPPORT];
4711
4712     if (param.mlo)
4713         param.use_chanctx = true;
4714
4715     if (info->attrs[HWSIM_ATTR_RADIO_NAME]) {
4716         hwname = kstrndup((char *)nla_data(info->attrs[HWSIM_ATTR_RADIO_NAME]),
4717                   nla_len(info->attrs[HWSIM_ATTR_RADIO_NAME]),
4718                   GFP_KERNEL);
4719         if (!hwname)
4720             return -ENOMEM;
4721         param.hwname = hwname;
4722     }
4723
4724     ret = mac80211_hwsim_new_radio(info, &param);
4725     kfree(hwname);
4726     return ret;
4727 }
4728
4729 static int hwsim_del_radio_nl(struct sk_buff *msg, struct genl_info *info)
4730 {
4731     struct mac80211_hwsim_data *data;
4732     s64 idx = -1;
4733     const char *hwname = NULL;
4734
4735     if (info->attrs[HWSIM_ATTR_RADIO_ID]) {
4736         idx = nla_get_u32(info->attrs[HWSIM_ATTR_RADIO_ID]);
4737     } else if (info->attrs[HWSIM_ATTR_RADIO_NAME]) {
4738         hwname = kstrndup((char *)nla_data(info->attrs[HWSIM_ATTR_RADIO_NAME]),
4739                   nla_len(info->attrs[HWSIM_ATTR_RADIO_NAME]),
4740                   GFP_KERNEL);
4741         if (!hwname)
4742             return -ENOMEM;
4743     } else
4744         return -EINVAL;
4745
4746     spin_lock_bh(&hwsim_radio_lock);
4747     list_for_each_entry(data, &hwsim_radios, list) {
4748         if (idx >= 0) {
4749             if (data->idx != idx)
4750                 continue;
4751         } else {
4752             if (!hwname ||
4753                 strcmp(hwname, wiphy_name(data->hw->wiphy)))
4754                 continue;
4755         }
4756
4757         if (!net_eq(wiphy_net(data->hw->wiphy), genl_info_net(info)))
4758             continue;
4759
4760         list_del(&data->list);
4761         rhashtable_remove_fast(&hwsim_radios_rht, &data->rht,
4762                        hwsim_rht_params);
4763         hwsim_radios_generation++;
4764         spin_unlock_bh(&hwsim_radio_lock);
4765         mac80211_hwsim_del_radio(data, wiphy_name(data->hw->wiphy),
4766                      info);
4767         kfree(hwname);
4768         return 0;
4769     }
4770     spin_unlock_bh(&hwsim_radio_lock);
4771
4772     kfree(hwname);
4773     return -ENODEV;
4774 }
4775
4776 static int hwsim_get_radio_nl(struct sk_buff *msg, struct genl_info *info)
4777 {
4778     struct mac80211_hwsim_data *data;
4779     struct sk_buff *skb;
4780     int idx, res = -ENODEV;
4781
4782     if (!info->attrs[HWSIM_ATTR_RADIO_ID])
4783         return -EINVAL;
4784     idx = nla_get_u32(info->attrs[HWSIM_ATTR_RADIO_ID]);
4785
4786     spin_lock_bh(&hwsim_radio_lock);
4787     list_for_each_entry(data, &hwsim_radios, list) {
4788         if (data->idx != idx)
4789             continue;
4790
4791         if (!net_eq(wiphy_net(data->hw->wiphy), genl_info_net(info)))
4792             continue;
4793
4794         skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
4795         if (!skb) {
4796             res = -ENOMEM;
4797             goto out_err;
4798         }
4799
4800         res = mac80211_hwsim_get_radio(skb, data, info->snd_portid,
4801                            info->snd_seq, NULL, 0);
4802         if (res < 0) {
4803             nlmsg_free(skb);
4804             goto out_err;
4805         }
4806
4807         res = genlmsg_reply(skb, info);
4808         break;
4809     }
4810
4811 out_err:
4812     spin_unlock_bh(&hwsim_radio_lock);
4813
4814     return res;
4815 }
4816
4817 static int hwsim_dump_radio_nl(struct sk_buff *skb,
4818                    struct netlink_callback *cb)
4819 {
4820     int last_idx = cb->args[0] - 1;
4821     struct mac80211_hwsim_data *data = NULL;
4822     int res = 0;
4823     void *hdr;
4824
4825     spin_lock_bh(&hwsim_radio_lock);
4826     cb->seq = hwsim_radios_generation;
4827
4828     if (last_idx >= hwsim_radio_idx-1)
4829         goto done;
4830
4831     list_for_each_entry(data, &hwsim_radios, list) {
4832         if (data->idx <= last_idx)
4833             continue;
4834
4835         if (!net_eq(wiphy_net(data->hw->wiphy), sock_net(skb->sk)))
4836             continue;
4837
4838         res = mac80211_hwsim_get_radio(skb, data,
4839                            NETLINK_CB(cb->skb).portid,
4840                            cb->nlh->nlmsg_seq, cb,
4841                            NLM_F_MULTI);
4842         if (res < 0)
4843             break;
4844
4845         last_idx = data->idx;
4846     }
4847
4848     cb->args[0] = last_idx + 1;
4849
4850     /* list changed, but no new element sent, set interrupted flag */
4851     if (skb->len == 0 && cb->prev_seq && cb->seq != cb->prev_seq) {
4852         hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid,
4853                   cb->nlh->nlmsg_seq, &hwsim_genl_family,
4854                   NLM_F_MULTI, HWSIM_CMD_GET_RADIO);
4855         if (hdr) {
4856             genl_dump_check_consistent(cb, hdr);
4857             genlmsg_end(skb, hdr);
4858         } else {
4859             res = -EMSGSIZE;
4860         }
4861     }
4862
4863 done:
4864     spin_unlock_bh(&hwsim_radio_lock);
4865     return res ?: skb->len;
4866 }
4867
4868 /* Generic Netlink operations array */
4869 static const struct genl_small_ops hwsim_ops[] = {
4870     {
4871         .cmd = HWSIM_CMD_REGISTER,
4872         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
4873         .doit = hwsim_register_received_nl,
4874         .flags = GENL_UNS_ADMIN_PERM,
4875     },
4876     {
4877         .cmd = HWSIM_CMD_FRAME,
4878         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
4879         .doit = hwsim_cloned_frame_received_nl,
4880     },
4881     {
4882         .cmd = HWSIM_CMD_TX_INFO_FRAME,
4883         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
4884         .doit = hwsim_tx_info_frame_received_nl,
4885     },
4886     {
4887         .cmd = HWSIM_CMD_NEW_RADIO,
4888         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
4889         .doit = hwsim_new_radio_nl,
4890         .flags = GENL_UNS_ADMIN_PERM,
4891     },
4892     {
4893         .cmd = HWSIM_CMD_DEL_RADIO,
4894         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
4895         .doit = hwsim_del_radio_nl,
4896         .flags = GENL_UNS_ADMIN_PERM,
4897     },
4898     {
4899         .cmd = HWSIM_CMD_GET_RADIO,
4900         .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
4901         .doit = hwsim_get_radio_nl,
4902         .dumpit = hwsim_dump_radio_nl,
4903     },
4904 };
4905
4906 static struct genl_family hwsim_genl_family __ro_after_init = {
4907     .name = "MAC80211_HWSIM",
4908     .version = 1,
4909     .maxattr = HWSIM_ATTR_MAX,
4910     .policy = hwsim_genl_policy,
4911     .netnsok = true,
4912     .module = THIS_MODULE,
4913     .small_ops = hwsim_ops,
4914     .n_small_ops = ARRAY_SIZE(hwsim_ops),
4915     .mcgrps = hwsim_mcgrps,
4916     .n_mcgrps = ARRAY_SIZE(hwsim_mcgrps),
4917 };
4918
4919 static void remove_user_radios(u32 portid)
4920 {
4921     struct mac80211_hwsim_data *entry, *tmp;
4922     LIST_HEAD(list);
4923
4924     spin_lock_bh(&hwsim_radio_lock);
4925     list_for_each_entry_safe(entry, tmp, &hwsim_radios, list) {
4926         if (entry->destroy_on_close && entry->portid == portid) {
4927             list_move(&entry->list, &list);
4928             rhashtable_remove_fast(&hwsim_radios_rht, &entry->rht,
4929                            hwsim_rht_params);
4930             hwsim_radios_generation++;
4931         }
4932     }
4933     spin_unlock_bh(&hwsim_radio_lock);
4934
4935     list_for_each_entry_safe(entry, tmp, &list, list) {
4936         list_del(&entry->list);
4937         mac80211_hwsim_del_radio(entry, wiphy_name(entry->hw->wiphy),
4938                      NULL);
4939     }
4940 }
4941
4942 static int mac80211_hwsim_netlink_notify(struct notifier_block *nb,
4943                      unsigned long state,
4944                      void *_notify)
4945 {
4946     struct netlink_notify *notify = _notify;
4947
4948     if (state != NETLINK_URELEASE)
4949         return NOTIFY_DONE;
4950
4951     remove_user_radios(notify->portid);
4952
4953     if (notify->portid == hwsim_net_get_wmediumd(notify->net)) {
4954         printk(KERN_INFO "mac80211_hwsim: wmediumd released netlink"
4955                " socket, switching to perfect channel medium\n");
4956         hwsim_register_wmediumd(notify->net, 0);
4957     }
4958     return NOTIFY_DONE;
4959
4960 }
4961
4962 static struct notifier_block hwsim_netlink_notifier = {
4963     .notifier_call = mac80211_hwsim_netlink_notify,
4964 };
4965
4966 static int __init hwsim_init_netlink(void)
4967 {
4968     int rc;
4969
4970     printk(KERN_INFO "mac80211_hwsim: initializing netlink\n");
4971
4972     rc = genl_register_family(&hwsim_genl_family);
4973     if (rc)
4974         goto failure;
4975
4976     rc = netlink_register_notifier(&hwsim_netlink_notifier);
4977     if (rc) {
4978         genl_unregister_family(&hwsim_genl_family);
4979         goto failure;
4980     }
4981
4982     return 0;
4983
4984 failure:
4985     pr_debug("mac80211_hwsim: error occurred in %s\n", __func__);
4986     return -EINVAL;
4987 }
4988
4989 static __net_init int hwsim_init_net(struct net *net)
4990 {
4991     return hwsim_net_set_netgroup(net);
4992 }
4993
4994 static void __net_exit hwsim_exit_net(struct net *net)
4995 {
4996     struct mac80211_hwsim_data *data, *tmp;
4997     LIST_HEAD(list);
4998
4999     spin_lock_bh(&hwsim_radio_lock);
5000     list_for_each_entry_safe(data, tmp, &hwsim_radios, list) {
5001         if (!net_eq(wiphy_net(data->hw->wiphy), net))
5002             continue;
5003
5004         /* Radios created in init_net are returned to init_net. */
5005         if (data->netgroup == hwsim_net_get_netgroup(&init_net))
5006             continue;
5007
5008         list_move(&data->list, &list);
5009         rhashtable_remove_fast(&hwsim_radios_rht, &data->rht,
5010                        hwsim_rht_params);
5011         hwsim_radios_generation++;
5012     }
5013     spin_unlock_bh(&hwsim_radio_lock);
5014
5015     list_for_each_entry_safe(data, tmp, &list, list) {
5016         list_del(&data->list);
5017         mac80211_hwsim_del_radio(data,
5018                      wiphy_name(data->hw->wiphy),
5019                      NULL);
5020     }
5021
5022     ida_free(&hwsim_netgroup_ida, hwsim_net_get_netgroup(net));
5023 }
5024
5025 static struct pernet_operations hwsim_net_ops = {
5026     .init = hwsim_init_net,
5027     .exit = hwsim_exit_net,
5028     .id   = &hwsim_net_id,
5029     .size = sizeof(struct hwsim_net),
5030 };
5031
5032 static void hwsim_exit_netlink(void)
5033 {
5034     /* unregister the notifier */
5035     netlink_unregister_notifier(&hwsim_netlink_notifier);
5036     /* unregister the family */
5037     genl_unregister_family(&hwsim_genl_family);
5038 }
5039
5040 #if IS_REACHABLE(CONFIG_VIRTIO)
5041 static void hwsim_virtio_tx_done(struct virtqueue *vq)
5042 {
5043     unsigned int len;
5044     struct sk_buff *skb;
5045     unsigned long flags;
5046
5047     spin_lock_irqsave(&hwsim_virtio_lock, flags);
5048     while ((skb = virtqueue_get_buf(vq, &len)))
5049         nlmsg_free(skb);
5050     spin_unlock_irqrestore(&hwsim_virtio_lock, flags);
5051 }
5052
5053 static int hwsim_virtio_handle_cmd(struct sk_buff *skb)
5054 {
5055     struct nlmsghdr *nlh;
5056     struct genlmsghdr *gnlh;
5057     struct nlattr *tb[HWSIM_ATTR_MAX + 1];
5058     struct genl_info info = {};
5059     int err;
5060
5061     nlh = nlmsg_hdr(skb);
5062     gnlh = nlmsg_data(nlh);
5063
5064     if (skb->len < nlh->nlmsg_len)
5065         return -EINVAL;
5066
5067     err = genlmsg_parse(nlh, &hwsim_genl_family, tb, HWSIM_ATTR_MAX,
5068                 hwsim_genl_policy, NULL);
5069     if (err) {
5070         pr_err_ratelimited("hwsim: genlmsg_parse returned %d\n", err);
5071         return err;
5072     }
5073
5074     info.attrs = tb;
5075
5076     switch (gnlh->cmd) {
5077     case HWSIM_CMD_FRAME:
5078         hwsim_cloned_frame_received_nl(skb, &info);
5079         break;
5080     case HWSIM_CMD_TX_INFO_FRAME:
5081         hwsim_tx_info_frame_received_nl(skb, &info);
5082         break;
5083     default:
5084         pr_err_ratelimited("hwsim: invalid cmd: %d\n", gnlh->cmd);
5085         return -EPROTO;
5086     }
5087     return 0;
5088 }
5089
5090 static void hwsim_virtio_rx_work(struct work_struct *work)
5091 {
5092     struct virtqueue *vq;
5093     unsigned int len;
5094     struct sk_buff *skb;
5095     struct scatterlist sg[1];
5096     int err;
5097     unsigned long flags;
5098
5099     spin_lock_irqsave(&hwsim_virtio_lock, flags);
5100     if (!hwsim_virtio_enabled)
5101         goto out_unlock;
5102
5103     skb = virtqueue_get_buf(hwsim_vqs[HWSIM_VQ_RX], &len);
5104     if (!skb)
5105         goto out_unlock;
5106     spin_unlock_irqrestore(&hwsim_virtio_lock, flags);
5107
5108     skb->data = skb->head;
5109     skb_reset_tail_pointer(skb);
5110     skb_put(skb, len);
5111     hwsim_virtio_handle_cmd(skb);
5112
5113     spin_lock_irqsave(&hwsim_virtio_lock, flags);
5114     if (!hwsim_virtio_enabled) {
5115         nlmsg_free(skb);
5116         goto out_unlock;
5117     }
5118     vq = hwsim_vqs[HWSIM_VQ_RX];
5119     sg_init_one(sg, skb->head, skb_end_offset(skb));
5120     err = virtqueue_add_inbuf(vq, sg, 1, skb, GFP_ATOMIC);
5121     if (WARN(err, "virtqueue_add_inbuf returned %d\n", err))
5122         nlmsg_free(skb);
5123     else
5124         virtqueue_kick(vq);
5125     schedule_work(&hwsim_virtio_rx);
5126
5127 out_unlock:
5128     spin_unlock_irqrestore(&hwsim_virtio_lock, flags);
5129 }
5130
5131 static void hwsim_virtio_rx_done(struct virtqueue *vq)
5132 {
5133     schedule_work(&hwsim_virtio_rx);
5134 }
5135
5136 static int init_vqs(struct virtio_device *vdev)
5137 {
5138     vq_callback_t *callbacks[HWSIM_NUM_VQS] = {
5139         [HWSIM_VQ_TX] = hwsim_virtio_tx_done,
5140         [HWSIM_VQ_RX] = hwsim_virtio_rx_done,
5141     };
5142     const char *names[HWSIM_NUM_VQS] = {
5143         [HWSIM_VQ_TX] = "tx",
5144         [HWSIM_VQ_RX] = "rx",
5145     };
5146
5147     return virtio_find_vqs(vdev, HWSIM_NUM_VQS,
5148                    hwsim_vqs, callbacks, names, NULL);
5149 }
5150
5151 static int fill_vq(struct virtqueue *vq)
5152 {
5153     int i, err;
5154     struct sk_buff *skb;
5155     struct scatterlist sg[1];
5156
5157     for (i = 0; i < virtqueue_get_vring_size(vq); i++) {
5158         skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
5159         if (!skb)
5160             return -ENOMEM;
5161
5162         sg_init_one(sg, skb->head, skb_end_offset(skb));
5163         err = virtqueue_add_inbuf(vq, sg, 1, skb, GFP_KERNEL);
5164         if (err) {
5165             nlmsg_free(skb);
5166             return err;
5167         }
5168     }
5169     virtqueue_kick(vq);
5170     return 0;
5171 }
5172
5173 static void remove_vqs(struct virtio_device *vdev)
5174 {
5175     int i;
5176
5177     virtio_reset_device(vdev);
5178
5179     for (i = 0; i < ARRAY_SIZE(hwsim_vqs); i++) {
5180         struct virtqueue *vq = hwsim_vqs[i];
5181         struct sk_buff *skb;
5182
5183         while ((skb = virtqueue_detach_unused_buf(vq)))
5184             nlmsg_free(skb);
5185     }
5186
5187     vdev->config->del_vqs(vdev);
5188 }
5189
5190 static int hwsim_virtio_probe(struct virtio_device *vdev)
5191 {
5192     int err;
5193     unsigned long flags;
5194
5195     spin_lock_irqsave(&hwsim_virtio_lock, flags);
5196     if (hwsim_virtio_enabled) {
5197         spin_unlock_irqrestore(&hwsim_virtio_lock, flags);
5198         return -EEXIST;
5199     }
5200     spin_unlock_irqrestore(&hwsim_virtio_lock, flags);
5201
5202     err = init_vqs(vdev);
5203     if (err)
5204         return err;
5205
5206     virtio_device_ready(vdev);
5207
5208     err = fill_vq(hwsim_vqs[HWSIM_VQ_RX]);
5209     if (err)
5210         goto out_remove;
5211
5212     spin_lock_irqsave(&hwsim_virtio_lock, flags);
5213     hwsim_virtio_enabled = true;
5214     spin_unlock_irqrestore(&hwsim_virtio_lock, flags);
5215
5216     schedule_work(&hwsim_virtio_rx);
5217     return 0;
5218
5219 out_remove:
5220     remove_vqs(vdev);
5221     return err;
5222 }
5223
5224 static void hwsim_virtio_remove(struct virtio_device *vdev)
5225 {
5226     hwsim_virtio_enabled = false;
5227
5228     cancel_work_sync(&hwsim_virtio_rx);
5229
5230     remove_vqs(vdev);
5231 }
5232
5233 /* MAC80211_HWSIM virtio device id table */
5234 static const struct virtio_device_id id_table[] = {
5235     { VIRTIO_ID_MAC80211_HWSIM, VIRTIO_DEV_ANY_ID },
5236     { 0 }
5237 };
5238 MODULE_DEVICE_TABLE(virtio, id_table);
5239
5240 static struct virtio_driver virtio_hwsim = {
5241     .driver.name = KBUILD_MODNAME,
5242     .driver.owner = THIS_MODULE,
5243     .id_table = id_table,
5244     .probe = hwsim_virtio_probe,
5245     .remove = hwsim_virtio_remove,
5246 };
5247
5248 static int hwsim_register_virtio_driver(void)
5249 {
5250     return register_virtio_driver(&virtio_hwsim);
5251 }
5252
5253 static void hwsim_unregister_virtio_driver(void)
5254 {
5255     unregister_virtio_driver(&virtio_hwsim);
5256 }
5257 #else
5258 static inline int hwsim_register_virtio_driver(void)
5259 {
5260     return 0;
5261 }
5262
5263 static inline void hwsim_unregister_virtio_driver(void)
5264 {
5265 }
5266 #endif
5267
5268 static int __init init_mac80211_hwsim(void)
5269 {
5270     int i, err;
5271
5272     if (radios < 0 || radios > 100)
5273         return -EINVAL;
5274
5275     if (channels < 1)
5276         return -EINVAL;
5277
5278     err = rhashtable_init(&hwsim_radios_rht, &hwsim_rht_params);
5279     if (err)
5280         return err;
5281
5282     err = register_pernet_device(&hwsim_net_ops);
5283     if (err)
5284         goto out_free_rht;
5285
5286     err = platform_driver_register(&mac80211_hwsim_driver);
5287     if (err)
5288         goto out_unregister_pernet;
5289
5290     err = hwsim_init_netlink();
5291     if (err)
5292         goto out_unregister_driver;
5293
5294     err = hwsim_register_virtio_driver();
5295     if (err)
5296         goto out_exit_netlink;
5297
5298     hwsim_class = class_create(THIS_MODULE, "mac80211_hwsim");
5299     if (IS_ERR(hwsim_class)) {
5300         err = PTR_ERR(hwsim_class);
5301         goto out_exit_virtio;
5302     }
5303
5304     hwsim_init_s1g_channels(hwsim_channels_s1g);
5305
5306     for (i = 0; i < radios; i++) {
5307         struct hwsim_new_radio_params param = { 0 };
5308
5309         param.channels = channels;
5310
5311         switch (regtest) {
5312         case HWSIM_REGTEST_DIFF_COUNTRY:
5313             if (i < ARRAY_SIZE(hwsim_alpha2s))
5314                 param.reg_alpha2 = hwsim_alpha2s[i];
5315             break;
5316         case HWSIM_REGTEST_DRIVER_REG_FOLLOW:
5317             if (!i)
5318                 param.reg_alpha2 = hwsim_alpha2s[0];
5319             break;
5320         case HWSIM_REGTEST_STRICT_ALL:
5321             param.reg_strict = true;
5322             fallthrough;
5323         case HWSIM_REGTEST_DRIVER_REG_ALL:
5324             param.reg_alpha2 = hwsim_alpha2s[0];
5325             break;
5326         case HWSIM_REGTEST_WORLD_ROAM:
5327             if (i == 0)
5328                 param.regd = &hwsim_world_regdom_custom_01;
5329             break;
5330         case HWSIM_REGTEST_CUSTOM_WORLD:
5331             param.regd = &hwsim_world_regdom_custom_01;
5332             break;
5333         case HWSIM_REGTEST_CUSTOM_WORLD_2:
5334             if (i == 0)
5335                 param.regd = &hwsim_world_regdom_custom_01;
5336             else if (i == 1)
5337                 param.regd = &hwsim_world_regdom_custom_02;
5338             break;
5339         case HWSIM_REGTEST_STRICT_FOLLOW:
5340             if (i == 0) {
5341                 param.reg_strict = true;
5342                 param.reg_alpha2 = hwsim_alpha2s[0];
5343             }
5344             break;
5345         case HWSIM_REGTEST_STRICT_AND_DRIVER_REG:
5346             if (i == 0) {
5347                 param.reg_strict = true;
5348                 param.reg_alpha2 = hwsim_alpha2s[0];
5349             } else if (i == 1) {
5350                 param.reg_alpha2 = hwsim_alpha2s[1];
5351             }
5352             break;
5353         case HWSIM_REGTEST_ALL:
5354             switch (i) {
5355             case 0:
5356                 param.regd = &hwsim_world_regdom_custom_01;
5357                 break;
5358             case 1:
5359                 param.regd = &hwsim_world_regdom_custom_02;
5360                 break;
5361             case 2:
5362                 param.reg_alpha2 = hwsim_alpha2s[0];
5363                 break;
5364             case 3:
5365                 param.reg_alpha2 = hwsim_alpha2s[1];
5366                 break;
5367             case 4:
5368                 param.reg_strict = true;
5369                 param.reg_alpha2 = hwsim_alpha2s[2];
5370                 break;
5371             }
5372             break;
5373         default:
5374             break;
5375         }
5376
5377         param.p2p_device = support_p2p_device;
5378         param.mlo = mlo;
5379         param.use_chanctx = channels > 1 || mlo;
5380         param.iftypes = HWSIM_IFTYPE_SUPPORT_MASK;
5381         if (param.p2p_device)
5382             param.iftypes |= BIT(NL80211_IFTYPE_P2P_DEVICE);
5383
5384         err = mac80211_hwsim_new_radio(NULL, &param);
5385         if (err < 0)
5386             goto out_free_radios;
5387     }
5388
5389     hwsim_mon = alloc_netdev(0, "hwsim%d", NET_NAME_UNKNOWN,
5390                  hwsim_mon_setup);
5391     if (hwsim_mon == NULL) {
5392         err = -ENOMEM;
5393         goto out_free_radios;
5394     }
5395
5396     rtnl_lock();
5397     err = dev_alloc_name(hwsim_mon, hwsim_mon->name);
5398     if (err < 0) {
5399         rtnl_unlock();
5400         goto out_free_mon;
5401     }
5402
5403     err = register_netdevice(hwsim_mon);
5404     if (err < 0) {
5405         rtnl_unlock();
5406         goto out_free_mon;
5407     }
5408     rtnl_unlock();
5409
5410     return 0;
5411
5412 out_free_mon:
5413     free_netdev(hwsim_mon);
5414 out_free_radios:
5415     mac80211_hwsim_free();
5416 out_exit_virtio:
5417     hwsim_unregister_virtio_driver();
5418 out_exit_netlink:
5419     hwsim_exit_netlink();
5420 out_unregister_driver:
5421     platform_driver_unregister(&mac80211_hwsim_driver);
5422 out_unregister_pernet:
5423     unregister_pernet_device(&hwsim_net_ops);
5424 out_free_rht:
5425     rhashtable_destroy(&hwsim_radios_rht);
5426     return err;
5427 }
5428 module_init(init_mac80211_hwsim);
5429
5430 static void __exit exit_mac80211_hwsim(void)
5431 {
5432     pr_debug("mac80211_hwsim: unregister radios\n");
5433
5434     hwsim_unregister_virtio_driver();
5435     hwsim_exit_netlink();
5436
5437     mac80211_hwsim_free();
5438
5439     rhashtable_destroy(&hwsim_radios_rht);
5440     unregister_netdev(hwsim_mon);
5441     platform_driver_unregister(&mac80211_hwsim_driver);
5442     unregister_pernet_device(&hwsim_net_ops);
5443 }
5444 module_exit(exit_mac80211_hwsim);