~hc/RK356X_SDK_RELEASE.git

..	..	@@ -8,7 +8,6 @@
8	8	#include <linux/completion.h>
9	9	#include <linux/debugfs.h>
10	10	#include <linux/device.h>
11		-#include <linux/extcon-provider.h>
12	11	#include <linux/hrtimer.h>
13	12	#include <linux/jiffies.h>
14	13	#include <linux/kernel.h>
..	..	@@ -18,11 +17,11 @@
18	17	#include <linux/power_supply.h>
19	18	#include <linux/proc_fs.h>
20	19	#include <linux/property.h>
21		-#include <linux/regulator/consumer.h>
22	20	#include <linux/sched/clock.h>
23	21	#include <linux/seq_file.h>
24	22	#include <linux/slab.h>
25	23	#include <linux/spinlock.h>
	24	+#include <linux/usb.h>
26	25	#include <linux/usb/pd.h>
27	26	#include <linux/usb/pd_ado.h>
28	27	#include <linux/usb/pd_bdo.h>
..	..	@@ -31,8 +30,8 @@
31	30	#include <linux/usb/role.h>
32	31	#include <linux/usb/tcpm.h>
33	32	#include <linux/usb/typec_altmode.h>
34		-#include <linux/usb/typec_dp.h>
35	33
	34	+#include <trace/hooks/typec.h>
36	35	#include <uapi/linux/sched/types.h>
37	36
38	37	#define FOREACH_STATE(S) \
..	..	@@ -145,7 +144,8 @@
145	144	S(PORT_RESET), \
146	145	S(PORT_RESET_WAIT_OFF), \
147	146	\
148		- S(AMS_START)
	147	+ S(AMS_START), \
	148	+ S(CHUNK_NOT_SUPP)
149	149
150	150	#define FOREACH_AMS(S) \
151	151	S(NONE_AMS), \
..	..	@@ -223,6 +223,14 @@
223	223	PD_MSG_DATA_SOURCE_CAP,
224	224	};
225	225
	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	+
226	234	/*
227	235	* Initial current capability of the new source when vSafe5V is applied during PD3.0 Fast Role Swap.
228	236	* Based on "Table 6-14 Fixed Supply PDO - Sink" of "USB Power Delivery Specification Revision 3.0,
..	..	@@ -234,24 +242,6 @@
234	242	FRS_5V_1P5A,
235	243	FRS_5V_3A,
236	244	};
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))
255	245
256	246	/* Events from low level driver */
257	247
..	..	@@ -270,7 +260,7 @@
270	260	#define ALTMODE_DISCOVERY_MAX (SVID_DISCOVERY_MAX * MODE_DISCOVERY_MAX)
271	261
272	262	#define GET_SINK_CAP_RETRY_MS 100
273		-#define SEND_NEW_MODE_NOTIFY_MS 20
	263	+#define SEND_DISCOVER_RETRY_MS 100
274	264
275	265	struct pd_mode_data {
276	266	int svid_index; /* current SVID index */
..	..	@@ -280,12 +270,27 @@
280	270	struct typec_altmode_desc altmode_desc[ALTMODE_DISCOVERY_MAX];
281	271	};
282	272
	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	+ */
283	285	struct pd_pps_data {
284	286	u32 min_volt;
	287	+ u32 req_min_volt;
285	288	u32 max_volt;
	289	+ u32 req_max_volt;
286	290	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;
289	294	bool supported;
290	295	bool active;
291	296	};
..	..	@@ -311,9 +316,6 @@
311	316	struct typec_partner_desc partner_desc;
312	317	struct typec_partner *partner;
313	318
314		- struct regulator *vbus;
315		- struct extcon_dev *extcon;
316		-
317	319	enum typec_cc_status cc_req;
318	320
319	321	enum typec_cc_status cc1;
..	..	@@ -323,14 +325,26 @@
323	325	bool attached;
324	326	bool connected;
325	327	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	+ */
326	333	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	+
327	341	bool vbus_never_low;
328	342	bool vbus_source;
329	343	bool vbus_charge;
330	344
	345	+ /* Set to true when Discover_Identity Command is expected to be sent in Ready states. */
331	346	bool send_discover;
332	347	bool op_vsafe5v;
333		- bool vbus_on;
334	348
335	349	int try_role;
336	350	int try_snk_count;
..	..	@@ -346,7 +360,9 @@
346	360	unsigned long delay_ms;
347	361
348	362	spinlock_t pd_event_lock;
	363	+#ifdef CONFIG_NO_GKI
349	364	struct mutex pd_handler_lock;
	365	+#endif
350	366	u32 pd_events;
351	367
352	368	struct kthread_work event_work;
..	..	@@ -356,9 +372,10 @@
356	372	struct kthread_work vdm_state_machine;
357	373	struct hrtimer enable_frs_timer;
358	374	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;
361	377	bool state_machine_running;
	378	+ /* Set to true when VDM State Machine has following actions. */
362	379	bool vdm_sm_running;
363	380
364	381	struct completion tx_complete;
..	..	@@ -378,10 +395,6 @@
378	395	bool explicit_contract;
379	396	unsigned int rx_msgid;
380	397
381		- u32 dp_pin_assignment;
382		- u32 dp_status;
383		- bool dp_configured;
384		-
385	398	/* Partner capabilities/requests */
386	399	u32 sink_request;
387	400	u32 source_caps[PDO_MAX_OBJECTS];
..	..	@@ -394,13 +407,18 @@
394	407	unsigned int nr_src_pdo;
395	408	u32 snk_pdo[PDO_MAX_OBJECTS];
396	409	unsigned int nr_snk_pdo;
	410	+ u32 snk_vdo_v1[VDO_MAX_OBJECTS];
	411	+ unsigned int nr_snk_vdo_v1;
397	412	u32 snk_vdo[VDO_MAX_OBJECTS];
398	413	unsigned int nr_snk_vdo;
399	414
400	415	unsigned int operating_snk_mw;
401	416	bool update_sink_caps;
402	417
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 */
404	422	u32 current_limit;
405	423	u32 supply_voltage;
406	424
..	..	@@ -437,17 +455,30 @@
437	455	/* port belongs to a self powered device */
438	456	bool self_powered;
439	457
440		- /* FRS */
441		- enum frs_typec_current frs_current;
	458	+ /* Sink FRS */
	459	+ enum frs_typec_current new_source_frs_current;
442	460
443	461	/* Sink caps have been queried */
444	462	bool sink_cap_done;
445	463
	464	+ /* Port is still in tCCDebounce */
	465	+ bool debouncing;
	466	+
446	467	/* Collision Avoidance and Atomic Message Sequence */
447	468	enum tcpm_state upcoming_state;
448	469	enum tcpm_ams ams;
	470	+ enum tcpm_ams next_ams;
449	471	bool in_ams;
450	472
	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;
451	482	#ifdef CONFIG_DEBUG_FS
452	483	struct dentry *dentry;
453	484	struct mutex logbuffer_lock; /* log buffer access lock */
..	..	@@ -505,9 +536,20 @@
505	536	((port)->try_src_count == 0 && (port)->try_role == TYPEC_SOURCE && \
506	537	(port)->port_type == TYPEC_PORT_DRP)
507	538
	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	+
508	547	#define tcpm_sink_tx_ok(port) \
509	548	(tcpm_port_is_sink(port) && \
510	549	((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)
511	553
512	554	static enum tcpm_state tcpm_default_state(struct tcpm_port *port)
513	555	{
..	..	@@ -521,12 +563,6 @@
521	563	return SNK_UNATTACHED;
522	564	}
523	565	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);
530	566	}
531	567
532	568	static bool tcpm_port_is_disconnected(struct tcpm_port *port)
..	..	@@ -557,6 +593,7 @@
557	593	char tmpbuffer[LOG_BUFFER_ENTRY_SIZE];
558	594	u64 ts_nsec = local_clock();
559	595	unsigned long rem_nsec;
	596	+ bool bypass_log = false;
560	597
561	598	mutex_lock(&port->logbuffer_lock);
562	599	if (!port->logbuffer[port->logbuffer_head]) {
..	..	@@ -569,6 +606,9 @@
569	606	}
570	607
571	608	vsnprintf(tmpbuffer, sizeof(tmpbuffer), fmt, args);
	609	+ trace_android_vh_typec_tcpm_log(tmpbuffer, &bypass_log);
	610	+ if (bypass_log)
	611	+ goto abort;
572	612
573	613	if (tcpm_log_full(port)) {
574	614	port->logbuffer_head = max(port->logbuffer_head - 1, 0);
..	..	@@ -705,17 +745,13 @@
705	745	}
706	746	DEFINE_SHOW_ATTRIBUTE(tcpm_debug);
707	747
708		-static struct dentry *rootdir;
709		-
710	748	static void tcpm_debugfs_init(struct tcpm_port *port)
711	749	{
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];
716	751
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,
719	755	port, &tcpm_debug_fops);
720	756	}
721	757
..	..	@@ -731,10 +767,6 @@
731	767	mutex_unlock(&port->logbuffer_lock);
732	768
733	769	debugfs_remove(port->dentry);
734		- if (list_empty(&rootdir->d_subdirs)) {
735		- debugfs_remove(rootdir);
736		- rootdir = NULL;
737		- }
738	770	}
739	771
740	772	#else
..	..	@@ -749,110 +781,39 @@
749	781
750	782	#endif
751	783
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		-
851	784	static void tcpm_set_cc(struct tcpm_port *port, enum typec_cc_status cc)
852	785	{
853	786	tcpm_log(port, "cc:=%d", cc);
854	787	port->cc_req = cc;
855	788	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	+ }
856	817	}
857	818
858	819	/*
..	..	@@ -888,10 +849,8 @@
888	849	return TYPEC_CC_RP_DEF;
889	850	}
890	851
891		-static int tcpm_ams_finish(struct tcpm_port *port)
	852	+static void tcpm_ams_finish(struct tcpm_port *port)
892	853	{
893		- int ret = 0;
894		-
895	854	tcpm_log(port, "AMS %s finished", tcpm_ams_str[port->ams]);
896	855
897	856	if (port->pd_capable && port->pwr_role == TYPEC_SOURCE) {
..	..	@@ -905,8 +864,6 @@
905	864
906	865	port->in_ams = false;
907	866	port->ams = NONE_AMS;
908		-
909		- return ret;
910	867	}
911	868
912	869	static int tcpm_pd_transmit(struct tcpm_port *port,
..	..	@@ -922,7 +879,7 @@
922	879	tcpm_log(port, "PD TX, type: %#x", type);
923	880
924	881	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);
926	883	if (ret < 0)
927	884	return ret;
928	885
..	..	@@ -1024,6 +981,21 @@
1024	981	return ret;
1025	982	}
1026	983
	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	+
1027	999	static u32 tcpm_get_current_limit(struct tcpm_port *port)
1028	1000	{
1029	1001	enum typec_cc_status cc;
..	..	@@ -1057,6 +1029,7 @@
1057	1029
1058	1030	port->supply_voltage = mv;
1059	1031	port->current_limit = max_ma;
	1032	+ power_supply_changed(port->psy);
1060	1033
1061	1034	if (port->tcpc->set_current_limit)
1062	1035	ret = port->tcpc->set_current_limit(port->tcpc, max_ma, mv);
..	..	@@ -1082,10 +1055,30 @@
1082	1055	else
1083	1056	orientation = TYPEC_ORIENTATION_REVERSE;
1084	1057
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	+ }
1089	1082
1090	1083	ret = tcpm_mux_set(port, TYPEC_STATE_USB, usb_role, orientation);
1091	1084	if (ret < 0)
..	..	@@ -1118,13 +1111,47 @@
1118	1111	return 0;
1119	1112	}
1120	1113
	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	+
1121	1136	static int tcpm_pd_send_source_caps(struct tcpm_port *port)
1122	1137	{
1123	1138	struct pd_message msg;
1124		- int i;
	1139	+ u32 pdo;
	1140	+ unsigned int i, nr_pdo = 0;
1125	1141
1126	1142	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) {
1128	1155	/* No source capabilities defined, sink only */
1129	1156	msg.header = PD_HEADER_LE(PD_CTRL_REJECT,
1130	1157	port->pwr_role,
..	..	@@ -1137,10 +1164,8 @@
1137	1164	port->data_role,
1138	1165	port->negotiated_rev,
1139	1166	port->message_id,
1140		- port->nr_src_pdo);
	1167	+ nr_pdo);
1141	1168	}
1142		- for (i = 0; i < port->nr_src_pdo; i++)
1143		- msg.payload[i] = cpu_to_le32(port->src_pdo[i]);
1144	1169
1145	1170	return tcpm_pd_transmit(port, TCPC_TX_SOP, &msg);
1146	1171	}
..	..	@@ -1148,10 +1173,22 @@
1148	1173	static int tcpm_pd_send_sink_caps(struct tcpm_port *port)
1149	1174	{
1150	1175	struct pd_message msg;
1151		- int i;
	1176	+ u32 pdo;
	1177	+ unsigned int i, nr_pdo = 0;
1152	1178
1153	1179	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) {
1155	1192	/* No sink capabilities defined, source only */
1156	1193	msg.header = PD_HEADER_LE(PD_CTRL_REJECT,
1157	1194	port->pwr_role,
..	..	@@ -1164,10 +1201,8 @@
1164	1201	port->data_role,
1165	1202	port->negotiated_rev,
1166	1203	port->message_id,
1167		- port->nr_snk_pdo);
	1204	+ nr_pdo);
