RTOS/middleware/can_stack/src/can_nm.c

/**
 * @file can_nm.c
 * @brief CAN Network Management implementation
 */

#include "can_nm.h"
#include <string.h>

/* CAN NM State */
typedef struct {
    bool initialized;
    CanNmConfig_t config;
    CanNmState_t current_state;
    CanNmNodeInfo_t nodes[CAN_NM_MAX_NODES];
    CanNmNetworkStateChangedCallback_t network_callback;
    CanNmNodeStateChangedCallback_t node_callback;
    CanNmBusSleepRequestCallback_t sleep_callback;
    CanNmWakeupIndicationCallback_t wakeup_callback;
    uint32_t state_timer;
    uint32_t repeat_message_timer;
    uint8_t repeat_message_count;
    Mutex_t mutex;
} CanNmState_t;

static CanNmState_t can_nm_state;

/* Initialize CAN NM */
KernelStatus_t can_nm_init(const CanNmConfig_t* config) {
    if (config == NULL || can_nm_state.initialized) {
        return KERNEL_ERROR;
    }

    /* Copy configuration */
    memcpy(&can_nm_state.config, config, sizeof(CanNmConfig_t));

    /* Initialize state */
    can_nm_state.current_state = CAN_NM_OFFLINE;
    can_nm_state.state_timer = 0;
    can_nm_state.repeat_message_timer = 0;
    can_nm_state.repeat_message_count = 0;

    /* Initialize nodes */
    for (int i = 0; i < CAN_NM_MAX_NODES; i++) {
        can_nm_state.nodes[i].node_id = i;
        can_nm_state.nodes[i].is_present = false;
        can_nm_state.nodes[i].is_awake = false;
        can_nm_state.nodes[i].last_message_time = 0;
        can_nm_state.nodes[i].state = CAN_NM_OFFLINE;
    }

    /* Create mutex */
    mutex_create(&can_nm_state.mutex, false);

    can_nm_state.initialized = true;
    return KERNEL_OK;
}

/* Start CAN NM */
KernelStatus_t can_nm_start(void) {
    if (!can_nm_state.initialized) {
        return KERNEL_ERROR;
    }

    mutex_lock(&can_nm_state.mutex, 100);

    can_nm_state.current_state = CAN_NM_REPEAT_MESSAGE;
    can_nm_state.repeat_message_count = 0;
    can_nm_state.repeat_message_timer = kernel_get_tick_count();

    mutex_unlock(&can_nm_state.mutex);

    /* Send first alive message */
    can_nm_send_message(CAN_NM_MSG_ALIVE);

    return KERNEL_OK;
}

/* Stop CAN NM */
KernelStatus_t can_nm_stop(void) {
    if (!can_nm_state.initialized) {
        return KERNEL_ERROR;
    }

    mutex_lock(&can_nm_state.mutex, 100);
    can_nm_state.current_state = CAN_NM_OFFLINE;
    mutex_unlock(&can_nm_state.mutex);

    return KERNEL_OK;
}

/* Request Bus Sleep */
KernelStatus_t can_nm_request_bus_sleep(void) {
    if (!can_nm_state.initialized) {
        return KERNEL_ERROR;
    }

    mutex_lock(&can_nm_state.mutex, 100);

    /* Send sleep indication */
    can_nm_send_message(CAN_NM_MSG_SLEEP_ACK);

    can_nm_state.current_state = CAN_NM_READY_SLEEP;

    mutex_unlock(&can_nm_state.mutex);

    return KERNEL_OK;
}

/* Network Release */
KernelStatus_t can_nm_network_release(void) {
    if (!can_nm_state.initialized) {
        return KERNEL_ERROR;
    }

    mutex_lock(&can_nm_state.mutex, 100);

    /* Check if all nodes are ready to sleep */
    bool all_ready = true;
    for (int i = 0; i < CAN_NM_MAX_NODES; i++) {
        if (can_nm_state.nodes[i].is_present &&
            can_nm_state.nodes[i].state != CAN_NM_READY_SLEEP) {
            all_ready = false;
            break;
        }
    }

    if (all_ready || can_nm_state.config.is_coordinator) {
        can_nm_state.current_state = CAN_NM_PREPARE_BUS_SLEEP;

        /* Send sleep confirmation */
        can_nm_send_message(CAN_NM_MSG_SLEEP_CONF);

        /* Call sleep callback */
        if (can_nm_state.sleep_callback != NULL) {
            can_nm_state.sleep_callback();
        }

        can_nm_state.current_state = CAN_NM_BUS_SLEEP;

        /* Call network state changed callback */
        if (can_nm_state.network_callback != NULL) {
            can_nm_state.network_callback(CAN_NM_BUS_SLEEP);
        }
    }

    mutex_unlock(&can_nm_state.mutex);

    return KERNEL_OK;
}

/* Network Request */
KernelStatus_t can_nm_network_request(void) {
    if (!can_nm_state.initialized) {
        return KERNEL_ERROR;
    }

    mutex_lock(&can_nm_state.mutex, 100);

    if (can_nm_state.current_state == CAN_NM_BUS_SLEEP) {
        can_nm_state.current_state = CAN_NM_REPEAT_MESSAGE;
        can_nm_state.repeat_message_count = 0;

        /* Send wakeup indication */
        can_nm_send_message(CAN_NM_MSG_ALIVE);

        /* Call wakeup callback */
        if (can_nm_state.wakeup_callback != NULL) {
            can_nm_state.wakeup_callback();
        }
    }

    mutex_unlock(&can_nm_state.mutex);

    return KERNEL_OK;
}

