pinctrl: amd: Implement pinmux functionality

Provide pinmux functionality by implementing pinmux_ops.

Signed-off-by: Basavaraj Natikar <Basavaraj.Natikar@amd.com>
Reviewed-by: Andy Shevchenko <andriy.shevchenko@linux.intel.com>
Link: https://lore.kernel.org/r/20220601152900.1012813-7-Basavaraj.Natikar@amd.com
Signed-off-by: Linus Walleij <linus.walleij@linaro.org>

drivers/pinctrl/pinctrl-amd.c

@@ -31,6 +31,7 @@
 #include <linux/bitops.h>
 #include <linux/pinctrl/pinconf.h>
 #include <linux/pinctrl/pinconf-generic.h>
+#include <linux/pinctrl/pinmux.h>
 
 #include "core.h"
 #include "pinctrl-utils.h"
@@ -955,10 +956,87 @@ static const struct dev_pm_ops amd_gpio_pm_ops = {
 };
 #endif
 
+static int amd_get_functions_count(struct pinctrl_dev *pctldev)
+{
+	return ARRAY_SIZE(pmx_functions);
+}
+
+static const char *amd_get_fname(struct pinctrl_dev *pctrldev, unsigned int selector)
+{
+	return pmx_functions[selector].name;
+}
+
+static int amd_get_groups(struct pinctrl_dev *pctrldev, unsigned int selector,
+			  const char * const **groups,
+			  unsigned int * const num_groups)
+{
+	struct amd_gpio *gpio_dev = pinctrl_dev_get_drvdata(pctrldev);
+
+	if (!gpio_dev->iomux_base) {
+		dev_err(&gpio_dev->pdev->dev, "iomux function %d group not supported\n", selector);
+		return -EINVAL;
+	}
+
+	*groups = pmx_functions[selector].groups;
+	*num_groups = pmx_functions[selector].ngroups;
+	return 0;
+}
+
+static int amd_set_mux(struct pinctrl_dev *pctrldev, unsigned int function, unsigned int group)
+{
+	struct amd_gpio *gpio_dev = pinctrl_dev_get_drvdata(pctrldev);
+	struct device *dev = &gpio_dev->pdev->dev;
+	struct pin_desc *pd;
+	int ind, index;
+
+	if (!gpio_dev->iomux_base)
+		return -EINVAL;
+
+	for (index = 0; index < NSELECTS; index++) {
+		if (strcmp(gpio_dev->groups[group].name,  pmx_functions[function].groups[index]))
+			continue;
+
+		if (readb(gpio_dev->iomux_base + pmx_functions[function].index) ==
+				FUNCTION_INVALID) {
+			dev_err(dev, "IOMUX_GPIO 0x%x not present or supported\n",
+				pmx_functions[function].index);
+			return -EINVAL;
+		}
+
+		writeb(index, gpio_dev->iomux_base + pmx_functions[function].index);
+
+		if (index != (readb(gpio_dev->iomux_base + pmx_functions[function].index) &
+					FUNCTION_MASK)) {
+			dev_err(dev, "IOMUX_GPIO 0x%x not present or supported\n",
+				pmx_functions[function].index);
+			return -EINVAL;
+		}
+
+		for (ind = 0; ind < gpio_dev->groups[group].npins; ind++) {
+			if (strncmp(gpio_dev->groups[group].name, "IMX_F", strlen("IMX_F")))
+				continue;
+
+			pd = pin_desc_get(gpio_dev->pctrl, gpio_dev->groups[group].pins[ind]);
+			pd->mux_owner = gpio_dev->groups[group].name;
+		}
+		break;
+	}
+
+	return 0;
+}
+
+static const struct pinmux_ops amd_pmxops = {
+	.get_functions_count = amd_get_functions_count,
+	.get_function_name = amd_get_fname,
+	.get_function_groups = amd_get_groups,
+	.set_mux = amd_set_mux,
+};
+
 static struct pinctrl_desc amd_pinctrl_desc = {
 	.pins	= kerncz_pins,
 	.npins = ARRAY_SIZE(kerncz_pins),
 	.pctlops = &amd_pinctrl_ops,
+	.pmxops = &amd_pmxops,
 	.confops = &amd_pinconf_ops,
 	.owner = THIS_MODULE,
 };