Module: xenomai-gch
Branch: for-head
Commit: 97323b3287b5ee8cad99a7fa67cd050bc51f76c4
URL:    
http://git.xenomai.org/?p=xenomai-gch.git;a=commit;h=97323b3287b5ee8cad99a7fa67cd050bc51f76c4

Author: Gilles Chanteperdrix <gilles.chanteperd...@xenomai.org>
Date:   Mon Nov 30 15:27:30 2009 +0100

native: fix rt_cond_wait restarting

---

 include/native/cond.h       |    6 ++-
 include/native/syscall.h    |    3 +-
 ksrc/skins/native/cond.c    |   51 ++++++++++++++++++++++++-----
 ksrc/skins/native/syscall.c |   73 +++++++++++++++++++++++++++++++++---------
 src/skins/native/cond.c     |   55 ++++++++++++++++++++++----------
 5 files changed, 143 insertions(+), 45 deletions(-)

diff --git a/include/native/cond.h b/include/native/cond.h
index 3a38e83..1d38b2c 100644
--- a/include/native/cond.h
+++ b/include/native/cond.h
@@ -82,8 +82,10 @@ static inline void __native_cond_flush_rq(xnqueue_t *rq)
        xeno_flush_rq(RT_COND, rq, cond);
 }
 
-int rt_cond_wait_inner(RT_COND *cond, RT_MUTEX *mutex,
-                      xntmode_t timeout_mode, RTIME timeout);
+int rt_cond_wait_prologue(RT_COND *cond, RT_MUTEX *mutex, unsigned *plockcnt,
+                         xntmode_t timeout_mode, RTIME timeout);
+
+int rt_cond_wait_epilogue(RT_MUTEX *mutex, unsigned lockcnt);
 
 #else /* !CONFIG_XENO_OPT_NATIVE_COND */
 
diff --git a/include/native/syscall.h b/include/native/syscall.h
index cd9e64a..2e3d7ec 100644
--- a/include/native/syscall.h
+++ b/include/native/syscall.h
@@ -75,7 +75,7 @@
 #define __native_cond_create        49
 #define __native_cond_bind          50
 #define __native_cond_delete        51
-#define __native_cond_wait          52
+#define __native_cond_wait_prologue 52
 #define __native_cond_signal        53
 #define __native_cond_broadcast     54
 #define __native_cond_inquire       55
@@ -127,6 +127,7 @@
 #define __native_buffer_clear       101
 #define __native_buffer_inquire     102
 #define __native_queue_flush        103
+#define __native_cond_wait_epilogue 104
 
 struct rt_arg_bulk {
 
diff --git a/ksrc/skins/native/cond.c b/ksrc/skins/native/cond.c
index cea823e..10727d1 100644
--- a/ksrc/skins/native/cond.c
+++ b/ksrc/skins/native/cond.c
@@ -372,14 +372,13 @@ int rt_cond_broadcast(RT_COND *cond)
        return err;
 }
 
