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

add can backend to logger

Browse Source
This commit is contained in:
Kilian Bracher 2025-04-22 15:27:37 +02:00
parent 99eefa4283
commit 354b6dc453
Signed by: k.bracher
SSH Key Fingerprint: SHA256:mXpyZkK7RDiJ7qeHCKJX108woM0cl5TrCvNBJASu6lM
6 changed files with 525 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

255
AMS_Master_Code/Core/Lib/isotp/isotp.c Normal file
View File

@ -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 <stdint.h>
#include <stddef.h>
#include <string.h>
#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;
}

41
AMS_Master_Code/Core/Lib/isotp/isotp.h Normal file
View File

@ -0,0 +1,41 @@
#ifndef ISOTP_H
#define ISOTP_H
#include <stddef.h>
#include <stdint.h>
#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

194
AMS_Master_Code/Core/Lib/logger/isotp_log_backend.c Normal file
View File

@ -0,0 +1,194 @@
#include "isotp_log_backend.h"
#include "log.h"
#include "isotp.h"
#include <string.h>
/* 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]);
}

26
AMS_Master_Code/Core/Lib/logger/isotp_log_backend.h Normal file
View File

@ -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 */

2
AMS_Master_Code/Core/Lib/logger/log.h
View File

@ -10,7 +10,7 @@
#ifndef __LOG_H
#define __LOG_H
#include "stm32h7xx_hal.h"
#include <stdint.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdbool.h>

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

@ -12,6 +12,14 @@
#include <math.h>
#include <stdint.h>
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);