zephyr: emul: Resend source capabilities

Resend source capabilities message from TCPC source partner emulator,
when previous message couldn't be send. This is in line with PD
specification.

BUG=b:224927648
BRANCH=none
TEST=zmake configure --test test-drivers

Signed-off-by: Tomasz Michalec <tm@semihalf.com>
Change-Id: I34a6b3d08546f3a17dcad629f4c6f09cd189f15f
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3528403
Reviewed-by: Fabio Baltieri <fabiobaltieri@google.com>
Tested-by: Tomasz Michalec <tmichalec@google.com>
Commit-Queue: Tomasz Michalec <tmichalec@google.com>

6 files changed, 174 insertions, 18 deletions

diff --git a/zephyr/emul/tcpc/emul_tcpci_partner_common.c b/zephyr/emul/tcpc/emul_tcpci_partner_common.c
index 983c9735f2..ff7b400b63 100644
--- a/zephyr/emul/tcpc/emul_tcpci_partner_common.c
+++ b/zephyr/emul/tcpc/emul_tcpci_partner_common.c
@@ -419,7 +419,7 @@ static void tcpci_partner_sender_response_timeout(struct k_work *work)
 void tcpci_partner_start_sender_response_timer(struct tcpci_partner_data *data)
 {
 	k_work_schedule(&data->sender_response_timeout,
-			TCPCI_PARTNER_RESPONSE_TIMEOUT_MS);
+			TCPCI_PARTNER_RESPONSE_TIMEOUT);
 	data->wait_for_response = true;
 }
 
diff --git a/zephyr/emul/tcpc/emul_tcpci_partner_drp.c b/zephyr/emul/tcpc/emul_tcpci_partner_drp.c
index f69ebc5d85..fb8eba4f58 100644
--- a/zephyr/emul/tcpc/emul_tcpci_partner_drp.c
+++ b/zephyr/emul/tcpc/emul_tcpci_partner_drp.c
@@ -161,14 +161,11 @@ static void tcpci_drp_emul_transmit_op(const struct emul *emul,
 	case TCPCI_PARTNER_COMMON_MSG_HANDLED:
 		if (!drp_emul->data.sink && PD_HEADER_CNT(header) == 0 &&
 		    PD_HEADER_TYPE(header) == PD_CTRL_SOFT_RESET) {
-			/*
-			 * As source, advertise capabilities after 15 ms after
-			 * soft reset
-			 */
-			tcpci_src_emul_send_capability_msg(
+			/* As source, advertise capabilities after soft reset */
+			tcpci_src_emul_send_capability_msg_with_timer(
 							&drp_emul->src_data,
 							&drp_emul->common_data,
-							15);
+							0);
 		}
 		/* Message handled nothing to do */
 		k_mutex_unlock(&drp_emul->common_data.transmit_mutex);
@@ -254,6 +251,7 @@ static void tcpci_drp_emul_disconnect_op(
 		CONTAINER_OF(ops, struct tcpci_drp_emul, ops);
 
 	tcpci_partner_common_disconnect(&drp_emul->common_data);
+	tcpci_src_emul_disconnect(&drp_emul->src_data);
 }
 
 /** Check description in emul_tcpci_partner_drp.h */
diff --git a/zephyr/emul/tcpc/emul_tcpci_partner_snk.c b/zephyr/emul/tcpc/emul_tcpci_partner_snk.c
index 052193ac40..2c770deb69 100644
--- a/zephyr/emul/tcpc/emul_tcpci_partner_snk.c
+++ b/zephyr/emul/tcpc/emul_tcpci_partner_snk.c
@@ -308,7 +308,7 @@ static void tcpci_snk_emul_start_partner_transition_timer(
 	struct tcpci_partner_data *common_data)
 {
 	k_work_schedule(&common_data->sender_response_timeout,
-			TCPCI_PARTNER_TRANSITION_TIMEOUT_MS);
+			TCPCI_PARTNER_TRANSITION_TIMEOUT);
 	data->wait_for_ps_rdy = true;
 }
 
