drivers/usb/typec/tcpm/tcpm.c

// SPDX-License-Identifier: GPL-2.0+
/*
 * Copyright 2015-2017 Google, Inc
 *
 * USB Power Delivery protocol stack.
 */

#include <linux/completion.h>
#include <linux/debugfs.h>
#include <linux/device.h>
#include <linux/hrtimer.h>
#include <linux/jiffies.h>
#include <linux/kernel.h>
#include <linux/kthread.h>
#include <linux/module.h>
#include <linux/mutex.h>
#include <linux/power_supply.h>
#include <linux/proc_fs.h>
#include <linux/property.h>
#include <linux/sched/clock.h>
#include <linux/seq_file.h>
#include <linux/slab.h>
#include <linux/spinlock.h>
#include <linux/usb.h>
#include <linux/usb/pd.h>
#include <linux/usb/pd_ado.h>
#include <linux/usb/pd_bdo.h>
#include <linux/usb/pd_ext_sdb.h>
#include <linux/usb/pd_vdo.h>
#include <linux/usb/role.h>
#include <linux/usb/tcpm.h>
#include <linux/usb/typec_altmode.h>

#include <trace/hooks/typec.h>
#include <uapi/linux/sched/types.h>

#define FOREACH_STATE(S)			\
	S(INVALID_STATE),			\
	S(TOGGLING),			\
	S(CHECK_CONTAMINANT),			\
	S(SRC_UNATTACHED),			\
	S(SRC_ATTACH_WAIT),			\
	S(SRC_ATTACHED),			\
	S(SRC_STARTUP),				\
	S(SRC_SEND_CAPABILITIES),		\
	S(SRC_SEND_CAPABILITIES_TIMEOUT),	\
	S(SRC_NEGOTIATE_CAPABILITIES),		\
	S(SRC_TRANSITION_SUPPLY),		\
	S(SRC_READY),				\
	S(SRC_WAIT_NEW_CAPABILITIES),		\
						\
	S(SNK_UNATTACHED),			\
	S(SNK_ATTACH_WAIT),			\
	S(SNK_DEBOUNCED),			\
	S(SNK_ATTACHED),			\
	S(SNK_STARTUP),				\
	S(SNK_DISCOVERY),			\
	S(SNK_DISCOVERY_DEBOUNCE),		\
	S(SNK_DISCOVERY_DEBOUNCE_DONE),		\
	S(SNK_WAIT_CAPABILITIES),		\
	S(SNK_NEGOTIATE_CAPABILITIES),		\
	S(SNK_NEGOTIATE_PPS_CAPABILITIES),	\
	S(SNK_TRANSITION_SINK),			\
	S(SNK_TRANSITION_SINK_VBUS),		\
	S(SNK_READY),				\
						\
	S(ACC_UNATTACHED),			\
	S(DEBUG_ACC_ATTACHED),			\
	S(AUDIO_ACC_ATTACHED),			\
	S(AUDIO_ACC_DEBOUNCE),			\
						\
	S(HARD_RESET_SEND),			\
	S(HARD_RESET_START),			\
	S(SRC_HARD_RESET_VBUS_OFF),		\
	S(SRC_HARD_RESET_VBUS_ON),		\
	S(SNK_HARD_RESET_SINK_OFF),		\
	S(SNK_HARD_RESET_WAIT_VBUS),		\
	S(SNK_HARD_RESET_SINK_ON),		\
						\
	S(SOFT_RESET),				\
	S(SRC_SOFT_RESET_WAIT_SNK_TX),		\
	S(SNK_SOFT_RESET),			\
	S(SOFT_RESET_SEND),			\
						\
	S(DR_SWAP_ACCEPT),			\
	S(DR_SWAP_SEND),			\
	S(DR_SWAP_SEND_TIMEOUT),		\
	S(DR_SWAP_CANCEL),			\
	S(DR_SWAP_CHANGE_DR),			\
						\
	S(PR_SWAP_ACCEPT),			\
	S(PR_SWAP_SEND),			\
	S(PR_SWAP_SEND_TIMEOUT),		\
	S(PR_SWAP_CANCEL),			\
	S(PR_SWAP_START),			\
	S(PR_SWAP_SRC_SNK_TRANSITION_OFF),	\
	S(PR_SWAP_SRC_SNK_SOURCE_OFF),		\
	S(PR_SWAP_SRC_SNK_SOURCE_OFF_CC_DEBOUNCED), \
	S(PR_SWAP_SRC_SNK_SINK_ON),		\
	S(PR_SWAP_SNK_SRC_SINK_OFF),		\
	S(PR_SWAP_SNK_SRC_SOURCE_ON),		\
	S(PR_SWAP_SNK_SRC_SOURCE_ON_VBUS_RAMPED_UP),    \
						\
	S(VCONN_SWAP_ACCEPT),			\
	S(VCONN_SWAP_SEND),			\
	S(VCONN_SWAP_SEND_TIMEOUT),		\
	S(VCONN_SWAP_CANCEL),			\
	S(VCONN_SWAP_START),			\
	S(VCONN_SWAP_WAIT_FOR_VCONN),		\
	S(VCONN_SWAP_TURN_ON_VCONN),		\
	S(VCONN_SWAP_TURN_OFF_VCONN),		\
						\
	S(FR_SWAP_SEND),			\
	S(FR_SWAP_SEND_TIMEOUT),		\
	S(FR_SWAP_SNK_SRC_TRANSITION_TO_OFF),			\
	S(FR_SWAP_SNK_SRC_NEW_SINK_READY),		\
	S(FR_SWAP_SNK_SRC_SOURCE_VBUS_APPLIED),	\
	S(FR_SWAP_CANCEL),			\
						\
	S(SNK_TRY),				\
	S(SNK_TRY_WAIT),			\
	S(SNK_TRY_WAIT_DEBOUNCE),               \
	S(SNK_TRY_WAIT_DEBOUNCE_CHECK_VBUS),    \
	S(SRC_TRYWAIT),				\
	S(SRC_TRYWAIT_DEBOUNCE),		\
	S(SRC_TRYWAIT_UNATTACHED),		\
						\
	S(SRC_TRY),				\
	S(SRC_TRY_WAIT),                        \
	S(SRC_TRY_DEBOUNCE),			\
	S(SNK_TRYWAIT),				\
	S(SNK_TRYWAIT_DEBOUNCE),		\
	S(SNK_TRYWAIT_VBUS),			\
	S(BIST_RX),				\
						\
	S(GET_STATUS_SEND),			\
	S(GET_STATUS_SEND_TIMEOUT),		\
	S(GET_PPS_STATUS_SEND),			\
	S(GET_PPS_STATUS_SEND_TIMEOUT),		\
						\
	S(GET_SINK_CAP),			\
	S(GET_SINK_CAP_TIMEOUT),		\
						\
	S(ERROR_RECOVERY),			\
	S(PORT_RESET),				\
	S(PORT_RESET_WAIT_OFF),			\
						\
	S(AMS_START),				\
	S(CHUNK_NOT_SUPP)

#define FOREACH_AMS(S)				\
	S(NONE_AMS),				\
	S(POWER_NEGOTIATION),			\
	S(GOTOMIN),				\
	S(SOFT_RESET_AMS),			\
	S(HARD_RESET),				\
	S(CABLE_RESET),				\
	S(GET_SOURCE_CAPABILITIES),		\
	S(GET_SINK_CAPABILITIES),		\
	S(POWER_ROLE_SWAP),			\
	S(FAST_ROLE_SWAP),			\
	S(DATA_ROLE_SWAP),			\
	S(VCONN_SWAP),				\
	S(SOURCE_ALERT),			\
	S(GETTING_SOURCE_EXTENDED_CAPABILITIES),\
	S(GETTING_SOURCE_SINK_STATUS),		\
	S(GETTING_BATTERY_CAPABILITIES),	\
	S(GETTING_BATTERY_STATUS),		\
	S(GETTING_MANUFACTURER_INFORMATION),	\
	S(SECURITY),				\
	S(FIRMWARE_UPDATE),			\
	S(DISCOVER_IDENTITY),			\
	S(SOURCE_STARTUP_CABLE_PLUG_DISCOVER_IDENTITY),	\
	S(DISCOVER_SVIDS),			\
	S(DISCOVER_MODES),			\
	S(DFP_TO_UFP_ENTER_MODE),		\
	S(DFP_TO_UFP_EXIT_MODE),		\
	S(DFP_TO_CABLE_PLUG_ENTER_MODE),	\
	S(DFP_TO_CABLE_PLUG_EXIT_MODE),		\
	S(ATTENTION),				\
	S(BIST),				\
	S(UNSTRUCTURED_VDMS),			\
	S(STRUCTURED_VDMS),			\
	S(COUNTRY_INFO),			\
	S(COUNTRY_CODES)

#define GENERATE_ENUM(e)	e
#define GENERATE_STRING(s)	#s

enum tcpm_state {
	FOREACH_STATE(GENERATE_ENUM)
};

static const char * const tcpm_states[] = {
	FOREACH_STATE(GENERATE_STRING)
};

enum tcpm_ams {
	FOREACH_AMS(GENERATE_ENUM)
};

static const char * const tcpm_ams_str[] = {
	FOREACH_AMS(GENERATE_STRING)
};

enum vdm_states {
	VDM_STATE_ERR_BUSY = -3,
	VDM_STATE_ERR_SEND = -2,
	VDM_STATE_ERR_TMOUT = -1,
	VDM_STATE_DONE = 0,
	/* Anything >0 represents an active state */
	VDM_STATE_READY = 1,
	VDM_STATE_BUSY = 2,
	VDM_STATE_WAIT_RSP_BUSY = 3,
	VDM_STATE_SEND_MESSAGE = 4,
};

enum pd_msg_request {
	PD_MSG_NONE = 0,
	PD_MSG_CTRL_REJECT,
	PD_MSG_CTRL_WAIT,
	PD_MSG_CTRL_NOT_SUPP,
	PD_MSG_DATA_SINK_CAP,
	PD_MSG_DATA_SOURCE_CAP,
};

enum adev_actions {
	ADEV_NONE = 0,
	ADEV_NOTIFY_USB_AND_QUEUE_VDM,
	ADEV_QUEUE_VDM,
	ADEV_QUEUE_VDM_SEND_EXIT_MODE_ON_FAIL,
	ADEV_ATTENTION,
};

/*
 * Initial current capability of the new source when vSafe5V is applied during PD3.0 Fast Role Swap.
 * Based on "Table 6-14 Fixed Supply PDO - Sink" of "USB Power Delivery Specification Revision 3.0,
 * Version 1.2"
 */
enum frs_typec_current {
	FRS_NOT_SUPPORTED,
	FRS_DEFAULT_POWER,
	FRS_5V_1P5A,
	FRS_5V_3A,
};

/* Events from low level driver */

#define TCPM_CC_EVENT		BIT(0)
#define TCPM_VBUS_EVENT		BIT(1)
#define TCPM_RESET_EVENT	BIT(2)
#define TCPM_FRS_EVENT		BIT(3)
#define TCPM_SOURCING_VBUS	BIT(4)
#define TCPM_PORT_CLEAN		BIT(5)

#define LOG_BUFFER_ENTRIES	1024
#define LOG_BUFFER_ENTRY_SIZE	128

/* Alternate mode support */

#define SVID_DISCOVERY_MAX	16
#define ALTMODE_DISCOVERY_MAX	(SVID_DISCOVERY_MAX * MODE_DISCOVERY_MAX)

#define GET_SINK_CAP_RETRY_MS	100
#define SEND_DISCOVER_RETRY_MS	100

struct pd_mode_data {
	int svid_index;		/* current SVID index		*/
	int nsvids;
	u16 svids[SVID_DISCOVERY_MAX];
	int altmodes;		/* number of alternate modes	*/
	struct typec_altmode_desc altmode_desc[ALTMODE_DISCOVERY_MAX];
};

/*
 * @min_volt: Actual min voltage at the local port
 * @req_min_volt: Requested min voltage to the port partner
 * @max_volt: Actual max voltage at the local port
 * @req_max_volt: Requested max voltage to the port partner
 * @max_curr: Actual max current at the local port
 * @req_max_curr: Requested max current of the port partner
 * @req_out_volt: Requested output voltage to the port partner
 * @req_op_curr: Requested operating current to the port partner
 * @supported: Parter has at least one APDO hence supports PPS
 * @active: PPS mode is active
 */
struct pd_pps_data {
	u32 min_volt;
	u32 req_min_volt;
	u32 max_volt;
	u32 req_max_volt;
	u32 max_curr;
	u32 req_max_curr;
	u32 req_out_volt;
	u32 req_op_curr;
	bool supported;
	bool active;
};

struct pd_data {
	struct usb_power_delivery *pd;
	struct usb_power_delivery_capabilities *source_cap;
	struct usb_power_delivery_capabilities_desc source_desc;
	struct usb_power_delivery_capabilities *sink_cap;
	struct usb_power_delivery_capabilities_desc sink_desc;
	unsigned int operating_snk_mw;
};

struct tcpm_port {
	struct device *dev;

	struct mutex lock;		/* tcpm state machine lock */
	struct kthread_worker *wq;

	struct typec_capability typec_caps;
	struct typec_port *typec_port;

	struct tcpc_dev	*tcpc;
	struct usb_role_switch *role_sw;

	enum typec_role vconn_role;
	enum typec_role pwr_role;
	enum typec_data_role data_role;
	enum typec_pwr_opmode pwr_opmode;

	struct usb_pd_identity partner_ident;
	struct typec_partner_desc partner_desc;
	struct typec_partner *partner;

	enum typec_cc_status cc_req;
	enum typec_cc_status src_rp;	/* work only if pd_supported == false */

	enum typec_cc_status cc1;
	enum typec_cc_status cc2;
	enum typec_cc_polarity polarity;

	bool attached;
	bool connected;
	bool registered;
	bool pd_supported;
	enum typec_port_type port_type;

	/*
	 * Set to true when vbus is greater than VSAFE5V min.
	 * Set to false when vbus falls below vSinkDisconnect max threshold.
	 */
	bool vbus_present;

