zephyr/drivers/can/can_net.c

/*
 * Copyright (c) 2019 Alexander Wachter
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <net/can.h>
#include <net/net_pkt.h>

#include <logging/log.h>
LOG_MODULE_REGISTER(net_can, CONFIG_CAN_NET_LOG_LEVEL);

struct mcast_filter_mapping {
	const struct in6_addr *addr;
	int filter_id;
};

struct net_can_context {
	const struct device *can_dev;
	struct net_if *iface;
	int recv_filter_id;
	struct mcast_filter_mapping mcast_mapping[NET_IF_MAX_IPV6_MADDR];
#ifdef CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR
	int eth_bridge_filter_id;
	int all_mcast_filter_id;
#endif
};

static struct net_if_mcast_monitor mcast_monitor;

struct mcast_filter_mapping *can_get_mcast_filter(struct net_can_context *ctx,
					      const struct in6_addr *addr)
{
	struct mcast_filter_mapping *map = ctx->mcast_mapping;

	for (int i = 0; i < NET_IF_MAX_IPV6_MADDR; i++) {
		if (map[i].addr == addr) {
			return &map[i];
		}
	}

	return NULL;
}

static inline uint8_t can_get_frame_datalength(struct zcan_frame *frame)
{
	/* TODO: Needs update when CAN FD support is added */
	return frame->dlc;
}

static inline uint16_t can_get_lladdr_src(struct zcan_frame *frame)
{
	return (frame->id >> CAN_NET_IF_ADDR_SRC_POS) &
	       CAN_NET_IF_ADDR_MASK;
}

static inline uint16_t can_get_lladdr_dest(struct zcan_frame *frame)
{
	uint16_t addr = (frame->id >> CAN_NET_IF_ADDR_DEST_POS) &
		     CAN_NET_IF_ADDR_MASK;

	if (frame->id & CAN_NET_IF_ADDR_MCAST_MASK) {
		addr |= CAN_NET_IF_IS_MCAST_BIT;
	}

	return addr;
}

static inline void can_set_lladdr(struct net_pkt *pkt, struct zcan_frame *frame)
{
	struct net_buf *buf = pkt->buffer;

	/* Put the destination at the beginning of the pkt.
	 * The net_canbus_lladdr has a size if 14 bits. To convert it to
	 * network byte order, we treat it as 16 bits here.
	 */
	net_pkt_lladdr_dst(pkt)->addr = buf->data;
	net_pkt_lladdr_dst(pkt)->len = sizeof(struct net_canbus_lladdr);
	net_pkt_lladdr_dst(pkt)->type = NET_LINK_CANBUS;
	net_buf_add_be16(buf, can_get_lladdr_dest(frame));
	net_buf_pull(buf, sizeof(uint16_t));

	/* Do the same as above for the source address */
	net_pkt_lladdr_src(pkt)->addr = buf->data;
	net_pkt_lladdr_src(pkt)->len = sizeof(struct net_canbus_lladdr);
	net_pkt_lladdr_src(pkt)->type = NET_LINK_CANBUS;
	net_buf_add_be16(buf, can_get_lladdr_src(frame));
	net_buf_pull(buf, sizeof(uint16_t));
}

static int net_can_send(const struct device *dev,
			const struct zcan_frame *frame,
			can_tx_callback_t cb, void *cb_arg, k_timeout_t timeout)
{
	struct net_can_context *ctx = dev->data;

	NET_ASSERT(frame->id_type == CAN_EXTENDED_IDENTIFIER);
	return can_send(ctx->can_dev, frame, timeout, cb, cb_arg);
}

static void net_can_recv(struct zcan_frame *frame, void *arg)
{
	struct net_can_context *ctx = (struct net_can_context *)arg;
	size_t pkt_size = 2 * sizeof(struct net_canbus_lladdr) +
			  can_get_frame_datalength(frame);
	struct net_pkt *pkt;
	int ret;

	NET_DBG("Frame with ID 0x%x received", frame->id);
	pkt = net_pkt_rx_alloc_with_buffer(ctx->iface, pkt_size, AF_UNSPEC, 0,
					   K_NO_WAIT);
	if (!pkt) {
		LOG_ERR("Failed to obtain net_pkt with size of %d", pkt_size);
		goto drop;
	}

	pkt->canbus_rx_ctx = NULL;

	can_set_lladdr(pkt, frame);
	net_pkt_cursor_init(pkt);
	ret = net_pkt_write(pkt, frame->data, can_get_frame_datalength(frame));
	if (ret) {
		LOG_ERR("Failed to append frame data to net_pkt");
		goto drop;
	}

	ret = net_recv_data(ctx->iface, pkt);
	if (ret < 0) {
		LOG_ERR("Packet dropped by NET stack");
		goto drop;
	}

	return;

drop:
	NET_INFO("pkt dropped");

	if (pkt) {
		net_pkt_unref(pkt);
	}
}

