| /* |
| * 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/capture.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 <sys/byteorder.h> |
| #include <net/ethernet.h> |
| #include <net/net_ip.h> |
| #include <string.h> |
| #include <random/rand32.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 uint16_t net_calc_chksum(struct net_pkt *pkt, uint8_t proto); |
| |
| static struct canbus_l2_ctx l2_ctx; |
| |
| static struct k_work_q net_canbus_workq; |
| K_KERNEL_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) |
| { |
| uint8_t version = (ip_hdr->vtc >> 4); |
| uint8_t tc = ((ip_hdr->vtc & 0x0F) << 4) | ((ip_hdr->tcflow & 0xF0 >> 4)); |
| uint32_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 net_pkt *pkt) |
| { |
| canbus_rx_report_err(pkt); |
| } |
| |
| static void submit_to_queue(struct k_fifo *queue, struct net_pkt *pkt) |
| { |
| k_fifo_put(queue, pkt); |
| } |
| |
| static void canbus_rx_report_err_from_isr(struct k_fifo *queue, |
| struct net_pkt *pkt) |
| { |
| submit_to_queue(queue, pkt); |
| } |
| |
| static void canbus_tx_timeout(struct _timeout *t) |
| { |
| struct canbus_isotp_tx_ctx *ctx = |
| CONTAINER_OF(t, struct canbus_isotp_tx_ctx, timeout); |
| struct net_if *iface = net_pkt_iface(ctx->pkt); |
| struct canbus_net_ctx *net_ctx = net_if_l2_data(iface); |
| |
| NET_ERR("TX Timeout. CTX: %p", ctx); |
| ctx->state = NET_CAN_TX_STATE_ERR; |
| |
| submit_to_queue(&net_ctx->tx_queue, ctx->pkt); |
| } |
| |
| static void canbus_rx_timeout(struct _timeout *t) |
| { |
| struct canbus_isotp_rx_ctx *ctx = |
| CONTAINER_OF(t, struct canbus_isotp_rx_ctx, timeout); |
| struct net_if *iface = net_pkt_iface(ctx->pkt); |
| struct canbus_net_ctx *net_ctx = net_if_l2_data(iface); |
| |
| NET_ERR("RX Timeout. CTX: %p", ctx); |
| ctx->state = NET_CAN_RX_STATE_TIMEOUT; |
| canbus_rx_report_err_from_isr(&net_ctx->rx_err_queue, 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); |
| struct net_if *iface = net_pkt_iface(ctx->pkt); |
| struct canbus_net_ctx *net_ctx = net_if_l2_data(iface); |
| |
| submit_to_queue(&net_ctx->tx_queue, ctx->pkt); |
| } |
| |
| static k_timeout_t canbus_stmin_to_ticks(uint8_t stmin) |
| { |
| /* 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)) { |
| return K_MSEC(NET_CAN_STMIN_MS_MAX); |
| } else if (stmin >= NET_CAN_STMIN_US_BEGIN) { |
| return K_USEC((stmin + 1 - NET_CAN_STMIN_US_BEGIN) * 100U); |
| } |
| |
| return K_MSEC(stmin); |
| } |
| |
| static uint16_t canbus_get_lladdr(struct net_linkaddr *net_lladdr) |
| { |
| NET_ASSERT(net_lladdr->len == sizeof(uint16_t)); |
| |
| return sys_be16_to_cpu(UNALIGNED_GET((uint16_t *)net_lladdr->addr)); |
| } |
| |
| static uint16_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 uint16_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) |
| { |
| uint16_t lladdr_be = UNALIGNED_GET((uint16_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(uint8_t state, |
| uint16_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(uint8_t state, |
| uint16_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 uint16_t canbus_receive_get_ff_length(struct net_pkt *pkt) |
| { |
| uint16_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, |
| uint8_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 uint32_t canbus_addr_to_id(uint16_t dest, uint16_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->id = canbus_addr_to_id(dest->addr, src->addr); |
| |
| if (mcast) { |
| frame->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(uint32_t err_flags, void *arg) |
| { |
| if (err_flags) { |
| NET_ERR("Sending FC frame failed: %d", err_flags); |
| } |
| } |
| |
| static int canbus_send_fc(const struct device *net_can_dev, |
| struct net_canbus_lladdr *dest, |
| struct net_canbus_lladdr *src, uint8_t fs) |
| { |
| const struct net_can_api *api = net_can_dev->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.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; |
| uint8_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; |
| const 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, |
| 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) |
| { |
| const 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; |
| uint16_t msg_len; |
| size_t new_pkt_len; |
| uint8_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, 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(uint32_t err_flags, void *arg) |
| { |
| struct net_pkt *pkt = (struct net_pkt *)arg; |
| struct canbus_isotp_tx_ctx *ctx = pkt->canbus_tx_ctx; |
| struct net_if *iface = net_pkt_iface(pkt); |
| struct canbus_net_ctx *net_ctx = net_if_l2_data(iface); |
| |
| 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; |
| } |
| |
| submit_to_queue(&net_ctx->tx_queue, pkt); |
| } |
| |
| static inline int canbus_send_cf(struct net_pkt *pkt) |
| { |
| struct canbus_isotp_tx_ctx *ctx = pkt->canbus_tx_ctx; |
| const struct device *net_can_dev = net_if_get_device(pkt->iface); |
| const struct net_can_api *api = net_can_dev->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, |
| 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, |
| 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 enum net_verdict canbus_process_fc_data(struct canbus_isotp_tx_ctx *ctx, |
| struct net_pkt *pkt) |
| { |
| struct net_buf *buf = pkt->frags; |
| uint8_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, |
| 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; |
| uint16_t src_addr = canbus_get_src_lladdr(pkt); |
| enum net_verdict ret; |
| struct net_if *iface = net_pkt_iface(pkt); |
| struct canbus_net_ctx *net_ctx = net_if_l2_data(iface); |
| |
| 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) { |
| submit_to_queue(&net_ctx->tx_queue, tx_ctx->pkt); |
| } |
| |
| return ret; |
| } |
| |
| static inline int canbus_send_ff(struct net_pkt *pkt, size_t len, bool mcast, |
| struct net_canbus_lladdr *dest_addr) |
| { |
| const struct device *net_can_dev = net_if_get_device(pkt->iface); |
| const struct net_can_api *api = net_can_dev->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.id, len, pkt->canbus_tx_ctx); |
| } else { |
| NET_DBG("Sending FF (unicast). ID: 0x%08x. PKT len: %zu" |
| " CTX: %p", |
| frame.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) |
| { |
| const struct device *net_can_dev = net_if_get_device(pkt->iface); |
| const struct net_can_api *api = net_can_dev->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); |
| struct net_if *iface = net_pkt_iface(ctx->pkt); |
| struct canbus_net_ctx *net_ctx = net_if_l2_data(iface); |
| |
| submit_to_queue(&net_ctx->tx_queue, ctx->pkt); |
| } |
| |
| 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; |
| |
| 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, |
| 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, |
| 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 uint16_t canbus_eth_to_can_addr(struct net_linkaddr *lladdr) |
| { |
| return (sys_be16_to_cpu(UNALIGNED_GET((uint16_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_capture_pkt(iface, 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; |
| uint8_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 *)((uint8_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; |
| uint8_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 = (uint8_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) |
| { |
| uint16_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 = (uint8_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 through " |
| "stack again", canbus_get_src_lladdr(pkt)); |
| } else { |
| NET_DBG("Push reassembled packet from " |
| "%02x:%02x:%02x:%02x:%02x:%02x through 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(const struct device *net_can_dev, |
| struct net_canbus_lladdr *ll_addr) |
| { |
| const struct net_can_api *api = net_can_dev->api; |
| struct zcan_frame frame; |
| int ret; |
| |
| canbus_set_frame_datalength(&frame, 0); |
| frame.rtr = CAN_REMOTEREQUEST; |
| frame.id_type = CAN_EXTENDED_IDENTIFIER; |
| frame.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(uint32_t err_flags, void *cb_arg) |
| { |
| static uint8_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); |
| const struct device *net_can_dev = net_if_get_device(iface); |
| const struct net_can_api *api = net_can_dev->api; |
| struct zcan_frame frame; |
| int ret; |
| |
| canbus_set_frame_datalength(&frame, 0); |
| frame.rtr = CAN_DATAFRAME; |
| frame.id_type = CAN_EXTENDED_IDENTIFIER; |
| frame.id = canbus_addr_to_id(NET_CAN_DAD_ADDR, |
| ntohs(UNALIGNED_GET((uint16_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(const struct device *net_can_dev, |
| int filter_id) |
| { |
| const struct net_can_api *api = net_can_dev->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(const 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->api; |
| struct zcan_filter filter = { |
| .id_type = CAN_EXTENDED_IDENTIFIER, |
| .rtr = CAN_DATAFRAME, |
| .rtr_mask = 1, |
| .id_mask = CAN_EXT_ID_MASK |
| }; |
| int filter_id; |
| |
| filter.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(const 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->api; |
| struct zcan_filter filter = { |
| .id_type = CAN_EXTENDED_IDENTIFIER, |
| .rtr = CAN_REMOTEREQUEST, |
| .rtr_mask = 1, |
| .id_mask = (CAN_NET_IF_ADDR_MASK << CAN_NET_IF_ADDR_DEST_POS) |
| }; |
| int filter_id; |
| |
| filter.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); |
| const 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, (uint8_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; |
| } |
| |
| static void queue_handler(struct canbus_net_ctx *ctx) |
| { |
| struct k_poll_event events[] = { |
| K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, |
| K_POLL_MODE_NOTIFY_ONLY, |
| &ctx->tx_queue), |
| K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, |
| K_POLL_MODE_NOTIFY_ONLY, |
| &ctx->rx_err_queue), |
| }; |
| |
| struct net_pkt *pkt; |
| int ret; |
| |
| while (1) { |
| ret = k_poll(events, ARRAY_SIZE(events), K_FOREVER); |
| if (ret) { |
| continue; |
| } |
| |
| if (events[0].state == K_POLL_STATE_FIFO_DATA_AVAILABLE) { |
| pkt = k_fifo_get(&ctx->tx_queue, K_NO_WAIT); |
| if (pkt != NULL) { |
| canbus_tx_work(pkt); |
| } |
| |
| events[0].state = K_POLL_STATE_NOT_READY; |
| } |
| |
| if (events[1].state == K_POLL_STATE_FIFO_DATA_AVAILABLE) { |
| pkt = k_fifo_get(&ctx->rx_err_queue, K_NO_WAIT); |
| if (pkt != NULL) { |
| rx_err_work_handler(pkt); |
| } |
| |
| events[1].state = K_POLL_STATE_NOT_READY; |
| } |
| } |
| } |
| |
| void net_6locan_init(struct net_if *iface) |
| { |
| struct canbus_net_ctx *ctx = net_if_l2_data(iface); |
| int thread_priority; |
| k_tid_t tid; |
| 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, K_SEM_MAX_LIMIT); |
| |
| /* This work queue should have precedence over the tx stream |
| * TODO thread_priority = tx_tc2thread(NET_TC_TX_COUNT -1) - 1; |
| */ |
| if (IS_ENABLED(CONFIG_NET_TC_THREAD_COOPERATIVE)) { |
| thread_priority = K_PRIO_COOP(CONFIG_NUM_COOP_PRIORITIES - 1); |
| } else { |
| thread_priority = K_PRIO_PREEMPT(6); |
| } |
| |
| k_work_queue_start(&net_canbus_workq, net_canbus_stack, |
| K_KERNEL_STACK_SIZEOF(net_canbus_stack), |
| thread_priority, NULL); |
| k_thread_name_set(&net_canbus_workq.thread, "isotp_work"); |
| NET_DBG("Workq started. Thread ID: %p", &net_canbus_workq.thread); |
| |
| k_fifo_init(&ctx->tx_queue); |
| k_fifo_init(&ctx->rx_err_queue); |
| |
| tid = k_thread_create(&ctx->queue_handler, ctx->queue_stack, |
| K_KERNEL_STACK_SIZEOF(ctx->queue_stack), |
| (k_thread_entry_t)queue_handler, |
| ctx, NULL, NULL, |
| thread_priority, 0, K_FOREVER); |
| if (!tid) { |
| NET_ERR("Cannot create queue handler thread for %d", |
| net_if_get_by_iface(iface)); |
| } else { |
| if (IS_ENABLED(CONFIG_THREAD_NAME)) { |
| #define MAX_NAME_LEN sizeof("isotp[01]") |
| char name[MAX_NAME_LEN]; |
| |
| snprintk(name, sizeof(name), "isotp[%d]", |
| net_if_get_by_iface(iface)); |
| k_thread_name_set(tid, name); |
| } |
| |
| k_thread_start(tid); |
| } |
| } |
| |
| static int canbus_enable(struct net_if *iface, bool state) |
| { |
| const struct device *net_can_dev = net_if_get_device(iface); |
| const struct net_can_api *api = net_can_dev->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); |