diff --git a/drivers/usb/pd/policy_engine.c b/drivers/usb/pd/policy_engine.c
index 7a1174b32a38..ccf71dc4f624 100644
--- a/drivers/usb/pd/policy_engine.c
+++ b/drivers/usb/pd/policy_engine.c
@@ -299,6 +299,7 @@ struct usbpd {
 	enum power_supply_typec_mode typec_mode;
 	enum power_supply_type	psy_type;
 	bool			vbus_present;
+	bool			pd_allowed;
 
 	enum data_role		current_dr;
 	enum power_role		current_pr;
@@ -625,7 +626,10 @@ static void usbpd_set_state(struct usbpd *pd, enum usbpd_state next_state)
 	case PE_SRC_STARTUP:
 		if (pd->current_dr == DR_NONE) {
 			pd->current_dr = DR_DFP;
-			/* Defer starting USB host mode until after PD */
+			/*
+			 * Defer starting USB host mode until PE_SRC_READY or
+			 * when PE_SRC_SEND_CAPABILITIES fails
+			 */
 		}
 
 		/* Set CC back to DRP toggle for the next disconnect */
@@ -660,7 +664,6 @@ static void usbpd_set_state(struct usbpd *pd, enum usbpd_state next_state)
 
 		pd->current_state = PE_SRC_SEND_CAPABILITIES;
 		if (pd->in_pr_swap) {
-			pd->in_pr_swap = false;
 			kick_sm(pd, SWAP_SOURCE_START_TIME);
 			break;
 		}
@@ -820,6 +823,9 @@ static void usbpd_set_state(struct usbpd *pd, enum usbpd_state next_state)
 			}
 		}
 
+		if (!pd->pd_allowed)
+			break;
+
 		/* Reset protocol layer */
 		pd->tx_msgid = 0;
 		pd->rx_msgid = -1;
@@ -847,7 +853,6 @@ static void usbpd_set_state(struct usbpd *pd, enum usbpd_state next_state)
 			pd->pd_phy_opened = true;
 		}
 
-		pd->in_pr_swap = false;
 		pd->current_voltage = 5000000;
 
 		pd->current_state = PE_SNK_WAIT_FOR_CAPABILITIES;
@@ -1373,11 +1378,11 @@ static void usbpd_sm(struct work_struct *w)
 		ctrl_recvd = pd->rx_msg_type;
 
 	/* Disconnect? */
