[PATCH] TWL4030: add function to send PB messages

[PATCH] TWL4030: add function to send PB messages

[Peter 'p2' De Schrijver]

And now with fix to make it compile.

This patch moves sending of powerbus messages to a separate function. It also
makes sure I2C access to the powerbus is enabled.

Signed-off-by: Peter 'p2' De Schrijver <peter.de-schrijver@nokia.com>
---
 drivers/regulator/twl4030-regulator.c |   72 +++++++++++++++++++++++++++++---
 1 files changed, 65 insertions(+), 7 deletions(-)

diff --git a/drivers/regulator/twl4030-regulator.c b/drivers/regulator/twl4030-regulator.c
index 472c35a..df9a94b 100644
--- a/drivers/regulator/twl4030-regulator.c
+++ b/drivers/regulator/twl4030-regulator.c
@@ -16,6 +16,7 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/i2c/twl4030.h>
+#include <linux/delay.h>
 
 
 /*
@@ -81,6 +82,69 @@ twl4030reg_write(struct twlreg_info *info, unsigned offset, u8 value)
 			value, info->base + offset);
 }
 
+static int twl4030_wait_pb_ready(void)
+{
+
+	u8 pb_status;
+	int status, timeout = 10;
+
+	do {
+		status = twl4030_i2c_read_u8(TWL4030_MODULE_PM_MASTER,
+						&pb_status, 0x14);
+		if (status < 0)
+			return status;
+
+		if (!(pb_status & 1))
+			return 0;
+
+		mdelay(1);
+		timeout--;
+
+	} while (timeout);
+
+	return -ETIMEDOUT;
+}
+
+static int twl4030_send_pb_msg(unsigned msg)
+{
+
+	u8 pb_state;
+	int status;
+
+	/* save powerbus configuration */
+	status = twl4030_i2c_read_u8(TWL4030_MODULE_PM_MASTER,
+					&pb_state, 0x14);
+	if (status < 0)
+		return status;
+
+	/* Enable I2C access to powerbus */
+	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
+					pb_state | (1<<1), 0x14);
+	if (status < 0)
+		return status;
+
+	status = twl4030_wait_pb_ready();
+	if (status < 0)
+		return status;
+
+	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, msg >> 8,
+			0x15 /* PB_WORD_MSB */);
+	if (status < 0)
+		return status;
+
+	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, msg & 0xff,
+			0x16 /* PB_WORD_LSB */);
+	if (status < 0)
+		return status;
+
+	status = twl4030_wait_pb_ready();
+	if (status < 0)
+		return status;
+
+	/* Restore powerbus configuration */
+	return twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, pb_state, 0x14);
+}
+
 /*----------------------------------------------------------------------*/
 
 /* generic power resource operations, which work on all regulators */
@@ -177,13 +241,7 @@ static int twl4030reg_set_mode(struct regulator_dev *rdev, unsigned mode)
 	if (!(status & (P3_GRP | P2_GRP | P1_GRP)))
 		return -EACCES;
 
-	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
-			message >> 8, 0x15 /* PB_WORD_MSB */ );
-	if (status >= 0)
-		return status;
-
-	return twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
-			message, 0x16 /* PB_WORD_LSB */ );
+	return twl4030_send_pb_msg(message);
 }
 
 /*----------------------------------------------------------------------*/
-- 
1.5.6.3

[PATCH] TWL4030: Make sure the regulator is active after calling twl4030reg_enable

[Peter 'p2' De Schrijver]

This patch makes sure a regulator is active when enabled. After a warm reboot, only adding
a regulator to a power group is not enough to activate it.

Signed-off-by: Peter 'p2' De Schrijver <peter.de-schrijver@nokia.com>
---
 drivers/regulator/twl4030-regulator.c |   11 +++++++++--
 1 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/drivers/regulator/twl4030-regulator.c b/drivers/regulator/twl4030-regulator.c
