[PATCH net-next v9 13/15] dpll: sit9531x: add support to get fractional frequency offset

[Ali Rouhi <arouhi@sitime.com>]

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

Report how far a PLL's reference is from nominal, as a fraction of the
nominal rate.

The device does not measure that directly, but it does run a feedback
divider that the loop adjusts to keep the reference in step: the
difference between the divider the loop is running and the divider the
configuration asked for is exactly the offset of the reference.  Both are
read from the chip -- the configured one from its registers, the running
one through the debug window -- and the offset falls out of the ratio.

The running value comes from the same latched debug window as the phase
offset, so it is triggered three times per sample for the same reason.

The running divider is two numbers read through a window that exposes one
tap at a time, so the numerator and the denominator come from separate
samples and a loop moving between them can tear the pair.  On a locked
loop the divider moves by parts per trillion between two register reads,
which is below what this measurement resolves; the part offers no way to
latch both at once.

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 | 194 +++++++++++++++++++++++++++++++++++
 drivers/dpll/sit9531x/dpll.c |  51 +++++++++
 drivers/dpll/sit9531x/regs.h |  12 +++
 3 files changed, 257 insertions(+)

diff --git a/drivers/dpll/sit9531x/core.c b/drivers/dpll/sit9531x/core.c
index 356d1eac4ec5..88bfa63b2975 100644
--- a/drivers/dpll/sit9531x/core.c
+++ b/drivers/dpll/sit9531x/core.c
@@ -1224,6 +1224,76 @@ static int sit9531x_is_xo_doubler_enabled(struct sit9531x_dev *sitdev)
 	return (~val >> SIT9531X_XO_DOUBLER_ENB_BIT) & 1u;
 }
 
