2 files changed, 24 insertions, 11 deletions

diff --git a/arch/arm/mach-s3c2440/mach-gta02.c b/arch/arm/mach-s3c2440/mach-gta02.c
index c9c5391f3d0..dc0ef0f2a5b 100644
--- a/arch/arm/mach-s3c2440/mach-gta02.c
+++ b/arch/arm/mach-s3c2440/mach-gta02.c
@@ -521,14 +521,9 @@ gta02_configure_pmu_for_charger(struct pcf50633 *pcf, void *unused, int res)
 		pcf50633_gpio_set(pcf, PCF50633_GPO, 0);
 	
 		ma = 1000;	
-		pcf->mbc.usb_active = 1;
-	} else {
+	} else
 		ma = 100;
 
-		/* We know that we can't charge now */
-		pcf->mbc.usb_active = 0;
-	}
-
 	pcf50633_mbc_usb_curlim_set(pcf, ma);
 }
 
@@ -540,9 +535,6 @@ static void gta02_charger_worker(struct work_struct *work)
 	struct pcf50633 *pcf = gta02_pcf_pdata.pcf;
 
 	if (gta02_usb_vbus_draw) {
-		/* We can charge now */
-		pcf->mbc.usb_active = 1;
-
 		pcf50633_mbc_usb_curlim_set(pcf, gta02_usb_vbus_draw);
 		return;
 	} else {
diff --git a/drivers/power/pcf50633-charger.c b/drivers/power/pcf50633-charger.c
index 9bbfaac48a1..5cfdb274f89 100644
--- a/drivers/power/pcf50633-charger.c
+++ b/drivers/power/pcf50633-charger.c
@@ -30,6 +30,8 @@ void pcf50633_mbc_usb_curlim_set(struct pcf50633 *pcf, int ma)
 {
 	int ret;
 	u8 bits;
+	int charging_start = 1;
+	u8 mbcs2, chgmod;
 
 	if (ma >= 1000)
 		bits = PCF50633_MBCC7_USB_1000mA;
@@ -37,8 +39,10 @@ void pcf50633_mbc_usb_curlim_set(struct pcf50633 *pcf, int ma)
 		bits = PCF50633_MBCC7_USB_500mA;
 	else if (ma >= 100)
 		bits = PCF50633_MBCC7_USB_100mA;
-	else
+	else {
 		bits = PCF50633_MBCC7_USB_SUSPEND;
+		charging_start = 0;
+	}
 
 	ret = pcf50633_reg_set_bit_mask(pcf, PCF50633_REG_MBCC7,
 					PCF50633_MBCC7_USB_MASK, bits);
@@ -47,6 +51,20 @@ void pcf50633_mbc_usb_curlim_set(struct pcf50633 *pcf, int ma)
 	else
 		dev_info(pcf->dev, "usb curlim to %d mA\n", ma);
 
+	mbcs2 = pcf50633_reg_read(pcf, PCF50633_REG_MBCS2);
+	chgmod = (mbcs2 & PCF50633_MBCS2_MBC_MASK);
+
+	/* If chgmod == BATFULL, setting chgena has no effect.
+	 * We need to set resume instead.
+	 */
+	if (chgmod != PCF50633_MBCS2_MBC_BAT_FULL)
+		pcf50633_reg_set_bit_mask(pcf, PCF50633_REG_MBCC1,
+				PCF50633_MBCC1_CHGENA, PCF50633_MBCC1_CHGENA);
+	else
+		pcf50633_reg_set_bit_mask(pcf, PCF50633_REG_MBCC1,
+				PCF50633_MBCC1_RESUME, PCF50633_MBCC1_RESUME);
+
+	pcf->mbc.usb_active = charging_start;
 	power_supply_changed(&pcf->mbc.usb);
 }
 EXPORT_SYMBOL_GPL(pcf50633_mbc_usb_curlim_set);
@@ -152,7 +170,10 @@ static void pcf50633_mbc_irq_handler(struct pcf50633 *pcf, int irq, void *data)
 	if (irq == PCF50633_IRQ_BATFULL) {
 		mbc->usb_active = 0;
 		mbc->adapter_active = 0;
-	}
+	} else if (irq == PCF50633_IRQ_USBLIMON)
+		mbc->usb_active = 0;
+	else if (irq == PCF50633_IRQ_USBLIMOFF)
+		mbc->usb_active = 1;
 
 	power_supply_changed(&mbc->ac);
 	power_supply_changed(&mbc->usb);