1168	1205	}
1169		- for (i = 0; i < port->nr_snk_pdo; i++)
1170		- msg.payload[i] = cpu_to_le32(port->snk_pdo[i]);
1171	1206
1172	1207	return tcpm_pd_transmit(port, TCPC_TX_SOP, &msg);
1173	1208	}
..	..	@@ -1203,14 +1238,13 @@
1203	1238	}
1204	1239	}
1205	1240
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)
1207	1242	{
1208	1243	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);
1211	1245	} 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);
1214	1248	}
1215	1249	}
1216	1250
..	..	@@ -1322,7 +1356,8 @@
1322	1356
1323	1357	tcpm_log(port, "AMS %s start", tcpm_ams_str[ams]);
1324	1358
1325		- if (!tcpm_ams_interruptible(port) && ams != HARD_RESET) {
	1359	+ if (!tcpm_ams_interruptible(port) &&
	1360	+ !(ams == HARD_RESET \|\| ams == SOFT_RESET_AMS)) {
1326	1361	port->upcoming_state = INVALID_STATE;
1327	1362	tcpm_log(port, "AMS %s not interruptible, aborting",
1328	1363	tcpm_ams_str[port->ams]);
..	..	@@ -1340,11 +1375,10 @@
1340	1375	tcpm_set_state(port, HARD_RESET_START, 0);
1341	1376	return ret;
1342	1377	} else if (ams == SOFT_RESET_AMS) {
1343		- if (!port->explicit_contract) {
1344		- port->upcoming_state = INVALID_STATE;
	1378	+ if (!port->explicit_contract)
1345	1379	tcpm_set_cc(port, tcpm_rp_cc(port));
1346		- return ret;
1347		- }
	1380	+ tcpm_set_state(port, SOFT_RESET_SEND, 0);
	1381	+ return ret;
1348	1382	} else if (tcpm_vdm_ams(port)) {
1349	1383	/* tSinkTx is enforced in vdm_run_state_machine */
1350	1384	if (port->negotiated_rev >= PD_REV30)
..	..	@@ -1413,24 +1447,48 @@
1413	1447	static void tcpm_queue_vdm(struct tcpm_port *port, const u32 header,
1414	1448	const u32 *data, int cnt)
1415	1449	{
	1450	+ WARN_ON(!mutex_is_locked(&port->lock));
	1451	+
	1452	+ /* Make sure we are not still processing a previous VDM packet */
	1453	+ WARN_ON(port->vdm_state > VDM_STATE_DONE);
	1454	+
1416	1455	port->vdo_count = cnt + 1;
1417	1456	port->vdo_data[0] = header;
1418	1457	memcpy(&port->vdo_data[1], data, sizeof(u32) * cnt);
1419	1458	/* Set ready, vdm state machine will actually send */
1420	1459	port->vdm_retries = 0;
1421	1460	port->vdm_state = VDM_STATE_READY;
	1461	+ port->vdm_sm_running = true;
	1462	+
	1463	+ mod_vdm_delayed_work(port, 0);
1422	1464	}
1423	1465
1424		-static void svdm_consume_identity(struct tcpm_port port, const __le32 payload,
1425		- int cnt)
	1466	+static void tcpm_queue_vdm_unlocked(struct tcpm_port *port, const u32 header,
	1467	+ const u32 *data, int cnt)
1426	1468	{
1427		- u32 vdo = le32_to_cpu(payload[VDO_INDEX_IDH]);
1428		- u32 product = le32_to_cpu(payload[VDO_INDEX_PRODUCT]);
	1469	+#ifdef CONFIG_NO_GKI
	1470	+ mutex_lock(&port->pd_handler_lock);
	1471	+ if (tcpm_port_is_disconnected(port))
	1472	+ goto unlock;
	1473	+#endif
	1474	+ mutex_lock(&port->lock);
	1475	+ tcpm_queue_vdm(port, header, data, cnt);
	1476	+ mutex_unlock(&port->lock);
	1477	+#ifdef CONFIG_NO_GKI
	1478	+unlock:
	1479	+ mutex_unlock(&port->pd_handler_lock);
	1480	+#endif
	1481	+}
	1482	+
	1483	+static void svdm_consume_identity(struct tcpm_port port, const u32 p, int cnt)
	1484	+{
	1485	+ u32 vdo = p[VDO_INDEX_IDH];
	1486	+ u32 product = p[VDO_INDEX_PRODUCT];
1429	1487
1430	1488	memset(&port->mode_data, 0, sizeof(port->mode_data));
1431	1489
1432	1490	port->partner_ident.id_header = vdo;
1433		- port->partner_ident.cert_stat = le32_to_cpu(payload[VDO_INDEX_CSTAT]);
	1491	+ port->partner_ident.cert_stat = p[VDO_INDEX_CSTAT];
1434	1492	port->partner_ident.product = product;
1435	1493
1436	1494	typec_partner_set_identity(port->partner);
..	..	@@ -1440,17 +1498,15 @@
1440	1498	PD_PRODUCT_PID(product), product & 0xffff);
1441	1499	}
1442	1500
1443		-static bool svdm_consume_svids(struct tcpm_port port, const __le32 payload,
1444		- int cnt)
	1501	+static bool svdm_consume_svids(struct tcpm_port port, const u32 p, int cnt)
1445	1502	{
1446	1503	struct pd_mode_data *pmdata = &port->mode_data;
1447	1504	int i;
1448	1505
1449	1506	for (i = 1; i < cnt; i++) {
1450		- u32 p = le32_to_cpu(payload[i]);
1451	1507	u16 svid;
1452	1508
1453		- svid = (p >> 16) & 0xffff;
	1509	+ svid = (p[i] >> 16) & 0xffff;
1454	1510	if (!svid)
1455	1511	return false;
1456	1512
..	..	@@ -1460,7 +1516,7 @@
1460	1516	pmdata->svids[pmdata->nsvids++] = svid;
1461	1517	tcpm_log(port, "SVID %d: 0x%x", pmdata->nsvids, svid);
1462	1518
1463		- svid = p & 0xffff;
	1519	+ svid = p[i] & 0xffff;
1464	1520	if (!svid)
1465	1521	return false;
1466	1522
..	..	@@ -1470,14 +1526,27 @@
1470	1526	pmdata->svids[pmdata->nsvids++] = svid;
1471	1527	tcpm_log(port, "SVID %d: 0x%x", pmdata->nsvids, svid);
1472	1528	}
1473		- return true;
	1529	+
	1530	+ /*
	1531	+ * PD3.0 Spec 6.4.4.3.2: The SVIDs are returned 2 per VDO (see Table
	1532	+ * 6-43), and can be returned maximum 6 VDOs per response (see Figure
	1533	+ * 6-19). If the Respondersupports 12 or more SVID then the Discover
	1534	+ * SVIDs Command Shall be executed multiple times until a Discover
	1535	+ * SVIDs VDO is returned ending either with a SVID value of 0x0000 in
	1536	+ * the last part of the last VDO or with a VDO containing two SVIDs
	1537	+ * with values of 0x0000.
	1538	+ *
	1539	+ * However, some odd dockers support SVIDs less than 12 but without
	1540	+ * 0x0000 in the last VDO, so we need to break the Discover SVIDs
	1541	+ * request and return false here.
	1542	+ */
	1543	+ return cnt == 7 ? true : false;
1474	1544	abort:
1475	1545	tcpm_log(port, "SVID_DISCOVERY_MAX(%d) too low!", SVID_DISCOVERY_MAX);
1476	1546	return false;
1477	1547	}
1478	1548
1479		-static void svdm_consume_modes(struct tcpm_port port, const __le32 payload,
1480		- int cnt)
	1549	+static void svdm_consume_modes(struct tcpm_port port, const u32 p, int cnt)
1481	1550	{
1482	1551	struct pd_mode_data *pmdata = &port->mode_data;
1483	1552	struct typec_altmode_desc *paltmode;
..	..	@@ -1494,7 +1563,7 @@
1494	1563
1495	1564	paltmode->svid = pmdata->svids[pmdata->svid_index];
1496	1565	paltmode->mode = i;
1497		- paltmode->vdo = le32_to_cpu(payload[i]);
	1566	+ paltmode->vdo = p[i];
1498	1567
1499	1568	tcpm_log(port, " Alternate mode %d: SVID 0x%04x, VDO %d: 0x%08x",
1500	1569	pmdata->altmodes, paltmode->svid,
..	..	@@ -1513,29 +1582,29 @@
1513	1582	for (i = 0; i < modep->altmodes; i++) {
1514	1583	altmode = typec_partner_register_altmode(port->partner,
1515	1584	&modep->altmode_desc[i]);
1516		- if (!altmode)
	1585	+ if (IS_ERR(altmode)) {
1517	1586	tcpm_log(port, "Failed to register partner SVID 0x%04x",
1518	1587	modep->altmode_desc[i].svid);
	1588	+ altmode = NULL;
	1589	+ }
1519	1590	port->partner_altmode[i] = altmode;
1520	1591	}
1521	1592	}
1522	1593
1523	1594	#define supports_modal(port) PD_IDH_MODAL_SUPP((port)->partner_ident.id_header)
1524	1595
1525		-static int tcpm_pd_svdm(struct tcpm_port port, const __le32 payload, int cnt,
1526		- u32 *response)
	1596	+static int tcpm_pd_svdm(struct tcpm_port port, struct typec_altmode adev,
	1597	+ const u32 p, int cnt, u32 response,
	1598	+ enum adev_actions *adev_action)
1527	1599	{
1528		- struct typec_altmode *adev;
	1600	+ struct typec_port *typec = port->typec_port;
1529	1601	struct typec_altmode *pdev;
1530	1602	struct pd_mode_data *modep;
1531		- u32 p[PD_MAX_PAYLOAD];
	1603	+ int svdm_version;
1532	1604	int rlen = 0;
1533	1605	int cmd_type;
1534	1606	int cmd;
1535	1607	int i;
1536		-
1537		- for (i = 0; i < cnt; i++)
1538		- p[i] = le32_to_cpu(payload[i]);
1539	1608
1540	1609	cmd_type = PD_VDO_CMDT(p[0]);
1541	1610	cmd = PD_VDO_CMD(p[0]);
..	..	@@ -1545,36 +1614,61 @@
1545	1614
1546	1615	modep = &port->mode_data;
1547	1616
1548		- adev = typec_match_altmode(port->port_altmode, ALTMODE_DISCOVERY_MAX,
1549		- PD_VDO_VID(p[0]), PD_VDO_OPOS(p[0]));
1550		-
1551	1617	pdev = typec_match_altmode(port->partner_altmode, ALTMODE_DISCOVERY_MAX,
1552	1618	PD_VDO_VID(p[0]), PD_VDO_OPOS(p[0]));
	1619	+
	1620	+ svdm_version = typec_get_negotiated_svdm_version(typec);
	1621	+ if (svdm_version < 0)
	1622	+ return 0;
1553	1623
1554	1624	switch (cmd_type) {
1555	1625	case CMDT_INIT:
1556	1626	switch (cmd) {
1557	1627	case CMD_DISCOVER_IDENT:
1558		- /* 6.4.4.3.1: Only respond as UFP (device) */
1559		- if (port->data_role == TYPEC_DEVICE &&
	1628	+ if (PD_VDO_VID(p[0]) != USB_SID_PD)
	1629	+ break;
	1630	+
	1631	+ if (PD_VDO_SVDM_VER(p[0]) < svdm_version) {
	1632	+ typec_partner_set_svdm_version(port->partner,
	1633	+ PD_VDO_SVDM_VER(p[0]));
	1634	+ svdm_version = PD_VDO_SVDM_VER(p[0]);
	1635	+ }
	1636	+
	1637	+ port->ams = DISCOVER_IDENTITY;
	1638	+ /*
	1639	+ * PD2.0 Spec 6.10.3: respond with NAK as DFP (data host)
	1640	+ * PD3.1 Spec 6.4.4.2.5.1: respond with NAK if "invalid field" or
	1641	+ * "wrong configuation" or "Unrecognized"
	1642	+ */
	1643	+ if ((port->data_role == TYPEC_DEVICE \|\| svdm_version >= SVDM_VER_2_0) &&
1560	1644	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;
	1645	+ if (svdm_version < SVDM_VER_2_0) {
	1646	+ for (i = 0; i < port->nr_snk_vdo_v1; i++)
	1647	+ response[i + 1] = port->snk_vdo_v1[i];
	1648	+ rlen = port->nr_snk_vdo_v1 + 1;
	1649	+
	1650	+ } else {
	1651	+ for (i = 0; i < port->nr_snk_vdo; i++)
	1652	+ response[i + 1] = port->snk_vdo[i];
	1653	+ rlen = port->nr_snk_vdo + 1;
	1654	+ }
1564	1655	}
1565	1656	break;
1566	1657	case CMD_DISCOVER_SVID:
	1658	+ port->ams = DISCOVER_SVIDS;
1567	1659	break;
1568	1660	case CMD_DISCOVER_MODES:
	1661	+ port->ams = DISCOVER_MODES;
1569	1662	break;
1570	1663	case CMD_ENTER_MODE:
	1664	+ port->ams = DFP_TO_UFP_ENTER_MODE;
1571	1665	break;
1572	1666	case CMD_EXIT_MODE:
	1667	+ port->ams = DFP_TO_UFP_EXIT_MODE;
1573	1668	break;
1574	1669	case CMD_ATTENTION:
1575	1670	/* Attention command does not have response */
1576		- if (adev)
1577		- typec_altmode_attention(adev, p[1]);
	1671	+ *adev_action = ADEV_ATTENTION;
1578	1672	return 0;
1579	1673	default:
1580	1674	break;
..	..	@@ -1588,6 +1682,8 @@
1588	1682	response[0] = p[0] \| VDO_CMDT(CMDT_RSP_BUSY);
1589	1683	rlen = 1;
1590	1684	}
	1685	+ response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) \|
	1686	+ (VDO_SVDM_VERS(typec_get_negotiated_svdm_version(typec)));
1591	1687	break;
1592	1688	case CMDT_RSP_ACK:
1593	1689	/* silently drop message if we are not connected */
..	..	@@ -1598,101 +1694,121 @@
1598	1694
1599	1695	switch (cmd) {
1600	1696	case CMD_DISCOVER_IDENT:
	1697	+ if (PD_VDO_SVDM_VER(p[0]) < svdm_version)
	1698	+ typec_partner_set_svdm_version(port->partner,
	1699	+ PD_VDO_SVDM_VER(p[0]));
1601	1700	/* 6.4.4.3.1 */
1602		- svdm_consume_identity(port, payload, cnt);
1603		- response[0] = VDO(USB_SID_PD, 1, CMD_DISCOVER_SVID);
	1701	+ svdm_consume_identity(port, p, cnt);
	1702	+ response[0] = VDO(USB_SID_PD, 1, typec_get_negotiated_svdm_version(typec),
	1703	+ CMD_DISCOVER_SVID);
1604	1704	rlen = 1;
1605	1705	break;
1606	1706	case CMD_DISCOVER_SVID:
1607	1707	/* 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);
	1708	+ if (svdm_consume_svids(port, p, cnt)) {
	1709	+ response[0] = VDO(USB_SID_PD, 1, svdm_version, CMD_DISCOVER_SVID);
1611	1710	rlen = 1;
1612	1711	} else if (modep->nsvids && supports_modal(port)) {
1613		- response[0] = VDO(modep->svids[0], 1,
	1712	+ response[0] = VDO(modep->svids[0], 1, svdm_version,
1614	1713	CMD_DISCOVER_MODES);
1615	1714	rlen = 1;
1616	1715	}
1617	1716	break;
1618	1717	case CMD_DISCOVER_MODES:
1619	1718	/* 6.4.4.3.3 */
1620		- svdm_consume_modes(port, payload, cnt);
	1719	+ svdm_consume_modes(port, p, cnt);
1621	1720	modep->svid_index++;
1622	1721	if (modep->svid_index < modep->nsvids) {
1623	1722	u16 svid = modep->svids[modep->svid_index];
1624		- response[0] = VDO(svid, 1, CMD_DISCOVER_MODES);
	1723	+ response[0] = VDO(svid, 1, svdm_version, CMD_DISCOVER_MODES);
1625	1724	rlen = 1;
1626		- } else {
	1725	+ } else if (port->data_role == TYPEC_HOST) {
1627	1726	tcpm_register_partner_altmodes(port);
1628		- port->vdm_sm_running = false;
1629	1727	}
1630	1728	break;
1631	1729	case CMD_ENTER_MODE:
1632	1730	if (adev && pdev) {
1633	1731	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		- }
	1732	+ *adev_action = ADEV_QUEUE_VDM_SEND_EXIT_MODE_ON_FAIL;
1641	1733	}
1642	1734	return 0;
1643	1735	case CMD_EXIT_MODE:
1644	1736	if (adev && pdev) {
1645	1737	typec_altmode_update_active(pdev, false);
1646		-
1647	1738	/* Back to USB Operation */
1648		- WARN_ON(typec_altmode_notify(adev,
1649		- TYPEC_STATE_USB,
1650		- NULL));
	1739	+ *adev_action = ADEV_NOTIFY_USB_AND_QUEUE_VDM;
	1740	+ return 0;
1651	1741	}
1652	1742	break;
	1743	+ case VDO_CMD_VENDOR(0) ... VDO_CMD_VENDOR(15):
	1744	+ break;
1653	1745	default:
	1746	+ /* Unrecognized SVDM */
	1747	+ response[0] = p[0] \| VDO_CMDT(CMDT_RSP_NAK);
	1748	+ rlen = 1;
	1749	+ response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) \|
	1750	+ (VDO_SVDM_VERS(svdm_version));
1654	1751	break;
1655	1752	}
1656	1753	break;
1657	1754	case CMDT_RSP_NAK:
1658	1755	tcpm_ams_finish(port);
1659	1756	switch (cmd) {
	1757	+ case CMD_DISCOVER_IDENT:
	1758	+ case CMD_DISCOVER_SVID:
	1759	+ case CMD_DISCOVER_MODES:
	1760	+ case VDO_CMD_VENDOR(0) ... VDO_CMD_VENDOR(15):
	1761	+ break;
1660	1762	case CMD_ENTER_MODE:
1661	1763	/* Back to USB Operation */
1662		- if (adev)
1663		- WARN_ON(typec_altmode_notify(adev,
1664		- TYPEC_STATE_USB,
1665		- NULL));
1666		- break;
	1764	+ *adev_action = ADEV_NOTIFY_USB_AND_QUEUE_VDM;
	1765	+ return 0;
1667	1766	default:
	1767	+ /* Unrecognized SVDM */
	1768	+ response[0] = p[0] \| VDO_CMDT(CMDT_RSP_NAK);
	1769	+ rlen = 1;
	1770	+ response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) \|
	1771	+ (VDO_SVDM_VERS(svdm_version));
1668	1772	break;
1669	1773	}
1670		- port->vdm_sm_running = false;
1671	1774	break;
1672	1775	default:
1673		- port->vdm_sm_running = false;
	1776	+ response[0] = p[0] \| VDO_CMDT(CMDT_RSP_NAK);
	1777	+ rlen = 1;
	1778	+ response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) \|
	1779	+ (VDO_SVDM_VERS(svdm_version));
1674	1780	break;
1675	1781	}
1676	1782
1677	1783	/* Informing the alternate mode drivers about everything */
1678		- if (adev)
1679		- typec_altmode_vdm(adev, p[0], &p[1], cnt);
1680		-
	1784	+ *adev_action = ADEV_QUEUE_VDM;
1681	1785	return rlen;
1682	1786	}
	1787	+
	1788	+static void tcpm_pd_handle_msg(struct tcpm_port *port,
	1789	+ enum pd_msg_request message,
	1790	+ enum tcpm_ams ams);
1683	1791
1684	1792	static void tcpm_handle_vdm_request(struct tcpm_port *port,
1685	1793	const __le32 *payload, int cnt)
1686	1794	{
1687		- int rlen = 0;
	1795	+ enum adev_actions adev_action = ADEV_NONE;
	1796	+ struct typec_altmode *adev;
	1797	+ u32 p[PD_MAX_PAYLOAD];
1688	1798	u32 response[8] = { };
1689		- u32 p0 = le32_to_cpu(payload[0]);
	1799	+ int i, rlen = 0;
	1800	+
	1801	+ for (i = 0; i < cnt; i++)
	1802	+ p[i] = le32_to_cpu(payload[i]);
	1803	+
	1804	+ adev = typec_match_altmode(port->port_altmode, ALTMODE_DISCOVERY_MAX,
	1805	+ PD_VDO_VID(p[0]), PD_VDO_OPOS(p[0]));
1690	1806
1691	1807	if (port->vdm_state == VDM_STATE_BUSY) {
1692	1808	/* If UFP responded busy retry after timeout */
1693		- if (PD_VDO_CMDT(p0) == CMDT_RSP_BUSY) {
	1809	+ if (PD_VDO_CMDT(p[0]) == CMDT_RSP_BUSY) {
1694	1810	port->vdm_state = VDM_STATE_WAIT_RSP_BUSY;
1695		- port->vdo_retry = (p0 & ~VDO_CMDT_MASK) \|
	1811	+ port->vdo_retry = (p[0] & ~VDO_CMDT_MASK) \|
1696	1812	CMDT_INIT;
1697	1813	mod_vdm_delayed_work(port, PD_T_VDM_BUSY);
1698	1814	return;
..	..	@@ -1700,29 +1816,110 @@
1700	1816	port->vdm_state = VDM_STATE_DONE;
1701	1817	}
1702	1818
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);
	1819	+ if (PD_VDO_SVDM(p[0]) && (adev \|\| tcpm_vdm_ams(port) \|\| port->nr_snk_vdo)) {
	1820	+ /*
	1821	+ * Here a SVDM is received (INIT or RSP or unknown). Set the vdm_sm_running in
	1822	+ * advance because we are dropping the lock but may send VDMs soon.
	1823	+ * For the cases of INIT received:
	1824	+ * - If no response to send, it will be cleared later in this function.
	1825	+ * - If there are responses to send, it will be cleared in the state machine.
	1826	+ * For the cases of RSP received:
	1827	+ * - If no further INIT to send, it will be cleared later in this function.
	1828	+ * - Otherwise, it will be cleared in the state machine if timeout or it will go
	1829	+ * back here until no further INIT to send.
	1830	+ * For the cases of unknown type received:
	1831	+ * - We will send NAK and the flag will be cleared in the state machine.
	1832	+ */
	1833	+ port->vdm_sm_running = true;
	1834	+ rlen = tcpm_pd_svdm(port, adev, p, cnt, response, &adev_action);
	1835	+ } else {
	1836	+ if (port->negotiated_rev >= PD_REV30)
	1837	+ tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
1709	1838	}
	1839	+
	1840	+ /*
	1841	+ * We are done with any state stored in the port struct now, except
	1842	+ * for any port struct changes done by the tcpm_queue_vdm() call
	1843	+ * below, which is a separate operation.
	1844	+ *
	1845	+ * So we can safely release the lock here; and we MUST release the
	1846	+ * lock here to avoid an AB BA lock inversion:
	1847	+ *
	1848	+ * If we keep the lock here then the lock ordering in this path is:
	1849	+ * 1. tcpm_pd_rx_handler take the tcpm port lock
	1850	+ * 2. One of the typec_altmode_* calls below takes the alt-mode's lock
	1851	+ *
	1852	+ * And we also have this ordering:
	1853	+ * 1. alt-mode driver takes the alt-mode's lock
	1854	+ * 2. alt-mode driver calls tcpm_altmode_enter which takes the
	1855	+ * tcpm port lock
	1856	+ *
	1857	+ * Dropping our lock here avoids this.
	1858	+ */
	1859	+ mutex_unlock(&port->lock);
	1860	+
	1861	+ if (adev) {
	1862	+ switch (adev_action) {
	1863	+ case ADEV_NONE:
	1864	+ break;
	1865	+ case ADEV_NOTIFY_USB_AND_QUEUE_VDM:
	1866	+ WARN_ON(typec_altmode_notify(adev, TYPEC_STATE_USB, NULL));
	1867	+ typec_altmode_vdm(adev, p[0], &p[1], cnt);
	1868	+ break;
	1869	+ case ADEV_QUEUE_VDM:
	1870	+ typec_altmode_vdm(adev, p[0], &p[1], cnt);
	1871	+ break;
	1872	+ case ADEV_QUEUE_VDM_SEND_EXIT_MODE_ON_FAIL:
	1873	+ if (typec_altmode_vdm(adev, p[0], &p[1], cnt)) {
	1874	+ int svdm_version = typec_get_negotiated_svdm_version(
	1875	+ port->typec_port);
	1876	+ if (svdm_version < 0)
	1877	+ break;
	1878	+
	1879	+ response[0] = VDO(adev->svid, 1, svdm_version,
	1880	+ CMD_EXIT_MODE);
	1881	+ response[0] \|= VDO_OPOS(adev->mode);
	1882	+ rlen = 1;
	1883	+ }
	1884	+ break;
	1885	+ case ADEV_ATTENTION:
	1886	+ typec_altmode_attention(adev, p[1]);
	1887	+ break;
	1888	+ }
	1889	+ }
	1890	+
	1891	+ /*
	1892	+ * We must re-take the lock here to balance the unlock in
	1893	+ * tcpm_pd_rx_handler, note that no changes, other then the
	1894	+ * tcpm_queue_vdm call, are made while the lock is held again.
	1895	+ * All that is done after the call is unwinding the call stack until
	1896	+ * we return to tcpm_pd_rx_handler and do the unlock there.
	1897	+ */
	1898	+ mutex_lock(&port->lock);
	1899	+
	1900	+ if (rlen > 0)
	1901	+ tcpm_queue_vdm(port, response[0], &response[1], rlen - 1);
	1902	+ else
	1903	+ port->vdm_sm_running = false;
1710	1904	}
1711	1905
1712	1906	static void tcpm_send_vdm(struct tcpm_port *port, u32 vid, int cmd,
1713	1907	const u32 *data, int count)
1714	1908	{
	1909	+ int svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
1715	1910	u32 header;
	1911	+
	1912	+ if (svdm_version < 0)
	1913	+ return;
1716	1914
1717	1915	if (WARN_ON(count > VDO_MAX_SIZE - 1))
1718	1916	count = VDO_MAX_SIZE - 1;
1719	1917
1720	1918	/* set VDM header with VID & CMD */
1721	1919	header = VDO(vid, ((vid & USB_SID_PD) == USB_SID_PD) ?
1722		- 1 : (PD_VDO_CMD(cmd) <= CMD_ATTENTION), cmd);
	1920	+ 1 : (PD_VDO_CMD(cmd) <= CMD_ATTENTION),
	1921	+ svdm_version, cmd);
1723	1922	tcpm_queue_vdm(port, header, data, count);
1724		-
1725		- mod_vdm_delayed_work(port, 0);
1726	1923	}
1727	1924
1728	1925	static unsigned int vdm_ready_timeout(u32 vdm_hdr)
..	..	@@ -1769,8 +1966,10 @@
1769	1966	* if there's traffic or we're not in PDO ready state don't send
1770	1967	* a VDM.
1771	1968	*/
1772		- if (port->state != SRC_READY && port->state != SNK_READY)
	1969	+ if (port->state != SRC_READY && port->state != SNK_READY) {
	1970	+ port->vdm_sm_running = false;
1773	1971	break;
	1972	+ }
1774	1973
1775	1974	/* TODO: AMS operation for Unstructured VDM */
1776	1975	if (PD_VDO_SVDM(vdo_hdr) && PD_VDO_CMDT(vdo_hdr) == CMDT_INIT) {
..	..	@@ -1779,6 +1978,9 @@
1779	1978	res = tcpm_ams_start(port, DISCOVER_IDENTITY);
1780	1979	if (res == 0)
1781	1980	port->send_discover = false;
	1981	+ else if (res == -EAGAIN)
	1982	+ mod_send_discover_delayed_work(port,
	1983	+ SEND_DISCOVER_RETRY_MS);
1782	1984	break;
1783	1985	case CMD_DISCOVER_SVID:
1784	1986	res = tcpm_ams_start(port, DISCOVER_SVIDS);
..	..	@@ -1804,7 +2006,7 @@
1804	2006	}
1805	2007
1806	2008	if (res < 0) {
1807		- port->vdm_sm_running = false;
	2009	+ port->vdm_state = VDM_STATE_ERR_BUSY;
1808	2010	return;
1809	2011	}
1810	2012	}
..	..	@@ -1820,6 +2022,7 @@
1820	2022	port->vdo_data[0] = port->vdo_retry;
1821	2023	port->vdo_count = 1;
1822	2024	port->vdm_state = VDM_STATE_READY;
	2025	+ tcpm_ams_finish(port);
