diff --git a/include/zephyr/posix/time.h b/include/zephyr/posix/time.h index 057d55f0e3a7ab..85cb7f8ae4226a 100644 --- 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 index 6733b34deaceff..51812ed53bc65e 100644 --- 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 index 39d47c443f5abd..aafb9dbeb79936 100644 --- 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); }