diff --git a/include/csp/csp_interface.h b/include/csp/csp_interface.h index 33a9f4748..5110704b8 100644 --- a/include/csp/csp_interface.h +++ b/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 */ diff --git a/include/csp/drivers/can_socketcan.h b/include/csp/drivers/can_socketcan.h index ffd867d2f..2d68b3b40 100644 --- a/include/csp/drivers/can_socketcan.h +++ b/include/csp/drivers/can_socketcan.h @@ -20,6 +20,8 @@ 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 @@ -27,23 +29,7 @@ extern "C" { * @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). diff --git a/include/csp/drivers/eth_linux.h b/include/csp/drivers/eth_linux.h index 7e08f2ef1..6778d3cd7 100644 --- a/include/csp/drivers/eth_linux.h +++ b/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 diff --git a/include/csp/interfaces/csp_if_eth.h b/include/csp/interfaces/csp_if_eth.h index ad9577213..1b77c4419 100644 --- a/include/csp/interfaces/csp_if_eth.h +++ b/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; diff --git a/include/csp/interfaces/csp_if_zmqhub.h b/include/csp/interfaces/csp_if_zmqhub.h index 17d972442..26f2c3b18 100644 --- a/include/csp/interfaces/csp_if_zmqhub.h +++ b/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 diff --git a/src/csp_id.c b/src/csp_id.c index ee5549576..74b5768a6 100644 --- a/src/csp_id.c +++ b/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; } diff --git a/src/csp_yaml.c b/src/csp_yaml.c index f77c2b6cb..c99027c19 100644 --- a/src/csp_yaml.c +++ b/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; diff --git a/src/drivers/can/can_socketcan.c b/src/drivers/can/can_socketcan.c index 2aab679d2..ec203daf7 100644 --- a/src/drivers/can/can_socketcan.c +++ b/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; diff --git a/src/drivers/eth/eth_linux.c b/src/drivers/eth/eth_linux.c index 04ba7fe98..46b3bbd8f 100644 --- a/src/drivers/eth/eth_linux.c +++ b/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) { diff --git a/src/interfaces/csp_if_can.c b/src/interfaces/csp_if_can.c index 880f31acc..6863a64d5 100644 --- a/src/interfaces/csp_if_can.c +++ b/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); diff --git a/src/interfaces/csp_if_eth.c b/src/interfaces/csp_if_eth.c index 000654292..35748e32a 100644 --- a/src/interfaces/csp_if_eth.c +++ b/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; diff --git a/src/interfaces/csp_if_zmqhub.c b/src/interfaces/csp_if_zmqhub.c index 75d629075..7ab33415e 100644 --- a/src/interfaces/csp_if_zmqhub.c +++ b/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); + } } }