Resguardo inicial

linux-7.2.3/drivers/ptp/Kconfig

@@ -0,0 +1,266 @@
+# SPDX-License-Identifier: GPL-2.0-only
+#
+# PTP clock support configuration
+#
+
+menu "PTP clock support"
+
+config PTP_1588_CLOCK
+	tristate "PTP clock support"
+	depends on NET && POSIX_TIMERS
+	default ETHERNET
+	select PPS
+	select NET_PTP_CLASSIFY
+	help
+	  The IEEE 1588 standard defines a method to precisely
+	  synchronize distributed clocks over Ethernet networks. The
+	  standard defines a Precision Time Protocol (PTP), which can
+	  be used to achieve synchronization within a few dozen
+	  microseconds. In addition, with the help of special hardware
+	  time stamping units, it can be possible to achieve
+	  synchronization to within a few hundred nanoseconds.
+
+	  This driver adds support for PTP clocks as character
+	  devices. If you want to use a PTP clock, then you should
+	  also enable at least one clock driver as well.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp.
+
+config PTP_1588_CLOCK_OPTIONAL
+	tristate
+	default y if PTP_1588_CLOCK=n
+	default PTP_1588_CLOCK
+	help
+	  Drivers that can optionally use the PTP_1588_CLOCK framework
+	  should depend on this symbol to prevent them from being built
+	  into vmlinux while the PTP support itself is in a loadable
+	  module.
+	  If PTP support is disabled, this dependency will still be
+	  met, and drivers refer to dummy helpers.
+
+config PTP_1588_CLOCK_DTE
+	tristate "Broadcom DTE as PTP clock"
+	depends on PTP_1588_CLOCK
+	depends on NET && HAS_IOMEM
+	depends on ARCH_BCM_MOBILE || (ARCH_BCM_IPROC && !(ARCH_BCM_NSP || ARCH_BCM_5301X)) || COMPILE_TEST
+	default y if ARCH_BCM_MOBILE || ARCH_BCM_IPROC
+	help
+	  This driver adds support for using the Digital timing engine
+	  (DTE) in the Broadcom SoC's as a PTP clock.
+
+	  The clock can be used in both wired and wireless networks
+	  for PTP purposes.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_dte.
+
+config PTP_1588_CLOCK_QORIQ
+	tristate "Freescale QorIQ 1588 timer as PTP clock"
+	depends on GIANFAR || FSL_DPAA_ETH || FSL_DPAA2_ETH || FSL_ENETC || FSL_ENETC_VF || COMPILE_TEST
+	depends on PTP_1588_CLOCK
+	default y if GIANFAR || FSL_DPAA_ETH || FSL_DPAA2_ETH || FSL_ENETC || FSL_ENETC_VF
+	help
+	  This driver adds support for using the Freescale QorIQ 1588
+	  timer as a PTP clock. This clock is only useful if your PTP
+	  programs are getting hardware time stamps on the PTP Ethernet
+	  packets using the SO_TIMESTAMPING API.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_qoriq.
+
+comment "Enable PHYLIB and NETWORK_PHY_TIMESTAMPING to see the additional clocks."
+	depends on PHYLIB=n || NETWORK_PHY_TIMESTAMPING=n
+
+config DP83640_PHY
+	tristate "Driver for the National Semiconductor DP83640 PHYTER"
+	depends on NETWORK_PHY_TIMESTAMPING
+	depends on PHYLIB
+	depends on PTP_1588_CLOCK
+	select CRC32
+	help
+	  Supports the DP83640 PHYTER with IEEE 1588 features.
+
+	  This driver adds support for using the DP83640 as a PTP
+	  clock. This clock is only useful if your PTP programs are
+	  getting hardware time stamps on the PTP Ethernet packets
+	  using the SO_TIMESTAMPING API.
+
+	  In order for this to work, your MAC driver must also
+	  implement the skb_tx_timestamp() function.
+
+config PTP_1588_CLOCK_INES
+	tristate "ZHAW InES PTP time stamping IP core"
+	depends on NETWORK_PHY_TIMESTAMPING
+	depends on HAS_IOMEM
+	depends on PHYLIB
+	depends on PTP_1588_CLOCK
+	help
+	  This driver adds support for using the ZHAW InES 1588 IP
+	  core.  This clock is only useful if the MII bus of your MAC
+	  is wired up to the core.
+
+config PTP_1588_CLOCK_PCH
+	tristate "Intel PCH EG20T as PTP clock"
+	depends on MIPS_GENERIC || X86_32 || COMPILE_TEST
+	depends on HAS_IOMEM && PCI
+	depends on NET
+	depends on PTP_1588_CLOCK
+	help
+	  This driver adds support for using the PCH EG20T as a PTP
+	  clock. The hardware supports time stamping of PTP packets
+	  when using the end-to-end delay (E2E) mechanism. The peer
+	  delay mechanism (P2P) is not supported.
+
+	  This clock is only useful if your PTP programs are getting
+	  hardware time stamps on the PTP Ethernet packets using the
+	  SO_TIMESTAMPING API.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_pch.
+
+config PTP_1588_CLOCK_KVM
+	tristate "KVM virtual PTP clock"
+	depends on PTP_1588_CLOCK
+	depends on (KVM_GUEST && X86) || (HAVE_ARM_SMCCC_DISCOVERY && ARM_ARCH_TIMER)
+	default y
+	help
+	  This driver adds support for using kvm infrastructure as a PTP
+	  clock. This clock is only useful if you are using KVM guests.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_kvm.
+
+config PTP_1588_CLOCK_VMCLOCK
+	tristate "Virtual machine PTP clock"
+	depends on X86_TSC || ARM_ARCH_TIMER
+	depends on PTP_1588_CLOCK && ARCH_SUPPORTS_INT128
+	default PTP_1588_CLOCK_KVM
+	help
+	  This driver adds support for using a virtual precision clock
+	  advertised by the hypervisor. This clock is only useful in virtual
+	  machines where such a device is present.
+
+	  Unlike the KVM virtual PTP clock, the VMCLOCK device offers support
+	  for reliable timekeeping even across live migration. So this driver
+	  is enabled by default whenever the KVM PTP clock is.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_vmclock.
+
+config PTP_1588_CLOCK_IDT82P33
+	tristate "IDT 82P33xxx PTP clock"
+	depends on PTP_1588_CLOCK && I2C
+	default n
+	help
+	  This driver adds support for using the IDT 82P33xxx as a PTP
+	  clock. This clock is only useful if your time stamping MAC
+	  is connected to the IDT chip.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_idt82p33.
+
+config PTP_1588_CLOCK_IDTCM
+	tristate "IDT CLOCKMATRIX as PTP clock"
+	depends on PTP_1588_CLOCK && I2C
+	default n
+	help
+	  This driver adds support for using IDT CLOCKMATRIX(TM) as a PTP
+	  clock. This clock is only useful if your time stamping MAC
+	  is connected to the IDT chip.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_clockmatrix.
+
+config PTP_1588_CLOCK_FC3W
+	tristate "RENESAS FemtoClock3 Wireless as PTP clock"
+	depends on PTP_1588_CLOCK && I2C
+	default n
+	help
+	  This driver adds support for using Renesas FemtoClock3 Wireless
+	  as a PTP clock. This clock is only useful if your time stamping
+	  MAC is connected to the RENESAS chip.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_fc3.
+
+config PTP_1588_CLOCK_MOCK
+	tristate "Mock-up PTP clock"
+	depends on PTP_1588_CLOCK
+	help
+	  This driver offers a set of PTP clock manipulation operations over
+	  the system monotonic time. It can be used by virtual network device
+	  drivers to emulate PTP capabilities.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_mock.
+
+config PTP_1588_CLOCK_VMW
+	tristate "VMware virtual PTP clock"
+	depends on ACPI && HYPERVISOR_GUEST && X86
+	depends on PTP_1588_CLOCK
+	help
+	  This driver adds support for using VMware virtual precision
+	  clock device as a PTP clock. This is only useful in virtual
+	  machines running on VMware virtual infrastructure.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_vmw.
+
+config PTP_1588_CLOCK_OCP
+	tristate "OpenCompute TimeCard as PTP clock"
+	depends on PTP_1588_CLOCK
+	depends on HAS_IOMEM && PCI
+	depends on I2C && MTD
+	depends on SERIAL_8250
+	depends on !S390
+	depends on COMMON_CLK
+	select NET_DEVLINK
+	select CRC16
+	select DPLL
+	help
+	  This driver adds support for an OpenCompute time card.
+
+	  The OpenCompute time card is an atomic clock along with
+	  a GPS receiver that provides a Grandmaster clock source
+	  for a PTP enabled network.
+
+	  More information is available at http://www.timingcard.com/
+
+config PTP_DFL_TOD
+	tristate "FPGA DFL ToD Driver"
+	depends on FPGA_DFL
+	depends on PTP_1588_CLOCK
+	help
+	  The DFL (Device Feature List) device driver for the Intel ToD
+	  (Time-of-Day) device in FPGA card. The ToD IP within the FPGA
+	  is exposed as PTP Hardware Clock (PHC) device to the Linux PTP
+	  stack to synchronize the system clock to its ToD information
+	  using phc2sys utility of the Linux PTP stack.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called ptp_dfl_tod.
+
+config PTP_S390
+	tristate "S390 PTP driver"
+	depends on PTP_1588_CLOCK
+	depends on S390
+	help
+	  This driver adds support for S390 time steering via the PtP
+	  interface. This works by adding a in-kernel clock delta value,
+	  which is always added to time values used in the kernel. The PtP
+	  driver provides the raw clock value without the delta to
+	  userspace. That way userspace programs like chrony could steer
+	  the kernel clock.
+
+config PTP_NETC_V4_TIMER
+	tristate "NXP NETC V4 Timer PTP Driver"
+	depends on PTP_1588_CLOCK
+	depends on PCI_MSI
+	help
+	  This driver adds support for using the NXP NETC V4 Timer as a PTP
+	  clock, the clock is used by ENETC V4 or NETC V4 Switch for PTP time
+	  synchronization. It also supports periodic output signal (e.g. PPS)
+	  and external trigger timestamping.
+
+endmenu

linux-7.2.3/drivers/ptp/Makefile

@@ -0,0 +1,24 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for PTP 1588 clock support.
+#
+
+ptp-y					:= ptp_clock.o ptp_chardev.o ptp_sysfs.o ptp_vclock.o
+ptp_kvm-$(CONFIG_X86)			:= ptp_kvm_x86.o ptp_kvm_common.o
+ptp_kvm-$(CONFIG_HAVE_ARM_SMCCC)	:= ptp_kvm_arm.o ptp_kvm_common.o
+obj-$(CONFIG_PTP_1588_CLOCK)		+= ptp.o
+obj-$(CONFIG_PTP_1588_CLOCK_DTE)	+= ptp_dte.o
+obj-$(CONFIG_PTP_1588_CLOCK_INES)	+= ptp_ines.o
+obj-$(CONFIG_PTP_1588_CLOCK_PCH)	+= ptp_pch.o
+obj-$(CONFIG_PTP_1588_CLOCK_KVM)	+= ptp_kvm.o
+obj-$(CONFIG_PTP_1588_CLOCK_VMCLOCK)	+= ptp_vmclock.o
+obj-$(CONFIG_PTP_1588_CLOCK_QORIQ)	+= ptp_qoriq.o
+obj-$(CONFIG_PTP_1588_CLOCK_IDTCM)	+= ptp_clockmatrix.o
+obj-$(CONFIG_PTP_1588_CLOCK_FC3W)	+= ptp_fc3.o
+obj-$(CONFIG_PTP_1588_CLOCK_IDT82P33)	+= ptp_idt82p33.o
+obj-$(CONFIG_PTP_1588_CLOCK_MOCK)	+= ptp_mock.o
+obj-$(CONFIG_PTP_1588_CLOCK_VMW)	+= ptp_vmw.o
+obj-$(CONFIG_PTP_1588_CLOCK_OCP)	+= ptp_ocp.o
+obj-$(CONFIG_PTP_DFL_TOD)		+= ptp_dfl_tod.o
+obj-$(CONFIG_PTP_S390)			+= ptp_s390.o
+obj-$(CONFIG_PTP_NETC_V4_TIMER)		+= ptp_netc.o

linux-7.2.3/drivers/ptp/ptp_chardev.c

