RTOS/middleware/safety/e2e_protection.c

/**
 * @file e2e_protection.c
 * @brief End-to-End Protection for safety-critical communication
 */

#include "kernel.h"
#include "can_driver.h"
#include <string.h>

/* E2E Protection Configuration */
#define E2E_MAX_PROFILES            8
#define E2E_MAX_DATA_LENGTH         64
#define E2E_CRC_POLYNOMIAL          0x2F  /* CRC-8 polynomial */

/* E2E Profile Types */
typedef enum {
    E2E_PROFILE_1 = 1,  /* CRC-8 */
    E2E_PROFILE_2 = 2,  /* CRC-16 */
    E2E_PROFILE_4 = 4,  /* CRC-32 */
    E2E_PROFILE_5 = 5,  /* Custom */
    E2E_PROFILE_7 = 7   /* Counter + CRC */
} E2eProfileType_t;

/* E2E Configuration */
typedef struct {
    E2eProfileType_t profile_type;
    uint16_t data_id;
    uint8_t data_length;
    uint8_t counter_offset;
    uint8_t crc_offset;
    uint8_t timeout_ms;
    uint32_t max_delta_counter;
} E2eConfig_t;

/* E2E State */
typedef struct {
    bool initialized;
    E2eConfig_t profiles[E2E_MAX_PROFILES];
    uint8_t profile_count;
    uint8_t counters[E2E_MAX_PROFILES];
    uint32_t last_receive_time[E2E_MAX_PROFILES];
    uint32_t error_counters[E2E_MAX_PROFILES];
    Mutex_t mutex;
} E2eState_t;

static E2eState_t e2e_state;

/* Initialize E2E Protection */
KernelStatus_t e2e_protection_init(void) {
    if (e2e_state.initialized) {
        return KERNEL_ERROR;
    }

    memset(&e2e_state, 0, sizeof(E2eState_t));
    mutex_create(&e2e_state.mutex, false);

    e2e_state.initialized = true;
    return KERNEL_OK;
}

/* Register E2E Profile */
KernelStatus_t e2e_register_profile(const E2eConfig_t* config) {
    if (!e2e_state.initialized || config == NULL) {
        return KERNEL_ERROR;
    }

    if (e2e_state.profile_count >= E2E_MAX_PROFILES) {
        return KERNEL_OUT_OF_MEMORY;
    }

    mutex_lock(&e2e_state.mutex, 100);

    e2e_state.profiles[e2e_state.profile_count] = *config;
    e2e_state.counters[e2e_state.profile_count] = 0;
    e2e_state.last_receive_time[e2e_state.profile_count] = 0;
    e2e_state.error_counters[e2e_state.profile_count] = 0;

    e2e_state.profile_count++;

    mutex_unlock(&e2e_state.mutex);

    return KERNEL_OK;
}

/* Protect Message (Add E2E header) */
KernelStatus_t e2e_protect_message(uint8_t profile_id, CanMessage_t* message) {
    if (!e2e_state.initialized || message == NULL) {
        return KERNEL_ERROR;
    }

    if (profile_id >= e2e_state.profile_count) {
        return KERNEL_INVALID_PARAMETER;
    }

    mutex_lock(&e2e_state.mutex, 100);

    E2eConfig_t* config = &e2e_state.profiles[profile_id];

    /* Add counter */
    message->data[config->counter_offset] = e2e_state.counters[profile_id];

    /* Calculate CRC */
    uint8_t crc = e2e_calculate_crc8(message->data, config->data_length);

    /* Add CRC */
    message->data[config->crc_offset] = crc;

    /* Increment counter */
    e2e_state.counters[profile_id]++;

    mutex_unlock(&e2e_state.mutex);

    return KERNEL_OK;
}

