mac80211: add struct ieee80211_tx_status support to ieee80211_add_tx_radiotap_header

Add support to ieee80211_add_tx_radiotap_header() for handling rates
reported via ieee80211_tx_status. This allows us to also report HE rates.

Signed-off-by: John Crispin <john@phrozen.org>
Link: https://lore.kernel.org/r/20190714154419.11854-4-john@phrozen.org
[remove text about 60 GHz, mac80211 doesn't support it, fix endianness issue]
Signed-off-by: Johannes Berg <johannes.berg@intel.com>

diff --git a/net/mac80211/status.c b/net/mac80211/status.c
index adf6269fa3637255662182e11cd395dfdacd8988..f03aa8924d232049c7d7cf3904740fe958630fbe 100644 (file)
--- a/net/mac80211/status.c
+++ b/net/mac80211/status.c
@@ -254,7 +254,8 @@ static void ieee80211_set_bar_pending(struct sta_info *sta, u8 tid, u16 ssn)
        tid_tx->bar_pending = true;
 }
 
-static int ieee80211_tx_radiotap_len(struct ieee80211_tx_info *info)
+static int ieee80211_tx_radiotap_len(struct ieee80211_tx_info *info,
+                                    struct ieee80211_tx_status *status)
 {
        int len = sizeof(struct ieee80211_radiotap_header);
 
@@ -272,7 +273,14 @@ static int ieee80211_tx_radiotap_len(struct ieee80211_tx_info *info)
 
        /* IEEE80211_RADIOTAP_MCS
         * IEEE80211_RADIOTAP_VHT */
-       if (info->status.rates[0].idx >= 0) {
+       if (status && status->rate) {
+               if (status->rate->flags & RATE_INFO_FLAGS_MCS)
+                       len += 3;
+               else if (status->rate->flags & RATE_INFO_FLAGS_VHT_MCS)
+                       len = ALIGN(len, 2) + 12;
+               else if (status->rate->flags & RATE_INFO_FLAGS_HE_MCS)
+                       len = ALIGN(len, 2) + 12;
+       } else if (info->status.rates[0].idx >= 0) {
                if (info->status.rates[0].flags & IEEE80211_TX_RC_MCS)
                        len += 3;
                else if (info->status.rates[0].flags & IEEE80211_TX_RC_VHT_MCS)
@@ -286,12 +294,14 @@ static void
 ieee80211_add_tx_radiotap_header(struct ieee80211_local *local,
                                 struct ieee80211_supported_band *sband,
                                 struct sk_buff *skb, int retry_count,
-                                int rtap_len, int shift)
+                                int rtap_len, int shift,
+                                struct ieee80211_tx_status *status)
 {
        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
        struct ieee80211_radiotap_header *rthdr;
        unsigned char *pos;
+       u16 legacy_rate = 0;
        u16 txflags;
 
        rthdr = skb_push(skb, rtap_len);
@@ -310,14 +320,22 @@ ieee80211_add_tx_radiotap_header(struct ieee80211_local *local,
         */
 
        /* IEEE80211_RADIOTAP_RATE */
-       if (info->status.rates[0].idx >= 0 &&
-           !(info->status.rates[0].flags & (IEEE80211_TX_RC_MCS |
-                                            IEEE80211_TX_RC_VHT_MCS))) {
-               u16 rate;
 
+       if (status && status->rate && !(status->rate->flags &
+                                       (RATE_INFO_FLAGS_MCS |
+                                        RATE_INFO_FLAGS_60G |
+                                        RATE_INFO_FLAGS_VHT_MCS |
+                                        RATE_INFO_FLAGS_HE_MCS)))
+               legacy_rate = status->rate->legacy;
+       else if (info->status.rates[0].idx >= 0 &&
+                !(info->status.rates[0].flags & (IEEE80211_TX_RC_MCS |
+                                                 IEEE80211_TX_RC_VHT_MCS)))
+               legacy_rate =
+                       sband->bitrates[info->status.rates[0].idx].bitrate;
+
+       if (legacy_rate) {
                rthdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_RATE);
-               rate = sband->bitrates[info->status.rates[0].idx].bitrate;
-               *pos = DIV_ROUND_UP(rate, 5 * (1 << shift));
+               *pos = DIV_ROUND_UP(legacy_rate, 5 * (1 << shift));
                /* padding for tx flags */
                pos += 2;
        }
@@ -341,7 +359,139 @@ ieee80211_add_tx_radiotap_header(struct ieee80211_local *local,
        *pos = retry_count;
        pos++;
 
-       if (info->status.rates[0].idx < 0)
+       if (status && status->rate &&
+           (status->rate->flags & RATE_INFO_FLAGS_MCS)) {
+               rthdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_MCS);
+               pos[0] = IEEE80211_RADIOTAP_MCS_HAVE_MCS |
+                        IEEE80211_RADIOTAP_MCS_HAVE_GI |
+                        IEEE80211_RADIOTAP_MCS_HAVE_BW;
+               if (status->rate->flags & RATE_INFO_FLAGS_SHORT_GI)
+                       pos[1] |= IEEE80211_RADIOTAP_MCS_SGI;
+               if (status->rate->bw == RATE_INFO_BW_40)
+                       pos[1] |= IEEE80211_RADIOTAP_MCS_BW_40;
+               pos[2] = status->rate->mcs;
+               pos += 3;
+       } else if (status && status->rate &&
+                  (status->rate->flags & RATE_INFO_FLAGS_VHT_MCS)) {
+               u16 known = local->hw.radiotap_vht_details &
+                       (IEEE80211_RADIOTAP_VHT_KNOWN_GI |
+                        IEEE80211_RADIOTAP_VHT_KNOWN_BANDWIDTH);
+
+               rthdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_VHT);
+
+               /* required alignment from rthdr */
+               pos = (u8 *)rthdr + ALIGN(pos - (u8 *)rthdr, 2);
+
+               /* u16 known - IEEE80211_RADIOTAP_VHT_KNOWN_* */
+               put_unaligned_le16(known, pos);
+               pos += 2;
+
+               /* u8 flags - IEEE80211_RADIOTAP_VHT_FLAG_* */
+               if (status->rate->flags & RATE_INFO_FLAGS_SHORT_GI)
+                       *pos |= IEEE80211_RADIOTAP_VHT_FLAG_SGI;
+               pos++;
+
+               /* u8 bandwidth */
+               switch (status->rate->bw) {
+               case RATE_INFO_BW_160:
+                       *pos = 11;
+                       break;
+               case RATE_INFO_BW_80:
+                       *pos = 2;
+                       break;
+               case RATE_INFO_BW_40:
+                       *pos = 1;
+                       break;
+               default:
+                       *pos = 0;
+                       break;
+               }
+
+               /* u8 mcs_nss[4] */
+               *pos = (status->rate->mcs << 4) | status->rate->nss;
+               pos += 4;
+
+               /* u8 coding */
+               pos++;
+               /* u8 group_id */
+               pos++;
+               /* u16 partial_aid */
+               pos += 2;
+       } else if (status && status->rate &&
+                  (status->rate->flags & RATE_INFO_FLAGS_HE_MCS)) {
+               struct ieee80211_radiotap_he *he;
+
+               rthdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_HE);
+
+               /* required alignment from rthdr */
+               pos = (u8 *)rthdr + ALIGN(pos - (u8 *)rthdr, 2);
+               he = (struct ieee80211_radiotap_he *)pos;
+
+               he->data1 = cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA1_FORMAT_SU |
+                                       IEEE80211_RADIOTAP_HE_DATA1_DATA_MCS_KNOWN |
+                                       IEEE80211_RADIOTAP_HE_DATA1_DATA_DCM_KNOWN |
+                                       IEEE80211_RADIOTAP_HE_DATA1_BW_RU_ALLOC_KNOWN);
+
+               he->data2 = cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA2_GI_KNOWN);
+
+#define HE_PREP(f, val) le16_encode_bits(val, IEEE80211_RADIOTAP_HE_##f)
+
+               he->data6 |= HE_PREP(DATA6_NSTS, status->rate->nss);
+
+#define CHECK_GI(s) \
+       BUILD_BUG_ON(IEEE80211_RADIOTAP_HE_DATA5_GI_##s != \
+       (int)NL80211_RATE_INFO_HE_GI_##s)
+
+               CHECK_GI(0_8);
+               CHECK_GI(1_6);
+               CHECK_GI(3_2);
+
+               he->data3 |= HE_PREP(DATA3_DATA_MCS, status->rate->mcs);
+               he->data3 |= HE_PREP(DATA3_DATA_DCM, status->rate->he_dcm);
+
+               he->data5 |= HE_PREP(DATA5_GI, status->rate->he_gi);
+
+               switch (status->rate->bw) {
+               case RATE_INFO_BW_20:
+                       he->data5 |= HE_PREP(DATA5_DATA_BW_RU_ALLOC,
+                                            IEEE80211_RADIOTAP_HE_DATA5_DATA_BW_RU_ALLOC_20MHZ);
+                       break;
+               case RATE_INFO_BW_40:
+                       he->data5 |= HE_PREP(DATA5_DATA_BW_RU_ALLOC,
+                                            IEEE80211_RADIOTAP_HE_DATA5_DATA_BW_RU_ALLOC_40MHZ);
+                       break;
+               case RATE_INFO_BW_80:
+                       he->data5 |= HE_PREP(DATA5_DATA_BW_RU_ALLOC,
+                                            IEEE80211_RADIOTAP_HE_DATA5_DATA_BW_RU_ALLOC_80MHZ);
+                       break;
+               case RATE_INFO_BW_160:
+                       he->data5 |= HE_PREP(DATA5_DATA_BW_RU_ALLOC,
+                                            IEEE80211_RADIOTAP_HE_DATA5_DATA_BW_RU_ALLOC_160MHZ);
+                       break;
+               case RATE_INFO_BW_HE_RU:
+#define CHECK_RU_ALLOC(s) \
+       BUILD_BUG_ON(IEEE80211_RADIOTAP_HE_DATA5_DATA_BW_RU_ALLOC_##s##T != \
+       NL80211_RATE_INFO_HE_RU_ALLOC_##s + 4)
+
+                       CHECK_RU_ALLOC(26);
+                       CHECK_RU_ALLOC(52);
+                       CHECK_RU_ALLOC(106);
+                       CHECK_RU_ALLOC(242);
+                       CHECK_RU_ALLOC(484);
+                       CHECK_RU_ALLOC(996);
+                       CHECK_RU_ALLOC(2x996);
+
+                       he->data5 |= HE_PREP(DATA5_DATA_BW_RU_ALLOC,
+                                            status->rate->he_ru_alloc + 4);
+                       break;
+               default:
+                       WARN_ONCE(1, "Invalid SU BW %d\n", status->rate->bw);
+               }
+
+               pos += sizeof(struct ieee80211_radiotap_he);
+       }
+
+       if ((status && status->rate) || info->status.rates[0].idx < 0)
                return;
 
        /* IEEE80211_RADIOTAP_MCS
@@ -655,14 +805,14 @@ void ieee80211_tx_monitor(struct ieee80211_local *local, struct sk_buff *skb,
        int rtap_len;
 
        /* send frame to monitor interfaces now */
-       rtap_len = ieee80211_tx_radiotap_len(info);
+       rtap_len = ieee80211_tx_radiotap_len(info, status);
        if (WARN_ON_ONCE(skb_headroom(skb) < rtap_len)) {
                pr_err("ieee80211_tx_status: headroom too small\n");
                dev_kfree_skb(skb);
                return;
        }
        ieee80211_add_tx_radiotap_header(local, sband, skb, retry_count,
-                                        rtap_len, shift);
+                                        rtap_len, shift, status);
 
        /* XXX: is this sufficient for BPF? */
        skb_reset_mac_header(skb);