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 *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(fsm->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 *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(htable_iterator_get_data(sit));
  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 *pstate;
 123
 124     /* Does this state exist in states' hash table ? */
 125     if ((pstate = htable_search(fsm->sttable, &stkey)) == NULL)
 126         return FSM_ENOTFOUND;
 127
 128     /*
 129      * Mark state as reachable
 130      *
 131      * XXX: Do we need to call fsm_mark_reachable_states() ?
 132      * By doing so, we guarantee that fsm's states's flags
 133      * are always uptodate.
 134      */
 135     STATE_MARK_AS_REACHABLE(pstate);
 136     fsm_mark_reachable_states(fsm);
 137
 138     /* Set fsm to new state */
 139     fsm->cstate = pstate;
 140
 141     return FSM_OK;
 142 }
 143
 144 unsigned int fsm_get_current_state(const fsm_t *fsm)
 145 {
 146     return *fsm->cstate->st_key;
 147 }
 148
 149 fsmret_t fsm_queue_event(fsm_t *fsm, unsigned int evtkey,
 150                          void *data, size_t size, unsigned int prio)
 151 {
 152     pqhead_t *phead;
 153     pqnode_t *pnode;
 154
 155     if (prio >= fsm->nqueues)
 156         return FSM_EPRIO;
 157
 158     /* Allocate memory for new pending event */
 159     if ((pnode = malloc(sizeof *pnode)) == NULL)
 160         return FSM_ENOMEM;
 161
 162     pnode->evtkey = evtkey;
 163     pnode->prio = prio;
 164
 165     /*
 166      * Allocate memory for data and copy them over.
 167      * Note that this strategy leads to memory fragmentation,
 168      * and should be addressed with a custom memory allocator,
 169      * in due time.
 170     */
 171     if ((pnode->data = malloc(size)) == NULL) {
 172         free(pnode);
 173         return FSM_ENOMEM;
 174     }
 175     memcpy(pnode->data, data, size);
 176
 177     /* Get the head of the queue with the appropriate priority */
 178     phead = &fsm->pqtable[prio];
 179
 180     /* Insert new event in tail (we serve from head) */
 181     fsm_pq_lock(fsm);
 182     STAILQ_INSERT_TAIL(phead, pnode, pq_next);
 183     fsm_pq_unlock(fsm);
 184
 185     return FSM_OK;
 186 }
 187 fsmret_t fsm_dequeue_event(fsm_t *fsm)
 188 {
 189     pqhead_t *phead;
 190     pqnode_t *pnode;
 191     unsigned int i;
 192
 193     /* Scan queues starting from the one with the biggest priority */
 194     i = fsm->nqueues - 1;
 195     do {
 196         phead = &fsm->pqtable[i];
 197         if ((pnode = STAILQ_FIRST(phead)) != NULL) {
 198             if (fsm_process_event(fsm, pnode->evtkey, pnode->data) == FSM_ENOTFOUND) {
 199                 /*
 200                  * FIXME: The event should stay in queue, if it has
 201                  * a sticky bit. But we haven't implemented such a bitmap
 202                  * in event's structure yet
 203                  */
 204             }
 205
 206             /* Delete event */
 207             pnode = STAILQ_FIRST(phead);
 208             STAILQ_REMOVE_HEAD(phead, pq_next);
 209             free(pnode->data);    /* Argh, memory fragmentation */
 210             free(pnode);
 211             return FSM_OK;
 212         }
 213     } while (i-- != 0);
 214
 215     return FSM_EMPTY;
 216 }
 217
 218 size_t fsm_get_queued_events(const fsm_t *fsm)
 219 {
 220     const pqhead_t *phead;
 221     const pqnode_t *pnode;
 222     size_t i, total;
 223
 224     total = 0;
 225     for (i = 0; i < fsm->nqueues; i++) {
 226         phead = &fsm->pqtable[i];
 227         STAILQ_FOREACH(pnode, phead, pq_next)
 228             total++;
 229     }
 230
 231     return total;
 232 }
 233
 234 fsmret_t fsm_process_event(fsm_t *fsm, unsigned int evtkey, void *data)
 235 {
 236     event_t *pevt;
 237
 238     /* Can the current state handle the incoming event ? */
 239     if ((pevt = htable_search(fsm->cstate->evttable, &evtkey)) == NULL)
 240         return FSM_ENOTFOUND;
 241
 242     /* Execute appropriate action */
 243     if (pevt->evt_actionf != NULL)
 244         pevt->evt_actionf(data);
 245
 246     /* Is the transition made to an existent state ? */
 247     if ((pevt->evt_newstate == NULL)
 248         || (htable_search(fsm->sttable, pevt->evt_newstate->st_key) == NULL))
 249             return FSM_ENOTFOUND;
 250
 251     /* Set new state */
 252     fsm->cstate = pevt->evt_newstate;
 253
 254     return FSM_OK;
 255 }
 256
 257 fsmret_t fsm_validate(const fsm_t *fsm)
 258 {
 259     /* Is FSM empty of states ? */
 260     if (htable_get_used(fsm->sttable) == 0)
 261         return FSM_EMPTY;
 262
 263     return FSM_CLEAN;
 264 }
 265
 266 void fsm_export_to_dot(const fsm_t *fsm, FILE *fp)
 267 {
 268     const state_t *pstate;
 269     const event_t *pevt;
 270     htable_iterator_t sit;    /* state iterator */
 271     htable_iterator_t eit;    /* event iterator */
 272
 273     fprintf(fp, "digraph {\n");
 274
 275     /* Traverse all states of FSM */
 276     htable_iterator_init(&sit);
 277     while ((sit.pnode = htable_get_next_elm(fsm->sttable, &sit)) != NULL) {
 278         /* Traverse all events associated with the current state */
 279         pstate = htable_iterator_get_data(sit);
 280         htable_iterator_init(&eit);
 281         while ((eit.pnode = htable_get_next_elm(pstate->evttable, &eit)) != NULL) {
 282             pevt = htable_iterator_get_data(eit);
 283             printf("S%u -> S%u [label=\"E%u\"]\n",
 284                    *(unsigned int *) htable_iterator_get_key(sit),
 285                    *(unsigned int *) (pevt->evt_newstate->st_key),
 286                    *(unsigned int *) htable_iterator_get_key(eit));
 287         }
 288     }
 289
 290     fprintf(fp, "}\n");
 291 }
 292
 293 void fsm_print_states(const fsm_t *fsm, FILE *fp)
 294 {
 295     const state_t *pstate;
 296     htable_iterator_t sit;    /* states iterator */
 297
 298     /* Traverse all states of FSM */
 299     htable_iterator_init(&sit);
 300     while ((sit.pnode = htable_get_next_elm(fsm->sttable, &sit)) != NULL) {
 301         pstate = htable_iterator_get_data(sit);
 302         fprintf(fp, "state [key = %u, reachable = %c]\n",
 303                 *(unsigned int *)(pstate->st_key),
 304                 STATE_IS_REACHABLE(pstate) ? 'T' : 'F');
 305         state_print_evts(pstate, fp);
 306     }
 307 }
 308
 309 void fsm_mark_reachable_states(fsm_t *fsm)
 310 {
 311     const state_t *pstate;
 312     const event_t *pevt;
 313     htable_iterator_t sit;    /* states iterator */
 314     htable_iterator_t eit;    /* event iterator */
 315
 316     /* For all states */
 317     htable_iterator_init(&sit);
 318     while ((sit.pnode = htable_get_next_elm(fsm->sttable, &sit)) != NULL) {
 319         pstate = htable_iterator_get_data(sit);
 320         htable_iterator_init(&eit);
 321
 322         /*
 323          * We mark a state as reachable, if and only if
 324          * there exist transitions to this state, from other
 325          * _reachable_ states.
 326          */
 327         while ((eit.pnode = htable_get_next_elm(pstate->evttable, &eit)) != NULL) {
 328             pevt = eit.pnode->hn_data;
 329             if (STATE_IS_REACHABLE(pstate))
 330                 STATE_MARK_AS_REACHABLE(pevt->evt_newstate);
 331         }
 332     }
 333 }
 334
 335 void fsm_minimize(fsm_t *fsm)
 336 {
 337     const state_t *pstate;
 338     htable_iterator_t sit;    /* states iterator */
 339
 340     /* Remove unreachable states */
 341     htable_iterator_init(&sit);
 342     while ((sit.pnode = htable_get_next_elm(fsm->sttable, &sit)) != NULL) {
 343         pstate = sit.pnode->hn_data;
 344     }
 345 }
 346
 347 /* Callback funtions */
 348 static unsigned int fsm_hashf(const void *key)
 349 {
 350     return *(const unsigned int *)key;
 351 }
 352
 353 static int fsm_cmpf(const void *arg1, const void *arg2)
 354 {
 355     unsigned int a = *(const unsigned int *)arg1;
 356     unsigned int b = *(const unsigned int *)arg2;
 357
 358     if (a > b)
 359         return -1;
 360     else if (a == b)
 361         return 0;
 362     else
 363         return 1;
 364 }
 365
 366 static void fsm_printf(const void *key, const void *data)
 367 {
 368     printf("key: %u ", *(const unsigned int *)key);
 369 }
 370
 371 static void fsm_pq_lock(const fsm_t *fsm)
 372 {
 373     /* Machine dependent code */
 374     pthread_mutex_lock((pthread_mutex_t *) fsm->mobj);
 375 }
 376
 377 static void fsm_pq_unlock(const fsm_t *fsm)
 378 {
 379     /* Machine dependent code */
 380     pthread_mutex_unlock((pthread_mutex_t *) fsm->mobj);
 381 }
 382