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

[Ali Rouhi]

From: Oleg Zadorozhnyi <Oleg.Zadorozhnyi@xxxxxxxxxxxxxxxxx>

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.

Signed-off-by: Oleg Zadorozhnyi <Oleg.Zadorozhnyi@xxxxxxxxxxxxxxxxx>
Assisted-by: Claude:claude-4-opus [chat]
Signed-off-by: Ali Rouhi <arouhi@xxxxxxxxxx>
---
 drivers/dpll/sit9531x/core.c | 151 +++++++++++++++++++++++++++++++++++
 drivers/dpll/sit9531x/dpll.c |  47 +++++++++++
 drivers/dpll/sit9531x/regs.h |  12 +++
 3 files changed, 210 insertions(+)

diff --git a/drivers/dpll/sit9531x/core.c b/drivers/dpll/sit9531x/core.c
index f552a9c73796..bd251ab60eee 100644
--- a/drivers/dpll/sit9531x/core.c
+++ b/drivers/dpll/sit9531x/core.c
@@ -931,6 +931,61 @@ 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, 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;
+
+	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 vendor 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
@@ -998,6 +1053,102 @@ 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.
+ *
+ * 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,
+				 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, 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;
+
+	*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;
+	if (configured <= 0)
+		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 f813126077e1..1a14255e89a8 100644
--- a/drivers/dpll/sit9531x/dpll.c
+++ b/drivers/dpll/sit9531x/dpll.c
@@ -528,6 +528,52 @@ sit9531x_dpll_input_pin_prio_set(const struct dpll_pin *pin, void *pin_priv,
 	return 0;
 }
 
+/*
+ * sit9531x_dpll_input_pin_phase_offset_get - read phase offset
+ *
+ * reads the TDC (Time-to-Digital Converter) hardware
+ * to measure the phase difference in picoseconds via
+ * sit9531x_phase_offset_read().
+ */
+/*
+ * 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;
+	int rc;
+
+	mutex_lock(&sitdev->multiop_lock);
+
+	/*
+	 * The periodic worker updates selected_ref under the same lock, so
+	 * test it here rather than before taking it: otherwise the
+	 * reference can change in between and the measurement gets
+	 * attributed to the wrong pin.
+	 */
+	if (sitdev->chan[sitdpll->id].selected_ref != dpin->id) {
+		mutex_unlock(&sitdev->multiop_lock);
+		return -ENODATA;
+	}
+
+	rc = sit9531x_pll_ffo_ppt(sitdev, sitdpll->id, &ffo->ffo);
+	mutex_unlock(&sitdev->multiop_lock);
+
+	return rc;
+}
+
 /*
  * sit9531x_dpll_input_pin_phase_offset_get - phase offset of a reference
  *
@@ -641,6 +687,7 @@ static const struct dpll_pin_ops sit9531x_dpll_input_pin_ops = {
 	 * 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 aa60be243e6d..98425c04d3d0 100644
--- a/drivers/dpll/sit9531x/regs.h
+++ b/drivers/dpll/sit9531x/regs.h
@@ -292,6 +292,18 @@
 #define SIT9531X_DBG_READ_CODE_TDC		69
 #define SIT9531X_TDC_SIGN_BIT		3
 
+/*
+ * 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.43.0