Merge "net: stmmac: Add multi queue support" (2b4df556) · Commits · e / devices / android_kernel_fairphone_FP5

drivers/net/ethernet/stmicro/stmmac/Makefile

+3 −0

Original line number	Diff line number	Diff line
		@@ -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
		@@ -31,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

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

+189 −2

Original line number	Diff line number	Diff line
		@@ -19,11 +19,12 @@
		#include <linux/dma-iommu.h>
		#include <linux/iommu.h>
		#include <linux/micrel_phy.h>

		#include <linux/tcp.h>
		#include <linux/ip.h>
		#include <linux/ipv6.h>
		#include "stmmac.h"
		#include "stmmac_platform.h"
		#include "dwmac-qcom-ethqos.h"

		#include "stmmac_ptp.h"

		#define RGMII_IO_MACRO_DEBUG1 0x20
		@@ -114,6 +115,160 @@
		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)
		{
		@@ -696,6 +851,22 @@ static int emac_emb_smmu_cb_probe(struct platform_device *pdev)
		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;
		@@ -755,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");
		@@ -798,6 +970,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(&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:

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

+146 −0

Original line number	Diff line number	Diff line
		@@ -25,6 +25,48 @@
		#define EMAC_HW_v2_3_1 0x20030001
		#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)
		#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 +111,114 @@ 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);
		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

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

0 → 100644

+459 −0

Original line number	Diff line number	Diff line
		// SPDX-License-Identifier: GPL-2.0-only
		/* Copyright (c) 2021, The Linux Foundation. All rights reserved.
		*/

		#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, priv->plat->clk_ptp_rate);
		}
		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 = readl_relaxed(ioaddr + MAC_PPS_CONTROL);
		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");
		}
		}

		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)
		{
		int interval, width;
		u32 sub_second_inc, value;
		void __iomem *ioaddr = priv->ioaddr;
		u32 val;
		struct pps_cfg eth_pps_cfg;

		if (copy_from_user(&eth_pps_cfg, (void __user *)req->ptr,
		sizeof(struct pps_cfg)))
		return -EFAULT;

		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;
		}

		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);

		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_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 *)&eth_pps_cfg;

		ppsout_config(priv, &req);
		return 0;
		}

		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;
		}

drivers/net/ethernet/stmicro/stmmac/stmmac.h

+2 −0

Original line number	Diff line number	Diff line
		@@ -260,6 +260,8 @@ 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);