iwlwifi: mvm: add radiotap data for HE

Add HE information to the radiotap data.
Signed-off-by: NLuca Coelho <luciano.coelho@intel.com>

drivers/net/wireless/intel/iwlwifi/mvm/rxmq.c

@@ -198,10 +198,20 @@ static void iwl_mvm_pass_packet_to_mac80211(struct iwl_mvm *mvm,
 					    struct sk_buff *skb, int queue,
 					    struct ieee80211_sta *sta)
 {
-	if (iwl_mvm_check_pn(mvm, skb, queue, sta))
+	struct ieee80211_rx_status *rx_status = IEEE80211_SKB_RXCB(skb);
+
+	if (iwl_mvm_check_pn(mvm, skb, queue, sta)) {
 		kfree_skb(skb);
-	else
+	} else {
+		unsigned int radiotap_len = 0;
+
+		if (rx_status->flag & RX_FLAG_RADIOTAP_HE)
+			radiotap_len += sizeof(struct ieee80211_radiotap_he);
+		if (rx_status->flag & RX_FLAG_RADIOTAP_HE_MU)
+			radiotap_len += sizeof(struct ieee80211_radiotap_he_mu);
+		__skb_push(skb, radiotap_len);
 		ieee80211_rx_napi(mvm->hw, sta, skb, napi);
+	}
 }
 
 static void iwl_mvm_get_signal_strength(struct iwl_mvm *mvm,
@@ -859,6 +869,8 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 	struct ieee80211_sta *sta = NULL;
 	struct sk_buff *skb;
 	u8 crypt_len = 0;
+	struct ieee80211_radiotap_he *he = NULL;
+	struct ieee80211_radiotap_he_mu *he_mu = NULL;
 	u32 he_type = 0xffffffff;
 	/* this is invalid e.g. because puncture type doesn't allow 0b11 */
 #define HE_PHY_DATA_INVAL ((u64)-1)
@@ -889,10 +901,43 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 	rx_status = IEEE80211_SKB_RXCB(skb);
 
 	if (rate_n_flags & RATE_MCS_HE_MSK) {
-		if (phy_info & IWL_RX_MPDU_PHY_TSF_OVERLOAD)
+		static const struct ieee80211_radiotap_he known = {
+			.data1 = cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA1_DATA_MCS_KNOWN |
+					     IEEE80211_RADIOTAP_HE_DATA1_DATA_DCM_KNOWN |
+					     IEEE80211_RADIOTAP_HE_DATA1_STBC_KNOWN |
+					     IEEE80211_RADIOTAP_HE_DATA1_CODING_KNOWN),
+			.data2 = cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA2_GI_KNOWN |
+					     IEEE80211_RADIOTAP_HE_DATA2_TXBF_KNOWN),
+		};
+		static const struct ieee80211_radiotap_he_mu mu_known = {
+			.flags1 = cpu_to_le16(IEEE80211_RADIOTAP_HE_MU_FLAGS1_SIG_B_MCS_KNOWN |
+					      IEEE80211_RADIOTAP_HE_MU_FLAGS1_SIG_B_DCM_KNOWN |
+					      IEEE80211_RADIOTAP_HE_MU_FLAGS1_SIG_B_SYMS_USERS_KNOWN |
+					      IEEE80211_RADIOTAP_HE_MU_FLAGS1_SIG_B_COMP_KNOWN),
+			.flags2 = cpu_to_le16(IEEE80211_RADIOTAP_HE_MU_FLAGS2_PUNC_FROM_SIG_A_BW_KNOWN),
+		};
+		unsigned int radiotap_len = 0;
+
+		he = skb_put_data(skb, &known, sizeof(known));
+		radiotap_len += sizeof(known);
+		rx_status->flag |= RX_FLAG_RADIOTAP_HE;
+
+		he_type = rate_n_flags & RATE_MCS_HE_TYPE_MSK;
+
+		if (phy_info & IWL_RX_MPDU_PHY_TSF_OVERLOAD) {
 			he_phy_data =
 				le64_to_cpu(desc->he_phy_data);
-		he_type = rate_n_flags & RATE_MCS_HE_TYPE_MSK;
+
+			if (he_type == RATE_MCS_HE_TYPE_MU) {
+				he_mu = skb_put_data(skb, &mu_known,
+						     sizeof(mu_known));
+				radiotap_len += sizeof(mu_known);
+				rx_status->flag |= RX_FLAG_RADIOTAP_HE_MU;
+			}
+		}
+
+		/* temporarily hide the radiotap data */
+		__skb_pull(skb, radiotap_len);
 	}
 
 	if (iwl_mvm_rx_crypto(mvm, hdr, rx_status, phy_info, desc,
@@ -921,6 +966,13 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 		/* TSF as indicated by the firmware is at INA time */
 		rx_status->flag |= RX_FLAG_MACTIME_PLCP_START;
 	} else if (he_type == RATE_MCS_HE_TYPE_SU) {
+		he->data1 |=
+			cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA1_UL_DL_KNOWN);
+		if (FIELD_GET(IWL_RX_HE_PHY_UPLINK,
+			      le64_to_cpu(desc->he_phy_data)))
+			he->data3 |=
+				cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA3_UL_DL);
+
 		if (!queue && !(phy_info & IWL_RX_MPDU_PHY_AMPDU)) {
 			rx_status->ampdu_reference = mvm->ampdu_ref;
 			mvm->ampdu_ref++;
@@ -931,8 +983,28 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 				      le64_to_cpu(desc->he_phy_data)))
 				rx_status->flag |= RX_FLAG_AMPDU_EOF_BIT;
 		}
