[PATCH net-next 3/3] net: stmmac: RX queue routing configuration

@@ -84,6 +84,16 @@ Optional properties:
 			- snps,avb-algorithm: Queue to be enabled as AVB
 		- snps,map-to-dma-channel: Channel to map
 	- snps,priority: RX queue priority
+	- Specifiy RX Queue for processing certain types of packets:
+		- snps,avcp-to-queue: AV Untagged Control packets
+		- snps,ptp-to-queue: PTP Packets
+		- snps,dcbcp-to-queue: DCB Control Packets
+		- snps,up-to-queue: Untagged Packets
+		- snps,multi-broad-cast-queue: Multicast & Broadcast Packets
+		- snps,tpqc-queue: Tagged PTPoE Packets Queuing Control. Values:
+			0x0 - Priority based (RX queues non-AV)
+			0x1 - Routes to queue indicated in "snps,ptp-to-queue"
+			0x2 - Priority based (RX queues AV enabled)
 - Multiple TX Queues parameters: below the list of all the parameters to
 				 configure the multiple TX queues:
 	- snps,tx-queues-to-use: number of TX queues to be used in the driver

@@ -112,8 +122,15 @@ Examples:
 	};
 
 	mtl_rx_setup: rx-queues-config {
-		snps,rx-queues-to-use = <1>;
+		snps,rx-queues-to-use = <0>;
 		snps,rx-sched-sp;
+		snps,avcp-to-queue = <0>;
+		snps,ptp-to-queue = <0>;
+		snps,dcbcp-to-queue = <0>;
+		snps,up-to-queue = <0>;
+		snps,multi-broad-cast-queue = <0>;
+		snps,tpqc-queue = <0>;
+
 		queue0 {
 			snps,dcb-algorithm;
 			snps,map-to-dma-channel = <0x0>;

@@ -473,6 +473,15 @@ struct stmmac_ops {
 	void (*rx_queue_prio)(struct mac_device_info *hw, u32 prio, u32 queue);
 	/* TX Queues Priority */
 	void (*tx_queue_prio)(struct mac_device_info *hw, u32 prio, u32 queue);
+	/* RX Queues Routing */
+	void (*rx_queue_routing)(struct mac_device_info *hw,
+				 bool route_avcp_enable, u8 route_avcp_queue,
+				 bool route_ptp_enable, u8 route_ptp_queue,
+				 bool route_dcbcp_enable, u8 route_dcbcp_queue,
+				 bool route_up_enable, u8 route_up_queue,
+				 bool route_multi_broad_enable,
+				 u8 route_multi_broad_queue,
+				 bool route_tpqc_enable, u8 route_tpqc_queue);
 	/* Program RX Algorithms */
 	void (*prog_mtl_rx_algorithms)(struct mac_device_info *hw, u32 rx_alg);
 	/* Program TX Algorithms */

@@ -44,6 +44,24 @@
 #define GMAC_ADDR_HIGH(reg)		(0x300 + reg * 8)
 #define GMAC_ADDR_LOW(reg)		(0x304 + reg * 8)
 
+/* RX Queues Routing */
+#define GMAC_RXQCTRL_AVCPQ_MASK		GENMASK(2, 0)
+#define GMAC_RXQCTRL_AVCPQ_SHIFT	0
+#define GMAC_RXQCTRL_PTPQ_MASK		GENMASK(6, 4)
+#define GMAC_RXQCTRL_PTPQ_SHIFT		4
+#define GMAC_RXQCTRL_DCBCPQ_MASK	GENMASK(10, 8)
+#define GMAC_RXQCTRL_DCBCPQ_SHIFT	8
+#define GMAC_RXQCTRL_UPQ_MASK		GENMASK(14, 12)
+#define GMAC_RXQCTRL_UPQ_SHIFT		12
+#define GMAC_RXQCTRL_MCBCQ_MASK		GENMASK(18, 16)
+#define GMAC_RXQCTRL_MCBCQ_SHIFT	16
+#define GMAC_RXQCTRL_MCBCQEN		BIT(20)
+#define GMAC_RXQCTRL_MCBCQEN_SHIFT	20
+#define GMAC_RXQCTRL_TACPQE		BIT(21)
+#define GMAC_RXQCTRL_TACPQE_SHIFT	21
+#define GMAC_RXQCTRL_TPQC_MASK		GENMASK(24, 22)
+#define GMAC_RXQCTRL_TPQC_SHIFT		22
+
 /* MAC Packet Filtering */
 #define GMAC_PACKET_FILTER_PR		BIT(0)
 #define GMAC_PACKET_FILTER_HMC		BIT(2)

@@ -109,6 +109,68 @@ static void dwmac4_tx_queue_priority(struct mac_device_info *hw,
 	writel(value, ioaddr + base_register);
 }
 
+static void dwmac4_tx_queue_routing(struct mac_device_info *hw,
+				    bool route_avcp_enable,
+				    u8 route_avcp_queue,
+				    bool route_ptp_enable,
+				    u8 route_ptp_queue,
+				    bool route_dcbcp_enable,
+				    u8 route_dcbcp_queue,
+				    bool route_up_enable,
+				    u8 route_up_queue,
+				    bool route_multi_broad_enable,
+				    u8 route_multi_broad_queue,
+				    bool route_tpqc_enable,
+				    u8 route_tpqc_queue)
+{
+	void __iomem *ioaddr = hw->pcsr;
+	u32 value;
+
+	value = readl(ioaddr + GMAC_RXQ_CTRL1);
+
+	if (route_avcp_enable) {
+		value &= ~GMAC_RXQCTRL_AVCPQ_MASK;
+		value |= (route_avcp_queue << GMAC_RXQCTRL_AVCPQ_SHIFT) &
+			 GMAC_RXQCTRL_AVCPQ_MASK;
+		value &= ~GMAC_RXQCTRL_TACPQE;
+		value |= 0x1 << GMAC_RXQCTRL_TACPQE_SHIFT;
+	}
+
+	if (route_ptp_enable) {
+		value &= ~GMAC_RXQCTRL_PTPQ_MASK;
+		value |= (route_ptp_queue << GMAC_RXQCTRL_PTPQ_SHIFT) &
+			 GMAC_RXQCTRL_PTPQ_MASK;
+	}
+
+	if (route_dcbcp_enable) {
+		value &= ~GMAC_RXQCTRL_DCBCPQ_MASK;
+		value |= (route_dcbcp_queue << GMAC_RXQCTRL_DCBCPQ_SHIFT) &
+			 GMAC_RXQCTRL_DCBCPQ_MASK;
+	}
+
+	if (route_up_enable) {
+		value &= ~GMAC_RXQCTRL_UPQ_MASK;
+		value |= (route_up_queue << GMAC_RXQCTRL_UPQ_SHIFT) &
+			 GMAC_RXQCTRL_UPQ_MASK;
+	}
+
+	if (route_multi_broad_enable) {
+		value &= ~GMAC_RXQCTRL_MCBCQ_MASK;
+		value |= (route_multi_broad_queue << GMAC_RXQCTRL_MCBCQ_SHIFT) &
+			 GMAC_RXQCTRL_MCBCQ_MASK;
+		value &= ~GMAC_RXQCTRL_MCBCQEN;
+		value |= 0x1 << GMAC_RXQCTRL_MCBCQEN_SHIFT;
+	}
+
+	if (route_tpqc_enable) {
+		value &= ~GMAC_RXQCTRL_TPQC_MASK;
+		value |= (route_tpqc_queue << GMAC_RXQCTRL_TPQC_SHIFT) &
+			 GMAC_RXQCTRL_TPQC_MASK;
+	}
+
+	writel(value, ioaddr + GMAC_RXQ_CTRL1);
+}
+
 static void dwmac4_prog_mtl_rx_algorithms(struct mac_device_info *hw,
 					  u32 rx_alg)
 {

@@ -640,6 +702,7 @@ static const struct stmmac_ops dwmac4_ops = {
 	.rx_queue_enable = dwmac4_rx_queue_enable,
 	.rx_queue_prio = dwmac4_rx_queue_priority,
 	.tx_queue_prio = dwmac4_tx_queue_priority,
+	.rx_queue_routing = dwmac4_tx_queue_routing,
 	.prog_mtl_rx_algorithms = dwmac4_prog_mtl_rx_algorithms,
 	.prog_mtl_tx_algorithms = dwmac4_prog_mtl_tx_algorithms,
 	.set_mtl_tx_queue_weight = dwmac4_set_mtl_tx_queue_weight,

@@ -2333,6 +2333,27 @@ static void stmmac_mac_config_tx_queues_prio(struct stmmac_priv *priv)
 }
 
 /**
+ *  stmmac_mac_config_rx_queues_routing - Configure RX Queue Routing
+ *  @priv: driver private structure
+ *  Description: It is used for configuring the RX queue routing
+ */
+static void stmmac_mac_config_rx_queues_routing(struct stmmac_priv *priv)
+{
+	priv->hw->mac->rx_queue_routing(priv->hw, priv->plat->route_avcp_enable,
+					priv->plat->route_avcp_queue,
+					priv->plat->route_ptp_enable,
+					priv->plat->route_ptp_queue,
+					priv->plat->route_dcbcp_enable,
+					priv->plat->route_dcbcp_queue,
+					priv->plat->route_up_enable,
+					priv->plat->route_up_queue,
+					priv->plat->route_multi_broad_enable,
+					priv->plat->route_multi_broad_queue,
+					priv->plat->route_tpqc_enable,
+					priv->plat->route_tpqc_queue);
+}
+
+/**
  *  stmmac_mtl_configuration - Configure MTL
  *  @priv: driver private structure
  *  Description: It is used for configurring MTL

@@ -2377,6 +2398,10 @@ static void stmmac_mtl_configuration(struct stmmac_priv *priv)
 	/* Set TX priorities */
 	if (tx_queues_count > 1 && priv->hw->mac->tx_queue_prio)
 		stmmac_mac_config_tx_queues_prio(priv);
+
+	/* Set RX routing */
+	if (rx_queues_count > 1 && priv->hw->mac->rx_queue_routing)
+		stmmac_mac_config_rx_queues_routing(priv);
 }
 
 /**

@@ -96,6 +96,14 @@ static void stmmac_default_data(struct plat_stmmacenet_data *plat)
 	/* Disable Priority config by default */
 	plat->tx_queues_cfg[0].use_prio = false;
 	plat->rx_queues_cfg[0].use_prio = false;
+
+	/* Disable RX queues routing by default */
+	plat->route_avcp_enable = false;
+	plat->route_ptp_enable = false;
+	plat->route_dcbcp_enable = false;
+	plat->route_up_enable = false;
+	plat->route_multi_broad_enable = false;
+	plat->route_tpqc_enable = false;
 }
 
 static int quark_default_data(struct plat_stmmacenet_data *plat,

@@ -165,6 +165,55 @@ static void stmmac_mtl_setup(struct platform_device *pdev,
 	else
 		plat->rx_sched_algorithm = MTL_RX_ALGORITHM_SP;
 
+	/* RX routing configuration */
+	if (of_property_read_u8(rx_node, "snps,avcp-to-queue",
+				&plat->route_avcp_queue)) {
+		plat->route_avcp_queue = 0;
+		plat->route_avcp_enable = false;
+	} else {
+		plat->route_avcp_enable = true;
+	}
+
+	if (of_property_read_u8(rx_node, "snps,ptp-to-queue",
+				&plat->route_ptp_queue)) {
+		plat->route_ptp_queue = 0;
+		plat->route_ptp_enable = false;
+	} else {
+		plat->route_ptp_enable = true;
+	}
+
+	if (of_property_read_u8(rx_node, "snps,dcbcp-to-queue",
+				&plat->route_dcbcp_queue)) {
+		plat->route_dcbcp_queue = 0;
+		plat->route_dcbcp_enable = false;
+	} else {
+		plat->route_dcbcp_enable = true;
+	}
+
+	if (of_property_read_u8(rx_node, "snps,up-to-queue",
+				&plat->route_up_queue)) {
+		plat->route_up_queue = 0;
+		plat->route_up_enable = false;
+	} else {
+		plat->route_up_enable = true;
+	}
+
+	if (of_property_read_u8(rx_node, "snps,multi-broad-cast-queue",
+				&plat->route_multi_broad_queue)) {
+		plat->route_multi_broad_queue = 0;
+		plat->route_multi_broad_enable = false;
+	} else {
+		plat->route_multi_broad_enable = true;
+	}
+
+	if (of_property_read_u8(rx_node, "snps,tpqc-queue",
+				&plat->route_tpqc_queue)) {
+		plat->route_tpqc_queue = 0;
+		plat->route_tpqc_enable = false;
+	} else {
+		plat->route_tpqc_enable = true;
+	}
+
 	/* Processing individual RX queue config */
 	for_each_child_of_node(rx_node, q_node) {
 		if (queue >= plat->rx_queues_to_use)

@@ -171,6 +171,18 @@ struct plat_stmmacenet_data {
 	u8 tx_queues_to_use;
 	u8 rx_sched_algorithm;
 	u8 tx_sched_algorithm;
+	bool route_avcp_enable;
+	u8 route_avcp_queue;
+	bool route_ptp_enable;
+	u8 route_ptp_queue;
+	bool route_dcbcp_enable;
+	u8 route_dcbcp_queue;
+	bool route_up_enable;
+	u8 route_up_queue;
+	bool route_multi_broad_enable;
+	u8 route_multi_broad_queue;
+	bool route_tpqc_enable;
+	u8 route_tpqc_queue;
 	struct stmmac_rxq_cfg rx_queues_cfg[MTL_MAX_RX_QUEUES];
 	struct stmmac_txq_cfg tx_queues_cfg[MTL_MAX_TX_QUEUES];
 	void (*fix_mac_speed)(void *priv, unsigned int speed);