	/*
	 * Set to true when vbus is less than VSAFE0V max.
	 * Set to false when vbus is greater than VSAFE0V max.
	 */
	bool vbus_vsafe0v;

	bool vbus_never_low;
	bool vbus_source;
	bool vbus_charge;

	/* Set to true when Discover_Identity Command is expected to be sent in Ready states. */
	bool send_discover;
	bool op_vsafe5v;

	int try_role;
	int try_snk_count;
	int try_src_count;

	enum pd_msg_request queued_message;

	enum tcpm_state enter_state;
	enum tcpm_state prev_state;
	enum tcpm_state state;
	enum tcpm_state delayed_state;
	ktime_t delayed_runtime;
	unsigned long delay_ms;

	spinlock_t pd_event_lock;
	u32 pd_events;

	struct kthread_work event_work;
	struct hrtimer state_machine_timer;
	struct kthread_work state_machine;
	struct hrtimer vdm_state_machine_timer;
	struct kthread_work vdm_state_machine;
	struct hrtimer enable_frs_timer;
	struct kthread_work enable_frs;
	struct hrtimer send_discover_timer;
	struct kthread_work send_discover_work;
	bool state_machine_running;
	/* Set to true when VDM State Machine has following actions. */
	bool vdm_sm_running;

	struct completion tx_complete;
	enum tcpm_transmit_status tx_status;

	struct mutex swap_lock;		/* swap command lock */
	bool swap_pending;
	bool non_pd_role_swap;
	struct completion swap_complete;
	int swap_status;

	unsigned int negotiated_rev;
	unsigned int message_id;
	unsigned int caps_count;
	unsigned int hard_reset_count;
	bool pd_capable;
	bool explicit_contract;
	unsigned int rx_msgid;

	/* USB PD objects */
	struct usb_power_delivery **pds;
	struct pd_data **pd_list;
	struct usb_power_delivery_capabilities *port_source_caps;
	struct usb_power_delivery_capabilities *port_sink_caps;
	struct usb_power_delivery *partner_pd;
	struct usb_power_delivery_capabilities *partner_source_caps;
	struct usb_power_delivery_capabilities *partner_sink_caps;
	struct usb_power_delivery *selected_pd;

	/* Partner capabilities/requests */
	u32 sink_request;
	u32 source_caps[PDO_MAX_OBJECTS];
	unsigned int nr_source_caps;
	u32 sink_caps[PDO_MAX_OBJECTS];
	unsigned int nr_sink_caps;

	/* Local capabilities */
	unsigned int pd_count;
	u32 src_pdo[PDO_MAX_OBJECTS];
	unsigned int nr_src_pdo;
	u32 snk_pdo[PDO_MAX_OBJECTS];
	unsigned int nr_snk_pdo;
	u32 snk_vdo_v1[VDO_MAX_OBJECTS];
	unsigned int nr_snk_vdo_v1;
	u32 snk_vdo[VDO_MAX_OBJECTS];
	unsigned int nr_snk_vdo;

	unsigned int operating_snk_mw;
	bool update_sink_caps;

	/* Requested current / voltage to the port partner */
	u32 req_current_limit;
	u32 req_supply_voltage;
	/* Actual current / voltage limit of the local port */
	u32 current_limit;
	u32 supply_voltage;

	/* Used to export TA voltage and current */
	struct power_supply *psy;
	struct power_supply_desc psy_desc;
	enum power_supply_usb_type usb_type;

	u32 bist_request;

	/* PD state for Vendor Defined Messages */
	enum vdm_states vdm_state;
	u32 vdm_retries;
	/* next Vendor Defined Message to send */
	u32 vdo_data[VDO_MAX_SIZE];
	u8 vdo_count;
	/* VDO to retry if UFP responder replied busy */
	u32 vdo_retry;

	/* PPS */
	struct pd_pps_data pps_data;
	struct completion pps_complete;
	bool pps_pending;
	int pps_status;

	/* Alternate mode data */
	struct pd_mode_data mode_data;
	struct typec_altmode *partner_altmode[ALTMODE_DISCOVERY_MAX];
	struct typec_altmode *port_altmode[ALTMODE_DISCOVERY_MAX];

	/* Deadline in jiffies to exit src_try_wait state */
	unsigned long max_wait;

	/* port belongs to a self powered device */
	bool self_powered;

	/* Sink FRS */
	enum frs_typec_current new_source_frs_current;

	/* Sink caps have been queried */
	bool sink_cap_done;

	/* Collision Avoidance and Atomic Message Sequence */
	enum tcpm_state upcoming_state;
	enum tcpm_ams ams;
	enum tcpm_ams next_ams;
	bool in_ams;

	/* Auto vbus discharge status */
	bool auto_vbus_discharge_enabled;

	/*
	 * When set, port requests PD_P_SNK_STDBY_MW upon entering SNK_DISCOVERY and
	 * the actual current limit after RX of PD_CTRL_PSRDY for PD link,
	 * SNK_READY for non-pd link.
	 */
	bool slow_charger_loop;

	/*
	 * When true indicates that the lower level drivers indicate potential presence
	 * of contaminant in the connector pins based on the tcpm state machine
	 * transitions.
	 */
	bool potential_contaminant;
#ifdef CONFIG_DEBUG_FS
	struct dentry *dentry;
	struct mutex logbuffer_lock;	/* log buffer access lock */
	int logbuffer_head;
	int logbuffer_tail;
	u8 *logbuffer[LOG_BUFFER_ENTRIES];
#endif
};

struct pd_rx_event {
	struct kthread_work work;
	struct tcpm_port *port;
	struct pd_message msg;
};

static const char * const pd_rev[] = {
	[PD_REV10]		= "rev1",
	[PD_REV20]		= "rev2",
	[PD_REV30]		= "rev3",
};

#define tcpm_cc_is_sink(cc) \
	((cc) == TYPEC_CC_RP_DEF || (cc) == TYPEC_CC_RP_1_5 || \
	 (cc) == TYPEC_CC_RP_3_0)

/* As long as cc is pulled up, we can consider it as sink. */
#define tcpm_port_is_sink(port) \
	(tcpm_cc_is_sink((port)->cc1) || tcpm_cc_is_sink((port)->cc2))

#define tcpm_cc_is_source(cc) ((cc) == TYPEC_CC_RD)
#define tcpm_cc_is_audio(cc) ((cc) == TYPEC_CC_RA)
#define tcpm_cc_is_open(cc) ((cc) == TYPEC_CC_OPEN)

#define tcpm_port_is_source(port) \
	((tcpm_cc_is_source((port)->cc1) && \
	 !tcpm_cc_is_source((port)->cc2)) || \
	 (tcpm_cc_is_source((port)->cc2) && \
	  !tcpm_cc_is_source((port)->cc1)))

#define tcpm_port_is_debug(port) \
	(tcpm_cc_is_source((port)->cc1) && tcpm_cc_is_source((port)->cc2))

#define tcpm_port_is_audio(port) \
	(tcpm_cc_is_audio((port)->cc1) && tcpm_cc_is_audio((port)->cc2))

#define tcpm_port_is_audio_detached(port) \
	((tcpm_cc_is_audio((port)->cc1) && tcpm_cc_is_open((port)->cc2)) || \
	 (tcpm_cc_is_audio((port)->cc2) && tcpm_cc_is_open((port)->cc1)))

#define tcpm_try_snk(port) \
	((port)->try_snk_count == 0 && (port)->try_role == TYPEC_SINK && \
	(port)->port_type == TYPEC_PORT_DRP)

#define tcpm_try_src(port) \
	((port)->try_src_count == 0 && (port)->try_role == TYPEC_SOURCE && \
	(port)->port_type == TYPEC_PORT_DRP)

#define tcpm_data_role_for_source(port) \
	((port)->typec_caps.data == TYPEC_PORT_UFP ? \
	TYPEC_DEVICE : TYPEC_HOST)

#define tcpm_data_role_for_sink(port) \
	((port)->typec_caps.data == TYPEC_PORT_DFP ? \
	TYPEC_HOST : TYPEC_DEVICE)

#define tcpm_sink_tx_ok(port) \
	(tcpm_port_is_sink(port) && \
	((port)->cc1 == TYPEC_CC_RP_3_0 || (port)->cc2 == TYPEC_CC_RP_3_0))

#define tcpm_wait_for_discharge(port) \
	(((port)->auto_vbus_discharge_enabled && !(port)->vbus_vsafe0v) ? PD_T_SAFE_0V : 0)

static enum tcpm_state tcpm_default_state(struct tcpm_port *port)
{
	if (port->port_type == TYPEC_PORT_DRP) {
		if (port->try_role == TYPEC_SINK)
			return SNK_UNATTACHED;
		else if (port->try_role == TYPEC_SOURCE)
			return SRC_UNATTACHED;
		/* Fall through to return SRC_UNATTACHED */
	} else if (port->port_type == TYPEC_PORT_SNK) {
		return SNK_UNATTACHED;
	}
	return SRC_UNATTACHED;
}

static bool tcpm_port_is_disconnected(struct tcpm_port *port)
{
	return (!port->attached && port->cc1 == TYPEC_CC_OPEN &&
		port->cc2 == TYPEC_CC_OPEN) ||
	       (port->attached && ((port->polarity == TYPEC_POLARITY_CC1 &&
				    port->cc1 == TYPEC_CC_OPEN) ||
				   (port->polarity == TYPEC_POLARITY_CC2 &&
				    port->cc2 == TYPEC_CC_OPEN)));
}

/*
 * Logging
 */

#ifdef CONFIG_DEBUG_FS

static bool tcpm_log_full(struct tcpm_port *port)
{
	return port->logbuffer_tail ==
		(port->logbuffer_head + 1) % LOG_BUFFER_ENTRIES;
}

__printf(2, 0)
static void _tcpm_log(struct tcpm_port *port, const char *fmt, va_list args)
{
	char tmpbuffer[LOG_BUFFER_ENTRY_SIZE];
	u64 ts_nsec = local_clock();
	unsigned long rem_nsec;
	bool bypass_log = false;

	mutex_lock(&port->logbuffer_lock);
	if (!port->logbuffer[port->logbuffer_head]) {
		port->logbuffer[port->logbuffer_head] =
				kzalloc(LOG_BUFFER_ENTRY_SIZE, GFP_KERNEL);
		if (!port->logbuffer[port->logbuffer_head]) {
			mutex_unlock(&port->logbuffer_lock);
			return;
		}
	}

	vsnprintf(tmpbuffer, sizeof(tmpbuffer), fmt, args);
	trace_android_vh_typec_tcpm_log(tmpbuffer, &bypass_log);
	if (bypass_log)
		goto abort;

	if (tcpm_log_full(port)) {
		port->logbuffer_head = max(port->logbuffer_head - 1, 0);
		strcpy(tmpbuffer, "overflow");
	}

	if (port->logbuffer_head < 0 ||
	    port->logbuffer_head >= LOG_BUFFER_ENTRIES) {
		dev_warn(port->dev,
			 "Bad log buffer index %d\n", port->logbuffer_head);
		goto abort;
	}

	if (!port->logbuffer[port->logbuffer_head]) {
		dev_warn(port->dev,
			 "Log buffer index %d is NULL\n", port->logbuffer_head);
		goto abort;
	}

	rem_nsec = do_div(ts_nsec, 1000000000);
	scnprintf(port->logbuffer[port->logbuffer_head],
		  LOG_BUFFER_ENTRY_SIZE, "[%5lu.%06lu] %s",
		  (unsigned long)ts_nsec, rem_nsec / 1000,
		  tmpbuffer);
	port->logbuffer_head = (port->logbuffer_head + 1) % LOG_BUFFER_ENTRIES;

abort:
	mutex_unlock(&port->logbuffer_lock);
}

__printf(2, 3)
static void tcpm_log(struct tcpm_port *port, const char *fmt, ...)
{
	va_list args;

	/* Do not log while disconnected and unattached */
	if (tcpm_port_is_disconnected(port) &&
	    (port->state == SRC_UNATTACHED || port->state == SNK_UNATTACHED ||
	     port->state == TOGGLING || port->state == CHECK_CONTAMINANT))
		return;

	va_start(args, fmt);
	_tcpm_log(port, fmt, args);
	va_end(args);
}

__printf(2, 3)
static void tcpm_log_force(struct tcpm_port *port, const char *fmt, ...)
{
	va_list args;

	va_start(args, fmt);
	_tcpm_log(port, fmt, args);
	va_end(args);
}

static void tcpm_log_source_caps(struct tcpm_port *port)
{
	int i;

	for (i = 0; i < port->nr_source_caps; i++) {
		u32 pdo = port->source_caps[i];
		enum pd_pdo_type type = pdo_type(pdo);
		char msg[64];

		switch (type) {
		case PDO_TYPE_FIXED:
			scnprintf(msg, sizeof(msg),
				  "%u mV, %u mA [%s%s%s%s%s%s]",
				  pdo_fixed_voltage(pdo),
				  pdo_max_current(pdo),
				  (pdo & PDO_FIXED_DUAL_ROLE) ?
							"R" : "",
				  (pdo & PDO_FIXED_SUSPEND) ?
							"S" : "",
				  (pdo & PDO_FIXED_HIGHER_CAP) ?
							"H" : "",
				  (pdo & PDO_FIXED_USB_COMM) ?
							"U" : "",
				  (pdo & PDO_FIXED_DATA_SWAP) ?
							"D" : "",
				  (pdo & PDO_FIXED_EXTPOWER) ?
							"E" : "");
			break;
		case PDO_TYPE_VAR:
			scnprintf(msg, sizeof(msg),
				  "%u-%u mV, %u mA",
				  pdo_min_voltage(pdo),
				  pdo_max_voltage(pdo),
				  pdo_max_current(pdo));
			break;
		case PDO_TYPE_BATT:
			scnprintf(msg, sizeof(msg),
				  "%u-%u mV, %u mW",
				  pdo_min_voltage(pdo),
				  pdo_max_voltage(pdo),
				  pdo_max_power(pdo));
			break;
		case PDO_TYPE_APDO:
			if (pdo_apdo_type(pdo) == APDO_TYPE_PPS)
				scnprintf(msg, sizeof(msg),
					  "%u-%u mV, %u mA",
					  pdo_pps_apdo_min_voltage(pdo),
					  pdo_pps_apdo_max_voltage(pdo),
					  pdo_pps_apdo_max_current(pdo));
			else
				strcpy(msg, "undefined APDO");
			break;
		default:
			strcpy(msg, "undefined");
			break;
		}
		tcpm_log(port, " PDO %d: type %d, %s",
			 i, type, msg);
	}
}

