diff --git a/epicardium/main.c b/epicardium/main.c index b6dad7636ad317a112f50f5b5fb4eb1cd64ea365..f4c004868d8366903010f4d1664e6b2b6f3ad5af 100644 --- a/epicardium/main.c +++ b/epicardium/main.c @@ -179,6 +179,18 @@ int main(void) panic("Failed to create %s task!", "Lifecycle"); } + /* Work Queue */ + if (xTaskCreate( + vWorkQueueTask, + (const char *)"Work Queue", + configMINIMAL_STACK_SIZE * 4, + NULL, + tskIDLE_PRIORITY + 1, + NULL) != pdPASS) { + panic("Failed to create %s task!", "Work Queue"); + } + workqueue_init(); + /* * Initialize serial driver data structures. */ diff --git a/epicardium/modules/meson.build b/epicardium/modules/meson.build index d77cfc2a2b7f491ad5b179d78ff4fc71fba7951c..31302e9f1e9f33a19557f202173879e0811f233c 100644 --- a/epicardium/modules/meson.build +++ b/epicardium/modules/meson.build @@ -28,5 +28,6 @@ module_sources = files( 'usb.c', 'vibra.c', 'watchdog.c', + 'work_queue.c', 'ws2812.c' ) diff --git a/epicardium/modules/modules.h b/epicardium/modules/modules.h index 14184bd9ca96b367ebcf57a7d8b7739a407df817..ccc8d0650497133bb193fc162fb112c6f364e82e 100644 --- a/epicardium/modules/modules.h +++ b/epicardium/modules/modules.h @@ -138,6 +138,13 @@ extern gpio_cfg_t gpio_configs[]; void sleep_deepsleep(void); +/* ---------- RNG ---------------------------------------------------------- */ void rng_init(void); +/* ---------- Work Queue --------------------------------------------------- */ +#define WORK_QUEUE_SIZE 20 +void workqueue_init(void); +int workqueue_schedule(void (*func)(void *data), void *data); +void vWorkQueueTask(void *pvParameters); + #endif /* MODULES_H */ diff --git a/epicardium/modules/work_queue.c b/epicardium/modules/work_queue.c new file mode 100644 index 0000000000000000000000000000000000000000..3af2c03cc0e12f3b9df555c25a8160cc4aa68829 --- /dev/null +++ b/epicardium/modules/work_queue.c @@ -0,0 +1,47 @@ +#include "epicardium.h" +#include "modules/log.h" +#include "modules.h" + +#include "FreeRTOS.h" +#include "queue.h" + +struct work { + void (*func)(void *data); + void *data; +}; + +static QueueHandle_t work_queue; +static uint8_t buffer[sizeof(struct work) * WORK_QUEUE_SIZE]; +static StaticQueue_t work_queue_data; + +void workqueue_init(void) +{ + work_queue = xQueueCreateStatic( + WORK_QUEUE_SIZE, sizeof(struct work), buffer, &work_queue_data + ); +} + +int workqueue_schedule(void (*func)(void *data), void *data) +{ + struct work work = { func, data }; + if (xQueueSend(work_queue, &work, 0) != pdTRUE) { + /* Likely full */ + LOG_WARN("workqueue", "could not schedule %p(%p)", func, data); + return -EAGAIN; + } + + return 0; +} + +void vWorkQueueTask(void *pvParameters) +{ + struct work work; + workqueue_init(); + while (1) { + if (xQueueReceive(work_queue, &work, portMAX_DELAY) == pdTRUE) { + if (work.func) { + work.func(work.data); + } + } + } +}