From 33fdbe772f41b988bf4f0c83f9fe8bf81ada349a Mon Sep 17 00:00:00 2001 From: Suraj Jaiswal Date: Wed, 17 Mar 2021 15:56:07 +0530 Subject: [PATCH] net: stmmac: Add multi queue support Add multi queue support for PTP, AVB traffic. Change-Id: I083fe8fa306b01da7d1baf2ef3b13c914329dd17 Acked-by: Nagarjuna Chaganti Signed-off-by: Suraj Jaiswal --- .../stmicro/stmmac/dwmac-qcom-ethqos.c | 158 +++++++++++++++++- .../stmicro/stmmac/dwmac-qcom-ethqos.h | 59 +++++++ .../ethernet/stmicro/stmmac/dwmac-qcom-pps.c | 72 ++++---- .../ethernet/stmicro/stmmac/stmmac_hwtstamp.c | 24 ++- .../net/ethernet/stmicro/stmmac/stmmac_main.c | 32 +++- .../ethernet/stmicro/stmmac/stmmac_platform.c | 6 + .../net/ethernet/stmicro/stmmac/stmmac_ptp.h | 1 + include/linux/stmmac.h | 7 + 8 files changed, 300 insertions(+), 59 deletions(-) diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c index a88b869c828f..d8ba39eb6755 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c @@ -19,7 +19,9 @@ #include #include #include - +#include +#include +#include #include "stmmac.h" #include "stmmac_platform.h" #include "dwmac-qcom-ethqos.h" @@ -115,6 +117,159 @@ bool phy_intr_en; struct emac_emb_smmu_cb_ctx emac_emb_smmu_ctx = {0}; struct qcom_ethqos *pethqos; +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]); +} + +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; + } + + ETHQOSDBG("tx_queue %d\n", txqueue_select); + return txqueue_select; +} + +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; + + ETHQOSDBG("\n"); + + 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; + + priv->plat->tx_queues_cfg[l_avb_struct.qinx].mode_to_use = + MTL_QUEUE_AVB; + priv->plat->tx_queues_cfg[l_avb_struct.qinx].send_slope = + avb_params->send_slope, + priv->plat->tx_queues_cfg[l_avb_struct.qinx].idle_slope = + avb_params->idle_slope, + priv->plat->tx_queues_cfg[l_avb_struct.qinx].high_credit = + avb_params->hi_credit, + priv->plat->tx_queues_cfg[l_avb_struct.qinx].low_credit = + avb_params->low_credit, + + priv->hw->mac->config_cbs(priv->hw, + priv->plat->tx_queues_cfg[l_avb_struct.qinx].send_slope, + priv->plat->tx_queues_cfg[l_avb_struct.qinx].idle_slope, + priv->plat->tx_queues_cfg[l_avb_struct.qinx].high_credit, + priv->plat->tx_queues_cfg[l_avb_struct.qinx].low_credit, + l_avb_struct.qinx); + + ETHQOSDBG("\n"); +} + +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; +} + +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; + int ret = 0; + + if (copy_from_user(&req, ifr->ifr_ifru.ifru_data, + sizeof(struct ifr_data_struct))) + return -EFAULT; + + switch (req.cmd) { + case ETHQOS_CONFIG_PPSOUT_CMD: + ret = ppsout_config(pdata, &req); + break; + case ETHQOS_AVB_ALGORITHM: + dwmac_qcom_program_avb_algorithm(pdata, &req); + break; + default: + break; + } + return ret; +} + static int rgmii_readl(struct qcom_ethqos *ethqos, unsigned int offset) { return readl(ethqos->rgmii_base + offset); @@ -771,6 +926,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 eb8e8da3a294..4caf1f507b86 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h @@ -26,6 +26,8 @@ #define EMAC_HW_vMAX 9 #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) @@ -162,4 +164,61 @@ int create_pps_interrupt_device_node(dev_t *pps_dev_t, struct cdev **pps_cdev, struct class **pps_class, char *pps_dev_node_name); +int ppsout_config(struct stmmac_priv *priv, struct ifr_data_struct *req); + +u16 dwmac_qcom_select_queue(struct net_device *dev, + struct sk_buff *skb, + struct net_device *sb_dev); + +#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; +}; + +void dwmac_qcom_program_avb_algorithm(struct stmmac_priv *priv, + struct ifr_data_struct *req); +unsigned int dwmac_qcom_get_plat_tx_coal_frames(struct sk_buff *skb); #endif diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c index 0c408ebac81d..086628b34164 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c @@ -107,7 +107,7 @@ static u32 pps_config_default_addend(void __iomem *ioaddr, priv->default_addend = 0xFFFFFFFF; } else { temp = (u64)((u64)ptp_clock << 32); - priv->default_addend = div_u64(temp, 250000000); + priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate); } priv->hw->ptp->config_addend(ioaddr, priv->default_addend); @@ -192,16 +192,21 @@ static void ethqos_unregister_pps_isr(struct stmmac_priv *priv, int ch) int ppsout_config(struct stmmac_priv *priv, struct ifr_data_struct *req) { - struct pps_cfg *eth_pps_cfg = (struct pps_cfg *)req->ptr; int interval, width; u32 sub_second_inc, value; void __iomem *ioaddr = priv->ioaddr; + u32 val; + struct pps_cfg eth_pps_cfg; - if (!eth_pps_cfg->ppsout_start) { - ppsout_stop(priv, eth_pps_cfg); - if (eth_pps_cfg->ppsout_ch == DWC_ETH_QOS_PPS_CH_2 || - eth_pps_cfg->ppsout_ch == DWC_ETH_QOS_PPS_CH_3) - ethqos_unregister_pps_isr(priv, eth_pps_cfg->ppsout_ch); + if (copy_from_user(ð_pps_cfg, (void __user *)req->ptr, + sizeof(struct pps_cfg))) + return -EFAULT; + + if (!eth_pps_cfg.ppsout_start) { + ppsout_stop(priv, ð_pps_cfg); + if (eth_pps_cfg.ppsout_ch == DWC_ETH_QOS_PPS_CH_2 || + eth_pps_cfg.ppsout_ch == DWC_ETH_QOS_PPS_CH_3) + ethqos_unregister_pps_isr(priv, eth_pps_cfg.ppsout_ch); return 0; } @@ -210,43 +215,43 @@ int ppsout_config(struct stmmac_priv *priv, struct ifr_data_struct *req) priv->hw->ptp->init_systime(priv->ptpaddr, 0, 0); priv->hw->ptp->adjust_systime(priv->ptpaddr, 0, 0, 0, 1); - u32 val = readl_relaxed(ioaddr + MAC_PPS_CONTROL); + val = readl_relaxed(ioaddr + MAC_PPS_CONTROL); sub_second_inc = pps_config_sub_second_increment - (priv->ptpaddr, eth_pps_cfg->ptpclk_freq, + (priv->ptpaddr, eth_pps_cfg.ptpclk_freq, priv->plat->has_gmac4); pps_config_default_addend(priv->ptpaddr, priv, - eth_pps_cfg->ptpclk_freq); + eth_pps_cfg.ptpclk_freq); - val &= ~PPSX_MASK(eth_pps_cfg->ppsout_ch); + val &= ~PPSX_MASK(eth_pps_cfg.ppsout_ch); - val |= PPSCMDX(eth_pps_cfg->ppsout_ch, 0x2); - val |= TRGTMODSELX(eth_pps_cfg->ppsout_ch, 0x2); + val |= PPSCMDX(eth_pps_cfg.ppsout_ch, 0x2); + val |= TRGTMODSELX(eth_pps_cfg.ppsout_ch, 0x2); val |= PPSEN0; - if (eth_pps_cfg->ppsout_ch == DWC_ETH_QOS_PPS_CH_2 || - eth_pps_cfg->ppsout_ch == DWC_ETH_QOS_PPS_CH_3) - ethqos_register_pps_isr(priv, eth_pps_cfg->ppsout_ch); + if (eth_pps_cfg.ppsout_ch == DWC_ETH_QOS_PPS_CH_2 || + eth_pps_cfg.ppsout_ch == DWC_ETH_QOS_PPS_CH_3) + ethqos_register_pps_isr(priv, eth_pps_cfg.ppsout_ch); writel_relaxed(0, ioaddr + - MAC_PPSX_TARGET_TIME_SEC(eth_pps_cfg->ppsout_ch)); + MAC_PPSX_TARGET_TIME_SEC(eth_pps_cfg.ppsout_ch)); writel_relaxed(0, ioaddr + - MAC_PPSX_TARGET_TIME_NSEC(eth_pps_cfg->ppsout_ch)); + MAC_PPSX_TARGET_TIME_NSEC(eth_pps_cfg.ppsout_ch)); - interval = ((eth_pps_cfg->ptpclk_freq + eth_pps_cfg->ppsout_freq / 2) - / eth_pps_cfg->ppsout_freq); + interval = ((eth_pps_cfg.ptpclk_freq + eth_pps_cfg.ppsout_freq / 2) + / eth_pps_cfg.ppsout_freq); - width = ((interval * eth_pps_cfg->ppsout_duty) + 50) / 100 - 1; + width = ((interval * eth_pps_cfg.ppsout_duty) + 50) / 100 - 1; if (width >= interval) width = interval - 1; if (width < 0) width = 0; writel_relaxed(interval, ioaddr + - MAC_PPSX_INTERVAL(eth_pps_cfg->ppsout_ch)); + MAC_PPSX_INTERVAL(eth_pps_cfg.ppsout_ch)); - writel_relaxed(width, ioaddr + MAC_PPSX_WIDTH(eth_pps_cfg->ppsout_ch)); + writel_relaxed(width, ioaddr + MAC_PPSX_WIDTH(eth_pps_cfg.ppsout_ch)); writel_relaxed(val, ioaddr + MAC_PPS_CONTROL); @@ -277,27 +282,6 @@ int ethqos_init_pps(struct stmmac_priv *priv) return 0; } -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; - struct pps_cfg eth_pps_cfg; - int ret = 0; - - if (copy_from_user(&req, ifr->ifr_ifru.ifru_data, - sizeof(struct ifr_data_struct))) - return -EFAULT; - if (copy_from_user(ð_pps_cfg, req.ptr, - sizeof(struct pps_cfg))) - return -EFAULT; - req.ptr = ð_pps_cfg; - switch (req.cmd) { - case ETHQOS_CONFIG_PPSOUT_CMD: - ret = ppsout_config(pdata, &req); - } - return ret; -} - static ssize_t pps_fops_read(struct file *filp, char __user *buf, size_t count, loff_t *f_pos) { diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c index e5d9007c8090..a456ef7699a6 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c @@ -23,7 +23,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; + unsigned long ss_inc = 0, sns_inc = 0, ptpclock = 0; u32 reg_value; /* For GMAC3.x, 4.x versions, in "fine adjustement mode" set sub-second @@ -35,24 +35,34 @@ static void config_sub_second_increment(void __iomem *ioaddr, * 2000000000ULL / ptp_clock. */ if (value & PTP_TCR_TSCFUPDT) - data = (2000000000ULL / ptp_clock); + ptpclock = (unsigned long)ptp_clock; else - data = (1000000000ULL / ptp_clock); + ptpclock = (unsigned long)ptp_clock; + + ss_inc = ((1 * 1000000000ULL) / ptpclock); + sns_inc = 1000000000ULL - (ss_inc * ptpclock); //take remainder + + //sns_inc needs to be multiplied by 2^8, per spec. + sns_inc = (sns_inc * 256) / ptpclock; /* 0.465ns accuracy */ if (!(value & PTP_TCR_TSCTRLSSR)) - data = (data * 1000) / 465; + ss_inc = (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 f31e691fe5c6..12ed4ebcdf93 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -526,6 +526,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; @@ -640,6 +641,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: @@ -652,6 +654,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: @@ -665,6 +668,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: @@ -697,14 +701,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, + priv->ptpaddr, priv->plat->clk_ptp_req_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; @@ -715,7 +718,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); @@ -2615,7 +2618,9 @@ 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 = ethqos_init_pps(priv); } @@ -3318,7 +3323,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 @@ -4066,6 +4072,18 @@ static int stmmac_set_mac_address(struct net_device *ndev, void *addr) 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; @@ -4348,13 +4366,13 @@ static const struct net_device_ops stmmac_netdev_ops = { .ndo_tx_timeout = stmmac_tx_timeout, .ndo_do_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 .ndo_set_mac_address = stmmac_set_mac_address, .ndo_vlan_rx_add_vid = stmmac_vlan_rx_add_vid, .ndo_vlan_rx_kill_vid = stmmac_vlan_rx_kill_vid, + .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_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c index df6dd8b476a2..993ca2006f94 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c @@ -597,6 +597,12 @@ stmmac_probe_config_dt(struct platform_device *pdev, const char **mac) dev_dbg(&pdev->dev, "PTP rate %d\n", plat->clk_ptp_rate); } + of_property_read_u32(np, + "snps,ptp-ref-clk-rate", &plat->clk_ptp_rate); + + of_property_read_u32(np, + "snps,ptp-req-clk-rate", &plat->clk_ptp_req_rate); + plat->stmmac_rst = devm_reset_control_get(&pdev->dev, STMMAC_RESOURCE_NAME); if (IS_ERR(plat->stmmac_rst)) { diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h index cf99a334ce7a..c020f7dba7fd 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h @@ -59,6 +59,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 dc60d03c4b60..d6489ff82b8a 100644 --- a/include/linux/stmmac.h +++ b/include/linux/stmmac.h @@ -13,6 +13,7 @@ #define __STMMAC_PLATFORM_DATA #include +#include #define MTL_MAX_RX_QUEUES 8 #define MTL_MAX_TX_QUEUES 8 @@ -168,6 +169,7 @@ struct plat_stmmacenet_data { struct clk *pclk; struct clk *clk_ptp_ref; unsigned int clk_ptp_rate; + unsigned int clk_ptp_req_rate; unsigned int clk_ref_rate; s32 ptp_max_adj; struct reset_control *stmmac_rst; @@ -179,5 +181,10 @@ struct plat_stmmacenet_data { int mac_port_sel_speed; bool en_tx_lpi_clockgating; int has_xgmac; + 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