1823	2026	break;
1824	2027	case VDM_STATE_BUSY:
1825	2028	port->vdm_state = VDM_STATE_ERR_TMOUT;
..	..	@@ -1888,7 +2091,7 @@
1888	2091	port->vdm_state != VDM_STATE_BUSY &&
1889	2092	port->vdm_state != VDM_STATE_SEND_MESSAGE);
1890	2093
1891		- if (port->vdm_state == VDM_STATE_ERR_TMOUT)
	2094	+ if (port->vdm_state < VDM_STATE_READY)
1892	2095	port->vdm_sm_running = false;
1893	2096
1894	2097	mutex_unlock(&port->lock);
..	..	@@ -2014,101 +2217,46 @@
2014	2217	return 0;
2015	2218	}
2016	2219
2017		-static int tcpm_altmode_enter(struct typec_altmode *altmode)
	2220	+static int tcpm_altmode_enter(struct typec_altmode altmode, u32 vdo)
2018	2221	{
2019	2222	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
	2223	+ int svdm_version;
2020	2224	u32 header;
2021		- int ret = 0;
2022	2225
2023		- mutex_lock(&port->pd_handler_lock);
2024		- if (tcpm_port_is_disconnected(port)) {
2025		- ret = -ENODEV;
2026		- goto unlock;
2027		- }
	2226	+ svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
	2227	+ if (svdm_version < 0)
	2228	+ return svdm_version;
2028	2229
2029		- mutex_lock(&port->lock);
2030		- header = VDO(altmode->svid, 1, CMD_ENTER_MODE);
	2230	+ header = VDO(altmode->svid, vdo ? 2 : 1, svdm_version, CMD_ENTER_MODE);
2031	2231	header \|= VDO_OPOS(altmode->mode);
2032	2232
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;
	2233	+ tcpm_queue_vdm_unlocked(port, header, vdo, vdo ? 1 : 0);
	2234	+ return 0;
2039	2235	}
2040	2236
2041	2237	static int tcpm_altmode_exit(struct typec_altmode *altmode)
2042	2238	{
2043	2239	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
	2240	+ int svdm_version;
2044	2241	u32 header;
2045		- int ret = 0;
2046	2242
2047		- mutex_lock(&port->pd_handler_lock);
2048		- if (tcpm_port_is_disconnected(port)) {
2049		- ret = -ENODEV;
2050		- goto unlock;
2051		- }
	2243	+ svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
	2244	+ if (svdm_version < 0)
	2245	+ return svdm_version;
2052	2246
2053		- mutex_lock(&port->lock);
2054		- header = VDO(altmode->svid, 1, CMD_EXIT_MODE);
	2247	+ header = VDO(altmode->svid, 1, svdm_version, CMD_EXIT_MODE);
2055	2248	header \|= VDO_OPOS(altmode->mode);
2056	2249
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;
	2250	+ tcpm_queue_vdm_unlocked(port, header, NULL, 0);
	2251	+ return 0;
2064	2252	}
2065	2253
2066	2254	static int tcpm_altmode_vdm(struct typec_altmode *altmode,
2067	2255	u32 header, const u32 *data, int count)
2068	2256	{
2069	2257	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
2070		- int ret = 0;
2071	2258
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		- }
	2259	+ tcpm_queue_vdm_unlocked(port, header, data, count - 1);
2112	2260
2113	2261	return 0;
2114	2262	}
..	..	@@ -2117,7 +2265,6 @@
2117	2265	.enter = tcpm_altmode_enter,
2118	2266	.exit = tcpm_altmode_exit,
2119	2267	.vdm = tcpm_altmode_vdm,
2120		- .notify = tcpm_altmode_notify,
2121	2268	};
2122	2269
2123	2270	/*
..	..	@@ -2142,20 +2289,108 @@
2142	2289
2143	2290	if (!type) {
2144	2291	tcpm_log(port, "Alert message received with no type");
	2292	+ tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
2145	2293	return;
2146	2294	}
2147	2295
2148	2296	/* Just handling non-battery alerts for now */
2149	2297	if (!(type & USB_PD_ADO_TYPE_BATT_STATUS_CHANGE)) {
2150		- switch (port->state) {
2151		- case SRC_READY:
2152		- case SNK_READY:
	2298	+ if (port->pwr_role == TYPEC_SOURCE) {
	2299	+ port->upcoming_state = GET_STATUS_SEND;
	2300	+ tcpm_ams_start(port, GETTING_SOURCE_SINK_STATUS);
	2301	+ } else {
	2302	+ /*
	2303	+ * Do not check SinkTxOk here in case the Source doesn't set its Rp to
	2304	+ * SinkTxOk in time.
	2305	+ */
	2306	+ port->ams = GETTING_SOURCE_SINK_STATUS;
2153	2307	tcpm_set_state(port, GET_STATUS_SEND, 0);
2154		- break;
2155		- default:
2156		- tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2157		- break;
2158	2308	}
	2309	+ } else {
	2310	+ tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
	2311	+ }
	2312	+}
	2313	+
	2314	+static int tcpm_set_auto_vbus_discharge_threshold(struct tcpm_port *port,
	2315	+ enum typec_pwr_opmode mode, bool pps_active,
	2316	+ u32 requested_vbus_voltage)
	2317	+{
	2318	+ int ret;
	2319	+
	2320	+ if (!port->tcpc->set_auto_vbus_discharge_threshold)
	2321	+ return 0;
	2322	+
	2323	+ ret = port->tcpc->set_auto_vbus_discharge_threshold(port->tcpc, mode, pps_active,
	2324	+ requested_vbus_voltage);
	2325	+ tcpm_log_force(port,
	2326	+ "set_auto_vbus_discharge_threshold mode:%d pps_active:%c vbus:%u ret:%d",
	2327	+ mode, pps_active ? 'y' : 'n', requested_vbus_voltage, ret);
	2328	+
	2329	+ return ret;
	2330	+}
	2331	+
	2332	+static void tcpm_pd_handle_state(struct tcpm_port *port,
	2333	+ enum tcpm_state state,
	2334	+ enum tcpm_ams ams,
	2335	+ unsigned int delay_ms)
	2336	+{
	2337	+ switch (port->state) {
	2338	+ case SRC_READY:
	2339	+ case SNK_READY:
	2340	+ port->ams = ams;
	2341	+ tcpm_set_state(port, state, delay_ms);
	2342	+ break;
	2343	+ /* 8.3.3.4.1.1 and 6.8.1 power transitioning */
	2344	+ case SNK_TRANSITION_SINK:
	2345	+ case SNK_TRANSITION_SINK_VBUS:
	2346	+ case SRC_TRANSITION_SUPPLY:
	2347	+ tcpm_set_state(port, HARD_RESET_SEND, 0);
	2348	+ break;
	2349	+ default:
	2350	+ if (!tcpm_ams_interruptible(port)) {
	2351	+ tcpm_set_state(port, port->pwr_role == TYPEC_SOURCE ?
	2352	+ SRC_SOFT_RESET_WAIT_SNK_TX :
	2353	+ SNK_SOFT_RESET,
	2354	+ 0);
	2355	+ } else {
	2356	+ /* process the Message 6.8.1 */
	2357	+ port->upcoming_state = state;
	2358	+ port->next_ams = ams;
	2359	+ tcpm_set_state(port, ready_state(port), delay_ms);
	2360	+ }
	2361	+ break;
	2362	+ }
	2363	+}
	2364	+
	2365	+static void tcpm_pd_handle_msg(struct tcpm_port *port,
	2366	+ enum pd_msg_request message,
	2367	+ enum tcpm_ams ams)
	2368	+{
	2369	+ switch (port->state) {
	2370	+ case SRC_READY:
	2371	+ case SNK_READY:
	2372	+ port->ams = ams;
	2373	+ tcpm_queue_message(port, message);
	2374	+ break;
	2375	+ /* PD 3.0 Spec 8.3.3.4.1.1 and 6.8.1 */
	2376	+ case SNK_TRANSITION_SINK:
	2377	+ case SNK_TRANSITION_SINK_VBUS:
	2378	+ case SRC_TRANSITION_SUPPLY:
	2379	+ tcpm_set_state(port, HARD_RESET_SEND, 0);
	2380	+ break;
	2381	+ default:
	2382	+ if (!tcpm_ams_interruptible(port)) {
	2383	+ tcpm_set_state(port, port->pwr_role == TYPEC_SOURCE ?
	2384	+ SRC_SOFT_RESET_WAIT_SNK_TX :
	2385	+ SNK_SOFT_RESET,
	2386	+ 0);
	2387	+ } else {
	2388	+ port->next_ams = ams;
	2389	+ tcpm_set_state(port, ready_state(port), 0);
	2390	+ /* 6.8.1 process the Message */
	2391	+ tcpm_queue_message(port, message);
	2392	+ }
	2393	+ break;
2159	2394	}
2160	2395	}
2161	2396
..	..	@@ -2166,15 +2401,18 @@
2166	2401	unsigned int cnt = pd_header_cnt_le(msg->header);
2167	2402	unsigned int rev = pd_header_rev_le(msg->header);
2168	2403	unsigned int i;
2169		- enum frs_typec_current frs_current;
	2404	+ enum frs_typec_current partner_frs_current;
2170	2405	bool frs_enable;
2171	2406	int ret;
2172	2407
	2408	+ if (tcpm_vdm_ams(port) && type != PD_DATA_VENDOR_DEF) {
	2409	+ port->vdm_state = VDM_STATE_ERR_BUSY;
	2410	+ tcpm_ams_finish(port);
	2411	+ mod_vdm_delayed_work(port, 0);
	2412	+ }
	2413	+
2173	2414	switch (type) {
2174	2415	case PD_DATA_SOURCE_CAP:
2175		- if (port->pwr_role != TYPEC_SINK)
2176		- break;
2177		-
2178	2416	for (i = 0; i < cnt; i++)
2179	2417	port->source_caps[i] = le32_to_cpu(msg->payload[i]);
2180	2418
..	..	@@ -2185,17 +2423,34 @@
2185	2423	tcpm_validate_caps(port, port->source_caps,
2186	2424	port->nr_source_caps);
2187	2425
	2426	+ trace_android_vh_typec_store_partner_src_caps(port, &port->nr_source_caps,
	2427	+ &port->source_caps);
	2428	+
2188	2429	/*
2189	2430	* Adjust revision in subsequent message headers, as required,
2190	2431	* to comply with 6.2.1.1.5 of the USB PD 3.0 spec. We don't
2191	2432	* support Rev 1.0 so just do nothing in that scenario.
2192	2433	*/
2193		- if (rev == PD_REV10)
	2434	+ if (rev == PD_REV10) {
	2435	+ if (port->ams == GET_SOURCE_CAPABILITIES)
	2436	+ tcpm_ams_finish(port);
2194	2437	break;
	2438	+ }
2195	2439
2196	2440	if (rev < PD_MAX_REV)
2197		- port->negotiated_rev = rev;
	2441	+ port->negotiated_rev = min_t(u16, rev, port->negotiated_rev);
2198	2442
	2443	+ if (port->pwr_role == TYPEC_SOURCE) {
	2444	+ if (port->ams == GET_SOURCE_CAPABILITIES)
	2445	+ tcpm_pd_handle_state(port, SRC_READY, NONE_AMS, 0);
	2446	+ /* Unexpected Source Capabilities */
	2447	+ else
	2448	+ tcpm_pd_handle_msg(port,
	2449	+ port->negotiated_rev < PD_REV30 ?
	2450	+ PD_MSG_CTRL_REJECT :
	2451	+ PD_MSG_CTRL_NOT_SUPP,
	2452	+ NONE_AMS);
	2453	+ } else if (port->state == SNK_WAIT_CAPABILITIES) {
2199	2454	/*
2200	2455	* This message may be received even if VBUS is not
2201	2456	* present. This is quite unexpected; see USB PD
..	..	@@ -2209,48 +2464,68 @@
2209	2464	* but be prepared to keep waiting for VBUS after it was
2210	2465	* handled.
2211	2466	*/
2212		- tcpm_set_state(port, SNK_NEGOTIATE_CAPABILITIES, 0);
	2467	+ port->ams = POWER_NEGOTIATION;
	2468	+ port->in_ams = true;
	2469	+ tcpm_set_state(port, SNK_NEGOTIATE_CAPABILITIES, 0);
	2470	+ } else {
	2471	+ if (port->ams == GET_SOURCE_CAPABILITIES)
	2472	+ tcpm_ams_finish(port);
	2473	+ tcpm_pd_handle_state(port, SNK_NEGOTIATE_CAPABILITIES,
	2474	+ POWER_NEGOTIATION, 0);
	2475	+ }
2213	2476	break;
2214	2477	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		-
2221	2478	/*
2222	2479	* Adjust revision in subsequent message headers, as required,
2223	2480	* to comply with 6.2.1.1.5 of the USB PD 3.0 spec. We don't
2224	2481	* support Rev 1.0 so just reject in that scenario.
2225	2482	*/
2226	2483	if (rev == PD_REV10) {
2227		- tcpm_queue_message(port, PD_MSG_CTRL_REJECT);
	2484	+ tcpm_pd_handle_msg(port,
	2485	+ port->negotiated_rev < PD_REV30 ?
	2486	+ PD_MSG_CTRL_REJECT :
	2487	+ PD_MSG_CTRL_NOT_SUPP,
	2488	+ NONE_AMS);
2228	2489	break;
2229	2490	}
2230	2491
2231	2492	if (rev < PD_MAX_REV)
2232		- port->negotiated_rev = rev;
	2493	+ port->negotiated_rev = min_t(u16, rev, port->negotiated_rev);
	2494	+
	2495	+ if (port->pwr_role != TYPEC_SOURCE \|\| cnt != 1) {
	2496	+ tcpm_pd_handle_msg(port,
	2497	+ port->negotiated_rev < PD_REV30 ?
	2498	+ PD_MSG_CTRL_REJECT :
	2499	+ PD_MSG_CTRL_NOT_SUPP,
	2500	+ NONE_AMS);
	2501	+ break;
	2502	+ }
2233	2503
2234	2504	port->sink_request = le32_to_cpu(msg->payload[0]);
2235	2505
2236	2506	if (port->vdm_sm_running && port->explicit_contract) {
2237		- tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
	2507	+ tcpm_pd_handle_msg(port, PD_MSG_CTRL_WAIT, port->ams);
2238	2508	break;
2239	2509	}
2240	2510
2241		- tcpm_set_state(port, SRC_NEGOTIATE_CAPABILITIES, 0);
	2511	+ if (port->state == SRC_SEND_CAPABILITIES)
	2512	+ tcpm_set_state(port, SRC_NEGOTIATE_CAPABILITIES, 0);
	2513	+ else
	2514	+ tcpm_pd_handle_state(port, SRC_NEGOTIATE_CAPABILITIES,
	2515	+ POWER_NEGOTIATION, 0);
2242	2516	break;
2243	2517	case PD_DATA_SINK_CAP:
2244	2518	/* We don't do anything with this at the moment... */
2245	2519	for (i = 0; i < cnt; i++)
2246	2520	port->sink_caps[i] = le32_to_cpu(msg->payload[i]);
2247	2521
2248		- frs_current = (port->sink_caps[0] & PDO_FIXED_FRS_CURR_MASK) >>
	2522	+ partner_frs_current = (port->sink_caps[0] & PDO_FIXED_FRS_CURR_MASK) >>
2249	2523	PDO_FIXED_FRS_CURR_SHIFT;
2250		- frs_enable = frs_current && (frs_current <= port->frs_current);
	2524	+ frs_enable = partner_frs_current && (partner_frs_current <=
	2525	+ port->new_source_frs_current);
2251	2526	tcpm_log(port,
2252	2527	"Port partner FRS capable partner_frs_current:%u port_frs_current:%u enable:%c",
2253		- frs_current, port->frs_current, frs_enable ? 'y' : 'n');
	2528	+ partner_frs_current, port->new_source_frs_current, frs_enable ? 'y' : 'n');
2254	2529	if (frs_enable) {
2255	2530	ret = port->tcpc->enable_frs(port->tcpc, true);
2256	2531	tcpm_log(port, "Enable FRS %s, ret:%d\n", ret ? "fail" : "success", ret);
..	..	@@ -2258,27 +2533,45 @@
2258	2533
2259	2534	port->nr_sink_caps = cnt;
2260	2535	port->sink_cap_done = true;
2261		- tcpm_set_state(port, SNK_READY, 0);
	2536	+ if (port->ams == GET_SINK_CAPABILITIES)
	2537	+ tcpm_set_state(port, ready_state(port), 0);
	2538	+ /* Unexpected Sink Capabilities */
	2539	+ else
	2540	+ tcpm_pd_handle_msg(port,
	2541	+ port->negotiated_rev < PD_REV30 ?
	2542	+ PD_MSG_CTRL_REJECT :
	2543	+ PD_MSG_CTRL_NOT_SUPP,
	2544	+ NONE_AMS);
2262	2545	break;
2263	2546	case PD_DATA_VENDOR_DEF:
2264	2547	tcpm_handle_vdm_request(port, msg->payload, cnt);
2265	2548	break;
2266	2549	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		- }
	2550	+ port->bist_request = le32_to_cpu(msg->payload[0]);
	2551	+ tcpm_pd_handle_state(port, BIST_RX, BIST, 0);
2271	2552	break;
2272	2553	case PD_DATA_ALERT:
2273		- tcpm_handle_alert(port, msg->payload, cnt);
	2554	+ if (port->state != SRC_READY && port->state != SNK_READY)
	2555	+ tcpm_pd_handle_state(port, port->pwr_role == TYPEC_SOURCE ?
	2556	+ SRC_SOFT_RESET_WAIT_SNK_TX : SNK_SOFT_RESET,
	2557	+ NONE_AMS, 0);
	2558	+ else
	2559	+ tcpm_handle_alert(port, msg->payload, cnt);
2274	2560	break;
2275	2561	case PD_DATA_BATT_STATUS:
2276	2562	case PD_DATA_GET_COUNTRY_INFO:
2277	2563	/* Currently unsupported */
2278		- tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
	2564	+ tcpm_pd_handle_msg(port, port->negotiated_rev < PD_REV30 ?
	2565	+ PD_MSG_CTRL_REJECT :
	2566	+ PD_MSG_CTRL_NOT_SUPP,
	2567	+ NONE_AMS);
2279	2568	break;
2280	2569	default:
2281		- tcpm_log(port, "Unhandled data message type %#x", type);
	2570	+ tcpm_pd_handle_msg(port, port->negotiated_rev < PD_REV30 ?
	2571	+ PD_MSG_CTRL_REJECT :
	2572	+ PD_MSG_CTRL_NOT_SUPP,
	2573	+ NONE_AMS);
	2574	+ tcpm_log(port, "Unrecognized data message type %#x", type);
2282	2575	break;
2283	2576	}
2284	2577	}
..	..	@@ -2298,31 +2591,25 @@
2298	2591	enum pd_ctrl_msg_type type = pd_header_type_le(msg->header);
2299	2592	enum tcpm_state next_state;
2300	2593
	2594	+ /*
	2595	+ * Stop VDM state machine if interrupted by other Messages while NOT_SUPP is allowed in
	2596	+ * VDM AMS if waiting for VDM responses and will be handled later.
	2597	+ */
	2598	+ if (tcpm_vdm_ams(port) && type != PD_CTRL_NOT_SUPP && type != PD_CTRL_GOOD_CRC) {
	2599	+ port->vdm_state = VDM_STATE_ERR_BUSY;
	2600	+ tcpm_ams_finish(port);
	2601	+ mod_vdm_delayed_work(port, 0);
	2602	+ }
	2603	+
2301	2604	switch (type) {
2302	2605	case PD_CTRL_GOOD_CRC:
2303	2606	case PD_CTRL_PING:
2304	2607	break;
2305	2608	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		- }
	2609	+ tcpm_pd_handle_msg(port, PD_MSG_DATA_SOURCE_CAP, GET_SOURCE_CAPABILITIES);
2315	2610	break;
2316	2611	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		- }
	2612	+ tcpm_pd_handle_msg(port, PD_MSG_DATA_SINK_CAP, GET_SINK_CAPABILITIES);
2326	2613	break;
2327	2614	case PD_CTRL_GOTO_MIN:
2328	2615	break;
..	..	@@ -2331,13 +2618,13 @@
2331	2618	case SNK_TRANSITION_SINK:
2332	2619	if (port->vbus_present) {
2333	2620	tcpm_set_current_limit(port,
2334		- port->current_limit,
2335		- port->supply_voltage);
	2621	+ port->req_current_limit,
	2622	+ port->req_supply_voltage);
2336	2623	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;
	2624	+ tcpm_set_auto_vbus_discharge_threshold(port,
	2625	+ TYPEC_PWR_MODE_PD,
	2626	+ port->pps_data.active,
	2627	+ port->supply_voltage);