static inline int add_mcast_filter(struct net_can_context *ctx,
				   const struct in6_addr *addr)
{
	static struct zcan_filter filter = {
		.id_type = CAN_EXTENDED_IDENTIFIER,
		.rtr = CAN_DATAFRAME,
		.rtr_mask = 1,
		.id_mask = CAN_NET_IF_ADDR_MCAST_MASK |
			       CAN_NET_IF_ADDR_DEST_MASK
	};
	const uint16_t group =
		sys_be16_to_cpu(UNALIGNED_GET((&addr->s6_addr16[7])));
	int filter_id;

	filter.id = CAN_NET_IF_ADDR_MCAST_MASK |
			((group & CAN_NET_IF_ADDR_MASK) <<
			 CAN_NET_IF_ADDR_DEST_POS);

	filter_id = can_add_rx_filter(ctx->can_dev, net_can_recv,
				      ctx, &filter);
	if (filter_id == CAN_NET_FILTER_NOT_SET) {
		return CAN_NET_FILTER_NOT_SET;
	}

	NET_DBG("Added mcast filter. Group 0x%04x. Filter:%d",
		group, filter_id);

	return filter_id;
}

static void mcast_cb(struct net_if *iface, const struct net_addr *addr,
		     bool is_joined)
{
	const struct device *dev = net_if_get_device(iface);
	struct net_can_context *ctx = dev->data;
	struct mcast_filter_mapping *filter_mapping;
	int filter_id;

	if (addr->family != AF_INET6) {
		return;
	}

	if (is_joined) {
		filter_mapping = can_get_mcast_filter(ctx, NULL);
		if (!filter_mapping) {
			NET_ERR("Can't get a free filter_mapping");
		}

		filter_id = add_mcast_filter(ctx, &addr->in6_addr);
		if (filter_id < 0) {
			NET_ERR("Can't add mcast filter");
			return;
		}

		filter_mapping->addr = &addr->in6_addr;
		filter_mapping->filter_id = filter_id;
	} else {
		filter_mapping = can_get_mcast_filter(ctx, &addr->in6_addr);
		if (!filter_mapping) {
			NET_ERR("No filter mapping found");
			return;
		}

		can_remove_rx_filter(ctx->can_dev, filter_mapping->filter_id);
		filter_mapping->addr = NULL;
	}
}

static void net_can_iface_init(struct net_if *iface)
{
	const struct device *dev = net_if_get_device(iface);
	struct net_can_context *ctx = dev->data;

	ctx->iface = iface;

	NET_DBG("Init CAN network interface %p dev %p", iface, dev);

	net_6locan_init(iface);

	net_if_mcast_mon_register(&mcast_monitor, iface, mcast_cb);
}

static int net_can_add_rx_filter(const struct device *dev, can_rx_callback_t cb,
				 void *cb_arg, const struct zcan_filter *filter)
{
	struct net_can_context *ctx = dev->data;

	return can_add_rx_filter(ctx->can_dev, cb, cb_arg, filter);
}

static void net_can_remove_rx_filter(const struct device *dev, int filter_id)
{
	struct net_can_context *ctx = dev->data;

	if (filter_id >= 0) {
		can_remove_rx_filter(ctx->can_dev, filter_id);
	}
}

static inline int can_add_unicast_filter(struct net_can_context *ctx)
{
	struct zcan_filter filter = {
		.id_type = CAN_EXTENDED_IDENTIFIER,
		.rtr = CAN_DATAFRAME,
		.rtr_mask = 1,
		.id_mask = CAN_NET_IF_ADDR_DEST_MASK
	};
	const uint8_t *link_addr = net_if_get_link_addr(ctx->iface)->addr;
	const uint16_t dest = sys_be16_to_cpu(UNALIGNED_GET((uint16_t *) link_addr));
	int filter_id;

	filter.id = (dest << CAN_NET_IF_ADDR_DEST_POS);

	filter_id = can_add_rx_filter(ctx->can_dev, net_can_recv,
				      ctx, &filter);
	if (filter_id == CAN_NET_FILTER_NOT_SET) {
		NET_ERR("Can't add FF filter");
		return CAN_NET_FILTER_NOT_SET;
	}

	NET_DBG("Added FF filter %d", filter_id);

	return filter_id;
}

