From 8d17995768010485caf4f1da7e7801c48b8a1ea5 Mon Sep 17 00:00:00 2001 From: Troels Jessen Date: Thu, 5 Mar 2026 08:23:31 +0100 Subject: [PATCH 1/9] Use CFP1 defines Replacing sizeof(...) macros when defines are anyway there and used in certain cases --- src/interfaces/csp_if_can.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c index f50897e60..729a3b104 100644 --- a/src/interfaces/csp_if_can.c +++ b/src/interfaces/csp_if_can.c @@ -64,7 +64,7 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u case CFP_BEGIN: /* Discard packet if DLC is less than CSP id + CSP length fields */ - if (dlc < (sizeof(uint32_t) + sizeof(uint16_t))) { + if (dlc < CFP1_DATA_OFFSET) { csp_dbg_can_errno = CSP_DBG_CAN_ERR_SHORT_BEGIN; iface->frame++; csp_can_pbuf_free(ifdata, packet, 1, task_woken); @@ -74,11 +74,11 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u csp_id_setup_rx(packet); /* Copy CSP identifier (header) */ - memcpy(packet->frame_begin, data, sizeof(uint32_t)); - packet->frame_length += sizeof(uint32_t); + memcpy(packet->frame_begin, data, CFP1_CSP_HEADER_SIZE); + packet->frame_length += CFP1_CSP_HEADER_SIZE; /* Copy CSP length (of data) */ - memcpy(&(packet->length), data + sizeof(uint32_t), sizeof(packet->length)); + memcpy(&(packet->length), data + CFP1_CSP_HEADER_SIZE, sizeof(packet->length)); packet->length = be16toh(packet->length); /* Overflow: check if incoming frame data length is larger than buffer length */ @@ -92,7 +92,7 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u packet->rx_count = 0; /* Set offset to prevent CSP header from being copied to CSP data */ - offset = sizeof(uint32_t) + sizeof(uint16_t); + offset = CFP1_DATA_OFFSET; /* Set remain field - increment to include begin packet */ packet->remain = CFP_REMAIN(id) + 1; From 02270c12269b4be785f71a5f58527b8164ce335f Mon Sep 17 00:00:00 2001 From: Troels Jessen Date: Thu, 5 Mar 2026 08:33:02 +0100 Subject: [PATCH 2/9] Move ETH pbuf header to private folder This is to align with CAN implementation --- src/drivers/eth/eth_linux.c | 5 ++++- src/interfaces/csp_if_eth.c | 2 +- src/interfaces/csp_if_eth_pbuf.c | 2 +- {include/csp => src}/interfaces/csp_if_eth_pbuf.h | 4 ---- 4 files changed, 6 insertions(+), 7 deletions(-) rename {include/csp => src}/interfaces/csp_if_eth_pbuf.h (96%) diff --git a/src/drivers/eth/eth_linux.c b/src/drivers/eth/eth_linux.c index 449d116cd..fdbf17d6b 100644 --- a/src/drivers/eth/eth_linux.c +++ b/src/drivers/eth/eth_linux.c @@ -7,12 +7,15 @@ #include #include +#include +#include +#include +#include #include #include #include #include -#include #include #include diff --git a/src/interfaces/csp_if_eth.c b/src/interfaces/csp_if_eth.c index 0d81d6246..732d0154d 100644 --- a/src/interfaces/csp_if_eth.c +++ b/src/interfaces/csp_if_eth.c @@ -1,6 +1,6 @@ #include -#include +#include "csp_if_eth_pbuf.h" #include #include diff --git a/src/interfaces/csp_if_eth_pbuf.c b/src/interfaces/csp_if_eth_pbuf.c index 3d7473645..c5bb609dc 100644 --- a/src/interfaces/csp_if_eth_pbuf.c +++ b/src/interfaces/csp_if_eth_pbuf.c @@ -4,7 +4,7 @@ #include #include -#include +#include "csp_if_eth_pbuf.h" /* Buffer element timeout in ms */ #define PBUF_TIMEOUT_MS 1000 diff --git a/include/csp/interfaces/csp_if_eth_pbuf.h b/src/interfaces/csp_if_eth_pbuf.h similarity index 96% rename from include/csp/interfaces/csp_if_eth_pbuf.h rename to src/interfaces/csp_if_eth_pbuf.h index 01e1e0e94..9d0556cd0 100644 --- a/include/csp/interfaces/csp_if_eth_pbuf.h +++ b/src/interfaces/csp_if_eth_pbuf.h @@ -27,11 +27,7 @@ #pragma once #include -#include #include -#include -#include -#include #include From b57140cd538e69621d281fd9d2cfca0ad752c391 Mon Sep 17 00:00:00 2001 From: Troels Jessen Date: Wed, 4 Mar 2026 11:05:20 +0100 Subject: [PATCH 3/9] Use csp_buffer_get_always only when incoming packets is not to be routed The functionality is used to stall a module in case it is running out of buffers, potentially causing a reboot. Nevertheless, running out of buffers is a valid situation if a module is routing between two physical networks with different capatilities (e.g. CAN vs ETH). --- src/interfaces/csp_if_can.c | 8 ++++++++ src/interfaces/csp_if_can_pbuf.c | 14 +++++++++++++- src/interfaces/csp_if_zmqhub.c | 9 ++++++++- 3 files changed, 29 insertions(+), 2 deletions(-) diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c index 729a3b104..f96a3255c 100644 --- a/src/interfaces/csp_if_can.c +++ b/src/interfaces/csp_if_can.c @@ -50,6 +50,10 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u if (packet == NULL) { if (CFP_TYPE(id) == CFP_BEGIN) { packet = csp_can_pbuf_new(ifdata, id, task_woken); + if (packet == NULL) { + iface->drop++; + return CSP_ERR_NOBUFS; + } } else { iface->frame++; return CSP_ERR_INVAL; @@ -261,6 +265,10 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u if (packet == NULL) { if (id & (CFP2_BEGIN_MASK << CFP2_BEGIN_OFFSET)) { packet = csp_can_pbuf_new(ifdata, id, task_woken); + if (packet == NULL) { + iface->drop++; + return CSP_ERR_NOBUFS; + } } else { iface->frame++; return CSP_ERR_INVAL; diff --git a/src/interfaces/csp_if_can_pbuf.c b/src/interfaces/csp_if_can_pbuf.c index faca94e1e..c53a2d558 100644 --- a/src/interfaces/csp_if_can_pbuf.c +++ b/src/interfaces/csp_if_can_pbuf.c @@ -6,6 +6,7 @@ #include #include #include +#include #include "../csp_buffer_private.h" @@ -51,7 +52,18 @@ csp_packet_t * csp_can_pbuf_new(csp_can_interface_data_t * ifdata, uint32_t id, uint32_t now = (task_woken) ? csp_get_ms_isr() : csp_get_ms(); - csp_packet_t * packet = (task_woken) ? csp_buffer_get_always_isr() : csp_buffer_get_always(); + csp_packet_t * packet; + if (csp_iflist_get_by_addr(((id >> CFP2_DST_OFFSET) & CFP2_DST_MASK)) != NULL) { + /* The packet is for us, make sure we don't silently ignore the situation if we can't process it */ + packet = (task_woken) ? csp_buffer_get_always_isr() : csp_buffer_get_always(); + } else { + /* The packet is not for us, it is ok to drop it if we don't have enough buffers*/ + packet = (task_woken) ? csp_buffer_get_isr(0) : csp_buffer_get(0); + } + + if (packet == NULL) { + return packet; + } packet->last_used = now; packet->cfpid = id; diff --git a/src/interfaces/csp_if_zmqhub.c b/src/interfaces/csp_if_zmqhub.c index b726decae..4b3ab9466 100644 --- a/src/interfaces/csp_if_zmqhub.c +++ b/src/interfaces/csp_if_zmqhub.c @@ -130,7 +130,14 @@ static void * csp_zmqhub_task(void * param) { } // Create new csp packet - packet = csp_buffer_get(0); + if (csp_iflist_get_by_addr(*((uint16_t*)&rx_data[2]) & 0x3FFF) != NULL) { + /* The packet is for us, make sure we don't silently ignore the situation if we can't process it */ + packet = csp_buffer_get_always(); + } else { + /* The packet is not for us, it is ok to drop it if we don't have enough buffers*/ + packet = csp_buffer_get(0); + } + if (packet == NULL) { csp_print("RX %s: Failed to get csp_buffer(%u)\n", drv->iface.name, datalen); zmq_msg_close(&msg); From e1d8d56c95e1beeebf51f521397899a80266711e Mon Sep 17 00:00:00 2001 From: Troels Jessen Date: Thu, 5 Mar 2026 07:35:47 +0100 Subject: [PATCH 4/9] Use csp_buffer_get_always only for packets to me Currently implemented for CAN, ETH and ZMQ --- include/csp/csp_id.h | 8 +++ src/csp_id.c | 84 ++++++++++++++++++-------------- src/interfaces/csp_if_can.c | 39 ++++++++------- src/interfaces/csp_if_can_pbuf.c | 6 +-- src/interfaces/csp_if_can_pbuf.h | 2 +- src/interfaces/csp_if_eth.c | 6 ++- src/interfaces/csp_if_eth_pbuf.c | 17 +++++-- src/interfaces/csp_if_eth_pbuf.h | 4 +- src/interfaces/csp_if_zmqhub.c | 8 +-- 9 files changed, 106 insertions(+), 68 deletions(-) diff --git a/include/csp/csp_id.h b/include/csp/csp_id.h index 5bdbab9cf..fc38623b3 100644 --- a/include/csp/csp_id.h +++ b/include/csp/csp_id.h @@ -15,6 +15,14 @@ extern "C" { */ void csp_id_prepend(csp_packet_t * packet); +/** + * Extract CSP header from a 4 (CSP1) or 6 (CSP2) byte array + * + * @param The first bytes of a CSP packet, representing CSP the header structure + * @return The extracted header ID + */ +csp_id_t csp_id_extract(const uint8_t * data); + /** * Strip CSP header fields from the packet's data buffer. * diff --git a/src/csp_id.c b/src/csp_id.c index 5e7dc7d78..6d7ab5509 100644 --- a/src/csp_id.c +++ b/src/csp_id.c @@ -63,16 +63,11 @@ static void csp_id1_prepend(csp_packet_t * packet, bool cspv1_fixup) { memcpy(packet->frame_begin, &id1, CSP_ID1_HEADER_SIZE); } -static int csp_id1_strip(csp_packet_t * packet, bool cspv1_fixup) { - - if (packet->frame_length < CSP_ID1_HEADER_SIZE) { - return -1; - } +static csp_id_t csp_id1_extract(const uint8_t * data, bool cspv1_fixup) { /* Get 32 bit in network byte order */ uint32_t id1_raw = 0; - memcpy(&id1_raw, packet->frame_begin, CSP_ID1_HEADER_SIZE); - packet->length = packet->frame_length - CSP_ID1_HEADER_SIZE; + memcpy(&id1_raw, data, CSP_ID1_HEADER_SIZE); /* Convert to host order */ uint32_t id1 = be32toh(id1_raw); @@ -83,14 +78,15 @@ static int csp_id1_strip(csp_packet_t * packet, bool cspv1_fixup) { /* Parse header: * Now in easy to work with in 32 bit register */ - packet->id.pri = (id1 >> CSP_ID1_PRIO_OFFSET) & CSP_ID1_PRIO_MASK; - packet->id.dst = (id1 >> CSP_ID1_DST_OFFSET) & CSP_ID1_DST_MASK; - packet->id.src = (id1 >> CSP_ID1_SRC_OFFSET) & CSP_ID1_SRC_MASK; - packet->id.dport = (id1 >> CSP_ID1_DPORT_OFFSET) & CSP_ID1_DPORT_MASK; - packet->id.sport = (id1 >> CSP_ID1_SPORT_OFFSET) & CSP_ID1_SPORT_MASK; - packet->id.flags = (id1 >> CSP_ID1_FLAGS_OFFSET) & CSP_ID1_FLAGS_MASK; - - return 0; + csp_id_t id; + id.pri = (id1 >> CSP_ID1_PRIO_OFFSET) & CSP_ID1_PRIO_MASK; + id.dst = (id1 >> CSP_ID1_DST_OFFSET) & CSP_ID1_DST_MASK; + id.src = (id1 >> CSP_ID1_SRC_OFFSET) & CSP_ID1_SRC_MASK; + id.dport = (id1 >> CSP_ID1_DPORT_OFFSET) & CSP_ID1_DPORT_MASK; + id.sport = (id1 >> CSP_ID1_SPORT_OFFSET) & CSP_ID1_SPORT_MASK; + id.flags = (id1 >> CSP_ID1_FLAGS_OFFSET) & CSP_ID1_FLAGS_MASK; + + return id; } static void csp_id1_setup_rx(csp_packet_t * packet) { @@ -147,17 +143,12 @@ static void csp_id2_prepend(csp_packet_t * packet) { memcpy(packet->frame_begin, &id2, CSP_ID2_HEADER_SIZE); } -static int csp_id2_strip(csp_packet_t * packet) { - - if (packet->frame_length < CSP_ID2_HEADER_SIZE) { - return -1; - } +static csp_id_t csp_id2_extract(const uint8_t* data) { /* Get 48 bit in network byte order: * Most significant byte ends in byte 0 */ uint64_t id2 = 0; - memcpy(&id2, packet->frame_begin, CSP_ID2_HEADER_SIZE); - packet->length = packet->frame_length - CSP_ID2_HEADER_SIZE; + memcpy(&id2, data, CSP_ID2_HEADER_SIZE); /* Convert to host order: * Most significant byte ends in byte 7, we then shift down @@ -166,14 +157,15 @@ static int csp_id2_strip(csp_packet_t * packet) { /* Parse header: * Now in easy to work with in 32 bit register */ - packet->id.pri = (id2 >> CSP_ID2_PRIO_OFFSET) & CSP_ID2_PRIO_MASK; - packet->id.dst = (id2 >> CSP_ID2_DST_OFFSET) & CSP_ID2_DST_MASK; - packet->id.src = (id2 >> CSP_ID2_SRC_OFFSET) & CSP_ID2_SRC_MASK; - packet->id.dport = (id2 >> CSP_ID2_DPORT_OFFSET) & CSP_ID2_DPORT_MASK; - packet->id.sport = (id2 >> CSP_ID2_SPORT_OFFSET) & CSP_ID2_SPORT_MASK; - packet->id.flags = (id2 >> CSP_ID2_FLAGS_OFFSET) & CSP_ID2_FLAGS_MASK; - - return 0; + csp_id_t id; + id.pri = (id2 >> CSP_ID2_PRIO_OFFSET) & CSP_ID2_PRIO_MASK; + id.dst = (id2 >> CSP_ID2_DST_OFFSET) & CSP_ID2_DST_MASK; + id.src = (id2 >> CSP_ID2_SRC_OFFSET) & CSP_ID2_SRC_MASK; + id.dport = (id2 >> CSP_ID2_DPORT_OFFSET) & CSP_ID2_DPORT_MASK; + id.sport = (id2 >> CSP_ID2_SPORT_OFFSET) & CSP_ID2_SPORT_MASK; + id.flags = (id2 >> CSP_ID2_FLAGS_OFFSET) & CSP_ID2_FLAGS_MASK; + + return id; } static void csp_id2_setup_rx(csp_packet_t * packet) { @@ -197,12 +189,22 @@ void csp_id_prepend(csp_packet_t * packet) { } } -int csp_id_strip(csp_packet_t * packet) { +csp_id_t csp_id_extract(const uint8_t * data) { if (csp_conf.version == 2) { - return csp_id2_strip(packet); + return csp_id2_extract(data); } else { - return csp_id1_strip(packet, false); + return csp_id1_extract(data, false); + } +} + +int csp_id_strip(csp_packet_t * packet) { + if (packet->frame_length < csp_id_get_header_size()) { + return -1; } + + packet->id = csp_id_extract(packet->frame_begin); + packet->length = packet->frame_length - csp_id_get_header_size(); + return 0; } #if (CSP_FIXUP_V1_ZMQ_LITTLE_ENDIAN) @@ -216,14 +218,24 @@ void csp_id_prepend_fixup_cspv1(csp_packet_t * packet) { } } -int csp_id_strip_fixup_cspv1(csp_packet_t * packet) { +csp_id_t csp_id_extract_fixup_cspv1(const uint8_t * data) { if (csp_conf.version == 2) { - return csp_id2_strip(packet); + return csp_id2_extract(data); } else { - return csp_id1_strip(packet, true); + return csp_id1_extract(data, true); } } +int csp_id_strip_fixup_cspv1(csp_packet_t * packet) { + if (packet->frame_length < csp_id_get_header_size()) { + return -1; + } + + packet->id = csp_id_extract_cspv1(packet->frame_begin); + packet->length = packet->frame_length - csp_id_get_header_size(); + return 0; +} + #endif int csp_id_setup_rx(csp_packet_t * packet) { diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c index f96a3255c..dce90065d 100644 --- a/src/interfaces/csp_if_can.c +++ b/src/interfaces/csp_if_can.c @@ -49,11 +49,18 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u csp_packet_t * packet = csp_can_pbuf_find(ifdata, id, CFP_ID_CONN_MASK, task_woken); if (packet == NULL) { if (CFP_TYPE(id) == CFP_BEGIN) { - packet = csp_can_pbuf_new(ifdata, id, task_woken); + uint8_t header[CFP1_CSP_HEADER_SIZE]; + /* Copy first 4 from data as they represent the CSP header, the data field is in network order */ + memcpy(header, data, CFP1_CSP_HEADER_SIZE); + csp_id_t csp_id = csp_id_extract(header); + packet = csp_can_pbuf_new(ifdata, id, csp_id, task_woken); + packet->id = csp_id; if (packet == NULL) { iface->drop++; return CSP_ERR_NOBUFS; } + memcpy(packet->frame_begin, data, CFP1_CSP_HEADER_SIZE); + packet->frame_length += CFP1_CSP_HEADER_SIZE; } else { iface->frame++; return CSP_ERR_INVAL; @@ -77,12 +84,8 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u csp_id_setup_rx(packet); - /* Copy CSP identifier (header) */ - memcpy(packet->frame_begin, data, CFP1_CSP_HEADER_SIZE); - packet->frame_length += CFP1_CSP_HEADER_SIZE; - /* Copy CSP length (of data) */ - memcpy(&(packet->length), data + CFP1_CSP_HEADER_SIZE, sizeof(packet->length)); + memcpy(&(packet->length), data + CFP1_CSP_HEADER_SIZE, CFP1_DATA_LEN_SIZE); packet->length = be16toh(packet->length); /* Overflow: check if incoming frame data length is larger than buffer length */ @@ -264,11 +267,23 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u csp_packet_t * packet = csp_can_pbuf_find(ifdata, id, CFP2_ID_CONN_MASK, task_woken); if (packet == NULL) { if (id & (CFP2_BEGIN_MASK << CFP2_BEGIN_OFFSET)) { - packet = csp_can_pbuf_new(ifdata, id, task_woken); + uint8_t header[6]; + uint16_t first_two = id >> CFP2_DST_OFFSET; + first_two = htobe16(first_two); + memcpy(header, &first_two, 2); + + /* Copy next 4 from data, the data field is in network order */ + memcpy(&header[2], data, 4); + csp_id_t csp_id = csp_id_extract(header); + packet = csp_can_pbuf_new(ifdata, id, csp_id, task_woken); if (packet == NULL) { iface->drop++; return CSP_ERR_NOBUFS; } + packet->id = csp_id; + memcpy(packet->frame_begin, header, csp_id_get_header_size()); + packet->frame_length = csp_id_get_header_size(); + packet->length = 0; } else { iface->frame++; return CSP_ERR_INVAL; @@ -293,16 +308,6 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u * Because the id field has already been converted in memory to a 32-bit * host-order field, extract the first two bytes and convert back to * network order */ - uint16_t first_two = id >> CFP2_DST_OFFSET; - first_two = htobe16(first_two); - memcpy(packet->frame_begin, &first_two, 2); - - /* Copy next 4 from data, the data field is in network order */ - memcpy(&packet->frame_begin[2], data, 4); - - packet->frame_length = 6; - packet->length = 0; - /* Move RX offset for incoming data */ data += 4; dlc -= 4; diff --git a/src/interfaces/csp_if_can_pbuf.c b/src/interfaces/csp_if_can_pbuf.c index c53a2d558..4fc761bd8 100644 --- a/src/interfaces/csp_if_can_pbuf.c +++ b/src/interfaces/csp_if_can_pbuf.c @@ -46,14 +46,14 @@ void csp_can_pbuf_free(csp_can_interface_data_t * ifdata, csp_packet_t * buffer, } -csp_packet_t * csp_can_pbuf_new(csp_can_interface_data_t * ifdata, uint32_t id, int * task_woken) { +csp_packet_t * csp_can_pbuf_new(csp_can_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, int * task_woken) { csp_can_pbuf_cleanup(ifdata, task_woken); uint32_t now = (task_woken) ? csp_get_ms_isr() : csp_get_ms(); - csp_packet_t * packet; - if (csp_iflist_get_by_addr(((id >> CFP2_DST_OFFSET) & CFP2_DST_MASK)) != NULL) { + csp_packet_t * packet = NULL; + if (csp_iflist_get_by_addr(csp_id.dst) != NULL) { /* The packet is for us, make sure we don't silently ignore the situation if we can't process it */ packet = (task_woken) ? csp_buffer_get_always_isr() : csp_buffer_get_always(); } else { diff --git a/src/interfaces/csp_if_can_pbuf.h b/src/interfaces/csp_if_can_pbuf.h index 08e53b70a..737fad1f4 100644 --- a/src/interfaces/csp_if_can_pbuf.h +++ b/src/interfaces/csp_if_can_pbuf.h @@ -11,6 +11,6 @@ typedef struct { } csp_can_pbuf_element_t; void csp_can_pbuf_free(csp_can_interface_data_t * ifdata, csp_packet_t * buffer, int buf_free, int * task_woken); -csp_packet_t * csp_can_pbuf_new(csp_can_interface_data_t * ifdata, uint32_t id, int * task_woken); +csp_packet_t * csp_can_pbuf_new(csp_can_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, int * task_woken); csp_packet_t * csp_can_pbuf_find(csp_can_interface_data_t * ifdata, uint32_t id, uint32_t mask, int * task_woken); void csp_can_pbuf_cleanup(csp_can_interface_data_t * ifdata, int * task_woken); diff --git a/src/interfaces/csp_if_eth.c b/src/interfaces/csp_if_eth.c index 732d0154d..9f4542b97 100644 --- a/src/interfaces/csp_if_eth.c +++ b/src/interfaces/csp_if_eth.c @@ -180,7 +180,11 @@ int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t recei return CSP_ERR_INVAL; } - csp_packet_t * packet = csp_eth_pbuf_find(ifdata, packet_id, task_woken); + uint8_t csp_header[6]; + memcpy(csp_header, eth_frame->frame_begin, 6); + csp_id_t csp_id = csp_id_extract(csp_header); + + csp_packet_t * packet = csp_eth_pbuf_find(ifdata, packet_id, csp_id, task_woken); if (packet == NULL) { iface->drop++; diff --git a/src/interfaces/csp_if_eth_pbuf.c b/src/interfaces/csp_if_eth_pbuf.c index c5bb609dc..3f22973a8 100644 --- a/src/interfaces/csp_if_eth_pbuf.c +++ b/src/interfaces/csp_if_eth_pbuf.c @@ -6,6 +6,8 @@ #include #include "csp_if_eth_pbuf.h" +#include "../csp_buffer_private.h" + /* Buffer element timeout in ms */ #define PBUF_TIMEOUT_MS 1000 @@ -41,11 +43,18 @@ void csp_eth_pbuf_free(csp_eth_interface_data_t * ifdata, csp_packet_t * buffer, } -csp_packet_t * csp_eth_pbuf_new(csp_eth_interface_data_t * ifdata, uint32_t id, uint32_t now, int * task_woken) { +csp_packet_t * csp_eth_pbuf_new(csp_eth_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, uint32_t now, int * task_woken) { csp_eth_pbuf_cleanup(ifdata, now, task_woken); - csp_packet_t * packet = (task_woken) ? csp_buffer_get_isr(0) : csp_buffer_get(0); + csp_packet_t * packet = NULL; + if (csp_iflist_get_by_addr(csp_id.dst) != NULL) { + /* The packet is for us, make sure we don't silently ignore the situation if we can't process it */ + packet = (task_woken) ? csp_buffer_get_always_isr() : csp_buffer_get_always(); + } else { + /* The packet is not for us, it is ok to drop it if we don't have enough buffers*/ + packet = (task_woken) ? csp_buffer_get_isr(0) : csp_buffer_get(0); + } if (packet == NULL) { return NULL; } @@ -92,7 +101,7 @@ void csp_eth_pbuf_cleanup(csp_eth_interface_data_t * ifdata, uint32_t now, int * } -csp_packet_t * csp_eth_pbuf_find(csp_eth_interface_data_t * ifdata, uint32_t id, int * task_woken) { +csp_packet_t * csp_eth_pbuf_find(csp_eth_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, int * task_woken) { uint32_t now = (task_woken) ? csp_get_ms_isr() : csp_get_ms(); @@ -106,6 +115,6 @@ csp_packet_t * csp_eth_pbuf_find(csp_eth_interface_data_t * ifdata, uint32_t id, packet = packet->next; } - return csp_eth_pbuf_new(ifdata, id, now, task_woken); + return csp_eth_pbuf_new(ifdata, id, csp_id, now, task_woken); } diff --git a/src/interfaces/csp_if_eth_pbuf.h b/src/interfaces/csp_if_eth_pbuf.h index 9d0556cd0..f41538919 100644 --- a/src/interfaces/csp_if_eth_pbuf.h +++ b/src/interfaces/csp_if_eth_pbuf.h @@ -43,8 +43,8 @@ typedef struct { } csp_eth_pbuf_element_t; void csp_eth_pbuf_free(csp_eth_interface_data_t * ifdata, csp_packet_t * buffer, int buf_free, int * task_woken); -csp_packet_t * csp_eth_pbuf_new(csp_eth_interface_data_t * ifdata, uint32_t id, uint32_t now, int * task_woken); -csp_packet_t * csp_eth_pbuf_find(csp_eth_interface_data_t * ifdata, uint32_t id, int * task_woken); +csp_packet_t * csp_eth_pbuf_new(csp_eth_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, uint32_t now, int * task_woken); +csp_packet_t * csp_eth_pbuf_find(csp_eth_interface_data_t * ifdata, uint32_t id, csp_id_t csp_id, int * task_woken); void csp_eth_pbuf_cleanup(csp_eth_interface_data_t * ifdata, uint32_t now, int * task_woken); #ifdef __cplusplus diff --git a/src/interfaces/csp_if_zmqhub.c b/src/interfaces/csp_if_zmqhub.c index 4b3ab9466..acf885572 100644 --- a/src/interfaces/csp_if_zmqhub.c +++ b/src/interfaces/csp_if_zmqhub.c @@ -129,6 +129,10 @@ static void * csp_zmqhub_task(void * param) { continue; } + // Copy the data from zmq to csp + uint8_t * rx_data = zmq_msg_data(&msg); + rx_data = csp_zmqhub_fixup_cspv1_del_dest_addr(rx_data, &datalen); + // Create new csp packet if (csp_iflist_get_by_addr(*((uint16_t*)&rx_data[2]) & 0x3FFF) != NULL) { /* The packet is for us, make sure we don't silently ignore the situation if we can't process it */ @@ -144,10 +148,6 @@ static void * csp_zmqhub_task(void * param) { continue; } - // Copy the data from zmq to csp - uint8_t * rx_data = zmq_msg_data(&msg); - rx_data = csp_zmqhub_fixup_cspv1_del_dest_addr(rx_data, &datalen); - csp_id_setup_rx(packet); memcpy(packet->frame_begin, rx_data, datalen); From 90a800db35828bc768c1bbb09ea437c608622cba Mon Sep 17 00:00:00 2001 From: Troels Jessen Date: Thu, 5 Mar 2026 09:16:23 +0100 Subject: [PATCH 5/9] Missing include --- src/interfaces/csp_if_zmqhub.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/interfaces/csp_if_zmqhub.c b/src/interfaces/csp_if_zmqhub.c index acf885572..63d7a9b9e 100644 --- a/src/interfaces/csp_if_zmqhub.c +++ b/src/interfaces/csp_if_zmqhub.c @@ -14,6 +14,7 @@ #include #include "../csp_macro.h" +#include "../csp_buffer_private.h" /** * ZMQ destination size (for libcsp1 backwards compatibility) From 65983f31bb5909d51de963a4d0b6633d927b44dc Mon Sep 17 00:00:00 2001 From: Troels Jessen Date: Thu, 5 Mar 2026 11:06:10 +0100 Subject: [PATCH 6/9] CAN fixes --- src/interfaces/csp_if_can.c | 56 ++++++++++++++++++------------------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c index dce90065d..1d3f35500 100644 --- a/src/interfaces/csp_if_can.c +++ b/src/interfaces/csp_if_can.c @@ -267,6 +267,18 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u csp_packet_t * packet = csp_can_pbuf_find(ifdata, id, CFP2_ID_CONN_MASK, task_woken); if (packet == NULL) { if (id & (CFP2_BEGIN_MASK << CFP2_BEGIN_OFFSET)) { + + /* Discard packet if DLC is less than CSP id + CSP length fields */ + if (dlc < 4) { + csp_dbg_can_errno = CSP_DBG_CAN_ERR_SHORT_BEGIN; + iface->frame++; + return CSP_ERR_INVAL; + } + + /* Copy first 2 bytes from CFP 2.0 header: + * Because the id field has already been converted in memory to a 32-bit + * host-order field, extract the first two bytes and convert back to + * network order */ uint8_t header[6]; uint16_t first_two = id >> CFP2_DST_OFFSET; first_two = htobe16(first_two); @@ -274,16 +286,28 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u /* Copy next 4 from data, the data field is in network order */ memcpy(&header[2], data, 4); + + /* Move RX offset for incoming data */ + data += 4; + dlc -= 4; + + /* Create CSP header info from the first bytes received */ csp_id_t csp_id = csp_id_extract(header); packet = csp_can_pbuf_new(ifdata, id, csp_id, task_woken); if (packet == NULL) { iface->drop++; return CSP_ERR_NOBUFS; } + + /* Prepare new CSP packet by adding header as extracted */ + csp_id_setup_rx(packet); packet->id = csp_id; memcpy(packet->frame_begin, header, csp_id_get_header_size()); packet->frame_length = csp_id_get_header_size(); packet->length = 0; + + /* Set next expected fragment counter to be 1 */ + packet->rx_count = 1; } else { iface->frame++; return CSP_ERR_INVAL; @@ -291,32 +315,8 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u } - /* BEGIN */ - if (id & (CFP2_BEGIN_MASK << CFP2_BEGIN_OFFSET)) { - - /* Discard packet if DLC is less than CSP id + CSP length fields */ - if (dlc < 4) { - csp_dbg_can_errno = CSP_DBG_CAN_ERR_SHORT_BEGIN; - iface->frame++; - csp_can_pbuf_free(ifdata, packet, 1, task_woken); - return CSP_ERR_INVAL; - } - - csp_id_setup_rx(packet); - - /* Copy first 2 bytes from CFP 2.0 header: - * Because the id field has already been converted in memory to a 32-bit - * host-order field, extract the first two bytes and convert back to - * network order */ - /* Move RX offset for incoming data */ - data += 4; - dlc -= 4; - - /* Set next expected fragment counter to be 1 */ - packet->rx_count = 1; - - /* FRAGMENT */ - } else { + /* FRAGMENT */ + if (!(id & (CFP2_BEGIN_MASK << CFP2_BEGIN_OFFSET))) { int fragment_counter = (id >> CFP2_FC_OFFSET) & CFP2_FC_MASK; @@ -350,8 +350,8 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u /* END */ if (id & (CFP2_END_MASK << CFP2_END_OFFSET)) { - /* Parse CSP header into csp_id type */ - csp_id_strip(packet); + /* Extract data length */ + packet->length = packet->frame_length - csp_id_get_header_size(); /* Rewrite incoming L2 broadcast to local node */ if (packet->id.dst == 0x3FFF) { From c25249ad8243bcade783f0c72ac3d2ed67ea06b5 Mon Sep 17 00:00:00 2001 From: Troels Jessen Date: Fri, 6 Mar 2026 12:35:11 +0100 Subject: [PATCH 7/9] Add improved buffer protection to ZMQ --- include/csp/csp_id.h | 9 +++++++++ src/csp_id.c | 2 +- src/interfaces/csp_if_can.c | 7 ++++--- src/interfaces/csp_if_eth.c | 14 ++++---------- src/interfaces/csp_if_zmqhub.c | 14 ++++++-------- 5 files changed, 24 insertions(+), 22 deletions(-) diff --git a/include/csp/csp_id.h b/include/csp/csp_id.h index fc38623b3..dc474e4dd 100644 --- a/include/csp/csp_id.h +++ b/include/csp/csp_id.h @@ -94,6 +94,8 @@ int csp_id_get_header_size(void); */ void csp_id_prepend_fixup_cspv1(csp_packet_t * packet); +csp_id_t csp_id_extract(const uint8_t * data); + /** * Strip CSPv1-compatible ID header (ZMQ fixup). * @@ -113,6 +115,13 @@ static inline void csp_id_prepend_fixup_cspv1(csp_packet_t * packet) { csp_id_prepend(packet); } +/** + * Wrapper for csp_id_extract when no fixup is required. + */ +static inline csp_id_t csp_id_extract_fixup_cspv1(const uint8_t * data) { + return csp_id_extract(data); +} + /** * Wrapper for csp_id_strip when no fixup is required. */ diff --git a/src/csp_id.c b/src/csp_id.c index 6d7ab5509..47d755bfe 100644 --- a/src/csp_id.c +++ b/src/csp_id.c @@ -231,7 +231,7 @@ int csp_id_strip_fixup_cspv1(csp_packet_t * packet) { return -1; } - packet->id = csp_id_extract_cspv1(packet->frame_begin); + packet->id = csp_id_extract_fixup_cspv1(packet->frame_begin); packet->length = packet->frame_length - csp_id_get_header_size(); return 0; } diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c index 1d3f35500..cd2eab2ef 100644 --- a/src/interfaces/csp_if_can.c +++ b/src/interfaces/csp_if_can.c @@ -54,11 +54,14 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u memcpy(header, data, CFP1_CSP_HEADER_SIZE); csp_id_t csp_id = csp_id_extract(header); packet = csp_can_pbuf_new(ifdata, id, csp_id, task_woken); - packet->id = csp_id; if (packet == NULL) { iface->drop++; return CSP_ERR_NOBUFS; } + + csp_id_setup_rx(packet); + packet->id = csp_id; + memcpy(packet->frame_begin, data, CFP1_CSP_HEADER_SIZE); packet->frame_length += CFP1_CSP_HEADER_SIZE; } else { @@ -82,8 +85,6 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u break; } - csp_id_setup_rx(packet); - /* Copy CSP length (of data) */ memcpy(&(packet->length), data + CFP1_CSP_HEADER_SIZE, CFP1_DATA_LEN_SIZE); packet->length = be16toh(packet->length); diff --git a/src/interfaces/csp_if_eth.c b/src/interfaces/csp_if_eth.c index 9f4542b97..89cd50e58 100644 --- a/src/interfaces/csp_if_eth.c +++ b/src/interfaces/csp_if_eth.c @@ -180,9 +180,7 @@ int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t recei return CSP_ERR_INVAL; } - uint8_t csp_header[6]; - memcpy(csp_header, eth_frame->frame_begin, 6); - csp_id_t csp_id = csp_id_extract(csp_header); + csp_id_t csp_id = csp_id_extract(eth_frame->frame_begin); csp_packet_t * packet = csp_eth_pbuf_find(ifdata, packet_id, csp_id, task_woken); @@ -195,6 +193,7 @@ int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t recei if (packet->frame_length == 0) { /* First segment */ csp_id_setup_rx(packet); + packet->id = csp_id; packet->frame_length = frame_length; packet->rx_count = 0; } @@ -221,14 +220,9 @@ int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t recei return CSP_ERR_NONE; } - csp_eth_pbuf_free(ifdata, packet, false, task_woken); + packet->length = packet->frame_length - csp_id_get_header_size(); - if (csp_id_strip(packet) != 0) { - csp_print("eth rx packet discarded due to error in ID field\n"); - iface->frame++; - (task_woken) ? csp_buffer_free_isr(packet) : csp_buffer_free(packet); - return CSP_ERR_INVAL; - } + csp_eth_pbuf_free(ifdata, packet, false, task_woken); /* Record CSP and MAC addresses of source */ csp_eth_arp_set_addr(eth_frame->ether_shost, packet->id.src); diff --git a/src/interfaces/csp_if_zmqhub.c b/src/interfaces/csp_if_zmqhub.c index 63d7a9b9e..061f72032 100644 --- a/src/interfaces/csp_if_zmqhub.c +++ b/src/interfaces/csp_if_zmqhub.c @@ -134,8 +134,10 @@ static void * csp_zmqhub_task(void * param) { uint8_t * rx_data = zmq_msg_data(&msg); rx_data = csp_zmqhub_fixup_cspv1_del_dest_addr(rx_data, &datalen); + csp_id_t csp_id = csp_id_extract_fixup_cspv1(rx_data); + // Create new csp packet - if (csp_iflist_get_by_addr(*((uint16_t*)&rx_data[2]) & 0x3FFF) != NULL) { + if (csp_iflist_get_by_addr(csp_id.dst) != NULL) { /* The packet is for us, make sure we don't silently ignore the situation if we can't process it */ packet = csp_buffer_get_always(); } else { @@ -150,17 +152,13 @@ static void * csp_zmqhub_task(void * param) { } csp_id_setup_rx(packet); + packet->id = csp_id; memcpy(packet->frame_begin, rx_data, datalen); packet->frame_length = datalen; + /* Extract data length */ + packet->length = packet->frame_length - csp_id_get_header_size(); - /* Parse the frame and strip the ID field */ - if (csp_id_strip_fixup_cspv1(packet) != 0) { - drv->iface.rx_error++; - csp_buffer_free(packet); - zmq_msg_close(&msg); - continue; - } // Route packet csp_qfifo_write(packet, &drv->iface, NULL); From 0468501b1af6a46c493d24da4d87351bc12cc4d7 Mon Sep 17 00:00:00 2001 From: Troels Jessen Date: Fri, 6 Mar 2026 15:58:25 +0100 Subject: [PATCH 8/9] Include a minimum size check for length There is no need to try handling an ethernet frame that does not contain a valid CSP header --- src/interfaces/csp_if_eth.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/interfaces/csp_if_eth.c b/src/interfaces/csp_if_eth.c index 89cd50e58..569695805 100644 --- a/src/interfaces/csp_if_eth.c +++ b/src/interfaces/csp_if_eth.c @@ -174,7 +174,7 @@ int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t recei return CSP_ERR_INVAL; } - if (frame_length == 0 || frame_length > (CSP_BUFFER_SIZE + csp_id_get_header_size())) { + if (frame_length < csp_id_get_header_size() || frame_length > (CSP_BUFFER_SIZE + csp_id_get_header_size())) { iface->frame++; csp_print("eth rx frame_length of %u is invalid\n", frame_length); return CSP_ERR_INVAL; From 4c3002ebe93f68b02e03eee4c4a2d93ea4408dc4 Mon Sep 17 00:00:00 2001 From: Troels Jessen Date: Fri, 6 Mar 2026 16:05:07 +0100 Subject: [PATCH 9/9] Commet fixes --- include/csp/csp_id.h | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/include/csp/csp_id.h b/include/csp/csp_id.h index dc474e4dd..5ba07958d 100644 --- a/include/csp/csp_id.h +++ b/include/csp/csp_id.h @@ -16,10 +16,10 @@ extern "C" { void csp_id_prepend(csp_packet_t * packet); /** - * Extract CSP header from a 4 (CSP1) or 6 (CSP2) byte array + * Extract CSP header from a 4 (CSP1) or 6 (CSP2) byte array. * - * @param The first bytes of a CSP packet, representing CSP the header structure - * @return The extracted header ID + * @param The first bytes of a CSP packet, representing the CSP header structure. + * @return The extracted header ID structure. */ csp_id_t csp_id_extract(const uint8_t * data); @@ -94,7 +94,13 @@ int csp_id_get_header_size(void); */ void csp_id_prepend_fixup_cspv1(csp_packet_t * packet); -csp_id_t csp_id_extract(const uint8_t * data); +/** + * Extract CSP header from a 4 (CSP1) or 6 (CSP2) byte array (ZMQ fixup). + * + * @param The first bytes of a CSP packet, representing the CSP header structure. + * @return The extracted header ID structure. + */ +csp_id_t csp_id_extract_fixup_cspv1(const uint8_t * data); /** * Strip CSPv1-compatible ID header (ZMQ fixup).