diff --git a/CMakeLists.txt b/CMakeLists.txt index 2192475b2..10ffb57b5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -232,6 +232,7 @@ set(Z_FEATURE_LINK_TCP 1 CACHE STRING "Toggle TCP links") set(Z_FEATURE_LINK_BLUETOOTH 0 CACHE STRING "Toggle Bluetooth links") set(Z_FEATURE_LINK_WS 0 CACHE STRING "Toggle WebSocket links") set(Z_FEATURE_LINK_SERIAL 0 CACHE STRING "Toggle Serial links") +set(Z_FEATURE_LINK_SERIAL_USB 0 CACHE STRING "Toggle Serial USB links") set(Z_FEATURE_SCOUTING_UDP 1 CACHE STRING "Toggle UDP scouting") set(Z_FEATURE_LINK_UDP_MULTICAST 1 CACHE STRING "Toggle UDP multicast links") set(Z_FEATURE_LINK_UDP_UNICAST 1 CACHE STRING "Toggle UDP unicast links") diff --git a/examples/rpi_pico/CMakeLists.txt b/examples/rpi_pico/CMakeLists.txt index 12ce2e237..483eb8d23 100644 --- a/examples/rpi_pico/CMakeLists.txt +++ b/examples/rpi_pico/CMakeLists.txt @@ -14,6 +14,7 @@ set(WIFI_PASSWORD "" CACHE STRING "WiFi Password") set(ZENOH_CONFIG_MODE "client" CACHE STRING "ZENOH_CONFIG_MODE") set(ZENOH_CONFIG_CONNECT CACHE STRING "ZENOH_CONFIG_CONNECT") set(ZENOH_CONFIG_LISTEN CACHE STRING "ZENOH_CONFIG_LISTEN") +option(ZENOH_USB_UART "Enable USB UART" OFF) message(STATUS "PICO_BOARD: ${PICO_BOARD}") message(STATUS "WIFI_SSID: ${WIFI_SSID}") @@ -26,6 +27,7 @@ endif() message(STATUS "ZENOH_CONFIG_MODE: ${ZENOH_CONFIG_MODE}") message(STATUS "ZENOH_CONFIG_CONNECT: ${ZENOH_CONFIG_CONNECT}") message(STATUS "ZENOH_CONFIG_LISTEN: ${ZENOH_CONFIG_LISTEN}") +message(STATUS "ZENOH_USB_UART: ${ZENOH_USB_UART}") configure_file( "${CMAKE_CURRENT_SOURCE_DIR}/include/config.h.in" @@ -52,6 +54,16 @@ else() set(WIFI_LIB "") endif() +if(ZENOH_USB_UART) + set(USB_LIBS + tinyusb_host + tinyusb_device + tinyusb_board + ) +else() + set(USB_LIBS "") +endif() + add_compile_definitions(LWIP_TIMEVAL_PRIVATE=0) pico_sdk_init() @@ -67,6 +79,11 @@ if (NOT WIFI_SUPPORT_ENABLED) endif() declare_cache_var(Z_FEATURE_LINK_SERIAL 1 STRING "Serial support") +if(ZENOH_USB_UART) + declare_cache_var(Z_FEATURE_LINK_SERIAL_USB 1 STRING "Serial USB support") +else() + declare_cache_var(Z_FEATURE_LINK_SERIAL_USB 0 STRING "Serial USB support") +endif() set(BUILD_SHARED_LIBS OFF) set(WITH_RPI_PICO ON) @@ -79,6 +96,7 @@ target_link_libraries(zenohpico_static pico_rand FreeRTOS-Kernel-Heap4 ${WIFI_LIB} + ${USB_LIBS} ) # Configure build @@ -98,6 +116,7 @@ target_link_libraries(main pico_rand FreeRTOS-Kernel-Heap4 ${WIFI_LIB} + ${USB_LIBS} ) function(add_example name) @@ -110,12 +129,17 @@ function(add_example name) FreeRTOS-Kernel-Heap4 zenohpico::lib ${WIFI_LIB} + ${USB_LIBS} ) target_include_directories(${name} PRIVATE ${CMAKE_CURRENT_LIST_DIR} ${CMAKE_CURRENT_LIST_DIR}/include ) - pico_enable_stdio_usb(${name} 1) + if(ZENOH_USB_UART) + pico_enable_stdio_uart(${name} 1) + else() + pico_enable_stdio_usb(${name} 1) + endif() pico_add_extra_outputs(${name}) endfunction() diff --git a/examples/rpi_pico/include/tusb_config.h b/examples/rpi_pico/include/tusb_config.h new file mode 100644 index 000000000..7cfefab94 --- /dev/null +++ b/examples/rpi_pico/include/tusb_config.h @@ -0,0 +1,111 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + */ + +#ifndef _TUSB_CONFIG_H_ +#define _TUSB_CONFIG_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Board Specific Configuration +//--------------------------------------------------------------------+ + +// RHPort number used for device can be defined by board.mk, default to port 0 +#ifndef BOARD_TUD_RHPORT +#define BOARD_TUD_RHPORT 0 +#endif + +// RHPort max operational speed can defined by board.mk +#ifndef BOARD_TUD_MAX_SPEED +#define BOARD_TUD_MAX_SPEED OPT_MODE_DEFAULT_SPEED +#endif + +//-------------------------------------------------------------------- +// COMMON CONFIGURATION +//-------------------------------------------------------------------- + +// defined by board.mk +#ifndef CFG_TUSB_MCU +#error CFG_TUSB_MCU must be defined +#endif + +#ifndef CFG_TUSB_OS +#define CFG_TUSB_OS OPT_OS_NONE +#endif + +#ifndef CFG_TUSB_DEBUG +#define CFG_TUSB_DEBUG 0 +#endif + +// Enable Device stack +#define CFG_TUD_ENABLED 1 + +// Default is max speed that hardware controller could support with on-chip PHY +#define CFG_TUD_MAX_SPEED BOARD_TUD_MAX_SPEED + +/* USB DMA on some MCUs can only access a specific SRAM region with restriction on alignment. + * Tinyusb use follows macros to declare transferring memory so that they can be put + * into those specific section. + * e.g + * - CFG_TUSB_MEM SECTION : __attribute__ (( section(".usb_ram") )) + * - CFG_TUSB_MEM_ALIGN : __attribute__ ((aligned(4))) + */ +#ifndef CFG_TUSB_MEM_SECTION +#define CFG_TUSB_MEM_SECTION +#endif + +#ifndef CFG_TUSB_MEM_ALIGN +#define CFG_TUSB_MEM_ALIGN __attribute__((aligned(4))) +#endif + +//-------------------------------------------------------------------- +// DEVICE CONFIGURATION +//-------------------------------------------------------------------- + +#ifndef CFG_TUD_ENDPOINT0_SIZE +#define CFG_TUD_ENDPOINT0_SIZE 64 +#endif + +//------------- CLASS -------------// +#define CFG_TUD_CDC 2 +#define CFG_TUD_MSC 0 +#define CFG_TUD_HID 0 +#define CFG_TUD_MIDI 0 +#define CFG_TUD_VENDOR 0 + +// CDC FIFO size of TX and RX +#define CFG_TUD_CDC_RX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64) +#define CFG_TUD_CDC_TX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64) + +// CDC Endpoint transfer buffer size, more is faster +#define CFG_TUD_CDC_EP_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64) + +#ifdef __cplusplus +} +#endif + +#endif /* _TUSB_CONFIG_H_ */ diff --git a/include/zenoh-pico/config.h b/include/zenoh-pico/config.h index 1578b1f50..7af396745 100644 --- a/include/zenoh-pico/config.h +++ b/include/zenoh-pico/config.h @@ -35,6 +35,7 @@ #define Z_FEATURE_LINK_BLUETOOTH 0 #define Z_FEATURE_LINK_WS 0 #define Z_FEATURE_LINK_SERIAL 0 +#define Z_FEATURE_LINK_SERIAL_USB 0 #define Z_FEATURE_SCOUTING_UDP 1 #define Z_FEATURE_LINK_UDP_MULTICAST 1 #define Z_FEATURE_LINK_UDP_UNICAST 1 diff --git a/include/zenoh-pico/config.h.in b/include/zenoh-pico/config.h.in index 175fc8fdf..9e58d1bbe 100644 --- a/include/zenoh-pico/config.h.in +++ b/include/zenoh-pico/config.h.in @@ -35,6 +35,7 @@ #define Z_FEATURE_LINK_BLUETOOTH @Z_FEATURE_LINK_BLUETOOTH@ #define Z_FEATURE_LINK_WS @Z_FEATURE_LINK_WS@ #define Z_FEATURE_LINK_SERIAL @Z_FEATURE_LINK_SERIAL@ +#define Z_FEATURE_LINK_SERIAL_USB @Z_FEATURE_LINK_SERIAL_USB@ #define Z_FEATURE_SCOUTING_UDP @Z_FEATURE_SCOUTING_UDP@ #define Z_FEATURE_LINK_UDP_MULTICAST @Z_FEATURE_LINK_UDP_MULTICAST@ #define Z_FEATURE_LINK_UDP_UNICAST @Z_FEATURE_LINK_UDP_UNICAST@ diff --git a/include/zenoh-pico/system/platform/rpi_pico.h b/include/zenoh-pico/system/platform/rpi_pico.h index dd6b13dc9..b5480548c 100644 --- a/include/zenoh-pico/system/platform/rpi_pico.h +++ b/include/zenoh-pico/system/platform/rpi_pico.h @@ -78,4 +78,11 @@ typedef struct { } #endif +#if Z_FEATURE_LINK_SERIAL_USB == 1 +void _z_usb_uart_init(); +void _z_usb_uart_deinit(); +void _z_usb_uart_write(const uint8_t *buf, int length); +uint8_t _z_usb_uart_getc(); +#endif + #endif // ZENOH_PICO_SYSTEM_RPI_PICO_TYPES_H diff --git a/src/system/rpi_pico/network.c b/src/system/rpi_pico/network.c index 0fa21de0f..db1091986 100644 --- a/src/system/rpi_pico/network.c +++ b/src/system/rpi_pico/network.c @@ -533,11 +533,15 @@ z_result_t _z_open_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, u z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_t baudrate) { z_result_t ret = _Z_RES_OK; - if (!sock || baudrate == 0) { - return _Z_ERR_INVALID; + sock->_serial = NULL; + +#if Z_FEATURE_LINK_SERIAL_USB == 1 + if (strcmp("usb", dev) == 0) { + _z_usb_uart_init(); + return ret; } +#endif - sock->_serial = NULL; uint32_t txpin = 0; uint32_t rxpin = 0; @@ -584,7 +588,15 @@ z_result_t _z_listen_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint3 return ret; } -void _z_close_serial(_z_sys_net_socket_t *sock) { uart_deinit(sock->_serial); } +void _z_close_serial(_z_sys_net_socket_t *sock) { + if (sock->_serial != NULL) { + uart_deinit(sock->_serial); + } else { +#if Z_FEATURE_LINK_SERIAL_USB == 1 + _z_usb_uart_deinit(); +#endif + } +} size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) { z_result_t ret = _Z_RES_OK; @@ -592,7 +604,11 @@ size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) uint8_t *before_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); size_t rb = 0; for (size_t i = 0; i < _Z_SERIAL_MAX_COBS_BUF_SIZE; i++) { +#if Z_FEATURE_LINK_SERIAL_USB == 1 + before_cobs[i] = (sock._serial == NULL) ? _z_usb_uart_getc() : uart_getc(sock._serial); +#else before_cobs[i] = uart_getc(sock._serial); +#endif rb = rb + (size_t)1; if (before_cobs[i] == (uint8_t)0x00) { @@ -677,7 +693,13 @@ size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t uint8_t *after_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE); ssize_t twb = _z_cobs_encode(before_cobs, i, after_cobs); after_cobs[twb] = 0x00; // Manually add the COBS delimiter - uart_write_blocking(sock._serial, after_cobs, twb + (ssize_t)1); + if (sock._serial == NULL) { +#if Z_FEATURE_LINK_SERIAL_USB == 1 + _z_usb_uart_write(after_cobs, twb + (ssize_t)1); +#endif + } else { + uart_write_blocking(sock._serial, after_cobs, twb + (ssize_t)1); + } z_free(before_cobs); z_free(after_cobs); diff --git a/src/system/rpi_pico/usb_uart.c b/src/system/rpi_pico/usb_uart.c new file mode 100644 index 000000000..19e86cd62 --- /dev/null +++ b/src/system/rpi_pico/usb_uart.c @@ -0,0 +1,239 @@ +// +// 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 "zenoh-pico/system/platform.h" + +#if Z_FEATURE_LINK_SERIAL == 1 && Z_FEATURE_LINK_SERIAL_USB == 1 + +#include "pico/unique_id.h" +#include "tusb.h" + +// ===== USB descriptors ===== +#ifndef USBD_VID +#define USBD_VID (0x2E8A) // Raspberry Pi +#endif + +#ifndef USBD_PID +#if PICO_RP2040 +#define USBD_PID (0x000a) // Raspberry Pi Pico SDK CDC for RP2040 +#else +#define USBD_PID (0x0009) // Raspberry Pi Pico SDK CDC +#endif +#endif + +#ifndef USBD_MANUFACTURER +#define USBD_MANUFACTURER "Raspberry Pi" +#endif + +#ifndef USBD_PRODUCT +#define USBD_PRODUCT "Pico" +#endif + +#define TUD_RPI_RESET_DESC_LEN 9 +#if !PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE +#define USBD_DESC_LEN (TUD_CONFIG_DESC_LEN + TUD_CDC_DESC_LEN) +#else +#define USBD_DESC_LEN (TUD_CONFIG_DESC_LEN + TUD_CDC_DESC_LEN + TUD_RPI_RESET_DESC_LEN) +#endif +#if !PICO_STDIO_USB_DEVICE_SELF_POWERED +#define USBD_CONFIGURATION_DESCRIPTOR_ATTRIBUTE (0) +#define USBD_MAX_POWER_MA (250) +#else +#define USBD_CONFIGURATION_DESCRIPTOR_ATTRIBUTE TUSB_DESC_CONFIG_ATT_SELF_POWERED +#define USBD_MAX_POWER_MA (1) +#endif + +#define USBD_ITF_CDC (0) // needs 2 interfaces +#if !PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE +#define USBD_ITF_MAX (2) +#else +#define USBD_ITF_RPI_RESET (2) +#define USBD_ITF_MAX (3) +#endif + +#define USBD_CDC_EP_CMD (0x81) +#define USBD_CDC_EP_OUT (0x02) +#define USBD_CDC_EP_IN (0x82) +#define USBD_CDC_CMD_MAX_SIZE (8) +#define USBD_CDC_IN_OUT_MAX_SIZE (64) + +#define USBD_STR_0 (0x00) +#define USBD_STR_MANUF (0x01) +#define USBD_STR_PRODUCT (0x02) +#define USBD_STR_SERIAL (0x03) +#define USBD_STR_CDC (0x04) +#define USBD_STR_RPI_RESET (0x05) + +// Note: descriptors returned from callbacks must exist long enough for transfer to complete + +static const tusb_desc_device_t usbd_desc_device = { + .bLength = sizeof(tusb_desc_device_t), + .bDescriptorType = TUSB_DESC_DEVICE, +// On Windows, if bcdUSB = 0x210 then a Microsoft OS 2.0 descriptor is required, else the device won't be detected +// This is only needed for driverless access to the reset interface - the CDC interface doesn't require these +// descriptors for driverless access, but will still not work if bcdUSB = 0x210 and no descriptor is provided. Therefore +// always use bcdUSB = 0x200 if the Microsoft OS 2.0 descriptor isn't enabled +#if PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE && PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR + .bcdUSB = 0x0210, +#else + .bcdUSB = 0x0200, +#endif + .bDeviceClass = TUSB_CLASS_MISC, + .bDeviceSubClass = MISC_SUBCLASS_COMMON, + .bDeviceProtocol = MISC_PROTOCOL_IAD, + .bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE, + .idVendor = USBD_VID, + .idProduct = USBD_PID, + .bcdDevice = 0x0100, + .iManufacturer = USBD_STR_MANUF, + .iProduct = USBD_STR_PRODUCT, + .iSerialNumber = USBD_STR_SERIAL, + .bNumConfigurations = 1, +}; + +#define TUD_RPI_RESET_DESCRIPTOR(_itfnum, _stridx) \ + /* Interface */ \ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 0, TUSB_CLASS_VENDOR_SPECIFIC, RESET_INTERFACE_SUBCLASS, \ + RESET_INTERFACE_PROTOCOL, _stridx, + +static const uint8_t usbd_desc_cfg[USBD_DESC_LEN] = { + TUD_CONFIG_DESCRIPTOR(1, USBD_ITF_MAX, USBD_STR_0, USBD_DESC_LEN, USBD_CONFIGURATION_DESCRIPTOR_ATTRIBUTE, + USBD_MAX_POWER_MA), + + TUD_CDC_DESCRIPTOR(USBD_ITF_CDC, USBD_STR_CDC, USBD_CDC_EP_CMD, USBD_CDC_CMD_MAX_SIZE, USBD_CDC_EP_OUT, + USBD_CDC_EP_IN, USBD_CDC_IN_OUT_MAX_SIZE), + +#if PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE + TUD_RPI_RESET_DESCRIPTOR(USBD_ITF_RPI_RESET, USBD_STR_RPI_RESET) +#endif +}; + +static char usbd_serial_str[PICO_UNIQUE_BOARD_ID_SIZE_BYTES * 2 + 1]; + +static const char *const usbd_desc_str[] = { + [USBD_STR_MANUF] = USBD_MANUFACTURER, [USBD_STR_PRODUCT] = USBD_PRODUCT, + [USBD_STR_SERIAL] = usbd_serial_str, [USBD_STR_CDC] = "Board CDC", +#if PICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE + [USBD_STR_RPI_RESET] = "Reset", +#endif +}; + +const uint8_t *tud_descriptor_device_cb(void) { return (const uint8_t *)&usbd_desc_device; } + +const uint8_t *tud_descriptor_configuration_cb(__unused uint8_t index) { return usbd_desc_cfg; } + +const uint16_t *tud_descriptor_string_cb(uint8_t index, __unused uint16_t langid) { +#ifndef USBD_DESC_STR_MAX +#define USBD_DESC_STR_MAX (20) +#elif USBD_DESC_STR_MAX > 127 +#error USBD_DESC_STR_MAX too high (max is 127). +#elif USBD_DESC_STR_MAX < 17 +#error USBD_DESC_STR_MAX too low (min is 17). +#endif + static uint16_t desc_str[USBD_DESC_STR_MAX]; + + // Assign the SN using the unique flash id + if (!usbd_serial_str[0]) { + pico_get_unique_board_id_string(usbd_serial_str, sizeof(usbd_serial_str)); + } + + uint8_t len; + if (index == 0) { + desc_str[1] = 0x0409; // supported language is English + len = 1; + } else { + if (index >= sizeof(usbd_desc_str) / sizeof(usbd_desc_str[0])) { + return NULL; + } + const char *str = usbd_desc_str[index]; + for (len = 0; len < USBD_DESC_STR_MAX - 1 && str[len]; ++len) { + desc_str[1 + len] = str[len]; + } + } + + // first byte is length (including header), second byte is string type + desc_str[0] = (uint16_t)((TUSB_DESC_STRING << 8) | (2 * len + 2)); + + return desc_str; +} + +// ===== USB UART methods ===== +static _z_mutex_t _z_usb_uart_mutex; +static _z_task_t _z_usb_uart_task; +static volatile bool _z_usb_uart_task_run; + +static void *_z_usb_uart_task_proc(void *) { + while (_z_usb_uart_task_run) { + _z_mutex_lock(&_z_usb_uart_mutex); + tud_task(); + _z_mutex_unlock(&_z_usb_uart_mutex); + z_sleep_ms(1); + } +} + +void _z_usb_uart_init() { + tud_init(BOARD_TUD_RHPORT); + _z_mutex_init(&_z_usb_uart_mutex); + _z_usb_uart_task_run = true; + _z_task_init(&_z_usb_uart_task, NULL, &_z_usb_uart_task_proc, NULL); + z_sleep_s(10); +} + +void _z_usb_uart_deinit() { + _z_usb_uart_task_run = false; + _z_task_join(&_z_usb_uart_task); + _z_mutex_drop(&_z_usb_uart_mutex); + tud_deinit(BOARD_TUD_RHPORT); +} + +void _z_usb_uart_write(const uint8_t *buf, int length) { + _z_mutex_lock(&_z_usb_uart_mutex); + for (int i = 0; i < length;) { + int n = length - i; + int avail = (int)tud_cdc_write_available(); + if (n > avail) { + n = avail; + } + if (n != 0) { + int n2 = (int)tud_cdc_write(buf + i, (uint32_t)n); + tud_task(); + tud_cdc_write_flush(); + i += n2; + } else { + tud_task(); + tud_cdc_write_flush(); + } + } + _z_mutex_unlock(&_z_usb_uart_mutex); +} + +uint8_t _z_usb_uart_getc() { + uint8_t ret = 0; + bool ready = false; + while (true) { + _z_mutex_lock(&_z_usb_uart_mutex); + ready = tud_cdc_available(); + if (ready) { + ret = tud_cdc_read_char(); + } + _z_mutex_unlock(&_z_usb_uart_mutex); + if (ready) { + break; + } else { + z_sleep_ms(1); + } + } + return ret; +} +#endif