mfd: twl-core: add atomic read-modify-write function

[pandora-kernel.git] / drivers / mfd / twl4030-madc.c

diff --git a/drivers/mfd/twl4030-madc.c b/drivers/mfd/twl4030-madc.c
index 7cbf2aa..834f824 100644 (file)
--- a/drivers/mfd/twl4030-madc.c
+++ b/drivers/mfd/twl4030-madc.c
@@ -740,6 +740,28 @@ static int __devinit twl4030_madc_probe(struct platform_device *pdev)
                        TWL4030_BCI_BCICTL1);
                goto err_i2c;
        }
+
+       /* Check that MADC clock is on */
+       ret = twl_i2c_read_u8(TWL4030_MODULE_INTBR, &regval, TWL4030_REG_GPBR1);
+       if (ret) {
+               dev_err(&pdev->dev, "unable to read reg GPBR1 0x%X\n",
+                               TWL4030_REG_GPBR1);
+               goto err_i2c;
+       }
+
+       /* If MADC clk is not on, turn it on */
+       if (!(regval & TWL4030_GPBR1_MADC_HFCLK_EN)) {
+               dev_info(&pdev->dev, "clk disabled, enabling\n");
+               regval |= TWL4030_GPBR1_MADC_HFCLK_EN;
+               ret = twl_i2c_write_u8(TWL4030_MODULE_INTBR, regval,
+                                      TWL4030_REG_GPBR1);
+               if (ret) {
+                       dev_err(&pdev->dev, "unable to write reg GPBR1 0x%X\n",
+                                       TWL4030_REG_GPBR1);
+                       goto err_i2c;
+               }
+       }
+
        platform_set_drvdata(pdev, madc);
        mutex_init(&madc->lock);
        ret = request_threaded_irq(platform_get_irq(pdev, 0), NULL,