qep_hsm.c

Go to the documentation of this file.

/*============================================================================
* QP/C Real-Time Embedded Framework (RTEF)
* Copyright (C) 2005 Quantum Leaps, LLC. All rights reserved.
*
* SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-QL-commercial
*
* This software is dual-licensed under the terms of the open source GNU
* General Public License version 3 (or any later version), or alternatively,
* under the terms of one of the closed source Quantum Leaps commercial
* licenses.
*
* The terms of the open source GNU General Public License version 3
* can be found at: <www.gnu.org/licenses/gpl-3.0>
*
* The terms of the closed source Quantum Leaps commercial licenses
* can be found at: <www.state-machine.com/licensing>
*
* Redistributions in source code must retain this top-level comment block.
* Plagiarizing this software to sidestep the license obligations is illegal.
*
* Contact information:
* <www.state-machine.com>
* <info@state-machine.com>
============================================================================*/
#define QP_IMPL           /* this is QP implementation */
#include "qep_port.h"     /* QEP port */
#include "qassert.h"      /* QP embedded systems-friendly assertions */
#ifdef Q_SPY              /* QS software tracing enabled? */
    #include "qs_port.h"  /* QS port */
    #include "qs_pkg.h"   /* QS facilities for pre-defined trace records */
#else
    #include "qs_dummy.h" /* disable the QS software tracing */
#endif /* Q_SPY */
 
Q_DEFINE_THIS_MODULE("qep_hsm")
 
/*==========================================================================*/
char const QP_versionStr[7] = QP_VERSION_STR;
 
/*==========================================================================*/
enum {
    QEP_EMPTY_SIG_ = 0, 
    QHSM_MAX_NEST_DEPTH_ = 6
};
 
static QEvt const QEP_reservedEvt_[] = {
    { (QSignal)QEP_EMPTY_SIG_, 0U, 0U },
    { (QSignal)Q_ENTRY_SIG,    0U, 0U },
    { (QSignal)Q_EXIT_SIG,     0U, 0U },
    { (QSignal)Q_INIT_SIG,     0U, 0U }
};
 
static inline QState QHsm_trig_(QHsm * const me,
                                QStateHandler const state, QSignal sig)
{
    return (*state)(me, &QEP_reservedEvt_[sig]);
}
 
#ifdef Q_SPY
    #define QEP_EXIT_(state_, qs_id_) do {                  \
        if ((*(state_))(me, &QEP_reservedEvt_[Q_EXIT_SIG])  \
             == (QState)Q_RET_HANDLED) {                    \
            QS_BEGIN_PRE_(QS_QEP_STATE_EXIT, (qs_id_))      \
                QS_OBJ_PRE_(me);                            \
                QS_FUN_PRE_(state_);                        \
            QS_END_PRE_()                                   \
        }                                                   \
    } while (false)
 
    #define QEP_ENTER_(state_, qs_id_) do {                 \
        if ((*(state_))(me, &QEP_reservedEvt_[Q_ENTRY_SIG]) \
             == (QState)Q_RET_HANDLED) {                    \
            QS_BEGIN_PRE_(QS_QEP_STATE_ENTRY, (qs_id_))     \
                QS_OBJ_PRE_(me);                            \
                QS_FUN_PRE_(state_);                        \
            QS_END_PRE_()                                   \
        }                                                   \
    } while (false)
 
    static int_fast8_t QHsm_tran_(QHsm * const me,
                                  QStateHandler path[QHSM_MAX_NEST_DEPTH_],
                                  uint_fast8_t const qs_id);
#else
    #define QEP_EXIT_(state_, dummy_) \
        ((void)(*(state_))(me, &QEP_reservedEvt_[Q_EXIT_SIG]))
 
    #define QEP_ENTER_(state_, dummy_) \
        ((void)(*(state_))(me, &QEP_reservedEvt_[Q_ENTRY_SIG]))
 
    static int_fast8_t QHsm_tran_(QHsm * const me,
                                  QStateHandler path[QHSM_MAX_NEST_DEPTH_]);
#endif /* Q_SPY */
 
