sics/devser.c

#include <math.h>
#include "ascon.h"
#include "devser.h"

typedef struct DevAction {
  struct DevAction *next;
  void *data;
  DevActionHandler *hdl;
  DevPrio prio;
  DevKillActionData *kill;
} DevAction;

typedef struct SchedHeader {
  struct SchedHeader *next;
  DevAction *actions;           /* list of actions for given interval and prio */
  DevAction *followingAction;
  double interval;
  double timeDue;
  DevPrio prio;
} SchedHeader;

struct DevSer {
  Ascon *asyncConn;             /* connection */
  DevAction *current;
  int killCurrent;
  DevAction *actions;           /* the action queue */
  DevAction *toKill;            /* list of actions to be killed */
  SchedHeader *headers;
  int steps;
};

static char *devPrio[NumberOfPRIO] = {
  "null", "slow", "read", "progress", "write", "halt", "start"
};

char *DevPrio2Text(DevPrio prio)
{
  if (prio <= 0 || prio >= NumberOfPRIO) {
    prio = NullPRIO;
  }
  return devPrio[prio];
}

DevPrio DevText2Prio(char *text)
{
  DevPrio prio;
  for (prio = 0; prio < NumberOfPRIO; prio++) {
    if (strcasecmp(text, devPrio[prio]) == 0)
      return prio;
  }
  return NullPRIO;
}

static void DevFreeActionList(DevAction * actions)
{
  DevAction *victim;
  while (actions != NULL) {
    victim = actions;
    actions = victim->next;
    assert(victim->data != NULL);
    if (victim->kill != NULL)
      victim->kill(victim->data);
    victim->data=NULL;
    free(victim);
  }
}

DevAction *DevNextAction(DevSer * devser)
{
  DevPrio prio;
  double now;
  SchedHeader *header;


  devser->current = NULL;
  if (devser->actions) {
    prio = devser->actions->prio;
  } else {
    prio = NullPRIO;
  }
  now = DoubleTime();
  for (header = devser->headers;
       header != NULL && header->prio > prio; header = header->next) {
    if (header->followingAction == NULL) {
      if (now >= header->timeDue) {
        header->followingAction = header->actions;
        if (header->interval <= 0) {
          header->timeDue = now;
        } else {
          header->timeDue = (floor(now / header->interval) + 1)
              * header->interval;
        }
      } else if (header->prio == StartPRIO && header->actions != NULL) {
        /* special case: poll with StartPRIO pending: block all other actions */
        return devser->current;
      }
    }
    if (header->followingAction != NULL) {
      devser->current = header->followingAction;
      devser->killCurrent = 0;
      header->followingAction = header->followingAction->next;
      return devser->current;
    }
  }
  if (devser->actions) {
    devser->current = devser->actions;
    devser->killCurrent = 1;
    devser->actions = devser->actions->next;
  }
  return devser->current;
}

int DevQueueTask(void *ds)
{
  DevSer *devser = ds;
  AsconStatus status;
  DevAction *action;
  char *sendData;
  char *replyData = NULL;
  
  if (devser->steps == 0)
    return 1;
  
  /* deferred deallocation of removed actions */
  DevFreeActionList(devser->toKill);
  devser->toKill = NULL;
  
  action = devser->current;
  if (action == NULL) {
    action = DevNextAction(devser);
  }

  while (action != NULL) {
    status = AsconTask(devser->asyncConn);
    if (status == AsconFailure) {
      replyData = AsconGetError(devser->asyncConn);
    } else if (status == AsconReady) {
      replyData = AsconRead(devser->asyncConn);
    } else {
      return 1;
    }
    if (devser->steps > 0) {    /* debugging mode */
      devser->steps--;
    }
    sendData = action->hdl(action->data, replyData, (status == AsconFailure));
    if (sendData != NULL) {
      AsconWrite(devser->asyncConn, sendData, 0);
      return 1;
    }
    if (devser->killCurrent) {
      if (action->kill != NULL)
        action->kill(action->data);
      devser->killCurrent = 0;
      free(action);
      devser->current = NULL;
    }
    action = DevNextAction(devser);
  }
  return 1;
}

DevSer *DevMake(SConnection * con, int argc, char *argv[])
{
  DevSer *devser = NULL;
  Ascon *asyncConn = NULL;

  asyncConn = AsconMake(con, argc, argv);
  if (!asyncConn) {
    return NULL;
  }
  devser = calloc(1, sizeof(*devser));
  assert(devser);
  devser->asyncConn = asyncConn;
  devser->current = NULL;
  devser->killCurrent = 0;
  devser->actions = NULL;
  devser->headers = NULL;
  devser->toKill = NULL;
  devser->steps = -1;           /* no debugging by default */
  TaskRegister(pServ->pTasker, DevQueueTask, NULL, NULL, devser, 0);
  return devser;
}

void DevDebugMode(DevSer * devser, int steps)
{
  devser->steps = steps;
}

DevAction *DevNewAction(void *data, DevActionHandler hdl,
                        DevKillActionData * killFunc, DevPrio prio)
{
  DevAction *action;
  action = calloc(1, sizeof(*action));
  assert(action);
  action->data = data;
  action->hdl = hdl;
  action->kill = killFunc;
  action->prio = prio;
  action->next = NULL;
  return action;
}

