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@gmail.com>
Signed-off-by: Sander Vanheule <sander@svanheule.net>
(cherry picked from commit 0b35a08a057848d909156604c4391a5d9f1d97e5)

1 files 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)