/* Process Received NM Message */
void can_nm_process_rx_message(const CanMessage_t* message) {
    if (!can_nm_state.initialized || message == NULL) {
        return;
    }

    /* Check if NM message */
    if (message->id.id < CAN_NM_MESSAGE_ID_BASE ||
        message->id.id >= CAN_NM_MESSAGE_ID_BASE + CAN_NM_MAX_NODES) {
        return;
    }

    /* Extract node ID and message type */
    uint8_t node_id = message->id.id - CAN_NM_MESSAGE_ID_BASE;
    uint8_t message_type = message->data[0] & 0x0F;

    mutex_lock(&can_nm_state.mutex, 100);

    /* Update node information */
    can_nm_state.nodes[node_id].is_present = true;
    can_nm_state.nodes[node_id].last_message_time = kernel_get_tick_count();

    /* Process message type */
    switch (message_type) {
        case CAN_NM_MSG_ALIVE:
            can_nm_state.nodes[node_id].is_awake = true;
            can_nm_state.nodes[node_id].state = CAN_NM_NORMAL_OPERATION;

            if (can_nm_state.node_callback != NULL) {
                can_nm_state.node_callback(node_id, true);
            }
            break;

        case CAN_NM_MSG_RING:
            /* Ring message - keep awake */
            can_nm_state.nodes[node_id].is_awake = true;
            break;

        case CAN_NM_MSG_SLEEP_ACK:
            can_nm_state.nodes[node_id].state = CAN_NM_READY_SLEEP;
            break;

        case CAN_NM_MSG_SLEEP_CONF:
            can_nm_state.nodes[node_id].state = CAN_NM_BUS_SLEEP;
            can_nm_state.nodes[node_id].is_awake = false;

            if (can_nm_state.node_callback != NULL) {
                can_nm_state.node_callback(node_id, false);
            }
            break;

        default:
            break;
    }

    mutex_unlock(&can_nm_state.mutex);
}

/* CAN NM Main Function */
void can_nm_main_function(void) {
    if (!can_nm_state.initialized) {
        return;
    }

    uint32_t current_time = kernel_get_tick_count();

    mutex_lock(&can_nm_state.mutex, 100);

    switch (can_nm_state.current_state) {
        case CAN_NM_REPEAT_MESSAGE:
            /* Send repeat messages */
            if ((current_time - can_nm_state.repeat_message_timer) >=
                can_nm_state.config.repeat_message_time_ms) {
                can_nm_send_message(CAN_NM_MSG_RING);
                can_nm_state.repeat_message_timer = current_time;
                can_nm_state.repeat_message_count++;

                /* Transition to normal operation after repeat messages */
                if (can_nm_state.repeat_message_count >= 3) {
                    can_nm_state.current_state = CAN_NM_NORMAL_OPERATION;

                    if (can_nm_state.network_callback != NULL) {
                        can_nm_state.network_callback(CAN_NM_NORMAL_OPERATION);
                    }
                }
            }
            break;

        case CAN_NM_NORMAL_OPERATION:
            /* Send periodic ring messages */
            if ((current_time - can_nm_state.state_timer) >=
                can_nm_state.config.timeout_ms / 2) {
                can_nm_send_message(CAN_NM_MSG_RING);
                can_nm_state.state_timer = current_time;
            }
            break;

        case CAN_NM_READY_SLEEP:
            /* Check if still ready to sleep */
            if ((current_time - can_nm_state.state_timer) >=
                can_nm_state.config.sleep_ack_timeout_ms) {
                can_nm_state.current_state = CAN_NM_NORMAL_OPERATION;
            }
            break;

        case CAN_NM_PREPARE_BUS_SLEEP:
            /* Transition to bus sleep */
            if ((current_time - can_nm_state.state_timer) >=
                can_nm_state.config.timeout_ms) {
                can_nm_state.current_state = CAN_NM_BUS_SLEEP;

                if (can_nm_state.network_callback != NULL) {
                    can_nm_state.network_callback(CAN_NM_BUS_SLEEP);
                }
            }
            break;

        default:
            break;
    }

    /* Check node timeouts */
    for (int i = 0; i < CAN_NM_MAX_NODES; i++) {
        if (can_nm_state.nodes[i].is_present &&
            can_nm_state.nodes[i].is_awake) {
            if ((current_time - can_nm_state.nodes[i].last_message_time) >
                can_nm_state.config.timeout_ms) {
                /* Node timeout */
                can_nm_state.nodes[i].is_present = false;
                can_nm_state.nodes[i].is_awake = false;

                if (can_nm_state.node_callback != NULL) {
                    can_nm_state.node_callback(i, false);
                }
            }
        }
    }

    mutex_unlock(&can_nm_state.mutex);
}

/* Send NM Message (internal) */
static void can_nm_send_message(uint8_t message_type) {
    CanMessage_t message;
    message.id.id = can_nm_state.config.message_id +
                    can_nm_state.config.node_id;
    message.id.is_extended = false;
    message.length = 8;
    message.data[0] = message_type;
    message.data[1] = can_nm_state.config.node_id;
    message.data[2] = can_nm_state.current_state;

    can_send_message(&message, 100);
}