Add nanosleep function (#383)

Signed-off-by: David Laseca Perez <davidlaseca@eprosima.com>

Author: David-LP99

CMakeLists.txt

@@ -1,7 +1,7 @@
 option(BUILD_ETHERNET_INTERFACE "Build ethernet interface support" OFF)
 option(BUILD_WLAN_INTERFACE "Build wlan interface support" ON)
 
-set(COMPONENT_SRC "")
+set(COMPONENT_SRC "posix_functions/nanosleep.c")
 
 if(BUILD_ETHERNET_INTERFACE)
     list(APPEND COMPONENT_SRC "network_interfaces/uros_ethernet_netif.c")
@@ -102,6 +102,10 @@ target_compile_definitions(${COMPONENT_LIB} PUBLIC RCL_MICROROS)
 add_dependencies(${COMPONENT_LIB} libmicroros-prebuilt)
 target_link_libraries(${COMPONENT_LIB} INTERFACE libmicroros-prebuilt)
 
+# libmicroros.a is linked after this component, so force the linker to pull
+# in the nanosleep() shim before rclc references it
+target_link_libraries(${COMPONENT_LIB} INTERFACE "-u nanosleep")
+
 set_directory_properties( PROPERTIES ADDITIONAL_CLEAN_FILES
     "${COMPONENT_DIR}/include;${COMPONENT_DIR}/micro_ros_dev;${COMPONENT_DIR}/micro_ros_src;${COMPONENT_DIR}/esp32_toolchain.cmake" )
 

posix_functions/nanosleep.c

@@ -0,0 +1,31 @@
+#include <errno.h>
+#include <time.h>
+#include <unistd.h>
+
+// ESP-IDF defines _POSIX_TIMERS but does not implement nanosleep()
+// Declared weak so that a native ESP-IDF implementation takes precedence
+__attribute__((weak)) int nanosleep(const struct timespec * req, struct timespec * rem)
+{
+    (void) rem; // Never interrupted: FreeRTOS has no signals
+
+    if (req == NULL ||
+        req->tv_sec < 0 ||
+        req->tv_nsec < 0 ||
+        req->tv_nsec >= 1000000000L) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    // Sleep the whole seconds without overflowing useconds_t
+    for (time_t s = 0; s < req->tv_sec; s++) {
+        usleep(1000000);
+    }
+
+    // Round the remaining nanoseconds up to microseconds
+    useconds_t us = (req->tv_nsec + 999) / 1000;
+    if (us > 0) {
+        usleep(us);
+    }
+
+    return 0;
+}