1 files changed, 95 insertions, 0 deletions

diff --git a/drivers/mfd/88pm860x-core.c b/drivers/mfd/88pm860x-core.c
index 17dfe9bb6d27..78c3a9e1fa1e 100644
--- a/drivers/mfd/88pm860x-core.c
+++ b/drivers/mfd/88pm860x-core.c
@@ -503,6 +503,99 @@ static void device_irq_exit(struct pm860x_chip *chip)
                 free_irq(chip->core_irq, chip);
 }
 
+int pm8606_osc_enable(struct pm860x_chip *chip, unsigned short client)
+{
+        int ret = -EIO;
+        struct i2c_client *i2c = (chip->id == CHIP_PM8606) ?
+                chip->client : chip->companion;
+
+        dev_dbg(chip->dev, "%s(B): client=0x%x\n", __func__, client);
+        dev_dbg(chip->dev, "%s(B): vote=0x%x status=%d\n",
+                        __func__, chip->osc_vote,
+                        chip->osc_status);
+
+        mutex_lock(&chip->osc_lock);
+        /* Update voting status */
+        chip->osc_vote |= client;
+        /* If reference group is off - turn on*/
+        if (chip->osc_status != PM8606_REF_GP_OSC_ON) {
+                chip->osc_status = PM8606_REF_GP_OSC_UNKNOWN;
+                /* Enable Reference group Vsys */
+                if (pm860x_set_bits(i2c, PM8606_VSYS,
+                                PM8606_VSYS_EN, PM8606_VSYS_EN))
+                        goto out;
+
+                /*Enable Internal Oscillator */
+                if (pm860x_set_bits(i2c, PM8606_MISC,
+                                PM8606_MISC_OSC_EN, PM8606_MISC_OSC_EN))
+                        goto out;
+                /* Update status (only if writes succeed) */
+                chip->osc_status = PM8606_REF_GP_OSC_ON;
+        }
+        mutex_unlock(&chip->osc_lock);
+
+        dev_dbg(chip->dev, "%s(A): vote=0x%x status=%d ret=%d\n",
+                        __func__, chip->osc_vote,
+                        chip->osc_status, ret);
+        return 0;
+out:
+        mutex_unlock(&chip->osc_lock);
+        return ret;
+}
+
+int pm8606_osc_disable(struct pm860x_chip *chip, unsigned short client)
+{
+        int ret = -EIO;
+        struct i2c_client *i2c = (chip->id == CHIP_PM8606) ?
+                chip->client : chip->companion;
+
+        dev_dbg(chip->dev, "%s(B): client=0x%x\n", __func__, client);
+        dev_dbg(chip->dev, "%s(B): vote=0x%x status=%d\n",
+                        __func__, chip->osc_vote,
+                        chip->osc_status);
+
+        mutex_lock(&chip->osc_lock);
+        /*Update voting status */
+        chip->osc_vote &= ~(client);
+        /* If reference group is off and this is the last client to release
+         * - turn off */
+        if ((chip->osc_status != PM8606_REF_GP_OSC_OFF) &&
+                        (chip->osc_vote == REF_GP_NO_CLIENTS)) {
+                chip->osc_status = PM8606_REF_GP_OSC_UNKNOWN;
+                /* Disable Reference group Vsys */
+                if (pm860x_set_bits(i2c, PM8606_VSYS, PM8606_VSYS_EN, 0))
+                        goto out;
+                /* Disable Internal Oscillator */
+                if (pm860x_set_bits(i2c, PM8606_MISC, PM8606_MISC_OSC_EN, 0))
+                        goto out;
+                chip->osc_status = PM8606_REF_GP_OSC_OFF;
+        }
+        mutex_unlock(&chip->osc_lock);
+
+        dev_dbg(chip->dev, "%s(A): vote=0x%x status=%d ret=%d\n",
+                        __func__, chip->osc_vote,
+                        chip->osc_status, ret);
+        return 0;
+out:
+        mutex_unlock(&chip->osc_lock);
+        return ret;
+}
+
+static void __devinit device_osc_init(struct i2c_client *i2c)
+{
+        struct pm860x_chip *chip = i2c_get_clientdata(i2c);
+
+        mutex_init(&chip->osc_lock);
+        /* init portofino reference group voting and status */
+        /* Disable Reference group Vsys */
+        pm860x_set_bits(i2c, PM8606_VSYS, PM8606_VSYS_EN, 0);
+        /* Disable Internal Oscillator */
+        pm860x_set_bits(i2c, PM8606_MISC, PM8606_MISC_OSC_EN, 0);
+
+        chip->osc_vote = REF_GP_NO_CLIENTS;
+        chip->osc_status = PM8606_REF_GP_OSC_OFF;
+}
+
 static void __devinit device_bk_init(struct pm860x_chip *chip,
                                      struct pm860x_platform_data *pdata)
 {
@@ -774,6 +867,7 @@ int __devinit pm860x_device_init(struct pm860x_chip *chip,
 
         switch (chip->id) {
         case CHIP_PM8606:
+                device_osc_init(chip->client);
                 device_bk_init(chip, pdata);
                 device_led_init(chip, pdata);
                 break;
@@ -785,6 +879,7 @@ int __devinit pm860x_device_init(struct pm860x_chip *chip,
         if (chip->companion) {
                 switch (chip->id) {
                 case CHIP_PM8607:
+                        device_osc_init(chip->companion);
                         device_bk_init(chip, pdata);
                         device_led_init(chip, pdata);
                         break;