[openwrt/openwrt] realtek: add QoS and rate control

LEDE Commits lede-commits at lists.infradead.org
Tue Jan 26 09:07:15 EST 2021


ynezz pushed a commit to openwrt/openwrt.git, branch master:
https://git.openwrt.org/dc9cc0d3e2a1ede4bcf2e6b5b0aded4dc50244af

commit dc9cc0d3e2a1ede4bcf2e6b5b0aded4dc50244af
Author: Birger Koblitz <git at birger-koblitz.de>
AuthorDate: Thu Jan 21 14:29:52 2021 +0100

    realtek: add QoS and rate control
    
    This adds support for identifying QoS information in packets
    and use this and rate control information to submit to multiple
    egress queues. The ethernet driver is also made to support
    2 egress and up to 32 egress queues.
    
    Signed-off-by: Birger Koblitz <git at birger-koblitz.de>
---
 .../files-5.4/drivers/net/dsa/rtl83xx/Makefile     |    2 +-
 .../files-5.4/drivers/net/dsa/rtl83xx/qos.c        |  576 +++++++++
 .../files-5.4/drivers/net/dsa/rtl83xx/rtl838x.h    |  267 ++++-
 .../files-5.4/drivers/net/dsa/rtl83xx/rtl839x.c    |  315 +++--
 .../files-5.4/drivers/net/dsa/rtl83xx/rtl83xx.h    |   83 +-
 .../files-5.4/drivers/net/ethernet/rtl838x_eth.c   | 1242 +++++++++++++++-----
 6 files changed, 1955 insertions(+), 530 deletions(-)

diff --git a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/Makefile b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/Makefile
index 52cc151a56..016184c3d9 100644
--- a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/Makefile
+++ b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/Makefile
@@ -1,3 +1,3 @@
 # SPDX-License-Identifier: GPL-2.0
 obj-$(CONFIG_NET_DSA_RTL83XX)	+= common.o dsa.o \
-	rtl838x.o rtl839x.o storm.o debugfs.o
+	rtl838x.o rtl839x.o rtl930x.o rtl931x.o debugfs.o qos.o
diff --git a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/qos.c b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/qos.c
new file mode 100644
index 0000000000..2fc8d37f3e
--- /dev/null
+++ b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/qos.c
@@ -0,0 +1,576 @@
+// SPDX-License-Identifier: GPL-2.0-only
+
+#include <net/dsa.h>
+#include <linux/delay.h>
+
+#include <asm/mach-rtl838x/mach-rtl83xx.h>
+#include "rtl83xx.h"
+
+static struct rtl838x_switch_priv *switch_priv;
+extern struct rtl83xx_soc_info soc_info;
+
+enum scheduler_type {
+	WEIGHTED_FAIR_QUEUE = 0,
+	WEIGHTED_ROUND_ROBIN,
+};
+
+int max_available_queue[] = {0, 1, 2, 3, 4, 5, 6, 7};
+int default_queue_weights[] = {1, 1, 1, 1, 1, 1, 1, 1};
+int dot1p_priority_remapping[] = {0, 1, 2, 3, 4, 5, 6, 7};
+
+static void rtl839x_read_scheduling_table(int port)
+{
+	u32 cmd = 1 << 9 /* Execute cmd */
+		| 0 << 8 /* Read */
+		| 0 << 6 /* Table type 0b00 */
+		| (port & 0x3f);
+	rtl839x_exec_tbl2_cmd(cmd);
+}
+
+static void rtl839x_write_scheduling_table(int port)
+{
+	u32 cmd = 1 << 9 /* Execute cmd */
+		| 1 << 8 /* Write */
+		| 0 << 6 /* Table type 0b00 */
+		| (port & 0x3f);
+	rtl839x_exec_tbl2_cmd(cmd);
+}
+
+static void rtl839x_read_out_q_table(int port)
+{
+	u32 cmd = 1 << 9 /* Execute cmd */
+		| 0 << 8 /* Read */
+		| 2 << 6 /* Table type 0b10 */
+		| (port & 0x3f);
+	rtl839x_exec_tbl2_cmd(cmd);
+}
+
+static void rtl838x_storm_enable(struct rtl838x_switch_priv *priv, int port, bool enable)
+{
+	// Enable Storm control for that port for UC, MC, and BC
+	if (enable)
+		sw_w32(0x7, RTL838X_STORM_CTRL_LB_CTRL(port));
+	else
+		sw_w32(0x0, RTL838X_STORM_CTRL_LB_CTRL(port));
+}
+
+u32 rtl838x_get_egress_rate(struct rtl838x_switch_priv *priv, int port)
+{
+	u32 rate;
+
+	if (port > priv->cpu_port)
+		return 0;
+	rate = sw_r32(RTL838X_SCHED_P_EGR_RATE_CTRL(port)) & 0x3fff;
+	return rate;
+}
+
+/* Sets the rate limit, 10MBit/s is equal to a rate value of 625 */
+int rtl838x_set_egress_rate(struct rtl838x_switch_priv *priv, int port, u32 rate)
+{
+	u32 old_rate;
+
+	if (port > priv->cpu_port)
+		return -1;
+
+	old_rate = sw_r32(RTL838X_SCHED_P_EGR_RATE_CTRL(port));
+	sw_w32(rate, RTL838X_SCHED_P_EGR_RATE_CTRL(port));
+
+	return old_rate;
+}
+
+/* Set the rate limit for a particular queue in Bits/s
+ * units of the rate is 16Kbps
+ */
+void rtl838x_egress_rate_queue_limit(struct rtl838x_switch_priv *priv, int port,
+					    int queue, u32 rate)
+{
+	if (port > priv->cpu_port)
+		return;
+	if (queue > 7)
+		return;
+	sw_w32(rate, RTL838X_SCHED_Q_EGR_RATE_CTRL(port, queue));
+}
+
+static void rtl838x_rate_control_init(struct rtl838x_switch_priv *priv)
+{
+	int i;
+
+	pr_info("Enabling Storm control\n");
+	// TICK_PERIOD_PPS
+	if (priv->id == 0x8380)
+		sw_w32_mask(0x3ff << 20, 434 << 20, RTL838X_SCHED_LB_TICK_TKN_CTRL_0);
+
+	// Set burst rate
+	sw_w32(0x00008000, RTL838X_STORM_CTRL_BURST_0); // UC
+	sw_w32(0x80008000, RTL838X_STORM_CTRL_BURST_1); // MC and BC
+
+	// Set burst Packets per Second to 32
+	sw_w32(0x00000020, RTL838X_STORM_CTRL_BURST_PPS_0); // UC
+	sw_w32(0x00200020, RTL838X_STORM_CTRL_BURST_PPS_1); // MC and BC
+
+	// Include IFG in storm control, rate based on bytes/s (0 = packets)
+	sw_w32_mask(0, 1 << 6 | 1 << 5, RTL838X_STORM_CTRL);
+	// Bandwidth control includes preamble and IFG (10 Bytes)
+	sw_w32_mask(0, 1, RTL838X_SCHED_CTRL);
+
+	// On SoCs except RTL8382M, set burst size of port egress
+	if (priv->id != 0x8382)
+		sw_w32_mask(0xffff, 0x800, RTL838X_SCHED_LB_THR);
+
+	/* Enable storm control on all ports with a PHY and limit rates,
+	 * for UC and MC for both known and unknown addresses */
+	for (i = 0; i < priv->cpu_port; i++) {
+		if (priv->ports[i].phy) {
+			sw_w32((1 << 18) | 0x8000, RTL838X_STORM_CTRL_PORT_UC(i));
+			sw_w32((1 << 18) | 0x8000, RTL838X_STORM_CTRL_PORT_MC(i));
+			sw_w32(0x8000, RTL838X_STORM_CTRL_PORT_BC(i));
+			rtl838x_storm_enable(priv, i, true);
+		}
+	}
+
+	// Attack prevention, enable all attack prevention measures
+	//sw_w32(0x1ffff, RTL838X_ATK_PRVNT_CTRL);
+	/* Attack prevention, drop (bit = 0) problematic packets on all ports.
+	 * Setting bit = 1 means: trap to CPU
+	 */
+	//sw_w32(0, RTL838X_ATK_PRVNT_ACT);
+	// Enable attack prevention on all ports
+	//sw_w32(0x0fffffff, RTL838X_ATK_PRVNT_PORT_EN);
+}
+
+/* Sets the rate limit, 10MBit/s is equal to a rate value of 625 */
+u32 rtl839x_get_egress_rate(struct rtl838x_switch_priv *priv, int port)
+{
+	u32 rate;
+
+	pr_debug("%s: Getting egress rate on port %d to %d\n", __func__, port, rate);
+	if (port >= priv->cpu_port)
+		return 0;
+
+	mutex_lock(&priv->reg_mutex);
+
+	rtl839x_read_scheduling_table(port);
+
+	rate = sw_r32(RTL839X_TBL_ACCESS_DATA_2(7));
+	rate <<= 12;
+	rate |= sw_r32(RTL839X_TBL_ACCESS_DATA_2(8)) >> 20;
+
+	mutex_unlock(&priv->reg_mutex);
+
+	return rate;
+}
+
+/* Sets the rate limit, 10MBit/s is equal to a rate value of 625, returns previous rate */
+int rtl839x_set_egress_rate(struct rtl838x_switch_priv *priv, int port, u32 rate)
+{
+	u32 old_rate;
+
+	pr_debug("%s: Setting egress rate on port %d to %d\n", __func__, port, rate);
+	if (port >= priv->cpu_port)
+		return -1;
+
+	mutex_lock(&priv->reg_mutex);
+
+	rtl839x_read_scheduling_table(port);
+
+	old_rate = sw_r32(RTL839X_TBL_ACCESS_DATA_2(7)) & 0xff;
+	old_rate <<= 12;
+	old_rate |= sw_r32(RTL839X_TBL_ACCESS_DATA_2(8)) >> 20;
+	sw_w32_mask(0xff, (rate >> 12) & 0xff, RTL839X_TBL_ACCESS_DATA_2(7));
+	sw_w32_mask(0xfff << 20, rate << 20, RTL839X_TBL_ACCESS_DATA_2(8));
+
+	rtl839x_write_scheduling_table(port);
+	
+	mutex_unlock(&priv->reg_mutex);
+
+	return old_rate;
+}
+
+/* Set the rate limit for a particular queue in Bits/s
+ * units of the rate is 16Kbps
+ */
+void rtl839x_egress_rate_queue_limit(struct rtl838x_switch_priv *priv, int port,
+					int queue, u32 rate)
+{
+	int lsb = 128 + queue * 20;
+	int low_byte = 8 - (lsb >> 5);
+	int start_bit = lsb - (low_byte << 5);
+	u32 high_mask = 0xfffff	>> (32 - start_bit);
+
+	pr_debug("%s: Setting egress rate on port %d, queue %d to %d\n",
+		__func__, port, queue, rate);
+	if (port >= priv->cpu_port)
+		return;
+	if (queue > 7)
+		return;
+
+	mutex_lock(&priv->reg_mutex);
+
+	rtl839x_read_scheduling_table(port);
+
+	sw_w32_mask(0xfffff << start_bit, (rate & 0xfffff) << start_bit,
+		    RTL839X_TBL_ACCESS_DATA_2(low_byte));
+	if (high_mask)
+		sw_w32_mask(high_mask, (rate & 0xfffff) >> (32- start_bit),
+			    RTL839X_TBL_ACCESS_DATA_2(low_byte - 1));
+
+	rtl839x_write_scheduling_table(port);
+
+	mutex_unlock(&priv->reg_mutex);
+}
+
+static void rtl839x_rate_control_init(struct rtl838x_switch_priv *priv)
+{
+	int p, q;
+
+	pr_info("%s: enabling rate control\n", __func__);
+	/* Tick length and token size settings for SoC with 250MHz,
+	 * RTL8350 family would use 50MHz
+	 */
+	// Set the special tick period
+	sw_w32(976563, RTL839X_STORM_CTRL_SPCL_LB_TICK_TKN_CTRL);
+	// Ingress tick period and token length 10G
+	sw_w32(18 << 11 | 151, RTL839X_IGR_BWCTRL_LB_TICK_TKN_CTRL_0);
+	// Ingress tick period and token length 1G
+	sw_w32(245 << 11 | 129, RTL839X_IGR_BWCTRL_LB_TICK_TKN_CTRL_1);
+	// Egress tick period 10G, bytes/token 10G and tick period 1G, bytes/token 1G
+	sw_w32(18 << 24 | 151 << 16 | 185 << 8 | 97, RTL839X_SCHED_LB_TICK_TKN_CTRL);
+	// Set the tick period of the CPU and the Token Len
+	sw_w32(3815 << 8 | 1, RTL839X_SCHED_LB_TICK_TKN_PPS_CTRL);
+
+	// Set the Weighted Fair Queueing burst size
+	sw_w32_mask(0xffff, 4500, RTL839X_SCHED_LB_THR);
+
+	// Storm-rate calculation is based on bytes/sec (bit 5), include IFG (bit 6)
+	sw_w32_mask(0, 1 << 5 | 1 << 6, RTL839X_STORM_CTRL);
+
+	/* Based on the rate control mode being bytes/s
+	 * set tick period and token length for 10G
+	 */
+	sw_w32(18 << 10 | 151, RTL839X_STORM_CTRL_LB_TICK_TKN_CTRL_0);
+	/* and for 1G ports */
+	sw_w32(246 << 10 | 129, RTL839X_STORM_CTRL_LB_TICK_TKN_CTRL_1);
+
+	/* Set default burst rates on all ports (the same for 1G / 10G) with a PHY
+	 * for UC, MC and BC
+	 * For 1G port, the minimum burst rate is 1700, maximum 65535,
+	 * For 10G ports it is 2650 and 1048575 respectively */
+	for (p = 0; p < priv->cpu_port; p++) {
+		if (priv->ports[p].phy && !priv->ports[p].is10G) {
+			sw_w32_mask(0xffff, 0x8000, RTL839X_STORM_CTRL_PORT_UC_1(p));
+			sw_w32_mask(0xffff, 0x8000, RTL839X_STORM_CTRL_PORT_MC_1(p));
+			sw_w32_mask(0xffff, 0x8000, RTL839X_STORM_CTRL_PORT_BC_1(p));
+		}
+	}
+
+	/* Setup ingress/egress per-port rate control */
+	for (p = 0; p < priv->cpu_port; p++) {
+		if (!priv->ports[p].phy)
+			continue;
+
+		if (priv->ports[p].is10G)
+			rtl839x_set_egress_rate(priv, p, 625000); // 10GB/s
+		else
+			rtl839x_set_egress_rate(priv, p, 62500);  // 1GB/s
+
+		// Setup queues: all RTL83XX SoCs have 8 queues, maximum rate
+		for (q = 0; q < 8; q++)
+			rtl839x_egress_rate_queue_limit(priv, p, q, 0xfffff);
+
+		if (priv->ports[p].is10G) {
+			// Set high threshold to maximum
+			sw_w32_mask(0xffff, 0xffff, RTL839X_IGR_BWCTRL_PORT_CTRL_10G_0(p));
+		} else {
+			// Set high threshold to maximum
+			sw_w32_mask(0xffff, 0xffff, RTL839X_IGR_BWCTRL_PORT_CTRL_1(p));
+		}
+	}
+
+	// Set global ingress low watermark rate
+	sw_w32(65532, RTL839X_IGR_BWCTRL_CTRL_LB_THR);
+}
+
+
+
+void rtl838x_setup_prio2queue_matrix(int *min_queues)
+{
+	int i;
+	u32 v;
+
+	pr_info("Current Intprio2queue setting: %08x\n", sw_r32(RTL838X_QM_INTPRI2QID_CTRL));
+	for (i = 0; i < MAX_PRIOS; i++)
+		v |= i << (min_queues[i] * 3);
+	sw_w32(v, RTL838X_QM_INTPRI2QID_CTRL);
+}
+
+void rtl839x_setup_prio2queue_matrix(int *min_queues)
+{
+	int i, q;
+
+	pr_info("Current Intprio2queue setting: %08x\n", sw_r32(RTL839X_QM_INTPRI2QID_CTRL(0)));
+	for (i = 0; i < MAX_PRIOS; i++) {
+		q = min_queues[i];
+		sw_w32(i << (q * 3), RTL839X_QM_INTPRI2QID_CTRL(q));
+	}
+}
+
+/* Sets the CPU queue depending on the internal priority of a packet */
+void rtl83xx_setup_prio2queue_cpu_matrix(int *max_queues)
+{
+	int reg = soc_info.family == RTL8380_FAMILY_ID ? RTL838X_QM_PKT2CPU_INTPRI_MAP 
+					: RTL839X_QM_PKT2CPU_INTPRI_MAP;
+	int i;
+	u32 v;
+
+	pr_info("QM_PKT2CPU_INTPRI_MAP: %08x\n", sw_r32(reg));
+	for (i = 0; i < MAX_PRIOS; i++)
+		v |= max_queues[i] << (i * 3);
+	sw_w32(v, reg);
+}
+
+void rtl83xx_setup_default_prio2queue(void)
+{
+	if (soc_info.family == RTL8380_FAMILY_ID) {
+		rtl838x_setup_prio2queue_matrix(max_available_queue);
+	} else {
+		rtl839x_setup_prio2queue_matrix(max_available_queue);
+	}
+	rtl83xx_setup_prio2queue_cpu_matrix(max_available_queue);
+}
+
+/* Sets the output queue assigned to a port, the port can be the CPU-port */
+void rtl839x_set_egress_queue(int port, int queue)
+{
+	sw_w32(queue << ((port % 10) *3), RTL839X_QM_PORT_QNUM(port));
+}
+
+/* Sets the priority assigned of an ingress port, the port can be the CPU-port */
+void rtl83xx_set_ingress_priority(int port, int priority)
+{
+	if (soc_info.family == RTL8380_FAMILY_ID)
+		sw_w32(priority << ((port % 10) *3), RTL838X_PRI_SEL_PORT_PRI(port));
+	else
+		sw_w32(priority << ((port % 10) *3), RTL839X_PRI_SEL_PORT_PRI(port));
+	
+}
+
+int rtl839x_get_scheduling_algorithm(struct rtl838x_switch_priv *priv, int port)
+{
+	u32 v;
+
+	mutex_lock(&priv->reg_mutex);
+
+	rtl839x_read_scheduling_table(port);
+	v = sw_r32(RTL839X_TBL_ACCESS_DATA_2(8));
+
+	mutex_unlock(&priv->reg_mutex);
+
+	if (v & BIT(19))
+		return WEIGHTED_ROUND_ROBIN;
+	return WEIGHTED_FAIR_QUEUE;
+}
+
+void rtl839x_set_scheduling_algorithm(struct rtl838x_switch_priv *priv, int port,
+				      enum scheduler_type sched)
+{
+	enum scheduler_type t = rtl839x_get_scheduling_algorithm(priv, port);
+	u32 v, oam_state, oam_port_state;
+	u32 count;
+	int i, egress_rate;
+
+	mutex_lock(&priv->reg_mutex);
+	/* Check whether we need to empty the egress queue of that port due to Errata E0014503 */
+	if (sched == WEIGHTED_FAIR_QUEUE && t == WEIGHTED_ROUND_ROBIN && port != priv->cpu_port) {
+		// Read Operations, Adminstatrion and Management control register
+		oam_state = sw_r32(RTL839X_OAM_CTRL);
+
+		// Get current OAM state
+		oam_port_state = sw_r32(RTL839X_OAM_PORT_ACT_CTRL(port));
+	
+		// Disable OAM to block traffice
+		v = sw_r32(RTL839X_OAM_CTRL);
+		sw_w32_mask(0, 1, RTL839X_OAM_CTRL);
+		v = sw_r32(RTL839X_OAM_CTRL);
+
+		// Set to trap action OAM forward (bits 1, 2) and OAM Mux Action Drop (bit 0)
+		sw_w32(0x2, RTL839X_OAM_PORT_ACT_CTRL(port));
+
+		// Set port egress rate to unlimited
+		egress_rate = rtl839x_set_egress_rate(priv, port, 0xFFFFF);
+	
+		// Wait until the egress used page count of that port is 0
+		i = 0;
+		do {
+			usleep_range(100, 200);
+			rtl839x_read_out_q_table(port);
+			count = sw_r32(RTL839X_TBL_ACCESS_DATA_2(6));
+			count >>= 20;
+			i++;
+		} while (i < 3500 && count > 0);
+	}
+
+	// Actually set the scheduling algorithm
+	rtl839x_read_scheduling_table(port);
+	sw_w32_mask(BIT(19), sched ? BIT(19) : 0, RTL839X_TBL_ACCESS_DATA_2(8));
+	rtl839x_write_scheduling_table(port);
+
+	if (sched == WEIGHTED_FAIR_QUEUE && t == WEIGHTED_ROUND_ROBIN && port != priv->cpu_port) {
+		// Restore OAM state to control register
+		sw_w32(oam_state, RTL839X_OAM_CTRL);
+
+		// Restore trap action state
+		sw_w32(oam_port_state, RTL839X_OAM_PORT_ACT_CTRL(port));
+
+		// Restore port egress rate
+		rtl839x_set_egress_rate(priv, port, egress_rate);
+	}
+
+	mutex_unlock(&priv->reg_mutex);
+}
+
+void rtl839x_set_scheduling_queue_weights(struct rtl838x_switch_priv *priv, int port,
+					  int *queue_weights)
+{
+	int i, lsb, low_byte, start_bit, high_mask;
+
+	mutex_lock(&priv->reg_mutex);
+
+	rtl839x_read_scheduling_table(port);
+
+	for (i = 0; i < 8; i++) {
+		lsb = 48 + i * 8;
+		low_byte = 8 - (lsb >> 5);
+		start_bit = lsb - (low_byte << 5);
+		high_mask = 0x3ff >> (32 - start_bit);
+		sw_w32_mask(0x3ff << start_bit, (queue_weights[i] & 0x3ff) << start_bit,
+				RTL839X_TBL_ACCESS_DATA_2(low_byte));
+		if (high_mask)
+			sw_w32_mask(high_mask, (queue_weights[i] & 0x3ff) >> (32- start_bit),
+					RTL839X_TBL_ACCESS_DATA_2(low_byte - 1));
+	}
+
+	rtl839x_write_scheduling_table(port);
+	mutex_unlock(&priv->reg_mutex);
+}
+
+void rtl838x_config_qos(void)
+{
+	int i, p;
+	u32 v;
+
+	pr_info("Setting up RTL838X QoS\n");
+	pr_info("RTL838X_PRI_SEL_TBL_CTRL(i): %08x\n", sw_r32(RTL838X_PRI_SEL_TBL_CTRL(0)));
+	rtl83xx_setup_default_prio2queue();
+
+	// Enable inner (bit 12) and outer (bit 13) priority remapping from DSCP
+	sw_w32_mask(0, BIT(12) | BIT(13), RTL838X_PRI_DSCP_INVLD_CTRL0);
+
+	/* Set default weight for calculating internal priority, in prio selection group 0
+	 * Port based (prio 3), Port outer-tag (4), DSCP (5), Inner Tag (6), Outer Tag (7)
+	 */
+	v = 3 | (4 << 3) | (5 << 6) | (6 << 9) | (7 << 12);
+	sw_w32(v, RTL838X_PRI_SEL_TBL_CTRL(0));
+
+	// Set the inner and outer priority one-to-one to re-marked outer dot1p priority
+	v = 0;
+	for (p = 0; p < 8; p++)
+		v |= p << (3 * p);
+	sw_w32(v, RTL838X_RMK_OPRI_CTRL);
+	sw_w32(v, RTL838X_RMK_IPRI_CTRL);
+
+	v = 0;
+	for (p = 0; p < 8; p++)
+		v |= (dot1p_priority_remapping[p] & 0x7) << (p * 3);
+	sw_w32(v, RTL838X_PRI_SEL_IPRI_REMAP);
+
+	// On all ports set scheduler type to WFQ
+	for (i = 0; i <= soc_info.cpu_port; i++)
+		sw_w32(0, RTL838X_SCHED_P_TYPE_CTRL(i));
+
+	// Enable egress scheduler for CPU-Port
+	sw_w32_mask(0, BIT(8), RTL838X_SCHED_LB_CTRL(soc_info.cpu_port));
+
+	// Enable egress drop allways on
+	sw_w32_mask(0, BIT(11), RTL838X_FC_P_EGR_DROP_CTRL(soc_info.cpu_port));
+
+	// Give special trap frames priority 7 (BPDUs) and routing exceptions:
+	sw_w32_mask(0, 7 << 3 | 7, RTL838X_QM_PKT2CPU_INTPRI_2);
+	// Give RMA frames priority 7:
+	sw_w32_mask(0, 7, RTL838X_QM_PKT2CPU_INTPRI_1);
+}
+
+void rtl839x_config_qos(void)
+{
+	int port, p, q;
+	u32 v;
+	struct rtl838x_switch_priv *priv = switch_priv;
+
+	pr_info("Setting up RTL839X QoS\n");
+	pr_info("RTL839X_PRI_SEL_TBL_CTRL(i): %08x\n", sw_r32(RTL839X_PRI_SEL_TBL_CTRL(0)));
+	rtl83xx_setup_default_prio2queue();
+
+	for (port = 0; port < soc_info.cpu_port; port++)
+		sw_w32(7, RTL839X_QM_PORT_QNUM(port));
+
+	// CPU-port gets queue number 7
+	sw_w32(7, RTL839X_QM_PORT_QNUM(soc_info.cpu_port));
+
+	for (port = 0; port <= soc_info.cpu_port; port++) {
+		rtl83xx_set_ingress_priority(port, 0);
+		rtl839x_set_scheduling_algorithm(priv, port, WEIGHTED_FAIR_QUEUE);
+		rtl839x_set_scheduling_queue_weights(priv, port, default_queue_weights);
+		// Do re-marking based on outer tag
+		sw_w32_mask(0, BIT(port % 32), RTL839X_RMK_PORT_DEI_TAG_CTRL(port));
+	}
+
+	// Remap dot1p priorities to internal priority, for this the outer tag needs be re-marked
+	v = 0;
+	for (p = 0; p < 8; p++)
+		v |= (dot1p_priority_remapping[p] & 0x7) << (p * 3);
+	sw_w32(v, RTL839X_PRI_SEL_IPRI_REMAP);
+	
+	/* Configure Drop Precedence for Drop Eligible Indicator (DEI)
+	 * Index 0: 0
+	 * Index 1: 2
+	 * Each indicator is 2 bits long
+	 */
+	sw_w32(2 << 2, RTL839X_PRI_SEL_DEI2DP_REMAP);
+
+	// Re-mark DEI: 4 bit-fields of 2 bits each, field 0 is bits 0-1, ...
+	sw_w32((0x1 << 2) | (0x1 << 4), RTL839X_RMK_DEI_CTRL);
+
+	/* Set Congestion avoidance drop probability to 0 for drop precedences 0-2 (bits 24-31)
+	 * low threshold (bits 0-11) to 4095 and high threshold (bits 12-23) to 4095
+	 * Weighted Random Early Detection (WRED) is used
+	 */
+	sw_w32(4095 << 12| 4095, RTL839X_WRED_PORT_THR_CTRL(0));
+	sw_w32(4095 << 12| 4095, RTL839X_WRED_PORT_THR_CTRL(1));
+	sw_w32(4095 << 12| 4095, RTL839X_WRED_PORT_THR_CTRL(2));
+
+	/* Set queue-based congestion avoidance properties, register fields are as
+	 * for forward RTL839X_WRED_PORT_THR_CTRL
+	 */
+	for (q = 0; q < 8; q++) {
+		sw_w32(255 << 24 | 78 << 12 | 68, RTL839X_WRED_QUEUE_THR_CTRL(q, 0));
+		sw_w32(255 << 24 | 74 << 12 | 64, RTL839X_WRED_QUEUE_THR_CTRL(q, 0));
+		sw_w32(255 << 24 | 70 << 12 | 60, RTL839X_WRED_QUEUE_THR_CTRL(q, 0));
+	}
+}
+
+void __init rtl83xx_setup_qos(struct rtl838x_switch_priv *priv)
+{
+	switch_priv = priv;
+
+	pr_info("In %s\n", __func__);
+
+	if (priv->family_id == RTL8380_FAMILY_ID)
+		return rtl838x_config_qos();
+	else if (priv->family_id == RTL8390_FAMILY_ID)
+		return rtl839x_config_qos();
+
+	if (priv->family_id == RTL8380_FAMILY_ID)
+		rtl838x_rate_control_init(priv);
+	else if (priv->family_id == RTL8390_FAMILY_ID)
+		rtl839x_rate_control_init(priv);
+	
+}
diff --git a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.h b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.h
index 1ebb4dff72..d5ca153a10 100644
--- a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.h
+++ b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.h
@@ -8,14 +8,18 @@
 /*
  * Register definition
  */
