Module: xenomai-forge
Branch: next
Commit: 454e7600ed19d7ddf0601420b08e7eda5768c0e8
URL:    
http://git.xenomai.org/?p=xenomai-forge.git;a=commit;h=454e7600ed19d7ddf0601420b08e7eda5768c0e8

Author: Philippe Gerum <r...@xenomai.org>
Date:   Mon Feb 17 18:23:51 2014 +0100

cobalt/sem: introduce sc_cobalt_sem_inquire syscall

---

 include/cobalt/uapi/sem.h     |    6 +++
 include/cobalt/uapi/syscall.h |    5 ++-
 kernel/cobalt/posix/sem.c     |   81 +++++++++++++++++++++++++++++++++++++++++
 kernel/cobalt/posix/sem.h     |    5 +++
 kernel/cobalt/posix/syscall.c |    1 +
 5 files changed, 96 insertions(+), 2 deletions(-)

diff --git a/include/cobalt/uapi/sem.h b/include/cobalt/uapi/sem.h
index e011141..69fc989 100644
--- a/include/cobalt/uapi/sem.h
+++ b/include/cobalt/uapi/sem.h
@@ -39,6 +39,12 @@ union cobalt_sem_union {
        } shadow_sem;
 };
 
+struct cobalt_sem_info {
+       unsigned int value;
+       int flags;
+       int nrwait;
+};
+
 /* For Cobalt's sem_init_np() extension. */
 #define SEM_FIFO       0x1
 #define SEM_PULSE      0x2
diff --git a/include/cobalt/uapi/syscall.h b/include/cobalt/uapi/syscall.h
index d536c89..f1549f7 100644
--- a/include/cobalt/uapi/syscall.h
+++ b/include/cobalt/uapi/syscall.h
@@ -50,7 +50,7 @@
 #define sc_cobalt_cond_destroy          26
 #define sc_cobalt_cond_wait_prologue    27
 #define sc_cobalt_cond_wait_epilogue    28
-/* 30-31 unimplemented */
+/* 29-30 unimplemented */
 #define sc_cobalt_mq_open               31
 #define sc_cobalt_mq_close              32
 #define sc_cobalt_mq_unlink             33
@@ -78,7 +78,8 @@
 #define sc_cobalt_sigtimedwait         55
 #define sc_cobalt_sigpending           56
 #define sc_cobalt_kill                 57
-/* 58-60 unimplemented */
+#define sc_cobalt_sem_inquire           58
+/* 59-60 unimplemented */
 #define sc_cobalt_mutexattr_init        61
 #define sc_cobalt_mutexattr_destroy     62
 #define sc_cobalt_mutexattr_gettype     63
diff --git a/kernel/cobalt/posix/sem.c b/kernel/cobalt/posix/sem.c
index be336d4..e0d22fd 100644
--- a/kernel/cobalt/posix/sem.c
+++ b/kernel/cobalt/posix/sem.c
@@ -774,6 +774,87 @@ int cobalt_sem_broadcast_np(struct __shadow_sem __user 
*u_sem)
        return err;
 }
 
