Adding upstream version 6.1.76.upstream/6.1.76upstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

1 files changed, 2481 insertions, 0 deletions

diff --git a/drivers/ptp/ptp_clockmatrix.c b/drivers/ptp/ptp_clockmatrix.c
new file mode 100644
index 000000000..c9d451bf8
--- /dev/null
+++ b/drivers/ptp/ptp_clockmatrix.c
@@ -0,0 +1,2481 @@
+// 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.
+ */
+#include <linux/firmware.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/delay.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/timekeeping.h>
+#include <linux/string.h>
+#include <linux/of.h>
+#include <linux/mfd/rsmu.h>
+#include <linux/mfd/idt8a340_reg.h>
+#include <asm/unaligned.h>
+
+#include "ptp_private.h"
+#include "ptp_clockmatrix.h"
+
+MODULE_DESCRIPTION("Driver for IDT ClockMatrix(TM) family");
+MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
+MODULE_AUTHOR("IDT support-1588 <IDT-support-1588@lm.renesas.com>");
+MODULE_VERSION("1.0");
+MODULE_LICENSE("GPL");
+
+/*
+ * The name of the firmware file to be loaded
+ * over-rides any automatic selection
+ */
+static char *firmware;
+module_param(firmware, charp, 0);
+
+#define SETTIME_CORRECTION (0)
+#define EXTTS_PERIOD_MS (95)
+
+static int _idtcm_adjfine(struct idtcm_channel *channel, long scaled_ppm);
+
+static inline int idtcm_read(struct idtcm *idtcm,
+			     u16 module,
+			     u16 regaddr,
+			     u8 *buf,
+			     u16 count)
+{
+	return regmap_bulk_read(idtcm->regmap, module + regaddr, buf, count);
+}
+
+static inline int idtcm_write(struct idtcm *idtcm,
+			      u16 module,
+			      u16 regaddr,
+			      u8 *buf,
+			      u16 count)
+{
+	return regmap_bulk_write(idtcm->regmap, module + regaddr, buf, count);
+}
+
+static int contains_full_configuration(struct idtcm *idtcm,
+				       const struct firmware *fw)
+{
+	struct idtcm_fwrc *rec = (struct idtcm_fwrc *)fw->data;
+	u16 scratch = IDTCM_FW_REG(idtcm->fw_ver, V520, SCRATCH);
+	s32 full_count;
+	s32 count = 0;
+	u16 regaddr;
+	u8 loaddr;
+	s32 len;
+
+	/* 4 bytes skipped every 0x80 */
+	full_count = (scratch - GPIO_USER_CONTROL) -
+		     ((scratch >> 7) - (GPIO_USER_CONTROL >> 7)) * 4;
+
+	/* If the firmware contains 'full configuration' SM_RESET can be used
+	 * to ensure proper configuration.
+	 *
+	 * Full configuration is defined as the number of programmable
+	 * bytes within the configuration range minus page offset addr range.
+	 */
+	for (len = fw->size; len > 0; len -= sizeof(*rec)) {
+		regaddr = rec->hiaddr << 8;
+		regaddr |= rec->loaddr;
+
+		loaddr = rec->loaddr;
+
+		rec++;
+
+		/* Top (status registers) and bottom are read-only */
+		if (regaddr < GPIO_USER_CONTROL || regaddr >= scratch)
+			continue;
+
+		/* Page size 128, last 4 bytes of page skipped */
+		if ((loaddr > 0x7b && loaddr <= 0x7f) || loaddr > 0xfb)
+			continue;
+
+		count++;
+	}
+
+	return (count >= full_count);
+}
+
+static int char_array_to_timespec(u8 *buf,
+				  u8 count,
+				  struct timespec64 *ts)
+{
+	u8 i;
+	u64 nsec;
+	time64_t sec;
+
+	if (count < TOD_BYTE_COUNT)
+		return 1;
+
+	/* Sub-nanoseconds are in buf[0]. */
+	nsec = buf[4];
+	for (i = 0; i < 3; i++) {
+		nsec <<= 8;
+		nsec |= buf[3 - i];
+	}
+
+	sec = buf[10];
+	for (i = 0; i < 5; i++) {
+		sec <<= 8;
+		sec |= buf[9 - i];
+	}
+
+	ts->tv_sec = sec;
+	ts->tv_nsec = nsec;
+
+	return 0;
+}
+
+static int timespec_to_char_array(struct timespec64 const *ts,
+				  u8 *buf,
+				  u8 count)
+{
+	u8 i;
+	s32 nsec;
+	time64_t sec;
+
+	if (count < TOD_BYTE_COUNT)
+		return 1;
+
+	nsec = ts->tv_nsec;
+	sec = ts->tv_sec;
+
+	/* Sub-nanoseconds are in buf[0]. */
+	buf[0] = 0;
+	for (i = 1; i < 5; i++) {
+		buf[i] = nsec & 0xff;
+		nsec >>= 8;
+	}
+
+	for (i = 5; i < TOD_BYTE_COUNT; i++) {
+
+		buf[i] = sec & 0xff;
+		sec >>= 8;
+	}
+
+	return 0;
+}
+
+static int idtcm_strverscmp(const char *version1, const char *version2)
+{
+	u8 ver1[3], ver2[3];
+	int i;
+
+	if (sscanf(version1, "%hhu.%hhu.%hhu",
+		   &ver1[0], &ver1[1], &ver1[2]) != 3)
+		return -1;
+	if (sscanf(version2, "%hhu.%hhu.%hhu",
+		   &ver2[0], &ver2[1], &ver2[2]) != 3)
+		return -1;
+
+	for (i = 0; i < 3; i++) {
+		if (ver1[i] > ver2[i])
+			return 1;
+		if (ver1[i] < ver2[i])
+			return -1;
+	}
+
+	return 0;
+}
+
+static enum fw_version idtcm_fw_version(const char *version)
+{
+	enum fw_version ver = V_DEFAULT;
+
+	if (idtcm_strverscmp(version, "4.8.7") >= 0)
+		ver = V487;
+
+	if (idtcm_strverscmp(version, "5.2.0") >= 0)
+		ver = V520;
+
+	return ver;
+}
+
+static int clear_boot_status(struct idtcm *idtcm)
+{
+	u8 buf[4] = {0};
+
+	return idtcm_write(idtcm, GENERAL_STATUS, BOOT_STATUS, buf, sizeof(buf));
+}
+
+static int read_boot_status(struct idtcm *idtcm, u32 *status)
+{
+	int err;
+	u8 buf[4] = {0};
+
+	err = idtcm_read(idtcm, GENERAL_STATUS, BOOT_STATUS, buf, sizeof(buf));
+
+	*status = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
+
+	return err;
+}
+
+static int wait_for_boot_status_ready(struct idtcm *idtcm)
+{
+	u32 status = 0;
+	u8 i = 30;	/* 30 * 100ms = 3s */
+	int err;
+
+	do {
+		err = read_boot_status(idtcm, &status);
+		if (err)
+			return err;
+
+		if (status == 0xA0)
+			return 0;
+
+		msleep(100);
+		i--;
+
+	} while (i);
+
+	dev_warn(idtcm->dev, "%s timed out", __func__);
+
+	return -EBUSY;
+}
+
+static int arm_tod_read_trig_sel_refclk(struct idtcm_channel *channel, u8 ref)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	u16 tod_read_cmd = IDTCM_FW_REG(idtcm->fw_ver, V520, TOD_READ_SECONDARY_CMD);
+	u8 val = 0;
+	int err;
+
+	val &= ~(WR_REF_INDEX_MASK << WR_REF_INDEX_SHIFT);
+	val |= (ref << WR_REF_INDEX_SHIFT);
+
+	err = idtcm_write(idtcm, channel->tod_read_secondary,
+			  TOD_READ_SECONDARY_SEL_CFG_0, &val, sizeof(val));
+	if (err)
+		return err;
+
+	val = 0 | (SCSR_TOD_READ_TRIG_SEL_REFCLK << TOD_READ_TRIGGER_SHIFT);
+
+	err = idtcm_write(idtcm, channel->tod_read_secondary, tod_read_cmd,
+			  &val, sizeof(val));
+	if (err)
+		dev_err(idtcm->dev, "%s: err = %d", __func__, err);
+
+	return err;
+}
+
+static bool is_single_shot(u8 mask)
+{
+	/* Treat single bit ToD masks as continuous trigger */
+	return !(mask <= 8 && is_power_of_2(mask));
+}
+
+static int idtcm_extts_enable(struct idtcm_channel *channel,
+			      struct ptp_clock_request *rq, int on)
+{
+	u8 index = rq->extts.index;
+	struct idtcm *idtcm;
+	u8 mask = 1 << index;
+	int err = 0;
+	u8 old_mask;
+	int ref;
+
+	idtcm = channel->idtcm;
+	old_mask = idtcm->extts_mask;
+
+	/* Reject requests with unsupported flags */
+	if (rq->extts.flags & ~(PTP_ENABLE_FEATURE |
+				PTP_RISING_EDGE |
+				PTP_FALLING_EDGE |
+				PTP_STRICT_FLAGS))
+		return -EOPNOTSUPP;
+
+	/* Reject requests to enable time stamping on falling edge */
+	if ((rq->extts.flags & PTP_ENABLE_FEATURE) &&
+	    (rq->extts.flags & PTP_FALLING_EDGE))
+		return -EOPNOTSUPP;
+
+	if (index >= MAX_TOD)
+		return -EINVAL;
+
+	if (on) {
+		/* Support triggering more than one TOD_0/1/2/3 by same pin */
+		/* Use the pin configured for the channel */
+		ref = ptp_find_pin(channel->ptp_clock, PTP_PF_EXTTS, channel->tod);
+
+		if (ref < 0) {
+			dev_err(idtcm->dev, "%s: No valid pin found for TOD%d!\n",
+				__func__, channel->tod);
+			return -EBUSY;
+		}
+
+		err = arm_tod_read_trig_sel_refclk(&idtcm->channel[index], ref);
+
+		if (err == 0) {
+			idtcm->extts_mask |= mask;
+			idtcm->event_channel[index] = channel;
+			idtcm->channel[index].refn = ref;
+			idtcm->extts_single_shot = is_single_shot(idtcm->extts_mask);
+
+			if (old_mask)
+				return 0;
+
+			schedule_delayed_work(&idtcm->extts_work,
+					      msecs_to_jiffies(EXTTS_PERIOD_MS));
+		}
+	} else {
+		idtcm->extts_mask &= ~mask;
+		idtcm->extts_single_shot = is_single_shot(idtcm->extts_mask);
+
+		if (idtcm->extts_mask == 0)
+			cancel_delayed_work(&idtcm->extts_work);
+	}
+
+	return err;
+}
+
+static int read_sys_apll_status(struct idtcm *idtcm, u8 *status)
+{
+	return idtcm_read(idtcm, STATUS, DPLL_SYS_APLL_STATUS, status,
+			  sizeof(u8));
+}
+
+static int read_sys_dpll_status(struct idtcm *idtcm, u8 *status)
+{
+	return idtcm_read(idtcm, STATUS, DPLL_SYS_STATUS, status, sizeof(u8));
+}
+
+static int wait_for_sys_apll_dpll_lock(struct idtcm *idtcm)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(LOCK_TIMEOUT_MS);
+	u8 apll = 0;
+	u8 dpll = 0;
+	int err;
+
+	do {
+		err = read_sys_apll_status(idtcm, &apll);
+		if (err)
+			return err;
+
+		err = read_sys_dpll_status(idtcm, &dpll);
+		if (err)
+			return err;
+
+		apll &= SYS_APLL_LOSS_LOCK_LIVE_MASK;
+		dpll &= DPLL_SYS_STATE_MASK;
+
+		if (apll == SYS_APLL_LOSS_LOCK_LIVE_LOCKED &&
+		    dpll == DPLL_STATE_LOCKED) {
+			return 0;
+		} else if (dpll == DPLL_STATE_FREERUN ||
+			   dpll == DPLL_STATE_HOLDOVER ||
+			   dpll == DPLL_STATE_OPEN_LOOP) {
+			dev_warn(idtcm->dev,
+				"No wait state: DPLL_SYS_STATE %d", dpll);
+			return -EPERM;
+		}
+
+		msleep(LOCK_POLL_INTERVAL_MS);
+	} while (time_is_after_jiffies(timeout));
+
+	dev_warn(idtcm->dev,
+		 "%d ms lock timeout: SYS APLL Loss Lock %d  SYS DPLL state %d",
+		 LOCK_TIMEOUT_MS, apll, dpll);
+
+	return -ETIME;
+}
+
+static void wait_for_chip_ready(struct idtcm *idtcm)
+{
+	if (wait_for_boot_status_ready(idtcm))
+		dev_warn(idtcm->dev, "BOOT_STATUS != 0xA0");
+
+	if (wait_for_sys_apll_dpll_lock(idtcm))
+		dev_warn(idtcm->dev,
+			 "Continuing while SYS APLL/DPLL is not locked");
+}
+
+static int _idtcm_gettime_triggered(struct idtcm_channel *channel,
+				    struct timespec64 *ts)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	u16 tod_read_cmd = IDTCM_FW_REG(idtcm->fw_ver, V520, TOD_READ_SECONDARY_CMD);
+	u8 buf[TOD_BYTE_COUNT];
+	u8 trigger;
+	int err;
+
+	err = idtcm_read(idtcm, channel->tod_read_secondary,
+			 tod_read_cmd, &trigger, sizeof(trigger));
+	if (err)
+		return err;
+
+	if (trigger & TOD_READ_TRIGGER_MASK)
+		return -EBUSY;
+
+	err = idtcm_read(idtcm, channel->tod_read_secondary,
+			 TOD_READ_SECONDARY_BASE, buf, sizeof(buf));
+	if (err)
+		return err;
+
+	return char_array_to_timespec(buf, sizeof(buf), ts);
+}
+
+static int _idtcm_gettime(struct idtcm_channel *channel,
+			  struct timespec64 *ts, u8 timeout)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	u16 tod_read_cmd = IDTCM_FW_REG(idtcm->fw_ver, V520, TOD_READ_PRIMARY_CMD);
+	u8 buf[TOD_BYTE_COUNT];
+	u8 trigger;
+	int err;
+
+	/* wait trigger to be 0 */
+	do {
+		if (timeout-- == 0)
+			return -EIO;
+
+		if (idtcm->calculate_overhead_flag)
+			idtcm->start_time = ktime_get_raw();
+
+		err = idtcm_read(idtcm, channel->tod_read_primary,
+				 tod_read_cmd, &trigger,
+				 sizeof(trigger));
+		if (err)
+			return err;
+	} while (trigger & TOD_READ_TRIGGER_MASK);
+
+	err = idtcm_read(idtcm, channel->tod_read_primary,
+			 TOD_READ_PRIMARY_BASE, buf, sizeof(buf));
+	if (err)
+		return err;
+
+	err = char_array_to_timespec(buf, sizeof(buf), ts);
+
+	return err;
+}
+
+static int idtcm_extts_check_channel(struct idtcm *idtcm, u8 todn)
+{
+	struct idtcm_channel *ptp_channel, *extts_channel;
+	struct ptp_clock_event event;
+	struct timespec64 ts;
+	u32 dco_delay = 0;
+	int err;
+
+	extts_channel = &idtcm->channel[todn];
+	ptp_channel = idtcm->event_channel[todn];
+
+	if (extts_channel == ptp_channel)
+		dco_delay = ptp_channel->dco_delay;
+
+	err = _idtcm_gettime_triggered(extts_channel, &ts);
+	if (err)
+		return err;
+
+	/* Triggered - save timestamp */
+	event.type = PTP_CLOCK_EXTTS;
+	event.index = todn;
+	event.timestamp = timespec64_to_ns(&ts) - dco_delay;
+	ptp_clock_event(ptp_channel->ptp_clock, &event);
+
+	return err;
+}
+
+static int _idtcm_gettime_immediate(struct idtcm_channel *channel,
+				    struct timespec64 *ts)
+{
+	struct idtcm *idtcm = channel->idtcm;
+
+	u16 tod_read_cmd = IDTCM_FW_REG(idtcm->fw_ver, V520, TOD_READ_PRIMARY_CMD);
+	u8 val = (SCSR_TOD_READ_TRIG_SEL_IMMEDIATE << TOD_READ_TRIGGER_SHIFT);
+	int err;
+
+	err = idtcm_write(idtcm, channel->tod_read_primary,
+			  tod_read_cmd, &val, sizeof(val));
+	if (err)
+		return err;
+
+	return _idtcm_gettime(channel, ts, 10);
+}
+
+static int _sync_pll_output(struct idtcm *idtcm,
+			    u8 pll,
+			    u8 sync_src,
+			    u8 qn,
+			    u8 qn_plus_1)
+{
+	int err;
+	u8 val;
+	u16 sync_ctrl0;
+	u16 sync_ctrl1;
+	u8 temp;
+
+	if (qn == 0 && qn_plus_1 == 0)
+		return 0;
+
+	switch (pll) {
+	case 0:
+		sync_ctrl0 = HW_Q0_Q1_CH_SYNC_CTRL_0;
+		sync_ctrl1 = HW_Q0_Q1_CH_SYNC_CTRL_1;
+		break;
+	case 1:
+		sync_ctrl0 = HW_Q2_Q3_CH_SYNC_CTRL_0;
+		sync_ctrl1 = HW_Q2_Q3_CH_SYNC_CTRL_1;
+		break;
+	case 2:
+		sync_ctrl0 = HW_Q4_Q5_CH_SYNC_CTRL_0;
+		sync_ctrl1 = HW_Q4_Q5_CH_SYNC_CTRL_1;
+		break;
+	case 3:
+		sync_ctrl0 = HW_Q6_Q7_CH_SYNC_CTRL_0;
+		sync_ctrl1 = HW_Q6_Q7_CH_SYNC_CTRL_1;
+		break;
+	case 4:
+		sync_ctrl0 = HW_Q8_CH_SYNC_CTRL_0;
+		sync_ctrl1 = HW_Q8_CH_SYNC_CTRL_1;
+		break;
+	case 5:
+		sync_ctrl0 = HW_Q9_CH_SYNC_CTRL_0;
+		sync_ctrl1 = HW_Q9_CH_SYNC_CTRL_1;
+		break;
+	case 6:
+		sync_ctrl0 = HW_Q10_CH_SYNC_CTRL_0;
+		sync_ctrl1 = HW_Q10_CH_SYNC_CTRL_1;
+		break;
+	case 7:
+		sync_ctrl0 = HW_Q11_CH_SYNC_CTRL_0;
+		sync_ctrl1 = HW_Q11_CH_SYNC_CTRL_1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	val = SYNCTRL1_MASTER_SYNC_RST;
+
+	/* Place master sync in reset */
+	err = idtcm_write(idtcm, 0, sync_ctrl1, &val, sizeof(val));
+	if (err)
+		return err;
+
+	err = idtcm_write(idtcm, 0, sync_ctrl0, &sync_src, sizeof(sync_src));
+	if (err)
+		return err;
+
+	/* Set sync trigger mask */
+	val |= SYNCTRL1_FBDIV_FRAME_SYNC_TRIG | SYNCTRL1_FBDIV_SYNC_TRIG;
+
+	if (qn)
+		val |= SYNCTRL1_Q0_DIV_SYNC_TRIG;
+
+	if (qn_plus_1)
+		val |= SYNCTRL1_Q1_DIV_SYNC_TRIG;
+
+	err = idtcm_write(idtcm, 0, sync_ctrl1, &val, sizeof(val));
+	if (err)
+		return err;
+
+	/* PLL5 can have OUT8 as second additional output. */
+	if (pll == 5 && qn_plus_1 != 0) {
+		err = idtcm_read(idtcm, 0, HW_Q8_CTRL_SPARE,
+				 &temp, sizeof(temp));
+		if (err)
+			return err;
+
+		temp &= ~(Q9_TO_Q8_SYNC_TRIG);
+
+		err = idtcm_write(idtcm, 0, HW_Q8_CTRL_SPARE,
+				  &temp, sizeof(temp));
+		if (err)
+			return err;
+
+		temp |= Q9_TO_Q8_SYNC_TRIG;
+
+		err = idtcm_write(idtcm, 0, HW_Q8_CTRL_SPARE,
+				  &temp, sizeof(temp));
+		if (err)
+			return err;
+	}
+
+	/* PLL6 can have OUT11 as second additional output. */
+	if (pll == 6 && qn_plus_1 != 0) {
+		err = idtcm_read(idtcm, 0, HW_Q11_CTRL_SPARE,
+				 &temp, sizeof(temp));
+		if (err)
+			return err;
+
+		temp &= ~(Q10_TO_Q11_SYNC_TRIG);
+
+		err = idtcm_write(idtcm, 0, HW_Q11_CTRL_SPARE,
+				  &temp, sizeof(temp));
+		if (err)
+			return err;
+
+		temp |= Q10_TO_Q11_SYNC_TRIG;
+
+		err = idtcm_write(idtcm, 0, HW_Q11_CTRL_SPARE,
+				  &temp, sizeof(temp));
+		if (err)
+			return err;
+	}
+
+	/* Place master sync out of reset */
+	val &= ~(SYNCTRL1_MASTER_SYNC_RST);
+	err = idtcm_write(idtcm, 0, sync_ctrl1, &val, sizeof(val));
+
+	return err;
+}
+
+static int idtcm_sync_pps_output(struct idtcm_channel *channel)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	u8 pll;
+	u8 qn;
+	u8 qn_plus_1;
+	int err = 0;
+	u8 out8_mux = 0;
+	u8 out11_mux = 0;
+	u8 temp;
+	u16 output_mask = channel->output_mask;
+
+	err = idtcm_read(idtcm, 0, HW_Q8_CTRL_SPARE,
+			 &temp, sizeof(temp));
+	if (err)
+		return err;
+
+	if ((temp & Q9_TO_Q8_FANOUT_AND_CLOCK_SYNC_ENABLE_MASK) ==
+	    Q9_TO_Q8_FANOUT_AND_CLOCK_SYNC_ENABLE_MASK)
+		out8_mux = 1;
+
+	err = idtcm_read(idtcm, 0, HW_Q11_CTRL_SPARE,
+			 &temp, sizeof(temp));
+	if (err)
+		return err;
+
+	if ((temp & Q10_TO_Q11_FANOUT_AND_CLOCK_SYNC_ENABLE_MASK) ==
+	    Q10_TO_Q11_FANOUT_AND_CLOCK_SYNC_ENABLE_MASK)
+		out11_mux = 1;
+
+	for (pll = 0; pll < 8; pll++) {
+		qn = 0;
+		qn_plus_1 = 0;
+
+		if (pll < 4) {
+			/* First 4 pll has 2 outputs */
+			qn = output_mask & 0x1;
+			output_mask = output_mask >> 1;
+			qn_plus_1 = output_mask & 0x1;
+			output_mask = output_mask >> 1;
+		} else if (pll == 4) {
+			if (out8_mux == 0) {
+				qn = output_mask & 0x1;
+				output_mask = output_mask >> 1;
+			}
+		} else if (pll == 5) {
+			if (out8_mux) {
+				qn_plus_1 = output_mask & 0x1;
+				output_mask = output_mask >> 1;
+			}
+			qn = output_mask & 0x1;
+			output_mask = output_mask >> 1;
+		} else if (pll == 6) {
+			qn = output_mask & 0x1;
+			output_mask = output_mask >> 1;
+			if (out11_mux) {
+				qn_plus_1 = output_mask & 0x1;
+				output_mask = output_mask >> 1;
+			}
+		} else if (pll == 7) {
+			if (out11_mux == 0) {
+				qn = output_mask & 0x1;
+				output_mask = output_mask >> 1;
+			}
+		}
+
+		if (qn != 0 || qn_plus_1 != 0)
+			err = _sync_pll_output(idtcm, pll, channel->sync_src,
+					       qn, qn_plus_1);
+
+		if (err)
+			return err;
+	}
+
+	return err;
+}
+
+static int _idtcm_set_dpll_hw_tod(struct idtcm_channel *channel,
+				  struct timespec64 const *ts,
+				  enum hw_tod_write_trig_sel wr_trig)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	u8 buf[TOD_BYTE_COUNT];
+	u8 cmd;
+	int err;
+	struct timespec64 local_ts = *ts;
+	s64 total_overhead_ns;
+
+	/* Configure HW TOD write trigger. */
+	err = idtcm_read(idtcm, channel->hw_dpll_n, HW_DPLL_TOD_CTRL_1,
+			 &cmd, sizeof(cmd));
+	if (err)
+		return err;
+
+	cmd &= ~(0x0f);
+	cmd |= wr_trig | 0x08;
+
+	err = idtcm_write(idtcm, channel->hw_dpll_n, HW_DPLL_TOD_CTRL_1,
+			  &cmd, sizeof(cmd));
+	if (err)
+		return err;
+
+	if (wr_trig  != HW_TOD_WR_TRIG_SEL_MSB) {
+		err = timespec_to_char_array(&local_ts, buf, sizeof(buf));
+		if (err)
+			return err;
+
+		err = idtcm_write(idtcm, channel->hw_dpll_n,
+				  HW_DPLL_TOD_OVR__0, buf, sizeof(buf));
+		if (err)
+			return err;
+	}
+
+	/* ARM HW TOD write trigger. */
+	cmd &= ~(0x08);
+
+	err = idtcm_write(idtcm, channel->hw_dpll_n, HW_DPLL_TOD_CTRL_1,
+			  &cmd, sizeof(cmd));
+
+	if (wr_trig == HW_TOD_WR_TRIG_SEL_MSB) {
+		if (idtcm->calculate_overhead_flag) {
+			/* Assumption: I2C @ 400KHz */
+			ktime_t diff = ktime_sub(ktime_get_raw(),
+						 idtcm->start_time);
+			total_overhead_ns =  ktime_to_ns(diff)
+					     + idtcm->tod_write_overhead_ns
+					     + SETTIME_CORRECTION;
+
+			timespec64_add_ns(&local_ts, total_overhead_ns);
+
+			idtcm->calculate_overhead_flag = 0;
+		}
+
+		err = timespec_to_char_array(&local_ts, buf, sizeof(buf));
+		if (err)
+			return err;
+
+		err = idtcm_write(idtcm, channel->hw_dpll_n,
+				  HW_DPLL_TOD_OVR__0, buf, sizeof(buf));
+	}
+
+	return err;
+}
+
+static int _idtcm_set_dpll_scsr_tod(struct idtcm_channel *channel,
+				    struct timespec64 const *ts,
+				    enum scsr_tod_write_trig_sel wr_trig,
+				    enum scsr_tod_write_type_sel wr_type)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	unsigned char buf[TOD_BYTE_COUNT], cmd;
+	struct timespec64 local_ts = *ts;
+	int err, count = 0;
+
+	timespec64_add_ns(&local_ts, SETTIME_CORRECTION);
+
+	err = timespec_to_char_array(&local_ts, buf, sizeof(buf));
+	if (err)
+		return err;
+
+	err = idtcm_write(idtcm, channel->tod_write, TOD_WRITE,
+			  buf, sizeof(buf));
+	if (err)
+		return err;
+
+	/* Trigger the write operation. */
+	err = idtcm_read(idtcm, channel->tod_write, TOD_WRITE_CMD,
+			 &cmd, sizeof(cmd));
+	if (err)
+		return err;
+
+	cmd &= ~(TOD_WRITE_SELECTION_MASK << TOD_WRITE_SELECTION_SHIFT);
+	cmd &= ~(TOD_WRITE_TYPE_MASK << TOD_WRITE_TYPE_SHIFT);
+	cmd |= (wr_trig << TOD_WRITE_SELECTION_SHIFT);
+	cmd |= (wr_type << TOD_WRITE_TYPE_SHIFT);
+
+	err = idtcm_write(idtcm, channel->tod_write, TOD_WRITE_CMD,
+			   &cmd, sizeof(cmd));
+	if (err)
+		return err;
+
+	/* Wait for the operation to complete. */
+	while (1) {
+		/* pps trigger takes up to 1 sec to complete */
+		if (wr_trig == SCSR_TOD_WR_TRIG_SEL_TODPPS)
+			msleep(50);
+
+		err = idtcm_read(idtcm, channel->tod_write, TOD_WRITE_CMD,
+				 &cmd, sizeof(cmd));
+		if (err)
+			return err;
+
+		if ((cmd & TOD_WRITE_SELECTION_MASK) == 0)
+			break;
+
+		if (++count > 20) {
+			dev_err(idtcm->dev,
+				"Timed out waiting for the write counter");
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+static int get_output_base_addr(enum fw_version ver, u8 outn)
+{
+	int base;
+
+	switch (outn) {
+	case 0:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_0);
+		break;
+	case 1:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_1);
+		break;
+	case 2:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_2);
+		break;
+	case 3:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_3);
+		break;
+	case 4:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_4);
+		break;
+	case 5:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_5);
+		break;
+	case 6:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_6);
+		break;
+	case 7:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_7);
+		break;
+	case 8:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_8);
+		break;
+	case 9:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_9);
+		break;
+	case 10:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_10);
+		break;
+	case 11:
+		base = IDTCM_FW_REG(ver, V520, OUTPUT_11);
+		break;
+	default:
+		base = -EINVAL;
+	}
+
+	return base;
+}
+
+static int _idtcm_settime_deprecated(struct idtcm_channel *channel,
+				     struct timespec64 const *ts)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	err = _idtcm_set_dpll_hw_tod(channel, ts, HW_TOD_WR_TRIG_SEL_MSB);
+	if (err) {
+		dev_err(idtcm->dev,
+			"%s: Set HW ToD failed", __func__);
+		return err;
+	}
+
+	return idtcm_sync_pps_output(channel);
+}
+
+static int _idtcm_settime(struct idtcm_channel *channel,
+			  struct timespec64 const *ts,
+			  enum scsr_tod_write_type_sel wr_type)
+{
+	return _idtcm_set_dpll_scsr_tod(channel, ts,
+					SCSR_TOD_WR_TRIG_SEL_IMMEDIATE,
+					wr_type);
+}
+
+static int idtcm_set_phase_pull_in_offset(struct idtcm_channel *channel,
+					  s32 offset_ns)
+{
+	int err;
+	int i;
+	struct idtcm *idtcm = channel->idtcm;
+	u8 buf[4];
+
+	for (i = 0; i < 4; i++) {
+		buf[i] = 0xff & (offset_ns);
+		offset_ns >>= 8;
+	}
+
+	err = idtcm_write(idtcm, channel->dpll_phase_pull_in, PULL_IN_OFFSET,
+			  buf, sizeof(buf));
+
+	return err;
+}
+
+static int idtcm_set_phase_pull_in_slope_limit(struct idtcm_channel *channel,
+					       u32 max_ffo_ppb)
+{
+	int err;
+	u8 i;
+	struct idtcm *idtcm = channel->idtcm;
+	u8 buf[3];
+
+	if (max_ffo_ppb & 0xff000000)
+		max_ffo_ppb = 0;
+
+	for (i = 0; i < 3; i++) {
+		buf[i] = 0xff & (max_ffo_ppb);
+		max_ffo_ppb >>= 8;
+	}
+
+	err = idtcm_write(idtcm, channel->dpll_phase_pull_in,
+			  PULL_IN_SLOPE_LIMIT, buf, sizeof(buf));
+
+	return err;
+}
+
+static int idtcm_start_phase_pull_in(struct idtcm_channel *channel)
+{
+	int err;
+	struct idtcm *idtcm = channel->idtcm;
+	u8 buf;
+
+	err = idtcm_read(idtcm, channel->dpll_phase_pull_in, PULL_IN_CTRL,
+			 &buf, sizeof(buf));
+	if (err)
+		return err;
+
+	if (buf == 0) {
+		buf = 0x01;
+		err = idtcm_write(idtcm, channel->dpll_phase_pull_in,
+				  PULL_IN_CTRL, &buf, sizeof(buf));
+	} else {
+		err = -EBUSY;
+	}
+
+	return err;
+}
+
+static int do_phase_pull_in_fw(struct idtcm_channel *channel,
+			       s32 offset_ns,
+			       u32 max_ffo_ppb)
+{
+	int err;
+
+	err = idtcm_set_phase_pull_in_offset(channel, -offset_ns);
+	if (err)
+		return err;
+
+	err = idtcm_set_phase_pull_in_slope_limit(channel, max_ffo_ppb);
+	if (err)
+		return err;
+
+	err = idtcm_start_phase_pull_in(channel);
+
+	return err;
+}
+
+static int set_tod_write_overhead(struct idtcm_channel *channel)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	s64 current_ns = 0;
+	s64 lowest_ns = 0;
+	int err;
+	u8 i;
+	ktime_t start;
+	ktime_t stop;
+	ktime_t diff;
+
+	char buf[TOD_BYTE_COUNT] = {0};
+
+	/* Set page offset */
+	idtcm_write(idtcm, channel->hw_dpll_n, HW_DPLL_TOD_OVR__0,
+		    buf, sizeof(buf));
+
+	for (i = 0; i < TOD_WRITE_OVERHEAD_COUNT_MAX; i++) {
+		start = ktime_get_raw();
+
+		err = idtcm_write(idtcm, channel->hw_dpll_n,
+				  HW_DPLL_TOD_OVR__0, buf, sizeof(buf));
+		if (err)
+			return err;
+
+		stop = ktime_get_raw();
+
+		diff = ktime_sub(stop, start);
+
+		current_ns = ktime_to_ns(diff);
+
+		if (i == 0) {
+			lowest_ns = current_ns;
+		} else {
+			if (current_ns < lowest_ns)
+				lowest_ns = current_ns;
+		}
+	}
+
+	idtcm->tod_write_overhead_ns = lowest_ns;
+
+	return err;
+}
+
+static int _idtcm_adjtime_deprecated(struct idtcm_channel *channel, s64 delta)
+{
+	int err;
+	struct idtcm *idtcm = channel->idtcm;
+	struct timespec64 ts;
+	s64 now;
+
+	if (abs(delta) < PHASE_PULL_IN_THRESHOLD_NS_DEPRECATED) {
+		err = channel->do_phase_pull_in(channel, delta, 0);
+	} else {
+		idtcm->calculate_overhead_flag = 1;
+
+		err = set_tod_write_overhead(channel);
+		if (err)
+			return err;
+
+		err = _idtcm_gettime_immediate(channel, &ts);
+		if (err)
+			return err;
+
+		now = timespec64_to_ns(&ts);
+		now += delta;
+
+		ts = ns_to_timespec64(now);
+
+		err = _idtcm_settime_deprecated(channel, &ts);
+	}
+
+	return err;
+}
+
+static int idtcm_state_machine_reset(struct idtcm *idtcm)
+{
+	u8 byte = SM_RESET_CMD;
+	u32 status = 0;
+	int err;
+	u8 i;
+
+	clear_boot_status(idtcm);
+
+	err = idtcm_write(idtcm, RESET_CTRL,
+			  IDTCM_FW_REG(idtcm->fw_ver, V520, SM_RESET),
+			  &byte, sizeof(byte));
+
+	if (!err) {
+		for (i = 0; i < 30; i++) {
+			msleep_interruptible(100);
+			read_boot_status(idtcm, &status);
+
+			if (status == 0xA0) {
+				dev_dbg(idtcm->dev,
+					"SM_RESET completed in %d ms", i * 100);
+				break;
+			}
+		}
+
+		if (!status)
+			dev_err(idtcm->dev,
+				"Timed out waiting for CM_RESET to complete");
+	}
+
+	return err;
+}
+
+static int idtcm_read_hw_rev_id(struct idtcm *idtcm, u8 *hw_rev_id)
+{
+	return idtcm_read(idtcm, HW_REVISION, REV_ID, hw_rev_id, sizeof(u8));
+}
+
+static int idtcm_read_product_id(struct idtcm *idtcm, u16 *product_id)
+{
+	int err;
+	u8 buf[2] = {0};
+
+	err = idtcm_read(idtcm, GENERAL_STATUS, PRODUCT_ID, buf, sizeof(buf));
+
+	*product_id = (buf[1] << 8) | buf[0];
+
+	return err;
+}
+
+static int idtcm_read_major_release(struct idtcm *idtcm, u8 *major)
+{
+	int err;
+	u8 buf = 0;
+
+	err = idtcm_read(idtcm, GENERAL_STATUS, MAJ_REL, &buf, sizeof(buf));
+
+	*major = buf >> 1;
+
+	return err;
+}
+
+static int idtcm_read_minor_release(struct idtcm *idtcm, u8 *minor)
+{
+	return idtcm_read(idtcm, GENERAL_STATUS, MIN_REL, minor, sizeof(u8));
+}
+
+static int idtcm_read_hotfix_release(struct idtcm *idtcm, u8 *hotfix)
+{
+	return idtcm_read(idtcm,
+			  GENERAL_STATUS,
+			  HOTFIX_REL,
+			  hotfix,
+			  sizeof(u8));
+}
+
+static int idtcm_read_otp_scsr_config_select(struct idtcm *idtcm,
+					     u8 *config_select)
+{
+	return idtcm_read(idtcm, GENERAL_STATUS, OTP_SCSR_CONFIG_SELECT,
+			  config_select, sizeof(u8));
+}
+
+static int set_pll_output_mask(struct idtcm *idtcm, u16 addr, u8 val)
+{
+	int err = 0;
+
+	switch (addr) {
+	case TOD0_OUT_ALIGN_MASK_ADDR:
+		SET_U16_LSB(idtcm->channel[0].output_mask, val);
+		break;
+	case TOD0_OUT_ALIGN_MASK_ADDR + 1:
+		SET_U16_MSB(idtcm->channel[0].output_mask, val);
+		break;
+	case TOD1_OUT_ALIGN_MASK_ADDR:
+		SET_U16_LSB(idtcm->channel[1].output_mask, val);
+		break;
+	case TOD1_OUT_ALIGN_MASK_ADDR + 1:
+		SET_U16_MSB(idtcm->channel[1].output_mask, val);
+		break;
+	case TOD2_OUT_ALIGN_MASK_ADDR:
+		SET_U16_LSB(idtcm->channel[2].output_mask, val);
+		break;
+	case TOD2_OUT_ALIGN_MASK_ADDR + 1:
+		SET_U16_MSB(idtcm->channel[2].output_mask, val);
+		break;
+	case TOD3_OUT_ALIGN_MASK_ADDR:
+		SET_U16_LSB(idtcm->channel[3].output_mask, val);
+		break;
+	case TOD3_OUT_ALIGN_MASK_ADDR + 1:
+		SET_U16_MSB(idtcm->channel[3].output_mask, val);
+		break;
+	default:
+		err = -EFAULT; /* Bad address */;
+		break;
+	}
+
+	return err;
+}
+
+static int set_tod_ptp_pll(struct idtcm *idtcm, u8 index, u8 pll)
+{
+	if (index >= MAX_TOD) {
+		dev_err(idtcm->dev, "ToD%d not supported", index);
+		return -EINVAL;
+	}
+
+	if (pll >= MAX_PLL) {
+		dev_err(idtcm->dev, "Pll%d not supported", pll);
+		return -EINVAL;
+	}
+
+	idtcm->channel[index].pll = pll;
+
+	return 0;
+}
+
+static int check_and_set_masks(struct idtcm *idtcm,
+			       u16 regaddr,
+			       u8 val)
+{
+	int err = 0;
+
+	switch (regaddr) {
+	case TOD_MASK_ADDR:
+		if ((val & 0xf0) || !(val & 0x0f)) {
+			dev_err(idtcm->dev, "Invalid TOD mask 0x%02x", val);
+			err = -EINVAL;
+		} else {
+			idtcm->tod_mask = val;
+		}
+		break;
+	case TOD0_PTP_PLL_ADDR:
+		err = set_tod_ptp_pll(idtcm, 0, val);
+		break;
+	case TOD1_PTP_PLL_ADDR:
+		err = set_tod_ptp_pll(idtcm, 1, val);
+		break;
+	case TOD2_PTP_PLL_ADDR:
+		err = set_tod_ptp_pll(idtcm, 2, val);
+		break;
+	case TOD3_PTP_PLL_ADDR:
+		err = set_tod_ptp_pll(idtcm, 3, val);
+		break;
+	default:
+		err = set_pll_output_mask(idtcm, regaddr, val);
+		break;
+	}
+
+	return err;
+}
+
+static void display_pll_and_masks(struct idtcm *idtcm)
+{
+	u8 i;
+	u8 mask;
+
+	dev_dbg(idtcm->dev, "tod_mask = 0x%02x", idtcm->tod_mask);
+
+	for (i = 0; i < MAX_TOD; i++) {
+		mask = 1 << i;
+
+		if (mask & idtcm->tod_mask)
+			dev_dbg(idtcm->dev,
+				"TOD%d pll = %d    output_mask = 0x%04x",
+				i, idtcm->channel[i].pll,
+				idtcm->channel[i].output_mask);
+	}
+}
+
+static int idtcm_load_firmware(struct idtcm *idtcm,
+			       struct device *dev)
+{
+	u16 scratch = IDTCM_FW_REG(idtcm->fw_ver, V520, SCRATCH);
+	char fname[128] = FW_FILENAME;
+	const struct firmware *fw;
+	struct idtcm_fwrc *rec;
+	u32 regaddr;
+	int err;
+	s32 len;
+	u8 val;
+	u8 loaddr;
+
+	if (firmware) /* module parameter */
+		snprintf(fname, sizeof(fname), "%s", firmware);
+
+	dev_info(idtcm->dev, "requesting firmware '%s'", fname);
+
+	err = request_firmware(&fw, fname, dev);
+	if (err) {
+		dev_err(idtcm->dev,
+			"Failed at line %d in %s!", __LINE__, __func__);
+		return err;
+	}
+
+	dev_dbg(idtcm->dev, "firmware size %zu bytes", fw->size);
+
+	rec = (struct idtcm_fwrc *) fw->data;
+
+	if (contains_full_configuration(idtcm, fw))
+		idtcm_state_machine_reset(idtcm);
+
+	for (len = fw->size; len > 0; len -= sizeof(*rec)) {
+		if (rec->reserved) {
+			dev_err(idtcm->dev,
+				"bad firmware, reserved field non-zero");
+			err = -EINVAL;
+		} else {
+			regaddr = rec->hiaddr << 8;
+			regaddr |= rec->loaddr;
+
+			val = rec->value;
+			loaddr = rec->loaddr;
+
+			rec++;
+
+			err = check_and_set_masks(idtcm, regaddr, val);
+		}
+
+		if (err != -EINVAL) {
+			err = 0;
+
+			/* Top (status registers) and bottom are read-only */
+			if (regaddr < GPIO_USER_CONTROL || regaddr >= scratch)
+				continue;
+
+			/* Page size 128, last 4 bytes of page skipped */
+			if ((loaddr > 0x7b && loaddr <= 0x7f) || loaddr > 0xfb)
+				continue;
+
+			err = idtcm_write(idtcm, regaddr, 0, &val, sizeof(val));
+		}
+
+		if (err)
+			goto out;
+	}
+
+	display_pll_and_masks(idtcm);
+
+out:
+	release_firmware(fw);
+	return err;
+}
+
+static int idtcm_output_enable(struct idtcm_channel *channel,
+			       bool enable, unsigned int outn)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	int base;
+	int err;
+	u8 val;
+
+	base = get_output_base_addr(idtcm->fw_ver, outn);
+
+	if (!(base > 0)) {
+		dev_err(idtcm->dev,
+			"%s - Unsupported out%d", __func__, outn);
+		return base;
+	}
+
+	err = idtcm_read(idtcm, (u16)base, OUT_CTRL_1, &val, sizeof(val));
+	if (err)
+		return err;
+
+	if (enable)
+		val |= SQUELCH_DISABLE;
+	else
+		val &= ~SQUELCH_DISABLE;
+
+	return idtcm_write(idtcm, (u16)base, OUT_CTRL_1, &val, sizeof(val));
+}
+
+static int idtcm_perout_enable(struct idtcm_channel *channel,
+			       struct ptp_perout_request *perout,
+			       bool enable)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	struct timespec64 ts = {0, 0};
+	int err;
+
+	err = idtcm_output_enable(channel, enable, perout->index);
+
+	if (err) {
+		dev_err(idtcm->dev, "Unable to set output enable");
+		return err;
+	}
+
+	/* Align output to internal 1 PPS */
+	return _idtcm_settime(channel, &ts, SCSR_TOD_WR_TYPE_SEL_DELTA_PLUS);
+}
+
+static int idtcm_get_pll_mode(struct idtcm_channel *channel,
+			      enum pll_mode *mode)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+	u8 dpll_mode;
+
+	err = idtcm_read(idtcm, channel->dpll_n,
+			 IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_MODE),
+			 &dpll_mode, sizeof(dpll_mode));
+	if (err)
+		return err;
+
+	*mode = (dpll_mode >> PLL_MODE_SHIFT) & PLL_MODE_MASK;
+
+	return 0;
+}
+
+static int idtcm_set_pll_mode(struct idtcm_channel *channel,
+			      enum pll_mode mode)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+	u8 dpll_mode;
+
+	err = idtcm_read(idtcm, channel->dpll_n,
+			 IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_MODE),
+			 &dpll_mode, sizeof(dpll_mode));
+	if (err)
+		return err;
+
+	dpll_mode &= ~(PLL_MODE_MASK << PLL_MODE_SHIFT);
+
+	dpll_mode |= (mode << PLL_MODE_SHIFT);
+
+	err = idtcm_write(idtcm, channel->dpll_n,
+			  IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_MODE),
+			  &dpll_mode, sizeof(dpll_mode));
+	return err;
+}
+
+static int idtcm_get_manual_reference(struct idtcm_channel *channel,
+				      enum manual_reference *ref)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	u8 dpll_manu_ref_cfg;
+	int err;
+
+	err = idtcm_read(idtcm, channel->dpll_ctrl_n,
+			 DPLL_CTRL_DPLL_MANU_REF_CFG,
+			 &dpll_manu_ref_cfg, sizeof(dpll_manu_ref_cfg));
+	if (err)
+		return err;
+
+	dpll_manu_ref_cfg &= (MANUAL_REFERENCE_MASK << MANUAL_REFERENCE_SHIFT);
+
+	*ref = dpll_manu_ref_cfg >> MANUAL_REFERENCE_SHIFT;
+
+	return 0;
+}
+
+static int idtcm_set_manual_reference(struct idtcm_channel *channel,
+				      enum manual_reference ref)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	u8 dpll_manu_ref_cfg;
+	int err;
+
+	err = idtcm_read(idtcm, channel->dpll_ctrl_n,
+			 DPLL_CTRL_DPLL_MANU_REF_CFG,
+			 &dpll_manu_ref_cfg, sizeof(dpll_manu_ref_cfg));
+	if (err)
+		return err;
+
+	dpll_manu_ref_cfg &= ~(MANUAL_REFERENCE_MASK << MANUAL_REFERENCE_SHIFT);
+
+	dpll_manu_ref_cfg |= (ref << MANUAL_REFERENCE_SHIFT);
+
+	err = idtcm_write(idtcm, channel->dpll_ctrl_n,
+			  DPLL_CTRL_DPLL_MANU_REF_CFG,
+			  &dpll_manu_ref_cfg, sizeof(dpll_manu_ref_cfg));
+
+	return err;
+}
+
+static int configure_dpll_mode_write_frequency(struct idtcm_channel *channel)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	err = idtcm_set_pll_mode(channel, PLL_MODE_WRITE_FREQUENCY);
+
+	if (err)
+		dev_err(idtcm->dev, "Failed to set pll mode to write frequency");
+	else
+		channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
+
+	return err;
+}
+
+static int configure_dpll_mode_write_phase(struct idtcm_channel *channel)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	err = idtcm_set_pll_mode(channel, PLL_MODE_WRITE_PHASE);
+
+	if (err)
+		dev_err(idtcm->dev, "Failed to set pll mode to write phase");
+	else
+		channel->mode = PTP_PLL_MODE_WRITE_PHASE;
+
+	return err;
+}
+
+static int configure_manual_reference_write_frequency(struct idtcm_channel *channel)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	err = idtcm_set_manual_reference(channel, MANU_REF_WRITE_FREQUENCY);
+
+	if (err)
+		dev_err(idtcm->dev, "Failed to set manual reference to write frequency");
+	else
+		channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
+
+	return err;
+}
+
+static int configure_manual_reference_write_phase(struct idtcm_channel *channel)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	err = idtcm_set_manual_reference(channel, MANU_REF_WRITE_PHASE);
+
+	if (err)
+		dev_err(idtcm->dev, "Failed to set manual reference to write phase");
+	else
+		channel->mode = PTP_PLL_MODE_WRITE_PHASE;
+
+	return err;
+}
+
+static int idtcm_stop_phase_pull_in(struct idtcm_channel *channel)
+{
+	int err;
+
+	err = _idtcm_adjfine(channel, channel->current_freq_scaled_ppm);
+	if (err)
+		return err;
+
+	channel->phase_pull_in = false;
+
+	return 0;
+}
+
+static long idtcm_work_handler(struct ptp_clock_info *ptp)
+{
+	struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
+	struct idtcm *idtcm = channel->idtcm;
+
+	mutex_lock(idtcm->lock);
+
+	(void)idtcm_stop_phase_pull_in(channel);
+
+	mutex_unlock(idtcm->lock);
+
+	/* Return a negative value here to not reschedule */
+	return -1;
+}
+
+static s32 phase_pull_in_scaled_ppm(s32 current_ppm, s32 phase_pull_in_ppb)
+{
+	/* ppb = scaled_ppm * 125 / 2^13 */
+	/* scaled_ppm = ppb * 2^13 / 125 */
+
+	s64 max_scaled_ppm = div_s64((s64)PHASE_PULL_IN_MAX_PPB << 13, 125);
+	s64 scaled_ppm = div_s64((s64)phase_pull_in_ppb << 13, 125);
+
+	current_ppm += scaled_ppm;
+
+	if (current_ppm > max_scaled_ppm)
+		current_ppm = max_scaled_ppm;
+	else if (current_ppm < -max_scaled_ppm)
+		current_ppm = -max_scaled_ppm;
+
+	return current_ppm;
+}
+
+static int do_phase_pull_in_sw(struct idtcm_channel *channel,
+			       s32 delta_ns,
+			       u32 max_ffo_ppb)
+{
+	s32 current_ppm = channel->current_freq_scaled_ppm;
+	u32 duration_ms = MSEC_PER_SEC;
+	s32 delta_ppm;
+	s32 ppb;
+	int err;
+
+	/* If the ToD correction is less than PHASE_PULL_IN_MIN_THRESHOLD_NS,
+	 * skip. The error introduced by the ToD adjustment procedure would
+	 * be bigger than the required ToD correction
+	 */
+	if (abs(delta_ns) < PHASE_PULL_IN_MIN_THRESHOLD_NS)
+		return 0;
+
+	if (max_ffo_ppb == 0)
+		max_ffo_ppb = PHASE_PULL_IN_MAX_PPB;
+
+	/* For most cases, keep phase pull-in duration 1 second */
+	ppb = delta_ns;
+	while (abs(ppb) > max_ffo_ppb) {
+		duration_ms *= 2;
+		ppb /= 2;
+	}
+
+	delta_ppm = phase_pull_in_scaled_ppm(current_ppm, ppb);
+
+	err = _idtcm_adjfine(channel, delta_ppm);
+
+	if (err)
+		return err;
+
+	/* schedule the worker to cancel phase pull-in */
+	ptp_schedule_worker(channel->ptp_clock,
+			    msecs_to_jiffies(duration_ms) - 1);
+
+	channel->phase_pull_in = true;
+
+	return 0;
+}
+
+static int initialize_operating_mode_with_manual_reference(struct idtcm_channel *channel,
+							   enum manual_reference ref)
+{
+	struct idtcm *idtcm = channel->idtcm;
+
+	channel->mode = PTP_PLL_MODE_UNSUPPORTED;
+	channel->configure_write_frequency = configure_manual_reference_write_frequency;
+	channel->configure_write_phase = configure_manual_reference_write_phase;
+	channel->do_phase_pull_in = do_phase_pull_in_sw;
+
+	switch (ref) {
+	case MANU_REF_WRITE_PHASE:
+		channel->mode = PTP_PLL_MODE_WRITE_PHASE;
+		break;
+	case MANU_REF_WRITE_FREQUENCY:
+		channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
+		break;
+	default:
+		dev_warn(idtcm->dev,
+			 "Unsupported MANUAL_REFERENCE: 0x%02x", ref);
+	}
+
+	return 0;
+}
+
+static int initialize_operating_mode_with_pll_mode(struct idtcm_channel *channel,
+						   enum pll_mode mode)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	int err = 0;
+
+	channel->mode = PTP_PLL_MODE_UNSUPPORTED;
+	channel->configure_write_frequency = configure_dpll_mode_write_frequency;
+	channel->configure_write_phase = configure_dpll_mode_write_phase;
+	channel->do_phase_pull_in = do_phase_pull_in_fw;
+
+	switch (mode) {
+	case  PLL_MODE_WRITE_PHASE:
+		channel->mode = PTP_PLL_MODE_WRITE_PHASE;
+		break;
+	case PLL_MODE_WRITE_FREQUENCY:
+		channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
+		break;
+	default:
+		dev_err(idtcm->dev,
+			"Unsupported PLL_MODE: 0x%02x", mode);
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
+static int initialize_dco_operating_mode(struct idtcm_channel *channel)
+{
+	enum manual_reference ref = MANU_REF_XO_DPLL;
+	enum pll_mode mode = PLL_MODE_DISABLED;
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	channel->mode = PTP_PLL_MODE_UNSUPPORTED;
+
+	err = idtcm_get_pll_mode(channel, &mode);
+	if (err) {
+		dev_err(idtcm->dev, "Unable to read pll mode!");
+		return err;
+	}
+
+	if (mode == PLL_MODE_PLL) {
+		err = idtcm_get_manual_reference(channel, &ref);
+		if (err) {
+			dev_err(idtcm->dev, "Unable to read manual reference!");
+			return err;
+		}
+		err = initialize_operating_mode_with_manual_reference(channel, ref);
+	} else {
+		err = initialize_operating_mode_with_pll_mode(channel, mode);
+	}
+
+	if (channel->mode == PTP_PLL_MODE_WRITE_PHASE)
+		channel->configure_write_frequency(channel);
+
+	return err;
+}
+
+/* PTP Hardware Clock interface */
+
+/*
+ * Maximum absolute value for write phase offset in picoseconds
+ *
+ * @channel:  channel
+ * @delta_ns: delta in nanoseconds
+ *
+ * Destination signed register is 32-bit register in resolution of 50ps
+ *
+ * 0x7fffffff * 50 =  2147483647 * 50 = 107374182350
+ */
+static int _idtcm_adjphase(struct idtcm_channel *channel, s32 delta_ns)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+	u8 i;
+	u8 buf[4] = {0};
+	s32 phase_50ps;
+	s64 offset_ps;
+
+	if (channel->mode != PTP_PLL_MODE_WRITE_PHASE) {
+		err = channel->configure_write_phase(channel);
+		if (err)
+			return err;
+	}
+
+	offset_ps = (s64)delta_ns * 1000;
+
+	/*
+	 * Check for 32-bit signed max * 50:
+	 *
+	 * 0x7fffffff * 50 =  2147483647 * 50 = 107374182350
+	 */
+	if (offset_ps > MAX_ABS_WRITE_PHASE_PICOSECONDS)
+		offset_ps = MAX_ABS_WRITE_PHASE_PICOSECONDS;
+	else if (offset_ps < -MAX_ABS_WRITE_PHASE_PICOSECONDS)
+		offset_ps = -MAX_ABS_WRITE_PHASE_PICOSECONDS;
+
+	phase_50ps = div_s64(offset_ps, 50);
+
+	for (i = 0; i < 4; i++) {
+		buf[i] = phase_50ps & 0xff;
+		phase_50ps >>= 8;
+	}
+
+	err = idtcm_write(idtcm, channel->dpll_phase, DPLL_WR_PHASE,
+			  buf, sizeof(buf));
+
+	return err;
+}
+
+static int _idtcm_adjfine(struct idtcm_channel *channel, long scaled_ppm)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	u8 i;
+	int err;
+	u8 buf[6] = {0};
+	s64 fcw;
+
+	if (channel->mode  != PTP_PLL_MODE_WRITE_FREQUENCY) {
+		err = channel->configure_write_frequency(channel);
+		if (err)
+			return err;
+	}
+
+	/*
+	 * Frequency Control Word unit is: 1.11 * 10^-10 ppm
+	 *
+	 * adjfreq:
+	 *       ppb * 10^9
+	 * FCW = ----------
+	 *          111
+	 *
+	 * adjfine:
+	 *       ppm_16 * 5^12
+	 * FCW = -------------
+	 *         111 * 2^4
+	 */
+
+	/* 2 ^ -53 = 1.1102230246251565404236316680908e-16 */
+	fcw = scaled_ppm * 244140625ULL;
+
+	fcw = div_s64(fcw, 1776);
+
+	for (i = 0; i < 6; i++) {
+		buf[i] = fcw & 0xff;
+		fcw >>= 8;
+	}
+
+	err = idtcm_write(idtcm, channel->dpll_freq, DPLL_WR_FREQ,
+			  buf, sizeof(buf));
+
+	return err;
+}
+
+static int idtcm_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	mutex_lock(idtcm->lock);
+	err = _idtcm_gettime_immediate(channel, ts);
+	mutex_unlock(idtcm->lock);
+
+	if (err)
+		dev_err(idtcm->dev, "Failed at line %d in %s!",
+			__LINE__, __func__);
+
+	return err;
+}
+
+static int idtcm_settime_deprecated(struct ptp_clock_info *ptp,
+				    const struct timespec64 *ts)
+{
+	struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	mutex_lock(idtcm->lock);
+	err = _idtcm_settime_deprecated(channel, ts);
+	mutex_unlock(idtcm->lock);
+
+	if (err)
+		dev_err(idtcm->dev,
+			"Failed at line %d in %s!", __LINE__, __func__);
+
+	return err;
+}
+
+static int idtcm_settime(struct ptp_clock_info *ptp,
+			 const struct timespec64 *ts)
+{
+	struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	mutex_lock(idtcm->lock);
+	err = _idtcm_settime(channel, ts, SCSR_TOD_WR_TYPE_SEL_ABSOLUTE);
+	mutex_unlock(idtcm->lock);
+
+	if (err)
+		dev_err(idtcm->dev,
+			"Failed at line %d in %s!", __LINE__, __func__);
+
+	return err;
+}
+
+static int idtcm_adjtime_deprecated(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	mutex_lock(idtcm->lock);
+	err = _idtcm_adjtime_deprecated(channel, delta);
+	mutex_unlock(idtcm->lock);
+
+	if (err)
+		dev_err(idtcm->dev,
+			"Failed at line %d in %s!", __LINE__, __func__);
+
+	return err;
+}
+
+static int idtcm_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
+	struct idtcm *idtcm = channel->idtcm;
+	struct timespec64 ts;
+	enum scsr_tod_write_type_sel type;
+	int err;
+
+	if (channel->phase_pull_in == true)
+		return -EBUSY;
+
+	mutex_lock(idtcm->lock);
+
+	if (abs(delta) < PHASE_PULL_IN_THRESHOLD_NS) {
+		err = channel->do_phase_pull_in(channel, delta, 0);
+	} else {
+		if (delta >= 0) {
+			ts = ns_to_timespec64(delta);
+			type = SCSR_TOD_WR_TYPE_SEL_DELTA_PLUS;
+		} else {
+			ts = ns_to_timespec64(-delta);
+			type = SCSR_TOD_WR_TYPE_SEL_DELTA_MINUS;
+		}
+		err = _idtcm_settime(channel, &ts, type);
+	}
+
+	mutex_unlock(idtcm->lock);
+
+	if (err)
+		dev_err(idtcm->dev,
+			"Failed at line %d in %s!", __LINE__, __func__);
+
+	return err;
+}
+
+static int idtcm_adjphase(struct ptp_clock_info *ptp, s32 delta)
+{
+	struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	mutex_lock(idtcm->lock);
+	err = _idtcm_adjphase(channel, delta);
+	mutex_unlock(idtcm->lock);
+
+	if (err)
+		dev_err(idtcm->dev,
+			"Failed at line %d in %s!", __LINE__, __func__);
+
+	return err;
+}
+
+static int idtcm_adjfine(struct ptp_clock_info *ptp,  long scaled_ppm)
+{
+	struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
+	struct idtcm *idtcm = channel->idtcm;
+	int err;
+
+	if (channel->phase_pull_in == true)
+		return 0;
+
+	if (scaled_ppm == channel->current_freq_scaled_ppm)
+		return 0;
+
+	mutex_lock(idtcm->lock);
+	err = _idtcm_adjfine(channel, scaled_ppm);
+	mutex_unlock(idtcm->lock);
+
+	if (err)
+		dev_err(idtcm->dev,
+			"Failed at line %d in %s!", __LINE__, __func__);
+	else
+		channel->current_freq_scaled_ppm = scaled_ppm;
+
+	return err;
+}
+
+static int idtcm_enable(struct ptp_clock_info *ptp,
+			struct ptp_clock_request *rq, int on)
+{
+	struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
+	struct idtcm *idtcm = channel->idtcm;
+	int err = -EOPNOTSUPP;
+
+	mutex_lock(idtcm->lock);
+
+	switch (rq->type) {
+	case PTP_CLK_REQ_PEROUT:
+		if (!on)
+			err = idtcm_perout_enable(channel, &rq->perout, false);
+		/* Only accept a 1-PPS aligned to the second. */
+		else if (rq->perout.start.nsec || rq->perout.period.sec != 1 ||
+			 rq->perout.period.nsec)
+			err = -ERANGE;
+		else
+			err = idtcm_perout_enable(channel, &rq->perout, true);
+		break;
+	case PTP_CLK_REQ_EXTTS:
+		err = idtcm_extts_enable(channel, rq, on);
+		break;
+	default:
+		break;
+	}
+
+	mutex_unlock(idtcm->lock);
+
+	if (err)
+		dev_err(channel->idtcm->dev,
+			"Failed in %s with err %d!", __func__, err);
+
+	return err;
+}
+
+static int idtcm_enable_tod(struct idtcm_channel *channel)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	struct timespec64 ts = {0, 0};
+	u16 tod_cfg = IDTCM_FW_REG(idtcm->fw_ver, V520, TOD_CFG);
+	u8 cfg;
+	int err;
+
+	/*
+	 * Start the TOD clock ticking.
+	 */
+	err = idtcm_read(idtcm, channel->tod_n, tod_cfg, &cfg, sizeof(cfg));
+	if (err)
+		return err;
+
+	cfg |= TOD_ENABLE;
+
+	err = idtcm_write(idtcm, channel->tod_n, tod_cfg, &cfg, sizeof(cfg));
+	if (err)
+		return err;
+
+	if (idtcm->fw_ver < V487)
+		return _idtcm_settime_deprecated(channel, &ts);
+	else
+		return _idtcm_settime(channel, &ts,
+				      SCSR_TOD_WR_TYPE_SEL_ABSOLUTE);
+}
+
+static void idtcm_set_version_info(struct idtcm *idtcm)
+{
+	u8 major;
+	u8 minor;
+	u8 hotfix;
+	u16 product_id;
+	u8 hw_rev_id;
+	u8 config_select;
+
+	idtcm_read_major_release(idtcm, &major);
+	idtcm_read_minor_release(idtcm, &minor);
+	idtcm_read_hotfix_release(idtcm, &hotfix);
+
+	idtcm_read_product_id(idtcm, &product_id);
+	idtcm_read_hw_rev_id(idtcm, &hw_rev_id);
+
+	idtcm_read_otp_scsr_config_select(idtcm, &config_select);
+
+	snprintf(idtcm->version, sizeof(idtcm->version), "%u.%u.%u",
+		 major, minor, hotfix);
+
+	idtcm->fw_ver = idtcm_fw_version(idtcm->version);
+
+	dev_info(idtcm->dev,
+		 "%d.%d.%d, Id: 0x%04x  HW Rev: %d  OTP Config Select: %d",
+		 major, minor, hotfix,
+		 product_id, hw_rev_id, config_select);
+}
+
+static int idtcm_verify_pin(struct ptp_clock_info *ptp, unsigned int pin,
+			    enum ptp_pin_function func, unsigned int chan)
+{
+	switch (func) {
+	case PTP_PF_NONE:
+	case PTP_PF_EXTTS:
+		break;
+	case PTP_PF_PEROUT:
+	case PTP_PF_PHYSYNC:
+		return -1;
+	}
+	return 0;
+}
+
+static struct ptp_pin_desc pin_config[MAX_TOD][MAX_REF_CLK];
+
+static const struct ptp_clock_info idtcm_caps = {
+	.owner		= THIS_MODULE,
+	.max_adj	= 244000,
+	.n_per_out	= 12,
+	.n_ext_ts	= MAX_TOD,
+	.n_pins		= MAX_REF_CLK,
+	.adjphase	= &idtcm_adjphase,
+	.adjfine	= &idtcm_adjfine,
+	.adjtime	= &idtcm_adjtime,
+	.gettime64	= &idtcm_gettime,
+	.settime64	= &idtcm_settime,
+	.enable		= &idtcm_enable,
+	.verify		= &idtcm_verify_pin,
+	.do_aux_work	= &idtcm_work_handler,
+};
+
+static const struct ptp_clock_info idtcm_caps_deprecated = {
+	.owner		= THIS_MODULE,
+	.max_adj	= 244000,
+	.n_per_out	= 12,
+	.n_ext_ts	= MAX_TOD,
+	.n_pins		= MAX_REF_CLK,
+	.adjphase	= &idtcm_adjphase,
+	.adjfine	= &idtcm_adjfine,
+	.adjtime	= &idtcm_adjtime_deprecated,
+	.gettime64	= &idtcm_gettime,
+	.settime64	= &idtcm_settime_deprecated,
+	.enable		= &idtcm_enable,
+	.verify		= &idtcm_verify_pin,
+	.do_aux_work	= &idtcm_work_handler,
+};
+
+static int configure_channel_pll(struct idtcm_channel *channel)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	int err = 0;
+
+	switch (channel->pll) {
+	case 0:
+		channel->dpll_freq = DPLL_FREQ_0;
+		channel->dpll_n = DPLL_0;
+		channel->hw_dpll_n = HW_DPLL_0;
+		channel->dpll_phase = DPLL_PHASE_0;
+		channel->dpll_ctrl_n = DPLL_CTRL_0;
+		channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_0;
+		break;
+	case 1:
+		channel->dpll_freq = DPLL_FREQ_1;
+		channel->dpll_n = DPLL_1;
+		channel->hw_dpll_n = HW_DPLL_1;
+		channel->dpll_phase = DPLL_PHASE_1;
+		channel->dpll_ctrl_n = DPLL_CTRL_1;
+		channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_1;
+		break;
+	case 2:
+		channel->dpll_freq = DPLL_FREQ_2;
+		channel->dpll_n = IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_2);
+		channel->hw_dpll_n = HW_DPLL_2;
+		channel->dpll_phase = DPLL_PHASE_2;
+		channel->dpll_ctrl_n = DPLL_CTRL_2;
+		channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_2;
+		break;
+	case 3:
+		channel->dpll_freq = DPLL_FREQ_3;
+		channel->dpll_n = DPLL_3;
+		channel->hw_dpll_n = HW_DPLL_3;
+		channel->dpll_phase = DPLL_PHASE_3;
+		channel->dpll_ctrl_n = DPLL_CTRL_3;
+		channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_3;
+		break;
+	case 4:
+		channel->dpll_freq = DPLL_FREQ_4;
+		channel->dpll_n = IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_4);
+		channel->hw_dpll_n = HW_DPLL_4;
+		channel->dpll_phase = DPLL_PHASE_4;
+		channel->dpll_ctrl_n = DPLL_CTRL_4;
+		channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_4;
+		break;
+	case 5:
+		channel->dpll_freq = DPLL_FREQ_5;
+		channel->dpll_n = DPLL_5;
+		channel->hw_dpll_n = HW_DPLL_5;
+		channel->dpll_phase = DPLL_PHASE_5;
+		channel->dpll_ctrl_n = DPLL_CTRL_5;
+		channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_5;
+		break;
+	case 6:
+		channel->dpll_freq = DPLL_FREQ_6;
+		channel->dpll_n = IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_6);
+		channel->hw_dpll_n = HW_DPLL_6;
+		channel->dpll_phase = DPLL_PHASE_6;
+		channel->dpll_ctrl_n = DPLL_CTRL_6;
+		channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_6;
+		break;
+	case 7:
+		channel->dpll_freq = DPLL_FREQ_7;
+		channel->dpll_n = DPLL_7;
+		channel->hw_dpll_n = HW_DPLL_7;
+		channel->dpll_phase = DPLL_PHASE_7;
+		channel->dpll_ctrl_n = DPLL_CTRL_7;
+		channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_7;
+		break;
+	default:
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
+/*
+ * Compensate for the PTP DCO input-to-output delay.
+ * This delay is 18 FOD cycles.
+ */
+static u32 idtcm_get_dco_delay(struct idtcm_channel *channel)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	u8 mbuf[8] = {0};
+	u8 nbuf[2] = {0};
+	u32 fodFreq;
+	int err;
+	u64 m;
+	u16 n;
+
+	err = idtcm_read(idtcm, channel->dpll_ctrl_n,
+			 DPLL_CTRL_DPLL_FOD_FREQ, mbuf, 6);
+	if (err)
+		return 0;
+
+	err = idtcm_read(idtcm, channel->dpll_ctrl_n,
+			 DPLL_CTRL_DPLL_FOD_FREQ + 6, nbuf, 2);
+	if (err)
+		return 0;
+
+	m = get_unaligned_le64(mbuf);
+	n = get_unaligned_le16(nbuf);
+
+	if (n == 0)
+		n = 1;
+
+	fodFreq = (u32)div_u64(m, n);
+
+	if (fodFreq >= 500000000)
+		return (u32)div_u64(18 * (u64)NSEC_PER_SEC, fodFreq);
+
+	return 0;
+}
+
+static int configure_channel_tod(struct idtcm_channel *channel, u32 index)
+{
+	enum fw_version fw_ver = channel->idtcm->fw_ver;
+
+	/* Set tod addresses */
+	switch (index) {
+	case 0:
+		channel->tod_read_primary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_PRIMARY_0);
+		channel->tod_read_secondary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_SECONDARY_0);
+		channel->tod_write = IDTCM_FW_REG(fw_ver, V520, TOD_WRITE_0);
+		channel->tod_n = IDTCM_FW_REG(fw_ver, V520, TOD_0);
+		channel->sync_src = SYNC_SOURCE_DPLL0_TOD_PPS;
+		break;
+	case 1:
+		channel->tod_read_primary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_PRIMARY_1);
+		channel->tod_read_secondary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_SECONDARY_1);
+		channel->tod_write = IDTCM_FW_REG(fw_ver, V520, TOD_WRITE_1);
+		channel->tod_n = IDTCM_FW_REG(fw_ver, V520, TOD_1);
+		channel->sync_src = SYNC_SOURCE_DPLL1_TOD_PPS;
+		break;
+	case 2:
+		channel->tod_read_primary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_PRIMARY_2);
+		channel->tod_read_secondary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_SECONDARY_2);
+		channel->tod_write = IDTCM_FW_REG(fw_ver, V520, TOD_WRITE_2);
+		channel->tod_n = IDTCM_FW_REG(fw_ver, V520, TOD_2);
+		channel->sync_src = SYNC_SOURCE_DPLL2_TOD_PPS;
+		break;
+	case 3:
+		channel->tod_read_primary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_PRIMARY_3);
+		channel->tod_read_secondary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_SECONDARY_3);
+		channel->tod_write = IDTCM_FW_REG(fw_ver, V520, TOD_WRITE_3);
+		channel->tod_n = IDTCM_FW_REG(fw_ver, V520, TOD_3);
+		channel->sync_src = SYNC_SOURCE_DPLL3_TOD_PPS;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int idtcm_enable_channel(struct idtcm *idtcm, u32 index)
+{
+	struct idtcm_channel *channel;
+	int err;
+	int i;
+
+	if (!(index < MAX_TOD))
+		return -EINVAL;
+
+	channel = &idtcm->channel[index];
+
+	channel->idtcm = idtcm;
+	channel->current_freq_scaled_ppm = 0;
+
+	/* Set pll addresses */
+	err = configure_channel_pll(channel);
+	if (err)
+		return err;
+
+	/* Set tod addresses */
+	err = configure_channel_tod(channel, index);
+	if (err)
+		return err;
+
+	if (idtcm->fw_ver < V487)
+		channel->caps = idtcm_caps_deprecated;
+	else
+		channel->caps = idtcm_caps;
+
+	snprintf(channel->caps.name, sizeof(channel->caps.name),
+		 "IDT CM TOD%u", index);
+
+	channel->caps.pin_config = pin_config[index];
+
+	for (i = 0; i < channel->caps.n_pins; ++i) {
+		struct ptp_pin_desc *ppd = &channel->caps.pin_config[i];
+
+		snprintf(ppd->name, sizeof(ppd->name), "input_ref%d", i);
+		ppd->index = i;
+		ppd->func = PTP_PF_NONE;
+		ppd->chan = index;
+	}
+
+	err = initialize_dco_operating_mode(channel);
+	if (err)
+		return err;
+
+	err = idtcm_enable_tod(channel);
+	if (err) {
+		dev_err(idtcm->dev,
+			"Failed at line %d in %s!", __LINE__, __func__);
+		return err;
+	}
+
+	channel->dco_delay = idtcm_get_dco_delay(channel);
+
+	channel->ptp_clock = ptp_clock_register(&channel->caps, NULL);
+
+	if (IS_ERR(channel->ptp_clock)) {
+		err = PTR_ERR(channel->ptp_clock);
+		channel->ptp_clock = NULL;
+		return err;
+	}
+
+	if (!channel->ptp_clock)
+		return -ENOTSUPP;
+
+	dev_info(idtcm->dev, "PLL%d registered as ptp%d",
+		 index, channel->ptp_clock->index);
+
+	return 0;
+}
+
+static int idtcm_enable_extts_channel(struct idtcm *idtcm, u32 index)
+{
+	struct idtcm_channel *channel;
+	int err;
+
+	if (!(index < MAX_TOD))
+		return -EINVAL;
+
+	channel = &idtcm->channel[index];
+	channel->idtcm = idtcm;
+
+	/* Set tod addresses */
+	err = configure_channel_tod(channel, index);
+	if (err)
+		return err;
+
+	channel->idtcm = idtcm;
+
+	return 0;
+}
+
+static void idtcm_extts_check(struct work_struct *work)
+{
+	struct idtcm *idtcm = container_of(work, struct idtcm, extts_work.work);
+	struct idtcm_channel *channel;
+	u8 mask;
+	int err;
+	int i;
+
+	if (idtcm->extts_mask == 0)
+		return;
+
+	mutex_lock(idtcm->lock);
+
+	for (i = 0; i < MAX_TOD; i++) {
+		mask = 1 << i;
+
+		if ((idtcm->extts_mask & mask) == 0)
+			continue;
+
+		err = idtcm_extts_check_channel(idtcm, i);
+
+		if (err == 0) {
+			/* trigger clears itself, so clear the mask */
+			if (idtcm->extts_single_shot) {
+				idtcm->extts_mask &= ~mask;
+			} else {
+				/* Re-arm */
+				channel = &idtcm->channel[i];
+				arm_tod_read_trig_sel_refclk(channel, channel->refn);
+			}
+		}
+	}
+
+	if (idtcm->extts_mask)
+		schedule_delayed_work(&idtcm->extts_work,
+				      msecs_to_jiffies(EXTTS_PERIOD_MS));
+
+	mutex_unlock(idtcm->lock);
+}
+
+static void ptp_clock_unregister_all(struct idtcm *idtcm)
+{
+	u8 i;
+	struct idtcm_channel *channel;
+
+	for (i = 0; i < MAX_TOD; i++) {
+		channel = &idtcm->channel[i];
+		if (channel->ptp_clock)
+			ptp_clock_unregister(channel->ptp_clock);
+	}
+}
+
+static void set_default_masks(struct idtcm *idtcm)
+{
+	idtcm->tod_mask = DEFAULT_TOD_MASK;
+	idtcm->extts_mask = 0;
+
+	idtcm->channel[0].tod = 0;
+	idtcm->channel[1].tod = 1;
+	idtcm->channel[2].tod = 2;
+	idtcm->channel[3].tod = 3;
+
+	idtcm->channel[0].pll = DEFAULT_TOD0_PTP_PLL;
+	idtcm->channel[1].pll = DEFAULT_TOD1_PTP_PLL;
+	idtcm->channel[2].pll = DEFAULT_TOD2_PTP_PLL;
+	idtcm->channel[3].pll = DEFAULT_TOD3_PTP_PLL;
+
+	idtcm->channel[0].output_mask = DEFAULT_OUTPUT_MASK_PLL0;
+	idtcm->channel[1].output_mask = DEFAULT_OUTPUT_MASK_PLL1;
+	idtcm->channel[2].output_mask = DEFAULT_OUTPUT_MASK_PLL2;
+	idtcm->channel[3].output_mask = DEFAULT_OUTPUT_MASK_PLL3;
+}
+
+static int idtcm_probe(struct platform_device *pdev)
+{
+	struct rsmu_ddata *ddata = dev_get_drvdata(pdev->dev.parent);
+	struct idtcm *idtcm;
+	int err;
+	u8 i;
+
+	idtcm = devm_kzalloc(&pdev->dev, sizeof(struct idtcm), GFP_KERNEL);
+
+	if (!idtcm)
+		return -ENOMEM;
+
+	idtcm->dev = &pdev->dev;
+	idtcm->mfd = pdev->dev.parent;
+	idtcm->lock = &ddata->lock;
+	idtcm->regmap = ddata->regmap;
+	idtcm->calculate_overhead_flag = 0;
+
+	INIT_DELAYED_WORK(&idtcm->extts_work, idtcm_extts_check);
+
+	set_default_masks(idtcm);
+
+	mutex_lock(idtcm->lock);
+
+	idtcm_set_version_info(idtcm);
+
+	err = idtcm_load_firmware(idtcm, &pdev->dev);
+
+	if (err)
+		dev_warn(idtcm->dev, "loading firmware failed with %d", err);
+
+	wait_for_chip_ready(idtcm);
+
+	if (idtcm->tod_mask) {
+		for (i = 0; i < MAX_TOD; i++) {
+			if (idtcm->tod_mask & (1 << i))
+				err = idtcm_enable_channel(idtcm, i);
+			else
+				err = idtcm_enable_extts_channel(idtcm, i);
+			if (err) {
+				dev_err(idtcm->dev,
+					"idtcm_enable_channel %d failed!", i);
+				break;
+			}
+		}
+	} else {
+		dev_err(idtcm->dev,
+			"no PLLs flagged as PHCs, nothing to do");
+		err = -ENODEV;
+	}
+
+	mutex_unlock(idtcm->lock);
+
+	if (err) {
+		ptp_clock_unregister_all(idtcm);
+		return err;
+	}
+
+	platform_set_drvdata(pdev, idtcm);
+
+	return 0;
+}
+
+static int idtcm_remove(struct platform_device *pdev)
+{
+	struct idtcm *idtcm = platform_get_drvdata(pdev);
+
+	idtcm->extts_mask = 0;
+	ptp_clock_unregister_all(idtcm);
+	cancel_delayed_work_sync(&idtcm->extts_work);
+
+	return 0;
+}
+
+static struct platform_driver idtcm_driver = {
+	.driver = {
+		.name = "8a3400x-phc",
+	},
+	.probe = idtcm_probe,
+	.remove	= idtcm_remove,
+};
+
+module_platform_driver(idtcm_driver);