/*==========================================================================*/
void QHsm_ctor(QHsm * const me, QStateHandler initial) {
    static struct QHsmVtable const vtable = { /* QHsm virtual table */
        &QHsm_init_,
        &QHsm_dispatch_
#ifdef Q_SPY
        ,&QHsm_getStateHandler_
#endif
    };
    me->vptr      = &vtable;
    me->state.fun = Q_STATE_CAST(&QHsm_top);
    me->temp.fun  = initial;
}
 
/*==========================================================================*/
#ifdef Q_SPY
void QHsm_init_(QHsm * const me, void const * const e,
                uint_fast8_t const qs_id)
#else
void QHsm_init_(QHsm * const me, void const * const e)
#endif
{
    QStateHandler t = me->state.fun;
 
    Q_REQUIRE_ID(200, (me->vptr != (struct QHsmVtable *)0)
                      && (me->temp.fun != Q_STATE_CAST(0))
                      && (t == Q_STATE_CAST(&QHsm_top)));
 
    /* execute the top-most initial tran. */
    QState r = (*me->temp.fun)(me, Q_EVT_CAST(QEvt));
 
    /* the top-most initial transition must be taken */
    Q_ASSERT_ID(210, r == (QState)Q_RET_TRAN);
 
    QS_CRIT_STAT_
    QS_BEGIN_PRE_(QS_QEP_STATE_INIT, qs_id)
        QS_OBJ_PRE_(me);           /* this state machine object */
        QS_FUN_PRE_(t);            /* the source state */
        QS_FUN_PRE_(me->temp.fun); /* the target of the initial transition */
    QS_END_PRE_()
 
    /* drill down into the state hierarchy with initial transitions... */
    do {
        QStateHandler path[QHSM_MAX_NEST_DEPTH_]; /* tran entry path array */
        int_fast8_t ip = 0; /* tran entry path index */
 
        path[0] = me->temp.fun;
        (void)QHsm_trig_(me, me->temp.fun, QEP_EMPTY_SIG_);
        while (me->temp.fun != t) {
            ++ip;
            Q_ASSERT_ID(220, ip < QHSM_MAX_NEST_DEPTH_);
            path[ip] = me->temp.fun;
            (void)QHsm_trig_(me, me->temp.fun, QEP_EMPTY_SIG_);
        }
        me->temp.fun = path[0];
 
        /* nested initial transition, drill into the target hierarchy... */
        do {
            QEP_ENTER_(path[ip], qs_id); /* enter path[ip] */
            --ip;
        } while (ip >= 0);
 
        t = path[0]; /* current state becomes the new source */
 
        r = QHsm_trig_(me, t, Q_INIT_SIG); /* execute initial transition */
 
#ifdef Q_SPY
        if (r == (QState)Q_RET_TRAN) {
            QS_BEGIN_PRE_(QS_QEP_STATE_INIT, qs_id)
                QS_OBJ_PRE_(me);           /* this state machine object */
                QS_FUN_PRE_(t);            /* the source state */
                QS_FUN_PRE_(me->temp.fun); /* target of the initial tran. */
            QS_END_PRE_()
        }
#endif /* Q_SPY */
 
    } while (r == (QState)Q_RET_TRAN);
 
    QS_BEGIN_PRE_(QS_QEP_INIT_TRAN, qs_id)
        QS_TIME_PRE_();    /* time stamp */
        QS_OBJ_PRE_(me);   /* this state machine object */
        QS_FUN_PRE_(t);    /* the new active state */
    QS_END_PRE_()
 
    me->state.fun = t; /* change the current active state */
    me->temp.fun  = t; /* mark the configuration as stable */
}
 
/*==========================================================================*/
QState QHsm_top(void const * const me, QEvt const * const e) {
    (void)me; /* suppress the "unused parameter" compiler warning */
    (void)e;  /* suppress the "unused parameter" compiler warning */
    return (QState)Q_RET_IGNORED; /* the top state ignores all events */
}
 
/*==========================================================================*/
#ifdef Q_SPY
void QHsm_dispatch_(QHsm * const me, QEvt const * const e,
                    uint_fast8_t const qs_id)
