usb: update state machine framework

- OBJ is renamed to context (CTX) for current and last state
 - State definition now includes its parent (no need for the boiler
plate function that takes in a signal)
 - The init_state, set_state, and exe_state have been re-written to take
advantages of new state machine definition. I had to add more logic to
handle hierarchical states fully.
 - Explicitly create the USB states at the bottom of the file with all
of the statics. Don't need to use macros (even though I did suggest them)
 - Use NULL when we do_nothing instead of calling into a function
 - Created a "private" enum in the C file that lists all of the states
in the file, that we can use to refer to a state (it is also the
index into the states array for that state).
 - Changed prototype of state function to return void, since we aren't
really using the return value and it cleans up a lot of return 0 that
aren't needed.
 - Add const to int port since we can and should
 - Moves struct definition to implementation file only to keep
implementation details private. We can access data through accessor if
needed.

BRANCH=none
BUG=none
TEST=all unit tests passes

Change-Id: I482a63e08f7d63022d5102b891a2fac0b0faa46f
Signed-off-by: Jett Rink <jettrink@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1733744
Commit-Queue: Denis Brockus <dbrockus@chromium.org>
Reviewed-by: Denis Brockus <dbrockus@chromium.org>

20 files changed, 2221 insertions, 2917 deletions

diff --git a/board/chocodile_vpdmcu/board.h b/board/chocodile_vpdmcu/board.h
index 850d1d0011..dc90b5f764 100644
--- a/board/chocodile_vpdmcu/board.h
+++ b/board/chocodile_vpdmcu/board.h
@@ -68,8 +68,6 @@
 #define CONFIG_VBOOT_HASH
 #define CONFIG_WATCHDOG
 #undef  CONFIG_WATCHDOG_HELP
-#undef  CONFIG_SM_NESTING_NUM
-#define CONFIG_SM_NESTING_NUM 3
 
 #define CONFIG_USB_PID 0x5036
 #define VPD_HW_VERSION 0x0001
diff --git a/common/usbc/usb_pe_ctvpd_sm.c b/common/usbc/usb_pe_ctvpd_sm.c
index 57c5663e73..ab62a5bc6f 100644
--- a/common/usbc/usb_pe_ctvpd_sm.c
+++ b/common/usbc/usb_pe_ctvpd_sm.c
@@ -25,26 +25,32 @@
  * implement a VCONN and Charge-Through VCONN Powered Device.
  */
 static struct policy_engine {
-	/*
-	 * struct sm_obj must be first. This is the state machine
-	 * object that keeps track of the current and last state
-	 * of the state machine.
-	 */
-	struct sm_obj obj;
+	/* state machine context */
+	struct sm_ctx ctx;
 	/* port flags, see PE_FLAGS_* */
 	uint32_t flags;
 } pe[CONFIG_USB_PD_PORT_COUNT];
 
-/* Policy Engine states */
-DECLARE_STATE(pe, request, WITH_RUN, NOOP);
+/* List of all policy-engine-level states */
+enum usb_pe_state {
+	PE_REQUEST,
+};
+
+/* Forward declare the full list of states. This is indexed by usb_pe_states */
+static const struct usb_state pe_states[];
+
+static void set_state_pe(const int port, enum usb_pe_state new_state)
+{
+	set_state(port, &pe[port].ctx, &pe_states[new_state]);
+}
 
 void pe_init(int port)
 {
 	pe[port].flags = 0;
-	sm_init_state(port, PE_OBJ(port), pe_request);
+	set_state_pe(port, PE_REQUEST);
 }
 
-void usbc_policy_engine(int port, int evt, int en)
+void pe_run(int port, int evt, int en)
 {
 	static enum sm_local_state local_state[CONFIG_USB_PD_PORT_COUNT];
 
@@ -55,17 +61,19 @@ void usbc_policy_engine(int port, int evt, int en)
 		/* fall through */
 	case SM_RUN:
 		if (!en) {
+			/* Exit all states and pause state machine. */
+			set_state(port, &pe[port].ctx, NULL);
 			local_state[port] = SM_PAUSED;
 			break;
 		}
 
-		sm_run_state_machine(port, PE_OBJ(port), SM_RUN_SIG);
+		exe_state(port, &pe[port].ctx);
 		break;
 	case SM_PAUSED:
 		if (en) {
 			/* Restart state machine right now. */
 			local_state[port] = SM_INIT;
-			usbc_policy_engine(port, evt, en);
+			pe_run(port, evt, en);
 		}
 		break;
 	}
@@ -102,20 +110,7 @@ void pe_message_sent(int port)
 	/* Do nothing */
 }
 
