MeshTalos-Client/main/tcphelper.c

/* BSD non-blocking socket example

   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "sys/socket.h"
#include "netdb.h"
#include "errno.h"
#include "esp_system.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "esp_wifi.h"

#include "mpack.h"
#include "main.h"
#include "mesh_netif.h"

/**
 * @brief Indicates that the file descriptor represents an invalid (uninitialized or closed) socket
 *
 * Used in the TCP server structure `sock[]` which holds list of active clients we serve.
 */
#define INVALID_SOCK (-1)

/**
 * @brief Time in ms to yield to all tasks when a non-blocking socket would block
 *
 * Non-blocking socket operations are typically executed in a separate task validating
 * the socket status. Whenever the socket returns `EAGAIN` (idle status, i.e. would block)
 * we have to yield to all tasks to prevent lower priority tasks from starving.
 */
#define YIELD_TO_ALL_MS 50

/**
 * @brief Utility to log socket errors
 *
 * @param[in] tag Logging tag
 * @param[in] sock Socket number
 * @param[in] err Socket errno
 * @param[in] message Message to print
 */
static void log_socket_error(const char *tag, const int sock, const int err, const char *message)
{
    ESP_LOGE(tag, "[sock=%d]: %s\n"
                  "error=%d: %s", sock, message, err, strerror(err));
}

/**
 * @brief Tries to receive data from specified sockets in a non-blocking way,
 *        i.e. returns immediately if no data.
 *
 * @param[in] tag Logging tag
 * @param[in] sock Socket for reception
 * @param[out] data Data pointer to write the received data
 * @param[in] max_len Maximum size of the allocated space for receiving data
 * @return
 *          >0 : Size of received data
 *          =0 : No data available
 *          -1 : Error occurred during socket read operation
 *          -2 : Socket is not connected, to distinguish between an actual socket error and active disconnection
 */
int try_receive(const char *tag, const int sock, char * data, size_t max_len)
{
    int len = recv(sock, data, max_len, 0);
    if (len < 0) {
        if (errno == EINPROGRESS || errno == EAGAIN || errno == EWOULDBLOCK) {
            return 0;   // Not an error
        }
        if (errno == ENOTCONN) {
            ESP_LOGW(tag, "[sock=%d]: Connection closed", sock);
            return -2;  // Socket has been disconnected
        }
        log_socket_error(tag, sock, errno, "Error occurred during receiving");
        return -1;
    }

    return len;
}

/**
 * @brief Sends the specified data to the socket. This function blocks until all bytes got sent.
 *
 * @param[in] tag Logging tag
 * @param[in] sock Socket to write data
 * @param[in] data Data to be written
 * @param[in] len Length of the data
 * @return
 *          >0 : Size the written data
 *          -1 : Error occurred during socket write operation
 */
int socket_send(const char *tag, const int sock, const char * data, const size_t len)
{
    int to_write = len;
    while (to_write > 0) {
        int written = send(sock, data + (len - to_write), to_write, 0);
        if (written < 0 && errno != EINPROGRESS && errno != EAGAIN && errno != EWOULDBLOCK) {
            log_socket_error(tag, sock, errno, "Error occurred during sending");
            return -1;
        }
        to_write -= written;
    }
    return len;
}

// variables
SemaphoreHandle_t lock = NULL;
TaskHandle_t *task_handle = NULL;

void tcp_initialize() {
    lock = xSemaphoreCreateMutex();

    return;
}

#if CONFIG_TCP_ROOT == 1
static const char *TAG = "nonblocking-socket-server";

/**
 * @brief Returns the string representation of client's address (accepted on this server)
 */
static inline char* get_clients_address(struct sockaddr_storage *source_addr)
{
    static char address_str[128];
    char *res = NULL;
    // Convert ip address to string
    if (source_addr->ss_family == PF_INET) {
        res = inet_ntoa_r(((struct sockaddr_in *)source_addr)->sin_addr, address_str, sizeof(address_str) - 1);
    }
#ifdef CONFIG_LWIP_IPV6
    else if (source_addr->ss_family == PF_INET6) {
        res = inet6_ntoa_r(((struct sockaddr_in6 *)source_addr)->sin6_addr, address_str, sizeof(address_str) - 1);
    }
#endif
    if (!res) {
        address_str[0] = '\0'; // Returns empty string if conversion didn't succeed
    }
    return address_str;
}

