Initial commit

1 files changed, 251 insertions, 0 deletions

diff --git a/common/src/tx_semaphore_prioritize.c b/common/src/tx_semaphore_prioritize.c
new file mode 100644
index 00000000..0072b7e6
--- /dev/null
+++ b/common/src/tx_semaphore_prioritize.c
@@ -0,0 +1,251 @@
+/**************************************************************************/
+/*                                                                        */
+/*       Copyright (c) Microsoft Corporation. All rights reserved.        */
+/*                                                                        */
+/*       This software is licensed under the Microsoft Software License   */
+/*       Terms for Microsoft Azure RTOS. Full text of the license can be  */
+/*       found in the LICENSE file at https://aka.ms/AzureRTOS_EULA       */
+/*       and in the root directory of this software.                      */
+/*                                                                        */
+/**************************************************************************/
+
+
+/**************************************************************************/
+/**************************************************************************/
+/**                                                                       */
+/** ThreadX Component                                                     */
+/**                                                                       */
+/**   Semaphore                                                           */
+/**                                                                       */
+/**************************************************************************/
+/**************************************************************************/
+
+#define TX_SOURCE_CODE
+
+
+/* Include necessary system files.  */
+
+#include "tx_api.h"
+#include "tx_trace.h"
+#include "tx_thread.h"
+#include "tx_semaphore.h"
+
+
+/**************************************************************************/
+/*                                                                        */
+/*  FUNCTION                                               RELEASE        */
+/*                                                                        */
+/*    _tx_semaphore_prioritize                            PORTABLE C      */
+/*                                                           6.0          */
+/*  AUTHOR                                                                */
+/*                                                                        */
+/*    William E. Lamie, Microsoft Corporation                             */
+/*                                                                        */
+/*  DESCRIPTION                                                           */
+/*                                                                        */
+/*    This function places the highest priority suspended thread at the   */
+/*    front of the suspension list.  All other threads remain in the same */
+/*    FIFO suspension order.                                              */
+/*                                                                        */
+/*  INPUT                                                                 */
+/*                                                                        */
+/*    semaphore_ptr                     Pointer to semaphore control block*/
+/*                                                                        */
+/*  OUTPUT                                                                */
+/*                                                                        */
+/*    status                            Completion status                 */
+/*                                                                        */
+/*  CALLS                                                                 */
+/*                                                                        */
+/*    _tx_thread_system_preempt_check   Check for preemption              */
+/*                                                                        */
+/*  CALLED BY                                                             */
+/*                                                                        */
+/*    Application Code                                                    */
+/*                                                                        */
+/*  RELEASE HISTORY                                                       */
+/*                                                                        */
+/*    DATE              NAME                      DESCRIPTION             */
+/*                                                                        */
+/*  05-19-2020     William E. Lamie         Initial Version 6.0           */
+/*                                                                        */
+/**************************************************************************/
+UINT  _tx_semaphore_prioritize(TX_SEMAPHORE *semaphore_ptr)
+{
+
+TX_INTERRUPT_SAVE_AREA
+
+TX_THREAD       *thread_ptr;
+TX_THREAD       *priority_thread_ptr;
+TX_THREAD       *head_ptr;
+UINT            suspended_count;
+TX_THREAD       *next_thread;
+TX_THREAD       *previous_thread;
+UINT            list_changed;
+
+
+    /* Disable interrupts.  */
+    TX_DISABLE
+
+    /* If trace is enabled, insert this event into the trace buffer.  */
+    TX_TRACE_IN_LINE_INSERT(TX_TRACE_SEMAPHORE_PRIORITIZE, semaphore_ptr, semaphore_ptr -> tx_semaphore_suspended_count, TX_POINTER_TO_ULONG_CONVERT(&suspended_count), 0, TX_TRACE_SEMAPHORE_EVENTS)
+
+    /* Log this kernel call.  */
+    TX_EL_SEMAPHORE_PRIORITIZE_INSERT
+
+    /* Pickup the suspended count.  */
+    suspended_count =  semaphore_ptr -> tx_semaphore_suspended_count;
+
+    /* Determine if there are fewer than 2 suspended threads.  */
+    if (suspended_count < ((UINT) 2))
+    {
+
+        /* Restore interrupts.  */
+        TX_RESTORE
+    }
+
+    /* Determine if there how many threads are suspended on this semaphore.  */
+    else if (suspended_count == ((UINT) 2))
+    {
+
+        /* Pickup the head pointer and the next pointer.  */
+        head_ptr =  semaphore_ptr -> tx_semaphore_suspension_list;
+        next_thread =  head_ptr -> tx_thread_suspended_next;
+
+        /* Determine if the next suspended thread has a higher priority.  */
+        if ((next_thread -> tx_thread_priority) < (head_ptr -> tx_thread_priority))
+        {
+
+            /* Yes, move the list head to the next thread.  */
+            semaphore_ptr -> tx_semaphore_suspension_list =  next_thread;
+        }
+
+        /* Restore interrupts.  */
+        TX_RESTORE
+    }
+    else
+    {
+
+        /* Remember the suspension count and head pointer.  */
+        head_ptr =   semaphore_ptr -> tx_semaphore_suspension_list;
+
+        /* Default the highest priority thread to the thread at the front of the list.  */
+        priority_thread_ptr =  head_ptr;
+
+        /* Setup search pointer.  */
+        thread_ptr =  priority_thread_ptr -> tx_thread_suspended_next;
+
+        /* Disable preemption.  */
+        _tx_thread_preempt_disable++;
+
+        /* Set the list changed flag to false.  */
+        list_changed =  TX_FALSE;
+
+        /* Search through the list to find the highest priority thread.  */
+        do
+        {
+
+            /* Is the current thread higher priority?  */
+            if (thread_ptr -> tx_thread_priority < priority_thread_ptr -> tx_thread_priority)
+            {
+
+                /* Yes, remember that this thread is the highest priority.  */
+                priority_thread_ptr =  thread_ptr;
+            }
+
+            /* Restore interrupts temporarily.  */
+            TX_RESTORE
+
+            /* Disable interrupts again.  */
+            TX_DISABLE
+
+            /* Determine if any changes to the list have occurred while 
+               interrupts were enabled.  */
+              
+            /* Is the list head the same?  */
+            if (head_ptr != semaphore_ptr -> tx_semaphore_suspension_list)
+            {
+            
+                /* The list head has changed, set the list changed flag.  */
+                list_changed =  TX_TRUE;
+            }
+            else
+            {
+            
+                /* Is the suspended count the same?  */
+                if (suspended_count != semaphore_ptr -> tx_semaphore_suspended_count)
+                {
+              
+                    /* The list head has changed, set the list changed flag.  */
+                    list_changed =  TX_TRUE;
+                }
+            }
+             
+            /* Determine if the list has changed.  */
+            if (list_changed == TX_FALSE)
+            {
+
+                /* Yes, everything is the same... move the thread pointer to the next thread.  */
+                thread_ptr =  thread_ptr -> tx_thread_suspended_next;
+            }
+            else
+            {
+
+                /* No, the list is been modified so we need to start the search over.  */
+
+                /* Save the suspension count and head pointer.  */
+                head_ptr =   semaphore_ptr -> tx_semaphore_suspension_list;
+                suspended_count =  semaphore_ptr -> tx_semaphore_suspended_count;
+
+                /* Default the highest priority thread to the thread at the front of the list.  */
+                priority_thread_ptr =  head_ptr;
+
+                /* Setup search pointer.  */
+                thread_ptr =  priority_thread_ptr -> tx_thread_suspended_next;
+
+                /* Reset the list changed flag.  */
+                list_changed =  TX_FALSE;
+            }
+
+        } while (thread_ptr != head_ptr);
+
+        /* Release preemption.  */
+        _tx_thread_preempt_disable--;
+
+        /* Now determine if the highest priority thread is at the front 
+           of the list.  */
+        if (priority_thread_ptr != head_ptr)
+        {
+
+            /* No, we need to move the highest priority suspended thread to the 
+               front of the list.  */
+
+            /* First, remove the highest priority thread by updating the
+               adjacent suspended threads.  */
+            next_thread =                                  priority_thread_ptr -> tx_thread_suspended_next;
+            previous_thread =                              priority_thread_ptr -> tx_thread_suspended_previous;
+            next_thread -> tx_thread_suspended_previous =  previous_thread;
+            previous_thread -> tx_thread_suspended_next =  next_thread;
+
+            /* Now, link the highest priority thread at the front of the list.  */
+            previous_thread =                                      head_ptr -> tx_thread_suspended_previous;
+            priority_thread_ptr -> tx_thread_suspended_next =      head_ptr;
+            priority_thread_ptr -> tx_thread_suspended_previous =  previous_thread;
+            previous_thread -> tx_thread_suspended_next =          priority_thread_ptr;
+            head_ptr -> tx_thread_suspended_previous =             priority_thread_ptr;
+
+            /* Move the list head pointer to the highest priority suspended thread.  */
+            semaphore_ptr -> tx_semaphore_suspension_list =  priority_thread_ptr;
+        }
+
+        /* Restore interrupts.  */
+        TX_RESTORE
+
+        /* Check for preemption.  */
+        _tx_thread_system_preempt_check();
+    }
+
+    /* Return completion status.  */
+    return(TX_SUCCESS);
+}
+