diff --git a/drivers/usb/chipidea/ci.h b/drivers/usb/chipidea/ci.h
index cd414559040f..05bc4d631cb9 100644
--- a/drivers/usb/chipidea/ci.h
+++ b/drivers/usb/chipidea/ci.h
@@ -428,9 +428,6 @@ int hw_port_test_set(struct ci_hdrc *ci, u8 mode);
 
 u8 hw_port_test_get(struct ci_hdrc *ci);
 
-int hw_wait_reg(struct ci_hdrc *ci, enum ci_hw_regs reg, u32 mask,
-				u32 value, unsigned int timeout_ms);
-
 void ci_platform_configure(struct ci_hdrc *ci);
 
 int dbg_create_files(struct ci_hdrc *ci);
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index 5c35f25e9bce..24859b44c45c 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -516,38 +516,6 @@ int hw_device_reset(struct ci_hdrc *ci)
 	return 0;
 }
 
-/**
- * hw_wait_reg: wait the register value
- *
- * Sometimes, it needs to wait register value before going on.
- * Eg, when switch to device mode, the vbus value should be lower
- * than OTGSC_BSV before connects to host.
- *
- * @ci: the controller
- * @reg: register index
- * @mask: mast bit
- * @value: the bit value to wait
- * @timeout_ms: timeout in millisecond
- *
- * This function returns an error code if timeout
- */
-int hw_wait_reg(struct ci_hdrc *ci, enum ci_hw_regs reg, u32 mask,
-				u32 value, unsigned int timeout_ms)
-{
-	unsigned long elapse = jiffies + msecs_to_jiffies(timeout_ms);
-
-	while (hw_read(ci, reg, mask) != value) {
-		if (time_after(jiffies, elapse)) {
-			dev_err(ci->dev, "timeout waiting for %08x in %d\n",
-					mask, reg);
-			return -ETIMEDOUT;
-		}
-		msleep(20);
-	}
-
-	return 0;
-}
-
 static irqreturn_t ci_irq(int irq, void *data)
 {
 	struct ci_hdrc *ci = data;
diff --git a/drivers/usb/chipidea/otg.c b/drivers/usb/chipidea/otg.c
index 03b6743461d1..a829607c3e4d 100644
--- a/drivers/usb/chipidea/otg.c
+++ b/drivers/usb/chipidea/otg.c
@@ -104,7 +104,31 @@ void ci_handle_vbus_change(struct ci_hdrc *ci)
 		usb_gadget_vbus_disconnect(&ci->gadget);
 }
 
-#define CI_VBUS_STABLE_TIMEOUT_MS 5000
+/**
+ * When we switch to device mode, the vbus value should be lower
+ * than OTGSC_BSV before connecting to host.
+ *
+ * @ci: the controller
+ *
+ * This function returns an error code if timeout
+ */
+static int hw_wait_vbus_lower_bsv(struct ci_hdrc *ci)
+{
+	unsigned long elapse = jiffies + msecs_to_jiffies(5000);
+	u32 mask = OTGSC_BSV;
+
+	while (hw_read_otgsc(ci, mask)) {
+		if (time_after(jiffies, elapse)) {
+			dev_err(ci->dev, "timeout waiting for %08x in OTGSC\n",
+					mask);
+			return -ETIMEDOUT;
+		}
+		msleep(20);
+	}
+
+	return 0;
+}
+
 static void ci_handle_id_switch(struct ci_hdrc *ci)
 {
 	enum ci_role role = ci_otg_role(ci);
@@ -116,9 +140,11 @@ static void ci_handle_id_switch(struct ci_hdrc *ci)
 		ci_role_stop(ci);
 
 		if (role == CI_ROLE_GADGET)
-			/* wait vbus lower than OTGSC_BSV */
-			hw_wait_reg(ci, OP_OTGSC, OTGSC_BSV, 0,
-					CI_VBUS_STABLE_TIMEOUT_MS);
+			/*
+			 * wait vbus lower than OTGSC_BSV before connecting
+			 * to host
+			 */
+			hw_wait_vbus_lower_bsv(ci);
 
 		ci_role_start(ci, role);
 	}