index df9a94b..80a4e10 100644
--- a/drivers/regulator/twl4030-regulator.c
+++ b/drivers/regulator/twl4030-regulator.c
@@ -177,14 +177,21 @@ static int twl4030reg_is_enabled(struct regulator_dev *rdev)
 static int twl4030reg_enable(struct regulator_dev *rdev)
 {
 	struct twlreg_info	*info = rdev_get_drvdata(rdev);
-	int			grp;
+	int			grp, status;
+	unsigned 		message;
 
 	grp = twl4030reg_read(info, VREG_GRP);
 	if (grp < 0)
 		return grp;
 
 	grp |= P1_GRP;
-	return twl4030reg_write(info, VREG_GRP, grp);
+	status =  twl4030reg_write(info, VREG_GRP, grp);
+	if (status < 0)
+		return status;
+
+	message = MSG_SINGULAR(DEV_GRP_P1, info->id, RES_STATE_ACTIVE);
+
+	return twl4030_send_pb_msg(message);
 }
 
 static int twl4030reg_disable(struct regulator_dev *rdev)
-- 
1.5.6.3

Re: [PATCH] TWL4030: add function to send PB messages

[Mark Brown]

On Thu, Apr 23, 2009 at 04:08:08PM +0300, Peter 'p2' De Schrijver wrote:
> And now with fix to make it compile.
> 
> This patch moves sending of powerbus messages to a separate function. It also
> makes sure I2C access to the powerbus is enabled.
> 
> Signed-off-by: Peter 'p2' De Schrijver <peter.de-schrijver@nokia.com>

CCing in Liam (the primary regulator maintainer) and David (the TWL4030
regulator expert).  When submitting patches it is always best to CC the
relevant maintainers, it is extremely easy for things to get dropped on
the floor otherwise.

> ---
>  drivers/regulator/twl4030-regulator.c |   72 +++++++++++++++++++++++++++++---
>  1 files changed, 65 insertions(+), 7 deletions(-)
> 
> diff --git a/drivers/regulator/twl4030-regulator.c b/drivers/regulator/twl4030-regulator.c
> index 472c35a..df9a94b 100644
> --- a/drivers/regulator/twl4030-regulator.c
> +++ b/drivers/regulator/twl4030-regulator.c
> @@ -16,6 +16,7 @@
>  #include <linux/regulator/driver.h>
>  #include <linux/regulator/machine.h>
>  #include <linux/i2c/twl4030.h>
> +#include <linux/delay.h>
>  
>  
>  /*
> @@ -81,6 +82,69 @@ twl4030reg_write(struct twlreg_info *info, unsigned offset, u8 value)
>  			value, info->base + offset);
>  }
>  
> +static int twl4030_wait_pb_ready(void)
> +{
> +
> +	u8 pb_status;
> +	int status, timeout = 10;
> +
> +	do {
> +		status = twl4030_i2c_read_u8(TWL4030_MODULE_PM_MASTER,
> +						&pb_status, 0x14);
> +		if (status < 0)
> +			return status;
> +
> +		if (!(pb_status & 1))
> +			return 0;
> +
> +		mdelay(1);
> +		timeout--;
> +
> +	} while (timeout);
> +
> +	return -ETIMEDOUT;
> +}
> +
> +static int twl4030_send_pb_msg(unsigned msg)
> +{
> +
> +	u8 pb_state;
> +	int status;
> +
> +	/* save powerbus configuration */
> +	status = twl4030_i2c_read_u8(TWL4030_MODULE_PM_MASTER,
> +					&pb_state, 0x14);
> +	if (status < 0)
> +		return status;
> +
> +	/* Enable I2C access to powerbus */
> +	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
> +					pb_state | (1<<1), 0x14);
> +	if (status < 0)
> +		return status;
> +
> +	status = twl4030_wait_pb_ready();
> +	if (status < 0)
> +		return status;
> +
> +	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, msg >> 8,
> +			0x15 /* PB_WORD_MSB */);
> +	if (status < 0)
> +		return status;
> +
> +	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, msg & 0xff,
> +			0x16 /* PB_WORD_LSB */);
> +	if (status < 0)
> +		return status;
> +
> +	status = twl4030_wait_pb_ready();
> +	if (status < 0)
> +		return status;
> +
> +	/* Restore powerbus configuration */
> +	return twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, pb_state, 0x14);
> +}
> +
>  /*----------------------------------------------------------------------*/
>  
>  /* generic power resource operations, which work on all regulators */
> @@ -177,13 +241,7 @@ static int twl4030reg_set_mode(struct regulator_dev *rdev, unsigned mode)
>  	if (!(status & (P3_GRP | P2_GRP | P1_GRP)))
>  		return -EACCES;
>  
> -	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
> -			message >> 8, 0x15 /* PB_WORD_MSB */ );
> -	if (status >= 0)
> -		return status;
> -
> -	return twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
> -			message, 0x16 /* PB_WORD_LSB */ );
> +	return twl4030_send_pb_msg(message);
>  }
>  
>  /*----------------------------------------------------------------------*/
> -- 
> 1.5.6.3
> 
> --
> To unsubscribe from this list: send the line "unsubscribe linux-kernel" in
> the body of a message to majordomo@vger.kernel.org
> More majordomo info at  http://vger.kernel.org/majordomo-info.html
> Please read the FAQ at  http://www.tux.org/lkml/
> 

