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