Module: xenomai-gch
Branch: for-head
Commit: 15332c016db193abda07e39fe2112797676fb774
URL:
http://git.xenomai.org/?p=xenomai-gch.git;a=commit;h=15332c016db193abda07e39fe2112797676fb774
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