@@ -0,0 +1,646 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * PTP 1588 clock support - character device implementation.
+ *
+ * Copyright (C) 2010 OMICRON electronics GmbH
+ */
+#include <linux/compat.h>
+#include <linux/module.h>
+#include <linux/posix-clock.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/timekeeping.h>
+#include <linux/debugfs.h>
+
+#include <linux/nospec.h>
+
+#include "ptp_private.h"
+
+static int ptp_disable_pinfunc(struct ptp_clock_info *ops,
+			       enum ptp_pin_function func, unsigned int chan)
+{
+	struct ptp_clock_request rq;
+	int err = 0;
+
+	memset(&rq, 0, sizeof(rq));
+
+	switch (func) {
+	case PTP_PF_NONE:
+		break;
+	case PTP_PF_EXTTS:
+		rq.type = PTP_CLK_REQ_EXTTS;
+		rq.extts.index = chan;
+		err = ops->enable(ops, &rq, 0);
+		break;
+	case PTP_PF_PEROUT:
+		rq.type = PTP_CLK_REQ_PEROUT;
+		rq.perout.index = chan;
+		err = ops->enable(ops, &rq, 0);
+		break;
+	case PTP_PF_PHYSYNC:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return err;
+}
+
+void ptp_disable_all_events(struct ptp_clock *ptp)
+{
+	struct ptp_clock_info *info = ptp->info;
+	unsigned int i;
+
+	mutex_lock(&ptp->pincfg_mux);
+	/* Disable any pins that may raise EXTTS events */
+	for (i = 0; i < info->n_pins; i++)
+		if (info->pin_config[i].func == PTP_PF_EXTTS)
+			ptp_disable_pinfunc(info, info->pin_config[i].func,
+					    info->pin_config[i].chan);
+
+	/* Disable the PPS event if the driver has PPS support */
+	if (info->pps) {
+		struct ptp_clock_request req = { .type = PTP_CLK_REQ_PPS };
+		info->enable(info, &req, 0);
+	}
+	mutex_unlock(&ptp->pincfg_mux);
+}
+
+int ptp_set_pinfunc(struct ptp_clock *ptp, unsigned int pin,
+		    enum ptp_pin_function func, unsigned int chan)
+{
+	struct ptp_clock_info *info = ptp->info;
+	struct ptp_pin_desc *pin1 = NULL, *pin2 = &info->pin_config[pin];
+	unsigned int i;
+
+	/* Check to see if any other pin previously had this function. */
+	for (i = 0; i < info->n_pins; i++) {
+		if (info->pin_config[i].func == func &&
+		    info->pin_config[i].chan == chan) {
+			pin1 = &info->pin_config[i];
+			break;
+		}
+	}
+	if (pin1 && i == pin)
+		return 0;
+
+	/* Check the desired function and channel. */
+	switch (func) {
+	case PTP_PF_NONE:
+		break;
+	case PTP_PF_EXTTS:
+		if (chan >= info->n_ext_ts)
+			return -EINVAL;
+		break;
+	case PTP_PF_PEROUT:
+		if (chan >= info->n_per_out)
+			return -EINVAL;
+		break;
+	case PTP_PF_PHYSYNC:
+		if (chan != 0)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (info->verify(info, pin, func, chan)) {
+		pr_err("driver cannot use function %u and channel %u on pin %u\n",
+		       func, chan, pin);
+		return -EOPNOTSUPP;
+	}
+
+	/* Disable whichever pin was previously assigned to this function and
+	 * channel.
+	 */
+	if (pin1) {
+		ptp_disable_pinfunc(info, func, chan);
+		pin1->func = PTP_PF_NONE;
+		pin1->chan = 0;
+	}
+
+	/* Disable whatever function was previously assigned to the requested
+	 * pin.
+	 */
+	ptp_disable_pinfunc(info, pin2->func, pin2->chan);
+	pin2->func = func;
+	pin2->chan = chan;
+
+	return 0;
+}
+
+int ptp_open(struct posix_clock_context *pccontext, fmode_t fmode)
+{
+	struct ptp_clock *ptp = container_of(pccontext->clk, struct ptp_clock, clock);
+	struct timestamp_event_queue *queue;
+	char debugfsname[32];
+
+	queue = kzalloc(sizeof(*queue), GFP_KERNEL);
+	if (!queue)
+		return -EINVAL;
+	queue->mask = bitmap_alloc(PTP_MAX_CHANNELS, GFP_KERNEL);
+	if (!queue->mask) {
+		kfree(queue);
+		return -EINVAL;
+	}
+	bitmap_set(queue->mask, 0, PTP_MAX_CHANNELS);
+	spin_lock_init(&queue->lock);
+	scoped_guard(spinlock_irq, &ptp->tsevqs_lock)
+		list_add_tail(&queue->qlist, &ptp->tsevqs);
+	pccontext->private_clkdata = queue;
+
+	/* Debugfs contents */
+	sprintf(debugfsname, "0x%p", queue);
+	queue->debugfs_instance =
+		debugfs_create_dir(debugfsname, ptp->debugfs_root);
+	queue->dfs_bitmap.array = (u32 *)queue->mask;
+	queue->dfs_bitmap.n_elements =
+		DIV_ROUND_UP(PTP_MAX_CHANNELS, BITS_PER_BYTE * sizeof(u32));
+	debugfs_create_u32_array("mask", 0444, queue->debugfs_instance,
+				 &queue->dfs_bitmap);
+
+	return 0;
+}
+
+int ptp_release(struct posix_clock_context *pccontext)
+{
+	struct timestamp_event_queue *queue = pccontext->private_clkdata;
+	struct ptp_clock *ptp =
+		container_of(pccontext->clk, struct ptp_clock, clock);
+
+	debugfs_remove(queue->debugfs_instance);
+	pccontext->private_clkdata = NULL;
+	scoped_guard(spinlock_irq, &ptp->tsevqs_lock)
+		list_del(&queue->qlist);
+	bitmap_free(queue->mask);
+	kfree(queue);
+	return 0;
+}
+
+static long ptp_clock_getcaps(struct ptp_clock *ptp, void __user *arg)
+{
+	struct ptp_clock_caps caps = {
+		.max_adj		= ptp->info->max_adj,
+		.n_alarm		= ptp->info->n_alarm,
+		.n_ext_ts		= ptp->info->n_ext_ts,
+		.n_per_out		= ptp->info->n_per_out,
+		.pps			= ptp->info->pps,
+		.n_pins			= ptp->info->n_pins,
+		.cross_timestamping	= ptp->info->getcrosststamp != NULL,
+		.adjust_phase		= ptp->info->adjphase != NULL &&
+					  ptp->info->getmaxphase != NULL,
+	};
+
+	if (caps.adjust_phase)
+		caps.max_phase_adj = ptp->info->getmaxphase(ptp->info);
+
+	return copy_to_user(arg, &caps, sizeof(caps)) ? -EFAULT : 0;
+}
+
+static long ptp_extts_request(struct ptp_clock *ptp, unsigned int cmd, void __user *arg)
+{
+	struct ptp_clock_request req = { .type = PTP_CLK_REQ_EXTTS };
+	struct ptp_clock_info *ops = ptp->info;
+	unsigned int supported_extts_flags;
+
+	if (copy_from_user(&req.extts, arg, sizeof(req.extts)))
+		return -EFAULT;
+
+	if (cmd == PTP_EXTTS_REQUEST2) {
+		/* Tell the drivers to check the flags carefully. */
+		req.extts.flags |= PTP_STRICT_FLAGS;
+		/* Make sure no reserved bit is set. */
+		if ((req.extts.flags & ~PTP_EXTTS_VALID_FLAGS) ||
+		    req.extts.rsv[0] || req.extts.rsv[1])
+			return -EINVAL;
+
+		/* Ensure one of the rising/falling edge bits is set. */
+		if ((req.extts.flags & PTP_ENABLE_FEATURE) &&
+		    (req.extts.flags & PTP_EXTTS_EDGES) == 0)
+			return -EINVAL;
+	} else {
+		req.extts.flags &= PTP_EXTTS_V1_VALID_FLAGS;
+		memset(req.extts.rsv, 0, sizeof(req.extts.rsv));
+	}
+
+	if (req.extts.index >= ops->n_ext_ts)
+		return -EINVAL;
+
+	supported_extts_flags = ptp->info->supported_extts_flags;
+	/* The PTP_ENABLE_FEATURE flag is always supported. */
+	supported_extts_flags |= PTP_ENABLE_FEATURE;
+	/* If the driver does not support strictly checking flags, the
+	 * PTP_RISING_EDGE and PTP_FALLING_EDGE flags are merely hints
+	 * which are not enforced.
+	 */
+	if (!(supported_extts_flags & PTP_STRICT_FLAGS))
+		supported_extts_flags |= PTP_EXTTS_EDGES;
+	/* Reject unsupported flags */
+	if (req.extts.flags & ~supported_extts_flags)
+		return -EOPNOTSUPP;
+
+	scoped_cond_guard(mutex_intr, return -ERESTARTSYS, &ptp->pincfg_mux)
+		return ops->enable(ops, &req, req.extts.flags & PTP_ENABLE_FEATURE ? 1 : 0);
+}
+
+static long ptp_perout_request(struct ptp_clock *ptp, unsigned int cmd, void __user *arg)
+{
+	struct ptp_clock_request req = { .type = PTP_CLK_REQ_PEROUT };
+	struct ptp_perout_request *perout = &req.perout;
+	struct ptp_clock_info *ops = ptp->info;
+
+	if (copy_from_user(perout, arg, sizeof(*perout)))
+		return -EFAULT;
+
+	if (cmd == PTP_PEROUT_REQUEST2) {
+		if (perout->flags & ~PTP_PEROUT_VALID_FLAGS)
+			return -EINVAL;
+
+		/*
+		 * The "on" field has undefined meaning if
+		 * PTP_PEROUT_DUTY_CYCLE isn't set, we must still treat it
+		 * as reserved, which must be set to zero.
+		 */
+		if (!(perout->flags & PTP_PEROUT_DUTY_CYCLE) &&
+		    !mem_is_zero(perout->rsv, sizeof(perout->rsv)))
+			return -EINVAL;
+
+		if (perout->flags & PTP_PEROUT_DUTY_CYCLE) {
+			/* The duty cycle must be subunitary. */
+			if (perout->on.sec > perout->period.sec ||
+			    (perout->on.sec == perout->period.sec &&
+			     perout->on.nsec > perout->period.nsec))
+				return -ERANGE;
+		}
+
+		if (perout->flags & PTP_PEROUT_PHASE) {
+			/*
+			 * The phase should be specified modulo the period,
+			 * therefore anything equal or larger than 1 period
+			 * is invalid.
+			 */
+			if (perout->phase.sec > perout->period.sec ||
+			    (perout->phase.sec == perout->period.sec &&
+			     perout->phase.nsec >= perout->period.nsec))
+				return -ERANGE;
+		}
+	} else {
+		perout->flags &= PTP_PEROUT_V1_VALID_FLAGS;
+		memset(perout->rsv, 0, sizeof(perout->rsv));
+	}
+
+	if (perout->index >= ops->n_per_out)
+		return -EINVAL;
+	if (perout->flags & ~ops->supported_perout_flags)
+		return -EOPNOTSUPP;
+
+	scoped_cond_guard(mutex_intr, return -ERESTARTSYS, &ptp->pincfg_mux)
+		return ops->enable(ops, &req, perout->period.sec || perout->period.nsec);
+}
+
+static long ptp_enable_pps(struct ptp_clock *ptp, bool enable)
+{
+	struct ptp_clock_request req = { .type = PTP_CLK_REQ_PPS };
+	struct ptp_clock_info *ops = ptp->info;
+
+	if (!capable(CAP_SYS_TIME))
+		return -EPERM;
+
+	scoped_cond_guard(mutex_intr, return -ERESTARTSYS, &ptp->pincfg_mux)
+		return ops->enable(ops, &req, enable);
+}
+
+typedef int (*ptp_crosststamp_fn)(struct ptp_clock_info *,
+				  struct system_device_crosststamp *);
+
+static long ptp_sys_offset_precise(struct ptp_clock *ptp, void __user *arg,
+				   ptp_crosststamp_fn crosststamp_fn)
+{
+	struct system_device_crosststamp xtstamp = { .clock_id = CLOCK_REALTIME };
+	struct ptp_sys_offset_precise precise_offset;
+	struct timespec64 ts;
+	int err;
+
+	if (!crosststamp_fn)
+		return -EOPNOTSUPP;
+
+	err = crosststamp_fn(ptp->info, &xtstamp);
+	if (err)
+		return err;
+
+	memset(&precise_offset, 0, sizeof(precise_offset));
+	ts = ktime_to_timespec64(xtstamp.device);
+	precise_offset.device.sec = ts.tv_sec;
+	precise_offset.device.nsec = ts.tv_nsec;
+	ts = ktime_to_timespec64(xtstamp.sys_systime);
+	precise_offset.sys_realtime.sec = ts.tv_sec;
+	precise_offset.sys_realtime.nsec = ts.tv_nsec;
+	ts = ktime_to_timespec64(xtstamp.sys_monoraw);
+	precise_offset.sys_monoraw.sec = ts.tv_sec;
+	precise_offset.sys_monoraw.nsec = ts.tv_nsec;
+
+	return copy_to_user(arg, &precise_offset, sizeof(precise_offset)) ? -EFAULT : 0;
+}
+
+typedef int (*ptp_gettimex_fn)(struct ptp_clock_info *,
+			       struct timespec64 *,
+			       struct ptp_system_timestamp *);
+
+static long ptp_sys_offset_extended(struct ptp_clock *ptp, void __user *arg,
+				    ptp_gettimex_fn gettimex_fn)
+{
+	struct ptp_sys_offset_extended *extoff __free(kfree) = NULL;
+	struct ptp_system_timestamp sts;
+
+	if (!gettimex_fn)
+		return -EOPNOTSUPP;
+
+	extoff = memdup_user(arg, sizeof(*extoff));
+	if (IS_ERR(extoff))
+		return PTR_ERR(extoff);
+
+	if (extoff->n_samples > PTP_MAX_SAMPLES || extoff->rsv[0] || extoff->rsv[1])
+		return -EINVAL;
+
+	switch (extoff->clockid) {
+	case CLOCK_REALTIME:
+	case CLOCK_MONOTONIC:
+	case CLOCK_MONOTONIC_RAW:
+		break;
+	case CLOCK_AUX ... CLOCK_AUX_LAST:
+		if (IS_ENABLED(CONFIG_POSIX_AUX_CLOCKS))
+			break;
+		fallthrough;
+	default:
+		return -EINVAL;
+	}
+
+	sts.clockid = extoff->clockid;
+	for (unsigned int i = 0; i < extoff->n_samples; i++) {
+		struct timespec64 ts;
+		int err;
+
+		err = gettimex_fn(ptp->info, &ts, &sts);
+		if (err)
+			return err;
+
+		/* Filter out disabled or unavailable clocks */
+		if (!sts.pre_sts.valid || !sts.post_sts.valid)
+			return -EINVAL;
+
+		extoff->ts[i][1].sec = ts.tv_sec;
+		extoff->ts[i][1].nsec = ts.tv_nsec;
+
+		ts = ktime_to_timespec64(sts.pre_sts.systime);
+		extoff->ts[i][0].sec = ts.tv_sec;
+		extoff->ts[i][0].nsec = ts.tv_nsec;
+
+		ts = ktime_to_timespec64(sts.post_sts.systime);
+		extoff->ts[i][2].sec = ts.tv_sec;
+		extoff->ts[i][2].nsec = ts.tv_nsec;
+	}
+
+	return copy_to_user(arg, extoff, sizeof(*extoff)) ? -EFAULT : 0;
+}
+
+static long ptp_sys_offset(struct ptp_clock *ptp, void __user *arg)
+{
+	struct ptp_sys_offset *sysoff __free(kfree) = NULL;
+	struct ptp_clock_time *pct;
+	struct timespec64 ts;
+
+	sysoff = memdup_user(arg, sizeof(*sysoff));
+	if (IS_ERR(sysoff))
+		return PTR_ERR(sysoff);
+
+	if (sysoff->n_samples > PTP_MAX_SAMPLES)
+		return -EINVAL;
+
+	pct = &sysoff->ts[0];
+	for (unsigned int i = 0; i < sysoff->n_samples; i++) {
+		struct ptp_clock_info *ops = ptp->info;
+		int err;
+
+		ktime_get_real_ts64(&ts);
+		pct->sec = ts.tv_sec;
+		pct->nsec = ts.tv_nsec;
+		pct++;
+		if (ops->gettimex64)
+			err = ops->gettimex64(ops, &ts, NULL);
+		else
+			err = ops->gettime64(ops, &ts);
+		if (err)
+			return err;
+		pct->sec = ts.tv_sec;
+		pct->nsec = ts.tv_nsec;
+		pct++;
+	}
+	ktime_get_real_ts64(&ts);
+	pct->sec = ts.tv_sec;
+	pct->nsec = ts.tv_nsec;
+
+	return copy_to_user(arg, sysoff, sizeof(*sysoff)) ? -EFAULT : 0;
+}
+
+static long ptp_pin_getfunc(struct ptp_clock *ptp, unsigned int cmd, void __user *arg)
+{
+	struct ptp_clock_info *ops = ptp->info;
+	struct ptp_pin_desc pd;
+
+	if (copy_from_user(&pd, arg, sizeof(pd)))
+		return -EFAULT;
+
+	if (cmd == PTP_PIN_GETFUNC2 && !mem_is_zero(pd.rsv, sizeof(pd.rsv)))
+		return -EINVAL;
+
+	if (pd.index >= ops->n_pins)
+		return -EINVAL;
+
+	scoped_cond_guard(mutex_intr, return -ERESTARTSYS, &ptp->pincfg_mux)
+		pd = ops->pin_config[array_index_nospec(pd.index, ops->n_pins)];
+
+	return copy_to_user(arg, &pd, sizeof(pd)) ? -EFAULT : 0;
+}
+
+static long ptp_pin_setfunc(struct ptp_clock *ptp, unsigned int cmd, void __user *arg)
+{
+	struct ptp_clock_info *ops = ptp->info;
+	struct ptp_pin_desc pd;
+	unsigned int pin_index;
+
+	if (copy_from_user(&pd, arg, sizeof(pd)))
+		return -EFAULT;
+
+	if (cmd == PTP_PIN_SETFUNC2 && !mem_is_zero(pd.rsv, sizeof(pd.rsv)))
+		return -EINVAL;
+
+	if (pd.index >= ops->n_pins)
+		return -EINVAL;
+
+	pin_index = array_index_nospec(pd.index, ops->n_pins);
+	scoped_cond_guard(mutex_intr, return -ERESTARTSYS, &ptp->pincfg_mux)
+		return ptp_set_pinfunc(ptp, pin_index, pd.func, pd.chan);
+}
+
+static long ptp_mask_clear_all(struct timestamp_event_queue *tsevq)
+{
+	bitmap_clear(tsevq->mask, 0, PTP_MAX_CHANNELS);
+	return 0;
+}
+
+static long ptp_mask_en_single(struct timestamp_event_queue *tsevq, void __user *arg)
+{
+	unsigned int channel;
+
+	if (copy_from_user(&channel, arg, sizeof(channel)))
+		return -EFAULT;
+	if (channel >= PTP_MAX_CHANNELS)
+		return -EFAULT;
+	set_bit(channel, tsevq->mask);
+	return 0;
+}
+
+long ptp_ioctl(struct posix_clock_context *pccontext, unsigned int cmd,
+	       unsigned long arg)
+{
+	struct ptp_clock *ptp = container_of(pccontext->clk, struct ptp_clock, clock);
+	void __user *argptr;
+
+	if (in_compat_syscall() && cmd != PTP_ENABLE_PPS && cmd != PTP_ENABLE_PPS2)
+		arg = (unsigned long)compat_ptr(arg);
+	argptr = (void __force __user *)arg;
+
+	switch (cmd) {
+	case PTP_CLOCK_GETCAPS:
+	case PTP_CLOCK_GETCAPS2:
+		return ptp_clock_getcaps(ptp, argptr);
+
+	case PTP_EXTTS_REQUEST:
+	case PTP_EXTTS_REQUEST2:
+		if ((pccontext->fp->f_mode & FMODE_WRITE) == 0)
+			return -EACCES;
+		return ptp_extts_request(ptp, cmd, argptr);
+
+	case PTP_PEROUT_REQUEST:
+	case PTP_PEROUT_REQUEST2:
+		if ((pccontext->fp->f_mode & FMODE_WRITE) == 0)
+			return -EACCES;
+		return ptp_perout_request(ptp, cmd, argptr);
+
+	case PTP_ENABLE_PPS:
+	case PTP_ENABLE_PPS2:
+		if ((pccontext->fp->f_mode & FMODE_WRITE) == 0)
+			return -EACCES;
+		return ptp_enable_pps(ptp, !!arg);
+
+	case PTP_SYS_OFFSET_PRECISE:
+	case PTP_SYS_OFFSET_PRECISE2:
+		return ptp_sys_offset_precise(ptp, argptr,
+					      ptp->info->getcrosststamp);
+
+	case PTP_SYS_OFFSET_EXTENDED:
+	case PTP_SYS_OFFSET_EXTENDED2:
+		return ptp_sys_offset_extended(ptp, argptr,
+					       ptp->info->gettimex64);
+
+	case PTP_SYS_OFFSET:
+	case PTP_SYS_OFFSET2:
+		return ptp_sys_offset(ptp, argptr);
+
+	case PTP_PIN_GETFUNC:
+	case PTP_PIN_GETFUNC2:
+		return ptp_pin_getfunc(ptp, cmd, argptr);
+
+	case PTP_PIN_SETFUNC:
+	case PTP_PIN_SETFUNC2:
+		if ((pccontext->fp->f_mode & FMODE_WRITE) == 0)
+			return -EACCES;
+		return ptp_pin_setfunc(ptp, cmd, argptr);
+
+	case PTP_MASK_CLEAR_ALL:
+		return ptp_mask_clear_all(pccontext->private_clkdata);
+
+	case PTP_MASK_EN_SINGLE:
+		return ptp_mask_en_single(pccontext->private_clkdata, argptr);
+
+	case PTP_SYS_OFFSET_PRECISE_CYCLES:
+		if (!ptp->has_cycles)
+			return -EOPNOTSUPP;
+		return ptp_sys_offset_precise(ptp, argptr,
+					      ptp->info->getcrosscycles);
+
+	case PTP_SYS_OFFSET_EXTENDED_CYCLES:
+		if (!ptp->has_cycles)
+			return -EOPNOTSUPP;
+		return ptp_sys_offset_extended(ptp, argptr,
+					       ptp->info->getcyclesx64);
+	default:
+		return -ENOTTY;
+	}
+}
+
+__poll_t ptp_poll(struct posix_clock_context *pccontext, struct file *fp,
+		  poll_table *wait)
+{
+	struct ptp_clock *ptp =
+		container_of(pccontext->clk, struct ptp_clock, clock);
+	struct timestamp_event_queue *queue;
+
+	queue = pccontext->private_clkdata;
+	if (!queue)
+		return EPOLLERR;
+
+	poll_wait(fp, &ptp->tsev_wq, wait);
+
+	return queue_cnt(queue) ? EPOLLIN : 0;
+}
+
+#define EXTTS_BUFSIZE (PTP_BUF_TIMESTAMPS * sizeof(struct ptp_extts_event))
+
+ssize_t ptp_read(struct posix_clock_context *pccontext, uint rdflags,
+		 char __user *buf, size_t cnt)
+{
+	struct ptp_clock *ptp =	container_of(pccontext->clk, struct ptp_clock, clock);
+	struct timestamp_event_queue *queue;
+	struct ptp_extts_event *event;
+	ssize_t result;
+
+	queue = pccontext->private_clkdata;
+	if (!queue)
+		return -EINVAL;
+
+	if (cnt % sizeof(*event) != 0)
+		return -EINVAL;
+
+	if (cnt > EXTTS_BUFSIZE)
+		cnt = EXTTS_BUFSIZE;
+
+	if (wait_event_interruptible(ptp->tsev_wq, ptp->defunct || queue_cnt(queue)))
+		return -ERESTARTSYS;
+
+	if (ptp->defunct)
+		return -ENODEV;
+
+	event = kmalloc(EXTTS_BUFSIZE, GFP_KERNEL);
+	if (!event)
+		return -ENOMEM;
+
+	scoped_guard(spinlock_irq, &queue->lock) {
+		size_t qcnt = min((size_t)queue_cnt(queue), cnt / sizeof(*event));
+
+		for (size_t i = 0; i < qcnt; i++) {
+			event[i] = queue->buf[queue->head];
+			/* Paired with READ_ONCE() in queue_cnt() */
+			WRITE_ONCE(queue->head, (queue->head + 1) % PTP_MAX_TIMESTAMPS);
+		}
+		cnt = qcnt * sizeof(*event);
+	}
+
+	result = cnt;
+	if (copy_to_user(buf, event, cnt))
+		result = -EFAULT;
+
+	kfree(event);
+	return result;
+}

linux-7.2.3/drivers/ptp/ptp_clock.c

@@ -0,0 +1,711 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * PTP 1588 clock support
+ *
+ * Copyright (C) 2010 OMICRON electronics GmbH
+ */
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/posix-clock.h>
+#include <linux/pps_kernel.h>
+#include <linux/property.h>
+#include <linux/slab.h>
+#include <linux/syscalls.h>
+#include <linux/uaccess.h>
+#include <linux/debugfs.h>
+#include <linux/xarray.h>
+#include <uapi/linux/sched/types.h>
+
+#include "ptp_private.h"
+
+#define PTP_MAX_ALARMS 4
+#define PTP_PPS_DEFAULTS (PPS_CAPTUREASSERT | PPS_OFFSETASSERT)
+#define PTP_PPS_EVENT PPS_CAPTUREASSERT
+#define PTP_PPS_MODE (PTP_PPS_DEFAULTS | PPS_CANWAIT | PPS_TSFMT_TSPEC)
+
+const struct class ptp_class = {
+	.name = "ptp",
+	.dev_groups = ptp_groups
+};
+
+/* private globals */
+
+static dev_t ptp_devt;
+
+static DEFINE_XARRAY_ALLOC(ptp_clocks_map);
+
+/* time stamp event queue operations */
+
+static inline int queue_free(struct timestamp_event_queue *q)
+{
+	return PTP_MAX_TIMESTAMPS - queue_cnt(q) - 1;
+}
+
+static void enqueue_external_timestamp(struct timestamp_event_queue *queue,
+				       struct ptp_clock_event *src)
+{
+	struct ptp_extts_event *dst;
+	struct timespec64 offset_ts;
+	unsigned long flags;
+	s64 seconds;
+	u32 remainder;
+
+	if (src->type == PTP_CLOCK_EXTTS) {
+		seconds = div_u64_rem(src->timestamp, 1000000000, &remainder);
+	} else if (src->type == PTP_CLOCK_EXTOFF) {
+		offset_ts = ns_to_timespec64(src->offset);
+		seconds = offset_ts.tv_sec;
+		remainder = offset_ts.tv_nsec;
+	} else {
+		WARN(1, "%s: unknown type %d\n", __func__, src->type);
+		return;
+	}
+
+	spin_lock_irqsave(&queue->lock, flags);
+
+	dst = &queue->buf[queue->tail];
+	dst->index = src->index;
+	dst->flags = PTP_EXTTS_EVENT_VALID;
+	dst->t.sec = seconds;
+	dst->t.nsec = remainder;
+	if (src->type == PTP_CLOCK_EXTOFF)
+		dst->flags |= PTP_EXT_OFFSET;
+
+	/* Both WRITE_ONCE() are paired with READ_ONCE() in queue_cnt() */
+	if (!queue_free(queue))
+		WRITE_ONCE(queue->head, (queue->head + 1) % PTP_MAX_TIMESTAMPS);
+
+	WRITE_ONCE(queue->tail, (queue->tail + 1) % PTP_MAX_TIMESTAMPS);
+
+	spin_unlock_irqrestore(&queue->lock, flags);
+}
+
+/* posix clock implementation */
+
+static int ptp_clock_getres(struct posix_clock *pc, struct timespec64 *tp)
+{
+	tp->tv_sec = 0;
+	tp->tv_nsec = 1;
+	return 0;
+}
+
+static int ptp_clock_settime(struct posix_clock *pc, const struct timespec64 *tp)
+{
+	struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
+
+	if (ptp_clock_freerun(ptp)) {
+		pr_err_ratelimited("ptp: physical clock is free running\n");
+		return -EBUSY;
+	}
+
+	if (!timespec64_valid_settod(tp))
+		return -EINVAL;
+
+	return  ptp->info->settime64(ptp->info, tp);
+}
+
+static int ptp_clock_gettime(struct posix_clock *pc, struct timespec64 *tp)
+{
+	struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
+	int err;
+
+	if (ptp->info->gettimex64)
+		err = ptp->info->gettimex64(ptp->info, tp, NULL);
+	else
+		err = ptp->info->gettime64(ptp->info, tp);
+	return err;
+}
+
+static int ptp_clock_adjtime(struct posix_clock *pc, struct __kernel_timex *tx)
+{
+	struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
+	struct ptp_clock_info *ops;
+	int err = -EOPNOTSUPP;
+
+	if (tx->modes & (ADJ_SETOFFSET | ADJ_FREQUENCY | ADJ_OFFSET) &&
+	    ptp_clock_freerun(ptp)) {
+		pr_err("ptp: physical clock is free running\n");
+		return -EBUSY;
+	}
+
+	ops = ptp->info;
+
+	if (tx->modes & ADJ_SETOFFSET) {
+		struct timespec64 ts, ts2;
+		ktime_t kt;
+		s64 delta;
+
+		ts.tv_sec  = tx->time.tv_sec;
+		ts.tv_nsec = tx->time.tv_usec;
+
+		if (!(tx->modes & ADJ_NANO))
+			ts.tv_nsec *= 1000;
+
+		if ((unsigned long) ts.tv_nsec >= NSEC_PER_SEC)
+			return -EINVAL;
+
+		/* Make sure the offset is valid */
+		err = ptp_clock_gettime(pc, &ts2);
+		if (err)
+			return err;
+		ts2 = timespec64_add(ts2, ts);
+		if (!timespec64_valid_settod(&ts2))
+			return -EINVAL;
+
+		kt = timespec64_to_ktime(ts);
+		delta = ktime_to_ns(kt);
+		err = ops->adjtime(ops, delta);
+	} else if (tx->modes & ADJ_FREQUENCY) {
+		long ppb = scaled_ppm_to_ppb(tx->freq);
+		if (ppb > ops->max_adj || ppb < -ops->max_adj)
+			return -ERANGE;
+		err = ops->adjfine(ops, tx->freq);
+		if (!err)
+			ptp->dialed_frequency = tx->freq;
+	} else if (tx->modes & ADJ_OFFSET) {
+		if (ops->adjphase) {
+			s32 max_phase_adj = ops->getmaxphase(ops);
+			s32 offset = tx->offset;
+
+			if (!(tx->modes & ADJ_NANO))
+				offset *= NSEC_PER_USEC;
+
+			if (offset > max_phase_adj || offset < -max_phase_adj)
+				return -ERANGE;
+
+			err = ops->adjphase(ops, offset);
+		}
+	} else if (tx->modes == 0) {
+		tx->freq = ptp->dialed_frequency;
+		err = 0;
+	}
+
+	return err;
+}
+
+static struct posix_clock_operations ptp_clock_ops = {
+	.owner		= THIS_MODULE,
+	.clock_adjtime	= ptp_clock_adjtime,
+	.clock_gettime	= ptp_clock_gettime,
+	.clock_getres	= ptp_clock_getres,
+	.clock_settime	= ptp_clock_settime,
+	.ioctl		= ptp_ioctl,
+	.open		= ptp_open,
+	.release	= ptp_release,
+	.poll		= ptp_poll,
+	.read		= ptp_read,
+};
+
+static void ptp_clock_release(struct device *dev)
+{
+	struct ptp_clock *ptp = container_of(dev, struct ptp_clock, dev);
+	struct timestamp_event_queue *tsevq;
+	unsigned long flags;
+
+	ptp_cleanup_pin_groups(ptp);
+	kfree(ptp->vclock_index);
+	mutex_destroy(&ptp->pincfg_mux);
+	mutex_destroy(&ptp->n_vclocks_mux);
+	/* Delete first entry */
+	spin_lock_irqsave(&ptp->tsevqs_lock, flags);
+	tsevq = list_first_entry(&ptp->tsevqs, struct timestamp_event_queue,
+				 qlist);
+	list_del(&tsevq->qlist);
+	spin_unlock_irqrestore(&ptp->tsevqs_lock, flags);
+	bitmap_free(tsevq->mask);
+	kfree(tsevq);
+	debugfs_remove(ptp->debugfs_root);
+	xa_erase(&ptp_clocks_map, ptp->index);
+	kfree(ptp);
+}
+
+static int ptp_getcycles64(struct ptp_clock_info *info, struct timespec64 *ts)
+{
+	if (info->getcyclesx64)
+		return info->getcyclesx64(info, ts, NULL);
+	else
+		return info->gettime64(info, ts);
+}
+
+static int ptp_enable(struct ptp_clock_info *ptp, struct ptp_clock_request *request, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static void ptp_aux_kworker(struct kthread_work *work)
+{
+	struct ptp_clock *ptp = container_of(work, struct ptp_clock,
+					     aux_work.work);
+	struct ptp_clock_info *info = ptp->info;
+	long delay;
+
+	delay = info->do_aux_work(info);
+
+	if (delay >= 0)
+		kthread_queue_delayed_work(ptp->kworker, &ptp->aux_work, delay);
+}
+
+static ssize_t ptp_n_perout_loopback_read(struct file *filep,
+					  char __user *buffer,
+					  size_t count, loff_t *pos)
+{
+	struct ptp_clock *ptp = filep->private_data;
+	char buf[12] = {};
+
+	snprintf(buf, sizeof(buf), "%d\n", ptp->info->n_per_lp);
+
+	return simple_read_from_buffer(buffer, count, pos, buf, strlen(buf));
+}
+
+static const struct file_operations ptp_n_perout_loopback_fops = {
+	.owner	= THIS_MODULE,
+	.open	= simple_open,
+	.read	= ptp_n_perout_loopback_read,
+};
+
+static ssize_t ptp_perout_loopback_write(struct file *filep,
+					 const char __user *buffer,
+					 size_t count, loff_t *ppos)
+{
+	struct ptp_clock *ptp = filep->private_data;
+	struct ptp_clock_info *ops = ptp->info;
+	unsigned int index, enable;
+	int len, cnt, err;
+	char buf[32] = {};
+
+	if (*ppos || !count)
+		return -EINVAL;
+
+	if (count >= sizeof(buf))
+		return -ENOSPC;
+
+	len = simple_write_to_buffer(buf, sizeof(buf) - 1,
+				     ppos, buffer, count);
+	if (len < 0)
+		return len;
+
+	buf[len] = '\0';
+	cnt = sscanf(buf, "%u %u", &index, &enable);
+	if (cnt != 2)
+		return -EINVAL;
+
+	if (index >= ops->n_per_lp)
+		return -EINVAL;
+
+	if (enable != 0 && enable != 1)
+		return -EINVAL;
+
+	err = ops->perout_loopback(ops, index, enable);
+	if (err)
+		return err;
+
+	return count;
+}
+
+static const struct file_operations ptp_perout_loopback_ops = {
+	.owner   = THIS_MODULE,
+	.open    = simple_open,
+	.write	 = ptp_perout_loopback_write,
+};
+
+/* public interface */
+
+struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info,
+				     struct device *parent)
+{
+	struct ptp_clock *ptp;
+	struct timestamp_event_queue *queue = NULL;
+	int err, index, major = MAJOR(ptp_devt);
+	char debugfsname[16];
+	size_t size;
+
+	if (WARN_ON_ONCE(info->n_alarm > PTP_MAX_ALARMS ||
+			 (!info->gettimex64 && !info->gettime64) ||
+			 !info->settime64))
+		return ERR_PTR(-EINVAL);
+
+	/* Initialize a clock structure. */
+	ptp = kzalloc_obj(struct ptp_clock);
+	if (!ptp) {
+		err = -ENOMEM;
+		goto no_memory;
+	}
+
+	err = xa_alloc(&ptp_clocks_map, &index, ptp, xa_limit_31b,
+		       GFP_KERNEL);
+	if (err)
+		goto no_slot;
+
+	ptp->clock.ops = ptp_clock_ops;
+	ptp->info = info;
+	ptp->devid = MKDEV(major, index);
+	ptp->index = index;
+	INIT_LIST_HEAD(&ptp->tsevqs);
+	queue = kzalloc_obj(*queue);
+	if (!queue) {
+		err = -ENOMEM;
+		goto no_memory_queue;
+	}
+	list_add_tail(&queue->qlist, &ptp->tsevqs);
+	spin_lock_init(&ptp->tsevqs_lock);
+	queue->mask = bitmap_alloc(PTP_MAX_CHANNELS, GFP_KERNEL);
+	if (!queue->mask) {
+		err = -ENOMEM;
+		goto no_memory_bitmap;
+	}
+	bitmap_set(queue->mask, 0, PTP_MAX_CHANNELS);
+	spin_lock_init(&queue->lock);
+	mutex_init(&ptp->pincfg_mux);
+	mutex_init(&ptp->n_vclocks_mux);
+	init_waitqueue_head(&ptp->tsev_wq);
+
+	if (ptp->info->getcycles64 || ptp->info->getcyclesx64) {
+		ptp->has_cycles = true;
+		if (!ptp->info->getcycles64 && ptp->info->getcyclesx64)
+			ptp->info->getcycles64 = ptp_getcycles64;
+	} else {
+		/* Free running cycle counter not supported, use time. */
+		ptp->info->getcycles64 = ptp_getcycles64;
+
+		if (ptp->info->gettimex64)
+			ptp->info->getcyclesx64 = ptp->info->gettimex64;
+
+		if (ptp->info->getcrosststamp)
+			ptp->info->getcrosscycles = ptp->info->getcrosststamp;
+	}
+
+	if (!ptp->info->enable)
+		ptp->info->enable = ptp_enable;
+
+	if (ptp->info->do_aux_work) {
+		kthread_init_delayed_work(&ptp->aux_work, ptp_aux_kworker);
+		ptp->kworker = kthread_run_worker(0, "ptp%d", ptp->index);
+		if (IS_ERR(ptp->kworker)) {
+			err = PTR_ERR(ptp->kworker);
+			pr_err("failed to create ptp aux_worker %d\n", err);
+			goto kworker_err;
+		}
+	}
+
+	/* PTP virtual clock is being registered under physical clock */
+	if (parent && parent->class && parent->class->name &&
+	    strcmp(parent->class->name, "ptp") == 0)
+		ptp->is_virtual_clock = true;
+
+	if (!ptp->is_virtual_clock) {
+		ptp->max_vclocks = PTP_DEFAULT_MAX_VCLOCKS;
+
+		size = sizeof(int) * ptp->max_vclocks;
+		ptp->vclock_index = kzalloc(size, GFP_KERNEL);
+		if (!ptp->vclock_index) {
+			err = -ENOMEM;
+			goto no_mem_for_vclocks;
+		}
+	}
+
+	err = ptp_populate_pin_groups(ptp);
+	if (err)
+		goto no_pin_groups;
+
+	/* Register a new PPS source. */
+	if (info->pps) {
+		struct pps_source_info pps;
+		memset(&pps, 0, sizeof(pps));
+		snprintf(pps.name, PPS_MAX_NAME_LEN, "ptp%d", index);
+		pps.mode = PTP_PPS_MODE;
+		pps.owner = info->owner;
+		ptp->pps_source = pps_register_source(&pps, PTP_PPS_DEFAULTS);
+		if (IS_ERR(ptp->pps_source)) {
+			err = PTR_ERR(ptp->pps_source);
+			pr_err("failed to register pps source\n");
+			goto no_pps;
+		}
+		ptp->pps_source->lookup_cookie = ptp;
+	}
+
+	/* Initialize a new device of our class in our clock structure. */
+	device_initialize(&ptp->dev);
+	ptp->dev.devt = ptp->devid;
+	ptp->dev.class = &ptp_class;
+	ptp->dev.parent = parent;
+	ptp->dev.groups = ptp->pin_attr_groups;
+	ptp->dev.release = ptp_clock_release;
+	dev_set_drvdata(&ptp->dev, ptp);
+	dev_set_name(&ptp->dev, "ptp%d", ptp->index);
+
+	/* Create a posix clock and link it to the device. */
+	err = posix_clock_register(&ptp->clock, &ptp->dev);
+	if (err) {
+		if (ptp->pps_source)
+			pps_unregister_source(ptp->pps_source);
+
+		if (ptp->kworker)
+			kthread_destroy_worker(ptp->kworker);
+
+		put_device(&ptp->dev);
+
+		pr_err("failed to create posix clock\n");
+		return ERR_PTR(err);
+	}
+
+	/* Debugfs initialization */
+	snprintf(debugfsname, sizeof(debugfsname), "ptp%d", ptp->index);
+	ptp->debugfs_root = debugfs_create_dir(debugfsname, NULL);
+	if (info->n_per_lp > 0 && info->perout_loopback) {
+		debugfs_create_file("n_perout_loopback", 0400, ptp->debugfs_root,
+				    ptp, &ptp_n_perout_loopback_fops);
+		debugfs_create_file("perout_loopback", 0200, ptp->debugfs_root,
+				    ptp, &ptp_perout_loopback_ops);
+	}
+
+	return ptp;
+
+no_pps:
+	ptp_cleanup_pin_groups(ptp);
+no_pin_groups:
+	kfree(ptp->vclock_index);
+no_mem_for_vclocks:
+	if (ptp->kworker)
+		kthread_destroy_worker(ptp->kworker);
+kworker_err:
+	mutex_destroy(&ptp->pincfg_mux);
+	mutex_destroy(&ptp->n_vclocks_mux);
+	bitmap_free(queue->mask);
+no_memory_bitmap:
+	list_del(&queue->qlist);
+	kfree(queue);
+no_memory_queue:
+	xa_erase(&ptp_clocks_map, index);
+no_slot:
+	kfree(ptp);
+no_memory:
+	return ERR_PTR(err);
+}
+EXPORT_SYMBOL(ptp_clock_register);
+
+static int unregister_vclock(struct device *dev, void *data)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+
+	ptp_vclock_unregister(info_to_vclock(ptp->info));
+	return 0;
+}
+
+int ptp_clock_unregister(struct ptp_clock *ptp)
+{
+	if (ptp_vclock_in_use(ptp)) {
+		device_for_each_child(&ptp->dev, NULL, unregister_vclock);
+	}
+
+	/* Get the device to stop posix_clock_unregister() doing the last put
+	 * and freeing the structure(s)
+	 */
+	get_device(&ptp->dev);
+
+	/* Wake up any userspace waiting for an event. */
+	ptp->defunct = 1;
+	wake_up_interruptible(&ptp->tsev_wq);
+
+	/* Tear down the POSIX clock, which removes the user interface. */
+	posix_clock_unregister(&ptp->clock);
+
+	/* Disable all sources of event generation. */
+	ptp_disable_all_events(ptp);
+
+	if (ptp->kworker) {
+		kthread_cancel_delayed_work_sync(&ptp->aux_work);
+		kthread_destroy_worker(ptp->kworker);
+	}
+
+	/* Release the clock's resources. */
+	if (ptp->pps_source)
+		pps_unregister_source(ptp->pps_source);
+
+	/* The final put, normally here, will invoke ptp_clock_release(). */
+	put_device(&ptp->dev);
+
+	return 0;
+}
+EXPORT_SYMBOL(ptp_clock_unregister);
+
+void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event)
+{
+	struct timestamp_event_queue *tsevq;
+	struct pps_event_time evt;
+	unsigned long flags;
+
+	switch (event->type) {
+
+	case PTP_CLOCK_ALARM:
+		break;
+
+	case PTP_CLOCK_EXTTS:
+	case PTP_CLOCK_EXTOFF:
+		/* Enqueue timestamp on selected queues */
+		spin_lock_irqsave(&ptp->tsevqs_lock, flags);
+		list_for_each_entry(tsevq, &ptp->tsevqs, qlist) {
+			if (test_bit((unsigned int)event->index, tsevq->mask))
+				enqueue_external_timestamp(tsevq, event);
+		}
+		spin_unlock_irqrestore(&ptp->tsevqs_lock, flags);
+		wake_up_interruptible(&ptp->tsev_wq);
+		break;
+
+	case PTP_CLOCK_PPS:
+		pps_get_ts(&evt);
+		pps_event(ptp->pps_source, &evt, PTP_PPS_EVENT, NULL);
+		break;
+
+	case PTP_CLOCK_PPSUSR:
+		pps_event(ptp->pps_source, &event->pps_times,
+			  PTP_PPS_EVENT, NULL);
+		break;
+	}
+}
+EXPORT_SYMBOL(ptp_clock_event);
+
+int ptp_clock_index(struct ptp_clock *ptp)
+{
+	return ptp->index;
+}
+EXPORT_SYMBOL(ptp_clock_index);
+
+static int ptp_clock_of_node_match(struct device *dev, const void *data)
+{
+	const struct device_node *parent_np = data;
+
+	return (dev->parent && dev_of_node(dev->parent) == parent_np);
+}
+
+int ptp_clock_index_by_of_node(struct device_node *np)
+{
+	struct ptp_clock *ptp;
+	struct device *dev;
+	int phc_index;
+
+	dev = class_find_device(&ptp_class, NULL, np,
+				ptp_clock_of_node_match);
+	if (!dev)
+		return -1;
+
+	ptp = dev_get_drvdata(dev);
+	phc_index = ptp_clock_index(ptp);
+	put_device(dev);
+
+	return phc_index;
+}
+EXPORT_SYMBOL_GPL(ptp_clock_index_by_of_node);
+
+static int ptp_clock_dev_match(struct device *dev, const void *data)
+{
+	const struct device *parent = data;
+
+	return dev->parent == parent;
+}
+
+int ptp_clock_index_by_dev(struct device *parent)
+{
+	struct ptp_clock *ptp;
+	struct device *dev;
+	int phc_index;
+
+	dev = class_find_device(&ptp_class, NULL, parent,
+				ptp_clock_dev_match);
+	if (!dev)
+		return -1;
+
+	ptp = dev_get_drvdata(dev);
+	phc_index = ptp_clock_index(ptp);
+	put_device(dev);
+
+	return phc_index;
+}
+EXPORT_SYMBOL_GPL(ptp_clock_index_by_dev);
+
+int ptp_find_pin(struct ptp_clock *ptp,
+		 enum ptp_pin_function func, unsigned int chan)
+{
+	struct ptp_pin_desc *pin = NULL;
+	int i;
+
+	for (i = 0; i < ptp->info->n_pins; i++) {
+		if (ptp->info->pin_config[i].func == func &&
+		    ptp->info->pin_config[i].chan == chan) {
+			pin = &ptp->info->pin_config[i];
+			break;
+		}
+	}
+
+	return pin ? i : -1;
+}
+EXPORT_SYMBOL(ptp_find_pin);
+
+int ptp_find_pin_unlocked(struct ptp_clock *ptp,
+			  enum ptp_pin_function func, unsigned int chan)
+{
+	int result;
+
+	mutex_lock(&ptp->pincfg_mux);
+
+	result = ptp_find_pin(ptp, func, chan);
+
+	mutex_unlock(&ptp->pincfg_mux);
+
+	return result;
+}
+EXPORT_SYMBOL(ptp_find_pin_unlocked);
+
+int ptp_schedule_worker(struct ptp_clock *ptp, unsigned long delay)
+{
+	return kthread_mod_delayed_work(ptp->kworker, &ptp->aux_work, delay);
+}
+EXPORT_SYMBOL(ptp_schedule_worker);
+
+void ptp_cancel_worker_sync(struct ptp_clock *ptp)
+{
+	kthread_cancel_delayed_work_sync(&ptp->aux_work);
+}
+EXPORT_SYMBOL(ptp_cancel_worker_sync);
+
+/* module operations */
+
+static void __exit ptp_exit(void)
+{
+	class_unregister(&ptp_class);
+	unregister_chrdev_region(ptp_devt, MINORMASK + 1);
+	xa_destroy(&ptp_clocks_map);
+}
+
+static int __init ptp_init(void)
+{
+	int err;
+
+	err = class_register(&ptp_class);
+	if (err) {
+		pr_err("ptp: failed to allocate class\n");
+		return err;
+	}
+
+	err = alloc_chrdev_region(&ptp_devt, 0, MINORMASK + 1, "ptp");
+	if (err < 0) {
+		pr_err("ptp: failed to allocate device region\n");
+		goto no_region;
+	}
+
+	pr_info("PTP clock support registered\n");
+	return 0;
+
+no_region:
+	class_unregister(&ptp_class);
+	return err;
+}
+
+subsys_initcall(ptp_init);
+module_exit(ptp_exit);
+
+MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
+MODULE_DESCRIPTION("PTP clocks support");
+MODULE_LICENSE("GPL");

linux-7.2.3/drivers/ptp/ptp_clockmatrix.h

