net: stmmac: Add multi queue support

Add multi queue support for PTP, AVB traffic.

Change-Id: I083fe8fa306b01da7d1baf2ef3b13c914329dd17
Acked-by: Nagarjuna Chaganti <nchagant@qti.qualcomm.com>
Signed-off-by: Suraj Jaiswal <jsuraj@codeaurora.org>

6 files changed, 239 insertions, 33 deletions

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c
index 830826b1d5374..37ad3cde8cb8f 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c
@@ -16,6 +16,9 @@
 #include <linux/debugfs.h>
 #include <linux/dma-iommu.h>
 #include <linux/iommu.h>
+#include <linux/tcp.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
 
 #include "stmmac.h"
 #include "stmmac_platform.h"
@@ -143,7 +146,135 @@ static void qcom_ethqos_read_iomacro_por_values(struct qcom_ethqos *ethqos)
 			(ethqos->rgmii_base + ethqos->por[i].offset);
 }
 
-static int ethqos_handle_prv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+static inline unsigned int dwmac_qcom_get_eth_type(unsigned char *buf)
+{
+	return
+		((((u16)buf[QTAG_ETH_TYPE_OFFSET] << 8) |
+		  buf[QTAG_ETH_TYPE_OFFSET + 1]) == ETH_P_8021Q) ?
+		(((u16)buf[QTAG_VLAN_ETH_TYPE_OFFSET] << 8) |
+		 buf[QTAG_VLAN_ETH_TYPE_OFFSET + 1]) :
+		 (((u16)buf[QTAG_ETH_TYPE_OFFSET] << 8) |
+		  buf[QTAG_ETH_TYPE_OFFSET + 1]);
+}
+
+static inline unsigned int dwmac_qcom_get_vlan_ucp(unsigned char  *buf)
+{
+	return
+		(((u16)buf[QTAG_UCP_FIELD_OFFSET] << 8)
+		 | buf[QTAG_UCP_FIELD_OFFSET + 1]);
+}
+
+static u16 dwmac_qcom_select_queue(struct net_device *dev,
+				   struct sk_buff *skb,
+				   struct net_device *sb_dev)
+{
+	u16 txqueue_select = ALL_OTHER_TRAFFIC_TX_CHANNEL;
+	unsigned int eth_type, priority;
+
+	/* Retrieve ETH type */
+	eth_type = dwmac_qcom_get_eth_type(skb->data);
+
+	if (eth_type == ETH_P_TSN) {
+		/* Read VLAN priority field from skb->data */
+		priority = dwmac_qcom_get_vlan_ucp(skb->data);
+
+		priority >>= VLAN_TAG_UCP_SHIFT;
+		if (priority == CLASS_A_TRAFFIC_UCP)
+			txqueue_select = CLASS_A_TRAFFIC_TX_CHANNEL;
+		else if (priority == CLASS_B_TRAFFIC_UCP)
+			txqueue_select = CLASS_B_TRAFFIC_TX_CHANNEL;
+		else
+			txqueue_select = ALL_OTHER_TX_TRAFFIC_IPA_DISABLED;
+	} else {
+		/* VLAN tagged IP packet or any other non vlan packets (PTP)*/
+		txqueue_select = ALL_OTHER_TX_TRAFFIC_IPA_DISABLED;
+	}
+
+	return txqueue_select;
+}
+
+static void dwmac_qcom_program_avb_algorithm(struct stmmac_priv *priv,
+					     struct ifr_data_struct *req)
+{
+	struct dwmac_qcom_avb_algorithm l_avb_struct, *u_avb_struct =
+		(struct dwmac_qcom_avb_algorithm *)req->ptr;
+	struct dwmac_qcom_avb_algorithm_params *avb_params;
+	struct stmmac_txq_cfg *txq_cfg;
+
+	if (copy_from_user(&l_avb_struct, (void __user *)u_avb_struct,
+			   sizeof(struct dwmac_qcom_avb_algorithm)))
+		ETHQOSERR("Failed to fetch AVB Struct\n");
+
+	if (priv->speed == SPEED_1000)
+		avb_params = &l_avb_struct.speed1000params;
+	else
+		avb_params = &l_avb_struct.speed100params;
+
+	/* Application uses 1 for CLASS A traffic and
+	 * 2 for CLASS B traffic
+	 * Configure right channel accordingly
+	 */
+	if (l_avb_struct.qinx == 1) {
+		l_avb_struct.qinx = CLASS_A_TRAFFIC_TX_CHANNEL;
+	} else if (l_avb_struct.qinx == 2) {
+		l_avb_struct.qinx = CLASS_B_TRAFFIC_TX_CHANNEL;
+	} else {
+		ETHQOSERR("Invalid index [%u] in AVB struct from user\n",
+			  l_avb_struct.qinx);
+		return;
+	}
+
+	txq_cfg = &priv->plat->tx_queues_cfg[l_avb_struct.qinx];
+	txq_cfg->mode_to_use = MTL_QUEUE_AVB;
+	txq_cfg->send_slope = avb_params->send_slope;
+	txq_cfg->idle_slope = avb_params->idle_slope;
+	txq_cfg->high_credit = avb_params->hi_credit;
+	txq_cfg->low_credit = avb_params->low_credit;
+
+	priv->hw->mac->config_cbs(priv->hw,
+				  txq_cfg->send_slope,
+				  txq_cfg->idle_slope,
+				  txq_cfg->high_credit,
+				  txq_cfg->low_credit,
+				  l_avb_struct.qinx);
+}
+
+unsigned int dwmac_qcom_get_plat_tx_coal_frames(struct sk_buff *skb)
+{
+	bool is_udp;
+	unsigned int eth_type;
+
+	eth_type = dwmac_qcom_get_eth_type(skb->data);
+
+#ifdef CONFIG_PTPSUPPORT_OBJ
+	if (eth_type == ETH_P_1588)
+		return PTP_INT_MOD;
+#endif
+
+	if (eth_type == ETH_P_TSN)
+		return AVB_INT_MOD;
+	if (eth_type == ETH_P_IP || eth_type == ETH_P_IPV6) {
+#ifdef CONFIG_PTPSUPPORT_OBJ
+		is_udp = (((eth_type == ETH_P_IP) &&
+			   (ip_hdr(skb)->protocol ==
+				IPPROTO_UDP)) ||
+			  ((eth_type == ETH_P_IPV6) &&
+			   (ipv6_hdr(skb)->nexthdr ==
+				IPPROTO_UDP)));
+
+		if (is_udp && ((udp_hdr(skb)->dest ==
+			htons(PTP_UDP_EV_PORT)) ||
+			(udp_hdr(skb)->dest ==
+			  htons(PTP_UDP_GEN_PORT))))
+			return PTP_INT_MOD;
+#endif
+		return IP_PKT_INT_MOD;
+	}
+	return DEFAULT_INT_MOD;
+}
+
+static int ethqos_handle_prv_ioctl(struct net_device *dev,
+				   struct ifreq *ifr, int cmd)
 {
 	struct stmmac_priv *pdata = netdev_priv(dev);
 	struct ifr_data_struct req;
@@ -162,6 +293,11 @@ static int ethqos_handle_prv_ioctl(struct net_device *dev, struct ifreq *ifr, in
 
 		ret = ppsout_config(pdata, &eth_pps_cfg);
 		break;
+	case ETHQOS_AVB_ALGORITHM:
+		dwmac_qcom_program_avb_algorithm(pdata, &req);
+		break;
+	default:
+		break;
 	}
 	return ret;
 }
