Handle packets to be routed forward and those for the itf owner differently

In the current csp_can_pbuf_new() function implementation, CSP buffer exhaustion will cause the calling thread to crash (csp_buffer_get_always() will assert).
In the context of a CSH instance acting as a router, this means that as soon as this situation occurs, the router will stop functioning *silently*,
requiring a manual restart of CSH (the router thread will enter an infinite loop due to csp_panic() not being over-written in CSH (separate issue to be addressed)).
We have observed this behaviour where bursts of packets to be routed will cause the router to stop working due to buffer exhaustion.
To mitigate this issue, csp_can_pbuf_new() was changed to handle buffer exhaustion more gracefully, dropping to be routed packets, while still aborting in case
where the packet was for the interface owner, but not buffers where available.
Dropping packets will allow code to free up CSP buffers once the burst is over, making the infrastructure much more resilient.

The same logic is applied in ZMQ's csp_zmqhub_task() function

Author: troelsjessen

src/interfaces/csp_if_can.c

@@ -58,6 +58,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;
@@ -269,6 +273,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;

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>
 
 #include "../csp_buffer_private.h"
 
@@ -51,15 +52,25 @@ 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) {
 
-	packet->last_used = now;
-	packet->cfpid = id;
-	packet->remain = 0;
+		packet->last_used = now;
+		packet->cfpid = id;
+		packet->remain = 0;
 
-	/* Insert at beginning, because easy */
-	packet->next = ifdata->pbufs;
-	ifdata->pbufs = packet;
+		/* Insert at beginning, because easy */
+		packet->next = ifdata->pbufs;
+		ifdata->pbufs = packet;
+	}
 
 	return packet;
 }

src/interfaces/csp_if_zmqhub.c

@@ -129,18 +129,26 @@ static void * csp_zmqhub_task(void * param) {
 			continue;
 		}
 
+		// Copy the data from zmq to csp
+		const uint8_t * rx_data = zmq_msg_data(&msg);
+		rx_data = csp_zmqhub_fixup_cspv1_del_dest_addr(rx_data, &datalen);
+
 		// 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);
+			drv->iface.drop++;
 			zmq_msg_close(&msg);
 			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);