From c75ce853a9b6b9f334e49c54f0d39554252b40e9 Mon Sep 17 00:00:00 2001
From: Con Kolivas <kernel@kolivas.org>
Date: Mon, 14 Oct 2013 11:22:57 +1100
Subject: [PATCH] Implement a generic cgsem_mswait similar to sem_timedwait

---
 util.c | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++
 util.h |  1 +
 2 files changed, 51 insertions(+)

diff --git a/util.c b/util.c
index 8e032810..2de5c1ca 100644
--- a/util.c
+++ b/util.c
@@ -881,6 +881,14 @@ void ms_to_timespec(struct timespec *spec, int64_t ms)
 	spec->tv_nsec = tvdiv.rem * 1000000;
 }
 
+void ms_to_timeval(struct timeval *val, int64_t ms)
+{
+	lldiv_t tvdiv = lldiv(ms, 1000);
+
+	val->tv_sec = tvdiv.quot;
+	val->tv_usec = tvdiv.rem * 1000;
+}
+
 void timeraddspec(struct timespec *a, const struct timespec *b)
 {
 	a->tv_sec += b->tv_sec;
@@ -2407,6 +2415,31 @@ void _cgsem_destroy(cgsem_t *cgsem)
 	close(cgsem->pipefd[1]);
 	close(cgsem->pipefd[0]);
 }
+
+/* This is similar to sem_timedwait but takes a millisecond value */
+int _cgsem_mswait(cgsem_t *cgsem, int ms, const char *file, const char *func, const int line)
+{
+	struct timeval timeout;
+	int ret, fd;
+	fd_set rd;
+	char buf;
+
+	fd = cgsem->pipefd[0];
+	FD_ZERO(&rd);
+	FD_SET(fd, &rd);
+	ms_to_timeval(&timeout, ms);
+	ret = select(fd + 1, &rd, NULL, NULL, &timeout);
+
+	if (ret > 0) {
+		ret = read(fd, &buf, 1);
+		return 0;
+	}
+	if (likely(!ret))
+		return ETIMEDOUT;
+	quitfrom(1, file, func, line, "Failed to sem_timedwait errno=%d cgsem=0x%p", errno, cgsem);
+	/* We don't reach here */
+	return 0;
+}
 #else
 void _cgsem_init(cgsem_t *cgsem, const char *file, const char *func, const int line)
 {
@@ -2427,6 +2460,23 @@ void _cgsem_wait(cgsem_t *cgsem, const char *file, const char *func, const int l
 		quitfrom(1, file, func, line, "Failed to sem_wait errno=%d cgsem=0x%p", errno, cgsem);
 }
 
+int _cgsem_mswait(cgsem_t *cgsem, int ms, const char *file, const char *func, const int line)
+{
+	struct timespec abs_timeout, ts_now;
+	struct timeval tv_now;
+	int ret;
+
+	cgtime(&tv_now);
+	timeval_to_spec(&ts_now, &tv_now);
+	ms_to_timespec(&abs_timeout, ms);
+	timeraddspec(&abs_timeout, &ts_now);
+	ret = sem_timedwait(cgsem, &abs_timeout);
+
+	if (unlikely(ret && ret != ETIMEDOUT))
+		quitfrom(1, file, func, line, "Failed to sem_timedwait errno=%d cgsem=0x%p", errno, cgsem);
+	return ret;
+}
+
 void _cgsem_destroy(cgsem_t *cgsem)
 {
 	sem_destroy(cgsem);
diff --git a/util.h b/util.h
index e318f779..b1dfc3b1 100644
--- a/util.h
+++ b/util.h
@@ -129,6 +129,7 @@ void _cgsem_destroy(cgsem_t *cgsem);
 #define cgsem_init(_sem) _cgsem_init(_sem, __FILE__, __func__, __LINE__)
 #define cgsem_post(_sem) _cgsem_post(_sem, __FILE__, __func__, __LINE__)
 #define cgsem_wait(_sem) _cgsem_wait(_sem, __FILE__, __func__, __LINE__)
+#define cgmsem_mswait(_sem, _timeout) _cgsem_mswait(_sem, _timeout, __FILE__, __func__, __LINE__)
 #define cgsem_destroy(_sem) _cgsem_destroy(_sem)
 
 /* Align a size_t to 4 byte boundaries for fussy arches */