Add improved buffer protection to ZMQ

Author: troelsjessen

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

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

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

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

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