Add full automotive RTOS project
Add kernel (Cortex-M0/M3/M4, Tricore, S32K, RISC-V ports), drivers, middleware (CAN stack, diagnostics, safety), applications, board support, build/test tooling, and documentation.
This commit is contained in:
root
@@ -0,0 +1,330 @@
|
||||
/**
|
||||
* @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);
|
||||
}
|
||||
@@ -0,0 +1,367 @@
|
||||
/**
|
||||
* @file can_tp.c
|
||||
* @brief CAN Transport Protocol implementation
|
||||
*/
|
||||
|
||||
#include "can_tp.h"
|
||||
#include <string.h>
|
||||
|
||||
/* CAN TP State */
|
||||
typedef struct {
|
||||
bool initialized;
|
||||
CanTpConfig_t config;
|
||||
CanTpConnection_t connections[CAN_TP_MAX_CONNECTIONS];
|
||||
CanTpMessageReceivedCallback_t rx_callback;
|
||||
CanTpMessageSentCallback_t tx_callback;
|
||||
CanTpErrorCallback_t error_callback;
|
||||
Mutex_t global_mutex;
|
||||
} CanTpState_t;
|
||||
|
||||
static CanTpState_t can_tp_state;
|
||||
|
||||
/* Initialize CAN TP */
|
||||
KernelStatus_t can_tp_init(const CanTpConfig_t* config) {
|
||||
if (config == NULL || can_tp_state.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
/* Copy configuration */
|
||||
memcpy(&can_tp_state.config, config, sizeof(CanTpConfig_t));
|
||||
|
||||
/* Initialize connections */
|
||||
for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
|
||||
CanTpConnection_t* conn = &can_tp_state.connections[i];
|
||||
conn->connection_id = i;
|
||||
conn->state = CAN_TP_IDLE;
|
||||
conn->stmin = config->stmin;
|
||||
conn->block_size = config->block_size;
|
||||
|
||||
semaphore_create(&conn->flow_control_semaphore, SEMAPHORE_BINARY, 0, 1);
|
||||
semaphore_create(&conn->complete_semaphore, SEMAPHORE_BINARY, 0, 1);
|
||||
mutex_create(&conn->connection_mutex, false);
|
||||
}
|
||||
|
||||
/* Create global mutex */
|
||||
mutex_create(&can_tp_state.global_mutex, false);
|
||||
|
||||
can_tp_state.initialized = true;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Send CAN TP Message */
|
||||
KernelStatus_t can_tp_send_message(const CanTpMessage_t* message,
|
||||
uint32_t timeout_ms) {
|
||||
if (!can_tp_state.initialized || message == NULL || message->data == NULL) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
/* Find free connection */
|
||||
CanTpConnection_t* conn = NULL;
|
||||
for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
|
||||
if (can_tp_state.connections[i].state == CAN_TP_IDLE) {
|
||||
conn = &can_tp_state.connections[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (conn == NULL) {
|
||||
return KERNEL_RESOURCE_BUSY;
|
||||
}
|
||||
|
||||
/* Lock connection */
|
||||
if (mutex_lock(&conn->connection_mutex, timeout_ms) != KERNEL_OK) {
|
||||
return KERNEL_TIMEOUT;
|
||||
}
|
||||
|
||||
/* Set up connection */
|
||||
conn->current_message = *message;
|
||||
conn->current_index = 0;
|
||||
conn->sequence_number = 0;
|
||||
conn->block_counter = 0;
|
||||
conn->is_sender = true;
|
||||
conn->state = CAN_TP_SEND_IN_PROGRESS;
|
||||
|
||||
/* Send single frame or first frame */
|
||||
CanMessage_t can_message;
|
||||
memset(&can_message, 0, sizeof(CanMessage_t));
|
||||
|
||||
if (message->length <= 7) {
|
||||
/* Single Frame */
|
||||
can_message.id.id = message->message_id;
|
||||
can_message.id.is_extended = true;
|
||||
can_message.length = message->length + 1;
|
||||
can_message.data[0] = (CAN_TP_FRAME_SINGLE << 4) | message->length;
|
||||
memcpy(&can_message.data[1], message->data, message->length);
|
||||
|
||||
/* Send message */
|
||||
if (can_send_message(&can_message, timeout_ms) != KERNEL_OK) {
|
||||
conn->state = CAN_TP_ERROR;
|
||||
mutex_unlock(&conn->connection_mutex);
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
conn->state = CAN_TP_IDLE;
|
||||
mutex_unlock(&conn->connection_mutex);
|
||||
|
||||
/* Signal completion */
|
||||
if (can_tp_state.tx_callback != NULL) {
|
||||
can_tp_state.tx_callback(conn->connection_id, true);
|
||||
}
|
||||
|
||||
return KERNEL_OK;
|
||||
} else {
|
||||
/* First Frame */
|
||||
can_message.id.id = message->message_id;
|
||||
can_message.id.is_extended = true;
|
||||
can_message.length = 8;
|
||||
can_message.data[0] = (CAN_TP_FRAME_FIRST << 4) | ((message->length >> 8) & 0x0F);
|
||||
can_message.data[1] = message->length & 0xFF;
|
||||
memcpy(&can_message.data[2], &message->data[0], 6);
|
||||
|
||||
/* Send first frame */
|
||||
if (can_send_message(&can_message, timeout_ms) != KERNEL_OK) {
|
||||
conn->state = CAN_TP_ERROR;
|
||||
mutex_unlock(&conn->connection_mutex);
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
conn->current_index = 6;
|
||||
conn->state = CAN_TP_WAIT_FLOW_CONTROL;
|
||||
}
|
||||
|
||||
/* Wait for flow control */
|
||||
if (semaphore_take(&conn->flow_control_semaphore, timeout_ms) != KERNEL_OK) {
|
||||
conn->state = CAN_TP_TIMEOUT;
|
||||
mutex_unlock(&conn->connection_mutex);
|
||||
return KERNEL_TIMEOUT;
|
||||
}
|
||||
|
||||
/* Send consecutive frames */
|
||||
while (conn->current_index < message->length) {
|
||||
/* Check block size */
|
||||
if (conn->block_counter >= conn->block_size && conn->block_size > 0) {
|
||||
/* Wait for another flow control */
|
||||
conn->block_counter = 0;
|
||||
if (semaphore_take(&conn->flow_control_semaphore, timeout_ms) != KERNEL_OK) {
|
||||
conn->state = CAN_TP_TIMEOUT;
|
||||
mutex_unlock(&conn->connection_mutex);
|
||||
return KERNEL_TIMEOUT;
|
||||
}
|
||||
}
|
||||
|
||||
/* Send consecutive frame */
|
||||
CanMessage_t consecutive_frame;
|
||||
consecutive_frame.id.id = message->message_id;
|
||||
consecutive_frame.id.is_extended = true;
|
||||
|
||||
uint16_t remaining = message->length - conn->current_index;
|
||||
uint8_t frame_length = (remaining > 7) ? 7 : remaining;
|
||||
|
||||
consecutive_frame.length = frame_length + 1;
|
||||
consecutive_frame.data[0] = (CAN_TP_FRAME_CONSECUTIVE << 4) |
|
||||
(conn->sequence_number & 0x0F);
|
||||
memcpy(&consecutive_frame.data[1],
|
||||
&message->data[conn->current_index], frame_length);
|
||||
|
||||
/* Send consecutive frame */
|
||||
if (can_send_message(&consecutive_frame, timeout_ms) != KERNEL_OK) {
|
||||
conn->state = CAN_TP_ERROR;
|
||||
mutex_unlock(&conn->connection_mutex);
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
conn->current_index += frame_length;
|
||||
conn->sequence_number = (conn->sequence_number + 1) & 0x0F;
|
||||
conn->block_counter++;
|
||||
|
||||
/* Wait for STMin */
|
||||
if (conn->stmin > 0) {
|
||||
kernel_delay(conn->stmin);
|
||||
}
|
||||
}
|
||||
|
||||
/* Message sent successfully */
|
||||
conn->state = CAN_TP_IDLE;
|
||||
mutex_unlock(&conn->connection_mutex);
|
||||
|
||||
/* Signal completion */
|
||||
if (can_tp_state.tx_callback != NULL) {
|
||||
can_tp_state.tx_callback(conn->connection_id, true);
|
||||
}
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Process Received CAN Message */
|
||||
void can_tp_process_rx_indication(const CanMessage_t* can_message) {
|
||||
if (!can_tp_state.initialized || can_message == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Parse frame type */
|
||||
uint8_t frame_type = (can_message->data[0] >> 4) & 0x0F;
|
||||
|
||||
switch (frame_type) {
|
||||
case CAN_TP_FRAME_SINGLE: {
|
||||
/* Single frame - complete message */
|
||||
uint8_t length = can_message->data[0] & 0x0F;
|
||||
|
||||
CanTpMessage_t tp_message;
|
||||
tp_message.message_id = can_message->id.id;
|
||||
tp_message.length = length;
|
||||
tp_message.data = (uint8_t*)&can_message->data[1];
|
||||
|
||||
/* Call callback */
|
||||
if (can_tp_state.rx_callback != NULL) {
|
||||
can_tp_state.rx_callback(&tp_message);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case CAN_TP_FRAME_FIRST: {
|
||||
/* First frame - start receiving multi-frame message */
|
||||
uint16_t total_length = ((can_message->data[0] & 0x0F) << 8) |
|
||||
can_message->data[1];
|
||||
|
||||
/* Find connection for receiving */
|
||||
for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
|
||||
CanTpConnection_t* conn = &can_tp_state.connections[i];
|
||||
|
||||
if (conn->state == CAN_TP_IDLE) {
|
||||
conn->state = CAN_TP_RECEIVE_IN_PROGRESS;
|
||||
conn->is_sender = false;
|
||||
conn->current_message.message_id = can_message->id.id;
|
||||
conn->current_message.length = total_length;
|
||||
conn->current_message.data = (uint8_t*)malloc(total_length);
|
||||
conn->current_index = 0;
|
||||
conn->sequence_number = 0;
|
||||
conn->block_counter = 0;
|
||||
|
||||
/* Copy first 6 bytes */
|
||||
memcpy(conn->current_message.data, &can_message->data[2], 6);
|
||||
conn->current_index = 6;
|
||||
|
||||
/* Send flow control */
|
||||
CanMessage_t fc_message;
|
||||
fc_message.id.id = can_message->id.id;
|
||||
fc_message.id.is_extended = true;
|
||||
fc_message.length = 8;
|
||||
fc_message.data[0] = (CAN_TP_FRAME_FLOW_CONTROL << 4) |
|
||||
CAN_TP_FC_CONTINUE;
|
||||
fc_message.data[1] = conn->block_size;
|
||||
fc_message.data[2] = conn->stmin;
|
||||
|
||||
can_send_message(&fc_message, CAN_TP_DEFAULT_TIMEOUT_MS);
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case CAN_TP_FRAME_CONSECUTIVE: {
|
||||
/* Consecutive frame - part of multi-frame message */
|
||||
uint8_t sequence_number = can_message->data[0] & 0x0F;
|
||||
|
||||
/* Find active receiving connection */
|
||||
for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
|
||||
CanTpConnection_t* conn = &can_tp_state.connections[i];
|
||||
|
||||
if (conn->state == CAN_TP_RECEIVE_IN_PROGRESS && !conn->is_sender) {
|
||||
if (sequence_number == conn->sequence_number) {
|
||||
/* Copy data */
|
||||
uint8_t frame_length = can_message->length - 1;
|
||||
memcpy(&conn->current_message.data[conn->current_index],
|
||||
&can_message->data[1], frame_length);
|
||||
conn->current_index += frame_length;
|
||||
conn->sequence_number = (conn->sequence_number + 1) & 0x0F;
|
||||
conn->block_counter++;
|
||||
|
||||
/* Check if complete */
|
||||
if (conn->current_index >= conn->current_message.length) {
|
||||
/* Message complete */
|
||||
if (can_tp_state.rx_callback != NULL) {
|
||||
can_tp_state.rx_callback(&conn->current_message);
|
||||
}
|
||||
|
||||
/* Free data */
|
||||
free(conn->current_message.data);
|
||||
conn->state = CAN_TP_IDLE;
|
||||
} else if (conn->block_counter >= conn->block_size) {
|
||||
/* Send another flow control */
|
||||
CanMessage_t fc_message;
|
||||
fc_message.id.id = conn->current_message.message_id;
|
||||
fc_message.id.is_extended = true;
|
||||
fc_message.length = 8;
|
||||
fc_message.data[0] = (CAN_TP_FRAME_FLOW_CONTROL << 4) |
|
||||
CAN_TP_FC_CONTINUE;
|
||||
fc_message.data[1] = conn->block_size;
|
||||
fc_message.data[2] = conn->stmin;
|
||||
|
||||
can_send_message(&fc_message, CAN_TP_DEFAULT_TIMEOUT_MS);
|
||||
conn->block_counter = 0;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case CAN_TP_FRAME_FLOW_CONTROL: {
|
||||
/* Flow control - update sending connection */
|
||||
uint8_t flow_status = can_message->data[0] & 0x0F;
|
||||
|
||||
for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
|
||||
CanTpConnection_t* conn = &can_tp_state.connections[i];
|
||||
|
||||
if (conn->state == CAN_TP_WAIT_FLOW_CONTROL && conn->is_sender) {
|
||||
if (flow_status == CAN_TP_FC_CONTINUE) {
|
||||
conn->block_size = can_message->data[1];
|
||||
conn->stmin = can_message->data[2];
|
||||
conn->block_counter = 0;
|
||||
|
||||
/* Signal flow control received */
|
||||
semaphore_give(&conn->flow_control_semaphore);
|
||||
} else if (flow_status == CAN_TP_FC_OVERFLOW) {
|
||||
conn->state = CAN_TP_ERROR;
|
||||
if (can_tp_state.error_callback != NULL) {
|
||||
can_tp_state.error_callback(conn->connection_id,
|
||||
CAN_TP_FC_OVERFLOW);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* CAN TP Main Function */
|
||||
void can_tp_main_function(void) {
|
||||
if (!can_tp_state.initialized) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Check timeouts */
|
||||
uint32_t current_time = kernel_get_tick_count();
|
||||
|
||||
for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
|
||||
CanTpConnection_t* conn = &can_tp_state.connections[i];
|
||||
|
||||
if (conn->state != CAN_TP_IDLE && conn->state != CAN_TP_ERROR) {
|
||||
if ((current_time - conn->timeout_timer) > CAN_TP_DEFAULT_TIMEOUT_MS) {
|
||||
/* Timeout occurred */
|
||||
conn->state = CAN_TP_TIMEOUT;
|
||||
|
||||
if (conn->current_message.data != NULL && !conn->is_sender) {
|
||||
free(conn->current_message.data);
|
||||
}
|
||||
|
||||
if (can_tp_state.error_callback != NULL) {
|
||||
can_tp_state.error_callback(conn->connection_id, CAN_TP_TIMEOUT);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,357 @@
|
||||
/**
|
||||
* @file uds.c
|
||||
* @brief Unified Diagnostic Services implementation
|
||||
*/
|
||||
|
||||
#include "uds.h"
|
||||
#include "dtc_manager.h"
|
||||
#include <string.h>
|
||||
|
||||
/* UDS State */
|
||||
typedef struct {
|
||||
bool initialized;
|
||||
UdsConfig_t config;
|
||||
UdsServiceCallback_t service_callbacks[0x100];
|
||||
UdsSecurityAccessCallback_t security_callback;
|
||||
UdsSessionChangedCallback_t session_callback;
|
||||
uint32_t security_attempt_count;
|
||||
uint32_t security_delay_timer;
|
||||
Mutex_t mutex;
|
||||
} UdsState_t;
|
||||
|
||||
static UdsState_t uds_state;
|
||||
|
||||
/* Initialize UDS */
|
||||
KernelStatus_t uds_init(const UdsConfig_t* config) {
|
||||
if (config == NULL || uds_state.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
/* Copy configuration */
|
||||
memcpy(&uds_state.config, config, sizeof(UdsConfig_t));
|
||||
|
||||
/* Initialize state */
|
||||
uds_state.security_attempt_count = 0;
|
||||
uds_state.security_delay_timer = 0;
|
||||
|
||||
/* Clear service callbacks */
|
||||
memset(uds_state.service_callbacks, 0, sizeof(uds_state.service_callbacks));
|
||||
|
||||
/* Create mutex */
|
||||
mutex_create(&uds_state.mutex, false);
|
||||
|
||||
uds_state.initialized = true;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Process UDS Message */
|
||||
KernelStatus_t uds_process_message(const UdsMessage_t* request,
|
||||
UdsMessage_t* response) {
|
||||
if (!uds_state.initialized || request == NULL || response == NULL) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&uds_state.mutex, uds_state.config.timeout_ms);
|
||||
|
||||
/* Initialize response */
|
||||
response->service_id = request->service_id + 0x40; /* Positive response */
|
||||
response->length = 0;
|
||||
|
||||
/* Check if service is supported */
|
||||
if (uds_state.service_callbacks[request->service_id] == NULL) {
|
||||
response->service_id = 0x7F; /* Negative response */
|
||||
response->data[0] = request->service_id;
|
||||
response->data[1] = UDS_RESPONSE_SERVICE_NOT_SUPPORTED;
|
||||
response->length = 2;
|
||||
mutex_unlock(&uds_state.mutex);
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Check security access */
|
||||
if (request->service_id == UDS_SID_SECURITY_ACCESS) {
|
||||
/* Handle security access separately */
|
||||
if (uds_process_security_access(request, response) != KERNEL_OK) {
|
||||
mutex_unlock(&uds_state.mutex);
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
} else if (request->service_id == UDS_SID_DIAGNOSTIC_SESSION_CONTROL) {
|
||||
/* Handle session control */
|
||||
if (uds_process_session_control(request, response) != KERNEL_OK) {
|
||||
mutex_unlock(&uds_state.mutex);
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
} else if (request->service_id == UDS_SID_READ_DTC_INFORMATION) {
|
||||
/* Handle DTC reading */
|
||||
if (uds_process_read_dtc(request, response) != KERNEL_OK) {
|
||||
mutex_unlock(&uds_state.mutex);
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
} else if (request->service_id == UDS_SID_CLEAR_DTC_INFORMATION) {
|
||||
/* Handle DTC clearing */
|
||||
if (uds_process_clear_dtc(request, response) != KERNEL_OK) {
|
||||
mutex_unlock(&uds_state.mutex);
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
} else if (request->service_id == UDS_SID_TESTER_PRESENT) {
|
||||
/* Handle tester present */
|
||||
response->data[0] = request->sub_function;
|
||||
response->length = 1;
|
||||
} else {
|
||||
/* Call registered service callback */
|
||||
uds_state.service_callbacks[request->service_id](request, response);
|
||||
}
|
||||
|
||||
mutex_unlock(&uds_state.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Process Security Access */
|
||||
static KernelStatus_t uds_process_security_access(const UdsMessage_t* request,
|
||||
UdsMessage_t* response) {
|
||||
uint8_t security_level = request->sub_function;
|
||||
|
||||
/* Check if delay timer is active */
|
||||
if (uds_state.security_delay_timer > 0) {
|
||||
uint32_t current_time = kernel_get_tick_count();
|
||||
if (current_time < uds_state.security_delay_timer) {
|
||||
response->service_id = 0x7F;
|
||||
response->data[0] = request->service_id;
|
||||
response->data[1] = UDS_RESPONSE_REQUIRED_TIME_DELAY_NOT_EXPIRED;
|
||||
response->length = 2;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
}
|
||||
|
||||
if (security_level % 2 == 1) {
|
||||
/* Request seed */
|
||||
if (uds_state.security_callback != NULL) {
|
||||
uint8_t seed[16];
|
||||
uint8_t seed_length = 0;
|
||||
|
||||
/* Generate seed */
|
||||
for (int i = 0; i < 16; i++) {
|
||||
seed[i] = rand() & 0xFF;
|
||||
seed_length++;
|
||||
}
|
||||
|
||||
/* Set response */
|
||||
response->data[0] = security_level;
|
||||
memcpy(&response->data[1], seed, seed_length);
|
||||
response->length = seed_length + 1;
|
||||
}
|
||||
} else {
|
||||
/* Send key */
|
||||
if (uds_state.security_callback != NULL) {
|
||||
bool access_granted = false;
|
||||
uint8_t key[16];
|
||||
uint8_t key_length = request->length - 1;
|
||||
|
||||
memcpy(key, &request->data[1], key_length);
|
||||
|
||||
/* Verify key */
|
||||
uds_state.security_callback(security_level - 1, NULL, key,
|
||||
key_length, &access_granted);
|
||||
|
||||
if (access_granted) {
|
||||
uds_state.config.current_security_level = security_level - 1;
|
||||
uds_state.security_attempt_count = 0;
|
||||
|
||||
response->data[0] = security_level;
|
||||
response->length = 1;
|
||||
} else {
|
||||
uds_state.security_attempt_count++;
|
||||
|
||||
if (uds_state.security_attempt_count >= 3) {
|
||||
/* Set delay timer */
|
||||
uds_state.security_delay_timer = kernel_get_tick_count() + 10000;
|
||||
uds_state.security_attempt_count = 0;
|
||||
|
||||
response->service_id = 0x7F;
|
||||
response->data[0] = request->service_id;
|
||||
response->data[1] = UDS_RESPONSE_EXCEED_NUMBER_OF_ATTEMPTS;
|
||||
response->length = 2;
|
||||
} else {
|
||||
response->service_id = 0x7F;
|
||||
response->data[0] = request->service_id;
|
||||
response->data[1] = UDS_RESPONSE_INVALID_KEY;
|
||||
response->length = 2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Process Session Control */
|
||||
static KernelStatus_t uds_process_session_control(const UdsMessage_t* request,
|
||||
UdsMessage_t* response) {
|
||||
UdsSessionType_t new_session = (UdsSessionType_t)request->sub_function;
|
||||
|
||||
/* Check if session is supported */
|
||||
if (new_session != UDS_SESSION_DEFAULT &&
|
||||
new_session != UDS_SESSION_PROGRAMMING &&
|
||||
new_session != UDS_SESSION_EXTENDED &&
|
||||
new_session != UDS_SESSION_SAFETY_SYSTEM) {
|
||||
response->service_id = 0x7F;
|
||||
response->data[0] = request->service_id;
|
||||
response->data[1] = UDS_RESPONSE_SUBFUNCTION_NOT_SUPPORTED;
|
||||
response->length = 2;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Store old session */
|
||||
UdsSessionType_t old_session = uds_state.config.current_session;
|
||||
|
||||
/* Update session */
|
||||
uds_state.config.current_session = new_session;
|
||||
|
||||
/* Reset security level for non-default sessions */
|
||||
if (new_session != UDS_SESSION_DEFAULT) {
|
||||
uds_state.config.current_security_level = UDS_SECURITY_LOCKED;
|
||||
}
|
||||
|
||||
/* Call session changed callback */
|
||||
if (uds_state.session_callback != NULL) {
|
||||
uds_state.session_callback(old_session, new_session);
|
||||
}
|
||||
|
||||
/* Set response */
|
||||
response->data[0] = request->sub_function;
|
||||
response->data[1] = 0x00; /* P2 server max high byte */
|
||||
response->data[2] = 0x32; /* P2 server max low byte (50ms) */
|
||||
response->data[3] = 0x01; /* P2* server max high byte */
|
||||
response->data[4] = 0xF4; /* P2* server max low byte (500ms) */
|
||||
response->length = 5;
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Process Read DTC */
|
||||
static KernelStatus_t uds_process_read_dtc(const UdsMessage_t* request,
|
||||
UdsMessage_t* response) {
|
||||
uint8_t sub_function = request->sub_function;
|
||||
|
||||
switch (sub_function) {
|
||||
case 0x01: /* Report number of DTC by status mask */
|
||||
case 0x02: /* Report DTC by status mask */
|
||||
case 0x04: /* Report DTC snapshot identification */
|
||||
case 0x06: /* Report DTC extended data */
|
||||
case 0x0A: /* Report supported DTCs */
|
||||
break;
|
||||
default:
|
||||
response->service_id = 0x7F;
|
||||
response->data[0] = request->service_id;
|
||||
response->data[1] = UDS_RESPONSE_SUBFUNCTION_NOT_SUPPORTED;
|
||||
response->length = 2;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Get DTC information */
|
||||
response->data[0] = 0x01; /* Availability mask */
|
||||
response->data[1] = 0x00; /* DTC format identifier */
|
||||
|
||||
/* Get number of DTCs */
|
||||
uint16_t dtc_count = dtc_manager_get_count();
|
||||
response->data[2] = (dtc_count >> 8) & 0xFF;
|
||||
response->data[3] = dtc_count & 0xFF;
|
||||
|
||||
response->length = 4;
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Process Clear DTC */
|
||||
static KernelStatus_t uds_process_clear_dtc(const UdsMessage_t* request,
|
||||
UdsMessage_t* response) {
|
||||
/* Clear all DTCs */
|
||||
dtc_manager_clear_all();
|
||||
|
||||
response->length = 0;
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Register Service Callback */
|
||||
KernelStatus_t uds_register_service_callback(UdsServiceId_t service_id,
|
||||
UdsServiceCallback_t callback) {
|
||||
if (!uds_state.initialized || callback == NULL) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
uds_state.service_callbacks[service_id] = callback;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Register Security Callback */
|
||||
KernelStatus_t uds_register_security_callback(UdsSecurityAccessCallback_t callback) {
|
||||
if (!uds_state.initialized || callback == NULL) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
uds_state.security_callback = callback;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Register Session Callback */
|
||||
KernelStatus_t uds_register_session_callback(UdsSessionChangedCallback_t callback) {
|
||||
if (!uds_state.initialized || callback == NULL) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
uds_state.session_callback = callback;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Set Session */
|
||||
KernelStatus_t uds_set_session(UdsSessionType_t session) {
|
||||
if (!uds_state.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
uds_state.config.current_session = session;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Get Session */
|
||||
UdsSessionType_t uds_get_session(void) {
|
||||
if (!uds_state.initialized) {
|
||||
return UDS_SESSION_DEFAULT;
|
||||
}
|
||||
|
||||
return uds_state.config.current_session;
|
||||
}
|
||||
|
||||
/* Set Security Level */
|
||||
KernelStatus_t uds_set_security_level(UdsSecurityLevel_t level) {
|
||||
if (!uds_state.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
uds_state.config.current_security_level = level;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Get Security Level */
|
||||
UdsSecurityLevel_t uds_get_security_level(void) {
|
||||
if (!uds_state.initialized) {
|
||||
return UDS_SECURITY_LOCKED;
|
||||
}
|
||||
|
||||
return uds_state.config.current_security_level;
|
||||
}
|
||||
|
||||
/* UDS Main Function */
|
||||
void uds_main_function(void) {
|
||||
if (!uds_state.initialized) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Check security delay timer */
|
||||
if (uds_state.security_delay_timer > 0) {
|
||||
if (kernel_get_tick_count() >= uds_state.security_delay_timer) {
|
||||
uds_state.security_delay_timer = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user