Date:   Wed, 14 Nov 2018 16:50:45 -0800
From:   Dalon Westergreen <dwesterg@...il.com>
To:     netdev@...r.kernel.org, dinguyen@...nel.org,
        thor.thayer@...ux.intel.com
Cc:     Dalon Westergreen <dalon.westergreen@...el.com>
Subject: [PATCH net-next 6/8] net: eth: altera: tse: add support for ptp and timestamping

From: Dalon Westergreen <dalon.westergreen@...el.com>

Add support for the ptp clock used with the tse, and update
the driver to support timestamping when enabled.  We also
enable debugfs entries for the ptp clock to allow some user
control and interaction with the ptp clock.

Signed-off-by: Dalon Westergreen <dalon.westergreen@...el.com>
---
 drivers/net/ethernet/altera/Kconfig           |   1 +
 drivers/net/ethernet/altera/Makefile          |   3 +-
 drivers/net/ethernet/altera/altera_ptp.c      | 473 ++++++++++++++++++
 drivers/net/ethernet/altera/altera_ptp.h      |  77 +++
 drivers/net/ethernet/altera/altera_tse.h      |  10 +
 .../net/ethernet/altera/altera_tse_ethtool.c  |  28 ++
 drivers/net/ethernet/altera/altera_tse_main.c | 164 +++++-
 7 files changed, 754 insertions(+), 2 deletions(-)
 create mode 100644 drivers/net/ethernet/altera/altera_ptp.c
 create mode 100644 drivers/net/ethernet/altera/altera_ptp.h

diff --git a/drivers/net/ethernet/altera/Kconfig b/drivers/net/ethernet/altera/Kconfig
index fdddba51473e..36aee0fc0b51 100644
--- a/drivers/net/ethernet/altera/Kconfig
+++ b/drivers/net/ethernet/altera/Kconfig
@@ -2,6 +2,7 @@ config ALTERA_TSE
 	tristate "Altera Triple-Speed Ethernet MAC support"
 	depends on HAS_DMA
 	select PHYLIB
+	select PTP_1588_CLOCK
 	---help---
 	  This driver supports the Altera Triple-Speed (TSE) Ethernet MAC.
 
diff --git a/drivers/net/ethernet/altera/Makefile b/drivers/net/ethernet/altera/Makefile
index d4a187e45369..ad80be42fa26 100644
--- a/drivers/net/ethernet/altera/Makefile
+++ b/drivers/net/ethernet/altera/Makefile
@@ -4,4 +4,5 @@
 
 obj-$(CONFIG_ALTERA_TSE) += altera_tse.o
 altera_tse-objs := altera_tse_main.o altera_tse_ethtool.o \
