Develop OS abstract layer functions

Ubiquitous/XiZi_AIoT/services/drivers/usb/components/osal/usb_osal.c

@@ -9,10 +9,11 @@ File name: usb_osal.c
 Description: adopt cherry USB to XiZi AIOT.
 Others: CherryUSB v0.1.2/CherryUSB/osal/usb_osal_freertos.c for references
                 https://github.com/cherry-embedded/CherryUSB/blob/v0.10.2/osal/usb_osal_freertos.c
+
 History: 
-1. Date: 2024-06-13
+1. Date: 2024-06-17
 Author: AIIT XUOS Lab
-Modification: rename the file name, and re-develop all OS abstract layer function.
+Modification: rename the file name, and re-develop all OS abstract layer functions.
 *************************************************/
 
 #include "usb_osal.h"
@@ -30,6 +31,8 @@ usb_osal_thread_t usb_osal_thread_create(const char *name, uint32_t stack_size,
     return (usb_osal_thread_t)ret;
 }
 
+
+
 void usb_osal_thread_delete(usb_osal_thread_t thread){
     int thread_id;
     thread_id = (int)thread;
@@ -38,6 +41,7 @@ void usb_osal_thread_delete(usb_osal_thread_t thread){
 }
 
 
+
 usb_osal_sem_t usb_osal_sem_create(uint32_t initial_count){
     int sem_id;
 
@@ -55,6 +59,7 @@ void usb_osal_sem_delete(usb_osal_sem_t sem){
 }
 
 
+
 int usb_osal_sem_take(usb_osal_sem_t sem, uint32_t timeout){
     int sem_id;
     sem_id = (int)sem;
@@ -68,6 +73,7 @@ int usb_osal_sem_take(usb_osal_sem_t sem, uint32_t timeout){
 }
 
 
+
 int usb_osal_sem_give(usb_osal_sem_t sem){
     int sem_id;
     sem_id = (int)sem;
@@ -80,6 +86,8 @@ int usb_osal_sem_give(usb_osal_sem_t sem){
     return 0;
 }
 
+
+
 usb_osal_mutex_t usb_osal_mutex_create(void){
     int mutex_id;
 
@@ -88,12 +96,16 @@ usb_osal_mutex_t usb_osal_mutex_create(void){
     return (usb_osal_mutex_t)mutex_id;
 }
 
+
+
 void usb_osal_mutex_delete(usb_osal_mutex_t mutex){
     int mutex_id;
     mutex_id = (int)mutex;
     semaphore_free(mutex_id);
 }
 
+
+
 int usb_osal_mutex_take(usb_osal_mutex_t mutex){
     int mutex_id;
     mutex_id = (int)mutex;
@@ -107,6 +119,7 @@ int usb_osal_mutex_take(usb_osal_mutex_t mutex){
 }
 
 
+
 int usb_osal_mutex_give(usb_osal_mutex_t mutex){
     int mutex_id;
     mutex_id = (int)mutex;
@@ -120,6 +133,7 @@ int usb_osal_mutex_give(usb_osal_mutex_t mutex){
 }
 
 
+
 osal_msg_queue_t osal_mq_create(const char *name,
                      size_t   msg_size,
                      size_t   max_msgs,
@@ -193,7 +207,41 @@ int osal_mq_send(osal_msg_queue_t mq,
                 void *msg,
                 size_t msg_len,
                 uint32_t timeout){
+    uint16_t header, tail, tail_nxt;
+    int mutex_id;
+    osal_mq_msg_header_t msg_tail;
+    uint8_t *msg_buf;
+
+    if(mq == NULL || msg == NULL || msg_len == 0)
         return -1;
+
+    if(msg_len > mq->msg_size)
+        return -1;
+
+    mutex_id = (int)(mq->mutex);
+    if(mutex_id <= 0)
+        return -1;
+
+    usb_osal_sem_take(mq->mutex, timeout);
+
+
+    tail = mq->tail;
+    header = mq->header;
+    /* message queue is full. */
+    tail_nxt = (tail + 1) > mq->max_msgs ? (tail + 1 -mq->max_msgs):(tail + 1);
+    if(tail_nxt == header){
+        usb_osal_sem_give(mq->mutex);
+        return 0;
+    }
+
+    msg_tail = (osal_mq_msg_header_t)(mq->data + (sizeof(struct osal_mq_msg_header) + mq->msg_size) * header);
+    msg_buf = (uint8_t *)(msg_tail + 1);
+
+    msg_tail->len = msg_len;
+    memcpy(msg_buf, msg, msg_len);
+    usb_osal_sem_give(mq->mutex);
+
+    return msg_len;
 }
 
 
@@ -233,11 +281,17 @@ int osal_mq_recv(osal_msg_queue_t mq,
     msg_buf = (uint8_t *)(msg_header + 1);
     copy_len = (msg_header->len > msg_len ? msg_len : msg_header->len);
     
+    memcpy(msg, msg_buf, copy_len);
 
-    return -1;
+    header++;
+    mq->header = (header > mq->max_msgs) ? (header - mq->max_msgs) : (header);
+    usb_osal_sem_give(mq->mutex);
+
+    return copy_len;
 }
 
 
+
 usb_osal_mq_t usb_osal_mq_create(uint32_t max_msgs){
     osal_msg_queue_t mq;
     mq = osal_mq_create("usb_mq", sizeof(uintptr_t *), max_msgs, 0);
@@ -245,6 +299,7 @@ usb_osal_mq_t usb_osal_mq_create(uint32_t max_msgs){
 }
 
 
+
 void usb_osal_mq_delete(usb_osal_mq_t mq){
     osal_msg_queue_t osal_mq;
     osal_mq = (osal_msg_queue_t)mq;
@@ -254,6 +309,7 @@ void usb_osal_mq_delete(usb_osal_mq_t mq){
 }
 
 
+
 int usb_osal_mq_recv(usb_osal_mq_t mq, uintptr_t *addr, uint32_t timeout){
     return osal_mq_recv((osal_msg_queue_t)mq, addr, sizeof(uintptr_t *), timeout);
 }