core: add fsm/hsm implementation

src/btstack_fsm.c

@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2024 BlueKitchen GmbH
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holders nor the names of
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ * 4. Any redistribution, use, or modification is done solely for
+ *    personal benefit and not for any commercial purpose or for
+ *    monetary gain.
+ *
+ * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
+ * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
+ * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * Please inquire about commercial licensing options at
+ * contact@bluekitchen-gmbh.com
+ *
+ */
+
+/**
+ * @title Finite State Machine (FSM)
+ *
+ */
+#define BTSTACK_FILE__ "btstack_fsm.c"
+
+#include <stddef.h>
+
+#include "btstack_fsm.h"
+
+#include "btstack_config.h"
+#include "btstack_debug.h"
+
+static btstack_fsm_event_t const entry_evt = { BTSTACK_FSM_ENTRY_SIG };
+static btstack_fsm_event_t const exit_evt = { BTSTACK_FSM_EXIT_SIG };
+
+btstack_fsm_state_t btstack_fsm_transit(btstack_fsm_t *me, btstack_fsm_state_handler_t target) {
+    me->state = target;
+    return BTSTACK_FSM_TRAN_STATUS;
+}
+
+void btstack_fsm_constructor(btstack_fsm_t * const me, btstack_fsm_state_handler_t initial) {
+    me->state = initial;
+}
+
+void btstack_fsm_init(btstack_fsm_t * const me, btstack_fsm_event_t const * const e) {
+    btstack_assert(me->state != NULL);
+    me->state(me, e);
+    me->state(me, &entry_evt);
+}
+
+btstack_fsm_state_t btstack_fsm_dispatch(btstack_fsm_t * const me, btstack_fsm_event_t const * const e) {
+    btstack_fsm_state_t status;
+    btstack_fsm_state_handler_t prev_state = me->state;
+    btstack_assert( me->state != NULL );
+
+    status = me->state(me, e);
+
+    if( status == BTSTACK_FSM_TRAN_STATUS ) {
+        prev_state(me, &exit_evt);
+        me->state(me, &entry_evt);
+    }
+    return status;
+}
+
+void btstack_fsm_dispatch_until(btstack_fsm_t * const me, btstack_fsm_event_t const * const e) {
+    btstack_fsm_state_t status;
+    do {
+        status = btstack_fsm_dispatch( me, e );
+    } while( status == BTSTACK_FSM_TRAN_STATUS );
+}
+

src/btstack_fsm.h

