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