-altera_msgdma.o altera_sgdma.o altera_utils.o
+		   altera_msgdma.o altera_sgdma.o altera_utils.o \
+		   altera_ptp.o
diff --git a/drivers/net/ethernet/altera/altera_ptp.c b/drivers/net/ethernet/altera/altera_ptp.c
new file mode 100644
index 000000000000..4467b3c90c59
--- /dev/null
+++ b/drivers/net/ethernet/altera/altera_ptp.c
@@ -0,0 +1,473 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Altera PTP Hardware Clock (PHC) Linux driver
+ * Copyright (C) 2015-2016 Altera Corporation. All rights reserved.
+ * Copyright (C) 2017-2018 Intel Corporation. All rights reserved.
+ *
+ * Author(s):
+ *	Dalon Westergreen <dalon.westergreen@...el.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/gcd.h>
+#include <linux/module.h>
+#include <linux/math64.h>
+#include <linux/net_tstamp.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+
+#include "altera_ptp.h"
+#include "altera_utils.h"
+
+#define NOMINAL_PPB			1000000000ULL
+#define TOD_PERIOD_MAX			0xfffff
+#define TOD_PERIOD_MIN			0
+#define TOD_DRIFT_ADJUST_FNS_MAX	0xffff
+#define TOD_DRIFT_ADJUST_RATE_MAX	0xffff
+#define TOD_ADJUST_COUNT_MAX		0xfffff
+#define TOD_ADJUST_MS_MAX		(((((TOD_PERIOD_MAX) >> 16) + 1) * \
+					  ((TOD_ADJUST_COUNT_MAX) + 1)) /  \
+					 1000000UL)
+
+/* A fine ToD HW clock offset adjustment.
+ * To perform the fine offset adjustment the AdjustPeriod register is used
+ * to replace the Period register for AdjustCount clock cycles in hardware.
+ */
+static int fine_adjust_tod_clock(struct altera_ptp_private *priv,
+				 u32 adjust_period, u32 adjust_count)
+{
+	int limit;
+
+	csrwr32(adjust_period, priv->tod_ctrl, tod_csroffs(adjust_period));
+	csrwr32(adjust_count, priv->tod_ctrl, tod_csroffs(adjust_count));
+
+	/* Wait for present offset adjustment update to complete */
+	limit = TOD_ADJUST_MS_MAX;
+	while (limit--) {
+		if (!csrrd32(priv->tod_ctrl, tod_csroffs(adjust_count)))
+			break;
+		mdelay(1);
+	}
+	if (limit < 0)
+		return -EBUSY;
+
+	return 0;
+}
+
+/* A coarse ToD HW clock offset adjustment.
+ * The coarse time adjustment performs by adding or subtracting the delta value
+ * from the current ToD HW clock time.
+ */
+static int coarse_adjust_tod_clock(struct altera_ptp_private *priv, s64 delta)
+{
+	u64 seconds;
+	u32 seconds_msb;
+	u32 seconds_lsb;
+	u32 nanosec;
+	u64 now;
+
+	if (delta == 0)
+		goto out;
+
+	/* Get current time */
+	nanosec = csrrd32(priv->tod_ctrl, tod_csroffs(nanosec));
+	seconds_lsb = csrrd32(priv->tod_ctrl, tod_csroffs(seconds_lsb));
+	seconds_msb = csrrd32(priv->tod_ctrl, tod_csroffs(seconds_msb));
+
+	/* Calculate new time */
+	seconds = (((u64)(seconds_msb & 0x0000ffff)) << 32) | seconds_lsb;
+	now = seconds * NSEC_PER_SEC + nanosec + delta;
+
+	seconds = div_u64_rem(now, NSEC_PER_SEC, &nanosec);
+	seconds_msb = upper_32_bits(seconds) & 0x0000ffff;
+	seconds_lsb = lower_32_bits(seconds);
+
+	/* Set corrected time */
+	csrwr32(seconds_msb, priv->tod_ctrl, tod_csroffs(seconds_msb));
+	csrwr32(seconds_lsb, priv->tod_ctrl, tod_csroffs(seconds_lsb));
+	csrwr32(nanosec, priv->tod_ctrl, tod_csroffs(nanosec));
+
+out:
+	return 0;
+}
+
+static int adjust_fine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+	struct altera_ptp_private *priv =
+		container_of(ptp, struct altera_ptp_private, ptp_clock_ops);
+	unsigned long flags;
+	int ret = 0;
+	u64 ppb;
+	u32 tod_period;
+	u32 tod_rem;
+	u32 tod_drift_adjust_fns;
+	u32 tod_drift_adjust_rate;
+	unsigned long rate;
+
+	priv->scaled_ppm = scaled_ppm;
+
+	/* only unlock if it is currently locked */
+	if (mutex_is_locked(&priv->ppm_mutex))
+		mutex_unlock(&priv->ppm_mutex);
+
+	if (!priv->ptp_correct_freq)
+		goto out;
+
+	rate = clk_get_rate(priv->tod_clk);
+
+	/* From scaled_ppm_to_ppb */
+	ppb = 1 + scaled_ppm;
+	ppb *= 125;
+	ppb >>= 13;
+
+	ppb += NOMINAL_PPB;
+
+	tod_period = div_u64_rem(ppb << 16, rate, &tod_rem);
+	if (tod_period > TOD_PERIOD_MAX) {
+		ret = -ERANGE;
+		goto out;
+	}
+
+	/* The drift of ToD adjusted periodically by adding a drift_adjust_fns
+	 * correction value every drift_adjust_rate count of clock cycles.
+	 */
+	tod_drift_adjust_fns = tod_rem / gcd(tod_rem, rate);
+	tod_drift_adjust_rate = rate / gcd(tod_rem, rate);
+
+	while ((tod_drift_adjust_fns > TOD_DRIFT_ADJUST_FNS_MAX) |
+		(tod_drift_adjust_rate > TOD_DRIFT_ADJUST_RATE_MAX)) {
+		tod_drift_adjust_fns = tod_drift_adjust_fns >> 1;
+		tod_drift_adjust_rate = tod_drift_adjust_rate >> 1;
+	}
+
+	if (tod_drift_adjust_fns == 0)
+		tod_drift_adjust_rate = 0;
+
+	spin_lock_irqsave(&priv->tod_lock, flags);
+	csrwr32(tod_period, priv->tod_ctrl, tod_csroffs(period));
+	csrwr32(0, priv->tod_ctrl, tod_csroffs(adjust_period));
+	csrwr32(0, priv->tod_ctrl, tod_csroffs(adjust_count));
+	csrwr32(tod_drift_adjust_fns, priv->tod_ctrl,
+		tod_csroffs(drift_adjust));
+	csrwr32(tod_drift_adjust_rate, priv->tod_ctrl,
+		tod_csroffs(drift_adjust_rate));
+	spin_unlock_irqrestore(&priv->tod_lock, flags);
+
+out:
+	return ret;
+}
+
+static int adjust_time(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct altera_ptp_private *priv =
+		container_of(ptp, struct altera_ptp_private, ptp_clock_ops);
+	int ret = 0;
+	u64 abs_delta;
+	unsigned long flags;
+	u32 period;
+	u32 diff;
+	u64 count;
+	u32 rem;
+
+	if (!priv->ptp_correct_offs)
+		goto out;
+
+	if (delta < 0)
+		abs_delta = -delta;
+	else
+		abs_delta = delta;
+
+	spin_lock_irqsave(&priv->tod_lock, flags);
+
+	/* Get the maximum possible value of the Period register offset
+	 * adjustment in nanoseconds scale. This depends on the current
+	 * Period register settings and the maximum and minimum possible
+	 * values of the Period register.
+	 */
+	period = csrrd32(priv->tod_ctrl, tod_csroffs(period));
+
+	if (delta < 0)
+		diff = (period - TOD_PERIOD_MIN) >> 16;
+	else
+		diff = (TOD_PERIOD_MAX - period) >> 16;
+
+	/* Find out the least number of cycles needed for the Period register
+	 * adjustment by delta nanoseconds.
+	 */
+	while (diff) {
+		count = div_u64_rem(abs_delta, diff, &rem);
+		if (rem == 0)
+			break;
+		diff--;
+	}
+
+	if (diff && count && count < TOD_ADJUST_COUNT_MAX) {
+		/* Perform the fine time offset adjustment */
+		if (delta < 0)
+			period -= (diff << 16);
+		else
+			period += (diff << 16);
+
+		ret = fine_adjust_tod_clock(priv, period, count);
+
+	} else {
+		/* Perform the coarse time offset adjustment */
+		ret = coarse_adjust_tod_clock(priv, delta);
+	}
+
+	spin_unlock_irqrestore(&priv->tod_lock, flags);
+
+out:
+	return ret;
+}
+
+static int get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	struct altera_ptp_private *priv =
+		container_of(ptp, struct altera_ptp_private, ptp_clock_ops);
+	unsigned long flags;
+	u64 seconds;
+	u32 seconds_msb;
+	u32 seconds_lsb;
+	u32 nanosec;
+
+	spin_lock_irqsave(&priv->tod_lock, flags);
+	nanosec = csrrd32(priv->tod_ctrl, tod_csroffs(nanosec));
+	seconds_lsb = csrrd32(priv->tod_ctrl, tod_csroffs(seconds_lsb));
+	seconds_msb = csrrd32(priv->tod_ctrl, tod_csroffs(seconds_msb));
+	spin_unlock_irqrestore(&priv->tod_lock, flags);
+
+	seconds = (((u64)(seconds_msb & 0x0000ffff)) << 32) | seconds_lsb;
+
+	ts->tv_nsec = nanosec;
+	ts->tv_sec = (__kernel_time_t)seconds;
+
+	return 0;
+}
+
+static int set_time(struct ptp_clock_info *ptp, const struct timespec64 *ts)
+{
+	struct altera_ptp_private *priv =
+		container_of(ptp, struct altera_ptp_private, ptp_clock_ops);
+	unsigned long flags;
+	u32 seconds_msb = upper_32_bits(ts->tv_sec) & 0x0000ffff;
+	u32 seconds_lsb = lower_32_bits(ts->tv_sec);
+	u32 nanosec = lower_32_bits(ts->tv_nsec);
+
+	spin_lock_irqsave(&priv->tod_lock, flags);
+	csrwr32(seconds_msb, priv->tod_ctrl, tod_csroffs(seconds_msb));
+	csrwr32(seconds_lsb, priv->tod_ctrl, tod_csroffs(seconds_lsb));
+	csrwr32(nanosec, priv->tod_ctrl, tod_csroffs(nanosec));
+	spin_unlock_irqrestore(&priv->tod_lock, flags);
+
+	return 0;
+}
+
+static int enable_feature(struct ptp_clock_info *ptp,
+			  struct ptp_clock_request *request, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static struct ptp_clock_info altera_ptp_clock_ops = {
+	.owner = THIS_MODULE,
+	.name = "altera_ptp",
+	.max_adj = 500000000,
+	.n_alarm = 0,
+	.n_ext_ts = 0,
+	.n_per_out = 0,
+	.pps = 0,
+	.adjfine = adjust_fine,
+	.adjtime = adjust_time,
+	.gettime64 = get_time,
+	.settime64 = set_time,
+	.enable = enable_feature,
+};
+
+/* ptp debugfs */
+static ssize_t ptp_tod_read(struct file *filp, char __user *buffer,
+			    size_t count, loff_t *ppos)
+{
+	struct altera_ptp_private *priv = filp->private_data;
+	unsigned long flags;
+	u32 seconds_msb;
+	u32 seconds_lsb;
+	u32 nanosec;
+
+	if (*ppos != 0)
+		return 0;
+
+	if (count < (sizeof(u32) * 3))
+		return -ENOSPC;
+
+	spin_lock_irqsave(&priv->tod_lock, flags);
+	nanosec = csrrd32(priv->tod_ctrl, tod_csroffs(nanosec));
+	seconds_lsb = csrrd32(priv->tod_ctrl, tod_csroffs(seconds_lsb));
+	seconds_msb = csrrd32(priv->tod_ctrl, tod_csroffs(seconds_msb));
+	spin_unlock_irqrestore(&priv->tod_lock, flags);
+
+	if (copy_to_user(buffer, &seconds_msb, sizeof(u32)))
+		return -EFAULT;
+	buffer += sizeof(u32);
+
+	if (copy_to_user(buffer, &seconds_lsb, sizeof(u32)))
+		return -EFAULT;
+	buffer += sizeof(u32);
+
+	if (copy_to_user(buffer, &nanosec, sizeof(u32)))
+		return -EFAULT;
+
+	return (sizeof(u32) * 3);
+}
+
+static const struct file_operations ptp_dbg_tod_ops = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = ptp_tod_read,
+	.write = NULL,
+};
+
+static ssize_t ptp_ppm_read(struct file *filp, char __user *buffer,
+			    size_t count, loff_t *ppos)
+{
+	struct altera_ptp_private *priv = filp->private_data;
+
+	if (*ppos != 0)
+		return 0;
+
+	if (count < sizeof(long))
+		return -ENOSPC;
+
+	if (mutex_lock_interruptible(&priv->ppm_mutex))
+		return -EAGAIN;
+
+	if (copy_to_user(buffer, &priv->scaled_ppm, sizeof(long)))
+		return -EFAULT;
+
+	return sizeof(long);
+}
+
+static const struct file_operations ptp_dbg_ppm_ops = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = ptp_ppm_read,
+	.write = NULL,
+};
+
+int altera_ptp_dbg_init(struct altera_ptp_private *priv,
+			struct dentry *dfs_dir_root)
+{
+	struct dentry *dfs_dir;
+
+	dfs_dir = debugfs_create_dir("ptp", dfs_dir_root);
+
+	/* Use u32 instead of bool */
+	if (!debugfs_create_u32("correct_offset", 0600, dfs_dir,
+				&priv->ptp_correct_offs))
+		return -ENOMEM;
+
+	/* Use u32 instead of bool */
+	if (!debugfs_create_u32("correct_frequency", 0600, dfs_dir,
+				&priv->ptp_correct_freq))
+		return -ENOMEM;
+
+	if (!debugfs_create_file("tod", 0400, dfs_dir,
+				 (void *)priv, &ptp_dbg_tod_ops))
+		return -ENOMEM;
+
+	if (!debugfs_create_file("scaled_ppm", 0400, dfs_dir,
+				 (void *)priv, &ptp_dbg_ppm_ops))
+		return -ENOMEM;
+
+	return 0;
+}
+
+/* Initialize PTP control block registers */
+int altera_init_ptp(struct altera_ptp_private *priv)
+{
+	int ret = 0;
+	struct timespec64 now;
+
+	ret = adjust_fine(&priv->ptp_clock_ops, 0l);
+	if (ret != 0)
+		goto out;
+
+	/* Initialize the hardware clock to the system time */
+	getnstimeofday64(&now);
+	set_time(&priv->ptp_clock_ops, &now);
+
+	priv->ptp_correct_offs = true;
+	priv->ptp_correct_freq = true;
+
+	spin_lock_init(&priv->tod_lock);
+
+	/* we want the mutex locked by default */
+	mutex_init(&priv->ppm_mutex);
+	mutex_lock(&priv->ppm_mutex);
+
+out:
+	return ret;
+}
+
+/* Register the PTP clock driver to kernel */
+int altera_ptp_register(struct altera_ptp_private *priv, struct device *device)
+{
+	int ret = 0;
+
+	priv->ptp_clock_ops = altera_ptp_clock_ops;
+
+	priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops, device);
+	if (IS_ERR(priv->ptp_clock)) {
+		priv->ptp_clock = NULL;
+		ret = -ENODEV;
+	}
+
+	if (priv->tod_clk)
+		ret = clk_prepare_enable(priv->tod_clk);
+
+	return ret;
+}
+
+/* Remove/unregister the ptp clock driver from the kernel */
+void altera_ptp_unregister(struct altera_ptp_private *priv)
+{
+	if (priv->ptp_clock) {
+		ptp_clock_unregister(priv->ptp_clock);
+		priv->ptp_clock = NULL;
+	}
+
+	if (priv->tod_clk)
+		clk_disable_unprepare(priv->tod_clk);
+}
+
+/* Common PTP probe function */
+int altera_ptp_probe(struct platform_device *pdev,
+		     struct altera_ptp_private *priv)
+{
+	int ret = -ENODEV;
+	struct resource *ptp_res;
+
+	priv->dev = (struct net_device *)platform_get_drvdata(pdev);
+
+	/* Time-of-Day (ToD) Clock address space */
+	ret = request_and_map(pdev, "tod_ctrl", &ptp_res,
+			      (void __iomem **)&priv->tod_ctrl);
+	if (ret)
+		goto err;
+
+	dev_info(&pdev->dev, "\tTOD Ctrl at 0x%08lx\n",
+		 (unsigned long)ptp_res->start);
+
+	/* Time-of-Day (ToD) Clock period clock */
+	priv->tod_clk = devm_clk_get(&pdev->dev, "tod_clk");
+	if (IS_ERR(priv->tod_clk)) {
+		dev_err(&pdev->dev, "cannot obtain ToD period clock\n");
+		ret = -ENXIO;
+		goto err;
+	}
+err:
+	return ret;
+}
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/altera/altera_ptp.h b/drivers/net/ethernet/altera/altera_ptp.h
new file mode 100644
index 000000000000..66cd11902743
--- /dev/null
+++ b/drivers/net/ethernet/altera/altera_ptp.h
@@ -0,0 +1,77 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Altera PTP Hardware Clock (PHC) Linux driver
+ * Copyright (C) 2015-2016 Altera Corporation. All rights reserved.
+ * Copyright (C) 2017-2018 Intel Corporation. All rights reserved.
+ *
+ * Author(s):
+ *	Dalon Westergreen <dalon.westergreen@...el.com>
+ */
+
+#ifndef __ALTERA_PTP_H__
+#define __ALTERA_PTP_H__
+
+#include <linux/debugfs.h>
+#include <linux/netdevice.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+
+/* Altera Time-of-Day (ToD) clock register space. */
+struct altera_eth_tod {
+	u32 seconds_msb;
+	u32 seconds_lsb;
+	u32 nanosec;
+	u32 reserved1[0x1];
+	u32 period;
+	u32 adjust_period;
+	u32 adjust_count;
+	u32 drift_adjust;
+	u32 drift_adjust_rate;
+};
+
+#define tod_csroffs(a)	(offsetof(struct altera_eth_tod, a))
+
+struct altera_ptp_private {
+	struct net_device *dev;
+
+	struct ptp_clock_info ptp_clock_ops;
+	struct ptp_clock *ptp_clock;
+
+	/* Time-of-Day (ToD) Clock address space */
+	struct altera_eth_tod __iomem *tod_ctrl;
+	struct clk *tod_clk;
+
+	/* Ability to turn on and turn off PTP offset correction and
+	 * frequency correction
+	 */
+	u32 ptp_correct_offs;
+	u32 ptp_correct_freq;
+
+	/* ToD clock registers protection */
+	spinlock_t tod_lock;
+
+	/* Debug for PTP interface with mutex for passing
+	 * scaled_ppm from adjfine to debugfs
+	 */
+	struct mutex ppm_mutex;
+	long scaled_ppm;
+};
+
+struct altera_timestamp {
+	u32 tstamp[3];
+};
+
+void altera_tx_hwtstamp(struct altera_ptp_private *priv, struct sk_buff *skb,
+			struct altera_timestamp *tstamp);
+void altera_rx_hwtstamp(struct altera_ptp_private *priv, struct sk_buff *skb,
+			struct altera_timestamp *tstamp);
+int altera_init_ptp(struct altera_ptp_private *priv);
+void altera_uninit_ptp(struct altera_ptp_private *priv);
+int altera_ptp_register(struct altera_ptp_private *priv, struct device *device);
+void altera_ptp_unregister(struct altera_ptp_private *priv);
+int altera_ptp_probe(struct platform_device *pdev,
+		     struct altera_ptp_private *priv);
+int altera_ptp_dbg_init(struct altera_ptp_private *priv,
+			struct dentry *dfs_dir);
+
+#endif /* __ALTERA_PTP_H__ */
diff --git a/drivers/net/ethernet/altera/altera_tse.h b/drivers/net/ethernet/altera/altera_tse.h
index f435fb0eca90..508d5d015ca6 100644
--- a/drivers/net/ethernet/altera/altera_tse.h
+++ b/drivers/net/ethernet/altera/altera_tse.h
@@ -39,6 +39,8 @@
 #include <linux/netdevice.h>
 #include <linux/phy.h>
 
