cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

pinctrl-equilibrium.c (23772B)


      1// SPDX-License-Identifier: GPL-2.0
      2/* Copyright (C) 2019 Intel Corporation */
      3
      4#include <linux/gpio/driver.h>
      5#include <linux/module.h>
      6#include <linux/of.h>
      7#include <linux/of_address.h>
      8#include <linux/of_irq.h>
      9#include <linux/pinctrl/pinctrl.h>
     10#include <linux/pinctrl/pinconf.h>
     11#include <linux/pinctrl/pinconf-generic.h>
     12#include <linux/pinctrl/pinmux.h>
     13#include <linux/platform_device.h>
     14#include <linux/property.h>
     15
     16#include "core.h"
     17#include "pinconf.h"
     18#include "pinmux.h"
     19#include "pinctrl-equilibrium.h"
     20
     21#define PIN_NAME_FMT	"io-%d"
     22#define PIN_NAME_LEN	10
     23#define PAD_REG_OFF	0x100
     24
     25static void eqbr_gpio_disable_irq(struct irq_data *d)
     26{
     27	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
     28	struct eqbr_gpio_ctrl *gctrl = gpiochip_get_data(gc);
     29	unsigned int offset = irqd_to_hwirq(d);
     30	unsigned long flags;
     31
     32	raw_spin_lock_irqsave(&gctrl->lock, flags);
     33	writel(BIT(offset), gctrl->membase + GPIO_IRNENCLR);
     34	raw_spin_unlock_irqrestore(&gctrl->lock, flags);
     35}
     36
     37static void eqbr_gpio_enable_irq(struct irq_data *d)
     38{
     39	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
     40	struct eqbr_gpio_ctrl *gctrl = gpiochip_get_data(gc);
     41	unsigned int offset = irqd_to_hwirq(d);
     42	unsigned long flags;
     43
     44	gc->direction_input(gc, offset);
     45	raw_spin_lock_irqsave(&gctrl->lock, flags);
     46	writel(BIT(offset), gctrl->membase + GPIO_IRNRNSET);
     47	raw_spin_unlock_irqrestore(&gctrl->lock, flags);
     48}
     49
     50static void eqbr_gpio_ack_irq(struct irq_data *d)
     51{
     52	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
     53	struct eqbr_gpio_ctrl *gctrl = gpiochip_get_data(gc);
     54	unsigned int offset = irqd_to_hwirq(d);
     55	unsigned long flags;
     56
     57	raw_spin_lock_irqsave(&gctrl->lock, flags);
     58	writel(BIT(offset), gctrl->membase + GPIO_IRNCR);
     59	raw_spin_unlock_irqrestore(&gctrl->lock, flags);
     60}
     61
     62static void eqbr_gpio_mask_ack_irq(struct irq_data *d)
     63{
     64	eqbr_gpio_disable_irq(d);
     65	eqbr_gpio_ack_irq(d);
     66}
     67
     68static inline void eqbr_cfg_bit(void __iomem *addr,
     69				unsigned int offset, unsigned int set)
     70{
     71	if (set)
     72		writel(readl(addr) | BIT(offset), addr);
     73	else
     74		writel(readl(addr) & ~BIT(offset), addr);
     75}
     76
     77static int eqbr_irq_type_cfg(struct gpio_irq_type *type,
     78			     struct eqbr_gpio_ctrl *gctrl,
     79			     unsigned int offset)
     80{
     81	unsigned long flags;
     82
     83	raw_spin_lock_irqsave(&gctrl->lock, flags);
     84	eqbr_cfg_bit(gctrl->membase + GPIO_IRNCFG, offset, type->trig_type);
     85	eqbr_cfg_bit(gctrl->membase + GPIO_EXINTCR1, offset, type->trig_type);
     86	eqbr_cfg_bit(gctrl->membase + GPIO_EXINTCR0, offset, type->logic_type);
     87	raw_spin_unlock_irqrestore(&gctrl->lock, flags);
     88
     89	return 0;
     90}
     91
     92static int eqbr_gpio_set_irq_type(struct irq_data *d, unsigned int type)
     93{
     94	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
     95	struct eqbr_gpio_ctrl *gctrl = gpiochip_get_data(gc);
     96	unsigned int offset = irqd_to_hwirq(d);
     97	struct gpio_irq_type it;
     98
     99	memset(&it, 0, sizeof(it));
    100
    101	if ((type & IRQ_TYPE_SENSE_MASK) == IRQ_TYPE_NONE)
    102		return 0;
    103
    104	switch (type) {
    105	case IRQ_TYPE_EDGE_RISING:
    106		it.trig_type = GPIO_EDGE_TRIG;
    107		it.edge_type = GPIO_SINGLE_EDGE;
    108		it.logic_type = GPIO_POSITIVE_TRIG;
    109		break;
    110
    111	case IRQ_TYPE_EDGE_FALLING:
    112		it.trig_type = GPIO_EDGE_TRIG;
    113		it.edge_type = GPIO_SINGLE_EDGE;
    114		it.logic_type = GPIO_NEGATIVE_TRIG;
    115		break;
    116
    117	case IRQ_TYPE_EDGE_BOTH:
    118		it.trig_type = GPIO_EDGE_TRIG;
    119		it.edge_type = GPIO_BOTH_EDGE;
    120		it.logic_type = GPIO_POSITIVE_TRIG;
    121		break;
    122
    123	case IRQ_TYPE_LEVEL_HIGH:
    124		it.trig_type = GPIO_LEVEL_TRIG;
    125		it.edge_type = GPIO_SINGLE_EDGE;
    126		it.logic_type = GPIO_POSITIVE_TRIG;
    127		break;
    128
    129	case IRQ_TYPE_LEVEL_LOW:
    130		it.trig_type = GPIO_LEVEL_TRIG;
    131		it.edge_type = GPIO_SINGLE_EDGE;
    132		it.logic_type = GPIO_NEGATIVE_TRIG;
    133		break;
    134
    135	default:
    136		return -EINVAL;
    137	}
    138
    139	eqbr_irq_type_cfg(&it, gctrl, offset);
    140	if (it.trig_type == GPIO_EDGE_TRIG)
    141		irq_set_handler_locked(d, handle_edge_irq);
    142	else
    143		irq_set_handler_locked(d, handle_level_irq);
    144
    145	return 0;
    146}
    147
    148static void eqbr_irq_handler(struct irq_desc *desc)
    149{
    150	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
    151	struct eqbr_gpio_ctrl *gctrl = gpiochip_get_data(gc);
    152	struct irq_chip *ic = irq_desc_get_chip(desc);
    153	unsigned long pins, offset;
    154
    155	chained_irq_enter(ic, desc);
    156	pins = readl(gctrl->membase + GPIO_IRNCR);
    157
    158	for_each_set_bit(offset, &pins, gc->ngpio)
    159		generic_handle_domain_irq(gc->irq.domain, offset);
    160
    161	chained_irq_exit(ic, desc);
    162}
    163
    164static int gpiochip_setup(struct device *dev, struct eqbr_gpio_ctrl *gctrl)
    165{
    166	struct gpio_irq_chip *girq;
    167	struct gpio_chip *gc;
    168
    169	gc = &gctrl->chip;
    170	gc->label = gctrl->name;
    171	gc->fwnode = gctrl->fwnode;
    172
    173	if (!fwnode_property_read_bool(gctrl->fwnode, "interrupt-controller")) {
    174		dev_dbg(dev, "gc %s: doesn't act as interrupt controller!\n",
    175			gctrl->name);
    176		return 0;
    177	}
    178
    179	gctrl->ic.name = "gpio_irq";
    180	gctrl->ic.irq_mask = eqbr_gpio_disable_irq;
    181	gctrl->ic.irq_unmask = eqbr_gpio_enable_irq;
    182	gctrl->ic.irq_ack = eqbr_gpio_ack_irq;
    183	gctrl->ic.irq_mask_ack = eqbr_gpio_mask_ack_irq;
    184	gctrl->ic.irq_set_type = eqbr_gpio_set_irq_type;
    185
    186	girq = &gctrl->chip.irq;
    187	girq->chip = &gctrl->ic;
    188	girq->parent_handler = eqbr_irq_handler;
    189	girq->num_parents = 1;
    190	girq->parents = devm_kcalloc(dev, 1, sizeof(*girq->parents), GFP_KERNEL);
    191	if (!girq->parents)
    192		return -ENOMEM;
    193
    194	girq->default_type = IRQ_TYPE_NONE;
    195	girq->handler = handle_bad_irq;
    196	girq->parents[0] = gctrl->virq;
    197
    198	return 0;
    199}
    200
    201static int gpiolib_reg(struct eqbr_pinctrl_drv_data *drvdata)
    202{
    203	struct device *dev = drvdata->dev;
    204	struct eqbr_gpio_ctrl *gctrl;
    205	struct device_node *np;
    206	struct resource res;
    207	int i, ret;
    208
    209	for (i = 0; i < drvdata->nr_gpio_ctrls; i++) {
    210		gctrl = drvdata->gpio_ctrls + i;
    211		np = to_of_node(gctrl->fwnode);
    212
    213		gctrl->name = devm_kasprintf(dev, GFP_KERNEL, "gpiochip%d", i);
    214		if (!gctrl->name)
    215			return -ENOMEM;
    216
    217		if (of_address_to_resource(np, 0, &res)) {
    218			dev_err(dev, "Failed to get GPIO register address\n");
    219			return -ENXIO;
    220		}
    221
    222		gctrl->membase = devm_ioremap_resource(dev, &res);
    223		if (IS_ERR(gctrl->membase))
    224			return PTR_ERR(gctrl->membase);
    225
    226		gctrl->virq = irq_of_parse_and_map(np, 0);
    227		if (!gctrl->virq) {
    228			dev_err(dev, "%s: failed to parse and map irq\n",
    229				gctrl->name);
    230			return -ENXIO;
    231		}
    232		raw_spin_lock_init(&gctrl->lock);
    233
    234		ret = bgpio_init(&gctrl->chip, dev, gctrl->bank->nr_pins / 8,
    235				 gctrl->membase + GPIO_IN,
    236				 gctrl->membase + GPIO_OUTSET,
    237				 gctrl->membase + GPIO_OUTCLR,
    238				 gctrl->membase + GPIO_DIR,
    239				 NULL, 0);
    240		if (ret) {
    241			dev_err(dev, "unable to init generic GPIO\n");
    242			return ret;
    243		}
    244
    245		ret = gpiochip_setup(dev, gctrl);
    246		if (ret)
    247			return ret;
    248
    249		ret = devm_gpiochip_add_data(dev, &gctrl->chip, gctrl);
    250		if (ret)
    251			return ret;
    252	}
    253
    254	return 0;
    255}
    256
    257static inline struct eqbr_pin_bank
    258*find_pinbank_via_pin(struct eqbr_pinctrl_drv_data *pctl, unsigned int pin)
    259{
    260	struct eqbr_pin_bank *bank;
    261	int i;
    262
    263	for (i = 0; i < pctl->nr_banks; i++) {
    264		bank = &pctl->pin_banks[i];
    265		if (pin >= bank->pin_base &&
    266		    (pin - bank->pin_base) < bank->nr_pins)
    267			return bank;
    268	}
    269
    270	return NULL;
    271}
    272
    273static const struct pinctrl_ops eqbr_pctl_ops = {
    274	.get_groups_count	= pinctrl_generic_get_group_count,
    275	.get_group_name		= pinctrl_generic_get_group_name,
    276	.get_group_pins		= pinctrl_generic_get_group_pins,
    277	.dt_node_to_map		= pinconf_generic_dt_node_to_map_all,
    278	.dt_free_map		= pinconf_generic_dt_free_map,
    279};
    280
    281static int eqbr_set_pin_mux(struct eqbr_pinctrl_drv_data *pctl,
    282			    unsigned int pmx, unsigned int pin)
    283{
    284	struct eqbr_pin_bank *bank;
    285	unsigned long flags;
    286	unsigned int offset;
    287	void __iomem *mem;
    288
    289	bank = find_pinbank_via_pin(pctl, pin);
    290	if (!bank) {
    291		dev_err(pctl->dev, "Couldn't find pin bank for pin %u\n", pin);
    292		return -ENODEV;
    293	}
    294	mem = bank->membase;
    295	offset = pin - bank->pin_base;
    296
    297	if (!(bank->aval_pinmap & BIT(offset))) {
    298		dev_err(pctl->dev,
    299			"PIN: %u is not valid, pinbase: %u, bitmap: %u\n",
    300			pin, bank->pin_base, bank->aval_pinmap);
    301		return -ENODEV;
    302	}
    303
    304	raw_spin_lock_irqsave(&pctl->lock, flags);
    305	writel(pmx, mem + (offset * 4));
    306	raw_spin_unlock_irqrestore(&pctl->lock, flags);
    307	return 0;
    308}
    309
    310static int eqbr_pinmux_set_mux(struct pinctrl_dev *pctldev,
    311			       unsigned int selector, unsigned int group)
    312{
    313	struct eqbr_pinctrl_drv_data *pctl = pinctrl_dev_get_drvdata(pctldev);
    314	struct function_desc *func;
    315	struct group_desc *grp;
    316	unsigned int *pinmux;
    317	int i;
    318
    319	func = pinmux_generic_get_function(pctldev, selector);
    320	if (!func)
    321		return -EINVAL;
    322
    323	grp = pinctrl_generic_get_group(pctldev, group);
    324	if (!grp)
    325		return -EINVAL;
    326
    327	pinmux = grp->data;
    328	for (i = 0; i < grp->num_pins; i++)
    329		eqbr_set_pin_mux(pctl, pinmux[i], grp->pins[i]);
    330
    331	return 0;
    332}
    333
    334static int eqbr_pinmux_gpio_request(struct pinctrl_dev *pctldev,
    335				    struct pinctrl_gpio_range *range,
    336				    unsigned int pin)
    337{
    338	struct eqbr_pinctrl_drv_data *pctl = pinctrl_dev_get_drvdata(pctldev);
    339
    340	return eqbr_set_pin_mux(pctl, EQBR_GPIO_MODE, pin);
    341}
    342
    343static const struct pinmux_ops eqbr_pinmux_ops = {
    344	.get_functions_count	= pinmux_generic_get_function_count,
    345	.get_function_name	= pinmux_generic_get_function_name,
    346	.get_function_groups	= pinmux_generic_get_function_groups,
    347	.set_mux		= eqbr_pinmux_set_mux,
    348	.gpio_request_enable	= eqbr_pinmux_gpio_request,
    349	.strict			= true,
    350};
    351
    352static int get_drv_cur(void __iomem *mem, unsigned int offset)
    353{
    354	unsigned int idx = offset / DRV_CUR_PINS; /* 0-15, 16-31 per register*/
    355	unsigned int pin_offset = offset % DRV_CUR_PINS;
    356
    357	return PARSE_DRV_CURRENT(readl(mem + REG_DRCC(idx)), pin_offset);
    358}
    359
    360static struct eqbr_gpio_ctrl
    361*get_gpio_ctrls_via_bank(struct eqbr_pinctrl_drv_data *pctl,
    362			struct eqbr_pin_bank *bank)
    363{
    364	int i;
    365
    366	for (i = 0; i < pctl->nr_gpio_ctrls; i++) {
    367		if (pctl->gpio_ctrls[i].bank == bank)
    368			return &pctl->gpio_ctrls[i];
    369	}
    370
    371	return NULL;
    372}
    373
    374static int eqbr_pinconf_get(struct pinctrl_dev *pctldev, unsigned int pin,
    375			    unsigned long *config)
    376{
    377	struct eqbr_pinctrl_drv_data *pctl = pinctrl_dev_get_drvdata(pctldev);
    378	enum pin_config_param param = pinconf_to_config_param(*config);
    379	struct eqbr_gpio_ctrl *gctrl;
    380	struct eqbr_pin_bank *bank;
    381	unsigned long flags;
    382	unsigned int offset;
    383	void __iomem *mem;
    384	u32 val;
    385
    386	bank = find_pinbank_via_pin(pctl, pin);
    387	if (!bank) {
    388		dev_err(pctl->dev, "Couldn't find pin bank for pin %u\n", pin);
    389		return -ENODEV;
    390	}
    391	mem = bank->membase;
    392	offset = pin - bank->pin_base;
    393
    394	if (!(bank->aval_pinmap & BIT(offset))) {
    395		dev_err(pctl->dev,
    396			"PIN: %u is not valid, pinbase: %u, bitmap: %u\n",
    397			pin, bank->pin_base, bank->aval_pinmap);
    398		return -ENODEV;
    399	}
    400
    401	raw_spin_lock_irqsave(&pctl->lock, flags);
    402	switch (param) {
    403	case PIN_CONFIG_BIAS_PULL_UP:
    404		val = !!(readl(mem + REG_PUEN) & BIT(offset));
    405		break;
    406	case PIN_CONFIG_BIAS_PULL_DOWN:
    407		val = !!(readl(mem + REG_PDEN) & BIT(offset));
    408		break;
    409	case PIN_CONFIG_DRIVE_OPEN_DRAIN:
    410		val = !!(readl(mem + REG_OD) & BIT(offset));
    411		break;
    412	case PIN_CONFIG_DRIVE_STRENGTH:
    413		val = get_drv_cur(mem, offset);
    414		break;
    415	case PIN_CONFIG_SLEW_RATE:
    416		val = !!(readl(mem + REG_SRC) & BIT(offset));
    417		break;
    418	case PIN_CONFIG_OUTPUT_ENABLE:
    419		gctrl = get_gpio_ctrls_via_bank(pctl, bank);
    420		if (!gctrl) {
    421			dev_err(pctl->dev, "Failed to find gpio via bank pinbase: %u, pin: %u\n",
    422				bank->pin_base, pin);
    423			raw_spin_unlock_irqrestore(&pctl->lock, flags);
    424			return -ENODEV;
    425		}
    426		val = !!(readl(gctrl->membase + GPIO_DIR) & BIT(offset));
    427		break;
    428	default:
    429		raw_spin_unlock_irqrestore(&pctl->lock, flags);
    430		return -ENOTSUPP;
    431	}
    432	raw_spin_unlock_irqrestore(&pctl->lock, flags);
    433	*config = pinconf_to_config_packed(param, val);
    434;
    435	return 0;
    436}
    437
    438static int eqbr_pinconf_set(struct pinctrl_dev *pctldev, unsigned int pin,
    439			    unsigned long *configs, unsigned int num_configs)
    440{
    441	struct eqbr_pinctrl_drv_data *pctl = pinctrl_dev_get_drvdata(pctldev);
    442	struct eqbr_gpio_ctrl *gctrl;
    443	enum pin_config_param param;
    444	struct eqbr_pin_bank *bank;
    445	unsigned int val, offset;
    446	struct gpio_chip *gc;
    447	unsigned long flags;
    448	void __iomem *mem;
    449	u32 regval, mask;
    450	int i;
    451
    452	for (i = 0; i < num_configs; i++) {
    453		param = pinconf_to_config_param(configs[i]);
    454		val = pinconf_to_config_argument(configs[i]);
    455
    456		bank = find_pinbank_via_pin(pctl, pin);
    457		if (!bank) {
    458			dev_err(pctl->dev,
    459				"Couldn't find pin bank for pin %u\n", pin);
    460			return -ENODEV;
    461		}
    462		mem = bank->membase;
    463		offset = pin - bank->pin_base;
    464
    465		switch (param) {
    466		case PIN_CONFIG_BIAS_PULL_UP:
    467			mem += REG_PUEN;
    468			mask = BIT(offset);
    469			break;
    470		case PIN_CONFIG_BIAS_PULL_DOWN:
    471			mem += REG_PDEN;
    472			mask = BIT(offset);
    473			break;
    474		case PIN_CONFIG_DRIVE_OPEN_DRAIN:
    475			mem += REG_OD;
    476			mask = BIT(offset);
    477			break;
    478		case PIN_CONFIG_DRIVE_STRENGTH:
    479			mem += REG_DRCC(offset / DRV_CUR_PINS);
    480			offset = (offset % DRV_CUR_PINS) * 2;
    481			mask = GENMASK(1, 0) << offset;
    482			break;
    483		case PIN_CONFIG_SLEW_RATE:
    484			mem += REG_SRC;
    485			mask = BIT(offset);
    486			break;
    487		case PIN_CONFIG_OUTPUT_ENABLE:
    488			gctrl = get_gpio_ctrls_via_bank(pctl, bank);
    489			if (!gctrl) {
    490				dev_err(pctl->dev, "Failed to find gpio via bank pinbase: %u, pin: %u\n",
    491					bank->pin_base, pin);
    492				return -ENODEV;
    493			}
    494			gc = &gctrl->chip;
    495			gc->direction_output(gc, offset, 0);
    496			continue;
    497		default:
    498			return -ENOTSUPP;
    499		}
    500
    501		raw_spin_lock_irqsave(&pctl->lock, flags);
    502		regval = readl(mem);
    503		regval = (regval & ~mask) | ((val << offset) & mask);
    504		writel(regval, mem);
    505		raw_spin_unlock_irqrestore(&pctl->lock, flags);
    506	}
    507
    508	return 0;
    509}
    510
    511static int eqbr_pinconf_group_get(struct pinctrl_dev *pctldev,
    512				  unsigned int group, unsigned long *config)
    513{
    514	unsigned int i, npins, old = 0;
    515	const unsigned int *pins;
    516	int ret;
    517
    518	ret = pinctrl_generic_get_group_pins(pctldev, group, &pins, &npins);
    519	if (ret)
    520		return ret;
    521
    522	for (i = 0; i < npins; i++) {
    523		if (eqbr_pinconf_get(pctldev, pins[i], config))
    524			return -ENOTSUPP;
    525
    526		if (i && old != *config)
    527			return -ENOTSUPP;
    528
    529		old = *config;
    530	}
    531	return 0;
    532}
    533
    534static int eqbr_pinconf_group_set(struct pinctrl_dev *pctldev,
    535				  unsigned int group, unsigned long *configs,
    536				  unsigned int num_configs)
    537{
    538	const unsigned int *pins;
    539	unsigned int i, npins;
    540	int ret;
    541
    542	ret = pinctrl_generic_get_group_pins(pctldev, group, &pins, &npins);
    543	if (ret)
    544		return ret;
    545
    546	for (i = 0; i < npins; i++) {
    547		ret = eqbr_pinconf_set(pctldev, pins[i], configs, num_configs);
    548		if (ret)
    549			return ret;
    550	}
    551	return 0;
    552}
    553
    554static const struct pinconf_ops eqbr_pinconf_ops = {
    555	.is_generic			= true,
    556	.pin_config_get			= eqbr_pinconf_get,
    557	.pin_config_set			= eqbr_pinconf_set,
    558	.pin_config_group_get		= eqbr_pinconf_group_get,
    559	.pin_config_group_set		= eqbr_pinconf_group_set,
    560	.pin_config_config_dbg_show	= pinconf_generic_dump_config,
    561};
    562
    563static bool is_func_exist(struct eqbr_pmx_func *funcs, const char *name,
    564			 unsigned int nr_funcs, unsigned int *idx)
    565{
    566	int i;
    567
    568	if (!funcs)
    569		return false;
    570
    571	for (i = 0; i < nr_funcs; i++) {
    572		if (funcs[i].name && !strcmp(funcs[i].name, name)) {
    573			*idx = i;
    574			return true;
    575		}
    576	}
    577
    578	return false;
    579}
    580
    581static int funcs_utils(struct device *dev, struct eqbr_pmx_func *funcs,
    582		       unsigned int *nr_funcs, funcs_util_ops op)
    583{
    584	struct device_node *node = dev->of_node;
    585	struct device_node *np;
    586	struct property *prop;
    587	const char *fn_name;
    588	unsigned int fid;
    589	int i, j;
    590
    591	i = 0;
    592	for_each_child_of_node(node, np) {
    593		prop = of_find_property(np, "groups", NULL);
    594		if (!prop)
    595			continue;
    596
    597		if (of_property_read_string(np, "function", &fn_name)) {
    598			/* some groups may not have function, it's OK */
    599			dev_dbg(dev, "Group %s: not function binded!\n",
    600				(char *)prop->value);
    601			continue;
    602		}
    603
    604		switch (op) {
    605		case OP_COUNT_NR_FUNCS:
    606			if (!is_func_exist(funcs, fn_name, *nr_funcs, &fid))
    607				*nr_funcs = *nr_funcs + 1;
    608			break;
    609
    610		case OP_ADD_FUNCS:
    611			if (!is_func_exist(funcs, fn_name, *nr_funcs, &fid))
    612				funcs[i].name = fn_name;
    613			break;
    614
    615		case OP_COUNT_NR_FUNC_GRPS:
    616			if (is_func_exist(funcs, fn_name, *nr_funcs, &fid))
    617				funcs[fid].nr_groups++;
    618			break;
    619
    620		case OP_ADD_FUNC_GRPS:
    621			if (is_func_exist(funcs, fn_name, *nr_funcs, &fid)) {
    622				for (j = 0; j < funcs[fid].nr_groups; j++)
    623					if (!funcs[fid].groups[j])
    624						break;
    625				funcs[fid].groups[j] = prop->value;
    626			}
    627			break;
    628
    629		default:
    630			of_node_put(np);
    631			return -EINVAL;
    632		}
    633		i++;
    634	}
    635
    636	return 0;
    637}
    638
    639static int eqbr_build_functions(struct eqbr_pinctrl_drv_data *drvdata)
    640{
    641	struct device *dev = drvdata->dev;
    642	struct eqbr_pmx_func *funcs = NULL;
    643	unsigned int nr_funcs = 0;
    644	int i, ret;
    645
    646	ret = funcs_utils(dev, funcs, &nr_funcs, OP_COUNT_NR_FUNCS);
    647	if (ret)
    648		return ret;
    649
    650	funcs = devm_kcalloc(dev, nr_funcs, sizeof(*funcs), GFP_KERNEL);
    651	if (!funcs)
    652		return -ENOMEM;
    653
    654	ret = funcs_utils(dev, funcs, &nr_funcs, OP_ADD_FUNCS);
    655	if (ret)
    656		return ret;
    657
    658	ret = funcs_utils(dev, funcs, &nr_funcs, OP_COUNT_NR_FUNC_GRPS);
    659	if (ret)
    660		return ret;
    661
    662	for (i = 0; i < nr_funcs; i++) {
    663		if (!funcs[i].nr_groups)
    664			continue;
    665		funcs[i].groups = devm_kcalloc(dev, funcs[i].nr_groups,
    666					       sizeof(*(funcs[i].groups)),
    667					       GFP_KERNEL);
    668		if (!funcs[i].groups)
    669			return -ENOMEM;
    670	}
    671
    672	ret = funcs_utils(dev, funcs, &nr_funcs, OP_ADD_FUNC_GRPS);
    673	if (ret)
    674		return ret;
    675
    676	for (i = 0; i < nr_funcs; i++) {
    677
    678		/* Ignore the same function with multiple groups */
    679		if (funcs[i].name == NULL)
    680			continue;
    681
    682		ret = pinmux_generic_add_function(drvdata->pctl_dev,
    683						  funcs[i].name,
    684						  funcs[i].groups,
    685						  funcs[i].nr_groups,
    686						  drvdata);
    687		if (ret < 0) {
    688			dev_err(dev, "Failed to register function %s\n",
    689				funcs[i].name);
    690			return ret;
    691		}
    692	}
    693
    694	return 0;
    695}
    696
    697static int eqbr_build_groups(struct eqbr_pinctrl_drv_data *drvdata)
    698{
    699	struct device *dev = drvdata->dev;
    700	struct device_node *node = dev->of_node;
    701	unsigned int *pinmux, pin_id, pinmux_id;
    702	struct group_desc group;
    703	struct device_node *np;
    704	struct property *prop;
    705	int j, err;
    706
    707	for_each_child_of_node(node, np) {
    708		prop = of_find_property(np, "groups", NULL);
    709		if (!prop)
    710			continue;
    711
    712		group.num_pins = of_property_count_u32_elems(np, "pins");
    713		if (group.num_pins < 0) {
    714			dev_err(dev, "No pins in the group: %s\n", prop->name);
    715			of_node_put(np);
    716			return -EINVAL;
    717		}
    718		group.name = prop->value;
    719		group.pins = devm_kcalloc(dev, group.num_pins,
    720					  sizeof(*(group.pins)), GFP_KERNEL);
    721		if (!group.pins) {
    722			of_node_put(np);
    723			return -ENOMEM;
    724		}
    725
    726		pinmux = devm_kcalloc(dev, group.num_pins, sizeof(*pinmux),
    727				      GFP_KERNEL);
    728		if (!pinmux) {
    729			of_node_put(np);
    730			return -ENOMEM;
    731		}
    732
    733		for (j = 0; j < group.num_pins; j++) {
    734			if (of_property_read_u32_index(np, "pins", j, &pin_id)) {
    735				dev_err(dev, "Group %s: Read intel pins id failed\n",
    736					group.name);
    737				of_node_put(np);
    738				return -EINVAL;
    739			}
    740			if (pin_id >= drvdata->pctl_desc.npins) {
    741				dev_err(dev, "Group %s: Invalid pin ID, idx: %d, pin %u\n",
    742					group.name, j, pin_id);
    743				of_node_put(np);
    744				return -EINVAL;
    745			}
    746			group.pins[j] = pin_id;
    747			if (of_property_read_u32_index(np, "pinmux", j, &pinmux_id)) {
    748				dev_err(dev, "Group %s: Read intel pinmux id failed\n",
    749					group.name);
    750				of_node_put(np);
    751				return -EINVAL;
    752			}
    753			pinmux[j] = pinmux_id;
    754		}
    755
    756		err = pinctrl_generic_add_group(drvdata->pctl_dev, group.name,
    757						group.pins, group.num_pins,
    758						pinmux);
    759		if (err < 0) {
    760			dev_err(dev, "Failed to register group %s\n", group.name);
    761			of_node_put(np);
    762			return err;
    763		}
    764		memset(&group, 0, sizeof(group));
    765		pinmux = NULL;
    766	}
    767
    768	return 0;
    769}
    770
    771static int pinctrl_reg(struct eqbr_pinctrl_drv_data *drvdata)
    772{
    773	struct pinctrl_desc *pctl_desc;
    774	struct pinctrl_pin_desc *pdesc;
    775	struct device *dev;
    776	unsigned int nr_pins;
    777	char *pin_names;
    778	int i, ret;
    779
    780	dev = drvdata->dev;
    781	pctl_desc = &drvdata->pctl_desc;
    782	pctl_desc->name = "eqbr-pinctrl";
    783	pctl_desc->owner = THIS_MODULE;
    784	pctl_desc->pctlops = &eqbr_pctl_ops;
    785	pctl_desc->pmxops = &eqbr_pinmux_ops;
    786	pctl_desc->confops = &eqbr_pinconf_ops;
    787	raw_spin_lock_init(&drvdata->lock);
    788
    789	for (i = 0, nr_pins = 0; i < drvdata->nr_banks; i++)
    790		nr_pins += drvdata->pin_banks[i].nr_pins;
    791
    792	pdesc = devm_kcalloc(dev, nr_pins, sizeof(*pdesc), GFP_KERNEL);
    793	if (!pdesc)
    794		return -ENOMEM;
    795	pin_names = devm_kcalloc(dev, nr_pins, PIN_NAME_LEN, GFP_KERNEL);
    796	if (!pin_names)
    797		return -ENOMEM;
    798
    799	for (i = 0; i < nr_pins; i++) {
    800		sprintf(pin_names, PIN_NAME_FMT, i);
    801		pdesc[i].number = i;
    802		pdesc[i].name = pin_names;
    803		pin_names += PIN_NAME_LEN;
    804	}
    805	pctl_desc->pins = pdesc;
    806	pctl_desc->npins = nr_pins;
    807	dev_dbg(dev, "pinctrl total pin number: %u\n", nr_pins);
    808
    809	ret = devm_pinctrl_register_and_init(dev, pctl_desc, drvdata,
    810					     &drvdata->pctl_dev);
    811	if (ret)
    812		return ret;
    813
    814	ret = eqbr_build_groups(drvdata);
    815	if (ret) {
    816		dev_err(dev, "Failed to build groups\n");
    817		return ret;
    818	}
    819
    820	ret = eqbr_build_functions(drvdata);
    821	if (ret) {
    822		dev_err(dev, "Failed to build functions\n");
    823		return ret;
    824	}
    825
    826	return pinctrl_enable(drvdata->pctl_dev);
    827}
    828
    829static int pinbank_init(struct device_node *np,
    830			struct eqbr_pinctrl_drv_data *drvdata,
    831			struct eqbr_pin_bank *bank, unsigned int id)
    832{
    833	struct device *dev = drvdata->dev;
    834	struct of_phandle_args spec;
    835	int ret;
    836
    837	bank->membase = drvdata->membase + id * PAD_REG_OFF;
    838
    839	ret = of_parse_phandle_with_fixed_args(np, "gpio-ranges", 3, 0, &spec);
    840	if (ret) {
    841		dev_err(dev, "gpio-range not available!\n");
    842		return ret;
    843	}
    844
    845	bank->pin_base = spec.args[1];
    846	bank->nr_pins = spec.args[2];
    847
    848	bank->aval_pinmap = readl(bank->membase + REG_AVAIL);
    849	bank->id = id;
    850
    851	dev_dbg(dev, "pinbank id: %d, reg: %px, pinbase: %u, pin number: %u, pinmap: 0x%x\n",
    852		id, bank->membase, bank->pin_base,
    853		bank->nr_pins, bank->aval_pinmap);
    854
    855	return ret;
    856}
    857
    858static int pinbank_probe(struct eqbr_pinctrl_drv_data *drvdata)
    859{
    860	struct device *dev = drvdata->dev;
    861	struct device_node *np_gpio;
    862	struct eqbr_gpio_ctrl *gctrls;
    863	struct eqbr_pin_bank *banks;
    864	int i, nr_gpio;
    865
    866	/* Count gpio bank number */
    867	nr_gpio = 0;
    868	for_each_node_by_name(np_gpio, "gpio") {
    869		if (of_device_is_available(np_gpio))
    870			nr_gpio++;
    871	}
    872
    873	if (!nr_gpio) {
    874		dev_err(dev, "NO pin bank available!\n");
    875		return -ENODEV;
    876	}
    877
    878	/* Count pin bank number and gpio controller number */
    879	banks = devm_kcalloc(dev, nr_gpio, sizeof(*banks), GFP_KERNEL);
    880	if (!banks)
    881		return -ENOMEM;
    882
    883	gctrls = devm_kcalloc(dev, nr_gpio, sizeof(*gctrls), GFP_KERNEL);
    884	if (!gctrls)
    885		return -ENOMEM;
    886
    887	dev_dbg(dev, "found %d gpio controller!\n", nr_gpio);
    888
    889	/* Initialize Pin bank */
    890	i = 0;
    891	for_each_node_by_name(np_gpio, "gpio") {
    892		if (!of_device_is_available(np_gpio))
    893			continue;
    894
    895		pinbank_init(np_gpio, drvdata, banks + i, i);
    896
    897		gctrls[i].fwnode = of_fwnode_handle(np_gpio);
    898		gctrls[i].bank = banks + i;
    899		i++;
    900	}
    901
    902	drvdata->pin_banks = banks;
    903	drvdata->nr_banks = nr_gpio;
    904	drvdata->gpio_ctrls = gctrls;
    905	drvdata->nr_gpio_ctrls = nr_gpio;
    906
    907	return 0;
    908}
    909
    910static int eqbr_pinctrl_probe(struct platform_device *pdev)
    911{
    912	struct eqbr_pinctrl_drv_data *drvdata;
    913	struct device *dev = &pdev->dev;
    914	int ret;
    915
    916	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
    917	if (!drvdata)
    918		return -ENOMEM;
    919
    920	drvdata->dev = dev;
    921
    922	drvdata->membase = devm_platform_ioremap_resource(pdev, 0);
    923	if (IS_ERR(drvdata->membase))
    924		return PTR_ERR(drvdata->membase);
    925
    926	ret = pinbank_probe(drvdata);
    927	if (ret)
    928		return ret;
    929
    930	ret = pinctrl_reg(drvdata);
    931	if (ret)
    932		return ret;
    933
    934	ret = gpiolib_reg(drvdata);
    935	if (ret)
    936		return ret;
    937
    938	platform_set_drvdata(pdev, drvdata);
    939	return 0;
    940}
    941
    942static const struct of_device_id eqbr_pinctrl_dt_match[] = {
    943	{ .compatible = "intel,lgm-io" },
    944	{}
    945};
    946MODULE_DEVICE_TABLE(of, eqbr_pinctrl_dt_match);
    947
    948static struct platform_driver eqbr_pinctrl_driver = {
    949	.probe	= eqbr_pinctrl_probe,
    950	.driver = {
    951		.name = "eqbr-pinctrl",
    952		.of_match_table = eqbr_pinctrl_dt_match,
    953	},
    954};
    955
    956module_platform_driver(eqbr_pinctrl_driver);
    957
    958MODULE_AUTHOR("Zhu Yixin <yixin.zhu@intel.com>, Rahul Tanwar <rahul.tanwar@intel.com>");
    959MODULE_DESCRIPTION("Pinctrl Driver for LGM SoC (Equilibrium)");
    960MODULE_LICENSE("GPL v2");