~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,139 @@
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;
	1727	+ } else {
	1728	+ /* Do dr_swap for ufp if the port supports drd */
	1729	+ if (port->typec_caps.data == TYPEC_PORT_DRD &&
	1730	+ !IS_ERR_OR_NULL(port->port_altmode[0])) {
	1731	+ port->vdm_sm_running = false;
	1732	+ port->upcoming_state = DR_SWAP_SEND;
	1733	+ tcpm_ams_start(port, DATA_ROLE_SWAP);
	1734	+ }
1629	1735	}
1630	1736	break;
1631	1737	case CMD_ENTER_MODE:
1632	1738	if (adev && pdev) {
1633	1739	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		- }
	1740	+ *adev_action = ADEV_QUEUE_VDM_SEND_EXIT_MODE_ON_FAIL;
1641	1741	}
1642	1742	return 0;
1643	1743	case CMD_EXIT_MODE:
1644	1744	if (adev && pdev) {
1645	1745	typec_altmode_update_active(pdev, false);
1646		-
1647	1746	/* Back to USB Operation */
1648		- WARN_ON(typec_altmode_notify(adev,
1649		- TYPEC_STATE_USB,
1650		- NULL));
	1747	+ *adev_action = ADEV_NOTIFY_USB_AND_QUEUE_VDM;
	1748	+ return 0;
1651	1749	}
1652	1750	break;
	1751	+ case VDO_CMD_VENDOR(0) ... VDO_CMD_VENDOR(15):
	1752	+ break;
1653	1753	default:
	1754	+ /* Unrecognized SVDM */
	1755	+ response[0] = p[0] \| VDO_CMDT(CMDT_RSP_NAK);
	1756	+ rlen = 1;
	1757	+ response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) \|
	1758	+ (VDO_SVDM_VERS(svdm_version));
1654	1759	break;
1655	1760	}
1656	1761	break;
1657	1762	case CMDT_RSP_NAK:
1658	1763	tcpm_ams_finish(port);
1659	1764	switch (cmd) {
	1765	+ case CMD_DISCOVER_IDENT:
	1766	+ /* Do dr_swap for ufp if the port supports drd */
	1767	+ if (port->typec_caps.data == TYPEC_PORT_DRD &&
	1768	+ port->data_role == TYPEC_DEVICE &&
	1769	+ !IS_ERR_OR_NULL(port->port_altmode[0])) {
	1770	+ port->vdm_sm_running = false;
	1771	+ port->upcoming_state = DR_SWAP_SEND;
	1772	+ tcpm_ams_start(port, DATA_ROLE_SWAP);
	1773	+ break;
	1774	+ }
	1775	+ fallthrough;
	1776	+ case CMD_DISCOVER_SVID:
	1777	+ case CMD_DISCOVER_MODES:
	1778	+ case VDO_CMD_VENDOR(0) ... VDO_CMD_VENDOR(15):
	1779	+ break;
1660	1780	case CMD_ENTER_MODE:
1661	1781	/* Back to USB Operation */
1662		- if (adev)
1663		- WARN_ON(typec_altmode_notify(adev,
1664		- TYPEC_STATE_USB,
1665		- NULL));
1666		- break;
	1782	+ *adev_action = ADEV_NOTIFY_USB_AND_QUEUE_VDM;
	1783	+ return 0;
1667	1784	default:
	1785	+ /* Unrecognized SVDM */
	1786	+ response[0] = p[0] \| VDO_CMDT(CMDT_RSP_NAK);
	1787	+ rlen = 1;
	1788	+ response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) \|
	1789	+ (VDO_SVDM_VERS(svdm_version));
1668	1790	break;
1669	1791	}
1670		- port->vdm_sm_running = false;
1671	1792	break;
1672	1793	default:
1673		- port->vdm_sm_running = false;
	1794	+ response[0] = p[0] \| VDO_CMDT(CMDT_RSP_NAK);
	1795	+ rlen = 1;
	1796	+ response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) \|
	1797	+ (VDO_SVDM_VERS(svdm_version));
1674	1798	break;
1675	1799	}
1676	1800
1677	1801	/* Informing the alternate mode drivers about everything */
1678		- if (adev)
1679		- typec_altmode_vdm(adev, p[0], &p[1], cnt);
1680		-
	1802	+ *adev_action = ADEV_QUEUE_VDM;
1681	1803	return rlen;
1682	1804	}
	1805	+
	1806	+static void tcpm_pd_handle_msg(struct tcpm_port *port,
	1807	+ enum pd_msg_request message,
	1808	+ enum tcpm_ams ams);
1683	1809
1684	1810	static void tcpm_handle_vdm_request(struct tcpm_port *port,
1685	1811	const __le32 *payload, int cnt)
1686	1812	{
1687		- int rlen = 0;
	1813	+ enum adev_actions adev_action = ADEV_NONE;
	1814	+ struct typec_altmode *adev;
	1815	+ u32 p[PD_MAX_PAYLOAD];
1688	1816	u32 response[8] = { };
1689		- u32 p0 = le32_to_cpu(payload[0]);
	1817	+ int i, rlen = 0;
	1818	+
	1819	+ for (i = 0; i < cnt; i++)
	1820	+ p[i] = le32_to_cpu(payload[i]);
	1821	+
	1822	+ adev = typec_match_altmode(port->port_altmode, ALTMODE_DISCOVERY_MAX,
	1823	+ PD_VDO_VID(p[0]), PD_VDO_OPOS(p[0]));
1690	1824
1691	1825	if (port->vdm_state == VDM_STATE_BUSY) {
1692	1826	/* If UFP responded busy retry after timeout */
1693		- if (PD_VDO_CMDT(p0) == CMDT_RSP_BUSY) {
	1827	+ if (PD_VDO_CMDT(p[0]) == CMDT_RSP_BUSY) {
1694	1828	port->vdm_state = VDM_STATE_WAIT_RSP_BUSY;
1695		- port->vdo_retry = (p0 & ~VDO_CMDT_MASK) \|
	1829	+ port->vdo_retry = (p[0] & ~VDO_CMDT_MASK) \|
1696	1830	CMDT_INIT;
1697	1831	mod_vdm_delayed_work(port, PD_T_VDM_BUSY);
1698	1832	return;
..	..	@@ -1700,29 +1834,110 @@
1700	1834	port->vdm_state = VDM_STATE_DONE;
1701	1835	}
1702	1836
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);
	1837	+ if (PD_VDO_SVDM(p[0]) && (adev \|\| tcpm_vdm_ams(port) \|\| port->nr_snk_vdo)) {
	1838	+ /*
	1839	+ * Here a SVDM is received (INIT or RSP or unknown). Set the vdm_sm_running in
	1840	+ * advance because we are dropping the lock but may send VDMs soon.
	1841	+ * For the cases of INIT received:
	1842	+ * - If no response to send, it will be cleared later in this function.
	1843	+ * - If there are responses to send, it will be cleared in the state machine.
	1844	+ * For the cases of RSP received:
	1845	+ * - If no further INIT to send, it will be cleared later in this function.
	1846	+ * - Otherwise, it will be cleared in the state machine if timeout or it will go
	1847	+ * back here until no further INIT to send.
	1848	+ * For the cases of unknown type received:
	1849	+ * - We will send NAK and the flag will be cleared in the state machine.
	1850	+ */
	1851	+ port->vdm_sm_running = true;
	1852	+ rlen = tcpm_pd_svdm(port, adev, p, cnt, response, &adev_action);
	1853	+ } else {
	1854	+ if (port->negotiated_rev >= PD_REV30)
	1855	+ tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
1709	1856	}
	1857	+
	1858	+ /*
	1859	+ * We are done with any state stored in the port struct now, except
	1860	+ * for any port struct changes done by the tcpm_queue_vdm() call
	1861	+ * below, which is a separate operation.
	1862	+ *
	1863	+ * So we can safely release the lock here; and we MUST release the
	1864	+ * lock here to avoid an AB BA lock inversion:
	1865	+ *
	1866	+ * If we keep the lock here then the lock ordering in this path is:
	1867	+ * 1. tcpm_pd_rx_handler take the tcpm port lock
	1868	+ * 2. One of the typec_altmode_* calls below takes the alt-mode's lock
	1869	+ *
	1870	+ * And we also have this ordering:
	1871	+ * 1. alt-mode driver takes the alt-mode's lock
	1872	+ * 2. alt-mode driver calls tcpm_altmode_enter which takes the
	1873	+ * tcpm port lock
	1874	+ *
	1875	+ * Dropping our lock here avoids this.
	1876	+ */
	1877	+ mutex_unlock(&port->lock);
	1878	+
	1879	+ if (adev) {
	1880	+ switch (adev_action) {
	1881	+ case ADEV_NONE:
	1882	+ break;
	1883	+ case ADEV_NOTIFY_USB_AND_QUEUE_VDM:
	1884	+ WARN_ON(typec_altmode_notify(adev, TYPEC_STATE_USB, NULL));
	1885	+ typec_altmode_vdm(adev, p[0], &p[1], cnt);
	1886	+ break;
	1887	+ case ADEV_QUEUE_VDM:
	1888	+ typec_altmode_vdm(adev, p[0], &p[1], cnt);
	1889	+ break;
	1890	+ case ADEV_QUEUE_VDM_SEND_EXIT_MODE_ON_FAIL:
	1891	+ if (typec_altmode_vdm(adev, p[0], &p[1], cnt)) {
	1892	+ int svdm_version = typec_get_negotiated_svdm_version(
	1893	+ port->typec_port);
	1894	+ if (svdm_version < 0)
	1895	+ break;
	1896	+
	1897	+ response[0] = VDO(adev->svid, 1, svdm_version,
	1898	+ CMD_EXIT_MODE);
	1899	+ response[0] \|= VDO_OPOS(adev->mode);
	1900	+ rlen = 1;
	1901	+ }
	1902	+ break;
	1903	+ case ADEV_ATTENTION:
	1904	+ typec_altmode_attention(adev, p[1]);
	1905	+ break;
	1906	+ }
	1907	+ }
	1908	+
	1909	+ /*
	1910	+ * We must re-take the lock here to balance the unlock in
	1911	+ * tcpm_pd_rx_handler, note that no changes, other then the
	1912	+ * tcpm_queue_vdm call, are made while the lock is held again.
	1913	+ * All that is done after the call is unwinding the call stack until
	1914	+ * we return to tcpm_pd_rx_handler and do the unlock there.
	1915	+ */
	1916	+ mutex_lock(&port->lock);
	1917	+
	1918	+ if (rlen > 0)
	1919	+ tcpm_queue_vdm(port, response[0], &response[1], rlen - 1);
	1920	+ else
	1921	+ port->vdm_sm_running = false;
1710	1922	}
1711	1923
1712	1924	static void tcpm_send_vdm(struct tcpm_port *port, u32 vid, int cmd,
1713	1925	const u32 *data, int count)
1714	1926	{
	1927	+ int svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
1715	1928	u32 header;
	1929	+
	1930	+ if (svdm_version < 0)
	1931	+ return;
1716	1932
1717	1933	if (WARN_ON(count > VDO_MAX_SIZE - 1))
1718	1934	count = VDO_MAX_SIZE - 1;
1719	1935
1720	1936	/* set VDM header with VID & CMD */
1721	1937	header = VDO(vid, ((vid & USB_SID_PD) == USB_SID_PD) ?
1722		- 1 : (PD_VDO_CMD(cmd) <= CMD_ATTENTION), cmd);
	1938	+ 1 : (PD_VDO_CMD(cmd) <= CMD_ATTENTION),
	1939	+ svdm_version, cmd);
1723	1940	tcpm_queue_vdm(port, header, data, count);
1724		-
1725		- mod_vdm_delayed_work(port, 0);
1726	1941	}
1727	1942
1728	1943	static unsigned int vdm_ready_timeout(u32 vdm_hdr)
..	..	@@ -1769,8 +1984,10 @@
1769	1984	* if there's traffic or we're not in PDO ready state don't send
1770	1985	* a VDM.
1771	1986	*/
1772		- if (port->state != SRC_READY && port->state != SNK_READY)
	1987	+ if (port->state != SRC_READY && port->state != SNK_READY) {
	1988	+ port->vdm_sm_running = false;
1773	1989	break;
	1990	+ }
1774	1991
1775	1992	/* TODO: AMS operation for Unstructured VDM */
1776	1993	if (PD_VDO_SVDM(vdo_hdr) && PD_VDO_CMDT(vdo_hdr) == CMDT_INIT) {
..	..	@@ -1779,6 +1996,9 @@
1779	1996	res = tcpm_ams_start(port, DISCOVER_IDENTITY);
1780	1997	if (res == 0)
1781	1998	port->send_discover = false;
	1999	+ else if (res == -EAGAIN)
	2000	+ mod_send_discover_delayed_work(port,
	2001	+ SEND_DISCOVER_RETRY_MS);
1782	2002	break;
1783	2003	case CMD_DISCOVER_SVID:
1784	2004	res = tcpm_ams_start(port, DISCOVER_SVIDS);
..	..	@@ -1804,7 +2024,7 @@
1804	2024	}
1805	2025
1806	2026	if (res < 0) {
1807		- port->vdm_sm_running = false;
	2027	+ port->vdm_state = VDM_STATE_ERR_BUSY;
1808	2028	return;
1809	2029	}
1810	2030	}
..	..	@@ -1820,6 +2040,7 @@
1820	2040	port->vdo_data[0] = port->vdo_retry;
1821	2041	port->vdo_count = 1;
1822	2042	port->vdm_state = VDM_STATE_READY;
	2043	+ tcpm_ams_finish(port);
1823	2044	break;
1824	2045	case VDM_STATE_BUSY:
1825	2046	port->vdm_state = VDM_STATE_ERR_TMOUT;
..	..	@@ -1888,7 +2109,7 @@
1888	2109	port->vdm_state != VDM_STATE_BUSY &&
1889	2110	port->vdm_state != VDM_STATE_SEND_MESSAGE);
1890	2111
1891		- if (port->vdm_state == VDM_STATE_ERR_TMOUT)
	2112	+ if (port->vdm_state < VDM_STATE_READY)
1892	2113	port->vdm_sm_running = false;
1893	2114
1894	2115	mutex_unlock(&port->lock);
..	..	@@ -2014,101 +2235,46 @@
2014	2235	return 0;
2015	2236	}
2016	2237
2017		-static int tcpm_altmode_enter(struct typec_altmode *altmode)
	2238	+static int tcpm_altmode_enter(struct typec_altmode altmode, u32 vdo)
2018	2239	{
2019	2240	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
	2241	+ int svdm_version;
2020	2242	u32 header;
2021		- int ret = 0;
2022	2243
2023		- mutex_lock(&port->pd_handler_lock);
2024		- if (tcpm_port_is_disconnected(port)) {
2025		- ret = -ENODEV;
2026		- goto unlock;
2027		- }
	2244	+ svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
	2245	+ if (svdm_version < 0)
	2246	+ return svdm_version;
2028	2247
2029		- mutex_lock(&port->lock);
2030		- header = VDO(altmode->svid, 1, CMD_ENTER_MODE);
	2248	+ header = VDO(altmode->svid, vdo ? 2 : 1, svdm_version, CMD_ENTER_MODE);
2031	2249	header \|= VDO_OPOS(altmode->mode);
2032	2250
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;
	2251	+ tcpm_queue_vdm_unlocked(port, header, vdo, vdo ? 1 : 0);
	2252	+ return 0;
2039	2253	}
2040	2254
2041	2255	static int tcpm_altmode_exit(struct typec_altmode *altmode)
2042	2256	{
2043	2257	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
	2258	+ int svdm_version;
2044	2259	u32 header;
2045		- int ret = 0;
2046	2260
2047		- mutex_lock(&port->pd_handler_lock);
2048		- if (tcpm_port_is_disconnected(port)) {
2049		- ret = -ENODEV;
2050		- goto unlock;
2051		- }
	2261	+ svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
	2262	+ if (svdm_version < 0)
	2263	+ return svdm_version;
2052	2264
2053		- mutex_lock(&port->lock);
2054		- header = VDO(altmode->svid, 1, CMD_EXIT_MODE);
	2265	+ header = VDO(altmode->svid, 1, svdm_version, CMD_EXIT_MODE);
2055	2266	header \|= VDO_OPOS(altmode->mode);
2056	2267
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;
	2268	+ tcpm_queue_vdm_unlocked(port, header, NULL, 0);
	2269	+ return 0;
2064	2270	}
2065	2271
2066	2272	static int tcpm_altmode_vdm(struct typec_altmode *altmode,
2067	2273	u32 header, const u32 *data, int count)
2068	2274	{
2069	2275	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
2070		- int ret = 0;
2071	2276
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		- }
	2277	+ tcpm_queue_vdm_unlocked(port, header, data, count - 1);