Re: [PATCH] TWL4030: Make sure the regulator is active after calling twl4030reg_enable

[Mark Brown]

On Thu, Apr 23, 2009 at 04:08:09PM +0300, Peter 'p2' De Schrijver wrote:
> This patch makes sure a regulator is active when enabled. After a warm reboot, only adding
> a regulator to a power group is not enough to activate it.

> Signed-off-by: Peter 'p2' De Schrijver <peter.de-schrijver@nokia.com>

CCing people in again.

> ---
>  drivers/regulator/twl4030-regulator.c |   11 +++++++++--
>  1 files changed, 9 insertions(+), 2 deletions(-)
> 
> diff --git a/drivers/regulator/twl4030-regulator.c b/drivers/regulator/twl4030-regulator.c
> index df9a94b..80a4e10 100644
> --- a/drivers/regulator/twl4030-regulator.c
> +++ b/drivers/regulator/twl4030-regulator.c
> @@ -177,14 +177,21 @@ static int twl4030reg_is_enabled(struct regulator_dev *rdev)
>  static int twl4030reg_enable(struct regulator_dev *rdev)
>  {
>  	struct twlreg_info	*info = rdev_get_drvdata(rdev);
> -	int			grp;
> +	int			grp, status;
> +	unsigned 		message;
>  
>  	grp = twl4030reg_read(info, VREG_GRP);
>  	if (grp < 0)
>  		return grp;
>  
>  	grp |= P1_GRP;
> -	return twl4030reg_write(info, VREG_GRP, grp);
> +	status =  twl4030reg_write(info, VREG_GRP, grp);
> +	if (status < 0)
> +		return status;
> +
> +	message = MSG_SINGULAR(DEV_GRP_P1, info->id, RES_STATE_ACTIVE);
> +
> +	return twl4030_send_pb_msg(message);
>  }
>  
>  static int twl4030reg_disable(struct regulator_dev *rdev)
> -- 
> 1.5.6.3
> 
> --
> To unsubscribe from this list: send the line "unsubscribe linux-kernel" in
> the body of a message to majordomo@vger.kernel.org
> More majordomo info at  http://vger.kernel.org/majordomo-info.html
> Please read the FAQ at  http://www.tux.org/lkml/
> 

-- 
"You grabbed my hand and we fell into it, like a daydream - or a fever."

Re: [PATCH] TWL4030: add function to send PB messages

[Peter 'p2' De Schrijver]

