rt_ipc.c

fsmlabs的real time linux的内核

int rt_task_ipc_init(RT_TASK_IPC *task, void (*fn)(int data), int data,
  int stack_size, int priority)
{
  /* initially task is not blocked on a semaphore */
  int ret;
  task->sem_at = NULL;
  task->magic = RT_TASK_IPC_MAGIC;
  ret = rt_task_init(MAKE_RT_TASK(task), fn, data, stack_size, priority);
  if (ret < 0) {
	  return ret;
  }
  (*MAKE_RT_TASK(task))->user[IPC_DATA_INDEX] = task;
  return 0;
}

/*************************************************************************
 * rt_task_ipc_delete -- rt_ipc version of rt_task_delete()
 *
 * RT-Linux programs using rt_ipc should use rt_task_ipc_delete instead of
 * rt_task_delete().  It removes the task from any semaphore or message queue
 * it is in, then calls rt_task_delete().  Note that its parameter is an
 * RT_TASK_IPC instead of an RT_TASK.
 *
 * Returns 0 if successful, or -EINVAL if 'task' does not refer to a valid
 * task.
 *************************************************************************/
int rt_task_ipc_delete(RT_TASK_IPC *task)
{
  int ret = 0;
  if (task->magic != RT_TASK_IPC_MAGIC)
    ret = EINVAL;
  else
  {
    /* for task deletion safety, must remove task from any sem or mq list */
    int flags;

    rtl_critical(flags);
    if (task->sem_at != NULL)
      unlink_sem_task(&(task->rte), task->sem_at);
    else if (task->mq_at != NULL)
      unlink_mq_task(&(task->rte), task->mq_at);
    rtl_end_critical(flags);
    ret = rt_task_delete(MAKE_RT_TASK(task));
  }
  return ret;
}

/*************************************************************************
 * rt_task_delay -- delay task 
 *
 * Delays the calling task until the time specified in 'duration'.
 *
 * Always returns 0.
 *************************************************************************/
int rt_task_delay(RTIME duration)
{
  int ret = 0;
  int flags;
	  rtl_critical(flags);
  /* mark the task as delayed */
  pthread_self()->period = 0;
  RTL_MARK_SUSPENDED(pthread_self());
  __rtl_setup_timeout(pthread_self(), HRT_FROM_8254(duration));
  /* set the time at which execution may resume */
  rtl_schedule();
  rtl_end_critical(flags);
  return ret;
}

/*************************************************************************
 * rt_mq_init -- initialize a real-time message queue
 *
 * Called to initialize a real-time message queue.  'mq' must point to a
 * statically allocated structure.  'max_msgs' is the maximum number of
 * messages allowed, and 'msg_size' is the size of each message.
 *
 * Returns 0 if successful, -ENOMEM if space for the queue could not be
 * allocated, or -EINVAL if called incorrectly.
 *************************************************************************/
int rt_mq_init(rt_mq_t *mq, int max_msgs, int msg_size)
{
  int ret = 0;
  if (max_msgs <= 0 || msg_size < 0)
    ret = -EINVAL;		/* must be positive */
  else
  {
    mq->magic = RT_MQ_MAGIC;
    mq->wait_list = NULL;
    mq->max_msgs = max_msgs;
    mq->msg_size = msg_size;
    /* for efficiency, the max size of the queue data is allocated */
    /* all in one piece at init time */
    if ((mq->q = kmalloc(max_msgs * msg_size, GFP_KERNEL)) == NULL)
      ret = -ENOMEM;
    else
    {
      mq->status = RT_MQ_EMPTY;
      mq->f = mq->r = mq->q;		/* initialize queue pointers */
    }
  }
  return ret;
}

/*************************************************************************
 * rt_mq_destroy -- remove a real-time message queue
 *
 * Removes a message queue previously created with rt_mq_create().  Message
 * queue deletion safety is implemented; i.e., any tasks blocked on this 
 * message queue when it is destroyed are allowed to run.
 *
 * Returns 0 if successful, -EINVAL if 'sem' is not a valid rt_sem_t.
 *************************************************************************/
int rt_mq_destroy(rt_mq_t *mq)
{
  int ret = 0;
  if (mq->magic != RT_MQ_MAGIC)
    ret = -EINVAL;
  else
  {
    /* unblock any tasks blocked on this message queue */
    while (rt_mq_send(mq, NULL, RT_MQ_NORMAL, RT_NO_WAIT) != 0)
      ;
    while (rt_mq_receive(mq, NULL, RT_NO_WAIT) != 0)
      ;
    kfree_s(mq->q, mq->max_msgs * mq->msg_size);
  }
  return ret;
}

/*************************************************************************
 * enqueue -- enqueue data
 *
 * Enqueues a block of data on the queue 'mq' with priority 'prio'.
 * An RT_MQ_NORMAL block goes to the rear of the queue, while an RT_MQ_URGENT
 * block goes to the front.  The status is set appropriately as RT_MQ_FULL
 * or RT_MQ_NEITHER.  enqueue() should not be called when RT_MQ_FULL.
 *************************************************************************/
