Re: [PATCH net-next v2 3/3] dpll: zl3073x: implement frequency monitoring

[Petr Oros <poros@redhat.com>]

> Extract common measurement latch logic from zl3073x_ref_ffo_update()
> into a new zl3073x_ref_freq_meas_latch() helper and add
> zl3073x_ref_freq_meas_update() that uses it to latch and read absolute
> input reference frequencies in Hz.
>
> Add meas_freq field to struct zl3073x_ref and the corresponding
> zl3073x_ref_meas_freq_get() accessor. The measured frequencies are
> updated periodically alongside the existing FFO measurements.
>
> Add freq_monitor boolean to struct zl3073x_dpll and implement the
> freq_monitor_set/get device callbacks to enable/disable frequency
> monitoring via the DPLL netlink interface.
>
> Implement measured_freq_get pin callback for input pins that returns the
> measured input frequency in Hz.
>
> Signed-off-by: Ivan Vecera <ivecera@redhat.com>
> ---
>   drivers/dpll/zl3073x/core.c | 88 +++++++++++++++++++++++++++++++------
>   drivers/dpll/zl3073x/dpll.c | 88 ++++++++++++++++++++++++++++++++++++-
>   drivers/dpll/zl3073x/dpll.h |  2 +
>   drivers/dpll/zl3073x/ref.h  | 14 ++++++
>   4 files changed, 178 insertions(+), 14 deletions(-)
>
> diff --git a/drivers/dpll/zl3073x/core.c b/drivers/dpll/zl3073x/core.c
> index 6363002d48d46..320c199637efa 100644
> --- a/drivers/dpll/zl3073x/core.c
> +++ b/drivers/dpll/zl3073x/core.c
> @@ -632,22 +632,21 @@ int zl3073x_ref_phase_offsets_update(struct zl3073x_dev *zldev, int channel)
>   }
>   
>   /**
> - * zl3073x_ref_ffo_update - update reference fractional frequency offsets
> + * zl3073x_ref_freq_meas_latch - latch reference frequency measurements
>    * @zldev: pointer to zl3073x_dev structure
> + * @type: measurement type (ZL_REF_FREQ_MEAS_CTRL_*)
>    *
> - * The function asks device to update fractional frequency offsets latch
> - * registers the latest measured values, reads and stores them into
> + * The function waits for the previous measurement to finish, selects all
> + * references and requests a new measurement of the given type.
>    *
>    * Return: 0 on success, <0 on error
>    */
>   static int
> -zl3073x_ref_ffo_update(struct zl3073x_dev *zldev)
> +zl3073x_ref_freq_meas_latch(struct zl3073x_dev *zldev, u8 type)
>   {
> -	int i, rc;
> +	int rc;
>   
> -	/* Per datasheet we have to wait for 'ref_freq_meas_ctrl' to be zero
> -	 * to ensure that the measured data are coherent.
> -	 */
> +	/* Wait for previous measurement to finish */
>   	rc = zl3073x_poll_zero_u8(zldev, ZL_REG_REF_FREQ_MEAS_CTRL,
>   				  ZL_REF_FREQ_MEAS_CTRL);
>   	if (rc)
> @@ -663,15 +662,64 @@ zl3073x_ref_ffo_update(struct zl3073x_dev *zldev)
>   	if (rc)
>   		return rc;
>   
> -	/* Request frequency offset measurement */
> -	rc = zl3073x_write_u8(zldev, ZL_REG_REF_FREQ_MEAS_CTRL,
> -			      ZL_REF_FREQ_MEAS_CTRL_REF_FREQ_OFF);
> +	/* Request measurement */
> +	rc = zl3073x_write_u8(zldev, ZL_REG_REF_FREQ_MEAS_CTRL, type);
>   	if (rc)
>   		return rc;
>   
>   	/* Wait for finish */
> -	rc = zl3073x_poll_zero_u8(zldev, ZL_REG_REF_FREQ_MEAS_CTRL,
> -				  ZL_REF_FREQ_MEAS_CTRL);
> +	return zl3073x_poll_zero_u8(zldev, ZL_REG_REF_FREQ_MEAS_CTRL,
> +				    ZL_REF_FREQ_MEAS_CTRL);
> +}
> +
> +/**
> + * zl3073x_ref_freq_meas_update - update measured input reference frequencies
> + * @zldev: pointer to zl3073x_dev structure
> + *
> + * The function asks device to latch measured input reference frequencies
> + * and stores the results in the ref state.
> + *
> + * Return: 0 on success, <0 on error
> + */
> +static int
> +zl3073x_ref_freq_meas_update(struct zl3073x_dev *zldev)
> +{
> +	int i, rc;
> +
> +	rc = zl3073x_ref_freq_meas_latch(zldev, ZL_REF_FREQ_MEAS_CTRL_REF_FREQ);
> +	if (rc)
> +		return rc;
> +
> +	/* Read measured frequencies in Hz (unsigned 32-bit, LSB = 1 Hz) */
> +	for (i = 0; i < ZL3073X_NUM_REFS; i++) {
> +		u32 value;
> +
> +		rc = zl3073x_read_u32(zldev, ZL_REG_REF_FREQ(i), &value);
> +		if (rc)
> +			return rc;
> +
> +		zldev->ref[i].meas_freq = value;
> +	}
> +
> +	return 0;
> +}
> +
> +/**
> + * zl3073x_ref_ffo_update - update reference fractional frequency offsets
> + * @zldev: pointer to zl3073x_dev structure
> + *
> + * The function asks device to update fractional frequency offsets latch
> + * registers the latest measured values, reads and stores them into
> + *
> + * Return: 0 on success, <0 on error
> + */
> +static int
> +zl3073x_ref_ffo_update(struct zl3073x_dev *zldev)
> +{
> +	int i, rc;
> +
> +	rc = zl3073x_ref_freq_meas_latch(zldev,
> +					 ZL_REF_FREQ_MEAS_CTRL_REF_FREQ_OFF);
>   	if (rc)
>   		return rc;
>   
> @@ -714,6 +762,20 @@ zl3073x_dev_periodic_work(struct kthread_work *work)
>   		dev_warn(zldev->dev, "Failed to update phase offsets: %pe\n",
>   			 ERR_PTR(rc));
>   
> +	/* Update measured input reference frequencies if any DPLL has
> +	 * frequency monitoring enabled.
> +	 */
> +	list_for_each_entry(zldpll, &zldev->dplls, list) {
> +		if (zldpll->freq_monitor) {
> +			rc = zl3073x_ref_freq_meas_update(zldev);
> +			if (rc)
> +				dev_warn(zldev->dev,
> +					 "Failed to update measured frequencies: %pe\n",
> +					 ERR_PTR(rc));
> +			break;
> +		}
> +	}
> +
>   	/* Update references' fractional frequency offsets */
>   	rc = zl3073x_ref_ffo_update(zldev);
>   	if (rc)
> diff --git a/drivers/dpll/zl3073x/dpll.c b/drivers/dpll/zl3073x/dpll.c
> index a29f606318f6d..c44bfecf2c265 100644
> --- a/drivers/dpll/zl3073x/dpll.c
> +++ b/drivers/dpll/zl3073x/dpll.c
> @@ -39,6 +39,7 @@
>    * @pin_state: last saved pin state
>    * @phase_offset: last saved pin phase offset
>    * @freq_offset: last saved fractional frequency offset
> + * @measured_freq: last saved measured frequency
>    */
>   struct zl3073x_dpll_pin {
>   	struct list_head	list;
> @@ -54,6 +55,7 @@ struct zl3073x_dpll_pin {
>   	enum dpll_pin_state	pin_state;
>   	s64			phase_offset;
>   	s64			freq_offset;
> +	u32			measured_freq;
>   };
>   
>   /*
> @@ -202,6 +204,20 @@ zl3073x_dpll_input_pin_ffo_get(const struct dpll_pin *dpll_pin, void *pin_priv,
>   	return 0;
>   }
>   
> +static int
> +zl3073x_dpll_input_pin_measured_freq_get(const struct dpll_pin *dpll_pin,
> +					 void *pin_priv,
> +					 const struct dpll_device *dpll,
> +					 void *dpll_priv, u64 *measured_freq,
> +					 struct netlink_ext_ack *extack)
> +{
> +	struct zl3073x_dpll_pin *pin = pin_priv;
> +
> +	*measured_freq = pin->measured_freq;
> +
> +	return 0;
> +}
> +
>   static int
>   zl3073x_dpll_input_pin_frequency_get(const struct dpll_pin *dpll_pin,
>   				     void *pin_priv,
> @@ -1116,6 +1132,35 @@ zl3073x_dpll_phase_offset_monitor_set(const struct dpll_device *dpll,
>   	return 0;
>   }
>   
> +static int
> +zl3073x_dpll_freq_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->freq_monitor)
> +		*state = DPLL_FEATURE_STATE_ENABLE;
> +	else
> +		*state = DPLL_FEATURE_STATE_DISABLE;
> +
> +	return 0;
> +}
> +
> +static int
> +zl3073x_dpll_freq_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->freq_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,
> @@ -1123,6 +1168,7 @@ static const struct dpll_pin_ops zl3073x_dpll_input_pin_ops = {
>   	.ffo_get = zl3073x_dpll_input_pin_ffo_get,
>   	.frequency_get = zl3073x_dpll_input_pin_frequency_get,
>   	.frequency_set = zl3073x_dpll_input_pin_frequency_set,
> +	.measured_freq_get = zl3073x_dpll_input_pin_measured_freq_get,
>   	.phase_offset_get = zl3073x_dpll_input_pin_phase_offset_get,
>   	.phase_adjust_get = zl3073x_dpll_input_pin_phase_adjust_get,
>   	.phase_adjust_set = zl3073x_dpll_input_pin_phase_adjust_set,
> @@ -1151,6 +1197,8 @@ static const struct dpll_device_ops zl3073x_dpll_device_ops = {
>   	.phase_offset_avg_factor_set = zl3073x_dpll_phase_offset_avg_factor_set,
>   	.phase_offset_monitor_get = zl3073x_dpll_phase_offset_monitor_get,
>   	.phase_offset_monitor_set = zl3073x_dpll_phase_offset_monitor_set,
> +	.freq_monitor_get = zl3073x_dpll_freq_monitor_get,
> +	.freq_monitor_set = zl3073x_dpll_freq_monitor_set,
>   	.supported_modes_get = zl3073x_dpll_supported_modes_get,
>   };
>   
> @@ -1593,6 +1641,39 @@ zl3073x_dpll_pin_ffo_check(struct zl3073x_dpll_pin *pin)
>   	return false;
>   }
>   
> +/**
> + * zl3073x_dpll_pin_measured_freq_check - check for pin measured frequency change
> + * @pin: pin to check
> + *
> + * Check for the given pin's measured frequency change.
> + *
> + * Return: true on measured frequency change, false otherwise
> + */
> +static bool
> +zl3073x_dpll_pin_measured_freq_check(struct zl3073x_dpll_pin *pin)
> +{
> +	struct zl3073x_dpll *zldpll = pin->dpll;
> +	struct zl3073x_dev *zldev = zldpll->dev;
> +	const struct zl3073x_ref *ref;
> +	u8 ref_id;
> +
> +	if (!zldpll->freq_monitor)
> +		return false;
> +
> +	ref_id = zl3073x_input_pin_ref_get(pin->id);
> +	ref = zl3073x_ref_state_get(zldev, ref_id);
> +
> +	if (pin->measured_freq != ref->meas_freq) {
> +		dev_dbg(zldev->dev, "%s measured freq changed: %u -> %u\n",
> +			pin->label, pin->measured_freq, ref->meas_freq);
> +		pin->measured_freq = ref->meas_freq;
> +
> +		return true;
> +	}
> +
> +	return false;
> +}
> +
>   /**
>    * zl3073x_dpll_changes_check - check for changes and send notifications
>    * @zldpll: pointer to zl3073x_dpll structure
> @@ -1677,13 +1758,18 @@ zl3073x_dpll_changes_check(struct zl3073x_dpll *zldpll)
>   			pin_changed = true;
>   		}
>   
> -		/* Check for phase offset and ffo change once per second */
> +		/* Check for phase offset, ffo, and measured freq change
> +		 * once per second.
> +		 */
>   		if (zldpll->check_count % 2 == 0) {
>   			if (zl3073x_dpll_pin_phase_offset_check(pin))
>   				pin_changed = true;
>   
>   			if (zl3073x_dpll_pin_ffo_check(pin))
>   				pin_changed = true;
> +
> +			if (zl3073x_dpll_pin_measured_freq_check(pin))
> +				pin_changed = true;
>   		}
>   
>   		if (pin_changed)
> diff --git a/drivers/dpll/zl3073x/dpll.h b/drivers/dpll/zl3073x/dpll.h
> index 115ee4f67e7ab..434c32a7db123 100644
> --- a/drivers/dpll/zl3073x/dpll.h
> +++ b/drivers/dpll/zl3073x/dpll.h
> @@ -15,6 +15,7 @@
>    * @id: DPLL index
>    * @check_count: periodic check counter
>    * @phase_monitor: is phase offset monitor enabled
> + * @freq_monitor: is frequency monitor enabled
>    * @ops: DPLL device operations for this instance
>    * @dpll_dev: pointer to registered DPLL device
>    * @tracker: tracking object for the acquired reference
> @@ -28,6 +29,7 @@ struct zl3073x_dpll {
>   	u8				id;
>   	u8				check_count;
>   	bool				phase_monitor;
> +	bool				freq_monitor;
>   	struct dpll_device_ops		ops;
>   	struct dpll_device		*dpll_dev;
>   	dpll_tracker			tracker;
> diff --git a/drivers/dpll/zl3073x/ref.h b/drivers/dpll/zl3073x/ref.h
> index 06d8d4d97ea26..be16be20dbc7e 100644
> --- a/drivers/dpll/zl3073x/ref.h
> +++ b/drivers/dpll/zl3073x/ref.h
> @@ -23,6 +23,7 @@ struct zl3073x_dev;
>    * @sync_ctrl: reference sync control
>    * @config: reference config
>    * @ffo: current fractional frequency offset
> + * @meas_freq: measured input frequency in Hz
>    * @mon_status: reference monitor status
>    */
>   struct zl3073x_ref {
> @@ -40,6 +41,7 @@ struct zl3073x_ref {
>   	);
>   	struct_group(stat, /* Status */
>   		s64	ffo;
> +		u32	meas_freq;
>   		u8	mon_status;
>   	);
>   };
> @@ -68,6 +70,18 @@ zl3073x_ref_ffo_get(const struct zl3073x_ref *ref)
>   	return ref->ffo;
>   }
>   
> +/**
> + * zl3073x_ref_meas_freq_get - get measured input frequency
> + * @ref: pointer to ref state
> + *
> + * Return: measured input frequency in Hz
> + */
> +static inline u32
> +zl3073x_ref_meas_freq_get(const struct zl3073x_ref *ref)
> +{
> +	return ref->meas_freq;
> +}
> +
>   /**
>    * zl3073x_ref_freq_get - get given input reference frequency
>    * @ref: pointer to ref state

LGTM

Reviewed-by: Petr Oros <poros@redhat.com>