From 8bcf0050844bb31e0179193470eb8b7433dea17a Mon Sep 17 00:00:00 2001
From: Christopher Friedt <chrisfriedt@gmail.com>
Date: Sat, 23 May 2020 06:09:28 -0400
Subject: [PATCH] lib: posix: nanosleep

This change adds support for nanosleep(2) documented at
https://pubs.opengroup.org/onlinepubs/9699919799/functions/nanosleep.html

N.B: Currently, this provides no better resolution than
k_busy_wait()

Fixes #25554

Signed-off-by: Christopher Friedt <chrisfriedt@gmail.com>
---
 include/posix/time.h     |  1 +
 lib/posix/CMakeLists.txt |  4 +++
 lib/posix/nanosleep.c    | 60 ++++++++++++++++++++++++++++++++++++++++
 3 files changed, 65 insertions(+)
 create mode 100644 lib/posix/nanosleep.c

diff --git a/include/posix/time.h b/include/posix/time.h
index 28518b01fce..a61b1d6693f 100644
--- a/include/posix/time.h
+++ b/include/posix/time.h
@@ -91,6 +91,7 @@ int timer_delete(timer_t timerid);
 int timer_gettime(timer_t timerid, struct itimerspec *its);
 int timer_settime(timer_t timerid, int flags, const struct itimerspec *value,
 		  struct itimerspec *ovalue);
+int nanosleep(const struct timespec *rqtp, struct timespec *rmtp);
 
 #ifdef __cplusplus
 }
diff --git a/lib/posix/CMakeLists.txt b/lib/posix/CMakeLists.txt
index e1703eaaa26..7dfade5ec7e 100644
--- a/lib/posix/CMakeLists.txt
+++ b/lib/posix/CMakeLists.txt
@@ -24,6 +24,10 @@ zephyr_library_sources_ifdef(CONFIG_POSIX_MQUEUE mqueue.c)
 zephyr_library_sources_ifdef(CONFIG_POSIX_FS fs.c)
 zephyr_library_sources_ifdef(CONFIG_EVENTFD eventfd.c)
 
+if(NOT (CONFIG_BOARD_NATIVE_POSIX OR CONFIG_BOARD_NATIVE_POSIX_64BIT))
+zephyr_library_sources(nanosleep.c)
+endif()
+
 zephyr_library_include_directories(
   ${ZEPHYR_BASE}/kernel/include
   ${ZEPHYR_BASE}/arch/${ARCH}/include
diff --git a/lib/posix/nanosleep.c b/lib/posix/nanosleep.c
new file mode 100644
index 00000000000..1d451a60d9d
--- /dev/null
+++ b/lib/posix/nanosleep.c
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2018 Friedt Professional Engineering Services, Inc
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stdint.h>
+#include <kernel.h>
+#include <limits.h>
+#include <errno.h>
+/* required for struct timespec */
+#include <posix/time.h>
+#include <sys_clock.h>
+
+/**
+ * @brief Suspend execution for nanosecond intervals.
+ *
+ * See IEEE 1003.1
+ */
+int nanosleep(const struct timespec *rqtp, struct timespec *rmtp)
+{
+	uint64_t ns;
+	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));
+	} else {
+		ns = rqtp->tv_sec * NSEC_PER_SEC + rqtp->tv_nsec;
+	}
+
+	/* currently we have no mechanism to achieve greater resolution */
+	k_busy_wait(ns / NSEC_PER_USEC);
+
+do_rmtp_update:
+	if (update_rmtp) {
+		rmtp->tv_sec = 0;
+		rmtp->tv_nsec = 0;
+	}
+
+	return 0;
+}