@@ -0,0 +1,142 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * PTP hardware clock driver for the IDT ClockMatrix(TM) family of timing and
+ * synchronization devices.
+ *
+ * Copyright (C) 2019 Integrated Device Technology, Inc., a Renesas Company.
+ */
+#ifndef PTP_IDTCLOCKMATRIX_H
+#define PTP_IDTCLOCKMATRIX_H
+
+#include <linux/ktime.h>
+#include <linux/mfd/idt8a340_reg.h>
+#include <linux/ptp_clock.h>
+#include <linux/regmap.h>
+
+#define FW_FILENAME	"idtcm.bin"
+#define MAX_TOD		(4)
+#define MAX_PLL		(8)
+#define MAX_REF_CLK	(16)
+
+#define MAX_ABS_WRITE_PHASE_NANOSECONDS (107374182L)
+
+#define TOD_MASK_ADDR		(0xFFA5)
+#define DEFAULT_TOD_MASK	(0x04)
+
+#define SET_U16_LSB(orig, val8) (orig = (0xff00 & (orig)) | (val8))
+#define SET_U16_MSB(orig, val8) (orig = (0x00ff & (orig)) | (val8 << 8))
+
+#define TOD0_PTP_PLL_ADDR		(0xFFA8)
+#define TOD1_PTP_PLL_ADDR		(0xFFA9)
+#define TOD2_PTP_PLL_ADDR		(0xFFAA)
+#define TOD3_PTP_PLL_ADDR		(0xFFAB)
+
+#define TOD0_OUT_ALIGN_MASK_ADDR	(0xFFB0)
+#define TOD1_OUT_ALIGN_MASK_ADDR	(0xFFB2)
+#define TOD2_OUT_ALIGN_MASK_ADDR	(0xFFB4)
+#define TOD3_OUT_ALIGN_MASK_ADDR	(0xFFB6)
+
+#define DEFAULT_OUTPUT_MASK_PLL0	(0x003)
+#define DEFAULT_OUTPUT_MASK_PLL1	(0x00c)
+#define DEFAULT_OUTPUT_MASK_PLL2	(0x030)
+#define DEFAULT_OUTPUT_MASK_PLL3	(0x0c0)
+
+#define DEFAULT_TOD0_PTP_PLL		(0)
+#define DEFAULT_TOD1_PTP_PLL		(1)
+#define DEFAULT_TOD2_PTP_PLL		(2)
+#define DEFAULT_TOD3_PTP_PLL		(3)
+
+#define PHASE_PULL_IN_THRESHOLD_NS_DEPRECATED	(150000)
+#define PHASE_PULL_IN_THRESHOLD_NS		(15000)
+#define TOD_WRITE_OVERHEAD_COUNT_MAX		(2)
+#define TOD_BYTE_COUNT				(11)
+
+#define LOCK_TIMEOUT_MS			(2000)
+#define LOCK_POLL_INTERVAL_MS		(10)
+
+#define IDTCM_MAX_WRITE_COUNT		(512)
+
+#define PHASE_PULL_IN_MAX_PPB		(144000)
+#define PHASE_PULL_IN_MIN_THRESHOLD_NS	(2)
+
+/*
+ * Return register address based on passed in firmware version
+ */
+#define IDTCM_FW_REG(FW, VER, REG)	(((FW) < (VER)) ? (REG) : (REG##_##VER))
+enum fw_version {
+	V_DEFAULT = 0,
+	V487 = 1,
+	V520 = 2,
+};
+
+/* PTP PLL Mode */
+enum ptp_pll_mode {
+	PTP_PLL_MODE_MIN = 0,
+	PTP_PLL_MODE_WRITE_FREQUENCY = PTP_PLL_MODE_MIN,
+	PTP_PLL_MODE_WRITE_PHASE,
+	PTP_PLL_MODE_UNSUPPORTED,
+	PTP_PLL_MODE_MAX = PTP_PLL_MODE_UNSUPPORTED,
+};
+
+struct idtcm;
+
+struct idtcm_channel {
+	struct ptp_clock_info	caps;
+	struct ptp_clock	*ptp_clock;
+	struct idtcm		*idtcm;
+	u16			dpll_phase;
+	u16			dpll_freq;
+	u16			dpll_n;
+	u16			dpll_ctrl_n;
+	u16			dpll_phase_pull_in;
+	u16			tod_read_primary;
+	u16			tod_read_secondary;
+	u16			tod_write;
+	u16			tod_n;
+	u16			hw_dpll_n;
+	u8			sync_src;
+	enum ptp_pll_mode	mode;
+	int			(*configure_write_frequency)(struct idtcm_channel *channel);
+	int			(*configure_write_phase)(struct idtcm_channel *channel);
+	int			(*do_phase_pull_in)(struct idtcm_channel *channel,
+						    s32 offset_ns, u32 max_ffo_ppb);
+	s32			current_freq_scaled_ppm;
+	bool			phase_pull_in;
+	u32			dco_delay;
+	/* last input trigger for extts */
+	u8			refn;
+	u8			pll;
+	u8			tod;
+	u16			output_mask;
+};
+
+struct idtcm {
+	struct idtcm_channel	channel[MAX_TOD];
+	struct device		*dev;
+	u8			tod_mask;
+	char			version[16];
+	enum fw_version		fw_ver;
+	/* Polls for external time stamps */
+	u8			extts_mask;
+	bool			extts_single_shot;
+	struct delayed_work	extts_work;
+	/* Remember the ptp channel to report extts */
+	struct idtcm_channel	*event_channel[MAX_TOD];
+	/* Mutex to protect operations from being interrupted */
+	struct mutex		*lock;
+	struct device		*mfd;
+	struct regmap		*regmap;
+	/* Overhead calculation for adjtime */
+	u8			calculate_overhead_flag;
+	s64			tod_write_overhead_ns;
+	ktime_t			start_time;
+};
+
+struct idtcm_fwrc {
+	u8 hiaddr;
+	u8 loaddr;
+	u8 value;
+	u8 reserved;
+} __packed;
+
+#endif /* PTP_IDTCLOCKMATRIX_H */

linux-7.2.3/drivers/ptp/ptp_dfl_tod.c

@@ -0,0 +1,332 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * DFL device driver for Time-of-Day (ToD) private feature
+ *
+ * Copyright (C) 2023 Intel Corporation
+ */
+
+#include <linux/bitfield.h>
+#include <linux/delay.h>
+#include <linux/dfl.h>
+#include <linux/gcd.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/spinlock.h>
+#include <linux/units.h>
+
+#define FME_FEATURE_ID_TOD		0x22
+
+/* ToD clock register space. */
+#define TOD_CLK_FREQ			0x038
+
+/*
+ * The read sequence of ToD timestamp registers: TOD_NANOSEC, TOD_SECONDSL and
+ * TOD_SECONDSH, because there is a hardware snapshot whenever the TOD_NANOSEC
+ * register is read.
+ *
+ * The ToD IP requires writing registers in the reverse order to the read sequence.
+ * The timestamp is corrected when the TOD_NANOSEC register is written, so the
+ * sequence of write TOD registers: TOD_SECONDSH, TOD_SECONDSL and TOD_NANOSEC.
+ */
+#define TOD_SECONDSH			0x100
+#define TOD_SECONDSL			0x104
+#define TOD_NANOSEC			0x108
+#define TOD_PERIOD			0x110
+#define TOD_ADJUST_PERIOD		0x114
+#define TOD_ADJUST_COUNT		0x118
+#define TOD_DRIFT_ADJUST		0x11c
+#define TOD_DRIFT_ADJUST_RATE		0x120
+#define PERIOD_FRAC_OFFSET		16
+#define SECONDS_MSB			GENMASK_ULL(47, 32)
+#define SECONDS_LSB			GENMASK_ULL(31, 0)
+#define TOD_SECONDSH_SEC_MSB		GENMASK_ULL(15, 0)
+
+#define CAL_SECONDS(m, l)		((FIELD_GET(TOD_SECONDSH_SEC_MSB, (m)) << 32) | (l))
+
+#define TOD_PERIOD_MASK		GENMASK_ULL(19, 0)
+#define TOD_PERIOD_MAX			FIELD_MAX(TOD_PERIOD_MASK)
+#define TOD_PERIOD_MIN			0
+#define TOD_DRIFT_ADJUST_MASK		GENMASK_ULL(15, 0)
+#define TOD_DRIFT_ADJUST_FNS_MAX	FIELD_MAX(TOD_DRIFT_ADJUST_MASK)
+#define TOD_DRIFT_ADJUST_RATE_MAX	TOD_DRIFT_ADJUST_FNS_MAX
+#define TOD_ADJUST_COUNT_MASK		GENMASK_ULL(19, 0)
+#define TOD_ADJUST_COUNT_MAX		FIELD_MAX(TOD_ADJUST_COUNT_MASK)
+#define TOD_ADJUST_INTERVAL_US		10
+#define TOD_ADJUST_MS			\
+		(((TOD_PERIOD_MAX >> 16) + 1) * (TOD_ADJUST_COUNT_MAX + 1))
+#define TOD_ADJUST_MS_MAX		(TOD_ADJUST_MS / MICRO)
+#define TOD_ADJUST_MAX_US		(TOD_ADJUST_MS_MAX * USEC_PER_MSEC)
+#define TOD_MAX_ADJ			(500 * MEGA)
+
+struct dfl_tod {
+	struct ptp_clock_info ptp_clock_ops;
+	struct device *dev;
+	struct ptp_clock *ptp_clock;
+
+	/* ToD Clock address space */
+	void __iomem *tod_ctrl;
+
+	/* ToD clock registers protection */
+	spinlock_t tod_lock;
+};
+
+/*
+ * A fine ToD HW clock offset adjustment. To perform the fine offset adjustment, the
+ * adjust_period and adjust_count argument are used to update the TOD_ADJUST_PERIOD
+ * and TOD_ADJUST_COUNT register for in hardware. The dt->tod_lock spinlock must be
+ * held when calling this function.
+ */
+static int fine_adjust_tod_clock(struct dfl_tod *dt, u32 adjust_period,
+				 u32 adjust_count)
+{
+	void __iomem *base = dt->tod_ctrl;
+	u32 val;
+
+	writel(adjust_period, base + TOD_ADJUST_PERIOD);
+	writel(adjust_count, base + TOD_ADJUST_COUNT);
+
+	/* Wait for present offset adjustment update to complete */
+	return readl_poll_timeout_atomic(base + TOD_ADJUST_COUNT, val, !val, TOD_ADJUST_INTERVAL_US,
+				  TOD_ADJUST_MAX_US);
+}
+
+/*
+ * 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 dfl_tod *dt, s64 delta)
+{
+	u32 seconds_msb, seconds_lsb, nanosec;
+	void __iomem *base = dt->tod_ctrl;
+	u64 seconds, now;
+
+	if (delta == 0)
+		return 0;
+
+	nanosec = readl(base + TOD_NANOSEC);
+	seconds_lsb = readl(base + TOD_SECONDSL);
+	seconds_msb = readl(base + TOD_SECONDSH);
+
+	/* Calculate new time */
+	seconds = CAL_SECONDS(seconds_msb, seconds_lsb);
+	now = seconds * NSEC_PER_SEC + nanosec + delta;
+
+	seconds = div_u64_rem(now, NSEC_PER_SEC, &nanosec);
+	seconds_msb = FIELD_GET(SECONDS_MSB, seconds);
+	seconds_lsb = FIELD_GET(SECONDS_LSB, seconds);
+
+	writel(seconds_msb, base + TOD_SECONDSH);
+	writel(seconds_lsb, base + TOD_SECONDSL);
+	writel(nanosec, base + TOD_NANOSEC);
+
+	return 0;
+}
+
+static int dfl_tod_adjust_fine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+	struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops);
+	u32 tod_period, tod_rem, tod_drift_adjust_fns, tod_drift_adjust_rate;
+	void __iomem *base = dt->tod_ctrl;
+	unsigned long flags, rate;
+	u64 ppb;
+
+	/* Get the clock rate from clock frequency register offset */
+	rate = readl(base + TOD_CLK_FREQ);
+
+	/* add GIGA as nominal ppb */
+	ppb = scaled_ppm_to_ppb(scaled_ppm) + GIGA;
+
+	tod_period = div_u64_rem(ppb << PERIOD_FRAC_OFFSET, rate, &tod_rem);
+	if (tod_period > TOD_PERIOD_MAX)
+		return -ERANGE;
+
+	/*
+	 * 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 >>= 1;
+		tod_drift_adjust_rate >>= 1;
+	}
+
+	if (tod_drift_adjust_fns == 0)
+		tod_drift_adjust_rate = 0;
+
+	spin_lock_irqsave(&dt->tod_lock, flags);
+	writel(tod_period, base + TOD_PERIOD);
+	writel(0, base + TOD_ADJUST_PERIOD);
+	writel(0, base + TOD_ADJUST_COUNT);
+	writel(tod_drift_adjust_fns, base + TOD_DRIFT_ADJUST);
+	writel(tod_drift_adjust_rate, base + TOD_DRIFT_ADJUST_RATE);
+	spin_unlock_irqrestore(&dt->tod_lock, flags);
+
+	return 0;
+}
+
+static int dfl_tod_adjust_time(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops);
+	u32 period, diff, rem, rem_period, adj_period;
+	void __iomem *base = dt->tod_ctrl;
+	unsigned long flags;
+	bool neg_adj;
+	u64 count;
+	int ret;
+
+	neg_adj = delta < 0;
+	if (neg_adj)
+		delta = -delta;
+
+	spin_lock_irqsave(&dt->tod_lock, flags);
+
+	/*
+	 * Get the maximum possible value of the Period register offset
+	 * adjustment in nanoseconds scale. This depends on the current
+	 * Period register setting and the maximum and minimum possible
+	 * values of the Period register.
+	 */
+	period = readl(base + TOD_PERIOD);
+
+	if (neg_adj) {
+		diff = (period - TOD_PERIOD_MIN) >> PERIOD_FRAC_OFFSET;
+		adj_period = period - (diff << PERIOD_FRAC_OFFSET);
+		count = div_u64_rem(delta, diff, &rem);
+		rem_period = period - (rem << PERIOD_FRAC_OFFSET);
+	} else {
+		diff = (TOD_PERIOD_MAX - period) >> PERIOD_FRAC_OFFSET;
+		adj_period = period + (diff << PERIOD_FRAC_OFFSET);
+		count = div_u64_rem(delta, diff, &rem);
+		rem_period = period + (rem << PERIOD_FRAC_OFFSET);
+	}
+
+	ret = 0;
+
+	if (count > TOD_ADJUST_COUNT_MAX) {
+		ret = coarse_adjust_tod_clock(dt, delta);
+	} else {
+		/* Adjust the period by count cycles to adjust the time */
+		if (count)
+			ret = fine_adjust_tod_clock(dt, adj_period, count);
+
+		/* If there is a remainder, adjust the period for an additional cycle */
+		if (rem)
+			ret = fine_adjust_tod_clock(dt, rem_period, 1);
+	}
+
+	spin_unlock_irqrestore(&dt->tod_lock, flags);
+
+	return ret;
+}
+
+static int dfl_tod_get_timex(struct ptp_clock_info *ptp, struct timespec64 *ts,
+			     struct ptp_system_timestamp *sts)
+{
+	struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops);
+	u32 seconds_msb, seconds_lsb, nanosec;
+	void __iomem *base = dt->tod_ctrl;
+	unsigned long flags;
+	u64 seconds;
+
+	spin_lock_irqsave(&dt->tod_lock, flags);
+	ptp_read_system_prets(sts);
+	nanosec = readl(base + TOD_NANOSEC);
+	seconds_lsb = readl(base + TOD_SECONDSL);
+	seconds_msb = readl(base + TOD_SECONDSH);
+	ptp_read_system_postts(sts);
+	spin_unlock_irqrestore(&dt->tod_lock, flags);
+
+	seconds = CAL_SECONDS(seconds_msb, seconds_lsb);
+
+	ts->tv_nsec = nanosec;
+	ts->tv_sec = seconds;
+
+	return 0;
+}
+
+static int dfl_tod_set_time(struct ptp_clock_info *ptp,
+			    const struct timespec64 *ts)
+{
+	struct dfl_tod *dt = container_of(ptp, struct dfl_tod, ptp_clock_ops);
+	u32 seconds_msb = FIELD_GET(SECONDS_MSB, ts->tv_sec);
+	u32 seconds_lsb = FIELD_GET(SECONDS_LSB, ts->tv_sec);
+	u32 nanosec = FIELD_GET(SECONDS_LSB, ts->tv_nsec);
+	void __iomem *base = dt->tod_ctrl;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dt->tod_lock, flags);
+	writel(seconds_msb, base + TOD_SECONDSH);
+	writel(seconds_lsb, base + TOD_SECONDSL);
+	writel(nanosec, base + TOD_NANOSEC);
+	spin_unlock_irqrestore(&dt->tod_lock, flags);
+
+	return 0;
+}
+
+static struct ptp_clock_info dfl_tod_clock_ops = {
+	.owner = THIS_MODULE,
+	.name = "dfl_tod",
+	.max_adj = TOD_MAX_ADJ,
+	.adjfine = dfl_tod_adjust_fine,
+	.adjtime = dfl_tod_adjust_time,
+	.gettimex64 = dfl_tod_get_timex,
+	.settime64 = dfl_tod_set_time,
+};
+
+static int dfl_tod_probe(struct dfl_device *ddev)
+{
+	struct device *dev = &ddev->dev;
+	struct dfl_tod *dt;
+
+	dt = devm_kzalloc(dev, sizeof(*dt), GFP_KERNEL);
+	if (!dt)
+		return -ENOMEM;
+
+	dt->tod_ctrl = devm_ioremap_resource(dev, &ddev->mmio_res);
+	if (IS_ERR(dt->tod_ctrl))
+		return PTR_ERR(dt->tod_ctrl);
+
+	dt->dev = dev;
+	spin_lock_init(&dt->tod_lock);
+	dev_set_drvdata(dev, dt);
+
+	dt->ptp_clock_ops = dfl_tod_clock_ops;
+
+	dt->ptp_clock = ptp_clock_register(&dt->ptp_clock_ops, dev);
+	if (IS_ERR(dt->ptp_clock))
+		return dev_err_probe(dt->dev, PTR_ERR(dt->ptp_clock),
+				     "Unable to register PTP clock\n");
+
+	return 0;
+}
+
+static void dfl_tod_remove(struct dfl_device *ddev)
+{
+	struct dfl_tod *dt = dev_get_drvdata(&ddev->dev);
+
+	ptp_clock_unregister(dt->ptp_clock);
+}
+
+static const struct dfl_device_id dfl_tod_ids[] = {
+	{ FME_ID, FME_FEATURE_ID_TOD },
+	{ }
+};
+MODULE_DEVICE_TABLE(dfl, dfl_tod_ids);
+
+static struct dfl_driver dfl_tod_driver = {
+	.drv = {
+		.name = "dfl-tod",
+	},
+	.id_table = dfl_tod_ids,
+	.probe = dfl_tod_probe,
+	.remove = dfl_tod_remove,
+};
+module_dfl_driver(dfl_tod_driver);
+
+MODULE_DESCRIPTION("FPGA DFL ToD driver");
+MODULE_AUTHOR("Intel Corporation");
+MODULE_LICENSE("GPL");

linux-7.2.3/drivers/ptp/ptp_dte.c