static void enqueue(rt_mq_t *mq, char *msg, RT_MQ_PRIO prio)
{
  if (prio == RT_MQ_NORMAL)
  {
    if (msg != NULL)
      memcpy(mq->r, msg, mq->msg_size);
    /* check for wraparound */
    if ((mq->r += mq->msg_size) == mq->q + mq->msg_size * mq->max_msgs)
      mq->r = mq->q;
  }
  else	/* prio == RT_MQ_URGENT */
  {
    /* check for wraparound */
    if (mq->f == mq->q)
      mq->f = mq->q + mq->msg_size * (mq->max_msgs- 1) ;
    else
      mq->f -= mq->msg_size;
    if (msg != NULL)
      memcpy(mq->f, msg, mq->msg_size);
  }
  if (mq->f == mq->r)		/* queue is now full */
    mq->status = RT_MQ_FULL;
  else
    mq->status = RT_MQ_NEITHER;
}

/*************************************************************************
 * dequeue -- dequeue data
 *
 * Dequeues a block of data from the queue 'mq'.  The status is set
 * appropriately as RT_MQ_EMPTY or RT_MQ_NEITHER.  dequeue() should not
 * be called when RT_MQ_EMPTY.
 *************************************************************************/
static void dequeue(rt_mq_t *mq, char *msg)
{
  if (msg != NULL)
    memcpy(msg, mq->f, mq->msg_size);
  /* check for wraparound */
  if ((mq->f += mq->msg_size) == mq->q + mq->msg_size * mq->max_msgs)
    mq->f = mq->q;
  if (mq->r == mq->f)		/* queue is now empty */
    mq->status = RT_MQ_EMPTY;
  else
    mq->status = RT_MQ_NEITHER;
}

/*************************************************************************
 * rt_mq_send -- message queue send operation
 *
 * Enqueues the data 'msg' on the message queue 'mq'.  The data is assumed
 * to be of the size with which rt_mq_init() was called.  If 'prio' is
 * RT_MQ_NORMAL, the data is queued at the end. If 'prio' is RT_MQ_URGENT,
 * the data is forced to the front of the queue.  'wait' specifies an
 * optional timeout period.  If 'wait' is RT_NO_WAIT, rt_mq_send()
 * returns immediately even if no space for the message is present.  If
 * 'wait' is RT_WAIT_FOREVER, no timeout occurs.  If 'wait' is any other
 * value, it reflects the time at which rt_mq_send() will wake up and
 * return with -ETIME.
 *
 * Returns 0 if successful, -ETIME if the operation timed out, -EAGAIN if
 * RT_NO_WAIT was specified and the operation could not be completed
 * immediately, or -EINVAL if the rt_mq_t is not valid.
 *************************************************************************/
int rt_mq_send(rt_mq_t *mq, char *msg, RT_MQ_PRIO prio, RTIME wait)
{
  int ret = 0;
  if (mq->magic != RT_MQ_MAGIC)
    ret = -EINVAL;		/* invalid rt_mq_t structure */
  else
  {
    int flags;

    rtl_critical(flags);
    switch (mq->status)
    {
    case RT_MQ_FULL:	/* q full -- this task must wait */
    {
      if (wait == RT_NO_WAIT)
        ret = -EAGAIN;	/* can't queue it -- just report error */
      else	/* wait is allowed */
      {
        RT_TASK_ENTRY *to_add = &(((RT_TASK_IPC *)rtl_current)->rte);
        /* put task on wait_list */
        to_add->task = rtl_current;
        to_add->prev = NULL;
        to_add->next = mq->wait_list;
        if (to_add->next != NULL)
          to_add->next->prev = to_add;
        mq->wait_list = to_add;
        /* indicate which mq the task is blocked at */
        ((RT_TASK_IPC *)rtl_current)->mq_at = mq;
        if (wait == RT_WAIT_FOREVER)
          rt_task_suspend(rtl_current);	/* suspend until receive */
        else
        {
          /* assume call timed out.  If this is not the case, */
          /* rt_mq_receive() will clear this flag */
          ((RT_TASK_IPC *)rtl_current)->timed_out = 1;
          /* delay until either receive occurs or timeout occurs */
          rt_task_delay(wait);
          if (((RT_TASK_IPC *)rtl_current)->timed_out)
          {
            /* timed out -- undo everything and return */
            unlink_mq_task(to_add, mq);
            ret = -ETIME;
            break;
          }
        }
        /* when again allowed to run, enqueue the data */
        enqueue(mq, msg, prio);		/* finally, enqueue the data */
      }
      break;
    }
    case RT_MQ_EMPTY:	/* q empty -- this operation might unblock a task */
    {
      RT_TASK_ENTRY *t, *to_run;
      enqueue(mq, msg, prio);	/* first, go ahead and enqueue the data */
      /* find the waiting task with the highest priority */
      /* search exhaustively all waiting tasks.  I don't want to keep */
      /* the list in priority order because I don't want to assume    */
      /* the task priorities won't change.                            */
      for (t=mq->wait_list, to_run=NULL ; t!=NULL ; t=t->next)
        if (to_run == NULL || GET_PRIO(t->task) < GET_PRIO(to_run->task))
          to_run = t;
      if (to_run != NULL)
      {
        /* remove the task to be run from the wait_list */
        unlink_mq_task(to_run, mq);
        /* mark that task as no longer waiting at mq */
        ((RT_TASK_IPC *)(to_run->task))->mq_at = NULL;
        /* rt_mq_receive() will return because of a send, not */
        /* because of a timeout */
        ((RT_TASK_IPC *)(to_run->task))->timed_out = 0;
        rt_task_wakeup(to_run->task);
      }
      break;
    }
    case RT_MQ_NEITHER:		/* space exists for new entry -- put it in */
      enqueue(mq, msg, prio);
      break;
    }
    rtl_end_critical(flags);
  }
  return ret;
}

/*************************************************************************
 * rt_mq_receive -- message queue receive operation
 *