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

@@ -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
  *

@@ -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,
 };
 
 /*

@@ -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.