@@ -1024,6 +1160,7 @@ static int qcom_ethqos_probe(struct platform_device *pdev)
 
 	plat_dat->bsp_priv = ethqos;
 	plat_dat->fix_mac_speed = ethqos_fix_mac_speed;
+	plat_dat->tx_select_queue = dwmac_qcom_select_queue;
 	plat_dat->has_gmac4 = 1;
 	plat_dat->pmt = 1;
 	plat_dat->tso_en = of_property_read_bool(np, "snps,tso");
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h
index cde51869f1010..0fd57b8c83e23 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h
@@ -23,6 +23,8 @@
 #define EMAC_HW_NONE 0
 
 #define ETHQOS_CONFIG_PPSOUT_CMD 44
+#define ETHQOS_AVB_ALGORITHM 27
+
 #define MAC_PPS_CONTROL			0x00000b70
 #define PPS_MAXIDX(x)			((((x) + 1) * 8) - 1)
 #define PPS_MINIDX(x)			((x) * 8)
@@ -154,4 +156,54 @@ void ethqos_remove_pps_dev(struct qcom_ethqos *ethqos);
 int ppsout_config(struct stmmac_priv *priv, struct pps_cfg *eth_pps_cfg);
 int ethqos_init_pps(void *priv_n);
 struct qcom_ethqos *get_pethqos(void);
