ptp: ptp_clockmatrix: update to support 4.8.7 firmware

With 4.8.7 firmware, adjtime can change delta instead of absolute time,
which greately increases snap accuracy. PPS alignment doesn't have to
be set for every single TOD change. Other minor changes includes:
adding more debug logs, increasing snap accuracy for pre 4.8.7 firmware
and supporting new tcs2bin format.

Signed-off-by: Min Li <min.li.xe@renesas.com>
Acked-by: Richard Cochran <richardcochran@gmail.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

3 files changed, 1045 insertions, 209 deletions

diff --git a/drivers/ptp/idt8a340_reg.h b/drivers/ptp/idt8a340_reg.h
index 69eedda9f731..b297c4aba2ba 100644
--- a/drivers/ptp/idt8a340_reg.h
+++ b/drivers/ptp/idt8a340_reg.h
@@ -20,6 +20,10 @@
 #define HW_DPLL_1                         (0x8b00)
 #define HW_DPLL_2                         (0x8c00)
 #define HW_DPLL_3                         (0x8d00)
+#define HW_DPLL_4                         (0x8e00)
+#define HW_DPLL_5                         (0x8f00)
+#define HW_DPLL_6                         (0x9000)
+#define HW_DPLL_7                         (0x9100)
 
 #define HW_DPLL_TOD_SW_TRIG_ADDR__0       (0x080)
 #define HW_DPLL_TOD_CTRL_1                (0x089)
@@ -57,6 +61,43 @@
 #define SYNCTRL1_Q1_DIV_SYNC_TRIG	BIT(1)
 #define SYNCTRL1_Q0_DIV_SYNC_TRIG	BIT(0)
 
+#define HW_Q8_CTRL_SPARE  (0xa7d4)
+#define HW_Q11_CTRL_SPARE (0xa7ec)
+
+/**
+ * Select FOD5 as sync_trigger for Q8 divider.
+ * Transition from logic zero to one
+ * sets trigger to sync Q8 divider.
+ *
+ * Unused when FOD4 is driving Q8 divider (normal operation).
+ */
+#define Q9_TO_Q8_SYNC_TRIG  BIT(1)
+
+/**
+ * Enable FOD5 as driver for clock and sync for Q8 divider.
+ * Enable fanout buffer for FOD5.
+ *
+ * Unused when FOD4 is driving Q8 divider (normal operation).
+ */
+#define Q9_TO_Q8_FANOUT_AND_CLOCK_SYNC_ENABLE_MASK  (BIT(0) | BIT(2))
+
+/**
+ * Select FOD6 as sync_trigger for Q11 divider.
+ * Transition from logic zero to one
+ * sets trigger to sync Q11 divider.
+ *
+ * Unused when FOD7 is driving Q11 divider (normal operation).
+ */
+#define Q10_TO_Q11_SYNC_TRIG  BIT(1)
+
+/**
+ * Enable FOD6 as driver for clock and sync for Q11 divider.
+ * Enable fanout buffer for FOD6.
+ *
+ * Unused when FOD7 is driving Q11 divider (normal operation).
+ */
+#define Q10_TO_Q11_FANOUT_AND_CLOCK_SYNC_ENABLE_MASK  (BIT(0) | BIT(2))
+
 #define RESET_CTRL                        0xc000
 #define SM_RESET                          0x0012
 #define SM_RESET_CMD                      0x5A
@@ -191,6 +232,7 @@
 
 #define DPLL_CTRL_0                       0xc600
 #define DPLL_CTRL_DPLL_MANU_REF_CFG       0x0001
+#define DPLL_CTRL_COMBO_MASTER_CFG        0x003a
 
 #define DPLL_CTRL_1                       0xc63c
 
@@ -646,6 +688,9 @@
 /* Bit definitions for the TOD_WRITE_CMD register */
 #define TOD_WRITE_SELECTION_SHIFT         (0)
 #define TOD_WRITE_SELECTION_MASK          (0xf)
+/* 4.8.7 */
+#define TOD_WRITE_TYPE_SHIFT              (4)
+#define TOD_WRITE_TYPE_MASK               (0x3)
 
 /* Bit definitions for the TOD_READ_PRIMARY_SEL_CFG_0 register */
 #define RD_PWM_DECODER_INDEX_SHIFT        (4)
@@ -658,4 +703,7 @@
 #define TOD_READ_TRIGGER_SHIFT            (0)
 #define TOD_READ_TRIGGER_MASK             (0xf)
 
+/* Bit definitions for the DPLL_CTRL_COMBO_MASTER_CFG register */
+#define COMBO_MASTER_HOLD                 BIT(0)
+
 #endif
diff --git a/drivers/ptp/ptp_clockmatrix.c b/drivers/ptp/ptp_clockmatrix.c
index ceb6bc58f3b4..73aaae5574ed 100644
--- a/drivers/ptp/ptp_clockmatrix.c
+++ b/drivers/ptp/ptp_clockmatrix.c
@@ -13,6 +13,7 @@
 #include <linux/jiffies.h>
 #include <linux/kernel.h>
 #include <linux/timekeeping.h>
+#include <linux/string.h>
 
 #include "ptp_private.h"
 #include "ptp_clockmatrix.h"
@@ -23,6 +24,13 @@ 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)
 
 static long set_write_phase_ready(struct ptp_clock_info *ptp)
@@ -95,6 +103,45 @@ static int timespec_to_char_array(struct timespec64 const *ts,
 	return 0;
 }
 