static int tcpm_debug_show(struct seq_file *s, void *v)
{
	struct tcpm_port *port = (struct tcpm_port *)s->private;
	int tail;

	mutex_lock(&port->logbuffer_lock);
	tail = port->logbuffer_tail;
	while (tail != port->logbuffer_head) {
		seq_printf(s, "%s\n", port->logbuffer[tail]);
		tail = (tail + 1) % LOG_BUFFER_ENTRIES;
	}
	if (!seq_has_overflowed(s))
		port->logbuffer_tail = tail;
	mutex_unlock(&port->logbuffer_lock);

	return 0;
}
DEFINE_SHOW_ATTRIBUTE(tcpm_debug);

static void tcpm_debugfs_init(struct tcpm_port *port)
{
	char name[NAME_MAX];

	mutex_init(&port->logbuffer_lock);
	snprintf(name, NAME_MAX, "tcpm-%s", dev_name(port->dev));
	port->dentry = debugfs_create_dir(name, usb_debug_root);
	debugfs_create_file("log", S_IFREG | 0444, port->dentry, port,
			    &tcpm_debug_fops);
}

static void tcpm_debugfs_exit(struct tcpm_port *port)
{
	int i;

	mutex_lock(&port->logbuffer_lock);
	for (i = 0; i < LOG_BUFFER_ENTRIES; i++) {
		kfree(port->logbuffer[i]);
		port->logbuffer[i] = NULL;
	}
	mutex_unlock(&port->logbuffer_lock);

	debugfs_remove(port->dentry);
}

#else

__printf(2, 3)
static void tcpm_log(const struct tcpm_port *port, const char *fmt, ...) { }
__printf(2, 3)
static void tcpm_log_force(struct tcpm_port *port, const char *fmt, ...) { }
static void tcpm_log_source_caps(struct tcpm_port *port) { }
static void tcpm_debugfs_init(const struct tcpm_port *port) { }
static void tcpm_debugfs_exit(const struct tcpm_port *port) { }

#endif

static void tcpm_set_cc(struct tcpm_port *port, enum typec_cc_status cc)
{
	tcpm_log(port, "cc:=%d", cc);
	port->cc_req = cc;
	port->tcpc->set_cc(port->tcpc, cc);
}

static int tcpm_enable_auto_vbus_discharge(struct tcpm_port *port, bool enable)
{
	int ret = 0;

	if (port->tcpc->enable_auto_vbus_discharge) {
		ret = port->tcpc->enable_auto_vbus_discharge(port->tcpc, enable);
		tcpm_log_force(port, "%s vbus discharge ret:%d", enable ? "enable" : "disable",
			       ret);
		if (!ret)
			port->auto_vbus_discharge_enabled = enable;
	}

	return ret;
}

static void tcpm_apply_rc(struct tcpm_port *port)
{
	/*
	 * TCPCI: Move to APPLY_RC state to prevent disconnect during PR_SWAP
	 * when Vbus auto discharge on disconnect is enabled.
	 */
	if (port->tcpc->enable_auto_vbus_discharge && port->tcpc->apply_rc) {
		tcpm_log(port, "Apply_RC");
		port->tcpc->apply_rc(port->tcpc, port->cc_req, port->polarity);
		tcpm_enable_auto_vbus_discharge(port, false);
	}
}

/*
 * Determine RP value to set based on maximum current supported
 * by a port if configured as source.
 * Returns CC value to report to link partner.
 */
static enum typec_cc_status tcpm_rp_cc(struct tcpm_port *port)
{
	const u32 *src_pdo = port->src_pdo;
	int nr_pdo = port->nr_src_pdo;
	int i;

	if (!port->pd_supported)
		return port->src_rp;

	/*
	 * Search for first entry with matching voltage.
	 * It should report the maximum supported current.
	 */
	for (i = 0; i < nr_pdo; i++) {
		const u32 pdo = src_pdo[i];

		if (pdo_type(pdo) == PDO_TYPE_FIXED &&
		    pdo_fixed_voltage(pdo) == 5000) {
			unsigned int curr = pdo_max_current(pdo);

			if (curr >= 3000)
				return TYPEC_CC_RP_3_0;
			else if (curr >= 1500)
				return TYPEC_CC_RP_1_5;
			return TYPEC_CC_RP_DEF;
		}
	}

	return TYPEC_CC_RP_DEF;
}

static void tcpm_ams_finish(struct tcpm_port *port)
{
	tcpm_log(port, "AMS %s finished", tcpm_ams_str[port->ams]);

	if (port->pd_capable && port->pwr_role == TYPEC_SOURCE) {
		if (port->negotiated_rev >= PD_REV30)
			tcpm_set_cc(port, SINK_TX_OK);
		else
			tcpm_set_cc(port, SINK_TX_NG);
	} else if (port->pwr_role == TYPEC_SOURCE) {
		tcpm_set_cc(port, tcpm_rp_cc(port));
	}

	port->in_ams = false;
	port->ams = NONE_AMS;
}

static int tcpm_pd_transmit(struct tcpm_port *port,
			    enum tcpm_transmit_type type,
			    const struct pd_message *msg)
{
	unsigned long timeout;
	int ret;

	if (msg)
		tcpm_log(port, "PD TX, header: %#x", le16_to_cpu(msg->header));
	else
		tcpm_log(port, "PD TX, type: %#x", type);

	reinit_completion(&port->tx_complete);
	ret = port->tcpc->pd_transmit(port->tcpc, type, msg, port->negotiated_rev);
	if (ret < 0)
		return ret;

	mutex_unlock(&port->lock);
	timeout = wait_for_completion_timeout(&port->tx_complete,
				msecs_to_jiffies(PD_T_TCPC_TX_TIMEOUT));
	mutex_lock(&port->lock);
	if (!timeout)
		return -ETIMEDOUT;

	switch (port->tx_status) {
	case TCPC_TX_SUCCESS:
		port->message_id = (port->message_id + 1) & PD_HEADER_ID_MASK;
		/*
		 * USB PD rev 2.0, 8.3.2.2.1:
		 * USB PD rev 3.0, 8.3.2.1.3:
		 * "... Note that every AMS is Interruptible until the first
		 * Message in the sequence has been successfully sent (GoodCRC
		 * Message received)."
		 */
		if (port->ams != NONE_AMS)
			port->in_ams = true;
		break;
	case TCPC_TX_DISCARDED:
		ret = -EAGAIN;
		break;
	case TCPC_TX_FAILED:
	default:
		ret = -EIO;
		break;
	}

	/* Some AMS don't expect responses. Finish them here. */
	if (port->ams == ATTENTION || port->ams == SOURCE_ALERT)
		tcpm_ams_finish(port);

	return ret;
}

void tcpm_pd_transmit_complete(struct tcpm_port *port,
			       enum tcpm_transmit_status status)
{
	tcpm_log(port, "PD TX complete, status: %u", status);
	port->tx_status = status;
	complete(&port->tx_complete);
}
EXPORT_SYMBOL_GPL(tcpm_pd_transmit_complete);

static int tcpm_mux_set(struct tcpm_port *port, int state,
			enum usb_role usb_role,
			enum typec_orientation orientation)
{
	int ret;

	tcpm_log(port, "Requesting mux state %d, usb-role %d, orientation %d",
		 state, usb_role, orientation);

	ret = typec_set_orientation(port->typec_port, orientation);
	if (ret)
		return ret;

	if (port->role_sw) {
		ret = usb_role_switch_set_role(port->role_sw, usb_role);
		if (ret)
			return ret;
	}

	return typec_set_mode(port->typec_port, state);
}

static int tcpm_set_polarity(struct tcpm_port *port,
			     enum typec_cc_polarity polarity)
{
	int ret;

	tcpm_log(port, "polarity %d", polarity);

	ret = port->tcpc->set_polarity(port->tcpc, polarity);
	if (ret < 0)
		return ret;

	port->polarity = polarity;

	return 0;
}

static int tcpm_set_vconn(struct tcpm_port *port, bool enable)
{
	int ret;

	tcpm_log(port, "vconn:=%d", enable);

	ret = port->tcpc->set_vconn(port->tcpc, enable);
	if (!ret) {
		port->vconn_role = enable ? TYPEC_SOURCE : TYPEC_SINK;
		typec_set_vconn_role(port->typec_port, port->vconn_role);
	}

	return ret;
}

static u32 tcpm_get_current_limit(struct tcpm_port *port)
{
	enum typec_cc_status cc;
	u32 limit;

	cc = port->polarity ? port->cc2 : port->cc1;
	switch (cc) {
	case TYPEC_CC_RP_1_5:
		limit = 1500;
		break;
	case TYPEC_CC_RP_3_0:
		limit = 3000;
		break;
	case TYPEC_CC_RP_DEF:
	default:
		if (port->tcpc->get_current_limit)
			limit = port->tcpc->get_current_limit(port->tcpc);
		else
			limit = 0;
		break;
	}

	return limit;
}

static int tcpm_set_current_limit(struct tcpm_port *port, u32 max_ma, u32 mv)
{
	int ret = -EOPNOTSUPP;

	tcpm_log(port, "Setting voltage/current limit %u mV %u mA", mv, max_ma);

	port->supply_voltage = mv;
	port->current_limit = max_ma;
	power_supply_changed(port->psy);

	if (port->tcpc->set_current_limit)
		ret = port->tcpc->set_current_limit(port->tcpc, max_ma, mv);

	return ret;
}

static int tcpm_set_attached_state(struct tcpm_port *port, bool attached)
{
	return port->tcpc->set_roles(port->tcpc, attached, port->pwr_role,
				     port->data_role);
}

static int tcpm_set_roles(struct tcpm_port *port, bool attached,
			  enum typec_role role, enum typec_data_role data)
{
	enum typec_orientation orientation;
	enum usb_role usb_role;
	int ret;

	if (port->polarity == TYPEC_POLARITY_CC1)
		orientation = TYPEC_ORIENTATION_NORMAL;
	else
		orientation = TYPEC_ORIENTATION_REVERSE;

	if (port->typec_caps.data == TYPEC_PORT_DRD) {
		if (data == TYPEC_HOST)
			usb_role = USB_ROLE_HOST;
		else
			usb_role = USB_ROLE_DEVICE;
	} else if (port->typec_caps.data == TYPEC_PORT_DFP) {
		if (data == TYPEC_HOST) {
			if (role == TYPEC_SOURCE)
				usb_role = USB_ROLE_HOST;
			else
				usb_role = USB_ROLE_NONE;
		} else {
			return -ENOTSUPP;
		}
	} else {
		if (data == TYPEC_DEVICE) {
			if (role == TYPEC_SINK)
				usb_role = USB_ROLE_DEVICE;
			else
				usb_role = USB_ROLE_NONE;
		} else {
			return -ENOTSUPP;
		}
	}

	ret = tcpm_mux_set(port, TYPEC_STATE_USB, usb_role, orientation);
	if (ret < 0)
		return ret;

	ret = port->tcpc->set_roles(port->tcpc, attached, role, data);
	if (ret < 0)
		return ret;

	port->pwr_role = role;
	port->data_role = data;
	typec_set_data_role(port->typec_port, data);
	typec_set_pwr_role(port->typec_port, role);

	return 0;
}

static int tcpm_set_pwr_role(struct tcpm_port *port, enum typec_role role)
{
	int ret;

	ret = port->tcpc->set_roles(port->tcpc, true, role,
				    port->data_role);
	if (ret < 0)
		return ret;

	port->pwr_role = role;
	typec_set_pwr_role(port->typec_port, role);

	return 0;
}

/*
 * Transform the PDO to be compliant to PD rev2.0.
 * Return 0 if the PDO type is not defined in PD rev2.0.
 * Otherwise, return the converted PDO.
 */
static u32 tcpm_forge_legacy_pdo(struct tcpm_port *port, u32 pdo, enum typec_role role)
{
	switch (pdo_type(pdo)) {
	case PDO_TYPE_FIXED:
		if (role == TYPEC_SINK)
			return pdo & ~PDO_FIXED_FRS_CURR_MASK;
		else
			return pdo & ~PDO_FIXED_UNCHUNK_EXT;
	case PDO_TYPE_VAR:
	case PDO_TYPE_BATT:
		return pdo;
	case PDO_TYPE_APDO:
	default:
		return 0;
	}
}

static int tcpm_pd_send_source_caps(struct tcpm_port *port)
{
	struct pd_message msg;
	u32 pdo;
	unsigned int i, nr_pdo = 0;

	memset(&msg, 0, sizeof(msg));

	for (i = 0; i < port->nr_src_pdo; i++) {
		if (port->negotiated_rev >= PD_REV30) {
			msg.payload[nr_pdo++] =	cpu_to_le32(port->src_pdo[i]);
		} else {
			pdo = tcpm_forge_legacy_pdo(port, port->src_pdo[i], TYPEC_SOURCE);
			if (pdo)
				msg.payload[nr_pdo++] = cpu_to_le32(pdo);
		}
	}

	if (!nr_pdo) {
		/* No source capabilities defined, sink only */
		msg.header = PD_HEADER_LE(PD_CTRL_REJECT,
					  port->pwr_role,
					  port->data_role,
					  port->negotiated_rev,
					  port->message_id, 0);
	} else {
		msg.header = PD_HEADER_LE(PD_DATA_SOURCE_CAP,
					  port->pwr_role,
					  port->data_role,
					  port->negotiated_rev,
					  port->message_id,
					  nr_pdo);
	}

	return tcpm_pd_transmit(port, TCPC_TX_SOP, &msg);
}