+
+#define QTAG_VLAN_ETH_TYPE_OFFSET 16
+#define QTAG_UCP_FIELD_OFFSET 14
+#define QTAG_ETH_TYPE_OFFSET 12
+#define PTP_UDP_EV_PORT 0x013F
+#define PTP_UDP_GEN_PORT 0x0140
+
+#define IPA_DMA_TX_CH 0
+#define IPA_DMA_RX_CH 0
+
+#define VLAN_TAG_UCP_SHIFT 13
+#define CLASS_A_TRAFFIC_UCP 3
+#define CLASS_A_TRAFFIC_TX_CHANNEL 3
+
+#define CLASS_B_TRAFFIC_UCP 2
+#define CLASS_B_TRAFFIC_TX_CHANNEL 2
+
+#define NON_TAGGED_IP_TRAFFIC_TX_CHANNEL 1
+#define ALL_OTHER_TRAFFIC_TX_CHANNEL 1
+#define ALL_OTHER_TX_TRAFFIC_IPA_DISABLED 0
+
+#define DEFAULT_INT_MOD 1
+#define AVB_INT_MOD 8
+#define IP_PKT_INT_MOD 32
+#define PTP_INT_MOD 1
+
+enum dwmac_qcom_queue_operating_mode {
+	DWMAC_QCOM_QDISABLED = 0X0,
+	DWMAC_QCOM_QAVB,
+	DWMAC_QCOM_QDCB,
+	DWMAC_QCOM_QGENERIC
+};
+
+struct dwmac_qcom_avb_algorithm_params {
+	unsigned int idle_slope;
+	unsigned int send_slope;
+	unsigned int hi_credit;
+	unsigned int low_credit;
+};
+
+struct dwmac_qcom_avb_algorithm {
+	unsigned int qinx;
+	unsigned int algorithm;
+	unsigned int cc;
+	struct dwmac_qcom_avb_algorithm_params speed100params;
+	struct dwmac_qcom_avb_algorithm_params speed1000params;
+	enum dwmac_qcom_queue_operating_mode op_mode;
+};
+
+unsigned int dwmac_qcom_get_plat_tx_coal_frames(struct sk_buff *skb);
 #endif
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
index 074e2cdfb0fa6..feae11be18682 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
@@ -27,7 +27,7 @@ static void config_sub_second_increment(void __iomem *ioaddr,
 		u32 ptp_clock, int gmac4, u32 *ssinc)
 {
 	u32 value = readl(ioaddr + PTP_TCR);
-	unsigned long data;
+	u64 ss_inc = 0, sns_inc = 0, ptpclock = 0;
 	u32 reg_value;
 
 	/* For GMAC3.x, 4.x versions, in "fine adjustement mode" set sub-second
@@ -39,24 +39,34 @@ static void config_sub_second_increment(void __iomem *ioaddr,
 	 * 2000000000ULL / ptp_clock.
 	 */
 	if (value & PTP_TCR_TSCFUPDT)
-		data = (2000000000ULL / ptp_clock);
+		ptpclock = (u64)ptp_clock;
 	else
-		data = (1000000000ULL / ptp_clock);
+		ptpclock = (u64)ptp_clock;
+
+	ss_inc = div_u64((1 * 1000000000ULL), ptpclock);
+	sns_inc = 1000000000ULL - (ss_inc * ptpclock); //take remainder
+
+	//sns_inc needs to be multiplied by 2^8, per spec.
+	sns_inc = div_u64((sns_inc * 256), ptpclock);
 
 	/* 0.465ns accuracy */
 	if (!(value & PTP_TCR_TSCTRLSSR))
-		data = (data * 1000) / 465;
+		ss_inc = div_u64((ss_inc * 1000), 465);
 
-	data &= PTP_SSIR_SSINC_MASK;
+	ss_inc &= PTP_SSIR_SSINC_MASK;
+	sns_inc &= PTP_SSIR_SNSINC_MASK;
+
+	reg_value = ss_inc;
 
-	reg_value = data;
 	if (gmac4)
 		reg_value <<= GMAC4_PTP_SSIR_SSINC_SHIFT;
 
+	reg_value |= (sns_inc << GMAC4_PTP_SSIR_SNSINC_SHIFT);
+
 	writel(reg_value, ioaddr + PTP_SSIR);
 
 	if (ssinc)
-		*ssinc = data;
+		*ssinc = reg_value;
 }
 
 static int init_systime(void __iomem *ioaddr, u32 sec, u32 nsec)
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index da6102e98fee3..e5ba784ff11cc 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -652,6 +652,7 @@ static int stmmac_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 	u32 ts_master_en = 0;
 	u32 ts_event_en = 0;
 	u32 sec_inc = 0;
