diff options
| -rw-r--r-- | drivers/dpll/zl3073x/dpll.c | 349 | ||||
| -rw-r--r-- | drivers/dpll/zl3073x/regs.h | 10 |
2 files changed, 359 insertions, 0 deletions
diff --git a/drivers/dpll/zl3073x/dpll.c b/drivers/dpll/zl3073x/dpll.c index d39094b7cbc8..cb0f1a43c5fb 100644 --- a/drivers/dpll/zl3073x/dpll.c +++ b/drivers/dpll/zl3073x/dpll.c @@ -8,6 +8,7 @@ #include <linux/dpll.h> #include <linux/err.h> #include <linux/kthread.h> +#include <linux/math64.h> #include <linux/mod_devicetable.h> #include <linux/module.h> #include <linux/netlink.h> @@ -85,6 +86,127 @@ zl3073x_dpll_pin_direction_get(const struct dpll_pin *dpll_pin, void *pin_priv, } /** + * zl3073x_dpll_input_ref_frequency_get - get input reference frequency + * @zldpll: pointer to zl3073x_dpll + * @ref_id: reference id + * @frequency: pointer to variable to store frequency + * + * Reads frequency of given input reference. + * + * Return: 0 on success, <0 on error + */ +static int +zl3073x_dpll_input_ref_frequency_get(struct zl3073x_dpll *zldpll, u8 ref_id, + u32 *frequency) +{ + struct zl3073x_dev *zldev = zldpll->dev; + u16 base, mult, num, denom; + int rc; + + guard(mutex)(&zldev->multiop_lock); + + /* Read reference configuration */ + rc = zl3073x_mb_op(zldev, ZL_REG_REF_MB_SEM, ZL_REF_MB_SEM_RD, + ZL_REG_REF_MB_MASK, BIT(ref_id)); + if (rc) + return rc; + + /* Read registers to compute resulting frequency */ + rc = zl3073x_read_u16(zldev, ZL_REG_REF_FREQ_BASE, &base); + if (rc) + return rc; + rc = zl3073x_read_u16(zldev, ZL_REG_REF_FREQ_MULT, &mult); + if (rc) + return rc; + rc = zl3073x_read_u16(zldev, ZL_REG_REF_RATIO_M, &num); + if (rc) + return rc; + rc = zl3073x_read_u16(zldev, ZL_REG_REF_RATIO_N, &denom); + if (rc) + return rc; + + /* Sanity check that HW has not returned zero denominator */ + if (!denom) { + dev_err(zldev->dev, + "Zero divisor for ref %u frequency got from device\n", + ref_id); + return -EINVAL; + } + + /* Compute the frequency */ + *frequency = mul_u64_u32_div(base * mult, num, denom); + + return rc; +} + +static int +zl3073x_dpll_input_pin_frequency_get(const struct dpll_pin *dpll_pin, + void *pin_priv, + const struct dpll_device *dpll, + void *dpll_priv, u64 *frequency, + struct netlink_ext_ack *extack) +{ + struct zl3073x_dpll *zldpll = dpll_priv; + struct zl3073x_dpll_pin *pin = pin_priv; + u32 ref_freq; + u8 ref; + int rc; + + /* Read and return ref frequency */ + ref = zl3073x_input_pin_ref_get(pin->id); + rc = zl3073x_dpll_input_ref_frequency_get(zldpll, ref, &ref_freq); + if (!rc) + *frequency = ref_freq; + + return rc; +} + +static int +zl3073x_dpll_input_pin_frequency_set(const struct dpll_pin *dpll_pin, + void *pin_priv, + const struct dpll_device *dpll, + void *dpll_priv, u64 frequency, + struct netlink_ext_ack *extack) +{ + struct zl3073x_dpll *zldpll = dpll_priv; + struct zl3073x_dev *zldev = zldpll->dev; + struct zl3073x_dpll_pin *pin = pin_priv; + u16 base, mult; + u8 ref; + int rc; + + /* Get base frequency and multiplier for the requested frequency */ + rc = zl3073x_ref_freq_factorize(frequency, &base, &mult); + if (rc) + return rc; + + guard(mutex)(&zldev->multiop_lock); + + /* Load 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)); + + /* Update base frequency, multiplier, numerator & denominator */ + rc = zl3073x_write_u16(zldev, ZL_REG_REF_FREQ_BASE, base); + if (rc) + return rc; + rc = zl3073x_write_u16(zldev, ZL_REG_REF_FREQ_MULT, mult); + if (rc) + return rc; + rc = zl3073x_write_u16(zldev, ZL_REG_REF_RATIO_M, 1); + if (rc) + return rc; + rc = zl3073x_write_u16(zldev, ZL_REG_REF_RATIO_N, 1); + 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_selected_ref_get - get currently selected reference * @zldpll: pointer to zl3073x_dpll * @ref: place to store selected reference @@ -519,6 +641,229 @@ zl3073x_dpll_input_pin_prio_set(const struct dpll_pin *dpll_pin, void *pin_priv, } static int +zl3073x_dpll_output_pin_frequency_get(const struct dpll_pin *dpll_pin, + void *pin_priv, + const struct dpll_device *dpll, + void *dpll_priv, u64 *frequency, + struct netlink_ext_ack *extack) +{ + struct zl3073x_dpll *zldpll = dpll_priv; + struct zl3073x_dev *zldev = zldpll->dev; + struct zl3073x_dpll_pin *pin = pin_priv; + struct device *dev = zldev->dev; + u8 out, signal_format, synth; + u32 output_div, synth_freq; + 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); + + guard(mutex)(&zldev->multiop_lock); + + /* Read output configuration into mailbox */ + 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; + + rc = zl3073x_read_u32(zldev, ZL_REG_OUTPUT_DIV, &output_div); + if (rc) + return rc; + + /* Check output divisor for zero */ + if (!output_div) { + dev_err(dev, "Zero divisor for output %u got from device\n", + out); + return -EINVAL; + } + + /* Read used signal format for the given output */ + signal_format = zl3073x_out_signal_format_get(zldev, out); + + switch (signal_format) { + case ZL_OUTPUT_MODE_SIGNAL_FORMAT_2_NDIV: + case ZL_OUTPUT_MODE_SIGNAL_FORMAT_2_NDIV_INV: + /* In case of divided format we have to distiguish between + * given output pin type. + */ + if (zl3073x_dpll_is_p_pin(pin)) { + /* For P-pin the resulting frequency is computed as + * simple division of synth frequency and output + * divisor. + */ + *frequency = synth_freq / output_div; + } else { + /* For N-pin we have to divide additionally by + * divisor stored in esync_period output mailbox + * register that is used as N-pin divisor for these + * modes. + */ + u32 ndiv; + + rc = zl3073x_read_u32(zldev, ZL_REG_OUTPUT_ESYNC_PERIOD, + &ndiv); + if (rc) + return rc; + + /* Check N-pin divisor for zero */ + if (!ndiv) { + dev_err(dev, + "Zero N-pin divisor for output %u got from device\n", + out); + return -EINVAL; + } + + /* Compute final divisor for N-pin */ + *frequency = synth_freq / output_div / ndiv; + } + break; + default: + /* In other modes the resulting frequency is computed as + * division of synth frequency and output divisor. + */ + *frequency = synth_freq / output_div; + break; + } + + return rc; +} + +static int +zl3073x_dpll_output_pin_frequency_set(const struct dpll_pin *dpll_pin, + void *pin_priv, + const struct dpll_device *dpll, + void *dpll_priv, u64 frequency, + struct netlink_ext_ack *extack) +{ + struct zl3073x_dpll *zldpll = dpll_priv; + struct zl3073x_dev *zldev = zldpll->dev; + struct zl3073x_dpll_pin *pin = pin_priv; + struct device *dev = zldev->dev; + u32 output_n_freq, output_p_freq; + u8 out, signal_format, synth; + u32 cur_div, new_div, ndiv; + u32 synth_freq; + 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); + new_div = synth_freq / (u32)frequency; + + /* Get used signal format for the given output */ + signal_format = zl3073x_out_signal_format_get(zldev, out); + + guard(mutex)(&zldev->multiop_lock); + + /* Load 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; + + /* Check signal format */ + if (signal_format != ZL_OUTPUT_MODE_SIGNAL_FORMAT_2_NDIV && + signal_format != ZL_OUTPUT_MODE_SIGNAL_FORMAT_2_NDIV_INV) { + /* For non N-divided signal formats the frequency is computed + * as division of synth frequency and output divisor. + */ + rc = zl3073x_write_u32(zldev, ZL_REG_OUTPUT_DIV, new_div); + if (rc) + return rc; + + /* For 50/50 duty cycle the divisor is equal to width */ + rc = zl3073x_write_u32(zldev, ZL_REG_OUTPUT_WIDTH, new_div); + if (rc) + return rc; + + /* Commit output configuration */ + return zl3073x_mb_op(zldev, + ZL_REG_OUTPUT_MB_SEM, ZL_OUTPUT_MB_SEM_WR, + ZL_REG_OUTPUT_MB_MASK, BIT(out)); + } + + /* For N-divided signal format get current divisor */ + rc = zl3073x_read_u32(zldev, ZL_REG_OUTPUT_DIV, &cur_div); + if (rc) + return rc; + + /* Check output divisor for zero */ + if (!cur_div) { + dev_err(dev, "Zero divisor for output %u got from device\n", + out); + return -EINVAL; + } + + /* Get N-pin divisor (shares the same register with esync */ + rc = zl3073x_read_u32(zldev, ZL_REG_OUTPUT_ESYNC_PERIOD, &ndiv); + if (rc) + return rc; + + /* Check N-pin divisor for zero */ + if (!ndiv) { + dev_err(dev, + "Zero N-pin divisor for output %u got from device\n", + out); + return -EINVAL; + } + + /* Compute current output frequency for P-pin */ + output_p_freq = synth_freq / cur_div; + + /* Compute current N-pin frequency */ + output_n_freq = output_p_freq / ndiv; + + if (zl3073x_dpll_is_p_pin(pin)) { + /* We are going to change output frequency for P-pin but + * if the requested frequency is less than current N-pin + * frequency then indicate a failure as we are not able + * to compute N-pin divisor to keep its frequency unchanged. + */ + if (frequency <= output_n_freq) + return -EINVAL; + + /* Update the output divisor */ + rc = zl3073x_write_u32(zldev, ZL_REG_OUTPUT_DIV, new_div); + if (rc) + return rc; + + /* For 50/50 duty cycle the divisor is equal to width */ + rc = zl3073x_write_u32(zldev, ZL_REG_OUTPUT_WIDTH, new_div); + if (rc) + return rc; + + /* Compute new divisor for N-pin */ + ndiv = (u32)frequency / output_n_freq; + } else { + /* We are going to change frequency of N-pin but if + * the requested freq is greater or equal than freq of P-pin + * in the output pair we cannot compute divisor for the N-pin. + * In this case indicate a failure. + */ + if (output_p_freq <= frequency) + return -EINVAL; + + /* Compute new divisor for N-pin */ + ndiv = output_p_freq / (u32)frequency; + } + + /* Update divisor for the N-pin */ + rc = zl3073x_write_u32(zldev, ZL_REG_OUTPUT_ESYNC_PERIOD, ndiv); + if (rc) + return rc; + + /* For 50/50 duty cycle the divisor is equal to width */ + rc = zl3073x_write_u32(zldev, ZL_REG_OUTPUT_ESYNC_WIDTH, ndiv); + if (rc) + return rc; + + /* Commit output configuration */ + 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, const struct dpll_device *dpll, @@ -611,6 +956,8 @@ zl3073x_dpll_mode_get(const struct dpll_device *dpll, void *dpll_priv, static const struct dpll_pin_ops zl3073x_dpll_input_pin_ops = { .direction_get = zl3073x_dpll_pin_direction_get, + .frequency_get = zl3073x_dpll_input_pin_frequency_get, + .frequency_set = zl3073x_dpll_input_pin_frequency_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, @@ -619,6 +966,8 @@ static const struct dpll_pin_ops zl3073x_dpll_input_pin_ops = { static const struct dpll_pin_ops zl3073x_dpll_output_pin_ops = { .direction_get = zl3073x_dpll_pin_direction_get, + .frequency_get = zl3073x_dpll_output_pin_frequency_get, + .frequency_set = zl3073x_dpll_output_pin_frequency_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 dafe62da81d0..493c63e72920 100644 --- a/drivers/dpll/zl3073x/regs.h +++ b/drivers/dpll/zl3073x/regs.h @@ -137,6 +137,11 @@ #define ZL_REF_MB_SEM_WR BIT(0) #define ZL_REF_MB_SEM_RD BIT(1) +#define ZL_REG_REF_FREQ_BASE ZL_REG(10, 0x05, 2) +#define ZL_REG_REF_FREQ_MULT ZL_REG(10, 0x07, 2) +#define ZL_REG_REF_RATIO_M ZL_REG(10, 0x09, 2) +#define ZL_REG_REF_RATIO_N ZL_REG(10, 0x0b, 2) + #define ZL_REG_REF_CONFIG ZL_REG(10, 0x0d, 1) #define ZL_REF_CONFIG_ENABLE BIT(0) #define ZL_REF_CONFIG_DIFF_EN BIT(2) @@ -195,4 +200,9 @@ #define ZL_OUTPUT_MODE_SIGNAL_FORMAT_2_NDIV 12 #define ZL_OUTPUT_MODE_SIGNAL_FORMAT_2_NDIV_INV 15 +#define ZL_REG_OUTPUT_DIV ZL_REG(14, 0x0c, 4) +#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) + #endif /* _ZL3073X_REGS_H */ |