@@ -0,0 +1,127 @@
+/*
+ * Copyright (C) 2024 BlueKitchen GmbH
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holders nor the names of
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ * 4. Any redistribution, use, or modification is done solely for
+ *    personal benefit and not for any commercial purpose or for
+ *    monetary gain.
+ *
+ * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
+ * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
+ * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * Please inquire about commercial licensing options at
+ * contact@bluekitchen-gmbh.com
+ *
+ */
+
+/**
+ * @title Finite State Machine (FSM)
+ *
+ */
+
+#ifndef BTSTACK_FSM_H
+#define BTSTACK_FSM_H
+
+#if defined __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+typedef uint16_t btstack_fsm_signal_t;
+enum btstack_fsm_reserved_signals_e {
+    BTSTACK_FSM_EMPTY_SIG,
+    BTSTACK_FSM_INIT_SIG,
+    BTSTACK_FSM_ENTRY_SIG,
+    BTSTACK_FSM_EXIT_SIG,
+    BTSTACK_FSM_USER_SIG
+};
+
+typedef struct {
+    btstack_fsm_signal_t sig;
+} btstack_fsm_event_t;
+
+typedef struct btstack_fsm_s btstack_fsm_t;
+
+typedef enum {
+    BTSTACK_FSM_TRAN_STATUS,
+    BTSTACK_FSM_SUPER_STATUS,
+    BTSTACK_FSM_HANDLED_STATUS,
+    BTSTACK_FSM_UNHANDLED_STATUS,
+    BTSTACK_FSM_IGNORED_STATUS
+} btstack_fsm_state_t;
+
+typedef btstack_fsm_state_t (*btstack_fsm_state_handler_t)(btstack_fsm_t * const me, btstack_fsm_event_t const * const e);
+
+struct btstack_fsm_s {
+    btstack_fsm_state_handler_t state;
+};
+
+/* API_START */
+
+/*
+ * @brief Request the transition from the current state to the given new state
+ * @param me the current state machine
+ * @param target the new state to transit to
+ * @result transition status
+ */
+btstack_fsm_state_t btstack_fsm_transit(btstack_fsm_t * const me, btstack_fsm_state_handler_t target);
+
+/*
+ * @brief Constructs a new state hierarchical machine machine, with storage for maximum hierarchy depth.
+ * @param me the current state machine
+ * @param initial the initial state
+ */
+void btstack_fsm_constructor(btstack_fsm_t * const me, btstack_fsm_state_handler_t initial);
+
+/*
+ * @brief Takes the initial transition of the state machine and sending it a BTSTACK_HSM_INIT_SIG
+ * @param me the current state machine
+ * @param e event
+ */
+void btstack_fsm_init(btstack_fsm_t * const me, btstack_fsm_event_t const * const e);
+
+/*
+ * @brief Dispatches the given event to the state machine, if a transition is requested, leave the old states and enter the new on.
+ *        Handling entering/exiting all states on the way.
+ * @param me the current state machine
+ * @param e event
+ */
+btstack_fsm_state_t btstack_fsm_dispatch(btstack_fsm_t * const me, btstack_fsm_event_t const * const e);
+
+/*
+ * @brief Dispatches the given event to the state machine until it was handled.
+ * @param me the current state machine
+ * @param e event
+ */
+void btstack_fsm_dispatch_until(btstack_fsm_t * const me, btstack_fsm_event_t const * const e);
+
+/* API_END */
+
+#if defined __cplusplus
+}
+#endif
+
+#endif
+

src/btstack_hsm.c

