templates/c/set-protocol/src/set_can.c

#include "set_can.h"

#include <string.h>

uint32_t setp_can_id_pack(const setp_can_id_t *value)
{
    if (value == NULL) {
        return 0U;
    }
    return ((uint32_t)SETP_CAN_ID_PREFIX << 24)
        | ((uint32_t)value->destination << 16)
        | ((uint32_t)value->source << 8)
        | ((uint32_t)(value->priority & 1U) << 7)
        | (uint32_t)(value->channel & 0x7FU);
}

bool setp_can_id_unpack(uint32_t raw, setp_can_id_t *out)
{
    if ((out == NULL) || ((raw & ~SETP_CAN_ID_MASK) != 0U)
            || ((raw & SETP_CAN_ID_PREFIX_MASK)
            != ((uint32_t)SETP_CAN_ID_PREFIX << 24))) {
        return false;
    }
    out->destination = (uint8_t)(raw >> 16);
    out->source = (uint8_t)(raw >> 8);
    out->priority = (uint8_t)((raw >> 7) & 1U);
    out->channel = (uint8_t)(raw & 0x7FU);
    return true;
}

bool setp_can_segment(const uint8_t *packet, uint16_t length, uint32_t can_id,
                      setp_can_send_fn send, void *user)
{
    setp_can_frame_t frame;
    uint16_t offset;
    uint8_t sequence = 1U;

    if ((packet == NULL) || (send == NULL)
            || (length < SETP_HEADER_SIZE + SETP_CRC_SIZE)
            || (length > SETP_FRAME_MAX)
            || ((can_id & ~SETP_CAN_ID_MASK) != 0U)) {
        return false;
    }
    (void)memset(&frame, 0, sizeof(frame));
    frame.id = can_id;
    frame.length = 8U;
    frame.data[0] = SETP_CAN_PCI_FIRST;
    setp_put_u16(&frame.data[1], length);
    (void)memcpy(&frame.data[3], packet, SETP_CAN_FIRST_DATA);
    if (!send(&frame, user)) {
        return false;
    }
    offset = SETP_CAN_FIRST_DATA;

    while (offset < length) {
        uint16_t remaining = (uint16_t)(length - offset);
        uint8_t chunk = remaining > SETP_CAN_CONSECUTIVE_DATA
            ? SETP_CAN_CONSECUTIVE_DATA : (uint8_t)remaining;
        (void)memset(frame.data, 0, sizeof(frame.data));
        frame.data[0] = (uint8_t)(SETP_CAN_PCI_CONSECUTIVE
                                  | (sequence & SETP_CAN_PCI_VALUE_MASK));
        (void)memcpy(&frame.data[1], &packet[offset], chunk);
        frame.length = (uint8_t)(1U + chunk);
        if (!send(&frame, user)) {
            return false;
        }
        offset = (uint16_t)(offset + chunk);
        sequence = (uint8_t)((sequence + 1U) & SETP_CAN_PCI_VALUE_MASK);
    }
    return true;
}

void setp_can_rx_init(setp_can_rx_t *state)
{
    if (state != NULL) {
        (void)memset(state, 0, sizeof(*state));
    }
}

static bool setp_can_expired(const setp_can_rx_t *state, uint32_t now_ms)
{
    return state->active && ((int32_t)(now_ms - state->deadline_ms) >= 0);
}

