minix/lib/libddekit/src/msg_queue.c

   1 #include "common.h"
   2
   3 #include <ddekit/memory.h>
   4 #include <ddekit/minix/msg_queue.h>
   5 #include <ddekit/panic.h>
   6 #include <ddekit/semaphore.h>
   7
   8 #define MESSAGE_QUEUE_SIZE 16
   9
  10 #ifdef DDEBUG_LEVEL_MSG_Q
  11 #undef DDEBUG
  12 #define DDEBUG DDEBUG_LEVEL_MSG_Q
  13 #endif
  14
  15 #include "debug.h"
  16
  17 struct ddekit_minix_msg_q {
  18
  19         unsigned from, to;
  20
  21         message messages[MESSAGE_QUEUE_SIZE];
  22         int ipc_status[MESSAGE_QUEUE_SIZE];
  23         ddekit_sem_t *msg_w_sem, *msg_r_sem;
  24         int msg_r_pos, msg_w_pos;
  25
  26         struct ddekit_minix_msg_q *next;
  27 };
  28
  29 static struct ddekit_minix_msg_q * _list = NULL;
  30 static void _ddekit_minix_queue_msg
  31                    (struct ddekit_minix_msg_q *mq, message *m, int ipc_status);
  32
  33 /*****************************************************************************
  34  *      ddekit_minix_create_msg_q                                            *
  35  ****************************************************************************/
  36 struct ddekit_minix_msg_q *
  37 ddekit_minix_create_msg_q(unsigned from, unsigned to)
  38 {
  39         struct ddekit_minix_msg_q *mq =  (struct ddekit_minix_msg_q *)
  40             ddekit_simple_malloc(sizeof(struct ddekit_minix_msg_q));
  41
  42         mq->from = from;
  43         mq->to   = to;
  44         mq->msg_w_pos = 0;
  45         mq->msg_r_pos = 0;
  46
  47         mq->msg_r_sem = ddekit_sem_init(0);
  48         mq->msg_w_sem = ddekit_sem_init(MESSAGE_QUEUE_SIZE);
  49
  50         /* TODO: check for overlapping message ranges */
  51         mq->next = _list;
  52         _list     = mq;
  53
  54         DDEBUG_MSG_VERBOSE("created msg_q from %x to %x\n", from , to);
  55
  56         return mq;
  57 }
  58
  59 /*****************************************************************************
  60  *      ddekit_minix_deregister_msg_q                                        *
  61  ****************************************************************************/
  62 void ddekit_minix_deregister_msg_q(struct ddekit_minix_msg_q *mq)
  63 {
  64         struct ddekit_minix_msg_q *prev =_list, *it;
  65
  66         for (it = _list->next; it != NULL ; it = it->next) {
  67                 if (it == mq) {
  68                         prev->next = it->next;
  69                         break;
  70                 }
  71                 prev=it;
  72         }
  73
  74         ddekit_sem_deinit(mq->msg_r_sem);
  75         ddekit_sem_deinit(mq->msg_w_sem);
  76
  77         ddekit_simple_free(mq);
  78
  79         DDEBUG_MSG_VERBOSE("destroyed msg_q from \n");
  80 }
  81
  82 /*****************************************************************************
  83  *     _ddekit_minix_queue_msg                                               *
  84  ****************************************************************************/
  85 static void
  86 _ddekit_minix_queue_msg (
  87         struct ddekit_minix_msg_q *mq,
  88         message *m,
  89         int ipc_status
  90 )
  91 {
  92         int full;
  93         full = ddekit_sem_down_try(mq->msg_w_sem);
  94
  95         if (full) {
  96                 /* Our message queue is full... */
  97                 int result;
  98                 DDEBUG_MSG_WARN("Receive queue is full. Dropping request.\n");
  99
 100                 /* XXX should reply to the sender with EIO or so, but for that
 101                  * we would need to look at the request and find a suitable
 102                  * reply code..
 103                  */
 104         } else {
 105                 /* queue the message */
 106                 memcpy(&mq->messages[mq->msg_w_pos], m, sizeof(message));
 107                 mq->ipc_status[mq->msg_w_pos] = ipc_status;
 108                 if (++mq->msg_w_pos == MESSAGE_QUEUE_SIZE) {
 109                         mq->msg_w_pos = 0;
 110                 }
 111                 DDEBUG_MSG_VERBOSE("ddekit_minix_queue_msg: queueing msg %x\n",
 112                                     m->m_type);
 113                 ddekit_sem_up(mq->msg_r_sem);
 114         }
 115 }
 116
 117 /*****************************************************************************
 118  *       ddekit_minix_queue_msg                                              *
 119  ****************************************************************************/
 120 void ddekit_minix_queue_msg(message *m, int ipc_status)
 121 {
 122         struct ddekit_minix_msg_q *it, *mq = NULL;
 123
 124         for (it = _list; it !=NULL ; it = it->next) {
 125                 if (m->m_type >= it->from && m->m_type <= it->to) {
 126                         mq = it;
 127                         break;
 128                 }
 129         }
 130         if (mq == NULL) {
 131                 DDEBUG_MSG_VERBOSE("no q for msgtype %x\n", m->m_type);
 132                 return;
 133         }
 134         _ddekit_minix_queue_msg(mq, m, ipc_status);
 135 }
 136
 137 /*****************************************************************************
 138  *        ddekit_minix_rcv                                                   *
 139  ****************************************************************************/
 140 void ddekit_minix_rcv (
 141         struct ddekit_minix_msg_q *mq,
 142         message *m,
 143         int *ipc_status
 144 )
 145 {
 146         DDEBUG_MSG_VERBOSE("waiting for message");
 147
 148         ddekit_sem_down(mq->msg_r_sem);
 149
 150         memcpy(m, &mq->messages[mq->msg_r_pos], sizeof(message));
 151         *ipc_status = mq->ipc_status[mq->msg_r_pos];
 152         if (++mq->msg_r_pos == MESSAGE_QUEUE_SIZE) {
 153                 mq->msg_r_pos = 0;
 154         }
 155
 156         DDEBUG_MSG_VERBOSE("unqueing message");
 157
 158         ddekit_sem_up(mq->msg_w_sem);
 159 }