diff --git a/include/csp/csp_id.h b/include/csp/csp_id.h
index 5ba07958d..80a287f9a 100644
--- a/include/csp/csp_id.h
+++ b/include/csp/csp_id.h
@@ -17,12 +17,29 @@ void csp_id_prepend(csp_packet_t * packet);
 
 /**
  * Extract CSP header from a 4 (CSP1) or 6 (CSP2) byte array.
+ * Uses csp_conf.version to determine the CSP version
  *
  * @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);
 
+/**
+ * Extract CSP header from a 4 (CSP1) byte array.
+ *
+ * @param The first bytes of a CSP packet, representing the CSP header structure.
+ * @return The extracted header ID structure.
+ */
+csp_id_t csp_id1_extract(const uint8_t * data);
+
+/**
+ * Extract CSP header from a 6 (CSP2) byte array.
+ *
+ * @param The first bytes of a CSP packet, representing the CSP header structure.
+ * @return The extracted header ID structure.
+ */
+csp_id_t csp_id2_extract(const uint8_t * data);
+
 /**
  * Strip CSP header fields from the packet's data buffer.
  *
diff --git a/include/csp/csp_interface.h b/include/csp/csp_interface.h
index b8daac3b6..746d9290d 100644
--- a/include/csp/csp_interface.h
+++ b/include/csp/csp_interface.h
@@ -36,6 +36,7 @@ struct csp_iface_s {
 	nexthop_t nexthop;          /**< Next hop (Tx) function */
 	csp_alias_add_t add_alias;  /**< Add receive address to interface (could be multicast receptions) */
 	uint8_t is_default;         /**< Set default IF flag (CSP supports multiple defaults) */
+	uint8_t	version;            /**< CSP version of the interface. 0=use csp_conf.version, 1=CSPv1, 2=CSPv2 */
 
 	/* Stats */
 	uint32_t tx;                /**< Successfully transmitted packets */
