path: root/fw/esp32/components/eos/sock.c

#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <sys/select.h>

#include <freertos/FreeRTOS.h>
#include <freertos/semphr.h>
#include <freertos/task.h>

#include <esp_system.h>
#include <esp_log.h>
#include <esp_err.h>
#include <esp_vfs.h>
#include <esp_vfs_dev.h>
#include <esp_vfs_eventfd.h>

#include <lwip/sockets.h>
#include <lwip/err.h>
#include <lwip/sockets.h>
#include <lwip/sys.h>
#include <lwip/netdb.h>
#include <lwip/dns.h>

#include "eos.h"
#include "net.h"
#include "sock.h"

static const char *TAG = "EOS SOCK";
static SemaphoreHandle_t mutex;
static int cmd_fd, rep_fd;
static int _socks[EOS_SOCK_MAX_SOCK];

#define CMD_OPEN    1
#define CMD_CLOSE   2
#define CMD_SEND    3
#define CMD_REOPEN  4

static int t_open_dgram(void) {
    struct sockaddr_in _myaddr;
    int sock;

    sock = socket(PF_INET, SOCK_DGRAM, 0);
    if (sock < 0) return sock;

    memset((char *)&_myaddr, 0, sizeof(_myaddr));
    _myaddr.sin_family = AF_INET;
    _myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    _myaddr.sin_port = htons(0);

    int rv = bind(sock, (struct sockaddr *)&_myaddr, sizeof(_myaddr));
    if (rv < 0) {
        close(sock);
        return rv;
    }
    return sock;
}

static void t_close(int sock) {
    close(sock);
}

static ssize_t t_sendto(int sock, void *msg, size_t msg_size, EOSNetAddr *addr) {
    struct sockaddr_in servaddr;

    memset((void *)&servaddr, 0, sizeof(servaddr));
    servaddr.sin_family = AF_INET;
    servaddr.sin_port = htons(addr->port);
    memcpy((void *)&servaddr.sin_addr, addr->host, sizeof(addr->host));
    return sendto(sock, msg, msg_size, 0, (struct sockaddr *)&servaddr, sizeof(servaddr));
}

static ssize_t t_recvfrom(int sock, void *msg, size_t msg_size, EOSNetAddr *addr) {
    struct sockaddr_in servaddr;
    socklen_t addrlen = sizeof(servaddr);
    memset((void *)&servaddr, 0, sizeof(servaddr));

    ssize_t recvlen = recvfrom(sock, msg, msg_size, 0, (struct sockaddr *)&servaddr, &addrlen);
    if (recvlen < 0) return recvlen;

    if (addr) {
        addr->port = ntohs(servaddr.sin_port);
        memcpy(addr->host, (void *)&servaddr.sin_addr, sizeof(addr->host));
    }
    return recvlen;
}

static void populate_fds(fd_set *fds, int *max_fd) {
    int i;

    *max_fd = cmd_fd;

    FD_ZERO(fds);
    FD_SET(cmd_fd, fds);
    for (i=0; i<EOS_SOCK_MAX_SOCK; i++) {
        if (_socks[i]) {
            FD_SET(_socks[i], fds);
            if (_socks[i] > *max_fd) {
                *max_fd = _socks[i];
            }
        }
    }
}