-int rt_cond_wait_inner(RT_COND *cond, RT_MUTEX *mutex,
+int rt_cond_wait_prologue(RT_COND *cond, RT_MUTEX *mutex, unsigned *plockcnt,
                       xntmode_t timeout_mode, RTIME timeout)
 {
-       int err, kicked = 0;
        xnthread_t *thread;
        xnflags_t info;
-       int lockcnt;
        spl_t s;
+       int err;
 
        if (timeout == TM_NONBLOCK)
                return -EWOULDBLOCK;
@@ -414,7 +413,7 @@ int rt_cond_wait_inner(RT_COND *cond, RT_MUTEX *mutex,
         * We can't use rt_mutex_release since that might reschedule
         * before enter xnsynch_sleep_on.
         */
-       lockcnt = mutex->lockcnt; /* Leave even if mutex is nested */
+       *plockcnt = mutex->lockcnt; /* Leave even if mutex is nested */
 
        mutex->lockcnt = 0;
 
@@ -429,15 +428,33 @@ int rt_cond_wait_inner(RT_COND *cond, RT_MUTEX *mutex,
                err = -ETIMEDOUT;       /* Timeout. */
        else if (info & XNBREAK) {
                err = -EINTR;   /* Unblocked. */
-               kicked = xnthread_test_info(thread, XNKICKED);
        }
 
-       rt_mutex_acquire(mutex, TM_INFINITE);
+      unlock_and_exit:
+
+       xnlock_put_irqrestore(&nklock, s);
+
+       return err;
+}
+
+int rt_cond_wait_epilogue(RT_MUTEX *mutex, unsigned lockcnt)
+{
+       int err;
+       spl_t s;
+
+       xnlock_get_irqsave(&nklock, s);
+
+       mutex = xeno_h2obj_validate(mutex, XENO_MUTEX_MAGIC, RT_MUTEX);
+
+       if (!mutex) {
+               err = xeno_handle_error(mutex, XENO_MUTEX_MAGIC, RT_MUTEX);
+               goto unlock_and_exit;
+       }
 
-       mutex->lockcnt = lockcnt; /* Adjust lockcnt */
+       err = rt_mutex_acquire(mutex, TM_INFINITE);
 
-       if (kicked)
-               xnthread_set_info(thread, XNKICKED);
+       if(!err) 
+               mutex->lockcnt = lockcnt; /* Adjust lockcnt */
 
       unlock_and_exit:
 
@@ -446,6 +463,22 @@ int rt_cond_wait_inner(RT_COND *cond, RT_MUTEX *mutex,
        return err;
 }
 
+static int rt_cond_wait_inner(RT_COND *cond, RT_MUTEX *mutex,
+                             xntmode_t timeout_mode, RTIME timeout)
+{
+       unsigned lockcnt;
+       int err;
+
+       err = rt_cond_wait_prologue(cond, mutex, &lockcnt, 
+                                   timeout_mode, timeout);
+
+       if(!err || err == -ETIMEDOUT || err == -EINTR)
+               do {
+                       err = rt_cond_wait_epilogue(mutex, lockcnt);
+               } while (err == -EINTR);
+
+       return err;
+}
 /**
  * @fn int rt_cond_wait(RT_COND *cond, RT_MUTEX *mutex, RTIME timeout)
  * @brief Wait on a condition.
diff --git a/ksrc/skins/native/syscall.c b/ksrc/skins/native/syscall.c
index 88dc233..b92d605 100644
--- a/ksrc/skins/native/syscall.c
+++ b/ksrc/skins/native/syscall.c
@@ -1826,19 +1826,22 @@ static int __rt_cond_delete(struct pt_regs *regs)
 }
 
 /*
- * int __rt_cond_wait(RT_COND_PLACEHOLDER *cph,
- *                    RT_MUTEX_PLACEHOLDER *mph,
- *                    xntmode_t timeout_mode,
- *                    RTIME *timeoutp)
+ * int __rt_cond_wait_prologue(RT_COND_PLACEHOLDER *cph,
+ *                            RT_MUTEX_PLACEHOLDER *mph,
+ *                            unsigned *plockcnt,
+ *                            xntmode_t timeout_mode,
+ *                            RTIME *timeoutp)
  */
 
-static int __rt_cond_wait(struct pt_regs *regs)
+static int __rt_cond_wait_prologue(struct pt_regs *regs)
 {
        RT_COND_PLACEHOLDER cph, mph;
        xntmode_t timeout_mode;
+       unsigned lockcnt;
        RT_MUTEX *mutex;
        RT_COND *cond;
        RTIME timeout;
+       int err;
 
        if (__xn_safe_copy_from_user(&cph, (void __user *)__xn_reg_arg1(regs),
                                     sizeof(cph)))
@@ -1858,13 +1861,47 @@ static int __rt_cond_wait(struct pt_regs *regs)
        if (!mutex)
                return -ESRCH;
 
-       timeout_mode = __xn_reg_arg3(regs);
+       timeout_mode = __xn_reg_arg4(regs);
 
-       if (__xn_safe_copy_from_user(&timeout, (void __user 
*)__xn_reg_arg4(regs),
+       if (__xn_safe_copy_from_user(&timeout, (void __user 
*)__xn_reg_arg5(regs),
                                     sizeof(timeout)))
                return -EFAULT;
 
-       return rt_cond_wait_inner(cond, mutex, timeout_mode, timeout);
+       err = rt_cond_wait_prologue(cond, mutex, &lockcnt, timeout_mode, 
timeout);
+
+       if (err == 0 || err == -ETIMEDOUT)
+               err = rt_cond_wait_epilogue(mutex, lockcnt);
+       
+       if (err == -EINTR && __xn_reg_arg3(regs)
+           && __xn_safe_copy_to_user((void __user *)__xn_reg_arg3(regs),
+                                     &lockcnt, sizeof(lockcnt)))
+               return -EFAULT;
+
+       return err;
+}
+
+/*
+ * int __rt_cond_wait_epilogue(RT_MUTEX_PLACEHOLODER *mph, unsigned lockcnt)
+ */
+
+static int __rt_cond_wait_epilogue(struct pt_regs *regs)
+{
+       RT_COND_PLACEHOLDER mph;
+       unsigned lockcnt;
+       RT_MUTEX *mutex;
+
+       if (__xn_safe_copy_from_user(&mph, (void __user *)__xn_reg_arg1(regs),
+                                    sizeof(mph)))
+               return -EFAULT;
+
+       mutex = (RT_MUTEX *)xnregistry_fetch(mph.opaque);
+
+       if (!mutex)
+               return -ESRCH;
+
+       lockcnt = __xn_reg_arg2(regs);
+
+       return rt_cond_wait_epilogue(mutex, lockcnt);
 }
 
 /*
@@ -1944,13 +1981,14 @@ static int __rt_cond_inquire(struct pt_regs *regs)
 
 #else /* !CONFIG_XENO_OPT_NATIVE_COND */
 
-#define __rt_cond_create    __rt_call_not_available
-#define __rt_cond_bind      __rt_call_not_available
-#define __rt_cond_delete    __rt_call_not_available
-#define __rt_cond_wait      __rt_call_not_available
-#define __rt_cond_signal    __rt_call_not_available
-#define __rt_cond_broadcast __rt_call_not_available
-#define __rt_cond_inquire   __rt_call_not_available
+#define __rt_cond_create       __rt_call_not_available
+#define __rt_cond_bind         __rt_call_not_available
+#define __rt_cond_delete       __rt_call_not_available
+#define __rt_cond_wait_prologue __rt_call_not_available
+#define __rt_cond_wait_epilogue __rt_call_not_available
+#define __rt_cond_signal       __rt_call_not_available
+#define __rt_cond_broadcast    __rt_call_not_available
+#define __rt_cond_inquire      __rt_call_not_available
 
 #endif /* CONFIG_XENO_OPT_NATIVE_COND */
 
@@ -4051,7 +4089,10 @@ static xnsysent_t __systab[] = {
        [__native_cond_create] = {&__rt_cond_create, __xn_exec_any},
        [__native_cond_bind] = {&__rt_cond_bind, __xn_exec_conforming},
        [__native_cond_delete] = {&__rt_cond_delete, __xn_exec_any},
-       [__native_cond_wait] = {&__rt_cond_wait, __xn_exec_primary},
+       [__native_cond_wait_prologue] = 
+               {&__rt_cond_wait_prologue, __xn_exec_primary},
+       [__native_cond_wait_epilogue] = 
+               {&__rt_cond_wait_epilogue, __xn_exec_primary},
        [__native_cond_signal] = {&__rt_cond_signal, __xn_exec_any},
        [__native_cond_broadcast] = {&__rt_cond_broadcast, __xn_exec_any},
        [__native_cond_inquire] = {&__rt_cond_inquire, __xn_exec_any},
diff --git a/src/skins/native/cond.c b/src/skins/native/cond.c
index feaad26..52c5ccb 100644
--- a/src/skins/native/cond.c
+++ b/src/skins/native/cond.c
@@ -41,46 +41,67 @@ int rt_cond_delete(RT_COND *cond)
 
 int rt_cond_wait(RT_COND *cond, RT_MUTEX *mutex, RTIME timeout)
 {
-#ifdef CONFIG_XENO_FASTSYNCH
        int saved_lockcnt, err;
 
+#ifdef CONFIG_XENO_FASTSYNCH
        saved_lockcnt = mutex->lockcnt;
 
-       err = XENOMAI_SKINCALL4(__native_muxid,
-                               __native_cond_wait, cond, mutex,
-                               XN_RELATIVE, &timeout);
+       err = XENOMAI_SKINCALL5(__native_muxid,
+                               __native_cond_wait_prologue, cond, mutex,
+                               NULL, XN_RELATIVE, &timeout);
 
-       mutex->lockcnt = saved_lockcnt;
+       while (err == -EINTR)
+               err = XENOMAI_SKINCALL2(__native_muxid,
+                                       __native_cond_wait_epilogue, mutex,
+                                       saved_lockcnt);
 
-       return err;
+       mutex->lockcnt = saved_lockcnt;
 
 #else /* !CONFIG_XENO_FASTSYNCH */
-       return XENOMAI_SKINCALL4(__native_muxid,
+       err = XENOMAI_SKINCALL5(__native_muxid,
                                 __native_cond_wait, cond, mutex,
-                                XN_RELATIVE, &timeout);
+                                &saved_lockcnt, XN_RELATIVE, &timeout);
+
+       while (err == -EINTR)
+               err = XENOMAI_SKINCALL2(__native_muxid,
+                                       __native_cond_wait_epilogue, mutex
+                                       saved_lockcnt);
+
 #endif /* !CONFIG_XENO_FASTSYNCH */
+
+       return err;
 }
 
 int rt_cond_wait_until(RT_COND *cond, RT_MUTEX *mutex, RTIME timeout)
 {
-#ifdef CONFIG_XENO_FASTSYNCH
        int saved_lockcnt, err;
 
+#ifdef CONFIG_XENO_FASTSYNCH
        saved_lockcnt = mutex->lockcnt;
 
-       err = XENOMAI_SKINCALL4(__native_muxid,
-                               __native_cond_wait, cond, mutex,
-                               XN_REALTIME, &timeout);
+       err = XENOMAI_SKINCALL5(__native_muxid,
+                               __native_cond_wait_prologue, cond, mutex,
+                               NULL, XN_REALTIME, &timeout);
 
-       mutex->lockcnt = saved_lockcnt;
+       while (err == -EINTR)
+               err = XENOMAI_SKINCALL2(__native_muxid,
+                                       __native_cond_wait_epilogue, mutex,
+                                       saved_lockcnt);
 
-       return err;
+       mutex->lockcnt = saved_lockcnt;
 
 #else /* !CONFIG_XENO_FASTSYNCH */
-       return XENOMAI_SKINCALL4(__native_muxid,
-                                __native_cond_wait, cond, mutex,
-                                XN_REALTIME, &timeout);
+       err = XENOMAI_SKINCALL5(__native_muxid,
+                               __native_cond_wait_prologue, cond, mutex,
+                               &saved_lockcnt, XN_REALTIME, &timeout);
+
+       while (err == -EINTR)
+               err = -XENOMAI_SKINCALL2(__native_muxid,
+                                        __native_cond_wait_epilogue, mutex,
+                                        saved_lockcnt);
 #endif /* !CONFIG_XENO_FASTSYNCH */
+
+       return err;
 }
 
 int rt_cond_signal(RT_COND *cond)


_______________________________________________
Xenomai-git mailing list
Xenomai-git@gna.org
https://mail.gna.org/listinfo/xenomai-git

Reply via email to