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).

Author: troelsjessen

include/csp/csp_id.h

@@ -7,13 +7,15 @@ extern "C" {
 #include <csp/csp.h>
 
 void csp_id_prepend(csp_packet_t * packet);
+csp_id_t csp_id_extract(const uint8_t * data);
 int csp_id_strip(csp_packet_t * packet);
 int csp_id_setup_rx(csp_packet_t * packet);
 unsigned int csp_id_get_host_bits(void);
 unsigned int csp_id_get_max_nodeid(void);
 unsigned int csp_id_get_max_port(void);
 
 int csp_id_is_broadcast(uint16_t addr, csp_iface_t * iface);
+int csp_id_get_header_size(void);
 
 #ifdef __cplusplus
 }

src/csp_id.c

@@ -6,7 +6,10 @@
  */
 
 #include <endian.h>
+#include <string.h>
+
 #include <csp/csp.h>
+#include <csp/csp_id.h>
 
 /**
  * CSP 1.x
@@ -40,46 +43,42 @@
 static void csp_id1_prepend(csp_packet_t * packet) {
 
 	/* Pack into 32-bit using host endian */
-	uint32_t id1 = (((uint32_t)(packet->id.pri) << CSP_ID1_PRIO_OFFSET) |
-					((uint32_t)(packet->id.dst) << CSP_ID1_DST_OFFSET) |
-					((uint32_t)(packet->id.src) << CSP_ID1_SRC_OFFSET) |
-					((uint32_t)(packet->id.dport) << CSP_ID1_DPORT_OFFSET) |
-					((uint32_t)(packet->id.sport) << CSP_ID1_SPORT_OFFSET) |
-					((uint32_t)(packet->id.flags) << CSP_ID1_FLAGS_OFFSET));
+	uint32_t id1_raw = (((uint32_t)(packet->id.pri) << CSP_ID1_PRIO_OFFSET) |
+						((uint32_t)(packet->id.dst) << CSP_ID1_DST_OFFSET) |
+						((uint32_t)(packet->id.src) << CSP_ID1_SRC_OFFSET) |
+						((uint32_t)(packet->id.dport) << CSP_ID1_DPORT_OFFSET) |
+						((uint32_t)(packet->id.sport) << CSP_ID1_SPORT_OFFSET) |
+						((uint32_t)(packet->id.flags) << CSP_ID1_FLAGS_OFFSET));
 
 	/* Convert to big / network endian */
-	id1 = htobe32(id1);
+	uint32_t id1 = htobe32(id1_raw);
 
 	packet->frame_begin = packet->data - CSP_ID1_HEADER_SIZE;
 	packet->frame_length = packet->length + CSP_ID1_HEADER_SIZE;
 
 	memcpy(packet->frame_begin, &id1, CSP_ID1_HEADER_SIZE);
 }
 
-static int csp_id1_strip(csp_packet_t * packet) {
-
-	if (packet->frame_length < CSP_ID1_HEADER_SIZE) {
-		return -1;
-	}
+static csp_id_t csp_id1_extract(const uint8_t * data) {
 
 	/* Get 32 bit in network byte order */
-	uint32_t id1 = 0;
-	memcpy(&id1, packet->frame_begin, CSP_ID1_HEADER_SIZE);
-	packet->length = packet->frame_length - CSP_ID1_HEADER_SIZE;
+	uint32_t id1_raw = 0;
+	memcpy(&id1_raw, data, CSP_ID1_HEADER_SIZE);
 
 	/* Convert to host order */
-	id1 = be32toh(id1);
+	uint32_t id1 = be32toh(id1_raw);
 
 	/* 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) {
@@ -136,17 +135,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
@@ -155,14 +149,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) {
@@ -186,13 +181,22 @@ void csp_id_prepend(csp_packet_t * packet) {
 	}
 }
 
-int csp_id_strip(csp_packet_t * packet) {
-	packet->timestamp_rx = 0;
+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);
+		return csp_id1_extract(data);
+	}
+}
+
+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;
 }
 
 int csp_id_setup_rx(csp_packet_t * packet) {
@@ -241,3 +245,11 @@ int csp_id_is_broadcast(uint16_t addr, csp_iface_t * iface) {
 	}
 	return 0;
 }
+
+int csp_id_get_header_size(void) {
+	if (csp_conf.version == 2) {
+		return CSP_ID2_HEADER_SIZE;
+	} else {
+		return CSP_ID1_HEADER_SIZE;
+	}
+}

src/interfaces/csp_if_can.c

@@ -59,7 +59,21 @@ int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t
 	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);
+			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 {
 			iface->frame++;
 			return CSP_ERR_INVAL;
@@ -81,14 +95,8 @@ int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t
 				break;
 			}
 
-			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  */
@@ -269,50 +277,56 @@ int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t
 	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);
-		} else {
-			iface->frame++;
-			return CSP_ERR_INVAL;
-		}
-	}
 
+			/* 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;
+			}
 
-	/* BEGIN */
-	if (id & (CFP2_BEGIN_MASK << CFP2_BEGIN_OFFSET)) {
+			/* 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);
+			memcpy(header, &first_two, 2);
+
+			/* 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;
 
-		/* 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;
+			/* Set next expected fragment counter to be 1 */
+			packet->rx_count = 1;
+		} else {
 			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 */
-		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;
-
-		/* 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;
 
@@ -346,8 +360,8 @@ int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t
 	/* 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) {

src/interfaces/csp_if_can_pbuf.c

@@ -6,6 +6,7 @@
 #include <csp/csp_error.h>
 #include <csp/arch/csp_time.h>
 #include <csp/interfaces/csp_if_can.h>
+#include <csp/csp_iflist.h>
 
 /* Buffer element timeout in ms */
 #define PBUF_TIMEOUT_MS 1000
@@ -43,13 +44,24 @@ 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 = (task_woken) ? csp_buffer_get_always_isr() : csp_buffer_get_always();
+	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 packet;
+	}
 
 	packet->last_used = now;
 	packet->cfpid = id;

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);

src/interfaces/csp_if_eth.c

@@ -177,7 +177,9 @@ 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);
+    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);
 
     if (packet == NULL) {
         iface->drop++;
@@ -188,6 +190,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;
     }
@@ -214,14 +217,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);