rtl8xxxu: Improve handling of txdesc32 vs txdesc40 handling

Further correct the handling of 40 byte TX descriptors.

Signed-off-by: Jes Sorensen <Jes.Sorensen@redhat.com>
Signed-off-by: Kalle Valo <kvalo@codeaurora.org>
diff --git a/drivers/net/wireless/realtek/rtl8xxxu/rtl8xxxu.c b/drivers/net/wireless/realtek/rtl8xxxu/rtl8xxxu.c
index 4fbf445..927fd3b 100644
--- a/drivers/net/wireless/realtek/rtl8xxxu/rtl8xxxu.c
+++ b/drivers/net/wireless/realtek/rtl8xxxu/rtl8xxxu.c
@@ -7018,15 +7018,6 @@
 		}
 	}
 
-	if (rate_flag & IEEE80211_TX_RC_MCS)
-		rate = tx_info->control.rates[0].idx + DESC_RATE_MCS0;
-	else
-		rate = tx_rate->hw_value;
-	tx_desc->txdw5 = cpu_to_le32(rate);
-
-	if (ieee80211_is_data(hdr->frame_control))
-		tx_desc->txdw5 |= cpu_to_le32(0x0001ff00);
-
 	/* (tx_info->flags & IEEE80211_TX_CTL_AMPDU) && */
 	ampdu_enable = false;
 	if (ieee80211_is_data_qos(hdr->frame_control) && sta) {
@@ -7041,8 +7032,18 @@
 		}
 	}
 
+	if (rate_flag & IEEE80211_TX_RC_MCS)
+		rate = tx_info->control.rates[0].idx + DESC_RATE_MCS0;
+	else
+		rate = tx_rate->hw_value;
+
 	seq_number = IEEE80211_SEQ_TO_SN(le16_to_cpu(hdr->seq_ctrl));
 	if (!usedesc40) {
+		tx_desc->txdw5 = cpu_to_le32(rate);
+
+		if (ieee80211_is_data(hdr->frame_control))
+			tx_desc->txdw5 |= cpu_to_le32(0x0001ff00);
+
 		tx_desc->txdw3 =
 			cpu_to_le32((u32)seq_number << TXDESC_SEQ_SHIFT_8723A);
 
@@ -7050,9 +7051,56 @@
 			tx_desc->txdw1 |= cpu_to_le32(TXDESC_AGG_ENABLE_8723A);
 		else
 			tx_desc->txdw1 |= cpu_to_le32(TXDESC_AGG_BREAK_8723A);
+
+		if (ieee80211_is_mgmt(hdr->frame_control)) {
+			tx_desc->txdw5 = cpu_to_le32(tx_rate->hw_value);
+			tx_desc->txdw4 |=
+				cpu_to_le32(TXDESC_USE_DRIVER_RATE_8723A);
+			tx_desc->txdw5 |=
+				cpu_to_le32(6 <<
+					    TXDESC_RETRY_LIMIT_SHIFT_8723A);
+			tx_desc->txdw5 |=
+				cpu_to_le32(TXDESC_RETRY_LIMIT_ENABLE_8723A);
+		}
+
+		if (ieee80211_is_data_qos(hdr->frame_control))
+			tx_desc->txdw4 |= cpu_to_le32(TXDESC_QOS_8723A);
+
+		if (rate_flag & IEEE80211_TX_RC_USE_SHORT_PREAMBLE ||
+		    (sta && vif && vif->bss_conf.use_short_preamble))
+			tx_desc->txdw4 |=
+				cpu_to_le32(TXDESC_SHORT_PREAMBLE_8723A);
+
+		if (rate_flag & IEEE80211_TX_RC_SHORT_GI ||
+		    (ieee80211_is_data_qos(hdr->frame_control) &&
+		     sta && sta->ht_cap.cap &
+		     (IEEE80211_HT_CAP_SGI_40 | IEEE80211_HT_CAP_SGI_20))) {
+			tx_desc->txdw5 |= cpu_to_le32(TXDESC_SHORT_GI);
+		}
+
+		if (rate_flag & IEEE80211_TX_RC_USE_RTS_CTS) {
+			/*
+			 * Use RTS rate 24M - does the mac80211 tell
+			 * us which to use?
+			 */
+			tx_desc->txdw4 |=
+				cpu_to_le32(DESC_RATE_24M <<
+					    TXDESC_RTS_RATE_SHIFT_8723A);
+			tx_desc->txdw4 |=
+				cpu_to_le32(TXDESC_RTS_CTS_ENABLE_8723A);
+			tx_desc->txdw4 |=
+				cpu_to_le32(TXDESC_HW_RTS_ENABLE_8723A);
+		}
 	} else {
 		tx_desc40 = (struct rtl8723bu_tx_desc *)tx_desc;
 
+		tx_desc40->txdw4 = cpu_to_le32(rate);
+		if (ieee80211_is_data(hdr->frame_control)) {
+			tx_desc->txdw4 |=
+				cpu_to_le32(0x1f <<
+					    TXDESC_DATA_RATE_FB_SHIFT_8723B);
+		}
+
 		tx_desc40->txdw9 =
 			cpu_to_le32((u32)seq_number << TXDESC_SEQ_SHIFT_8723B);
 
@@ -7061,35 +7109,38 @@
 				cpu_to_le32(TXDESC_AGG_ENABLE_8723B);
 		else
 			tx_desc40->txdw2 |= cpu_to_le32(TXDESC_AGG_BREAK_8723B);
+
+		if (ieee80211_is_mgmt(hdr->frame_control)) {
+			tx_desc40->txdw4 = cpu_to_le32(tx_rate->hw_value);
+			tx_desc40->txdw3 |=
+				cpu_to_le32(TXDESC_USE_DRIVER_RATE_8723B);
+			tx_desc40->txdw4 |=
+				cpu_to_le32(6 <<
+					    TXDESC_RETRY_LIMIT_SHIFT_8723B);
+			tx_desc40->txdw4 |=
+				cpu_to_le32(TXDESC_RETRY_LIMIT_ENABLE_8723B);
+		}
+
+		if (rate_flag & IEEE80211_TX_RC_USE_SHORT_PREAMBLE ||
+		    (sta && vif && vif->bss_conf.use_short_preamble))
+			tx_desc40->txdw5 |=
+				cpu_to_le32(TXDESC_SHORT_PREAMBLE_8723B);
+
+		if (rate_flag & IEEE80211_TX_RC_USE_RTS_CTS) {
+			/*
+			 * Use RTS rate 24M - does the mac80211 tell
+			 * us which to use?
+			 */
+			tx_desc->txdw4 |=
+				cpu_to_le32(DESC_RATE_24M <<
+					    TXDESC_RTS_RATE_SHIFT_8723B);
+			tx_desc->txdw3 |=
+				cpu_to_le32(TXDESC_RTS_CTS_ENABLE_8723B);
+			tx_desc->txdw3 |=
+				cpu_to_le32(TXDESC_HW_RTS_ENABLE_8723B);
+		}
 	};
 
