rt2x00: Set MIMO PS flag in tx descriptor for STAs in dynamic SMPS mode

Signed-off-by: Helmut Schaa <helmut.schaa@googlemail.com>
Signed-off-by: Ivo van Doorn <IvDoorn@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index b66e0fd..833b157 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -437,7 +437,8 @@
 	rt2x00_desc_read(txwi, 0, &word);
 	rt2x00_set_field32(&word, TXWI_W0_FRAG,
 			   test_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags));
-	rt2x00_set_field32(&word, TXWI_W0_MIMO_PS, 0);
+	rt2x00_set_field32(&word, TXWI_W0_MIMO_PS,
+			   test_bit(ENTRY_TXD_HT_MIMO_PS, &txdesc->flags));
 	rt2x00_set_field32(&word, TXWI_W0_CF_ACK, 0);
 	rt2x00_set_field32(&word, TXWI_W0_TS,
 			   test_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags));
diff --git a/drivers/net/wireless/rt2x00/rt2x00ht.c b/drivers/net/wireless/rt2x00/rt2x00ht.c
index c004cd3..ad3c7ff 100644
--- a/drivers/net/wireless/rt2x00/rt2x00ht.c
+++ b/drivers/net/wireless/rt2x00/rt2x00ht.c
@@ -54,6 +54,16 @@
 	 */
 	if (txrate->flags & IEEE80211_TX_RC_MCS) {
 		txdesc->mcs = txrate->idx;
+
+		/*
+		 * MIMO PS should be set to 1 for STA's using dynamic SM PS
+		 * when using more then one tx stream (>MCS7).
+		 */
+		if (tx_info->control.sta && txdesc->mcs > 7 &&
+		    (tx_info->control.sta->ht_cap.cap &
+		     (WLAN_HT_CAP_SM_PS_DYNAMIC <<
+		      IEEE80211_HT_CAP_SM_PS_SHIFT)))
+			__set_bit(ENTRY_TXD_HT_MIMO_PS, &txdesc->flags);
 	} else {
 		txdesc->mcs = rt2x00_get_rate_mcs(hwrate->mcs);
 		if (txrate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.h b/drivers/net/wireless/rt2x00/rt2x00queue.h
index 38b47919..2d3bf84 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.h
@@ -268,6 +268,7 @@
  * @ENTRY_TXD_HT_AMPDU: This frame is part of an AMPDU.
  * @ENTRY_TXD_HT_BW_40: Use 40MHz Bandwidth.
  * @ENTRY_TXD_HT_SHORT_GI: Use short GI.
+ * @ENTRY_TXD_HT_MIMO_PS: The receiving STA is in dynamic SM PS mode.
  */
 enum txentry_desc_flags {
 	ENTRY_TXD_RTS_FRAME,
@@ -286,6 +287,7 @@
 	ENTRY_TXD_HT_AMPDU,
 	ENTRY_TXD_HT_BW_40,
 	ENTRY_TXD_HT_SHORT_GI,
+	ENTRY_TXD_HT_MIMO_PS,
 };
 
 /**