[PATCH net-next v8 10/15] dpll: sit9531x: add support to adjust output phase

[Ali Rouhi <arouhi@sitime.com>]

From: Oleg Zadorozhnyi <Oleg.Zadorozhnyi@devoxsoftware.com>

Shift an output in time against the others driven by the same PLL.  The
device has a coarse delay counted in VCO cycles and a three-bit fine field
in fixed thirty-picosecond steps, so a requested offset is split between
the two and what the core reads back is what the registers hold rather
than what was asked for.

Delay only ever advances, so an offset larger than one output period is
folded back into a single period -- for a periodic signal that is the same
phase.  The write takes effect in the programming state, which is left
with the loops re-locked even when a write inside it failed.

Signed-off-by: Oleg Zadorozhnyi <Oleg.Zadorozhnyi@devoxsoftware.com>
Assisted-by: Claude:claude-4-opus [chat]
Signed-off-by: Ali Rouhi <arouhi@sitime.com>
---
 drivers/dpll/sit9531x/core.c | 143 +++++++++++++++++++++++++++++++++++
 drivers/dpll/sit9531x/dpll.c |  51 +++++++++++++
 drivers/dpll/sit9531x/regs.h |  26 +++++++
 3 files changed, 220 insertions(+)

diff --git a/drivers/dpll/sit9531x/core.c b/drivers/dpll/sit9531x/core.c
index 0f886cfd1401..be033d7cfe7c 100644
--- a/drivers/dpll/sit9531x/core.c
+++ b/drivers/dpll/sit9531x/core.c
@@ -1342,6 +1342,149 @@ int sit9531x_output_freq_get(struct sit9531x_dev *sitdev, u8 out_idx,
  * output period, which is identical for a periodic signal.
  */
 
+int sit9531x_output_phase_adjust_set(struct sit9531x_dev *sitdev,
+				     u8 out_idx, s32 phase_ps)
+{
+	const struct sit9531x_chip_info *info = sitdev->info;
+	u64 abs_ps, fvco, coarse, coarse_ps, rem_ps;
+	u8 page, base, prog6_val, fine = 0;
+	u8 pll_idx, slot;
+	u32 freq;
+	int rc, ret;
+
+	lockdep_assert_held(&sitdev->multiop_lock);
+
+	if (out_idx >= info->num_outputs)
+		return -EINVAL;
+
+	pll_idx = sitdev->out[out_idx].pll_idx;
+	if (pll_idx >= SIT9531X_NUM_PLLS)
+		return -EINVAL;
+
+	freq = sitdev->out[out_idx].freq;
+	if (!freq)
+		return -EINVAL;
+
+	fvco = sit9531x_get_fvco(sitdev, pll_idx);
+	if (!fvco)
+		return -EIO;
+
+	/*
+	 * Convert to unsigned absolute delay.  Negative phase (advance)
+	 * is rendered as T_out - |phase|, modulo the output period.
+	 */
+	if (phase_ps == 0) {
+		abs_ps = 0;
+	} else if (phase_ps > 0) {
+		abs_ps = (u64)phase_ps;
+	} else {
+		u64 t_out_ps = div64_u64(1000000000000ULL, freq);
+		u64 advance = (u64)(-(s64)phase_ps);
+
+		if (t_out_ps == 0)
+			return -EINVAL;
+		/*
+		 * div64_u64_rem() rather than the % operator: a 64-bit
+		 * modulo has no compiler helper on 32-bit targets and
+		 * leaves the module with an undefined __umoddi3.
+		 */
+		div64_u64_rem(advance, t_out_ps, &advance);
+		abs_ps = (advance == 0) ? 0 : (t_out_ps - advance);
+	}
+
+	/*
+	 * coarse_cycles = abs_ps * Fvco / 1e12 ps/s.
+	 * mul_u64_u64_div_u64() avoids overflow when abs_ps approaches
+	 * one second of 1 PPS wrap-around.
+	 */
+	coarse = mul_u64_u64_div_u64(abs_ps, fvco, 1000000000000ULL);
+	if (coarse >= (1ULL << SIT9531X_OUT_PRG_COARSE_BITS))
+		return -ERANGE;
+
+	/* Fine delay = round((abs_ps - coarse * vco_period_ps) / 30 ps) */
+	coarse_ps = mul_u64_u64_div_u64(coarse, 1000000000000ULL, fvco);
+	rem_ps = (abs_ps > coarse_ps) ? (abs_ps - coarse_ps) : 0;
+	if (rem_ps) {
+		u64 steps;
+
+		steps = div64_u64(rem_ps + SIT9531X_OUT_PRG_FINE_STEP_PS / 2,
+				  SIT9531X_OUT_PRG_FINE_STEP_PS);
+		if (steps > SIT9531X_OUT_PRG_FINE_MAX)
+			steps = SIT9531X_OUT_PRG_FINE_MAX;
+		fine = (u8)steps;
+	}
+
+	/*
+	 * Map logical output index to the chip's physical output slot.
+	 * On SiT95317 the eight logical outputs land on chip slots
+	 * {0, 3, 4, 5, 7, 8, 9, 11}; on SiT95316 the map is identity.
+	 * Page/base must address the slot, not the logical index.
+	 */
+	slot = info->clkout_map[out_idx];
+	page = (slot > SIT9531X_PAGE_OUTSYS0_SLOT_MAX) ?
+	       SIT9531X_PAGE_OUTSYS1 : SIT9531X_PAGE_OUTSYS0;
+	base = SIT9531X_OUT_PRG_DELAY_BASE +
+	       SIT9531X_OUT_PRG_SLOT_STRIDE * (slot % 6);
+
+	/*
+	 * The PRG_RST_DELAY bytes live in the output system, so the writes
+	 * only take effect when made inside the PRG_CMD programming state and
+	 * committed to the NVM shadow, exactly like sit9531x_output_freq_set().
+	 */
+	rc = sit9531x_prg_enter(sitdev);
+	if (rc)
+		return rc;
+
+	/* PROG6 RMW: preserve OPSTG_VCASC_BUMP in [7:5] */
+	rc = sit9531x_read_u8(sitdev, SIT9531X_REG(page, base),
+			      &prog6_val);
+	if (rc)
+		goto commit;
+
+	prog6_val &= SIT9531X_OUT_PRG_OPSTG_MASK;
+	prog6_val |= (fine << SIT9531X_OUT_PRG_FINE_SHIFT) &
+		     SIT9531X_OUT_PRG_FINE_MASK;
+	prog6_val |= (u8)((coarse >> 32) & SIT9531X_OUT_PRG_COARSE_HI_MASK);
+
+	rc = sit9531x_write_u8(sitdev, SIT9531X_REG(page, base),
+			       prog6_val);
+	if (rc)
+		goto commit;
+	rc = sit9531x_write_u8(sitdev, SIT9531X_REG(page, base + 1),
+			       (u8)((coarse >> 24) & 0xFF));
+	if (rc)
+		goto commit;
+	rc = sit9531x_write_u8(sitdev, SIT9531X_REG(page, base + 2),
+			       (u8)((coarse >> 16) & 0xFF));
+	if (rc)
+		goto commit;
+	rc = sit9531x_write_u8(sitdev, SIT9531X_REG(page, base + 3),
+			       (u8)((coarse >> 8) & 0xFF));
+	if (rc)
+		goto commit;
+	rc = sit9531x_write_u8(sitdev, SIT9531X_REG(page, base + 4),
+			       (u8)(coarse & 0xFF));
+
+commit:
+	/*
+	 * Always leave the PRG_CMD state via prg_commit(), even on a
+	 * mid-sequence write failure, so the output loops are re-locked rather
+	 * than stranded unlocked; keep the first error.
+	 */
+	ret = sit9531x_prg_commit(sitdev);
+	if (ret && !rc)
+		rc = ret;
+	if (rc)
+		return rc;
+
+	/*
+	 * Restart the output divider phase so the freshly programmed delay is
+	 * applied against a known edge instead of the divider's arbitrary
+	 * running phase.
+	 */
+	return sit9531x_output_phase_flush(sitdev, pll_idx);
+}
+
 /*
  * sit9531x_clear_notifications - clear all notification registers
  *
diff --git a/drivers/dpll/sit9531x/dpll.c b/drivers/dpll/sit9531x/dpll.c
index 6365a83e4c52..c07fb2256510 100644
--- a/drivers/dpll/sit9531x/dpll.c
+++ b/drivers/dpll/sit9531x/dpll.c
@@ -736,12 +736,63 @@ sit9531x_dpll_output_pin_state_on_dpll_set(const struct dpll_pin *pin,
 	return rc;
 }
 
+/*
+ * sit9531x_dpll_output_pin_phase_adjust_get - read output phase adjustment
+ *
+ * returns cached value.
+ */
+static int
+sit9531x_dpll_output_pin_phase_adjust_get(const struct dpll_pin *pin,
+					  void *pin_priv,
+					  const struct dpll_device *dpll,
+					  void *dpll_priv, s32 *phase_adjust,
+					  struct netlink_ext_ack *extack)
+{
+	struct sit9531x_dpll_pin *dpin = pin_priv;
+
+	*phase_adjust = dpin->phase_adjust;
+	return 0;
+}
+
+/*
+ * sit9531x_dpll_output_pin_phase_adjust_set - set output phase adjustment
+ *
+ * Programs the per-output PRG_RST_DELAY registers for deterministic
+ * phase offset; see sit9531x_output_phase_adjust_set() in core.c.
+ */
+static int
+sit9531x_dpll_output_pin_phase_adjust_set(const struct dpll_pin *pin,
+					  void *pin_priv,
+					  const struct dpll_device *dpll,
+					  void *dpll_priv, s32 phase_adjust,
+					  struct netlink_ext_ack *extack)
+{
+	struct sit9531x_dpll_pin *dpin = pin_priv;
+	struct sit9531x_dpll *sitdpll = dpll_priv;
+	struct sit9531x_dev *sitdev = sitdpll->dev;
+	int rc;
+
+	mutex_lock(&sitdev->multiop_lock);
+	rc = sit9531x_output_phase_adjust_set(sitdev, dpin->id, phase_adjust);
+	mutex_unlock(&sitdev->multiop_lock);
+
+	if (rc) {
+		NL_SET_ERR_MSG(extack, "Phase adjust failed");
+		return rc;
+	}
+
+	dpin->phase_adjust = phase_adjust;
+	return 0;
+}
+
 static const struct dpll_pin_ops sit9531x_dpll_output_pin_ops = {
 	.direction_get		= sit9531x_dpll_output_pin_direction_get,
 	.frequency_get		= sit9531x_dpll_output_pin_frequency_get,
 	.frequency_set		= sit9531x_dpll_output_pin_frequency_set,
 	.state_on_dpll_get	= sit9531x_dpll_output_pin_state_on_dpll_get,
 	.state_on_dpll_set	= sit9531x_dpll_output_pin_state_on_dpll_set,
+	.phase_adjust_get	= sit9531x_dpll_output_pin_phase_adjust_get,
+	.phase_adjust_set	= sit9531x_dpll_output_pin_phase_adjust_set,
 };
 
 const struct dpll_pin_ops *
diff --git a/drivers/dpll/sit9531x/regs.h b/drivers/dpll/sit9531x/regs.h
index 031c1a460d4a..91d33abd15c3 100644
--- a/drivers/dpll/sit9531x/regs.h
+++ b/drivers/dpll/sit9531x/regs.h
@@ -189,6 +189,32 @@
 #define SIT9531X_REG_OUTSYS_DEBUG		SIT9531X_REG(0x03, 0xBD)
 #define SIT9531X_DEBUG_UNLOCK_VAL		0xC3
 
+/*
+ * Per-output programmable phase delay: 34-bit coarse (in VCO clock
+ * cycles) plus a 3-bit fine field with fixed 30 ps steps.  Each output
+ * has a five-byte block PROG6..PROG2:
+ *
+ *   base + 0  PROG6  [7:5] OPSTG_VCASC_BUMP (preserve via RMW)
+ *                    [4:2] PRG_RST_FINE_DELAY[2:0]
+ *                    [1:0] PRG_RST_DELAY[33:32]
+ *   base + 1  PROG5  [7:0] PRG_RST_DELAY[31:24]
+ *   base + 2  PROG4  [7:0] PRG_RST_DELAY[23:16]
+ *   base + 3  PROG3  [7:0] PRG_RST_DELAY[15:8]
+ *   base + 4  PROG2  [7:0] PRG_RST_DELAY[7:0]
+ *
+ * Outputs 0-5 are on Page 3, outputs 6-11 on Page 4.  The block base
+ * within a page is 0x15 + 16 * (out_idx % 6).
+ */
+#define SIT9531X_OUT_PRG_DELAY_BASE		0x15
+#define SIT9531X_OUT_PRG_SLOT_STRIDE		0x10
+#define SIT9531X_OUT_PRG_OPSTG_MASK		0xE0	/* bits [7:5], preserve */
+#define SIT9531X_OUT_PRG_FINE_SHIFT		2
+#define SIT9531X_OUT_PRG_FINE_MASK		0x1C	/* bits [4:2] */
+#define SIT9531X_OUT_PRG_COARSE_HI_MASK		0x03	/* bits [1:0] */
+#define SIT9531X_OUT_PRG_FINE_STEP_PS		30
+#define SIT9531X_OUT_PRG_FINE_MAX		7	/* 3-bit field */
+#define SIT9531X_OUT_PRG_COARSE_BITS		34
+
 /*
  * On-demand phase-flush fired from a register rather than a GPIO pin.
  * DIVO_PHASE_SEL_REG selects the in-register trigger source and
-- 
2.43.0