mirror
/
esp-idf
kopia lustrzana https://github.com/espressif/esp-idf
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
esp-idf/components/esp_netif/vfs_l2tap/esp_vfs_l2tap.c

628 wiersze
20 KiB
C
Czysty Wina Historia

/*
* SPDX-FileCopyrightText: 2021-2022 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <stdio.h>
#include <stdatomic.h>
#include <sys/fcntl.h>
#include <sys/param.h>
#include <sys/queue.h>
#include "arpa/inet.h" // for ntohs, etc.
#include "errno.h"
#include "esp_vfs_l2tap.h"
#include "lwip/prot/ethernet.h" // Ethernet headers
#include "esp_vfs.h"
#include "esp_log.h"
#include "esp_check.h"
#include "esp_netif.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "freertos/queue.h"
#if CONFIG_ESP_NETIF_L2_TAP
#define INVALID_FD (-1)
#define L2TAP_MAX_FDS CONFIG_ESP_NETIF_L2_TAP_MAX_FDS
#define RX_QUEUE_MAX_SIZE CONFIG_ESP_NETIF_L2_TAP_RX_QUEUE_SIZE
typedef enum {
L2TAP_SOCK_STATE_READY,
L2TAP_SOCK_STATE_OPENED,
L2TAP_SOCK_STATE_CLOSING
} l2tap_socket_state_t;
typedef struct {
_Atomic l2tap_socket_state_t state;
bool non_blocking;
l2tap_iodriver_handle driver_handle;
uint16_t ethtype_filter;
QueueHandle_t rx_queue;
SemaphoreHandle_t close_done_sem;
esp_err_t (*driver_transmit)(l2tap_iodriver_handle io_handle, void *buffer, size_t len);
void (*driver_free_rx_buffer)(l2tap_iodriver_handle io_handle, void* buffer);
} l2tap_context_t;
typedef struct {
void *buff;
size_t len;
} frame_queue_entry_t;
typedef struct {
esp_vfs_select_sem_t select_sem;
fd_set *readfds;
fd_set *writefds;
fd_set *errorfds;
fd_set readfds_orig;
fd_set writefds_orig;
fd_set errorfds_orig;
} l2tap_select_args_t;
typedef enum {
L2TAP_SELECT_READ_NOTIF,
L2TAP_SELECT_WRITE_NOTIF,
L2TAP_SELECT_ERR_NOTIF
} l2tap_select_notif_e;
static l2tap_context_t s_l2tap_sockets[L2TAP_MAX_FDS] = {0};
static bool s_is_registered = false;
static portMUX_TYPE s_critical_section_lock = portMUX_INITIALIZER_UNLOCKED;
static l2tap_select_args_t **s_registered_selects = NULL;
static int32_t s_registered_select_cnt = 0;
static const char *TAG = "vfs_l2tap";
static void l2tap_select_notify(int fd, l2tap_select_notif_e select_notif);
/* ================== Utils ====================== */
static esp_err_t init_rx_queue(l2tap_context_t *l2tap_socket)
{
l2tap_socket->rx_queue = xQueueCreate(RX_QUEUE_MAX_SIZE, sizeof(frame_queue_entry_t));
ESP_RETURN_ON_FALSE(l2tap_socket->rx_queue, ESP_ERR_NO_MEM, TAG, "create work queue failed");
return ESP_OK;
}
static esp_err_t push_rx_queue(l2tap_context_t *l2tap_socket, void *buff, size_t len)
{
frame_queue_entry_t frame_info;
frame_info.buff = buff;
frame_info.len = len;
// try send to queue and check if the queue is full
if (xQueueSend(l2tap_socket->rx_queue, &frame_info, 0) != pdTRUE) {
return ESP_ERR_NO_MEM;
}
return ESP_OK;
}
static ssize_t pop_rx_queue(l2tap_context_t *l2tap_socket, void *buff, size_t len)
{
TickType_t timeout = portMAX_DELAY;
if (l2tap_socket->non_blocking) {
timeout = 0;
}
frame_queue_entry_t frame_info;
if (xQueueReceive(l2tap_socket->rx_queue, &frame_info, timeout) == pdTRUE) {
// empty queue was issued indicating the fd is going to be closed
if (frame_info.len == 0) {
// indicate to "clean_task" that task waiting for queue was unblocked
push_rx_queue(l2tap_socket, NULL, 0);
goto err;
}
if (len > frame_info.len) {
len = frame_info.len;
}
memcpy(buff, frame_info.buff, len);
l2tap_socket->driver_free_rx_buffer(l2tap_socket->driver_handle, frame_info.buff);
} else {
goto err;
}
return len;
err:
return -1;
}
static bool rx_queue_empty(l2tap_context_t *l2tap_socket)
{
return (uxQueueMessagesWaiting(l2tap_socket->rx_queue) == 0);
}
static void flush_rx_queue(l2tap_context_t *l2tap_socket)
{
frame_queue_entry_t frame_info;
while (xQueueReceive(l2tap_socket->rx_queue, &frame_info, 0) == pdTRUE) {
if (frame_info.len > 0) {
free(frame_info.buff);
}
}
}
static void delete_rx_queue(l2tap_context_t *l2tap_socket)
{
vQueueDelete(l2tap_socket->rx_queue);
l2tap_socket->rx_queue = NULL;
}
static inline void l2tap_lock(void)
{
portENTER_CRITICAL(&s_critical_section_lock);
}
static inline void l2tap_unlock(void)
{
portEXIT_CRITICAL(&s_critical_section_lock);
}
static inline void default_free_rx_buffer(l2tap_iodriver_handle io_handle, void* buffer)
{
free(buffer);
}
/* ================== ESP NETIF L2 TAP intf ====================== */
esp_err_t esp_vfs_l2tap_eth_filter(l2tap_iodriver_handle driver_handle, void *buff, size_t *size)
{
struct eth_hdr *eth_header = buff;
uint16_t eth_type = ntohs(eth_header->type);
for (int i = 0; i < L2TAP_MAX_FDS; i++) {
if (atomic_load(&s_l2tap_sockets[i].state) == L2TAP_SOCK_STATE_OPENED) {
l2tap_lock(); // read of socket config needs to be atomic since it can be manipulated from other task
if (s_l2tap_sockets[i].driver_handle == driver_handle && (s_l2tap_sockets[i].ethtype_filter == eth_type ||
// IEEE 802.2 Frame is identified based on its length which is less than IEEE802.3 max length (Ethernet II Types IDs start over this value)
// Note that IEEE 802.2 LLC resolution is expected to be performed by upper stream app
(s_l2tap_sockets[i].ethtype_filter <= ETH_IEEE802_3_MAX_LEN && eth_type <= ETH_IEEE802_3_MAX_LEN))) {
l2tap_unlock();
if (push_rx_queue(&s_l2tap_sockets[i], buff, *size) != ESP_OK) {
// just tail drop when queue is full
s_l2tap_sockets[i].driver_free_rx_buffer(s_l2tap_sockets[i].driver_handle, buff);
ESP_LOGD(TAG, "fd %d rx queue is full", i);
}
l2tap_lock();
if (s_registered_select_cnt) {
l2tap_select_notify(i, L2TAP_SELECT_READ_NOTIF);
}
l2tap_unlock();
*size = 0; // the frame is not passed to IP stack when size set to 0
} else {
l2tap_unlock();
}
}
}
return ESP_OK;
}
/* ====================== vfs ====================== */
static int l2tap_open(const char *path, int flags, int mode)
{
int fd;
// Find free fd and initialize
for (fd = 0; fd < L2TAP_MAX_FDS; fd++) {
l2tap_socket_state_t exp_state = L2TAP_SOCK_STATE_READY;
if (atomic_compare_exchange_strong(&s_l2tap_sockets[fd].state, &exp_state,
L2TAP_SOCK_STATE_OPENED)) {
if (init_rx_queue(&s_l2tap_sockets[fd]) != ESP_OK) {
atomic_store(&s_l2tap_sockets[fd].state, L2TAP_SOCK_STATE_READY);
goto err;
}
s_l2tap_sockets[fd].ethtype_filter = 0x0;
s_l2tap_sockets[fd].driver_handle = NULL;
s_l2tap_sockets[fd].non_blocking = ((flags & O_NONBLOCK) == O_NONBLOCK);
s_l2tap_sockets[fd].driver_transmit = esp_eth_transmit;
s_l2tap_sockets[fd].driver_free_rx_buffer = default_free_rx_buffer;
return fd;
}
}
err:
return INVALID_FD;
}
static ssize_t l2tap_write(int fd, const void *data, size_t size)
{
ssize_t ret = -1;
if (size == 0) {
return 0;
}
if (atomic_load(&s_l2tap_sockets[fd].state) == L2TAP_SOCK_STATE_OPENED) {
if (s_l2tap_sockets[fd].ethtype_filter > ETH_IEEE802_3_MAX_LEN &&
((struct eth_hdr *)data)->type != htons(s_l2tap_sockets[fd].ethtype_filter)) {
// bad message
errno = EBADMSG;
goto err;
}
if (s_l2tap_sockets[fd].driver_transmit(s_l2tap_sockets[fd].driver_handle, (void *)data, size) == ESP_OK) {
ret = size;
} else {
// I/O error
errno = EIO;
}
} else {
// bad file desc
errno = EBADF;
}
err:
return ret;
}
static ssize_t l2tap_read(int fd, void *data, size_t size)
{
// fd might be in process of closing (close was already called but preempted)
if (atomic_load(&s_l2tap_sockets[fd].state) != L2TAP_SOCK_STATE_OPENED) {
// bad file desc
errno = EBADF;
return -1;
}
if (size == 0) {
return 0;
}
ssize_t actual_size = -1;
if ((actual_size = pop_rx_queue(&s_l2tap_sockets[fd], data, size)) < 0) {
errno = EAGAIN;
}
return actual_size;
}
void l2tap_clean_task(void *task_param)
{
l2tap_context_t *l2tap_socket = (l2tap_context_t *)task_param;
// push empty queue to unblock possibly blocking task
push_rx_queue(l2tap_socket, NULL, 0);
// wait for the indication that blocking task was executed (unblocked)
pop_rx_queue(l2tap_socket, NULL, 0);
// now, all higher priority tasks should finished their execution and new accesses to the queue were prevended
// by L2TAP_SOCK_STATE_CLOSING => we are free to free queue resources
flush_rx_queue(l2tap_socket);
delete_rx_queue(l2tap_socket);
// unblock task which originally called close
xSemaphoreGive(l2tap_socket->close_done_sem);
// all done, delete itsefl
vTaskDelete(NULL);
}
static int l2tap_close(int fd)
{
if (atomic_load(&s_l2tap_sockets[fd].state) != L2TAP_SOCK_STATE_OPENED) {
// not valid opened fd
errno = EBADF;
return -1;
}
// prevent any further manipulations with the socket (already started will be finished though)
atomic_store(&s_l2tap_sockets[fd].state, L2TAP_SOCK_STATE_CLOSING);
if ((s_l2tap_sockets[fd].close_done_sem = xSemaphoreCreateBinary()) == NULL) {
ESP_LOGE(TAG, "create close_done_sem failed");
return -1;
}
// If one task is blocked in I/O operation and another task tries to close the fd, the first task is
// unblocked by pushing empty queue in low priority task (to ensure context switch to the first task).
// The first's task read operation then ends with error and the low priority task frees the queue resources.
if (xTaskCreate(l2tap_clean_task, "l2tap_clean_task", 1024, &s_l2tap_sockets[fd], tskIDLE_PRIORITY, NULL) == pdFAIL) {
ESP_LOGE(TAG, "create l2tap_clean_task failed");
return -1;
}
// wait for the low priority close task & then delete the semaphore
xSemaphoreTake(s_l2tap_sockets[fd].close_done_sem, portMAX_DELAY);
vSemaphoreDelete(s_l2tap_sockets[fd].close_done_sem); // no worries to delete, this task owns the semaphore
// indicate that socket is ready to be used again
atomic_store(&s_l2tap_sockets[fd].state, L2TAP_SOCK_STATE_READY);
return 0;
}
static int l2tap_ioctl(int fd, int cmd, va_list args)
{
esp_netif_t *esp_netif;
switch (cmd) {
case L2TAP_S_RCV_FILTER: ;
uint16_t *new_ethtype_filter = va_arg(args, uint16_t *);
l2tap_lock();
// socket needs to be assigned to interface at first
if (s_l2tap_sockets[fd].driver_handle == NULL) {
// Permission denied (filter change is denied at this state)
errno = EACCES;
l2tap_unlock();
goto err;
}
// do nothing when same filter is to be set
if (s_l2tap_sockets[fd].ethtype_filter != *new_ethtype_filter) {
// check if the ethtype filter is not already used by other socket of the same interface
for (int i = 0; i < L2TAP_MAX_FDS; i++) {
if (atomic_load(&s_l2tap_sockets[i].state) == L2TAP_SOCK_STATE_OPENED &&
s_l2tap_sockets[i].driver_handle == s_l2tap_sockets[fd].driver_handle &&
s_l2tap_sockets[i].ethtype_filter == *new_ethtype_filter) {
// invalid argument
errno = EINVAL;
l2tap_unlock();
goto err;
}
}
s_l2tap_sockets[fd].ethtype_filter = *new_ethtype_filter;
}
l2tap_unlock();
break;
case L2TAP_G_RCV_FILTER: ;
uint16_t *ethtype_filter_dest = va_arg(args, uint16_t *);
*ethtype_filter_dest = s_l2tap_sockets[fd].ethtype_filter;
break;
case L2TAP_S_INTF_DEVICE: ;
const char *str = va_arg(args, const char *);
esp_netif = esp_netif_get_handle_from_ifkey(str);
if (esp_netif == NULL) {
// No such device
errno = ENODEV;
goto err;
}
l2tap_lock();
s_l2tap_sockets[fd].driver_handle = esp_netif_get_io_driver(esp_netif);
l2tap_unlock();
break;
case L2TAP_G_INTF_DEVICE: ;
const char **str_p = va_arg(args, const char **);
*str_p = NULL;
esp_netif = NULL;
while ((esp_netif = esp_netif_next(esp_netif)) != NULL) {
if (s_l2tap_sockets[fd].driver_handle == esp_netif_get_io_driver(esp_netif)) {
*str_p = esp_netif_get_ifkey(esp_netif);
}
}
break;
case L2TAP_S_DEVICE_DRV_HNDL: ;
l2tap_iodriver_handle set_driver_hdl = va_arg(args, l2tap_iodriver_handle);
if (set_driver_hdl == NULL) {
// No such device (not valid driver handle)
errno = ENODEV;
goto err;
}
l2tap_lock();
s_l2tap_sockets[fd].driver_handle = set_driver_hdl;
l2tap_unlock();
break;
case L2TAP_G_DEVICE_DRV_HNDL: ;
l2tap_iodriver_handle *get_driver_hdl = va_arg(args, l2tap_iodriver_handle*);
*get_driver_hdl = s_l2tap_sockets[fd].driver_handle;
break;
default:
// unsupported operation
errno = ENOSYS;
goto err;
break;
}
va_end(args);
return 0;
err:
va_end(args);
return -1;
}
static int l2tap_fcntl(int fd, int cmd, int arg)
{
int result = 0;
if (cmd == F_GETFL) {
if (s_l2tap_sockets[fd].non_blocking) {
result |= O_NONBLOCK;
}
} else if (cmd == F_SETFL) {
s_l2tap_sockets[fd].non_blocking = (arg & O_NONBLOCK) != 0;
} else {
// unsupported operation
result = -1;
errno = ENOSYS;
}
return result;
}
#ifdef CONFIG_VFS_SUPPORT_SELECT
static esp_err_t register_select(l2tap_select_args_t *args)
{
esp_err_t ret = ESP_ERR_INVALID_ARG;
if (args) {
const int new_size = s_registered_select_cnt + 1;
l2tap_select_args_t **registered_selects_new;
if ((registered_selects_new = realloc(s_registered_selects, new_size * sizeof(l2tap_select_args_t *))) == NULL) {
ret = ESP_ERR_NO_MEM;
} else {
s_registered_selects = registered_selects_new;
s_registered_selects[s_registered_select_cnt] = args;
s_registered_select_cnt = new_size;
ret = ESP_OK;
}
}
return ret;
}
static esp_err_t unregister_select(l2tap_select_args_t *args)
{
esp_err_t ret = ESP_OK;
if (args) {
ret = ESP_ERR_INVALID_STATE;
for (int i = 0; i < s_registered_select_cnt; ++i) {
if (s_registered_selects[i] == args) {
const int new_size = s_registered_select_cnt - 1;
// The item is removed by overwriting it with the last item. The subsequent rellocation will drop the
// last item.
s_registered_selects[i] = s_registered_selects[new_size];
s_registered_selects = realloc(s_registered_selects, new_size * sizeof(l2tap_select_args_t *));
if (s_registered_selects || new_size == 0) {
s_registered_select_cnt = new_size;
ret = ESP_OK;
} else {
ret = ESP_ERR_NO_MEM;
}
break;
}
}
}
return ret;
}
static void l2tap_select_notify(int fd, l2tap_select_notif_e select_notif)
{
for (int i = 0; i < s_registered_select_cnt; i++) {
l2tap_select_args_t *args = s_registered_selects[i];
if (args) {
switch (select_notif) {
case L2TAP_SELECT_READ_NOTIF:
if (FD_ISSET(fd, &args->readfds_orig)) {
FD_SET(fd, args->readfds);
esp_vfs_select_triggered(args->select_sem);
}
break;
case L2TAP_SELECT_WRITE_NOTIF:
if (FD_ISSET(fd, &args->writefds_orig)) {
FD_SET(fd, args->writefds);
esp_vfs_select_triggered(args->select_sem);
}
break;
case L2TAP_SELECT_ERR_NOTIF:
if (FD_ISSET(fd, &args->errorfds_orig)) {
FD_SET(fd, args->errorfds);
esp_vfs_select_triggered(args->select_sem);
}
break;
}
}
}
}
static esp_err_t l2tap_start_select(int nfds, fd_set *readfds, fd_set *writefds, fd_set *exceptfds,
esp_vfs_select_sem_t select_sem, void **end_select_args)
{
const int max_fds = MIN(nfds, L2TAP_MAX_FDS);
*end_select_args = NULL;
l2tap_select_args_t *args = malloc(sizeof(l2tap_select_args_t));
if (args == NULL) {
return ESP_ERR_NO_MEM;
}
args->select_sem = select_sem;
args->readfds = readfds;
args->writefds = writefds;
args->errorfds = exceptfds;
args->readfds_orig = *readfds; // store the original values because they will be set to zero
args->writefds_orig = *writefds;
args->errorfds_orig = *exceptfds;
FD_ZERO(readfds);
FD_ZERO(writefds);
FD_ZERO(exceptfds);
l2tap_lock();
for (int i = 0; i < max_fds; i++) {
if (FD_ISSET(i, &args->readfds_orig)) {
if (!rx_queue_empty(&s_l2tap_sockets[i])) {
// signalize immediately when data is buffered
FD_SET(i, readfds);
esp_vfs_select_triggered(args->select_sem);
}
}
}
esp_err_t ret = register_select(args);
if (ret != ESP_OK) {
l2tap_unlock();
free(args);
return ret;
}
l2tap_unlock();
*end_select_args = args;
return ESP_OK;
}
static esp_err_t l2tap_end_select(void *end_select_args)
{
l2tap_select_args_t *args = end_select_args;
if (args == NULL) {
return ESP_OK;
}
l2tap_lock();
esp_err_t ret = unregister_select(args);
l2tap_unlock();
if (args) {
free(args);
}
return ret;
}
#endif //CONFIG_VFS_SUPPORT_SELECT
esp_err_t esp_vfs_l2tap_intf_register(l2tap_vfs_config_t *config)
{
l2tap_vfs_config_t def_config = L2TAP_VFS_CONFIG_DEFAULT();
if (config == NULL) {
ESP_LOGD(TAG, "vfs is to be registered with default settings");
config = &def_config;
}
ESP_RETURN_ON_FALSE(!s_is_registered, ESP_ERR_INVALID_STATE, TAG, "vfs is already registered");
s_is_registered = true;
esp_vfs_t vfs = {
.flags = ESP_VFS_FLAG_DEFAULT,
.write = &l2tap_write,
.open = &l2tap_open,
.close = &l2tap_close,
.read = &l2tap_read,
.fcntl = &l2tap_fcntl,
.ioctl = &l2tap_ioctl,
#ifdef CONFIG_VFS_SUPPORT_SELECT
.start_select = &l2tap_start_select,
.end_select = &l2tap_end_select,
#endif // CONFIG_VFS_SUPPORT_SELECT
};
ESP_RETURN_ON_ERROR(esp_vfs_register(config->base_path, &vfs, NULL), TAG, "vfs register error");
return ESP_OK;
}
esp_err_t esp_vfs_l2tap_intf_unregister(const char *base_path)
{
for (int i = 0; i < L2TAP_MAX_FDS; i++) {
ESP_RETURN_ON_FALSE(atomic_load(&s_l2tap_sockets[i].state) == L2TAP_SOCK_STATE_READY,
ESP_ERR_INVALID_STATE, TAG, "all FDs need to be closed");
}
if (base_path == NULL) {
ESP_RETURN_ON_ERROR(esp_vfs_unregister(L2TAP_VFS_DEFAULT_PATH), TAG, "vfs un-register error");
} else {
ESP_RETURN_ON_ERROR(esp_vfs_unregister(base_path), TAG, "vfs un-register error");
}
s_is_registered = false;
return ESP_OK;
}
#endif // CONFIG_ESP_NETIF_L2_TAP
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik