FaSTTUBe/ams-master
56
1
Fork 0
Code Issues 2 Pull Requests Packages Projects Releases Wiki Activity

isotp update/fixup

Browse Source
This commit is contained in:
Kilian Bracher 2025-05-05 02:36:14 +02:00
parent 162f58abe2
commit 976eba57d4
Signed by: k.bracher
SSH Key Fingerprint: SHA256:mXpyZkK7RDiJ7qeHCKJX108woM0cl5TrCvNBJASu6lM
5 changed files with 149 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

161
AMS_Master_Code/Core/Lib/isotp/isotp.c
View File

@ -60,7 +60,7 @@ struct isotp_message_state {
#if ISOTP_RX #if ISOTP_RX
uint16_t data_len; uint16_t data_len;
#endif #endif
uint16_t id; isotp_conn_id_t conn_id; // Store connection ID instead of searching
enum : uint8_t { enum : uint8_t {
ISOTP_NEW_MESSAGE = 0, ISOTP_NEW_MESSAGE = 0,
ISOTP_WAITING_FLOW_CONTROL = 1, ISOTP_WAITING_FLOW_CONTROL = 1,
@ -88,12 +88,17 @@ static struct isotp_message_state rx_queue[MAX_ISOTP_MESSAGES_IN_FLIGHT] = {[0 .
#endif #endif
#define INVALID_CAN_ID 0xFFFF #define INVALID_CAN_ID 0xFFFF
#define INVALID_CONN_ID MAX_ISOTP_CONNECTIONS
static struct {uint16_t src_id; uint16_t dst_id;} connections[MAX_ISOTP_CONNECTIONS] = { static struct {
[0 ... MAX_ISOTP_CONNECTIONS - 1] = {.src_id=INVALID_CAN_ID, .dst_id=INVALID_CAN_ID} uint16_t them;
uint16_t us;
isotp_flags_t flags;
} connections[MAX_ISOTP_CONNECTIONS] = {
[0 ... MAX_ISOTP_CONNECTIONS - 1] = {.them=INVALID_CAN_ID, .us=INVALID_CAN_ID, .flags=0}
}; };
uint32_t last_called; static uint32_t last_called;
isotp_status_t isotp_init() { isotp_status_t isotp_init() {
// platform-specific initialization // platform-specific initialization
@ -107,6 +112,14 @@ isotp_status_t isotp_init() {
#define DATA_TYPE const uint8_t * #define DATA_TYPE const uint8_t *
#endif #endif
/**
* Helper function to check if a connection has a specific flag set
*/
static inline bool check_flag(isotp_conn_id_t conn_id, isotp_flags_t flags) {
return (conn_id < MAX_ISOTP_CONNECTIONS &&
(connections[conn_id].flags & flags) == flags);
}
// Internal helper structure to return both status and index // Internal helper structure to return both status and index
typedef struct { typedef struct {
isotp_status_t status; isotp_status_t status;
@ -115,7 +128,7 @@ typedef struct {
// Internal helper function to validate message parameters and find available slot // Internal helper function to validate message parameters and find available slot
[[gnu::access(none, 2, 3)]] [[gnu::access(none, 2, 3)]]
static isotp_validation_result_t isotp_validate_message(uint16_t id, const uint8_t *data, size_t datalen) { static isotp_validation_result_t isotp_validate_message(isotp_conn_id_t conn_id, const uint8_t *data, size_t datalen) {
isotp_validation_result_t result = {.status = ISOTP_ERROR, .index = 0}; isotp_validation_result_t result = {.status = ISOTP_ERROR, .index = 0};
if (datalen > MAX_ISOTP_DATA_SIZE_MESSAGE) { if (datalen > MAX_ISOTP_DATA_SIZE_MESSAGE) {
@ -128,23 +141,15 @@ static isotp_validation_result_t isotp_validate_message(uint16_t id, const uint8
return result; return result;
} }
// Check if connection exists // Validate connection ID
bool connection_found = false; if (conn_id >= MAX_ISOTP_CONNECTIONS || connections[conn_id].them == INVALID_CAN_ID) {
for (size_t i = 0; i < MAX_ISOTP_CONNECTIONS; i++) {
if (connections[i].dst_id == id) {
connection_found = true;
break;
}
}
if (!connection_found) {
result.status = ISOTP_CONNECTION_NOT_FOUND; result.status = ISOTP_CONNECTION_NOT_FOUND;
return result; return result;
} }
// Check if message with same ID is already in flight // Check if message with same connection ID is already in flight
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) { for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
if (!(tx_queue[i].state == ISOTP_DONE || tx_queue[i].state == ISOTP_FAILED) && tx_queue[i].id == id) { if (!(tx_queue[i].state == ISOTP_DONE || tx_queue[i].state == ISOTP_FAILED) && tx_queue[i].conn_id == conn_id) {
result.status = ISOTP_MESSAGE_ALREADY_IN_FLIGHT; result.status = ISOTP_MESSAGE_ALREADY_IN_FLIGHT;
return result; return result;
} }
@ -163,14 +168,14 @@ static isotp_validation_result_t isotp_validate_message(uint16_t id, const uint8
return result; return result;
} }
isotp_status_t isotp_add_message(uint16_t id, const uint8_t * data, size_t datalen) { isotp_status_t isotp_add_message(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen) {
isotp_validation_result_t result = isotp_validate_message(id, data, datalen); isotp_validation_result_t result = isotp_validate_message(conn_id, data, datalen);
if (result.status == ISOTP_OK) { if (result.status == ISOTP_OK) {
// Add message to the queue at the found index // Add message to the queue at the found index
tx_queue[result.index] = (struct isotp_message_state) { tx_queue[result.index] = (struct isotp_message_state) {
.data = (DATA_TYPE) data, .data = (DATA_TYPE) data,
.id = id, .conn_id = conn_id,
.state = ISOTP_NEW_MESSAGE, .state = ISOTP_NEW_MESSAGE,
.seq_n = 1, .seq_n = 1,
.remaining = datalen .remaining = datalen
@ -180,9 +185,9 @@ isotp_status_t isotp_add_message(uint16_t id, const uint8_t * data, size_t datal
return result.status; return result.status;
} }
isotp_status_t isotp_try_add_message(uint16_t id, const uint8_t * data, size_t datalen) { isotp_status_t isotp_try_add_message(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen) {
// Just validate without adding to the queue // Just validate without adding to the queue
isotp_validation_result_t result = isotp_validate_message(id, data, datalen); isotp_validation_result_t result = isotp_validate_message(conn_id, data, datalen);
return result.status; return result.status;
} }
@ -199,6 +204,7 @@ isotp_status_t isotp_update() {
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) { for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
isotp_message msg = {}; isotp_message msg = {};
auto conn = connections[tx_queue[i].conn_id];
switch (tx_queue[i].state) { switch (tx_queue[i].state) {
case ISOTP_NEW_MESSAGE: case ISOTP_NEW_MESSAGE:
@ -207,9 +213,9 @@ isotp_status_t isotp_update() {
memcpy(msg.single.data, tx_queue[i].data, tx_queue[i].remaining); memcpy(msg.single.data, tx_queue[i].data, tx_queue[i].remaining);
#if ISOTP_PADDING #if ISOTP_PADDING
memset(msg.single.data + tx_queue[i].remaining, ISOTP_PADDING, MAX_ISOTP_DATA_SINGLE - tx_queue[i].remaining); memset(msg.single.data + tx_queue[i].remaining, ISOTP_PADDING, MAX_ISOTP_DATA_SINGLE - tx_queue[i].remaining);
TRY_CAN_TRANSMIT(tx_queue[i].id, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE); TRY_CAN_TRANSMIT(conn.us, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE);
#else #else
TRY_CAN_TRANSMIT(tx_queue[i].id, (const uint8_t *)&msg, tx_queue[i].remaining + 1); TRY_CAN_TRANSMIT(conn.us, (const uint8_t *)&msg, tx_queue[i].remaining + 1);
#endif #endif
tx_queue[i].remaining = 0; tx_queue[i].remaining = 0;
tx_queue[i].state = ISOTP_DONE; tx_queue[i].state = ISOTP_DONE;
@ -217,12 +223,25 @@ isotp_status_t isotp_update() {
msg.first.header_1 = ISOTP_FIRST_FRAME | ((tx_queue[i].remaining >> 8) & 0x0F); msg.first.header_1 = ISOTP_FIRST_FRAME | ((tx_queue[i].remaining >> 8) & 0x0F);
msg.first.header_2 = tx_queue[i].remaining & 0xFF; msg.first.header_2 = tx_queue[i].remaining & 0xFF;
memcpy(msg.first.data, tx_queue[i].data, MAX_ISOTP_DATA_FIRST); memcpy(msg.first.data, tx_queue[i].data, MAX_ISOTP_DATA_FIRST);
TRY_CAN_TRANSMIT(tx_queue[i].id, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE); TRY_CAN_TRANSMIT(conn.us, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE);
tx_queue[i].remaining -= MAX_ISOTP_DATA_FIRST; tx_queue[i].remaining -= MAX_ISOTP_DATA_FIRST;
tx_queue[i].data += MAX_ISOTP_DATA_FIRST; tx_queue[i].data += MAX_ISOTP_DATA_FIRST;
// Check if we're in broadcast mode - don't wait for flow control
isotp_conn_id_t conn_id = tx_queue[i].conn_id;
// Connection ID was already validated during isotp_add_message
if (check_flag(conn_id, ISOTP_FLAGS_BROADCAST)) {
// In broadcast mode, assume we can send directly
tx_queue[i].flow_control.state = ISOTP_CONTINUE;
tx_queue[i].flow_control.block_size = 0; // No block size limit
tx_queue[i].flow_control.st_min = 0; // No separation time
tx_queue[i].state = ISOTP_READY;
} else {
tx_queue[i].timestamp = now; // Set timestamp when entering WAITING_FLOW_CONTROL state tx_queue[i].timestamp = now; // Set timestamp when entering WAITING_FLOW_CONTROL state
tx_queue[i].state = ISOTP_WAITING_FLOW_CONTROL; tx_queue[i].state = ISOTP_WAITING_FLOW_CONTROL;
} }
}
break; break;
case ISOTP_READY: case ISOTP_READY:
ISOTP_SEND_AGAIN: ISOTP_SEND_AGAIN:
@ -246,12 +265,12 @@ isotp_status_t isotp_update() {
#if ISOTP_PADDING #if ISOTP_PADDING
if (bytes_to_send < MAX_ISOTP_DATA_CONSECUTIVE) { if (bytes_to_send < MAX_ISOTP_DATA_CONSECUTIVE) {
memset(msg.consecutive.data + bytes_to_send, ISOTP_PADDING, MAX_ISOTP_DATA_CONSECUTIVE - bytes_to_send); memset(msg.consecutive.data + bytes_to_send, ISOTP_PADDING, MAX_ISOTP_DATA_CONSECUTIVE - bytes_to_send);
TRY_CAN_TRANSMIT(tx_queue[i].id, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE); TRY_CAN_TRANSMIT(conn.us, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE);
} else { } else {
TRY_CAN_TRANSMIT(tx_queue[i].id, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE); TRY_CAN_TRANSMIT(conn.us, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE);
} }
#else #else
TRY_CAN_TRANSMIT(tx_queue[i].id, (const uint8_t *)&msg, bytes_to_send + 1); TRY_CAN_TRANSMIT(conn.us, (const uint8_t *)&msg, bytes_to_send + 1);
#endif #endif
tx_queue[i].data += bytes_to_send; tx_queue[i].data += bytes_to_send;
@ -292,14 +311,20 @@ isotp_status_t isotp_update() {
return ISOTP_OK; return ISOTP_OK;
} }
isotp_status_t isotp_add_connection(uint16_t src_id, uint16_t dst_id) { int isotp_add_connection(uint16_t them, uint16_t us, isotp_flags_t flags) {
if (flags > (ISOTP_FLAGS_BROADCAST | ISOTP_FLAGS_LISTEN_ONLY)) {
return ISOTP_INVALID_FLAGS;
}
for (size_t i = 0; i < MAX_ISOTP_CONNECTIONS; i++) { for (size_t i = 0; i < MAX_ISOTP_CONNECTIONS; i++) {
if (connections[i].src_id == INVALID_CAN_ID) { if (connections[i].them == INVALID_CAN_ID) {
connections[i].src_id = src_id; connections[i].them = them;
connections[i].dst_id = dst_id; connections[i].us = us;
return ISOTP_OK; connections[i].flags = flags;
return i; // Return the connection ID on success
} }
} }
return ISOTP_TOO_MANY_CONNECTIONS; return ISOTP_TOO_MANY_CONNECTIONS;
} }
@ -309,8 +334,22 @@ enum FC_status : uint8_t {
ISOTP_FC_ABORT = 2 ISOTP_FC_ABORT = 2
}; };
// Function prototypes for internal functions
#if ISOTP_RX #if ISOTP_RX
static isotp_status_t isotp_send_fc(uint16_t id, enum FC_status status, uint8_t block_size, uint8_t st_min) { static isotp_status_t isotp_send_fc(isotp_conn_id_t conn_id, enum FC_status status, uint8_t block_size, uint8_t st_min);
static isotp_status_t isotp_handle_single_frame(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen);
static isotp_status_t isotp_handle_first_frame(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen);
static isotp_status_t isotp_handle_consecutive_frame(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen);
#endif
static isotp_status_t isotp_handle_flow_control(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen);
#if ISOTP_RX
static isotp_status_t isotp_send_fc(isotp_conn_id_t conn_id, enum FC_status status, uint8_t block_size, uint8_t st_min) {
// Check if we're in listen-only mode first
if (check_flag(conn_id, ISOTP_FLAGS_LISTEN_ONLY)) {
return ISOTP_OK; // Silently succeed but don't actually send anything
}
isotp_message msg = {}; isotp_message msg = {};
msg.single.header = ISOTP_FLOW_CONTROL | (status & 0x0F); msg.single.header = ISOTP_FLOW_CONTROL | (status & 0x0F);
msg.single.data[0] = block_size; msg.single.data[0] = block_size;
@ -318,11 +357,11 @@ static isotp_status_t isotp_send_fc(uint16_t id, enum FC_status status, uint8_t
#if ISOTP_PADDING #if ISOTP_PADDING
memset(msg.single.data + 2, ISOTP_PADDING, MAX_ISOTP_DATA_SINGLE - 2); memset(msg.single.data + 2, ISOTP_PADDING, MAX_ISOTP_DATA_SINGLE - 2);
if (!isotp_transmit(id, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE)) { if (!isotp_transmit(connections[conn_id].us, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE)) {
return ISOTP_CAN_TRANSMIT_ERROR; return ISOTP_CAN_TRANSMIT_ERROR;
} }
#else #else
if (!isotp_transmit(id, (const uint8_t *)&msg, 3)) { if (!isotp_transmit(connections[conn_id].us, (const uint8_t *)&msg, 3)) {
return ISOTP_CAN_TRANSMIT_ERROR; return ISOTP_CAN_TRANSMIT_ERROR;
} }
#endif #endif
@ -330,7 +369,7 @@ static isotp_status_t isotp_send_fc(uint16_t id, enum FC_status status, uint8_t
return ISOTP_OK; return ISOTP_OK;
} }
isotp_status_t isotp_handle_first_frame(uint16_t id, uint16_t dst_id, const uint8_t * data, size_t datalen) { static isotp_status_t isotp_handle_first_frame(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen) {
if (datalen < 3) { if (datalen < 3) {
return ISOTP_INVALID_FRAME; return ISOTP_INVALID_FRAME;
} }
@ -344,13 +383,13 @@ isotp_status_t isotp_handle_first_frame(uint16_t id, uint16_t dst_id, const uint
if (rx_queue[i].state == ISOTP_DONE || rx_queue[i].state == ISOTP_FAILED) { if (rx_queue[i].state == ISOTP_DONE || rx_queue[i].state == ISOTP_FAILED) {
uint8_t* rx_buffer = isotp_get_rx_buffer(length); uint8_t* rx_buffer = isotp_get_rx_buffer(length);
if (rx_buffer == NULL) { if (rx_buffer == NULL) {
isotp_send_fc(dst_id, ISOTP_FC_ABORT, 0, 0); // Abort isotp_send_fc(conn_id, ISOTP_FC_ABORT, 0, 0); // Abort
return ISOTP_BUFFER_CREATION_ERROR; return ISOTP_BUFFER_CREATION_ERROR;
} }
rx_queue[i] = (struct isotp_message_state) { rx_queue[i] = (struct isotp_message_state) {
.data = rx_buffer, .data = rx_buffer,
.id = id, .conn_id = conn_id, // Store connection ID
.remaining = length, .remaining = length,
.data_len = length, .data_len = length,
.state = ISOTP_NEW_MESSAGE, .state = ISOTP_NEW_MESSAGE,
@ -361,15 +400,17 @@ isotp_status_t isotp_handle_first_frame(uint16_t id, uint16_t dst_id, const uint
rx_queue[i].data += MAX_ISOTP_DATA_FIRST; rx_queue[i].data += MAX_ISOTP_DATA_FIRST;
rx_queue[i].timestamp = isotp_get_time(); rx_queue[i].timestamp = isotp_get_time();
rx_queue[i].state = ISOTP_READY; rx_queue[i].state = ISOTP_READY;
isotp_send_fc(dst_id, ISOTP_FC_CTS, 0, ISOTP_RX_MIN_ST); // Send flow control frame
isotp_send_fc(conn_id, ISOTP_FC_CTS, 0, ISOTP_RX_MIN_ST); // Send flow control frame
return ISOTP_OK; return ISOTP_OK;
} }
} }
isotp_send_fc(dst_id, ISOTP_FC_ABORT, 0, 0); // Abort
isotp_send_fc(conn_id, ISOTP_FC_ABORT, 0, 0); // Abort
return ISOTP_TOO_MANY_MESSAGES; return ISOTP_TOO_MANY_MESSAGES;
} }
isotp_status_t isotp_handle_consecutive_frame(uint16_t id, uint16_t dst_id, const uint8_t * data, size_t datalen) { static isotp_status_t isotp_handle_consecutive_frame(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen) {
if (datalen < 2) { if (datalen < 2) {
return ISOTP_INVALID_FRAME; return ISOTP_INVALID_FRAME;
} }
@ -377,12 +418,12 @@ isotp_status_t isotp_handle_consecutive_frame(uint16_t id, uint16_t dst_id, cons
uint8_t seq_n = data[0] & 0x0F; uint8_t seq_n = data[0] & 0x0F;
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) { for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
if (rx_queue[i].state == ISOTP_DONE || rx_queue[i].state == ISOTP_FAILED || rx_queue[i].id != id) { if (rx_queue[i].state == ISOTP_DONE || rx_queue[i].state == ISOTP_FAILED || rx_queue[i].conn_id != conn_id) {
continue; continue;
} }
if (rx_queue[i].seq_n != seq_n) { if (rx_queue[i].seq_n != seq_n) {
isotp_send_fc(dst_id, ISOTP_FC_ABORT, 0, 0); // Abort isotp_send_fc(conn_id, ISOTP_FC_ABORT, 0, 0); // Abort
rx_queue[i].state = ISOTP_FAILED; rx_queue[i].state = ISOTP_FAILED;
isotp_free_rx_buffer(rx_queue[i].data - rx_queue[i].data_len + rx_queue[i].remaining); // Free the buffer isotp_free_rx_buffer(rx_queue[i].data - rx_queue[i].data_len + rx_queue[i].remaining); // Free the buffer
return ISOTP_INVALID_SEQUENCE; // Sequence number mismatch return ISOTP_INVALID_SEQUENCE; // Sequence number mismatch
@ -391,7 +432,6 @@ isotp_status_t isotp_handle_consecutive_frame(uint16_t id, uint16_t dst_id, cons
rx_queue[i].seq_n++; rx_queue[i].seq_n++;
rx_queue[i].seq_n &= 0x0F; // Wrap around sequence number rx_queue[i].seq_n &= 0x0F; // Wrap around sequence number
size_t bytes_to_copy = rx_queue[i].remaining <= MAX_ISOTP_DATA_CONSECUTIVE ? rx_queue[i].remaining : MAX_ISOTP_DATA_CONSECUTIVE; size_t bytes_to_copy = rx_queue[i].remaining <= MAX_ISOTP_DATA_CONSECUTIVE ? rx_queue[i].remaining : MAX_ISOTP_DATA_CONSECUTIVE;
memcpy(rx_queue[i].data, data + 1, bytes_to_copy); memcpy(rx_queue[i].data, data + 1, bytes_to_copy);
rx_queue[i].data += bytes_to_copy; rx_queue[i].data += bytes_to_copy;
@ -400,18 +440,18 @@ isotp_status_t isotp_handle_consecutive_frame(uint16_t id, uint16_t dst_id, cons
if (rx_queue[i].remaining == 0) { if (rx_queue[i].remaining == 0) {
rx_queue[i].state = ISOTP_DONE; rx_queue[i].state = ISOTP_DONE;
isotp_on_message_received(dst_id, rx_queue[i].data - rx_queue[i].data_len, rx_queue[i].data_len); isotp_on_message_received(connections[conn_id].them, rx_queue[i].data - rx_queue[i].data_len, rx_queue[i].data_len);
isotp_free_rx_buffer(rx_queue[i].data - rx_queue[i].data_len); // Free the buffer after message is received isotp_free_rx_buffer(rx_queue[i].data - rx_queue[i].data_len); // Free the buffer after message is received
} }
return ISOTP_OK; return ISOTP_OK;
} }
isotp_send_fc(dst_id, ISOTP_FC_ABORT, 0, 0); // Abort isotp_send_fc(conn_id, ISOTP_FC_ABORT, 0, 0); // Abort
return ISOTP_INVALID_PARAMETER; return ISOTP_INVALID_MESSAGE; // No matching message found
} }
isotp_status_t isotp_handle_single_frame(uint16_t id, const uint8_t * data, size_t datalen) { static isotp_status_t isotp_handle_single_frame(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen) {
if (datalen < 2) { if (datalen < 2) {
return ISOTP_INVALID_FRAME; return ISOTP_INVALID_FRAME;
} }
@ -425,18 +465,18 @@ isotp_status_t isotp_handle_single_frame(uint16_t id, const uint8_t * data, size
return ISOTP_INVALID_FRAME; return ISOTP_INVALID_FRAME;
} }
isotp_on_message_received(id, data + 1, length); // Send the correct length of data isotp_on_message_received(connections[conn_id].them, data + 1, length); // Send the correct length of data
return ISOTP_OK; return ISOTP_OK;
} }
#endif // ISOTP_RX #endif // ISOTP_RX
static isotp_status_t isotp_handle_flow_control(uint16_t id, const uint8_t * data, size_t datalen) { static isotp_status_t isotp_handle_flow_control(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen) {
if (datalen < 3) { if (datalen < 3) {
return ISOTP_INVALID_FRAME; return ISOTP_INVALID_FRAME;
} }
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) { for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
if (tx_queue[i].state == ISOTP_DONE || tx_queue[i].state == ISOTP_FAILED || tx_queue[i].id != id) { if (tx_queue[i].state == ISOTP_DONE || tx_queue[i].state == ISOTP_FAILED || tx_queue[i].conn_id != conn_id) {
continue; continue;
} }
@ -476,15 +516,16 @@ isotp_status_t isotp_handle_incoming(uint16_t id, const uint8_t * data, size_t d
return ISOTP_INVALID_PARAMETER; return ISOTP_INVALID_PARAMETER;
} }
uint16_t dst_id = INVALID_CAN_ID; // Find the matching connection ID
isotp_conn_id_t conn_id = INVALID_CONN_ID;
for (size_t i = 0; i < MAX_ISOTP_CONNECTIONS; i++) { for (size_t i = 0; i < MAX_ISOTP_CONNECTIONS; i++) {
if (connections[i].src_id == id) { if (connections[i].them == id) {
dst_id = connections[i].dst_id; conn_id = i;
break; break;
} }
} }
if (dst_id == INVALID_CAN_ID) { if (conn_id == INVALID_CONN_ID) {
return ISOTP_CONNECTION_NOT_FOUND; return ISOTP_CONNECTION_NOT_FOUND;
} }
@ -493,14 +534,14 @@ isotp_status_t isotp_handle_incoming(uint16_t id, const uint8_t * data, size_t d
switch (type) { switch (type) {
#if ISOTP_RX #if ISOTP_RX
case ISOTP_SINGLE_FRAME: case ISOTP_SINGLE_FRAME:
return isotp_handle_single_frame(id, data, datalen); return isotp_handle_single_frame(conn_id, data, datalen);
case ISOTP_FIRST_FRAME: case ISOTP_FIRST_FRAME:
return isotp_handle_first_frame(id, dst_id, data, datalen); return isotp_handle_first_frame(conn_id, data, datalen);
case ISOTP_CONSECUTIVE_FRAME: case ISOTP_CONSECUTIVE_FRAME:
return isotp_handle_consecutive_frame(id, dst_id, data, datalen); return isotp_handle_consecutive_frame(conn_id, data, datalen);
#endif #endif
case ISOTP_FLOW_CONTROL: case ISOTP_FLOW_CONTROL:
return isotp_handle_flow_control(dst_id, data, datalen); return isotp_handle_flow_control(conn_id, data, datalen);
default: default:
return ISOTP_INVALID_FRAME; return ISOTP_INVALID_FRAME;
} }

27
AMS_Master_Code/Core/Lib/isotp/isotp.h
View File

@ -26,6 +26,8 @@ typedef enum isotp_status {
ISOTP_CONNECTION_NOT_FOUND = -9, ISOTP_CONNECTION_NOT_FOUND = -9,
ISOTP_BUFFER_CREATION_ERROR = -10, ISOTP_BUFFER_CREATION_ERROR = -10,
ISOTP_INVALID_SEQUENCE = -11, ISOTP_INVALID_SEQUENCE = -11,
ISOTP_INVALID_FLAGS = -12,
ISOTP_INVALID_MESSAGE = -13,
} isotp_status_t; } isotp_status_t;
static inline const char *isotp_status_to_string(isotp_status_t status) { static inline const char *isotp_status_to_string(isotp_status_t status) {
@ -42,15 +44,34 @@ static inline const char *isotp_status_to_string(isotp_status_t status) {
case ISOTP_CONNECTION_NOT_FOUND: return "ISOTP_CONNECTION_NOT_FOUND"; case ISOTP_CONNECTION_NOT_FOUND: return "ISOTP_CONNECTION_NOT_FOUND";
case ISOTP_BUFFER_CREATION_ERROR: return "ISOTP_BUFFER_CREATION_ERROR"; case ISOTP_BUFFER_CREATION_ERROR: return "ISOTP_BUFFER_CREATION_ERROR";
case ISOTP_INVALID_SEQUENCE: return "ISOTP_INVALID_SEQUENCE"; case ISOTP_INVALID_SEQUENCE: return "ISOTP_INVALID_SEQUENCE";
case ISOTP_INVALID_FLAGS: return "ISOTP_INVALID_FLAGS";
case ISOTP_INVALID_MESSAGE: return "ISOTP_INVALID_MESSAGE";
default: return "UNKNOWN_STATUS"; default: return "UNKNOWN_STATUS";
} }
} }
typedef enum [[clang::flag_enum]] {
ISOTP_FLAGS_NONE = 0,
ISOTP_FLAGS_LISTEN_ONLY = 1 << 0, // do not send flow control frames
ISOTP_FLAGS_BROADCAST = 1 << 1, // linux socketcan broadcast (do not wait for flow control)
} isotp_flags_t;
// Connection ID is an index into the connections array
typedef uint16_t isotp_conn_id_t;
isotp_status_t isotp_init(); isotp_status_t isotp_init();
isotp_status_t isotp_update(); isotp_status_t isotp_update();
[[gnu::access(read_only, 2, 3)]] isotp_status_t isotp_handle_incoming(uint16_t id, const uint8_t *data, size_t datalen); [[gnu::access(read_only, 2, 3)]] isotp_status_t isotp_handle_incoming(uint16_t id, const uint8_t *data, size_t datalen);
isotp_status_t isotp_add_connection(uint16_t src_id, uint16_t dst_id); /**
[[gnu::access(read_only, 2, 3)]] isotp_status_t isotp_add_message(uint16_t id, const uint8_t * data, size_t datalen); * @brief Add a new connection to the isotp stack.
[[gnu::access(none, 2, 3)]] isotp_status_t isotp_try_add_message(uint16_t id, const uint8_t * data, size_t datalen); * @param them The CAN ID of the remote device.
* @param us The CAN ID that will be used by this device.
* @param flags Configuration flags for the connection (e.g., ISOTP_FLAGS_LISTEN_ONLY, ISOTP_FLAGS_BROADCAST).
* @return On success: Connection ID (index >= 0) that should be used in subsequent isotp_add_message calls.
* On failure: One of the negative isotp_status_t error codes.
*/
int isotp_add_connection(uint16_t them, uint16_t us, isotp_flags_t flags);
[[gnu::access(read_only, 2, 3)]] isotp_status_t isotp_add_message(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen);
[[gnu::access(none, 2, 3)]] isotp_status_t isotp_try_add_message(isotp_conn_id_t conn_id, const uint8_t * data, size_t datalen);
#endif // ISOTP_H #endif // ISOTP_H

12
AMS_Master_Code/Core/Lib/logger/isotp_log_backend.c
View File

@ -11,6 +11,7 @@ static bool streaming_enabled = false;
static log_level_t streaming_level = LOG_LEVEL_NOISY; static log_level_t streaming_level = LOG_LEVEL_NOISY;
static bool buffer_dropped_messages = false; static bool buffer_dropped_messages = false;
static bool using_buffer_2 = false; static bool using_buffer_2 = false;
static isotp_conn_id_t isotp_connection_id = 0;
/* Buffer for log messages */ /* Buffer for log messages */
// The buffer is dynamically split to allow double-buffering while data is being sent // The buffer is dynamically split to allow double-buffering while data is being sent
@ -101,12 +102,13 @@ void isotp_log_backend_init(void) {
isotp_backend_registered = log_register_backend(&isotp_backend); isotp_backend_registered = log_register_backend(&isotp_backend);
/* Set up ISO-TP connection */ /* Set up ISO-TP connection */
isotp_status_t status = isotp_add_connection(ISOTP_LOG_FC_CAN_ID, ISOTP_LOG_CAN_ID); int status = isotp_add_connection(ISOTP_LOG_FC_CAN_ID, ISOTP_LOG_CAN_ID, ISOTP_FLAGS_NONE);
if (status != ISOTP_OK) { if (status < 0) {
log_error("Failed to add ISO-TP connection: %s", isotp_status_to_string(status)); log_error("Failed to add ISO-TP connection: %s", isotp_status_to_string((isotp_status_t)status));
return; return;
} }
log_info("ISO-TP backend initialized"); isotp_connection_id = (isotp_conn_id_t)status;
log_info("ISO-TP backend initialized with connection ID: %d", isotp_connection_id);
} }
/* ISO-TP backend initialization */ /* ISO-TP backend initialization */
@ -158,7 +160,7 @@ static void isotp_backend_flush(void) {
/* Only send if we have data and a previous send is not in progress */ /* Only send if we have data and a previous send is not in progress */
if (buffer_size > 0) { if (buffer_size > 0) {
isotp_status_t status = isotp_add_message( isotp_status_t status = isotp_add_message(
ISOTP_LOG_CAN_ID, isotp_connection_id,
message_buffer.buffer + buffer_offset, message_buffer.buffer + buffer_offset,
buffer_size buffer_size
); );

16
AMS_Master_Code/Core/Src/can.c
View File

@ -18,6 +18,8 @@
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
static isotp_conn_id_t isotp_connection_id = 0;
bool isotp_transmit(uint16_t id, const uint8_t *data, size_t datalen) { bool isotp_transmit(uint16_t id, const uint8_t *data, size_t datalen) {
return ftcan_transmit(id, data, datalen) == HAL_OK; return ftcan_transmit(id, data, datalen) == HAL_OK;
} }
@ -30,6 +32,15 @@ void can_init(FDCAN_HandleTypeDef *handle) {
ftcan_init(handle); ftcan_init(handle);
ftcan_add_filter(CAN_ID_SHUNT_BASE, 0xFF0); ftcan_add_filter(CAN_ID_SHUNT_BASE, 0xFF0);
ftcan_add_filter(CAN_ID_AMS_IN, 0xFFF); ftcan_add_filter(CAN_ID_AMS_IN, 0xFFF);
ftcan_add_filter(CAN_ID_AMS_DETAILS_FC, 0xFFF);
ftcan_add_filter(ISOTP_LOG_FC_CAN_ID, 0xFFF);
auto status = isotp_add_connection(CAN_ID_AMS_DETAILS_FC, CAN_ID_AMS_DETAILS, ISOTP_FLAGS_NONE);
if (status < 0) {
log_warning("Failed to add ISO-TP connection: %s", isotp_status_to_string((isotp_status_t)status));
}
isotp_connection_id = (isotp_conn_id_t)status;
// ftcan_add_filter(CAN_ID_SLAVE_PANIC, 0xFFF); // ftcan_add_filter(CAN_ID_SLAVE_PANIC, 0xFFF);
// ftcan_add_filter(CAN_ID_SLAVE_STATUS_BASE, 0xFF0); // ftcan_add_filter(CAN_ID_SLAVE_STATUS_BASE, 0xFF0);
// ftcan_add_filter(CAN_ID_SLAVE_LOG, 0xFFF); // ftcan_add_filter(CAN_ID_SLAVE_LOG, 0xFFF);
@ -63,7 +74,7 @@ HAL_StatusTypeDef can_send_details() {
auto module = &modules[module_index]; auto module = &modules[module_index];
auto data_ptr = &data[1]; auto data_ptr = &data[1];
isotp_status_t status = isotp_try_add_message(CAN_ID_AMS_DETAILS_FC, data, sizeof(data)); isotp_status_t status = isotp_try_add_message(isotp_connection_id, data, sizeof(data));
switch (status) { switch (status) {
case ISOTP_OK: case ISOTP_OK:
break; break;
@ -128,7 +139,7 @@ HAL_StatusTypeDef can_send_details() {
data_ptr = ftcan_marshal_signed(data_ptr, cellTemps[module_index][i], 2); data_ptr = ftcan_marshal_signed(data_ptr, cellTemps[module_index][i], 2);
} }
if ((status = isotp_add_message(CAN_ID_AMS_DETAILS, data, sizeof(data))) != ISOTP_OK) { if ((status = isotp_add_message(isotp_connection_id, data, sizeof(data))) != ISOTP_OK) {
log_warning("isotp_add_message failed unexpectedly: %s", isotp_status_to_string(status)); log_warning("isotp_add_message failed unexpectedly: %s", isotp_status_to_string(status));
return HAL_ERROR; return HAL_ERROR;
} }
@ -152,6 +163,7 @@ void ftcan_msg_received_cb(uint16_t id, size_t len, const uint8_t *data) {
switch (id) { switch (id) {
case ISOTP_LOG_FC_CAN_ID: case ISOTP_LOG_FC_CAN_ID:
case CAN_ID_AMS_DETAILS_FC:
auto status = isotp_handle_incoming(id, data, len); auto status = isotp_handle_incoming(id, data, len);
if (status != ISOTP_OK) { if (status != ISOTP_OK) {
log_debug("Error when handling flow control: %s", isotp_status_to_string(status)); log_debug("Error when handling flow control: %s", isotp_status_to_string(status));

1
AMS_Master_Code/Core/Src/main.c
View File

@ -199,7 +199,6 @@ int main(void)
// init for master functions // init for master functions
can_init(&hfdcan1); can_init(&hfdcan1);
isotp_init(); isotp_init();
isotp_add_connection(CAN_ID_AMS_DETAILS_FC, CAN_ID_AMS_DETAILS);
// for testing. in the final code can log streaming will be enabled by can message // for testing. in the final code can log streaming will be enabled by can message
isotp_log_enable_streaming(LOG_LEVEL_INFO); isotp_log_enable_streaming(LOG_LEVEL_INFO);