From 354b6dc45344ad8c4821304c665764aedd402f3e Mon Sep 17 00:00:00 2001 From: Kilian Bracher Date: Tue, 22 Apr 2025 15:27:37 +0200 Subject: [PATCH] add can backend to logger --- AMS_Master_Code/Core/Lib/isotp/isotp.c | 255 ++++++++++++++++++ AMS_Master_Code/Core/Lib/isotp/isotp.h | 41 +++ .../Core/Lib/logger/isotp_log_backend.c | 194 +++++++++++++ .../Core/Lib/logger/isotp_log_backend.h | 26 ++ AMS_Master_Code/Core/Lib/logger/log.h | 2 +- AMS_Master_Code/Core/Src/can.c | 8 + 6 files changed, 525 insertions(+), 1 deletion(-) create mode 100644 AMS_Master_Code/Core/Lib/isotp/isotp.c create mode 100644 AMS_Master_Code/Core/Lib/isotp/isotp.h create mode 100644 AMS_Master_Code/Core/Lib/logger/isotp_log_backend.c create mode 100644 AMS_Master_Code/Core/Lib/logger/isotp_log_backend.h diff --git a/AMS_Master_Code/Core/Lib/isotp/isotp.c b/AMS_Master_Code/Core/Lib/isotp/isotp.c new file mode 100644 index 0000000..f80c558 --- /dev/null +++ b/AMS_Master_Code/Core/Lib/isotp/isotp.c @@ -0,0 +1,255 @@ +/* + * Author: Kilian + * Date: 2025-04-20 + * Description: This file contains a TX-only implementation of the ISO-TP protocol. + * + * References: + * https://munich.dissec.to/kb/chapters/isotp/isotp.html#protocol-basics-iso-15765-2 + * https://www.kernel.org/doc/html/next//networking/iso15765-2.html + * https://en.wikipedia.org/wiki/ISO_15765-2 + * https://ramn.readthedocs.io/en/latest/userguide/isotp_tutorial.html + */ +#include "isotp.h" + +#include +#include +#include + +#define MAX_CAN_DATA_SIZE 8 +#define MAX_ISOTP_DATA_SIZE_MESSAGE 4095 +#define MAX_ISOTP_DATA_SINGLE 7 +#define MAX_ISOTP_DATA_FIRST 6 +#define MAX_ISOTP_DATA_CONSECUTIVE 7 + +typedef union [[gnu::packed]] { + struct { + uint8_t header; + uint8_t data[MAX_ISOTP_DATA_SINGLE]; + } single; + struct { + uint8_t header_1; + uint8_t header_2; + uint8_t data[MAX_ISOTP_DATA_FIRST]; + } first; + struct { + uint8_t header; + uint8_t data[MAX_ISOTP_DATA_CONSECUTIVE]; + } consecutive; +} isotp_message; + +enum { + ISOTP_SINGLE_FRAME = 0 << 4, + ISOTP_FIRST_FRAME = 1 << 4, + ISOTP_CONSECUTIVE_FRAME = 2 << 4, + ISOTP_FLOW_CONTROL = 3 << 4 +}; + +struct isotp_message_state { + const uint8_t * data; + size_t remaining; + uint16_t id; + uint32_t timestamp; // Timestamp for timeout handling + enum : uint8_t { + ISOTP_NEW_MESSAGE = 0, + ISOTP_WAITING_FLOW_CONTROL = 1, + ISOTP_READY = 2, + ISOTP_DONE = 3, + ISOTP_FAILED = 4 + } state; + struct { + enum : uint8_t { + ISOTP_CONTINUE = 0, + ISOTP_WAIT = 1, + ISOTP_ABORT = 2 + } state; + uint8_t block_size; + uint8_t st_min; // in ms, we round up to 1 ms if sub 1 ms + uint8_t st_min_counter; + } flow_control; + uint8_t index; +}; + +static struct isotp_message_state tx_queue[MAX_ISOTP_MESSAGES_IN_FLIGHT] = {[0 ... MAX_ISOTP_MESSAGES_IN_FLIGHT - 1] = {.state=ISOTP_DONE }}; + +[[gnu::weak]] bool isotp_can_transmit(uint16_t, const uint8_t *, size_t) { return false; } +[[gnu::weak]] uint32_t isotp_can_get_time() { return 0; } + +uint32_t last_called; + +isotp_status_t isotp_init() { + // platform-specific initialization + last_called = isotp_can_get_time() - 100; + return (isotp_status_t) {ISOTP_OK}; +} + +isotp_status_t isotp_add_message(uint16_t id, const uint8_t * data, size_t datalen) { + if (datalen > MAX_ISOTP_DATA_SIZE_MESSAGE) { + return ISOTP_DATA_TOO_LONG; + } + + if (data == NULL || datalen == 0) { + return ISOTP_INVALID_PARAMETER; + } + + 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) { + return ISOTP_MESSAGE_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] = (struct isotp_message_state) {data, datalen, id, 0, ISOTP_NEW_MESSAGE, {}, 1}; + return ISOTP_OK; + } + } + + return ISOTP_TOO_MANY_MESSAGES; +} + +/* Macro to check CAN transmission result and handle errors */ +#define TRY_CAN_TRANSMIT(id, data, size) \ + if (!isotp_can_transmit(id, data, size)) { \ + tx_queue[i].state = ISOTP_FAILED; \ + return ISOTP_CAN_TRANSMIT_ERROR; \ + } + +isotp_status_t isotp_update() { + uint32_t now = isotp_can_get_time(); + uint32_t delta = now - last_called; + for (size_t i = 0; i < MAX_ISOTP_MESSAGES_IN_FLIGHT; i++) { + + isotp_message msg = {}; + + switch (tx_queue[i].state) { + case ISOTP_NEW_MESSAGE: + if (tx_queue[i].remaining <= MAX_ISOTP_DATA_SINGLE) { + msg.single.header = ISOTP_SINGLE_FRAME | (tx_queue[i].remaining & 0x0F); + 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); + #else + TRY_CAN_TRANSMIT(tx_queue[i].id, (const uint8_t *)&msg, tx_queue[i].remaining + 1); + #endif + tx_queue[i].remaining = 0; + tx_queue[i].state = ISOTP_DONE; + } else { + 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); + tx_queue[i].remaining -= MAX_ISOTP_DATA_FIRST; + tx_queue[i].data += MAX_ISOTP_DATA_FIRST; + 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: + if (tx_queue[i].flow_control.st_min_counter <= delta) { + tx_queue[i].flow_control.st_min_counter = tx_queue[i].flow_control.st_min; + } else { + tx_queue[i].flow_control.st_min_counter -= delta; + break; + } + if (tx_queue[i].flow_control.block_size == 1) { + tx_queue[i].state = ISOTP_WAITING_FLOW_CONTROL; + } + if (tx_queue[i].flow_control.block_size != 0) { + tx_queue[i].flow_control.block_size--; + } + + msg.consecutive.header = ISOTP_CONSECUTIVE_FRAME | (tx_queue[i].index & 0x0F); + size_t bytes_to_send = tx_queue[i].remaining <= MAX_ISOTP_DATA_CONSECUTIVE ? tx_queue[i].remaining : MAX_ISOTP_DATA_CONSECUTIVE; + memcpy(msg.consecutive.data, tx_queue[i].data, bytes_to_send); + + #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); + } else { + TRY_CAN_TRANSMIT(tx_queue[i].id, (const uint8_t *)&msg, MAX_CAN_DATA_SIZE); + } + #else + TRY_CAN_TRANSMIT(tx_queue[i].id, (const uint8_t *)&msg, bytes_to_send + 1); + #endif + + tx_queue[i].data += bytes_to_send; + tx_queue[i].remaining -= bytes_to_send; + tx_queue[i].index++; + + if (tx_queue[i].remaining == 0) { + tx_queue[i].state = ISOTP_DONE; + } + + if (tx_queue[i].state == ISOTP_READY) { + if (tx_queue[i].flow_control.st_min == 0) { // no delay needed, and we are not waiting for flow control -> send again + goto ISOTP_SEND_AGAIN; + } + } + break; + case ISOTP_WAITING_FLOW_CONTROL: + if (now - tx_queue[i].timestamp > ISOTP_FC_WAIT_TIMEOUT) { + tx_queue[i].state = ISOTP_FAILED; // Timeout handling + } + break; + case ISOTP_DONE: + case ISOTP_FAILED: + break; + } + } + last_called = now; + return ISOTP_OK; +} + +isotp_status_t isotp_handle_flow_control(uint16_t id, const uint8_t * data, size_t datalen) { + if (datalen < 3) { + return ISOTP_NOT_FLOW_CONTROL; + } + + if (data == NULL) { + return ISOTP_INVALID_PARAMETER; + } + + 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) { + continue; + } + + uint8_t type = data[0] & 0xF0; + uint8_t status = data[0] & 0x0F; + uint8_t block_size = data[1]; + uint8_t st_min = data[2]; + + if (type != ISOTP_FLOW_CONTROL) { + return ISOTP_NOT_FLOW_CONTROL; + } + + switch (status) { + case 0: // Continue + tx_queue[i].flow_control.state = ISOTP_CONTINUE; + break; + case 1: // Wait + tx_queue[i].flow_control.state = ISOTP_WAIT; + tx_queue[i].state = ISOTP_WAITING_FLOW_CONTROL; + tx_queue[i].timestamp = isotp_can_get_time(); // Update timestamp for timeout handling + return ISOTP_OK; + case 2: // Abort + tx_queue[i].flow_control.state = ISOTP_ABORT; + tx_queue[i].state = ISOTP_FAILED; + return ISOTP_OK; + default: + return ISOTP_NOT_FLOW_CONTROL; + } + + tx_queue[i].flow_control.block_size = block_size; + tx_queue[i].flow_control.st_min = st_min > 0x7F ? 1 : st_min; // round up to 1 ms + tx_queue[i].flow_control.st_min_counter = st_min > 0x7F ? 1 : st_min; + tx_queue[i].state = ISOTP_READY; + + return ISOTP_OK; + } + return ISOTP_INVALID_PARAMETER; +} + diff --git a/AMS_Master_Code/Core/Lib/isotp/isotp.h b/AMS_Master_Code/Core/Lib/isotp/isotp.h new file mode 100644 index 0000000..dd068a6 --- /dev/null +++ b/AMS_Master_Code/Core/Lib/isotp/isotp.h @@ -0,0 +1,41 @@ +#ifndef ISOTP_H +#define ISOTP_H + +#include +#include + +#define MAX_ISOTP_MESSAGES_IN_FLIGHT 10 +#define ISOTP_FC_WAIT_TIMEOUT 1000 // Timeout for flow control frames (ms) +//#define ISOTP_PADDING 0xAA // optional padding byte + +typedef enum : int16_t { + ISOTP_OK = 0, + ISOTP_ERROR = -1, + ISOTP_DATA_TOO_LONG = -2, + ISOTP_INVALID_PARAMETER = -3, + ISOTP_TOO_MANY_MESSAGES = -4, + ISOTP_MESSAGE_ALREADY_IN_FLIGHT = -5, + ISOTP_NOT_FLOW_CONTROL = -6, + ISOTP_CAN_TRANSMIT_ERROR = -7, +} isotp_status_t; + +static inline const char *isotp_status_to_string(isotp_status_t status) { + switch (status) { + case ISOTP_OK: return "ISOTP_OK"; + case ISOTP_ERROR: return "ISOTP_ERROR"; + case ISOTP_DATA_TOO_LONG: return "ISOTP_DATA_TOO_LONG"; + case ISOTP_INVALID_PARAMETER: return "ISOTP_INVALID_PARAMETER"; + case ISOTP_TOO_MANY_MESSAGES: return "ISOTP_TOO_MANY_MESSAGES"; + case ISOTP_MESSAGE_ALREADY_IN_FLIGHT: return "ISOTP_MESSAGE_ALREADY_IN_FLIGHT"; + case ISOTP_NOT_FLOW_CONTROL: return "ISOTP_NOT_FLOW_CONTROL"; + case ISOTP_CAN_TRANSMIT_ERROR: return "ISOTP_CAN_TRANSMIT_ERROR"; + default: return "UNKNOWN_STATUS"; + } +} + +isotp_status_t isotp_init(); +[[gnu::access(read_only, 2, 3)]] isotp_status_t isotp_add_message(uint16_t id, const uint8_t *data, size_t datalen); +isotp_status_t isotp_update(); +[[gnu::access(read_only, 2, 3)]] isotp_status_t isotp_handle_flow_control(uint16_t id, const uint8_t *data, size_t datalen); + +#endif // ISOTP_H \ No newline at end of file diff --git a/AMS_Master_Code/Core/Lib/logger/isotp_log_backend.c b/AMS_Master_Code/Core/Lib/logger/isotp_log_backend.c new file mode 100644 index 0000000..fcfd46f --- /dev/null +++ b/AMS_Master_Code/Core/Lib/logger/isotp_log_backend.c @@ -0,0 +1,194 @@ +#include "isotp_log_backend.h" +#include "log.h" +#include "isotp.h" + +#include + +/* Static variables */ +static bool isotp_backend_registered = false; + +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; + +/* Buffer for log messages */ +// The buffer is dynamically split to allow double-buffering while data is being sent +// Our ISO-TP implementation requires that the message data is live in memory until the message is fully sent +// Once streaming is enabled, we swap buffers after each message is sent to ensure that the data is not overwritten +static struct { + uint32_t write_pos; + uint32_t write_pos_2; + uint8_t buffer[ISOTP_LOG_BUFFER_1_BYTES + ISOTP_LOG_BUFFER_2_BYTES]; +} message_buffer = {}; + +/* Functions for the message buffer */ +static bool buffer_message(const log_message_t* message) { + if (message == NULL || message->message_length == 0) { + return false; + } + + /* Check if we have enough space - select the appropriate buffer */ + uint32_t* write_pos = using_buffer_2 ? &message_buffer.write_pos_2 : &message_buffer.write_pos; + + /* Calculate buffer offset - buffer 2 starts at ISOTP_LOG_BUFFER_1_BYTES */ + uint32_t buffer_offset = using_buffer_2 ? ISOTP_LOG_BUFFER_1_BYTES : 0; + uint32_t max_size = using_buffer_2 ? ISOTP_LOG_BUFFER_2_BYTES : ISOTP_LOG_BUFFER_1_BYTES; // Each buffer has its own size + + /* Check if we have enough space in the current buffer */ + if (*write_pos + message->message_length > max_size) { + buffer_dropped_messages = true; + return false; /* Buffer full */ + } + + /* Write message bytes */ + for (size_t i = 0; i < message->message_length; i++) { + message_buffer.buffer[buffer_offset + *write_pos] = message->message[i]; + (*write_pos)++; + } + + return true; +} + +/* Swap buffers and prepare for sending */ +static bool swap_buffers_for_sending() { + /* Don't swap if there's nothing to send */ + uint32_t* current_pos = using_buffer_2 ? &message_buffer.write_pos_2 : &message_buffer.write_pos; + if (*current_pos == 0) { + return false; + } + + /* Swap to the other buffer for future writes */ + using_buffer_2 = !using_buffer_2; + + /* Reset the new current buffer position */ + uint32_t* new_current_pos = using_buffer_2 ? &message_buffer.write_pos_2 : &message_buffer.write_pos; + *new_current_pos = 0; + + return true; +} + +/* ISO-TP backend functions */ +static void isotp_backend_init(void); +static bool isotp_backend_is_enabled(log_level_t level); +static void isotp_backend_write(const log_message_t* message); +static void isotp_backend_flush(void); +static void isotp_backend_clear(void); + +/* ISO-TP backend definition */ +static const log_backend_t isotp_backend = { + .name = "ISO-TP", + .init = isotp_backend_init, + .is_enabled = isotp_backend_is_enabled, + .write = isotp_backend_write, + .flush = isotp_backend_flush, + .clear = isotp_backend_clear +}; + +/* Return the ISO-TP backend definition */ +const log_backend_t* isotp_log_get_backend(void) { + return &isotp_backend; +} + +/* Check if the ISO-TP backend is registered */ +bool isotp_log_is_backend_registered(void) { + return isotp_backend_registered; +} + +/* Initialize the ISO-TP backend and register it */ +void isotp_log_backend_init(void) { + /* Register the backend */ + isotp_backend_registered = log_register_backend(&isotp_backend); + + /* Initialize the ISO-TP stack if registration was successful */ + if (isotp_backend_registered) { + isotp_init(); + } +} + +/* ISO-TP backend initialization */ +static void isotp_backend_init(void) { + /* Reset the message buffer */ + message_buffer.write_pos = 0; + message_buffer.write_pos_2 = 0; + buffer_dropped_messages = false; + using_buffer_2 = false; +} + +/* Check if the ISO-TP backend is enabled for the given log level */ +static bool isotp_backend_is_enabled(log_level_t level) { + return streaming_enabled ? + (level <= streaming_level) : + (level <= ISOTP_LOG_BUFFER_MIN_LEVEL); // log level where messages are buffered before streaming mode is enabled +} + +/* Write a message to the ISO-TP backend */ +static void isotp_backend_write(const log_message_t *message) { + if (message == NULL) { + return; + } + + /* Buffer the message regardless of streaming state */ + buffer_message(message); + + /* If streaming is enabled, schedule the messages for sending */ + if (streaming_enabled) { + /* Auto-flush if we have data in the buffer and are in streaming mode */ + isotp_backend_flush(); + } +} + +/* Process buffered log messages and send them via ISO-TP */ +void isotp_log_process(void) { + /* Process ISO-TP updates to send any pending messages */ + isotp_update(); +} + +/* Flush the ISO-TP backend */ +static void isotp_backend_flush(void) { + /* Swap buffers if we have data to send and we're ready to send */ + if (streaming_enabled) { + /* Get the buffer that is currently being written to */ + uint32_t buffer_offset = !using_buffer_2 ? 0 : ISOTP_LOG_BUFFER_1_BYTES; + uint32_t buffer_size = !using_buffer_2 ? message_buffer.write_pos : message_buffer.write_pos_2; + + /* 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, + message_buffer.buffer + buffer_offset, + buffer_size + ); + + if (status == ISOTP_OK) { + /* Successfully queued for sending, now swap the buffers */ + swap_buffers_for_sending(); + if (buffer_dropped_messages) { + log_warning("Buffered messages were dropped"); + buffer_dropped_messages = false; + } + } + } + } +} + +/* Clear functionality not really applicable for ISO-TP */ +static void isotp_backend_clear(void) { +#if USE_ANSI_ESCAPE_CODES + /* Clear the console using ANSI escape codes */ + isotp_backend_write(&(const log_message_t){.message = "\033[2J\033[H", .message_length = 7}); +#endif +} + +/* Public function to check if the backend is enabled for a given level */ +bool isotp_log_is_enabled(log_level_t level) { + return isotp_backend_is_enabled(level); +} + +/* Public function to enable streaming of log messages */ +void isotp_log_enable_streaming(log_level_t level) { + streaming_enabled = true; + streaming_level = level; + isotp_backend_flush(); + log_info("Now streaming log messages over CAN at level %s", log_level_names[level]); +} diff --git a/AMS_Master_Code/Core/Lib/logger/isotp_log_backend.h b/AMS_Master_Code/Core/Lib/logger/isotp_log_backend.h new file mode 100644 index 0000000..a0cc273 --- /dev/null +++ b/AMS_Master_Code/Core/Lib/logger/isotp_log_backend.h @@ -0,0 +1,26 @@ +#ifndef ISOTP_LOG_BACKEND_H +#define ISOTP_LOG_BACKEND_H +#include "log.h" + +/* ISO-TP Backend Configuration */ +#define ISOTP_LOG_BUFFER_MIN_LEVEL LOG_LEVEL_WARNING // log level where messages are buffered before streaming mode is enabled +#define ISOTP_LOG_BUFFER_1_BYTES 1024 // Primary buffer, used when streaming is disabled +#define ISOTP_LOG_BUFFER_2_BYTES 256 // Secondary buffer, used for double-buffering during streaming, can be smaller + + +/* ISO-TP CAN ID configuration */ +#define ISOTP_LOG_CAN_ID 0x123 // CAN ID to use for ISO-TP log messages + +/* ISO-TP backend API */ +void isotp_log_backend_init(void); +bool isotp_log_is_backend_registered(void); +const log_backend_t* isotp_log_get_backend(void); +void isotp_log_enable_streaming(log_level_t level); + +/* Backend status check function */ +bool isotp_log_is_enabled(log_level_t level); + +/* Process function - should be called periodically to send buffered messages */ +void isotp_log_process(void); + +#endif /* ISOTP_LOG_BACKEND_H */ \ No newline at end of file diff --git a/AMS_Master_Code/Core/Lib/logger/log.h b/AMS_Master_Code/Core/Lib/logger/log.h index 6ee2982..0e2c2ef 100644 --- a/AMS_Master_Code/Core/Lib/logger/log.h +++ b/AMS_Master_Code/Core/Lib/logger/log.h @@ -10,7 +10,7 @@ #ifndef __LOG_H #define __LOG_H -#include "stm32h7xx_hal.h" +#include #include #include #include diff --git a/AMS_Master_Code/Core/Src/can.c b/AMS_Master_Code/Core/Src/can.c index 849fef5..23d6484 100644 --- a/AMS_Master_Code/Core/Src/can.c +++ b/AMS_Master_Code/Core/Src/can.c @@ -12,6 +12,14 @@ #include #include +bool isotp_can_transmit(uint16_t id, const uint8_t *data, size_t datalen) { + return ftcan_transmit(id, data, datalen) == HAL_OK; +} + +uint32_t isotp_can_get_time() { + return HAL_GetTick(); +} + void can_init(FDCAN_HandleTypeDef *handle) { ftcan_init(handle); ftcan_add_filter(CAN_ID_SHUNT_BASE, 0xFF0);