5 files changed, 55 insertions, 2 deletions
diff --git a/board/freescale/common/pfuze.c b/board/freescale/common/pfuze.c
index 2cd1794429..4980bf7b00 100644
--- a/board/freescale/common/pfuze.c
+++ b/board/freescale/common/pfuze.c
@@ -5,9 +5,47 @@
  */
 
 #include <common.h>
+#include <errno.h>
 #include <power/pmic.h>
 #include <power/pfuze100_pmic.h>
 
+int pfuze_mode_init(struct pmic *p, u32 mode)
+{
+	unsigned char offset, i, switch_num;
+	u32 id, ret;
+
+	pmic_reg_read(p, PFUZE100_DEVICEID, &id);
+	id = id & 0xf;
+
+	if (id == 0) {
+		switch_num = 6;
+		offset = PFUZE100_SW1CMODE;
+	} else if (id == 1) {
+		switch_num = 4;
+		offset = PFUZE100_SW2MODE;
+	} else {
+		printf("Not supported, id=%d\n", id);
+		return -EINVAL;
+	}
+
+	ret = pmic_reg_write(p, PFUZE100_SW1ABMODE, mode);
+	if (ret < 0) {
+		printf("Set SW1AB mode error!\n");
+		return ret;
+	}
+
+	for (i = 0; i < switch_num - 1; i++) {
+		ret = pmic_reg_write(p, offset + i * SWITCH_SIZE, mode);
+		if (ret < 0) {
+			printf("Set switch 0x%x mode error!\n",
+			       offset + i * SWITCH_SIZE);
+			return ret;
+		}
+	}
+
+	return ret;
+}
+
 struct pmic *pfuze_common_init(unsigned char i2cbus)
 {
 	struct pmic *p;
diff --git a/board/freescale/common/pfuze.h b/board/freescale/common/pfuze.h
index 7a4126cca0..53cfc99225 100644
--- a/board/freescale/common/pfuze.h
+++ b/board/freescale/common/pfuze.h
@@ -8,5 +8,6 @@
 #define __PFUZE_BOARD_HELPER__
 
 struct pmic *pfuze_common_init(unsigned char i2cbus);
+int pfuze_mode_init(struct pmic *p, u32 mode);
 
 #endif
diff --git a/board/freescale/mx6qsabreauto/mx6qsabreauto.c b/board/freescale/mx6qsabreauto/mx6qsabreauto.c
index a90360f2df..b76e4eb528 100644
--- a/board/freescale/mx6qsabreauto/mx6qsabreauto.c
+++ b/board/freescale/mx6qsabreauto/mx6qsabreauto.c
@@ -29,6 +29,7 @@
 #include <asm/arch/crm_regs.h>
 #include <pca953x.h>
 #include <power/pmic.h>
+#include <power/pfuze100_pmic.h>
 #include "../common/pfuze.h"
 
 DECLARE_GLOBAL_DATA_PTR;
@@ -494,11 +495,16 @@ int board_spi_cs_gpio(unsigned bus, unsigned cs)
 int power_init_board(void)
 {
 	struct pmic *p;
+	unsigned int ret;
 
 	p = pfuze_common_init(I2C_PMIC);
 	if (!p)
 		return -ENODEV;
 
+	ret = pfuze_mode_init(p, APS_PFM);
+	if (ret < 0)
+		return ret;
+
 	return 0;
 }
 
diff --git a/board/freescale/mx6sabresd/mx6sabresd.c b/board/freescale/mx6sabresd/mx6sabresd.c
index 2f7198d3bf..bb2dd9624b 100644
--- a/board/freescale/mx6sabresd/mx6sabresd.c
+++ b/board/freescale/mx6sabresd/mx6sabresd.c
@@ -631,12 +631,16 @@ int board_init(void)
 int power_init_board(void)
 {
 	struct pmic *p;
-	unsigned int reg;
+	unsigned int reg, ret;
 
 	p = pfuze_common_init(I2C_PMIC);
 	if (!p)
 		return -ENODEV;
 
+	ret = pfuze_mode_init(p, APS_PFM);
+	if (ret < 0)
+		return ret;
+
 	/* Increase VGEN3 from 2.5 to 2.8V */
 	pmic_reg_read(p, PFUZE100_VGEN3VOL, &reg);
 	reg &= ~LDO_VOL_MASK;
diff --git a/board/freescale/mx6sxsabresd/mx6sxsabresd.c b/board/freescale/mx6sxsabresd/mx6sxsabresd.c
index a2c9aae32c..2ff960e3bb 100644
--- a/board/freescale/mx6sxsabresd/mx6sxsabresd.c
+++ b/board/freescale/mx6sxsabresd/mx6sxsabresd.c
@@ -199,12 +199,16 @@ static struct i2c_pads_info i2c_pad_info1 = {
 int power_init_board(void)
 {
 	struct pmic *p;
-	unsigned int reg;
+	unsigned int reg, ret;
 
 	p = pfuze_common_init(I2C_PMIC);
 	if (!p)
 		return -ENODEV;
 
+	ret = pfuze_mode_init(p, APS_PFM);
+	if (ret < 0)
+		return ret;
+
 	/* Enable power of VGEN5 3V3, needed for SD3 */
 	pmic_reg_read(p, PFUZE100_VGEN5VOL, &reg);
 	reg &= ~LDO_VOL_MASK;