Bug fix in vres tx thread. Fixed budgeting.

[frescor/fwp.git] / fwp / lib / fwp / fwp_msgq.c

diff --git a/fwp/lib/fwp/fwp_msgq.c b/fwp/lib/fwp/fwp_msgq.c
index 4dddb973cd5e800a934f4ba9c91b45247f7188ca..0fcc73d0f4d4bc607026e2d6c18bfb8d97cbbaa5 100644 (file)
--- a/fwp/lib/fwp/fwp_msgq.c
+++ b/fwp/lib/fwp/fwp_msgq.c
@@ -56,7 +56,7 @@ void fwp_msgq_init(struct fwp_msgq *msgq)
        msgq->in = 0;
        msgq->out = 0;
        pthread_mutex_init(&msgq->lock, NULL); /* fast mutex */
-       sem_init(&msgq->empty_lock, 0, 0);
+       sem_init(&msgq->msg_sem, 0, 0);
 }
 
 /**
@@ -91,9 +91,9 @@ int fwp_msgq_enqueue(struct fwp_msgq *msgq, struct fwp_msgb *msgb)
        msgq->nr_pending++;
        msgq->in = (++msgq->in) & (FWP_MSGQ_SIZE - 1);
 
+       sem_post(&msgq->msg_sem);
        /* release queue mutex */
        pthread_mutex_unlock(&msgq->lock);
-       sem_post(&msgq->empty_lock);
 
        return 0;
 }
@@ -110,9 +110,7 @@ struct fwp_msgb* fwp_msgq_dequeue(struct fwp_msgq *msgq)
 {
        struct fwp_msgb* msgb;
        
-       if (msgq->in == msgq->out)
-               return NULL;
-                 
+       sem_wait(&msgq->msg_sem);
        /* acquire queue mutex */
        pthread_mutex_lock(&msgq->lock);
        
@@ -138,7 +136,17 @@ void fwp_msgq_dequeue_all(struct fwp_msgq *msgq)
 {
        struct fwp_msgb *msgb;
 
-       while ((msgb = fwp_msgq_dequeue(msgq))) { 
+       /* acquire queue mutex */
+       pthread_mutex_lock(&msgq->lock);
+       
+       while (msgq->in != msgq->out){
+               msgb = msgq->queue[msgq->out];
+               msgq->nr_pending--;
+               msgq->out = (++msgq->out) & (FWP_MSGQ_SIZE - 1);
                fwp_msgb_free(msgb);
        }
+       
+       sem_init(&msgq->msg_sem, 0, 0);
+       /* release queue mutex */
+       pthread_mutex_unlock(&msgq->lock);
 }