misc/fsm/fsm.c

   1 #include "fsm.h"
   2
   3 #include "states.h"
   4 #include "types.h"
   5
   6 #include <sys/queue.h>
   7 #include <stdio.h>
   8 #include <stdlib.h>
   9 #include <string.h>
  10 #include <pthread.h>
  11
  12 /* Callback funtions prototypes */
  13 static unsigned int fsm_hashf(const void *key);
  14 static int fsm_cmpf(const void *arg1, const void *arg2);
  15 static void fsm_printf(const void *key, const void *data);
  16 static void fsm_pq_lock(const fsm_t *fsm);
  17 static void fsm_pq_unlock(const fsm_t *fsm);
  18
  19 fsmret_t fsm_init(fsm_t **fsm, size_t size, unsigned int factor, unsigned int nqueues)
  20 {
  21     unsigned int i;
  22
  23     /* FIXME: Validate input  */
  24
  25     /* Allocate memory for fsm data structure */
  26     if ((*fsm = malloc(sizeof **fsm)) == NULL)
  27         return FSM_ENOMEM;
  28
  29     /* Allocate memory for fsm's states' hash table */
  30     if (((*fsm)->sttable = malloc(sizeof *(*fsm)->sttable)) == NULL) {
  31         free(*fsm);
  32         return FSM_ENOMEM;
  33     }
  34
  35     /* Allocate memory for priority queues */
  36     if (((*fsm)->pqtable = malloc(nqueues * sizeof *(*fsm)->pqtable)) == NULL) {
  37         free((*fsm)->sttable);
  38         free(*fsm);
  39         return FSM_ENOMEM;
  40     }
  41
  42     /* Allocate memory for "mutex object" -- machine dependent code */
  43     if (((*fsm)->mobj = malloc(sizeof(pthread_mutex_t))) == NULL) {
  44         free((*fsm)->mobj);
  45         free((*fsm)->sttable);
  46         free(*fsm);
  47         return FSM_ENOMEM;
  48     }
  49     /* Machine dependent code */
  50     pthread_mutex_init((pthread_mutex_t *)(*fsm)->mobj, NULL);
  51
  52     /* Initialize queues */
  53     (*fsm)->nqueues = nqueues;
  54     for (i = 0; i < nqueues; i++)
  55         STAILQ_INIT(&(*fsm)->pqtable[i]);
  56
  57     /* Initialize states' hash table */
  58     if (htable_init((*fsm)->sttable, size, factor,
  59                     fsm_hashf, fsm_cmpf, fsm_printf) == HT_NOMEM) {
  60         free((*fsm)->pqtable);
  61         free((*fsm)->sttable);
  62         free(*fsm);
  63         return FSM_ENOMEM;
  64     }
  65
  66     return FSM_OK;
  67 }
  68
  69 fsmret_t fsm_add_state(fsm_t *fsm, unsigned int key, state_t *state)
  70 {
  71     /* There is no need to allocate memory for state's key,
  72        since this is done in state_init() */
  73
  74     *state->st_key = key;
  75
  76     /* Insert state to hash table */
  77     if (htable_insert(fsm->sttable, state->st_key, state) == HT_EXISTS)
  78         return FSM_EEXISTS;
  79
  80     return FSM_OK;
  81 }
  82
  83 fsmret_t fsm_free(fsm_t *fsm)
  84 {
  85     pqhead_t *phead;
  86     pqnode_t *pnode;
  87     const hnode_t *pstnode;
  88     unsigned int i, stpos;
  89
  90     /* Free states' table */
  91     pstnode = NULL;
  92     stpos = 0;
  93     while ((pstnode = htable_get_next_elm(fsm->sttable, &stpos, pstnode)) != NULL)
  94         state_free(pstnode->hn_data);
  95
  96     /* Shallow free */
  97     htable_free_all_obj(fsm->sttable, HT_FREEKEY);
  98     htable_free(fsm->sttable);
  99     free(fsm->sttable);
 100
 101     /* Free queues' elements */
 102     for (i = 0; i < fsm->nqueues; i++) {
 103         phead = &fsm->pqtable[i];
 104         while (STAILQ_FIRST(phead) != NULL) {
 105             pnode = STAILQ_FIRST(phead);
 106             STAILQ_REMOVE_HEAD(phead, pq_next);
 107             free(pnode->data);
 108             free(pnode);
 109         }
 110     }
 111
 112     free(fsm->pqtable);
 113     free(fsm->mobj);
 114     free(fsm);
 115
 116     return FSM_OK;
 117 }
 118
 119 void fsm_print_states(const fsm_t *fsm)
 120 {
 121     htable_print(fsm->sttable);
 122 }
 123
 124 fsmret_t fsm_set_state(fsm_t *fsm, unsigned int stkey)
 125 {
 126     state_t *state;
 127
 128     /* Does this state exist in states' hash table ? */
 129     if ((state = htable_search(fsm->sttable, &stkey)) == NULL)
 130         return FSM_ENOTFOUND;
 131
 132     /* Set fsm to new state */
 133     fsm->cstate = state;
 134
 135     return FSM_OK;
 136 }
 137
 138 unsigned int fsm_get_current_state(const fsm_t *fsm)
 139 {
 140     return *fsm->cstate->st_key;
 141 }
 142
 143 fsmret_t fsm_queue_event(fsm_t *fsm, unsigned int evtkey, void *data, size_t size, unsigned int prio)
 144 {
 145     pqhead_t *phead;
 146     pqnode_t *pnode;
 147
 148     if (prio >= fsm->nqueues)
 149         return FSM_EPRIO;
 150
 151     /* Allocate memory for new pending event */
 152     if ((pnode = malloc(sizeof *pnode)) == NULL)
 153         return FSM_ENOMEM;
 154
 155     pnode->evtkey = evtkey;
 156     pnode->prio = prio;
 157
 158     /*
 159      * Allocate memory for data and copy them over.
 160      * Note that this strategy leads to memory fragmentation,
 161      * and should be addressed with a custom memory allocator,
 162      * in due time.
 163     */
 164     if ((pnode->data = malloc(size)) == NULL) {
 165         free(pnode);
 166         return FSM_ENOMEM;
 167     }
 168     memcpy(pnode->data, data, size);
 169
 170     /* Get the head of the queue with the appropriate priority */
 171     phead = &fsm->pqtable[prio];
 172
 173     /* Insert new event in tail (we serve from head) */
 174     fsm_pq_lock(fsm);
 175     STAILQ_INSERT_TAIL(phead, pnode, pq_next);
 176     fsm_pq_unlock(fsm);
 177
 178     return FSM_OK;
 179 }
 180 fsmret_t fsm_dequeue_event(fsm_t *fsm)
 181 {
 182     pqhead_t *phead;
 183     pqnode_t *pnode;
 184     unsigned int i;
 185
 186     /* Scan queues starting from the one with the biggest priority */
 187     i = fsm->nqueues - 1;
 188     do {
 189         phead = &fsm->pqtable[i];
 190         if ((pnode = STAILQ_FIRST(phead)) != NULL) {
 191             if (fsm_process_event(fsm, pnode->evtkey, pnode->data) == FSM_ENOTFOUND) {
 192                 /*
 193                  * FIXME: The event should stay in queue, if it has
 194                  * a sticky bit. But we haven't implemented such a bitmap
 195                  * in event's structure yet
 196                  */
 197             }
 198
 199             /* Delete event */
 200             pnode = STAILQ_FIRST(phead);
 201             STAILQ_REMOVE_HEAD(phead, pq_next);
 202             free(pnode->data);    /* Argh, memory fragmentation */
 203             free(pnode);
 204             return FSM_OK;
 205         }
 206     } while (i-- != 0);
 207
 208     return FSM_EMPTY;
 209 }
 210
 211 size_t fsm_get_queued_events(const fsm_t *fsm)
 212 {
 213     const pqhead_t *phead;
 214     const pqnode_t *pnode;
 215     size_t i, total;
 216
 217     total = 0;
 218     for (i = 0; i < fsm->nqueues; i++) {
 219         phead = &fsm->pqtable[i];
 220         STAILQ_FOREACH(pnode, phead, pq_next)
 221             total++;
 222     }
 223
 224     return total;
 225 }
 226
 227 fsmret_t fsm_process_event(fsm_t *fsm, unsigned int evtkey, void *data)
 228 {
 229     event_t *event;
 230
 231     /* Can the current state handle the incoming event ? */
 232     if ((event = htable_search(fsm->cstate->evttable, &evtkey)) == NULL)
 233         return FSM_ENOTFOUND;
 234
 235     /* Execute appropriate action */
 236     if (event->evt_actionf != NULL)
 237         event->evt_actionf(data);
 238
 239     /* Is the transition made to an existent state ? */
 240     if ((event->evt_newstate == NULL)
 241         || (htable_search(fsm->sttable, event->evt_newstate->st_key) == NULL))
 242             return FSM_ENOTFOUND;
 243
 244     /* Set new state */
 245     fsm->cstate = event->evt_newstate;
 246
 247     return FSM_OK;
 248 }
 249
 250 fsmret_t fsm_validate(const fsm_t *fsm)
 251 {
 252     /* Is FSM empty of states ? */
 253     if (htable_get_used(fsm->sttable) == 0)
 254         return FSM_EMPTY;
 255
 256     return FSM_CLEAN;
 257 }
 258
 259 void fsm_export_to_dot(const fsm_t *fsm, FILE *fp)
 260 {
 261     const hnode_t *pstnode;
 262     const hnode_t *pevtnode;
 263     unsigned int stpos;
 264     unsigned int evtpos;
 265
 266     fprintf(fp, "digraph {\n");
 267
 268     /* Traverse all states of FSM */
 269     pstnode = NULL;
 270     stpos = 0;
 271     while ((pstnode = htable_get_next_elm(fsm->sttable, &stpos, pstnode)) != NULL) {
 272
 273         /* Traverse all events associated with the current state */
 274         pevtnode = NULL;
 275         evtpos = 0;
 276         while ((pevtnode = htable_get_next_elm(((state_t *)(pstnode->hn_data))->evttable, &evtpos, pevtnode)) != NULL) {
 277             printf("S%u -> S%u [label=\"E%u\"]\n",
 278                    *(unsigned int *)pstnode->hn_key,
 279                    *(unsigned int *)(((event_t *)pevtnode->hn_data)->evt_newstate->st_key),
 280                    *(unsigned int *)pevtnode->hn_key);
 281         }
 282     }
 283
 284     fprintf(fp, "}\n");
 285 }
 286
 287
 288 /* Callback funtions */
 289 static unsigned int fsm_hashf(const void *key)
 290 {
 291     return *(const unsigned int *)key;
 292 }
 293
 294 static int fsm_cmpf(const void *arg1, const void *arg2)
 295 {
 296     unsigned int a = *(const unsigned int *)arg1;
 297     unsigned int b = *(const unsigned int *)arg2;
 298
 299     if (a > b)
 300         return -1;
 301     else if (a == b)
 302         return 0;
 303     else
 304         return 1;
 305 }
 306
 307 static void fsm_printf(const void *key, const void *data)
 308 {
 309     printf("key: %u ", *(const unsigned int *)key);
 310 }
 311
 312 static void fsm_pq_lock(const fsm_t *fsm)
 313 {
 314     /* Machine dependent code */
 315     pthread_mutex_lock((pthread_mutex_t *) fsm->mobj);
 316 }
 317
 318 static void fsm_pq_unlock(const fsm_t *fsm)
 319 {
 320     /* Machine dependent code */
 321     pthread_mutex_unlock((pthread_mutex_t *) fsm->mobj);
 322 }