@@ -0,0 +1,169 @@
+/*
+ * Copyright (C) 2024 BlueKitchen GmbH
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holders nor the names of
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ * 4. Any redistribution, use, or modification is done solely for
+ *    personal benefit and not for any commercial purpose or for
+ *    monetary gain.
+ *
+ * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
+ * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
+ * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * Please inquire about commercial licensing options at
+ * contact@bluekitchen-gmbh.com
+ *
+ */
+#define BTSTACK_FILE__ "btstack_hsm.c"
+
+#include <stddef.h>
+#include <string.h>
+#include <stdbool.h>
+
+#include "btstack_config.h"
+#include "btstack_debug.h"
+
+#include "btstack_hsm.h"
+
+btstack_hsm_state_t btstack_hsm_transit(btstack_hsm_t * const me, btstack_hsm_state_handler_t const target) {
+    me->temp = target;
+    return BTSTACK_HSM_TRAN_STATUS;
+}
+
+btstack_hsm_state_t btstack_hsm_super(btstack_hsm_t * const me, btstack_hsm_state_handler_t const target) {
+    me->temp = target;
+    return BTSTACK_HSM_SUPER_STATUS;
+}
+
+btstack_hsm_state_t btstack_hsm_top(btstack_hsm_t * const me, btstack_hsm_event_t const * const e) {
+    UNUSED(me);
+    UNUSED(e);
+    return BTSTACK_HSM_IGNORED_STATUS;
+}
+
+void btstack_hsm_constructor(btstack_hsm_t * const me, btstack_hsm_state_handler_t initial, btstack_hsm_state_handler_t path[], int8_t depth) {
+    me->state = btstack_hsm_top;
+    me->temp = initial;
+    me->path = path;
+    me->depth = depth;
+}
+
+static btstack_hsm_state_t btstack_hsm_get_super( btstack_hsm_t * const me, btstack_hsm_state_handler_t const handler) {
+    // empty event to trigger default state action, a.k. the super state
+    static btstack_hsm_event_t const empty_evt = { BTSTACK_HSM_EMPTY_SIG };
+    return handler( me, &empty_evt );
+}
+
+static btstack_hsm_event_t const entry_evt = { BTSTACK_HSM_ENTRY_SIG };
+static btstack_hsm_event_t const exit_evt = { BTSTACK_HSM_EXIT_SIG };
+
+void btstack_hsm_init(btstack_hsm_t * const me, btstack_hsm_event_t const * const e) {
+    btstack_assert(me->state != NULL);
+    btstack_assert(me->temp != NULL);
+    btstack_hsm_state_handler_t target = me->state;
+    btstack_hsm_state_t status = me->temp(me, e);
+    btstack_assert( status == BTSTACK_HSM_TRAN_STATUS );
+    static btstack_hsm_event_t const init_evt = { BTSTACK_HSM_INIT_SIG };
+
+    int_fast8_t level;
+    btstack_hsm_state_handler_t *root_path = me->path;
+    memset(root_path, 0, sizeof(btstack_hsm_state_handler_t)*me->depth);
+
+    do {
+        level = 0;
+        btstack_hsm_state_handler_t current = me->temp;
+        for(; current != target; current=me->temp, level++ ) {
+            root_path[level] = current;
+            btstack_hsm_get_super( me, current );
+        }
+        for(; level>0;) {
+            root_path[--level]( me, &entry_evt );
+        }
+        target = root_path[0];
+        status = target( me, &init_evt );
+    } while (status == BTSTACK_HSM_TRAN_STATUS);
+    btstack_assert( status != BTSTACK_HSM_TRAN_STATUS );
+    me->state = target;
+}
+
+static void btstack_hsm_handler_super_cache( btstack_hsm_t * const me, btstack_hsm_state_handler_t cache[], int idx, btstack_hsm_state_handler_t handler ) {
+    if( cache[idx] == NULL ) {
+        cache[idx] = handler;
+        btstack_hsm_get_super(me, handler);
+    } else {
+        me->temp = cache[idx];
+    }
+}
+
+btstack_hsm_state_t btstack_hsm_dispatch(btstack_hsm_t * const me, btstack_hsm_event_t const * const e) {
+    btstack_hsm_state_t status;
+    btstack_hsm_state_handler_t current = me->state;
+    do {
+        status = current(me, e);
+        // if the state doesn't handle the event try at the super state too
+        if( status == BTSTACK_HSM_UNHANDLED_STATUS ) {
+            status = btstack_hsm_get_super( me, current );
+        }
+        current = me->temp;
+    } while( status == BTSTACK_HSM_SUPER_STATUS );
+
+    // if we don't switch states we are done now
+    if( status != BTSTACK_HSM_TRAN_STATUS ) {
+        return status;
+    }
+    btstack_hsm_state_handler_t source = me->state;
+    btstack_hsm_state_handler_t target = me->temp;
+    btstack_hsm_state_handler_t *root_path = me->path;
+    memset(root_path, 0, sizeof(btstack_hsm_state_handler_t)*me->depth);
+
+    // why should that be possible/necessary?
+    btstack_assert( source != target );
+
+    // handle entry/exit edges
+    int_fast8_t level = 0;
+    bool lca_found = false;
+    // calculates the lowest common ancestor of the state graph
+    for(; source != btstack_hsm_top; source=me->temp) {
+        level = 0;
+        for(current=target; current != btstack_hsm_top; current=me->temp, ++level ) {
+            if( current == source ) {
+                lca_found = true;
+                break;
+            }
+
+            btstack_hsm_handler_super_cache( me, root_path, level, current );
+        }
+        if( lca_found == true ) {
+            break;
+        }
+        source( me, &exit_evt );
+        btstack_hsm_get_super( me, source );
+    }
+
+    for(level--; level > 0; ) {
+        root_path[--level]( me, &entry_evt );
+    }
+    me->state = target;
+    return status;
+}
+

src/btstack_hsm.h