@@ -0,0 +1,335 @@
+// SPDX-License-Identifier: GPL-2.0-only
+// Copyright 2017 Broadcom
+
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/types.h>
+
+#define DTE_NCO_LOW_TIME_REG	0x00
+#define DTE_NCO_TIME_REG	0x04
+#define DTE_NCO_OVERFLOW_REG	0x08
+#define DTE_NCO_INC_REG		0x0c
+
+#define DTE_NCO_SUM2_MASK	0xffffffff
+#define DTE_NCO_SUM2_SHIFT	4ULL
+
+#define DTE_NCO_SUM3_MASK	0xff
+#define DTE_NCO_SUM3_SHIFT	36ULL
+#define DTE_NCO_SUM3_WR_SHIFT	8
+
+#define DTE_NCO_TS_WRAP_MASK	0xfff
+#define DTE_NCO_TS_WRAP_LSHIFT	32
+
+#define DTE_NCO_INC_DEFAULT	0x80000000
+#define DTE_NUM_REGS_TO_RESTORE	4
+
+/* Full wrap around is 44bits in ns (~4.887 hrs) */
+#define DTE_WRAP_AROUND_NSEC_SHIFT 44
+
+/* 44 bits NCO */
+#define DTE_NCO_MAX_NS	0xFFFFFFFFFFFLL
+
+/* 125MHz with 3.29 reg cfg */
+#define DTE_PPB_ADJ(ppb) (u32)(div64_u64((((u64)abs(ppb) * BIT(28)) +\
+				      62500000ULL), 125000000ULL))
+
+/* ptp dte priv structure */
+struct ptp_dte {
+	void __iomem *regs;
+	struct ptp_clock *ptp_clk;
+	struct ptp_clock_info caps;
+	struct device *dev;
+	u32 ts_ovf_last;
+	u32 ts_wrap_cnt;
+	spinlock_t lock;
+	u32 reg_val[DTE_NUM_REGS_TO_RESTORE];
+};
+
+static void dte_write_nco(void __iomem *regs, s64 ns)
+{
+	u32 sum2, sum3;
+
+	sum2 = (u32)((ns >> DTE_NCO_SUM2_SHIFT) & DTE_NCO_SUM2_MASK);
+	/* compensate for ignoring sum1 */
+	if (sum2 != DTE_NCO_SUM2_MASK)
+		sum2++;
+
+	/* to write sum3, bits [15:8] needs to be written */
+	sum3 = (u32)(((ns >> DTE_NCO_SUM3_SHIFT) & DTE_NCO_SUM3_MASK) <<
+		     DTE_NCO_SUM3_WR_SHIFT);
+
+	writel(0, (regs + DTE_NCO_LOW_TIME_REG));
+	writel(sum2, (regs + DTE_NCO_TIME_REG));
+	writel(sum3, (regs + DTE_NCO_OVERFLOW_REG));
+}
+
+static s64 dte_read_nco(void __iomem *regs)
+{
+	u32 sum2, sum3;
+	s64 ns;
+
+	/*
+	 * ignoring sum1 (4 bits) gives a 16ns resolution, which
+	 * works due to the async register read.
+	 */
+	sum3 = readl(regs + DTE_NCO_OVERFLOW_REG) & DTE_NCO_SUM3_MASK;
+	sum2 = readl(regs + DTE_NCO_TIME_REG);
+	ns = ((s64)sum3 << DTE_NCO_SUM3_SHIFT) |
+		 ((s64)sum2 << DTE_NCO_SUM2_SHIFT);
+
+	return ns;
+}
+
+static void dte_write_nco_delta(struct ptp_dte *ptp_dte, s64 delta)
+{
+	s64 ns;
+
+	ns = dte_read_nco(ptp_dte->regs);
+
+	/* handle wraparound conditions */
+	if ((delta < 0) && (abs(delta) > ns)) {
+		if (ptp_dte->ts_wrap_cnt) {
+			ns += DTE_NCO_MAX_NS + delta;
+			ptp_dte->ts_wrap_cnt--;
+		} else {
+			ns = 0;
+		}
+	} else {
+		ns += delta;
+		if (ns > DTE_NCO_MAX_NS) {
+			ptp_dte->ts_wrap_cnt++;
+			ns -= DTE_NCO_MAX_NS;
+		}
+	}
+
+	dte_write_nco(ptp_dte->regs, ns);
+
+	ptp_dte->ts_ovf_last = (ns >> DTE_NCO_TS_WRAP_LSHIFT) &
+			DTE_NCO_TS_WRAP_MASK;
+}
+
+static s64 dte_read_nco_with_ovf(struct ptp_dte *ptp_dte)
+{
+	u32 ts_ovf;
+	s64 ns = 0;
+
+	ns = dte_read_nco(ptp_dte->regs);
+
+	/*Timestamp overflow: 8 LSB bits of sum3, 4 MSB bits of sum2 */
+	ts_ovf = (ns >> DTE_NCO_TS_WRAP_LSHIFT) & DTE_NCO_TS_WRAP_MASK;
+
+	/* Check for wrap around */
+	if (ts_ovf < ptp_dte->ts_ovf_last)
+		ptp_dte->ts_wrap_cnt++;
+
+	ptp_dte->ts_ovf_last = ts_ovf;
+
+	/* adjust for wraparounds */
+	ns += (s64)(BIT_ULL(DTE_WRAP_AROUND_NSEC_SHIFT) * ptp_dte->ts_wrap_cnt);
+
+	return ns;
+}
+
+static int ptp_dte_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+	s32 ppb = scaled_ppm_to_ppb(scaled_ppm);
+	u32 nco_incr;
+	unsigned long flags;
+	struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
+
+	if (abs(ppb) > ptp_dte->caps.max_adj) {
+		dev_err(ptp_dte->dev, "ppb adj too big\n");
+		return -EINVAL;
+	}
+
+	if (ppb < 0)
+		nco_incr = DTE_NCO_INC_DEFAULT - DTE_PPB_ADJ(ppb);
+	else
+		nco_incr = DTE_NCO_INC_DEFAULT + DTE_PPB_ADJ(ppb);
+
+	spin_lock_irqsave(&ptp_dte->lock, flags);
+	writel(nco_incr, ptp_dte->regs + DTE_NCO_INC_REG);
+	spin_unlock_irqrestore(&ptp_dte->lock, flags);
+
+	return 0;
+}
+
+static int ptp_dte_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	unsigned long flags;
+	struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
+
+	spin_lock_irqsave(&ptp_dte->lock, flags);
+	dte_write_nco_delta(ptp_dte, delta);
+	spin_unlock_irqrestore(&ptp_dte->lock, flags);
+
+	return 0;
+}
+
+static int ptp_dte_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	unsigned long flags;
+	struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
+
+	spin_lock_irqsave(&ptp_dte->lock, flags);
+	*ts = ns_to_timespec64(dte_read_nco_with_ovf(ptp_dte));
+	spin_unlock_irqrestore(&ptp_dte->lock, flags);
+
+	return 0;
+}
+
+static int ptp_dte_settime(struct ptp_clock_info *ptp,
+			     const struct timespec64 *ts)
+{
+	unsigned long flags;
+	struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps);
+
+	spin_lock_irqsave(&ptp_dte->lock, flags);
+
+	/* Disable nco increment */
+	writel(0, ptp_dte->regs + DTE_NCO_INC_REG);
+
+	dte_write_nco(ptp_dte->regs, timespec64_to_ns(ts));
+
+	/* reset overflow and wrap counter */
+	ptp_dte->ts_ovf_last = 0;
+	ptp_dte->ts_wrap_cnt = 0;
+
+	/* Enable nco increment */
+	writel(DTE_NCO_INC_DEFAULT, ptp_dte->regs + DTE_NCO_INC_REG);
+
+	spin_unlock_irqrestore(&ptp_dte->lock, flags);
+
+	return 0;
+}
+
+static int ptp_dte_enable(struct ptp_clock_info *ptp,
+			    struct ptp_clock_request *rq, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static const struct ptp_clock_info ptp_dte_caps = {
+	.owner		= THIS_MODULE,
+	.name		= "DTE PTP timer",
+	.max_adj	= 50000000,
+	.n_ext_ts	= 0,
+	.n_pins		= 0,
+	.pps		= 0,
+	.adjfine	= ptp_dte_adjfine,
+	.adjtime	= ptp_dte_adjtime,
+	.gettime64	= ptp_dte_gettime,
+	.settime64	= ptp_dte_settime,
+	.enable		= ptp_dte_enable,
+};
+
+static int ptp_dte_probe(struct platform_device *pdev)
+{
+	struct ptp_dte *ptp_dte;
+	struct device *dev = &pdev->dev;
+
+	ptp_dte = devm_kzalloc(dev, sizeof(struct ptp_dte), GFP_KERNEL);
+	if (!ptp_dte)
+		return -ENOMEM;
+
+	ptp_dte->regs = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(ptp_dte->regs))
+		return PTR_ERR(ptp_dte->regs);
+
+	spin_lock_init(&ptp_dte->lock);
+
+	ptp_dte->dev = dev;
+	ptp_dte->caps = ptp_dte_caps;
+	ptp_dte->ptp_clk = ptp_clock_register(&ptp_dte->caps, &pdev->dev);
+	if (IS_ERR(ptp_dte->ptp_clk)) {
+		dev_err(dev,
+			"%s: Failed to register ptp clock\n", __func__);
+		return PTR_ERR(ptp_dte->ptp_clk);
+	}
+
+	platform_set_drvdata(pdev, ptp_dte);
+
+	dev_info(dev, "ptp clk probe done\n");
+
+	return 0;
+}
+
+static void ptp_dte_remove(struct platform_device *pdev)
+{
+	struct ptp_dte *ptp_dte = platform_get_drvdata(pdev);
+	u8 i;
+
+	ptp_clock_unregister(ptp_dte->ptp_clk);
+
+	for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++)
+		writel(0, ptp_dte->regs + (i * sizeof(u32)));
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int ptp_dte_suspend(struct device *dev)
+{
+	struct ptp_dte *ptp_dte = dev_get_drvdata(dev);
+	u8 i;
+
+	for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) {
+		ptp_dte->reg_val[i] =
+			readl(ptp_dte->regs + (i * sizeof(u32)));
+	}
+
+	/* disable the nco */
+	writel(0, ptp_dte->regs + DTE_NCO_INC_REG);
+
+	return 0;
+}
+
+static int ptp_dte_resume(struct device *dev)
+{
+	struct ptp_dte *ptp_dte = dev_get_drvdata(dev);
+	u8 i;
+
+	for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) {
+		if ((i * sizeof(u32)) != DTE_NCO_OVERFLOW_REG)
+			writel(ptp_dte->reg_val[i],
+				(ptp_dte->regs + (i * sizeof(u32))));
+		else
+			writel(((ptp_dte->reg_val[i] &
+				DTE_NCO_SUM3_MASK) << DTE_NCO_SUM3_WR_SHIFT),
+				(ptp_dte->regs + (i * sizeof(u32))));
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops ptp_dte_pm_ops = {
+	.suspend = ptp_dte_suspend,
+	.resume = ptp_dte_resume
+};
+
+#define PTP_DTE_PM_OPS	(&ptp_dte_pm_ops)
+#else
+#define PTP_DTE_PM_OPS	NULL
+#endif
+
+static const struct of_device_id ptp_dte_of_match[] = {
+	{ .compatible = "brcm,ptp-dte", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ptp_dte_of_match);
+
+static struct platform_driver ptp_dte_driver = {
+	.driver = {
+		.name = "ptp-dte",
+		.pm = PTP_DTE_PM_OPS,
+		.of_match_table = ptp_dte_of_match,
+	},
+	.probe    = ptp_dte_probe,
+	.remove   = ptp_dte_remove,
+};
+module_platform_driver(ptp_dte_driver);
+
+MODULE_AUTHOR("Broadcom");
+MODULE_DESCRIPTION("Broadcom DTE PTP Clock driver");
+MODULE_LICENSE("GPL v2");

linux-7.2.3/drivers/ptp/ptp_fc3.h

@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * PTP hardware clock driver for the FemtoClock3 family of timing and
+ * synchronization devices.
+ *
+ * Copyright (C) 2023 Integrated Device Technology, Inc., a Renesas Company.
+ */
+#ifndef PTP_IDTFC3_H
+#define PTP_IDTFC3_H
+
+#include <linux/ktime.h>
+#include <linux/ptp_clock.h>
+#include <linux/regmap.h>
+
+#define FW_FILENAME	"idtfc3.bin"
+
+#define MAX_FFO_PPB	(244000)
+#define TDC_GET_PERIOD	(10)
+
+struct idtfc3 {
+	struct ptp_clock_info	caps;
+	struct ptp_clock	*ptp_clock;
+	struct device		*dev;
+	/* Mutex to protect operations from being interrupted */
+	struct mutex		*lock;
+	struct device		*mfd;
+	struct regmap		*regmap;
+	struct idtfc3_hw_param	hw_param;
+	u32			sub_sync_count;
+	u32			ns_per_sync;
+	int			tdc_offset_sign;
+	u64			tdc_apll_freq;
+	u32			time_ref_freq;
+	u16			fod_n;
+	u8			lpf_mode;
+	/* Time counter */
+	u32			last_counter;
+	s64			ns;
+	u32			ns_per_counter;
+	u32			tc_update_period;
+	u32			tc_write_timeout;
+	s64			tod_write_overhead;
+};
+
+#endif /* PTP_IDTFC3_H */

linux-7.2.3/drivers/ptp/ptp_idt82p33.h

@@ -0,0 +1,114 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * PTP hardware clock driver for the IDT 82P33XXX family of clocks.
+ *
+ * Copyright (C) 2019 Integrated Device Technology, Inc., a Renesas Company.
+ */
+#ifndef PTP_IDT82P33_H
+#define PTP_IDT82P33_H
+
+#include <linux/ktime.h>
+#include <linux/mfd/idt82p33_reg.h>
+#include <linux/regmap.h>
+
+#define FW_FILENAME	"idt82p33xxx.bin"
+#define MAX_PHC_PLL	(2)
+#define MAX_TRIG_CLK	(3)
+#define MAX_PER_OUT	(11)
+#define TOD_BYTE_COUNT	(10)
+#define DCO_MAX_PPB     (92000)
+#define MAX_MEASURMENT_COUNT	(5)
+#define SNAP_THRESHOLD_NS	(10000)
+#define IMMEDIATE_SNAP_THRESHOLD_NS (50000)
+#define DDCO_THRESHOLD_NS	(5)
+#define IDT82P33_MAX_WRITE_COUNT	(512)
+
+#define PLLMASK_ADDR_HI	0xFF
+#define PLLMASK_ADDR_LO	0xA5
+
+#define PLL0_OUTMASK_ADDR_HI	0xFF
+#define PLL0_OUTMASK_ADDR_LO	0xB0
+
+#define PLL1_OUTMASK_ADDR_HI	0xFF
+#define PLL1_OUTMASK_ADDR_LO	0xB2
+
+#define PLL2_OUTMASK_ADDR_HI	0xFF
+#define PLL2_OUTMASK_ADDR_LO	0xB4
+
+#define PLL3_OUTMASK_ADDR_HI	0xFF
+#define PLL3_OUTMASK_ADDR_LO	0xB6
+
+#define DEFAULT_PLL_MASK	(0x01)
+#define DEFAULT_OUTPUT_MASK_PLL0	(0xc0)
+#define DEFAULT_OUTPUT_MASK_PLL1	DEFAULT_OUTPUT_MASK_PLL0
+
+/**
+ * @brief Maximum absolute value for write phase offset in nanoseconds
+ */
+#define WRITE_PHASE_OFFSET_LIMIT (20000l)
+
+/** @brief Phase offset resolution
+ *
+ *  DPLL phase offset = 10^15 fs / ( System Clock  * 2^13)
+ *                    = 10^15 fs / ( 1638400000 * 2^23)
+ *                    = 74.5058059692382 fs
+ */
+#define IDT_T0DPLL_PHASE_RESOL 74506
+
+/* PTP Hardware Clock interface */
+struct idt82p33_channel {
+	struct ptp_clock_info	caps;
+	struct ptp_clock	*ptp_clock;
+	struct idt82p33		*idt82p33;
+	enum pll_mode		pll_mode;
+	/* Workaround for TOD-to-output alignment issue */
+	struct delayed_work	adjtime_work;
+	s32			current_freq;
+	/* double dco mode */
+	bool			ddco;
+	u8			output_mask;
+	/* last input trigger for extts */
+	u8			tod_trigger;
+	bool			discard_next_extts;
+	u8			plln;
+	/* remember last tod_sts for extts */
+	u8			extts_tod_sts[TOD_BYTE_COUNT];
+	u16			dpll_tod_cnfg;
+	u16			dpll_tod_trigger;
+	u16			dpll_tod_sts;
+	u16			dpll_mode_cnfg;
+	u16			dpll_freq_cnfg;
+	u16			dpll_phase_cnfg;
+	u16			dpll_sync_cnfg;
+	u16			dpll_input_mode_cnfg;
+};
+
+struct idt82p33 {
+	struct idt82p33_channel	channel[MAX_PHC_PLL];
+	struct device		*dev;
+	u8			pll_mask;
+	/* Polls for external time stamps */
+	u8			extts_mask;
+	bool			extts_single_shot;
+	struct delayed_work	extts_work;
+	/* Remember the ptp channel to report extts */
+	struct idt82p33_channel	*event_channel[MAX_PHC_PLL];
+	/* Mutex to protect operations from being interrupted */
+	struct mutex		*lock;
+	struct regmap		*regmap;
+	struct device		*mfd;
+	/* Overhead calculation for adjtime */
+	ktime_t			start_time;
+	int			calculate_overhead_flag;
+	s64			tod_write_overhead_ns;
+};
+
+/* firmware interface */
+struct idt82p33_fwrc {
+	u8 hiaddr;
+	u8 loaddr;
+	u8 value;
+	u8 reserved;
+} __packed;
+
+#endif /* PTP_IDT82P33_H */

linux-7.2.3/drivers/ptp/ptp_ines.c

@@ -0,0 +1,814 @@
+// SPDX-License-Identifier: GPL-2.0
+//
+// Copyright (C) 2018 MOSER-BAER AG
+//
+
+#define pr_fmt(fmt) "InES_PTP: " fmt
+
+#include <linux/ethtool.h>
+#include <linux/export.h>
+#include <linux/if_vlan.h>
+#include <linux/mii_timestamper.h>
+#include <linux/module.h>
+#include <linux/net_tstamp.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/phy.h>
+#include <linux/platform_device.h>
+#include <linux/ptp_classify.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/stddef.h>
+
+MODULE_DESCRIPTION("Driver for the ZHAW InES PTP time stamping IP core");
+MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
+MODULE_VERSION("1.0");
+MODULE_LICENSE("GPL");
+
+/* GLOBAL register */
+#define MCAST_MAC_SELECT_SHIFT	2
+#define MCAST_MAC_SELECT_MASK	0x3
+#define IO_RESET		BIT(1)
+#define PTP_RESET		BIT(0)
+
+/* VERSION register */
+#define IF_MAJOR_VER_SHIFT	12
+#define IF_MAJOR_VER_MASK	0xf
+#define IF_MINOR_VER_SHIFT	8
+#define IF_MINOR_VER_MASK	0xf
+#define FPGA_MAJOR_VER_SHIFT	4
+#define FPGA_MAJOR_VER_MASK	0xf
+#define FPGA_MINOR_VER_SHIFT	0
+#define FPGA_MINOR_VER_MASK	0xf
+
+/* INT_STAT register */
+#define RX_INTR_STATUS_3	BIT(5)
+#define RX_INTR_STATUS_2	BIT(4)
+#define RX_INTR_STATUS_1	BIT(3)
+#define TX_INTR_STATUS_3	BIT(2)
+#define TX_INTR_STATUS_2	BIT(1)
+#define TX_INTR_STATUS_1	BIT(0)
+
+/* INT_MSK register */
+#define RX_INTR_MASK_3		BIT(5)
+#define RX_INTR_MASK_2		BIT(4)
+#define RX_INTR_MASK_1		BIT(3)
+#define TX_INTR_MASK_3		BIT(2)
+#define TX_INTR_MASK_2		BIT(1)
+#define TX_INTR_MASK_1		BIT(0)
+
+/* BUF_STAT register */
+#define RX_FIFO_NE_3		BIT(5)
+#define RX_FIFO_NE_2		BIT(4)
+#define RX_FIFO_NE_1		BIT(3)
+#define TX_FIFO_NE_3		BIT(2)
+#define TX_FIFO_NE_2		BIT(1)
+#define TX_FIFO_NE_1		BIT(0)
+
+/* PORT_CONF register */
+#define CM_ONE_STEP		BIT(6)
+#define PHY_SPEED_SHIFT		4
+#define PHY_SPEED_MASK		0x3
+#define P2P_DELAY_WR_POS_SHIFT	2
+#define P2P_DELAY_WR_POS_MASK	0x3
+#define PTP_MODE_SHIFT		0
+#define PTP_MODE_MASK		0x3
+
+/* TS_STAT_TX register */
+#define TS_ENABLE		BIT(15)
+#define DATA_READ_POS_SHIFT	8
+#define DATA_READ_POS_MASK	0x1f
+#define DISCARDED_EVENTS_SHIFT	4
+#define DISCARDED_EVENTS_MASK	0xf
+
+#define INES_N_PORTS		3
+#define INES_REGISTER_SIZE	0x80
+#define INES_PORT_OFFSET	0x20
+#define INES_PORT_SIZE		0x20
+#define INES_FIFO_DEPTH		90
+#define INES_MAX_EVENTS		100
+
+#define BC_PTP_V1		0
+#define BC_PTP_V2		1
+#define TC_E2E_PTP_V2		2
+#define TC_P2P_PTP_V2		3
+
+#define PHY_SPEED_10		0
+#define PHY_SPEED_100		1
+#define PHY_SPEED_1000		2
+
+#define PORT_CONF \
+	((PHY_SPEED_1000 << PHY_SPEED_SHIFT) | (BC_PTP_V2 << PTP_MODE_SHIFT))
+
+#define ines_read32(s, r)	__raw_readl((void __iomem *)&s->regs->r)
+#define ines_write32(s, v, r)	__raw_writel(v, (void __iomem *)&s->regs->r)
+
+#define MESSAGE_TYPE_SYNC		1
+#define MESSAGE_TYPE_P_DELAY_REQ	2
+#define MESSAGE_TYPE_P_DELAY_RESP	3
+#define MESSAGE_TYPE_DELAY_REQ		4
+
+static LIST_HEAD(ines_clocks);
+static DEFINE_MUTEX(ines_clocks_lock);
+
+struct ines_global_regs {
+	u32 id;
+	u32 test;
+	u32 global;
+	u32 version;
+	u32 test2;
+	u32 int_stat;
+	u32 int_msk;
+	u32 buf_stat;
+};
+
+struct ines_port_registers {
+	u32 port_conf;
+	u32 p_delay;
+	u32 ts_stat_tx;
+	u32 ts_stat_rx;
+	u32 ts_tx;
+	u32 ts_rx;
+};
+
+struct ines_timestamp {
+	struct list_head list;
+	unsigned long	tmo;
+	u16		tag;
+	u64		sec;
+	u64		nsec;
+	u64		clkid;
+	u16		portnum;
+	u16		seqid;
+};
+
+struct ines_port {
+	struct ines_port_registers	*regs;
+	struct mii_timestamper		mii_ts;
+	struct ines_clock		*clock;
+	bool				rxts_enabled;
+	bool				txts_enabled;
+	unsigned int			index;
+	struct delayed_work		ts_work;
+	/* lock protects event list and tx_skb */
+	spinlock_t			lock;
+	struct sk_buff			*tx_skb;
+	struct list_head		events;
+	struct list_head		pool;
+	struct ines_timestamp		pool_data[INES_MAX_EVENTS];
+};
+
+struct ines_clock {
+	struct ines_port		port[INES_N_PORTS];
+	struct ines_global_regs __iomem	*regs;
+	void __iomem			*base;
+	struct device_node		*node;
+	struct device			*dev;
+	struct list_head		list;
+};
+
+static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
+		       struct ines_timestamp *ts, struct device *dev);
+static int ines_rxfifo_read(struct ines_port *port);
+static u64 ines_rxts64(struct ines_port *port, unsigned int words);
+static bool ines_timestamp_expired(struct ines_timestamp *ts);
+static u64 ines_txts64(struct ines_port *port, unsigned int words);
+static void ines_txtstamp_work(struct work_struct *work);
+static bool is_sync_pdelay_resp(struct sk_buff *skb, int type);
+static u8 tag_to_msgtype(u8 tag);
+
+static void ines_clock_cleanup(struct ines_clock *clock)
+{
+	struct ines_port *port;
+	int i;
+
+	for (i = 0; i < INES_N_PORTS; i++) {
+		port = &clock->port[i];
+		cancel_delayed_work_sync(&port->ts_work);
+	}
+}
+
+static int ines_clock_init(struct ines_clock *clock, struct device *device,
+			   void __iomem *addr)
+{
+	struct device_node *node = device->of_node;
+	unsigned long port_addr;
+	struct ines_port *port;
+	int i, j;
+
+	INIT_LIST_HEAD(&clock->list);
+	clock->node = node;
+	clock->dev  = device;
+	clock->base = addr;
+	clock->regs = clock->base;
+
+	for (i = 0; i < INES_N_PORTS; i++) {
+		port = &clock->port[i];
+		port_addr = (unsigned long) clock->base +
+			INES_PORT_OFFSET + i * INES_PORT_SIZE;
+		port->regs = (struct ines_port_registers *) port_addr;
+		port->clock = clock;
+		port->index = i;
+		INIT_DELAYED_WORK(&port->ts_work, ines_txtstamp_work);
+		spin_lock_init(&port->lock);
+		INIT_LIST_HEAD(&port->events);
+		INIT_LIST_HEAD(&port->pool);
+		for (j = 0; j < INES_MAX_EVENTS; j++)
+			list_add(&port->pool_data[j].list, &port->pool);
+	}
+
+	ines_write32(clock, 0xBEEF, test);
+	ines_write32(clock, 0xBEEF, test2);
+
+	dev_dbg(device, "ID      0x%x\n", ines_read32(clock, id));
+	dev_dbg(device, "TEST    0x%x\n", ines_read32(clock, test));
+	dev_dbg(device, "VERSION 0x%x\n", ines_read32(clock, version));
+	dev_dbg(device, "TEST2   0x%x\n", ines_read32(clock, test2));
+
+	for (i = 0; i < INES_N_PORTS; i++) {
+		port = &clock->port[i];
+		ines_write32(port, PORT_CONF, port_conf);
+	}
+
+	return 0;
+}
+
+static struct ines_port *ines_find_port(struct device_node *node, u32 index)
+{
+	struct ines_port *port = NULL;
+	struct ines_clock *clock;
+	struct list_head *this;
+
+	mutex_lock(&ines_clocks_lock);
+	list_for_each(this, &ines_clocks) {
+		clock = list_entry(this, struct ines_clock, list);
+		if (clock->node == node) {
+			port = &clock->port[index];
+			break;
+		}
+	}
+	mutex_unlock(&ines_clocks_lock);
+	return port;
+}
+
+static u64 ines_find_rxts(struct ines_port *port, struct sk_buff *skb, int type)
+{
+	struct list_head *this, *next;
+	struct ines_timestamp *ts;
+	unsigned long flags;
+	u64 ns = 0;
+
+	if (type == PTP_CLASS_NONE)
+		return 0;
+
+	spin_lock_irqsave(&port->lock, flags);
+	ines_rxfifo_read(port);
+	list_for_each_safe(this, next, &port->events) {
+		ts = list_entry(this, struct ines_timestamp, list);
+		if (ines_timestamp_expired(ts)) {
+			list_del_init(&ts->list);
+			list_add(&ts->list, &port->pool);
+			continue;
+		}
+		if (ines_match(skb, type, ts, port->clock->dev)) {
+			ns = ts->sec * 1000000000ULL + ts->nsec;
+			list_del_init(&ts->list);
+			list_add(&ts->list, &port->pool);
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	return ns;
+}
+
+static u64 ines_find_txts(struct ines_port *port, struct sk_buff *skb)
+{
+	unsigned int class = ptp_classify_raw(skb), i;
+	u32 data_rd_pos, buf_stat, mask, ts_stat_tx;
+	struct ines_timestamp ts;
+	unsigned long flags;
+	u64 ns = 0;
+
+	mask = TX_FIFO_NE_1 << port->index;
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	for (i = 0; i < INES_FIFO_DEPTH; i++) {
+
+		buf_stat = ines_read32(port->clock, buf_stat);
+		if (!(buf_stat & mask)) {
+			dev_dbg(port->clock->dev,
+				  "Tx timestamp FIFO unexpectedly empty\n");
+			break;
+		}
+		ts_stat_tx = ines_read32(port, ts_stat_tx);
+		data_rd_pos = (ts_stat_tx >> DATA_READ_POS_SHIFT) &
+			DATA_READ_POS_MASK;
+		if (data_rd_pos) {
+			dev_err(port->clock->dev,
+				"unexpected Tx read pos %u\n", data_rd_pos);
+			break;
+		}
+
+		ts.tag     = ines_read32(port, ts_tx);
+		ts.sec     = ines_txts64(port, 3);
+		ts.nsec    = ines_txts64(port, 2);
+		ts.clkid   = ines_txts64(port, 4);
+		ts.portnum = ines_read32(port, ts_tx);
+		ts.seqid   = ines_read32(port, ts_tx);
+
+		if (ines_match(skb, class, &ts, port->clock->dev)) {
+			ns = ts.sec * 1000000000ULL + ts.nsec;
+			break;
+		}
+	}
+
+	spin_unlock_irqrestore(&port->lock, flags);
+	return ns;
+}
+
+static int ines_hwtstamp_get(struct mii_timestamper *mii_ts,
+			     struct kernel_hwtstamp_config *cfg)
+{
+	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
+	unsigned long flags;
+	u32 port_conf;
+
+	cfg->rx_filter = port->rxts_enabled ? HWTSTAMP_FILTER_PTP_V2_EVENT
+					    : HWTSTAMP_FILTER_NONE;
+	if (port->txts_enabled) {
+		spin_lock_irqsave(&port->lock, flags);
+		port_conf = ines_read32(port, port_conf);
+		spin_unlock_irqrestore(&port->lock, flags);
+		cfg->tx_type = (port_conf & CM_ONE_STEP) ? HWTSTAMP_TX_ONESTEP_P2P
+							 : HWTSTAMP_TX_OFF;
+	} else {
+		cfg->tx_type = HWTSTAMP_TX_OFF;
+	}
+
+	return 0;
+}
+
+static int ines_hwtstamp_set(struct mii_timestamper *mii_ts,
+			     struct kernel_hwtstamp_config *cfg,
+			     struct netlink_ext_ack *extack)
+{
+	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
+	u32 cm_one_step = 0, port_conf, ts_stat_rx, ts_stat_tx;
+	unsigned long flags;
+
+	switch (cfg->tx_type) {
+	case HWTSTAMP_TX_OFF:
+		ts_stat_tx = 0;
+		break;
+	case HWTSTAMP_TX_ON:
+		ts_stat_tx = TS_ENABLE;
+		break;
+	case HWTSTAMP_TX_ONESTEP_P2P:
+		ts_stat_tx = TS_ENABLE;
+		cm_one_step = CM_ONE_STEP;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	switch (cfg->rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		ts_stat_rx = 0;
+		break;
+	case HWTSTAMP_FILTER_ALL:
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+		return -ERANGE;
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+		ts_stat_rx = TS_ENABLE;
+		cfg->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	port_conf = ines_read32(port, port_conf);
+	port_conf &= ~CM_ONE_STEP;
+	port_conf |= cm_one_step;
+
+	ines_write32(port, port_conf, port_conf);
+	ines_write32(port, ts_stat_rx, ts_stat_rx);
+	ines_write32(port, ts_stat_tx, ts_stat_tx);
+
+	port->rxts_enabled = ts_stat_rx == TS_ENABLE;
+	port->txts_enabled = ts_stat_tx == TS_ENABLE;
+
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	return 0;
+}
+
+static void ines_link_state(struct mii_timestamper *mii_ts,
+			    struct phy_device *phydev)
+{
+	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
+	u32 port_conf, speed_conf;
+	unsigned long flags;
+
+	switch (phydev->speed) {
+	case SPEED_10:
+		speed_conf = PHY_SPEED_10 << PHY_SPEED_SHIFT;
+		break;
+	case SPEED_100:
+		speed_conf = PHY_SPEED_100 << PHY_SPEED_SHIFT;
+		break;
+	case SPEED_1000:
+		speed_conf = PHY_SPEED_1000 << PHY_SPEED_SHIFT;
+		break;
+	default:
+		dev_err(port->clock->dev, "bad speed: %d\n", phydev->speed);
+		return;
+	}
+	spin_lock_irqsave(&port->lock, flags);
+
+	port_conf = ines_read32(port, port_conf);
+	port_conf &= ~(0x3 << PHY_SPEED_SHIFT);
+	port_conf |= speed_conf;
+
+	ines_write32(port, port_conf, port_conf);
+
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
+		       struct ines_timestamp *ts, struct device *dev)
+{
+	struct ptp_header *hdr;
+	u16 portn, seqid;
+	u8 msgtype;
+	u64 clkid;
+
+	if (unlikely(ptp_class & PTP_CLASS_V1))
+		return false;
+
+	hdr = ptp_parse_header(skb, ptp_class);
+	if (!hdr)
+		return false;
+
+	msgtype = ptp_get_msgtype(hdr, ptp_class);
+	clkid = be64_to_cpup((__be64 *)&hdr->source_port_identity.clock_identity.id[0]);
+	portn = be16_to_cpu(hdr->source_port_identity.port_number);
+	seqid = be16_to_cpu(hdr->sequence_id);
+
+	if (tag_to_msgtype(ts->tag & 0x7) != msgtype) {
+		dev_dbg(dev, "msgtype mismatch ts %hhu != skb %hhu\n",
+			tag_to_msgtype(ts->tag & 0x7), msgtype);
+		return false;
+	}
+	if (ts->clkid != clkid) {
+		dev_dbg(dev, "clkid mismatch ts %llx != skb %llx\n",
+			ts->clkid, clkid);
+		return false;
+	}
+	if (ts->portnum != portn) {
+		dev_dbg(dev, "portn mismatch ts %hu != skb %hu\n",
+			ts->portnum, portn);
+		return false;
+	}
+	if (ts->seqid != seqid) {
+		dev_dbg(dev, "seqid mismatch ts %hu != skb %hu\n",
+			ts->seqid, seqid);
+		return false;
+	}
+
+	return true;
+}
+
+static bool ines_rxtstamp(struct mii_timestamper *mii_ts,
+			  struct sk_buff *skb, int type)
+{
+	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
+	struct skb_shared_hwtstamps *ssh;
+	u64 ns;
+
+	if (!port->rxts_enabled)
+		return false;
+
+	ns = ines_find_rxts(port, skb, type);
+	if (!ns)
+		return false;
+
+	ssh = skb_hwtstamps(skb);
+	ssh->hwtstamp = ns_to_ktime(ns);
+	netif_rx(skb);
+
+	return true;
+}
+
+static int ines_rxfifo_read(struct ines_port *port)
+{
+	u32 data_rd_pos, buf_stat, mask, ts_stat_rx;
+	struct ines_timestamp *ts;
+	unsigned int i;
+
+	mask = RX_FIFO_NE_1 << port->index;
+
+	for (i = 0; i < INES_FIFO_DEPTH; i++) {
+		if (list_empty(&port->pool)) {
+			dev_err(port->clock->dev, "event pool is empty\n");
+			return -1;
+		}
+		buf_stat = ines_read32(port->clock, buf_stat);
+		if (!(buf_stat & mask))
+			break;
+
+		ts_stat_rx = ines_read32(port, ts_stat_rx);
+		data_rd_pos = (ts_stat_rx >> DATA_READ_POS_SHIFT) &
+			DATA_READ_POS_MASK;
+		if (data_rd_pos) {
+			dev_err(port->clock->dev, "unexpected Rx read pos %u\n",
+				data_rd_pos);
+			break;
+		}
+
+		ts = list_first_entry(&port->pool, struct ines_timestamp, list);
+		ts->tmo     = jiffies + HZ;
+		ts->tag     = ines_read32(port, ts_rx);
+		ts->sec     = ines_rxts64(port, 3);
+		ts->nsec    = ines_rxts64(port, 2);
+		ts->clkid   = ines_rxts64(port, 4);
+		ts->portnum = ines_read32(port, ts_rx);
+		ts->seqid   = ines_read32(port, ts_rx);
+
+		list_del_init(&ts->list);
+		list_add_tail(&ts->list, &port->events);
+	}
+
+	return 0;
+}
+
+static u64 ines_rxts64(struct ines_port *port, unsigned int words)
+{
+	unsigned int i;
+	u64 result;
+	u16 word;
+
+	word = ines_read32(port, ts_rx);
+	result = word;
+	words--;
+	for (i = 0; i < words; i++) {
+		word = ines_read32(port, ts_rx);
+		result <<= 16;
+		result |= word;
+	}
+	return result;
+}
+
+static bool ines_timestamp_expired(struct ines_timestamp *ts)
+{
+	return time_after(jiffies, ts->tmo);
+}
+
+static int ines_ts_info(struct mii_timestamper *mii_ts,
+			struct kernel_ethtool_ts_info *info)
+{
+	info->so_timestamping =
+		SOF_TIMESTAMPING_TX_HARDWARE |
+		SOF_TIMESTAMPING_TX_SOFTWARE |
+		SOF_TIMESTAMPING_RX_HARDWARE |
+		SOF_TIMESTAMPING_RAW_HARDWARE;
+
+	info->tx_types =
+		(1 << HWTSTAMP_TX_OFF) |
+		(1 << HWTSTAMP_TX_ON) |
+		(1 << HWTSTAMP_TX_ONESTEP_P2P);
+
+	info->rx_filters =
+		(1 << HWTSTAMP_FILTER_NONE) |
+		(1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
+
+	return 0;
+}
+
+static u64 ines_txts64(struct ines_port *port, unsigned int words)
+{
+	unsigned int i;
+	u64 result;
+	u16 word;
+
+	word = ines_read32(port, ts_tx);
+	result = word;
+	words--;
+	for (i = 0; i < words; i++) {
+		word = ines_read32(port, ts_tx);
+		result <<= 16;
+		result |= word;
+	}
+	return result;
+}
+
+static bool ines_txts_onestep(struct ines_port *port, struct sk_buff *skb, int type)
+{
+	unsigned long flags;
+	u32 port_conf;
+
+	spin_lock_irqsave(&port->lock, flags);
+	port_conf = ines_read32(port, port_conf);
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	if (port_conf & CM_ONE_STEP)
+		return is_sync_pdelay_resp(skb, type);
+
+	return false;
+}
+
+static void ines_txtstamp(struct mii_timestamper *mii_ts,
+			  struct sk_buff *skb, int type)
+{
+	struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
+	struct sk_buff *old_skb = NULL;
+	unsigned long flags;
+
+	if (!port->txts_enabled || ines_txts_onestep(port, skb, type)) {
+		kfree_skb(skb);
+		return;
+	}
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	if (port->tx_skb)
+		old_skb = port->tx_skb;
+
+	port->tx_skb = skb;
+
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	kfree_skb(old_skb);
+
+	schedule_delayed_work(&port->ts_work, 1);
+}
+
+static void ines_txtstamp_work(struct work_struct *work)
+{
+	struct ines_port *port =
+		container_of(work, struct ines_port, ts_work.work);
+	struct skb_shared_hwtstamps ssh;
+	struct sk_buff *skb;
+	unsigned long flags;
+	u64 ns;
+
+	spin_lock_irqsave(&port->lock, flags);
+	skb = port->tx_skb;
+	port->tx_skb = NULL;
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	ns = ines_find_txts(port, skb);
+	if (!ns) {
+		kfree_skb(skb);
+		return;
+	}
+	ssh.hwtstamp = ns_to_ktime(ns);
+	skb_complete_tx_timestamp(skb, &ssh);
+}
+
+static bool is_sync_pdelay_resp(struct sk_buff *skb, int type)
+{
+	struct ptp_header *hdr;
+	u8 msgtype;
+
+	hdr = ptp_parse_header(skb, type);
+	if (!hdr)
+		return false;
+
+	msgtype = ptp_get_msgtype(hdr, type);
+
+	switch (msgtype) {
+	case PTP_MSGTYPE_SYNC:
+	case PTP_MSGTYPE_PDELAY_RESP:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static u8 tag_to_msgtype(u8 tag)
+{
+	switch (tag) {
+	case MESSAGE_TYPE_SYNC:
+		return PTP_MSGTYPE_SYNC;
+	case MESSAGE_TYPE_P_DELAY_REQ:
+		return PTP_MSGTYPE_PDELAY_REQ;
+	case MESSAGE_TYPE_P_DELAY_RESP:
+		return PTP_MSGTYPE_PDELAY_RESP;
+	case MESSAGE_TYPE_DELAY_REQ:
+		return PTP_MSGTYPE_DELAY_REQ;
+	}
+	return 0xf;
+}
+
+static struct mii_timestamper *ines_ptp_probe_channel(struct device *device,
+						      unsigned int index)
+{
+	struct device_node *node = device->of_node;
+	struct ines_port *port;
+
+	if (index > INES_N_PORTS - 1) {
+		dev_err(device, "bad port index %u\n", index);
+		return ERR_PTR(-EINVAL);
+	}
+	port = ines_find_port(node, index);
+	if (!port) {
+		dev_err(device, "missing port index %u\n", index);
+		return ERR_PTR(-ENODEV);
+	}
+	port->mii_ts.rxtstamp = ines_rxtstamp;
+	port->mii_ts.txtstamp = ines_txtstamp;
+	port->mii_ts.hwtstamp_set = ines_hwtstamp_set;
+	port->mii_ts.hwtstamp_get = ines_hwtstamp_get;
+	port->mii_ts.link_state = ines_link_state;
+	port->mii_ts.ts_info = ines_ts_info;
+
+	return &port->mii_ts;
+}
+
+static void ines_ptp_release_channel(struct device *device,
+				     struct mii_timestamper *mii_ts)
+{
+}
+
+static struct mii_timestamping_ctrl ines_ctrl = {
+	.probe_channel = ines_ptp_probe_channel,
+	.release_channel = ines_ptp_release_channel,
+};
+
+static int ines_ptp_ctrl_probe(struct platform_device *pld)
+{
+	struct ines_clock *clock;
+	void __iomem *addr;
+	int err = 0;
+
+	addr = devm_platform_ioremap_resource(pld, 0);
+	if (IS_ERR(addr)) {
+		err = PTR_ERR(addr);
+		goto out;
+	}
+	clock = kzalloc_obj(*clock);
+	if (!clock) {
+		err = -ENOMEM;
+		goto out;
+	}
+	if (ines_clock_init(clock, &pld->dev, addr)) {
+		kfree(clock);
+		err = -ENOMEM;
+		goto out;
+	}
+	err = register_mii_tstamp_controller(&pld->dev, &ines_ctrl);
+	if (err) {
+		kfree(clock);
+		goto out;
+	}
+	mutex_lock(&ines_clocks_lock);
+	list_add_tail(&ines_clocks, &clock->list);
+	mutex_unlock(&ines_clocks_lock);
+
+	dev_set_drvdata(&pld->dev, clock);
+out:
+	return err;
+}
+
+static void ines_ptp_ctrl_remove(struct platform_device *pld)
+{
+	struct ines_clock *clock = dev_get_drvdata(&pld->dev);
+
+	unregister_mii_tstamp_controller(&pld->dev);
+	mutex_lock(&ines_clocks_lock);
+	list_del(&clock->list);
+	mutex_unlock(&ines_clocks_lock);
+	ines_clock_cleanup(clock);
+	kfree(clock);
+}
+
+static const struct of_device_id ines_ptp_ctrl_of_match[] = {
+	{ .compatible = "ines,ptp-ctrl" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match);
+
+static struct platform_driver ines_ptp_ctrl_driver = {
+	.probe  = ines_ptp_ctrl_probe,
+	.remove = ines_ptp_ctrl_remove,
+	.driver = {
+		.name = "ines_ptp_ctrl",
+		.of_match_table = ines_ptp_ctrl_of_match,
+	},
+};
+module_platform_driver(ines_ptp_ctrl_driver);

linux-7.2.3/drivers/ptp/ptp_kvm_arm.c

@@ -0,0 +1,32 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ *  Virtual PTP 1588 clock for use with KVM guests
+ *  Copyright (C) 2019 ARM Ltd.
+ *  All Rights Reserved
+ */
+
+#include <linux/arm-smccc.h>
+#include <linux/ptp_kvm.h>
+
+#include <asm/arch_timer.h>
+#include <asm/hypervisor.h>
+
+int kvm_arch_ptp_init(void)
+{
+	int ret;
+
+	ret = kvm_arm_hyp_service_available(ARM_SMCCC_KVM_FUNC_PTP);
+	if (ret <= 0)
+		return -EOPNOTSUPP;
+
+	return 0;
+}
+
+void kvm_arch_ptp_exit(void)
+{
+}
+
+int kvm_arch_ptp_get_clock(struct timespec64 *ts)
+{
+	return kvm_arch_ptp_get_crosststamp(NULL, ts, NULL);
+}

linux-7.2.3/drivers/ptp/ptp_kvm_common.c

@@ -0,0 +1,159 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Virtual PTP 1588 clock for use with KVM guests
+ *
+ * Copyright (C) 2017 Red Hat Inc.
+ */
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/ptp_kvm.h>
+#include <uapi/linux/kvm_para.h>
+#include <asm/kvm_para.h>
+#include <uapi/asm/kvm_para.h>
+
+#include <linux/ptp_clock_kernel.h>
+
+struct kvm_ptp_clock {
+	struct ptp_clock *ptp_clock;
+	struct ptp_clock_info caps;
+};
+
+static DEFINE_SPINLOCK(kvm_ptp_lock);
+
+static int ptp_kvm_get_time_fn(ktime_t *device_time,
+			       struct system_counterval_t *system_counter,
+			       void *ctx)
+{
+	enum clocksource_ids cs_id;
+	struct timespec64 tspec;
+	u64 cycle;
+	int ret;
+
+	spin_lock(&kvm_ptp_lock);
+
+	preempt_disable_notrace();
+	ret = kvm_arch_ptp_get_crosststamp(&cycle, &tspec, &cs_id);
+	if (ret) {
+		spin_unlock(&kvm_ptp_lock);
+		preempt_enable_notrace();
+		return ret;
+	}
+
+	preempt_enable_notrace();
+
+	system_counter->cycles = cycle;
+	system_counter->cs_id = cs_id;
+
+	*device_time = timespec64_to_ktime(tspec);
+
+	spin_unlock(&kvm_ptp_lock);
+
+	return 0;
+}
+
+static int ptp_kvm_getcrosststamp(struct ptp_clock_info *ptp,
+				  struct system_device_crosststamp *xtstamp)
+{
+	return get_device_system_crosststamp(ptp_kvm_get_time_fn, NULL,
+					     NULL, xtstamp);
+}
+
+/*
+ * PTP clock operations
+ */
+
+static int ptp_kvm_adjfine(struct ptp_clock_info *ptp, long delta)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ptp_kvm_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ptp_kvm_settime(struct ptp_clock_info *ptp,
+			   const struct timespec64 *ts)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ptp_kvm_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	long ret;
+	struct timespec64 tspec;
+
+	spin_lock(&kvm_ptp_lock);
+
+	ret = kvm_arch_ptp_get_clock(&tspec);
+	if (ret) {
+		spin_unlock(&kvm_ptp_lock);
+		return ret;
+	}
+
+	spin_unlock(&kvm_ptp_lock);
+
+	memcpy(ts, &tspec, sizeof(struct timespec64));
+
+	return 0;
+}
+
+static int ptp_kvm_enable(struct ptp_clock_info *ptp,
+			  struct ptp_clock_request *rq, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static const struct ptp_clock_info ptp_kvm_caps = {
+	.owner		= THIS_MODULE,
+	.name		= "KVM virtual PTP",
+	.max_adj	= 0,
+	.n_ext_ts	= 0,
+	.n_pins		= 0,
+	.pps		= 0,
+	.adjfine	= ptp_kvm_adjfine,
+	.adjtime	= ptp_kvm_adjtime,
+	.gettime64	= ptp_kvm_gettime,
+	.settime64	= ptp_kvm_settime,
+	.enable		= ptp_kvm_enable,
+	.getcrosststamp = ptp_kvm_getcrosststamp,
+};
+
+/* module operations */
+
+static struct kvm_ptp_clock kvm_ptp_clock;
+
+static void __exit ptp_kvm_exit(void)
+{
+	ptp_clock_unregister(kvm_ptp_clock.ptp_clock);
+	kvm_arch_ptp_exit();
+}
+
+static int __init ptp_kvm_init(void)
+{
+	long ret;
+
+	ret = kvm_arch_ptp_init();
+	if (ret) {
+		if (ret != -EOPNOTSUPP)
+			pr_err("fail to initialize ptp_kvm");
+		return ret;
+	}
+
+	kvm_ptp_clock.caps = ptp_kvm_caps;
+
+	kvm_ptp_clock.ptp_clock = ptp_clock_register(&kvm_ptp_clock.caps, NULL);
+
+	return PTR_ERR_OR_ZERO(kvm_ptp_clock.ptp_clock);
+}
+
+module_init(ptp_kvm_init);
+module_exit(ptp_kvm_exit);
+
+MODULE_AUTHOR("Marcelo Tosatti <mtosatti@redhat.com>");
+MODULE_DESCRIPTION("PTP clock using KVMCLOCK");
+MODULE_LICENSE("GPL");

linux-7.2.3/drivers/ptp/ptp_kvm_x86.c

@@ -0,0 +1,129 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Virtual PTP 1588 clock for use with KVM guests
+ *
+ * Copyright (C) 2017 Red Hat Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <asm/pvclock.h>
+#include <asm/kvmclock.h>
+#include <linux/module.h>
+#include <uapi/asm/kvm_para.h>
+#include <uapi/linux/kvm_para.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/ptp_kvm.h>
+#include <linux/set_memory.h>
+
+static phys_addr_t clock_pair_gpa;
+static struct kvm_clock_pairing clock_pair_glbl;
+static struct kvm_clock_pairing *clock_pair;
+
+int kvm_arch_ptp_init(void)
+{
+	struct page *p;
+	long ret;
+
+	if (!kvm_para_available())
+		return -EOPNOTSUPP;
+
+	if (cc_platform_has(CC_ATTR_GUEST_MEM_ENCRYPT)) {
+		p = alloc_page(GFP_KERNEL | __GFP_ZERO);
+		if (!p)
+			return -ENOMEM;
+
+		clock_pair = page_address(p);
+		ret = set_memory_decrypted((unsigned long)clock_pair, 1);
+		if (ret) {
+			__free_page(p);
+			clock_pair = NULL;
+			goto nofree;
+		}
+	} else {
+		clock_pair = &clock_pair_glbl;
+	}
+
+	clock_pair_gpa = slow_virt_to_phys(clock_pair);
+	if (!pvclock_get_pvti_cpu0_va()) {
+		ret = -EOPNOTSUPP;
+		goto err;
+	}
+
+	ret = kvm_hypercall2(KVM_HC_CLOCK_PAIRING, clock_pair_gpa,
+			     KVM_CLOCK_PAIRING_WALLCLOCK);
+	if (ret == -KVM_ENOSYS) {
+		ret = -EOPNOTSUPP;
+		goto err;
+	}
+
+	return ret;
+
+err:
+	kvm_arch_ptp_exit();
+nofree:
+	return ret;
+}
+
+void kvm_arch_ptp_exit(void)
+{
+	if (cc_platform_has(CC_ATTR_GUEST_MEM_ENCRYPT)) {
+		WARN_ON(set_memory_encrypted((unsigned long)clock_pair, 1));
+		free_page((unsigned long)clock_pair);
+		clock_pair = NULL;
+	}
+}
+
+int kvm_arch_ptp_get_clock(struct timespec64 *ts)
+{
+	long ret;
+
+	ret = kvm_hypercall2(KVM_HC_CLOCK_PAIRING,
+			     clock_pair_gpa,
+			     KVM_CLOCK_PAIRING_WALLCLOCK);
+	if (ret != 0) {
+		pr_err_ratelimited("clock offset hypercall ret %lu\n", ret);
+		return -EOPNOTSUPP;
+	}
+
+	ts->tv_sec = clock_pair->sec;
+	ts->tv_nsec = clock_pair->nsec;
+
+	return 0;
+}
+
+int kvm_arch_ptp_get_crosststamp(u64 *cycle, struct timespec64 *tspec,
+			      enum clocksource_ids *cs_id)
+{
+	struct pvclock_vcpu_time_info *src;
+	unsigned int version;
+	long ret;
+
+	src = this_cpu_pvti();
+
+	do {
+		/*
+		 * We are using a TSC value read in the hosts
+		 * kvm_hc_clock_pairing handling.
+		 * So any changes to tsc_to_system_mul
+		 * and tsc_shift or any other pvclock
+		 * data invalidate that measurement.
+		 */
+		version = pvclock_read_begin(src);
+
+		ret = kvm_hypercall2(KVM_HC_CLOCK_PAIRING,
+				     clock_pair_gpa,
+				     KVM_CLOCK_PAIRING_WALLCLOCK);
+		if (ret != 0) {
+			pr_err_ratelimited("clock pairing hypercall ret %lu\n", ret);
+			return -EOPNOTSUPP;
+		}
+		tspec->tv_sec = clock_pair->sec;
+		tspec->tv_nsec = clock_pair->nsec;
+		*cycle = __pvclock_read_cycles(src, clock_pair->tsc);
+	} while (pvclock_read_retry(src, version));
+
+	*cs_id = CSID_X86_KVM_CLK;
+
+	return 0;
+}

linux-7.2.3/drivers/ptp/ptp_mock.c

@@ -0,0 +1,175 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright 2023 NXP
+ *
+ * Mock-up PTP Hardware Clock driver for virtual network devices
+ *
+ * Create a PTP clock which offers PTP time manipulation operations
+ * using a timecounter/cyclecounter on top of CLOCK_MONOTONIC_RAW.
+ */
+
+#include <linux/ptp_clock_kernel.h>
+#include <linux/ptp_mock.h>
+#include <linux/timecounter.h>
+
+/* Clamp scaled_ppm between -2,097,152,000 and 2,097,152,000,
+ * and thus "adj" between -68,719,476 and 68,719,476
+ */
+#define MOCK_PHC_MAX_ADJ_PPB		32000000
+/* Timestamps from ktime_get_raw() have 1 ns resolution, so the scale factor
+ * (MULT >> SHIFT) needs to be 1. Pick SHIFT as 31 bits, which translates
+ * MULT(freq 0) into 0x80000000.
+ */
+#define MOCK_PHC_CC_SHIFT		31
+#define MOCK_PHC_CC_MULT		(1 << MOCK_PHC_CC_SHIFT)
+#define MOCK_PHC_FADJ_SHIFT		9
+#define MOCK_PHC_FADJ_DENOMINATOR	15625ULL
+
+/* The largest cycle_delta that timecounter_read_delta() can handle without a
+ * 64-bit overflow during the multiplication with cc->mult, given the max "adj"
+ * we permit, is ~8.3 seconds. Make sure readouts are more frequent than that.
+ */
+#define MOCK_PHC_REFRESH_INTERVAL	(HZ * 5)
+
+#define info_to_phc(d) container_of((d), struct mock_phc, info)
+
+struct mock_phc {
+	struct ptp_clock_info info;
+	struct ptp_clock *clock;
+	struct timecounter tc;
+	struct cyclecounter cc;
+	spinlock_t lock;
+};
+
+static u64 mock_phc_cc_read(struct cyclecounter *cc)
+{
+	return ktime_get_raw_ns();
+}
+
+static int mock_phc_adjfine(struct ptp_clock_info *info, long scaled_ppm)
+{
+	struct mock_phc *phc = info_to_phc(info);
+	s64 adj;
+
+	adj = (s64)scaled_ppm << MOCK_PHC_FADJ_SHIFT;
+	adj = div_s64(adj, MOCK_PHC_FADJ_DENOMINATOR);
+
+	spin_lock(&phc->lock);
+	timecounter_read(&phc->tc);
+	phc->cc.mult = MOCK_PHC_CC_MULT + adj;
+	spin_unlock(&phc->lock);
+
+	return 0;
+}
+
+static int mock_phc_adjtime(struct ptp_clock_info *info, s64 delta)
+{
+	struct mock_phc *phc = info_to_phc(info);
+
+	spin_lock(&phc->lock);
+	timecounter_adjtime(&phc->tc, delta);
+	spin_unlock(&phc->lock);
+
+	return 0;
+}
+
+static int mock_phc_settime64(struct ptp_clock_info *info,
+			      const struct timespec64 *ts)
+{
+	struct mock_phc *phc = info_to_phc(info);
+	u64 ns = timespec64_to_ns(ts);
+
+	spin_lock(&phc->lock);
+	timecounter_init(&phc->tc, &phc->cc, ns);
+	spin_unlock(&phc->lock);
+
+	return 0;
+}
+
+static int mock_phc_gettime64(struct ptp_clock_info *info, struct timespec64 *ts)
+{
+	struct mock_phc *phc = info_to_phc(info);
+	u64 ns;
+
+	spin_lock(&phc->lock);
+	ns = timecounter_read(&phc->tc);
+	spin_unlock(&phc->lock);
+
+	*ts = ns_to_timespec64(ns);
+
+	return 0;
+}
+
+static long mock_phc_refresh(struct ptp_clock_info *info)
+{
+	struct timespec64 ts;
+
+	mock_phc_gettime64(info, &ts);
+
+	return MOCK_PHC_REFRESH_INTERVAL;
+}
+
+int mock_phc_index(struct mock_phc *phc)
+{
+	return ptp_clock_index(phc->clock);
+}
+EXPORT_SYMBOL_GPL(mock_phc_index);
+
+struct mock_phc *mock_phc_create(struct device *dev)
+{
+	struct mock_phc *phc;
+	int err;
+
+	phc = kzalloc_obj(*phc);
+	if (!phc) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	phc->info = (struct ptp_clock_info) {
+		.owner		= THIS_MODULE,
+		.name		= "Mock-up PTP clock",
+		.max_adj	= MOCK_PHC_MAX_ADJ_PPB,
+		.adjfine	= mock_phc_adjfine,
+		.adjtime	= mock_phc_adjtime,
+		.gettime64	= mock_phc_gettime64,
+		.settime64	= mock_phc_settime64,
+		.do_aux_work	= mock_phc_refresh,
+	};
+
+	phc->cc = (struct cyclecounter) {
+		.read	= mock_phc_cc_read,
+		.mask	= CYCLECOUNTER_MASK(64),
+		.mult	= MOCK_PHC_CC_MULT,
+		.shift	= MOCK_PHC_CC_SHIFT,
+	};
+
+	spin_lock_init(&phc->lock);
+	timecounter_init(&phc->tc, &phc->cc, 0);
+
+	phc->clock = ptp_clock_register(&phc->info, dev);
+	if (IS_ERR(phc->clock)) {
+		err = PTR_ERR(phc->clock);
+		goto out_free_phc;
+	}
+
+	ptp_schedule_worker(phc->clock, MOCK_PHC_REFRESH_INTERVAL);
+
+	return phc;
+
+out_free_phc:
+	kfree(phc);
+out:
+	return ERR_PTR(err);
+}
+EXPORT_SYMBOL_GPL(mock_phc_create);
+
+void mock_phc_destroy(struct mock_phc *phc)
+{
+	ptp_clock_unregister(phc->clock);
+	kfree(phc);
+}
+EXPORT_SYMBOL_GPL(mock_phc_destroy);
+
+MODULE_DESCRIPTION("Mock-up PTP Hardware Clock driver");
+MODULE_LICENSE("GPL");

linux-7.2.3/drivers/ptp/ptp_pch.c

@@ -0,0 +1,545 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * PTP 1588 clock using the EG20T PCH
+ *
+ * Copyright (C) 2010 OMICRON electronics GmbH
+ * Copyright (C) 2011-2012 LAPIS SEMICONDUCTOR Co., LTD.
+ *
+ * This code was derived from the IXP46X driver.
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/hex.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/io-64-nonatomic-lo-hi.h>
+#include <linux/io-64-nonatomic-hi-lo.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/ptp_pch.h>
+#include <linux/slab.h>
+
+#define STATION_ADDR_LEN	20
+#define PCI_DEVICE_ID_PCH_1588	0x8819
+#define IO_MEM_BAR 1
+
+#define DEFAULT_ADDEND 0xA0000000
+#define TICKS_NS_SHIFT  5
+#define N_EXT_TS	2
+
+enum pch_status {
+	PCH_SUCCESS,
+	PCH_INVALIDPARAM,
+	PCH_NOTIMESTAMP,
+	PCH_INTERRUPTMODEINUSE,
+	PCH_FAILED,
+	PCH_UNSUPPORTED,
+};
+
+/*
+ * struct pch_ts_regs - IEEE 1588 registers
+ */
+struct pch_ts_regs {
+	u32 control;
+	u32 event;
+	u32 addend;
+	u32 accum;
+	u32 test;
+	u32 ts_compare;
+	u32 rsystime_lo;
+	u32 rsystime_hi;
+	u32 systime_lo;
+	u32 systime_hi;
+	u32 trgt_lo;
+	u32 trgt_hi;
+	u32 asms_lo;
+	u32 asms_hi;
+	u32 amms_lo;
+	u32 amms_hi;
+	u32 ch_control;
+	u32 ch_event;
+	u32 tx_snap_lo;
+	u32 tx_snap_hi;
+	u32 rx_snap_lo;
+	u32 rx_snap_hi;
+	u32 src_uuid_lo;
+	u32 src_uuid_hi;
+	u32 can_status;
+	u32 can_snap_lo;
+	u32 can_snap_hi;
+	u32 ts_sel;
+	u32 ts_st[6];
+	u32 reserve1[14];
+	u32 stl_max_set_en;
+	u32 stl_max_set;
+	u32 reserve2[13];
+	u32 srst;
+};
+
+#define PCH_TSC_RESET		(1 << 0)
+#define PCH_TSC_TTM_MASK	(1 << 1)
+#define PCH_TSC_ASMS_MASK	(1 << 2)
+#define PCH_TSC_AMMS_MASK	(1 << 3)
+#define PCH_TSC_PPSM_MASK	(1 << 4)
+#define PCH_TSE_TTIPEND		(1 << 1)
+#define PCH_TSE_SNS		(1 << 2)
+#define PCH_TSE_SNM		(1 << 3)
+#define PCH_TSE_PPS		(1 << 4)
+#define PCH_CC_MM		(1 << 0)
+#define PCH_CC_TA		(1 << 1)
+
+#define PCH_CC_MODE_SHIFT	16
+#define PCH_CC_MODE_MASK	0x001F0000
+#define PCH_CC_VERSION		(1 << 31)
+#define PCH_CE_TXS		(1 << 0)
+#define PCH_CE_RXS		(1 << 1)
+#define PCH_CE_OVR		(1 << 0)
+#define PCH_CE_VAL		(1 << 1)
+#define PCH_ECS_ETH		(1 << 0)
+
+#define PCH_ECS_CAN		(1 << 1)
+
+#define PCH_IEEE1588_ETH	(1 << 0)
+#define PCH_IEEE1588_CAN	(1 << 1)
+
+/*
+ * struct pch_dev - Driver private data
+ */
+struct pch_dev {
+	struct pch_ts_regs __iomem *regs;
+	struct ptp_clock *ptp_clock;
+	struct ptp_clock_info caps;
+	int exts0_enabled;
+	int exts1_enabled;
+
+	u32 irq;
+	struct pci_dev *pdev;
+	spinlock_t register_lock;
+};
+
+/*
+ * struct pch_params - 1588 module parameter
+ */
+struct pch_params {
+	u8 station[STATION_ADDR_LEN];
+};
+
+/* structure to hold the module parameters */
+static struct pch_params pch_param = {
+	"00:00:00:00:00:00"
+};
+
+/*
+ * Register access functions
+ */
+static inline void pch_eth_enable_set(struct pch_dev *chip)
+{
+	u32 val;
+	/* SET the eth_enable bit */
+	val = ioread32(&chip->regs->ts_sel) | (PCH_ECS_ETH);
+	iowrite32(val, (&chip->regs->ts_sel));
+}
+
+static u64 pch_systime_read(struct pch_ts_regs __iomem *regs)
+{
+	u64 ns;
+
+	ns = ioread64_lo_hi(&regs->systime_lo);
+
+	return ns << TICKS_NS_SHIFT;
+}
+
+static void pch_systime_write(struct pch_ts_regs __iomem *regs, u64 ns)
+{
+	iowrite64_lo_hi(ns >> TICKS_NS_SHIFT, &regs->systime_lo);
+}
+
+static inline void pch_block_reset(struct pch_dev *chip)
+{
+	u32 val;
+	/* Reset Hardware Assist block */
+	val = ioread32(&chip->regs->control) | PCH_TSC_RESET;
+	iowrite32(val, (&chip->regs->control));
+	val = val & ~PCH_TSC_RESET;
+	iowrite32(val, (&chip->regs->control));
+}
+
+void pch_ch_control_write(struct pci_dev *pdev, u32 val)
+{
+	struct pch_dev *chip = pci_get_drvdata(pdev);
+
+	iowrite32(val, (&chip->regs->ch_control));
+}
+EXPORT_SYMBOL(pch_ch_control_write);
+
+u32 pch_ch_event_read(struct pci_dev *pdev)
+{
+	struct pch_dev *chip = pci_get_drvdata(pdev);
+	u32 val;
+
+	val = ioread32(&chip->regs->ch_event);
+
+	return val;
+}
+EXPORT_SYMBOL(pch_ch_event_read);
+
+void pch_ch_event_write(struct pci_dev *pdev, u32 val)
+{
+	struct pch_dev *chip = pci_get_drvdata(pdev);
+
+	iowrite32(val, (&chip->regs->ch_event));
+}
+EXPORT_SYMBOL(pch_ch_event_write);
+
+u32 pch_src_uuid_lo_read(struct pci_dev *pdev)
+{
+	struct pch_dev *chip = pci_get_drvdata(pdev);
+	u32 val;
+
+	val = ioread32(&chip->regs->src_uuid_lo);
+
+	return val;
+}
+EXPORT_SYMBOL(pch_src_uuid_lo_read);
+
+u32 pch_src_uuid_hi_read(struct pci_dev *pdev)
+{
+	struct pch_dev *chip = pci_get_drvdata(pdev);
+	u32 val;
+
+	val = ioread32(&chip->regs->src_uuid_hi);
+
+	return val;
+}
+EXPORT_SYMBOL(pch_src_uuid_hi_read);
+
+u64 pch_rx_snap_read(struct pci_dev *pdev)
+{
+	struct pch_dev *chip = pci_get_drvdata(pdev);
+	u64 ns;
+
+	ns = ioread64_lo_hi(&chip->regs->rx_snap_lo);
+
+	return ns << TICKS_NS_SHIFT;
+}
+EXPORT_SYMBOL(pch_rx_snap_read);
+
+u64 pch_tx_snap_read(struct pci_dev *pdev)
+{
+	struct pch_dev *chip = pci_get_drvdata(pdev);
+	u64 ns;
+
+	ns = ioread64_lo_hi(&chip->regs->tx_snap_lo);
+
+	return ns << TICKS_NS_SHIFT;
+}
+EXPORT_SYMBOL(pch_tx_snap_read);
+
+/* This function enables all 64 bits in system time registers [high & low].
+This is a work-around for non continuous value in the SystemTime Register*/
+static void pch_set_system_time_count(struct pch_dev *chip)
+{
+	iowrite32(0x01, &chip->regs->stl_max_set_en);
+	iowrite32(0xFFFFFFFF, &chip->regs->stl_max_set);
+	iowrite32(0x00, &chip->regs->stl_max_set_en);
+}
+
+static void pch_reset(struct pch_dev *chip)
+{
+	/* Reset Hardware Assist */
+	pch_block_reset(chip);
+
+	/* enable all 32 bits in system time registers */
+	pch_set_system_time_count(chip);
+}
+
+/**
+ * pch_set_station_address() - This API sets the station address used by
+ *				    IEEE 1588 hardware when looking at PTP
+ *				    traffic on the  ethernet interface
+ * @addr:	dress which contain the column separated address to be used.
+ * @pdev:	PCI device.
+ */
+int pch_set_station_address(u8 *addr, struct pci_dev *pdev)
+{
+	struct pch_dev *chip = pci_get_drvdata(pdev);
+	bool valid;
+	u64 mac;
+
+	/* Verify the parameter */
+	if ((chip->regs == NULL) || addr == (u8 *)NULL) {
+		dev_err(&pdev->dev,
+			"invalid params returning PCH_INVALIDPARAM\n");
+		return PCH_INVALIDPARAM;
+	}
+
+	valid = mac_pton(addr, (u8 *)&mac);
+	if (!valid) {
+		dev_err(&pdev->dev, "invalid params returning PCH_INVALIDPARAM\n");
+		return PCH_INVALIDPARAM;
+	}
+
+	dev_dbg(&pdev->dev, "invoking pch_station_set\n");
+	iowrite64_lo_hi(mac, &chip->regs->ts_st);
+	return 0;
+}
+EXPORT_SYMBOL(pch_set_station_address);
+
+/*
+ * Interrupt service routine
+ */
+static irqreturn_t isr(int irq, void *priv)
+{
+	struct pch_dev *pch_dev = priv;
+	struct pch_ts_regs __iomem *regs = pch_dev->regs;
+	struct ptp_clock_event event;
+	u32 ack = 0, val;
+
+	val = ioread32(&regs->event);
+
+	if (val & PCH_TSE_SNS) {
+		ack |= PCH_TSE_SNS;
+		if (pch_dev->exts0_enabled) {
+			event.type = PTP_CLOCK_EXTTS;
+			event.index = 0;
+			event.timestamp = ioread64_hi_lo(&regs->asms_hi);
+			event.timestamp <<= TICKS_NS_SHIFT;
+			ptp_clock_event(pch_dev->ptp_clock, &event);
+		}
+	}
+
+	if (val & PCH_TSE_SNM) {
+		ack |= PCH_TSE_SNM;
+		if (pch_dev->exts1_enabled) {
+			event.type = PTP_CLOCK_EXTTS;
+			event.index = 1;
+			event.timestamp = ioread64_hi_lo(&regs->asms_hi);
+			event.timestamp <<= TICKS_NS_SHIFT;
+			ptp_clock_event(pch_dev->ptp_clock, &event);
+		}
+	}
+
+	if (val & PCH_TSE_TTIPEND)
+		ack |= PCH_TSE_TTIPEND; /* this bit seems to be always set */
+
+	if (ack) {
+		iowrite32(ack, &regs->event);
+		return IRQ_HANDLED;
+	} else
+		return IRQ_NONE;
+}
+
+/*
+ * PTP clock operations
+ */
+
+static int ptp_pch_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+	u32 addend;
+	struct pch_dev *pch_dev = container_of(ptp, struct pch_dev, caps);
+	struct pch_ts_regs __iomem *regs = pch_dev->regs;
+
+	addend = adjust_by_scaled_ppm(DEFAULT_ADDEND, scaled_ppm);
+
+	iowrite32(addend, &regs->addend);
+
+	return 0;
+}
+
+static int ptp_pch_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	s64 now;
+	unsigned long flags;
+	struct pch_dev *pch_dev = container_of(ptp, struct pch_dev, caps);
+	struct pch_ts_regs __iomem *regs = pch_dev->regs;
+
+	spin_lock_irqsave(&pch_dev->register_lock, flags);
+	now = pch_systime_read(regs);
+	now += delta;
+	pch_systime_write(regs, now);
+	spin_unlock_irqrestore(&pch_dev->register_lock, flags);
+
+	return 0;
+}
+
+static int ptp_pch_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	u64 ns;
+	unsigned long flags;
+	struct pch_dev *pch_dev = container_of(ptp, struct pch_dev, caps);
+	struct pch_ts_regs __iomem *regs = pch_dev->regs;
+
+	spin_lock_irqsave(&pch_dev->register_lock, flags);
+	ns = pch_systime_read(regs);
+	spin_unlock_irqrestore(&pch_dev->register_lock, flags);
+
+	*ts = ns_to_timespec64(ns);
+	return 0;
+}
+
+static int ptp_pch_settime(struct ptp_clock_info *ptp,
+			   const struct timespec64 *ts)
+{
+	u64 ns;
+	unsigned long flags;
+	struct pch_dev *pch_dev = container_of(ptp, struct pch_dev, caps);
+	struct pch_ts_regs __iomem *regs = pch_dev->regs;
+
+	ns = timespec64_to_ns(ts);
+
+	spin_lock_irqsave(&pch_dev->register_lock, flags);
+	pch_systime_write(regs, ns);
+	spin_unlock_irqrestore(&pch_dev->register_lock, flags);
+
+	return 0;
+}
+
+static int ptp_pch_enable(struct ptp_clock_info *ptp,
+			  struct ptp_clock_request *rq, int on)
+{
+	struct pch_dev *pch_dev = container_of(ptp, struct pch_dev, caps);
+
+	switch (rq->type) {
+	case PTP_CLK_REQ_EXTTS:
+		switch (rq->extts.index) {
+		case 0:
+			pch_dev->exts0_enabled = on ? 1 : 0;
+			break;
+		case 1:
+			pch_dev->exts1_enabled = on ? 1 : 0;
+			break;
+		default:
+			return -EINVAL;
+		}
+		return 0;
+	default:
+		break;
+	}
+
+	return -EOPNOTSUPP;
+}
+
+static const struct ptp_clock_info ptp_pch_caps = {
+	.owner		= THIS_MODULE,
+	.name		= "PCH timer",
+	.max_adj	= 50000000,
+	.n_ext_ts	= N_EXT_TS,
+	.n_pins		= 0,
+	.pps		= 0,
+	.adjfine	= ptp_pch_adjfine,
+	.adjtime	= ptp_pch_adjtime,
+	.gettime64	= ptp_pch_gettime,
+	.settime64	= ptp_pch_settime,
+	.enable		= ptp_pch_enable,
+};
+
+static void pch_remove(struct pci_dev *pdev)
+{
+	struct pch_dev *chip = pci_get_drvdata(pdev);
+
+	free_irq(pdev->irq, chip);
+	ptp_clock_unregister(chip->ptp_clock);
+}
+
+static s32
+pch_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	s32 ret;
+	unsigned long flags;
+	struct pch_dev *chip;
+
+	chip = devm_kzalloc(&pdev->dev, sizeof(*chip), GFP_KERNEL);
+	if (chip == NULL)
+		return -ENOMEM;
+
+	/* enable the 1588 pci device */
+	ret = pcim_enable_device(pdev);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "could not enable the pci device\n");
+		return ret;
+	}
+
+	/* get the virtual address to the 1588 registers */
+	chip->regs = pcim_iomap_region(pdev, IO_MEM_BAR, KBUILD_MODNAME);
+	ret = PTR_ERR_OR_ZERO(chip->regs);
+	if (ret) {
+		dev_err(&pdev->dev, "could not locate IO memory address\n");
+		return ret;
+	}
+
+	chip->caps = ptp_pch_caps;
+	chip->ptp_clock = ptp_clock_register(&chip->caps, &pdev->dev);
+	if (IS_ERR(chip->ptp_clock))
+		return PTR_ERR(chip->ptp_clock);
+
+	spin_lock_init(&chip->register_lock);
+
+	ret = request_irq(pdev->irq, &isr, IRQF_SHARED, KBUILD_MODNAME, chip);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "failed to get irq %d\n", pdev->irq);
+		goto err_req_irq;
+	}
+
+	/* indicate success */
+	chip->irq = pdev->irq;
+	chip->pdev = pdev;
+	pci_set_drvdata(pdev, chip);
+
+	spin_lock_irqsave(&chip->register_lock, flags);
+	/* reset the ieee1588 h/w */
+	pch_reset(chip);
+
+	iowrite32(DEFAULT_ADDEND, &chip->regs->addend);
+	iowrite64_lo_hi(1, &chip->regs->trgt_lo);
+	iowrite32(PCH_TSE_TTIPEND, &chip->regs->event);
+
+	pch_eth_enable_set(chip);
+
+	if (strcmp(pch_param.station, "00:00:00:00:00:00") != 0) {
+		if (pch_set_station_address(pch_param.station, pdev) != 0) {
+			dev_err(&pdev->dev,
+			"Invalid station address parameter\n"
+			"Module loaded but station address not set correctly\n"
+			);
+		}
+	}
+	spin_unlock_irqrestore(&chip->register_lock, flags);
+	return 0;
+
+err_req_irq:
+	ptp_clock_unregister(chip->ptp_clock);
+
+	dev_err(&pdev->dev, "probe failed(ret=0x%x)\n", ret);
+
+	return ret;
+}
+
+static const struct pci_device_id pch_ieee1588_pcidev_id[] = {
+	{
+	  .vendor = PCI_VENDOR_ID_INTEL,
+	  .device = PCI_DEVICE_ID_PCH_1588
+	 },
+	{0}
+};
+MODULE_DEVICE_TABLE(pci, pch_ieee1588_pcidev_id);
+
+static struct pci_driver pch_driver = {
+	.name = KBUILD_MODNAME,
+	.id_table = pch_ieee1588_pcidev_id,
+	.probe = pch_probe,
+	.remove = pch_remove,
+};
+module_pci_driver(pch_driver);
+
+module_param_string(station,
+		    pch_param.station, sizeof(pch_param.station), 0444);
+MODULE_PARM_DESC(station,
+	 "IEEE 1588 station address to use - colon separated hex values");
+
+MODULE_AUTHOR("LAPIS SEMICONDUCTOR, <tshimizu818@gmail.com>");
+MODULE_DESCRIPTION("PTP clock using the EG20T timer");
+MODULE_LICENSE("GPL");

linux-7.2.3/drivers/ptp/ptp_private.h

@@ -0,0 +1,175 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * PTP 1588 clock support - private declarations for the core module.
+ *
+ * Copyright (C) 2010 OMICRON electronics GmbH
+ */
+#ifndef _PTP_PRIVATE_H_
+#define _PTP_PRIVATE_H_
+
+#include <linux/cdev.h>
+#include <linux/device.h>
+#include <linux/kthread.h>
+#include <linux/mutex.h>
+#include <linux/posix-clock.h>
+#include <linux/ptp_clock.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/time.h>
+#include <linux/list.h>
+#include <linux/bitmap.h>
+#include <linux/debugfs.h>
+
+#define PTP_MAX_TIMESTAMPS 128
+#define PTP_BUF_TIMESTAMPS 30
+#define PTP_DEFAULT_MAX_VCLOCKS 20
+#define PTP_MAX_VCLOCKS_LIMIT (KMALLOC_MAX_SIZE/(sizeof(int)))
+#define PTP_MAX_CHANNELS 2048
+
+enum {
+	PTP_LOCK_PHYSICAL = 0,
+	PTP_LOCK_VIRTUAL,
+};
+
+struct timestamp_event_queue {
+	struct ptp_extts_event buf[PTP_MAX_TIMESTAMPS];
+	int head;
+	int tail;
+	spinlock_t lock;
+	struct list_head qlist;
+	unsigned long *mask;
+	struct dentry *debugfs_instance;
+	struct debugfs_u32_array dfs_bitmap;
+};
+
+struct ptp_clock {
+	struct posix_clock clock;
+	struct device dev;
+	struct ptp_clock_info *info;
+	dev_t devid;
+	int index; /* index into clocks.map */
+	struct pps_device *pps_source;
+	long dialed_frequency; /* remembers the frequency adjustment */
+	struct list_head tsevqs; /* timestamp fifo list */
+	spinlock_t tsevqs_lock; /* protects tsevqs from concurrent access */
+	struct mutex pincfg_mux; /* protect concurrent info->pin_config access */
+	wait_queue_head_t tsev_wq;
+	int defunct; /* tells readers to go away when clock is being removed */
+	struct device_attribute *pin_dev_attr;
+	struct attribute **pin_attr;
+	struct attribute_group pin_attr_group;
+	/* 1st entry is a pointer to the real group, 2nd is NULL terminator */
+	const struct attribute_group *pin_attr_groups[2];
+	struct kthread_worker *kworker;
+	struct kthread_delayed_work aux_work;
+	unsigned int max_vclocks;
+	unsigned int n_vclocks;
+	int *vclock_index;
+	struct mutex n_vclocks_mux; /* protect concurrent n_vclocks access */
+	bool is_virtual_clock;
+	bool has_cycles;
+	struct dentry *debugfs_root;
+};
+
+#define info_to_vclock(d) container_of((d), struct ptp_vclock, info)
+#define cc_to_vclock(d) container_of((d), struct ptp_vclock, cc)
+#define dw_to_vclock(d) container_of((d), struct ptp_vclock, refresh_work)
+
+struct ptp_vclock {
+	struct ptp_clock *pclock;
+	struct ptp_clock_info info;
+	struct ptp_clock *clock;
+	struct hlist_node vclock_hash_node;
+	struct cyclecounter cc;
+	struct timecounter tc;
+	struct mutex lock;	/* protects tc/cc */
+};
+
+/*
+ * The function queue_cnt() is safe for readers to call without
+ * holding q->lock. Readers use this function to verify that the queue
+ * is nonempty before proceeding with a dequeue operation. The fact
+ * that a writer might concurrently increment the tail does not
+ * matter, since the queue remains nonempty nonetheless.
+ */
+static inline int queue_cnt(const struct timestamp_event_queue *q)
+{
+	/*
+	 * Paired with WRITE_ONCE() in enqueue_external_timestamp(),
+	 * ptp_read(), extts_fifo_show().
+	 */
+	int cnt = READ_ONCE(q->tail) - READ_ONCE(q->head);
+	return cnt < 0 ? PTP_MAX_TIMESTAMPS + cnt : cnt;
+}
+
+/* Check if ptp virtual clock is in use */
+static inline bool ptp_vclock_in_use(struct ptp_clock *ptp)
+{
+	bool in_use = false;
+
+	/* Virtual clocks can't be stacked on top of virtual clocks.
+	 * Avoid acquiring the n_vclocks_mux on virtual clocks, to allow this
+	 * function to be called from code paths where the n_vclocks_mux of the
+	 * parent physical clock is already held. Functionally that's not an
+	 * issue, but lockdep would complain, because they have the same lock
+	 * class.
+	 */
+	if (ptp->is_virtual_clock)
+		return false;
+
+	if (mutex_lock_interruptible(&ptp->n_vclocks_mux))
+		return true;
+
+	if (ptp->n_vclocks)
+		in_use = true;
+
+	mutex_unlock(&ptp->n_vclocks_mux);
+
+	return in_use;
+}
+
+/* Check if ptp clock shall be free running */
+static inline bool ptp_clock_freerun(struct ptp_clock *ptp)
+{
+	if (ptp->has_cycles)
+		return false;
+
+	return ptp_vclock_in_use(ptp);
+}
+
+extern const struct class ptp_class;
+
+/*
+ * see ptp_chardev.c
+ */
+
+void ptp_disable_all_events(struct ptp_clock *ptp);
+
+/* caller must hold pincfg_mux */
+int ptp_set_pinfunc(struct ptp_clock *ptp, unsigned int pin,
+		    enum ptp_pin_function func, unsigned int chan);
+
+long ptp_ioctl(struct posix_clock_context *pccontext, unsigned int cmd,
+	       unsigned long arg);
+
+int ptp_open(struct posix_clock_context *pccontext, fmode_t fmode);
+
+int ptp_release(struct posix_clock_context *pccontext);
+
+ssize_t ptp_read(struct posix_clock_context *pccontext, uint flags, char __user *buf,
+		 size_t cnt);
+
+__poll_t ptp_poll(struct posix_clock_context *pccontext, struct file *fp,
+		  poll_table *wait);
+
+/*
+ * see ptp_sysfs.c
+ */
+
+extern const struct attribute_group *ptp_groups[];
+
+int ptp_populate_pin_groups(struct ptp_clock *ptp);
+void ptp_cleanup_pin_groups(struct ptp_clock *ptp);
+
+struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock);
+void ptp_vclock_unregister(struct ptp_vclock *vclock);
+#endif