+	u32 av_8021asm_en = 0;
 	u32 value = 0;
 	bool xmac;
 
@@ -762,6 +763,7 @@ static int stmmac_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 			ptp_over_ethernet = PTP_TCR_TSIPENA;
+			av_8021asm_en = PTP_TCR_AV8021ASMEN;
 			break;
 
 		case HWTSTAMP_FILTER_PTP_V2_SYNC:
@@ -774,6 +776,7 @@ static int stmmac_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 			ptp_over_ethernet = PTP_TCR_TSIPENA;
+			av_8021asm_en = PTP_TCR_AV8021ASMEN;
 			break;
 
 		case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
@@ -787,6 +790,7 @@ static int stmmac_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 			ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 			ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 			ptp_over_ethernet = PTP_TCR_TSIPENA;
+			av_8021asm_en = PTP_TCR_AV8021ASMEN;
 			break;
 
 		case HWTSTAMP_FILTER_NTP_ALL:
@@ -819,14 +823,13 @@ static int stmmac_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 		value = (PTP_TCR_TSENA | PTP_TCR_TSCFUPDT | PTP_TCR_TSCTRLSSR |
 			 tstamp_all | ptp_v2 | ptp_over_ethernet |
 			 ptp_over_ipv6_udp | ptp_over_ipv4_udp | ts_event_en |
-			 ts_master_en | snap_type_sel);
+			 ts_master_en | snap_type_sel | av_8021asm_en);
 		stmmac_config_hw_tstamping(priv, priv->ptpaddr, value);
 
 		/* program Sub Second Increment reg */
 		stmmac_config_sub_second_increment(priv,
 				priv->ptpaddr, priv->plat->clk_ptp_rate,
 				xmac, &sec_inc);
-		temp = div_u64(1000000000ULL, sec_inc);
 
 		/* Store sub second increment and flags for later use */
 		priv->sub_second_inc = sec_inc;
@@ -837,7 +840,7 @@ static int stmmac_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 		 * addend = (2^32)/freq_div_ratio;
 		 * where, freq_div_ratio = 1e9ns/sec_inc
 		 */
-		temp = (u64)(temp << 32);
+		temp = (u64)((u64)priv->plat->clk_ptp_req_rate << 32);
 		priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate);
 		stmmac_config_addend(priv, priv->ptpaddr, priv->default_addend);
 
@@ -3344,7 +3347,8 @@ static int stmmac_hw_setup(struct net_device *dev, bool init_ptp)
 		else if (ret)
 			netdev_warn(priv->dev, "PTP init failed\n");
 		else
-			ret = clk_set_rate(priv->plat->clk_ptp_ref, 96000000);
+			clk_set_rate(priv->plat->clk_ptp_ref,
+				     priv->plat->clk_ptp_rate);
 
 		ret = priv->plat->init_pps(priv);
 	}
@@ -4223,7 +4227,8 @@ static netdev_tx_t stmmac_tso_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (priv->sarc_type)
 		stmmac_set_desc_sarc(priv, first, priv->sarc_type);
 
