posix: implement clock_nanosleep

Implements the posix clock_nanosleep function, where both relative and absolute sleeps are made as absolute sleeps. The nanosleep() function is a special case of clock_nanosleep(), and so has been refactored to simply call it. Signed-off-by: Tom Finet <tom.codeninja@gmail.com>
zephyrproject-rtos · Aug 30, 2023 · d09a1c3 · d09a1c3
1 parent 295eb1b
commit d09a1c3
Show file tree

Hide file tree

Showing 3 changed files with 72 additions and 42 deletions.
diff --git a/include/zephyr/posix/time.h b/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
 }

diff --git a/lib/posix/clock.c b/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.
  *

diff --git a/lib/posix/nanosleep.c b/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);
 }