2341	2628	tcpm_set_state(port, SNK_READY, 0);
2342	2629	} else {
2343	2630	/*
..	..	@@ -2361,6 +2648,11 @@
2361	2648	tcpm_set_state(port, FR_SWAP_SNK_SRC_NEW_SINK_READY, 0);
2362	2649	break;
2363	2650	default:
	2651	+ tcpm_pd_handle_state(port,
	2652	+ port->pwr_role == TYPEC_SOURCE ?
	2653	+ SRC_SOFT_RESET_WAIT_SNK_TX :
	2654	+ SNK_SOFT_RESET,
	2655	+ NONE_AMS, 0);
2364	2656	break;
2365	2657	}
2366	2658	break;
..	..	@@ -2370,26 +2662,28 @@
2370	2662	switch (port->state) {
2371	2663	case SNK_NEGOTIATE_CAPABILITIES:
2372	2664	/* USB PD specification, Figure 8-43 */
2373		- if (port->explicit_contract) {
	2665	+ if (port->explicit_contract)
2374	2666	next_state = SNK_READY;
2375		- if (port->data_role == TYPEC_HOST &&
2376		- port->send_discover)
2377		- port->vdm_sm_running = true;
2378		- } else {
	2667	+ else
2379	2668	next_state = SNK_WAIT_CAPABILITIES;
2380		- }
	2669	+
	2670	+ /* Threshold was relaxed before sending Request. Restore it back. */
	2671	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
	2672	+ port->pps_data.active,
	2673	+ port->supply_voltage);
2381	2674	tcpm_set_state(port, next_state, 0);
2382	2675	break;
2383	2676	case SNK_NEGOTIATE_PPS_CAPABILITIES:
2384	2677	/* 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;
	2678	+ port->pps_data.req_out_volt = port->supply_voltage;
	2679	+ port->pps_data.req_op_curr = port->current_limit;
2387	2680	port->pps_status = (type == PD_CTRL_WAIT ?
2388	2681	-EAGAIN : -EOPNOTSUPP);
2389	2682
2390		- if (port->data_role == TYPEC_HOST &&
2391		- port->send_discover)
2392		- port->vdm_sm_running = true;
	2683	+ /* Threshold was relaxed before sending Request. Restore it back. */
	2684	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
	2685	+ port->pps_data.active,
	2686	+ port->supply_voltage);
2393	2687
2394	2688	tcpm_set_state(port, SNK_READY, 0);
2395	2689	break;
..	..	@@ -2415,7 +2709,22 @@
2415	2709	port->sink_cap_done = true;
2416	2710	tcpm_set_state(port, ready_state(port), 0);
2417	2711	break;
	2712	+ case SRC_READY:
	2713	+ case SNK_READY:
	2714	+ if (port->vdm_state > VDM_STATE_READY) {
	2715	+ port->vdm_state = VDM_STATE_DONE;
	2716	+ if (tcpm_vdm_ams(port))
	2717	+ tcpm_ams_finish(port);
	2718	+ mod_vdm_delayed_work(port, 0);
	2719	+ break;
	2720	+ }
	2721	+ fallthrough;
2418	2722	default:
	2723	+ tcpm_pd_handle_state(port,
	2724	+ port->pwr_role == TYPEC_SOURCE ?
	2725	+ SRC_SOFT_RESET_WAIT_SNK_TX :
	2726	+ SNK_SOFT_RESET,
	2727	+ NONE_AMS, 0);
2419	2728	break;
2420	2729	}
2421	2730	break;
..	..	@@ -2427,13 +2736,15 @@
2427	2736	break;
2428	2737	case SNK_NEGOTIATE_PPS_CAPABILITIES:
2429	2738	port->pps_data.active = true;
2430		- port->supply_voltage = port->pps_data.out_volt;
2431		- port->current_limit = port->pps_data.op_curr;
	2739	+ port->pps_data.min_volt = port->pps_data.req_min_volt;
	2740	+ port->pps_data.max_volt = port->pps_data.req_max_volt;
	2741	+ port->pps_data.max_curr = port->pps_data.req_max_curr;
	2742	+ port->req_supply_voltage = port->pps_data.req_out_volt;
	2743	+ port->req_current_limit = port->pps_data.req_op_curr;
	2744	+ power_supply_changed(port->psy);
2432	2745	tcpm_set_state(port, SNK_TRANSITION_SINK, 0);
2433	2746	break;
2434	2747	case SOFT_RESET_SEND:
2435		- port->message_id = 0;
2436		- port->rx_msgid = -1;
2437	2748	if (port->ams == SOFT_RESET_AMS)
2438	2749	tcpm_ams_finish(port);
2439	2750	if (port->pwr_role == TYPEC_SOURCE) {
..	..	@@ -2444,10 +2755,6 @@
2444	2755	}
2445	2756	break;
2446	2757	case DR_SWAP_SEND:
2447		- if (port->data_role == TYPEC_DEVICE &&
2448		- port->send_discover)
2449		- port->vdm_sm_running = true;
2450		-
2451	2758	tcpm_set_state(port, DR_SWAP_CHANGE_DR, 0);
2452	2759	break;
2453	2760	case PR_SWAP_SEND:
..	..	@@ -2460,69 +2767,62 @@
2460	2767	tcpm_set_state(port, FR_SWAP_SNK_SRC_TRANSITION_TO_OFF, 0);
2461	2768	break;
2462	2769	default:
	2770	+ tcpm_pd_handle_state(port,
	2771	+ port->pwr_role == TYPEC_SOURCE ?
	2772	+ SRC_SOFT_RESET_WAIT_SNK_TX :
	2773	+ SNK_SOFT_RESET,
	2774	+ NONE_AMS, 0);
2463	2775	break;
2464	2776	}
2465	2777	break;
2466	2778	case PD_CTRL_SOFT_RESET:
	2779	+ port->ams = SOFT_RESET_AMS;
2467	2780	tcpm_set_state(port, SOFT_RESET, 0);
2468	2781	break;
2469	2782	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		- }
2474	2783	/*
2475	2784	* XXX
2476	2785	* 6.3.9: If an alternate mode is active, a request to swap
2477	2786	* alternate modes shall trigger a port reset.
2478	2787	*/
2479		- switch (port->state) {
2480		- case SRC_READY:
2481		- case SNK_READY:
2482		- if (port->vdm_sm_running) {
	2788	+ if (port->typec_caps.data != TYPEC_PORT_DRD) {
	2789	+ tcpm_pd_handle_msg(port,
	2790	+ port->negotiated_rev < PD_REV30 ?
	2791	+ PD_MSG_CTRL_REJECT :
	2792	+ PD_MSG_CTRL_NOT_SUPP,
	2793	+ NONE_AMS);
	2794	+ } else {
	2795	+ if (port->send_discover) {
2483	2796	tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2484	2797	break;
2485	2798	}
2486		- tcpm_set_state(port, DR_SWAP_ACCEPT, 0);
2487		- break;
2488		- default:
2489		- tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2490		- break;
	2799	+
	2800	+ tcpm_pd_handle_state(port, DR_SWAP_ACCEPT, DATA_ROLE_SWAP, 0);
2491	2801	}
2492	2802	break;
2493	2803	case PD_CTRL_PR_SWAP:
2494	2804	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) {
	2805	+ tcpm_pd_handle_msg(port,
	2806	+ port->negotiated_rev < PD_REV30 ?
	2807	+ PD_MSG_CTRL_REJECT :
	2808	+ PD_MSG_CTRL_NOT_SUPP,
	2809	+ NONE_AMS);
	2810	+ } else {
	2811	+ if (port->send_discover) {
2502	2812	tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2503	2813	break;
2504	2814	}
2505		- tcpm_set_state(port, PR_SWAP_ACCEPT, 0);
2506		- break;
2507		- default:
2508		- tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2509		- break;
	2815	+
	2816	+ tcpm_pd_handle_state(port, PR_SWAP_ACCEPT, POWER_ROLE_SWAP, 0);
2510	2817	}
2511	2818	break;
2512	2819	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:
	2820	+ if (port->send_discover) {
2523	2821	tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2524	2822	break;
2525	2823	}
	2824	+
	2825	+ tcpm_pd_handle_state(port, VCONN_SWAP_ACCEPT, VCONN_SWAP, 0);
2526	2826	break;
2527	2827	case PD_CTRL_GET_SOURCE_CAP_EXT:
2528	2828	case PD_CTRL_GET_STATUS:
..	..	@@ -2530,10 +2830,19 @@
2530	2830	case PD_CTRL_GET_PPS_STATUS:
2531	2831	case PD_CTRL_GET_COUNTRY_CODES:
2532	2832	/* Currently not supported */
2533		- tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
	2833	+ tcpm_pd_handle_msg(port,
	2834	+ port->negotiated_rev < PD_REV30 ?
	2835	+ PD_MSG_CTRL_REJECT :
	2836	+ PD_MSG_CTRL_NOT_SUPP,
	2837	+ NONE_AMS);
2534	2838	break;
2535	2839	default:
2536		- tcpm_log(port, "Unhandled ctrl message type %#x", type);
	2840	+ tcpm_pd_handle_msg(port,
	2841	+ port->negotiated_rev < PD_REV30 ?
	2842	+ PD_MSG_CTRL_REJECT :
	2843	+ PD_MSG_CTRL_NOT_SUPP,
	2844	+ NONE_AMS);
	2845	+ tcpm_log(port, "Unrecognized ctrl message type %#x", type);
2537	2846	break;
2538	2847	}
2539	2848	}
..	..	@@ -2544,34 +2853,37 @@
2544	2853	enum pd_ext_msg_type type = pd_header_type_le(msg->header);
2545	2854	unsigned int data_size = pd_ext_header_data_size_le(msg->ext_msg.header);
2546	2855
2547		- if (!(msg->ext_msg.header & PD_EXT_HDR_CHUNKED)) {
	2856	+ /* stopping VDM state machine if interrupted by other Messages */
	2857	+ if (tcpm_vdm_ams(port)) {
	2858	+ port->vdm_state = VDM_STATE_ERR_BUSY;
	2859	+ tcpm_ams_finish(port);
	2860	+ mod_vdm_delayed_work(port, 0);
	2861	+ }
	2862	+
	2863	+ if (!(le16_to_cpu(msg->ext_msg.header) & PD_EXT_HDR_CHUNKED)) {
	2864	+ tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
2548	2865	tcpm_log(port, "Unchunked extended messages unsupported");
2549	2866	return;
2550	2867	}
2551	2868
2552	2869	if (data_size > PD_EXT_MAX_CHUNK_DATA) {
	2870	+ tcpm_pd_handle_state(port, CHUNK_NOT_SUPP, NONE_AMS, PD_T_CHUNK_NOT_SUPP);
2553	2871	tcpm_log(port, "Chunk handling not yet supported");
2554	2872	return;
2555	2873	}
2556	2874
2557	2875	switch (type) {
2558	2876	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;
2569	2877	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);
	2878	+ if (port->ams == GETTING_SOURCE_SINK_STATUS) {
	2879	+ tcpm_ams_finish(port);
	2880	+ tcpm_set_state(port, ready_state(port), 0);
	2881	+ } else {
	2882	+ /* unexpected Status or PPS_Status Message */
	2883	+ tcpm_pd_handle_state(port, port->pwr_role == TYPEC_SOURCE ?
	2884	+ SRC_SOFT_RESET_WAIT_SNK_TX : SNK_SOFT_RESET,
	2885	+ NONE_AMS, 0);
	2886	+ }
2575	2887	break;
2576	2888	case PD_EXT_SOURCE_CAP_EXT:
2577	2889	case PD_EXT_GET_BATT_CAP:
..	..	@@ -2585,10 +2897,11 @@
2585	2897	case PD_EXT_FW_UPDATE_RESPONSE:
2586	2898	case PD_EXT_COUNTRY_INFO:
2587	2899	case PD_EXT_COUNTRY_CODES:
2588		- tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
	2900	+ tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
2589	2901	break;
2590	2902	default:
2591		- tcpm_log(port, "Unhandled extended message type %#x", type);
	2903	+ tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
	2904	+ tcpm_log(port, "Unrecognized extended message type %#x", type);
2592	2905	break;
2593	2906	}
2594	2907	}
..	..	@@ -2633,7 +2946,7 @@
2633	2946	"Data role mismatch, initiating error recovery");
2634	2947	tcpm_set_state(port, ERROR_RECOVERY, 0);
2635	2948	} else {
2636		- if (msg->header & PD_HEADER_EXT_HDR)
	2949	+ if (le16_to_cpu(msg->header) & PD_HEADER_EXT_HDR)
2637	2950	tcpm_pd_ext_msg_request(port, msg);
2638	2951	else if (cnt)
2639	2952	tcpm_pd_data_request(port, msg);
..	..	@@ -2684,6 +2997,7 @@
2684	2997	static bool tcpm_send_queued_message(struct tcpm_port *port)
2685	2998	{
2686	2999	enum pd_msg_request queued_message;
	3000	+ int ret;
2687	3001
2688	3002	do {
2689	3003	queued_message = port->queued_message;
..	..	@@ -2700,10 +3014,27 @@
2700	3014	tcpm_pd_send_control(port, PD_CTRL_NOT_SUPP);
2701	3015	break;
2702	3016	case PD_MSG_DATA_SINK_CAP:
2703		- tcpm_pd_send_sink_caps(port);
	3017	+ ret = tcpm_pd_send_sink_caps(port);
	3018	+ if (ret < 0) {
	3019	+ tcpm_log(port, "Unable to send snk caps, ret=%d", ret);
	3020	+ tcpm_set_state(port, SNK_SOFT_RESET, 0);
	3021	+ }
	3022	+ tcpm_ams_finish(port);
2704	3023	break;
2705	3024	case PD_MSG_DATA_SOURCE_CAP:
2706		- tcpm_pd_send_source_caps(port);
	3025	+ ret = tcpm_pd_send_source_caps(port);
	3026	+ if (ret < 0) {
	3027	+ tcpm_log(port,
	3028	+ "Unable to send src caps, ret=%d",
	3029	+ ret);
	3030	+ tcpm_set_state(port, SOFT_RESET_SEND, 0);
	3031	+ } else if (port->pwr_role == TYPEC_SOURCE) {
	3032	+ tcpm_ams_finish(port);
	3033	+ tcpm_set_state(port, HARD_RESET_SEND,
	3034	+ PD_T_SENDER_RESPONSE);
	3035	+ } else {
	3036	+ tcpm_ams_finish(port);
	3037	+ }
2707	3038	break;
2708	3039	default:
2709	3040	break;
..	..	@@ -2791,6 +3122,7 @@
2791	3122
2792	3123	port->pps_data.supported = false;
2793	3124	port->usb_type = POWER_SUPPLY_USB_TYPE_PD;
	3125	+ power_supply_changed(port->psy);
2794	3126
2795	3127	/*
2796	3128	* Select the source PDO providing the most power which has a
..	..	@@ -2815,6 +3147,7 @@
2815	3147	port->pps_data.supported = true;
2816	3148	port->usb_type =
2817	3149	POWER_SUPPLY_USB_TYPE_PD_PPS;
	3150	+ power_supply_changed(port->psy);
2818	3151	}
2819	3152	continue;
2820	3153	default:
..	..	@@ -2884,7 +3217,8 @@
2884	3217	unsigned int i, j, max_mw = 0, max_mv = 0;
2885	3218	unsigned int min_src_mv, max_src_mv, src_ma, src_mw;
2886	3219	unsigned int min_snk_mv, max_snk_mv;
2887		- u32 pdo;
	3220	+ unsigned int max_op_mv;
	3221	+ u32 pdo, src, snk;
2888	3222	unsigned int src_pdo = 0, snk_pdo = 0;
2889	3223
2890	3224	/*
..	..	@@ -2934,16 +3268,18 @@
2934	3268	continue;
2935	3269	}
2936	3270
2937		- if (max_src_mv <= max_snk_mv &&
2938		- min_src_mv >= min_snk_mv) {
	3271	+ if (min_src_mv <= max_snk_mv &&
	3272	+ max_src_mv >= min_snk_mv) {
	3273	+ max_op_mv = min(max_src_mv, max_snk_mv);
	3274	+ src_mw = (max_op_mv * src_ma) / 1000;
2939	3275	/* Prefer higher voltages if available */
2940	3276	if ((src_mw == max_mw &&
2941		- min_src_mv > max_mv) \|\|
	3277	+ max_op_mv > max_mv) \|\|
2942	3278	src_mw > max_mw) {
2943	3279	src_pdo = i;
2944	3280	snk_pdo = j;
2945	3281	max_mw = src_mw;
2946		- max_mv = max_src_mv;
	3282	+ max_mv = max_op_mv;
2947	3283	}
2948	3284	}
2949	3285	}
..	..	@@ -2956,16 +3292,19 @@
2956	3292	}
2957	3293
2958	3294	if (src_pdo) {
2959		- pdo = port->source_caps[src_pdo];
	3295	+ src = port->source_caps[src_pdo];
	3296	+ snk = port->snk_pdo[snk_pdo];
2960	3297
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);
	3298	+ port->pps_data.req_min_volt = max(pdo_pps_apdo_min_voltage(src),
	3299	+ pdo_pps_apdo_min_voltage(snk));
	3300	+ port->pps_data.req_max_volt = min(pdo_pps_apdo_max_voltage(src),
	3301	+ pdo_pps_apdo_max_voltage(snk));
	3302	+ port->pps_data.req_max_curr = min_pps_apdo_current(src, snk);
	3303	+ port->pps_data.req_out_volt = min(port->pps_data.req_max_volt,
	3304	+ max(port->pps_data.req_min_volt,
	3305	+ port->pps_data.req_out_volt));
	3306	+ port->pps_data.req_op_curr = min(port->pps_data.req_max_curr,
	3307	+ port->pps_data.req_op_curr);
2969	3308	}
2970	3309
2971	3310	return src_pdo;
..	..	@@ -3045,8 +3384,8 @@
3045	3384	flags & RDO_CAP_MISMATCH ? " [mismatch]" : "");
3046	3385	}
3047	3386
3048		- port->current_limit = ma;
3049		- port->supply_voltage = mv;
	3387	+ port->req_current_limit = ma;
	3388	+ port->req_supply_voltage = mv;
3050	3389
3051	3390	return 0;
3052	3391	}
..	..	@@ -3060,6 +3399,12 @@
3060	3399	ret = tcpm_pd_build_request(port, &rdo);
3061	3400	if (ret < 0)
3062	3401	return ret;
	3402	+
	3403	+ /*
	3404	+ * Relax the threshold as voltage will be adjusted after Accept Message plus tSrcTransition.
	3405	+ * It is safer to modify the threshold here.
	3406	+ */
	3407	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, 0);
3063	3408
3064	3409	memset(&msg, 0, sizeof(msg));
3065	3410	msg.header = PD_HEADER_LE(PD_DATA_REQUEST,
..	..	@@ -3092,10 +3437,10 @@
3092	3437	tcpm_log(port, "Invalid APDO selected!");
3093	3438	return -EINVAL;
3094	3439	}
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;
	3440	+ max_mv = port->pps_data.req_max_volt;
	3441	+ max_ma = port->pps_data.req_max_curr;
	3442	+ out_mv = port->pps_data.req_out_volt;
	3443	+ op_ma = port->pps_data.req_op_curr;
3099	3444	break;
3100	3445	default:
3101	3446	tcpm_log(port, "Invalid PDO selected!");
..	..	@@ -3142,8 +3487,8 @@
3142	3487	tcpm_log(port, "Requesting APDO %d: %u mV, %u mA",
3143	3488	src_pdo_index, out_mv, op_ma);
3144	3489
3145		- port->pps_data.op_curr = op_ma;
3146		- port->pps_data.out_volt = out_mv;
	3490	+ port->pps_data.req_op_curr = op_ma;
	3491	+ port->pps_data.req_out_volt = out_mv;
3147	3492
3148	3493	return 0;
3149	3494	}
..	..	@@ -3157,6 +3502,9 @@
3157	3502	ret = tcpm_pd_build_pps_request(port, &rdo);
3158	3503	if (ret < 0)
3159	3504	return ret;
	3505	+
	3506	+ /* Relax the threshold as voltage will be adjusted right after Accept Message. */
	3507	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, 0);
