Subversion Repositories shark

/*
 * Project: S.Ha.R.K.
 *
 * Coordinators:
 *   Giorgio Buttazzo    <giorgio@sssup.it>
 *   Paolo Gai           <pj@gandalf.sssup.it>
 *
 * Authors     :
 *   Paolo Gai           <pj@gandalf.sssup.it>
 *   (see the web pages for full authors list)
 *
 * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
 *
 * http://www.sssup.it
 * http://retis.sssup.it
 * http://shark.sssup.it
 */

/**
 ------------
 CVS :        $Id: mqueue.c,v 1.4 2003-11-05 15:05:11 giacomo Exp $

 File:        $File$
 Revision:    $Revision: 1.4 $
 Last update: $Date: 2003-11-05 15:05:11 $
 ------------

 POSIX message queues

**/

/*
 * Copyright (C) 2000 Paolo Gai
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <mqueue.h>
#include <ll/string.h>
#include <kernel/types.h>
#include <kernel/var.h>
#include <kernel/func.h>
#include <errno.h>
#include <stdarg.h>
#include <pthread.h>
#include <sys/types.h>

/* some flags... */
#define MQ_USED                 1
#define MQ_NONBLOCK             2
#define MQ_NOTIFICATION_PRESENT 4

static int mq_once = 1;

struct mq_elem {
  unsigned int mq_prio;   /* the priority of a message */
  ssize_t msglen;         /* the length of a message   */
  int next;               /* the priority queue        */
};

/* Semaphores descriptor tables */
static struct mq_des {
    char *name;              /* a name */
    int  flags;              /* flags... */

    long maxmsg;             /* maximum number of messages */
    long msgsize;            /* Maximum message size */

    long count;              /* Number of messages currently queued */
    long start;              /* first not-empty message */

    BYTE *mq_data;           /* the data... */
    struct mq_elem *mq_info; /* the priorities */
    int mq_first;            /* the first empty message */

    struct sigevent notification; /* the notification, valid only if the
                                     correct bit is set */

    /* the blocked processes queues */
    IQUEUE blocked_send;
    IQUEUE blocked_rcv;

    int next;            /* the mq queue */
} mq_table[MQ_OPEN_MAX];


/* this -IS- an extension to the proc_table!!! */
static struct {
    int intsig;   /* Normally it is =0, -1 only when a task is woken up
                     by a signal */
    int mqdes;    /* message queue on which a task is blocked (meaningless
                     if the task is not blocked...) */
} mqproc_table[MAX_PROC];

static int free_mq;         /* Queue of free sem                    */

mqd_t mq_open(const char *name, int oflag, ...)
{
    int     i;
    int     found = 0;
    mode_t  m;
    mqd_t   mq;
    struct mq_attr *attr;
    SYS_FLAGS f;

    f = kern_fsave();

    for (i = 0; i < MQ_OPEN_MAX; i++)
      if (mq_table[i].flags & MQ_USED) {
        if (strcmp((char*)name, mq_table[i].name) == 0) {
          found = 1;
          break;
        }
      }
    if (found) {
      if (oflag == (O_CREAT | O_EXCL)) {
          errno = EEXIST;
          kern_frestore(f);
          return -1;
      } else {
          kern_frestore(f);
          return i;
      }
    } else {
      if (!(oflag & O_CREAT)) {
          errno = ENOENT;
          kern_frestore(f);
          return -1;
      } else if (!(oflag & O_RDWR)) {
          errno = EACCES;
          kern_frestore(f);
          return -1;
      } else {
          va_list l;

          va_start(l, oflag);
            m = va_arg(l,mode_t);
            attr = va_arg(l, struct mq_attr *);
          va_end(l);

          mq = free_mq;
          if (mq != -1) {
            mq_table[mq].name = kern_alloc(strlen((char *)name)+1);
            if (!mq_table[mq].name) {
              errno = ENOSPC;
              kern_frestore(f);
              return -1;
            }
            strcpy(mq_table[mq].name, (char *)name);

            if (attr) {
              mq_table[mq].maxmsg  = attr->mq_maxmsg;
              mq_table[mq].msgsize = attr->mq_msgsize;
            }
            else {
              mq_table[mq].maxmsg  = MQ_DEFAULT_MAXMSG;
              mq_table[mq].msgsize = MQ_DEFAULT_MSGSIZE;
            }
            iq_init(&mq_table[mq].blocked_send, &freedesc, 0);
            iq_init(&mq_table[mq].blocked_rcv, &freedesc, 0);

            mq_table[mq].count = 0;
            mq_table[mq].start = -1;
       
            mq_table[mq].mq_first = 0;

            if (oflag & O_NONBLOCK)
              mq_table[mq].flags = MQ_USED | MQ_NONBLOCK;
            else
              mq_table[mq].flags = MQ_USED;

            mq_table[mq].mq_data = (BYTE *)
              kern_alloc(mq_table[mq].maxmsg * mq_table[mq].msgsize);
            if (!mq_table[mq].mq_data) {
              kern_free(mq_table[mq].name,strlen((char *)name)+1);

              errno = ENOSPC;
              kern_frestore(f);
              return -1;
            }

            mq_table[mq].mq_info = (struct mq_elem *)
              kern_alloc(mq_table[mq].maxmsg * sizeof(struct mq_elem));
            if (!mq_table[mq].mq_info) {
              kern_free(mq_table[mq].name,strlen((char *)name)+1);
              kern_free(mq_table[mq].mq_data,
                        mq_table[mq].maxmsg * mq_table[mq].msgsize);

              errno = ENOSPC;
              kern_frestore(f);
              return -1;
            }

            /* set up the element queue */
            for (i=0; i<mq_table[mq].maxmsg-1; i++)
              mq_table[mq].mq_info[i].next = i+1;
            mq_table[mq].mq_info[mq_table[mq].maxmsg-1].next = -1;
            mq_table[mq].mq_first = 0;

            free_mq = mq_table[mq].next;
            kern_frestore(f);
            return mq;
          }
          else {
            errno = ENOSPC;
            kern_frestore(f);
            return -1;
          }
      }
    }
}

