diff --git a/vwifi.c b/vwifi.c index 318cdee..e9837d8 100644 --- a/vwifi.c +++ b/vwifi.c @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -35,6 +36,12 @@ MODULE_DESCRIPTION("virtual cfg80211 driver"); #define SCAN_TIMEOUT_MS 100 /*< millisecond */ +#define VWIFI_ALL_RATES_2GHZ 0x0FFF +#define VWIFI_ALL_RATES_5GHZ 0x0FF0 // bit 4~11 set + +static const struct ieee80211_rate vwifi_supported_rates[]; +static const int vwifi_supported_rates_count = 12; + /* Note: vwifi_cipher_suites is an array of int defining which cipher suites * are supported. A pointer to this array and the number of entries is passed * on to upper layers. @@ -88,6 +95,7 @@ struct vwifi_vif { struct wireless_dev wdev; struct net_device *ndev; struct net_device_stats stats; + struct cfg80211_bitrate_mask bitrate_mask; size_t ssid_len; /* Currently connected BSS id */ @@ -206,6 +214,10 @@ static int station = 2; module_param(station, int, 0444); MODULE_PARM_DESC(station, "Number of virtual interfaces running in STA mode."); +static int ap = 0; +module_param(ap, int, 0444); +MODULE_PARM_DESC(ap, "number of AP mode interfaces to create"); + /* Global context */ static struct vwifi_context *vwifi = NULL; @@ -548,6 +560,46 @@ static void inform_bss(struct vwifi_vif *vif) } } +static int vwifi_set_bitrate_mask(struct wiphy *wiphy, + struct net_device *dev, + unsigned int link_id, + const u8 *peer, + const struct cfg80211_bitrate_mask *mask) +{ + struct vwifi_vif *vif = netdev_priv(dev); + + if (!(dev->flags & IFF_UP) || !netif_running(dev)) { + return -ENETDOWN; + } + + bool has_valid_rate = false; + + for (int band = 0; band < NUM_NL80211_BANDS; band++) { + u32 user_mask = mask->control[band].legacy; + u32 basic_mask = 0; + + if (band == NL80211_BAND_2GHZ) + basic_mask = VWIFI_ALL_RATES_2GHZ; + else if (band == NL80211_BAND_5GHZ) + basic_mask = VWIFI_ALL_RATES_5GHZ; + else + continue; + + if (user_mask & basic_mask) { + has_valid_rate = true; + break; + } + } + + if (!has_valid_rate) + return -EINVAL; + + memcpy(&vif->bitrate_mask, mask, sizeof(*mask)); + pr_info("vwifi: bitrate mask saved. legacy 2.4GHz = 0x%x\n", + mask->control[NL80211_BAND_2GHZ].legacy); + return 0; +} + /* Helper function that prepares a structure with self-defined BSS information * and "informs" the kernel about the "new" Independent BSS. */ @@ -871,6 +923,26 @@ static int __vwifi_ndo_start_xmit(struct vwifi_vif *vif, eth_hdr->h_source); } + int bitrate_kbps = 0; + const struct cfg80211_bitrate_mask *mask = &vif->bitrate_mask; + + for (int band = 0; band < NUM_NL80211_BANDS; band++) { + u32 mask_val = mask->control[band].legacy; + if (mask_val) { + for (int i = 0; i < vwifi_supported_rates_count; i++) { + if (mask_val & (1 << i)) { + bitrate_kbps = vwifi_supported_rates[i].bitrate * 100; + break; + } + } + break; + } + } + + if (bitrate_kbps > 0) { + int delay_us = (datalen * 8 * 1000) / bitrate_kbps; + udelay(delay_us); + } /* Directly send to rx_queue, simulate the rx interrupt */ vwifi_rx(dest_vif->ndev); @@ -1425,15 +1497,40 @@ static int vwifi_get_station(struct wiphy *wiphy, * https://semfionetworks.com/blog/mcs-table-updated-with-80211ax-data-rates/ * IEEE 802.11n : https://zh.wikipedia.org/zh-tw/IEEE_802.11n */ - sinfo->rxrate.flags |= RATE_INFO_FLAGS_MCS; - sinfo->rxrate.mcs = 31; - sinfo->rxrate.bw = RATE_INFO_BW_20; - sinfo->rxrate.n_bonded_ch = 1; - - sinfo->txrate.flags |= RATE_INFO_FLAGS_MCS; - sinfo->txrate.mcs = 31; - sinfo->txrate.bw = RATE_INFO_BW_20; - sinfo->txrate.n_bonded_ch = 1; + const struct cfg80211_bitrate_mask *mask = &vif->bitrate_mask; + bool legacy_rate_set = false; + + for (int band = 0; band < NUM_NL80211_BANDS; band++) { + u32 mask_val = mask->control[band].legacy; + if (mask_val) { + int bitrate = 0; + for (int i = 0; i < 32; i++) { + if (mask_val & (1 << i)) { + bitrate = vwifi_supported_rates[i].bitrate; + break; + } + } + + if (bitrate > 0) { + sinfo->txrate.legacy = bitrate; + sinfo->rxrate.legacy = bitrate; + legacy_rate_set = true; + break; + } + } + } + + if (!legacy_rate_set) { + sinfo->txrate.flags |= RATE_INFO_FLAGS_MCS; + sinfo->txrate.mcs = 31; + sinfo->txrate.bw = RATE_INFO_BW_20; + sinfo->txrate.n_bonded_ch = 1; + + sinfo->rxrate.flags |= RATE_INFO_FLAGS_MCS; + sinfo->rxrate.mcs = 31; + sinfo->rxrate.bw = RATE_INFO_BW_20; + sinfo->rxrate.n_bonded_ch = 1; + } return 0; } @@ -2170,6 +2267,7 @@ static struct cfg80211_ops vwifi_cfg_ops = { .get_tx_power = vwifi_get_tx_power, .join_ibss = vwifi_join_ibss, .leave_ibss = vwifi_leave_ibss, + .set_bitrate_mask = vwifi_set_bitrate_mask, }; /* Macro for defining 2GHZ channel array */ @@ -3340,6 +3438,14 @@ static int __init vwifi_init(void) goto interface_add; } + for (int i = 0; i < ap; ++i) { + struct wiphy *wiphy = vwifi_cfg80211_add(); + if (!wiphy) + goto cfg80211_add; + if (!vwifi_interface_add(wiphy, i)) + goto interface_add; + } + nl_sk = netlink_kernel_create(&init_net, NETLINK_USERSOCK, &nl_config); if (!nl_sk) { pr_info("Error creating netlink socket\n");