From 8f4ddb8e7d94de2e2937e61653541e60b2829927 Mon Sep 17 00:00:00 2001 From: Alexander Bushnev Date: Mon, 2 Dec 2024 13:16:20 +0100 Subject: [PATCH] Add base rpi_pico_w platform --- CMakeLists.txt | 7 + examples/rpi_pico_w/CMakeLists.txt | 2 +- .../zenoh-pico/system/platform/rpi_pico_w.h | 71 +++++ include/zenoh-pico/system/platform_common.h | 2 + src/system/rpi_pico_w/network.c | 264 ++++++++++++++++++ src/system/rpi_pico_w/system.c | 255 +++++++++++++++++ 6 files changed, 600 insertions(+), 1 deletion(-) create mode 100644 include/zenoh-pico/system/platform/rpi_pico_w.h create mode 100644 src/system/rpi_pico_w/network.c create mode 100644 src/system/rpi_pico_w/system.c diff --git a/CMakeLists.txt b/CMakeLists.txt index b0874e934..91b9ed774 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,6 +45,7 @@ include(GNUInstallDirs) option(BUILD_SHARED_LIBS "Build shared libraries if ON, otherwise build static libraries" ON) option(WITH_ZEPHYR "Build for Zephyr RTOS" OFF) option(WITH_FREERTOS_PLUS_TCP "Build for FreeRTOS RTOS and FreeRTOS-Plus-TCP network stack" OFF) +option(WITH_RPI_PICO_W "Build for Raspberry Pico W" OFF) set(ZENOH_DEBUG 0 CACHE STRING "Use this to set the ZENOH_DEBUG variable") set(CMAKE_EXPORT_COMPILE_COMMANDS ON CACHE INTERNAL "") if(CMAKE_EXPORT_COMPILE_COMMANDS) @@ -181,6 +182,8 @@ elseif(CMAKE_SYSTEM_NAME MATCHES "Generic") pico_add_compile_definition(ZENOH_ZEPHYR) elseif(WITH_FREERTOS_PLUS_TCP) pico_add_compile_definition(ZENOH_FREERTOS_PLUS_TCP) +elseif(WITH_RPI_PICO_W) + pico_add_compile_definition(ZENOH_RPI_PICO_W) endif() else() message(FATAL_ERROR "zenoh-pico is not yet available on ${CMAKE_SYSTEM_NAME} platform") @@ -273,6 +276,7 @@ message(STATUS "Unicast batch max size: ${BATCH_UNICAST_SIZE}") message(STATUS "Multicast batch max size: ${BATCH_MULTICAST_SIZE}") message(STATUS "Build for Zephyr RTOS: ${WITH_ZEPHYR}") message(STATUS "Build for FreeRTOS-Plus-TCP: ${WITH_FREERTOS_PLUS_TCP}") +message(STATUS "Build for Raspberry Pico W: ${WITH_RPI_PICO_W}") message(STATUS "Configuring for ${CMAKE_SYSTEM_NAME}") if(SKBUILD) @@ -334,6 +338,9 @@ if(WITH_ZEPHYR) elseif(WITH_FREERTOS_PLUS_TCP) file (GLOB Sources_Freertos_Plus_TCP "src/system/freertos_plus_tcp/*.c") list(APPEND Sources ${Sources_Freertos_Plus_TCP}) +elseif(WITH_RPI_PICO_W) + file (GLOB Sources_RPI_Pico_W "src/system/rpi_pico_w/*.c") + list(APPEND Sources ${Sources_RPI_Pico_W}) elseif(CMAKE_SYSTEM_NAME MATCHES "Linux" OR CMAKE_SYSTEM_NAME MATCHES "Darwin" OR CMAKE_SYSTEM_NAME MATCHES "BSD" OR POSIX_COMPATIBLE) file (GLOB Sources_Unix "src/system/unix/*.c" "src/system/unix/link/*.c") list(APPEND Sources ${Sources_Unix}) diff --git a/examples/rpi_pico_w/CMakeLists.txt b/examples/rpi_pico_w/CMakeLists.txt index f90d3567b..b39470c7a 100644 --- a/examples/rpi_pico_w/CMakeLists.txt +++ b/examples/rpi_pico_w/CMakeLists.txt @@ -26,7 +26,7 @@ pico_enable_stdio_usb(zenohpico_rpi_pico_w_examples 1) pico_enable_stdio_uart(zenohpico_rpi_pico_w_examples 1) set(BUILD_SHARED_LIBS OFF) -set(WITH_FREERTOS_PLUS_TCP ON) +set(WITH_RPI_PICO_W ON) set(ZENOH_DEBUG 5) #add_compile_options(-Wno-strict-prototypes) #add_compile_options(-Wno-pragmas) diff --git a/include/zenoh-pico/system/platform/rpi_pico_w.h b/include/zenoh-pico/system/platform/rpi_pico_w.h new file mode 100644 index 000000000..5becc4602 --- /dev/null +++ b/include/zenoh-pico/system/platform/rpi_pico_w.h @@ -0,0 +1,71 @@ +// +// Copyright (c) 2023 Fictionlab sp. z o.o. +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// Błażej Sowa, + +#ifndef ZENOH_PICO_SYSTEM_FREERTOS_PLUS_TCP_TYPES_H +#define ZENOH_PICO_SYSTEM_FREERTOS_PLUS_TCP_TYPES_H + +#include "FreeRTOS.h" +// #include "FreeRTOS_IP.h" +#include "event_groups.h" +#include "lwip/ip4_addr.h" +#include "semphr.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if Z_FEATURE_MULTI_THREAD == 1 +typedef struct { + const char *name; + UBaseType_t priority; + size_t stack_depth; +#if (configSUPPORT_STATIC_ALLOCATION == 1) + bool static_allocation; + StackType_t *stack_buffer; + StaticTask_t *task_buffer; +#endif /* SUPPORT_STATIC_ALLOCATION */ +} z_task_attr_t; + +typedef struct { + TaskHandle_t handle; + EventGroupHandle_t join_event; +} _z_task_t; + +typedef SemaphoreHandle_t _z_mutex_t; +typedef void *_z_condvar_t; +#endif // Z_MULTI_THREAD == 1 + +typedef struct timespec z_clock_t; +typedef struct timeval z_time_t; + +typedef struct { + union { +#if Z_FEATURE_LINK_TCP == 1 || Z_FEATURE_LINK_UDP_MULTICAST == 1 || Z_FEATURE_LINK_UDP_UNICAST == 1 + int _fd; +#endif + }; +} _z_sys_net_socket_t; + +typedef struct { + union { +#if Z_FEATURE_LINK_TCP == 1 || Z_FEATURE_LINK_UDP_MULTICAST == 1 || Z_FEATURE_LINK_UDP_UNICAST == 1 + struct addrinfo *_iptcp; +#endif + }; +} _z_sys_net_endpoint_t; + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/include/zenoh-pico/system/platform_common.h b/include/zenoh-pico/system/platform_common.h index 60d85c4e6..d76f9c552 100644 --- a/include/zenoh-pico/system/platform_common.h +++ b/include/zenoh-pico/system/platform_common.h @@ -46,6 +46,8 @@ #include "zenoh-pico/system/platform/flipper.h" #elif defined(ZENOH_FREERTOS_PLUS_TCP) #include "zenoh-pico/system/platform/freertos_plus_tcp.h" +#elif defined(ZENOH_RPI_PICO_W) +#include "zenoh-pico/system/platform/rpi_pico_w.h" #else #include "zenoh-pico/system/platform/void.h" #error "Unknown platform" diff --git a/src/system/rpi_pico_w/network.c b/src/system/rpi_pico_w/network.c new file mode 100644 index 000000000..aaf5fc29f --- /dev/null +++ b/src/system/rpi_pico_w/network.c @@ -0,0 +1,264 @@ +// +// Copyright (c) 2024 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, + +#include + +#include "lwip/dns.h" +#include "lwip/ip4_addr.h" +#include "lwip/netdb.h" +#include "lwip/pbuf.h" +#include "lwip/sockets.h" +#include "lwip/udp.h" +#include "zenoh-pico/system/platform.h" +#include "zenoh-pico/utils/logging.h" +#include "zenoh-pico/utils/pointers.h" +#include "zenoh-pico/utils/result.h" + +#if Z_FEATURE_LINK_TCP == 1 +/*------------------ TCP sockets ------------------*/ +z_result_t _z_create_endpoint_tcp(_z_sys_net_endpoint_t *ep, const char *s_address, const char *s_port) { + z_result_t ret = _Z_RES_OK; + + struct addrinfo hints; + (void)memset(&hints, 0, sizeof(hints)); + hints.ai_family = PF_UNSPEC; // Allow IPv4 or IPv6 + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = 0; + hints.ai_protocol = IPPROTO_TCP; + + if (getaddrinfo(s_address, s_port, &hints, &ep->_iptcp) < 0) { + ret = _Z_ERR_GENERIC; + } + + _Z_DEBUG("_z_create_endpoint_tcp: %s:%s -> %li", s_address, s_port, ep->_iptcp->ai_addrlen); + return ret; +} + +void _z_free_endpoint_tcp(_z_sys_net_endpoint_t *ep) { freeaddrinfo(ep->_iptcp); } + +z_result_t _z_open_tcp(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout) { + z_result_t ret = _Z_RES_OK; + + sock->_fd = socket(rep._iptcp->ai_family, rep._iptcp->ai_socktype, rep._iptcp->ai_protocol); + if (sock->_fd != -1) { + // TODO: check what options not supported + /* +z_time_t tv; +tv.tv_sec = tout / (uint32_t)1000; +tv.tv_usec = (tout % (uint32_t)1000) * (uint32_t)1000; +if ((ret == _Z_RES_OK) && (setsockopt(sock->_fd, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, sizeof(tv)) < 0)) { + ret = _Z_ERR_GENERIC; +} + +int flags = 1; +if ((ret == _Z_RES_OK) && + (setsockopt(sock->_fd, SOL_SOCKET, SO_KEEPALIVE, (void *)&flags, sizeof(flags)) < 0)) { + ret = _Z_ERR_GENERIC; +} +#if Z_FEATURE_TCP_NODELAY == 1 +if ((ret == _Z_RES_OK) && + (setsockopt(sock->_fd, IPPROTO_TCP, TCP_NODELAY, (void *)&flags, sizeof(flags)) < 0)) { + ret = _Z_ERR_GENERIC; +} +#endif +struct linger ling; +ling.l_onoff = 1; +ling.l_linger = Z_TRANSPORT_LEASE / 1000; +if ((ret == _Z_RES_OK) && + (setsockopt(sock->_fd, SOL_SOCKET, SO_LINGER, (void *)&ling, sizeof(struct linger)) < 0)) { + ret = _Z_ERR_GENERIC; +} + +*/ + struct addrinfo *it = NULL; + for (it = rep._iptcp; it != NULL; it = it->ai_next) { + if ((ret == _Z_RES_OK) && (connect(sock->_fd, it->ai_addr, it->ai_addrlen) < 0)) { + if (it->ai_next == NULL) { + ret = _Z_ERR_GENERIC; + break; + } + } else { + break; + } + } + + if (ret != _Z_RES_OK) { + close(sock->_fd); + } + } else { + ret = _Z_ERR_GENERIC; + } + + return ret; +} + +z_result_t _z_listen_tcp(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t lep) { + z_result_t ret = _Z_RES_OK; + (void)sock; + (void)lep; + + // @TODO: To be implemented + ret = _Z_ERR_GENERIC; + + return ret; +} + +void _z_close_tcp(_z_sys_net_socket_t *sock) { + shutdown(sock->_fd, SHUT_RDWR); + close(sock->_fd); +} + +size_t _z_read_tcp(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { + ssize_t rb = recv(sock._fd, ptr, len, 0); + if (rb < (ssize_t)0) { + return SIZE_MAX; + } + + return (size_t)rb; +} + +size_t _z_read_exact_tcp(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { + size_t n = 0; + uint8_t *pos = &ptr[0]; + + do { + size_t rb = _z_read_tcp(sock, pos, len - n); + if (rb == SIZE_MAX) { + n = rb; + break; + } + + n = n + rb; + pos = _z_ptr_u8_offset(pos, n); + } while (n != len); + + return n; +} + +size_t _z_send_tcp(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len) { +#if defined(ZENOH_LINUX) + return (size_t)send(sock._fd, ptr, len, MSG_NOSIGNAL); +#else + return send(sock._fd, ptr, len, 0); +#endif +} +#endif + +#if Z_FEATURE_LINK_UDP_UNICAST == 1 || Z_FEATURE_LINK_UDP_MULTICAST == 1 +/*------------------ UDP sockets ------------------*/ +z_result_t _z_create_endpoint_udp(_z_sys_net_endpoint_t *ep, const char *s_address, const char *s_port) { + z_result_t ret = _Z_RES_OK; + + struct addrinfo hints; + (void)memset(&hints, 0, sizeof(hints)); + hints.ai_family = PF_UNSPEC; // Allow IPv4 or IPv6 + hints.ai_socktype = SOCK_DGRAM; + hints.ai_flags = 0; + hints.ai_protocol = IPPROTO_UDP; + + if (getaddrinfo(s_address, s_port, &hints, &ep->_iptcp) < 0) { + ret = _Z_ERR_GENERIC; + } + + return ret; +} + +void _z_free_endpoint_udp(_z_sys_net_endpoint_t *ep) { freeaddrinfo(ep->_iptcp); } +#endif + +#if Z_FEATURE_LINK_UDP_UNICAST == 1 +z_result_t _z_open_udp_unicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout) { + z_result_t ret = _Z_RES_OK; + + sock->_fd = socket(rep._iptcp->ai_family, rep._iptcp->ai_socktype, rep._iptcp->ai_protocol); + if (sock->_fd != -1) { + z_time_t tv; + tv.tv_sec = tout / (uint32_t)1000; + tv.tv_usec = (tout % (uint32_t)1000) * (uint32_t)1000; + if ((ret == _Z_RES_OK) && (setsockopt(sock->_fd, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, sizeof(tv)) < 0)) { + ret = _Z_ERR_GENERIC; + } + + if (ret != _Z_RES_OK) { + close(sock->_fd); + } + } else { + ret = _Z_ERR_GENERIC; + } + + return ret; +} + +z_result_t _z_listen_udp_unicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t lep, uint32_t tout) { + (void)sock; + (void)lep; + (void)tout; + + // @TODO: To be implemented + ret = _Z_ERR_GENERIC; + + return ret; +} + +void _z_close_udp_unicast(_z_sys_net_socket_t *sock) { close(sock->_fd); } + +size_t _z_read_udp_unicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { + struct sockaddr_storage raddr; + unsigned int addrlen = sizeof(struct sockaddr_storage); + + ssize_t rb = recvfrom(sock._fd, ptr, len, 0, (struct sockaddr *)&raddr, &addrlen); + if (rb < (ssize_t)0) { + return SIZE_MAX; + } + return (size_t)rb; +} + +size_t _z_read_exact_udp_unicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { + size_t n = 0; + uint8_t *pos = &ptr[0]; + + do { + size_t rb = _z_read_udp_unicast(sock, pos, len - n); + if (rb == SIZE_MAX) { + n = rb; + break; + } + + n = n + rb; + pos = _z_ptr_u8_offset(pos, (ptrdiff_t)n); + } while (n != len); + + return n; +} + +size_t _z_send_udp_unicast(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len, + const _z_sys_net_endpoint_t rep) { + return (size_t)sendto(sock._fd, ptr, len, 0, rep._iptcp->ai_addr, rep._iptcp->ai_addrlen); +} +#endif + +#if Z_FEATURE_LINK_UDP_MULTICAST == 1 +#error "UDP Multicast not supported yet on Raspberry Pico W port of Zenoh-Pico" +#endif + +#if Z_FEATURE_LINK_BLUETOOTH == 1 +#error "Bluetooth not supported yet on Raspberry Pico W port of Zenoh-Pico" +#endif + +#if Z_FEATURE_LINK_SERIAL == 1 +#error "Serial not supported yet on Raspberry Pico W port of Zenoh-Pico" +#endif + +#if Z_FEATURE_RAWETH_TRANSPORT == 1 +#error "Raw ethernet transport not supported yet on Raspberry Pico W port of Zenoh-Pico" +#endif diff --git a/src/system/rpi_pico_w/system.c b/src/system/rpi_pico_w/system.c new file mode 100644 index 000000000..4caaeefee --- /dev/null +++ b/src/system/rpi_pico_w/system.c @@ -0,0 +1,255 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// Copyright (c) 2023 Fictionlab sp. z o.o. +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// Błażej Sowa, + +#include +#include +#include +#include + +#include "FreeRTOS.h" +#include "zenoh-pico/config.h" +#include "zenoh-pico/system/platform.h" + +/*------------------ Random ------------------*/ +uint8_t z_random_u8(void) { return z_random_u32(); } + +uint16_t z_random_u16(void) { return z_random_u32(); } + +uint32_t z_random_u32(void) { return rand(); } + +uint64_t z_random_u64(void) { + uint64_t ret = 0; + ret |= z_random_u32(); + ret = ret << 32; + ret |= z_random_u32(); + return ret; +} + +void z_random_fill(void *buf, size_t len) { + for (size_t i = 0; i < len; i++) { + *((uint8_t *)buf) = z_random_u8(); + } +} + +/*------------------ Memory ------------------*/ +void *z_malloc(size_t size) { return pvPortMalloc(size); } + +void *z_realloc(void *ptr, size_t size) { return pvPortRealloc(ptr, size); } + +void z_free(void *ptr) { vPortFree(ptr); } + +#if Z_FEATURE_MULTI_THREAD == 1 +// In FreeRTOS, tasks created using xTaskCreate must end with vTaskDelete. +// A task function should __not__ simply return. +typedef struct { + void *(*fun)(void *); + void *arg; + EventGroupHandle_t join_event; +} z_task_arg; + +static void z_task_wrapper(void *arg) { + z_task_arg *targ = (z_task_arg *)arg; + targ->fun(targ->arg); + xEventGroupSetBits(targ->join_event, 1); + vTaskDelete(NULL); +} + +static z_task_attr_t z_default_task_attr = { + .name = "", + .priority = configMAX_PRIORITIES / 2, + .stack_depth = 5120, +#if (configSUPPORT_STATIC_ALLOCATION == 1) + .static_allocation = false, + .stack_buffer = NULL, + .task_buffer = NULL, +#endif /* SUPPORT_STATIC_ALLOCATION */ +}; + +/*------------------ Thread ------------------*/ +z_result_t _z_task_init(_z_task_t *task, z_task_attr_t *attr, void *(*fun)(void *), void *arg) { + z_task_arg *z_arg = (z_task_arg *)z_malloc(sizeof(z_task_arg)); + if (z_arg == NULL) { + return -1; + } + + z_arg->fun = fun; + z_arg->arg = arg; + z_arg->join_event = task->join_event = xEventGroupCreate(); + + if (attr == NULL) { + attr = &z_default_task_attr; + } + +#if (configSUPPORT_STATIC_ALLOCATION == 1) + if (attr->static_allocation) { + task->handle = xTaskCreateStatic(z_task_wrapper, attr->name, attr->stack_depth, z_arg, attr->priority, + attr->stack_buffer, attr->task_buffer); + if (task->handle == NULL) { + return -1; + } + } else { +#endif /* SUPPORT_STATIC_ALLOCATION */ + if (xTaskCreate(z_task_wrapper, attr->name, attr->stack_depth, z_arg, attr->priority, &task->handle) != + pdPASS) { + return -1; + } +#if (configSUPPORT_STATIC_ALLOCATION == 1) + } +#endif /* SUPPORT_STATIC_ALLOCATION */ + + return 0; +} + +z_result_t _z_task_join(_z_task_t *task) { + xEventGroupWaitBits(task->join_event, 1, pdFALSE, pdFALSE, portMAX_DELAY); + return 0; +} + +z_result_t _z_task_detach(_z_task_t *task) { + // Not implemented + return _Z_ERR_GENERIC; +} + +z_result_t _z_task_cancel(_z_task_t *task) { + vTaskDelete(task->handle); + return 0; +} + +void _z_task_free(_z_task_t **task) { + z_free((*task)->join_event); + z_free(*task); +} + +/*------------------ Mutex ------------------*/ +z_result_t _z_mutex_init(_z_mutex_t *m) { + *m = xSemaphoreCreateRecursiveMutex(); + return *m == NULL ? -1 : 0; +} + +z_result_t _z_mutex_drop(_z_mutex_t *m) { + z_free(*m); + return 0; +} + +z_result_t _z_mutex_lock(_z_mutex_t *m) { return xSemaphoreTakeRecursive(*m, portMAX_DELAY) == pdTRUE ? 0 : -1; } + +z_result_t _z_mutex_try_lock(_z_mutex_t *m) { return xSemaphoreTakeRecursive(*m, 0) == pdTRUE ? 0 : -1; } + +z_result_t _z_mutex_unlock(_z_mutex_t *m) { return xSemaphoreGiveRecursive(*m) == pdTRUE ? 0 : -1; } + +/*------------------ CondVar ------------------*/ +// Condition variables not supported in FreeRTOS +z_result_t _z_condvar_init(_z_condvar_t *cv) { return -1; } +z_result_t _z_condvar_drop(_z_condvar_t *cv) { return -1; } +z_result_t _z_condvar_signal(_z_condvar_t *cv) { return -1; } +z_result_t _z_condvar_signal_all(_z_condvar_t *cv) { return -1; } +z_result_t _z_condvar_wait(_z_condvar_t *cv, _z_mutex_t *m) { return -1; } +#endif // Z_MULTI_THREAD == 1 + +/*------------------ Sleep ------------------*/ +z_result_t z_sleep_us(size_t time) { + vTaskDelay(pdMS_TO_TICKS(time / 1000)); + return 0; +} + +z_result_t z_sleep_ms(size_t time) { + vTaskDelay(pdMS_TO_TICKS(time)); + return 0; +} + +z_result_t z_sleep_s(size_t time) { + vTaskDelay(pdMS_TO_TICKS(time * 1000)); + return 0; +} + +/*------------------ Clock ------------------*/ +z_clock_t z_clock_now(void) { + z_clock_t now; + // clock_gettime(CLOCK_MONOTONIC, &now); + return now; +} + +unsigned long z_clock_elapsed_us(z_clock_t *instant) { + z_clock_t now; + // clock_gettime(CLOCK_MONOTONIC, &now); + + unsigned long elapsed = + (unsigned long)(1000000 * (now.tv_sec - instant->tv_sec) + (now.tv_nsec - instant->tv_nsec) / 1000); + return elapsed; +} + +unsigned long z_clock_elapsed_ms(z_clock_t *instant) { + z_clock_t now; + // clock_gettime(CLOCK_MONOTONIC, &now); + + unsigned long elapsed = + (unsigned long)(1000 * (now.tv_sec - instant->tv_sec) + (now.tv_nsec - instant->tv_nsec) / 1000000); + return elapsed; +} + +unsigned long z_clock_elapsed_s(z_clock_t *instant) { + z_clock_t now; + // clock_gettime(CLOCK_MONOTONIC, &now); + + unsigned long elapsed = (unsigned long)(now.tv_sec - instant->tv_sec); + return elapsed; +} + +/*------------------ Time ------------------*/ +z_time_t z_time_now(void) { + z_time_t now; + gettimeofday(&now, NULL); + return now; +} + +const char *z_time_now_as_str(char *const buf, unsigned long buflen) { + z_time_t tv = z_time_now(); + struct tm ts; + ts = *localtime(&tv.tv_sec); + strftime(buf, buflen, "%Y-%m-%dT%H:%M:%SZ", &ts); + return buf; +} + +unsigned long z_time_elapsed_us(z_time_t *time) { + z_time_t now; + gettimeofday(&now, NULL); + + unsigned long elapsed = (unsigned long)(1000000 * (now.tv_sec - time->tv_sec) + (now.tv_usec - time->tv_usec)); + return elapsed; +} + +unsigned long z_time_elapsed_ms(z_time_t *time) { + z_time_t now; + gettimeofday(&now, NULL); + + unsigned long elapsed = (unsigned long)(1000 * (now.tv_sec - time->tv_sec) + (now.tv_usec - time->tv_usec) / 1000); + return elapsed; +} + +unsigned long z_time_elapsed_s(z_time_t *time) { + z_time_t now; + gettimeofday(&now, NULL); + + unsigned long elapsed = (unsigned long)(now.tv_sec - time->tv_sec); + return elapsed; +} + +z_result_t _z_get_time_since_epoch(_z_time_since_epoch *t) { + z_time_t now; + gettimeofday(&now, NULL); + t->secs = (uint32_t)now.tv_sec; + t->nanos = (uint32_t)now.tv_usec * 1000; + return 0; +}