Module: xenomai-gch
Branch: for-forge
Commit: 8c1b69a6945fb70cfce579c517a1d534abb6fad8
URL:
http://git.xenomai.org/?p=xenomai-gch.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
