[openwrt/openwrt] realtek: correct egress frame priority assignment

[LEDE Commits]

svanheule pushed a commit to openwrt/openwrt.git, branch master:
https://git.openwrt.org/0b35a08a057848d909156604c4391a5d9f1d97e5

commit 0b35a08a057848d909156604c4391a5d9f1d97e5
Author: Sander Vanheule <sander at svanheule.net>
AuthorDate: Sun Jun 19 12:38:49 2022 +0200

    realtek: correct egress frame priority assignment
    
    Priority values passed to the egress (TX) frame header initialiser are
    invalid when smaller than 0, and should not be assigned to the frame.
    Queue assignment is then left to the switch core logic.
    
    Current code for RTL83xx forces the passed priority value to be
    positive, by always masking it to the lower bits, resulting in the
    priority always being set and enabled. RTL93xx code doesn't even check
    the value and unconditionally assigns the (32 bit) value to the (5 bit)
    QID field without masking.
    
    Fix priority assignment by only setting the AS_QID/AS_PRI flag when a
    valid value is passed, and properly mask the value to not overflow the
    QID/PRI field.
    
    For RTL839x, also assign the priority to the right part of the frame
    header. Counting from the leftmost bit, AS_PRI and PRI are in bits 36
    and 37-39. The means they should be assigned to the third 16 bit value,
    containing bits 32-47.
    
    Tested-by: Luiz Angelo Daros de Luca <luizluca at gmail.com>
    Signed-off-by: Sander Vanheule <sander at svanheule.net>
---
 .../files-5.10/drivers/net/ethernet/rtl838x_eth.c  | 26 ++++++++++++----------
 1 file changed, 14 insertions(+), 12 deletions(-)

diff --git a/target/linux/realtek/files-5.10/drivers/net/ethernet/rtl838x_eth.c b/target/linux/realtek/files-5.10/drivers/net/ethernet/rtl838x_eth.c
index 8c7813015a..1de420dea1 100644
--- a/target/linux/realtek/files-5.10/drivers/net/ethernet/rtl838x_eth.c
+++ b/target/linux/realtek/files-5.10/drivers/net/ethernet/rtl838x_eth.c
@@ -94,8 +94,6 @@ struct notify_b {
 
 static void rtl838x_create_tx_header(struct p_hdr *h, int dest_port, int prio)
 {
-	prio &= 0x7;
-
 	if (dest_port > 0) {
 		// cpu_tag[0] is reserved on the RTL83XX SoCs
 		h->cpu_tag[1] = 0x0401;  // BIT 10: RTL8380_CPU_TAG, BIT0: L2LEARNING on
@@ -103,16 +101,15 @@ static void rtl838x_create_tx_header(struct p_hdr *h, int dest_port, int prio)
 		h->cpu_tag[3] = 0x0000;
 		h->cpu_tag[4] = BIT(dest_port) >> 16;
 		h->cpu_tag[5] = BIT(dest_port) & 0xffff;
-		// Set internal priority and AS_PRIO
+
+		/* Set internal priority (PRI) and enable (AS_PRI) */
 		if (prio >= 0)
-			h->cpu_tag[2] |= (prio | 0x8) << 12;
+			h->cpu_tag[2] |= ((prio & 0x7) | BIT(3)) << 12;
 	}
 }
 
 static void rtl839x_create_tx_header(struct p_hdr *h, int dest_port, int prio)
 {
-	prio &= 0x7;
-
 	if (dest_port > 0) {
 		// cpu_tag[0] is reserved on the RTL83XX SoCs
 		h->cpu_tag[1] = 0x0100; // RTL8390_CPU_TAG marker
@@ -128,9 +125,10 @@ static void rtl839x_create_tx_header(struct p_hdr *h, int dest_port, int prio)
 		}
 		h->cpu_tag[2] |= BIT(4); // Enable destination port mask use
 		h->cpu_tag[2] |= BIT(7); // Enable L2 Learning
-		// Set internal priority and AS_PRIO
+
+		/* Set internal priority (PRI) and enable (AS_PRI) */
 		if (prio >= 0)
-			h->cpu_tag[1] |= prio | BIT(3);
+			h->cpu_tag[2] |= ((prio & 0x7) | BIT(3)) << 8;
 	}
 }
 
@@ -138,21 +136,21 @@ static void rtl930x_create_tx_header(struct p_hdr *h, int dest_port, int prio)
 {
 	h->cpu_tag[0] = 0x8000;  // CPU tag marker
 	h->cpu_tag[1] = h->cpu_tag[2] = 0;
-	if (prio >= 0)
-		h->cpu_tag[2] = BIT(13) | prio << 8; // Enable and set Priority Queue
 	h->cpu_tag[3] = 0;
 	h->cpu_tag[4] = 0;
 	h->cpu_tag[5] = 0;
 	h->cpu_tag[6] = BIT(dest_port) >> 16;
 	h->cpu_tag[7] = BIT(dest_port) & 0xffff;
+
+	/* Enable (AS_QID) and set priority queue (QID) */
+	if (prio >= 0)
+		h->cpu_tag[2] = (BIT(5) | (prio & 0x1f)) << 8;
 }
 
 static void rtl931x_create_tx_header(struct p_hdr *h, int dest_port, int prio)
 {
 	h->cpu_tag[0] = 0x8000;  // CPU tag marker
 	h->cpu_tag[1] = h->cpu_tag[2] = 0;
-	if (prio >= 0)
-		h->cpu_tag[2] = BIT(13) | prio << 8; // Enable and set Priority Queue
 	h->cpu_tag[3] = 0;
 	h->cpu_tag[4] = h->cpu_tag[5] = h->cpu_tag[6] = h->cpu_tag[7] = 0;
 	if (dest_port >= 32) {
@@ -163,6 +161,10 @@ static void rtl931x_create_tx_header(struct p_hdr *h, int dest_port, int prio)
 		h->cpu_tag[6] = BIT(dest_port) >> 16;
 		h->cpu_tag[7] = BIT(dest_port) & 0xffff;
 	}
+
+	/* Enable (AS_QID) and set priority queue (QID) */
+	if (prio >= 0)
+		h->cpu_tag[2] = (BIT(5) | (prio & 0x1f)) << 8;
 }
 
 static void rtl93xx_header_vlan_set(struct p_hdr *h, int vlan)