net: sockets: can: Add dispatcher

We need to dispatch the received CAN frame if there are multiple
sockets interested in the same CAN-IDs.

Signed-off-by: Jukka Rissanen <[email protected]>

Author: jukkar

subsys/net/ip/net_context.c

@@ -1708,6 +1708,13 @@ int net_context_recv(struct net_context *context,
 		} else if (IS_ENABLED(CONFIG_NET_SOCKETS_CAN) &&
 			   net_context_get_family(context) == AF_CAN) {
 			ret = recv_raw(context, cb, timeout, 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;
 		}

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

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,87 @@ 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 (receivers[i].iface != net_pkt_iface(pkt)) {
+			continue;
+		}
+
+		can_copy_zframe_to_frame(zframe, &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 ((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);
+
+			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);
+
+				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);
 
-	/* Normal packet */
-	net_pkt_set_eof(pkt, false);
+			k_fifo_put(&ctx->recv_q, clone);
+		}
+	}
 
-	k_fifo_put(&ctx->recv_q, pkt);
+	if (clone && clone != pkt) {
+		net_pkt_unref(pkt);
+	}
 }
 
 static int zcan_bind_ctx(struct net_context *ctx, const struct sockaddr *addr,
@@ -232,20 +298,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;
 }
 
@@ -361,52 +425,190 @@ 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 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 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;
 
-			can_copy_filter_to_zfilter((struct can_filter *)optval,
-						   &zfilter);
+			filter = &((struct can_filter *)optval)[i];
 
-			return api->setsockopt(dev, obj, level, optname,
-					       &zfilter, sizeof(zfilter));
+			/* 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;
+			}
+		}
+
+		if (ret < 0) {
+			can_unregister_filters(iface, obj, optval, count);
+
+			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 = {