Lib: SMF: Add initial transition and smf_set_handled()

Brings SMF framework closer into alignment with accepted Hierarchical State
Machine operation by:
1. Allowing 'programming by difference' by having some child states handle
   events and prevent propagation up to the parent run actions while others
   propagate events up to a common handler in a parent state.
2. Optionally allow initial transitions within a parent state to determine
   the most nested child state to transition to.
3. Adding a test case for `CONFIG_SMF_INITIAL_TRANSITION` and
   `smf_set_handled()`
4. Updating documentation for the new API (and fixing some references)

There was discussion in https://github.com/zephyrproject-rtos/zephyr/issues/55344
about not making the initial transition a Kconfig option, but I'm not sure
of any way else of doing it without permanently adding a pointer to each
`smf_state` entry, which is a problem for resource-constrained devices.

This does not fix https://github.com/zephyrproject-rtos/zephyr/issues/66341
but documentation has been updated to warn users of the issue.

Signed-off-by: Glenn Andrews <glenn.andrews.42@gmail.com>
diff --git a/doc/services/smf/index.rst b/doc/services/smf/index.rst
index d5f1f91..bc745c0 100644
--- a/doc/services/smf/index.rst
+++ b/doc/services/smf/index.rst
@@ -20,7 +20,7 @@
 Entry actions, another function implements the Run actions, and the last
 function implements the Exit actions. The prototype for these functions is as
 follows: ``void funct(void *obj)``, where the ``obj`` parameter is a user
-defined structure that has the state machine context, ``struct smf_ctx``, as
+defined structure that has the state machine context, :c:struct:`smf_ctx`, as
 its first member. For example::
 
    struct user_object {
@@ -28,9 +28,9 @@
       /* All User Defined Data Follows */
    };
 
-The ``struct smf_ctx`` member must be first because the state machine
-framework's functions casts the user defined object to the ``struct smf_ctx``
-type with the following macro: ``SMF_CTX(o)``
+The :c:struct:`smf_ctx` member must be first because the state machine
+framework's functions casts the user defined object to the :c:struct:`smf_ctx`
+type with the :c:macro:`SMF_CTX` macro.
 
 For example instead of doing this ``(struct smf_ctx *)&user_obj``, you could
 use ``SMF_CTX(&user_obj)``.
@@ -39,12 +39,19 @@
 machine. But to enable the creation of a hierarchical state machine, the
 :kconfig:option:`CONFIG_SMF_ANCESTOR_SUPPORT` option must be enabled.
 
+By default, the hierarchical state machine does not support initial transitions
+to child states on entering a superstate. To enable them the
+:kconfig:option:`CONFIG_SMF_INITIAL_TRANSITION` option must be enabled.
+
 The following macro can be used for easy state creation:
 
 * :c:macro:`SMF_CREATE_STATE` Create a state
 
-**NOTE:** The :c:macro:`SMF_CREATE_STATE` macro takes an additional parameter
-when :kconfig:option:`CONFIG_SMF_ANCESTOR_SUPPORT` is enabled.
+.. note:: The :c:macro:`SMF_CREATE_STATE` macro takes an additional parameter
+   for the parent state when :kconfig:option:`CONFIG_SMF_ANCESTOR_SUPPORT` is
+   enabled . The :c:macro:`SMF_CREATE_STATE` macro takes two additional
+   parameters for the parent state and initial transition when the
+   :kconfig:option:`CONFIG_SMF_INITIAL_TRANSITION` option is enabled.
 
 State Machine Creation
 ======================
@@ -71,33 +78,61 @@
    };
 
 