3160	3508
3161	3509	memset(&msg, 0, sizeof(msg));
3162	3510	msg.header = PD_HEADER_LE(PD_DATA_REQUEST,
..	..	@@ -3178,10 +3526,6 @@
3178	3526
3179	3527	tcpm_log(port, "vbus:=%d charge=%d", enable, port->vbus_charge);
3180	3528
3181		- ret = tcpm_send_vbus_notify(port, enable);
3182		- if (ret)
3183		- return ret;
3184		-
3185	3529	ret = port->tcpc->set_vbus(port->tcpc, enable, port->vbus_charge);
3186	3530	if (ret < 0)
3187	3531	return ret;
..	..	@@ -3199,17 +3543,13 @@
3199	3543
3200	3544	if (charge != port->vbus_charge) {
3201	3545	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		-
3207	3546	ret = port->tcpc->set_vbus(port->tcpc, port->vbus_source,
3208	3547	charge);
3209	3548	if (ret < 0)
3210	3549	return ret;
3211	3550	}
3212	3551	port->vbus_charge = charge;
	3552	+ power_supply_changed(port->psy);
3213	3553	return 0;
3214	3554	}
3215	3555
..	..	@@ -3228,10 +3568,6 @@
3228	3568	static int tcpm_init_vbus(struct tcpm_port *port)
3229	3569	{
3230	3570	int ret;
3231		-
3232		- ret = tcpm_send_vbus_notify(port, false);
3233		- if (ret)
3234		- return ret;
3235	3571
3236	3572	ret = port->tcpc->set_vbus(port->tcpc, false, false);
3237	3573	port->vbus_source = false;
..	..	@@ -3280,7 +3616,9 @@
3280	3616	if (ret < 0)
3281	3617	return ret;
3282	3618
3283		- ret = tcpm_set_roles(port, true, TYPEC_SOURCE, TYPEC_HOST);
	3619	+ tcpm_enable_auto_vbus_discharge(port, true);
	3620	+
	3621	+ ret = tcpm_set_roles(port, true, TYPEC_SOURCE, tcpm_data_role_for_source(port));
3284	3622	if (ret < 0)
3285	3623	return ret;
3286	3624
..	..	@@ -3309,12 +3647,8 @@
3309	3647	port->partner = NULL;
3310	3648
3311	3649	port->attached = true;
	3650	+ port->debouncing = false;
3312	3651	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);
3318	3652
3319	3653	return 0;
3320	3654
..	..	@@ -3350,26 +3684,26 @@
3350	3684	memset(modep, 0, sizeof(*modep));
3351	3685	}
3352	3686
	3687	+static void tcpm_set_partner_usb_comm_capable(struct tcpm_port *port, bool capable)
	3688	+{
	3689	+ tcpm_log(port, "Setting usb_comm capable %s", capable ? "true" : "false");
	3690	+
	3691	+ if (port->tcpc->set_partner_usb_comm_capable)
	3692	+ port->tcpc->set_partner_usb_comm_capable(port->tcpc, capable);
	3693	+}
	3694	+
3353	3695	static void tcpm_reset_port(struct tcpm_port *port)
3354	3696	{
	3697	+ tcpm_enable_auto_vbus_discharge(port, false);
3355	3698	port->in_ams = false;
3356	3699	port->ams = NONE_AMS;
3357	3700	port->vdm_sm_running = false;
3358	3701	tcpm_unregister_altmodes(port);
3359	3702	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		- }
	3703	+ port->attached = false;
3371	3704	port->pd_capable = false;
3372	3705	port->pps_data.supported = false;
	3706	+ tcpm_set_partner_usb_comm_capable(port, false);
3373	3707
3374	3708	/*
3375	3709	* First Rx ID should be 0; set this to a sentinel of -1 so that
..	..	@@ -3388,12 +3722,11 @@
3388	3722	port->try_src_count = 0;
3389	3723	port->try_snk_count = 0;
3390	3724	port->usb_type = POWER_SUPPLY_USB_TYPE_C;
	3725	+ power_supply_changed(port->psy);
3391	3726	port->nr_sink_caps = 0;
3392	3727	port->sink_cap_done = false;
3393	3728	if (port->tcpc->enable_frs)
3394	3729	port->tcpc->enable_frs(port->tcpc, false);
3395		-
3396		- power_supply_changed(port->psy);
3397	3730	}
3398	3731
3399	3732	static void tcpm_detach(struct tcpm_port *port)
..	..	@@ -3403,6 +3736,11 @@
3403	3736
3404	3737	if (!port->attached)
3405	3738	return;
	3739	+
	3740	+ if (port->tcpc->set_bist_data) {
	3741	+ tcpm_log(port, "disable BIST MODE TESTDATA");
	3742	+ port->tcpc->set_bist_data(port->tcpc, false);
	3743	+ }
3406	3744
3407	3745	tcpm_reset_port(port);
3408	3746	}
..	..	@@ -3424,7 +3762,9 @@
3424	3762	if (ret < 0)
3425	3763	return ret;
3426	3764
3427		- ret = tcpm_set_roles(port, true, TYPEC_SINK, TYPEC_DEVICE);
	3765	+ tcpm_enable_auto_vbus_discharge(port, true);
	3766	+
	3767	+ ret = tcpm_set_roles(port, true, TYPEC_SINK, tcpm_data_role_for_sink(port));
3428	3768	if (ret < 0)
3429	3769	return ret;
3430	3770
..	..	@@ -3433,12 +3773,8 @@
3433	3773	port->partner = NULL;
3434	3774
3435	3775	port->attached = true;
	3776	+ port->debouncing = false;
3436	3777	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);
3442	3778
3443	3779	return 0;
3444	3780	}
..	..	@@ -3455,7 +3791,8 @@
3455	3791	if (port->attached)
3456	3792	return 0;
3457	3793
3458		- ret = tcpm_set_roles(port, true, TYPEC_SOURCE, TYPEC_HOST);
	3794	+ ret = tcpm_set_roles(port, true, TYPEC_SOURCE,
	3795	+ tcpm_data_role_for_source(port));
3459	3796	if (ret < 0)
3460	3797	return ret;
3461	3798
..	..	@@ -3464,10 +3801,7 @@
3464	3801	tcpm_typec_connect(port);
3465	3802
3466	3803	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);
	3804	+ port->debouncing = false;
3471	3805
3472	3806	return 0;
3473	3807	}
..	..	@@ -3504,14 +3838,15 @@
3504	3838	return SNK_UNATTACHED;
3505	3839	}
3506	3840
3507		-static void tcpm_check_send_discover(struct tcpm_port *port)
	3841	+bool tcpm_is_toggling(struct tcpm_port *port)
3508	3842	{
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		- }
	3843	+ if (port->port_type == TYPEC_PORT_DRP)
	3844	+ return port->state == SRC_UNATTACHED \|\| port->state == SNK_UNATTACHED \|\|
	3845	+ port->state == TOGGLING;
	3846	+
	3847	+ return false;
3514	3848	}
	3849	+EXPORT_SYMBOL_GPL(tcpm_is_toggling);
3515	3850
3516	3851	static void tcpm_swap_complete(struct tcpm_port *port, int result)
3517	3852	{
..	..	@@ -3540,8 +3875,11 @@
3540	3875	{
3541	3876	int ret;
3542	3877	enum typec_pwr_opmode opmode;
3543		- unsigned int msecs;
	3878	+ unsigned int msecs, timer_val_msecs;
3544	3879	enum tcpm_state upcoming_state;
	3880	+ const char *state_name;
	3881	+ u32 current_limit;
	3882	+ bool adjust;
3545	3883
3546	3884	port->enter_state = port->state;
3547	3885	switch (port->state) {
..	..	@@ -3552,6 +3890,15 @@
3552	3890	if (!port->non_pd_role_swap)
3553	3891	tcpm_swap_complete(port, -ENOTCONN);
3554	3892	tcpm_src_detach(port);
	3893	+ if (port->debouncing) {
	3894	+ port->debouncing = false;
	3895	+ if (port->tcpc->check_contaminant &&
	3896	+ port->tcpc->check_contaminant(port->tcpc)) {
	3897	+ /* Contaminant detection would handle toggling */
	3898	+ tcpm_set_state(port, TOGGLING, 0);
	3899	+ break;
	3900	+ }
	3901	+ }
3555	3902	if (tcpm_start_toggling(port, tcpm_rp_cc(port))) {
3556	3903	tcpm_set_state(port, TOGGLING, 0);
3557	3904	break;
..	..	@@ -3561,20 +3908,25 @@
3561	3908	tcpm_set_state(port, SNK_UNATTACHED, PD_T_DRP_SNK);
3562	3909	break;
3563	3910	case SRC_ATTACH_WAIT:
	3911	+ port->debouncing = true;
	3912	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	3913	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SRC_ATTACH_WAIT],
	3914	+ CC_DEBOUNCE, &timer_val_msecs);
3564	3915	if (tcpm_port_is_debug(port))
3565	3916	tcpm_set_state(port, DEBUG_ACC_ATTACHED,
3566		- PD_T_CC_DEBOUNCE);
	3917	+ timer_val_msecs);
3567	3918	else if (tcpm_port_is_audio(port))
3568	3919	tcpm_set_state(port, AUDIO_ACC_ATTACHED,
3569		- PD_T_CC_DEBOUNCE);
3570		- else if (tcpm_port_is_source(port))
	3920	+ timer_val_msecs);
	3921	+ else if (tcpm_port_is_source(port) && port->vbus_vsafe0v)
3571	3922	tcpm_set_state(port,
3572	3923	tcpm_try_snk(port) ? SNK_TRY
3573	3924	: SRC_ATTACHED,
3574		- PD_T_CC_DEBOUNCE);
	3925	+ timer_val_msecs);
3575	3926	break;
3576	3927
3577	3928	case SNK_TRY:
	3929	+ port->debouncing = false;
3578	3930	port->try_snk_count++;
3579	3931	/*
3580	3932	* Requirements:
..	..	@@ -3597,15 +3949,13 @@
3597	3949	break;
3598	3950	case SNK_TRY_WAIT_DEBOUNCE:
3599	3951	tcpm_set_state(port, SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS,
3600		- PD_T_PD_DEBOUNCE);
	3952	+ PD_T_TRY_CC_DEBOUNCE);
3601	3953	break;
3602	3954	case SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS:
3603		- if (port->vbus_present && tcpm_port_is_sink(port)) {
	3955	+ if (port->vbus_present && tcpm_port_is_sink(port))
3604	3956	tcpm_set_state(port, SNK_ATTACHED, 0);
3605		- } else {
3606		- tcpm_set_state(port, SRC_TRYWAIT, 0);
	3957	+ else
3607	3958	port->max_wait = 0;
3608		- }
3609	3959	break;
3610	3960	case SRC_TRYWAIT:
3611	3961	tcpm_set_cc(port, tcpm_rp_cc(port));
..	..	@@ -3624,7 +3974,10 @@
3624	3974	}
3625	3975	break;
3626	3976	case SRC_TRYWAIT_DEBOUNCE:
3627		- tcpm_set_state(port, SRC_ATTACHED, PD_T_CC_DEBOUNCE);
	3977	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	3978	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SRC_TRYWAIT_DEBOUNCE],
	3979	+ CC_DEBOUNCE, &timer_val_msecs);
	3980	+ tcpm_set_state(port, SRC_ATTACHED, timer_val_msecs);
3628	3981	break;
3629	3982	case SRC_TRYWAIT_UNATTACHED:
3630	3983	tcpm_set_state(port, SNK_UNATTACHED, 0);
..	..	@@ -3640,7 +3993,7 @@
3640	3993	typec_set_pwr_opmode(port->typec_port, opmode);
3641	3994	port->pwr_opmode = TYPEC_PWR_MODE_USB;
3642	3995	port->caps_count = 0;
3643		- port->negotiated_rev = PD_MAX_REV;
	3996	+ port->negotiated_rev = (((port->typec_caps.pd_revision >> 8) & 0xff) - 1);
3644	3997	port->message_id = 0;
3645	3998	port->rx_msgid = -1;
3646	3999	port->explicit_contract = false;
..	..	@@ -3710,6 +4063,8 @@
3710	4063	}
3711	4064	} else {
3712	4065	tcpm_pd_send_control(port, PD_CTRL_ACCEPT);
	4066	+ tcpm_set_partner_usb_comm_capable(port,
	4067	+ !!(port->sink_request & RDO_USB_COMM));
3713	4068	tcpm_set_state(port, SRC_TRANSITION_SUPPLY,
3714	4069	PD_T_SRC_TRANSITION);
3715	4070	}
..	..	@@ -3733,6 +4088,11 @@
3733	4088
3734	4089	if (port->ams != NONE_AMS)
3735	4090	tcpm_ams_finish(port);
	4091	+ if (port->next_ams != NONE_AMS) {
	4092	+ port->ams = port->next_ams;
	4093	+ port->next_ams = NONE_AMS;
	4094	+ }
	4095	+
3736	4096	/*
3737	4097	* If previous AMS is interrupted, switch to the upcoming
3738	4098	* state.
..	..	@@ -3744,7 +4104,18 @@
3744	4104	break;
3745	4105	}
3746	4106
3747		- tcpm_check_send_discover(port);
	4107	+ /*
	4108	+ * 6.4.4.3.1 Discover Identity
	4109	+ * "The Discover Identity Command Shall only be sent to SOP when there is an
	4110	+ * Explicit Contract."
	4111	+ * For now, this driver only supports SOP for DISCOVER_IDENTITY, thus using
	4112	+ * port->explicit_contract to decide whether to send the command.
	4113	+ */
	4114	+ if (port->explicit_contract)
	4115	+ mod_send_discover_delayed_work(port, 0);
	4116	+ else
	4117	+ port->send_discover = false;
	4118	+
3748	4119	/*
3749	4120	* 6.3.5
3750	4121	* Sending ping messages is not necessary if
..	..	@@ -3769,6 +4140,15 @@
3769	4140	tcpm_swap_complete(port, -ENOTCONN);
3770	4141	tcpm_pps_complete(port, -ENOTCONN);
3771	4142	tcpm_snk_detach(port);
	4143	+ if (port->debouncing) {
	4144	+ port->debouncing = false;
	4145	+ if (port->tcpc->check_contaminant &&
	4146	+ port->tcpc->check_contaminant(port->tcpc)) {
	4147	+ /* Contaminant detection would handle toggling */
	4148	+ tcpm_set_state(port, TOGGLING, 0);
	4149	+ break;
	4150	+ }
	4151	+ }
3772	4152	if (tcpm_start_toggling(port, TYPEC_CC_RD)) {
3773	4153	tcpm_set_state(port, TOGGLING, 0);
3774	4154	break;
..	..	@@ -3778,25 +4158,33 @@
3778	4158	tcpm_set_state(port, SRC_UNATTACHED, PD_T_DRP_SRC);
3779	4159	break;
3780	4160	case SNK_ATTACH_WAIT:
	4161	+ port->debouncing = true;
	4162	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	4163	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_ATTACH_WAIT],
	4164	+ CC_DEBOUNCE, &timer_val_msecs);
3781	4165	if ((port->cc1 == TYPEC_CC_OPEN &&
3782	4166	port->cc2 != TYPEC_CC_OPEN) \|\|
3783	4167	(port->cc1 != TYPEC_CC_OPEN &&
3784	4168	port->cc2 == TYPEC_CC_OPEN))
3785	4169	tcpm_set_state(port, SNK_DEBOUNCED,
3786		- PD_T_CC_DEBOUNCE);
	4170	+ timer_val_msecs);
3787	4171	else if (tcpm_port_is_disconnected(port))
3788	4172	tcpm_set_state(port, SNK_UNATTACHED,
3789		- PD_T_PD_DEBOUNCE);
	4173	+ timer_val_msecs);
3790	4174	break;
3791	4175	case SNK_DEBOUNCED:
3792		- if (tcpm_port_is_disconnected(port))
	4176	+ if (tcpm_port_is_disconnected(port)) {
3793	4177	tcpm_set_state(port, SNK_UNATTACHED,
3794	4178	PD_T_PD_DEBOUNCE);
3795		- else if (port->vbus_present)
	4179	+ } else if (port->vbus_present) {
3796	4180	tcpm_set_state(port,
3797	4181	tcpm_try_src(port) ? SRC_TRY
3798	4182	: SNK_ATTACHED,
3799	4183	0);
	4184	+ port->debouncing = false;
	4185	+ } else {
	4186	+ port->debouncing = false;
	4187	+ }
3800	4188	break;
3801	4189	case SRC_TRY:
3802	4190	port->try_src_count++;
..	..	@@ -3822,8 +4210,11 @@
3822	4210	tcpm_set_state(port, SRC_ATTACHED, PD_T_PD_DEBOUNCE);
3823	4211	break;
3824	4212	case SNK_TRYWAIT:
	4213	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	4214	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_TRYWAIT],
	4215	+ CC_DEBOUNCE, &timer_val_msecs);
3825	4216	tcpm_set_cc(port, TYPEC_CC_RD);
3826		- tcpm_set_state(port, SNK_TRYWAIT_VBUS, PD_T_CC_DEBOUNCE);
	4217	+ tcpm_set_state(port, SNK_TRYWAIT_VBUS, timer_val_msecs);
3827	4218	break;
3828	4219	case SNK_TRYWAIT_VBUS:
3829	4220	/*
..	..	@@ -3853,7 +4244,7 @@
3853	4244	port->cc2 : port->cc1);
3854	4245	typec_set_pwr_opmode(port->typec_port, opmode);
3855	4246	port->pwr_opmode = TYPEC_PWR_MODE_USB;
3856		- port->negotiated_rev = PD_MAX_REV;
	4247	+ port->negotiated_rev = (((port->typec_caps.pd_revision >> 8) & 0xff) - 1);
3857	4248	port->message_id = 0;
3858	4249	port->rx_msgid = -1;
3859	4250	port->explicit_contract = false;
..	..	@@ -3863,13 +4254,22 @@
3863	4254	/* SRC -> SNK POWER/FAST_ROLE_SWAP finished */
3864	4255	tcpm_ams_finish(port);
3865	4256
3866		- tcpm_set_state(port, SNK_DISCOVERY, 0);
	4257	+ timer_val_msecs = 0;
	4258	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_STARTUP],
	4259	+ SINK_DISCOVERY_BC12, &timer_val_msecs);
	4260	+ tcpm_set_state(port, SNK_DISCOVERY, timer_val_msecs);
3867	4261	break;
3868	4262	case SNK_DISCOVERY:
3869	4263	if (port->vbus_present) {
3870		- tcpm_set_current_limit(port,
3871		- tcpm_get_current_limit(port),
3872		- 5000);
	4264	+ current_limit = tcpm_get_current_limit(port);
	4265	+ trace_android_vh_typec_tcpm_adj_current_limit(tcpm_states[SNK_DISCOVERY],
	4266	+ port->current_limit,
	4267	+ port->supply_voltage,
	4268	+ port->pd_capable,
	4269	+ &current_limit, &adjust);
	4270	+ if (port->slow_charger_loop && (current_limit > PD_P_SNK_STDBY_MW / 5))
	4271	+ current_limit = PD_P_SNK_STDBY_MW / 5;
	4272	+ tcpm_set_current_limit(port, current_limit, 5000);
3873	4273	tcpm_set_charge(port, true);
3874	4274	tcpm_set_state(port, SNK_WAIT_CAPABILITIES, 0);
3875	4275	break;
..	..	@@ -3884,8 +4284,10 @@
3884	4284	PD_T_DB_DETECT : PD_T_NO_RESPONSE);
3885	4285	break;
3886	4286	case SNK_DISCOVERY_DEBOUNCE:
3887		- tcpm_set_state(port, SNK_DISCOVERY_DEBOUNCE_DONE,
3888		- PD_T_CC_DEBOUNCE);
	4287	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	4288	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_DISCOVERY_DEBOUNCE],
	4289	+ CC_DEBOUNCE, &timer_val_msecs);
	4290	+ tcpm_set_state(port, SNK_DISCOVERY_DEBOUNCE_DONE, timer_val_msecs);
3889	4291	break;
3890	4292	case SNK_DISCOVERY_DEBOUNCE_DONE:
3891	4293	if (!tcpm_port_is_disconnected(port) &&
..	..	@@ -3898,11 +4300,16 @@
3898	4300	tcpm_set_state(port, unattached_state(port), 0);
3899	4301	break;
3900	4302	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;
	4303	+ if (port->prev_state != SOFT_RESET_SEND) {
	4304	+ ret = port->tcpc->set_pd_rx(port->tcpc, true);
	4305	+ if (ret < 0) {
	4306	+ tcpm_set_state(port, SNK_READY, 0);
	4307	+ break;
	4308	+ }
3905	4309	}
	4310	+ timer_val_msecs = PD_T_SINK_WAIT_CAP;
	4311	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_WAIT_CAPABILITIES],
	4312	+ SINK_WAIT_CAP, &timer_val_msecs);
3906	4313	/*
3907	4314	* If VBUS has never been low, and we time out waiting
3908	4315	* for source cap, try a soft reset first, in case we
..	..	@@ -3912,17 +4319,23 @@
3912	4319	if (port->vbus_never_low) {
3913	4320	port->vbus_never_low = false;
3914	4321	tcpm_set_state(port, SNK_SOFT_RESET,
3915		- PD_T_SINK_WAIT_CAP);
	4322	+ timer_val_msecs);
3916	4323	} else {
3917	4324	tcpm_set_state(port, hard_reset_state(port),
3918		- PD_T_SINK_WAIT_CAP);
	4325	+ timer_val_msecs);
3919	4326	}
3920	4327	break;
3921	4328	case SNK_NEGOTIATE_CAPABILITIES:
3922	4329	port->pd_capable = true;
	4330	+ tcpm_set_partner_usb_comm_capable(port,
	4331	+ !!(port->source_caps[0] & PDO_FIXED_USB_COMM));
3923	4332	port->hard_reset_count = 0;
3924	4333	ret = tcpm_pd_send_request(port);
3925	4334	if (ret < 0) {
	4335	+ /* Restore back to the original state */
	4336	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
	4337	+ port->pps_data.active,
	4338	+ port->supply_voltage);