+/*
+ * sit9531x_dbg_sample - latch and read a signal pathway debug sample
+ * @sitdev:	device pointer
+ * @pll_idx:	PLL index (0-3)
+ * @read_code:	which tap of the pathway to sample
+ * @buf:	result, least significant byte first
+ * @len:	bytes to read, at most SIT9531X_DBG_DATA_BYTES
+ *
+ * Return: 0 on success, <0 on error
+ */
+static int sit9531x_dbg_sample(struct sit9531x_dev *sitdev, u8 pll_idx,
+			       u8 read_code, bool low_freq_clk,
+			       u8 *buf, unsigned int len)
+{
+	unsigned int i;
+	int rc;
+	u8 v;
+
+	if (len > SIT9531X_DBG_DATA_BYTES)
+		return -EINVAL;
+
+	rc = sit9531x_write_pll_u8(sitdev, pll_idx, SIT9531X_PLL_REG_DEBUG,
+				   SIT9531X_PLL_DEBUG_UNLOCK);
+	if (rc)
+		return rc;
+
+	/*
+	 * Select the debug clock this tap needs.  The device never clears
+	 * the bit, so a read that left it to whatever the previous one set
+	 * would depend on the order the taps happened to be read in.  Taps
+	 * below 200 kHz need the slow clock; the divider taps do not.
+	 */
+	rc = sit9531x_update_pll_u8(sitdev, pll_idx,
+				    SIT9531X_PLL_REG_DBG_WRITE_CODE,
+				    SIT9531X_DBG_LOW_FREQ_CLK_BIT,
+				    low_freq_clk ?
+				    SIT9531X_DBG_LOW_FREQ_CLK_BIT : 0);
+	if (rc)
+		return rc;
+
+	rc = sit9531x_write_pll_u8(sitdev, pll_idx,
+				   SIT9531X_PLL_REG_DBG_READ_CODE, read_code);
+	if (rc)
+		return rc;
+
+	/*
+	 * Reading the trigger latches a sample of the selected tap.  Read it
+	 * three times, as the documented phase-difference procedure does and as
+	 * sit9531x_phase_offset_read() already did: a single read returns
+	 * the previous latch, so a caller sampling repeatedly gets the same
+	 * value back however much the tap has moved.
+	 */
+	for (i = 0; i < SIT9531X_DBG_LATCH_READS; i++) {
+		rc = sit9531x_read_pll_u8(sitdev, pll_idx,
+					  SIT9531X_PLL_REG_DBG_TRIGGER, &v);
+		if (rc)
+			return rc;
+	}
+
+	for (i = 0; i < len; i++) {
+		rc = sit9531x_read_pll_u8(sitdev, pll_idx,
+					  SIT9531X_PLL_REG_DBG_DATA_0 + i,
+					  &buf[i]);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
 /*
  * DIVN as a fixed-point value: int_part plus fracn/fracd, carried with
  * SIT9531X_DIVN_SCALE steps per unit.  The scale keeps a whole DIVN
@@ -1291,6 +1361,130 @@ static int sit9531x_divn_static(struct sit9531x_dev *sitdev, u8 pll_idx,
 	return 0;
 }
 
+/*
+ * sit9531x_divn_runtime - read the DIVN the digital loop is commanding
+ * @sitdev:	device pointer
+ * @pll_idx:	PLL index (0-3)
+ * @divn:	result, fixed point as per sit9531x_divn_fixed()
+ *
+ * Same quantity as sit9531x_divn_static(), but sampled from the running
+ * loop rather than from the configuration registers, and carried at a
+ * wider precision: the numerator is 48 bits, two's complement, the
+ * denominator 49.  The integer part shares its tap with the numerator.
+ *
+ * The denominator is taken as it reads.  The configuration register holds
+ * the divisor minus one, and correcting for that reproduces a profile's
+ * stated VCO exactly, to the last bit of the fraction; this tap is a wider
+ * field sampled from the loop itself and the documented readback uses it as it
+ * comes, so the bias is not applied here.
+ *
+ * The numerator and the denominator come from two separate latches:
+ * the debug window exposes one tap at a time, so the pair can be torn
+ * by a loop that moves between them.  The divider changes by parts per
+ * trillion between samples on a locked loop, which is below what this
+ * measurement resolves.
+ *
+ * Return: 0 on success, <0 on error
+ */
+static int sit9531x_divn_runtime(struct sit9531x_dev *sitdev, u8 pll_idx,
+				 s64 *divn)
+{
+	u8 buf[SIT9531X_DBG_DATA_BYTES];
+	u64 fracn_raw = 0, fracd = 0;
+	u32 int_part;
+	int rc, i;
+
+	rc = sit9531x_dbg_sample(sitdev, pll_idx, SIT9531X_DBG_READ_CODE_DIVN,
+				 false, buf, SIT9531X_DBG_DATA_BYTES);
+	if (rc)
+		return rc;
+
+	for (i = 5; i >= 0; i--)
+		fracn_raw = (fracn_raw << 8) | buf[i];
+
+	int_part = buf[6] | ((u32)(buf[7] & SIT9531X_DIVN_RT_INT_HI_BIT) << 8);
+
+	rc = sit9531x_dbg_sample(sitdev, pll_idx,
+				 SIT9531X_DBG_READ_CODE_DIVN_DEN, false, buf,
+				 SIT9531X_DBG_DATA_BYTES);
+	if (rc)
+		return rc;
+
+	for (i = 5; i >= 0; i--)
+		fracd = (fracd << 8) | buf[i];
+
+	fracd |= (u64)(buf[6] & SIT9531X_DIVN_RT_INT_HI_BIT) << 48;
+
+	/*
+	 * sit9531x_divn_fixed() drops the fractional part when the
+	 * denominator is zero.  That is right for a configuration register
+	 * that was never programmed, but a running loop reading zero means
+	 * the sample is unusable, and returning the whole part alone would
+	 * put a wrong DIVN into the frequency offset without saying so.
+	 */
+	if (!fracd)
+		return -ENODATA;
+
+	*divn = sit9531x_divn_fixed(int_part,
+				    sign_extend64(fracn_raw,
+						  SIT9531X_DIVN_RT_NUM_BITS - 1),
+				    fracd);
+
+	return 0;
+}
+
+/**
+ * sit9531x_pll_ffo_ppt - fractional frequency offset of a PLL's reference
+ * @sitdev:	device pointer
+ * @pll_idx:	PLL index (0-3)
+ * @ffo:	result in parts per trillion
+ *
+ * A locked PLL commands whatever DIVN keeps its VCO tracking the
+ * reference.  How far that sits from the configured DIVN is how far the
+ * reference sits from the local oscillator, which is the fractional
+ * frequency offset the DPLL ABI reports for the pin feeding the device.
+ *
+ * Caller must hold sitdev->multiop_lock.
+ *
+ * Return: 0 on success, -ENODATA when DIVN is not programmed, <0 on
+ * error.
+ */
+int sit9531x_pll_ffo_ppt(struct sit9531x_dev *sitdev, u8 pll_idx, s64 *ffo)
+{
+	s64 configured, running, delta;
+	u64 magnitude;
+	int rc;
+
+	lockdep_assert_held(&sitdev->multiop_lock);
+
+	if (pll_idx >= SIT9531X_NUM_PLLS)
+		return -EINVAL;
+
+	rc = sit9531x_divn_static(sitdev, pll_idx, &configured);
+	if (rc)
+		return rc;
+	/*
+	 * Scaling by a divider far below one whole cycle overflows the
+	 * 64-bit product and comes back as ~0, which would be reported
+	 * as a plausible offset.  A DIVN that small is not a running
+	 * loop, so treat it as no measurement.
+	 */
+	if (configured < SIT9531X_DIVN_SCALE)
+		return -ENODATA;
+
+	rc = sit9531x_divn_runtime(sitdev, pll_idx, &running);
+	if (rc)
+		return rc;
+
+	delta = running - configured;
+	magnitude = mul_u64_u64_div_u64(abs(delta), SIT9531X_PPT_PER_UNIT,
+					(u64)configured);
+
+	*ffo = delta < 0 ? -(s64)magnitude : (s64)magnitude;
+
+	return 0;
+}
+
 /*
  * sit9531x_get_fvco - read VCO frequency from chip's DIVN registers
  *
diff --git a/drivers/dpll/sit9531x/dpll.c b/drivers/dpll/sit9531x/dpll.c
index 1dfd83b635e2..2a973364a9cd 100644
--- a/drivers/dpll/sit9531x/dpll.c
+++ b/drivers/dpll/sit9531x/dpll.c
@@ -603,6 +603,50 @@ sit9531x_dpll_input_pin_prio_set(const struct dpll_pin *pin, void *pin_priv,
 	return 0;
 }
 
+/*
+ * sit9531x_dpll_input_pin_ffo_get - read the input's frequency offset
+ *
+ * The offset is derived from how far the PLL's running DIVN sits from
+ * its configured one, which only says something about the reference the
+ * PLL is actually tracking.  For every other input there is no
+ * measurement, and -ENODATA leaves the attribute out rather than
+ * reporting the active reference's figure against the wrong pin.
+ */
+static int
+sit9531x_dpll_input_pin_ffo_get(const struct dpll_pin *pin, void *pin_priv,
+				const struct dpll_device *dpll, void *dpll_priv,
+				struct dpll_ffo_param *ffo,
+				struct netlink_ext_ack *extack)
+{
+	struct sit9531x_dpll_pin *dpin = pin_priv;
+	struct sit9531x_dpll *sitdpll = dpll_priv;
+	struct sit9531x_dev *sitdev = sitdpll->dev;
+	enum dpll_pin_state state;
+	int rc;
+
+	mutex_lock(&sitdev->multiop_lock);
+
+	/*
+	 * Publish FFO only for the input the DPLL is actively tracking.
+	 * selected_ref alone is not enough (free-run, LOL, holdover), so use
+	 * the same CONNECTED criterion as the generic selection-state logic.
+	 */
+	sit9531x_dpll_selection_state_get(sitdev, sitdpll, dpin->id, &state);
+	if (state != DPLL_PIN_STATE_CONNECTED) {
+		mutex_unlock(&sitdev->multiop_lock);
+		return -ENODATA;
+	}
+
+	rc = sit9531x_pll_ffo_ppt(sitdev, sitdpll->id, &ffo->ffo);
+	mutex_unlock(&sitdev->multiop_lock);
+
+	if (rc && rc != -ENODATA)
+		NL_SET_ERR_MSG(extack,
+			       "Failed to measure the frequency offset of the selected reference");
+
+	return rc;
+}
+
 /*
  * sit9531x_dpll_input_pin_phase_offset_get - phase offset of a reference
  *
@@ -712,6 +756,13 @@ static const struct dpll_pin_ops sit9531x_dpll_input_pin_ops = {
 	.prio_get		= sit9531x_dpll_input_pin_prio_get,
 	.prio_set		= sit9531x_dpll_input_pin_prio_set,
 	.phase_offset_get	= sit9531x_dpll_input_pin_phase_offset_get,
+	/*
+	 * The measurement compares the PLL's running feedback divider with
+	 * its configured one, so it describes the device's own reference
+	 * rather than a port rate.
+	 */
+	.supported_ffo		= BIT(DPLL_FFO_PIN_DEVICE),
+	.ffo_get		= sit9531x_dpll_input_pin_ffo_get,
 };
 
 /*
diff --git a/drivers/dpll/sit9531x/regs.h b/drivers/dpll/sit9531x/regs.h
index 08eae8a2c279..78e8a47e96ab 100644
--- a/drivers/dpll/sit9531x/regs.h
+++ b/drivers/dpll/sit9531x/regs.h
@@ -309,6 +309,18 @@
 #define SIT9531X_TDC_SIGN_BIT		3
 #define SIT9531X_TDC_MAG_HI_MASK	GENMASK(2, 0)
 
+/*
+ * Read codes of the running DIVN taps.  Unlike the configuration
+ * registers these report what the digital loop currently commands, so
+ * they carry the correction the loop applies to track its reference.
+ * The integer part and the numerator share one tap, the denominator
+ * has its own.
+ */
+#define SIT9531X_DBG_READ_CODE_DIVN		0x57
+#define SIT9531X_DBG_READ_CODE_DIVN_DEN	0x56
+#define SIT9531X_DIVN_RT_NUM_BITS		48
+#define SIT9531X_DIVN_RT_INT_HI_BIT		BIT(0)
+
 /*
  * DIVN carried as fixed point, and the unit the DPLL ABI wants the
  * fractional frequency offset in.  Equal in value, distinct in meaning.
-- 
2.39.2 (Apple Git-143)