+static int idtcm_strverscmp(const char *ver1, const char *ver2)
+{
+	u8 num1;
+	u8 num2;
+	int result = 0;
+
+	/* loop through each level of the version string */
+	while (result == 0) {
+		/* extract leading version numbers */
+		if (kstrtou8(ver1, 10, &num1) < 0)
+			return -1;
+
+		if (kstrtou8(ver2, 10, &num2) < 0)
+			return -1;
+
+		/* if numbers differ, then set the result */
+		if (num1 < num2)
+			result = -1;
+		else if (num1 > num2)
+			result = 1;
+		else {
+			/* if numbers are the same, go to next level */
+			ver1 = strchr(ver1, '.');
+			ver2 = strchr(ver2, '.');
+			if (!ver1 && !ver2)
+				break;
+			else if (!ver1)
+				result = -1;
+			else if (!ver2)
+				result = 1;
+			else {
+				ver1++;
+				ver2++;
+			}
+		}
+	}
+	return result;
+}
+
 static int idtcm_xfer(struct idtcm *idtcm,
 		      u8 regaddr,
 		      u8 *buf,
@@ -104,6 +151,7 @@ static int idtcm_xfer(struct idtcm *idtcm,
 	struct i2c_client *client = idtcm->client;
 	struct i2c_msg msg[2];
 	int cnt;
+	char *fmt = "i2c_transfer failed at %d in %s for %s, at addr: %04X!\n";
 
 	msg[0].addr = client->addr;
 	msg[0].flags = 0;
@@ -118,7 +166,12 @@ static int idtcm_xfer(struct idtcm *idtcm,
 	cnt = i2c_transfer(client->adapter, msg, 2);
 
 	if (cnt < 0) {
-		dev_err(&client->dev, "i2c_transfer returned %d\n", cnt);
+		dev_err(&client->dev,
+			fmt,
+			__LINE__,
+			__func__,
+			write ? "write" : "read",
+			regaddr);
 		return cnt;
 	} else if (cnt != 2) {
 		dev_err(&client->dev,
@@ -144,10 +197,12 @@ static int idtcm_page_offset(struct idtcm *idtcm, u8 val)
 
 	err = idtcm_xfer(idtcm, PAGE_ADDR, buf, sizeof(buf), 1);
 
-	if (err)
+	if (err) {
+		idtcm->page_offset = 0xff;
 		dev_err(&idtcm->client->dev, "failed to set page offset\n");
-	else
+	} else {
 		idtcm->page_offset = val;
+	}
 
 	return err;
 }
@@ -198,6 +253,7 @@ static int _idtcm_gettime(struct idtcm_channel *channel,
 {
 	struct idtcm *idtcm = channel->idtcm;
 	u8 buf[TOD_BYTE_COUNT];
+	u8 timeout = 10;
 	u8 trigger;
 	int err;
 
@@ -208,16 +264,29 @@ static int _idtcm_gettime(struct idtcm_channel *channel,
 
 	trigger &= ~(TOD_READ_TRIGGER_MASK << TOD_READ_TRIGGER_SHIFT);
 	trigger |= (1 << TOD_READ_TRIGGER_SHIFT);
-	trigger |= TOD_READ_TRIGGER_MODE;
+	trigger &= ~TOD_READ_TRIGGER_MODE; /* single shot */
 
 	err = idtcm_write(idtcm, channel->tod_read_primary,
 			  TOD_READ_PRIMARY_CMD, &trigger, sizeof(trigger));
-
 	if (err)
 		return err;
 
-	if (idtcm->calculate_overhead_flag)
-		idtcm->start_time = ktime_get_raw();
+	/* wait trigger to be 0 */
+	while (trigger & TOD_READ_TRIGGER_MASK) {
+
+		if (idtcm->calculate_overhead_flag)
+			idtcm->start_time = ktime_get_raw();
+
+		err = idtcm_read(idtcm, channel->tod_read_primary,
+				 TOD_READ_PRIMARY_CMD, &trigger,
+				 sizeof(trigger));
+
+		if (err)
+			return err;
+
+		if (--timeout == 0)
+			return -EIO;
+	}
 
 	err = idtcm_read(idtcm, channel->tod_read_primary,
 			 TOD_READ_PRIMARY, buf, sizeof(buf));
@@ -240,6 +309,7 @@ static int _sync_pll_output(struct idtcm *idtcm,
 	u8 val;
 	u16 sync_ctrl0;
 	u16 sync_ctrl1;
+	u8 temp;
 
 	if ((qn == 0) && (qn_plus_1 == 0))
 		return 0;
@@ -305,6 +375,50 @@ static int _sync_pll_output(struct idtcm *idtcm,
 	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));
@@ -312,6 +426,30 @@ static int _sync_pll_output(struct idtcm *idtcm,
 	return err;
 }
 
+static int sync_source_dpll_tod_pps(u16 tod_addr, u8 *sync_src)
+{
+	int err = 0;
+
+	switch (tod_addr) {
+	case TOD_0:
+		*sync_src = SYNC_SOURCE_DPLL0_TOD_PPS;
+		break;
+	case TOD_1:
+		*sync_src = SYNC_SOURCE_DPLL1_TOD_PPS;
+		break;
+	case TOD_2:
+		*sync_src = SYNC_SOURCE_DPLL2_TOD_PPS;
+		break;
+	case TOD_3:
+		*sync_src = SYNC_SOURCE_DPLL3_TOD_PPS;
+		break;
+	default:
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
 static int idtcm_sync_pps_output(struct idtcm_channel *channel)
 {
 	struct idtcm *idtcm = channel->idtcm;
@@ -321,37 +459,68 @@ static int idtcm_sync_pps_output(struct idtcm_channel *channel)
 	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;
 
-	switch (channel->dpll_n) {
-	case DPLL_0:
-		sync_src = SYNC_SOURCE_DPLL0_TOD_PPS;
-		break;
-	case DPLL_1:
-		sync_src = SYNC_SOURCE_DPLL1_TOD_PPS;
-		break;
-	case DPLL_2:
-		sync_src = SYNC_SOURCE_DPLL2_TOD_PPS;
-		break;
-	case DPLL_3:
-		sync_src = SYNC_SOURCE_DPLL3_TOD_PPS;
-		break;
-	default:
-		return -EINVAL;
-	}
+	err = sync_source_dpll_tod_pps(channel->tod_n, &sync_src);
+	if (err)
+		return err;
 
-	for (pll = 0; pll < 8; pll++) {
+	err = idtcm_read(idtcm, 0, HW_Q8_CTRL_SPARE,
+			 &temp, sizeof(temp));
+	if (err)
+		return err;
 
-		qn = output_mask & 0x1;
-		output_mask = output_mask >> 1;
+	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 {
-			qn_plus_1 = 0;
+		} 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))
@@ -365,7 +534,7 @@ static int idtcm_sync_pps_output(struct idtcm_channel *channel)
 	return err;
 }
 
-static int _idtcm_set_dpll_tod(struct idtcm_channel *channel,
+static int _idtcm_set_dpll_hw_tod(struct idtcm_channel *channel,
 			       struct timespec64 const *ts,
 			       enum hw_tod_write_trig_sel wr_trig)
 {
@@ -439,17 +608,78 @@ static int _idtcm_set_dpll_tod(struct idtcm_channel *channel,
 	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 == 0)
+			break;
+
+		if (++count > 20) {
+			dev_err(&idtcm->client->dev,
+				"Timed out waiting for the write counter\n");
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
 static int _idtcm_settime(struct idtcm_channel *channel,
 			  struct timespec64 const *ts,
 			  enum hw_tod_write_trig_sel wr_trig)
 {
 	struct idtcm *idtcm = channel->idtcm;
-	s32 retval;
 	int err;
 	int i;
 	u8 trig_sel;
 
-	err = _idtcm_set_dpll_tod(channel, ts, wr_trig);
+	err = _idtcm_set_dpll_hw_tod(channel, ts, wr_trig);
 
 	if (err)
 		return err;
@@ -469,12 +699,24 @@ static int _idtcm_settime(struct idtcm_channel *channel,
 		err = 1;
 	}
 
-	if (err)
+	if (err) {
+		dev_err(&idtcm->client->dev,
+			"Failed at line %d in func %s!\n",
+			__LINE__,
+			__func__);
 		return err;
+	}
 
-	retval = idtcm_sync_pps_output(channel);
+	return idtcm_sync_pps_output(channel);
+}
 
-	return retval;
+static int _idtcm_settime_v487(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,
@@ -565,6 +807,50 @@ static int idtcm_do_phase_pull_in(struct idtcm_channel *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;
+
+	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();
+
+		current_ns = ktime_to_ns(stop - start);
+
+		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(struct idtcm_channel *channel, s64 delta)
 {
 	int err;
@@ -577,6 +863,11 @@ static int _idtcm_adjtime(struct idtcm_channel *channel, s64 delta)
 	} else {
 		idtcm->calculate_overhead_flag = 1;
 
+		err = set_tod_write_overhead(channel);
+
+		if (err)
+			return err;
+
 		err = _idtcm_gettime(channel, &ts);
 
 		if (err)
@@ -656,93 +947,118 @@ static int idtcm_read_otp_scsr_config_select(struct idtcm *idtcm,
 			  config_select, sizeof(u8));
 }
 
-static int process_pll_mask(struct idtcm *idtcm, u32 addr, u8 val, u8 *mask)
-{
-	int err = 0;
-
-	if (addr == PLL_MASK_ADDR) {
-		if ((val & 0xf0) || !(val & 0xf)) {
-			dev_err(&idtcm->client->dev,
-				"Invalid PLL mask 0x%hhx\n", val);
-			err = -EINVAL;
-		}
-		*mask = val;
-	}
-
-	return err;
-}
-
 static int set_pll_output_mask(struct idtcm *idtcm, u16 addr, u8 val)
 {
 	int err = 0;
 
 	switch (addr) {
-	case OUTPUT_MASK_PLL0_ADDR:
+	case TOD0_OUT_ALIGN_MASK_ADDR:
 		SET_U16_LSB(idtcm->channel[0].output_mask, val);
 		break;
-	case OUTPUT_MASK_PLL0_ADDR + 1:
+	case TOD0_OUT_ALIGN_MASK_ADDR + 1:
 		SET_U16_MSB(idtcm->channel[0].output_mask, val);
 		break;
-	case OUTPUT_MASK_PLL1_ADDR:
+	case TOD1_OUT_ALIGN_MASK_ADDR:
 		SET_U16_LSB(idtcm->channel[1].output_mask, val);
 		break;
-	case OUTPUT_MASK_PLL1_ADDR + 1:
+	case TOD1_OUT_ALIGN_MASK_ADDR + 1:
 		SET_U16_MSB(idtcm->channel[1].output_mask, val);
 		break;
-	case OUTPUT_MASK_PLL2_ADDR:
+	case TOD2_OUT_ALIGN_MASK_ADDR:
 		SET_U16_LSB(idtcm->channel[2].output_mask, val);
 		break;
-	case OUTPUT_MASK_PLL2_ADDR + 1:
+	case TOD2_OUT_ALIGN_MASK_ADDR + 1:
 		SET_U16_MSB(idtcm->channel[2].output_mask, val);
 		break;
-	case OUTPUT_MASK_PLL3_ADDR:
+	case TOD3_OUT_ALIGN_MASK_ADDR:
 		SET_U16_LSB(idtcm->channel[3].output_mask, val);
 		break;
-	case OUTPUT_MASK_PLL3_ADDR + 1:
+	case TOD3_OUT_ALIGN_MASK_ADDR + 1:
 		SET_U16_MSB(idtcm->channel[3].output_mask, val);
 		break;
 	default:
-		err = -EINVAL;
+		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->client->dev, "ToD%d not supported\n", index);
+		return -EINVAL;
+	}
+
+	if (pll >= MAX_PLL) {
+		dev_err(&idtcm->client->dev, "Pll%d not supported\n", 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;
 
-	if (set_pll_output_mask(idtcm, regaddr, val)) {
-		/* Not an output mask, check for pll mask */
-		err = process_pll_mask(idtcm, regaddr, val, &idtcm->pll_mask);
+	switch (regaddr) {
+	case TOD_MASK_ADDR:
+		if ((val & 0xf0) || !(val & 0x0f)) {
+			dev_err(&idtcm->client->dev,
+				"Invalid TOD mask 0x%hhx\n", 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_output_masks(struct idtcm *idtcm)
+static void display_pll_and_masks(struct idtcm *idtcm)
 {
 	u8 i;
 	u8 mask;
 
-	dev_dbg(&idtcm->client->dev, "pllmask = 0x%02x\n", idtcm->pll_mask);
+	dev_dbg(&idtcm->client->dev, "tod_mask = 0x%02x\n", idtcm->tod_mask);
 
-	for (i = 0; i < MAX_PHC_PLL; i++) {
+	for (i = 0; i < MAX_TOD; i++) {
 		mask = 1 << i;
 
-		if (mask & idtcm->pll_mask)
+		if (mask & idtcm->tod_mask)
 			dev_dbg(&idtcm->client->dev,
-				"PLL%d output_mask = 0x%04x\n",
-				i, idtcm->channel[i].output_mask);
+				"TOD%d pll = %d    output_mask = 0x%04x\n",
+				i, idtcm->channel[i].pll,
+				idtcm->channel[i].output_mask);
 	}
 }
 
 static int idtcm_load_firmware(struct idtcm *idtcm,
 			       struct device *dev)
 {
+	char fname[128] = FW_FILENAME;
 	const struct firmware *fw;
 	struct idtcm_fwrc *rec;
 	u32 regaddr;
@@ -751,12 +1067,20 @@ static int idtcm_load_firmware(struct idtcm *idtcm,
 	u8 val;
 	u8 loaddr;
 
-	dev_dbg(&idtcm->client->dev, "requesting firmware '%s'\n", FW_FILENAME);
+	if (firmware) /* module parameter */
+		snprintf(fname, sizeof(fname), "%s", firmware);
 
-	err = request_firmware(&fw, FW_FILENAME, dev);
+	dev_dbg(&idtcm->client->dev, "requesting firmware '%s'\n", fname);
 
-	if (err)
+	err = request_firmware(&fw, fname, dev);
+
+	if (err) {
+		dev_err(&idtcm->client->dev,
+			"Failed at line %d in func %s!\n",
+			__LINE__,
+			__func__);
 		return err;
+	}
 
 	dev_dbg(&idtcm->client->dev, "firmware size %zu bytes\n", fw->size);
 
@@ -783,7 +1107,9 @@ static int idtcm_load_firmware(struct idtcm *idtcm,
 			err = check_and_set_masks(idtcm, regaddr, val);
 		}
 
-		if (err == 0) {
+		if (err != -EINVAL) {
+			err = 0;
+
 			/* Top (status registers) and bottom are read-only */
 			if ((regaddr < GPIO_USER_CONTROL)
 			    || (regaddr >= SCRATCH))
@@ -801,42 +1127,22 @@ static int idtcm_load_firmware(struct idtcm *idtcm,
 			goto out;
 	}
 
-	display_pll_and_output_masks(idtcm);
+	display_pll_and_masks(idtcm);
 
 out:
 	release_firmware(fw);
 	return err;
 }
 
-static int idtcm_pps_enable(struct idtcm_channel *channel, bool enable)
+static int idtcm_output_enable(struct idtcm_channel *channel,
+			       bool enable, unsigned int outn)
 {
 	struct idtcm *idtcm = channel->idtcm;
-	u32 module;
-	u8 val;
 	int err;
+	u8 val;
 
-	/*
-	 * This assumes that the 1-PPS is on the second of the two
-	 * output.  But is this always true?
-	 */
-	switch (channel->dpll_n) {
-	case DPLL_0:
-		module = OUTPUT_1;
-		break;
-	case DPLL_1:
-		module = OUTPUT_3;
-		break;
-	case DPLL_2:
-		module = OUTPUT_5;
-		break;
-	case DPLL_3:
-		module = OUTPUT_7;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	err = idtcm_read(idtcm, module, OUT_CTRL_1, &val, sizeof(val));
+	err = idtcm_read(idtcm, OUTPUT_MODULE_FROM_INDEX(outn),
+			 OUT_CTRL_1, &val, sizeof(val));
 
 	if (err)
 		return err;
@@ -846,14 +1152,50 @@ static int idtcm_pps_enable(struct idtcm_channel *channel, bool enable)
 	else
 		val &= ~SQUELCH_DISABLE;
 
-	err = idtcm_write(idtcm, module, OUT_CTRL_1, &val, sizeof(val));
+	return idtcm_write(idtcm, OUTPUT_MODULE_FROM_INDEX(outn),
+			   OUT_CTRL_1, &val, sizeof(val));
+}
 
-	if (err)
-		return err;
+static int idtcm_output_mask_enable(struct idtcm_channel *channel,
+				    bool enable)
+{
+	u16 mask;
+	int err;
+	u8 outn;
+
+	mask = channel->output_mask;
+	outn = 0;
+
+	while (mask) {
+
+		if (mask & 0x1) {
+
+			err = idtcm_output_enable(channel, enable, outn);
+
+			if (err)
+				return err;
+		}
+
+		mask >>= 0x1;
+		outn++;
+	}
 
 	return 0;
 }
 
+static int idtcm_perout_enable(struct idtcm_channel *channel,
+			       bool enable,
+			       struct ptp_perout_request *perout)
+{
+	unsigned int flags = perout->flags;
+
+	if (flags == PEROUT_ENABLE_OUTPUT_MASK)
+		return idtcm_output_mask_enable(channel, enable);
+
+	/* Enable/disable individual output instead */
+	return idtcm_output_enable(channel, enable, perout->index);
+}
+
 static int idtcm_set_pll_mode(struct idtcm_channel *channel,
 			      enum pll_mode pll_mode)
 {
@@ -940,10 +1282,8 @@ static int _idtcm_adjphase(struct idtcm_channel *channel, s32 delta_ns)
 	return err;
 }
 
-static int idtcm_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+static int _idtcm_adjfine(struct idtcm_channel *channel, long scaled_ppm)
 {
-	struct idtcm_channel *channel =
-		container_of(ptp, struct idtcm_channel, caps);
 	struct idtcm *idtcm = channel->idtcm;
 	u8 i;
 	bool neg_adj = 0;
@@ -970,15 +1310,15 @@ static int idtcm_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 	 * FCW = -------------
 	 *         111 * 2^4
 	 */
-	if (ppb < 0) {
+	if (scaled_ppm < 0) {
 		neg_adj = 1;
-		ppb = -ppb;
+		scaled_ppm = -scaled_ppm;
 	}
 
 	/* 2 ^ -53 = 1.1102230246251565404236316680908e-16 */
-	fcw = ppb * 1000000000000ULL;
+	fcw = scaled_ppm * 244140625ULL;
 
-	fcw = div_u64(fcw, 111022);
+	fcw = div_u64(fcw, 1776);
 
 	if (neg_adj)
 		fcw = -fcw;
@@ -988,12 +1328,9 @@ static int idtcm_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 		fcw >>= 8;
 	}
 
-	mutex_lock(&idtcm->reg_lock);
-
 	err = idtcm_write(idtcm, channel->dpll_freq, DPLL_WR_FREQ,
 			  buf, sizeof(buf));
 
-	mutex_unlock(&idtcm->reg_lock);
 	return err;
 }
 
@@ -1008,6 +1345,12 @@ static int idtcm_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 
 	err = _idtcm_gettime(channel, ts);
 
+	if (err)
+		dev_err(&idtcm->client->dev,
+			"Failed at line %d in func %s!\n",
+			__LINE__,
+			__func__);
+
 	mutex_unlock(&idtcm->reg_lock);
 
 	return err;
@@ -1025,6 +1368,35 @@ static int idtcm_settime(struct ptp_clock_info *ptp,
 
 	err = _idtcm_settime(channel, ts, HW_TOD_WR_TRIG_SEL_MSB);
 
+	if (err)
+		dev_err(&idtcm->client->dev,
+			"Failed at line %d in func %s!\n",
+			__LINE__,
+			__func__);
+
+	mutex_unlock(&idtcm->reg_lock);
+
+	return err;
+}
+
+static int idtcm_settime_v487(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->reg_lock);
+
+	err = _idtcm_settime_v487(channel, ts, SCSR_TOD_WR_TYPE_SEL_ABSOLUTE);
+
+	if (err)
+		dev_err(&idtcm->client->dev,
+			"Failed at line %d in func %s!\n",
+			__LINE__,
+			__func__);
+
 	mutex_unlock(&idtcm->reg_lock);
 
 	return err;
@@ -1041,6 +1413,54 @@ static int idtcm_adjtime(struct ptp_clock_info *ptp, s64 delta)
 
 	err = _idtcm_adjtime(channel, delta);
 
+	if (err)
+		dev_err(&idtcm->client->dev,
+			"Failed at line %d in func %s!\n",
+			__LINE__,
+			__func__);
+
+	mutex_unlock(&idtcm->reg_lock);
+
+	return err;
+}
+
+static int idtcm_adjtime_v487(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 (abs(delta) < PHASE_PULL_IN_THRESHOLD_NS_V487) {
+		err = idtcm_do_phase_pull_in(channel, delta, 0);
+		if (err)
+			dev_err(&idtcm->client->dev,
+				"Failed at line %d in func %s!\n",
+				__LINE__,
+				__func__);
+		return err;
+	}
+
+	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;
+	}
+
+	mutex_lock(&idtcm->reg_lock);
+
+	err = _idtcm_settime_v487(channel, &ts, type);
+
+	if (err)
+		dev_err(&idtcm->client->dev,
+			"Failed at line %d in func %s!\n",
+			__LINE__,
+			__func__);
+
 	mutex_unlock(&idtcm->reg_lock);
 
 	return err;
@@ -1059,6 +1479,36 @@ static int idtcm_adjphase(struct ptp_clock_info *ptp, s32 delta)
 
 	err = _idtcm_adjphase(channel, delta);
 
+	if (err)
+		dev_err(&idtcm->client->dev,
+			"Failed at line %d in func %s!\n",
+			__LINE__,
+			__func__);
+
+	mutex_unlock(&idtcm->reg_lock);
+
+	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;
+
+	mutex_lock(&idtcm->reg_lock);
+
+	err = _idtcm_adjfine(channel, scaled_ppm);
+
+	if (err)
+		dev_err(&idtcm->client->dev,
+			"Failed at line %d in func %s!\n",
+			__LINE__,
+			__func__);
+
 	mutex_unlock(&idtcm->reg_lock);
 
 	return err;
@@ -1067,20 +1517,35 @@ static int idtcm_adjphase(struct ptp_clock_info *ptp, s32 delta)
 static int idtcm_enable(struct ptp_clock_info *ptp,
 			struct ptp_clock_request *rq, int on)
 {
+	int err;
+
 	struct idtcm_channel *channel =
 		container_of(ptp, struct idtcm_channel, caps);
 
 	switch (rq->type) {
 	case PTP_CLK_REQ_PEROUT:
-		if (!on)
-			return idtcm_pps_enable(channel, false);
+		if (!on) {
+			err = idtcm_perout_enable(channel, false, &rq->perout);
+			if (err)
+				dev_err(&channel->idtcm->client->dev,
+					"Failed at line %d in func %s!\n",
+					__LINE__,
+					__func__);
+			return err;
+		}
 
 		/* Only accept a 1-PPS aligned to the second. */
 		if (rq->perout.start.nsec || rq->perout.period.sec != 1 ||
 		    rq->perout.period.nsec)
 			return -ERANGE;
 
-		return idtcm_pps_enable(channel, true);
+		err = idtcm_perout_enable(channel, true, &rq->perout);
+		if (err)
+			dev_err(&channel->idtcm->client->dev,
+				"Failed at line %d in func %s!\n",
+				__LINE__,
+				__func__);
+		return err;
 	default:
 		break;
 	}
@@ -1088,17 +1553,237 @@ static int idtcm_enable(struct ptp_clock_info *ptp,
 	return -EOPNOTSUPP;
 }
 
-static int idtcm_enable_tod(struct idtcm_channel *channel)
+static int _enable_pll_tod_sync(struct idtcm *idtcm,
+				u8 pll,
+				u8 sync_src,
+				u8 qn,
+				u8 qn_plus_1)
+{
+	int err;
+	u8 val;
+	u16 dpll;
+	u16 out0 = 0, out1 = 0;
+
+	if ((qn == 0) && (qn_plus_1 == 0))
+		return 0;
+
+	switch (pll) {
+	case 0:
+		dpll = DPLL_0;
+		if (qn)
+			out0 = OUTPUT_0;
+		if (qn_plus_1)
+			out1 = OUTPUT_1;
+		break;
+	case 1:
+		dpll = DPLL_1;
+		if (qn)
+			out0 = OUTPUT_2;
+		if (qn_plus_1)
+			out1 = OUTPUT_3;
+		break;
+	case 2:
+		dpll = DPLL_2;
+		if (qn)
+			out0 = OUTPUT_4;
+		if (qn_plus_1)
+			out1 = OUTPUT_5;
+		break;
+	case 3:
+		dpll = DPLL_3;
+		if (qn)
+			out0 = OUTPUT_6;
+		if (qn_plus_1)
+			out1 = OUTPUT_7;
+		break;
+	case 4:
+		dpll = DPLL_4;
+		if (qn)
+			out0 = OUTPUT_8;
+		break;
+	case 5:
+		dpll = DPLL_5;
+		if (qn)
+			out0 = OUTPUT_9;
+		if (qn_plus_1)
+			out1 = OUTPUT_8;
+		break;
+	case 6:
+		dpll = DPLL_6;
+		if (qn)
+			out0 = OUTPUT_10;
+		if (qn_plus_1)
+			out1 = OUTPUT_11;
+		break;
+	case 7:
+		dpll = DPLL_7;
+		if (qn)
+			out0 = OUTPUT_11;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/*
+	 * Enable OUTPUT OUT_SYNC.
+	 */
+	if (out0) {
+		err = idtcm_read(idtcm, out0, OUT_CTRL_1, &val, sizeof(val));
+
+		if (err)
+			return err;
+
+		val &= ~OUT_SYNC_DISABLE;
+
+		err = idtcm_write(idtcm, out0, OUT_CTRL_1, &val, sizeof(val));
+
+		if (err)
+			return err;
+	}
+
+	if (out1) {
+		err = idtcm_read(idtcm, out1, OUT_CTRL_1, &val, sizeof(val));
+
+		if (err)
+			return err;
+
+		val &= ~OUT_SYNC_DISABLE;
+
+		err = idtcm_write(idtcm, out1, OUT_CTRL_1, &val, sizeof(val));
+
+		if (err)
+			return err;
+	}
+
+	/* enable dpll sync tod pps, must be set before dpll_mode */
+	err = idtcm_read(idtcm, dpll, DPLL_TOD_SYNC_CFG, &val, sizeof(val));
+	if (err)
+		return err;
+
+	val &= ~(TOD_SYNC_SOURCE_MASK << TOD_SYNC_SOURCE_SHIFT);
+	val |= (sync_src << TOD_SYNC_SOURCE_SHIFT);
+	val |= TOD_SYNC_EN;
+
+	return idtcm_write(idtcm, dpll, DPLL_TOD_SYNC_CFG, &val, sizeof(val));
+}
+
+static int idtcm_enable_tod_sync(struct idtcm_channel *channel)
 {
 	struct idtcm *idtcm = channel->idtcm;
-	struct timespec64 ts = {0, 0};
+
+	u8 pll;
+	u8 sync_src;
+	u8 qn;
+	u8 qn_plus_1;
 	u8 cfg;
-	int err;
+	int err = 0;
+	u16 output_mask = channel->output_mask;
+	u8 out8_mux = 0;
+	u8 out11_mux = 0;
+	u8 temp;
+
+	/*
+	 * set tod_out_sync_enable to 0.
+	 */
+	err = idtcm_read(idtcm, channel->tod_n, TOD_CFG, &cfg, sizeof(cfg));
+	if (err)
+		return err;
+
+	cfg &= ~TOD_OUT_SYNC_ENABLE;
+
+	err = idtcm_write(idtcm, channel->tod_n, TOD_CFG, &cfg, sizeof(cfg));
+	if (err)
+		return err;
+
+	switch (channel->tod_n) {
+	case TOD_0:
+		sync_src = 0;
+		break;
+	case TOD_1:
+		sync_src = 1;
+		break;
+	case TOD_2:
+		sync_src = 2;
+		break;
+	case TOD_3:
+		sync_src = 3;
+		break;
+	default:
+		return -EINVAL;
+	}
 
-	err = idtcm_pps_enable(channel, false);
+	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 = _enable_pll_tod_sync(idtcm, pll, sync_src, qn,
+					       qn_plus_1);
+
+		if (err)
+			return err;
+	}
+
+	return err;
+}
+
+static int idtcm_enable_tod(struct idtcm_channel *channel)
+{
+	struct idtcm *idtcm = channel->idtcm;
+	struct timespec64 ts = {0, 0};
+	u8 cfg;
+	int err;
+
 	/*
 	 * Start the TOD clock ticking.
 	 */
@@ -1134,16 +1819,32 @@ static void idtcm_display_version_info(struct idtcm *idtcm)
 
 	idtcm_read_otp_scsr_config_select(idtcm, &config_select);
 
+	snprintf(idtcm->version, sizeof(idtcm->version), "%u.%u.%u",
+		 major, minor, hotfix);
+
 	dev_info(&idtcm->client->dev, fmt, major, minor, hotfix,
 		 product_id, hw_rev_id, config_select);
 }
 
+static const struct ptp_clock_info idtcm_caps_v487 = {
+	.owner		= THIS_MODULE,
+	.max_adj	= 244000,
+	.n_per_out	= 12,
+	.adjphase	= &idtcm_adjphase,
+	.adjfine	= &idtcm_adjfine,
+	.adjtime	= &idtcm_adjtime_v487,
+	.gettime64	= &idtcm_gettime,
+	.settime64	= &idtcm_settime_v487,
+	.enable		= &idtcm_enable,
+	.do_aux_work	= &set_write_phase_ready,
+};
+
 static const struct ptp_clock_info idtcm_caps = {
 	.owner		= THIS_MODULE,
 	.max_adj	= 244000,
-	.n_per_out	= 1,
+	.n_per_out	= 12,
 	.adjphase	= &idtcm_adjphase,
-	.adjfreq	= &idtcm_adjfreq,
+	.adjfine	= &idtcm_adjfine,
 	.adjtime	= &idtcm_adjtime,
 	.gettime64	= &idtcm_gettime,
 	.settime64	= &idtcm_settime,
@@ -1151,24 +1852,14 @@ static const struct ptp_clock_info idtcm_caps = {
 	.do_aux_work	= &set_write_phase_ready,
 };
 
-
-static int idtcm_enable_channel(struct idtcm *idtcm, u32 index)
+static int configure_channel_pll(struct idtcm_channel *channel)
 {
-	struct idtcm_channel *channel;
-	int err;
-
-	if (!(index < MAX_PHC_PLL))
-		return -EINVAL;
-
-	channel = &idtcm->channel[index];
+	int err = 0;
 
-	switch (index) {
+	switch (channel->pll) {
 	case 0:
 		channel->dpll_freq = DPLL_FREQ_0;
 		channel->dpll_n = DPLL_0;
-		channel->tod_read_primary = TOD_READ_PRIMARY_0;
-		channel->tod_write = TOD_WRITE_0;
-		channel->tod_n = TOD_0;
 		channel->hw_dpll_n = HW_DPLL_0;
 		channel->dpll_phase = DPLL_PHASE_0;
 		channel->dpll_ctrl_n = DPLL_CTRL_0;
@@ -1177,9 +1868,6 @@ static int idtcm_enable_channel(struct idtcm *idtcm, u32 index)
 	case 1:
 		channel->dpll_freq = DPLL_FREQ_1;
 		channel->dpll_n = DPLL_1;
-		channel->tod_read_primary = TOD_READ_PRIMARY_1;
-		channel->tod_write = TOD_WRITE_1;
-		channel->tod_n = TOD_1;
 		channel->hw_dpll_n = HW_DPLL_1;
 		channel->dpll_phase = DPLL_PHASE_1;
 		channel->dpll_ctrl_n = DPLL_CTRL_1;
@@ -1188,9 +1876,6 @@ static int idtcm_enable_channel(struct idtcm *idtcm, u32 index)
 	case 2:
 		channel->dpll_freq = DPLL_FREQ_2;
 		channel->dpll_n = DPLL_2;
-		channel->tod_read_primary = TOD_READ_PRIMARY_2;
-		channel->tod_write = TOD_WRITE_2;
-		channel->tod_n = TOD_2;
 		channel->hw_dpll_n = HW_DPLL_2;
 		channel->dpll_phase = DPLL_PHASE_2;
 		channel->dpll_ctrl_n = DPLL_CTRL_2;
@@ -1199,31 +1884,129 @@ static int idtcm_enable_channel(struct idtcm *idtcm, u32 index)
 	case 3:
 		channel->dpll_freq = DPLL_FREQ_3;
 		channel->dpll_n = DPLL_3;
-		channel->tod_read_primary = TOD_READ_PRIMARY_3;
-		channel->tod_write = TOD_WRITE_3;
-		channel->tod_n = TOD_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 = 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 = 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;
+}
+
+static int idtcm_enable_channel(struct idtcm *idtcm, u32 index)
+{
+	struct idtcm_channel *channel;
+	int err;
+
+	if (!(index < MAX_TOD))
+		return -EINVAL;
+
+	channel = &idtcm->channel[index];
+
+	/* Set pll addresses */
+	err = configure_channel_pll(channel);
+	if (err)
+		return err;
+
+	/* Set tod addresses */
+	switch (index) {
+	case 0:
+		channel->tod_read_primary = TOD_READ_PRIMARY_0;
+		channel->tod_write = TOD_WRITE_0;
+		channel->tod_n = TOD_0;
+		break;
+	case 1:
+		channel->tod_read_primary = TOD_READ_PRIMARY_1;
+		channel->tod_write = TOD_WRITE_1;
+		channel->tod_n = TOD_1;
+		break;
+	case 2:
+		channel->tod_read_primary = TOD_READ_PRIMARY_2;
+		channel->tod_write = TOD_WRITE_2;
+		channel->tod_n = TOD_2;
+		break;
+	case 3:
+		channel->tod_read_primary = TOD_READ_PRIMARY_3;
+		channel->tod_write = TOD_WRITE_3;
+		channel->tod_n = TOD_3;
+		break;
 	default:
 		return -EINVAL;
 	}
 
 	channel->idtcm = idtcm;
 
-	channel->caps = idtcm_caps;
+	if (idtcm_strverscmp(idtcm->version, "4.8.7") >= 0)
+		channel->caps = idtcm_caps_v487;
+	else
+		channel->caps = idtcm_caps;
+
 	snprintf(channel->caps.name, sizeof(channel->caps.name),
-		 "IDT CM PLL%u", index);
+		 "IDT CM TOD%u", index);
+
+	if (idtcm_strverscmp(idtcm->version, "4.8.7") >= 0) {
+		err = idtcm_enable_tod_sync(channel);
+		if (err) {
+			dev_err(&idtcm->client->dev,
+				"Failed at line %d in func %s!\n",
+				__LINE__,
+				__func__);
+			return err;
+		}
+	}
 
 	err = idtcm_set_pll_mode(channel, PLL_MODE_WRITE_FREQUENCY);
-	if (err)
+	if (err) {
+		dev_err(&idtcm->client->dev,
+			"Failed at line %d in func %s!\n",
+			__LINE__,
+			__func__);
 		return err;
+	}
 
 	err = idtcm_enable_tod(channel);
-	if (err)
+	if (err) {
+		dev_err(&idtcm->client->dev,
+			"Failed at line %d in func %s!\n",
+			__LINE__,
+			__func__);
 		return err;
+	}
 
 	channel->ptp_clock = ptp_clock_register(&channel->caps, NULL);
 
@@ -1249,7 +2032,7 @@ static void ptp_clock_unregister_all(struct idtcm *idtcm)
 	u8 i;
 	struct idtcm_channel *channel;
 
-	for (i = 0; i < MAX_PHC_PLL; i++) {
+	for (i = 0; i < MAX_TOD; i++) {
 
 		channel = &idtcm->channel[i];
 
@@ -1260,7 +2043,12 @@ static void ptp_clock_unregister_all(struct idtcm *idtcm)
 
 static void set_default_masks(struct idtcm *idtcm)
 {
-	idtcm->pll_mask = DEFAULT_PLL_MASK;
+	idtcm->tod_mask = DEFAULT_TOD_MASK;
+
+	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;
@@ -1268,51 +2056,13 @@ static void set_default_masks(struct idtcm *idtcm)
 	idtcm->channel[3].output_mask = DEFAULT_OUTPUT_MASK_PLL3;
 }
 
-static int set_tod_write_overhead(struct idtcm *idtcm)
-{
-	int err;
-	u8 i;
-
-	s64 total_ns = 0;
-
-	ktime_t start;
-	ktime_t stop;
-
-	char buf[TOD_BYTE_COUNT];
-
-	struct idtcm_channel *channel = &idtcm->channel[2];
-
-	/* 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();
-
-		total_ns += ktime_to_ns(stop - start);
-	}
-
-	idtcm->tod_write_overhead_ns = div_s64(total_ns,
-					       TOD_WRITE_OVERHEAD_COUNT_MAX);
-
-	return err;
-}
-
 static int idtcm_probe(struct i2c_client *client,
 		       const struct i2c_device_id *id)
 {
 	struct idtcm *idtcm;
 	int err;
 	u8 i;
+	char *fmt = "Failed at %d in line %s with channel output %d!\n";
 
 	/* Unused for now */
 	(void)id;
@@ -1333,25 +2083,24 @@ static int idtcm_probe(struct i2c_client *client,
 
 	idtcm_display_version_info(idtcm);
 
-	err = set_tod_write_overhead(idtcm);
-
-	if (err) {
-		mutex_unlock(&idtcm->reg_lock);
-		return err;
-	}
-
 	err = idtcm_load_firmware(idtcm, &client->dev);
 
 	if (err)
 		dev_warn(&idtcm->client->dev,
 			 "loading firmware failed with %d\n", err);
 
-	if (idtcm->pll_mask) {
-		for (i = 0; i < MAX_PHC_PLL; i++) {
-			if (idtcm->pll_mask & (1 << i)) {
+	if (idtcm->tod_mask) {
+		for (i = 0; i < MAX_TOD; i++) {
+			if (idtcm->tod_mask & (1 << i)) {
 				err = idtcm_enable_channel(idtcm, i);
-				if (err)
+				if (err) {
+					dev_err(&idtcm->client->dev,
+						fmt,
+						__LINE__,
+						__func__,
+						i);
 					break;
+				}
 			}
 		}
 	} else {
diff --git a/drivers/ptp/ptp_clockmatrix.h b/drivers/ptp/ptp_clockmatrix.h
index 3de0eb72889c..ffae56c5d97f 100644
--- a/drivers/ptp/ptp_clockmatrix.h
+++ b/drivers/ptp/ptp_clockmatrix.h
@@ -13,32 +13,48 @@
 #include "idt8a340_reg.h"
 
 #define FW_FILENAME	"idtcm.bin"
-#define MAX_PHC_PLL	4
+#define MAX_TOD		(4)
+#define MAX_PLL		(8)
 
 #define MAX_ABS_WRITE_PHASE_PICOSECONDS (107374182350LL)
 
-#define PLL_MASK_ADDR		(0xFFA5)
-#define DEFAULT_PLL_MASK	(0x04)
+#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 OUTPUT_MASK_PLL0_ADDR		(0xFFB0)
-#define OUTPUT_MASK_PLL1_ADDR		(0xFFB2)
-#define OUTPUT_MASK_PLL2_ADDR		(0xFFB4)
-#define OUTPUT_MASK_PLL3_ADDR		(0xFFB6)
+#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 POST_SM_RESET_DELAY_MS		(3000)
 #define PHASE_PULL_IN_THRESHOLD_NS	(150000)
-#define TOD_WRITE_OVERHEAD_COUNT_MAX	(5)
+#define PHASE_PULL_IN_THRESHOLD_NS_V487	(15000)
+#define TOD_WRITE_OVERHEAD_COUNT_MAX	(2)
 #define TOD_BYTE_COUNT			(11)
 #define WR_PHASE_SETUP_MS		(5000)
 
+#define OUTPUT_MODULE_FROM_INDEX(index)	(OUTPUT_0 + (index) * 0x10)
+
+#define PEROUT_ENABLE_OUTPUT_MASK		(0xdeadbeef)
+
 /* Values of DPLL_N.DPLL_MODE.PLL_MODE */
 enum pll_mode {
 	PLL_MODE_MIN = 0,
@@ -48,7 +64,8 @@ enum pll_mode {
 	PLL_MODE_GPIO_INC_DEC = 3,
 	PLL_MODE_SYNTHESIS = 4,
 	PLL_MODE_PHASE_MEASUREMENT = 5,
-	PLL_MODE_MAX = PLL_MODE_PHASE_MEASUREMENT,
+	PLL_MODE_DISABLED = 6,
+	PLL_MODE_MAX = PLL_MODE_DISABLED,
 };
 
 enum hw_tod_write_trig_sel {
@@ -63,6 +80,26 @@ enum hw_tod_write_trig_sel {
 	WR_TRIG_SEL_MAX = HW_TOD_WR_TRIG_SEL_FOD_SYNC,
 };
 
+/* 4.8.7 only */
+enum scsr_tod_write_trig_sel {
+	SCSR_TOD_WR_TRIG_SEL_DISABLE = 0,
+	SCSR_TOD_WR_TRIG_SEL_IMMEDIATE = 1,
+	SCSR_TOD_WR_TRIG_SEL_REFCLK = 2,
+	SCSR_TOD_WR_TRIG_SEL_PWMPPS = 3,
+	SCSR_TOD_WR_TRIG_SEL_TODPPS = 4,
+	SCSR_TOD_WR_TRIG_SEL_SYNCFOD = 5,
+	SCSR_TOD_WR_TRIG_SEL_GPIO = 6,
+	SCSR_TOD_WR_TRIG_SEL_MAX = SCSR_TOD_WR_TRIG_SEL_GPIO,
+};
+
+/* 4.8.7 only */
+enum scsr_tod_write_type_sel {
+	SCSR_TOD_WR_TYPE_SEL_ABSOLUTE = 0,
+	SCSR_TOD_WR_TYPE_SEL_DELTA_PLUS = 1,
+	SCSR_TOD_WR_TYPE_SEL_DELTA_MINUS = 2,
+	SCSR_TOD_WR_TYPE_SEL_MAX = SCSR_TOD_WR_TYPE_SEL_DELTA_MINUS,
+};
+
 struct idtcm;
 
 struct idtcm_channel {
@@ -79,15 +116,17 @@ struct idtcm_channel {
 	u16			tod_n;
 	u16			hw_dpll_n;
 	enum pll_mode		pll_mode;
+	u8			pll;
 	u16			output_mask;
 	int			write_phase_ready;
 };
 
 struct idtcm {
-	struct idtcm_channel	channel[MAX_PHC_PLL];
+	struct idtcm_channel	channel[MAX_TOD];
 	struct i2c_client	*client;
 	u8			page_offset;
-	u8			pll_mask;
+	u8			tod_mask;
+	char			version[16];
 
 	/* Overhead calculation for adjtime */
 	u8			calculate_overhead_flag;