static int tcpm_pd_send_sink_caps(struct tcpm_port *port)
{
	struct pd_message msg;
	u32 pdo;
	unsigned int i, nr_pdo = 0;

	memset(&msg, 0, sizeof(msg));

	for (i = 0; i < port->nr_snk_pdo; i++) {
		if (port->negotiated_rev >= PD_REV30) {
			msg.payload[nr_pdo++] =	cpu_to_le32(port->snk_pdo[i]);
		} else {
			pdo = tcpm_forge_legacy_pdo(port, port->snk_pdo[i], TYPEC_SINK);
			if (pdo)
				msg.payload[nr_pdo++] = cpu_to_le32(pdo);
		}
	}

	if (!nr_pdo) {
		/* No sink capabilities defined, source only */
		msg.header = PD_HEADER_LE(PD_CTRL_REJECT,
					  port->pwr_role,
					  port->data_role,
					  port->negotiated_rev,
					  port->message_id, 0);
	} else {
		msg.header = PD_HEADER_LE(PD_DATA_SINK_CAP,
					  port->pwr_role,
					  port->data_role,
					  port->negotiated_rev,
					  port->message_id,
					  nr_pdo);
	}

	return tcpm_pd_transmit(port, TCPC_TX_SOP, &msg);
}

static void mod_tcpm_delayed_work(struct tcpm_port *port, unsigned int delay_ms)
{
	if (delay_ms) {
		hrtimer_start(&port->state_machine_timer, ms_to_ktime(delay_ms), HRTIMER_MODE_REL);
	} else {
		hrtimer_cancel(&port->state_machine_timer);
		kthread_queue_work(port->wq, &port->state_machine);
	}
}

static void mod_vdm_delayed_work(struct tcpm_port *port, unsigned int delay_ms)
{
	if (delay_ms) {
		hrtimer_start(&port->vdm_state_machine_timer, ms_to_ktime(delay_ms),
			      HRTIMER_MODE_REL);
	} else {
		hrtimer_cancel(&port->vdm_state_machine_timer);
		kthread_queue_work(port->wq, &port->vdm_state_machine);
	}
}

static void mod_enable_frs_delayed_work(struct tcpm_port *port, unsigned int delay_ms)
{
	if (delay_ms) {
		hrtimer_start(&port->enable_frs_timer, ms_to_ktime(delay_ms), HRTIMER_MODE_REL);
	} else {
		hrtimer_cancel(&port->enable_frs_timer);
		kthread_queue_work(port->wq, &port->enable_frs);
	}
}

static void mod_send_discover_delayed_work(struct tcpm_port *port, unsigned int delay_ms)
{
	if (delay_ms) {
		hrtimer_start(&port->send_discover_timer, ms_to_ktime(delay_ms), HRTIMER_MODE_REL);
	} else {
		hrtimer_cancel(&port->send_discover_timer);
		kthread_queue_work(port->wq, &port->send_discover_work);
	}
}

static void tcpm_set_state(struct tcpm_port *port, enum tcpm_state state,
			   unsigned int delay_ms)
{
	if (delay_ms) {
		tcpm_log(port, "pending state change %s -> %s @ %u ms [%s %s]",
			 tcpm_states[port->state], tcpm_states[state], delay_ms,
			 pd_rev[port->negotiated_rev], tcpm_ams_str[port->ams]);
		port->delayed_state = state;
		mod_tcpm_delayed_work(port, delay_ms);
		port->delayed_runtime = ktime_add(ktime_get(), ms_to_ktime(delay_ms));
		port->delay_ms = delay_ms;
	} else {
		tcpm_log(port, "state change %s -> %s [%s %s]",
			 tcpm_states[port->state], tcpm_states[state],
			 pd_rev[port->negotiated_rev], tcpm_ams_str[port->ams]);
		port->delayed_state = INVALID_STATE;
		port->prev_state = port->state;
		port->state = state;
		/*
		 * Don't re-queue the state machine work item if we're currently
		 * in the state machine and we're immediately changing states.
		 * tcpm_state_machine_work() will continue running the state
		 * machine.
		 */
		if (!port->state_machine_running)
			mod_tcpm_delayed_work(port, 0);
	}
}

static void tcpm_set_state_cond(struct tcpm_port *port, enum tcpm_state state,
				unsigned int delay_ms)
{
	if (port->enter_state == port->state)
		tcpm_set_state(port, state, delay_ms);
	else
		tcpm_log(port,
			 "skipped %sstate change %s -> %s [%u ms], context state %s [%s %s]",
			 delay_ms ? "delayed " : "",
			 tcpm_states[port->state], tcpm_states[state],
			 delay_ms, tcpm_states[port->enter_state],
			 pd_rev[port->negotiated_rev], tcpm_ams_str[port->ams]);
}

static void tcpm_queue_message(struct tcpm_port *port,
			       enum pd_msg_request message)
{
	port->queued_message = message;
	mod_tcpm_delayed_work(port, 0);
}

static bool tcpm_vdm_ams(struct tcpm_port *port)
{
	switch (port->ams) {
	case DISCOVER_IDENTITY:
	case SOURCE_STARTUP_CABLE_PLUG_DISCOVER_IDENTITY:
	case DISCOVER_SVIDS:
	case DISCOVER_MODES:
	case DFP_TO_UFP_ENTER_MODE:
	case DFP_TO_UFP_EXIT_MODE:
	case DFP_TO_CABLE_PLUG_ENTER_MODE:
	case DFP_TO_CABLE_PLUG_EXIT_MODE:
	case ATTENTION:
	case UNSTRUCTURED_VDMS:
	case STRUCTURED_VDMS:
		break;
	default:
		return false;
	}

	return true;
}

static bool tcpm_ams_interruptible(struct tcpm_port *port)
{
	switch (port->ams) {
	/* Interruptible AMS */
	case NONE_AMS:
	case SECURITY:
	case FIRMWARE_UPDATE:
	case DISCOVER_IDENTITY:
	case SOURCE_STARTUP_CABLE_PLUG_DISCOVER_IDENTITY:
	case DISCOVER_SVIDS:
	case DISCOVER_MODES:
	case DFP_TO_UFP_ENTER_MODE:
	case DFP_TO_UFP_EXIT_MODE:
	case DFP_TO_CABLE_PLUG_ENTER_MODE:
	case DFP_TO_CABLE_PLUG_EXIT_MODE:
	case UNSTRUCTURED_VDMS:
	case STRUCTURED_VDMS:
	case COUNTRY_INFO:
	case COUNTRY_CODES:
		break;
	/* Non-Interruptible AMS */
	default:
		if (port->in_ams)
			return false;
		break;
	}

	return true;
}

static int tcpm_ams_start(struct tcpm_port *port, enum tcpm_ams ams)
{
	int ret = 0;

	tcpm_log(port, "AMS %s start", tcpm_ams_str[ams]);

	if (!tcpm_ams_interruptible(port) &&
	    !(ams == HARD_RESET || ams == SOFT_RESET_AMS)) {
		port->upcoming_state = INVALID_STATE;
		tcpm_log(port, "AMS %s not interruptible, aborting",
			 tcpm_ams_str[port->ams]);
		return -EAGAIN;
	}

	if (port->pwr_role == TYPEC_SOURCE) {
		enum typec_cc_status cc_req = port->cc_req;

		port->ams = ams;

		if (ams == HARD_RESET) {
			tcpm_set_cc(port, tcpm_rp_cc(port));
			tcpm_pd_transmit(port, TCPC_TX_HARD_RESET, NULL);
			tcpm_set_state(port, HARD_RESET_START, 0);
			return ret;
		} else if (ams == SOFT_RESET_AMS) {
			if (!port->explicit_contract)
				tcpm_set_cc(port, tcpm_rp_cc(port));
			tcpm_set_state(port, SOFT_RESET_SEND, 0);
			return ret;
		} else if (tcpm_vdm_ams(port)) {
			/* tSinkTx is enforced in vdm_run_state_machine */
			if (port->negotiated_rev >= PD_REV30)
				tcpm_set_cc(port, SINK_TX_NG);
			return ret;
		}

		if (port->negotiated_rev >= PD_REV30)
			tcpm_set_cc(port, SINK_TX_NG);

		switch (port->state) {
		case SRC_READY:
		case SRC_STARTUP:
		case SRC_SOFT_RESET_WAIT_SNK_TX:
		case SOFT_RESET:
		case SOFT_RESET_SEND:
			if (port->negotiated_rev >= PD_REV30)
				tcpm_set_state(port, AMS_START,
					       cc_req == SINK_TX_OK ?
					       PD_T_SINK_TX : 0);
			else
				tcpm_set_state(port, AMS_START, 0);
			break;
		default:
			if (port->negotiated_rev >= PD_REV30)
				tcpm_set_state(port, SRC_READY,
					       cc_req == SINK_TX_OK ?
					       PD_T_SINK_TX : 0);
			else
				tcpm_set_state(port, SRC_READY, 0);
			break;
		}
	} else {
		if (port->negotiated_rev >= PD_REV30 &&
		    !tcpm_sink_tx_ok(port) &&
		    ams != SOFT_RESET_AMS &&
		    ams != HARD_RESET) {
			port->upcoming_state = INVALID_STATE;
			tcpm_log(port, "Sink TX No Go");
			return -EAGAIN;
		}

		port->ams = ams;

		if (ams == HARD_RESET) {
			tcpm_pd_transmit(port, TCPC_TX_HARD_RESET, NULL);
			tcpm_set_state(port, HARD_RESET_START, 0);
			return ret;
		} else if (tcpm_vdm_ams(port)) {
			return ret;
		}

		if (port->state == SNK_READY ||
		    port->state == SNK_SOFT_RESET)
			tcpm_set_state(port, AMS_START, 0);
		else
			tcpm_set_state(port, SNK_READY, 0);
	}

	return ret;
}

/*
 * VDM/VDO handling functions
 */
static void tcpm_queue_vdm(struct tcpm_port *port, const u32 header,
			   const u32 *data, int cnt)
{
	u32 vdo_hdr = port->vdo_data[0];

	WARN_ON(!mutex_is_locked(&port->lock));

	/* If is sending discover_identity, handle received message first */
	if (PD_VDO_SVDM(vdo_hdr) && PD_VDO_CMD(vdo_hdr) == CMD_DISCOVER_IDENT) {
		port->send_discover = true;
		mod_send_discover_delayed_work(port, SEND_DISCOVER_RETRY_MS);
	} else {
		/* Make sure we are not still processing a previous VDM packet */
		WARN_ON(port->vdm_state > VDM_STATE_DONE);
	}

	port->vdo_count = cnt + 1;
	port->vdo_data[0] = header;
	memcpy(&port->vdo_data[1], data, sizeof(u32) * cnt);
	/* Set ready, vdm state machine will actually send */
	port->vdm_retries = 0;
	port->vdm_state = VDM_STATE_READY;
	port->vdm_sm_running = true;

	mod_vdm_delayed_work(port, 0);
}

static void tcpm_queue_vdm_unlocked(struct tcpm_port *port, const u32 header,
				    const u32 *data, int cnt)
{
	if (port->state != SRC_READY && port->state != SNK_READY)
		return;

	mutex_lock(&port->lock);
	tcpm_queue_vdm(port, header, data, cnt);
	mutex_unlock(&port->lock);
}

static void svdm_consume_identity(struct tcpm_port *port, const u32 *p, int cnt)
{
	u32 vdo = p[VDO_INDEX_IDH];
	u32 product = p[VDO_INDEX_PRODUCT];

	memset(&port->mode_data, 0, sizeof(port->mode_data));

	port->partner_ident.id_header = vdo;
	port->partner_ident.cert_stat = p[VDO_INDEX_CSTAT];
	port->partner_ident.product = product;

	if (port->partner)
		typec_partner_set_identity(port->partner);

	tcpm_log(port, "Identity: %04x:%04x.%04x",
		 PD_IDH_VID(vdo),
		 PD_PRODUCT_PID(product), product & 0xffff);
}

static bool svdm_consume_svids(struct tcpm_port *port, const u32 *p, int cnt)
{
	struct pd_mode_data *pmdata = &port->mode_data;
	int i;

	for (i = 1; i < cnt; i++) {
		u16 svid;

		svid = (p[i] >> 16) & 0xffff;
		if (!svid)
			return false;

		if (pmdata->nsvids >= SVID_DISCOVERY_MAX)
			goto abort;

		pmdata->svids[pmdata->nsvids++] = svid;
		tcpm_log(port, "SVID %d: 0x%x", pmdata->nsvids, svid);

		svid = p[i] & 0xffff;
		if (!svid)
			return false;

		if (pmdata->nsvids >= SVID_DISCOVERY_MAX)
			goto abort;

		pmdata->svids[pmdata->nsvids++] = svid;
		tcpm_log(port, "SVID %d: 0x%x", pmdata->nsvids, svid);
	}

	/*
	 * PD3.0 Spec 6.4.4.3.2: The SVIDs are returned 2 per VDO (see Table
	 * 6-43), and can be returned maximum 6 VDOs per response (see Figure
	 * 6-19). If the Respondersupports 12 or more SVID then the Discover
	 * SVIDs Command Shall be executed multiple times until a Discover
	 * SVIDs VDO is returned ending either with a SVID value of 0x0000 in
	 * the last part of the last VDO or with a VDO containing two SVIDs
	 * with values of 0x0000.
	 *
	 * However, some odd dockers support SVIDs less than 12 but without
	 * 0x0000 in the last VDO, so we need to break the Discover SVIDs
	 * request and return false here.
	 */
	return cnt == 7;
abort:
	tcpm_log(port, "SVID_DISCOVERY_MAX(%d) too low!", SVID_DISCOVERY_MAX);
	return false;
}

static void svdm_consume_modes(struct tcpm_port *port, const u32 *p, int cnt)
{
	struct pd_mode_data *pmdata = &port->mode_data;
	struct typec_altmode_desc *paltmode;
	int i;

	if (pmdata->altmodes >= ARRAY_SIZE(port->partner_altmode)) {
		/* Already logged in svdm_consume_svids() */
		return;
	}

	for (i = 1; i < cnt; i++) {
		paltmode = &pmdata->altmode_desc[pmdata->altmodes];
		memset(paltmode, 0, sizeof(*paltmode));

		paltmode->svid = pmdata->svids[pmdata->svid_index];
		paltmode->mode = i;
		paltmode->vdo = p[i];

		tcpm_log(port, " Alternate mode %d: SVID 0x%04x, VDO %d: 0x%08x",
			 pmdata->altmodes, paltmode->svid,
			 paltmode->mode, paltmode->vdo);

		pmdata->altmodes++;
	}
}

