stm32mp157/pinctrl/pinctrl_virtual/pinctrl_virtual_driver.c

#include <linux/module.h>
#include <linux/err.h>
#include <linux/init.h>
#include <linux/io.h>
#include <linux/mfd/syscon.h>
#include <linux/uaccess.h>
#include <linux/of.h>
#include <linux/of_device.h>
#include <linux/of_address.h>
#include <linux/pinctrl/machine.h>
#include <linux/pinctrl/pinconf.h>
#include <linux/pinctrl/pinctrl.h>
#include <linux/pinctrl/pinmux.h>
#include <linux/slab.h>
#include <linux/regmap.h>
#include "core.h"

static struct pinctrl_dev *g_pinctrl_dev;

static const struct pinctrl_pin_desc pins[] = {
    {0, "pin0", NULL},
    {1, "pin1", NULL},
    {2, "pin2", NULL},
    {3, "pin3", NULL},
};

struct virtual_functions_desc {
    const char *name;
    const char **groups;
    int num_groups;
};

static const char *func0_grps[] = {"pin0", "pin1", "pin2", "pin3"};
static const char *func1_grps[] = {"pin0", "pin1"};
static const char *func2_grps[] = {"pin2", "pin3"};

static const struct virtual_functions_desc g_funcs_desc[] = {
    {"gpio", func0_grps, 4},
    {"i2c", func1_grps, 2},
    {"uart", func2_grps, 2},
};

static unsigned long g_configs[4];

static int virtual_pctrl_get_groups_count(struct pinctrl_dev *pctldev)
{
    return pctldev->desc->npins;
}

static const char *virtual_pctrl_get_group_name(struct pinctrl_dev *pctldev, unsigned selector)
{
    return pctldev->desc->pins[selector].name;
}

static int virtual_pctrl_get_group_pins(struct pinctrl_dev *pctldev, unsigned selector, const unsigned **pins, unsigned *npins)
{
    if (selector >= pctldev->desc->npins)
        return -EINVAL;
    *pins = &pctldev->desc->pins[selector].number;
    *npins = 1;
    return 0;
}

static void virtual_pctrl_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s, unsigned offset)
{
    seq_printf(s, "%s\r\n", dev_name(pctldev->dev));
}

static int virtual_pctrl_dt_node_to_map(struct pinctrl_dev *pctldev, struct device_node *np, struct pinctrl_map **map, unsigned *num_maps)
{
    int i;
    int num_pins = 0;
    const char *pin;
    const char *function;
    uint32_t config;
    struct pinctrl_map *new_maps;
    unsigned long *configs;

    while (1) {
        if (of_property_read_string_index(np, "groups", num_pins, &pin))
            break;
        num_pins++;
    }
    new_maps = kmalloc(sizeof(struct pinctrl_map) * num_pins * 2, GFP_KERNEL);
    for (i = 0; i < num_pins; i++) {
        of_property_read_string_index(np, "groups", i, &pin);
        of_property_read_string_index(np, "functions", i, &function);
        of_property_read_u32_index(np, "configs", i, &config);

        configs = kmalloc(sizeof(*configs), GFP_KERNEL);

        new_maps[i * 2].type = PIN_MAP_TYPE_MUX_GROUP;
        new_maps[i * 2].data.mux.function = function;
        new_maps[i * 2].data.mux.group = pin;

        new_maps[i * 2 + 1].type = PIN_MAP_TYPE_CONFIGS_GROUP;
        new_maps[i * 2 + 1].data.configs.group_or_pin = pin;
        new_maps[i * 2 + 1].data.configs.configs = configs;
        configs[0] = config;
        new_maps[i * 2 + 1].data.configs.num_configs = 1;
    }
    *map = new_maps;
    *num_maps = num_pins * 2;
    return 0;
}

static void virtual_pctrl_dt_free_map(struct pinctrl_dev *pctldev, struct pinctrl_map *map, unsigned num_maps)
{
    while (num_maps--) {
        if (map->type == PIN_MAP_TYPE_CONFIGS_PIN)
            kfree(map->data.configs.configs);
        kfree(map);
        map++;
    }
}

static const struct pinctrl_ops virtual_pctrl_ops = {
    .get_groups_count = virtual_pctrl_get_groups_count,
    .get_group_name = virtual_pctrl_get_group_name,
    .get_group_pins = virtual_pctrl_get_group_pins,
    .pin_dbg_show = virtual_pctrl_pin_dbg_show,
    .dt_node_to_map = virtual_pctrl_dt_node_to_map,
    .dt_free_map = virtual_pctrl_dt_free_map,
};

static int virtual_pmx_get_funcs_count(struct pinctrl_dev *pctldev)
{
    return ARRAY_SIZE(g_funcs_desc);
}

static const char *virtual_pmx_get_func_name(struct pinctrl_dev *pctldev, unsigned selector)
{
    return g_funcs_desc[selector].name;
}

