From 35f01673acff6335a415f44f4ecda27226af861e Mon Sep 17 00:00:00 2001 From: Alexander Wachter Date: Tue, 6 Aug 2019 16:12:24 +0200 Subject: [PATCH] net: l2: 6LoCAN implementation This commit is an implementation of 6LoCAN, a 6Lo adaption layer for Controller Area Networks. 6LoCAN is not yet standardised. Signed-off-by: Alexander Wachter --- CODEOWNERS | 1 + drivers/can/CMakeLists.txt | 1 + drivers/can/Kconfig | 1 + drivers/can/Kconfig.net | 37 + drivers/can/can_net.c | 258 +++++ include/net/can.h | 230 ++++ include/net/net_ip.h | 3 +- include/net/net_l2.h | 5 + include/net/net_linkaddr.h | 2 + include/net/net_pkt.h | 8 + samples/net/sockets/can/prj.conf | 2 +- samples/net/sockets/can/src/main.c | 4 +- subsys/net/ip/Kconfig | 7 +- subsys/net/ip/canbus_socket.c | 2 +- subsys/net/ip/ipv6_nbr.c | 2 + subsys/net/ip/net_context.c | 4 +- subsys/net/ip/net_if.c | 3 + subsys/net/ip/net_shell.c | 10 + subsys/net/ip/tcp.c | 4 +- subsys/net/ip/utils.c | 3 + subsys/net/l2/CMakeLists.txt | 2 +- subsys/net/l2/Kconfig | 7 +- subsys/net/l2/canbus/6locan.c | 1344 ++++++++++++++++++++++++ subsys/net/l2/canbus/CMakeLists.txt | 4 +- subsys/net/l2/canbus/Kconfig | 68 ++ subsys/net/l2/canbus/canbus.c | 49 - subsys/net/l2/canbus/canbus_internal.h | 98 ++ tests/net/all/prj.conf | 2 + 28 files changed, 2096 insertions(+), 65 deletions(-) create mode 100644 drivers/can/Kconfig.net create mode 100644 drivers/can/can_net.c create mode 100644 include/net/can.h create mode 100644 subsys/net/l2/canbus/6locan.c create mode 100644 subsys/net/l2/canbus/Kconfig delete mode 100644 subsys/net/l2/canbus/canbus.c create mode 100644 subsys/net/l2/canbus/canbus_internal.h diff --git a/CODEOWNERS b/CODEOWNERS index 8eee2f59bf..9e1570c2ac 100644 --- a/CODEOWNERS +++ b/CODEOWNERS @@ -353,6 +353,7 @@ /subsys/net/lib/sockets/ @jukkar @tbursztyka @pfalcon /subsys/net/lib/tls_credentials/ @rlubos /subsys/net/l2/ @jukkar @tbursztyka +/subsys/net/l2/canbus/ @alexanderwachter @jukkar /subsys/power/ @wentongwu @pizi-nordic /subsys/settings/ @nvlsianpu /subsys/shell/ @jakub-uC @nordic-krch diff --git a/drivers/can/CMakeLists.txt b/drivers/can/CMakeLists.txt index 76ba60bc04..7784e2e278 100644 --- a/drivers/can/CMakeLists.txt +++ b/drivers/can/CMakeLists.txt @@ -8,3 +8,4 @@ zephyr_sources_ifdef(CONFIG_CAN_MCUX_FLEXCAN can_mcux_flexcan.c) zephyr_sources_ifdef(CONFIG_USERSPACE can_handlers.c) zephyr_sources_ifdef(CONFIG_CAN_SHELL can_shell.c) +zephyr_sources_ifdef(CONFIG_CAN_NET can_net.c) diff --git a/drivers/can/Kconfig b/drivers/can/Kconfig index 978df35707..f1118992d3 100644 --- a/drivers/can/Kconfig +++ b/drivers/can/Kconfig @@ -64,5 +64,6 @@ source "drivers/can/Kconfig.stm32" source "drivers/can/Kconfig.mcux" source "drivers/can/Kconfig.mcp2515" source "drivers/can/Kconfig.loopback" +source "drivers/can/Kconfig.net" endif # CAN diff --git a/drivers/can/Kconfig.net b/drivers/can/Kconfig.net new file mode 100644 index 0000000000..f74da66ad6 --- /dev/null +++ b/drivers/can/Kconfig.net @@ -0,0 +1,37 @@ +# Kconfig.net - Configuration options for IPv6 over CAN + +# +# Copyright (c) 2019 Alexander Wachter +# +# SPDX-License-Identifier: Apache-2.0 +# + +config CAN_NET + bool "Enable 6loCAN network interface [EXPERIMENTAL]" + help + Enable IPv6 Networking over can (6loCAN) + +if CAN_NET + +module = CAN_NET +module-dep = NET_LOG +module-str = Log level for Network CAN +module-help = Enables logging for CAN L2 networking +source "subsys/net/Kconfig.template.log_config.net" + +config CAN_NET_NAME + string "Network device name" + default "NET_CAN" + help + Name of the network device driver for IPv6 over CAN. + +config CAN_NET_INIT_PRIORITY + int "CAN NET driver init priority" + default 80 + help + CAN NET device driver initialization priority. + Do not mess with it unless you know what you are doing. + Note that the priority needs to be lower than the net stack + so that it can start before the networking sub-system. + +endif #CAN_NET diff --git a/drivers/can/can_net.c b/drivers/can/can_net.c new file mode 100644 index 0000000000..f887ee6a36 --- /dev/null +++ b/drivers/can/can_net.c @@ -0,0 +1,258 @@ +/* + * Copyright (c) 2019 Alexander Wachter + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +#include +LOG_MODULE_REGISTER(net_can, CONFIG_CAN_NET_LOG_LEVEL); + +struct net_can_context { + struct device *can_dev; + struct net_if *iface; + int recv_filter_id; + int mcast_filter_id; +}; + +static inline u8_t can_get_frame_datalength(struct zcan_frame *frame) +{ + /* TODO: Needs update when CAN FD support is added */ + return frame->dlc; +} + +static inline u16_t can_get_lladdr_src(struct zcan_frame *frame) +{ + return (frame->ext_id >> CAN_NET_IF_ADDR_SRC_POS) & + CAN_NET_IF_ADDR_MASK; +} + +static inline u16_t can_get_lladdr_dest(struct zcan_frame *frame) +{ + return (frame->ext_id >> CAN_NET_IF_ADDR_DEST_POS) & + CAN_NET_IF_ADDR_MASK; +} + +static inline void can_set_lladdr(struct net_pkt *pkt, struct zcan_frame *frame) +{ + struct net_buf *buf = pkt->buffer; + + /* Put the destination at the beginning of the pkt. + * The net_canbus_lladdr has a size if 14 bits. To convert it to + * network byte order, we treat it as 16 bits here. + */ + net_pkt_lladdr_dst(pkt)->addr = buf->data; + net_pkt_lladdr_dst(pkt)->len = sizeof(struct net_canbus_lladdr); + net_pkt_lladdr_dst(pkt)->type = NET_LINK_CANBUS; + net_buf_add_be16(buf, can_get_lladdr_dest(frame)); + net_buf_pull(buf, sizeof(u16_t)); + + /* Do the same as above for the source address */ + net_pkt_lladdr_src(pkt)->addr = buf->data; + net_pkt_lladdr_src(pkt)->len = sizeof(struct net_canbus_lladdr); + net_pkt_lladdr_src(pkt)->type = NET_LINK_CANBUS; + net_buf_add_be16(buf, can_get_lladdr_src(frame)); + net_buf_pull(buf, sizeof(u16_t)); +} + +static void net_can_iface_init(struct net_if *iface) +{ + struct device *dev = net_if_get_device(iface); + struct net_can_context *ctx = dev->driver_data; + + ctx->iface = iface; + + NET_DBG("Init CAN network interface %p dev %p", iface, dev); + + net_6locan_init(iface); +} + +static int net_can_send(struct device *dev, const struct zcan_frame *frame, + can_tx_callback_t cb, void *cb_arg, s32_t timeout) +{ + struct net_can_context *ctx = dev->driver_data; + + NET_ASSERT(frame->id_type == CAN_EXTENDED_IDENTIFIER); + return can_send(ctx->can_dev, frame, timeout, cb, cb_arg); +} + +static void net_can_recv(struct zcan_frame *frame, void *arg) +{ + struct net_can_context *ctx = (struct net_can_context *)arg; + size_t pkt_size = 2 * sizeof(struct net_canbus_lladdr) + + can_get_frame_datalength(frame); + struct net_pkt *pkt; + int ret; + + NET_DBG("Frame with ID 0x%x received", frame->ext_id); + pkt = net_pkt_rx_alloc_with_buffer(ctx->iface, pkt_size, AF_UNSPEC, 0, + K_NO_WAIT); + if (!pkt) { + LOG_ERR("Failed to obtain net_pkt with size of %d", pkt_size); + goto drop; + } + + pkt->canbus_rx_ctx = NULL; + + can_set_lladdr(pkt, frame); + net_pkt_cursor_init(pkt); + ret = net_pkt_write(pkt, frame->data, can_get_frame_datalength(frame)); + if (ret) { + LOG_ERR("Failed to append frame data to net_pkt"); + goto drop; + } + + ret = net_recv_data(ctx->iface, pkt); + if (ret < 0) { + LOG_ERR("Packet dropped by NET stack"); + goto drop; + } + + return; + +drop: + NET_INFO("pkt dropped"); + + if (pkt) { + net_pkt_unref(pkt); + } +} + +static int can_attach_filter(struct device *dev, can_rx_callback_t cb, void *cb_arg, + const struct zcan_filter *filter) +{ + struct net_can_context *ctx = dev->driver_data; + + return can_attach_isr(ctx->can_dev, cb, cb_arg, filter); +} + +static void can_detach_filter(struct device *dev, int filter_id) +{ + struct net_can_context *ctx = dev->driver_data; + + if (filter_id >= 0) { + can_detach(ctx->can_dev, filter_id); + } +} + +static inline int can_attach_unicast_filter(struct net_can_context *ctx) +{ + struct zcan_filter filter = { + .id_type = CAN_EXTENDED_IDENTIFIER, + .rtr = CAN_DATAFRAME, + .rtr_mask = 1, + .ext_id_mask = CAN_NET_IF_ADDR_DEST_MASK + }; + const u8_t *link_addr = net_if_get_link_addr(ctx->iface)->addr; + const u16_t dest = sys_be16_to_cpu(UNALIGNED_GET((u16_t *) link_addr)); + int filter_id; + + filter.ext_id = (dest << CAN_NET_IF_ADDR_DEST_POS); + + filter_id = can_attach_isr(ctx->can_dev, net_can_recv, + ctx, &filter); + if (filter_id == CAN_NET_FILTER_NOT_SET) { + NET_ERR("Can't attach FF filter"); + return CAN_NET_FILTER_NOT_SET; + } + + NET_DBG("Attached FF filter %d", filter_id); + + return filter_id; +} + +static inline int can_attach_mcast_filter(struct net_can_context *ctx) +{ + struct zcan_filter filter = { + .id_type = CAN_EXTENDED_IDENTIFIER, + .rtr = CAN_DATAFRAME, + .rtr_mask = 1, + .ext_id_mask = CAN_NET_IF_ADDR_DEST_MASK + }; + int filter_id; + + filter.ext_id = (NET_CAN_MULTICAST_ADDR << CAN_NET_IF_ADDR_DEST_POS); + + filter_id = can_attach_isr(ctx->can_dev, net_can_recv, + ctx, &filter); + if (filter_id == CAN_NET_FILTER_NOT_SET) { + NET_ERR("Can't attach multicast filter"); + return CAN_NET_FILTER_NOT_SET; + } + + NET_DBG("Attached multicast filter %d", filter_id); + + return filter_id; +} + +static int can_enable(struct device *dev, bool enable) +{ + struct net_can_context *ctx = dev->driver_data; + + if (enable) { + if (ctx->recv_filter_id == CAN_NET_FILTER_NOT_SET) { + ctx->recv_filter_id = can_attach_unicast_filter(ctx); + if (ctx->recv_filter_id < 0) { + return -EIO; + } + } + + if (ctx->mcast_filter_id == CAN_NET_FILTER_NOT_SET) { + ctx->mcast_filter_id = can_attach_mcast_filter(ctx); + if (ctx->mcast_filter_id < 0) { + can_detach(ctx->can_dev, ctx->recv_filter_id); + return -EIO; + } + } + } else { + if (ctx->recv_filter_id != CAN_NET_FILTER_NOT_SET) { + can_detach(ctx->can_dev, ctx->recv_filter_id); + } + + if (ctx->mcast_filter_id != CAN_NET_FILTER_NOT_SET) { + can_detach(ctx->can_dev, ctx->mcast_filter_id); + } + } + + return 0; +} + +static struct net_can_api net_can_api_inst = { + .iface_api.init = net_can_iface_init, + + .send = net_can_send, + .attach_filter = can_attach_filter, + .detach_filter = can_detach_filter, + .enable = can_enable, +}; + +static int net_can_init(struct device *dev) +{ + struct device *can_dev = device_get_binding(DT_CAN_1_NAME); + struct net_can_context *ctx = dev->driver_data; + + ctx->recv_filter_id = CAN_NET_FILTER_NOT_SET; + ctx->mcast_filter_id = CAN_NET_FILTER_NOT_SET; + + if (!can_dev) { + NET_ERR("Can't get binding to CAN device %s", DT_CAN_1_NAME); + return -EIO; + } + + NET_DBG("Init net CAN device %p (%s) for dev %p (%s)", + dev, dev->config->name, can_dev, can_dev->config->name); + + ctx->can_dev = can_dev; + + return 0; +} + +static struct net_can_context net_can_context_1; + +NET_DEVICE_INIT(net_can_1, CONFIG_CAN_NET_NAME, net_can_init, + &net_can_context_1, NULL, + CONFIG_CAN_NET_INIT_PRIORITY, + &net_can_api_inst, + CANBUS_L2, NET_L2_GET_CTX_TYPE(CANBUS_L2), NET_CAN_MTU); diff --git a/include/net/can.h b/include/net/can.h new file mode 100644 index 0000000000..b28028ed8c --- /dev/null +++ b/include/net/can.h @@ -0,0 +1,230 @@ +/** @file + * @brief IPv6 Networking over CAN definitions. + * + * Definitions for IPv6 Networking over CAN support. + */ + +/* + * Copyright (c) 2019 Alexander Wachter + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_INCLUDE_NET_CAN_H_ +#define ZEPHYR_INCLUDE_NET_CAN_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include + +/** + * @brief IPv6 over CAN library + * @defgroup net_can Network Core Library + * @ingroup networking + * @{ + */ + +/** + * CAN L2 driver API. Used by 6loCAN. + */ + +/* + * Abbreviations + * BS Block Size + * CAN_DL CAN LL data size + * CF Consecutive Frame + * CTS Continue to send + * DLC Data length code + * FC Flow Control + * FF First Frame + * FS Flow Status + */ + +/** @cond INTERNAL_HIDDEN */ + +#define NET_CAN_DL 8 +#define NET_CAN_MTU 0x0FFF + +/* 0x3DFF - bit 4 to 10 must not be zero. Also prevent stuffing bit*/ +#define NET_CAN_MULTICAST_ADDR 0x3DFF +#define NET_CAN_DAD_ADDR 0x3DFE +#define NET_CAN_ETH_TRANSLATOR_ADDR 0x3DF0 +#define NET_CAN_MAX_ADDR 0x3DEF +#define NET_CAN_MIN_ADDR 0x0100 + +#define CAN_NET_IF_ADDR_MASK 0x3FFF +#define CAN_NET_IF_ADDR_BYTE_LEN 2U +#define CAN_NET_IF_ADDR_DEST_POS 14U +#define CAN_NET_IF_ADDR_DEST_MASK (CAN_NET_IF_ADDR_MASK << CAN_NET_IF_ADDR_DEST_POS) +#define CAN_NET_IF_ADDR_SRC_POS 0U +#define CAN_NET_IF_ADDR_SRC_MASK (CAN_NET_IF_ADDR_MASK << CAN_NET_IF_ADDR_SRC_POS) +#define CAN_NET_IF_ADDR_MCAST_POS 28U +#define CAN_NET_IF_ADDR_MCAST_MASK (1UL << CAN_NET_IF_ADDR_MCAST_POS) + +#define CAN_NET_IF_IS_MCAST_BIT (1U << 14) + +#define CAN_NET_FILTER_NOT_SET -1 + +/** @endcond */ + +/* + * +-----------+ +-----------+ + * | | | | + * | IPv6 | | IPv6 | + * | 6LoCAN | | 6LoCAN | + * | | | | + * +----+-+----+ +----+-+----+ + * | | | | +---+ + * +----+ | | | | / \ +-+ + * | | | | | | +--+ / \_/ \ + * +++ +---+ +--+----+ +-------+ +--+----+ +--/ \_/ \ + * | | \ / | \ / \ / | \ / / | + * | | X | X CAN X | X | Internet | + * | | / \ | / \ / \ | / \ \ / + * +++ +---+ +----+--+ +-------+ +----+--+ +--+ / + * | | +-------------------+ + * +----+ + */ +struct net_can_api { + /** + * The net_if_api must be placed in first position in this + * struct so that we are compatible with network interface API. + */ + struct net_if_api iface_api; + + /** Send a single CAN frame */ + int (*send)(struct device *dev, const struct zcan_frame *frame, + can_tx_callback_t cb, void *cb_arg, s32_t timeout); + /** Attach a filter with it's callback */ + int (*attach_filter)(struct device *dev, can_rx_callback_t cb, + void *cb_arg, const struct zcan_filter *filter); + /** Detach a filter */ + void (*detach_filter)(struct device *dev, int filter_id); + /** Enable or disable the reception of frames for net CAN */ + int (*enable)(struct device *dev, bool enable); +}; + +/** @cond INTERNAL_HIDDEN */ + +#define CANBUS_L2_CTX_TYPE struct net_canbus_context * + +/** + * Context for canbus net device. + */ +struct canbus_net_ctx { + /** Filter ID for link layer duplicate address detection. */ + int dad_filter_id; + /** Work item for responding to link layer DAD requests. */ + struct k_work dad_work; + /** The interface associated with this device */ + struct net_if *iface; + /** The link layer address chosen for this interface */ + u16_t ll_addr; +}; + +/** + * Canbus link layer addresses have a length of 14 bit for source and destination. + * Both together are 28 bit to fit a CAN extended identifier with 29 bit length. + */ +struct net_canbus_lladdr { + u16_t addr : 14; +}; + +/** + * STmin is split in two valid ranges: + * 0-127: 0ms-127ms + * 128-240: Reserved + * 241-249: 100us-900us (multiples of 100us) + * 250- : Reserved + */ +struct canbus_fc_opts { + /** Block size. Number of CF PDUs before next CF is sent */ + u8_t bs; + /**< Minimum separation time. Min time between frames */ + u8_t stmin; +}; + +/** + * Context for a transmission of messages that didn't fit in a single frame. + * These messages Start with a FF (First Frame) that is in case of unicast + * acknowledged by a FC (Frame Control). After that, one or more CF + * (Consecutive frames) carry the rest of the message. + */ +struct canbus_isotp_tx_ctx { + /** Pkt containing the data to transmit */ + struct net_pkt *pkt; + /** Timeout for TX Timeout and separation time */ + struct _timeout timeout; + /** Frame Control options received from FC frame */ + struct canbus_fc_opts opts; + /** CAN destination address */ + struct net_canbus_lladdr dest_addr; + /** Remaining data to transmit in bytes */ + u16_t rem_len; + /** Number of bytes in the tx queue */ + s8_t tx_backlog; + /** State of the transmission */ + u8_t state; + /** Actual block number that is transmitted. Counts from BS to 0 */ + u8_t act_block_nr; + /** Number of WAIT frames received */ + u8_t wft; + /** Sequence number that is added to CF */ + u8_t sn : 4; + /** Transmission is multicast */ + u8_t is_mcast : 1; +}; + +/** + * Context for reception of messages that are not single frames. + * This is the counterpart of the canbus_isotp_tx_ctx. + */ +struct canbus_isotp_rx_ctx { + /** Pkt that is large enough to hold the entire message */ + struct net_pkt *pkt; + /** Timeout for RX timeout*/ + struct _timeout timeout; + /** Remaining data to receive. Goes from message length to zero */ + u16_t rem_len; + /** State of the reception */ + u8_t state; + /** Number of frames received in this block. Counts from BS to 0 */ + u8_t act_block_nr; + /** Number of WAIT frames transmitted */ + u8_t wft; + /** Expected sequence number in CF */ + u8_t sn : 4; +}; + +/** + * Initialization of the canbus L2. + * + * This function starts the TX workqueue and does some initialization. + */ +void net_6locan_init(struct net_if *iface); + +/** + * Ethernet frame input function for Ethernet to 6LoCAN translation + * + * This function checks the destination link layer address for addresses + * that has to be forwarded. Frames that need to be forwarded are forwarded here. + */ +enum net_verdict net_canbus_translate_eth_frame(struct net_if *iface, + struct net_pkt *pkt); + +/** @endcond */ + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /* ZEPHYR_INCLUDE_NET_CAN_H_ */ diff --git a/include/net/net_ip.h b/include/net/net_ip.h index 8cde461cb4..22ca64b74b 100644 --- a/include/net/net_ip.h +++ b/include/net/net_ip.h @@ -1038,7 +1038,8 @@ static inline void net_ipv6_addr_create_iid(struct in6_addr *addr, /* The generated IPv6 shall not toggle the * Universal/Local bit. RFC 6282 ch 3.2.2 */ - if (lladdr->type == NET_LINK_IEEE802154) { + if (lladdr->type == NET_LINK_IEEE802154 || + lladdr->type == NET_LINK_CANBUS) { UNALIGNED_PUT(0, &addr->s6_addr32[2]); addr->s6_addr[11] = 0xff; addr->s6_addr[12] = 0xfe; diff --git a/include/net/net_l2.h b/include/net/net_l2.h index af553d7314..ff890e2ec9 100644 --- a/include/net/net_l2.h +++ b/include/net/net_l2.h @@ -120,6 +120,11 @@ NET_L2_DECLARE_PUBLIC(OPENTHREAD_L2); NET_L2_DECLARE_PUBLIC(CANBUS_RAW_L2); #endif /* CONFIG_NET_L2_CANBUS_RAW */ +#ifdef CONFIG_NET_L2_CANBUS +#define CANBUS_L2 CANBUS +NET_L2_DECLARE_PUBLIC(CANBUS_L2); +#endif /* CONFIG_NET_L2_CANBUS */ + #define NET_L2_INIT(_name, _recv_fn, _send_fn, _enable_fn, _get_flags_fn) \ const struct net_l2 (NET_L2_GET_NAME(_name)) __used \ __attribute__((__section__(".net_l2.init"))) = { \ diff --git a/include/net/net_linkaddr.h b/include/net/net_linkaddr.h index b2c71ddc45..053afd9de8 100644 --- a/include/net/net_linkaddr.h +++ b/include/net/net_linkaddr.h @@ -57,6 +57,8 @@ enum net_link_type { NET_LINK_DUMMY, /** CANBUS link address. */ NET_LINK_CANBUS_RAW, + /** 6loCAN link address. */ + NET_LINK_CANBUS, } __packed; /** diff --git a/include/net/net_pkt.h b/include/net/net_pkt.h index 4ddbc352ed..50ec62c4ae 100644 --- a/include/net/net_pkt.h +++ b/include/net/net_pkt.h @@ -41,6 +41,8 @@ extern "C" { */ struct net_context; +struct canbus_net_isotp_tx_ctx; +struct canbus_net_isotp_rx_ctx; /* buffer cursor used in net_pkt */ @@ -208,6 +210,12 @@ struct net_pkt { #if defined(CONFIG_IEEE802154) u8_t ieee802154_rssi; /* Received Signal Strength Indication */ u8_t ieee802154_lqi; /* Link Quality Indicator */ +#endif +#if defined(CONFIG_NET_L2_CANBUS) + union { + struct canbus_isotp_tx_ctx *canbus_tx_ctx; + struct canbus_isotp_rx_ctx *canbus_rx_ctx; + }; #endif /* @endcond */ }; diff --git a/samples/net/sockets/can/prj.conf b/samples/net/sockets/can/prj.conf index eaae713e08..a8cda21de1 100644 --- a/samples/net/sockets/can/prj.conf +++ b/samples/net/sockets/can/prj.conf @@ -12,7 +12,7 @@ CONFIG_NET_MGMT=n CONFIG_NET_TCP=n CONFIG_NET_UDP=n -CONFIG_NET_DEFAULT_IF_CANBUS=y +CONFIG_NET_DEFAULT_IF_CANBUS_RAW=y CONFIG_NET_PKT_RX_COUNT=30 CONFIG_NET_PKT_TX_COUNT=30 diff --git a/samples/net/sockets/can/src/main.c b/samples/net/sockets/can/src/main.c index f3e6ac466e..6749466e16 100644 --- a/samples/net/sockets/can/src/main.c +++ b/samples/net/sockets/can/src/main.c @@ -81,7 +81,7 @@ static int create_socket(const struct can_filter *filter) } can_addr.can_ifindex = net_if_get_by_iface( - net_if_get_first_by_type(&NET_L2_GET_NAME(CANBUS))); + net_if_get_first_by_type(&NET_L2_GET_NAME(CANBUS_RAW))); can_addr.can_family = PF_CAN; ret = bind(fd, (struct sockaddr *)&can_addr, sizeof(can_addr)); @@ -170,7 +170,7 @@ static int setup_socket(void) can_copy_zfilter_to_filter(&zfilter, &filter); - iface = net_if_get_first_by_type(&NET_L2_GET_NAME(CANBUS)); + iface = net_if_get_first_by_type(&NET_L2_GET_NAME(CANBUS_RAW)); if (!iface) { LOG_ERR("No CANBUS network interface found!"); return -ENOENT; diff --git a/subsys/net/ip/Kconfig b/subsys/net/ip/Kconfig index 53983b3a13..751316ebb2 100644 --- a/subsys/net/ip/Kconfig +++ b/subsys/net/ip/Kconfig @@ -547,9 +547,14 @@ config NET_DEFAULT_IF_DUMMY depends on NET_L2_DUMMY config NET_DEFAULT_IF_CANBUS - bool "Socket CAN interface" + bool "6LoCAN (IPv6 over CAN) interface" depends on NET_L2_CANBUS +config NET_DEFAULT_IF_CANBUS_RAW + bool "Socket CAN interface" + depends on NET_L2_CANBUS_RAW + + endchoice config NET_PKT_TIMESTAMP diff --git a/subsys/net/ip/canbus_socket.c b/subsys/net/ip/canbus_socket.c index 8804d21c55..1e20ecc762 100644 --- a/subsys/net/ip/canbus_socket.c +++ b/subsys/net/ip/canbus_socket.c @@ -21,7 +21,7 @@ LOG_MODULE_REGISTER(net_sockets_can, CONFIG_NET_SOCKETS_LOG_LEVEL); enum net_verdict net_canbus_socket_input(struct net_pkt *pkt) { if (net_pkt_family(pkt) == AF_CAN && - net_if_l2(net_pkt_iface(pkt)) == &NET_L2_GET_NAME(CANBUS)) { + net_if_l2(net_pkt_iface(pkt)) == &NET_L2_GET_NAME(CANBUS_RAW)) { return net_conn_input(pkt, NULL, CAN_RAW, NULL); } diff --git a/subsys/net/ip/ipv6_nbr.c b/subsys/net/ip/ipv6_nbr.c index fade5960b1..6088db4c79 100644 --- a/subsys/net/ip/ipv6_nbr.c +++ b/subsys/net/ip/ipv6_nbr.c @@ -976,6 +976,8 @@ static inline u8_t get_llao_len(struct net_if *iface) return 8; } else if (net_if_get_link_addr(iface)->len == 8U) { return 16; + } else if (net_if_get_link_addr(iface)->len == 2U) { + return 8; } /* What else could it be? */ diff --git a/subsys/net/ip/net_context.c b/subsys/net/ip/net_context.c index 33371f224f..0ac8ded048 100644 --- a/subsys/net/ip/net_context.c +++ b/subsys/net/ip/net_context.c @@ -446,11 +446,11 @@ static int bind_default(struct net_context *context) if (context->iface >= 0) { return 0; } else { -#if defined(CONFIG_NET_L2_CANBUS) +#if defined(CONFIG_NET_L2_CANBUS_RAW) struct net_if *iface; iface = net_if_get_first_by_type( - &NET_L2_GET_NAME(CANBUS)); + &NET_L2_GET_NAME(CANBUS_RAW)); if (!iface) { return -ENOENT; } diff --git a/subsys/net/ip/net_if.c b/subsys/net/ip/net_if.c index e3ec26fa81..013c7d2347 100644 --- a/subsys/net/ip/net_if.c +++ b/subsys/net/ip/net_if.c @@ -399,6 +399,9 @@ struct net_if *net_if_get_default(void) #if defined(CONFIG_NET_DEFAULT_IF_OFFLOAD) iface = net_if_get_first_by_type(NULL); #endif +#if defined(CONFIG_NET_DEFAULT_IF_CANBUS_RAW) + iface = net_if_get_first_by_type(&NET_L2_GET_NAME(CANBUS_RAW)); +#endif #if defined(CONFIG_NET_DEFAULT_IF_CANBUS) iface = net_if_get_first_by_type(&NET_L2_GET_NAME(CANBUS)); #endif diff --git a/subsys/net/ip/net_shell.c b/subsys/net/ip/net_shell.c index 9e30d5c346..86926ad67b 100644 --- a/subsys/net/ip/net_shell.c +++ b/subsys/net/ip/net_shell.c @@ -207,6 +207,16 @@ static const char *iface2str(struct net_if *iface, const char **extra) } #endif +#ifdef CONFIG_NET_L2_CANBUS_RAW + if (net_if_l2(iface) == &NET_L2_GET_NAME(CANBUS_RAW)) { + if (extra) { + *extra = "=========="; + } + + return "CANBUS_RAW"; + } +#endif + if (extra) { *extra = "=============="; } diff --git a/subsys/net/ip/tcp.c b/subsys/net/ip/tcp.c index 1338b57028..e6b2d3ad78 100644 --- a/subsys/net/ip/tcp.c +++ b/subsys/net/ip/tcp.c @@ -172,7 +172,9 @@ static inline u32_t retry_timeout(const struct net_tcp *tcp) ((IS_ENABLED(CONFIG_NET_L2_BT) && \ net_pkt_lladdr_dst(pkt)->type == NET_LINK_BLUETOOTH) || \ (IS_ENABLED(CONFIG_NET_L2_IEEE802154) && \ - net_pkt_lladdr_dst(pkt)->type == NET_LINK_IEEE802154))) + net_pkt_lladdr_dst(pkt)->type == NET_LINK_IEEE802154) || \ + (IS_ENABLED(CONFIG_NET_L2_CANBUS) && \ + net_pkt_lladdr_dst(pkt)->type == NET_LINK_CANBUS))) /* The ref should not be done for Bluetooth and IEEE 802.15.4 which use * IPv6 header compression (6lo). For BT and 802.15.4 we copy the pkt diff --git a/subsys/net/ip/utils.c b/subsys/net/ip/utils.c index 15849148dc..295ca34110 100644 --- a/subsys/net/ip/utils.c +++ b/subsys/net/ip/utils.c @@ -95,6 +95,9 @@ char *net_sprint_ll_addr_buf(const u8_t *ll, u8_t ll_len, case 6: len = 6U; break; + case 2: + len = 2U; + break; default: len = 6U; break; diff --git a/subsys/net/l2/CMakeLists.txt b/subsys/net/l2/CMakeLists.txt index 2170441cf4..c54e906def 100644 --- a/subsys/net/l2/CMakeLists.txt +++ b/subsys/net/l2/CMakeLists.txt @@ -28,6 +28,6 @@ if(CONFIG_NET_L2_WIFI_MGMT OR CONFIG_NET_L2_WIFI_SHELL) add_subdirectory(wifi) endif() -if(CONFIG_NET_L2_CANBUS) +if(CONFIG_NET_L2_CANBUS OR CONFIG_NET_L2_CANBUS_RAW) add_subdirectory(canbus) endif() diff --git a/subsys/net/l2/Kconfig b/subsys/net/l2/Kconfig index 1cc1a56620..9b1d387772 100644 --- a/subsys/net/l2/Kconfig +++ b/subsys/net/l2/Kconfig @@ -77,6 +77,8 @@ source "subsys/net/l2/ieee802154/Kconfig" source "subsys/net/l2/openthread/Kconfig" +source "subsys/net/l2/canbus/Kconfig" + config NET_L2_WIFI_MGMT bool "Enable Wi-Fi Management support" select NET_MGMT @@ -100,9 +102,4 @@ config NET_L2_WIFI_SHELL This can be used for controlling Wi-Fi through the console via exposing a shell module named "wifi". -config NET_L2_CANBUS - bool "Enable CANBUS l2 layer" - help - Add a CANBUS L2 layer driver. - endmenu diff --git a/subsys/net/l2/canbus/6locan.c b/subsys/net/l2/canbus/6locan.c new file mode 100644 index 0000000000..8727c98b00 --- /dev/null +++ b/subsys/net/l2/canbus/6locan.c @@ -0,0 +1,1344 @@ +/* + * Copyright (c) 2019 Alexander Wachter. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +LOG_MODULE_REGISTER(net_l2_canbus, CONFIG_NET_L2_CANBUS_LOG_LEVEL); + +#include +#include +#include +#include +#include +#include "canbus_internal.h" +#include <6lo.h> +#include +#include +#include +#include +#include +#include + +#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 z_ms_to_ticks(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 canbus_get_lladdr(net_pkt_lladdr_src(pkt)); +} + +static u16_t canbus_get_dest_lladdr(struct net_pkt *pkt) +{ + return canbus_get_lladdr(net_pkt_lladdr_dst(pkt)); +} + +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 size_t canbus_total_lladdr_len(struct net_pkt *pkt) +{ + ARG_UNUSED(pkt); + + 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; + + lladdr = net_pkt_lladdr_src(dst); + lladdr->addr = net_pkt_cursor_get_pos(dst); + + + 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, canbus_rx_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, canbus_tx_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); + + 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; + + 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, + z_ms_to_ticks(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, + z_ms_to_ticks(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, + z_ms_to_ticks(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, + z_ms_to_ticks(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, + z_ms_to_ticks(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 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); + } + + 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; + + 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; + 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_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, + z_ms_to_ticks(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, + z_ms_to_ticks(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; + 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 { + 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)" : ""); + + if ((pkt_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; +} + +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); + 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); diff --git a/subsys/net/l2/canbus/CMakeLists.txt b/subsys/net/l2/canbus/CMakeLists.txt index b718b07607..d477362742 100644 --- a/subsys/net/l2/canbus/CMakeLists.txt +++ b/subsys/net/l2/canbus/CMakeLists.txt @@ -1,8 +1,10 @@ # SPDX-License-Identifier: Apache-2.0 zephyr_library() +zephyr_library_include_directories(. ${ZEPHYR_BASE}/subsys/net/ip) zephyr_library_compile_definitions_ifdef( CONFIG_NEWLIB_LIBC __LINUX_ERRNO_EXTENSIONS__ ) -zephyr_library_sources_ifdef(CONFIG_NET_L2_CANBUS canbus.c) +zephyr_library_sources_ifdef(CONFIG_NET_L2_CANBUS_RAW canbus_raw.c) +zephyr_library_sources_ifdef(CONFIG_NET_L2_CANBUS 6locan.c) diff --git a/subsys/net/l2/canbus/Kconfig b/subsys/net/l2/canbus/Kconfig new file mode 100644 index 0000000000..c47f9a26c0 --- /dev/null +++ b/subsys/net/l2/canbus/Kconfig @@ -0,0 +1,68 @@ +# +# Copyright (c) 2019 Alexander Wachter +# +# SPDX-License-Identifier: Apache-2.0 +# + +config NET_L2_CANBUS + bool "Enable CANBUS L2 layer [EXPERIMENTAL]" + depends on CAN_NET + select NET_6LO + help + Add a CANBUS L2 layer driver. This is the layer for IPv6 over CAN + (6loCAN). It uses IPHC to compress the IP header and ISO-TP for + flow control and reassembling. + +if NET_L2_CANBUS + +config NET_L2_CANBUS_USE_FIXED_ADDR + bool "Use fixed L2 address" + help + Use a fixed L2 address for 6LoCAN instead of a random chosen one. + +config NET_L2_CANBUS_FIXED_ADDR + hex "L2 address" + depends on NET_L2_CANBUS_USE_FIXED_ADDR + range 0x00FF 0x3DEF + +config NET_L2_CANBUS_DAD_RETRIES + int "Number of DAD retries" + default 5 + help + Number of retries for Duplicate Address Detection. + Greater than one only makes sense for random link layer addresses. + +config NET_L2_CANBUS_STMIN + int "STmin" + default 0 + range 0 127 + help + Minimal separation time between frames in ms. + The timer starts when the frame is queued and the next frame is + transmitted after expiration. + STmin is chosen by the receiver and transmitted in the FC + (Flow Control) frame. See also: ISO 15765-2:2016 + +config NET_L2_CANBUS_BS + int "BS (Block Size)" + default 8 + range 0 256 + help + Number of CF (Contiguous Frame) PDUs before next FC (Flow Control) + frame is sent. Zero value means all frames are sent consecutive + without an additional FC frame. + A BS counter at the sender counts from one to BS. When BS is reached, + the sender waits for a FC frame again an BS is reset. + See also: ISO 15765-2:2016 + +module = NET_L2_CANBUS +module-dep = NET_LOG +module-str = Log level for CANbus L2 layer +module-help = Enables CANbus L2 to output debug messages. +source "subsys/net/Kconfig.template.log_config.net" +endif #NET_L2_CANBUS + +config NET_L2_CANBUS_RAW + bool "Enable CANBUS RAW l2 layer" + help + Add a CANBUS L2 layer driver. This is the layer for SOCKET CAN. diff --git a/subsys/net/l2/canbus/canbus.c b/subsys/net/l2/canbus/canbus.c deleted file mode 100644 index e1ab61dd08..0000000000 --- a/subsys/net/l2/canbus/canbus.c +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (c) 2019 Intel Corporation. - * - * SPDX-License-Identifier: Apache-2.0 - */ - -#include -LOG_MODULE_REGISTER(net_l2_canbus, LOG_LEVEL_NONE); - -#include -#include -#include -#include -#include - -static inline enum net_verdict canbus_recv(struct net_if *iface, - struct net_pkt *pkt) -{ - net_pkt_lladdr_src(pkt)->addr = NULL; - net_pkt_lladdr_src(pkt)->len = 0U; - net_pkt_lladdr_src(pkt)->type = NET_LINK_CANBUS; - net_pkt_lladdr_dst(pkt)->addr = NULL; - net_pkt_lladdr_dst(pkt)->len = 0U; - net_pkt_lladdr_dst(pkt)->type = NET_LINK_CANBUS; - - net_pkt_set_family(pkt, AF_CAN); - - return NET_CONTINUE; -} - -static inline int canbus_send(struct net_if *iface, struct net_pkt *pkt) -{ - const struct canbus_api *api = net_if_get_device(iface)->driver_api; - int ret; - - if (!api) { - return -ENOENT; - } - - ret = api->send(net_if_get_device(iface), pkt); - if (!ret) { - ret = net_pkt_get_len(pkt); - net_pkt_unref(pkt); - } - - return ret; -} - -NET_L2_INIT(CANBUS_L2, canbus_recv, canbus_send, NULL, NULL); diff --git a/subsys/net/l2/canbus/canbus_internal.h b/subsys/net/l2/canbus/canbus_internal.h new file mode 100644 index 0000000000..76064e07bc --- /dev/null +++ b/subsys/net/l2/canbus/canbus_internal.h @@ -0,0 +1,98 @@ +/* + * Copyright (c) 2019 Alexander Wachter + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_SUBSYS_NET_L2_CANBUS_INTERNAL_H_ +#define ZEPHYR_SUBSYS_NET_L2_CANBUS_INTERNAL_H_ + + +#ifdef NET_CAN_USE_CAN_FD +#define NET_CAN_DL 64 +#else +#define NET_CAN_DL 8 +#endif/*NET_CAN_USE_CAN_FD*/ + +/* Protocol control information*/ +#define NET_CAN_PCI_SF 0x00 /* Single frame*/ +#define NET_CAN_PCI_FF 0x01 /* First frame */ +#define NET_CAN_PCI_CF 0x02 /* Consecutive frame */ +#define NET_CAN_PCI_FC 0x03 /* Flow control frame */ + +#define NET_CAN_PCI_TYPE_BYTE 0 +#define NET_CAN_PCI_TYPE_POS 4 +#define NET_CAN_PCI_TYPE_MASK 0xF0 +#define NET_CAN_PCI_TYPE_SF (NET_CAN_PCI_SF << NET_CAN_PCI_TYPE_POS) +#define NET_CAN_PCI_TYPE_FF (NET_CAN_PCI_FF << NET_CAN_PCI_TYPE_POS) +#define NET_CAN_PCI_TYPE_CF (NET_CAN_PCI_CF << NET_CAN_PCI_TYPE_POS) +#define NET_CAN_PCI_TYPE_FC (NET_CAN_PCI_FC << NET_CAN_PCI_TYPE_POS) + +#define NET_CAN_PCI_SF_DL_MASK 0x0F + +#define NET_CAN_PCI_FF_DL_UPPER_BYTE 0 +#define NET_CAN_PCI_FF_DL_UPPER_MASK 0x0F +#define NET_CAN_PCI_FF_DL_LOWER_BYTE 1 + +#define NET_CAN_PCI_FS_BYTE 0 +#define NET_CAN_PCI_FS_MASK 0x0F +#define NET_CAN_PCI_BS_BYTE 1 +#define NET_CAN_PCI_ST_MIN_BYTE 2 + +#define NET_CAN_PCI_FS_CTS 0x0 +#define NET_CAN_PCI_FS_WAIT 0x1 +#define NET_CAN_PCI_FS_OVFLW 0x2 + +#define NET_CAN_PCI_SN_MASK 0x0F + +#define NET_CAN_FF_DL_MIN (NET_CAN_CAN_DL) + +#define NET_CAN_WFT_FIRST 0xFF + +#define NET_CAN_BS_TIME K_MSEC(1000) +#define NET_CAN_A_TIME K_MSEC(1000) + +#define NET_CAN_FF_CF_TIME K_MSEC(1) + +#define NET_CAN_STMIN_MAX 0xFA +#define NET_CAN_STMIN_MS_MAX 0x7F +#define NET_CAN_STMIN_US_BEGIN 0xF1 +#define NET_CAN_STMIN_US_END 0xF9 + +#ifdef __cplusplus +extern "C" { +#endif + +enum net_can_isotp_tx_state { + NET_CAN_TX_STATE_UNUSED, + NET_CAN_TX_STATE_RESET, + NET_CAN_TX_STATE_WAIT_FC, + NET_CAN_TX_STATE_SEND_CF, + NET_CAN_TX_STATE_WAIT_ST, + NET_CAN_TX_STATE_WAIT_TX_BACKLOG, + NET_CAN_TX_STATE_FIN, + NET_CAN_TX_STATE_ERR +}; + +enum net_can_isotp_rx_state { + NET_CAN_RX_STATE_UNUSED, + NET_CAN_RX_STATE_RESET, + NET_CAN_RX_STATE_FF, + NET_CAN_RX_STATE_CF, + NET_CAN_RX_STATE_FIN, + NET_CAN_RX_STATE_TIMEOUT +}; + +struct canbus_l2_ctx { + struct canbus_isotp_tx_ctx tx_ctx[CONFIG_NET_PKT_TX_COUNT]; + struct canbus_isotp_rx_ctx rx_ctx[CONFIG_NET_PKT_RX_COUNT]; + struct k_mutex tx_ctx_mtx; + struct k_mutex rx_ctx_mtx; + struct k_sem tx_sem; +}; + +#ifdef __cplusplus +} +#endif + +#endif /* ZEPHYR_SUBSYS_NET_L2_CANBUS_INTERNAL_H_ */ diff --git a/tests/net/all/prj.conf b/tests/net/all/prj.conf index 5c3a851eeb..9760f30233 100644 --- a/tests/net/all/prj.conf +++ b/tests/net/all/prj.conf @@ -95,6 +95,7 @@ CONFIG_NET_L2_IEEE802154_SECURITY=y CONFIG_NET_L2_IEEE802154_SECURITY_CRYPTO_DEV_NAME="CRYPTO-DEV" CONFIG_NET_L2_DUMMY=y CONFIG_NET_L2_ETHERNET=y +CONFIG_NET_L2_CANBUS=y CONFIG_NET_L2_CANBUS_RAW=y CONFIG_NET_L2_ETHERNET_MGMT=y CONFIG_NET_L2_IEEE802154_RADIO_DFLT_TX_POWER=2 @@ -368,6 +369,7 @@ CONFIG_NET_SOCKETS_TLS_MAX_CREDENTIALS=10 # Network interface defaults CONFIG_NET_DEFAULT_IF_BLUETOOTH=y +CONFIG_NET_DEFAULT_IF_CANBUS=y CONFIG_NET_DEFAULT_IF_CANBUS_RAW=y CONFIG_NET_DEFAULT_IF_DUMMY=y CONFIG_NET_DEFAULT_IF_ETHERNET=y