Module: xenomai-forge
Branch: master
Commit: 8c1b69a6945fb70cfce579c517a1d534abb6fad8
URL:    
http://git.xenomai.org/?p=xenomai-forge.git;a=commit;h=8c1b69a6945fb70cfce579c517a1d534abb6fad8

Author: Gilles Chanteperdrix <gilles.chanteperd...@xenomai.org>
Date:   Sun Dec  8 18:51:57 2013 +0100

cobalt/semaphore: use semaphore count in user-space

---

 lib/cobalt/semaphore.c |  141 ++++++++++++++++++++++++++++++++++--------------
 1 file changed, 102 insertions(+), 39 deletions(-)

diff --git a/lib/cobalt/semaphore.c b/lib/cobalt/semaphore.c
index c3a2668..fde4bfd 100644
--- a/lib/cobalt/semaphore.c
+++ b/lib/cobalt/semaphore.c
@@ -25,6 +25,18 @@
 #include <asm/xenomai/syscall.h>
 #include "internal.h"
 
+static inline struct sem_dat *sem_get_datp(struct __shadow_sem *shadow)
+{
+       unsigned pshared = shadow->datp_offset < 0;
+       
+       if (pshared) 
+               return (struct sem_dat *)
+                       (cobalt_sem_heap[1] - shadow->datp_offset);
+       else
+               return (struct sem_dat *)
+                       (cobalt_sem_heap[0] + shadow->datp_offset);
+}
+
 COBALT_IMPL(int, sem_init, (sem_t *sem, int pshared, unsigned value))
 {
        struct __shadow_sem *_sem = &((union cobalt_sem_union 
*)sem)->shadow_sem;
@@ -32,12 +44,13 @@ COBALT_IMPL(int, sem_init, (sem_t *sem, int pshared, 
unsigned value))
 
        err = -XENOMAI_SKINCALL3(__cobalt_muxid,
                                 sc_cobalt_sem_init, _sem, pshared, value);
-       if (!err)
-               return 0;
-
-       errno = err;
-
-       return -1;
+       if (err < 0) {
+               errno = err;
+               return -1;
+       }
+       
+       __cobalt_prefault(sem_get_datp(_sem));
+       return 0;
 }
 
 COBALT_IMPL(int, sem_destroy, (sem_t *sem))
@@ -62,6 +75,8 @@ COBALT_IMPL(int, sem_destroy, (sem_t *sem))
 COBALT_IMPL(int, sem_post, (sem_t *sem))
 {
        struct __shadow_sem *_sem = &((union cobalt_sem_union 
*)sem)->shadow_sem;
+       struct sem_dat *datp;
+       long value;
        int err;
 
        if (_sem->magic != COBALT_SEM_MAGIC
@@ -70,6 +85,29 @@ COBALT_IMPL(int, sem_post, (sem_t *sem))
                return -1;
        }
 
+       datp = sem_get_datp(_sem);
+
+       value = atomic_long_read(&datp->value);
+
+       if (value >= 0) {
+               long old, new;
+               
+               if (datp->flags & SEM_PULSE)
+                       return 0;
+
+               do {
+                       old = value;
+                       new = value + 1;
+
+                       value = atomic_long_cmpxchg(&datp->value, old, new);
+                       if (value < 0)
+                               goto do_syscall;
+               } while (value != old);
+
+               return 0;
+       }       
+
+  do_syscall:
        err = -XENOMAI_SKINCALL1(__cobalt_muxid, sc_cobalt_sem_post, _sem);
        if (!err)
                return 0;
@@ -78,10 +116,11 @@ COBALT_IMPL(int, sem_post, (sem_t *sem))
        return -1;
 }
 
-COBALT_IMPL(int, sem_wait, (sem_t *sem))
+COBALT_IMPL(int, sem_trywait, (sem_t *sem))
 {
        struct __shadow_sem *_sem = &((union cobalt_sem_union 
*)sem)->shadow_sem;
-       int err, oldtype;
+       struct sem_dat *datp;
+       long value;
 
        if (_sem->magic != COBALT_SEM_MAGIC
            && _sem->magic != COBALT_NAMED_SEM_MAGIC) {
@@ -89,56 +128,70 @@ COBALT_IMPL(int, sem_wait, (sem_t *sem))
                return -1;
        }
 
-       pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
+       datp = sem_get_datp(_sem);
 
-       err = -XENOMAI_SKINCALL1(__cobalt_muxid, sc_cobalt_sem_wait, _sem);
+       value = atomic_long_read(&datp->value);
 
-       pthread_setcanceltype(oldtype, NULL);
+       if (value > 0) {
+               long old, new;
 
-       if (!err)
+               do {
+                       old = value;
+                       new = value - 1;
+                       
+                       value = atomic_long_cmpxchg(&datp->value, old, new);
+                       if (value <= 0)
+                               goto eagain;
+               } while (value != old);
+               
                return 0;
+       }
 
-       errno = err;
+  eagain:
+       errno = EAGAIN;
        return -1;
 }
 