2112	2278
2113	2279	return 0;
2114	2280	}
..	..	@@ -2117,7 +2283,6 @@
2117	2283	.enter = tcpm_altmode_enter,
2118	2284	.exit = tcpm_altmode_exit,
2119	2285	.vdm = tcpm_altmode_vdm,
2120		- .notify = tcpm_altmode_notify,
2121	2286	};
2122	2287
2123	2288	/*
..	..	@@ -2142,20 +2307,108 @@
2142	2307
2143	2308	if (!type) {
2144	2309	tcpm_log(port, "Alert message received with no type");
	2310	+ tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
2145	2311	return;
2146	2312	}
2147	2313
2148	2314	/* Just handling non-battery alerts for now */
2149	2315	if (!(type & USB_PD_ADO_TYPE_BATT_STATUS_CHANGE)) {
2150		- switch (port->state) {
2151		- case SRC_READY:
2152		- case SNK_READY:
	2316	+ if (port->pwr_role == TYPEC_SOURCE) {
	2317	+ port->upcoming_state = GET_STATUS_SEND;
	2318	+ tcpm_ams_start(port, GETTING_SOURCE_SINK_STATUS);
	2319	+ } else {
	2320	+ /*
	2321	+ * Do not check SinkTxOk here in case the Source doesn't set its Rp to
	2322	+ * SinkTxOk in time.
	2323	+ */
	2324	+ port->ams = GETTING_SOURCE_SINK_STATUS;
2153	2325	tcpm_set_state(port, GET_STATUS_SEND, 0);
2154		- break;
2155		- default:
2156		- tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2157		- break;
2158	2326	}
	2327	+ } else {
	2328	+ tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
	2329	+ }
	2330	+}
	2331	+
	2332	+static int tcpm_set_auto_vbus_discharge_threshold(struct tcpm_port *port,
	2333	+ enum typec_pwr_opmode mode, bool pps_active,
	2334	+ u32 requested_vbus_voltage)
	2335	+{
	2336	+ int ret;
	2337	+
	2338	+ if (!port->tcpc->set_auto_vbus_discharge_threshold)
	2339	+ return 0;
	2340	+
	2341	+ ret = port->tcpc->set_auto_vbus_discharge_threshold(port->tcpc, mode, pps_active,
	2342	+ requested_vbus_voltage);
	2343	+ tcpm_log_force(port,
	2344	+ "set_auto_vbus_discharge_threshold mode:%d pps_active:%c vbus:%u ret:%d",
	2345	+ mode, pps_active ? 'y' : 'n', requested_vbus_voltage, ret);
	2346	+
	2347	+ return ret;
	2348	+}
	2349	+
	2350	+static void tcpm_pd_handle_state(struct tcpm_port *port,
	2351	+ enum tcpm_state state,
	2352	+ enum tcpm_ams ams,
	2353	+ unsigned int delay_ms)
	2354	+{
	2355	+ switch (port->state) {
	2356	+ case SRC_READY:
	2357	+ case SNK_READY:
	2358	+ port->ams = ams;
	2359	+ tcpm_set_state(port, state, delay_ms);
	2360	+ break;
	2361	+ /* 8.3.3.4.1.1 and 6.8.1 power transitioning */
	2362	+ case SNK_TRANSITION_SINK:
	2363	+ case SNK_TRANSITION_SINK_VBUS:
	2364	+ case SRC_TRANSITION_SUPPLY:
	2365	+ tcpm_set_state(port, HARD_RESET_SEND, 0);
	2366	+ break;
	2367	+ default:
	2368	+ if (!tcpm_ams_interruptible(port)) {
	2369	+ tcpm_set_state(port, port->pwr_role == TYPEC_SOURCE ?
	2370	+ SRC_SOFT_RESET_WAIT_SNK_TX :
	2371	+ SNK_SOFT_RESET,
	2372	+ 0);
	2373	+ } else {
	2374	+ /* process the Message 6.8.1 */
	2375	+ port->upcoming_state = state;
	2376	+ port->next_ams = ams;
	2377	+ tcpm_set_state(port, ready_state(port), delay_ms);
	2378	+ }
	2379	+ break;
	2380	+ }
	2381	+}
	2382	+
	2383	+static void tcpm_pd_handle_msg(struct tcpm_port *port,
	2384	+ enum pd_msg_request message,
	2385	+ enum tcpm_ams ams)
	2386	+{
	2387	+ switch (port->state) {
	2388	+ case SRC_READY:
	2389	+ case SNK_READY:
	2390	+ port->ams = ams;
	2391	+ tcpm_queue_message(port, message);
	2392	+ break;
	2393	+ /* PD 3.0 Spec 8.3.3.4.1.1 and 6.8.1 */
	2394	+ case SNK_TRANSITION_SINK:
	2395	+ case SNK_TRANSITION_SINK_VBUS:
	2396	+ case SRC_TRANSITION_SUPPLY:
	2397	+ tcpm_set_state(port, HARD_RESET_SEND, 0);
	2398	+ break;
	2399	+ default:
	2400	+ if (!tcpm_ams_interruptible(port)) {
	2401	+ tcpm_set_state(port, port->pwr_role == TYPEC_SOURCE ?
	2402	+ SRC_SOFT_RESET_WAIT_SNK_TX :
	2403	+ SNK_SOFT_RESET,
	2404	+ 0);
	2405	+ } else {
	2406	+ port->next_ams = ams;
	2407	+ tcpm_set_state(port, ready_state(port), 0);
	2408	+ /* 6.8.1 process the Message */
	2409	+ tcpm_queue_message(port, message);
	2410	+ }
	2411	+ break;
2159	2412	}
2160	2413	}
2161	2414
..	..	@@ -2166,15 +2419,18 @@
2166	2419	unsigned int cnt = pd_header_cnt_le(msg->header);
2167	2420	unsigned int rev = pd_header_rev_le(msg->header);
2168	2421	unsigned int i;
2169		- enum frs_typec_current frs_current;
	2422	+ enum frs_typec_current partner_frs_current;
2170	2423	bool frs_enable;
2171	2424	int ret;
2172	2425
	2426	+ if (tcpm_vdm_ams(port) && type != PD_DATA_VENDOR_DEF) {
	2427	+ port->vdm_state = VDM_STATE_ERR_BUSY;
	2428	+ tcpm_ams_finish(port);
	2429	+ mod_vdm_delayed_work(port, 0);
	2430	+ }
	2431	+
2173	2432	switch (type) {
2174	2433	case PD_DATA_SOURCE_CAP:
2175		- if (port->pwr_role != TYPEC_SINK)
2176		- break;
2177		-
2178	2434	for (i = 0; i < cnt; i++)
2179	2435	port->source_caps[i] = le32_to_cpu(msg->payload[i]);
2180	2436
..	..	@@ -2185,17 +2441,34 @@
2185	2441	tcpm_validate_caps(port, port->source_caps,
2186	2442	port->nr_source_caps);
2187	2443
	2444	+ trace_android_vh_typec_store_partner_src_caps(port, &port->nr_source_caps,
	2445	+ &port->source_caps);
	2446	+
2188	2447	/*
2189	2448	* Adjust revision in subsequent message headers, as required,
2190	2449	* to comply with 6.2.1.1.5 of the USB PD 3.0 spec. We don't
2191	2450	* support Rev 1.0 so just do nothing in that scenario.
2192	2451	*/
2193		- if (rev == PD_REV10)
	2452	+ if (rev == PD_REV10) {
	2453	+ if (port->ams == GET_SOURCE_CAPABILITIES)
	2454	+ tcpm_ams_finish(port);
2194	2455	break;
	2456	+ }
2195	2457
2196	2458	if (rev < PD_MAX_REV)
2197		- port->negotiated_rev = rev;
	2459	+ port->negotiated_rev = min_t(u16, rev, port->negotiated_rev);
2198	2460
	2461	+ if (port->pwr_role == TYPEC_SOURCE) {
	2462	+ if (port->ams == GET_SOURCE_CAPABILITIES)
	2463	+ tcpm_pd_handle_state(port, SRC_READY, NONE_AMS, 0);
	2464	+ /* Unexpected Source Capabilities */
	2465	+ else
	2466	+ tcpm_pd_handle_msg(port,
	2467	+ port->negotiated_rev < PD_REV30 ?
	2468	+ PD_MSG_CTRL_REJECT :
	2469	+ PD_MSG_CTRL_NOT_SUPP,
	2470	+ NONE_AMS);
	2471	+ } else if (port->state == SNK_WAIT_CAPABILITIES) {
2199	2472	/*
2200	2473	* This message may be received even if VBUS is not
2201	2474	* present. This is quite unexpected; see USB PD
..	..	@@ -2209,48 +2482,68 @@
2209	2482	* but be prepared to keep waiting for VBUS after it was
2210	2483	* handled.
2211	2484	*/
2212		- tcpm_set_state(port, SNK_NEGOTIATE_CAPABILITIES, 0);
	2485	+ port->ams = POWER_NEGOTIATION;
	2486	+ port->in_ams = true;
	2487	+ tcpm_set_state(port, SNK_NEGOTIATE_CAPABILITIES, 0);
	2488	+ } else {
	2489	+ if (port->ams == GET_SOURCE_CAPABILITIES)
	2490	+ tcpm_ams_finish(port);
	2491	+ tcpm_pd_handle_state(port, SNK_NEGOTIATE_CAPABILITIES,
	2492	+ POWER_NEGOTIATION, 0);
	2493	+ }
2213	2494	break;
2214	2495	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	2496	/*
2222	2497	* Adjust revision in subsequent message headers, as required,
2223	2498	* to comply with 6.2.1.1.5 of the USB PD 3.0 spec. We don't
2224	2499	* support Rev 1.0 so just reject in that scenario.
2225	2500	*/
2226	2501	if (rev == PD_REV10) {
2227		- tcpm_queue_message(port, PD_MSG_CTRL_REJECT);
	2502	+ tcpm_pd_handle_msg(port,
	2503	+ port->negotiated_rev < PD_REV30 ?
	2504	+ PD_MSG_CTRL_REJECT :
	2505	+ PD_MSG_CTRL_NOT_SUPP,
	2506	+ NONE_AMS);
2228	2507	break;
2229	2508	}
2230	2509
2231	2510	if (rev < PD_MAX_REV)
2232		- port->negotiated_rev = rev;
	2511	+ port->negotiated_rev = min_t(u16, rev, port->negotiated_rev);
	2512	+
	2513	+ if (port->pwr_role != TYPEC_SOURCE \|\| cnt != 1) {
	2514	+ tcpm_pd_handle_msg(port,
	2515	+ port->negotiated_rev < PD_REV30 ?
	2516	+ PD_MSG_CTRL_REJECT :
	2517	+ PD_MSG_CTRL_NOT_SUPP,
	2518	+ NONE_AMS);
	2519	+ break;
	2520	+ }
2233	2521
2234	2522	port->sink_request = le32_to_cpu(msg->payload[0]);
2235	2523
2236	2524	if (port->vdm_sm_running && port->explicit_contract) {
2237		- tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
	2525	+ tcpm_pd_handle_msg(port, PD_MSG_CTRL_WAIT, port->ams);
2238	2526	break;
2239	2527	}
2240	2528
2241		- tcpm_set_state(port, SRC_NEGOTIATE_CAPABILITIES, 0);
	2529	+ if (port->state == SRC_SEND_CAPABILITIES)
	2530	+ tcpm_set_state(port, SRC_NEGOTIATE_CAPABILITIES, 0);
	2531	+ else
	2532	+ tcpm_pd_handle_state(port, SRC_NEGOTIATE_CAPABILITIES,
	2533	+ POWER_NEGOTIATION, 0);
2242	2534	break;
2243	2535	case PD_DATA_SINK_CAP:
2244	2536	/* We don't do anything with this at the moment... */
2245	2537	for (i = 0; i < cnt; i++)
2246	2538	port->sink_caps[i] = le32_to_cpu(msg->payload[i]);
2247	2539
2248		- frs_current = (port->sink_caps[0] & PDO_FIXED_FRS_CURR_MASK) >>
	2540	+ partner_frs_current = (port->sink_caps[0] & PDO_FIXED_FRS_CURR_MASK) >>
2249	2541	PDO_FIXED_FRS_CURR_SHIFT;
2250		- frs_enable = frs_current && (frs_current <= port->frs_current);
	2542	+ frs_enable = partner_frs_current && (partner_frs_current <=
	2543	+ port->new_source_frs_current);
2251	2544	tcpm_log(port,
2252	2545	"Port partner FRS capable partner_frs_current:%u port_frs_current:%u enable:%c",
2253		- frs_current, port->frs_current, frs_enable ? 'y' : 'n');
	2546	+ partner_frs_current, port->new_source_frs_current, frs_enable ? 'y' : 'n');
2254	2547	if (frs_enable) {
2255	2548	ret = port->tcpc->enable_frs(port->tcpc, true);
2256	2549	tcpm_log(port, "Enable FRS %s, ret:%d\n", ret ? "fail" : "success", ret);
..	..	@@ -2258,27 +2551,45 @@
2258	2551
2259	2552	port->nr_sink_caps = cnt;
2260	2553	port->sink_cap_done = true;
2261		- tcpm_set_state(port, SNK_READY, 0);
	2554	+ if (port->ams == GET_SINK_CAPABILITIES)
	2555	+ tcpm_set_state(port, ready_state(port), 0);
	2556	+ /* Unexpected Sink Capabilities */
	2557	+ else
	2558	+ tcpm_pd_handle_msg(port,
	2559	+ port->negotiated_rev < PD_REV30 ?
	2560	+ PD_MSG_CTRL_REJECT :
	2561	+ PD_MSG_CTRL_NOT_SUPP,
	2562	+ NONE_AMS);
2262	2563	break;
2263	2564	case PD_DATA_VENDOR_DEF:
2264	2565	tcpm_handle_vdm_request(port, msg->payload, cnt);
2265	2566	break;
2266	2567	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		- }
	2568	+ port->bist_request = le32_to_cpu(msg->payload[0]);
	2569	+ tcpm_pd_handle_state(port, BIST_RX, BIST, 0);
2271	2570	break;
2272	2571	case PD_DATA_ALERT:
2273		- tcpm_handle_alert(port, msg->payload, cnt);
	2572	+ if (port->state != SRC_READY && port->state != SNK_READY)
	2573	+ tcpm_pd_handle_state(port, port->pwr_role == TYPEC_SOURCE ?
	2574	+ SRC_SOFT_RESET_WAIT_SNK_TX : SNK_SOFT_RESET,
	2575	+ NONE_AMS, 0);
	2576	+ else
	2577	+ tcpm_handle_alert(port, msg->payload, cnt);
2274	2578	break;
2275	2579	case PD_DATA_BATT_STATUS:
2276	2580	case PD_DATA_GET_COUNTRY_INFO:
2277	2581	/* Currently unsupported */
2278		- tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
	2582	+ tcpm_pd_handle_msg(port, port->negotiated_rev < PD_REV30 ?
	2583	+ PD_MSG_CTRL_REJECT :
	2584	+ PD_MSG_CTRL_NOT_SUPP,
	2585	+ NONE_AMS);
2279	2586	break;
2280	2587	default:
2281		- tcpm_log(port, "Unhandled data message type %#x", type);
	2588	+ tcpm_pd_handle_msg(port, port->negotiated_rev < PD_REV30 ?
	2589	+ PD_MSG_CTRL_REJECT :
	2590	+ PD_MSG_CTRL_NOT_SUPP,
	2591	+ NONE_AMS);
	2592	+ tcpm_log(port, "Unrecognized data message type %#x", type);
2282	2593	break;
2283	2594	}
2284	2595	}
..	..	@@ -2298,31 +2609,25 @@
2298	2609	enum pd_ctrl_msg_type type = pd_header_type_le(msg->header);
2299	2610	enum tcpm_state next_state;
2300	2611
	2612	+ /*
	2613	+ * Stop VDM state machine if interrupted by other Messages while NOT_SUPP is allowed in
	2614	+ * VDM AMS if waiting for VDM responses and will be handled later.
	2615	+ */
	2616	+ if (tcpm_vdm_ams(port) && type != PD_CTRL_NOT_SUPP && type != PD_CTRL_GOOD_CRC) {
	2617	+ port->vdm_state = VDM_STATE_ERR_BUSY;
	2618	+ tcpm_ams_finish(port);
	2619	+ mod_vdm_delayed_work(port, 0);
	2620	+ }
	2621	+
2301	2622	switch (type) {
2302	2623	case PD_CTRL_GOOD_CRC:
2303	2624	case PD_CTRL_PING:
2304	2625	break;
2305	2626	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		- }
	2627	+ tcpm_pd_handle_msg(port, PD_MSG_DATA_SOURCE_CAP, GET_SOURCE_CAPABILITIES);
2315	2628	break;
2316	2629	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		- }
	2630	+ tcpm_pd_handle_msg(port, PD_MSG_DATA_SINK_CAP, GET_SINK_CAPABILITIES);
2326	2631	break;
2327	2632	case PD_CTRL_GOTO_MIN:
2328	2633	break;
..	..	@@ -2331,13 +2636,13 @@
2331	2636	case SNK_TRANSITION_SINK:
2332	2637	if (port->vbus_present) {
2333	2638	tcpm_set_current_limit(port,
2334		- port->current_limit,
2335		- port->supply_voltage);
	2639	+ port->req_current_limit,
	2640	+ port->req_supply_voltage);
2336	2641	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;
	2642	+ tcpm_set_auto_vbus_discharge_threshold(port,
	2643	+ TYPEC_PWR_MODE_PD,
	2644	+ port->pps_data.active,
	2645	+ port->supply_voltage);