@@ -0,0 +1,138 @@
+/*
+ * Copyright (C) 2024 BlueKitchen GmbH
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holders nor the names of
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ * 4. Any redistribution, use, or modification is done solely for
+ *    personal benefit and not for any commercial purpose or for
+ *    monetary gain.
+ *
+ * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
+ * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
+ * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * Please inquire about commercial licensing options at
+ * contact@bluekitchen-gmbh.com
+ *
+ */
+
+/**
+ * @title Hierarchical State Machine (HSM)
+ *
+ */
+
+#ifndef BTSTACK_HSM_H
+#define BTSTACK_HSM_H
+
+#if defined __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+typedef uint16_t btstack_hsm_signal_t;
+enum btstack_hsm_reserved_signals_e {
+    BTSTACK_HSM_EMPTY_SIG,
+    BTSTACK_HSM_INIT_SIG,
+    BTSTACK_HSM_ENTRY_SIG,
+    BTSTACK_HSM_EXIT_SIG,
+    BTSTACK_HSM_USER_SIG
+};
+
+typedef struct {
+    btstack_hsm_signal_t sig;
+} btstack_hsm_event_t;
+typedef struct btstack_hsm_s btstack_hsm_t;
+typedef enum {
+    BTSTACK_HSM_TRAN_STATUS,
+    BTSTACK_HSM_SUPER_STATUS,
+    BTSTACK_HSM_HANDLED_STATUS,
+    BTSTACK_HSM_UNHANDLED_STATUS,
+    BTSTACK_HSM_IGNORED_STATUS
+} btstack_hsm_state_t;
+
+typedef btstack_hsm_state_t (*btstack_hsm_state_handler_t)(btstack_hsm_t * const me, btstack_hsm_event_t const * const e);
+
+struct btstack_hsm_s {
+    btstack_hsm_state_handler_t state;
+    btstack_hsm_state_handler_t temp;
+    btstack_hsm_state_handler_t *path;
+    int_fast8_t depth;
+};
+
+/* API_START */
+
+/*
+ * @brief Request the transition from the current state to the given new state
+ * @param me the current state machine
+ * @param target the new state to transit to
+ * @result transition status
+ */
+btstack_hsm_state_t btstack_hsm_transit(btstack_hsm_t * const me, btstack_hsm_state_handler_t const target);
+
+/*
+ * @brief Specifies the upper state in a state hierarchy
+ * @param me the current state machine
+ * @param target the next parent state in the hierarchy
+ * @result transition status
+ */
+btstack_hsm_state_t btstack_hsm_super(btstack_hsm_t * const me, btstack_hsm_state_handler_t const target);
+
+/*
+ * @brief Placeholder state to mark the top most state in a state hierarchy, the root state. Ignores all events.
+ * @param me the current state machine
+ * @param e event
+ * @result ignored status
+ */
+btstack_hsm_state_t btstack_hsm_top(btstack_hsm_t * const me, btstack_hsm_event_t const * const e);
+
+/*
+ * @brief Constructs a new state hierarchical machine machine, with storage for maximum hierarchy depth.
+ * @param me the current state machine
+ * @param initial the initial state
+ * @param array of btstack_hsm_state_handler_t elements with the same number of elements as the maximum number of nested state machines.
+ * @param The number of nested state machines.
+ */
+void btstack_hsm_constructor(btstack_hsm_t * const me, btstack_hsm_state_handler_t initial, btstack_hsm_state_handler_t path[], int8_t depth);
+
+/*
+ * @brief Takes the initial transition of the state machine and sending it a BTSTACK_HSM_INIT_SIG
+ * @param me the current state machine
+ * @param e event
+ */
+void btstack_hsm_init(btstack_hsm_t * const me, btstack_hsm_event_t const * const e);
+
+/*
+ * @brief Dispatches the given event to the state machine, if a transition is requested, leave the old states and enter the new on.
+ *        Honoring the hierarchy and handling entering/exiting all states on the way.
+ * @param me the current state machine
+ * @param e event
+ */
+btstack_hsm_state_t btstack_hsm_dispatch(btstack_hsm_t * const me, btstack_hsm_event_t const * const e);
+
+/* API_END */
+
+#if defined __cplusplus
+}
+#endif
+
+#endif