-To set the initial state, the ``smf_set_initial`` function should be
+This example creates three hierarchical states with an initial transition
+from parent state S0 to child state S2::
+
+   enum demo_state { S0, S1, S2 };
+
+   /* Forward declaration of state table */
+   const struct smf_state demo_states[];
+
+   const struct smf_state demo_states[] = {
+      [S0] = SMF_CREATE_STATE(s0_entry, s0_run, s0_exit, NULL, demo_states[S2]),
+      [S1] = SMF_CREATE_STATE(s1_entry, s1_run, s1_exit, demo_states[S0], NULL),
+      [S2] = SMF_CREATE_STATE(s2_entry, s2_run, s2_exit, demo_states[S0], NULL)
+   };
+
+To set the initial state, the :c:func:`smf_set_initial` function should be
 called. It has the following prototype:
 ``void smf_set_initial(smf_ctx *ctx, smf_state *state)``
 
-To transition from one state to another, the ``smf_set_state`` function is
-used and it has the following prototype:
+To transition from one state to another, the :c:func:`smf_set_state`
+function is used and it has the following prototype:
 ``void smf_set_state(smf_ctx *ctx, smf_state *state)``
 
-**NOTE:** While the state machine is running, smf_set_state should only be
-called from the Entry and Run functions. Calling smf_set_state from the Exit
-functions doesn't make sense and will generate a warning.
+.. note:: If :kconfig:option:`CONFIG_SMF_INITIAL_TRANSITION` is not set,
+   :c:func:`smf_set_initial` and :c:func:`smf_set_state` function should
+   not be passed a parent state as they will not know which child state
+   to transition to. Transitioning to a parent state is OK if an initial
+   transition to a child state is defined. A well-formed HSM will have
+   initial transitions defined for all parent states.
+
+.. note:: While the state machine is running, smf_set_state should only be
+   called from the Entry and Run functions. Calling smf_set_state from the
+   Exit functions doesn't make sense and will generate a warning.
 
 State Machine Execution
 =======================
 
-To run the state machine, the ``smf_run_state`` function should be called in
-some application dependent way. An application should cease calling
+To run the state machine, the :c:func:`smf_run_state` function should be
+called in some application dependent way. An application should cease calling
 smf_run_state if it returns a non-zero value. The function has the following
 prototype: ``int32_t smf_run_state(smf_ctx *ctx)``
 
+Preventing Parent Run Actions
+=============================
+
+Calling :c:func:`smf_set_handled` prevents calling the run action of parent
+states. It is not required to call :c:func:`smf_set_handled` if the state
+calls :c:func:`smf_set_state`.
+
 State Machine Termination
 =========================
 
-To terminate the state machine, the ``smf_terminate`` function should be
-called. It can be called from the entry, run, or exit action. The function
-takes a non-zero user defined value that's returned by the ``smf_run_state``
-function. The function has the following prototype:
+To terminate the state machine, the :c:func:`smf_terminate` function should
+be called. It can be called from the entry, run, or exit action. The
+function takes a non-zero user defined value that's returned by the
+:c:func:`smf_run_state` function. The function has the following prototype:
 ``void smf_terminate(smf_ctx *ctx, int32_t val)``
 
 Flat State Machine Example
@@ -316,8 +351,11 @@
  - Ancestor exit actions are executed after the sibling exit actions. For
    example, the s1_exit function is called before the parent_exit function
    is called.
- - The parent_run function only executes if the child_run function returns
-   without transitioning to another state, ie. calling smf_set_state.
+ - The parent_run function only executes if the child_run function does not
+   call either :c:func:`smf_set_state` or :c:func:`smf_set_handled`.
+ - When a parent state intitiates a transition to self, the parents's exit
+   action is not called, e.g. instead of child_exit, parent_exit, parent_entry
+   it performs child_exit, parent_entry
 
 Event Driven State Machine Example
 ==================================
@@ -466,3 +504,41 @@
 			}
 		}
 	}