+	} else if (he_mu && he_phy_data != HE_PHY_DATA_INVAL) {
+		he_mu->flags1 |=
+			le16_encode_bits(FIELD_GET(IWL_RX_HE_PHY_SIBG_SYM_OR_USER_NUM_MASK,
+						   he_phy_data),
+					 IEEE80211_RADIOTAP_HE_MU_FLAGS2_SIG_B_SYMS_USERS);
+		he_mu->flags1 |=
+			le16_encode_bits(FIELD_GET(IWL_RX_HE_PHY_SIGB_DCM,
+						   he_phy_data),
+					 IEEE80211_RADIOTAP_HE_MU_FLAGS1_SIG_B_DCM);
+		he_mu->flags1 |=
+			le16_encode_bits(FIELD_GET(IWL_RX_HE_PHY_SIGB_MCS_MASK,
+						   he_phy_data),
+					 IEEE80211_RADIOTAP_HE_MU_FLAGS1_SIG_B_MCS);
+		he_mu->flags2 |=
+			le16_encode_bits(FIELD_GET(IWL_RX_HE_PHY_SIGB_COMPRESSION,
+						   he_phy_data),
+					 IEEE80211_RADIOTAP_HE_MU_FLAGS2_SIG_B_COMP);
+		he_mu->flags2 |=
+			le16_encode_bits(FIELD_GET(IWL_RX_HE_PHY_PREAMBLE_PUNC_TYPE_MASK,
+						   he_phy_data),
+					 IEEE80211_RADIOTAP_HE_MU_FLAGS2_PUNC_FROM_SIG_A_BW);
 	}
-
 	rx_status->device_timestamp = le32_to_cpu(desc->gp2_on_air_rise);
 	rx_status->band = desc->channel > 14 ? NL80211_BAND_5GHZ :
 					       NL80211_BAND_2GHZ;
@@ -1132,6 +1204,17 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 			rx_status->he_ru = NL80211_RATE_INFO_HE_RU_ALLOC_2x996;
 			break;
 		}