#else
void QHsm_dispatch_(QHsm * const me, QEvt const * const e)
#endif
{
    QStateHandler t = me->state.fun;
    QS_CRIT_STAT_
 
    Q_REQUIRE_ID(400, (t != Q_STATE_CAST(0))
                       && (t == me->temp.fun));
 
    QS_BEGIN_PRE_(QS_QEP_DISPATCH, qs_id)
        QS_TIME_PRE_();         /* time stamp */
        QS_SIG_PRE_(e->sig);    /* the signal of the event */
        QS_OBJ_PRE_(me);        /* this state machine object */
        QS_FUN_PRE_(t);         /* the current state */
    QS_END_PRE_()
 
    QStateHandler s;
    QState r;
    /* process the event hierarchically... */
    do {
        s = me->temp.fun;
        r = (*s)(me, e); /* invoke state handler s */
 
        if (r == (QState)Q_RET_UNHANDLED) { /* unhandled due to a guard? */
 
            QS_BEGIN_PRE_(QS_QEP_UNHANDLED, qs_id)
                QS_SIG_PRE_(e->sig); /* the signal of the event */
                QS_OBJ_PRE_(me);     /* this state machine object */
                QS_FUN_PRE_(s);      /* the current state */
            QS_END_PRE_()
 
            r = QHsm_trig_(me, s, QEP_EMPTY_SIG_); /* find superstate of s */
        }
    } while (r == (QState)Q_RET_SUPER);
 
    /* regular transition taken? */
    if (r >= (QState)Q_RET_TRAN) {
        QStateHandler path[QHSM_MAX_NEST_DEPTH_];
 
        path[0] = me->temp.fun; /* save the target of the transition */
        path[1] = t;
        path[2] = s;
 
        /* exit current state to transition source s... */
        for (; t != s; t = me->temp.fun) {
            if (QHsm_trig_(me, t, Q_EXIT_SIG) == (QState)Q_RET_HANDLED) {
                QS_BEGIN_PRE_(QS_QEP_STATE_EXIT, qs_id)
                    QS_OBJ_PRE_(me);  /* this state machine object */
                    QS_FUN_PRE_(t);   /* the exited state */
                QS_END_PRE_()
 
                /* find superstate of t */
                (void)QHsm_trig_(me, t, QEP_EMPTY_SIG_);
            }
        }
 
        int_fast8_t ip;
 
#ifdef Q_SPY
        ip = QHsm_tran_(me, path, qs_id);
        if (r == (QState)Q_RET_TRAN_HIST) {
            QS_BEGIN_PRE_(QS_QEP_TRAN_HIST, qs_id)
                QS_OBJ_PRE_(me);      /* this state machine object */
                QS_FUN_PRE_(t);       /* the source of the transition */
                QS_FUN_PRE_(path[0]); /* the target of the tran. to history */
            QS_END_PRE_()
        }
#else
        ip = QHsm_tran_(me, path);
#endif /* Q_SPY */
 
        /* execute state entry actions in the desired order... */
        for (; ip >= 0; --ip) {
            QEP_ENTER_(path[ip], qs_id);  /* enter path[ip] */
        }
 
        t = path[0];      /* stick the target into register */
        me->temp.fun = t; /* update the next state */
 
        /* while nested initial transition... */
        while (QHsm_trig_(me, t, Q_INIT_SIG) == (QState)Q_RET_TRAN) {
 
            QS_BEGIN_PRE_(QS_QEP_STATE_INIT, qs_id)
                QS_OBJ_PRE_(me);           /* this state machine object */
                QS_FUN_PRE_(t);            /* the source (pseudo)state */
                QS_FUN_PRE_(me->temp.fun); /* the target of the transition */
            QS_END_PRE_()
 
            ip = 0;
            path[0] = me->temp.fun;
 
            /* find superstate */
            (void)QHsm_trig_(me, me->temp.fun, QEP_EMPTY_SIG_);
 
            while (me->temp.fun != t) {
                ++ip;
                path[ip] = me->temp.fun;
                /* find superstate */
                (void)QHsm_trig_(me, me->temp.fun, QEP_EMPTY_SIG_);
            }
            me->temp.fun = path[0];
 
            /* entry path must not overflow */
            Q_ASSERT_ID(410, ip < QHSM_MAX_NEST_DEPTH_);
 
            /* retrace the entry path in reverse (correct) order... */
            do {
                QEP_ENTER_(path[ip], qs_id); /* enter path[ip] */
                --ip;
            } while (ip >= 0);
 
            t = path[0]; /* current state becomes the new source */
        }
 
        QS_BEGIN_PRE_(QS_QEP_TRAN, qs_id)
            QS_TIME_PRE_();          /* time stamp */
            QS_SIG_PRE_(e->sig);     /* the signal of the event */
            QS_OBJ_PRE_(me);         /* this state machine object */
            QS_FUN_PRE_(s);          /* the source of the transition */
            QS_FUN_PRE_(t);          /* the new active state */
        QS_END_PRE_()
    }
 
#ifdef Q_SPY
    else if (r == (QState)Q_RET_HANDLED) {
 
        QS_BEGIN_PRE_(QS_QEP_INTERN_TRAN, qs_id)
            QS_TIME_PRE_();          /* time stamp */
            QS_SIG_PRE_(e->sig);     /* the signal of the event */
            QS_OBJ_PRE_(me);         /* this state machine object */
            QS_FUN_PRE_(s);          /* the source state */
        QS_END_PRE_()
 
    }
    else {
 
        QS_BEGIN_PRE_(QS_QEP_IGNORED, qs_id)
            QS_TIME_PRE_();          /* time stamp */
            QS_SIG_PRE_(e->sig);     /* the signal of the event */
            QS_OBJ_PRE_(me);         /* this state machine object */
            QS_FUN_PRE_(me->state.fun); /* the current state */
        QS_END_PRE_()
 
    }
#endif /* Q_SPY */
 
    me->state.fun = t; /* change the current active state */
    me->temp.fun  = t; /* mark the configuration as stable */
}
 
