zephyr: emul: Add common handler for TCPCI partner

Add common handler for messages send to TCPCI partner emulator.

BUG=none
BRANCH=none
TEST=make configure --test zephyr/test/drivers

Signed-off-by: Tomasz Michalec <tm@semihalf.com>
Change-Id: Ib2d9ef0005e377efd8de04afdc24ed3a6bd7ff1d
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3358577
Reviewed-by: Sam Hurst <shurst@google.com>
Tested-by: Tomasz Michalec <tmichalec@google.com>
Commit-Queue: Tomasz Michalec <tmichalec@google.com>

6 files changed, 301 insertions, 31 deletions

diff --git a/zephyr/emul/tcpc/emul_tcpci_partner_common.c b/zephyr/emul/tcpc/emul_tcpci_partner_common.c
index abd86ed890..8d3d6948d8 100644
--- a/zephyr/emul/tcpc/emul_tcpci_partner_common.c
+++ b/zephyr/emul/tcpc/emul_tcpci_partner_common.c
@@ -276,6 +276,159 @@ int tcpci_partner_clear_msg_queue(struct tcpci_partner_data *data)
 	return 0;
 }
 
+/**
+ * @brief Reset common data to state after hard reset (reset counters, flags,
+ *        clear message queue)
+ *
+ * @param data Pointer to TCPCI partner emulator
+ */
+static void tcpci_partner_common_reset(struct tcpci_partner_data *data)
+{
+	tcpci_partner_clear_msg_queue(data);
+	data->msg_id = 0;
+	data->recv_msg_id = -1;
+	data->wait_for_response = false;
+	data->in_soft_reset = false;
+}
+
+/** Check description in emul_common_tcpci_partner.h */
+void tcpci_partner_common_send_hard_reset(struct tcpci_partner_data *data)
+{
+	struct tcpci_partner_msg *msg;
+
+	tcpci_partner_common_reset(data);
+
+	msg = tcpci_partner_alloc_msg(0);
+	msg->msg.type = TCPCI_MSG_TX_HARD_RESET;
+
+	tcpci_partner_send_msg(data, msg, 0);
+}
+
+/** Check description in emul_common_tcpci_partner.h */
+void tcpci_partner_common_send_soft_reset(struct tcpci_partner_data *data)
+{
+	/* Reset counters */
+	data->msg_id = 0;
+	data->recv_msg_id = -1;
+	/* Send message */
+	tcpci_partner_send_control_msg(data, PD_CTRL_SOFT_RESET, 0);
+	/* Wait for accept of soft reset */
+	data->wait_for_response = true;
+	data->in_soft_reset = true;
+}
+
+/** Check description in emul_common_tcpci_partner.h */
+enum tcpci_partner_handler_res tcpci_partner_common_msg_handler(
+	struct tcpci_partner_data *data,
+	const struct tcpci_emul_msg *tx_msg,
+	enum tcpci_msg_type type,
+	enum tcpci_emul_tx_status tx_status)
+{
+	uint16_t header;
+	int msg_type;
+
+	tcpci_emul_partner_msg_status(data->tcpci_emul, tx_status);
+	/* If receiving message was unsuccessful, abandon processing message */
+	if (tx_status != TCPCI_EMUL_TX_SUCCESS) {
+		return TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED;
+	}
+
+	LOG_HEXDUMP_INF(tx_msg->buf, tx_msg->cnt,
+			"USB-C partner emulator received message");
+
+	/* Handle hard reset */
+	if (type == TCPCI_MSG_TX_HARD_RESET) {
+		tcpci_partner_common_reset(data);
+
+		return TCPCI_PARTNER_COMMON_MSG_HARD_RESET;
+	}
+
+	/* Handle only SOP messages */
+	if (type != TCPCI_MSG_SOP) {
+		return TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED;
+	}
+
+	header = sys_get_le16(tx_msg->buf);
+	msg_type = PD_HEADER_TYPE(header);
+
+	if (PD_HEADER_ID(header) == data->recv_msg_id &&
+	    msg_type != PD_CTRL_SOFT_RESET) {
+		/* Repeated message mark as handled */
+		return TCPCI_PARTNER_COMMON_MSG_HANDLED;
+	}
+
+	data->recv_msg_id = PD_HEADER_ID(header);
+
+	if (PD_HEADER_CNT(header)) {
+		switch (PD_HEADER_TYPE(header)) {
+		case PD_DATA_VENDOR_DEF:
+			/* VDM (vendor defined message) - ignore */
+			return TCPCI_PARTNER_COMMON_MSG_HANDLED;
+		default:
+			/* No other common handlers for data messages */
+			return TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED;
+		}
+	}
+
+	if (data->common_handler_masked & BIT(msg_type)) {
+		/* This message type is masked from common handler */
+		return TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED;
+	}
+
+	/* Handle control message */
+	switch (PD_HEADER_TYPE(header)) {
+	case PD_CTRL_SOFT_RESET:
+		data->msg_id = 0;
+		tcpci_partner_send_control_msg(data, PD_CTRL_ACCEPT, 0);
+		return TCPCI_PARTNER_COMMON_MSG_HANDLED;
+
+	case PD_CTRL_REJECT:
+		if (data->in_soft_reset) {
+			tcpci_partner_common_send_hard_reset(data);
+
+			return TCPCI_PARTNER_COMMON_MSG_HARD_RESET;
+		}
+		/* Fall through */
+	case PD_CTRL_ACCEPT:
+		if (data->wait_for_response) {
+			if (data->in_soft_reset) {
+				/*
+				 * Accept is response to soft reset send by
+				 * common code. It is handled here
+				 */
+				data->wait_for_response = false;
+				data->in_soft_reset = false;
+
+				return TCPCI_PARTNER_COMMON_MSG_HANDLED;
+			}
+			/*
+			 * Accept/reject is expected message and emulator code
+			 * should handle it
+			 */
+			return TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED;
+		}
+
+		/* Unexpected message - trigger soft reset */
+		tcpci_partner_common_send_soft_reset(data);
+
+		return TCPCI_PARTNER_COMMON_MSG_HANDLED;
+	}
+
+	return TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED;
+}
+
+/** Check description in emul_common_tcpci_partner.h */
+void tcpci_partner_common_handler_mask_msg(struct tcpci_partner_data *data,
+					   enum pd_ctrl_msg_type type,
+					   bool enable)
+{
+	if (enable) {
+		data->common_handler_masked |= BIT(type);
+	} else {
+		data->common_handler_masked &= ~BIT(type);
+	}
+}
+
 /** Check description in emul_common_tcpci_partner.h */
 void tcpci_partner_init(struct tcpci_partner_data *data)
 {
@@ -283,4 +436,5 @@ void tcpci_partner_init(struct tcpci_partner_data *data)
 		     NULL);
 	sys_slist_init(&data->to_send);
 	k_mutex_init(&data->to_send_mutex);
