posix: clock_nanosleep implementation

include/zephyr/posix/time.h

@@ -96,6 +96,8 @@ int timer_settime(timer_t timerid, int flags, const struct itimerspec *value,
 		  struct itimerspec *ovalue);
 int timer_getoverrun(timer_t timerid);
 int nanosleep(const struct timespec *rqtp, struct timespec *rmtp);
+int clock_nanosleep(clockid_t clock_id, int flags,
+		    const struct timespec *rqtp, struct timespec *rmtp);
 
 #ifdef __cplusplus
 }

lib/posix/clock.c

@@ -106,6 +106,75 @@ int clock_settime(clockid_t clock_id, const struct timespec *tp)
 	return 0;
 }
 
+/**
+ * @brief Suspend execution for a nanosecond interval, or
+ * until some absolute time relative to the specified clock.
+ *
+ * See IEEE 1003.1
+ */
+int clock_nanosleep(clockid_t clock_id, int flags, const struct timespec *rqtp,
+		    struct timespec *rmtp)
+{
+	uint64_t ns;
+	uint64_t us;
+	uint64_t uptime_ns;
+	k_spinlock_key_t key;
+	const bool update_rmtp = rmtp != NULL;
+
+	if (!(clock_id == CLOCK_REALTIME || clock_id == CLOCK_MONOTONIC)) {
+		errno = EINVAL;
+		return -1;
+	}
+
+	if (rqtp == NULL) {
+		errno = EFAULT;
+		return -1;
+	}
+
+	if (rqtp->tv_sec < 0 || rqtp->tv_nsec < 0 || rqtp->tv_nsec >= NSEC_PER_SEC) {
+		errno = EINVAL;
+		return -1;
+	}
+
+	if ((flags & TIMER_ABSTIME) == 0 &&
+	    unlikely(rqtp->tv_sec >= ULLONG_MAX / NSEC_PER_SEC)) {
+
+		ns = rqtp->tv_nsec + NSEC_PER_SEC
+			+ k_sleep(K_SECONDS(rqtp->tv_sec - 1)) * NSEC_PER_MSEC;
+	} else {
+		ns = rqtp->tv_sec * NSEC_PER_SEC + rqtp->tv_nsec;
+	}
+
+	uptime_ns = k_cyc_to_ns_ceil64(k_cycle_get_32());
+
+	if (flags & TIMER_ABSTIME && clock_id == CLOCK_REALTIME) {
+		key = k_spin_lock(&rt_clock_base_lock);
+		ns -= rt_clock_base.tv_sec * NSEC_PER_SEC + rt_clock_base.tv_nsec;
+		k_spin_unlock(&rt_clock_base_lock, key);
+	}
+
+	if ((flags & TIMER_ABSTIME) == 0) {
+		ns += uptime_ns;
+	}
+
+	if (ns <= uptime_ns) {
+		goto do_rmtp_update;
+	}
+
+	us = DIV_ROUND_UP(ns, NSEC_PER_USEC);
+	do {
+		us = k_sleep(K_TIMEOUT_ABS_US(us)) * 1000;
+	} while (us != 0);
+
+do_rmtp_update:
+	if (update_rmtp) {
+		rmtp->tv_sec = 0;
+		rmtp->tv_nsec = 0;
+	}
+
+	return 0;
+}
+
 /**
  * @brief Get current real time.
  *

lib/posix/nanosleep.c

@@ -20,46 +20,5 @@
  */
 int nanosleep(const struct timespec *rqtp, struct timespec *rmtp)
 {
-	uint64_t ns;
-	uint64_t us;
-	const bool update_rmtp = rmtp != NULL;
-
-	if (rqtp == NULL) {
-		errno = EFAULT;
-		return -1;
-	}
-
-	if (rqtp->tv_sec < 0 || rqtp->tv_nsec < 0
-		|| rqtp->tv_nsec >= NSEC_PER_SEC) {
-		errno = EINVAL;
-		return -1;
-	}
-
-	if (rqtp->tv_sec == 0 && rqtp->tv_nsec == 0) {
-		goto do_rmtp_update;
-	}
-
-	if (unlikely(rqtp->tv_sec >= ULLONG_MAX / NSEC_PER_SEC)) {
-		/* If a user passes this in, we could be here a while, but
-		 * at least it's technically correct-ish
-		 */
-		ns = rqtp->tv_nsec + NSEC_PER_SEC
-			+ k_sleep(K_SECONDS(rqtp->tv_sec - 1)) * NSEC_PER_MSEC;
-	} else {
-		ns = rqtp->tv_sec * NSEC_PER_SEC + rqtp->tv_nsec;
-	}
-
-	/* TODO: improve upper bound when hr timers are available */
-	us = DIV_ROUND_UP(ns, NSEC_PER_USEC);
-	do {
-		us = k_usleep(us);
-	} while (us != 0);
-
-do_rmtp_update:
-	if (update_rmtp) {
-		rmtp->tv_sec = 0;
-		rmtp->tv_nsec = 0;
-	}
-
-	return 0;
+	return clock_nanosleep(CLOCK_MONOTONIC, 0, rqtp, rmtp);
 }

