mt7603: fix buffering of tx packets for powersave clients

Use PSE registers to loop back any pending tx packets for a client that just went to sleep back into the MCU rx queue. Re-queue those packets to the tx queue immediately after a client wakes up. Signed-off-by: Felix Fietkau <[email protected]>
2026-10-24 13:26:48 +00:00 · 2018-11-20 16:18:22 +01:00
parent 82363abeaf
commit c09e8a489f
5 changed files with 210 additions and 29 deletions
@@ -61,6 +61,42 @@ mt7603_init_tx_queue(struct mt7603_dev *dev, struct mt76_queue *q,
 	return 0;
 }

+static void
+mt7603_rx_loopback_skb(struct mt7603_dev *dev, struct sk_buff *skb)
+{
+	__le32 *txd = (__le32 *)skb->data;
+	struct mt7603_sta *msta;
+	struct mt76_wcid *wcid;
+	int idx;
+	u32 val;
+
+	if (skb->len < sizeof(MT_TXD_SIZE) + sizeof(struct ieee80211_hdr))
+		goto free;
+
+	val = le32_to_cpu(txd[1]);
+	idx = FIELD_GET(MT_TXD1_WLAN_IDX, val);
+	skb->priority = FIELD_GET(MT_TXD1_TID, val);
+
+	if (idx >= MT7603_WTBL_STA - 1)
+		goto free;
+
+	wcid = rcu_dereference(dev->mt76.wcid[idx]);
+	if (!wcid)
+		goto free;
+
+	msta = container_of(wcid, struct mt7603_sta, wcid);
+	val = le32_to_cpu(txd[0]);
+	skb_set_queue_mapping(skb, FIELD_GET(MT_TXD0_Q_IDX, val));
+
+	spin_lock_bh(&dev->ps_lock);
+	__skb_queue_tail(&msta->psq, skb);
+	spin_unlock_bh(&dev->ps_lock);
+	return;
+
+free:
+	dev_kfree_skb(skb);
+}
+
 void mt7603_queue_rx_skb(struct mt76_dev *mdev, enum mt76_rxq_id q,
 			 struct sk_buff *skb)
 {
@@ -71,6 +107,14 @@ void mt7603_queue_rx_skb(struct mt76_dev *mdev, enum mt76_rxq_id q,

 	type = FIELD_GET(MT_RXD0_PKT_TYPE, le32_to_cpu(rxd[0]));

+	if (q == MT_RXQ_MCU) {
+		if (type == PKT_TYPE_RX_EVENT)
+			mt7603_mcu_rx_event(dev, skb);
+		else
+			mt7603_rx_loopback_skb(dev, skb);
+		return;
+	}
+
 	switch (type) {
 	case PKT_TYPE_TXS:
 		for (rxd++; rxd + 5 <= end; rxd += 5)
@@ -156,32 +156,83 @@ void mt7603_wtbl_init(struct mt7603_dev *dev, int idx, int vif,
 		mt76_wr(dev, addr + i, 0);
 }

-void mt7603_wtbl_set_ps(struct mt7603_dev *dev, int idx, bool enabled)
+static void
+mt7603_wtbl_set_skip_tx(struct mt7603_dev *dev, int idx, bool enabled)
 {
 	u32 addr = mt7603_wtbl1_addr(idx);
-	u32 reg = mt76_rr(dev, addr + 3 * 4);
-	u32 val = (reg & ~MT_WTBL1_W3_POWER_SAVE) |
-		  (enabled * MT_WTBL1_W3_POWER_SAVE);
+	u32 val = mt76_rr(dev, addr + 3 * 4);

-	if (reg == val)
-		return;
+	val &= ~MT_WTBL1_W3_SKIP_TX;
+	val |= enabled * MT_WTBL1_W3_SKIP_TX;

-	mt76_set(dev, MT_WTBL1_OR, MT_WTBL1_OR_PSM_WRITE);
 	mt76_wr(dev, addr + 3 * 4, val);
-	mt76_clear(dev, MT_WTBL1_OR, MT_WTBL1_OR_PSM_WRITE);
 }

-void mt7603_wtbl_set_skip_tx(struct mt7603_dev *dev, int idx, bool enabled)
+void mt7603_filter_tx(struct mt7603_dev *dev, int idx, bool abort)
 {
-	u32 addr = mt7603_wtbl1_addr(idx);
-	u32 reg = mt76_rr(dev, addr + 3 * 4);
-	u32 val = (reg & ~MT_WTBL1_W3_SKIP_TX) |
-		  (enabled * MT_WTBL1_W3_SKIP_TX);
+	int i, port, queue;

-	if (reg == val)
-		return;
+	if (abort) {
+		port = 3; /* PSE */
+		queue = 8; /* free queue */
+	} else {
+		port = 0; /* HIF */
+		queue = 1; /* MCU queue */
+	}

-	mt76_wr(dev, addr + 3 * 4, val);
+	mt7603_wtbl_set_skip_tx(dev, idx, true);
+
+	mt76_wr(dev, MT_TX_ABORT, MT_TX_ABORT_EN |
+			FIELD_PREP(MT_TX_ABORT_WCID, idx));
+
+	for (i = 0; i < 4; i++) {
+		mt76_wr(dev, MT_DMA_FQCR0, MT_DMA_FQCR0_BUSY |
+			FIELD_PREP(MT_DMA_FQCR0_TARGET_WCID, idx) |
+			FIELD_PREP(MT_DMA_FQCR0_TARGET_QID, i) |
+			FIELD_PREP(MT_DMA_FQCR0_DEST_PORT_ID, port) |
+			FIELD_PREP(MT_DMA_FQCR0_DEST_QUEUE_ID, queue));
+
+		WARN_ON_ONCE(!mt76_poll(dev, MT_DMA_FQCR0, MT_DMA_FQCR0_BUSY,
+					0, 5000));
+	}
+
+	mt76_wr(dev, MT_TX_ABORT, 0);
+
+	mt7603_wtbl_set_skip_tx(dev, idx, false);
+}
+
+void mt7603_wtbl_set_ps(struct mt7603_dev *dev, struct mt7603_sta *sta,
+			bool enabled)
+{
+	int idx = sta->wcid.idx;
+	u32 addr;
+
+	spin_lock_bh(&dev->ps_lock);
+
+	if (sta->ps == enabled)
+		goto out;
+
+	mt76_wr(dev, MT_PSE_RTA,
+		FIELD_PREP(MT_PSE_RTA_TAG_ID, idx) |
+		FIELD_PREP(MT_PSE_RTA_PORT_ID, 0) |
+		FIELD_PREP(MT_PSE_RTA_QUEUE_ID, 1) |
+		FIELD_PREP(MT_PSE_RTA_REDIRECT_EN, enabled) |
+		MT_PSE_RTA_WRITE | MT_PSE_RTA_BUSY);
+
+	mt76_poll(dev, MT_PSE_RTA, MT_PSE_RTA_BUSY, 0, 5000);
+
+	if (enabled)
+		mt7603_filter_tx(dev, idx, false);
+
+	addr = mt7603_wtbl1_addr(idx);
+	mt76_set(dev, MT_WTBL1_OR, MT_WTBL1_OR_PSM_WRITE);
+	mt76_rmw(dev, addr + 3 * 4, MT_WTBL1_W3_POWER_SAVE,
+		 enabled * MT_WTBL1_W3_POWER_SAVE);
+	mt76_clear(dev, MT_WTBL1_OR, MT_WTBL1_OR_PSM_WRITE);
+	sta->ps = enabled;
+
+out:
+	spin_unlock_bh(&dev->ps_lock);
 }

 void mt7603_wtbl_clear(struct mt7603_dev *dev, int idx)
@@ -882,13 +933,16 @@ int mt7603_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	struct ieee80211_key_conf *key = info->control.hw_key;
 	int pid;

-	if (!wcid) {
+	if (!wcid)
 		wcid = &dev->global_sta.wcid;
-	} else {
+
+	if (sta) {
+		msta = (struct mt7603_sta *) sta->drv_priv;
+
 		if ((info->flags & (IEEE80211_TX_CTL_NO_PS_BUFFER |
-				    IEEE80211_TX_CTL_CLEAR_PS_FILT)) &&
-		    test_bit(MT_WCID_FLAG_PS, &wcid->flags))
-			mt7603_wtbl_set_skip_tx(dev, wcid->idx, false);
+				    IEEE80211_TX_CTL_CLEAR_PS_FILT)) ||
+		    (info->control.flags & IEEE80211_TX_CTRL_PS_RESPONSE))
+			mt7603_wtbl_set_ps(dev, msta, false);
 	}

 	pid = mt76_tx_status_skb_add(mdev, wcid, skb);
@@ -95,7 +95,6 @@ mt7603_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)

 	eth_broadcast_addr(bc_addr);
 	mt7603_wtbl_init(dev, idx, mvif->idx, bc_addr);