int mq_close(mqd_t mqdes)
{
    SYS_FLAGS f;

    f = kern_fsave();

    if (mqdes < 0 ||
        mqdes >= MQ_OPEN_MAX ||
        !(mq_table[mqdes].flags & MQ_USED) ) {
      errno = EBADF;
      kern_frestore(f);
      return -1;
    }

    kern_free(mq_table[mqdes].name, strlen(mq_table[mqdes].name)+1);
    kern_free(mq_table[mqdes].mq_data,
              mq_table[mqdes].maxmsg * mq_table[mqdes].msgsize);
    kern_free(mq_table[mqdes].mq_info,
              mq_table[mqdes].maxmsg * sizeof(struct mq_elem));

    mq_table[mqdes].flags = 0;
    mq_table[mqdes].next = free_mq;
    free_mq = mqdes;

    kern_frestore(f);
    return 0;
}

int mq_unlink(const char *name)
{
    int i;
    int found = 0;
    SYS_FLAGS f;

    f = kern_fsave();

    for (i = 0; i < MQ_OPEN_MAX; i++)
      if (mq_table[i].flags & MQ_USED) {
        if (strcmp((char*)name, mq_table[i].name) == 0) {
          found = 1;
        }
      }

    if (found) {
      kern_free(mq_table[i].name, strlen((char *)name)+1);
      kern_free(mq_table[i].mq_data,
                mq_table[i].maxmsg * mq_table[i].msgsize);
      kern_free(mq_table[i].mq_info,
                mq_table[i].maxmsg * sizeof(struct mq_elem));
 
      mq_table[i].flags = 0;
      mq_table[i].next = free_mq;
      free_mq = i;
      kern_frestore(f);
      return 0;
    } else {
      errno = ENOENT;
      kern_frestore(f);
      return -1;
    }
}

/* this function inserts a message in amessage queue mantaining the
   priority order */
static void insert_mq_entry(mqd_t mqdes, int newmsg)
{
    int prio; /* the priority of the message to insert */
    int p,q;  /* the messages... */

    p = NIL;
    q = mq_table[mqdes].start;
    prio = mq_table[mqdes].mq_info[ newmsg ].mq_prio;

    while ((q != NIL) && (prio <= mq_table[mqdes].mq_info[ q ].mq_prio)) {
        p = q;
        q = mq_table[mqdes].mq_info[ q ].next;
    }

    if (p != NIL)
      mq_table[mqdes].mq_info[ p ].next = newmsg;
    else
      mq_table[mqdes].start = newmsg;

    mq_table[mqdes].mq_info[ newmsg ].next = q;
}






/* this is the test that is done when a task is being killed
   and it is waiting on a sigwait */