3926	4339	/* Let the Source send capabilities again. */
3927	4340	tcpm_set_state(port, SNK_WAIT_CAPABILITIES, 0);
3928	4341	} else {
..	..	@@ -3933,6 +4346,10 @@
3933	4346	case SNK_NEGOTIATE_PPS_CAPABILITIES:
3934	4347	ret = tcpm_pd_send_pps_request(port);
3935	4348	if (ret < 0) {
	4349	+ /* Restore back to the original state */
	4350	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
	4351	+ port->pps_data.active,
	4352	+ port->supply_voltage);
3936	4353	port->pps_status = ret;
3937	4354	/*
3938	4355	* If this was called due to updates to sink
..	..	@@ -3949,6 +4366,23 @@
3949	4366	}
3950	4367	break;
3951	4368	case SNK_TRANSITION_SINK:
	4369	+ /* From the USB PD spec:
	4370	+ * "The Sink Shall transition to Sink Standby before a positive or
	4371	+ * negative voltage transition of VBUS. During Sink Standby
	4372	+ * the Sink Shall reduce its power draw to pSnkStdby."
	4373	+ *
	4374	+ * This is not applicable to PPS though as the port can continue
	4375	+ * to draw negotiated power without switching to standby.
	4376	+ */
	4377	+ if (port->supply_voltage != port->req_supply_voltage && !port->pps_data.active &&
	4378	+ port->current_limit * port->supply_voltage / 1000 > PD_P_SNK_STDBY_MW) {
	4379	+ u32 stdby_ma = PD_P_SNK_STDBY_MW * 1000 / port->supply_voltage;
	4380	+
	4381	+ tcpm_log(port, "Setting standby current %u mV @ %u mA",
	4382	+ port->supply_voltage, stdby_ma);
	4383	+ tcpm_set_current_limit(port, stdby_ma, port->supply_voltage);
	4384	+ }
	4385	+ fallthrough;
3952	4386	case SNK_TRANSITION_SINK_VBUS:
3953	4387	tcpm_set_state(port, hard_reset_state(port),
3954	4388	PD_T_PS_TRANSITION);
..	..	@@ -3962,6 +4396,19 @@
3962	4396	port->pwr_opmode = TYPEC_PWR_MODE_PD;
3963	4397	}
3964	4398
	4399	+ current_limit = tcpm_get_current_limit(port);
	4400	+ adjust = false;
	4401	+ trace_android_vh_typec_tcpm_adj_current_limit(tcpm_states[SNK_READY],
	4402	+ port->current_limit,
	4403	+ port->supply_voltage,
	4404	+ port->pd_capable,
	4405	+ &current_limit,
	4406	+ &adjust);
	4407	+ if (adjust)
	4408	+ tcpm_set_current_limit(port, current_limit, 5000);
	4409	+
	4410	+ if (!port->pd_capable && port->slow_charger_loop)
	4411	+ tcpm_set_current_limit(port, tcpm_get_current_limit(port), 5000);
3965	4412	tcpm_swap_complete(port, 0);
3966	4413	tcpm_typec_connect(port);
3967	4414	mod_enable_frs_delayed_work(port, 0);
..	..	@@ -3969,6 +4416,11 @@
3969	4416
3970	4417	if (port->ams != NONE_AMS)
3971	4418	tcpm_ams_finish(port);
	4419	+ if (port->next_ams != NONE_AMS) {
	4420	+ port->ams = port->next_ams;
	4421	+ port->next_ams = NONE_AMS;
	4422	+ }
	4423	+
3972	4424	/*
3973	4425	* If previous AMS is interrupted, switch to the upcoming
3974	4426	* state.
..	..	@@ -3980,11 +4432,19 @@
3980	4432	break;
3981	4433	}
3982	4434
3983		- tcpm_check_send_discover(port);
	4435	+ /*
	4436	+ * 6.4.4.3.1 Discover Identity
	4437	+ * "The Discover Identity Command Shall only be sent to SOP when there is an
	4438	+ * Explicit Contract."
	4439	+ * For now, this driver only supports SOP for DISCOVER_IDENTITY, thus using
	4440	+ * port->explicit_contract.
	4441	+ */
	4442	+ if (port->explicit_contract)
	4443	+ mod_send_discover_delayed_work(port, 0);
	4444	+ else
	4445	+ port->send_discover = false;
	4446	+
3984	4447	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);
3988	4448	break;
3989	4449
3990	4450	/* Accessory states */
..	..	@@ -3999,7 +4459,10 @@
3999	4459	tcpm_set_state(port, ACC_UNATTACHED, 0);
4000	4460	break;
4001	4461	case AUDIO_ACC_DEBOUNCE:
4002		- tcpm_set_state(port, ACC_UNATTACHED, PD_T_CC_DEBOUNCE);
	4462	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	4463	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[AUDIO_ACC_DEBOUNCE],
	4464	+ CC_DEBOUNCE, &timer_val_msecs);
	4465	+ tcpm_set_state(port, ACC_UNATTACHED, timer_val_msecs);
4003	4466	break;
4004	4467
4005	4468	/* Hard_Reset states */
..	..	@@ -4029,13 +4492,31 @@
4029	4492	tcpm_set_state(port, SNK_HARD_RESET_SINK_OFF, 0);
4030	4493	break;
4031	4494	case SRC_HARD_RESET_VBUS_OFF:
4032		- tcpm_set_vconn(port, true);
	4495	+ /*
	4496	+ * 7.1.5 Response to Hard Resets
	4497	+ * Hard Reset Signaling indicates a communication failure has occurred and the
	4498	+ * Source Shall stop driving VCONN, Shall remove Rp from the VCONN pin and Shall
	4499	+ * drive VBUS to vSafe0V as shown in Figure 7-9.
	4500	+ */
	4501	+ tcpm_set_vconn(port, false);
4033	4502	tcpm_set_vbus(port, false);
4034	4503	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);
	4504	+ tcpm_data_role_for_source(port));
	4505	+ /*
	4506	+ * If tcpc fails to notify vbus off, TCPM will wait for PD_T_SAFE_0V +
	4507	+ * PD_T_SRC_RECOVER before turning vbus back on.
	4508	+ * From Table 7-12 Sequence Description for a Source Initiated Hard Reset:
	4509	+ * 4. Policy Engine waits tPSHardReset after sending Hard Reset Signaling and then
	4510	+ * tells the Device Policy Manager to instruct the power supply to perform a
	4511	+ * Hard Reset. The transition to vSafe0V Shall occur within tSafe0V (t2).
	4512	+ * 5. After tSrcRecover the Source applies power to VBUS in an attempt to
	4513	+ * re-establish communication with the Sink and resume USB Default Operation.
	4514	+ * The transition to vSafe5V Shall occur within tSrcTurnOn(t4).
	4515	+ */
	4516	+ tcpm_set_state(port, SRC_HARD_RESET_VBUS_ON, PD_T_SAFE_0V + PD_T_SRC_RECOVER);
4037	4517	break;
4038	4518	case SRC_HARD_RESET_VBUS_ON:
	4519	+ tcpm_set_vconn(port, true);
4039	4520	tcpm_set_vbus(port, true);
4040	4521	if (port->ams == HARD_RESET)
4041	4522	tcpm_ams_finish(port);
..	..	@@ -4044,12 +4525,14 @@
4044	4525	tcpm_set_state(port, SRC_UNATTACHED, PD_T_PS_SOURCE_ON);
4045	4526	break;
4046	4527	case SNK_HARD_RESET_SINK_OFF:
	4528	+ /* Do not discharge/disconnect during hard reseet */
	4529	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, 0);
4047	4530	memset(&port->pps_data, 0, sizeof(port->pps_data));
4048	4531	tcpm_set_vconn(port, false);
4049	4532	if (port->pd_capable)
4050	4533	tcpm_set_charge(port, false);
4051	4534	tcpm_set_roles(port, port->self_powered, TYPEC_SINK,
4052		- TYPEC_DEVICE);
	4535	+ tcpm_data_role_for_sink(port));
4053	4536	/*
4054	4537	* VBUS may or may not toggle, depending on the adapter.
4055	4538	* If it doesn't toggle, transition to SNK_HARD_RESET_SINK_ON
..	..	@@ -4090,6 +4573,7 @@
4090	4573	if (port->ams == HARD_RESET)
4091	4574	tcpm_ams_finish(port);
4092	4575	tcpm_set_attached_state(port, true);
	4576	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, VSAFE5V);
4093	4577	tcpm_set_state(port, SNK_STARTUP, 0);
4094	4578	break;
4095	4579
..	..	@@ -4098,6 +4582,7 @@
4098	4582	port->message_id = 0;
4099	4583	port->rx_msgid = -1;
4100	4584	tcpm_pd_send_control(port, PD_CTRL_ACCEPT);
	4585	+ tcpm_ams_finish(port);
4101	4586	if (port->pwr_role == TYPEC_SOURCE) {
4102	4587	port->upcoming_state = SRC_SEND_CAPABILITIES;
4103	4588	tcpm_ams_start(port, POWER_NEGOTIATION);
..	..	@@ -4115,6 +4600,7 @@
4115	4600	case SOFT_RESET_SEND:
4116	4601	port->message_id = 0;
4117	4602	port->rx_msgid = -1;
	4603	+ port->tcpc->set_pd_rx(port->tcpc, true);
4118	4604	if (tcpm_pd_send_control(port, PD_CTRL_SOFT_RESET))
4119	4605	tcpm_set_state_cond(port, hard_reset_state(port), 0);
4120	4606	else
..	..	@@ -4125,52 +4611,33 @@
4125	4611	/* DR_Swap states */
4126	4612	case DR_SWAP_SEND:
4127	4613	tcpm_pd_send_control(port, PD_CTRL_DR_SWAP);
	4614	+ if (port->data_role == TYPEC_DEVICE \|\| port->negotiated_rev > PD_REV20)
	4615	+ port->send_discover = true;
4128	4616	tcpm_set_state_cond(port, DR_SWAP_SEND_TIMEOUT,
4129	4617	PD_T_SENDER_RESPONSE);
4130	4618	break;
4131	4619	case DR_SWAP_ACCEPT:
4132	4620	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;
	4621	+ if (port->data_role == TYPEC_DEVICE \|\| port->negotiated_rev > PD_REV20)
	4622	+ port->send_discover = true;
4136	4623	tcpm_set_state_cond(port, DR_SWAP_CHANGE_DR, 0);
4137	4624	break;
4138	4625	case DR_SWAP_SEND_TIMEOUT:
4139	4626	tcpm_swap_complete(port, -ETIMEDOUT);
	4627	+ port->send_discover = false;
	4628	+ tcpm_ams_finish(port);
4140	4629	tcpm_set_state(port, ready_state(port), 0);
4141	4630	break;
4142	4631	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		-
4161	4632	if (port->data_role == TYPEC_HOST) {
4162	4633	tcpm_unregister_altmodes(port);
4163	4634	tcpm_set_roles(port, true, port->pwr_role,
4164	4635	TYPEC_DEVICE);
4165		- tcpm_send_data_role_notify(port, false, TYPEC_HOST);
4166		- mod_data_role_swap_work(port, SEND_NEW_MODE_NOTIFY_MS);
4167	4636	} else {
4168	4637	tcpm_set_roles(port, true, port->pwr_role,
4169	4638	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);
4173	4639	}
	4640	+ tcpm_ams_finish(port);
4174	4641	tcpm_set_state(port, ready_state(port), 0);
4175	4642	break;
4176	4643
..	..	@@ -4185,7 +4652,10 @@
4185	4652	tcpm_set_state(port, ERROR_RECOVERY, 0);
4186	4653	break;
4187	4654	case FR_SWAP_SNK_SRC_TRANSITION_TO_OFF:
4188		- tcpm_set_state(port, ERROR_RECOVERY, PD_T_PS_SOURCE_OFF);
	4655	+ timer_val_msecs = PD_T_PS_SOURCE_OFF;
	4656	+ state_name = tcpm_states[FR_SWAP_SNK_SRC_TRANSITION_TO_OFF];
	4657	+ trace_android_vh_typec_tcpm_get_timer(state_name, SOURCE_OFF, &timer_val_msecs);
	4658	+ tcpm_set_state(port, ERROR_RECOVERY, timer_val_msecs);
4189	4659	break;
4190	4660	case FR_SWAP_SNK_SRC_NEW_SINK_READY:
4191	4661	if (port->vbus_source)
..	..	@@ -4218,6 +4688,7 @@
4218	4688	tcpm_set_state(port, ready_state(port), 0);
4219	4689	break;
4220	4690	case PR_SWAP_START:
	4691	+ tcpm_apply_rc(port);
4221	4692	if (port->pwr_role == TYPEC_SOURCE)
4222	4693	tcpm_set_state(port, PR_SWAP_SRC_SNK_TRANSITION_OFF,
4223	4694	PD_T_SRC_TRANSITION);
..	..	@@ -4225,6 +4696,10 @@
4225	4696	tcpm_set_state(port, PR_SWAP_SNK_SRC_SINK_OFF, 0);
4226	4697	break;
4227	4698	case PR_SWAP_SRC_SNK_TRANSITION_OFF:
	4699	+ /*
	4700	+ * Prevent vbus discharge circuit from turning on during PR_SWAP
	4701	+ * as this is not a disconnect.
	4702	+ */
4228	4703	tcpm_set_vbus(port, false);
4229	4704	port->explicit_contract = false;
4230	4705	/* allow time for Vbus discharge, must be < tSrcSwapStdby */
..	..	@@ -4232,10 +4707,13 @@
4232	4707	PD_T_SRCSWAPSTDBY);
4233	4708	break;
4234	4709	case PR_SWAP_SRC_SNK_SOURCE_OFF:
	4710	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	4711	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[PR_SWAP_SRC_SNK_SOURCE_OFF],
	4712	+ CC_DEBOUNCE, &timer_val_msecs);
4235	4713	tcpm_set_cc(port, TYPEC_CC_RD);
4236	4714	/* allow CC debounce */
4237	4715	tcpm_set_state(port, PR_SWAP_SRC_SNK_SOURCE_OFF_CC_DEBOUNCED,
4238		- PD_T_CC_DEBOUNCE);
	4716	+ timer_val_msecs);
4239	4717	break;
4240	4718	case PR_SWAP_SRC_SNK_SOURCE_OFF_CC_DEBOUNCED:
4241	4719	/*
..	..	@@ -4250,17 +4728,29 @@
4250	4728	tcpm_set_state(port, ERROR_RECOVERY, 0);
4251	4729	break;
4252	4730	}
4253		- tcpm_set_state_cond(port, SNK_UNATTACHED, PD_T_PS_SOURCE_ON);
	4731	+ tcpm_set_state(port, ERROR_RECOVERY, PD_T_PS_SOURCE_ON_PRS);
4254	4732	break;
4255	4733	case PR_SWAP_SRC_SNK_SINK_ON:
	4734	+ tcpm_enable_auto_vbus_discharge(port, true);
	4735	+ /* Set the vbus disconnect threshold for implicit contract */
	4736	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, VSAFE5V);
4256	4737	tcpm_set_state(port, SNK_STARTUP, 0);
4257	4738	break;
4258	4739	case PR_SWAP_SNK_SRC_SINK_OFF:
	4740	+ timer_val_msecs = PD_T_PS_SOURCE_OFF;
	4741	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[PR_SWAP_SNK_SRC_SINK_OFF],
	4742	+ SOURCE_OFF, &timer_val_msecs);
	4743	+ /*
	4744	+ * Prevent vbus discharge circuit from turning on during PR_SWAP
	4745	+ * as this is not a disconnect.
	4746	+ */
	4747	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB,
	4748	+ port->pps_data.active, 0);
4259	4749	tcpm_set_charge(port, false);
4260		- tcpm_set_state(port, hard_reset_state(port),
4261		- PD_T_PS_SOURCE_OFF);
	4750	+ tcpm_set_state(port, hard_reset_state(port), timer_val_msecs);
4262	4751	break;
4263	4752	case PR_SWAP_SNK_SRC_SOURCE_ON:
	4753	+ tcpm_enable_auto_vbus_discharge(port, true);
4264	4754	tcpm_set_cc(port, tcpm_rp_cc(port));
4265	4755	tcpm_set_vbus(port, true);
4266	4756	/*
..	..	@@ -4286,6 +4776,7 @@
4286	4776
4287	4777	case VCONN_SWAP_ACCEPT:
4288	4778	tcpm_pd_send_control(port, PD_CTRL_ACCEPT);
	4779	+ tcpm_ams_finish(port);
4289	4780	tcpm_set_state(port, VCONN_SWAP_START, 0);
4290	4781	break;
4291	4782	case VCONN_SWAP_SEND:
..	..	@@ -4295,8 +4786,6 @@
4295	4786	break;
4296	4787	case VCONN_SWAP_SEND_TIMEOUT:
4297	4788	tcpm_swap_complete(port, -ETIMEDOUT);
4298		- if (port->data_role == TYPEC_HOST && port->send_discover)
4299		- port->vdm_sm_running = true;
4300	4789	tcpm_set_state(port, ready_state(port), 0);
4301	4790	break;
4302	4791	case VCONN_SWAP_START:
..	..	@@ -4312,14 +4801,10 @@
4312	4801	case VCONN_SWAP_TURN_ON_VCONN:
4313	4802	tcpm_set_vconn(port, true);
4314	4803	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;
4317	4804	tcpm_set_state(port, ready_state(port), 0);
4318	4805	break;
4319	4806	case VCONN_SWAP_TURN_OFF_VCONN:
4320	4807	tcpm_set_vconn(port, false);
4321		- if (port->data_role == TYPEC_HOST && port->send_discover)
4322		- port->vdm_sm_running = true;
4323	4808	tcpm_set_state(port, ready_state(port), 0);
4324	4809	break;
4325	4810
..	..	@@ -4327,8 +4812,6 @@
4327	4812	case PR_SWAP_CANCEL:
4328	4813	case VCONN_SWAP_CANCEL:
4329	4814	tcpm_swap_complete(port, port->swap_status);
4330		- if (port->data_role == TYPEC_HOST && port->send_discover)
4331		- port->vdm_sm_running = true;
4332	4815	if (port->pwr_role == TYPEC_SOURCE)
4333	4816	tcpm_set_state(port, SRC_READY, 0);
4334	4817	else
..	..	@@ -4345,12 +4828,18 @@
4345	4828	switch (BDO_MODE_MASK(port->bist_request)) {
4346	4829	case BDO_MODE_CARRIER2:
4347	4830	tcpm_pd_transmit(port, TCPC_TX_BIST_MODE_2, NULL);
	4831	+ tcpm_set_state(port, unattached_state(port),
	4832	+ PD_T_BIST_CONT_MODE);
	4833	+ break;
	4834	+ case BDO_MODE_TESTDATA:
	4835	+ if (port->tcpc->set_bist_data) {
	4836	+ tcpm_log(port, "Enable BIST MODE TESTDATA");
	4837	+ port->tcpc->set_bist_data(port->tcpc, true);
	4838	+ }
4348	4839	break;
4349	4840	default:
4350	4841	break;
4351	4842	}
4352		- /* Always switch to unattached state */
4353		- tcpm_set_state(port, unattached_state(port), 0);
4354	4843	break;
4355	4844	case GET_STATUS_SEND:
4356	4845	tcpm_pd_send_control(port, PD_CTRL_GET_STATUS);
..	..	@@ -4383,14 +4872,17 @@
4383	4872	break;
4384	4873	case PORT_RESET:
4385	4874	tcpm_reset_port(port);
4386		- tcpm_set_cc(port, TYPEC_CC_OPEN);
	4875	+ tcpm_set_cc(port, TYPEC_CC_RD);
4387	4876	tcpm_set_state(port, PORT_RESET_WAIT_OFF,
4388	4877	PD_T_ERROR_RECOVERY);
4389	4878	break;
4390	4879	case PORT_RESET_WAIT_OFF:
	4880	+ timer_val_msecs = PD_T_PS_SOURCE_OFF;
	4881	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[PORT_RESET_WAIT_OFF],
	4882	+ SOURCE_OFF, &timer_val_msecs);
4391	4883	tcpm_set_state(port,
4392	4884	tcpm_default_state(port),
4393		- port->vbus_present ? PD_T_PS_SOURCE_OFF : 0);
	4885	+ port->vbus_present ? timer_val_msecs : 0);
4394	4886	break;
4395	4887
4396	4888	/* AMS intermediate state */
..	..	@@ -4404,6 +4896,12 @@
4404	4896	upcoming_state = port->upcoming_state;
4405	4897	port->upcoming_state = INVALID_STATE;
4406	4898	tcpm_set_state(port, upcoming_state, 0);
	4899	+ break;
	4900	+
	4901	+ /* Chunk state */
	4902	+ case CHUNK_NOT_SUPP:
	4903	+ tcpm_pd_send_control(port, PD_CTRL_NOT_SUPP);
	4904	+ tcpm_set_state(port, port->pwr_role == TYPEC_SOURCE ? SRC_READY : SNK_READY, 0);