+#include "altera_ptp.h"
+
 #define ALTERA_TSE_SW_RESET_WATCHDOG_CNTR	10000
 #define ALTERA_TSE_MAC_FIFO_WIDTH		4	/* TX/RX FIFO width in
 							 * bytes
@@ -428,6 +430,12 @@ struct altera_tse_private {
 	/* TSE Revision */
 	u32	revision;
 
+	/* Shared PTP structure */
+	struct altera_ptp_private ptp_priv;
+	int hwts_tx_en;
+	int hwts_rx_en;
+	u32 ptp_enable;
+
 	/* mSGDMA Rx Dispatcher address space */
 	void __iomem *rx_dma_csr;
 	void __iomem *rx_dma_desc;
@@ -494,6 +502,8 @@ struct altera_tse_private {
 	u32 msg_enable;
 
 	struct altera_dmaops *dmaops;
+
+	struct dentry *dfs_dir;
 };
 
 /* Function prototypes
diff --git a/drivers/net/ethernet/altera/altera_tse_ethtool.c b/drivers/net/ethernet/altera/altera_tse_ethtool.c
index 2998655ab316..eeb6e5558c6a 100644
--- a/drivers/net/ethernet/altera/altera_tse_ethtool.c
+++ b/drivers/net/ethernet/altera/altera_tse_ethtool.c
@@ -30,6 +30,7 @@
 #include <linux/ethtool.h>
 #include <linux/kernel.h>
 #include <linux/netdevice.h>
+#include <linux/net_tstamp.h>
 #include <linux/phy.h>
 
 #include "altera_tse.h"
@@ -234,6 +235,32 @@ static void tse_get_regs(struct net_device *dev, struct ethtool_regs *regs,
 		buf[i] = csrrd32(priv->mac_dev, i * 4);
 }
 
+static int tse_get_ts_info(struct net_device *dev,
+			   struct ethtool_ts_info *info)
+{
+	struct altera_tse_private *priv = netdev_priv(dev);
+
+	if (priv->ptp_enable) {
+		if (priv->ptp_priv.ptp_clock)
+			info->phc_index =
+				ptp_clock_index(priv->ptp_priv.ptp_clock);
+
+		info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
+					SOF_TIMESTAMPING_RX_HARDWARE |
+					SOF_TIMESTAMPING_RAW_HARDWARE;
+
+		info->tx_types = (1 << HWTSTAMP_TX_OFF) |
+						 (1 << HWTSTAMP_TX_ON);
+
+		info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
+						   (1 << HWTSTAMP_FILTER_ALL);
+
+		return 0;
+	} else {
+		return ethtool_op_get_ts_info(dev, info);
+	}
+}
+
 static const struct ethtool_ops tse_ethtool_ops = {
 	.get_drvinfo = tse_get_drvinfo,
 	.get_regs_len = tse_reglen,
@@ -246,6 +273,7 @@ static const struct ethtool_ops tse_ethtool_ops = {
 	.set_msglevel = tse_set_msglevel,
 	.get_link_ksettings = phy_ethtool_get_link_ksettings,
 	.set_link_ksettings = phy_ethtool_set_link_ksettings,
+	.get_ts_info = tse_get_ts_info,
 };
 
 void altera_tse_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/altera/altera_tse_main.c b/drivers/net/ethernet/altera/altera_tse_main.c
index b25d03506470..3b0c940189be 100644
--- a/drivers/net/ethernet/altera/altera_tse_main.c
+++ b/drivers/net/ethernet/altera/altera_tse_main.c
@@ -29,14 +29,18 @@
  */
 
 #include <linux/atomic.h>
+#include <linux/clk.h>
+#include <linux/debugfs.h>
 #include <linux/delay.h>
 #include <linux/etherdevice.h>
+#include <linux/if_ether.h>
 #include <linux/if_vlan.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/mii.h>
+#include <linux/net_tstamp.h>
 #include <linux/netdevice.h>
 #include <linux/of_device.h>
 #include <linux/of_mdio.h>
@@ -51,6 +55,7 @@
 #include "altera_tse.h"
 #include "altera_sgdma.h"
 #include "altera_msgdma.h"
+#include "altera_ptp.h"
 
 static atomic_t instance_count = ATOMIC_INIT(~0);
 /* Module parameters */
@@ -609,7 +614,14 @@ static int tse_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (ret)
 		goto out;
 
-	skb_tx_timestamp(skb);
+	/* Provide a hardware time stamp if requested.
+	 */
+	if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
+		     priv->hwts_tx_en))
+		/* declare that device is doing timestamping */
+		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+	else
+		skb_tx_timestamp(skb);
 
 	priv->tx_prod++;
 	dev->stats.tx_bytes += skb->len;
