dpll: zl3073x: Implement phase offset monitor feature

Implement phase offset monitor feature to allow a user to monitor
phase offsets across all available inputs.

The device firmware periodically performs phase offsets measurements for
all available DPLL channels and input references. The driver can ask
the firmware to fill appropriate latch registers with measured values.

There are 2 sets of latch registers for phase offsets reporting:

1) DPLL-to-connected-ref: up to 5 registers that contain values for
   phase offset between particular DPLL channel and its connected input
   reference.
2) selected-DPLL-to-ref: 10  registers that contain values for phase
   offsets between selected DPLL channel and all available input
   references.

Both are filled with single read request so the driver can read
DPLL-to-connected-ref phase offset for all DPLL channels at once.
This was implemented in the previous patch.

To read selected-DPLL-to-ref registers for all DPLLs a separate read
request has to be sent to device firmware for each DPLL channel.

To implement phase offset monitor feature:
* Extend zl3073x_ref_phase_offsets_update() to select given DPLL channel
  in phase offset read request. The caller can set channel==-1 if it
  will not read Type2 registers.
* Use this extended function to update phase offset latch registers
  during zl3073x_dpll_changes_check() call if phase monitor is enabled
* Extend zl3073x_dpll_pin_phase_offset_check() to check phase offset
  changes for all available input references
* Extend zl3073x_dpll_input_pin_phase_offset_get() to report phase
  offset values for all available input references
* Implement phase offset monitor callbacks to enable/disable this
  feature

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

@@ -672,14 +672,25 @@ zl3073x_dev_state_fetch(struct zl3073x_dev *zldev)
 /**
  * zl3073x_ref_phase_offsets_update - update reference phase offsets
  * @zldev: pointer to zl3073x_dev structure
+ * @channel: DPLL channel number or -1
  *
- * Ask device to update phase offsets latch registers with the latest
- * measured values.
+ * The function asks device to update phase offsets latch registers with
+ * the latest measured values. There are 2 sets of latch registers:
+ *
+ * 1) Up to 5 DPLL-to-connected-ref registers that contain phase offset
+ *    values between particular DPLL channel and its *connected* input
+ *    reference.
+ *
+ * 2) 10 selected-DPLL-to-all-ref registers that contain phase offset values
+ *    between selected DPLL channel and all input references.
+ *
+ * If the caller is interested in 2) then it has to pass DPLL channel number
+ * in @channel parameter. If it is interested only in 1) then it should pass
+ * @channel parameter with value of -1.
  *
  * Return: 0 on success, <0 on error
  */
-static int
-zl3073x_ref_phase_offsets_update(struct zl3073x_dev *zldev)
+int zl3073x_ref_phase_offsets_update(struct zl3073x_dev *zldev, int channel)
 {
 	int rc;
 
@@ -691,6 +702,13 @@ zl3073x_ref_phase_offsets_update(struct zl3073x_dev *zldev)
 	if (rc)
 		return rc;
 
+	/* Select DPLL channel if it is specified */
+	if (channel != -1) {
+		rc = zl3073x_write_u8(zldev, ZL_REG_DPLL_MEAS_IDX, channel);
+		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);
@@ -711,7 +729,7 @@ zl3073x_dev_periodic_work(struct kthread_work *work)
 	int rc;
 
 	/* Update DPLL-to-connected-ref phase offsets registers */
-	rc = zl3073x_ref_phase_offsets_update(zldev);
+	rc = zl3073x_ref_phase_offsets_update(zldev, -1);
 	if (rc)
 		dev_warn(zldev->dev, "Failed to update phase offsets: %pe\n",
 			 ERR_PTR(rc));

drivers/dpll/zl3073x/core.h

@@ -130,6 +130,7 @@ int zl3073x_write_u48(struct zl3073x_dev *zldev, unsigned int reg, u64 val);
  *****************/
 
 int zl3073x_ref_freq_factorize(u32 freq, u16 *base, u16 *mult);
+int zl3073x_ref_phase_offsets_update(struct zl3073x_dev *zldev, int channel);
 
 static inline bool
 zl3073x_is_n_pin(u8 id)

drivers/dpll/zl3073x/dpll.c

@@ -509,16 +509,19 @@ zl3073x_dpll_input_pin_phase_offset_get(const struct dpll_pin *dpll_pin,
 	struct zl3073x_dev *zldev = zldpll->dev;
 	struct zl3073x_dpll_pin *pin = pin_priv;
 	u8 conn_ref, ref, ref_status;
+	s64 ref_phase;
 	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 */
+	/* Report phase offset only for currently connected pin if the phase
+	 * monitor feature is disabled.
+	 */
 	ref = zl3073x_input_pin_ref_get(pin->id);
-	if (ref != conn_ref) {
+	if (!zldpll->phase_monitor && ref != conn_ref) {
 		*phase_offset = 0;
 
 		return 0;
@@ -536,8 +539,37 @@ zl3073x_dpll_input_pin_phase_offset_get(const struct dpll_pin *dpll_pin,
 		return 0;
 	}
 
-	/* Report the latest measured phase offset for the connected ref */
-	*phase_offset = pin->phase_offset * DPLL_PHASE_OFFSET_DIVIDER;
+	ref_phase = pin->phase_offset;
+
+	/* The DPLL being locked to a higher freq than the current ref
+	 * the phase offset is modded to the period of the signal
+	 * the dpll is locked to.
+	 */
+	if (ZL3073X_DPLL_REF_IS_VALID(conn_ref) && conn_ref != ref) {
+		u32 conn_freq, ref_freq;
+
+		/* Get frequency of connected ref */
+		rc = zl3073x_dpll_input_ref_frequency_get(zldpll, conn_ref,
+							  &conn_freq);
+		if (rc)
+			return rc;
+
+		/* Get frequency of given ref */
+		rc = zl3073x_dpll_input_ref_frequency_get(zldpll, ref,
+							  &ref_freq);
+		if (rc)
+			return rc;
+
+		if (conn_freq > ref_freq) {
+			s64 conn_period, div_factor;
+
+			conn_period = div_s64(PSEC_PER_SEC, conn_freq);
+			div_factor = div64_s64(ref_phase, conn_period);
+			ref_phase -= conn_period * div_factor;
+		}
+	}
+
+	*phase_offset = ref_phase * DPLL_PHASE_OFFSET_DIVIDER;
 
 	return rc;
 }
@@ -1343,6 +1375,35 @@ zl3073x_dpll_mode_get(const struct dpll_device *dpll, void *dpll_priv,
 	return 0;
 }
 
+static int
+zl3073x_dpll_phase_offset_monitor_get(const struct dpll_device *dpll,
+				      void *dpll_priv,
+				      enum dpll_feature_state *state,
+				      struct netlink_ext_ack *extack)
+{
+	struct zl3073x_dpll *zldpll = dpll_priv;
+
+	if (zldpll->phase_monitor)
+		*state = DPLL_FEATURE_STATE_ENABLE;
+	else
+		*state = DPLL_FEATURE_STATE_DISABLE;
+
+	return 0;
+}
+
+static int
+zl3073x_dpll_phase_offset_monitor_set(const struct dpll_device *dpll,
+				      void *dpll_priv,
+				      enum dpll_feature_state state,
+				      struct netlink_ext_ack *extack)
+{
+	struct zl3073x_dpll *zldpll = dpll_priv;
+
+	zldpll->phase_monitor = (state == DPLL_FEATURE_STATE_ENABLE);
+
+	return 0;
+}
+
 static const struct dpll_pin_ops zl3073x_dpll_input_pin_ops = {
 	.direction_get = zl3073x_dpll_pin_direction_get,
 	.esync_get = zl3073x_dpll_input_pin_esync_get,
@@ -1368,6 +1429,8 @@ static const struct dpll_pin_ops zl3073x_dpll_output_pin_ops = {
 static const struct dpll_device_ops zl3073x_dpll_device_ops = {
 	.lock_status_get = zl3073x_dpll_lock_status_get,
 	.mode_get = zl3073x_dpll_mode_get,
+	.phase_offset_monitor_get = zl3073x_dpll_phase_offset_monitor_get,
+	.phase_offset_monitor_set = zl3073x_dpll_phase_offset_monitor_set,
 };
 
 /**
@@ -1733,16 +1796,47 @@ zl3073x_dpll_pin_phase_offset_check(struct zl3073x_dpll_pin *pin)
 {
 	struct zl3073x_dpll *zldpll = pin->dpll;
 	struct zl3073x_dev *zldev = zldpll->dev;
+	unsigned int reg;
 	s64 phase_offset;
+	u8 ref;
 	int rc;
 
-	/* Do not check phase offset if the pin is not connected one */
-	if (pin->pin_state != DPLL_PIN_STATE_CONNECTED)
+	ref = zl3073x_input_pin_ref_get(pin->id);
+
+	/* Select register to read phase offset value depending on pin and
+	 * phase monitor state:
+	 * 1) For connected pin use dpll_phase_err_data register
+	 * 2) For other pins use appropriate ref_phase register if the phase
+	 *    monitor feature is enabled and reference monitor does not
+	 *    report signal errors for given input pin
+	 */
+	if (pin->pin_state == DPLL_PIN_STATE_CONNECTED) {
+		reg = ZL_REG_DPLL_PHASE_ERR_DATA(zldpll->id);
+	} else if (zldpll->phase_monitor) {
+		u8 status;
+
+		/* Get reference monitor status */
+		rc = zl3073x_read_u8(zldev, ZL_REG_REF_MON_STATUS(ref),
+				     &status);
+		if (rc) {
+			dev_err(zldev->dev,
+				"Failed to read %s refmon status: %pe\n",
+				pin->label, ERR_PTR(rc));
+
+			return false;
+		}
+
+		if (status != ZL_REF_MON_STATUS_OK)
+			return false;
+
+		reg = ZL_REG_REF_PHASE(ref);
+	} else {
+		/* The pin is not connected or phase monitor disabled */
 		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);
+	/* Read measured phase offset value */
+	rc = zl3073x_read_u48(zldev, reg, &phase_offset);
 	if (rc) {
 		dev_err(zldev->dev, "Failed to read ref phase offset: %pe\n",
 			ERR_PTR(rc));
@@ -1807,6 +1901,19 @@ zl3073x_dpll_changes_check(struct zl3073x_dpll *zldpll)
 	    zldpll->refsel_mode != ZL_DPLL_MODE_REFSEL_MODE_REFLOCK)
 		return;
 
+	/* Update phase offset latch registers for this DPLL if the phase
+	 * offset monitor feature is enabled.
+	 */
+	if (zldpll->phase_monitor) {
+		rc = zl3073x_ref_phase_offsets_update(zldev, zldpll->id);
+		if (rc) {
+			dev_err(zldev->dev,
+				"Failed to update phase offsets: %pe\n",
+				ERR_PTR(rc));
+			return;
+		}
+	}
+
 	list_for_each_entry(pin, &zldpll->pins, list) {
 		enum dpll_pin_state state;
 		bool pin_changed = false;

drivers/dpll/zl3073x/dpll.h

@@ -16,6 +16,7 @@
  * @refsel_mode: reference selection mode
  * @forced_ref: selected reference in forced reference lock mode
  * @check_count: periodic check counter
+ * @phase_monitor: is phase offset monitor enabled
  * @dpll_dev: pointer to registered DPLL device
  * @lock_status: last saved DPLL lock status
  * @pins: list of pins
@@ -27,6 +28,7 @@ struct zl3073x_dpll {
 	u8				refsel_mode;
 	u8				forced_ref;
 	u8				check_count;
+	bool				phase_monitor;
 	struct dpll_device		*dpll_dev;
 	enum dpll_lock_status		lock_status;
 	struct list_head		pins;

drivers/dpll/zl3073x/regs.h

@@ -101,6 +101,9 @@
 #define ZL_REG_REF_PHASE_ERR_READ_RQST		ZL_REG(4, 0x0f, 1)
 #define ZL_REF_PHASE_ERR_READ_RQST_RD		BIT(0)
 
+#define ZL_REG_REF_PHASE(_idx)						\
+	ZL_REG_IDX(_idx, 4, 0x20, 6, ZL3073X_NUM_REFS, 6)
+
 /***********************
  * Register Page 5, DPLL
  ***********************/
@@ -119,6 +122,9 @@
 #define ZL_DPLL_MEAS_CTRL_EN			BIT(0)
 #define ZL_DPLL_MEAS_CTRL_AVG_FACTOR		GENMASK(7, 4)
 
+#define ZL_REG_DPLL_MEAS_IDX			ZL_REG(5, 0x51, 1)
+#define ZL_DPLL_MEAS_IDX			GENMASK(2, 0)
+
 #define ZL_REG_DPLL_PHASE_ERR_READ_MASK		ZL_REG(5, 0x54, 1)
 
 #define ZL_REG_DPLL_PHASE_ERR_DATA(_idx)				\