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