tests/posix/common/src/nanosleep.c

@@ -11,20 +11,32 @@
 #include <zephyr/sys_clock.h>
 #include <zephyr/ztest.h>
 
-ZTEST(posix_apis, test_nanosleep_errors_errno)
+#define SELECT_NANOSLEEP       1
+#define SELECT_CLOCK_NANOSLEEP 0
+
+static inline int select_nanosleep(int selection, clockid_t clock_id, int flags,
+				   const struct timespec *rqtp, struct timespec *rmtp)
+{
+	if (selection == SELECT_NANOSLEEP) {
+		return nanosleep(rqtp, rmtp);
+	}
+	return clock_nanosleep(clock_id, flags, rqtp, rmtp);
+}
+
+static void common_errors(int selection, clockid_t clock_id, int flags)
 {
 	struct timespec rem = {};
 	struct timespec req = {};
 
 	/*
 	 * invalid parameters
 	 */
-	zassert_equal(nanosleep(NULL, NULL), -1);
+	zassert_equal(select_nanosleep(selection, clock_id, flags, NULL, NULL), -1);
 	zassert_equal(errno, EFAULT);
 
 	/* NULL request */
 	errno = 0;
-	zassert_equal(nanosleep(NULL, &rem), -1);
+	zassert_equal(select_nanosleep(selection, clock_id, flags, NULL, &rem), -1);
 	zassert_equal(errno, EFAULT);
 	/* Expect rem to be the same when function returns */
 	zassert_equal(rem.tv_sec, 0, "actual: %d expected: %d", rem.tv_sec, 0);
@@ -33,23 +45,23 @@ ZTEST(posix_apis, test_nanosleep_errors_errno)
 	/* negative times */
 	errno = 0;
 	req = (struct timespec){.tv_sec = -1, .tv_nsec = 0};
-	zassert_equal(nanosleep(&req, NULL), -1);
+	zassert_equal(select_nanosleep(selection, clock_id, flags, &req, NULL), -1);
 	zassert_equal(errno, EINVAL);
 
 	errno = 0;
 	req = (struct timespec){.tv_sec = 0, .tv_nsec = -1};
-	zassert_equal(nanosleep(&req, NULL), -1);
+	zassert_equal(select_nanosleep(selection, clock_id, flags, &req, NULL), -1);
 	zassert_equal(errno, EINVAL);
 
 	errno = 0;
 	req = (struct timespec){.tv_sec = -1, .tv_nsec = -1};
-	zassert_equal(nanosleep(&req, NULL), -1);
+	zassert_equal(select_nanosleep(selection, clock_id, flags, &req, NULL), -1);
 	zassert_equal(errno, EINVAL);
 
 	/* nanoseconds too high */
 	errno = 0;
 	req = (struct timespec){.tv_sec = 0, .tv_nsec = 1000000000};
-	zassert_equal(nanosleep(&req, NULL), -1);
+	zassert_equal(select_nanosleep(selection, clock_id, flags, &req, NULL), -1);
 	zassert_equal(errno, EINVAL);
 
 	/*
@@ -59,13 +71,13 @@ ZTEST(posix_apis, test_nanosleep_errors_errno)
 
 	/* Happy path, plus make sure the const input is unmodified */
 	req = (struct timespec){.tv_sec = 1, .tv_nsec = 1};
-	zassert_equal(nanosleep(&req, NULL), 0);
+	zassert_equal(select_nanosleep(selection, clock_id, flags, &req, NULL), 0);
 	zassert_equal(errno, 0);
 	zassert_equal(req.tv_sec, 1);
 	zassert_equal(req.tv_nsec, 1);
 
 	/* Sleep for 0.0 s. Expect req & rem to be the same when function returns */
-	zassert_equal(nanosleep(&req, &rem), 0);
+	zassert_equal(select_nanosleep(selection, clock_id, flags, &req, &rem), 0);
 	zassert_equal(errno, 0);
 	zassert_equal(rem.tv_sec, 0, "actual: %d expected: %d", rem.tv_sec, 0);
 	zassert_equal(rem.tv_nsec, 0, "actual: %d expected: %d", rem.tv_nsec, 0);
@@ -77,61 +89,150 @@ ZTEST(posix_apis, test_nanosleep_errors_errno)
 	 * Expect rem to be zero after returning.
 	 */
 	req = (struct timespec){.tv_sec = 0, .tv_nsec = 1};
-	zassert_equal(nanosleep(&req, &req), 0);
+	zassert_equal(select_nanosleep(selection, clock_id, flags, &req, &req), 0);
 	zassert_equal(errno, 0);
 	zassert_equal(req.tv_sec, 0, "actual: %d expected: %d", req.tv_sec, 0);
 	zassert_equal(req.tv_nsec, 0, "actual: %d expected: %d", req.tv_nsec, 0);
 }
 