-#define RTL838X_CPU_PORT			28
-#define RTL839X_CPU_PORT			52
-
 #define RTL838X_MAC_PORT_CTRL(port)		(0xd560 + (((port) << 7)))
 #define RTL839X_MAC_PORT_CTRL(port)		(0x8004 + (((port) << 7)))
+#define RTL930X_MAC_PORT_CTRL(port)		(0x3260 + (((port) << 6)))
+#define RTL930X_MAC_L2_PORT_CTRL(port)		(0x3268 + (((port) << 6)))
+#define RTL931X_MAC_PORT_CTRL(port)		(0x6004 + (((port) << 7)))
+
 #define RTL838X_RST_GLB_CTRL_0			(0x003c)
+
 #define RTL838X_MAC_FORCE_MODE_CTRL		(0xa104)
 #define RTL839X_MAC_FORCE_MODE_CTRL		(0x02bc)
+#define RTL930X_MAC_FORCE_MODE_CTRL		(0xCA1C)
+#define RTL931X_MAC_FORCE_MODE_CTRL		(0x0DCC)
 
 #define RTL838X_DMY_REG31			(0x3b28)
 #define RTL838X_SDS_MODE_SEL			(0x0028)
@@ -23,20 +27,25 @@
 #define RTL838X_INT_MODE_CTRL			(0x005c)
 #define RTL838X_CHIP_INFO			(0x00d8)
 #define RTL839X_CHIP_INFO			(0x0ff4)
-#define RTL838X_SDS4_REG28			(0xef80)
-#define RTL838X_SDS4_DUMMY0			(0xef8c)
-#define RTL838X_SDS5_EXT_REG6			(0xf18c)
 #define RTL838X_PORT_ISO_CTRL(port)		(0x4100 + ((port) << 2))
 #define RTL839X_PORT_ISO_CTRL(port)		(0x1400 + ((port) << 3))
-#define RTL8380_SDS4_FIB_REG0			(0xF800)
+
+/* Packet statistics */
 #define RTL838X_STAT_PORT_STD_MIB		(0x1200)
 #define RTL839X_STAT_PORT_STD_MIB		(0xC000)
+#define RTL930X_STAT_PORT_MIB_CNTR		(0x0664)
 #define RTL838X_STAT_RST			(0x3100)
 #define RTL839X_STAT_RST			(0xF504)
+#define RTL930X_STAT_RST			(0x3240)
+#define RTL931X_STAT_RST			(0x7ef4)
 #define RTL838X_STAT_PORT_RST			(0x3104)
 #define RTL839X_STAT_PORT_RST			(0xF508)
+#define RTL930X_STAT_PORT_RST			(0x3244)
+#define RTL931X_STAT_PORT_RST			(0x7ef8)
 #define RTL838X_STAT_CTRL			(0x3108)
 #define RTL839X_STAT_CTRL			(0x04cc)
+#define RTL930X_STAT_CTRL			(0x3248)
+#define RTL931X_STAT_CTRL			(0x5720)
 
 /* Registers of the internal Serdes of the 8390 */
 #define RTL8390_SDS0_1_XSG0			(0xA000)
@@ -47,53 +56,95 @@
 #define RTL839X_SDS12_13_PWR1			(0xb980)
 
 /* Registers of the internal Serdes of the 8380 */
-#define MAPLE_SDS4_REG0r			RTL838X_SDS4_REG28
-#define MAPLE_SDS5_REG0r			(RTL838X_SDS4_REG28 + 0x100)
-#define MAPLE_SDS4_REG3r			RTL838X_SDS4_DUMMY0
-#define MAPLE_SDS5_REG3r			(RTL838X_SDS4_REG28 + 0x100)
-#define MAPLE_SDS4_FIB_REG0r			(RTL838X_SDS4_REG28 + 0x880)
-#define MAPLE_SDS5_FIB_REG0r			(RTL838X_SDS4_REG28 + 0x980)
+#define RTL838X_SDS4_FIB_REG0			(0xF800)
+#define RTL838X_SDS4_REG28			(0xef80)
+#define RTL838X_SDS4_DUMMY0			(0xef8c)
+#define RTL838X_SDS5_EXT_REG6			(0xf18c)
 
 /* VLAN registers */
+#define RTL838X_VLAN_CTRL			(0x3A74)
 #define RTL838X_VLAN_PROFILE(idx)		(0x3A88 + ((idx) << 2))
 #define RTL838X_VLAN_PORT_EGR_FLTR		(0x3A84)
-#define RTL838X_VLAN_PORT_PB_VLAN(port)		(0x3C00 + ((port) << 2))
+#define RTL838X_VLAN_PORT_PB_VLAN		(0x3C00)
 #define RTL838X_VLAN_PORT_IGR_FLTR(port)	(0x3A7C + (((port >> 4) << 2)))
 #define RTL838X_VLAN_PORT_IGR_FLTR_0		(0x3A7C)
 #define RTL838X_VLAN_PORT_IGR_FLTR_1		(0x3A7C + 4)
-#define RTL838X_VLAN_PORT_TAG_STS_CTRL(port)	(0xA530 + (((port) << 2)))
+#define RTL838X_VLAN_PORT_TAG_STS_CTRL		(0xA530)
+
 #define RTL839X_VLAN_PROFILE(idx)		(0x25C0 + (((idx) << 3)))
 #define RTL839X_VLAN_CTRL			(0x26D4)
-#define RTL839X_VLAN_PORT_PB_VLAN(port)		(0x26D8 + (((port) << 2)))
+#define RTL839X_VLAN_PORT_PB_VLAN		(0x26D8)
 #define RTL839X_VLAN_PORT_IGR_FLTR(port)	(0x27B4 + (((port >> 4) << 2)))
 #define RTL839X_VLAN_PORT_EGR_FLTR(port)	(0x27C4 + (((port >> 5) << 2)))
-#define RTL839X_VLAN_PORT_TAG_STS_CTRL(port)	(0x6828 + (((port) << 2)))
+#define RTL839X_VLAN_PORT_TAG_STS_CTRL		(0x6828)
+
+#define RTL930X_VLAN_PROFILE_SET(idx)		(0x9c60 + (((idx) * 20)))
+#define RTL930X_VLAN_CTRL			(0x82D4)
+#define RTL930X_VLAN_PORT_PB_VLAN		(0x82D8)
+#define RTL930X_VLAN_PORT_IGR_FLTR(port)	(0x83C0 + (((port >> 4) << 2)))
+#define RTL930X_VLAN_PORT_EGR_FLTR		(0x83C8)
+#define RTL930X_VLAN_PORT_TAG_STS_CTRL		(0xCE24)
+
+#define RTL931X_VLAN_PROFILE_SET(idx)		(0x9800 + (((idx) * 28)))
+#define RTL931X_VLAN_CTRL			(0x94E4)
+#define RTL931X_VLAN_PORT_IGR_FLTR(port)	(0x96B4 + (((port >> 4) << 2)))
+#define RTL931X_VLAN_PORT_EGR_FLTR(port)	(0x96C4 + (((port >> 5) << 2)))
+#define RTL931X_VLAN_PORT_TAG_CTRL		(0x4860)
 
-/* Table 0/1 access registers */
+/* Table access registers */
 #define RTL838X_TBL_ACCESS_CTRL_0		(0x6914)
 #define RTL838X_TBL_ACCESS_DATA_0(idx)		(0x6918 + ((idx) << 2))
 #define RTL838X_TBL_ACCESS_CTRL_1		(0xA4C8)
 #define RTL838X_TBL_ACCESS_DATA_1(idx)		(0xA4CC + ((idx) << 2))
+
 #define RTL839X_TBL_ACCESS_CTRL_0		(0x1190)
 #define RTL839X_TBL_ACCESS_DATA_0(idx)		(0x1194 + ((idx) << 2))
 #define RTL839X_TBL_ACCESS_CTRL_1		(0x6b80)
 #define RTL839X_TBL_ACCESS_DATA_1(idx)		(0x6b84 + ((idx) << 2))
+#define RTL839X_TBL_ACCESS_CTRL_2		(0x611C)
+#define RTL839X_TBL_ACCESS_DATA_2(i)		(0x6120 + (((i) << 2)))
+
+#define RTL930X_TBL_ACCESS_CTRL_0		(0xB340)
+#define RTL930X_TBL_ACCESS_DATA_0(idx)		(0xB344 + ((idx) << 2))
+#define RTL930X_TBL_ACCESS_CTRL_1		(0xB3A0)
+#define RTL930X_TBL_ACCESS_DATA_1(idx)		(0xB3A4 + ((idx) << 2))
+#define RTL930X_TBL_ACCESS_CTRL_2		(0xCE04)
+#define RTL930X_TBL_ACCESS_DATA_2(i)		(0xCE08 + (((i) << 2)))
+
+#define RTL931X_TBL_ACCESS_CTRL_0		(0x8500)
+#define RTL931X_TBL_ACCESS_DATA_0(idx)		(0x8508 + ((idx) << 2))
+#define RTL931X_TBL_ACCESS_CTRL_1		(0x40C0)
+#define RTL931X_TBL_ACCESS_DATA_1(idx)		(0x40C4 + ((idx) << 2))
+#define RTL931X_TBL_ACCESS_CTRL_2		(0x8528)
+#define RTL931X_TBL_ACCESS_DATA_2(i)		(0x852C + (((i) << 2)))
+#define RTL931X_TBL_ACCESS_CTRL_3		(0x0200)
+#define RTL931X_TBL_ACCESS_DATA_3(i)		(0x0204 + (((i) << 2)))
+#define RTL931X_TBL_ACCESS_CTRL_4		(0x20DC)
+#define RTL931X_TBL_ACCESS_DATA_4(i)		(0x20E0 + (((i) << 2)))
+#define RTL931X_TBL_ACCESS_CTRL_5		(0x7E1C)
+#define RTL931X_TBL_ACCESS_DATA_5(i)		(0x7E20 + (((i) << 2)))
 
 /* MAC handling */
 #define RTL838X_MAC_LINK_STS			(0xa188)
 #define RTL839X_MAC_LINK_STS			(0x0390)
-#define RTL838X_MAC_LINK_SPD_STS(port)		(0xa190 + (((port >> 4) << 2)))
-#define RTL839X_MAC_LINK_SPD_STS(port)		(0x03a0 + (((port >> 4) << 2)))
+#define RTL930X_MAC_LINK_STS			(0xCB10)
+#define RTL931X_MAC_LINK_STS			(0x0EC0)
+#define RTL838X_MAC_LINK_SPD_STS(p)		(0xa190 + (((p >> 4) << 2)))
+#define RTL839X_MAC_LINK_SPD_STS(p)		(0x03a0 + (((p >> 4) << 2)))
+#define RTL930X_MAC_LINK_SPD_STS(p)		(0xCB18 + (((p >> 3) << 2)))
+#define RTL931X_MAC_LINK_SPD_STS(p)		(0x0ED0 + (((p >> 3) << 2)))
 #define RTL838X_MAC_LINK_DUP_STS		(0xa19c)
 #define RTL839X_MAC_LINK_DUP_STS		(0x03b0)
+#define RTL930X_MAC_LINK_DUP_STS		(0xCB28)
+#define RTL931X_MAC_LINK_DUP_STS		(0x0EF0)
 #define RTL838X_MAC_TX_PAUSE_STS		(0xa1a0)
 #define RTL839X_MAC_TX_PAUSE_STS		(0x03b8)
+#define RTL930X_MAC_TX_PAUSE_STS		(0xCB2C)
+#define RTL931X_MAC_TX_PAUSE_STS		(0x0EF8)
 #define RTL838X_MAC_RX_PAUSE_STS		(0xa1a4)
 #define RTL839X_MAC_RX_PAUSE_STS		(0x03c0)
-#define RTL838X_EEE_TX_TIMER_GIGA_CTRL		(0xaa04)
-#define RTL838X_EEE_TX_TIMER_GELITE_CTRL	(0xaa08)
-
-#define RTL838X_DMA_IF_CTRL			(0x9f58)
+#define RTL930X_MAC_RX_PAUSE_STS		(0xCB30)
+#define RTL931X_MAC_RX_PAUSE_STS		(0x0F00)
 
 /* MAC link state bits */
 #define FORCE_EN				(1 << 0)
@@ -108,36 +159,71 @@
 #define RTL838X_EEE_PORT_TX_EN			(0x014c)
 #define RTL838X_EEE_PORT_RX_EN			(0x0150)
 #define RTL838X_EEE_CLK_STOP_CTRL		(0x0148)
+#define RTL838X_EEE_TX_TIMER_GIGA_CTRL		(0xaa04)
+#define RTL838X_EEE_TX_TIMER_GELITE_CTRL	(0xaa08)
 
 /* L2 functionality */
 #define RTL838X_L2_CTRL_0			(0x3200)
 #define RTL839X_L2_CTRL_0			(0x3800)
+#define RTL930X_L2_CTRL				(0x8FD8)
+#define RTL931X_L2_CTRL				(0xC800)
 #define RTL838X_L2_CTRL_1			(0x3204)
 #define RTL839X_L2_CTRL_1			(0x3804)
+#define RTL930X_L2_AGE_CTRL			(0x8FDC)
+#define RTL931X_L2_AGE_CTRL			(0xC804)
 #define RTL838X_L2_PORT_AGING_OUT		(0x3358)
 #define RTL839X_L2_PORT_AGING_OUT		(0x3b74)
+#define	RTL930X_L2_PORT_AGE_CTRL		(0x8FE0)
+#define	RTL931X_L2_PORT_AGE_CTRL		(0xc808)
 #define RTL838X_TBL_ACCESS_L2_CTRL		(0x6900)
 #define RTL839X_TBL_ACCESS_L2_CTRL		(0x1180)
+#define RTL930X_TBL_ACCESS_L2_CTRL		(0xB320)
+#define RTL930X_TBL_ACCESS_L2_METHOD_CTRL	(0xB324)
 #define RTL838X_TBL_ACCESS_L2_DATA(idx)		(0x6908 + ((idx) << 2))
 #define RTL839X_TBL_ACCESS_L2_DATA(idx)		(0x1184 + ((idx) << 2))
+#define RTL930X_TBL_ACCESS_L2_DATA(idx)		(0xab08 + ((idx) << 2))
 #define RTL838X_L2_TBL_FLUSH_CTRL		(0x3370)
 #define RTL839X_L2_TBL_FLUSH_CTRL		(0x3ba0)
+#define RTL930X_L2_TBL_FLUSH_CTRL		(0x9404)
+#define RTL931X_L2_TBL_FLUSH_CTRL		(0xCD9C)
+
 #define RTL838X_L2_PORT_NEW_SALRN(p)		(0x328c + (((p >> 4) << 2)))
 #define RTL839X_L2_PORT_NEW_SALRN(p)		(0x38F0 + (((p >> 4) << 2)))
+#define RTL930X_L2_PORT_SALRN(p)		(0x8FEC + (((p >> 4) << 2)))
+#define RTL931X_L2_PORT_NEW_SALRN(p)		(0xC820 + (((p >> 4) << 2)))
 #define RTL838X_L2_PORT_NEW_SA_FWD(p)		(0x3294 + (((p >> 4) << 2)))
 #define RTL839X_L2_PORT_NEW_SA_FWD(p)		(0x3900 + (((p >> 4) << 2)))
