From 8d6c46c7cd626f4f5ee24d9288aa05d9e516e014 Mon Sep 17 00:00:00 2001
From: Rahul Kar <karahulx@amazon.com>
Date: Fri, 17 Jan 2025 11:26:28 +0000
Subject: [PATCH] Add semaphore APIs for benchmarking

---
 include/light-weight-mutex.h |   3 +
 light-weight-mutex.c         | 112 +++++++++++++++++++++++++++++++----
 2 files changed, 105 insertions(+), 10 deletions(-)

diff --git a/include/light-weight-mutex.h b/include/light-weight-mutex.h
index b0282044c7..14416274d7 100644
--- a/include/light-weight-mutex.h
+++ b/include/light-weight-mutex.h
@@ -53,6 +53,9 @@ void vRemoveMutexItemFromList( void * pvMutexHandle );
 BaseType_t lightMutexTake( LightWeightMutex_t * xMutex,
                            TickType_t xTicksToWait );
 BaseType_t lightMutexGive( LightWeightMutex_t * xMutex );
+BaseType_t lightSemaphoreTake( LightWeightMutex_t * xMutex,
+                           TickType_t xTicksToWait );
+BaseType_t lightSemaphoreGive( LightWeightMutex_t * xMutex );
 
 /* *INDENT-OFF* */
 #if defined( __cplusplus )
diff --git a/light-weight-mutex.c b/light-weight-mutex.c
index 75dcae9144..af563bad16 100644
--- a/light-weight-mutex.c
+++ b/light-weight-mutex.c
@@ -35,6 +35,7 @@
     {
         TaskHandle_t currentTask = xTaskGetCurrentTaskHandle();
         BaseType_t xReturn = pdFALSE;
+        BaseType_t xInheritanceOccurred = pdFALSE;
         TickType_t startTime = xTaskGetTickCount();
 
         /* Check the pxMutex pointer is not NULL. */
@@ -51,7 +52,6 @@
                     pxMutex->lock_count = 1;
                     if( uxTaskPriorityGet( currentTask ) < pxMutex->uxCeilingPriority )
                     {
-                        BaseType_t xInheritanceOccurred = pdFALSE;
                         vInsertMutexToHolderList( currentTask, &( pxMutex->xMutexHolderListItem ) );
                         xInheritanceOccurred = xTaskCeilingPriorityInherit( pxMutex->uxCeilingPriority );
                     }
@@ -77,16 +77,16 @@
                         /* Get the ceiling priority of next mutex held.
                          * If it not there set to base priority.
                          */
-                        // LightWeightMutex_t * pxNextMutex = pvRemoveMutexToHolderList( ( void * const ) pxMutex );
+                        LightWeightMutex_t * pxNextMutex = pvRemoveMutexToHolderList( ( void * const ) pxMutex );
 
-                        // if( pxNextMutex != NULL )
-                        // {
-                        //     xTaskCeilingPriorityDisInherit( pxNextMutex->uxCeilingPriority );
-                        // }
-                        // else
-                        // {
-                        //     xTaskCeilingPriorityDisInheritToBasePrio();
-                        // }
+                        if( pxNextMutex != NULL )
+                        {
+                            xTaskCeilingPriorityDisInherit( pxNextMutex->uxCeilingPriority );
+                        }
+                        else
+                        {
+                            xTaskCeilingPriorityDisInheritToBasePrio();
+                        }
 
                         xReturn = pdFALSE;
                         taskEXIT_CRITICAL();
@@ -154,6 +154,98 @@
         return xReturn;
     }
 
+/*-----------------------------------------------------------*/
+
+    BaseType_t lightSemaphoreTake( LightWeightMutex_t * pxMutex, TickType_t xTicksToWait )
+    {
+        TaskHandle_t currentTask = xTaskGetCurrentTaskHandle();
+        BaseType_t xReturn = pdFALSE;
+        TickType_t startTime = xTaskGetTickCount();
+
+        /* Check the pxMutex pointer is not NULL. */
+        configASSERT( ( pxMutex != NULL ) && ( xTicksToWait != 0U ) );
+
+        for( ; ; )
+        {
+            taskENTER_CRITICAL();
+            {
+                if( pxMutex->lock_count == 0U )
+                {
+                    /*Atomic_Store_u32( &pxMutex->owner, ( uintptr_t ) currentTask ); */
+                    pxMutex->owner = ( uintptr_t ) currentTask;
+                    pxMutex->lock_count = 1;
+                    xReturn = pdTRUE;
+                    taskEXIT_CRITICAL();
+                    break;
+                }
+
+                /*if( ( uintptr_t ) Atomic_Load_u32( &pxMutex->owner ) == ( uintptr_t ) currentTask ) */
+                if( pxMutex->owner == ( uintptr_t ) currentTask )
+                {
+                    pxMutex->lock_count++;
+                    xReturn = pdTRUE;
+                    taskEXIT_CRITICAL();
+                    break;
+                }
+
+                if( xTicksToWait != portMAX_DELAY )
+                {
+                    /* Timed out */
+                    if( ( xTaskGetTickCount() - startTime ) >= xTicksToWait )
+                    {
+                        xReturn = pdFALSE;
+                        taskEXIT_CRITICAL();
+                        break;
+                    }
+                }
+
+                vTaskPlaceOnEventList( &( pxMutex->xTasksWaitingForMutex ), xTicksToWait );
+                taskYIELD();
+            }
+            taskEXIT_CRITICAL();
+        }
+
+        return xReturn;
+    }
+
+/*-----------------------------------------------------------*/
+
+    BaseType_t lightSemaphoreGive( LightWeightMutex_t * pxMutex )
+    {
+        /* Check the pxMutex pointer is not NULL and the mutex has already been taken earlier. */
+        BaseType_t xReturn = pdTRUE;
+
+        configASSERT( ( pxMutex != NULL ) || ( pxMutex->lock_count != 0U ) );
+
+        /*if( ( uintptr_t ) Atomic_Load_u32( &pxMutex->owner ) != ( uintptr_t ) xTaskGetCurrentTaskHandle() ) */
+        if( pxMutex->owner != ( uintptr_t ) xTaskGetCurrentTaskHandle() )
+        {
+            xReturn = pdFALSE;
+        }
+        else
+        {
+            taskENTER_CRITICAL();
+            {
+                pxMutex->lock_count--;
+
+                if( pxMutex->lock_count == 0U )
+                {
+                    /* Update the current owner of the mutex to 0. */
+
+                    /*Atomic_Store_u32( &pxMutex->owner, ( uintptr_t ) 0U ); */
+                    pxMutex->owner = ( uintptr_t ) 0U;
+                    /* The mutex is no longer being held. */
+                    xReturn = pdTRUE;
+                    /* Get the new owner, if any. */
+                    prvAssignLWMutexOwner( pxMutex );
+                }
+            }
+            taskEXIT_CRITICAL();
+        }
+
+        return xReturn;
+    }
+
 /*-----------------------------------------------------------*/
 
 /* This entire source file will be skipped if the application is not configured