-	if (pd->typec_mode == POWER_SUPPLY_TYPEC_NONE) {
+	if (pd->typec_mode == POWER_SUPPLY_TYPEC_NONE && !pd->in_pr_swap) {
 		if (pd->current_state == PE_UNKNOWN)
 			goto sm_done;
 
-		usbpd_info(&pd->dev, "USB PD disconnect\n");
+		usbpd_info(&pd->dev, "USB Type-C disconnect\n");
 
 		if (pd->pd_phy_opened) {
 			pd_phy_close();
@@ -1484,6 +1489,10 @@ static void usbpd_sm(struct work_struct *w)
 		}
 		break;
 
+	case PE_SRC_STARTUP:
+		usbpd_set_state(pd, PE_SRC_STARTUP);
+		break;
+
 	case PE_SRC_SEND_CAPABILITIES:
 		ret = pd_send_msg(pd, MSG_SOURCE_CAPABILITIES, default_src_caps,
 				ARRAY_SIZE(default_src_caps), SOP_MSG);
@@ -1621,6 +1630,10 @@ static void usbpd_sm(struct work_struct *w)
 		usbpd_set_state(pd, PE_SRC_TRANSITION_TO_DEFAULT);
 		break;
 
+	case PE_SNK_STARTUP:
+		usbpd_set_state(pd, PE_SNK_STARTUP);
+		break;
+
 	case PE_SNK_WAIT_FOR_CAPABILITIES:
 		if (data_recvd == MSG_SOURCE_CAPABILITIES) {
 			val.intval = 0;
@@ -2008,9 +2021,8 @@ static int psy_changed(struct notifier_block *nb, unsigned long evt, void *ptr)
 {
 	struct usbpd *pd = container_of(nb, struct usbpd, psy_nb);
 	union power_supply_propval val;
-	bool pd_allowed;
 	enum power_supply_typec_mode typec_mode;
-	enum power_supply_type psy_type;
+	bool do_work = false;
 	int ret;
 
 	if (ptr != pd->usb_psy || evt != PSY_EVENT_PROP_CHANGED)
@@ -2024,7 +2036,9 @@ static int psy_changed(struct notifier_block *nb, unsigned long evt, void *ptr)
 		return ret;
 	}
 
-	pd_allowed = val.intval;
+	if (pd->pd_allowed != val.intval)
+		do_work = true;
+	pd->pd_allowed = val.intval;
 
 	ret = power_supply_get_property(pd->usb_psy,
 			POWER_SUPPLY_PROP_PRESENT, &val);
@@ -2044,38 +2058,6 @@ static int psy_changed(struct notifier_block *nb, unsigned long evt, void *ptr)
 
 	typec_mode = val.intval;
 
-	/*
-	 * Don't proceed if cable is connected but PD_ALLOWED is false.
-	 * It means the PMIC may still be in the middle of performing
-	 * charger type detection.
-	 */
-	if (!pd_allowed && typec_mode != POWER_SUPPLY_TYPEC_NONE)
-		return 0;
-
-	/*
-	 * Workaround for PMIC HW bug.
-	 *
-	 * During hard reset or PR swap (sink to source) when VBUS goes to 0
-	 * the CC logic will report this as a disconnection. In those cases it
-	 * can be ignored, however the downside is that pd->hard_reset can be
-	 * momentarily true even when a non-PD capable source is attached, and
-	 * can't be distinguished from a physical disconnect. In that case,
-	 * allow for the common case of disconnecting from an SDP.
-	 *
-	 * The less common case is a PD-capable SDP which will result in a
-	 * hard reset getting treated like a disconnect. We can live with this
-	 * until the HW bug is fixed: in which disconnection won't be reported
-	 * on VBUS loss alone unless pullup is also removed from CC.
-	 */
-	if (typec_mode == POWER_SUPPLY_TYPEC_NONE &&
-		(pd->in_pr_swap ||
-		 (pd->psy_type != POWER_SUPPLY_TYPE_USB &&
-		  pd->current_state == PE_SNK_TRANSITION_TO_DEFAULT))) {
-		usbpd_dbg(&pd->dev, "Ignoring disconnect due to %s\n",
-				pd->in_pr_swap ? "PR swap" : "hard reset");
-		return 0;
-	}
-
 	ret = power_supply_get_property(pd->usb_psy,
 			POWER_SUPPLY_PROP_TYPE, &val);
 	if (ret) {
@@ -2083,61 +2065,91 @@ static int psy_changed(struct notifier_block *nb, unsigned long evt, void *ptr)
 		return ret;
 	}
 
-	psy_type = val.intval;
+	if (pd->psy_type != val.intval)
+		do_work = true;
+	pd->psy_type = val.intval;
 
 	usbpd_dbg(&pd->dev, "typec mode:%d present:%d type:%d orientation:%d\n",
-			typec_mode, pd->vbus_present, psy_type,
+			typec_mode, pd->vbus_present, pd->psy_type,
 			usbpd_get_plug_orientation(pd));
 
-	/* any change? */
-	if (pd->typec_mode == typec_mode && pd->psy_type == psy_type)
-		return 0;
+	if (pd->typec_mode != typec_mode) {
+		pd->typec_mode = typec_mode;
+		do_work = true;
 
-	pd->typec_mode = typec_mode;
-	pd->psy_type = psy_type;
+		switch (typec_mode) {
+		/* Disconnect */
+		case POWER_SUPPLY_TYPEC_NONE:
+			if (pd->in_pr_swap) {
+				usbpd_dbg(&pd->dev, "Ignoring disconnect due to PR swap\n");
+				do_work = false;
+			}
 
-	switch (typec_mode) {
-	/* Disconnect */
-	case POWER_SUPPLY_TYPEC_NONE:
-		kick_sm(pd, 0);
-		break;
+			/*
+			 * Workaround for PMIC HW bug.
+			 *
+			 * During hard reset when VBUS goes to 0 the CC logic
+			 * will report this as a disconnection. In those cases
+			 * it can be ignored, however the downside is that
+			 * pd->hard_reset can be momentarily true even when a
+			 * non-PD capable source is attached, and can't be
+			 * distinguished from a physical disconnect. In that
+			 * case, allow for the common case of disconnecting
+			 * from an SDP.
+			 *
+			 * The less common case is a PD-capable SDP which will
+			 * result in a hard reset getting treated like a
+			 * disconnect. We can live with this until the HW bug
+			 * is fixed: in which disconnection won't be reported
+			 * on VBUS loss alone unless pullup is also removed
+			 * from CC.
+			 */
+			if (pd->psy_type != POWER_SUPPLY_TYPE_USB &&
+				  pd->current_state ==
+					  PE_SNK_TRANSITION_TO_DEFAULT) {
+				usbpd_dbg(&pd->dev, "Ignoring disconnect due to hard reset\n");
+				do_work = false;
+			}
 
-	/* Sink states */
-	case POWER_SUPPLY_TYPEC_SOURCE_DEFAULT:
-	case POWER_SUPPLY_TYPEC_SOURCE_MEDIUM:
-	case POWER_SUPPLY_TYPEC_SOURCE_HIGH:
-		usbpd_info(&pd->dev, "Type-C Source (%s) connected\n",
-				src_current(typec_mode));
-		if (pd->current_pr != PR_SINK ||
-			pd->current_state == PE_SNK_TRANSITION_TO_DEFAULT) {
+			break;
+
+		/* Sink states */
+		case POWER_SUPPLY_TYPEC_SOURCE_DEFAULT:
+		case POWER_SUPPLY_TYPEC_SOURCE_MEDIUM:
+		case POWER_SUPPLY_TYPEC_SOURCE_HIGH:
+			usbpd_info(&pd->dev, "Type-C Source (%s) connected\n",
+					src_current(typec_mode));
 			pd->current_pr = PR_SINK;
-			kick_sm(pd, 0);
-		}
-		break;
+			pd->in_pr_swap = false;
+			break;
 
-	/* Source states */
-	case POWER_SUPPLY_TYPEC_SINK_POWERED_CABLE:
-	case POWER_SUPPLY_TYPEC_SINK:
-		usbpd_info(&pd->dev, "Type-C Sink%s connected\n",
-				typec_mode == POWER_SUPPLY_TYPEC_SINK ?
-					"" : " (powered)");
-		if (pd->current_pr != PR_SRC) {
+		/* Source states */
+		case POWER_SUPPLY_TYPEC_SINK_POWERED_CABLE:
+		case POWER_SUPPLY_TYPEC_SINK:
+			usbpd_info(&pd->dev, "Type-C Sink%s connected\n",
+					typec_mode == POWER_SUPPLY_TYPEC_SINK ?
+						"" : " (powered)");
 			pd->current_pr = PR_SRC;
-			kick_sm(pd, 0);
-		}
-		break;
+			pd->in_pr_swap = false;
+			break;
 
-	case POWER_SUPPLY_TYPEC_SINK_DEBUG_ACCESSORY:
-		usbpd_info(&pd->dev, "Type-C Debug Accessory connected\n");
-		break;
-	case POWER_SUPPLY_TYPEC_SINK_AUDIO_ADAPTER:
-		usbpd_info(&pd->dev, "Type-C Analog Audio Adapter connected\n");
-		break;
-	default:
-		usbpd_warn(&pd->dev, "Unsupported typec mode:%d\n", typec_mode);
-		break;
+		case POWER_SUPPLY_TYPEC_SINK_DEBUG_ACCESSORY:
+			usbpd_info(&pd->dev, "Type-C Debug Accessory connected\n");
+			break;
+		case POWER_SUPPLY_TYPEC_SINK_AUDIO_ADAPTER:
+			usbpd_info(&pd->dev, "Type-C Analog Audio Adapter connected\n");
+			break;
+		default:
+			usbpd_warn(&pd->dev, "Unsupported typec mode:%d\n",
+					typec_mode);
+			break;
+		}
 	}
 
+	/* only queue state machine if CC state or PD_ALLOWED changes */
+	if (do_work)
+		kick_sm(pd, 0);
+
 	return 0;
 }