-COBALT_IMPL(int, sem_timedwait, (sem_t *sem, const struct timespec *ts))
+COBALT_IMPL(int, sem_wait, (sem_t *sem))
 {
        struct __shadow_sem *_sem = &((union cobalt_sem_union 
*)sem)->shadow_sem;
        int err, oldtype;
 
-       if (_sem->magic != COBALT_SEM_MAGIC
-           && _sem->magic != COBALT_NAMED_SEM_MAGIC) {
-               errno = EINVAL;
-               return -1;
-       }
-
+       err = __RT(sem_trywait(sem));
+       
+       if (err != -1 || errno != EAGAIN)
+               return err;
+       
        pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
 
-       err = -XENOMAI_SKINCALL2(__cobalt_muxid,
-                                sc_cobalt_sem_timedwait, _sem, ts);
+       err = -XENOMAI_SKINCALL1(__cobalt_muxid, sc_cobalt_sem_wait, _sem);
 
        pthread_setcanceltype(oldtype, NULL);
 
-       if (!err)
+       if (err == 0)
                return 0;
-
+       
        errno = err;
        return -1;
 }
 
-COBALT_IMPL(int, sem_trywait, (sem_t *sem))
+COBALT_IMPL(int, sem_timedwait, (sem_t *sem, const struct timespec *ts))
 {
        struct __shadow_sem *_sem = &((union cobalt_sem_union 
*)sem)->shadow_sem;
-       int err;
+       int err, oldtype;
 
-       if (_sem->magic != COBALT_SEM_MAGIC
-           && _sem->magic != COBALT_NAMED_SEM_MAGIC) {
-               errno = EINVAL;
-               return -1;
-       }
+       err = __RT(sem_trywait(sem));
+       
+       if (err != -1 || errno != EAGAIN)
+               return err;     
+
+       pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
+
+       err = -XENOMAI_SKINCALL2(__cobalt_muxid,
+                                sc_cobalt_sem_timedwait, _sem, ts);
+
+       pthread_setcanceltype(oldtype, NULL);
 
-       err = -XENOMAI_SKINCALL1(__cobalt_muxid, sc_cobalt_sem_trywait, _sem);
        if (!err)
                return 0;
 
@@ -149,7 +202,8 @@ COBALT_IMPL(int, sem_trywait, (sem_t *sem))
 COBALT_IMPL(int, sem_getvalue, (sem_t *sem, int *sval))
 {
        struct __shadow_sem *_sem = &((union cobalt_sem_union 
*)sem)->shadow_sem;
-       int err;
+       struct sem_dat *datp;
+       long value;
 
        if (_sem->magic != COBALT_SEM_MAGIC
            && _sem->magic != COBALT_NAMED_SEM_MAGIC) {
@@ -157,13 +211,14 @@ COBALT_IMPL(int, sem_getvalue, (sem_t *sem, int *sval))
                return -1;
        }
 
-       err = -XENOMAI_SKINCALL2(__cobalt_muxid,
-                                sc_cobalt_sem_getvalue, _sem, sval);
-       if (!err)
-               return 0;
+       datp = sem_get_datp(_sem);
 
-       errno = err;
-       return -1;
+       value = atomic_long_read(&datp->value);
+       if (value < 0 && (datp->flags & SEM_REPORT) == 0)
+               value = 0;
+
+       *sval = value;
+       return 0;
 }
 
 COBALT_IMPL(sem_t *, sem_open, (const char *name, int oflags, ...))
@@ -257,6 +312,8 @@ int sem_init_np(sem_t *sem, int flags, unsigned int value)
 int sem_broadcast_np(sem_t *sem)
 {
        struct __shadow_sem *_sem = &((union cobalt_sem_union 
*)sem)->shadow_sem;
+       struct sem_dat *datp;
+       long value;
        int err;
 
        if (_sem->magic != COBALT_SEM_MAGIC
@@ -265,6 +322,12 @@ int sem_broadcast_np(sem_t *sem)
                return -1;
        }
 
+       datp = sem_get_datp(_sem);
+
+       value = atomic_long_read(&datp->value);
+       if (value >= 0)
+               return 0;
+
        err = -XENOMAI_SKINCALL1(__cobalt_muxid,
                                 sc_cobalt_sem_broadcast_np, _sem);
        if (!err)


_______________________________________________
Xenomai-git mailing list
Xenomai-git@xenomai.org
http://www.xenomai.org/mailman/listinfo/xenomai-git

Reply via email to