2341	2646	tcpm_set_state(port, SNK_READY, 0);
2342	2647	} else {
2343	2648	/*
..	..	@@ -2361,6 +2666,11 @@
2361	2666	tcpm_set_state(port, FR_SWAP_SNK_SRC_NEW_SINK_READY, 0);
2362	2667	break;
2363	2668	default:
	2669	+ tcpm_pd_handle_state(port,
	2670	+ port->pwr_role == TYPEC_SOURCE ?
	2671	+ SRC_SOFT_RESET_WAIT_SNK_TX :
	2672	+ SNK_SOFT_RESET,
	2673	+ NONE_AMS, 0);
2364	2674	break;
2365	2675	}
2366	2676	break;
..	..	@@ -2370,26 +2680,28 @@
2370	2680	switch (port->state) {
2371	2681	case SNK_NEGOTIATE_CAPABILITIES:
2372	2682	/* USB PD specification, Figure 8-43 */
2373		- if (port->explicit_contract) {
	2683	+ if (port->explicit_contract)
2374	2684	next_state = SNK_READY;
2375		- if (port->data_role == TYPEC_HOST &&
2376		- port->send_discover)
2377		- port->vdm_sm_running = true;
2378		- } else {
	2685	+ else
2379	2686	next_state = SNK_WAIT_CAPABILITIES;
2380		- }
	2687	+
	2688	+ /* Threshold was relaxed before sending Request. Restore it back. */
	2689	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
	2690	+ port->pps_data.active,
	2691	+ port->supply_voltage);
2381	2692	tcpm_set_state(port, next_state, 0);
2382	2693	break;
2383	2694	case SNK_NEGOTIATE_PPS_CAPABILITIES:
2384	2695	/* 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;
	2696	+ port->pps_data.req_out_volt = port->supply_voltage;
	2697	+ port->pps_data.req_op_curr = port->current_limit;
2387	2698	port->pps_status = (type == PD_CTRL_WAIT ?
2388	2699	-EAGAIN : -EOPNOTSUPP);
2389	2700
2390		- if (port->data_role == TYPEC_HOST &&
2391		- port->send_discover)
2392		- port->vdm_sm_running = true;
	2701	+ /* Threshold was relaxed before sending Request. Restore it back. */
	2702	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
	2703	+ port->pps_data.active,
	2704	+ port->supply_voltage);
2393	2705
2394	2706	tcpm_set_state(port, SNK_READY, 0);
2395	2707	break;
..	..	@@ -2415,7 +2727,22 @@
2415	2727	port->sink_cap_done = true;
2416	2728	tcpm_set_state(port, ready_state(port), 0);
2417	2729	break;
	2730	+ case SRC_READY:
	2731	+ case SNK_READY:
	2732	+ if (port->vdm_state > VDM_STATE_READY) {
	2733	+ port->vdm_state = VDM_STATE_DONE;
	2734	+ if (tcpm_vdm_ams(port))
	2735	+ tcpm_ams_finish(port);
	2736	+ mod_vdm_delayed_work(port, 0);
	2737	+ break;
	2738	+ }
	2739	+ fallthrough;
2418	2740	default:
	2741	+ tcpm_pd_handle_state(port,
	2742	+ port->pwr_role == TYPEC_SOURCE ?
	2743	+ SRC_SOFT_RESET_WAIT_SNK_TX :
	2744	+ SNK_SOFT_RESET,
	2745	+ NONE_AMS, 0);
2419	2746	break;
2420	2747	}
2421	2748	break;
..	..	@@ -2427,13 +2754,15 @@
2427	2754	break;
2428	2755	case SNK_NEGOTIATE_PPS_CAPABILITIES:
2429	2756	port->pps_data.active = true;
2430		- port->supply_voltage = port->pps_data.out_volt;
2431		- port->current_limit = port->pps_data.op_curr;
	2757	+ port->pps_data.min_volt = port->pps_data.req_min_volt;
	2758	+ port->pps_data.max_volt = port->pps_data.req_max_volt;
	2759	+ port->pps_data.max_curr = port->pps_data.req_max_curr;
	2760	+ port->req_supply_voltage = port->pps_data.req_out_volt;
	2761	+ port->req_current_limit = port->pps_data.req_op_curr;
	2762	+ power_supply_changed(port->psy);
2432	2763	tcpm_set_state(port, SNK_TRANSITION_SINK, 0);
2433	2764	break;
2434	2765	case SOFT_RESET_SEND:
2435		- port->message_id = 0;
2436		- port->rx_msgid = -1;
2437	2766	if (port->ams == SOFT_RESET_AMS)
2438	2767	tcpm_ams_finish(port);
2439	2768	if (port->pwr_role == TYPEC_SOURCE) {
..	..	@@ -2444,10 +2773,6 @@
2444	2773	}
2445	2774	break;
2446	2775	case DR_SWAP_SEND:
2447		- if (port->data_role == TYPEC_DEVICE &&
2448		- port->send_discover)
2449		- port->vdm_sm_running = true;
2450		-
2451	2776	tcpm_set_state(port, DR_SWAP_CHANGE_DR, 0);
2452	2777	break;
2453	2778	case PR_SWAP_SEND:
..	..	@@ -2460,69 +2785,62 @@
2460	2785	tcpm_set_state(port, FR_SWAP_SNK_SRC_TRANSITION_TO_OFF, 0);
2461	2786	break;
2462	2787	default:
	2788	+ tcpm_pd_handle_state(port,
	2789	+ port->pwr_role == TYPEC_SOURCE ?
	2790	+ SRC_SOFT_RESET_WAIT_SNK_TX :
	2791	+ SNK_SOFT_RESET,
	2792	+ NONE_AMS, 0);
2463	2793	break;
2464	2794	}
2465	2795	break;
2466	2796	case PD_CTRL_SOFT_RESET:
	2797	+ port->ams = SOFT_RESET_AMS;
2467	2798	tcpm_set_state(port, SOFT_RESET, 0);
2468	2799	break;
2469	2800	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	2801	/*
2475	2802	* XXX
2476	2803	* 6.3.9: If an alternate mode is active, a request to swap
2477	2804	* alternate modes shall trigger a port reset.
2478	2805	*/
2479		- switch (port->state) {
2480		- case SRC_READY:
2481		- case SNK_READY:
2482		- if (port->vdm_sm_running) {
	2806	+ if (port->typec_caps.data != TYPEC_PORT_DRD) {
	2807	+ tcpm_pd_handle_msg(port,
	2808	+ port->negotiated_rev < PD_REV30 ?
	2809	+ PD_MSG_CTRL_REJECT :
	2810	+ PD_MSG_CTRL_NOT_SUPP,
	2811	+ NONE_AMS);
	2812	+ } else {
	2813	+ if (port->send_discover) {
2483	2814	tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2484	2815	break;
2485	2816	}
2486		- tcpm_set_state(port, DR_SWAP_ACCEPT, 0);
2487		- break;
2488		- default:
2489		- tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2490		- break;
	2817	+
	2818	+ tcpm_pd_handle_state(port, DR_SWAP_ACCEPT, DATA_ROLE_SWAP, 0);
2491	2819	}
2492	2820	break;
2493	2821	case PD_CTRL_PR_SWAP:
2494	2822	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) {
	2823	+ tcpm_pd_handle_msg(port,
	2824	+ port->negotiated_rev < PD_REV30 ?
	2825	+ PD_MSG_CTRL_REJECT :
	2826	+ PD_MSG_CTRL_NOT_SUPP,
	2827	+ NONE_AMS);
	2828	+ } else {
	2829	+ if (port->send_discover) {
2502	2830	tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2503	2831	break;
2504	2832	}
2505		- tcpm_set_state(port, PR_SWAP_ACCEPT, 0);
2506		- break;
2507		- default:
2508		- tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2509		- break;
	2833	+
	2834	+ tcpm_pd_handle_state(port, PR_SWAP_ACCEPT, POWER_ROLE_SWAP, 0);
2510	2835	}
2511	2836	break;
2512	2837	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:
	2838	+ if (port->send_discover) {
2523	2839	tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
2524	2840	break;
2525	2841	}
	2842	+
	2843	+ tcpm_pd_handle_state(port, VCONN_SWAP_ACCEPT, VCONN_SWAP, 0);
2526	2844	break;
2527	2845	case PD_CTRL_GET_SOURCE_CAP_EXT:
2528	2846	case PD_CTRL_GET_STATUS:
..	..	@@ -2530,10 +2848,19 @@
2530	2848	case PD_CTRL_GET_PPS_STATUS:
2531	2849	case PD_CTRL_GET_COUNTRY_CODES:
2532	2850	/* Currently not supported */
2533		- tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
	2851	+ tcpm_pd_handle_msg(port,
	2852	+ port->negotiated_rev < PD_REV30 ?
	2853	+ PD_MSG_CTRL_REJECT :
	2854	+ PD_MSG_CTRL_NOT_SUPP,
	2855	+ NONE_AMS);
2534	2856	break;
2535	2857	default:
2536		- tcpm_log(port, "Unhandled ctrl message type %#x", type);
	2858	+ tcpm_pd_handle_msg(port,
	2859	+ port->negotiated_rev < PD_REV30 ?
	2860	+ PD_MSG_CTRL_REJECT :
	2861	+ PD_MSG_CTRL_NOT_SUPP,
	2862	+ NONE_AMS);
	2863	+ tcpm_log(port, "Unrecognized ctrl message type %#x", type);
2537	2864	break;
2538	2865	}
2539	2866	}
..	..	@@ -2544,34 +2871,37 @@
2544	2871	enum pd_ext_msg_type type = pd_header_type_le(msg->header);
2545	2872	unsigned int data_size = pd_ext_header_data_size_le(msg->ext_msg.header);
2546	2873
2547		- if (!(msg->ext_msg.header & PD_EXT_HDR_CHUNKED)) {
	2874	+ /* stopping VDM state machine if interrupted by other Messages */
	2875	+ if (tcpm_vdm_ams(port)) {
	2876	+ port->vdm_state = VDM_STATE_ERR_BUSY;
	2877	+ tcpm_ams_finish(port);
	2878	+ mod_vdm_delayed_work(port, 0);
	2879	+ }
	2880	+
	2881	+ if (!(le16_to_cpu(msg->ext_msg.header) & PD_EXT_HDR_CHUNKED)) {
	2882	+ tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
2548	2883	tcpm_log(port, "Unchunked extended messages unsupported");
2549	2884	return;
2550	2885	}
2551	2886
2552	2887	if (data_size > PD_EXT_MAX_CHUNK_DATA) {
	2888	+ tcpm_pd_handle_state(port, CHUNK_NOT_SUPP, NONE_AMS, PD_T_CHUNK_NOT_SUPP);
2553	2889	tcpm_log(port, "Chunk handling not yet supported");
2554	2890	return;
2555	2891	}
2556	2892
2557	2893	switch (type) {
2558	2894	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	2895	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);
	2896	+ if (port->ams == GETTING_SOURCE_SINK_STATUS) {
	2897	+ tcpm_ams_finish(port);
	2898	+ tcpm_set_state(port, ready_state(port), 0);
	2899	+ } else {
	2900	+ /* unexpected Status or PPS_Status Message */
	2901	+ tcpm_pd_handle_state(port, port->pwr_role == TYPEC_SOURCE ?
	2902	+ SRC_SOFT_RESET_WAIT_SNK_TX : SNK_SOFT_RESET,
	2903	+ NONE_AMS, 0);
	2904	+ }
2575	2905	break;
2576	2906	case PD_EXT_SOURCE_CAP_EXT:
2577	2907	case PD_EXT_GET_BATT_CAP:
..	..	@@ -2585,10 +2915,11 @@
2585	2915	case PD_EXT_FW_UPDATE_RESPONSE:
2586	2916	case PD_EXT_COUNTRY_INFO:
2587	2917	case PD_EXT_COUNTRY_CODES:
2588		- tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
	2918	+ tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
2589	2919	break;
2590	2920	default:
2591		- tcpm_log(port, "Unhandled extended message type %#x", type);
	2921	+ tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
	2922	+ tcpm_log(port, "Unrecognized extended message type %#x", type);
2592	2923	break;
2593	2924	}
2594	2925	}
..	..	@@ -2633,7 +2964,7 @@
2633	2964	"Data role mismatch, initiating error recovery");
2634	2965	tcpm_set_state(port, ERROR_RECOVERY, 0);
2635	2966	} else {
2636		- if (msg->header & PD_HEADER_EXT_HDR)
	2967	+ if (le16_to_cpu(msg->header) & PD_HEADER_EXT_HDR)
2637	2968	tcpm_pd_ext_msg_request(port, msg);
2638	2969	else if (cnt)
2639	2970	tcpm_pd_data_request(port, msg);
..	..	@@ -2684,6 +3015,7 @@
2684	3015	static bool tcpm_send_queued_message(struct tcpm_port *port)
2685	3016	{
2686	3017	enum pd_msg_request queued_message;
	3018	+ int ret;
2687	3019
2688	3020	do {
2689	3021	queued_message = port->queued_message;
..	..	@@ -2700,10 +3032,27 @@
2700	3032	tcpm_pd_send_control(port, PD_CTRL_NOT_SUPP);
2701	3033	break;
2702	3034	case PD_MSG_DATA_SINK_CAP:
2703		- tcpm_pd_send_sink_caps(port);
	3035	+ ret = tcpm_pd_send_sink_caps(port);
	3036	+ if (ret < 0) {
	3037	+ tcpm_log(port, "Unable to send snk caps, ret=%d", ret);
	3038	+ tcpm_set_state(port, SNK_SOFT_RESET, 0);
	3039	+ }
	3040	+ tcpm_ams_finish(port);
2704	3041	break;
2705	3042	case PD_MSG_DATA_SOURCE_CAP:
2706		- tcpm_pd_send_source_caps(port);
	3043	+ ret = tcpm_pd_send_source_caps(port);
	3044	+ if (ret < 0) {
	3045	+ tcpm_log(port,
	3046	+ "Unable to send src caps, ret=%d",
	3047	+ ret);
	3048	+ tcpm_set_state(port, SOFT_RESET_SEND, 0);
	3049	+ } else if (port->pwr_role == TYPEC_SOURCE) {
	3050	+ tcpm_ams_finish(port);
	3051	+ tcpm_set_state(port, HARD_RESET_SEND,
	3052	+ PD_T_SENDER_RESPONSE);
	3053	+ } else {
	3054	+ tcpm_ams_finish(port);
	3055	+ }
2707	3056	break;
2708	3057	default:
2709	3058	break;
..	..	@@ -2791,6 +3140,7 @@
2791	3140
2792	3141	port->pps_data.supported = false;
2793	3142	port->usb_type = POWER_SUPPLY_USB_TYPE_PD;
	3143	+ power_supply_changed(port->psy);
2794	3144
2795	3145	/*
2796	3146	* Select the source PDO providing the most power which has a
..	..	@@ -2815,6 +3165,7 @@
2815	3165	port->pps_data.supported = true;
2816	3166	port->usb_type =
2817	3167	POWER_SUPPLY_USB_TYPE_PD_PPS;
	3168	+ power_supply_changed(port->psy);
2818	3169	}
2819	3170	continue;
2820	3171	default:
..	..	@@ -2884,7 +3235,8 @@
2884	3235	unsigned int i, j, max_mw = 0, max_mv = 0;
2885	3236	unsigned int min_src_mv, max_src_mv, src_ma, src_mw;
2886	3237	unsigned int min_snk_mv, max_snk_mv;
2887		- u32 pdo;
	3238	+ unsigned int max_op_mv;
	3239	+ u32 pdo, src, snk;
2888	3240	unsigned int src_pdo = 0, snk_pdo = 0;
2889	3241
2890	3242	/*
..	..	@@ -2934,16 +3286,18 @@
2934	3286	continue;
2935	3287	}
2936	3288
2937		- if (max_src_mv <= max_snk_mv &&
2938		- min_src_mv >= min_snk_mv) {
	3289	+ if (min_src_mv <= max_snk_mv &&
	3290	+ max_src_mv >= min_snk_mv) {
	3291	+ max_op_mv = min(max_src_mv, max_snk_mv);
	3292	+ src_mw = (max_op_mv * src_ma) / 1000;
2939	3293	/* Prefer higher voltages if available */
2940	3294	if ((src_mw == max_mw &&
2941		- min_src_mv > max_mv) \|\|
	3295	+ max_op_mv > max_mv) \|\|
2942	3296	src_mw > max_mw) {
2943	3297	src_pdo = i;
2944	3298	snk_pdo = j;
2945	3299	max_mw = src_mw;
2946		- max_mv = max_src_mv;
	3300	+ max_mv = max_op_mv;
2947	3301	}
2948	3302	}
2949	3303	}
..	..	@@ -2956,16 +3310,19 @@
2956	3310	}
2957	3311
2958	3312	if (src_pdo) {
2959		- pdo = port->source_caps[src_pdo];
	3313	+ src = port->source_caps[src_pdo];
	3314	+ snk = port->snk_pdo[snk_pdo];
2960	3315
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);
	3316	+ port->pps_data.req_min_volt = max(pdo_pps_apdo_min_voltage(src),
	3317	+ pdo_pps_apdo_min_voltage(snk));
	3318	+ port->pps_data.req_max_volt = min(pdo_pps_apdo_max_voltage(src),
	3319	+ pdo_pps_apdo_max_voltage(snk));
	3320	+ port->pps_data.req_max_curr = min_pps_apdo_current(src, snk);
	3321	+ port->pps_data.req_out_volt = min(port->pps_data.req_max_volt,
	3322	+ max(port->pps_data.req_min_volt,
	3323	+ port->pps_data.req_out_volt));
	3324	+ port->pps_data.req_op_curr = min(port->pps_data.req_max_curr,
	3325	+ port->pps_data.req_op_curr);
