From f14a04c6ef5cde3f812aefd9827eddcf4cf2f13e Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Fri, 24 Nov 2017 06:38:07 +0100
Subject: Add RTEMS thread API

Update #2843.
---
 cpukit/score/src/semaphore.c | 99 ++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 96 insertions(+), 3 deletions(-)

(limited to 'cpukit/score/src/semaphore.c')

diff --git a/cpukit/score/src/semaphore.c b/cpukit/score/src/semaphore.c
index 4edd25a387..f76ee332a4 100644
--- a/cpukit/score/src/semaphore.c
+++ b/cpukit/score/src/semaphore.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2015, 2017 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -18,8 +18,9 @@
 
 #include <rtems/score/semaphoreimpl.h>
 #include <rtems/score/statesimpl.h>
+#include <rtems/score/threadimpl.h>
 
-#include <limits.h>
+#include <errno.h>
 
 RTEMS_STATIC_ASSERT(
   offsetof( Sem_Control, Queue )
@@ -71,6 +72,66 @@ void _Semaphore_Wait( struct _Semaphore_Control *_sem )
   }
 }
 
+int _Semaphore_Wait_timed_ticks( struct _Semaphore_Control *_sem, uint32_t ticks )
+{
+  Sem_Control          *sem;
+  ISR_Level             level;
+  Thread_queue_Context  queue_context;
+  Thread_Control       *executing;
+  unsigned int          count;
+
+  sem = _Sem_Get( _sem );
+  _Thread_queue_Context_initialize( &queue_context );
+  _Thread_queue_Context_ISR_disable( &queue_context, level );
+  executing = _Sem_Queue_acquire_critical( sem, &queue_context );
+
+  count = sem->count;
+  if ( __predict_true( count > 0 ) ) {
+    sem->count = count - 1;
+    _Sem_Queue_release( sem, level, &queue_context );
+    return 0;
+  } else {
+    _Thread_queue_Context_set_thread_state(
+      &queue_context,
+      STATES_WAITING_FOR_SEMAPHORE
+    );
+    _Thread_queue_Context_set_enqueue_timeout_ticks( &queue_context, ticks );
+    _Thread_queue_Context_set_ISR_level( &queue_context, level );
+    _Thread_queue_Enqueue(
+      &sem->Queue.Queue,
+      SEMAPHORE_TQ_OPERATIONS,
+      executing,
+      &queue_context
+    );
+    return STATUS_GET_POSIX( _Thread_Wait_get_status( executing ) );
+  }
+}
+
+int _Semaphore_Try_wait( struct _Semaphore_Control *_sem )
+{
+  Sem_Control          *sem;
+  ISR_Level             level;
+  Thread_queue_Context  queue_context;
+  unsigned int          count;
+  int                   eno;
+
+  sem = _Sem_Get( _sem );
+  _Thread_queue_Context_initialize( &queue_context );
+  _Thread_queue_Context_ISR_disable( &queue_context, level );
+  _Sem_Queue_acquire_critical( sem, &queue_context );
+
+  count = sem->count;
+  if ( __predict_true( count > 0 ) ) {
+    sem->count = count - 1;
+    eno = 0;
+  } else {
+    eno = EAGAIN;
+  }
+
+  _Sem_Queue_release( sem, level, &queue_context );
+  return eno;
+}
+
 void _Semaphore_Post( struct _Semaphore_Control *_sem )
 {
   Sem_Control          *sem;
@@ -85,7 +146,6 @@ void _Semaphore_Post( struct _Semaphore_Control *_sem )
 
   heads = sem->Queue.Queue.heads;
   if ( __predict_true( heads == NULL ) ) {
-    _Assert( sem->count < UINT_MAX );
     ++sem->count;
     _Sem_Queue_release( sem, level, &queue_context );
   } else {
@@ -104,3 +164,36 @@ void _Semaphore_Post( struct _Semaphore_Control *_sem )
     );
   }
 }
+
+void _Semaphore_Post_binary( struct _Semaphore_Control *_sem )
+{
+  Sem_Control          *sem;
+  ISR_Level             level;
+  Thread_queue_Context  queue_context;
+  Thread_queue_Heads   *heads;
+
+  sem = _Sem_Get( _sem );
+  _Thread_queue_Context_initialize( &queue_context );
+  _Thread_queue_Context_ISR_disable( &queue_context, level );
+  _Sem_Queue_acquire_critical( sem, &queue_context );
+
+  heads = sem->Queue.Queue.heads;
+  if ( __predict_true( heads == NULL ) ) {
+    sem->count = 1;
+    _Sem_Queue_release( sem, level, &queue_context );
+  } else {
+    const Thread_queue_Operations *operations;
+    Thread_Control *first;
+
+    _Thread_queue_Context_set_ISR_level( &queue_context, level );
+    operations = SEMAPHORE_TQ_OPERATIONS;
+    first = ( *operations->first )( heads );
+
+    _Thread_queue_Extract_critical(
+      &sem->Queue.Queue,
+      operations,
+      first,
+      &queue_context
+    );
+  }
+}
-- 
cgit v1.2.3