+	tcpci_partner_common_reset(data);
 }
diff --git a/zephyr/emul/tcpc/emul_tcpci_partner_snk.c b/zephyr/emul/tcpc/emul_tcpci_partner_snk.c
index 2f4d8628b5..cfeb11b7dd 100644
--- a/zephyr/emul/tcpc/emul_tcpci_partner_snk.c
+++ b/zephyr/emul/tcpc/emul_tcpci_partner_snk.c
@@ -282,6 +282,8 @@ static void tcpci_snk_emul_handle_source_cap(struct tcpci_snk_emul_data *data,
 		rdo = tcpci_snk_emul_create_rdo(pdo, data->pdo[0], 1);
 	}
 
+	/* Expect response for request */
+	data->common_data.wait_for_response = true;
 	tcpci_partner_send_data_msg(&data->common_data, PD_DATA_REQUEST, &rdo,
 				    1 /* = data_obj_num */, 0 /* = delay */);
 }
@@ -304,16 +306,26 @@ static void tcpci_snk_emul_transmit_op(const struct emul *emul,
 {
 	struct tcpci_snk_emul_data *data =
 		CONTAINER_OF(ops, struct tcpci_snk_emul_data, ops);
+	enum tcpci_partner_handler_res processed;
 	uint16_t header;
 
-	/* Acknowledge that message was sent successfully */
-	tcpci_emul_partner_msg_status(emul, TCPCI_EMUL_TX_SUCCESS);
+	processed = tcpci_partner_common_msg_handler(&data->common_data,
+						     tx_msg, type,
+						     TCPCI_EMUL_TX_SUCCESS);
+	switch (processed) {
+	case TCPCI_PARTNER_COMMON_MSG_HARD_RESET:
+		/* Handle hard reset */
+		data->wait_for_ps_rdy = false;
+		data->pd_completed = false;
 
-	/* Handle hard reset */
-	if (type == TCPCI_MSG_TX_HARD_RESET) {
-		tcpci_partner_clear_msg_queue(&data->common_data);
-		data->common_data.msg_id = 0;
 		return;
+	case TCPCI_PARTNER_COMMON_MSG_HANDLED:
+		/* Message handled nothing to do */
+		return;
+	case TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED:
+	default:
+		/* Continue */
+		break;
 	}
 
 	/* Handle only SOP messages */
@@ -321,9 +333,6 @@ static void tcpci_snk_emul_transmit_op(const struct emul *emul,
 		return;
 	}
 
-	LOG_HEXDUMP_INF(tx_msg->buf, tx_msg->cnt,
-			"USB-C sink received message");
-
 	header = sys_get_le16(tx_msg->buf);
 
 	if (PD_HEADER_CNT(header)) {
@@ -332,9 +341,6 @@ static void tcpci_snk_emul_transmit_op(const struct emul *emul,
 		case PD_DATA_SOURCE_CAP:
 			tcpci_snk_emul_handle_source_cap(data, tx_msg);
 			break;
-		case PD_DATA_VENDOR_DEF:
-			/* VDM (vendor defined message) - ignore */
-			break;
 		default:
 			tcpci_partner_send_control_msg(&data->common_data,
 						       PD_CTRL_REJECT, 0);
@@ -354,18 +360,24 @@ static void tcpci_snk_emul_transmit_op(const struct emul *emul,
 			tcpci_partner_send_control_msg(&data->common_data,
 						       PD_CTRL_REJECT, 0);
 			break;
-		case PD_CTRL_SOFT_RESET:
-			data->common_data.msg_id = 0;
-			tcpci_partner_send_control_msg(&data->common_data,
-						       PD_CTRL_ACCEPT, 0);
+		case PD_CTRL_PING:
 			break;
-		case PD_CTRL_ACCEPT:
+		case PD_CTRL_PS_RDY:
+			__ASSERT(data->wait_for_ps_rdy,
+				 "Unexpected PS RDY message");
+			data->wait_for_ps_rdy = false;
+			data->pd_completed = true;
 			break;
 		case PD_CTRL_REJECT:
+			/* Request rejected. Ask for capabilities again. */
+			tcpci_partner_send_control_msg(&data->common_data,
+						       PD_CTRL_GET_SOURCE_CAP,
+						       0);
+			data->common_data.wait_for_response = false;
 			break;
-		case PD_CTRL_PING:
-			break;
-		case PD_CTRL_PS_RDY:
+		case PD_CTRL_ACCEPT:
+			data->common_data.wait_for_response = false;
+			data->wait_for_ps_rdy = true;
 			break;
 		default:
 			tcpci_partner_send_control_msg(&data->common_data,
@@ -409,6 +421,9 @@ int tcpci_snk_emul_connect_to_tcpci(struct tcpci_snk_emul_data *data,
 		data->common_data.tcpci_emul = tcpci_emul;
 	}
 
+	data->wait_for_ps_rdy = false;
+	data->pd_completed = false;
+
 	return ret;
 }
 
diff --git a/zephyr/emul/tcpc/emul_tcpci_partner_src.c b/zephyr/emul/tcpc/emul_tcpci_partner_src.c
index 8dad1c9c5e..11cc5e4033 100644
--- a/zephyr/emul/tcpc/emul_tcpci_partner_src.c
+++ b/zephyr/emul/tcpc/emul_tcpci_partner_src.c
@@ -6,6 +6,7 @@
 #include <logging/log.h>
 LOG_MODULE_REGISTER(tcpci_src_emul, CONFIG_TCPCI_EMUL_LOG_LEVEL);
 
+#include <sys/byteorder.h>
 #include <zephyr.h>
 
 #include "common.h"
@@ -59,21 +60,33 @@ static void tcpci_src_emul_transmit_op(const struct emul *emul,
 {
 	struct tcpci_src_emul_data *data =
 		CONTAINER_OF(ops, struct tcpci_src_emul_data, ops);
+	enum tcpci_partner_handler_res processed;
 	uint16_t header;
 
-	/* Acknowledge that message was sent successfully */
-	tcpci_emul_partner_msg_status(emul, TCPCI_EMUL_TX_SUCCESS);
+	processed = tcpci_partner_common_msg_handler(&data->common_data,
+						     tx_msg, type,
+						     TCPCI_EMUL_TX_SUCCESS);
+	/* Handle hard reset */
+	if (processed == TCPCI_PARTNER_COMMON_MSG_HARD_RESET) {
+		/* Send capability after 15 ms to establish PD again */
+		tcpci_src_emul_send_capability_msg(data, 15);
+
+		return;
+	}
 
 	/* Handle only SOP messages */
 	if (type != TCPCI_MSG_SOP) {
 		return;
 	}
 
-	LOG_HEXDUMP_DBG(tx_msg->buf, tx_msg->cnt, "Source received message");
-
-	header = (tx_msg->buf[1] << 8) | tx_msg->buf[0];
+	header = sys_get_le16(tx_msg->buf);
 
 	if (PD_HEADER_CNT(header)) {
+		if (processed == TCPCI_PARTNER_COMMON_MSG_HANDLED) {
+			/* Message already processed by common handler */
+			return;
+		}
+
 		/* Handle data message */
 		switch (PD_HEADER_TYPE(header)) {
 		case PD_DATA_REQUEST:
@@ -83,15 +96,21 @@ static void tcpci_src_emul_transmit_op(const struct emul *emul,
 			tcpci_partner_send_control_msg(&data->common_data,
 						       PD_CTRL_PS_RDY, 15);
 			break;
-		case PD_DATA_VENDOR_DEF:
-			/* VDM (vendor defined message) - ignore */
-			break;
 		default:
 			tcpci_partner_send_control_msg(&data->common_data,
 						       PD_CTRL_REJECT, 0);
 			break;
 		}
 	} else {
+		if (processed == TCPCI_PARTNER_COMMON_MSG_HANDLED &&
+		    PD_HEADER_TYPE(header) != PD_CTRL_SOFT_RESET) {
+			/*
+			 * Only soft reset requires additional handling after
+			 * common handler
+			 */
+			return;
+		}
+
 		/* Handle control message */
 		switch (PD_HEADER_TYPE(header)) {
 		case PD_CTRL_GET_SOURCE_CAP:
@@ -106,9 +125,6 @@ static void tcpci_src_emul_transmit_op(const struct emul *emul,
 						       PD_CTRL_REJECT, 0);
 			break;
 		case PD_CTRL_SOFT_RESET:
-			data->common_data.msg_id = 0;
-			tcpci_partner_send_control_msg(&data->common_data,
-						       PD_CTRL_ACCEPT, 0);
 			/* Send capability after 15 ms to establish PD again */
 			tcpci_src_emul_send_capability_msg(data, 15);
 			break;
diff --git a/zephyr/include/emul/tcpc/emul_tcpci_partner_common.h b/zephyr/include/emul/tcpc/emul_tcpci_partner_common.h
index 3337316453..066a167703 100644
--- a/zephyr/include/emul/tcpc/emul_tcpci_partner_common.h
+++ b/zephyr/include/emul/tcpc/emul_tcpci_partner_common.h
@@ -41,12 +41,29 @@ struct tcpci_partner_data {
 	struct k_mutex to_send_mutex;
 	/** Next SOP message id */
 	int msg_id;
+	/** Last received message id */
+	int recv_msg_id;
 	/** Power role (used in message header) */
 	enum pd_power_role power_role;
 	/** Data role (used in message header) */
 	enum pd_data_role data_role;
 	/** Revision (used in message header) */
 	enum pd_rev_type rev;
+	/**
+	 * Mask for control message types that shouldn't be handled
+	 * in common message handler
+	 */
+	uint32_t common_handler_masked;
+	/**
+	 * True if accept and reject messages shouldn't trigger soft reset
+	 * in common message handler
+	 */
+	bool wait_for_response;
+	/**
+	 * If emulator triggers soft reset, it waits for accept. If accept
+	 * doesn't arrive, hard reset is triggered.
+	 */
+	bool in_soft_reset;
 };
 
 /** Structure of message used by TCPCI partner emulator */
@@ -63,6 +80,13 @@ struct tcpci_partner_msg {
 	int data_objects;
 };
 
+/** Result of common handler */
+enum tcpci_partner_handler_res {
+	TCPCI_PARTNER_COMMON_MSG_HANDLED,
+	TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED,
+	TCPCI_PARTNER_COMMON_MSG_HARD_RESET
+};
+
 /**
  * @brief Initialise common TCPCI partner emulator. Need to be called before
  *        any other function.
@@ -157,6 +181,61 @@ int tcpci_partner_send_data_msg(struct tcpci_partner_data *data,
 int tcpci_partner_clear_msg_queue(struct tcpci_partner_data *data);
 
 /**
+ * @brief Send hard reset and set common data to state after hard reset (reset
+ *        counters, flags, clear message queue)
+ *
+ * @param data Pointer to TCPCI partner emulator
+ */
+void tcpci_partner_common_send_hard_reset(struct tcpci_partner_data *data);
+
+/**
+ * @brief Send hard reset and set common data to state after soft reset (reset
+ *        counters, set flags to wait for accept)
+ *
+ * @param data Pointer to TCPCI partner emulator
+ */
+void tcpci_partner_common_send_soft_reset(struct tcpci_partner_data *data);
+
+/**
+ * @brief Common handler for TCPCI messages. It handles hard reset, soft reset,
+ *        repeated messages. It handles vendor defined messages by skipping
+ *        them. Accept and reject messages are handled when soft reset is send.
+ *        Accept/reject messages are skipped when wait_for_response flag is set.
+ *        All control messages may be masked by
+ *        @ref tcpci_partner_common_handler_mask_msg
+ *        If @p tx_status isn't success, then all message handling is skipped.
+ *
+ * @param data Pointer to TCPCI partner emulator
+ * @param tx_msg Message received by partner emulator
+ * @param type Type of message
+ * @param tx_status Status which should be returned to TCPCI emulator
+ *
+ * @param TCPCI_PARTNER_COMMON_MSG_HANDLED Message was handled by common code
+ * @param TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED Message wasn't handled
+ * @param TCPCI_PARTNER_COMMON_MSG_HARD_RESET Message was handled by sending
+ *                                            hard reset
+ */
+enum tcpci_partner_handler_res tcpci_partner_common_msg_handler(
+	struct tcpci_partner_data *data,
+	const struct tcpci_emul_msg *tx_msg,
+	enum tcpci_msg_type type,
+	enum tcpci_emul_tx_status tx_status);
+
+
+/**
+ * @brief Select if @ref tcpci_partner_common_msg_handler should handle specific
+ *        control message type.
+ *
+ * @param data Pointer to TCPCI partner emulator
+ * @param type Control message to mask/unmask
+ * @param enable If true message of that type is handled, if false common
+ *               handler doesn't handle message of that type
+ */
+void tcpci_partner_common_handler_mask_msg(struct tcpci_partner_data *data,
+					   enum pd_ctrl_msg_type type,
+					   bool enable);
+
+/**
  * @}
  */
 
diff --git a/zephyr/include/emul/tcpc/emul_tcpci_partner_snk.h b/zephyr/include/emul/tcpc/emul_tcpci_partner_snk.h
index 7e2482a4ad..f094705799 100644
--- a/zephyr/include/emul/tcpc/emul_tcpci_partner_snk.h
+++ b/zephyr/include/emul/tcpc/emul_tcpci_partner_snk.h
@@ -35,6 +35,10 @@ struct tcpci_snk_emul_data {
 	struct tcpci_emul_partner_ops ops;
 	/** Power data objects returned in sink capabilities message */
 	uint32_t pdo[PDO_MAX_OBJECTS];
+	/** Emulator is waiting for PS RDY message */
+	bool wait_for_ps_rdy;
+	/** PS RDY was received and PD negotiation is completed */
+	bool pd_completed;
 };
 
 /**
diff --git a/zephyr/test/drivers/src/integration_usb.c b/zephyr/test/drivers/src/integration_usb.c
index 72dddbb686..64a420687c 100644
--- a/zephyr/test/drivers/src/integration_usb.c
+++ b/zephyr/test/drivers/src/integration_usb.c
@@ -198,6 +198,8 @@ static void test_attach_sink(void)
 	/* Wait for PD negotiation */
 	k_sleep(K_SECONDS(10));
 
+	/* Test if partner believe that PD negotiation is completed */
+	zassert_true(my_sink.pd_completed, NULL);
 	/*
 	 * Test that SRC ready is achieved
 	 * TODO: Change it to examining EC_CMD_TYPEC_STATUS