2969	3326	}
2970	3327
2971	3328	return src_pdo;
..	..	@@ -3045,8 +3402,8 @@
3045	3402	flags & RDO_CAP_MISMATCH ? " [mismatch]" : "");
3046	3403	}
3047	3404
3048		- port->current_limit = ma;
3049		- port->supply_voltage = mv;
	3405	+ port->req_current_limit = ma;
	3406	+ port->req_supply_voltage = mv;
3050	3407
3051	3408	return 0;
3052	3409	}
..	..	@@ -3060,6 +3417,12 @@
3060	3417	ret = tcpm_pd_build_request(port, &rdo);
3061	3418	if (ret < 0)
3062	3419	return ret;
	3420	+
	3421	+ /*
	3422	+ * Relax the threshold as voltage will be adjusted after Accept Message plus tSrcTransition.
	3423	+ * It is safer to modify the threshold here.
	3424	+ */
	3425	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, 0);
3063	3426
3064	3427	memset(&msg, 0, sizeof(msg));
3065	3428	msg.header = PD_HEADER_LE(PD_DATA_REQUEST,
..	..	@@ -3092,10 +3455,10 @@
3092	3455	tcpm_log(port, "Invalid APDO selected!");
3093	3456	return -EINVAL;
3094	3457	}
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;
	3458	+ max_mv = port->pps_data.req_max_volt;
	3459	+ max_ma = port->pps_data.req_max_curr;
	3460	+ out_mv = port->pps_data.req_out_volt;
	3461	+ op_ma = port->pps_data.req_op_curr;
3099	3462	break;
3100	3463	default:
3101	3464	tcpm_log(port, "Invalid PDO selected!");
..	..	@@ -3142,8 +3505,8 @@
3142	3505	tcpm_log(port, "Requesting APDO %d: %u mV, %u mA",
3143	3506	src_pdo_index, out_mv, op_ma);
3144	3507
3145		- port->pps_data.op_curr = op_ma;
3146		- port->pps_data.out_volt = out_mv;
	3508	+ port->pps_data.req_op_curr = op_ma;
	3509	+ port->pps_data.req_out_volt = out_mv;
3147	3510
3148	3511	return 0;
3149	3512	}
..	..	@@ -3157,6 +3520,9 @@
3157	3520	ret = tcpm_pd_build_pps_request(port, &rdo);
3158	3521	if (ret < 0)
3159	3522	return ret;
	3523	+
	3524	+ /* Relax the threshold as voltage will be adjusted right after Accept Message. */
	3525	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, 0);
3160	3526
3161	3527	memset(&msg, 0, sizeof(msg));
3162	3528	msg.header = PD_HEADER_LE(PD_DATA_REQUEST,
..	..	@@ -3178,10 +3544,6 @@
3178	3544
3179	3545	tcpm_log(port, "vbus:=%d charge=%d", enable, port->vbus_charge);
3180	3546
3181		- ret = tcpm_send_vbus_notify(port, enable);
3182		- if (ret)
3183		- return ret;
3184		-
3185	3547	ret = port->tcpc->set_vbus(port->tcpc, enable, port->vbus_charge);
3186	3548	if (ret < 0)
3187	3549	return ret;
..	..	@@ -3199,17 +3561,13 @@
3199	3561
3200	3562	if (charge != port->vbus_charge) {
3201	3563	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	3564	ret = port->tcpc->set_vbus(port->tcpc, port->vbus_source,
3208	3565	charge);
3209	3566	if (ret < 0)
3210	3567	return ret;
3211	3568	}
3212	3569	port->vbus_charge = charge;
	3570	+ power_supply_changed(port->psy);
3213	3571	return 0;
3214	3572	}
3215	3573
..	..	@@ -3228,10 +3586,6 @@
3228	3586	static int tcpm_init_vbus(struct tcpm_port *port)
3229	3587	{
3230	3588	int ret;
3231		-
3232		- ret = tcpm_send_vbus_notify(port, false);
3233		- if (ret)
3234		- return ret;
3235	3589
3236	3590	ret = port->tcpc->set_vbus(port->tcpc, false, false);
3237	3591	port->vbus_source = false;
..	..	@@ -3280,7 +3634,9 @@
3280	3634	if (ret < 0)
3281	3635	return ret;
3282	3636
3283		- ret = tcpm_set_roles(port, true, TYPEC_SOURCE, TYPEC_HOST);
	3637	+ tcpm_enable_auto_vbus_discharge(port, true);
	3638	+
	3639	+ ret = tcpm_set_roles(port, true, TYPEC_SOURCE, tcpm_data_role_for_source(port));
3284	3640	if (ret < 0)
3285	3641	return ret;
3286	3642
..	..	@@ -3309,12 +3665,8 @@
3309	3665	port->partner = NULL;
3310	3666
3311	3667	port->attached = true;
	3668	+ port->debouncing = false;
3312	3669	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	3670
3319	3671	return 0;
3320	3672
..	..	@@ -3350,26 +3702,26 @@
3350	3702	memset(modep, 0, sizeof(*modep));
3351	3703	}
3352	3704
	3705	+static void tcpm_set_partner_usb_comm_capable(struct tcpm_port *port, bool capable)
	3706	+{
	3707	+ tcpm_log(port, "Setting usb_comm capable %s", capable ? "true" : "false");
	3708	+
	3709	+ if (port->tcpc->set_partner_usb_comm_capable)
	3710	+ port->tcpc->set_partner_usb_comm_capable(port->tcpc, capable);
	3711	+}
	3712	+
3353	3713	static void tcpm_reset_port(struct tcpm_port *port)
3354	3714	{
	3715	+ tcpm_enable_auto_vbus_discharge(port, false);
3355	3716	port->in_ams = false;
3356	3717	port->ams = NONE_AMS;
3357	3718	port->vdm_sm_running = false;
3358	3719	tcpm_unregister_altmodes(port);
3359	3720	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		- }
	3721	+ port->attached = false;
3371	3722	port->pd_capable = false;
3372	3723	port->pps_data.supported = false;
	3724	+ tcpm_set_partner_usb_comm_capable(port, false);
3373	3725
3374	3726	/*
3375	3727	* First Rx ID should be 0; set this to a sentinel of -1 so that
..	..	@@ -3388,12 +3740,11 @@
3388	3740	port->try_src_count = 0;
3389	3741	port->try_snk_count = 0;
3390	3742	port->usb_type = POWER_SUPPLY_USB_TYPE_C;
	3743	+ power_supply_changed(port->psy);
3391	3744	port->nr_sink_caps = 0;
3392	3745	port->sink_cap_done = false;
3393	3746	if (port->tcpc->enable_frs)
3394	3747	port->tcpc->enable_frs(port->tcpc, false);
3395		-
3396		- power_supply_changed(port->psy);
3397	3748	}
3398	3749
3399	3750	static void tcpm_detach(struct tcpm_port *port)
..	..	@@ -3403,6 +3754,11 @@
3403	3754
3404	3755	if (!port->attached)
3405	3756	return;
	3757	+
	3758	+ if (port->tcpc->set_bist_data) {
	3759	+ tcpm_log(port, "disable BIST MODE TESTDATA");
	3760	+ port->tcpc->set_bist_data(port->tcpc, false);
	3761	+ }
3406	3762
3407	3763	tcpm_reset_port(port);
3408	3764	}
..	..	@@ -3424,7 +3780,9 @@
3424	3780	if (ret < 0)
3425	3781	return ret;
3426	3782
3427		- ret = tcpm_set_roles(port, true, TYPEC_SINK, TYPEC_DEVICE);
	3783	+ tcpm_enable_auto_vbus_discharge(port, true);
	3784	+
	3785	+ ret = tcpm_set_roles(port, true, TYPEC_SINK, tcpm_data_role_for_sink(port));
3428	3786	if (ret < 0)
3429	3787	return ret;
3430	3788
..	..	@@ -3433,12 +3791,8 @@
3433	3791	port->partner = NULL;
3434	3792
3435	3793	port->attached = true;
	3794	+ port->debouncing = false;
3436	3795	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	3796
3443	3797	return 0;
3444	3798	}
..	..	@@ -3455,7 +3809,8 @@
3455	3809	if (port->attached)
3456	3810	return 0;
3457	3811
3458		- ret = tcpm_set_roles(port, true, TYPEC_SOURCE, TYPEC_HOST);
	3812	+ ret = tcpm_set_roles(port, true, TYPEC_SOURCE,
	3813	+ tcpm_data_role_for_source(port));
3459	3814	if (ret < 0)
3460	3815	return ret;
3461	3816
..	..	@@ -3464,10 +3819,7 @@
3464	3819	tcpm_typec_connect(port);
3465	3820
3466	3821	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);
	3822	+ port->debouncing = false;
3471	3823
3472	3824	return 0;
3473	3825	}
..	..	@@ -3504,14 +3856,15 @@
3504	3856	return SNK_UNATTACHED;
3505	3857	}
3506	3858
3507		-static void tcpm_check_send_discover(struct tcpm_port *port)
	3859	+bool tcpm_is_toggling(struct tcpm_port *port)
3508	3860	{
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		- }
	3861	+ if (port->port_type == TYPEC_PORT_DRP)
	3862	+ return port->state == SRC_UNATTACHED \|\| port->state == SNK_UNATTACHED \|\|
	3863	+ port->state == TOGGLING;
	3864	+
	3865	+ return false;
3514	3866	}
	3867	+EXPORT_SYMBOL_GPL(tcpm_is_toggling);
3515	3868
3516	3869	static void tcpm_swap_complete(struct tcpm_port *port, int result)
3517	3870	{
..	..	@@ -3540,8 +3893,11 @@
3540	3893	{
3541	3894	int ret;
3542	3895	enum typec_pwr_opmode opmode;
3543		- unsigned int msecs;
	3896	+ unsigned int msecs, timer_val_msecs;
3544	3897	enum tcpm_state upcoming_state;
	3898	+ const char *state_name;
	3899	+ u32 current_limit;
	3900	+ bool adjust;
3545	3901
3546	3902	port->enter_state = port->state;
3547	3903	switch (port->state) {
..	..	@@ -3552,6 +3908,15 @@
3552	3908	if (!port->non_pd_role_swap)
3553	3909	tcpm_swap_complete(port, -ENOTCONN);
3554	3910	tcpm_src_detach(port);
	3911	+ if (port->debouncing) {
	3912	+ port->debouncing = false;
	3913	+ if (port->tcpc->check_contaminant &&
	3914	+ port->tcpc->check_contaminant(port->tcpc)) {
	3915	+ /* Contaminant detection would handle toggling */
	3916	+ tcpm_set_state(port, TOGGLING, 0);
	3917	+ break;
	3918	+ }
	3919	+ }
3555	3920	if (tcpm_start_toggling(port, tcpm_rp_cc(port))) {
3556	3921	tcpm_set_state(port, TOGGLING, 0);
3557	3922	break;
..	..	@@ -3561,20 +3926,25 @@
3561	3926	tcpm_set_state(port, SNK_UNATTACHED, PD_T_DRP_SNK);
3562	3927	break;
3563	3928	case SRC_ATTACH_WAIT:
	3929	+ port->debouncing = true;
	3930	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	3931	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SRC_ATTACH_WAIT],
	3932	+ CC_DEBOUNCE, &timer_val_msecs);
3564	3933	if (tcpm_port_is_debug(port))
3565	3934	tcpm_set_state(port, DEBUG_ACC_ATTACHED,
3566		- PD_T_CC_DEBOUNCE);
	3935	+ timer_val_msecs);
3567	3936	else if (tcpm_port_is_audio(port))
3568	3937	tcpm_set_state(port, AUDIO_ACC_ATTACHED,
3569		- PD_T_CC_DEBOUNCE);
3570		- else if (tcpm_port_is_source(port))
	3938	+ timer_val_msecs);
	3939	+ else if (tcpm_port_is_source(port) && port->vbus_vsafe0v)
3571	3940	tcpm_set_state(port,
3572	3941	tcpm_try_snk(port) ? SNK_TRY
3573	3942	: SRC_ATTACHED,
3574		- PD_T_CC_DEBOUNCE);
	3943	+ timer_val_msecs);
3575	3944	break;
3576	3945
3577	3946	case SNK_TRY:
	3947	+ port->debouncing = false;
3578	3948	port->try_snk_count++;
3579	3949	/*
3580	3950	* Requirements:
..	..	@@ -3597,15 +3967,13 @@
3597	3967	break;
3598	3968	case SNK_TRY_WAIT_DEBOUNCE:
3599	3969	tcpm_set_state(port, SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS,
3600		- PD_T_PD_DEBOUNCE);
	3970	+ PD_T_TRY_CC_DEBOUNCE);
3601	3971	break;
3602	3972	case SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS:
3603		- if (port->vbus_present && tcpm_port_is_sink(port)) {
	3973	+ if (port->vbus_present && tcpm_port_is_sink(port))
3604	3974	tcpm_set_state(port, SNK_ATTACHED, 0);
3605		- } else {
3606		- tcpm_set_state(port, SRC_TRYWAIT, 0);
	3975	+ else
3607	3976	port->max_wait = 0;
3608		- }
3609	3977	break;
3610	3978	case SRC_TRYWAIT:
3611	3979	tcpm_set_cc(port, tcpm_rp_cc(port));
..	..	@@ -3624,7 +3992,10 @@
3624	3992	}
3625	3993	break;
3626	3994	case SRC_TRYWAIT_DEBOUNCE:
3627		- tcpm_set_state(port, SRC_ATTACHED, PD_T_CC_DEBOUNCE);
	3995	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	3996	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SRC_TRYWAIT_DEBOUNCE],
	3997	+ CC_DEBOUNCE, &timer_val_msecs);
	3998	+ tcpm_set_state(port, SRC_ATTACHED, timer_val_msecs);
3628	3999	break;
3629	4000	case SRC_TRYWAIT_UNATTACHED:
3630	4001	tcpm_set_state(port, SNK_UNATTACHED, 0);
..	..	@@ -3640,7 +4011,7 @@
3640	4011	typec_set_pwr_opmode(port->typec_port, opmode);
3641	4012	port->pwr_opmode = TYPEC_PWR_MODE_USB;
3642	4013	port->caps_count = 0;
3643		- port->negotiated_rev = PD_MAX_REV;
	4014	+ port->negotiated_rev = (((port->typec_caps.pd_revision >> 8) & 0xff) - 1);
3644	4015	port->message_id = 0;
3645	4016	port->rx_msgid = -1;
3646	4017	port->explicit_contract = false;
..	..	@@ -3710,6 +4081,8 @@
3710	4081	}
3711	4082	} else {
3712	4083	tcpm_pd_send_control(port, PD_CTRL_ACCEPT);
	4084	+ tcpm_set_partner_usb_comm_capable(port,
	4085	+ !!(port->sink_request & RDO_USB_COMM));
3713	4086	tcpm_set_state(port, SRC_TRANSITION_SUPPLY,
3714	4087	PD_T_SRC_TRANSITION);
3715	4088	}
..	..	@@ -3733,6 +4106,11 @@
3733	4106
3734	4107	if (port->ams != NONE_AMS)
3735	4108	tcpm_ams_finish(port);
	4109	+ if (port->next_ams != NONE_AMS) {
	4110	+ port->ams = port->next_ams;
	4111	+ port->next_ams = NONE_AMS;
	4112	+ }
	4113	+
3736	4114	/*
3737	4115	* If previous AMS is interrupted, switch to the upcoming
3738	4116	* state.
..	..	@@ -3744,7 +4122,18 @@
3744	4122	break;
3745	4123	}
3746	4124
3747		- tcpm_check_send_discover(port);
	4125	+ /*
	4126	+ * 6.4.4.3.1 Discover Identity
	4127	+ * "The Discover Identity Command Shall only be sent to SOP when there is an
	4128	+ * Explicit Contract."
	4129	+ * For now, this driver only supports SOP for DISCOVER_IDENTITY, thus using
	4130	+ * port->explicit_contract to decide whether to send the command.
	4131	+ */
	4132	+ if (port->explicit_contract)
	4133	+ mod_send_discover_delayed_work(port, 0);
	4134	+ else
	4135	+ port->send_discover = false;
	4136	+
3748	4137	/*
3749	4138	* 6.3.5
3750	4139	* Sending ping messages is not necessary if
..	..	@@ -3769,6 +4158,15 @@
3769	4158	tcpm_swap_complete(port, -ENOTCONN);
3770	4159	tcpm_pps_complete(port, -ENOTCONN);
3771	4160	tcpm_snk_detach(port);
	4161	+ if (port->debouncing) {
	4162	+ port->debouncing = false;
	4163	+ if (port->tcpc->check_contaminant &&
	4164	+ port->tcpc->check_contaminant(port->tcpc)) {
	4165	+ /* Contaminant detection would handle toggling */
	4166	+ tcpm_set_state(port, TOGGLING, 0);
	4167	+ break;
	4168	+ }
	4169	+ }
3772	4170	if (tcpm_start_toggling(port, TYPEC_CC_RD)) {
3773	4171	tcpm_set_state(port, TOGGLING, 0);
3774	4172	break;
..	..	@@ -3778,25 +4176,33 @@
3778	4176	tcpm_set_state(port, SRC_UNATTACHED, PD_T_DRP_SRC);
3779	4177	break;
3780	4178	case SNK_ATTACH_WAIT:
	4179	+ port->debouncing = true;
	4180	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	4181	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_ATTACH_WAIT],
	4182	+ CC_DEBOUNCE, &timer_val_msecs);