@@ -1250,6 +1262,13 @@ static int tse_open(struct net_device *dev)
 	if (dev->phydev)
 		phy_start(dev->phydev);
 
+	ret = altera_init_ptp(&priv->ptp_priv);
+	if (ret)
+		netdev_warn(dev, "Failed PTP initialization\n");
+
+	priv->hwts_tx_en = 0;
+	priv->hwts_rx_en = 0;
+
 	napi_enable(&priv->napi);
 	netif_start_queue(dev);
 
@@ -1321,6 +1340,83 @@ static int tse_shutdown(struct net_device *dev)
 	return 0;
 }
 
+/* ioctl to configure timestamping */
+static int tse_do_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	struct altera_tse_private *priv = netdev_priv(dev);
+	struct hwtstamp_config config;
+
+	if (!netif_running(dev))
+		return -EINVAL;
+
+	if (!priv->ptp_enable)	{
+		netdev_alert(priv->dev, "Timestamping not supported");
+		return -EOPNOTSUPP;
+	}
+
+	if (cmd == SIOCSHWTSTAMP) {
+		if (copy_from_user(&config, ifr->ifr_data,
+				   sizeof(struct hwtstamp_config)))
+			return -EFAULT;
+
+		if (config.flags)
+			return -EINVAL;
+
+		switch (config.tx_type) {
+		case HWTSTAMP_TX_OFF:
+			priv->hwts_tx_en = 0;
+			break;
+		case HWTSTAMP_TX_ON:
+			priv->hwts_tx_en = 1;
+			break;
+		default:
+			return -ERANGE;
+		}
+
+		switch (config.rx_filter) {
+		case HWTSTAMP_FILTER_NONE:
+			priv->hwts_rx_en = 0;
+			config.rx_filter = HWTSTAMP_FILTER_NONE;
+			break;
+		default:
+			priv->hwts_rx_en = 1;
+			config.rx_filter = HWTSTAMP_FILTER_ALL;
+			break;
+		}
+
+		if (copy_to_user(ifr->ifr_data, &config,
+				 sizeof(struct hwtstamp_config)))
+			return -EFAULT;
+		else
+			return 0;
+	}
+
+	if (cmd == SIOCGHWTSTAMP) {
+		config.flags = 0;
+
+		if (priv->hwts_tx_en)
+			config.tx_type = HWTSTAMP_TX_ON;
+		else
+			config.tx_type = HWTSTAMP_TX_OFF;
+
+		if (priv->hwts_rx_en)
+			config.rx_filter = HWTSTAMP_FILTER_ALL;
+		else
+			config.rx_filter = HWTSTAMP_FILTER_NONE;
+
+		if (copy_to_user(ifr->ifr_data, &config,
+				 sizeof(struct hwtstamp_config)))
+			return -EFAULT;
+		else
+			return 0;
+	}
+
+	if (!dev->phydev)
+		return -EINVAL;
+
+	return phy_mii_ioctl(dev->phydev, ifr, cmd);
+}
+
 static struct net_device_ops altera_tse_netdev_ops = {
 	.ndo_open		= tse_open,
 	.ndo_stop		= tse_shutdown,
@@ -1329,8 +1425,44 @@ static struct net_device_ops altera_tse_netdev_ops = {
 	.ndo_set_rx_mode	= tse_set_rx_mode,
 	.ndo_change_mtu		= tse_change_mtu,
 	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_do_ioctl		= tse_do_ioctl,
 };
 
+/* Debugfs entries */
+int altera_tse_dbg_init(struct net_device *dev)
+{
+	struct altera_tse_private *priv = netdev_priv(dev);
+	char *buf;
+
+	buf = kasprintf(GFP_KERNEL, "%s_%s",
+			ALTERA_TSE_RESOURCE_NAME, dev->name);
+
+	priv->dfs_dir = debugfs_create_dir(buf, NULL);
+	if (!priv->dfs_dir) {
+		netdev_err(dev, "debugfs create directory failed\n");
+		goto err_dfs;
+	}
+
+	if (priv->ptp_enable)
+		if (altera_ptp_dbg_init(&priv->ptp_priv, priv->dfs_dir))
+			goto err_entry;
+
+	return 0;
+
+err_entry:
+	debugfs_remove_recursive(priv->dfs_dir);
+	priv->dfs_dir = NULL;
+err_dfs:
+	return -ENOMEM;
+}
+
+static void altera_tse_dbg_exit(struct net_device *dev)
+{
+	struct altera_tse_private *priv = netdev_priv(dev);
+
+	debugfs_remove_recursive(priv->dfs_dir);
+}
+
 /* Probe Altera TSE MAC device
  */
 static int altera_tse_probe(struct platform_device *pdev)
@@ -1580,6 +1712,31 @@ static int altera_tse_probe(struct platform_device *pdev)
 		netdev_err(ndev, "Cannot attach to PHY (error: %d)\n", ret);
 		goto err_init_phy;
 	}
