Merge "net: stmmac: SDx55: add pps signal and interrupts support" (d6258366) · Commits · e / devices / android_kernel_xiaomi_nabu

drivers/net/ethernet/stmicro/stmmac/Makefile

+1 −0

Original line number	Diff line number	Diff line
		@@ -15,6 +15,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

drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c

+170 −0

Original line number	Diff line number	Diff line
		@@ -7,10 +7,20 @@
		#include <linux/platform_device.h>
		#include <linux/phy.h>
		#include <linux/regulator/consumer.h>
		#include <linux/of_gpio.h>
		#include <linux/io.h>
		#include <linux/iopoll.h>
		#include <linux/mii.h>
		#include <linux/of_mdio.h>
		#include <linux/slab.h>
		#include <linux/ipc_logging.h>
		#include <linux/poll.h>
		#include <linux/debugfs.h>

		#include "stmmac.h"
		#include "stmmac_platform.h"
		#include "dwmac-qcom-ethqos.h"
		#include "stmmac_ptp.h"

		#define RGMII_IO_MACRO_DEBUG1 0x20
		#define EMAC_SYSTEM_LOW_POWER_DEBUG 0x28
		@@ -69,6 +79,29 @@
		#define EMAC_I0_EMAC_CORE_HW_VERSION_RGOFFADDR 0x00000070
		#define EMAC_HW_v2_3_2_RG 0x20030002

		#define MII_BUSY 0x00000001
		#define MII_WRITE 0x00000002

		/* GMAC4 defines */
		#define MII_GMAC4_GOC_SHIFT 2
		#define MII_GMAC4_WRITE BIT(MII_GMAC4_GOC_SHIFT)
		#define MII_GMAC4_READ (3 << MII_GMAC4_GOC_SHIFT)

		#define MII_BUSY 0x00000001
		#define MII_WRITE 0x00000002

		#define DWC_ETH_QOS_PHY_INTR_STATUS 0x0013

		#define LINK_UP 1
		#define LINK_DOWN 0

		#define LINK_DOWN_STATE 0x800
		#define LINK_UP_STATE 0x400

		bool phy_intr_en;

		struct qcom_ethqos *pethqos;

		static int rgmii_readl(struct qcom_ethqos *ethqos, unsigned int offset)
		{
		return readl(ethqos->rgmii_base + offset);
		@@ -452,6 +485,120 @@ static void ethqos_fix_mac_speed(void *priv, unsigned int speed)
		ethqos_configure(ethqos);
		}

		static int ethqos_mdio_read(struct stmmac_priv *priv, int phyaddr, int phyreg)
		{
		unsigned int mii_address = priv->hw->mii.addr;
		unsigned int mii_data = priv->hw->mii.data;
		u32 v;
		int data;
		u32 value = MII_BUSY;

		value \|= (phyaddr << priv->hw->mii.addr_shift)
		& priv->hw->mii.addr_mask;
		value \|= (phyreg << priv->hw->mii.reg_shift) & priv->hw->mii.reg_mask;
		value \|= (priv->clk_csr << priv->hw->mii.clk_csr_shift)
		& priv->hw->mii.clk_csr_mask;
		if (priv->plat->has_gmac4)
		value \|= MII_GMAC4_READ;

		if (readl_poll_timeout(priv->ioaddr + mii_address, v, !(v & MII_BUSY),
		100, 10000))
		return -EBUSY;

		writel_relaxed(value, priv->ioaddr + mii_address);

		if (readl_poll_timeout(priv->ioaddr + mii_address, v, !(v & MII_BUSY),
		100, 10000))
		return -EBUSY;

		/* Read the data from the MII data register */
		data = (int)readl_relaxed(priv->ioaddr + mii_data);

		return data;
		}

		static int ethqos_phy_intr_config(struct qcom_ethqos *ethqos)
		{
		int ret = 0;

		ethqos->phy_intr = platform_get_irq_byname(ethqos->pdev, "phy-intr");

		if (ethqos->phy_intr < 0) {
		if (ethqos->phy_intr != -EPROBE_DEFER) {
		dev_err(&ethqos->pdev->dev,
		"PHY IRQ configuration information not found\n");
		}
		ret = 1;
		}

		return ret;
		}

		static void ethqos_handle_phy_interrupt(struct qcom_ethqos *ethqos)
		{
		int phy_intr_status = 0;
		struct platform_device *pdev = ethqos->pdev;

		struct net_device *dev = platform_get_drvdata(pdev);
		struct stmmac_priv *priv = netdev_priv(dev);

		phy_intr_status = ethqos_mdio_read(priv, priv->plat->phy_addr,
		DWC_ETH_QOS_PHY_INTR_STATUS);

		if (phy_intr_status & LINK_UP_STATE)
		phy_mac_interrupt(dev->phydev, LINK_UP);
		else if (phy_intr_status & LINK_DOWN_STATE)
		phy_mac_interrupt(dev->phydev, LINK_DOWN);
		}

		static void ethqos_defer_phy_isr_work(struct work_struct *work)
		{
		struct qcom_ethqos *ethqos =
		container_of(work, struct qcom_ethqos, emac_phy_work);

		ethqos_handle_phy_interrupt(ethqos);
		}

		static irqreturn_t ETHQOS_PHY_ISR(int irq, void *dev_data)
		{
		struct qcom_ethqos ethqos = (struct qcom_ethqos )dev_data;

		queue_work(system_wq, &ethqos->emac_phy_work);
		}

		static int ethqos_phy_intr_enable(struct qcom_ethqos *ethqos)
		{
		int ret = 0;
		struct net_device *dev = platform_get_drvdata(ethqos->pdev);

		INIT_WORK(&ethqos->emac_phy_work, ethqos_defer_phy_isr_work);
		ret = request_irq(ethqos->phy_intr, ETHQOS_PHY_ISR,
		IRQF_SHARED, "stmmac", ethqos);
		if (ret) {
		ETHQOSERR("Unable to register PHY IRQ %d\n",
		ethqos->phy_intr);
		return ret;
		}
		phy_intr_en = true;
		return ret;
		}

		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;
		@@ -473,6 +620,8 @@ static int qcom_ethqos_probe(struct platform_device *pdev)
		ethqos->pdev = pdev;

		ethqos_init_reqgulators(ethqos);
		ethqos_init_gpio(ethqos);

		plat_dat = stmmac_probe_config_dt(pdev, &stmmac_res.mac);
		if (IS_ERR(plat_dat)) {
		dev_err(&pdev->dev, "dt configuration failed\n");
		@@ -517,8 +666,26 @@ static int qcom_ethqos_probe(struct platform_device *pdev)
		ethqos->emac_ver = rgmii_readl(ethqos,
		EMAC_I0_EMAC_CORE_HW_VERSION_RGOFFADDR);

		if (!ethqos_phy_intr_config(ethqos))
		ethqos_phy_intr_enable(ethqos);
		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(&ethqos->avb_class_a_dev_t,
		&ethqos->avb_class_a_cdev,
		&ethqos->avb_class_a_class,
		AVB_CLASS_A_POLL_DEV_NODE);

		create_pps_interrupt_device_node(&ethqos->avb_class_b_dev_t,
		&ethqos->avb_class_b_cdev,
		&ethqos->avb_class_b_class,
		AVB_CLASS_B_POLL_DEV_NODE);
		}

		pethqos = ethqos;
		return ret;

		err_clk:
		@@ -541,6 +708,9 @@ static int qcom_ethqos_remove(struct platform_device *pdev)

		ret = stmmac_pltfr_remove(pdev);
		clk_disable_unprepare(ethqos->rgmii_clk);

		if (phy_intr_en)
		free_irq(ethqos->phy_intr, ethqos);
		ethqos_disable_regulators(ethqos);

		return ret;

drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h

+94 −0

Original line number	Diff line number	Diff line
		@@ -28,6 +28,46 @@
		#define SDCC_USR_CTL 0x18
		#define RGMII_IO_MACRO_CONFIG2 0x1C

		#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;
		@@ -59,6 +99,11 @@ struct qcom_ethqos {
		struct clk *rgmii_clk;
		unsigned int speed;

		int gpio_phy_intr_redirect;
		u32 phy_intr;
		/* Work struct for handling phy interrupt */
		struct work_struct emac_phy_work;

		const struct ethqos_emac_por *por;
		unsigned int num_por;
		unsigned int emac_ver;
		@@ -67,8 +112,57 @@ struct qcom_ethqos {
		struct regulator *reg_rgmii;
		struct regulator *reg_emac_phy;
		struct regulator *reg_rgmii_io_pads;

		u32 pps_class_a_irq;
		u32 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

drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-gpio.c

+96 −0

Original line number	Diff line number	Diff line
		@@ -15,6 +15,7 @@
		#include <linux/platform_device.h>
		#include <linux/phy.h>
		#include <linux/regulator/consumer.h>
		#include <linux/of_gpio.h>

		#include "stmmac.h"
		#include "dwmac-qcom-ethqos.h"
		@@ -23,6 +24,47 @@
		#define EMAC_VREG_RGMII_NAME "vreg_rgmii"
		#define EMAC_VREG_EMAC_PHY_NAME "vreg_emac_phy"
		#define EMAC_VREG_RGMII_IO_PADS_NAME "vreg_rgmii_io_pads"
		#define EMAC_PIN_PPS0 "dev-emac_pin_pps_0"


		static int setup_gpio_input_common
		(struct device dev, const char name, int *gpio)
		{
		int ret = 0;

		if (of_find_property(dev->of_node, name, NULL)) {
		*gpio = ret = of_get_named_gpio(dev->of_node, name, 0);
		if (ret >= 0) {
		ret = gpio_request(*gpio, name);
		if (ret) {
		ETHQOSERR("%s: Can't get GPIO %s, ret = %d\n",
		name, *gpio);
		*gpio = -1;
		return ret;
		}

		ret = gpio_direction_input(*gpio);
		if (ret) {
		ETHQOSERR(
		"%s: Can't set GPIO %s direction, ret = %d\n",
		name, ret);
		return ret;
		}
		} else {
		if (ret == -EPROBE_DEFER)
		ETHQOSERR("get EMAC_GPIO probe defer\n");
		else
		ETHQOSERR("can't get gpio %s ret %d\n", name,
		ret);
		return ret;
		}
		} else {
		ETHQOSERR("can't find gpio %s\n", name);
		ret = -EINVAL;
		}

		return ret;
		}

		int ethqos_init_reqgulators(struct qcom_ethqos *ethqos)
		{
		@@ -139,3 +181,57 @@ void ethqos_disable_regulators(struct qcom_ethqos *ethqos)
		ethqos->gdsc_emac = NULL;
		}
		}

		void ethqos_free_gpios(struct qcom_ethqos *ethqos)
		{
		if (gpio_is_valid(ethqos->gpio_phy_intr_redirect))
		gpio_free(ethqos->gpio_phy_intr_redirect);
		ethqos->gpio_phy_intr_redirect = -1;
		}

		int ethqos_init_gpio(struct qcom_ethqos *ethqos)
		{
		struct pinctrl *pinctrl;
		struct pinctrl_state *emac_pps_0;

		ethqos->gpio_phy_intr_redirect = -1;
		int ret = 0;

		pinctrl = devm_pinctrl_get(&ethqos->pdev->dev);
		if (IS_ERR_OR_NULL(pinctrl)) {
		ret = PTR_ERR(pinctrl);
		ETHQOSERR("Failed to get pinctrl, err = %d\n", ret);
		return ret;
		}
		ETHQOSDBG("get pinctrl succeed\n");

		ret = setup_gpio_input_common(
		&ethqos->pdev->dev, "qcom,phy-intr-redirect",
		&ethqos->gpio_phy_intr_redirect);

		if (ret) {
		ETHQOSERR("Failed to setup <%s> gpio\n",
		"qcom,phy-intr-redirect");
		goto gpio_error;
		}

		emac_pps_0 = pinctrl_lookup_state(pinctrl, EMAC_PIN_PPS0);
		if (IS_ERR_OR_NULL(emac_pps_0)) {
		ret = PTR_ERR(emac_pps_0);
		ETHQOSERR("Failed to get emac_pps_0, err = %d\n", ret);
		return ret;
		}
		ETHQOSDBG("Get emac_pps_0 succeed\n");
		ret = pinctrl_select_state(pinctrl, emac_pps_0);
		if (ret)
		ETHQOSERR("Unable to set emac_pps_0 state, err = %d\n",
		ret);
		else
		ETHQOSDBG("Set emac_pps_0 succeed\n");

		return ret;

		gpio_error:
		ethqos_free_gpios(ethqos);
		return ret;
		}

drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-pps.c

0 → 100644

+444 −0

Original line number	Diff line number	Diff line
		/* Copyright (c) 2019, The Linux Foundation. All rights reserved.
		*
		* This program is free software; you can redistribute it and/or modify
		* it under the terms of the GNU General Public License version 2 and
		* only version 2 as published by the Free Software Foundation.
		*
		* This program is distributed in the hope that it will be useful,
		* but WITHOUT ANY WARRANTY; without even the implied warranty of
		* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		* GNU General Public License for more details.
		*/

		#include <linux/module.h>
		#include <linux/of.h>
		#include <linux/of_device.h>
		#include <linux/platform_device.h>
		#include <linux/phy.h>
		#include <linux/regulator/consumer.h>
		#include <linux/of_gpio.h>
		#include <linux/io.h>
		#include <linux/iopoll.h>
		#include <linux/mii.h>
		#include <linux/of_mdio.h>
		#include <linux/slab.h>
		#include <linux/ipc_logging.h>
		#include <linux/poll.h>
		#include <linux/debugfs.h>
		#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 = 1;
		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 = 1;
		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(&eth_pps_cfg, req.ptr,
		sizeof(struct pps_cfg)))
		return -EFAULT;
		req.ptr = &eth_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 = 0;
		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 = 0;
		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 == 1) {
		//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 == 1) {
		//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;
		}