diff --git a/src/platforms/posix/px4_layer/px4_sem.cpp b/src/platforms/posix/px4_layer/px4_sem.cpp index 84795eb16a..9abd25bc59 100644 --- a/src/platforms/posix/px4_layer/px4_sem.cpp +++ b/src/platforms/posix/px4_layer/px4_sem.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #ifdef __PX4_DARWIN @@ -61,53 +62,107 @@ int px4_sem_init(px4_sem_t *s, int pshared, unsigned value) int px4_sem_wait(px4_sem_t *s) { - pthread_mutex_lock(&(s->lock)); + int ret = pthread_mutex_lock(&(s->lock)); + + if (ret) { + return ret; + } + s->value--; if (s->value < 0) { - pthread_cond_wait(&(s->wait), &(s->lock)); + ret = pthread_cond_wait(&(s->wait), &(s->lock)); + } else { + ret = 0; } - pthread_mutex_unlock(&(s->lock)); + if (ret) { + PX4_WARN("px4_sem_wait failure"); + } - return 0; + int mret = pthread_mutex_unlock(&(s->lock)); + + return (ret) ? ret : mret; } int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime) { - pthread_mutex_lock(&(s->lock)); + int ret = pthread_mutex_lock(&(s->lock)); + + if (ret) { + return ret; + } + s->value--; if (s->value < 0) { - pthread_cond_timedwait(&(s->wait), &(s->lock), abstime); + ret = pthread_cond_timedwait(&(s->wait), &(s->lock), abstime); + } else { + ret = 0; } - pthread_mutex_unlock(&(s->lock)); + int err = errno; + #ifdef __PX4_DARWIN + err = ret; + #endif + + if (err != 0 && err != ETIMEDOUT) { + setbuf(stdout, NULL); + setbuf(stderr, NULL); + const unsigned NAMELEN = 32; + char thread_name[NAMELEN] = {}; + (void)pthread_getname_np(pthread_self(), thread_name, NAMELEN); + PX4_WARN("%s: px4_sem_timedwait failure: ret: %d, %s", thread_name, ret, strerror(err)); + } - return 0; + int mret = pthread_mutex_unlock(&(s->lock)); + + return (ret) ? ret : mret; } int px4_sem_post(px4_sem_t *s) { - pthread_mutex_lock(&(s->lock)); + int ret = pthread_mutex_lock(&(s->lock)); + + if (ret) { + return ret; + } + s->value++; if (s->value <= 0) { - pthread_cond_signal(&(s->wait)); + ret = pthread_cond_signal(&(s->wait)); + } else { + ret = 0; } - pthread_mutex_unlock(&(s->lock)); + if (ret) { + PX4_WARN("px4_sem_post failure"); + } - return 0; + int mret = pthread_mutex_unlock(&(s->lock)); + + // return the cond signal failure if present, + // else return the mutex status + return (ret) ? ret : mret; } int px4_sem_getvalue(px4_sem_t *s, int *sval) { - pthread_mutex_lock(&(s->lock)); + int ret = pthread_mutex_lock(&(s->lock)); + + if (ret) { + PX4_WARN("px4_sem_getvalue failure"); + } + + if (ret) { + return ret; + } + *sval = s->value; - pthread_mutex_unlock(&(s->lock)); + ret = pthread_mutex_unlock(&(s->lock)); - return 0; + return ret; } int px4_sem_destroy(px4_sem_t *s)