diff --git a/include/csp/csp_types.h b/include/csp/csp_types.h
index 578a4f01c..cc60c7934 100644
--- a/include/csp/csp_types.h
+++ b/include/csp/csp_types.h
@@ -133,6 +133,7 @@ typedef struct csp_packet_s {
 
 	struct csp_packet_s * next; /*< Used for lists / queues of packets */
 
+	uint8_t	version;            /*< CSP version of the packet. 0=use csp_conf.version, 1=CSPv1, 2=CSPv2 */
 
 	/**
 	 * Additional header bytes, to prepend packed data before transmission
diff --git a/src/csp_buffer.c b/src/csp_buffer.c
index 884a4ae4d..0e47cd852 100644
--- a/src/csp_buffer.c
+++ b/src/csp_buffer.c
@@ -48,6 +48,7 @@ static csp_packet_t * csp_packet_init(csp_packet_t * packet)
 	packet->length = 0;
 	packet->frame_begin = packet->data;
 	packet->frame_length = 0;
+	packet->version = 0;
 
 	csp_id_clear(&packet->id);
 
diff --git a/src/csp_id.c b/src/csp_id.c
index 47d755bfe..ac8b58793 100644
--- a/src/csp_id.c
+++ b/src/csp_id.c
@@ -63,7 +63,7 @@ static void csp_id1_prepend(csp_packet_t * packet, bool cspv1_fixup) {
 	memcpy(packet->frame_begin, &id1, CSP_ID1_HEADER_SIZE);
 }
 
-static csp_id_t csp_id1_extract(const uint8_t * data, bool cspv1_fixup) {
+static csp_id_t csp_id1_extract_local(const uint8_t * data, bool cspv1_fixup) {
 
 	/* Get 32 bit in network byte order */
 	uint32_t id1_raw = 0;
@@ -143,7 +143,7 @@ static void csp_id2_prepend(csp_packet_t * packet) {
 	memcpy(packet->frame_begin, &id2, CSP_ID2_HEADER_SIZE);
 }
 
-static csp_id_t csp_id2_extract(const uint8_t* data) {
+static csp_id_t csp_id2_extract_local(const uint8_t* data) {
 
 	/* Get 48 bit in network byte order:
 	 * Most significant byte ends in byte 0 */
@@ -181,8 +181,12 @@ static void csp_id2_setup_rx(csp_packet_t * packet) {
  * That would actually be nicer, but it can be done later, it works for now.
  */
 
+static uint8_t csp_id_get_version(csp_packet_t * packet) {
+	return packet->version == 2 || packet->version == 1 ? packet->version : csp_conf.version;
+}
+
 void csp_id_prepend(csp_packet_t * packet) {
-	if (csp_conf.version == 2) {
+	if (csp_id_get_version(packet) == 2) {
 		csp_id2_prepend(packet);
 	} else {
 		csp_id1_prepend(packet, false);
@@ -191,19 +195,32 @@ void csp_id_prepend(csp_packet_t * packet) {
 
 csp_id_t csp_id_extract(const uint8_t * data) {
 	if (csp_conf.version == 2) {
-		return csp_id2_extract(data);
+		return csp_id2_extract_local(data);
 	} else {
-		return csp_id1_extract(data, false);
+		return csp_id1_extract_local(data, false);
 	}
 }
 
+csp_id_t csp_id1_extract(const uint8_t * data) {
+
+	return csp_id1_extract_local(data, false);
+}
+
+csp_id_t csp_id2_extract(const uint8_t* data) {
+
+	return csp_id2_extract_local(data);
+}
+
 int csp_id_strip(csp_packet_t * packet) {
-	if (packet->frame_length < csp_id_get_header_size()) {
+
+	uint8_t version = csp_id_get_version(packet);
+	uint16_t header_size = version == 2 ? CSP_ID2_HEADER_SIZE : CSP_ID1_HEADER_SIZE;
+	if (packet->frame_length < header_size) {
 		return -1;
 	}
 
-	packet->id = csp_id_extract(packet->frame_begin);
-	packet->length = packet->frame_length - csp_id_get_header_size();
+	packet->id = version == 2 ? csp_id2_extract_local(packet->frame_begin) : csp_id1_extract_local(packet->frame_begin, false);
+	packet->length = packet->frame_length - header_size;
 	return 0;
 }
 
@@ -220,9 +237,9 @@ void csp_id_prepend_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_extract(data);
+		return csp_id2_extract_local(data);
 	} else {
-		return csp_id1_extract(data, true);
+		return csp_id1_extract_local(data, true);
 	}
 }
 
@@ -239,7 +256,7 @@ int csp_id_strip_fixup_cspv1(csp_packet_t * packet) {
 #endif
 
 int csp_id_setup_rx(csp_packet_t * packet) {
-	if (csp_conf.version == 2) {
+	if (csp_id_get_version(packet) == 2) {
 		csp_id2_setup_rx(packet);
 		return CSP_ID2_HEADER_SIZE;
 	} else {
diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c
index 2930bc1e9..168f1e10e 100644
--- a/src/interfaces/csp_if_can.c
+++ b/src/interfaces/csp_if_can.c
@@ -53,13 +53,14 @@ static int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u
 			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);
+			csp_id_t csp_id = csp_id1_extract(header);
 			packet = csp_can_pbuf_new(ifdata, id, csp_id, task_woken);
 			if (packet == NULL) {
 				iface->drop++;
 				return CSP_ERR_NOBUFS;
 			}
 
+			packet->version = iface->version;
 			csp_id_setup_rx(packet);
 			packet->id = csp_id;
 
@@ -175,6 +176,18 @@ static int csp_can1_tx(csp_iface_t * iface, uint16_t via, csp_packet_t * packet,
 
 	csp_can_interface_data_t * ifdata = iface->interface_data;
 
+	if (packet->version == 0) {
+		/* Set packet version as it is needed in csp_id_prepend */
+		packet->version = iface->version;
+
+		/* Is dst and src addresses valid for CSPv1 which is max 0x1F (5 bits)*/
+		if (packet->id.dst > 0x1F || packet->id.src > 0x1F) {
+			return CSP_ERR_INVAL;
+		}
+	} else if (packet->version != iface->version) {
+		return CSP_ERR_INVAL;
+	}
+
 	/* Get an unique CFP id */
 	const uint32_t ident = ifdata->cfp_packet_counter++; // Atomic operation as cfp_packet_counter is of type atomic_int
 
@@ -294,13 +307,14 @@ static int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, u
 			dlc -= 4;
 
 			/* Create CSP header info from the first bytes received */
-			csp_id_t csp_id = csp_id_extract(header);
+			csp_id_t csp_id = csp_id2_extract(header);
 			packet = csp_can_pbuf_new(ifdata, id, csp_id, task_woken);
 			if (packet == NULL) {
 				iface->drop++;
 				return CSP_ERR_NOBUFS;
 			}
 
+			packet->version = iface->version;
 			/* Prepare new CSP packet by adding header as extracted */
 			csp_id_setup_rx(packet);
 			packet->id = csp_id;
@@ -486,7 +500,11 @@ int csp_can_add_interface(csp_iface_t * iface) {
 
 	ifdata->cfp_packet_counter = 0;
 
-	if (csp_conf.version == 1) {
+	if (iface->version == 0) {
+		iface->version = csp_conf.version;
+	}
+
+	if (iface->version == 1) {
 		iface->nexthop = csp_can1_tx;
 	} else {
 		iface->nexthop = csp_can2_tx;
@@ -510,7 +528,7 @@ int csp_can_remove_interface(csp_iface_t * iface) {
 
 int csp_can_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t dlc, uint32_t timestamp_rx, int * task_woken) {
 
-	if (csp_conf.version == 1) {
+	if (iface->version == 1) {
 		return csp_can1_rx(iface, id, data, dlc, task_woken);
 	} else {
 		return csp_can2_rx(iface, id, data, dlc, timestamp_rx, task_woken);