static int mq_cancellation_point(PID i, void *arg)
{
    LEVEL l;

    if (proc_table[i].status == WAIT_MQSEND) {
      /* the task that have to be killed is waiting on a mq_send */

      /* we have to extract the task from the blocked queue... */
      iq_extract(i,&mq_table[mqproc_table[i].mqdes].blocked_send);

      /* and the task have to be reinserted into the ready queues, so it
         will fall into task_testcancel */
      l = proc_table[i].task_level;
      level_table[l]->public_unblock(l,i);

      return 1;
    }

    if (proc_table[i].status == WAIT_MQRECEIVE) {
      /* the task that have to be killed is waiting on a mq_send */

      /* we have to extract the task from the blocked queue... */
      iq_extract(i, &mq_table[mqproc_table[i].mqdes].blocked_rcv);

      /* and the task have to be reinserted into the ready queues, so it
         will fall into task_testcancel */
      l = proc_table[i].task_level;
      level_table[l]->public_unblock(l,i);

      return 1;
    }

    return 0;
}

int mq_interrupted_by_signal(PID i, void *arg)
{
    LEVEL l;

    if (proc_table[i].status == WAIT_MQSEND) {
      /* the task is waiting on a nanosleep and it is still receiving a
         signal... */
      mqproc_table[exec_shadow].intsig = 1;

      /* we have to extract the task from the blocked queue... */
      iq_extract(i, &mq_table[mqproc_table[i].mqdes].blocked_send);

      /* and the task have to be reinserted into the ready queues, so it
         will fall into task_testcancel */
      l = proc_table[i].task_level;
      level_table[l]->public_unblock(l,i);

      return 1;
    }

    if (proc_table[i].status == WAIT_MQRECEIVE) {
      /* the task is waiting on a nanosleep and it is still receiving a
         signal... */
      mqproc_table[exec_shadow].intsig = 1;

      /* we have to extract the task from the blocked queue... */
      iq_extract(i, &mq_table[mqproc_table[i].mqdes].blocked_rcv);

      /* and the task have to be reinserted into the ready queues, so it
         will fall into task_testcancel */
      l = proc_table[i].task_level;
      level_table[l]->public_unblock(l,i);

      return 1;
    }

    return 0;
}





