diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index a614a2a7fd25..a5936e2aeaaa 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -835,11 +835,13 @@ static void ci_start_new_role(struct ci_hdrc *ci)
 {
 	enum ci_role role = ci_get_role(ci);
 
-	if (ci->role != role)
+	if (ci->role != role) {
 		ci_handle_id_switch(ci);
-
-	if (role == CI_ROLE_GADGET)
+	} else if (role == CI_ROLE_GADGET) {
+		if (ci->vbus_active)
+			usb_gadget_vbus_disconnect(&ci->gadget);
 		ci_handle_vbus_connected(ci);
+	}
 }
 
 static void ci_power_lost_work(struct work_struct *work)
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index 33e7f9c28b0c..a0accd5b8945 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -2077,10 +2077,6 @@ static void udc_suspend_for_power_lost(struct ci_hdrc *ci)
 /* Power lost with device mode */
 static void udc_resume_from_power_lost(struct ci_hdrc *ci)
 {
-	/* Force disconnect if power lost with vbus on */
-	if (!ci_otg_is_fsm_mode(ci) && ci->vbus_active)
-		usb_gadget_vbus_disconnect(&ci->gadget);
-
 	if (ci->is_otg)
 		hw_write_otgsc(ci, OTGSC_BSVIS | OTGSC_BSVIE,
 					OTGSC_BSVIS | OTGSC_BSVIE);