net: l2: canbus: Add support for canbus Ethernet translator
This commit adds support for a 6LoCAN Ethernet border translator. CAN frames with the translator CAN address are translated and forwarded to Ethernet. Ethernet frames with the first 34 bits matching the MAC address of the translator are translated and forwarded to 6LoCAN. Signed-off-by: Alexander Wachter <alexander.wachter@student.tugraz.at>
This commit is contained in:
Alexander Wachter
Jukka Rissanen
parent
64d495df45
commit
d864f5de5b
5 changed files with 633 additions and 41 deletions
|
|
@ -10,13 +10,38 @@
|
|||
#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 {
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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 <net/can.h>
|
||||
|
||||
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],
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -20,6 +20,9 @@ LOG_MODULE_REGISTER(net_ethernet, CONFIG_NET_L2_ETHERNET_LOG_LEVEL);
|
|||
#endif
|
||||
|
||||
#include <syscall_handler.h>
|
||||
#if defined(CONFIG_NET_L2_CANBUS_ETH_TRANSLATOR)
|
||||
#include <net/can.h>
|
||||
#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(
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue