diff --git a/drivers/dpll/zl3073x/dpll.c b/drivers/dpll/zl3073x/dpll.c index 11a7c4a58e25..a63a3434da74 100644 --- a/drivers/dpll/zl3073x/dpll.c +++ b/drivers/dpll/zl3073x/dpll.c @@ -574,6 +574,85 @@ zl3073x_dpll_input_pin_phase_offset_get(const struct dpll_pin *dpll_pin, return rc; } +static int +zl3073x_dpll_input_pin_phase_adjust_get(const struct dpll_pin *dpll_pin, + void *pin_priv, + const struct dpll_device *dpll, + void *dpll_priv, + s32 *phase_adjust, + struct netlink_ext_ack *extack) +{ + struct zl3073x_dpll *zldpll = dpll_priv; + struct zl3073x_dev *zldev = zldpll->dev; + struct zl3073x_dpll_pin *pin = pin_priv; + s64 phase_comp; + u8 ref; + int rc; + + guard(mutex)(&zldev->multiop_lock); + + /* Read reference configuration */ + ref = zl3073x_input_pin_ref_get(pin->id); + rc = zl3073x_mb_op(zldev, ZL_REG_REF_MB_SEM, ZL_REF_MB_SEM_RD, + ZL_REG_REF_MB_MASK, BIT(ref)); + if (rc) + return rc; + + /* Read current phase offset compensation */ + rc = zl3073x_read_u48(zldev, ZL_REG_REF_PHASE_OFFSET_COMP, &phase_comp); + if (rc) + return rc; + + /* Perform sign extension for 48bit signed value */ + phase_comp = sign_extend64(phase_comp, 47); + + /* Reverse two's complement negation applied during set and convert + * to 32bit signed int + */ + *phase_adjust = (s32)-phase_comp; + + return rc; +} + +static int +zl3073x_dpll_input_pin_phase_adjust_set(const struct dpll_pin *dpll_pin, + void *pin_priv, + const struct dpll_device *dpll, + void *dpll_priv, + s32 phase_adjust, + struct netlink_ext_ack *extack) +{ + struct zl3073x_dpll *zldpll = dpll_priv; + struct zl3073x_dev *zldev = zldpll->dev; + struct zl3073x_dpll_pin *pin = pin_priv; + s64 phase_comp; + u8 ref; + int rc; + + /* The value in the register is stored as two's complement negation + * of requested value. + */ + phase_comp = -phase_adjust; + + guard(mutex)(&zldev->multiop_lock); + + /* Read reference configuration */ + ref = zl3073x_input_pin_ref_get(pin->id); + rc = zl3073x_mb_op(zldev, ZL_REG_REF_MB_SEM, ZL_REF_MB_SEM_RD, + ZL_REG_REF_MB_MASK, BIT(ref)); + if (rc) + return rc; + + /* Write the requested value into the compensation register */ + rc = zl3073x_write_u48(zldev, ZL_REG_REF_PHASE_OFFSET_COMP, phase_comp); + if (rc) + return rc; + + /* Commit reference configuration */ + return zl3073x_mb_op(zldev, ZL_REG_REF_MB_SEM, ZL_REF_MB_SEM_WR, + ZL_REG_REF_MB_MASK, BIT(ref)); +} + /** * zl3073x_dpll_ref_prio_get - get priority for given input pin * @pin: pointer to pin @@ -1284,6 +1363,114 @@ zl3073x_dpll_output_pin_frequency_set(const struct dpll_pin *dpll_pin, ZL_REG_OUTPUT_MB_MASK, BIT(out)); } +static int +zl3073x_dpll_output_pin_phase_adjust_get(const struct dpll_pin *dpll_pin, + void *pin_priv, + const struct dpll_device *dpll, + void *dpll_priv, + s32 *phase_adjust, + struct netlink_ext_ack *extack) +{ + struct zl3073x_dpll *zldpll = dpll_priv; + struct zl3073x_dev *zldev = zldpll->dev; + struct zl3073x_dpll_pin *pin = pin_priv; + u32 synth_freq; + s32 phase_comp; + u8 out, synth; + int rc; + + out = zl3073x_output_pin_out_get(pin->id); + synth = zl3073x_out_synth_get(zldev, out); + synth_freq = zl3073x_synth_freq_get(zldev, synth); + + /* Check synth freq for zero */ + if (!synth_freq) { + dev_err(zldev->dev, "Got zero synth frequency for output %u\n", + out); + return -EINVAL; + } + + guard(mutex)(&zldev->multiop_lock); + + /* Read output configuration */ + rc = zl3073x_mb_op(zldev, ZL_REG_OUTPUT_MB_SEM, ZL_OUTPUT_MB_SEM_RD, + ZL_REG_OUTPUT_MB_MASK, BIT(out)); + if (rc) + return rc; + + /* Read current output phase compensation */ + rc = zl3073x_read_u32(zldev, ZL_REG_OUTPUT_PHASE_COMP, &phase_comp); + if (rc) + return rc; + + /* Value in register is expressed in half synth clock cycles */ + phase_comp *= (int)div_u64(PSEC_PER_SEC, 2 * synth_freq); + + /* Reverse two's complement negation applied during 'set' */ + *phase_adjust = -phase_comp; + + return rc; +} + +static int +zl3073x_dpll_output_pin_phase_adjust_set(const struct dpll_pin *dpll_pin, + void *pin_priv, + const struct dpll_device *dpll, + void *dpll_priv, + s32 phase_adjust, + struct netlink_ext_ack *extack) +{ + struct zl3073x_dpll *zldpll = dpll_priv; + struct zl3073x_dev *zldev = zldpll->dev; + struct zl3073x_dpll_pin *pin = pin_priv; + int half_synth_cycle; + u32 synth_freq; + u8 out, synth; + int rc; + + /* Get attached synth */ + out = zl3073x_output_pin_out_get(pin->id); + synth = zl3073x_out_synth_get(zldev, out); + + /* Get synth's frequency */ + synth_freq = zl3073x_synth_freq_get(zldev, synth); + + /* Value in register is expressed in half synth clock cycles so + * the given phase adjustment a multiple of half synth clock. + */ + half_synth_cycle = (int)div_u64(PSEC_PER_SEC, 2 * synth_freq); + + if ((phase_adjust % half_synth_cycle) != 0) { + NL_SET_ERR_MSG_FMT(extack, + "Phase adjustment value has to be multiple of %d", + half_synth_cycle); + return -EINVAL; + } + phase_adjust /= half_synth_cycle; + + /* The value in the register is stored as two's complement negation + * of requested value. + */ + phase_adjust = -phase_adjust; + + guard(mutex)(&zldev->multiop_lock); + + /* Read output configuration */ + rc = zl3073x_mb_op(zldev, ZL_REG_OUTPUT_MB_SEM, ZL_OUTPUT_MB_SEM_RD, + ZL_REG_OUTPUT_MB_MASK, BIT(out)); + if (rc) + return rc; + + /* Write the requested value into the compensation register */ + rc = zl3073x_write_u32(zldev, ZL_REG_OUTPUT_PHASE_COMP, phase_adjust); + if (rc) + return rc; + + /* Update output configuration from mailbox */ + return zl3073x_mb_op(zldev, ZL_REG_OUTPUT_MB_SEM, ZL_OUTPUT_MB_SEM_WR, + ZL_REG_OUTPUT_MB_MASK, BIT(out)); +} + static int zl3073x_dpll_output_pin_state_on_dpll_get(const struct dpll_pin *dpll_pin, void *pin_priv, @@ -1411,6 +1598,8 @@ static const struct dpll_pin_ops zl3073x_dpll_input_pin_ops = { .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, + .phase_adjust_get = zl3073x_dpll_input_pin_phase_adjust_get, + .phase_adjust_set = zl3073x_dpll_input_pin_phase_adjust_set, .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, @@ -1423,6 +1612,8 @@ static const struct dpll_pin_ops zl3073x_dpll_output_pin_ops = { .esync_set = zl3073x_dpll_output_pin_esync_set, .frequency_get = zl3073x_dpll_output_pin_frequency_get, .frequency_set = zl3073x_dpll_output_pin_frequency_set, + .phase_adjust_get = zl3073x_dpll_output_pin_phase_adjust_get, + .phase_adjust_set = zl3073x_dpll_output_pin_phase_adjust_set, .state_on_dpll_get = zl3073x_dpll_output_pin_state_on_dpll_get, }; diff --git a/drivers/dpll/zl3073x/regs.h b/drivers/dpll/zl3073x/regs.h index 9ee2f44a2eec..a382cd4a109f 100644 --- a/drivers/dpll/zl3073x/regs.h +++ b/drivers/dpll/zl3073x/regs.h @@ -168,6 +168,8 @@ #define ZL_REF_CONFIG_ENABLE BIT(0) #define ZL_REF_CONFIG_DIFF_EN BIT(2) +#define ZL_REG_REF_PHASE_OFFSET_COMP ZL_REG(10, 0x28, 6) + #define ZL_REG_REF_SYNC_CTRL ZL_REG(10, 0x2e, 1) #define ZL_REF_SYNC_CTRL_MODE GENMASK(2, 0) #define ZL_REF_SYNC_CTRL_MODE_REFSYNC_PAIR_OFF 0 @@ -237,5 +239,6 @@ #define ZL_REG_OUTPUT_WIDTH ZL_REG(14, 0x10, 4) #define ZL_REG_OUTPUT_ESYNC_PERIOD ZL_REG(14, 0x14, 4) #define ZL_REG_OUTPUT_ESYNC_WIDTH ZL_REG(14, 0x18, 4) +#define ZL_REG_OUTPUT_PHASE_COMP ZL_REG(14, 0x20, 4) #endif /* _ZL3073X_REGS_H */