static void tcpm_register_partner_altmodes(struct tcpm_port *port)
{
	struct pd_mode_data *modep = &port->mode_data;
	struct typec_altmode *altmode;
	int i;

	if (!port->partner)
		return;

	for (i = 0; i < modep->altmodes; i++) {
		altmode = typec_partner_register_altmode(port->partner,
						&modep->altmode_desc[i]);
		if (IS_ERR(altmode)) {
			tcpm_log(port, "Failed to register partner SVID 0x%04x",
				 modep->altmode_desc[i].svid);
			altmode = NULL;
		}
		port->partner_altmode[i] = altmode;
	}
}

#define supports_modal(port)	PD_IDH_MODAL_SUPP((port)->partner_ident.id_header)

static int tcpm_pd_svdm(struct tcpm_port *port, struct typec_altmode *adev,
			const u32 *p, int cnt, u32 *response,
			enum adev_actions *adev_action)
{
	struct typec_port *typec = port->typec_port;
	struct typec_altmode *pdev;
	struct pd_mode_data *modep;
	int svdm_version;
	int rlen = 0;
	int cmd_type;
	int cmd;
	int i;

	cmd_type = PD_VDO_CMDT(p[0]);
	cmd = PD_VDO_CMD(p[0]);

	tcpm_log(port, "Rx VDM cmd 0x%x type %d cmd %d len %d",
		 p[0], cmd_type, cmd, cnt);

	modep = &port->mode_data;

	pdev = typec_match_altmode(port->partner_altmode, ALTMODE_DISCOVERY_MAX,
				   PD_VDO_VID(p[0]), PD_VDO_OPOS(p[0]));

	svdm_version = typec_get_negotiated_svdm_version(typec);
	if (svdm_version < 0)
		return 0;

	switch (cmd_type) {
	case CMDT_INIT:
		switch (cmd) {
		case CMD_DISCOVER_IDENT:
			if (PD_VDO_VID(p[0]) != USB_SID_PD)
				break;

			if (IS_ERR_OR_NULL(port->partner))
				break;

			if (PD_VDO_SVDM_VER(p[0]) < svdm_version) {
				typec_partner_set_svdm_version(port->partner,
							       PD_VDO_SVDM_VER(p[0]));
				svdm_version = PD_VDO_SVDM_VER(p[0]);
			}

			port->ams = DISCOVER_IDENTITY;
			/*
			 * PD2.0 Spec 6.10.3: respond with NAK as DFP (data host)
			 * PD3.1 Spec 6.4.4.2.5.1: respond with NAK if "invalid field" or
			 * "wrong configuation" or "Unrecognized"
			 */
			if ((port->data_role == TYPEC_DEVICE || svdm_version >= SVDM_VER_2_0) &&
			    port->nr_snk_vdo) {
				if (svdm_version < SVDM_VER_2_0) {
					for (i = 0; i < port->nr_snk_vdo_v1; i++)
						response[i + 1] = port->snk_vdo_v1[i];
					rlen = port->nr_snk_vdo_v1 + 1;

				} else {
					for (i = 0; i < port->nr_snk_vdo; i++)
						response[i + 1] = port->snk_vdo[i];
					rlen = port->nr_snk_vdo + 1;
				}
			}
			break;
		case CMD_DISCOVER_SVID:
			port->ams = DISCOVER_SVIDS;
			break;
		case CMD_DISCOVER_MODES:
			port->ams = DISCOVER_MODES;
			break;
		case CMD_ENTER_MODE:
			port->ams = DFP_TO_UFP_ENTER_MODE;
			break;
		case CMD_EXIT_MODE:
			port->ams = DFP_TO_UFP_EXIT_MODE;
			break;
		case CMD_ATTENTION:
			/* Attention command does not have response */
			*adev_action = ADEV_ATTENTION;
			return 0;
		default:
			break;
		}
		if (rlen >= 1) {
			response[0] = p[0] | VDO_CMDT(CMDT_RSP_ACK);
		} else if (rlen == 0) {
			response[0] = p[0] | VDO_CMDT(CMDT_RSP_NAK);
			rlen = 1;
		} else {
			response[0] = p[0] | VDO_CMDT(CMDT_RSP_BUSY);
			rlen = 1;
		}
		response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) |
			      (VDO_SVDM_VERS(typec_get_negotiated_svdm_version(typec)));
		break;
	case CMDT_RSP_ACK:
		/* silently drop message if we are not connected */
		if (IS_ERR_OR_NULL(port->partner))
			break;

		tcpm_ams_finish(port);

		switch (cmd) {
		case CMD_DISCOVER_IDENT:
			if (PD_VDO_SVDM_VER(p[0]) < svdm_version)
				typec_partner_set_svdm_version(port->partner,
							       PD_VDO_SVDM_VER(p[0]));
			/* 6.4.4.3.1 */
			svdm_consume_identity(port, p, cnt);
			response[0] = VDO(USB_SID_PD, 1, typec_get_negotiated_svdm_version(typec),
					  CMD_DISCOVER_SVID);
			rlen = 1;
			break;
		case CMD_DISCOVER_SVID:
			/* 6.4.4.3.2 */
			if (svdm_consume_svids(port, p, cnt)) {
				response[0] = VDO(USB_SID_PD, 1, svdm_version, CMD_DISCOVER_SVID);
				rlen = 1;
			} else if (modep->nsvids && supports_modal(port)) {
				response[0] = VDO(modep->svids[0], 1, svdm_version,
						  CMD_DISCOVER_MODES);
				rlen = 1;
			}
			break;
		case CMD_DISCOVER_MODES:
			/* 6.4.4.3.3 */
			svdm_consume_modes(port, p, cnt);
			modep->svid_index++;
			if (modep->svid_index < modep->nsvids) {
				u16 svid = modep->svids[modep->svid_index];
				response[0] = VDO(svid, 1, svdm_version, CMD_DISCOVER_MODES);
				rlen = 1;
			} else {
				tcpm_register_partner_altmodes(port);
			}
			break;
		case CMD_ENTER_MODE:
			if (adev && pdev) {
				typec_altmode_update_active(pdev, true);
				*adev_action = ADEV_QUEUE_VDM_SEND_EXIT_MODE_ON_FAIL;
			}
			return 0;
		case CMD_EXIT_MODE:
			if (adev && pdev) {
				typec_altmode_update_active(pdev, false);
				/* Back to USB Operation */
				*adev_action = ADEV_NOTIFY_USB_AND_QUEUE_VDM;
				return 0;
			}
			break;
		case VDO_CMD_VENDOR(0) ... VDO_CMD_VENDOR(15):
			break;
		default:
			/* Unrecognized SVDM */
			response[0] = p[0] | VDO_CMDT(CMDT_RSP_NAK);
			rlen = 1;
			response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) |
				      (VDO_SVDM_VERS(svdm_version));
			break;
		}
		break;
	case CMDT_RSP_NAK:
		tcpm_ams_finish(port);
		switch (cmd) {
		case CMD_DISCOVER_IDENT:
		case CMD_DISCOVER_SVID:
		case CMD_DISCOVER_MODES:
		case VDO_CMD_VENDOR(0) ... VDO_CMD_VENDOR(15):
			break;
		case CMD_ENTER_MODE:
			/* Back to USB Operation */
			*adev_action = ADEV_NOTIFY_USB_AND_QUEUE_VDM;
			return 0;
		default:
			/* Unrecognized SVDM */
			response[0] = p[0] | VDO_CMDT(CMDT_RSP_NAK);
			rlen = 1;
			response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) |
				      (VDO_SVDM_VERS(svdm_version));
			break;
		}
		break;
	default:
		response[0] = p[0] | VDO_CMDT(CMDT_RSP_NAK);
		rlen = 1;
		response[0] = (response[0] & ~VDO_SVDM_VERS_MASK) |
			      (VDO_SVDM_VERS(svdm_version));
		break;
	}

	/* Informing the alternate mode drivers about everything */
	*adev_action = ADEV_QUEUE_VDM;
	return rlen;
}

static void tcpm_pd_handle_msg(struct tcpm_port *port,
			       enum pd_msg_request message,
			       enum tcpm_ams ams);

static void tcpm_handle_vdm_request(struct tcpm_port *port,
				    const __le32 *payload, int cnt)
{
	enum adev_actions adev_action = ADEV_NONE;
	struct typec_altmode *adev;
	u32 p[PD_MAX_PAYLOAD];
	u32 response[8] = { };
	int i, rlen = 0;

	for (i = 0; i < cnt; i++)
		p[i] = le32_to_cpu(payload[i]);

	adev = typec_match_altmode(port->port_altmode, ALTMODE_DISCOVERY_MAX,
				   PD_VDO_VID(p[0]), PD_VDO_OPOS(p[0]));

	if (port->vdm_state == VDM_STATE_BUSY) {
		/* If UFP responded busy retry after timeout */
		if (PD_VDO_CMDT(p[0]) == CMDT_RSP_BUSY) {
			port->vdm_state = VDM_STATE_WAIT_RSP_BUSY;
			port->vdo_retry = (p[0] & ~VDO_CMDT_MASK) |
				CMDT_INIT;
			mod_vdm_delayed_work(port, PD_T_VDM_BUSY);
			return;
		}
		port->vdm_state = VDM_STATE_DONE;
	}

	if (PD_VDO_SVDM(p[0]) && (adev || tcpm_vdm_ams(port) || port->nr_snk_vdo)) {
		/*
		 * Here a SVDM is received (INIT or RSP or unknown). Set the vdm_sm_running in
		 * advance because we are dropping the lock but may send VDMs soon.
		 * For the cases of INIT received:
		 *  - If no response to send, it will be cleared later in this function.
		 *  - If there are responses to send, it will be cleared in the state machine.
		 * For the cases of RSP received:
		 *  - If no further INIT to send, it will be cleared later in this function.
		 *  - Otherwise, it will be cleared in the state machine if timeout or it will go
		 *    back here until no further INIT to send.
		 * For the cases of unknown type received:
		 *  - We will send NAK and the flag will be cleared in the state machine.
		 */
		port->vdm_sm_running = true;
		rlen = tcpm_pd_svdm(port, adev, p, cnt, response, &adev_action);
	} else {
		if (port->negotiated_rev >= PD_REV30)
			tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
	}

	/*
	 * We are done with any state stored in the port struct now, except
	 * for any port struct changes done by the tcpm_queue_vdm() call
	 * below, which is a separate operation.
	 *
	 * So we can safely release the lock here; and we MUST release the
	 * lock here to avoid an AB BA lock inversion:
	 *
	 * If we keep the lock here then the lock ordering in this path is:
	 * 1. tcpm_pd_rx_handler take the tcpm port lock
	 * 2. One of the typec_altmode_* calls below takes the alt-mode's lock
	 *
	 * And we also have this ordering:
	 * 1. alt-mode driver takes the alt-mode's lock
	 * 2. alt-mode driver calls tcpm_altmode_enter which takes the
	 *    tcpm port lock
	 *
	 * Dropping our lock here avoids this.
	 */
	mutex_unlock(&port->lock);

	if (adev) {
		switch (adev_action) {
		case ADEV_NONE:
			break;
		case ADEV_NOTIFY_USB_AND_QUEUE_VDM:
			WARN_ON(typec_altmode_notify(adev, TYPEC_STATE_USB, NULL));
			typec_altmode_vdm(adev, p[0], &p[1], cnt);
			break;
		case ADEV_QUEUE_VDM:
			typec_altmode_vdm(adev, p[0], &p[1], cnt);
			break;
		case ADEV_QUEUE_VDM_SEND_EXIT_MODE_ON_FAIL:
			if (typec_altmode_vdm(adev, p[0], &p[1], cnt)) {
				int svdm_version = typec_get_negotiated_svdm_version(
									port->typec_port);
				if (svdm_version < 0)
					break;

				response[0] = VDO(adev->svid, 1, svdm_version,
						  CMD_EXIT_MODE);
				response[0] |= VDO_OPOS(adev->mode);
				rlen = 1;
			}
			break;
		case ADEV_ATTENTION:
			typec_altmode_attention(adev, p[1]);
			break;
		}
	}

	/*
	 * We must re-take the lock here to balance the unlock in
	 * tcpm_pd_rx_handler, note that no changes, other then the
	 * tcpm_queue_vdm call, are made while the lock is held again.
	 * All that is done after the call is unwinding the call stack until
	 * we return to tcpm_pd_rx_handler and do the unlock there.
	 */
	mutex_lock(&port->lock);

	if (rlen > 0)
		tcpm_queue_vdm(port, response[0], &response[1], rlen - 1);
	else
		port->vdm_sm_running = false;
}

static void tcpm_send_vdm(struct tcpm_port *port, u32 vid, int cmd,
			  const u32 *data, int count)
{
	int svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
	u32 header;

	if (svdm_version < 0)
		return;

	if (WARN_ON(count > VDO_MAX_SIZE - 1))
		count = VDO_MAX_SIZE - 1;

	/* set VDM header with VID & CMD */
	header = VDO(vid, ((vid & USB_SID_PD) == USB_SID_PD) ?
			1 : (PD_VDO_CMD(cmd) <= CMD_ATTENTION),
			svdm_version, cmd);
	tcpm_queue_vdm(port, header, data, count);
}

static unsigned int vdm_ready_timeout(u32 vdm_hdr)
{
	unsigned int timeout;
	int cmd = PD_VDO_CMD(vdm_hdr);

	/* its not a structured VDM command */
	if (!PD_VDO_SVDM(vdm_hdr))
		return PD_T_VDM_UNSTRUCTURED;

	switch (PD_VDO_CMDT(vdm_hdr)) {
	case CMDT_INIT:
		if (cmd == CMD_ENTER_MODE || cmd == CMD_EXIT_MODE)
			timeout = PD_T_VDM_WAIT_MODE_E;
		else
			timeout = PD_T_VDM_SNDR_RSP;
		break;
	default:
		if (cmd == CMD_ENTER_MODE || cmd == CMD_EXIT_MODE)
			timeout = PD_T_VDM_E_MODE;
		else
			timeout = PD_T_VDM_RCVR_RSP;
		break;
	}
	return timeout;
}

