diff --git a/drivers/can/socket_can_generic.h b/drivers/can/socket_can_generic.h index 538b84d79c5d8..25f56075bbf1e 100644 --- a/drivers/can/socket_can_generic.h +++ b/drivers/can/socket_can_generic.h @@ -80,6 +80,7 @@ static inline int socket_can_setsockopt(struct device *dev, void *obj, const void *optval, socklen_t optlen) { struct socket_can_context *socket_context = dev->driver_data; + struct net_context *ctx = obj; int ret; if (level != SOL_CAN_RAW && optname != CAN_RAW_FILTER) { @@ -96,12 +97,22 @@ static inline int socket_can_setsockopt(struct device *dev, void *obj, return -1; } + net_context_set_filter_id(ctx, ret); + return 0; } +static inline void socket_can_close(struct device *dev, int filter_id) +{ + struct socket_can_context *socket_context = dev->driver_data; + + can_detach(socket_context->can_dev, filter_id); +} + static struct canbus_api socket_can_api = { .iface_api.init = socket_can_iface_init, .send = socket_can_send, + .close = socket_can_close, .setsockopt = socket_can_setsockopt, }; diff --git a/include/net/net_context.h b/include/net/net_context.h index e4c0b92a86eca..6d982547cb7bc 100644 --- a/include/net/net_context.h +++ b/include/net/net_context.h @@ -274,6 +274,10 @@ struct net_context { void *offload_context; #endif /* CONFIG_NET_OFFLOAD */ +#if defined(CONFIG_NET_SOCKETS_CAN) + int can_filter_id; +#endif /* CONFIG_NET_SOCKETS_CAN */ + /** Option values */ struct { #if defined(CONFIG_NET_CONTEXT_PRIORITY) @@ -432,6 +436,56 @@ static inline void net_context_set_type(struct net_context *context, context->flags |= flag; } +/** + * @brief Set CAN filter id for this network context. + * + * @details This function sets the CAN filter id of the context. + * + * @param context Network context. + * @param filter_id CAN filter id + */ +#if defined(CONFIG_NET_SOCKETS_CAN) +static inline void net_context_set_filter_id(struct net_context *context, + int filter_id) +{ + NET_ASSERT(context); + + context->can_filter_id = filter_id; +} +#else +static inline void net_context_set_filter_id(struct net_context *context, + int filter_id) +{ + ARG_UNUSED(context); + ARG_UNUSED(filter_id); +} +#endif + +/** + * @brief Get CAN filter id for this network context. + * + * @details This function gets the CAN filter id of the context. + * + * @param context Network context. + * + * @return Filter id of this network context + */ +#if defined(CONFIG_NET_SOCKETS_CAN) +static inline int net_context_get_filter_id(struct net_context *context) +{ + NET_ASSERT(context); + + return context->can_filter_id; +} +#else +static inline int net_context_get_filter_id(struct net_context *context) +{ + ARG_UNUSED(context); + + return -1; +} +#endif + /** * @brief Get context IP protocol for this network context. * diff --git a/include/net/socket_can.h b/include/net/socket_can.h index 6e04e6c296340..2eaceac32f28f 100644 --- a/include/net/socket_can.h +++ b/include/net/socket_can.h @@ -65,6 +65,9 @@ struct canbus_api { /** Send a CAN packet by socket */ int (*send)(struct device *dev, struct net_pkt *pkt); + /** Close the related CAN socket */ + void (*close)(struct device *dev, int filter_id); + /** Set socket CAN option */ int (*setsockopt)(struct device *dev, void *obj, int level, int optname, const void *optval, socklen_t optlen); diff --git a/samples/net/sockets/can/sample.yaml b/samples/net/sockets/can/sample.yaml index 4a0372af05d10..72c6b4a066aa0 100644 --- a/samples/net/sockets/can/sample.yaml +++ b/samples/net/sockets/can/sample.yaml @@ -1,9 +1,15 @@ common: tags: net socket can CAN depends_on: can + harness: can + platform_whitelist: stm32f072b_disco nucleo_l432kc sample: description: Test BSD sockets CAN support name: Socket CAN example tests: - sample.net.sockets.can: - platform_whitelist: stm32f072b_disco nucleo_l432kc + sample.net.sockets.can.test_with_one_socket: + extra_configs: + - CONFIG_NET_SOCKETS_CAN_RECEIVERS=1 + sample.net.sockets.can.test_with_two_sockets: + extra_configs: + - CONFIG_NET_SOCKETS_CAN_RECEIVERS=2 diff --git a/samples/net/sockets/can/src/main.c b/samples/net/sockets/can/src/main.c index 8ae5f3efb8c62..f3e6ac466e8f3 100644 --- a/samples/net/sockets/can/src/main.c +++ b/samples/net/sockets/can/src/main.c @@ -12,14 +12,33 @@ LOG_MODULE_REGISTER(net_socket_can_sample, LOG_LEVEL_DBG); #include #include -#define PRIORITY 7 -#define STACKSIZE 750 +#define PRIORITY k_thread_priority_get(k_current_get()) +#define STACKSIZE 1024 #define SLEEP_PERIOD K_SECONDS(1) static k_tid_t tx_tid; static K_THREAD_STACK_DEFINE(tx_stack, STACKSIZE); static struct k_thread tx_data; +/* For testing purposes, we create another RX receiver if configured so */ +#if CONFIG_NET_SOCKETS_CAN_RECEIVERS == 2 +static k_tid_t rx_tid; +static K_THREAD_STACK_DEFINE(rx_stack, STACKSIZE); +static struct k_thread rx_data; +#endif + +#define CLOSE_PERIOD 15 + +static const struct zcan_filter zfilter = { + .id_type = CAN_STANDARD_IDENTIFIER, + .rtr = CAN_DATAFRAME, + .std_id = 0x1, + .rtr_mask = 1, + .std_id_mask = CAN_STD_ID_MASK +}; + +static struct can_filter filter; + static void tx(int *can_fd) { int fd = POINTER_TO_INT(can_fd); @@ -50,15 +69,46 @@ static void tx(int *can_fd) } } -static void rx(int fd) +static int create_socket(const struct can_filter *filter) { + struct sockaddr_can can_addr; + int fd, ret; + + fd = socket(AF_CAN, SOCK_RAW, CAN_RAW); + if (fd < 0) { + LOG_ERR("Cannot create %s CAN socket (%d)", "2nd", fd); + return fd; + } + + can_addr.can_ifindex = net_if_get_by_iface( + net_if_get_first_by_type(&NET_L2_GET_NAME(CANBUS))); + can_addr.can_family = PF_CAN; + + ret = bind(fd, (struct sockaddr *)&can_addr, sizeof(can_addr)); + if (ret < 0) { + LOG_ERR("Cannot bind %s CAN socket (%d)", "2nd", -errno); + (void)close(fd); + return ret; + } + + (void)setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FILTER, filter, + sizeof(*filter)); + + return fd; +} + +static void rx(int *can_fd, int *do_close_period, + const struct can_filter *filter) +{ + int close_period = POINTER_TO_INT(do_close_period); + int fd = POINTER_TO_INT(can_fd); struct sockaddr_can can_addr; socklen_t addr_len; struct zcan_frame msg; struct can_frame frame; int ret; - LOG_DBG("Waiting CAN data..."); + LOG_DBG("[%d] Waiting CAN data...", fd); while (1) { u8_t *data; @@ -69,14 +119,15 @@ static void rx(int fd) ret = recvfrom(fd, &frame, sizeof(struct can_frame), 0, (struct sockaddr *)&can_addr, &addr_len); if (ret < 0) { - LOG_ERR("Cannot receive CAN message (%d)", ret); + LOG_ERR("[%d] Cannot receive CAN message (%d)", fd, + -errno); continue; } can_copy_frame_to_zframe(&frame, &msg); - LOG_INF("CAN msg: type 0x%x RTR 0x%x EID 0x%x DLC 0x%x", - msg.id_type, msg.rtr, msg.std_id, msg.dlc); + LOG_INF("[%d] CAN msg: type 0x%x RTR 0x%x EID 0x%x DLC 0x%x", + fd, msg.id_type, msg.rtr, msg.std_id, msg.dlc); if (!msg.rtr) { if (msg.dlc > 8) { @@ -87,24 +138,34 @@ static void rx(int fd) LOG_HEXDUMP_INF(data, msg.dlc, "Data"); } else { - LOG_INF("EXT Remote message received"); + LOG_INF("[%d] EXT Remote message received", fd); + } + + if (POINTER_TO_INT(do_close_period) > 0) { + close_period--; + if (close_period <= 0) { + (void)close(fd); + + k_sleep(K_SECONDS(1)); + + fd = create_socket(filter); + if (fd < 0) { + LOG_ERR("Cannot get socket (%d)", + -errno); + return; + } + + close_period = POINTER_TO_INT(do_close_period); + } } } } static int setup_socket(void) { - const struct zcan_filter zfilter = { - .id_type = CAN_STANDARD_IDENTIFIER, - .rtr = CAN_DATAFRAME, - .std_id = 0x1, - .rtr_mask = 1, - .std_id_mask = CAN_STD_ID_MASK - }; - struct can_filter filter; struct sockaddr_can can_addr; struct net_if *iface; - int fd; + int fd, rx_fd; int ret; can_copy_zfilter_to_filter(&zfilter, &filter); @@ -118,7 +179,7 @@ static int setup_socket(void) fd = socket(AF_CAN, SOCK_RAW, CAN_RAW); if (fd < 0) { ret = -errno; - LOG_ERR("Cannot create CAN socket (%d)", ret); + LOG_ERR("Cannot create %s CAN socket (%d)", "1st", ret); return ret; } @@ -128,7 +189,7 @@ static int setup_socket(void) ret = bind(fd, (struct sockaddr *)&can_addr, sizeof(can_addr)); if (ret < 0) { ret = -errno; - LOG_ERR("Cannot bind CAN socket (%d)", ret); + LOG_ERR("Cannot bind %s CAN socket (%d)", "1st", ret); goto cleanup; } @@ -154,7 +215,38 @@ static int setup_socket(void) LOG_DBG("Started socket CAN TX thread"); - return fd; + LOG_INF("1st RX fd %d", fd); + + rx_fd = fd; + +#if CONFIG_NET_SOCKETS_CAN_RECEIVERS == 2 + fd = create_socket(&filter); + if (fd >= 0) { + rx_tid = k_thread_create(&rx_data, rx_stack, + K_THREAD_STACK_SIZEOF(rx_stack), + (k_thread_entry_t)rx, + INT_TO_POINTER(fd), + INT_TO_POINTER(CLOSE_PERIOD), + &filter, PRIORITY, 0, K_NO_WAIT); + if (!rx_tid) { + ret = -ENOENT; + errno = -ret; + LOG_ERR("Cannot create 2nd RX thread!"); + goto cleanup2; + } + + LOG_INF("2nd RX fd %d", fd); + } else { + LOG_ERR("2nd RX not created (%d)", fd); + } +#endif + + return rx_fd; + +#if CONFIG_NET_SOCKETS_CAN_RECEIVERS == 2 +cleanup2: + (void)close(rx_fd); +#endif cleanup: (void)close(fd); @@ -165,11 +257,14 @@ void main(void) { int fd; + /* Let the device start before doing anything */ + k_sleep(K_SECONDS(2)); + fd = setup_socket(); if (fd < 0) { LOG_ERR("Cannot start CAN application (%d)", fd); return; } - rx(fd); + rx(INT_TO_POINTER(fd), NULL, NULL); } diff --git a/subsys/net/ip/connection.c b/subsys/net/ip/connection.c index 3942312a90078..820b49374eb0b 100644 --- a/subsys/net/ip/connection.c +++ b/subsys/net/ip/connection.c @@ -306,6 +306,10 @@ int net_conn_register(u16_t proto, u8_t family, if (net_sin(local_addr)->sin_addr.s_addr) { flags |= NET_CONN_LOCAL_ADDR_SPEC; } + } else if (IS_ENABLED(CONFIG_NET_SOCKETS_CAN) && + local_addr->sa_family == AF_CAN) { + memcpy(&conn->local_addr, local_addr, + sizeof(struct sockaddr_can)); } else { NET_ERR("Local address family not set"); goto error; diff --git a/subsys/net/ip/net_context.c b/subsys/net/ip/net_context.c index 6343f12fee9ea..428bf569bb493 100644 --- a/subsys/net/ip/net_context.c +++ b/subsys/net/ip/net_context.c @@ -330,7 +330,8 @@ int net_context_unref(struct net_context *context) net_tcp_unref(context); if (context->conn_handler) { - if (IS_ENABLED(CONFIG_NET_TCP) || IS_ENABLED(CONFIG_NET_UDP)) { + if (IS_ENABLED(CONFIG_NET_TCP) || IS_ENABLED(CONFIG_NET_UDP) || + IS_ENABLED(CONFIG_NET_SOCKETS_CAN)) { net_conn_unregister(context->conn_handler); } @@ -1645,6 +1646,7 @@ static enum net_verdict net_context_raw_packet_received( static int recv_raw(struct net_context *context, net_context_recv_cb_t cb, s32_t timeout, + struct sockaddr *local_addr, void *user_data) { int ret; @@ -1665,7 +1667,7 @@ static int recv_raw(struct net_context *context, ret = net_conn_register(net_context_get_ip_proto(context), net_context_get_family(context), - NULL, NULL, 0, 0, + NULL, local_addr, 0, 0, net_context_raw_packet_received, user_data, &context->conn_handler); @@ -1704,10 +1706,23 @@ int net_context_recv(struct net_context *context, } else { if (IS_ENABLED(CONFIG_NET_SOCKETS_PACKET) && net_context_get_family(context) == AF_PACKET) { - ret = recv_raw(context, cb, timeout, user_data); + ret = recv_raw(context, cb, timeout, NULL, user_data); } else if (IS_ENABLED(CONFIG_NET_SOCKETS_CAN) && net_context_get_family(context) == AF_CAN) { - ret = recv_raw(context, cb, timeout, user_data); + struct sockaddr_can local_addr = { + .can_family = AF_CAN, + }; + + ret = recv_raw(context, cb, timeout, + (struct sockaddr *)&local_addr, + user_data); + if (ret == -EALREADY) { + /* This is perfectly normal for CAN sockets. + * The SocketCAN will dispatch the packet to + * correct net_context listener. + */ + ret = 0; + } } else { ret = -EPROTOTYPE; } diff --git a/subsys/net/ip/net_core.c b/subsys/net/ip/net_core.c index e2deea7e82713..fd1f4023fae0a 100644 --- a/subsys/net/ip/net_core.c +++ b/subsys/net/ip/net_core.c @@ -426,9 +426,13 @@ static inline void l3_init(void) net_ipv4_autoconf_init(); -#if defined(CONFIG_NET_UDP) || defined(CONFIG_NET_TCP) - net_conn_init(); -#endif + if (IS_ENABLED(CONFIG_NET_UDP) || + IS_ENABLED(CONFIG_NET_TCP) || + IS_ENABLED(CONFIG_NET_SOCKETS_PACKET) || + IS_ENABLED(CONFIG_NET_SOCKETS_CAN)) { + net_conn_init(); + } + net_tcp_init(); net_route_init(); diff --git a/subsys/net/ip/net_shell.c b/subsys/net/ip/net_shell.c index 54e91c30e8dc0..f4caa5c2e94c5 100644 --- a/subsys/net/ip/net_shell.c +++ b/subsys/net/ip/net_shell.c @@ -935,6 +935,11 @@ static void conn_handler_cb(struct net_conn *conn, void *user_data) &net_sin(&conn->remote_addr)->sin_addr), ntohs(net_sin(&conn->remote_addr)->sin_port)); } else +#endif +#ifdef CONFIG_NET_L2_CANBUS + if (conn->local_addr.sa_family == AF_CAN) { + snprintk(addr_local, sizeof(addr_local), "-"); + } else #endif if (conn->local_addr.sa_family == AF_UNSPEC) { snprintk(addr_local, sizeof(addr_local), "AF_UNSPEC"); diff --git a/subsys/net/lib/sockets/Kconfig b/subsys/net/lib/sockets/Kconfig index a8c561ddefdb6..8e682cce72733 100644 --- a/subsys/net/lib/sockets/Kconfig +++ b/subsys/net/lib/sockets/Kconfig @@ -124,6 +124,14 @@ config NET_SOCKETS_CAN help The value depends on your network needs. +config NET_SOCKETS_CAN_RECEIVERS + int "How many simultaneous SocketCAN receivers are allowed" + default 1 + depends on NET_SOCKETS_CAN + help + The value tells how many sockets can receive data from same + Socket-CAN interface. + module = NET_SOCKETS module-dep = NET_LOG module-str = Log level for BSD sockets compatible API calls diff --git a/subsys/net/lib/sockets/sockets_can.c b/subsys/net/lib/sockets/sockets_can.c index ead553d2cbea2..7a1e57c1feb99 100644 --- a/subsys/net/lib/sockets/sockets_can.c +++ b/subsys/net/lib/sockets/sockets_can.c @@ -22,6 +22,17 @@ LOG_MODULE_REGISTER(net_sock_can, CONFIG_NET_SOCKETS_LOG_LEVEL); #include "sockets_internal.h" +#define MEM_ALLOC_TIMEOUT K_MSEC(50) + +struct can_recv { + struct net_if *iface; + struct net_context *ctx; + canid_t can_id; + canid_t can_mask; +}; + +static struct can_recv receivers[CONFIG_NET_SOCKETS_CAN_RECEIVERS]; + extern const struct socket_op_vtable sock_fd_op_vtable; static const struct socket_op_vtable can_sock_fd_op_vtable; @@ -77,32 +88,88 @@ static void zcan_received_cb(struct net_context *ctx, struct net_pkt *pkt, union net_proto_header *proto_hdr, int status, void *user_data) { - NET_DBG("ctx %p pkt %p st %d ud %p", ctx, pkt, status, user_data); + /* The ctx parameter is not really relevant here. It refers to first + * net_context that was used when registering CAN socket. + * In practice there can be multiple sockets that are interested in + * same CAN id packets. That is why we need to implement the dispatcher + * which will give the packet to correct net_context(s). + */ + struct net_pkt *clone = NULL; + int i; - /* if pkt is NULL, EOF */ - if (!pkt) { - struct net_pkt *last_pkt = k_fifo_peek_tail(&ctx->recv_q); + for (i = 0; i < ARRAY_SIZE(receivers); i++) { + struct zcan_frame *zframe = + (struct zcan_frame *)net_pkt_data(pkt); + struct can_frame frame; - if (!last_pkt) { - /* If there're no packets in the queue, recv() may - * be blocked waiting on it to become non-empty, - * so cancel that wait. + if (!receivers[i].ctx || + receivers[i].iface != net_pkt_iface(pkt)) { + continue; + } + + can_copy_zframe_to_frame(zframe, &frame); + + if ((frame.can_id & receivers[i].can_mask) != + (receivers[i].can_id & receivers[i].can_mask)) { + continue; + } + + /* If there are multiple receivers configured, we use the + * original net_pkt as a template, and just clone it to all + * recipients. This is done like this so that we avoid the + * original net_pkt being freed while we are cloning it. + */ + if (pkt != NULL && ARRAY_SIZE(receivers) > 1) { + /* There are multiple receivers, we need to clone + * the packet. */ - sock_set_eof(ctx); - k_fifo_cancel_wait(&ctx->recv_q); - NET_DBG("Marked socket %p as peer-closed", ctx); + clone = net_pkt_clone(pkt, MEM_ALLOC_TIMEOUT); + if (!clone) { + /* Sent the packet to at least one recipient + * if there is no memory to clone the packet. + */ + clone = pkt; + } } else { - net_pkt_set_eof(last_pkt, true); - NET_DBG("Set EOF flag on pkt %p", ctx); + clone = pkt; } - return; - } + ctx = receivers[i].ctx; + + NET_DBG("[%d] ctx %p pkt %p st %d", i, ctx, clone, status); + + /* if pkt is NULL, EOF */ + if (!clone) { + struct net_pkt *last_pkt = + k_fifo_peek_tail(&ctx->recv_q); - /* Normal packet */ - net_pkt_set_eof(pkt, false); + if (!last_pkt) { + /* If there're no packets in the queue, + * recv() may be blocked waiting on it to + * become non-empty, so cancel that wait. + */ + sock_set_eof(ctx); + k_fifo_cancel_wait(&ctx->recv_q); - k_fifo_put(&ctx->recv_q, pkt); + NET_DBG("Marked socket %p as peer-closed", ctx); + } else { + net_pkt_set_eof(last_pkt, true); + + NET_DBG("Set EOF flag on pkt %p", ctx); + } + + return; + } else { + /* Normal packet */ + net_pkt_set_eof(clone, false); + + k_fifo_put(&ctx->recv_q, clone); + } + } + + if (clone && clone != pkt) { + net_pkt_unref(pkt); + } } static int zcan_bind_ctx(struct net_context *ctx, const struct sockaddr *addr, @@ -232,20 +299,18 @@ static ssize_t zcan_recvfrom_ctx(struct net_context *ctx, void *buf, } if (net_pkt_read(pkt, (void *)&zframe, sizeof(zframe))) { + net_pkt_unref(pkt); + errno = EIO; return -1; } - if (!(flags & ZSOCK_MSG_PEEK)) { - net_pkt_unref(pkt); - } else { - net_pkt_cursor_init(pkt); - } - NET_ASSERT(recv_len == sizeof(struct can_frame)); can_copy_zframe_to_frame(&zframe, (struct can_frame *)buf); + net_pkt_unref(pkt); + return recv_len; } @@ -279,8 +344,86 @@ static ssize_t can_sock_write_vmeth(void *obj, const void *buffer, return zcan_sendto_ctx(obj, buffer, count, 0, NULL, 0); } +static bool is_already_attached(struct can_filter *filter, + struct net_if *iface, + struct net_context *ctx) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(receivers); i++) { + if (receivers[i].ctx != ctx && receivers[i].iface == iface && + ((receivers[i].can_id & receivers[i].can_mask) == + (UNALIGNED_GET(&filter->can_id) & + UNALIGNED_GET(&filter->can_mask)))) { + return true; + } + } + + return false; +} + +static int close_socket(struct net_context *ctx) +{ + const struct canbus_api *api; + struct net_if *iface; + struct device *dev; + + iface = net_context_get_iface(ctx); + dev = net_if_get_device(iface); + api = dev->driver_api; + + if (!api || !api->close) { + return -ENOTSUP; + } + + api->close(dev, net_context_get_filter_id(ctx)); + + return 0; +} + +static int can_close_socket(struct net_context *ctx) +{ + int i, ret; + + for (i = 0; i < ARRAY_SIZE(receivers); i++) { + if (receivers[i].ctx == ctx) { + struct can_filter filter; + + receivers[i].ctx = NULL; + + filter.can_id = receivers[i].can_id; + filter.can_mask = receivers[i].can_mask; + + if (!is_already_attached(&filter, + net_context_get_iface(ctx), + ctx)) { + /* We can detach now as there are no other + * sockets that have same filter. + */ + ret = close_socket(ctx); + if (ret < 0) { + return ret; + } + } + + return 0; + } + } + + return 0; +} + static int can_sock_ioctl_vmeth(void *obj, unsigned int request, va_list args) { + if (request == ZFD_IOCTL_CLOSE) { + int ret; + + ret = can_close_socket(obj); + if (ret < 0) { + NET_DBG("Cannot detach net_context %p (%d)", obj, ret); + } + } + return sock_fd_op_vtable.fd_vtable.ioctl(obj, request, args); } @@ -361,52 +504,172 @@ static int can_sock_getsockopt_vmeth(void *obj, int level, int optname, return zcan_getsockopt_ctx(obj, level, optname, optval, optlen); } +static int can_register_receiver(struct net_if *iface, struct net_context *ctx, + canid_t can_id, canid_t can_mask) +{ + int i; + + NET_DBG("Max %lu receivers", ARRAY_SIZE(receivers)); + + for (i = 0; i < ARRAY_SIZE(receivers); i++) { + if (receivers[i].ctx != NULL) { + continue; + } + + receivers[i].ctx = ctx; + receivers[i].iface = iface; + receivers[i].can_id = can_id; + receivers[i].can_mask = can_mask; + + return i; + } + + return -ENOENT; +} + +static void can_unregister_receiver(struct net_if *iface, + struct net_context *ctx, + canid_t can_id, canid_t can_mask) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(receivers); i++) { + if (receivers[i].ctx == ctx && + receivers[i].iface == iface && + receivers[i].can_id == can_id && + receivers[i].can_mask == can_mask) { + receivers[i].ctx = NULL; + return; + } + } +} + +static int can_register_filters(struct net_if *iface, struct net_context *ctx, + const struct can_filter *filters, int count) +{ + int i, ret; + + NET_DBG("Registering %d filters", count); + + for (i = 0; i < count; i++) { + ret = can_register_receiver(iface, ctx, filters[i].can_id, + filters[i].can_mask); + if (ret < 0) { + goto revert; + } + } + + return 0; + +revert: + for (i = 0; i < count; i++) { + can_unregister_receiver(iface, ctx, filters[i].can_id, + filters[i].can_mask); + } + + return ret; +} + +static void can_unregister_filters(struct net_if *iface, + struct net_context *ctx, + const struct can_filter *filters, + int count) +{ + int i; + + NET_DBG("Unregistering %d filters", count); + + for (i = 0; i < count; i++) { + can_unregister_receiver(iface, ctx, filters[i].can_id, + filters[i].can_mask); + } +} + static int can_sock_setsockopt_vmeth(void *obj, int level, int optname, const void *optval, socklen_t optlen) { - if (level == SOL_CAN_RAW) { - const struct canbus_api *api; - struct net_if *iface; - struct device *dev; + const struct canbus_api *api; + struct net_if *iface; + struct device *dev; + int ret; - /* The application must use can_filter and then we convert - * it to zcan_filter as the CANBUS drivers expects that. - */ - if (optname == CAN_RAW_FILTER && - optlen != sizeof(struct can_filter)) { - errno = EINVAL; - return -1; - } + if (level != SOL_CAN_RAW) { + return zcan_setsockopt_ctx(obj, level, optname, optval, optlen); + } - if (optval == NULL) { + /* The application must use CAN_filter and then we convert + * it to zcan_filter as the CANBUS drivers expects that. + */ + if (optname == CAN_RAW_FILTER && optlen != sizeof(struct can_filter)) { + errno = EINVAL; + return -1; + } + + if (optval == NULL) { + errno = EINVAL; + return -1; + } + + iface = net_context_get_iface(obj); + dev = net_if_get_device(iface); + api = dev->driver_api; + + if (!api || !api->setsockopt) { + errno = ENOTSUP; + return -1; + } + + if (optname == CAN_RAW_FILTER) { + int count, i; + + if (optlen % sizeof(struct can_filter) != 0) { errno = EINVAL; return -1; } - iface = net_context_get_iface(obj); - dev = net_if_get_device(iface); - api = dev->driver_api; + count = optlen / sizeof(struct can_filter); - if (!api || !api->setsockopt) { - errno = ENOTSUP; + ret = can_register_filters(iface, obj, optval, count); + if (ret < 0) { + errno = -ret; return -1; } - if (optname == CAN_RAW_FILTER) { + for (i = 0; i < count; i++) { + struct can_filter *filter; struct zcan_filter zfilter; + bool duplicate; + + filter = &((struct can_filter *)optval)[i]; + + /* If someone has already attached the same filter to + * same interface, we do not need to do it here again. + */ + duplicate = is_already_attached(filter, iface, obj); + if (duplicate) { + continue; + } + + can_copy_filter_to_zfilter(filter, &zfilter); + + ret = api->setsockopt(dev, obj, level, optname, + &zfilter, sizeof(zfilter)); + if (ret < 0) { + break; + } + } - can_copy_filter_to_zfilter((struct can_filter *)optval, - &zfilter); + if (ret < 0) { + can_unregister_filters(iface, obj, optval, count); - return api->setsockopt(dev, obj, level, optname, - &zfilter, sizeof(zfilter)); + errno = -ret; + return -1; } - return api->setsockopt(dev, obj, level, optname, - optval, optlen); + return 0; } - return zcan_setsockopt_ctx(obj, level, optname, optval, optlen); + return api->setsockopt(dev, obj, level, optname, optval, optlen); } static const struct socket_op_vtable can_sock_fd_op_vtable = {