diff --git a/src/linux/Makefile b/src/linux/Makefile index 7c1de7c1f88b..eca5c0c1679f 100644 --- a/src/linux/Makefile +++ b/src/linux/Makefile @@ -5,7 +5,7 @@ dirs-y += src/linux src/generic src-y += linux/main.c linux/timer.c linux/console.c linux/watchdog.c src-y += linux/pca9685.c linux/spidev.c linux/analog.c linux/hard_pwm.c src-y += linux/i2c.c linux/gpio.c generic/crc16_ccitt.c generic/alloc.c -src-y += linux/sensor_ds18b20.c +src-y += linux/sensor_ds18b20.c linux/ringbuf.c CFLAGS_klipper.elf += -lutil -lrt -lpthread diff --git a/src/linux/console.c b/src/linux/console.c index 96782096d6ce..abcd93f20038 100644 --- a/src/linux/console.c +++ b/src/linux/console.c @@ -14,13 +14,20 @@ #include // chmod #include // struct timespec #include // ttyname +#include // pthread_create #include "board/irq.h" // irq_wait #include "board/misc.h" // console_sendf #include "command.h" // command_find_block #include "internal.h" // console_setup #include "sched.h" // sched_wake_task +#include "ringbuf.h" // SPSC ring buf static struct pollfd main_pfd[1]; +static struct ring_buf outputq; +static struct ring_buf inputq; +static pthread_t main; +static pthread_t reader; +static pthread_t writer; #define MP_TTY_IDX 0 // Report 'errno' in a message written to stderr @@ -31,6 +38,47 @@ report_errno(char *where, int rc) fprintf(stderr, "Got error %d in %s: (%d)%s\n", rc, where, e, strerror(e)); } +/**************************************************************** + * Threaded IO + ****************************************************************/ + +static void * +tty_reader(void *_unused) +{ + static uint8_t buf[128]; + while (1) { + int ret = read(main_pfd[MP_TTY_IDX].fd, buf, sizeof(buf)); + if (ret < 0) { + report_errno("read", ret); + continue; + } + while (ring_buffer_available_to_write(&inputq) < ret) + nanosleep(&(struct timespec){.tv_nsec = 10000}, NULL); + ring_buffer_write(&inputq, buf, ret); + } + + return NULL; +} + +static void * +tty_writer(void *_unused) +{ + static uint8_t buf[128]; + while (1) { + int available = ring_buffer_available_to_read(&outputq); + if (available == 0) { + nanosleep(&(struct timespec){.tv_nsec = 100000}, NULL); + continue; + } + + int len = ring_buffer_read(&outputq, buf, sizeof(buf)); + int ret = write(main_pfd[MP_TTY_IDX].fd, buf, len); + if (ret < 0) + report_errno("write", ret); + } + + return NULL; +} /**************************************************************** * Setup @@ -74,9 +122,6 @@ console_setup(char *name) report_errno("openpty", ret); return -1; } - ret = set_non_blocking(mfd); - if (ret) - return -1; ret = set_close_on_exec(mfd); if (ret) return -1; @@ -109,6 +154,15 @@ console_setup(char *name) if (ret) return -1; + ring_buffer_init(&inputq); + ring_buffer_init(&outputq); + + main = pthread_self(); + pthread_create(&reader, NULL, tty_reader, NULL); + pthread_setschedparam(reader, SCHED_OTHER, &(struct sched_param){.sched_priority = 0}); + pthread_create(&writer, NULL, tty_writer, NULL); + pthread_setschedparam(writer, SCHED_OTHER, &(struct sched_param){.sched_priority = 0}); + return 0; } @@ -135,16 +189,8 @@ console_task(void) return; // Read data - int ret = read(main_pfd[MP_TTY_IDX].fd, &receive_buf[receive_pos] - , sizeof(receive_buf) - receive_pos); - if (ret < 0) { - if (errno == EWOULDBLOCK) { - ret = 0; - } else { - report_errno("read", ret); - return; - } - } + int ret = ring_buffer_read(&inputq, &receive_buf[receive_pos], + sizeof(receive_buf) - receive_pos); if (ret == 15 && receive_buf[receive_pos+14] == '\n' && memcmp(&receive_buf[receive_pos], "FORCE_SHUTDOWN\n", 15) == 0) shutdown("Force shutdown command"); @@ -171,23 +217,20 @@ console_sendf(const struct command_encoder *ce, va_list args) // Generate message uint8_t buf[MESSAGE_MAX]; uint_fast8_t msglen = command_encode_and_frame(buf, ce, args); + while (ring_buffer_available_to_write(&outputq) < msglen) + nanosleep(&(struct timespec){.tv_nsec = 1000}, NULL); // Transmit message - int ret = write(main_pfd[MP_TTY_IDX].fd, buf, msglen); - if (ret < 0) - report_errno("write", ret); + ring_buffer_write(&outputq, buf, msglen); } -// Sleep until a signal received (waking early for console input if needed) +// Sleep for the specified time or until a signal interrupts void -console_sleep(sigset_t *sigset) +console_sleep(void) { - int ret = ppoll(main_pfd, ARRAY_SIZE(main_pfd), NULL, sigset); - if (ret <= 0) { - if (errno != EINTR) - report_errno("ppoll main_pfd", ret); + if (ring_buffer_available_to_read(&inputq) > 0) { + sched_wake_task(&console_wake); return; } - if (main_pfd[MP_TTY_IDX].revents) - sched_wake_task(&console_wake); + nanosleep(&(struct timespec){.tv_nsec = 500000}, NULL); } diff --git a/src/linux/internal.h b/src/linux/internal.h index db841ac91d84..56621aa63ef7 100644 --- a/src/linux/internal.h +++ b/src/linux/internal.h @@ -19,7 +19,7 @@ void report_errno(char *where, int rc); int set_non_blocking(int fd); int set_close_on_exec(int fd); int console_setup(char *name); -void console_sleep(sigset_t *sigset); +void console_sleep(void); // timer.c int timer_check_periodic(uint32_t *ts); diff --git a/src/linux/ringbuf.c b/src/linux/ringbuf.c new file mode 100644 index 000000000000..bcd967258b85 --- /dev/null +++ b/src/linux/ringbuf.c @@ -0,0 +1,76 @@ +#include +#include +#include +#include +#include "ringbuf.h" + +// Simple Single Producer Single Consumer Ring Buffer + +void ring_buffer_init(struct ring_buf *rb) +{ + atomic_store(&rb->head, 0); + atomic_store(&rb->tail, 0); + atomic_store(&rb->size, 0); +} + +int ring_buffer_available_to_read(const struct ring_buf *rb) +{ + return atomic_load(&rb->size); +} + +int ring_buffer_available_to_write(const struct ring_buf *rb) +{ + return RING_BUFFER_SIZE - atomic_load(&rb->size); +} + +int ring_buffer_write(struct ring_buf *rb, const uint8_t *data, int length) +{ + int available = ring_buffer_available_to_write(rb); + int to_write = (length < available) ? length : available; + + int head = atomic_load_explicit(&rb->head, memory_order_acquire); + int first_chunk = RING_BUFFER_SIZE - head; + if (first_chunk > to_write) { + first_chunk = to_write; + } + memcpy(&rb->buffer[head], data, first_chunk); + head = (head + first_chunk) % RING_BUFFER_SIZE; + atomic_store_explicit(&rb->head, head, memory_order_release); + atomic_fetch_add(&rb->size, first_chunk); + + int second_chunk = to_write - first_chunk; + if (second_chunk > 0) { + memcpy(&rb->buffer[0], data + first_chunk, second_chunk); + } + head = (head + second_chunk) % RING_BUFFER_SIZE; + atomic_store_explicit(&rb->head, head, memory_order_release); + atomic_fetch_add(&rb->size, second_chunk); + + return to_write; +} + +int ring_buffer_read(struct ring_buf *rb, uint8_t *data, int length) +{ + int available = ring_buffer_available_to_read(rb); + int to_read = (length < available) ? length : available; + + int tail = atomic_load_explicit(&rb->tail, memory_order_acquire); + int first_chunk = RING_BUFFER_SIZE - tail; + if (first_chunk > to_read) { + first_chunk = to_read; + } + memcpy(data, &rb->buffer[tail], first_chunk); + tail = (tail + first_chunk) % RING_BUFFER_SIZE; + atomic_store_explicit(&rb->tail, tail, memory_order_release); + atomic_fetch_sub(&rb->size, first_chunk); + + int second_chunk = to_read - first_chunk; + if (second_chunk > 0) { + memcpy(data + first_chunk, &rb->buffer[0], second_chunk); + tail = second_chunk % RING_BUFFER_SIZE; + } + atomic_store_explicit(&rb->tail, tail, memory_order_release); + atomic_fetch_sub(&rb->size, second_chunk); + + return to_read; +} diff --git a/src/linux/ringbuf.h b/src/linux/ringbuf.h new file mode 100644 index 000000000000..189511b49e26 --- /dev/null +++ b/src/linux/ringbuf.h @@ -0,0 +1,16 @@ +#include + +#define RING_BUFFER_SIZE 8192 +struct ring_buf +{ + uint8_t buffer[RING_BUFFER_SIZE]; + _Atomic int head; + _Atomic int tail; + _Atomic int size; +}; + +void ring_buffer_init(struct ring_buf *rb); +int ring_buffer_available_to_read(const struct ring_buf *rb); +int ring_buffer_available_to_write(const struct ring_buf *rb); +int ring_buffer_write(struct ring_buf *rb, const uint8_t *data, int length); +int ring_buffer_read(struct ring_buf *rb, uint8_t *data, int length); diff --git a/src/linux/timer.c b/src/linux/timer.c index 8eda62a54ddc..d8a392d353bd 100644 --- a/src/linux/timer.c +++ b/src/linux/timer.c @@ -269,10 +269,7 @@ irq_wait(void) { // Must atomically sleep until signaled if (!readl(&TimerInfo.must_wake_timers)) { - timer_disable_signals(); - if (!TimerInfo.must_wake_timers) - console_sleep(&TimerInfo.ss_sleep); - timer_enable_signals(); + console_sleep(); } irq_poll(); }