+int cobalt_sem_inquire(struct __shadow_sem __user *u_sem,
+                      struct cobalt_sem_info __user *u_info,
+                      pid_t __user *u_waitlist,
+                      size_t waitsz)
+{
+       int val, nrwait = 0, nrpids, ret = 0;
+       unsigned long pstamp, nstamp = 0;
+       struct cobalt_sem_info info;
+       pid_t *t = NULL, fbuf[16];
+       struct xnthread *thread;
+       struct cobalt_sem *sem;
+       xnhandle_t handle;
+       spl_t s;
+
+       __xn_get_user(handle, &u_sem->handle);
+
+       nrpids = waitsz / sizeof(pid_t);
+
+       xnlock_get_irqsave(&nklock, s);
+
+       for (;;) {
+               pstamp = nstamp;
+               sem = xnregistry_lookup(handle, &nstamp);
+               if (sem == NULL || sem->magic != COBALT_SEM_MAGIC) {
+                       xnlock_put_irqrestore(&nklock, s);
+                       return -EINVAL;
+               }
+               /*
+                * Allocate memory to return the wait list without
+                * holding any lock, then revalidate the handle.
+                */
+               if (t == NULL) {
+                       val = atomic_long_read(&sem->datp->value);
+                       if (val >= 0)
+                               break;
+                       xnlock_put_irqrestore(&nklock, s);
+                       if (nrpids > -val)
+                               nrpids = -val;
+                       if (-val <= ARRAY_SIZE(fbuf))
+                               t = fbuf; /* Use fast buffer. */
+                       else {
+                               t = xnmalloc(-val * sizeof(pid_t));
+                               if (t == NULL)
+                                       return -ENOMEM;
+                       }
+                       xnlock_get_irqsave(&nklock, s);
+               } else if (pstamp == nstamp)
+                       break;
+               else if (val != atomic_long_read(&sem->datp->value)) {
+                       xnlock_put_irqrestore(&nklock, s);
+                       if (t != fbuf)
+                               xnfree(t);
+                       t = NULL;
+                       xnlock_get_irqsave(&nklock, s);
+               }
+       }
+
+       info.flags = sem->flags;
+       info.value = (sem->flags & SEM_REPORT) || val >= 0 ? val : 0;
+       info.nrwait = val < 0 ? -val : 0;
+
+       if (xnsynch_pended_p(&sem->synchbase) && u_waitlist != NULL) {
+               xnsynch_for_each_sleeper(thread, &sem->synchbase) {
+                       if (nrwait >= nrpids)
+                               break;
+                       t[nrwait++] = xnthread_host_pid(thread);
+               }
+       }
+
+       xnlock_put_irqrestore(&nklock, s);
+
+       ret = __xn_safe_copy_to_user(u_info, &info, sizeof(info));
+       if (ret == 0 && nrwait > 0)
+               ret = __xn_safe_copy_to_user(u_waitlist, t, nrwait * 
sizeof(pid_t));
+
+       if (t && t != fbuf)
+               xnfree(t);
+
+       return ret ?: nrwait;
+}
+
 void cobalt_semq_cleanup(struct cobalt_kqueues *q)
 {
        struct cobalt_sem *sem, *tmp;
diff --git a/kernel/cobalt/posix/sem.h b/kernel/cobalt/posix/sem.h
index aca8154..ecf3894 100644
--- a/kernel/cobalt/posix/sem.h
+++ b/kernel/cobalt/posix/sem.h
@@ -97,6 +97,11 @@ int cobalt_sem_init_np(struct __shadow_sem __user *u_sem,
 
 int cobalt_sem_broadcast_np(struct __shadow_sem __user *u_sem);
 
+int cobalt_sem_inquire(struct __shadow_sem __user *u_sem,
+                      struct cobalt_sem_info __user *u_info,
+                      pid_t __user *u_waitlist,
+                      size_t waitsz);
+
 void cobalt_semq_cleanup(struct cobalt_kqueues *q);
 
 void cobalt_sem_pkg_init(void);
diff --git a/kernel/cobalt/posix/syscall.c b/kernel/cobalt/posix/syscall.c
index 4f4759b..5914469 100644
--- a/kernel/cobalt/posix/syscall.c
+++ b/kernel/cobalt/posix/syscall.c
@@ -105,6 +105,7 @@ static struct xnsyscall cobalt_syscalls[] = {
        SKINCALL_DEF(sc_cobalt_sem_unlink, cobalt_sem_unlink, any),
        SKINCALL_DEF(sc_cobalt_sem_init_np, cobalt_sem_init_np, any),
        SKINCALL_DEF(sc_cobalt_sem_broadcast_np, cobalt_sem_broadcast_np, any),
+       SKINCALL_DEF(sc_cobalt_sem_inquire, cobalt_sem_inquire, any),
        SKINCALL_DEF(sc_cobalt_clock_getres, cobalt_clock_getres, any),
        SKINCALL_DEF(sc_cobalt_clock_gettime, cobalt_clock_gettime, any),
        SKINCALL_DEF(sc_cobalt_clock_settime, cobalt_clock_settime, any),


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

Reply via email to