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 */
  SchedHeader *headers;
  ErrMsg *errmsg;
  int steps;
  int stopTask;
};
  
static char *devPrio[NumberOfPRIO] = {
  "null", "slow", "read", "progress", "write", "halt"
};

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;
    if (victim->kill != NULL) victim->kill(victim->data);
    free(victim);
  }
}

static void DevKillTask(void *ds) {
  DevSer *devser = ds;
  
  if (devser->stopTask) {
    free(devser);
  } else {
    devser->stopTask = 1;
  }
}

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;
        }
      }
    }
    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;
  
  if (devser->steps == 0) return 1;
  if (devser->stopTask) {
    return 0;
  }
  action = devser->current;
  if (action == NULL) {
    action = DevNextAction(devser);
  }

  while (action != NULL) {
    status = AsconTask(devser->asyncConn);
    if (status == AsconFailure) {
      devser->errmsg = AsconGetErrList(devser->asyncConn);
    } else if (status != AsconReady) {
      return 1;
    }
    if (devser->steps > 0) { /* debugging mode */
      devser->steps--;
    }
    if(status == AsconFailure){
      replyData = devser->errmsg->text;
    } else {
      replyData = AsconRead(devser->asyncConn);
    }
    sendData = action->hdl(action->data, replyData);
    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;
}

void DevSigFun(void *ds, int iSignal, void *pSigData) {
  DevSer *devser = ds;
  AsconStatus status;
  DevAction *action;
  SchedHeader *header;

  if (devser->stopTask) {
    return;
  }
  for (header = devser->headers; header != NULL; header = header->next) {
  // TODO ffr Set interrupt level on the action handler node's sics_int property
  // An action handler should clear the sics_int property after it is called.
  }
}  
   
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->stopTask = 0;
  devser->steps = -1; /* no debugging by default */
  TaskRegister(pServ->pTasker, DevQueueTask, NULL, DevKillTask, 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);
  h = devser->headers;
  while (h != NULL) {
    victim = h;
    h = victim->next;
    DevFreeActionList(victim->actions);
    free(victim);
  }
  if (devser->stopTask) {
    free(devser);
  } else {
    devser->stopTask = 1;
  }
}

void 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; /* there is already an identical action */
    }
    ptr2Last = &action->next;
  }
  new = DevNewAction(actionData, hdl, killFunc, prio);
  new->next = action;
  *ptr2Last = new;
}

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;
        if (action->kill != NULL) action->kill(action->data);
        free(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 = action->next) {
    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++;
}