/*==========================================================================*/
#ifdef Q_SPY
static int_fast8_t QHsm_tran_(QHsm * const me,
                              QStateHandler path[QHSM_MAX_NEST_DEPTH_],
                              uint_fast8_t const qs_id)
#else
static int_fast8_t QHsm_tran_(QHsm * const me,
                              QStateHandler path[QHSM_MAX_NEST_DEPTH_])
#endif
{
    int_fast8_t ip = -1; /* transition entry path index */
    QStateHandler t = path[0];
    QStateHandler const s = path[2];
    QS_CRIT_STAT_
 
    /* (a) check source==target (transition to self)... */
    if (s == t) {
        QEP_EXIT_(s, qs_id); /* exit the source */
        ip = 0; /* enter the target */
    }
    else {
        /* find superstate of target */
        (void)QHsm_trig_(me, t, QEP_EMPTY_SIG_);
 
        t = me->temp.fun;
 
        /* (b) check source==target->super... */
        if (s == t) {
            ip = 0; /* enter the target */
        }
        else {
            /* find superstate of src */
            (void)QHsm_trig_(me, s, QEP_EMPTY_SIG_);
 
            /* (c) check source->super==target->super... */
            if (me->temp.fun == t) {
                QEP_EXIT_(s, qs_id); /* exit the source */
                ip = 0; /* enter the target */
            }
            else {
                /* (d) check source->super==target... */
                if (me->temp.fun == path[0]) {
                    QEP_EXIT_(s, qs_id); /* exit the source */
                }
                else {
                    /* (e) check rest of source==target->super->super..
                    * and store the entry path along the way
                    */
                    int_fast8_t iq = 0; /* indicate that LCA not found */
                    ip = 1; /* enter target and its superstate */
                    path[1] = t;      /* save the superstate of target */
                    t = me->temp.fun; /* save source->super */
 
                    /* find target->super->super... */
                    QState r = QHsm_trig_(me, path[1], QEP_EMPTY_SIG_);
                    while (r == (QState)Q_RET_SUPER) {
                        ++ip;
                        path[ip] = me->temp.fun; /* store the entry path */
                        if (me->temp.fun == s) { /* is it the source? */
                            iq = 1; /* indicate that LCA found */
 
                            /* entry path must not overflow */
                            Q_ASSERT_ID(510,
                                ip < QHSM_MAX_NEST_DEPTH_);
                            --ip; /* do not enter the source */
                            r = (QState)Q_RET_HANDLED; /* terminate loop */
                        }
                         /* it is not the source, keep going up */
                        else {
                            r = QHsm_trig_(me, me->temp.fun, QEP_EMPTY_SIG_);
                        }
                    }
 
                    /* the LCA not found yet? */
                    if (iq == 0) {
 
                        /* entry path must not overflow */
                        Q_ASSERT_ID(520, ip < QHSM_MAX_NEST_DEPTH_);
 
                        QEP_EXIT_(s, qs_id); /* exit the source */
 
                        /* (f) check the rest of source->super
                        *                  == target->super->super...
                        */
                        iq = ip;
                        r = (QState)Q_RET_IGNORED; /* LCA NOT found */
                        do {
                            if (t == path[iq]) { /* is this the LCA? */
                                r = (QState)Q_RET_HANDLED; /* LCA found */
                                ip = iq - 1; /* do not enter LCA */
                                iq = -1; /* cause termintion of the loop */
                            }
                            else {
                                --iq; /* try lower superstate of target */
                            }
                        } while (iq >= 0);
 
                        /* LCA not found? */
                        if (r != (QState)Q_RET_HANDLED) {
                            /* (g) check each source->super->...
                            * for each target->super...
                            */
                            r = (QState)Q_RET_IGNORED; /* keep looping */
                            do {
                                /* exit t unhandled? */
                                if (QHsm_trig_(me, t, Q_EXIT_SIG)
                                    == (QState)Q_RET_HANDLED)
                                {
                                    QS_BEGIN_PRE_(QS_QEP_STATE_EXIT, qs_id)
                                        QS_OBJ_PRE_(me);
                                        QS_FUN_PRE_(t);
                                    QS_END_PRE_()
 
                                    (void)QHsm_trig_(me, t, QEP_EMPTY_SIG_);
                                }
                                t = me->temp.fun; /* set to super of t */
                                iq = ip;
                                do {
                                    /* is this LCA? */
                                    if (t == path[iq]) {
                                        /* do not enter LCA */
                                        ip = (int_fast8_t)(iq - 1);
                                        iq = -1; /* break out of inner loop */
                                        /* break out of outer loop */
                                        r = (QState)Q_RET_HANDLED;
                                    }
                                    else {
                                        --iq;
                                    }
                                } while (iq >= 0);
                            } while (r != (QState)Q_RET_HANDLED);
                        }
                    }
                }
            }
        }
    }
    return ip;
}
 