+		he->data2 |=
+			le16_encode_bits(offs,
+					 IEEE80211_RADIOTAP_HE_DATA2_RU_OFFSET);
+		he->data2 |=
+			cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA2_PRISEC_80_KNOWN);
+		if (he_phy_data & IWL_RX_HE_PHY_RU_ALLOC_SEC80)
+			he->data2 |=
+				cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA2_PRISEC_80_SEC);
+	} else if (he) {
+		he->data1 |=
+			cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA1_BW_RU_ALLOC_KNOWN);
 	}
 
 	if (!(rate_n_flags & RATE_MCS_CCK_MSK) &&
@@ -1158,7 +1241,7 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 		rx_status->enc_flags |= stbc << RX_ENC_FLAG_STBC_SHIFT;
 		if (rate_n_flags & RATE_MCS_BF_MSK)
 			rx_status->enc_flags |= RX_ENC_FLAG_BF;
-	} else if (rate_n_flags & RATE_MCS_HE_MSK) {
+	} else if (he) {
 		u8 stbc = (rate_n_flags & RATE_MCS_STBC_MSK) >>
 				RATE_MCS_STBC_POS;
 		rx_status->nss =
@@ -1173,6 +1256,20 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 		rx_status->he_dcm =
 			!!(rate_n_flags & RATE_HE_DUAL_CARRIER_MODE_MSK);
 
+#define CHECK_TYPE(F)							\
+	BUILD_BUG_ON(IEEE80211_RADIOTAP_HE_DATA1_FORMAT_ ## F !=	\
+		     (RATE_MCS_HE_TYPE_ ## F >> RATE_MCS_HE_TYPE_POS))
+
+		CHECK_TYPE(SU);
+		CHECK_TYPE(EXT_SU);
+		CHECK_TYPE(MU);
+		CHECK_TYPE(TRIG);
+
+		he->data1 |= cpu_to_le16(he_type >> RATE_MCS_HE_TYPE_POS);
+
+		if (rate_n_flags & RATE_MCS_BF_POS)
+			he->data5 |= cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA5_TXBF);
+
 		switch ((rate_n_flags & RATE_MCS_HE_GI_LTF_MSK) >>
 			RATE_MCS_HE_GI_LTF_POS) {
 		case 0:
@@ -1191,6 +1288,65 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 				rx_status->he_gi = NL80211_RATE_INFO_HE_GI_3_2;
 			break;
 		}
+
+		switch (he_type) {
+		case RATE_MCS_HE_TYPE_SU: {
+			u16 val;
+
+			/* LTF syms correspond to streams */
+			he->data2 |=
+				cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA2_NUM_LTF_SYMS_KNOWN);
+			switch (rx_status->nss) {
+			case 1:
+				val = 0;
+				break;
+			case 2:
+				val = 1;
+				break;
+			case 3:
+			case 4:
+				val = 2;
+				break;
+			case 5:
+			case 6:
+				val = 3;
+				break;
+			case 7:
+			case 8:
+				val = 4;
+				break;
+			default:
+				WARN_ONCE(1, "invalid nss: %d\n",
+					  rx_status->nss);
+				val = 0;
+			}
+			he->data5 |=
+				le16_encode_bits(val,
+						 IEEE80211_RADIOTAP_HE_DATA5_NUM_LTF_SYMS);
+			}
+			break;
+		case RATE_MCS_HE_TYPE_MU: {
+			u16 val;
+
+			if (he_phy_data == HE_PHY_DATA_INVAL)
+				break;
+
+			val = FIELD_GET(IWL_RX_HE_PHY_HE_LTF_NUM_MASK,
+					le64_to_cpu(desc->he_phy_data));
+
+			he->data2 |=
+				cpu_to_le16(IEEE80211_RADIOTAP_HE_DATA2_NUM_LTF_SYMS_KNOWN);
+			he->data5 |=
+				cpu_to_le16(FIELD_PREP(
+					IEEE80211_RADIOTAP_HE_DATA5_NUM_LTF_SYMS,
+					val));
+			}
+			break;
+		case RATE_MCS_HE_TYPE_EXT_SU:
+		case RATE_MCS_HE_TYPE_TRIG:
+			/* not supported yet */
+			break;
+		}
 	} else {
 		int rate = iwl_mvm_legacy_rate_to_mac80211_idx(rate_n_flags,
 							       rx_status->band);
@@ -1202,6 +1358,7 @@ void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
 			goto out;
 		}
 		rx_status->rate_idx = rate;
+
 	}
 
 	/* management stuff on default queue */