/* Check Message (Verify E2E header) */
KernelStatus_t e2e_check_message(uint8_t profile_id, const CanMessage_t* message,
                                 bool* is_valid) {
    if (!e2e_state.initialized || message == NULL || is_valid == NULL) {
        return KERNEL_ERROR;
    }

    if (profile_id >= e2e_state.profile_count) {
        return KERNEL_INVALID_PARAMETER;
    }

    mutex_lock(&e2e_state.mutex, 100);

    E2eConfig_t* config = &e2e_state.profiles[profile_id];
    *is_valid = false;

    /* Check counter */
    uint8_t received_counter = message->data[config->counter_offset];
    uint8_t expected_counter = e2e_state.counters[profile_id];

    uint32_t delta = (received_counter - expected_counter) & 0xFF;

    if (delta > config->max_delta_counter) {
        /* Counter error */
        e2e_state.error_counters[profile_id]++;
        mutex_unlock(&e2e_state.mutex);
        return KERNEL_ERROR;
    }

    /* Check CRC */
    uint8_t calculated_crc = e2e_calculate_crc8(message->data,
                                                 config->data_length);
    uint8_t received_crc = message->data[config->crc_offset];

    if (calculated_crc != received_crc) {
        /* CRC error */
        e2e_state.error_counters[profile_id]++;
        mutex_unlock(&e2e_state.mutex);
        return KERNEL_ERROR;
    }

    /* Update state */
    e2e_state.counters[profile_id] = received_counter;
    e2e_state.last_receive_time[profile_id] = kernel_get_tick_count();

    *is_valid = true;

    mutex_unlock(&e2e_state.mutex);

    return KERNEL_OK;
}

/* Calculate CRC-8 */
static uint8_t e2e_calculate_crc8(const uint8_t* data, uint8_t length) {
    uint8_t crc = 0xFF;  /* Initial value */

    for (uint8_t i = 0; i < length; i++) {
        crc ^= data[i];

        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x80) {
                crc = (crc << 1) ^ E2E_CRC_POLYNOMIAL;
            } else {
                crc <<= 1;
            }
        }
    }

    return crc;
}

/* Calculate CRC-16 */
static uint16_t e2e_calculate_crc16(const uint8_t* data, uint8_t length) {
    uint16_t crc = 0xFFFF;

    for (uint8_t i = 0; i < length; i++) {
        crc ^= (data[i] << 8);

        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x8000) {
                crc = (crc << 1) ^ 0x1021;
            } else {
                crc <<= 1;
            }
        }
    }

    return crc;
}

/* Calculate CRC-32 */
static uint32_t e2e_calculate_crc32(const uint8_t* data, uint8_t length) {
    uint32_t crc = 0xFFFFFFFF;

    for (uint8_t i = 0; i < length; i++) {
        crc ^= data[i];

        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 1) {
                crc = (crc >> 1) ^ 0xEDB88320;
            } else {
                crc >>= 1;
            }
        }
    }

    return ~crc;
}

/* Get E2E Error Counter */
uint32_t e2e_get_error_count(uint8_t profile_id) {
    if (profile_id >= e2e_state.profile_count) {
        return 0;
    }

    return e2e_state.error_counters[profile_id];
}

/* Reset E2E State */
KernelStatus_t e2e_reset(uint8_t profile_id) {
    if (!e2e_state.initialized) {
        return KERNEL_ERROR;
    }

    if (profile_id >= e2e_state.profile_count) {
        return KERNEL_INVALID_PARAMETER;
    }

    mutex_lock(&e2e_state.mutex, 100);

    e2e_state.counters[profile_id] = 0;
    e2e_state.last_receive_time[profile_id] = 0;
    e2e_state.error_counters[profile_id] = 0;

    mutex_unlock(&e2e_state.mutex);

    return KERNEL_OK;
}

/* E2E Main Function */
void e2e_protection_main_function(void) {
    if (!e2e_state.initialized) {
        return;
    }

    uint32_t current_time = kernel_get_tick_count();

    mutex_lock(&e2e_state.mutex, 100);

    /* Check timeouts */
    for (uint8_t i = 0; i < e2e_state.profile_count; i++) {
        E2eConfig_t* config = &e2e_state.profiles[i];

        if (e2e_state.last_receive_time[i] > 0) {
            if ((current_time - e2e_state.last_receive_time[i]) >
                config->timeout_ms) {
                /* Timeout error */
                e2e_state.error_counters[i]++;
                e2e_state.last_receive_time[i] = 0;
            }
        }
    }

    mutex_unlock(&e2e_state.mutex);
}