linux-7.2.3/drivers/ptp/ptp_qoriq.c

@@ -0,0 +1,700 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * PTP 1588 clock for Freescale QorIQ 1588 timer
+ *
+ * Copyright (C) 2010 OMICRON electronics GmbH
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/device.h>
+#include <linux/hrtimer.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/timex.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+
+#include <linux/fsl/ptp_qoriq.h>
+
+/*
+ * Register access functions
+ */
+
+/* Caller must hold ptp_qoriq->lock. */
+static u64 tmr_cnt_read(struct ptp_qoriq *ptp_qoriq)
+{
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+	u64 ns;
+	u32 lo, hi;
+
+	lo = ptp_qoriq->read(&regs->ctrl_regs->tmr_cnt_l);
+	hi = ptp_qoriq->read(&regs->ctrl_regs->tmr_cnt_h);
+	ns = ((u64) hi) << 32;
+	ns |= lo;
+	return ns;
+}
+
+/* Caller must hold ptp_qoriq->lock. */
+static void tmr_cnt_write(struct ptp_qoriq *ptp_qoriq, u64 ns)
+{
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+	u32 hi = ns >> 32;
+	u32 lo = ns & 0xffffffff;
+
+	ptp_qoriq->write(&regs->ctrl_regs->tmr_cnt_l, lo);
+	ptp_qoriq->write(&regs->ctrl_regs->tmr_cnt_h, hi);
+}
+
+static u64 tmr_offset_read(struct ptp_qoriq *ptp_qoriq)
+{
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+	u32 lo, hi;
+	u64 ns;
+
+	lo = ptp_qoriq->read(&regs->ctrl_regs->tmroff_l);
+	hi = ptp_qoriq->read(&regs->ctrl_regs->tmroff_h);
+	ns = ((u64) hi) << 32;
+	ns |= lo;
+	return ns;
+}
+
+static void tmr_offset_write(struct ptp_qoriq *ptp_qoriq, u64 delta_ns)
+{
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+	u32 lo = delta_ns & 0xffffffff;
+	u32 hi = delta_ns >> 32;
+
+	ptp_qoriq->write(&regs->ctrl_regs->tmroff_l, lo);
+	ptp_qoriq->write(&regs->ctrl_regs->tmroff_h, hi);
+}
+
+/* Caller must hold ptp_qoriq->lock. */
+static void set_alarm(struct ptp_qoriq *ptp_qoriq)
+{
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+	u64 ns;
+	u32 lo, hi;
+
+	ns = tmr_cnt_read(ptp_qoriq) + tmr_offset_read(ptp_qoriq)
+				     + 1500000000ULL;
+
+	ns = div_u64(ns, 1000000000UL) * 1000000000ULL;
+	ns -= ptp_qoriq->tclk_period;
+	hi = ns >> 32;
+	lo = ns & 0xffffffff;
+	ptp_qoriq->write(&regs->alarm_regs->tmr_alarm1_l, lo);
+	ptp_qoriq->write(&regs->alarm_regs->tmr_alarm1_h, hi);
+}
+
+/* Caller must hold ptp_qoriq->lock. */
+static void set_fipers(struct ptp_qoriq *ptp_qoriq)
+{
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+
+	set_alarm(ptp_qoriq);
+	ptp_qoriq->write(&regs->fiper_regs->tmr_fiper1, ptp_qoriq->tmr_fiper1);
+	ptp_qoriq->write(&regs->fiper_regs->tmr_fiper2, ptp_qoriq->tmr_fiper2);
+
+	if (ptp_qoriq->fiper3_support)
+		ptp_qoriq->write(&regs->fiper_regs->tmr_fiper3,
+				 ptp_qoriq->tmr_fiper3);
+}
+
+int extts_clean_up(struct ptp_qoriq *ptp_qoriq, int index, bool update_event)
+{
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+	struct ptp_clock_event event;
+	void __iomem *reg_etts_l;
+	void __iomem *reg_etts_h;
+	u32 valid, lo, hi;
+
+	switch (index) {
+	case 0:
+		valid = ETS1_VLD;
+		reg_etts_l = &regs->etts_regs->tmr_etts1_l;
+		reg_etts_h = &regs->etts_regs->tmr_etts1_h;
+		break;
+	case 1:
+		valid = ETS2_VLD;
+		reg_etts_l = &regs->etts_regs->tmr_etts2_l;
+		reg_etts_h = &regs->etts_regs->tmr_etts2_h;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	event.type = PTP_CLOCK_EXTTS;
+	event.index = index;
+
+	if (ptp_qoriq->extts_fifo_support)
+		if (!(ptp_qoriq->read(&regs->ctrl_regs->tmr_stat) & valid))
+			return 0;
+
+	do {
+		lo = ptp_qoriq->read(reg_etts_l);
+		hi = ptp_qoriq->read(reg_etts_h);
+
+		if (update_event) {
+			event.timestamp = ((u64) hi) << 32;
+			event.timestamp |= lo;
+			ptp_clock_event(ptp_qoriq->clock, &event);
+		}
+
+		if (!ptp_qoriq->extts_fifo_support)
+			break;
+	} while (ptp_qoriq->read(&regs->ctrl_regs->tmr_stat) & valid);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(extts_clean_up);
+
+/*
+ * Interrupt service routine
+ */
+
+irqreturn_t ptp_qoriq_isr(int irq, void *priv)
+{
+	struct ptp_qoriq *ptp_qoriq = priv;
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+	struct ptp_clock_event event;
+	u32 ack = 0, mask, val, irqs;
+
+	spin_lock(&ptp_qoriq->lock);
+
+	val = ptp_qoriq->read(&regs->ctrl_regs->tmr_tevent);
+	mask = ptp_qoriq->read(&regs->ctrl_regs->tmr_temask);
+
+	spin_unlock(&ptp_qoriq->lock);
+
+	irqs = val & mask;
+
+	if (irqs & ETS1) {
+		ack |= ETS1;
+		extts_clean_up(ptp_qoriq, 0, true);
+	}
+
+	if (irqs & ETS2) {
+		ack |= ETS2;
+		extts_clean_up(ptp_qoriq, 1, true);
+	}
+
+	if (irqs & PP1) {
+		ack |= PP1;
+		event.type = PTP_CLOCK_PPS;
+		ptp_clock_event(ptp_qoriq->clock, &event);
+	}
+
+	if (ack) {
+		ptp_qoriq->write(&regs->ctrl_regs->tmr_tevent, ack);
+		return IRQ_HANDLED;
+	} else
+		return IRQ_NONE;
+}
+EXPORT_SYMBOL_GPL(ptp_qoriq_isr);
+
+/*
+ * PTP clock operations
+ */
+
+int ptp_qoriq_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+	u64 adj, diff;
+	u32 tmr_add;
+	int neg_adj = 0;
+	struct ptp_qoriq *ptp_qoriq = container_of(ptp, struct ptp_qoriq, caps);
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+
+	if (scaled_ppm < 0) {
+		neg_adj = 1;
+		scaled_ppm = -scaled_ppm;
+	}
+	tmr_add = ptp_qoriq->tmr_add;
+	adj = tmr_add;
+
+	/*
+	 * Calculate diff and round() to the nearest integer
+	 *
+	 * diff = adj * (ppb / 1000000000)
+	 *      = adj * scaled_ppm / 65536000000
+	 */
+	diff = mul_u64_u64_div_u64(adj, scaled_ppm, 32768000000);
+	diff = DIV64_U64_ROUND_UP(diff, 2);
+
+	tmr_add = neg_adj ? tmr_add - diff : tmr_add + diff;
+	ptp_qoriq->write(&regs->ctrl_regs->tmr_add, tmr_add);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ptp_qoriq_adjfine);
+
+int ptp_qoriq_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct ptp_qoriq *ptp_qoriq = container_of(ptp, struct ptp_qoriq, caps);
+	s64 now, curr_delta;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ptp_qoriq->lock, flags);
+
+	/* On LS1021A, eTSEC2 and eTSEC3 do not take into account the TMR_OFF
+	 * adjustment
+	 */
+	if (ptp_qoriq->etsec) {
+		now = tmr_cnt_read(ptp_qoriq);
+		now += delta;
+		tmr_cnt_write(ptp_qoriq, now);
+	} else {
+		curr_delta = tmr_offset_read(ptp_qoriq);
+		curr_delta += delta;
+		tmr_offset_write(ptp_qoriq, curr_delta);
+	}
+	set_fipers(ptp_qoriq);
+
+	spin_unlock_irqrestore(&ptp_qoriq->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ptp_qoriq_adjtime);
+
+int ptp_qoriq_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	u64 ns;
+	unsigned long flags;
+	struct ptp_qoriq *ptp_qoriq = container_of(ptp, struct ptp_qoriq, caps);
+
+	spin_lock_irqsave(&ptp_qoriq->lock, flags);
+
+	ns = tmr_cnt_read(ptp_qoriq) + tmr_offset_read(ptp_qoriq);
+
+	spin_unlock_irqrestore(&ptp_qoriq->lock, flags);
+
+	*ts = ns_to_timespec64(ns);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ptp_qoriq_gettime);
+
+int ptp_qoriq_settime(struct ptp_clock_info *ptp,
+		      const struct timespec64 *ts)
+{
+	u64 ns;
+	unsigned long flags;
+	struct ptp_qoriq *ptp_qoriq = container_of(ptp, struct ptp_qoriq, caps);
+
+	ns = timespec64_to_ns(ts);
+
+	spin_lock_irqsave(&ptp_qoriq->lock, flags);
+
+	tmr_offset_write(ptp_qoriq, 0);
+	tmr_cnt_write(ptp_qoriq, ns);
+	set_fipers(ptp_qoriq);
+
+	spin_unlock_irqrestore(&ptp_qoriq->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ptp_qoriq_settime);
+
+int ptp_qoriq_enable(struct ptp_clock_info *ptp,
+		     struct ptp_clock_request *rq, int on)
+{
+	struct ptp_qoriq *ptp_qoriq = container_of(ptp, struct ptp_qoriq, caps);
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+	unsigned long flags;
+	u32 bit, mask = 0;
+
+	switch (rq->type) {
+	case PTP_CLK_REQ_EXTTS:
+		switch (rq->extts.index) {
+		case 0:
+			bit = ETS1EN;
+			break;
+		case 1:
+			bit = ETS2EN;
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		if (on)
+			extts_clean_up(ptp_qoriq, rq->extts.index, false);
+
+		break;
+	case PTP_CLK_REQ_PPS:
+		bit = PP1EN;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	spin_lock_irqsave(&ptp_qoriq->lock, flags);
+
+	mask = ptp_qoriq->read(&regs->ctrl_regs->tmr_temask);
+	if (on) {
+		mask |= bit;
+		ptp_qoriq->write(&regs->ctrl_regs->tmr_tevent, bit);
+	} else {
+		mask &= ~bit;
+	}
+
+	ptp_qoriq->write(&regs->ctrl_regs->tmr_temask, mask);
+
+	spin_unlock_irqrestore(&ptp_qoriq->lock, flags);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ptp_qoriq_enable);
+
+static const struct ptp_clock_info ptp_qoriq_caps = {
+	.owner		= THIS_MODULE,
+	.name		= "qoriq ptp clock",
+	.max_adj	= 512000,
+	.n_alarm	= 0,
+	.n_ext_ts	= N_EXT_TS,
+	.n_per_out	= 0,
+	.n_pins		= 0,
+	.pps		= 1,
+	.adjfine	= ptp_qoriq_adjfine,
+	.adjtime	= ptp_qoriq_adjtime,
+	.gettime64	= ptp_qoriq_gettime,
+	.settime64	= ptp_qoriq_settime,
+	.enable		= ptp_qoriq_enable,
+};
+
+/**
+ * ptp_qoriq_nominal_freq - calculate nominal frequency according to
+ *			    reference clock frequency
+ *
+ * @clk_src: reference clock frequency
+ *
+ * The nominal frequency is the desired clock frequency.
+ * It should be less than the reference clock frequency.
+ * It should be a factor of 1000MHz.
+ *
+ * Return the nominal frequency
+ */
+static u32 ptp_qoriq_nominal_freq(u32 clk_src)
+{
+	u32 remainder = 0;
+
+	clk_src /= 1000000;
+	remainder = clk_src % 100;
+	if (remainder) {
+		clk_src -= remainder;
+		clk_src += 100;
+	}
+
+	do {
+		clk_src -= 100;
+
+	} while (1000 % clk_src);
+
+	return clk_src * 1000000;
+}
+
+/**
+ * ptp_qoriq_auto_config - calculate a set of default configurations
+ *
+ * @ptp_qoriq: pointer to ptp_qoriq
+ * @node: pointer to device_node
+ *
+ * If below dts properties are not provided, this function will be
+ * called to calculate a set of default configurations for them.
+ *   "fsl,tclk-period"
+ *   "fsl,tmr-prsc"
+ *   "fsl,tmr-add"
+ *   "fsl,tmr-fiper1"
+ *   "fsl,tmr-fiper2"
+ *   "fsl,tmr-fiper3" (required only for DPAA2 and ENETC hardware)
+ *   "fsl,max-adj"
+ *
+ * Return 0 if success
+ */
+static int ptp_qoriq_auto_config(struct ptp_qoriq *ptp_qoriq,
+				 struct device_node *node)
+{
+	struct clk *clk;
+	u64 freq_comp;
+	u64 max_adj;
+	u32 nominal_freq;
+	u32 remainder = 0;
+	u32 clk_src = 0;
+
+	ptp_qoriq->cksel = DEFAULT_CKSEL;
+
+	clk = of_clk_get(node, 0);
+	if (!IS_ERR(clk)) {
+		clk_src = clk_get_rate(clk);
+		clk_put(clk);
+	}
+
+	if (clk_src <= 100000000UL) {
+		pr_err("error reference clock value, or lower than 100MHz\n");
+		return -EINVAL;
+	}
+
+	nominal_freq = ptp_qoriq_nominal_freq(clk_src);
+	if (!nominal_freq)
+		return -EINVAL;
+
+	ptp_qoriq->tclk_period = 1000000000UL / nominal_freq;
+	ptp_qoriq->tmr_prsc = DEFAULT_TMR_PRSC;
+
+	/* Calculate initial frequency compensation value for TMR_ADD register.
+	 * freq_comp = ceil(2^32 / freq_ratio)
+	 * freq_ratio = reference_clock_freq / nominal_freq
+	 */
+	freq_comp = ((u64)1 << 32) * nominal_freq;
+	freq_comp = div_u64_rem(freq_comp, clk_src, &remainder);
+	if (remainder)
+		freq_comp++;
+
+	ptp_qoriq->tmr_add = freq_comp;
+	ptp_qoriq->tmr_fiper1 = DEFAULT_FIPER1_PERIOD - ptp_qoriq->tclk_period;
+	ptp_qoriq->tmr_fiper2 = DEFAULT_FIPER2_PERIOD - ptp_qoriq->tclk_period;
+	ptp_qoriq->tmr_fiper3 = DEFAULT_FIPER3_PERIOD - ptp_qoriq->tclk_period;
+
+	/* max_adj = 1000000000 * (freq_ratio - 1.0) - 1
+	 * freq_ratio = reference_clock_freq / nominal_freq
+	 */
+	max_adj = 1000000000ULL * (clk_src - nominal_freq);
+	max_adj = div_u64(max_adj, nominal_freq) - 1;
+	ptp_qoriq->caps.max_adj = max_adj;
+
+	return 0;
+}
+
+static int ptp_qoriq_perout_loopback(struct ptp_clock_info *ptp,
+				     unsigned int index, int on)
+{
+	struct ptp_qoriq *ptp_qoriq = container_of(ptp, struct ptp_qoriq, caps);
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+	u32 loopback_bit = index ? PP2L : PP1L;
+	u32 tmr_ctrl;
+
+	tmr_ctrl = ptp_qoriq->read(&regs->ctrl_regs->tmr_ctrl);
+	if (on)
+		tmr_ctrl |= loopback_bit;
+	else
+		tmr_ctrl &= ~loopback_bit;
+
+	ptp_qoriq->write(&regs->ctrl_regs->tmr_ctrl, tmr_ctrl);
+
+	return 0;
+}
+
+int ptp_qoriq_init(struct ptp_qoriq *ptp_qoriq, void __iomem *base,
+		   const struct ptp_clock_info *caps)
+{
+	struct device_node *node = ptp_qoriq->dev->of_node;
+	struct ptp_qoriq_registers *regs;
+	struct timespec64 now;
+	unsigned long flags;
+	u32 tmr_ctrl;
+
+	if (!node)
+		return -ENODEV;
+
+	ptp_qoriq->base = base;
+	ptp_qoriq->caps = *caps;
+	ptp_qoriq->caps.n_per_lp = 2;
+	ptp_qoriq->caps.perout_loopback = ptp_qoriq_perout_loopback;
+
+	if (of_property_read_u32(node, "fsl,cksel", &ptp_qoriq->cksel))
+		ptp_qoriq->cksel = DEFAULT_CKSEL;
+
+	if (of_property_read_bool(node, "fsl,extts-fifo"))
+		ptp_qoriq->extts_fifo_support = true;
+	else
+		ptp_qoriq->extts_fifo_support = false;
+
+	if (of_device_is_compatible(node, "fsl,dpaa2-ptp") ||
+	    of_device_is_compatible(node, "fsl,enetc-ptp"))
+		ptp_qoriq->fiper3_support = true;
+
+	if (of_property_read_u32(node,
+				 "fsl,tclk-period", &ptp_qoriq->tclk_period) ||
+	    of_property_read_u32(node,
+				 "fsl,tmr-prsc", &ptp_qoriq->tmr_prsc) ||
+	    of_property_read_u32(node,
+				 "fsl,tmr-add", &ptp_qoriq->tmr_add) ||
+	    of_property_read_u32(node,
+				 "fsl,tmr-fiper1", &ptp_qoriq->tmr_fiper1) ||
+	    of_property_read_u32(node,
+				 "fsl,tmr-fiper2", &ptp_qoriq->tmr_fiper2) ||
+	    of_property_read_u32(node,
+				 "fsl,max-adj", &ptp_qoriq->caps.max_adj) ||
+	    (ptp_qoriq->fiper3_support &&
+	     of_property_read_u32(node, "fsl,tmr-fiper3",
+				  &ptp_qoriq->tmr_fiper3))) {
+		pr_warn("device tree node missing required elements, try automatic configuration\n");
+
+		if (ptp_qoriq_auto_config(ptp_qoriq, node))
+			return -ENODEV;
+	}
+
+	if (of_property_read_bool(node, "little-endian")) {
+		ptp_qoriq->read = qoriq_read_le;
+		ptp_qoriq->write = qoriq_write_le;
+	} else {
+		ptp_qoriq->read = qoriq_read_be;
+		ptp_qoriq->write = qoriq_write_be;
+	}
+
+	/* The eTSEC uses differnt memory map with DPAA/ENETC */
+	if (of_device_is_compatible(node, "fsl,etsec-ptp")) {
+		ptp_qoriq->etsec = true;
+		ptp_qoriq->regs.ctrl_regs = base + ETSEC_CTRL_REGS_OFFSET;
+		ptp_qoriq->regs.alarm_regs = base + ETSEC_ALARM_REGS_OFFSET;
+		ptp_qoriq->regs.fiper_regs = base + ETSEC_FIPER_REGS_OFFSET;
+		ptp_qoriq->regs.etts_regs = base + ETSEC_ETTS_REGS_OFFSET;
+	} else {
+		ptp_qoriq->regs.ctrl_regs = base + CTRL_REGS_OFFSET;
+		ptp_qoriq->regs.alarm_regs = base + ALARM_REGS_OFFSET;
+		ptp_qoriq->regs.fiper_regs = base + FIPER_REGS_OFFSET;
+		ptp_qoriq->regs.etts_regs = base + ETTS_REGS_OFFSET;
+	}
+
+	spin_lock_init(&ptp_qoriq->lock);
+
+	ktime_get_real_ts64(&now);
+	ptp_qoriq_settime(&ptp_qoriq->caps, &now);
+
+	tmr_ctrl =
+	  (ptp_qoriq->tclk_period & TCLK_PERIOD_MASK) << TCLK_PERIOD_SHIFT |
+	  (ptp_qoriq->cksel & CKSEL_MASK) << CKSEL_SHIFT;
+
+	spin_lock_irqsave(&ptp_qoriq->lock, flags);
+
+	regs = &ptp_qoriq->regs;
+	ptp_qoriq->write(&regs->ctrl_regs->tmr_ctrl, tmr_ctrl);
+	ptp_qoriq->write(&regs->ctrl_regs->tmr_add, ptp_qoriq->tmr_add);
+	ptp_qoriq->write(&regs->ctrl_regs->tmr_prsc, ptp_qoriq->tmr_prsc);
+	ptp_qoriq->write(&regs->fiper_regs->tmr_fiper1, ptp_qoriq->tmr_fiper1);
+	ptp_qoriq->write(&regs->fiper_regs->tmr_fiper2, ptp_qoriq->tmr_fiper2);
+
+	if (ptp_qoriq->fiper3_support)
+		ptp_qoriq->write(&regs->fiper_regs->tmr_fiper3,
+				 ptp_qoriq->tmr_fiper3);
+
+	set_alarm(ptp_qoriq);
+	ptp_qoriq->write(&regs->ctrl_regs->tmr_ctrl,
+			 tmr_ctrl|FIPERST|RTPE|TE|FRD);
+
+	spin_unlock_irqrestore(&ptp_qoriq->lock, flags);
+
+	ptp_qoriq->clock = ptp_clock_register(&ptp_qoriq->caps, ptp_qoriq->dev);
+	if (IS_ERR(ptp_qoriq->clock))
+		return PTR_ERR(ptp_qoriq->clock);
+
+	ptp_qoriq->phc_index = ptp_clock_index(ptp_qoriq->clock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ptp_qoriq_init);
+
+void ptp_qoriq_free(struct ptp_qoriq *ptp_qoriq)
+{
+	struct ptp_qoriq_registers *regs = &ptp_qoriq->regs;
+
+	ptp_qoriq->write(&regs->ctrl_regs->tmr_temask, 0);
+	ptp_qoriq->write(&regs->ctrl_regs->tmr_ctrl,   0);
+
+	ptp_clock_unregister(ptp_qoriq->clock);
+	iounmap(ptp_qoriq->base);
+	free_irq(ptp_qoriq->irq, ptp_qoriq);
+}
+EXPORT_SYMBOL_GPL(ptp_qoriq_free);
+
+static int ptp_qoriq_probe(struct platform_device *dev)
+{
+	struct ptp_qoriq *ptp_qoriq;
+	int err = -ENOMEM;
+	void __iomem *base;
+
+	ptp_qoriq = kzalloc_obj(*ptp_qoriq);
+	if (!ptp_qoriq)
+		goto no_memory;
+
+	ptp_qoriq->dev = &dev->dev;
+
+	err = -ENODEV;
+
+	ptp_qoriq->irq = platform_get_irq(dev, 0);
+	if (ptp_qoriq->irq < 0) {
+		pr_err("irq not in device tree\n");
+		goto no_node;
+	}
+	if (request_irq(ptp_qoriq->irq, ptp_qoriq_isr, IRQF_SHARED,
+			DRIVER, ptp_qoriq)) {
+		pr_err("request_irq failed\n");
+		goto no_node;
+	}
+
+	ptp_qoriq->rsrc = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	if (!ptp_qoriq->rsrc) {
+		pr_err("no resource\n");
+		goto no_resource;
+	}
+	if (request_resource(&iomem_resource, ptp_qoriq->rsrc)) {
+		pr_err("resource busy\n");
+		goto no_resource;
+	}
+
+	base = ioremap(ptp_qoriq->rsrc->start,
+		       resource_size(ptp_qoriq->rsrc));
+	if (!base) {
+		pr_err("ioremap ptp registers failed\n");
+		goto no_ioremap;
+	}
+
+	err = ptp_qoriq_init(ptp_qoriq, base, &ptp_qoriq_caps);
+	if (err)
+		goto no_clock;
+
+	platform_set_drvdata(dev, ptp_qoriq);
+	return 0;
+
+no_clock:
+	iounmap(base);
+no_ioremap:
+	release_resource(ptp_qoriq->rsrc);
+no_resource:
+	free_irq(ptp_qoriq->irq, ptp_qoriq);
+no_node:
+	kfree(ptp_qoriq);
+no_memory:
+	return err;
+}
+
+static void ptp_qoriq_remove(struct platform_device *dev)
+{
+	struct ptp_qoriq *ptp_qoriq = platform_get_drvdata(dev);
+
+	ptp_qoriq_free(ptp_qoriq);
+	release_resource(ptp_qoriq->rsrc);
+	kfree(ptp_qoriq);
+}
+
+static const struct of_device_id match_table[] = {
+	{ .compatible = "fsl,etsec-ptp" },
+	{ .compatible = "fsl,fman-ptp-timer" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, match_table);
+
+static struct platform_driver ptp_qoriq_driver = {
+	.driver = {
+		.name		= "ptp_qoriq",
+		.of_match_table	= match_table,
+	},
+	.probe       = ptp_qoriq_probe,
+	.remove      = ptp_qoriq_remove,
+};
+
+module_platform_driver(ptp_qoriq_driver);
+
+MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
+MODULE_DESCRIPTION("PTP clock for Freescale QorIQ 1588 timer");
+MODULE_LICENSE("GPL");

linux-7.2.3/drivers/ptp/ptp_s390.c

@@ -0,0 +1,133 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * s390 PTP clock driver
+ *
+ */
+
+#include "ptp_private.h"
+#include <linux/time.h>
+#include <asm/stp.h>
+
+static struct ptp_clock *ptp_stcke_clock, *ptp_qpt_clock;
+
+static int ptp_s390_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ptp_s390_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	return -EOPNOTSUPP;
+}
+
+static struct timespec64 eitod_to_timespec64(union tod_clock *clk)
+{
+	return ns_to_timespec64(eitod_to_ns(clk->eitod - TOD_UNIX_EPOCH));
+}
+
+static struct timespec64 tod_to_timespec64(unsigned long tod)
+{
+	return ns_to_timespec64(tod_to_ns(tod - TOD_UNIX_EPOCH));
+}
+
+static int ptp_s390_stcke_gettime(struct ptp_clock_info *ptp,
+				  struct timespec64 *ts)
+{
+	union tod_clock tod;
+
+	if (!stp_enabled())
+		return -EOPNOTSUPP;
+
+	store_tod_clock_ext(&tod);
+	*ts = eitod_to_timespec64(&tod);
+	return 0;
+}
+
+static int ptp_s390_qpt_gettime(struct ptp_clock_info *ptp,
+				struct timespec64 *ts)
+{
+	unsigned long tod;
+
+	ptff(&tod, sizeof(tod), PTFF_QPT);
+	*ts = tod_to_timespec64(tod);
+	return 0;
+}
+
+static int ptp_s390_settime(struct ptp_clock_info *ptp,
+			    const struct timespec64 *ts)
+{
+	return -EOPNOTSUPP;
+}
+
+static int s390_arch_ptp_get_crosststamp(ktime_t *device_time,
+					 struct system_counterval_t *system_counter,
+					 void *ctx)
+{
+	union tod_clock clk;
+
+	store_tod_clock_ext(&clk);
+	*device_time = ns_to_ktime(tod_to_ns(clk.tod - TOD_UNIX_EPOCH));
+	system_counter->cycles = clk.tod;
+	system_counter->cs_id = CSID_S390_TOD;
+	return 0;
+}
+
+static int ptp_s390_getcrosststamp(struct ptp_clock_info *ptp,
+				   struct system_device_crosststamp *xtstamp)
+{
+	if (!stp_enabled())
+		return -EOPNOTSUPP;
+	return get_device_system_crosststamp(s390_arch_ptp_get_crosststamp, NULL, NULL, xtstamp);
+}
+
+static struct ptp_clock_info ptp_s390_stcke_info = {
+	.owner		= THIS_MODULE,
+	.name		= "s390 STCKE Clock",
+	.max_adj	= 0,
+	.adjfine	= ptp_s390_adjfine,
+	.adjtime	= ptp_s390_adjtime,
+	.gettime64	= ptp_s390_stcke_gettime,
+	.settime64	= ptp_s390_settime,
+	.getcrosststamp = ptp_s390_getcrosststamp,
+};
+
+static struct ptp_clock_info ptp_s390_qpt_info = {
+	.owner		= THIS_MODULE,
+	.name		= "s390 Physical Clock",
+	.max_adj	= 0,
+	.adjfine	= ptp_s390_adjfine,
+	.adjtime	= ptp_s390_adjtime,
+	.gettime64	= ptp_s390_qpt_gettime,
+	.settime64	= ptp_s390_settime,
+};
+
+static __init int ptp_s390_init(void)
+{
+	ptp_stcke_clock = ptp_clock_register(&ptp_s390_stcke_info, NULL);
+	if (IS_ERR(ptp_stcke_clock))
+		return PTR_ERR(ptp_stcke_clock);
+
+	if (!test_facility(28) || !ptff_query(PTFF_QPT))
+		return 0;
+
+	ptp_qpt_clock = ptp_clock_register(&ptp_s390_qpt_info, NULL);
+	if (IS_ERR(ptp_qpt_clock)) {
+		ptp_clock_unregister(ptp_stcke_clock);
+		return PTR_ERR(ptp_qpt_clock);
+	}
+	return 0;
+}
+
+static __exit void ptp_s390_exit(void)
+{
+	if (ptp_qpt_clock)
+		ptp_clock_unregister(ptp_qpt_clock);
+	ptp_clock_unregister(ptp_stcke_clock);
+}
+
+module_init(ptp_s390_init);
+module_exit(ptp_s390_exit);
+
+MODULE_AUTHOR("Sven Schnelle <svens@linux.ibm.com>");
+MODULE_DESCRIPTION("s390 Physical/STCKE Clock PtP Driver");
+MODULE_LICENSE("GPL");

linux-7.2.3/drivers/ptp/ptp_sysfs.c

@@ -0,0 +1,481 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * PTP 1588 clock support - sysfs interface.
+ *
+ * Copyright (C) 2010 OMICRON electronics GmbH
+ * Copyright 2021 NXP
+ */
+#include <linux/capability.h>
+#include <linux/slab.h>
+
+#include "ptp_private.h"
+
+static ssize_t clock_name_show(struct device *dev,
+			       struct device_attribute *attr, char *page)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	return sysfs_emit(page, "%s\n", ptp->info->name);
+}
+static DEVICE_ATTR_RO(clock_name);
+
+static ssize_t max_phase_adjustment_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *page)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+
+	return sysfs_emit(page, "%d\n", ptp->info->getmaxphase(ptp->info));
+}
+static DEVICE_ATTR_RO(max_phase_adjustment);
+
+#define PTP_SHOW_INT(name, var)						\
+static ssize_t var##_show(struct device *dev,				\
+			   struct device_attribute *attr, char *page)	\
+{									\
+	struct ptp_clock *ptp = dev_get_drvdata(dev);			\
+	return sysfs_emit(page, "%d\n", ptp->info->var);	\
+}									\
+static DEVICE_ATTR(name, 0444, var##_show, NULL);
+
+PTP_SHOW_INT(max_adjustment, max_adj);
+PTP_SHOW_INT(n_alarms, n_alarm);
+PTP_SHOW_INT(n_external_timestamps, n_ext_ts);
+PTP_SHOW_INT(n_periodic_outputs, n_per_out);
+PTP_SHOW_INT(n_programmable_pins, n_pins);
+PTP_SHOW_INT(pps_available, pps);
+
+static ssize_t extts_enable_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t count)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	struct ptp_clock_info *ops = ptp->info;
+	struct ptp_clock_request req = { .type = PTP_CLK_REQ_EXTTS };
+	int cnt, enable;
+	int err = -EINVAL;
+
+	cnt = sscanf(buf, "%u %d", &req.extts.index, &enable);
+	if (cnt != 2)
+		goto out;
+	if (req.extts.index >= ops->n_ext_ts)
+		goto out;
+
+	err = ops->enable(ops, &req, enable ? 1 : 0);
+	if (err)
+		goto out;
+
+	return count;
+out:
+	return err;
+}
+static DEVICE_ATTR(extts_enable, 0220, NULL, extts_enable_store);
+
+static ssize_t extts_fifo_show(struct device *dev,
+			       struct device_attribute *attr, char *page)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	struct timestamp_event_queue *queue;
+	struct ptp_extts_event event;
+	unsigned long flags;
+	size_t qcnt;
+	int cnt = 0;
+
+	cnt = list_count_nodes(&ptp->tsevqs);
+	if (cnt <= 0)
+		goto out;
+
+	/* The sysfs fifo will always draw from the fist queue */
+	queue = list_first_entry(&ptp->tsevqs, struct timestamp_event_queue,
+				 qlist);
+
+	memset(&event, 0, sizeof(event));
+	spin_lock_irqsave(&queue->lock, flags);
+	qcnt = queue_cnt(queue);
+	if (qcnt) {
+		event = queue->buf[queue->head];
+		/* Paired with READ_ONCE() in queue_cnt() */
+		WRITE_ONCE(queue->head, (queue->head + 1) % PTP_MAX_TIMESTAMPS);
+	}
+	spin_unlock_irqrestore(&queue->lock, flags);
+
+	if (!qcnt)
+		goto out;
+
+	cnt = sysfs_emit(page, "%u %lld %u\n",
+			 event.index, event.t.sec, event.t.nsec);
+out:
+	return cnt;
+}
+static DEVICE_ATTR(fifo, 0444, extts_fifo_show, NULL);
+
+static ssize_t period_store(struct device *dev,
+			    struct device_attribute *attr,
+			    const char *buf, size_t count)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	struct ptp_clock_info *ops = ptp->info;
+	struct ptp_clock_request req = { .type = PTP_CLK_REQ_PEROUT };
+	int cnt, enable, err = -EINVAL;
+
+	cnt = sscanf(buf, "%u %lld %u %lld %u", &req.perout.index,
+		     &req.perout.start.sec, &req.perout.start.nsec,
+		     &req.perout.period.sec, &req.perout.period.nsec);
+	if (cnt != 5)
+		goto out;
+	if (req.perout.index >= ops->n_per_out)
+		goto out;
+
+	enable = req.perout.period.sec || req.perout.period.nsec;
+	err = ops->enable(ops, &req, enable);
+	if (err)
+		goto out;
+
+	return count;
+out:
+	return err;
+}
+static DEVICE_ATTR(period, 0220, NULL, period_store);
+
+static ssize_t pps_enable_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	struct ptp_clock_info *ops = ptp->info;
+	struct ptp_clock_request req = { .type = PTP_CLK_REQ_PPS };
+	int cnt, enable;
+	int err = -EINVAL;
+
+	if (!capable(CAP_SYS_TIME))
+		return -EPERM;
+
+	cnt = sscanf(buf, "%d", &enable);
+	if (cnt != 1)
+		goto out;
+
+	err = ops->enable(ops, &req, enable ? 1 : 0);
+	if (err)
+		goto out;
+
+	return count;
+out:
+	return err;
+}
+static DEVICE_ATTR(pps_enable, 0220, NULL, pps_enable_store);
+
+static int unregister_vclock(struct device *dev, void *data)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	struct ptp_clock_info *info = ptp->info;
+	struct ptp_vclock *vclock;
+	u32 *num = data;
+
+	vclock = info_to_vclock(info);
+	dev_info(dev->parent, "delete virtual clock ptp%d\n",
+		 vclock->clock->index);
+
+	ptp_vclock_unregister(vclock);
+	(*num)--;
+
+	/* For break. Not error. */
+	if (*num == 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static ssize_t n_vclocks_show(struct device *dev,
+			      struct device_attribute *attr, char *page)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	ssize_t size;
+
+	if (mutex_lock_interruptible(&ptp->n_vclocks_mux))
+		return -ERESTARTSYS;
+
+	size = sysfs_emit(page, "%u\n", ptp->n_vclocks);
+
+	mutex_unlock(&ptp->n_vclocks_mux);
+
+	return size;
+}
+
+static ssize_t n_vclocks_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	struct ptp_vclock *vclock;
+	int err = -EINVAL;
+	u32 num, i;
+
+	if (kstrtou32(buf, 0, &num))
+		return err;
+
+	if (mutex_lock_interruptible(&ptp->n_vclocks_mux))
+		return -ERESTARTSYS;
+
+	if (num > ptp->max_vclocks) {
+		dev_err(dev, "max value is %d\n", ptp->max_vclocks);
+		goto out;
+	}
+
+	/* Need to create more vclocks */
+	if (num > ptp->n_vclocks) {
+		for (i = 0; i < num - ptp->n_vclocks; i++) {
+			vclock = ptp_vclock_register(ptp);
+			if (!vclock)
+				goto out;
+
+			*(ptp->vclock_index + ptp->n_vclocks + i) =
+				vclock->clock->index;
+
+			dev_info(dev, "new virtual clock ptp%d\n",
+				 vclock->clock->index);
+		}
+	}
+
+	/* Need to delete vclocks */
+	if (num < ptp->n_vclocks) {
+		i = ptp->n_vclocks - num;
+		device_for_each_child_reverse(dev, &i,
+					      unregister_vclock);
+
+		for (i = 1; i <= ptp->n_vclocks - num; i++)
+			*(ptp->vclock_index + ptp->n_vclocks - i) = -1;
+	}
+
+	/* Need to inform about changed physical clock behavior */
+	if (!ptp->has_cycles) {
+		if (num == 0)
+			dev_info(dev, "only physical clock in use now\n");
+		else
+			dev_info(dev, "guarantee physical clock free running\n");
+	}
+
+	ptp->n_vclocks = num;
+	mutex_unlock(&ptp->n_vclocks_mux);
+
+	return count;
+out:
+	mutex_unlock(&ptp->n_vclocks_mux);
+	return err;
+}
+static DEVICE_ATTR_RW(n_vclocks);
+
+static ssize_t max_vclocks_show(struct device *dev,
+				struct device_attribute *attr, char *page)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	ssize_t size;
+
+	size = sysfs_emit(page, "%u\n", ptp->max_vclocks);
+
+	return size;
+}
+
+static ssize_t max_vclocks_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	unsigned int *vclock_index;
+	int err = -EINVAL;
+	size_t size;
+	u32 max;
+
+	if (kstrtou32(buf, 0, &max) || max == 0 || max > PTP_MAX_VCLOCKS_LIMIT)
+		return -EINVAL;
+
+	if (max == ptp->max_vclocks)
+		return count;
+
+	if (mutex_lock_interruptible(&ptp->n_vclocks_mux))
+		return -ERESTARTSYS;
+
+	if (max < ptp->n_vclocks)
+		goto out;
+
+	vclock_index = kcalloc(max, sizeof(int), GFP_KERNEL);
+	if (!vclock_index) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	size = sizeof(int) * ptp->n_vclocks;
+	memcpy(vclock_index, ptp->vclock_index, size);
+
+	kfree(ptp->vclock_index);
+	ptp->vclock_index = vclock_index;
+	ptp->max_vclocks = max;
+
+	mutex_unlock(&ptp->n_vclocks_mux);
+
+	return count;
+out:
+	mutex_unlock(&ptp->n_vclocks_mux);
+	return err;
+}
+static DEVICE_ATTR_RW(max_vclocks);
+
+static struct attribute *ptp_attrs[] = {
+	&dev_attr_clock_name.attr,
+
+	&dev_attr_max_adjustment.attr,
+	&dev_attr_max_phase_adjustment.attr,
+	&dev_attr_n_alarms.attr,
+	&dev_attr_n_external_timestamps.attr,
+	&dev_attr_n_periodic_outputs.attr,
+	&dev_attr_n_programmable_pins.attr,
+	&dev_attr_pps_available.attr,
+
+	&dev_attr_extts_enable.attr,
+	&dev_attr_fifo.attr,
+	&dev_attr_period.attr,
+	&dev_attr_pps_enable.attr,
+	&dev_attr_n_vclocks.attr,
+	&dev_attr_max_vclocks.attr,
+	NULL
+};
+
+static umode_t ptp_is_attribute_visible(struct kobject *kobj,
+					struct attribute *attr, int n)
+{
+	struct device *dev = kobj_to_dev(kobj);
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	struct ptp_clock_info *info = ptp->info;
+	umode_t mode = attr->mode;
+
+	if (attr == &dev_attr_extts_enable.attr ||
+	    attr == &dev_attr_fifo.attr) {
+		if (!info->n_ext_ts)
+			mode = 0;
+	} else if (attr == &dev_attr_period.attr) {
+		if (!info->n_per_out)
+			mode = 0;
+	} else if (attr == &dev_attr_pps_enable.attr) {
+		if (!info->pps)
+			mode = 0;
+	} else if (attr == &dev_attr_n_vclocks.attr ||
+		   attr == &dev_attr_max_vclocks.attr) {
+		if (ptp->is_virtual_clock)
+			mode = 0;
+	} else if (attr == &dev_attr_max_phase_adjustment.attr) {
+		if (!info->adjphase || !info->getmaxphase)
+			mode = 0;
+	}
+
+	return mode;
+}
+
+static const struct attribute_group ptp_group = {
+	.is_visible	= ptp_is_attribute_visible,
+	.attrs		= ptp_attrs,
+};
+
+const struct attribute_group *ptp_groups[] = {
+	&ptp_group,
+	NULL
+};
+
+static int ptp_pin_name2index(struct ptp_clock *ptp, const char *name)
+{
+	int i;
+	for (i = 0; i < ptp->info->n_pins; i++) {
+		if (!strcmp(ptp->info->pin_config[i].name, name))
+			return i;
+	}
+	return -1;
+}
+
+static ssize_t ptp_pin_show(struct device *dev, struct device_attribute *attr,
+			    char *page)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	unsigned int func, chan;
+	int index;
+
+	index = ptp_pin_name2index(ptp, attr->attr.name);
+	if (index < 0)
+		return -EINVAL;
+
+	if (mutex_lock_interruptible(&ptp->pincfg_mux))
+		return -ERESTARTSYS;
+
+	func = ptp->info->pin_config[index].func;
+	chan = ptp->info->pin_config[index].chan;
+
+	mutex_unlock(&ptp->pincfg_mux);
+
+	return sysfs_emit(page, "%u %u\n", func, chan);
+}
+
+static ssize_t ptp_pin_store(struct device *dev, struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct ptp_clock *ptp = dev_get_drvdata(dev);
+	unsigned int func, chan;
+	int cnt, err, index;
+
+	cnt = sscanf(buf, "%u %u", &func, &chan);
+	if (cnt != 2)
+		return -EINVAL;
+
+	index = ptp_pin_name2index(ptp, attr->attr.name);
+	if (index < 0)
+		return -EINVAL;
+
+	if (mutex_lock_interruptible(&ptp->pincfg_mux))
+		return -ERESTARTSYS;
+	err = ptp_set_pinfunc(ptp, index, func, chan);
+	mutex_unlock(&ptp->pincfg_mux);
+	if (err)
+		return err;
+
+	return count;
+}
+
+int ptp_populate_pin_groups(struct ptp_clock *ptp)
+{
+	struct ptp_clock_info *info = ptp->info;
+	int err = -ENOMEM, i, n_pins = info->n_pins;
+
+	if (!n_pins)
+		return 0;
+
+	ptp->pin_dev_attr = kzalloc_objs(*ptp->pin_dev_attr, n_pins);
+	if (!ptp->pin_dev_attr)
+		goto no_dev_attr;
+
+	ptp->pin_attr = kzalloc_objs(*ptp->pin_attr, 1 + n_pins);
+	if (!ptp->pin_attr)
+		goto no_pin_attr;
+
+	for (i = 0; i < n_pins; i++) {
+		struct device_attribute *da = &ptp->pin_dev_attr[i];
+		sysfs_attr_init(&da->attr);
+		da->attr.name = info->pin_config[i].name;
+		da->attr.mode = 0644;
+		da->show = ptp_pin_show;
+		da->store = ptp_pin_store;
+		ptp->pin_attr[i] = &da->attr;
+	}
+
+	ptp->pin_attr_group.name = "pins";
+	ptp->pin_attr_group.attrs = ptp->pin_attr;
+
+	ptp->pin_attr_groups[0] = &ptp->pin_attr_group;
+
+	return 0;
+
+no_pin_attr:
+	kfree(ptp->pin_dev_attr);
+no_dev_attr:
+	return err;
+}
+
+void ptp_cleanup_pin_groups(struct ptp_clock *ptp)
+{
+	kfree(ptp->pin_attr);
+	kfree(ptp->pin_dev_attr);
+}