-static int pe_request(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*pe_request_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int pe_request_entry(int port)
-{
-	return 0;
-}
-
-static int pe_request_run(int port)
+static void pe_request_run(const int port)
 {
 	uint32_t *payload = (uint32_t *)emsg[port].buf;
 	uint32_t header = emsg[port].header;
@@ -130,16 +125,16 @@ static int pe_request_run(int port)
 		 */
 
 		if (PD_HEADER_TYPE(header) != PD_DATA_VENDOR_DEF)
-			return 0;
+			return;
 
 		if (PD_HEADER_CNT(header) == 0)
-			return 0;
+			return;
 
 		if (!PD_VDO_SVDM(vdo))
-			return 0;
+			return;
 
 		if (PD_VDO_CMD(vdo) != CMD_DISCOVER_IDENT)
-			return 0;
+			return;
 
 #ifdef CONFIG_USB_TYPEC_CTVPD
 		/*
@@ -199,6 +194,11 @@ static int pe_request_run(int port)
 		prl_send_data_msg(port, TCPC_TX_SOP_PRIME,
 					PD_DATA_VENDOR_DEF);
 	}
-
-	return 0;
 }
+
+/* All policy-engine-level states. */
+static const struct usb_state pe_states[] = {
+	[PE_REQUEST] = {
+		.run    = pe_request_run,
+	},
+};
diff --git a/common/usbc/usb_prl_sm.c b/common/usbc/usb_prl_sm.c
index e99a2e208a..5c0caeee21 100644
--- a/common/usbc/usb_prl_sm.c
+++ b/common/usbc/usb_prl_sm.c
@@ -48,24 +48,59 @@
 /* PD counter definitions */
 #define PD_MESSAGE_ID_COUNT 7
 
-#define RCH_OBJ(port)	(SM_OBJ(rch[port]))
-#define TCH_OBJ(port)	(SM_OBJ(tch[port]))
-#define PRL_TX_OBJ(port)   (SM_OBJ(prl_tx[port]))
-#define PRL_HR_OBJ(port)   (SM_OBJ(prl_hr[port]))
+static enum sm_local_state local_state[CONFIG_USB_PD_PORT_COUNT];
+
+/* Protocol Transmit States (Section 6.11.2.2) */
+enum usb_prl_tx_state {
+	PRL_TX_PHY_LAYER_RESET,
+	PRL_TX_WAIT_FOR_MESSAGE_REQUEST,
+	PRL_TX_LAYER_RESET_FOR_TRANSMIT,
+	PRL_TX_WAIT_FOR_PHY_RESPONSE,
+	PRL_TX_SRC_SOURCE_TX,
+	PRL_TX_SNK_START_AMS,
+	PRL_TX_SRC_PENDING,
+	PRL_TX_SNK_PENDING,
+	PRL_TX_DISCARD_MESSAGE,
+};
+
+/* Protocol Hard Reset States (Section 6.11.2.4) */
+enum usb_prl_hr_state {
+	PRL_HR_WAIT_FOR_REQUEST,
+	PRL_HR_RESET_LAYER,
+	PRL_HR_WAIT_FOR_PHY_HARD_RESET_COMPLETE,
+	PRL_HR_WAIT_FOR_PE_HARD_RESET_COMPLETE,
+};
+
+/* Chunked Rx states (Section 6.11.2.1.2) */
+enum usb_rch_state {
+	RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER,
+	RCH_PROCESSING_EXTENDED_MESSAGE,
+	RCH_REQUESTING_CHUNK,
+	RCH_WAITING_CHUNK,
+	RCH_REPORT_ERROR,
+};
+
+/* Chunked Tx states (Section 6.11.2.1.3) */
+enum usb_tch_state {
+	TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE,
+	TCH_WAIT_FOR_TRANSMISSION_COMPLETE,
+	TCH_CONSTRUCT_CHUNKED_MESSAGE,
+	TCH_SENDING_CHUNKED_MESSAGE,
+	TCH_WAIT_CHUNK_REQUEST,
+	TCH_MESSAGE_RECEIVED,
+};
+
+/* Forward declare full list of states. Index by above enums. */
+static const struct usb_state prl_tx_states[];
+static const struct usb_state prl_hr_states[];
+static const struct usb_state rch_states[];
+static const struct usb_state tch_states[];
 
-#define RCH_TEST_OBJ(port) (SM_OBJ(rch[(port)].obj))
-#define TCH_TEST_OBJ(port) (SM_OBJ(tch[(port)].obj))
-#define PRL_TX_TEST_OBJ(port) (SM_OBJ(prl_tx[(port)].obj))
-#define PRL_HR_TEST_OBJ(port) (SM_OBJ(prl_hr[(port)].obj))
-
-static enum sm_local_state local_state[CONFIG_USB_PD_PORT_COUNT] = {SM_INIT};
 
 /* Chunked Rx State Machine Object */
 static struct rx_chunked {
-	/* struct sm_obj must be first. */
-	struct sm_obj obj;
-	/* state id */
-	enum rch_state_id state_id;
+	/* state machine context */
+	struct sm_ctx ctx;
 	/* PRL_FLAGS */
 	uint32_t flags;
 	/* protocol timer */
@@ -74,10 +109,8 @@ static struct rx_chunked {
 
 /* Chunked Tx State Machine Object */
 static struct tx_chunked {
-	/* struct sm_obj must be first. */
-	struct sm_obj obj;
-	/* state id */
-	enum tch_state_id state_id;
+	/* state machine context */
+	struct sm_ctx ctx;
 	/* state machine flags */
 	uint32_t flags;
 	/* protocol timer */
@@ -92,10 +125,8 @@ static struct protocol_layer_rx {
 
 /* Message Transmission State Machine Object */
 static struct protocol_layer_tx {
-	/* struct sm_obj must be first. */
-	struct sm_obj obj;
-	/* state id */
-	enum prl_tx_state_id state_id;
+	/* state machine context */
+	struct sm_ctx ctx;
 	/* state machine flags */
 	uint32_t flags;
 	/* protocol timer */
@@ -114,10 +145,8 @@ static struct protocol_layer_tx {
 
 /* Hard Reset State Machine Object */
 static struct protocol_hard_reset {
-	/* struct sm_obj must be first. */
-	struct sm_obj obj;
-	/* state id */
-	enum prl_hr_state_id state_id;
+	/* state machine context */
+	struct sm_ctx ctx;
 	/* state machine flags */
 	uint32_t flags;
 	/* protocol timer */
@@ -128,7 +157,6 @@ static struct protocol_hard_reset {
 static struct pd_message {
 	/* message status flags */
 	uint32_t status_flags;
-
 	/* SOP* */
 	enum tcpm_transmit_type xmit_type;
 	/* type of message */
@@ -149,47 +177,61 @@ static struct pd_message {
 
 struct extended_msg emsg[CONFIG_USB_PD_PORT_COUNT];
 
-/* Protocol Layer States */
 /* Common Protocol Layer Message Transmission */
-static void  prl_tx_construct_message(int port);
+static void prl_tx_construct_message(int port);
+static void prl_rx_wait_for_phy_message(const int port, int evt);
 
-DECLARE_STATE(prl, tx_phy_layer_reset, WITH_RUN, NOOP);
-DECLARE_STATE(prl, tx_wait_for_message_request, WITH_RUN, NOOP);
-DECLARE_STATE(prl, tx_layer_reset_for_transmit, WITH_RUN, NOOP);
-DECLARE_STATE(prl, tx_wait_for_phy_response, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(prl, tx_src_source_tx, WITH_RUN, NOOP);
-DECLARE_STATE(prl, tx_snk_start_ams, WITH_RUN, NOOP);
+/* Set the protocol transmit statemachine to a new state. */
+static void set_state_prl_tx(const int port,
+			     const enum usb_prl_tx_state new_state)
+{
+	set_state(port, &prl_tx[port].ctx, &prl_tx_states[new_state]);
+}
 
-/* Source Protocol Layser Message Transmission */
-DECLARE_STATE(prl, tx_src_pending, WITH_RUN, NOOP);
+/* Get the protocol transmit statemachine's current state. */
+test_export_static enum usb_prl_tx_state prl_tx_get_state(const int port)
+{
+	return prl_tx[port].ctx.current - &prl_tx_states[0];
+}
+
+/* Set the hard reset statemachine to a new state. */
+static void set_state_prl_hr(const int port,
+			     const enum usb_prl_hr_state new_state)
+{
+	set_state(port, &prl_hr[port].ctx, &prl_hr_states[new_state]);
+}
 
-/* Sink Protocol Layer Message Transmission */
-DECLARE_STATE(prl, tx_snk_pending, WITH_RUN, NOOP);
-DECLARE_STATE(prl, tx_discard_message, WITH_RUN, NOOP);
+#ifdef TEST_BUILD
+/* Get the hard reset statemachine's current state. */
+enum usb_prl_hr_state prl_hr_get_state(const int port)
+{
+	return prl_hr[port].ctx.current - &prl_hr_states[0];
+}
+#endif
 
-/* Protocol Layer Message Reception */
-static int prl_rx_wait_for_phy_message(int port, int evt);
+/* Set the chunked Rx statemachine to a new state. */
+static void set_state_rch(const int port, const enum usb_rch_state new_state)
+{
+	set_state(port, &rch[port].ctx, &rch_states[new_state]);
+}
 
-/* Hard Reset Operation */
-DECLARE_STATE(prl, hr_wait_for_request, WITH_RUN, NOOP);
-DECLARE_STATE(prl, hr_reset_layer, WITH_RUN, NOOP);
-DECLARE_STATE(prl, hr_wait_for_phy_hard_reset_complete, WITH_RUN, NOOP);
-DECLARE_STATE(prl, hr_wait_for_pe_hard_reset_complete, WITH_RUN, WITH_EXIT);
-
-/* Chunked Rx */
-DECLARE_STATE(rch, wait_for_message_from_protocol_layer, WITH_RUN, NOOP);
-DECLARE_STATE(rch, processing_extended_message, WITH_RUN, NOOP);
-DECLARE_STATE(rch, requesting_chunk, WITH_RUN, NOOP);
-DECLARE_STATE(rch, waiting_chunk, WITH_RUN, NOOP);
-DECLARE_STATE(rch, report_error, WITH_RUN, NOOP);
-
-/* Chunked Tx */
-DECLARE_STATE(tch, wait_for_message_request_from_pe, WITH_RUN, NOOP);
-DECLARE_STATE(tch, wait_for_transmission_complete, WITH_RUN, NOOP);
-DECLARE_STATE(tch, construct_chunked_message, WITH_RUN, NOOP);
-DECLARE_STATE(tch, sending_chunked_message, WITH_RUN, NOOP);
-DECLARE_STATE(tch, wait_chunk_request, WITH_RUN, NOOP);
-DECLARE_STATE(tch, message_received, WITH_RUN, NOOP);
+/* Get the chunked Rx statemachine's current state. */
+test_export_static enum usb_rch_state rch_get_state(const int port)
+{
+	return rch[port].ctx.current - &rch_states[0];
+}
+
+/* Set the chunked Tx statemachine to a new state. */
+static void set_state_tch(const int port, const enum usb_tch_state new_state)
+{
+	set_state(port, &tch[port].ctx, &tch_states[new_state]);
+}
+
+/* Get the chunked Tx statemachine's current state. */
+test_export_static enum usb_tch_state tch_get_state(const int port)
+{
+	return tch[port].ctx.current - &tch_states[0];
+}
 
 void pd_transmit_complete(int port, int status)
 {
@@ -203,22 +245,22 @@ void pd_execute_hard_reset(int port)
 		return;
 
 	prl_hr[port].flags |= PRL_FLAGS_PORT_PARTNER_HARD_RESET;
-	sm_set_state(port, PRL_HR_OBJ(port), prl_hr_reset_layer);
+	set_state_prl_hr(port, PRL_HR_RESET_LAYER);
 	task_set_event(PD_PORT_TO_TASK_ID(port), PD_EVENT_SM, 0);
 }
 
 void prl_execute_hard_reset(int port)
 {
 	/* Only allow async. function calls when state machine is running */
-	if (local_state[port] !=  SM_RUN)
+	if (local_state[port] != SM_RUN)
 		return;
 
 	prl_hr[port].flags |= PRL_FLAGS_PE_HARD_RESET;
-	sm_set_state(port, PRL_HR_OBJ(port), prl_hr_reset_layer);
+	set_state_prl_hr(port, PRL_HR_RESET_LAYER);
 	task_set_event(PD_PORT_TO_TASK_ID(port), PD_EVENT_SM, 0);
 }
 
-void prl_init(int port)
+static void prl_init(int port)
 {
 	int i;
 
@@ -243,32 +285,10 @@ void prl_init(int port)
 		prl_tx[port].msg_id_counter[i] = 0;
 	}
 
-	sm_init_state(port, PRL_TX_OBJ(port), prl_tx_phy_layer_reset);
-	sm_init_state(port, RCH_OBJ(port),
-				rch_wait_for_message_from_protocol_layer);
-	sm_init_state(port, TCH_OBJ(port),
-				tch_wait_for_message_request_from_pe);
-	sm_init_state(port, PRL_HR_OBJ(port), prl_hr_wait_for_request);
-}
-
-enum rch_state_id get_rch_state_id(int port)
-{
-	return rch[port].state_id;
-}
-
-enum tch_state_id get_tch_state_id(int port)
-{
-	return tch[port].state_id;
-}
-
-enum prl_tx_state_id get_prl_tx_state_id(int port)
-{
-	return prl_tx[port].state_id;
-}
-
-enum prl_hr_state_id get_prl_hr_state_id(int port)
-{
-	return prl_hr[port].state_id;
+	set_state_prl_tx(port, PRL_TX_PHY_LAYER_RESET);
+	set_state_rch(port, RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER);
+	set_state_tch(port, TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
+	set_state_prl_hr(port, PRL_HR_WAIT_FOR_REQUEST);
 }
 
 void prl_start_ams(int port)
@@ -324,12 +344,7 @@ void prl_send_ext_data_msg(int port,
 	task_set_event(PD_PORT_TO_TASK_ID(port), PD_EVENT_SM, 0);
 }
 
-void prl_reset(int port)
-{
-	local_state[port] = SM_INIT;
-}
-
-void usbc_protocol_layer(int port, int evt, int en)
+void prl_run(int port, int evt, int en)
 {
 	switch (local_state[port]) {
 	case SM_INIT:
@@ -338,14 +353,23 @@ void usbc_protocol_layer(int port, int evt, int en)
 		/* fall through */
 	case SM_RUN:
 		/* If disabling, wait until message is sent. */
-		if (!en && tch[port].state_id ==
-					TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE) {
+		if (!en && tch_get_state(port) ==
+				   TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE) {
 			/* Disable RX */
 #if defined(CONFIG_USB_TYPEC_CTVPD) || defined(CONFIG_USB_TYPEC_VPD)
 			vpd_rx_enable(0);
 #else
 			tcpm_set_rx_enable(port, 0);
 #endif
+			/*
+			 * While we are paused, exit all states and wait until
+			 * initialized again.
+			 */
+			set_state(port, &prl_tx[port].ctx, NULL);
+			set_state(port, &rch[port].ctx, NULL);
+			set_state(port, &tch[port].ctx, NULL);
+			set_state(port, &prl_hr[port].ctx, NULL);
+
 			local_state[port] = SM_PAUSED;
 			break;
 		}
@@ -354,31 +378,26 @@ void usbc_protocol_layer(int port, int evt, int en)
 		prl_rx_wait_for_phy_message(port, evt);
 
 		/* Run RX Chunked state machine */
-		sm_run_state_machine(port, RCH_OBJ(port), SM_RUN_SIG);
+		exe_state(port, &rch[port].ctx);
 
 		/* Run TX Chunked state machine */
-		sm_run_state_machine(port, TCH_OBJ(port), SM_RUN_SIG);
+		exe_state(port, &tch[port].ctx);
 
 		/* Run Protocol Layer Message Transmission state machine */
-		sm_run_state_machine(port, PRL_TX_OBJ(port), SM_RUN_SIG);
+		exe_state(port, &prl_tx[port].ctx);
 
 		/* Run Protocol Layer Hard Reset state machine */
-		sm_run_state_machine(port, PRL_HR_OBJ(port), SM_RUN_SIG);
+		exe_state(port, &prl_hr[port].ctx);
 		break;
 	case SM_PAUSED:
 		if (en) {
 			local_state[port] = SM_INIT;
-			usbc_protocol_layer(port, evt, en);
+			prl_run(port, evt, en);
 		}
 		break;
 	}
 }
 
-enum sm_local_state prl_get_local_state(int port)
-{
-	return local_state[port];
-}
-
 void prl_set_rev(int port, enum pd_rev_type rev)
 {
 	pdmsg[port].rev = rev;
@@ -390,54 +409,29 @@ enum pd_rev_type prl_get_rev(int port)
 }
 
 /* Common Protocol Layer Message Transmission */
-static int prl_tx_phy_layer_reset(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*prl_tx_phy_layer_reset_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_tx_phy_layer_reset_entry(int port)
+static void prl_tx_phy_layer_reset_entry(const int port)
 {
-	prl_tx[port].state_id = PRL_TX_PHY_LAYER_RESET;
-
-#if defined(CONFIG_USB_TYPEC_CTVPD) || defined(CONFIG_USB_TYPEC_VPD)
-	vpd_rx_enable(1);
-#else
-	tcpm_init(port);
-	tcpm_set_rx_enable(port, 1);
-#endif
-
-	return 0;
-}
-
-static int prl_tx_phy_layer_reset_run(int port)
-{
-	sm_set_state(port, PRL_TX_OBJ(port),
-				prl_tx_wait_for_message_request);
-	return 0;
+	if (IS_ENABLED(CONFIG_USB_TYPEC_CTVPD)
+	 || IS_ENABLED(CONFIG_USB_TYPEC_VPD)) {
+		vpd_rx_enable(1);
+	} else {
+		tcpm_init(port);
+		tcpm_set_rx_enable(port, 1);
+	}
 }
 
-static int prl_tx_wait_for_message_request(int port, enum sm_signal sig)
+static void prl_tx_phy_layer_reset_run(const int port)
 {
-	int ret;
-
-	ret = (*prl_tx_wait_for_message_request_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
+	set_state_prl_tx(port, PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 }
 
-static int prl_tx_wait_for_message_request_entry(int port)
+static void prl_tx_wait_for_message_request_entry(const int port)
 {
-	prl_tx[port].state_id = PRL_TX_WAIT_FOR_MESSAGE_REQUEST;
-
 	/* Reset RetryCounter */
 	prl_tx[port].retry_counter = 0;
-
-	return 0;
 }
 
-static int prl_tx_wait_for_message_request_run(int port)
+static void prl_tx_wait_for_message_request_run(const int port)
 {
 	if (prl_tx[port].flags & PRL_FLAGS_MSG_XMIT) {
 		prl_tx[port].flags &= ~PRL_FLAGS_MSG_XMIT;
@@ -446,8 +440,7 @@ static int prl_tx_wait_for_message_request_run(int port)
 		 */
 		if ((pdmsg[port].msg_type == PD_CTRL_SOFT_RESET) &&
 							(emsg[port].len == 0)) {
-			sm_set_state(port, PRL_TX_OBJ(port),
-					prl_tx_layer_reset_for_transmit);
+			set_state_prl_tx(port, PRL_TX_LAYER_RESET_FOR_TRANSMIT);
 		}
 		/*
 		 * Message pending (except Soft Reset)
@@ -455,11 +448,10 @@ static int prl_tx_wait_for_message_request_run(int port)
 		else {
 			/* NOTE: PRL_TX_Construct_Message State embedded here */
 			prl_tx_construct_message(port);
-			sm_set_state(port, PRL_TX_OBJ(port),
-						prl_tx_wait_for_phy_response);
+			set_state_prl_tx(port, PRL_TX_WAIT_FOR_PHY_RESPONSE);
 		}
 
-		return 0;
+		return;
 	} else if ((pdmsg[port].rev == PD_REV30) &&
 		(prl_tx[port].flags &
 				(PRL_FLAGS_START_AMS | PRL_FLAGS_END_AMS))) {
@@ -470,9 +462,8 @@ static int prl_tx_wait_for_message_request_run(int port)
 			 */
 			if (prl_tx[port].flags & PRL_FLAGS_START_AMS) {
 				prl_tx[port].flags &= ~PRL_FLAGS_START_AMS;
-				sm_set_state(port, PRL_TX_OBJ(port),
-						prl_tx_src_source_tx);
-				return 0;
+				set_state_prl_tx(port, PRL_TX_SRC_SOURCE_TX);
+				return;
 			}
 			/*
 			 * End of AMS notification received from
@@ -493,14 +484,11 @@ static int prl_tx_wait_for_message_request_run(int port)
 				 * First Message in AMS notification
 				 * received from Policy Engine.
 				 */
-				sm_set_state(port, PRL_TX_OBJ(port),
-							prl_tx_snk_start_ams);
-				return 0;
+				set_state_prl_tx(port, PRL_TX_SNK_START_AMS);
+				return;
 			}
 		}
 	}
-
-	return SM_RUN_SUPER;
 }
 
 static void increment_msgid_counter(int port)
@@ -513,123 +501,61 @@ static void increment_msgid_counter(int port)
 /*
  * PrlTxDiscard
  */
-static int prl_tx_discard_message(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*prl_tx_discard_message_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_tx_discard_message_entry(int port)
+static void prl_tx_discard_message_entry(const int port)
 {
-	prl_tx[port].state_id = PRL_TX_DISCARD_MESSAGE;
-
 	/* Increment msgidCounter */
 	increment_msgid_counter(port);
-	sm_set_state(port, PRL_TX_OBJ(port), prl_tx_phy_layer_reset);
-
-	return 0;
-}
-
-static int prl_tx_discard_message_run(int port)
-{
-	return SM_RUN_SUPER;
+	set_state_prl_tx(port, PRL_TX_PHY_LAYER_RESET);
 }
 
 /*
  * PrlTxSrcSourceTx
  */
-static int prl_tx_src_source_tx(int port, enum sm_signal sig)
+static void prl_tx_src_source_tx_entry(const int port)
 {
-	int ret;
-
-	ret = (*prl_tx_src_source_tx_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_tx_src_source_tx_entry(int port)
-{
-	prl_tx[port].state_id = PRL_TX_SRC_SOURCE_TX;
-
 	/* Set Rp = SinkTxNG */
 	tcpm_select_rp_value(port, SINK_TX_NG);
 	tcpm_set_cc(port, TYPEC_CC_RP);
-
-	return 0;
 }
 
-static int prl_tx_src_source_tx_run(int port)
+static void prl_tx_src_source_tx_run(const int port)
 {
 	if (prl_tx[port].flags & PRL_FLAGS_MSG_XMIT) {
 		prl_tx[port].flags &= ~PRL_FLAGS_MSG_XMIT;
 
-		sm_set_state(port, PRL_TX_OBJ(port), prl_tx_src_pending);
+		set_state_prl_tx(port, PRL_TX_SRC_PENDING);
 	}
-
-	return SM_RUN_SUPER;
 }
 
 /*
  * PrlTxSnkStartAms
  */
-static int prl_tx_snk_start_ams(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*prl_tx_snk_start_ams_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_tx_snk_start_ams_entry(int port)
-{
-	prl_tx[port].state_id = PRL_TX_SNK_START_OF_AMS;
-	return 0;
-}
-
-static int prl_tx_snk_start_ams_run(int port)
+static void prl_tx_snk_start_ams_run(const int port)
 {
 	if (prl_tx[port].flags & PRL_FLAGS_MSG_XMIT) {
 		prl_tx[port].flags &= ~PRL_FLAGS_MSG_XMIT;
 
-		sm_set_state(port, PRL_TX_OBJ(port), prl_tx_snk_pending);
-		return 0;
+		set_state_prl_tx(port, PRL_TX_SNK_PENDING);
 	}
-
-	return SM_RUN_SUPER;
 }
 
 /*
  * PrlTxLayerResetForTransmit
  */
-static int prl_tx_layer_reset_for_transmit(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*prl_tx_layer_reset_for_transmit_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_tx_layer_reset_for_transmit_entry(int port)
+static void prl_tx_layer_reset_for_transmit_entry(const int port)
 {
 	int i;
 
-	prl_tx[port].state_id = PRL_TX_LAYER_RESET_FOR_TRANSMIT;
-
 	/* Reset MessageIdCounters */
 	for (i = 0; i < NUM_XMIT_TYPES; i++)
 		prl_tx[port].msg_id_counter[i] = 0;
-
-	return 0;
 }
 
-static int prl_tx_layer_reset_for_transmit_run(int port)
+static void prl_tx_layer_reset_for_transmit_run(const int port)
 {
 	/* NOTE: PRL_Tx_Construct_Message State embedded here */
 	prl_tx_construct_message(port);
-	sm_set_state(port, PRL_TX_OBJ(port), prl_tx_wait_for_phy_response);
-
-	return 0;
+	set_state_prl_tx(port, PRL_TX_WAIT_FOR_PHY_RESPONSE);
 }
 
 static void prl_tx_construct_message(int port)
@@ -654,23 +580,12 @@ static void prl_tx_construct_message(int port)
 /*
  * PrlTxWaitForPhyResponse
  */
-static int prl_tx_wait_for_phy_response(int port, enum sm_signal sig)
+static void prl_tx_wait_for_phy_response_entry(const int port)
 {
-	int ret;
-
-	ret = (*prl_tx_wait_for_phy_response_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_tx_wait_for_phy_response_entry(int port)
-{
-	prl_tx[port].state_id = PRL_TX_WAIT_FOR_PHY_RESPONSE;
-
 	prl_tx[port].tcpc_tx_timeout = get_time().val + PD_T_TCPC_TX_TIMEOUT;
-	return 0;
 }
 
-static int prl_tx_wait_for_phy_response_run(int port)
+static void prl_tx_wait_for_phy_response_run(const int port)
 {
 	/* Wait until TX is complete */
 
@@ -711,15 +626,14 @@ static int prl_tx_wait_for_phy_response_run(int port)
 
 			/* Increment message id counter */
 			increment_msgid_counter(port);
-			sm_set_state(port, PRL_TX_OBJ(port),
-					prl_tx_wait_for_message_request);
-			return 0;
+			set_state_prl_tx(port, PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
+			return;
 		}
 
 		/* Try to resend the message. */
 		/* NOTE: PRL_TX_Construct_Message State embedded here. */
 		prl_tx_construct_message(port);
-		return 0;
+		return;
 	}
 
 	if (prl_tx[port].xmit_status == TCPC_TX_COMPLETE_SUCCESS) {
@@ -730,43 +644,27 @@ static int prl_tx_wait_for_phy_response_run(int port)
 		increment_msgid_counter(port);
 		/* Inform Policy Engine Message was sent */
 		pdmsg[port].status_flags |= PRL_FLAGS_TX_COMPLETE;
-		sm_set_state(port, PRL_TX_OBJ(port),
-			prl_tx_wait_for_message_request);
-		return 0;
+		set_state_prl_tx(port, PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
+		return;
 	}
-
-	return SM_RUN_SUPER;
 }
 
-static int prl_tx_wait_for_phy_response_exit(int port)
+static void prl_tx_wait_for_phy_response_exit(const int port)
 {
 	prl_tx[port].xmit_status = TCPC_TX_UNSET;
-	return 0;
 }
 
 /* Source Protocol Layer Message Transmission */
 /*
  * PrlTxSrcPending
  */
-static int prl_tx_src_pending(int port, enum sm_signal sig)
+static void prl_tx_src_pending_entry(const int port)
 {
-	int ret;
-
-	ret = (*prl_tx_src_pending_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_tx_src_pending_entry(int port)
-{
-	prl_tx[port].state_id = PRL_TX_SRC_PENDING;
-
 	/* Start SinkTxTimer */
 	prl_tx[port].sink_tx_timer = get_time().val + PD_T_SINK_TX;
-
-	return 0;
 }
 
-static int prl_tx_src_pending_run(int port)
+static void prl_tx_src_pending_run(const int port)
 {
 
 	if (get_time().val > prl_tx[port].sink_tx_timer) {
@@ -776,42 +674,24 @@ static int prl_tx_src_pending_run(int port)
 		 */
 		if ((emsg[port].len == 0) &&
 			(pdmsg[port].msg_type == PD_CTRL_SOFT_RESET)) {
-			sm_set_state(port, PRL_TX_OBJ(port),
-					prl_tx_layer_reset_for_transmit);
+			set_state_prl_tx(port, PRL_TX_LAYER_RESET_FOR_TRANSMIT);
 		}
 		/* Message pending (except Soft Reset) &
 		 * SinkTxTimer timeout
 		 */
 		else {
 			prl_tx_construct_message(port);
-			sm_set_state(port, PRL_TX_OBJ(port),
-					prl_tx_wait_for_phy_response);
+			set_state_prl_tx(port, PRL_TX_WAIT_FOR_PHY_RESPONSE);
 		}
 
-		return 0;
+		return;
 	}
-
-	return SM_RUN_SUPER;
 }
 
 /*
  * PrlTxSnkPending
  */
-static int prl_tx_snk_pending(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*prl_tx_snk_pending_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_tx_snk_pending_entry(int port)
-{
-	prl_tx[port].state_id = PRL_TX_SNK_PENDING;
-	return 0;
-}
-
-static int prl_tx_snk_pending_run(int port)
+static void prl_tx_snk_pending_run(const int port)
 {
 	enum tcpc_cc_voltage_status cc1, cc2;
 
@@ -823,8 +703,7 @@ static int prl_tx_snk_pending_run(int port)
 		 */
 		if ((pdmsg[port].msg_type == PD_CTRL_SOFT_RESET) &&
 					(emsg[port].len == 0)) {
-			sm_set_state(port, PRL_TX_OBJ(port),
-				prl_tx_layer_reset_for_transmit);
+			set_state_prl_tx(port, PRL_TX_LAYER_RESET_FOR_TRANSMIT);
 		}
 		/*
 		 * Message pending (except Soft Reset) &
@@ -832,60 +711,34 @@ static int prl_tx_snk_pending_run(int port)
 		 */
 		else {
 			prl_tx_construct_message(port);
-			sm_set_state(port, PRL_TX_OBJ(port),
-					prl_tx_wait_for_phy_response);
+			set_state_prl_tx(port, PRL_TX_WAIT_FOR_PHY_RESPONSE);
 		}
-		return 0;
+		return;
 	}
-
-	return SM_RUN_SUPER;
 }
 
 /* Hard Reset Operation */
 
-static int prl_hr_wait_for_request(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*prl_hr_wait_for_request_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_hr_wait_for_request_entry(int port)
+static void prl_hr_wait_for_request_entry(const int port)
 {
-	prl_hr[port].state_id = PRL_HR_WAIT_FOR_REQUEST;
-
 	prl_hr[port].flags = 0;
-	return 0;
 }
 
-static int prl_hr_wait_for_request_run(int port)
+static void prl_hr_wait_for_request_run(const int port)
 {
 	if (prl_hr[port].flags & PRL_FLAGS_PE_HARD_RESET ||
 		prl_hr[port].flags & PRL_FLAGS_PORT_PARTNER_HARD_RESET) {
-		sm_set_state(port, PRL_HR_OBJ(port), prl_hr_reset_layer);
+		set_state_prl_hr(port, PRL_HR_RESET_LAYER);
 	}
-
-	return 0;
 }
 
 /*
  * PrlHrResetLayer
  */
-static int prl_hr_reset_layer(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*prl_hr_reset_layer_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_hr_reset_layer_entry(int port)
+static void prl_hr_reset_layer_entry(const int port)
 {
 	int i;
 
-	prl_hr[port].state_id = PRL_HR_RESET_LAYER;
-
 	/* reset messageIDCounters */
 	for (i = 0; i < NUM_XMIT_TYPES; i++)
 		prl_tx[port].msg_id_counter[i] = 0;
@@ -893,13 +746,10 @@ static int prl_hr_reset_layer_entry(int port)
 	 * Protocol Layer message transmission transitions to
 	 * PRL_Tx_Wait_For_Message_Request state.
 	 */
-	sm_set_state(port, PRL_TX_OBJ(port),
-		prl_tx_wait_for_message_request);
-
-	return 0;
+	set_state_prl_tx(port, PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 }
 
-static int prl_hr_reset_layer_run(int port)
+static void prl_hr_reset_layer_run(const int port)
 {
 	/*
 	 * Protocol Layer reset Complete &
@@ -908,8 +758,7 @@ static int prl_hr_reset_layer_run(int port)
 	if (prl_hr[port].flags & PRL_FLAGS_PE_HARD_RESET) {
 		/* Request PHY to perform a Hard Reset */
 		prl_send_ctrl_msg(port, TCPC_TX_HARD_RESET, 0);
-		sm_set_state(port, PRL_HR_OBJ(port),
-			prl_hr_wait_for_phy_hard_reset_complete);
+		set_state_prl_hr(port, PRL_HR_WAIT_FOR_PHY_HARD_RESET_COMPLETE);
 	}
 	/*
 	 * Protocol Layer reset complete &
@@ -918,36 +767,21 @@ static int prl_hr_reset_layer_run(int port)
 	else {
 		/* Inform Policy Engine of the Hard Reset */
 		pe_got_hard_reset(port);
-		sm_set_state(port, PRL_HR_OBJ(port),
-			prl_hr_wait_for_pe_hard_reset_complete);
+		set_state_prl_hr(port, PRL_HR_WAIT_FOR_PE_HARD_RESET_COMPLETE);
 	}
-
-	return 0;
 }
 
 /*
  * PrlHrWaitForPhyHardResetComplete
  */
-static int prl_hr_wait_for_phy_hard_reset_complete(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*prl_hr_wait_for_phy_hard_reset_complete_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_hr_wait_for_phy_hard_reset_complete_entry(int port)
+static void prl_hr_wait_for_phy_hard_reset_complete_entry(const int port)
 {
-	prl_hr[port].state_id = PRL_HR_WAIT_FOR_PHY_HARD_RESET_COMPLETE;
-
 	/* Start HardResetCompleteTimer */
 	prl_hr[port].hard_reset_complete_timer =
 			get_time().val + PD_T_PS_HARD_RESET;
-
-	return 0;
 }
 
-static int prl_hr_wait_for_phy_hard_reset_complete_run(int port)
+static void prl_hr_wait_for_phy_hard_reset_complete_run(const int port)
 {
 	/*
 	 * Wait for hard reset from PHY
@@ -959,53 +793,31 @@ static int prl_hr_wait_for_phy_hard_reset_complete_run(int port)
 
 		/* Inform Policy Engine Hard Reset was sent */
 		pe_hard_reset_sent(port);
-		sm_set_state(port, PRL_HR_OBJ(port),
-			prl_hr_wait_for_pe_hard_reset_complete);
+		set_state_prl_hr(port, PRL_HR_WAIT_FOR_PE_HARD_RESET_COMPLETE);
 
-		return 0;
+		return;
 	}
-
-	return SM_RUN_SUPER;
 }
 
 /*
  * PrlHrWaitForPeHardResetComplete
  */
-static int prl_hr_wait_for_pe_hard_reset_complete(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*prl_hr_wait_for_pe_hard_reset_complete_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int prl_hr_wait_for_pe_hard_reset_complete_entry(int port)
-{
-	prl_hr[port].state_id = PRL_HR_WAIT_FOR_PE_HARD_RESET_COMPLETE;
-	return 0;
-}
-
-static int prl_hr_wait_for_pe_hard_reset_complete_run(int port)
+static void prl_hr_wait_for_pe_hard_reset_complete_run(const int port)
 {
 	/*
 	 * Wait for Hard Reset complete indication from Policy Engine
 	 */
 	if (prl_hr[port].flags & PRL_FLAGS_HARD_RESET_COMPLETE)
-		sm_set_state(port, PRL_HR_OBJ(port), prl_hr_wait_for_request);
-
-	return SM_RUN_SUPER;
+		set_state_prl_hr(port, PRL_HR_WAIT_FOR_REQUEST);
 }
 
-static int prl_hr_wait_for_pe_hard_reset_complete_exit(int port)
+static void prl_hr_wait_for_pe_hard_reset_complete_exit(const int port)
 {
 	/* Exit from Hard Reset */
 
-	sm_set_state(port, PRL_TX_OBJ(port), prl_tx_phy_layer_reset);
-	sm_set_state(port, RCH_OBJ(port),
-				rch_wait_for_message_from_protocol_layer);
-	sm_set_state(port, TCH_OBJ(port), tch_wait_for_message_request_from_pe);
-
-	return 0;
+	set_state_prl_tx(port, PRL_TX_PHY_LAYER_RESET);
+	set_state_rch(port, RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER);
+	set_state_tch(port, TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
 }
 
 static void copy_chunk_to_ext(int port)
@@ -1024,15 +836,6 @@ static void copy_chunk_to_ext(int port)
 /*
  * Chunked Rx State Machine
  */
-static int rch_wait_for_message_from_protocol_layer(int port,
-							enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*rch_wait_for_message_from_protocol_layer_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
 static inline void rch_clear_abort_set_chunking(int port)
 {
 	/* Clear Abort flag */
@@ -1042,14 +845,12 @@ static inline void rch_clear_abort_set_chunking(int port)
 	rch[port].flags = PRL_FLAGS_CHUNKING;
 }
 
-static int rch_wait_for_message_from_protocol_layer_entry(int port)
+static void rch_wait_for_message_from_protocol_layer_entry(const int port)
 {
-	rch[port].state_id = RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER;
 	rch_clear_abort_set_chunking(port);
-	return 0;
 }
 
-static int rch_wait_for_message_from_protocol_layer_run(int port)
+static void rch_wait_for_message_from_protocol_layer_run(const int port)
 {
 	if (rch[port].flags & PRL_FLAGS_MSG_RECEIVED) {
 		rch[port].flags &= ~PRL_FLAGS_MSG_RECEIVED;
@@ -1066,11 +867,10 @@ static int rch_wait_for_message_from_protocol_layer_run(int port)
 			 * Received Extended Message &
 			 * (Chunking = 1 & Chunked = 1)
 			 */
-			if ((rch[port].flags & PRL_FLAGS_CHUNKING) &&
-					chunked) {
-				sm_set_state(port, RCH_OBJ(port),
-				       rch_processing_extended_message);
-				return 0;
+			if ((rch[port].flags & PRL_FLAGS_CHUNKING) && chunked) {
+				set_state_rch(port,
+					      RCH_PROCESSING_EXTENDED_MESSAGE);
+				return;
 			}
 			/*
 			 * (Received Extended Message &
@@ -1089,9 +889,8 @@ static int rch_wait_for_message_from_protocol_layer_run(int port)
 			 * Chunked != Chunking
 			 */
 			else {
-				sm_set_state(port, RCH_OBJ(port),
-							rch_report_error);
-				return 0;
+				set_state_rch(port, RCH_REPORT_ERROR);
+				return;
 			}
 		}
 		/*
@@ -1110,34 +909,21 @@ static int rch_wait_for_message_from_protocol_layer_run(int port)
 		 * revision lower than PD3.0
 		 */
 		else {
-			sm_set_state(port, RCH_OBJ(port),
-						rch_report_error);
-			return 0;
+			set_state_rch(port, RCH_REPORT_ERROR);
+			return;
 		}
 	}
-
-	return SM_RUN_SUPER;
 }
 
 /*
  * RchProcessingExtendedMessage
  */
-static int rch_processing_extended_message(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*rch_processing_extended_message_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int rch_processing_extended_message_entry(int port)
+static void rch_processing_extended_message_entry(const int port)
 {
 	uint32_t header = emsg[port].header;
 	uint16_t exhdr = GET_EXT_HEADER(pdmsg[port].chk_buf[0]);
 	uint8_t chunk_num = PD_EXT_HEADER_CHUNK_NUM(exhdr);
 
-	rch[port].state_id = RCH_PROCESSING_EXTENDED_MESSAGE;
-
 	/*
 	 * If first chunk:
 	 *   Set Chunk_number_expected = 0 and
@@ -1148,11 +934,9 @@ static int rch_processing_extended_message_entry(int port)
 		pdmsg[port].num_bytes_received = 0;
 		pdmsg[port].msg_type = PD_HEADER_TYPE(header);
 	}
-
-	return 0;
 }
 
-static int rch_processing_extended_message_run(int port)
+static void rch_processing_extended_message_run(const int port)
 {
 	uint16_t exhdr = GET_EXT_HEADER(pdmsg[port].chk_buf[0]);
 	uint8_t chunk_num = PD_EXT_HEADER_CHUNK_NUM(exhdr);
@@ -1163,8 +947,7 @@ static int rch_processing_extended_message_run(int port)
 	 * Abort Flag Set
 	 */
 	if (pdmsg[port].status_flags & PRL_FLAGS_ABORT) {
-		sm_set_state(port, RCH_OBJ(port),
-			rch_wait_for_message_from_protocol_layer);
+		set_state_rch(port, RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER);
 	}
 	/*
 	 * If expected Chunk Number:
@@ -1181,8 +964,8 @@ static int rch_processing_extended_message_run(int port)
 		/* Make sure extended message buffer does not overflow */
 		if (pdmsg[port].num_bytes_received +
 					byte_num > EXTENDED_BUFFER_SIZE) {
-			sm_set_state(port, RCH_OBJ(port), rch_report_error);
-			return 0;
+			set_state_rch(port, RCH_REPORT_ERROR);
+			return;
 		}
 
 		/* Append data */
@@ -1200,39 +983,27 @@ static int rch_processing_extended_message_run(int port)
 			emsg[port].len = pdmsg[port].num_bytes_received;
 			 /* Pass Message to Policy Engine */
 			pe_pass_up_message(port);
-			sm_set_state(port, RCH_OBJ(port),
-			      rch_wait_for_message_from_protocol_layer);
+			set_state_rch(port,
+				      RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER);
 		}
 		/*
 		 * Message not Complete
 		 */
 		else
-			sm_set_state(port, RCH_OBJ(port), rch_requesting_chunk);
+			set_state_rch(port, RCH_REQUESTING_CHUNK);
 	}
 	/*
 	 * Unexpected Chunk Number
 	 */
 	else
-		sm_set_state(port, RCH_OBJ(port), rch_report_error);
-
-	return 0;
+		set_state_rch(port, RCH_REPORT_ERROR);
 }
 
 /*
  * RchRequestingChunk
  */
-static int rch_requesting_chunk(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*rch_requesting_chunk_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int rch_requesting_chunk_entry(int port)
+static void rch_requesting_chunk_entry(const int port)
 {
-	rch[port].state_id = RCH_REQUESTING_CHUNK;
-
 	/*
 	 * Send Chunk Request to Protocol Layer
 	 * with chunk number = Chunk_Number_Expected
@@ -1247,11 +1018,9 @@ static int rch_requesting_chunk_entry(int port)
 	pdmsg[port].ext = 1;
 	prl_tx[port].flags |= PRL_FLAGS_MSG_XMIT;
 	task_set_event(PD_PORT_TO_TASK_ID(port), PD_EVENT_TX, 0);
-
-	return 0;
 }
 
-static int rch_requesting_chunk_run(int port)
+static void rch_requesting_chunk_run(const int port)
 {
 	/*
 	 * Transmission Error from Protocol Layer or
@@ -1263,45 +1032,30 @@ static int rch_requesting_chunk_run(int port)
 		 * Leave PRL_FLAGS_MSG_RECEIVED flag set. It'll be
 		 * cleared in rch_report_error state
 		 */
-		sm_set_state(port, RCH_OBJ(port), rch_report_error);
+		set_state_rch(port, RCH_REPORT_ERROR);
 	}
 	/*
 	 * Message Transmitted received from Protocol Layer
 	 */
 	else if (pdmsg[port].status_flags & PRL_FLAGS_TX_COMPLETE) {
 		pdmsg[port].status_flags &= ~PRL_FLAGS_TX_COMPLETE;
-		sm_set_state(port, RCH_OBJ(port), rch_waiting_chunk);
-	} else
-		return SM_RUN_SUPER;
-
-	return 0;
+		set_state_rch(port, RCH_WAITING_CHUNK);
+	}
 }
 
 /*
  * RchWaitingChunk
  */
-static int rch_waiting_chunk(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*rch_waiting_chunk_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int rch_waiting_chunk_entry(int port)
+static void rch_waiting_chunk_entry(const int port)
 {
-	rch[port].state_id = RCH_WAITING_CHUNK;
-
 	/*
 	 * Start ChunkSenderResponseTimer
 	 */
 	rch[port].chunk_sender_response_timer =
 		get_time().val + PD_T_CHUNK_SENDER_RESPONSE;
-
-	return 0;
 }
 
-static int rch_waiting_chunk_run(int port)
+static void rch_waiting_chunk_run(const int port)
 {
 	if ((rch[port].flags & PRL_FLAGS_MSG_RECEIVED)) {
 		/*
@@ -1316,9 +1070,8 @@ static int rch_waiting_chunk_run(int port)
 			 * Other Message Received from Protocol Layer
 			 */
 			if (PD_EXT_HEADER_REQ_CHUNK(exhdr) ||
-					!PD_EXT_HEADER_CHUNKED(exhdr)) {
-				sm_set_state(port, RCH_OBJ(port),
-							rch_report_error);
+			    !PD_EXT_HEADER_CHUNKED(exhdr)) {
+				set_state_rch(port, RCH_REPORT_ERROR);
 			}
 			/*
 			 * Chunk response Received from Protocol Layer
@@ -1329,39 +1082,24 @@ static int rch_waiting_chunk_run(int port)
 				 * PRL_FLAGS_MSG_RECEIVED flag.
 				 */
 				rch[port].flags &= ~PRL_FLAGS_MSG_RECEIVED;
-				sm_set_state(port, RCH_OBJ(port),
-					rch_processing_extended_message);
+				set_state_rch(port,
+					      RCH_PROCESSING_EXTENDED_MESSAGE);
 			}
-
-			return 0;
 		}
 	}
 	/*
 	 * ChunkSenderResponseTimer Timeout
 	 */
 	else if (get_time().val > rch[port].chunk_sender_response_timer) {
-		sm_set_state(port, RCH_OBJ(port), rch_report_error);
-		return 0;
+		set_state_rch(port, RCH_REPORT_ERROR);
 	}
-
-	return SM_RUN_SUPER;
 }
 
 /*
  * RchReportError
  */
-static int rch_report_error(int port, enum sm_signal sig)
+static void rch_report_error_entry(const int port)
 {
-	int ret;
-
-	ret = (*rch_report_error_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int rch_report_error_entry(int port)
-{
-	rch[port].state_id = RCH_REPORT_ERROR;
-
 	/*
 	 * If the state was entered because a message was received,
 	 * this message is passed to the Policy Engine.
@@ -1379,29 +1117,16 @@ static int rch_report_error_entry(int port)
 		/* Report error */
 		pe_report_error(port, ERR_RCH_CHUNKED);
 	}
-
-	return 0;
 }
 
-static int rch_report_error_run(int port)
+static void rch_report_error_run(const int port)
 {
-	sm_set_state(port, RCH_OBJ(port),
-		rch_wait_for_message_from_protocol_layer);
-
-	return 0;
+	set_state_rch(port, RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER);
 }
 
 /*
  * Chunked Tx State Machine
  */
-static int tch_wait_for_message_request_from_pe(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tch_wait_for_message_request_from_pe_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
 static inline void tch_clear_abort_set_chunking(int port)
 {
 	/* Clear Abort flag */
@@ -1411,22 +1136,20 @@ static inline void tch_clear_abort_set_chunking(int port)
 	tch[port].flags = PRL_FLAGS_CHUNKING;
 }
 
-static int tch_wait_for_message_request_from_pe_entry(int port)
+static void tch_wait_for_message_request_from_pe_entry(const int port)
 {
-	tch[port].state_id = TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE;
 	tch_clear_abort_set_chunking(port);
-	return 0;
 }
 
-static int tch_wait_for_message_request_from_pe_run(int port)
+static void tch_wait_for_message_request_from_pe_run(const int port)
 {
 	/*
 	 * Any message received and not in state TCH_Wait_Chunk_Request
 	 */
 	if (tch[port].flags & PRL_FLAGS_MSG_RECEIVED) {
 		tch[port].flags &= ~PRL_FLAGS_MSG_RECEIVED;
-		sm_set_state(port, TCH_OBJ(port), tch_message_received);
-		return 0;
+		set_state_tch(port, TCH_MESSAGE_RECEIVED);
+		return;
 	} else if (tch[port].flags & PRL_FLAGS_MSG_XMIT) {
 		tch[port].flags &= ~PRL_FLAGS_MSG_XMIT;
 		/*
@@ -1435,7 +1158,7 @@ static int tch_wait_for_message_request_from_pe_run(int port)
 		 *
 		 * Discard the Message
 		 */
-		if (rch[port].state_id !=
+		if (rch_get_state(port) !=
 				RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER) {
 			/* Report Error To Policy Engine */
 			pe_report_error(port, ERR_TCH_XMIT);
@@ -1448,8 +1171,8 @@ static int tch_wait_for_message_request_from_pe_run(int port)
 			     (tch[port].flags & PRL_FLAGS_CHUNKING)) {
 				pdmsg[port].send_offset = 0;
 				pdmsg[port].chunk_number_to_send = 0;
-				sm_set_state(port, TCH_OBJ(port),
-					tch_construct_chunked_message);
+				set_state_tch(port,
+					      TCH_CONSTRUCT_CHUNKED_MESSAGE);
 			} else
 			/*
 			 * Non-Extended Message Request
@@ -1460,7 +1183,7 @@ static int tch_wait_for_message_request_from_pe_run(int port)
 					/* Report Error To Policy Engine */
 					pe_report_error(port, ERR_TCH_XMIT);
 					tch_clear_abort_set_chunking(port);
-					return 0;
+					return;
 				}
 
 				/* Copy message to chunked buffer */
@@ -1480,43 +1203,27 @@ static int tch_wait_for_message_request_from_pe_run(int port)
 						(emsg[port].len + 3) >> 2;
 				/* Pass Message to Protocol Layer */
 				prl_tx[port].flags |= PRL_FLAGS_MSG_XMIT;
-				sm_set_state(port, TCH_OBJ(port),
-					tch_wait_for_transmission_complete);
+				set_state_tch(port,
+					TCH_WAIT_FOR_TRANSMISSION_COMPLETE);
 			}
 
-			return 0;
+			return;
 		}
 	}
-
-	return SM_RUN_SUPER;
 }
 
 /*
  * TchWaitForTransmissionComplete
  */
-static int tch_wait_for_transmission_complete(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tch_wait_for_transmission_complete_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tch_wait_for_transmission_complete_entry(int port)
-{
-	tch[port].state_id = TCH_WAIT_FOR_TRANSMISSION_COMPLETE;
-	return 0;
-}
-
-static int tch_wait_for_transmission_complete_run(int port)
+static void tch_wait_for_transmission_complete_run(const int port)
 {
 	/*
 	 * Any message received and not in state TCH_Wait_Chunk_Request
 	 */
 	if (tch[port].flags & PRL_FLAGS_MSG_RECEIVED) {
 		tch[port].flags &= ~PRL_FLAGS_MSG_RECEIVED;
-		sm_set_state(port, TCH_OBJ(port), tch_message_received);
-		return 0;
+		set_state_tch(port, TCH_MESSAGE_RECEIVED);
+		return;
 	}
 
 	/*
@@ -1524,8 +1231,7 @@ static int tch_wait_for_transmission_complete_run(int port)
 	 */
 	if (pdmsg[port].status_flags & PRL_FLAGS_TX_COMPLETE) {
 		pdmsg[port].status_flags &= ~PRL_FLAGS_TX_COMPLETE;
-		sm_set_state(port, TCH_OBJ(port),
-			tch_wait_for_message_request_from_pe);
+		set_state_tch(port, TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
 
 		/* Tell PE message was sent */
 		pe_message_sent(port);
@@ -1537,39 +1243,26 @@ static int tch_wait_for_transmission_complete_run(int port)
 		pdmsg[port].status_flags &= ~PRL_FLAGS_TX_ERROR;
 		/* Tell PE an error occurred */
 		pe_report_error(port, ERR_TCH_XMIT);
-		sm_set_state(port, TCH_OBJ(port),
-			tch_wait_for_message_request_from_pe);
+		set_state_tch(port, TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
 	}
-
-	return 0;
 }
 
 /*
  * TchConstructChunkedMessage
  */
-static int tch_construct_chunked_message(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tch_construct_chunked_message_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tch_construct_chunked_message_entry(int port)
+static void tch_construct_chunked_message_entry(const int port)
 {
 	uint16_t *ext_hdr;
 	uint8_t *data;
 	uint16_t num;
 
-	tch[port].state_id = TCH_CONSTRUCT_CHUNKED_MESSAGE;
-
 	/*
 	 * Any message received and not in state TCH_Wait_Chunk_Request
 	 */
 	if (tch[port].flags & PRL_FLAGS_MSG_RECEIVED) {
 		tch[port].flags &= ~PRL_FLAGS_MSG_RECEIVED;
-		sm_set_state(port, TCH_OBJ(port), tch_message_received);
-		return 0;
+		set_state_tch(port, TCH_MESSAGE_RECEIVED);
+		return;
 	}
 	/* Prepare to copy chunk into chk_buf */
 
@@ -1603,47 +1296,28 @@ static int tch_construct_chunked_message_entry(int port)
 	/* Pass message chunk to Protocol Layer */
 	prl_tx[port].flags |= PRL_FLAGS_MSG_XMIT;
 	task_set_event(PD_PORT_TO_TASK_ID(port), PD_EVENT_SM, 0);
-
-	return 0;
 }
 
-static int tch_construct_chunked_message_run(int port)
+static void tch_construct_chunked_message_run(const int port)
 {
 	if (pdmsg[port].status_flags & PRL_FLAGS_ABORT)
-		sm_set_state(port, TCH_OBJ(port),
-			tch_wait_for_message_request_from_pe);
+		set_state_tch(port, TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
 	else
-		sm_set_state(port, TCH_OBJ(port),
-				tch_sending_chunked_message);
-	return 0;
+		set_state_tch(port, TCH_SENDING_CHUNKED_MESSAGE);
 }
 
 /*
  * TchSendingChunkedMessage
  */
-static int tch_sending_chunked_message(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tch_sending_chunked_message_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tch_sending_chunked_message_entry(int port)
-{
-	tch[port].state_id = TCH_SENDING_CHUNKED_MESSAGE;
-	return 0;
-}
-
-static int tch_sending_chunked_message_run(int port)
+static void tch_sending_chunked_message_run(const int port)
 {
 	/*
 	 * Any message received and not in state TCH_Wait_Chunk_Request
 	 */
 	if (tch[port].flags & PRL_FLAGS_MSG_RECEIVED) {
 		tch[port].flags &= ~PRL_FLAGS_MSG_RECEIVED;
-		sm_set_state(port, TCH_OBJ(port), tch_message_received);
-		return 0;
+		set_state_tch(port, TCH_MESSAGE_RECEIVED);
+		return;
 	}
 
 	/*
@@ -1651,16 +1325,14 @@ static int tch_sending_chunked_message_run(int port)
 	 */
 	if (pdmsg[port].status_flags & PRL_FLAGS_TX_ERROR) {
 		pe_report_error(port, ERR_TCH_XMIT);
-		sm_set_state(port, TCH_OBJ(port),
-			tch_wait_for_message_request_from_pe);
+		set_state_tch(port, TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
 	}
 	/*
 	 * Message Transmitted from Protocol Layer &
 	 * Last Chunk
 	 */
 	else if (emsg[port].len == pdmsg[port].send_offset) {
-		sm_set_state(port, TCH_OBJ(port),
-			tch_wait_for_message_request_from_pe);
+		set_state_tch(port, TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
 
 		/* Tell PE message was sent */
 		pe_message_sent(port);
@@ -1670,35 +1342,22 @@ static int tch_sending_chunked_message_run(int port)
 	 * Not Last Chunk
 	 */
 	else
-		sm_set_state(port, TCH_OBJ(port), tch_wait_chunk_request);
-
-	return 0;
+		set_state_tch(port, TCH_WAIT_CHUNK_REQUEST);
 }
 
 /*
  * TchWaitChunkRequest
  */
-static int tch_wait_chunk_request(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tch_wait_chunk_request_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tch_wait_chunk_request_entry(int port)
+static void tch_wait_chunk_request_entry(const int port)
 {
-	tch[port].state_id = TCH_WAIT_CHUNK_REQUEST;
-
 	/* Increment Chunk Number to Send */
 	pdmsg[port].chunk_number_to_send++;
 	/* Start Chunk Sender Request Timer */
 	tch[port].chunk_sender_request_timer =
 		get_time().val + PD_T_CHUNK_SENDER_REQUEST;
-	return 0;
 }
 
-static int tch_wait_chunk_request_run(int port)
+static void tch_wait_chunk_request_run(const int port)
 {
 	if (tch[port].flags & PRL_FLAGS_MSG_RECEIVED) {
 		tch[port].flags &= ~PRL_FLAGS_MSG_RECEIVED;
@@ -1713,9 +1372,9 @@ static int tch_wait_chunk_request_run(int port)
 				 * Chunk Number = Chunk Number to Send
 				 */
 				if (PD_EXT_HEADER_CHUNK_NUM(exthdr) ==
-				     pdmsg[port].chunk_number_to_send) {
-					sm_set_state(port, TCH_OBJ(port),
-					tch_construct_chunked_message);
+				    pdmsg[port].chunk_number_to_send) {
+					set_state_tch(port,
+						TCH_CONSTRUCT_CHUNKED_MESSAGE);
 				}
 				/*
 				 * Chunk Request Received &
@@ -1723,180 +1382,249 @@ static int tch_wait_chunk_request_run(int port)
 				 */
 				else {
 					pe_report_error(port, ERR_TCH_CHUNKED);
-					sm_set_state(port, TCH_OBJ(port),
-					tch_wait_for_message_request_from_pe);
+					set_state_tch(port,
+					  TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
 				}
-				return 0;
+				return;
 			}
 		}
 
 		/*
 		 * Other message received
 		 */
-		sm_set_state(port, TCH_OBJ(port), tch_message_received);
+		set_state_tch(port, TCH_MESSAGE_RECEIVED);
 	}
 	/*
 	 * ChunkSenderRequestTimer timeout
 	 */
 	else if (get_time().val >=
 			tch[port].chunk_sender_request_timer) {
-		sm_set_state(port, TCH_OBJ(port),
-			tch_wait_for_message_request_from_pe);
+		set_state_tch(port, TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
 
 		/* Tell PE message was sent */
 		pe_message_sent(port);
 	}
-
-	return 0;
 }
 
 /*
  * TchMessageReceived
  */
-static int tch_message_received(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tch_message_received_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tch_message_received_entry(int port)
+static void tch_message_received_entry(const int port)
 {
-	tch[port].state_id = TCH_MESSAGE_RECEIVED;
-
 	/* Pass message to chunked Rx */
 	rch[port].flags |= PRL_FLAGS_MSG_RECEIVED;
 	task_set_event(PD_PORT_TO_TASK_ID(port), PD_EVENT_SM, 0);
 
-	return 0;
 }
 
-static int tch_message_received_run(int port)
+static void tch_message_received_run(const int port)
 {
-	sm_set_state(port, TCH_OBJ(port),
-		tch_wait_for_message_request_from_pe);
-
-	return 0;
+	set_state_tch(port, TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
 }
 
 /*
  * Protocol Layer Message Reception State Machine
  */
-static int prl_rx_wait_for_phy_message(int port, int evt)
+static void prl_rx_wait_for_phy_message(const int port, int evt)
 {
 	uint32_t header;
 	uint8_t type;
 	uint8_t cnt;
 	uint8_t sop;
 	int8_t msid;
-	int ret;
-
-	/* process any potential incoming message */
-	if (tcpm_has_pending_message(port)) {
-		ret = tcpm_dequeue_message(port, pdmsg[port].chk_buf, &header);
-		if (ret == 0) {
-			emsg[port].header = header;
-			type = PD_HEADER_TYPE(header);
-			cnt = PD_HEADER_CNT(header);
-			msid = PD_HEADER_ID(header);
-			sop = PD_HEADER_GET_SOP(header);
-
-			if (cnt == 0 && type == PD_CTRL_SOFT_RESET) {
-				int i;
-
-				for (i = 0; i < NUM_XMIT_TYPES; i++) {
-					/* Clear MessageIdCounter */
-					prl_tx[port].msg_id_counter[i] = 0;
-					/* Clear stored MessageID value */
-					prl_rx[port].msg_id[i] = -1;
-				}
 
-				/* Inform Policy Engine of Soft Reset */
-				pe_got_soft_reset(port);
+	/* If we don't have any message, just stop processing now. */
+	if (!tcpm_has_pending_message(port) ||
+	    tcpm_dequeue_message(port, pdmsg[port].chk_buf, &header))
+		return;
 
-				/* Soft Reset occurred */
-				sm_set_state(port, PRL_TX_OBJ(port),
-						prl_tx_phy_layer_reset);
-				sm_set_state(port, RCH_OBJ(port),
-				rch_wait_for_message_from_protocol_layer);
-				sm_set_state(port, TCH_OBJ(port),
-				tch_wait_for_message_request_from_pe);
-			}
+	emsg[port].header = header;
+	type = PD_HEADER_TYPE(header);
+	cnt = PD_HEADER_CNT(header);
+	msid = PD_HEADER_ID(header);
+	sop = PD_HEADER_GET_SOP(header);
 
-			/*
-			 * Ignore if this is a duplicate message.
-			 */
-			if (prl_rx[port].msg_id[sop] != msid) {
-				/*
-				 * Discard any pending tx message if this is
-				 * not a ping message
-				 */
-				if ((pdmsg[port].rev == PD_REV30) &&
-					(cnt == 0) && type != PD_CTRL_PING) {
-					if (prl_tx[port].state_id ==
-						PRL_TX_SRC_PENDING ||
-						prl_tx[port].state_id ==
-						PRL_TX_SNK_PENDING) {
-						sm_set_state(port,
-							PRL_TX_OBJ(port),
-							prl_tx_discard_message);
-					}
-				}
+	if (cnt == 0 && type == PD_CTRL_SOFT_RESET) {
+		int i;
 
-				/* Store Message Id */
-				prl_rx[port].msg_id[sop] = msid;
+		for (i = 0; i < NUM_XMIT_TYPES; i++) {
+			/* Clear MessageIdCounter */
+			prl_tx[port].msg_id_counter[i] = 0;
+			/* Clear stored MessageID value */
+			prl_rx[port].msg_id[i] = -1;
+		}
 
-				/* RTR Chunked Message Router States. */
-				/*
-				 * Received Ping from Protocol Layer
-				 */
-				if (cnt == 0 && type == PD_CTRL_PING) {
-					/* NOTE: RTR_PING State embedded
-					 * here.
-					 */
-					emsg[port].len = 0;
-					pe_pass_up_message(port);
-					return 0;
-				}
-				/*
-				 * Message (not Ping) Received from
-				 * Protocol Layer & Doing Tx Chunks
-				 */
-				else if (tch[port].state_id !=
-					TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE) {
-					/* NOTE: RTR_TX_CHUNKS State embedded
-					 * here.
-					 */
-					/*
-					 * Send Message to Tx Chunk
-					 * Chunk State Machine
-					 */
-					tch[port].flags |=
-							PRL_FLAGS_MSG_RECEIVED;
-				}
-				/*
-				 * Message (not Ping) Received from
-				 * Protocol Layer & Not Doing Tx Chunks
-				 */
-				else {
-					/*
-					 * NOTE: RTR_RX_CHUNKS State embedded
-					 * here.
-					 */
-					/*
-					 * Send Message to Rx
-					 * Chunk State Machine
-					 */
-					rch[port].flags |=
-							PRL_FLAGS_MSG_RECEIVED;
-				}
+		/* Inform Policy Engine of Soft Reset */
+		pe_got_soft_reset(port);
 
-				task_set_event(PD_PORT_TO_TASK_ID(port),
-								PD_EVENT_SM, 0);
-			}
-		}
+		/* Soft Reset occurred */
+		set_state_prl_tx(port, PRL_TX_PHY_LAYER_RESET);
+		set_state_rch(port, RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER);
+		set_state_tch(port, TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
+	}
+
+	/*
+	 * Ignore if this is a duplicate message. Stop processing.
+	 */
+	if (prl_rx[port].msg_id[sop] == msid)
+		return;
+
+	/*
+	 * Discard any pending tx message if this is
+	 * not a ping message
+	 */
+	if ((pdmsg[port].rev == PD_REV30) &&
+	   (cnt == 0) && type != PD_CTRL_PING) {
+		if (prl_tx_get_state(port) == PRL_TX_SRC_PENDING ||
+		    prl_tx_get_state(port) == PRL_TX_SNK_PENDING)
+			set_state_prl_tx(port, PRL_TX_DISCARD_MESSAGE);
+	}
+
+	/* Store Message Id */
+	prl_rx[port].msg_id[sop] = msid;
+
+	/* RTR Chunked Message Router States. */
+	/*
+	 * Received Ping from Protocol Layer
+	 */
+	if (cnt == 0 && type == PD_CTRL_PING) {
+		/* NOTE: RTR_PING State embedded here. */
+		emsg[port].len = 0;
+		pe_pass_up_message(port);
+		return;
+	}
+	/*
+	 * Message (not Ping) Received from
+	 * Protocol Layer & Doing Tx Chunks
+	 */
+	else if (tch_get_state(port) != TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE) {
+		/* NOTE: RTR_TX_CHUNKS State embedded here. */
+		/*
+		 * Send Message to Tx Chunk
+		 * Chunk State Machine
+		 */
+		tch[port].flags |= PRL_FLAGS_MSG_RECEIVED;
+	}
+	/*
+	 * Message (not Ping) Received from
+	 * Protocol Layer & Not Doing Tx Chunks
+	 */
+	else {
+		/* NOTE: RTR_RX_CHUNKS State embedded here. */
+		/*
+		 * Send Message to Rx
+		 * Chunk State Machine
+		 */
+		rch[port].flags |= PRL_FLAGS_MSG_RECEIVED;
 	}
 
-	return 0;
+	task_set_event(PD_PORT_TO_TASK_ID(port), PD_EVENT_SM, 0);
 }
+
+/* All necessary Protocol Transmit States (Section 6.11.2.2) */
+static const struct usb_state prl_tx_states[] = {
+	[PRL_TX_PHY_LAYER_RESET] = {
+		.entry  = prl_tx_phy_layer_reset_entry,
+		.run    = prl_tx_phy_layer_reset_run,
+	},
+	[PRL_TX_WAIT_FOR_MESSAGE_REQUEST] = {
+		.entry  = prl_tx_wait_for_message_request_entry,
+		.run    = prl_tx_wait_for_message_request_run,
+	},
+	[PRL_TX_LAYER_RESET_FOR_TRANSMIT] = {
+		.entry  = prl_tx_layer_reset_for_transmit_entry,
+		.run    = prl_tx_layer_reset_for_transmit_run,
+	},
+	[PRL_TX_WAIT_FOR_PHY_RESPONSE] = {
+		.entry  = prl_tx_wait_for_phy_response_entry,
+		.run    = prl_tx_wait_for_phy_response_run,
+		.exit   = prl_tx_wait_for_phy_response_exit,
+	},
+	[PRL_TX_SRC_SOURCE_TX] = {
+		.entry  = prl_tx_src_source_tx_entry,
+		.run    = prl_tx_src_source_tx_run,
+	},
+	[PRL_TX_SNK_START_AMS] = {
+		.run    = prl_tx_snk_start_ams_run,
+	},
+	[PRL_TX_SRC_PENDING] = {
+		.entry  = prl_tx_src_pending_entry,
+		.run    = prl_tx_src_pending_run,
+	},
+	[PRL_TX_SNK_PENDING] = {
+		.run    = prl_tx_snk_pending_run,
+	},
+	[PRL_TX_DISCARD_MESSAGE] = {
+		.entry  = prl_tx_discard_message_entry,
+	},
+};
+
+/* All necessary Protocol Hard Reset States (Section 6.11.2.4) */
+static const struct usb_state prl_hr_states[] = {
+	[PRL_HR_WAIT_FOR_REQUEST] = {
+		.entry  = prl_hr_wait_for_request_entry,
+		.run    = prl_hr_wait_for_request_run,
+	},
+	[PRL_HR_RESET_LAYER] = {
+		.entry  = prl_hr_reset_layer_entry,
+		.run    = prl_hr_reset_layer_run,
+	},
+	[PRL_HR_WAIT_FOR_PHY_HARD_RESET_COMPLETE] = {
+		.entry  = prl_hr_wait_for_phy_hard_reset_complete_entry,
+		.run    = prl_hr_wait_for_phy_hard_reset_complete_run,
+	},
+	[PRL_HR_WAIT_FOR_PE_HARD_RESET_COMPLETE] = {
+		.run    = prl_hr_wait_for_pe_hard_reset_complete_run,
+		.exit   = prl_hr_wait_for_pe_hard_reset_complete_exit,
+	},
+};
+
+/* All necessary Chunked Rx states (Section 6.11.2.1.2) */
+static const struct usb_state rch_states[] = {
+	[RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER] = {
+		.entry  = rch_wait_for_message_from_protocol_layer_entry,
+		.run    = rch_wait_for_message_from_protocol_layer_run,
+	},
+	[RCH_PROCESSING_EXTENDED_MESSAGE] = {
+		.entry  = rch_processing_extended_message_entry,
+		.run    = rch_processing_extended_message_run,
+	},
+	[RCH_REQUESTING_CHUNK] = {
+		.entry  = rch_requesting_chunk_entry,
+		.run    = rch_requesting_chunk_run,
+	},
+	[RCH_WAITING_CHUNK] = {
+		.entry  = rch_waiting_chunk_entry,
+		.run    = rch_waiting_chunk_run,
+	},
+	[RCH_REPORT_ERROR] = {
+		.entry  = rch_report_error_entry,
+		.run    = rch_report_error_run,
+	},
+};
+
+/* All necessary Chunked Tx states (Section 6.11.2.1.3) */
+static const struct usb_state tch_states[] = {
+	[TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE] = {
+		.entry  = tch_wait_for_message_request_from_pe_entry,
+		.run    = tch_wait_for_message_request_from_pe_run,
+	},
+	[TCH_WAIT_FOR_TRANSMISSION_COMPLETE] = {
+		.run    = tch_wait_for_transmission_complete_run,
+	},
+	[TCH_CONSTRUCT_CHUNKED_MESSAGE] = {
+		.entry  = tch_construct_chunked_message_entry,
+		.run    = tch_construct_chunked_message_run,
+	},
+	[TCH_SENDING_CHUNKED_MESSAGE] = {
+		.run    = tch_sending_chunked_message_run,
+	},
+	[TCH_WAIT_CHUNK_REQUEST] = {
+		.entry  = tch_wait_chunk_request_entry,
+		.run    = tch_wait_chunk_request_run,
+	},
+	[TCH_MESSAGE_RECEIVED] = {
+		.entry  = tch_message_received_entry,
+		.run    = tch_message_received_run,
+	},
+};
diff --git a/common/usbc/usb_sm.c b/common/usbc/usb_sm.c
index 0260406450..3419bf5bb1 100644
--- a/common/usbc/usb_sm.c
+++ b/common/usbc/usb_sm.c
@@ -4,174 +4,191 @@
  */
 
 #include "common.h"
-#include "task.h"
+#include "console.h"
+#include "stdbool.h"
 #include "usb_pd.h"
 #include "usb_sm.h"
 #include "util.h"
-#include "console.h"
-
-void sm_init_state(int port, struct sm_obj *obj, sm_state target)
-{
-#if (CONFIG_SM_NESTING_NUM > 0)
-	int i;
 
-	sm_state tmp_super[CONFIG_SM_NESTING_NUM];
+#ifdef CONFIG_COMMON_RUNTIME
+#define CPRINTF(format, args...) cprintf(CC_USB, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_USB, format, ## args)
+#else /* CONFIG_COMMON_RUNTIME */
+#define CPRINTF(format, args...)
+#define CPRINTS(format, args...)
 #endif
 
-	obj->last_state = NULL;
-	obj->task_state = target;
+/* Private structure (to this file) used to track state machine context */
+struct internal_ctx {
+	usb_state_ptr last_entered;
+	uint32_t running : 1;
+	uint32_t enter	 : 1;
+	uint32_t exit	 : 1;
+};
+BUILD_ASSERT(sizeof(struct internal_ctx) ==
+	     member_size(struct sm_ctx, internal));
+
+/* Gets the first shared parent state between a and b (inclusive) */
+static usb_state_ptr shared_parent_state(usb_state_ptr a, usb_state_ptr b)
+{
+	const usb_state_ptr orig_b = b;
 
-#if (CONFIG_SM_NESTING_NUM > 0)
+	/* There are no common ancestors */
+	if (b == NULL)
+		return NULL;
 
-	/* Prepare to execute all entry actions of the target's super states */
+	/* This assumes that both A and B are NULL terminated without cycles */
+	while (a != NULL) {
+		/* We found a match return */
+		if (a == b)
+			return a;
 
-	/*
-	 * Get targets super state. This will be NULL if the target
-	 * has no super state
-	 */
-	tmp_super[CONFIG_SM_NESTING_NUM - 1] =
-			(sm_state)(uintptr_t)target(port, SM_SUPER_SIG);
-
-	/* Get all super states of the target */
-	for (i = CONFIG_SM_NESTING_NUM - 1; i > 0; i--) {
-		if (tmp_super[i] != NULL)
-			tmp_super[i - 1] =
-			(sm_state)(uintptr_t)tmp_super[i](port, SM_SUPER_SIG);
-		else
-			tmp_super[i - 1] = NULL;
+		/*
+		 * Otherwise, increment b down the list for comparison until we
+		 * run out, then increment a and start over on b for comparison
+		 */
+		if (b->parent == NULL) {
+			a = a->parent;
+			b = orig_b;
+		} else {
+			b = b->parent;
+		}
 	}
 
-	/* Execute all super state entry actions in forward order */
-	for (i = 0; i < CONFIG_SM_NESTING_NUM; i++)
-		if (tmp_super[i] != NULL)
-			tmp_super[i](port, SM_ENTRY_SIG);
-#endif
-
-	/* Now execute the target entry action */
-	target(port, SM_ENTRY_SIG);
+	return NULL;
 }
 
-int sm_set_state(int port, struct sm_obj *obj, sm_state target)
+/*
+ * Call all entry functions of parents before children. If set_state is called
+ * during one of the entry functions, then do not call any remaining entry
+ * functions.
+ */
+static void call_entry_functions(const int port,
+			       struct internal_ctx *const internal,
+			       const usb_state_ptr stop,
+			       const usb_state_ptr current)
 {
-#if (CONFIG_SM_NESTING_NUM > 0)
-	int i;
-	int no_execute;
+	if (current == stop)
+		return;
 
-	sm_state tmp_super[CONFIG_SM_NESTING_NUM];
-	sm_state target_super;
-	sm_state last_super;
-	sm_state super;
+	call_entry_functions(port, internal, stop, current->parent);
 
-	/* Execute all exit actions is reverse order */
+	/*
+	 * If the previous entry function called set_state, then don't enter
+	 * remaining states.
+	 */
+	if (!internal->enter)
+		return;
 
-	/* Get target's super state */
-	target_super = (sm_state)(uintptr_t)target(port, SM_SUPER_SIG);
-	tmp_super[0] = obj->task_state;
+	/* Track the latest state that was entered, so we can exit properly. */
+	internal->last_entered = current;
+	if (current->entry)
+		current->entry(port);
+}
 
-	do {
-		/* Execute exit action */
-		tmp_super[0](port, SM_EXIT_SIG);
+/*
+ * Call all exit functions of children before parents. Note set_state is ignored
+ * during an exit function.
+ */
+static void call_exit_functions(const int port, const usb_state_ptr stop,
+			      const usb_state_ptr current)
+{
+	if (current == stop)
+		return;
 
-		/* Get super state */
-		tmp_super[0] =
-			(sm_state)(uintptr_t)tmp_super[0](port, SM_SUPER_SIG);
-		/*
-		 * No need to execute a super state's exit action that has
-		 * shared ancestry with the target.
-		 */
-		super = target_super;
-		while (super != NULL) {
-			if (tmp_super[0] == super) {
-				tmp_super[0] = NULL;
-				break;
-			}
-
-			/* Get target state next super state if it exists */
-			super = (sm_state)(uintptr_t)super(port, SM_SUPER_SIG);
-		}
-	} while (tmp_super[0] != NULL);
+	if (current->exit)
+		current->exit(port);
 
-	/* All done executing the exit actions */
-#else
-	obj->task_state(port, SM_EXIT_SIG);
-#endif
-	/* update the state variables */
-	obj->last_state = obj->task_state;
-	obj->task_state = target;
-
-#if (CONFIG_SM_NESTING_NUM > 0)
-	/* Prepare to execute all entry actions of the target's super states */
-
-	tmp_super[CONFIG_SM_NESTING_NUM - 1] =
-				(sm_state)(uintptr_t)target(port, SM_SUPER_SIG);
-
-	/* Get all super states of the target */
-	for (i = CONFIG_SM_NESTING_NUM - 1; i > 0; i--) {
-		if (tmp_super[i] != NULL)
-			tmp_super[i - 1] =
-			(sm_state)(uintptr_t)tmp_super[i](port, SM_SUPER_SIG);
-		else
-			tmp_super[i - 1] = NULL;
+	call_exit_functions(port, stop, current->parent);
+}
+
+void set_state(const int port, struct sm_ctx *const ctx,
+	       const usb_state_ptr new_state)
+{
+	struct internal_ctx * const internal = (void *) ctx->internal;
+	usb_state_ptr last_state;
+	usb_state_ptr shared_parent;
+
+	/*
+	 * It does not make sense to call set_state in an exit phase of a state
+	 * since we are already in a transition; we would always ignore the
+	 * intended state to transition into.
+	 */
+	if (internal->exit) {
+		CPRINTF("C%d: Ignoring set state to 0x%08x within 0x%08x",
+			port, new_state, ctx->current);
+		return;
 	}
 
-	/* Get super state of last state */
-	last_super = (sm_state)(uintptr_t)obj->last_state(port, SM_SUPER_SIG);
+	/*
+	 * Determine the last state that was entered. Normally it is current,
+	 * but we could have called set_state within an entry phase, so we
+	 * shouldn't exit any states that weren't fully entered.
+	 */
+	last_state = internal->enter ? internal->last_entered : ctx->current;
 
-	/* Execute all super state entry actions in forward order */
-	for (i = 0; i < CONFIG_SM_NESTING_NUM; i++) {
-		/* No super state */
-		if (tmp_super[i] == NULL)
-			continue;
+	/* We don't exit and re-enter shared parent states */
+	shared_parent = shared_parent_state(last_state, new_state);
 
-		/*
-		 * We only want to execute the target state's super state entry
-		 * action if it doesn't share a super state with the previous
-		 * state.
-		 */
-		super = last_super;
-		no_execute = 0;
-		while (super != NULL) {
-			if (tmp_super[i] == super) {
-				no_execute = 1;
-				break;
-			}
-
-			/* Get last state's next super state if it exists */
-			super = (sm_state)(uintptr_t)super(port, SM_SUPER_SIG);
-		}
+	/*
+	 * Exit all of the non-common states from the last state.
+	 */
+	internal->exit = true;
+	call_exit_functions(port, shared_parent, last_state);
+	internal->exit = false;
 
-		/* Execute super state's entry */
-		if (!no_execute)
-			tmp_super[i](port, SM_ENTRY_SIG);
-	}
-#endif
+	ctx->previous = ctx->current;
+	ctx->current = new_state;
 
-	/* Now execute the target entry action */
-	target(port, SM_ENTRY_SIG);
+	/*
+	 * Enter all new non-common states. last_entered will contain the last
+	 * state that successfully entered before another set_state was called.
+	 */
+	internal->last_entered = NULL;
+	internal->enter = true;
+	call_entry_functions(port, internal, shared_parent, ctx->current);
+	/*
+	 * Setting enter to false ensures that all pending entry calls will be
+	 * skipped (in the case of a parent state calling set_state, which means
+	 * we should not enter any child states)
+	 */
+	internal->enter = false;
 
-	return 0;
+	/*
+	 * If we set_state while we are running a child state, then stop running
+	 * any remaining parent states.
+	 */
+	internal->running = false;
 }
 
-void sm_run_state_machine(int port, struct sm_obj *obj, enum sm_signal sig)
+/*
+ * Call all run functions of children before parents. If set_state is called
+ * during one of the entry functions, then do not call any remaining entry
+ * functions.
+ */
+static void call_run_functions(const int port,
+			     const struct internal_ctx *const internal,
+			     const usb_state_ptr current)
 {
-#if (CONFIG_SM_NESTING_NUM > 0)
-	sm_state state = obj->task_state;
-
-	do {
-		state = (sm_state)(uintptr_t)state(port, sig);
-	} while (state != NULL);
-#else
-	obj->task_state(port, sig);
-#endif
-}
+	if (!current)
+		return;
 
-int sm_do_nothing(int port)
-{
-	return 0;
+	/* If set_state is called during run, don't call remain functions. */
+	if (!internal->running)
+		return;
+
+	if (current->run)
+		current->run(port);
+
+	call_run_functions(port, internal, current->parent);
 }
 
-int sm_get_super_state(int port)
+void exe_state(const int port, struct sm_ctx *const ctx)
 {
-	return SM_RUN_SUPER;
+	struct internal_ctx * const internal = (void *) ctx->internal;
+
+	internal->running = true;
+	call_run_functions(port, internal, ctx->current);
+	internal->running = false;
 }
 
diff --git a/common/usbc/usb_tc_ctvpd_sm.c b/common/usbc/usb_tc_ctvpd_sm.c
index 447db85be5..f837b27dbc 100644
--- a/common/usbc/usb_tc_ctvpd_sm.c
+++ b/common/usbc/usb_tc_ctvpd_sm.c
@@ -9,7 +9,6 @@
 #include "task.h"
 #include "tcpm.h"
 #include "usb_pd.h"
-#include "usb_tc_ctvpd_sm.h"
 #include "usb_tc_sm.h"
 #include "vpd_api.h"
 
@@ -24,7 +23,7 @@
 #endif
 
 /* Type-C Layer Flags */
-#define TC_FLAGS_VCONN_ON           (1 << 0)
+#define TC_FLAGS_VCONN_ON           BIT(0)
 
 #define SUPPORT_TIMER_RESET_INIT     0
 #define SUPPORT_TIMER_RESET_REQUEST  1
@@ -34,56 +33,164 @@
  * This is the Type-C Port object that contains information needed to
  * implement a Charge Through VCONN Powered Device.
  */
-struct type_c tc[CONFIG_USB_PD_PORT_COUNT];
-
-/* Type-C states */
-DECLARE_STATE(tc, disabled, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(tc, error_recovery, WITH_RUN, NOOP);
-DECLARE_STATE(tc, unattached_snk, WITH_RUN, NOOP);
-DECLARE_STATE(tc, attach_wait_snk, WITH_RUN, NOOP);
-DECLARE_STATE(tc, attached_snk, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(tc, try_snk, WITH_RUN, NOOP);
-DECLARE_STATE(tc, unattached_src, WITH_RUN, NOOP);
-DECLARE_STATE(tc, attach_wait_src, WITH_RUN, NOOP);
-DECLARE_STATE(tc, try_wait_src, WITH_RUN, NOOP);
-DECLARE_STATE(tc, attached_src, WITH_RUN, NOOP);
-DECLARE_STATE(tc, ct_try_snk, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(tc, ct_attach_wait_unsupported, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(tc, ct_attached_unsupported, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(tc, ct_unattached_unsupported, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(tc, ct_unattached_vpd, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(tc, ct_disabled_vpd, WITH_RUN, NOOP);
-DECLARE_STATE(tc, ct_attached_vpd, WITH_RUN, NOOP);
-DECLARE_STATE(tc, ct_attach_wait_vpd, WITH_RUN, WITH_EXIT);
-
-/* Super States */
-DECLARE_STATE(tc, host_rard_ct_rd, NOOP, NOOP);
-DECLARE_STATE(tc, host_open_ct_open, NOOP, NOOP);
-DECLARE_STATE(tc, vbus_cc_iso, NOOP, NOOP);
-DECLARE_STATE(tc, host_rp3_ct_rd, NOOP, NOOP);
-DECLARE_STATE(tc, host_rp3_ct_rpu, NOOP, NOOP);
-DECLARE_STATE(tc, host_rpu_ct_rd, NOOP, NOOP);
+static struct type_c {
+	/* state machine context */
+	struct sm_ctx ctx;
+	/* current port power role (VPD, SOURCE or SINK) */
+	uint8_t power_role;
+	/* current port data role (DFP or UFP) */
+	uint8_t data_role;
+	/* Higher-level power deliver state machines are enabled if true. */
+	uint8_t pd_enable;
+	/* event timeout */
+	uint64_t evt_timeout;
+	/* port flags, see TC_FLAGS_* */
+	uint32_t flags;
+	/*
+	 * Time a charge-through port shall wait before it can determine it
+	 * is attached
+	 */
+	uint64_t cc_debounce;
+	/* Time a host port shall wait before it can determine it is attached */
+	uint64_t host_cc_debounce;
+	/* Time a Sink port shall wait before it can determine it is detached
+	 * due to the potential for USB PD signaling on CC as described in
+	 * the state definitions.
+	 */
+	uint64_t pd_debounce;
+	/* Maintains state of billboard device */
+	int billboard_presented;
+	/*
+	 * Time a port shall wait before it can determine it is
+	 * re-attached during the try-wait process.
+	 */
+	uint64_t try_wait_debounce;
+	/* charge-through support timer */
+	uint64_t support_timer;
+	/* reset the charge-through support timer */
+	uint8_t support_timer_reset;
+	/* VPD host port cc state */
+	enum pd_cc_states host_cc_state;
+	uint8_t ct_cc;
+	/* The cc state */
+	enum pd_cc_states cc_state;
+	uint64_t next_role_swap;
+} tc[CONFIG_USB_PD_PORT_COUNT];
+
+/* List of all TypeC-level states */
+enum usb_tc_state {
+	/* Normal States */
+	TC_DISABLED,
+	TC_UNATTACHED_SNK,
+	TC_ATTACH_WAIT_SNK,
+	TC_ATTACHED_SNK,
+	TC_ERROR_RECOVERY,
+	TC_TRY_SNK,
+	TC_UNATTACHED_SRC,
+	TC_ATTACH_WAIT_SRC,
+	TC_TRY_WAIT_SRC,
+	TC_ATTACHED_SRC,
+	TC_CT_TRY_SNK,
+	TC_CT_ATTACH_WAIT_UNSUPPORTED,
+	TC_CT_ATTACHED_UNSUPPORTED,
+	TC_CT_UNATTACHED_UNSUPPORTED,
+	TC_CT_UNATTACHED_VPD,
+	TC_CT_DISABLED_VPD,
+	TC_CT_ATTACHED_VPD,
+	TC_CT_ATTACH_WAIT_VPD,
+	/* Super States */
+	TC_VBUS_CC_ISO,
+	TC_HOST_RARD_CT_RD,
+	TC_HOST_OPEN_CT_OPEN,
+	TC_HOST_RP3_CT_RD,
+	TC_HOST_RP3_CT_RPU,
+	TC_HOST_RPU_CT_RD,
+};
+
+/* Forward declare the full list of states. This is indexed by usb_tc_state */
+static const struct usb_state tc_states[];
+
+
+#ifdef CONFIG_COMMON_RUNTIME
+/* List of human readable state names for console debugging */
+const char * const tc_state_names[] = {
+	[TC_DISABLED] = "Disabled",
+	[TC_UNATTACHED_SNK] = "Unattached.SNK",
+	[TC_ATTACH_WAIT_SNK] = "AttachWait.SNK",
+	[TC_ATTACHED_SNK] = "Attached.SNK",
+	[TC_ERROR_RECOVERY] = "ErrorRecovery",
+	[TC_TRY_SNK] = "Try.SNK",
+	[TC_UNATTACHED_SRC] = "Unattached.SRC",
+	[TC_ATTACH_WAIT_SRC] = "AttachWait.SRC",
+	[TC_TRY_WAIT_SRC] = "TryWait.SRC",
+	[TC_ATTACHED_SRC] = "Attached.SRC",
+	[TC_CT_TRY_SNK] = "CTTry.SNK",
+	[TC_CT_ATTACH_WAIT_UNSUPPORTED] = "CTAttachWait.Unsupported",
+	[TC_CT_ATTACHED_UNSUPPORTED] = "CTAttached.Unsupported",
+	[TC_CT_UNATTACHED_UNSUPPORTED] = "CTUnattached.Unsupported",
+	[TC_CT_UNATTACHED_VPD] = "CTUnattached.VPD",
+	[TC_CT_DISABLED_VPD] = "CTDisabled.VPD",
+	[TC_CT_ATTACHED_VPD] = "CTAttached.VPD",
+	[TC_CT_ATTACH_WAIT_VPD] = "CTAttachWait.VPD",
+};
+#endif
+
+/* Forward declare private, common functions */
+static void set_state_tc(const int port, enum usb_tc_state new_state);
+
+/* Public TypeC functions */
+
+int tc_get_power_role(int port)
+{
+	return tc[port].power_role;
+}
+
+int tc_get_data_role(int port)
+{
+	return tc[port].data_role;
+}
+
+uint8_t tc_get_polarity(int port)
+{
+	/* Does not track polarity */
+	return 0;
+}
+
+uint8_t tc_get_pd_enabled(int port)
+{
+	return tc[port].pd_enable;
+}
+
+void tc_set_power_role(int port, int role)
+{
+	tc[port].power_role = role;
+}
+
+uint64_t tc_get_timeout(int port)
+{
+	return tc[port].evt_timeout;
+}
+
+void tc_set_timeout(int port, uint64_t timeout)
+{
+	tc[port].evt_timeout = timeout;
+}
 
 void tc_reset_support_timer(int port)
 {
 	tc[port].support_timer_reset |= SUPPORT_TIMER_RESET_REQUEST;
 }
 
-void tc_state_init(int port, enum typec_state_id start_state)
+void tc_state_init(int port)
 {
 	int res = 0;
-	sm_state this_state;
 
 	res = tc_restart_tcpc(port);
-	if (res)
-		this_state = tc_disabled;
-	else
-		this_state = (start_state == TC_UNATTACHED_SRC) ?
-				tc_unattached_src : tc_unattached_snk;
 
 	CPRINTS("TCPC p%d init %s", port, res ? "failed" : "ready");
 
-	sm_init_state(port, TC_OBJ(port), this_state);
+	/* Disable if restart failed, otherwise start in default state. */
+	set_state_tc(port, res ? TC_DISABLED : TC_UNATTACHED_SNK);
 
 	/* Disable pd state machines */
 	tc[port].pd_enable = 0;
@@ -99,6 +206,36 @@ void tc_event_check(int port, int evt)
 	/* Do Nothing */
 }
 
+void tc_run(const int port)
+{
+	exe_state(port, &tc[port].ctx);
+}
+
+/* Internal Functions */
+
+/* Set the TypeC state machine to a new state. */
+static void set_state_tc(const int port, enum usb_tc_state new_state)
+{
+	set_state(port, &tc[port].ctx, &tc_states[new_state]);
+}
+
+/* Get the current TypeC state. */
+test_export_static enum usb_tc_state get_state_tc(const int port)
+{
+	return tc[port].ctx.current - &tc_states[0];
+}
+
+/* Get the previous TypeC state. */
+static enum usb_tc_state get_last_state_tc(const int port)
+{
+	return tc[port].ctx.previous - &tc_states[0];
+}
+
+static void print_current_state(const int port)
+{
+	CPRINTS("C%d: %s", port, tc_state_names[get_state_tc(port)]);
+}
+
 /**
  * Disabled
  *
@@ -108,38 +245,26 @@ void tc_event_check(int port, int evt)
  *   Remove the terminations from Host
  *   Remove the terminations from Charge-Through
  */
-static int tc_disabled(int port, enum sm_signal sig)
+static void tc_disabled_entry(const int port)
 {
-	int ret = 0;
-
-	ret = (*tc_disabled_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_open_ct_open);
+	print_current_state(port);
 }
 
-static int tc_disabled_entry(int port)
-{
-	tc[port].state_id = TC_DISABLED;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
-	return 0;
-}
-
-static int tc_disabled_run(int port)
+static void tc_disabled_run(const int port)
 {
 	task_wait_event(-1);
-	return SM_RUN_SUPER;
 }
 
-static int tc_disabled_exit(int port)
+static void tc_disabled_exit(const int port)
 {
-#ifndef CONFIG_USB_PD_TCPC
-	if (tc_restart_tcpc(port) != 0) {
-		CPRINTS("TCPC p%d restart failed!", port);
-		return 0;
+	if (!IS_ENABLED(CONFIG_USB_PD_TCPC)) {
+		if (tc_restart_tcpc(port) != 0) {
+			CPRINTS("TCPC p%d restart failed!", port);
+			return;
+		}
 	}
-#endif
-	CPRINTS("TCPC p%d resumed!", port);
 
-	return 0;
+	CPRINTS("TCPC p%d resumed!", port);
 }
 
 /**
@@ -151,29 +276,17 @@ static int tc_disabled_exit(int port)
  *   Remove the terminations from Host
  *   Remove the terminations from Charge-Through
  */
-static int tc_error_recovery(int port, enum sm_signal sig)
+static void tc_error_recovery_entry(const int port)
 {
-	int ret = 0;
-
-	ret = (*tc_error_recovery_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_open_ct_open);
-}
-
-static int tc_error_recovery_entry(int port)
-{
-	tc[port].state_id = TC_ERROR_RECOVERY;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 	/* Use cc_debounce state variable for error recovery timeout */
 	tc[port].cc_debounce = get_time().val + PD_T_ERROR_RECOVERY;
-	return 0;
 }
 
-static int tc_error_recovery_run(int port)
+static void tc_error_recovery_run(const int port)
 {
 	if (get_time().val > tc[port].cc_debounce)
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
-
-	return SM_RUN_SUPER;
+		set_state_tc(port, TC_UNATTACHED_SNK);
 }
 
 /**
@@ -185,27 +298,16 @@ static int tc_error_recovery_run(int port)
  *   Place Ra on VCONN and Rd on Host CC
  *   Place Rd on Charge-Through CCs
  */
-static int tc_unattached_snk(int port, enum sm_signal sig)
-{
-	int ret = 0;
-
-	ret = (*tc_unattached_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rard_ct_rd);
-}
-
-static int tc_unattached_snk_entry(int port)
+static void tc_unattached_snk_entry(const int port)
 {
-	tc[port].state_id = TC_UNATTACHED_SNK;
-	if (tc[port].obj.last_state != tc_unattached_src)
-		CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	if (get_last_state_tc(port) != TC_UNATTACHED_SRC)
+		print_current_state(port);
 
 	tc[port].flags &= ~TC_FLAGS_VCONN_ON;
 	tc[port].cc_state = PD_CC_UNSET;
-
-	return 0;
 }
 
-static int tc_unattached_snk_run(int port)
+static void tc_unattached_snk_run(const int port)
 {
 	int host_cc;
 	int new_cc_state;
@@ -220,8 +322,10 @@ static int tc_unattached_snk_run(int port)
 	 * detected, as indicated by the SNK.Rp state on its Host-side
 	 * port’s CC pin.
 	 */
-	if (cc_is_rp(host_cc))
-		return sm_set_state(port, TC_OBJ(port), tc_attach_wait_snk);
+	if (cc_is_rp(host_cc)) {
+		set_state_tc(port, TC_ATTACH_WAIT_SNK);
+		return;
+	}
 
 	/* Check Charge-Through CCs for connection */
 	vpd_ct_get_cc(&cc1, &cc2);
@@ -241,7 +345,7 @@ static int tc_unattached_snk_run(int port)
 
 	/* Wait for Charge-Through CC debounce */
 	if (get_time().val < tc[port].cc_debounce)
-		return 0;
+		return;
 
 	/*
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
@@ -253,10 +357,10 @@ static int tc_unattached_snk_run(int port)
 	 *   2) VBUS is detected
 	 */
 	if (vpd_is_ct_vbus_present() &&
-				tc[port].cc_state == PD_CC_DFP_ATTACHED)
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_src);
-
-	return SM_RUN_SUPER;
+				tc[port].cc_state == PD_CC_DFP_ATTACHED) {
+		set_state_tc(port, TC_UNATTACHED_SRC);
+		return;
+	}
 }
 
 /**
@@ -268,24 +372,13 @@ static int tc_unattached_snk_run(int port)
  *   Place Ra on VCONN and Rd on Host CC
  *   Place Rd on Charge-Through CCs
  */
-static int tc_attach_wait_snk(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_attach_wait_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rard_ct_rd);
-}
-
-static int tc_attach_wait_snk_entry(int port)
+static void tc_attach_wait_snk_entry(const int port)
 {
-	tc[port].state_id = TC_ATTACH_WAIT_SNK;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 	tc[port].host_cc_state = PD_CC_UNSET;
-
-	return 0;
 }
 
-static int tc_attach_wait_snk_run(int port)
+static void tc_attach_wait_snk_run(const int port)
 {
 	int host_new_cc_state;
 	int host_cc;
@@ -307,12 +400,12 @@ static int tc_attach_wait_snk_run(int port)
 		else
 			tc[port].host_cc_debounce = get_time().val +
 							PD_T_PD_DEBOUNCE;
-		return 0;
+		return;
 	}
 
 	/* Wait for Host CC debounce */
 	if (get_time().val < tc[port].host_cc_debounce)
-		return 0;
+		return;
 
 	/*
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
@@ -325,28 +418,17 @@ static int tc_attach_wait_snk_run(int port)
 	 */
 	if (tc[port].host_cc_state == PD_CC_DFP_ATTACHED &&
 			(vpd_is_vconn_present() || vpd_is_host_vbus_present()))
-		sm_set_state(port, TC_OBJ(port), tc_attached_snk);
+		set_state_tc(port, TC_ATTACHED_SNK);
 	else if (tc[port].host_cc_state == PD_CC_NONE)
-		sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
-
-	return 0;
+		set_state_tc(port, TC_UNATTACHED_SNK);
 }
 
 /**
  * Attached.SNK
  */
-static int tc_attached_snk(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_attached_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tc_attached_snk_entry(int port)
+static void tc_attached_snk_entry(const int port)
 {
-	tc[port].state_id = TC_ATTACHED_SNK;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Enable PD */
 	tc[port].pd_enable = 1;
@@ -368,18 +450,18 @@ static int tc_attached_snk_entry(int port)
 
 	/* Sample host CC every 2ms */
 	tc_set_timeout(port, 2*MSEC);
-
-	return 0;
 }
 
-static int tc_attached_snk_run(int port)
+static void tc_attached_snk_run(const int port)
 {
 	int host_new_cc_state;
 	int host_cc;
 
 	/* Has host vbus and vconn been removed */
-	if (!vpd_is_host_vbus_present() && !vpd_is_vconn_present())
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
+	if (!vpd_is_host_vbus_present() && !vpd_is_vconn_present()) {
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
+	}
 
 	/*
 	 * Reset the Charge-Through Support Timer when it first
@@ -404,12 +486,12 @@ static int tc_attached_snk_run(int port)
 	if (tc[port].host_cc_state != host_new_cc_state) {
 		tc[port].host_cc_state = host_new_cc_state;
 		tc[port].host_cc_debounce = get_time().val + PD_T_VPDCTDD;
-		return 0;
+		return;
 	}
 
 	/* Wait for Host CC debounce */
 	if (get_time().val < tc[port].host_cc_debounce)
-		return 0;
+		return;
 
 	if (vpd_is_vconn_present()) {
 		if (!(tc[port].flags & TC_FLAGS_VCONN_ON)) {
@@ -423,9 +505,10 @@ static int tc_attached_snk_run(int port)
 		 * to CTUnattached.VPD if VCONN is present and the state of
 		 * its Host-side port’s CC pin is SNK.Open for tVPDCTDD.
 		 */
-		if (tc[port].host_cc_state == PD_CC_NONE)
-			return sm_set_state(port, TC_OBJ(port),
-							tc_ct_unattached_vpd);
+		if (tc[port].host_cc_state == PD_CC_NONE) {
+			set_state_tc(port, TC_CT_UNATTACHED_VPD);
+			return;
+		}
 	}
 
 	/* Check the Support Timer */
@@ -438,76 +521,44 @@ static int tc_attached_snk_run(int port)
 		tc[port].billboard_presented = 1;
 		vpd_present_billboard(BB_SNK);
 	}
-
-	return 0;
 }
 
-static int tc_attached_snk_exit(int port)
+static void tc_attached_snk_exit(const int port)
 {
 	/* Reset timeout value to 10ms */
 	tc_set_timeout(port, 10*MSEC);
 	tc[port].billboard_presented = 0;
 	vpd_present_billboard(BB_NONE);
-
-	return 0;
 }
 
 /**
  * Super State HOST_RA_CT_RD
  */
-static int tc_host_rard_ct_rd(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_host_rard_ct_rd_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_vbus_cc_iso);
-}
-
-static int tc_host_rard_ct_rd_entry(int port)
+static void tc_host_rard_ct_rd_entry(const int port)
 {
 	/* Place Ra on VCONN and Rd on Host CC */
 	vpd_host_set_pull(TYPEC_CC_RA_RD, 0);
 
 	/* Place Rd on Charge-Through CCs */
 	vpd_ct_set_pull(TYPEC_CC_RD, 0);
-
-	return 0;
 }
 
 /**
  * Super State HOST_OPEN_CT_OPEN
  */
-static int tc_host_open_ct_open(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_host_open_ct_open_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_vbus_cc_iso);
-}
-
-static int tc_host_open_ct_open_entry(int port)
+static void tc_host_open_ct_open_entry(const int port)
 {
 	/* Remove the terminations from Host */
 	vpd_host_set_pull(TYPEC_CC_OPEN, 0);
 
 	/* Remove the terminations from Charge-Through */
 	vpd_ct_set_pull(TYPEC_CC_OPEN, 0);
-
-	return 0;
 }
 
 /**
  * Super State VBUS_CC_ISO
  */
-static int tc_vbus_cc_iso(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_vbus_cc_iso_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tc_vbus_cc_iso_entry(int port)
+static void tc_vbus_cc_iso_entry(const int port)
 {
 	/* Isolate the Host-side port from the Charge-Through port */
 	vpd_vbus_pass_en(0);
@@ -517,8 +568,6 @@ static int tc_vbus_cc_iso_entry(int port)
 
 	/* Enable mcu communication and cc */
 	vpd_mcu_cc_en(1);
-
-	return 0;
 }
 
 /**
@@ -530,33 +579,24 @@ static int tc_vbus_cc_iso_entry(int port)
  *   Place RpUSB on Host CC
  *   Place Rd on Charge-Through CCs
  */
-static int tc_unattached_src(int port, enum sm_signal sig)
+static void tc_unattached_src_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_unattached_src_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rpu_ct_rd);
-}
-
-static int tc_unattached_src_entry(int port)
-{
-	tc[port].state_id = TC_UNATTACHED_SRC;
-	if (tc[port].obj.last_state != tc_unattached_snk)
-		CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	if (get_last_state_tc(port) != TC_UNATTACHED_SNK)
+		print_current_state(port);
 
 	/* Get power from VBUS */
 	vpd_vconn_pwr_sel_odl(PWR_VBUS);
 
 	/* Make sure it's the Charge-Through Port's VBUS */
-	if (!vpd_is_ct_vbus_present())
-		return sm_set_state(port, TC_OBJ(port), tc_error_recovery);
+	if (!vpd_is_ct_vbus_present()) {
+		set_state_tc(port, TC_ERROR_RECOVERY);
+		return;
+	}
 
 	tc[port].next_role_swap = get_time().val + PD_T_DRP_SRC;
-
-	return 0;
 }
 
-static int tc_unattached_src_run(int port)
+static void tc_unattached_src_run(const int port)
 {
 	int host_cc;
 
@@ -568,18 +608,20 @@ static int tc_unattached_src_run(int port)
 	 * vSafe0V and SRC.Rd state is detected on the Host-side
 	 * port’s CC pin.
 	 */
-	if (!vpd_is_host_vbus_present() && host_cc == TYPEC_CC_VOLT_RD)
-		return sm_set_state(port, TC_OBJ(port), tc_attach_wait_src);
+	if (!vpd_is_host_vbus_present() && host_cc == TYPEC_CC_VOLT_RD) {
+		set_state_tc(port, TC_ATTACH_WAIT_SRC);
+		return;
+	}
 
 	/*
 	 * Transition to Unattached.SNK within tDRPTransition or
 	 * if Charge-Through VBUS is removed.
 	 */
 	if (!vpd_is_ct_vbus_present() ||
-				get_time().val > tc[port].next_role_swap)
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
-
-	return SM_RUN_SUPER;
+				get_time().val > tc[port].next_role_swap) {
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
+	}
 }
 
 /**
@@ -591,25 +633,14 @@ static int tc_unattached_src_run(int port)
  *   Place RpUSB on Host CC
  *   Place Rd on Charge-Through CCs
  */
-static int tc_attach_wait_src(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_attach_wait_src_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rpu_ct_rd);
-}
-
-static int tc_attach_wait_src_entry(int port)
+static void tc_attach_wait_src_entry(const int port)
 {
-	tc[port].state_id = TC_ATTACH_WAIT_SRC;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	tc[port].host_cc_state = PD_CC_UNSET;
-
-	return 0;
 }
 
-static int tc_attach_wait_src_run(int port)
+static void tc_attach_wait_src_run(const int port)
 {
 	int host_new_cc_state;
 	int host_cc;
@@ -630,19 +661,21 @@ static int tc_attach_wait_src_run(int port)
 	 * shall detect the SRC.Open state within tSRCDisconnect, but
 	 * should detect it as quickly as possible.
 	 */
-	if (host_new_cc_state == PD_CC_NONE || !vpd_is_ct_vbus_present())
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
+	if (host_new_cc_state == PD_CC_NONE || !vpd_is_ct_vbus_present()) {
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
+	}
 
 	/* Debounce the Host CC state */
 	if (tc[port].host_cc_state != host_new_cc_state) {
 		tc[port].host_cc_state = host_new_cc_state;
 		tc[port].cc_debounce = get_time().val + PD_T_CC_DEBOUNCE;
-		return 0;
+		return;
 	}
 
 	/* Wait for Host CC debounce */
 	if (get_time().val < tc[port].cc_debounce)
-		return 0;
+		return;
 
 	/*
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
@@ -650,27 +683,18 @@ static int tc_attach_wait_src_run(int port)
 	 * state is on the Host-side port’s CC pin for at least tCCDebounce.
 	 */
 	if (tc[port].host_cc_state == PD_CC_UFP_ATTACHED &&
-						!vpd_is_host_vbus_present())
-		return sm_set_state(port, TC_OBJ(port), tc_try_snk);
-
-	return SM_RUN_SUPER;
+						!vpd_is_host_vbus_present()) {
+		set_state_tc(port, TC_TRY_SNK);
+		return;
+	}
 }
 
 /**
  * Attached.SRC
  */
-static int tc_attached_src(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_attached_src_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tc_attached_src_entry(int port)
+static void tc_attached_src_entry(const int port)
 {
-	tc[port].state_id = TC_ATTACHED_SRC;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Enable PD */
 	tc[port].pd_enable = 1;
@@ -685,11 +709,9 @@ static int tc_attached_src_entry(int port)
 	 * VBUS
 	 */
 	vpd_vconn_pwr_sel_odl(PWR_VBUS);
-
-	return 0;
 }
 
-static int tc_attached_src_run(int port)
+static void tc_attached_src_run(const int port)
 {
 	int host_cc;
 
@@ -704,31 +726,19 @@ static int tc_attached_src_run(int port)
 	 * tSRCDisconnect, but should detect it as quickly as possible.
 	 */
 	if (!vpd_is_ct_vbus_present() || host_cc == TYPEC_CC_VOLT_OPEN)
-		sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
-
-	return 0;
+		set_state_tc(port, TC_UNATTACHED_SNK);
 }
 
 /**
  * Super State HOST_RPU_CT_RD
  */
-static int tc_host_rpu_ct_rd(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_host_rpu_ct_rd_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_vbus_cc_iso);
-}
-
-static int tc_host_rpu_ct_rd_entry(int port)
+static void tc_host_rpu_ct_rd_entry(const int port)
 {
 	/* Place RpUSB on Host CC */
 	vpd_host_set_pull(TYPEC_CC_RP, TYPEC_RP_USB);
 
 	/* Place Rd on Charge-Through CCs */
 	vpd_ct_set_pull(TYPEC_CC_RD, 0);
-
-	return 0;
 }
 
 /**
@@ -740,35 +750,26 @@ static int tc_host_rpu_ct_rd_entry(int port)
  *   Place Ra on VCONN and Rd on Host CC
  *   Place Rd on Charge-Through CCs
  */
-static int tc_try_snk(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_try_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rard_ct_rd);
-}
-
-static int tc_try_snk_entry(int port)
+static void tc_try_snk_entry(const int port)
 {
-	tc[port].state_id = TC_TRY_SNK;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Get power from VBUS */
 	vpd_vconn_pwr_sel_odl(PWR_VBUS);
 
 	/* Make sure it's the Charge-Through Port's VBUS */
-	if (!vpd_is_ct_vbus_present())
-		return sm_set_state(port, TC_OBJ(port), tc_error_recovery);
+	if (!vpd_is_ct_vbus_present()) {
+		set_state_tc(port, TC_ERROR_RECOVERY);
+		return;
+	}
 
 	tc[port].host_cc_state = PD_CC_UNSET;
 
 	/* Using next_role_swap timer as try_src timer */
 	tc[port].next_role_swap = get_time().val + PD_T_DRP_TRY;
-
-	return 0;
 }
 
-static int tc_try_snk_run(int port)
+static void tc_try_snk_run(const int port)
 {
 	int host_new_cc_state;
 	int host_cc;
@@ -778,7 +779,7 @@ static int tc_try_snk_run(int port)
 	 * port’s CC pins for the SNK.Rp
 	 */
 	if (get_time().val < tc[port].next_role_swap)
-		return 0;
+		return;
 
 	/* Check Host CC for connection */
 	vpd_host_get_cc(&host_cc);
@@ -792,12 +793,12 @@ static int tc_try_snk_run(int port)
 	if (tc[port].host_cc_state != host_new_cc_state) {
 		tc[port].host_cc_state = host_new_cc_state;
 		tc[port].cc_debounce = get_time().val + PD_T_DEBOUNCE;
-		return 0;
+		return;
 	}
 
 	/* Wait for Host CC debounce */
 	if (get_time().val < tc[port].cc_debounce)
-		return 0;
+		return;
 
 	/*
 	 * The Charge-Through VCONN-Powered USB Device shall then transition to
@@ -811,11 +812,9 @@ static int tc_try_snk_run(int port)
 	 */
 	if (tc[port].host_cc_state == PD_CC_DFP_ATTACHED &&
 			(vpd_is_host_vbus_present() || vpd_is_vconn_present()))
-		sm_set_state(port, TC_OBJ(port), tc_attached_snk);
+		set_state_tc(port, TC_ATTACHED_SNK);
 	else if (tc[port].host_cc_state == PD_CC_NONE)
-		sm_set_state(port, TC_OBJ(port), tc_try_wait_src);
-
-	return 0;
+		set_state_tc(port, TC_TRY_WAIT_SRC);
 }
 
 /**
@@ -827,26 +826,15 @@ static int tc_try_snk_run(int port)
  *   Place RpUSB on Host CC
  *   Place Rd on Charge-Through CCs
  */
-static int tc_try_wait_src(int port, enum sm_signal sig)
+static void tc_try_wait_src_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_try_wait_src_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rpu_ct_rd);
-}
-
-static int tc_try_wait_src_entry(int port)
-{
-	tc[port].state_id = TC_TRY_WAIT_SRC;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	tc[port].host_cc_state = PD_CC_UNSET;
 	tc[port].next_role_swap = get_time().val + PD_T_DRP_TRY;
-
-	return 0;
 }
 
-static int tc_try_wait_src_run(int port)
+static void tc_try_wait_src_run(const int port)
 {
 	int host_new_cc_state;
 	int host_cc;
@@ -864,7 +852,7 @@ static int tc_try_wait_src_run(int port)
 		tc[port].host_cc_state = host_new_cc_state;
 		tc[port].host_cc_debounce =
 					get_time().val + PD_T_TRY_CC_DEBOUNCE;
-		return 0;
+		return;
 	}
 
 	if (get_time().val > tc[port].host_cc_debounce) {
@@ -875,9 +863,10 @@ static int tc_try_wait_src_run(int port)
 		 * at least tTryCCDebounce.
 		 */
 		if (tc[port].host_cc_state == PD_CC_UFP_ATTACHED &&
-						!vpd_is_host_vbus_present())
-			return sm_set_state(port, TC_OBJ(port),
-							tc_attached_src);
+						!vpd_is_host_vbus_present()) {
+			set_state_tc(port, TC_ATTACHED_SRC);
+			return;
+		}
 	}
 
 	if (get_time().val > tc[port].next_role_swap) {
@@ -886,12 +875,11 @@ static int tc_try_wait_src_run(int port)
 		 * to Unattached.SNK after tDRPTry if the Host-side port’s CC
 		 * pin is not in the SRC.Rd state.
 		 */
-		if (tc[port].host_cc_state == PD_CC_NONE)
-			return sm_set_state(port, TC_OBJ(port),
-							tc_unattached_snk);
+		if (tc[port].host_cc_state == PD_CC_NONE) {
+			set_state_tc(port, TC_UNATTACHED_SNK);
+			return;
+		}
 	}
-
-	return SM_RUN_SUPER;
 }
 
 /**
@@ -904,18 +892,9 @@ static int tc_try_wait_src_run(int port)
  *   Connect Charge-Through Rd
  *   Get power from VCONN
  */
-static int tc_ct_try_snk(int port, enum sm_signal sig)
+static void tc_ct_try_snk_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_ct_try_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rp3_ct_rd);
-}
-
-static int tc_ct_try_snk_entry(int port)
-{
-	tc[port].state_id = TC_CTTRY_SNK;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Enable PD */
 	tc[port].pd_enable = 1;
@@ -923,11 +902,9 @@ static int tc_ct_try_snk_entry(int port)
 
 	tc[port].cc_state = PD_CC_UNSET;
 	tc[port].next_role_swap = get_time().val + PD_T_DRP_TRY;
-
-	return 0;
 }
 
-static int tc_ct_try_snk_run(int port)
+static void tc_ct_try_snk_run(const int port)
 {
 	int new_cc_state;
 	int cc1;
@@ -938,7 +915,7 @@ static int tc_ct_try_snk_run(int port)
 	 * port’s CC pins for the SNK.Rp
 	 */
 	if (get_time().val < tc[port].next_role_swap)
-		return 0;
+		return;
 
 	/* Check CT CC for connection */
 	vpd_ct_get_cc(&cc1, &cc2);
@@ -952,8 +929,10 @@ static int tc_ct_try_snk_run(int port)
 	 * The Charge-Through VCONN-Powered USB Device shall transition
 	 * to Unattached.SNK if VCONN falls below vVCONNDisconnect.
 	 */
-	if (!vpd_is_vconn_present())
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
+	if (!vpd_is_vconn_present()) {
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
+	}
 
 	/* Debounce the CT CC state */
 	if (tc[port].cc_state != new_cc_state) {
@@ -961,7 +940,7 @@ static int tc_ct_try_snk_run(int port)
 		tc[port].cc_debounce = get_time().val + PD_T_DEBOUNCE;
 		tc[port].try_wait_debounce = get_time().val + PD_T_TRY_WAIT;
 
-		return 0;
+		return;
 	}
 
 	if (get_time().val > tc[port].cc_debounce) {
@@ -973,9 +952,10 @@ static int tc_ct_try_snk_run(int port)
 		 * Charge-Through port.
 		 */
 		if (tc[port].cc_state == PD_CC_DFP_ATTACHED &&
-				vpd_is_ct_vbus_present())
-			return sm_set_state(port, TC_OBJ(port),
-							tc_ct_attached_vpd);
+				vpd_is_ct_vbus_present()) {
+			set_state_tc(port, TC_CT_ATTACHED_VPD);
+			return;
+		}
 	}
 
 	if (get_time().val > tc[port].try_wait_debounce) {
@@ -984,20 +964,18 @@ static int tc_ct_try_snk_run(int port)
 		 * to CTAttached.Unsupported if SNK.Rp state is not detected
 		 * for tDRPTryWait.
 		 */
-		if (tc[port].cc_state == PD_CC_NONE)
-			return sm_set_state(port, TC_OBJ(port),
-					tc_ct_attached_unsupported);
+		if (tc[port].cc_state == PD_CC_NONE) {
+			set_state_tc(port,
+					TC_CT_ATTACHED_UNSUPPORTED);
+			return;
+		}
 	}
-
-	return SM_RUN_SUPER;
 }
 
-static int tc_ct_try_snk_exit(int port)
+static void tc_ct_try_snk_exit(const int port)
 {
 	/* Disable PD */
 	tc[port].pd_enable = 0;
-
-	return 0;
 }
 
 /**
@@ -1010,29 +988,18 @@ static int tc_ct_try_snk_exit(int port)
  *    Place RPUSB on Charge-Through CC
  *    Get power from VCONN
  */
-static int tc_ct_attach_wait_unsupported(int port, enum sm_signal sig)
+static void tc_ct_attach_wait_unsupported_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_ct_attach_wait_unsupported_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rp3_ct_rpu);
-}
-
-static int tc_ct_attach_wait_unsupported_entry(int port)
-{
-	tc[port].state_id = TC_CTATTACH_WAIT_UNSUPPORTED;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Enable PD */
 	tc[port].pd_enable = 1;
 	set_polarity(port, 0);
 
 	tc[port].cc_state = PD_CC_UNSET;
-
-	return 0;
 }
 
-static int tc_ct_attach_wait_unsupported_run(int port)
+static void tc_ct_attach_wait_unsupported_run(const int port)
 {
 	int new_cc_state;
 	int cc1;
@@ -1052,19 +1019,21 @@ static int tc_ct_attach_wait_unsupported_run(int port)
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
 	 * Unattached.SNK if VCONN falls below vVCONNDisconnect.
 	 */
-	if (!vpd_is_vconn_present())
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
+	if (!vpd_is_vconn_present()) {
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
+	}
 
 	/* Debounce the cc state */
 	if (tc[port].cc_state != new_cc_state) {
 		tc[port].cc_state = new_cc_state;
 		tc[port].cc_debounce = get_time().val + PD_T_CC_DEBOUNCE;
-		return 0;
+		return;
 	}
 
 	/* Wait for CC debounce */
 	if (get_time().val < tc[port].cc_debounce)
-		return 0;
+		return;
 
 	/*
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
@@ -1077,19 +1046,15 @@ static int tc_ct_attach_wait_unsupported_run(int port)
 	 * pins is SRC.Ra. for at least tCCDebounce.
 	 */
 	if (new_cc_state == PD_CC_NONE)
-		sm_set_state(port, TC_OBJ(port), tc_ct_unattached_vpd);
+		set_state_tc(port, TC_CT_UNATTACHED_VPD);
 	else /* PD_CC_DFP_ATTACHED or PD_CC_AUDIO_ACC */
-		sm_set_state(port, TC_OBJ(port), tc_ct_try_snk);
-
-	return 0;
+		set_state_tc(port, TC_CT_TRY_SNK);
 }
 
-static int tc_ct_attach_wait_unsupported_exit(int port)
+static void tc_ct_attach_wait_unsupported_exit(const int port)
 {
 	/* Disable PD */
 	tc[port].pd_enable = 0;
-
-	return 0;
 }
 
 /**
@@ -1102,26 +1067,15 @@ static int tc_ct_attach_wait_unsupported_exit(int port)
  *    Place RPUSB on Charge-Through CC
  *    Get power from VCONN
  */
-static int tc_ct_attached_unsupported(int port, enum sm_signal sig)
+static void tc_ct_attached_unsupported_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_ct_attached_unsupported_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rp3_ct_rpu);
-}
-
-static int tc_ct_attached_unsupported_entry(int port)
-{
-	tc[port].state_id = TC_CTATTACHED_UNSUPPORTED;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Present Billboard device */
 	vpd_present_billboard(BB_SNK);
-
-	return 0;
 }
 
-static int tc_ct_attached_unsupported_run(int port)
+static void tc_ct_attached_unsupported_run(const int port)
 {
 	int cc1;
 	int cc2;
@@ -1129,8 +1083,10 @@ static int tc_ct_attached_unsupported_run(int port)
 	/* Check CT CC for connection */
 	vpd_ct_get_cc(&cc1, &cc2);
 
-	if (!vpd_is_vconn_present())
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
+	if (!vpd_is_vconn_present()) {
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
+	}
 
 	/*
 	 * The Charge-Through VCONN-Powered USB Device shall transition to
@@ -1140,17 +1096,15 @@ static int tc_ct_attached_unsupported_run(int port)
 	 */
 	if ((cc1 == TYPEC_CC_VOLT_OPEN && cc2 == TYPEC_CC_VOLT_OPEN) ||
 	    (cc1 == TYPEC_CC_VOLT_OPEN && cc2 == TYPEC_CC_VOLT_RA) ||
-	    (cc1 == TYPEC_CC_VOLT_RA && cc2 == TYPEC_CC_VOLT_OPEN))
-		return sm_set_state(port, TC_OBJ(port), tc_ct_unattached_vpd);
-
-	return SM_RUN_SUPER;
+	    (cc1 == TYPEC_CC_VOLT_RA && cc2 == TYPEC_CC_VOLT_OPEN)) {
+		set_state_tc(port, TC_CT_UNATTACHED_VPD);
+		return;
+	}
 }
 
-static int tc_ct_attached_unsupported_exit(int port)
+static void tc_ct_attached_unsupported_exit(const int port)
 {
 	vpd_present_billboard(BB_NONE);
-
-	return 0;
 }
 
 /**
@@ -1163,30 +1117,19 @@ static int tc_ct_attached_unsupported_exit(int port)
  *    Place RPUSB on Charge-Through CC
  *    Get power from VCONN
  */
-static int tc_ct_unattached_unsupported(int port, enum sm_signal sig)
+static void tc_ct_unattached_unsupported_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_ct_unattached_unsupported_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rp3_ct_rpu);
-}
-
-static int tc_ct_unattached_unsupported_entry(int port)
-{
-	tc[port].state_id = TC_CTUNATTACHED_UNSUPPORTED;
-	if (tc[port].obj.last_state != tc_ct_unattached_vpd)
-		CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	if (get_last_state_tc(port) != TC_CT_UNATTACHED_VPD)
+		print_current_state(port);
 
 	/* Enable PD */
 	tc[port].pd_enable = 1;
 	set_polarity(port, 0);
 
 	tc[port].next_role_swap = get_time().val + PD_T_DRP_SRC;
-
-	return 0;
 }
 
-static int tc_ct_unattached_unsupported_run(int port)
+static void tc_ct_unattached_unsupported_run(const int port)
 {
 	int cc1;
 	int cc2;
@@ -1201,33 +1144,35 @@ static int tc_ct_unattached_unsupported_run(int port)
 	 * least one of the Charge-Through port’s CC pins or SRC.Ra state
 	 * on both the CC1 and CC2 pins.
 	 */
-	if (cc_is_at_least_one_rd(cc1, cc2) || cc_is_audio_acc(cc1, cc2))
-		return sm_set_state(port, TC_OBJ(port),
-				tc_ct_attach_wait_unsupported);
+	if (cc_is_at_least_one_rd(cc1, cc2) || cc_is_audio_acc(cc1, cc2)) {
+		set_state_tc(port,
+				TC_CT_ATTACH_WAIT_UNSUPPORTED);
+		return;
+	}
 
 	/*
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
 	 * Unattached.SNK if VCONN falls below vVCONNDisconnect.
 	 */
-	if (!vpd_is_vconn_present())
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
+	if (!vpd_is_vconn_present()) {
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
+	}
 
 	/*
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
 	 * CTUnattached.VPD within tDRPTransition after dcSRC.DRP ∙ tDRP.
 	 */
-	if (get_time().val > tc[port].next_role_swap)
-		return sm_set_state(port, TC_OBJ(port), tc_ct_unattached_vpd);
-
-	return SM_RUN_SUPER;
+	if (get_time().val > tc[port].next_role_swap) {
+		set_state_tc(port, TC_CT_UNATTACHED_VPD);
+		return;
+	}
 }
 
-static int tc_ct_unattached_unsupported_exit(int port)
+static void tc_ct_unattached_unsupported_exit(const int port)
 {
 	/* Disable PD */
 	tc[port].pd_enable = 0;
-
-	return 0;
 }
 
 /**
@@ -1240,30 +1185,19 @@ static int tc_ct_unattached_unsupported_exit(int port)
  *    Connect Charge-Through Rd
  *    Get power from VCONN
  */
-static int tc_ct_unattached_vpd(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_ct_unattached_vpd_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rp3_ct_rd);
-}
-
-static int tc_ct_unattached_vpd_entry(int port)
+static void tc_ct_unattached_vpd_entry(const int port)
 {
-	tc[port].state_id = TC_CTUNATTACHED_VPD;
-	if (tc[port].obj.last_state != tc_ct_unattached_unsupported)
-		CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	if (get_last_state_tc(port) != TC_CT_UNATTACHED_UNSUPPORTED)
+		print_current_state(port);
 
 	/* Enable PD */
 	tc[port].pd_enable = 1;
 	set_polarity(port, 0);
 
 	tc[port].cc_state = PD_CC_UNSET;
-
-	return 0;
 }
 
-static int tc_ct_unattached_vpd_run(int port)
+static void tc_ct_unattached_vpd_run(const int port)
 {
 	int new_cc_state;
 	int cc1;
@@ -1285,27 +1219,29 @@ static int tc_ct_unattached_vpd_run(int port)
 	 * Charge-Through port, as indicated by the SNK.Rp state on
 	 * exactly one of the Charge-Through port’s CC pins.
 	 */
-	if (new_cc_state == PD_CC_DFP_ATTACHED)
-		return sm_set_state(port, TC_OBJ(port),
-					tc_ct_attach_wait_vpd);
+	if (new_cc_state == PD_CC_DFP_ATTACHED) {
+		set_state_tc(port, TC_CT_ATTACH_WAIT_VPD);
+		return;
+	}
 
 	/*
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
 	 * Unattached.SNK if VCONN falls below vVCONNDisconnect.
 	 */
-	if (!vpd_is_vconn_present())
-		return sm_set_state(port, TC_OBJ(port),
-				tc_unattached_snk);
+	if (!vpd_is_vconn_present()) {
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
+	}
 
 	/* Debounce the cc state */
 	if (new_cc_state != tc[port].cc_state) {
 		tc[port].cc_state = new_cc_state;
 		tc[port].cc_debounce = get_time().val + PD_T_DRP_SRC;
-		return 0;
+		return;
 	}
 
 	if (get_time().val < tc[port].cc_debounce)
-		return 0;
+		return;
 
 	/*
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
@@ -1313,19 +1249,16 @@ static int tc_ct_unattached_vpd_run(int port)
 	 * of both the Charge-Through port’s CC1 and CC2 pins is SNK.Open
 	 * for tDRP-dcSRC.DRP ∙ tDRP, or if directed.
 	 */
-	if (tc[port].cc_state == PD_CC_NONE)
-		return sm_set_state(port, TC_OBJ(port),
-					tc_ct_unattached_unsupported);
-
-	return SM_RUN_SUPER;
+	if (tc[port].cc_state == PD_CC_NONE) {
+		set_state_tc(port, TC_CT_UNATTACHED_UNSUPPORTED);
+		return;
+	}
 }
 
-static int tc_ct_unattached_vpd_exit(int port)
+static void tc_ct_unattached_vpd_exit(const int port)
 {
 	/* Disable PD */
 	tc[port].pd_enable = 0;
-
-	return 0;
 }
 
 /**
@@ -1337,57 +1270,34 @@ static int tc_ct_unattached_vpd_exit(int port)
  *   Remove the terminations from Host
  *   Remove the terminations from Charge-Through
  */
-static int tc_ct_disabled_vpd(int port, enum sm_signal sig)
+static void tc_ct_disabled_vpd_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_ct_disabled_vpd_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_open_ct_open);
-}
-
-static int tc_ct_disabled_vpd_entry(int port)
-{
-	tc[port].state_id = TC_CTDISABLED_VPD;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Get power from VBUS */
 	vpd_vconn_pwr_sel_odl(PWR_VBUS);
 
 	tc[port].next_role_swap = get_time().val + PD_T_VPDDISABLE;
-
-	return 0;
 }
 
-static int tc_ct_disabled_vpd_run(int port)
+static void tc_ct_disabled_vpd_run(const int port)
 {
 	/*
 	 * A Charge-Through VCONN-Powered USB Device shall transition
 	 * to Unattached.SNK after tVPDDisable.
 	 */
 	if (get_time().val > tc[port].next_role_swap)
-		sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
-
-	return 0;
+		set_state_tc(port, TC_UNATTACHED_SNK);
 }
 
 /**
  * CTAttached.VPD
  */
-static int tc_ct_attached_vpd(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_ct_attached_vpd_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tc_ct_attached_vpd_entry(int port)
+static void tc_ct_attached_vpd_entry(const int port)
 {
 	int cc1;
 	int cc2;
-
-	tc[port].state_id = TC_CTATTACHED_VPD;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Get power from VCONN */
 	vpd_vconn_pwr_sel_odl(PWR_VCONN);
@@ -1430,11 +1340,9 @@ static int tc_ct_attached_vpd_entry(int port)
 	vpd_vbus_pass_en(1);
 
 	tc[port].cc_state = PD_CC_UNSET;
-
-	return 0;
 }
 
-static int tc_ct_attached_vpd_run(int port)
+static void tc_ct_attached_vpd_run(const int port)
 {
 	int new_cc_state;
 	int cc1;
@@ -1444,8 +1352,10 @@ static int tc_ct_attached_vpd_run(int port)
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
 	 * CTDisabled.VPD if VCONN falls below vVCONNDisconnect.
 	 */
-	if (!vpd_is_vconn_present())
-		return sm_set_state(port, TC_OBJ(port), tc_ct_disabled_vpd);
+	if (!vpd_is_vconn_present()) {
+		set_state_tc(port, TC_CT_DISABLED_VPD);
+		return;
+	}
 
 	/* Check CT CC for connection */
 	vpd_ct_get_cc(&cc1, &cc2);
@@ -1458,11 +1368,11 @@ static int tc_ct_attached_vpd_run(int port)
 	if (new_cc_state != tc[port].cc_state) {
 		tc[port].cc_state = new_cc_state;
 		tc[port].cc_debounce = get_time().val + PD_T_VPDCTDD;
-		return 0;
+		return;
 	}
 
 	if (get_time().val < tc[port].pd_debounce)
-		return 0;
+		return;
 
 	/*
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
@@ -1470,9 +1380,7 @@ static int tc_ct_attached_vpd_run(int port)
 	 * state of the passed-through CC pin is SNK.Open for tVPDCTDD.
 	 */
 	if (tc[port].cc_state == PD_CC_NONE && !vpd_is_ct_vbus_present())
-		sm_set_state(port, TC_OBJ(port), tc_ct_unattached_vpd);
-
-	return 0;
+		set_state_tc(port, TC_CT_UNATTACHED_VPD);
 }
 
 /**
@@ -1485,18 +1393,9 @@ static int tc_ct_attached_vpd_run(int port)
  *    Connect Charge-Through Rd
  *    Get power from VCONN
  */
-static int tc_ct_attach_wait_vpd(int port, enum sm_signal sig)
+static void tc_ct_attach_wait_vpd_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_ct_attach_wait_vpd_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rp3_ct_rd);
-}
-
-static int tc_ct_attach_wait_vpd_entry(int port)
-{
-	tc[port].state_id = TC_CTATTACH_WAIT_VPD;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Enable PD */
 	tc[port].pd_enable = 1;
@@ -1506,10 +1405,9 @@ static int tc_ct_attach_wait_vpd_entry(int port)
 
 	/* Sample CCs every 2ms */
 	tc_set_timeout(port, 2 * MSEC);
-	return 0;
 }
 
-static int tc_ct_attach_wait_vpd_run(int port)
+static void tc_ct_attach_wait_vpd_run(const int port)
 {
 	int new_cc_state;
 	int cc1;
@@ -1529,8 +1427,10 @@ static int tc_ct_attach_wait_vpd_run(int port)
 	 * A Charge-Through VCONN-Powered USB Device shall transition to
 	 * CTDisabled.VPD if VCONN falls below vVCONNDisconnect.
 	 */
-	if (!vpd_is_vconn_present())
-		return sm_set_state(port, TC_OBJ(port), tc_ct_disabled_vpd);
+	if (!vpd_is_vconn_present()) {
+		set_state_tc(port, TC_CT_DISABLED_VPD);
+		return;
+	}
 
 	/* Debounce the cc state */
 	if (new_cc_state != tc[port].cc_state) {
@@ -1539,7 +1439,7 @@ static int tc_ct_attach_wait_vpd_run(int port)
 						PD_T_CC_DEBOUNCE;
 		tc[port].pd_debounce = get_time().val +
 						PD_T_PD_DEBOUNCE;
-		return 0;
+		return;
 	}
 
 	if (get_time().val > tc[port].pd_debounce) {
@@ -1549,9 +1449,10 @@ static int tc_ct_attach_wait_vpd_run(int port)
 		 * port’s CC1 and CC2 pins are SNK.Open for at least
 		 * tPDDebounce.
 		 */
-		if (tc[port].cc_state  == PD_CC_NONE)
-			return sm_set_state(port, TC_OBJ(port),
-						tc_ct_unattached_vpd);
+		if (tc[port].cc_state  == PD_CC_NONE) {
+			set_state_tc(port, TC_CT_UNATTACHED_VPD);
+			return;
+		}
 	}
 
 	if (get_time().val > tc[port].cc_debounce) {
@@ -1563,38 +1464,26 @@ static int tc_ct_attach_wait_vpd_run(int port)
 		 * detected.
 		 */
 		if (tc[port].cc_state  == PD_CC_DFP_ATTACHED &&
-						vpd_is_ct_vbus_present())
-			return sm_set_state(port, TC_OBJ(port),
-							tc_ct_attached_vpd);
+						vpd_is_ct_vbus_present()) {
+			set_state_tc(port, TC_CT_ATTACHED_VPD);
+			return;
+		}
 	}
-
-	return SM_RUN_SUPER;
 }
 
-static int tc_ct_attach_wait_vpd_exit(int port)
+static void tc_ct_attach_wait_vpd_exit(const int port)
 {
 	/* Disable PD */
 	tc[port].pd_enable = 0;
 
 	/* Reset timeout value to 10ms */
 	tc_set_timeout(port, 10 * MSEC);
-
-	return 0;
 }
 
 /**
  * Super State HOST_RP3_CT_RD
  */
-static int tc_host_rp3_ct_rd(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_host_rp3_ct_rd_sig[sig])(port);
-
-	return SM_SUPER(ret, sig, tc_vbus_cc_iso);
-}
-
-static int tc_host_rp3_ct_rd_entry(int port)
+static void tc_host_rp3_ct_rd_entry(const int port)
 {
 	/* Place RP3A0 on Host CC */
 	vpd_host_set_pull(TYPEC_CC_RP, TYPEC_RP_3A0);
@@ -1609,26 +1498,16 @@ static int tc_host_rp3_ct_rd_entry(int port)
 
 	/* Make sure vconn is on */
 	if (!vpd_is_vconn_present())
-		sm_set_state(port, TC_OBJ(port), tc_error_recovery);
+		set_state_tc(port, TC_ERROR_RECOVERY);
 
 	/* Get power from VCONN */
 	vpd_vconn_pwr_sel_odl(PWR_VCONN);
-
-	return 0;
 }
 
 /**
  * Super State HOST_RP3_CT_RPU
  */
-static int tc_host_rp3_ct_rpu(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_host_rp3_ct_rpu_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_vbus_cc_iso);
-}
-
-static int tc_host_rp3_ct_rpu_entry(int port)
+static void tc_host_rp3_ct_rpu_entry(const int port)
 {
 	/* Place RP3A0 on Host CC */
 	vpd_host_set_pull(TYPEC_CC_RP, TYPEC_RP_3A0);
@@ -1643,10 +1522,165 @@ static int tc_host_rp3_ct_rpu_entry(int port)
 
 	/* Make sure vconn is on */
 	if (!vpd_is_vconn_present())
-		sm_set_state(port, TC_OBJ(port), tc_error_recovery);
+		set_state_tc(port, TC_ERROR_RECOVERY);
 
 	/* Get power from VCONN */
 	vpd_vconn_pwr_sel_odl(PWR_VCONN);
-
-	return 0;
 }
+
+/* All necessary Type-C states */
+
+/*
+ * Type-C State Hierarchy (Sub-States are listed inside the boxes)
+ *
+ * | TC_VBUS_CC_ISO ------------------------------------------------------|
+ * |                                                                      |
+ * |  | TC_HOST_RARD_CT_RD -----------| | TC_HOST_OPEN_CT_OPEN ---------| |
+ * |  |                               | |                               | |
+ * |  | TC_UNATTACHED_SNK             | | TC_DISABLED                   | |
+ * |  | TC_ATTACH_WAIT_SNK            | | TC_ERROR_RECOVERY             | |
+ * |  | TC_TRY_SNK                    | |-------------------------------| |
+ * |  |-------------------------------|                                   |
+ * |                                                                      |
+ * |  | TC_HOST_RP3_CT_RD ------------| | TC_HOST_RPU_CT_RD ------------| |
+ * |  |                               | |                               | |
+ * |  | TC_CT_TRY_SNK                 | | TC_UNATTACHED_SRC             | |
+ * |  | TC_CT_UNATTACHED_VPD          | | TC_ATTACH_WAIT_SRC            | |
+ * |  | TC_CT_ATTACH_WAIT_VPD         | | TC_TRY_WAIT_SR                | |
+ * |  |-------------------------------| |-------------------------------| |
+ * |                                                                      |
+ * |  | TC_HOST_RP3_CT_RPU -----------|                                   |
+ * |  |                               |                                   |
+ * |  | TC_CT_ATTACH_WAIT_UNSUPPORTED |                                   |
+ * |  | TC_CT_ATTACHED_UNSUPPORTED    |                                   |
+ * |  | TC_CT_UNATTACHED_UNSUPPORTED  |                                   |
+ * |  |-------------------------------|                                   |
+ * |----------------------------------------------------------------------|
+ *
+ * TC_ATTACHED_SNK
+ * TC_ATTACHED_SRC
+ * TC_CT_ATTACHED_VPD
+ *
+ */
+static const struct usb_state tc_states[] = {
+	/* Super States */
+	[TC_VBUS_CC_ISO] = {
+		.entry  = tc_vbus_cc_iso_entry,
+	},
+	[TC_HOST_RARD_CT_RD] = {
+		.entry  = tc_host_rard_ct_rd_entry,
+		.parent = &tc_states[TC_VBUS_CC_ISO],
+	},
+	[TC_HOST_OPEN_CT_OPEN] = {
+		.entry  = tc_host_open_ct_open_entry,
+		.parent = &tc_states[TC_VBUS_CC_ISO],
+	},
+	[TC_HOST_RP3_CT_RD] = {
+		.entry  = tc_host_rp3_ct_rd_entry,
+		.parent = &tc_states[TC_VBUS_CC_ISO],
+	},
+	[TC_HOST_RP3_CT_RPU] = {
+		.entry  = tc_host_rp3_ct_rpu_entry,
+		.parent = &tc_states[TC_VBUS_CC_ISO],
+	},
+	[TC_HOST_RPU_CT_RD] = {
+		.entry  = tc_host_rpu_ct_rd_entry,
+		.parent = &tc_states[TC_VBUS_CC_ISO],
+	},
+	/* Normal States */
+	[TC_DISABLED] = {
+		.entry  = tc_disabled_entry,
+		.run    = tc_disabled_run,
+		.exit   = tc_disabled_exit,
+		.parent = &tc_states[TC_HOST_OPEN_CT_OPEN],
+	},
+	[TC_UNATTACHED_SNK] = {
+		.entry  = tc_unattached_snk_entry,
+		.run    = tc_unattached_snk_run,
+		.parent = &tc_states[TC_HOST_RARD_CT_RD],
+	},
+	[TC_ATTACH_WAIT_SNK] = {
+		.entry  = tc_attach_wait_snk_entry,
+		.run    = tc_attach_wait_snk_run,
+		.parent = &tc_states[TC_HOST_RARD_CT_RD],
+	},
+	[TC_ATTACHED_SNK] = {
+		.entry  = tc_attached_snk_entry,
+		.run    = tc_attached_snk_run,
+		.exit   = tc_attached_snk_exit,
+	},
+	[TC_ERROR_RECOVERY] = {
+		.entry  = tc_error_recovery_entry,
+		.run    = tc_error_recovery_run,
+		.parent = &tc_states[TC_HOST_OPEN_CT_OPEN],
+	},
+	[TC_TRY_SNK] = {
+		.entry  = tc_try_snk_entry,
+		.run    = tc_try_snk_run,
+		.parent = &tc_states[TC_HOST_RARD_CT_RD],
+	},
+	[TC_UNATTACHED_SRC] = {
+		.entry  = tc_unattached_src_entry,
+		.run    = tc_unattached_src_run,
+		.parent = &tc_states[TC_HOST_RPU_CT_RD],
+	},
+	[TC_ATTACH_WAIT_SRC] = {
+		.entry  = tc_attach_wait_src_entry,
+		.run    = tc_attach_wait_src_run,
+		.parent = &tc_states[TC_HOST_RPU_CT_RD],
+	},
+	[TC_TRY_WAIT_SRC] = {
+		.entry  = tc_try_wait_src_entry,
+		.run    = tc_try_wait_src_run,
+		.parent = &tc_states[TC_HOST_RPU_CT_RD],
+	},
+	[TC_ATTACHED_SRC] = {
+		.entry  = tc_attached_src_entry,
+		.run    = tc_attached_src_run,
+	},
+	[TC_CT_TRY_SNK] = {
+		.entry  = tc_ct_try_snk_entry,
+		.run    = tc_ct_try_snk_run,
+		.exit   = tc_ct_try_snk_exit,
+		.parent = &tc_states[TC_HOST_RP3_CT_RD],
+	},
+	[TC_CT_ATTACH_WAIT_UNSUPPORTED] = {
+		.entry  = tc_ct_attach_wait_unsupported_entry,
+		.run    = tc_ct_attach_wait_unsupported_run,
+		.exit   = tc_ct_attach_wait_unsupported_exit,
+		.parent = &tc_states[TC_HOST_RP3_CT_RPU],
+	},
+	[TC_CT_ATTACHED_UNSUPPORTED] = {
+		.entry  = tc_ct_attached_unsupported_entry,
+		.run    = tc_ct_attached_unsupported_run,
+		.exit   = tc_ct_attached_unsupported_exit,
+		.parent = &tc_states[TC_HOST_RP3_CT_RPU],
+	},
+	[TC_CT_UNATTACHED_UNSUPPORTED] = {
+		.entry  = tc_ct_unattached_unsupported_entry,
+		.run    = tc_ct_unattached_unsupported_run,
+		.exit   = tc_ct_unattached_unsupported_exit,
+		.parent = &tc_states[TC_HOST_RP3_CT_RPU],
+	},
+	[TC_CT_UNATTACHED_VPD] = {
+		.entry  = tc_ct_unattached_vpd_entry,
+		.run    = tc_ct_unattached_vpd_run,
+		.exit   = tc_ct_unattached_vpd_exit,
+		.parent = &tc_states[TC_HOST_RP3_CT_RD],
+	},
+	[TC_CT_DISABLED_VPD] = {
+		.entry  = tc_ct_disabled_vpd_entry,
+		.run    = tc_ct_disabled_vpd_run,
+		.parent = &tc_states[TC_HOST_OPEN_CT_OPEN],
+	},
+	[TC_CT_ATTACHED_VPD] = {
+		.entry  = tc_ct_attached_vpd_entry,
+		.run    = tc_ct_attached_vpd_run,
+	},
+	[TC_CT_ATTACH_WAIT_VPD] = {
+		.entry  = tc_ct_attach_wait_vpd_entry,
+		.run    = tc_ct_attach_wait_vpd_run,
+		.exit   = tc_ct_attach_wait_vpd_exit,
+		.parent = &tc_states[TC_HOST_RP3_CT_RD],
+	},
+};
diff --git a/common/usbc/usb_tc_drp_acc_trysrc_sm.c b/common/usbc/usb_tc_drp_acc_trysrc_sm.c
index 4267e8e302..8ec7e8ffbc 100644
--- a/common/usbc/usb_tc_drp_acc_trysrc_sm.c
+++ b/common/usbc/usb_tc_drp_acc_trysrc_sm.c
@@ -14,7 +14,7 @@
 #include "usb_common.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
-#include "usb_tc_drp_acc_trysrc_sm.h"
+#include "usb_sm.h"
 #include "usb_tc_sm.h"
 #include "usbc_ppc.h"
 
@@ -39,11 +39,112 @@
 #define TC_FLAGS_LPM_ENGAGED              BIT(4)
 #define TC_FLAGS_LPM_REQUESTED            BIT(5)
 
+/* List of all TypeC-level states */
+enum usb_tc_state {
+	/* Normal States */
+	TC_DISABLED,
+	TC_ERROR_RECOVERY,
+	TC_UNATTACHED_SNK,
+	TC_ATTACH_WAIT_SNK,
+	TC_ATTACHED_SNK,
+	TC_UNORIENTED_DBG_ACC_SRC,
+	TC_DBG_ACC_SNK,
+	TC_UNATTACHED_SRC,
+	TC_ATTACH_WAIT_SRC,
+	TC_ATTACHED_SRC,
+	TC_TRY_SRC,
+	TC_TRY_WAIT_SNK,
+	/* Super States */
+	TC_CC_OPEN,
+	TC_CC_RD,
+	TC_CC_RP,
+};
+/* Forward declare the full list of states. This is indexed by usb_tc_state */
+static const struct usb_state tc_states[];
 
-struct type_c tc[CONFIG_USB_PD_PORT_COUNT];
+#ifdef CONFIG_COMMON_RUNTIME
+/* List of human readable state names for console debugging */
+static const char * const tc_state_names[] = {
+	[TC_DISABLED] = "Disabled",
+	[TC_ERROR_RECOVERY] = "ErrorRecovery",
+	[TC_UNATTACHED_SNK] = "Unattached.SNK",
+	[TC_ATTACH_WAIT_SNK] = "AttachWait.SNK",
+	[TC_ATTACHED_SNK] = "Attached.SNK",
+	[TC_UNORIENTED_DBG_ACC_SRC] = "UnorientedDebugAccessory.SRC",
+	[TC_DBG_ACC_SNK] = "DebugAccessory.SNK",
+	[TC_UNATTACHED_SRC] = "Unattached.SRC",
+	[TC_ATTACH_WAIT_SRC] = "AttachWait.SRC",
+	[TC_ATTACHED_SRC] = "Attached.SRC",
+	[TC_TRY_SRC] = "Try.SRC",
+	[TC_TRY_WAIT_SNK] = "TryWait.SNK",
+};
+#endif
+
+/* Generate a compiler error if invalid states are referenced */
+#ifndef CONFIG_USB_PD_TRY_SRC
+#define TC_TRY_SRC	TC_TRY_SRC_UNDEFINED
+#define TC_TRY_WAIT_SNK	TC_TRY_WAIT_SNK_UNDEFINED
+#endif
+
+static struct type_c {
+	/* state machine context */
+	struct sm_ctx ctx;
+	/* current port power role (SOURCE or SINK) */
+	uint8_t power_role;
+	/* current port data role (DFP or UFP) */
+	uint8_t data_role;
+	/* Higher-level power deliver state machines are enabled if true. */
+	uint8_t pd_enable;
+	/* Port polarity : 0 => CC1 is CC line, 1 => CC2 is CC line */
+	uint8_t polarity;
+	/* port flags, see TC_FLAGS_* */
+	uint32_t flags;
+	/* event timeout */
+	uint64_t evt_timeout;
+	/* Time a port shall wait before it can determine it is attached */
+	uint64_t cc_debounce;
+	/*
+	 * Time a Sink port shall wait before it can determine it is detached
+	 * due to the potential for USB PD signaling on CC as described in
+	 * the state definitions.
+	 */
+	uint64_t pd_debounce;
+#ifdef CONFIG_USB_PD_TRY_SRC
+	/*
+	 * Time a port shall wait before it can determine it is
+	 * re-attached during the try-wait process.
+	 */
+	uint64_t try_wait_debounce;
+#endif
+	/* The cc state */
+	enum pd_cc_states cc_state;
+	/* Role toggle timer */
+	uint64_t next_role_swap;
+	/* Generic timer */
+	uint64_t timeout;
+
+#ifdef CONFIG_USB_PD_TCPC_LOW_POWER
+	/* Time to enter low power mode */
+	uint64_t low_power_time;
+	/* Tasks to notify after TCPC has been reset */
+	int tasks_waiting_on_reset;
+	/* Tasks preventing TCPC from entering low power mode */
+	int tasks_preventing_lpm;
+#endif
+	/* Voltage on CC pin */
+	enum tcpc_cc_voltage_status cc_voltage;
+	/* Type-C current */
+	typec_current_t typec_curr;
+	/* Type-C current change */
+	typec_current_t typec_curr_change;
+	/* Attached ChromeOS device id, RW hash, and current RO / RW image */
+	uint16_t dev_id;
+	uint32_t dev_rw_hash[PD_RW_HASH_SIZE/4];
+	enum ec_current_image current_image;
+} tc[CONFIG_USB_PD_PORT_COUNT];
 
 /* Port dual-role state */
-enum pd_dual_role_states drp_state[CONFIG_USB_PD_PORT_COUNT] = {
+static enum pd_dual_role_states drp_state[CONFIG_USB_PD_PORT_COUNT] = {
 	[0 ... (CONFIG_USB_PD_PORT_COUNT - 1)] =
 		CONFIG_USB_PD_INITIAL_DRP_STATE};
 
@@ -55,7 +156,10 @@ enum pd_dual_role_states drp_state[CONFIG_USB_PD_PORT_COUNT] = {
 static struct ec_params_usb_pd_rw_hash_entry rw_hash_table[RW_HASH_ENTRIES];
 #endif
 
+/* Forward declare common, private functions */
 static void tc_set_data_role(int port, int role);
+static void set_state_tc(const int port, const enum usb_tc_state new_state);
+test_export_static enum usb_tc_state get_state_tc(const int port);
 
 #ifdef CONFIG_USB_PD_TRY_SRC
 /* Enable variable for Try.SRC states */
@@ -64,51 +168,6 @@ static void pd_update_try_source(void);
 #endif
 
 /*
- * Type-C State Hierarchy (Sub-States are listed inside the boxes)
- *
- * |TC_CC_RD --------------|	|TC_CC_RP ------------------------|
- * |			   |	|				  |
- * |	TC_UNATTACHED_SNK  |	|	TC_UNATTACHED_SRC         |
- * |	TC_ATTACH_WAIT_SNK |	|	TC_ATTACH_WAIT_SRC        |
- * |	TC_TRY_WAIT_SNK    |	|	TC_TRY_SRC                |
- * |	TC_DBG_ACC_SNK     |	|	TC_UNORIENTED_DBG_ACC_SRC |
- * |-----------------------|	|---------------------------------|
- *
- * |TC_CC_OPEN -----------|
- * |                      |
- * |	TC_DISABLED       |
- * |	TC_ERROR_RECOVERY |
- * |----------------------|
- *
- * TC_ATTACHED_SNK		TC_ATTACHED_SRC
- *
- */
-
-/*
- * Type-C states
- */
-DECLARE_STATE(tc, disabled, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(tc, error_recovery, WITH_RUN, NOOP);
-DECLARE_STATE(tc, unattached_snk, WITH_RUN, NOOP);
-DECLARE_STATE(tc, attach_wait_snk, WITH_RUN, NOOP);
-DECLARE_STATE(tc, attached_snk, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(tc, dbg_acc_snk, WITH_RUN, NOOP);
-DECLARE_STATE(tc, unattached_src, WITH_RUN, NOOP);
-DECLARE_STATE(tc, attach_wait_src, WITH_RUN, NOOP);
-DECLARE_STATE(tc, attached_src, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(tc, unoriented_dbg_acc_src, WITH_RUN, NOOP);
-
-#ifdef CONFIG_USB_PD_TRY_SRC
-DECLARE_STATE(tc, try_src, WITH_RUN, NOOP);
-DECLARE_STATE(tc, try_wait_snk, WITH_RUN, NOOP);
-#endif
-
-/* Super States */
-DECLARE_STATE(tc, cc_rd, NOOP, NOOP);
-DECLARE_STATE(tc, cc_rp, NOOP, NOOP);
-DECLARE_STATE(tc, cc_open, NOOP, NOOP);
-
-/*
  * Public Functions
  *
  * NOTE: Functions prefixed with pd_ are defined in usb_pd.h
@@ -161,23 +220,16 @@ void pd_request_power_swap(int port)
 
 void pd_set_suspend(int port, int enable)
 {
-	sm_state state;
-
 	if (pd_is_port_enabled(port) == !enable)
 		return;
 
-	if (enable)
-		state = tc_disabled;
-	else
-		state = (TC_DEFAULT_STATE(port) == TC_UNATTACHED_SRC) ?
-				tc_unattached_src : tc_unattached_snk;
-
-	sm_set_state(port, TC_OBJ(port), state);
+	set_state_tc(port,
+		enable ? TC_DISABLED : TC_UNATTACHED_SNK);
 }
 
 int pd_is_port_enabled(int port)
 {
-	return !(tc[port].state_id == TC_DISABLED);
+	return get_state_tc(port) != TC_DISABLED;
 }
 
 int pd_fetch_acc_log_entry(int port)
@@ -210,8 +262,8 @@ void pd_vbus_low(int port)
 
 int pd_is_connected(int port)
 {
-	return (tc[port].state_id == TC_ATTACHED_SNK) ||
-				(tc[port].state_id == TC_ATTACHED_SRC);
+	return (get_state_tc(port) == TC_ATTACHED_SNK) ||
+				(get_state_tc(port) == TC_ATTACHED_SRC);
 }
 
 #ifdef CONFIG_USB_PD_ALT_MODE_DFP
@@ -230,7 +282,7 @@ void pd_prepare_sysjump(void)
 
 void tc_src_power_off(int port)
 {
-	if (tc[port].state_id == TC_ATTACHED_SRC) {
+	if (get_state_tc(port) == TC_ATTACHED_SRC) {
 		/* Remove VBUS */
 		pd_power_supply_reset(port);
 
@@ -248,24 +300,19 @@ void tc_start_error_recovery(int port)
 	 *   The port should transition to the ErrorRecovery state
 	 *   from any other state when directed.
 	 */
-	sm_set_state(port, TC_OBJ(port), tc_error_recovery);
+	set_state_tc(port, TC_ERROR_RECOVERY);
 }
 
-void tc_state_init(int port, enum typec_state_id start_state)
+static void restart_tc_sm(int port, enum usb_tc_state start_state)
 {
 	int res = 0;
-	sm_state this_state;
 
 	res = tc_restart_tcpc(port);
-	if (res)
-		this_state = tc_disabled;
-	else
-		this_state = (start_state == TC_UNATTACHED_SRC) ?
-				tc_unattached_src : tc_unattached_snk;
 
 	CPRINTS("TCPC p%d init %s", port, res ? "failed" : "ready");
 
-	sm_init_state(port, TC_OBJ(port), this_state);
+	/* Disable if restart failed, otherwise start in default state. */
+	set_state_tc(port, res ? TC_DISABLED : start_state);
 
 	if (IS_ENABLED(CONFIG_USBC_SS_MUX))
 		/* Initialize USB mux to its default state */
@@ -284,13 +331,77 @@ void tc_state_init(int port, enum typec_state_id start_state)
 	tc[port].evt_timeout = 5*MSEC;
 }
 
+void tc_state_init(int port)
+{
+	/* Unattached.SNK is the default starting state. */
+	restart_tc_sm(port, TC_UNATTACHED_SNK);
+}
+
+int tc_get_power_role(int port)
+{
+	return tc[port].power_role;
+}
+
+int tc_get_data_role(int port)
+{
+	return tc[port].data_role;
+}
+
+uint8_t tc_get_polarity(int port)
+{
+	return tc[port].polarity;
+}
+
+uint8_t tc_get_pd_enabled(int port)
+{
+	return tc[port].pd_enable;
+}
+
+void tc_set_power_role(int port, int role)
+{
+	tc[port].power_role = role;
+}
+
+uint64_t tc_get_timeout(int port)
+{
+	return tc[port].evt_timeout;
+}
+
+void tc_set_timeout(int port, uint64_t timeout)
+{
+	tc[port].evt_timeout = timeout;
+}
+
+void tc_event_check(int port, int evt)
+{
+	/* NO EVENTS TO CHECK */
+}
+
 /*
  * Private Functions
  */
 
-void tc_event_check(int port, int evt)
+/* Set the TypeC state machine to a new state. */
+static void set_state_tc(const int port, const enum usb_tc_state new_state)
 {
-	/* NO EVENTS TO CHECK */
+	set_state(port, &tc[port].ctx, &tc_states[new_state]);
+}
+
+/* Get the current TypeC state. */
+test_export_static enum usb_tc_state get_state_tc(const int port)
+{
+	return tc[port].ctx.current - &tc_states[0];
+}
+
+/* Get the previous TypeC state. */
+static enum usb_tc_state get_last_state_tc(const int port)
+{
+	return tc[port].ctx.previous - &tc_states[0];
+}
+
+static void print_current_state(const int port)
+{
+	CPRINTS("C%d: %s", port, tc_state_names[get_state_tc(port)]);
 }
 
 /*
@@ -779,40 +890,26 @@ static void sink_power_sub_states(int port)
  *   Remove the terminations from CC
  *   Set's VBUS and VCONN off
  */
-static int tc_disabled(int port, enum sm_signal sig)
+static void tc_disabled_entry(const int port)
 {
-	int ret = 0;
-
-	ret = (*tc_disabled_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_cc_open);
+	print_current_state(port);
 }
 
-static int tc_disabled_entry(int port)
-{
-	tc[port].state_id = TC_DISABLED;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
-
-	return 0;
-}
-
-static int tc_disabled_run(int port)
+static void tc_disabled_run(const int port)
 {
 	task_wait_event(-1);
-	return SM_RUN_SUPER;
 }
 
-static int tc_disabled_exit(int port)
+static void tc_disabled_exit(const int port)
 {
 	if (!IS_ENABLED(CONFIG_USB_PD_TCPC)) {
 		if (tc_restart_tcpc(port) != 0) {
 			CPRINTS("TCPC p%d restart failed!", port);
-			return 0;
+			return;
 		}
 	}
 
 	CPRINTS("TCPC p%d resumed!", port);
-
-	return 0;
 }
 
 /**
@@ -822,31 +919,19 @@ static int tc_disabled_exit(int port)
  *   Remove the terminations from CC
  *   Set's VBUS and VCONN off
  */
-static int tc_error_recovery(int port, enum sm_signal sig)
+static void tc_error_recovery_entry(const int port)
 {
-	int ret = 0;
-
-	ret = (*tc_error_recovery_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_cc_open);
-}
-
-static int tc_error_recovery_entry(int port)
-{
-	tc[port].state_id = TC_ERROR_RECOVERY;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	tc[port].timeout = get_time().val + PD_T_ERROR_RECOVERY;
-	return 0;
 }
 
-static int tc_error_recovery_run(int port)
+static void tc_error_recovery_run(const int port)
 {
 	if (tc[port].timeout > 0 && get_time().val > tc[port].timeout) {
 		tc[port].timeout = 0;
-		tc_state_init(port, TC_UNATTACHED_SRC);
+		restart_tc_sm(port, TC_UNATTACHED_SRC);
 	}
-
-	return 0;
 }
 
 /**
@@ -857,19 +942,10 @@ static int tc_error_recovery_run(int port)
  *   Place Rd on CC
  *   Set power role to SINK
  */
-static int tc_unattached_snk(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_unattached_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_cc_rd);
-}
-
-static int tc_unattached_snk_entry(int port)
+static void tc_unattached_snk_entry(const int port)
 {
-	tc[port].state_id = TC_UNATTACHED_SNK;
-	if (tc[port].obj.last_state != tc_unattached_src)
-		CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	if (get_last_state_tc(port) != TC_UNATTACHED_SRC)
+		print_current_state(port);
 
 	if (IS_ENABLED(CONFIG_CHARGE_MANAGER))
 		charge_manager_update_dualrole(port, CAP_UNKNOWN);
@@ -880,11 +956,9 @@ static int tc_unattached_snk_entry(int port)
 	 */
 	pd_execute_data_swap(port, PD_ROLE_DISCONNECTED);
 	tc[port].next_role_swap = get_time().val + PD_T_DRP_SNK;
-
-	return 0;
 }
 
-static int tc_unattached_snk_run(int port)
+static void tc_unattached_snk_run(const int port)
 {
 	enum tcpc_cc_voltage_status cc1, cc2;
 
@@ -907,13 +981,13 @@ static int tc_unattached_snk_run(int port)
 	 */
 	if (cc_is_rp(cc1) || cc_is_rp(cc2)) {
 		/* Connection Detected */
-		sm_set_state(port, TC_OBJ(port), tc_attach_wait_snk);
+		set_state_tc(port, TC_ATTACH_WAIT_SNK);
 	} else if (get_time().val > tc[port].next_role_swap) {
 		/* DRP Toggle */
-		sm_set_state(port, TC_OBJ(port), tc_unattached_src);
+		set_state_tc(port, TC_UNATTACHED_SRC);
 	}
 
-	return 0;
+	return;
 }
 
 /**
@@ -924,24 +998,14 @@ static int tc_unattached_snk_run(int port)
  *   Place Rd on CC
  *   Set power role to SINK
  */
-static int tc_attach_wait_snk(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_attach_wait_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_cc_rd);
-}
-
-static int tc_attach_wait_snk_entry(int port)
+static void tc_attach_wait_snk_entry(const int port)
 {
-	tc[port].state_id = TC_ATTACH_WAIT_SNK;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	tc[port].cc_state = PD_CC_UNSET;
-	return 0;
 }
 
-static int tc_attach_wait_snk_run(int port)
+static void tc_attach_wait_snk_run(const int port)
 {
 	enum tcpc_cc_voltage_status cc1, cc2;
 	enum pd_cc_states new_cc_state;
@@ -961,7 +1025,7 @@ static int tc_attach_wait_snk_run(int port)
 		tc[port].cc_debounce = get_time().val + PD_T_CC_DEBOUNCE;
 		tc[port].pd_debounce = get_time().val + PD_T_PD_DEBOUNCE;
 		tc[port].cc_state = new_cc_state;
-		return 0;
+		return;
 	}
 
 	/*
@@ -971,12 +1035,13 @@ static int tc_attach_wait_snk_run(int port)
 	if (new_cc_state == PD_CC_NONE &&
 				get_time().val > tc[port].pd_debounce) {
 		/* We are detached */
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_src);
+		set_state_tc(port, TC_UNATTACHED_SRC);
+		return;
 	}
 
 	/* Wait for CC debounce */
 	if (get_time().val < tc[port].cc_debounce)
-		return 0;
+		return;
 
 	/*
 	 * The port shall transition to Attached.SNK after the state of only one
@@ -995,38 +1060,26 @@ static int tc_attach_wait_snk_run(int port)
 		if (new_cc_state == PD_CC_DFP_ATTACHED) {
 #ifdef CONFIG_USB_PD_TRY_SRC
 			if (pd_try_src_enable)
-				sm_set_state(port, TC_OBJ(port), tc_try_src);
+				set_state_tc(port, TC_TRY_SRC);
 			else
 #endif
-				sm_set_state(port, TC_OBJ(port),
-							tc_attached_snk);
+				set_state_tc(port, TC_ATTACHED_SNK);
 		} else {
 			/* new_cc_state is PD_CC_DEBUG_ACC */
 			TC_SET_FLAG(port, TC_FLAGS_TS_DTS_PARTNER);
-			sm_set_state(port, TC_OBJ(port), tc_dbg_acc_snk);
+			set_state_tc(port, TC_DBG_ACC_SNK);
 		}
 	}
-
-	return SM_RUN_SUPER;
 }
 
 /**
  * Attached.SNK
  */
-static int tc_attached_snk(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_attached_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tc_attached_snk_entry(int port)
+static void tc_attached_snk_entry(const int port)
 {
 	enum tcpc_cc_voltage_status cc1, cc2;
 
-	tc[port].state_id = TC_ATTACHED_SNK;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Get connector orientation */
 	tcpm_get_cc(port, &cc1, &cc2);
@@ -1052,27 +1105,24 @@ static int tc_attached_snk_entry(int port)
 	tcpm_set_cc(port, TYPEC_CC_RD);
 
 	tc[port].cc_debounce = 0;
-	return 0;
 }
 
-static int tc_attached_snk_run(int port)
+static void tc_attached_snk_run(const int port)
 {
 	/* Detach detection */
-	if (!pd_is_vbus_present(port))
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
+	if (!pd_is_vbus_present(port)) {
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
+	}
 
 	/* Run Sink Power Sub-State */
 	sink_power_sub_states(port);
-
-	return 0;
 }
 
-static int tc_attached_snk_exit(int port)
+static void tc_attached_snk_exit(const int port)
 {
 	/* Stop drawing power */
 	sink_stop_drawing_current(port);
-
-	return 0;
 }
 
 /**
@@ -1083,28 +1133,17 @@ static int tc_attached_snk_exit(int port)
  *  Place Rp on CC
  *  Set power role to SOURCE
  */
-static int tc_unoriented_dbg_acc_src(int port, enum sm_signal sig)
+static void tc_unoriented_dbg_acc_src_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_unoriented_dbg_acc_src_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_cc_rp);
-}
-
-static int tc_unoriented_dbg_acc_src_entry(int port)
-{
-	tc[port].state_id = TC_UNORIENTED_DEBUG_ACCESSORY_SRC;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Enable VBUS */
 	pd_set_power_supply_ready(port);
 
 	/* Any board specific unoriented debug setup should be added below */
-
-	return 0;
 }
 
-static int tc_unoriented_dbg_acc_src_run(int port)
+static void tc_unoriented_dbg_acc_src_run(const int port)
 {
 	enum tcpc_cc_voltage_status cc1, cc2;
 
@@ -1121,10 +1160,8 @@ static int tc_unoriented_dbg_acc_src_run(int port)
 		charge_manager_set_ceil(port, CEIL_REQUESTOR_PD,
 							CHARGE_CEIL_NONE);
 
-		sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
+		set_state_tc(port, TC_UNATTACHED_SNK);
 	}
-
-	return 0;
 }
 
 /**
@@ -1135,33 +1172,20 @@ static int tc_unoriented_dbg_acc_src_run(int port)
  *   Place Rd on CC
  *   Set power role to SINK
  */
-static int tc_dbg_acc_snk(int port, enum sm_signal sig)
+static void tc_dbg_acc_snk_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_dbg_acc_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_cc_rd);
-}
-
-static int tc_dbg_acc_snk_entry(int port)
-{
-	tc[port].state_id = TC_DEBUG_ACCESSORY_SNK;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/*
 	 * TODO(b/137759869): Board specific debug accessory setup should
 	 * be add here.
 	 */
-
-	return 0;
 }
 
-static int tc_dbg_acc_snk_run(int port)
+static void tc_dbg_acc_snk_run(const int port)
 {
 	if (!pd_is_vbus_present(port))
-		sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
-
-	return 0;
+		set_state_tc(port, TC_UNATTACHED_SNK);
 }
 
 /**
@@ -1172,19 +1196,10 @@ static int tc_dbg_acc_snk_run(int port)
  *   Place Rp on CC
  *   Set power role to SOURCE
  */
-static int tc_unattached_src(int port, enum sm_signal sig)
+static void tc_unattached_src_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_unattached_src_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_cc_rp);
-}
-
-static int tc_unattached_src_entry(int port)
-{
-	tc[port].state_id = TC_UNATTACHED_SRC;
-	if (tc[port].obj.last_state != tc_unattached_snk)
-		CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	if (get_last_state_tc(port) != TC_UNATTACHED_SNK)
+		print_current_state(port);
 
 	if (IS_ENABLED(CONFIG_USBC_PPC)) {
 		/* There is no sink connected. */
@@ -1209,11 +1224,9 @@ static int tc_unattached_src_entry(int port)
 	pd_execute_data_swap(port, PD_ROLE_DISCONNECTED);
 
 	tc[port].next_role_swap = get_time().val + PD_T_DRP_SRC;
-
-	return 0;
 }
 
-static int tc_unattached_src_run(int port)
+static void tc_unattached_src_run(const int port)
 {
 	enum tcpc_cc_voltage_status cc1, cc2;
 
@@ -1229,11 +1242,9 @@ static int tc_unattached_src_run(int port)
 	 * after dcSRC.DRP ∙ tDRP
 	 */
 	if (cc_is_at_least_one_rd(cc1, cc2) || cc_is_audio_acc(cc1, cc2))
-		sm_set_state(port, TC_OBJ(port), tc_attach_wait_src);
+		set_state_tc(port, TC_ATTACH_WAIT_SRC);
 	else if (get_time().val > tc[port].next_role_swap)
-		sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
-
-	return SM_RUN_SUPER;
+		set_state_tc(port, TC_UNATTACHED_SNK);
 }
 
 /**
@@ -1244,25 +1255,14 @@ static int tc_unattached_src_run(int port)
  *   Place Rp on CC
  *   Set power role to SOURCE
  */
-static int tc_attach_wait_src(int port, enum sm_signal sig)
+static void tc_attach_wait_src_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_attach_wait_src_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_cc_rp);
-}
-
-static int tc_attach_wait_src_entry(int port)
-{
-	tc[port].state_id = TC_ATTACH_WAIT_SRC;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	tc[port].cc_state = PD_CC_UNSET;
-
-	return 0;
 }
 
-static int tc_attach_wait_src_run(int port)
+static void tc_attach_wait_src_run(const int port)
 {
 	enum tcpc_cc_voltage_status cc1, cc2;
 	enum pd_cc_states new_cc_state;
@@ -1282,19 +1282,20 @@ static int tc_attach_wait_src_run(int port)
 		new_cc_state = PD_CC_AUDIO_ACC;
 	} else {
 		/* No UFP */
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
 	}
 
 	/* Debounce the cc state */
 	if (new_cc_state != tc[port].cc_state) {
 		tc[port].cc_debounce = get_time().val + PD_T_CC_DEBOUNCE;
 		tc[port].cc_state = new_cc_state;
-		return 0;
+		return;
 	}
 
 	/* Wait for CC debounce */
 	if (get_time().val < tc[port].cc_debounce)
-		return 0;
+		return;
 
 	/*
 	 * The port shall transition to Attached.SRC when VBUS is at vSafe0V
@@ -1307,34 +1308,25 @@ static int tc_attach_wait_src_run(int port)
 	 * tCCDebounce.
 	 */
 	if (!pd_is_vbus_present(port)) {
-		if (new_cc_state == PD_CC_UFP_ATTACHED)
-			return sm_set_state(port, TC_OBJ(port),
-							tc_attached_src);
-		else if (new_cc_state == PD_CC_DEBUG_ACC)
-			return sm_set_state(port, TC_OBJ(port),
-						tc_unoriented_dbg_acc_src);
+		if (new_cc_state == PD_CC_UFP_ATTACHED) {
+			set_state_tc(port, TC_ATTACHED_SRC);
+			return;
+		} else if (new_cc_state == PD_CC_DEBUG_ACC) {
+			set_state_tc(port, TC_UNORIENTED_DBG_ACC_SRC);
+			return;
+		}
 	}
 
-	return 0;
 }
 
 /**
  * Attached.SRC
  */
-static int tc_attached_src(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_attached_src_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tc_attached_src_entry(int port)
+static void tc_attached_src_entry(const int port)
 {
 	enum tcpc_cc_voltage_status cc1, cc2;
 
-	tc[port].state_id = TC_ATTACHED_SRC;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Get connector orientation */
 	tcpm_get_cc(port, &cc1, &cc2);
@@ -1374,11 +1366,9 @@ static int tc_attached_src_entry(int port)
 	/* Inform PPC that a sink is connected. */
 	if (IS_ENABLED(CONFIG_USBC_PPC))
 		ppc_sink_is_connected(port, 1);
-
-	return 0;
 }
 
-static int tc_attached_src_run(int port)
+static void tc_attached_src_run(const int port)
 {
 	enum tcpc_cc_voltage_status cc1, cc2;
 	enum pd_cc_states new_cc_state;
@@ -1401,7 +1391,7 @@ static int tc_attached_src_run(int port)
 	}
 
 	if (get_time().val < tc[port].cc_debounce)
-		return 0;
+		return;
 
 	/*
 	 * When the SRC.Open state is detected on the monitored CC pin, a DRP
@@ -1414,26 +1404,18 @@ static int tc_attached_src_run(int port)
 	 * Attached.SRC.
 	 */
 	if (tc[port].cc_state == PD_CC_NO_UFP) {
-		if (IS_ENABLED(CONFIG_USB_PD_TRY_SRC))
-			return sm_set_state(port, TC_OBJ(port),
-							tc_try_wait_snk);
-		else
-			return sm_set_state(port, TC_OBJ(port),
-							tc_unattached_snk);
+		set_state_tc(port, IS_ENABLED(CONFIG_USB_PD_TRY_SRC) ?
+			TC_TRY_WAIT_SNK : TC_UNATTACHED_SNK);
 	}
-
-	return 0;
 }
 
-static int tc_attached_src_exit(int port)
+static void tc_attached_src_exit(const int port)
 {
 	/*
 	 * A port shall cease to supply VBUS within tVBUSOFF of exiting
 	 * Attached.SRC.
 	 */
 	tc_src_power_off(port);
-
-	return 0;
 }
 
 /**
@@ -1445,26 +1427,16 @@ static int tc_attached_src_exit(int port)
  *   Set power role to SOURCE
  */
 #ifdef CONFIG_USB_PD_TRY_SRC
-static int tc_try_src(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_try_src_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_cc_rp);
-}
-
-static int tc_try_src_entry(int port)
+static void tc_try_src_entry(const int port)
 {
-	tc[port].state_id = TC_TRY_SRC;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	tc[port].cc_state = PD_CC_UNSET;
 	tc[port].try_wait_debounce = get_time().val + PD_T_DRP_TRY;
 	tc[port].timeout = get_time().val + PD_T_TRY_TIMEOUT;
-	return 0;
 }
 
-static int tc_try_src_run(int port)
+static void tc_try_src_run(const int port)
 {
 	enum tcpc_cc_voltage_status cc1, cc2;
 	enum pd_cc_states new_cc_state;
@@ -1491,7 +1463,7 @@ static int tc_try_src_run(int port)
 	 */
 	if (get_time().val > tc[port].cc_debounce) {
 		if (new_cc_state == PD_CC_UFP_ATTACHED)
-			sm_set_state(port, TC_OBJ(port), tc_attached_src);
+			set_state_tc(port, TC_ATTACHED_SRC);
 	}
 
 	/*
@@ -1503,11 +1475,9 @@ static int tc_try_src_run(int port)
 		if ((get_time().val > tc[port].try_wait_debounce &&
 					!pd_is_vbus_present(port)) ||
 					get_time().val > tc[port].timeout) {
-			sm_set_state(port, TC_OBJ(port), tc_try_wait_snk);
+			set_state_tc(port, TC_TRY_WAIT_SNK);
 		}
 	}
-
-	return 0;
 }
 
 /**
@@ -1518,26 +1488,15 @@ static int tc_try_src_run(int port)
  *   Place Rd on CC
  *   Set power role to SINK
  */
-static int tc_try_wait_snk(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_try_wait_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_cc_rd);
-}
-
-static int tc_try_wait_snk_entry(int port)
+static void tc_try_wait_snk_entry(const int port)
 {
-	tc[port].state_id = TC_TRY_WAIT_SNK;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	tc[port].cc_state = PD_CC_UNSET;
 	tc[port].try_wait_debounce = get_time().val + PD_T_CC_DEBOUNCE;
-
-	return 0;
 }
 
-static int tc_try_wait_snk_run(int port)
+static void tc_try_wait_snk_run(const int port)
 {
 	enum tcpc_cc_voltage_status cc1, cc2;
 	enum pd_cc_states new_cc_state;
@@ -1563,7 +1522,8 @@ static int tc_try_wait_snk_run(int port)
 	 */
 	if ((get_time().val > tc[port].pd_debounce) &&
 						(new_cc_state == PD_CC_NONE)) {
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
 	}
 
 	/*
@@ -1572,9 +1532,7 @@ static int tc_try_wait_snk_run(int port)
 	 */
 	if (get_time().val > tc[port].try_wait_debounce &&
 						pd_is_vbus_present(port))
-		sm_set_state(port, TC_OBJ(port), tc_attached_snk);
-
-	return 0;
+		set_state_tc(port, TC_ATTACHED_SNK);
 }
 
 #endif
@@ -1582,15 +1540,7 @@ static int tc_try_wait_snk_run(int port)
 /**
  * Super State CC_RD
  */
-static int tc_cc_rd(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_cc_rd_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tc_cc_rd_entry(int port)
+static void tc_cc_rd_entry(const int port)
 {
 	/* Disable VCONN */
 	if (IS_ENABLED(CONFIG_USBC_VCONN))
@@ -1605,22 +1555,12 @@ static int tc_cc_rd_entry(int port)
 	/* Set power role to sink */
 	tc_set_power_role(port, PD_ROLE_SINK);
 	tcpm_set_msg_header(port, tc[port].power_role, tc[port].data_role);
-
-	return 0;
 }
 
 /**
  * Super State CC_RP
  */
-static int tc_cc_rp(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_cc_rp_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tc_cc_rp_entry(int port)
+static void tc_cc_rp_entry(const int port)
 {
 	/* Disable VCONN */
 	if (IS_ENABLED(CONFIG_USBC_VCONN))
@@ -1636,22 +1576,12 @@ static int tc_cc_rp_entry(int port)
 	 */
 	tcpm_select_rp_value(port, CONFIG_USB_PD_PULLUP);
 	tcpm_set_cc(port, TYPEC_CC_RP);
-
-	return 0;
 }
 
 /**
  * Super State CC_OPEN
  */
-static int tc_cc_open(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_cc_open_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tc_cc_open_entry(int port)
+static void tc_cc_open_entry(const int port)
 {
 	/* Disable VBUS */
 	pd_power_supply_reset(port);
@@ -1673,6 +1603,106 @@ static int tc_cc_open_entry(int port)
 		 */
 		ppc_clear_oc_event_counter(port);
 	}
+}
 
-	return 0;
+void tc_run(const int port)
+{
+	exe_state(port, &tc[port].ctx);
 }
+
+/*
+ * Type-C State Hierarchy (Sub-States are listed inside the boxes)
+ *
+ * |TC_CC_RD --------------|	|TC_CC_RP ------------------------|
+ * |			   |	|				  |
+ * |	TC_UNATTACHED_SNK  |	|	TC_UNATTACHED_SRC         |
+ * |	TC_ATTACH_WAIT_SNK |	|	TC_ATTACH_WAIT_SRC        |
+ * |	TC_TRY_WAIT_SNK    |	|	TC_TRY_SRC                |
+ * |	TC_DBG_ACC_SNK     |	|	TC_UNORIENTED_DBG_ACC_SRC |
+ * |-----------------------|	|---------------------------------|
+ *
+ * |TC_CC_OPEN -----------|
+ * |                      |
+ * |	TC_DISABLED       |
+ * |	TC_ERROR_RECOVERY |
+ * |----------------------|
+ *
+ * TC_ATTACHED_SNK		TC_ATTACHED_SRC
+ *
+ */
+static const struct usb_state tc_states[] = {
+	/* Super States */
+	[TC_CC_OPEN] = {
+		.entry	= tc_cc_open_entry,
+	},
+	[TC_CC_RD] = {
+		.entry	= tc_cc_rd_entry,
+	},
+	[TC_CC_RP] = {
+		.entry	= tc_cc_rp_entry,
+	},
+	/* Normal States */
+	[TC_DISABLED] = {
+		.entry	= tc_disabled_entry,
+		.run	= tc_disabled_run,
+		.exit	= tc_disabled_exit,
+		.parent = &tc_states[TC_CC_OPEN],
+	},
+	[TC_ERROR_RECOVERY] = {
+		.entry	= tc_error_recovery_entry,
+		.run	= tc_error_recovery_run,
+		.parent = &tc_states[TC_CC_OPEN],
+	},
+	[TC_UNATTACHED_SNK] = {
+		.entry	= tc_unattached_snk_entry,
+		.run	= tc_unattached_snk_run,
+		.parent = &tc_states[TC_CC_RD],
+	},
+	[TC_ATTACH_WAIT_SNK] = {
+		.entry	= tc_attach_wait_snk_entry,
+		.run	= tc_attach_wait_snk_run,
+		.parent = &tc_states[TC_CC_RD],
+	},
+	[TC_ATTACHED_SNK] = {
+		.entry	= tc_attached_snk_entry,
+		.run	= tc_attached_snk_run,
+		.exit	= tc_attached_snk_exit,
+	},
+	[TC_UNORIENTED_DBG_ACC_SRC] = {
+		.entry	= tc_unoriented_dbg_acc_src_entry,
+		.run	= tc_unoriented_dbg_acc_src_run,
+		.parent = &tc_states[TC_CC_RP],
+	},
+	[TC_DBG_ACC_SNK] = {
+		.entry	= tc_dbg_acc_snk_entry,
+		.run	= tc_dbg_acc_snk_run,
+		.parent = &tc_states[TC_CC_RD],
+	},
+	[TC_UNATTACHED_SRC] = {
+		.entry	= tc_unattached_src_entry,
+		.run	= tc_unattached_src_run,
+		.parent = &tc_states[TC_CC_RP],
+	},
+	[TC_ATTACH_WAIT_SRC] = {
+		.entry	= tc_attach_wait_src_entry,
+		.run	= tc_attach_wait_src_run,
+		.parent = &tc_states[TC_CC_RP],
+	},
+	[TC_ATTACHED_SRC] = {
+		.entry	= tc_attached_src_entry,
+		.run	= tc_attached_src_run,
+		.exit	= tc_attached_src_exit,
+	},
+#ifdef CONFIG_USB_PD_TRY_SRC
+	[TC_TRY_SRC] = {
+		.entry	= tc_try_src_entry,
+		.run	= tc_try_src_run,
+		.parent = &tc_states[TC_CC_RP],
+	},
+	[TC_TRY_WAIT_SNK] = {
+		.entry	= tc_try_wait_snk_entry,
+		.run	= tc_try_wait_snk_run,
+		.parent = &tc_states[TC_CC_RD],
+	},
+#endif /* CONFIG_USB_PD_TRY_SRC */
+};
diff --git a/common/usbc/usb_tc_vpd_sm.c b/common/usbc/usb_tc_vpd_sm.c
index 9030bc4e39..1abbf7d4f2 100644
--- a/common/usbc/usb_tc_vpd_sm.c
+++ b/common/usbc/usb_tc_vpd_sm.c
@@ -10,7 +10,7 @@
 #include "tcpm.h"
 #include "usb_pd.h"
 #include "usb_tc_sm.h"
-#include "usb_tc_vpd_sm.h"
+#include "usb_sm.h"
 #include "vpd_api.h"
 
 /* USB Type-C VCONN Powered Device module */
@@ -30,33 +30,68 @@
  * This is the Type-C Port object that contains information needed to
  * implement a VCONN Powered Device.
  */
-struct type_c tc[CONFIG_USB_PD_PORT_COUNT];
+static struct type_c {
+	/* state machine context */
+	struct sm_ctx ctx;
+	/* current port power role (VPD, SOURCE or SINK) */
+	uint8_t power_role;
+	/* current port data role (DFP or UFP) */
+	uint8_t data_role;
+	/* Higher-level power deliver state machines are enabled if true. */
+	uint8_t pd_enable;
+	/* event timeout */
+	uint64_t evt_timeout;
+	/* port flags, see TC_FLAGS_* */
+	uint32_t flags;
+	/* Time a port shall wait before it can determine it is attached */
+	uint64_t cc_debounce;
+	/* VPD host port cc state */
+	enum pd_cc_states host_cc_state;
+	uint8_t ct_cc;
+} tc[CONFIG_USB_PD_PORT_COUNT];
+
+/* List of all TypeC-level states */
+enum usb_tc_state {
+	/* Normal States */
+	TC_DISABLED,
+	TC_UNATTACHED_SNK,
+	TC_ATTACH_WAIT_SNK,
+	TC_ATTACHED_SNK,
+	/* Super States */
+	TC_VBUS_CC_ISO,
+	TC_HOST_RARD,
+	TC_HOST_OPEN,
+};
+/* Forward declare the full list of states. This is indexed by usb_tc_state */
+static const struct usb_state tc_states[];
 
-/* Type-C states */
-DECLARE_STATE(tc, disabled, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(tc, unattached_snk, WITH_RUN, NOOP);
-DECLARE_STATE(tc, attach_wait_snk, WITH_RUN, NOOP);
-DECLARE_STATE(tc, attached_snk, WITH_RUN, WITH_EXIT);
+#ifdef CONFIG_COMMON_RUNTIME
+/* List of human readable state names for console debugging */
+static const char * const tc_state_names[] = {
+	[TC_DISABLED] = "Disabled",
+	[TC_UNATTACHED_SNK] = "Unattached.SNK",
+	[TC_ATTACH_WAIT_SNK] = "AttachWait.SNK",
+	[TC_ATTACHED_SNK] = "Attached.SNK",
+};
+#endif
 
-/* Super States */
-DECLARE_STATE(tc, host_rard, NOOP, NOOP);
-DECLARE_STATE(tc, host_open, NOOP, NOOP);
-DECLARE_STATE(tc, vbus_cc_iso, NOOP, NOOP);
+/* Forward declare private, common functions */
+static void set_state_tc(const int port, enum usb_tc_state new_state);
 
-void tc_state_init(int port, enum typec_state_id start_state)
+/* Public TypeC functions */
+
+void tc_state_init(int port)
 {
 	int res = 0;
-	sm_state this_state;
 
 	res = tc_restart_tcpc(port);
 
 	CPRINTS("TCPC p%d init %s", port, res ? "failed" : "ready");
-	this_state = res ? tc_disabled : tc_unattached_snk;
 
 	/* Disable TCPC RX until connection is established */
 	tcpm_set_rx_enable(port, 0);
 
-	sm_init_state(port, TC_OBJ(port), this_state);
+	set_state_tc(port, res ? TC_DISABLED : TC_UNATTACHED_SNK);
 
 	/* Disable pd state machines */
 	tc[port].pd_enable = 0;
@@ -66,11 +101,69 @@ void tc_state_init(int port, enum typec_state_id start_state)
 	tc[port].flags = 0;
 }
 
+
+int tc_get_power_role(int port)
+{
+	return tc[port].power_role;
+}
+
+int tc_get_data_role(int port)
+{
+	return tc[port].data_role;
+}
+
+uint8_t tc_get_polarity(int port)
+{
+	/* Does not track polarity yet */
+	return 0;
+}
+
+uint8_t tc_get_pd_enabled(int port)
+{
+	return tc[port].pd_enable;
+}
+
+void tc_set_power_role(int port, int role)
+{
+	tc[port].power_role = role;
+}
+
+uint64_t tc_get_timeout(int port)
+{
+	return tc[port].evt_timeout;
+}
+
+void tc_set_timeout(int port, uint64_t timeout)
+{
+	tc[port].evt_timeout = timeout;
+}
+
 void tc_event_check(int port, int evt)
 {
 	/* Do Nothing */
 }
 
+/*
+ * Private Functions
+ */
+
+/* Set the TypeC state machine to a new state. */
+static void set_state_tc(const int port, const enum usb_tc_state new_state)
+{
+	set_state(port, &tc[port].ctx, &tc_states[new_state]);
+}
+
+/* Get the current TypeC state. */
+test_export_static enum usb_tc_state get_state_tc(const int port)
+{
+	return tc[port].ctx.current - &tc_states[0];
+}
+
+static void print_current_state(const int port)
+{
+	CPRINTS("C%d: %s", port, tc_state_names[get_state_tc(port)]);
+}
+
 /**
  * Disabled
  *
@@ -78,40 +171,26 @@ void tc_event_check(int port, int evt)
  *   Enable mcu communication
  *   Remove the terminations from Host CC
  */
-static int tc_disabled(int port, enum sm_signal sig)
+static void tc_disabled_entry(const int port)
 {
-	int ret = 0;
-
-	ret = (*tc_disabled_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_open);
-}
-
-static int tc_disabled_entry(int port)
-{
-	tc[port].state_id = TC_DISABLED;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
-
-	return 0;
+	print_current_state(port);
 }
 
-static int tc_disabled_run(int port)
+static void tc_disabled_run(const int port)
 {
 	task_wait_event(-1);
-
-	return SM_RUN_SUPER;
 }
 
-static int tc_disabled_exit(int port)
+static void tc_disabled_exit(const int port)
 {
-#ifndef CONFIG_USB_PD_TCPC
-	if (tc_restart_tcpc(port) != 0) {
-		CPRINTS("TCPC p%d restart failed!", port);
-		return 0;
+	if (!IS_ENABLED(CONFIG_USB_PD_TCPC)) {
+		if (tc_restart_tcpc(port) != 0) {
+			CPRINTS("TCPC p%d restart failed!", port);
+			return;
+		}
 	}
-#endif
-	CPRINTS("TCPC p%d resumed!", port);
 
-	return 0;
+	CPRINTS("TCPC p%d resumed!", port);
 }
 
 /**
@@ -121,23 +200,12 @@ static int tc_disabled_exit(int port)
  *   Enable mcu communication
  *   Place Ra on VCONN and Rd on Host CC
  */
-static int tc_unattached_snk(int port, enum sm_signal sig)
+static void tc_unattached_snk_entry(const int port)
 {
-	int ret;
-
-	ret = (*tc_unattached_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rard);
+	print_current_state(port);
 }
 
-static int tc_unattached_snk_entry(int port)
-{
-	tc[port].state_id = TC_UNATTACHED_SNK;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
-
-	return 0;
-}
-
-static int tc_unattached_snk_run(int port)
+static void tc_unattached_snk_run(const int port)
 {
 	int host_cc;
 
@@ -150,9 +218,7 @@ static int tc_unattached_snk_run(int port)
 	 * port’s CC pin.
 	 */
 	if (cc_is_rp(host_cc))
-		return sm_set_state(port, TC_OBJ(port), tc_attach_wait_snk);
-
-	return SM_RUN_SUPER;
+		set_state_tc(port, TC_ATTACH_WAIT_SNK);
 }
 
 /**
@@ -162,24 +228,13 @@ static int tc_unattached_snk_run(int port)
  *   Enable mcu communication
  *   Place Ra on VCONN and Rd on Host CC
  */
-static int tc_attach_wait_snk(int port, enum sm_signal sig)
+static void tc_attach_wait_snk_entry(const int port)
 {
-	int ret = 0;
-
-	ret = (*tc_attach_wait_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_host_rard);
-}
-
-static int tc_attach_wait_snk_entry(int port)
-{
-	tc[port].state_id = TC_ATTACH_WAIT_SNK;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 	tc[port].host_cc_state = PD_CC_UNSET;
-
-	return 0;
 }
 
-static int tc_attach_wait_snk_run(int port)
+static void tc_attach_wait_snk_run(const int port)
 {
 	int host_new_cc_state;
 	int host_cc;
@@ -202,12 +257,12 @@ static int tc_attach_wait_snk_run(int port)
 			tc[port].cc_debounce = get_time().val +
 							PD_T_PD_DEBOUNCE;
 
-		return 0;
+		return;
 	}
 
 	/* Wait for Host CC debounce */
 	if (get_time().val < tc[port].cc_debounce)
-		return 0;
+		return;
 
 	/*
 	 * A VCONN-Powered USB Device shall transition to
@@ -220,41 +275,30 @@ static int tc_attach_wait_snk_run(int port)
 	 */
 	if (tc[port].host_cc_state == PD_CC_DFP_ATTACHED &&
 			(vpd_is_vconn_present() || vpd_is_host_vbus_present()))
-		sm_set_state(port, TC_OBJ(port), tc_attached_snk);
+		set_state_tc(port, TC_ATTACHED_SNK);
 	else if (tc[port].host_cc_state == PD_CC_NONE)
-		sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
-
-	return 0;
+		set_state_tc(port, TC_UNATTACHED_SNK);
 }
 
 /**
  * Attached.SNK
  */
-static int tc_attached_snk(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_attached_snk_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tc_attached_snk_entry(int port)
+static void tc_attached_snk_entry(const int port)
 {
-	tc[port].state_id = TC_ATTACHED_SNK;
-	CPRINTS("C%d: %s", port, tc_state_names[tc[port].state_id]);
+	print_current_state(port);
 
 	/* Enable PD */
 	tc[port].pd_enable = 1;
 	set_polarity(port, 0);
-
-	return 0;
 }
 
-static int tc_attached_snk_run(int port)
+static void tc_attached_snk_run(const int port)
 {
 	/* Has host vbus and vconn been removed */
-	if (!vpd_is_host_vbus_present() && !vpd_is_vconn_present())
-		return sm_set_state(port, TC_OBJ(port), tc_unattached_snk);
+	if (!vpd_is_host_vbus_present() && !vpd_is_vconn_present()) {
+		set_state_tc(port, TC_UNATTACHED_SNK);
+		return;
+	}
 
 	if (vpd_is_vconn_present()) {
 		if (!(tc[port].flags & TC_FLAGS_VCONN_ON)) {
@@ -263,72 +307,94 @@ static int tc_attached_snk_run(int port)
 			tc[port].flags |= TC_FLAGS_VCONN_ON;
 		}
 	}
-
-	return 0;
 }
 
-static int tc_attached_snk_exit(int port)
+static void tc_attached_snk_exit(const int port)
 {
 	/* Disable PD */
 	tc[port].pd_enable = 0;
 	tc[port].flags &= ~TC_FLAGS_VCONN_ON;
-
-	return 0;
 }
 
 /**
  * Super State HOST_RARD
  */
-static int tc_host_rard(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_host_rard_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_vbus_cc_iso);
-}
-
-static int tc_host_rard_entry(int port)
+static void tc_host_rard_entry(const int port)
 {
 	/* Place Ra on VCONN and Rd on Host CC */
 	vpd_host_set_pull(TYPEC_CC_RA_RD, 0);
-
-	return 0;
 }
 
 /**
  * Super State HOST_OPEN
  */
-static int tc_host_open(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_host_open_sig[sig])(port);
-	return SM_SUPER(ret, sig, tc_vbus_cc_iso);
-}
-
-static int tc_host_open_entry(int port)
+static void tc_host_open_entry(const int port)
 {
 	/* Remove the terminations from Host CC */
 	vpd_host_set_pull(TYPEC_CC_OPEN, 0);
-
-	return 0;
 }
 
 /**
  * Super State VBUS_CC_ISO
  */
-static int tc_vbus_cc_iso(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*tc_vbus_cc_iso_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int tc_vbus_cc_iso_entry(int port)
+static void tc_vbus_cc_iso_entry(const int port)
 {
 	/* Enable mcu communication and cc */
 	vpd_mcu_cc_en(1);
+}
 
-	return 0;
+void tc_run(const int port)
+{
+	exe_state(port, &tc[port].ctx);
 }
+
+/*
+ * Type-C State Hierarchy (Sub-States are listed inside the boxes)
+ *
+ * | TC_VBUS_CC_ISO ----------------------------------------|
+ * |                                                        |
+ * |  | TC_HOST_RARD -----------| | TC_HOST_OPEN ---------| |
+ * |  |                         | |                       | |
+ * |  | TC_UNATTACHED_SNK       | | TC_DISABLED           | |
+ * |  | TC_ATTACH_WAIT_SNK      | |-----------------------| |
+ * |  |-------------------------|                           |
+ * |--------------------------------------------------------|
+ *
+ * TC_ATTACHED_SNK
+ */
+static const struct usb_state tc_states[] = {
+	/* Super States */
+	[TC_VBUS_CC_ISO] = {
+		.entry  = tc_vbus_cc_iso_entry,
+	},
+	[TC_HOST_RARD] = {
+		.entry  = tc_host_rard_entry,
+		.parent = &tc_states[TC_VBUS_CC_ISO],
+	},
+	[TC_HOST_OPEN] = {
+		.entry  = tc_host_open_entry,
+		.parent = &tc_states[TC_VBUS_CC_ISO],
+	},
+	/* Normal States */
+	[TC_DISABLED] = {
+		.entry  = tc_disabled_entry,
+		.run    = tc_disabled_run,
+		.exit   = tc_disabled_exit,
+		.parent = &tc_states[TC_HOST_OPEN],
+	},
+	[TC_UNATTACHED_SNK] = {
+		.entry  = tc_unattached_snk_entry,
+		.run    = tc_unattached_snk_run,
+		.parent = &tc_states[TC_HOST_RARD],
+	},
+	[TC_ATTACH_WAIT_SNK] = {
+		.entry  = tc_attach_wait_snk_entry,
+		.run    = tc_attach_wait_snk_run,
+		.parent = &tc_states[TC_HOST_RARD],
+	},
+	[TC_ATTACHED_SNK] = {
+		.entry  = tc_attached_snk_entry,
+		.run    = tc_attached_snk_run,
+		.exit   = tc_attached_snk_exit,
+	},
+};
diff --git a/common/usbc/usbc_task.c b/common/usbc/usbc_task.c
index c081f7649f..f34bdee088 100644
--- a/common/usbc/usbc_task.c
+++ b/common/usbc/usbc_task.c
@@ -32,90 +32,6 @@
 #include "usbc_ppc.h"
 #include "version.h"
 
-/* Include USB Type-C State Machine Header File */
-#if defined(CONFIG_USB_TYPEC_CTVPD)
-#include "usb_tc_ctvpd_sm.h"
-#elif defined(CONFIG_USB_TYPEC_VPD)
-#include "usb_tc_vpd_sm.h"
-#elif defined(CONFIG_USB_TYPEC_DRP_ACC_TRYSRC)
-#include "usb_tc_drp_acc_trysrc_sm.h"
-#else
-#error "A USB Type-C State Machine must be defined."
-#endif
-
-#ifdef CONFIG_COMMON_RUNTIME
-#define CPRINTF(format, args...) cprintf(CC_USB, format, ## args)
-#define CPRINTS(format, args...) cprints(CC_USB, format, ## args)
-#else /* CONFIG_COMMON_RUNTIME */
-#define CPRINTF(format, args...)
-#define CPRINTS(format, args...)
-#endif
-
-#ifdef CONFIG_COMMON_RUNTIME
-const char * const tc_state_names[] = {
-	"Disabled",
-	"Unattached.SNK",
-	"AttachWait.SNK",
-	"Attached.SNK",
-#if !defined(CONFIG_USB_TYPEC_VPD)
-	"ErrorRecovery",
-	"Unattached.SRC",
-	"AttachWait.SRC",
-	"Attached.SRC",
-#endif
-#if !defined(CONFIG_USB_TYPEC_CTVPD) && !defined(CONFIG_USB_TYPEC_VPD)
-	"AudioAccessory",
-	"OrientedDebugAccessory.SRC",
-	"UnorientedDebugAccessory.SRC",
-	"DebugAccessory.SNK",
-	"Try.SRC",
-	"TryWait.SNK",
-	"CTUnattached.SNK",
-	"CTAttached.SNK",
-#endif
-#if defined(CONFIG_USB_TYPEC_CTVPD)
-	"CTTry.SNK",
-	"CTAttached.Unsupported",
-	"CTAttachWait.Unsupported",
-	"CTUnattached.Unsupported",
-	"CTUnattached.VPD",
-	"CTAttachWait.VPD",
-	"CTAttached.VPD",
-	"CTDisabled.VPD",
-	"Try.SNK",
-	"TryWait.SRC"
-#endif
-};
-BUILD_ASSERT(ARRAY_SIZE(tc_state_names) == TC_STATE_COUNT);
-#endif
-
-/* Public Functions */
-
-int tc_get_power_role(int port)
-{
-	return tc[port].power_role;
-}
-
-int tc_get_data_role(int port)
-{
-	return tc[port].data_role;
-}
-
-void tc_set_power_role(int port, int role)
-{
-	tc[port].power_role = role;
-}
-
-void tc_set_timeout(int port, uint64_t timeout)
-{
-	tc[port].evt_timeout = timeout;
-}
-
-enum typec_state_id get_typec_state_id(int port)
-{
-	return tc[port].state_id;
-}
-
 int tc_restart_tcpc(int port)
 {
 	return tcpm_init(port);
@@ -139,7 +55,7 @@ void set_usb_mux_with_current_data_role(int port)
 #ifdef CONFIG_POWER_COMMON
 	if (chipset_in_or_transitioning_to_state(CHIPSET_STATE_ANY_OFF)) {
 		usb_mux_set(port, TYPEC_MUX_NONE, USB_SWITCH_DISCONNECT,
-			tc[port].polarity);
+			tc_get_polarity(port));
 		return;
 	}
 #endif /* CONFIG_POWER_COMMON */
@@ -152,20 +68,20 @@ void set_usb_mux_with_current_data_role(int port)
 	 */
 	if (!pd_is_connected(port))
 		usb_mux_set(port, TYPEC_MUX_NONE, USB_SWITCH_DISCONNECT,
-			tc[port].polarity);
+			tc_get_polarity(port));
 	/*
 	 * If new data role isn't DFP and we only support DFP, also disconnect.
 	 */
 	else if (IS_ENABLED(CONFIG_USBC_SS_MUX_DFP_ONLY) &&
-			tc[port].data_role != PD_ROLE_DFP)
+			tc_get_data_role(port) != PD_ROLE_DFP)
 		usb_mux_set(port, TYPEC_MUX_NONE, USB_SWITCH_DISCONNECT,
-			tc[port].polarity);
+			tc_get_polarity(port));
 	/*
 	 * Otherwise connect mux since we are in S3+
 	 */
 	else
 		usb_mux_set(port, TYPEC_MUX_USB, USB_SWITCH_CONNECT,
-			tc[port].polarity);
+			tc_get_polarity(port));
 #endif /* CONFIG_USBC_SS_MUX */
 }
 
@@ -228,7 +144,7 @@ void pd_task(void *u)
 {
 	int port = TASK_ID_TO_PD_PORT(task_get_current());
 
-	tc_state_init(port, TC_DEFAULT_STATE(port));
+	tc_state_init(port);
 
 	if (IS_ENABLED(CONFIG_USBC_PPC))
 		ppc_init(port);
@@ -246,30 +162,27 @@ void pd_task(void *u)
 
 	while (1) {
 		/* wait for next event/packet or timeout expiration */
-		tc[port].evt = task_wait_event(tc[port].evt_timeout);
+		const uint32_t evt = task_wait_event(tc_get_timeout(port));
 
 		/* handle events that affect the state machine as a whole */
-		tc_event_check(port, tc[port].evt);
+		tc_event_check(port, evt);
 
-#ifdef CONFIG_USB_PD_TCPC
 		/*
 		 * run port controller task to check CC and/or read incoming
 		 * messages
 		 */
-		tcpc_run(port, tc[port].evt);
-#endif
+		if (IS_ENABLED(CONFIG_USB_PD_TCPC))
+			tcpc_run(port, evt);
 
-#ifdef CONFIG_USB_PE_SM
-		/* run policy engine state machine */
-		usbc_policy_engine(port, tc[port].evt, tc[port].pd_enable);
-#endif
+		if (IS_ENABLED(CONFIG_USB_PE_SM))
+			/* Run policy engine state machine */
+			pe_run(port, evt, tc_get_pd_enabled(port));
 
-#ifdef CONFIG_USB_PRL_SM
-		/* run protocol state machine */
-		usbc_protocol_layer(port, tc[port].evt, tc[port].pd_enable);
-#endif
+		if (IS_ENABLED(CONFIG_USB_PRL_SM))
+			/* Run protocol state machine */
+			prl_run(port, evt, tc_get_pd_enabled(port));
 
-		/* run typec state machine */
-		sm_run_state_machine(port, TC_OBJ(port), SM_RUN_SIG);
+		/* Run TypeC state machine */
+		tc_run(port);
 	}
 }
diff --git a/include/config.h b/include/config.h
index 832f1a740a..3b23804552 100644
--- a/include/config.h
+++ b/include/config.h
@@ -870,12 +870,6 @@
 #undef CONFIG_USB_SM_FRAMEWORK
 
 /*
- * This is the maximum number of levels in the hierarchical
- * state machine framework. Set to 0 for a flat state machine.
- */
-#define CONFIG_SM_NESTING_NUM 3
-
-/*
  * Define to enable Type-C State Machine. Must be enabled
  * with CONFIG_USB_SM_FRAMEWORK
  */
diff --git a/include/usb_pe_sm.h b/include/usb_pe_sm.h
index c536389d36..1243484878 100644
--- a/include/usb_pe_sm.h
+++ b/include/usb_pe_sm.h
@@ -17,12 +17,6 @@ enum pe_error {
 	ERR_TCH_XMIT,
 };
 
-/*
- * PE_OBJ is a convenience macro to access struct sm_obj, which
- * must be the first member of struct policy_engine.
- */
-#define PE_OBJ(port)   (SM_OBJ(pe[port]))
-
 /**
  * Initialize the Policy Engine State Machine
  *
@@ -37,7 +31,7 @@ void pe_init(int port);
  * @param evt  system event, ie: PD_EVENT_RX
  * @param en   0 to disable the machine, 1 to enable the machine
  */
-void usbc_policy_engine(int port, int evt, int en);
+void pe_run(int port, int evt, int en);
 
 /**
  * Informs the Policy Engine that a message was successfully sent
diff --git a/include/usb_prl_sm.h b/include/usb_prl_sm.h
index 28f3a44078..4116bbe1a3 100644
--- a/include/usb_prl_sm.h
+++ b/include/usb_prl_sm.h
@@ -11,58 +11,6 @@
 #include "usb_pd.h"
 #include "usb_pd_tcpm.h"
 
-enum prl_tx_state_id {
-	PRL_TX_PHY_LAYER_RESET,
-	PRL_TX_WAIT_FOR_MESSAGE_REQUEST,
-	PRL_TX_LAYER_RESET_FOR_TRANSMIT,
-	PRL_TX_CONSTRUCT_MESSAGE,
-	PRL_TX_WAIT_FOR_PHY_RESPONSE,
-	PRL_TX_MATCH_MESSAGE_ID,
-	PRL_TX_MESSAGE_SENT,
-	PRL_TX_CHECK_RETRY_COUNTER,
-	PRL_TX_TRANSMISSION_ERROR,
-	PRL_TX_DISCARD_MESSAGE,
-
-	PRL_TX_SRC_SINK_TX,
-	PRL_TX_SRC_SOURCE_TX,
-	PRL_TX_SRC_PENDING,
-
-	PRL_TX_SNK_START_OF_AMS,
-	PRL_TX_SNK_PENDING,
-};
-
-enum prl_hr_state_id {
-	PRL_HR_WAIT_FOR_REQUEST,
-	PRL_HR_RESET_LAYER,
-	PRL_HR_INDICATE_HARD_RESET,
-	PRL_HR_WAIT_FOR_PHY_HARD_RESET_COMPLETE,
-	PRL_HR_PHY_HARD_RESET_REQUESTED,
-	PRL_HR_WAIT_FOR_PE_HARD_RESET_COMPLETE,
-	PRL_HR_PE_HARD_RESET_COMPLETE,
-};
-
-enum rch_state_id {
-	RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER,
-	RCH_PASS_UP_MESSAGE,
-	RCH_PROCESSING_EXTENDED_MESSAGE,
-	RCH_REQUESTING_CHUNK,
-	RCH_WAITING_CHUNK,
-	RCH_REPORT_ERROR,
-};
-
-enum tch_state_id {
-	TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE,
-	TCH_PASS_DOWN_MESSAGE,
-	TCH_WAIT_FOR_TRANSMISSION_COMPLETE,
-	TCH_MESSAGE_SENT,
-	TCH_PREPARE_TO_SEND_CHUNKED_MESSAGE,
-	TCH_CONSTRUCT_CHUNKED_MESSAGE,
-	TCH_SENDING_CHUNKED_MESSAGE,
-	TCH_WAIT_CHUNK_REQUEST,
-	TCH_MESSAGE_RECEIVED,
-	TCH_REPORT_ERROR,
-};
-
 /*
  * Number of times the Protocol Layer will try to transmit a message
  * before giving up and signaling an error
@@ -70,67 +18,13 @@ enum tch_state_id {
 #define N_RETRY_COUNT 2
 
 /**
- * Initialize the Protocol Layer State Machine
- *
- * @param port USB-C port number
- */
-void prl_init(int port);
-
-/**
- * Resets the Protocol Layer State Machine
- *
- * @param port USB-C port number
- */
-void prl_reset(int port);
-
-/**
- * Get Chunked Rx State Machine state id
- *
- * @param port USB-C port number
- * @return id
- */
-enum rch_state_id get_rch_state_id(int port);
-
-/**
- * Get Chunked Tx State Machine state id
- *
- * @param port USB-C port number
- * @return id
- */
-enum tch_state_id get_tch_state_id(int port);
-
-/**
- * Get Message Transmission State Machine state id
- *
- * @param port USB-C port number
- * @return id
- */
-enum prl_tx_state_id get_prl_tx_state_id(int port);
-
-/**
- * Get Hard Reset State Machine state id
- *
- * @param port USB-C port number
- * @return id
- */
-enum prl_hr_state_id get_prl_hr_state_id(int port);
-
-/**
- * Returns the state of the PRL state machine
- * @return SM_INIT for initializing
- *         SM_RUN for running
- *         SM_PAUSED for paused
- */
-enum sm_local_state prl_get_local_state(int port);
-
-/**
  * Runs the Protocol Layer State Machine
  *
  * @param port USB-C port number
  * @param evt  system event, ie: PD_EVENT_RX
  * @param en   0 to disable the machine, 1 to enable the machine
  */
-void usbc_protocol_layer(int port, int evt, int en);
+void prl_run(int port, int evt, int en);
 
 /**
  * Set the PD revision
diff --git a/include/usb_sm.h b/include/usb_sm.h
index e57cd0b873..f9f10b439f 100644
--- a/include/usb_sm.h
+++ b/include/usb_sm.h
@@ -8,142 +8,61 @@
 #ifndef __CROS_EC_USB_SM_H
 #define __CROS_EC_USB_SM_H
 
-#define DECLARE_SM_FUNC_(prefix, name, run, exit) \
-			DECLARE_SM_FUNC_##run(prefix, name); \
-			DECLARE_SM_FUNC_##exit(prefix, name)
-
-#define DECLARE_SM_FUNC_WITH_RUN(prefix, name)    static int \
-				prefix##_##name##_run(int port)
-
-#define DECLARE_SM_FUNC_WITH_EXIT(prefix, name)   static int \
-				prefix##_##name##_exit(int port)
-
-#define DECLARE_SM_FUNC_NOOP(prefix, name)
-
-#define DECLARE_SM_SIG_RUN(prefix, name, run)  DECLARE_SM_##run(prefix, name)
-#define DECLARE_SM_WITH_RUN(prefix, name)   prefix##_##name##_run
-
-#define DECLARE_SM_SIG_EXIT(prefix, name, exit)  DECLARE_SM_##exit(prefix, name)
-#define DECLARE_SM_WITH_EXIT(prefix, name)   prefix##_##name##_exit
-
-#define DECLARE_SM_NOOP(prefix, name)  sm_do_nothing
+/* Function pointer that implements a portion of a usb state */
+typedef void (*state_execution)(const int port);
 
 /*
- * Helper macro for the declaration of states.
- *
- * @param prefix - prefix of state function name
- * @param name   - name of state
- * @param run    - if WITH_RUN, generates a run state function name
- *                 if NOOP, generates a do nothing function name
- * @param exit   - if WITH_EXIT, generates an exit state function name
- *                 if NOOP, generates a do nothing function name
- *
- * EXAMPLE:
- *
- * DECLARE_STATE(tc, test, WITH_RUN, WITH_EXIT); generates the following:
+ * General usb state that can be used in multiple state machines.
  *
- * static unsigned int tc_test(int port, enum sm_signal sig);
- * static unsigned int tc_test_entry(int port);
- * static unsigned int tc_test_run(int port);
- * static unsigned int tc_test_exit(int port);
- * static const state_sig tc_test_sig[] = {
- *	tc_test_entry,
- *	tc_test_run,
- *	tc_test_exit,
- *	sm_get_super_state };
- *
- * DECLARE_STATE(tc, test, NOOP, NOOP); generates the following:
- *
- * static unsigned int tc_test(int port, enum sm_signal sig);
- * static unsigned int tc_test_entry(int port);
- * static const state_sig tc_test_sig[] = {
- *      tc_test_entry,
- *      sm_do_nothing,
- *      sm_do_nothing,
- *      sm_get_super_state };
+ * entry - Optional method that will be run when this state is entered
+ * run   - Optional method that will be run repeatedly during state machine loop
+ * exit  - Optional method that will be run when this state exists
+ * parent- Optional parent usb_state that contains common entry/run/exit
+ *	implementation between various usb state. All parent entry/run
+ *	functions will before any child entry/run functions. All parent exit
+ *	functions will run after any child exit functions.
  */
-#define DECLARE_STATE(prefix, name, run, exit) \
-static int prefix##_##name(int port, enum sm_signal sig); \
-static int prefix##_##name##_entry(int port); \
-DECLARE_SM_FUNC_(prefix, name, run, exit); \
-static const state_sig prefix##_##name##_sig[] = { \
-prefix##_##name##_entry, \
-DECLARE_SM_SIG_RUN(prefix, name, run), \
-DECLARE_SM_SIG_EXIT(prefix, name, exit), \
-sm_get_super_state \
-}
+struct usb_state {
+	const state_execution entry;
+	const state_execution run;
+	const state_execution exit;
+	const struct usb_state *parent;
+};
+
+typedef const struct usb_state *usb_state_ptr;
 
-#define SM_OBJ(smo)    ((struct sm_obj *)&smo)
-#define SM_SUPER(r, sig, s)  ((((r) == 0) || ((sig) == SM_ENTRY_SIG) || \
-			((sig) == SM_EXIT_SIG)) ? 0 : ((uintptr_t)(s)))
-#define SM_RUN_SUPER	1
+/* Defines the current context of the usb statemachine. */
+struct sm_ctx {
+	usb_state_ptr current;
+	usb_state_ptr previous;
+	/* We use intptr_t type to accommodate host tests ptr size variance */
+	intptr_t internal[2];
+};
 
 /* Local state machine states */
 enum sm_local_state {
-	SM_INIT,
+	SM_INIT = 0, /* Ensure static variables initialize to SM_INIT */
 	SM_RUN,
-	SM_PAUSED
-};
-
-/* State Machine signals */
-enum sm_signal {
-	SM_ENTRY_SIG = 0,
-	SM_RUN_SIG,
-	SM_EXIT_SIG,
-	SM_SUPER_SIG,
+	SM_PAUSED,
 };
 
-typedef int (*state_sig)(int port);
-typedef int (*sm_state)(int port, enum sm_signal sig);
-
-struct sm_obj {
-	sm_state task_state;
-	sm_state last_state;
-};
-
-/**
- * Initialize a State Machine
- *
- * @param port   USB-C port number
- * @param obj    State machine object
- * @param target Initial state of state machine
- */
-void sm_init_state(int port, struct sm_obj *obj, sm_state target);
-
-/**
- * Changes a state machines state
- *
- * @param port   USB-C port number
- * @param obj    State machine object
- * @param target State to transition to
- * @return 0
- */
-int sm_set_state(int port, struct sm_obj *obj, sm_state target);
-
-/**
- * Runs a state machine
- *
- * @param port USB-C port number
- * @param obj  State machine object
- * @param sig  State machine signal
- */
-void sm_run_state_machine(int port, struct sm_obj *obj, enum sm_signal sig);
-
 /**
- * Substitute this function for states that do not implement a
- * run or exit state.
+ * Changes a state machines state. This handles exiting the previous state and
+ * entering the target state. A common parent state will not exited nor be
+ * re-entered.
  *
- * @param port USB-C port number
- * @return 0
+ * @param port      USB-C port number
+ * @param ctx       State machine context
+ * @param new_state State to transition to (NULL is valid and exits all states)
  */
-int sm_do_nothing(int port);
+void set_state(int port, struct sm_ctx *ctx, usb_state_ptr new_state);
 
 /**
- * Called by the state machine framework to execute a states super state.
+ * Runs one iteration of a state machine (including any parent states)
  *
  * @param port USB-C port number
- * @return RUN_SUPER
+ * @param ctx  State machine context
  */
-int sm_get_super_state(int port);
+void exe_state(int port, struct sm_ctx *ctx);
 
 #endif /* __CROS_EC_USB_SM_H */
diff --git a/include/usb_tc_ctvpd_sm.h b/include/usb_tc_ctvpd_sm.h
deleted file mode 100644
index ab964761f9..0000000000
--- a/include/usb_tc_ctvpd_sm.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/* Copyright 2019 The Chromium OS Authors. All rights reserved.
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE file.
- */
-
-#ifndef __CROS_EC_USB_TC_CTVPD_SM_H
-#define __CROS_EC_USB_TC_CTVPD_SM_H
-
-#include "usb_sm.h"
-#include "usb_tc_sm.h"
-
-/* Port default state at startup */
-#define TC_DEFAULT_STATE(port) TC_UNATTACHED_SNK
-
-/**
- * This is the Type-C Port object that contains information needed to
- * implement a Charge Through VCONN Powered Device.
- */
-struct type_c {
-	/* struct sm_obj must be first */
-	struct sm_obj obj;
-	/* state id */
-	enum typec_state_id state_id;
-	/* current port power role (VPD, SOURCE or SINK) */
-	uint8_t power_role;
-	/* current port data role (DFP or UFP) */
-	uint8_t data_role;
-	/* enable power delivery state machines */
-	uint8_t pd_enable;
-	/* event timeout */
-	uint64_t evt_timeout;
-	/* state machine event */
-	int evt;
-	/* port flags, see TC_FLAGS_* */
-	uint32_t flags;
-	/*
-	 * Time a charge-through port shall wait before it can determine it
-	 * is attached
-	 */
-	uint64_t cc_debounce;
-	/* Time a host port shall wait before it can determine it is attached */
-	uint64_t host_cc_debounce;
-	/* Time a Sink port shall wait before it can determine it is detached
-	 * due to the potential for USB PD signaling on CC as described in
-	 * the state definitions.
-	 */
-	uint64_t pd_debounce;
-	/* Maintains state of billboard device */
-	int billboard_presented;
-	/*
-	 * Time a port shall wait before it can determine it is
-	 * re-attached during the try-wait process.
-	 */
-	uint64_t try_wait_debounce;
-	/* charge-through support timer */
-	uint64_t support_timer;
-	/* reset the charge-through support timer */
-	uint8_t support_timer_reset;
-	/* VPD host port cc state */
-	enum pd_cc_states host_cc_state;
-	uint8_t ct_cc;
-	/* The cc state */
-	enum pd_cc_states cc_state;
-	uint64_t next_role_swap;
-};
-
-extern struct type_c tc[];
-
-#endif /* __CROS_EC_USB_TC_CTVPD_SM_H */
diff --git a/include/usb_tc_drp_acc_trysrc_sm.h b/include/usb_tc_drp_acc_trysrc_sm.h
deleted file mode 100644
index 03327c0e3a..0000000000
--- a/include/usb_tc_drp_acc_trysrc_sm.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/* Copyright 2019 The Chromium OS Authors. All rights reserved.
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE file.
- */
-
-#ifndef __CROS_EC_USB_TC_DRP_ACC_TRYSRC_H
-#define __CROS_EC_USB_TC_DRP_ACC_TRYSRC_H
-
-#include "usb_sm.h"
-#include "usb_tc_sm.h"
-
-/* Port default state at startup */
-#define TC_DEFAULT_STATE(port) TC_UNATTACHED_SNK
-
-/**
- * This is the Type-C Port object that contains information needed to
- * implement a USB Type-C DRP with Accessory and Try.SRC module
- *   See Figure 4-16 in Release 1.4 of USB Type-C Spec.
- */
-struct type_c {
-	/*
-	 * struct sm_obj must be first. This is the state machine
-	 * object that keeps track of the current and last state
-	 * of the state machine.
-	 */
-	struct sm_obj obj;
-	/* state id */
-	enum typec_state_id state_id;
-	/* current port power role (SOURCE or SINK) */
-	uint8_t power_role;
-	/* current port data role (DFP or UFP) */
-	uint8_t data_role;
-	/* Port polarity : 0 => CC1 is CC line, 1 => CC2 is CC line */
-	uint8_t polarity;
-	/* port flags, see TC_FLAGS_* */
-	uint32_t flags;
-	/* event timeout */
-	uint64_t evt_timeout;
-	/* state machine event */
-	int evt;
-	/* Time a port shall wait before it can determine it is attached */
-	uint64_t cc_debounce;
-	/*
-	 * Time a Sink port shall wait before it can determine it is detached
-	 * due to the potential for USB PD signaling on CC as described in
-	 * the state definitions.
-	 */
-	uint64_t pd_debounce;
-#ifdef CONFIG_USB_PD_TRY_SRC
-	/*
-	 * Time a port shall wait before it can determine it is
-	 * re-attached during the try-wait process.
-	 */
-	uint64_t try_wait_debounce;
-#endif
-	/* Voltage on CC pin */
-	enum tcpc_cc_voltage_status cc_voltage;
-	/* The cc state */
-	enum pd_cc_states cc_state;
-	/* Role toggle timer */
-	uint64_t next_role_swap;
-	/* Generic timer */
-	uint64_t timeout;
-
-#ifdef CONFIG_USB_PD_TCPC_LOW_POWER
-	/* Time to enter low power mode */
-	uint64_t low_power_time;
-	/* Tasks to notify after TCPC has been reset */
-	int tasks_waiting_on_reset;
-	/* Tasks preventing TCPC from entering low power mode */
-	int tasks_preventing_lpm;
-#endif
-	/* Type-C current */
-	typec_current_t typec_curr;
-	/* Type-C current change */
-	typec_current_t typec_curr_change;
-	/* Attached ChromeOS device id, RW hash, and current RO / RW image */
-	uint16_t dev_id;
-	uint32_t dev_rw_hash[PD_RW_HASH_SIZE/4];
-	enum ec_current_image current_image;
-};
-
-extern struct type_c tc[];
-
-#endif /* __CROS_EC_USB_TC_DRP_ACC_TRYSRC_H */
diff --git a/include/usb_tc_sm.h b/include/usb_tc_sm.h
index 39ba6e026d..c46f449e44 100644
--- a/include/usb_tc_sm.h
+++ b/include/usb_tc_sm.h
@@ -11,56 +11,11 @@
 #include "usb_sm.h"
 #include "usb_pd_tcpm.h"
 
-enum typec_state_id {
-	TC_DISABLED,
-	TC_UNATTACHED_SNK,
-	TC_ATTACH_WAIT_SNK,
-	TC_ATTACHED_SNK,
-#if !defined(CONFIG_USB_TYPEC_VPD)
-	TC_ERROR_RECOVERY,
-	TC_UNATTACHED_SRC,
-	TC_ATTACH_WAIT_SRC,
-	TC_ATTACHED_SRC,
-#endif
-#if !defined(CONFIG_USB_TYPEC_CTVPD) && !defined(CONFIG_USB_TYPEC_VPD)
-	TC_AUDIO_ACCESSORY,
-	TC_ORIENTED_DEBUG_ACCESSORY_SRC,
-	TC_UNORIENTED_DEBUG_ACCESSORY_SRC,
-	TC_DEBUG_ACCESSORY_SNK,
-	TC_TRY_SRC,
-	TC_TRY_WAIT_SNK,
-	TC_CTUNATTACHED_SNK,
-	TC_CTATTACHED_SNK,
-#endif
-#if defined(CONFIG_USB_TYPEC_CTVPD)
-	TC_CTTRY_SNK,
-	TC_CTATTACHED_UNSUPPORTED,
-	TC_CTATTACH_WAIT_UNSUPPORTED,
-	TC_CTUNATTACHED_UNSUPPORTED,
-	TC_CTUNATTACHED_VPD,
-	TC_CTATTACH_WAIT_VPD,
-	TC_CTATTACHED_VPD,
-	TC_CTDISABLED_VPD,
-	TC_TRY_SNK,
-	TC_TRY_WAIT_SRC,
-#endif
-	/* Number of states. Not an actual state. */
-	TC_STATE_COUNT,
-};
-
-extern const char * const tc_state_names[];
-
 #define TC_SET_FLAG(port, flag) atomic_or(&tc[port].flags, (flag))
 #define TC_CLR_FLAG(port, flag) atomic_clear(&tc[port].flags, (flag))
 #define TC_CHK_FLAG(port, flag) (tc[port].flags & (flag))
 
 /*
- * TC_OBJ is a convenience macro to access struct sm_obj, which
- * must be the first member of struct type_c.
- */
-#define TC_OBJ(port)   (SM_OBJ(tc[port]))
-
-/*
  * Type C supply voltage (mV)
  *
  * This is the maximum voltage a sink can request
@@ -77,13 +32,6 @@ extern const char * const tc_state_names[];
 #define TYPE_C_AUDIO_ACC_CURRENT  500 /* mA */
 
 /**
- * Get the id of the current Type-C state
- *
- * @param port USB-C port number
- */
-enum typec_state_id get_typec_state_id(int port);
-
-/**
  * Get current data role
  *
  * @param port USB-C port number
@@ -100,6 +48,23 @@ int tc_get_data_role(int port);
 int tc_get_power_role(int port);
 
 /**
+ * Get current polarity
+ *
+ * @param port USB-C port number
+ * @return 0 for CC1 as primary, 1 for CC2 as primary
+ */
+uint8_t tc_get_polarity(int port);
+
+/**
+ * Get Power Deliever communication state. If disabled, both protocol and policy
+ * engine are disabled and should not run.
+ *
+ * @param port USB-C port number
+ * @return 0 if pd is disabled, 1 is pd is enabled
+ */
+uint8_t tc_get_pd_enabled(int port);
+
+/**
  * Set the power role
  *
  * @param port USB-C port number
@@ -118,6 +83,14 @@ void tc_set_power_role(int port, int role);
 void set_usb_mux_with_current_data_role(int port);
 
 /**
+ * Get loop timeout value
+ *
+ * @param port USB-C port number
+ * @return time in ms
+ */
+uint64_t tc_get_timeout(int port);
+
+/**
  * Set loop timeout value
  *
  * @param port USB-C port number
@@ -146,12 +119,8 @@ void set_polarity(int port, int polarity);
  * TypeC state machine
  *
  * @param port USB-C port number
- * @param sm_state initial state of the state machine. Must be
- *			UNATTACHED_SNK or UNATTACHED_SRC, any other
- *			state id value will be interpreted as
- *			UNATTACHED_SNK.
  */
-void tc_state_init(int port, enum typec_state_id start_state);
+void tc_state_init(int port);
 
 /**
  * Called by the state machine framework to handle events
@@ -162,6 +131,13 @@ void tc_state_init(int port, enum typec_state_id start_state);
  */
 void tc_event_check(int port, int evt);
 
+/**
+ * Runs the TypeC layer statemachine
+ *
+ * @param port USB-C port number
+ */
+void tc_run(const int port);
+
 #ifdef CONFIG_USB_TYPEC_CTVPD
 /**
  * Resets the charge-through support timer. This can be
diff --git a/include/usb_tc_vpd_sm.h b/include/usb_tc_vpd_sm.h
deleted file mode 100644
index 8beaebe5ba..0000000000
--- a/include/usb_tc_vpd_sm.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/* Copyright 2019 The Chromium OS Authors. All rights reserved.
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE file.
- */
-
-#ifndef __CROS_EC_USB_TC_VPD_SM_H
-#define __CROS_EC_USB_TC_VPD_SM_H
-
-#include "usb_sm.h"
-#include "usb_tc_sm.h"
-
-/* Port default state at startup */
-#define TC_DEFAULT_STATE(port) TC_UNATTACHED_SNK
-
-/**
- * This is the Type-C Port object that contains information needed to
- * implement a VCONN Powered Device.
- */
-struct type_c {
-	/*
-	 * struct sm_obj must be first. This is the state machine
-	 * object that keeps track of the current and last state
-	 * of the state machine.
-	 */
-	struct sm_obj obj;
-	/* state id */
-	enum typec_state_id state_id;
-	/* current port power role (VPD, SOURCE or SINK) */
-	uint8_t power_role;
-	/* current port data role (DFP or UFP) */
-	uint8_t data_role;
-	/* bool: enable power delivery state machines */
-	uint8_t pd_enable;
-	/* event timeout */
-	uint64_t evt_timeout;
-	/* state machine event */
-	int evt;
-	/* port flags, see TC_FLAGS_* */
-	uint32_t flags;
-	/* Time a port shall wait before it can determine it is attached */
-	uint64_t cc_debounce;
-	/* VPD host port cc state */
-	enum pd_cc_states host_cc_state;
-	uint8_t ct_cc;
-};
-
-extern struct type_c tc[];
-
-#endif /* __CROS_EC_USB_TC_VPD_SM_H */
diff --git a/test/test_config.h b/test/test_config.h
index 942c8c8501..1ca557bbef 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -236,8 +236,6 @@ int ncp15wb_calculate_temp(uint16_t adc);
 #undef CONFIG_USB_PRL_SM
 #undef CONFIG_USB_PE_SM
 #undef CONFIG_USB_TYPEC_SM
-#undef  CONFIG_SM_NESTING_NUM
-#define CONFIG_SM_NESTING_NUM 3
 #define CONFIG_USB_SM_FRAMEWORK
 #endif
 
@@ -246,8 +244,6 @@ int ncp15wb_calculate_temp(uint16_t adc);
 #undef CONFIG_USB_PRL_SM
 #undef CONFIG_USB_PE_SM
 #undef CONFIG_USB_TYPEC_SM
-#undef  CONFIG_SM_NESTING_NUM
-#define CONFIG_SM_NESTING_NUM 2
 #define CONFIG_USB_SM_FRAMEWORK
 #endif
 
@@ -256,8 +252,6 @@ int ncp15wb_calculate_temp(uint16_t adc);
 #undef CONFIG_USB_PRL_SM
 #undef CONFIG_USB_PE_SM
 #undef CONFIG_USB_TYPEC_SM
-#undef  CONFIG_SM_NESTING_NUM
-#define CONFIG_SM_NESTING_NUM 1
 #define CONFIG_USB_SM_FRAMEWORK
 #endif
 
@@ -266,14 +260,10 @@ int ncp15wb_calculate_temp(uint16_t adc);
 #undef CONFIG_USB_PRL_SM
 #undef CONFIG_USB_PE_SM
 #undef CONFIG_USB_TYPEC_SM
-#undef  CONFIG_SM_NESTING_NUM
-#define CONFIG_SM_NESTING_NUM 0
 #define CONFIG_USB_SM_FRAMEWORK
 #endif
 
 #if defined(TEST_USB_PRL)
-#undef  CONFIG_SM_NESTING_NUM
-#define CONFIG_SM_NESTING_NUM 3
 #define CONFIG_USB_PD_PORT_COUNT 2
 #define CONFIG_USB_SM_FRAMEWORK
 #undef CONFIG_USB_PE_SM
@@ -292,8 +282,6 @@ int ncp15wb_calculate_temp(uint16_t adc);
 #else
 #define CONFIG_USB_TYPEC_CTVPD
 #endif
-#undef  CONFIG_SM_NESTING_NUM
-#define CONFIG_SM_NESTING_NUM 3
 
 #define CONFIG_USB_PID 0x5036
 #define VPD_HW_VERSION 0x0001
diff --git a/test/usb_prl.c b/test/usb_prl.c
index 36f007b057..0851e0cfc2 100644
--- a/test/usb_prl.c
+++ b/test/usb_prl.c
@@ -20,6 +20,54 @@
 #define PORT0 0
 #define PORT1 1
 
+/*
+ * These enum definitions are declared in usb_prl_sm and are private to that
+ * file. If those definitions are re-ordered, then we need to update these
+ * definitions (should be very rare).
+ */
+enum usb_prl_tx_state {
+	PRL_TX_PHY_LAYER_RESET,
+	PRL_TX_WAIT_FOR_MESSAGE_REQUEST,
+	PRL_TX_LAYER_RESET_FOR_TRANSMIT,
+	PRL_TX_WAIT_FOR_PHY_RESPONSE,
+	PRL_TX_SRC_SOURCE_TX,
+	PRL_TX_SNK_START_AMS,
+	PRL_TX_SRC_PENDING,
+	PRL_TX_SNK_PENDING,
+	PRL_TX_DISCARD_MESSAGE,
+};
+
+enum usb_prl_hr_state {
+	PRL_HR_WAIT_FOR_REQUEST,
+	PRL_HR_RESET_LAYER,
+	PRL_HR_WAIT_FOR_PHY_HARD_RESET_COMPLETE,
+	PRL_HR_WAIT_FOR_PE_HARD_RESET_COMPLETE,
+};
+
+enum usb_rch_state {
+	RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER,
+	RCH_PROCESSING_EXTENDED_MESSAGE,
+	RCH_REQUESTING_CHUNK,
+	RCH_WAITING_CHUNK,
+	RCH_REPORT_ERROR,
+};
+
+enum usb_tch_state {
+	TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE,
+	TCH_WAIT_FOR_TRANSMISSION_COMPLETE,
+	TCH_CONSTRUCT_CHUNKED_MESSAGE,
+	TCH_SENDING_CHUNKED_MESSAGE,
+	TCH_WAIT_CHUNK_REQUEST,
+	TCH_MESSAGE_RECEIVED,
+};
+
+/* Defined in implementation */
+enum usb_prl_tx_state prl_tx_get_state(const int port);
+enum usb_prl_hr_state prl_hr_get_state(const int port);
+enum usb_rch_state rch_get_state(const int port);
+enum usb_tch_state tch_get_state(const int port);
+
+
 static uint32_t test_data[] = {
 	0x00010203, 0x04050607, 0x08090a0b, 0x0c0d0e0f,
 	0x10111213, 0x14151617, 0x1819a0b0, 0xc0d0e0f0,
@@ -331,7 +379,7 @@ static int simulate_receive_extended_data(int port,
 		 * Wait for request chunk message
 		 */
 		req_timeout = 0;
-		while (get_rch_state_id(port) != RCH_REQUESTING_CHUNK &&
+		while (rch_get_state(port) != RCH_REQUESTING_CHUNK &&
 							req_timeout < 5) {
 			req_timeout++;
 			msleep(2);
@@ -675,13 +723,13 @@ static int test_initial_states(void)
 
 	enable_prl(port, 1);
 
-	TEST_ASSERT(get_prl_tx_state_id(port) ==
+	TEST_ASSERT(prl_tx_get_state(port) ==
 				PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
-	TEST_ASSERT(get_rch_state_id(port) ==
+	TEST_ASSERT(rch_get_state(port) ==
 				RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER);
-	TEST_ASSERT(get_tch_state_id(port) ==
+	TEST_ASSERT(tch_get_state(port) ==
 				TCH_WAIT_FOR_MESSAGE_REQUEST_FROM_PE);
-	TEST_ASSERT(get_prl_hr_state_id(port) ==
+	TEST_ASSERT(prl_hr_get_state(port) ==
 				PRL_HR_WAIT_FOR_REQUEST);
 
 	return EC_SUCCESS;
@@ -701,7 +749,7 @@ static int test_send_ctrl_msg(void)
 		task_wake(PD_PORT_TO_TASK_ID(port));
 		task_wait_event(40 * MSEC);
 
-		TEST_ASSERT(get_prl_tx_state_id(port) ==
+		TEST_ASSERT(prl_tx_get_state(port) ==
 					PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 
 		TEST_ASSERT(simulate_send_ctrl_msg_request_from_pe(port,
@@ -739,7 +787,7 @@ static int test_send_ctrl_msg_with_retry_and_fail(void)
 	task_wake(PD_PORT_TO_TASK_ID(port));
 	task_wait_event(40 * MSEC);
 
-	TEST_ASSERT(get_prl_tx_state_id(port) ==
+	TEST_ASSERT(prl_tx_get_state(port) ==
 			PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 
 	TEST_ASSERT(simulate_send_ctrl_msg_request_from_pe(port,
@@ -761,7 +809,7 @@ static int test_send_ctrl_msg_with_retry_and_fail(void)
 	task_wake(PD_PORT_TO_TASK_ID(port));
 	task_wait_event(40 * MSEC);
 
-	TEST_ASSERT(get_prl_tx_state_id(port) ==
+	TEST_ASSERT(prl_tx_get_state(port) ==
 					PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 
 	pd_port[port].mock_pe_message_sent = 0;
@@ -801,7 +849,7 @@ static int test_send_ctrl_msg_with_retry_and_success(void)
 	task_wake(PD_PORT_TO_TASK_ID(port));
 	task_wait_event(40 * MSEC);
 
-	TEST_ASSERT(get_prl_tx_state_id(port) ==
+	TEST_ASSERT(prl_tx_get_state(port) ==
 				PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 
 	pd_port[port].mock_got_soft_reset = 0;
@@ -827,7 +875,7 @@ static int test_send_ctrl_msg_with_retry_and_success(void)
 	task_wake(PD_PORT_TO_TASK_ID(port));
 	task_wait_event(40 * MSEC);
 
-	TEST_ASSERT(get_prl_tx_state_id(port) ==
+	TEST_ASSERT(prl_tx_get_state(port) ==
 				PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 
 	pd_port[port].mock_pe_message_sent = 0;
@@ -876,7 +924,7 @@ static int test_send_data_msg(void)
 		task_wake(PD_PORT_TO_TASK_ID(port));
 		task_wait_event(40 * MSEC);
 
-		TEST_ASSERT(get_prl_tx_state_id(port) ==
+		TEST_ASSERT(prl_tx_get_state(port) ==
 					PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 
 		TEST_ASSERT(simulate_send_data_msg_request_from_pe(port,
@@ -913,7 +961,7 @@ static int test_send_data_msg_to_much_data(void)
 	task_wake(PD_PORT_TO_TASK_ID(port));
 	task_wait_event(40 * MSEC);
 
-	TEST_ASSERT(get_prl_tx_state_id(port) ==
+	TEST_ASSERT(prl_tx_get_state(port) ==
 				PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 
 	/* Try to send 29-bytes */
@@ -954,7 +1002,7 @@ static int test_send_extended_data_msg(void)
 		task_wake(PD_PORT_TO_TASK_ID(port));
 		task_wait_event(40 * MSEC);
 
-		TEST_ASSERT(get_prl_tx_state_id(port) ==
+		TEST_ASSERT(prl_tx_get_state(port) ==
 					PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 
 		TEST_ASSERT(simulate_send_extended_data_msg(
@@ -992,7 +1040,7 @@ static int test_receive_soft_reset_msg(void)
 	task_wake(PD_PORT_TO_TASK_ID(port));
 	task_wait_event(40 * MSEC);
 
-	TEST_ASSERT(get_rch_state_id(port) ==
+	TEST_ASSERT(rch_get_state(port) ==
 			RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER);
 
 	pd_port[port].mock_got_soft_reset = 0;
@@ -1035,7 +1083,7 @@ static int test_receive_control_msg(void)
 	task_wake(PD_PORT_TO_TASK_ID(port));
 	task_wait_event(40 * MSEC);
 
-	TEST_ASSERT(get_rch_state_id(port) ==
+	TEST_ASSERT(rch_get_state(port) ==
 			RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER);
 
 	pd_port[port].mock_got_soft_reset = 0;
@@ -1075,7 +1123,7 @@ static int test_receive_data_msg(void)
 		task_wake(PD_PORT_TO_TASK_ID(port));
 		task_wait_event(40 * MSEC);
 
-		TEST_ASSERT(get_rch_state_id(port) ==
+		TEST_ASSERT(rch_get_state(port) ==
 				RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER);
 		TEST_ASSERT(simulate_receive_data(port,
 						PD_DATA_BATTERY_STATUS, i));
@@ -1100,7 +1148,7 @@ static int test_receive_extended_data_msg(void)
 	task_wake(PD_PORT_TO_TASK_ID(port));
 	task_wait_event(40 * MSEC);
 
-	TEST_ASSERT(get_rch_state_id(port) ==
+	TEST_ASSERT(rch_get_state(port) ==
 			RCH_WAIT_FOR_MESSAGE_FROM_PROTOCOL_LAYER);
 
 	for (len = 29; len <= 260; len++)
@@ -1125,7 +1173,7 @@ static int test_send_soft_reset_msg(void)
 	task_wake(PD_PORT_TO_TASK_ID(port));
 	task_wait_event(40 * MSEC);
 
-	TEST_ASSERT(get_prl_tx_state_id(port) ==
+	TEST_ASSERT(prl_tx_get_state(port) ==
 				PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 
 	TEST_ASSERT(simulate_send_ctrl_msg_request_from_pe(port,
@@ -1138,7 +1186,7 @@ static int test_send_soft_reset_msg(void)
 						pd_port[port].msg_tx_id);
 	inc_tx_id(port);
 
-	TEST_ASSERT(get_prl_tx_state_id(port) ==
+	TEST_ASSERT(prl_tx_get_state(port) ==
 					PRL_TX_LAYER_RESET_FOR_TRANSMIT);
 
 	cycle_through_state_machine(port, 3, 10 * MSEC);
@@ -1167,24 +1215,24 @@ static int test_pe_execute_hard_reset_msg(void)
 	task_wake(PD_PORT_TO_TASK_ID(port));
 	task_wait_event(40 * MSEC);
 
-	TEST_ASSERT(get_prl_hr_state_id(port) == PRL_HR_WAIT_FOR_REQUEST);
+	TEST_ASSERT(prl_hr_get_state(port) == PRL_HR_WAIT_FOR_REQUEST);
 
 	/* Simulate receiving hard reset from policy engine */
 	prl_execute_hard_reset(port);
 
-	TEST_ASSERT(get_prl_hr_state_id(port) == PRL_HR_RESET_LAYER);
-	TEST_ASSERT(get_prl_tx_state_id(port) ==
+	TEST_ASSERT(prl_hr_get_state(port) == PRL_HR_RESET_LAYER);
+	TEST_ASSERT(prl_tx_get_state(port) ==
 					PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 
 	cycle_through_state_machine(port, 1, 10 * MSEC);
 
-	TEST_ASSERT(get_prl_hr_state_id(port) ==
+	TEST_ASSERT(prl_hr_get_state(port) ==
 				PRL_HR_WAIT_FOR_PHY_HARD_RESET_COMPLETE);
 
 	cycle_through_state_machine(port, 2, PD_T_PS_HARD_RESET);
 	TEST_ASSERT(pd_port[port].mock_pe_hard_reset_sent);
 
-	TEST_ASSERT(get_prl_hr_state_id(port) ==
+	TEST_ASSERT(prl_hr_get_state(port) ==
 				PRL_HR_WAIT_FOR_PE_HARD_RESET_COMPLETE);
 
 	/* Simulate policy engine indicating that it is done hard reset */
@@ -1192,7 +1240,7 @@ static int test_pe_execute_hard_reset_msg(void)
 
 	cycle_through_state_machine(port, 1, 10 * MSEC);
 
-	TEST_ASSERT(get_prl_hr_state_id(port) == PRL_HR_WAIT_FOR_REQUEST);
+	TEST_ASSERT(prl_hr_get_state(port) == PRL_HR_WAIT_FOR_REQUEST);
 
 	enable_prl(port, 0);
 
@@ -1214,24 +1262,24 @@ static int test_phy_execute_hard_reset_msg(void)
 	task_wake(PD_PORT_TO_TASK_ID(port));
 	task_wait_event(40 * MSEC);
 
-	TEST_ASSERT(get_prl_hr_state_id(port) == PRL_HR_WAIT_FOR_REQUEST);
+	TEST_ASSERT(prl_hr_get_state(port) == PRL_HR_WAIT_FOR_REQUEST);
 
 	/* Simulate receiving hard reset from port partner */
 	pd_execute_hard_reset(port);
 
-	TEST_ASSERT(get_prl_hr_state_id(port) == PRL_HR_RESET_LAYER);
-	TEST_ASSERT(get_prl_tx_state_id(port) ==
+	TEST_ASSERT(prl_hr_get_state(port) == PRL_HR_RESET_LAYER);
+	TEST_ASSERT(prl_tx_get_state(port) ==
 					PRL_TX_WAIT_FOR_MESSAGE_REQUEST);
 
 	cycle_through_state_machine(port, 1, 10 * MSEC);
 
-	TEST_ASSERT(get_prl_hr_state_id(port) ==
+	TEST_ASSERT(prl_hr_get_state(port) ==
 				PRL_HR_WAIT_FOR_PE_HARD_RESET_COMPLETE);
 
 	cycle_through_state_machine(port, 2, PD_T_PS_HARD_RESET);
 	TEST_ASSERT(pd_port[port].mock_pe_got_hard_reset);
 
-	TEST_ASSERT(get_prl_hr_state_id(port) ==
+	TEST_ASSERT(prl_hr_get_state(port) ==
 				PRL_HR_WAIT_FOR_PE_HARD_RESET_COMPLETE);
 
 	/* Simulate policy engine indicating that it is done hard reset */
@@ -1239,7 +1287,7 @@ static int test_phy_execute_hard_reset_msg(void)
 
 	cycle_through_state_machine(port, 1, 10 * MSEC);
 
-	TEST_ASSERT(get_prl_hr_state_id(port) == PRL_HR_WAIT_FOR_REQUEST);
+	TEST_ASSERT(prl_hr_get_state(port) == PRL_HR_WAIT_FOR_REQUEST);
 
 	enable_prl(port, 0);
 
@@ -1255,7 +1303,7 @@ int pd_task(void *u)
 		evt = task_wait_event(-1);
 
 		tcpc_run(port, evt);
-		usbc_protocol_layer(port, evt, pd_port[port].pd_enable);
+		prl_run(port, evt, pd_port[port].pd_enable);
 	}
 
 	return EC_SUCCESS;
diff --git a/test/usb_sm_framework_h3.c b/test/usb_sm_framework_h3.c
index 8976e0b973..f6ecc721ce 100644
--- a/test/usb_sm_framework_h3.c
+++ b/test/usb_sm_framework_h3.c
@@ -113,41 +113,37 @@ enum state_id {
 };
 
 #define PORT0   0
-#define TSM_OBJ(port)   (SM_OBJ(sm[port]))
 
 struct sm_ {
 	/* struct sm_obj must be first */
-	struct sm_obj obj;
+	struct sm_ctx ctx;
 	int sv_tmp;
 	int idx;
-	int seq[55];
+	int seq[SEQUENCE_SIZE];
 } sm[1];
 
-#if defined(TEST_USB_SM_FRAMEWORK_H3)
-DECLARE_STATE(sm, test_super_A1, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(sm, test_super_B1, WITH_RUN, WITH_EXIT);
-#endif
-
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2)
-DECLARE_STATE(sm, test_super_A2, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(sm, test_super_B2, WITH_RUN, WITH_EXIT);
-#endif
-
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2) || \
-	defined(TEST_USB_SM_FRAMEWORK_H1)
-DECLARE_STATE(sm, test_super_A3, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(sm, test_super_B3, WITH_RUN, WITH_EXIT);
-#endif
-
-DECLARE_STATE(sm, test_A4, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(sm, test_A5, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(sm, test_A6, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(sm, test_A7, WITH_RUN, WITH_EXIT);
+enum state {
+	SM_TEST_SUPER_A1,
+	SM_TEST_SUPER_A2,
+	SM_TEST_SUPER_A3,
+	SM_TEST_SUPER_B1,
+	SM_TEST_SUPER_B2,
+	SM_TEST_SUPER_B3,
+	SM_TEST_A4,
+	SM_TEST_A5,
+	SM_TEST_A6,
+	SM_TEST_A7,
+	SM_TEST_B4,
+	SM_TEST_B5,
+	SM_TEST_B6,
+	SM_TEST_C,
+};
+static const struct usb_state states[];
 
-DECLARE_STATE(sm, test_B4, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(sm, test_B5, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(sm, test_B6, WITH_RUN, WITH_EXIT);
-DECLARE_STATE(sm, test_C, WITH_RUN, WITH_EXIT);
+static void set_state_sm(const int port, const enum state new_state)
+{
+	set_state(port, &sm[port].ctx, &states[new_state]);
+}
 
 static void clear_seq(int port)
 {
@@ -159,494 +155,267 @@ static void clear_seq(int port)
 		sm[port].seq[i] = 0;
 }
 
-#if defined(TEST_USB_SM_FRAMEWORK_H3)
-static int sm_test_super_A1(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_super_A1_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int sm_test_super_A1_entry(int port)
+static void sm_test_super_A1_entry(const int port)
 {
 	sm[port].seq[sm[port].idx++] = ENTER_A1;
-	return 0;
 }
 
-static int sm_test_super_A1_run(int port)
+static void sm_test_super_A1_run(const int port)
 {
 	sm[port].seq[sm[port].idx++] = RUN_A1;
-	return 0;
 }
 
-static int sm_test_super_A1_exit(int port)
+static void sm_test_super_A1_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_A1;
-	return 0;
 }
 
-static int sm_test_super_B1(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_super_B1_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
-}
-
-static int sm_test_super_B1_entry(int port)
+static void sm_test_super_B1_entry(const int port)
 {
 	sm[port].seq[sm[port].idx++] = ENTER_B1;
-	return 0;
 }
 
-static int sm_test_super_B1_run(int port)
+static void sm_test_super_B1_run(const int port)
 {
 	sm[port].seq[sm[port].idx++] = RUN_B1;
-	return 0;
 }
 
-static int sm_test_super_B1_exit(int port)
+static void sm_test_super_B1_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_B1;
-	return 0;
-}
-#endif
-
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2)
-static int sm_test_super_A2(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_super_A2_sig[sig])(port);
-#if defined(TEST_USB_SM_FRAMEWORK_H3)
-	return SM_SUPER(ret, sig, sm_test_super_A1);
-#else
-	return SM_SUPER(ret, sig, 0);
-#endif
 }
 
-static int sm_test_super_A2_entry(int port)
+static void sm_test_super_A2_entry(const int port)
 {
 	sm[port].seq[sm[port].idx++] = ENTER_A2;
-	return 0;
 }
 
-static int sm_test_super_A2_run(int port)
+static void sm_test_super_A2_run(const int port)
 {
 	sm[port].seq[sm[port].idx++] = RUN_A2;
-	return SM_RUN_SUPER;
 }
 
-static int sm_test_super_A2_exit(int port)
+static void sm_test_super_A2_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_A2;
-	return 0;
 }
 
-static int sm_test_super_B2(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_super_B2_sig[sig])(port);
-#if defined(TEST_USB_SM_FRAMEWORK_H3)
-	return SM_SUPER(ret, sig, sm_test_super_B1);
-#else
-	return SM_SUPER(ret, sig, 0);
-#endif
-}
 
-static int sm_test_super_B2_entry(int port)
+static void sm_test_super_B2_entry(const int port)
 {
 	sm[port].seq[sm[port].idx++] = ENTER_B2;
-	return 0;
 }
 
-static int sm_test_super_B2_run(int port)
+static void sm_test_super_B2_run(const int port)
 {
 	sm[port].seq[sm[port].idx++] = RUN_B2;
-	return SM_RUN_SUPER;
 }
 
-static int sm_test_super_B2_exit(int port)
+static void sm_test_super_B2_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_B2;
-	return 0;
-}
-#endif
-
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2) || \
-		defined(TEST_USB_SM_FRAMEWORK_H1)
-static int sm_test_super_A3(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_super_A3_sig[sig])(port);
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2)
-	return SM_SUPER(ret, sig, sm_test_super_A2);
-#else
-	return SM_SUPER(ret, sig, 0);
-#endif
 }
 
-static int sm_test_super_A3_entry(int port)
+static void sm_test_super_A3_entry(const int port)
 {
 	sm[port].seq[sm[port].idx++] = ENTER_A3;
-	return 0;
 }
 
-static int sm_test_super_A3_run(int port)
+static void sm_test_super_A3_run(const int port)
 {
 	sm[port].seq[sm[port].idx++] = RUN_A3;
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2)
-	return SM_RUN_SUPER;
-#else
-	return 0;
-#endif
 }
 
-static int sm_test_super_A3_exit(int port)
+static void sm_test_super_A3_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_A3;
-	return 0;
-}
-
-static int sm_test_super_B3(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_super_B3_sig[sig])(port);
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2)
-	return SM_SUPER(ret, sig, sm_test_super_B2);
-#else
-	return SM_SUPER(ret, sig, 0);
-#endif
 }
 
-static int sm_test_super_B3_entry(int port)
+static void sm_test_super_B3_entry(const int port)
 {
 	sm[port].seq[sm[port].idx++] = ENTER_B3;
-	return 0;
 }
 
-static int sm_test_super_B3_run(int port)
+static void sm_test_super_B3_run(const int port)
 {
 	sm[port].seq[sm[port].idx++] = RUN_B3;
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2)
-	return SM_RUN_SUPER;
-#else
-	return 0;
-#endif
 }
 
-static int sm_test_super_B3_exit(int port)
+static void sm_test_super_B3_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_B3;
-	return 0;
-}
-#endif
-
-
-static int sm_test_A4(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_A4_sig[sig])(port);
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2) || \
-		defined(TEST_USB_SM_FRAMEWORK_H1)
-	return SM_SUPER(ret, sig, sm_test_super_A3);
-#else
-	return SM_SUPER(ret, sig, 0);
-#endif
 }
 
-static int sm_test_A4_entry(int port)
+static void sm_test_A4_entry(const int port)
 {
 	sm[port].sv_tmp = 0;
 	sm[port].seq[sm[port].idx++] = ENTER_A4;
-	return 0;
 }
 
-static int sm_test_A4_run(int port)
+static void sm_test_A4_run(const int port)
 {
 	if (sm[port].sv_tmp == 0) {
 		sm[port].sv_tmp = 1;
 		sm[port].seq[sm[port].idx++] = RUN_A4;
 	} else {
-		sm_set_state(port, TSM_OBJ(port), sm_test_B4);
-		return 0;
+		set_state_sm(port, SM_TEST_B4);
 	}
-
-	return SM_RUN_SUPER;
 }
 
-static int sm_test_A4_exit(int port)
+static void sm_test_A4_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_A4;
-	return 0;
 }
 
-static int sm_test_A5(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_A5_sig[sig])(port);
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2) || \
-		defined(TEST_USB_SM_FRAMEWORK_H1)
-	return SM_SUPER(ret, sig, sm_test_super_A3);
-#else
-	return SM_SUPER(ret, sig, 0);
-#endif
-}
 
-static int sm_test_A5_entry(int port)
+static void sm_test_A5_entry(const int port)
 {
 	sm[port].sv_tmp = 0;
 	sm[port].seq[sm[port].idx++] = ENTER_A5;
-	return 0;
 }
 
-static int sm_test_A5_run(int port)
+static void sm_test_A5_run(const int port)
 {
 	if (sm[port].sv_tmp == 0) {
 		sm[port].sv_tmp = 1;
 		sm[port].seq[sm[port].idx++] = RUN_A5;
 	} else {
-		sm_set_state(port, TSM_OBJ(port), sm_test_A4);
-		return 0;
+		set_state_sm(port, SM_TEST_A4);
 	}
-
-	return SM_RUN_SUPER;
 }
 
-static int sm_test_A5_exit(int port)
+static void sm_test_A5_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_A5;
-	return 0;
 }
 
-static int sm_test_A6(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_A6_sig[sig])(port);
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2)
-	return SM_SUPER(ret, sig, sm_test_super_A2);
-#else
-	return SM_SUPER(ret, sig, 0);
-#endif
-}
 
-static int sm_test_A6_entry(int port)
+static void sm_test_A6_entry(const int port)
 {
 	sm[port].sv_tmp = 0;
 	sm[port].seq[sm[port].idx++] = ENTER_A6;
-	return 0;
 }
 
-static int sm_test_A6_run(int port)
+static void sm_test_A6_run(const int port)
 {
 	if (sm[port].sv_tmp == 0) {
 		sm[port].sv_tmp = 1;
 		sm[port].seq[sm[port].idx++] = RUN_A6;
 	} else {
-		sm_set_state(port, TSM_OBJ(port), sm_test_A5);
-		return 0;
+		set_state_sm(port, SM_TEST_A5);
 	}
-
-	return SM_RUN_SUPER;
 }
 
-static int sm_test_A6_exit(int port)
+static void sm_test_A6_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_A6;
-	return 0;
-}
-
-static int sm_test_A7(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_A7_sig[sig])(port);
-#if defined(TEST_USB_SM_FRAMEWORK_H3)
-	return SM_SUPER(ret, sig, sm_test_super_A1);
-#else
-	return SM_SUPER(ret, sig, 0);
-#endif
 }
 
-static int sm_test_A7_entry(int port)
+static void sm_test_A7_entry(const int port)
 {
 	sm[port].sv_tmp = 0;
 	sm[port].seq[sm[port].idx++] = ENTER_A7;
-	return 0;
 }
 
-static int sm_test_A7_run(int port)
+static void sm_test_A7_run(const int port)
 {
 	if (sm[port].sv_tmp == 0) {
 		sm[port].sv_tmp = 1;
 		sm[port].seq[sm[port].idx++] = RUN_A7;
 	} else {
-		sm_set_state(port, TSM_OBJ(port), sm_test_A6);
-		return 0;
+		set_state_sm(port, SM_TEST_A6);
 	}
-
-	return SM_RUN_SUPER;
 }
 
-static int sm_test_A7_exit(int port)
+static void sm_test_A7_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_A7;
-	return 0;
-}
-
-static int sm_test_B4(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_B4_sig[sig])(port);
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2) || \
-		defined(TEST_USB_SM_FRAMEWORK_H1)
-	return SM_SUPER(ret, sig, sm_test_super_B3);
-#else
-	return SM_SUPER(ret, sig, 0);
-#endif
 }
 
-static int sm_test_B4_entry(int port)
+static void sm_test_B4_entry(const int port)
 {
 	sm[port].sv_tmp = 0;
 	sm[port].seq[sm[port].idx++] = ENTER_B4;
-	return 0;
 }
 
-static int sm_test_B4_run(int port)
+static void sm_test_B4_run(const int port)
 {
 	if (sm[port].sv_tmp == 0) {
 		sm[port].seq[sm[port].idx++] = RUN_B4;
 		sm[port].sv_tmp = 1;
 	} else {
-		sm_set_state(port, TSM_OBJ(port), sm_test_B5);
-		return 0;
+		set_state_sm(port, SM_TEST_B5);
 	}
-
-	return SM_RUN_SUPER;
 }
 
-static int sm_test_B4_exit(int port)
+static void sm_test_B4_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_B4;
-	return 0;
 }
 
-static int sm_test_B5(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_B5_sig[sig])(port);
-#if defined(TEST_USB_SM_FRAMEWORK_H3) || defined(TEST_USB_SM_FRAMEWORK_H2)
-	return SM_SUPER(ret, sig, sm_test_super_B2);
-#else
-	return SM_SUPER(ret, sig, 0);
-#endif
-}
 
-static int sm_test_B5_entry(int port)
+static void sm_test_B5_entry(const int port)
 {
 	sm[port].sv_tmp = 0;
 	sm[port].seq[sm[port].idx++] = ENTER_B5;
-	return 0;
 }
 
-static int sm_test_B5_run(int port)
+static void sm_test_B5_run(const int port)
 {
 	if (sm[port].sv_tmp == 0) {
 		sm[port].sv_tmp = 1;
 		sm[port].seq[sm[port].idx++] = RUN_B5;
 	} else {
-		sm_set_state(port, TSM_OBJ(port), sm_test_B6);
-		return 0;
+		set_state_sm(port, SM_TEST_B6);
 	}
-
-	return SM_RUN_SUPER;
 }
 
-static int sm_test_B5_exit(int port)
+static void sm_test_B5_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_B5;
-	return 0;
 }
 
-static int sm_test_B6(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_B6_sig[sig])(port);
-#if defined(TEST_USB_SM_FRAMEWORK_H3)
-	return SM_SUPER(ret, sig, sm_test_super_B1);
-#else
-	return SM_SUPER(ret, sig, 0);
-#endif
-}
 
-static int sm_test_B6_entry(int port)
+static void sm_test_B6_entry(const int port)
 {
 	sm[port].sv_tmp = 0;
 	sm[port].seq[sm[port].idx++] = ENTER_B6;
-	return 0;
 }
 
-static int sm_test_B6_run(int port)
+static void sm_test_B6_run(const int port)
 {
 	if (sm[port].sv_tmp == 0) {
 		sm[port].sv_tmp = 1;
 		sm[port].seq[sm[port].idx++] = RUN_B6;
 	} else {
-		sm_set_state(port, TSM_OBJ(port), sm_test_C);
-		return 0;
+		set_state_sm(port, SM_TEST_C);
 	}
-
-	return SM_RUN_SUPER;
 }
 
-static int sm_test_B6_exit(int port)
+static void sm_test_B6_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_B6;
-	return 0;
-}
-
-static int sm_test_C(int port, enum sm_signal sig)
-{
-	int ret;
-
-	ret = (*sm_test_C_sig[sig])(port);
-	return SM_SUPER(ret, sig, 0);
 }
 
-static int sm_test_C_entry(int port)
+static void sm_test_C_entry(const int port)
 {
 	sm[port].sv_tmp = 0;
 	sm[port].seq[sm[port].idx++] = ENTER_C;
-	return 0;
 }
 
-static int sm_test_C_run(int port)
+static void sm_test_C_run(const int port)
 {
 	if (sm[port].sv_tmp == 0) {
 		sm[port].seq[sm[port].idx++] = RUN_C;
 		sm[port].sv_tmp = 1;
 	} else {
-		sm_set_state(port, TSM_OBJ(port), sm_test_A7);
+		set_state_sm(port, SM_TEST_A7);
 	}
-
-	return 0;
 }
 
-static int sm_test_C_exit(int port)
+static void sm_test_C_exit(const int port)
 {
 	sm[port].seq[sm[port].idx++] = EXIT_C;
-	return 0;
 }
 
 static void run_sm(void)
@@ -655,14 +424,13 @@ static void run_sm(void)
 	task_wait_event(5 * MSEC);
 }
 
-#if defined(TEST_USB_SM_FRAMEWORK_H0)
-static int test_hierarchy_0(void)
+test_static int test_hierarchy_0(void)
 {
 	int port = PORT0;
 	int i = 0;
 
 	clear_seq(port);
-	sm_init_state(port, TSM_OBJ(port), sm_test_A4);
+	set_state_sm(port, SM_TEST_A4);
 
 	run_sm();
 	TEST_EQ(sm[port].seq[i], ENTER_A4, "%d"); ++i;
@@ -728,17 +496,14 @@ static int test_hierarchy_0(void)
 
 	return EC_SUCCESS;
 }
-#endif
 
-
-#if defined(TEST_USB_SM_FRAMEWORK_H1)
-static int test_hierarchy_1(void)
+test_static int test_hierarchy_1(void)
 {
 	int port = PORT0;
 	int i = 0;
 
 	clear_seq(port);
-	sm_init_state(port, TSM_OBJ(port), sm_test_A4);
+	set_state_sm(port, SM_TEST_A4);
 
 	run_sm();
 	TEST_EQ(sm[port].seq[i], ENTER_A3, "%d"); ++i;
@@ -812,18 +577,15 @@ static int test_hierarchy_1(void)
 
 	return EC_SUCCESS;
 }
-#endif
 
-
-#if defined(TEST_USB_SM_FRAMEWORK_H2)
-static int test_hierarchy_2(void)
+test_static int test_hierarchy_2(void)
 {
 
 	int port = PORT0;
 	int i = 0;
 
 	clear_seq(port);
-	sm_init_state(port, TSM_OBJ(port), sm_test_A4);
+	set_state_sm(port, SM_TEST_A4);
 
 	run_sm();
 	TEST_EQ(sm[port].seq[i], ENTER_A2, "%d"); ++i;
@@ -907,17 +669,15 @@ static int test_hierarchy_2(void)
 
 	return EC_SUCCESS;
 }
-#endif
 
-#if defined(TEST_USB_SM_FRAMEWORK_H3)
-static int test_hierarchy_3(void)
+test_static int test_hierarchy_3(void)
 {
 
 	int port = PORT0;
 	int i = 0;
 
 	clear_seq(port);
-	sm_init_state(port, TSM_OBJ(port), sm_test_A4);
+	set_state_sm(port, SM_TEST_A4);
 
 	run_sm();
 	TEST_EQ(sm[port].seq[i], ENTER_A1, "%d"); ++i;
@@ -1013,7 +773,125 @@ static int test_hierarchy_3(void)
 
 	return EC_SUCCESS;
 }
+
+
+#ifdef TEST_USB_SM_FRAMEWORK_H3
+#define TEST_AT_LEAST_3
+#endif
+
+#if defined(TEST_AT_LEAST_3) || defined(TEST_USB_SM_FRAMEWORK_H2)
+#define TEST_AT_LEAST_2
+#endif
+
+#if defined(TEST_AT_LEAST_2) || defined(TEST_USB_SM_FRAMEWORK_H1)
+#define TEST_AT_LEAST_1
+#endif
+
+static const struct usb_state states[] = {
+	[SM_TEST_SUPER_A1] = {
+		.entry  = sm_test_super_A1_entry,
+		.run    = sm_test_super_A1_run,
+		.exit   = sm_test_super_A1_exit,
+	},
+	[SM_TEST_SUPER_A2] = {
+		.entry  = sm_test_super_A2_entry,
+		.run    = sm_test_super_A2_run,
+		.exit   = sm_test_super_A2_exit,
+#ifdef TEST_AT_LEAST_3
+		.parent = &states[SM_TEST_SUPER_A1],
+#endif
+	},
+	[SM_TEST_SUPER_A3] = {
+		.entry  = sm_test_super_A3_entry,
+		.run    = sm_test_super_A3_run,
+		.exit   = sm_test_super_A3_exit,
+#ifdef TEST_AT_LEAST_2
+		.parent = &states[SM_TEST_SUPER_A2],
+#endif
+	},
+	[SM_TEST_SUPER_B1] = {
+		.entry  = sm_test_super_B1_entry,
+		.run    = sm_test_super_B1_run,
+		.exit   = sm_test_super_B1_exit,
+	},
+	[SM_TEST_SUPER_B2] = {
+		.entry  = sm_test_super_B2_entry,
+		.run    = sm_test_super_B2_run,
+		.exit   = sm_test_super_B2_exit,
+#ifdef TEST_AT_LEAST_3
+		.parent = &states[SM_TEST_SUPER_B1],
+#endif
+	},
+	[SM_TEST_SUPER_B3] = {
+		.entry  = sm_test_super_B3_entry,
+		.run    = sm_test_super_B3_run,
+		.exit   = sm_test_super_B3_exit,
+#ifdef TEST_AT_LEAST_2
+		.parent = &states[SM_TEST_SUPER_B2],
 #endif
+	},
+	[SM_TEST_A4] = {
+		.entry  = sm_test_A4_entry,
+		.run    = sm_test_A4_run,
+		.exit   = sm_test_A4_exit,
+#ifdef TEST_AT_LEAST_1
+		.parent = &states[SM_TEST_SUPER_A3],
+#endif
+	},
+	[SM_TEST_A5] = {
+		.entry  = sm_test_A5_entry,
+		.run    = sm_test_A5_run,
+		.exit   = sm_test_A5_exit,
+#ifdef TEST_AT_LEAST_1
+		.parent = &states[SM_TEST_SUPER_A3],
+#endif
+	},
+	[SM_TEST_A6] = {
+		.entry  = sm_test_A6_entry,
+		.run    = sm_test_A6_run,
+		.exit   = sm_test_A6_exit,
+#ifdef TEST_AT_LEAST_2
+		.parent = &states[SM_TEST_SUPER_A2],
+#endif
+	},
+	[SM_TEST_A7] = {
+		.entry  = sm_test_A7_entry,
+		.run    = sm_test_A7_run,
+		.exit   = sm_test_A7_exit,
+#ifdef TEST_AT_LEAST_3
+		.parent = &states[SM_TEST_SUPER_A1],
+#endif
+	},
+	[SM_TEST_B4] = {
+		.entry  = sm_test_B4_entry,
+		.run    = sm_test_B4_run,
+		.exit   = sm_test_B4_exit,
+#ifdef TEST_AT_LEAST_1
+		.parent = &states[SM_TEST_SUPER_B3],
+#endif
+	},
+	[SM_TEST_B5] = {
+		.entry  = sm_test_B5_entry,
+		.run    = sm_test_B5_run,
+		.exit   = sm_test_B5_exit,
+#ifdef TEST_AT_LEAST_2
+		.parent = &states[SM_TEST_SUPER_B2],
+#endif
+	},
+	[SM_TEST_B6] = {
+		.entry  = sm_test_B6_entry,
+		.run    = sm_test_B6_run,
+		.exit   = sm_test_B6_exit,
+#ifdef TEST_AT_LEAST_3
+		.parent = &states[SM_TEST_SUPER_B1],
+#endif
+	},
+	[SM_TEST_C] = {
+		.entry  = sm_test_C_entry,
+		.run    = sm_test_C_run,
+		.exit   = sm_test_C_exit,
+	},
+};
 
 int test_task(void *u)
 {
@@ -1023,7 +901,7 @@ int test_task(void *u)
 		/* wait for next event/packet or timeout expiration */
 		task_wait_event(-1);
 		/* run state machine */
-		sm_run_state_machine(port, TSM_OBJ(port), SM_RUN_SIG);
+		exe_state(port, &sm[port].ctx);
 	}
 
 	return EC_SUCCESS;
diff --git a/test/usb_typec_ctvpd.c b/test/usb_typec_ctvpd.c
index b2a100ad25..2d45b4f882 100644
--- a/test/usb_typec_ctvpd.c
+++ b/test/usb_typec_ctvpd.c
@@ -24,6 +24,36 @@ enum vbus_type {VBUS_0 = 0, VBUS_5 = 5000};
 enum vconn_type {VCONN_0 = 0, VCONN_3 = 3000, VCONN_5 = 5000};
 enum snk_con_voltage_type {SRC_CON_DEF, SRC_CON_1_5, SRC_CON_3_0};
 
+/*
+ * These enum definitions are declared in usb_tc_*_sm and are private to that
+ * file. If those definitions are re-ordered, then we need to update these
+ * definitions (should be very rare).
+ */
+enum usb_tc_state {
+	/* Normal States */
+	TC_DISABLED,
+	TC_UNATTACHED_SNK,
+	TC_ATTACH_WAIT_SNK,
+	TC_ATTACHED_SNK,
+	TC_ERROR_RECOVERY,
+	TC_TRY_SNK,
+	TC_UNATTACHED_SRC,
+	TC_ATTACH_WAIT_SRC,
+	TC_TRY_WAIT_SRC,
+	TC_ATTACHED_SRC,
+	TC_CT_TRY_SNK,
+	TC_CT_ATTACH_WAIT_UNSUPPORTED,
+	TC_CT_ATTACHED_UNSUPPORTED,
+	TC_CT_UNATTACHED_UNSUPPORTED,
+	TC_CT_UNATTACHED_VPD,
+	TC_CT_DISABLED_VPD,
+	TC_CT_ATTACHED_VPD,
+	TC_CT_ATTACH_WAIT_VPD,
+};
+
+/* Defined in implementation */
+enum usb_tc_state get_state_tc(const int port);
+
 struct pd_port_t {
 	int host_mode;
 	int has_vbus;
@@ -39,13 +69,13 @@ uint64_t wait_for_state_change(int port, uint64_t timeout)
 {
 	uint64_t start;
 	uint64_t wait;
-	uint32_t state = get_typec_state_id(port);
+	enum usb_tc_state state = get_state_tc(port);
 
 	task_wake(PD_PORT_TO_TASK_ID(port));
 
 	wait = get_time().val + timeout;
 	start = get_time().val;
-	while (get_typec_state_id(port) == state && get_time().val < wait)
+	while (get_state_tc(port) == state && get_time().val < wait)
 		task_wait_event(5 * MSEC);
 
 	return get_time().val - start;
@@ -330,7 +360,7 @@ static int test_vpd_host_src_detection(void)
 	 * Host is configured properly and start state is UNATTACHED_SNK
 	 */
 	TEST_ASSERT(check_host_ra_rd());
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	/*
 	 * TEST:
@@ -342,7 +372,7 @@ static int test_vpd_host_src_detection(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACH_WAIT_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACH_WAIT_SNK);
 
 	/*
 	 * TEST:
@@ -373,7 +403,7 @@ static int test_vpd_host_src_detection(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	return EC_SUCCESS;
 }
@@ -394,7 +424,7 @@ static int test_vpd_host_src_detection_vbus(void)
 	 */
 
 	TEST_ASSERT(check_host_ra_rd());
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	/*
 	 * TEST:
@@ -406,7 +436,7 @@ static int test_vpd_host_src_detection_vbus(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACH_WAIT_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACH_WAIT_SNK);
 
 	/*
 	 * TEST:
@@ -418,7 +448,7 @@ static int test_vpd_host_src_detection_vbus(void)
 
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 10 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACHED_SNK);
 
 	/*
 	 * TEST:
@@ -429,7 +459,7 @@ static int test_vpd_host_src_detection_vbus(void)
 
 	wait_for_state_change(port, 10 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	return EC_SUCCESS;
 }
@@ -450,7 +480,7 @@ static int test_vpd_host_src_detection_vconn(void)
 	 */
 
 	TEST_ASSERT(check_host_ra_rd());
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	/*
 	 * TEST:
@@ -462,7 +492,7 @@ static int test_vpd_host_src_detection_vconn(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACH_WAIT_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACH_WAIT_SNK);
 
 	/*
 	 * TEST:
@@ -474,7 +504,7 @@ static int test_vpd_host_src_detection_vconn(void)
 
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 10 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACHED_SNK);
 
 	/* VCONN was detected. Make sure RA is removed */
 	task_wake(PD_PORT_TO_TASK_ID(port));
@@ -490,7 +520,7 @@ static int test_vpd_host_src_detection_vconn(void)
 
 	wait_for_state_change(port, 10 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	host_disconnect_source();
 
@@ -540,7 +570,7 @@ static int test_vpd_host_src_detection_message_reception(void)
 	 */
 
 	TEST_ASSERT(check_host_ra_rd());
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	/*
 	 * Transition to ATTACHED_SNK
@@ -550,11 +580,11 @@ static int test_vpd_host_src_detection_message_reception(void)
 
 	wait_for_state_change(port, 10 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACH_WAIT_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACH_WAIT_SNK);
 
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 20 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACHED_SNK);
 
 	/* Run state machines to enable rx monitoring */
 	task_wake(PD_PORT_TO_TASK_ID(port));
@@ -606,7 +636,7 @@ static int test_vpd_host_src_detection_message_reception(void)
 
 	wait_for_state_change(port, 10 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	host_disconnect_source();
 
@@ -634,7 +664,7 @@ static int test_ctvpd_behavior_case1(void)
 	 */
 
 	/* 1. DRP and CTVPD are both in the unattached state */
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	/*
 	 *   a. DRP alternates between Unattached.SRC and Unattached.SNK
@@ -670,10 +700,10 @@ static int test_ctvpd_behavior_case1(void)
 	 *       Charge-Through port’s CC1 and CC2 pins
 	 */
 	wait_for_state_change(port, 40 * MSEC);
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACH_WAIT_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACH_WAIT_SNK);
 
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 40 * MSEC);
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACHED_SNK);
 	TEST_ASSERT(check_ct_ccs_hz());
 
 	/*
@@ -702,7 +732,7 @@ static int test_ctvpd_behavior_case1(void)
 	 *    d. CTVPD apply Rd on its Charge-Through port’s CC1 and CC2 pins
 	 */
 	wait_for_state_change(port, 40 * MSEC);
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_VPD);
 
 	/*
 	 * 6. While the CTVPD in CTUnattached.VPD state and the DRP in
@@ -723,32 +753,32 @@ static int test_ctvpd_behavior_case1(void)
 	TEST_ASSERT(ct_connect_source(CC2, VBUS_0));
 
 	wait_for_state_change(port, 40 * MSEC);
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTATTACH_WAIT_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_ATTACH_WAIT_VPD);
 
 	/* Remove Power Source */
 	TEST_ASSERT(ct_disconnect_source());
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_VPD);
 
 	/* Attach Sink */
 	TEST_ASSERT(ct_connect_sink(CC1, SRC_CON_DEF));
 
 	wait_for_state_change(port, PD_T_DRP_SNK);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_UNSUPPORTED);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_UNSUPPORTED);
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTATTACH_WAIT_UNSUPPORTED);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_ATTACH_WAIT_UNSUPPORTED);
 
 	/* Remove VCONN (Host detach) */
 	mock_set_vconn(VCONN_0);
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	return EC_SUCCESS;
 }
@@ -780,25 +810,25 @@ static int test_ctvpd_behavior_case2(void)
 	 *       pins and Rp termination advertising 3.0 A on the Host-side
 	 *       port’s CC pin
 	 */
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	host_connect_source(VBUS_5);
 	mock_set_vconn(VCONN_3);
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACH_WAIT_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACH_WAIT_SNK);
 
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACHED_SNK);
 
 	/* Remove Host CC */
 	mock_set_host_cc_source_voltage(0);
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_VPD);
 	TEST_ASSERT(check_ct_ccs_rd());
 	TEST_ASSERT(check_host_rp3a0());
 
@@ -836,11 +866,11 @@ static int test_ctvpd_behavior_case2(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTATTACH_WAIT_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_ATTACH_WAIT_VPD);
 
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTATTACHED_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_ATTACHED_VPD);
 	TEST_ASSERT(moch_get_ct_cl_sel() == CT_CC2);
 	TEST_ASSERT(check_host_cc_open());
 	TEST_ASSERT(check_ct_ccs_hz());
@@ -870,7 +900,7 @@ static int test_ctvpd_behavior_case2(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTDISABLED_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_DISABLED_VPD);
 
 	return EC_SUCCESS;
 }
@@ -899,7 +929,7 @@ static int test_ctvpd_behavior_case3(void)
 	 *    a. CTVPD has applied Rd on the Charge-Through port’s CC1 and CC2
 	 *       pins and Rd on the Host-side port’s CC pin
 	 */
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	TEST_ASSERT(check_ct_ccs_rd());
 	TEST_ASSERT(check_host_ra_rd());
@@ -923,7 +953,7 @@ static int test_ctvpd_behavior_case3(void)
 	 *       between Unattached.SRC and Unattached.SNK
 	 */
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 40 * MSEC);
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SRC);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SRC);
 
 	/*
 	 * 4. While the CTVPD alternates between Unattached.SRC and
@@ -942,14 +972,14 @@ static int test_ctvpd_behavior_case3(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACH_WAIT_SRC);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACH_WAIT_SRC);
 
 	/* Remove VBUS */
 	TEST_ASSERT(ct_disconnect_source());
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	return EC_SUCCESS;
 }
@@ -1004,7 +1034,7 @@ static int test_ctvpd_behavior_case4(void)
 	 *    b. CTVPD has applied Rd on its Charge-Through port’s CC1 and CC2
 	 *       pins and Rd on the Host-side port’s CC pin
 	 */
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	TEST_ASSERT(check_ct_ccs_rd());
 	TEST_ASSERT(check_host_ra_rd());
@@ -1038,11 +1068,11 @@ static int test_ctvpd_behavior_case4(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACH_WAIT_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACH_WAIT_SNK);
 
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACHED_SNK);
 	TEST_ASSERT(check_ct_ccs_hz());
 
 	/*
@@ -1109,7 +1139,7 @@ static int test_ctvpd_behavior_case4(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_VPD);
 	TEST_ASSERT(check_ct_ccs_rd());
 	TEST_ASSERT(check_host_rp3a0());
 
@@ -1119,16 +1149,16 @@ static int test_ctvpd_behavior_case4(void)
 	 */
 	wait_for_state_change(port, PD_T_DRP_SRC + 10 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_UNSUPPORTED);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_UNSUPPORTED);
 
 	wait_for_state_change(port, PD_T_DRP_SRC + 10 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_VPD);
 	TEST_ASSERT(ct_connect_source(CC2, VBUS_5));
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTATTACH_WAIT_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_ATTACH_WAIT_VPD);
 
 	return EC_SUCCESS;
 }
@@ -1159,7 +1189,7 @@ static int test_ctvpd_behavior_case5(void)
 	 *    b. CTVPD apply Rd on the Charge-Through port’s CC1 and CC2 pins
 	 *       and Rd on the Host-side port’s CC pin
 	 */
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	TEST_ASSERT(check_ct_ccs_rd());
 	TEST_ASSERT(check_host_ra_rd());
@@ -1184,7 +1214,7 @@ static int test_ctvpd_behavior_case5(void)
 	 */
 
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 40 * MSEC);
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SRC);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SRC);
 
 	/* Connect Host With Dead Battery */
 	host_connect_sink(SRC_CON_DEF);