static void vdm_run_state_machine(struct tcpm_port *port)
{
	struct pd_message msg;
	int i, res = 0;
	u32 vdo_hdr = port->vdo_data[0];

	switch (port->vdm_state) {
	case VDM_STATE_READY:
		/* Only transmit VDM if attached */
		if (!port->attached) {
			port->vdm_state = VDM_STATE_ERR_BUSY;
			break;
		}

		/*
		 * if there's traffic or we're not in PDO ready state don't send
		 * a VDM.
		 */
		if (port->state != SRC_READY && port->state != SNK_READY) {
			port->vdm_sm_running = false;
			break;
		}

		/* TODO: AMS operation for Unstructured VDM */
		if (PD_VDO_SVDM(vdo_hdr) && PD_VDO_CMDT(vdo_hdr) == CMDT_INIT) {
			switch (PD_VDO_CMD(vdo_hdr)) {
			case CMD_DISCOVER_IDENT:
				res = tcpm_ams_start(port, DISCOVER_IDENTITY);
				if (res == 0) {
					port->send_discover = false;
				} else if (res == -EAGAIN) {
					port->vdo_data[0] = 0;
					mod_send_discover_delayed_work(port,
								       SEND_DISCOVER_RETRY_MS);
				}
				break;
			case CMD_DISCOVER_SVID:
				res = tcpm_ams_start(port, DISCOVER_SVIDS);
				break;
			case CMD_DISCOVER_MODES:
				res = tcpm_ams_start(port, DISCOVER_MODES);
				break;
			case CMD_ENTER_MODE:
				res = tcpm_ams_start(port, DFP_TO_UFP_ENTER_MODE);
				break;
			case CMD_EXIT_MODE:
				res = tcpm_ams_start(port, DFP_TO_UFP_EXIT_MODE);
				break;
			case CMD_ATTENTION:
				res = tcpm_ams_start(port, ATTENTION);
				break;
			case VDO_CMD_VENDOR(0) ... VDO_CMD_VENDOR(15):
				res = tcpm_ams_start(port, STRUCTURED_VDMS);
				break;
			default:
				res = -EOPNOTSUPP;
				break;
			}

			if (res < 0) {
				port->vdm_state = VDM_STATE_ERR_BUSY;
				return;
			}
		}

		port->vdm_state = VDM_STATE_SEND_MESSAGE;
		mod_vdm_delayed_work(port, (port->negotiated_rev >= PD_REV30 &&
					    port->pwr_role == TYPEC_SOURCE &&
					    PD_VDO_SVDM(vdo_hdr) &&
					    PD_VDO_CMDT(vdo_hdr) == CMDT_INIT) ?
					   PD_T_SINK_TX : 0);
		break;
	case VDM_STATE_WAIT_RSP_BUSY:
		port->vdo_data[0] = port->vdo_retry;
		port->vdo_count = 1;
		port->vdm_state = VDM_STATE_READY;
		tcpm_ams_finish(port);
		break;
	case VDM_STATE_BUSY:
		port->vdm_state = VDM_STATE_ERR_TMOUT;
		if (port->ams != NONE_AMS)
			tcpm_ams_finish(port);
		break;
	case VDM_STATE_ERR_SEND:
		/*
		 * A partner which does not support USB PD will not reply,
		 * so this is not a fatal error. At the same time, some
		 * devices may not return GoodCRC under some circumstances,
		 * so we need to retry.
		 */
		if (port->vdm_retries < 3) {
			tcpm_log(port, "VDM Tx error, retry");
			port->vdm_retries++;
			port->vdm_state = VDM_STATE_READY;
			if (PD_VDO_SVDM(vdo_hdr) && PD_VDO_CMDT(vdo_hdr) == CMDT_INIT)
				tcpm_ams_finish(port);
		} else {
			tcpm_ams_finish(port);
		}
		break;
	case VDM_STATE_SEND_MESSAGE:
		/* Prepare and send VDM */
		memset(&msg, 0, sizeof(msg));
		msg.header = PD_HEADER_LE(PD_DATA_VENDOR_DEF,
					  port->pwr_role,
					  port->data_role,
					  port->negotiated_rev,
					  port->message_id, port->vdo_count);
		for (i = 0; i < port->vdo_count; i++)
			msg.payload[i] = cpu_to_le32(port->vdo_data[i]);
		res = tcpm_pd_transmit(port, TCPC_TX_SOP, &msg);
		if (res < 0) {
			port->vdm_state = VDM_STATE_ERR_SEND;
		} else {
			unsigned long timeout;

			port->vdm_retries = 0;
			port->vdo_data[0] = 0;
			port->vdm_state = VDM_STATE_BUSY;
			timeout = vdm_ready_timeout(vdo_hdr);
			mod_vdm_delayed_work(port, timeout);
		}
		break;
	default:
		break;
	}
}

static void vdm_state_machine_work(struct kthread_work *work)
{
	struct tcpm_port *port = container_of(work, struct tcpm_port, vdm_state_machine);
	enum vdm_states prev_state;

	mutex_lock(&port->lock);

	/*
	 * Continue running as long as the port is not busy and there was
	 * a state change.
	 */
	do {
		prev_state = port->vdm_state;
		vdm_run_state_machine(port);
	} while (port->vdm_state != prev_state &&
		 port->vdm_state != VDM_STATE_BUSY &&
		 port->vdm_state != VDM_STATE_SEND_MESSAGE);

	if (port->vdm_state < VDM_STATE_READY)
		port->vdm_sm_running = false;

	mutex_unlock(&port->lock);
}

enum pdo_err {
	PDO_NO_ERR,
	PDO_ERR_NO_VSAFE5V,
	PDO_ERR_VSAFE5V_NOT_FIRST,
	PDO_ERR_PDO_TYPE_NOT_IN_ORDER,
	PDO_ERR_FIXED_NOT_SORTED,
	PDO_ERR_VARIABLE_BATT_NOT_SORTED,
	PDO_ERR_DUPE_PDO,
	PDO_ERR_PPS_APDO_NOT_SORTED,
	PDO_ERR_DUPE_PPS_APDO,
};

static const char * const pdo_err_msg[] = {
	[PDO_ERR_NO_VSAFE5V] =
	" err: source/sink caps should at least have vSafe5V",
	[PDO_ERR_VSAFE5V_NOT_FIRST] =
	" err: vSafe5V Fixed Supply Object Shall always be the first object",
	[PDO_ERR_PDO_TYPE_NOT_IN_ORDER] =
	" err: PDOs should be in the following order: Fixed; Battery; Variable",
	[PDO_ERR_FIXED_NOT_SORTED] =
	" err: Fixed supply pdos should be in increasing order of their fixed voltage",
	[PDO_ERR_VARIABLE_BATT_NOT_SORTED] =
	" err: Variable/Battery supply pdos should be in increasing order of their minimum voltage",
	[PDO_ERR_DUPE_PDO] =
	" err: Variable/Batt supply pdos cannot have same min/max voltage",
	[PDO_ERR_PPS_APDO_NOT_SORTED] =
	" err: Programmable power supply apdos should be in increasing order of their maximum voltage",
	[PDO_ERR_DUPE_PPS_APDO] =
	" err: Programmable power supply apdos cannot have same min/max voltage and max current",
};

static enum pdo_err tcpm_caps_err(struct tcpm_port *port, const u32 *pdo,
				  unsigned int nr_pdo)
{
	unsigned int i;

	/* Should at least contain vSafe5v */
	if (nr_pdo < 1)
		return PDO_ERR_NO_VSAFE5V;

	/* The vSafe5V Fixed Supply Object Shall always be the first object */
	if (pdo_type(pdo[0]) != PDO_TYPE_FIXED ||
	    pdo_fixed_voltage(pdo[0]) != VSAFE5V)
		return PDO_ERR_VSAFE5V_NOT_FIRST;

	for (i = 1; i < nr_pdo; i++) {
		if (pdo_type(pdo[i]) < pdo_type(pdo[i - 1])) {
			return PDO_ERR_PDO_TYPE_NOT_IN_ORDER;
		} else if (pdo_type(pdo[i]) == pdo_type(pdo[i - 1])) {
			enum pd_pdo_type type = pdo_type(pdo[i]);

			switch (type) {
			/*
			 * The remaining Fixed Supply Objects, if
			 * present, shall be sent in voltage order;
			 * lowest to highest.
			 */
			case PDO_TYPE_FIXED:
				if (pdo_fixed_voltage(pdo[i]) <=
				    pdo_fixed_voltage(pdo[i - 1]))
					return PDO_ERR_FIXED_NOT_SORTED;
				break;
			/*
			 * The Battery Supply Objects and Variable
			 * supply, if present shall be sent in Minimum
			 * Voltage order; lowest to highest.
			 */
			case PDO_TYPE_VAR:
			case PDO_TYPE_BATT:
				if (pdo_min_voltage(pdo[i]) <
				    pdo_min_voltage(pdo[i - 1]))
					return PDO_ERR_VARIABLE_BATT_NOT_SORTED;
				else if ((pdo_min_voltage(pdo[i]) ==
					  pdo_min_voltage(pdo[i - 1])) &&
					 (pdo_max_voltage(pdo[i]) ==
					  pdo_max_voltage(pdo[i - 1])))
					return PDO_ERR_DUPE_PDO;
				break;
			/*
			 * The Programmable Power Supply APDOs, if present,
			 * shall be sent in Maximum Voltage order;
			 * lowest to highest.
			 */
			case PDO_TYPE_APDO:
				if (pdo_apdo_type(pdo[i]) != APDO_TYPE_PPS)
					break;

				if (pdo_pps_apdo_max_voltage(pdo[i]) <
				    pdo_pps_apdo_max_voltage(pdo[i - 1]))
					return PDO_ERR_PPS_APDO_NOT_SORTED;
				else if (pdo_pps_apdo_min_voltage(pdo[i]) ==
					  pdo_pps_apdo_min_voltage(pdo[i - 1]) &&
					 pdo_pps_apdo_max_voltage(pdo[i]) ==
					  pdo_pps_apdo_max_voltage(pdo[i - 1]) &&
					 pdo_pps_apdo_max_current(pdo[i]) ==
					  pdo_pps_apdo_max_current(pdo[i - 1]))
					return PDO_ERR_DUPE_PPS_APDO;
				break;
			default:
				tcpm_log_force(port, " Unknown pdo type");
			}
		}
	}

	return PDO_NO_ERR;
}

static int tcpm_validate_caps(struct tcpm_port *port, const u32 *pdo,
			      unsigned int nr_pdo)
{
	enum pdo_err err_index = tcpm_caps_err(port, pdo, nr_pdo);

	if (err_index != PDO_NO_ERR) {
		tcpm_log_force(port, " %s", pdo_err_msg[err_index]);
		return -EINVAL;
	}

	return 0;
}

static int tcpm_altmode_enter(struct typec_altmode *altmode, u32 *vdo)
{
	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
	int svdm_version;
	u32 header;

	svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
	if (svdm_version < 0)
		return svdm_version;

	header = VDO(altmode->svid, vdo ? 2 : 1, svdm_version, CMD_ENTER_MODE);
	header |= VDO_OPOS(altmode->mode);

	tcpm_queue_vdm_unlocked(port, header, vdo, vdo ? 1 : 0);
	return 0;
}

static int tcpm_altmode_exit(struct typec_altmode *altmode)
{
	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);
	int svdm_version;
	u32 header;

	svdm_version = typec_get_negotiated_svdm_version(port->typec_port);
	if (svdm_version < 0)
		return svdm_version;

	header = VDO(altmode->svid, 1, svdm_version, CMD_EXIT_MODE);
	header |= VDO_OPOS(altmode->mode);

	tcpm_queue_vdm_unlocked(port, header, NULL, 0);
	return 0;
}

static int tcpm_altmode_vdm(struct typec_altmode *altmode,
			    u32 header, const u32 *data, int count)
{
	struct tcpm_port *port = typec_altmode_get_drvdata(altmode);

	tcpm_queue_vdm_unlocked(port, header, data, count - 1);

	return 0;
}

static const struct typec_altmode_ops tcpm_altmode_ops = {
	.enter = tcpm_altmode_enter,
	.exit = tcpm_altmode_exit,
	.vdm = tcpm_altmode_vdm,
};

/*
 * PD (data, control) command handling functions
 */
static inline enum tcpm_state ready_state(struct tcpm_port *port)
{
	if (port->pwr_role == TYPEC_SOURCE)
		return SRC_READY;
	else
		return SNK_READY;
}

static int tcpm_pd_send_control(struct tcpm_port *port,
				enum pd_ctrl_msg_type type);

static void tcpm_handle_alert(struct tcpm_port *port, const __le32 *payload,
			      int cnt)
{
	u32 p0 = le32_to_cpu(payload[0]);
	unsigned int type = usb_pd_ado_type(p0);

	if (!type) {
		tcpm_log(port, "Alert message received with no type");
		tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
		return;
	}

	/* Just handling non-battery alerts for now */
	if (!(type & USB_PD_ADO_TYPE_BATT_STATUS_CHANGE)) {
		if (port->pwr_role == TYPEC_SOURCE) {
			port->upcoming_state = GET_STATUS_SEND;
			tcpm_ams_start(port, GETTING_SOURCE_SINK_STATUS);
		} else {
			/*
			 * Do not check SinkTxOk here in case the Source doesn't set its Rp to
			 * SinkTxOk in time.
			 */
			port->ams = GETTING_SOURCE_SINK_STATUS;
			tcpm_set_state(port, GET_STATUS_SEND, 0);
		}
	} else {
		tcpm_queue_message(port, PD_MSG_CTRL_NOT_SUPP);
	}
}

static int tcpm_set_auto_vbus_discharge_threshold(struct tcpm_port *port,
						  enum typec_pwr_opmode mode, bool pps_active,
						  u32 requested_vbus_voltage)
{
	int ret;

	if (!port->tcpc->set_auto_vbus_discharge_threshold)
		return 0;

	ret = port->tcpc->set_auto_vbus_discharge_threshold(port->tcpc, mode, pps_active,
							    requested_vbus_voltage);
	tcpm_log_force(port,
		       "set_auto_vbus_discharge_threshold mode:%d pps_active:%c vbus:%u ret:%d",
		       mode, pps_active ? 'y' : 'n', requested_vbus_voltage, ret);

	return ret;
}

