From bb8a927b64a2bed4835a6cf12ebf71eb29487bd9 Mon Sep 17 00:00:00 2001 From: Dirk Helbig Date: Wed, 31 Jan 2024 10:40:29 +0100 Subject: [PATCH] core: add fsm/hsm implementation --- src/btstack_fsm.c | 89 ++++++++++++++++++++++++ src/btstack_fsm.h | 127 ++++++++++++++++++++++++++++++++++ src/btstack_hsm.c | 169 ++++++++++++++++++++++++++++++++++++++++++++++ src/btstack_hsm.h | 138 +++++++++++++++++++++++++++++++++++++ 4 files changed, 523 insertions(+) create mode 100644 src/btstack_fsm.c create mode 100644 src/btstack_fsm.h create mode 100644 src/btstack_hsm.c create mode 100644 src/btstack_hsm.h diff --git a/src/btstack_fsm.c b/src/btstack_fsm.c new file mode 100644 index 000000000..dcf01adbc --- /dev/null +++ b/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 + +#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 ); +} + diff --git a/src/btstack_fsm.h b/src/btstack_fsm.h new file mode 100644 index 000000000..e896c0899 --- /dev/null +++ b/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 + +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 + diff --git a/src/btstack_hsm.c b/src/btstack_hsm.c new file mode 100644 index 000000000..11b9ae25e --- /dev/null +++ b/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 +#include +#include + +#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; +} + diff --git a/src/btstack_hsm.h b/src/btstack_hsm.h new file mode 100644 index 000000000..cbc97ed11 --- /dev/null +++ b/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 + +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