/*
* Copyright (c) 2019 Alexander Wachter.
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <logging/log.h>
LOG_MODULE_REGISTER(net_l2_canbus, CONFIG_NET_L2_CANBUS_LOG_LEVEL);
#include <net/net_core.h>
#include <net/net_l2.h>
#include <net/net_if.h>
#include <net/net_pkt.h>
#include <net/can.h>
#include "canbus_internal.h"
#include <6lo.h>
#include <timeout_q.h>
#include <string.h>
#include <misc/byteorder.h>
#include <net/ethernet.h>
#include <net/net_ip.h>
#include <string.h>
#define NET_CAN_WFTMAX 2
#define NET_CAN_ALLOC_TIMEOUT K_MSEC(100)
/* Minimal separation time betwee frames */
#define NET_CAN_STMIN CONFIG_NET_L2_CANBUS_STMIN
#define NET_CAN_BS CONFIG_NET_L2_CANBUS_BS
#define NET_CAN_DAD_SEND_RETRY 5
#define NET_CAN_DAD_TIMEOUT K_MSEC(100)
extern u16_t net_calc_chksum(struct net_pkt *pkt, u8_t proto);
static struct canbus_l2_ctx l2_ctx;
static struct k_work_q net_canbus_workq;
K_THREAD_STACK_DEFINE(net_canbus_stack, 512);
char *net_sprint_addr(sa_family_t af, const void *addr);
#if CONFIG_NET_L2_CANBUS_LOG_LEVEL >= LOG_LEVEL_DBG
static void canbus_print_ip_hdr(struct net_ipv6_hdr *ip_hdr)
{
u8_t version = (ip_hdr->vtc >> 4);
u8_t tc = ((ip_hdr->vtc & 0x0F) << 4) | ((ip_hdr->tcflow & 0xF0 >> 4));
u32_t flow = ((ip_hdr->tcflow & 0x0F) << 16) | ip_hdr->flow;
NET_DBG("IP header: Version: 0x%x, TC: 0x%x, Flow Label: 0x%x, "
"Payload Length: %u, Next Header: 0x%x, Hop Limit: %u, "
"Src: %s, Dest: %s",
version, tc, flow, ntohs(ip_hdr->len), ip_hdr->nexthdr,
ip_hdr->hop_limit,
log_strdup(net_sprint_addr(AF_INET6, &ip_hdr->src)),
log_strdup(net_sprint_addr(AF_INET6, &ip_hdr->dst)));
}
#else
#define canbus_print_ip_hdr(...)
#endif
static void canbus_free_tx_ctx(struct canbus_isotp_tx_ctx *ctx)
{
k_mutex_lock(&l2_ctx.tx_ctx_mtx, K_FOREVER);
ctx->state = NET_CAN_TX_STATE_UNUSED;
k_mutex_unlock(&l2_ctx.tx_ctx_mtx);
}
static void canbus_free_rx_ctx(struct canbus_isotp_rx_ctx *ctx)
{
k_mutex_lock(&l2_ctx.rx_ctx_mtx, K_FOREVER);
ctx->state = NET_CAN_RX_STATE_UNUSED;
k_mutex_unlock(&l2_ctx.rx_ctx_mtx);
}
static void canbus_tx_finish(struct net_pkt *pkt)
{
struct canbus_isotp_tx_ctx *ctx = pkt->canbus_tx_ctx;
if (ctx->state != NET_CAN_TX_STATE_RESET) {
z_abort_timeout(&ctx->timeout);
}
canbus_free_tx_ctx(ctx);
net_pkt_unref(pkt);
k_sem_give(&l2_ctx.tx_sem);
}
static void canbus_rx_finish(struct net_pkt *pkt)
{
struct canbus_isotp_rx_ctx *ctx = pkt->canbus_rx_ctx;
canbus_free_rx_ctx(ctx);
}
static void canbus_tx_report_err(struct net_pkt *pkt)
{
canbus_tx_finish(pkt);
}
static void canbus_rx_report_err(struct net_pkt *pkt)
{
canbus_rx_finish(pkt);
net_pkt_unref(pkt);
}
static void rx_err_work_handler(struct k_work *item)
{
struct net_pkt *pkt = CONTAINER_OF(item, struct net_pkt, work);
canbus_rx_report_err(pkt);
}
static void canbus_rx_report_err_from_isr(struct net_pkt *pkt)
{
k_work_init(&pkt->work, rx_err_work_handler);
k_work_submit_to_queue(&net_canbus_workq, &pkt->work);
}
static void canbus_tx_timeout(struct _timeout *t)
{
struct canbus_isotp_tx_ctx *ctx =
CONTAINER_OF(t, struct canbus_isotp_tx_ctx, timeout);
NET_ERR("TX Timeout. CTX: %p", ctx);
ctx->state = NET_CAN_TX_STATE_ERR;
k_work_submit_to_queue(&net_canbus_workq, &ctx->pkt->work);
}
static void canbus_rx_timeout(struct _timeout *t)
{
struct canbus_isotp_rx_ctx *ctx =
CONTAINER_OF(t, struct canbus_isotp_rx_ctx, timeout);
NET_ERR("RX Timeout. CTX: %p", ctx);
ctx->state = NET_CAN_RX_STATE_TIMEOUT;
canbus_rx_report_err_from_isr(ctx->pkt);
}
static void canbus_st_min_timeout(struct _timeout *t)
{
struct canbus_isotp_tx_ctx *ctx =
CONTAINER_OF(t, struct canbus_isotp_tx_ctx, timeout);
k_work_submit_to_queue(&net_canbus_workq, &ctx->pkt->work);
}
static s32_t canbus_stmin_to_ticks(u8_t stmin)
{
s32_t time_ms;
/* According to ISO 15765-2 stmin should be 127ms if value is corrupt */
if (stmin > NET_CAN_STMIN_MAX ||
(stmin > NET_CAN_STMIN_MS_MAX && stmin < NET_CAN_STMIN_US_BEGIN)) {
time_ms = K_MSEC(NET_CAN_STMIN_MS_MAX);
} else if (stmin >= NET_CAN_STMIN_US_BEGIN) {
/* This should be 100us-900us but zephyr can't handle that */
time_ms = K_MSEC(1);
} else {
time_ms = stmin;
}
return k_ms_to_ticks_ceil32(time_ms);
}
static u16_t canbus_get_lladdr(struct net_linkaddr *net_lladdr)
{
NET_ASSERT(net_lladdr->len == sizeof(u16_t));
return sys_be16_to_cpu(UNALIGNED_GET((u16_t *)net_lladdr->addr));
}
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;
}
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_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)
{
u16_t lladdr_be = UNALIGNED_GET((u16_t *)net_pkt_lladdr_dst(pkt)->addr);
return (sys_be16_to_cpu(lladdr_be) & CAN_NET_IF_IS_MCAST_BIT);
}
static bool canbus_src_is_translator(struct net_pkt *pkt)
{
return ((canbus_get_src_lladdr(pkt) & CAN_NET_IF_ADDR_MASK) ==
NET_CAN_ETH_TRANSLATOR_ADDR);
}
static bool canbus_dest_is_translator(struct net_pkt *pkt)
{
return (net_pkt_lladdr_dst(pkt)->type == NET_LINK_ETHERNET ||
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)
{
/* 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);
}
static inline void canbus_cpy_lladdr(struct net_pkt *dst, struct net_pkt *src)
{
struct net_linkaddr *lladdr;
lladdr = net_pkt_lladdr_dst(dst);
lladdr->addr = net_pkt_cursor_get_pos(dst);
net_pkt_write(dst, net_pkt_lladdr_dst(src)->addr,
sizeof(struct net_canbus_lladdr));
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 (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;
NET_DBG("Inline MAC: %02x:%02x:%02x:%02x:%02x:%02x",
lladdr->addr[0], lladdr->addr[1], lladdr->addr[2],
lladdr->addr[3], lladdr->addr[4], lladdr->addr[5]);
} else {
net_pkt_write(dst, net_pkt_lladdr_src(src)->addr,
sizeof(struct net_canbus_lladdr));
lladdr->len = sizeof(struct net_canbus_lladdr);
lladdr->type = NET_LINK_CANBUS;
}
}
static struct canbus_isotp_rx_ctx *canbus_get_rx_ctx(u8_t state,
u16_t src_addr)
{
int i;
struct canbus_isotp_rx_ctx *ret = NULL;
k_mutex_lock(&l2_ctx.rx_ctx_mtx, K_FOREVER);
for (i = 0; i < ARRAY_SIZE(l2_ctx.rx_ctx); i++) {
struct canbus_isotp_rx_ctx *ctx = &l2_ctx.rx_ctx[i];
if (ctx->state == state) {
if (state == NET_CAN_RX_STATE_UNUSED) {
ctx->state = NET_CAN_RX_STATE_RESET;
z_init_timeout(&ctx->timeout);
ret = ctx;
break;
}
if (canbus_get_src_lladdr(ctx->pkt) == src_addr) {
ret = ctx;
break;
}
}
}
k_mutex_unlock(&l2_ctx.rx_ctx_mtx);
return ret;
}
static struct canbus_isotp_tx_ctx *canbus_get_tx_ctx(u8_t state,
u16_t dest_addr)
{
int i;
struct canbus_isotp_tx_ctx *ret = NULL;
k_mutex_lock(&l2_ctx.tx_ctx_mtx, K_FOREVER);
for (i = 0; i < ARRAY_SIZE(l2_ctx.tx_ctx); i++) {
struct canbus_isotp_tx_ctx *ctx = &l2_ctx.tx_ctx[i];
if (ctx->state == state) {
if (state == NET_CAN_TX_STATE_UNUSED) {
ctx->state = NET_CAN_TX_STATE_RESET;
z_init_timeout(&ctx->timeout);
ret = ctx;
break;
}
if (ctx->dest_addr.addr == dest_addr) {
ret = ctx;
break;
}
}
}
k_mutex_unlock(&l2_ctx.tx_ctx_mtx);
return ret;
}
static inline u16_t canbus_receive_get_ff_length(struct net_pkt *pkt)
{
u16_t len;
int ret;
ret = net_pkt_read_be16(pkt, &len);
if (ret < 0) {
NET_ERR("Can't read length");
}
return len & 0x0FFF;
}
static inline size_t canbus_get_sf_length(struct net_pkt *pkt)
{
size_t len;
net_buf_pull_u8(pkt->frags);
len = net_buf_pull_u8(pkt->frags);
return len;
}
static inline void canbus_set_frame_datalength(struct zcan_frame *frame,
u8_t length)
{
/* TODO: Needs update when CAN FD support is added */
NET_ASSERT(length <= NET_CAN_DL);
frame->dlc = length;
}
static enum net_verdict canbus_finish_pkt(struct net_pkt *pkt)
{
/* Pull the ll addresses to ignore them in upper layers */
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)) {
NET_ERR("6lo uncompression failed");
return NET_DROP;
}
net_pkt_cursor_init(pkt);
return NET_CONTINUE;
}
static inline u32_t canbus_addr_to_id(u16_t dest, u16_t src)
{
return (dest << CAN_NET_IF_ADDR_DEST_POS) |
(src << CAN_NET_IF_ADDR_SRC_POS);
}
static void canbus_set_frame_addr(struct zcan_frame *frame,
const struct net_canbus_lladdr *dest,
const struct net_canbus_lladdr *src,
bool mcast)
{
frame->id_type = CAN_EXTENDED_IDENTIFIER;
frame->rtr = CAN_DATAFRAME;
frame->ext_id = canbus_addr_to_id(dest->addr, src->addr);
if (mcast) {
frame->ext_id |= CAN_NET_IF_ADDR_MCAST_MASK;
}
}
static void canbus_set_frame_addr_pkt(struct zcan_frame *frame,
struct net_pkt *pkt,
struct net_canbus_lladdr *dest_addr,
bool mcast)
{
struct net_canbus_lladdr src_addr;
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);
}
static void canbus_fc_send_cb(u32_t err_flags, void *arg)
{
if (err_flags) {
NET_ERR("Sending FC frame failed: %d", err_flags);
}
}
static int canbus_send_fc(struct device *net_can_dev,
struct net_canbus_lladdr *dest,
struct net_canbus_lladdr *src, u8_t fs)
{
const struct net_can_api *api = net_can_dev->driver_api;
struct zcan_frame frame = {
.id_type = CAN_EXTENDED_IDENTIFIER,
.rtr = CAN_DATAFRAME,
};
NET_ASSERT(!(fs & NET_CAN_PCI_TYPE_MASK));
canbus_set_frame_addr(&frame, dest, src, false);
frame.data[0] = NET_CAN_PCI_TYPE_FC | fs;
/* BS (Block Size) */
frame.data[1] = NET_CAN_BS;
/* STmin (minimum Seperation Time) */
frame.data[2] = NET_CAN_STMIN;
canbus_set_frame_datalength(&frame, 3);
NET_DBG("Sending FC to ID: 0x%08x", frame.ext_id);
return api->send(net_can_dev, &frame, canbus_fc_send_cb, NULL,
K_FOREVER);
}
static int canbus_process_cf_data(struct net_pkt *frag_pkt,
struct canbus_isotp_rx_ctx *ctx)
{
struct net_pkt *pkt = ctx->pkt;
size_t data_len = net_pkt_get_len(frag_pkt) - 1;
u8_t pci;
int ret;
pci = net_buf_pull_u8(frag_pkt->frags);
if ((pci & NET_CAN_PCI_SN_MASK) != ctx->sn) {
NET_ERR("Sequence number missmatch. Expect %u, got %u",
ctx->sn, pci & NET_CAN_PCI_SN_MASK);
goto err;
}
ctx->sn++;
if (data_len > ctx->rem_len) {
NET_DBG("Remove padding of %d bytes", data_len - ctx->rem_len);
data_len = ctx->rem_len;
}
net_pkt_cursor_init(frag_pkt);
NET_DBG("Appending CF data to pkt (%d bytes)", data_len);
ret = net_pkt_copy(pkt, frag_pkt, data_len);
if (ret < 0) {
NET_ERR("Failed to write data to pkt [%d]", ret);
goto err;
}
ctx->rem_len -= data_len;
NET_DBG("%u bytes remaining", ctx->rem_len);
return 0;
err:
canbus_rx_report_err(pkt);
return -1;
}
static enum net_verdict canbus_process_cf(struct net_pkt *pkt)
{
struct canbus_isotp_rx_ctx *rx_ctx;
enum net_verdict ret;
struct device *net_can_dev;
struct net_canbus_lladdr src, dest;
bool mcast;
mcast = canbus_dest_is_mcast(pkt);
rx_ctx = canbus_get_rx_ctx(NET_CAN_RX_STATE_CF,
canbus_get_src_lladdr(pkt));
if (!rx_ctx) {
NET_INFO("Got CF but can't find a CTX that is waiting for it. "
"Src: 0x%04x", canbus_get_src_lladdr(pkt));
return NET_DROP;
}
z_abort_timeout(&rx_ctx->timeout);
ret = canbus_process_cf_data(pkt, rx_ctx);
if (ret < 0) {
return NET_DROP;
}
net_pkt_unref(pkt);
if (rx_ctx->rem_len == 0) {
rx_ctx->state = NET_CAN_RX_STATE_FIN;
ret = net_recv_data(pkt->iface, rx_ctx->pkt);
if (ret < 0) {
NET_ERR("Packet dropped by NET stack");
net_pkt_unref(pkt);
}
} else {
z_add_timeout(&rx_ctx->timeout, canbus_rx_timeout,
k_ms_to_ticks_ceil32(NET_CAN_BS_TIME));
if (NET_CAN_BS != 0 && !mcast) {
rx_ctx->act_block_nr++;
if (rx_ctx->act_block_nr >= NET_CAN_BS) {
NET_DBG("BS reached. Send FC");
src.addr = canbus_get_src_lladdr(pkt);
dest.addr = canbus_get_dest_lladdr(pkt);
net_can_dev = net_if_get_device(pkt->iface);
ret = canbus_send_fc(net_can_dev, &src, &dest,
NET_CAN_PCI_FS_CTS);
if (ret) {
NET_ERR("Failed to send FC CTS. BS: %d",
NET_CAN_BS);
canbus_rx_report_err(rx_ctx->pkt);
return NET_OK;
}
rx_ctx->act_block_nr = 0;
}
}
}
return NET_OK;
}
static enum net_verdict canbus_process_ff(struct net_pkt *pkt)
{
struct device *net_can_dev = net_if_get_device(pkt->iface);
struct canbus_isotp_rx_ctx *rx_ctx = NULL;
struct net_pkt *new_pkt = NULL;
int ret;
struct net_canbus_lladdr src, dest;
u16_t msg_len;
size_t new_pkt_len;
u8_t data_len;
bool mcast;
mcast = canbus_dest_is_mcast(pkt);
src.addr = canbus_get_src_lladdr(pkt);
dest.addr = canbus_get_dest_lladdr(pkt);
net_pkt_cursor_init(pkt);
msg_len = canbus_receive_get_ff_length(pkt);
new_pkt_len = msg_len + canbus_total_lladdr_len(pkt);
new_pkt = net_pkt_rx_alloc_with_buffer(pkt->iface, new_pkt_len,
AF_INET6, 0,
NET_CAN_ALLOC_TIMEOUT);
if (!new_pkt) {
NET_ERR("Failed to obtain net_pkt with size of %d", new_pkt_len);
if (!mcast) {
canbus_send_fc(net_can_dev, &src, &dest,
NET_CAN_PCI_FS_OVFLW);
}
goto err;
}
rx_ctx = canbus_get_rx_ctx(NET_CAN_RX_STATE_UNUSED, 0);
if (!rx_ctx) {
NET_ERR("No rx context left");
if (!mcast) {
canbus_send_fc(net_can_dev, &src, &dest,
NET_CAN_PCI_FS_OVFLW);
}
goto err;
}
rx_ctx->act_block_nr = 0;
rx_ctx->pkt = new_pkt;
new_pkt->canbus_rx_ctx = rx_ctx;
net_pkt_cursor_init(new_pkt);
data_len = net_pkt_remaining_data(pkt);
canbus_cpy_lladdr(new_pkt, pkt);
rx_ctx->sn = 1;
ret = net_pkt_copy(new_pkt, pkt, net_pkt_remaining_data(pkt));
if (ret) {
NET_ERR("Failed to write to pkt [%d]", ret);
goto err;
}
rx_ctx->rem_len = msg_len - data_len;
net_pkt_unref(pkt);
if (!mcast) {
/* switch src and dest because we are answering */
ret = canbus_send_fc(net_can_dev, &src, &dest,
NET_CAN_PCI_FS_CTS);
if (ret) {
NET_ERR("Failed to send FC CTS");
canbus_rx_report_err(new_pkt);
return NET_OK;
}
}
/* At this point we expect to get Consecutive frames directly */
z_add_timeout(&rx_ctx->timeout, canbus_rx_timeout,
k_ms_to_ticks_ceil32(NET_CAN_BS_TIME));
rx_ctx->state = NET_CAN_RX_STATE_CF;
NET_DBG("Processed FF from 0x%04x (%scast)"
"Msg length: %u CTX: %p",
src.addr, mcast ? "m" : "uni", msg_len, rx_ctx);
return NET_OK;
err:
if (new_pkt) {
net_pkt_unref(new_pkt);
}
if (rx_ctx) {
canbus_free_rx_ctx(rx_ctx);
}
return NET_DROP;
}
static enum net_verdict canbus_process_sf(struct net_pkt *pkt)
{
size_t data_len;
size_t pkt_len;
net_pkt_set_family(pkt, AF_INET6);
data_len = canbus_get_sf_length(pkt);
pkt_len = net_pkt_get_len(pkt);
if (data_len > pkt_len) {
NET_ERR("SF datalen > pkt size");
return NET_DROP;
}
if (pkt_len != data_len) {
NET_DBG("Remove padding (%d byte)", pkt_len - data_len);
net_pkt_update_length(pkt, data_len);
}
return canbus_finish_pkt(pkt);
}
static void canbus_tx_frame_isr(u32_t err_flags, void *arg)
{
struct net_pkt *pkt = (struct net_pkt *)arg;
struct canbus_isotp_tx_ctx *ctx = pkt->canbus_tx_ctx;
ctx->tx_backlog--;
if (ctx->state == NET_CAN_TX_STATE_WAIT_TX_BACKLOG) {
if (ctx->tx_backlog > 0) {
return;
}
ctx->state = NET_CAN_TX_STATE_FIN;
}
k_work_submit_to_queue(&net_canbus_workq, &pkt->work);
}
static inline int canbus_send_cf(struct net_pkt *pkt)
{
struct canbus_isotp_tx_ctx *ctx = pkt->canbus_tx_ctx;
struct device *net_can_dev = net_if_get_device(pkt->iface);
const struct net_can_api *api = net_can_dev->driver_api;
struct zcan_frame frame;
struct net_pkt_cursor cursor_backup;
int ret, len;
canbus_set_frame_addr_pkt(&frame, pkt, &ctx->dest_addr, ctx->is_mcast);
/* sn wraps around at 0xF automatically because it has a 4 bit size */
frame.data[0] = NET_CAN_PCI_TYPE_CF | ctx->sn;
len = MIN(ctx->rem_len, NET_CAN_DL - 1);
canbus_set_frame_datalength(&frame, len + 1);
net_pkt_cursor_backup(pkt, &cursor_backup);
net_pkt_read(pkt, &frame.data[1], len);
ret = api->send(net_can_dev, &frame, canbus_tx_frame_isr,
pkt, K_NO_WAIT);
if (ret == CAN_TX_OK) {
ctx->sn++;
ctx->rem_len -= len;
ctx->act_block_nr--;
ctx->tx_backlog++;
} else {
net_pkt_cursor_restore(pkt, &cursor_backup);
}
NET_DBG("CF sent. %d bytes left. CTX: %p", ctx->rem_len, ctx);
return ret ? ret : ctx->rem_len;
}
static void canbus_tx_work(struct net_pkt *pkt)
{
int ret;
struct canbus_isotp_tx_ctx *ctx = pkt->canbus_tx_ctx;
NET_ASSERT(ctx);
switch (ctx->state) {
case NET_CAN_TX_STATE_SEND_CF:
do {
ret = canbus_send_cf(ctx->pkt);
if (!ret) {
ctx->state = NET_CAN_TX_STATE_WAIT_TX_BACKLOG;
break;
}
if (ret < 0 && ret != CAN_TIMEOUT) {
NET_ERR("Failed to send CF. CTX: %p", ctx);
canbus_tx_report_err(pkt);
break;
}
if (ctx->opts.bs && !ctx->is_mcast &&
!ctx->act_block_nr) {
NET_DBG("BS reached. Wait for FC again. CTX: %p",
ctx);
ctx->state = NET_CAN_TX_STATE_WAIT_FC;
z_add_timeout(&ctx->timeout, canbus_tx_timeout,
k_ms_to_ticks_ceil32(NET_CAN_BS_TIME));
break;
} else if (ctx->opts.stmin) {
ctx->state = NET_CAN_TX_STATE_WAIT_ST;
break;
}
} while (ret > 0);
break;
case NET_CAN_TX_STATE_WAIT_ST:
NET_DBG("SM wait ST. CTX: %p", ctx);
z_add_timeout(&ctx->timeout, canbus_st_min_timeout,
k_ms_to_ticks_ceil32(canbus_stmin_to_ticks(ctx->opts.stmin)));
ctx->state = NET_CAN_TX_STATE_SEND_CF;
break;
case NET_CAN_TX_STATE_ERR:
NET_DBG("SM handle error. CTX: %p", ctx);
canbus_tx_report_err(pkt);
break;
case NET_CAN_TX_STATE_FIN:
canbus_tx_finish(ctx->pkt);
NET_DBG("SM finish. CTX: %p", ctx);
break;
default:
break;
}
}
static void canbus_tx_work_handler(struct k_work *item)
{
struct net_pkt *pkt = CONTAINER_OF(item, struct net_pkt, work);
canbus_tx_work(pkt);
}
static enum net_verdict canbus_process_fc_data(struct canbus_isotp_tx_ctx *ctx,
struct net_pkt *pkt)
{
struct net_buf *buf = pkt->frags;
u8_t pci;
pci = net_buf_pull_u8(buf);
switch (pci & NET_CAN_PCI_FS_MASK) {
case NET_CAN_PCI_FS_CTS:
if (net_buf_frags_len(buf) != 2) {
NET_ERR("Frame length error for CTS");
canbus_tx_report_err(pkt);
return NET_DROP;
}
ctx->state = NET_CAN_TX_STATE_SEND_CF;
ctx->wft = 0;
ctx->opts.bs = net_buf_pull_u8(buf);
ctx->opts.stmin = net_buf_pull_u8(buf);
ctx->act_block_nr = ctx->opts.bs;
z_abort_timeout(&ctx->timeout);
NET_DBG("Got CTS. BS: %d, STmin: %d. CTX: %p",
ctx->opts.bs, ctx->opts.stmin, ctx);
net_pkt_unref(pkt);
return NET_OK;
case NET_CAN_PCI_FS_WAIT:
NET_DBG("Got WAIT frame. CTX: %p", ctx);
z_abort_timeout(&ctx->timeout);
z_add_timeout(&ctx->timeout, canbus_tx_timeout,
k_ms_to_ticks_ceil32(NET_CAN_BS_TIME));
if (ctx->wft >= NET_CAN_WFTMAX) {
NET_INFO("Got to many wait frames. CTX: %p", ctx);
ctx->state = NET_CAN_TX_STATE_ERR;
}
ctx->wft++;
return NET_OK;
case NET_CAN_PCI_FS_OVFLW:
NET_ERR("Got overflow FC frame. CTX: %p", ctx);
ctx->state = NET_CAN_TX_STATE_ERR;
return NET_OK;
default:
NET_ERR("Invalid Frame Status. CTX: %p", ctx);
ctx->state = NET_CAN_TX_STATE_ERR;
break;
}
return NET_DROP;
}
static enum net_verdict canbus_process_fc(struct net_pkt *pkt)
{
struct canbus_isotp_tx_ctx *tx_ctx;
u16_t src_addr = canbus_get_src_lladdr(pkt);
enum net_verdict ret;
tx_ctx = canbus_get_tx_ctx(NET_CAN_TX_STATE_WAIT_FC, src_addr);
if (!tx_ctx) {
NET_WARN("Got FC frame from 0x%04x but can't find any "
"CTX waiting for it", src_addr);
return NET_DROP;
}
ret = canbus_process_fc_data(tx_ctx, pkt);
if (ret == NET_OK) {
k_work_submit_to_queue(&net_canbus_workq, &tx_ctx->pkt->work);
}
return ret;
}
static inline int canbus_send_ff(struct net_pkt *pkt, size_t len, bool mcast,
struct net_canbus_lladdr *dest_addr)
{
struct device *net_can_dev = net_if_get_device(pkt->iface);
const struct net_can_api *api = net_can_dev->driver_api;
struct net_linkaddr *lladdr_inline;
struct zcan_frame frame;
int ret, index = 0;
canbus_set_frame_addr_pkt(&frame, pkt, dest_addr, mcast);
canbus_set_frame_datalength(&frame, NET_CAN_DL);
if (mcast) {
NET_DBG("Sending FF (multicast). ID: 0x%08x. PKT len: %zu"
" CTX: %p",
frame.ext_id, len, pkt->canbus_tx_ctx);
} else {
NET_DBG("Sending FF (unicast). ID: 0x%08x. PKT len: %zu"
" CTX: %p",
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;
}
frame.data[index++] = NET_CAN_PCI_TYPE_FF | (len >> 8);
frame.data[index++] = len & 0xFF;
/* According to ISO, FF has sn 0 and is incremented to one
* alltough it's not part of the FF frame
*/
pkt->canbus_tx_ctx->sn = 1;
if (!mcast && canbus_dest_is_translator(pkt)) {
lladdr_inline = net_pkt_lladdr_dst(pkt);
memcpy(&frame.data[index], lladdr_inline->addr,
lladdr_inline->len);
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;
ret = api->send(net_can_dev, &frame, NULL, NULL, K_FOREVER);
if (ret != CAN_TX_OK) {
NET_ERR("Sending FF failed [%d]. CTX: %p",
ret, pkt->canbus_tx_ctx);
}
return ret;
}
static inline int canbus_send_single_frame(struct net_pkt *pkt, size_t len,
bool mcast,
struct net_canbus_lladdr *dest_addr)
{
struct device *net_can_dev = net_if_get_device(pkt->iface);
const struct net_can_api *api = net_can_dev->driver_api;
int index = 0;
struct zcan_frame frame;
struct net_linkaddr *lladdr_dest;
int ret;
canbus_set_frame_addr_pkt(&frame, pkt, dest_addr, mcast);
frame.data[index++] = NET_CAN_PCI_TYPE_SF;
frame.data[index++] = len;
NET_ASSERT((len + (!mcast && canbus_dest_is_translator(pkt)) ?
net_pkt_lladdr_dst(pkt)->len : 0) <= NET_CAN_DL - 1);
if (!mcast && canbus_dest_is_translator(pkt)) {
lladdr_dest = net_pkt_lladdr_dst(pkt);
memcpy(&frame.data[index], lladdr_dest->addr, lladdr_dest->len);
index += lladdr_dest->len;
}
net_pkt_read(pkt, &frame.data[index], len);
canbus_set_frame_datalength(&frame, len + index);
ret = api->send(net_can_dev, &frame, NULL, NULL, K_FOREVER);
if (ret != CAN_TX_OK) {
NET_ERR("Sending SF failed [%d]", ret);
return -EIO;
}
return 0;
}
static void canbus_start_sending_cf(struct _timeout *t)
{
struct canbus_isotp_tx_ctx *ctx =
CONTAINER_OF(t, struct canbus_isotp_tx_ctx, timeout);
k_work_submit_to_queue(&net_canbus_workq, &ctx->pkt->work);
}
static int canbus_send_multiple_frames(struct net_pkt *pkt, size_t len,
bool mcast,
struct net_canbus_lladdr *dest_addr)
{
struct canbus_isotp_tx_ctx *tx_ctx = NULL;
int ret;
tx_ctx = canbus_get_tx_ctx(NET_CAN_TX_STATE_UNUSED, 0);
if (!tx_ctx) {
NET_ERR("No tx context left");
k_sem_give(&l2_ctx.tx_sem);
return -EAGAIN;
}
tx_ctx->pkt = pkt;
pkt->canbus_tx_ctx = tx_ctx;
tx_ctx->is_mcast = mcast;
tx_ctx->dest_addr = *dest_addr;
tx_ctx->rem_len = net_pkt_get_len(pkt);
tx_ctx->tx_backlog = 0;
k_work_init(&pkt->work, canbus_tx_work_handler);
ret = canbus_send_ff(pkt, len, mcast, dest_addr);
if (ret != CAN_TX_OK) {
NET_ERR("Failed to send FF [%d]", ret);
canbus_tx_report_err(pkt);
return -EIO;
}
if (!mcast) {
z_add_timeout(&tx_ctx->timeout, canbus_tx_timeout,
k_ms_to_ticks_ceil32(NET_CAN_BS_TIME));
tx_ctx->state = NET_CAN_TX_STATE_WAIT_FC;
} else {
tx_ctx->state = NET_CAN_TX_STATE_SEND_CF;
z_add_timeout(&tx_ctx->timeout, canbus_start_sending_cf,
k_ms_to_ticks_ceil32(NET_CAN_FF_CF_TIME));
}
return 0;
}
static void canbus_ipv6_mcast_to_dest(struct net_pkt *pkt,
struct net_canbus_lladdr *dest_addr)
{
dest_addr->addr =
sys_be16_to_cpu(UNALIGNED_GET(&NET_IPV6_HDR(pkt)->dst.s6_addr16[7]));
}
static inline u16_t canbus_eth_to_can_addr(struct net_linkaddr *lladdr)
{
return (sys_be16_to_cpu(UNALIGNED_GET((u16_t *)&lladdr->addr[4])) &
CAN_NET_IF_ADDR_MASK);
}
static int canbus_send(struct net_if *iface, struct net_pkt *pkt)
{
int ret = 0;
int comp_len;
size_t pkt_len, inline_lladdr_len;
struct net_canbus_lladdr dest_addr;
bool mcast;
if (net_pkt_family(pkt) != AF_INET6) {
return -EINVAL;
}
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);
}
net_pkt_cursor_init(pkt);
canbus_print_ip_hdr((struct net_ipv6_hdr *)net_pkt_cursor_get_pos(pkt));
comp_len = net_6lo_compress(pkt, true);
if (comp_len < 0) {
NET_ERR("IPHC failed [%d]", comp_len);
return comp_len;
}
NET_DBG("IPv6 hdr compressed by %d bytes", comp_len);
net_pkt_cursor_init(pkt);
pkt_len = net_pkt_get_len(pkt);
NET_DBG("Send CAN frame to 0x%04x%s", dest_addr.addr,
mcast ? " (mcast)" : "");
inline_lladdr_len = (!mcast && canbus_dest_is_translator(pkt)) ?
net_pkt_lladdr_dst(pkt)->len : 0;
if ((pkt_len + inline_lladdr_len) > (NET_CAN_DL - 1)) {
k_sem_take(&l2_ctx.tx_sem, K_FOREVER);
ret = canbus_send_multiple_frames(pkt, pkt_len, mcast,
&dest_addr);
} else {
ret = canbus_send_single_frame(pkt, pkt_len, mcast, &dest_addr);
canbus_tx_finish(pkt);
}
return ret;
}
static enum net_verdict canbus_process_frame(struct net_pkt *pkt)
{
enum net_verdict ret = NET_DROP;
u8_t pci_type;
net_pkt_cursor_init(pkt);
ret = net_pkt_read_u8(pkt, &pci_type);
if (ret < 0) {
NET_ERR("Can't read PCI");
}
pci_type = (pci_type & NET_CAN_PCI_TYPE_MASK) >> NET_CAN_PCI_TYPE_POS;
switch (pci_type) {
case NET_CAN_PCI_SF:
ret = canbus_process_sf(pkt);
break;
case NET_CAN_PCI_FF:
ret = canbus_process_ff(pkt);
break;
case NET_CAN_PCI_CF:
ret = canbus_process_cf(pkt);
break;
case NET_CAN_PCI_FC:
ret = canbus_process_fc(pkt);
break;
default:
NET_ERR("Unknown PCI number %u", pci_type);
break;
}
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)
{
struct net_linkaddr *lladdr = net_pkt_lladdr_src(pkt);
enum net_verdict ret = NET_DROP;
if (pkt->canbus_rx_ctx) {
if (lladdr->len == sizeof(struct net_canbus_lladdr)) {
NET_DBG("Push reassembled packet from 0x%04x trough "
"stack again", canbus_get_src_lladdr(pkt));
} else {
NET_DBG("Push reassembled packet from "
"%02x:%02x:%02x:%02x:%02x:%02x trough stack again",
lladdr->addr[0], lladdr->addr[1], lladdr->addr[2],
lladdr->addr[3], lladdr->addr[4], lladdr->addr[5]);
}
if (pkt->canbus_rx_ctx->state == NET_CAN_RX_STATE_FIN) {
canbus_rx_finish(pkt);
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");
}
} else {
ret = canbus_process_frame(pkt);
}
return ret;
}
static inline int canbus_send_dad_request(struct device *net_can_dev,
struct net_canbus_lladdr *ll_addr)
{
const struct net_can_api *api = net_can_dev->driver_api;
struct zcan_frame frame;
int ret;
canbus_set_frame_datalength(&frame, 0);
frame.rtr = CAN_REMOTEREQUEST;
frame.id_type = CAN_EXTENDED_IDENTIFIER;
frame.ext_id = canbus_addr_to_id(ll_addr->addr,
sys_rand32_get() & CAN_NET_IF_ADDR_MASK);
ret = api->send(net_can_dev, &frame, NULL, NULL, K_FOREVER);
if (ret != CAN_TX_OK) {
NET_ERR("Sending DAD request failed [%d]", ret);
return -EIO;
}
return 0;
}
static void canbus_send_dad_resp_cb(u32_t err_flags, void *cb_arg)
{
static u8_t fail_cnt;
struct k_work *work = (struct k_work *)cb_arg;
if (err_flags) {
NET_ERR("Failed to send dad response [%u]", err_flags);
if (err_flags != CAN_TX_BUS_OFF &&
fail_cnt < NET_CAN_DAD_SEND_RETRY) {
k_work_submit_to_queue(&net_canbus_workq, work);
}
fail_cnt++;
} else {
fail_cnt = 0;
}
}
static inline void canbus_send_dad_response(struct k_work *item)
{
struct canbus_net_ctx *ctx = CONTAINER_OF(item, struct canbus_net_ctx,
dad_work);
struct net_if *iface = ctx->iface;
struct net_linkaddr *ll_addr = net_if_get_link_addr(iface);
struct device *net_can_dev = net_if_get_device(iface);
const struct net_can_api *api = net_can_dev->driver_api;
struct zcan_frame frame;
int ret;
canbus_set_frame_datalength(&frame, 0);
frame.rtr = CAN_DATAFRAME;
frame.id_type = CAN_EXTENDED_IDENTIFIER;
frame.ext_id = canbus_addr_to_id(NET_CAN_DAD_ADDR,
ntohs(UNALIGNED_GET((u16_t *) ll_addr->addr)));
ret = api->send(net_can_dev, &frame, canbus_send_dad_resp_cb, item,
K_FOREVER);
if (ret != CAN_TX_OK) {
NET_ERR("Sending SF failed [%d]", ret);
} else {
NET_INFO("DAD response sent");
}
}
static inline void canbus_detach_filter(struct device *net_can_dev,
int filter_id)
{
const struct net_can_api *api = net_can_dev->driver_api;
api->detach_filter(net_can_dev, filter_id);
}
static void canbus_dad_resp_cb(struct zcan_frame *frame, void *arg)
{
struct k_sem *dad_sem = (struct k_sem *)arg;
k_sem_give(dad_sem);
}
static inline
int canbus_attach_dad_resp_filter(struct device *net_can_dev,
struct net_canbus_lladdr *ll_addr,
struct k_sem *dad_sem)
{
const struct net_can_api *api = net_can_dev->driver_api;
struct zcan_filter filter = {
.id_type = CAN_EXTENDED_IDENTIFIER,
.rtr = CAN_DATAFRAME,
.rtr_mask = 1,
.ext_id_mask = CAN_EXT_ID_MASK
};
int filter_id;
filter.ext_id = canbus_addr_to_id(NET_CAN_DAD_ADDR, ll_addr->addr);
filter_id = api->attach_filter(net_can_dev, canbus_dad_resp_cb,
dad_sem, &filter);
if (filter_id == CAN_NO_FREE_FILTER) {
NET_ERR("Can't attach dad response filter");
}
return filter_id;
}
static void canbus_dad_request_cb(struct zcan_frame *frame, void *arg)
{
struct k_work *work = (struct k_work *)arg;
k_work_submit_to_queue(&net_canbus_workq, work);
}
static inline int canbus_attach_dad_filter(struct device *net_can_dev,
struct net_canbus_lladdr *ll_addr,
struct k_work *dad_work)
{
const struct net_can_api *api = net_can_dev->driver_api;
struct zcan_filter filter = {
.id_type = CAN_EXTENDED_IDENTIFIER,
.rtr = CAN_REMOTEREQUEST,
.rtr_mask = 1,
.ext_id_mask = (CAN_NET_IF_ADDR_MASK << CAN_NET_IF_ADDR_DEST_POS)
};
int filter_id;
filter.ext_id = canbus_addr_to_id(ll_addr->addr, 0);
filter_id = api->attach_filter(net_can_dev, canbus_dad_request_cb,
dad_work, &filter);
if (filter_id == CAN_NO_FREE_FILTER) {
NET_ERR("Can't attach dad filter");
}
return filter_id;
}
static inline int canbus_init_ll_addr(struct net_if *iface)
{
struct canbus_net_ctx *ctx = net_if_l2_data(iface);
struct device *net_can_dev = net_if_get_device(iface);
int dad_resp_filter_id = CAN_NET_FILTER_NOT_SET;
struct net_canbus_lladdr ll_addr;
int ret;
struct k_sem dad_sem;
#if defined(CONFIG_NET_L2_CANBUS_USE_FIXED_ADDR)
ll_addr.addr = CONFIG_NET_L2_CANBUS_FIXED_ADDR;
#else
do {
ll_addr.addr = sys_rand32_get() % (NET_CAN_MAX_ADDR + 1);
} while (ll_addr.addr < NET_CAN_MIN_ADDR);
#endif
/* Add address early for DAD response */
ctx->ll_addr = sys_cpu_to_be16(ll_addr.addr);
net_if_set_link_addr(iface, (u8_t *)&ctx->ll_addr, sizeof(ll_addr),
NET_LINK_CANBUS);
dad_resp_filter_id = canbus_attach_dad_resp_filter(net_can_dev, &ll_addr,
&dad_sem);
if (dad_resp_filter_id < 0) {
return -EIO;
}
/*
* Attach this filter now to defend this address instantly.
* This filter is not called for own DAD because loopback is not
* enabled.
*/
ctx->dad_filter_id = canbus_attach_dad_filter(net_can_dev, &ll_addr,
&ctx->dad_work);
if (ctx->dad_filter_id < 0) {
ret = -EIO;
goto dad_err;
}
k_sem_init(&dad_sem, 0, 1);
ret = canbus_send_dad_request(net_can_dev, &ll_addr);
if (ret) {
ret = -EIO;
goto dad_err;
}
ret = k_sem_take(&dad_sem, NET_CAN_DAD_TIMEOUT);
canbus_detach_filter(net_can_dev, dad_resp_filter_id);
dad_resp_filter_id = CAN_NET_FILTER_NOT_SET;
if (ret != -EAGAIN) {
NET_INFO("DAD failed");
ret = -EAGAIN;
goto dad_err;
}
return 0;
dad_err:
net_if_set_link_addr(iface, NULL, 0, NET_LINK_CANBUS);
if (ctx->dad_filter_id != CAN_NET_FILTER_NOT_SET) {
canbus_detach_filter(net_can_dev, ctx->dad_filter_id);
ctx->dad_filter_id = CAN_NET_FILTER_NOT_SET;
}
if (dad_resp_filter_id != CAN_NET_FILTER_NOT_SET) {
canbus_detach_filter(net_can_dev, dad_resp_filter_id);
}
return ret;
}
void net_6locan_init(struct net_if *iface)
{
struct canbus_net_ctx *ctx = net_if_l2_data(iface);
u8_t thread_priority;
int i;
NET_DBG("Init CAN net interface");
for (i = 0; i < ARRAY_SIZE(l2_ctx.tx_ctx); i++) {
l2_ctx.tx_ctx[i].state = NET_CAN_TX_STATE_UNUSED;
}
for (i = 0; i < ARRAY_SIZE(l2_ctx.rx_ctx); i++) {
l2_ctx.rx_ctx[i].state = NET_CAN_RX_STATE_UNUSED;
}
ctx->dad_filter_id = CAN_NET_FILTER_NOT_SET;
ctx->iface = iface;
k_work_init(&ctx->dad_work, canbus_send_dad_response);
k_mutex_init(&l2_ctx.tx_ctx_mtx);
k_mutex_init(&l2_ctx.rx_ctx_mtx);
k_sem_init(&l2_ctx.tx_sem, 1, INT_MAX);
/* This work queue should have precedence over the tx stream
* TODO thread_priority = tx_tc2thread(NET_TC_TX_COUNT -1) - 1;
*/
thread_priority = 6;
k_work_q_start(&net_canbus_workq, net_canbus_stack,
K_THREAD_STACK_SIZEOF(net_canbus_stack),
K_PRIO_COOP(thread_priority));
k_thread_name_set(&net_canbus_workq.thread, "isotp_work");
NET_DBG("Workq started. Thread ID: %p", &net_canbus_workq.thread);
}
static int canbus_enable(struct net_if *iface, bool state)
{
struct device *net_can_dev = net_if_get_device(iface);
const struct net_can_api *api = net_can_dev->driver_api;
struct canbus_net_ctx *ctx = net_if_l2_data(iface);
int dad_retry_cnt, ret;
NET_DBG("start to bring iface %p %s", iface, state ? "up" : "down");
if (state) {
for (dad_retry_cnt = CONFIG_NET_L2_CANBUS_DAD_RETRIES;
dad_retry_cnt; dad_retry_cnt--) {
ret = canbus_init_ll_addr(iface);
if (ret == 0) {
break;
} else if (ret == -EIO) {
return -EIO;
}
}
if (ret != 0) {
return ret;
}
} else {
if (ctx->dad_filter_id != CAN_NET_FILTER_NOT_SET) {
canbus_detach_filter(net_can_dev, ctx->dad_filter_id);
}
}
ret = api->enable(net_can_dev, state);
if (!ret) {
NET_DBG("Iface %p is up", iface);
}
return ret;
}
static enum net_l2_flags canbus_net_flags(struct net_if *iface)
{
return NET_L2_MULTICAST;
}
NET_L2_INIT(CANBUS_L2, canbus_recv, canbus_send, canbus_enable,
canbus_net_flags);