forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2024-05-13 9d77db3c730780c8ef5ccd4b66403ff5675cfe4e 
 kernel/drivers/usb/typec/tcpm/tcpm.c
....@@ -8,7 +8,6 @@
88 #include <linux/completion.h>
99 #include <linux/debugfs.h>
1010 #include <linux/device.h>
11
-#include <linux/extcon-provider.h>
1211 #include <linux/hrtimer.h>
1312 #include <linux/jiffies.h>
1413 #include <linux/kernel.h>
....@@ -18,11 +17,11 @@
1817 #include <linux/power_supply.h>
1918 #include <linux/proc_fs.h>
2019 #include <linux/property.h>
21
-#include <linux/regulator/consumer.h>
2220 #include <linux/sched/clock.h>
2321 #include <linux/seq_file.h>
2422 #include <linux/slab.h>
2523 #include <linux/spinlock.h>
24
+#include <linux/usb.h>
2625 #include <linux/usb/pd.h>
2726 #include <linux/usb/pd_ado.h>
2827 #include <linux/usb/pd_bdo.h>
....@@ -31,8 +30,8 @@
3130 #include <linux/usb/role.h>
3231 #include <linux/usb/tcpm.h>
3332 #include <linux/usb/typec_altmode.h>
34
-#include <linux/usb/typec_dp.h>
3533 
34
+#include <trace/hooks/typec.h>
3635 #include <uapi/linux/sched/types.h>
3736 
3837 #define FOREACH_STATE(S)			\
....@@ -145,7 +144,8 @@
145144 	S(PORT_RESET),				\
146145 	S(PORT_RESET_WAIT_OFF),			\
147146 						\
148
-	S(AMS_START)
147
+	S(AMS_START),				\
148
+	S(CHUNK_NOT_SUPP)
149149 
150150 #define FOREACH_AMS(S)				\
151151 	S(NONE_AMS),				\
....@@ -223,6 +223,14 @@
223223 	PD_MSG_DATA_SOURCE_CAP,
224224 };
225225 
226
+enum adev_actions {
227
+	ADEV_NONE = 0,
228
+	ADEV_NOTIFY_USB_AND_QUEUE_VDM,
229
+	ADEV_QUEUE_VDM,
230
+	ADEV_QUEUE_VDM_SEND_EXIT_MODE_ON_FAIL,
231
+	ADEV_ATTENTION,
232
+};
233
+
226234 /*
227235  * Initial current capability of the new source when vSafe5V is applied during PD3.0 Fast Role Swap.
228236  * Based on "Table 6-14 Fixed Supply PDO - Sink" of "USB Power Delivery Specification Revision 3.0,
....@@ -234,24 +242,6 @@
234242 	FRS_5V_1P5A,
235243 	FRS_5V_3A,
236244 };
237
-
238
-static const unsigned int tcpm_cable[] = {
239
-	EXTCON_USB,
240
-	EXTCON_USB_HOST,
241
-	EXTCON_USB_VBUS_EN,
242
-	EXTCON_CHG_USB_SDP,
243
-	EXTCON_CHG_USB_CDP,
244
-	EXTCON_CHG_USB_DCP,
245
-	EXTCON_CHG_USB_SLOW,
246
-	EXTCON_CHG_USB_FAST,
247
-	EXTCON_DISP_DP,
248
-	EXTCON_NONE,
249
-};
250
-
251
-/* Pin assignments where one channel is for USB */
252
-#define DP_PIN_ASSIGN_MULTI_FUNCTION_MASK	(BIT(DP_PIN_ASSIGN_B) | \
253
-						 BIT(DP_PIN_ASSIGN_D) | \
254
-						 BIT(DP_PIN_ASSIGN_F))
255245 
256246 /* Events from low level driver */
257247 
....@@ -270,7 +260,7 @@
270260 #define ALTMODE_DISCOVERY_MAX	(SVID_DISCOVERY_MAX * MODE_DISCOVERY_MAX)
271261 
272262 #define GET_SINK_CAP_RETRY_MS	100
273
-#define SEND_NEW_MODE_NOTIFY_MS 20
263
+#define SEND_DISCOVER_RETRY_MS	100
274264 
275265 struct pd_mode_data {
276266 	int svid_index;		/* current SVID index		*/
....@@ -280,12 +270,27 @@
280270 	struct typec_altmode_desc altmode_desc[ALTMODE_DISCOVERY_MAX];
281271 };
282272 
273
+/*
274
+ * @min_volt: Actual min voltage at the local port
275
+ * @req_min_volt: Requested min voltage to the port partner
276
+ * @max_volt: Actual max voltage at the local port
277
+ * @req_max_volt: Requested max voltage to the port partner
278
+ * @max_curr: Actual max current at the local port
279
+ * @req_max_curr: Requested max current of the port partner
280
+ * @req_out_volt: Requested output voltage to the port partner
281
+ * @req_op_curr: Requested operating current to the port partner
282
+ * @supported: Parter has atleast one APDO hence supports PPS
283
+ * @active: PPS mode is active
284
+ */
283285 struct pd_pps_data {
284286 	u32 min_volt;
287
+	u32 req_min_volt;
285288 	u32 max_volt;
289
+	u32 req_max_volt;
286290 	u32 max_curr;
287
-	u32 out_volt;
288
-	u32 op_curr;
291
+	u32 req_max_curr;
292
+	u32 req_out_volt;
293
+	u32 req_op_curr;
289294 	bool supported;
290295 	bool active;
291296 };
....@@ -311,9 +316,6 @@
311316 	struct typec_partner_desc partner_desc;
312317 	struct typec_partner *partner;
313318 
314
-	struct regulator *vbus;
315
-	struct extcon_dev *extcon;
316
-
317319 	enum typec_cc_status cc_req;
318320 
319321 	enum typec_cc_status cc1;
....@@ -323,14 +325,26 @@
323325 	bool attached;
324326 	bool connected;
325327 	enum typec_port_type port_type;
328
+
329
+	/*
330
+	 * Set to true when vbus is greater than VSAFE5V min.
331
+	 * Set to false when vbus falls below vSinkDisconnect max threshold.
332
+	 */
326333 	bool vbus_present;
334
+
335
+	/*
336
+	 * Set to true when vbus is less than VSAFE0V max.
337
+	 * Set to false when vbus is greater than VSAFE0V max.
338
+	 */
339
+	bool vbus_vsafe0v;
340
+
327341 	bool vbus_never_low;
328342 	bool vbus_source;
329343 	bool vbus_charge;
330344 
345
+	/* Set to true when Discover_Identity Command is expected to be sent in Ready states. */
331346 	bool send_discover;
332347 	bool op_vsafe5v;
333
-	bool vbus_on;
334348 
335349 	int try_role;
336350 	int try_snk_count;
....@@ -346,7 +360,9 @@
346360 	unsigned long delay_ms;
347361 
348362 	spinlock_t pd_event_lock;
363
+#ifdef CONFIG_NO_GKI
349364 	struct mutex pd_handler_lock;
365
+#endif
350366 	u32 pd_events;
351367 
352368 	struct kthread_work event_work;
....@@ -356,9 +372,10 @@
356372 	struct kthread_work vdm_state_machine;
357373 	struct hrtimer enable_frs_timer;
358374 	struct kthread_work enable_frs;
359
-	struct kthread_work data_role_swap;
360
-	struct hrtimer data_role_swap_timer;
375
+	struct hrtimer send_discover_timer;
376
+	struct kthread_work send_discover_work;
361377 	bool state_machine_running;
378
+	/* Set to true when VDM State Machine has following actions. */
362379 	bool vdm_sm_running;
363380 
364381 	struct completion tx_complete;
....@@ -378,10 +395,6 @@
378395 	bool explicit_contract;
379396 	unsigned int rx_msgid;
380397 
381
-	u32 dp_pin_assignment;
382
-	u32 dp_status;
383
-	bool dp_configured;
384
-
385398 	/* Partner capabilities/requests */
386399 	u32 sink_request;
387400 	u32 source_caps[PDO_MAX_OBJECTS];
....@@ -394,13 +407,18 @@
394407 	unsigned int nr_src_pdo;
395408 	u32 snk_pdo[PDO_MAX_OBJECTS];
396409 	unsigned int nr_snk_pdo;
410
+	u32 snk_vdo_v1[VDO_MAX_OBJECTS];
411
+	unsigned int nr_snk_vdo_v1;
397412 	u32 snk_vdo[VDO_MAX_OBJECTS];
398413 	unsigned int nr_snk_vdo;
399414 
400415 	unsigned int operating_snk_mw;
401416 	bool update_sink_caps;
402417 
403
-	/* Requested current / voltage */
418
+	/* Requested current / voltage to the port partner */
419
+	u32 req_current_limit;
420
+	u32 req_supply_voltage;
421
+	/* Actual current / voltage limit of the local port */
404422 	u32 current_limit;
405423 	u32 supply_voltage;
406424 
....@@ -437,17 +455,30 @@
437455 	/* port belongs to a self powered device */
438456 	bool self_powered;
439457 
440
-	/* FRS */
441
-	enum frs_typec_current frs_current;
458
+	/* Sink FRS */
459
+	enum frs_typec_current new_source_frs_current;
442460 
443461 	/* Sink caps have been queried */
444462 	bool sink_cap_done;
445463 
464
+	/* Port is still in tCCDebounce */
465
+	bool debouncing;
466
+
446467 	/* Collision Avoidance and Atomic Message Sequence */
447468 	enum tcpm_state upcoming_state;
448469 	enum tcpm_ams ams;
470
+	enum tcpm_ams next_ams;
449471 	bool in_ams;
450472 
473
+	/* Auto vbus discharge status */
474
+	bool auto_vbus_discharge_enabled;
475
+
476
+	/*
477
+	 * When set, port requests PD_P_SNK_STDBY_MW upon entering SNK_DISCOVERY and
478
+	 * the actual currrent limit after RX of PD_CTRL_PSRDY for PD link,
479
+	 * SNK_READY for non-pd link.
480
+	 */
481
+	bool slow_charger_loop;
451482 #ifdef CONFIG_DEBUG_FS
452483 	struct dentry *dentry;
453484 	struct mutex logbuffer_lock;	/* log buffer access lock */
....@@ -505,9 +536,20 @@
505536 	((port)->try_src_count == 0 && (port)->try_role == TYPEC_SOURCE && \
506537 	(port)->port_type == TYPEC_PORT_DRP)
507538 
539
+#define tcpm_data_role_for_source(port) \
540
+	((port)->typec_caps.data == TYPEC_PORT_UFP ? \
541
+	TYPEC_DEVICE : TYPEC_HOST)
542
+
543
+#define tcpm_data_role_for_sink(port) \
544
+	((port)->typec_caps.data == TYPEC_PORT_DFP ? \
545
+	TYPEC_HOST : TYPEC_DEVICE)
546
+
508547 #define tcpm_sink_tx_ok(port) \
509548 	(tcpm_port_is_sink(port) && \
510549 	((port)->cc1 == TYPEC_CC_RP_3_0 || (port)->cc2 == TYPEC_CC_RP_3_0))
550
+
551
+#define tcpm_wait_for_discharge(port) \
552
+	(((port)->auto_vbus_discharge_enabled && !(port)->vbus_vsafe0v) ? PD_T_SAFE_0V : 0)
511553 
512554 static enum tcpm_state tcpm_default_state(struct tcpm_port *port)
513555 {
....@@ -521,12 +563,6 @@
521563 		return SNK_UNATTACHED;
522564 	}
523565 	return SRC_UNATTACHED;
524
-}
525
-
526
-static inline
527
-struct tcpm_port *typec_cap_to_tcpm(const struct typec_capability *cap)
528
-{
529
-	return container_of(cap, struct tcpm_port, typec_caps);
530566 }
531567 
532568 static bool tcpm_port_is_disconnected(struct tcpm_port *port)
....@@ -557,6 +593,7 @@
557593 	char tmpbuffer[LOG_BUFFER_ENTRY_SIZE];
558594 	u64 ts_nsec = local_clock();
559595 	unsigned long rem_nsec;
596
+	bool bypass_log = false;
560597 
561598 	mutex_lock(&port->logbuffer_lock);
562599 	if (!port->logbuffer[port->logbuffer_head]) {
....@@ -569,6 +606,9 @@
569606 	}
570607 
571608 	vsnprintf(tmpbuffer, sizeof(tmpbuffer), fmt, args);
609
+	trace_android_vh_typec_tcpm_log(tmpbuffer, &bypass_log);
610
+	if (bypass_log)
611
+		goto abort;
572612 
573613 	if (tcpm_log_full(port)) {
574614 		port->logbuffer_head = max(port->logbuffer_head - 1, 0);
....@@ -705,17 +745,13 @@
705745 }
706746 DEFINE_SHOW_ATTRIBUTE(tcpm_debug);
707747 
708
-static struct dentry *rootdir;
709
-
710748 static void tcpm_debugfs_init(struct tcpm_port *port)
711749 {
712
-	mutex_init(&port->logbuffer_lock);
713
-	/* /sys/kernel/debug/tcpm/usbcX */
714
-	if (!rootdir)
715
-		rootdir = debugfs_create_dir("tcpm", NULL);
750
+	char name[NAME_MAX];
716751 
717
-	port->dentry = debugfs_create_file(dev_name(port->dev),
718
-					   S_IFREG | 0444, rootdir,
752
+	mutex_init(&port->logbuffer_lock);
753
+	snprintf(name, NAME_MAX, "tcpm-%s", dev_name(port->dev));
754
+	port->dentry = debugfs_create_file(name, S_IFREG | 0444, usb_debug_root,
719755 					   port, &tcpm_debug_fops);
720756 }
721757 
....@@ -731,10 +767,6 @@
731767 	mutex_unlock(&port->logbuffer_lock);
732768 
733769 	debugfs_remove(port->dentry);
734
-	if (list_empty(&rootdir->d_subdirs)) {
735
-		debugfs_remove(rootdir);
736
-		rootdir = NULL;
737
-	}
738770 }
739771 
740772 #else
....@@ -749,110 +781,39 @@
749781 
750782 #endif
751783 
752
-static int tcpm_send_vbus_notify(struct tcpm_port *port, bool enable)
753
-{
754
-	int ret = 0;
755
-
756
-	if (port->vbus_on == enable) {
757
-		tcpm_log(port, "vbus is already %s", enable ? "on" : "Off");
758
-		goto done;
759
-	}
760
-
761
-	if (port->vbus) {
762
-		if (enable)
763
-			ret = regulator_enable(port->vbus);
764
-		else
765
-			ret = regulator_disable(port->vbus);
766
-		if (ret < 0) {
767
-			dev_err(port->dev, "failed to %s vbus supply(%d)\n",
768
-				enable ? "enable" : "disable", ret);
769
-			goto done;
770
-		}
771
-
772
-		/* Only set state here, don't sync notifier to PMIC */
773
-		extcon_set_state(port->extcon, EXTCON_USB_VBUS_EN, enable);
774
-	} else {
775
-		extcon_set_state(port->extcon, EXTCON_USB_VBUS_EN, enable);
776
-		extcon_sync(port->extcon, EXTCON_USB_VBUS_EN);
777
-		tcpm_log(port, "tcpm driver send extcon to %s vbus 5v\n",
778
-			 enable ? "enable" : "disable");
779
-	}
780
-
781
-	port->vbus_on = enable;
782
-
783
-done:
784
-	return ret;
785
-}
786
-
787
-static void tcpm_send_orientation_notify(struct tcpm_port *port)
788
-{
789
-	union extcon_property_value property;
790
-
791
-	property.intval = port->polarity;
792
-	extcon_set_property(port->extcon, EXTCON_USB,
793
-			    EXTCON_PROP_USB_TYPEC_POLARITY, property);
794
-	extcon_set_property(port->extcon, EXTCON_USB_HOST,
795
-			    EXTCON_PROP_USB_TYPEC_POLARITY, property);
796
-	extcon_set_property(port->extcon, EXTCON_DISP_DP,
797
-			    EXTCON_PROP_USB_TYPEC_POLARITY, property);
798
-}
799
-
800
-static void tcpm_send_data_role_notify(struct tcpm_port *port, bool attached,
801
-				  enum typec_data_role data)
802
-{
803
-	bool dfp = false;
804
-	bool ufp = false;
805
-
806
-	if (attached) {
807
-		if (data == TYPEC_HOST)
808
-			dfp = true;
809
-		else
810
-			ufp = true;
811
-	}
812
-	extcon_set_state(port->extcon, EXTCON_USB, ufp);
813
-	extcon_set_state(port->extcon, EXTCON_USB_HOST, dfp);
814
-	extcon_sync(port->extcon, EXTCON_USB);
815
-	extcon_sync(port->extcon, EXTCON_USB_HOST);
816
-}
817
-
818
-static void tcpm_send_dp_notify(struct tcpm_port *port)
819
-{
820
-	union extcon_property_value property;
821
-	bool usb_ss = false;
822
-	bool hpd = false;
823
-
824
-	if (port->dp_configured) {
825
-		usb_ss = (port->dp_pin_assignment &
826
-			  DP_PIN_ASSIGN_MULTI_FUNCTION_MASK) ? true : false;
827
-		hpd = !!(port->dp_status & DP_STATUS_HPD_STATE);
828
-	}
829
-	property.intval = usb_ss;
830
-	extcon_set_property(port->extcon, EXTCON_USB,
831
-			    EXTCON_PROP_USB_SS, property);
832
-	extcon_set_property(port->extcon, EXTCON_USB_HOST,
833
-			    EXTCON_PROP_USB_SS, property);
834
-	extcon_set_property(port->extcon, EXTCON_DISP_DP,
835
-			    EXTCON_PROP_USB_SS, property);
836
-	extcon_set_state(port->extcon, EXTCON_DISP_DP, port->dp_configured && hpd);
837
-	extcon_sync(port->extcon, EXTCON_DISP_DP);
838
-}
839
-
840
-static void tcpm_send_power_change_notify(struct tcpm_port *port)
841
-{
842
-	union extcon_property_value property;
843
-
844
-	property.intval = (port->current_limit << 15 | port->supply_voltage);
845
-	extcon_set_property(port->extcon, EXTCON_CHG_USB_FAST,
846
-			    EXTCON_PROP_USB_TYPEC_POLARITY, property);
847
-	extcon_set_state(port->extcon, EXTCON_CHG_USB_FAST, true);
848
-	extcon_sync(port->extcon, EXTCON_CHG_USB_FAST);
849
-}
850
-
851784 static void tcpm_set_cc(struct tcpm_port *port, enum typec_cc_status cc)
852785 {
853786 	tcpm_log(port, "cc:=%d", cc);
854787 	port->cc_req = cc;
855788 	port->tcpc->set_cc(port->tcpc, cc);
789
+}
790
+
791
+static int tcpm_enable_auto_vbus_discharge(struct tcpm_port *port, bool enable)
792
+{
793
+	int ret = 0;
794
+
795
+	if (port->tcpc->enable_auto_vbus_discharge) {
796
+		ret = port->tcpc->enable_auto_vbus_discharge(port->tcpc, enable);
797
+		tcpm_log_force(port, "%s vbus discharge ret:%d", enable ? "enable" : "disable",
798
+			       ret);
799
+		if (!ret)
800
+			port->auto_vbus_discharge_enabled = enable;
801
+	}
802
+
803
+	return ret;
804
+}
805
+
806
+static void tcpm_apply_rc(struct tcpm_port *port)
807
+{
808
+	/*
809
+	 * TCPCI: Move to APPLY_RC state to prevent disconnect during PR_SWAP
810
+	 * when Vbus auto discharge on disconnect is enabled.
811
+	 */
812
+	if (port->tcpc->enable_auto_vbus_discharge && port->tcpc->apply_rc) {
813
+		tcpm_log(port, "Apply_RC");
814
+		port->tcpc->apply_rc(port->tcpc, port->cc_req, port->polarity);
815
+		tcpm_enable_auto_vbus_discharge(port, false);
816
+	}
856817 }
857818 
858819 /*
....@@ -888,10 +849,8 @@
888849 	return TYPEC_CC_RP_DEF;
889850 }
890851 
891
-static int tcpm_ams_finish(struct tcpm_port *port)
852
+static void tcpm_ams_finish(struct tcpm_port *port)
892853 {
893
-	int ret = 0;
894
-
895854 	tcpm_log(port, "AMS %s finished", tcpm_ams_str[port->ams]);
896855 
897856 	if (port->pd_capable && port->pwr_role == TYPEC_SOURCE) {
....@@ -905,8 +864,6 @@
905864 
906865 	port->in_ams = false;
907866 	port->ams = NONE_AMS;
908
-
909
-	return ret;
910867 }
911868 
912869 static int tcpm_pd_transmit(struct tcpm_port *port,
....@@ -922,7 +879,7 @@
922879 		tcpm_log(port, "PD TX, type: %#x", type);
923880 
924881 	reinit_completion(&port->tx_complete);
925
-	ret = port->tcpc->pd_transmit(port->tcpc, type, msg);
882
+	ret = port->tcpc->pd_transmit(port->tcpc, type, msg, port->negotiated_rev);
926883 	if (ret < 0)
927884 		return ret;
928885 
....@@ -1024,6 +981,21 @@
1024981 	return ret;
1025982 }
1026983 
984
+bool tcpm_is_debouncing(struct tcpm_port *port)
985
+{
986
+	bool debounce;
987
+
988
+	if (!port)
989
+		return false;
990
+
991
+	mutex_lock(&port->lock);
992
+	debounce = port->debouncing;
993
+	mutex_unlock(&port->lock);
994
+
995
+	return debounce;
996
+}
997
+EXPORT_SYMBOL_GPL(tcpm_is_debouncing);
998
+
1027999 static u32 tcpm_get_current_limit(struct tcpm_port *port)
10281000 {
10291001 	enum typec_cc_status cc;
....@@ -1057,6 +1029,7 @@
10571029 
10581030 	port->supply_voltage = mv;
10591031 	port->current_limit = max_ma;
1032
+	power_supply_changed(port->psy);
10601033 
10611034 	if (port->tcpc->set_current_limit)
10621035 		ret = port->tcpc->set_current_limit(port->tcpc, max_ma, mv);
....@@ -1082,10 +1055,30 @@
10821055 	else
10831056 		orientation = TYPEC_ORIENTATION_REVERSE;
10841057 
1085
-	if (data == TYPEC_HOST)
1086
-		usb_role = USB_ROLE_HOST;
1087
-	else
1088
-		usb_role = USB_ROLE_DEVICE;
1058
+	if (port->typec_caps.data == TYPEC_PORT_DRD) {
1059
+		if (data == TYPEC_HOST)
1060
+			usb_role = USB_ROLE_HOST;
1061
+		else
1062
+			usb_role = USB_ROLE_DEVICE;
1063
+	} else if (port->typec_caps.data == TYPEC_PORT_DFP) {
1064
+		if (data == TYPEC_HOST) {
1065
+			if (role == TYPEC_SOURCE)
1066
+				usb_role = USB_ROLE_HOST;
1067
+			else
1068
+				usb_role = USB_ROLE_NONE;
1069
+		} else {
1070
+			return -ENOTSUPP;
1071
+		}
1072
+	} else {
1073
+		if (data == TYPEC_DEVICE) {
1074
+			if (role == TYPEC_SINK)
1075
+				usb_role = USB_ROLE_DEVICE;
1076
+			else
1077
+				usb_role = USB_ROLE_NONE;
1078
+		} else {
1079
+			return -ENOTSUPP;
1080
+		}
1081
+	}
10891082 
10901083 	ret = tcpm_mux_set(port, TYPEC_STATE_USB, usb_role, orientation);
10911084 	if (ret < 0)
....@@ -1118,13 +1111,47 @@
11181111 	return 0;
11191112 }
11201113 
1114
+/*
1115
+ * Transform the PDO to be compliant to PD rev2.0.
1116
+ * Return 0 if the PDO type is not defined in PD rev2.0.
1117
+ * Otherwise, return the converted PDO.
1118
+ */
1119
+static u32 tcpm_forge_legacy_pdo(struct tcpm_port *port, u32 pdo, enum typec_role role)
1120
+{
1121
+	switch (pdo_type(pdo)) {
1122
+	case PDO_TYPE_FIXED:
1123
+		if (role == TYPEC_SINK)
1124
+			return pdo & ~PDO_FIXED_FRS_CURR_MASK;
1125
+		else
1126
+			return pdo & ~PDO_FIXED_UNCHUNK_EXT;
1127
+	case PDO_TYPE_VAR:
1128
+	case PDO_TYPE_BATT:
1129
+		return pdo;
1130
+	case PDO_TYPE_APDO:
1131
+	default:
1132
+		return 0;
1133
+	}
1134
+}
1135
+
11211136 static int tcpm_pd_send_source_caps(struct tcpm_port *port)
11221137 {
11231138 	struct pd_message msg;
1124
-	int i;
1139
+	u32 pdo;
1140
+	unsigned int i, nr_pdo = 0;
11251141 
11261142 	memset(&msg, 0, sizeof(msg));
1127
-	if (!port->nr_src_pdo) {
1143
+
1144
+	for (i = 0; i < port->nr_src_pdo; i++) {
1145
+		if (port->negotiated_rev >= PD_REV30) {
1146
+			msg.payload[nr_pdo++] =	cpu_to_le32(port->src_pdo[i]);
1147
+		} else {
1148
+			pdo = tcpm_forge_legacy_pdo(port, port->src_pdo[i], TYPEC_SOURCE);
1149
+			if (pdo)
1150
+				msg.payload[nr_pdo++] = cpu_to_le32(pdo);
1151
+		}
1152
+	}
1153
+
1154
+	if (!nr_pdo) {
11281155 		/* No source capabilities defined, sink only */
11291156 		msg.header = PD_HEADER_LE(PD_CTRL_REJECT,
11301157 					  port->pwr_role,
....@@ -1137,10 +1164,8 @@
11371164 					  port->data_role,
11381165 					  port->negotiated_rev,
11391166 					  port->message_id,
1140
-					  port->nr_src_pdo);
1167
+					  nr_pdo);
11411168 	}
1142
-	for (i = 0; i < port->nr_src_pdo; i++)
1143
-		msg.payload[i] = cpu_to_le32(port->src_pdo[i]);
11441169 
11451170 	return tcpm_pd_transmit(port, TCPC_TX_SOP, &msg);
11461171 }
....@@ -1148,10 +1173,22 @@
11481173 static int tcpm_pd_send_sink_caps(struct tcpm_port *port)
11491174 {
11501175 	struct pd_message msg;
1151
-	int i;
1176
+	u32 pdo;
1177
+	unsigned int i, nr_pdo = 0;
11521178 
11531179 	memset(&msg, 0, sizeof(msg));
1154
-	if (!port->nr_snk_pdo) {
1180
+
1181
+	for (i = 0; i < port->nr_snk_pdo; i++) {
1182
+		if (port->negotiated_rev >= PD_REV30) {
1183
+			msg.payload[nr_pdo++] =	cpu_to_le32(port->snk_pdo[i]);
1184
+		} else {
1185
+			pdo = tcpm_forge_legacy_pdo(port, port->snk_pdo[i], TYPEC_SINK);
1186
+			if (pdo)
1187
+				msg.payload[nr_pdo++] = cpu_to_le32(pdo);
1188
+		}
1189
+	}
1190
+
1191
+	if (!nr_pdo) {
11551192 		/* No sink capabilities defined, source only */
11561193 		msg.header = PD_HEADER_LE(PD_CTRL_REJECT,
11571194 					  port->pwr_role,
....@@ -1164,10 +1201,8 @@
11641201 					  port->data_role,
11651202 					  port->negotiated_rev,
11661203 					  port->message_id,
1167
-					  port->nr_snk_pdo);
1204
+					  nr_pdo);
11681205 	}
1169
-	for (i = 0; i < port->nr_snk_pdo; i++)
1170
-		msg.payload[i] = cpu_to_le32(port->snk_pdo[i]);
11711206 
11721207 	return tcpm_pd_transmit(port, TCPC_TX_SOP, &msg);
11731208 }
....@@ -1203,14 +1238,13 @@
12031238 	}
12041239 }
12051240 
1206
-static void mod_data_role_swap_work(struct tcpm_port *port, unsigned int delay_ms)
1241
+static void mod_send_discover_delayed_work(struct tcpm_port *port, unsigned int delay_ms)
12071242 {
12081243 	if (delay_ms) {
1209
-		hrtimer_start(&port->data_role_swap_timer,
1210
-			      ms_to_ktime(delay_ms), HRTIMER_MODE_REL);
1244
+		hrtimer_start(&port->send_discover_timer, ms_to_ktime(delay_ms), HRTIMER_MODE_REL);
12111245 	} else {
1212
-		hrtimer_cancel(&port->data_role_swap_timer);
1213
-		kthread_queue_work(port->wq, &port->data_role_swap);
1246
+		hrtimer_cancel(&port->send_discover_timer);
1247
+		kthread_queue_work(port->wq, &port->send_discover_work);
12141248 	}
12151249 }
12161250 
....@@ -1322,7 +1356,8 @@
13221356 
13231357 	tcpm_log(port, "AMS %s start", tcpm_ams_str[ams]);
13241358 
1325
-	if (!tcpm_ams_interruptible(port) && ams != HARD_RESET) {
1359
+	if (!tcpm_ams_interruptible(port) &&
1360
+	    !(ams == HARD_RESET || ams == SOFT_RESET_AMS)) {
13261361 		port->upcoming_state = INVALID_STATE;
13271362 		tcpm_log(port, "AMS %s not interruptible, aborting",
13281363 			 tcpm_ams_str[port->ams]);
....@@ -1340,11 +1375,10 @@
13401375 			tcpm_set_state(port, HARD_RESET_START, 0);
13411376 			return ret;
13421377 		} else if (ams == SOFT_RESET_AMS) {
1343
-			if (!port->explicit_contract) {
1344
-				port->upcoming_state = INVALID_STATE;
1378
+			if (!port->explicit_contract)
13451379 				tcpm_set_cc(port, tcpm_rp_cc(port));
1346
-				return ret;
1347
-			}
1380
+			tcpm_set_state(port, SOFT_RESET_SEND, 0);
1381
+			return ret;
13481382 		} else if (tcpm_vdm_ams(port)) {
13491383 			/* tSinkTx is enforced in vdm_run_state_machine */
13501384 			if (port->negotiated_rev >= PD_REV30)
....@@ -1413,24 +1447,56 @@
14131447 static void tcpm_queue_vdm(struct tcpm_port *port, const u32 header,
14141448 			   const u32 *data, int cnt)
14151449 {
1450
+	u32 vdo_hdr = port->vdo_data[0];
1451
+
1452
+	WARN_ON(!mutex_is_locked(&port->lock));
1453
+
1454
+	/* If is sending discover_identity, handle received message first */
1455
+	if (PD_VDO_SVDM(vdo_hdr) && PD_VDO_CMD(vdo_hdr) == CMD_DISCOVER_IDENT) {
1456
+		port->send_discover = true;
1457
+		mod_send_discover_delayed_work(port, SEND_DISCOVER_RETRY_MS);
1458
+	} else {
1459
+		/* Make sure we are not still processing a previous VDM packet */
1460
+		WARN_ON(port->vdm_state > VDM_STATE_DONE);
1461
+	}
1462
+
14161463 	port->vdo_count = cnt + 1;
14171464 	port->vdo_data[0] = header;
14181465 	memcpy(&port->vdo_data[1], data, sizeof(u32) * cnt);
14191466 	/* Set ready, vdm state machine will actually send */
14201467 	port->vdm_retries = 0;
14211468 	port->vdm_state = VDM_STATE_READY;
1469
+	port->vdm_sm_running = true;
1470
+
1471
+	mod_vdm_delayed_work(port, 0);
14221472 }
14231473 
1424
-static void svdm_consume_identity(struct tcpm_port *port, const __le32 *payload,
1425
-				  int cnt)
1474
+static void tcpm_queue_vdm_unlocked(struct tcpm_port *port, const u32 header,
1475
+				    const u32 *data, int cnt)
14261476 {
1427
-	u32 vdo = le32_to_cpu(payload[VDO_INDEX_IDH]);
1428
-	u32 product = le32_to_cpu(payload[VDO_INDEX_PRODUCT]);
1477
+#ifdef CONFIG_NO_GKI
1478
+	mutex_lock(&port->pd_handler_lock);
1479
+	if (tcpm_port_is_disconnected(port))
1480
+		goto unlock;
1481
+#endif
1482
+	mutex_lock(&port->lock);
1483
+	tcpm_queue_vdm(port, header, data, cnt);
1484
+	mutex_unlock(&port->lock);
1485
+#ifdef CONFIG_NO_GKI
1486
+unlock:
1487
+	mutex_unlock(&port->pd_handler_lock);
1488
+#endif
1489
+}
1490
+
1491
+static void svdm_consume_identity(struct tcpm_port *port, const u32 *p, int cnt)
1492
+{
1493
+	u32 vdo = p[VDO_INDEX_IDH];
1494
+	u32 product = p[VDO_INDEX_PRODUCT];
14291495 
14301496 	memset(&port->mode_data, 0, sizeof(port->mode_data));
14311497 
14321498 	port->partner_ident.id_header = vdo;
1433
-	port->partner_ident.cert_stat = le32_to_cpu(payload[VDO_INDEX_CSTAT]);
1499
+	port->partner_ident.cert_stat = p[VDO_INDEX_CSTAT];
14341500 	port->partner_ident.product = product;
14351501 
14361502 	typec_partner_set_identity(port->partner);
....@@ -1440,17 +1506,15 @@
14401506 		 PD_PRODUCT_PID(product), product & 0xffff);
14411507 }
14421508 
1443
-static bool svdm_consume_svids(struct tcpm_port *port, const __le32 *payload,
1444
-			       int cnt)
1509
+static bool svdm_consume_svids(struct tcpm_port *port, const u32 *p, int cnt)
14451510 {
14461511 	struct pd_mode_data *pmdata = &port->mode_data;
14471512 	int i;
14481513 
14491514 	for (i = 1; i < cnt; i++) {
1450
-		u32 p = le32_to_cpu(payload[i]);
14511515 		u16 svid;
14521516 
1453
-		svid = (p >> 16) & 0xffff;
1517
+		svid = (p[i] >> 16) & 0xffff;
14541518 		if (!svid)
14551519 			return false;
14561520 
....@@ -1460,7 +1524,7 @@
14601524 		pmdata->svids[pmdata->nsvids++] = svid;
14611525 		tcpm_log(port, "SVID %d: 0x%x", pmdata->nsvids, svid);
14621526 
1463
-		svid = p & 0xffff;
1527
+		svid = p[i] & 0xffff;
14641528 		if (!svid)
14651529 			return false;
14661530 
....@@ -1470,14 +1534,27 @@
14701534 		pmdata->svids[pmdata->nsvids++] = svid;
14711535 		tcpm_log(port, "SVID %d: 0x%x", pmdata->nsvids, svid);
14721536 	}
1473
-	return true;
1537
+
1538
+	/*
1539
+	 * PD3.0 Spec 6.4.4.3.2: The SVIDs are returned 2 per VDO (see Table
1540
+	 * 6-43), and can be returned maximum 6 VDOs per response (see Figure
1541
+	 * 6-19). If the Respondersupports 12 or more SVID then the Discover
1542
+	 * SVIDs Command Shall be executed multiple times until a Discover
1543
+	 * SVIDs VDO is returned ending either with a SVID value of 0x0000 in
1544
+	 * the last part of the last VDO or with a VDO containing two SVIDs
1545
+	 * with values of 0x0000.
1546
+	 *
1547
+	 * However, some odd dockers support SVIDs less than 12 but without
1548
+	 * 0x0000 in the last VDO, so we need to break the Discover SVIDs
1549
+	 * request and return false here.
1550
+	 */
1551
+	return cnt == 7;
14741552 abort:
14751553 	tcpm_log(port, "SVID_DISCOVERY_MAX(%d) too low!", SVID_DISCOVERY_MAX);
14761554 	return false;
14771555 }
14781556 
1479
-static void svdm_consume_modes(struct tcpm_port *port, const __le32 *payload,
1480
-			       int cnt)
1557
+static void svdm_consume_modes(struct tcpm_port *port, const u32 *p, int cnt)
14811558 {
14821559 	struct pd_mode_data *pmdata = &port->mode_data;
14831560 	struct typec_altmode_desc *paltmode;
....@@ -1494,7 +1571,7 @@
14941571 
14951572 		paltmode->svid = pmdata->svids[pmdata->svid_index];
14961573 		paltmode->mode = i;
1497
-		paltmode->vdo = le32_to_cpu(payload[i]);
1574
+		paltmode->vdo = p[i];
14981575 
14991576 		tcpm_log(port, " Alternate mode %d: SVID 0x%04x, VDO %d: 0x%08x",
15001577 			 pmdata->altmodes, paltmode->svid,
....@@ -1513,29 +1590,29 @@
15131590 	for (i = 0; i < modep->altmodes; i++) {
15141591 		altmode = typec_partner_register_altmode(port->partner,
15151592 						&modep->altmode_desc[i]);
1516
-		if (!altmode)
1593
+		if (IS_ERR(altmode)) {
15171594 			tcpm_log(port, "Failed to register partner SVID 0x%04x",
15181595 				 modep->altmode_desc[i].svid);
1596
+			altmode = NULL;
1597
+		}
15191598 		port->partner_altmode[i] = altmode;
15201599 	}
15211600 }
15221601 
15231602 #define supports_modal(port)	PD_IDH_MODAL_SUPP((port)->partner_ident.id_header)
15241603 
1525
-static int tcpm_pd_svdm(struct tcpm_port *port, const __le32 *payload, int cnt,
1526
-			u32 *response)
1604
+static int tcpm_pd_svdm(struct tcpm_port *port, struct typec_altmode *adev,
1605
+			const u32 *p, int cnt, u32 *response,
1606
+			enum adev_actions *adev_action)
15271607 {
1528
-	struct typec_altmode *adev;
1608
+	struct typec_port *typec = port->typec_port;
15291609 	struct typec_altmode *pdev;
15301610 	struct pd_mode_data *modep;
1531
-	u32 p[PD_MAX_PAYLOAD];
1611
+	int svdm_version;
15321612 	int rlen = 0;
15331613 	int cmd_type;
15341614 	int cmd;
15351615 	int i;
1536
-
1537
-	for (i = 0; i < cnt; i++)
1538
-		p[i] = le32_to_cpu(payload[i]);
15391616 
15401617 	cmd_type = PD_VDO_CMDT(p[0]);
15411618 	cmd = PD_VDO_CMD(p[0]);
....@@ -1545,36 +1622,61 @@
15451622 
15461623 	modep = &port->mode_data;
15471624 
1548
-	adev = typec_match_altmode(port->port_altmode, ALTMODE_DISCOVERY_MAX,
1549
-				   PD_VDO_VID(p[0]), PD_VDO_OPOS(p[0]));
1550
-
15511625 	pdev = typec_match_altmode(port->partner_altmode, ALTMODE_DISCOVERY_MAX,
15521626 				   PD_VDO_VID(p[0]), PD_VDO_OPOS(p[0]));
1627
+
1628
+	svdm_version = typec_get_negotiated_svdm_version(typec);
1629
+	if (svdm_version < 0)
1630
+		return 0;
15531631 
15541632 	switch (cmd_type) {
15551633 	case CMDT_INIT:
15561634 		switch (cmd) {
15571635 		case CMD_DISCOVER_IDENT:
1558
-			/* 6.4.4.3.1: Only respond as UFP (device) */
1559
-			if (port->data_role == TYPEC_DEVICE &&
1636
+			if (PD_VDO_VID(p[0]) != USB_SID_PD)
1637
+				break;
1638
+
1639
+			if (PD_VDO_SVDM_VER(p[0]) < svdm_version) {
1640
+				typec_partner_set_svdm_version(port->partner,
1641
+							       PD_VDO_SVDM_VER(p[0]));
1642
+				svdm_version = PD_VDO_SVDM_VER(p[0]);
1643
+			}
1644
+
1645
+			port->ams = DISCOVER_IDENTITY;
1646
+			/*
1647
+			 * PD2.0 Spec 6.10.3: respond with NAK as DFP (data host)
1648
+			 * PD3.1 Spec 6.4.4.2.5.1: respond with NAK if "invalid field" or
1649
+			 * "wrong configuation" or "Unrecognized"
1650
+			 */
1651
+			if ((port->data_role == TYPEC_DEVICE || svdm_version >= SVDM_VER_2_0) &&
15601652 			    port->nr_snk_vdo) {
1561
-				for (i = 0; i <  port->nr_snk_vdo; i++)
1562
-					response[i + 1] = port->snk_vdo[i];
1563
-				rlen = port->nr_snk_vdo + 1;
1653
+				if (svdm_version < SVDM_VER_2_0) {
1654
+					for (i = 0; i < port->nr_snk_vdo_v1; i++)
1655
+						response[i + 1] = port->snk_vdo_v1[i];
1656
+					rlen = port->nr_snk_vdo_v1 + 1;
1657
+
1658
+				} else {
1659
+					for (i = 0; i < port->nr_snk_vdo; i++)
1660
+						response[i + 1] = port->snk_vdo[i];
1661
+					rlen = port->nr_snk_vdo + 1;
1662
+				}
15641663 			}
15651664 			break;
15661665 		case CMD_DISCOVER_SVID:
1666
+			port->ams = DISCOVER_SVIDS;
15671667 			break;
15681668 		case CMD_DISCOVER_MODES:
1669
+			port->ams = DISCOVER_MODES;
15691670 			break;
15701671 		case CMD_ENTER_MODE:
1672
+			port->ams = DFP_TO_UFP_ENTER_MODE;
15711673 			break;
15721674 		case CMD_EXIT_MODE:
1675
+			port->ams = DFP_TO_UFP_EXIT_MODE;
15731676 			break;
15741677 		case CMD_ATTENTION:
15751678 			/* Attention command does not have response */
1576
-			if (adev)
1577
-				typec_altmode_attention(adev, p[1]);
1679
+			*adev_action = ADEV_ATTENTION;
15781680 			return 0;
15791681 		default:
15801682 			break;
....@@ -1588,6 +1690,8 @@
15881690 			response[0] = p[0] | VDO_CMDT(CMDT_RSP_BUSY);
15891691 			rlen = 1;
15901692 		}
1693
+		response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) |
1694
+			      (VDO_SVDM_VERS(typec_get_negotiated_svdm_version(typec)));
15911695 		break;
15921696 	case CMDT_RSP_ACK:
15931697 		/* silently drop message if we are not connected */
....@@ -1598,101 +1702,139 @@
15981702 
15991703 		switch (cmd) {
16001704 		case CMD_DISCOVER_IDENT:
1705
+			if (PD_VDO_SVDM_VER(p[0]) < svdm_version)
1706
+				typec_partner_set_svdm_version(port->partner,
1707
+							       PD_VDO_SVDM_VER(p[0]));
16011708 			/* 6.4.4.3.1 */
1602
-			svdm_consume_identity(port, payload, cnt);
1603
-			response[0] = VDO(USB_SID_PD, 1, CMD_DISCOVER_SVID);
1709
+			svdm_consume_identity(port, p, cnt);
1710
+			response[0] = VDO(USB_SID_PD, 1, typec_get_negotiated_svdm_version(typec),
1711
+					  CMD_DISCOVER_SVID);
16041712 			rlen = 1;
16051713 			break;
16061714 		case CMD_DISCOVER_SVID:
16071715 			/* 6.4.4.3.2 */
1608
-			if (svdm_consume_svids(port, payload, cnt)) {
1609
-				response[0] = VDO(USB_SID_PD, 1,
1610
-						  CMD_DISCOVER_SVID);
1716
+			if (svdm_consume_svids(port, p, cnt)) {
1717
+				response[0] = VDO(USB_SID_PD, 1, svdm_version, CMD_DISCOVER_SVID);
16111718 				rlen = 1;
16121719 			} else if (modep->nsvids && supports_modal(port)) {
1613
-				response[0] = VDO(modep->svids[0], 1,
1720
+				response[0] = VDO(modep->svids[0], 1, svdm_version,
16141721 						  CMD_DISCOVER_MODES);
16151722 				rlen = 1;
16161723 			}
16171724 			break;
16181725 		case CMD_DISCOVER_MODES:
16191726 			/* 6.4.4.3.3 */
1620
-			svdm_consume_modes(port, payload, cnt);
1727
+			svdm_consume_modes(port, p, cnt);
16211728 			modep->svid_index++;
16221729 			if (modep->svid_index < modep->nsvids) {
16231730 				u16 svid = modep->svids[modep->svid_index];
1624
-				response[0] = VDO(svid, 1, CMD_DISCOVER_MODES);
1731
+				response[0] = VDO(svid, 1, svdm_version, CMD_DISCOVER_MODES);
16251732 				rlen = 1;
1626
-			} else {
1733
+			} else if (port->data_role == TYPEC_HOST) {
16271734 				tcpm_register_partner_altmodes(port);
1628
-				port->vdm_sm_running = false;
1735
+			} else {
1736
+				/* Do dr_swap for ufp if the port supports drd */
1737
+				if (port->typec_caps.data == TYPEC_PORT_DRD &&
1738
+				    !IS_ERR_OR_NULL(port->port_altmode[0])) {
1739
+					port->vdm_sm_running = false;
1740
+					port->upcoming_state = DR_SWAP_SEND;
1741
+					tcpm_ams_start(port, DATA_ROLE_SWAP);
1742
+				}
16291743 			}
16301744 			break;
16311745 		case CMD_ENTER_MODE:
16321746 			if (adev && pdev) {
16331747 				typec_altmode_update_active(pdev, true);
1634
-
1635
-				if (typec_altmode_vdm(adev, p[0], &p[1], cnt)) {
1636
-					response[0] = VDO(adev->svid, 1,
1637
-							  CMD_EXIT_MODE);
1638
-					response[0] |= VDO_OPOS(adev->mode);
1639
-					return 1;
1640
-				}
1748
+				*adev_action = ADEV_QUEUE_VDM_SEND_EXIT_MODE_ON_FAIL;
16411749 			}
16421750 			return 0;
16431751 		case CMD_EXIT_MODE:
16441752 			if (adev && pdev) {
16451753 				typec_altmode_update_active(pdev, false);
1646
-
16471754 				/* Back to USB Operation */
1648
-				WARN_ON(typec_altmode_notify(adev,
1649
-							     TYPEC_STATE_USB,
1650
-							     NULL));
1755
+				*adev_action = ADEV_NOTIFY_USB_AND_QUEUE_VDM;
1756
+				return 0;
16511757 			}
16521758 			break;
1759
+		case VDO_CMD_VENDOR(0) ... VDO_CMD_VENDOR(15):
1760
+			break;
16531761 		default:
1762
+			/* Unrecognized SVDM */
1763
+			response[0] = p[0] | VDO_CMDT(CMDT_RSP_NAK);
1764
+			rlen = 1;
1765
+			response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) |
1766
+				      (VDO_SVDM_VERS(svdm_version));
16541767 			break;
16551768 		}
16561769 		break;
16571770 	case CMDT_RSP_NAK:
16581771 		tcpm_ams_finish(port);
16591772 		switch (cmd) {
1773
+		case CMD_DISCOVER_IDENT:
1774
+			/* Do dr_swap for ufp if the port supports drd */
1775
+			if (port->typec_caps.data == TYPEC_PORT_DRD &&
1776
+			    port->data_role == TYPEC_DEVICE &&
1777
+			    !IS_ERR_OR_NULL(port->port_altmode[0])) {
1778
+				port->vdm_sm_running = false;
1779
+				port->upcoming_state = DR_SWAP_SEND;
1780
+				tcpm_ams_start(port, DATA_ROLE_SWAP);
1781
+				break;
1782
+			}
1783
+			fallthrough;
1784
+		case CMD_DISCOVER_SVID:
1785
+		case CMD_DISCOVER_MODES:
1786
+		case VDO_CMD_VENDOR(0) ... VDO_CMD_VENDOR(15):
1787
+			break;
16601788 		case CMD_ENTER_MODE:
16611789 			/* Back to USB Operation */
1662
-			if (adev)
1663
-				WARN_ON(typec_altmode_notify(adev,
1664
-							     TYPEC_STATE_USB,
1665
-							     NULL));
1666
-			break;
1790
+			*adev_action = ADEV_NOTIFY_USB_AND_QUEUE_VDM;
1791
+			return 0;
16671792 		default:
1793
+			/* Unrecognized SVDM */
1794
+			response[0] = p[0] | VDO_CMDT(CMDT_RSP_NAK);
1795
+			rlen = 1;
1796
+			response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) |
1797
+				      (VDO_SVDM_VERS(svdm_version));
16681798 			break;
16691799 		}
1670
-		port->vdm_sm_running = false;
16711800 		break;
16721801 	default:
1673
-		port->vdm_sm_running = false;
1802
+		response[0] = p[0] | VDO_CMDT(CMDT_RSP_NAK);
1803
+		rlen = 1;
1804
+		response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) |
1805
+			      (VDO_SVDM_VERS(svdm_version));
16741806 		break;
16751807 	}
16761808 
16771809 	/* Informing the alternate mode drivers about everything */
1678
-	if (adev)
1679
-		typec_altmode_vdm(adev, p[0], &p[1], cnt);
1680
-
1810
+	*adev_action = ADEV_QUEUE_VDM;
16811811 	return rlen;
16821812 }
1813
+
1814
+static void tcpm_pd_handle_msg(struct tcpm_port *port,
1815
+			       enum pd_msg_request message,
1816
+			       enum tcpm_ams ams);
16831817 
16841818 static void tcpm_handle_vdm_request(struct tcpm_port *port,
16851819 				    const __le32 *payload, int cnt)
16861820 {
1687
-	int rlen = 0;
1821
+	enum adev_actions adev_action = ADEV_NONE;
1822
+	struct typec_altmode *adev;
1823
+	u32 p[PD_MAX_PAYLOAD];
16881824 	u32 response[8] = { };
1689
-	u32 p0 = le32_to_cpu(payload[0]);
1825
+	int i, rlen = 0;
1826
+
1827
+	for (i = 0; i < cnt; i++)
1828
+		p[i] = le32_to_cpu(payload[i]);
1829
+
1830
+	adev = typec_match_altmode(port->port_altmode, ALTMODE_DISCOVERY_MAX,
1831
+				   PD_VDO_VID(p[0]), PD_VDO_OPOS(p[0]));
16901832 
16911833 	if (port->vdm_state == VDM_STATE_BUSY) {
16921834 		/* If UFP responded busy retry after timeout */
1693
-		if (PD_VDO_CMDT(p0) == CMDT_RSP_BUSY) {
1835
+		if (PD_VDO_CMDT(p[0]) == CMDT_RSP_BUSY) {
16941836 			port->vdm_state = VDM_STATE_WAIT_RSP_BUSY;
1695
-			port->vdo_retry = (p0 & ~VDO_CMDT_MASK) |
1837
+			port->vdo_retry = (p[0] & ~VDO_CMDT_MASK) |
16961838 				CMDT_INIT;
16971839 			mod_vdm_delayed_work(port, PD_T_VDM_BUSY);
16981840 			return;
....@@ -1700,29 +1842,111 @@
17001842 		port->vdm_state = VDM_STATE_DONE;
17011843 	}
17021844 
1703
-	if (PD_VDO_SVDM(p0))
1704
-		rlen = tcpm_pd_svdm(port, payload, cnt, response);
1705
-
1706
-	if (rlen > 0) {
1707
-		tcpm_queue_vdm(port, response[0], &response[1], rlen - 1);
1708
-		mod_vdm_delayed_work(port, 0);
1845
+	if (PD_VDO_SVDM(p[0]) && (adev || tcpm_vdm_ams(port) || port->nr_snk_vdo)) {
1846
+		/*
1847
+		 * Here a SVDM is received (INIT or RSP or unknown). Set the vdm_sm_running in
1848
+		 * advance because we are dropping the lock but may send VDMs soon.
1849
+		 * For the cases of INIT received:
1850
+		 *  - If no response to send, it will be cleared later in this function.
1851
+		 *  - If there are responses to send, it will be cleared in the state machine.
1852
+		 * For the cases of RSP received:
1853
+		 *  - If no further INIT to send, it will be cleared later in this function.
1854
+		 *  - Otherwise, it will be cleared in the state machine if timeout or it will go
1855
+		 *    back here until no further INIT to send.
1856
+		 * For the cases of unknown type received:
1857
+		 *  - We will send NAK and the flag will be cleared in the state machine.
1858
+		 */
1859
+		port->vdm_sm_running = true;
1860
+		rlen = tcpm_pd_svdm(port, adev, p, cnt, response, &adev_action);
1861
+	} else {
1862
+		if (port->negotiated_rev >= PD_REV30)
1863
+			tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
17091864 	}
1865
+
1866
+	/*
1867
+	 * We are done with any state stored in the port struct now, except
1868
+	 * for any port struct changes done by the tcpm_queue_vdm() call
1869
+	 * below, which is a separate operation.
1870
+	 *
1871
+	 * So we can safely release the lock here; and we MUST release the
1872
+	 * lock here to avoid an AB BA lock inversion:
1873
+	 *
1874
+	 * If we keep the lock here then the lock ordering in this path is:
1875
+	 * 1. tcpm_pd_rx_handler take the tcpm port lock
1876
+	 * 2. One of the typec_altmode_* calls below takes the alt-mode's lock
1877
+	 *
1878
+	 * And we also have this ordering:
1879
+	 * 1. alt-mode driver takes the alt-mode's lock
1880
+	 * 2. alt-mode driver calls tcpm_altmode_enter which takes the
1881
+	 *    tcpm port lock
1882
+	 *
1883
+	 * Dropping our lock here avoids this.
1884
+	 */
1885
+	mutex_unlock(&port->lock);
1886
+
1887
+	if (adev) {
1888
+		switch (adev_action) {
1889
+		case ADEV_NONE:
1890
+			break;
1891
+		case ADEV_NOTIFY_USB_AND_QUEUE_VDM:
1892
+			WARN_ON(typec_altmode_notify(adev, TYPEC_STATE_USB, NULL));
1893
+			typec_altmode_vdm(adev, p[0], &p[1], cnt);
1894
+			break;
1895
+		case ADEV_QUEUE_VDM:
1896
+			typec_altmode_vdm(adev, p[0], &p[1], cnt);
1897
+			break;
1898
+		case ADEV_QUEUE_VDM_SEND_EXIT_MODE_ON_FAIL:
1899
+			if (typec_altmode_vdm(adev, p[0], &p[1], cnt)) {
1900
+				int svdm_version = typec_get_negotiated_svdm_version(
1901
+									port->typec_port);
1902
+				if (svdm_version < 0)
1903
+					break;
1904
+
1905
+				response[0] = VDO(adev->svid, 1, svdm_version,
1906
+						  CMD_EXIT_MODE);
1907
+				response[0] |= VDO_OPOS(adev->mode);
1908
+				rlen = 1;
1909
+			}
1910
+			break;
1911
+		case ADEV_ATTENTION:
1912
+			if (typec_altmode_attention(adev, p[1]))
1913
+				tcpm_log(port, "typec_altmode_attention no port partner altmode");
1914
+			break;
1915
+		}
1916
+	}
1917
+
1918
+	/*
1919
+	 * We must re-take the lock here to balance the unlock in
1920
+	 * tcpm_pd_rx_handler, note that no changes, other then the
1921
+	 * tcpm_queue_vdm call, are made while the lock is held again.
1922
+	 * All that is done after the call is unwinding the call stack until
1923
+	 * we return to tcpm_pd_rx_handler and do the unlock there.
1924
+	 */
1925
+	mutex_lock(&port->lock);
1926
+
1927
+	if (rlen > 0)
1928
+		tcpm_queue_vdm(port, response[0], &response[1], rlen - 1);
1929
+	else
1930
+		port->vdm_sm_running = false;
17101931 }
17111932 
17121933 static void tcpm_send_vdm(struct tcpm_port *port, u32 vid, int cmd,
17131934 			  const u32 *data, int count)
17141935 {
1936
+	int svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
17151937 	u32 header;
1938
+
1939
+	if (svdm_version < 0)
1940
+		return;
17161941 
17171942 	if (WARN_ON(count > VDO_MAX_SIZE - 1))
17181943 		count = VDO_MAX_SIZE - 1;
17191944 
17201945 	/* set VDM header with VID & CMD */
17211946 	header = VDO(vid, ((vid & USB_SID_PD) == USB_SID_PD) ?
1722
-			1 : (PD_VDO_CMD(cmd) <= CMD_ATTENTION), cmd);
1947
+			1 : (PD_VDO_CMD(cmd) <= CMD_ATTENTION),
1948
+			svdm_version, cmd);
17231949 	tcpm_queue_vdm(port, header, data, count);
1724
-
1725
-	mod_vdm_delayed_work(port, 0);
17261950 }
17271951 
17281952 static unsigned int vdm_ready_timeout(u32 vdm_hdr)
....@@ -1769,16 +1993,23 @@
17691993 		 * if there's traffic or we're not in PDO ready state don't send
17701994 		 * a VDM.
17711995 		 */
1772
-		if (port->state != SRC_READY && port->state != SNK_READY)
1996
+		if (port->state != SRC_READY && port->state != SNK_READY) {
1997
+			port->vdm_sm_running = false;
17731998 			break;
1999
+		}
17742000 
17752001 		/* TODO: AMS operation for Unstructured VDM */
17762002 		if (PD_VDO_SVDM(vdo_hdr) && PD_VDO_CMDT(vdo_hdr) == CMDT_INIT) {
17772003 			switch (PD_VDO_CMD(vdo_hdr)) {
17782004 			case CMD_DISCOVER_IDENT:
17792005 				res = tcpm_ams_start(port, DISCOVER_IDENTITY);
1780
-				if (res == 0)
2006
+				if (res == 0) {
17812007 					port->send_discover = false;
2008
+				} else if (res == -EAGAIN) {
2009
+					port->vdo_data[0] = 0;
2010
+					mod_send_discover_delayed_work(port,
2011
+								       SEND_DISCOVER_RETRY_MS);
2012
+				}
17822013 				break;
17832014 			case CMD_DISCOVER_SVID:
17842015 				res = tcpm_ams_start(port, DISCOVER_SVIDS);
....@@ -1804,7 +2035,7 @@
18042035 			}
18052036 
18062037 			if (res < 0) {
1807
-				port->vdm_sm_running = false;
2038
+				port->vdm_state = VDM_STATE_ERR_BUSY;
18082039 				return;
18092040 			}
18102041 		}
....@@ -1820,6 +2051,7 @@
18202051 		port->vdo_data[0] = port->vdo_retry;
18212052 		port->vdo_count = 1;
18222053 		port->vdm_state = VDM_STATE_READY;
2054
+		tcpm_ams_finish(port);
18232055 		break;
18242056 	case VDM_STATE_BUSY:
18252057 		port->vdm_state = VDM_STATE_ERR_TMOUT;
....@@ -1860,6 +2092,7 @@
18602092 			unsigned long timeout;
18612093 
18622094 			port->vdm_retries = 0;
2095
+			port->vdo_data[0] = 0;
18632096 			port->vdm_state = VDM_STATE_BUSY;
18642097 			timeout = vdm_ready_timeout(vdo_hdr);
18652098 			mod_vdm_delayed_work(port, timeout);
....@@ -1888,7 +2121,7 @@
18882121 		 port->vdm_state != VDM_STATE_BUSY &&
18892122 		 port->vdm_state != VDM_STATE_SEND_MESSAGE);
18902123 
1891
-	if (port->vdm_state == VDM_STATE_ERR_TMOUT)
2124
+	if (port->vdm_state < VDM_STATE_READY)
18922125 		port->vdm_sm_running = false;
18932126 
18942127 	mutex_unlock(&port->lock);
....@@ -2014,101 +2247,46 @@
20142247 	return 0;
20152248 }
20162249 
2017
-static int tcpm_altmode_enter(struct typec_altmode *altmode)
2250
+static int tcpm_altmode_enter(struct typec_altmode *altmode, u32 *vdo)
20182251 {
20192252 	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
2253
+	int svdm_version;
20202254 	u32 header;
2021
-	int ret = 0;
20222255 
2023
-	mutex_lock(&port->pd_handler_lock);
2024
-	if (tcpm_port_is_disconnected(port)) {
2025
-		ret = -ENODEV;
2026
-		goto unlock;
2027
-	}
2256
+	svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
2257
+	if (svdm_version < 0)
2258
+		return svdm_version;
20282259 
2029
-	mutex_lock(&port->lock);
2030
-	header = VDO(altmode->svid, 1, CMD_ENTER_MODE);
2260
+	header = VDO(altmode->svid, vdo ? 2 : 1, svdm_version, CMD_ENTER_MODE);
20312261 	header |= VDO_OPOS(altmode->mode);
20322262 
2033
-	tcpm_queue_vdm(port, header, NULL, 0);
2034
-	mod_vdm_delayed_work(port, 0);
2035
-	mutex_unlock(&port->lock);
2036
-unlock:
2037
-	mutex_unlock(&port->pd_handler_lock);
2038
-	return ret;
2263
+	tcpm_queue_vdm_unlocked(port, header, vdo, vdo ? 1 : 0);
2264
+	return 0;
20392265 }
20402266 
20412267 static int tcpm_altmode_exit(struct typec_altmode *altmode)
20422268 {
20432269 	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
2270
+	int svdm_version;
20442271 	u32 header;
2045
-	int ret = 0;
20462272 
2047
-	mutex_lock(&port->pd_handler_lock);
2048
-	if (tcpm_port_is_disconnected(port)) {
2049
-		ret = -ENODEV;
2050
-		goto unlock;
2051
-	}
2273
+	svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
2274
+	if (svdm_version < 0)
2275
+		return svdm_version;
20522276 
2053
-	mutex_lock(&port->lock);
2054
-	header = VDO(altmode->svid, 1, CMD_EXIT_MODE);
2277
+	header = VDO(altmode->svid, 1, svdm_version, CMD_EXIT_MODE);
20552278 	header |= VDO_OPOS(altmode->mode);
20562279 
2057
-	tcpm_queue_vdm(port, header, NULL, 0);
2058
-	mod_vdm_delayed_work(port, 0);
2059
-	mutex_unlock(&port->lock);
2060
-
2061
-unlock:
2062
-	mutex_unlock(&port->pd_handler_lock);
2063
-	return ret;
2280
+	tcpm_queue_vdm_unlocked(port, header, NULL, 0);
2281
+	return 0;
20642282 }
20652283 
20662284 static int tcpm_altmode_vdm(struct typec_altmode *altmode,
20672285 			    u32 header, const u32 *data, int count)
20682286 {
20692287 	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
2070
-	int ret = 0;
20712288 
2072
-	mutex_lock(&port->pd_handler_lock);
2073
-	if (tcpm_port_is_disconnected(port)) {
2074
-		ret = -ENODEV;
2075
-		goto unlock;
2076
-	}
2077
-
2078
-	mutex_lock(&port->lock);
2079
-	tcpm_queue_vdm(port, header, data, count - 1);
2080
-	mod_vdm_delayed_work(port, 0);
2081
-	mutex_unlock(&port->lock);
2082
-
2083
-unlock:
2084
-	mutex_unlock(&port->pd_handler_lock);
2085
-	return ret;
2086
-}
2087
-
2088
-static int tcpm_altmode_notify(struct typec_altmode *altmode,
2089
-			       unsigned long conf, void *data)
2090
-{
2091
-	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
2092
-	struct typec_displayport_data *dp_data = data;
2093
-
2094
-	if ((conf >= TYPEC_DP_STATE_A) && (conf <= TYPEC_DP_STATE_F)) {
2095
-		if (port->dp_configured) {
2096
-			port->dp_status = dp_data->status;
2097
-			tcpm_send_dp_notify(port);
2098
-			dev_info(port->dev, "dp_status %x\n", dp_data->status);
2099
-		} else {
2100
-			port->dp_pin_assignment = dp_data->conf;
2101
-			port->dp_configured = true;
2102
-			dev_info(port->dev, "DP pin assignment 0x%x\n",
2103
-				 port->dp_pin_assignment);
2104
-		}
2105
-	} else if ((conf == TYPEC_STATE_USB) && (port->dp_configured)) {
2106
-		/* may receive ufp device response Exit Mode Command Ack */
2107
-		port->dp_status = 0;
2108
-		port->dp_pin_assignment = 0;
2109
-		port->dp_configured = false;
2110
-		tcpm_send_dp_notify(port);
2111
-	}
2289
+	tcpm_queue_vdm_unlocked(port, header, data, count - 1);
21122290 
21132291 	return 0;
21142292 }
....@@ -2117,7 +2295,6 @@
21172295 	.enter = tcpm_altmode_enter,
21182296 	.exit = tcpm_altmode_exit,
21192297 	.vdm = tcpm_altmode_vdm,
2120
-	.notify = tcpm_altmode_notify,
21212298 };
21222299 
21232300 /*
....@@ -2142,20 +2319,108 @@
21422319 
21432320 	if (!type) {
21442321 		tcpm_log(port, "Alert message received with no type");
2322
+		tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
21452323 		return;
21462324 	}
21472325 
21482326 	/* Just handling non-battery alerts for now */
21492327 	if (!(type & USB_PD_ADO_TYPE_BATT_STATUS_CHANGE)) {
2150
-		switch (port->state) {
2151
-		case SRC_READY:
2152
-		case SNK_READY:
2328
+		if (port->pwr_role == TYPEC_SOURCE) {
2329
+			port->upcoming_state = GET_STATUS_SEND;
2330
+			tcpm_ams_start(port, GETTING_SOURCE_SINK_STATUS);
2331
+		} else {
2332
+			/*
2333
+			 * Do not check SinkTxOk here in case the Source doesn't set its Rp to
2334
+			 * SinkTxOk in time.
2335
+			 */
2336
+			port->ams = GETTING_SOURCE_SINK_STATUS;
21532337 			tcpm_set_state(port, GET_STATUS_SEND, 0);
2154
-			break;
2155
-		default:
2156
-			tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2157
-			break;
21582338 		}
2339
+	} else {
2340
+		tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
2341
+	}
2342
+}
2343
+
2344
+static int tcpm_set_auto_vbus_discharge_threshold(struct tcpm_port *port,
2345
+						  enum typec_pwr_opmode mode, bool pps_active,
2346
+						  u32 requested_vbus_voltage)
2347
+{
2348
+	int ret;
2349
+
2350
+	if (!port->tcpc->set_auto_vbus_discharge_threshold)
2351
+		return 0;
2352
+
2353
+	ret = port->tcpc->set_auto_vbus_discharge_threshold(port->tcpc, mode, pps_active,
2354
+							    requested_vbus_voltage);
2355
+	tcpm_log_force(port,
2356
+		       "set_auto_vbus_discharge_threshold mode:%d pps_active:%c vbus:%u ret:%d",
2357
+		       mode, pps_active ? 'y' : 'n', requested_vbus_voltage, ret);
2358
+
2359
+	return ret;
2360
+}
2361
+
2362
+static void tcpm_pd_handle_state(struct tcpm_port *port,
2363
+				 enum tcpm_state state,
2364
+				 enum tcpm_ams ams,
2365
+				 unsigned int delay_ms)
2366
+{
2367
+	switch (port->state) {
2368
+	case SRC_READY:
2369
+	case SNK_READY:
2370
+		port->ams = ams;
2371
+		tcpm_set_state(port, state, delay_ms);
2372
+		break;
2373
+	/* 8.3.3.4.1.1 and 6.8.1 power transitioning */
2374
+	case SNK_TRANSITION_SINK:
2375
+	case SNK_TRANSITION_SINK_VBUS:
2376
+	case SRC_TRANSITION_SUPPLY:
2377
+		tcpm_set_state(port, HARD_RESET_SEND, 0);
2378
+		break;
2379
+	default:
2380
+		if (!tcpm_ams_interruptible(port)) {
2381
+			tcpm_set_state(port, port->pwr_role == TYPEC_SOURCE ?
2382
+				       SRC_SOFT_RESET_WAIT_SNK_TX :
2383
+				       SNK_SOFT_RESET,
2384
+				       0);
2385
+		} else {
2386
+			/* process the Message 6.8.1 */
2387
+			port->upcoming_state = state;
2388
+			port->next_ams = ams;
2389
+			tcpm_set_state(port, ready_state(port), delay_ms);
2390
+		}
2391
+		break;
2392
+	}
2393
+}
2394
+
2395
+static void tcpm_pd_handle_msg(struct tcpm_port *port,
2396
+			       enum pd_msg_request message,
2397
+			       enum tcpm_ams ams)
2398
+{
2399
+	switch (port->state) {
2400
+	case SRC_READY:
2401
+	case SNK_READY:
2402
+		port->ams = ams;
2403
+		tcpm_queue_message(port, message);
2404
+		break;
2405
+	/* PD 3.0 Spec 8.3.3.4.1.1 and 6.8.1 */
2406
+	case SNK_TRANSITION_SINK:
2407
+	case SNK_TRANSITION_SINK_VBUS:
2408
+	case SRC_TRANSITION_SUPPLY:
2409
+		tcpm_set_state(port, HARD_RESET_SEND, 0);
2410
+		break;
2411
+	default:
2412
+		if (!tcpm_ams_interruptible(port)) {
2413
+			tcpm_set_state(port, port->pwr_role == TYPEC_SOURCE ?
2414
+				       SRC_SOFT_RESET_WAIT_SNK_TX :
2415
+				       SNK_SOFT_RESET,
2416
+				       0);
2417
+		} else {
2418
+			port->next_ams = ams;
2419
+			tcpm_set_state(port, ready_state(port), 0);
2420
+			/* 6.8.1 process the Message */
2421
+			tcpm_queue_message(port, message);
2422
+		}
2423
+		break;
21592424 	}
21602425 }
21612426 
....@@ -2166,15 +2431,18 @@
21662431 	unsigned int cnt = pd_header_cnt_le(msg->header);
21672432 	unsigned int rev = pd_header_rev_le(msg->header);
21682433 	unsigned int i;
2169
-	enum frs_typec_current frs_current;
2434
+	enum frs_typec_current partner_frs_current;
21702435 	bool frs_enable;
21712436 	int ret;
21722437 
2438
+	if (tcpm_vdm_ams(port) && type != PD_DATA_VENDOR_DEF) {
2439
+		port->vdm_state = VDM_STATE_ERR_BUSY;
2440
+		tcpm_ams_finish(port);
2441
+		mod_vdm_delayed_work(port, 0);
2442
+	}
2443
+
21732444 	switch (type) {
21742445 	case PD_DATA_SOURCE_CAP:
2175
-		if (port->pwr_role != TYPEC_SINK)
2176
-			break;
2177
-
21782446 		for (i = 0; i < cnt; i++)
21792447 			port->source_caps[i] = le32_to_cpu(msg->payload[i]);
21802448 
....@@ -2185,17 +2453,34 @@
21852453 		tcpm_validate_caps(port, port->source_caps,
21862454 				   port->nr_source_caps);
21872455 
2456
+		trace_android_vh_typec_store_partner_src_caps(port, &port->nr_source_caps,
2457
+							      &port->source_caps);
2458
+
21882459 		/*
21892460 		 * Adjust revision in subsequent message headers, as required,
21902461 		 * to comply with 6.2.1.1.5 of the USB PD 3.0 spec. We don't
21912462 		 * support Rev 1.0 so just do nothing in that scenario.
21922463 		 */
2193
-		if (rev == PD_REV10)
2464
+		if (rev == PD_REV10) {
2465
+			if (port->ams == GET_SOURCE_CAPABILITIES)
2466
+				tcpm_ams_finish(port);
21942467 			break;
2468
+		}
21952469 
21962470 		if (rev < PD_MAX_REV)
2197
-			port->negotiated_rev = rev;
2471
+			port->negotiated_rev = min_t(u16, rev, port->negotiated_rev);
21982472 
2473
+		if (port->pwr_role == TYPEC_SOURCE) {
2474
+			if (port->ams == GET_SOURCE_CAPABILITIES)
2475
+				tcpm_pd_handle_state(port, SRC_READY, NONE_AMS, 0);
2476
+			/* Unexpected Source Capabilities */
2477
+			else
2478
+				tcpm_pd_handle_msg(port,
2479
+						   port->negotiated_rev < PD_REV30 ?
2480
+						   PD_MSG_CTRL_REJECT :
2481
+						   PD_MSG_CTRL_NOT_SUPP,
2482
+						   NONE_AMS);
2483
+		} else if (port->state == SNK_WAIT_CAPABILITIES) {
21992484 		/*
22002485 		 * This message may be received even if VBUS is not
22012486 		 * present. This is quite unexpected; see USB PD
....@@ -2209,48 +2494,68 @@
22092494 		 * but be prepared to keep waiting for VBUS after it was
22102495 		 * handled.
22112496 		 */
2212
-		tcpm_set_state(port, SNK_NEGOTIATE_CAPABILITIES, 0);
2497
+			port->ams = POWER_NEGOTIATION;
2498
+			port->in_ams = true;
2499
+			tcpm_set_state(port, SNK_NEGOTIATE_CAPABILITIES, 0);
2500
+		} else {
2501
+			if (port->ams == GET_SOURCE_CAPABILITIES)
2502
+				tcpm_ams_finish(port);
2503
+			tcpm_pd_handle_state(port, SNK_NEGOTIATE_CAPABILITIES,
2504
+					     POWER_NEGOTIATION, 0);
2505
+		}
22132506 		break;
22142507 	case PD_DATA_REQUEST:
2215
-		if (port->pwr_role != TYPEC_SOURCE ||
2216
-		    cnt != 1) {
2217
-			tcpm_queue_message(port, PD_MSG_CTRL_REJECT);
2218
-			break;
2219
-		}
2220
-
22212508 		/*
22222509 		 * Adjust revision in subsequent message headers, as required,
22232510 		 * to comply with 6.2.1.1.5 of the USB PD 3.0 spec. We don't
22242511 		 * support Rev 1.0 so just reject in that scenario.
22252512 		 */
22262513 		if (rev == PD_REV10) {
2227
-			tcpm_queue_message(port, PD_MSG_CTRL_REJECT);
2514
+			tcpm_pd_handle_msg(port,
2515
+					   port->negotiated_rev < PD_REV30 ?
2516
+					   PD_MSG_CTRL_REJECT :
2517
+					   PD_MSG_CTRL_NOT_SUPP,
2518
+					   NONE_AMS);
22282519 			break;
22292520 		}
22302521 
22312522 		if (rev < PD_MAX_REV)
2232
-			port->negotiated_rev = rev;
2523
+			port->negotiated_rev = min_t(u16, rev, port->negotiated_rev);
2524
+
2525
+		if (port->pwr_role != TYPEC_SOURCE || cnt != 1) {
2526
+			tcpm_pd_handle_msg(port,
2527
+					   port->negotiated_rev < PD_REV30 ?
2528
+					   PD_MSG_CTRL_REJECT :
2529
+					   PD_MSG_CTRL_NOT_SUPP,
2530
+					   NONE_AMS);
2531
+			break;
2532
+		}
22332533 
22342534 		port->sink_request = le32_to_cpu(msg->payload[0]);
22352535 
22362536 		if (port->vdm_sm_running && port->explicit_contract) {
2237
-			tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2537
+			tcpm_pd_handle_msg(port, PD_MSG_CTRL_WAIT, port->ams);
22382538 			break;
22392539 		}
22402540 
2241
-		tcpm_set_state(port, SRC_NEGOTIATE_CAPABILITIES, 0);
2541
+		if (port->state == SRC_SEND_CAPABILITIES)
2542
+			tcpm_set_state(port, SRC_NEGOTIATE_CAPABILITIES, 0);
2543
+		else
2544
+			tcpm_pd_handle_state(port, SRC_NEGOTIATE_CAPABILITIES,
2545
+					     POWER_NEGOTIATION, 0);
22422546 		break;
22432547 	case PD_DATA_SINK_CAP:
22442548 		/* We don't do anything with this at the moment... */
22452549 		for (i = 0; i < cnt; i++)
22462550 			port->sink_caps[i] = le32_to_cpu(msg->payload[i]);
22472551 
2248
-		frs_current = (port->sink_caps[0] & PDO_FIXED_FRS_CURR_MASK) >>
2552
+		partner_frs_current = (port->sink_caps[0] & PDO_FIXED_FRS_CURR_MASK) >>
22492553 			PDO_FIXED_FRS_CURR_SHIFT;
2250
-		frs_enable = frs_current && (frs_current <= port->frs_current);
2554
+		frs_enable = partner_frs_current && (partner_frs_current <=
2555
+						     port->new_source_frs_current);
22512556 		tcpm_log(port,
22522557 			 "Port partner FRS capable partner_frs_current:%u port_frs_current:%u enable:%c",
2253
-			 frs_current, port->frs_current, frs_enable ? 'y' : 'n');
2558
+			 partner_frs_current, port->new_source_frs_current, frs_enable ? 'y' : 'n');
22542559 		if (frs_enable) {
22552560 			ret  = port->tcpc->enable_frs(port->tcpc, true);
22562561 			tcpm_log(port, "Enable FRS %s, ret:%d\n", ret ? "fail" : "success", ret);
....@@ -2258,27 +2563,45 @@
22582563 
22592564 		port->nr_sink_caps = cnt;
22602565 		port->sink_cap_done = true;
2261
-		tcpm_set_state(port, SNK_READY, 0);
2566
+		if (port->ams == GET_SINK_CAPABILITIES)
2567
+			tcpm_set_state(port, ready_state(port), 0);
2568
+		/* Unexpected Sink Capabilities */
2569
+		else
2570
+			tcpm_pd_handle_msg(port,
2571
+					   port->negotiated_rev < PD_REV30 ?
2572
+					   PD_MSG_CTRL_REJECT :
2573
+					   PD_MSG_CTRL_NOT_SUPP,
2574
+					   NONE_AMS);
22622575 		break;
22632576 	case PD_DATA_VENDOR_DEF:
22642577 		tcpm_handle_vdm_request(port, msg->payload, cnt);
22652578 		break;
22662579 	case PD_DATA_BIST:
2267
-		if (port->state == SRC_READY || port->state == SNK_READY) {
2268
-			port->bist_request = le32_to_cpu(msg->payload[0]);
2269
-			tcpm_set_state(port, BIST_RX, 0);
2270
-		}
2580
+		port->bist_request = le32_to_cpu(msg->payload[0]);
2581
+		tcpm_pd_handle_state(port, BIST_RX, BIST, 0);
22712582 		break;
22722583 	case PD_DATA_ALERT:
2273
-		tcpm_handle_alert(port, msg->payload, cnt);
2584
+		if (port->state != SRC_READY && port->state != SNK_READY)
2585
+			tcpm_pd_handle_state(port, port->pwr_role == TYPEC_SOURCE ?
2586
+					     SRC_SOFT_RESET_WAIT_SNK_TX : SNK_SOFT_RESET,
2587
+					     NONE_AMS, 0);
2588
+		else
2589
+			tcpm_handle_alert(port, msg->payload, cnt);
22742590 		break;
22752591 	case PD_DATA_BATT_STATUS:
22762592 	case PD_DATA_GET_COUNTRY_INFO:
22772593 		/* Currently unsupported */
2278
-		tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
2594
+		tcpm_pd_handle_msg(port, port->negotiated_rev < PD_REV30 ?
2595
+				   PD_MSG_CTRL_REJECT :
2596
+				   PD_MSG_CTRL_NOT_SUPP,
2597
+				   NONE_AMS);
22792598 		break;
22802599 	default:
2281
-		tcpm_log(port, "Unhandled data message type %#x", type);
2600
+		tcpm_pd_handle_msg(port, port->negotiated_rev < PD_REV30 ?
2601
+				   PD_MSG_CTRL_REJECT :
2602
+				   PD_MSG_CTRL_NOT_SUPP,
2603
+				   NONE_AMS);
2604
+		tcpm_log(port, "Unrecognized data message type %#x", type);
22822605 		break;
22832606 	}
22842607 }
....@@ -2298,31 +2621,25 @@
22982621 	enum pd_ctrl_msg_type type = pd_header_type_le(msg->header);
22992622 	enum tcpm_state next_state;
23002623 
2624
+	/*
2625
+	 * Stop VDM state machine if interrupted by other Messages while NOT_SUPP is allowed in
2626
+	 * VDM AMS if waiting for VDM responses and will be handled later.
2627
+	 */
2628
+	if (tcpm_vdm_ams(port) && type != PD_CTRL_NOT_SUPP && type != PD_CTRL_GOOD_CRC) {
2629
+		port->vdm_state = VDM_STATE_ERR_BUSY;
2630
+		tcpm_ams_finish(port);
2631
+		mod_vdm_delayed_work(port, 0);
2632
+	}
2633
+
23012634 	switch (type) {
23022635 	case PD_CTRL_GOOD_CRC:
23032636 	case PD_CTRL_PING:
23042637 		break;
23052638 	case PD_CTRL_GET_SOURCE_CAP:
2306
-		switch (port->state) {
2307
-		case SRC_READY:
2308
-		case SNK_READY:
2309
-			tcpm_queue_message(port, PD_MSG_DATA_SOURCE_CAP);
2310
-			break;
2311
-		default:
2312
-			tcpm_queue_message(port, PD_MSG_CTRL_REJECT);
2313
-			break;
2314
-		}
2639
+		tcpm_pd_handle_msg(port, PD_MSG_DATA_SOURCE_CAP, GET_SOURCE_CAPABILITIES);
23152640 		break;
23162641 	case PD_CTRL_GET_SINK_CAP:
2317
-		switch (port->state) {
2318
-		case SRC_READY:
2319
-		case SNK_READY:
2320
-			tcpm_queue_message(port, PD_MSG_DATA_SINK_CAP);
2321
-			break;
2322
-		default:
2323
-			tcpm_queue_message(port, PD_MSG_CTRL_REJECT);
2324
-			break;
2325
-		}
2642
+		tcpm_pd_handle_msg(port, PD_MSG_DATA_SINK_CAP, GET_SINK_CAPABILITIES);
23262643 		break;
23272644 	case PD_CTRL_GOTO_MIN:
23282645 		break;
....@@ -2331,13 +2648,13 @@
23312648 		case SNK_TRANSITION_SINK:
23322649 			if (port->vbus_present) {
23332650 				tcpm_set_current_limit(port,
2334
-						       port->current_limit,
2335
-						       port->supply_voltage);
2651
+						       port->req_current_limit,
2652
+						       port->req_supply_voltage);
23362653 				port->explicit_contract = true;
2337
-				/* Set VDM running flag ASAP */
2338
-				if (port->data_role == TYPEC_HOST &&
2339
-				    port->send_discover)
2340
-					port->vdm_sm_running = true;
2654
+				tcpm_set_auto_vbus_discharge_threshold(port,
2655
+								       TYPEC_PWR_MODE_PD,
2656
+								       port->pps_data.active,
2657
+								       port->supply_voltage);
23412658 				tcpm_set_state(port, SNK_READY, 0);
23422659 			} else {
23432660 				/*
....@@ -2361,6 +2678,11 @@
23612678 			tcpm_set_state(port, FR_SWAP_SNK_SRC_NEW_SINK_READY, 0);
23622679 			break;
23632680 		default:
2681
+			tcpm_pd_handle_state(port,
2682
+					     port->pwr_role == TYPEC_SOURCE ?
2683
+					     SRC_SOFT_RESET_WAIT_SNK_TX :
2684
+					     SNK_SOFT_RESET,
2685
+					     NONE_AMS, 0);
23642686 			break;
23652687 		}
23662688 		break;
....@@ -2370,26 +2692,28 @@
23702692 		switch (port->state) {
23712693 		case SNK_NEGOTIATE_CAPABILITIES:
23722694 			/* USB PD specification, Figure 8-43 */
2373
-			if (port->explicit_contract) {
2695
+			if (port->explicit_contract)
23742696 				next_state = SNK_READY;
2375
-				if (port->data_role == TYPEC_HOST &&
2376
-				    port->send_discover)
2377
-					port->vdm_sm_running = true;
2378
-			} else {
2697
+			else
23792698 				next_state = SNK_WAIT_CAPABILITIES;
2380
-			}
2699
+
2700
+			/* Threshold was relaxed before sending Request. Restore it back. */
2701
+			tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
2702
+							       port->pps_data.active,
2703
+							       port->supply_voltage);
23812704 			tcpm_set_state(port, next_state, 0);
23822705 			break;
23832706 		case SNK_NEGOTIATE_PPS_CAPABILITIES:
23842707 			/* Revert data back from any requested PPS updates */
2385
-			port->pps_data.out_volt = port->supply_voltage;
2386
-			port->pps_data.op_curr = port->current_limit;
2708
+			port->pps_data.req_out_volt = port->supply_voltage;
2709
+			port->pps_data.req_op_curr = port->current_limit;
23872710 			port->pps_status = (type == PD_CTRL_WAIT ?
23882711 					    -EAGAIN : -EOPNOTSUPP);
23892712 
2390
-			if (port->data_role == TYPEC_HOST &&
2391
-			    port->send_discover)
2392
-				port->vdm_sm_running = true;
2713
+			/* Threshold was relaxed before sending Request. Restore it back. */
2714
+			tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
2715
+							       port->pps_data.active,
2716
+							       port->supply_voltage);
23932717 
23942718 			tcpm_set_state(port, SNK_READY, 0);
23952719 			break;
....@@ -2415,7 +2739,22 @@
24152739 			port->sink_cap_done = true;
24162740 			tcpm_set_state(port, ready_state(port), 0);
24172741 			break;
2742
+		case SRC_READY:
2743
+		case SNK_READY:
2744
+			if (port->vdm_state > VDM_STATE_READY) {
2745
+				port->vdm_state = VDM_STATE_DONE;
2746
+				if (tcpm_vdm_ams(port))
2747
+					tcpm_ams_finish(port);
2748
+				mod_vdm_delayed_work(port, 0);
2749
+				break;
2750
+			}
2751
+			fallthrough;
24182752 		default:
2753
+			tcpm_pd_handle_state(port,
2754
+					     port->pwr_role == TYPEC_SOURCE ?
2755
+					     SRC_SOFT_RESET_WAIT_SNK_TX :
2756
+					     SNK_SOFT_RESET,
2757
+					     NONE_AMS, 0);
24192758 			break;
24202759 		}
24212760 		break;
....@@ -2427,13 +2766,15 @@
24272766 			break;
24282767 		case SNK_NEGOTIATE_PPS_CAPABILITIES:
24292768 			port->pps_data.active = true;
2430
-			port->supply_voltage = port->pps_data.out_volt;
2431
-			port->current_limit = port->pps_data.op_curr;
2769
+			port->pps_data.min_volt = port->pps_data.req_min_volt;
2770
+			port->pps_data.max_volt = port->pps_data.req_max_volt;
2771
+			port->pps_data.max_curr = port->pps_data.req_max_curr;
2772
+			port->req_supply_voltage = port->pps_data.req_out_volt;
2773
+			port->req_current_limit = port->pps_data.req_op_curr;
2774
+			power_supply_changed(port->psy);
24322775 			tcpm_set_state(port, SNK_TRANSITION_SINK, 0);
24332776 			break;
24342777 		case SOFT_RESET_SEND:
2435
-			port->message_id = 0;
2436
-			port->rx_msgid = -1;
24372778 			if (port->ams == SOFT_RESET_AMS)
24382779 				tcpm_ams_finish(port);
24392780 			if (port->pwr_role == TYPEC_SOURCE) {
....@@ -2444,10 +2785,6 @@
24442785 			}
24452786 			break;
24462787 		case DR_SWAP_SEND:
2447
-			if (port->data_role == TYPEC_DEVICE &&
2448
-			    port->send_discover)
2449
-				port->vdm_sm_running = true;
2450
-
24512788 			tcpm_set_state(port, DR_SWAP_CHANGE_DR, 0);
24522789 			break;
24532790 		case PR_SWAP_SEND:
....@@ -2460,69 +2797,62 @@
24602797 			tcpm_set_state(port, FR_SWAP_SNK_SRC_TRANSITION_TO_OFF, 0);
24612798 			break;
24622799 		default:
2800
+			tcpm_pd_handle_state(port,
2801
+					     port->pwr_role == TYPEC_SOURCE ?
2802
+					     SRC_SOFT_RESET_WAIT_SNK_TX :
2803
+					     SNK_SOFT_RESET,
2804
+					     NONE_AMS, 0);
24632805 			break;
24642806 		}
24652807 		break;
24662808 	case PD_CTRL_SOFT_RESET:
2809
+		port->ams = SOFT_RESET_AMS;
24672810 		tcpm_set_state(port, SOFT_RESET, 0);
24682811 		break;
24692812 	case PD_CTRL_DR_SWAP:
2470
-		if (port->port_type != TYPEC_PORT_DRP) {
2471
-			tcpm_queue_message(port, PD_MSG_CTRL_REJECT);
2472
-			break;
2473
-		}
24742813 		/*
24752814 		 * XXX
24762815 		 * 6.3.9: If an alternate mode is active, a request to swap
24772816 		 * alternate modes shall trigger a port reset.
24782817 		 */
2479
-		switch (port->state) {
2480
-		case SRC_READY:
2481
-		case SNK_READY:
2482
-			if (port->vdm_sm_running) {
2818
+		if (port->typec_caps.data != TYPEC_PORT_DRD) {
2819
+			tcpm_pd_handle_msg(port,
2820
+					   port->negotiated_rev < PD_REV30 ?
2821
+					   PD_MSG_CTRL_REJECT :
2822
+					   PD_MSG_CTRL_NOT_SUPP,
2823
+					   NONE_AMS);
2824
+		} else {
2825
+			if (port->send_discover) {
24832826 				tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
24842827 				break;
24852828 			}
2486
-			tcpm_set_state(port, DR_SWAP_ACCEPT, 0);
2487
-			break;
2488
-		default:
2489
-			tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2490
-			break;
2829
+
2830
+			tcpm_pd_handle_state(port, DR_SWAP_ACCEPT, DATA_ROLE_SWAP, 0);
24912831 		}
24922832 		break;
24932833 	case PD_CTRL_PR_SWAP:
24942834 		if (port->port_type != TYPEC_PORT_DRP) {
2495
-			tcpm_queue_message(port, PD_MSG_CTRL_REJECT);
2496
-			break;
2497
-		}
2498
-		switch (port->state) {
2499
-		case SRC_READY:
2500
-		case SNK_READY:
2501
-			if (port->vdm_sm_running) {
2835
+			tcpm_pd_handle_msg(port,
2836
+					   port->negotiated_rev < PD_REV30 ?
2837
+					   PD_MSG_CTRL_REJECT :
2838
+					   PD_MSG_CTRL_NOT_SUPP,
2839
+					   NONE_AMS);
2840
+		} else {
2841
+			if (port->send_discover) {
25022842 				tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
25032843 				break;
25042844 			}
2505
-			tcpm_set_state(port, PR_SWAP_ACCEPT, 0);
2506
-			break;
2507
-		default:
2508
-			tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2509
-			break;
2845
+
2846
+			tcpm_pd_handle_state(port, PR_SWAP_ACCEPT, POWER_ROLE_SWAP, 0);
25102847 		}
25112848 		break;
25122849 	case PD_CTRL_VCONN_SWAP:
2513
-		switch (port->state) {
2514
-		case SRC_READY:
2515
-		case SNK_READY:
2516
-			if (port->vdm_sm_running) {
2517
-				tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2518
-				break;
2519
-			}
2520
-			tcpm_set_state(port, VCONN_SWAP_ACCEPT, 0);
2521
-			break;
2522
-		default:
2850
+		if (port->send_discover) {
25232851 			tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
25242852 			break;
25252853 		}
2854
+
2855
+		tcpm_pd_handle_state(port, VCONN_SWAP_ACCEPT, VCONN_SWAP, 0);
25262856 		break;
25272857 	case PD_CTRL_GET_SOURCE_CAP_EXT:
25282858 	case PD_CTRL_GET_STATUS:
....@@ -2530,10 +2860,19 @@
25302860 	case PD_CTRL_GET_PPS_STATUS:
25312861 	case PD_CTRL_GET_COUNTRY_CODES:
25322862 		/* Currently not supported */
2533
-		tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
2863
+		tcpm_pd_handle_msg(port,
2864
+				   port->negotiated_rev < PD_REV30 ?
2865
+				   PD_MSG_CTRL_REJECT :
2866
+				   PD_MSG_CTRL_NOT_SUPP,
2867
+				   NONE_AMS);
25342868 		break;
25352869 	default:
2536
-		tcpm_log(port, "Unhandled ctrl message type %#x", type);
2870
+		tcpm_pd_handle_msg(port,
2871
+				   port->negotiated_rev < PD_REV30 ?
2872
+				   PD_MSG_CTRL_REJECT :
2873
+				   PD_MSG_CTRL_NOT_SUPP,
2874
+				   NONE_AMS);
2875
+		tcpm_log(port, "Unrecognized ctrl message type %#x", type);
25372876 		break;
25382877 	}
25392878 }
....@@ -2544,34 +2883,37 @@
25442883 	enum pd_ext_msg_type type = pd_header_type_le(msg->header);
25452884 	unsigned int data_size = pd_ext_header_data_size_le(msg->ext_msg.header);
25462885 
2547
-	if (!(msg->ext_msg.header & PD_EXT_HDR_CHUNKED)) {
2886
+	/* stopping VDM state machine if interrupted by other Messages */
2887
+	if (tcpm_vdm_ams(port)) {
2888
+		port->vdm_state = VDM_STATE_ERR_BUSY;
2889
+		tcpm_ams_finish(port);
2890
+		mod_vdm_delayed_work(port, 0);
2891
+	}
2892
+
2893
+	if (!(le16_to_cpu(msg->ext_msg.header) & PD_EXT_HDR_CHUNKED)) {
2894
+		tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
25482895 		tcpm_log(port, "Unchunked extended messages unsupported");
25492896 		return;
25502897 	}
25512898 
25522899 	if (data_size > PD_EXT_MAX_CHUNK_DATA) {
2900
+		tcpm_pd_handle_state(port, CHUNK_NOT_SUPP, NONE_AMS, PD_T_CHUNK_NOT_SUPP);
25532901 		tcpm_log(port, "Chunk handling not yet supported");
25542902 		return;
25552903 	}
25562904 
25572905 	switch (type) {
25582906 	case PD_EXT_STATUS:
2559
-		/*
2560
-		 * If PPS related events raised then get PPS status to clear
2561
-		 * (see USB PD 3.0 Spec, 6.5.2.4)
2562
-		 */
2563
-		if (msg->ext_msg.data[USB_PD_EXT_SDB_EVENT_FLAGS] &
2564
-		    USB_PD_EXT_SDB_PPS_EVENTS)
2565
-			tcpm_set_state(port, GET_PPS_STATUS_SEND, 0);
2566
-		else
2567
-			tcpm_set_state(port, ready_state(port), 0);
2568
-		break;
25692907 	case PD_EXT_PPS_STATUS:
2570
-		/*
2571
-		 * For now the PPS status message is used to clear events
2572
-		 * and nothing more.
2573
-		 */
2574
-		tcpm_set_state(port, ready_state(port), 0);
2908
+		if (port->ams == GETTING_SOURCE_SINK_STATUS) {
2909
+			tcpm_ams_finish(port);
2910
+			tcpm_set_state(port, ready_state(port), 0);
2911
+		} else {
2912
+			/* unexpected Status or PPS_Status Message */
2913
+			tcpm_pd_handle_state(port, port->pwr_role == TYPEC_SOURCE ?
2914
+					     SRC_SOFT_RESET_WAIT_SNK_TX : SNK_SOFT_RESET,
2915
+					     NONE_AMS, 0);
2916
+		}
25752917 		break;
25762918 	case PD_EXT_SOURCE_CAP_EXT:
25772919 	case PD_EXT_GET_BATT_CAP:
....@@ -2585,10 +2927,11 @@
25852927 	case PD_EXT_FW_UPDATE_RESPONSE:
25862928 	case PD_EXT_COUNTRY_INFO:
25872929 	case PD_EXT_COUNTRY_CODES:
2588
-		tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
2930
+		tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
25892931 		break;
25902932 	default:
2591
-		tcpm_log(port, "Unhandled extended message type %#x", type);
2933
+		tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
2934
+		tcpm_log(port, "Unrecognized extended message type %#x", type);
25922935 		break;
25932936 	}
25942937 }
....@@ -2633,7 +2976,7 @@
26332976 				 "Data role mismatch, initiating error recovery");
26342977 			tcpm_set_state(port, ERROR_RECOVERY, 0);
26352978 		} else {
2636
-			if (msg->header & PD_HEADER_EXT_HDR)
2979
+			if (le16_to_cpu(msg->header) & PD_HEADER_EXT_HDR)
26372980 				tcpm_pd_ext_msg_request(port, msg);
26382981 			else if (cnt)
26392982 				tcpm_pd_data_request(port, msg);
....@@ -2684,6 +3027,7 @@
26843027 static bool tcpm_send_queued_message(struct tcpm_port *port)
26853028 {
26863029 	enum pd_msg_request queued_message;
3030
+	int ret;
26873031 
26883032 	do {
26893033 		queued_message = port->queued_message;
....@@ -2700,10 +3044,27 @@
27003044 			tcpm_pd_send_control(port, PD_CTRL_NOT_SUPP);
27013045 			break;
27023046 		case PD_MSG_DATA_SINK_CAP:
2703
-			tcpm_pd_send_sink_caps(port);
3047
+			ret = tcpm_pd_send_sink_caps(port);
3048
+			if (ret < 0) {
3049
+				tcpm_log(port, "Unable to send snk caps, ret=%d", ret);
3050
+				tcpm_set_state(port, SNK_SOFT_RESET, 0);
3051
+			}
3052
+			tcpm_ams_finish(port);
27043053 			break;
27053054 		case PD_MSG_DATA_SOURCE_CAP:
2706
-			tcpm_pd_send_source_caps(port);
3055
+			ret = tcpm_pd_send_source_caps(port);
3056
+			if (ret < 0) {
3057
+				tcpm_log(port,
3058
+					 "Unable to send src caps, ret=%d",
3059
+					 ret);
3060
+				tcpm_set_state(port, SOFT_RESET_SEND, 0);
3061
+			} else if (port->pwr_role == TYPEC_SOURCE) {
3062
+				tcpm_ams_finish(port);
3063
+				tcpm_set_state(port, HARD_RESET_SEND,
3064
+					       PD_T_SENDER_RESPONSE);
3065
+			} else {
3066
+				tcpm_ams_finish(port);
3067
+			}
27073068 			break;
27083069 		default:
27093070 			break;
....@@ -2791,6 +3152,7 @@
27913152 
27923153 	port->pps_data.supported = false;
27933154 	port->usb_type = POWER_SUPPLY_USB_TYPE_PD;
3155
+	power_supply_changed(port->psy);
27943156 
27953157 	/*
27963158 	 * Select the source PDO providing the most power which has a
....@@ -2815,6 +3177,7 @@
28153177 				port->pps_data.supported = true;
28163178 				port->usb_type =
28173179 					POWER_SUPPLY_USB_TYPE_PD_PPS;
3180
+				power_supply_changed(port->psy);
28183181 			}
28193182 			continue;
28203183 		default:
....@@ -2884,7 +3247,8 @@
28843247 	unsigned int i, j, max_mw = 0, max_mv = 0;
28853248 	unsigned int min_src_mv, max_src_mv, src_ma, src_mw;
28863249 	unsigned int min_snk_mv, max_snk_mv;
2887
-	u32 pdo;
3250
+	unsigned int max_op_mv;
3251
+	u32 pdo, src, snk;
28883252 	unsigned int src_pdo = 0, snk_pdo = 0;
28893253 
28903254 	/*
....@@ -2934,16 +3298,18 @@
29343298 					continue;
29353299 				}
29363300 
2937
-				if (max_src_mv <= max_snk_mv &&
2938
-				    min_src_mv >= min_snk_mv) {
3301
+				if (min_src_mv <= max_snk_mv &&
3302
+				    max_src_mv >= min_snk_mv) {
3303
+					max_op_mv = min(max_src_mv, max_snk_mv);
3304
+					src_mw = (max_op_mv * src_ma) / 1000;
29393305 					/* Prefer higher voltages if available */
29403306 					if ((src_mw == max_mw &&
2941
-					     min_src_mv > max_mv) ||
3307
+					     max_op_mv > max_mv) ||
29423308 					    src_mw > max_mw) {
29433309 						src_pdo = i;
29443310 						snk_pdo = j;
29453311 						max_mw = src_mw;
2946
-						max_mv = max_src_mv;
3312
+						max_mv = max_op_mv;
29473313 					}
29483314 				}
29493315 			}
....@@ -2956,16 +3322,19 @@
29563322 	}
29573323 
29583324 	if (src_pdo) {
2959
-		pdo = port->source_caps[src_pdo];
3325
+		src = port->source_caps[src_pdo];
3326
+		snk = port->snk_pdo[snk_pdo];
29603327 
2961
-		port->pps_data.min_volt = pdo_pps_apdo_min_voltage(pdo);
2962
-		port->pps_data.max_volt = pdo_pps_apdo_max_voltage(pdo);
2963
-		port->pps_data.max_curr =
2964
-			min_pps_apdo_current(pdo, port->snk_pdo[snk_pdo]);
2965
-		port->pps_data.out_volt =
2966
-			min(pdo_pps_apdo_max_voltage(pdo), port->pps_data.out_volt);
2967
-		port->pps_data.op_curr =
2968
-			min(port->pps_data.max_curr, port->pps_data.op_curr);
3328
+		port->pps_data.req_min_volt = max(pdo_pps_apdo_min_voltage(src),
3329
+						  pdo_pps_apdo_min_voltage(snk));
3330
+		port->pps_data.req_max_volt = min(pdo_pps_apdo_max_voltage(src),
3331
+						  pdo_pps_apdo_max_voltage(snk));
3332
+		port->pps_data.req_max_curr = min_pps_apdo_current(src, snk);
3333
+		port->pps_data.req_out_volt = min(port->pps_data.req_max_volt,
3334
+						  max(port->pps_data.req_min_volt,
3335
+						      port->pps_data.req_out_volt));
3336
+		port->pps_data.req_op_curr = min(port->pps_data.req_max_curr,
3337
+						 port->pps_data.req_op_curr);
29693338 	}
29703339 
29713340 	return src_pdo;
....@@ -3045,8 +3414,8 @@
30453414 			 flags & RDO_CAP_MISMATCH ? " [mismatch]" : "");
30463415 	}
30473416 
3048
-	port->current_limit = ma;
3049
-	port->supply_voltage = mv;
3417
+	port->req_current_limit = ma;
3418
+	port->req_supply_voltage = mv;
30503419 
30513420 	return 0;
30523421 }
....@@ -3060,6 +3429,12 @@
30603429 	ret = tcpm_pd_build_request(port, &rdo);
30613430 	if (ret < 0)
30623431 		return ret;
3432
+
3433
+	/*
3434
+	 * Relax the threshold as voltage will be adjusted after Accept Message plus tSrcTransition.
3435
+	 * It is safer to modify the threshold here.
3436
+	 */
3437
+	tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, 0);
30633438 
30643439 	memset(&msg, 0, sizeof(msg));
30653440 	msg.header = PD_HEADER_LE(PD_DATA_REQUEST,
....@@ -3092,10 +3467,10 @@
30923467 			tcpm_log(port, "Invalid APDO selected!");
30933468 			return -EINVAL;
30943469 		}
3095
-		max_mv = port->pps_data.max_volt;
3096
-		max_ma = port->pps_data.max_curr;
3097
-		out_mv = port->pps_data.out_volt;
3098
-		op_ma = port->pps_data.op_curr;
3470
+		max_mv = port->pps_data.req_max_volt;
3471
+		max_ma = port->pps_data.req_max_curr;
3472
+		out_mv = port->pps_data.req_out_volt;
3473
+		op_ma = port->pps_data.req_op_curr;
30993474 		break;
31003475 	default:
31013476 		tcpm_log(port, "Invalid PDO selected!");
....@@ -3142,8 +3517,8 @@
31423517 	tcpm_log(port, "Requesting APDO %d: %u mV, %u mA",
31433518 		 src_pdo_index, out_mv, op_ma);
31443519 
3145
-	port->pps_data.op_curr = op_ma;
3146
-	port->pps_data.out_volt = out_mv;
3520
+	port->pps_data.req_op_curr = op_ma;
3521
+	port->pps_data.req_out_volt = out_mv;
31473522 
31483523 	return 0;
31493524 }
....@@ -3157,6 +3532,9 @@
31573532 	ret = tcpm_pd_build_pps_request(port, &rdo);
31583533 	if (ret < 0)
31593534 		return ret;
3535
+
3536
+	/* Relax the threshold as voltage will be adjusted right after Accept Message. */
3537
+	tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, 0);
31603538 
31613539 	memset(&msg, 0, sizeof(msg));
31623540 	msg.header = PD_HEADER_LE(PD_DATA_REQUEST,
....@@ -3178,10 +3556,6 @@
31783556 
31793557 	tcpm_log(port, "vbus:=%d charge=%d", enable, port->vbus_charge);
31803558 
3181
-	ret = tcpm_send_vbus_notify(port, enable);
3182
-	if (ret)
3183
-		return ret;
3184
-
31853559 	ret = port->tcpc->set_vbus(port->tcpc, enable, port->vbus_charge);
31863560 	if (ret < 0)
31873561 		return ret;
....@@ -3199,17 +3573,13 @@
31993573 
32003574 	if (charge != port->vbus_charge) {
32013575 		tcpm_log(port, "vbus=%d charge:=%d", port->vbus_source, charge);
3202
-
3203
-		ret = tcpm_send_vbus_notify(port, port->vbus_source);
3204
-		if (ret < 0)
3205
-			return ret;
3206
-
32073576 		ret = port->tcpc->set_vbus(port->tcpc, port->vbus_source,
32083577 					   charge);
32093578 		if (ret < 0)
32103579 			return ret;
32113580 	}
32123581 	port->vbus_charge = charge;
3582
+	power_supply_changed(port->psy);
32133583 	return 0;
32143584 }
32153585 
....@@ -3228,10 +3598,6 @@
32283598 static int tcpm_init_vbus(struct tcpm_port *port)
32293599 {
32303600 	int ret;
3231
-
3232
-	ret = tcpm_send_vbus_notify(port, false);
3233
-	if (ret)
3234
-		return ret;
32353601 
32363602 	ret = port->tcpc->set_vbus(port->tcpc, false, false);
32373603 	port->vbus_source = false;
....@@ -3280,7 +3646,9 @@
32803646 	if (ret < 0)
32813647 		return ret;
32823648 
3283
-	ret = tcpm_set_roles(port, true, TYPEC_SOURCE, TYPEC_HOST);
3649
+	tcpm_enable_auto_vbus_discharge(port, true);
3650
+
3651
+	ret = tcpm_set_roles(port, true, TYPEC_SOURCE, tcpm_data_role_for_source(port));
32843652 	if (ret < 0)
32853653 		return ret;
32863654 
....@@ -3309,12 +3677,8 @@
33093677 	port->partner = NULL;
33103678 
33113679 	port->attached = true;
3680
+	port->debouncing = false;
33123681 	port->send_discover = true;
3313
-
3314
-	dev_info(port->dev, "CC connected in %s as DFP\n",
3315
-		 polarity ? "CC2" : "CC1");
3316
-	tcpm_send_orientation_notify(port);
3317
-	tcpm_send_data_role_notify(port, port->attached, port->data_role);
33183682 
33193683 	return 0;
33203684 
....@@ -3350,26 +3714,26 @@
33503714 	memset(modep, 0, sizeof(*modep));
33513715 }
33523716 
3717
+static void tcpm_set_partner_usb_comm_capable(struct tcpm_port *port, bool capable)
3718
+{
3719
+	tcpm_log(port, "Setting usb_comm capable %s", capable ? "true" : "false");
3720
+
3721
+	if (port->tcpc->set_partner_usb_comm_capable)
3722
+		port->tcpc->set_partner_usb_comm_capable(port->tcpc, capable);
3723
+}
3724
+
33533725 static void tcpm_reset_port(struct tcpm_port *port)
33543726 {
3727
+	tcpm_enable_auto_vbus_discharge(port, false);
33553728 	port->in_ams = false;
33563729 	port->ams = NONE_AMS;
33573730 	port->vdm_sm_running = false;
33583731 	tcpm_unregister_altmodes(port);
33593732 	tcpm_typec_disconnect(port);
3360
-	if (port->attached) {
3361
-		port->attached = false;
3362
-		if (port->dp_configured) {
3363
-			port->dp_configured = false;
3364
-			port->dp_pin_assignment = 0;
3365
-			port->dp_status = 0;
3366
-			tcpm_send_dp_notify(port);
3367
-		}
3368
-		tcpm_send_data_role_notify(port, port->attached,
3369
-					   port->data_role);
3370
-	}
3733
+	port->attached = false;
33713734 	port->pd_capable = false;
33723735 	port->pps_data.supported = false;
3736
+	tcpm_set_partner_usb_comm_capable(port, false);
33733737 
33743738 	/*
33753739 	 * First Rx ID should be 0; set this to a sentinel of -1 so that
....@@ -3388,12 +3752,11 @@
33883752 	port->try_src_count = 0;
33893753 	port->try_snk_count = 0;
33903754 	port->usb_type = POWER_SUPPLY_USB_TYPE_C;
3755
+	power_supply_changed(port->psy);
33913756 	port->nr_sink_caps = 0;
33923757 	port->sink_cap_done = false;
33933758 	if (port->tcpc->enable_frs)
33943759 		port->tcpc->enable_frs(port->tcpc, false);
3395
-
3396
-	power_supply_changed(port->psy);
33973760 }
33983761 
33993762 static void tcpm_detach(struct tcpm_port *port)
....@@ -3403,6 +3766,11 @@
34033766 
34043767 	if (!port->attached)
34053768 		return;
3769
+
3770
+	if (port->tcpc->set_bist_data) {
3771
+		tcpm_log(port, "disable BIST MODE TESTDATA");
3772
+		port->tcpc->set_bist_data(port->tcpc, false);
3773
+	}
34063774 
34073775 	tcpm_reset_port(port);
34083776 }
....@@ -3424,7 +3792,9 @@
34243792 	if (ret < 0)
34253793 		return ret;
34263794 
3427
-	ret = tcpm_set_roles(port, true, TYPEC_SINK, TYPEC_DEVICE);
3795
+	tcpm_enable_auto_vbus_discharge(port, true);
3796
+
3797
+	ret = tcpm_set_roles(port, true, TYPEC_SINK, tcpm_data_role_for_sink(port));
34283798 	if (ret < 0)
34293799 		return ret;
34303800 
....@@ -3433,12 +3803,8 @@
34333803 	port->partner = NULL;
34343804 
34353805 	port->attached = true;
3806
+	port->debouncing = false;
34363807 	port->send_discover = true;
3437
-
3438
-	dev_info(port->dev, "CC connected in %s as UFP\n",
3439
-		 port->cc1 != TYPEC_CC_OPEN ? "CC1" : "CC2");
3440
-	tcpm_send_orientation_notify(port);
3441
-	tcpm_send_data_role_notify(port, port->attached, port->data_role);
34423808 
34433809 	return 0;
34443810 }
....@@ -3455,7 +3821,8 @@
34553821 	if (port->attached)
34563822 		return 0;
34573823 
3458
-	ret = tcpm_set_roles(port, true, TYPEC_SOURCE, TYPEC_HOST);
3824
+	ret = tcpm_set_roles(port, true, TYPEC_SOURCE,
3825
+			     tcpm_data_role_for_source(port));
34593826 	if (ret < 0)
34603827 		return ret;
34613828 
....@@ -3464,10 +3831,7 @@
34643831 	tcpm_typec_connect(port);
34653832 
34663833 	port->attached = true;
3467
-
3468
-	dev_info(port->dev, "CC connected as Audio Accessory\n");
3469
-	tcpm_send_orientation_notify(port);
3470
-	tcpm_send_data_role_notify(port, port->attached, port->data_role);
3834
+	port->debouncing = false;
34713835 
34723836 	return 0;
34733837 }
....@@ -3504,14 +3868,15 @@
35043868 	return SNK_UNATTACHED;
35053869 }
35063870 
3507
-static void tcpm_check_send_discover(struct tcpm_port *port)
3871
+bool tcpm_is_toggling(struct tcpm_port *port)
35083872 {
3509
-	if (port->data_role == TYPEC_HOST && port->send_discover &&
3510
-	    port->pd_capable) {
3511
-		tcpm_send_vdm(port, USB_SID_PD, CMD_DISCOVER_IDENT, NULL, 0);
3512
-		port->send_discover = false;
3513
-	}
3873
+	if (port->port_type == TYPEC_PORT_DRP)
3874
+		return port->state == SRC_UNATTACHED || port->state == SNK_UNATTACHED ||
3875
+			port->state == TOGGLING;
3876
+
3877
+	return false;
35143878 }
3879
+EXPORT_SYMBOL_GPL(tcpm_is_toggling);
35153880 
35163881 static void tcpm_swap_complete(struct tcpm_port *port, int result)
35173882 {
....@@ -3540,8 +3905,11 @@
35403905 {
35413906 	int ret;
35423907 	enum typec_pwr_opmode opmode;
3543
-	unsigned int msecs;
3908
+	unsigned int msecs, timer_val_msecs;
35443909 	enum tcpm_state upcoming_state;
3910
+	const char *state_name;
3911
+	u32 current_limit;
3912
+	bool adjust;
35453913 
35463914 	port->enter_state = port->state;
35473915 	switch (port->state) {
....@@ -3552,6 +3920,15 @@
35523920 		if (!port->non_pd_role_swap)
35533921 			tcpm_swap_complete(port, -ENOTCONN);
35543922 		tcpm_src_detach(port);
3923
+		if (port->debouncing) {
3924
+			port->debouncing = false;
3925
+			if (port->tcpc->check_contaminant &&
3926
+			    port->tcpc->check_contaminant(port->tcpc)) {
3927
+				/* Contaminant detection would handle toggling */
3928
+				tcpm_set_state(port, TOGGLING, 0);
3929
+				break;
3930
+			}
3931
+		}
35553932 		if (tcpm_start_toggling(port, tcpm_rp_cc(port))) {
35563933 			tcpm_set_state(port, TOGGLING, 0);
35573934 			break;
....@@ -3561,20 +3938,25 @@
35613938 			tcpm_set_state(port, SNK_UNATTACHED, PD_T_DRP_SNK);
35623939 		break;
35633940 	case SRC_ATTACH_WAIT:
3941
+		port->debouncing = true;
3942
+		timer_val_msecs = PD_T_CC_DEBOUNCE;
3943
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[SRC_ATTACH_WAIT],
3944
+						      CC_DEBOUNCE, &timer_val_msecs);
35643945 		if (tcpm_port_is_debug(port))
35653946 			tcpm_set_state(port, DEBUG_ACC_ATTACHED,
3566
-				       PD_T_CC_DEBOUNCE);
3947
+				       timer_val_msecs);
35673948 		else if (tcpm_port_is_audio(port))
35683949 			tcpm_set_state(port, AUDIO_ACC_ATTACHED,
3569
-				       PD_T_CC_DEBOUNCE);
3570
-		else if (tcpm_port_is_source(port))
3950
+				       timer_val_msecs);
3951
+		else if (tcpm_port_is_source(port) && port->vbus_vsafe0v)
35713952 			tcpm_set_state(port,
35723953 				       tcpm_try_snk(port) ? SNK_TRY
35733954 							  : SRC_ATTACHED,
3574
-				       PD_T_CC_DEBOUNCE);
3955
+				       timer_val_msecs);
35753956 		break;
35763957 
35773958 	case SNK_TRY:
3959
+		port->debouncing = false;
35783960 		port->try_snk_count++;
35793961 		/*
35803962 		 * Requirements:
....@@ -3597,15 +3979,13 @@
35973979 		break;
35983980 	case SNK_TRY_WAIT_DEBOUNCE:
35993981 		tcpm_set_state(port, SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS,
3600
-			       PD_T_PD_DEBOUNCE);
3982
+			       PD_T_TRY_CC_DEBOUNCE);
36013983 		break;
36023984 	case SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS:
3603
-		if (port->vbus_present && tcpm_port_is_sink(port)) {
3985
+		if (port->vbus_present && tcpm_port_is_sink(port))
36043986 			tcpm_set_state(port, SNK_ATTACHED, 0);
3605
-		} else {
3606
-			tcpm_set_state(port, SRC_TRYWAIT, 0);
3987
+		else
36073988 			port->max_wait = 0;
3608
-		}
36093989 		break;
36103990 	case SRC_TRYWAIT:
36113991 		tcpm_set_cc(port, tcpm_rp_cc(port));
....@@ -3624,7 +4004,10 @@
36244004 		}
36254005 		break;
36264006 	case SRC_TRYWAIT_DEBOUNCE:
3627
-		tcpm_set_state(port, SRC_ATTACHED, PD_T_CC_DEBOUNCE);
4007
+		timer_val_msecs = PD_T_CC_DEBOUNCE;
4008
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[SRC_TRYWAIT_DEBOUNCE],
4009
+						      CC_DEBOUNCE, &timer_val_msecs);
4010
+		tcpm_set_state(port, SRC_ATTACHED, timer_val_msecs);
36284011 		break;
36294012 	case SRC_TRYWAIT_UNATTACHED:
36304013 		tcpm_set_state(port, SNK_UNATTACHED, 0);
....@@ -3640,7 +4023,7 @@
36404023 		typec_set_pwr_opmode(port->typec_port, opmode);
36414024 		port->pwr_opmode = TYPEC_PWR_MODE_USB;
36424025 		port->caps_count = 0;
3643
-		port->negotiated_rev = PD_MAX_REV;
4026
+		port->negotiated_rev = (((port->typec_caps.pd_revision >> 8) & 0xff) - 1);
36444027 		port->message_id = 0;
36454028 		port->rx_msgid = -1;
36464029 		port->explicit_contract = false;
....@@ -3710,6 +4093,8 @@
37104093 			}
37114094 		} else {
37124095 			tcpm_pd_send_control(port, PD_CTRL_ACCEPT);
4096
+			tcpm_set_partner_usb_comm_capable(port,
4097
+							  !!(port->sink_request & RDO_USB_COMM));
37134098 			tcpm_set_state(port, SRC_TRANSITION_SUPPLY,
37144099 				       PD_T_SRC_TRANSITION);
37154100 		}
....@@ -3733,6 +4118,11 @@
37334118 
37344119 		if (port->ams != NONE_AMS)
37354120 			tcpm_ams_finish(port);
4121
+		if (port->next_ams != NONE_AMS) {
4122
+			port->ams = port->next_ams;
4123
+			port->next_ams = NONE_AMS;
4124
+		}
4125
+
37364126 		/*
37374127 		 * If previous AMS is interrupted, switch to the upcoming
37384128 		 * state.
....@@ -3744,7 +4134,18 @@
37444134 			break;
37454135 		}
37464136 
3747
-		tcpm_check_send_discover(port);
4137
+		/*
4138
+		 * 6.4.4.3.1 Discover Identity
4139
+		 * "The Discover Identity Command Shall only be sent to SOP when there is an
4140
+		 * Explicit Contract."
4141
+		 * For now, this driver only supports SOP for DISCOVER_IDENTITY, thus using
4142
+		 * port->explicit_contract to decide whether to send the command.
4143
+		 */
4144
+		if (port->explicit_contract)
4145
+			mod_send_discover_delayed_work(port, 0);
4146
+		else
4147
+			port->send_discover = false;
4148
+
37484149 		/*
37494150 		 * 6.3.5
37504151 		 * Sending ping messages is not necessary if
....@@ -3769,6 +4170,15 @@
37694170 			tcpm_swap_complete(port, -ENOTCONN);
37704171 		tcpm_pps_complete(port, -ENOTCONN);
37714172 		tcpm_snk_detach(port);
4173
+		if (port->debouncing) {
4174
+			port->debouncing = false;
4175
+			if (port->tcpc->check_contaminant &&
4176
+			    port->tcpc->check_contaminant(port->tcpc)) {
4177
+				/* Contaminant detection would handle toggling */
4178
+				tcpm_set_state(port, TOGGLING, 0);
4179
+				break;
4180
+			}
4181
+		}
37724182 		if (tcpm_start_toggling(port, TYPEC_CC_RD)) {
37734183 			tcpm_set_state(port, TOGGLING, 0);
37744184 			break;
....@@ -3778,25 +4188,33 @@
37784188 			tcpm_set_state(port, SRC_UNATTACHED, PD_T_DRP_SRC);
37794189 		break;
37804190 	case SNK_ATTACH_WAIT:
4191
+		port->debouncing = true;
4192
+		timer_val_msecs = PD_T_CC_DEBOUNCE;
4193
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_ATTACH_WAIT],
4194
+						      CC_DEBOUNCE, &timer_val_msecs);
37814195 		if ((port->cc1 == TYPEC_CC_OPEN &&
37824196 		     port->cc2 != TYPEC_CC_OPEN) ||
37834197 		    (port->cc1 != TYPEC_CC_OPEN &&
37844198 		     port->cc2 == TYPEC_CC_OPEN))
37854199 			tcpm_set_state(port, SNK_DEBOUNCED,
3786
-				       PD_T_CC_DEBOUNCE);
4200
+				       timer_val_msecs);
37874201 		else if (tcpm_port_is_disconnected(port))
37884202 			tcpm_set_state(port, SNK_UNATTACHED,
3789
-				       PD_T_PD_DEBOUNCE);
4203
+				       timer_val_msecs);
37904204 		break;
37914205 	case SNK_DEBOUNCED:
3792
-		if (tcpm_port_is_disconnected(port))
4206
+		if (tcpm_port_is_disconnected(port)) {
37934207 			tcpm_set_state(port, SNK_UNATTACHED,
37944208 				       PD_T_PD_DEBOUNCE);
3795
-		else if (port->vbus_present)
4209
+		} else if (port->vbus_present) {
37964210 			tcpm_set_state(port,
37974211 				       tcpm_try_src(port) ? SRC_TRY
37984212 							  : SNK_ATTACHED,
37994213 				       0);
4214
+			port->debouncing = false;
4215
+		} else {
4216
+			port->debouncing = false;
4217
+		}
38004218 		break;
38014219 	case SRC_TRY:
38024220 		port->try_src_count++;
....@@ -3822,8 +4240,11 @@
38224240 		tcpm_set_state(port, SRC_ATTACHED, PD_T_PD_DEBOUNCE);
38234241 		break;
38244242 	case SNK_TRYWAIT:
4243
+		timer_val_msecs = PD_T_CC_DEBOUNCE;
4244
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_TRYWAIT],
4245
+						      CC_DEBOUNCE, &timer_val_msecs);
38254246 		tcpm_set_cc(port, TYPEC_CC_RD);
3826
-		tcpm_set_state(port, SNK_TRYWAIT_VBUS, PD_T_CC_DEBOUNCE);
4247
+		tcpm_set_state(port, SNK_TRYWAIT_VBUS, timer_val_msecs);
38274248 		break;
38284249 	case SNK_TRYWAIT_VBUS:
38294250 		/*
....@@ -3853,7 +4274,7 @@
38534274 					      port->cc2 : port->cc1);
38544275 		typec_set_pwr_opmode(port->typec_port, opmode);
38554276 		port->pwr_opmode = TYPEC_PWR_MODE_USB;
3856
-		port->negotiated_rev = PD_MAX_REV;
4277
+		port->negotiated_rev = (((port->typec_caps.pd_revision >> 8) & 0xff) - 1);
38574278 		port->message_id = 0;
38584279 		port->rx_msgid = -1;
38594280 		port->explicit_contract = false;
....@@ -3863,13 +4284,22 @@
38634284 			/* SRC -> SNK POWER/FAST_ROLE_SWAP finished */
38644285 			tcpm_ams_finish(port);
38654286 
3866
-		tcpm_set_state(port, SNK_DISCOVERY, 0);
4287
+		timer_val_msecs = 0;
4288
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_STARTUP],
4289
+						      SINK_DISCOVERY_BC12, &timer_val_msecs);
4290
+		tcpm_set_state(port, SNK_DISCOVERY, timer_val_msecs);
38674291 		break;
38684292 	case SNK_DISCOVERY:
38694293 		if (port->vbus_present) {
3870
-			tcpm_set_current_limit(port,
3871
-					       tcpm_get_current_limit(port),
3872
-					       5000);
4294
+			current_limit = tcpm_get_current_limit(port);
4295
+			trace_android_vh_typec_tcpm_adj_current_limit(tcpm_states[SNK_DISCOVERY],
4296
+								      port->current_limit,
4297
+								      port->supply_voltage,
4298
+								      port->pd_capable,
4299
+								      &current_limit, &adjust);
4300
+			if (port->slow_charger_loop && (current_limit > PD_P_SNK_STDBY_MW / 5))
4301
+				current_limit = PD_P_SNK_STDBY_MW / 5;
4302
+			tcpm_set_current_limit(port, current_limit, 5000);
38734303 			tcpm_set_charge(port, true);
38744304 			tcpm_set_state(port, SNK_WAIT_CAPABILITIES, 0);
38754305 			break;
....@@ -3884,8 +4314,10 @@
38844314 					PD_T_DB_DETECT : PD_T_NO_RESPONSE);
38854315 		break;
38864316 	case SNK_DISCOVERY_DEBOUNCE:
3887
-		tcpm_set_state(port, SNK_DISCOVERY_DEBOUNCE_DONE,
3888
-			       PD_T_CC_DEBOUNCE);
4317
+		timer_val_msecs = PD_T_CC_DEBOUNCE;
4318
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_DISCOVERY_DEBOUNCE],
4319
+						      CC_DEBOUNCE, &timer_val_msecs);
4320
+		tcpm_set_state(port, SNK_DISCOVERY_DEBOUNCE_DONE, timer_val_msecs);
38894321 		break;
38904322 	case SNK_DISCOVERY_DEBOUNCE_DONE:
38914323 		if (!tcpm_port_is_disconnected(port) &&
....@@ -3898,11 +4330,16 @@
38984330 		tcpm_set_state(port, unattached_state(port), 0);
38994331 		break;
39004332 	case SNK_WAIT_CAPABILITIES:
3901
-		ret = port->tcpc->set_pd_rx(port->tcpc, true);
3902
-		if (ret < 0) {
3903
-			tcpm_set_state(port, SNK_READY, 0);
3904
-			break;
4333
+		if (port->prev_state != SOFT_RESET_SEND) {
4334
+			ret = port->tcpc->set_pd_rx(port->tcpc, true);
4335
+			if (ret < 0) {
4336
+				tcpm_set_state(port, SNK_READY, 0);
4337
+				break;
4338
+			}
39054339 		}
4340
+		timer_val_msecs = PD_T_SINK_WAIT_CAP;
4341
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_WAIT_CAPABILITIES],
4342
+						      SINK_WAIT_CAP, &timer_val_msecs);
39064343 		/*
39074344 		 * If VBUS has never been low, and we time out waiting
39084345 		 * for source cap, try a soft reset first, in case we
....@@ -3912,17 +4349,23 @@
39124349 		if (port->vbus_never_low) {
39134350 			port->vbus_never_low = false;
39144351 			tcpm_set_state(port, SNK_SOFT_RESET,
3915
-				       PD_T_SINK_WAIT_CAP);
4352
+				       timer_val_msecs);
39164353 		} else {
39174354 			tcpm_set_state(port, hard_reset_state(port),
3918
-				       PD_T_SINK_WAIT_CAP);
4355
+				       timer_val_msecs);
39194356 		}
39204357 		break;
39214358 	case SNK_NEGOTIATE_CAPABILITIES:
39224359 		port->pd_capable = true;
4360
+		tcpm_set_partner_usb_comm_capable(port,
4361
+						  !!(port->source_caps[0] & PDO_FIXED_USB_COMM));
39234362 		port->hard_reset_count = 0;
39244363 		ret = tcpm_pd_send_request(port);
39254364 		if (ret < 0) {
4365
+			/* Restore back to the original state */
4366
+			tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
4367
+							       port->pps_data.active,
4368
+							       port->supply_voltage);
39264369 			/* Let the Source send capabilities again. */
39274370 			tcpm_set_state(port, SNK_WAIT_CAPABILITIES, 0);
39284371 		} else {
....@@ -3933,6 +4376,10 @@
39334376 	case SNK_NEGOTIATE_PPS_CAPABILITIES:
39344377 		ret = tcpm_pd_send_pps_request(port);
39354378 		if (ret < 0) {
4379
+			/* Restore back to the original state */
4380
+			tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
4381
+							       port->pps_data.active,
4382
+							       port->supply_voltage);
39364383 			port->pps_status = ret;
39374384 			/*
39384385 			 * If this was called due to updates to sink
....@@ -3949,6 +4396,23 @@
39494396 		}
39504397 		break;
39514398 	case SNK_TRANSITION_SINK:
4399
+		/* From the USB PD spec:
4400
+		 * "The Sink Shall transition to Sink Standby before a positive or
4401
+		 * negative voltage transition of VBUS. During Sink Standby
4402
+		 * the Sink Shall reduce its power draw to pSnkStdby."
4403
+		 *
4404
+		 * This is not applicable to PPS though as the port can continue
4405
+		 * to draw negotiated power without switching to standby.
4406
+		 */
4407
+		if (port->supply_voltage != port->req_supply_voltage && !port->pps_data.active &&
4408
+		    port->current_limit * port->supply_voltage / 1000 > PD_P_SNK_STDBY_MW) {
4409
+			u32 stdby_ma = PD_P_SNK_STDBY_MW * 1000 / port->supply_voltage;
4410
+
4411
+			tcpm_log(port, "Setting standby current %u mV @ %u mA",
4412
+				 port->supply_voltage, stdby_ma);
4413
+			tcpm_set_current_limit(port, stdby_ma, port->supply_voltage);
4414
+		}
4415
+		fallthrough;
39524416 	case SNK_TRANSITION_SINK_VBUS:
39534417 		tcpm_set_state(port, hard_reset_state(port),
39544418 			       PD_T_PS_TRANSITION);
....@@ -3962,6 +4426,19 @@
39624426 			port->pwr_opmode = TYPEC_PWR_MODE_PD;
39634427 		}
39644428 
4429
+		current_limit = tcpm_get_current_limit(port);
4430
+		adjust = false;
4431
+		trace_android_vh_typec_tcpm_adj_current_limit(tcpm_states[SNK_READY],
4432
+							      port->current_limit,
4433
+							      port->supply_voltage,
4434
+							      port->pd_capable,
4435
+							      &current_limit,
4436
+							      &adjust);
4437
+		if (adjust)
4438
+			tcpm_set_current_limit(port, current_limit, 5000);
4439
+
4440
+		if (!port->pd_capable && port->slow_charger_loop)
4441
+			tcpm_set_current_limit(port, tcpm_get_current_limit(port), 5000);
39654442 		tcpm_swap_complete(port, 0);
39664443 		tcpm_typec_connect(port);
39674444 		mod_enable_frs_delayed_work(port, 0);
....@@ -3969,6 +4446,11 @@
39694446 
39704447 		if (port->ams != NONE_AMS)
39714448 			tcpm_ams_finish(port);
4449
+		if (port->next_ams != NONE_AMS) {
4450
+			port->ams = port->next_ams;
4451
+			port->next_ams = NONE_AMS;
4452
+		}
4453
+
39724454 		/*
39734455 		 * If previous AMS is interrupted, switch to the upcoming
39744456 		 * state.
....@@ -3980,11 +4462,19 @@
39804462 			break;
39814463 		}
39824464 
3983
-		tcpm_check_send_discover(port);
4465
+		/*
4466
+		 * 6.4.4.3.1 Discover Identity
4467
+		 * "The Discover Identity Command Shall only be sent to SOP when there is an
4468
+		 * Explicit Contract."
4469
+		 * For now, this driver only supports SOP for DISCOVER_IDENTITY, thus using
4470
+		 * port->explicit_contract.
4471
+		 */
4472
+		if (port->explicit_contract)
4473
+			mod_send_discover_delayed_work(port, 0);
4474
+		else
4475
+			port->send_discover = false;
4476
+
39844477 		power_supply_changed(port->psy);
3985
-		if (port->usb_type == POWER_SUPPLY_USB_TYPE_PD ||
3986
-		    port->usb_type == POWER_SUPPLY_USB_TYPE_PD_PPS)
3987
-			tcpm_send_power_change_notify(port);
39884478 		break;
39894479 
39904480 	/* Accessory states */
....@@ -3999,7 +4489,10 @@
39994489 			tcpm_set_state(port, ACC_UNATTACHED, 0);
40004490 		break;
40014491 	case AUDIO_ACC_DEBOUNCE:
4002
-		tcpm_set_state(port, ACC_UNATTACHED, PD_T_CC_DEBOUNCE);
4492
+		timer_val_msecs = PD_T_CC_DEBOUNCE;
4493
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[AUDIO_ACC_DEBOUNCE],
4494
+						      CC_DEBOUNCE, &timer_val_msecs);
4495
+		tcpm_set_state(port, ACC_UNATTACHED, timer_val_msecs);
40034496 		break;
40044497 
40054498 	/* Hard_Reset states */
....@@ -4029,13 +4522,31 @@
40294522 			tcpm_set_state(port, SNK_HARD_RESET_SINK_OFF, 0);
40304523 		break;
40314524 	case SRC_HARD_RESET_VBUS_OFF:
4032
-		tcpm_set_vconn(port, true);
4525
+		/*
4526
+		 * 7.1.5 Response to Hard Resets
4527
+		 * Hard Reset Signaling indicates a communication failure has occurred and the
4528
+		 * Source Shall stop driving VCONN, Shall remove Rp from the VCONN pin and Shall
4529
+		 * drive VBUS to vSafe0V as shown in Figure 7-9.
4530
+		 */
4531
+		tcpm_set_vconn(port, false);
40334532 		tcpm_set_vbus(port, false);
40344533 		tcpm_set_roles(port, port->self_powered, TYPEC_SOURCE,
4035
-			       TYPEC_HOST);
4036
-		tcpm_set_state(port, SRC_HARD_RESET_VBUS_ON, PD_T_SRC_RECOVER);
4534
+			       tcpm_data_role_for_source(port));
4535
+		/*
4536
+		 * If tcpc fails to notify vbus off, TCPM will wait for PD_T_SAFE_0V +
4537
+		 * PD_T_SRC_RECOVER before turning vbus back on.
4538
+		 * From Table 7-12 Sequence Description for a Source Initiated Hard Reset:
4539
+		 * 4. Policy Engine waits tPSHardReset after sending Hard Reset Signaling and then
4540
+		 * tells the Device Policy Manager to instruct the power supply to perform a
4541
+		 * Hard Reset. The transition to vSafe0V Shall occur within tSafe0V (t2).
4542
+		 * 5. After tSrcRecover the Source applies power to VBUS in an attempt to
4543
+		 * re-establish communication with the Sink and resume USB Default Operation.
4544
+		 * The transition to vSafe5V Shall occur within tSrcTurnOn(t4).
4545
+		 */
4546
+		tcpm_set_state(port, SRC_HARD_RESET_VBUS_ON, PD_T_SAFE_0V + PD_T_SRC_RECOVER);
40374547 		break;
40384548 	case SRC_HARD_RESET_VBUS_ON:
4549
+		tcpm_set_vconn(port, true);
40394550 		tcpm_set_vbus(port, true);
40404551 		if (port->ams == HARD_RESET)
40414552 			tcpm_ams_finish(port);
....@@ -4044,12 +4555,14 @@
40444555 		tcpm_set_state(port, SRC_UNATTACHED, PD_T_PS_SOURCE_ON);
40454556 		break;
40464557 	case SNK_HARD_RESET_SINK_OFF:
4558
+		/* Do not discharge/disconnect during hard reseet */
4559
+		tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, 0);
40474560 		memset(&port->pps_data, 0, sizeof(port->pps_data));
40484561 		tcpm_set_vconn(port, false);
40494562 		if (port->pd_capable)
40504563 			tcpm_set_charge(port, false);
40514564 		tcpm_set_roles(port, port->self_powered, TYPEC_SINK,
4052
-			       TYPEC_DEVICE);
4565
+			       tcpm_data_role_for_sink(port));
40534566 		/*
40544567 		 * VBUS may or may not toggle, depending on the adapter.
40554568 		 * If it doesn't toggle, transition to SNK_HARD_RESET_SINK_ON
....@@ -4090,6 +4603,7 @@
40904603 		if (port->ams == HARD_RESET)
40914604 			tcpm_ams_finish(port);
40924605 		tcpm_set_attached_state(port, true);
4606
+		tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, VSAFE5V);
40934607 		tcpm_set_state(port, SNK_STARTUP, 0);
40944608 		break;
40954609 
....@@ -4098,6 +4612,7 @@
40984612 		port->message_id = 0;
40994613 		port->rx_msgid = -1;
41004614 		tcpm_pd_send_control(port, PD_CTRL_ACCEPT);
4615
+		tcpm_ams_finish(port);
41014616 		if (port->pwr_role == TYPEC_SOURCE) {
41024617 			port->upcoming_state = SRC_SEND_CAPABILITIES;
41034618 			tcpm_ams_start(port, POWER_NEGOTIATION);
....@@ -4115,6 +4630,7 @@
41154630 	case SOFT_RESET_SEND:
41164631 		port->message_id = 0;
41174632 		port->rx_msgid = -1;
4633
+		port->tcpc->set_pd_rx(port->tcpc, true);
41184634 		if (tcpm_pd_send_control(port, PD_CTRL_SOFT_RESET))
41194635 			tcpm_set_state_cond(port, hard_reset_state(port), 0);
41204636 		else
....@@ -4125,52 +4641,33 @@
41254641 	/* DR_Swap states */
41264642 	case DR_SWAP_SEND:
41274643 		tcpm_pd_send_control(port, PD_CTRL_DR_SWAP);
4644
+		if (port->data_role == TYPEC_DEVICE || port->negotiated_rev > PD_REV20)
4645
+			port->send_discover = true;
41284646 		tcpm_set_state_cond(port, DR_SWAP_SEND_TIMEOUT,
41294647 				    PD_T_SENDER_RESPONSE);
41304648 		break;
41314649 	case DR_SWAP_ACCEPT:
41324650 		tcpm_pd_send_control(port, PD_CTRL_ACCEPT);
4133
-		/* Set VDM state machine running flag ASAP */
4134
-		if (port->data_role == TYPEC_DEVICE && port->send_discover)
4135
-			port->vdm_sm_running = true;
4651
+		if (port->data_role == TYPEC_DEVICE || port->negotiated_rev > PD_REV20)
4652
+			port->send_discover = true;
41364653 		tcpm_set_state_cond(port, DR_SWAP_CHANGE_DR, 0);
41374654 		break;
41384655 	case DR_SWAP_SEND_TIMEOUT:
41394656 		tcpm_swap_complete(port, -ETIMEDOUT);
4657
+		port->send_discover = false;
4658
+		tcpm_ams_finish(port);
41404659 		tcpm_set_state(port, ready_state(port), 0);
41414660 		break;
41424661 	case DR_SWAP_CHANGE_DR:
4143
-		/*
4144
-		 * After the DR_swap process is executed, for the
4145
-		 * Rockchip platform, the tcpm framework needs to
4146
-		 * send two notifications to the dwc3 driver, the
4147
-		 * two notifications require a time interval.
4148
-		 *
4149
-		 * (a) For Type-C devices with DP function,
4150
-		 * a notification will be sent here, another
4151
-		 * notification can be sent after receiving
4152
-		 * Attention cmd.
4153
-		 * (b) But for Type-C devices without DP function,
4154
-		 * the tcpm framework will only send a notification once.
4155
-		 *
4156
-		 * Based on the above reasons, it is necessary to start
4157
-		 * the timer here, wait for 20 ms to start work and send
4158
-		 * the notification again.
4159
-		 */
4160
-
41614662 		if (port->data_role == TYPEC_HOST) {
41624663 			tcpm_unregister_altmodes(port);
41634664 			tcpm_set_roles(port, true, port->pwr_role,
41644665 				       TYPEC_DEVICE);
4165
-			tcpm_send_data_role_notify(port, false, TYPEC_HOST);
4166
-			mod_data_role_swap_work(port, SEND_NEW_MODE_NOTIFY_MS);
41674666 		} else {
41684667 			tcpm_set_roles(port, true, port->pwr_role,
41694668 				       TYPEC_HOST);
4170
-			port->send_discover = true;
4171
-			tcpm_send_data_role_notify(port, false, TYPEC_DEVICE);
4172
-			mod_data_role_swap_work(port, SEND_NEW_MODE_NOTIFY_MS);
41734669 		}
4670
+		tcpm_ams_finish(port);
41744671 		tcpm_set_state(port, ready_state(port), 0);
41754672 		break;
41764673 
....@@ -4185,7 +4682,10 @@
41854682 		tcpm_set_state(port, ERROR_RECOVERY, 0);
41864683 		break;
41874684 	case FR_SWAP_SNK_SRC_TRANSITION_TO_OFF:
4188
-		tcpm_set_state(port, ERROR_RECOVERY, PD_T_PS_SOURCE_OFF);
4685
+		timer_val_msecs = PD_T_PS_SOURCE_OFF;
4686
+		state_name = tcpm_states[FR_SWAP_SNK_SRC_TRANSITION_TO_OFF];
4687
+		trace_android_vh_typec_tcpm_get_timer(state_name, SOURCE_OFF, &timer_val_msecs);
4688
+		tcpm_set_state(port, ERROR_RECOVERY, timer_val_msecs);
41894689 		break;
41904690 	case FR_SWAP_SNK_SRC_NEW_SINK_READY:
41914691 		if (port->vbus_source)
....@@ -4218,6 +4718,7 @@
42184718 		tcpm_set_state(port, ready_state(port), 0);
42194719 		break;
42204720 	case PR_SWAP_START:
4721
+		tcpm_apply_rc(port);
42214722 		if (port->pwr_role == TYPEC_SOURCE)
42224723 			tcpm_set_state(port, PR_SWAP_SRC_SNK_TRANSITION_OFF,
42234724 				       PD_T_SRC_TRANSITION);
....@@ -4225,6 +4726,10 @@
42254726 			tcpm_set_state(port, PR_SWAP_SNK_SRC_SINK_OFF, 0);
42264727 		break;
42274728 	case PR_SWAP_SRC_SNK_TRANSITION_OFF:
4729
+		/*
4730
+		 * Prevent vbus discharge circuit from turning on during PR_SWAP
4731
+		 * as this is not a disconnect.
4732
+		 */
42284733 		tcpm_set_vbus(port, false);
42294734 		port->explicit_contract = false;
42304735 		/* allow time for Vbus discharge, must be < tSrcSwapStdby */
....@@ -4232,10 +4737,13 @@
42324737 			       PD_T_SRCSWAPSTDBY);
42334738 		break;
42344739 	case PR_SWAP_SRC_SNK_SOURCE_OFF:
4740
+		timer_val_msecs = PD_T_CC_DEBOUNCE;
4741
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[PR_SWAP_SRC_SNK_SOURCE_OFF],
4742
+						      CC_DEBOUNCE, &timer_val_msecs);
42354743 		tcpm_set_cc(port, TYPEC_CC_RD);
42364744 		/* allow CC debounce */
42374745 		tcpm_set_state(port, PR_SWAP_SRC_SNK_SOURCE_OFF_CC_DEBOUNCED,
4238
-			       PD_T_CC_DEBOUNCE);
4746
+			       timer_val_msecs);
42394747 		break;
42404748 	case PR_SWAP_SRC_SNK_SOURCE_OFF_CC_DEBOUNCED:
42414749 		/*
....@@ -4250,17 +4758,29 @@
42504758 			tcpm_set_state(port, ERROR_RECOVERY, 0);
42514759 			break;
42524760 		}
4253
-		tcpm_set_state_cond(port, SNK_UNATTACHED, PD_T_PS_SOURCE_ON);
4761
+		tcpm_set_state(port, ERROR_RECOVERY, PD_T_PS_SOURCE_ON_PRS);
42544762 		break;
42554763 	case PR_SWAP_SRC_SNK_SINK_ON:
4764
+		tcpm_enable_auto_vbus_discharge(port, true);
4765
+		/* Set the vbus disconnect threshold for implicit contract */
4766
+		tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, VSAFE5V);
42564767 		tcpm_set_state(port, SNK_STARTUP, 0);
42574768 		break;
42584769 	case PR_SWAP_SNK_SRC_SINK_OFF:
4770
+		timer_val_msecs = PD_T_PS_SOURCE_OFF;
4771
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[PR_SWAP_SNK_SRC_SINK_OFF],
4772
+						      SOURCE_OFF, &timer_val_msecs);
4773
+		/*
4774
+		 * Prevent vbus discharge circuit from turning on during PR_SWAP
4775
+		 * as this is not a disconnect.
4776
+		 */
4777
+		tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB,
4778
+						       port->pps_data.active, 0);
42594779 		tcpm_set_charge(port, false);
4260
-		tcpm_set_state(port, hard_reset_state(port),
4261
-			       PD_T_PS_SOURCE_OFF);
4780
+		tcpm_set_state(port, hard_reset_state(port), timer_val_msecs);
42624781 		break;
42634782 	case PR_SWAP_SNK_SRC_SOURCE_ON:
4783
+		tcpm_enable_auto_vbus_discharge(port, true);
42644784 		tcpm_set_cc(port, tcpm_rp_cc(port));
42654785 		tcpm_set_vbus(port, true);
42664786 		/*
....@@ -4286,6 +4806,7 @@
42864806 
42874807 	case VCONN_SWAP_ACCEPT:
42884808 		tcpm_pd_send_control(port, PD_CTRL_ACCEPT);
4809
+		tcpm_ams_finish(port);
42894810 		tcpm_set_state(port, VCONN_SWAP_START, 0);
42904811 		break;
42914812 	case VCONN_SWAP_SEND:
....@@ -4295,8 +4816,6 @@
42954816 		break;
42964817 	case VCONN_SWAP_SEND_TIMEOUT:
42974818 		tcpm_swap_complete(port, -ETIMEDOUT);
4298
-		if (port->data_role == TYPEC_HOST && port->send_discover)
4299
-			port->vdm_sm_running = true;
43004819 		tcpm_set_state(port, ready_state(port), 0);
43014820 		break;
43024821 	case VCONN_SWAP_START:
....@@ -4312,14 +4831,10 @@
43124831 	case VCONN_SWAP_TURN_ON_VCONN:
43134832 		tcpm_set_vconn(port, true);
43144833 		tcpm_pd_send_control(port, PD_CTRL_PS_RDY);
4315
-		if (port->data_role == TYPEC_HOST && port->send_discover)
4316
-			port->vdm_sm_running = true;
43174834 		tcpm_set_state(port, ready_state(port), 0);
43184835 		break;
43194836 	case VCONN_SWAP_TURN_OFF_VCONN:
43204837 		tcpm_set_vconn(port, false);
4321
-		if (port->data_role == TYPEC_HOST && port->send_discover)
4322
-			port->vdm_sm_running = true;
43234838 		tcpm_set_state(port, ready_state(port), 0);
43244839 		break;
43254840 
....@@ -4327,8 +4842,6 @@
43274842 	case PR_SWAP_CANCEL:
43284843 	case VCONN_SWAP_CANCEL:
43294844 		tcpm_swap_complete(port, port->swap_status);
4330
-		if (port->data_role == TYPEC_HOST && port->send_discover)
4331
-			port->vdm_sm_running = true;
43324845 		if (port->pwr_role == TYPEC_SOURCE)
43334846 			tcpm_set_state(port, SRC_READY, 0);
43344847 		else
....@@ -4345,12 +4858,18 @@
43454858 		switch (BDO_MODE_MASK(port->bist_request)) {
43464859 		case BDO_MODE_CARRIER2:
43474860 			tcpm_pd_transmit(port, TCPC_TX_BIST_MODE_2, NULL);
4861
+			tcpm_set_state(port, unattached_state(port),
4862
+				       PD_T_BIST_CONT_MODE);
4863
+			break;
4864
+		case BDO_MODE_TESTDATA:
4865
+			if (port->tcpc->set_bist_data) {
4866
+				tcpm_log(port, "Enable BIST MODE TESTDATA");
4867
+				port->tcpc->set_bist_data(port->tcpc, true);
4868
+			}
43484869 			break;
43494870 		default:
43504871 			break;
43514872 		}
4352
-		/* Always switch to unattached state */
4353
-		tcpm_set_state(port, unattached_state(port), 0);
43544873 		break;
43554874 	case GET_STATUS_SEND:
43564875 		tcpm_pd_send_control(port, PD_CTRL_GET_STATUS);
....@@ -4383,14 +4902,17 @@
43834902 		break;
43844903 	case PORT_RESET:
43854904 		tcpm_reset_port(port);
4386
-		tcpm_set_cc(port, TYPEC_CC_OPEN);
4905
+		tcpm_set_cc(port, TYPEC_CC_RD);
43874906 		tcpm_set_state(port, PORT_RESET_WAIT_OFF,
43884907 			       PD_T_ERROR_RECOVERY);
43894908 		break;
43904909 	case PORT_RESET_WAIT_OFF:
4910
+		timer_val_msecs = PD_T_PS_SOURCE_OFF;
4911
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[PORT_RESET_WAIT_OFF],
4912
+						      SOURCE_OFF, &timer_val_msecs);
43914913 		tcpm_set_state(port,
43924914 			       tcpm_default_state(port),
4393
-			       port->vbus_present ? PD_T_PS_SOURCE_OFF : 0);
4915
+			       port->vbus_present ? timer_val_msecs : 0);
43944916 		break;
43954917 
43964918 	/* AMS intermediate state */
....@@ -4404,6 +4926,12 @@
44044926 		upcoming_state = port->upcoming_state;
44054927 		port->upcoming_state = INVALID_STATE;
44064928 		tcpm_set_state(port, upcoming_state, 0);
4929
+		break;
4930
+
4931
+	/* Chunk state */
4932
+	case CHUNK_NOT_SUPP:
4933
+		tcpm_pd_send_control(port, PD_CTRL_NOT_SUPP);
4934
+		tcpm_set_state(port, port->pwr_role == TYPEC_SOURCE ? SRC_READY : SNK_READY, 0);
44074935 		break;
44084936 	default:
44094937 		WARN(1, "Unexpected port state %d\n", port->state);
....@@ -4488,11 +5016,16 @@
44885016 			tcpm_set_state(port, SRC_ATTACH_WAIT, 0);
44895017 		break;
44905018 	case SRC_ATTACHED:
5019
+	case SRC_STARTUP:
44915020 	case SRC_SEND_CAPABILITIES:
44925021 	case SRC_READY:
44935022 		if (tcpm_port_is_disconnected(port) ||
4494
-		    !tcpm_port_is_source(port))
4495
-			tcpm_set_state(port, SRC_UNATTACHED, 0);
5023
+		    !tcpm_port_is_source(port)) {
5024
+			if (port->port_type == TYPEC_PORT_SRC)
5025
+				tcpm_set_state(port, SRC_UNATTACHED, tcpm_wait_for_discharge(port));
5026
+			else
5027
+				tcpm_set_state(port, SNK_UNATTACHED, tcpm_wait_for_discharge(port));
5028
+		}
44965029 		break;
44975030 	case SNK_UNATTACHED:
44985031 		if (tcpm_port_is_sink(port))
....@@ -4522,7 +5055,23 @@
45225055 			tcpm_set_state(port, SNK_DEBOUNCED, 0);
45235056 		break;
45245057 	case SNK_READY:
4525
-		if (tcpm_port_is_disconnected(port))
5058
+		/*
5059
+		 * EXIT condition is based primarily on vbus disconnect and CC is secondary.
5060
+		 * "A port that has entered into USB PD communications with the Source and
5061
+		 * has seen the CC voltage exceed vRd-USB may monitor the CC pin to detect
5062
+		 * cable disconnect in addition to monitoring VBUS.
5063
+		 *
5064
+		 * A port that is monitoring the CC voltage for disconnect (but is not in
5065
+		 * the process of a USB PD PR_Swap or USB PD FR_Swap) shall transition to
5066
+		 * Unattached.SNK within tSinkDisconnect after the CC voltage remains below
5067
+		 * vRd-USB for tPDDebounce."
5068
+		 *
5069
+		 * When set_auto_vbus_discharge_threshold is enabled, CC pins go
5070
+		 * away before vbus decays to disconnect threshold. Allow
5071
+		 * disconnect to be driven by vbus disconnect when auto vbus
5072
+		 * discharge is enabled.
5073
+		 */
5074
+		if (!port->auto_vbus_discharge_enabled && tcpm_port_is_disconnected(port))
45265075 			tcpm_set_state(port, unattached_state(port), 0);
45275076 		else if (!port->pd_capable &&
45285077 			 (cc1 != old_cc1 || cc2 != old_cc2))
....@@ -4587,6 +5136,12 @@
45875136 		if (!tcpm_port_is_sink(port))
45885137 			tcpm_set_state(port, SNK_TRYWAIT_DEBOUNCE, 0);
45895138 		break;
5139
+	case SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS:
5140
+		if (!tcpm_port_is_sink(port))
5141
+			tcpm_set_state(port, SRC_TRYWAIT, PD_T_TRY_CC_DEBOUNCE);
5142
+		else
5143
+			tcpm_set_state(port, SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS, 0);
5144
+		break;
45905145 	case SNK_TRYWAIT:
45915146 		/* Do nothing, waiting for tCCDebounce */
45925147 		break;
....@@ -4615,9 +5170,13 @@
46155170 		 * Ignore CC changes here.
46165171 		 */
46175172 		break;
4618
-
46195173 	default:
4620
-		if (tcpm_port_is_disconnected(port))
5174
+		/*
5175
+		 * While acting as sink and auto vbus discharge is enabled, Allow disconnect
5176
+		 * to be driven by vbus disconnect.
5177
+		 */
5178
+		if (tcpm_port_is_disconnected(port) && !(port->pwr_role == TYPEC_SINK &&
5179
+							 port->auto_vbus_discharge_enabled))
46215180 			tcpm_set_state(port, unattached_state(port), 0);
46225181 		break;
46235182 	}
....@@ -4627,12 +5186,15 @@
46275186 {
46285187 	tcpm_log_force(port, "VBUS on");
46295188 	port->vbus_present = true;
5189
+	/*
5190
+	 * When vbus_present is true i.e. Voltage at VBUS is greater than VSAFE5V implicitly
5191
+	 * states that vbus is not at VSAFE0V, hence clear the vbus_vsafe0v flag here.
5192
+	 */
5193
+	port->vbus_vsafe0v = false;
5194
+
46305195 	switch (port->state) {
46315196 	case SNK_TRANSITION_SINK_VBUS:
46325197 		port->explicit_contract = true;
4633
-		/* Set the VDM flag ASAP */
4634
-		if (port->data_role == TYPEC_HOST && port->send_discover)
4635
-			port->vdm_sm_running = true;
46365198 		tcpm_set_state(port, SNK_READY, 0);
46375199 		break;
46385200 	case SNK_DISCOVERY:
....@@ -4676,11 +5238,24 @@
46765238 	case SNK_TRYWAIT_DEBOUNCE:
46775239 		/* Do nothing, waiting for Rp */
46785240 		break;
5241
+	case SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS:
5242
+		if (port->vbus_present && tcpm_port_is_sink(port))
5243
+			tcpm_set_state(port, SNK_ATTACHED, 0);
5244
+		break;
46795245 	case SRC_TRY_WAIT:
46805246 	case SRC_TRY_DEBOUNCE:
46815247 		/* Do nothing, waiting for sink detection */
46825248 		break;
5249
+	case FR_SWAP_SEND:
5250
+	case FR_SWAP_SEND_TIMEOUT:
5251
+	case FR_SWAP_SNK_SRC_TRANSITION_TO_OFF:
5252
+	case FR_SWAP_SNK_SRC_SOURCE_VBUS_APPLIED:
5253
+		if (port->tcpc->frs_sourcing_vbus)
5254
+			port->tcpc->frs_sourcing_vbus(port->tcpc);
5255
+		break;
46835256 	case FR_SWAP_SNK_SRC_NEW_SINK_READY:
5257
+		if (port->tcpc->frs_sourcing_vbus)
5258
+			port->tcpc->frs_sourcing_vbus(port->tcpc);
46845259 		tcpm_set_state(port, FR_SWAP_SNK_SRC_SOURCE_VBUS_APPLIED, 0);
46855260 		break;
46865261 
....@@ -4706,12 +5281,8 @@
47065281 	case SNK_HARD_RESET_SINK_OFF:
47075282 		tcpm_set_state(port, SNK_HARD_RESET_WAIT_VBUS, 0);
47085283 		break;
4709
-	case SRC_HARD_RESET_VBUS_OFF:
4710
-		tcpm_set_state(port, SRC_HARD_RESET_VBUS_ON, 0);
4711
-		break;
47125284 	case HARD_RESET_SEND:
47135285 		break;
4714
-
47155286 	case SNK_TRY:
47165287 		/* Do nothing, waiting for timeout */
47175288 		break;
....@@ -4729,6 +5300,7 @@
47295300 		break;
47305301 	case SNK_ATTACH_WAIT:
47315302 	case SNK_DEBOUNCED:
5303
+		port->debouncing = false;
47325304 		/* Do nothing, as TCPM is still waiting for vbus to reaach VSAFE5V to connect */
47335305 		break;
47345306 
....@@ -4743,6 +5315,14 @@
47435315 		/* Do nothing, expected */
47445316 		break;
47455317 
5318
+	case PR_SWAP_SNK_SRC_SOURCE_ON:
5319
+		/*
5320
+		 * Do nothing when vbus off notification is received.
5321
+		 * TCPM can wait for PD_T_NEWSRC in PR_SWAP_SNK_SRC_SOURCE_ON
5322
+		 * for the vbus source to ramp up.
5323
+		 */
5324
+		break;
5325
+
47465326 	case PORT_RESET_WAIT_OFF:
47475327 		tcpm_set_state(port, tcpm_default_state(port), 0);
47485328 		break;
....@@ -4750,6 +5330,25 @@
47505330 	case SRC_TRY_WAIT:
47515331 	case SRC_TRY_DEBOUNCE:
47525332 		/* Do nothing, waiting for sink detection */
5333
+		break;
5334
+
5335
+	case SRC_STARTUP:
5336
+	case SRC_SEND_CAPABILITIES:
5337
+	case SRC_SEND_CAPABILITIES_TIMEOUT:
5338
+	case SRC_NEGOTIATE_CAPABILITIES:
5339
+	case SRC_TRANSITION_SUPPLY:
5340
+	case SRC_READY:
5341
+	case SRC_WAIT_NEW_CAPABILITIES:
5342
+		/*
5343
+		 * Force to unattached state to re-initiate connection.
5344
+		 * DRP port should move to Unattached.SNK instead of Unattached.SRC if
5345
+		 * sink removed. Although sink removal here is due to source's vbus collapse,
5346
+		 * treat it the same way for consistency.
5347
+		 */
5348
+		if (port->port_type == TYPEC_PORT_SRC)
5349
+			tcpm_set_state(port, SRC_UNATTACHED, tcpm_wait_for_discharge(port));
5350
+		else
5351
+			tcpm_set_state(port, SNK_UNATTACHED, tcpm_wait_for_discharge(port));
47535352 		break;
47545353 
47555354 	case PORT_RESET:
....@@ -4768,8 +5367,58 @@
47685367 		break;
47695368 
47705369 	default:
4771
-		if (port->pwr_role == TYPEC_SINK &&
4772
-		    port->attached)
5370
+		if (port->pwr_role == TYPEC_SINK && port->attached)
5371
+			tcpm_set_state(port, SNK_UNATTACHED, tcpm_wait_for_discharge(port));
5372
+		break;
5373
+	}
5374
+}
5375
+
5376
+static void _tcpm_pd_vbus_vsafe0v(struct tcpm_port *port)
5377
+{
5378
+	unsigned int timer_val_msecs;
5379
+
5380
+	tcpm_log_force(port, "VBUS VSAFE0V");
5381
+	port->vbus_vsafe0v = true;
5382
+	switch (port->state) {
5383
+	case SRC_HARD_RESET_VBUS_OFF:
5384
+		/*
5385
+		 * After establishing the vSafe0V voltage condition on VBUS, the Source Shall wait
5386
+		 * tSrcRecover before re-applying VCONN and restoring VBUS to vSafe5V.
5387
+		 */
5388
+		tcpm_set_state(port, SRC_HARD_RESET_VBUS_ON, PD_T_SRC_RECOVER);
5389
+		break;
5390
+	case SRC_ATTACH_WAIT:
5391
+		timer_val_msecs = PD_T_CC_DEBOUNCE;
5392
+		trace_android_vh_typec_tcpm_get_timer(tcpm_states[SRC_ATTACH_WAIT],
5393
+						      CC_DEBOUNCE, &timer_val_msecs);
5394
+		if (tcpm_port_is_source(port))
5395
+			tcpm_set_state(port, tcpm_try_snk(port) ? SNK_TRY : SRC_ATTACHED,
5396
+				       timer_val_msecs);
5397
+		break;
5398
+	case SRC_STARTUP:
5399
+	case SRC_SEND_CAPABILITIES:
5400
+	case SRC_SEND_CAPABILITIES_TIMEOUT:
5401
+	case SRC_NEGOTIATE_CAPABILITIES:
5402
+	case SRC_TRANSITION_SUPPLY:
5403
+	case SRC_READY:
5404
+	case SRC_WAIT_NEW_CAPABILITIES:
5405
+		if (port->auto_vbus_discharge_enabled) {
5406
+			if (port->port_type == TYPEC_PORT_SRC)
5407
+				tcpm_set_state(port, SRC_UNATTACHED, 0);
5408
+			else
5409
+				tcpm_set_state(port, SNK_UNATTACHED, 0);
5410
+		}
5411
+		break;
5412
+	case PR_SWAP_SNK_SRC_SINK_OFF:
5413
+	case PR_SWAP_SNK_SRC_SOURCE_ON:
5414
+		/* Do nothing, vsafe0v is expected during transition */
5415
+		break;
5416
+	case SNK_ATTACH_WAIT:
5417
+	case SNK_DEBOUNCED:
5418
+		/*Do nothing, still waiting for VSAFE5V for connect */
5419
+		break;
5420
+	default:
5421
+		if (port->pwr_role == TYPEC_SINK && port->auto_vbus_discharge_enabled)
47735422 			tcpm_set_state(port, SNK_UNATTACHED, 0);
47745423 		break;
47755424 	}
....@@ -4778,9 +5427,13 @@
47785427 static void _tcpm_pd_hard_reset(struct tcpm_port *port)
47795428 {
47805429 	tcpm_log_force(port, "Received hard reset");
5430
+	if (port->bist_request == BDO_MODE_TESTDATA && port->tcpc->set_bist_data)
5431
+		port->tcpc->set_bist_data(port->tcpc, false);
47815432 
47825433 	if (port->ams != NONE_AMS)
47835434 		port->ams = NONE_AMS;
5435
+	if (port->hard_reset_count < PD_N_HARD_RESET_COUNT)
5436
+		port->ams = HARD_RESET;
47845437 	/*
47855438 	 * If we keep receiving hard reset requests, executing the hard reset
47865439 	 * must have failed. Revert to error recovery if that happens.
....@@ -4797,7 +5450,9 @@
47975450 					      event_work);
47985451 	u32 events;
47995452 
5453
+#ifdef CONFIG_NO_GKI
48005454 	mutex_lock(&port->pd_handler_lock);
5455
+#endif
48015456 	mutex_lock(&port->lock);
48025457 
48035458 	spin_lock(&port->pd_event_lock);
....@@ -4811,10 +5466,19 @@
48115466 			bool vbus;
48125467 
48135468 			vbus = port->tcpc->get_vbus(port->tcpc);
4814
-			if (vbus)
5469
+			if (vbus) {
48155470 				_tcpm_pd_vbus_on(port);
4816
-			else
5471
+			} else {
48175472 				_tcpm_pd_vbus_off(port);
5473
+				/*
5474
+				 * When TCPC does not support detecting vsafe0v voltage level,
5475
+				 * treat vbus absent as vsafe0v. Else invoke is_vbus_vsafe0v
5476
+				 * to see if vbus has discharge to VSAFE0V.
5477
+				 */
5478
+				if (!port->tcpc->is_vbus_vsafe0v ||
5479
+				    port->tcpc->is_vbus_vsafe0v(port->tcpc))
5480
+					_tcpm_pd_vbus_vsafe0v(port);
5481
+			}
48185482 		}
48195483 		if (events & TCPM_CC_EVENT) {
48205484 			enum typec_cc_status cc1, cc2;
....@@ -4851,7 +5515,9 @@
48515515 	}
48525516 	spin_unlock(&port->pd_event_lock);
48535517 	mutex_unlock(&port->lock);
5518
+#ifdef CONFIG_NO_GKI
48545519 	mutex_unlock(&port->pd_handler_lock);
5520
+#endif
48555521 }
48565522 
48575523 void tcpm_cc_change(struct tcpm_port *port)
....@@ -4931,27 +5597,41 @@
49315597 	mutex_unlock(&port->lock);
49325598 }
49335599 
4934
-static void tcpm_data_role_swap_work(struct kthread_work *work)
5600
+static void tcpm_send_discover_work(struct kthread_work *work)
49355601 {
4936
-	struct tcpm_port *port =
4937
-		container_of(work, struct tcpm_port, data_role_swap);
5602
+	struct tcpm_port *port = container_of(work, struct tcpm_port, send_discover_work);
49385603 
4939
-	if (tcpm_port_is_disconnected(port))
4940
-		return;
5604
+	mutex_lock(&port->lock);
5605
+	/* No need to send DISCOVER_IDENTITY anymore */
5606
+	if (!port->send_discover)
5607
+		goto unlock;
49415608 
4942
-	tcpm_send_data_role_notify(port, true, port->data_role);
5609
+	if (port->data_role == TYPEC_DEVICE && port->negotiated_rev < PD_REV30) {
5610
+		port->send_discover = false;
5611
+		goto unlock;
5612
+	}
5613
+
5614
+	/* Retry if the port is not idle */
5615
+	if ((port->state != SRC_READY && port->state != SNK_READY) || port->vdm_sm_running) {
5616
+		mod_send_discover_delayed_work(port, SEND_DISCOVER_RETRY_MS);
5617
+		goto unlock;
5618
+	}
5619
+
5620
+	tcpm_send_vdm(port, USB_SID_PD, CMD_DISCOVER_IDENT, NULL, 0);
5621
+
5622
+unlock:
5623
+	mutex_unlock(&port->lock);
49435624 }
49445625 
4945
-static int tcpm_dr_set(const struct typec_capability *cap,
4946
-		       enum typec_data_role data)
5626
+static int tcpm_dr_set(struct typec_port *p, enum typec_data_role data)
49475627 {
4948
-	struct tcpm_port *port = typec_cap_to_tcpm(cap);
5628
+	struct tcpm_port *port = typec_get_drvdata(p);
49495629 	int ret;
49505630 
49515631 	mutex_lock(&port->swap_lock);
49525632 	mutex_lock(&port->lock);
49535633 
4954
-	if (port->port_type != TYPEC_PORT_DRP) {
5634
+	if (port->typec_caps.data != TYPEC_PORT_DRD) {
49555635 		ret = -EINVAL;
49565636 		goto port_unlock;
49575637 	}
....@@ -5015,10 +5695,9 @@
50155695 	return ret;
50165696 }
50175697 
5018
-static int tcpm_pr_set(const struct typec_capability *cap,
5019
-		       enum typec_role role)
5698
+static int tcpm_pr_set(struct typec_port *p, enum typec_role role)
50205699 {
5021
-	struct tcpm_port *port = typec_cap_to_tcpm(cap);
5700
+	struct tcpm_port *port = typec_get_drvdata(p);
50225701 	int ret;
50235702 
50245703 	mutex_lock(&port->swap_lock);
....@@ -5065,10 +5744,9 @@
50655744 	return ret;
50665745 }
50675746 
5068
-static int tcpm_vconn_set(const struct typec_capability *cap,
5069
-			  enum typec_role role)
5747
+static int tcpm_vconn_set(struct typec_port *p, enum typec_role role)
50705748 {
5071
-	struct tcpm_port *port = typec_cap_to_tcpm(cap);
5749
+	struct tcpm_port *port = typec_get_drvdata(p);
50725750 	int ret;
50735751 
50745752 	mutex_lock(&port->swap_lock);
....@@ -5111,9 +5789,9 @@
51115789 	return ret;
51125790 }
51135791 
5114
-static int tcpm_try_role(const struct typec_capability *cap, int role)
5792
+static int tcpm_try_role(struct typec_port *p, int role)
51155793 {
5116
-	struct tcpm_port *port = typec_cap_to_tcpm(cap);
5794
+	struct tcpm_port *port = typec_get_drvdata(p);
51175795 	struct tcpc_dev	*tcpc = port->tcpc;
51185796 	int ret = 0;
51195797 
....@@ -5129,7 +5807,7 @@
51295807 	return ret;
51305808 }
51315809 
5132
-static int tcpm_pps_set_op_curr(struct tcpm_port *port, u16 op_curr)
5810
+static int tcpm_pps_set_op_curr(struct tcpm_port *port, u16 req_op_curr)
51335811 {
51345812 	unsigned int target_mw;
51355813 	int ret;
....@@ -5147,12 +5825,12 @@
51475825 		goto port_unlock;
51485826 	}
51495827 
5150
-	if (op_curr > port->pps_data.max_curr) {
5828
+	if (req_op_curr > port->pps_data.max_curr) {
51515829 		ret = -EINVAL;
51525830 		goto port_unlock;
51535831 	}
51545832 
5155
-	target_mw = (op_curr * port->pps_data.out_volt) / 1000;
5833
+	target_mw = (req_op_curr * port->supply_voltage) / 1000;
51565834 	if (target_mw < port->operating_snk_mw) {
51575835 		ret = -EINVAL;
51585836 		goto port_unlock;
....@@ -5166,10 +5844,10 @@
51665844 	}
51675845 
51685846 	/* Round down operating current to align with PPS valid steps */
5169
-	op_curr = op_curr - (op_curr % RDO_PROG_CURR_MA_STEP);
5847
+	req_op_curr = req_op_curr - (req_op_curr % RDO_PROG_CURR_MA_STEP);
51705848 
51715849 	reinit_completion(&port->pps_complete);
5172
-	port->pps_data.op_curr = op_curr;
5850
+	port->pps_data.req_op_curr = req_op_curr;
51735851 	port->pps_status = 0;
51745852 	port->pps_pending = true;
51755853 	mutex_unlock(&port->lock);
....@@ -5190,7 +5868,7 @@
51905868 	return ret;
51915869 }
51925870 
5193
-static int tcpm_pps_set_out_volt(struct tcpm_port *port, u16 out_volt)
5871
+static int tcpm_pps_set_out_volt(struct tcpm_port *port, u16 req_out_volt)
51945872 {
51955873 	unsigned int target_mw;
51965874 	int ret;
....@@ -5208,13 +5886,13 @@
52085886 		goto port_unlock;
52095887 	}
52105888 
5211
-	if (out_volt < port->pps_data.min_volt ||
5212
-	    out_volt > port->pps_data.max_volt) {
5889
+	if (req_out_volt < port->pps_data.min_volt ||
5890
+	    req_out_volt > port->pps_data.max_volt) {
52135891 		ret = -EINVAL;
52145892 		goto port_unlock;
52155893 	}
52165894 
5217
-	target_mw = (port->pps_data.op_curr * out_volt) / 1000;
5895
+	target_mw = (port->current_limit * req_out_volt) / 1000;
52185896 	if (target_mw < port->operating_snk_mw) {
52195897 		ret = -EINVAL;
52205898 		goto port_unlock;
....@@ -5228,10 +5906,10 @@
52285906 	}
52295907 
52305908 	/* Round down output voltage to align with PPS valid steps */
5231
-	out_volt = out_volt - (out_volt % RDO_PROG_VOLT_MV_STEP);
5909
+	req_out_volt = req_out_volt - (req_out_volt % RDO_PROG_VOLT_MV_STEP);
52325910 
52335911 	reinit_completion(&port->pps_complete);
5234
-	port->pps_data.out_volt = out_volt;
5912
+	port->pps_data.req_out_volt = req_out_volt;
52355913 	port->pps_status = 0;
52365914 	port->pps_pending = true;
52375915 	mutex_unlock(&port->lock);
....@@ -5289,8 +5967,8 @@
52895967 
52905968 	/* Trigger PPS request or move back to standard PDO contract */
52915969 	if (activate) {
5292
-		port->pps_data.out_volt = port->supply_voltage;
5293
-		port->pps_data.op_curr = port->current_limit;
5970
+		port->pps_data.req_out_volt = port->supply_voltage;
5971
+		port->pps_data.req_op_curr = port->current_limit;
52945972 	}
52955973 	mutex_unlock(&port->lock);
52965974 
....@@ -5327,6 +6005,24 @@
53276005 	if (port->vbus_present)
53286006 		port->vbus_never_low = true;
53296007 
6008
+	/*
6009
+	 * 1. When vbus_present is true, voltage on VBUS is already at VSAFE5V.
6010
+	 * So implicitly vbus_vsafe0v = false.
6011
+	 *
6012
+	 * 2. When vbus_present is false and TCPC does NOT support querying
6013
+	 * vsafe0v status, then, it's best to assume vbus is at VSAFE0V i.e.
6014
+	 * vbus_vsafe0v is true.
6015
+	 *
6016
+	 * 3. When vbus_present is false and TCPC does support querying vsafe0v,
6017
+	 * then, query tcpc for vsafe0v status.
6018
+	 */
6019
+	if (port->vbus_present)
6020
+		port->vbus_vsafe0v = false;
6021
+	else if (!port->tcpc->is_vbus_vsafe0v)
6022
+		port->vbus_vsafe0v = true;
6023
+	else
6024
+		port->vbus_vsafe0v = port->tcpc->is_vbus_vsafe0v(port->tcpc);
6025
+
53306026 	tcpm_set_state(port, tcpm_default_state(port), 0);
53316027 
53326028 	if (port->tcpc->get_cc(port->tcpc, &cc1, &cc2) == 0)
....@@ -5339,10 +6035,9 @@
53396035 	tcpm_set_state(port, PORT_RESET, 0);
53406036 }
53416037 
5342
-static int tcpm_port_type_set(const struct typec_capability *cap,
5343
-			      enum typec_port_type type)
6038
+static int tcpm_port_type_set(struct typec_port *p, enum typec_port_type type)
53446039 {
5345
-	struct tcpm_port *port = typec_cap_to_tcpm(cap);
6040
+	struct tcpm_port *port = typec_get_drvdata(p);
53466041 
53476042 	mutex_lock(&port->lock);
53486043 	if (type == port->port_type)
....@@ -5367,6 +6062,14 @@
53676062 	return 0;
53686063 }
53696064 
6065
+static const struct typec_operations tcpm_ops = {
6066
+	.try_role = tcpm_try_role,
6067
+	.dr_set = tcpm_dr_set,
6068
+	.pr_set = tcpm_pr_set,
6069
+	.vconn_set = tcpm_vconn_set,
6070
+	.port_type_set = tcpm_port_type_set
6071
+};
6072
+
53706073 void tcpm_tcpc_reset(struct tcpm_port *port)
53716074 {
53726075 	mutex_lock(&port->lock);
....@@ -5376,63 +6079,48 @@
53766079 }
53776080 EXPORT_SYMBOL_GPL(tcpm_tcpc_reset);
53786081 
5379
-static int tcpm_copy_pdos(u32 *dest_pdo, const u32 *src_pdo,
5380
-			  unsigned int nr_pdo)
5381
-{
5382
-	unsigned int i;
5383
-
5384
-	if (nr_pdo > PDO_MAX_OBJECTS)
5385
-		nr_pdo = PDO_MAX_OBJECTS;
5386
-
5387
-	for (i = 0; i < nr_pdo; i++)
5388
-		dest_pdo[i] = src_pdo[i];
5389
-
5390
-	return nr_pdo;
5391
-}
5392
-
53936082 static int tcpm_fw_get_caps(struct tcpm_port *port,
53946083 			    struct fwnode_handle *fwnode)
53956084 {
53966085 	const char *cap_str;
53976086 	int ret;
5398
-	u32 mw, frs_current;
6087
+	u32 mw, frs_current, pd_revision;
53996088 
54006089 	if (!fwnode)
54016090 		return -EINVAL;
54026091 
5403
-	/* get vbus regulator */
5404
-	port->vbus = devm_regulator_get_optional(port->dev, "vbus");
5405
-	if (IS_ERR(port->vbus)) {
5406
-		if (PTR_ERR(port->vbus) == -EPROBE_DEFER)
5407
-			return PTR_ERR(port->vbus);
5408
-
5409
-		dev_info(port->dev, "may no need vbus regulator\n");
5410
-		port->vbus = NULL;
5411
-	}
6092
+	ret = fwnode_property_read_u32(fwnode, "pd-revision",
6093
+				       &pd_revision);
6094
+	if (ret < 0)
6095
+		port->typec_caps.pd_revision = 0x0300;
6096
+	else
6097
+		port->typec_caps.pd_revision = pd_revision & 0xffff;
54126098 
54136099 	/* USB data support is optional */
54146100 	ret = fwnode_property_read_string(fwnode, "data-role", &cap_str);
54156101 	if (ret == 0) {
5416
-		port->typec_caps.data = typec_find_port_data_role(cap_str);
5417
-		if (port->typec_caps.data < 0)
5418
-			return -EINVAL;
6102
+		ret = typec_find_port_data_role(cap_str);
6103
+		if (ret < 0)
6104
+			return ret;
6105
+		port->typec_caps.data = ret;
54196106 	}
54206107 
54216108 	ret = fwnode_property_read_string(fwnode, "power-role", &cap_str);
54226109 	if (ret < 0)
54236110 		return ret;
54246111 
5425
-	port->typec_caps.type = typec_find_port_power_role(cap_str);
5426
-	if (port->typec_caps.type < 0)
5427
-		return -EINVAL;
6112
+	ret = typec_find_port_power_role(cap_str);
6113
+	if (ret < 0)
6114
+		return ret;
6115
+	port->typec_caps.type = ret;
54286116 	port->port_type = port->typec_caps.type;
54296117 
6118
+	port->slow_charger_loop = fwnode_property_read_bool(fwnode, "slow-charger-loop");
54306119 	if (port->port_type == TYPEC_PORT_SNK)
54316120 		goto sink;
54326121 
54336122 	/* Get source pdos */
5434
-	ret = fwnode_property_read_u32_array(fwnode, "source-pdos",
5435
-					     NULL, 0);
6123
+	ret = fwnode_property_count_u32(fwnode, "source-pdos");
54366124 	if (ret <= 0)
54376125 		return -EINVAL;
54386126 
....@@ -5456,8 +6144,7 @@
54566144 		return -EINVAL;
54576145 sink:
54586146 	/* Get sink pdos */
5459
-	ret = fwnode_property_read_u32_array(fwnode, "sink-pdos",
5460
-					     NULL, 0);
6147
+	ret = fwnode_property_count_u32(fwnode, "sink-pdos");
54616148 	if (ret <= 0)
54626149 		return -EINVAL;
54636150 
....@@ -5476,9 +6163,10 @@
54766163 
54776164 	/* FRS can only be supported byb DRP ports */
54786165 	if (port->port_type == TYPEC_PORT_DRP) {
5479
-		ret = fwnode_property_read_u32(fwnode, "frs-typec-current", &frs_current);
6166
+		ret = fwnode_property_read_u32(fwnode, "new-source-frs-typec-current",
6167
+					       &frs_current);
54806168 		if (ret >= 0 && frs_current <= FRS_5V_3A)
5481
-			port->frs_current = frs_current;
6169
+			port->new_source_frs_current = frs_current;
54826170 	}
54836171 
54846172 	/* sink-vdos is optional */
....@@ -5495,49 +6183,49 @@
54956183 			return ret;
54966184 	}
54976185 
5498
-	return 0;
5499
-}
6186
+	/* If sink-vdos is found, sink-vdos-v1 is expected for backward compatibility. */
6187
+	if (port->nr_snk_vdo) {
6188
+		ret = fwnode_property_count_u32(fwnode, "sink-vdos-v1");
6189
+		if (ret < 0)
6190
+			return ret;
6191
+		else if (ret == 0)
6192
+			return -ENODATA;
55006193 
5501
-int tcpm_update_source_capabilities(struct tcpm_port *port, const u32 *pdo,
5502
-				    unsigned int nr_pdo)
5503
-{
5504
-	if (tcpm_validate_caps(port, pdo, nr_pdo))
5505
-		return -EINVAL;
5506
-
5507
-	mutex_lock(&port->lock);
5508
-	port->nr_src_pdo = tcpm_copy_pdos(port->src_pdo, pdo, nr_pdo);
5509
-	switch (port->state) {
5510
-	case SRC_UNATTACHED:
5511
-	case SRC_ATTACH_WAIT:
5512
-	case SRC_TRYWAIT:
5513
-		tcpm_set_cc(port, tcpm_rp_cc(port));
5514
-		break;
5515
-	case SRC_SEND_CAPABILITIES:
5516
-	case SRC_NEGOTIATE_CAPABILITIES:
5517
-	case SRC_READY:
5518
-	case SRC_WAIT_NEW_CAPABILITIES:
5519
-		tcpm_set_cc(port, tcpm_rp_cc(port));
5520
-		tcpm_set_state(port, SRC_SEND_CAPABILITIES, 0);
5521
-		break;
5522
-	default:
5523
-		break;
6194
+		port->nr_snk_vdo_v1 = min(ret, VDO_MAX_OBJECTS);
6195
+		ret = fwnode_property_read_u32_array(fwnode, "sink-vdos-v1",
6196
+						     port->snk_vdo_v1,
6197
+						     port->nr_snk_vdo_v1);
6198
+		if (ret < 0)
6199
+			return ret;
55246200 	}
5525
-	mutex_unlock(&port->lock);
6201
+
55266202 	return 0;
55276203 }
5528
-EXPORT_SYMBOL_GPL(tcpm_update_source_capabilities);
55296204 
5530
-int tcpm_update_sink_capabilities(struct tcpm_port *port, const u32 *pdo,
5531
-				  unsigned int nr_pdo,
6205
+static int tcpm_copy_pdos(u32 *dest_pdo, const u32 *src_pdo, unsigned int nr_pdo)
6206
+{
6207
+	unsigned int i;
6208
+
6209
+	if (nr_pdo > PDO_MAX_OBJECTS)
6210
+		nr_pdo = PDO_MAX_OBJECTS;
6211
+
6212
+	for (i = 0; i < nr_pdo; i++)
6213
+		dest_pdo[i] = src_pdo[i];
6214
+
6215
+	return nr_pdo;
6216
+}
6217
+
6218
+int tcpm_update_sink_capabilities(struct tcpm_port *port, const u32 *pdo, unsigned int nr_pdo,
55326219 				  unsigned int operating_snk_mw)
55336220 {
6221
+	int ret = 0;
6222
+
55346223 	if (tcpm_validate_caps(port, pdo, nr_pdo))
55356224 		return -EINVAL;
55366225 
55376226 	mutex_lock(&port->lock);
55386227 	port->nr_snk_pdo = tcpm_copy_pdos(port->snk_pdo, pdo, nr_pdo);
55396228 	port->operating_snk_mw = operating_snk_mw;
5540
-	port->update_sink_caps = true;
55416229 
55426230 	switch (port->state) {
55436231 	case SNK_NEGOTIATE_CAPABILITIES:
....@@ -5546,15 +6234,25 @@
55466234 	case SNK_TRANSITION_SINK:
55476235 	case SNK_TRANSITION_SINK_VBUS:
55486236 		if (port->pps_data.active)
5549
-			tcpm_set_state(port, SNK_NEGOTIATE_PPS_CAPABILITIES, 0);
6237
+			port->upcoming_state = SNK_NEGOTIATE_PPS_CAPABILITIES;
6238
+		else if (port->pd_capable)
6239
+			port->upcoming_state = SNK_NEGOTIATE_CAPABILITIES;
55506240 		else
5551
-			tcpm_set_state(port, SNK_NEGOTIATE_CAPABILITIES, 0);
6241
+			break;
6242
+
6243
+		port->update_sink_caps = true;
6244
+
6245
+		ret = tcpm_ams_start(port, POWER_NEGOTIATION);
6246
+		if (ret == -EAGAIN) {
6247
+			port->upcoming_state = INVALID_STATE;
6248
+			break;
6249
+		}
55526250 		break;
55536251 	default:
55546252 		break;
55556253 	}
55566254 	mutex_unlock(&port->lock);
5557
-	return 0;
6255
+	return ret;
55586256 }
55596257 EXPORT_SYMBOL_GPL(tcpm_update_sink_capabilities);
55606258 
....@@ -5639,6 +6337,27 @@
56396337 	return 0;
56406338 }
56416339 
6340
+static int tcpm_psy_get_input_power_limit(struct tcpm_port *port,
6341
+					  union power_supply_propval *val)
6342
+{
6343
+	unsigned int src_mv, src_ma, max_src_mw = 0;
6344
+	unsigned int i, tmp;
6345
+
6346
+	for (i = 0; i < port->nr_source_caps; i++) {
6347
+		u32 pdo = port->source_caps[i];
6348
+
6349
+		if (pdo_type(pdo) == PDO_TYPE_FIXED) {
6350
+			src_mv = pdo_fixed_voltage(pdo);
6351
+			src_ma = pdo_max_current(pdo);
6352
+			tmp = src_mv * src_ma / 1000;
6353
+			max_src_mw = tmp > max_src_mw ? tmp : max_src_mw;
6354
+		}
6355
+	}
6356
+
6357
+	val->intval = max_src_mw;
6358
+	return 0;
6359
+}
6360
+
56426361 static int tcpm_psy_get_prop(struct power_supply *psy,
56436362 			     enum power_supply_property psp,
56446363 			     union power_supply_propval *val)
....@@ -5668,11 +6387,13 @@
56686387 	case POWER_SUPPLY_PROP_CURRENT_NOW:
56696388 		ret = tcpm_psy_get_current_now(port, val);
56706389 		break;
6390
+	case POWER_SUPPLY_PROP_INPUT_POWER_LIMIT:
6391
+		tcpm_psy_get_input_power_limit(port, val);
6392
+		break;
56716393 	default:
56726394 		ret = -EINVAL;
56736395 		break;
56746396 	}
5675
-
56766397 	return ret;
56776398 }
56786399 
....@@ -5724,7 +6445,7 @@
57246445 		ret = -EINVAL;
57256446 		break;
57266447 	}
5727
-
6448
+	power_supply_changed(port->psy);
57286449 	return ret;
57296450 }
57306451 
....@@ -5807,92 +6528,18 @@
58076528 	return HRTIMER_NORESTART;
58086529 }
58096530 
5810
-static enum hrtimer_restart data_role_swap_timer_handler(struct hrtimer *timer)
6531
+static enum hrtimer_restart send_discover_timer_handler(struct hrtimer *timer)
58116532 {
5812
-	struct tcpm_port *port =
5813
-		container_of(timer, struct tcpm_port, data_role_swap_timer);
6533
+	struct tcpm_port *port = container_of(timer, struct tcpm_port, send_discover_timer);
58146534 
5815
-	kthread_queue_work(port->wq, &port->data_role_swap);
6535
+	kthread_queue_work(port->wq, &port->send_discover_work);
58166536 	return HRTIMER_NORESTART;
5817
-}
5818
-
5819
-static int tcpm_extcon_register(struct tcpm_port *port)
5820
-{
5821
-	struct extcon_dev *extcon;
5822
-	int ret;
5823
-
5824
-	extcon = devm_extcon_dev_allocate(port->dev, tcpm_cable);
5825
-	if (IS_ERR(extcon)) {
5826
-		dev_err(port->dev, "allocate extcon failed\n");
5827
-		return -ENOMEM;
5828
-	}
5829
-
5830
-	ret = devm_extcon_dev_register(port->dev, extcon);
5831
-	if (ret) {
5832
-		dev_err(port->dev, "failed to register extcon: %d\n", ret);
5833
-		goto init_err;
5834
-	}
5835
-
5836
-	ret = extcon_set_property_capability(extcon, EXTCON_USB,
5837
-					     EXTCON_PROP_USB_TYPEC_POLARITY);
5838
-	if (ret) {
5839
-		dev_err(port->dev, "failed to set USB property capability: %d\n", ret);
5840
-		goto init_err;
5841
-	}
5842
-
5843
-	ret = extcon_set_property_capability(extcon, EXTCON_USB_HOST,
5844
-					     EXTCON_PROP_USB_TYPEC_POLARITY);
5845
-	if (ret) {
5846
-		dev_err(port->dev, "failed to set USB_HOST property capability: %d\n", ret);
5847
-		goto init_err;
5848
-	}
5849
-
5850
-	ret = extcon_set_property_capability(extcon, EXTCON_DISP_DP,
5851
-					     EXTCON_PROP_USB_TYPEC_POLARITY);
5852
-	if (ret) {
5853
-		dev_err(port->dev, "failed to set DISP_DP property capability: %d\n", ret);
5854
-		goto init_err;
5855
-	}
5856
-
5857
-	ret = extcon_set_property_capability(extcon, EXTCON_USB, EXTCON_PROP_USB_SS);
5858
-	if (ret) {
5859
-		dev_err(port->dev, "failed to set USB USB_SS property capability: %d\n", ret);
5860
-		goto init_err;
5861
-	}
5862
-
5863
-	ret = extcon_set_property_capability(extcon, EXTCON_USB_HOST, EXTCON_PROP_USB_SS);
5864
-	if (ret) {
5865
-		dev_err(port->dev, "failed to set USB_HOST USB_SS property capability: %d", ret);
5866
-		goto init_err;
5867
-	}
5868
-
5869
-	ret = extcon_set_property_capability(extcon, EXTCON_DISP_DP, EXTCON_PROP_USB_SS);
5870
-	if (ret) {
5871
-		dev_err(port->dev, "failed to set DISP_DP USB_SS property capability: %d", ret);
5872
-		goto init_err;
5873
-	}
5874
-
5875
-	ret = extcon_set_property_capability(extcon, EXTCON_CHG_USB_FAST,
5876
-					    EXTCON_PROP_USB_TYPEC_POLARITY);
5877
-	if (ret) {
5878
-		dev_err(port->dev, "failed to set USB_PD property capability: %d\n", ret);
5879
-		goto init_err;
5880
-	}
5881
-
5882
-	port->extcon = extcon;
5883
-	tcpm_log(port, "init extcon finished\n");
5884
-
5885
-	return 0;
5886
-
5887
-init_err:
5888
-	return ret;
58896537 }
58906538 
58916539 struct tcpm_port *tcpm_register_port(struct device *dev, struct tcpc_dev *tcpc)
58926540 {
58936541 	struct tcpm_port *port;
58946542 	int err;
5895
-	struct sched_param param = { .sched_priority = MAX_RT_PRIO - 1 };
58966543 
58976544 	if (!dev || !tcpc ||
58986545 	    !tcpc->get_vbus || !tcpc->set_cc || !tcpc->get_cc ||
....@@ -5909,26 +6556,28 @@
59096556 
59106557 	mutex_init(&port->lock);
59116558 	mutex_init(&port->swap_lock);
6559
+#ifdef CONFIG_NO_GKI
59126560 	mutex_init(&port->pd_handler_lock);
6561
+#endif
59136562 
59146563 	port->wq = kthread_create_worker(0, dev_name(dev));
59156564 	if (IS_ERR(port->wq))
59166565 		return ERR_CAST(port->wq);
5917
-	sched_setscheduler_nocheck(port->wq->task, SCHED_FIFO, &param);
6566
+	sched_set_fifo(port->wq->task);
59186567 
59196568 	kthread_init_work(&port->state_machine, tcpm_state_machine_work);
59206569 	kthread_init_work(&port->vdm_state_machine, vdm_state_machine_work);
59216570 	kthread_init_work(&port->event_work, tcpm_pd_event_handler);
59226571 	kthread_init_work(&port->enable_frs, tcpm_enable_frs_work);
5923
-	kthread_init_work(&port->data_role_swap, tcpm_data_role_swap_work);
6572
+	kthread_init_work(&port->send_discover_work, tcpm_send_discover_work);
59246573 	hrtimer_init(&port->state_machine_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
59256574 	port->state_machine_timer.function = state_machine_timer_handler;
59266575 	hrtimer_init(&port->vdm_state_machine_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
59276576 	port->vdm_state_machine_timer.function = vdm_state_machine_timer_handler;
59286577 	hrtimer_init(&port->enable_frs_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
59296578 	port->enable_frs_timer.function = enable_frs_timer_handler;
5930
-	hrtimer_init(&port->data_role_swap_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
5931
-	port->data_role_swap_timer.function = data_role_swap_timer_handler;
6579
+	hrtimer_init(&port->send_discover_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
6580
+	port->send_discover_timer.function = send_discover_timer_handler;
59326581 
59336582 	spin_lock_init(&port->pd_event_lock);
59346583 
....@@ -5936,11 +6585,6 @@
59366585 	init_completion(&port->swap_complete);
59376586 	init_completion(&port->pps_complete);
59386587 	tcpm_debugfs_init(port);
5939
-
5940
-	/* init extcon */
5941
-	err = tcpm_extcon_register(port);
5942
-	if (err)
5943
-		goto out_destroy_wq;
59446588 
59456589 	err = tcpm_fw_get_caps(port, tcpc->fwnode);
59466590 	if (err < 0)
....@@ -5950,12 +6594,10 @@
59506594 
59516595 	port->typec_caps.fwnode = tcpc->fwnode;
59526596 	port->typec_caps.revision = 0x0120;	/* Type-C spec release 1.2 */
5953
-	port->typec_caps.pd_revision = 0x0300;	/* USB-PD spec release 3.0 */
5954
-	port->typec_caps.dr_set = tcpm_dr_set;
5955
-	port->typec_caps.pr_set = tcpm_pr_set;
5956
-	port->typec_caps.vconn_set = tcpm_vconn_set;
5957
-	port->typec_caps.try_role = tcpm_try_role;
5958
-	port->typec_caps.port_type_set = tcpm_port_type_set;
6597
+	port->typec_caps.svdm_version = SVDM_VER_2_0;
6598
+	port->typec_caps.driver_data = port;
6599
+	port->typec_caps.ops = &tcpm_ops;
6600
+	port->typec_caps.orientation_aware = 1;
59596601 
59606602 	port->partner_desc.identity = &port->partner_ident;
59616603 	port->port_type = port->typec_caps.type;
....@@ -5968,12 +6610,13 @@
59686610 
59696611 	err = devm_tcpm_psy_register(port);
59706612 	if (err)
5971
-		goto out_destroy_wq;
6613
+		goto out_role_sw_put;
6614
+	power_supply_changed(port->psy);
59726615 
59736616 	port->typec_port = typec_register_port(port->dev, &port->typec_caps);
59746617 	if (IS_ERR(port->typec_port)) {
59756618 		err = PTR_ERR(port->typec_port);
5976
-		goto out_destroy_wq;
6619
+		goto out_role_sw_put;
59776620 	}
59786621 
59796622 	typec_port_register_altmodes(port->typec_port,
....@@ -5987,8 +6630,10 @@
59876630 	tcpm_log(port, "%s: registered", dev_name(dev));
59886631 	return port;
59896632 
5990
-out_destroy_wq:
6633
+out_role_sw_put:
59916634 	usb_role_switch_put(port->role_sw);
6635
+out_destroy_wq:
6636
+	tcpm_debugfs_exit(port);
59926637 	kthread_destroy_worker(port->wq);
59936638 	return ERR_PTR(err);
59946639 }
....@@ -5998,10 +6643,10 @@
59986643 {
59996644 	int i;
60006645 
6646
+	hrtimer_cancel(&port->send_discover_timer);
60016647 	hrtimer_cancel(&port->enable_frs_timer);
60026648 	hrtimer_cancel(&port->vdm_state_machine_timer);
60036649 	hrtimer_cancel(&port->state_machine_timer);
6004
-	hrtimer_cancel(&port->data_role_swap_timer);
60056650 
60066651 	tcpm_reset_port(port);
60076652 	for (i = 0; i < ARRAY_SIZE(port->port_altmode); i++)