dpll: zl3073x: Add support to get phase offset on connected input pin

Add support to get phase offset for the connected input pin. Implement
the appropriate callback and function that performs DPLL to connected
reference phase error measurement and notifies DPLL core about changes.

The measurement is performed internally by device on background 40 times
per second but the measured value is read each second and compared with
previous value.

Reviewed-by: Jiri Pirko <[email protected]>
Tested-by: Prathosh Satish <[email protected]>
Co-developed-by: Prathosh Satish <[email protected]>
Signed-off-by: Prathosh Satish <[email protected]>
Signed-off-by: Ivan Vecera <[email protected]>
Link: https://patch.msgid.link/[email protected]
Signed-off-by: Paolo Abeni <[email protected]>

Author: ivecera

drivers/dpll/zl3073x/core.c

@@ -669,12 +669,52 @@ zl3073x_dev_state_fetch(struct zl3073x_dev *zldev)
 	return rc;
 }
 
+/**
+ * zl3073x_ref_phase_offsets_update - update reference phase offsets
+ * @zldev: pointer to zl3073x_dev structure
+ *
+ * Ask device to update phase offsets latch registers with the latest
+ * measured values.
+ *
+ * Return: 0 on success, <0 on error
+ */
+static int
+zl3073x_ref_phase_offsets_update(struct zl3073x_dev *zldev)
+{
+	int rc;
+
+	/* Per datasheet we have to wait for 'dpll_ref_phase_err_rqst_rd'
+	 * to be zero to ensure that the measured data are coherent.
+	 */
+	rc = zl3073x_poll_zero_u8(zldev, ZL_REG_REF_PHASE_ERR_READ_RQST,
+				  ZL_REF_PHASE_ERR_READ_RQST_RD);
+	if (rc)
+		return rc;
+
+	/* Request to update phase offsets measurement values */
+	rc = zl3073x_write_u8(zldev, ZL_REG_REF_PHASE_ERR_READ_RQST,
+			      ZL_REF_PHASE_ERR_READ_RQST_RD);
+	if (rc)
+		return rc;
+
+	/* Wait for finish */
+	return zl3073x_poll_zero_u8(zldev, ZL_REG_REF_PHASE_ERR_READ_RQST,
+				    ZL_REF_PHASE_ERR_READ_RQST_RD);
+}
+
 static void
 zl3073x_dev_periodic_work(struct kthread_work *work)
 {
 	struct zl3073x_dev *zldev = container_of(work, struct zl3073x_dev,
 						 work.work);
 	struct zl3073x_dpll *zldpll;
+	int rc;
+
+	/* Update DPLL-to-connected-ref phase offsets registers */
+	rc = zl3073x_ref_phase_offsets_update(zldev);
+	if (rc)
+		dev_warn(zldev->dev, "Failed to update phase offsets: %pe\n",
+			 ERR_PTR(rc));
 
 	list_for_each_entry(zldpll, &zldev->dplls, list)
 		zl3073x_dpll_changes_check(zldpll);
@@ -767,6 +807,46 @@ zl3073x_devm_dpll_init(struct zl3073x_dev *zldev, u8 num_dplls)
 	return rc;
 }
 
+/**
+ * zl3073x_dev_phase_meas_setup - setup phase offset measurement
+ * @zldev: pointer to zl3073x_dev structure
+ * @num_channels: number of DPLL channels
+ *
+ * Enable phase offset measurement block, set measurement averaging factor
+ * and enable DPLL-to-its-ref phase measurement for all DPLLs.
+ *
+ * Returns: 0 on success, <0 on error
+ */
+static int
+zl3073x_dev_phase_meas_setup(struct zl3073x_dev *zldev, int num_channels)
+{
+	u8 dpll_meas_ctrl, mask;
+	int i, rc;
+
+	/* Read DPLL phase measurement control register */
+	rc = zl3073x_read_u8(zldev, ZL_REG_DPLL_MEAS_CTRL, &dpll_meas_ctrl);
+	if (rc)
+		return rc;
+
+	/* Setup phase measurement averaging factor */
+	dpll_meas_ctrl &= ~ZL_DPLL_MEAS_CTRL_AVG_FACTOR;
+	dpll_meas_ctrl |= FIELD_PREP(ZL_DPLL_MEAS_CTRL_AVG_FACTOR, 3);
+
+	/* Enable DPLL measurement block */
+	dpll_meas_ctrl |= ZL_DPLL_MEAS_CTRL_EN;
+
+	/* Update phase measurement control register */
+	rc = zl3073x_write_u8(zldev, ZL_REG_DPLL_MEAS_CTRL, dpll_meas_ctrl);
+	if (rc)
+		return rc;
+
+	/* Enable DPLL-to-connected-ref measurement for each channel */
+	for (i = 0, mask = 0; i < num_channels; i++)
+		mask |= BIT(i);
+
+	return zl3073x_write_u8(zldev, ZL_REG_DPLL_PHASE_ERR_READ_MASK, mask);
+}
+
 /**
  * zl3073x_dev_probe - initialize zl3073x device
  * @zldev: pointer to zl3073x device
@@ -839,6 +919,12 @@ int zl3073x_dev_probe(struct zl3073x_dev *zldev,
 	if (rc)
 		return rc;
 
+	/* Setup phase offset measurement block */
+	rc = zl3073x_dev_phase_meas_setup(zldev, chip_info->num_channels);
+	if (rc)
+		return dev_err_probe(zldev->dev, rc,
+				     "Failed to setup phase measurement\n");
+
 	/* Register DPLL channels */
 	rc = zl3073x_devm_dpll_init(zldev, chip_info->num_channels);
 	if (rc)