-#define RTL838X_L2_PORT_SALRN(p)		(0x328c + (((p >> 4) << 2)))
-#define RTL839X_L2_PORT_SALRN(p)		(0x38F0 + (((p >> 4) << 2)))
+#define RTL930X_L2_PORT_NEW_SA_FWD(p)		(0x8FF4 + (((p / 10) << 2)))
+#define RTL931X_L2_PORT_NEW_SA_FWD(p)		(0xC830 + (((p / 10) << 2)))
+
+#define RTL930X_ST_CTRL				(0x8798)
+
+#define RTL930X_L2_PORT_SABLK_CTRL		(0x905c)
+#define RTL930X_L2_PORT_DABLK_CTRL		(0x9060)
+
+#define RTL838X_RMA_BPDU_FLD_PMSK		(0x4348)
+#define RTL930X_RMA_BPDU_FLD_PMSK		(0x9F18)
+#define RTL931X_RMA_BPDU_FLD_PMSK		(0x8950)
+#define RTL839X_RMA_BPDU_FLD_PMSK		(0x125C)
 
 /* Port Mirroring */
-#define RTL838X_MIR_CTRL(grp)			(0x5D00 + (((grp) << 2)))
-#define RTL838X_MIR_DPM_CTRL(grp)		(0x5D20 + (((grp) << 2)))
-#define RTL838X_MIR_SPM_CTRL(grp)		(0x5D10 + (((grp) << 2)))
-#define RTL839X_MIR_CTRL(grp)			(0x2500 + (((grp) << 2)))
-#define RTL839X_MIR_DPM_CTRL(grp)		(0x2530 + (((grp) << 2)))
-#define RTL839X_MIR_SPM_CTRL(grp)		(0x2510 + (((grp) << 2)))
-
-/* Storm control */
+#define RTL838X_MIR_CTRL			(0x5D00)
+#define RTL838X_MIR_DPM_CTRL			(0x5D20)
+#define RTL838X_MIR_SPM_CTRL			(0x5D10)
+
+#define RTL839X_MIR_CTRL			(0x2500)
+#define RTL839X_MIR_DPM_CTRL			(0x2530)
+#define RTL839X_MIR_SPM_CTRL			(0x2510)
+
+#define RTL930X_MIR_CTRL			(0xA2A0)
+#define RTL930X_MIR_DPM_CTRL			(0xA2C0)
+#define RTL930X_MIR_SPM_CTRL			(0xA2B0)
+
+#define RTL931X_MIR_CTRL			(0xAF00)
+#define RTL931X_MIR_DPM_CTRL			(0xAF30)
+#define RTL931X_MIR_SPM_CTRL			(0xAF10)
+
+/* Storm/rate control and scheduling */
 #define RTL838X_STORM_CTRL			(0x4700)
 #define RTL839X_STORM_CTRL			(0x1800)
 #define RTL838X_STORM_CTRL_LB_CTRL(p)		(0x4884 + (((p) << 2)))
@@ -145,12 +231,23 @@
 #define RTL838X_STORM_CTRL_BURST_PPS_1		(0x4878)
 #define RTL838X_STORM_CTRL_BURST_0		(0x487c)
 #define RTL838X_STORM_CTRL_BURST_1		(0x4880)
+#define RTL839X_STORM_CTRL_LB_TICK_TKN_CTRL_0	(0x1804)
+#define RTL839X_STORM_CTRL_LB_TICK_TKN_CTRL_1	(0x1808)
 #define RTL838X_SCHED_CTRL			(0xB980)
+#define RTL839X_SCHED_CTRL			(0x60F4)
 #define RTL838X_SCHED_LB_TICK_TKN_CTRL_0	(0xAD58)
 #define RTL838X_SCHED_LB_TICK_TKN_CTRL_1	(0xAD5C)
 #define RTL839X_SCHED_LB_TICK_TKN_CTRL_0	(0x1804)
 #define RTL839X_SCHED_LB_TICK_TKN_CTRL_1	(0x1808)
+#define RTL839X_STORM_CTRL_SPCL_LB_TICK_TKN_CTRL (0x2000)
+#define RTL839X_IGR_BWCTRL_LB_TICK_TKN_CTRL_0	(0x1604)
+#define RTL839X_IGR_BWCTRL_LB_TICK_TKN_CTRL_1	(0x1608)
+#define RTL839X_SCHED_LB_TICK_TKN_CTRL		(0x60F8)
+#define RTL839X_SCHED_LB_TICK_TKN_PPS_CTRL	(0x6200)
 #define RTL838X_SCHED_LB_THR			(0xB984)
+#define RTL839X_SCHED_LB_THR			(0x60FC)
+#define RTL838X_SCHED_P_EGR_RATE_CTRL(p)	(0xC008 + (((p) << 7)))
+#define RTL838X_SCHED_Q_EGR_RATE_CTRL(p, q)	(0xC00C + (p << 7) + (((q) << 2)))
 #define RTL838X_STORM_CTRL_PORT_BC_EXCEED	(0x470C)
 #define RTL838X_STORM_CTRL_PORT_MC_EXCEED	(0x4710)
 #define RTL838X_STORM_CTRL_PORT_UC_EXCEED	(0x4714)
@@ -160,6 +257,25 @@
 #define RTL838X_STORM_CTRL_PORT_UC(p)		(0x4718 + (((p) << 2)))
 #define RTL838X_STORM_CTRL_PORT_MC(p)		(0x478c + (((p) << 2)))
 #define RTL838X_STORM_CTRL_PORT_BC(p)		(0x4800 + (((p) << 2)))
+#define RTL839X_STORM_CTRL_PORT_UC_0(p)		(0x185C + (((p) << 3)))
+#define RTL839X_STORM_CTRL_PORT_UC_1(p)		(0x1860 + (((p) << 3)))
+#define RTL839X_STORM_CTRL_PORT_MC_0(p)		(0x19FC + (((p) << 3)))
+#define RTL839X_STORM_CTRL_PORT_MC_1(p)		(0x1a00 + (((p) << 3)))
+#define RTL839X_STORM_CTRL_PORT_BC_0(p)		(0x1B9C + (((p) << 3)))
+#define RTL839X_STORM_CTRL_PORT_BC_1(p)		(0x1BA0 + (((p) << 3)))
+#define RTL839X_TBL_ACCESS_CTRL_2		(0x611C)
+#define RTL839X_TBL_ACCESS_DATA_2(i)		(0x6120 + (((i) << 2)))
+#define RTL839X_IGR_BWCTRL_PORT_CTRL_10G_0(p)	(0x1618 + (((p) << 3)))
+#define RTL839X_IGR_BWCTRL_PORT_CTRL_10G_1(p)	(0x161C + (((p) << 3)))
+#define RTL839X_IGR_BWCTRL_PORT_CTRL_0(p)	(0x1640 + (((p) << 3)))
+#define RTL839X_IGR_BWCTRL_PORT_CTRL_1(p)	(0x1644 + (((p) << 3)))
+#define RTL839X_IGR_BWCTRL_CTRL_LB_THR		(0x1614)
+
+/* Link aggregation (Trunking) */
+#define RTL839X_TRK_MBR_CTR			(0x2200)
+#define RTL838X_TRK_MBR_CTR			(0x3E00)
+#define RTL930X_TRK_MBR_CTRL			(0xA41C)
+#define RTL931X_TRK_MBR_CTRL			(0xB8D0)
 
 /* Attack prevention */
 #define RTL838X_ATK_PRVNT_PORT_EN		(0x5B00)
@@ -167,6 +283,48 @@
 #define RTL838X_ATK_PRVNT_ACT			(0x5B08)
 #define RTL838X_ATK_PRVNT_STS			(0x5B1C)
 
+/* 802.1X */
+#define RTL838X_SPCL_TRAP_EAPOL_CTRL		(0x6988)
+#define RTL839X_SPCL_TRAP_EAPOL_CTRL		(0x105C)
+
+/* QoS */
+#define RTL838X_QM_INTPRI2QID_CTRL		(0x5F00)
+#define RTL839X_QM_INTPRI2QID_CTRL(q)		(0x1110 + (q << 2))
+#define RTL839X_QM_PORT_QNUM(p)			(0x1130 + (((p / 10) << 2)))
+#define RTL838X_PRI_SEL_PORT_PRI(p)		(0x5FB8 + (((p / 10) << 2)))
+#define RTL839X_PRI_SEL_PORT_PRI(p)		(0x10A8 + (((p / 10) << 2)))
+#define RTL838X_QM_PKT2CPU_INTPRI_MAP		(0x5F10)
+#define RTL839X_QM_PKT2CPU_INTPRI_MAP		(0x1154)
+#define RTL838X_PRI_SEL_CTRL			(0x10E0)
+#define RTL839X_PRI_SEL_CTRL			(0x10E0)
+#define RTL838X_PRI_SEL_TBL_CTRL(i)		(0x5FD8 + (((i) << 2)))
+#define RTL839X_PRI_SEL_TBL_CTRL(i)		(0x10D0 + (((i) << 2)))
+#define RTL838X_QM_PKT2CPU_INTPRI_0		(0x5F04)
+#define RTL838X_QM_PKT2CPU_INTPRI_1		(0x5F08)
+#define RTL838X_QM_PKT2CPU_INTPRI_2		(0x5F0C)
+#define RTL839X_OAM_CTRL			(0x2100)
+#define RTL839X_OAM_PORT_ACT_CTRL(p)	 	(0x2104 + (((p) << 2)))
+#define RTL839X_RMK_PORT_DEI_TAG_CTRL(p)	(0x6A9C + (((p >> 5) << 2)))
+#define RTL839X_PRI_SEL_IPRI_REMAP		(0x1080)
+#define RTL838X_PRI_SEL_IPRI_REMAP		(0x5F8C)
+#define RTL839X_PRI_SEL_DEI2DP_REMAP		(0x10EC)
+#define RTL839X_PRI_SEL_DSCP2DP_REMAP_ADDR(i)	(0x10F0 + (((i >> 4) << 2)))
+#define RTL839X_RMK_DEI_CTRL			(0x6AA4)
+#define RTL839X_WRED_PORT_THR_CTRL(i)		(0x6084 + ((i) << 2))
+#define RTL839X_WRED_QUEUE_THR_CTRL(q, i) 	(0x6090 + ((q) * 12) + ((i) << 2))
+#define RTL838X_PRI_DSCP_INVLD_CTRL0		(0x5FE8)
+#define RTL838X_RMK_IPRI_CTRL			(0xA460)
+#define RTL838X_RMK_OPRI_CTRL			(0xA464)
+#define RTL838X_SCHED_P_TYPE_CTRL(p)		(0xC04C + (((p) << 7)))
+#define RTL838X_SCHED_LB_CTRL(p)		(0xC004 + (((p) << 7)))
+#define RTL838X_FC_P_EGR_DROP_CTRL(p)		(0x6B1C + (((p) << 2)))
+
+/* Debug features */
+#define RTL930X_STAT_PRVTE_DROP_COUNTER0	(0xB5B8)
+
+#define MAX_LAGS 16
+#define MAX_PRIOS 8
+
 enum phy_type {
 	PHY_NONE = 0,
 	PHY_RTL838X_SDS = 1,
@@ -182,6 +340,9 @@ struct rtl838x_port {
 	u16 pvid;
 	bool eee_enabled;
 	enum phy_type phy;
+	bool is10G;
+	bool is2G5;
+	u8 sds_num;
 	const struct dsa_port *dp;
 };
 
@@ -217,6 +378,8 @@ struct rtl838x_l2_entry {
 	bool suspended;
 	bool next_hop;
 	int age;
+	u8 trunk;
+	u8 stackDev;
 	u16 mc_portmask_index;
 };
 
@@ -231,8 +394,12 @@ struct rtl838x_reg {
 	u64 (*get_port_reg_le)(int reg);
 	int stat_port_rst;
 	int stat_rst;
-	int (*stat_port_std_mib)(int p);
+	int stat_port_std_mib;
 	int (*port_iso_ctrl)(int p);
+	void (*traffic_enable)(int source, int dest);
+	void (*traffic_disable)(int source, int dest);
+	void (*traffic_set)(int source, u64 dest_matrix);
+	u64 (*traffic_get)(int source);
 	int l2_ctrl_0;
 	int l2_ctrl_1;
 	int l2_port_aging_out;
@@ -248,13 +415,16 @@ struct rtl838x_reg {
 	void (*vlan_tables_read)(u32 vlan, struct rtl838x_vlan_info *info);
 	void (*vlan_set_tagged)(u32 vlan, struct rtl838x_vlan_info *info);
 	void (*vlan_set_untagged)(u32 vlan, u64 portmask);
+	void (*vlan_profile_dump)(int index);
+	void (*stp_get)(struct rtl838x_switch_priv *priv, u16 msti, u32 port_state[]);
+	void (*stp_set)(struct rtl838x_switch_priv *priv, u16 msti, u32 port_state[]);
 	int  (*mac_force_mode_ctrl)(int port);
 	int  (*mac_port_ctrl)(int port);
 	int  (*l2_port_new_salrn)(int port);
 	int  (*l2_port_new_sa_fwd)(int port);
-	int  (*mir_ctrl)(int group);
-	int  (*mir_dpm)(int group);
-	int  (*mir_spm)(int group);
+	int mir_ctrl;
+	int mir_dpm;
+	int mir_spm;
 	int mac_link_sts;
 	int mac_link_dup_sts;
 	int  (*mac_link_spd_sts)(int port);
@@ -262,11 +432,14 @@ struct rtl838x_reg {
 	int mac_tx_pause_sts;
 	u64 (*read_l2_entry_using_hash)(u32 hash, u32 position, struct rtl838x_l2_entry *e);
 	u64 (*read_cam)(int idx, struct rtl838x_l2_entry *e);
-	int (*vlan_profile)(int profile);
-	int (*vlan_port_egr_filter)(int port);
-	int (*vlan_port_igr_filter)(int port);
-	int (*vlan_port_pb)(int port);
-	int (*vlan_port_tag_sts_ctrl)(int port);
+	int vlan_port_egr_filter;
+	int vlan_port_igr_filter;
+	int vlan_port_pb;
+	int vlan_port_tag_sts_ctrl;
+	int (*rtl838x_vlan_port_tag_sts_ctrl)(int port);
+	int (*trk_mbr_ctr)(int group);
+	int rma_bpdu_fld_pmask;
+	int spcl_trap_eapol_ctrl;
 };
 
 struct rtl838x_switch_priv {
@@ -276,7 +449,7 @@ struct rtl838x_switch_priv {
 	u16 id;
 	u16 family_id;
 	char version;
-	struct rtl838x_port ports[54]; /* TODO: correct size! */
+	struct rtl838x_port ports[57];
 	struct mutex reg_mutex;
 	int link_state_irq;
 	int mirror_group_ports[4];
@@ -284,8 +457,14 @@ struct rtl838x_switch_priv {
 	const struct rtl838x_reg *r;
 	u8 cpu_port;
 	u8 port_mask;
+	u8 port_width;
+	u64 irq_mask;
 	u32 fib_entries;
 	struct dentry *dbgfs_dir;
+	int n_lags;
+	u64 lags_port_members[MAX_LAGS];
+	struct net_device *lag_devs[MAX_LAGS];
+	struct notifier_block nb;
 };
 
 void rtl838x_dbgfs_init(struct rtl838x_switch_priv *priv);
diff --git a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl839x.c b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl839x.c
index 8dd123f609..5106bd2e9d 100644
--- a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl839x.c
+++ b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl839x.c
@@ -4,53 +4,18 @@
 #include "rtl83xx.h"
 
 extern struct mutex smi_lock;
+extern struct rtl83xx_soc_info soc_info;
 
-
-static inline void rtl839x_mask_port_reg_be(u64 clear, u64 set, int reg)
-{
-	sw_w32_mask((u32)(clear >> 32), (u32)(set >> 32), reg);
-	sw_w32_mask((u32)(clear & 0xffffffff), (u32)(set & 0xffffffff), reg + 4);
-}
-
-static inline u64 rtl839x_get_port_reg_be(int reg)
+void rtl839x_print_matrix(void)
 {
-	u64 v = sw_r32(reg);
-
-	v <<= 32;
-	v |= sw_r32(reg + 4);
-	return v;
-}
-
-static inline void rtl839x_set_port_reg_be(u64 set, int reg)
-{
-	sw_w32(set >> 32, reg);
-	sw_w32(set & 0xffffffff, reg + 4);
-}
-
-static inline void rtl839x_mask_port_reg_le(u64 clear, u64 set, int reg)
-{
-	sw_w32_mask((u32)clear, (u32)set, reg);
-	sw_w32_mask((u32)(clear >> 32), (u32)(set >> 32), reg + 4);
-}
-
-static inline void rtl839x_set_port_reg_le(u64 set, int reg)
-{
-	sw_w32(set, reg);
-	sw_w32(set >> 32, reg + 4);
-}
-
-static inline u64 rtl839x_get_port_reg_le(int reg)
-{
-	u64 v = sw_r32(reg + 4);
-
-	v <<= 32;
-	v |= sw_r32(reg);
-	return v;
-}
+	volatile u64 *ptr9;
+	int i;
 
-static inline int rtl839x_stat_port_std_mib(int p)
-{
-	return RTL839X_STAT_PORT_STD_MIB + (p << 8);
+	ptr9 = RTL838X_SW_BASE + RTL839X_PORT_ISO_CTRL(0);
+	for (i = 0; i < 52; i += 4)
+		pr_debug("> %16llx %16llx %16llx %16llx\n",
+			ptr9[i + 0], ptr9[i + 1], ptr9[i + 2], ptr9[i + 3]);
+	pr_debug("CPU_PORT> %16llx\n", ptr9[52]);
 }
 
 static inline int rtl839x_port_iso_ctrl(int p)
@@ -70,6 +35,12 @@ static inline void rtl839x_exec_tbl1_cmd(u32 cmd)
 	do { } while (sw_r32(RTL839X_TBL_ACCESS_CTRL_1) & BIT(16));
 }
 
+inline void rtl839x_exec_tbl2_cmd(u32 cmd)
+{
+	sw_w32(cmd, RTL839X_TBL_ACCESS_CTRL_2);
+	do { } while (sw_r32(RTL839X_TBL_ACCESS_CTRL_2) & (1 << 9));
+}
+
 static inline int rtl839x_tbl_access_data_0(int i)
 {
 	return RTL839X_TBL_ACCESS_DATA_0(i);
@@ -81,33 +52,33 @@ static void rtl839x_vlan_tables_read(u32 vlan, struct rtl838x_vlan_info *info)
 	u64 v;
 	u32 u, w;
 
-	cmd = BIT(16) /* Execute cmd */
+	cmd = 1 << 16 /* Execute cmd */
 		| 0 << 15 /* Read */
 		| 0 << 12 /* Table type 0b000 */
 		| (vlan & 0xfff);
 	rtl839x_exec_tbl0_cmd(cmd);
 
-	v = sw_r32(RTL838X_TBL_ACCESS_DATA_0(0));
+	v = sw_r32(RTL839X_TBL_ACCESS_DATA_0(0));
 	v <<= 32;
-	u = sw_r32(RTL838X_TBL_ACCESS_DATA_0(1));
+	u = sw_r32(RTL839X_TBL_ACCESS_DATA_0(1));
 	v |= u;
 	info->tagged_ports = v >> 11;
 
-	w = sw_r32(RTL838X_TBL_ACCESS_DATA_0(2));
+	w = sw_r32(RTL839X_TBL_ACCESS_DATA_0(2));
 
 	info->profile_id = w >> 30 | ((u & 1) << 2);
 	info->hash_mc_fid = !!(u & 2);
 	info->hash_uc_fid = !!(u & 4);
 	info->fid = (u >> 3) & 0xff;
 
-	cmd = BIT(16) /* Execute cmd */
-		| 0 << 15 /* Read */
-		| 0 << 12 /* Table type 0b000 */
+	cmd = 1 << 15 /* Execute cmd */
+		| 0 << 14 /* Read */
+		| 0 << 12 /* Table type 0b00 */
 		| (vlan & 0xfff);
 	rtl839x_exec_tbl1_cmd(cmd);
-	v = sw_r32(RTL838X_TBL_ACCESS_DATA_1(0));
+	v = sw_r32(RTL839X_TBL_ACCESS_DATA_1(0));
 	v <<= 32;
-	v |= sw_r32(RTL838X_TBL_ACCESS_DATA_1(1));
+	v |= sw_r32(RTL839X_TBL_ACCESS_DATA_1(1));
 	info->untagged_ports = v >> 11;
 }
 
@@ -161,45 +132,18 @@ static inline int rtl839x_l2_port_new_sa_fwd(int p)
 	return RTL839X_L2_PORT_NEW_SA_FWD(p);
 }
 
-static inline int rtl839x_mir_ctrl(int group)
-{
-	return RTL839X_MIR_CTRL(group);
-}
-
-static inline int rtl839x_mir_dpm(int group)
-{
-	return RTL839X_MIR_DPM_CTRL(group);
-}
-
-static inline int rtl839x_mir_spm(int group)
-{
-	return RTL839X_MIR_SPM_CTRL(group);
-}
-
 static inline int rtl839x_mac_link_spd_sts(int p)
 {
 	return RTL839X_MAC_LINK_SPD_STS(p);
 }
 
-static u64 rtl839x_read_l2_entry_using_hash(u32 hash, u32 position, struct rtl838x_l2_entry *e)
+static inline int rtl839x_trk_mbr_ctr(int group)
 {
-	u64 entry;
-	u32 r[3];
-
-	/* Search in SRAM, with hash and at position in hash bucket (0-3) */
-	u32 idx = (0 << 14) | (hash << 2) | position;
-
-	u32 cmd = BIT(17) /* Execute cmd */
-		| 0 << 16 /* Read */
-		| 0 << 14 /* Table type 0b00 */
-		| (idx & 0x3fff);
-
-	sw_w32(cmd, RTL839X_TBL_ACCESS_L2_CTRL);
-	do { }  while (sw_r32(RTL839X_TBL_ACCESS_L2_CTRL) & BIT(17));
-	r[0] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(0));
-	r[1] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(1));
-	r[2] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(2));
+	return RTL839X_TRK_MBR_CTR + (group << 3);
+}
 
