Multicast

include/csp/csp_interface.h

@@ -34,6 +34,7 @@ struct csp_iface_s {
 	void * driver_data;         /**< Driver data, only known/used by the driver layer, e.g. device/channel references. */
 	nexthop_t nexthop;          /**< Next hop (Tx) function */
 	uint8_t is_default;         /**< Set default IF flag (CSP supports multiple defaults) */
+	uint8_t broadcast_size;     /**< Select size of broadcast domain of the particular interface (defaulting to 1) */
 
 	/* Stats */
 	uint32_t tx;                /**< Successfully transmitted packets */

include/csp/drivers/can_socketcan.h

@@ -20,30 +20,16 @@ extern "C" {
  * @param[in] device CAN device name (Linux device).
  * @param[in] ifname CSP interface name, use #CSP_IF_CAN_DEFAULT_NAME for default name.
  * @param[in] node_id CSP address of the interface.
+ * @param[in] netmask Netmask size of the interface.
+ * @param[in] broadcast_size Number of broadcast nodes accepted on the subnet.
  * @param[in] bitrate if different from 0, it will be attempted to change the
  *            bitrate on the CAN device - this may require increased OS privileges.
  * @param[in] promisc if true, receive all CAN frames. If false a filter
  *                    is set on the CAN device, using device->addr
  * @param[out] return_iface the added interface.
  * @return The added interface, or NULL in case of failure.
  */
-int csp_can_socketcan_open_and_add_interface(const char * device, const char * ifname, unsigned int node_id, int bitrate, bool promisc, csp_iface_t ** return_iface);
-
-/**
- * Initialize socketcan and add CSP interface.
- *
- * :bdg-warning-line:`deprecated` version 1.6, use csp_can_socketcan_open_and_add_interface()
- *
- * Parameters:
- * @param[in] device CAN device name (Linux device).
- * @param[in] node_id CSP address of the interface.
- * @param[in] bitrate if different from 0, it will be attempted to change the
- *            bitrate on the CAN device - this may require increased OS privileges.
- * @param[in] promisc if true, receive all CAN frames. If false a filter
- *                    is set on the CAN device, using device->addr
- * @return The added interface, or NULL in case of failure.
- */
-csp_iface_t * csp_can_socketcan_init(const char * device, unsigned int node_id, int bitrate, bool promisc);
+int csp_can_socketcan_open_and_add_interface(const char * device, const char * ifname, unsigned int node_id, unsigned int mask, unsigned int broadcast_size, int bitrate, bool promisc, csp_iface_t ** return_iface);
 
 /**
  * Stop the Rx thread and free resources (testing).

include/csp/drivers/eth_linux.h

@@ -18,12 +18,10 @@
  * @param[in] ifname ifname CSP interface name.
  * @param[in] mtu MTU for the transmitted ethernet frames.
  * @param[in] node_id CSP address of the interface.
- * @param[in] promisc if true, receive all CAN frames. If false a filter
- *                    is set before forwarding packets to the router
  * @param[out] return_iface the added interface.
  * @return #CSP_ERR_NONE on success, otherwise an error code.
  */
-int csp_eth_init(const char * device, const char * ifname, int mtu, unsigned int node_id, bool promisc, csp_iface_t ** return_iface);
+int csp_eth_init(const char * device, const char * ifname, int mtu, unsigned int node_id, csp_iface_t ** return_iface);
 
 /**
  * Transmit an CSP ethernet frame

include/csp/interfaces/csp_if_eth.h

@@ -90,7 +90,6 @@ typedef int (*csp_eth_driver_tx_t)(void * driver_data, csp_eth_header_t * eth_fr
  */
 typedef struct {
 	csp_iface_t iface;
-	bool promisc;
 	uint16_t tx_mtu;
 	csp_eth_driver_tx_t tx_func;
 	csp_eth_header_t * tx_buf;

include/csp/interfaces/csp_if_zmqhub.h

@@ -120,7 +120,7 @@ int csp_zmqhub_init_w_name_endpoints_rxfilter(const char * ifname, uint16_t addr
  * If a secret key curve zmq is enabled
  */
 
-int csp_zmqhub_init_filter2(const char * ifname, const char * host, uint16_t addr, uint16_t netmask, int promisc, csp_iface_t ** return_interface, char * sec_key, uint16_t subport, uint16_t pubport);
+int csp_zmqhub_init_filter2(const char * ifname, const char * host, uint16_t addr, uint16_t netmask, uint8_t broadcast_size, int promisc, csp_iface_t ** return_interface, char * sec_key, uint16_t subport, uint16_t pubport);
 
 
 #ifdef __cplusplus

src/csp_id.c

@@ -232,7 +232,8 @@ unsigned int csp_id_get_max_port(void) {
 int csp_id_is_broadcast(uint16_t addr, csp_iface_t * iface) {
 	uint16_t hostmask = (1 << (csp_id_get_host_bits() - iface->netmask)) - 1;
 	uint16_t netmask = (1 << csp_id_get_host_bits()) - 1 - hostmask;
-	if (((addr & hostmask) == hostmask) && ((addr & netmask) == (iface->addr & netmask))) {
+	uint16_t hostaddr = addr & hostmask;
+	if ((hostaddr > hostmask - iface->broadcast_size) && ((addr & netmask) == (iface->addr & netmask))) {
 		return 1;
 	}
 

src/csp_yaml.c

@@ -54,6 +54,12 @@ static void csp_yaml_end_if(struct data_s * data, unsigned int * dfl_addr) {
 		}
 	}
 
+	int netmask = atoi(data->netmask);
+
+	if (netmask == 0) {
+		netmask = 8;
+	}
+
 	/* UART */
     if (strcmp(data->driver, "kiss") == 0) {
 
@@ -127,7 +133,7 @@ static void csp_yaml_end_if(struct data_s * data, unsigned int * dfl_addr) {
 			promisc = (strcmp("true", data->promisc) == 0) ? 1 : 0;
 		}
 
-		csp_zmqhub_init_filter2(data->name, data->server, addr, atoi(data->netmask), promisc, &iface, NULL, CSP_ZMQPROXY_SUBSCRIBE_PORT, CSP_ZMQPROXY_PUBLISH_PORT);
+		csp_zmqhub_init_filter2(data->name, data->server, addr, atoi(data->netmask), 1, promisc, &iface, NULL, CSP_ZMQPROXY_SUBSCRIBE_PORT, CSP_ZMQPROXY_PUBLISH_PORT);
 
 	}
 #endif
@@ -142,7 +148,7 @@ static void csp_yaml_end_if(struct data_s * data, unsigned int * dfl_addr) {
 			return;
 		}
 
-		int error = csp_can_socketcan_open_and_add_interface(data->device, data->name, addr, 1000000, true, &iface);
+		int error = csp_can_socketcan_open_and_add_interface(data->device, data->name, addr, netmask, 1, 1000000, true, &iface);
 		if (error != CSP_ERR_NONE) {
 			csp_print("failed to add CAN interface [%s], error: %d", data->device, error);
 			return;
@@ -157,8 +163,6 @@ static void csp_yaml_end_if(struct data_s * data, unsigned int * dfl_addr) {
         return;
     }
 
-	iface->addr = addr;
-	iface->netmask = atoi(data->netmask);
 	iface->name = strdup(data->name);
 	iface->is_default = (data->is_dfl) ? 1 : 0;
 

src/drivers/can/can_socketcan.c

@@ -155,7 +155,7 @@ static int csp_can_tx_frame(void * driver_data, uint32_t id, const uint8_t * dat
 
 int csp_can_socketcan_set_promisc(const bool promisc, can_context_t * ctx) {
 
-	struct can_filter filter[3] = { {
+	struct can_filter filter[4] = { {
 		.can_id = CFP_MAKE_DST(ctx->iface.addr),
 		.can_mask = 0x0000, /* receive anything */
 	} };
@@ -164,20 +164,56 @@ int csp_can_socketcan_set_promisc(const bool promisc, can_context_t * ctx) {
 		return CSP_ERR_INVAL;
 	}
 
-	int num_filters = 1;
+	int num_filters = 0;
 	if (!promisc) {
 		if (csp_conf.version == 1) {
-			num_filters = 1;
-			filter[0].can_id = CFP_MAKE_DST(ctx->iface.addr);
-			filter[0].can_mask = CFP_MAKE_DST((1 << CFP_HOST_SIZE) - 1);
+			filter[num_filters].can_id = CFP_MAKE_DST(ctx->iface.addr);
+			filter[num_filters].can_mask = CFP_MAKE_DST((1 << CFP_HOST_SIZE) - 1);
+			num_filters++;
 		} else {
-			num_filters = 3;
-			filter[0].can_id = ctx->iface.addr << CFP2_DST_OFFSET;
-			filter[0].can_mask = CFP2_DST_MASK << CFP2_DST_OFFSET;
-			filter[1].can_id = ((1 << (csp_id_get_host_bits() - ctx->iface.netmask)) - 1) << CFP2_DST_OFFSET;
-			filter[1].can_mask = CFP2_DST_MASK << CFP2_DST_OFFSET;
-			filter[2].can_id = 0x3FFF << CFP2_DST_OFFSET;
-			filter[2].can_mask = CFP2_DST_MASK << CFP2_DST_OFFSET;
+			// Add filter for specific interface address
+			filter[num_filters].can_id = ctx->iface.addr << CFP2_DST_OFFSET;
+			filter[num_filters].can_mask = CFP2_DST_MASK << CFP2_DST_OFFSET;
+			num_filters++;
+
+			uint16_t hostmask = (1 << (csp_id_get_host_bits() - ctx->iface.netmask)) - 1;
+
+			// Add filter for local broadcast addresses
+			switch (ctx->iface.broadcast_size) {
+				case 1:
+				default:
+				filter[num_filters].can_id = (ctx->iface.addr | hostmask) << CFP2_DST_OFFSET;
+				filter[num_filters].can_mask = CFP2_DST_MASK << CFP2_DST_OFFSET;
+				num_filters++;
+				break;
+
+				case 2:
+				filter[num_filters].can_id = (ctx->iface.addr | hostmask) << CFP2_DST_OFFSET;
+				filter[num_filters].can_mask = (CFP2_DST_MASK & ~1) << CFP2_DST_OFFSET;
+				num_filters++;
+				break;
+
+				case 3:
+				filter[num_filters].can_id = (ctx->iface.addr | hostmask) << CFP2_DST_OFFSET;
+				filter[num_filters].can_mask = (CFP2_DST_MASK & ~1) << CFP2_DST_OFFSET;
+				num_filters++;
+
+				filter[num_filters].can_id = ((ctx->iface.addr | hostmask) & ~2) << CFP2_DST_OFFSET;
+				filter[num_filters].can_mask = CFP2_DST_MASK << CFP2_DST_OFFSET;
+				num_filters++;
+				break;
+
+				case 4:
+				filter[num_filters].can_id = (ctx->iface.addr | hostmask) << CFP2_DST_OFFSET;
+				filter[num_filters].can_mask = (CFP2_DST_MASK & ~3) << CFP2_DST_OFFSET;
+				num_filters++;
+				break;
+			}
+
+			// Add filter for global broadcast address
+			filter[num_filters].can_id = CFP2_DST_MASK << CFP2_DST_OFFSET;
+			filter[num_filters].can_mask = CFP2_DST_MASK << CFP2_DST_OFFSET;
+			num_filters++;
 		}
 	}
 
@@ -190,7 +226,7 @@ int csp_can_socketcan_set_promisc(const bool promisc, can_context_t * ctx) {
 }
 
 
-int csp_can_socketcan_open_and_add_interface(const char * device, const char * ifname, unsigned int node_id, int bitrate, bool promisc, csp_iface_t ** return_iface) {
+int csp_can_socketcan_open_and_add_interface(const char * device, const char * ifname, unsigned int node_id, unsigned int netmask, unsigned int broadcast_size, int bitrate, bool promisc, csp_iface_t ** return_iface) {
 	if (ifname == NULL) {
 		ifname = CSP_IF_CAN_DEFAULT_NAME;
 	}
@@ -214,6 +250,8 @@ int csp_can_socketcan_open_and_add_interface(const char * device, const char * i
 	strncpy(ctx->name, ifname, sizeof(ctx->name) - 1);
 	ctx->iface.name = ctx->name;
 	ctx->iface.addr = node_id;
+	ctx->iface.netmask = netmask;
+	ctx->iface.broadcast_size = broadcast_size;
 	ctx->iface.interface_data = &ctx->ifdata;
 	ctx->iface.driver_data = ctx;
 	ctx->ifdata.tx_func = csp_can_tx_frame;
@@ -276,12 +314,6 @@ int csp_can_socketcan_open_and_add_interface(const char * device, const char * i
 	return CSP_ERR_NONE;
 }
 
-csp_iface_t * csp_can_socketcan_init(const char * device, unsigned int node_id, int bitrate, bool promisc) {
-	csp_iface_t * return_iface;
-	int res = csp_can_socketcan_open_and_add_interface(device, CSP_IF_CAN_DEFAULT_NAME, node_id, bitrate, promisc, &return_iface);
-	return (res == CSP_ERR_NONE) ? return_iface : NULL;
-}
-
 int csp_can_socketcan_stop(csp_iface_t * iface) {
 	can_context_t * ctx = iface->driver_data;
 

src/drivers/eth/eth_linux.c

@@ -67,7 +67,7 @@ void * csp_eth_rx_loop(void * param) {
 
 static uint8_t csp_eth_tx_buffer[CSP_ETH_BUF_SIZE];
 
-int csp_eth_init(const char * device, const char * ifname, int mtu, unsigned int node_id, bool promisc, csp_iface_t ** return_iface) {
+int csp_eth_init(const char * device, const char * ifname, int mtu, unsigned int node_id, csp_iface_t ** return_iface) {
 
 	eth_context_t * ctx = calloc(1, sizeof(*ctx));
 	if (ctx == NULL) {
@@ -82,7 +82,6 @@ int csp_eth_init(const char * device, const char * ifname, int mtu, unsigned int
 	ctx->ifdata.iface.addr = node_id;
 	ctx->ifdata.iface.driver_data = ctx;
     ctx->ifdata.iface.interface_data = &ctx->ifdata;
-    ctx->ifdata.promisc = promisc;
 
     /* Ether header 14 byte, seg header 4 byte, CSP header 6 byte */
     if (mtu < 24) {

src/interfaces/csp_if_can.c

@@ -140,11 +140,6 @@ int csp_can1_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t
 			if (packet->rx_count != packet->length)
 				break;
 
-			/* Rewrite incoming L2 broadcast to local node */
-			if (packet->id.dst == 0x1F) {
-				packet->id.dst = iface->addr;
-			}
-
 			/* Free packet buffer */
 			csp_can_pbuf_free(ifdata, packet, 0, task_woken);
 
@@ -349,11 +344,6 @@ int csp_can2_rx(csp_iface_t * iface, uint32_t id, const uint8_t * data, uint8_t
 		/* Parse CSP header into csp_id type */
 		csp_id_strip(packet);
 
-		/* Rewrite incoming L2 broadcast to local node */
-		if (packet->id.dst == 0x3FFF) {
-			packet->id.dst = iface->addr;
-		}
-
 		/* Free packet buffer */
 		csp_can_pbuf_free(ifdata, packet, 0, task_woken);
 

src/interfaces/csp_if_eth.c

@@ -226,12 +226,6 @@ int csp_eth_rx(csp_iface_t * iface, csp_eth_header_t * eth_frame, uint32_t recei
     /* Record CSP and MAC addresses of source */
     csp_eth_arp_set_addr(eth_frame->ether_shost, packet->id.src);
 
-    if (packet->id.dst != iface->addr && !ifdata->promisc) {
-        csp_eth_pbuf_free(ifdata, packet, true, task_woken);
-        (task_woken) ? csp_buffer_free_isr(packet) : csp_buffer_free(packet);
-        return CSP_ERR_NONE;
-    }
-
     csp_qfifo_write(packet, iface, task_woken);
 
     return CSP_ERR_NONE;

src/interfaces/csp_if_zmqhub.c

@@ -218,7 +218,7 @@ int csp_zmqhub_init_w_name_endpoints_rxfilter(const char * ifname, uint16_t addr
 	return CSP_ERR_NONE;
 }
 
-int csp_zmqhub_init_filter2(const char * ifname, const char * host, uint16_t addr, uint16_t netmask, int promisc, csp_iface_t ** return_interface, char * sec_key, uint16_t subport, uint16_t pubport) {
+int csp_zmqhub_init_filter2(const char * ifname, const char * host, uint16_t addr, uint16_t netmask, uint8_t broadcast_size, int promisc, csp_iface_t ** return_interface, char * sec_key, uint16_t subport, uint16_t pubport) {
 
 	char pub[100];
 	csp_zmqhub_make_endpoint(host, subport, pub, sizeof(pub));
@@ -235,8 +235,15 @@ int csp_zmqhub_init_filter2(const char * ifname, const char * host, uint16_t add
 		ifname = CSP_ZMQHUB_IF_NAME;
 	}
 
+	if (broadcast_size == 0) {
+		broadcast_size = 1;
+	}
+
 	strncpy(drv->name, ifname, sizeof(drv->name) - 1);
 	drv->iface.name = drv->name;
+	drv->iface.addr = addr;
+	drv->iface.netmask = netmask;
+	drv->iface.broadcast_size = broadcast_size;
 	drv->iface.driver_data = drv;
 	drv->iface.nexthop = csp_zmqhub_tx;
 
@@ -304,17 +311,21 @@ int csp_zmqhub_init_filter2(const char * ifname, const char * host, uint16_t add
 	} else {
 
 		/* This needs to be static, because ZMQ does not copy the filter value to the
-		 * outgoing packet for each setsockopt call */
-		static uint16_t filt[4][3];
+		 * outgoing packet for each setsockopt call 
+		 * One entry is reserved for local node, one for global broadcast and one per local broadcast */
+		static uint16_t filt[4][6];
+		assert(broadcast_size <= 4); // A maximum of four local broadcast addresses are supported due to the filt array size
 
 		for (int i = 0; i < 4; i++) {
-			//int i = CSP_PRIO_NORM;
 			filt[i][0] = __builtin_bswap16((i << 14) | addr);
-			filt[i][1] = __builtin_bswap16((i << 14) | addr | hostmask);
-			filt[i][2] = __builtin_bswap16((i << 14) | 16383);
+			filt[i][1] = __builtin_bswap16((i << 14) | 16383);
 			ret = zmq_setsockopt(drv->subscriber, ZMQ_SUBSCRIBE, &filt[i][0], 2);
 			ret = zmq_setsockopt(drv->subscriber, ZMQ_SUBSCRIBE, &filt[i][1], 2);
-			ret = zmq_setsockopt(drv->subscriber, ZMQ_SUBSCRIBE, &filt[i][2], 2);
+
+			for (int j = 0; j < broadcast_size; j++) {
+				filt[i][j + 2] = __builtin_bswap16((i << 14) | (addr | (hostmask - j)));
+				ret = zmq_setsockopt(drv->subscriber, ZMQ_SUBSCRIBE, &filt[i][j+2], 2);
+			}
 		}
 
 	}