static void tcp_server_task(void *pvParameters)
{
    static char rx_buffer[128];
    struct addrinfo hints = { .ai_socktype = SOCK_STREAM };
    struct addrinfo *address_info;
    int listen_sock = INVALID_SOCK;
    const size_t max_socks = CONFIG_LWIP_MAX_SOCKETS - 1;
    static int sock[CONFIG_LWIP_MAX_SOCKETS - 1];

    // Prepare a list of file descriptors to hold client's sockets, mark all of them as invalid, i.e. available
    for (int i=0; i<max_socks; ++i) {
        sock[i] = INVALID_SOCK;
    }

    // Translating the hostname or a string representation of an IP to address_info
    int res = getaddrinfo("0.0.0.0", CONFIG_TCP_SERVER_PORT, &hints, &address_info);
    if (res != 0 || address_info == NULL) {
        ESP_LOGE(TAG, "couldn't get hostname for `%s` "
                      "getaddrinfo() returns %d, addrinfo=%p", "0.0.0.0", res, address_info);
        goto error;
    }

    // Creating a listener socket
    listen_sock = socket(address_info->ai_family, address_info->ai_socktype, address_info->ai_protocol);

    if (listen_sock < 0) {
        log_socket_error(TAG, listen_sock, errno, "Unable to create socket");
        goto error;
    }
    ESP_LOGI(TAG, "Listener socket created");

    // Marking the socket as non-blocking
    int flags = fcntl(listen_sock, F_GETFL);
    if (fcntl(listen_sock, F_SETFL, flags | O_NONBLOCK) == -1) {
        log_socket_error(TAG, listen_sock, errno, "Unable to set socket non blocking");
        goto error;
    }
    ESP_LOGI(TAG, "Socket marked as non blocking");

    // Binding socket to the given address
    int err = bind(listen_sock, address_info->ai_addr, address_info->ai_addrlen);
    if (err != 0) {
        log_socket_error(TAG, listen_sock, errno, "Socket unable to bind");
        goto error;
    }
    ESP_LOGI(TAG, "Socket bound on %s:%s", "0.0.0.0", CONFIG_TCP_SERVER_PORT);

    // Set queue (backlog) of pending connections to one (can be more)
    err = listen(listen_sock, 1);
    if (err != 0) {
        log_socket_error(TAG, listen_sock, errno, "Error occurred during listen");
        goto error;
    }
    ESP_LOGI(TAG, "Socket listening");

    // Main loop for accepting new connections and serving all connected clients
    while (1) {
        struct sockaddr_storage source_addr; // Large enough for both IPv4 or IPv6
        socklen_t addr_len = sizeof(source_addr);

        // Find a free socket
        int new_sock_index = 0;
        for (new_sock_index=0; new_sock_index<max_socks; ++new_sock_index) {
            if (sock[new_sock_index] == INVALID_SOCK) {
                break;
            }
        }

        // We accept a new connection only if we have a free socket
        if (new_sock_index < max_socks) {
            // Try to accept a new connections
            sock[new_sock_index] = accept(listen_sock, (struct sockaddr *)&source_addr, &addr_len);

            if (sock[new_sock_index] < 0) {
                if (errno == EWOULDBLOCK) { // The listener socket did not accepts any connection
                                            // continue to serve open connections and try to accept again upon the next iteration
                    ESP_LOGV(TAG, "No pending connections...");
                } else {
                    log_socket_error(TAG, listen_sock, errno, "Error when accepting connection");
                    goto error;
                }
            } else {
                // We have a new client connected -> print it's address
                ESP_LOGI(TAG, "[sock=%d]: Connection accepted from IP:%s", sock[new_sock_index], get_clients_address(&source_addr));

                // ...and set the client's socket non-blocking
                flags = fcntl(sock[new_sock_index], F_GETFL);
                if (fcntl(sock[new_sock_index], F_SETFL, flags | O_NONBLOCK) == -1) {
                    log_socket_error(TAG, sock[new_sock_index], errno, "Unable to set socket non blocking");
                    goto error;
                }
                ESP_LOGI(TAG, "[sock=%d]: Socket marked as non blocking", sock[new_sock_index]);
            }
        }

        // We serve all the connected clients in this loop
        for (int i=0; i<max_socks; ++i) {
            if (sock[i] != INVALID_SOCK) {
                // send
                int result = tcp_server_callback(TAG, sock[i]);
                if (result != 0) {
                    ESP_LOGE(TAG, "[sock=%d]: tcp_server_callback returned %d -> closing the socket", sock[i], result);
                    close(sock[i]);
                    sock[i] = INVALID_SOCK;
                }
            } // one client's socket
        } // for all sockets

        // Yield to other tasks
        vTaskDelay(pdMS_TO_TICKS(YIELD_TO_ALL_MS));
    }

error:
    if (listen_sock != INVALID_SOCK) {
        close(listen_sock);
    }

    for (int i=0; i<max_socks; ++i) {
        if (sock[i] != INVALID_SOCK) {
            close(sock[i]);
        }
    }

    free(address_info);
    vTaskDelete(NULL);
}

