Merge pull request #676 from armink/bug_fix

[DeviceDriver] fix the dataqueue issues.

2 files changed, 14 insertions, 27 deletions

diff --git a/components/drivers/include/rtdevice.h b/components/drivers/include/rtdevice.h
index 4564491003..3b22248c97 100644
--- a/components/drivers/include/rtdevice.h
+++ b/components/drivers/include/rtdevice.h
@@ -134,7 +134,6 @@ struct rt_data_queue
 {
     rt_uint16_t size;
     rt_uint16_t lwm;
-    rt_bool_t   waiting_lwm;
 
     rt_uint16_t get_index;
     rt_uint16_t put_index;
diff --git a/components/drivers/src/dataqueue.c b/components/drivers/src/dataqueue.c
index 1702d20710..a10b0805b4 100644
--- a/components/drivers/src/dataqueue.c
+++ b/components/drivers/src/dataqueue.c
@@ -20,6 +20,7 @@
  * Change Logs:
  * Date           Author       Notes
  * 2012-09-30     Bernard      first version.
+ * 2016-10-31     armink       fix some resume push and pop thread bugs
  */
 
 #include <rtthread.h>
@@ -44,7 +45,6 @@ rt_data_queue_init(struct rt_data_queue *queue,
 
     queue->size = size;
     queue->lwm = lwm;
-    queue->waiting_lwm = RT_FALSE;
 
     queue->get_index = 0;
     queue->put_index = 0;
@@ -67,7 +67,6 @@ rt_err_t rt_data_queue_push(struct rt_data_queue *queue,
                             rt_size_t data_size,
                             rt_int32_t timeout)
 {
-    rt_uint16_t mask;
     rt_ubase_t  level;
     rt_thread_t thread;
     rt_err_t    result;
@@ -76,13 +75,10 @@ rt_err_t rt_data_queue_push(struct rt_data_queue *queue,
 
     result = RT_EOK;
     thread = rt_thread_self();
-    mask = queue->size - 1;
 
     level = rt_hw_interrupt_disable();
     while (queue->put_index - queue->get_index == queue->size)
     {
-        queue->waiting_lwm = RT_TRUE;
-
         /* queue is full */
         if (timeout == 0)
         {
@@ -122,14 +118,13 @@ rt_err_t rt_data_queue_push(struct rt_data_queue *queue,
         if (result != RT_EOK) goto __exit;
     }
 
-    queue->queue[queue->put_index & mask].data_ptr  = data_ptr;
-    queue->queue[queue->put_index & mask].data_size = data_size;
+    queue->queue[queue->put_index % queue->size].data_ptr  = data_ptr;
+    queue->queue[queue->put_index % queue->size].data_size = data_size;
     queue->put_index += 1;
 
+    /* there is at least one thread in suspended list */
     if (!rt_list_isempty(&(queue->suspended_pop_list)))
     {
-        /* there is at least one thread in suspended list */
-
         /* get thread entry */
         thread = rt_list_entry(queue->suspended_pop_list.next,
                                struct rt_thread,
@@ -164,7 +159,6 @@ rt_err_t rt_data_queue_pop(struct rt_data_queue *queue,
     rt_ubase_t  level;
     rt_thread_t thread;
     rt_err_t    result;
-    rt_uint16_t mask;
 
     RT_ASSERT(queue != RT_NULL);
     RT_ASSERT(data_ptr != RT_NULL);
@@ -172,7 +166,6 @@ rt_err_t rt_data_queue_pop(struct rt_data_queue *queue,
 
     result = RT_EOK;
     thread = rt_thread_self();
-    mask   = queue->size - 1;
 
     level = rt_hw_interrupt_disable();
     while (queue->get_index == queue->put_index)
@@ -216,20 +209,14 @@ rt_err_t rt_data_queue_pop(struct rt_data_queue *queue,
             goto __exit;
     }
 
-    *data_ptr = queue->queue[queue->get_index & mask].data_ptr;
-    *size     = queue->queue[queue->get_index & mask].data_size;
+    *data_ptr = queue->queue[queue->get_index % queue->size].data_ptr;
+    *size     = queue->queue[queue->get_index % queue->size].data_size;
 
     queue->get_index += 1;
 
-    if ((queue->waiting_lwm == RT_TRUE) && 
-        (queue->put_index - queue->get_index) <= queue->lwm)
+    if ((queue->put_index - queue->get_index) <= queue->lwm)
     {
-        queue->waiting_lwm = RT_FALSE;
-
-        /*
-         * there is at least one thread in suspended list
-         * and less than low water mark
-         */
+        /* there is at least one thread in suspended list */
         if (!rt_list_isempty(&(queue->suspended_push_list)))
         {
             /* get thread entry */
@@ -244,6 +231,10 @@ rt_err_t rt_data_queue_pop(struct rt_data_queue *queue,
             /* perform a schedule */
             rt_schedule();
         }
+        else
+        {
+            rt_hw_interrupt_enable(level);
+        }
 
         if (queue->evt_notify != RT_NULL)
             queue->evt_notify(queue, RT_DATAQUEUE_EVENT_LWM);
@@ -267,12 +258,9 @@ rt_err_t rt_data_queue_peak(struct rt_data_queue *queue,
                             rt_size_t *size)
 {
     rt_ubase_t  level;
-    rt_uint16_t mask;
 
     RT_ASSERT(queue != RT_NULL);
 
-    mask = queue->size - 1;
-
     level = rt_hw_interrupt_disable();
 
     if (queue->get_index == queue->put_index) 
@@ -282,8 +270,8 @@ rt_err_t rt_data_queue_peak(struct rt_data_queue *queue,
         return -RT_EEMPTY;
     }
 
-    *data_ptr = queue->queue[queue->get_index & mask].data_ptr;
-    *size     = queue->queue[queue->get_index & mask].data_size;
+    *data_ptr = queue->queue[queue->get_index % queue->size].data_ptr;
+    *size     = queue->queue[queue->get_index % queue->size].data_size;
 
     rt_hw_interrupt_enable(level);