[PATCH 2/2] wifi: rtw88: usb: Fix urbs with size multiple of bulkout_size

[Sascha Hauer]

The hardware can't handle urbs with a data size of multiple of
bulkout_size. With such a packet the endpoint gets stuck and only
replugging the hardware helps.

Fix this by moving the header eight bytes down, thus making the packet
eight bytes bigger. The same is done in rtw_usb_write_data_rsvd_page()
already, but not yet for the tx data.

Fixes: a82dfd33d1237 ("wifi: rtw88: Add common USB chip support")
Signed-off-by: Sascha Hauer <s.hauer@xxxxxxxxxxxxxx>
---
 drivers/net/wireless/realtek/rtw88/tx.h  |  2 ++
 drivers/net/wireless/realtek/rtw88/usb.c | 34 +++++++++++++++---------
 2 files changed, 24 insertions(+), 12 deletions(-)

diff --git a/drivers/net/wireless/realtek/rtw88/tx.h b/drivers/net/wireless/realtek/rtw88/tx.h
index a2f3ac326041b..38ce9c7ae62ed 100644
--- a/drivers/net/wireless/realtek/rtw88/tx.h
+++ b/drivers/net/wireless/realtek/rtw88/tx.h
@@ -75,6 +75,8 @@
 	le32p_replace_bits((__le32 *)(txdesc) + 0x07, value, GENMASK(15, 0))
 #define SET_TX_DESC_DMA_TXAGG_NUM(txdesc, value)				\
 	le32p_replace_bits((__le32 *)(txdesc) + 0x07, value, GENMASK(31, 24))
+#define GET_TX_DESC_OFFSET(txdesc)	                                      \
+	le32_get_bits(*((__le32 *)(txdesc) + 0x00), GENMASK(23, 16))
 #define GET_TX_DESC_PKT_OFFSET(txdesc)						\
 	le32_get_bits(*((__le32 *)(txdesc) + 0x01), GENMASK(28, 24))
 #define GET_TX_DESC_QSEL(txdesc)						\
diff --git a/drivers/net/wireless/realtek/rtw88/usb.c b/drivers/net/wireless/realtek/rtw88/usb.c
index d9e995544e405..08cd480958b6b 100644
--- a/drivers/net/wireless/realtek/rtw88/usb.c
+++ b/drivers/net/wireless/realtek/rtw88/usb.c
@@ -281,6 +281,7 @@ static int rtw_usb_write_port(struct rtw_dev *rtwdev, u8 qsel, struct sk_buff *s
 static bool rtw_usb_tx_agg_skb(struct rtw_usb *rtwusb, struct sk_buff_head *list)
 {
 	struct rtw_dev *rtwdev = rtwusb->rtwdev;
+	const struct rtw_chip_info *chip = rtwdev->chip;
 	struct rtw_usb_txcb *txcb;
 	struct sk_buff *skb_head;
 	struct sk_buff *skb_iter;
@@ -299,16 +300,11 @@ static bool rtw_usb_tx_agg_skb(struct rtw_usb *rtwusb, struct sk_buff_head *list
 
 	skb_iter = skb_dequeue(list);
 
-	if (skb_queue_empty(list)) {
-		skb_head = skb_iter;
-		goto queue;
-	}
-
 	skb_head = dev_alloc_skb(RTW_USB_MAX_XMITBUF_SZ);
-	if (!skb_head) {
-		skb_head = skb_iter;
-		goto queue;
-	}
+	if (!skb_head)
+		return false;
+
+	skb_reserve(skb_head, RTW_USB_PACKET_OFFSET_SZ);
 
 	while (skb_iter) {
 		unsigned long flags;
@@ -326,17 +322,31 @@ static bool rtw_usb_tx_agg_skb(struct rtw_usb *rtwusb, struct sk_buff_head *list
 
 		skb_iter = skb_peek(list);
 
-		if (skb_iter && skb_iter->len + skb_head->len <= RTW_USB_MAX_XMITBUF_SZ)
+		if (skb_iter && skb_iter->len + skb_head->len <=
+		    RTW_USB_MAX_XMITBUF_SZ - RTW_USB_PACKET_OFFSET_SZ)
 			__skb_unlink(skb_iter, list);
 		else
 			skb_iter = NULL;
 		spin_unlock_irqrestore(&list->lock, flags);
 	}
 
-	if (agg_num > 1)
+	if (skb_head->len % rtwusb->bulkout_size == 0) {
+		unsigned int offset;
+
+		skb_push(skb_head, RTW_USB_PACKET_OFFSET_SZ);
+
+		memmove(skb_head->data, skb_head->data + 8, chip->tx_pkt_desc_sz);
+
+		offset = GET_TX_DESC_OFFSET(skb_head->data);
+		offset += RTW_USB_PACKET_OFFSET_SZ;
+		SET_TX_DESC_OFFSET(skb_head->data, offset);
+
+		SET_TX_DESC_PKT_OFFSET(skb_head->data, 1);
+		rtw_usb_fill_tx_checksum(rtwusb, skb_head, agg_num);
+	} else if (agg_num > 1) {
 		rtw_usb_fill_tx_checksum(rtwusb, skb_head, agg_num);
+	}
 
-queue:
 	skb_queue_tail(&txcb->tx_ack_queue, skb_head);
 
 	rtw_usb_write_port(rtwdev, GET_TX_DESC_QSEL(skb_head->data), skb_head,
-- 
2.30.2