From 360ca869c8646b35c197da62d66745150d59084b Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Tue, 1 Mar 2016 13:27:14 +0000
Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9002
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 os/common/abstractions/nasa_osal/src/osapi.c | 72 ++++++++++++++++++++++++----
 1 file changed, 62 insertions(+), 10 deletions(-)

(limited to 'os')

diff --git a/os/common/abstractions/nasa_osal/src/osapi.c b/os/common/abstractions/nasa_osal/src/osapi.c
index babde2975..f24b25d3c 100644
--- a/os/common/abstractions/nasa_osal/src/osapi.c
+++ b/os/common/abstractions/nasa_osal/src/osapi.c
@@ -405,7 +405,8 @@ int32 OS_TimerDelete(uint32 timer_id) {
 
   /* Range check.*/
   if ((otp < &osal.timers[0]) ||
-      (otp >= &osal.timers[OS_MAX_TIMERS])) {
+      (otp >= &osal.timers[OS_MAX_TIMERS]) ||
+      (otp->is_free)) {
     return OS_ERR_INVALID_ID;
   }
 
@@ -441,7 +442,8 @@ int32 OS_TimerSet(uint32 timer_id, uint32 start_time, uint32 interval_time) {
 
   /* Range check.*/
   if ((otp < &osal.timers[0]) ||
-      (otp >= &osal.timers[OS_MAX_TIMERS])) {
+      (otp >= &osal.timers[OS_MAX_TIMERS]) ||
+      (otp->is_free)) {
     return OS_ERR_INVALID_ID;
   }
 
@@ -509,7 +511,8 @@ int32 OS_TimerGetInfo(uint32 timer_id, OS_timer_prop_t *timer_prop) {
 
   /* Range check.*/
   if ((otp < &osal.timers[0]) ||
-      (otp >= &osal.timers[OS_MAX_TIMERS])) {
+      (otp >= &osal.timers[OS_MAX_TIMERS]) ||
+      (otp->is_free)) {
     return OS_ERR_INVALID_ID;
   }
 
@@ -603,7 +606,8 @@ int32 OS_QueueDelete(uint32 queue_id) {
 
   /* Range check.*/
   if ((oqp < &osal.queues[0]) ||
-      (oqp >= &osal.queues[OS_MAX_QUEUES])) {
+      (oqp >= &osal.queues[OS_MAX_QUEUES]) ||
+      (oqp->is_free)) {
     return OS_ERR_INVALID_ID;
   }
 
@@ -638,14 +642,62 @@ int32 OS_QueueDelete(uint32 queue_id) {
 
 int32 OS_QueueGet(uint32 queue_id, void *data, uint32 size,
                   uint32 *size_copied, int32 timeout) {
+  osal_queue_t *oqp = (osal_queue_t *)queue_id;
+  msg_t msg, msgsts;
+  void *body;
 
-  (void)queue_id;
-  (void)data;
-  (void)size;
-  (void)size_copied;
-  (void)timeout;
+  /* NULL pointer checks.*/
+  if ((data == NULL) || (size_copied == NULL)) {
+    return OS_INVALID_POINTER;
+  }
 
-  return OS_ERR_NOT_IMPLEMENTED;
+  /* Range check.*/
+  if ((oqp < &osal.queues[0]) ||
+      (oqp >= &osal.queues[OS_MAX_QUEUES]) ||
+      (oqp->is_free)) {
+    return OS_ERR_INVALID_ID;
+  }
+
+  /* Check on minimum size.*/
+  if (size < oqp->size) {
+    return OS_QUEUE_INVALID_SIZE;
+  }
+
+  /* Special time handling.*/
+  if (timeout == OS_PEND) {
+    msgsts = chMBFetch(&oqp->mb, &msg, TIME_INFINITE);
+    if (msgsts < MSG_OK) {
+      *size_copied = 0;
+      return OS_ERROR;
+    }
+  }
+  else if (timeout == OS_CHECK) {
+    msgsts = chMBFetch(&oqp->mb, &msg, TIME_IMMEDIATE);
+    if (msgsts < MSG_OK) {
+      *size_copied = 0;
+      return OS_QUEUE_EMPTY;
+    }
+  }
+  else {
+    msgsts = chMBFetch(&oqp->mb, &msg, (systime_t)timeout);
+    if (msgsts < MSG_OK) {
+      *size_copied = 0;
+      return OS_QUEUE_TIMEOUT;
+    }
+  }
+
+  /* Message body and size.*/
+  *size_copied = ((osal_message_t *)msg)->size;
+  body  = (void *)((osal_message_t *)msg)->buf;
+
+  /* Copying the message body.*/
+  memcpy(data, body, *size_copied);
+
+  /* Freeing the message buffer.*/
+  chPoolFree(&oqp->messages, (void *)msg);
+  chSemSignal(&oqp->free_msgs);
+
+  return OS_SUCCESS;
 }
 
 int32 OS_QueuePut(uint32 queue_id, void *data, uint32 size, uint32 flags) {
-- 
cgit v1.2.3