@@ -1204,11 +1234,11 @@ static int test_ctvpd_behavior_case5(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACH_WAIT_SRC);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACH_WAIT_SRC);
 
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 10 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_TRY_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_TRY_SNK);
 	TEST_ASSERT(check_host_ra_rd());
 
 	/* 5. DRP in dead battery condition remains in Unattached.SNK */
@@ -1230,12 +1260,12 @@ static int test_ctvpd_behavior_case5(void)
 	wait_for_state_change(port, PD_T_TRY_CC_DEBOUNCE +
 						PD_T_DRP_TRY + 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_TRY_WAIT_SRC);
+	TEST_ASSERT(get_state_tc(port) == TC_TRY_WAIT_SRC);
 	TEST_ASSERT(check_host_rpusb());
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACHED_SRC);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACHED_SRC);
 	TEST_ASSERT(mock_get_vbus_pass_en());
 
 	/*
@@ -1268,7 +1298,7 @@ static int test_ctvpd_behavior_case5(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 
 	return EC_SUCCESS;
 }
@@ -1299,7 +1329,7 @@ static int test_ctvpd_behavior_case6(void)
 	 *    b. CTVPD has applied Rd on its Charge-Through port’s CC1 and CC2
 	 *       pins and Rd on the Host-side port’s CC pin
 	 */
