1 /*
2 * Copyright (c) 2024 Glenn Andrews
3 * State Machine example copyright (c) Miro Samek
4 *
5 * Implementation of the statechart in Figure 2.11 of
6 * Practical UML Statecharts in C/C++, 2nd Edition by Miro Samek
7 * https://www.state-machine.com/psicc2
8 * Used with permission of the author.
9 *
10 * SPDX-License-Identifier: Apache-2.0
11 */
12
13 #include <zephyr/kernel.h>
14 #include <zephyr/smf.h>
15 #include <stdbool.h>
16 #include "hsm_psicc2_thread.h"
17 #include <zephyr/logging/log.h>
18
19 LOG_MODULE_REGISTER(hsm_psicc2_thread);
20
21 /* User defined object */
22 struct s_object {
23 /* This must be first */
24 struct smf_ctx ctx;
25
26 /* Other state specific data add here */
27 struct hsm_psicc2_event event;
28 int foo;
29 } s_obj;
30
31 /* Declaration of possible states */
32 enum demo_states {
33 STATE_INITIAL,
34 STATE_S,
35 STATE_S1,
36 STATE_S2,
37 STATE_S11,
38 STATE_S21,
39 STATE_S211,
40 };
41
42 /* Forward declaration of state table */
43 static const struct smf_state demo_states[];
44
45 /********* STATE_INITIAL *********/
initial_entry(void * o)46 static void initial_entry(void *o)
47 {
48 LOG_INF("%s", __func__);
49 struct s_object *obj = (struct s_object *)o;
50
51 obj->foo = false;
52 }
53
initial_run(void * o)54 static void initial_run(void *o)
55 {
56 LOG_INF("%s", __func__);
57 }
58
initial_exit(void * o)59 static void initial_exit(void *o)
60 {
61 LOG_INF("%s", __func__);
62 }
63
64 /********* STATE_S *********/
s_entry(void * o)65 static void s_entry(void *o)
66 {
67 LOG_INF("%s", __func__);
68 }
69
s_run(void * o)70 static void s_run(void *o)
71 {
72 LOG_INF("%s", __func__);
73 struct s_object *obj = (struct s_object *)o;
74
75 switch (obj->event.event_id) {
76 case EVENT_E:
77 LOG_INF("%s received EVENT_E", __func__);
78 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S11]);
79 break;
80 case EVENT_I:
81 if (obj->foo) {
82 LOG_INF("%s received EVENT_I and set foo false", __func__);
83 obj->foo = false;
84 } else {
85 LOG_INF("%s received EVENT_I and did nothing", __func__);
86 }
87 smf_set_handled(SMF_CTX(obj));
88 break;
89 case EVENT_TERMINATE:
90 LOG_INF("%s received SMF_EVENT_TERMINATE. Terminating", __func__);
91 smf_set_terminate(SMF_CTX(obj), -1);
92 }
93 }
94
s_exit(void * o)95 static void s_exit(void *o)
96 {
97 LOG_INF("%s", __func__);
98 }
99
100 /********* STATE_S1 *********/
s1_entry(void * o)101 static void s1_entry(void *o)
102 {
103 LOG_INF("%s", __func__);
104 }
105
s1_run(void * o)106 static void s1_run(void *o)
107 {
108 LOG_INF("%s", __func__);
109 struct s_object *obj = (struct s_object *)o;
110
111 switch (obj->event.event_id) {
112 case EVENT_A:
113 LOG_INF("%s received EVENT_A", __func__);
114 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S1]);
115 break;
116 case EVENT_B:
117 LOG_INF("%s received EVENT_B", __func__);
118 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S11]);
119 break;
120 case EVENT_C:
121 LOG_INF("%s received EVENT_C", __func__);
122 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S2]);
123 break;
124 case EVENT_D:
125 if (!obj->foo) {
126 LOG_INF("%s received EVENT_D and acted on it", __func__);
127 obj->foo = true;
128 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S]);
129 } else {
130 LOG_INF("%s received EVENT_D and ignored it", __func__);
131 }
132 break;
133 case EVENT_F:
134 LOG_INF("%s received EVENT_F", __func__);
135 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S211]);
136 break;
137 case EVENT_I:
138 LOG_INF("%s received EVENT_I", __func__);
139 smf_set_handled(SMF_CTX(obj));
140 break;
141 }
142 }
143
s1_exit(void * o)144 static void s1_exit(void *o)
145 {
146 LOG_INF("%s", __func__);
147 }
148
149 /********* STATE_S11 *********/
s11_entry(void * o)150 static void s11_entry(void *o)
151 {
152 LOG_INF("%s", __func__);
153 }
154
s11_run(void * o)155 static void s11_run(void *o)
156 {
157 LOG_INF("%s", __func__);
158 struct s_object *obj = (struct s_object *)o;
159
160 switch (obj->event.event_id) {
161 case EVENT_D:
162 if (obj->foo) {
163 LOG_INF("%s received EVENT_D and acted upon it", __func__);
164 obj->foo = false;
165 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S1]);
166 } else {
167 LOG_INF("%s received EVENT_D and ignored it", __func__);
168 }
169 break;
170 case EVENT_G:
171 LOG_INF("%s received EVENT_G", __func__);
172 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S21]);
173 break;
174 case EVENT_H:
175 LOG_INF("%s received EVENT_H", __func__);
176 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S]);
177 break;
178 }
179 }
180
s11_exit(void * o)181 static void s11_exit(void *o)
182 {
183 LOG_INF("%s", __func__);
184 }
185
186 /********* STATE_S2 *********/
s2_entry(void * o)187 static void s2_entry(void *o)
188 {
189 LOG_INF("%s", __func__);
190 }
191
s2_run(void * o)192 static void s2_run(void *o)
193 {
194 LOG_INF("%s", __func__);
195 struct s_object *obj = (struct s_object *)o;
196
197 switch (obj->event.event_id) {
198 case EVENT_C:
199 LOG_INF("%s received EVENT_C", __func__);
200 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S1]);
201 break;
202 case EVENT_F:
203 LOG_INF("%s received EVENT_F", __func__);
204 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S11]);
205 break;
206 case EVENT_I:
207 if (!obj->foo) {
208 LOG_INF("%s received EVENT_I and set foo true", __func__);
209 obj->foo = true;
210 smf_set_handled(SMF_CTX(obj));
211 } else {
212 LOG_INF("%s received EVENT_I and did nothing", __func__);
213 }
214 break;
215 }
216 }
217
s2_exit(void * o)218 static void s2_exit(void *o)
219 {
220 LOG_INF("%s", __func__);
221 }
222
223 /********* STATE_S21 *********/
s21_entry(void * o)224 static void s21_entry(void *o)
225 {
226 LOG_INF("%s", __func__);
227 }
228
s21_run(void * o)229 static void s21_run(void *o)
230 {
231 LOG_INF("%s", __func__);
232 struct s_object *obj = (struct s_object *)o;
233
234 switch (obj->event.event_id) {
235 case EVENT_A:
236 LOG_INF("%s received EVENT_A", __func__);
237 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S21]);
238 break;
239 case EVENT_B:
240 LOG_INF("%s received EVENT_B", __func__);
241 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S211]);
242 break;
243 case EVENT_G:
244 LOG_INF("%s received EVENT_G", __func__);
245 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S1]);
246 break;
247 }
248 }
249
s21_exit(void * o)250 static void s21_exit(void *o)
251 {
252 LOG_INF("%s", __func__);
253 }
254
255 /********* STATE_S211 *********/
s211_entry(void * o)256 static void s211_entry(void *o)
257 {
258 LOG_INF("%s", __func__);
259 }
260
s211_run(void * o)261 static void s211_run(void *o)
262 {
263 LOG_INF("%s", __func__);
264 struct s_object *obj = (struct s_object *)o;
265
266 switch (obj->event.event_id) {
267 case EVENT_D:
268 LOG_INF("%s received EVENT_D", __func__);
269 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S21]);
270 break;
271 case EVENT_H:
272 LOG_INF("%s received EVENT_H", __func__);
273 smf_set_state(SMF_CTX(obj), &demo_states[STATE_S]);
274 break;
275 }
276 }
277
s211_exit(void * o)278 static void s211_exit(void *o)
279 {
280 LOG_INF("%s", __func__);
281 }
282
283 /* State storage: handler functions, parent states and initial transition states */
284 static const struct smf_state demo_states[] = {
285 [STATE_INITIAL] = SMF_CREATE_STATE(initial_entry, initial_run, initial_exit, NULL,
286 &demo_states[STATE_S2]),
287 [STATE_S] = SMF_CREATE_STATE(s_entry, s_run, s_exit, &demo_states[STATE_INITIAL],
288 &demo_states[STATE_S11]),
289 [STATE_S1] = SMF_CREATE_STATE(s1_entry, s1_run, s1_exit, &demo_states[STATE_S],
290 &demo_states[STATE_S11]),
291 [STATE_S2] = SMF_CREATE_STATE(s2_entry, s2_run, s2_exit, &demo_states[STATE_S],
292 &demo_states[STATE_S211]),
293 [STATE_S11] = SMF_CREATE_STATE(s11_entry, s11_run, s11_exit, &demo_states[STATE_S1], NULL),
294 [STATE_S21] = SMF_CREATE_STATE(s21_entry, s21_run, s21_exit, &demo_states[STATE_S2],
295 &demo_states[STATE_S211]),
296 [STATE_S211] =
297 SMF_CREATE_STATE(s211_entry, s211_run, s211_exit, &demo_states[STATE_S21], NULL),
298 };
299
300 K_THREAD_STACK_DEFINE(hsm_psicc2_thread_stack, HSM_PSICC2_THREAD_STACK_SIZE);
301 K_MSGQ_DEFINE(hsm_psicc2_msgq, sizeof(struct hsm_psicc2_event), HSM_PSICC2_THREAD_EVENT_QUEUE_SIZE,
302 1);
303
304 static struct k_thread hsm_psicc2_thread_data;
305 static k_tid_t hsm_psicc2_thread_tid;
306
hsm_psicc2_thread(void * arg1,void * arg2,void * arg3)307 static void hsm_psicc2_thread(void *arg1, void *arg2, void *arg3)
308 {
309 smf_set_initial(SMF_CTX(&s_obj), &demo_states[STATE_INITIAL]);
310 while (1) {
311 int rc = k_msgq_get(&hsm_psicc2_msgq, &s_obj.event, K_FOREVER);
312
313 if (rc == 0) {
314 /* Run state machine with given message */
315 rc = smf_run_state(SMF_CTX(&s_obj));
316
317 if (rc) {
318 /* State machine terminates if a non-zero value is returned */
319 LOG_INF("%s terminating state machine thread", __func__);
320 break;
321 }
322 } else {
323 LOG_ERR("Waiting for event failed, code %d", rc);
324 }
325 }
326 }
327
hsm_psicc2_thread_run(void)328 void hsm_psicc2_thread_run(void)
329 {
330 hsm_psicc2_thread_tid =
331 k_thread_create(&hsm_psicc2_thread_data, hsm_psicc2_thread_stack,
332 K_THREAD_STACK_SIZEOF(hsm_psicc2_thread_stack), hsm_psicc2_thread,
333 NULL, NULL, NULL, HSM_PSICC2_THREAD_PRIORITY, 0, K_NO_WAIT);
334
335 k_thread_name_set(hsm_psicc2_thread_tid, "psicc2_thread");
336 }
337