int mq_send(mqd_t mqdes, const char *msg_ptr, size_t msg_len,
            unsigned int msg_prio)
{
  int newmsg;
  SYS_FLAGS f;

  task_testcancel();

  f = kern_fsave();

  /* first, if it is the first time that mq_receive or mq_send is called,
     register the cancellation point */
  if (mq_once) {
    mq_once = 0;
    register_cancellation_point(mq_cancellation_point, NULL);
    register_interruptable_point(mq_interrupted_by_signal, NULL);
  }

  if (mqdes < 0 ||
      mqdes >= MQ_OPEN_MAX ||
      !(mq_table[mqdes].flags & MQ_USED) ) {
    errno = EBADF;
    kern_frestore(f);
    return -1;
  }

  if (msg_len > mq_table[mqdes].msgsize) {
    errno = EMSGSIZE;
    kern_frestore(f);
    return -1;
  }

  if (msg_prio > MQ_PRIO_MAX) {
    errno = EINVAL;
    kern_frestore(f);
    return -1;
  }

  /* block the task if necessary */
  if (mq_table[mqdes].mq_first == -1) {
    /* the message queue is full!!! */
    if (mq_table[mqdes].flags & O_NONBLOCK) {
      errno = EAGAIN;
      kern_frestore(f);
      return -1;
    }
    else {
      LEVEL l;

      /* we block the task until:
         - a message is received, or
         - a signal is sent to the task, or
         - the task is killed               */

      mqproc_table[exec_shadow].intsig = 0;

      kern_epilogue_macro();
   
      l = proc_table[exec_shadow].task_level;
      level_table[l]->public_block(l,exec_shadow);

      /* we insert the task in the message queue */
      proc_table[exec_shadow].status = WAIT_MQSEND;
      iq_priority_insert(exec_shadow,&mq_table[mqdes].blocked_send);

      /* and finally we reschedule */
      exec = exec_shadow = -1;
      scheduler();
      ll_context_to(proc_table[exec_shadow].context);
      kern_deliver_pending_signals();

      /* mq_send is a cancellation point... */
      task_testcancel();

      if (mqproc_table[exec_shadow].intsig) {
        errno = EINTR;
        kern_frestore(f);
        return -1;
      }
    }
  }

  /* Now there is space to insert a new message */
  /* alloc a descriptor */
  newmsg = mq_table[mqdes].mq_first;
  mq_table[mqdes].mq_first = mq_table[mqdes].mq_info[newmsg].next;
  mq_table[mqdes].count++;

  /* fill the data */
  memcpy(mq_table[mqdes].mq_data + newmsg * mq_table[mqdes].msgsize,
         msg_ptr, msg_len);
  mq_table[mqdes].mq_info[ newmsg ].mq_prio = msg_prio;
  mq_table[mqdes].mq_info[ newmsg ].msglen  = msg_len;

  /* insert the data in an ordered way */
  insert_mq_entry(mqdes, newmsg);

//  kern_printf("Ûmq_des=%d, newmsg=%d, count=%dÛ",
//              mqdes, newmsg, mq_table[mqdes].count);

  if (mq_table[mqdes].count == 1) {
    /* the mq was empty */
    PID p;

    p = iq_getfirst(&mq_table[mqdes].blocked_rcv);

    if ( p != NIL) {
      /* The first blocked task has to be woken up */
      LEVEL l;

      proc_table[exec_shadow].context = ll_context_from();

      l = proc_table[p].task_level;
      level_table[l]->public_unblock(l,p);

      /* Preempt if necessary */
      scheduler();
      kern_context_load(proc_table[exec_shadow].context);
      return 0;
    }
    else if (mq_table[mqdes].flags & MQ_NOTIFICATION_PRESENT) {
      mq_table[mqdes].flags &= ~MQ_NOTIFICATION_PRESENT;

      // manage the notification...
      if (mq_table[mqdes].notification.sigev_notify == SIGEV_SIGNAL) {
          // there is no signal pending... post the signal!!!
          sigqueue_internal(0,
                            mq_table[mqdes].notification.sigev_signo,
                            mq_table[mqdes].notification.sigev_value,
                            SI_MESGQ);
      } else if (mq_table[mqdes].notification.sigev_notify == SIGEV_THREAD) {
        /* a new thread must be created; note that the pthread_create
           calls task_createn and task_activate; if task_activate is called
           into signal handlers and calls event_need_reschedule */
        pthread_t new_thread;
   
        if (mq_table[mqdes].notification.sigev_notify_attributes)
          pthread_create(&new_thread,
                         mq_table[mqdes].notification.sigev_notify_attributes,
                         (void *(*)(void *))mq_table[mqdes].notification.sigev_notify_function,
                         mq_table[mqdes].notification.sigev_value.sival_ptr);
        else {
          pthread_attr_t new_attr;
          // the task must be created detached
          pthread_attr_init(&new_attr);
          pthread_attr_setdetachstate(&new_attr, PTHREAD_CREATE_DETACHED);
   
          pthread_create(&new_thread,
                         &new_attr,
                         (void *(*)(void *))mq_table[mqdes].notification.sigev_notify_function,
                         &mq_table[mqdes].notification.sigev_value);
        }
      }
    }
  }

  kern_frestore(f);
  return 0;
}