4407	4905	break;
4408	4906	default:
4409	4907	WARN(1, "Unexpected port state %d\n", port->state);
..	..	@@ -4488,11 +4986,16 @@
4488	4986	tcpm_set_state(port, SRC_ATTACH_WAIT, 0);
4489	4987	break;
4490	4988	case SRC_ATTACHED:
	4989	+ case SRC_STARTUP:
4491	4990	case SRC_SEND_CAPABILITIES:
4492	4991	case SRC_READY:
4493	4992	if (tcpm_port_is_disconnected(port) \|\|
4494		- !tcpm_port_is_source(port))
4495		- tcpm_set_state(port, SRC_UNATTACHED, 0);
	4993	+ !tcpm_port_is_source(port)) {
	4994	+ if (port->port_type == TYPEC_PORT_SRC)
	4995	+ tcpm_set_state(port, SRC_UNATTACHED, tcpm_wait_for_discharge(port));
	4996	+ else
	4997	+ tcpm_set_state(port, SNK_UNATTACHED, tcpm_wait_for_discharge(port));
	4998	+ }
4496	4999	break;
4497	5000	case SNK_UNATTACHED:
4498	5001	if (tcpm_port_is_sink(port))
..	..	@@ -4522,7 +5025,23 @@
4522	5025	tcpm_set_state(port, SNK_DEBOUNCED, 0);
4523	5026	break;
4524	5027	case SNK_READY:
4525		- if (tcpm_port_is_disconnected(port))
	5028	+ /*
	5029	+ * EXIT condition is based primarily on vbus disconnect and CC is secondary.
	5030	+ * "A port that has entered into USB PD communications with the Source and
	5031	+ * has seen the CC voltage exceed vRd-USB may monitor the CC pin to detect
	5032	+ * cable disconnect in addition to monitoring VBUS.
	5033	+ *
	5034	+ * A port that is monitoring the CC voltage for disconnect (but is not in
	5035	+ * the process of a USB PD PR_Swap or USB PD FR_Swap) shall transition to
	5036	+ * Unattached.SNK within tSinkDisconnect after the CC voltage remains below
	5037	+ * vRd-USB for tPDDebounce."
	5038	+ *
	5039	+ * When set_auto_vbus_discharge_threshold is enabled, CC pins go
	5040	+ * away before vbus decays to disconnect threshold. Allow
	5041	+ * disconnect to be driven by vbus disconnect when auto vbus
	5042	+ * discharge is enabled.
	5043	+ */
	5044	+ if (!port->auto_vbus_discharge_enabled && tcpm_port_is_disconnected(port))
4526	5045	tcpm_set_state(port, unattached_state(port), 0);
4527	5046	else if (!port->pd_capable &&
4528	5047	(cc1 != old_cc1 \|\| cc2 != old_cc2))
..	..	@@ -4587,6 +5106,12 @@
4587	5106	if (!tcpm_port_is_sink(port))
4588	5107	tcpm_set_state(port, SNK_TRYWAIT_DEBOUNCE, 0);
4589	5108	break;
	5109	+ case SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS:
	5110	+ if (!tcpm_port_is_sink(port))
	5111	+ tcpm_set_state(port, SRC_TRYWAIT, PD_T_TRY_CC_DEBOUNCE);
	5112	+ else
	5113	+ tcpm_set_state(port, SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS, 0);
	5114	+ break;
4590	5115	case SNK_TRYWAIT:
4591	5116	/* Do nothing, waiting for tCCDebounce */
4592	5117	break;
..	..	@@ -4615,9 +5140,13 @@
4615	5140	* Ignore CC changes here.
4616	5141	*/
4617	5142	break;
4618		-
4619	5143	default:
4620		- if (tcpm_port_is_disconnected(port))
	5144	+ /*
	5145	+ * While acting as sink and auto vbus discharge is enabled, Allow disconnect
	5146	+ * to be driven by vbus disconnect.
	5147	+ */
	5148	+ if (tcpm_port_is_disconnected(port) && !(port->pwr_role == TYPEC_SINK &&
	5149	+ port->auto_vbus_discharge_enabled))
4621	5150	tcpm_set_state(port, unattached_state(port), 0);
4622	5151	break;
4623	5152	}
..	..	@@ -4627,12 +5156,15 @@
4627	5156	{
4628	5157	tcpm_log_force(port, "VBUS on");
4629	5158	port->vbus_present = true;
	5159	+ /*
	5160	+ * When vbus_present is true i.e. Voltage at VBUS is greater than VSAFE5V implicitly
	5161	+ * states that vbus is not at VSAFE0V, hence clear the vbus_vsafe0v flag here.
	5162	+ */
	5163	+ port->vbus_vsafe0v = false;
	5164	+
4630	5165	switch (port->state) {
4631	5166	case SNK_TRANSITION_SINK_VBUS:
4632	5167	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;
4636	5168	tcpm_set_state(port, SNK_READY, 0);
4637	5169	break;
4638	5170	case SNK_DISCOVERY:
..	..	@@ -4676,11 +5208,24 @@
4676	5208	case SNK_TRYWAIT_DEBOUNCE:
4677	5209	/* Do nothing, waiting for Rp */
4678	5210	break;
	5211	+ case SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS:
	5212	+ if (port->vbus_present && tcpm_port_is_sink(port))
	5213	+ tcpm_set_state(port, SNK_ATTACHED, 0);
	5214	+ break;
4679	5215	case SRC_TRY_WAIT:
4680	5216	case SRC_TRY_DEBOUNCE:
4681	5217	/* Do nothing, waiting for sink detection */
4682	5218	break;
	5219	+ case FR_SWAP_SEND:
	5220	+ case FR_SWAP_SEND_TIMEOUT:
	5221	+ case FR_SWAP_SNK_SRC_TRANSITION_TO_OFF:
	5222	+ case FR_SWAP_SNK_SRC_SOURCE_VBUS_APPLIED:
	5223	+ if (port->tcpc->frs_sourcing_vbus)
	5224	+ port->tcpc->frs_sourcing_vbus(port->tcpc);
	5225	+ break;
4683	5226	case FR_SWAP_SNK_SRC_NEW_SINK_READY:
	5227	+ if (port->tcpc->frs_sourcing_vbus)
	5228	+ port->tcpc->frs_sourcing_vbus(port->tcpc);
4684	5229	tcpm_set_state(port, FR_SWAP_SNK_SRC_SOURCE_VBUS_APPLIED, 0);
4685	5230	break;
4686	5231
..	..	@@ -4706,12 +5251,8 @@
4706	5251	case SNK_HARD_RESET_SINK_OFF:
4707	5252	tcpm_set_state(port, SNK_HARD_RESET_WAIT_VBUS, 0);
4708	5253	break;
4709		- case SRC_HARD_RESET_VBUS_OFF:
4710		- tcpm_set_state(port, SRC_HARD_RESET_VBUS_ON, 0);
4711		- break;
4712	5254	case HARD_RESET_SEND:
4713	5255	break;
4714		-
4715	5256	case SNK_TRY:
4716	5257	/* Do nothing, waiting for timeout */
4717	5258	break;
..	..	@@ -4729,6 +5270,7 @@
4729	5270	break;
4730	5271	case SNK_ATTACH_WAIT:
4731	5272	case SNK_DEBOUNCED:
	5273	+ port->debouncing = false;
4732	5274	/* Do nothing, as TCPM is still waiting for vbus to reaach VSAFE5V to connect */
4733	5275	break;
4734	5276
..	..	@@ -4743,6 +5285,14 @@
4743	5285	/* Do nothing, expected */
4744	5286	break;
4745	5287
	5288	+ case PR_SWAP_SNK_SRC_SOURCE_ON:
	5289	+ /*
	5290	+ * Do nothing when vbus off notification is received.
	5291	+ * TCPM can wait for PD_T_NEWSRC in PR_SWAP_SNK_SRC_SOURCE_ON
	5292	+ * for the vbus source to ramp up.
	5293	+ */
	5294	+ break;
	5295	+
4746	5296	case PORT_RESET_WAIT_OFF:
4747	5297	tcpm_set_state(port, tcpm_default_state(port), 0);
4748	5298	break;
..	..	@@ -4750,6 +5300,25 @@
4750	5300	case SRC_TRY_WAIT:
4751	5301	case SRC_TRY_DEBOUNCE:
4752	5302	/* Do nothing, waiting for sink detection */
	5303	+ break;
	5304	+
	5305	+ case SRC_STARTUP:
	5306	+ case SRC_SEND_CAPABILITIES:
	5307	+ case SRC_SEND_CAPABILITIES_TIMEOUT:
	5308	+ case SRC_NEGOTIATE_CAPABILITIES:
	5309	+ case SRC_TRANSITION_SUPPLY:
	5310	+ case SRC_READY:
	5311	+ case SRC_WAIT_NEW_CAPABILITIES:
	5312	+ /*
	5313	+ * Force to unattached state to re-initiate connection.
	5314	+ * DRP port should move to Unattached.SNK instead of Unattached.SRC if
	5315	+ * sink removed. Although sink removal here is due to source's vbus collapse,
	5316	+ * treat it the same way for consistency.
	5317	+ */
	5318	+ if (port->port_type == TYPEC_PORT_SRC)
	5319	+ tcpm_set_state(port, SRC_UNATTACHED, tcpm_wait_for_discharge(port));
	5320	+ else
	5321	+ tcpm_set_state(port, SNK_UNATTACHED, tcpm_wait_for_discharge(port));
4753	5322	break;
4754	5323
4755	5324	case PORT_RESET:
..	..	@@ -4768,8 +5337,58 @@
4768	5337	break;
4769	5338
4770	5339	default:
4771		- if (port->pwr_role == TYPEC_SINK &&
4772		- port->attached)
	5340	+ if (port->pwr_role == TYPEC_SINK && port->attached)
	5341	+ tcpm_set_state(port, SNK_UNATTACHED, tcpm_wait_for_discharge(port));
	5342	+ break;
	5343	+ }
	5344	+}
	5345	+
	5346	+static void _tcpm_pd_vbus_vsafe0v(struct tcpm_port *port)
	5347	+{
	5348	+ unsigned int timer_val_msecs;
	5349	+
	5350	+ tcpm_log_force(port, "VBUS VSAFE0V");
	5351	+ port->vbus_vsafe0v = true;
	5352	+ switch (port->state) {
	5353	+ case SRC_HARD_RESET_VBUS_OFF:
	5354	+ /*
	5355	+ * After establishing the vSafe0V voltage condition on VBUS, the Source Shall wait
	5356	+ * tSrcRecover before re-applying VCONN and restoring VBUS to vSafe5V.
	5357	+ */
	5358	+ tcpm_set_state(port, SRC_HARD_RESET_VBUS_ON, PD_T_SRC_RECOVER);
	5359	+ break;
	5360	+ case SRC_ATTACH_WAIT:
	5361	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	5362	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SRC_ATTACH_WAIT],
	5363	+ CC_DEBOUNCE, &timer_val_msecs);
	5364	+ if (tcpm_port_is_source(port))
	5365	+ tcpm_set_state(port, tcpm_try_snk(port) ? SNK_TRY : SRC_ATTACHED,
	5366	+ timer_val_msecs);
	5367	+ break;
	5368	+ case SRC_STARTUP:
	5369	+ case SRC_SEND_CAPABILITIES:
	5370	+ case SRC_SEND_CAPABILITIES_TIMEOUT:
	5371	+ case SRC_NEGOTIATE_CAPABILITIES:
	5372	+ case SRC_TRANSITION_SUPPLY:
	5373	+ case SRC_READY:
	5374	+ case SRC_WAIT_NEW_CAPABILITIES:
	5375	+ if (port->auto_vbus_discharge_enabled) {
	5376	+ if (port->port_type == TYPEC_PORT_SRC)
	5377	+ tcpm_set_state(port, SRC_UNATTACHED, 0);
	5378	+ else
	5379	+ tcpm_set_state(port, SNK_UNATTACHED, 0);
	5380	+ }
	5381	+ break;
	5382	+ case PR_SWAP_SNK_SRC_SINK_OFF:
	5383	+ case PR_SWAP_SNK_SRC_SOURCE_ON:
	5384	+ /* Do nothing, vsafe0v is expected during transition */
	5385	+ break;
	5386	+ case SNK_ATTACH_WAIT:
	5387	+ case SNK_DEBOUNCED:
	5388	+ /Do nothing, still waiting for VSAFE5V for connect /
	5389	+ break;
	5390	+ default:
	5391	+ if (port->pwr_role == TYPEC_SINK && port->auto_vbus_discharge_enabled)
4773	5392	tcpm_set_state(port, SNK_UNATTACHED, 0);
4774	5393	break;
4775	5394	}
..	..	@@ -4778,9 +5397,13 @@
4778	5397	static void _tcpm_pd_hard_reset(struct tcpm_port *port)
4779	5398	{
4780	5399	tcpm_log_force(port, "Received hard reset");
	5400	+ if (port->bist_request == BDO_MODE_TESTDATA && port->tcpc->set_bist_data)
	5401	+ port->tcpc->set_bist_data(port->tcpc, false);
4781	5402
4782	5403	if (port->ams != NONE_AMS)
4783	5404	port->ams = NONE_AMS;
	5405	+ if (port->hard_reset_count < PD_N_HARD_RESET_COUNT)
	5406	+ port->ams = HARD_RESET;
4784	5407	/*
4785	5408	* If we keep receiving hard reset requests, executing the hard reset
4786	5409	* must have failed. Revert to error recovery if that happens.
..	..	@@ -4797,7 +5420,9 @@
4797	5420	event_work);
4798	5421	u32 events;
4799	5422
	5423	+#ifdef CONFIG_NO_GKI
4800	5424	mutex_lock(&port->pd_handler_lock);
	5425	+#endif
4801	5426	mutex_lock(&port->lock);
4802	5427
4803	5428	spin_lock(&port->pd_event_lock);
..	..	@@ -4811,10 +5436,19 @@
4811	5436	bool vbus;
4812	5437
4813	5438	vbus = port->tcpc->get_vbus(port->tcpc);
4814		- if (vbus)
	5439	+ if (vbus) {
4815	5440	_tcpm_pd_vbus_on(port);
4816		- else
	5441	+ } else {
4817	5442	_tcpm_pd_vbus_off(port);
	5443	+ /*
	5444	+ * When TCPC does not support detecting vsafe0v voltage level,
	5445	+ * treat vbus absent as vsafe0v. Else invoke is_vbus_vsafe0v
	5446	+ * to see if vbus has discharge to VSAFE0V.
	5447	+ */
	5448	+ if (!port->tcpc->is_vbus_vsafe0v \|\|
	5449	+ port->tcpc->is_vbus_vsafe0v(port->tcpc))
	5450	+ _tcpm_pd_vbus_vsafe0v(port);
	5451	+ }
4818	5452	}
4819	5453	if (events & TCPM_CC_EVENT) {
4820	5454	enum typec_cc_status cc1, cc2;
..	..	@@ -4851,7 +5485,9 @@
4851	5485	}
4852	5486	spin_unlock(&port->pd_event_lock);
4853	5487	mutex_unlock(&port->lock);
	5488	+#ifdef CONFIG_NO_GKI
4854	5489	mutex_unlock(&port->pd_handler_lock);
	5490	+#endif
4855	5491	}
4856	5492
4857	5493	void tcpm_cc_change(struct tcpm_port *port)
..	..	@@ -4931,27 +5567,41 @@
4931	5567	mutex_unlock(&port->lock);
4932	5568	}
4933	5569
4934		-static void tcpm_data_role_swap_work(struct kthread_work *work)
	5570	+static void tcpm_send_discover_work(struct kthread_work *work)
4935	5571	{
4936		- struct tcpm_port *port =
4937		- container_of(work, struct tcpm_port, data_role_swap);
	5572	+ struct tcpm_port *port = container_of(work, struct tcpm_port, send_discover_work);
4938	5573
4939		- if (tcpm_port_is_disconnected(port))
4940		- return;
	5574	+ mutex_lock(&port->lock);
	5575	+ /* No need to send DISCOVER_IDENTITY anymore */
	5576	+ if (!port->send_discover)
	5577	+ goto unlock;
4941	5578
4942		- tcpm_send_data_role_notify(port, true, port->data_role);
	5579	+ if (port->data_role == TYPEC_DEVICE && port->negotiated_rev < PD_REV30) {
	5580	+ port->send_discover = false;
	5581	+ goto unlock;
	5582	+ }
	5583	+
	5584	+ /* Retry if the port is not idle */
	5585	+ if ((port->state != SRC_READY && port->state != SNK_READY) \|\| port->vdm_sm_running) {
	5586	+ mod_send_discover_delayed_work(port, SEND_DISCOVER_RETRY_MS);
	5587	+ goto unlock;
	5588	+ }
	5589	+
	5590	+ tcpm_send_vdm(port, USB_SID_PD, CMD_DISCOVER_IDENT, NULL, 0);
	5591	+
	5592	+unlock:
	5593	+ mutex_unlock(&port->lock);
4943	5594	}
4944	5595
4945		-static int tcpm_dr_set(const struct typec_capability *cap,
4946		- enum typec_data_role data)
	5596	+static int tcpm_dr_set(struct typec_port *p, enum typec_data_role data)
