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