ssize_t mq_receive(mqd_t mqdes, char *msg_ptr, size_t msg_len,
                   unsigned int *msg_prio)
{
  int msg;
  PID p;
  ssize_t returnvalue;
  SYS_FLAGS f;

  task_testcancel();

  f = kern_fsave();

  /* first, if it is the first time that mq_receive or mq_send is called,
     register the cancellation point */
  if (mq_once) {
    mq_once = 0;
    register_cancellation_point(mq_cancellation_point, NULL);
    register_interruptable_point(mq_interrupted_by_signal, NULL);
  }

  if (mqdes < 0 ||
      mqdes >= MQ_OPEN_MAX ||
      !(mq_table[mqdes].flags & MQ_USED) ) {
    errno = EBADF;
    kern_frestore(f);
    return -1;
  }

  if (msg_len > mq_table[mqdes].msgsize) {
    errno = EMSGSIZE;
    kern_frestore(f);
    return -1;
  }

  /* block the task if necessary */
  if (mq_table[mqdes].start == -1) {
    /* the message queue is empty!!! */
    if (mq_table[mqdes].flags & O_NONBLOCK) {
      errno = EAGAIN;
      kern_frestore(f);
      return -1;
    }
    else {
      LEVEL l;

      /* we block the task until:
         - a message arrives, or
         - a signal is sent to the task, or
         - the task is killed               */

      mqproc_table[exec_shadow].intsig = 0;

      kern_epilogue_macro();
   
      l = proc_table[exec_shadow].task_level;
      level_table[l]->public_block(l,exec_shadow);

      /* we insert the task into the message queue */
      proc_table[exec_shadow].status = WAIT_MQRECEIVE;
      iq_priority_insert(exec_shadow,&mq_table[mqdes].blocked_rcv);

      /* and finally we reschedule */
      exec = exec_shadow = -1;
      scheduler();
      ll_context_to(proc_table[exec_shadow].context);
      kern_deliver_pending_signals();

      /* mq_receive is a cancellation point... */
      task_testcancel();

      if (mqproc_table[exec_shadow].intsig) {
        errno = EINTR;
        kern_frestore(f);
        return -1;
      }
    }
  }

  /* Now there is at least one message...
     copy it to the destination, ... */
  msg = mq_table[mqdes].start;
  memcpy(msg_ptr,
         mq_table[mqdes].mq_data + msg * mq_table[mqdes].msgsize,
         mq_table[mqdes].msgsize);

  /* ...update the first messagee and the counters, ... */
  mq_table[mqdes].count++;
  mq_table[mqdes].start = mq_table[mqdes].mq_info[ msg ].next;
  /* and finally the free message queue */
  mq_table[mqdes].mq_info[ msg ].next = mq_table[mqdes].mq_first;
  mq_table[mqdes].mq_first = msg;

  /* return the priority if required */
  if (msg_prio) {
    *msg_prio = mq_table[mqdes].mq_info[ msg ].mq_prio;
  }

  /* set the returnvalue */
  returnvalue = mq_table[mqdes].mq_info[ msg ].msglen;

  /* if the mq was full, there may be a task into blocked-send queue */
  p = iq_getfirst(&mq_table[mqdes].blocked_send);

  if ( p != NIL) {
    /* The first blocked task on send has to be woken up */
    LEVEL l;

    proc_table[exec_shadow].context = ll_context_from();

    l = proc_table[p].task_level;
    level_table[l]->public_unblock(l,p);

    /* Preempt if necessary */
    scheduler();
    kern_context_load(proc_table[exec_shadow].context);
    return returnvalue;
  }

  kern_frestore(f);
  return returnvalue;
}

int mq_notify(mqd_t mqdes, const struct sigevent *notification)
{
  SYS_FLAGS f;

  f = kern_fsave();

  if (mqdes < 0 ||
      mqdes >= MQ_OPEN_MAX ||
      !(mq_table[mqdes].flags & MQ_USED) ) {
    errno = EBADF;
    kern_frestore(f);
    return -1;
  }

  if (mq_table[mqdes].flags & MQ_NOTIFICATION_PRESENT) {
    if (!notification) {
      mq_table[mqdes].flags &= ~MQ_NOTIFICATION_PRESENT;
      kern_frestore(f);
      return 0;
    }
    else {
      errno = EBUSY;
      kern_frestore(f);
      return -1;
    }
  }

  mq_table[mqdes].flags |= MQ_NOTIFICATION_PRESENT;

  memcpy(&mq_table[mqdes].notification, notification,sizeof(struct sigevent));

  kern_frestore(f);
  return 0;
}

int mq_setattr(mqd_t mqdes, const struct mq_attr *mqstat,
               struct mq_attr *omqstat)
{
  SYS_FLAGS f;

  f = kern_fsave();

  if (mqdes < 0 ||
      mqdes >= MQ_OPEN_MAX ||
      !(mq_table[mqdes].flags & MQ_USED) ) {
    errno = EBADF;
    kern_frestore(f);
    return -1;
  }

  if (omqstat) {
    omqstat->mq_flags   = mq_table[mqdes].flags & O_NONBLOCK;
    omqstat->mq_maxmsg  = mq_table[mqdes].maxmsg;
    omqstat->mq_msgsize = mq_table[mqdes].msgsize;
    omqstat->mq_curmsgs = mq_table[mqdes].count;
  }

  mq_table[mqdes].flags = (mq_table[mqdes].flags & ~O_NONBLOCK) |
                          (mqstat->mq_flags & O_NONBLOCK);
  kern_frestore(f);
  return 0;
}

int mq_getattr(mqd_t mqdes, struct mq_attr *mqstat)
{
  SYS_FLAGS f;

  f = kern_fsave();

  if (mqdes < 0 ||
      mqdes >= MQ_OPEN_MAX ||
      !(mq_table[mqdes].flags & MQ_USED) ) {
    errno = EBADF;
    kern_frestore(f);
    return -1;
  }

  mqstat->mq_flags   = mq_table[mqdes].flags & O_NONBLOCK;
  mqstat->mq_maxmsg  = mq_table[mqdes].maxmsg;
  mqstat->mq_msgsize = mq_table[mqdes].msgsize;
  mqstat->mq_curmsgs = mq_table[mqdes].count;

  kern_frestore(f);
  return 0;
}