void DevKill(DevSer * devser)
{
  SchedHeader *h, *victim;

  if (devser->asyncConn) {
    AsconKill(devser->asyncConn);
  }
  DevFreeActionList(devser->actions);
  DevFreeActionList(devser->toKill);
  h = devser->headers;
  while (h != NULL) {
    victim = h;
    h = victim->next;
    DevFreeActionList(victim->actions);
    free(victim);
  }
  if (devser->killCurrent) {
    if (devser->current->kill != NULL) devser->current->kill(devser->current);
    devser->killCurrent = 0;
    free(devser->current);
  }
  TaskRemove(pServ->pTasker, DevQueueTask, devser);
  free(devser);
}

void DevDisconnect(DevSer * devser)
{
  if (devser->asyncConn) {
    AsconDisconnect(devser->asyncConn);
  }
}

int DevQueue(DevSer * devser, void *actionData, DevPrio prio,
             DevActionHandler hdl, DevActionMatch * matchFunc,
             DevKillActionData * killFunc)
{
  DevAction *action, **ptr2Last;
  DevAction *new;

  if (prio <= NullPRIO)
    prio = NullPRIO + 1;
  if (prio >= NumberOfPRIO)
    prio = NumberOfPRIO - 1;
  ptr2Last = &devser->actions;
  for (action = devser->actions; action != NULL && action->prio >= prio;
       action = action->next) {
    if (action->hdl == hdl && matchFunc(actionData, action->data)) {
      return 0;                 /* there is already an identical action */
    }
    ptr2Last = &action->next;
  }
  new = DevNewAction(actionData, hdl, killFunc, prio);
  new->next = action;
  *ptr2Last = new;
  return 1;
}

int DevUnschedule(DevSer * devser, void *actionData,
                  DevActionHandler hdl, DevActionMatch * matchFunc)
{
  SchedHeader *header = NULL;
  DevAction **ptr2Last = NULL;
  DevAction *action = NULL;
  int cnt = 0;

  /* scan through all headers */
  for (header = devser->headers; header != NULL; header = header->next) {
    ptr2Last = &header->actions;
    for (action = header->actions; action != NULL; action = *ptr2Last) {
      if (action->hdl == hdl && matchFunc(actionData, action->data)) {
        if (action == header->followingAction) {
          /* advance followingAction if equal */
          header->followingAction = action->next;
        }
        if (action == devser->current) {
          devser->current = NULL;
          devser->killCurrent = 0;      /* should already be 0 */
        }
        cnt++;
        /* remove from list */
        *ptr2Last = action->next;
        /* add to kill list */
        action->next = devser->toKill;
        devser->toKill = action;
      } else {
        ptr2Last = &action->next;
      }
    }
  }
  return cnt;
}

int DevSchedule(DevSer * devser, void *actionData,
                DevPrio prio, double interval,
                DevActionHandler hdl, DevActionMatch * matchFunc,
                DevKillActionData * killFunc)
{
  SchedHeader *header = NULL;
  SchedHeader **ptr2LastHeader = NULL;
  SchedHeader *newHeader;
  DevAction *action = NULL;
  DevAction **ptr2Last = NULL;
  DevAction *newAction;
  int ret;

  if (prio <= NullPRIO)
    prio = NullPRIO + 1;
  if (prio >= NumberOfPRIO)
    prio = NumberOfPRIO - 1;
  ret = DevUnschedule(devser, actionData, hdl, matchFunc);

  newAction = DevNewAction(actionData, hdl, killFunc, prio);
  /* find matching header */
  ptr2LastHeader = &devser->headers;
  for (header = devser->headers; header != NULL; header = *ptr2LastHeader) {
    if (header->prio == newAction->prio && header->interval == interval) {
      /* append new action at the tail */
      ptr2Last = &header->actions;
      for (action = header->actions; action != NULL; action = action->next) {
        ptr2Last = &action->next;
      }
      *ptr2Last = newAction;
      assert(newAction->next == NULL);
      return ret;
    } else if (header->prio < newAction->prio ||
               (header->prio == newAction->prio
                && header->interval > interval)) {
      break;
    }
    if (header->actions == NULL) {
      /* remove empty header */
      *ptr2LastHeader = header->next;
      free(header);
    } else {
      ptr2LastHeader = &header->next;
    }
  }

  /* insert new header */
  newHeader = calloc(1, sizeof(*newHeader));
  assert(newHeader);
  newHeader->actions = newAction;
  newHeader->followingAction = NULL;
  newHeader->prio = newAction->prio;
  newHeader->interval = interval;
  newHeader->next = header;
  newHeader->timeDue = DoubleTime() + interval;
  *ptr2LastHeader = newHeader;
  return ret;
}

int DevRemoveAction(DevSer * devser, void *actionData)
{
  SchedHeader *header = NULL;
  DevAction **ptr2Last = NULL;
  DevAction *action = NULL; 
  int cnt = 0;
  
  
  /* Remove current action, if matched. If a reply is pending, the next
     action will get the reply. But as in the inital state no reply is
     expected, this should not harm. */
  action = devser->current;
  if (action != NULL && actionData == action->data) {
    if (devser->killCurrent) {
      if (action->kill != NULL)
        action->kill(action->data);
      devser->killCurrent = 0;
      free(action);
    }
    devser->current = NULL;
  }
  /* remove from queue */
  ptr2Last = &devser->actions;
  for (action = devser->actions; action != NULL; action = *ptr2Last) {
    if (actionData == action->data) {
      cnt++;
      /* remove from list */
      *ptr2Last = action->next;
      if (action->kill != NULL)
        action->kill(action->data);
      free(action);
    } else {
      ptr2Last = &action->next;
    }
  }
  return cnt++;
}