/*==========================================================================*/
#ifdef Q_SPY
QStateHandler QHsm_getStateHandler_(QHsm * const me) {
    return me->state.fun;
}
#endif
 
/*==========================================================================*/
bool QHsm_isIn(QHsm * const me, QStateHandler const state) {
 
    Q_REQUIRE_ID(600, me->temp.fun == me->state.fun);
 
    bool inState = false; /* assume that this HSM is not in 'state' */
 
    /* scan the state hierarchy bottom-up */
    QState r;
    do {
        /* do the states match? */
        if (me->temp.fun == state) {
            inState = true;            /* 'true' means that match found */
            r = (QState)Q_RET_IGNORED; /* break out of the loop */
        }
        else {
            r = QHsm_trig_(me, me->temp.fun, QEP_EMPTY_SIG_);
        }
    } while (r != (QState)Q_RET_IGNORED); /* QHsm_top() state not reached */
    me->temp.fun = me->state.fun; /* restore the stable state configuration */
 
    return inState; /* return the status */
}
 
/*==========================================================================*/
QStateHandler QHsm_childState(QHsm * const me,
                              QStateHandler const parent)
{
    QStateHandler child = me->state.fun; /* start with the current state */
    bool isFound = false; /* start with the child not found */
 
    /* establish stable state configuration */
    me->temp.fun = me->state.fun;
    QState r;
    do {
        /* is this the parent of the current child? */
        if (me->temp.fun == parent) {
            isFound = true; /* child is found */
            r = (QState)Q_RET_IGNORED; /* break out of the loop */
        }
        else {
            child = me->temp.fun;
            r = QHsm_trig_(me, me->temp.fun, QEP_EMPTY_SIG_);
        }
    } while (r != (QState)Q_RET_IGNORED); /* QHsm_top() state not reached */
    me->temp.fun = me->state.fun; /* establish stable state configuration */
 
    Q_ENSURE_ID(810, isFound);
#ifdef Q_NASSERT
    (void)isFound; /* avoid compiler warning about unused variable */
#endif
 
    return child; /* return the child */
}