-	mt7603_wtbl_set_skip_tx(dev, idx, false);

 	rcu_assign_pointer(dev->mt76.wcid[idx], &mvif->sta.wcid);
 	mt7603_txq_init(dev, vif->txq);
@@ -307,10 +306,12 @@ mt7603_sta_add(struct mt76_dev *mdev, struct ieee80211_vif *vif,
 	if (idx < 0)
 		return -ENOSPC;

+	__skb_queue_head_init(&msta->psq);
+	msta->ps = ~0;
 	msta->wcid.sta = 1;
 	msta->wcid.idx = idx;
 	mt7603_wtbl_init(dev, idx, mvif->idx, sta->addr);
-	mt7603_wtbl_set_skip_tx(dev, idx, false);
+	mt7603_wtbl_set_ps(dev, msta, false);
 	mt7603_wtbl_update_cap(dev, sta);

 	if (vif->type == NL80211_IFTYPE_AP)
@@ -324,21 +325,80 @@ mt7603_sta_remove(struct mt76_dev *mdev, struct ieee80211_vif *vif,
 		  struct ieee80211_sta *sta)
 {
 	struct mt7603_dev *dev = container_of(mdev, struct mt7603_dev, mt76);
+	struct mt7603_sta *msta = (struct mt7603_sta *)sta->drv_priv;
 	struct mt76_wcid *wcid = (struct mt76_wcid *) sta->drv_priv;

-	mt7603_wtbl_set_skip_tx(dev, wcid->idx, true);
+	spin_lock_bh(&dev->ps_lock);
+	__skb_queue_purge(&msta->psq);
+	mt7603_filter_tx(dev, wcid->idx, true);
+	spin_unlock_bh(&dev->ps_lock);
+
 	mt7603_wtbl_init(dev, wcid->idx, 0, NULL);
 }

+static void
+mt7603_ps_tx_list(struct mt7603_dev *dev, struct sk_buff_head *list)
+{
+	struct sk_buff *skb;
+
+	while ((skb = __skb_dequeue(list)) != NULL)
+		mt7603_tx_queue_mcu(dev, skb_get_queue_mapping(skb), skb);
+}
+
 void
 mt7603_sta_ps(struct mt76_dev *mdev, struct ieee80211_sta *sta, bool ps)
 {
 	struct mt7603_dev *dev = container_of(mdev, struct mt7603_dev, mt76);
 	struct mt7603_sta *msta = (struct mt7603_sta *)sta->drv_priv;
-	int idx = msta->wcid.idx;
+	struct sk_buff_head list;

 	mt76_stop_tx_queues(&dev->mt76, sta, false);
-	mt7603_wtbl_set_skip_tx(dev, idx, ps);
+	mt7603_wtbl_set_ps(dev, msta, ps);
+	if (ps)
+		return;
+
+	__skb_queue_head_init(&list);
+
+	spin_lock_bh(&dev->ps_lock);
+	skb_queue_splice_tail_init(&msta->psq, &list);
+	spin_unlock_bh(&dev->ps_lock);
+
+	mt7603_ps_tx_list(dev, &list);
+}
+
+static void
+mt7603_release_buffered_frames(struct ieee80211_hw *hw,
+			       struct ieee80211_sta *sta,
+			       u16 tids, int nframes,
+			       enum ieee80211_frame_release_type reason,
+			       bool more_data)
+{
+	struct mt7603_dev *dev = hw->priv;
+	struct mt7603_sta *msta = (struct mt7603_sta *)sta->drv_priv;
+	struct sk_buff_head list;
+	struct sk_buff *skb, *tmp;
+
+	__skb_queue_head_init(&list);
+
+	spin_lock_bh(&dev->ps_lock);
+	skb_queue_walk_safe(&msta->psq, skb, tmp) {
+		if (!nframes)
+			break;
+
+		if (!(tids & BIT(skb->priority)))
+			continue;
+
+		skb_set_queue_mapping(skb, MT_TXQ_PSD);
+		__skb_unlink(skb, &msta->psq);
+		__skb_queue_tail(&list, skb);
+	}
+	spin_unlock_bh(&dev->ps_lock);
+
+	mt7603_ps_tx_list(dev, &list);
+
+	if (nframes)
+		mt76_release_buffered_frames(hw, sta, tids, nframes, reason,
+					     more_data);
 }

 static int
@@ -606,7 +666,7 @@ const struct ieee80211_ops mt7603_ops = {
 	.get_txpower = mt7603_get_txpower,
 	.wake_tx_queue = mt76_wake_tx_queue,
 	.sta_rate_tbl_update = mt7603_sta_rate_tbl_update,
-	.release_buffered_frames = mt76_release_buffered_frames,
+	.release_buffered_frames = mt7603_release_buffered_frames,
 	.set_coverage_class = mt7603_set_coverage_class,
 	.set_tim = mt7603_set_tim,
 	.get_survey = mt76_get_survey,
@@ -65,6 +65,9 @@ struct mt7603_sta {
 	struct mt76_wcid wcid; /* must be first */

 	struct mt7603_vif *vif;
+
+	struct sk_buff_head psq;
+
 	struct ieee80211_tx_rate rates[8];
 	u8 rate_count;
 	u8 n_rates;
@@ -74,6 +77,8 @@ struct mt7603_sta {
 	u8 ampdu_count;
 	u8 ampdu_tx_count;
 	u8 ampdu_acked;
+
+	u8 ps;
 };

 struct mt7603_vif {
@@ -116,6 +121,8 @@ struct mt7603_dev {

 	struct mt76_queue q_rx;

+	spinlock_t ps_lock;
+
 	u8 mcu_running;

 	u8 beacon_mask;
@@ -210,8 +217,9 @@ void mt7603_wtbl_set_rates(struct mt7603_dev *dev, struct mt7603_sta *sta,
 			   struct ieee80211_tx_rate *rates);
 int mt7603_wtbl_set_key(struct mt7603_dev *dev, int wcid,
 			struct ieee80211_key_conf *key);
-void mt7603_wtbl_set_ps(struct mt7603_dev *dev, int idx, bool enabled);
-void mt7603_wtbl_set_skip_tx(struct mt7603_dev *dev, int idx, bool val);
+void mt7603_wtbl_set_ps(struct mt7603_dev *dev, struct mt7603_sta *sta,
+			bool enabled);
+void mt7603_filter_tx(struct mt7603_dev *dev, int idx, bool abort);

 int mt7603_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 			  struct sk_buff *skb, struct mt76_queue *q,
@@ -218,6 +218,17 @@

 #define MT_DMA_DCR0			MT_WF_DMA(0x000)
 #define MT_DMA_DCR1			MT_WF_DMA(0x004)
+
+#define MT_DMA_FQCR0			MT_WF_DMA(0x008)
+#define MT_DMA_FQCR0_TARGET_WCID	GENMASK(7, 0)
+#define MT_DMA_FQCR0_TARGET_BSS		GENMASK(13, 8)
+#define MT_DMA_FQCR0_TARGET_QID		GENMASK(20, 16)
+#define MT_DMA_FQCR0_DEST_PORT_ID	GENMASK(23, 22)
+#define MT_DMA_FQCR0_DEST_QUEUE_ID	GENMASK(28, 24)
+#define MT_DMA_FQCR0_MODE		BIT(29)
+#define MT_DMA_FQCR0_STATUS		BIT(30)
+#define MT_DMA_FQCR0_BUSY		BIT(31)
+
 #define MT_DMA_RCFR0			MT_WF_DMA(0x070)
 #define MT_DMA_VCFR0			MT_WF_DMA(0x07c)

@@ -306,6 +317,10 @@ enum {
 					 ((n) > 4 ? (n) - 4 : \
 					  (n) ? (n) + 3 : 0)) * 4)

+#define MT_TX_ABORT			MT_WF_ARB(0x134)
+#define MT_TX_ABORT_EN			BIT(0)
+#define MT_TX_ABORT_WCID		GENMASK(15, 8)
+
 #define MT_WF_TMAC_BASE			0x21600
 #define MT_WF_TMAC(ofs)			(MT_WF_TMAC_BASE + (ofs))