diff options
Diffstat (limited to 'yocto/esp32d')
| -rw-r--r-- | yocto/esp32d/Makefile | 15 | ||||
| -rw-r--r-- | yocto/esp32d/msgq.c | 43 | ||||
| -rw-r--r-- | yocto/esp32d/msgq.h | 22 | ||||
| -rw-r--r-- | yocto/esp32d/spi.c | 411 | ||||
| -rw-r--r-- | yocto/esp32d/spi.h | 31 | ||||
| -rw-r--r-- | yocto/esp32d/tun.c | 84 | ||||
| -rw-r--r-- | yocto/esp32d/tun.h | 5 |
7 files changed, 611 insertions, 0 deletions
diff --git a/yocto/esp32d/Makefile b/yocto/esp32d/Makefile new file mode 100644 index 0000000..677d09d --- /dev/null +++ b/yocto/esp32d/Makefile @@ -0,0 +1,15 @@ +#CFLAGS = +LDFLAGS = -pthread -lgpiod +TARGET = esp32d +obj = msgq.o spi.o tun.o + +all: $(TARGET) + +%.o: %.c %.h + $(CC) $(CFLAGS) -c $< + +$(TARGET): $(obj) + $(CC) $(obj) $(LDFLAGS) -o $@ + +clean: + rm -f $(TARGET) *.o diff --git a/yocto/esp32d/msgq.c b/yocto/esp32d/msgq.c new file mode 100644 index 0000000..3039f13 --- /dev/null +++ b/yocto/esp32d/msgq.c @@ -0,0 +1,43 @@ +#include <stdlib.h> +#include <pthread.h> + +#include "msgq.h" + +#define IDX_MASK(IDX, SIZE) ((IDX) & ((SIZE) - 1)) + +int msgq_init(MSGQueue *msgq, unsigned char **array, uint16_t size) { + int rv; + + msgq->idx_r = 0; + msgq->idx_w = 0; + msgq->size = size; + msgq->array = array; + rv = pthread_mutex_init(&msgq->mutex, NULL); + if (rv) { + return MSGQ_ERR; + } + + rv = pthread_cond_init(&msgq->cond, NULL); + if (rv) { + pthread_mutex_destroy(&msgq->mutex); + return MSGQ_ERR; + } + return MSGQ_OK; +} + +int msgq_push(MSGQueue *msgq, unsigned char *buffer) { + if ((uint16_t)(msgq->idx_w - msgq->idx_r) == msgq->size) return MSGQ_ERR_FULL; + + msgq->array[IDX_MASK(msgq->idx_w++, msgq->size)] = buffer; + return MSGQ_OK; +} + +unsigned char *msgq_pop(MSGQueue *msgq) { + if (msgq->idx_r == msgq->idx_w) return NULL; + + return msgq->array[IDX_MASK(msgq->idx_r++, msgq->size)]; +} + +uint16_t msgq_len(MSGQueue *msgq) { + return (uint16_t)(msgq->idx_w - msgq->idx_r); +} diff --git a/yocto/esp32d/msgq.h b/yocto/esp32d/msgq.h new file mode 100644 index 0000000..32b20d0 --- /dev/null +++ b/yocto/esp32d/msgq.h @@ -0,0 +1,22 @@ +#include <stdint.h> + +#define MSGQ_OK 0 +#define MSGQ_ERR -1 + +#define MSGQ_ERR_SIZE -10 +#define MSGQ_ERR_FULL -11 +#define MSGQ_ERR_EMPTY -12 + +typedef struct MSGQueue { + uint16_t idx_r; + uint16_t idx_w; + uint16_t size; + unsigned char **array; + pthread_mutex_t mutex; + pthread_cond_t cond; +} MSGQueue; + +int msgq_init(MSGQueue *msgq, unsigned char **array, uint16_t size); +int msgq_push(MSGQueue *msgq, unsigned char *buffer); +unsigned char *msgq_pop(MSGQueue *msgq); +uint16_t msgq_len(MSGQueue *msgq); diff --git a/yocto/esp32d/spi.c b/yocto/esp32d/spi.c new file mode 100644 index 0000000..db46a63 --- /dev/null +++ b/yocto/esp32d/spi.c @@ -0,0 +1,411 @@ +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <assert.h> +#include <stdio.h> +#include <fcntl.h> +#include <sys/ioctl.h> + +#include <linux/spi/spidev.h> + +#include <gpiod.h> +#include <pthread.h> + +#include "msgq.h" +#include "tun.h" +#include "spi.h" + +static pthread_t worker_thd; +static pthread_t rtscts_thd; +static pthread_t msg_handler_thd; +static pthread_t tun_handler_thd; +static pthread_mutex_t mutex; +static pthread_cond_t cond; + +static MSGQueue spi_bufq; +static unsigned char *spi_bufq_array[SPI_SIZE_BUFQ]; + +static MSGQueue spi_msgq_in; +static unsigned char *spi_msgq_in_array[SPI_SIZE_MSGQ_IN]; + +static MSGQueue spi_msgq_out; +static unsigned char *spi_msgq_out_array[SPI_SIZE_MSGQ_OUT]; + +static uint32_t spi_speed = SPI_SPEED; +static int spi_fd; +static volatile int spi_cts; + +struct gpiod_line_request *request = NULL; + +static void _spi_wait4cts(void) { + pthread_mutex_lock(&mutex); + while (!spi_cts) { + pthread_cond_wait(&cond, &mutex); + } + spi_cts = 0; + pthread_mutex_unlock(&mutex); +} + +static int _spi_xchg(unsigned char *buffer) { + int rv; + uint16_t len_tx; + uint16_t len_rx; + struct spi_ioc_transfer tr; + + memset(&tr, 0, sizeof(tr)); + tr.tx_buf = (unsigned long)buffer; + tr.rx_buf = (unsigned long)buffer; + tr.speed_hz = spi_speed; + + len_tx = (uint16_t)buffer[1] << 8; + len_tx |= (uint16_t)buffer[2] & 0xFF; + if (len_tx > SPI_MTU) return SPI_ERR; + + if (buffer[0]) { + len_tx += SPI_SIZE_HDR; + // esp32 dma workaraund + if (len_tx < 8) { + len_tx = 8; + } else if (len_tx % 4 != 0) { + len_tx = (len_tx / 4 + 1) * 4; + } + + tr.len = len_tx; + } else { + /* nothing to send, reset esp32 spi transaction */ + tr.len = 1; + + _spi_wait4cts(); + rv = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &tr); + if (rv < 0) return SPI_ERR_MSG; + + /* receive SPI_SIZE_RECEIVE bytes in first transaction (estimate) */ + len_tx = SPI_SIZE_RECEIVE + SPI_SIZE_HDR; + tr.len = len_tx; + buffer[1] = 0; + buffer[2] = 0; + } + + _spi_wait4cts(); + rv = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &tr); + if (rv < 0) return SPI_ERR_MSG; + + len_rx = (uint16_t)buffer[1] << 8; + len_rx |= (uint16_t)buffer[2] & 0xFF; + if (len_rx > SPI_MTU) return SPI_ERR; + + len_rx += SPI_SIZE_HDR; + if (len_rx > len_tx) { + tr.tx_buf = (unsigned long)NULL; + tr.rx_buf = (unsigned long)(buffer + len_tx); + + len_tx = len_rx - len_tx; + // esp32 dma workaraund + if (len_tx < 8) { + len_tx = 8; + } else if (len_tx % 4 != 0) { + len_tx = (len_tx / 4 + 1) * 4; + } + + tr.len = len_tx; + + _spi_wait4cts(); + rv = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &tr); + if (rv < 0) return SPI_ERR_MSG; + } + + return SPI_OK; +} + +static void *worker(void *arg) { + MSGQueue *bufq = &spi_bufq; + MSGQueue *msgq_in = &spi_msgq_in; + MSGQueue *msgq_out = &spi_msgq_out; + int rv; + unsigned char *buffer; + + while (1) { + pthread_mutex_lock(&msgq_out->mutex); + buffer = msgq_pop(msgq_out); + if ((buffer == NULL) && (gpiod_line_request_get_value(request, SPI_GPIO_RTS) == GPIOD_LINE_VALUE_INACTIVE)) { + pthread_mutex_lock(&bufq->mutex); + buffer = msgq_pop(bufq); + pthread_mutex_unlock(&bufq->mutex); + } + if (buffer == NULL) { + pthread_cond_wait(&msgq_out->cond, &msgq_out->mutex); + buffer = msgq_pop(msgq_out); + } + pthread_mutex_unlock(&msgq_out->mutex); + if (buffer) { + rv = _spi_xchg(buffer); + if (rv || (buffer[0] == 0)) { + buffer[0] = 0; + buffer[1] = 0; + buffer[2] = 0; + pthread_mutex_lock(&bufq->mutex); + msgq_push(bufq, buffer); + pthread_mutex_unlock(&bufq->mutex); + } else { + pthread_mutex_lock(&msgq_in->mutex); + rv = msgq_push(msgq_in, buffer); + if (rv == MSGQ_OK) pthread_cond_signal(&msgq_in->cond); + pthread_mutex_unlock(&msgq_in->mutex); + + if (rv) { + pthread_mutex_lock(&bufq->mutex); + msgq_push(bufq, buffer); + pthread_mutex_unlock(&bufq->mutex); + } + } + } + } + + return NULL; +} + +static void *rtscts_handler(void *arg) { + MSGQueue *msgq_out = &spi_msgq_out; + struct gpiod_edge_event_buffer *event_buffer; + struct gpiod_edge_event *event; + int rv; + + event_buffer = gpiod_edge_event_buffer_new(1); + + while (1) { + rv = gpiod_line_request_read_edge_events(request, event_buffer, 1); + if (rv != 1) continue; + + event = gpiod_edge_event_buffer_get_event(event_buffer, 0); + switch (gpiod_edge_event_get_line_offset(event)) { + case SPI_GPIO_RTS: { + pthread_mutex_lock(&msgq_out->mutex); + pthread_cond_signal(&msgq_out->cond); + pthread_mutex_unlock(&msgq_out->mutex); + break; + } + + case SPI_GPIO_CTS: { + pthread_mutex_lock(&mutex); + spi_cts = 1; + pthread_cond_signal(&cond); + pthread_mutex_unlock(&mutex); + break; + } + } + } + + gpiod_edge_event_buffer_free(event_buffer); + return NULL; +} + +static void *msg_handler(void *arg) { + MSGQueue *bufq = &spi_bufq; + MSGQueue *msgq_in = &spi_msgq_in; + unsigned char *buffer; + unsigned char mtype; + uint16_t len; + int rv; + + while (1) { + pthread_mutex_lock(&msgq_in->mutex); + buffer = msgq_pop(msgq_in); + if (buffer == NULL) { + pthread_cond_wait(&msgq_in->cond, &msgq_in->mutex); + buffer = msgq_pop(msgq_in); + } + pthread_mutex_unlock(&msgq_in->mutex); + if (buffer) { + mtype = buffer[0]; + len = (uint16_t)buffer[1] << 8; + len |= (uint16_t)buffer[2] & 0xFF; + + switch (mtype) { + case SPI_MTYPE_TUN: + tun_write(buffer + SPI_SIZE_HDR, len); + break; + } + buffer[0] = 0; + buffer[1] = 0; + buffer[2] = 0; + pthread_mutex_lock(&bufq->mutex); + msgq_push(bufq, buffer); + pthread_mutex_unlock(&bufq->mutex); + } + } + + return NULL; +} + +static void *tun_handler(void *arg) { + unsigned char *buffer; + ssize_t len; + + while (1) { + buffer = spi_alloc(); + if (buffer == NULL) continue; + + len = tun_read(buffer + SPI_SIZE_HDR, SPI_SIZE_BUF - SPI_SIZE_HDR); + if (len < 0) { + perror("tun read"); + continue; + } + spi_xchg(SPI_MTYPE_TUN, buffer, len); + } + + return NULL; +} + +unsigned char *spi_alloc(void) { + MSGQueue *bufq = &spi_bufq; + unsigned char *buffer; + + pthread_mutex_lock(&bufq->mutex); + buffer = msgq_pop(bufq); + pthread_mutex_unlock(&bufq->mutex); + + return buffer; +} + +void spi_free(unsigned char *buffer) { + MSGQueue *bufq = &spi_bufq; + + buffer[0] = 0; + buffer[1] = 0; + buffer[2] = 0; + pthread_mutex_lock(&bufq->mutex); + msgq_push(bufq, buffer); + pthread_mutex_unlock(&bufq->mutex); +} + +int spi_xchg(unsigned char mtype, unsigned char *buffer, uint16_t len) { + MSGQueue *bufq = &spi_bufq; + MSGQueue *msgq_out = &spi_msgq_out; + int rv; + + buffer[0] = mtype; + buffer[1] = len >> 8; + buffer[2] = len & 0xFF; + + pthread_mutex_lock(&msgq_out->mutex); + rv = msgq_push(msgq_out, buffer); + if (rv == MSGQ_OK) pthread_cond_signal(&msgq_out->cond); + pthread_mutex_unlock(&msgq_out->mutex); + + if (rv) { + pthread_mutex_lock(&bufq->mutex); + msgq_push(bufq, buffer); + pthread_mutex_unlock(&bufq->mutex); + } + + return rv; +} + +int gpio_init(void) { + struct gpiod_chip *chip = NULL; + struct gpiod_line_settings *line_settings = NULL; + struct gpiod_line_config *line_cfg = NULL; + struct gpiod_request_config *req_cfg = NULL; + int rv; + + unsigned int line_offsets[2] = { SPI_GPIO_RTS, SPI_GPIO_CTS }; + + chip = gpiod_chip_open(SPI_GPIO_DEV); + if (chip == NULL) goto gpio_init_fin; + + line_settings = gpiod_line_settings_new(); + if (line_settings == NULL) goto gpio_init_fin; + + gpiod_line_settings_set_direction(line_settings, GPIOD_LINE_DIRECTION_INPUT); + gpiod_line_settings_set_edge_detection(line_settings, GPIOD_LINE_EDGE_FALLING); + gpiod_line_settings_set_bias(line_settings, GPIOD_LINE_BIAS_PULL_UP); + + line_cfg = gpiod_line_config_new(); + if (line_cfg == NULL) goto gpio_init_fin; + + rv = gpiod_line_config_add_line_settings(line_cfg, line_offsets, 2, line_settings); + if (rv) goto gpio_init_fin; + + req_cfg = gpiod_request_config_new(); + if (req_cfg == NULL) goto gpio_init_fin; + + gpiod_request_config_set_consumer(req_cfg, "rts-cts"); + + request = gpiod_chip_request_lines(chip, req_cfg, line_cfg); + +gpio_init_fin: + rv = (request ? SPI_OK : (chip ? SPI_ERR : SPI_ERR_OPEN)); + + if (req_cfg) gpiod_request_config_free(req_cfg); + if (line_cfg) gpiod_line_config_free(line_cfg); + if (line_settings) gpiod_line_settings_free(line_settings); + if (chip) gpiod_chip_close(chip); + + return rv; +} + +int spi_init(void) { + unsigned char *buffer; + int rv, i; + + spi_fd = open(SPI_DEV, O_RDWR); + if (spi_fd < 0) return SPI_ERR_OPEN; + + rv = ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); + if (rv == -1) return SPI_ERR; + + rv = msgq_init(&spi_bufq, spi_bufq_array, SPI_SIZE_BUFQ); + assert(rv == MSGQ_OK); + + rv = msgq_init(&spi_msgq_in, spi_msgq_in_array, SPI_SIZE_MSGQ_IN); + assert(rv == MSGQ_OK); + + rv = msgq_init(&spi_msgq_out, spi_msgq_out_array, SPI_SIZE_MSGQ_OUT); + assert(rv == MSGQ_OK); + + for (i=0; i<SPI_SIZE_BUFQ; i++) { + buffer = malloc(SPI_SIZE_BUF); + assert(buffer); + msgq_push(&spi_bufq, buffer); + } + + rv = pthread_mutex_init(&mutex, NULL); + assert(rv == 0); + + rv = pthread_cond_init(&cond, NULL); + assert(rv == 0); + + /* assret initial contitions */ + pthread_mutex_lock(&mutex); + spi_cts = (gpiod_line_request_get_value(request, SPI_GPIO_CTS) == GPIOD_LINE_VALUE_INACTIVE); + pthread_mutex_unlock(&mutex); + + rv = pthread_create(&worker_thd, NULL, worker, NULL); + assert(rv == 0); + + rv = pthread_create(&rtscts_thd, NULL, rtscts_handler, NULL); + assert(rv == 0); + + rv = pthread_create(&msg_handler_thd, NULL, msg_handler, NULL); + assert(rv == 0); + + rv = pthread_create(&tun_handler_thd, NULL, tun_handler, NULL); + assert(rv == 0); + + return SPI_OK; +} + +int main(int argc, char *argv[]) { + int rv; + + rv = tun_init(SPI_TUN_NAME); + if (rv) printf("TUN INIT ERR\n"); + + rv = gpio_init(); + if (rv) printf("GPIO INIT ERR\n"); + + rv = spi_init(); + if (rv) printf("SPI INIT ERR\n"); + + while (1); +} diff --git a/yocto/esp32d/spi.h b/yocto/esp32d/spi.h new file mode 100644 index 0000000..4d9ff86 --- /dev/null +++ b/yocto/esp32d/spi.h @@ -0,0 +1,31 @@ +#include <stdint.h> + +#define SPI_DEV "/dev/spidev0" +#define SPI_SPEED 10000000 + +#define SPI_TUN_NAME "tun0" + +#define SPI_GPIO_DEV "/dev/gpiochip3" +#define SPI_GPIO_CTS 28 +#define SPI_GPIO_RTS 25 + +#define SPI_MTU 1500 +#define SPI_SIZE_HDR 3 +#define SPI_SIZE_BUF (SPI_MTU + SPI_SIZE_HDR) +#define SPI_SIZE_RECEIVE 16 /* guestimate on number of bytes for transaction initiated by falling RTS */ + +#define SPI_SIZE_BUFQ 64 +#define SPI_SIZE_MSGQ_IN 32 +#define SPI_SIZE_MSGQ_OUT 32 + +#define SPI_MTYPE_TUN 1 + +#define SPI_OK 0 +#define SPI_ERR -1 +#define SPI_ERR_OPEN -10 +#define SPI_ERR_MSG -11 + +unsigned char *spi_alloc(void); +void spi_free(unsigned char *buffer); +int spi_xchg(unsigned char mtype, unsigned char *buffer, uint16_t len); +int spi_init(void);
\ No newline at end of file diff --git a/yocto/esp32d/tun.c b/yocto/esp32d/tun.c new file mode 100644 index 0000000..75043a3 --- /dev/null +++ b/yocto/esp32d/tun.c @@ -0,0 +1,84 @@ +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <stdio.h> +#include <fcntl.h> +#include <sys/ioctl.h> + +#include <linux/if.h> +#include <linux/if_tun.h> + +#include <pthread.h> + +#include "spi.h" +#include "tun.h" + +static pthread_t read_thd; + +static int tun_fd; +static char tun_name[IFNAMSIZ]; + +static int tun_alloc(char *dev, int flags) { + struct ifreq ifr; + int fd, err; + char *clonedev = "/dev/net/tun"; + + /* Arguments taken by the function: + * + * char *dev: the name of an interface (or '\0'). MUST have enough + * space to hold the interface name if '\0' is passed + * int flags: interface flags (eg, IFF_TUN etc.) + */ + + fd = open(clonedev, O_RDWR); + if (fd < 0) { + return fd; + } + + memset(&ifr, 0, sizeof(ifr)); + ifr.ifr_flags = flags; /* IFF_TUN or IFF_TAP, plus maybe IFF_NO_PI */ + + if (*dev) { + /* if a device name was specified, put it in the structure; otherwise, + * the kernel will try to allocate the "next" device of the + * specified type */ + strncpy(ifr.ifr_name, dev, IFNAMSIZ); + } + + /* try to create the device */ + err = ioctl(fd, TUNSETIFF, (void *) &ifr); + if (err < 0) { + close(fd); + return err; + } + + /* if the operation was successful, write back the name of the + * interface to the variable "dev", so the caller can know + * it. Note that the caller MUST reserve space in *dev (see calling + * code below) */ + strcpy(dev, ifr.ifr_name); + + /* this is the special file descriptor that the caller will use to talk + * with the virtual interface */ + return fd; +} + +ssize_t tun_read(unsigned char *buffer, size_t buf_size) { + return read(tun_fd, buffer, buf_size); +} + +ssize_t tun_write(unsigned char *buffer, size_t buf_len) { + return write(tun_fd, buffer, buf_len); +} + +int tun_init(char *name) { + int rv; + + if (strlen(name) >= sizeof(tun_name) - 1) return -1; + strcpy(tun_name, name); + + tun_fd = tun_alloc(tun_name, IFF_TUN | IFF_NO_PI); + if (tun_fd < 0) return -1; + + return 0; +} diff --git a/yocto/esp32d/tun.h b/yocto/esp32d/tun.h new file mode 100644 index 0000000..793a4c9 --- /dev/null +++ b/yocto/esp32d/tun.h @@ -0,0 +1,5 @@ +#include <sys/types.h> + +ssize_t tun_read(unsigned char *buffer, size_t buf_size); +ssize_t tun_write(unsigned char *buffer, size_t buf_len); +int tun_init(char *name);
\ No newline at end of file |