diff --git a/Documentation/netlink/specs/dpll.yaml b/Documentation/netlink/specs/dpll.yaml index 78d0724d7e12..b55afa77eac4 100644 --- a/Documentation/netlink/specs/dpll.yaml +++ b/Documentation/netlink/specs/dpll.yaml @@ -550,6 +550,7 @@ operations: request: attributes: - id + - mode - phase-offset-monitor - phase-offset-avg-factor - diff --git a/drivers/dpll/dpll_netlink.c b/drivers/dpll/dpll_netlink.c index 64944f601ee5..499bca460b1e 100644 --- a/drivers/dpll/dpll_netlink.c +++ b/drivers/dpll/dpll_netlink.c @@ -128,18 +128,29 @@ dpll_msg_add_mode_supported(struct sk_buff *msg, struct dpll_device *dpll, struct netlink_ext_ack *extack) { const struct dpll_device_ops *ops = dpll_device_ops(dpll); + DECLARE_BITMAP(modes, DPLL_MODE_MAX + 1) = { 0 }; enum dpll_mode mode; int ret; - /* No mode change is supported now, so the only supported mode is the - * one obtained by mode_get(). - */ + if (ops->supported_modes_get) { + ret = ops->supported_modes_get(dpll, dpll_priv(dpll), modes, + extack); + if (ret) + return ret; + } else { + /* If the supported modes are not reported by the driver, the + * only supported mode is the one obtained by mode_get(). + */ + ret = ops->mode_get(dpll, dpll_priv(dpll), &mode, extack); + if (ret) + return ret; - ret = ops->mode_get(dpll, dpll_priv(dpll), &mode, extack); - if (ret) - return ret; - if (nla_put_u32(msg, DPLL_A_MODE_SUPPORTED, mode)) - return -EMSGSIZE; + __set_bit(mode, modes); + } + + for_each_set_bit(mode, modes, DPLL_MODE_MAX + 1) + if (nla_put_u32(msg, DPLL_A_MODE_SUPPORTED, mode)) + return -EMSGSIZE; return 0; } @@ -842,6 +853,45 @@ int dpll_pin_change_ntf(struct dpll_pin *pin) } EXPORT_SYMBOL_GPL(dpll_pin_change_ntf); +static int +dpll_mode_set(struct dpll_device *dpll, struct nlattr *a, + struct netlink_ext_ack *extack) +{ + const struct dpll_device_ops *ops = dpll_device_ops(dpll); + DECLARE_BITMAP(modes, DPLL_MODE_MAX + 1) = { 0 }; + enum dpll_mode mode = nla_get_u32(a), old_mode; + int ret; + + if (!(ops->mode_set && ops->supported_modes_get)) { + NL_SET_ERR_MSG_ATTR(extack, a, + "dpll device does not support mode switch"); + return -EOPNOTSUPP; + } + + ret = ops->mode_get(dpll, dpll_priv(dpll), &old_mode, extack); + if (ret) { + NL_SET_ERR_MSG(extack, "unable to get current mode"); + return ret; + } + + if (mode == old_mode) + return 0; + + ret = ops->supported_modes_get(dpll, dpll_priv(dpll), modes, extack); + if (ret) { + NL_SET_ERR_MSG(extack, "unable to get supported modes"); + return ret; + } + + if (!test_bit(mode, modes)) { + NL_SET_ERR_MSG(extack, + "dpll device does not support requested mode"); + return -EINVAL; + } + + return ops->mode_set(dpll, dpll_priv(dpll), mode, extack); +} + static int dpll_phase_offset_monitor_set(struct dpll_device *dpll, struct nlattr *a, struct netlink_ext_ack *extack) @@ -1797,6 +1847,11 @@ dpll_set_from_nlattr(struct dpll_device *dpll, struct genl_info *info) nla_for_each_attr(a, genlmsg_data(info->genlhdr), genlmsg_len(info->genlhdr), rem) { switch (nla_type(a)) { + case DPLL_A_MODE: + ret = dpll_mode_set(dpll, a, info->extack); + if (ret) + return ret; + break; case DPLL_A_PHASE_OFFSET_MONITOR: ret = dpll_phase_offset_monitor_set(dpll, a, info->extack); diff --git a/drivers/dpll/dpll_nl.c b/drivers/dpll/dpll_nl.c index 36d11ff195df..a2b22d492114 100644 --- a/drivers/dpll/dpll_nl.c +++ b/drivers/dpll/dpll_nl.c @@ -45,6 +45,7 @@ static const struct nla_policy dpll_device_get_nl_policy[DPLL_A_ID + 1] = { /* DPLL_CMD_DEVICE_SET - do */ static const struct nla_policy dpll_device_set_nl_policy[DPLL_A_PHASE_OFFSET_AVG_FACTOR + 1] = { [DPLL_A_ID] = { .type = NLA_U32, }, + [DPLL_A_MODE] = NLA_POLICY_RANGE(NLA_U32, 1, 2), [DPLL_A_PHASE_OFFSET_MONITOR] = NLA_POLICY_MAX(NLA_U32, 1), [DPLL_A_PHASE_OFFSET_AVG_FACTOR] = { .type = NLA_U32, }, }; diff --git a/drivers/dpll/zl3073x/dpll.c b/drivers/dpll/zl3073x/dpll.c index 9879d85d29af..7d8ed948b970 100644 --- a/drivers/dpll/zl3073x/dpll.c +++ b/drivers/dpll/zl3073x/dpll.c @@ -100,6 +100,20 @@ zl3073x_dpll_pin_direction_get(const struct dpll_pin *dpll_pin, void *pin_priv, return 0; } +static struct zl3073x_dpll_pin * +zl3073x_dpll_pin_get_by_ref(struct zl3073x_dpll *zldpll, u8 ref_id) +{ + struct zl3073x_dpll_pin *pin; + + list_for_each_entry(pin, &zldpll->pins, list) { + if (zl3073x_dpll_is_input_pin(pin) && + zl3073x_input_pin_ref_get(pin->id) == ref_id) + return pin; + } + + return NULL; +} + static int zl3073x_dpll_input_pin_esync_get(const struct dpll_pin *dpll_pin, void *pin_priv, @@ -1137,6 +1151,26 @@ zl3073x_dpll_lock_status_get(const struct dpll_device *dpll, void *dpll_priv, return 0; } +static int +zl3073x_dpll_supported_modes_get(const struct dpll_device *dpll, + void *dpll_priv, unsigned long *modes, + struct netlink_ext_ack *extack) +{ + struct zl3073x_dpll *zldpll = dpll_priv; + + /* We support switching between automatic and manual mode, except in + * a case where the DPLL channel is configured to run in NCO mode. + * In this case, report only the manual mode to which the NCO is mapped + * as the only supported one. + */ + if (zldpll->refsel_mode != ZL_DPLL_MODE_REFSEL_MODE_NCO) + __set_bit(DPLL_MODE_AUTOMATIC, modes); + + __set_bit(DPLL_MODE_MANUAL, modes); + + return 0; +} + static int zl3073x_dpll_mode_get(const struct dpll_device *dpll, void *dpll_priv, enum dpll_mode *mode, struct netlink_ext_ack *extack) @@ -1217,6 +1251,82 @@ zl3073x_dpll_phase_offset_avg_factor_set(const struct dpll_device *dpll, return 0; } +static int +zl3073x_dpll_mode_set(const struct dpll_device *dpll, void *dpll_priv, + enum dpll_mode mode, struct netlink_ext_ack *extack) +{ + struct zl3073x_dpll *zldpll = dpll_priv; + u8 hw_mode, mode_refsel, ref; + int rc; + + rc = zl3073x_dpll_selected_ref_get(zldpll, &ref); + if (rc) { + NL_SET_ERR_MSG_MOD(extack, "failed to get selected reference"); + return rc; + } + + if (mode == DPLL_MODE_MANUAL) { + /* We are switching from automatic to manual mode: + * - if we have a valid reference selected during auto mode then + * we will switch to forced reference lock mode and use this + * reference for selection + * - if NO valid reference is selected, we will switch to forced + * holdover mode or freerun mode, depending on the current + * lock status + */ + if (ZL3073X_DPLL_REF_IS_VALID(ref)) + hw_mode = ZL_DPLL_MODE_REFSEL_MODE_REFLOCK; + else if (zldpll->lock_status == DPLL_LOCK_STATUS_UNLOCKED) + hw_mode = ZL_DPLL_MODE_REFSEL_MODE_FREERUN; + else + hw_mode = ZL_DPLL_MODE_REFSEL_MODE_HOLDOVER; + } else { + /* We are switching from manual to automatic mode: + * - if there is a valid reference selected then ensure that + * it is selectable after switch to automatic mode + * - switch to automatic mode + */ + struct zl3073x_dpll_pin *pin; + + pin = zl3073x_dpll_pin_get_by_ref(zldpll, ref); + if (pin && !pin->selectable) { + /* Restore pin priority in HW */ + rc = zl3073x_dpll_ref_prio_set(pin, pin->prio); + if (rc) { + NL_SET_ERR_MSG_MOD(extack, + "failed to restore pin priority"); + return rc; + } + + pin->selectable = true; + } + + hw_mode = ZL_DPLL_MODE_REFSEL_MODE_AUTO; + } + + /* Build mode_refsel value */ + mode_refsel = FIELD_PREP(ZL_DPLL_MODE_REFSEL_MODE, hw_mode); + + if (ZL3073X_DPLL_REF_IS_VALID(ref)) + mode_refsel |= FIELD_PREP(ZL_DPLL_MODE_REFSEL_REF, ref); + + /* Update dpll_mode_refsel register */ + rc = zl3073x_write_u8(zldpll->dev, ZL_REG_DPLL_MODE_REFSEL(zldpll->id), + mode_refsel); + if (rc) { + NL_SET_ERR_MSG_MOD(extack, + "failed to set reference selection mode"); + return rc; + } + + zldpll->refsel_mode = hw_mode; + + if (ZL3073X_DPLL_REF_IS_VALID(ref)) + zldpll->forced_ref = ref; + + return 0; +} + static int zl3073x_dpll_phase_offset_monitor_get(const struct dpll_device *dpll, void *dpll_priv, @@ -1276,10 +1386,12 @@ 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, + .mode_set = zl3073x_dpll_mode_set, .phase_offset_avg_factor_get = zl3073x_dpll_phase_offset_avg_factor_get, .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, + .supported_modes_get = zl3073x_dpll_supported_modes_get, }; /** diff --git a/include/linux/dpll.h b/include/linux/dpll.h index 562f520b23c2..c6d0248fa527 100644 --- a/include/linux/dpll.h +++ b/include/linux/dpll.h @@ -20,6 +20,11 @@ struct dpll_pin_esync; struct dpll_device_ops { int (*mode_get)(const struct dpll_device *dpll, void *dpll_priv, enum dpll_mode *mode, struct netlink_ext_ack *extack); + int (*mode_set)(const struct dpll_device *dpll, void *dpll_priv, + enum dpll_mode mode, struct netlink_ext_ack *extack); + int (*supported_modes_get)(const struct dpll_device *dpll, + void *dpll_priv, unsigned long *modes, + struct netlink_ext_ack *extack); int (*lock_status_get)(const struct dpll_device *dpll, void *dpll_priv, enum dpll_lock_status *status, enum dpll_lock_status_error *status_error,