> > @@ -81,6 +82,69 @@ twl4030reg_write(struct twlreg_info *info, unsigned offset, u8 value)
> >  			value, info->base + offset);
> >  }
> >  
> > +static int twl4030_wait_pb_ready(void)
> > +{
> > +
> > +	u8 pb_status;
> > +	int status, timeout = 10;
> > +
> > +	do {
> > +		status = twl4030_i2c_read_u8(TWL4030_MODULE_PM_MASTER,
> > +						&pb_status, 0x14);
> > +		if (status < 0)
> > +			return status;
> > +
> 
> Worth a comment that PB_CFG.BIT(0) == PB_I2C_BUSY ... true if there's
> a word queued for the power bus, but not yet sent.  And that we assume
> no other I2C master is sending such events...
> 

The multi master situation seems inherently racy to me anyway as you
need to write 2 bytes to 2 different registers to send 1 message. Or is there
a way to keep mastership of the bus between transactions ?

> > +	/* Enable I2C access to powerbus */
> > +	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
> > +					pb_state | (1<<1), 0x14);
> > +	if (status < 0)
> > +		return status;
> > +
> > +	status = twl4030_wait_pb_ready();
> 
> I'd probably combine wait_pb_ready() with this; not that
> this is wrong, but you should only need to set BIT(1) once,
> and there's no need to re-read that byte to test BIT(0).
> 
> Minor point ... I hate needless I/O.  This isn't a critical
> path though.
> 

Indeed. Exactly why I didn't try to optimize on I/O here :)

Cheers,

Peter.

-- 
goa is a state of mind

Re: [PATCH] TWL4030: add function to send PB messages

[David Brownell]

On Wednesday 22 April 2009, Peter 'p2' De Schrijver wrote:
> This patch moves sending of powerbus messages to a separate function.

Can you add #defines for those register offsets, and use
those as names instead of 0x14/0x15/0x16?  And at least
use BIT(x) instead of (1 << x).

If this moves into twl4030_core.c (as I ask about elsewhere),
those registers can just be defines local to that file.

Otherwise this seems ok.


> It also 
> makes sure I2C access to the powerbus is enabled.
> 
> Signed-off-by: Peter 'p2' De Schrijver <peter.de-schrijver@nokia.com>
> ---
>  drivers/regulator/twl4030-regulator.c |   72 +++++++++++++++++++++++++++++---
>  1 files changed, 65 insertions(+), 7 deletions(-)
> 
> diff --git a/drivers/regulator/twl4030-regulator.c b/drivers/regulator/twl4030-regulator.c
> index 472c35a..df9a94b 100644
> --- a/drivers/regulator/twl4030-regulator.c
> +++ b/drivers/regulator/twl4030-regulator.c
> @@ -16,6 +16,7 @@
>  #include <linux/regulator/driver.h>
>  #include <linux/regulator/machine.h>
>  #include <linux/i2c/twl4030.h>
> +#include <linux/delay.h>
>  
>  
>  /*
> @@ -81,6 +82,69 @@ twl4030reg_write(struct twlreg_info *info, unsigned offset, u8 value)
>  			value, info->base + offset);
>  }
>  
> +static int twl4030_wait_pb_ready(void)
> +{
> +
> +	u8 pb_status;
> +	int status, timeout = 10;
> +
> +	do {
> +		status = twl4030_i2c_read_u8(TWL4030_MODULE_PM_MASTER,
> +						&pb_status, 0x14);
> +		if (status < 0)
> +			return status;
> +

Worth a comment that PB_CFG.BIT(0) == PB_I2C_BUSY ... true if there's
a word queued for the power bus, but not yet sent.  And that we assume
no other I2C master is sending such events...


> +		if (!(pb_status & 1))
> +			return 0;
> +
> +		mdelay(1);
> +		timeout--;
> +
> +	} while (timeout);
> +
> +	return -ETIMEDOUT;
> +}
> +
> +static int twl4030_send_pb_msg(unsigned msg)
> +{
> +
> +	u8 pb_state;
> +	int status;
> +
> +	/* save powerbus configuration */
> +	status = twl4030_i2c_read_u8(TWL4030_MODULE_PM_MASTER,
> +					&pb_state, 0x14);
> +	if (status < 0)
> +		return status;
> +
> +	/* Enable I2C access to powerbus */
> +	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
> +					pb_state | (1<<1), 0x14);
> +	if (status < 0)
> +		return status;
> +
> +	status = twl4030_wait_pb_ready();

I'd probably combine wait_pb_ready() with this; not that
this is wrong, but you should only need to set BIT(1) once,
and there's no need to re-read that byte to test BIT(0).