-	if (ieee80211_is_data_qos(hdr->frame_control))
-		tx_desc->txdw4 |= cpu_to_le32(TXDESC_QOS);
-	if (rate_flag & IEEE80211_TX_RC_USE_SHORT_PREAMBLE ||
-	    (sta && vif && vif->bss_conf.use_short_preamble))
-		tx_desc->txdw4 |= cpu_to_le32(TXDESC_SHORT_PREAMBLE);
-	if (rate_flag & IEEE80211_TX_RC_SHORT_GI ||
-	    (ieee80211_is_data_qos(hdr->frame_control) &&
-	     sta && sta->ht_cap.cap &
-	     (IEEE80211_HT_CAP_SGI_40 | IEEE80211_HT_CAP_SGI_20))) {
-		tx_desc->txdw5 |= cpu_to_le32(TXDESC_SHORT_GI);
-	}
-	if (ieee80211_is_mgmt(hdr->frame_control)) {
-		tx_desc->txdw5 = cpu_to_le32(tx_rate->hw_value);
-		tx_desc->txdw4 |= cpu_to_le32(TXDESC_USE_DRIVER_RATE_8723A);
-		tx_desc->txdw5 |=
-			cpu_to_le32(6 << TXDESC_RETRY_LIMIT_SHIFT_8723A);
-		tx_desc->txdw5 |= cpu_to_le32(TXDESC_RETRY_LIMIT_ENABLE_8723A);
-	}
-
-	if (rate_flag & IEEE80211_TX_RC_USE_RTS_CTS) {
-		/* Use RTS rate 24M - does the mac80211 tell us which to use? */
-		tx_desc->txdw4 |= cpu_to_le32(DESC_RATE_24M <<
-					      TXDESC_RTS_RATE_SHIFT_8723A);
-		tx_desc->txdw4 |= cpu_to_le32(TXDESC_RTS_CTS_ENABLE_8723A);
-		tx_desc->txdw4 |= cpu_to_le32(TXDESC_HW_RTS_ENABLE_8723A);
-	}
-
 	rtl8xxxu_calc_tx_desc_csum(tx_desc);
 
 	usb_fill_bulk_urb(&tx_urb->urb, priv->udev, priv->pipe_out[queue],