drivers/dpll/zl3073x/dpll.c

@@ -36,6 +36,7 @@
  * @selectable: pin is selectable in automatic mode
  * @esync_control: embedded sync is controllable
  * @pin_state: last saved pin state
+ * @phase_offset: last saved pin phase offset
  */
 struct zl3073x_dpll_pin {
 	struct list_head	list;
@@ -48,6 +49,7 @@ struct zl3073x_dpll_pin {
 	bool			selectable;
 	bool			esync_control;
 	enum dpll_pin_state	pin_state;
+	s64			phase_offset;
 };
 
 /*
@@ -496,6 +498,50 @@ zl3073x_dpll_connected_ref_get(struct zl3073x_dpll *zldpll, u8 *ref)
 	return 0;
 }
 
+static int
+zl3073x_dpll_input_pin_phase_offset_get(const struct dpll_pin *dpll_pin,
+					void *pin_priv,
+					const struct dpll_device *dpll,
+					void *dpll_priv, s64 *phase_offset,
+					struct netlink_ext_ack *extack)
+{
+	struct zl3073x_dpll *zldpll = dpll_priv;
+	struct zl3073x_dev *zldev = zldpll->dev;
+	struct zl3073x_dpll_pin *pin = pin_priv;
+	u8 conn_ref, ref, ref_status;
+	int rc;
+
+	/* Get currently connected reference */
+	rc = zl3073x_dpll_connected_ref_get(zldpll, &conn_ref);
+	if (rc)
+		return rc;
+
+	/* Report phase offset only for currently connected pin */
+	ref = zl3073x_input_pin_ref_get(pin->id);
+	if (ref != conn_ref) {
+		*phase_offset = 0;
+
+		return 0;
+	}
+
+	/* Get this pin monitor status */
+	rc = zl3073x_read_u8(zldev, ZL_REG_REF_MON_STATUS(ref), &ref_status);
+	if (rc)
+		return rc;
+
+	/* Report phase offset only if the input pin signal is present */
+	if (ref_status != ZL_REF_MON_STATUS_OK) {
+		*phase_offset = 0;
+
+		return 0;
+	}
+
+	/* Report the latest measured phase offset for the connected ref */
+	*phase_offset = pin->phase_offset * DPLL_PHASE_OFFSET_DIVIDER;
+
+	return rc;
+}
+
 /**
  * zl3073x_dpll_ref_prio_get - get priority for given input pin
  * @pin: pointer to pin
@@ -1303,6 +1349,7 @@ static const struct dpll_pin_ops zl3073x_dpll_input_pin_ops = {
 	.esync_set = zl3073x_dpll_input_pin_esync_set,
 	.frequency_get = zl3073x_dpll_input_pin_frequency_get,
 	.frequency_set = zl3073x_dpll_input_pin_frequency_set,
+	.phase_offset_get = zl3073x_dpll_input_pin_phase_offset_get,
 	.prio_get = zl3073x_dpll_input_pin_prio_get,
 	.prio_set = zl3073x_dpll_input_pin_prio_set,
 	.state_on_dpll_get = zl3073x_dpll_input_pin_state_on_dpll_get,
@@ -1673,6 +1720,51 @@ zl3073x_dpll_device_unregister(struct zl3073x_dpll *zldpll)
 	zldpll->dpll_dev = NULL;
 }
 
+/**
+ * zl3073x_dpll_pin_phase_offset_check - check for pin phase offset change
+ * @pin: pin to check
+ *
+ * Check for the change of DPLL to connected pin phase offset change.
+ *
+ * Return: true on phase offset change, false otherwise
+ */
+static bool
+zl3073x_dpll_pin_phase_offset_check(struct zl3073x_dpll_pin *pin)
+{
+	struct zl3073x_dpll *zldpll = pin->dpll;
+	struct zl3073x_dev *zldev = zldpll->dev;
+	s64 phase_offset;
+	int rc;
+
+	/* Do not check phase offset if the pin is not connected one */
+	if (pin->pin_state != DPLL_PIN_STATE_CONNECTED)
+		return false;
+
+	/* Read DPLL-to-connected-ref phase offset measurement value */
+	rc = zl3073x_read_u48(zldev, ZL_REG_DPLL_PHASE_ERR_DATA(zldpll->id),
+			      &phase_offset);
+	if (rc) {
+		dev_err(zldev->dev, "Failed to read ref phase offset: %pe\n",
+			ERR_PTR(rc));
+
+		return false;
+	}
+
+	/* Convert to ps */
+	phase_offset = div_s64(sign_extend64(phase_offset, 47), 100);
+
+	/* Compare with previous value */
+	if (phase_offset != pin->phase_offset) {
+		dev_dbg(zldev->dev, "%s phase offset changed: %lld -> %lld\n",
+			pin->label, pin->phase_offset, phase_offset);
+		pin->phase_offset = phase_offset;
+
+		return true;
+	}
+
+	return false;
+}
+
 /**
  * zl3073x_dpll_changes_check - check for changes and send notifications
  * @zldpll: pointer to zl3073x_dpll structure
@@ -1691,6 +1783,8 @@ zl3073x_dpll_changes_check(struct zl3073x_dpll *zldpll)
 	struct zl3073x_dpll_pin *pin;
 	int rc;
 
+	zldpll->check_count++;
+
 	/* Get current lock status for the DPLL */
 	rc = zl3073x_dpll_lock_status_get(zldpll->dpll_dev, zldpll,
 					  &lock_status, NULL, NULL);
