wifi: rtw88: usb: fix priority queue to endpoint mapping

The RTW88 chipsets have four different priority queues in hardware. For
the USB type chipsets the packets destined for a specific priority queue
must be sent through the endpoint corresponding to the queue. This was
not fully understood when porting from the RTW88 USB out of tree driver
and thus violated.

This patch implements the qsel to endpoint mapping as in
get_usb_bulkout_id_88xx() in the downstream driver.

Without this the driver often issues "timed out to flush queue 3"
warnings and often TX stalls completely.
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>
Tested-by: ValdikSS <iam@valdikss.org.ru>
Tested-by: Alexandru gagniuc <mr.nuke.me@gmail.com>
Tested-by: Larry Finger <Larry.Finger@lwfinger.net>
Cc: stable@vger.kernel.org
Reviewed-by: Ping-Ke Shih <pkshih@realtek.com>
Signed-off-by: Kalle Valo <kvalo@kernel.org>
Link: https://lore.kernel.org/r/20230417140358.2240429-2-s.hauer@pengutronix.de

drivers/net/wireless/realtek/rtw88/usb.c

@@ -118,6 +118,22 @@ static void rtw_usb_write32(struct rtw_dev *rtwdev, u32 addr, u32 val)
 	rtw_usb_write(rtwdev, addr, val, 4);
 }
 
+static int dma_mapping_to_ep(enum rtw_dma_mapping dma_mapping)
+{
+	switch (dma_mapping) {
+	case RTW_DMA_MAPPING_HIGH:
+		return 0;
+	case RTW_DMA_MAPPING_NORMAL:
+		return 1;
+	case RTW_DMA_MAPPING_LOW:
+		return 2;
+	case RTW_DMA_MAPPING_EXTRA:
+		return 3;
+	default:
+		return -EINVAL;
+	}
+}
+
 static int rtw_usb_parse(struct rtw_dev *rtwdev,
 			 struct usb_interface *interface)
 {
@@ -129,6 +145,8 @@ static int rtw_usb_parse(struct rtw_dev *rtwdev,
 	int num_out_pipes = 0;
 	int i;
 	u8 num;
+	const struct rtw_chip_info *chip = rtwdev->chip;
+	const struct rtw_rqpn *rqpn;
 
 	for (i = 0; i < interface_desc->bNumEndpoints; i++) {
 		endpoint = &host_interface->endpoint[i].desc;
@@ -183,31 +201,34 @@ static int rtw_usb_parse(struct rtw_dev *rtwdev,
 
 	rtwdev->hci.bulkout_num = num_out_pipes;
 
-	switch (num_out_pipes) {
-	case 4:
-	case 3:
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID0] = 2;
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID1] = 2;
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID2] = 2;
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID3] = 2;
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID4] = 1;
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID5] = 1;
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID6] = 0;
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID7] = 0;
-		break;
-	case 2:
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID0] = 1;
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID1] = 1;
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID2] = 1;
-		rtwusb->qsel_to_ep[TX_DESC_QSEL_TID3] = 1;
-		break;
-	case 1:
-		break;
-	default:
-		rtw_err(rtwdev, "failed to get out_pipes(%d)\n", num_out_pipes);
+	if (num_out_pipes < 1 || num_out_pipes > 4) {
+		rtw_err(rtwdev, "invalid number of endpoints %d\n", num_out_pipes);
 		return -EINVAL;
 	}
 
+	rqpn = &chip->rqpn_table[num_out_pipes];
+
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID0] = dma_mapping_to_ep(rqpn->dma_map_be);
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID1] = dma_mapping_to_ep(rqpn->dma_map_bk);
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID2] = dma_mapping_to_ep(rqpn->dma_map_bk);
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID3] = dma_mapping_to_ep(rqpn->dma_map_be);
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID4] = dma_mapping_to_ep(rqpn->dma_map_vi);
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID5] = dma_mapping_to_ep(rqpn->dma_map_vi);
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID6] = dma_mapping_to_ep(rqpn->dma_map_vo);
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID7] = dma_mapping_to_ep(rqpn->dma_map_vo);
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID8] = -EINVAL;
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID9] = -EINVAL;
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID10] = -EINVAL;
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID11] = -EINVAL;
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID12] = -EINVAL;
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID13] = -EINVAL;
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID14] = -EINVAL;
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_TID15] = -EINVAL;
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_BEACON] = dma_mapping_to_ep(rqpn->dma_map_hi);
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_HIGH] = dma_mapping_to_ep(rqpn->dma_map_hi);
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_MGMT] = dma_mapping_to_ep(rqpn->dma_map_mg);
+	rtwusb->qsel_to_ep[TX_DESC_QSEL_H2C] = dma_mapping_to_ep(rqpn->dma_map_hi);
+
 	return 0;
 }
 
@@ -250,7 +271,7 @@ static void rtw_usb_write_port_tx_complete(struct urb *urb)
 static int qsel_to_ep(struct rtw_usb *rtwusb, unsigned int qsel)
 {
 	if (qsel >= ARRAY_SIZE(rtwusb->qsel_to_ep))
-		return 0;
+		return -EINVAL;
 
 	return rtwusb->qsel_to_ep[qsel];
 }
@@ -265,6 +286,9 @@ static int rtw_usb_write_port(struct rtw_dev *rtwdev, u8 qsel, struct sk_buff *s
 	int ret;
 	int ep = qsel_to_ep(rtwusb, qsel);
 
+	if (ep < 0)
+		return ep;
+
 	pipe = usb_sndbulkpipe(usbd, rtwusb->out_ep[ep]);
 	urb = usb_alloc_urb(0, GFP_ATOMIC);
 	if (!urb)