setp_can_rx_result_t setp_can_rx_feed(setp_can_rx_t *state,
                                      const setp_can_frame_t *frame,
                                      uint32_t now_ms, setp_can_packet_t *out)
{
    uint8_t pci;
    if ((state == NULL) || (frame == NULL) || (out == NULL)
            || (frame->length == 0U) || (frame->length > 8U)) {
        return SETP_CAN_RX_FORMAT_ERROR;
    }
    if (setp_can_expired(state, now_ms)) {
        state->active = 0U;
        return SETP_CAN_RX_TIMEOUT;
    }
    pci = (uint8_t)(frame->data[0] & SETP_CAN_PCI_TYPE_MASK);
    if (pci == SETP_CAN_PCI_FIRST) {
        uint16_t total;
        if (frame->length != 8U) {
            return SETP_CAN_RX_FORMAT_ERROR;
        }
        total = setp_get_u16(&frame->data[1]);
        if ((total < SETP_HEADER_SIZE + SETP_CRC_SIZE) || (total > SETP_FRAME_MAX)) {
            return SETP_CAN_RX_FORMAT_ERROR;
        }
        (void)memcpy(state->buffer, &frame->data[3], SETP_CAN_FIRST_DATA);
        state->expected_length = total;
        state->received_length = SETP_CAN_FIRST_DATA;
        state->can_id = frame->id;
        state->deadline_ms = now_ms + SETP_CAN_REASSEMBLY_TIMEOUT_MS;
        state->next_sequence = 1U;
        state->active = 1U;
        return SETP_CAN_RX_NONE;
    }
    if (pci == SETP_CAN_PCI_CONSECUTIVE) {
        uint8_t sequence = (uint8_t)(frame->data[0] & SETP_CAN_PCI_VALUE_MASK);
        uint16_t remaining;
        uint8_t chunk;
        if (!state->active || (frame->id != state->can_id)
                || (sequence != state->next_sequence) || (frame->length < 2U)) {
            state->active = 0U;
            return SETP_CAN_RX_SEQUENCE_ERROR;
        }
        remaining = (uint16_t)(state->expected_length - state->received_length);
        chunk = (uint8_t)(frame->length - 1U);
        if (chunk > remaining) {
            state->active = 0U;
            return SETP_CAN_RX_FORMAT_ERROR;
        }
        (void)memcpy(&state->buffer[state->received_length], &frame->data[1], chunk);
        state->received_length = (uint16_t)(state->received_length + chunk);
        state->next_sequence = (uint8_t)((state->next_sequence + 1U)
                                         & SETP_CAN_PCI_VALUE_MASK);
        state->deadline_ms = now_ms + SETP_CAN_REASSEMBLY_TIMEOUT_MS;
        if (state->received_length == state->expected_length) {
            setp_can_id_t id;
            uint16_t source;
            uint16_t destination;
            uint8_t priority;
            if (!setp_can_id_unpack(state->can_id, &id)
                    || (state->expected_length < SETP_HEADER_SIZE + SETP_CRC_SIZE)
                    || (state->buffer[0] != SETP_SOF0)
                    || (state->buffer[1] != SETP_SOF1)) {
                state->active = 0U;
                return SETP_CAN_RX_FORMAT_ERROR;
            }
            source = setp_get_u16(&state->buffer[6]);
            destination = setp_get_u16(&state->buffer[8]);
            priority = (state->buffer[3] & SETP_FLAG_PRIORITY) != 0U ? 1U : 0U;
            if ((source != id.source) || (destination != id.destination)
                    || (priority != id.priority)) {
                state->active = 0U;
                return SETP_CAN_RX_FORMAT_ERROR;
            }
            out->data = state->buffer;
            out->length = state->expected_length;
            out->can_id = state->can_id;
            state->active = 0U;
            return SETP_CAN_RX_COMPLETE;
        }
        return SETP_CAN_RX_NONE;
    }
    if (pci == SETP_CAN_PCI_FLOW_CONTROL) {
        return SETP_CAN_RX_NONE;
    }
    return SETP_CAN_RX_FORMAT_ERROR;
}

bool setp_can_flow_control(setp_can_frame_t *out, uint32_t can_id,
                           uint8_t status, uint8_t block_size, uint8_t st_min_ms)
{
    if ((out == NULL) || (status > SETP_CAN_FC_OVERFLOW)
            || (st_min_ms > 0x7FU) || ((can_id & ~SETP_CAN_ID_MASK) != 0U)) {
        return false;
    }
    (void)memset(out, 0, sizeof(*out));
    out->id = can_id;
    out->length = 3U;
    out->data[0] = (uint8_t)(SETP_CAN_PCI_FLOW_CONTROL | status);
    out->data[1] = block_size;
    out->data[2] = st_min_ms;
    return true;
}