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