pinctrl-plgpio.c

  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
 43struct 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 */
 67struct 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 */
 82static 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
 93static 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
103static 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 */
115static 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
134static 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
167static 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
184static int plgpio_set_value(struct gpio_chip *chip, unsigned int offset,
185			    int value)
186{
187	struct plgpio *plgpio = gpiochip_get_data(chip);
188
189	if (offset >= chip->ngpio)
190		return -EINVAL;
191
192	/* get correct offset for "offset" pin */
193	if (plgpio->p2o && (plgpio->p2o_regs & PTO_WDATA_REG)) {
194		offset = plgpio->p2o(offset);
195		if (offset == -1)
196			return -EINVAL;
197	}
198
199	if (value)
200		plgpio_reg_set(plgpio->regmap, offset, plgpio->regs.wdata);
201	else
202		plgpio_reg_reset(plgpio->regmap, offset, plgpio->regs.wdata);
203
204	return 0;
205}
206
207static int plgpio_request(struct gpio_chip *chip, unsigned offset)
208{
209	struct plgpio *plgpio = gpiochip_get_data(chip);
210	unsigned long flags;
211	int ret = 0;
212
213	if (offset >= chip->ngpio)
214		return -EINVAL;
215
216	ret = pinctrl_gpio_request(chip, offset);
217	if (ret)
218		return ret;
219
220	if (!IS_ERR(plgpio->clk)) {
221		ret = clk_enable(plgpio->clk);
222		if (ret)
223			goto err0;
224	}
225
226	if (plgpio->regs.enb == -1)
227		return 0;
228
229	/*
230	 * put gpio in IN mode before enabling it. This make enabling gpio safe
231	 */
232	ret = plgpio_direction_input(chip, offset);
233	if (ret)
234		goto err1;
235
236	/* get correct offset for "offset" pin */
237	if (plgpio->p2o && (plgpio->p2o_regs & PTO_ENB_REG)) {
238		offset = plgpio->p2o(offset);
239		if (offset == -1) {
240			ret = -EINVAL;
241			goto err1;
242		}
243	}
244
245	spin_lock_irqsave(&plgpio->lock, flags);
246	plgpio_reg_set(plgpio->regmap, offset, plgpio->regs.enb);
247	spin_unlock_irqrestore(&plgpio->lock, flags);
248	return 0;
249
250err1:
251	if (!IS_ERR(plgpio->clk))
252		clk_disable(plgpio->clk);
253err0:
254	pinctrl_gpio_free(chip, offset);
255	return ret;
256}
257
258static void plgpio_free(struct gpio_chip *chip, unsigned offset)
259{
260	struct plgpio *plgpio = gpiochip_get_data(chip);
261	unsigned long flags;
262
263	if (offset >= chip->ngpio)
264		return;
265
266	if (plgpio->regs.enb == -1)
267		goto disable_clk;
268
269	/* get correct offset for "offset" pin */
270	if (plgpio->p2o && (plgpio->p2o_regs & PTO_ENB_REG)) {
271		offset = plgpio->p2o(offset);
272		if (offset == -1)
273			return;
274	}
275
276	spin_lock_irqsave(&plgpio->lock, flags);
277	plgpio_reg_reset(plgpio->regmap, offset, plgpio->regs.enb);
278	spin_unlock_irqrestore(&plgpio->lock, flags);
279
280disable_clk:
281	if (!IS_ERR(plgpio->clk))
282		clk_disable(plgpio->clk);
283
284	pinctrl_gpio_free(chip, offset);
285}
286
287/* PLGPIO IRQ */
288static void plgpio_irq_disable(struct irq_data *d)
289{
290	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
291	struct plgpio *plgpio = gpiochip_get_data(gc);
292	int offset = d->hwirq;
293	unsigned long flags;
294
295	/* get correct offset for "offset" pin */
296	if (plgpio->p2o && (plgpio->p2o_regs & PTO_IE_REG)) {
297		offset = plgpio->p2o(offset);
298		if (offset == -1)
299			return;
300	}
301
302	spin_lock_irqsave(&plgpio->lock, flags);
303	plgpio_reg_set(plgpio->regmap, offset, plgpio->regs.ie);
304	spin_unlock_irqrestore(&plgpio->lock, flags);
305	gpiochip_disable_irq(gc, irqd_to_hwirq(d));
306}
307
308static void plgpio_irq_enable(struct irq_data *d)
309{
310	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
311	struct plgpio *plgpio = gpiochip_get_data(gc);
312	int offset = d->hwirq;
313	unsigned long flags;
314
315	/* get correct offset for "offset" pin */
316	if (plgpio->p2o && (plgpio->p2o_regs & PTO_IE_REG)) {
317		offset = plgpio->p2o(offset);
318		if (offset == -1)
319			return;
320	}
321
322	gpiochip_enable_irq(gc, irqd_to_hwirq(d));
323	spin_lock_irqsave(&plgpio->lock, flags);
324	plgpio_reg_reset(plgpio->regmap, offset, plgpio->regs.ie);
325	spin_unlock_irqrestore(&plgpio->lock, flags);
326}
327
328static int plgpio_irq_set_type(struct irq_data *d, unsigned trigger)
329{
330	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
331	struct plgpio *plgpio = gpiochip_get_data(gc);
332	int offset = d->hwirq;
333	u32 reg_off;
334	unsigned int supported_type = 0, val;
335
336	if (offset >= plgpio->chip.ngpio)
337		return -EINVAL;
338
339	if (plgpio->regs.eit == -1)
340		supported_type = IRQ_TYPE_LEVEL_HIGH;
341	else
342		supported_type = IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING;
343
344	if (!(trigger & supported_type))
345		return -EINVAL;
346
347	if (plgpio->regs.eit == -1)
348		return 0;
349
350	reg_off = REG_OFFSET(0, plgpio->regs.eit, offset);
351	regmap_read(plgpio->regmap, reg_off, &val);
352
353	offset = PIN_OFFSET(offset);
354	if (trigger & IRQ_TYPE_EDGE_RISING)
355		regmap_write(plgpio->regmap, reg_off, val | (1 << offset));
356	else
357		regmap_write(plgpio->regmap, reg_off, val & ~(1 << offset));
358
359	return 0;
360}
361
362static const struct irq_chip plgpio_irqchip = {
363	.name		= "PLGPIO",
364	.irq_enable	= plgpio_irq_enable,
365	.irq_disable	= plgpio_irq_disable,
366	.irq_set_type	= plgpio_irq_set_type,
367	.flags		= IRQCHIP_IMMUTABLE,
368	GPIOCHIP_IRQ_RESOURCE_HELPERS,
369};
370
371static void plgpio_irq_handler(struct irq_desc *desc)
372{
373	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
374	struct plgpio *plgpio = gpiochip_get_data(gc);
375	struct irq_chip *irqchip = irq_desc_get_chip(desc);
376	int regs_count, count, pin, offset, i = 0;
377	u32 pending;
378	unsigned long pendingl;
379
380	count = plgpio->chip.ngpio;
381	regs_count = DIV_ROUND_UP(count, MAX_GPIO_PER_REG);
382
383	chained_irq_enter(irqchip, desc);
384	/* check all plgpio MIS registers for a possible interrupt */
385	for (; i < regs_count; i++) {
386		regmap_read(plgpio->regmap, plgpio->regs.mis +
387			i * sizeof(int *), &pending);
388		if (!pending)
389			continue;
390
391		/* clear interrupts */
392		regmap_write(plgpio->regmap, plgpio->regs.mis +
393			i * sizeof(int *), ~pending);
394		/*
395		 * clear extra bits in last register having gpios < MAX/REG
396		 * ex: Suppose there are max 102 plgpios. then last register
397		 * must have only (102 - MAX_GPIO_PER_REG * 3) = 6 relevant bits
398		 * so, we must not take other 28 bits into consideration for
399		 * checking interrupt. so clear those bits.
400		 */
401		count = count - i * MAX_GPIO_PER_REG;
402		if (count < MAX_GPIO_PER_REG)
403			pending &= (1 << count) - 1;
404
405		pendingl = pending;
406		for_each_set_bit(offset, &pendingl, MAX_GPIO_PER_REG) {
407			/* get correct pin for "offset" */
408			if (plgpio->o2p && (plgpio->p2o_regs & PTO_MIS_REG)) {
409				pin = plgpio->o2p(offset);
410				if (pin == -1)
411					continue;
412			} else
413				pin = offset;
414
415			/* get correct irq line number */
416			pin = i * MAX_GPIO_PER_REG + pin;
417			generic_handle_domain_irq(gc->irq.domain, pin);
418		}
419	}
420	chained_irq_exit(irqchip, desc);
421}
422
423/*
424 * pin to offset and offset to pin converter functions
425 *
426 * In spear310 there is inconsistency among bit positions in plgpio regiseters,
427 * for different plgpio pins. For example: for pin 27, bit offset is 23, pin
428 * 28-33 are not supported, pin 95 has offset bit 95, bit 100 has offset bit 1
429 */
430static int spear310_p2o(int pin)
431{
432	int offset = pin;
433
434	if (pin <= 27)
435		offset += 4;
436	else if (pin <= 33)
437		offset = -1;
438	else if (pin <= 97)
439		offset -= 2;
440	else if (pin <= 101)
441		offset = 101 - pin;
442	else
443		offset = -1;
444
445	return offset;
446}
447
448static int spear310_o2p(int offset)
449{
450	if (offset <= 3)
451		return 101 - offset;
452	else if (offset <= 31)
453		return offset - 4;
454	else
455		return offset + 2;
456}
457
458static int plgpio_probe_dt(struct platform_device *pdev, struct plgpio *plgpio)
459{
460	struct device_node *np = pdev->dev.of_node;
461	int ret = -EINVAL;
462	u32 val;
463
464	if (of_machine_is_compatible("st,spear310")) {
465		plgpio->p2o = spear310_p2o;
466		plgpio->o2p = spear310_o2p;
467		plgpio->p2o_regs = PTO_WDATA_REG | PTO_DIR_REG | PTO_IE_REG |
468			PTO_RDATA_REG | PTO_MIS_REG;
469	}
470
471	if (!of_property_read_u32(np, "st-plgpio,ngpio", &val)) {
472		plgpio->chip.ngpio = val;
473	} else {
474		dev_err(&pdev->dev, "DT: Invalid ngpio field\n");
475		goto end;
476	}
477
478	if (!of_property_read_u32(np, "st-plgpio,enb-reg", &val))
479		plgpio->regs.enb = val;
480	else
481		plgpio->regs.enb = -1;
482
483	if (!of_property_read_u32(np, "st-plgpio,wdata-reg", &val)) {
484		plgpio->regs.wdata = val;
485	} else {
486		dev_err(&pdev->dev, "DT: Invalid wdata reg\n");
487		goto end;
488	}
489
490	if (!of_property_read_u32(np, "st-plgpio,dir-reg", &val)) {
491		plgpio->regs.dir = val;
492	} else {
493		dev_err(&pdev->dev, "DT: Invalid dir reg\n");
494		goto end;
495	}
496
497	if (!of_property_read_u32(np, "st-plgpio,ie-reg", &val)) {
498		plgpio->regs.ie = val;
499	} else {
500		dev_err(&pdev->dev, "DT: Invalid ie reg\n");
501		goto end;
502	}
503
504	if (!of_property_read_u32(np, "st-plgpio,rdata-reg", &val)) {
505		plgpio->regs.rdata = val;
506	} else {
507		dev_err(&pdev->dev, "DT: Invalid rdata reg\n");
508		goto end;
509	}
510
511	if (!of_property_read_u32(np, "st-plgpio,mis-reg", &val)) {
512		plgpio->regs.mis = val;
513	} else {
514		dev_err(&pdev->dev, "DT: Invalid mis reg\n");
515		goto end;
516	}
517
518	if (!of_property_read_u32(np, "st-plgpio,eit-reg", &val))
519		plgpio->regs.eit = val;
520	else
521		plgpio->regs.eit = -1;
522
523	return 0;
524
525end:
526	return ret;
527}
528
529static int plgpio_probe(struct platform_device *pdev)
530{
531	struct device_node *regmap_np;
532	struct plgpio *plgpio;
533	int ret, irq;
534
535	plgpio = devm_kzalloc(&pdev->dev, sizeof(*plgpio), GFP_KERNEL);
536	if (!plgpio)
537		return -ENOMEM;
538
539	regmap_np = of_parse_phandle(pdev->dev.of_node, "regmap", 0);
540	if (regmap_np) {
541		plgpio->regmap = device_node_to_regmap(regmap_np);
542		of_node_put(regmap_np);
543		if (IS_ERR(plgpio->regmap)) {
544			dev_err(&pdev->dev, "Retrieve regmap failed (%pe)\n",
545				plgpio->regmap);
546			return PTR_ERR(plgpio->regmap);
547		}
548	} else {
549		plgpio->regmap = device_node_to_regmap(pdev->dev.of_node);
550		if (IS_ERR(plgpio->regmap)) {
551			dev_err(&pdev->dev, "Init regmap failed (%pe)\n",
552				plgpio->regmap);
553			return PTR_ERR(plgpio->regmap);
554		}
555	}
556
557	ret = plgpio_probe_dt(pdev, plgpio);
558	if (ret) {
559		dev_err(&pdev->dev, "DT probe failed\n");
560		return ret;
561	}
562
563	plgpio->clk = devm_clk_get(&pdev->dev, NULL);
564	if (IS_ERR(plgpio->clk))
565		dev_warn(&pdev->dev, "clk_get() failed, work without it\n");
566
567#ifdef CONFIG_PM_SLEEP
568	plgpio->csave_regs = devm_kcalloc(&pdev->dev,
569			DIV_ROUND_UP(plgpio->chip.ngpio, MAX_GPIO_PER_REG),
570			sizeof(*plgpio->csave_regs),
571			GFP_KERNEL);
572	if (!plgpio->csave_regs)
573		return -ENOMEM;
574#endif
575
576	platform_set_drvdata(pdev, plgpio);
577	spin_lock_init(&plgpio->lock);
578
579	plgpio->chip.base = -1;
580	plgpio->chip.request = plgpio_request;
581	plgpio->chip.free = plgpio_free;
582	plgpio->chip.direction_input = plgpio_direction_input;
583	plgpio->chip.direction_output = plgpio_direction_output;
584	plgpio->chip.get = plgpio_get_value;
585	plgpio->chip.set = plgpio_set_value;
586	plgpio->chip.label = dev_name(&pdev->dev);
587	plgpio->chip.parent = &pdev->dev;
588	plgpio->chip.owner = THIS_MODULE;
589
590	if (!IS_ERR(plgpio->clk)) {
591		ret = clk_prepare(plgpio->clk);
592		if (ret) {
593			dev_err(&pdev->dev, "clk prepare failed\n");
594			return ret;
595		}
596	}
597
598	irq = platform_get_irq(pdev, 0);
599	if (irq > 0) {
600		struct gpio_irq_chip *girq;
601
602		girq = &plgpio->chip.irq;
603		gpio_irq_chip_set_chip(girq, &plgpio_irqchip);
604		girq->parent_handler = plgpio_irq_handler;
605		girq->num_parents = 1;
606		girq->parents = devm_kcalloc(&pdev->dev, 1,
607					     sizeof(*girq->parents),
608					     GFP_KERNEL);
609		if (!girq->parents)
610			return -ENOMEM;
611		girq->parents[0] = irq;
612		girq->default_type = IRQ_TYPE_NONE;
613		girq->handler = handle_simple_irq;
614		dev_info(&pdev->dev, "PLGPIO registering with IRQs\n");
615	} else {
616		dev_info(&pdev->dev, "PLGPIO registering without IRQs\n");
617	}
618
619	ret = gpiochip_add_data(&plgpio->chip, plgpio);
620	if (ret) {
621		dev_err(&pdev->dev, "unable to add gpio chip\n");
622		goto unprepare_clk;
623	}
624
625	return 0;
626
627unprepare_clk:
628	if (!IS_ERR(plgpio->clk))
629		clk_unprepare(plgpio->clk);
630
631	return ret;
632}
633
634#ifdef CONFIG_PM_SLEEP
635static int plgpio_suspend(struct device *dev)
636{
637	struct plgpio *plgpio = dev_get_drvdata(dev);
638	int i, reg_count = DIV_ROUND_UP(plgpio->chip.ngpio, MAX_GPIO_PER_REG);
639	u32 off;
640
641	for (i = 0; i < reg_count; i++) {
642		off = i * sizeof(int *);
643
644		if (plgpio->regs.enb != -1)
645			regmap_read(plgpio->regmap, plgpio->regs.enb + off,
646				&plgpio->csave_regs[i].enb);
647		if (plgpio->regs.eit != -1)
648			regmap_read(plgpio->regmap, plgpio->regs.eit + off,
649				&plgpio->csave_regs[i].eit);
650		regmap_read(plgpio->regmap, plgpio->regs.wdata + off,
651				&plgpio->csave_regs[i].wdata);
652		regmap_read(plgpio->regmap, plgpio->regs.dir + off,
653				&plgpio->csave_regs[i].dir);
654		regmap_read(plgpio->regmap, plgpio->regs.ie + off,
655				&plgpio->csave_regs[i].ie);
656	}
657
658	return 0;
659}
660
661/*
662 * This is used to correct the values in end registers. End registers contain
663 * extra bits that might be used for other purpose in platform. So, we shouldn't
664 * overwrite these bits. This macro, reads given register again, preserves other
665 * bit values (non-plgpio bits), and retain captured value (plgpio bits).
666 */
667#define plgpio_prepare_reg(__reg, _off, _mask, _tmp)		\
668{								\
669	regmap_read(plgpio->regmap, plgpio->regs.__reg + _off, &_tmp); \
670	_tmp &= ~_mask;						\
671	plgpio->csave_regs[i].__reg =				\
672		_tmp | (plgpio->csave_regs[i].__reg & _mask);	\
673}
674
675static int plgpio_resume(struct device *dev)
676{
677	struct plgpio *plgpio = dev_get_drvdata(dev);
678	int i, reg_count = DIV_ROUND_UP(plgpio->chip.ngpio, MAX_GPIO_PER_REG);
679	u32 off;
680	u32 mask, tmp;
681
682	for (i = 0; i < reg_count; i++) {
683		off = i * sizeof(int *);
684
685		if (i == reg_count - 1) {
686			mask = (1 << (plgpio->chip.ngpio - i *
687						MAX_GPIO_PER_REG)) - 1;
688
689			if (plgpio->regs.enb != -1)
690				plgpio_prepare_reg(enb, off, mask, tmp);
691
692			if (plgpio->regs.eit != -1)
693				plgpio_prepare_reg(eit, off, mask, tmp);
694
695			plgpio_prepare_reg(wdata, off, mask, tmp);
696			plgpio_prepare_reg(dir, off, mask, tmp);
697			plgpio_prepare_reg(ie, off, mask, tmp);
698		}
699
700		regmap_write(plgpio->regmap, plgpio->regs.wdata + off,
701			plgpio->csave_regs[i].wdata);
702
703		regmap_write(plgpio->regmap, plgpio->regs.dir + off,
704			plgpio->csave_regs[i].dir);
705
706		if (plgpio->regs.eit != -1)
707			regmap_write(plgpio->regmap, plgpio->regs.eit + off,
708				plgpio->csave_regs[i].eit);
709
710		regmap_write(plgpio->regmap, plgpio->regs.ie + off,
711			plgpio->csave_regs[i].ie);
712
713		if (plgpio->regs.enb != -1)
714			regmap_write(plgpio->regmap, plgpio->regs.enb + off,
715				plgpio->csave_regs[i].enb);
716	}
717
718	return 0;
719}
720#endif
721
722static SIMPLE_DEV_PM_OPS(plgpio_dev_pm_ops, plgpio_suspend, plgpio_resume);
723
724static const struct of_device_id plgpio_of_match[] = {
725	{ .compatible = "st,spear-plgpio" },
726	{}
727};
728
729static struct platform_driver plgpio_driver = {
730	.probe = plgpio_probe,
731	.driver = {
732		.name = "spear-plgpio",
733		.pm = &plgpio_dev_pm_ops,
734		.of_match_table = plgpio_of_match,
735	},
736};
737
738static int __init plgpio_init(void)
739{
740	return platform_driver_register(&plgpio_driver);
741}
742subsys_initcall(plgpio_init);