drivers/mfd/twl6040-core.c

   1 /*
   2  * MFD driver for TWL6040 audio device
   3  *
   4  * Authors:     Misael Lopez Cruz <misael.lopez@ti.com>
   5  *              Jorge Eduardo Candelaria <jorge.candelaria@ti.com>
   6  *              Peter Ujfalusi <peter.ujfalusi@ti.com>
   7  *
   8  * Copyright:   (C) 2011 Texas Instruments, Inc.
   9  *
  10  * This program is free software; you can redistribute it and/or modify
  11  * it under the terms of the GNU General Public License version 2 as
  12  * published by the Free Software Foundation.
  13  *
  14  * This program is distributed in the hope that it will be useful, but
  15  * WITHOUT ANY WARRANTY; without even the implied warranty of
  16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17  * General Public License for more details.
  18  *
  19  * You should have received a copy of the GNU General Public License
  20  * along with this program; if not, write to the Free Software
  21  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
  22  * 02110-1301 USA
  23  *
  24  */
  25
  26 #include <linux/module.h>
  27 #include <linux/types.h>
  28 #include <linux/slab.h>
  29 #include <linux/kernel.h>
  30 #include <linux/err.h>
  31 #include <linux/platform_device.h>
  32 #include <linux/of.h>
  33 #include <linux/of_irq.h>
  34 #include <linux/of_gpio.h>
  35 #include <linux/of_platform.h>
  36 #include <linux/gpio.h>
  37 #include <linux/delay.h>
  38 #include <linux/i2c.h>
  39 #include <linux/regmap.h>
  40 #include <linux/err.h>
  41 #include <linux/mfd/core.h>
  42 #include <linux/mfd/twl6040.h>
  43 #include <linux/regulator/consumer.h>
  44
  45 #define VIBRACTRL_MEMBER(reg) ((reg == TWL6040_REG_VIBCTLL) ? 0 : 1)
  46 #define TWL6040_NUM_SUPPLIES    (2)
  47
  48 static bool twl6040_has_vibra(struct twl6040_platform_data *pdata,
  49                               struct device_node *node)
  50 {
  51         if (pdata && pdata->vibra)
  52                 return true;
  53
  54 #ifdef CONFIG_OF
  55         if (of_find_node_by_name(node, "vibra"))
  56                 return true;
  57 #endif
  58
  59         return false;
  60 }
  61
  62 int twl6040_reg_read(struct twl6040 *twl6040, unsigned int reg)
  63 {
  64         int ret;
  65         unsigned int val;
  66
  67         /* Vibra control registers from cache */
  68         if (unlikely(reg == TWL6040_REG_VIBCTLL ||
  69                      reg == TWL6040_REG_VIBCTLR)) {
  70                 val = twl6040->vibra_ctrl_cache[VIBRACTRL_MEMBER(reg)];
  71         } else {
  72                 ret = regmap_read(twl6040->regmap, reg, &val);
  73                 if (ret < 0)
  74                         return ret;
  75         }
  76
  77         return val;
  78 }
  79 EXPORT_SYMBOL(twl6040_reg_read);
  80
  81 int twl6040_reg_write(struct twl6040 *twl6040, unsigned int reg, u8 val)
  82 {
  83         int ret;
  84
  85         ret = regmap_write(twl6040->regmap, reg, val);
  86         /* Cache the vibra control registers */
  87         if (reg == TWL6040_REG_VIBCTLL || reg == TWL6040_REG_VIBCTLR)
  88                 twl6040->vibra_ctrl_cache[VIBRACTRL_MEMBER(reg)] = val;
  89
  90         return ret;
  91 }
  92 EXPORT_SYMBOL(twl6040_reg_write);
  93
  94 int twl6040_set_bits(struct twl6040 *twl6040, unsigned int reg, u8 mask)
  95 {
  96         return regmap_update_bits(twl6040->regmap, reg, mask, mask);
  97 }
  98 EXPORT_SYMBOL(twl6040_set_bits);
  99
 100 int twl6040_clear_bits(struct twl6040 *twl6040, unsigned int reg, u8 mask)
 101 {
 102         return regmap_update_bits(twl6040->regmap, reg, mask, 0);
 103 }
 104 EXPORT_SYMBOL(twl6040_clear_bits);
 105
 106 /* twl6040 codec manual power-up sequence */
 107 static int twl6040_power_up_manual(struct twl6040 *twl6040)
 108 {
 109         u8 ldoctl, ncpctl, lppllctl;
 110         int ret;
 111
 112         /* enable high-side LDO, reference system and internal oscillator */
 113         ldoctl = TWL6040_HSLDOENA | TWL6040_REFENA | TWL6040_OSCENA;
 114         ret = twl6040_reg_write(twl6040, TWL6040_REG_LDOCTL, ldoctl);
 115         if (ret)
 116                 return ret;
 117         usleep_range(10000, 10500);
 118
 119         /* enable negative charge pump */
 120         ncpctl = TWL6040_NCPENA;
 121         ret = twl6040_reg_write(twl6040, TWL6040_REG_NCPCTL, ncpctl);
 122         if (ret)
 123                 goto ncp_err;
 124         usleep_range(1000, 1500);
 125
 126         /* enable low-side LDO */
 127         ldoctl |= TWL6040_LSLDOENA;
 128         ret = twl6040_reg_write(twl6040, TWL6040_REG_LDOCTL, ldoctl);
 129         if (ret)
 130                 goto lsldo_err;
 131         usleep_range(1000, 1500);
 132
 133         /* enable low-power PLL */
 134         lppllctl = TWL6040_LPLLENA;
 135         ret = twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL, lppllctl);
 136         if (ret)
 137                 goto lppll_err;
 138         usleep_range(5000, 5500);
 139
 140         /* disable internal oscillator */
 141         ldoctl &= ~TWL6040_OSCENA;
 142         ret = twl6040_reg_write(twl6040, TWL6040_REG_LDOCTL, ldoctl);
 143         if (ret)
 144                 goto osc_err;
 145
 146         return 0;
 147
 148 osc_err:
 149         lppllctl &= ~TWL6040_LPLLENA;
 150         twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL, lppllctl);
 151 lppll_err:
 152         ldoctl &= ~TWL6040_LSLDOENA;
 153         twl6040_reg_write(twl6040, TWL6040_REG_LDOCTL, ldoctl);
 154 lsldo_err:
 155         ncpctl &= ~TWL6040_NCPENA;
 156         twl6040_reg_write(twl6040, TWL6040_REG_NCPCTL, ncpctl);
 157 ncp_err:
 158         ldoctl &= ~(TWL6040_HSLDOENA | TWL6040_REFENA | TWL6040_OSCENA);
 159         twl6040_reg_write(twl6040, TWL6040_REG_LDOCTL, ldoctl);
 160
 161         dev_err(twl6040->dev, "manual power-up failed\n");
 162         return ret;
 163 }
 164
 165 /* twl6040 manual power-down sequence */
 166 static void twl6040_power_down_manual(struct twl6040 *twl6040)
 167 {
 168         u8 ncpctl, ldoctl, lppllctl;
 169
 170         ncpctl = twl6040_reg_read(twl6040, TWL6040_REG_NCPCTL);
 171         ldoctl = twl6040_reg_read(twl6040, TWL6040_REG_LDOCTL);
 172         lppllctl = twl6040_reg_read(twl6040, TWL6040_REG_LPPLLCTL);
 173
 174         /* enable internal oscillator */
 175         ldoctl |= TWL6040_OSCENA;
 176         twl6040_reg_write(twl6040, TWL6040_REG_LDOCTL, ldoctl);
 177         usleep_range(1000, 1500);
 178
 179         /* disable low-power PLL */
 180         lppllctl &= ~TWL6040_LPLLENA;
 181         twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL, lppllctl);
 182
 183         /* disable low-side LDO */
 184         ldoctl &= ~TWL6040_LSLDOENA;
 185         twl6040_reg_write(twl6040, TWL6040_REG_LDOCTL, ldoctl);
 186
 187         /* disable negative charge pump */
 188         ncpctl &= ~TWL6040_NCPENA;
 189         twl6040_reg_write(twl6040, TWL6040_REG_NCPCTL, ncpctl);
 190
 191         /* disable high-side LDO, reference system and internal oscillator */
 192         ldoctl &= ~(TWL6040_HSLDOENA | TWL6040_REFENA | TWL6040_OSCENA);
 193         twl6040_reg_write(twl6040, TWL6040_REG_LDOCTL, ldoctl);
 194 }
 195
 196 static irqreturn_t twl6040_readyint_handler(int irq, void *data)
 197 {
 198         struct twl6040 *twl6040 = data;
 199
 200         complete(&twl6040->ready);
 201
 202         return IRQ_HANDLED;
 203 }
 204
 205 static irqreturn_t twl6040_thint_handler(int irq, void *data)
 206 {
 207         struct twl6040 *twl6040 = data;
 208         u8 status;
 209
 210         status = twl6040_reg_read(twl6040, TWL6040_REG_STATUS);
 211         if (status & TWL6040_TSHUTDET) {
 212                 dev_warn(twl6040->dev, "Thermal shutdown, powering-off");
 213                 twl6040_power(twl6040, 0);
 214         } else {
 215                 dev_warn(twl6040->dev, "Leaving thermal shutdown, powering-on");
 216                 twl6040_power(twl6040, 1);
 217         }
 218
 219         return IRQ_HANDLED;
 220 }
 221
 222 static int twl6040_power_up_automatic(struct twl6040 *twl6040)
 223 {
 224         int time_left;
 225
 226         gpio_set_value(twl6040->audpwron, 1);
 227
 228         time_left = wait_for_completion_timeout(&twl6040->ready,
 229                                                 msecs_to_jiffies(144));
 230         if (!time_left) {
 231                 u8 intid;
 232
 233                 dev_warn(twl6040->dev, "timeout waiting for READYINT\n");
 234                 intid = twl6040_reg_read(twl6040, TWL6040_REG_INTID);
 235                 if (!(intid & TWL6040_READYINT)) {
 236                         dev_err(twl6040->dev, "automatic power-up failed\n");
 237                         gpio_set_value(twl6040->audpwron, 0);
 238                         return -ETIMEDOUT;
 239                 }
 240         }
 241
 242         return 0;
 243 }
 244
 245 int twl6040_power(struct twl6040 *twl6040, int on)
 246 {
 247         int ret = 0;
 248
 249         mutex_lock(&twl6040->mutex);
 250
 251         if (on) {
 252                 /* already powered-up */
 253                 if (twl6040->power_count++)
 254                         goto out;
 255
 256                 if (gpio_is_valid(twl6040->audpwron)) {
 257                         /* use automatic power-up sequence */
 258                         ret = twl6040_power_up_automatic(twl6040);
 259                         if (ret) {
 260                                 twl6040->power_count = 0;
 261                                 goto out;
 262                         }
 263                 } else {
 264                         /* use manual power-up sequence */
 265                         ret = twl6040_power_up_manual(twl6040);
 266                         if (ret) {
 267                                 twl6040->power_count = 0;
 268                                 goto out;
 269                         }
 270                 }
 271                 /* Default PLL configuration after power up */
 272                 twl6040->pll = TWL6040_SYSCLK_SEL_LPPLL;
 273                 twl6040->sysclk = 19200000;
 274                 twl6040->mclk = 32768;
 275         } else {
 276                 /* already powered-down */
 277                 if (!twl6040->power_count) {
 278                         dev_err(twl6040->dev,
 279                                 "device is already powered-off\n");
 280                         ret = -EPERM;
 281                         goto out;
 282                 }
 283
 284                 if (--twl6040->power_count)
 285                         goto out;
 286
 287                 if (gpio_is_valid(twl6040->audpwron)) {
 288                         /* use AUDPWRON line */
 289                         gpio_set_value(twl6040->audpwron, 0);
 290
 291                         /* power-down sequence latency */
 292                         usleep_range(500, 700);
 293                 } else {
 294                         /* use manual power-down sequence */
 295                         twl6040_power_down_manual(twl6040);
 296                 }
 297                 twl6040->sysclk = 0;
 298                 twl6040->mclk = 0;
 299         }
 300
 301 out:
 302         mutex_unlock(&twl6040->mutex);
 303         return ret;
 304 }
 305 EXPORT_SYMBOL(twl6040_power);
 306
 307 int twl6040_set_pll(struct twl6040 *twl6040, int pll_id,
 308                     unsigned int freq_in, unsigned int freq_out)
 309 {
 310         u8 hppllctl, lppllctl;
 311         int ret = 0;
 312
 313         mutex_lock(&twl6040->mutex);
 314
 315         hppllctl = twl6040_reg_read(twl6040, TWL6040_REG_HPPLLCTL);
 316         lppllctl = twl6040_reg_read(twl6040, TWL6040_REG_LPPLLCTL);
 317
 318         /* Force full reconfiguration when switching between PLL */
 319         if (pll_id != twl6040->pll) {
 320                 twl6040->sysclk = 0;
 321                 twl6040->mclk = 0;
 322         }
 323
 324         switch (pll_id) {
 325         case TWL6040_SYSCLK_SEL_LPPLL:
 326                 /* low-power PLL divider */
 327                 /* Change the sysclk configuration only if it has been canged */
 328                 if (twl6040->sysclk != freq_out) {
 329                         switch (freq_out) {
 330                         case 17640000:
 331                                 lppllctl |= TWL6040_LPLLFIN;
 332                                 break;
 333                         case 19200000:
 334                                 lppllctl &= ~TWL6040_LPLLFIN;
 335                                 break;
 336                         default:
 337                                 dev_err(twl6040->dev,
 338                                         "freq_out %d not supported\n",
 339                                         freq_out);
 340                                 ret = -EINVAL;
 341                                 goto pll_out;
 342                         }
 343                         twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL,
 344                                           lppllctl);
 345                 }
 346
 347                 /* The PLL in use has not been change, we can exit */
 348                 if (twl6040->pll == pll_id)
 349                         break;
 350
 351                 switch (freq_in) {
 352                 case 32768:
 353                         lppllctl |= TWL6040_LPLLENA;
 354                         twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL,
 355                                           lppllctl);
 356                         mdelay(5);
 357                         lppllctl &= ~TWL6040_HPLLSEL;
 358                         twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL,
 359                                           lppllctl);
 360                         hppllctl &= ~TWL6040_HPLLENA;
 361                         twl6040_reg_write(twl6040, TWL6040_REG_HPPLLCTL,
 362                                           hppllctl);
 363                         break;
 364                 default:
 365                         dev_err(twl6040->dev,
 366                                 "freq_in %d not supported\n", freq_in);
 367                         ret = -EINVAL;
 368                         goto pll_out;
 369                 }
 370                 break;
 371         case TWL6040_SYSCLK_SEL_HPPLL:
 372                 /* high-performance PLL can provide only 19.2 MHz */
 373                 if (freq_out != 19200000) {
 374                         dev_err(twl6040->dev,
 375                                 "freq_out %d not supported\n", freq_out);
 376                         ret = -EINVAL;
 377                         goto pll_out;
 378                 }
 379
 380                 if (twl6040->mclk != freq_in) {
 381                         hppllctl &= ~TWL6040_MCLK_MSK;
 382
 383                         switch (freq_in) {
 384                         case 12000000:
 385                                 /* PLL enabled, active mode */
 386                                 hppllctl |= TWL6040_MCLK_12000KHZ |
 387                                             TWL6040_HPLLENA;
 388                                 break;
 389                         case 19200000:
 390                                 /*
 391                                 * PLL disabled
 392                                 * (enable PLL if MCLK jitter quality
 393                                 *  doesn't meet specification)
 394                                 */
 395                                 hppllctl |= TWL6040_MCLK_19200KHZ;
 396                                 break;
 397                         case 26000000:
 398                                 /* PLL enabled, active mode */
 399                                 hppllctl |= TWL6040_MCLK_26000KHZ |
 400                                             TWL6040_HPLLENA;
 401                                 break;
 402                         case 38400000:
 403                                 /* PLL enabled, active mode */
 404                                 hppllctl |= TWL6040_MCLK_38400KHZ |
 405                                             TWL6040_HPLLENA;
 406                                 break;
 407                         default:
 408                                 dev_err(twl6040->dev,
 409                                         "freq_in %d not supported\n", freq_in);
 410                                 ret = -EINVAL;
 411                                 goto pll_out;
 412                         }
 413
 414                         /*
 415                          * enable clock slicer to ensure input waveform is
 416                          * square
 417                          */
 418                         hppllctl |= TWL6040_HPLLSQRENA;
 419
 420                         twl6040_reg_write(twl6040, TWL6040_REG_HPPLLCTL,
 421                                           hppllctl);
 422                         usleep_range(500, 700);
 423                         lppllctl |= TWL6040_HPLLSEL;
 424                         twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL,
 425                                           lppllctl);
 426                         lppllctl &= ~TWL6040_LPLLENA;
 427                         twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL,
 428                                           lppllctl);
 429                 }
 430                 break;
 431         default:
 432                 dev_err(twl6040->dev, "unknown pll id %d\n", pll_id);
 433                 ret = -EINVAL;
 434                 goto pll_out;
 435         }
 436
 437         twl6040->sysclk = freq_out;
 438         twl6040->mclk = freq_in;
 439         twl6040->pll = pll_id;
 440
 441 pll_out:
 442         mutex_unlock(&twl6040->mutex);
 443         return ret;
 444 }
 445 EXPORT_SYMBOL(twl6040_set_pll);
 446
 447 int twl6040_get_pll(struct twl6040 *twl6040)
 448 {
 449         if (twl6040->power_count)
 450                 return twl6040->pll;
 451         else
 452                 return -ENODEV;
 453 }
 454 EXPORT_SYMBOL(twl6040_get_pll);
 455
 456 unsigned int twl6040_get_sysclk(struct twl6040 *twl6040)
 457 {
 458         return twl6040->sysclk;
 459 }
 460 EXPORT_SYMBOL(twl6040_get_sysclk);
 461
 462 /* Get the combined status of the vibra control register */
 463 int twl6040_get_vibralr_status(struct twl6040 *twl6040)
 464 {
 465         u8 status;
 466
 467         status = twl6040->vibra_ctrl_cache[0] | twl6040->vibra_ctrl_cache[1];
 468         status &= (TWL6040_VIBENA | TWL6040_VIBSEL);
 469
 470         return status;
 471 }
 472 EXPORT_SYMBOL(twl6040_get_vibralr_status);
 473
 474 static struct resource twl6040_vibra_rsrc[] = {
 475         {
 476                 .flags = IORESOURCE_IRQ,
 477         },
 478 };
 479
 480 static struct resource twl6040_codec_rsrc[] = {
 481         {
 482                 .flags = IORESOURCE_IRQ,
 483         },
 484 };
 485
 486 static bool twl6040_readable_reg(struct device *dev, unsigned int reg)
 487 {
 488         /* Register 0 is not readable */
 489         if (!reg)
 490                 return false;
 491         return true;
 492 }
 493
 494 static struct regmap_config twl6040_regmap_config = {
 495         .reg_bits = 8,
 496         .val_bits = 8,
 497         .max_register = TWL6040_REG_STATUS, /* 0x2e */
 498
 499         .readable_reg = twl6040_readable_reg,
 500 };
 501
 502 static int __devinit twl6040_probe(struct i2c_client *client,
 503                                      const struct i2c_device_id *id)
 504 {
 505         struct twl6040_platform_data *pdata = client->dev.platform_data;
 506         struct device_node *node = client->dev.of_node;
 507         struct twl6040 *twl6040;
 508         struct mfd_cell *cell = NULL;
 509         int irq, ret, children = 0;
 510
 511         if (!pdata && !node) {
 512                 dev_err(&client->dev, "Platform data is missing\n");
 513                 return -EINVAL;
 514         }
 515
 516         /* In order to operate correctly we need valid interrupt config */
 517         if (!client->irq) {
 518                 dev_err(&client->dev, "Invalid IRQ configuration\n");
 519                 return -EINVAL;
 520         }
 521
 522         twl6040 = devm_kzalloc(&client->dev, sizeof(struct twl6040),
 523                                GFP_KERNEL);
 524         if (!twl6040) {
 525                 ret = -ENOMEM;
 526                 goto err;
 527         }
 528
 529         twl6040->regmap = devm_regmap_init_i2c(client, &twl6040_regmap_config);
 530         if (IS_ERR(twl6040->regmap)) {
 531                 ret = PTR_ERR(twl6040->regmap);
 532                 goto err;
 533         }
 534
 535         i2c_set_clientdata(client, twl6040);
 536
 537         twl6040->supplies[0].supply = "vio";
 538         twl6040->supplies[1].supply = "v2v1";
 539         ret = regulator_bulk_get(&client->dev, TWL6040_NUM_SUPPLIES,
 540                                  twl6040->supplies);
 541         if (ret != 0) {
 542                 dev_err(&client->dev, "Failed to get supplies: %d\n", ret);
 543                 goto regulator_get_err;
 544         }
 545
 546         ret = regulator_bulk_enable(TWL6040_NUM_SUPPLIES, twl6040->supplies);
 547         if (ret != 0) {
 548                 dev_err(&client->dev, "Failed to enable supplies: %d\n", ret);
 549                 goto power_err;
 550         }
 551
 552         twl6040->dev = &client->dev;
 553         twl6040->irq = client->irq;
 554
 555         mutex_init(&twl6040->mutex);
 556         init_completion(&twl6040->ready);
 557
 558         twl6040->rev = twl6040_reg_read(twl6040, TWL6040_REG_ASICREV);
 559
 560         /* ERRATA: Automatic power-up is not possible in ES1.0 */
 561         if (twl6040_get_revid(twl6040) > TWL6040_REV_ES1_0) {
 562                 if (pdata)
 563                         twl6040->audpwron = pdata->audpwron_gpio;
 564                 else
 565                         twl6040->audpwron = of_get_named_gpio(node,
 566                                                 "ti,audpwron-gpio", 0);
 567         } else
 568                 twl6040->audpwron = -EINVAL;
 569
 570         if (gpio_is_valid(twl6040->audpwron)) {
 571                 ret = gpio_request_one(twl6040->audpwron, GPIOF_OUT_INIT_LOW,
 572                                        "audpwron");
 573                 if (ret)
 574                         goto gpio_err;
 575         }
 576
 577         /* codec interrupt */
 578         ret = twl6040_irq_init(twl6040);
 579         if (ret)
 580                 goto irq_init_err;
 581
 582         ret = request_threaded_irq(twl6040->irq_base + TWL6040_IRQ_READY,
 583                                    NULL, twl6040_readyint_handler, IRQF_ONESHOT,
 584                                    "twl6040_irq_ready", twl6040);
 585         if (ret) {
 586                 dev_err(twl6040->dev, "READY IRQ request failed: %d\n", ret);
 587                 goto readyirq_err;
 588         }
 589
 590         ret = request_threaded_irq(twl6040->irq_base + TWL6040_IRQ_TH,
 591                                    NULL, twl6040_thint_handler, IRQF_ONESHOT,
 592                                    "twl6040_irq_th", twl6040);
 593         if (ret) {
 594                 dev_err(twl6040->dev, "Thermal IRQ request failed: %d\n", ret);
 595                 goto thirq_err;
 596         }
 597
 598         /* dual-access registers controlled by I2C only */
 599         twl6040_set_bits(twl6040, TWL6040_REG_ACCCTL, TWL6040_I2CSEL);
 600
 601         /*
 602          * The main functionality of twl6040 to provide audio on OMAP4+ systems.
 603          * We can add the ASoC codec child whenever this driver has been loaded.
 604          * The ASoC codec can work without pdata, pass the platform_data only if
 605          * it has been provided.
 606          */
 607         irq = twl6040->irq_base + TWL6040_IRQ_PLUG;
 608         cell = &twl6040->cells[children];
 609         cell->name = "twl6040-codec";
 610         twl6040_codec_rsrc[0].start = irq;
 611         twl6040_codec_rsrc[0].end = irq;
 612         cell->resources = twl6040_codec_rsrc;
 613         cell->num_resources = ARRAY_SIZE(twl6040_codec_rsrc);
 614         if (pdata && pdata->codec) {
 615                 cell->platform_data = pdata->codec;
 616                 cell->pdata_size = sizeof(*pdata->codec);
 617         }
 618         children++;
 619
 620         if (twl6040_has_vibra(pdata, node)) {
 621                 irq = twl6040->irq_base + TWL6040_IRQ_VIB;
 622
 623                 cell = &twl6040->cells[children];
 624                 cell->name = "twl6040-vibra";
 625                 twl6040_vibra_rsrc[0].start = irq;
 626                 twl6040_vibra_rsrc[0].end = irq;
 627                 cell->resources = twl6040_vibra_rsrc;
 628                 cell->num_resources = ARRAY_SIZE(twl6040_vibra_rsrc);
 629
 630                 if (pdata && pdata->vibra) {
 631                         cell->platform_data = pdata->vibra;
 632                         cell->pdata_size = sizeof(*pdata->vibra);
 633                 }
 634                 children++;
 635         }
 636
 637         /*
 638          * Enable the GPO driver in the following cases:
 639          * DT booted kernel or legacy boot with valid gpo platform_data
 640          */
 641         if (!pdata || (pdata && pdata->gpo)) {
 642                 cell = &twl6040->cells[children];
 643                 cell->name = "twl6040-gpo";
 644
 645                 if (pdata) {
 646                         cell->platform_data = pdata->gpo;
 647                         cell->pdata_size = sizeof(*pdata->gpo);
 648                 }
 649                 children++;
 650         }
 651
 652         ret = mfd_add_devices(&client->dev, -1, twl6040->cells, children,
 653                               NULL, 0, NULL);
 654         if (ret)
 655                 goto mfd_err;
 656
 657         return 0;
 658
 659 mfd_err:
 660         free_irq(twl6040->irq_base + TWL6040_IRQ_TH, twl6040);
 661 thirq_err:
 662         free_irq(twl6040->irq_base + TWL6040_IRQ_READY, twl6040);
 663 readyirq_err:
 664         twl6040_irq_exit(twl6040);
 665 irq_init_err:
 666         if (gpio_is_valid(twl6040->audpwron))
 667                 gpio_free(twl6040->audpwron);
 668 gpio_err:
 669         regulator_bulk_disable(TWL6040_NUM_SUPPLIES, twl6040->supplies);
 670 power_err:
 671         regulator_bulk_free(TWL6040_NUM_SUPPLIES, twl6040->supplies);
 672 regulator_get_err:
 673         i2c_set_clientdata(client, NULL);
 674 err:
 675         return ret;
 676 }
 677
 678 static int __devexit twl6040_remove(struct i2c_client *client)
 679 {
 680         struct twl6040 *twl6040 = i2c_get_clientdata(client);
 681
 682         if (twl6040->power_count)
 683                 twl6040_power(twl6040, 0);
 684
 685         if (gpio_is_valid(twl6040->audpwron))
 686                 gpio_free(twl6040->audpwron);
 687
 688         free_irq(twl6040->irq_base + TWL6040_IRQ_READY, twl6040);
 689         free_irq(twl6040->irq_base + TWL6040_IRQ_TH, twl6040);
 690         twl6040_irq_exit(twl6040);
 691
 692         mfd_remove_devices(&client->dev);
 693         i2c_set_clientdata(client, NULL);
 694
 695         regulator_bulk_disable(TWL6040_NUM_SUPPLIES, twl6040->supplies);
 696         regulator_bulk_free(TWL6040_NUM_SUPPLIES, twl6040->supplies);
 697
 698         return 0;
 699 }
 700
 701 static const struct i2c_device_id twl6040_i2c_id[] = {
 702         { "twl6040", 0, },
 703         { "twl6041", 0, },
 704         { },
 705 };
 706 MODULE_DEVICE_TABLE(i2c, twl6040_i2c_id);
 707
 708 static struct i2c_driver twl6040_driver = {
 709         .driver = {
 710                 .name = "twl6040",
 711                 .owner = THIS_MODULE,
 712         },
 713         .probe          = twl6040_probe,
 714         .remove         = __devexit_p(twl6040_remove),
 715         .id_table       = twl6040_i2c_id,
 716 };
 717
 718 module_i2c_driver(twl6040_driver);
 719
 720 MODULE_DESCRIPTION("TWL6040 MFD");
 721 MODULE_AUTHOR("Misael Lopez Cruz <misael.lopez@ti.com>");
 722 MODULE_AUTHOR("Jorge Eduardo Candelaria <jorge.candelaria@ti.com>");
 723 MODULE_LICENSE("GPL");
 724 MODULE_ALIAS("platform:twl6040");