static void udp_rcvr_task(void *pvParameters) {
    EOSNetAddr addr;
    unsigned char *buffer;
    uint16_t buf_len;
    fd_set all_fds, read_fds;
    uint8_t sock_i;
    uint8_t cmd[8];
    ssize_t _rv;
    int sock, max_fd, i;
    int rv;

    assert(sizeof(buffer) == 4);

    populate_fds(&all_fds, &max_fd);
    while (1) {
        memcpy(&read_fds, &all_fds, sizeof(fd_set));
        rv = select(max_fd + 1, &read_fds, NULL, NULL, NULL);
        if (rv <= 0) {
            ESP_LOGE(TAG, "SELECT ERR:%d", rv);
            continue;
        }

        for (i=0; i<EOS_SOCK_MAX_SOCK; i++) {
            sock = _socks[i];
            if (sock && FD_ISSET(sock, &read_fds)) {
                unsigned char *_buf;

                buffer = eos_net_alloc();
                _rv = t_recvfrom(sock, buffer + EOS_SOCK_SIZE_UDP_HDR, EOS_NET_SIZE_BUF - EOS_SOCK_SIZE_UDP_HDR, &addr);
                if (_rv < 0) {
                    eos_net_free(buffer);
                    ESP_LOGE(TAG, "RECV ERR:%d", _rv);
                }
                _buf = buffer;
                _buf[0] = EOS_SOCK_MTYPE_PKT;
                _buf[1] = i + 1;
                _buf += 2;
                memcpy(_buf, addr.host, sizeof(addr.host));
                _buf += sizeof(addr.host);
                _buf[0] = addr.port >> 8;
                _buf[1] = addr.port;
                _buf += sizeof(addr.port);
                rv = eos_net_send(EOS_NET_MTYPE_SOCK, buffer, _rv + EOS_SOCK_SIZE_UDP_HDR);
                if (rv) ESP_LOGE(TAG, "NET SEND ERR:%d", rv);
            }
        }
        if (FD_ISSET(cmd_fd, &read_fds)) {
            rv = read(cmd_fd, cmd, sizeof(cmd));
            assert(rv == sizeof(cmd));

            switch (cmd[0]) {
                case CMD_OPEN: {
                    sock = t_open_dgram();
                    sock_i = 0;
                    if (sock > 0) {
                        for (i=0; i<EOS_SOCK_MAX_SOCK; i++) {
                            if (_socks[i] == 0) {
                                sock_i = i + 1;
                                _socks[i] = sock;
                                break;
                            }
                        }
                        if (sock_i) {
                            populate_fds(&all_fds, &max_fd);
                        }
                    }
                    memset(cmd, 0, sizeof(cmd));
                    cmd[1] = sock_i;

                    rv = write(rep_fd, cmd, sizeof(cmd));
                    assert(rv == sizeof(cmd));

                    break;
                }

                case CMD_CLOSE: {
                    sock_i = cmd[1] - 1;
                    sock = _socks[sock_i];
                    if (sock) {
                        t_close(sock);
                        _socks[sock_i] = 0;
                        populate_fds(&all_fds, &max_fd);
                    }

                    rv = write(rep_fd, cmd, sizeof(cmd));
                    assert(rv == sizeof(cmd));

                    break;
                }

                case CMD_SEND: {
                    sock_i = cmd[1] - 1;
                    buf_len  = (uint16_t)cmd[2] << 8;
                    buf_len |= (uint16_t)cmd[3];
                    memcpy(&buffer, cmd + 4, sizeof(buffer));

                    sock = _socks[sock_i];
                    memcpy(addr.host, buffer, sizeof(addr.host));
                    buffer += sizeof(addr.host);
                    buf_len -= sizeof(addr.host);
                    addr.port  = (uint16_t)buffer[0] << 8;
                    addr.port |= (uint16_t)buffer[1];
                    buffer += sizeof(addr.port);
                    buf_len -= sizeof(addr.port);
                    _rv = t_sendto(sock, buffer, buf_len, &addr);

                    memset(cmd, 0, sizeof(cmd));

                    rv = write(rep_fd, cmd, sizeof(cmd));
                    assert(rv == sizeof(cmd));

                    break;
                }

                case CMD_REOPEN: {
                    buffer = NULL;
                    buf_len = 0;

                    for (i=0; i<EOS_SOCK_MAX_SOCK; i++) {
                        sock = _socks[i];
                        if (sock) {
                            t_close(sock);
                            _socks[i] = -1;
                        }
                    }
                    for (i=0; i<EOS_SOCK_MAX_SOCK; i++) {
                        sock = _socks[i];
                        if (sock) {
                            sock = t_open_dgram();
                            if (sock > 0) {
                                _socks[i] = sock;
                            } else {
                                _socks[i] = 0;
                                if (buffer) {
                                    buffer[buf_len] = i + 1;
                                    buf_len++;
                                } else {
                                    buffer = eos_net_alloc();
                                    buffer[0] = EOS_SOCK_MTYPE_CLOSE;
                                    buffer[1] = i + 1;
                                    buf_len = 2;
                                }
                            }
                        }
                    }
                    if (buffer) {
                        rv = eos_net_send(EOS_NET_MTYPE_SOCK, buffer, buf_len);
                        if (rv) ESP_LOGE(TAG, "NET SEND ERR:%d", rv);
                    }

                    rv = write(rep_fd, cmd, sizeof(cmd));
                    assert(rv == sizeof(cmd));

                    break;
                }

                default:
                    ESP_LOGE(TAG, "BAD CMD:%d", cmd[0]);
                    break;
            }

        }
    }

    vTaskDelete(NULL);
}

