Valid 3 code version

Ubiquitous/XiZi_AIoT/softkernel/task/semaphore.c

@@ -24,29 +24,28 @@
 void semaphore_pool_init(struct XiziSemaphorePool* sem_pool)
 {
     assert(sem_pool != NULL);
-    sem_pool->next_sem_id = 1;
+    sem_pool->next_sem_id = INVALID_SEM_ID + 1;
     slab_init(&sem_pool->allocator, sizeof(struct ksemaphore));
     doubleListNodeInit(&sem_pool->sem_list_guard);
+    rbtree_init(&sem_pool->sem_pool_map);
+    sem_pool->nr_sem = 0;
 }
 
-static inline struct ksemaphore* ksemaphore_get_by_id(struct XiziSemaphorePool* sem_pool, int sem_id)
+static inline struct ksemaphore* ksemaphore_get_by_id(struct XiziSemaphorePool* sem_pool, sem_id_t sem_id)
 {
-    struct ksemaphore* sem = NULL;
-    DOUBLE_LIST_FOR_EACH_ENTRY(sem, &sem_pool->sem_list_guard, sem_list_node)
-    {
-        if (sem->id == sem_id) {
-            return sem;
-        }
+    RbtNode* target_sem_node = rbt_search(&sem_pool->sem_pool_map, sem_id);
+    if (target_sem_node == NULL) {
+        return NULL;
     }
-    return NULL;
+    return (struct ksemaphore*)target_sem_node->data;
 }
 
-int ksemaphore_alloc(struct XiziSemaphorePool* sem_pool, int val)
+sem_id_t ksemaphore_alloc(struct XiziSemaphorePool* sem_pool, sem_val_t val)
 {
     struct ksemaphore* sem = (struct ksemaphore*)slab_alloc(&sem_pool->allocator);
     if (sem == NULL) {
         ERROR("No memeory to alloc new semaphore.\n");
-        return -1;
+        return INVALID_SEM_ID;
     }
 
     /* No error down here */
@@ -55,19 +54,34 @@ int ksemaphore_alloc(struct XiziSemaphorePool* sem_pool, int val)
     sem->id = sem_pool->next_sem_id++;
     if (UNLIKELY(sem->id == 0)) {
         slab_free(&sem_pool->allocator, sem);
-        return -1;
+        return INVALID_SEM_ID;
     }
     sem->val = val;
     doubleListNodeInit(&sem->sem_list_node);
     doubleListNodeInit(&sem->wait_list_guard);
 
-    /* list sem to sem_pool */
+    rbt_insert(&sem_pool->sem_pool_map, sem->id, sem);
     doubleListAddOnHead(&sem->sem_list_node, &sem_pool->sem_list_guard);
+    sem_pool->nr_sem++;
 
     return sem->id;
 }
 
-bool ksemaphore_wait(struct XiziSemaphorePool* sem_pool, struct Thread* thd, uint32_t sem_id)
+bool ksemaphore_consume(struct XiziSemaphorePool* sem_pool, sem_id_t sem_id, sem_val_t decre)
+{
+    struct ksemaphore* sem = ksemaphore_get_by_id(sem_pool, sem_id);
+    // invalid sem id
+    if (sem == NULL) {
+        return false;
+    }
+
+    // if (decre >= 0) {
+    sem->val -= decre;
+    // }
+    return true;
+}
+
+bool ksemaphore_wait(struct XiziSemaphorePool* sem_pool, struct Thread* thd, sem_id_t sem_id)
 {
     assert(thd != NULL);
     assert(thd->state == RUNNING);
@@ -77,6 +91,7 @@ bool ksemaphore_wait(struct XiziSemaphorePool* sem_pool, struct Thread* thd, uin
     if (sem == NULL) {
         return false;
     }
+    // DEBUG("%s waiting sem %lu(%d), nr_sem: %d I\n", thd->name, sem_id, sem->val, sem_pool->nr_sem);
 
     // no need to wait
     if (sem->val > 0) {
@@ -91,7 +106,7 @@ bool ksemaphore_wait(struct XiziSemaphorePool* sem_pool, struct Thread* thd, uin
     return true;
 }
 
-bool ksemaphore_signal(struct XiziSemaphorePool* sem_pool, uint32_t sem_id)
+bool ksemaphore_signal(struct XiziSemaphorePool* sem_pool, sem_id_t sem_id)
 {
     /* find sem */
     struct ksemaphore* sem = ksemaphore_get_by_id(sem_pool, sem_id);
@@ -105,6 +120,7 @@ bool ksemaphore_signal(struct XiziSemaphorePool* sem_pool, uint32_t sem_id)
             struct Thread* thd = CONTAINER_OF(sem->wait_list_guard.next, struct Thread, node);
             assert(thd != NULL && thd->state == BLOCKED);
             xizi_task_manager.task_unblock(thd);
+            // DEBUG("waking %s\n", thd->name);
         }
     }
 
@@ -112,7 +128,20 @@ bool ksemaphore_signal(struct XiziSemaphorePool* sem_pool, uint32_t sem_id)
     return true;
 }
 
-bool ksemaphore_free(struct XiziSemaphorePool* sem_pool, uint32_t sem_id)
+bool ksemaphore_signal_no_wake(struct XiziSemaphorePool* sem_pool, sem_id_t sem_id)
+{
+    /* find sem */
+    struct ksemaphore* sem = ksemaphore_get_by_id(sem_pool, sem_id);
+    // invalid sem id
+    if (sem == NULL) {
+        return false;
+    }
+
+    sem->val++;
+    return true;
+}
+
+bool ksemaphore_free(struct XiziSemaphorePool* sem_pool, sem_id_t sem_id)
 {
     /* find sem */
     struct ksemaphore* sem = ksemaphore_get_by_id(sem_pool, sem_id);
@@ -128,8 +157,10 @@ bool ksemaphore_free(struct XiziSemaphorePool* sem_pool, uint32_t sem_id)
         xizi_task_manager.task_unblock(thd);
     }
 
+    rbt_delete(&sem_pool->sem_pool_map, sem_id);
     doubleListDel(&sem->sem_list_node);
     slab_free(&sem_pool->allocator, sem);
+    sem_pool->nr_sem--;
 
     return true;
 }