Minor point ... I hate needless I/O.  This isn't a critical
path though.


> +	if (status < 0)
> +		return status;
> +
> +	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, msg >> 8,
> +			0x15 /* PB_WORD_MSB */);
> +	if (status < 0)
> +		return status;
> +
> +	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, msg & 0xff,
> +			0x16 /* PB_WORD_LSB */);
> +	if (status < 0)
> +		return status;
> +
> +	status = twl4030_wait_pb_ready();
> +	if (status < 0)
> +		return status;
> +
> +	/* Restore powerbus configuration */
> +	return twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, pb_state, 0x14);
> +}
> +
>  /*----------------------------------------------------------------------*/
>  
>  /* generic power resource operations, which work on all regulators */
> @@ -177,13 +241,7 @@ static int twl4030reg_set_mode(struct regulator_dev *rdev, unsigned mode)
>  	if (!(status & (P3_GRP | P2_GRP | P1_GRP)))
>  		return -EACCES;
>  
> -	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
> -			message >> 8, 0x15 /* PB_WORD_MSB */ );
> -	if (status >= 0)
> -		return status;
> -
> -	return twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
> -			message, 0x16 /* PB_WORD_LSB */ );
> +	return twl4030_send_pb_msg(message);
>  }
>  
>  /*----------------------------------------------------------------------*/
> -- 
> 1.5.6.3
> 
> --
> To unsubscribe from this list: send the line "unsubscribe linux-omap" in
> the body of a message to majordomo@vger.kernel.org
> More majordomo info at  http://vger.kernel.org/majordomo-info.html
> 
> 

[PATCH] TWL4030: add function to send PB messages

[Peter 'p2' De Schrijver]

And now with fix to make it compile.

This patch moves sending of powerbus messages to a separate function. It also
makes sure I2C access to the powerbus is enabled.

Signed-off-by: Peter 'p2' De Schrijver <peter.de-schrijver@nokia.com>
---
 drivers/regulator/twl4030-regulator.c |   72 +++++++++++++++++++++++++++++---
 1 files changed, 65 insertions(+), 7 deletions(-)

diff --git a/drivers/regulator/twl4030-regulator.c b/drivers/regulator/twl4030-regulator.c
index 472c35a..df9a94b 100644
--- a/drivers/regulator/twl4030-regulator.c
+++ b/drivers/regulator/twl4030-regulator.c
@@ -16,6 +16,7 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/i2c/twl4030.h>
+#include <linux/delay.h>
 
 
 /*
@@ -81,6 +82,69 @@ twl4030reg_write(struct twlreg_info *info, unsigned offset, u8 value)
 			value, info->base + offset);
 }
 
+static int twl4030_wait_pb_ready(void)
+{
+
+	u8 pb_status;
+	int status, timeout = 10;
+
+	do {
+		status = twl4030_i2c_read_u8(TWL4030_MODULE_PM_MASTER,
+						&pb_status, 0x14);
+		if (status < 0)
+			return status;
+
+		if (!(pb_status & 1))
+			return 0;
+
+		mdelay(1);
+		timeout--;
+
+	} while (timeout);
+
+	return -ETIMEDOUT;
+}
+
+static int twl4030_send_pb_msg(unsigned msg)
+{
+
+	u8 pb_state;
+	int status;
+
+	/* save powerbus configuration */
+	status = twl4030_i2c_read_u8(TWL4030_MODULE_PM_MASTER,
+					&pb_state, 0x14);
+	if (status < 0)
+		return status;
+
+	/* Enable I2C access to powerbus */
+	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
+					pb_state | (1<<1), 0x14);
+	if (status < 0)
+		return status;
+
+	status = twl4030_wait_pb_ready();
+	if (status < 0)
+		return status;
+
+	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, msg >> 8,
+			0x15 /* PB_WORD_MSB */);
+	if (status < 0)
+		return status;
+
+	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, msg & 0xff,
+			0x16 /* PB_WORD_LSB */);
+	if (status < 0)
+		return status;
+
+	status = twl4030_wait_pb_ready();
+	if (status < 0)
+		return status;
+
+	/* Restore powerbus configuration */
+	return twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, pb_state, 0x14);
+}
+
 /*----------------------------------------------------------------------*/
 
 /* generic power resource operations, which work on all regulators */
