net: dpaa2: move DPAA2 PTP driver out of staging/

This patch is to move DPAA2 PTP driver out of staging/
since the dpaa2-eth had been moved out.

Signed-off-by: Yangbo Lu <yangbo.lu@nxp.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

1 files changed, 229 insertions, 0 deletions

diff --git a/drivers/net/ethernet/freescale/dpaa2/dpaa2-ptp.c b/drivers/net/ethernet/freescale/dpaa2/dpaa2-ptp.c
new file mode 100644
index 000000000000..2e90d5a3d920
--- /dev/null
+++ b/drivers/net/ethernet/freescale/dpaa2/dpaa2-ptp.c
@@ -0,0 +1,229 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright 2013-2016 Freescale Semiconductor Inc.
+ * Copyright 2016-2018 NXP
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/fsl/mc.h>
+
+#include "dpaa2-ptp.h"
+
+struct ptp_dpaa2_priv {
+	struct fsl_mc_device *rtc_mc_dev;
+	struct ptp_clock *clock;
+	struct ptp_clock_info caps;
+	u32 freq_comp;
+};
+
+/* PTP clock operations */
+static int ptp_dpaa2_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+	struct ptp_dpaa2_priv *ptp_dpaa2 =
+		container_of(ptp, struct ptp_dpaa2_priv, caps);
+	struct fsl_mc_device *mc_dev = ptp_dpaa2->rtc_mc_dev;
+	struct device *dev = &mc_dev->dev;
+	u64 adj;
+	u32 diff, tmr_add;
+	int neg_adj = 0;
+	int err = 0;
+
+	if (ppb < 0) {
+		neg_adj = 1;
+		ppb = -ppb;
+	}
+
+	tmr_add = ptp_dpaa2->freq_comp;
+	adj = tmr_add;
+	adj *= ppb;
+	diff = div_u64(adj, 1000000000ULL);
+
+	tmr_add = neg_adj ? tmr_add - diff : tmr_add + diff;
+
+	err = dprtc_set_freq_compensation(mc_dev->mc_io, 0,
+					  mc_dev->mc_handle, tmr_add);
+	if (err)
+		dev_err(dev, "dprtc_set_freq_compensation err %d\n", err);
+	return 0;
+}
+
+static int ptp_dpaa2_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct ptp_dpaa2_priv *ptp_dpaa2 =
+		container_of(ptp, struct ptp_dpaa2_priv, caps);
+	struct fsl_mc_device *mc_dev = ptp_dpaa2->rtc_mc_dev;
+	struct device *dev = &mc_dev->dev;
+	s64 now;
+	int err = 0;
+
+	err = dprtc_get_time(mc_dev->mc_io, 0, mc_dev->mc_handle, &now);
+	if (err) {
+		dev_err(dev, "dprtc_get_time err %d\n", err);
+		return 0;
+	}
+
+	now += delta;
+
+	err = dprtc_set_time(mc_dev->mc_io, 0, mc_dev->mc_handle, now);
+	if (err) {
+		dev_err(dev, "dprtc_set_time err %d\n", err);
+		return 0;
+	}
+	return 0;
+}
+
+static int ptp_dpaa2_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	struct ptp_dpaa2_priv *ptp_dpaa2 =
+		container_of(ptp, struct ptp_dpaa2_priv, caps);
+	struct fsl_mc_device *mc_dev = ptp_dpaa2->rtc_mc_dev;
+	struct device *dev = &mc_dev->dev;
+	u64 ns;
+	u32 remainder;
+	int err = 0;
+
+	err = dprtc_get_time(mc_dev->mc_io, 0, mc_dev->mc_handle, &ns);
+	if (err) {
+		dev_err(dev, "dprtc_get_time err %d\n", err);
+		return 0;
+	}
+
+	ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
+	ts->tv_nsec = remainder;
+	return 0;
+}
+
+static int ptp_dpaa2_settime(struct ptp_clock_info *ptp,
+			     const struct timespec64 *ts)
+{
+	struct ptp_dpaa2_priv *ptp_dpaa2 =
+		container_of(ptp, struct ptp_dpaa2_priv, caps);
+	struct fsl_mc_device *mc_dev = ptp_dpaa2->rtc_mc_dev;
+	struct device *dev = &mc_dev->dev;
+	u64 ns;
+	int err = 0;
+
+	ns = ts->tv_sec * 1000000000ULL;
+	ns += ts->tv_nsec;
+
+	err = dprtc_set_time(mc_dev->mc_io, 0, mc_dev->mc_handle, ns);
+	if (err)
+		dev_err(dev, "dprtc_set_time err %d\n", err);
+	return 0;
+}
+
+static struct ptp_clock_info ptp_dpaa2_caps = {
+	.owner		= THIS_MODULE,
+	.name		= "DPAA2 PTP Clock",
+	.max_adj	= 512000,
+	.n_alarm	= 2,
+	.n_ext_ts	= 2,
+	.n_per_out	= 3,
+	.n_pins		= 0,
+	.pps		= 1,
+	.adjfreq	= ptp_dpaa2_adjfreq,
+	.adjtime	= ptp_dpaa2_adjtime,
+	.gettime64	= ptp_dpaa2_gettime,
+	.settime64	= ptp_dpaa2_settime,
+};
+
+static int rtc_probe(struct fsl_mc_device *mc_dev)
+{
+	struct device *dev = &mc_dev->dev;
+	struct ptp_dpaa2_priv *ptp_dpaa2;
+	u32 tmr_add = 0;
+	int err;
+
+	ptp_dpaa2 = kzalloc(sizeof(*ptp_dpaa2), GFP_KERNEL);
+	if (!ptp_dpaa2)
+		return -ENOMEM;
+
+	err = fsl_mc_portal_allocate(mc_dev, 0, &mc_dev->mc_io);
+	if (err) {
+		dev_err(dev, "fsl_mc_portal_allocate err %d\n", err);
+		goto err_exit;
+	}
+
+	err = dprtc_open(mc_dev->mc_io, 0, mc_dev->obj_desc.id,
+			 &mc_dev->mc_handle);
+	if (err) {
+		dev_err(dev, "dprtc_open err %d\n", err);
+		goto err_free_mcp;
+	}
+
+	ptp_dpaa2->rtc_mc_dev = mc_dev;
+
+	err = dprtc_get_freq_compensation(mc_dev->mc_io, 0,
+					  mc_dev->mc_handle, &tmr_add);
+	if (err) {
+		dev_err(dev, "dprtc_get_freq_compensation err %d\n", err);
+		goto err_close;
+	}
+
+	ptp_dpaa2->freq_comp = tmr_add;
+	ptp_dpaa2->caps = ptp_dpaa2_caps;
+
+	ptp_dpaa2->clock = ptp_clock_register(&ptp_dpaa2->caps, dev);
+	if (IS_ERR(ptp_dpaa2->clock)) {
+		err = PTR_ERR(ptp_dpaa2->clock);
+		goto err_close;
+	}
+
+	dpaa2_phc_index = ptp_clock_index(ptp_dpaa2->clock);
+
+	dev_set_drvdata(dev, ptp_dpaa2);
+
+	return 0;
+
+err_close:
+	dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
+err_free_mcp:
+	fsl_mc_portal_free(mc_dev->mc_io);
+err_exit:
+	kfree(ptp_dpaa2);
+	dev_set_drvdata(dev, NULL);
+	return err;
+}
+
+static int rtc_remove(struct fsl_mc_device *mc_dev)
+{
+	struct ptp_dpaa2_priv *ptp_dpaa2;
+	struct device *dev = &mc_dev->dev;
+
+	ptp_dpaa2 = dev_get_drvdata(dev);
+	ptp_clock_unregister(ptp_dpaa2->clock);
+
+	dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
+	fsl_mc_portal_free(mc_dev->mc_io);
+
+	kfree(ptp_dpaa2);
+	dev_set_drvdata(dev, NULL);
+
+	return 0;
+}
+
+static const struct fsl_mc_device_id rtc_match_id_table[] = {
+	{
+		.vendor = FSL_MC_VENDOR_FREESCALE,
+		.obj_type = "dprtc",
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(fslmc, rtc_match_id_table);
+
+static struct fsl_mc_driver rtc_drv = {
+	.driver = {
+		.name = KBUILD_MODNAME,
+		.owner = THIS_MODULE,
+	},
+	.probe = rtc_probe,
+	.remove = rtc_remove,
+	.match_id_table = rtc_match_id_table,
+};
+
+module_fsl_mc_driver(rtc_drv);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("DPAA2 PTP Clock Driver");