-	skb_tx_timestamp(skb);
+	if (!priv->hwts_tx_en)
+		skb_tx_timestamp(skb);
 
 	if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
 		     priv->hwts_tx_en)) {
@@ -4454,7 +4459,8 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (priv->sarc_type)
 		stmmac_set_desc_sarc(priv, first, priv->sarc_type);
 
-	skb_tx_timestamp(skb);
+	if (!priv->hwts_tx_en)
+		skb_tx_timestamp(skb);
 
 	/* Ready to fill the first descriptor and set the OWN bit w/o any
 	 * problems because all the descriptors are actually ready to be
@@ -5976,24 +5982,6 @@ static int stmmac_setup_tc(struct net_device *ndev, enum tc_setup_type type,
 	}
 }
 
-static u16 stmmac_select_queue(struct net_device *dev, struct sk_buff *skb,
-			       struct net_device *sb_dev)
-{
-	int gso = skb_shinfo(skb)->gso_type;
-
-	if (gso & (SKB_GSO_TCPV4 | SKB_GSO_TCPV6 | SKB_GSO_UDP_L4)) {
-		/*
-		 * There is no way to determine the number of TSO/USO
-		 * capable Queues. Let's use always the Queue 0
-		 * because if TSO/USO is supported then at least this
-		 * one will be capable.
-		 */
-		return 0;
-	}
-
-	return netdev_pick_tx(dev, skb, NULL) % dev->real_num_tx_queues;
-}
-
 static int stmmac_set_mac_address(struct net_device *ndev, void *addr)
 {
 	struct stmmac_priv *priv = netdev_priv(ndev);
@@ -6017,6 +6005,18 @@ set_mac_error:
 	return ret;
 }
 
+static u16 stmmac_tx_select_queue(struct net_device *dev,
+				  struct sk_buff *skb,
+				  struct net_device *sb_dev)
+{
+	struct stmmac_priv *priv = netdev_priv(dev);
+
+	if (likely(priv->plat->tx_select_queue))
+		return priv->plat->tx_select_queue(dev, skb, sb_dev);
+
+	return netdev_pick_tx(dev, skb, NULL) % dev->real_num_tx_queues;
+}
+
 #ifdef CONFIG_DEBUG_FS
 static struct dentry *stmmac_fs_dir;
 
@@ -6635,7 +6635,6 @@ static const struct net_device_ops stmmac_netdev_ops = {
 	.ndo_tx_timeout = stmmac_tx_timeout,
 	.ndo_eth_ioctl = stmmac_ioctl,
 	.ndo_setup_tc = stmmac_setup_tc,
-	.ndo_select_queue = stmmac_select_queue,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller = stmmac_poll_controller,
 #endif
@@ -6645,6 +6644,7 @@ static const struct net_device_ops stmmac_netdev_ops = {
 	.ndo_bpf = stmmac_bpf,
 	.ndo_xdp_xmit = stmmac_xdp_xmit,
 	.ndo_xsk_wakeup = stmmac_xsk_wakeup,
+	.ndo_select_queue = stmmac_tx_select_queue,
 };
 
 static void stmmac_reset_subtask(struct stmmac_priv *priv)
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h
index 64f2b2f8e3402..d37708b4fc99d 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h
@@ -62,6 +62,7 @@
 #define	PTP_TCR_SNAPTYPSEL_1	BIT(16)
 /* Enable MAC address for PTP Frame Filtering */
 #define	PTP_TCR_TSENMACADDR	BIT(18)
+#define	PTP_TCR_AV8021ASMEN	BIT(28)
 
 /* SSIR defines */
 #define	PTP_SSIR_SSINC_MASK		0xff
diff --git a/include/linux/stmmac.h b/include/linux/stmmac.h
index 968bfd7ae3de8..9f3cce640c820 100644
--- a/include/linux/stmmac.h
+++ b/include/linux/stmmac.h
@@ -14,6 +14,7 @@
 
 #include <linux/platform_device.h>
 #include <linux/phy.h>
+#include <linux/netdevice.h>
 
 #define MTL_MAX_RX_QUEUES	8
 #define MTL_MAX_TX_QUEUES	8
@@ -294,5 +295,10 @@ struct plat_stmmacenet_data {
 	int (*handle_prv_ioctl)(struct net_device *dev, struct ifreq *ifr,
 				int cmd);
 	int (*phy_intr_enable)(void *priv);
+	u16	(*tx_select_queue)
+		(struct net_device *dev, struct sk_buff *skb,
+		 struct net_device *sb_dev);
+	unsigned int (*get_plat_tx_coal_frames)
+		(struct sk_buff *skb);
 };
 #endif