diff --git a/drivers/can/can_net.c b/drivers/can/can_net.c index f887ee6a36e..213217e81fa 100644 --- a/drivers/can/can_net.c +++ b/drivers/can/can_net.c @@ -10,13 +10,38 @@ #include 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 { struct device *can_dev; struct net_if *iface; int recv_filter_id; - int mcast_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 u8_t can_get_frame_datalength(struct zcan_frame *frame) { /* TODO: Needs update when CAN FD support is added */ @@ -31,8 +56,14 @@ static inline u16_t can_get_lladdr_src(struct zcan_frame *frame) static inline u16_t can_get_lladdr_dest(struct zcan_frame *frame) { - return (frame->ext_id >> CAN_NET_IF_ADDR_DEST_POS) & - CAN_NET_IF_ADDR_MASK; + u16_t addr = (frame->ext_id >> CAN_NET_IF_ADDR_DEST_POS) & + CAN_NET_IF_ADDR_MASK; + + if (frame->ext_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) @@ -57,18 +88,6 @@ static inline void can_set_lladdr(struct net_pkt *pkt, struct zcan_frame *frame) net_buf_pull(buf, sizeof(u16_t)); } -static void net_can_iface_init(struct net_if *iface) -{ - struct device *dev = net_if_get_device(iface); - struct net_can_context *ctx = dev->driver_data; - - ctx->iface = iface; - - NET_DBG("Init CAN network interface %p dev %p", iface, dev); - - net_6locan_init(iface); -} - static int net_can_send(struct device *dev, const struct zcan_frame *frame, can_tx_callback_t cb, void *cb_arg, s32_t timeout) { @@ -120,7 +139,86 @@ drop: } } -static int can_attach_filter(struct device *dev, can_rx_callback_t cb, void *cb_arg, +static inline int attach_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, + .ext_id_mask = CAN_NET_IF_ADDR_MCAST_MASK | + CAN_NET_IF_ADDR_DEST_MASK + }; + const u16_t group = + sys_be16_to_cpu(UNALIGNED_GET((&addr->s6_addr16[7]))); + int filter_id; + + filter.ext_id = CAN_NET_IF_ADDR_MCAST_MASK | + ((group & CAN_NET_IF_ADDR_MASK) << + CAN_NET_IF_ADDR_DEST_POS); + + filter_id = can_attach_isr(ctx->can_dev, net_can_recv, + ctx, &filter); + if (filter_id == CAN_NET_FILTER_NOT_SET) { + return CAN_NET_FILTER_NOT_SET; + } + + NET_DBG("Attached mcast filter. Group 0x%04x. Filter:%d", + group, filter_id); + + return filter_id; +} + +static void mcast_cb(struct net_if *iface, const struct in6_addr *addr, + bool is_joined) +{ + struct device *dev = net_if_get_device(iface); + struct net_can_context *ctx = dev->driver_data; + struct mcast_filter_mapping *filter_mapping; + int filter_id; + + 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 = attach_mcast_filter(ctx, addr); + if (filter_id < 0) { + NET_ERR("Can't attach mcast filter"); + return; + } + + filter_mapping->addr = addr; + filter_mapping->filter_id = filter_id; + } else { + filter_mapping = can_get_mcast_filter(ctx, addr); + if (!filter_mapping) { + NET_ERR("No filter mapping found"); + return; + } + + can_detach(ctx->can_dev, filter_mapping->filter_id); + filter_mapping->addr = NULL; + } +} + +static void net_can_iface_init(struct net_if *iface) +{ + struct device *dev = net_if_get_device(iface); + struct net_can_context *ctx = dev->driver_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 can_attach_filter(struct device *dev, can_rx_callback_t cb, + void *cb_arg, const struct zcan_filter *filter) { struct net_can_context *ctx = dev->driver_data; @@ -163,30 +261,57 @@ static inline int can_attach_unicast_filter(struct net_can_context *ctx) return filter_id; } -static inline int can_attach_mcast_filter(struct net_can_context *ctx) +#ifdef CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR +static inline int can_attach_eth_bridge_filter(struct net_can_context *ctx) { - struct zcan_filter filter = { + const struct zcan_filter filter = { .id_type = CAN_EXTENDED_IDENTIFIER, .rtr = CAN_DATAFRAME, .rtr_mask = 1, - .ext_id_mask = CAN_NET_IF_ADDR_DEST_MASK + .ext_id_mask = CAN_NET_IF_ADDR_DEST_MASK, + .ext_id = (NET_CAN_ETH_TRANSLATOR_ADDR << + CAN_NET_IF_ADDR_DEST_POS) }; - int filter_id; - filter.ext_id = (NET_CAN_MULTICAST_ADDR << CAN_NET_IF_ADDR_DEST_POS); + int filter_id; filter_id = can_attach_isr(ctx->can_dev, net_can_recv, ctx, &filter); if (filter_id == CAN_NET_FILTER_NOT_SET) { - NET_ERR("Can't attach multicast filter"); + NET_ERR("Can't attach ETH bridge filter"); return CAN_NET_FILTER_NOT_SET; } - NET_DBG("Attached multicast filter %d", filter_id); + NET_DBG("Attached ETH bridge filter %d", filter_id); return filter_id; } +static inline int can_attach_all_mcast_filter(struct net_can_context *ctx) +{ + const struct zcan_filter filter = { + .id_type = CAN_EXTENDED_IDENTIFIER, + .rtr = CAN_DATAFRAME, + .rtr_mask = 1, + .ext_id_mask = CAN_NET_IF_ADDR_MCAST_MASK, + .ext_id = CAN_NET_IF_ADDR_MCAST_MASK + }; + + int filter_id; + + filter_id = can_attach_isr(ctx->can_dev, net_can_recv, + ctx, &filter); + if (filter_id == CAN_NET_FILTER_NOT_SET) { + NET_ERR("Can't attach all mcast filter"); + return CAN_NET_FILTER_NOT_SET; + } + + NET_DBG("Attached all mcast filter %d", filter_id); + + return filter_id; +} +#endif /*CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR*/ + static int can_enable(struct device *dev, bool enable) { struct net_can_context *ctx = dev->driver_data; @@ -199,21 +324,38 @@ static int can_enable(struct device *dev, bool enable) } } - if (ctx->mcast_filter_id == CAN_NET_FILTER_NOT_SET) { - ctx->mcast_filter_id = can_attach_mcast_filter(ctx); - if (ctx->mcast_filter_id < 0) { +#ifdef CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR + if (ctx->eth_bridge_filter_id == CAN_NET_FILTER_NOT_SET) { + ctx->eth_bridge_filter_id = can_attach_eth_bridge_filter(ctx); + if (ctx->eth_bridge_filter_id < 0) { can_detach(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_attach_all_mcast_filter(ctx); + if (ctx->all_mcast_filter_id < 0) { + can_detach(ctx->can_dev, ctx->recv_filter_id); + can_detach(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_detach(ctx->can_dev, ctx->recv_filter_id); } - if (ctx->mcast_filter_id != CAN_NET_FILTER_NOT_SET) { - can_detach(ctx->can_dev, ctx->mcast_filter_id); +#ifdef CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR + if (ctx->eth_bridge_filter_id != CAN_NET_FILTER_NOT_SET) { + can_detach(ctx->can_dev, ctx->eth_bridge_filter_id); } + + if (ctx->all_mcast_filter_id != CAN_NET_FILTER_NOT_SET) { + can_detach(ctx->can_dev, ctx->all_mcast_filter_id); + } +#endif /*CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR*/ } return 0; @@ -234,7 +376,10 @@ static int net_can_init(struct device *dev) struct net_can_context *ctx = dev->driver_data; ctx->recv_filter_id = CAN_NET_FILTER_NOT_SET; - ctx->mcast_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 (!can_dev) { NET_ERR("Can't get binding to CAN device %s", DT_CAN_1_NAME); diff --git a/drivers/ethernet/eth_stm32_hal.c b/drivers/ethernet/eth_stm32_hal.c index da9414a02c4..03a9d04330e 100644 --- a/drivers/ethernet/eth_stm32_hal.c +++ b/drivers/ethernet/eth_stm32_hal.c @@ -44,6 +44,29 @@ static u8_t dma_rx_buffer[ETH_RXBUFNB][ETH_RX_BUF_SIZE] __aligned(4); static u8_t dma_tx_buffer[ETH_TXBUFNB][ETH_TX_BUF_SIZE] __aligned(4); #endif /* CONFIG_ETH_STM32_HAL_USE_DTCM_FOR_DMA_BUFFER */ +#if defined(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) +#include + +static void set_mac_to_translator_addr(u8_t *mac_addr) +{ + /* Set the last 14 bit to the translator link layer address to avoid + * address collissions with the 6LoCAN address range + */ + mac_addr[4] = (mac_addr[4] & 0xC0) | (NET_CAN_ETH_TRANSLATOR_ADDR >> 8); + mac_addr[5] = NET_CAN_ETH_TRANSLATOR_ADDR & 0xFF; +} + +static void enable_canbus_eth_translator_filter(ETH_HandleTypeDef *heth, + u8_t *mac_addr) +{ + heth->Instance->MACA1LR = (mac_addr[3] << 24U) | (mac_addr[2] << 16U) | + (mac_addr[1] << 8U) | mac_addr[0]; + /*enable filter 1 and ignore byte 5 and 6 for filtering*/ + heth->Instance->MACA1HR = ETH_MACA1HR_AE | ETH_MACA1HR_MBC_HBits15_8 | + ETH_MACA1HR_MBC_HBits7_0; +} +#endif /*CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR*/ + static inline void disable_mcast_filter(ETH_HandleTypeDef *heth) { __ASSERT_NO_MSG(heth != NULL); @@ -341,6 +364,9 @@ static void eth_iface_init(struct net_if *iface) #if defined(CONFIG_ETH_STM32_HAL_RANDOM_MAC) generate_mac(dev_data->mac_addr); #endif +#if defined(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) + set_mac_to_translator_addr(dev_data->mac_addr); +#endif heth->Init.MACAddr = dev_data->mac_addr; @@ -376,6 +402,10 @@ static void eth_iface_init(struct net_if *iface) disable_mcast_filter(heth); +#if defined(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) + enable_canbus_eth_translator_filter(heth, dev_data->mac_addr); +#endif + LOG_DBG("MAC %02x:%02x:%02x:%02x:%02x:%02x", dev_data->mac_addr[0], dev_data->mac_addr[1], dev_data->mac_addr[2], dev_data->mac_addr[3], diff --git a/subsys/net/l2/canbus/6locan.c b/subsys/net/l2/canbus/6locan.c index 76f16be5a71..c12ce09f674 100644 --- a/subsys/net/l2/canbus/6locan.c +++ b/subsys/net/l2/canbus/6locan.c @@ -173,16 +173,16 @@ static u16_t canbus_get_lladdr(struct net_linkaddr *net_lladdr) static u16_t canbus_get_src_lladdr(struct net_pkt *pkt) { return net_pkt_lladdr_src(pkt)->type == NET_LINK_CANBUS ? - canbus_get_lladdr(net_pkt_lladdr_src(pkt)) : - NET_CAN_ETH_TRANSLATOR_ADDR; + canbus_get_lladdr(net_pkt_lladdr_src(pkt)) : + NET_CAN_ETH_TRANSLATOR_ADDR; } static u16_t canbus_get_dest_lladdr(struct net_pkt *pkt) { return net_pkt_lladdr_dst(pkt)->type == NET_LINK_CANBUS && - net_pkt_lladdr_dst(pkt)->len == sizeof(struct net_can_lladdr) ? - canbus_get_lladdr(net_pkt_lladdr_dst(pkt)) : - NET_CAN_ETH_TRANSLATOR_ADDR; + net_pkt_lladdr_dst(pkt)->len == sizeof(struct net_canbus_lladdr) ? + canbus_get_lladdr(net_pkt_lladdr_dst(pkt)) : + NET_CAN_ETH_TRANSLATOR_ADDR; } static inline bool canbus_dest_is_mcast(struct net_pkt *pkt) @@ -194,7 +194,7 @@ static inline bool canbus_dest_is_mcast(struct net_pkt *pkt) static bool canbus_src_is_translator(struct net_pkt *pkt) { - return ((get_src_lladdr(pkt) & CAN_NET_IF_ADDR_MASK) == + return ((canbus_get_src_lladdr(pkt) & CAN_NET_IF_ADDR_MASK) == NET_CAN_ETH_TRANSLATOR_ADDR); } @@ -204,9 +204,27 @@ static bool canbus_dest_is_translator(struct net_pkt *pkt) net_pkt_lladdr_dst(pkt)->len == sizeof(struct net_eth_addr)); } +#if defined(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) +static bool canbus_is_for_translator(struct net_pkt *pkt) +{ + return ((net_pkt_lladdr_dst(pkt)->type == NET_LINK_CANBUS) && + (canbus_get_lladdr(net_pkt_lladdr_dst(pkt)) == + NET_CAN_ETH_TRANSLATOR_ADDR)); +} +#else +#define canbus_is_for_translator(...) false +#endif /* CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR */ + static size_t canbus_total_lladdr_len(struct net_pkt *pkt) { - ARG_UNUSED(pkt); + /* This pkt will be farowarded to Ethernet + * Destination MAC is carried inline, source is going to be extended + */ + if (IS_ENABLED(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) && + canbus_is_for_translator(pkt)) { + return sizeof(struct net_eth_addr) + + sizeof(struct net_canbus_lladdr); + } return 2U * sizeof(struct net_canbus_lladdr); } @@ -222,10 +240,17 @@ static inline void canbus_cpy_lladdr(struct net_pkt *dst, struct net_pkt *src) lladdr->len = sizeof(struct net_canbus_lladdr); lladdr->type = NET_LINK_CANBUS; + if (IS_ENABLED(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) && + canbus_is_for_translator(src)) { + /* Make room for address extension */ + net_pkt_skip(dst, sizeof(struct net_eth_addr) - + sizeof(struct net_canbus_lladdr)); + } + lladdr = net_pkt_lladdr_src(dst); lladdr->addr = net_pkt_cursor_get_pos(dst); - if (src_is_translator(src)) { + if (canbus_src_is_translator(src)) { net_pkt_copy(dst, src, sizeof(struct net_eth_addr)); lladdr->len = sizeof(struct net_eth_addr); lladdr->type = NET_LINK_ETHERNET; @@ -336,6 +361,18 @@ static enum net_verdict canbus_finish_pkt(struct net_pkt *pkt) net_buf_pull(pkt->buffer, net_pkt_lladdr_dst(pkt)->len + net_pkt_lladdr_src(pkt)->len); + if (IS_ENABLED(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) && + canbus_is_for_translator(pkt)) { + /* Pull room for address extension */ + net_buf_pull(pkt->buffer, sizeof(struct net_eth_addr) - + net_pkt_lladdr_src(pkt)->len); + /* Set the destination address to the inline MAC and pull it */ + net_pkt_cursor_init(pkt); + net_pkt_lladdr_dst(pkt)->addr = net_pkt_cursor_get_pos(pkt); + net_pkt_lladdr_dst(pkt)->type = NET_LINK_ETHERNET; + net_pkt_lladdr_dst(pkt)->len = sizeof(struct net_eth_addr); + net_buf_pull(pkt->buffer, sizeof(struct net_eth_addr)); + } net_pkt_cursor_init(pkt); if (!net_6lo_uncompress(pkt)) { @@ -376,7 +413,12 @@ static void canbus_set_frame_addr_pkt(struct zcan_frame *frame, { struct net_canbus_lladdr src_addr; - src_addr.addr = canbus_get_lladdr(net_if_get_link_addr(pkt->iface)); + if (IS_ENABLED(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) && + net_pkt_lladdr_src(pkt)->type == NET_LINK_ETHERNET) { + src_addr.addr = NET_CAN_ETH_TRANSLATOR_ADDR; + } else { + src_addr.addr = canbus_get_lladdr(net_if_get_link_addr(pkt->iface)); + } canbus_set_frame_addr(frame, dest_addr, &src_addr, mcast); } @@ -855,6 +897,14 @@ static inline int canbus_send_ff(struct net_pkt *pkt, size_t len, bool mcast, frame.ext_id, len, pkt->canbus_tx_ctx); } +#if defined(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) + NET_ASSERT(mcast || !(canbus_dest_is_translator(pkt) && + canbus_src_is_translator(pkt))); + + if (canbus_src_is_translator(pkt)) { + len += net_pkt_lladdr_src(pkt)->len; + } +#endif if (!mcast && canbus_dest_is_translator(pkt)) { len += net_pkt_lladdr_dst(pkt)->len; } @@ -874,6 +924,14 @@ static inline int canbus_send_ff(struct net_pkt *pkt, size_t len, bool mcast, index += lladdr_inline->len; } + if (IS_ENABLED(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) && + net_pkt_lladdr_src(pkt)->type == NET_LINK_ETHERNET) { + lladdr_inline = net_pkt_lladdr_src(pkt); + memcpy(&frame.data[index], lladdr_inline->addr, + lladdr_inline->len); + index += lladdr_inline->len; + } + net_pkt_read(pkt, &frame.data[index], NET_CAN_DL - index); pkt->canbus_tx_ctx->rem_len -= NET_CAN_DL - index; @@ -1004,6 +1062,17 @@ static int canbus_send(struct net_if *iface, struct net_pkt *pkt) mcast = net_ipv6_is_addr_mcast(&NET_IPV6_HDR(pkt)->dst); if (mcast || canbus_dest_is_mcast(pkt)) { canbus_ipv6_mcast_to_dest(pkt, &dest_addr); + } else if (IS_ENABLED(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) && + net_pkt_lladdr_dst(pkt)->type == NET_LINK_ETHERNET) { + struct net_linkaddr *lladdr = net_pkt_lladdr_dst(pkt); + + lladdr->type = NET_LINK_CANBUS; + lladdr->len = sizeof(struct net_canbus_lladdr); + dest_addr.addr = canbus_eth_to_can_addr(net_pkt_lladdr_dst(pkt)); + NET_DBG("Translated %02x:%02x:%02x:%02x:%02x:%02x to 0x%04x", + lladdr->addr[0], lladdr->addr[1], lladdr->addr[2], + lladdr->addr[3], lladdr->addr[4], lladdr->addr[5], + dest_addr.addr); } else { dest_addr.addr = canbus_get_dest_lladdr(pkt); } @@ -1071,6 +1140,330 @@ static enum net_verdict canbus_process_frame(struct net_pkt *pkt) return ret; } +#if defined(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) +static void forward_eth_frame(struct net_pkt *pkt, struct net_if *canbus_iface) +{ + pkt->iface = canbus_iface; + net_if_queue_tx(canbus_iface, pkt); +} + +static struct net_ipv6_hdr *get_ip_hdr_from_eth_frame(struct net_pkt *pkt) +{ + return (struct net_ipv6_hdr *)((u8_t *)net_pkt_data(pkt) + + sizeof(struct net_eth_hdr)); +} + +enum net_verdict net_canbus_translate_eth_frame(struct net_if *iface, + struct net_pkt *pkt) +{ + struct net_linkaddr *lladdr = net_pkt_lladdr_dst(pkt); + struct net_pkt *clone_pkt; + struct net_if *canbus_iface; + + /* Forward only IPv6 frames */ + if ((get_ip_hdr_from_eth_frame(pkt)->vtc & 0xf0) != 0x60) { + return NET_CONTINUE; + } + + /* This frame is for the Ethernet interface itself */ + if (net_linkaddr_cmp(net_if_get_link_addr(iface), lladdr)) { + NET_DBG("Frame is for Ethernet only %02x:%02x:%02x:%02x:%02x:%02x", + lladdr->addr[0], lladdr->addr[1], lladdr->addr[2], + lladdr->addr[3], lladdr->addr[4], lladdr->addr[5]); + return NET_CONTINUE; + } + + canbus_iface = net_if_get_first_by_type(&NET_L2_GET_NAME(CANBUS)); + + net_pkt_cursor_init(pkt); + /* Forward all broadcasts */ + if (net_eth_is_addr_broadcast((struct net_eth_addr *)lladdr->addr) || + net_eth_is_addr_multicast((struct net_eth_addr *)lladdr->addr)) { + if (!canbus_iface || !net_if_is_up(canbus_iface)) { + NET_ERR("No canbus iface"); + return NET_CONTINUE; + } + + clone_pkt = net_pkt_shallow_clone(pkt, NET_CAN_ALLOC_TIMEOUT); + if (clone_pkt) { + NET_DBG("Frame is %scast %02x:%02x:%02x:%02x:%02x:%02x,", + net_eth_is_addr_broadcast( + (struct net_eth_addr *)lladdr->addr) ? "broad" : + "multi", + lladdr->addr[0], lladdr->addr[1], lladdr->addr[2], + lladdr->addr[3], lladdr->addr[4], lladdr->addr[5]); + net_pkt_set_family(clone_pkt, AF_INET6); + forward_eth_frame(clone_pkt, canbus_iface); + } else { + NET_ERR("PKT forwarding: cloning failed"); + } + + return NET_CONTINUE; + } + + if (!canbus_iface || !net_if_is_up(canbus_iface)) { + NET_ERR("No canbus iface"); + return NET_DROP; + } + + /* This frame is for 6LoCAN only */ + net_pkt_set_family(pkt, AF_INET6); + net_buf_pull(pkt->buffer, sizeof(struct net_eth_hdr)); + forward_eth_frame(pkt, canbus_iface); + NET_DBG("Frame is for CANBUS: 0x%04x", canbus_get_dest_lladdr(pkt)); + + return NET_OK; +} + +static void forward_can_frame(struct net_pkt *pkt, struct net_if *eth_iface) +{ + net_pkt_set_iface(pkt, eth_iface); + net_if_queue_tx(eth_iface, pkt); +} + +static void rewrite_icmp_hdr(struct net_pkt *pkt, struct net_icmp_hdr *icmp_hdr) +{ + int ret; + + net_pkt_cursor_init(pkt); + net_pkt_skip(pkt, sizeof(struct net_ipv6_hdr)); + ret = net_icmpv6_create(pkt, icmp_hdr->type, icmp_hdr->code); + if (ret) { + NET_ERR("Can't create ICMP HDR"); + return; + } + + net_pkt_cursor_init(pkt); + net_pkt_skip(pkt, sizeof(struct net_ipv6_hdr)); + ret = net_icmpv6_finalize(pkt); + if (ret) { + NET_ERR("Can't finalize ICMP HDR"); + } +} + +static void extend_llao(struct net_pkt *pkt, struct net_linkaddr *mac_addr) +{ + NET_PKT_DATA_ACCESS_CONTIGUOUS_DEFINE(icmp_access, struct net_icmp_hdr); + NET_PKT_DATA_ACCESS_CONTIGUOUS_DEFINE(icmp_opt_access, + struct net_icmpv6_nd_opt_hdr); + NET_PKT_DATA_ACCESS_CONTIGUOUS_DEFINE(llao_access, + struct net_eth_addr); + struct net_pkt_cursor cursor_backup; + struct net_icmp_hdr *icmp_hdr; + struct net_icmpv6_nd_opt_hdr *icmp_opt_hdr; + u8_t *llao, llao_backup[2]; + int ret; + + net_pkt_cursor_backup(pkt, &cursor_backup); + net_pkt_cursor_init(pkt); + net_pkt_set_overwrite(pkt, true); + net_pkt_skip(pkt, sizeof(struct net_ipv6_hdr)); + + if (net_calc_chksum(pkt, IPPROTO_ICMPV6) != 0U) { + NET_ERR("Invalid checksum"); + return; + } + + icmp_hdr = (struct net_icmp_hdr *)net_pkt_get_data(pkt, &icmp_access); + if (!icmp_hdr) { + NET_ERR("No ICMP6 HDR"); + goto done; + } + + switch (icmp_hdr->type) { + + case NET_ICMPV6_NS: + net_pkt_skip(pkt, sizeof(struct net_icmpv6_ns_hdr)); + NET_DBG("Extend NS SLLAO"); + break; + + case NET_ICMPV6_NA: + net_pkt_skip(pkt, sizeof(struct net_icmpv6_na_hdr)); + NET_DBG("Extend NA TLLAO"); + break; + + case NET_ICMPV6_RS: + net_pkt_skip(pkt, sizeof(struct net_icmpv6_rs_hdr)); + NET_DBG("Extend RS SLLAO"); + break; + + case NET_ICMPV6_RA: + net_pkt_skip(pkt, sizeof(struct net_icmpv6_ra_hdr)); + NET_DBG("Extend RA SLLAO"); + break; + + default: + goto done; + } + + net_pkt_acknowledge_data(pkt, &icmp_access); + + icmp_opt_hdr = (struct net_icmpv6_nd_opt_hdr *) + net_pkt_get_data(pkt, &icmp_opt_access); + if (!icmp_opt_hdr) { + NET_DBG("No LLAO opt to extend"); + goto done; + } + + net_pkt_acknowledge_data(pkt, &icmp_opt_access); + + if (icmp_opt_hdr->type != NET_ICMPV6_ND_OPT_SLLAO && + (icmp_hdr->type == NET_ICMPV6_NA && + icmp_opt_hdr->type != NET_ICMPV6_ND_OPT_TLLAO)) { + NET_DBG("opt was not LLAO"); + goto done; + } + + if (icmp_opt_hdr->len != 1) { + NET_ERR("LLAO len is %u. This should be 1 for 6LoCAN", + icmp_opt_hdr->len); + goto done; + } + + llao = (u8_t *)net_pkt_get_data(pkt, &llao_access); + if (!llao) { + NET_ERR("Can't read LLAO"); + goto done; + } + + memcpy(llao_backup, llao, sizeof(struct net_canbus_lladdr)); + memcpy(llao, mac_addr->addr, mac_addr->len); + + llao[4] = (llao[4] & 0xC0) | llao_backup[0]; + llao[5] = llao_backup[1]; + + ret = net_pkt_set_data(pkt, &llao_access); + if (ret < 0) { + NET_ERR("Failed to write MAC to LLAO [%d]", ret); + goto done; + } + + rewrite_icmp_hdr(pkt, icmp_hdr); + + NET_DBG("LLAO extended to %02x:%02x:%02x:%02x:%02x:%02x", + llao[0], llao[1], llao[2], llao[3], llao[4], llao[5]); + +done: + net_pkt_cursor_restore(pkt, &cursor_backup); +} + +static bool pkt_is_icmp(struct net_pkt *pkt) +{ + NET_PKT_DATA_ACCESS_CONTIGUOUS_DEFINE(ipv6_access, struct net_ipv6_hdr); + struct net_ipv6_hdr *ipv6_hdr = + (struct net_ipv6_hdr *)net_pkt_get_data(pkt, &ipv6_access); + + if (!ipv6_hdr) { + NET_ERR("No IPv6 HDR"); + return false; + } + + return (ipv6_hdr->nexthdr == IPPROTO_ICMPV6); +} + +static void swap_scr_lladdr(struct net_pkt *pkt, struct net_pkt *pkt_clone) +{ + struct net_linkaddr *lladdr_origin = net_pkt_lladdr_src(pkt); + struct net_linkaddr *lladdr_clone = net_pkt_lladdr_src(pkt_clone); + size_t offset; + + offset = lladdr_origin->addr - pkt->buffer->data; + lladdr_clone->addr = pkt_clone->buffer->data + offset; +} + +static void can_to_eth_lladdr(struct net_pkt *pkt, struct net_if *eth_iface, + bool bcast) +{ + u16_t src_can_addr = canbus_get_src_lladdr(pkt); + struct net_linkaddr *lladdr_src = net_pkt_lladdr_src(pkt); + struct net_linkaddr *lladdr_dst; + + if (bcast) { + lladdr_dst = net_pkt_lladdr_dst(pkt); + lladdr_dst->len = sizeof(struct net_eth_addr); + lladdr_dst->type = NET_LINK_ETHERNET; + lladdr_dst->addr = (u8_t *)net_eth_broadcast_addr()->addr; + } + + lladdr_src->addr = net_pkt_lladdr_src(pkt)->addr - + (sizeof(struct net_eth_addr) - lladdr_src->len); + memcpy(lladdr_src->addr, net_if_get_link_addr(eth_iface)->addr, + sizeof(struct net_eth_addr)); + lladdr_src->addr[4] = (lladdr_src->addr[4] & 0xC0) | (src_can_addr >> 8U); + lladdr_src->addr[5] = src_can_addr & 0xFF; + lladdr_src->len = sizeof(struct net_eth_addr); + lladdr_src->type = NET_LINK_ETHERNET; +} + +void translate_to_eth_frame(struct net_pkt *pkt, bool is_bcast, + struct net_if *eth_iface) +{ + struct net_linkaddr *dest_addr = net_pkt_lladdr_dst(pkt); + struct net_linkaddr *src_addr = net_pkt_lladdr_src(pkt); + bool is_icmp; + + is_icmp = pkt_is_icmp(pkt); + + can_to_eth_lladdr(pkt, eth_iface, is_bcast); + canbus_print_ip_hdr((struct net_ipv6_hdr *)net_pkt_cursor_get_pos(pkt)); + NET_DBG("Forward frame to %02x:%02x:%02x:%02x:%02x:%02x. " + "Src: %02x:%02x:%02x:%02x:%02x:%02x", + dest_addr->addr[0], dest_addr->addr[1], dest_addr->addr[2], + dest_addr->addr[3], dest_addr->addr[4], dest_addr->addr[5], + src_addr->addr[0], src_addr->addr[1], src_addr->addr[2], + src_addr->addr[3], src_addr->addr[4], src_addr->addr[5]); + + if (is_icmp) { + extend_llao(pkt, net_if_get_link_addr(eth_iface)); + } +} + +static enum net_verdict canbus_forward_to_eth(struct net_pkt *pkt) +{ + struct net_pkt *pkt_clone; + struct net_if *eth_iface; + + eth_iface = net_if_get_first_by_type(&NET_L2_GET_NAME(ETHERNET)); + if (!eth_iface || !net_if_is_up(eth_iface)) { + NET_ERR("No Ethernet iface available"); + if (canbus_is_for_translator(pkt)) { + return NET_DROP; + } else { + return NET_CONTINUE; + } + } + + if (canbus_dest_is_mcast(pkt)) { + /* net_pkt_clone can't be called on a pkt where + * net_buf_pull was called on. We need to clone + * first and then finish the pkt. + */ + pkt_clone = net_pkt_clone(pkt, NET_CAN_ALLOC_TIMEOUT); + if (pkt_clone) { + swap_scr_lladdr(pkt, pkt_clone); + canbus_finish_pkt(pkt_clone); + translate_to_eth_frame(pkt_clone, true, eth_iface); + forward_can_frame(pkt_clone, eth_iface); + NET_DBG("Len: %zu", net_pkt_get_len(pkt_clone)); + } else { + NET_ERR("Failed to clone pkt"); + } + } + + canbus_finish_pkt(pkt); + + if (net_pkt_lladdr_dst(pkt)->type == NET_LINK_ETHERNET) { + translate_to_eth_frame(pkt, false, eth_iface); + forward_can_frame(pkt, eth_iface); + return NET_OK; + } + + return NET_CONTINUE; +} +#else +#define canbus_forward_to_eth(...) 0 +#endif /*CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR*/ + static enum net_verdict canbus_recv(struct net_if *iface, struct net_pkt *pkt) { @@ -1090,9 +1483,14 @@ static enum net_verdict canbus_recv(struct net_if *iface, if (pkt->canbus_rx_ctx->state == NET_CAN_RX_STATE_FIN) { canbus_rx_finish(pkt); - canbus_finish_pkt(pkt); - canbus_print_ip_hdr(NET_IPV6_HDR(pkt)); - ret = NET_CONTINUE; + + if (IS_ENABLED(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR)) { + ret = canbus_forward_to_eth(pkt); + } else { + canbus_finish_pkt(pkt); + canbus_print_ip_hdr(NET_IPV6_HDR(pkt)); + ret = NET_CONTINUE; + } } else { NET_ERR("Expected pkt in FIN state"); } diff --git a/subsys/net/l2/canbus/Kconfig b/subsys/net/l2/canbus/Kconfig index c47f9a26c03..6d8556d7f8f 100644 --- a/subsys/net/l2/canbus/Kconfig +++ b/subsys/net/l2/canbus/Kconfig @@ -55,6 +55,16 @@ config NET_L2_CANBUS_BS the sender waits for a FC frame again an BS is reset. See also: ISO 15765-2:2016 +config NET_L2_CANBUS_ETH_TRANSLATOR + bool "Enable 6LoCAN to Ethernet translator" + depends on NET_L2_ETHERNET + help + Enable a 6LoCAN Ethernet translator. With this translator it is + possible to connect a 6LoCAN network to a Ethernet network directly, + via a Switch or trough a router. Messages that goes through the + translator have a special address and the MAC address is carried inline. + The packet is forwarded with uncompressed IPv6 header. + module = NET_L2_CANBUS module-dep = NET_LOG module-str = Log level for CANbus L2 layer diff --git a/subsys/net/l2/ethernet/ethernet.c b/subsys/net/l2/ethernet/ethernet.c index cc9e4df2b75..f4a3cfec188 100644 --- a/subsys/net/l2/ethernet/ethernet.c +++ b/subsys/net/l2/ethernet/ethernet.c @@ -20,6 +20,9 @@ LOG_MODULE_REGISTER(net_ethernet, CONFIG_NET_L2_ETHERNET_LOG_LEVEL); #endif #include +#if defined(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) +#include +#endif #include "arp.h" #include "eth_stats.h" @@ -244,6 +247,12 @@ static enum net_verdict ethernet_recv(struct net_if *iface, net_pkt_lladdr_dst(pkt)); } +#if defined(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR) + if (net_canbus_translate_eth_frame(iface, pkt) == NET_OK) { + return NET_OK; + } +#endif + if (!net_eth_is_addr_broadcast((struct net_eth_addr *)lladdr->addr) && !net_eth_is_addr_multicast((struct net_eth_addr *)lladdr->addr) && !net_eth_is_addr_lldp_multicast(