-	TEST_ASSERT(get_typec_state_id(port) == TC_UNATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_UNATTACHED_SNK);
 	TEST_ASSERT(check_ct_ccs_rd());
 	TEST_ASSERT(check_host_ra_rd());
 
@@ -1330,11 +1360,11 @@ static int test_ctvpd_behavior_case6(void)
 	 */
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACH_WAIT_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACH_WAIT_SNK);
 
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_ATTACHED_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_ATTACHED_SNK);
 	TEST_ASSERT(check_ct_ccs_hz());
 
 	/*
@@ -1367,7 +1397,7 @@ static int test_ctvpd_behavior_case6(void)
 	 */
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_VPD);
 	TEST_ASSERT(check_host_rp3a0());
 	TEST_ASSERT(mock_get_vbus_pass_en() == 0);
 	TEST_ASSERT(check_ct_ccs_rd());
@@ -1382,15 +1412,15 @@ static int test_ctvpd_behavior_case6(void)
 	 */
 	wait_for_state_change(port, PD_T_DRP_SNK + 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_UNSUPPORTED);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_UNSUPPORTED);
 
 	wait_for_state_change(port, PD_T_DRP_SNK + 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_VPD);
 
 	wait_for_state_change(port, PD_T_DRP_SNK + 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_UNSUPPORTED);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_UNSUPPORTED);
 
 	/*
 	 * 7. CTVPD transitions from CTUnattached.Unsupported to CTTry.SNK
@@ -1409,11 +1439,11 @@ static int test_ctvpd_behavior_case6(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTATTACH_WAIT_UNSUPPORTED);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_ATTACH_WAIT_UNSUPPORTED);
 
 	wait_for_state_change(port, PD_T_CC_DEBOUNCE + 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTTRY_SNK);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_TRY_SNK);
 	TEST_ASSERT(check_ct_ccs_rd());
 
 	/*
@@ -1426,7 +1456,7 @@ static int test_ctvpd_behavior_case6(void)
 
 	wait_for_state_change(port, PD_T_DRP_TRY + PD_T_TRY_WAIT + 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTATTACHED_UNSUPPORTED);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_ATTACHED_UNSUPPORTED);
 
 	/*
 	 * 9. While the CTVPD in CTAttached.Unsupported state, the DRP in
@@ -1455,7 +1485,7 @@ static int test_ctvpd_behavior_case6(void)
 
 	wait_for_state_change(port, 40 * MSEC);
 
-	TEST_ASSERT(get_typec_state_id(port) == TC_CTUNATTACHED_VPD);
+	TEST_ASSERT(get_state_tc(port) == TC_CT_UNATTACHED_VPD);
 
 	return EC_SUCCESS;
 }