+
+Hierarchical State Machine Example With Initial Transitions
+===========================================================
+
+:zephyr_file:`tests/lib/smf/src/test_lib_initial_transitions_smf.c` defines
+a state machine for testing initial transitions and :c:func:`smf_set_handled`.
+The statechart for this test is below.
+
+.. graphviz::
+   :caption: Test state machine for initial trnasitions and ``smf_set_handled``
+
+   digraph smf_hierarchical_initial {
+      compound=true;
+      node [style = rounded];
+      smf_set_initial [shape=plaintext];
+      ab_init_state [shape = point];
+      STATE_A [shape = box];
+      STATE_B [shape = box];
+      STATE_C [shape = box];
+      STATE_D [shape = box];
+
+      subgraph cluster_ab {
+         label = "PARENT_AB";
+         style = rounded;
+         ab_init_state -> STATE_A;
+         STATE_A -> STATE_B;
+      }
+
+      subgraph cluster_c {
+         label = "PARENT_C";
+         style = rounded;
+	 STATE_C -> STATE_C
+      }
+
+      smf_set_initial -> STATE_A [lhead=cluster_ab]
+      STATE_B -> STATE_C
+      STATE_C -> STATE_D
+   }
diff --git a/include/zephyr/smf.h b/include/zephyr/smf.h
index 2fdcbbd..b4aee26 100644
--- a/include/zephyr/smf.h
+++ b/include/zephyr/smf.h
@@ -18,6 +18,7 @@
  * @param _exit   State exit function
  * @param _parent State parent object or NULL
  */
+#ifndef CONFIG_SMF_INITIAL_TRANSITION
 #define SMF_CREATE_STATE(_entry, _run, _exit, _parent) \
 { \
 	.entry  = _entry, \
@@ -25,6 +26,25 @@
 	.exit   = _exit,  \
 	.parent = _parent \
 }
+#else
+/**
+ * @brief Macro to create a hierarchical state.
+ *
+ * @param _entry   State entry function
+ * @param _run     State run function
+ * @param _exit    State exit function
+ * @param _parent  State parent object or NULL
+ * @param _initial State initial transition object or NULL
+ */
+#define SMF_CREATE_STATE(_entry, _run, _exit, _parent, _initial) \
+{ \
+	.entry   = _entry,  \
+	.run     = _run,    \
+	.exit    = _exit,   \
+	.parent  = _parent, \
+	.initial = _initial \
+}
+#endif /* CONFIG_SMF_INITIAL_TRANSITION */
 
 #else
 
@@ -87,6 +107,13 @@
 	 *	that parent's exit and entry functions do not execute.
 	 */
 	const struct smf_state *parent;
+
+#ifdef CONFIG_SMF_INITIAL_TRANSITION
+	/**
+	 * Optional initial transition state. NULL for leaf states.
+	 */
+	const struct smf_state *initial;
+#endif
 };
 
 /** Defines the current context of the state machine. */
@@ -137,6 +164,15 @@
 void smf_set_terminate(struct smf_ctx *ctx, int32_t val);
 
 /**
+ * @brief Tell the SMF to stop propagating the event to ancestors. This allows
+ *        HSMs to implement 'programming by difference' where substates can
+ *        handle events on their own or propagate up to a common handler.
+ *
+ * @param ctx  State machine context
+ */
+void smf_set_handled(struct smf_ctx *ctx);
+
+/**
  * @brief Runs one iteration of a state machine (including any parent states)
  *
  * @param ctx  State machine context
diff --git a/lib/smf/Kconfig b/lib/smf/Kconfig
index 4204e9c..19cd165 100644
--- a/lib/smf/Kconfig
+++ b/lib/smf/Kconfig
@@ -13,4 +13,10 @@
 	help
 	   If y, then the state machine framework includes ancestor state support
 
+config SMF_INITIAL_TRANSITION
+	depends on SMF_ANCESTOR_SUPPORT
+	bool "Support initial transitions for ancestor states"
+	help
+	   If y, then each state can have an initial transition to a sub-state
+
 endif # SMF
diff --git a/lib/smf/smf.c b/lib/smf/smf.c
index 01fb42e..d869de4 100644
--- a/lib/smf/smf.c
+++ b/lib/smf/smf.c
@@ -18,6 +18,7 @@
 	bool new_state : 1;
 	bool terminate : 1;
 	bool exit      : 1;
+	bool handled   : 1;
 };
 
 static bool share_paren(const struct smf_state *test_state,
@@ -118,6 +119,12 @@
 		return true;
 	}
 
+	if (internal->handled) {
+		/* Event was handled by this state. Stop propagating */
+		internal->handled = false;
+		return false;
+	}
+
 	/* Try to run parent run actions */
 	for (const struct smf_state *tmp_state = ctx->current->parent;
 	     tmp_state != NULL;
@@ -133,6 +140,12 @@
 			if (internal->new_state) {
 				break;
 			}
+
+			if (internal->handled) {
+				/* Event was handled by this state. Stop propagating */
+				internal->handled = false;
+				break;
+			}
 		}
 	}
 