3781	4183	if ((port->cc1 == TYPEC_CC_OPEN &&
3782	4184	port->cc2 != TYPEC_CC_OPEN) \|\|
3783	4185	(port->cc1 != TYPEC_CC_OPEN &&
3784	4186	port->cc2 == TYPEC_CC_OPEN))
3785	4187	tcpm_set_state(port, SNK_DEBOUNCED,
3786		- PD_T_CC_DEBOUNCE);
	4188	+ timer_val_msecs);
3787	4189	else if (tcpm_port_is_disconnected(port))
3788	4190	tcpm_set_state(port, SNK_UNATTACHED,
3789		- PD_T_PD_DEBOUNCE);
	4191	+ timer_val_msecs);
3790	4192	break;
3791	4193	case SNK_DEBOUNCED:
3792		- if (tcpm_port_is_disconnected(port))
	4194	+ if (tcpm_port_is_disconnected(port)) {
3793	4195	tcpm_set_state(port, SNK_UNATTACHED,
3794	4196	PD_T_PD_DEBOUNCE);
3795		- else if (port->vbus_present)
	4197	+ } else if (port->vbus_present) {
3796	4198	tcpm_set_state(port,
3797	4199	tcpm_try_src(port) ? SRC_TRY
3798	4200	: SNK_ATTACHED,
3799	4201	0);
	4202	+ port->debouncing = false;
	4203	+ } else {
	4204	+ port->debouncing = false;
	4205	+ }
3800	4206	break;
3801	4207	case SRC_TRY:
3802	4208	port->try_src_count++;
..	..	@@ -3822,8 +4228,11 @@
3822	4228	tcpm_set_state(port, SRC_ATTACHED, PD_T_PD_DEBOUNCE);
3823	4229	break;
3824	4230	case SNK_TRYWAIT:
	4231	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	4232	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_TRYWAIT],
	4233	+ CC_DEBOUNCE, &timer_val_msecs);
3825	4234	tcpm_set_cc(port, TYPEC_CC_RD);
3826		- tcpm_set_state(port, SNK_TRYWAIT_VBUS, PD_T_CC_DEBOUNCE);
	4235	+ tcpm_set_state(port, SNK_TRYWAIT_VBUS, timer_val_msecs);
3827	4236	break;
3828	4237	case SNK_TRYWAIT_VBUS:
3829	4238	/*
..	..	@@ -3853,7 +4262,7 @@
3853	4262	port->cc2 : port->cc1);
3854	4263	typec_set_pwr_opmode(port->typec_port, opmode);
3855	4264	port->pwr_opmode = TYPEC_PWR_MODE_USB;
3856		- port->negotiated_rev = PD_MAX_REV;
	4265	+ port->negotiated_rev = (((port->typec_caps.pd_revision >> 8) & 0xff) - 1);
3857	4266	port->message_id = 0;
3858	4267	port->rx_msgid = -1;
3859	4268	port->explicit_contract = false;
..	..	@@ -3863,13 +4272,22 @@
3863	4272	/* SRC -> SNK POWER/FAST_ROLE_SWAP finished */
3864	4273	tcpm_ams_finish(port);
3865	4274
3866		- tcpm_set_state(port, SNK_DISCOVERY, 0);
	4275	+ timer_val_msecs = 0;
	4276	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_STARTUP],
	4277	+ SINK_DISCOVERY_BC12, &timer_val_msecs);
	4278	+ tcpm_set_state(port, SNK_DISCOVERY, timer_val_msecs);
3867	4279	break;
3868	4280	case SNK_DISCOVERY:
3869	4281	if (port->vbus_present) {
3870		- tcpm_set_current_limit(port,
3871		- tcpm_get_current_limit(port),
3872		- 5000);
	4282	+ current_limit = tcpm_get_current_limit(port);
	4283	+ trace_android_vh_typec_tcpm_adj_current_limit(tcpm_states[SNK_DISCOVERY],
	4284	+ port->current_limit,
	4285	+ port->supply_voltage,
	4286	+ port->pd_capable,
	4287	+ &current_limit, &adjust);
	4288	+ if (port->slow_charger_loop && (current_limit > PD_P_SNK_STDBY_MW / 5))
	4289	+ current_limit = PD_P_SNK_STDBY_MW / 5;
	4290	+ tcpm_set_current_limit(port, current_limit, 5000);
3873	4291	tcpm_set_charge(port, true);
3874	4292	tcpm_set_state(port, SNK_WAIT_CAPABILITIES, 0);
3875	4293	break;
..	..	@@ -3884,8 +4302,10 @@
3884	4302	PD_T_DB_DETECT : PD_T_NO_RESPONSE);
3885	4303	break;
3886	4304	case SNK_DISCOVERY_DEBOUNCE:
3887		- tcpm_set_state(port, SNK_DISCOVERY_DEBOUNCE_DONE,
3888		- PD_T_CC_DEBOUNCE);
	4305	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	4306	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_DISCOVERY_DEBOUNCE],
	4307	+ CC_DEBOUNCE, &timer_val_msecs);
	4308	+ tcpm_set_state(port, SNK_DISCOVERY_DEBOUNCE_DONE, timer_val_msecs);
3889	4309	break;
3890	4310	case SNK_DISCOVERY_DEBOUNCE_DONE:
3891	4311	if (!tcpm_port_is_disconnected(port) &&
..	..	@@ -3898,11 +4318,16 @@
3898	4318	tcpm_set_state(port, unattached_state(port), 0);
3899	4319	break;
3900	4320	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;
	4321	+ if (port->prev_state != SOFT_RESET_SEND) {
	4322	+ ret = port->tcpc->set_pd_rx(port->tcpc, true);
	4323	+ if (ret < 0) {
	4324	+ tcpm_set_state(port, SNK_READY, 0);
	4325	+ break;
	4326	+ }
3905	4327	}
	4328	+ timer_val_msecs = PD_T_SINK_WAIT_CAP;
	4329	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SNK_WAIT_CAPABILITIES],
	4330	+ SINK_WAIT_CAP, &timer_val_msecs);
3906	4331	/*
3907	4332	* If VBUS has never been low, and we time out waiting
3908	4333	* for source cap, try a soft reset first, in case we
..	..	@@ -3912,17 +4337,23 @@
3912	4337	if (port->vbus_never_low) {
3913	4338	port->vbus_never_low = false;
3914	4339	tcpm_set_state(port, SNK_SOFT_RESET,
3915		- PD_T_SINK_WAIT_CAP);
	4340	+ timer_val_msecs);
3916	4341	} else {
3917	4342	tcpm_set_state(port, hard_reset_state(port),
3918		- PD_T_SINK_WAIT_CAP);
	4343	+ timer_val_msecs);
3919	4344	}
3920	4345	break;
3921	4346	case SNK_NEGOTIATE_CAPABILITIES:
3922	4347	port->pd_capable = true;
	4348	+ tcpm_set_partner_usb_comm_capable(port,
	4349	+ !!(port->source_caps[0] & PDO_FIXED_USB_COMM));
3923	4350	port->hard_reset_count = 0;
3924	4351	ret = tcpm_pd_send_request(port);
3925	4352	if (ret < 0) {
	4353	+ /* Restore back to the original state */
	4354	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
	4355	+ port->pps_data.active,
	4356	+ port->supply_voltage);
3926	4357	/* Let the Source send capabilities again. */
3927	4358	tcpm_set_state(port, SNK_WAIT_CAPABILITIES, 0);
3928	4359	} else {
..	..	@@ -3933,6 +4364,10 @@
3933	4364	case SNK_NEGOTIATE_PPS_CAPABILITIES:
3934	4365	ret = tcpm_pd_send_pps_request(port);
3935	4366	if (ret < 0) {
	4367	+ /* Restore back to the original state */
	4368	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
	4369	+ port->pps_data.active,
	4370	+ port->supply_voltage);
3936	4371	port->pps_status = ret;
3937	4372	/*
3938	4373	* If this was called due to updates to sink
..	..	@@ -3949,6 +4384,23 @@
3949	4384	}
3950	4385	break;
3951	4386	case SNK_TRANSITION_SINK:
	4387	+ /* From the USB PD spec:
	4388	+ * "The Sink Shall transition to Sink Standby before a positive or
	4389	+ * negative voltage transition of VBUS. During Sink Standby
	4390	+ * the Sink Shall reduce its power draw to pSnkStdby."
	4391	+ *
	4392	+ * This is not applicable to PPS though as the port can continue
	4393	+ * to draw negotiated power without switching to standby.
	4394	+ */
	4395	+ if (port->supply_voltage != port->req_supply_voltage && !port->pps_data.active &&
	4396	+ port->current_limit * port->supply_voltage / 1000 > PD_P_SNK_STDBY_MW) {
	4397	+ u32 stdby_ma = PD_P_SNK_STDBY_MW * 1000 / port->supply_voltage;
	4398	+
	4399	+ tcpm_log(port, "Setting standby current %u mV @ %u mA",
	4400	+ port->supply_voltage, stdby_ma);
	4401	+ tcpm_set_current_limit(port, stdby_ma, port->supply_voltage);
	4402	+ }
	4403	+ fallthrough;
3952	4404	case SNK_TRANSITION_SINK_VBUS:
3953	4405	tcpm_set_state(port, hard_reset_state(port),
3954	4406	PD_T_PS_TRANSITION);
..	..	@@ -3962,6 +4414,19 @@
3962	4414	port->pwr_opmode = TYPEC_PWR_MODE_PD;
3963	4415	}
3964	4416
	4417	+ current_limit = tcpm_get_current_limit(port);
	4418	+ adjust = false;
	4419	+ trace_android_vh_typec_tcpm_adj_current_limit(tcpm_states[SNK_READY],
	4420	+ port->current_limit,
	4421	+ port->supply_voltage,
	4422	+ port->pd_capable,
	4423	+ &current_limit,
	4424	+ &adjust);
	4425	+ if (adjust)
	4426	+ tcpm_set_current_limit(port, current_limit, 5000);
	4427	+
	4428	+ if (!port->pd_capable && port->slow_charger_loop)
	4429	+ tcpm_set_current_limit(port, tcpm_get_current_limit(port), 5000);
3965	4430	tcpm_swap_complete(port, 0);
3966	4431	tcpm_typec_connect(port);
3967	4432	mod_enable_frs_delayed_work(port, 0);
..	..	@@ -3969,6 +4434,11 @@
3969	4434
3970	4435	if (port->ams != NONE_AMS)
3971	4436	tcpm_ams_finish(port);
	4437	+ if (port->next_ams != NONE_AMS) {
	4438	+ port->ams = port->next_ams;
	4439	+ port->next_ams = NONE_AMS;
	4440	+ }
	4441	+
3972	4442	/*
3973	4443	* If previous AMS is interrupted, switch to the upcoming
3974	4444	* state.
..	..	@@ -3980,11 +4450,19 @@
3980	4450	break;
3981	4451	}
3982	4452
3983		- tcpm_check_send_discover(port);
	4453	+ /*
	4454	+ * 6.4.4.3.1 Discover Identity
	4455	+ * "The Discover Identity Command Shall only be sent to SOP when there is an
	4456	+ * Explicit Contract."
	4457	+ * For now, this driver only supports SOP for DISCOVER_IDENTITY, thus using
	4458	+ * port->explicit_contract.
	4459	+ */
	4460	+ if (port->explicit_contract)
	4461	+ mod_send_discover_delayed_work(port, 0);
	4462	+ else
	4463	+ port->send_discover = false;
	4464	+
3984	4465	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	4466	break;
3989	4467
3990	4468	/* Accessory states */
..	..	@@ -3999,7 +4477,10 @@
3999	4477	tcpm_set_state(port, ACC_UNATTACHED, 0);
4000	4478	break;
4001	4479	case AUDIO_ACC_DEBOUNCE:
4002		- tcpm_set_state(port, ACC_UNATTACHED, PD_T_CC_DEBOUNCE);
	4480	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	4481	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[AUDIO_ACC_DEBOUNCE],
	4482	+ CC_DEBOUNCE, &timer_val_msecs);
	4483	+ tcpm_set_state(port, ACC_UNATTACHED, timer_val_msecs);
4003	4484	break;
4004	4485
4005	4486	/* Hard_Reset states */
..	..	@@ -4029,13 +4510,31 @@
4029	4510	tcpm_set_state(port, SNK_HARD_RESET_SINK_OFF, 0);
4030	4511	break;
4031	4512	case SRC_HARD_RESET_VBUS_OFF:
4032		- tcpm_set_vconn(port, true);
	4513	+ /*
	4514	+ * 7.1.5 Response to Hard Resets
	4515	+ * Hard Reset Signaling indicates a communication failure has occurred and the
	4516	+ * Source Shall stop driving VCONN, Shall remove Rp from the VCONN pin and Shall
	4517	+ * drive VBUS to vSafe0V as shown in Figure 7-9.
	4518	+ */
	4519	+ tcpm_set_vconn(port, false);
4033	4520	tcpm_set_vbus(port, false);
4034	4521	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);
	4522	+ tcpm_data_role_for_source(port));
	4523	+ /*
	4524	+ * If tcpc fails to notify vbus off, TCPM will wait for PD_T_SAFE_0V +
	4525	+ * PD_T_SRC_RECOVER before turning vbus back on.
	4526	+ * From Table 7-12 Sequence Description for a Source Initiated Hard Reset:
	4527	+ * 4. Policy Engine waits tPSHardReset after sending Hard Reset Signaling and then
	4528	+ * tells the Device Policy Manager to instruct the power supply to perform a
	4529	+ * Hard Reset. The transition to vSafe0V Shall occur within tSafe0V (t2).
	4530	+ * 5. After tSrcRecover the Source applies power to VBUS in an attempt to
	4531	+ * re-establish communication with the Sink and resume USB Default Operation.
	4532	+ * The transition to vSafe5V Shall occur within tSrcTurnOn(t4).
	4533	+ */
	4534	+ tcpm_set_state(port, SRC_HARD_RESET_VBUS_ON, PD_T_SAFE_0V + PD_T_SRC_RECOVER);
4037	4535	break;
4038	4536	case SRC_HARD_RESET_VBUS_ON:
	4537	+ tcpm_set_vconn(port, true);
4039	4538	tcpm_set_vbus(port, true);
4040	4539	if (port->ams == HARD_RESET)
4041	4540	tcpm_ams_finish(port);
..	..	@@ -4044,12 +4543,14 @@
4044	4543	tcpm_set_state(port, SRC_UNATTACHED, PD_T_PS_SOURCE_ON);
4045	4544	break;
4046	4545	case SNK_HARD_RESET_SINK_OFF:
	4546	+ /* Do not discharge/disconnect during hard reseet */
	4547	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, 0);
4047	4548	memset(&port->pps_data, 0, sizeof(port->pps_data));
4048	4549	tcpm_set_vconn(port, false);
4049	4550	if (port->pd_capable)
4050	4551	tcpm_set_charge(port, false);
4051	4552	tcpm_set_roles(port, port->self_powered, TYPEC_SINK,
4052		- TYPEC_DEVICE);
	4553	+ tcpm_data_role_for_sink(port));
4053	4554	/*
4054	4555	* VBUS may or may not toggle, depending on the adapter.
4055	4556	* If it doesn't toggle, transition to SNK_HARD_RESET_SINK_ON
..	..	@@ -4090,6 +4591,7 @@
4090	4591	if (port->ams == HARD_RESET)
4091	4592	tcpm_ams_finish(port);
4092	4593	tcpm_set_attached_state(port, true);
	4594	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, VSAFE5V);
4093	4595	tcpm_set_state(port, SNK_STARTUP, 0);
4094	4596	break;
4095	4597
..	..	@@ -4098,6 +4600,7 @@
4098	4600	port->message_id = 0;
4099	4601	port->rx_msgid = -1;
4100	4602	tcpm_pd_send_control(port, PD_CTRL_ACCEPT);
	4603	+ tcpm_ams_finish(port);
4101	4604	if (port->pwr_role == TYPEC_SOURCE) {
4102	4605	port->upcoming_state = SRC_SEND_CAPABILITIES;
4103	4606	tcpm_ams_start(port, POWER_NEGOTIATION);
..	..	@@ -4115,6 +4618,7 @@
4115	4618	case SOFT_RESET_SEND:
4116	4619	port->message_id = 0;
4117	4620	port->rx_msgid = -1;
	4621	+ port->tcpc->set_pd_rx(port->tcpc, true);
4118	4622	if (tcpm_pd_send_control(port, PD_CTRL_SOFT_RESET))
4119	4623	tcpm_set_state_cond(port, hard_reset_state(port), 0);
4120	4624	else
..	..	@@ -4125,52 +4629,33 @@
4125	4629	/* DR_Swap states */
4126	4630	case DR_SWAP_SEND:
4127	4631	tcpm_pd_send_control(port, PD_CTRL_DR_SWAP);
	4632	+ if (port->data_role == TYPEC_DEVICE \|\| port->negotiated_rev > PD_REV20)
	4633	+ port->send_discover = true;
4128	4634	tcpm_set_state_cond(port, DR_SWAP_SEND_TIMEOUT,
4129	4635	PD_T_SENDER_RESPONSE);
4130	4636	break;
4131	4637	case DR_SWAP_ACCEPT:
4132	4638	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;
	4639	+ if (port->data_role == TYPEC_DEVICE \|\| port->negotiated_rev > PD_REV20)
	4640	+ port->send_discover = true;
4136	4641	tcpm_set_state_cond(port, DR_SWAP_CHANGE_DR, 0);
4137	4642	break;
4138	4643	case DR_SWAP_SEND_TIMEOUT:
4139	4644	tcpm_swap_complete(port, -ETIMEDOUT);
	4645	+ port->send_discover = false;
	4646	+ tcpm_ams_finish(port);