+static void rtl839x_fill_l2_entry(u32 r[], struct rtl838x_l2_entry *e)
+{
 	/* Table contains different entry types, we need to identify the right one:
 	 * Check for MC entries, first
 	 */
@@ -220,12 +164,12 @@ static u64 rtl839x_read_l2_entry_using_hash(u32 hash, u32 position, struct rtl83
 			e->vid = (r[2] >> 4) & 0xfff;
 			e->rvid = (r[0] >> 20) & 0xfff;
 			e->port = (r[2] >> 24) & 0x3f;
-			e->block_da = !!(r[2] & BIT(19));
-			e->block_sa = !!(r[2] & BIT(20));
-			e->suspended = !!(r[2] & BIT(17));
-			e->next_hop = !!(r[2] & BIT(16));
+			e->block_da = !!(r[2] & (1 << 19));
+			e->block_sa = !!(r[2] & (1 << 20));
+			e->suspended = !!(r[2] & (1 << 17));
+			e->next_hop = !!(r[2] & (1 << 16));
 			if (e->next_hop)
-				pr_debug("Found next hop entry, need to read data\n");
+				pr_info("Found next hop entry, need to read data\n");
 			e->age = (r[2] >> 21) & 3;
 			e->valid = true;
 			if (!(r[2] & 0xc0fd0000)) /* Check for valid entry */
@@ -246,6 +190,28 @@ static u64 rtl839x_read_l2_entry_using_hash(u32 hash, u32 position, struct rtl83
 		e->valid = true;
 		e->type = IP6_MULTICAST;
 	}
+}
+
+static u64 rtl839x_read_l2_entry_using_hash(u32 hash, u32 position, struct rtl838x_l2_entry *e)
+{
+	u64 entry;
+	u32 r[3];
+
+	/* Search in SRAM, with hash and at position in hash bucket (0-3) */
+	u32 idx = (0 << 14) | (hash << 2) | position;
+
+	u32 cmd = 1 << 17 /* Execute cmd */
+		| 0 << 16 /* Read */
+		| 0 << 14 /* Table type 0b00 */
+		| (idx & 0x3fff);
+
+	sw_w32(cmd, RTL839X_TBL_ACCESS_L2_CTRL);
+	do { }  while (sw_r32(RTL839X_TBL_ACCESS_L2_CTRL) & (1 << 17));
+	r[0] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(0));
+	r[1] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(1));
+	r[2] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(2));
+
+	rtl839x_fill_l2_entry(r, e);
 
 	entry = (((u64) r[0]) << 12) | ((r[1] & 0xfffffff0) << 12) | ((r[2] >> 4) & 0xfff);
 	return entry;
@@ -256,33 +222,22 @@ static u64 rtl839x_read_cam(int idx, struct rtl838x_l2_entry *e)
 	u64 entry;
 	u32 r[3];
 
-	u32 cmd = BIT(17) /* Execute cmd */
+	u32 cmd = 1 << 17 /* Execute cmd */
 		| 0 << 16 /* Read */
-		| BIT(14) /* Table type 0b01 */
+		| 1 << 14 /* Table type 0b01 */
 		| (idx & 0x3f);
 	sw_w32(cmd, RTL839X_TBL_ACCESS_L2_CTRL);
-	do { }  while (sw_r32(RTL839X_TBL_ACCESS_L2_CTRL) & BIT(17));
+	do { }  while (sw_r32(RTL839X_TBL_ACCESS_L2_CTRL) & (1 << 17));
 	r[0] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(0));
 	r[1] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(1));
 	r[2] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(2));
 
-	e->mac[0] = (r[0] >> 12);
-	e->mac[1] = (r[0] >> 4);
-	e->mac[2] = ((r[1] >> 28) | (r[0] << 4));
-	e->mac[3] = (r[1] >> 20);
-	e->mac[4] = (r[1] >> 12);
-	e->mac[5] = (r[1] >> 4);
-	e->is_static = !!((r[2] >> 18) & 1);
-	e->vid = (r[2] >> 4) & 0xfff;
-	e->rvid = (r[0] >> 20) & 0xfff;
-	e->port = (r[2] >> 24) & 0x3f;
-
-	e->valid = true;
-	if (!(r[2] & 0x10fd0000)) /* Check for invalid entry */
-		e->valid = false;
 
+	rtl839x_fill_l2_entry(r, e);
 	if (e->valid)
-		pr_debug("Found in CAM: R1 %x R2 %x R3 %x\n", r[0], r[1], r[2]);
+		pr_info("Found in CAM: R1 %x R2 %x R3 %x\n", r[0], r[1], r[2]);
+	else
+		return 0;
 
 	entry = (((u64) r[0]) << 12) | ((r[1] & 0xfffffff0) << 12) | ((r[2] >> 4) & 0xfff);
 	return entry;
@@ -303,62 +258,25 @@ static inline int rtl839x_vlan_port_igr_filter(int port)
 	return RTL839X_VLAN_PORT_IGR_FLTR(port);
 }
 
-static inline int rtl839x_vlan_port_pb(int port)
+u64 rtl839x_traffic_get(int source)
 {
-	return RTL839X_VLAN_PORT_PB_VLAN(port);
+	return rtl839x_get_port_reg_be(rtl839x_port_iso_ctrl(source));
 }
 
-static inline int rtl839x_vlan_port_tag_sts_ctrl(int port)
+void rtl839x_traffic_set(int source, u64 dest_matrix)
 {
-	return RTL839X_VLAN_PORT_TAG_STS_CTRL(port);
+	rtl839x_set_port_reg_be(dest_matrix, rtl839x_port_iso_ctrl(source));
 }
 
-const struct rtl838x_reg rtl839x_reg = {
-	.mask_port_reg_be = rtl839x_mask_port_reg_be,
-	.set_port_reg_be = rtl839x_set_port_reg_be,
-	.get_port_reg_be = rtl839x_get_port_reg_be,
-	.mask_port_reg_le = rtl839x_mask_port_reg_le,
-	.set_port_reg_le = rtl839x_set_port_reg_le,
-	.get_port_reg_le = rtl839x_get_port_reg_le,
-	.stat_port_rst = RTL839X_STAT_PORT_RST,
-	.stat_rst = RTL839X_STAT_RST,
-	.stat_port_std_mib = rtl839x_stat_port_std_mib,
-	.port_iso_ctrl = rtl839x_port_iso_ctrl,
-	.l2_ctrl_0 = RTL839X_L2_CTRL_0,
-	.l2_ctrl_1 = RTL839X_L2_CTRL_1,
-	.l2_port_aging_out = RTL839X_L2_PORT_AGING_OUT,
-	.smi_poll_ctrl = RTL839X_SMI_PORT_POLLING_CTRL,
-	.l2_tbl_flush_ctrl = RTL839X_L2_TBL_FLUSH_CTRL,
-	.exec_tbl0_cmd = rtl839x_exec_tbl0_cmd,
-	.exec_tbl1_cmd = rtl839x_exec_tbl1_cmd,
-	.tbl_access_data_0 = rtl839x_tbl_access_data_0,
-	.isr_glb_src = RTL839X_ISR_GLB_SRC,
-	.isr_port_link_sts_chg = RTL839X_ISR_PORT_LINK_STS_CHG,
-	.imr_port_link_sts_chg = RTL839X_IMR_PORT_LINK_STS_CHG,
-	.imr_glb = RTL839X_IMR_GLB,
-	.vlan_tables_read = rtl839x_vlan_tables_read,
-	.vlan_set_tagged = rtl839x_vlan_set_tagged,
-	.vlan_set_untagged = rtl839x_vlan_set_untagged,
-	.mac_force_mode_ctrl = rtl839x_mac_force_mode_ctrl,
-	.mac_port_ctrl = rtl839x_mac_port_ctrl,
-	.l2_port_new_salrn = rtl839x_l2_port_new_salrn,
-	.l2_port_new_sa_fwd = rtl839x_l2_port_new_sa_fwd,
-	.mir_ctrl = rtl839x_mir_ctrl,
-	.mir_dpm = rtl839x_mir_dpm,
-	.mir_spm = rtl839x_mir_spm,
-	.mac_link_sts = RTL839X_MAC_LINK_STS,
-	.mac_link_dup_sts = RTL839X_MAC_LINK_DUP_STS,
-	.mac_link_spd_sts = rtl839x_mac_link_spd_sts,
-	.mac_rx_pause_sts = RTL839X_MAC_RX_PAUSE_STS,
-	.mac_tx_pause_sts = RTL839X_MAC_TX_PAUSE_STS,
-	.read_l2_entry_using_hash = rtl839x_read_l2_entry_using_hash,
-	.read_cam = rtl839x_read_cam,
-	.vlan_profile = rtl839x_vlan_profile,
-	.vlan_port_egr_filter = rtl839x_vlan_port_egr_filter,
-	.vlan_port_igr_filter = rtl839x_vlan_port_igr_filter,
-	.vlan_port_pb = rtl839x_vlan_port_pb,
-	.vlan_port_tag_sts_ctrl = rtl839x_vlan_port_tag_sts_ctrl,
-};
+void rtl839x_traffic_enable(int source, int dest)
+{
+	rtl839x_mask_port_reg_be(0, BIT_ULL(dest), rtl839x_port_iso_ctrl(source));
+}
+
+void rtl839x_traffic_disable(int source, int dest)
+{
+	rtl839x_mask_port_reg_be(BIT(dest), 0, rtl839x_port_iso_ctrl(source));
+}
 
 irqreturn_t rtl839x_switch_irq(int irq, void *dev_id)
 {
@@ -512,3 +430,84 @@ void rtl839x_vlan_profile_dump(int index)
 		index, profile & 1, (profile >> 1) & 0xfff, (profile >> 13) & 0xfff,
 		(profile1) & 0xfff);
 }
+
+static void rtl839x_stp_get(struct rtl838x_switch_priv *priv, u16 msti, u32 port_state[])
+{
+	int i;
+	u32 cmd = 1 << 16 /* Execute cmd */
+		| 0 << 15 /* Read */
+		| 5 << 12 /* Table type 0b101 */
+		| (msti & 0xfff);
+	priv->r->exec_tbl0_cmd(cmd);
+
+	for (i = 0; i < 4; i++)
+		port_state[i] = sw_r32(priv->r->tbl_access_data_0(i));
+}
+
+static void rtl839x_stp_set(struct rtl838x_switch_priv *priv, u16 msti, u32 port_state[])
+{
+	int i;
+	u32 cmd = 1 << 16 /* Execute cmd */
+		| 1 << 15 /* Write */
+		| 5 << 12 /* Table type 0b101 */
+		| (msti & 0xfff);
+	for (i = 0; i < 4; i++)
+		sw_w32(port_state[i], priv->r->tbl_access_data_0(i));
+	priv->r->exec_tbl0_cmd(cmd);
+}
+
+const struct rtl838x_reg rtl839x_reg = {
+	.mask_port_reg_be = rtl839x_mask_port_reg_be,
+	.set_port_reg_be = rtl839x_set_port_reg_be,
+	.get_port_reg_be = rtl839x_get_port_reg_be,
+	.mask_port_reg_le = rtl839x_mask_port_reg_le,
+	.set_port_reg_le = rtl839x_set_port_reg_le,
+	.get_port_reg_le = rtl839x_get_port_reg_le,
+	.stat_port_rst = RTL839X_STAT_PORT_RST,
+	.stat_rst = RTL839X_STAT_RST,
+	.stat_port_std_mib = RTL839X_STAT_PORT_STD_MIB,
+	.traffic_enable = rtl839x_traffic_enable,
+	.traffic_disable = rtl839x_traffic_disable,
+	.traffic_get = rtl839x_traffic_get,
+	.traffic_set = rtl839x_traffic_set,
+	.port_iso_ctrl = rtl839x_port_iso_ctrl,
+	.l2_ctrl_0 = RTL839X_L2_CTRL_0,
+	.l2_ctrl_1 = RTL839X_L2_CTRL_1,
+	.l2_port_aging_out = RTL839X_L2_PORT_AGING_OUT,
+	.smi_poll_ctrl = RTL839X_SMI_PORT_POLLING_CTRL,
+	.l2_tbl_flush_ctrl = RTL839X_L2_TBL_FLUSH_CTRL,
+	.exec_tbl0_cmd = rtl839x_exec_tbl0_cmd,
+	.exec_tbl1_cmd = rtl839x_exec_tbl1_cmd,
+	.tbl_access_data_0 = rtl839x_tbl_access_data_0,
+	.isr_glb_src = RTL839X_ISR_GLB_SRC,
+	.isr_port_link_sts_chg = RTL839X_ISR_PORT_LINK_STS_CHG,
+	.imr_port_link_sts_chg = RTL839X_IMR_PORT_LINK_STS_CHG,
+	.imr_glb = RTL839X_IMR_GLB,
+	.vlan_tables_read = rtl839x_vlan_tables_read,
+	.vlan_set_tagged = rtl839x_vlan_set_tagged,
+	.vlan_set_untagged = rtl839x_vlan_set_untagged,
+	.vlan_profile_dump = rtl839x_vlan_profile_dump,
+	.stp_get = rtl839x_stp_get,
+	.stp_set = rtl839x_stp_set,
+	.mac_force_mode_ctrl = rtl839x_mac_force_mode_ctrl,
+	.mac_port_ctrl = rtl839x_mac_port_ctrl,
+	.l2_port_new_salrn = rtl839x_l2_port_new_salrn,
+	.l2_port_new_sa_fwd = rtl839x_l2_port_new_sa_fwd,
+	.mir_ctrl = RTL839X_MIR_CTRL,
+	.mir_dpm = RTL839X_MIR_DPM_CTRL,
+	.mir_spm = RTL839X_MIR_SPM_CTRL,
+	.mac_link_sts = RTL839X_MAC_LINK_STS,
+	.mac_link_dup_sts = RTL839X_MAC_LINK_DUP_STS,
+	.mac_link_spd_sts = rtl839x_mac_link_spd_sts,
+	.mac_rx_pause_sts = RTL839X_MAC_RX_PAUSE_STS,
+	.mac_tx_pause_sts = RTL839X_MAC_TX_PAUSE_STS,
+	.read_l2_entry_using_hash = rtl839x_read_l2_entry_using_hash,
+	.read_cam = rtl839x_read_cam,
+	.vlan_port_egr_filter = RTL839X_VLAN_PORT_EGR_FLTR(0),
+	.vlan_port_igr_filter = RTL839X_VLAN_PORT_IGR_FLTR(0),
+	.vlan_port_pb = RTL839X_VLAN_PORT_PB_VLAN,
+	.vlan_port_tag_sts_ctrl = RTL839X_VLAN_PORT_TAG_STS_CTRL,
+	.trk_mbr_ctr = rtl839x_trk_mbr_ctr,
+	.rma_bpdu_fld_pmask = RTL839X_RMA_BPDU_FLD_PMSK,
+	.spcl_trap_eapol_ctrl = RTL839X_SPCL_TRAP_EAPOL_CTRL,
+};
diff --git a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl83xx.h b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl83xx.h
index 3953512525..fd0455a6cd 100644
--- a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl83xx.h
+++ b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl83xx.h
@@ -24,7 +24,71 @@ struct rtl83xx_mib_desc {
 	const char *name;
 };
 
-void __init rtl83xx_storm_control_init(struct rtl838x_switch_priv *priv);
+/* API for switch table access */
+struct table_reg {
+	u16 addr;
+	u16 data;
+	u8  max_data;
+	u8 c_bit;
+	u8 t_bit;
+	u8 rmode;
+	u8 tbl;
+	struct mutex lock;
+};
+
+#define TBL_DESC(_addr, _data, _max_data, _c_bit, _t_bit, _rmode) \
+		{  .addr = _addr, .data = _data, .max_data = _max_data, .c_bit = _c_bit, \
+		    .t_bit = _t_bit, .rmode = _rmode \
+		}
+
+typedef enum {
+	RTL8380_TBL_L2 = 0,
+	RTL8380_TBL_0,
+	RTL8380_TBL_1,
+	RTL8390_TBL_L2,
+	RTL8390_TBL_0,
+	RTL8390_TBL_1,
+	RTL8390_TBL_2,
+	RTL9300_TBL_L2,
+	RTL9300_TBL_0,
+	RTL9300_TBL_1,
+	RTL9300_TBL_2,
+	RTL9300_TBL_HSB,
+	RTL9300_TBL_HSA,
+	RTL9310_TBL_0,
+	RTL9310_TBL_1,
+	RTL9310_TBL_2,
+	RTL9310_TBL_3,
+	RTL9310_TBL_4,
+	RTL9310_TBL_5,
+	RTL_TBL_END
+} rtl838x_tbl_reg_t;
+
+void rtl_table_init(void);
+struct table_reg *rtl_table_get(rtl838x_tbl_reg_t r, int t);
+void rtl_table_release(struct table_reg *r);
+void rtl_table_read(struct table_reg *r, int idx);
+void rtl_table_write(struct table_reg *r, int idx);
+inline u16 rtl_table_data(struct table_reg *r, int i);
+inline u32 rtl_table_data_r(struct table_reg *r, int i);
+inline void rtl_table_data_w(struct table_reg *r, u32 v, int i);
+
+void __init rtl83xx_setup_qos(struct rtl838x_switch_priv *priv);
+int read_phy(u32 port, u32 page, u32 reg, u32 *val);
+int write_phy(u32 port, u32 page, u32 reg, u32 val);
+
+/* Port register accessor functions for the RTL839x and RTL931X SoCs */
+void rtl839x_mask_port_reg_be(u64 clear, u64 set, int reg);
+u64 rtl839x_get_port_reg_be(int reg);
+void rtl839x_set_port_reg_be(u64 set, int reg);
+void rtl839x_mask_port_reg_le(u64 clear, u64 set, int reg);
+void rtl839x_set_port_reg_le(u64 set, int reg);
+u64 rtl839x_get_port_reg_le(int reg);
+
+/* Port register accessor functions for the RTL838x and RTL930X SoCs */
+void rtl838x_mask_port_reg(u64 clear, u64 set, int reg);
+void rtl838x_set_port_reg(u64 set, int reg);
+u64 rtl838x_get_port_reg(int reg);
 
 /* RTL838x-specific */
 u32 rtl838x_hash(struct rtl838x_switch_priv *priv, u64 seed);
@@ -32,6 +96,9 @@ irqreturn_t rtl838x_switch_irq(int irq, void *dev_id);
 void rtl8380_get_version(struct rtl838x_switch_priv *priv);
 void rtl838x_vlan_profile_dump(int index);
 int rtl83xx_dsa_phy_read(struct dsa_switch *ds, int phy_addr, int phy_reg);
+void rtl8380_sds_rst(int mac);
+int rtl8380_sds_power(int mac, int val);
+void rtl838x_print_matrix(void);
 
 /* RTL839x-specific */
 u32 rtl839x_hash(struct rtl838x_switch_priv *priv, u64 seed);
@@ -39,6 +106,20 @@ irqreturn_t rtl839x_switch_irq(int irq, void *dev_id);
 void rtl8390_get_version(struct rtl838x_switch_priv *priv);
 void rtl839x_vlan_profile_dump(int index);
 int rtl83xx_dsa_phy_write(struct dsa_switch *ds, int phy_addr, int phy_reg, u16 val);
+void rtl839x_exec_tbl2_cmd(u32 cmd);
+void rtl839x_print_matrix(void);
+
+/* RTL930x-specific */
+u32 rtl930x_hash(struct rtl838x_switch_priv *priv, u64 seed);
+irqreturn_t rtl930x_switch_irq(int irq, void *dev_id);
+irqreturn_t rtl839x_switch_irq(int irq, void *dev_id);
+void rtl930x_vlan_profile_dump(int index);
+int rtl9300_sds_power(int mac, int val);
+void rtl9300_sds_rst(int sds_num, u32 mode);
+void rtl930x_print_matrix(void);
+
+/* RTL931x-specific */
+irqreturn_t rtl931x_switch_irq(int irq, void *dev_id);
 
 #endif /* _NET_DSA_RTL83XX_H */
 
diff --git a/target/linux/realtek/files-5.4/drivers/net/ethernet/rtl838x_eth.c b/target/linux/realtek/files-5.4/drivers/net/ethernet/rtl838x_eth.c
index fec842674e..7931daff07 100644
--- a/target/linux/realtek/files-5.4/drivers/net/ethernet/rtl838x_eth.c
+++ b/target/linux/realtek/files-5.4/drivers/net/ethernet/rtl838x_eth.c
@@ -16,6 +16,7 @@
 #include <linux/of_mdio.h>
 #include <linux/module.h>
 #include <linux/phylink.h>
+#include <linux/pkt_sched.h>
 #include <net/dsa.h>
 #include <net/switchdev.h>
 #include <asm/cacheflush.h>
@@ -26,20 +27,24 @@
 extern struct rtl83xx_soc_info soc_info;
 
 /*
- * Maximum number of RX rings is 8, assigned by switch based on
- * packet/port priortity (not implemented)
- * Maximum number of TX rings is 2 (only ring 0 used)
- * RX ringlength needs to be at least 200, otherwise CPU and Switch
- * may gridlock.
+ * Maximum number of RX rings is 8 on RTL83XX and 32 on the 93XX
+ * The ring is assigned by switch based on packet/port priortity
+ * Maximum number of TX rings is 2, Ring 2 being the high priority
+ * ring on the RTL93xx SoCs. MAX_RING_SIZE * RING_BUFFER gives
+ * the memory used for the ring buffer.
  */
-#define RXRINGS		8
-#define RXRINGLEN	300
+#define MAX_RXRINGS	32
+#define MAX_RXLEN	100
+#define MAX_ENTRIES	(200 * 8)
 #define TXRINGS		2
-#define TXRINGLEN	160
+// BUG: TXRINGLEN can be 160
+#define TXRINGLEN	16
 #define NOTIFY_EVENTS	10
 #define NOTIFY_BLOCKS	10
 #define TX_EN		0x8
 #define RX_EN		0x4
+#define TX_EN_93XX	0x20
+#define RX_EN_93XX	0x10
 #define TX_DO		0x2
 #define WRAP		0x2
 
