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@birger-koblitz.de>

1 files changed, 157 insertions, 158 deletions

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,
+};