+
+	priv->ptp_enable = of_property_read_bool(pdev->dev.of_node,
+						 "altr,has-ptp");
+	dev_info(&pdev->dev, "PTP Enable: %d\n", priv->ptp_enable);
+
+	if (priv->ptp_enable) {
+		/* MAP PTP */
+		ret = altera_ptp_probe(pdev, &priv->ptp_priv);
+		if (ret) {
+			dev_err(&pdev->dev, "cannot map PTP\n");
+			goto err_init_phy;
+		}
+		ret = altera_ptp_register(&priv->ptp_priv,
+					  priv->device);
+		if (ret) {
+			dev_err(&pdev->dev, "Failed to register PTP clock\n");
+			ret = -ENXIO;
+			goto err_init_phy;
+		}
+	}
+
+#ifdef CONFIG_DEBUG_FS
+	if (altera_tse_dbg_init(ndev))
+		goto err_init_phy;
+#endif
 	return 0;
 
 err_init_phy:
@@ -1606,7 +1763,12 @@ static int altera_tse_remove(struct platform_device *pdev)
 			of_phy_deregister_fixed_link(priv->device->of_node);
 	}
 
+#ifdef CONFIG_DEBUG_FS
+	altera_tse_dbg_exit(ndev);
+#endif
 	platform_set_drvdata(pdev, NULL);
+	if (priv->ptp_enable)
+		altera_ptp_unregister(&priv->ptp_priv);
 	altera_tse_mdio_destroy(ndev);
 	unregister_netdev(ndev);
 	free_netdev(ndev);
-- 
2.19.1

Powered by blists - more mailing lists