work_queue.c

#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);
			}
		}
	}
}