@@ -53,11 +58,10 @@ extern struct rtl83xx_soc_info soc_info;
 struct p_hdr {
 	uint8_t		*buf;
 	uint16_t	reserved;
-	uint16_t	size;   /* buffer size */
+	uint16_t	size;		/* buffer size */
 	uint16_t	offset;
-	uint16_t	len;    /* pkt len */
-	uint16_t	reserved2;
-	uint16_t	cpu_tag[5];
+	uint16_t	len;		/* pkt len */
+	uint16_t	cpu_tag[10];
 } __packed __aligned(1);
 
 struct n_event {
@@ -70,14 +74,14 @@ struct n_event {
 } __packed __aligned(1);
 
 struct ring_b {
-	uint32_t	rx_r[RXRINGS][RXRINGLEN];
+	uint32_t	rx_r[MAX_RXRINGS][MAX_RXLEN];
 	uint32_t	tx_r[TXRINGS][TXRINGLEN];
-	struct	p_hdr	rx_header[RXRINGS][RXRINGLEN];
+	struct	p_hdr	rx_header[MAX_RXRINGS][MAX_RXLEN];
 	struct	p_hdr	tx_header[TXRINGS][TXRINGLEN];
-	uint32_t	c_rx[RXRINGS];
+	uint32_t	c_rx[MAX_RXRINGS];
 	uint32_t	c_tx[TXRINGS];
-	uint8_t		rx_space[RXRINGS*RXRINGLEN*RING_BUFFER];
-	uint8_t		tx_space[TXRINGLEN*RING_BUFFER];
+	uint8_t		tx_space[TXRINGS * TXRINGLEN * RING_BUFFER];
+	uint8_t		*rx_space;
 };
 
 struct notify_block {
@@ -91,132 +95,230 @@ struct notify_b {
 	u32			reserved2[8];
 };
 
-inline void rtl838x_create_tx_header(struct p_hdr *h, int dest_port)
+void rtl838x_create_tx_header(struct p_hdr *h, int dest_port, int prio)
 {
+	prio &= 0x7;
+
 	if (dest_port > 0) {
-		h->cpu_tag[0] = 0x0400;
-		h->cpu_tag[1] = 0x0200;
-		h->cpu_tag[2] = 0x0000;
-		h->cpu_tag[3] = (1 << dest_port) >> 16;
-		h->cpu_tag[4] = (1 << dest_port) & 0xffff;
-	} else {
-		h->cpu_tag[0] = 0;
-		h->cpu_tag[1] = 0;
-		h->cpu_tag[2] = 0;
-		h->cpu_tag[3] = 0;
-		h->cpu_tag[4] = 0;
+		// cpu_tag[0] is reserved on the RTL83XX SoCs
+		h->cpu_tag[1] = 0x0400;
+		h->cpu_tag[2] = 0x0200;
+		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
+		if (prio >= 0)
+			h->cpu_tag[2] |= (prio | 0x8) << 12;
 	}
 }
 
-inline void rtl839x_create_tx_header(struct p_hdr *h, int dest_port)
+void rtl839x_create_tx_header(struct p_hdr *h, int dest_port, int prio)
 {
+	prio &= 0x7;
+
 	if (dest_port > 0) {
-		h->cpu_tag[0] = 0x0100;
-		h->cpu_tag[1] = ((1 << (dest_port - 32)) >> 16) | (1 << 21);
-		h->cpu_tag[2] = (1 << (dest_port - 32)) & 0xffff;
-		h->cpu_tag[3] = (1 << dest_port) >> 16;
-		h->cpu_tag[4] = (1 << dest_port) & 0xffff;
-	} else {
-		h->cpu_tag[0] = 0;
-		h->cpu_tag[1] = 0;
-		h->cpu_tag[2] = 0;
-		h->cpu_tag[3] = 0;
-		h->cpu_tag[4] = 0;
+		// cpu_tag[0] is reserved on the RTL83XX SoCs
+		h->cpu_tag[1] = 0x0100;
+		h->cpu_tag[2] = h->cpu_tag[3] = h->cpu_tag[4] = h->cpu_tag[5] = 0;
+		if (dest_port >= 32) {
+			dest_port -= 32;
+			h->cpu_tag[2] = BIT(dest_port) >> 16;
+			h->cpu_tag[3] = BIT(dest_port) & 0xffff;
+		} else {
+			h->cpu_tag[4] = BIT(dest_port) >> 16;
+			h->cpu_tag[5] = BIT(dest_port) & 0xffff;
+		}
+		h->cpu_tag[6] |= BIT(21); // Enable destination port mask use
+		// Set internal priority and AS_PRIO
+		if (prio >= 0)
+			h->cpu_tag[1] |= prio | BIT(3);
 	}
 }
 
+void rtl930x_create_tx_header(struct p_hdr *h, int dest_port, int prio)
+{
+	h->cpu_tag[0] = 0x8000;
+	h->cpu_tag[1] = 0;  // TODO: Fill port and prio
+	h->cpu_tag[2] = 0;
+	h->cpu_tag[3] = 0;
+	h->cpu_tag[4] = 0;
+	h->cpu_tag[5] = 0;
+	h->cpu_tag[6] = 0;
+	h->cpu_tag[7] = 0xffff;
+}
+
+void rtl931x_create_tx_header(struct p_hdr *h, int dest_port, int prio)
+{
+	h->cpu_tag[0] = 0x8000;
+	h->cpu_tag[1] = 0;  // TODO: Fill port and prio
+	h->cpu_tag[2] = 0;
+	h->cpu_tag[3] = 0;
+	h->cpu_tag[4] = 0;
+	h->cpu_tag[5] = 0;
+	h->cpu_tag[6] = 0;
+	h->cpu_tag[7] = 0xffff;
+}
+
+struct rtl838x_rx_q {
+	int id;
+	struct rtl838x_eth_priv *priv;
+	struct napi_struct napi;
+};
+
 struct rtl838x_eth_priv {
 	struct net_device *netdev;
 	struct platform_device *pdev;
 	void		*membase;
 	spinlock_t	lock;
 	struct mii_bus	*mii_bus;
-	struct napi_struct napi;
+	struct rtl838x_rx_q rx_qs[MAX_RXRINGS];
 	struct phylink *phylink;
 	struct phylink_config phylink_config;
 	u16 id;
 	u16 family_id;
 	const struct rtl838x_reg *r;
 	u8 cpu_port;
-	u8 port_mask;
 	u32 lastEvent;
-};
-
-static const struct rtl838x_reg rtl838x_reg = {
-	.mac_port_ctrl = rtl838x_mac_port_ctrl,
-	.dma_if_intr_sts = RTL838X_DMA_IF_INTR_STS,
-	.dma_if_intr_msk = RTL838X_DMA_IF_INTR_MSK,
-	.dma_if_ctrl = RTL838X_DMA_IF_CTRL,
-	.mac_force_mode_ctrl = rtl838x_mac_force_mode_ctrl,
-	.dma_rx_base = rtl838x_dma_rx_base,
-	.dma_tx_base = rtl838x_dma_tx_base,
-	.dma_if_rx_ring_size = rtl838x_dma_if_rx_ring_size,
-	.dma_if_rx_ring_cntr = rtl838x_dma_if_rx_ring_cntr,
-	.dma_if_rx_cur = rtl838x_dma_if_rx_cur,
-	.rst_glb_ctrl = RTL838X_RST_GLB_CTRL_0,
-	.get_mac_link_sts = rtl838x_get_mac_link_sts,
-	.get_mac_link_dup_sts = rtl838x_get_mac_link_dup_sts,
-	.get_mac_link_spd_sts = rtl838x_get_mac_link_spd_sts,
-	.get_mac_rx_pause_sts = rtl838x_get_mac_rx_pause_sts,
-	.get_mac_tx_pause_sts = rtl838x_get_mac_tx_pause_sts,
-	.mac = RTL838X_MAC,
-	.l2_tbl_flush_ctrl = RTL838X_L2_TBL_FLUSH_CTRL,
-};
-
-static const struct rtl838x_reg rtl839x_reg = {
-	.mac_port_ctrl = rtl839x_mac_port_ctrl,
-	.dma_if_intr_sts = RTL839X_DMA_IF_INTR_STS,
-	.dma_if_intr_msk = RTL839X_DMA_IF_INTR_MSK,
-	.dma_if_ctrl = RTL839X_DMA_IF_CTRL,
-	.mac_force_mode_ctrl = rtl839x_mac_force_mode_ctrl,
-	.dma_rx_base = rtl839x_dma_rx_base,
-	.dma_tx_base = rtl839x_dma_tx_base,
-	.dma_if_rx_ring_size = rtl839x_dma_if_rx_ring_size,
-	.dma_if_rx_ring_cntr = rtl839x_dma_if_rx_ring_cntr,
-	.dma_if_rx_cur = rtl839x_dma_if_rx_cur,
-	.rst_glb_ctrl = RTL839X_RST_GLB_CTRL,
-	.get_mac_link_sts = rtl839x_get_mac_link_sts,
-	.get_mac_link_dup_sts = rtl839x_get_mac_link_dup_sts,
-	.get_mac_link_spd_sts = rtl839x_get_mac_link_spd_sts,
-	.get_mac_rx_pause_sts = rtl839x_get_mac_rx_pause_sts,
-	.get_mac_tx_pause_sts = rtl839x_get_mac_tx_pause_sts,
-	.mac = RTL839X_MAC,
-	.l2_tbl_flush_ctrl = RTL839X_L2_TBL_FLUSH_CTRL,
+	u16 rxrings;
+	u16 rxringlen;
 };
 
 extern int rtl838x_phy_init(struct rtl838x_eth_priv *priv);
 extern int rtl838x_read_sds_phy(int phy_addr, int phy_reg);
 extern int rtl839x_read_sds_phy(int phy_addr, int phy_reg);
 extern int rtl839x_write_sds_phy(int phy_addr, int phy_reg, u16 v);
+extern int rtl930x_read_sds_phy(int phy_addr, int page, int phy_reg);
+extern int rtl930x_write_sds_phy(int phy_addr, int page, int phy_reg, u16 v);
+extern int rtl930x_read_mmd_phy(u32 port, u32 devnum, u32 regnum, u32 *val);
+extern int rtl930x_write_mmd_phy(u32 port, u32 devnum, u32 regnum, u32 val);
+
+/*
+ * On the RTL93XX, the RTL93XX_DMA_IF_RX_RING_CNTR track the fill level of 
+ * the rings. Writing x into these registers substracts x from its content.
+ * When the content reaches the ring size, the ASIC no longer adds
+ * packets to this receive queue.
+ */
+void rtl838x_update_cntr(int r, int released)
+{
+	// This feature is not available on RTL838x SoCs
+}
+
+void rtl839x_update_cntr(int r, int released)
+{
+	// This feature is not available on RTL839x SoCs
+}
+
+void rtl930x_update_cntr(int r, int released)
+{
+	int pos = (r % 3) * 10;
+	u32 reg = RTL930X_DMA_IF_RX_RING_CNTR + ((r / 3) << 2);
+	u32 v = sw_r32(reg);
+
+	v = (v >> pos) & 0x3ff;
+	pr_debug("RX: Work done %d, old value: %d, pos %d, reg %04x\n", released, v, pos, reg);
+	sw_w32_mask(0x3ff << pos, released << pos, reg);
+	sw_w32(v, reg);
+}
+
+void rtl931x_update_cntr(int r, int released)
+{
+	int pos = (r % 3) * 10;
+	u32 reg = RTL931X_DMA_IF_RX_RING_CNTR + ((r / 3) << 2);
+
+	sw_w32_mask(0x3ff << pos, released << pos, reg);
+}
+
+struct dsa_tag {
+	u8	reason;
+	u8	queue;
+	u16	port;
+	u8	l2_offloaded;
+	u8	prio;
+};
+
+bool rtl838x_decode_tag(struct p_hdr *h, struct dsa_tag *t)
+{
+	t->reason = h->cpu_tag[3] & 0xf;
+	if (t->reason != 15)
+	pr_debug("Reason: %d\n", t->reason);
+	t->queue = (h->cpu_tag[0] & 0xe0) >> 5;
+	if (t->reason != 4) // NIC_RX_REASON_SPECIAL_TRAP
+		t->l2_offloaded = 1;
+	else
+		t->l2_offloaded = 0;
+	t->port = h->cpu_tag[1] & 0x1f;
+
+	return t->l2_offloaded;
+}
+
+bool rtl839x_decode_tag(struct p_hdr *h, struct dsa_tag *t)
+{
+	t->reason = h->cpu_tag[4] & 0x1f;
+	if (t->reason != 31)
+	pr_debug("Reason: %d\n", t->reason);
+		t->queue = (h->cpu_tag[3] & 0xe000) >> 13;
+	if ((t->reason != 7) && (t->reason != 8)) // NIC_RX_REASON_RMA_USR
+		t->l2_offloaded = 1;
+	else
+		t->l2_offloaded = 0;
+	
+	t->port = h->cpu_tag[1] & 0x3f;
+
+	return t->l2_offloaded;
+}
+
+bool rtl931x_decode_tag(struct p_hdr *h, struct dsa_tag *t)
+{
+	t->reason = h->cpu_tag[7] & 0x3f;
+	pr_debug("Reason %d\n", t->reason);
+	t->queue =  (h->cpu_tag[2] >> 11) & 0x1f;
+	if (t->reason >= 19 && t->reason <= 27)
+		t->l2_offloaded = 0;
+	else
+		t->l2_offloaded = 1;
+	t->port = (h->cpu_tag[0] >> 8) & 0x3f;
+
+	return t->l2_offloaded;
+}
+
+bool rtl930x_decode_tag(struct p_hdr *h, struct dsa_tag *t)
+{
+	rtl931x_decode_tag(h, t);
+	t->port &= 0x1f;
+	return t->l2_offloaded;
+}
 
 /*
  * Discard the RX ring-buffers, called as part of the net-ISR
  * when the buffer runs over
  * Caller needs to hold priv->lock
  */
-static void rtl838x_rb_cleanup(struct rtl838x_eth_priv *priv)
+static void rtl838x_rb_cleanup(struct rtl838x_eth_priv *priv, int status)
 {
 	int r;
 	u32	*last;
 	struct p_hdr *h;
 	struct ring_b *ring = priv->membase;
 
-	for (r = 0; r < RXRINGS; r++) {
-		last = (u32 *)KSEG1ADDR(sw_r32(priv->r->dma_if_rx_cur(r)));
+	for (r = 0; r < priv->rxrings; r++) {
+		pr_debug("In %s working on r: %d\n", __func__, r);
+		last = (u32 *)KSEG1ADDR(sw_r32(priv->r->dma_if_rx_cur + r * 4));
 		do {
 			if ((ring->rx_r[r][ring->c_rx[r]] & 0x1))
 				break;
+			pr_debug("Got something: %d\n", ring->c_rx[r]);
 			h = &ring->rx_header[r][ring->c_rx[r]];
+			memset(h, 0, sizeof(struct p_hdr));
 			h->buf = (u8 *)KSEG1ADDR(ring->rx_space
-					+ r * ring->c_rx[r] * RING_BUFFER);
+					+ r * priv->rxringlen * RING_BUFFER
+					+ ring->c_rx[r] * RING_BUFFER);
 			h->size = RING_BUFFER;
-			h->len = 0;
 			/* make sure the header is visible to the ASIC */
 			mb();
 
 			ring->rx_r[r][ring->c_rx[r]] = KSEG1ADDR(h) | 0x1
-				| (ring->c_rx[r] == (RXRINGLEN - 1) ? WRAP : 0x1);
-			ring->c_rx[r] = (ring->c_rx[r] + 1) % RXRINGLEN;
+				| (ring->c_rx[r] == (priv->rxringlen - 1) ? WRAP : 0x1);
+			ring->c_rx[r] = (ring->c_rx[r] + 1) % priv->rxringlen;
 		} while (&ring->rx_r[r][ring->c_rx[r]] != last);
 	}
 }
@@ -229,25 +331,25 @@ struct fdb_update_work {
 
 void rtl838x_fdb_sync(struct work_struct *work)
 {
-       const struct fdb_update_work *uw =
-               container_of(work, struct fdb_update_work, work);
-       struct switchdev_notifier_fdb_info info;
-       u8 addr[ETH_ALEN];
-       int i = 0;
-       int action;
-
-       while (uw->macs[i]) {
-               action = (uw->macs[i] & (1ULL << 63)) ? SWITCHDEV_FDB_ADD_TO_BRIDGE
-                               : SWITCHDEV_FDB_DEL_TO_BRIDGE;
-               u64_to_ether_addr(uw->macs[i] & 0xffffffffffffULL, addr);
-               info.addr = &addr[0];
-               info.vid = 0;
-               info.offloaded = 1;
-               pr_debug("FDB entry %d: %llx, action %d\n", i, uw->macs[0], action);
-               call_switchdev_notifiers(action, uw->ndev, &info.info, NULL);
-               i++;
-       }
-       kfree(work);
+	const struct fdb_update_work *uw =
+		container_of(work, struct fdb_update_work, work);
+	struct switchdev_notifier_fdb_info info;
+	u8 addr[ETH_ALEN];
+	int i = 0;
+	int action;
+
+	while (uw->macs[i]) {
+		action = (uw->macs[i] & (1ULL << 63)) ? SWITCHDEV_FDB_ADD_TO_BRIDGE
+				: SWITCHDEV_FDB_DEL_TO_BRIDGE;
+		u64_to_ether_addr(uw->macs[i] & 0xffffffffffffULL, addr);
+		info.addr = &addr[0];
+		info.vid = 0;
+		info.offloaded = 1;
+		pr_debug("FDB entry %d: %llx, action %d\n", i, uw->macs[0], action);
+		call_switchdev_notifiers(action, uw->ndev, &info.info, NULL);
+		i++;
+	}
+	kfree(work);
 }
 
 static void rtl839x_l2_notification_handler(struct rtl838x_eth_priv *priv)
@@ -288,19 +390,20 @@ static void rtl839x_l2_notification_handler(struct rtl838x_eth_priv *priv)
 	priv->lastEvent = e;
 }
 
-static irqreturn_t rtl838x_net_irq(int irq, void *dev_id)
+static irqreturn_t rtl83xx_net_irq(int irq, void *dev_id)
 {
 	struct net_device *dev = dev_id;
 	struct rtl838x_eth_priv *priv = netdev_priv(dev);
 	u32 status = sw_r32(priv->r->dma_if_intr_sts);
 	bool triggered = false;
 	u32 atk = sw_r32(RTL838X_ATK_PRVNT_STS);
+	int i;
 	u32 storm_uc = sw_r32(RTL838X_STORM_CTRL_PORT_UC_EXCEED);
 	u32 storm_mc = sw_r32(RTL838X_STORM_CTRL_PORT_MC_EXCEED);
 	u32 storm_bc = sw_r32(RTL838X_STORM_CTRL_PORT_BC_EXCEED);
 
+	pr_debug("IRQ: %08x\n", status);
 	if (storm_uc || storm_mc || storm_bc) {
-
 		pr_warn("Storm control UC: %08x, MC: %08x, BC: %08x\n",
 			storm_uc, storm_mc, storm_bc);
 
@@ -325,20 +428,23 @@ static irqreturn_t rtl838x_net_irq(int irq, void *dev_id)
 
 	/* RX interrupt */
 	if (status & 0x0ff00) {
-		/* Disable RX interrupt */
-		if (triggered)
-			pr_info("RX\n");
-		sw_w32_mask(0xff00, 0, priv->r->dma_if_intr_msk);
-		sw_w32(0x0000ff00, priv->r->dma_if_intr_sts);
-		napi_schedule(&priv->napi);
+		/* ACK and disable RX interrupt for this ring */
+		sw_w32_mask(0xff00 & status, 0, priv->r->dma_if_intr_msk);
+		sw_w32(0x0000ff00 & status, priv->r->dma_if_intr_sts);
+		for (i = 0; i < priv->rxrings; i++) {
+			if (status & BIT(i + 8)) {
+				pr_debug("Scheduling queue: %d\n", i);
+				napi_schedule(&priv->rx_qs[i].napi);
+			}
+		}
 	}
 
 	/* RX buffer overrun */
 	if (status & 0x000ff) {
 		pr_info("RX buffer overrun: status %x, mask: %x\n",
 			 status, sw_r32(priv->r->dma_if_intr_msk));
-		sw_w32(0x000000ff, priv->r->dma_if_intr_sts);
-		rtl838x_rb_cleanup(priv);
+		sw_w32(status, priv->r->dma_if_intr_sts);
+		rtl838x_rb_cleanup(priv, status & 0xff);
 	}
 
 	if (priv->family_id == RTL8390_FAMILY_ID && status & 0x00100000) {
@@ -360,14 +466,185 @@ static irqreturn_t rtl838x_net_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static irqreturn_t rtl93xx_net_irq(int irq, void *dev_id)
+{
+	struct net_device *dev = dev_id;
+	struct rtl838x_eth_priv *priv = netdev_priv(dev);
+	u32 status_rx_r = sw_r32(priv->r->dma_if_intr_rx_runout_sts);
+	u32 status_rx = sw_r32(priv->r->dma_if_intr_rx_done_sts);
+	u32 status_tx = sw_r32(priv->r->dma_if_intr_tx_done_sts);
+	int i;
+
+	pr_debug("In %s, status_tx: %08x, status_rx: %08x, status_rx_r: %08x\n",
+		__func__, status_tx, status_rx, status_rx_r);
+	spin_lock(&priv->lock);
+
+	/*  Ignore TX interrupt */
+	if (status_tx) {
+		/* Clear ISR */
+		pr_debug("TX done\n");
+		sw_w32(status_tx, priv->r->dma_if_intr_tx_done_sts);
+	}
+
+	/* RX interrupt */
+	if (status_rx) {
+		pr_debug("RX IRQ\n");
+		/* ACK and disable RX interrupt for given rings */
+		sw_w32(status_rx, priv->r->dma_if_intr_rx_done_sts);
+		sw_w32_mask(status_rx, 0, priv->r->dma_if_intr_rx_done_msk);
+		for (i = 0; i < priv->rxrings; i++) {
+			if (status_rx & BIT(i)) {
+				pr_debug("Scheduling queue: %d\n", i);
+				napi_schedule(&priv->rx_qs[i].napi);
+			}
+		}
+	}
+
+	/* RX buffer overrun */
+	if (status_rx_r) {
+		pr_debug("RX buffer overrun: status %x, mask: %x\n",
+			 status_rx_r, sw_r32(priv->r->dma_if_intr_rx_runout_msk));
+		sw_w32(status_rx_r, priv->r->dma_if_intr_rx_runout_sts);
+		rtl838x_rb_cleanup(priv, status_rx_r);
+	}
+
+	spin_unlock(&priv->lock);
+	return IRQ_HANDLED;
+}
+
+static const struct rtl838x_reg rtl838x_reg = {
+	.net_irq = rtl83xx_net_irq,
+	.mac_port_ctrl = rtl838x_mac_port_ctrl,
+	.dma_if_intr_sts = RTL838X_DMA_IF_INTR_STS,
+	.dma_if_intr_msk = RTL838X_DMA_IF_INTR_MSK,
+	.dma_if_ctrl = RTL838X_DMA_IF_CTRL,
+	.mac_force_mode_ctrl = RTL838X_MAC_FORCE_MODE_CTRL,
+	.dma_rx_base = RTL838X_DMA_RX_BASE,
+	.dma_tx_base = RTL838X_DMA_TX_BASE,
+	.dma_if_rx_ring_size = rtl838x_dma_if_rx_ring_size,
+	.dma_if_rx_ring_cntr = rtl838x_dma_if_rx_ring_cntr,
+	.dma_if_rx_cur = RTL838X_DMA_IF_RX_CUR,
+	.rst_glb_ctrl = RTL838X_RST_GLB_CTRL_0,
+	.get_mac_link_sts = rtl838x_get_mac_link_sts,
+	.get_mac_link_dup_sts = rtl838x_get_mac_link_dup_sts,
+	.get_mac_link_spd_sts = rtl838x_get_mac_link_spd_sts,
+	.get_mac_rx_pause_sts = rtl838x_get_mac_rx_pause_sts,
+	.get_mac_tx_pause_sts = rtl838x_get_mac_tx_pause_sts,
+	.mac = RTL838X_MAC,
+	.l2_tbl_flush_ctrl = RTL838X_L2_TBL_FLUSH_CTRL,
+	.update_cntr = rtl838x_update_cntr,
+	.create_tx_header = rtl838x_create_tx_header,
+	.decode_tag = rtl838x_decode_tag,
+};
+
+static const struct rtl838x_reg rtl839x_reg = {
+	.net_irq = rtl83xx_net_irq,
+	.mac_port_ctrl = rtl839x_mac_port_ctrl,
+	.dma_if_intr_sts = RTL839X_DMA_IF_INTR_STS,
+	.dma_if_intr_msk = RTL839X_DMA_IF_INTR_MSK,
+	.dma_if_ctrl = RTL839X_DMA_IF_CTRL,
+	.mac_force_mode_ctrl = RTL839X_MAC_FORCE_MODE_CTRL,
+	.dma_rx_base = RTL839X_DMA_RX_BASE,
+	.dma_tx_base = RTL839X_DMA_TX_BASE,
+	.dma_if_rx_ring_size = rtl839x_dma_if_rx_ring_size,
+	.dma_if_rx_ring_cntr = rtl839x_dma_if_rx_ring_cntr,
+	.dma_if_rx_cur = RTL839X_DMA_IF_RX_CUR,
+	.rst_glb_ctrl = RTL839X_RST_GLB_CTRL,
+	.get_mac_link_sts = rtl839x_get_mac_link_sts,
+	.get_mac_link_dup_sts = rtl839x_get_mac_link_dup_sts,
+	.get_mac_link_spd_sts = rtl839x_get_mac_link_spd_sts,
+	.get_mac_rx_pause_sts = rtl839x_get_mac_rx_pause_sts,
+	.get_mac_tx_pause_sts = rtl839x_get_mac_tx_pause_sts,
+	.mac = RTL839X_MAC,
+	.l2_tbl_flush_ctrl = RTL839X_L2_TBL_FLUSH_CTRL,
+	.update_cntr = rtl839x_update_cntr,
+	.create_tx_header = rtl839x_create_tx_header,
+	.decode_tag = rtl839x_decode_tag,
+};
+
+static const struct rtl838x_reg rtl930x_reg = {
+	.net_irq = rtl93xx_net_irq,
+	.mac_port_ctrl = rtl930x_mac_port_ctrl,
+	.dma_if_intr_rx_runout_sts = RTL930X_DMA_IF_INTR_RX_RUNOUT_STS,
+	.dma_if_intr_rx_done_sts = RTL930X_DMA_IF_INTR_RX_DONE_STS,
+	.dma_if_intr_tx_done_sts = RTL930X_DMA_IF_INTR_TX_DONE_STS,
+	.dma_if_intr_rx_runout_msk = RTL930X_DMA_IF_INTR_RX_RUNOUT_MSK,
+	.dma_if_intr_rx_done_msk = RTL930X_DMA_IF_INTR_RX_DONE_MSK,
+	.dma_if_intr_tx_done_msk = RTL930X_DMA_IF_INTR_TX_DONE_MSK,
+	.l2_ntfy_if_intr_sts = RTL930X_L2_NTFY_IF_INTR_STS,
+	.l2_ntfy_if_intr_msk = RTL930X_L2_NTFY_IF_INTR_MSK,
+	.dma_if_ctrl = RTL930X_DMA_IF_CTRL,
+	.mac_force_mode_ctrl = RTL930X_MAC_FORCE_MODE_CTRL,
+	.dma_rx_base = RTL930X_DMA_RX_BASE,
+	.dma_tx_base = RTL930X_DMA_TX_BASE,
+	.dma_if_rx_ring_size = rtl930x_dma_if_rx_ring_size,
+	.dma_if_rx_ring_cntr = rtl930x_dma_if_rx_ring_cntr,
+	.dma_if_rx_cur = RTL930X_DMA_IF_RX_CUR,
+	.rst_glb_ctrl = RTL930X_RST_GLB_CTRL_0,
+	.get_mac_link_sts = rtl930x_get_mac_link_sts,
+	.get_mac_link_dup_sts = rtl930x_get_mac_link_dup_sts,
+	.get_mac_link_spd_sts = rtl930x_get_mac_link_spd_sts,
+	.get_mac_rx_pause_sts = rtl930x_get_mac_rx_pause_sts,
+	.get_mac_tx_pause_sts = rtl930x_get_mac_tx_pause_sts,
+	.mac = RTL930X_MAC_L2_ADDR_CTRL,
+	.l2_tbl_flush_ctrl = RTL930X_L2_TBL_FLUSH_CTRL,
+	.update_cntr = rtl930x_update_cntr,
+	.create_tx_header = rtl930x_create_tx_header,
+	.decode_tag = rtl930x_decode_tag,
+};
+
+static const struct rtl838x_reg rtl931x_reg = {
+	.net_irq = rtl93xx_net_irq,
+	.mac_port_ctrl = rtl931x_mac_port_ctrl,
+	.dma_if_intr_rx_runout_sts = RTL931X_DMA_IF_INTR_RX_RUNOUT_STS,
+	.dma_if_intr_rx_done_sts = RTL931X_DMA_IF_INTR_RX_DONE_STS,
+	.dma_if_intr_tx_done_sts = RTL931X_DMA_IF_INTR_TX_DONE_STS,
+	.dma_if_intr_rx_runout_msk = RTL931X_DMA_IF_INTR_RX_RUNOUT_MSK,
+	.dma_if_intr_rx_done_msk = RTL931X_DMA_IF_INTR_RX_DONE_MSK,
+	.dma_if_intr_tx_done_msk = RTL931X_DMA_IF_INTR_TX_DONE_MSK,
+	.l2_ntfy_if_intr_sts = RTL931X_L2_NTFY_IF_INTR_STS,
+	.l2_ntfy_if_intr_msk = RTL931X_L2_NTFY_IF_INTR_MSK,
+	.dma_if_ctrl = RTL931X_DMA_IF_CTRL,
+	.mac_force_mode_ctrl = RTL931X_MAC_FORCE_MODE_CTRL,
+	.dma_rx_base = RTL931X_DMA_RX_BASE,
+	.dma_tx_base = RTL931X_DMA_TX_BASE,
+	.dma_if_rx_ring_size = rtl931x_dma_if_rx_ring_size,
+	.dma_if_rx_ring_cntr = rtl931x_dma_if_rx_ring_cntr,
+	.dma_if_rx_cur = RTL931X_DMA_IF_RX_CUR,
+	.rst_glb_ctrl = RTL931X_RST_GLB_CTRL,
+	.get_mac_link_sts = rtl931x_get_mac_link_sts,
+	.get_mac_link_dup_sts = rtl931x_get_mac_link_dup_sts,
+	.get_mac_link_spd_sts = rtl931x_get_mac_link_spd_sts,
+	.get_mac_rx_pause_sts = rtl931x_get_mac_rx_pause_sts,
+	.get_mac_tx_pause_sts = rtl931x_get_mac_tx_pause_sts,
+	.mac = RTL931X_MAC_L2_ADDR_CTRL,
+	.l2_tbl_flush_ctrl = RTL931X_L2_TBL_FLUSH_CTRL,
+	.update_cntr = rtl931x_update_cntr,
+	.create_tx_header = rtl931x_create_tx_header,
+	.decode_tag = rtl931x_decode_tag,
+};
+
 static void rtl838x_hw_reset(struct rtl838x_eth_priv *priv)
 {
 	u32 int_saved, nbuf;
-
+	int i, pos;
+	
 	pr_info("RESETTING %x, CPU_PORT %d\n", priv->family_id, priv->cpu_port);
-	/* Stop TX/RX */
 	sw_w32_mask(0x3, 0, priv->r->mac_port_ctrl(priv->cpu_port));
-	mdelay(500);
+	mdelay(100);
+
+	/* Disable and clear interrupts */
+	if (priv->family_id == RTL9300_FAMILY_ID || priv->family_id == RTL9310_FAMILY_ID) {
+		sw_w32(0x00000000, priv->r->dma_if_intr_rx_runout_msk);
+		sw_w32(0xffffffff, priv->r->dma_if_intr_rx_runout_sts);
+		sw_w32(0x00000000, priv->r->dma_if_intr_rx_done_msk);
+		sw_w32(0xffffffff, priv->r->dma_if_intr_rx_done_sts);
+		sw_w32(0x00000000, priv->r->dma_if_intr_tx_done_msk);
+		sw_w32(0x0000000f, priv->r->dma_if_intr_tx_done_sts);
+	} else {
+		sw_w32(0x00000000, priv->r->dma_if_intr_msk);
+		sw_w32(0xffffffff, priv->r->dma_if_intr_sts);
+	}
 
 	if (priv->family_id == RTL8390_FAMILY_ID) {
 		/* Preserve L2 notification and NBUF settings */
@@ -382,18 +659,31 @@ static void rtl838x_hw_reset(struct rtl838x_eth_priv *priv)
 		sw_w32(0xffffffff, priv->r->dma_if_intr_sts);
 	}
 
-	/* Reset NIC and Queue */
-	sw_w32(0x08, priv->r->rst_glb_ctrl);
-	if (priv->family_id == RTL8390_FAMILY_ID)
-		sw_w32(0xffffffff, RTL839X_DMA_IF_RX_RING_CNTR);
-	do { /* Reset NIC */
-		udelay(20);
-	} while (sw_r32(priv->r->rst_glb_ctrl) & 0x08);
-	do { /* Reset Queues */
+	/* Reset NIC  */
+	if (priv->family_id == RTL9300_FAMILY_ID || priv->family_id == RTL9310_FAMILY_ID)
+		sw_w32(0x4, priv->r->rst_glb_ctrl);
+	else
+		sw_w32(0x8, priv->r->rst_glb_ctrl);
+
+	do { /* Wait for reset of NIC and Queues done */
 		udelay(20);
-	} while (sw_r32(priv->r->rst_glb_ctrl) & 0x04);
+	} while (sw_r32(priv->r->rst_glb_ctrl) & 0xc);
 	mdelay(100);
 
+	/* Setup Head of Line */
+	if (priv->family_id == RTL8380_FAMILY_ID)
+		sw_w32(0, RTL838X_DMA_IF_RX_RING_SIZE);  // Disabled on RTL8380
+	if (priv->family_id == RTL8390_FAMILY_ID)
+		sw_w32(0xffffffff, RTL839X_DMA_IF_RX_RING_CNTR);
+	if (priv->family_id == RTL9300_FAMILY_ID) {
+		for (i = 0; i < priv->rxrings; i++) {
+			pos = (i % 3) * 10;
+			sw_w32_mask(0x3ff << pos, 0, priv->r->dma_if_rx_ring_size(i));
+			sw_w32_mask(0x3ff << pos, priv->rxringlen,
+				    priv->r->dma_if_rx_ring_cntr(i));
+		}
+	}
+
 	/* Re-enable link change interrupt */
 	if (priv->family_id == RTL8390_FAMILY_ID) {
 		sw_w32(0xffffffff, RTL839X_ISR_PORT_LINK_STS_CHG);
@@ -405,32 +695,6 @@ static void rtl838x_hw_reset(struct rtl838x_eth_priv *priv)
 		sw_w32_mask(7 << 20, int_saved & (7 << 20), priv->r->dma_if_intr_msk);
 		sw_w32(nbuf, RTL839X_DMA_IF_NBUF_BASE_DESC_ADDR_CTRL);
 	}
-
-	/* Restart TX/RX to CPU port */
-	sw_w32_mask(0x0, 0x3, priv->r->mac_port_ctrl(priv->cpu_port));
-
-	if (priv->family_id == RTL8380_FAMILY_ID) {
-		/* Set Speed, duplex, flow control
-		 * FORCE_EN | LINK_EN | NWAY_EN | DUP_SEL
-		 * | SPD_SEL = 0b10 | FORCE_FC_EN | PHY_MASTER_SLV_MANUAL_EN
-		 * | MEDIA_SEL
-		 */
-		sw_w32(0x6192F, priv->r->mac_force_mode_ctrl(priv->cpu_port));
-		/* allow CRC errors on CPU-port */
-		sw_w32_mask(0, 0x8, priv->r->mac_port_ctrl(priv->cpu_port));
-	} else {
-		/* CPU port joins Lookup Miss Flooding Portmask */
-		sw_w32(0x28000, RTL839X_TBL_ACCESS_L2_CTRL);
-		sw_w32_mask(0, 0x80000000, RTL839X_TBL_ACCESS_L2_DATA(0));
-		sw_w32(0x38000, RTL839X_TBL_ACCESS_L2_CTRL);
-
-		/* Force CPU port link up */
-		sw_w32_mask(0, 3, priv->r->mac_force_mode_ctrl(priv->cpu_port));
-	}
-
-	/* Disable and clear interrupts */
-	sw_w32(0x00000000, priv->r->dma_if_intr_msk);
-	sw_w32(0xffffffff, priv->r->dma_if_intr_sts);
 }
 
 static void rtl838x_hw_ring_setup(struct rtl838x_eth_priv *priv)
@@ -438,11 +702,11 @@ static void rtl838x_hw_ring_setup(struct rtl838x_eth_priv *priv)
 	int i;
 	struct ring_b *ring = priv->membase;
 
-	for (i = 0; i < RXRINGS; i++)
-		sw_w32(KSEG1ADDR(&ring->rx_r[i]), priv->r->dma_rx_base(i));
+	for (i = 0; i < priv->rxrings; i++)
+		sw_w32(KSEG1ADDR(&ring->rx_r[i]), priv->r->dma_rx_base + i * 4);
 
 	for (i = 0; i < TXRINGS; i++)
-		sw_w32(KSEG1ADDR(&ring->tx_r[i]), priv->r->dma_tx_base(i));
+		sw_w32(KSEG1ADDR(&ring->tx_r[i]), priv->r->dma_tx_base + i * 4);
 }
 
 static void rtl838x_hw_en_rxtx(struct rtl838x_eth_priv *priv)
@@ -456,8 +720,19 @@ static void rtl838x_hw_en_rxtx(struct rtl838x_eth_priv *priv)
 	/* Enable RX done, RX overflow and TX done interrupts */
 	sw_w32(0xfffff, priv->r->dma_if_intr_msk);
 
-	/* Enable traffic, engine expects empty FCS field */
+	/* Enable DMA, engine expects empty FCS field */
 	sw_w32_mask(0, RX_EN | TX_EN, priv->r->dma_if_ctrl);
+
+	/* Restart TX/RX to CPU port */
+	sw_w32_mask(0x0, 0x3, priv->r->mac_port_ctrl(priv->cpu_port));
+	/* Set Speed, duplex, flow control
+	 * FORCE_EN | LINK_EN | NWAY_EN | DUP_SEL
+	 * | SPD_SEL = 0b10 | FORCE_FC_EN | PHY_MASTER_SLV_MANUAL_EN
+	 * | MEDIA_SEL
+	 */
+	sw_w32(0x6192F, priv->r->mac_force_mode_ctrl + priv->cpu_port * 4);
+	/* allow CRC errors on CPU-port */
+	sw_w32_mask(0, 0x8, priv->r->mac_port_ctrl(priv->cpu_port));
 }
 
 static void rtl839x_hw_en_rxtx(struct rtl838x_eth_priv *priv)
@@ -468,27 +743,71 @@ static void rtl839x_hw_en_rxtx(struct rtl838x_eth_priv *priv)
 	/* Enable Notify, RX done, RX overflow and TX done interrupts */
 	sw_w32(0x007fffff, priv->r->dma_if_intr_msk); // Notify IRQ!
 
-	/* Enable traffic */
+	/* Enable DMA */
 	sw_w32_mask(0, RX_EN | TX_EN, priv->r->dma_if_ctrl);
+
+	/* Restart TX/RX to CPU port */
+	sw_w32_mask(0x0, 0x3, priv->r->mac_port_ctrl(priv->cpu_port));
+
+	/* CPU port joins Lookup Miss Flooding Portmask */
+	// TODO: The code below should also work for the RTL838x
+	sw_w32(0x28000, RTL839X_TBL_ACCESS_L2_CTRL);
+	sw_w32_mask(0, 0x80000000, RTL839X_TBL_ACCESS_L2_DATA(0));
+	sw_w32(0x38000, RTL839X_TBL_ACCESS_L2_CTRL);
+
+	/* Force CPU port link up */
+	sw_w32_mask(0, 3, priv->r->mac_force_mode_ctrl + priv->cpu_port * 4);
 }
 
-static void rtl838x_setup_ring_buffer(struct ring_b *ring)
+static void rtl93xx_hw_en_rxtx(struct rtl838x_eth_priv *priv)
+{
+	int i, pos;
+	u32 v;
+
+	/* Setup CPU-Port: RX Buffer truncated at 1600 Bytes */
+	sw_w32(0x06400040, priv->r->dma_if_ctrl);
+
+	for (i = 0; i < priv->rxrings; i++) {
+		pos = (i % 3) * 10;
+		sw_w32_mask(0x3ff << pos, priv->rxringlen << pos, priv->r->dma_if_rx_ring_size(i));
+
+		// Some SoCs have issues with missing underflow protection
+		v = (sw_r32(priv->r->dma_if_rx_ring_cntr(i)) >> pos) & 0x3ff;
+		sw_w32_mask(0x3ff << pos, v, priv->r->dma_if_rx_ring_cntr(i));
+	}
+
+	/* Enable Notify, RX done, RX overflow and TX done interrupts */
+	sw_w32(0xffffffff, priv->r->dma_if_intr_rx_runout_msk);
+	sw_w32(0xffffffff, priv->r->dma_if_intr_rx_done_msk);
+	sw_w32(0x0000000f, priv->r->dma_if_intr_tx_done_msk);
+
+	/* Enable DMA */
+	sw_w32_mask(0, RX_EN_93XX | TX_EN_93XX, priv->r->dma_if_ctrl);
+
+	/* Restart TX/RX to CPU port */
+	sw_w32_mask(0x0, 0x3, priv->r->mac_port_ctrl(priv->cpu_port));
+
+	sw_w32_mask(0, BIT(priv->cpu_port), RTL930X_L2_UNKN_UC_FLD_PMSK);
+	sw_w32(0x217, priv->r->mac_force_mode_ctrl + priv->cpu_port * 4);
+}
+
+static void rtl838x_setup_ring_buffer(struct rtl838x_eth_priv *priv, struct ring_b *ring)
 {
 	int i, j;
 
 	struct p_hdr *h;
 
-	for (i = 0; i < RXRINGS; i++) {
-		for (j = 0; j < RXRINGLEN; j++) {
+	for (i = 0; i < priv->rxrings; i++) {
+		for (j = 0; j < priv->rxringlen; j++) {
 			h = &ring->rx_header[i][j];
-			h->buf = (u8 *)KSEG1ADDR(ring->rx_space + i * j * RING_BUFFER);
-			h->reserved = 0;
+			memset(h, 0, sizeof(struct p_hdr));
+			h->buf = (u8 *)KSEG1ADDR(ring->rx_space
+					+ i * priv->rxringlen * RING_BUFFER
+					+ j * RING_BUFFER);
 			h->size = RING_BUFFER;
-			h->offset = 0;
-			h->len = 0;
-			memset(&h->cpu_tag, 0, sizeof(uint16_t[5]));
 			/* All rings owned by switch, last one wraps */
-			ring->rx_r[i][j] = KSEG1ADDR(h) | 1 | (j == (RXRINGLEN - 1) ? WRAP : 0);
+			ring->rx_r[i][j] = KSEG1ADDR(h) | 1 
+					   | (j == (priv->rxringlen - 1) ? WRAP : 0);
 		}
 		ring->c_rx[i] = 0;
 	}
@@ -496,12 +815,11 @@ static void rtl838x_setup_ring_buffer(struct ring_b *ring)
 	for (i = 0; i < TXRINGS; i++) {
 		for (j = 0; j < TXRINGLEN; j++) {
 			h = &ring->tx_header[i][j];
-			h->buf = (u8 *)KSEG1ADDR(ring->tx_space + i * j * RING_BUFFER);
-			h->reserved = 0;
+			memset(h, 0, sizeof(struct p_hdr));
+			h->buf = (u8 *)KSEG1ADDR(ring->tx_space
+					+ i * TXRINGLEN * RING_BUFFER
+					+ j * RING_BUFFER);
 			h->size = RING_BUFFER;
-			h->offset = 0;
-			h->len = 0;
-			memset(&h->cpu_tag, 0, sizeof(uint16_t[5]));
 			ring->tx_r[i][j] = KSEG1ADDR(&ring->tx_header[i][j]);
 		}
 		/* Last header is wrapping around */
@@ -535,13 +853,14 @@ static int rtl838x_eth_open(struct net_device *ndev)
 	unsigned long flags;
 	struct rtl838x_eth_priv *priv = netdev_priv(ndev);
 	struct ring_b *ring = priv->membase;
-	int err;
+	int i, err;
 
-	pr_info("%s called: RX rings %d, TX rings %d\n", __func__, RXRINGS, TXRINGS);
+	pr_info("%s called: RX rings %d(length %d), TX rings %d(length %d)\n",
+		__func__, priv->rxrings, priv->rxringlen, TXRINGS, TXRINGLEN);
 
 	spin_lock_irqsave(&priv->lock, flags);
 	rtl838x_hw_reset(priv);
-	rtl838x_setup_ring_buffer(ring);
+	rtl838x_setup_ring_buffer(priv, ring);
 	if (priv->family_id == RTL8390_FAMILY_ID) {
 		rtl839x_setup_notify_ring_buffer(priv);
 		/* Make sure the ring structure is visible to the ASIC */
@@ -550,8 +869,7 @@ static int rtl838x_eth_open(struct net_device *ndev)
 	}
 
 	rtl838x_hw_ring_setup(priv);
-	err = request_irq(ndev->irq, rtl838x_net_irq, IRQF_SHARED,
-			ndev->name, ndev);
+	err = request_irq(ndev->irq, priv->r->net_irq, IRQF_SHARED, ndev->name, ndev);
 	if (err) {
 		netdev_err(ndev, "%s: could not acquire interrupt: %d\n",
 			   __func__, err);
@@ -559,21 +877,39 @@ static int rtl838x_eth_open(struct net_device *ndev)
 	}
 	phylink_start(priv->phylink);
 
-	napi_enable(&priv->napi);
-	netif_start_queue(ndev);
+	for (i = 0; i < priv->rxrings; i++)
+		napi_enable(&priv->rx_qs[i].napi);
 
-	if (priv->family_id == RTL8380_FAMILY_ID) {
+	switch (priv->family_id) {
+	case RTL8380_FAMILY_ID:
 		rtl838x_hw_en_rxtx(priv);
 		/* Trap IGMP traffic to CPU-Port */
 		sw_w32(0x3, RTL838X_SPCL_TRAP_IGMP_CTRL);
 		/* Flush learned FDB entries on link down of a port */
-		sw_w32_mask(0, 1 << 7, RTL838X_L2_CTRL_0);
-	} else {
+		sw_w32_mask(0, BIT(7), RTL838X_L2_CTRL_0);
+		break;
+	case RTL8390_FAMILY_ID:
 		rtl839x_hw_en_rxtx(priv);
 		sw_w32(0x3, RTL839X_SPCL_TRAP_IGMP_CTRL);
-		sw_w32_mask(0, 1 << 7, RTL839X_L2_CTRL_0);
+		/* Flush learned FDB entries on link down of a port */
+		sw_w32_mask(0, BIT(7), RTL839X_L2_CTRL_0);
+		break;
+	case RTL9300_FAMILY_ID:
+		rtl93xx_hw_en_rxtx(priv);
+		/* Flush learned FDB entries on link down of a port */
+		sw_w32_mask(0, BIT(7), RTL930X_L2_CTRL);
+		sw_w32_mask(BIT(28), 0, RTL930X_L2_PORT_SABLK_CTRL);
+		sw_w32_mask(BIT(28), 0, RTL930X_L2_PORT_DABLK_CTRL);
+		break;
+
+	case RTL9310_FAMILY_ID:
+		rtl93xx_hw_en_rxtx(priv);
+//		TODO: Add trapping of IGMP frames to CPU-port
+		break;
 	}
 
+	netif_tx_start_all_queues(ndev);
+
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	return 0;
@@ -581,10 +917,20 @@ static int rtl838x_eth_open(struct net_device *ndev)
 
 static void rtl838x_hw_stop(struct rtl838x_eth_priv *priv)
 {
-	u32 force_mac = priv->family_id == RTL8380_FAMILY_ID ? 0x6192D : 0x75;
+	u32 force_mac = priv->family_id == RTL8380_FAMILY_ID ? 0x6192C : 0x75;
 	u32 clear_irq = priv->family_id == RTL8380_FAMILY_ID ? 0x000fffff : 0x007fffff;
 	int i;
 
+	// Disable RX/TX from/to CPU-port
+	sw_w32_mask(0x3, 0, priv->r->mac_port_ctrl(priv->cpu_port));
+
+	/* Disable traffic */
+	if (priv->family_id == RTL9300_FAMILY_ID || priv->family_id == RTL9310_FAMILY_ID)
+		sw_w32_mask(RX_EN_93XX | TX_EN_93XX, 0, priv->r->dma_if_ctrl);
+	else
+		sw_w32_mask(RX_EN | TX_EN, 0, priv->r->dma_if_ctrl);
+	mdelay(200); // Test, whether this is needed
+
 	/* Block all ports */
 	if (priv->family_id == RTL8380_FAMILY_ID) {
 		sw_w32(0x03000000, RTL838X_TBL_ACCESS_DATA_0(0));
@@ -598,24 +944,33 @@ static void rtl838x_hw_stop(struct rtl838x_eth_priv *priv)
 			sw_w32(1 << 26 | 1 << 23 | i << 5, priv->r->l2_tbl_flush_ctrl);
 			do { } while (sw_r32(priv->r->l2_tbl_flush_ctrl) & (1 << 26));
 		}
-	} else {
+	} else if (priv->family_id == RTL8390_FAMILY_ID) {
 		for (i = 0; i <= priv->cpu_port; i++) {
 			sw_w32(1 << 28 | 1 << 25 | i << 5, priv->r->l2_tbl_flush_ctrl);
 			do { } while (sw_r32(priv->r->l2_tbl_flush_ctrl) & (1 << 28));
 		}
 	}
+	// TODO: L2 flush register is 64 bit on RTL931X and 930X
 
 	/* CPU-Port: Link down */
-	sw_w32(force_mac, priv->r->mac_force_mode_ctrl(priv->cpu_port));
+	if (priv->family_id == RTL8380_FAMILY_ID || priv->family_id == RTL8390_FAMILY_ID)
+		sw_w32(force_mac, priv->r->mac_force_mode_ctrl + priv->cpu_port * 4);
+	else
+		sw_w32_mask(0x3, 0, priv->r->mac_force_mode_ctrl + priv->cpu_port *4);
 	mdelay(100);
 
-	/* Disable traffic */
-	sw_w32_mask(RX_EN | TX_EN, 0, priv->r->dma_if_ctrl);
-	mdelay(200); // Test, whether this is needed
-
 	/* Disable all TX/RX interrupts */
-	sw_w32(0x00000000, priv->r->dma_if_intr_msk);
-	sw_w32(clear_irq, priv->r->dma_if_intr_sts);
+	if (priv->family_id == RTL9300_FAMILY_ID || priv->family_id == RTL9310_FAMILY_ID) {
+		sw_w32(0x00000000, priv->r->dma_if_intr_rx_runout_msk);
+		sw_w32(0xffffffff, priv->r->dma_if_intr_rx_runout_sts);
+		sw_w32(0x00000000, priv->r->dma_if_intr_rx_done_msk);
+		sw_w32(0xffffffff, priv->r->dma_if_intr_rx_done_sts);
+		sw_w32(0x00000000, priv->r->dma_if_intr_tx_done_msk);
+		sw_w32(0x0000000f, priv->r->dma_if_intr_tx_done_sts);
+	} else {
+		sw_w32(0x00000000, priv->r->dma_if_intr_msk);
+		sw_w32(clear_irq, priv->r->dma_if_intr_sts);
+	}
 
 	/* Disable TX/RX DMA */
 	sw_w32(0x00000000, priv->r->dma_if_ctrl);
@@ -625,6 +980,7 @@ static void rtl838x_hw_stop(struct rtl838x_eth_priv *priv)
 static int rtl838x_eth_stop(struct net_device *ndev)
 {
 	unsigned long flags;
+	int i;
 	struct rtl838x_eth_priv *priv = netdev_priv(ndev);
 
 	pr_info("in %s\n", __func__);
@@ -633,8 +989,12 @@ static int rtl838x_eth_stop(struct net_device *ndev)
 	phylink_stop(priv->phylink);
 	rtl838x_hw_stop(priv);
 	free_irq(ndev->irq, ndev);
-	napi_disable(&priv->napi);
-	netif_stop_queue(ndev);
+
+	for (i = 0; i < priv->rxrings; i++)
+		napi_disable(&priv->rx_qs[i].napi);
+
+	netif_tx_stop_all_queues(ndev);
+
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	return 0;
@@ -680,12 +1040,50 @@ static void rtl838x_eth_set_multicast_list(struct net_device *ndev)
 	}
 }
 
+static void rtl930x_eth_set_multicast_list(struct net_device *ndev)
+{
+	if (!(ndev->flags & (IFF_PROMISC | IFF_ALLMULTI))) {
+		sw_w32(0x0, RTL930X_RMA_CTRL_0);
+		sw_w32(0x0, RTL930X_RMA_CTRL_1);
+		sw_w32(0x0, RTL930X_RMA_CTRL_2);
+	}
+	if (ndev->flags & IFF_ALLMULTI) {
+		sw_w32(0x7fffffff, RTL930X_RMA_CTRL_0);
+		sw_w32(0x7fffffff, RTL930X_RMA_CTRL_1);
+		sw_w32(0x7fffffff, RTL930X_RMA_CTRL_2);
+	}
+	if (ndev->flags & IFF_PROMISC) {
+		sw_w32(0x7fffffff, RTL930X_RMA_CTRL_0);
+		sw_w32(0x7fffffff, RTL930X_RMA_CTRL_1);
+		sw_w32(0x7fffffff, RTL930X_RMA_CTRL_2);
+	}
+}
+
+static void rtl931x_eth_set_multicast_list(struct net_device *ndev)
+{
+	if (!(ndev->flags & (IFF_PROMISC | IFF_ALLMULTI))) {
+		sw_w32(0x0, RTL931X_RMA_CTRL_0);
+		sw_w32(0x0, RTL931X_RMA_CTRL_1);
+		sw_w32(0x0, RTL931X_RMA_CTRL_2);
+	}
+	if (ndev->flags & IFF_ALLMULTI) {
+		sw_w32(0x7fffffff, RTL931X_RMA_CTRL_0);
+		sw_w32(0x7fffffff, RTL931X_RMA_CTRL_1);
+		sw_w32(0x7fffffff, RTL931X_RMA_CTRL_2);
+	}
+	if (ndev->flags & IFF_PROMISC) {
+		sw_w32(0x7fffffff, RTL931X_RMA_CTRL_0);
+		sw_w32(0x7fffffff, RTL931X_RMA_CTRL_1);
+		sw_w32(0x7fffffff, RTL931X_RMA_CTRL_2);
+	}
+}
+
 static void rtl838x_eth_tx_timeout(struct net_device *ndev)
 {
 	unsigned long flags;
 	struct rtl838x_eth_priv *priv = netdev_priv(ndev);
 
-	pr_info("in %s\n", __func__);
+	pr_warn("%s\n", __func__);
 	spin_lock_irqsave(&priv->lock, flags);
 	rtl838x_hw_stop(priv);
 	rtl838x_hw_ring_setup(priv);
@@ -705,6 +1103,10 @@ static int rtl838x_eth_tx(struct sk_buff *skb, struct net_device *dev)
 	unsigned long flags;
 	struct p_hdr *h;
 	int dest_port = -1;
+	int q = skb_get_queue_mapping(skb) % TXRINGS;
+
+	if (q) // Check for high prio queue
+		pr_debug("SKB priority: %d\n", skb->priority);
 
 	spin_lock_irqsave(&priv->lock, flags);
 	len = skb->len;
@@ -729,44 +1131,48 @@ static int rtl838x_eth_tx(struct sk_buff *skb, struct net_device *dev)
 	}
 
 	/* We can send this packet if CPU owns the descriptor */
-	if (!(ring->tx_r[0][ring->c_tx[0]] & 0x1)) {
-		/* Set descriptor for tx */
-		h = &ring->tx_header[0][ring->c_tx[0]];
+	if (!(ring->tx_r[q][ring->c_tx[q]] & 0x1)) {
 
-		h->buf = (u8 *)KSEG1ADDR(ring->tx_space);
+		/* Set descriptor for tx */
+		h = &ring->tx_header[q][ring->c_tx[q]];
 		h->size = len;
 		h->len = len;
 
-		/* Create cpu_tag */
-		if (priv->family_id == RTL8380_FAMILY_ID)
-			rtl838x_create_tx_header(h, dest_port);
-		else
-			rtl839x_create_tx_header(h, dest_port);
+		priv->r->create_tx_header(h, dest_port, skb->priority >> 1);
 
 		/* Copy packet data to tx buffer */
 		memcpy((void *)KSEG1ADDR(h->buf), skb->data, len);
 		/* Make sure packet data is visible to ASIC */
-		mb(); /* wmb() probably works, too */
+		wmb();
 
 		/* Hand over to switch */
-		ring->tx_r[0][ring->c_tx[0]] = ring->tx_r[0][ring->c_tx[0]] | 0x1;
-
-		/* BUG: before tx fetch, need to make sure right data is accessed
-		 * This might not be necessary on newer RTL839x, though.
-		 */
-		for (i = 0; i < 10; i++) {
-			val = sw_r32(priv->r->dma_if_ctrl);
-			if ((val & 0xc) == 0xc)
-				break;
+		ring->tx_r[q][ring->c_tx[q]] |= 1;
+
+		// Before starting TX, prevent a Lextra bus bug on RTL8380 SoCs
+		if (priv->family_id == RTL8380_FAMILY_ID) {
+			for (i = 0; i < 10; i++) {
+				val = sw_r32(priv->r->dma_if_ctrl);
+				if ((val & 0xc) == 0xc)
+					break;
+			}
 		}
 
 		/* Tell switch to send data */
-		sw_w32_mask(0, TX_DO, priv->r->dma_if_ctrl);
+		if (priv->family_id == RTL9310_FAMILY_ID
+			|| priv->family_id == RTL9300_FAMILY_ID) {
+			// Ring ID q == 0: Low priority, Ring ID = 1: High prio queue
+			if (!q)
+				sw_w32_mask(0, BIT(2), priv->r->dma_if_ctrl);
+			else
+				sw_w32_mask(0, BIT(3), priv->r->dma_if_ctrl);
+		} else {
+			sw_w32_mask(0, TX_DO, priv->r->dma_if_ctrl);
+		}
 
 		dev->stats.tx_packets++;
 		dev->stats.tx_bytes += len;
 		dev_kfree_skb(skb);
-		ring->c_tx[0] = (ring->c_tx[0] + 1) % TXRINGLEN;
+		ring->c_tx[q] = (ring->c_tx[q] + 1) % TXRINGLEN;
 		ret = NETDEV_TX_OK;
 	} else {
 		dev_warn(&priv->pdev->dev, "Data is owned by switch\n");
@@ -777,6 +1183,30 @@ txdone:
 	return ret;
 }
 
+/*
+ * Return queue number for TX. On the RTL83XX, these queues have equal priority
+ * so we do round-robin
+ */
+u16 rtl83xx_pick_tx_queue(struct net_device *dev, struct sk_buff *skb,
+			  struct net_device *sb_dev)
+{
+	static u8 last = 0;
+
+	last++;
+	return last % TXRINGS;
+}
+
+/*
+ * Return queue number for TX. On the RTL93XX, queue 1 is the high priority queue
+ */
+u16 rtl93xx_pick_tx_queue(struct net_device *dev, struct sk_buff *skb,
+			  struct net_device *sb_dev)
+{
+	if (skb->priority >= TC_PRIO_CONTROL)
+		return 1;
+	return 0;
+}
+
 static int rtl838x_hw_receive(struct net_device *dev, int r, int budget)
 {
 	struct rtl838x_eth_priv *priv = netdev_priv(dev);
@@ -789,27 +1219,24 @@ static int rtl838x_hw_receive(struct net_device *dev, int r, int budget)
 	u32	*last;
 	struct p_hdr *h;
 	bool dsa = netdev_uses_dsa(dev);
+	struct dsa_tag tag;
 
 	spin_lock_irqsave(&priv->lock, flags);
-	last = (u32 *)KSEG1ADDR(sw_r32(priv->r->dma_if_rx_cur(r)));
+	last = (u32 *)KSEG1ADDR(sw_r32(priv->r->dma_if_rx_cur + r * 4));
+	pr_debug("---------------------------------------------------------- RX - %d\n", r);
 
-	if (&ring->rx_r[r][ring->c_rx[r]] == last) {
-		spin_unlock_irqrestore(&priv->lock, flags);
-		return 0;
-	}
 	do {
 		if ((ring->rx_r[r][ring->c_rx[r]] & 0x1)) {
-			netdev_warn(dev, "WARNING Ring contention: ring %x, last %x, current %x, cPTR %x, ISR %x\n", r, (uint32_t)last,
-				    (u32) &ring->rx_r[r][ring->c_rx[r]],
-				    ring->rx_r[r][ring->c_rx[r]],
-				sw_r32(priv->r->dma_if_intr_sts));
+			if (&ring->rx_r[r][ring->c_rx[r]] != last) {
+				netdev_warn(dev, "Ring contention: r: %x, last %x, cur %x\n",
+				    r, (uint32_t)last, (u32) &ring->rx_r[r][ring->c_rx[r]]);
+			}
 			break;
 		}
 
 		h = &ring->rx_header[r][ring->c_rx[r]];
 		data = (u8 *)KSEG1ADDR(h->buf);
 		len = h->len;
-
 		if (!len)
 			break;
 		work_done++;
@@ -826,7 +1253,7 @@ static int rtl838x_hw_receive(struct net_device *dev, int r, int budget)
 			/* BUG: Prevent bug on RTL838x SoCs*/
 			if (priv->family_id == RTL8380_FAMILY_ID) {
 				sw_w32(0xffffffff, priv->r->dma_if_rx_ring_size(0));
-				for (i = 0; i < RXRINGS; i++) {
+				for (i = 0; i < priv->rxrings; i++) {
 					/* Update each ring cnt */
 					val = sw_r32(priv->r->dma_if_rx_ring_cntr(i));
 					sw_w32(val, priv->r->dma_if_rx_ring_cntr(i));
@@ -839,12 +1266,19 @@ static int rtl838x_hw_receive(struct net_device *dev, int r, int budget)
 			memcpy(skb->data, (u8 *)KSEG1ADDR(data), len);
 			/* Overwrite CRC with cpu_tag */
 			if (dsa) {
+				priv->r->decode_tag(h, &tag);
 				skb->data[len-4] = 0x80;
-				skb->data[len-3] = h->cpu_tag[0] & priv->port_mask;
+				skb->data[len-3] = tag.port;
 				skb->data[len-2] = 0x10;
 				skb->data[len-1] = 0x00;
+				if (tag.l2_offloaded)
+					skb->data[len-3] |= 0x40;
 			}
 
+			if (tag.queue >= 0)
+				pr_debug("Queue: %d, len: %d, reason %d port %d\n",
+					 tag.queue, len, tag.reason, tag.port);
+
 			skb->protocol = eth_type_trans(skb, dev);
 			dev->stats.rx_packets++;
 			dev->stats.rx_bytes += len;
@@ -856,35 +1290,47 @@ static int rtl838x_hw_receive(struct net_device *dev, int r, int budget)
 			dev->stats.rx_dropped++;
 		}
 
-		h->buf = (u8 *)KSEG1ADDR(ring->rx_space
-				+ r * ring->c_rx[r] * RING_BUFFER);
+		/* Reset header structure */
+		memset(h, 0, sizeof(struct p_hdr));
+		h->buf = data;
 		h->size = RING_BUFFER;
-		h->len = 0;
-		memset(&h->cpu_tag, 0, sizeof(uint16_t[5]));
 
-		ring->rx_r[r][ring->c_rx[r]]
-			= KSEG1ADDR(h) | 0x1 | (ring->c_rx[r] == (RXRINGLEN-1) ? WRAP : 0x1);
-		ring->c_rx[r] = (ring->c_rx[r] + 1) % RXRINGLEN;
+		ring->rx_r[r][ring->c_rx[r]] = KSEG1ADDR(h) | 0x1 
+			| (ring->c_rx[r] == (priv->rxringlen - 1) ? WRAP : 0x1);
+		ring->c_rx[r] = (ring->c_rx[r] + 1) % priv->rxringlen;
+		last = (u32 *)KSEG1ADDR(sw_r32(priv->r->dma_if_rx_cur + r * 4));
 	} while (&ring->rx_r[r][ring->c_rx[r]] != last && work_done < budget);
 
+	// Update counters
+	priv->r->update_cntr(r, 0);
+
 	spin_unlock_irqrestore(&priv->lock, flags);
 	return work_done;
 }
 
 static int rtl838x_poll_rx(struct napi_struct *napi, int budget)
 {
-	struct rtl838x_eth_priv *priv = container_of(napi, struct rtl838x_eth_priv, napi);
-	int work_done = 0, r = 0;
-
-	while (work_done < budget && r < RXRINGS) {
-		work_done += rtl838x_hw_receive(priv->netdev, r, budget - work_done);
-		r++;
+	struct rtl838x_rx_q *rx_q = container_of(napi, struct rtl838x_rx_q, napi);
+	struct rtl838x_eth_priv *priv = rx_q->priv;
+	int work_done = 0;
+	int r = rx_q->id;
+	int work;
+
+	while (work_done < budget) {
+		work = rtl838x_hw_receive(priv->netdev, r, budget - work_done);
+		if (!work)
+			break;
+		work_done += work;
 	}
 
 	if (work_done < budget) {
 		napi_complete_done(napi, work_done);
+
 		/* Enable RX interrupt */
-		sw_w32_mask(0, 0xfffff, priv->r->dma_if_intr_msk);
+		if (priv->family_id == RTL9300_FAMILY_ID || priv->family_id == RTL9310_FAMILY_ID)
+			sw_w32(0xffffffff, priv->r->dma_if_intr_rx_done_msk);
+		else
+			sw_w32_mask(0, 0xf00ff | BIT(r + 8), priv->r->dma_if_intr_msk);
 	}
 	return work_done;
 }
@@ -960,9 +1406,9 @@ static void rtl838x_mac_an_restart(struct phylink_config *config)
 
 	pr_info("In %s\n", __func__);
 	/* Restart by disabling and re-enabling link */
-	sw_w32(0x6192D, priv->r->mac_force_mode_ctrl(priv->cpu_port));
+	sw_w32(0x6192D, priv->r->mac_force_mode_ctrl + priv->cpu_port * 4);
 	mdelay(20);
-	sw_w32(0x6192F, priv->r->mac_force_mode_ctrl(priv->cpu_port));
+	sw_w32(0x6192F, priv->r->mac_force_mode_ctrl + priv->cpu_port * 4);
 }
 
 static int rtl838x_mac_pcs_get_state(struct phylink_config *config,
@@ -1114,94 +1560,65 @@ static int rtl838x_set_link_ksettings(struct net_device *ndev,
 	return phylink_ethtool_ksettings_set(priv->phylink, cmd);
 }
 
-int rtl839x_read_sds_phy(int phy_addr, int phy_reg)
+static int rtl838x_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 {
-	int offset = 0;
-	int reg;
 	u32 val;
+	int err;
+	struct rtl838x_eth_priv *priv = bus->priv;
 
-	if (phy_addr == 49)
-		offset = 0x100;
-
-	/* For the RTL8393 internal SerDes, we simulate a PHY ID in registers 2/3
-	 * which would otherwise read as 0
-	 */
-	if (soc_info.id == 0x8393) {
-		if (phy_reg == 2)
-			return 0x1c;
-		if (phy_reg == 3)
-			return 0x8393;
-	}
-
-	reg = (phy_reg << 1) & 0xfc;
-	val = sw_r32(RTL839X_SDS12_13_XSG0 + offset + 0x80 + reg);
-
-	if (phy_reg & 1)
-		val = (val >> 16) & 0xffff;
-	else
-		val &= 0xffff;
+	if (mii_id >= 24 && mii_id <= 27 && priv->id == 0x8380)
+		return rtl838x_read_sds_phy(mii_id, regnum);
+	err = rtl838x_read_phy(mii_id, 0, regnum, &val);
+	if (err)
+		return err;
 	return val;
 }
 
-int rtl838x_read_sds_phy(int phy_addr, int phy_reg)
+static int rtl839x_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 {
-	int offset = 0;
 	u32 val;
+	int err;
+	struct rtl838x_eth_priv *priv = bus->priv;
 
-	if (phy_addr == 26)
-		offset = 0x100;
-	val = sw_r32(MAPLE_SDS4_FIB_REG0r + offset + (phy_reg << 2)) & 0xffff;
+	if (mii_id >= 48 && mii_id <= 49 && priv->id == 0x8393)
+		return rtl839x_read_sds_phy(mii_id, regnum);
 
+	err = rtl839x_read_phy(mii_id, 0, regnum, &val);
+	if (err)
+		return err;
 	return val;
 }
 
-int rtl839x_write_sds_phy(int phy_addr, int phy_reg, u16 v)
+static int rtl930x_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 {
-	int offset = 0;
-	int reg;
 	u32 val;
+	int err;
 
-	if (phy_addr == 49)
-		offset = 0x100;
+	// TODO: These are hard-coded for the 2 Fibre Ports of the XGS1210
+	if (mii_id >= 26 && mii_id <= 27)
+		return rtl930x_read_sds_phy(mii_id - 18, 0, regnum);
 
-	reg = (phy_reg << 1) & 0xfc;
-	val = v;
-	if (phy_reg & 1) {
-		val = val << 16;
-		sw_w32_mask(0xffff0000, val,
-			    RTL839X_SDS12_13_XSG0 + offset + 0x80 + reg);
+	if (regnum & MII_ADDR_C45) {
+		regnum &= ~MII_ADDR_C45;
+		err = rtl930x_read_mmd_phy(mii_id, regnum >> 16, regnum & 0xffff, &val);
 	} else {
-		sw_w32_mask(0xffff, val,
-			    RTL839X_SDS12_13_XSG0 + offset + 0x80 + reg);
+		err = rtl930x_read_phy(mii_id, 0, regnum, &val);
 	}
-
-	return 0;
-}
-
-static int rtl838x_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
-{
-	u32 val;
-	int err;
-	struct rtl838x_eth_priv *priv = bus->priv;
-
-	if (mii_id >= 24 && mii_id <= 27 && priv->id == 0x8380)
-		return rtl838x_read_sds_phy(mii_id, regnum);
-	err = rtl838x_read_phy(mii_id, 0, regnum, &val);
 	if (err)
 		return err;
 	return val;
 }
 
-static int rtl839x_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
+static int rtl931x_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 {
 	u32 val;
 	int err;
-	struct rtl838x_eth_priv *priv = bus->priv;
+//	struct rtl838x_eth_priv *priv = bus->priv;
 
-	if (mii_id >= 48 && mii_id <= 49 && priv->id == 0x8393)
-		return rtl839x_read_sds_phy(mii_id, regnum);
+//	if (mii_id >= 48 && mii_id <= 49 && priv->id == 0x8393)
+//		return rtl839x_read_sds_phy(mii_id, regnum);
 
-	err = rtl839x_read_phy(mii_id, 0, regnum, &val);
+	err = rtl931x_read_phy(mii_id, 0, regnum, &val);
 	if (err)
 		return err;
 	return val;
@@ -1216,7 +1633,7 @@ static int rtl838x_mdio_write(struct mii_bus *bus, int mii_id,
 	if (mii_id >= 24 && mii_id <= 27 && priv->id == 0x8380) {
 		if (mii_id == 26)
 			offset = 0x100;
-		sw_w32(value, MAPLE_SDS4_FIB_REG0r + offset + (regnum << 2));
+		sw_w32(value, RTL838X_SDS4_FIB_REG0 + offset + (regnum << 2));
 		return 0;
 	}
 	return rtl838x_write_phy(mii_id, 0, regnum, value);
@@ -1233,6 +1650,32 @@ static int rtl839x_mdio_write(struct mii_bus *bus, int mii_id,
 	return rtl839x_write_phy(mii_id, 0, regnum, value);
 }
 
+static int rtl930x_mdio_write(struct mii_bus *bus, int mii_id,
+			      int regnum, u16 value)
+{
+//	struct rtl838x_eth_priv *priv = bus->priv;
+
+//	if (mii_id >= 48 && mii_id <= 49 && priv->id == 0x8393)
+//		return rtl839x_write_sds_phy(mii_id, regnum, value);
+	if (regnum & MII_ADDR_C45) {
+		regnum &= ~MII_ADDR_C45;
+		return rtl930x_write_mmd_phy(mii_id, regnum >> 16, regnum & 0xffff, value);
+	}
+
+	return rtl930x_write_phy(mii_id, 0, regnum, value);
+}
+
+static int rtl931x_mdio_write(struct mii_bus *bus, int mii_id,
+			      int regnum, u16 value)
+{
+//	struct rtl838x_eth_priv *priv = bus->priv;
+
+//	if (mii_id >= 48 && mii_id <= 49 && priv->id == 0x8393)
+//		return rtl839x_write_sds_phy(mii_id, regnum, value);
+
+	return rtl931x_write_phy(mii_id, 0, regnum, value);
+}
+
 static int rtl838x_mdio_reset(struct mii_bus *bus)
 {
 	pr_info("%s called\n", __func__);
@@ -1262,6 +1705,62 @@ static int rtl839x_mdio_reset(struct mii_bus *bus)
 	return 0;
 }
 
+static int rtl931x_mdio_reset(struct mii_bus *bus)
+{
+	sw_w32(0x00000000, RTL931X_SMI_PORT_POLLING_CTRL);
+	sw_w32(0x00000000, RTL931X_SMI_PORT_POLLING_CTRL + 4);
+
+	pr_info("%s called\n", __func__);
+
+	return 0;
+}
+
+static int rtl930x_mdio_reset(struct mii_bus *bus)
+{
+	int i;
+	int pos;
+
+	pr_info("RTL930X_SMI_PORT0_15_POLLING_SEL %08x 16-27: %08x\n",
+		sw_r32(RTL930X_SMI_PORT0_15_POLLING_SEL),
+		sw_r32(RTL930X_SMI_PORT16_27_POLLING_SEL));
+
+	pr_info("%s: Enable SMI polling on SMI bus 0, SMI1, SMI2, disable on SMI3\n", __func__);
+	sw_w32_mask(BIT(20) | BIT(21) | BIT(22), BIT(23), RTL930X_SMI_GLB_CTRL);
+
+	pr_info("RTL9300 Powering on SerDes ports\n");
+	rtl9300_sds_power(24, 1);
+	rtl9300_sds_power(25, 1);
+	rtl9300_sds_power(26, 1);
+	rtl9300_sds_power(27, 1);
+	mdelay(200);
+
+	// RTL930X_SMI_PORT0_15_POLLING_SEL 55550000 16-27: 00f9aaaa
+	// i.e SMI=0 for all ports
+	for (i = 0; i < 5; i++)
+		pr_info("port phy: %08x\n", sw_r32(RTL930X_SMI_PORT0_5_ADDR + i *4));
+
+	// 1-to-1 mapping of port to phy-address
+	for (i = 0; i < 24; i++) {
+		pos = (i % 6) * 5;
+		sw_w32_mask(0x1f << pos, i << pos, RTL930X_SMI_PORT0_5_ADDR + (i / 6) * 4);
+	}
+
+	// ports 24 and 25 have PHY addresses 8 and 9, ports 26/27 PHY 26/27
+	sw_w32(8 | 9 << 5 | 26 << 10 | 27 << 15, RTL930X_SMI_PORT0_5_ADDR + 4 * 4);
+
+	// Ports 24 and 25 live on SMI bus 1 and 2
+	sw_w32_mask(0x3 << 16, 0x1 << 16, RTL930X_SMI_PORT16_27_POLLING_SEL);
+	sw_w32_mask(0x3 << 18, 0x2 << 18, RTL930X_SMI_PORT16_27_POLLING_SEL);
+
+	// SMI bus 1 and 2 speak Clause 45 TODO: Configure from .dts
+	sw_w32_mask(0, BIT(17) | BIT(18), RTL930X_SMI_GLB_CTRL);
+
+	// Ports 24 and 25 are 2.5 Gig, set this type (1)
+	sw_w32_mask(0x7 << 12, 1 << 12, RTL930X_SMI_MAC_TYPE_CTRL);
+	sw_w32_mask(0x7 << 15, 1 << 15, RTL930X_SMI_MAC_TYPE_CTRL);
+
+	return 0;
+}
 
 static int rtl838x_mdio_init(struct rtl838x_eth_priv *priv)
 {
@@ -1287,16 +1786,33 @@ static int rtl838x_mdio_init(struct rtl838x_eth_priv *priv)
 		goto err_put_node;
 	}
 
-	if (priv->family_id == RTL8380_FAMILY_ID) {
+	switch(priv->family_id) {
+	case RTL8380_FAMILY_ID:
 		priv->mii_bus->name = "rtl838x-eth-mdio";
 		priv->mii_bus->read = rtl838x_mdio_read;
 		priv->mii_bus->write = rtl838x_mdio_write;
 		priv->mii_bus->reset = rtl838x_mdio_reset;
-	} else {
+		break;
+	case RTL8390_FAMILY_ID:
 		priv->mii_bus->name = "rtl839x-eth-mdio";
 		priv->mii_bus->read = rtl839x_mdio_read;
 		priv->mii_bus->write = rtl839x_mdio_write;
 		priv->mii_bus->reset = rtl839x_mdio_reset;
+		break;
+	case RTL9300_FAMILY_ID:
+		priv->mii_bus->name = "rtl930x-eth-mdio";
+		priv->mii_bus->read = rtl930x_mdio_read;
+		priv->mii_bus->write = rtl930x_mdio_write;
+		priv->mii_bus->reset = rtl930x_mdio_reset;
+	//	priv->mii_bus->probe_capabilities = MDIOBUS_C22_C45; TODO for linux 5.9
+		break;
+	case RTL9310_FAMILY_ID:
+		priv->mii_bus->name = "rtl931x-eth-mdio";
+		priv->mii_bus->read = rtl931x_mdio_read;
+		priv->mii_bus->write = rtl931x_mdio_write;
+		priv->mii_bus->reset = rtl931x_mdio_reset;
+//		priv->mii_bus->probe_capabilities = MDIOBUS_C22_C45;  TODO for linux 5.9
+		break;
 	}
 	priv->mii_bus->priv = priv;
 	priv->mii_bus->parent = &priv->pdev->dev;
@@ -1325,12 +1841,46 @@ static const struct net_device_ops rtl838x_eth_netdev_ops = {
 	.ndo_open = rtl838x_eth_open,
 	.ndo_stop = rtl838x_eth_stop,
 	.ndo_start_xmit = rtl838x_eth_tx,
+	.ndo_select_queue = rtl83xx_pick_tx_queue,
 	.ndo_set_mac_address = rtl838x_set_mac_address,
 	.ndo_validate_addr = eth_validate_addr,
 	.ndo_set_rx_mode = rtl838x_eth_set_multicast_list,
 	.ndo_tx_timeout = rtl838x_eth_tx_timeout,
 };
 
+static const struct net_device_ops rtl839x_eth_netdev_ops = {
+	.ndo_open = rtl838x_eth_open,
+	.ndo_stop = rtl838x_eth_stop,
+	.ndo_start_xmit = rtl838x_eth_tx,
+	.ndo_select_queue = rtl83xx_pick_tx_queue,
+	.ndo_set_mac_address = rtl838x_set_mac_address,
+	.ndo_validate_addr = eth_validate_addr,
+	.ndo_set_rx_mode = rtl839x_eth_set_multicast_list,
+	.ndo_tx_timeout = rtl838x_eth_tx_timeout,
+};
+
+static const struct net_device_ops rtl930x_eth_netdev_ops = {
+	.ndo_open = rtl838x_eth_open,
+	.ndo_stop = rtl838x_eth_stop,
+	.ndo_start_xmit = rtl838x_eth_tx,
+	.ndo_select_queue = rtl93xx_pick_tx_queue,
+	.ndo_set_mac_address = rtl838x_set_mac_address,
+	.ndo_validate_addr = eth_validate_addr,
+	.ndo_set_rx_mode = rtl930x_eth_set_multicast_list,
+	.ndo_tx_timeout = rtl838x_eth_tx_timeout,
+};
+
+static const struct net_device_ops rtl931x_eth_netdev_ops = {
+	.ndo_open = rtl838x_eth_open,
+	.ndo_stop = rtl838x_eth_stop,
+	.ndo_start_xmit = rtl838x_eth_tx,
+	.ndo_select_queue = rtl93xx_pick_tx_queue,
+	.ndo_set_mac_address = rtl838x_set_mac_address,
+	.ndo_validate_addr = eth_validate_addr,
+	.ndo_set_rx_mode = rtl931x_eth_set_multicast_list,
+	.ndo_tx_timeout = rtl838x_eth_tx_timeout,
+};
+
 static const struct phylink_mac_ops rtl838x_phylink_ops = {
 	.validate = rtl838x_validate,
 	.mac_link_state = rtl838x_mac_pcs_get_state,
@@ -1354,7 +1904,8 @@ static int __init rtl838x_eth_probe(struct platform_device *pdev)
 	const void *mac;
 	phy_interface_t phy_mode;
 	struct phylink *phylink;
-	int err = 0;
+	int err = 0, i, rxrings, rxringlen;
+	struct ring_b *ring;
 
 	pr_info("Probing RTL838X eth device pdev: %x, dev: %x\n",
 		(u32)pdev, (u32)(&(pdev->dev)));
@@ -1364,7 +1915,13 @@ static int __init rtl838x_eth_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	dev = alloc_etherdev(sizeof(struct rtl838x_eth_priv));
+	rxrings = (soc_info.family == RTL8380_FAMILY_ID 
+			|| soc_info.family == RTL8390_FAMILY_ID) ? 8 : 32;
+	rxrings = rxrings > MAX_RXRINGS ? MAX_RXRINGS : rxrings;
+	rxringlen = MAX_ENTRIES / rxrings;
+	rxringlen = rxringlen > MAX_RXLEN ? MAX_RXLEN : rxringlen;
+
+	dev = alloc_etherdev_mqs(sizeof(struct rtl838x_eth_priv), TXRINGS, rxrings);
 	if (!dev) {
 		err = -ENOMEM;
 		goto err_free;
@@ -1392,8 +1949,8 @@ static int __init rtl838x_eth_probe(struct platform_device *pdev)
 	}
 
 	/* Allocate buffer memory */
-	priv->membase = dmam_alloc_coherent(&pdev->dev,
-				sizeof(struct ring_b) + sizeof(struct notify_b),
+	priv->membase = dmam_alloc_coherent(&pdev->dev, rxrings * rxringlen * RING_BUFFER
+				+ sizeof(struct ring_b) + sizeof(struct notify_b),
 				(void *)&dev->mem_start, GFP_KERNEL);
 	if (!priv->membase) {
 		dev_err(&pdev->dev, "cannot allocate DMA buffer\n");
@@ -1401,6 +1958,10 @@ static int __init rtl838x_eth_probe(struct platform_device *pdev)
 		goto err_free;
 	}
 
+	// Allocate ring-buffer space at the end of the allocated memory
+	ring = priv->membase;
+	ring->rx_space = priv->membase + sizeof(struct ring_b) + sizeof(struct notify_b);
+
 	spin_lock_init(&priv->lock);
 
 	/* obtain device IRQ number */
@@ -1412,6 +1973,8 @@ static int __init rtl838x_eth_probe(struct platform_device *pdev)
 		dev->irq = res->start;
 	}
 	dev->ethtool_ops = &rtl838x_ethtool_ops;
+	dev->min_mtu = ETH_ZLEN;
+	dev->max_mtu = 1536;
 
 	priv->id = soc_info.id;
 	priv->family_id = soc_info.family;
@@ -1423,15 +1986,33 @@ static int __init rtl838x_eth_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	if (priv->family_id == 0x8390) {
-		priv->cpu_port = RTL839X_CPU_PORT;
-		priv->r = &rtl839x_reg;
-		priv->port_mask = 0x3f;
-	} else {
+	switch (priv->family_id) {
+	case RTL8380_FAMILY_ID:
 		priv->cpu_port = RTL838X_CPU_PORT;
 		priv->r = &rtl838x_reg;
-		priv->port_mask = 0x1f;
+		dev->netdev_ops = &rtl838x_eth_netdev_ops;
+		break;
+	case RTL8390_FAMILY_ID:
+		priv->cpu_port = RTL839X_CPU_PORT;
+		priv->r = &rtl839x_reg;
+		dev->netdev_ops = &rtl839x_eth_netdev_ops;
+		break;
+	case RTL9300_FAMILY_ID:
+		priv->cpu_port = RTL930X_CPU_PORT;
+		priv->r = &rtl930x_reg;
+		dev->netdev_ops = &rtl930x_eth_netdev_ops;
+		break;
+	case RTL9310_FAMILY_ID:
+		priv->cpu_port = RTL931X_CPU_PORT;
+		priv->r = &rtl931x_reg;
+		dev->netdev_ops = &rtl931x_eth_netdev_ops;
+		break;
+	default:
+		pr_err("Unknown SoC family\n");
+		return -ENODEV;
 	}
+	priv->rxringlen = rxringlen;
+	priv->rxrings = rxrings;
 
 	rtl8380_init_mac(priv);
 
@@ -1464,7 +2045,6 @@ static int __init rtl838x_eth_probe(struct platform_device *pdev)
 	pr_info("Using MAC %08x%08x\n", sw_r32(priv->r->mac),
 					sw_r32(priv->r->mac + 4));
 	strcpy(dev->name, "eth%d");
-	dev->netdev_ops = &rtl838x_eth_netdev_ops;
 	priv->pdev = pdev;
 	priv->netdev = dev;
 
@@ -1476,7 +2056,12 @@ static int __init rtl838x_eth_probe(struct platform_device *pdev)
 	if (err)
 		goto err_free;
 
-	netif_napi_add(dev, &priv->napi, rtl838x_poll_rx, 64);
+	for (i = 0; i < priv->rxrings; i++) {
+		priv->rx_qs[i].id = i;
+		priv->rx_qs[i].priv = priv;
+		netif_napi_add(dev, &priv->rx_qs[i].napi, rtl838x_poll_rx, 64);
+	}
+
 	platform_set_drvdata(pdev, dev);
 
 	phy_mode = of_get_phy_mode(dn);
@@ -1508,13 +2093,18 @@ static int rtl838x_eth_remove(struct platform_device *pdev)
 {
 	struct net_device *dev = platform_get_drvdata(pdev);
 	struct rtl838x_eth_priv *priv = netdev_priv(dev);
+	int i;
 
 	if (dev) {
 		pr_info("Removing platform driver for rtl838x-eth\n");
 		rtl838x_mdio_remove(priv);
 		rtl838x_hw_stop(priv);
-		netif_stop_queue(dev);
-		netif_napi_del(&priv->napi);
+
+		netif_tx_stop_all_queues(dev);
+
+		for (i = 0; i < priv->rxrings; i++)
+			netif_napi_del(&priv->rx_qs[i].napi);
+
 		unregister_netdev(dev);
 		free_netdev(dev);
 	}



More information about the lede-commits mailing list