diff --git a/zephyr/emul/tcpc/emul_tcpci_partner_src.c b/zephyr/emul/tcpc/emul_tcpci_partner_src.c
index 504b05504f..9e2a90b227 100644
--- a/zephyr/emul/tcpc/emul_tcpci_partner_src.c
+++ b/zephyr/emul/tcpc/emul_tcpci_partner_src.c
@@ -15,6 +15,44 @@ LOG_MODULE_REGISTER(tcpci_src_emul, CONFIG_TCPCI_EMUL_LOG_LEVEL);
 #include "emul/tcpc/emul_tcpci.h"
 #include "usb_pd.h"
 
+/**
+ * @brief Start source capability timer. Capability message will be send after
+ *        @p time.
+ *
+ * @param data Pointer to USB-C source device emulator data
+ * @param time Time to delay before sending capability message
+ */
+static void tcpci_src_emul_start_source_capability_custom_time(
+	struct tcpci_src_emul_data *data, k_timeout_t time)
+{
+	/* Use reschedule */
+	k_work_reschedule(&data->source_capability_timeout, time);
+}
+
+/**
+ * @brief Start source capability timer. Capability message will be send after
+ *        TCPCI_SOURCE_CAPABILITY_TIMEOUT milliseconds.
+ *
+ * @param data Pointer to USB-C source device emulator data
+ */
+static void tcpci_src_emul_start_source_capability_timer(
+	struct tcpci_src_emul_data *data)
+{
+	tcpci_src_emul_start_source_capability_custom_time(
+			data, TCPCI_SOURCE_CAPABILITY_TIMEOUT);
+}
+
+/**
+ * @brief Stop source capability timer. Capability message will not be repeated.
+ *
+ * @param data Pointer to USB-C source device emulator data
+ */
+static void tcpci_src_emul_stop_source_capability_timer(
+	struct tcpci_src_emul_data *data)
+{
+	k_work_cancel_delayable(&data->source_capability_timeout);
+}
+
 /** Check description in emul_tcpci_partner_src.h */
 int tcpci_src_emul_send_capability_msg(struct tcpci_src_emul_data *data,
 				       struct tcpci_partner_data *common_data,
@@ -35,6 +73,34 @@ int tcpci_src_emul_send_capability_msg(struct tcpci_src_emul_data *data,
 }
 
 /** Check description in emul_tcpci_partner_src.h */
+int tcpci_src_emul_send_capability_msg_with_timer(
+	struct tcpci_src_emul_data *data,
+	struct tcpci_partner_data *common_data,
+	uint64_t delay)
+{
+	int ret;
+
+	if (delay > 0) {
+		tcpci_src_emul_start_source_capability_custom_time(
+							data, K_MSEC(delay));
+		return TCPCI_EMUL_TX_SUCCESS;
+	}
+
+	ret = tcpci_src_emul_send_capability_msg(data, common_data, 0);
+
+	if (ret != 0) {
+		tcpci_src_emul_start_source_capability_timer(data);
+	}
+	/*
+	 * If sending message was successful, SenderResponse timer should be
+	 * started. However this will break emulation until RECEIVE_DETECT
+	 * register is properly implemented in TCPCI emulator.
+	 */
+
+	return TCPCI_EMUL_TX_SUCCESS;
+}
+
+/** Check description in emul_tcpci_partner_src.h */
 enum tcpci_partner_handler_res tcpci_src_emul_handle_sop_msg(
 	struct tcpci_src_emul_data *data,
 	struct tcpci_partner_data *common_data,
@@ -48,6 +114,8 @@ enum tcpci_partner_handler_res tcpci_src_emul_handle_sop_msg(
 		/* Handle data message */
 		switch (PD_HEADER_TYPE(header)) {
 		case PD_DATA_REQUEST:
+			tcpci_partner_stop_sender_response_timer(common_data);
+			/* TODO(b/224925855): Validate if request can be met */
 			tcpci_partner_send_control_msg(common_data,
 						       PD_CTRL_ACCEPT, 0);
 			/* PS ready after 15 ms */
@@ -65,9 +133,9 @@ enum tcpci_partner_handler_res tcpci_src_emul_handle_sop_msg(
 							   0);
 			return TCPCI_PARTNER_COMMON_MSG_HANDLED;
 		case PD_CTRL_SOFT_RESET:
-			/* Send capability after 15 ms to establish PD again */
-			tcpci_src_emul_send_capability_msg(data, common_data,
-							   15);
+			/* Send capability to establish PD again */
+			tcpci_src_emul_send_capability_msg_with_timer(
+							data, common_data, 0);
 			return TCPCI_PARTNER_COMMON_MSG_HANDLED;
 		default:
 			return TCPCI_PARTNER_COMMON_MSG_NOT_HANDLED;
@@ -75,14 +143,47 @@ enum tcpci_partner_handler_res tcpci_src_emul_handle_sop_msg(
 	}
 }
 
+/**
+ * @brief Handler for repeating SourceCapability message
+ *
+ * @param timer Pointer to timer which triggered timeout
+ */
+static void tcpci_src_emul_source_capability_timeout(struct k_work *work)
+{
+	struct k_work_delayable *dwork = k_work_delayable_from_work(work);
+	struct tcpci_src_emul_data *data =
+		CONTAINER_OF(dwork, struct tcpci_src_emul_data,
+			     source_capability_timeout);
+	struct tcpci_partner_data *common_data = data->common_data;
+
+	if (k_mutex_lock(&common_data->transmit_mutex, K_NO_WAIT) != 0) {
+		/*
+		 * Emulator is probably handling received message,
+		 * try later if timer wasn't stopped.
+		 */
+		k_work_submit(work);
+		return;
+	}
+
+	/* Make sure that timer isn't stopped */
+	if (k_work_busy_get(work) & K_WORK_CANCELING) {
+		k_mutex_unlock(&common_data->transmit_mutex);
+		return;
+	}
+
+	tcpci_src_emul_send_capability_msg_with_timer(data, common_data, 0);
+
+	k_mutex_unlock(&common_data->transmit_mutex);
+}
+
 /** Check description in emul_tcpci_partner_src.h */
 void tcpci_src_emul_hard_reset(void *data)
 {
 	struct tcpci_src_emul_data *src_emul_data = data;
 
-	/* Send capability after 15 ms to establish PD again */
-	tcpci_src_emul_send_capability_msg(src_emul_data,
-					   src_emul_data->common_data, 15);
+	/* Send capability to establish PD again */
+	tcpci_src_emul_send_capability_msg_with_timer(
+			src_emul_data, src_emul_data->common_data, 0);
 }
 
 /**
@@ -176,6 +277,12 @@ static void tcpci_src_emul_rx_consumed_op(
 	tcpci_partner_free_msg(msg);
 }
 
+/** Check description in emul_tcpci_partner_src.h */
+void tcpci_src_emul_disconnect(struct tcpci_src_emul_data *data)
+{
+	tcpci_src_emul_stop_source_capability_timer(data);
+}
+
 /**
  * @brief Function called when emulator is disconnected from TCPCI
  *
@@ -190,6 +297,7 @@ static void tcpci_src_emul_disconnect_op(
 		CONTAINER_OF(ops, struct tcpci_src_emul, ops);
 
 	tcpci_partner_common_disconnect(&src_emul->common_data);
+	tcpci_src_emul_disconnect(&src_emul->data);
 }
 
 /** Check description in emul_tcpci_partner_src.h */
@@ -209,8 +317,15 @@ int tcpci_src_emul_connect_to_tcpci(struct tcpci_src_emul_data *data,
 	}
 
 	common_data->tcpci_emul = tcpci_emul;
-
-	return tcpci_src_emul_send_capability_msg(data, common_data, 0);
+	/*
+	 * It is not required to wait on connection before sending source
+	 * capabilities, but it is permit. Timeout is obligatory for power swap.
+	 */
+	tcpci_src_emul_send_capability_msg_with_timer(
+					data, data->common_data,
+					TCPCI_SWAP_SOURCE_START_TIMEOUT_MS);
+
+	return 0;
 }
 
 #define PDO_FIXED_FLAGS_MASK						\
@@ -322,6 +437,8 @@ void tcpci_src_emul_init_data(struct tcpci_src_emul_data *data,
 		data->pdo[i] = 0;
 	}
 
+	k_work_init_delayable(&data->source_capability_timeout,
+			      tcpci_src_emul_source_capability_timeout);
 	data->common_data = common_data;
 }
 
diff --git a/zephyr/include/emul/tcpc/emul_tcpci_partner_common.h b/zephyr/include/emul/tcpc/emul_tcpci_partner_common.h
index 45e2edac6a..5b211e0a7c 100644
--- a/zephyr/include/emul/tcpc/emul_tcpci_partner_common.h
+++ b/zephyr/include/emul/tcpc/emul_tcpci_partner_common.h
@@ -28,9 +28,21 @@
  */
 
 /** Timeout for other side to respond to PD message */
-#define TCPCI_PARTNER_RESPONSE_TIMEOUT_MS	K_MSEC(30)
+#define TCPCI_PARTNER_RESPONSE_TIMEOUT_MS	30
+#define TCPCI_PARTNER_RESPONSE_TIMEOUT			\
+		K_MSEC(TCPCI_PARTNER_RESPONSE_TIMEOUT_MS)
 /** Timeout for source to transition to requested state after accept */
-#define TCPCI_PARTNER_TRANSITION_TIMEOUT_MS	K_MSEC(550)
+#define TCPCI_PARTNER_TRANSITION_TIMEOUT_MS	550
+#define TCPCI_PARTNER_TRANSITION_TIMEOUT		\
+		K_MSEC(TCPCI_PARTNER_TRANSITION_TIMEOUT_MS)
+/** Timeout for source to send capability again after failure */
+#define TCPCI_SOURCE_CAPABILITY_TIMEOUT_MS	150
+#define TCPCI_SOURCE_CAPABILITY_TIMEOUT			\
+		K_MSEC(TCPCI_SOURCE_CAPABILITY_TIMEOUT_MS)
+/** Timeout for source to send capability message after power swap */
+#define TCPCI_SWAP_SOURCE_START_TIMEOUT_MS	20
+#define TCPCI_SWAP_SOURCE_START_TIMEOUT			\
+		K_MSEC(TCPCI_SWAP_SOURCE_START_TIMEOUT_MS)
 
 /**
  * @brief Function type that is used by TCPCI partner emulator on hard reset
diff --git a/zephyr/include/emul/tcpc/emul_tcpci_partner_src.h b/zephyr/include/emul/tcpc/emul_tcpci_partner_src.h
index e530df5500..14b353b6c5 100644
--- a/zephyr/include/emul/tcpc/emul_tcpci_partner_src.h
+++ b/zephyr/include/emul/tcpc/emul_tcpci_partner_src.h
@@ -33,6 +33,8 @@ struct tcpci_src_emul_data {
 	uint32_t pdo[PDO_MAX_OBJECTS];
 	/** Pointer to common TCPCI partner data */
 	struct tcpci_partner_data *common_data;
+	/** Delayed work which is executed on SourceCapability timeout */
+	struct k_work_delayable source_capability_timeout;
 };
 
 /** Structure describing standalone source device emulator */