@@ -1715,6 +1809,7 @@ zl3073x_dpll_changes_check(struct zl3073x_dpll *zldpll)
 
 	list_for_each_entry(pin, &zldpll->pins, list) {
 		enum dpll_pin_state state;
+		bool pin_changed = false;
 
 		/* Output pins change checks are not necessary because output
 		 * states are constant.
@@ -1734,8 +1829,16 @@ zl3073x_dpll_changes_check(struct zl3073x_dpll *zldpll)
 			dev_dbg(dev, "%s state changed: %u->%u\n", pin->label,
 				pin->pin_state, state);
 			pin->pin_state = state;
-			dpll_pin_change_ntf(pin->dpll_pin);
+			pin_changed = true;
 		}
+
+		/* Check for phase offset change once per second */
+		if (zldpll->check_count % 2 == 0)
+			if (zl3073x_dpll_pin_phase_offset_check(pin))
+				pin_changed = true;
+
+		if (pin_changed)
+			dpll_pin_change_ntf(pin->dpll_pin);
 	}
 }
 

drivers/dpll/zl3073x/dpll.h

@@ -15,6 +15,7 @@
  * @id: DPLL index
  * @refsel_mode: reference selection mode
  * @forced_ref: selected reference in forced reference lock mode
+ * @check_count: periodic check counter
  * @dpll_dev: pointer to registered DPLL device
  * @lock_status: last saved DPLL lock status
  * @pins: list of pins
@@ -25,6 +26,7 @@ struct zl3073x_dpll {
 	u8				id;
 	u8				refsel_mode;
 	u8				forced_ref;
+	u8				check_count;
 	struct dpll_device		*dpll_dev;
 	enum dpll_lock_status		lock_status;
 	struct list_head		pins;

drivers/dpll/zl3073x/regs.h

@@ -94,6 +94,13 @@
 #define ZL_DPLL_REFSEL_STATUS_STATE		GENMASK(6, 4)
 #define ZL_DPLL_REFSEL_STATUS_STATE_LOCK	4
 
+/**********************
+ * Register Page 4, Ref
+ **********************/
+
+#define ZL_REG_REF_PHASE_ERR_READ_RQST		ZL_REG(4, 0x0f, 1)
+#define ZL_REF_PHASE_ERR_READ_RQST_RD		BIT(0)
+
 /***********************
  * Register Page 5, DPLL
  ***********************/
@@ -108,6 +115,15 @@
 #define ZL_DPLL_MODE_REFSEL_MODE_NCO		4
 #define ZL_DPLL_MODE_REFSEL_REF			GENMASK(7, 4)
 
+#define ZL_REG_DPLL_MEAS_CTRL			ZL_REG(5, 0x50, 1)
+#define ZL_DPLL_MEAS_CTRL_EN			BIT(0)
+#define ZL_DPLL_MEAS_CTRL_AVG_FACTOR		GENMASK(7, 4)
+
+#define ZL_REG_DPLL_PHASE_ERR_READ_MASK		ZL_REG(5, 0x54, 1)
+
+#define ZL_REG_DPLL_PHASE_ERR_DATA(_idx)				\
+	ZL_REG_IDX(_idx, 5, 0x55, 6, ZL3073X_MAX_CHANNELS, 6)
+
 /***********************************
  * Register Page 9, Synth and Output
  ***********************************/