static void tcpm_pd_handle_state(struct tcpm_port *port,
				 enum tcpm_state state,
				 enum tcpm_ams ams,
				 unsigned int delay_ms)
{
	switch (port->state) {
	case SRC_READY:
	case SNK_READY:
		port->ams = ams;
		tcpm_set_state(port, state, delay_ms);
		break;
	/* 8.3.3.4.1.1 and 6.8.1 power transitioning */
	case SNK_TRANSITION_SINK:
	case SNK_TRANSITION_SINK_VBUS:
	case SRC_TRANSITION_SUPPLY:
		tcpm_set_state(port, HARD_RESET_SEND, 0);
		break;
	default:
		if (!tcpm_ams_interruptible(port)) {
			tcpm_set_state(port, port->pwr_role == TYPEC_SOURCE ?
				       SRC_SOFT_RESET_WAIT_SNK_TX :
				       SNK_SOFT_RESET,
				       0);
		} else {
			/* process the Message 6.8.1 */
			port->upcoming_state = state;
			port->next_ams = ams;
			tcpm_set_state(port, ready_state(port), delay_ms);
		}
		break;
	}
}

static void tcpm_pd_handle_msg(struct tcpm_port *port,
			       enum pd_msg_request message,
			       enum tcpm_ams ams)
{
	switch (port->state) {
	case SRC_READY:
	case SNK_READY:
		port->ams = ams;
		tcpm_queue_message(port, message);
		break;
	/* PD 3.0 Spec 8.3.3.4.1.1 and 6.8.1 */
	case SNK_TRANSITION_SINK:
	case SNK_TRANSITION_SINK_VBUS:
	case SRC_TRANSITION_SUPPLY:
		tcpm_set_state(port, HARD_RESET_SEND, 0);
		break;
	default:
		if (!tcpm_ams_interruptible(port)) {
			tcpm_set_state(port, port->pwr_role == TYPEC_SOURCE ?
				       SRC_SOFT_RESET_WAIT_SNK_TX :
				       SNK_SOFT_RESET,
				       0);
		} else {
			port->next_ams = ams;
			tcpm_set_state(port, ready_state(port), 0);
			/* 6.8.1 process the Message */
			tcpm_queue_message(port, message);
		}
		break;
	}
}

static int tcpm_register_source_caps(struct tcpm_port *port)
{
	struct usb_power_delivery_desc desc = { port->negotiated_rev };
	struct usb_power_delivery_capabilities_desc caps = { };
	struct usb_power_delivery_capabilities *cap;

	if (!port->partner_pd)
		port->partner_pd = usb_power_delivery_register(NULL, &desc);
	if (IS_ERR(port->partner_pd))
		return PTR_ERR(port->partner_pd);

	memcpy(caps.pdo, port->source_caps, sizeof(u32) * port->nr_source_caps);
	caps.role = TYPEC_SOURCE;

	cap = usb_power_delivery_register_capabilities(port->partner_pd, &caps);
	if (IS_ERR(cap))
		return PTR_ERR(cap);

	port->partner_source_caps = cap;

	return 0;
}

static int tcpm_register_sink_caps(struct tcpm_port *port)
{
	struct usb_power_delivery_desc desc = { port->negotiated_rev };
	struct usb_power_delivery_capabilities_desc caps = { };
	struct usb_power_delivery_capabilities *cap;

	if (!port->partner_pd)
		port->partner_pd = usb_power_delivery_register(NULL, &desc);
	if (IS_ERR(port->partner_pd))
		return PTR_ERR(port->partner_pd);

	memcpy(caps.pdo, port->sink_caps, sizeof(u32) * port->nr_sink_caps);
	caps.role = TYPEC_SINK;

	cap = usb_power_delivery_register_capabilities(port->partner_pd, &caps);
	if (IS_ERR(cap))
		return PTR_ERR(cap);

	port->partner_sink_caps = cap;

	return 0;
}

static void tcpm_pd_data_request(struct tcpm_port *port,
				 const struct pd_message *msg)
{
	enum pd_data_msg_type type = pd_header_type_le(msg->header);
	unsigned int cnt = pd_header_cnt_le(msg->header);
	unsigned int rev = pd_header_rev_le(msg->header);
	unsigned int i;
	enum frs_typec_current partner_frs_current;
	bool frs_enable;
	int ret;

	if (tcpm_vdm_ams(port) && type != PD_DATA_VENDOR_DEF) {
		port->vdm_state = VDM_STATE_ERR_BUSY;
		tcpm_ams_finish(port);
		mod_vdm_delayed_work(port, 0);
	}

	switch (type) {
	case PD_DATA_SOURCE_CAP:
		for (i = 0; i < cnt; i++)
			port->source_caps[i] = le32_to_cpu(msg->payload[i]);

		port->nr_source_caps = cnt;

		tcpm_log_source_caps(port);

		tcpm_validate_caps(port, port->source_caps,
				   port->nr_source_caps);

		tcpm_register_source_caps(port);
		trace_android_vh_typec_store_partner_src_caps(&port->nr_source_caps,
							      &port->source_caps);
		/*
		 * Adjust revision in subsequent message headers, as required,
		 * to comply with 6.2.1.1.5 of the USB PD 3.0 spec. We don't
		 * support Rev 1.0 so just do nothing in that scenario.
		 */
		if (rev == PD_REV10) {
			if (port->ams == GET_SOURCE_CAPABILITIES)
				tcpm_ams_finish(port);
			break;
		}

		if (rev < PD_MAX_REV)
			port->negotiated_rev = rev;

		if (port->pwr_role == TYPEC_SOURCE) {
			if (port->ams == GET_SOURCE_CAPABILITIES)
				tcpm_pd_handle_state(port, SRC_READY, NONE_AMS, 0);
			/* Unexpected Source Capabilities */
			else
				tcpm_pd_handle_msg(port,
						   port->negotiated_rev < PD_REV30 ?
						   PD_MSG_CTRL_REJECT :
						   PD_MSG_CTRL_NOT_SUPP,
						   NONE_AMS);
		} else if (port->state == SNK_WAIT_CAPABILITIES) {
		/*
		 * This message may be received even if VBUS is not
		 * present. This is quite unexpected; see USB PD
		 * specification, sections 8.3.3.6.3.1 and 8.3.3.6.3.2.
		 * However, at the same time, we must be ready to
		 * receive this message and respond to it 15ms after
		 * receiving PS_RDY during power swap operations, no matter
		 * if VBUS is available or not (USB PD specification,
		 * section 6.5.9.2).
		 * So we need to accept the message either way,
		 * but be prepared to keep waiting for VBUS after it was
		 * handled.
		 */
			port->ams = POWER_NEGOTIATION;
			port->in_ams = true;
			tcpm_set_state(port, SNK_NEGOTIATE_CAPABILITIES, 0);
		} else {
			if (port->ams == GET_SOURCE_CAPABILITIES)
				tcpm_ams_finish(port);
			tcpm_pd_handle_state(port, SNK_NEGOTIATE_CAPABILITIES,
					     POWER_NEGOTIATION, 0);
		}
		break;
	case PD_DATA_REQUEST:
		/*
		 * Adjust revision in subsequent message headers, as required,
		 * to comply with 6.2.1.1.5 of the USB PD 3.0 spec. We don't
		 * support Rev 1.0 so just reject in that scenario.
		 */
		if (rev == PD_REV10) {
			tcpm_pd_handle_msg(port,
					   port->negotiated_rev < PD_REV30 ?
					   PD_MSG_CTRL_REJECT :
					   PD_MSG_CTRL_NOT_SUPP,
					   NONE_AMS);
			break;
		}

		if (rev < PD_MAX_REV)
			port->negotiated_rev = rev;

		if (port->pwr_role != TYPEC_SOURCE || cnt != 1) {
			tcpm_pd_handle_msg(port,
					   port->negotiated_rev < PD_REV30 ?
					   PD_MSG_CTRL_REJECT :
					   PD_MSG_CTRL_NOT_SUPP,
					   NONE_AMS);
			break;
		}

		port->sink_request = le32_to_cpu(msg->payload[0]);

		if (port->vdm_sm_running && port->explicit_contract) {
			tcpm_pd_handle_msg(port, PD_MSG_CTRL_WAIT, port->ams);
			break;
		}

		if (port->state == SRC_SEND_CAPABILITIES)
			tcpm_set_state(port, SRC_NEGOTIATE_CAPABILITIES, 0);
		else
			tcpm_pd_handle_state(port, SRC_NEGOTIATE_CAPABILITIES,
					     POWER_NEGOTIATION, 0);
		break;
	case PD_DATA_SINK_CAP:
		/* We don't do anything with this at the moment... */
		for (i = 0; i < cnt; i++)
			port->sink_caps[i] = le32_to_cpu(msg->payload[i]);

		partner_frs_current = (port->sink_caps[0] & PDO_FIXED_FRS_CURR_MASK) >>
			PDO_FIXED_FRS_CURR_SHIFT;
		frs_enable = partner_frs_current && (partner_frs_current <=
						     port->new_source_frs_current);
		tcpm_log(port,
			 "Port partner FRS capable partner_frs_current:%u port_frs_current:%u enable:%c",
			 partner_frs_current, port->new_source_frs_current, frs_enable ? 'y' : 'n');
		if (frs_enable) {
			ret  = port->tcpc->enable_frs(port->tcpc, true);
			tcpm_log(port, "Enable FRS %s, ret:%d\n", ret ? "fail" : "success", ret);
		}

		port->nr_sink_caps = cnt;
		port->sink_cap_done = true;
		tcpm_register_sink_caps(port);

		if (port->ams == GET_SINK_CAPABILITIES)
			tcpm_set_state(port, ready_state(port), 0);
		/* Unexpected Sink Capabilities */
		else
			tcpm_pd_handle_msg(port,
					   port->negotiated_rev < PD_REV30 ?
					   PD_MSG_CTRL_REJECT :
					   PD_MSG_CTRL_NOT_SUPP,
					   NONE_AMS);
		break;
	case PD_DATA_VENDOR_DEF:
		tcpm_handle_vdm_request(port, msg->payload, cnt);
		break;
	case PD_DATA_BIST:
		port->bist_request = le32_to_cpu(msg->payload[0]);
		tcpm_pd_handle_state(port, BIST_RX, BIST, 0);
		break;
	case PD_DATA_ALERT:
		if (port->state != SRC_READY && port->state != SNK_READY)
			tcpm_pd_handle_state(port, port->pwr_role == TYPEC_SOURCE ?
					     SRC_SOFT_RESET_WAIT_SNK_TX : SNK_SOFT_RESET,
					     NONE_AMS, 0);
		else
			tcpm_handle_alert(port, msg->payload, cnt);
		break;
	case PD_DATA_BATT_STATUS:
	case PD_DATA_GET_COUNTRY_INFO:
		/* Currently unsupported */
		tcpm_pd_handle_msg(port, port->negotiated_rev < PD_REV30 ?
				   PD_MSG_CTRL_REJECT :
				   PD_MSG_CTRL_NOT_SUPP,
				   NONE_AMS);
		break;
	default:
		tcpm_pd_handle_msg(port, port->negotiated_rev < PD_REV30 ?
				   PD_MSG_CTRL_REJECT :
				   PD_MSG_CTRL_NOT_SUPP,
				   NONE_AMS);
		tcpm_log(port, "Unrecognized data message type %#x", type);
		break;
	}
}

static void tcpm_pps_complete(struct tcpm_port *port, int result)
{
	if (port->pps_pending) {
		port->pps_status = result;
		port->pps_pending = false;
		complete(&port->pps_complete);
	}
}

