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
uint16_t data_len;
#endif
uint16_t id;
isotp_conn_id_t conn_id; // Store connection ID instead of searching
enum : uint8_t {
ISOTP_NEW_MESSAGE = 0,
ISOTP_WAITING_FLOW_CONTROL = 1,
@ -88,12 +88,17 @@ static struct isotp_message_state rx_queue[MAX_ISOTP_MESSAGES_IN_FLIGHT] = {[0 .
#endif
#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] = {
[0 ... MAX_ISOTP_CONNECTIONS - 1] = {.src_id=INVALID_CAN_ID, .dst_id=INVALID_CAN_ID}
static struct {
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() {
// platform-specific initialization
@ -107,6 +112,14 @@ isotp_status_t isotp_init() {
#define DATA_TYPE const uint8_t *
#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
typedef struct {
isotp_status_t status;
@ -115,7 +128,7 @@ typedef struct {
// Internal helper function to validate message parameters and find available slot
[[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};
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;
}
// Check if connection exists
bool connection_found = false;
for (size_t i = 0; i < MAX_ISOTP_CONNECTIONS; i++) {
if (connections[i].dst_id == id) {
connection_found = true;
break;
}
}
if (!connection_found) {
// Validate connection ID
if (conn_id >= MAX_ISOTP_CONNECTIONS || connections[conn_id].them == INVALID_CAN_ID) {
result.status = ISOTP_CONNECTION_NOT_FOUND;
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++) {
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;
return result;
}
@ -163,14 +168,14 @@ static isotp_validation_result_t isotp_validate_message(uint16_t id, const uint8
return result;
}
isotp_status_t isotp_add_message(uint16_t id, const uint8_t * data, size_t datalen) {
isotp_validation_result_t result = isotp_validate_message(id, data, 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(conn_id, data, datalen);
if (result.status == ISOTP_OK) {
// Add message to the queue at the found index
tx_queue[result.index] = (struct isotp_message_state) {
.data = (DATA_TYPE) data,
.id = id,
.conn_id = conn_id,
.state = ISOTP_NEW_MESSAGE,
.seq_n = 1,
.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;
}
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
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;
}
@ -199,6 +204,7 @@ isotp_status_t isotp_update() {
for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) {
isotp_message msg = {};
auto conn = connections[tx_queue[i].conn_id];
switch (tx_queue[i].state) {
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);
#if ISOTP_PADDING
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
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
tx_queue[i].remaining = 0;
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_2 = tx_queue[i].remaining & 0xFF;
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].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].state = ISOTP_WAITING_FLOW_CONTROL;
}
}
break;
case ISOTP_READY:
ISOTP_SEND_AGAIN:
@ -246,12 +265,12 @@ isotp_status_t isotp_update() {
#if ISOTP_PADDING
if (bytes_to_send < MAX_ISOTP_DATA_CONSECUTIVE) {
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 {
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
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
tx_queue[i].data += bytes_to_send;
@ -292,14 +311,20 @@ isotp_status_t isotp_update() {
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++) {
if (connections[i].src_id == INVALID_CAN_ID) {
connections[i].src_id = src_id;
connections[i].dst_id = dst_id;
return ISOTP_OK;
if (connections[i].them == INVALID_CAN_ID) {
connections[i].them = them;
connections[i].us = us;
connections[i].flags = flags;
return i; // Return the connection ID on success
}
}
return ISOTP_TOO_MANY_CONNECTIONS;
}
@ -309,8 +334,22 @@ enum FC_status : uint8_t {
ISOTP_FC_ABORT = 2
};
// Function prototypes for internal functions
#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 = {};
msg.single.header = ISOTP_FLOW_CONTROL | (status & 0x0F);
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
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;
}
#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;
}
#endif
@ -330,7 +369,7 @@ static isotp_status_t isotp_send_fc(uint16_t id, enum FC_status status, uint8_t
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) {
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) {
uint8_t* rx_buffer = isotp_get_rx_buffer(length);
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;
}
rx_queue[i] = (struct isotp_message_state) {
.data = rx_buffer,
.id = id,
.conn_id = conn_id, // Store connection ID
.remaining = length,
.data_len = length,
.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].timestamp = isotp_get_time();
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;
}
}
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;
}
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) {
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;
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;
}
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;
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
@ -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 &= 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;
memcpy(rx_queue[i].data, data + 1, 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) {
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
}
return ISOTP_OK;
}
isotp_send_fc(dst_id, ISOTP_FC_ABORT, 0, 0); // Abort
return ISOTP_INVALID_PARAMETER;
isotp_send_fc(conn_id, ISOTP_FC_ABORT, 0, 0); // Abort
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) {
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;
}
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;
}
#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) {
return ISOTP_INVALID_FRAME;
}
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;
}
@ -476,15 +516,16 @@ isotp_status_t isotp_handle_incoming(uint16_t id, const uint8_t * data, size_t d
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++) {
if (connections[i].src_id == id) {
dst_id = connections[i].dst_id;
if (connections[i].them == id) {
conn_id = i;
break;
}
}
if (dst_id == INVALID_CAN_ID) {
if (conn_id == INVALID_CONN_ID) {
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) {
#if ISOTP_RX
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:
return isotp_handle_first_frame(id, dst_id, data, datalen);
return isotp_handle_first_frame(conn_id, data, datalen);
case ISOTP_CONSECUTIVE_FRAME:
return isotp_handle_consecutive_frame(id, dst_id, data, datalen);
return isotp_handle_consecutive_frame(conn_id, data, datalen);
#endif
case ISOTP_FLOW_CONTROL:
return isotp_handle_flow_control(dst_id, data, datalen);
return isotp_handle_flow_control(conn_id, data, datalen);
default:
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_BUFFER_CREATION_ERROR = -10,
ISOTP_INVALID_SEQUENCE = -11,
ISOTP_INVALID_FLAGS = -12,
ISOTP_INVALID_MESSAGE = -13,
} isotp_status_t;
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_BUFFER_CREATION_ERROR: return "ISOTP_BUFFER_CREATION_ERROR";
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";
}
}
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_update();
[[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);
[[gnu::access(none, 2, 3)]] isotp_status_t isotp_try_add_message(uint16_t id, const uint8_t * data, size_t datalen);
/**
* @brief Add a new connection to the isotp stack.
* @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

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 bool buffer_dropped_messages = false;
static bool using_buffer_2 = false;
static isotp_conn_id_t isotp_connection_id = 0;
/* Buffer for log messages */
// 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);
/* Set up ISO-TP connection */
isotp_status_t status = isotp_add_connection(ISOTP_LOG_FC_CAN_ID, ISOTP_LOG_CAN_ID);
if (status != ISOTP_OK) {
log_error("Failed to add ISO-TP connection: %s", isotp_status_to_string(status));
int status = isotp_add_connection(ISOTP_LOG_FC_CAN_ID, ISOTP_LOG_CAN_ID, ISOTP_FLAGS_NONE);
if (status < 0) {
log_error("Failed to add ISO-TP connection: %s", isotp_status_to_string((isotp_status_t)status));
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 */
@ -158,7 +160,7 @@ static void isotp_backend_flush(void) {
/* Only send if we have data and a previous send is not in progress */
if (buffer_size > 0) {
isotp_status_t status = isotp_add_message(
ISOTP_LOG_CAN_ID,
isotp_connection_id,
message_buffer.buffer + buffer_offset,
buffer_size
);

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

@ -18,6 +18,8 @@
#include <stdint.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) {
return ftcan_transmit(id, data, datalen) == HAL_OK;
}
@ -30,6 +32,15 @@ void can_init(FDCAN_HandleTypeDef *handle) {
ftcan_init(handle);
ftcan_add_filter(CAN_ID_SHUNT_BASE, 0xFF0);
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_STATUS_BASE, 0xFF0);
// ftcan_add_filter(CAN_ID_SLAVE_LOG, 0xFFF);
@ -63,7 +74,7 @@ HAL_StatusTypeDef can_send_details() {
auto module = &modules[module_index];
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) {
case ISOTP_OK:
break;
@ -128,7 +139,7 @@ HAL_StatusTypeDef can_send_details() {
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));
return HAL_ERROR;
}
@ -152,6 +163,7 @@ void ftcan_msg_received_cb(uint16_t id, size_t len, const uint8_t *data) {
switch (id) {
case ISOTP_LOG_FC_CAN_ID:
case CAN_ID_AMS_DETAILS_FC:
auto status = isotp_handle_incoming(id, data, len);
if (status != ISOTP_OK) {
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
can_init(&hfdcan1);
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
isotp_log_enable_streaming(LOG_LEVEL_INFO);