static void sock_handler(unsigned char _mtype, unsigned char *buffer, uint16_t buf_len) {
    unsigned char mtype;
    uint8_t sock_i;
    uint8_t cmd[8];
    int rv;

    if (buf_len < 1) return;

    memset(cmd, 0, sizeof(cmd));

    mtype = buffer[0];
    switch (mtype) {
        case EOS_SOCK_MTYPE_PKT: {
            unsigned char *_buf = buffer;

            if (buf_len < EOS_SOCK_SIZE_UDP_HDR) return;

            sock_i = buffer[1];
            if ((sock_i == 0) || (sock_i > EOS_SOCK_MAX_SOCK)) return;

            _buf += 2;
            cmd[0] = CMD_SEND;
            cmd[1] = sock_i;
            cmd[2] = buf_len >> 8;
            cmd[3] = buf_len;
            memcpy(cmd + 4, &_buf, sizeof(_buf));

            xSemaphoreTake(mutex, portMAX_DELAY);

            rv = write(cmd_fd, cmd, sizeof(cmd));
            assert(rv == sizeof(cmd));

            rv = read(rep_fd, cmd, sizeof(cmd));
            assert(rv == sizeof(cmd));

            xSemaphoreGive(mutex);

            assert(cmd[0] == CMD_SEND);
            break;
        }

        case EOS_SOCK_MTYPE_OPEN_DGRAM: {
            cmd[0] = CMD_OPEN;

            xSemaphoreTake(mutex, portMAX_DELAY);

            rv = write(cmd_fd, cmd, sizeof(cmd));
            assert(rv == sizeof(cmd));

            rv = read(rep_fd, cmd, sizeof(cmd));
            assert(rv == sizeof(cmd));

            xSemaphoreGive(mutex);

            assert(cmd[0] == CMD_OPEN);
            sock_i = cmd[1];
            buffer[0] = EOS_SOCK_MTYPE_OPEN_DGRAM;
            buffer[1] = sock_i;
            eos_net_reply(EOS_NET_MTYPE_SOCK, buffer, 2);
            break;
        }

        case EOS_SOCK_MTYPE_CLOSE: {
            if (buf_len < 2) return;

            sock_i = buffer[1];
            if ((sock_i == 0) || (sock_i > EOS_SOCK_MAX_SOCK)) return;

            cmd[0] = CMD_CLOSE;
            cmd[1] = sock_i;

            xSemaphoreTake(mutex, portMAX_DELAY);

            rv = write(cmd_fd, cmd, sizeof(cmd));
            assert(rv == sizeof(cmd));

            rv = read(rep_fd, cmd, sizeof(cmd));
            assert(rv == sizeof(cmd));

            xSemaphoreGive(mutex);

            assert(cmd[0] == CMD_CLOSE);
            break;
        }
    }
}

void eos_sock_init(void) {
    esp_vfs_eventfd_config_t config = ESP_VFS_EVENTD_CONFIG_DEFAULT();
    esp_err_t ret;

    ret = esp_vfs_eventfd_register(&config);
    assert(ret == ESP_OK);

    cmd_fd = eventfd(0, 0);
    assert(cmd_fd > 0);

    rep_fd = eventfd(0, 0);
    assert(rep_fd > 0);

    mutex = xSemaphoreCreateBinary();
    assert(mutex != NULL);
    xSemaphoreGive(mutex);

    eos_net_set_handler(EOS_NET_MTYPE_SOCK, sock_handler);

    ESP_LOGI(TAG, "INIT");
}

void eos_sock_run(void) {
    BaseType_t rv;

    rv = xTaskCreate(&udp_rcvr_task, "udp_rcvr", EOS_TASK_SSIZE_SOCK, NULL, EOS_TASK_PRIORITY_SOCK, NULL);
    assert(rv == pdPASS);

    ESP_LOGI(TAG, "RUN");
}

void eos_sock_reopen(void) {
    uint8_t cmd[8];
    int rv;

    cmd[0] = CMD_REOPEN;
    xSemaphoreTake(mutex, portMAX_DELAY);

    rv = write(cmd_fd, cmd, sizeof(cmd));
    assert(rv == sizeof(cmd));

    rv = read(rep_fd, cmd, sizeof(cmd));
    assert(rv == sizeof(cmd));

    xSemaphoreGive(mutex);
    assert(cmd[0] == CMD_REOPEN);
}