@@ -137,6 +139,26 @@ int tcpci_src_emul_send_capability_msg(struct tcpci_src_emul_data *data,
 				       uint64_t delay);
 
 /**
+ * @brief Send capability message constructed from source device emulator PDOs.
+ *        SourceCapability timer is started when message wasn't send
+ *        successfully. Emulator will try to send source capability message
+ *        again on timeout. Otherwise SenderResponse timer is started and
+ *        emulator will wait for Request message.
+ *
+ * @param data Pointer to USB-C source device emulator data
+ * @param common_data Pointer to common TCPCI partner data
+ * @param delay Optional delay
+ *
+ * @return TCPCI_EMUL_TX_SUCCESS on success
+ * @return -ENOMEM when there is no free memory for message
+ * @return -EINVAL on TCPCI emulator add RX message error
+ */
+int tcpci_src_emul_send_capability_msg_with_timer(
+	struct tcpci_src_emul_data *data,
+	struct tcpci_partner_data *common_data,
+	uint64_t delay);
+
+/**
  * @brief Handle SOP messages as TCPCI source device. It handles request,
  *        get source cap and soft reset messages.
  *
@@ -161,6 +183,13 @@ enum tcpci_partner_handler_res tcpci_src_emul_handle_sop_msg(
 void tcpci_src_emul_hard_reset(void *data);
 
 /**
+ * @brief Disable source capabilities timer on disconnect
+ *
+ * @param data Pointer to USB-C source device emulator data
+ */
+void tcpci_src_emul_disconnect(struct tcpci_src_emul_data *data);
+
+/**
  * @}
  */