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