1 /*
2  * SPEAr platform PLGPIO driver
3  *
4  * Copyright (C) 2012 ST Microelectronics
5  * Viresh Kumar <viresh.kumar@linaro.org>
6  *
7  * This file is licensed under the terms of the GNU General Public
8  * License version 2. This program is licensed "as is" without any
9  * warranty of any kind, whether express or implied.
10  */
11 
12 #include <linux/clk.h>
13 #include <linux/err.h>
14 #include <linux/gpio/driver.h>
15 #include <linux/io.h>
16 #include <linux/init.h>
17 #include <linux/mfd/syscon.h>
18 #include <linux/of.h>
19 #include <linux/of_platform.h>
20 #include <linux/pinctrl/consumer.h>
21 #include <linux/platform_device.h>
22 #include <linux/pm.h>
23 #include <linux/regmap.h>
24 #include <linux/spinlock.h>
25 
26 #define MAX_GPIO_PER_REG		32
27 #define PIN_OFFSET(pin)			(pin % MAX_GPIO_PER_REG)
28 #define REG_OFFSET(base, reg, pin)	(base + reg + (pin / MAX_GPIO_PER_REG) \
29 							* sizeof(int *))
30 
31 /*
32  * plgpio pins in all machines are not one to one mapped, bitwise with registers
33  * bits. These set of macros define register masks for which below functions
34  * (pin_to_offset and offset_to_pin) are required to be called.
35  */
36 #define PTO_ENB_REG		0x001
37 #define PTO_WDATA_REG		0x002
38 #define PTO_DIR_REG		0x004
39 #define PTO_IE_REG		0x008
40 #define PTO_RDATA_REG		0x010
41 #define PTO_MIS_REG		0x020
42 
43 struct plgpio_regs {
44 	u32 enb;		/* enable register */
45 	u32 wdata;		/* write data register */
46 	u32 dir;		/* direction set register */
47 	u32 rdata;		/* read data register */
48 	u32 ie;			/* interrupt enable register */
49 	u32 mis;		/* mask interrupt status register */
50 	u32 eit;		/* edge interrupt type */
51 };
52 
53 /*
54  * struct plgpio: plgpio driver specific structure
55  *
56  * lock: lock for guarding gpio registers
57  * base: base address of plgpio block
58  * chip: gpio framework specific chip information structure
59  * p2o: function ptr for pin to offset conversion. This is required only for
60  *	machines where mapping b/w pin and offset is not 1-to-1.
61  * o2p: function ptr for offset to pin conversion. This is required only for
62  *	machines where mapping b/w pin and offset is not 1-to-1.
63  * p2o_regs: mask of registers for which p2o and o2p are applicable
64  * regs: register offsets
65  * csave_regs: context save registers for standby/sleep/hibernate cases
66  */
67 struct plgpio {
68 	spinlock_t		lock;
69 	struct regmap		*regmap;
70 	struct clk		*clk;
71 	struct gpio_chip	chip;
72 	int			(*p2o)(int pin);	/* pin_to_offset */
73 	int			(*o2p)(int offset);	/* offset_to_pin */
74 	u32			p2o_regs;
75 	struct plgpio_regs	regs;
76 #ifdef CONFIG_PM_SLEEP
77 	struct plgpio_regs	*csave_regs;
78 #endif
79 };
80 
81 /* register manipulation inline functions */
82 static inline u32 is_plgpio_set(struct regmap *regmap, u32 pin, u32 reg)
83 {
84 	u32 offset = PIN_OFFSET(pin);
85 	u32 reg_off = REG_OFFSET(0, reg, pin);
86 	u32 val;
87 
88 	regmap_read(regmap, reg_off, &val);
89 
90 	return !!(val & (1 << offset));
91 }
92 
93 static inline void plgpio_reg_set(struct regmap *regmap, u32 pin, u32 reg)
94 {
95 	u32 offset = PIN_OFFSET(pin);
96 	u32 reg_off = REG_OFFSET(0, reg, pin);
97 	u32 mask;
98 
99 	mask = 1 << offset;
100 	regmap_update_bits(regmap, reg_off, mask, mask);
101 }
102 
103 static inline void plgpio_reg_reset(struct regmap *regmap, u32 pin, u32 reg)
104 {
105 	u32 offset = PIN_OFFSET(pin);
106 	u32 reg_off = REG_OFFSET(0, reg, pin);
107 	u32 mask;
108 
109 	mask = 1 << offset;
110 	regmap_update_bits(regmap, reg_off, mask, 0);
111 }
112 
113 
114 /* gpio framework specific routines */
115 static int plgpio_direction_input(struct gpio_chip *chip, unsigned offset)
116 {
117 	struct plgpio *plgpio = gpiochip_get_data(chip);
118 	unsigned long flags;
119 
120 	/* get correct offset for "offset" pin */
121 	if (plgpio->p2o && (plgpio->p2o_regs & PTO_DIR_REG)) {
122 		offset = plgpio->p2o(offset);
123 		if (offset == -1)
124 			return -EINVAL;
125 	}
126 
127 	spin_lock_irqsave(&plgpio->lock, flags);
128 	plgpio_reg_set(plgpio->regmap, offset, plgpio->regs.dir);
129 	spin_unlock_irqrestore(&plgpio->lock, flags);
130 
131 	return 0;
132 }
133 
134 static int plgpio_direction_output(struct gpio_chip *chip, unsigned offset,
135 		int value)
136 {
137 	struct plgpio *plgpio = gpiochip_get_data(chip);
138 	unsigned long flags;
139 	unsigned dir_offset = offset, wdata_offset = offset, tmp;
140 
141 	/* get correct offset for "offset" pin */
142 	if (plgpio->p2o && (plgpio->p2o_regs & (PTO_DIR_REG | PTO_WDATA_REG))) {
143 		tmp = plgpio->p2o(offset);
144 		if (tmp == -1)
145 			return -EINVAL;
146 
147 		if (plgpio->p2o_regs & PTO_DIR_REG)
148 			dir_offset = tmp;
149 		if (plgpio->p2o_regs & PTO_WDATA_REG)
150 			wdata_offset = tmp;
151 	}
152 
153 	spin_lock_irqsave(&plgpio->lock, flags);
154 	if (value)
155 		plgpio_reg_set(plgpio->regmap, wdata_offset,
156 				plgpio->regs.wdata);
157 	else
158 		plgpio_reg_reset(plgpio->regmap, wdata_offset,
159 				plgpio->regs.wdata);
160 
161 	plgpio_reg_reset(plgpio->regmap, dir_offset, plgpio->regs.dir);
162 	spin_unlock_irqrestore(&plgpio->lock, flags);
163 
164 	return 0;
165 }
166 
167 static int plgpio_get_value(struct gpio_chip *chip, unsigned offset)
168 {
169 	struct plgpio *plgpio = gpiochip_get_data(chip);
170 
171 	if (offset >= chip->ngpio)
172 		return -EINVAL;
173 
174 	/* get correct offset for "offset" pin */
175 	if (plgpio->p2o && (plgpio->p2o_regs & PTO_RDATA_REG)) {
176 		offset = plgpio->p2o(offset);
177 		if (offset == -1)
178 			return -EINVAL;
179 	}
180 
181 	return is_plgpio_set(plgpio->regmap, offset, plgpio->regs.rdata);
182 }
183 
184 static void plgpio_set_value(struct gpio_chip *chip, unsigned offset, int value)
185 {
186 	struct plgpio *plgpio = gpiochip_get_data(chip);
187 
188 	if (offset >= chip->ngpio)
189 		return;
190 
191 	/* get correct offset for "offset" pin */
192 	if (plgpio->p2o && (plgpio->p2o_regs & PTO_WDATA_REG)) {
193 		offset = plgpio->p2o(offset);
194 		if (offset == -1)
195 			return;
196 	}
197 
198 	if (value)
199 		plgpio_reg_set(plgpio->regmap, offset, plgpio->regs.wdata);
200 	else
201 		plgpio_reg_reset(plgpio->regmap, offset, plgpio->regs.wdata);
202 }
203 
204 static int plgpio_request(struct gpio_chip *chip, unsigned offset)
205 {
206 	struct plgpio *plgpio = gpiochip_get_data(chip);
207 	int gpio = chip->base + offset;
208 	unsigned long flags;
209 	int ret = 0;
210 
211 	if (offset >= chip->ngpio)
212 		return -EINVAL;
213 
214 	ret = pinctrl_gpio_request(gpio);
215 	if (ret)
216 		return ret;
217 
218 	if (!IS_ERR(plgpio->clk)) {
219 		ret = clk_enable(plgpio->clk);
220 		if (ret)
221 			goto err0;
222 	}
223 
224 	if (plgpio->regs.enb == -1)
225 		return 0;
226 
227 	/*
228 	 * put gpio in IN mode before enabling it. This make enabling gpio safe
229 	 */
230 	ret = plgpio_direction_input(chip, offset);
231 	if (ret)
232 		goto err1;
233 
234 	/* get correct offset for "offset" pin */
235 	if (plgpio->p2o && (plgpio->p2o_regs & PTO_ENB_REG)) {
236 		offset = plgpio->p2o(offset);
237 		if (offset == -1) {
238 			ret = -EINVAL;
239 			goto err1;
240 		}
241 	}
242 
243 	spin_lock_irqsave(&plgpio->lock, flags);
244 	plgpio_reg_set(plgpio->regmap, offset, plgpio->regs.enb);
245 	spin_unlock_irqrestore(&plgpio->lock, flags);
246 	return 0;
247 
248 err1:
249 	if (!IS_ERR(plgpio->clk))
250 		clk_disable(plgpio->clk);
251 err0:
252 	pinctrl_gpio_free(gpio);
253 	return ret;
254 }
255 
256 static void plgpio_free(struct gpio_chip *chip, unsigned offset)
257 {
258 	struct plgpio *plgpio = gpiochip_get_data(chip);
259 	int gpio = chip->base + offset;
260 	unsigned long flags;
261 
262 	if (offset >= chip->ngpio)
263 		return;
264 
265 	if (plgpio->regs.enb == -1)
266 		goto disable_clk;
267 
268 	/* get correct offset for "offset" pin */
269 	if (plgpio->p2o && (plgpio->p2o_regs & PTO_ENB_REG)) {
270 		offset = plgpio->p2o(offset);
271 		if (offset == -1)
272 			return;
273 	}
274 
275 	spin_lock_irqsave(&plgpio->lock, flags);
276 	plgpio_reg_reset(plgpio->regmap, offset, plgpio->regs.enb);
277 	spin_unlock_irqrestore(&plgpio->lock, flags);
278 
279 disable_clk:
280 	if (!IS_ERR(plgpio->clk))
281 		clk_disable(plgpio->clk);
282 
283 	pinctrl_gpio_free(gpio);
284 }
285 
286 /* PLGPIO IRQ */
287 static void plgpio_irq_disable(struct irq_data *d)
288 {
289 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
290 	struct plgpio *plgpio = gpiochip_get_data(gc);
291 	int offset = d->hwirq;
292 	unsigned long flags;
293 
294 	/* get correct offset for "offset" pin */
295 	if (plgpio->p2o && (plgpio->p2o_regs & PTO_IE_REG)) {
296 		offset = plgpio->p2o(offset);
297 		if (offset == -1)
298 			return;
299 	}
300 
301 	spin_lock_irqsave(&plgpio->lock, flags);
302 	plgpio_reg_set(plgpio->regmap, offset, plgpio->regs.ie);
303 	spin_unlock_irqrestore(&plgpio->lock, flags);
304 	gpiochip_disable_irq(gc, irqd_to_hwirq(d));
305 }
306 
307 static void plgpio_irq_enable(struct irq_data *d)
308 {
309 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
310 	struct plgpio *plgpio = gpiochip_get_data(gc);
311 	int offset = d->hwirq;
312 	unsigned long flags;
313 
314 	/* get correct offset for "offset" pin */
315 	if (plgpio->p2o && (plgpio->p2o_regs & PTO_IE_REG)) {
316 		offset = plgpio->p2o(offset);
317 		if (offset == -1)
318 			return;
319 	}
320 
321 	gpiochip_enable_irq(gc, irqd_to_hwirq(d));
322 	spin_lock_irqsave(&plgpio->lock, flags);
323 	plgpio_reg_reset(plgpio->regmap, offset, plgpio->regs.ie);
324 	spin_unlock_irqrestore(&plgpio->lock, flags);
325 }
326 
327 static int plgpio_irq_set_type(struct irq_data *d, unsigned trigger)
328 {
329 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
330 	struct plgpio *plgpio = gpiochip_get_data(gc);
331 	int offset = d->hwirq;
332 	u32 reg_off;
333 	unsigned int supported_type = 0, val;
334 
335 	if (offset >= plgpio->chip.ngpio)
336 		return -EINVAL;
337 
338 	if (plgpio->regs.eit == -1)
339 		supported_type = IRQ_TYPE_LEVEL_HIGH;
340 	else
341 		supported_type = IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING;
342 
343 	if (!(trigger & supported_type))
344 		return -EINVAL;
345 
346 	if (plgpio->regs.eit == -1)
347 		return 0;
348 
349 	reg_off = REG_OFFSET(0, plgpio->regs.eit, offset);
350 	regmap_read(plgpio->regmap, reg_off, &val);
351 
352 	offset = PIN_OFFSET(offset);
353 	if (trigger & IRQ_TYPE_EDGE_RISING)
354 		regmap_write(plgpio->regmap, reg_off, val | (1 << offset));
355 	else
356 		regmap_write(plgpio->regmap, reg_off, val & ~(1 << offset));
357 
358 	return 0;
359 }
360 
361 static const struct irq_chip plgpio_irqchip = {
362 	.name		= "PLGPIO",
363 	.irq_enable	= plgpio_irq_enable,
364 	.irq_disable	= plgpio_irq_disable,
365 	.irq_set_type	= plgpio_irq_set_type,
366 	.flags		= IRQCHIP_IMMUTABLE,
367 	GPIOCHIP_IRQ_RESOURCE_HELPERS,
368 };
369 
370 static void plgpio_irq_handler(struct irq_desc *desc)
371 {
372 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
373 	struct plgpio *plgpio = gpiochip_get_data(gc);
374 	struct irq_chip *irqchip = irq_desc_get_chip(desc);
375 	int regs_count, count, pin, offset, i = 0;
376 	u32 pending;
377 	unsigned long pendingl;
378 
379 	count = plgpio->chip.ngpio;
380 	regs_count = DIV_ROUND_UP(count, MAX_GPIO_PER_REG);
381 
382 	chained_irq_enter(irqchip, desc);
383 	/* check all plgpio MIS registers for a possible interrupt */
384 	for (; i < regs_count; i++) {
385 		regmap_read(plgpio->regmap, plgpio->regs.mis +
386 			i * sizeof(int *), &pending);
387 		if (!pending)
388 			continue;
389 
390 		/* clear interrupts */
391 		regmap_write(plgpio->regmap, plgpio->regs.mis +
392 			i * sizeof(int *), ~pending);
393 		/*
394 		 * clear extra bits in last register having gpios < MAX/REG
395 		 * ex: Suppose there are max 102 plgpios. then last register
396 		 * must have only (102 - MAX_GPIO_PER_REG * 3) = 6 relevant bits
397 		 * so, we must not take other 28 bits into consideration for
398 		 * checking interrupt. so clear those bits.
399 		 */
400 		count = count - i * MAX_GPIO_PER_REG;
401 		if (count < MAX_GPIO_PER_REG)
402 			pending &= (1 << count) - 1;
403 
404 		pendingl = pending;
405 		for_each_set_bit(offset, &pendingl, MAX_GPIO_PER_REG) {
406 			/* get correct pin for "offset" */
407 			if (plgpio->o2p && (plgpio->p2o_regs & PTO_MIS_REG)) {
408 				pin = plgpio->o2p(offset);
409 				if (pin == -1)
410 					continue;
411 			} else
412 				pin = offset;
413 
414 			/* get correct irq line number */
415 			pin = i * MAX_GPIO_PER_REG + pin;
416 			generic_handle_domain_irq(gc->irq.domain, pin);
417 		}
418 	}
419 	chained_irq_exit(irqchip, desc);
420 }
421 
422 /*
423  * pin to offset and offset to pin converter functions
424  *
425  * In spear310 there is inconsistency among bit positions in plgpio regiseters,
426  * for different plgpio pins. For example: for pin 27, bit offset is 23, pin
427  * 28-33 are not supported, pin 95 has offset bit 95, bit 100 has offset bit 1
428  */
429 static int spear310_p2o(int pin)
430 {
431 	int offset = pin;
432 
433 	if (pin <= 27)
434 		offset += 4;
435 	else if (pin <= 33)
436 		offset = -1;
437 	else if (pin <= 97)
438 		offset -= 2;
439 	else if (pin <= 101)
440 		offset = 101 - pin;
441 	else
442 		offset = -1;
443 
444 	return offset;
445 }
446 
447 static int spear310_o2p(int offset)
448 {
449 	if (offset <= 3)
450 		return 101 - offset;
451 	else if (offset <= 31)
452 		return offset - 4;
453 	else
454 		return offset + 2;
455 }
456 
457 static int plgpio_probe_dt(struct platform_device *pdev, struct plgpio *plgpio)
458 {
459 	struct device_node *np = pdev->dev.of_node;
460 	int ret = -EINVAL;
461 	u32 val;
462 
463 	if (of_machine_is_compatible("st,spear310")) {
464 		plgpio->p2o = spear310_p2o;
465 		plgpio->o2p = spear310_o2p;
466 		plgpio->p2o_regs = PTO_WDATA_REG | PTO_DIR_REG | PTO_IE_REG |
467 			PTO_RDATA_REG | PTO_MIS_REG;
468 	}
469 
470 	if (!of_property_read_u32(np, "st-plgpio,ngpio", &val)) {
471 		plgpio->chip.ngpio = val;
472 	} else {
473 		dev_err(&pdev->dev, "DT: Invalid ngpio field\n");
474 		goto end;
475 	}
476 
477 	if (!of_property_read_u32(np, "st-plgpio,enb-reg", &val))
478 		plgpio->regs.enb = val;
479 	else
480 		plgpio->regs.enb = -1;
481 
482 	if (!of_property_read_u32(np, "st-plgpio,wdata-reg", &val)) {
483 		plgpio->regs.wdata = val;
484 	} else {
485 		dev_err(&pdev->dev, "DT: Invalid wdata reg\n");
486 		goto end;
487 	}
488 
489 	if (!of_property_read_u32(np, "st-plgpio,dir-reg", &val)) {
490 		plgpio->regs.dir = val;
491 	} else {
492 		dev_err(&pdev->dev, "DT: Invalid dir reg\n");
493 		goto end;
494 	}
495 
496 	if (!of_property_read_u32(np, "st-plgpio,ie-reg", &val)) {
497 		plgpio->regs.ie = val;
498 	} else {
499 		dev_err(&pdev->dev, "DT: Invalid ie reg\n");
500 		goto end;
501 	}
502 
503 	if (!of_property_read_u32(np, "st-plgpio,rdata-reg", &val)) {
504 		plgpio->regs.rdata = val;
505 	} else {
506 		dev_err(&pdev->dev, "DT: Invalid rdata reg\n");
507 		goto end;
508 	}
509 
510 	if (!of_property_read_u32(np, "st-plgpio,mis-reg", &val)) {
511 		plgpio->regs.mis = val;
512 	} else {
513 		dev_err(&pdev->dev, "DT: Invalid mis reg\n");
514 		goto end;
515 	}
516 
517 	if (!of_property_read_u32(np, "st-plgpio,eit-reg", &val))
518 		plgpio->regs.eit = val;
519 	else
520 		plgpio->regs.eit = -1;
521 
522 	return 0;
523 
524 end:
525 	return ret;
526 }
527 
528 static int plgpio_probe(struct platform_device *pdev)
529 {
530 	struct device_node *regmap_np;
531 	struct plgpio *plgpio;
532 	int ret, irq;
533 
534 	plgpio = devm_kzalloc(&pdev->dev, sizeof(*plgpio), GFP_KERNEL);
535 	if (!plgpio)
536 		return -ENOMEM;
537 
538 	regmap_np = of_parse_phandle(pdev->dev.of_node, "regmap", 0);
539 	if (regmap_np) {
540 		plgpio->regmap = device_node_to_regmap(regmap_np);
541 		of_node_put(regmap_np);
542 		if (IS_ERR(plgpio->regmap)) {
543 			dev_err(&pdev->dev, "Retrieve regmap failed (%pe)\n",
544 				plgpio->regmap);
545 			return PTR_ERR(plgpio->regmap);
546 		}
547 	} else {
548 		plgpio->regmap = device_node_to_regmap(pdev->dev.of_node);
549 		if (IS_ERR(plgpio->regmap)) {
550 			dev_err(&pdev->dev, "Init regmap failed (%pe)\n",
551 				plgpio->regmap);
552 			return PTR_ERR(plgpio->regmap);
553 		}
554 	}
555 
556 	ret = plgpio_probe_dt(pdev, plgpio);
557 	if (ret) {
558 		dev_err(&pdev->dev, "DT probe failed\n");
559 		return ret;
560 	}
561 
562 	plgpio->clk = devm_clk_get(&pdev->dev, NULL);
563 	if (IS_ERR(plgpio->clk))
564 		dev_warn(&pdev->dev, "clk_get() failed, work without it\n");
565 
566 #ifdef CONFIG_PM_SLEEP
567 	plgpio->csave_regs = devm_kcalloc(&pdev->dev,
568 			DIV_ROUND_UP(plgpio->chip.ngpio, MAX_GPIO_PER_REG),
569 			sizeof(*plgpio->csave_regs),
570 			GFP_KERNEL);
571 	if (!plgpio->csave_regs)
572 		return -ENOMEM;
573 #endif
574 
575 	platform_set_drvdata(pdev, plgpio);
576 	spin_lock_init(&plgpio->lock);
577 
578 	plgpio->chip.base = -1;
579 	plgpio->chip.request = plgpio_request;
580 	plgpio->chip.free = plgpio_free;
581 	plgpio->chip.direction_input = plgpio_direction_input;
582 	plgpio->chip.direction_output = plgpio_direction_output;
583 	plgpio->chip.get = plgpio_get_value;
584 	plgpio->chip.set = plgpio_set_value;
585 	plgpio->chip.label = dev_name(&pdev->dev);
586 	plgpio->chip.parent = &pdev->dev;
587 	plgpio->chip.owner = THIS_MODULE;
588 
589 	if (!IS_ERR(plgpio->clk)) {
590 		ret = clk_prepare(plgpio->clk);
591 		if (ret) {
592 			dev_err(&pdev->dev, "clk prepare failed\n");
593 			return ret;
594 		}
595 	}
596 
597 	irq = platform_get_irq(pdev, 0);
598 	if (irq > 0) {
599 		struct gpio_irq_chip *girq;
600 
601 		girq = &plgpio->chip.irq;
602 		gpio_irq_chip_set_chip(girq, &plgpio_irqchip);
603 		girq->parent_handler = plgpio_irq_handler;
604 		girq->num_parents = 1;
605 		girq->parents = devm_kcalloc(&pdev->dev, 1,
606 					     sizeof(*girq->parents),
607 					     GFP_KERNEL);
608 		if (!girq->parents)
609 			return -ENOMEM;
610 		girq->parents[0] = irq;
611 		girq->default_type = IRQ_TYPE_NONE;
612 		girq->handler = handle_simple_irq;
613 		dev_info(&pdev->dev, "PLGPIO registering with IRQs\n");
614 	} else {
615 		dev_info(&pdev->dev, "PLGPIO registering without IRQs\n");
616 	}
617 
618 	ret = gpiochip_add_data(&plgpio->chip, plgpio);
619 	if (ret) {
620 		dev_err(&pdev->dev, "unable to add gpio chip\n");
621 		goto unprepare_clk;
622 	}
623 
624 	return 0;
625 
626 unprepare_clk:
627 	if (!IS_ERR(plgpio->clk))
628 		clk_unprepare(plgpio->clk);
629 
630 	return ret;
631 }
632 
633 #ifdef CONFIG_PM_SLEEP
634 static int plgpio_suspend(struct device *dev)
635 {
636 	struct plgpio *plgpio = dev_get_drvdata(dev);
637 	int i, reg_count = DIV_ROUND_UP(plgpio->chip.ngpio, MAX_GPIO_PER_REG);
638 	u32 off;
639 
640 	for (i = 0; i < reg_count; i++) {
641 		off = i * sizeof(int *);
642 
643 		if (plgpio->regs.enb != -1)
644 			regmap_read(plgpio->regmap, plgpio->regs.enb + off,
645 				&plgpio->csave_regs[i].enb);
646 		if (plgpio->regs.eit != -1)
647 			regmap_read(plgpio->regmap, plgpio->regs.eit + off,
648 				&plgpio->csave_regs[i].eit);
649 		regmap_read(plgpio->regmap, plgpio->regs.wdata + off,
650 				&plgpio->csave_regs[i].wdata);
651 		regmap_read(plgpio->regmap, plgpio->regs.dir + off,
652 				&plgpio->csave_regs[i].dir);
653 		regmap_read(plgpio->regmap, plgpio->regs.ie + off,
654 				&plgpio->csave_regs[i].ie);
655 	}
656 
657 	return 0;
658 }
659 
660 /*
661  * This is used to correct the values in end registers. End registers contain
662  * extra bits that might be used for other purpose in platform. So, we shouldn't
663  * overwrite these bits. This macro, reads given register again, preserves other
664  * bit values (non-plgpio bits), and retain captured value (plgpio bits).
665  */
666 #define plgpio_prepare_reg(__reg, _off, _mask, _tmp)		\
667 {								\
668 	regmap_read(plgpio->regmap, plgpio->regs.__reg + _off, &_tmp); \
669 	_tmp &= ~_mask;						\
670 	plgpio->csave_regs[i].__reg =				\
671 		_tmp | (plgpio->csave_regs[i].__reg & _mask);	\
672 }
673 
674 static int plgpio_resume(struct device *dev)
675 {
676 	struct plgpio *plgpio = dev_get_drvdata(dev);
677 	int i, reg_count = DIV_ROUND_UP(plgpio->chip.ngpio, MAX_GPIO_PER_REG);
678 	u32 off;
679 	u32 mask, tmp;
680 
681 	for (i = 0; i < reg_count; i++) {
682 		off = i * sizeof(int *);
683 
684 		if (i == reg_count - 1) {
685 			mask = (1 << (plgpio->chip.ngpio - i *
686 						MAX_GPIO_PER_REG)) - 1;
687 
688 			if (plgpio->regs.enb != -1)
689 				plgpio_prepare_reg(enb, off, mask, tmp);
690 
691 			if (plgpio->regs.eit != -1)
692 				plgpio_prepare_reg(eit, off, mask, tmp);
693 
694 			plgpio_prepare_reg(wdata, off, mask, tmp);
695 			plgpio_prepare_reg(dir, off, mask, tmp);
696 			plgpio_prepare_reg(ie, off, mask, tmp);
697 		}
698 
699 		regmap_write(plgpio->regmap, plgpio->regs.wdata + off,
700 			plgpio->csave_regs[i].wdata);
701 
702 		regmap_write(plgpio->regmap, plgpio->regs.dir + off,
703 			plgpio->csave_regs[i].dir);
704 
705 		if (plgpio->regs.eit != -1)
706 			regmap_write(plgpio->regmap, plgpio->regs.eit + off,
707 				plgpio->csave_regs[i].eit);
708 
709 		regmap_write(plgpio->regmap, plgpio->regs.ie + off,
710 			plgpio->csave_regs[i].ie);
711 
712 		if (plgpio->regs.enb != -1)
713 			regmap_write(plgpio->regmap, plgpio->regs.enb + off,
714 				plgpio->csave_regs[i].enb);
715 	}
716 
717 	return 0;
718 }
719 #endif
720 
721 static SIMPLE_DEV_PM_OPS(plgpio_dev_pm_ops, plgpio_suspend, plgpio_resume);
722 
723 static const struct of_device_id plgpio_of_match[] = {
724 	{ .compatible = "st,spear-plgpio" },
725 	{}
726 };
727 
728 static struct platform_driver plgpio_driver = {
729 	.probe = plgpio_probe,
730 	.driver = {
731 		.name = "spear-plgpio",
732 		.pm = &plgpio_dev_pm_ops,
733 		.of_match_table = plgpio_of_match,
734 	},
735 };
736 
737 static int __init plgpio_init(void)
738 {
739 	return platform_driver_register(&plgpio_driver);
740 }
741 subsys_initcall(plgpio_init);
742