TCP2UART/App/route_msg.c

#include "route_msg.h"

#include <string.h>

#include "task.h"

typedef struct {
    route_msg_t msg;
    uint8_t data[ROUTE_MSG_MAX_PAYLOAD];
    uint8_t in_use;
} route_slot_t;

static route_slot_t g_route_slots[ROUTE_MSG_POOL_SIZE];

const char *route_send_result_to_str(route_send_result_t result)
{
    switch (result) {
    case ROUTE_SEND_OK:
        return "ok";
    case ROUTE_SEND_INVALID_INPUT:
        return "invalid";
    case ROUTE_SEND_POOL_EXHAUSTED:
        return "pool";
    case ROUTE_SEND_QUEUE_FULL:
        return "queue";
    default:
        return "unknown";
    }
}

void route_msg_init(void)
{
    memset(g_route_slots, 0, sizeof(g_route_slots));
}

static route_msg_t *route_msg_try_alloc_locked(void)
{
    uint32_t index;

    for (index = 0; index < ROUTE_MSG_POOL_SIZE; ++index) {
        if (g_route_slots[index].in_use == 0u) {
            g_route_slots[index].in_use = 1u;
            g_route_slots[index].msg.data = g_route_slots[index].data;
            g_route_slots[index].msg.len = 0u;
            g_route_slots[index].msg.src_id = 0u;
            g_route_slots[index].msg.dst_mask = 0u;
            g_route_slots[index].msg.conn_type = 0u;
            return &g_route_slots[index].msg;
        }
    }

    return NULL;
}

route_msg_t *route_msg_alloc(TickType_t wait_ticks)
{
    TickType_t start_tick = xTaskGetTickCount();
    route_msg_t *msg;

    do {
        taskENTER_CRITICAL();
        msg = route_msg_try_alloc_locked();
        taskEXIT_CRITICAL();
        if (msg != NULL) {
            return msg;
        }
        if (wait_ticks == 0u) {
            break;
        }
        vTaskDelay(pdMS_TO_TICKS(1));
    } while ((xTaskGetTickCount() - start_tick) < wait_ticks);

    return NULL;
}

route_msg_t *route_msg_alloc_from_isr(BaseType_t *xHigherPriorityTaskWoken)
{
    route_msg_t *msg;
    UBaseType_t saved_interrupt_status;

    (void)xHigherPriorityTaskWoken;
    saved_interrupt_status = taskENTER_CRITICAL_FROM_ISR();
    msg = route_msg_try_alloc_locked();
    taskEXIT_CRITICAL_FROM_ISR(saved_interrupt_status);
    return msg;
}

void route_msg_free(route_msg_t *msg)
{
    uint32_t index;

    if (msg == NULL) {
        return;
    }

    taskENTER_CRITICAL();
    for (index = 0; index < ROUTE_MSG_POOL_SIZE; ++index) {
        if (&g_route_slots[index].msg == msg) {
            g_route_slots[index].in_use = 0u;
            g_route_slots[index].msg.len = 0u;
            break;
        }
    }
    taskEXIT_CRITICAL();
}

void route_msg_free_from_isr(route_msg_t *msg)
{
    uint32_t index;
    UBaseType_t saved_interrupt_status;

    if (msg == NULL) {
        return;
    }

    saved_interrupt_status = taskENTER_CRITICAL_FROM_ISR();
    for (index = 0; index < ROUTE_MSG_POOL_SIZE; ++index) {
        if (&g_route_slots[index].msg == msg) {
            g_route_slots[index].in_use = 0u;
            g_route_slots[index].msg.len = 0u;
            break;
        }
    }
    taskEXIT_CRITICAL_FROM_ISR(saved_interrupt_status);
}

static route_send_result_t route_prepare(route_msg_t *msg,
                                         uint8_t src_id,
                                         uint8_t dst_mask,
                                         uint8_t conn_type,
                                         const uint8_t *data,
                                         uint16_t len)
{
    if (msg == NULL || data == NULL || len == 0u || len > ROUTE_MSG_MAX_PAYLOAD) {
        return ROUTE_SEND_INVALID_INPUT;
    }

    msg->src_id = src_id;
    msg->dst_mask = dst_mask;
    msg->conn_type = conn_type;
    msg->len = len;
    memcpy(msg->data, data, len);
    return ROUTE_SEND_OK;
}

static route_send_result_t route_validate_args(QueueHandle_t queue,
                                               const uint8_t *data,
                                               uint16_t len)
{
    if (queue == NULL || data == NULL || len == 0u || len > ROUTE_MSG_MAX_PAYLOAD) {
        return ROUTE_SEND_INVALID_INPUT;
    }

    return ROUTE_SEND_OK;
}

route_send_result_t route_send(QueueHandle_t queue,
                               uint8_t src_id,
                               uint8_t dst_mask,
                               uint8_t conn_type,
                               const uint8_t *data,
                               uint16_t len,
                               TickType_t wait_ticks)
{
    route_send_result_t result;
    route_msg_t *msg;

    result = route_validate_args(queue, data, len);
    if (result != ROUTE_SEND_OK) {
        return result;
    }

    msg = route_msg_alloc(wait_ticks);

    if (msg == NULL) {
        return ROUTE_SEND_POOL_EXHAUSTED;
    }

    result = route_prepare(msg, src_id, dst_mask, conn_type, data, len);
    if (result != ROUTE_SEND_OK) {
        route_msg_free(msg);
        return result;
    }

    if (xQueueSend(queue, &msg, wait_ticks) != pdPASS) {
        route_msg_free(msg);
        return ROUTE_SEND_QUEUE_FULL;
    }

    return ROUTE_SEND_OK;
}

route_send_result_t route_send_from_isr(QueueHandle_t queue,
                                        uint8_t src_id,
                                        uint8_t dst_mask,
                                        uint8_t conn_type,
                                        const uint8_t *data,
                                        uint16_t len,
                                        BaseType_t *xHigherPriorityTaskWoken)
{
    route_send_result_t result;
    route_msg_t *msg;

    result = route_validate_args(queue, data, len);
    if (result != ROUTE_SEND_OK) {
        return result;
    }

    msg = route_msg_alloc_from_isr(xHigherPriorityTaskWoken);

    if (msg == NULL) {
        return ROUTE_SEND_POOL_EXHAUSTED;
    }

    result = route_prepare(msg, src_id, dst_mask, conn_type, data, len);
    if (result != ROUTE_SEND_OK) {
        route_msg_free_from_isr(msg);
        return result;
    }

    if (xQueueSendFromISR(queue, &msg, xHigherPriorityTaskWoken) != pdPASS) {
        route_msg_free_from_isr(msg);
        return ROUTE_SEND_QUEUE_FULL;
    }

    return ROUTE_SEND_OK;
}