4140	4647	tcpm_set_state(port, ready_state(port), 0);
4141	4648	break;
4142	4649	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	4650	if (port->data_role == TYPEC_HOST) {
4162	4651	tcpm_unregister_altmodes(port);
4163	4652	tcpm_set_roles(port, true, port->pwr_role,
4164	4653	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	4654	} else {
4168	4655	tcpm_set_roles(port, true, port->pwr_role,
4169	4656	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	4657	}
	4658	+ tcpm_ams_finish(port);
4174	4659	tcpm_set_state(port, ready_state(port), 0);
4175	4660	break;
4176	4661
..	..	@@ -4185,7 +4670,10 @@
4185	4670	tcpm_set_state(port, ERROR_RECOVERY, 0);
4186	4671	break;
4187	4672	case FR_SWAP_SNK_SRC_TRANSITION_TO_OFF:
4188		- tcpm_set_state(port, ERROR_RECOVERY, PD_T_PS_SOURCE_OFF);
	4673	+ timer_val_msecs = PD_T_PS_SOURCE_OFF;
	4674	+ state_name = tcpm_states[FR_SWAP_SNK_SRC_TRANSITION_TO_OFF];
	4675	+ trace_android_vh_typec_tcpm_get_timer(state_name, SOURCE_OFF, &timer_val_msecs);
	4676	+ tcpm_set_state(port, ERROR_RECOVERY, timer_val_msecs);
4189	4677	break;
4190	4678	case FR_SWAP_SNK_SRC_NEW_SINK_READY:
4191	4679	if (port->vbus_source)
..	..	@@ -4218,6 +4706,7 @@
4218	4706	tcpm_set_state(port, ready_state(port), 0);
4219	4707	break;
4220	4708	case PR_SWAP_START:
	4709	+ tcpm_apply_rc(port);
4221	4710	if (port->pwr_role == TYPEC_SOURCE)
4222	4711	tcpm_set_state(port, PR_SWAP_SRC_SNK_TRANSITION_OFF,
4223	4712	PD_T_SRC_TRANSITION);
..	..	@@ -4225,6 +4714,10 @@
4225	4714	tcpm_set_state(port, PR_SWAP_SNK_SRC_SINK_OFF, 0);
4226	4715	break;
4227	4716	case PR_SWAP_SRC_SNK_TRANSITION_OFF:
	4717	+ /*
	4718	+ * Prevent vbus discharge circuit from turning on during PR_SWAP
	4719	+ * as this is not a disconnect.
	4720	+ */
4228	4721	tcpm_set_vbus(port, false);
4229	4722	port->explicit_contract = false;
4230	4723	/* allow time for Vbus discharge, must be < tSrcSwapStdby */
..	..	@@ -4232,10 +4725,13 @@
4232	4725	PD_T_SRCSWAPSTDBY);
4233	4726	break;
4234	4727	case PR_SWAP_SRC_SNK_SOURCE_OFF:
	4728	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	4729	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[PR_SWAP_SRC_SNK_SOURCE_OFF],
	4730	+ CC_DEBOUNCE, &timer_val_msecs);
4235	4731	tcpm_set_cc(port, TYPEC_CC_RD);
4236	4732	/* allow CC debounce */
4237	4733	tcpm_set_state(port, PR_SWAP_SRC_SNK_SOURCE_OFF_CC_DEBOUNCED,
4238		- PD_T_CC_DEBOUNCE);
	4734	+ timer_val_msecs);
4239	4735	break;
4240	4736	case PR_SWAP_SRC_SNK_SOURCE_OFF_CC_DEBOUNCED:
4241	4737	/*
..	..	@@ -4250,17 +4746,29 @@
4250	4746	tcpm_set_state(port, ERROR_RECOVERY, 0);
4251	4747	break;
4252	4748	}
4253		- tcpm_set_state_cond(port, SNK_UNATTACHED, PD_T_PS_SOURCE_ON);
	4749	+ tcpm_set_state(port, ERROR_RECOVERY, PD_T_PS_SOURCE_ON_PRS);
4254	4750	break;
4255	4751	case PR_SWAP_SRC_SNK_SINK_ON:
	4752	+ tcpm_enable_auto_vbus_discharge(port, true);
	4753	+ /* Set the vbus disconnect threshold for implicit contract */
	4754	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB, false, VSAFE5V);
4256	4755	tcpm_set_state(port, SNK_STARTUP, 0);
4257	4756	break;
4258	4757	case PR_SWAP_SNK_SRC_SINK_OFF:
	4758	+ timer_val_msecs = PD_T_PS_SOURCE_OFF;
	4759	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[PR_SWAP_SNK_SRC_SINK_OFF],
	4760	+ SOURCE_OFF, &timer_val_msecs);
	4761	+ /*
	4762	+ * Prevent vbus discharge circuit from turning on during PR_SWAP
	4763	+ * as this is not a disconnect.
	4764	+ */
	4765	+ tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_USB,
	4766	+ port->pps_data.active, 0);
4259	4767	tcpm_set_charge(port, false);
4260		- tcpm_set_state(port, hard_reset_state(port),
4261		- PD_T_PS_SOURCE_OFF);
	4768	+ tcpm_set_state(port, hard_reset_state(port), timer_val_msecs);
4262	4769	break;
4263	4770	case PR_SWAP_SNK_SRC_SOURCE_ON:
	4771	+ tcpm_enable_auto_vbus_discharge(port, true);
4264	4772	tcpm_set_cc(port, tcpm_rp_cc(port));
4265	4773	tcpm_set_vbus(port, true);
4266	4774	/*
..	..	@@ -4286,6 +4794,7 @@
4286	4794
4287	4795	case VCONN_SWAP_ACCEPT:
4288	4796	tcpm_pd_send_control(port, PD_CTRL_ACCEPT);
	4797	+ tcpm_ams_finish(port);
4289	4798	tcpm_set_state(port, VCONN_SWAP_START, 0);
4290	4799	break;
4291	4800	case VCONN_SWAP_SEND:
..	..	@@ -4295,8 +4804,6 @@
4295	4804	break;
4296	4805	case VCONN_SWAP_SEND_TIMEOUT:
4297	4806	tcpm_swap_complete(port, -ETIMEDOUT);
4298		- if (port->data_role == TYPEC_HOST && port->send_discover)
4299		- port->vdm_sm_running = true;
4300	4807	tcpm_set_state(port, ready_state(port), 0);
4301	4808	break;
4302	4809	case VCONN_SWAP_START:
..	..	@@ -4312,14 +4819,10 @@
4312	4819	case VCONN_SWAP_TURN_ON_VCONN:
4313	4820	tcpm_set_vconn(port, true);
4314	4821	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	4822	tcpm_set_state(port, ready_state(port), 0);
4318	4823	break;
4319	4824	case VCONN_SWAP_TURN_OFF_VCONN:
4320	4825	tcpm_set_vconn(port, false);
4321		- if (port->data_role == TYPEC_HOST && port->send_discover)
4322		- port->vdm_sm_running = true;
4323	4826	tcpm_set_state(port, ready_state(port), 0);
4324	4827	break;
4325	4828
..	..	@@ -4327,8 +4830,6 @@
4327	4830	case PR_SWAP_CANCEL:
4328	4831	case VCONN_SWAP_CANCEL:
4329	4832	tcpm_swap_complete(port, port->swap_status);
4330		- if (port->data_role == TYPEC_HOST && port->send_discover)
4331		- port->vdm_sm_running = true;
4332	4833	if (port->pwr_role == TYPEC_SOURCE)
4333	4834	tcpm_set_state(port, SRC_READY, 0);
4334	4835	else
..	..	@@ -4345,12 +4846,18 @@
4345	4846	switch (BDO_MODE_MASK(port->bist_request)) {
4346	4847	case BDO_MODE_CARRIER2:
4347	4848	tcpm_pd_transmit(port, TCPC_TX_BIST_MODE_2, NULL);
	4849	+ tcpm_set_state(port, unattached_state(port),
	4850	+ PD_T_BIST_CONT_MODE);
	4851	+ break;
	4852	+ case BDO_MODE_TESTDATA:
	4853	+ if (port->tcpc->set_bist_data) {
	4854	+ tcpm_log(port, "Enable BIST MODE TESTDATA");
	4855	+ port->tcpc->set_bist_data(port->tcpc, true);
	4856	+ }
4348	4857	break;
4349	4858	default:
4350	4859	break;
4351	4860	}
4352		- /* Always switch to unattached state */
4353		- tcpm_set_state(port, unattached_state(port), 0);
4354	4861	break;
4355	4862	case GET_STATUS_SEND:
4356	4863	tcpm_pd_send_control(port, PD_CTRL_GET_STATUS);
..	..	@@ -4383,14 +4890,17 @@
4383	4890	break;
4384	4891	case PORT_RESET:
4385	4892	tcpm_reset_port(port);
4386		- tcpm_set_cc(port, TYPEC_CC_OPEN);
	4893	+ tcpm_set_cc(port, TYPEC_CC_RD);
4387	4894	tcpm_set_state(port, PORT_RESET_WAIT_OFF,
4388	4895	PD_T_ERROR_RECOVERY);
4389	4896	break;
4390	4897	case PORT_RESET_WAIT_OFF:
	4898	+ timer_val_msecs = PD_T_PS_SOURCE_OFF;
	4899	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[PORT_RESET_WAIT_OFF],
	4900	+ SOURCE_OFF, &timer_val_msecs);
4391	4901	tcpm_set_state(port,
4392	4902	tcpm_default_state(port),
4393		- port->vbus_present ? PD_T_PS_SOURCE_OFF : 0);
	4903	+ port->vbus_present ? timer_val_msecs : 0);
4394	4904	break;
4395	4905
4396	4906	/* AMS intermediate state */
..	..	@@ -4404,6 +4914,12 @@
4404	4914	upcoming_state = port->upcoming_state;
4405	4915	port->upcoming_state = INVALID_STATE;
4406	4916	tcpm_set_state(port, upcoming_state, 0);
	4917	+ break;
	4918	+
	4919	+ /* Chunk state */
	4920	+ case CHUNK_NOT_SUPP:
	4921	+ tcpm_pd_send_control(port, PD_CTRL_NOT_SUPP);
	4922	+ tcpm_set_state(port, port->pwr_role == TYPEC_SOURCE ? SRC_READY : SNK_READY, 0);
4407	4923	break;
4408	4924	default:
4409	4925	WARN(1, "Unexpected port state %d\n", port->state);
..	..	@@ -4488,11 +5004,16 @@
4488	5004	tcpm_set_state(port, SRC_ATTACH_WAIT, 0);
4489	5005	break;
4490	5006	case SRC_ATTACHED:
	5007	+ case SRC_STARTUP:
4491	5008	case SRC_SEND_CAPABILITIES:
4492	5009	case SRC_READY:
4493	5010	if (tcpm_port_is_disconnected(port) \|\|
4494		- !tcpm_port_is_source(port))
4495		- tcpm_set_state(port, SRC_UNATTACHED, 0);
	5011	+ !tcpm_port_is_source(port)) {
	5012	+ if (port->port_type == TYPEC_PORT_SRC)
	5013	+ tcpm_set_state(port, SRC_UNATTACHED, tcpm_wait_for_discharge(port));
	5014	+ else
	5015	+ tcpm_set_state(port, SNK_UNATTACHED, tcpm_wait_for_discharge(port));
	5016	+ }
4496	5017	break;
4497	5018	case SNK_UNATTACHED:
4498	5019	if (tcpm_port_is_sink(port))
..	..	@@ -4522,7 +5043,23 @@
4522	5043	tcpm_set_state(port, SNK_DEBOUNCED, 0);
4523	5044	break;
4524	5045	case SNK_READY:
4525		- if (tcpm_port_is_disconnected(port))
	5046	+ /*
	5047	+ * EXIT condition is based primarily on vbus disconnect and CC is secondary.
	5048	+ * "A port that has entered into USB PD communications with the Source and
	5049	+ * has seen the CC voltage exceed vRd-USB may monitor the CC pin to detect
	5050	+ * cable disconnect in addition to monitoring VBUS.
	5051	+ *
	5052	+ * A port that is monitoring the CC voltage for disconnect (but is not in
	5053	+ * the process of a USB PD PR_Swap or USB PD FR_Swap) shall transition to
	5054	+ * Unattached.SNK within tSinkDisconnect after the CC voltage remains below
	5055	+ * vRd-USB for tPDDebounce."
	5056	+ *
	5057	+ * When set_auto_vbus_discharge_threshold is enabled, CC pins go
	5058	+ * away before vbus decays to disconnect threshold. Allow
	5059	+ * disconnect to be driven by vbus disconnect when auto vbus
	5060	+ * discharge is enabled.
	5061	+ */
	5062	+ if (!port->auto_vbus_discharge_enabled && tcpm_port_is_disconnected(port))
4526	5063	tcpm_set_state(port, unattached_state(port), 0);
4527	5064	else if (!port->pd_capable &&
4528	5065	(cc1 != old_cc1 \|\| cc2 != old_cc2))
..	..	@@ -4587,6 +5124,12 @@
4587	5124	if (!tcpm_port_is_sink(port))
4588	5125	tcpm_set_state(port, SNK_TRYWAIT_DEBOUNCE, 0);
4589	5126	break;
	5127	+ case SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS:
	5128	+ if (!tcpm_port_is_sink(port))
	5129	+ tcpm_set_state(port, SRC_TRYWAIT, PD_T_TRY_CC_DEBOUNCE);
	5130	+ else
	5131	+ tcpm_set_state(port, SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS, 0);
	5132	+ break;
4590	5133	case SNK_TRYWAIT:
4591	5134	/* Do nothing, waiting for tCCDebounce */
4592	5135	break;
..	..	@@ -4615,9 +5158,13 @@
4615	5158	* Ignore CC changes here.
4616	5159	*/
4617	5160	break;
4618		-
4619	5161	default:
4620		- if (tcpm_port_is_disconnected(port))
	5162	+ /*
	5163	+ * While acting as sink and auto vbus discharge is enabled, Allow disconnect
	5164	+ * to be driven by vbus disconnect.
	5165	+ */
	5166	+ if (tcpm_port_is_disconnected(port) && !(port->pwr_role == TYPEC_SINK &&
	5167	+ port->auto_vbus_discharge_enabled))
