1
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
39
40
41
42
43
44
45
46
47
48
49
50
51
52
56
57
66
67
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
93
94
95
96
97
98
99
100
101
104
105
106
107
108
109
110
113
114
115
116
117
122
123
126
127
128
131
132
133
134
135
136
137
141
142
151
152
157
158
162
163
167
168
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
192
193
196
197
198
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
239
240
258
259
260
261
262
263
264
265
270
271
274
275
276
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
311
312
313
314
315
319
320
321
322
326
327
328
329
330
331
332
333
334
335
336
337
341
342
343
344
345
346
347
348
349
350
356
357
371
372
373
377
378
379
380
385
386
387
388
389
396
407
411
416
417
418
419
420
421
422
423
424
440
441
442
443
444
445
446
447
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
481
482
483
484
485
486
487
488
516
517
518
519
520
521
522
523
524
525
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
551
552
553
558
559
560
561
562
563
564
565
566
567
568
569
572
573
574
575
576
577
580
581
582
583
584
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
610
611
612
613
614
615
616
617
618
619
620
624
625
630
631
632
633
/* ... */
#include <stdio.h>
#include <stdatomic.h>
#include <sys/fcntl.h>
#include <sys/param.h>
#include <sys/queue.h>
#include "arpa/inet.h"
#include "errno.h"
#include "esp_vfs_l2tap.h"
#include "lwip/prot/ethernet.h"
#include "esp_vfs.h"
#include "esp_log.h"
#include "esp_check.h"
#include "esp_netif.h"
#include "esp_eth_driver.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "freertos/queue.h"17 includes
#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);
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;
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) {
if (frame_info.len == 0) {
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_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();
if (s_l2tap_sockets[i].driver_handle == driver_handle && (s_l2tap_sockets[i].ethtype_filter == eth_type ||
(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) {
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;
}{...} else {
l2tap_unlock();
}{...}
}{...}
}{...}
return ESP_OK;
}{ ... }
static int l2tap_open(const char *path, int flags, int mode)
{
int fd;
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)) {
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 {
errno = EIO;
}{...}
}{...} else {
errno = EBADF;
}{...}
err:
return ret;
}{ ... }
static ssize_t l2tap_read(int fd, void *data, size_t size)
{
if (atomic_load(&s_l2tap_sockets[fd].state) != L2TAP_SOCK_STATE_OPENED) {
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_rx_queue(l2tap_socket, NULL, 0);
pop_rx_queue(l2tap_socket, NULL, 0);
flush_rx_queue(l2tap_socket);
delete_rx_queue(l2tap_socket);
xSemaphoreGive(l2tap_socket->close_done_sem);
vTaskDelete(NULL);
}{ ... }
static int l2tap_close(int fd)
{
if (atomic_load(&s_l2tap_sockets[fd].state) != L2TAP_SOCK_STATE_OPENED) {
errno = EBADF;
return -1;
}{...}
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 (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;
}{...}
xSemaphoreTake(s_l2tap_sockets[fd].close_done_sem, portMAX_DELAY);
vSemaphoreDelete(s_l2tap_sockets[fd].close_done_sem);
atomic_store(&s_l2tap_sockets[fd].state, L2TAP_SOCK_STATE_READY);
return 0;
}{ ... }
static bool netif_driver_matches(esp_netif_t *netif, void* driver)
{
return esp_netif_get_io_driver(netif) == driver;
}{ ... }
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();
if (s_l2tap_sockets[fd].driver_handle == NULL) {
errno = EACCES;
l2tap_unlock();
goto err;
}{...}
if (s_l2tap_sockets[fd].ethtype_filter != *new_ethtype_filter) {
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) {
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) {
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;
if ((esp_netif = esp_netif_find_if(netif_driver_matches, s_l2tap_sockets[fd].driver_handle)) != NULL) {
*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) {
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:
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 {
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;
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;
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])) {
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;
}{ ... }
static const esp_vfs_select_ops_t s_vfs_l2tap_select = {
.start_select = &l2tap_start_select,
.end_select = &l2tap_end_select,
}{...};
/* ... */#endif
static const esp_vfs_fs_ops_t s_vfs_l2tap = {
.write = &l2tap_write,
.open = &l2tap_open,
.close = &l2tap_close,
.read = &l2tap_read,
.fcntl = &l2tap_fcntl,
.ioctl = &l2tap_ioctl,
#ifdef CONFIG_VFS_SUPPORT_SELECT
.select = &s_vfs_l2tap_select,
#endif
}{...};
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_RETURN_ON_ERROR(esp_vfs_register_fs(config->base_path, &s_vfs_l2tap, ESP_VFS_FLAG_STATIC, 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;
}{ ... }