void start_tcp_server(void)
{
    if (xSemaphoreTake(lock, 1000 / portTICK_PERIOD_MS) == false)
    {
        return;
    }

    // stop old task
    if (task_handle) {
        ESP_LOGI(TAG, "Detected old task, deleting...");

        vTaskDelete(*task_handle);
        free(task_handle);
        task_handle = NULL;
    }

    // new task
    task_handle = malloc(sizeof(TaskHandle_t));
    memset(task_handle, 0, sizeof(TaskHandle_t));

    xTaskCreate(tcp_server_task, "tcp_server", 4096, NULL, 5, task_handle);

    xSemaphoreGive(lock);
    return;
}
#else
static const char *TAG = "nonblocking-socket-client";

size_t mp_fill(mpack_tree_t *reader, char *buffer, size_t count) {
    int sock = (int)reader->context;
    ESP_LOGI(TAG, "expect %lu bytes", count);
    ssize_t r = recv(sock, buffer, count, 0);
    ESP_LOGI(TAG, "received %ld bytes", r);

    if (r <= 0) {
        mpack_tree_flag_error(reader, mpack_error_io);
        return 0;
    }

    return r;
}

// tcp client
static void tcp_client_task(void *pvParameters)
{
    static char rx_buffer[CONFIG_TCP_RXBUFFER_SIZE];
    int sock = INVALID_SOCK;

    struct addrinfo hints = { .ai_socktype = SOCK_STREAM };
    struct addrinfo *address_info;

    int res = getaddrinfo(CONFIG_TCP_SERVER_IP, CONFIG_TCP_SERVER_PORT, &hints, &address_info);
    if (res != 0 || address_info == NULL) {
        ESP_LOGE(TAG, "couldn't get hostname for `%s` "
                      "getaddrinfo() returns %d, addrinfo=%p", CONFIG_TCP_SERVER_IP, res, address_info);
        goto error;
    }

    // Creating client's socket
    sock = socket(address_info->ai_family, address_info->ai_socktype, address_info->ai_protocol);
    if (sock < 0) {
        log_socket_error(TAG, sock, errno, "Unable to create socket");
        goto error;
    }
    ESP_LOGI(TAG, "Socket created, connecting to %s:%s", CONFIG_TCP_SERVER_IP, CONFIG_TCP_SERVER_PORT);

    // Marking the socket as non-blocking
    //int flags = fcntl(sock, F_GETFL);
    //if (fcntl(sock, F_SETFL, flags) == -1) {
    //    log_socket_error(TAG, sock, errno, "Unable to set socket non blocking");
    //}

    if (connect(sock, address_info->ai_addr, address_info->ai_addrlen) != 0) {
        ESP_LOGE(TAG, "Failed to connect server");
    }

    //if (connect(sock, address_info->ai_addr, address_info->ai_addrlen) != 0) {
    //    if (errno == EINPROGRESS) {
    //        ESP_LOGD(TAG, "connection in progress");
    //        fd_set fdset;
    //        FD_ZERO(&fdset);
    //        FD_SET(sock, &fdset);
//
    //        // Connection in progress -> have to wait until the connecting socket is marked as writable, i.e. connection completes
    //        res = select(sock+1, NULL, &fdset, NULL, NULL);
    //        if (res < 0) {
    //            log_socket_error(TAG, sock, errno, "Error during connection: select for socket to be writable");
    //            goto error;
    //        } else if (res == 0) {
    //            log_socket_error(TAG, sock, errno, "Connection timeout: select for socket to be writable");
    //            goto error;
    //        } else {
    //            int sockerr;
    //            socklen_t len = (socklen_t)sizeof(int);
//
    //            if (getsockopt(sock, SOL_SOCKET, SO_ERROR, (void*)(&sockerr), &len) < 0) {
    //                log_socket_error(TAG, sock, errno, "Error when getting socket error using getsockopt()");
    //                goto error;
    //            }
    //            if (sockerr) {
    //                log_socket_error(TAG, sock, sockerr, "Connection error");
    //                goto error;
    //            }
    //        }
    //    } else {
    //        log_socket_error(TAG, sock, errno, "Socket is unable to connect");
    //        goto error;
    //    }
    //}

    ESP_LOGI(TAG, "Connected to %s:%s", CONFIG_TCP_SERVER_IP, CONFIG_TCP_SERVER_PORT);
    
    // mpack
    mpack_tree_t tree;
    mpack_tree_init_stream(&tree, mp_fill, (void *)sock, CONFIG_IMAGE_BUF_SIZE, 30);

    while (true) {
      mpack_tree_parse(&tree);
      if (mpack_tree_error(&tree) != mpack_ok) {
        ESP_LOGE(TAG, "mperror: %d\n", mpack_tree_error(&tree));
        break;
      }

      // parse
      mpack_node_t root = mpack_tree_root(&tree);
      char *cmd = mpack_node_cstr_alloc(mpack_node_map_cstr(root, "command"), 30);

      size_t index = 0;
      size_t data_size = 0;
      char *data = NULL;
      if (strcmp(cmd, "update_image") == 0) {
        index = mpack_node_u64(mpack_node_map_cstr(root, "index"));

        data_size = mpack_node_data_len(mpack_node_map_cstr(root, "data"));
        data = (char *)mpack_node_data(mpack_node_map_cstr(root, "data"));
      }
      
      // callback
      request_t req;
      memset(&req, 0, sizeof(req));
      req.command = cmd;
      req.index = index;
      req.data = data;
      req.data_len = data_size;

      int result = tcp_client_callback(&req);
      char tx_buffer[128] = {0};
      switch (result) {
          case 0:
              strcpy(tx_buffer, "ok\n");
              break;
          case -2:
              strcpy(tx_buffer, "invalid request\n");
              break;
          case -3:
              strcpy(tx_buffer, "locked\n");
              break;
          default:
              strcpy(tx_buffer, "unknown error\n");
              break;
      }

      // send result
      int send_len = socket_send(TAG, sock, tx_buffer, strlen(tx_buffer));
      if (send_len < 0) {
          ESP_LOGE(TAG, "Error occurred during socket_send");
          goto error;
      }
      ESP_LOGI(TAG, "Written: %.*s", send_len, tx_buffer);

      // end
      free(cmd);
      cmd=NULL;
    }


error:
    if (sock != INVALID_SOCK) {
        close(sock);
    }
    free(address_info);
    
    free(task_handle);
    task_handle = NULL;
    vTaskDelete(NULL);
}

void start_tcp_client(void) {
    if (xSemaphoreTake(lock, 1000 / portTICK_PERIOD_MS) == false)
    {
        return;
    }

    // stop old task
    if (task_handle) {
        ESP_LOGI(TAG, "Detected old task, deleting...");

        vTaskDelete(*task_handle);
        free(task_handle);
        task_handle = NULL;
    }

    // new task
    task_handle = malloc(sizeof(TaskHandle_t));
    memset(task_handle, 0, sizeof(TaskHandle_t));

    xTaskCreate(tcp_client_task, "tcp_client", 4096, NULL, 5, task_handle);

    xSemaphoreGive(lock);
    return;
}
#endif