drivers/mfd/stmpe.c

   1 /*
   2  * Copyright (C) ST-Ericsson SA 2010
   3  *
   4  * License Terms: GNU General Public License, version 2
   5  * Author: Rabin Vincent <rabin.vincent@stericsson.com> for ST-Ericsson
   6  */
   7
   8 #include <linux/kernel.h>
   9 #include <linux/module.h>
  10 #include <linux/interrupt.h>
  11 #include <linux/irq.h>
  12 #include <linux/slab.h>
  13 #include <linux/i2c.h>
  14 #include <linux/mfd/core.h>
  15 #include <linux/mfd/stmpe.h>
  16 #include "stmpe.h"
  17
  18 static int __stmpe_enable(struct stmpe *stmpe, unsigned int blocks)
  19 {
  20         return stmpe->variant->enable(stmpe, blocks, true);
  21 }
  22
  23 static int __stmpe_disable(struct stmpe *stmpe, unsigned int blocks)
  24 {
  25         return stmpe->variant->enable(stmpe, blocks, false);
  26 }
  27
  28 static int __stmpe_reg_read(struct stmpe *stmpe, u8 reg)
  29 {
  30         int ret;
  31
  32         ret = i2c_smbus_read_byte_data(stmpe->i2c, reg);
  33         if (ret < 0)
  34                 dev_err(stmpe->dev, "failed to read reg %#x: %d\n",
  35                         reg, ret);
  36
  37         dev_vdbg(stmpe->dev, "rd: reg %#x => data %#x\n", reg, ret);
  38
  39         return ret;
  40 }
  41
  42 static int __stmpe_reg_write(struct stmpe *stmpe, u8 reg, u8 val)
  43 {
  44         int ret;
  45
  46         dev_vdbg(stmpe->dev, "wr: reg %#x <= %#x\n", reg, val);
  47
  48         ret = i2c_smbus_write_byte_data(stmpe->i2c, reg, val);
  49         if (ret < 0)
  50                 dev_err(stmpe->dev, "failed to write reg %#x: %d\n",
  51                         reg, ret);
  52
  53         return ret;
  54 }
  55
  56 static int __stmpe_set_bits(struct stmpe *stmpe, u8 reg, u8 mask, u8 val)
  57 {
  58         int ret;
  59
  60         ret = __stmpe_reg_read(stmpe, reg);
  61         if (ret < 0)
  62                 return ret;
  63
  64         ret &= ~mask;
  65         ret |= val;
  66
  67         return __stmpe_reg_write(stmpe, reg, ret);
  68 }
  69
  70 static int __stmpe_block_read(struct stmpe *stmpe, u8 reg, u8 length,
  71                               u8 *values)
  72 {
  73         int ret;
  74
  75         ret = i2c_smbus_read_i2c_block_data(stmpe->i2c, reg, length, values);
  76         if (ret < 0)
  77                 dev_err(stmpe->dev, "failed to read regs %#x: %d\n",
  78                         reg, ret);
  79
  80         dev_vdbg(stmpe->dev, "rd: reg %#x (%d) => ret %#x\n", reg, length, ret);
  81         stmpe_dump_bytes("stmpe rd: ", values, length);
  82
  83         return ret;
  84 }
  85
  86 static int __stmpe_block_write(struct stmpe *stmpe, u8 reg, u8 length,
  87                         const u8 *values)
  88 {
  89         int ret;
  90
  91         dev_vdbg(stmpe->dev, "wr: regs %#x (%d)\n", reg, length);
  92         stmpe_dump_bytes("stmpe wr: ", values, length);
  93
  94         ret = i2c_smbus_write_i2c_block_data(stmpe->i2c, reg, length,
  95                                              values);
  96         if (ret < 0)
  97                 dev_err(stmpe->dev, "failed to write regs %#x: %d\n",
  98                         reg, ret);
  99
 100         return ret;
 101 }
 102
 103 /**
 104  * stmpe_enable - enable blocks on an STMPE device
 105  * @stmpe:      Device to work on
 106  * @blocks:     Mask of blocks (enum stmpe_block values) to enable
 107  */
 108 int stmpe_enable(struct stmpe *stmpe, unsigned int blocks)
 109 {
 110         int ret;
 111
 112         mutex_lock(&stmpe->lock);
 113         ret = __stmpe_enable(stmpe, blocks);
 114         mutex_unlock(&stmpe->lock);
 115
 116         return ret;
 117 }
 118 EXPORT_SYMBOL_GPL(stmpe_enable);
 119
 120 /**
 121  * stmpe_disable - disable blocks on an STMPE device
 122  * @stmpe:      Device to work on
 123  * @blocks:     Mask of blocks (enum stmpe_block values) to enable
 124  */
 125 int stmpe_disable(struct stmpe *stmpe, unsigned int blocks)
 126 {
 127         int ret;
 128
 129         mutex_lock(&stmpe->lock);
 130         ret = __stmpe_disable(stmpe, blocks);
 131         mutex_unlock(&stmpe->lock);
 132
 133         return ret;
 134 }
 135 EXPORT_SYMBOL_GPL(stmpe_disable);
 136
 137 /**
 138  * stmpe_reg_read() - read a single STMPE register
 139  * @stmpe:      Device to read from
 140  * @reg:        Register to read
 141  */
 142 int stmpe_reg_read(struct stmpe *stmpe, u8 reg)
 143 {
 144         int ret;
 145
 146         mutex_lock(&stmpe->lock);
 147         ret = __stmpe_reg_read(stmpe, reg);
 148         mutex_unlock(&stmpe->lock);
 149
 150         return ret;
 151 }
 152 EXPORT_SYMBOL_GPL(stmpe_reg_read);
 153
 154 /**
 155  * stmpe_reg_write() - write a single STMPE register
 156  * @stmpe:      Device to write to
 157  * @reg:        Register to write
 158  * @val:        Value to write
 159  */
 160 int stmpe_reg_write(struct stmpe *stmpe, u8 reg, u8 val)
 161 {
 162         int ret;
 163
 164         mutex_lock(&stmpe->lock);
 165         ret = __stmpe_reg_write(stmpe, reg, val);
 166         mutex_unlock(&stmpe->lock);
 167
 168         return ret;
 169 }
 170 EXPORT_SYMBOL_GPL(stmpe_reg_write);
 171
 172 /**
 173  * stmpe_set_bits() - set the value of a bitfield in a STMPE register
 174  * @stmpe:      Device to write to
 175  * @reg:        Register to write
 176  * @mask:       Mask of bits to set
 177  * @val:        Value to set
 178  */
 179 int stmpe_set_bits(struct stmpe *stmpe, u8 reg, u8 mask, u8 val)
 180 {
 181         int ret;
 182
 183         mutex_lock(&stmpe->lock);
 184         ret = __stmpe_set_bits(stmpe, reg, mask, val);
 185         mutex_unlock(&stmpe->lock);
 186
 187         return ret;
 188 }
 189 EXPORT_SYMBOL_GPL(stmpe_set_bits);
 190
 191 /**
 192  * stmpe_block_read() - read multiple STMPE registers
 193  * @stmpe:      Device to read from
 194  * @reg:        First register
 195  * @length:     Number of registers
 196  * @values:     Buffer to write to
 197  */
 198 int stmpe_block_read(struct stmpe *stmpe, u8 reg, u8 length, u8 *values)
 199 {
 200         int ret;
 201
 202         mutex_lock(&stmpe->lock);
 203         ret = __stmpe_block_read(stmpe, reg, length, values);
 204         mutex_unlock(&stmpe->lock);
 205
 206         return ret;
 207 }
 208 EXPORT_SYMBOL_GPL(stmpe_block_read);
 209
 210 /**
 211  * stmpe_block_write() - write multiple STMPE registers
 212  * @stmpe:      Device to write to
 213  * @reg:        First register
 214  * @length:     Number of registers
 215  * @values:     Values to write
 216  */
 217 int stmpe_block_write(struct stmpe *stmpe, u8 reg, u8 length,
 218                       const u8 *values)
 219 {
 220         int ret;
 221
 222         mutex_lock(&stmpe->lock);
 223         ret = __stmpe_block_write(stmpe, reg, length, values);
 224         mutex_unlock(&stmpe->lock);
 225
 226         return ret;
 227 }
 228 EXPORT_SYMBOL_GPL(stmpe_block_write);
 229
 230 /**
 231  * stmpe_set_altfunc: set the alternate function for STMPE pins
 232  * @stmpe:      Device to configure
 233  * @pins:       Bitmask of pins to affect
 234  * @block:      block to enable alternate functions for
 235  *
 236  * @pins is assumed to have a bit set for each of the bits whose alternate
 237  * function is to be changed, numbered according to the GPIOXY numbers.
 238  *
 239  * If the GPIO module is not enabled, this function automatically enables it in
 240  * order to perform the change.
 241  */
 242 int stmpe_set_altfunc(struct stmpe *stmpe, u32 pins, enum stmpe_block block)
 243 {
 244         struct stmpe_variant_info *variant = stmpe->variant;
 245         u8 regaddr = stmpe->regs[STMPE_IDX_GPAFR_U_MSB];
 246         int af_bits = variant->af_bits;
 247         int numregs = DIV_ROUND_UP(stmpe->num_gpios * af_bits, 8);
 248         int afperreg = 8 / af_bits;
 249         int mask = (1 << af_bits) - 1;
 250         u8 regs[numregs];
 251         int af;
 252         int ret;
 253
 254         mutex_lock(&stmpe->lock);
 255
 256         ret = __stmpe_enable(stmpe, STMPE_BLOCK_GPIO);
 257         if (ret < 0)
 258                 goto out;
 259
 260         ret = __stmpe_block_read(stmpe, regaddr, numregs, regs);
 261         if (ret < 0)
 262                 goto out;
 263
 264         af = variant->get_altfunc(stmpe, block);
 265
 266         while (pins) {
 267                 int pin = __ffs(pins);
 268                 int regoffset = numregs - (pin / afperreg) - 1;
 269                 int pos = (pin % afperreg) * (8 / afperreg);
 270
 271                 regs[regoffset] &= ~(mask << pos);
 272                 regs[regoffset] |= af << pos;
 273
 274                 pins &= ~(1 << pin);
 275         }
 276
 277         ret = __stmpe_block_write(stmpe, regaddr, numregs, regs);
 278
 279 out:
 280         mutex_unlock(&stmpe->lock);
 281         return ret;
 282 }
 283 EXPORT_SYMBOL_GPL(stmpe_set_altfunc);
 284
 285 /*
 286  * GPIO (all variants)
 287  */
 288
 289 static struct resource stmpe_gpio_resources[] = {
 290         /* Start and end filled dynamically */
 291         {
 292                 .flags  = IORESOURCE_IRQ,
 293         },
 294 };
 295
 296 static struct mfd_cell stmpe_gpio_cell = {
 297         .name           = "stmpe-gpio",
 298         .resources      = stmpe_gpio_resources,
 299         .num_resources  = ARRAY_SIZE(stmpe_gpio_resources),
 300 };
 301
 302 /*
 303  * Keypad (1601, 2401, 2403)
 304  */
 305
 306 static struct resource stmpe_keypad_resources[] = {
 307         {
 308                 .name   = "KEYPAD",
 309                 .start  = 0,
 310                 .end    = 0,
 311                 .flags  = IORESOURCE_IRQ,
 312         },
 313         {
 314                 .name   = "KEYPAD_OVER",
 315                 .start  = 1,
 316                 .end    = 1,
 317                 .flags  = IORESOURCE_IRQ,
 318         },
 319 };
 320
 321 static struct mfd_cell stmpe_keypad_cell = {
 322         .name           = "stmpe-keypad",
 323         .resources      = stmpe_keypad_resources,
 324         .num_resources  = ARRAY_SIZE(stmpe_keypad_resources),
 325 };
 326
 327 /*
 328  * Touchscreen (STMPE811)
 329  */
 330
 331 static struct resource stmpe_ts_resources[] = {
 332         {
 333                 .name   = "TOUCH_DET",
 334                 .start  = 0,
 335                 .end    = 0,
 336                 .flags  = IORESOURCE_IRQ,
 337         },
 338         {
 339                 .name   = "FIFO_TH",
 340                 .start  = 1,
 341                 .end    = 1,
 342                 .flags  = IORESOURCE_IRQ,
 343         },
 344 };
 345
 346 static struct mfd_cell stmpe_ts_cell = {
 347         .name           = "stmpe-ts",
 348         .resources      = stmpe_ts_resources,
 349         .num_resources  = ARRAY_SIZE(stmpe_ts_resources),
 350 };
 351
 352 /*
 353  * STMPE811
 354  */
 355
 356 static const u8 stmpe811_regs[] = {
 357         [STMPE_IDX_CHIP_ID]     = STMPE811_REG_CHIP_ID,
 358         [STMPE_IDX_ICR_LSB]     = STMPE811_REG_INT_CTRL,
 359         [STMPE_IDX_IER_LSB]     = STMPE811_REG_INT_EN,
 360         [STMPE_IDX_ISR_MSB]     = STMPE811_REG_INT_STA,
 361         [STMPE_IDX_GPMR_LSB]    = STMPE811_REG_GPIO_MP_STA,
 362         [STMPE_IDX_GPSR_LSB]    = STMPE811_REG_GPIO_SET_PIN,
 363         [STMPE_IDX_GPCR_LSB]    = STMPE811_REG_GPIO_CLR_PIN,
 364         [STMPE_IDX_GPDR_LSB]    = STMPE811_REG_GPIO_DIR,
 365         [STMPE_IDX_GPRER_LSB]   = STMPE811_REG_GPIO_RE,
 366         [STMPE_IDX_GPFER_LSB]   = STMPE811_REG_GPIO_FE,
 367         [STMPE_IDX_GPAFR_U_MSB] = STMPE811_REG_GPIO_AF,
 368         [STMPE_IDX_IEGPIOR_LSB] = STMPE811_REG_GPIO_INT_EN,
 369         [STMPE_IDX_ISGPIOR_MSB] = STMPE811_REG_GPIO_INT_STA,
 370         [STMPE_IDX_GPEDR_MSB]   = STMPE811_REG_GPIO_ED,
 371 };
 372
 373 static struct stmpe_variant_block stmpe811_blocks[] = {
 374         {
 375                 .cell   = &stmpe_gpio_cell,
 376                 .irq    = STMPE811_IRQ_GPIOC,
 377                 .block  = STMPE_BLOCK_GPIO,
 378         },
 379         {
 380                 .cell   = &stmpe_ts_cell,
 381                 .irq    = STMPE811_IRQ_TOUCH_DET,
 382                 .block  = STMPE_BLOCK_TOUCHSCREEN,
 383         },
 384 };
 385
 386 static int stmpe811_enable(struct stmpe *stmpe, unsigned int blocks,
 387                            bool enable)
 388 {
 389         unsigned int mask = 0;
 390
 391         if (blocks & STMPE_BLOCK_GPIO)
 392                 mask |= STMPE811_SYS_CTRL2_GPIO_OFF;
 393
 394         if (blocks & STMPE_BLOCK_ADC)
 395                 mask |= STMPE811_SYS_CTRL2_ADC_OFF;
 396
 397         if (blocks & STMPE_BLOCK_TOUCHSCREEN)
 398                 mask |= STMPE811_SYS_CTRL2_TSC_OFF;
 399
 400         return __stmpe_set_bits(stmpe, STMPE811_REG_SYS_CTRL2, mask,
 401                                 enable ? 0 : mask);
 402 }
 403
 404 static int stmpe811_get_altfunc(struct stmpe *stmpe, enum stmpe_block block)
 405 {
 406         /* 0 for touchscreen, 1 for GPIO */
 407         return block != STMPE_BLOCK_TOUCHSCREEN;
 408 }
 409
 410 static struct stmpe_variant_info stmpe811 = {
 411         .name           = "stmpe811",
 412         .id_val         = 0x0811,
 413         .id_mask        = 0xffff,
 414         .num_gpios      = 8,
 415         .af_bits        = 1,
 416         .regs           = stmpe811_regs,
 417         .blocks         = stmpe811_blocks,
 418         .num_blocks     = ARRAY_SIZE(stmpe811_blocks),
 419         .num_irqs       = STMPE811_NR_INTERNAL_IRQS,
 420         .enable         = stmpe811_enable,
 421         .get_altfunc    = stmpe811_get_altfunc,
 422 };
 423
 424 /*
 425  * STMPE1601
 426  */
 427
 428 static const u8 stmpe1601_regs[] = {
 429         [STMPE_IDX_CHIP_ID]     = STMPE1601_REG_CHIP_ID,
 430         [STMPE_IDX_ICR_LSB]     = STMPE1601_REG_ICR_LSB,
 431         [STMPE_IDX_IER_LSB]     = STMPE1601_REG_IER_LSB,
 432         [STMPE_IDX_ISR_MSB]     = STMPE1601_REG_ISR_MSB,
 433         [STMPE_IDX_GPMR_LSB]    = STMPE1601_REG_GPIO_MP_LSB,
 434         [STMPE_IDX_GPSR_LSB]    = STMPE1601_REG_GPIO_SET_LSB,
 435         [STMPE_IDX_GPCR_LSB]    = STMPE1601_REG_GPIO_CLR_LSB,
 436         [STMPE_IDX_GPDR_LSB]    = STMPE1601_REG_GPIO_SET_DIR_LSB,
 437         [STMPE_IDX_GPRER_LSB]   = STMPE1601_REG_GPIO_RE_LSB,
 438         [STMPE_IDX_GPFER_LSB]   = STMPE1601_REG_GPIO_FE_LSB,
 439         [STMPE_IDX_GPAFR_U_MSB] = STMPE1601_REG_GPIO_AF_U_MSB,
 440         [STMPE_IDX_IEGPIOR_LSB] = STMPE1601_REG_INT_EN_GPIO_MASK_LSB,
 441         [STMPE_IDX_ISGPIOR_MSB] = STMPE1601_REG_INT_STA_GPIO_MSB,
 442         [STMPE_IDX_GPEDR_MSB]   = STMPE1601_REG_GPIO_ED_MSB,
 443 };
 444
 445 static struct stmpe_variant_block stmpe1601_blocks[] = {
 446         {
 447                 .cell   = &stmpe_gpio_cell,
 448                 .irq    = STMPE24XX_IRQ_GPIOC,
 449                 .block  = STMPE_BLOCK_GPIO,
 450         },
 451         {
 452                 .cell   = &stmpe_keypad_cell,
 453                 .irq    = STMPE24XX_IRQ_KEYPAD,
 454                 .block  = STMPE_BLOCK_KEYPAD,
 455         },
 456 };
 457
 458 static int stmpe1601_enable(struct stmpe *stmpe, unsigned int blocks,
 459                             bool enable)
 460 {
 461         unsigned int mask = 0;
 462
 463         if (blocks & STMPE_BLOCK_GPIO)
 464                 mask |= STMPE1601_SYS_CTRL_ENABLE_GPIO;
 465
 466         if (blocks & STMPE_BLOCK_KEYPAD)
 467                 mask |= STMPE1601_SYS_CTRL_ENABLE_KPC;
 468
 469         return __stmpe_set_bits(stmpe, STMPE1601_REG_SYS_CTRL, mask,
 470                                 enable ? mask : 0);
 471 }
 472
 473 static int stmpe1601_get_altfunc(struct stmpe *stmpe, enum stmpe_block block)
 474 {
 475         switch (block) {
 476         case STMPE_BLOCK_PWM:
 477                 return 2;
 478
 479         case STMPE_BLOCK_KEYPAD:
 480                 return 1;
 481
 482         case STMPE_BLOCK_GPIO:
 483         default:
 484                 return 0;
 485         }
 486 }
 487
 488 static struct stmpe_variant_info stmpe1601 = {
 489         .name           = "stmpe1601",
 490         .id_val         = 0x0210,
 491         .id_mask        = 0xfff0,       /* at least 0x0210 and 0x0212 */
 492         .num_gpios      = 16,
 493         .af_bits        = 2,
 494         .regs           = stmpe1601_regs,
 495         .blocks         = stmpe1601_blocks,
 496         .num_blocks     = ARRAY_SIZE(stmpe1601_blocks),
 497         .num_irqs       = STMPE1601_NR_INTERNAL_IRQS,
 498         .enable         = stmpe1601_enable,
 499         .get_altfunc    = stmpe1601_get_altfunc,
 500 };
 501
 502 /*
 503  * STMPE24XX
 504  */
 505
 506 static const u8 stmpe24xx_regs[] = {
 507         [STMPE_IDX_CHIP_ID]     = STMPE24XX_REG_CHIP_ID,
 508         [STMPE_IDX_ICR_LSB]     = STMPE24XX_REG_ICR_LSB,
 509         [STMPE_IDX_IER_LSB]     = STMPE24XX_REG_IER_LSB,
 510         [STMPE_IDX_ISR_MSB]     = STMPE24XX_REG_ISR_MSB,
 511         [STMPE_IDX_GPMR_LSB]    = STMPE24XX_REG_GPMR_LSB,
 512         [STMPE_IDX_GPSR_LSB]    = STMPE24XX_REG_GPSR_LSB,
 513         [STMPE_IDX_GPCR_LSB]    = STMPE24XX_REG_GPCR_LSB,
 514         [STMPE_IDX_GPDR_LSB]    = STMPE24XX_REG_GPDR_LSB,
 515         [STMPE_IDX_GPRER_LSB]   = STMPE24XX_REG_GPRER_LSB,
 516         [STMPE_IDX_GPFER_LSB]   = STMPE24XX_REG_GPFER_LSB,
 517         [STMPE_IDX_GPAFR_U_MSB] = STMPE24XX_REG_GPAFR_U_MSB,
 518         [STMPE_IDX_IEGPIOR_LSB] = STMPE24XX_REG_IEGPIOR_LSB,
 519         [STMPE_IDX_ISGPIOR_MSB] = STMPE24XX_REG_ISGPIOR_MSB,
 520         [STMPE_IDX_GPEDR_MSB]   = STMPE24XX_REG_GPEDR_MSB,
 521 };
 522
 523 static struct stmpe_variant_block stmpe24xx_blocks[] = {
 524         {
 525                 .cell   = &stmpe_gpio_cell,
 526                 .irq    = STMPE24XX_IRQ_GPIOC,
 527                 .block  = STMPE_BLOCK_GPIO,
 528         },
 529         {
 530                 .cell   = &stmpe_keypad_cell,
 531                 .irq    = STMPE24XX_IRQ_KEYPAD,
 532                 .block  = STMPE_BLOCK_KEYPAD,
 533         },
 534 };
 535
 536 static int stmpe24xx_enable(struct stmpe *stmpe, unsigned int blocks,
 537                             bool enable)
 538 {
 539         unsigned int mask = 0;
 540
 541         if (blocks & STMPE_BLOCK_GPIO)
 542                 mask |= STMPE24XX_SYS_CTRL_ENABLE_GPIO;
 543
 544         if (blocks & STMPE_BLOCK_KEYPAD)
 545                 mask |= STMPE24XX_SYS_CTRL_ENABLE_KPC;
 546
 547         return __stmpe_set_bits(stmpe, STMPE24XX_REG_SYS_CTRL, mask,
 548                                 enable ? mask : 0);
 549 }
 550
 551 static int stmpe24xx_get_altfunc(struct stmpe *stmpe, enum stmpe_block block)
 552 {
 553         switch (block) {
 554         case STMPE_BLOCK_ROTATOR:
 555                 return 2;
 556
 557         case STMPE_BLOCK_KEYPAD:
 558                 return 1;
 559
 560         case STMPE_BLOCK_GPIO:
 561         default:
 562                 return 0;
 563         }
 564 }
 565
 566 static struct stmpe_variant_info stmpe2401 = {
 567         .name           = "stmpe2401",
 568         .id_val         = 0x0101,
 569         .id_mask        = 0xffff,
 570         .num_gpios      = 24,
 571         .af_bits        = 2,
 572         .regs           = stmpe24xx_regs,
 573         .blocks         = stmpe24xx_blocks,
 574         .num_blocks     = ARRAY_SIZE(stmpe24xx_blocks),
 575         .num_irqs       = STMPE24XX_NR_INTERNAL_IRQS,
 576         .enable         = stmpe24xx_enable,
 577         .get_altfunc    = stmpe24xx_get_altfunc,
 578 };
 579
 580 static struct stmpe_variant_info stmpe2403 = {
 581         .name           = "stmpe2403",
 582         .id_val         = 0x0120,
 583         .id_mask        = 0xffff,
 584         .num_gpios      = 24,
 585         .af_bits        = 2,
 586         .regs           = stmpe24xx_regs,
 587         .blocks         = stmpe24xx_blocks,
 588         .num_blocks     = ARRAY_SIZE(stmpe24xx_blocks),
 589         .num_irqs       = STMPE24XX_NR_INTERNAL_IRQS,
 590         .enable         = stmpe24xx_enable,
 591         .get_altfunc    = stmpe24xx_get_altfunc,
 592 };
 593
 594 static struct stmpe_variant_info *stmpe_variant_info[] = {
 595         [STMPE811]      = &stmpe811,
 596         [STMPE1601]     = &stmpe1601,
 597         [STMPE2401]     = &stmpe2401,
 598         [STMPE2403]     = &stmpe2403,
 599 };
 600
 601 static irqreturn_t stmpe_irq(int irq, void *data)
 602 {
 603         struct stmpe *stmpe = data;
 604         struct stmpe_variant_info *variant = stmpe->variant;
 605         int num = DIV_ROUND_UP(variant->num_irqs, 8);
 606         u8 israddr = stmpe->regs[STMPE_IDX_ISR_MSB];
 607         u8 isr[num];
 608         int ret;
 609         int i;
 610
 611         ret = stmpe_block_read(stmpe, israddr, num, isr);
 612         if (ret < 0)
 613                 return IRQ_NONE;
 614
 615         for (i = 0; i < num; i++) {
 616                 int bank = num - i - 1;
 617                 u8 status = isr[i];
 618                 u8 clear;
 619
 620                 status &= stmpe->ier[bank];
 621                 if (!status)
 622                         continue;
 623
 624                 clear = status;
 625                 while (status) {
 626                         int bit = __ffs(status);
 627                         int line = bank * 8 + bit;
 628
 629                         handle_nested_irq(stmpe->irq_base + line);
 630                         status &= ~(1 << bit);
 631                 }
 632
 633                 stmpe_reg_write(stmpe, israddr + i, clear);
 634         }
 635
 636         return IRQ_HANDLED;
 637 }
 638
 639 static void stmpe_irq_lock(unsigned int irq)
 640 {
 641         struct stmpe *stmpe = get_irq_chip_data(irq);
 642
 643         mutex_lock(&stmpe->irq_lock);
 644 }
 645
 646 static void stmpe_irq_sync_unlock(unsigned int irq)
 647 {
 648         struct stmpe *stmpe = get_irq_chip_data(irq);
 649         struct stmpe_variant_info *variant = stmpe->variant;
 650         int num = DIV_ROUND_UP(variant->num_irqs, 8);
 651         int i;
 652
 653         for (i = 0; i < num; i++) {
 654                 u8 new = stmpe->ier[i];
 655                 u8 old = stmpe->oldier[i];
 656
 657                 if (new == old)
 658                         continue;
 659
 660                 stmpe->oldier[i] = new;
 661                 stmpe_reg_write(stmpe, stmpe->regs[STMPE_IDX_IER_LSB] - i, new);
 662         }
 663
 664         mutex_unlock(&stmpe->irq_lock);
 665 }
 666
 667 static void stmpe_irq_mask(unsigned int irq)
 668 {
 669         struct stmpe *stmpe = get_irq_chip_data(irq);
 670         int offset = irq - stmpe->irq_base;
 671         int regoffset = offset / 8;
 672         int mask = 1 << (offset % 8);
 673
 674         stmpe->ier[regoffset] &= ~mask;
 675 }
 676
 677 static void stmpe_irq_unmask(unsigned int irq)
 678 {
 679         struct stmpe *stmpe = get_irq_chip_data(irq);
 680         int offset = irq - stmpe->irq_base;
 681         int regoffset = offset / 8;
 682         int mask = 1 << (offset % 8);
 683
 684         stmpe->ier[regoffset] |= mask;
 685 }
 686
 687 static struct irq_chip stmpe_irq_chip = {
 688         .name                   = "stmpe",
 689         .bus_lock               = stmpe_irq_lock,
 690         .bus_sync_unlock        = stmpe_irq_sync_unlock,
 691         .mask                   = stmpe_irq_mask,
 692         .unmask                 = stmpe_irq_unmask,
 693 };
 694
 695 static int __devinit stmpe_irq_init(struct stmpe *stmpe)
 696 {
 697         int num_irqs = stmpe->variant->num_irqs;
 698         int base = stmpe->irq_base;
 699         int irq;
 700
 701         for (irq = base; irq < base + num_irqs; irq++) {
 702                 set_irq_chip_data(irq, stmpe);
 703                 set_irq_chip_and_handler(irq, &stmpe_irq_chip,
 704                                          handle_edge_irq);
 705                 set_irq_nested_thread(irq, 1);
 706 #ifdef CONFIG_ARM
 707                 set_irq_flags(irq, IRQF_VALID);
 708 #else
 709                 set_irq_noprobe(irq);
 710 #endif
 711         }
 712
 713         return 0;
 714 }
 715
 716 static void stmpe_irq_remove(struct stmpe *stmpe)
 717 {
 718         int num_irqs = stmpe->variant->num_irqs;
 719         int base = stmpe->irq_base;
 720         int irq;
 721
 722         for (irq = base; irq < base + num_irqs; irq++) {
 723 #ifdef CONFIG_ARM
 724                 set_irq_flags(irq, 0);
 725 #endif
 726                 set_irq_chip_and_handler(irq, NULL, NULL);
 727                 set_irq_chip_data(irq, NULL);
 728         }
 729 }
 730
 731 static int __devinit stmpe_chip_init(struct stmpe *stmpe)
 732 {
 733         unsigned int irq_trigger = stmpe->pdata->irq_trigger;
 734         struct stmpe_variant_info *variant = stmpe->variant;
 735         u8 icr = STMPE_ICR_LSB_GIM;
 736         unsigned int id;
 737         u8 data[2];
 738         int ret;
 739
 740         ret = stmpe_block_read(stmpe, stmpe->regs[STMPE_IDX_CHIP_ID],
 741                                ARRAY_SIZE(data), data);
 742         if (ret < 0)
 743                 return ret;
 744
 745         id = (data[0] << 8) | data[1];
 746         if ((id & variant->id_mask) != variant->id_val) {
 747                 dev_err(stmpe->dev, "unknown chip id: %#x\n", id);
 748                 return -EINVAL;
 749         }
 750
 751         dev_info(stmpe->dev, "%s detected, chip id: %#x\n", variant->name, id);
 752
 753         /* Disable all modules -- subdrivers should enable what they need. */
 754         ret = stmpe_disable(stmpe, ~0);
 755         if (ret)
 756                 return ret;
 757
 758         if (irq_trigger == IRQF_TRIGGER_FALLING ||
 759             irq_trigger == IRQF_TRIGGER_RISING)
 760                 icr |= STMPE_ICR_LSB_EDGE;
 761
 762         if (irq_trigger == IRQF_TRIGGER_RISING ||
 763             irq_trigger == IRQF_TRIGGER_HIGH)
 764                 icr |= STMPE_ICR_LSB_HIGH;
 765
 766         if (stmpe->pdata->irq_invert_polarity)
 767                 icr ^= STMPE_ICR_LSB_HIGH;
 768
 769         return stmpe_reg_write(stmpe, stmpe->regs[STMPE_IDX_ICR_LSB], icr);
 770 }
 771
 772 static int __devinit stmpe_add_device(struct stmpe *stmpe,
 773                                       struct mfd_cell *cell, int irq)
 774 {
 775         return mfd_add_devices(stmpe->dev, stmpe->pdata->id, cell, 1,
 776                                NULL, stmpe->irq_base + irq);
 777 }
 778
 779 static int __devinit stmpe_devices_init(struct stmpe *stmpe)
 780 {
 781         struct stmpe_variant_info *variant = stmpe->variant;
 782         unsigned int platform_blocks = stmpe->pdata->blocks;
 783         int ret = -EINVAL;
 784         int i;
 785
 786         for (i = 0; i < variant->num_blocks; i++) {
 787                 struct stmpe_variant_block *block = &variant->blocks[i];
 788
 789                 if (!(platform_blocks & block->block))
 790                         continue;
 791
 792                 platform_blocks &= ~block->block;
 793                 ret = stmpe_add_device(stmpe, block->cell, block->irq);
 794                 if (ret)
 795                         return ret;
 796         }
 797
 798         if (platform_blocks)
 799                 dev_warn(stmpe->dev,
 800                          "platform wants blocks (%#x) not present on variant",
 801                          platform_blocks);
 802
 803         return ret;
 804 }
 805
 806 static int __devinit stmpe_probe(struct i2c_client *i2c,
 807                                  const struct i2c_device_id *id)
 808 {
 809         struct stmpe_platform_data *pdata = i2c->dev.platform_data;
 810         struct stmpe *stmpe;
 811         int ret;
 812
 813         if (!pdata)
 814                 return -EINVAL;
 815
 816         stmpe = kzalloc(sizeof(struct stmpe), GFP_KERNEL);
 817         if (!stmpe)
 818                 return -ENOMEM;
 819
 820         mutex_init(&stmpe->irq_lock);
 821         mutex_init(&stmpe->lock);
 822
 823         stmpe->dev = &i2c->dev;
 824         stmpe->i2c = i2c;
 825
 826         stmpe->pdata = pdata;
 827         stmpe->irq_base = pdata->irq_base;
 828
 829         stmpe->partnum = id->driver_data;
 830         stmpe->variant = stmpe_variant_info[stmpe->partnum];
 831         stmpe->regs = stmpe->variant->regs;
 832         stmpe->num_gpios = stmpe->variant->num_gpios;
 833
 834         i2c_set_clientdata(i2c, stmpe);
 835
 836         ret = stmpe_chip_init(stmpe);
 837         if (ret)
 838                 goto out_free;
 839
 840         ret = stmpe_irq_init(stmpe);
 841         if (ret)
 842                 goto out_free;
 843
 844         ret = request_threaded_irq(stmpe->i2c->irq, NULL, stmpe_irq,
 845                                    pdata->irq_trigger | IRQF_ONESHOT,
 846                                    "stmpe", stmpe);
 847         if (ret) {
 848                 dev_err(stmpe->dev, "failed to request IRQ: %d\n", ret);
 849                 goto out_removeirq;
 850         }
 851
 852         ret = stmpe_devices_init(stmpe);
 853         if (ret) {
 854                 dev_err(stmpe->dev, "failed to add children\n");
 855                 goto out_removedevs;
 856         }
 857
 858         return 0;
 859
 860 out_removedevs:
 861         mfd_remove_devices(stmpe->dev);
 862         free_irq(stmpe->i2c->irq, stmpe);
 863 out_removeirq:
 864         stmpe_irq_remove(stmpe);
 865 out_free:
 866         kfree(stmpe);
 867         return ret;
 868 }
 869
 870 static int __devexit stmpe_remove(struct i2c_client *client)
 871 {
 872         struct stmpe *stmpe = i2c_get_clientdata(client);
 873
 874         mfd_remove_devices(stmpe->dev);
 875
 876         free_irq(stmpe->i2c->irq, stmpe);
 877         stmpe_irq_remove(stmpe);
 878
 879         kfree(stmpe);
 880
 881         return 0;
 882 }
 883
 884 static const struct i2c_device_id stmpe_id[] = {
 885         { "stmpe811", STMPE811 },
 886         { "stmpe1601", STMPE1601 },
 887         { "stmpe2401", STMPE2401 },
 888         { "stmpe2403", STMPE2403 },
 889         { }
 890 };
 891 MODULE_DEVICE_TABLE(i2c, stmpe_id);
 892
 893 static struct i2c_driver stmpe_driver = {
 894         .driver.name    = "stmpe",
 895         .driver.owner   = THIS_MODULE,
 896         .probe          = stmpe_probe,
 897         .remove         = __devexit_p(stmpe_remove),
 898         .id_table       = stmpe_id,
 899 };
 900
 901 static int __init stmpe_init(void)
 902 {
 903         return i2c_add_driver(&stmpe_driver);
 904 }
 905 subsys_initcall(stmpe_init);
 906
 907 static void __exit stmpe_exit(void)
 908 {
 909         i2c_del_driver(&stmpe_driver);
 910 }
 911 module_exit(stmpe_exit);
 912
 913 MODULE_LICENSE("GPL v2");
 914 MODULE_DESCRIPTION("STMPE MFD core driver");
 915 MODULE_AUTHOR("Rabin Vincent <rabin.vincent@stericsson.com>");