From 8c0c4bdace99e25c2dcf18ab6cf450ce093a9d7b Mon Sep 17 00:00:00 2001 From: Jorge Ramirez-Ortiz Date: Fri, 21 Mar 2025 15:21:28 +0100 Subject: [PATCH] net: latmon: Add latency monitor service Latmon service to interface with Xenomai's Latmus. Link: https://evlproject.org/core/benchmarks/#latmus-gpio-response-time Signed-off-by: Jorge Ramirez-Ortiz --- doc/connectivity/networking/api/latmon.rst | 124 ++++ doc/connectivity/networking/api/protocols.rst | 1 + include/zephyr/net/latmon.h | 97 +++ subsys/net/lib/CMakeLists.txt | 1 + subsys/net/lib/Kconfig | 2 + subsys/net/lib/latmon/CMakeLists.txt | 5 + subsys/net/lib/latmon/Kconfig | 58 ++ subsys/net/lib/latmon/latmon.c | 568 ++++++++++++++++++ 8 files changed, 856 insertions(+) create mode 100644 doc/connectivity/networking/api/latmon.rst create mode 100644 include/zephyr/net/latmon.h create mode 100644 subsys/net/lib/latmon/CMakeLists.txt create mode 100644 subsys/net/lib/latmon/Kconfig create mode 100644 subsys/net/lib/latmon/latmon.c diff --git a/doc/connectivity/networking/api/latmon.rst b/doc/connectivity/networking/api/latmon.rst new file mode 100644 index 00000000000..2679953be1f --- /dev/null +++ b/doc/connectivity/networking/api/latmon.rst @@ -0,0 +1,124 @@ +.. _latmon: + +Latmon Network Service +###################### + +.. contents:: + :local: + :depth: 2 + +Overview +******** + +Provides the functionality required for network-based latency monitoring, including socket +management, client-server communication, and data exchange with the Latmus service running +on the System Under Test (SUT). + +The Latmon network service is responsible for establishing and managing network +communication between the Latmon application (running on a Zephyr-based board) and +the Latmus service (running on the SUT). + +It uses TCP sockets for reliable communication and UDP sockets for broadcasting +the IP address of the Latmon device. + +API Reference +************* + +.. doxygengroup:: latmon + +Features +******** + +- **Socket Management**: Creates and manages TCP and UDP sockets for communication. +- **Client-Server Communication**: Handles incoming connections from the Latmus service. +- **Data Exchange**: Sends latency metrics and histogram data to the Latmus service. +- **IP Address Broadcasting**: Broadcasts the IP address of the Latmon device to facilitate + discovery by the Latmus service. +- **Thread-Safe Design**: Uses Zephyr's kernel primitives (e.g., message queues and semaphores) for + synchronization. + +Workflow +******** + +Socket Creation +=============== + +The :c:func:`net_latmon_get_socket()` function is called to create and configure a TCP socket to +communicate with the Latmus service. A connection address can be specified as a paramenter to +bind the socket to a specific interface and port. + +Connection Handling +=================== + +The :c:func:`net_latmon_connect()` function waits for a connection from the Latmus service. +If no connection is received within the timeout period, the service broadcasts its IP address +using UDP and returns ``-EAGAIN``. +If the broadcast request cannot be sent, the function returns ``-1``, and the client should quit. + +Monitoring Start +================ + +Once a connection is established, the :c:func:`net_latmon_start()` function is called to +start the monitoring process. This function uses a callback to calculate latency deltas +and sends the data to the Latmus service. + +Monitoring Status +================= + +The :c:func:`net_latmon_running()` function can be used to check if the monitoring process is active. + +Thread Management +================= + +The service uses Zephyr threads to handle incoming connections and manage the monitoring +process. + +Enabling the Latmon Service +*************************** + +The following configuration option must be enabled in the :file:`prj.conf` file. + +- :kconfig:option:`CONFIG_NET_LATMON` + +The following options may be configured to customize the Latmon service: + +- :kconfig:option:`CONFIG_NET_LATMON_PORT` - Port number for the Latmon service. +- :kconfig:option:`CONFIG_NET_LATMON_XFER_THREAD_STACK_SIZE` +- :kconfig:option:`CONFIG_NET_LATMON_XFER_THREAD_PRIORITY` +- :kconfig:option:`CONFIG_NET_LATMON_THREAD_STACK_SIZE` +- :kconfig:option:`CONFIG_NET_LATMON_THREAD_PRIORITY` +- :kconfig:option:`CONFIG_NET_LATMON_MONITOR_THREAD_STACK_SIZE` +- :kconfig:option:`CONFIG_NET_LATMON_MONITOR_THREAD_PRIORITY` + +Example Usage +************* + +.. code-block:: c + + #include + #include + + void main(void) + { + struct in_addr ip; + int server_socket, client_socket; + + /* Create and configure the server socket */ + server_socket = net_latmon_get_socket(NULL); + + while (1) { + /* Wait for a connection from the Latmus service */ + client_socket = net_latmon_connect(server_socket, &ip); + if (client_socket < 0) { + if (client_socket == -EAGAIN) { + continue; + } + goto out; + } + + /* Start the latency monitoring process */ + net_latmon_start(client_socket, measure_latency_cycles); + } + out: + close(server_socket); + } diff --git a/doc/connectivity/networking/api/protocols.rst b/doc/connectivity/networking/api/protocols.rst index f4f3b1f88c1..b172089cd45 100644 --- a/doc/connectivity/networking/api/protocols.rst +++ b/doc/connectivity/networking/api/protocols.rst @@ -17,3 +17,4 @@ Protocols mqtt_sn ptp tftp + latmon diff --git a/include/zephyr/net/latmon.h b/include/zephyr/net/latmon.h new file mode 100644 index 00000000000..1a6831761a9 --- /dev/null +++ b/include/zephyr/net/latmon.h @@ -0,0 +1,97 @@ +/** @file + * @brief Latency Monitor API + */ + +/* + * Copyright (c) 2025 Jorge A. Ramirez Ortiz + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_INCLUDE_NET_LATMON_H_ +#define ZEPHYR_INCLUDE_NET_LATMON_H_ + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Latency Monitor + * @defgroup latmon Latency Monitor + * @ingroup networking + * @{ + */ + +/** + * @typedef net_latmon_measure_t + * @brief Callback function type for retrieving latency deltas. + * + * @param delta Pointer to store the calculated latency delta in cycles. + * @return 0 on success, negative errno code on failure. + */ +typedef int (*net_latmon_measure_t)(uint32_t *delta); + +/** + * @brief Start the latency monitor. + * + * @details This function starts the latency monitor, which measures + * latency using the provided callback function to calculate deltas. Samples + * are sent to the connected Latmus client. + * + * @param latmus A valid socket descriptor connected to latmus + * @param measure_func A callback function to execute the delta calculation. + */ +void net_latmon_start(int latmus, net_latmon_measure_t measure_func); + +/** + * @brief Wait for a connection from a Latmus client. + * + * @details This function blocks until a Latmus client connects to the + * specified socket. Once connected, the client's IP address is stored + * in the provided `ip` structure. + * + * @param socket A valid socket descriptor for listening. + * @param ip The client's IP address. + * @return A valid client socket descriptor connected to latmus on success, + * negative errno code on failure. + * + */ +int net_latmon_connect(int socket, struct in_addr *ip); + +/** + * @brief Get a socket for the Latmus service. + * + * @details This function creates and returns a socket to wait for Latmus + * connections + * + * @param bind_addr The address to bind the socket to. If NULL, the socket + * will be bound to the first available address using the build time configured + * latmus port. + * + * @return A valid socket descriptor on success, negative errno code on failure. + */ +int net_latmon_get_socket(struct sockaddr *bind_addr); + +/** + * @brief Check if the latency monitor is running. + * + * @details This function checks whether the latency monitor is currently + * active and running. + * + * @return True if the latency monitor is running, false if it is waiting for a + * Latmus connection + */ +bool net_latmon_running(void); + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /* ZEPHYR_INCLUDE_NET_LATMON_H_ */ diff --git a/subsys/net/lib/CMakeLists.txt b/subsys/net/lib/CMakeLists.txt index ee23de2ab02..1a0d432cdaa 100644 --- a/subsys/net/lib/CMakeLists.txt +++ b/subsys/net/lib/CMakeLists.txt @@ -8,6 +8,7 @@ add_subdirectory_ifdef(CONFIG_SNTP sntp) add_subdirectory_ifdef(CONFIG_MQTT_LIB mqtt) add_subdirectory_ifdef(CONFIG_MQTT_SN_LIB mqtt_sn) add_subdirectory_ifdef(CONFIG_PTP ptp) +add_subdirectory_ifdef(CONFIG_NET_LATMON latmon) add_subdirectory_ifdef(CONFIG_TFTP_LIB tftp) add_subdirectory_ifdef(CONFIG_NET_CONFIG_SETTINGS config) add_subdirectory_ifdef(CONFIG_NET_SOCKETS sockets) diff --git a/subsys/net/lib/Kconfig b/subsys/net/lib/Kconfig index e530877125a..84d88788cab 100644 --- a/subsys/net/lib/Kconfig +++ b/subsys/net/lib/Kconfig @@ -7,6 +7,8 @@ source "subsys/net/lib/coap/Kconfig" source "subsys/net/lib/dns/Kconfig" +source "subsys/net/lib/latmon/Kconfig" + source "subsys/net/lib/mqtt/Kconfig" source "subsys/net/lib/mqtt_sn/Kconfig" diff --git a/subsys/net/lib/latmon/CMakeLists.txt b/subsys/net/lib/latmon/CMakeLists.txt new file mode 100644 index 00000000000..ce29c56f0d5 --- /dev/null +++ b/subsys/net/lib/latmon/CMakeLists.txt @@ -0,0 +1,5 @@ +zephyr_library() + +zephyr_library_sources( + latmon.c +) diff --git a/subsys/net/lib/latmon/Kconfig b/subsys/net/lib/latmon/Kconfig new file mode 100644 index 00000000000..d798d84838d --- /dev/null +++ b/subsys/net/lib/latmon/Kconfig @@ -0,0 +1,58 @@ +# Copyright (c) 2025 Jorge A. Ramirez-Ortiz +# SPDX-License-Identifier: Apache-2.0 + +config NET_LATMON + bool "Latency monitoring support" + select EXPERIMENTAL + select NET_SOCKETS + depends on NET_TCP + help + This option enables the latency monitoring support for Zephyr + +config NET_LATMON_PORT + int "Latmon - Latmus communication port" + default 2306 + help + Specify the port number used for Latmon - Latmus communication. + +config NET_LATMON_XFER_THREAD_STACK_SIZE + int "Stack size for the network transfer thread" + default 8192 + help + Specify the stack size for the network transfer thread used in latency monitoring. + +config NET_LATMON_XFER_THREAD_PRIORITY + int "Priority for the network transfer thread" + default 14 + help + Specify the priority for the network transfer thread used in latency monitoring. + +config NET_LATMON_THREAD_STACK_SIZE + int "Stack size for the Latmon thread" + default 8192 + help + Specify the stack size for the Latmon thread used in latency monitoring. + +config NET_LATMON_THREAD_PRIORITY + int "Priority for the Latmon thread" + default 14 + help + Specify the priority for the Latmon thread used in latency monitoring. + +config NET_LATMON_MONITOR_THREAD_STACK_SIZE + int "Stack size for the monitor thread" + default 8192 + help + Specify the stack size for the monitor thread used in latency monitoring. + +config NET_LATMON_MONITOR_THREAD_PRIORITY + int "Priority for the monitor thread" + default -16 + help + Specify the priority for the monitor thread used in latency monitoring. + +module = LATMON +module-dep = NET_LOG +module-str = Latency monitoring Service +module-help = This option enables the latency monitoring support for Zephyr +source "subsys/net/Kconfig.template.log_config.net" diff --git a/subsys/net/lib/latmon/latmon.c b/subsys/net/lib/latmon/latmon.c new file mode 100644 index 00000000000..7b6fddc97c0 --- /dev/null +++ b/subsys/net/lib/latmon/latmon.c @@ -0,0 +1,568 @@ +/* + * + * SPDX-License-Identifier: Apache-2.0 + * + * Copyright (c) 2025 Jorge Ramirez-Ortiz + */ + + #include +LOG_MODULE_REGISTER(latmon, CONFIG_LATMON_LOG_LEVEL); + +#include +#include + +/* Latmon < -- > Latmus Interface */ +#define LATMON_NET_PORT CONFIG_NET_LATMON_PORT + +struct latmon_net_request { + uint32_t period_usecs; + uint32_t histogram_cells; +} __packed; + +struct latmon_net_data { + int32_t sum_lat_hi; + int32_t sum_lat_lo; + int32_t min_lat; + int32_t max_lat; + uint32_t overruns; + uint32_t samples; +} __packed; + +/* Private IPC: Zephyr application to Latmon service */ +struct latmon_message { + net_latmon_measure_t measure_func; + int latmus; /* latmus connection */ +}; + +K_MSGQ_DEFINE(latmon_msgq, sizeof(struct latmon_message), 2, 4); + +/* + * Note: Using a small period (e.g., less than 100 microseconds) may result in + * the reporting too good interrupt latencies during a short test due to cache + * effects. + */ +struct latmus_conf { + uint32_t max_samples; + uint32_t period; /* in usecs */ + uint32_t cells; +}; + +/* + * Each cell represents a 1 usec timespan. + * note: the sampling period cannot be longer than 1 sec. + */ +#define MAX_SAMPLING_PERIOD_USEC 1000000 +#define HISTOGRAM_CELLS_MAX 1000 +struct latmon_data { + bool warmed; /* sample data can be used */ + uint32_t histogram[HISTOGRAM_CELLS_MAX]; + uint32_t current_samples; + uint32_t overruns; + uint32_t min_lat; + uint32_t max_lat; + uint64_t sum_lat; +}; + +/* Message queue for sample data transfers */ +K_MSGQ_DEFINE(xfer_msgq, sizeof(struct latmon_data), 10, 4); + +/* Network transfer thread: sends data to Latmus */ +#define XFER_THREAD_STACK_SIZE CONFIG_NET_LATMON_XFER_THREAD_STACK_SIZE +#define XFER_THREAD_PRIORITY CONFIG_NET_LATMON_XFER_THREAD_PRIORITY +K_THREAD_STACK_DEFINE(xfer_thread_stack, XFER_THREAD_STACK_SIZE); +static struct k_thread xfer_thread; + +/* Latmon thread: receives application requests */ +#define LATMON_THREAD_PRIORITY CONFIG_NET_LATMON_THREAD_PRIORITY +#define LATMON_STACK_SIZE CONFIG_NET_LATMON_THREAD_STACK_SIZE + +/* Monitor thread: performs the sampling */ +#define MONITOR_THREAD_PRIORITY CONFIG_NET_LATMON_MONITOR_THREAD_PRIORITY +#define MONITOR_STACK_SIZE CONFIG_NET_LATMON_MONITOR_THREAD_STACK_SIZE +static K_THREAD_STACK_DEFINE(monitor_stack, MONITOR_STACK_SIZE); + +static struct k_thread monitor_thread; +static k_tid_t monitor_tid; +static bool abort_monitor; + +/* Synchronization */ +static K_SEM_DEFINE(latmon_done, 0, 1); +static K_SEM_DEFINE(monitor_done, 0, 1); + +static ssize_t send_net_data(int latmus, const void *buf, size_t count) +{ + ssize_t total_written = 0; + ssize_t bytes_written; + + while (count > 0) { + const char *p = (const char *)buf + total_written; + + bytes_written = zsock_send(latmus, p, count, 0); + if (bytes_written < 0) { + if (errno == EINTR) { + continue; + } + return -1; + } + if (bytes_written == 0) { + break; + } + + total_written += bytes_written; + count -= bytes_written; + } + + return total_written; +} + +static int send_sample_data(int latmus, struct latmon_data *data) +{ + struct latmon_net_data ndata = { + .sum_lat_lo = htonl(data->sum_lat & 0xffffffff), + .sum_lat_hi = htonl(data->sum_lat >> 32), + .samples = htonl(data->current_samples), + .overruns = htonl(data->overruns), + .min_lat = htonl(data->min_lat), + .max_lat = htonl(data->max_lat), + }; + + /* Reset the data */ + data->min_lat = UINT32_MAX; + data->current_samples = 0; + data->overruns = 0; + data->max_lat = 0; + data->sum_lat = 0; + + return (send_net_data(latmus, &ndata, sizeof(ndata)) <= 0 ? -1 : 0); +} + +static int send_trailing_data(int latmus, struct latmus_conf *conf, + struct latmon_data *data) +{ + int count = conf->cells * sizeof(data->histogram[0]); + ssize_t ret = 0; + + if (data->current_samples != 0 && send_sample_data(latmus, data) < 0) { + return -1; + } + + /* send empty frame */ + if (send_sample_data(latmus, data) < 0) { + return -1; + } + + /* send histogram if enabled (ie, conf->cells > 0) */ + for (int cell = 0; cell < conf->cells; cell++) { + data->histogram[cell] = htonl(data->histogram[cell]); + } + + ret = send_net_data(latmus, data->histogram, count); + memset(data->histogram, 0, count); + + if (ret < 0) { + LOG_INF("failed tx histogram (ret=%d, errno %d)", ret, errno); + return -1; + } + + return 0; +} + +static int prepare_sample_data(uint32_t delta, struct latmus_conf *conf, + struct latmon_data *data) +{ + uint32_t delta_ns = k_cyc_to_ns_floor64(delta); + uint32_t delta_us = delta_ns / 1000; + + data->sum_lat += delta_ns; + + if (delta_ns < data->min_lat) { + data->min_lat = delta_ns; + } + + if (delta_ns > data->max_lat) { + data->max_lat = delta_ns; + } + + while (delta_us > conf->period) { + data->overruns++; + delta_us -= conf->period; + } + + if (conf->cells != 0) { + if (delta_us >= conf->cells) { + /* histogram outlier */ + delta_us = conf->cells - 1; + } + + data->histogram[delta_us]++; + } + + return ++data->current_samples < conf->max_samples ? -EAGAIN : 0; +} + +static int enqueue_sample_data(struct latmon_data *data) +{ + int ret = 0; + + /* Drop the warming samples */ + if (data->warmed == false) { + data->warmed = true; + goto out; + } + + /* Enqueue the data for transfer */ + ret = k_msgq_put(&xfer_msgq, data, K_NO_WAIT); + if (ret < 0) { + LOG_ERR("Failed to enqueue netdata (queue full)"); + } +out: + /* Reset the data */ + data->min_lat = UINT32_MAX; + data->current_samples = 0; + data->overruns = 0; + data->max_lat = 0; + data->sum_lat = 0; + + return ret; +} + +static void xfer_thread_func(void *p1, void *p2, void *p3) +{ + int latmus = *(int *)p1; + struct latmon_data sample; + + LOG_INF("Transfer thread priority: %d", XFER_THREAD_PRIORITY); + + for (;;) { + if (k_msgq_get(&xfer_msgq, &sample, K_FOREVER) != 0) { + LOG_ERR("Failed to get sample data to transfer"); + continue; + } + + if (send_sample_data(latmus, &sample) < 0) { + LOG_ERR("Failed to transfer sample data"); + break; + } + } +} + +static void start_xfer_thread(int *latmus) +{ + k_thread_create(&xfer_thread, xfer_thread_stack, XFER_THREAD_STACK_SIZE, + xfer_thread_func, latmus, NULL, NULL, + XFER_THREAD_PRIORITY, 0, K_MSEC(10)); +} + +static void abort_xfer_thread(void) +{ + k_thread_abort(&xfer_thread); +} + +static int measure(uint32_t *delta, struct latmon_message *msg, + struct latmon_data *data, + struct latmus_conf *conf) +{ + if (data->warmed == true) { + k_usleep(conf->period); + } + + if (msg->measure_func(delta) < 0) { + if (data->overruns++ > conf->max_samples / 2) { + return -1; + } + /* Just an overrun */ + return 1; + } + return 0; +} + +static void monitor_thread_func(void *p1, void *p2, void *p3) +{ + struct latmon_message *msg = p1; + struct latmus_conf *conf = p2; + struct latmon_data *data = p3; + uint32_t delta = 0; + int ret = 0; + + LOG_INF("Monitor thread priority: %d", MONITOR_THREAD_PRIORITY); + + /* Prepare transfer thread */ + start_xfer_thread(&msg->latmus); + + LOG_INF("\tmonitoring started:"); + LOG_INF("\t - samples per period: %u", conf->max_samples); + LOG_INF("\t - period: %u usecs", conf->period); + LOG_INF("\t - histogram cells: %u", conf->cells); + + /* Sampling loop */ + memset(data, 0, sizeof(*data)); + data->min_lat = UINT32_MAX; + data->warmed = false; + delta = 0; + do { + ret = measure(&delta, msg, data, conf); + if (ret != 0) { + if (ret < 0) { + LOG_ERR("\tExcessive overruns, abort!"); + goto out; + } + continue; + } + + if (prepare_sample_data(delta, conf, data) == -EAGAIN) { + continue; + } + + ret = enqueue_sample_data(data); + /* Abort allowed after all samples have been queued */ + } while (abort_monitor == false && ret == 0); +out: + abort_xfer_thread(); + k_sem_give(&monitor_done); + monitor_tid = NULL; + + LOG_INF("\tmonitoring stopped"); +} + +static int broadcast_ip_address(struct in_addr *ip_addr) +{ + char ip_str[NET_IPV4_ADDR_LEN]; + struct sockaddr_in broadcast; + int sock = -1; + int ret = -1; + + if (ip_addr == NULL || ip_addr->s_addr == INADDR_ANY) { + LOG_ERR("Invalid IP address for broadcast"); + return -1; + } + + sock = zsock_socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (sock < 0) { + LOG_ERR("Failed to create broadcast socket : %d", errno); + return -1; + } + + broadcast.sin_addr.s_addr = htonl(INADDR_BROADCAST); + broadcast.sin_port = htons(LATMON_NET_PORT); + broadcast.sin_family = AF_INET; + + if (net_addr_ntop(AF_INET, ip_addr, ip_str, sizeof(ip_str)) == NULL) { + LOG_ERR("Failed to convert IP address to string"); + ret = -1; + goto out; + } + + ret = zsock_sendto(sock, ip_str, strlen(ip_str), 0, + (struct sockaddr *)&broadcast, sizeof(broadcast)); + +out: + zsock_close(sock); + + return ret; +} + +/* Get a socket to listen to Latmus requests */ +int net_latmon_get_socket(struct sockaddr *connection_addr) +{ + struct sockaddr_in addr = { + .sin_family = AF_INET, + .sin_addr.s_addr = htonl(INADDR_ANY), + .sin_port = htons(LATMON_NET_PORT) + }; + int s, on = 1; + + if (connection_addr != NULL) { + memcpy(&addr, connection_addr, sizeof(addr)); + } + + s = zsock_socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + if (s < 0) { + LOG_ERR("failed to create latmon socket : %d", errno); + return -1; + } + + zsock_setsockopt(s, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)); + if (zsock_bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + LOG_ERR("failed to bind latmon socket : %d", errno); + zsock_close(s); + return -1; + } + + if (zsock_listen(s, 1) < 0) { + LOG_ERR("failed to listen on latmon socket : %d", errno); + zsock_close(s); + return -1; + } + + return s; +} + +/* Waits for connection from Latmus */ +int net_latmon_connect(int socket, struct in_addr *ip) +{ + struct zsock_pollfd fd[1] = { {.fd = socket, .events = ZSOCK_POLLIN } }; + struct sockaddr_in clnt_addr; + const int timeout = 5000; + socklen_t len; + int latmus = -1; + int ret; + + LOG_INF("Waiting for Latmus ... "); + + /* Broadcast Latmon's address every timeout seconds until connected */ + ret = zsock_poll(fd, 1, timeout); + if (ret < 0) { + LOG_ERR("Poll error: %d", errno); + return -1; + } else if (ret == 0) { + /* Timeout waiting for connection */ + if (broadcast_ip_address(ip) < 0) { + LOG_ERR("Broadcast error"); + return -1; + } + + /* Client should retry the connection if broadcast succeeded */ + return -EAGAIN; + } + + /* + * As per MISRA guidelines, an 'else' clause is required. However, we + * chose to prioritize adherence to the project's code style guidelines. + */ + len = sizeof(clnt_addr); + latmus = zsock_accept(socket, (struct sockaddr *)&clnt_addr, &len); + if (latmus < 0) { + LOG_INF("Failed accepting new connection..."); + return -1; + } + + return latmus; +} + +void net_latmon_start(int latmus, net_latmon_measure_t measure_f) +{ + struct latmon_message msg = { + .measure_func = measure_f, + .latmus = latmus, + }; + + k_msgq_put(&latmon_msgq, &msg, K_NO_WAIT); + k_sem_take(&latmon_done, K_FOREVER); +} + +bool net_latmon_running(void) +{ + return monitor_tid ? true : false; +} + +static int get_latmus_conf(ssize_t len, struct latmon_net_request *req, + struct latmus_conf *conf) +{ + if (len != sizeof(*req)) { + return -1; + } + + if (ntohl(req->period_usecs) == 0) { + LOG_ERR("null period received, invalid\n"); + return -1; + } + + if (ntohl(req->period_usecs) > MAX_SAMPLING_PERIOD_USEC) { + LOG_ERR("invalid period received: %u usecs\n", + ntohl(req->period_usecs)); + return -1; + } + + if (ntohl(req->histogram_cells) > HISTOGRAM_CELLS_MAX) { + LOG_ERR("invalid histogram size received: %u > %u cells\n", + ntohl(req->histogram_cells), HISTOGRAM_CELLS_MAX); + return -1; + } + + conf->period = ntohl(req->period_usecs); + conf->cells = ntohl(req->histogram_cells); + conf->max_samples = MAX_SAMPLING_PERIOD_USEC / conf->period; + + return 0; +} + +static void start_monitoring(struct latmon_message *msg, + struct latmus_conf *conf, + struct latmon_data *data) +{ + k_sem_reset(&monitor_done); + abort_monitor = false; + + memset(data, 0, sizeof(*data)); + monitor_tid = k_thread_create(&monitor_thread, monitor_stack, + MONITOR_STACK_SIZE, monitor_thread_func, + msg, conf, data, MONITOR_THREAD_PRIORITY, 0, K_NO_WAIT); +} + +static void stop_monitoring(void) +{ + if (monitor_tid == 0) { + return; + } + + abort_monitor = true; + k_sem_take(&monitor_done, K_FOREVER); +} + +static void handle_connection(struct latmon_message *msg) +{ +#if (K_HEAP_MEM_POOL_SIZE > 0) + struct latmus_conf *conf = k_malloc(sizeof(*conf)); + struct latmon_data *data = k_malloc(sizeof(*data)); + struct latmon_net_request req; + ssize_t len; + + if (conf == 0 || data == 0) { + LOG_ERR("Failed to allocate memory, check HEAP_MEM_POOL_SIZE"); + goto out; + } + + memset(conf, 0, sizeof(*conf)); + + for (;;) { + len = zsock_recv(msg->latmus, &req, sizeof(req), 0); + stop_monitoring(); + if (get_latmus_conf(len, &req, conf) < 0) { + /* Send the histogram */ + if (send_trailing_data(msg->latmus, conf, data) < 0) { + break; + } + memset(conf, 0, sizeof(*conf)); + continue; + } + start_monitoring(msg, conf, data); + } +out: + k_free(conf); + k_free(data); + zsock_close(msg->latmus); + k_sem_give(&latmon_done); +#else + LOG_ERR("No heap configured"); +#endif +} + +static int latmon_server_thread_func(void *p1, void *p2, void *p3) +{ + struct latmon_message msg = { }; + + LOG_INF("Latmon server thread priority: %d", LATMON_THREAD_PRIORITY); + + for (;;) { + k_msgq_get(&latmon_msgq, &msg, K_FOREVER); + + /* Only latmus can stop the monitoring, so hang in there */ + handle_connection(&msg); + } + + return 0; +} + +K_THREAD_DEFINE(latmon_server_id, LATMON_STACK_SIZE, + latmon_server_thread_func, NULL, NULL, NULL, + LATMON_THREAD_PRIORITY, 0, 0);