add tx aggregation for mt7663s

index e0603e82e33d..8621c6f579aa 100644
@@ -323,7 +323,7 @@ static int mt7663s_probe(struct sdio_func *func,
 {
 	static const struct mt76_driver_ops drv_ops = {
 		.txwi_size = MT_USB_TXD_SIZE,
-		.drv_flags = MT_DRV_RX_DMA_HDR | MT_DRV_HW_MGMT_TXQ,
+		.drv_flags = MT_DRV_RX_DMA_HDR,
 		.tx_prepare_skb = mt7663_usb_sdio_tx_prepare_skb,
 		.tx_complete_skb = mt7663_usb_sdio_tx_complete_skb,
 		.tx_status_data = mt7663_usb_sdio_tx_status_data,

index 3557df52846f..b2b528cba7f1 100644
@@ -124,16 +124,14 @@ static int mt7663s_tx_update_sched(struct mt76_dev *dev,
 				   bool mcu)
 {
 	struct mt76_sdio *sdio = &dev->sdio;
-	struct mt76_phy *mphy = &dev->phy;
-	struct ieee80211_hdr *hdr;
 	int size, ret = -EBUSY;
 
+	if (!test_bit(MT76_STATE_MCU_RUNNING, &dev->phy.state))
+		return 0;
+
 	size = DIV_ROUND_UP(e->buf_sz + sdio->sched.deficit, MT_PSE_PAGE_SZ);
 
 	if (mcu) {
-		if (!test_bit(MT76_STATE_MCU_RUNNING, &mphy->state))
-			return 0;
-
 		mutex_lock(&sdio->sched.lock);
 		if (sdio->sched.pse_mcu_quota > size) {
 			sdio->sched.pse_mcu_quota -= size;

@@ -144,10 +142,6 @@ static int mt7663s_tx_update_sched(struct mt76_dev *dev,
 		return ret;
 	}
 
-	hdr = (struct ieee80211_hdr *)(e->skb->data + MT_USB_TXD_SIZE);
-	if (ieee80211_is_ctl(hdr->frame_control))
-		return 0;
-
 	mutex_lock(&sdio->sched.lock);
 	if (sdio->sched.pse_data_quota > size &&
 	    sdio->sched.ple_data_quota > 0) {

index b2b528cba7f1..ced7820021c2 100644
@@ -119,52 +119,57 @@ static int mt7663s_rx_run_queue(struct mt76_dev *dev, enum mt76_rxq_id qid,
 	return i;
 }
 
-static int mt7663s_tx_update_sched(struct mt76_dev *dev,
-				   struct mt76_queue_entry *e,
-				   bool mcu)
+static int mt7663s_tx_pick_quota(struct mt76_dev *dev, enum mt76_txq_id qid,
+				 int buf_sz, int *pse_size, int *ple_size)
 {
 	struct mt76_sdio *sdio = &dev->sdio;
-	int size, ret = -EBUSY;
+	int pse_sz;
 
 	if (!test_bit(MT76_STATE_MCU_RUNNING, &dev->phy.state))
 		return 0;
 
-	size = DIV_ROUND_UP(e->buf_sz + sdio->sched.deficit, MT_PSE_PAGE_SZ);
+	pse_sz = DIV_ROUND_UP(buf_sz + sdio->sched.deficit, MT_PSE_PAGE_SZ);
 
-	if (mcu) {
-		mutex_lock(&sdio->sched.lock);
-		if (sdio->sched.pse_mcu_quota > size) {
-			sdio->sched.pse_mcu_quota -= size;
-			ret = 0;
-		}
-		mutex_unlock(&sdio->sched.lock);
+	if (qid == MT_TXQ_MCU) {
+		if (sdio->sched.pse_mcu_quota < *pse_size + pse_sz)
+			return -EBUSY;
+	} else {
+		if (sdio->sched.pse_data_quota < *pse_size + pse_sz ||
+		    sdio->sched.ple_data_quota < *ple_size)
+			return -EBUSY;
 
-		return ret;
+		*ple_size = *ple_size + 1;
 	}
+	*pse_size = *pse_size + pse_sz;
+
+	return 0;
+}
 
+static void mt7663s_tx_update_quota(struct mt76_sdio *sdio, enum mt76_txq_id qid,
+				    int pse_size, int ple_size)
+{
 	mutex_lock(&sdio->sched.lock);
-	if (sdio->sched.pse_data_quota > size &&
-	    sdio->sched.ple_data_quota > 0) {
-		sdio->sched.pse_data_quota -= size;
-		sdio->sched.ple_data_quota--;
-		ret = 0;
+	if (qid == MT_TXQ_MCU) {
+		sdio->sched.pse_mcu_quota -= pse_size;
+	} else {
+		sdio->sched.pse_data_quota -= pse_size;
+		sdio->sched.ple_data_quota -= ple_size;
 	}
 	mutex_unlock(&sdio->sched.lock);
-
-	return ret;
 }
 
-static int mt7663s_tx_run_queue(struct mt76_dev *dev, struct mt76_queue *q)
+static int mt7663s_tx_run_queue(struct mt76_dev *dev, enum mt76_txq_id qid)
 {
-	bool mcu = q == dev->q_tx[MT_TXQ_MCU];
+	int nframes = 0, pse_sz = 0, ple_sz = 0;
+	struct mt76_queue *q = dev->q_tx[qid];
 	struct mt76_sdio *sdio = &dev->sdio;
-	int nframes = 0;
 
 	while (q->first != q->head) {
 		struct mt76_queue_entry *e = &q->entry[q->first];
 		int err, len = e->skb->len;
 
-		if (mt7663s_tx_update_sched(dev, e, mcu))
+		if (mt7663s_tx_pick_quota(dev, qid, e->buf_sz, &pse_sz,
+					  &ple_sz))
 			break;
 
 		if (len > sdio->func->cur_blksize)

@@ -184,6 +189,7 @@ static int mt7663s_tx_run_queue(struct mt76_dev *dev, struct mt76_queue *q)
 		q->first = (q->first + 1) % q->ndesc;
 		nframes++;
 	}
+	mt7663s_tx_update_quota(sdio, qid, pse_sz, ple_sz);
 
 	return nframes;
 }

@@ -198,7 +204,7 @@ void mt7663s_tx_work(struct work_struct *work)
 	for (i = 0; i < MT_TXQ_MCU_WA; i++) {
 		int ret;
 
-		ret = mt7663s_tx_run_queue(dev, dev->q_tx[i]);
+		ret = mt7663s_tx_run_queue(dev, i);
 		if (ret < 0)
 			break;
 

index ced7820021c2..ebae2a24c45b 100644
@@ -158,6 +158,24 @@ static void mt7663s_tx_update_quota(struct mt76_sdio *sdio, enum mt76_txq_id qid
 	mutex_unlock(&sdio->sched.lock);
 }
 
+static int __mt7663s_xmit_queue(struct mt76_dev *dev, u8 *data, int len)
+{
+	struct mt76_sdio *sdio = &dev->sdio;
+	int err;
+
+	if (len > sdio->func->cur_blksize)
+		len = roundup(len, sdio->func->cur_blksize);
+
+	sdio_claim_host(sdio->func);
+	err = sdio_writesb(sdio->func, MCR_WTDR1, data, len);
+	sdio_release_host(sdio->func);
+
+	if (err)
+		dev_err(dev->dev, "sdio write failed: %d\n", err);
+
+	return err;
+}
+
 static int mt7663s_tx_run_queue(struct mt76_dev *dev, enum mt76_txq_id qid)
 {
 	int nframes = 0, pse_sz = 0, ple_sz = 0;

@@ -166,24 +184,15 @@ static int mt7663s_tx_run_queue(struct mt76_dev *dev, enum mt76_txq_id qid)
 
 	while (q->first != q->head) {
 		struct mt76_queue_entry *e = &q->entry[q->first];
-		int err, len = e->skb->len;
+		int err;
 
 		if (mt7663s_tx_pick_quota(dev, qid, e->buf_sz, &pse_sz,
 					  &ple_sz))
 			break;
 
-		if (len > sdio->func->cur_blksize)
-			len = roundup(len, sdio->func->cur_blksize);
-
-		/* TODO: skb_walk_frags and then write to SDIO port */
-		sdio_claim_host(sdio->func);
-		err = sdio_writesb(sdio->func, MCR_WTDR1, e->skb->data, len);
-		sdio_release_host(sdio->func);
-
-		if (err) {
-			dev_err(dev->dev, "sdio write failed: %d\n", err);
-			return -EIO;
-		}
+		err = __mt7663s_xmit_queue(dev, e->skb->data, e->skb->len);
+		if (err)
+			return err;
 
 		e->done = true;
 		q->first = (q->first + 1) % q->ndesc;

index 59eed5374b38..209d23846d4c 100644
@@ -1048,7 +1048,7 @@ mt76u_bulk_msg(struct mt76_dev *dev, void *data, int len, int *actual_len,
 	return usb_bulk_msg(udev, pipe, data, len, actual_len, timeout);
 }
 
-int mt76_skb_adjust_pad(struct sk_buff *skb);
+int mt76_skb_adjust_pad(struct sk_buff *skb, int pad);
 int mt76u_vendor_request(struct mt76_dev *dev, u8 req,
 			 u8 req_type, u16 val, u16 offset,
 			 void *buf, size_t len);

index ebae2a24c45b..4754b3abd0a2 100644
@@ -190,6 +190,8 @@ static int mt7663s_tx_run_queue(struct mt76_dev *dev, enum mt76_txq_id qid)
 					  &ple_sz))
 			break;
 
+		__skb_put_zero(e->skb, 4);
+
 		err = __mt7663s_xmit_queue(dev, e->skb->data, e->skb->len);
 		if (err)
 			return err;

index adbed373798e..4d8be366af31 100644
@@ -18,8 +18,7 @@ mt7663u_mcu_send_message(struct mt76_dev *mdev, struct sk_buff *skb,
 			 int cmd, bool wait_resp)
 {
 	struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
-	int ret, seq, ep;
-	u32 len;
+	int ret, seq, ep, len, pad;
 
 	mutex_lock(&mdev->mcu.mutex);
 

@@ -31,7 +30,8 @@ mt7663u_mcu_send_message(struct mt76_dev *mdev, struct sk_buff *skb,
 
 	len = skb->len;
 	put_unaligned_le32(len, skb_push(skb, sizeof(len)));
-	ret = mt76_skb_adjust_pad(skb);
+	pad = round_up(skb->len, 4) + 4 - skb->len;
+	ret = mt76_skb_adjust_pad(skb, pad);
 	if (ret < 0)
 		goto out;
 

index 56d82bd3a615..c24f26499537 100644
@@ -247,6 +247,7 @@ int mt7663_usb_sdio_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
 	struct sk_buff *skb = tx_info->skb;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	int pad;
 
 	if ((info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE) &&
 	    !msta->rate_probe) {

@@ -262,9 +263,12 @@ int mt7663_usb_sdio_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 		u32 len = skb->len;
 
 		put_unaligned_le32(len, skb_push(skb, sizeof(len)));
+		pad = round_up(skb->len, 4) + 4 - skb->len;
+	} else {
+		pad = round_up(skb->len, 4) - skb->len;
 	}
 
-	return mt76_skb_adjust_pad(skb);
+	return mt76_skb_adjust_pad(skb, pad);
 }
 EXPORT_SYMBOL_GPL(mt7663_usb_sdio_tx_prepare_skb);
 

index 5d0a2857f193..2c2f56112b57 100644
@@ -45,7 +45,7 @@ EXPORT_SYMBOL_GPL(mt76x02u_mac_start);
 
 int mt76x02u_skb_dma_info(struct sk_buff *skb, int port, u32 flags)
 {
-	u32 info;
+	u32 info, pad;
 
 	/* Buffer layout:
 	 *	|   4B   | xfer len |      pad       |  4B  |

@@ -57,7 +57,8 @@ int mt76x02u_skb_dma_info(struct sk_buff *skb, int port, u32 flags)
 	       FIELD_PREP(MT_TXD_INFO_DPORT, port) | flags;
 	put_unaligned_le32(info, skb_push(skb, sizeof(info)));
 
-	return mt76_skb_adjust_pad(skb);
+	pad = round_up(skb->len, 4) + 4 - skb->len;
+	return mt76_skb_adjust_pad(skb, pad);
 }
 
 int mt76x02u_tx_prepare_skb(struct mt76_dev *mdev, void *data,

index 7777d4944081..9a4d95a2a707 100644
@@ -226,12 +226,13 @@ mt76s_tx_queue_skb_raw(struct mt76_dev *dev, enum mt76_txq_id qid,
 		       struct sk_buff *skb, u32 tx_info)
 {
 	struct mt76_queue *q = dev->q_tx[qid];
-	int ret = -ENOSPC, len = skb->len;
+	int ret = -ENOSPC, len = skb->len, pad;
 
 	if (q->queued == q->ndesc)
 		goto error;
 
-	ret = mt76_skb_adjust_pad(skb);
+	pad = round_up(skb->len, 4) - skb->len;
+	ret = mt76_skb_adjust_pad(skb, pad);
 	if (ret)
 		goto error;
 

index b6e52a324e28..ce15efcf7e3a 100644
@@ -694,13 +694,9 @@ u8 mt76_ac_to_hwq(u8 ac)
 }
 EXPORT_SYMBOL_GPL(mt76_ac_to_hwq);
 
-int mt76_skb_adjust_pad(struct sk_buff *skb)
+int mt76_skb_adjust_pad(struct sk_buff *skb, int pad)
 {
 	struct sk_buff *iter, *last = skb;
-	u32 pad;
-
-	/* Add zero pad of 4 - 7 bytes */
-	pad = round_up(skb->len, 4) + 4 - skb->len;
 
 	/* First packet of a A-MSDU burst keeps track of the whole burst
 	 * length, need to update length of it and the last packet.

index 4754b3abd0a2..4033fe431312 100644
@@ -19,21 +19,40 @@
 #include "sdio.h"
 #include "mac.h"
 
-static void mt7663s_refill_sched_quota(struct mt76_dev *dev, u32 *data)
+static int mt7663s_refill_sched_quota(struct mt76_dev *dev, u32 *data)
 {
+	u32 ple_ac_data_quota[] = {
+		FIELD_GET(TXQ_CNT_L, data[4]), /* VO */
+		FIELD_GET(TXQ_CNT_H, data[3]), /* VI */
+		FIELD_GET(TXQ_CNT_L, data[3]), /* BE */
+		FIELD_GET(TXQ_CNT_H, data[2]), /* BK */
+	};
+	u32 pse_ac_data_quota[] = {
+		FIELD_GET(TXQ_CNT_H, data[1]), /* VO */
+		FIELD_GET(TXQ_CNT_L, data[1]), /* VI */
+		FIELD_GET(TXQ_CNT_H, data[0]), /* BE */
+		FIELD_GET(TXQ_CNT_L, data[0]), /* BK */
+	};
+	u32 pse_mcu_quota = FIELD_GET(TXQ_CNT_L, data[2]);
+	u32 pse_data_quota = 0, ple_data_quota = 0;
 	struct mt76_sdio *sdio = &dev->sdio;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pse_ac_data_quota); i++) {
+		pse_data_quota += pse_ac_data_quota[i];
+		ple_data_quota += ple_ac_data_quota[i];
+	}
+
+	if (!pse_data_quota && !ple_data_quota && !pse_mcu_quota)
+		return 0;
 
 	mutex_lock(&sdio->sched.lock);
-	sdio->sched.pse_data_quota += FIELD_GET(TXQ_CNT_L, data[0]) + /* BK */
-				      FIELD_GET(TXQ_CNT_H, data[0]) + /* BE */
-				      FIELD_GET(TXQ_CNT_L, data[1]) + /* VI */
-				      FIELD_GET(TXQ_CNT_H, data[1]);  /* VO */
-	sdio->sched.ple_data_quota += FIELD_GET(TXQ_CNT_H, data[2]) + /* BK */
-				      FIELD_GET(TXQ_CNT_L, data[3]) + /* BE */
-				      FIELD_GET(TXQ_CNT_H, data[3]) + /* VI */
-				      FIELD_GET(TXQ_CNT_L, data[4]);  /* VO */
-	sdio->sched.pse_mcu_quota += FIELD_GET(TXQ_CNT_L, data[2]);
+	sdio->sched.pse_mcu_quota += pse_mcu_quota;
+	sdio->sched.pse_data_quota += pse_data_quota;
+	sdio->sched.ple_data_quota += ple_data_quota;
 	mutex_unlock(&sdio->sched.lock);
+
+	return pse_data_quota + ple_data_quota + pse_mcu_quota;
 }
 
 static struct sk_buff *mt7663s_build_rx_skb(void *data, int data_len,

@@ -259,10 +278,8 @@ void mt7663s_rx_work(struct work_struct *work)
 		}
 	}
 
-	if (intr->isr & WHIER_TX_DONE_INT_EN) {
-		mt7663s_refill_sched_quota(dev, intr->tx.wtqcr);
+	if (mt7663s_refill_sched_quota(dev, intr->tx.wtqcr))
 		queue_work(sdio->txrx_wq, &sdio->tx.xmit_work);
-	}
 
 	if (nframes) {
 		queue_work(sdio->txrx_wq, &sdio->rx.recv_work);

index 7014846fd854..bd692431b07f 100644
@@ -307,10 +307,7 @@ mt76_phy_init(struct mt76_dev *dev, struct ieee80211_hw *hw)
 
 	if (!(dev->drv->drv_flags & MT_DRV_AMSDU_OFFLOAD)) {
 		ieee80211_hw_set(hw, TX_AMSDU);
-
-		/* TODO: avoid linearization for SDIO */
-		if (!mt76_is_sdio(dev))
-			ieee80211_hw_set(hw, TX_FRAG_LIST);
+		ieee80211_hw_set(hw, TX_FRAG_LIST);
 	}
 
 	ieee80211_hw_set(hw, MFP_CAPABLE);

index 209d23846d4c..c4a02b5aa990 100644
@@ -444,6 +444,7 @@ struct mt76_usb {
 	} mcu;
 };
 
+#define MT76S_XMIT_BUF_SZ	(16 * PAGE_SIZE)
 struct mt76_sdio {
 	struct workqueue_struct *txrx_wq;
 	struct {

@@ -457,6 +458,8 @@ struct mt76_sdio {
 
 	struct work_struct stat_work;
 
+	u8 *xmit_buf[MT_TXQ_MCU_WA];
+
 	struct sdio_func *func;
 	void *intr_data;
 

index 8621c6f579aa..874c929d8552 100644
@@ -346,7 +346,7 @@ static int mt7663s_probe(struct sdio_func *func,
 	struct ieee80211_ops *ops;
 	struct mt7615_dev *dev;
 	struct mt76_dev *mdev;
-	int ret;
+	int i, ret;
 
 	ops = devm_kmemdup(&func->dev, &mt7615_ops, sizeof(mt7615_ops),
 			   GFP_KERNEL);

@@ -387,6 +387,16 @@ static int mt7663s_probe(struct sdio_func *func,
 		goto err_deinit;
 	}
 
+	for (i = 0; i < ARRAY_SIZE(mdev->sdio.xmit_buf); i++) {
+		mdev->sdio.xmit_buf[i] = devm_kmalloc(mdev->dev,
+						      MT76S_XMIT_BUF_SZ,
+						      GFP_KERNEL);
+		if (!mdev->sdio.xmit_buf[i]) {
+			ret = -ENOMEM;
+			goto err_deinit;
+		}
+	}
+
 	ret = mt76s_alloc_queues(&dev->mt76);
 	if (ret)
 		goto err_deinit;

77 lines not on this page · raw message

index e82f6bdcbce4..2486cda3243b 100644
@@ -278,9 +278,12 @@ void mt7663s_rx_work(struct work_struct *work)
 	/* disable interrupt */
 	sdio_claim_host(sdio->func);
 	sdio_writel(sdio->func, WHLPCR_INT_EN_CLR, MCR_WHLPCR, NULL);
-	sdio_readsb(sdio->func, intr, MCR_WHISR, sizeof(struct mt76s_intr));
+	ret = sdio_readsb(sdio->func, intr, MCR_WHISR, sizeof(*intr));
 	sdio_release_host(sdio->func);
 
+	if (ret < 0)
+		goto out;
+
 	trace_dev_irq(dev, intr->isr, 0);
 
 	if (intr->isr & WHIER_RX0_DONE_INT_EN) {

@@ -306,7 +309,7 @@ void mt7663s_rx_work(struct work_struct *work)
 		queue_work(sdio->txrx_wq, &sdio->rx.recv_work);
 		return;
 	}
-
+out:
 	/* enable interrupt */
 	sdio_claim_host(sdio->func);
 	sdio_writel(sdio->func, WHLPCR_INT_EN_SET, MCR_WHLPCR, NULL);