4621	5168	tcpm_set_state(port, unattached_state(port), 0);
4622	5169	break;
4623	5170	}
..	..	@@ -4627,12 +5174,15 @@
4627	5174	{
4628	5175	tcpm_log_force(port, "VBUS on");
4629	5176	port->vbus_present = true;
	5177	+ /*
	5178	+ * When vbus_present is true i.e. Voltage at VBUS is greater than VSAFE5V implicitly
	5179	+ * states that vbus is not at VSAFE0V, hence clear the vbus_vsafe0v flag here.
	5180	+ */
	5181	+ port->vbus_vsafe0v = false;
	5182	+
4630	5183	switch (port->state) {
4631	5184	case SNK_TRANSITION_SINK_VBUS:
4632	5185	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	5186	tcpm_set_state(port, SNK_READY, 0);
4637	5187	break;
4638	5188	case SNK_DISCOVERY:
..	..	@@ -4676,11 +5226,24 @@
4676	5226	case SNK_TRYWAIT_DEBOUNCE:
4677	5227	/* Do nothing, waiting for Rp */
4678	5228	break;
	5229	+ case SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS:
	5230	+ if (port->vbus_present && tcpm_port_is_sink(port))
	5231	+ tcpm_set_state(port, SNK_ATTACHED, 0);
	5232	+ break;
4679	5233	case SRC_TRY_WAIT:
4680	5234	case SRC_TRY_DEBOUNCE:
4681	5235	/* Do nothing, waiting for sink detection */
4682	5236	break;
	5237	+ case FR_SWAP_SEND:
	5238	+ case FR_SWAP_SEND_TIMEOUT:
	5239	+ case FR_SWAP_SNK_SRC_TRANSITION_TO_OFF:
	5240	+ case FR_SWAP_SNK_SRC_SOURCE_VBUS_APPLIED:
	5241	+ if (port->tcpc->frs_sourcing_vbus)
	5242	+ port->tcpc->frs_sourcing_vbus(port->tcpc);
	5243	+ break;
4683	5244	case FR_SWAP_SNK_SRC_NEW_SINK_READY:
	5245	+ if (port->tcpc->frs_sourcing_vbus)
	5246	+ port->tcpc->frs_sourcing_vbus(port->tcpc);
4684	5247	tcpm_set_state(port, FR_SWAP_SNK_SRC_SOURCE_VBUS_APPLIED, 0);
4685	5248	break;
4686	5249
..	..	@@ -4706,12 +5269,8 @@
4706	5269	case SNK_HARD_RESET_SINK_OFF:
4707	5270	tcpm_set_state(port, SNK_HARD_RESET_WAIT_VBUS, 0);
4708	5271	break;
4709		- case SRC_HARD_RESET_VBUS_OFF:
4710		- tcpm_set_state(port, SRC_HARD_RESET_VBUS_ON, 0);
4711		- break;
4712	5272	case HARD_RESET_SEND:
4713	5273	break;
4714		-
4715	5274	case SNK_TRY:
4716	5275	/* Do nothing, waiting for timeout */
4717	5276	break;
..	..	@@ -4729,6 +5288,7 @@
4729	5288	break;
4730	5289	case SNK_ATTACH_WAIT:
4731	5290	case SNK_DEBOUNCED:
	5291	+ port->debouncing = false;
4732	5292	/* Do nothing, as TCPM is still waiting for vbus to reaach VSAFE5V to connect */
4733	5293	break;
4734	5294
..	..	@@ -4743,6 +5303,14 @@
4743	5303	/* Do nothing, expected */
4744	5304	break;
4745	5305
	5306	+ case PR_SWAP_SNK_SRC_SOURCE_ON:
	5307	+ /*
	5308	+ * Do nothing when vbus off notification is received.
	5309	+ * TCPM can wait for PD_T_NEWSRC in PR_SWAP_SNK_SRC_SOURCE_ON
	5310	+ * for the vbus source to ramp up.
	5311	+ */
	5312	+ break;
	5313	+
4746	5314	case PORT_RESET_WAIT_OFF:
4747	5315	tcpm_set_state(port, tcpm_default_state(port), 0);
4748	5316	break;
..	..	@@ -4750,6 +5318,25 @@
4750	5318	case SRC_TRY_WAIT:
4751	5319	case SRC_TRY_DEBOUNCE:
4752	5320	/* Do nothing, waiting for sink detection */
	5321	+ break;
	5322	+
	5323	+ case SRC_STARTUP:
	5324	+ case SRC_SEND_CAPABILITIES:
	5325	+ case SRC_SEND_CAPABILITIES_TIMEOUT:
	5326	+ case SRC_NEGOTIATE_CAPABILITIES:
	5327	+ case SRC_TRANSITION_SUPPLY:
	5328	+ case SRC_READY:
	5329	+ case SRC_WAIT_NEW_CAPABILITIES:
	5330	+ /*
	5331	+ * Force to unattached state to re-initiate connection.
	5332	+ * DRP port should move to Unattached.SNK instead of Unattached.SRC if
	5333	+ * sink removed. Although sink removal here is due to source's vbus collapse,
	5334	+ * treat it the same way for consistency.
	5335	+ */
	5336	+ if (port->port_type == TYPEC_PORT_SRC)
	5337	+ tcpm_set_state(port, SRC_UNATTACHED, tcpm_wait_for_discharge(port));
	5338	+ else
	5339	+ tcpm_set_state(port, SNK_UNATTACHED, tcpm_wait_for_discharge(port));
4753	5340	break;
4754	5341
4755	5342	case PORT_RESET:
..	..	@@ -4768,8 +5355,58 @@
4768	5355	break;
4769	5356
4770	5357	default:
4771		- if (port->pwr_role == TYPEC_SINK &&
4772		- port->attached)
	5358	+ if (port->pwr_role == TYPEC_SINK && port->attached)
	5359	+ tcpm_set_state(port, SNK_UNATTACHED, tcpm_wait_for_discharge(port));
	5360	+ break;
	5361	+ }
	5362	+}
	5363	+
	5364	+static void _tcpm_pd_vbus_vsafe0v(struct tcpm_port *port)
	5365	+{
	5366	+ unsigned int timer_val_msecs;
	5367	+
	5368	+ tcpm_log_force(port, "VBUS VSAFE0V");
	5369	+ port->vbus_vsafe0v = true;
	5370	+ switch (port->state) {
	5371	+ case SRC_HARD_RESET_VBUS_OFF:
	5372	+ /*
	5373	+ * After establishing the vSafe0V voltage condition on VBUS, the Source Shall wait
	5374	+ * tSrcRecover before re-applying VCONN and restoring VBUS to vSafe5V.
	5375	+ */
	5376	+ tcpm_set_state(port, SRC_HARD_RESET_VBUS_ON, PD_T_SRC_RECOVER);
	5377	+ break;
	5378	+ case SRC_ATTACH_WAIT:
	5379	+ timer_val_msecs = PD_T_CC_DEBOUNCE;
	5380	+ trace_android_vh_typec_tcpm_get_timer(tcpm_states[SRC_ATTACH_WAIT],
	5381	+ CC_DEBOUNCE, &timer_val_msecs);
	5382	+ if (tcpm_port_is_source(port))
	5383	+ tcpm_set_state(port, tcpm_try_snk(port) ? SNK_TRY : SRC_ATTACHED,
	5384	+ timer_val_msecs);
	5385	+ break;
	5386	+ case SRC_STARTUP:
	5387	+ case SRC_SEND_CAPABILITIES:
	5388	+ case SRC_SEND_CAPABILITIES_TIMEOUT:
	5389	+ case SRC_NEGOTIATE_CAPABILITIES:
	5390	+ case SRC_TRANSITION_SUPPLY:
	5391	+ case SRC_READY:
	5392	+ case SRC_WAIT_NEW_CAPABILITIES:
	5393	+ if (port->auto_vbus_discharge_enabled) {
	5394	+ if (port->port_type == TYPEC_PORT_SRC)
	5395	+ tcpm_set_state(port, SRC_UNATTACHED, 0);
	5396	+ else
	5397	+ tcpm_set_state(port, SNK_UNATTACHED, 0);
	5398	+ }
	5399	+ break;
	5400	+ case PR_SWAP_SNK_SRC_SINK_OFF:
	5401	+ case PR_SWAP_SNK_SRC_SOURCE_ON:
	5402	+ /* Do nothing, vsafe0v is expected during transition */
	5403	+ break;
	5404	+ case SNK_ATTACH_WAIT:
	5405	+ case SNK_DEBOUNCED:
	5406	+ /Do nothing, still waiting for VSAFE5V for connect /
	5407	+ break;
	5408	+ default:
	5409	+ if (port->pwr_role == TYPEC_SINK && port->auto_vbus_discharge_enabled)
4773	5410	tcpm_set_state(port, SNK_UNATTACHED, 0);
4774	5411	break;
4775	5412	}
..	..	@@ -4778,9 +5415,13 @@
4778	5415	static void _tcpm_pd_hard_reset(struct tcpm_port *port)
4779	5416	{
4780	5417	tcpm_log_force(port, "Received hard reset");
	5418	+ if (port->bist_request == BDO_MODE_TESTDATA && port->tcpc->set_bist_data)
	5419	+ port->tcpc->set_bist_data(port->tcpc, false);
4781	5420
4782	5421	if (port->ams != NONE_AMS)
4783	5422	port->ams = NONE_AMS;
	5423	+ if (port->hard_reset_count < PD_N_HARD_RESET_COUNT)
	5424	+ port->ams = HARD_RESET;
4784	5425	/*
4785	5426	* If we keep receiving hard reset requests, executing the hard reset
4786	5427	* must have failed. Revert to error recovery if that happens.
..	..	@@ -4797,7 +5438,9 @@
4797	5438	event_work);
4798	5439	u32 events;
4799	5440
	5441	+#ifdef CONFIG_NO_GKI
4800	5442	mutex_lock(&port->pd_handler_lock);
	5443	+#endif
4801	5444	mutex_lock(&port->lock);
4802	5445
4803	5446	spin_lock(&port->pd_event_lock);
..	..	@@ -4811,10 +5454,19 @@
4811	5454	bool vbus;
4812	5455
4813	5456	vbus = port->tcpc->get_vbus(port->tcpc);
4814		- if (vbus)
	5457	+ if (vbus) {
4815	5458	_tcpm_pd_vbus_on(port);
4816		- else
	5459	+ } else {
4817	5460	_tcpm_pd_vbus_off(port);
	5461	+ /*
	5462	+ * When TCPC does not support detecting vsafe0v voltage level,
	5463	+ * treat vbus absent as vsafe0v. Else invoke is_vbus_vsafe0v
	5464	+ * to see if vbus has discharge to VSAFE0V.
	5465	+ */
	5466	+ if (!port->tcpc->is_vbus_vsafe0v \|\|
	5467	+ port->tcpc->is_vbus_vsafe0v(port->tcpc))
	5468	+ _tcpm_pd_vbus_vsafe0v(port);
	5469	+ }
4818	5470	}
4819	5471	if (events & TCPM_CC_EVENT) {
4820	5472	enum typec_cc_status cc1, cc2;
..	..	@@ -4851,7 +5503,9 @@
4851	5503	}
4852	5504	spin_unlock(&port->pd_event_lock);
4853	5505	mutex_unlock(&port->lock);
	5506	+#ifdef CONFIG_NO_GKI
4854	5507	mutex_unlock(&port->pd_handler_lock);
	5508	+#endif
4855	5509	}
4856	5510
4857	5511	void tcpm_cc_change(struct tcpm_port *port)
..	..	@@ -4931,27 +5585,41 @@
4931	5585	mutex_unlock(&port->lock);
4932	5586	}
4933	5587
4934		-static void tcpm_data_role_swap_work(struct kthread_work *work)
	5588	+static void tcpm_send_discover_work(struct kthread_work *work)
4935	5589	{
4936		- struct tcpm_port *port =
4937		- container_of(work, struct tcpm_port, data_role_swap);
	5590	+ struct tcpm_port *port = container_of(work, struct tcpm_port, send_discover_work);
4938	5591
4939		- if (tcpm_port_is_disconnected(port))
4940		- return;
	5592	+ mutex_lock(&port->lock);
	5593	+ /* No need to send DISCOVER_IDENTITY anymore */
	5594	+ if (!port->send_discover)
	5595	+ goto unlock;
4941	5596
4942		- tcpm_send_data_role_notify(port, true, port->data_role);
	5597	+ if (port->data_role == TYPEC_DEVICE && port->negotiated_rev < PD_REV30) {
	5598	+ port->send_discover = false;
	5599	+ goto unlock;
	5600	+ }
	5601	+
	5602	+ /* Retry if the port is not idle */
	5603	+ if ((port->state != SRC_READY && port->state != SNK_READY) \|\| port->vdm_sm_running) {
	5604	+ mod_send_discover_delayed_work(port, SEND_DISCOVER_RETRY_MS);
	5605	+ goto unlock;
	5606	+ }
	5607	+
	5608	+ tcpm_send_vdm(port, USB_SID_PD, CMD_DISCOVER_IDENT, NULL, 0);
	5609	+
	5610	+unlock:
	5611	+ mutex_unlock(&port->lock);
4943	5612	}
4944	5613
4945		-static int tcpm_dr_set(const struct typec_capability *cap,
4946		- enum typec_data_role data)
	5614	+static int tcpm_dr_set(struct typec_port *p, enum typec_data_role data)
4947	5615	{
4948		- struct tcpm_port *port = typec_cap_to_tcpm(cap);
	5616	+ struct tcpm_port *port = typec_get_drvdata(p);
4949	5617	int ret;
4950	5618
4951	5619	mutex_lock(&port->swap_lock);
4952	5620	mutex_lock(&port->lock);
4953	5621
4954		- if (port->port_type != TYPEC_PORT_DRP) {
	5622	+ if (port->typec_caps.data != TYPEC_PORT_DRD) {
4955	5623	ret = -EINVAL;
4956	5624	goto port_unlock;
4957	5625	}
..	..	@@ -5015,10 +5683,9 @@
5015	5683	return ret;
5016	5684	}
5017	5685
5018		-static int tcpm_pr_set(const struct typec_capability *cap,
5019		- enum typec_role role)
	5686	+static int tcpm_pr_set(struct typec_port *p, enum typec_role role)
5020	5687	{
5021		- struct tcpm_port *port = typec_cap_to_tcpm(cap);
	5688	+ struct tcpm_port *port = typec_get_drvdata(p);
5022	5689	int ret;
5023	5690
5024	5691	mutex_lock(&port->swap_lock);
..	..	@@ -5065,10 +5732,9 @@
5065	5732	return ret;
5066	5733	}
5067	5734
5068		-static int tcpm_vconn_set(const struct typec_capability *cap,
5069		- enum typec_role role)
	5735	+static int tcpm_vconn_set(struct typec_port *p, enum typec_role role)
5070	5736	{
5071		- struct tcpm_port *port = typec_cap_to_tcpm(cap);
	5737	+ struct tcpm_port *port = typec_get_drvdata(p);
5072	5738	int ret;
5073	5739
5074	5740	mutex_lock(&port->swap_lock);
..	..	@@ -5111,9 +5777,9 @@
5111	5777	return ret;
5112	5778	}
5113	5779
5114		-static int tcpm_try_role(const struct typec_capability *cap, int role)
	5780	+static int tcpm_try_role(struct typec_port *p, int role)
5115	5781	{
5116		- struct tcpm_port *port = typec_cap_to_tcpm(cap);
	5782	+ struct tcpm_port *port = typec_get_drvdata(p);
5117	5783	struct tcpc_dev *tcpc = port->tcpc;
5118	5784	int ret = 0;
5119	5785
..	..	@@ -5129,7 +5795,7 @@
5129	5795	return ret;
5130	5796	}
5131	5797
5132		-static int tcpm_pps_set_op_curr(struct tcpm_port *port, u16 op_curr)
	5798	+static int tcpm_pps_set_op_curr(struct tcpm_port *port, u16 req_op_curr)
5133	5799	{
5134	5800	unsigned int target_mw;
5135	5801	int ret;
..	..	@@ -5147,12 +5813,12 @@
5147	5813	goto port_unlock;
5148	5814	}
5149	5815
5150		- if (op_curr > port->pps_data.max_curr) {
	5816	+ if (req_op_curr > port->pps_data.max_curr) {
5151	5817	ret = -EINVAL;
5152	5818	goto port_unlock;
5153	5819	}
5154	5820
5155		- target_mw = (op_curr * port->pps_data.out_volt) / 1000;
	5821	+ target_mw = (req_op_curr * port->supply_voltage) / 1000;
5156	5822	if (target_mw < port->operating_snk_mw) {
5157	5823	ret = -EINVAL;
5158	5824	goto port_unlock;
..	..	@@ -5166,10 +5832,10 @@
5166	5832	}
5167	5833
5168	5834	/* Round down operating current to align with PPS valid steps */
5169		- op_curr = op_curr - (op_curr % RDO_PROG_CURR_MA_STEP);
	5835	+ req_op_curr = req_op_curr - (req_op_curr % RDO_PROG_CURR_MA_STEP);
5170	5836
5171	5837	reinit_completion(&port->pps_complete);
5172		- port->pps_data.op_curr = op_curr;
	5838	+ port->pps_data.req_op_curr = req_op_curr;
5173	5839	port->pps_status = 0;
5174	5840	port->pps_pending = true;
5175	5841	mutex_unlock(&port->lock);
..	..	@@ -5190,7 +5856,7 @@
5190	5856	return ret;
5191	5857	}
5192	5858
5193		-static int tcpm_pps_set_out_volt(struct tcpm_port *port, u16 out_volt)
	5859	+static int tcpm_pps_set_out_volt(struct tcpm_port *port, u16 req_out_volt)
5194	5860	{
5195	5861	unsigned int target_mw;
5196	5862	int ret;
..	..	@@ -5208,13 +5874,13 @@
5208	5874	goto port_unlock;
5209	5875	}
5210	5876
5211		- if (out_volt < port->pps_data.min_volt \|\|
5212		- out_volt > port->pps_data.max_volt) {
	5877	+ if (req_out_volt < port->pps_data.min_volt \|\|
	5878	+ req_out_volt > port->pps_data.max_volt) {
5213	5879	ret = -EINVAL;
5214	5880	goto port_unlock;
5215	5881	}
5216	5882
5217		- target_mw = (port->pps_data.op_curr * out_volt) / 1000;
	5883	+ target_mw = (port->current_limit * req_out_volt) / 1000;
5218	5884	if (target_mw < port->operating_snk_mw) {
5219	5885	ret = -EINVAL;
5220	5886	goto port_unlock;
..	..	@@ -5228,10 +5894,10 @@
5228	5894	}
5229	5895
5230	5896	/* Round down output voltage to align with PPS valid steps */
5231		- out_volt = out_volt - (out_volt % RDO_PROG_VOLT_MV_STEP);
	5897	+ req_out_volt = req_out_volt - (req_out_volt % RDO_PROG_VOLT_MV_STEP);
5232	5898
5233	5899	reinit_completion(&port->pps_complete);
5234		- port->pps_data.out_volt = out_volt;
	5900	+ port->pps_data.req_out_volt = req_out_volt;
5235	5901	port->pps_status = 0;
5236	5902	port->pps_pending = true;
5237	5903	mutex_unlock(&port->lock);
..	..	@@ -5289,8 +5955,8 @@
5289	5955
5290	5956	/* Trigger PPS request or move back to standard PDO contract */
5291	5957	if (activate) {
5292		- port->pps_data.out_volt = port->supply_voltage;
5293		- port->pps_data.op_curr = port->current_limit;
	5958	+ port->pps_data.req_out_volt = port->supply_voltage;
	5959	+ port->pps_data.req_op_curr = port->current_limit;
5294	5960	}
5295	5961	mutex_unlock(&port->lock);
5296	5962
..	..	@@ -5327,6 +5993,24 @@
5327	5993	if (port->vbus_present)
5328	5994	port->vbus_never_low = true;
5329	5995
	5996	+ /*
	5997	+ * 1. When vbus_present is true, voltage on VBUS is already at VSAFE5V.
	5998	+ * So implicitly vbus_vsafe0v = false.
	5999	+ *
	6000	+ * 2. When vbus_present is false and TCPC does NOT support querying
	6001	+ * vsafe0v status, then, it's best to assume vbus is at VSAFE0V i.e.
	6002	+ * vbus_vsafe0v is true.
	6003	+ *
	6004	+ * 3. When vbus_present is false and TCPC does support querying vsafe0v,
	6005	+ * then, query tcpc for vsafe0v status.
	6006	+ */
	6007	+ if (port->vbus_present)
	6008	+ port->vbus_vsafe0v = false;
	6009	+ else if (!port->tcpc->is_vbus_vsafe0v)
	6010	+ port->vbus_vsafe0v = true;
	6011	+ else
	6012	+ port->vbus_vsafe0v = port->tcpc->is_vbus_vsafe0v(port->tcpc);
	6013	+
5330	6014	tcpm_set_state(port, tcpm_default_state(port), 0);
5331	6015
5332	6016	if (port->tcpc->get_cc(port->tcpc, &cc1, &cc2) == 0)
..	..	@@ -5339,10 +6023,9 @@
5339	6023	tcpm_set_state(port, PORT_RESET, 0);
5340	6024	}
5341	6025
5342		-static int tcpm_port_type_set(const struct typec_capability *cap,
5343		- enum typec_port_type type)
	6026	+static int tcpm_port_type_set(struct typec_port *p, enum typec_port_type type)
5344	6027	{
5345		- struct tcpm_port *port = typec_cap_to_tcpm(cap);
	6028	+ struct tcpm_port *port = typec_get_drvdata(p);
5346	6029
5347	6030	mutex_lock(&port->lock);
5348	6031	if (type == port->port_type)
..	..	@@ -5367,6 +6050,14 @@
5367	6050	return 0;
5368	6051	}
5369	6052
	6053	+static const struct typec_operations tcpm_ops = {
	6054	+ .try_role = tcpm_try_role,
	6055	+ .dr_set = tcpm_dr_set,
	6056	+ .pr_set = tcpm_pr_set,
	6057	+ .vconn_set = tcpm_vconn_set,
	6058	+ .port_type_set = tcpm_port_type_set
	6059	+};
	6060	+
5370	6061	void tcpm_tcpc_reset(struct tcpm_port *port)
5371	6062	{
5372	6063	mutex_lock(&port->lock);
..	..	@@ -5376,63 +6067,48 @@
5376	6067	}
5377	6068	EXPORT_SYMBOL_GPL(tcpm_tcpc_reset);
5378	6069
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	6070	static int tcpm_fw_get_caps(struct tcpm_port *port,
5394	6071	struct fwnode_handle *fwnode)
5395	6072	{
5396	6073	const char *cap_str;
5397	6074	int ret;
5398		- u32 mw, frs_current;
	6075	+ u32 mw, frs_current, pd_revision;
5399	6076
5400	6077	if (!fwnode)
5401	6078	return -EINVAL;
5402	6079
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		- }
	6080	+ ret = fwnode_property_read_u32(fwnode, "pd-revision",
	6081	+ &pd_revision);
	6082	+ if (ret < 0)
	6083	+ port->typec_caps.pd_revision = 0x0300;
	6084	+ else
	6085	+ port->typec_caps.pd_revision = pd_revision & 0xffff;
5412	6086
5413	6087	/* USB data support is optional */
5414	6088	ret = fwnode_property_read_string(fwnode, "data-role", &cap_str);
5415	6089	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;
	6090	+ ret = typec_find_port_data_role(cap_str);
	6091	+ if (ret < 0)
	6092	+ return ret;
	6093	+ port->typec_caps.data = ret;
5419	6094	}
5420	6095
5421	6096	ret = fwnode_property_read_string(fwnode, "power-role", &cap_str);
5422	6097	if (ret < 0)
5423	6098	return ret;
5424	6099
5425		- port->typec_caps.type = typec_find_port_power_role(cap_str);
5426		- if (port->typec_caps.type < 0)
5427		- return -EINVAL;
	6100	+ ret = typec_find_port_power_role(cap_str);
	6101	+ if (ret < 0)
	6102	+ return ret;
	6103	+ port->typec_caps.type = ret;