static int virtual_pmx_get_groups(struct pinctrl_dev *pctldev, unsigned selector, const char * const **groups, unsigned * const num_groups)
{
    *groups = g_funcs_desc[selector].groups;
    *num_groups = g_funcs_desc[selector].num_groups;
    return 0;
}

static int virtual_pmx_set_mux(struct pinctrl_dev *pctldev, unsigned selector, unsigned group)
{
    printk("set %s as %s\r\n", pctldev->desc->pins[group].name, g_funcs_desc[selector].name);
    return 0;
}

static const struct pinmux_ops virtual_pmx_ops = {
    .get_functions_count = virtual_pmx_get_funcs_count,
    .get_function_name = virtual_pmx_get_func_name,
    .get_function_groups = virtual_pmx_get_groups,
    .set_mux = virtual_pmx_set_mux,
    .gpio_set_direction = NULL,
};

static int virtual_pinconf_get(struct pinctrl_dev *pctldev, unsigned pin_id, unsigned long *config)
{
    *config = g_configs[pin_id];
    return 0;
}

static int virtual_pinconf_set(struct pinctrl_dev *pctldev, unsigned pin_id, unsigned long *config, unsigned num_configs)
{
    if (num_configs != 1)
        return -EINVAL;
    g_configs[pin_id] = *config;
    printk("config %s as 0x%lX\r\n", pctldev->desc->pins[pin_id].name, *config);
    return 0;
}

static int virtual_pinconf_group_get(struct pinctrl_dev *pctldev,
                                     unsigned group,
                                     unsigned long *config)
{
    return virtual_pinconf_get(pctldev, group, config);
}

static int virtual_pinconf_group_set(struct pinctrl_dev *pctldev,
                                     unsigned group,
                                     unsigned long *configs,
                                     unsigned num_configs)
{
    int ret = 0;
    unsigned i;
    
    for (i = 0; i < num_configs; i++) {
        ret = virtual_pinconf_set(pctldev, group, &configs[i], 1);
        if (ret)
            break;
    }
    
    printk("config group %s as 0x%lX\n", 
           pctldev->desc->pins[group].name, 
           num_configs > 0 ? configs[0] : 0);
    return ret;
}

static void virtual_pinconf_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s, unsigned pin_id)
{
    seq_printf(s, "0x%lX\r\n", g_configs[pin_id]);
}

static void virtual_pinconf_group_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s, unsigned pin_id)
{
    seq_printf(s, "0x%lX\r\n", g_configs[pin_id]);
}

static const struct pinconf_ops virtual_pinconf_ops = {
    .pin_config_get = virtual_pinconf_get,
    .pin_config_set = virtual_pinconf_set,
    .pin_config_group_get = virtual_pinconf_group_get,
    .pin_config_group_set = virtual_pinconf_group_set,
    .pin_config_dbg_show = virtual_pinconf_dbg_show,
    .pin_config_group_dbg_show = virtual_pinconf_group_dbg_show,
};

static const struct of_device_id pinctrl_virtual_of_match[] = {
    { .compatible = "jzh,pinctrl-virtual", },
    {  },
};

static int pinctrl_virtual_probe(struct platform_device *pdev)
{
    struct pinctrl_desc *pinctrl;

    printk("%s %s %d\r\n", __FILE__, __FUNCTION__, __LINE__);

    pinctrl = devm_kzalloc(&pdev->dev, sizeof(*pinctrl), GFP_KERNEL);
    pinctrl->name = dev_name(&pdev->dev);
    pinctrl->owner = THIS_MODULE;
    pinctrl->pins = pins;
    pinctrl->npins = ARRAY_SIZE(pins);
    pinctrl->pctlops = &virtual_pctrl_ops;
    pinctrl->pmxops = &virtual_pmx_ops;
    pinctrl->confops = &virtual_pinconf_ops;
    g_pinctrl_dev = devm_pinctrl_register(&pdev->dev, pinctrl, NULL);

    return 0;
}

static int pinctrl_virtual_remove(struct platform_device *pdev)
{
    printk("%s %s %d\r\n", __FILE__, __FUNCTION__, __LINE__);
    return 0;
}

static struct platform_driver pinctrl_virtual_driver = {
    .probe = pinctrl_virtual_probe,
    .remove = pinctrl_virtual_remove,
    .driver = {
        .name = "pinctrl-virtual",
        .of_match_table = of_match_ptr(pinctrl_virtual_of_match),
    },
};

static int __init pinctrl_virtual_init(void)
{
    printk("%s %s %d\r\n", __FILE__, __FUNCTION__, __LINE__);
    return platform_driver_register(&pinctrl_virtual_driver);
}

static void __exit pinctrl_virtual_exit(void)
{
    printk("%s %s %d\r\n", __FILE__, __FUNCTION__, __LINE__);
    platform_driver_unregister(&pinctrl_virtual_driver);
}

module_init(pinctrl_virtual_init);
module_exit(pinctrl_virtual_exit);
MODULE_DESCRIPTION("Virtual Pin Control Driver");
MODULE_AUTHOR("jzhgonha@163.com");
MODULE_LICENSE("GPL");