@@ -175,6 +188,16 @@
 {
 	struct internal_ctx * const internal = (void *) &ctx->internal;
 
+
+#ifdef CONFIG_SMF_INITIAL_TRANSITION
+	/*
+	 * The final target will be the deepest leaf state that
+	 * the target contains. Set that as the real target.
+	 */
+	while (init_state->initial) {
+		init_state = init_state->initial;
+	}
+#endif
 	internal->exit = false;
 	internal->terminate = false;
 	ctx->current = init_state;
@@ -234,6 +257,16 @@
 
 	internal->exit = false;
 
+#ifdef CONFIG_SMF_INITIAL_TRANSITION
+	/*
+	 * The final target will be the deepest leaf state that
+	 * the target contains. Set that as the real target.
+	 */
+	while (target->initial) {
+		target = target->initial;
+	}
+#endif
+
 	/* update the state variables */
 	ctx->previous = ctx->current;
 	ctx->current = target;
@@ -262,6 +295,13 @@
 	ctx->terminate_val = val;
 }
 
+void smf_set_handled(struct smf_ctx *ctx)
+{
+	struct internal_ctx *const internal = (void *)&ctx->internal;
+
+	internal->handled = true;
+}
+
 int32_t smf_run_state(struct smf_ctx *const ctx)
 {
 	struct internal_ctx * const internal = (void *) &ctx->internal;
diff --git a/tests/lib/smf/CMakeLists.txt b/tests/lib/smf/CMakeLists.txt
index 147223b..08844e7 100644
--- a/tests/lib/smf/CMakeLists.txt
+++ b/tests/lib/smf/CMakeLists.txt
@@ -6,7 +6,9 @@
 
 target_sources(app PRIVATE src/main.c)
 
-if(CONFIG_SMF_ANCESTOR_SUPPORT)
+if(CONFIG_SMF_INITIAL_TRANSITION)
+  target_sources(app PRIVATE src/test_lib_initial_transitions_smf.c)
+elseif(CONFIG_SMF_ANCESTOR_SUPPORT)
   target_sources(app PRIVATE src/test_lib_hierarchical_smf.c
     src/test_lib_hierarchical_5_ancestor_smf.c)
 else()
diff --git a/tests/lib/smf/src/test_lib_initial_transitions_smf.c b/tests/lib/smf/src/test_lib_initial_transitions_smf.c
new file mode 100644
index 0000000..2e00dc2
--- /dev/null
+++ b/tests/lib/smf/src/test_lib_initial_transitions_smf.c
@@ -0,0 +1,501 @@
+/*
+ * Copyright 2024 Glenn Andrews
+ * based on test_lib_hierarchical_smf.c
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <zephyr/ztest.h>
+#include <zephyr/smf.h>
+
+/*
+ * Hierarchical Test Transition:
+ *
+ *	PARENT_AB_ENTRY --> A_ENTRY --> A_RUN --> PARENT_AB_RUN ---|
+ *	                                                           |
+ *	|----------------------------------------------------------|
+ *	|
+ *	|--> A_EXIT --> B_ENTRY --> B_RUN --> B_EXIT --------------|
+ *	                                                           |
+ *	|----------------------------------------------------------|
+ *	|
+ *	|--> PARENT_AB_EXIT --> PARENT_C_ENTRY --> C_ENTRY --------|
+ *	                                                           |
+ *	|----------------------------------------------------------|
+ *      |
+ *      |--> C_RUN(#1) --> C_RUN(#2) --> C_EXIT --> PARENT_C_RUN --|
+ *	                                                           |
+ *      |----------------------------------------------------------|
+ *      |
+ *	|--> PARENT_C_EXIT
+ */
+
+#define TEST_OBJECT(o) ((struct test_object *)o)
+
+#define SMF_RUN                 4
+
+/* Initial Setup */
+#define PARENT_AB_ENTRY_BIT     (1 << 0)
+#define STATE_A_ENTRY_BIT       (1 << 1)
+
+/* Run 0 */
+#define STATE_A_RUN_BIT         (1 << 2)
+#define PARENT_AB_RUN_BIT       (1 << 3)
+#define STATE_A_EXIT_BIT        (1 << 4)
+#define STATE_B_ENTRY_BIT       (1 << 5)
+
+/* Run 1 */
+#define STATE_B_RUN_BIT         (1 << 6)
+#define STATE_B_EXIT_BIT        (1 << 7)
+#define PARENT_AB_EXIT_BIT      (1 << 8)
+#define PARENT_C_ENTRY_BIT      (1 << 9)
+#define STATE_C_ENTRY_BIT       (1 << 10)
+
+/* Run 2 */
+#define STATE_C_1ST_RUN_BIT     (1 << 11)
+
+/* Run 3 */
+#define STATE_C_2ND_RUN_BIT     (1 << 12)
+#define PARENT_C_RUN_BIT        (1 << 13)
+#define STATE_C_EXIT_BIT        (1 << 14)
+#define PARENT_C_EXIT_BIT       (1 << 15)
+
+#define TEST_PARENT_ENTRY_VALUE_NUM 0
+#define TEST_PARENT_RUN_VALUE_NUM   3
+#define TEST_PARENT_EXIT_VALUE_NUM  8
+#define TEST_ENTRY_VALUE_NUM        1
+#define TEST_RUN_VALUE_NUM          6
+#define TEST_EXIT_VALUE_NUM         14
+#define TEST_VALUE_NUM              16
+static uint32_t test_value[] = {
+	0x00,   /* PARENT_AB_ENTRY */
+	0x01,   /* STATE_A_ENTRY */
+	0x03,   /* STATE_A_RUN */
+	0x07,   /* PARENT_AB_RUN */
+	0x0f,   /* STATE_A_EXIT */
+	0x1f,   /* STATE_B_ENTRY */
+	0x3f,   /* STATE_B_RUN */
+	0x7f,   /* STATE_B_EXIT */
+	0xff,   /* PARENT_AB_EXIT */
+	0x1ff,  /* PARENT_C_ENTRY */
+	0x3ff,  /* STATE_C_ENTRY */
+	0x7ff,  /* STATE_C_1ST_RUN */
+	0xfff,  /* STATE_C_2ND_RUN */
+	0x1fff, /* STATE_C_EXIT */
+	0x3fff, /* PARENT_C_RUN */
+	0x7fff, /* PARENT_C_EXIT */
+	0xffff, /* FINAL VALUE */
+};
+
+/* Forward declaration of test_states */
+static const struct smf_state test_states[];
+
+/* List of all TypeC-level states */
+enum test_state {
+	PARENT_AB,
+	PARENT_C,
+	STATE_A,
+	STATE_B,
+	STATE_C,
+	STATE_D
+};
+
+enum terminate_action {
+	NONE,
+	PARENT_ENTRY,
+	PARENT_RUN,
+	PARENT_EXIT,
+	ENTRY,
+	RUN,
+	EXIT
+};
+
+static struct test_object {
+	struct smf_ctx ctx;
+	uint32_t transition_bits;
+	uint32_t tv_idx;
+	enum terminate_action terminate;
+	uint32_t first_time;
+} test_obj;
+
+static void parent_ab_entry(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx = 0;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test Parent AB entry failed");
+
+	if (o->terminate == PARENT_ENTRY) {
+		smf_set_terminate(obj, -1);
+		return;
+	}
+
+	o->transition_bits |= PARENT_AB_ENTRY_BIT;
+}
+
+static void parent_ab_run(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test Parent AB run failed");
+
+	if (o->terminate == PARENT_RUN) {
+		smf_set_terminate(obj, -1);
+		return;
+	}
+
+	o->transition_bits |= PARENT_AB_RUN_BIT;
+
+	smf_set_state(SMF_CTX(obj), &test_states[STATE_B]);
+}
+
+static void parent_ab_exit(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test Parent AB exit failed");
+
+	if (o->terminate == PARENT_EXIT) {
+		smf_set_terminate(obj, -1);
+		return;
+	}
+
+	o->transition_bits |= PARENT_AB_EXIT_BIT;
+}
+
+static void parent_c_entry(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test Parent C entry failed");
+	o->transition_bits |= PARENT_C_ENTRY_BIT;
+}
+
+static void parent_c_run(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	if (o->first_time) {
+		/* This state should not be reached */
+		zassert_true(0, "Test Parent C run failed");
+	} else {
+		o->transition_bits |= PARENT_C_RUN_BIT;
+
+		smf_set_state(SMF_CTX(obj), &test_states[STATE_D]);
+	}
+}
+
+static void parent_c_exit(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test Parent C exit failed");
+	o->transition_bits |= PARENT_C_EXIT_BIT;
+}
+
+static void state_a_entry(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test State A entry failed");
+
+	if (o->terminate == ENTRY) {
+		smf_set_terminate(obj, -1);
+		return;
+	}
+
+	o->transition_bits |= STATE_A_ENTRY_BIT;
+}
+
+static void state_a_run(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test State A run failed");
+
+	o->transition_bits |= STATE_A_RUN_BIT;
+
+	/* Return to parent run state */
+}
+
+static void state_a_exit(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test State A exit failed");
+	o->transition_bits |= STATE_A_EXIT_BIT;
+}
+
+static void state_b_entry(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test State B entry failed");
+	o->transition_bits |= STATE_B_ENTRY_BIT;
+}
+
+static void state_b_run(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test State B run failed");
+
+	if (o->terminate == RUN) {
+		smf_set_terminate(obj, -1);
+		return;
+	}
+
+	o->transition_bits |= STATE_B_RUN_BIT;
+
+	smf_set_state(SMF_CTX(obj), &test_states[STATE_C]);
+}
+
+static void state_b_exit(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test State B exit failed");
+	o->transition_bits |= STATE_B_EXIT_BIT;
+}
+
+static void state_c_entry(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test State C entry failed");
+	o->transition_bits |= STATE_C_ENTRY_BIT;
+}
+
+static void state_c_run(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test State C run failed");
+
+	if (o->first_time) {
+		o->first_time = false;
+		o->transition_bits |= STATE_C_1ST_RUN_BIT;
+		smf_set_handled(SMF_CTX(obj));
+	} else {
+		/* Do nothing, Let parent handle it */
+		o->transition_bits |= STATE_C_2ND_RUN_BIT;
+	}
+}
+
+static void state_c_exit(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+
+	zassert_equal(o->transition_bits, test_value[o->tv_idx],
+		      "Test State C exit failed");
+
+	if (o->terminate == EXIT) {
+		smf_set_terminate(obj, -1);
+		return;
+	}
+
+	o->transition_bits |= STATE_C_EXIT_BIT;
+}
+
+static void state_d_entry(void *obj)
+{
+	struct test_object *o = TEST_OBJECT(obj);
+
+	o->tv_idx++;
+}
+
+static void state_d_run(void *obj)
+{
+	/* Do nothing */
+}
+
+static void state_d_exit(void *obj)
+{
+	/* Do nothing */
+}
+
+static const struct smf_state test_states[] = {
+	[PARENT_AB] = SMF_CREATE_STATE(parent_ab_entry, parent_ab_run,
+				       parent_ab_exit, NULL, &test_states[STATE_A]),
+	[PARENT_C] = SMF_CREATE_STATE(parent_c_entry, parent_c_run,
+				      parent_c_exit, NULL, &test_states[STATE_C]),
+	[STATE_A] = SMF_CREATE_STATE(state_a_entry, state_a_run, state_a_exit,
+				     &test_states[PARENT_AB], NULL),
+	[STATE_B] = SMF_CREATE_STATE(state_b_entry, state_b_run, state_b_exit,
+				     &test_states[PARENT_AB], NULL),
+	[STATE_C] = SMF_CREATE_STATE(state_c_entry, state_c_run, state_c_exit,
+				     &test_states[PARENT_C], NULL),
+	[STATE_D] = SMF_CREATE_STATE(state_d_entry, state_d_run, state_d_exit,
+				     NULL, NULL),
+};
+
+ZTEST(smf_tests, test_smf_initial_transitions)
+{
+	/* A) Test state transitions */
+
+	test_obj.transition_bits = 0;
+	test_obj.first_time = 1;
+	test_obj.terminate = NONE;
+	smf_set_initial((struct smf_ctx *)&test_obj, &test_states[PARENT_AB]);
+
+	for (int i = 0; i < SMF_RUN; i++) {
+		if (smf_run_state((struct smf_ctx *)&test_obj) < 0) {
+			break;
+		}
+	}
+
+	zassert_equal(TEST_VALUE_NUM, test_obj.tv_idx,
+		      "Incorrect test value index");
+	zassert_equal(test_obj.transition_bits, test_value[test_obj.tv_idx],
+		      "Final state not reached");
+
+	/* B) Test termination in parent entry action */
+
+	test_obj.transition_bits = 0;
+	test_obj.first_time = 1;
+	test_obj.terminate = PARENT_ENTRY;
+	smf_set_initial((struct smf_ctx *)&test_obj, &test_states[PARENT_AB]);
+
+	for (int i = 0; i < SMF_RUN; i++) {
+		if (smf_run_state((struct smf_ctx *)&test_obj) < 0) {
+			break;
+		}
+	}
+
+	zassert_equal(TEST_PARENT_ENTRY_VALUE_NUM, test_obj.tv_idx,
+		      "Incorrect test value index for parent entry termination");
+	zassert_equal(test_obj.transition_bits, test_value[test_obj.tv_idx],
+		      "Final parent entry termination state not reached");
+
+	/* C) Test termination in parent run action */
+
+	test_obj.transition_bits = 0;
+	test_obj.first_time = 1;
+	test_obj.terminate = PARENT_RUN;
+	smf_set_initial((struct smf_ctx *)&test_obj, &test_states[PARENT_AB]);
+
+	for (int i = 0; i < SMF_RUN; i++) {
+		if (smf_run_state((struct smf_ctx *)&test_obj) < 0) {
+			break;
+		}
+	}
+
+	zassert_equal(TEST_PARENT_RUN_VALUE_NUM, test_obj.tv_idx,
+		      "Incorrect test value index for parent run termination");
+	zassert_equal(test_obj.transition_bits, test_value[test_obj.tv_idx],
+		      "Final parent run termination state not reached");
+
+	/* D) Test termination in parent exit action */
+
+	test_obj.transition_bits = 0;
+	test_obj.first_time = 1;
+	test_obj.terminate = PARENT_EXIT;
+	smf_set_initial((struct smf_ctx *)&test_obj, &test_states[PARENT_AB]);
+
+	for (int i = 0; i < SMF_RUN; i++) {
+		if (smf_run_state((struct smf_ctx *)&test_obj) < 0) {
+			break;
+		}
+	}
+
+	zassert_equal(TEST_PARENT_EXIT_VALUE_NUM, test_obj.tv_idx,
+		      "Incorrect test value index for parent exit termination");
+	zassert_equal(test_obj.transition_bits, test_value[test_obj.tv_idx],
+		      "Final parent exit termination state not reached");
+
+	/* E) Test termination in child entry action */
+
+	test_obj.transition_bits = 0;
+	test_obj.first_time = 1;
+	test_obj.terminate = ENTRY;
+	smf_set_initial((struct smf_ctx *)&test_obj, &test_states[PARENT_AB]);
+
+	for (int i = 0; i < SMF_RUN; i++) {
+		if (smf_run_state((struct smf_ctx *)&test_obj) < 0) {
+			break;
+		}
+	}
+
+	zassert_equal(TEST_ENTRY_VALUE_NUM, test_obj.tv_idx,
+		      "Incorrect test value index for entry termination");
+	zassert_equal(test_obj.transition_bits, test_value[test_obj.tv_idx],
+		      "Final entry termination state not reached");
+
+	/* F) Test termination in child run action */
+
+	test_obj.transition_bits = 0;
+	test_obj.first_time = 1;
+	test_obj.terminate = RUN;
+	smf_set_initial((struct smf_ctx *)&test_obj, &test_states[PARENT_AB]);
+
+	for (int i = 0; i < SMF_RUN; i++) {
+		if (smf_run_state((struct smf_ctx *)&test_obj) < 0) {
+			break;
+		}
+	}
+
+	zassert_equal(TEST_RUN_VALUE_NUM, test_obj.tv_idx,
+		      "Incorrect test value index for run termination");
+	zassert_equal(test_obj.transition_bits, test_value[test_obj.tv_idx],
+		      "Final run termination state not reached");
+
+	/* G) Test termination in child exit action */
+
+	test_obj.transition_bits = 0;
+	test_obj.first_time = 1;
+	test_obj.terminate = EXIT;
+	smf_set_initial((struct smf_ctx *)&test_obj, &test_states[PARENT_AB]);
+
+	for (int i = 0; i < SMF_RUN; i++) {
+		if (smf_run_state((struct smf_ctx *)&test_obj) < 0) {
+			break;
+		}
+	}
+
+	zassert_equal(TEST_EXIT_VALUE_NUM, test_obj.tv_idx,
+		      "Incorrect test value index for exit termination");
+	zassert_equal(test_obj.transition_bits, test_value[test_obj.tv_idx],
+		      "Final exit termination state not reached");
+}
diff --git a/tests/lib/smf/src/test_lib_smf.h b/tests/lib/smf/src/test_lib_smf.h
index 998e332..6296352 100644
--- a/tests/lib/smf/src/test_lib_smf.h
+++ b/tests/lib/smf/src/test_lib_smf.h
@@ -10,5 +10,6 @@
 void test_smf_flat(void);
 void test_smf_hierarchical(void);
 void test_smf_hierarchical_5_ancestors(void);
+void test_smf_initial_transitions(void);
 
 #endif /* ZEPHYR_TEST_LIB_SMF_H_ */
diff --git a/tests/lib/smf/testcase.yaml b/tests/lib/smf/testcase.yaml
index 761f86e..fff0bf0 100644
--- a/tests/lib/smf/testcase.yaml
+++ b/tests/lib/smf/testcase.yaml
@@ -8,3 +8,7 @@
   libraries.smf.hierarchical:
     extra_configs:
       - CONFIG_SMF_ANCESTOR_SUPPORT=y
+  libraries.smf.initial_transition:
+    extra_configs:
+      - CONFIG_SMF_ANCESTOR_SUPPORT=y
+      - CONFIG_SMF_INITIAL_TRANSITION=y