Add tryEnter() to Rodos semaphore

api/rodos-semaphore.h

@@ -47,6 +47,11 @@ class Semaphore {
    ** The owner may reenter the semaphore without deadlock */
   void enter();
 
+  /** Caller will return false if semaphore is occupied,
+   *  true if the caller obtained the lock.
+   ** The owner may reenter the semaphore without deadlock */
+  bool tryEnter();
+
   /** caller does not block. Resumes one waiting thread (enter) */
   void leave();
 

src/bare-metal-generic/rodos-semaphore.cpp

@@ -50,6 +50,23 @@ void Semaphore::enter() {
   caller->yield(); // wating with prio_ceiling, maybe some one more important wants to work?
 }
 
+bool Semaphore::tryEnter() {
+  Thread* caller = Thread::getCurrentThread();
+  int32_t callerPriority = caller->getPriority();
+  {
+    PRIORITY_CEILER_IN_SCOPE();
+    // Check if semaphore is occupied by another thread
+    if ((owner != 0) && (owner != caller) ) {
+      return false;
+    }
+    owner = caller;
+    ownerPriority = callerPriority;
+    ownerEnterCnt++;
+  } // end of prio_ceiling
+  caller->yield();
+  return true;
+}
+
 /**
  *  caller does not block. resumes one waiting thread (enter)
  */

src/on-posix/rodos-semaphore.cpp

@@ -45,15 +45,36 @@ Semaphore::Semaphore() :
 void Semaphore::enter() {
   Thread* caller = Thread::getCurrentThread();
   if(owner == caller) {
-	ownerEnterCnt++;
-	return;
+    ownerEnterCnt++;
+    return;
   }
   pthread_mutex_t *mutexp = (pthread_mutex_t*)context;
   pthread_mutex_lock(mutexp);
   owner =  caller;
   ownerEnterCnt = 1;
 }
 
+/**
+* Caller will return false if semaphore is occupied,
+* true if he obtained the lock.
+* Owner may always re-enter.
+*/
+bool Semaphore::tryEnter() {
+  Thread* caller = Thread::getCurrentThread();
+  if(owner == caller) {
+    ownerEnterCnt++;
+    return true;
+  }
+  pthread_mutex_t *mutexp = (pthread_mutex_t*)context;
+  if(pthread_mutex_trylock(mutexp) == 0){
+    // Success
+    owner =  caller;
+    ownerEnterCnt = 1;
+    return true;
+  }
+  return false;
+}
+
 /**
 *  caller does not block. resumes one waiting trhead (enter)
 */