-static void common(const uint32_t s, uint32_t ns)
+ZTEST(posix_apis, test_nanosleep_errors_errno)
+{
+	common_errors(SELECT_NANOSLEEP, CLOCK_REALTIME, 0);
+}
+
+ZTEST(posix_apis, test_clock_nanosleep_errors_errno)
+{
+	struct timespec rem = {};
+	struct timespec req = {};
+
+	common_errors(SELECT_CLOCK_NANOSLEEP, CLOCK_MONOTONIC, TIMER_ABSTIME);
+
+	/* Absolute timeout in the past. */
+	clock_gettime(CLOCK_MONOTONIC, &req);
+	zassert_equal(clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &req, &rem), 0);
+	zassert_equal(rem.tv_sec, 0, "actual: %d expected: %d", rem.tv_sec, 0);
+	zassert_equal(rem.tv_nsec, 0, "actual: %d expected: %d", rem.tv_nsec, 0);
+
+	/* Absolute timeout in the past relative to the realtime clock. */
+	clock_gettime(CLOCK_REALTIME, &req);
+	zassert_equal(clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &req, &rem), 0);
+	zassert_equal(rem.tv_sec, 0, "actual: %d expected: %d", rem.tv_sec, 0);
+	zassert_equal(rem.tv_nsec, 0, "actual: %d expected: %d", rem.tv_nsec, 0);
+}
+
+static void common_lower_bound_check(int selection, clockid_t clock_id, int flags, const uint32_t s,
+				     uint32_t ns)
 {
 	int r;
+	uint64_t actual_ns;
+	uint64_t exp_ns;
 	uint32_t now;
 	uint32_t then;
 	struct timespec rem = {0, 0};
 	struct timespec req = {s, ns};
 
 	errno = 0;
 	then = k_cycle_get_32();
-	r = nanosleep(&req, &rem);
+	r = select_nanosleep(selection, clock_id, flags, &req, &rem);
 	now = k_cycle_get_32();
 
-	zassert_equal(r, 0, "actual: %d expected: %d", r, -1);
+	zassert_equal(r, 0, "actual: %d expected: %d", r, 0);
 	zassert_equal(errno, 0, "actual: %d expected: %d", errno, 0);
 	zassert_equal(req.tv_sec, s, "actual: %d expected: %d", req.tv_sec, s);
 	zassert_equal(req.tv_nsec, ns, "actual: %d expected: %d", req.tv_nsec, ns);
 	zassert_equal(rem.tv_sec, 0, "actual: %d expected: %d", rem.tv_sec, 0);
 	zassert_equal(rem.tv_nsec, 0, "actual: %d expected: %d", rem.tv_nsec, 0);
 
-	uint64_t actual_ns = k_cyc_to_ns_ceil64((now - then));
-	uint64_t exp_ns = (uint64_t)s * NSEC_PER_SEC + ns;
+	actual_ns = k_cyc_to_ns_ceil64((now - then * selection));
+
+	exp_ns = (uint64_t)s * NSEC_PER_SEC + ns;
 	/* round up to the nearest microsecond for k_busy_wait() */
 	exp_ns = DIV_ROUND_UP(exp_ns, NSEC_PER_USEC) * NSEC_PER_USEC;
 
 	/* lower bounds check */
-	zassert_true(actual_ns >= exp_ns,
-		"actual: %llu expected: %llu", actual_ns, exp_ns);
+	zassert_true(actual_ns >= exp_ns, "actual: %llu expected: %llu", actual_ns, exp_ns);
 
 	/* TODO: Upper bounds check when hr timers are available */
 }
 
 ZTEST(posix_apis, test_nanosleep_execution)
 {
 	/* sleep for 1ns */
-	common(0, 1);
+	common_lower_bound_check(SELECT_NANOSLEEP, 0, 0, 0, 1);
 
 	/* sleep for 1us + 1ns */
-	common(0, 1001);
+	common_lower_bound_check(SELECT_NANOSLEEP, 0, 0, 0, 1001);
 
 	/* sleep for 500000000ns */
-	common(0, 500000000);
+	common_lower_bound_check(SELECT_NANOSLEEP, 0, 0, 0, 500000000);
 
 	/* sleep for 1s */
-	common(1, 0);
+	common_lower_bound_check(SELECT_NANOSLEEP, 0, 0, 1, 0);
 
 	/* sleep for 1s + 1ns */
-	common(1, 1);
+	common_lower_bound_check(SELECT_NANOSLEEP, 0, 0, 1, 1);
 
 	/* sleep for 1s + 1us + 1ns */
-	common(1, 1001);
+	common_lower_bound_check(SELECT_NANOSLEEP, 0, 0, 1, 1001);
+}
+
+ZTEST(posix_apis, test_clock_nanosleep_execution)
+{
+	struct timespec ts;
+
+	clock_gettime(CLOCK_MONOTONIC, &ts);
+
+	/* absolute sleeps with the monotonic clock and reference time ts */
+
+	/* until 1s + 1ns past the reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_MONOTONIC, TIMER_ABSTIME,
+		ts.tv_sec + 1, 1);
+
+	/* until 1s + 1us past the reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_MONOTONIC, TIMER_ABSTIME,
+		ts.tv_sec + 1, 1000);
+
+	/* until 1s + 500000000ns past the reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_MONOTONIC, TIMER_ABSTIME,
+		ts.tv_sec + 1, 500000000);
+
+	/* until 2s past the reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_MONOTONIC, TIMER_ABSTIME,
+		ts.tv_sec + 2, 0);
+
+	/* until 2s + 1ns past the reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_MONOTONIC, TIMER_ABSTIME,
+		ts.tv_sec + 2, 1);
+
+	/* until 2s + 1us + 1ns past reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_MONOTONIC, TIMER_ABSTIME,
+		ts.tv_sec + 2, 1001);
+
+	clock_gettime(CLOCK_REALTIME, &ts);
+
+	/* absolute sleeps with the real time clock and adjusted reference time ts */
+
+	/* until 1s + 1ns past the reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_REALTIME, TIMER_ABSTIME,
+				 ts.tv_sec + 1, 1);
+
+	/* until 1s + 1us past the reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_REALTIME, TIMER_ABSTIME,
+				 ts.tv_sec + 1, 1000);
+
+	/* until 1s + 500000000ns past the reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_REALTIME, TIMER_ABSTIME,
+				 ts.tv_sec + 1, 500000000);
+
+	/* until 2s past the reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_REALTIME, TIMER_ABSTIME,
+				 ts.tv_sec + 2, 0);
+
+	/* until 2s + 1ns past the reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_REALTIME, TIMER_ABSTIME,
+				 ts.tv_sec + 2, 1);
+
+	/* until 2s + 1us + 1ns past the reference time */
+	common_lower_bound_check(SELECT_CLOCK_NANOSLEEP, CLOCK_REALTIME, TIMER_ABSTIME,
+				 ts.tv_sec + 2, 1001);
 }