[PATCH net-next v2 3/3] net: ethernet: ti: am65-cpts: adjust pps following ptp changes

[Siddharth Vadapalli]

From: Grygorii Strashko <grygorii.strashko@xxxxxx>

When CPTS clock is sync/adjusted by running linuxptp (ptp4l) it will cause
PPS jitter as Genf running PPS is not adjusted.

The same PPM adjustment has to be applied to GenF as to PHC clock to
correct PPS length and keep them in sync.

Testing:
 Master:
  ptp4l -P -2 -H -i eth0 -l 6 -m -q -p /dev/ptp1 -f ptp.cfg &
  testptp -d /dev/ptp1 -P 1
  ppstest /dev/pps0

 Slave:
  linuxptp/ptp4l -P -2 -H -i eth0 -l 6 -m -q -p /dev/ptp1 -f ptp1.cfg -s &
    <port 1: UNCALIBRATED to SLAVE on MASTER_CLOCK_SELECTED;>
  testptp -d /dev/ptp1 -P 1
  ppstest /dev/pps0

Master log:
source 0 - assert 620.000000689, sequence: 530
source 0 - assert 621.000000689, sequence: 531
source 0 - assert 622.000000689, sequence: 532
source 0 - assert 623.000000689, sequence: 533
source 0 - assert 624.000000689, sequence: 534
source 0 - assert 625.000000689, sequence: 535
source 0 - assert 626.000000689, sequence: 536
source 0 - assert 627.000000689, sequence: 537
source 0 - assert 628.000000689, sequence: 538
source 0 - assert 629.000000689, sequence: 539
source 0 - assert 630.000000689, sequence: 540
source 0 - assert 631.000000689, sequence: 541
source 0 - assert 632.000000689, sequence: 542
source 0 - assert 633.000000689, sequence: 543
source 0 - assert 634.000000689, sequence: 544
source 0 - assert 635.000000689, sequence: 545

Slave log:
source 0 - assert 620.000000706, sequence: 252
source 0 - assert 621.000000709, sequence: 253
source 0 - assert 622.000000707, sequence: 254
source 0 - assert 623.000000707, sequence: 255
source 0 - assert 624.000000706, sequence: 256
source 0 - assert 625.000000705, sequence: 257
source 0 - assert 626.000000709, sequence: 258
source 0 - assert 627.000000709, sequence: 259
source 0 - assert 628.000000707, sequence: 260
source 0 - assert 629.000000706, sequence: 261
source 0 - assert 630.000000710, sequence: 262
source 0 - assert 631.000000708, sequence: 263
source 0 - assert 632.000000705, sequence: 264
source 0 - assert 633.000000710, sequence: 265
source 0 - assert 634.000000708, sequence: 266
source 0 - assert 635.000000707, sequence: 267

Signed-off-by: Grygorii Strashko <grygorii.strashko@xxxxxx>
Signed-off-by: Siddharth Vadapalli <s-vadapalli@xxxxxx>
---
 drivers/net/ethernet/ti/am65-cpts.c | 59 ++++++++++++++++++++++++-----
 1 file changed, 49 insertions(+), 10 deletions(-)

diff --git a/drivers/net/ethernet/ti/am65-cpts.c b/drivers/net/ethernet/ti/am65-cpts.c
index c405af266b41..bf0f74b20ba6 100644
--- a/drivers/net/ethernet/ti/am65-cpts.c
+++ b/drivers/net/ethernet/ti/am65-cpts.c
@@ -405,10 +405,13 @@ static irqreturn_t am65_cpts_interrupt(int irq, void *dev_id)
 static int am65_cpts_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 {
 	struct am65_cpts *cpts = container_of(ptp, struct am65_cpts, ptp_info);
+	u32 pps_ctrl_val = 0, pps_ppm_hi = 0, pps_ppm_low = 0;
 	s32 ppb = scaled_ppm_to_ppb(scaled_ppm);
+	int pps_index = cpts->pps_genf_idx;
+	u64 adj_period, pps_adj_period;
+	u32 ctrl_val, ppm_hi, ppm_low;
+	unsigned long flags;
 	int neg_adj = 0;
-	u64 adj_period;
-	u32 val;
 
 	if (ppb < 0) {
 		neg_adj = 1;
@@ -428,17 +431,53 @@ static int am65_cpts_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 
 	mutex_lock(&cpts->ptp_clk_lock);
 
-	val = am65_cpts_read32(cpts, control);
+	ctrl_val = am65_cpts_read32(cpts, control);
 	if (neg_adj)
-		val |= AM65_CPTS_CONTROL_TS_PPM_DIR;
+		ctrl_val |= AM65_CPTS_CONTROL_TS_PPM_DIR;
 	else
-		val &= ~AM65_CPTS_CONTROL_TS_PPM_DIR;
-	am65_cpts_write32(cpts, val, control);
+		ctrl_val &= ~AM65_CPTS_CONTROL_TS_PPM_DIR;
+
+	ppm_hi = upper_32_bits(adj_period) & 0x3FF;
+	ppm_low = lower_32_bits(adj_period);
+
+	if (cpts->pps_enabled) {
+		pps_ctrl_val = am65_cpts_read32(cpts, genf[pps_index].control);
+		if (neg_adj)
+			pps_ctrl_val &= ~BIT(1);
+		else
+			pps_ctrl_val |= BIT(1);
+
+		/* GenF PPM will do correction using cpts refclk tick which is
+		 * (cpts->ts_add_val + 1) ns, so GenF length PPM adj period
+		 * need to be corrected.
+		 */
+		pps_adj_period = adj_period * (cpts->ts_add_val + 1);
+		pps_ppm_hi = upper_32_bits(pps_adj_period) & 0x3FF;
+		pps_ppm_low = lower_32_bits(pps_adj_period);
+	}
+
+	spin_lock_irqsave(&cpts->lock, flags);
 
-	val = upper_32_bits(adj_period) & 0x3FF;
-	am65_cpts_write32(cpts, val, ts_ppm_hi);
-	val = lower_32_bits(adj_period);
-	am65_cpts_write32(cpts, val, ts_ppm_low);
+	/* All below writes must be done extremely fast:
+	 *  - delay between PPM dir and PPM value changes can cause err due old
+	 *    PPM correction applied in wrong direction
+	 *  - delay between CPTS-clock PPM cfg and GenF PPM cfg can cause err
+	 *    due CPTS-clock PPM working with new cfg while GenF PPM cfg still
+	 *    with old for short period of time
+	 */
+
+	am65_cpts_write32(cpts, ctrl_val, control);
+	am65_cpts_write32(cpts, ppm_hi, ts_ppm_hi);
+	am65_cpts_write32(cpts, ppm_low, ts_ppm_low);
+
+	if (cpts->pps_enabled) {
+		am65_cpts_write32(cpts, pps_ctrl_val, genf[pps_index].control);
+		am65_cpts_write32(cpts, pps_ppm_hi, genf[pps_index].ppm_hi);
+		am65_cpts_write32(cpts, pps_ppm_low, genf[pps_index].ppm_low);
+	}
+
+	/* All GenF/EstF can be updated here the same way */
+	spin_unlock_irqrestore(&cpts->lock, flags);
 
 	mutex_unlock(&cpts->ptp_clk_lock);
 
-- 
2.25.1