5428	6104	port->port_type = port->typec_caps.type;
5429	6105
	6106	+ port->slow_charger_loop = fwnode_property_read_bool(fwnode, "slow-charger-loop");
5430	6107	if (port->port_type == TYPEC_PORT_SNK)
5431	6108	goto sink;
5432	6109
5433	6110	/* Get source pdos */
5434		- ret = fwnode_property_read_u32_array(fwnode, "source-pdos",
5435		- NULL, 0);
	6111	+ ret = fwnode_property_count_u32(fwnode, "source-pdos");
5436	6112	if (ret <= 0)
5437	6113	return -EINVAL;
5438	6114
..	..	@@ -5456,8 +6132,7 @@
5456	6132	return -EINVAL;
5457	6133	sink:
5458	6134	/* Get sink pdos */
5459		- ret = fwnode_property_read_u32_array(fwnode, "sink-pdos",
5460		- NULL, 0);
	6135	+ ret = fwnode_property_count_u32(fwnode, "sink-pdos");
5461	6136	if (ret <= 0)
5462	6137	return -EINVAL;
5463	6138
..	..	@@ -5476,9 +6151,10 @@
5476	6151
5477	6152	/* FRS can only be supported byb DRP ports */
5478	6153	if (port->port_type == TYPEC_PORT_DRP) {
5479		- ret = fwnode_property_read_u32(fwnode, "frs-typec-current", &frs_current);
	6154	+ ret = fwnode_property_read_u32(fwnode, "new-source-frs-typec-current",
	6155	+ &frs_current);
5480	6156	if (ret >= 0 && frs_current <= FRS_5V_3A)
5481		- port->frs_current = frs_current;
	6157	+ port->new_source_frs_current = frs_current;
5482	6158	}
5483	6159
5484	6160	/* sink-vdos is optional */
..	..	@@ -5495,49 +6171,49 @@
5495	6171	return ret;
5496	6172	}
5497	6173
5498		- return 0;
5499		-}
	6174	+ /* If sink-vdos is found, sink-vdos-v1 is expected for backward compatibility. */
	6175	+ if (port->nr_snk_vdo) {
	6176	+ ret = fwnode_property_count_u32(fwnode, "sink-vdos-v1");
	6177	+ if (ret < 0)
	6178	+ return ret;
	6179	+ else if (ret == 0)
	6180	+ return -ENODATA;
5500	6181
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;
	6182	+ port->nr_snk_vdo_v1 = min(ret, VDO_MAX_OBJECTS);
	6183	+ ret = fwnode_property_read_u32_array(fwnode, "sink-vdos-v1",
	6184	+ port->snk_vdo_v1,
	6185	+ port->nr_snk_vdo_v1);
	6186	+ if (ret < 0)
	6187	+ return ret;
5524	6188	}
5525		- mutex_unlock(&port->lock);
	6189	+
5526	6190	return 0;
5527	6191	}
5528		-EXPORT_SYMBOL_GPL(tcpm_update_source_capabilities);
5529	6192
5530		-int tcpm_update_sink_capabilities(struct tcpm_port port, const u32 pdo,
5531		- unsigned int nr_pdo,
	6193	+static int tcpm_copy_pdos(u32 dest_pdo, const u32 src_pdo, unsigned int nr_pdo)
	6194	+{
	6195	+ unsigned int i;
	6196	+
	6197	+ if (nr_pdo > PDO_MAX_OBJECTS)
	6198	+ nr_pdo = PDO_MAX_OBJECTS;
	6199	+
	6200	+ for (i = 0; i < nr_pdo; i++)
	6201	+ dest_pdo[i] = src_pdo[i];
	6202	+
	6203	+ return nr_pdo;
	6204	+}
	6205	+
	6206	+int tcpm_update_sink_capabilities(struct tcpm_port port, const u32 pdo, unsigned int nr_pdo,
5532	6207	unsigned int operating_snk_mw)
5533	6208	{
	6209	+ int ret = 0;
	6210	+
5534	6211	if (tcpm_validate_caps(port, pdo, nr_pdo))
5535	6212	return -EINVAL;
5536	6213
5537	6214	mutex_lock(&port->lock);
5538	6215	port->nr_snk_pdo = tcpm_copy_pdos(port->snk_pdo, pdo, nr_pdo);
5539	6216	port->operating_snk_mw = operating_snk_mw;
5540		- port->update_sink_caps = true;
5541	6217
5542	6218	switch (port->state) {
5543	6219	case SNK_NEGOTIATE_CAPABILITIES:
..	..	@@ -5546,15 +6222,25 @@
5546	6222	case SNK_TRANSITION_SINK:
5547	6223	case SNK_TRANSITION_SINK_VBUS:
5548	6224	if (port->pps_data.active)
5549		- tcpm_set_state(port, SNK_NEGOTIATE_PPS_CAPABILITIES, 0);
	6225	+ port->upcoming_state = SNK_NEGOTIATE_PPS_CAPABILITIES;
	6226	+ else if (port->pd_capable)
	6227	+ port->upcoming_state = SNK_NEGOTIATE_CAPABILITIES;
5550	6228	else
5551		- tcpm_set_state(port, SNK_NEGOTIATE_CAPABILITIES, 0);
	6229	+ break;
	6230	+
	6231	+ port->update_sink_caps = true;
	6232	+
	6233	+ ret = tcpm_ams_start(port, POWER_NEGOTIATION);
	6234	+ if (ret == -EAGAIN) {
	6235	+ port->upcoming_state = INVALID_STATE;
	6236	+ break;
	6237	+ }
5552	6238	break;
5553	6239	default:
5554	6240	break;
5555	6241	}
5556	6242	mutex_unlock(&port->lock);
5557		- return 0;
	6243	+ return ret;
5558	6244	}
5559	6245	EXPORT_SYMBOL_GPL(tcpm_update_sink_capabilities);
5560	6246
..	..	@@ -5639,6 +6325,27 @@
5639	6325	return 0;
5640	6326	}
5641	6327
	6328	+static int tcpm_psy_get_input_power_limit(struct tcpm_port *port,
	6329	+ union power_supply_propval *val)
	6330	+{
	6331	+ unsigned int src_mv, src_ma, max_src_mw = 0;
	6332	+ unsigned int i, tmp;
	6333	+
	6334	+ for (i = 0; i < port->nr_source_caps; i++) {
	6335	+ u32 pdo = port->source_caps[i];
	6336	+
	6337	+ if (pdo_type(pdo) == PDO_TYPE_FIXED) {
	6338	+ src_mv = pdo_fixed_voltage(pdo);
	6339	+ src_ma = pdo_max_current(pdo);
	6340	+ tmp = src_mv * src_ma / 1000;
	6341	+ max_src_mw = tmp > max_src_mw ? tmp : max_src_mw;
	6342	+ }
	6343	+ }
	6344	+
	6345	+ val->intval = max_src_mw;
	6346	+ return 0;
	6347	+}
	6348	+
5642	6349	static int tcpm_psy_get_prop(struct power_supply *psy,
5643	6350	enum power_supply_property psp,
5644	6351	union power_supply_propval *val)
..	..	@@ -5668,11 +6375,13 @@
5668	6375	case POWER_SUPPLY_PROP_CURRENT_NOW:
5669	6376	ret = tcpm_psy_get_current_now(port, val);
5670	6377	break;
	6378	+ case POWER_SUPPLY_PROP_INPUT_POWER_LIMIT:
	6379	+ tcpm_psy_get_input_power_limit(port, val);
	6380	+ break;
5671	6381	default:
5672	6382	ret = -EINVAL;
5673	6383	break;
5674	6384	}
5675		-
5676	6385	return ret;
5677	6386	}
5678	6387
..	..	@@ -5724,7 +6433,7 @@
5724	6433	ret = -EINVAL;
5725	6434	break;
5726	6435	}
5727		-
	6436	+ power_supply_changed(port->psy);
5728	6437	return ret;
5729	6438	}
5730	6439
..	..	@@ -5807,92 +6516,18 @@
5807	6516	return HRTIMER_NORESTART;
5808	6517	}
5809	6518
5810		-static enum hrtimer_restart data_role_swap_timer_handler(struct hrtimer *timer)
	6519	+static enum hrtimer_restart send_discover_timer_handler(struct hrtimer *timer)
5811	6520	{
5812		- struct tcpm_port *port =
5813		- container_of(timer, struct tcpm_port, data_role_swap_timer);
	6521	+ struct tcpm_port *port = container_of(timer, struct tcpm_port, send_discover_timer);
5814	6522
5815		- kthread_queue_work(port->wq, &port->data_role_swap);
	6523	+ kthread_queue_work(port->wq, &port->send_discover_work);
5816	6524	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	6525	}
5890	6526
5891	6527	struct tcpm_port tcpm_register_port(struct device dev, struct tcpc_dev *tcpc)
5892	6528	{
5893	6529	struct tcpm_port *port;
5894	6530	int err;
5895		- struct sched_param param = { .sched_priority = MAX_RT_PRIO - 1 };
5896	6531
5897	6532	if (!dev \|\| !tcpc \|\|
5898	6533	!tcpc->get_vbus \|\| !tcpc->set_cc \|\| !tcpc->get_cc \|\|
..	..	@@ -5909,26 +6544,28 @@
5909	6544
5910	6545	mutex_init(&port->lock);
5911	6546	mutex_init(&port->swap_lock);
	6547	+#ifdef CONFIG_NO_GKI
5912	6548	mutex_init(&port->pd_handler_lock);
	6549	+#endif
5913	6550
5914	6551	port->wq = kthread_create_worker(0, dev_name(dev));
5915	6552	if (IS_ERR(port->wq))
5916	6553	return ERR_CAST(port->wq);
5917		- sched_setscheduler_nocheck(port->wq->task, SCHED_FIFO, &param);
	6554	+ sched_set_fifo(port->wq->task);
5918	6555
5919	6556	kthread_init_work(&port->state_machine, tcpm_state_machine_work);
5920	6557	kthread_init_work(&port->vdm_state_machine, vdm_state_machine_work);
5921	6558	kthread_init_work(&port->event_work, tcpm_pd_event_handler);
5922	6559	kthread_init_work(&port->enable_frs, tcpm_enable_frs_work);
5923		- kthread_init_work(&port->data_role_swap, tcpm_data_role_swap_work);
	6560	+ kthread_init_work(&port->send_discover_work, tcpm_send_discover_work);
5924	6561	hrtimer_init(&port->state_machine_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
5925	6562	port->state_machine_timer.function = state_machine_timer_handler;
5926	6563	hrtimer_init(&port->vdm_state_machine_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
5927	6564	port->vdm_state_machine_timer.function = vdm_state_machine_timer_handler;
5928	6565	hrtimer_init(&port->enable_frs_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
5929	6566	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;
	6567	+ hrtimer_init(&port->send_discover_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
	6568	+ port->send_discover_timer.function = send_discover_timer_handler;
5932	6569
5933	6570	spin_lock_init(&port->pd_event_lock);
5934	6571
..	..	@@ -5936,11 +6573,6 @@
5936	6573	init_completion(&port->swap_complete);
5937	6574	init_completion(&port->pps_complete);
5938	6575	tcpm_debugfs_init(port);
5939		-
5940		- /* init extcon */
5941		- err = tcpm_extcon_register(port);
5942		- if (err)
5943		- goto out_destroy_wq;
5944	6576
5945	6577	err = tcpm_fw_get_caps(port, tcpc->fwnode);
5946	6578	if (err < 0)
..	..	@@ -5950,12 +6582,10 @@
5950	6582
5951	6583	port->typec_caps.fwnode = tcpc->fwnode;
5952	6584	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;
	6585	+ port->typec_caps.svdm_version = SVDM_VER_2_0;
	6586	+ port->typec_caps.driver_data = port;
	6587	+ port->typec_caps.ops = &tcpm_ops;
	6588	+ port->typec_caps.orientation_aware = 1;
5959	6589
5960	6590	port->partner_desc.identity = &port->partner_ident;
5961	6591	port->port_type = port->typec_caps.type;
..	..	@@ -5968,12 +6598,13 @@
5968	6598
5969	6599	err = devm_tcpm_psy_register(port);
5970	6600	if (err)
5971		- goto out_destroy_wq;
	6601	+ goto out_role_sw_put;
	6602	+ power_supply_changed(port->psy);
5972	6603
5973	6604	port->typec_port = typec_register_port(port->dev, &port->typec_caps);
5974	6605	if (IS_ERR(port->typec_port)) {
5975	6606	err = PTR_ERR(port->typec_port);
5976		- goto out_destroy_wq;
	6607	+ goto out_role_sw_put;
5977	6608	}
5978	6609
5979	6610	typec_port_register_altmodes(port->typec_port,
..	..	@@ -5987,8 +6618,10 @@
5987	6618	tcpm_log(port, "%s: registered", dev_name(dev));
5988	6619	return port;
5989	6620
5990		-out_destroy_wq:
	6621	+out_role_sw_put:
5991	6622	usb_role_switch_put(port->role_sw);
	6623	+out_destroy_wq:
	6624	+ tcpm_debugfs_exit(port);
5992	6625	kthread_destroy_worker(port->wq);
5993	6626	return ERR_PTR(err);
5994	6627	}
..	..	@@ -5998,10 +6631,10 @@
5998	6631	{
5999	6632	int i;
6000	6633
	6634	+ hrtimer_cancel(&port->send_discover_timer);
6001	6635	hrtimer_cancel(&port->enable_frs_timer);
6002	6636	hrtimer_cancel(&port->vdm_state_machine_timer);
6003	6637	hrtimer_cancel(&port->state_machine_timer);
6004		- hrtimer_cancel(&port->data_role_swap_timer);
6005	6638
6006	6639	tcpm_reset_port(port);
6007	6640	for (i = 0; i < ARRAY_SIZE(port->port_altmode); i++)