2 files changed, 25 insertions, 2 deletions

diff --git a/drivers/usb/chipidea/otg_fsm.c b/drivers/usb/chipidea/otg_fsm.c
index fff809ca3a18..028f7687ad29 100644
--- a/drivers/usb/chipidea/otg_fsm.c
+++ b/drivers/usb/chipidea/otg_fsm.c
@@ -592,10 +592,18 @@ int ci_otg_fsm_work(struct ci_hdrc *ci)
         /*
          * Don't do fsm transition for B device
          * when there is no gadget class driver
+         * only handle charger notify
          */
         if (ci->fsm.id && !(ci->driver) &&
-                ci->transceiver->state < OTG_STATE_A_IDLE)
+                ci->transceiver->state < OTG_STATE_A_IDLE) {
+                if (ci->b_sess_valid_event) {
+                        if (ci->fsm.b_sess_vld)
+                                usb_gadget_vbus_connect(&ci->gadget);
+                        else
+                                usb_gadget_vbus_disconnect(&ci->gadget);
+                }
                 return 0;
+        }
 
         if (otg_statemachine(&ci->fsm)) {
                 if (ci->transceiver->state == OTG_STATE_A_IDLE) {
@@ -764,6 +772,7 @@ irqreturn_t ci_otg_fsm_irq(struct ci_hdrc *ci)
                         }
                 } else if (otg_int_src & OTGSC_BSVIS) {
                         hw_write_otgsc(ci, OTGSC_BSVIS, OTGSC_BSVIS);
+                        ci->b_sess_valid_event = true;
                         if (otgsc & OTGSC_BSV) {
                                 fsm->b_sess_vld = 1;
                                 ci_otg_del_timer(ci, B_SSEND_SRP);
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index 5c20822160ea..a32c18cffd62 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -1463,6 +1463,17 @@ static const struct usb_ep_ops usb_ep_ops = {
 /******************************************************************************
  * GADGET block
  *****************************************************************************/
+static bool ci_otg_fsm_charger_conn(struct ci_hdrc *ci)
+{
+        /*
+         * OTG port is in fsm mode
+         * only do charger notify for b sess valid event, or
+         * when power up.
+         */
+        return !ci_otg_is_fsm_mode(ci) || ci->fsm.power_up ||
+                                        ci->b_sess_valid_event;
+}
+
 static int ci_udc_vbus_session(struct usb_gadget *_gadget, int is_active)
 {
         struct ci_hdrc *ci = container_of(_gadget, struct ci_hdrc, gadget);
@@ -1477,7 +1488,7 @@ static int ci_udc_vbus_session(struct usb_gadget *_gadget, int is_active)
         spin_unlock_irqrestore(&ci->lock, flags);
 
         /* Charger Detection */
-        if (ci->platdata->notify_event) {
+        if (ci->platdata->notify_event && ci_otg_fsm_charger_conn(ci)) {
                 /*
                  * Keep controller active when the cable is connected,
                  * It can make disconnect interrupt (BSV 1->0) occur when
@@ -1503,6 +1514,9 @@ static int ci_udc_vbus_session(struct usb_gadget *_gadget, int is_active)
                 }
         }
 
+        if (ci_otg_is_fsm_mode(ci) && ci->b_sess_valid_event)
+                ci->b_sess_valid_event = false;
+
         if (gadget_ready) {
                 if (is_active) {
                         pm_runtime_get_sync(&_gadget->dev);