linux-7.2.3/drivers/ptp/ptp_vclock.c

@@ -0,0 +1,306 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * PTP virtual clock driver
+ *
+ * Copyright 2021 NXP
+ */
+#include <linux/slab.h>
+#include <linux/hashtable.h>
+#include "ptp_private.h"
+
+#define PTP_VCLOCK_CC_SHIFT		31
+#define PTP_VCLOCK_CC_MULT		(1 << PTP_VCLOCK_CC_SHIFT)
+#define PTP_VCLOCK_FADJ_SHIFT		9
+#define PTP_VCLOCK_FADJ_DENOMINATOR	15625ULL
+#define PTP_VCLOCK_REFRESH_INTERVAL	(HZ * 2)
+
+/* protects vclock_hash addition/deletion */
+static DEFINE_SPINLOCK(vclock_hash_lock);
+
+static DEFINE_READ_MOSTLY_HASHTABLE(vclock_hash, 8);
+
+DEFINE_STATIC_SRCU(vclock_srcu);
+
+static void ptp_vclock_hash_add(struct ptp_vclock *vclock)
+{
+	spin_lock(&vclock_hash_lock);
+
+	hlist_add_head_rcu(&vclock->vclock_hash_node,
+			   &vclock_hash[vclock->clock->index % HASH_SIZE(vclock_hash)]);
+
+	spin_unlock(&vclock_hash_lock);
+}
+
+static void ptp_vclock_hash_del(struct ptp_vclock *vclock)
+{
+	spin_lock(&vclock_hash_lock);
+
+	hlist_del_init_rcu(&vclock->vclock_hash_node);
+
+	spin_unlock(&vclock_hash_lock);
+
+	synchronize_srcu(&vclock_srcu);
+}
+
+static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+	struct ptp_vclock *vclock = info_to_vclock(ptp);
+	s64 adj;
+
+	adj = (s64)scaled_ppm << PTP_VCLOCK_FADJ_SHIFT;
+	adj = div_s64(adj, PTP_VCLOCK_FADJ_DENOMINATOR);
+
+	if (mutex_lock_interruptible(&vclock->lock))
+		return -EINTR;
+	timecounter_read(&vclock->tc);
+	vclock->cc.mult = PTP_VCLOCK_CC_MULT + adj;
+	mutex_unlock(&vclock->lock);
+
+	return 0;
+}
+
+static int ptp_vclock_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct ptp_vclock *vclock = info_to_vclock(ptp);
+
+	if (mutex_lock_interruptible(&vclock->lock))
+		return -EINTR;
+	timecounter_adjtime(&vclock->tc, delta);
+	mutex_unlock(&vclock->lock);
+
+	return 0;
+}
+
+static int ptp_vclock_gettime(struct ptp_clock_info *ptp,
+			      struct timespec64 *ts)
+{
+	struct ptp_vclock *vclock = info_to_vclock(ptp);
+	u64 ns;
+
+	if (mutex_lock_interruptible(&vclock->lock))
+		return -EINTR;
+	ns = timecounter_read(&vclock->tc);
+	mutex_unlock(&vclock->lock);
+	*ts = ns_to_timespec64(ns);
+
+	return 0;
+}
+
+static int ptp_vclock_gettimex(struct ptp_clock_info *ptp,
+			       struct timespec64 *ts,
+			       struct ptp_system_timestamp *sts)
+{
+	struct ptp_vclock *vclock = info_to_vclock(ptp);
+	struct ptp_clock *pptp = vclock->pclock;
+	struct timespec64 pts;
+	int err;
+	u64 ns;
+
+	err = pptp->info->getcyclesx64(pptp->info, &pts, sts);
+	if (err)
+		return err;
+
+	if (mutex_lock_interruptible(&vclock->lock))
+		return -EINTR;
+	ns = timecounter_cyc2time(&vclock->tc, timespec64_to_ns(&pts));
+	mutex_unlock(&vclock->lock);
+
+	*ts = ns_to_timespec64(ns);
+
+	return 0;
+}
+
+static int ptp_vclock_settime(struct ptp_clock_info *ptp,
+			      const struct timespec64 *ts)
+{
+	struct ptp_vclock *vclock = info_to_vclock(ptp);
+	u64 ns = timespec64_to_ns(ts);
+
+	if (mutex_lock_interruptible(&vclock->lock))
+		return -EINTR;
+	timecounter_init(&vclock->tc, &vclock->cc, ns);
+	mutex_unlock(&vclock->lock);
+
+	return 0;
+}
+
+static int ptp_vclock_getcrosststamp(struct ptp_clock_info *ptp,
+				     struct system_device_crosststamp *xtstamp)
+{
+	struct ptp_vclock *vclock = info_to_vclock(ptp);
+	struct ptp_clock *pptp = vclock->pclock;
+	int err;
+	u64 ns;
+
+	err = pptp->info->getcrosscycles(pptp->info, xtstamp);
+	if (err)
+		return err;
+
+	if (mutex_lock_interruptible(&vclock->lock))
+		return -EINTR;
+	ns = timecounter_cyc2time(&vclock->tc, ktime_to_ns(xtstamp->device));
+	mutex_unlock(&vclock->lock);
+
+	xtstamp->device = ns_to_ktime(ns);
+
+	return 0;
+}
+
+static long ptp_vclock_refresh(struct ptp_clock_info *ptp)
+{
+	struct ptp_vclock *vclock = info_to_vclock(ptp);
+	struct timespec64 ts;
+
+	ptp_vclock_gettime(&vclock->info, &ts);
+
+	return PTP_VCLOCK_REFRESH_INTERVAL;
+}
+
+static void ptp_vclock_set_subclass(struct ptp_clock *ptp)
+{
+	lockdep_set_subclass(&ptp->clock.rwsem, PTP_LOCK_VIRTUAL);
+}
+
+static const struct ptp_clock_info ptp_vclock_info = {
+	.owner		= THIS_MODULE,
+	.name		= "ptp virtual clock",
+	.max_adj	= 500000000,
+	.adjfine	= ptp_vclock_adjfine,
+	.adjtime	= ptp_vclock_adjtime,
+	.settime64	= ptp_vclock_settime,
+	.do_aux_work	= ptp_vclock_refresh,
+};
+
+static u64 ptp_vclock_read(struct cyclecounter *cc)
+{
+	struct ptp_vclock *vclock = cc_to_vclock(cc);
+	struct ptp_clock *ptp = vclock->pclock;
+	struct timespec64 ts = {};
+
+	ptp->info->getcycles64(ptp->info, &ts);
+
+	return timespec64_to_ns(&ts);
+}
+
+static const struct cyclecounter ptp_vclock_cc = {
+	.read	= ptp_vclock_read,
+	.mask	= CYCLECOUNTER_MASK(32),
+	.mult	= PTP_VCLOCK_CC_MULT,
+	.shift	= PTP_VCLOCK_CC_SHIFT,
+};
+
+struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock)
+{
+	struct ptp_vclock *vclock;
+
+	vclock = kzalloc_obj(*vclock);
+	if (!vclock)
+		return NULL;
+
+	vclock->pclock = pclock;
+	vclock->info = ptp_vclock_info;
+	if (pclock->info->getcyclesx64)
+		vclock->info.gettimex64 = ptp_vclock_gettimex;
+	else
+		vclock->info.gettime64 = ptp_vclock_gettime;
+	if (pclock->info->getcrosscycles)
+		vclock->info.getcrosststamp = ptp_vclock_getcrosststamp;
+	vclock->cc = ptp_vclock_cc;
+
+	snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt",
+		 pclock->index);
+
+	INIT_HLIST_NODE(&vclock->vclock_hash_node);
+
+	mutex_init(&vclock->lock);
+
+	vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev);
+	if (IS_ERR_OR_NULL(vclock->clock)) {
+		kfree(vclock);
+		return NULL;
+	}
+
+	ptp_vclock_set_subclass(vclock->clock);
+
+	timecounter_init(&vclock->tc, &vclock->cc, 0);
+	ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL);
+
+	ptp_vclock_hash_add(vclock);
+
+	return vclock;
+}
+
+void ptp_vclock_unregister(struct ptp_vclock *vclock)
+{
+	ptp_vclock_hash_del(vclock);
+
+	ptp_clock_unregister(vclock->clock);
+	kfree(vclock);
+}
+
+#if IS_BUILTIN(CONFIG_PTP_1588_CLOCK)
+int ptp_get_vclocks_index(int pclock_index, int **vclock_index)
+{
+	char name[PTP_CLOCK_NAME_LEN] = "";
+	struct ptp_clock *ptp;
+	struct device *dev;
+	int num = 0;
+
+	if (pclock_index < 0)
+		return num;
+
+	snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", pclock_index);
+	dev = class_find_device_by_name(&ptp_class, name);
+	if (!dev)
+		return num;
+
+	ptp = dev_get_drvdata(dev);
+
+	if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) {
+		put_device(dev);
+		return num;
+	}
+
+	*vclock_index = kzalloc(sizeof(int) * ptp->n_vclocks, GFP_KERNEL);
+	if (!(*vclock_index))
+		goto out;
+
+	memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks);
+	num = ptp->n_vclocks;
+out:
+	mutex_unlock(&ptp->n_vclocks_mux);
+	put_device(dev);
+	return num;
+}
+EXPORT_SYMBOL(ptp_get_vclocks_index);
+
+ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index)
+{
+	unsigned int hash = vclock_index % HASH_SIZE(vclock_hash);
+	struct ptp_vclock *vclock;
+	u64 vclock_ns = 0;
+	int srcu_idx;
+	u64 ns;
+
+	ns = ktime_to_ns(*hwtstamp);
+
+	srcu_idx = srcu_read_lock(&vclock_srcu);
+
+	hlist_for_each_entry_srcu(vclock, &vclock_hash[hash], vclock_hash_node,
+				  srcu_read_lock_held(&vclock_srcu)) {
+		if (vclock->clock->index != vclock_index)
+			continue;
+
+		if (mutex_lock_interruptible(&vclock->lock))
+			break;
+		vclock_ns = timecounter_cyc2time(&vclock->tc, ns);
+		mutex_unlock(&vclock->lock);
+		break;
+	}
+
+	srcu_read_unlock(&vclock_srcu, srcu_idx);
+
+	return ns_to_ktime(vclock_ns);
+}
+EXPORT_SYMBOL(ptp_convert_timestamp);
+#endif