static void tcpm_pd_ctrl_request(struct tcpm_port *port,
				 const struct pd_message *msg)
{
	enum pd_ctrl_msg_type type = pd_header_type_le(msg->header);
	enum tcpm_state next_state;

	/*
	 * Stop VDM state machine if interrupted by other Messages while NOT_SUPP is allowed in
	 * VDM AMS if waiting for VDM responses and will be handled later.
	 */
	if (tcpm_vdm_ams(port) && type != PD_CTRL_NOT_SUPP && type != PD_CTRL_GOOD_CRC) {
		port->vdm_state = VDM_STATE_ERR_BUSY;
		tcpm_ams_finish(port);
		mod_vdm_delayed_work(port, 0);
	}

	switch (type) {
	case PD_CTRL_GOOD_CRC:
	case PD_CTRL_PING:
		break;
	case PD_CTRL_GET_SOURCE_CAP:
		tcpm_pd_handle_msg(port, PD_MSG_DATA_SOURCE_CAP, GET_SOURCE_CAPABILITIES);
		break;
	case PD_CTRL_GET_SINK_CAP:
		tcpm_pd_handle_msg(port, PD_MSG_DATA_SINK_CAP, GET_SINK_CAPABILITIES);
		break;
	case PD_CTRL_GOTO_MIN:
		break;
	case PD_CTRL_PS_RDY:
		switch (port->state) {
		case SNK_TRANSITION_SINK:
			if (port->vbus_present) {
				tcpm_set_current_limit(port,
						       port->req_current_limit,
						       port->req_supply_voltage);
				port->explicit_contract = true;
				tcpm_set_auto_vbus_discharge_threshold(port,
								       TYPEC_PWR_MODE_PD,
								       port->pps_data.active,
								       port->supply_voltage);
				tcpm_set_state(port, SNK_READY, 0);
			} else {
				/*
				 * Seen after power swap. Keep waiting for VBUS
				 * in a transitional state.
				 */
				tcpm_set_state(port,
					       SNK_TRANSITION_SINK_VBUS, 0);
			}
			break;
		case PR_SWAP_SRC_SNK_SOURCE_OFF_CC_DEBOUNCED:
			tcpm_set_state(port, PR_SWAP_SRC_SNK_SINK_ON, 0);
			break;
		case PR_SWAP_SNK_SRC_SINK_OFF:
			tcpm_set_state(port, PR_SWAP_SNK_SRC_SOURCE_ON, 0);
			break;
		case VCONN_SWAP_WAIT_FOR_VCONN:
			tcpm_set_state(port, VCONN_SWAP_TURN_OFF_VCONN, 0);
			break;
		case FR_SWAP_SNK_SRC_TRANSITION_TO_OFF:
			tcpm_set_state(port, FR_SWAP_SNK_SRC_NEW_SINK_READY, 0);
			break;
		default:
			tcpm_pd_handle_state(port,
					     port->pwr_role == TYPEC_SOURCE ?
					     SRC_SOFT_RESET_WAIT_SNK_TX :
					     SNK_SOFT_RESET,
					     NONE_AMS, 0);
			break;
		}
		break;
	case PD_CTRL_REJECT:
	case PD_CTRL_WAIT:
	case PD_CTRL_NOT_SUPP:
		switch (port->state) {
		case SNK_NEGOTIATE_CAPABILITIES:
			/* USB PD specification, Figure 8-43 */
			if (port->explicit_contract)
				next_state = SNK_READY;
			else
				next_state = SNK_WAIT_CAPABILITIES;

			/* Threshold was relaxed before sending Request. Restore it back. */
			tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
							       port->pps_data.active,
							       port->supply_voltage);
			tcpm_set_state(port, next_state, 0);
			break;
		case SNK_NEGOTIATE_PPS_CAPABILITIES:
			/* Revert data back from any requested PPS updates */
			port->pps_data.req_out_volt = port->supply_voltage;
			port->pps_data.req_op_curr = port->current_limit;
			port->pps_status = (type == PD_CTRL_WAIT ?
					    -EAGAIN : -EOPNOTSUPP);

			/* Threshold was relaxed before sending Request. Restore it back. */
			tcpm_set_auto_vbus_discharge_threshold(port, TYPEC_PWR_MODE_PD,
							       port->pps_data.active,
							       port->supply_voltage);

			tcpm_set_state(port, SNK_READY, 0);
			break;
		case DR_SWAP_SEND:
			port->swap_status = (type == PD_CTRL_WAIT ?
					     -EAGAIN : -EOPNOTSUPP);
			tcpm_set_state(port, DR_SWAP_CANCEL, 0);
			break;
		case PR_SWAP_SEND:
			port->swap_status = (type == PD_CTRL_WAIT ?
					     -EAGAIN : -EOPNOTSUPP);
			tcpm_set_state(port, PR_SWAP_CANCEL, 0);
			break;
		case VCONN_SWAP_SEND:
			port->swap_status = (type == PD_CTRL_WAIT ?
					     -EAGAIN : -EOPNOTSUPP);
			tcpm_set_state(port, VCONN_SWAP_CANCEL, 0);
			break;
		case FR_SWAP_SEND:
			tcpm_set_state(port, FR_SWAP_CANCEL, 0);
			break;
		case GET_SINK_CAP:
			port->sink_cap_done = true;
			tcpm_set_state(port, ready_state(port), 0);
			break;
		/*
		 * Some port partners do not support GET_STATUS, avoid soft reset the link to
		 * prevent redundant power re-negotiation
		 */
		case GET_STATUS_SEND:
			tcpm_set_state(port, ready_state(port), 0);
			break;
		case SRC_READY:
		case SNK_READY:
			if (port->vdm_state > VDM_STATE_READY) {
				port->vdm_state = VDM_STATE_DONE;
				if (tcpm_vdm_ams(port))
					tcpm_ams_finish(port);
				mod_vdm_delayed_work(port, 0);
				break;
			}
			fallthrough;
		default:
			tcpm_pd_handle_state(port,
					     port->pwr_role == TYPEC_SOURCE ?
					     SRC_SOFT_RESET_WAIT_SNK_TX :
					     SNK_SOFT_RESET,
					     NONE_AMS, 0);
			break;
		}
		break;
	case PD_CTRL_ACCEPT:
		switch (port->state) {
		case SNK_NEGOTIATE_CAPABILITIES:
			port->pps_data.active = false;
			tcpm_set_state(port, SNK_TRANSITION_SINK, 0);
			break;
		case SNK_NEGOTIATE_PPS_CAPABILITIES:
			port->pps_data.active = true;
			port->pps_data.min_volt = port->pps_data.req_min_volt;
			port->pps_data.max_volt = port->pps_data.req_max_volt;
			port->pps_data.max_curr = port->pps_data.req_max_curr;
			port->req_supply_voltage = port->pps_data.req_out_volt;
			port->req_current_limit = port->pps_data.req_op_curr;
			power_supply_changed(port->psy);
			tcpm_set_state(port, SNK_TRANSITION_SINK, 0);
			break;
		case SOFT_RESET_SEND:
			if (port->ams == SOFT_RESET_AMS)
				tcpm_ams_finish(port);
			if (port->pwr_role == TYPEC_SOURCE) {
				port->upcoming_state = SRC_SEND_CAPABILITIES;
				tcpm_ams_start(port, POWER_NEGOTIATION);
			} else {
				tcpm_set_state(port, SNK_WAIT_CAPABILITIES, 0);
			}
			break;
		case DR_SWAP_SEND:
			tcpm_set_state(port, DR_SWAP_CHANGE_DR, 0);
			break;
		case PR_SWAP_SEND:
			tcpm_set_state(port, PR_SWAP_START, 0);
			break;
		case VCONN_SWAP_SEND:
			tcpm_set_state(port, VCONN_SWAP_START, 0);
			break;
		case FR_SWAP_SEND:
			tcpm_set_state(port, FR_SWAP_SNK_SRC_TRANSITION_TO_OFF, 0);
			break;
		default:
			tcpm_pd_handle_state(port,
					     port->pwr_role == TYPEC_SOURCE ?
					     SRC_SOFT_RESET_WAIT_SNK_TX :
					     SNK_SOFT_RESET,
					     NONE_AMS, 0);
			break;
		}
		break;
	case PD_CTRL_SOFT_RESET:
		port->ams = SOFT_RESET_AMS;
		tcpm_set_state(port, SOFT_RESET, 0);
		break;
	case PD_CTRL_DR_SWAP:
		/*
		 * XXX
		 * 6.3.9: If an alternate mode is active, a request to swap
		 * alternate modes shall trigger a port reset.
		 */
		if (port->typec_caps.data != TYPEC_PORT_DRD) {
			tcpm_pd_handle_msg(port,
					   port->negotiated_rev < PD_REV30 ?
					   PD_MSG_CTRL_REJECT :
					   PD_MSG_CTRL_NOT_SUPP,
					   NONE_AMS);
		} else {
			if (port->send_discover && port->negotiated_rev < PD_REV30) {
				tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
				break;
			}

			tcpm_pd_handle_state(port, DR_SWAP_ACCEPT, DATA_ROLE_SWAP, 0);
		}
		break;
	case PD_CTRL_PR_SWAP:
		if (port->port_type != TYPEC_PORT_DRP) {
			tcpm_pd_handle_msg(port,
					   port->negotiated_rev < PD_REV30 ?
					   PD_MSG_CTRL_REJECT :
					   PD_MSG_CTRL_NOT_SUPP,
					   NONE_AMS);
		} else {
			if (port->send_discover && port->negotiated_rev < PD_REV30) {
				tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
				break;
			}

			tcpm_pd_handle_state(port, PR_SWAP_ACCEPT, POWER_ROLE_SWAP, 0);
		}
		break;
	case PD_CTRL_VCONN_SWAP:
		if (port->send_discover && port->negotiated_rev < PD_REV30) {
			tcpm_queue_message(port, PD_MSG_CTRL_WAIT);
			break;
		}

		tcpm_pd_handle_state(port, VCONN_SWAP_ACCEPT, VCONN_SWAP, 0);
		break;
	case PD_CTRL_GET_SOURCE_CAP_EXT:
	case PD_CTRL_GET_STATUS:
	case PD_CTRL_FR_SWAP:
	case PD_CTRL_GET_PPS_STATUS:
	case PD_CTRL_GET_COUNTRY_CODES:
		/* Currently not supported */
		tcpm_pd_handle_msg(port,
				   port->negotiated_rev < PD_REV30 ?
				   PD_MSG_CTRL_REJECT :
				   PD_MSG_CTRL_NOT_SUPP,
				   NONE_AMS);
		break;
	default:
		tcpm_pd_handle_msg(port,
				   port->negotiated_rev < PD_REV30 ?
				   PD_MSG_CTRL_REJECT :
				   PD_MSG_CTRL_NOT_SUPP,
				   NONE_AMS);
		tcpm_log(port, "Unrecognized ctrl message type %#x", type);
		break;
	}
}

static void tcpm_pd_ext_msg_request(struct tcpm_port *port,
				    const struct pd_message *msg)
{
	enum pd_ext_msg_type type = pd_header_type_le(msg->header);
	unsigned int data_size = pd_ext_header_data_size_le(msg->ext_msg.header);

	/* stopping VDM state machine if interrupted by other Messages */
	if (tcpm_vdm_ams(port)) {
		port->vdm_state = VDM_STATE_ERR_BUSY;
		tcpm_ams_finish(port);
		mod_vdm_delayed_work(port, 0);
	}

	if (!(le16_to_cpu(msg->ext_msg.header) & PD_EXT_HDR_CHUNKED)) {
		tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
		tcpm_log(port, "Unchunked extended messages unsupported");
		return;
	}

	if (data_size > PD_EXT_MAX_CHUNK_DATA) {
		tcpm_pd_handle_state(port, CHUNK_NOT_SUPP, NONE_AMS, PD_T_CHUNK_NOT_SUPP);
		tcpm_log(port, "Chunk handling not yet supported");
		return;
	}

	switch (type) {
	case PD_EXT_STATUS:
	case PD_EXT_PPS_STATUS:
		if (port->ams == GETTING_SOURCE_SINK_STATUS) {
			tcpm_ams_finish(port);
			tcpm_set_state(port, ready_state(port), 0);
		} else {
			/* unexpected Status or PPS_Status Message */
			tcpm_pd_handle_state(port, port->pwr_role == TYPEC_SOURCE ?
					     SRC_SOFT_RESET_WAIT_SNK_TX : SNK_SOFT_RESET,
					     NONE_AMS, 0);
		}
		break;
	case PD_EXT_SOURCE_CAP_EXT:
	case PD_EXT_GET_BATT_CAP:
	case PD_EXT_GET_BATT_STATUS:
	case PD_EXT_BATT_CAP:
	case PD_EXT_GET_MANUFACTURER_INFO:
	case PD_EXT_MANUFACTURER_INFO:
	case PD_EXT_SECURITY_REQUEST:
	case PD_EXT_SECURITY_RESPONSE:
	case PD_EXT_FW_UPDATE_REQUEST:
	case PD_EXT_FW_UPDATE_RESPONSE:
	case PD_EXT_COUNTRY_INFO:
	case PD_EXT_COUNTRY_CODES:
		tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
		break;
	default:
		tcpm_pd_handle_msg(port, PD_MSG_CTRL_NOT_SUPP, NONE_AMS);
		tcpm_log(port, "Unrecognized extended message type %#x", type);
		break;
	}
}

static void tcpm_pd_rx_handler(struct kthread_work *work)
{
	struct pd_rx_event *event = container_of(work,
						 struct pd_rx_event, work);
	const struct pd_message *msg = &event->msg;
	unsigned int cnt = pd_header_cnt_le(msg->header);
	struct tcpm_port *port = event->port;

	mutex_lock(&port->lock);

	tcpm_log(port, "PD RX, header: %#x [%d]", le16_to_cpu(msg->header),
		 port->attached);

	if (port->attached) {
		enum pd_ctrl_msg_type type = pd_header_type_le(msg->header);
		unsigned int msgid = pd_header_msgid_le(msg->header);

		/*
		 * USB PD standard, 6.6.1.2:
		 * "... if MessageID value in a received Message is the
		 * same as the stored value, the receiver shall return a
		 * GoodCRC Message with that MessageID value and drop
		 * the Message (this is a retry of an already received
		 * Message). Note: this shall not apply to the Soft_Reset
		 * Message which always has a MessageID value of zero."
		 */
		if (msgid == port->rx_msgid && type != PD_CTRL_SOFT_RESET)
			goto done;
		port->rx_msgid = msgid;

		/*
		 * If both ends believe to be DFP/host, we have a data role
		 * mismatch.
		 */
		if (!!(le16_to_cpu(msg->header) & PD_HEADER_DATA_ROLE) ==
		    (port->data_role == TYPEC_HOST)) {
			tcpm_log(port,
				 "Data role mismatch, initiating error recovery");
			tcpm_set_state(port, ERROR_RECOVERY, 0);
		} else {
			if (le16_to_cpu(msg->header) & PD_HEADER_EXT_HDR)
				tcpm_pd_ext_msg_request(port, msg);
			else if (cnt)
				tcpm_pd_data_request(port, msg);
			else
				tcpm_pd_ctrl_request(port, msg);
		}
	}

done:
	mutex_unlock(&port->lock);
	kfree(event);
}

void tcpm_pd_receive(struct tcpm_port *port, const struct pd_message *msg)
{
	struct pd_rx_event *event;

	event = kzalloc(sizeof(*event), GFP_ATOMIC);
	if (!event)
		return;

	kthread_init_work(&event->work, tcpm_pd_rx_handler);
	event->port = port;
	memcpy(&event->msg, msg, sizeof(*msg));
	kthread_queue_work(port->wq, &event->work);
}
EXPORT_SYMBOL_GPL(tcpm_pd_receive);

static int tcpm_pd_send_control(struct tcpm_port *port,
				enum pd_ctrl_msg_type type)
{
	struct pd_message msg;

	memset(&msg, 0, sizeof(msg));
	msg.header = PD_HEADER_LE(type, port->pwr_role,
				  port->data_role,
				  port->negotiated_rev,
				  port->message_id, 0);

	return tcpm_pd_transmit(port, TCPC_TX_SOP, &msg);
}

/*
 * Send queued message without affecting state.
 * Return true if state machine should go back to sleep,
 * false otherwise.
 */
static bool tcpm_send_queued_message(struct tcpm_port *port)
{
	enum pd_msg_request queued_m