diff --git a/drivers/pinctrl/pinctrl-scmi.c b/drivers/pinctrl/pinctrl-scmi.c index 1bb36ca477b7..1d85a16f300d 100644 --- a/drivers/pinctrl/pinctrl-scmi.c +++ b/drivers/pinctrl/pinctrl-scmi.c @@ -24,6 +24,7 @@ #include "pinctrl-utils.h" #include "core.h" #include "pinconf.h" +#include "pinmux.h" #define DRV_NAME "scmi-pinctrl" @@ -37,8 +38,6 @@ struct scmi_pinctrl { struct scmi_protocol_handle *ph; struct pinctrl_dev *pctldev; struct pinctrl_desc pctl_desc; - struct pinfunction *functions; - unsigned int nr_functions; }; static int pinctrl_scmi_get_groups_count(struct pinctrl_dev *pctldev) @@ -84,86 +83,6 @@ static const struct pinctrl_ops pinctrl_scmi_pinctrl_ops = { #endif }; -static int pinctrl_scmi_get_functions_count(struct pinctrl_dev *pctldev) -{ - struct scmi_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev); - - return pinctrl_ops->count_get(pmx->ph, FUNCTION_TYPE); -} - -static const char *pinctrl_scmi_get_function_name(struct pinctrl_dev *pctldev, - unsigned int selector) -{ - int ret; - const char *name; - struct scmi_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev); - - ret = pinctrl_ops->name_get(pmx->ph, selector, FUNCTION_TYPE, &name); - if (ret) { - dev_err(pmx->dev, "get name failed with err %d", ret); - return NULL; - } - - return name; -} - -static int pinctrl_scmi_get_function_groups(struct pinctrl_dev *pctldev, - unsigned int selector, - const char * const **p_groups, - unsigned int * const p_num_groups) -{ - struct pinfunction *func; - const unsigned int *group_ids; - unsigned int num_groups; - const char **groups; - int ret, i; - struct scmi_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev); - - if (!p_groups || !p_num_groups) - return -EINVAL; - - if (selector >= pmx->nr_functions) - return -EINVAL; - - func = &pmx->functions[selector]; - if (func->ngroups) - goto done; - - ret = pinctrl_ops->function_groups_get(pmx->ph, selector, &num_groups, - &group_ids); - if (ret) { - dev_err(pmx->dev, "Unable to get function groups, err %d", ret); - return ret; - } - if (!num_groups) - return -EINVAL; - - groups = kcalloc(num_groups, sizeof(*groups), GFP_KERNEL); - if (!groups) - return -ENOMEM; - - for (i = 0; i < num_groups; i++) { - groups[i] = pinctrl_scmi_get_group_name(pctldev, group_ids[i]); - if (!groups[i]) { - ret = -EINVAL; - goto err_free; - } - } - - func->ngroups = num_groups; - func->groups = groups; -done: - *p_groups = func->groups; - *p_num_groups = func->ngroups; - - return 0; - -err_free: - kfree(groups); - - return ret; -} - static int pinctrl_scmi_func_set_mux(struct pinctrl_dev *pctldev, unsigned int selector, unsigned int group) { @@ -190,9 +109,9 @@ static int pinctrl_scmi_free(struct pinctrl_dev *pctldev, unsigned int offset) static const struct pinmux_ops pinctrl_scmi_pinmux_ops = { .request = pinctrl_scmi_request, .free = pinctrl_scmi_free, - .get_functions_count = pinctrl_scmi_get_functions_count, - .get_function_name = pinctrl_scmi_get_function_name, - .get_function_groups = pinctrl_scmi_get_function_groups, + .get_functions_count = pinmux_generic_get_function_count, + .get_function_name = pinmux_generic_get_function_name, + .get_function_groups = pinmux_generic_get_function_groups, .set_mux = pinctrl_scmi_func_set_mux, }; @@ -487,6 +406,60 @@ static const struct pinconf_ops pinctrl_scmi_pinconf_ops = { .pin_config_config_dbg_show = pinconf_generic_dump_config, }; +static int pinctrl_scmi_get_functions(struct scmi_pinctrl *pmx) +{ + unsigned int nr_funcs, nr_groups, i, j; + const unsigned int *group_ids; + const char *fname; + const char **gnames; + int ret; + + nr_funcs = pinctrl_ops->count_get(pmx->ph, FUNCTION_TYPE); + + for (i = 0; i < nr_funcs; i++) { + ret = pinctrl_ops->name_get(pmx->ph, i, FUNCTION_TYPE, + &fname); + if (ret) + return ret; + + ret = pinctrl_ops->function_groups_get(pmx->ph, i, + &nr_groups, + &group_ids); + if (ret) + return ret; + + if (!nr_groups) + dev_warn(pmx->dev, + "Function %s contains no groups\n", + fname); + + gnames = devm_kmalloc_array(pmx->dev, nr_groups, + sizeof(*gnames), GFP_KERNEL); + if (!gnames) + return -ENOMEM; + + for (j = 0; j < nr_groups; j++) { + ret = pinctrl_ops->name_get(pmx->ph, group_ids[j], + GROUP_TYPE, &gnames[j]); + if (ret) + return ret; + } + + ret = pinmux_generic_add_function(pmx->pctldev, fname, gnames, + nr_groups, NULL); + if (ret < 0) + return ret; + if (ret != i) { + dev_err(pmx->dev, + "Duplicate function name: %s index: %u selector: %d\n", + fname, i, ret); + return -EINVAL; + } + } + + return 0; +} + static int pinctrl_scmi_get_pins(struct scmi_pinctrl *pmx, struct pinctrl_desc *desc) { @@ -571,11 +544,9 @@ static int scmi_pinctrl_probe(struct scmi_device *sdev) if (ret) return dev_err_probe(dev, ret, "Failed to register pinctrl\n"); - pmx->nr_functions = pinctrl_scmi_get_functions_count(pmx->pctldev); - pmx->functions = devm_kcalloc(dev, pmx->nr_functions, - sizeof(*pmx->functions), GFP_KERNEL); - if (!pmx->functions) - return -ENOMEM; + ret = pinctrl_scmi_get_functions(pmx); + if (ret) + return dev_err_probe(dev, ret, "Failed to register functions\n"); return pinctrl_enable(pmx->pctldev); }