linux-7.2.3/drivers/ptp/ptp_vmclock.c

@@ -0,0 +1,797 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Virtual PTP 1588 clock for use with LM-safe VMclock device.
+ *
+ * Copyright © 2024 Amazon.com, Inc. or its affiliates.
+ */
+
+#include "linux/poll.h"
+#include "linux/types.h"
+#include "linux/wait.h"
+#include <linux/acpi.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/miscdevice.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <uapi/linux/vmclock-abi.h>
+
+#include <linux/ptp_clock_kernel.h>
+
+#ifdef CONFIG_X86
+#include <asm/pvclock.h>
+#include <asm/kvmclock.h>
+#endif
+
+#ifdef CONFIG_KVM_GUEST
+#define SUPPORT_KVMCLOCK
+#endif
+
+static DEFINE_IDA(vmclock_ida);
+
+ACPI_MODULE_NAME("vmclock");
+
+struct vmclock_state {
+	struct resource res;
+	struct vmclock_abi *clk;
+	struct miscdevice miscdev;
+	wait_queue_head_t disrupt_wait;
+	struct ptp_clock_info ptp_clock_info;
+	struct ptp_clock *ptp_clock;
+	enum clocksource_ids cs_id, sys_cs_id;
+	int index;
+	char *name;
+};
+
+#define VMCLOCK_MAX_WAIT ms_to_ktime(100)
+
+/* Require at least the flags field to be present. All else can be optional. */
+#define VMCLOCK_MIN_SIZE offsetof(struct vmclock_abi, pad)
+
+#define VMCLOCK_FIELD_PRESENT(_c, _f)			  \
+	(le32_to_cpu((_c)->size) >= (offsetof(struct vmclock_abi, _f) +	\
+				     sizeof((_c)->_f)))
+
+/*
+ * Multiply a 64-bit count by a 64-bit tick 'period' in units of seconds >> 64
+ * and add the fractional second part of the reference time.
+ *
+ * The result is a 128-bit value, the top 64 bits of which are seconds, and
+ * the low 64 bits are (seconds >> 64).
+ */
+static uint64_t mul_u64_u64_shr_add_u64(uint64_t *res_hi, uint64_t delta,
+					uint64_t period, uint8_t shift,
+					uint64_t frac_sec)
+{
+	unsigned __int128 res = (unsigned __int128)delta * period;
+
+	res >>= shift;
+	res += frac_sec;
+	*res_hi = res >> 64;
+	return (uint64_t)res;
+}
+
+static bool tai_adjust(struct vmclock_abi *clk, uint64_t *sec)
+{
+	if (clk->time_type == VMCLOCK_TIME_TAI)
+		return true;
+
+	if (clk->time_type == VMCLOCK_TIME_UTC &&
+	    (le64_to_cpu(clk->flags) & VMCLOCK_FLAG_TAI_OFFSET_VALID)) {
+		if (sec)
+			*sec -= (int16_t)le16_to_cpu(clk->tai_offset_sec);
+		return true;
+	}
+	return false;
+}
+
+static int vmclock_get_crosststamp(struct vmclock_state *st,
+				   struct ptp_system_timestamp *sts,
+				   struct system_counterval_t *system_counter,
+				   struct timespec64 *tspec)
+{
+	ktime_t deadline = ktime_add(ktime_get(), VMCLOCK_MAX_WAIT);
+	uint64_t cycle, delta, seq, frac_sec;
+
+#ifdef CONFIG_X86
+	/*
+	 * We'd expect the hypervisor to know this and to report the clock
+	 * status as VMCLOCK_STATUS_UNRELIABLE. But be paranoid.
+	 */
+	if (check_tsc_unstable())
+		return -EINVAL;
+#endif
+
+	while (1) {
+		seq = le32_to_cpu(st->clk->seq_count) & ~1ULL;
+
+		/*
+		 * This pairs with a write barrier in the hypervisor
+		 * which populates this structure.
+		 */
+		virt_rmb();
+
+		if (st->clk->clock_status == VMCLOCK_STATUS_UNRELIABLE)
+			return -EINVAL;
+
+		/*
+		 * When invoked for gettimex64(), fill in the pre/post system
+		 * times. The simple case is when system time is based on the
+		 * same counter as st->cs_id, in which case all three times
+		 * will be derived from the *same* counter value.
+		 *
+		 * If the system isn't using the same counter, then the value
+		 * from ptp_read_system_prets() will still be used as pre_ts,
+		 * and ptp_read_system_postts() is called to populate postts
+		 * after calling get_cycles().
+		 */
+		if (sts) {
+			ptp_read_system_prets(sts);
+			if (sts->pre_sts.cs_id == st->cs_id) {
+				cycle = sts->pre_sts.cycles;
+				sts->post_sts = sts->pre_sts;
+			} else if (sts->pre_sts.hw_csid == st->cs_id &&
+				   sts->pre_sts.hw_cycles) {
+				cycle = sts->pre_sts.hw_cycles;
+				sts->post_sts = sts->pre_sts;
+			} else {
+				cycle = get_cycles();
+				ptp_read_system_postts(sts);
+			}
+		} else {
+			cycle = get_cycles();
+		}
+
+		delta = cycle - le64_to_cpu(st->clk->counter_value);
+
+		frac_sec = mul_u64_u64_shr_add_u64(&tspec->tv_sec, delta,
+						   le64_to_cpu(st->clk->counter_period_frac_sec),
+						   st->clk->counter_period_shift,
+						   le64_to_cpu(st->clk->time_frac_sec));
+		tspec->tv_nsec = mul_u64_u64_shr(frac_sec, NSEC_PER_SEC, 64);
+		tspec->tv_sec += le64_to_cpu(st->clk->time_sec);
+
+		if (!tai_adjust(st->clk, &tspec->tv_sec))
+			return -EINVAL;
+
+		/*
+		 * This pairs with a write barrier in the hypervisor
+		 * which populates this structure.
+		 */
+		virt_rmb();
+		if (seq == le32_to_cpu(st->clk->seq_count))
+			break;
+
+		if (ktime_after(ktime_get(), deadline))
+			return -ETIMEDOUT;
+	}
+
+	if (system_counter) {
+		system_counter->cycles = cycle;
+		system_counter->cs_id = st->cs_id;
+	}
+
+	return 0;
+}
+
+#ifdef SUPPORT_KVMCLOCK
+/*
+ * In the case where the system is using the KVM clock for timekeeping, convert
+ * the TSC value into a KVM clock time in order to return a paired reading that
+ * get_device_system_crosststamp() can cope with.
+ */
+static int vmclock_get_crosststamp_kvmclock(struct vmclock_state *st,
+					    struct ptp_system_timestamp *sts,
+					    struct system_counterval_t *system_counter,
+					    struct timespec64 *tspec)
+{
+	struct pvclock_vcpu_time_info *pvti = this_cpu_pvti();
+	unsigned int pvti_ver;
+	int ret;
+
+	preempt_disable_notrace();
+
+	do {
+		pvti_ver = pvclock_read_begin(pvti);
+
+		ret = vmclock_get_crosststamp(st, sts, system_counter, tspec);
+		if (ret)
+			break;
+
+		system_counter->cycles = __pvclock_read_cycles(pvti,
+							       system_counter->cycles);
+		system_counter->cs_id = CSID_X86_KVM_CLK;
+
+		/*
+		 * This retry should never really happen; if the TSC is
+		 * stable and reliable enough across vCPUS that it is sane
+		 * for the hypervisor to expose a VMCLOCK device which uses
+		 * it as the reference counter, then the KVM clock sohuld be
+		 * in 'master clock mode' and basically never changed. But
+		 * the KVM clock is a fickle and often broken thing, so do
+		 * it "properly" just in case.
+		 */
+	} while (pvclock_read_retry(pvti, pvti_ver));
+
+	preempt_enable_notrace();
+
+	return ret;
+}
+#endif
+
+static int ptp_vmclock_get_time_fn(ktime_t *device_time,
+				   struct system_counterval_t *system_counter,
+				   void *ctx)
+{
+	struct vmclock_state *st = ctx;
+	struct timespec64 tspec;
+	int ret;
+
+#ifdef SUPPORT_KVMCLOCK
+	if (READ_ONCE(st->sys_cs_id) == CSID_X86_KVM_CLK)
+		ret = vmclock_get_crosststamp_kvmclock(st, NULL, system_counter,
+						       &tspec);
+	else
+#endif
+		ret = vmclock_get_crosststamp(st, NULL, system_counter, &tspec);
+
+	if (!ret)
+		*device_time = timespec64_to_ktime(tspec);
+
+	return ret;
+}
+
+static int ptp_vmclock_getcrosststamp(struct ptp_clock_info *ptp,
+				      struct system_device_crosststamp *xtstamp)
+{
+	struct vmclock_state *st = container_of(ptp, struct vmclock_state,
+						ptp_clock_info);
+	int ret = get_device_system_crosststamp(ptp_vmclock_get_time_fn, st,
+						NULL, xtstamp);
+#ifdef SUPPORT_KVMCLOCK
+	/*
+	 * On x86, the KVM clock may be used for the system time. We can
+	 * actually convert a TSC reading to that, and return a paired
+	 * timestamp that get_device_system_crosststamp() *can* handle.
+	 */
+	if (ret == -ENODEV) {
+		struct system_time_snapshot systime_snapshot;
+
+		ktime_get_snapshot_id(CLOCK_REALTIME, &systime_snapshot);
+
+		if (systime_snapshot.cs_id == CSID_X86_TSC ||
+		    systime_snapshot.cs_id == CSID_X86_KVM_CLK) {
+			WRITE_ONCE(st->sys_cs_id, systime_snapshot.cs_id);
+			ret = get_device_system_crosststamp(ptp_vmclock_get_time_fn,
+							    st, NULL, xtstamp);
+		}
+	}
+#endif
+	return ret;
+}
+
+/*
+ * PTP clock operations
+ */
+
+static int ptp_vmclock_adjfine(struct ptp_clock_info *ptp, long delta)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ptp_vmclock_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ptp_vmclock_settime(struct ptp_clock_info *ptp,
+			   const struct timespec64 *ts)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ptp_vmclock_gettimex(struct ptp_clock_info *ptp, struct timespec64 *ts,
+				struct ptp_system_timestamp *sts)
+{
+	struct vmclock_state *st = container_of(ptp, struct vmclock_state,
+						ptp_clock_info);
+
+	return vmclock_get_crosststamp(st, sts, NULL, ts);
+}
+
+static int ptp_vmclock_enable(struct ptp_clock_info *ptp,
+			  struct ptp_clock_request *rq, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static const struct ptp_clock_info ptp_vmclock_info = {
+	.owner		= THIS_MODULE,
+	.max_adj	= 0,
+	.n_ext_ts	= 0,
+	.n_pins		= 0,
+	.pps		= 0,
+	.adjfine	= ptp_vmclock_adjfine,
+	.adjtime	= ptp_vmclock_adjtime,
+	.gettimex64	= ptp_vmclock_gettimex,
+	.settime64	= ptp_vmclock_settime,
+	.enable		= ptp_vmclock_enable,
+	.getcrosststamp = ptp_vmclock_getcrosststamp,
+};
+
+static struct ptp_clock *vmclock_ptp_register(struct device *dev,
+					      struct vmclock_state *st)
+{
+	enum clocksource_ids cs_id;
+
+	if (IS_ENABLED(CONFIG_ARM64) &&
+	    st->clk->counter_id == VMCLOCK_COUNTER_ARM_VCNT) {
+		/* Can we check it's the virtual counter? */
+		cs_id = CSID_ARM_ARCH_COUNTER;
+	} else if (IS_ENABLED(CONFIG_X86) &&
+		   st->clk->counter_id == VMCLOCK_COUNTER_X86_TSC) {
+		cs_id = CSID_X86_TSC;
+	} else {
+		return NULL;
+	}
+
+	/* Accept TAI directly, or UTC with valid offset for conversion to TAI */
+	if (!tai_adjust(st->clk, NULL)) {
+		dev_info(dev, "vmclock does not provide unambiguous time\n");
+		return NULL;
+	}
+
+	st->sys_cs_id = cs_id;
+	st->cs_id = cs_id;
+	st->ptp_clock_info = ptp_vmclock_info;
+	strscpy(st->ptp_clock_info.name, st->name);
+
+	return ptp_clock_register(&st->ptp_clock_info, dev);
+}
+
+struct vmclock_file_state {
+	struct vmclock_state *st;
+	atomic_t seq;
+};
+
+static int vmclock_miscdev_mmap(struct file *fp, struct vm_area_struct *vma)
+{
+	struct vmclock_file_state *fst = fp->private_data;
+	struct vmclock_state *st = fst->st;
+
+	if ((vma->vm_flags & (VM_READ|VM_WRITE)) != VM_READ)
+		return -EROFS;
+
+	/*
+	 * Restrict the read-only mapping so it cannot be upgraded to
+	 * writable later with mprotect().
+	 */
+	vm_flags_clear(vma, VM_MAYWRITE);
+
+	if (vma->vm_end - vma->vm_start != PAGE_SIZE || vma->vm_pgoff)
+		return -EINVAL;
+
+	if (io_remap_pfn_range(vma, vma->vm_start,
+			       st->res.start >> PAGE_SHIFT, PAGE_SIZE,
+			       vma->vm_page_prot))
+		return -EAGAIN;
+
+	return 0;
+}
+
+static ssize_t vmclock_miscdev_read(struct file *fp, char __user *buf,
+				    size_t count, loff_t *ppos)
+{
+	ktime_t deadline = ktime_add(ktime_get(), VMCLOCK_MAX_WAIT);
+	struct vmclock_file_state *fst = fp->private_data;
+	struct vmclock_state *st = fst->st;
+	uint32_t seq, old_seq;
+	size_t max_count;
+
+	if (*ppos >= PAGE_SIZE)
+		return 0;
+
+	max_count = PAGE_SIZE - *ppos;
+	if (count > max_count)
+		count = max_count;
+
+	old_seq = atomic_read(&fst->seq);
+	while (1) {
+		seq = le32_to_cpu(st->clk->seq_count) & ~1U;
+		/* Pairs with hypervisor wmb */
+		virt_rmb();
+
+		if (copy_to_user(buf, ((char *)st->clk) + *ppos, count))
+			return -EFAULT;
+
+		/* Pairs with hypervisor wmb */
+		virt_rmb();
+		if (seq == le32_to_cpu(st->clk->seq_count)) {
+			/*
+			 * Either we updated fst->seq to seq (the latest version we observed)
+			 * or someone else did (old_seq == seq), so we can break.
+			 */
+			if (atomic_try_cmpxchg(&fst->seq, &old_seq, seq) ||
+			    old_seq == seq) {
+				break;
+			}
+		}
+
+		if (ktime_after(ktime_get(), deadline))
+			return -ETIMEDOUT;
+	}
+
+	*ppos += count;
+	return count;
+}
+
+static __poll_t vmclock_miscdev_poll(struct file *fp, poll_table *wait)
+{
+	struct vmclock_file_state *fst = fp->private_data;
+	struct vmclock_state *st = fst->st;
+	uint32_t seq;
+
+	/*
+	 * Hypervisor will not send us any notifications, so fail immediately
+	 * to avoid having caller sleeping for ever.
+	 */
+	if (!(le64_to_cpu(st->clk->flags) & VMCLOCK_FLAG_NOTIFICATION_PRESENT))
+		return POLLHUP;
+
+	poll_wait(fp, &st->disrupt_wait, wait);
+
+	seq = le32_to_cpu(st->clk->seq_count);
+	if (atomic_read(&fst->seq) != seq)
+		return POLLIN | POLLRDNORM;
+
+	return 0;
+}
+
+static int vmclock_miscdev_open(struct inode *inode, struct file *fp)
+{
+	struct vmclock_state *st = container_of(fp->private_data,
+						struct vmclock_state, miscdev);
+	struct vmclock_file_state *fst = kzalloc_obj(*fst);
+
+	if (!fst)
+		return -ENOMEM;
+
+	fst->st = st;
+	atomic_set(&fst->seq, 0);
+
+	fp->private_data = fst;
+
+	return 0;
+}
+
+static int vmclock_miscdev_release(struct inode *inode, struct file *fp)
+{
+	kfree(fp->private_data);
+	return 0;
+}
+
+static const struct file_operations vmclock_miscdev_fops = {
+	.owner = THIS_MODULE,
+	.open = vmclock_miscdev_open,
+	.release = vmclock_miscdev_release,
+	.mmap = vmclock_miscdev_mmap,
+	.read = vmclock_miscdev_read,
+	.poll = vmclock_miscdev_poll,
+};
+
+/* module operations */
+
+#if IS_ENABLED(CONFIG_ACPI)
+static acpi_status vmclock_acpi_resources(struct acpi_resource *ares, void *data)
+{
+	struct vmclock_state *st = data;
+	struct resource_win win;
+	struct resource *res = &win.res;
+
+	if (ares->type == ACPI_RESOURCE_TYPE_END_TAG)
+		return AE_OK;
+
+	/* There can be only one */
+	if (resource_type(&st->res) == IORESOURCE_MEM)
+		return AE_ERROR;
+
+	if (acpi_dev_resource_memory(ares, res) ||
+	    acpi_dev_resource_address_space(ares, &win)) {
+
+		if (resource_type(res) != IORESOURCE_MEM ||
+		    resource_size(res) < sizeof(st->clk))
+			return AE_ERROR;
+
+		st->res = *res;
+		return AE_OK;
+	}
+
+	return AE_ERROR;
+}
+
+static void
+vmclock_acpi_notification_handler(acpi_handle __always_unused handle,
+				  u32 __always_unused event, void *dev)
+{
+	struct device *device = dev;
+	struct vmclock_state *st = device->driver_data;
+
+	wake_up_interruptible(&st->disrupt_wait);
+}
+
+static int vmclock_setup_acpi_notification(struct device *dev)
+{
+	struct acpi_device *adev = ACPI_COMPANION(dev);
+	acpi_status status;
+
+	/*
+	 * This should never happen as this function is only called when
+	 * has_acpi_companion(dev) is true, but the logic is sufficiently
+	 * complex that Coverity can't see the tautology.
+	 */
+	if (!adev)
+		return -ENODEV;
+
+	status = acpi_install_notify_handler(adev->handle, ACPI_DEVICE_NOTIFY,
+					     vmclock_acpi_notification_handler,
+					     dev);
+	if (ACPI_FAILURE(status)) {
+		dev_err(dev, "failed to install notification handler");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int vmclock_probe_acpi(struct device *dev, struct vmclock_state *st)
+{
+	struct acpi_device *adev = ACPI_COMPANION(dev);
+	acpi_status status;
+
+	/*
+	 * This should never happen as this function is only called when
+	 * has_acpi_companion(dev) is true, but the logic is sufficiently
+	 * complex that Coverity can't see the tautology.
+	 */
+	if (!adev)
+		return -ENODEV;
+
+	status = acpi_walk_resources(adev->handle, METHOD_NAME__CRS,
+				     vmclock_acpi_resources, st);
+	if (ACPI_FAILURE(status) || resource_type(&st->res) != IORESOURCE_MEM) {
+		dev_err(dev, "failed to get resources\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+#endif /* CONFIG_ACPI */
+
+static irqreturn_t vmclock_of_irq_handler(int __always_unused irq, void *_st)
+{
+	struct vmclock_state *st = _st;
+
+	wake_up_interruptible(&st->disrupt_wait);
+	return IRQ_HANDLED;
+}
+
+static int vmclock_probe_dt(struct device *dev, struct vmclock_state *st)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	st->res = *res;
+
+	return 0;
+}
+
+static int vmclock_setup_of_notification(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	int irq;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	return devm_request_irq(dev, irq, vmclock_of_irq_handler, IRQF_SHARED,
+				"vmclock", dev->driver_data);
+}
+
+static int vmclock_setup_notification(struct device *dev,
+				      struct vmclock_state *st)
+{
+	/* The device does not support notifications. Nothing else to do */
+	if (!(le64_to_cpu(st->clk->flags) & VMCLOCK_FLAG_NOTIFICATION_PRESENT))
+		return 0;
+
+#if IS_ENABLED(CONFIG_ACPI)
+	if (has_acpi_companion(dev))
+		return vmclock_setup_acpi_notification(dev);
+#endif
+	return vmclock_setup_of_notification(dev);
+}
+
+static void vmclock_remove(void *data)
+{
+	struct device *dev = data;
+	struct vmclock_state *st = dev->driver_data;
+
+	if (!st) {
+		dev_err(dev, "%s called with NULL driver_data", __func__);
+		return;
+	}
+
+#if IS_ENABLED(CONFIG_ACPI)
+	if (has_acpi_companion(dev))
+		acpi_remove_notify_handler(ACPI_COMPANION(dev)->handle,
+					   ACPI_DEVICE_NOTIFY,
+					   vmclock_acpi_notification_handler);
+#endif
+
+	if (st->ptp_clock)
+		ptp_clock_unregister(st->ptp_clock);
+
+	if (st->miscdev.minor != MISC_DYNAMIC_MINOR)
+		misc_deregister(&st->miscdev);
+
+	dev->driver_data = NULL;
+}
+
+static void vmclock_put_idx(void *data)
+{
+	struct vmclock_state *st = data;
+
+	ida_free(&vmclock_ida, st->index);
+}
+
+static int vmclock_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct vmclock_state *st;
+	int ret;
+
+	st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
+	if (!st)
+		return -ENOMEM;
+
+#if IS_ENABLED(CONFIG_ACPI)
+	if (has_acpi_companion(dev))
+		ret = vmclock_probe_acpi(dev, st);
+	else
+#endif
+		ret = vmclock_probe_dt(dev, st);
+
+	if (ret) {
+		dev_info(dev, "Failed to obtain physical address: %d\n", ret);
+		return ret;
+	}
+
+	if (resource_size(&st->res) < VMCLOCK_MIN_SIZE) {
+		dev_info(dev, "Region too small (0x%llx)\n",
+			 resource_size(&st->res));
+		return -EINVAL;
+	}
+	st->clk = devm_memremap(dev, st->res.start, resource_size(&st->res),
+				MEMREMAP_WB | MEMREMAP_DEC);
+	if (IS_ERR(st->clk)) {
+		ret = PTR_ERR(st->clk);
+		dev_info(dev, "failed to map shared memory\n");
+		st->clk = NULL;
+		return ret;
+	}
+
+	if (le32_to_cpu(st->clk->magic) != VMCLOCK_MAGIC ||
+	    le32_to_cpu(st->clk->size) > resource_size(&st->res) ||
+	    le16_to_cpu(st->clk->version) != 1) {
+		dev_info(dev, "vmclock magic fields invalid\n");
+		return -EINVAL;
+	}
+
+	ret = ida_alloc(&vmclock_ida, GFP_KERNEL);
+	if (ret < 0)
+		return ret;
+
+	st->index = ret;
+	ret = devm_add_action_or_reset(&pdev->dev, vmclock_put_idx, st);
+	if (ret)
+		return ret;
+
+	st->name = devm_kasprintf(&pdev->dev, GFP_KERNEL, "vmclock%d", st->index);
+	if (!st->name)
+		return -ENOMEM;
+
+	st->miscdev.minor = MISC_DYNAMIC_MINOR;
+
+	init_waitqueue_head(&st->disrupt_wait);
+	dev->driver_data = st;
+
+	ret = devm_add_action_or_reset(&pdev->dev, vmclock_remove, dev);
+	if (ret)
+		return ret;
+
+	ret = vmclock_setup_notification(dev, st);
+	if (ret)
+		return ret;
+
+	/*
+	 * If the structure is big enough, it can be mapped to userspace.
+	 * Theoretically a guest OS even using larger pages could still
+	 * use 4KiB PTEs to map smaller MMIO regions like this, but let's
+	 * cross that bridge if/when we come to it.
+	 */
+	if (le32_to_cpu(st->clk->size) >= PAGE_SIZE) {
+		st->miscdev.fops = &vmclock_miscdev_fops;
+		st->miscdev.name = st->name;
+
+		ret = misc_register(&st->miscdev);
+		if (ret)
+			return ret;
+	}
+
+	/* If there is valid clock information, register a PTP clock */
+	if (VMCLOCK_FIELD_PRESENT(st->clk, time_frac_sec)) {
+		/* Can return a silent NULL, or an error. */
+		st->ptp_clock = vmclock_ptp_register(dev, st);
+		if (IS_ERR(st->ptp_clock)) {
+			ret = PTR_ERR(st->ptp_clock);
+			st->ptp_clock = NULL;
+			return ret;
+		}
+	}
+
+	if (!st->miscdev.minor && !st->ptp_clock) {
+		/* Neither miscdev nor PTP registered */
+		dev_info(dev, "vmclock: Neither miscdev nor PTP available; not registering\n");
+		return -ENODEV;
+	}
+
+	dev_info(dev, "%s: registered %s%s%s\n", st->name,
+		 st->miscdev.minor ? "miscdev" : "",
+		 (st->miscdev.minor && st->ptp_clock) ? ", " : "",
+		 st->ptp_clock ? "PTP" : "");
+
+	return 0;
+}
+
+static const struct acpi_device_id vmclock_acpi_ids[] = {
+	{ "AMZNC10C", 0 },
+	{ "VMCLOCK", 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(acpi, vmclock_acpi_ids);
+
+static const struct of_device_id vmclock_of_ids[] = {
+	{ .compatible = "amazon,vmclock", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, vmclock_of_ids);
+
+static struct platform_driver vmclock_platform_driver = {
+	.probe		= vmclock_probe,
+	.driver	= {
+		.name	= "vmclock",
+		.acpi_match_table = vmclock_acpi_ids,
+		.of_match_table = vmclock_of_ids,
+	},
+};
+
+module_platform_driver(vmclock_platform_driver)
+
+MODULE_AUTHOR("David Woodhouse <dwmw2@infradead.org>");
+MODULE_DESCRIPTION("PTP clock using VMCLOCK");
+MODULE_LICENSE("GPL");

linux-7.2.3/drivers/ptp/ptp_vmw.c

@@ -0,0 +1,134 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-2-Clause
+/*
+ * Copyright (C) 2020 VMware, Inc., Palo Alto, CA., USA
+ *
+ * PTP clock driver for VMware precision clock virtual device.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/acpi.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/ptp_clock_kernel.h>
+#include <asm/hypervisor.h>
+#include <asm/vmware.h>
+
+#define VMWARE_CMD_PCLK(nr) ((nr << 16) | 97)
+#define VMWARE_CMD_PCLK_GETTIME VMWARE_CMD_PCLK(0)
+
+static struct ptp_clock *ptp_vmw_clock;
+
+
+static int ptp_vmw_pclk_read(u64 *ns)
+{
+	u32 ret, nsec_hi, nsec_lo;
+
+	ret = vmware_hypercall3(VMWARE_CMD_PCLK_GETTIME, 0,
+				&nsec_hi, &nsec_lo);
+	if (ret == 0)
+		*ns = ((u64)nsec_hi << 32) | nsec_lo;
+	return ret;
+}
+
+/*
+ * PTP clock ops.
+ */
+
+static int ptp_vmw_adjtime(struct ptp_clock_info *info, s64 delta)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ptp_vmw_adjfine(struct ptp_clock_info *info, long delta)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ptp_vmw_gettime(struct ptp_clock_info *info, struct timespec64 *ts)
+{
+	u64 ns;
+
+	if (ptp_vmw_pclk_read(&ns) != 0)
+		return -EIO;
+	*ts = ns_to_timespec64(ns);
+	return 0;
+}
+
+static int ptp_vmw_settime(struct ptp_clock_info *info,
+			  const struct timespec64 *ts)
+{
+	return -EOPNOTSUPP;
+}
+
+static int ptp_vmw_enable(struct ptp_clock_info *info,
+			 struct ptp_clock_request *request, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+static struct ptp_clock_info ptp_vmw_clock_info = {
+	.owner		= THIS_MODULE,
+	.name		= "ptp_vmw",
+	.max_adj	= 0,
+	.adjtime	= ptp_vmw_adjtime,
+	.adjfine	= ptp_vmw_adjfine,
+	.gettime64	= ptp_vmw_gettime,
+	.settime64	= ptp_vmw_settime,
+	.enable		= ptp_vmw_enable,
+};
+
+/*
+ * ACPI driver ops for VMware "precision clock" virtual device.
+ */
+
+static int ptp_vmw_acpi_probe(struct platform_device *pdev)
+{
+	ptp_vmw_clock = ptp_clock_register(&ptp_vmw_clock_info, NULL);
+	if (IS_ERR(ptp_vmw_clock)) {
+		pr_err("failed to register ptp clock\n");
+		return PTR_ERR(ptp_vmw_clock);
+	}
+	return 0;
+}
+
+static void ptp_vmw_acpi_remove(struct platform_device *pdev)
+{
+	ptp_clock_unregister(ptp_vmw_clock);
+}
+
+static const struct acpi_device_id ptp_vmw_acpi_device_ids[] = {
+	{ "VMW0005", 0 },
+	{ "", 0 },
+};
+
+MODULE_DEVICE_TABLE(acpi, ptp_vmw_acpi_device_ids);
+
+static struct platform_driver ptp_vmw_acpi_driver = {
+	.probe = ptp_vmw_acpi_probe,
+	.remove = ptp_vmw_acpi_remove,
+	.driver = {
+		.name = "ptp_vmw_acpi",
+		.acpi_match_table = ptp_vmw_acpi_device_ids,
+	},
+};
+
+static int __init ptp_vmw_init(void)
+{
+	if (x86_hyper_type != X86_HYPER_VMWARE)
+		return -1;
+	return platform_driver_register(&ptp_vmw_acpi_driver);
+}
+
+static void __exit ptp_vmw_exit(void)
+{
+	platform_driver_unregister(&ptp_vmw_acpi_driver);
+}
+
+module_init(ptp_vmw_init);
+module_exit(ptp_vmw_exit);
+
+MODULE_DESCRIPTION("VMware virtual PTP clock driver");
+MODULE_AUTHOR("VMware, Inc.");
+MODULE_LICENSE("Dual BSD/GPL");