@@ -177,13 +241,7 @@ static int twl4030reg_set_mode(struct regulator_dev *rdev, unsigned mode)
 	if (!(status & (P3_GRP | P2_GRP | P1_GRP)))
 		return -EACCES;
 
-	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
-			message >> 8, 0x15 /* PB_WORD_MSB */ );
-	if (status >= 0)
-		return status;
-
-	return twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
-			message, 0x16 /* PB_WORD_LSB */ );
+	return twl4030_send_pb_msg(message);
 }
 
 /*----------------------------------------------------------------------*/
-- 
1.5.6.3

[PATCH] TWL4030: add function to send PB messages

[Peter 'p2' De Schrijver]

This patch moves sending of powerbus messages to a separate function. It also
makes sure I2C access to the powerbus is enabled.

Signed-off-by: Peter 'p2' De Schrijver <peter.de-schrijver@nokia.com>
---
 drivers/regulator/twl4030-regulator.c |   72 +++++++++++++++++++++++++++++---
 1 files changed, 65 insertions(+), 7 deletions(-)

diff --git a/drivers/regulator/twl4030-regulator.c b/drivers/regulator/twl4030-regulator.c
index 472c35a..df9a94b 100644
--- a/drivers/regulator/twl4030-regulator.c
+++ b/drivers/regulator/twl4030-regulator.c
@@ -16,6 +16,7 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/i2c/twl4030.h>
+#include <linux/delay.h>
 
 
 /*
@@ -81,6 +82,69 @@ twl4030reg_write(struct twlreg_info *info, unsigned offset, u8 value)
 			value, info->base + offset);
 }
 
+static int twl4030_wait_pb_ready(void)
+{
+
+	u8 pb_status;
+	int status, timeout = 10;
+
+	do {
+		status = twl4030_i2c_read_u8(TWL4030_MODULE_PM_MASTER,
+						&pb_status, 0x14);
+		if (status < 0)
+			return status;
+
+		if (!(pb_status & 1))
+			return 0;
+
+		mdelay(1);
+		timeout--;
+
+	} while (timeout);
+
+	return -ETIMEDOUT;
+}
+
+static int twl4030_send_pb_msg(unsigned msg)
+{
+
+	u8 pb_state;
+	int status;
+
+	/* save powerbus configuration */
+	status = twl4030_i2c_read_u8(TWL4030_MODULE_PM_MASTER,
+					&pb_state, 0x14);
+	if (status < 0)
+		return status;
+
+	/* Enable I2C access to powerbus */
+	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
+					pb_state | (1<<1));
+	if (status < 0)
+		return status;
+
+	status = twl4030_wait_pb_ready();
+	if (status < 0)
+		return status;
+
+	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, msg >> 8,
+			0x15 /* PB_WORD_MSB */);
+	if (status < 0)
+		return status;
+
+	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, msg & 0xff,
+			0x16 /* PB_WORD_LSB */);
+	if (status < 0)
+		return status;
+
+	status = twl4030_wait_pb_ready();
+	if (status < 0)
+		return status;
+
+	/* Restore powerbus configuration */
+	return twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER, pb_state, 0x14);
+}
+
 /*----------------------------------------------------------------------*/
 
 /* generic power resource operations, which work on all regulators */
@@ -177,13 +241,7 @@ static int twl4030reg_set_mode(struct regulator_dev *rdev, unsigned mode)
 	if (!(status & (P3_GRP | P2_GRP | P1_GRP)))
 		return -EACCES;
 
-	status = twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
-			message >> 8, 0x15 /* PB_WORD_MSB */ );
-	if (status >= 0)
-		return status;
-
-	return twl4030_i2c_write_u8(TWL4030_MODULE_PM_MASTER,
-			message, 0x16 /* PB_WORD_LSB */ );
+	return twl4030_send_pb_msg(message);
 }
 
 /*----------------------------------------------------------------------*/
-- 
1.5.6.3