#ifdef CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR
static inline int can_add_eth_bridge_filter(struct net_can_context *ctx)
{
	const struct zcan_filter filter = {
		.id_type = CAN_EXTENDED_IDENTIFIER,
		.rtr = CAN_DATAFRAME,
		.rtr_mask = 1,
		.id_mask = CAN_NET_IF_ADDR_DEST_MASK,
		.id = (NET_CAN_ETH_TRANSLATOR_ADDR <<
			   CAN_NET_IF_ADDR_DEST_POS)
	};

	int filter_id;

	filter_id = can_add_rx_filter(ctx->can_dev, net_can_recv,
				      ctx, &filter);
	if (filter_id == CAN_NET_FILTER_NOT_SET) {
		NET_ERR("Can't add ETH bridge filter");
		return CAN_NET_FILTER_NOT_SET;
	}

	NET_DBG("Added ETH bridge filter %d", filter_id);

	return filter_id;
}

static inline int can_add_all_mcast_filter(struct net_can_context *ctx)
{
	const struct zcan_filter filter = {
		.id_type = CAN_EXTENDED_IDENTIFIER,
		.rtr = CAN_DATAFRAME,
		.rtr_mask = 1,
		.id_mask = CAN_NET_IF_ADDR_MCAST_MASK,
		.id = CAN_NET_IF_ADDR_MCAST_MASK
	};

	int filter_id;

	filter_id = can_add_rx_filter(ctx->can_dev, net_can_recv,
				      ctx, &filter);
	if (filter_id == CAN_NET_FILTER_NOT_SET) {
		NET_ERR("Can't add all mcast filter");
		return CAN_NET_FILTER_NOT_SET;
	}

	NET_DBG("Added all mcast filter %d", filter_id);

	return filter_id;
}
#endif /*CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR*/

static int can_enable(const struct device *dev, bool enable)
{
	struct net_can_context *ctx = dev->data;

	if (enable) {
		if (ctx->recv_filter_id == CAN_NET_FILTER_NOT_SET) {
			ctx->recv_filter_id = can_add_unicast_filter(ctx);
			if (ctx->recv_filter_id < 0) {
				return -EIO;
			}
		}

#ifdef CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR
		if (ctx->eth_bridge_filter_id == CAN_NET_FILTER_NOT_SET) {
			ctx->eth_bridge_filter_id = can_add_eth_bridge_filter(ctx);
			if (ctx->eth_bridge_filter_id < 0) {
				can_remove_rx_filter(ctx->can_dev, ctx->recv_filter_id);
				return -EIO;
			}
		}

		if (ctx->all_mcast_filter_id == CAN_NET_FILTER_NOT_SET) {
			ctx->all_mcast_filter_id = can_add_all_mcast_filter(ctx);
			if (ctx->all_mcast_filter_id < 0) {
				can_remove_rx_filter(ctx->can_dev, ctx->recv_filter_id);
				can_remove_rx_filter(ctx->can_dev, ctx->eth_bridge_filter_id);
				return -EIO;
			}
		}
#endif /*CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR*/
	} else {
		if (ctx->recv_filter_id != CAN_NET_FILTER_NOT_SET) {
			can_remove_rx_filter(ctx->can_dev, ctx->recv_filter_id);
		}

#ifdef CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR
		if (ctx->eth_bridge_filter_id != CAN_NET_FILTER_NOT_SET) {
			can_remove_rx_filter(ctx->can_dev, ctx->eth_bridge_filter_id);
		}

		if (ctx->all_mcast_filter_id != CAN_NET_FILTER_NOT_SET) {
			can_remove_rx_filter(ctx->can_dev, ctx->all_mcast_filter_id);
		}
#endif /*CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR*/
	}

	return 0;
}

static struct net_can_api net_can_api_inst = {
	.iface_api.init = net_can_iface_init,

	.send = net_can_send,
	.add_rx_filter = net_can_add_rx_filter,
	.remove_rx_filter = net_can_remove_rx_filter,
	.enable = can_enable,
};

static int net_can_init(const struct device *dev)
{
	const struct device *can_dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_canbus));
	struct net_can_context *ctx = dev->data;

	ctx->recv_filter_id = CAN_NET_FILTER_NOT_SET;
#ifdef CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR
	ctx->eth_bridge_filter_id = CAN_NET_FILTER_NOT_SET;
	ctx->all_mcast_filter_id = CAN_NET_FILTER_NOT_SET;
#endif /*CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR*/

	if (!device_is_ready(can_dev)) {
		NET_ERR("CAN device not ready");
		return -ENODEV;
	}

	NET_DBG("Init net CAN device %p (%s) for dev %p (%s)",
		dev, dev->name, can_dev, can_dev->name);

	ctx->can_dev = can_dev;

	return 0;
}

static struct net_can_context net_can_context_1;

NET_DEVICE_INIT(net_can_1, CONFIG_CAN_NET_NAME, net_can_init,
		NULL, &net_can_context_1, NULL,
		CONFIG_CAN_NET_INIT_PRIORITY,
		&net_can_api_inst,
		CANBUS_L2, NET_L2_GET_CTX_TYPE(CANBUS_L2), NET_CAN_MTU);