4947	5597	{
4948		- struct tcpm_port *port = typec_cap_to_tcpm(cap);
	5598	+ struct tcpm_port *port = typec_get_drvdata(p);
4949	5599	int ret;
4950	5600
4951	5601	mutex_lock(&port->swap_lock);
4952	5602	mutex_lock(&port->lock);
4953	5603
4954		- if (port->port_type != TYPEC_PORT_DRP) {
	5604	+ if (port->typec_caps.data != TYPEC_PORT_DRD) {
4955	5605	ret = -EINVAL;
4956	5606	goto port_unlock;
4957	5607	}
..	..	@@ -5015,10 +5665,9 @@
5015	5665	return ret;
5016	5666	}
5017	5667
5018		-static int tcpm_pr_set(const struct typec_capability *cap,
5019		- enum typec_role role)
	5668	+static int tcpm_pr_set(struct typec_port *p, enum typec_role role)
5020	5669	{
5021		- struct tcpm_port *port = typec_cap_to_tcpm(cap);
	5670	+ struct tcpm_port *port = typec_get_drvdata(p);
5022	5671	int ret;
5023	5672
5024	5673	mutex_lock(&port->swap_lock);
..	..	@@ -5065,10 +5714,9 @@
5065	5714	return ret;
5066	5715	}
5067	5716
5068		-static int tcpm_vconn_set(const struct typec_capability *cap,
5069		- enum typec_role role)
	5717	+static int tcpm_vconn_set(struct typec_port *p, enum typec_role role)
5070	5718	{
5071		- struct tcpm_port *port = typec_cap_to_tcpm(cap);
	5719	+ struct tcpm_port *port = typec_get_drvdata(p);
5072	5720	int ret;
5073	5721
5074	5722	mutex_lock(&port->swap_lock);
..	..	@@ -5111,9 +5759,9 @@
5111	5759	return ret;
5112	5760	}
5113	5761
5114		-static int tcpm_try_role(const struct typec_capability *cap, int role)
	5762	+static int tcpm_try_role(struct typec_port *p, int role)
5115	5763	{
5116		- struct tcpm_port *port = typec_cap_to_tcpm(cap);
	5764	+ struct tcpm_port *port = typec_get_drvdata(p);
5117	5765	struct tcpc_dev *tcpc = port->tcpc;
5118	5766	int ret = 0;
5119	5767
..	..	@@ -5129,7 +5777,7 @@
5129	5777	return ret;
5130	5778	}
5131	5779
5132		-static int tcpm_pps_set_op_curr(struct tcpm_port *port, u16 op_curr)
	5780	+static int tcpm_pps_set_op_curr(struct tcpm_port *port, u16 req_op_curr)
5133	5781	{
5134	5782	unsigned int target_mw;
5135	5783	int ret;
..	..	@@ -5147,12 +5795,12 @@
5147	5795	goto port_unlock;
5148	5796	}
5149	5797
5150		- if (op_curr > port->pps_data.max_curr) {
	5798	+ if (req_op_curr > port->pps_data.max_curr) {
5151	5799	ret = -EINVAL;
5152	5800	goto port_unlock;
5153	5801	}
5154	5802
5155		- target_mw = (op_curr * port->pps_data.out_volt) / 1000;
	5803	+ target_mw = (req_op_curr * port->supply_voltage) / 1000;
5156	5804	if (target_mw < port->operating_snk_mw) {
5157	5805	ret = -EINVAL;
5158	5806	goto port_unlock;
..	..	@@ -5166,10 +5814,10 @@
5166	5814	}
5167	5815
5168	5816	/* Round down operating current to align with PPS valid steps */
5169		- op_curr = op_curr - (op_curr % RDO_PROG_CURR_MA_STEP);
	5817	+ req_op_curr = req_op_curr - (req_op_curr % RDO_PROG_CURR_MA_STEP);
5170	5818
5171	5819	reinit_completion(&port->pps_complete);
5172		- port->pps_data.op_curr = op_curr;
	5820	+ port->pps_data.req_op_curr = req_op_curr;
5173	5821	port->pps_status = 0;
5174	5822	port->pps_pending = true;
5175	5823	mutex_unlock(&port->lock);
..	..	@@ -5190,7 +5838,7 @@
5190	5838	return ret;
5191	5839	}
5192	5840
5193		-static int tcpm_pps_set_out_volt(struct tcpm_port *port, u16 out_volt)
	5841	+static int tcpm_pps_set_out_volt(struct tcpm_port *port, u16 req_out_volt)
5194	5842	{
5195	5843	unsigned int target_mw;
5196	5844	int ret;
..	..	@@ -5208,13 +5856,13 @@
5208	5856	goto port_unlock;
5209	5857	}
5210	5858
5211		- if (out_volt < port->pps_data.min_volt \|\|
5212		- out_volt > port->pps_data.max_volt) {
	5859	+ if (req_out_volt < port->pps_data.min_volt \|\|
	5860	+ req_out_volt > port->pps_data.max_volt) {
5213	5861	ret = -EINVAL;
5214	5862	goto port_unlock;
5215	5863	}
5216	5864
5217		- target_mw = (port->pps_data.op_curr * out_volt) / 1000;
	5865	+ target_mw = (port->current_limit * req_out_volt) / 1000;
5218	5866	if (target_mw < port->operating_snk_mw) {
5219	5867	ret = -EINVAL;
5220	5868	goto port_unlock;
..	..	@@ -5228,10 +5876,10 @@
5228	5876	}
5229	5877
5230	5878	/* Round down output voltage to align with PPS valid steps */
5231		- out_volt = out_volt - (out_volt % RDO_PROG_VOLT_MV_STEP);
	5879	+ req_out_volt = req_out_volt - (req_out_volt % RDO_PROG_VOLT_MV_STEP);
5232	5880
5233	5881	reinit_completion(&port->pps_complete);
5234		- port->pps_data.out_volt = out_volt;
	5882	+ port->pps_data.req_out_volt = req_out_volt;
5235	5883	port->pps_status = 0;
5236	5884	port->pps_pending = true;
5237	5885	mutex_unlock(&port->lock);
..	..	@@ -5289,8 +5937,8 @@
5289	5937
5290	5938	/* Trigger PPS request or move back to standard PDO contract */
5291	5939	if (activate) {
5292		- port->pps_data.out_volt = port->supply_voltage;
5293		- port->pps_data.op_curr = port->current_limit;
	5940	+ port->pps_data.req_out_volt = port->supply_voltage;
	5941	+ port->pps_data.req_op_curr = port->current_limit;
5294	5942	}
5295	5943	mutex_unlock(&port->lock);
5296	5944
..	..	@@ -5327,6 +5975,24 @@
5327	5975	if (port->vbus_present)
5328	5976	port->vbus_never_low = true;
5329	5977
	5978	+ /*
	5979	+ * 1. When vbus_present is true, voltage on VBUS is already at VSAFE5V.
	5980	+ * So implicitly vbus_vsafe0v = false.
	5981	+ *
	5982	+ * 2. When vbus_present is false and TCPC does NOT support querying
	5983	+ * vsafe0v status, then, it's best to assume vbus is at VSAFE0V i.e.
	5984	+ * vbus_vsafe0v is true.
	5985	+ *
	5986	+ * 3. When vbus_present is false and TCPC does support querying vsafe0v,
	5987	+ * then, query tcpc for vsafe0v status.
	5988	+ */
	5989	+ if (port->vbus_present)
	5990	+ port->vbus_vsafe0v = false;
	5991	+ else if (!port->tcpc->is_vbus_vsafe0v)
	5992	+ port->vbus_vsafe0v = true;
	5993	+ else
	5994	+ port->vbus_vsafe0v = port->tcpc->is_vbus_vsafe0v(port->tcpc);
	5995	+
5330	5996	tcpm_set_state(port, tcpm_default_state(port), 0);
5331	5997
5332	5998	if (port->tcpc->get_cc(port->tcpc, &cc1, &cc2) == 0)
..	..	@@ -5339,10 +6005,9 @@
5339	6005	tcpm_set_state(port, PORT_RESET, 0);
5340	6006	}
5341	6007
5342		-static int tcpm_port_type_set(const struct typec_capability *cap,
5343		- enum typec_port_type type)
	6008	+static int tcpm_port_type_set(struct typec_port *p, enum typec_port_type type)
5344	6009	{
5345		- struct tcpm_port *port = typec_cap_to_tcpm(cap);
	6010	+ struct tcpm_port *port = typec_get_drvdata(p);
5346	6011
5347	6012	mutex_lock(&port->lock);
5348	6013	if (type == port->port_type)
..	..	@@ -5367,6 +6032,14 @@
5367	6032	return 0;
5368	6033	}
5369	6034
	6035	+static const struct typec_operations tcpm_ops = {
	6036	+ .try_role = tcpm_try_role,
	6037	+ .dr_set = tcpm_dr_set,
	6038	+ .pr_set = tcpm_pr_set,
	6039	+ .vconn_set = tcpm_vconn_set,
	6040	+ .port_type_set = tcpm_port_type_set
	6041	+};
	6042	+
5370	6043	void tcpm_tcpc_reset(struct tcpm_port *port)
5371	6044	{
5372	6045	mutex_lock(&port->lock);
..	..	@@ -5376,63 +6049,48 @@
5376	6049	}
5377	6050	EXPORT_SYMBOL_GPL(tcpm_tcpc_reset);
5378	6051
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		-
5393	6052	static int tcpm_fw_get_caps(struct tcpm_port *port,
5394	6053	struct fwnode_handle *fwnode)
5395	6054	{
5396	6055	const char *cap_str;
5397	6056	int ret;
5398		- u32 mw, frs_current;
	6057	+ u32 mw, frs_current, pd_revision;
5399	6058
5400	6059	if (!fwnode)
5401	6060	return -EINVAL;
5402	6061
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		- }
	6062	+ ret = fwnode_property_read_u32(fwnode, "pd-revision",
	6063	+ &pd_revision);
	6064	+ if (ret < 0)
	6065	+ port->typec_caps.pd_revision = 0x0300;
	6066	+ else
	6067	+ port->typec_caps.pd_revision = pd_revision & 0xffff;
5412	6068
5413	6069	/* USB data support is optional */
5414	6070	ret = fwnode_property_read_string(fwnode, "data-role", &cap_str);
5415	6071	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;
	6072	+ ret = typec_find_port_data_role(cap_str);
	6073	+ if (ret < 0)
	6074	+ return ret;
	6075	+ port->typec_caps.data = ret;
5419	6076	}
5420	6077
5421	6078	ret = fwnode_property_read_string(fwnode, "power-role", &cap_str);
5422	6079	if (ret < 0)
5423	6080	return ret;
5424	6081
5425		- port->typec_caps.type = typec_find_port_power_role(cap_str);
5426		- if (port->typec_caps.type < 0)
5427		- return -EINVAL;
	6082	+ ret = typec_find_port_power_role(cap_str);
	6083	+ if (ret < 0)
	6084	+ return ret;
	6085	+ port->typec_caps.type = ret;
5428	6086	port->port_type = port->typec_caps.type;
5429	6087
	6088	+ port->slow_charger_loop = fwnode_property_read_bool(fwnode, "slow-charger-loop");
5430	6089	if (port->port_type == TYPEC_PORT_SNK)
5431	6090	goto sink;
5432	6091
5433	6092	/* Get source pdos */
5434		- ret = fwnode_property_read_u32_array(fwnode, "source-pdos",
5435		- NULL, 0);
	6093	+ ret = fwnode_property_count_u32(fwnode, "source-pdos");
5436	6094	if (ret <= 0)
5437	6095	return -EINVAL;
5438	6096
..	..	@@ -5456,8 +6114,7 @@
5456	6114	return -EINVAL;
5457	6115	sink:
5458	6116	/* Get sink pdos */
5459		- ret = fwnode_property_read_u32_array(fwnode, "sink-pdos",
5460		- NULL, 0);
	6117	+ ret = fwnode_property_count_u32(fwnode, "sink-pdos");
5461	6118	if (ret <= 0)
5462	6119	return -EINVAL;
5463	6120
..	..	@@ -5476,9 +6133,10 @@
5476	6133
5477	6134	/* FRS can only be supported byb DRP ports */
5478	6135	if (port->port_type == TYPEC_PORT_DRP) {
5479		- ret = fwnode_property_read_u32(fwnode, "frs-typec-current", &frs_current);
	6136	+ ret = fwnode_property_read_u32(fwnode, "new-source-frs-typec-current",
	6137	+ &frs_current);
5480	6138	if (ret >= 0 && frs_current <= FRS_5V_3A)
5481		- port->frs_current = frs_current;
	6139	+ port->new_source_frs_current = frs_current;
5482	6140	}
5483	6141
5484	6142	/* sink-vdos is optional */
..	..	@@ -5495,49 +6153,49 @@
5495	6153	return ret;
5496	6154	}
5497	6155
5498		- return 0;
5499		-}
	6156	+ /* If sink-vdos is found, sink-vdos-v1 is expected for backward compatibility. */
	6157	+ if (port->nr_snk_vdo) {
	6158	+ ret = fwnode_property_count_u32(fwnode, "sink-vdos-v1");
	6159	+ if (ret < 0)
	6160	+ return ret;
	6161	+ else if (ret == 0)
	6162	+ return -ENODATA;
5500	6163
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;
	6164	+ port->nr_snk_vdo_v1 = min(ret, VDO_MAX_OBJECTS);
	6165	+ ret = fwnode_property_read_u32_array(fwnode, "sink-vdos-v1",
	6166	+ port->snk_vdo_v1,
	6167	+ port->nr_snk_vdo_v1);
	6168	+ if (ret < 0)
	6169	+ return ret;
5524	6170	}
5525		- mutex_unlock(&port->lock);
	6171	+
5526	6172	return 0;
5527	6173	}
5528		-EXPORT_SYMBOL_GPL(tcpm_update_source_capabilities);
5529	6174
5530		-int tcpm_update_sink_capabilities(struct tcpm_port port, const u32 pdo,
5531		- unsigned int nr_pdo,
	6175	+static int tcpm_copy_pdos(u32 dest_pdo, const u32 src_pdo, unsigned int nr_pdo)
	6176	+{
	6177	+ unsigned int i;
	6178	+
	6179	+ if (nr_pdo > PDO_MAX_OBJECTS)
	6180	+ nr_pdo = PDO_MAX_OBJECTS;
	6181	+
	6182	+ for (i = 0; i < nr_pdo; i++)
	6183	+ dest_pdo[i] = src_pdo[i];
	6184	+
	6185	+ return nr_pdo;
	6186	+}
	6187	+
	6188	+int tcpm_update_sink_capabilities(struct tcpm_port port, const u32 pdo, unsigned int nr_pdo,
5532	6189	unsigned int operating_snk_mw)
5533	6190	{
	6191	+ int ret = 0;
	6192	+
5534	6193	if (tcpm_validate_caps(port, pdo, nr_pdo))
5535	6194	return -EINVAL;
5536	6195
5537	6196	mutex_lock(&port->lock);
5538	6197	port->nr_snk_pdo = tcpm_copy_pdos(port->snk_pdo, pdo, nr_pdo);
5539	6198	port->operating_snk_mw = operating_snk_mw;
5540		- port->update_sink_caps = true;
5541	6199
5542	6200	switch (port->state) {
5543	6201	case SNK_NEGOTIATE_CAPABILITIES:
..	..	@@ -5546,15 +6204,25 @@
5546	6204	case SNK_TRANSITION_SINK:
5547	6205	case SNK_TRANSITION_SINK_VBUS:
5548	6206	if (port->pps_data.active)
5549		- tcpm_set_state(port, SNK_NEGOTIATE_PPS_CAPABILITIES, 0);
	6207	+ port->upcoming_state = SNK_NEGOTIATE_PPS_CAPABILITIES;
	6208	+ else if (port->pd_capable)
	6209	+ port->upcoming_state = SNK_NEGOTIATE_CAPABILITIES;
5550	6210	else
5551		- tcpm_set_state(port, SNK_NEGOTIATE_CAPABILITIES, 0);
	6211	+ break;
	6212	+
	6213	+ port->update_sink_caps = true;
	6214	+
	6215	+ ret = tcpm_ams_start(port, POWER_NEGOTIATION);
	6216	+ if (ret == -EAGAIN) {
	6217	+ port->upcoming_state = INVALID_STATE;
	6218	+ break;
	6219	+ }
5552	6220	break;
5553	6221	default:
5554	6222	break;
5555	6223	}
5556	6224	mutex_unlock(&port->lock);
5557		- return 0;
	6225	+ return ret;
5558	6226	}
5559	6227	EXPORT_SYMBOL_GPL(tcpm_update_sink_capabilities);
5560	6228
..	..	@@ -5639,6 +6307,27 @@
5639	6307	return 0;
5640	6308	}
5641	6309
	6310	+static int tcpm_psy_get_input_power_limit(struct tcpm_port *port,
	6311	+ union power_supply_propval *val)
	6312	+{
	6313	+ unsigned int src_mv, src_ma, max_src_mw = 0;
	6314	+ unsigned int i, tmp;
	6315	+
	6316	+ for (i = 0; i < port->nr_source_caps; i++) {
	6317	+ u32 pdo = port->source_caps[i];
	6318	+
	6319	+ if (pdo_type(pdo) == PDO_TYPE_FIXED) {
	6320	+ src_mv = pdo_fixed_voltage(pdo);
	6321	+ src_ma = pdo_max_current(pdo);
	6322	+ tmp = src_mv * src_ma / 1000;
	6323	+ max_src_mw = tmp > max_src_mw ? tmp : max_src_mw;
	6324	+ }
	6325	+ }
	6326	+
	6327	+ val->intval = max_src_mw;
	6328	+ return 0;
	6329	+}
	6330	+
5642	6331	static int tcpm_psy_get_prop(struct power_supply *psy,
5643	6332	enum power_supply_property psp,
5644	6333	union power_supply_propval *val)
..	..	@@ -5668,11 +6357,13 @@
5668	6357	case POWER_SUPPLY_PROP_CURRENT_NOW:
5669	6358	ret = tcpm_psy_get_current_now(port, val);
5670	6359	break;
	6360	+ case POWER_SUPPLY_PROP_INPUT_POWER_LIMIT:
	6361	+ tcpm_psy_get_input_power_limit(port, val);
	6362	+ break;
5671	6363	default:
5672	6364	ret = -EINVAL;
5673	6365	break;
5674	6366	}
5675		-
5676	6367	return ret;
5677	6368	}
5678	6369
..	..	@@ -5724,7 +6415,7 @@
5724	6415	ret = -EINVAL;
5725	6416	break;
5726	6417	}
5727		-
	6418	+ power_supply_changed(port->psy);
5728	6419	return ret;
5729	6420	}
5730	6421
..	..	@@ -5807,92 +6498,18 @@
5807	6498	return HRTIMER_NORESTART;
5808	6499	}
5809	6500
5810		-static enum hrtimer_restart data_role_swap_timer_handler(struct hrtimer *timer)
	6501	+static enum hrtimer_restart send_discover_timer_handler(struct hrtimer *timer)
5811	6502	{
5812		- struct tcpm_port *port =
5813		- container_of(timer, struct tcpm_port, data_role_swap_timer);
	6503	+ struct tcpm_port *port = container_of(timer, struct tcpm_port, send_discover_timer);
5814	6504
5815		- kthread_queue_work(port->wq, &port->data_role_swap);
	6505	+ kthread_queue_work(port->wq, &port->send_discover_work);
5816	6506	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;
5889	6507	}
5890	6508
5891	6509	struct tcpm_port tcpm_register_port(struct device dev, struct tcpc_dev *tcpc)
5892	6510	{
5893	6511	struct tcpm_port *port;
5894	6512	int err;
5895		- struct sched_param param = { .sched_priority = MAX_RT_PRIO - 1 };
5896	6513
5897	6514	if (!dev \|\| !tcpc \|\|
5898	6515	!tcpc->get_vbus \|\| !tcpc->set_cc \|\| !tcpc->get_cc \|\|
..	..	@@ -5909,26 +6526,28 @@
5909	6526
5910	6527	mutex_init(&port->lock);
5911	6528	mutex_init(&port->swap_lock);
	6529	+#ifdef CONFIG_NO_GKI
5912	6530	mutex_init(&port->pd_handler_lock);
	6531	+#endif
5913	6532
5914	6533	port->wq = kthread_create_worker(0, dev_name(dev));
5915	6534	if (IS_ERR(port->wq))
5916	6535	return ERR_CAST(port->wq);
5917		- sched_setscheduler_nocheck(port->wq->task, SCHED_FIFO, &param);
	6536	+ sched_set_fifo(port->wq->task);
5918	6537
5919	6538	kthread_init_work(&port->state_machine, tcpm_state_machine_work);
5920	6539	kthread_init_work(&port->vdm_state_machine, vdm_state_machine_work);
5921	6540	kthread_init_work(&port->event_work, tcpm_pd_event_handler);
5922	6541	kthread_init_work(&port->enable_frs, tcpm_enable_frs_work);
5923		- kthread_init_work(&port->data_role_swap, tcpm_data_role_swap_work);
	6542	+ kthread_init_work(&port->send_discover_work, tcpm_send_discover_work);
5924	6543	hrtimer_init(&port->state_machine_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
5925	6544	port->state_machine_timer.function = state_machine_timer_handler;
5926	6545	hrtimer_init(&port->vdm_state_machine_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
5927	6546	port->vdm_state_machine_timer.function = vdm_state_machine_timer_handler;
5928	6547	hrtimer_init(&port->enable_frs_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
5929	6548	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;
	6549	+ hrtimer_init(&port->send_discover_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
	6550	+ port->send_discover_timer.function = send_discover_timer_handler;
5932	6551
5933	6552	spin_lock_init(&port->pd_event_lock);
5934	6553
..	..	@@ -5936,11 +6555,6 @@
5936	6555	init_completion(&port->swap_complete);
5937	6556	init_completion(&port->pps_complete);
5938	6557	tcpm_debugfs_init(port);
5939		-
5940		- /* init extcon */
5941		- err = tcpm_extcon_register(port);
5942		- if (err)
5943		- goto out_destroy_wq;
5944	6558
5945	6559	err = tcpm_fw_get_caps(port, tcpc->fwnode);
5946	6560	if (err < 0)
..	..	@@ -5950,12 +6564,10 @@
5950	6564
5951	6565	port->typec_caps.fwnode = tcpc->fwnode;
5952	6566	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;
	6567	+ port->typec_caps.svdm_version = SVDM_VER_2_0;
	6568	+ port->typec_caps.driver_data = port;
	6569	+ port->typec_caps.ops = &tcpm_ops;
	6570	+ port->typec_caps.orientation_aware = 1;
5959	6571
5960	6572	port->partner_desc.identity = &port->partner_ident;
5961	6573	port->port_type = port->typec_caps.type;
..	..	@@ -5968,12 +6580,13 @@
5968	6580
5969	6581	err = devm_tcpm_psy_register(port);
5970	6582	if (err)
5971		- goto out_destroy_wq;
	6583	+ goto out_role_sw_put;
	6584	+ power_supply_changed(port->psy);
5972	6585
5973	6586	port->typec_port = typec_register_port(port->dev, &port->typec_caps);
5974	6587	if (IS_ERR(port->typec_port)) {
5975	6588	err = PTR_ERR(port->typec_port);
5976		- goto out_destroy_wq;
	6589	+ goto out_role_sw_put;
5977	6590	}
5978	6591
5979	6592	typec_port_register_altmodes(port->typec_port,
..	..	@@ -5987,8 +6600,10 @@
5987	6600	tcpm_log(port, "%s: registered", dev_name(dev));
5988	6601	return port;
5989	6602
5990		-out_destroy_wq:
	6603	+out_role_sw_put:
5991	6604	usb_role_switch_put(port->role_sw);
	6605	+out_destroy_wq:
	6606	+ tcpm_debugfs_exit(port);
5992	6607	kthread_destroy_worker(port->wq);
5993	6608	return ERR_PTR(err);
5994	6609	}
..	..	@@ -5998,10 +6613,10 @@
5998	6613	{
5999	6614	int i;
6000	6615
	6616	+ hrtimer_cancel(&port->send_discover_timer);
6001	6617	hrtimer_cancel(&port->enable_frs_timer);
6002	6618	hrtimer_cancel(&port->vdm_state_machine_timer);
6003	6619	hrtimer_cancel(&port->state_machine_timer);
6004		- hrtimer_cancel(&port->data_role_swap_timer);
6005	6620
6006	6621	tcpm_reset_port(port);
6007	6622	for (i = 0; i < ARRAY_SIZE(port->port_altmode); i++)