From 66fb692b8948cb53bc7c64f067110dfdd90deaaf Mon Sep 17 00:00:00 2001 From: Suraj Jaiswal Date: Wed, 17 Mar 2021 15:30:09 +0530 Subject: [PATCH 1/3] net: stmmac: SDx55: add pps signal and interrupts support Add ioctl support to handle and generate pps siganal & interrupt.Add device nodes for pps interrupts to interact with user space. Change-Id: I401326443e69bfd0fb37986004bdc47f41dc57b7 Acked-by: Nagarjuna Chaganti Signed-off-by: Suraj Jaiswal --- drivers/net/ethernet/stmicro/stmmac/Makefile | 1 + .../stmicro/stmmac/dwmac-qcom-ethqos.c | 33 +- .../stmicro/stmmac/dwmac-qcom-ethqos.h | 87 ++++ .../ethernet/stmicro/stmmac/dwmac-qcom-pps.c | 436 ++++++++++++++++++ drivers/net/ethernet/stmicro/stmmac/stmmac.h | 1 + .../net/ethernet/stmicro/stmmac/stmmac_main.c | 4 + .../net/ethernet/stmicro/stmmac/stmmac_ptp.h | 2 + 7 files changed, 563 insertions(+), 1 deletion(-) create mode 100644 drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c diff --git a/drivers/net/ethernet/stmicro/stmmac/Makefile b/drivers/net/ethernet/stmicro/stmmac/Makefile index 005cc7baca52..524040278562 100644 --- a/drivers/net/ethernet/stmicro/stmmac/Makefile +++ b/drivers/net/ethernet/stmicro/stmmac/Makefile @@ -20,6 +20,7 @@ obj-$(CONFIG_DWMAC_MESON) += dwmac-meson.o dwmac-meson8b.o obj-$(CONFIG_DWMAC_OXNAS) += dwmac-oxnas.o obj-$(CONFIG_DWMAC_QCOM_ETHQOS) += dwmac-qcom-gpio.o obj-$(CONFIG_DWMAC_QCOM_ETHQOS) += dwmac-qcom-ethqos.o +obj-$(CONFIG_DWMAC_QCOM_ETHQOS) += dwmac-qcom-pps.o obj-$(CONFIG_DWMAC_ROCKCHIP) += dwmac-rk.o obj-$(CONFIG_DWMAC_SOCFPGA) += dwmac-altr-socfpga.o obj-$(CONFIG_DWMAC_STI) += dwmac-sti.o diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c index ebea98ef0c12..a88b869c828f 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c @@ -23,7 +23,6 @@ #include "stmmac.h" #include "stmmac_platform.h" #include "dwmac-qcom-ethqos.h" - #include "stmmac_ptp.h" #define RGMII_IO_MACRO_DEBUG1 0x20 @@ -114,6 +113,7 @@ bool phy_intr_en; struct emac_emb_smmu_cb_ctx emac_emb_smmu_ctx = {0}; +struct qcom_ethqos *pethqos; static int rgmii_readl(struct qcom_ethqos *ethqos, unsigned int offset) { @@ -696,6 +696,22 @@ smmu_probe_done: return result; } +static void ethqos_pps_irq_config(struct qcom_ethqos *ethqos) +{ + ethqos->pps_class_a_irq = + platform_get_irq_byname(ethqos->pdev, "ptp_pps_irq_0"); + if (ethqos->pps_class_a_irq < 0) { + if (ethqos->pps_class_a_irq != -EPROBE_DEFER) + ETHQOSERR("class_a_irq config info not found\n"); + } + ethqos->pps_class_b_irq = + platform_get_irq_byname(ethqos->pdev, "ptp_pps_irq_1"); + if (ethqos->pps_class_b_irq < 0) { + if (ethqos->pps_class_b_irq != -EPROBE_DEFER) + ETHQOSERR("class_b_irq config info not found\n"); + } +} + static int qcom_ethqos_probe(struct platform_device *pdev) { struct device_node *np = pdev->dev.of_node; @@ -798,6 +814,21 @@ static int qcom_ethqos_probe(struct platform_device *pdev) else ETHQOSERR("Phy interrupt configuration failed"); rgmii_dump(ethqos); + + if (ethqos->emac_ver == EMAC_HW_v2_3_2_RG) { + ethqos_pps_irq_config(ethqos); + create_pps_interrupt_device_node(ðqos->avb_class_a_dev_t, + ðqos->avb_class_a_cdev, + ðqos->avb_class_a_class, + AVB_CLASS_A_POLL_DEV_NODE); + + create_pps_interrupt_device_node(ðqos->avb_class_b_dev_t, + ðqos->avb_class_b_cdev, + ðqos->avb_class_b_class, + AVB_CLASS_B_POLL_DEV_NODE); + } + + pethqos = ethqos; return ret; err_clk: diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h index f204d1b01742..eb8e8da3a294 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h @@ -25,6 +25,46 @@ #define EMAC_HW_v2_3_1 0x20030001 #define EMAC_HW_vMAX 9 +#define ETHQOS_CONFIG_PPSOUT_CMD 44 +#define MAC_PPS_CONTROL 0x00000b70 +#define PPS_MAXIDX(x) ((((x) + 1) * 8) - 1) +#define PPS_MINIDX(x) ((x) * 8) +#define MCGRENX(x) BIT(PPS_MAXIDX(x)) +#define PPSEN0 BIT(4) +#define MAC_PPSX_TARGET_TIME_SEC(x) (0x00000b80 + ((x) * 0x10)) +#define MAC_PPSX_TARGET_TIME_NSEC(x) (0x00000b84 + ((x) * 0x10)) +#define TRGTBUSY0 BIT(31) +#define TTSL0 GENMASK(30, 0) +#define MAC_PPSX_INTERVAL(x) (0x00000b88 + ((x) * 0x10)) +#define MAC_PPSX_WIDTH(x) (0x00000b8c + ((x) * 0x10)) + +#define DWC_ETH_QOS_PPS_CH_2 2 +#define DWC_ETH_QOS_PPS_CH_3 3 + +#define AVB_CLASS_A_POLL_DEV_NODE "avb_class_a_intr" + +#define AVB_CLASS_B_POLL_DEV_NODE "avb_class_b_intr" + +#define AVB_CLASS_A_CHANNEL_NUM 2 +#define AVB_CLASS_B_CHANNEL_NUM 3 + +static inline u32 PPSCMDX(u32 x, u32 val) +{ + return (GENMASK(PPS_MINIDX(x) + 3, PPS_MINIDX(x)) & + ((val) << PPS_MINIDX(x))); +} + +static inline u32 TRGTMODSELX(u32 x, u32 val) +{ + return (GENMASK(PPS_MAXIDX(x) - 1, PPS_MAXIDX(x) - 2) & + ((val) << (PPS_MAXIDX(x) - 2))); +} + +static inline u32 PPSX_MASK(u32 x) +{ + return GENMASK(PPS_MAXIDX(x), PPS_MINIDX(x)); +} + struct ethqos_emac_por { unsigned int offset; unsigned int value; @@ -69,10 +109,57 @@ struct qcom_ethqos { struct regulator *reg_rgmii; struct regulator *reg_emac_phy; struct regulator *reg_rgmii_io_pads; + + int pps_class_a_irq; + int pps_class_b_irq; + + struct pinctrl_state *emac_pps_0; + + /* avb_class_a dev node variables*/ + dev_t avb_class_a_dev_t; + struct cdev *avb_class_a_cdev; + struct class *avb_class_a_class; + + /* avb_class_b dev node variables*/ + dev_t avb_class_b_dev_t; + struct cdev *avb_class_b_cdev; + struct class *avb_class_b_class; + + unsigned long avb_class_a_intr_cnt; + unsigned long avb_class_b_intr_cnt; +}; + +struct pps_cfg { + unsigned int ptpclk_freq; + unsigned int ppsout_freq; + unsigned int ppsout_ch; + unsigned int ppsout_duty; + unsigned int ppsout_start; +}; + +struct ifr_data_struct { + unsigned int flags; + unsigned int qinx; /* dma channel no to be configured */ + unsigned int cmd; + unsigned int context_setup; + unsigned int connected_speed; + unsigned int rwk_filter_values[8]; + unsigned int rwk_filter_length; + int command_error; + int test_done; + void *ptr; +}; + +struct pps_info { + int channel_no; }; int ethqos_init_reqgulators(struct qcom_ethqos *ethqos); void ethqos_disable_regulators(struct qcom_ethqos *ethqos); int ethqos_init_gpio(struct qcom_ethqos *ethqos); void ethqos_free_gpios(struct qcom_ethqos *ethqos); +int create_pps_interrupt_device_node(dev_t *pps_dev_t, + struct cdev **pps_cdev, + struct class **pps_class, + char *pps_dev_node_name); #endif diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c new file mode 100644 index 000000000000..0b8756b548b5 --- /dev/null +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c @@ -0,0 +1,436 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* Copyright (c) 2021, The Linux Foundation. All rights reserved. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "stmmac.h" +#include "stmmac_platform.h" +#include "stmmac_ptp.h" +#include "dwmac-qcom-ethqos.h" + +extern struct qcom_ethqos *pethqos; + +static bool avb_class_a_msg_wq_flag; +static bool avb_class_b_msg_wq_flag; + +static DECLARE_WAIT_QUEUE_HEAD(avb_class_a_msg_wq); +static DECLARE_WAIT_QUEUE_HEAD(avb_class_b_msg_wq); + +static int strlcmp(const char *s, const char *t, size_t n) +{ + int ret; + + while (n-- && *t != '\0') { + if (*s != *t) { + ret = ((unsigned char)*s - (unsigned char)*t); + n = 0; + } else { + ++s, ++t; + ret = (unsigned char)*s; + } + } + return ret; +} + +static u32 pps_config_sub_second_increment(void __iomem *ioaddr, + u32 ptp_clock, int gmac4) +{ + u32 value = readl_relaxed(ioaddr + PTP_TCR); + unsigned long data; + unsigned int sns_inc = 0; + u32 reg_value; + u32 reg_value2; + /* For GMAC3.x, 4.x versions, convert the ptp_clock to nano second + * formula = (1/ptp_clock) * 1000000000 + * where ptp_clock is 50MHz if fine method is used to update system + */ + if (value & PTP_TCR_TSCFUPDT) { + data = (1000000000ULL / ptp_clock); + sns_inc = 1000000000ull - (data * ptp_clock); + sns_inc = (sns_inc * 256) / ptp_clock; + + } else { + data = (1000000000ULL / ptp_clock); + } + /* 0.465ns accuracy */ + if (!(value & PTP_TCR_TSCTRLSSR)) + data = (data * 1000) / 465; + + data &= PTP_SSIR_SSINC_MASK; + + reg_value = data; + if (gmac4) + reg_value <<= GMAC4_PTP_SSIR_SSINC_SHIFT; + + sns_inc &= PTP_SSIR_SNSINC_MASK; + reg_value2 = sns_inc; + if (gmac4) + reg_value2 <<= GMAC4_PTP_SSIR_SNSINC_SHIFT; + writel_relaxed(reg_value + reg_value2, ioaddr + PTP_SSIR); + return data; +} + +static u32 pps_config_default_addend(void __iomem *ioaddr, + struct stmmac_priv *priv, u32 ptp_clock) +{ + u64 temp; + + /* formula is : + * addend = 2^32/freq_div_ratio; + * + * where, freq_div_ratio = DWC_ETH_QOS_SYSCLOCK/50MHz + * + * hence, addend = ((2^32) * 50MHz)/DWC_ETH_QOS_SYSCLOCK; + * + * NOTE: DWC_ETH_QOS_SYSCLOCK should be >= 50MHz to + * achive 20ns accuracy. + * + * 2^x * y == (y << x), hence + * 2^32 * 50000000 ==> (50000000 << 32) + */ + if (ptp_clock == 250000000) { + // If PTP_CLOCK == SYS_CLOCK, best we can do is 2^32 - 1 + priv->default_addend = 0xFFFFFFFF; + } else { + temp = (u64)((u64)ptp_clock << 32); + priv->default_addend = div_u64(temp, 250000000); + } + priv->hw->ptp->config_addend(ioaddr, priv->default_addend); + + return 1; +} + +int ppsout_stop(struct stmmac_priv *priv, struct pps_cfg *eth_pps_cfg) +{ + u32 val; + void __iomem *ioaddr = priv->ioaddr; + + val |= PPSCMDX(eth_pps_cfg->ppsout_ch, 0x5); + val |= TRGTMODSELX(eth_pps_cfg->ppsout_ch, 0x3); + val |= PPSEN0; + writel_relaxed(val, ioaddr + MAC_PPS_CONTROL); + return 0; +} + +static irqreturn_t ethqos_pps_avb_class_a(int irq, void *dev_id) +{ + struct stmmac_priv *priv = + (struct stmmac_priv *)dev_id; + + struct qcom_ethqos *ethqos = priv->plat->bsp_priv; + + ethqos->avb_class_a_intr_cnt++; + avb_class_a_msg_wq_flag = true; + wake_up_interruptible(&avb_class_a_msg_wq); + + return IRQ_HANDLED; +} + +static irqreturn_t ethqos_pps_avb_class_b(int irq, void *dev_id) +{ + struct stmmac_priv *priv = + (struct stmmac_priv *)dev_id; + + struct qcom_ethqos *ethqos = priv->plat->bsp_priv; + + ethqos->avb_class_b_intr_cnt++; + avb_class_b_msg_wq_flag = true; + wake_up_interruptible(&avb_class_b_msg_wq); + return IRQ_HANDLED; +} + +static void ethqos_register_pps_isr(struct stmmac_priv *priv, int ch) +{ + int ret; + struct qcom_ethqos *ethqos = priv->plat->bsp_priv; + + if (ch == DWC_ETH_QOS_PPS_CH_2) { + ret = request_irq(ethqos->pps_class_a_irq, + ethqos_pps_avb_class_a, + IRQF_TRIGGER_RISING, "stmmac_pps", priv); + if (ret) + ETHQOSERR("pps_avb_class_a_irq Failed ret=%d\n", ret); + else + ETHQOSDBG("pps_avb_class_a_irq pass\n"); + + } else if (ch == DWC_ETH_QOS_PPS_CH_3) { + ret = request_irq(ethqos->pps_class_b_irq, + ethqos_pps_avb_class_b, + IRQF_TRIGGER_RISING, "stmmac_pps", priv); + if (ret) + ETHQOSERR("pps_avb_class_b_irq Failed ret=%d\n", ret); + else + ETHQOSDBG("pps_avb_class_b_irq pass\n"); + } +} + +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; + + if (!eth_pps_cfg->ppsout_start) { + ppsout_stop(priv, eth_pps_cfg); + return 0; + } + + value = (PTP_TCR_TSENA | PTP_TCR_TSCFUPDT | PTP_TCR_TSUPDT); + priv->hw->ptp->config_hw_tstamping(priv->ptpaddr, value); + 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); + + sub_second_inc = pps_config_sub_second_increment + (priv->ptpaddr, eth_pps_cfg->ptpclk_freq, + priv->plat->has_gmac4); + pps_config_default_addend(priv->ptpaddr, priv, + eth_pps_cfg->ptpclk_freq); + + 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 |= 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); + + writel_relaxed(0, ioaddr + + MAC_PPSX_TARGET_TIME_SEC(eth_pps_cfg->ppsout_ch)); + + writel_relaxed(0, ioaddr + + 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); + + 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)); + + writel_relaxed(width, ioaddr + MAC_PPSX_WIDTH(eth_pps_cfg->ppsout_ch)); + + writel_relaxed(val, ioaddr + MAC_PPS_CONTROL); + + 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) +{ + unsigned int len = 0, buf_len = 5000; + char *temp_buf; + ssize_t ret_cnt = 0; + struct pps_info *info; + + info = filp->private_data; + + if (info->channel_no == AVB_CLASS_A_CHANNEL_NUM) { + avb_class_a_msg_wq_flag = false; + temp_buf = kzalloc(buf_len, GFP_KERNEL); + if (!temp_buf) + return -ENOMEM; + + if (pethqos) + len = scnprintf(temp_buf, buf_len, + "%ld\n", pethqos->avb_class_a_intr_cnt); + else + len = scnprintf(temp_buf, buf_len, "0\n"); + + ret_cnt = simple_read_from_buffer(buf, count, f_pos, + temp_buf, len); + kfree(temp_buf); + if (pethqos) + ETHQOSERR("poll pps2intr info=%d sent by kernel\n", + pethqos->avb_class_a_intr_cnt); + } else if (info->channel_no == AVB_CLASS_B_CHANNEL_NUM) { + avb_class_b_msg_wq_flag = false; + temp_buf = kzalloc(buf_len, GFP_KERNEL); + if (!temp_buf) + return -ENOMEM; + + if (pethqos) + len = scnprintf(temp_buf, buf_len, + "%ld\n", pethqos->avb_class_b_intr_cnt); + else + len = scnprintf(temp_buf, buf_len, "0\n"); + + ret_cnt = simple_read_from_buffer + (buf, count, f_pos, temp_buf, len); + kfree(temp_buf); + + } else { + ETHQOSERR("invalid channel %d\n", info->channel_no); + } + return ret_cnt; +} + +static unsigned int pps_fops_poll(struct file *file, poll_table *wait) +{ + unsigned int mask = 0; + struct pps_info *info; + + info = file->private_data; + if (info->channel_no == AVB_CLASS_A_CHANNEL_NUM) { + ETHQOSERR("avb_class_a_fops_poll wait\n"); + + poll_wait(file, &avb_class_a_msg_wq, wait); + + if (avb_class_a_msg_wq_flag) { + //Sending read mask + mask |= POLLIN | POLLRDNORM; + } + } else if (info->channel_no == AVB_CLASS_B_CHANNEL_NUM) { + poll_wait(file, &avb_class_b_msg_wq, wait); + + if (avb_class_b_msg_wq_flag) { + //Sending read mask + mask |= POLLIN | POLLRDNORM; + } + } else { + ETHQOSERR("invalid channel %d\n", info->channel_no); + } + return mask; +} + +static int pps_open(struct inode *inode, struct file *file) +{ + struct pps_info *info; + + info = kmalloc(sizeof(*info), GFP_KERNEL); + if (!info) + return -ENOMEM; + + if (!strlcmp(file->f_path.dentry->d_iname, + AVB_CLASS_A_POLL_DEV_NODE, + strlen(AVB_CLASS_A_POLL_DEV_NODE))) { + ETHQOSERR("pps open file name =%s\n", + file->f_path.dentry->d_iname); + info->channel_no = AVB_CLASS_A_CHANNEL_NUM; + } else if (!strlcmp(file->f_path.dentry->d_iname, + AVB_CLASS_B_POLL_DEV_NODE, + strlen(AVB_CLASS_B_POLL_DEV_NODE))) { + ETHQOSERR("pps open file name =%s\n", + file->f_path.dentry->d_iname); + info->channel_no = AVB_CLASS_B_CHANNEL_NUM; + } else { + ETHQOSERR("stsrncmp failed for %s\n", + file->f_path.dentry->d_iname); + } + file->private_data = info; + return 0; +} + +static int pps_release(struct inode *inode, struct file *file) +{ + kfree(file->private_data); + return 0; +} + +static const struct file_operations pps_fops = { + .owner = THIS_MODULE, + .open = pps_open, + .release = pps_release, + .read = pps_fops_read, + .poll = pps_fops_poll, +}; + +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 ret; + + ret = alloc_chrdev_region(pps_dev_t, 0, 1, + pps_dev_node_name); + if (ret) { + ETHQOSERR("alloc_chrdev_region error for node %s\n", + pps_dev_node_name); + goto alloc_chrdev1_region_fail; + } + + *pps_cdev = cdev_alloc(); + if (!*pps_cdev) { + ret = -ENOMEM; + ETHQOSERR("failed to alloc cdev\n"); + goto fail_alloc_cdev; + } + cdev_init(*pps_cdev, &pps_fops); + + ret = cdev_add(*pps_cdev, *pps_dev_t, 1); + if (ret < 0) { + ETHQOSERR(":cdev_add err=%d\n", -ret); + goto cdev1_add_fail; + } + + *pps_class = class_create(THIS_MODULE, pps_dev_node_name); + if (!*pps_class) { + ret = -ENODEV; + ETHQOSERR("failed to create class\n"); + goto fail_create_class; + } + + if (!device_create(*pps_class, NULL, + *pps_dev_t, NULL, pps_dev_node_name)) { + ret = -EINVAL; + ETHQOSERR("failed to create device_create\n"); + goto fail_create_device; + } + + return 0; + +fail_create_device: + class_destroy(*pps_class); +fail_create_class: + cdev_del(*pps_cdev); +cdev1_add_fail: +fail_alloc_cdev: + unregister_chrdev_region(*pps_dev_t, 1); +alloc_chrdev1_region_fail: + return ret; +} diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h index dc97628b04d7..dad7fa94eecf 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h @@ -260,6 +260,7 @@ extern struct emac_emb_smmu_cb_ctx emac_emb_smmu_ctx; #define GET_MEM_PDEV_DEV (emac_emb_smmu_ctx.valid ? \ &emac_emb_smmu_ctx.smmu_pdev->dev : priv->device) +int ethqos_handle_prv_ioctl(struct net_device *dev, struct ifreq *rq, int cmd); extern bool phy_intr_en; int stmmac_mdio_unregister(struct net_device *ndev); diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c index 44157d0b439b..da51b83f9d1c 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -2614,6 +2614,8 @@ static int stmmac_hw_setup(struct net_device *dev, bool init_ptp) netdev_warn(priv->dev, "PTP not supported by HW\n"); else if (ret) netdev_warn(priv->dev, "PTP init failed\n"); + else + ret = clk_set_rate(priv->plat->clk_ptp_ref, 96000000); } priv->tx_lpi_timer = STMMAC_DEFAULT_TWT_LS; @@ -3972,6 +3974,8 @@ static int stmmac_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) break; case SIOCGHWTSTAMP: ret = stmmac_hwtstamp_get(dev, rq); + case SIOCDEVPRIVATE: + ret = ethqos_handle_prv_ioctl(dev, rq, cmd); break; default: break; diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h index 7abb1d47e7da..cf99a334ce7a 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h @@ -63,5 +63,7 @@ /* SSIR defines */ #define PTP_SSIR_SSINC_MASK 0xff #define GMAC4_PTP_SSIR_SSINC_SHIFT 16 +#define PTP_SSIR_SNSINC_MASK 0xff +#define GMAC4_PTP_SSIR_SNSINC_SHIFT 8 #endif /* __STMMAC_PTP_H__ */ From f5cfd4ff35ac286988e87814d2671f2f6324a6f7 Mon Sep 17 00:00:00 2001 From: Suraj Jaiswal Date: Wed, 17 Mar 2021 15:48:15 +0530 Subject: [PATCH 2/3] net: stmmac: Enable PPS0 on bootup Enable PPS0 for 19.2 MHz frequency on initializing PTP. Change-Id: I15d5fc2d15dd38926970ac7a61f9362604a860a8 Acked-by: Nagarjuna Chaganti Signed-off-by: Suraj Jaiswal --- drivers/net/ethernet/stmicro/stmmac/Makefile | 2 + .../ethernet/stmicro/stmmac/dwmac-qcom-pps.c | 39 +++++++++++++++++++ drivers/net/ethernet/stmicro/stmmac/stmmac.h | 1 + .../net/ethernet/stmicro/stmmac/stmmac_main.c | 6 +++ 4 files changed, 48 insertions(+) diff --git a/drivers/net/ethernet/stmicro/stmmac/Makefile b/drivers/net/ethernet/stmicro/stmmac/Makefile index 524040278562..eb129df30f30 100644 --- a/drivers/net/ethernet/stmicro/stmmac/Makefile +++ b/drivers/net/ethernet/stmicro/stmmac/Makefile @@ -32,5 +32,7 @@ obj-$(CONFIG_DWMAC_GENERIC) += dwmac-generic.o stmmac-platform-objs:= stmmac_platform.o dwmac-altr-socfpga-objs := altr_tse_pcs.o dwmac-socfpga.o +ccflags-$(CONFIG_PTP_1588_CLOCK)+=-DCONFIG_PTPSUPPORT_OBJ + obj-$(CONFIG_STMMAC_PCI) += stmmac-pci.o stmmac-pci-objs:= stmmac_pci.o diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c index 0b8756b548b5..0c408ebac81d 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c @@ -119,6 +119,7 @@ int ppsout_stop(struct stmmac_priv *priv, struct pps_cfg *eth_pps_cfg) u32 val; void __iomem *ioaddr = priv->ioaddr; + val = readl_relaxed(ioaddr + MAC_PPS_CONTROL); val |= PPSCMDX(eth_pps_cfg->ppsout_ch, 0x5); val |= TRGTMODSELX(eth_pps_cfg->ppsout_ch, 0x3); val |= PPSEN0; @@ -178,6 +179,17 @@ static void ethqos_register_pps_isr(struct stmmac_priv *priv, int ch) } } +static void ethqos_unregister_pps_isr(struct stmmac_priv *priv, int ch) +{ + struct qcom_ethqos *ethqos = priv->plat->bsp_priv; + + if (ch == DWC_ETH_QOS_PPS_CH_2) { + free_irq(ethqos->pps_class_a_irq, priv); + } else if (ch == DWC_ETH_QOS_PPS_CH_3) { + free_irq(ethqos->pps_class_b_irq, priv); + } +} + int ppsout_config(struct stmmac_priv *priv, struct ifr_data_struct *req) { struct pps_cfg *eth_pps_cfg = (struct pps_cfg *)req->ptr; @@ -187,6 +199,9 @@ int ppsout_config(struct stmmac_priv *priv, struct ifr_data_struct *req) 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); return 0; } @@ -238,6 +253,30 @@ int ppsout_config(struct stmmac_priv *priv, struct ifr_data_struct *req) return 0; } +int ethqos_init_pps(struct stmmac_priv *priv) +{ + u32 value; + struct ifr_data_struct req = {0}; + struct pps_cfg eth_pps_cfg = {0}; + + priv->ptpaddr = priv->ioaddr + PTP_GMAC4_OFFSET; + value = (PTP_TCR_TSENA | PTP_TCR_TSCFUPDT | PTP_TCR_TSUPDT); + priv->hw->ptp->config_hw_tstamping(priv->ptpaddr, value); + priv->hw->ptp->init_systime(priv->ptpaddr, 0, 0); + priv->hw->ptp->adjust_systime(priv->ptpaddr, 0, 0, 0, 1); + + /*Configuaring PPS0 PPS output frequency to default 19.2 Mhz*/ + eth_pps_cfg.ppsout_ch = 0; + eth_pps_cfg.ptpclk_freq = 62500000; + eth_pps_cfg.ppsout_freq = 19200000; + eth_pps_cfg.ppsout_start = 1; + eth_pps_cfg.ppsout_duty = 50; + req.ptr = (void *)ð_pps_cfg; + + ppsout_config(priv, &req); + return 0; +} + int ethqos_handle_prv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd) { struct stmmac_priv *pdata = netdev_priv(dev); diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h index dad7fa94eecf..6142ee2d3a11 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h @@ -261,6 +261,7 @@ extern struct emac_emb_smmu_cb_ctx emac_emb_smmu_ctx; #define GET_MEM_PDEV_DEV (emac_emb_smmu_ctx.valid ? \ &emac_emb_smmu_ctx.smmu_pdev->dev : priv->device) int ethqos_handle_prv_ioctl(struct net_device *dev, struct ifreq *rq, int cmd); +int ethqos_init_pps(struct stmmac_priv *priv); extern bool phy_intr_en; int stmmac_mdio_unregister(struct net_device *ndev); diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c index da51b83f9d1c..f31e691fe5c6 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -2616,6 +2616,7 @@ static int stmmac_hw_setup(struct net_device *dev, bool init_ptp) netdev_warn(priv->dev, "PTP init failed\n"); else ret = clk_set_rate(priv->plat->clk_ptp_ref, 96000000); + ret = ethqos_init_pps(priv); } priv->tx_lpi_timer = STMMAC_DEFAULT_TWT_LS; @@ -2719,7 +2720,12 @@ static int stmmac_open(struct net_device *dev) goto init_error; } +#ifdef CONFIG_PTPSUPPORT_OBJ ret = stmmac_hw_setup(dev, true); +#else + ret = stmmac_hw_setup(dev, false); +#endif + if (ret < 0) { netdev_err(priv->dev, "%s: Hw setup failed\n", __func__); goto init_error; From 33fdbe772f41b988bf4f0c83f9fe8bf81ada349a Mon Sep 17 00:00:00 2001 From: Suraj Jaiswal Date: Wed, 17 Mar 2021 15:56:07 +0530 Subject: [PATCH 3/3] 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