diff --git a/include/canbus/isotp.h b/include/canbus/isotp.h new file mode 100644 index 00000000000..4721ba9f40c --- /dev/null +++ b/include/canbus/isotp.h @@ -0,0 +1,393 @@ +/* + * Copyright (c) 2019 Alexander Wachter + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file + * @brief Public API for ISO-TP (ISO 15765-2:2016) + * + * ISO-TP is a transport protocol for CAN (Controller Area Network) + */ + +#ifndef ZEPHYR_INCLUDE_ISOTP_H_ +#define ZEPHYR_INCLUDE_ISOTP_H_ + +/** + * @brief CAN ISO-TP Interf + * @defgroup can_isotp CAN ISO-TP Interface + * @ingroup CAN + * @{ + */ + +#include +#include +#include + +/* + * 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 + * AE Adders Extension + */ + +/* + * N_Result according to ISO 15765-2:2016 + * ISOTP_ prefix is used to be zephyr conform + */ + +/** Completed successfully */ +#define ISOTP_N_OK 0 + +/** Ar/As has timed out */ +#define ISOTP_N_TIMEOUT_A -1 + +/** Reception of next FC has timed out */ +#define ISOTP_N_TIMEOUT_BS -2 + +/** Cr has timed out */ +#define ISOTP_N_TIMEOUT_CR -3 + +/** Unexpected sequence number */ +#define ISOTP_N_WRONG_SN -4 + +/** Invalid flow status received*/ +#define ISOTP_N_INVALID_FS -5 + +/** Unexpected PDU received */ +#define ISOTP_N_UNEXP_PDU -6 + +/** Maximum number of WAIT flowStatus PDUs exceeded */ +#define ISOTP_N_WFT_OVRN -7 + +/** FlowStatus OVFLW PDU was received */ +#define ISOTP_N_BUFFER_OVERFLW -8 + +/** General error */ +#define ISOTP_N_ERROR -9 + +/** Implementation specific errors */ + +/** Can't bind or send because the CAN device has no filter left*/ +#define ISOTP_NO_FREE_FILTER -10 + +/** No net buffer left to allocate */ +#define ISOTP_NO_NET_BUF_LEFT -11 + +/** Not sufficient space in the buffer left for the data */ +#define ISOTP_NO_BUF_DATA_LEFT -12 + +/** No context buffer left to allocate */ +#define ISOTP_NO_CTX_LEFT -13 + +/** Timeout for recv */ +#define ISOTP_RECV_TIMEOUT -14 + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief ISO-TP message id struct + * + * Used to pass addresses to the bind and send functions. + */ +struct isotp_msg_id { + /** Message identifier*/ + union { + u32_t std_id : 11; + u32_t ext_id : 29; + }; + /** extended address */ + u8_t ext_addr; + /** Indicates the identifier type (standard or extended) */ + u8_t id_type : 1; + /** Indicates if extended addressing is used */ + u8_t use_ext_addr : 1; +}; + +/* + * STmin is split in two valid ranges: + * 0-127: 0ms-127ms + * 128-240: Reserved + * 241-249: 100us-900us (multiples of 100us) + * 250- : Reserved + */ + +/** + * @brief ISO-TP frame control options struct + * + * Used to pass the options to the bind and send functions. + */ +struct isotp_fc_opts { + u8_t bs; /**< Block size. Number of CF PDUs before next CF is sent */ + u8_t stmin; /**< Minimum separation time. Min time between frames */ +}; + +typedef void (*isotp_tx_callback_t)(int error_nr, void *arg); + +struct isotp_send_ctx; +struct isotp_recv_ctx; + +/** + * @brief Bind an address to a receiving context. + * + * This function binds an RX and TX address combination to an RX context. + * When data arrives from the specified address, it is buffered and can be read + * by calling isotp_recv. + * When calling this routine, a filter is applied in the CAN device, and the + * context is initialized. The context must be valid until calling unbind. + * + * @param ctx Context to store the internal states. + * @param can_dev The CAN device to be used for sending and receiving. + * @param rx_addr Identifier for incoming data. + * @param tx_addr Identifier for FC frames. + * @param opts Flow control options. + * @param timeout Timeout for FF SF buffer allocation. + * + * @retval ISOTP_N_OK on success + * @retval ISOTP_NO_FREE_FILTER if CAN device has no filters left. + */ +int isotp_bind(struct isotp_recv_ctx *ctx, struct device *can_dev, + const struct isotp_msg_id *rx_addr, + const struct isotp_msg_id *tx_addr, + const struct isotp_fc_opts *opts, + s32_t timeout); + +/** + * @brief Unbind a context from the interface + * + * This function removes the binding from isotp_bind. + * The filter is detached from the CAN device, and if a transmission is ongoing, + * buffers are freed. + * The context can be discarded safely after calling this function. + * + * @param ctx Context that should be unbound. + */ +void isotp_unbind(struct isotp_recv_ctx *ctx); + +/** + * @brief Read out received data from fifo. + * + * This function reads the data from the receive FIFO of the context. + * It blocks if the FIFO is empty. + * If an error occurs, the function returns a negative number and leaves the + * data buffer unchanged. + * + * @param ctx Context that is already bound. + * @param data Pointer to a buffer where the data is copied to. + * @param len Size of the buffer. + * @param timeout Timeout for incoming data. + * + * @retval Number of bytes copied on success + * @retval ISOTP_WAIT_TIMEOUT when "timeout" timed out + * @retval ISOTP_N_* on error + */ +int isotp_recv(struct isotp_recv_ctx *ctx, u8_t *data, size_t len, + s32_t timeout); + +/** + * @brief Get the net buffer on data reception + * + * This function reads incoming data into net-buffers. + * It blocks until the entire packet is received, BS is reached, or an error + * occurred. If BS was zero, the data is in a single net_buf. Otherwise, + * the data is fragmented in chunks of BS size. + * The net-buffers are referenced and must be freed with net_buf_unref after the + * data is processed. + * + * @param ctx Context that is already bound. + * @param buffer Pointer where the net_buf pointer is written to. + * @param timeout Timeout for incoming data. + * + * @retval Remaining data length for this transfer if BS > 0, 0 for BS = 0 + * @retval ISOTP_WAIT_TIMEOUT when "timeout" timed out + * @retval ISOTP_N_* on error + */ +int isotp_recv_net(struct isotp_recv_ctx *ctx, struct net_buf **buffer, + s32_t timeout); + +/** + * @brief Send data + * + * This function is used to send data to a peer that listens to the tx_addr. + * An internal work-queue is used to transfer the segmented data. + * Data and context must be valid until the transmission has finished. + * If a complete_cb is given, this function is non-blocking, and the callback + * is called on completion with the return value as a parameter. + * + * @param ctx Context to store the internal states. + * @param can_dev The CAN device to be used for sending and receiving. + * @param data Data to be sent. + * @param len Length of the data to be sent. + * @param rx_addr Identifier for FC frames. + * @param tx_addr Identifier for outgoing frames the receiver listens on. + * @param complete_cb Function called on completion or NULL. + * @param cb_arg Argument passed to the complete callback. + * + * @retval ISOTP_N_OK on success + * @retval ISOTP_N_* on error + */ +int isotp_send(struct isotp_send_ctx *ctx, struct device *can_dev, + const u8_t *data, size_t len, + const struct isotp_msg_id *tx_addr, + const struct isotp_msg_id *rx_addr, + isotp_tx_callback_t complete_cb, void *cb_arg); + +#ifdef CONFIG_ISOTP_ENABLE_CONTEXT_BUFFERS +/** + * @brief Send data with buffered context + * + * This function is similar to isotp_send, but the context is automatically + * allocated from an internal pool. + * + * @param can_dev The CAN device to be used for sending and receiving. + * @param data Data to be sent. + * @param len Length of the data to be sent. + * @param rx_addr Identifier for FC frames. + * @param tx_addr Identifier for outgoing frames the receiver listens on. + * @param complete_cb Function called on completion or NULL. + * @param cb_arg Argument passed to the complete callback. + * @param timeout Timeout for buffer allocation. + * + * @retval ISOTP_N_OK on success + * @retval ISOTP_N_* on error + */ +int isotp_send_ctx_buf(struct device *can_dev, + const u8_t *data, size_t len, + const struct isotp_msg_id *tx_addr, + const struct isotp_msg_id *rx_addr, + isotp_tx_callback_t complete_cb, void *cb_arg, + s32_t timeout); + +/** + * @brief Send data with buffered context + * + * This function is similar to isotp_send_ctx_buf, but the data is carried in + * a net_buf. net_buf_unref is called on the net_buf when sending is completed. + * + * @param can_dev The CAN device to be used for sending and receiving. + * @param data Data to be sent. + * @param len Length of the data to be sent. + * @param rx_addr Identifier for FC frames. + * @param tx_addr Identifier for outgoing frames the receiver listens on. + * @param complete_cb Function called on completion or NULL. + * @param cb_arg Argument passed to the complete callback. + * @param timeout Timeout for buffer allocation. + * + * @retval ISOTP_N_OK on success + * @retval ISOTP_* on error + */ +int isotp_send_net_ctx_buf(struct device *can_dev, + struct net_buf *data, + const struct isotp_msg_id *tx_addr, + const struct isotp_msg_id *rx_addr, + isotp_tx_callback_t complete_cb, void *cb_arg, + s32_t timeout); + +#endif /*CONFIG_ISOTP_ENABLE_CONTEXT_BUFFERS*/ + +#if defined(CONFIG_ISOTP_USE_TX_BUF) && \ + defined(CONFIG_ISOTP_ENABLE_CONTEXT_BUFFERS) +/** + * @brief Send data with buffered context + * + * This function is similar to isotp_send, but the context is automatically + * allocated from an internal pool and the data to be send is buffered in an + * internal net_buff. + * + * @param can_dev The CAN device to be used for sending and receiving. + * @param data Data to be sent. + * @param len Length of the data to be sent. + * @param rx_addr Identifier for FC frames. + * @param tx_addr Identifier for outgoing frames the receiver listens on. + * @param complete_cb Function called on completion or NULL. + * @param cb_arg Argument passed to the complete callback. + * @param timeout Timeout for buffer allocation. + * + * @retval ISOTP_N_OK on success + * @retval ISOTP_* on error + */ +int isotp_send_buf(struct device *can_dev, + const u8_t *data, size_t len, + const struct isotp_msg_id *tx_addr, + const struct isotp_msg_id *rx_addr, + isotp_tx_callback_t complete_cb, void *cb_arg, + s32_t timeout); +#endif + +/** @cond INTERNAL_HIDDEN */ + +struct isotp_callback { + isotp_tx_callback_t cb; + void *arg; +}; + +struct isotp_send_ctx { + int filter_id; + u32_t error_nr; + struct device *can_dev; + union { + struct net_buf *buf; + struct { + const u8_t *data; + size_t len; + }; + }; + struct k_work work; + struct _timeout timeout; + union { + struct isotp_callback fin_cb; + struct k_sem fin_sem; + }; + struct isotp_fc_opts opts; + u8_t state; + u8_t tx_backlog; + struct isotp_msg_id rx_addr; + struct isotp_msg_id tx_addr; + u8_t wft; + u8_t bs; + u8_t sn : 4; + u8_t is_net_buf : 1; + u8_t is_ctx_slab : 1; + u8_t has_callback: 1; +}; + +struct isotp_recv_ctx { + int filter_id; + struct device *can_dev; + struct net_buf *buf; + struct net_buf *act_frag; + sys_snode_t alloc_node; + u32_t length; + int error_nr; + struct k_work work; + struct _timeout timeout; + struct k_fifo fifo; + struct isotp_msg_id rx_addr; + struct isotp_msg_id tx_addr; + struct isotp_fc_opts opts; + u8_t state; + u8_t bs; + u8_t wft; + u8_t sn_expected : 4; +}; + +/** @endcond */ + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /* ZEPHYR_INCLUDE_ISOTP_H_ */ diff --git a/subsys/canbus/CMakeLists.txt b/subsys/canbus/CMakeLists.txt index 06064a472f4..bf827ab546a 100644 --- a/subsys/canbus/CMakeLists.txt +++ b/subsys/canbus/CMakeLists.txt @@ -1,3 +1,4 @@ # SPDX-License-Identifier: Apache-2.0 add_subdirectory_if_kconfig(canopen) +add_subdirectory_if_kconfig(isotp) diff --git a/subsys/canbus/Kconfig b/subsys/canbus/Kconfig index 3410b3fecbc..dc68b5a77fe 100644 --- a/subsys/canbus/Kconfig +++ b/subsys/canbus/Kconfig @@ -6,5 +6,6 @@ menu "Controller Area Network (CAN) bus subsystem" source "subsys/canbus/canopen/Kconfig" +source "subsys/canbus/isotp/Kconfig" endmenu diff --git a/subsys/canbus/isotp/CMakeLists.txt b/subsys/canbus/isotp/CMakeLists.txt new file mode 100644 index 00000000000..4f6e0266ceb --- /dev/null +++ b/subsys/canbus/isotp/CMakeLists.txt @@ -0,0 +1,4 @@ +# SPDX-License-Identifier: Apache-2.0 + +zephyr_library() +zephyr_library_sources_if_kconfig(isotp.c) diff --git a/subsys/canbus/isotp/Kconfig b/subsys/canbus/isotp/Kconfig new file mode 100644 index 00000000000..31482916a4f --- /dev/null +++ b/subsys/canbus/isotp/Kconfig @@ -0,0 +1,133 @@ +# ISO-TP configuration options + +# Copyright (c) 2019 Alexander Wachter +# SPDX-License-Identifier: Apache-2.0 + +menuconfig ISOTP + bool "ISO-TP Transport [EXPERIMENTAL]" + select NET_BUF + select POLL + help + Enable ISO TP support for CAN + +if ISOTP + +module = ISOTP +module-str = ISOTP +source "subsys/logging/Kconfig.template.log_config" + +config ISOTP_WFTMAX + int "WFTmax (Max WAIT frames before aborting)." + default 10 + range 0 254 + help + This value defines the maximum number of WAIT frames before the transmission + is aborted. + +config ISOTP_BS_TIMEOUT + int "Bs timeout [ms] (timeout for receiving the frame control)" + default 1000 + range 200 10000 + help + Timeout for the reception of the next FC frame. ISO 15765-2: 1000ms + +config ISOTP_A_TIMEOUT + int "Ar and As timeout [ms] (sending and receiving timeout)" + default 1000 + range 200 10000 + help + As (sender transmit timeout) and Ar (receiver transmit timeout). + ISO 15765-2: 1000ms + +config ISOTP_CR_TIMEOUT + int "Cr timeout [ms] (timeout for consecutive frames)" + default 1000 + range 200 10000 + help + Cr (receiver consecutive frame) timeout. + ISO 15765-2: 1000ms + +config ISOTP_WORKQUEUE_PRIO + int "Priority level of the RX and TX work queue" + default 2 + help + This value defines the priority level of the work queue thread that + handles flow control, consecutive sending, receiving and callbacks. + +config ISOTP_WORKQ_STACK_SIZE + int "Work queue stack size" + default 256 + help + This value defines the stack size of the work queue thread that + handles flow control, consecutive sending, receiving and callbacks. + +config ISOTP_RX_BUF_COUNT + int "Number of data buffers for receiving data" + default 4 + help + Each data buffer will occupy ISOTP_RX_BUF_SIZE + smallish + header (sizeof(struct net_buf)) amount of data. + +config ISOTP_RX_BUF_SIZE + int "Size of one buffer data block" + default 56 + help + This value defines the size of a single block in the pool. The number of + blocks is given by ISOTP_RX_BUF_COUNT. To be efficient use a multiple of + CAN_DL - 1 (for classic can : 8 - 1 = 7). + +config ISOTP_RX_SF_FF_BUF_COUNT + int "Number of SF and FF data buffers for receiving data" + default 4 + help + This buffer is used for first and single frames. It is extra because the + buffer has to be ready for the first reception in isr context and therefor + is allocated when binding. + Each buffer will occupy CAN_DL - 1 byte + header (sizeof(struct net_buf)) + amount of data. + +config ISOTP_USE_TX_BUF + bool "Buffer tx writes" + default n + help + Copy the outgoing data to a net buffer so that the calling function + can discard the data. + +if ISOTP_USE_TX_BUF + +config ISOTP_TX_BUF_COUNT + int "Number of data buffers for sending data" + default 4 + help + Each data buffer will occupy CONFIG_NET_BUF_DATA_SIZE + smallish + header (sizeof(struct net_buf)) amount of data. If context buffers + are used, use the same size here. + +config ISOTP_BUF_TX_DATA_POOL_SIZE + int "Size of the memory pool where buffers are allocated from" + default 256 + help + This value defines the size of the memory pool where the buffers + for sending are allocated from. + +endif # ISOTP_USE_TX_BUF + +config ISOTP_ENABLE_CONTEXT_BUFFERS + bool "Enable buffered tx contexts" + default y + help + This option enables buffered sending contexts. This makes send and + forget possible. A memory slab is used to buffer the context. + +if ISOTP_ENABLE_CONTEXT_BUFFERS + +config ISOTP_TX_CONTEXT_BUF_COUNT + int "Amount of context buffers for sending data" + default 4 + help + This defines the size of the memory slab where the buffers are + allocated from. + +endif # ISOTP_ENABLE_CONTEXT_BUFFERS + +endif # ISOTP diff --git a/subsys/canbus/isotp/isotp.c b/subsys/canbus/isotp/isotp.c new file mode 100644 index 00000000000..2a1c25c3cba --- /dev/null +++ b/subsys/canbus/isotp/isotp.c @@ -0,0 +1,1268 @@ +/* + * Copyright (c) 2019 Alexander Wachter + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "isotp_internal.h" +#include +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(isotp, CONFIG_ISOTP_LOG_LEVEL); + +#ifdef CONFIG_ISOTP_ENABLE_CONTEXT_BUFFERS +K_MEM_SLAB_DEFINE(ctx_slab, sizeof(struct isotp_send_ctx), + CONFIG_ISOTP_TX_CONTEXT_BUF_COUNT, 4); +#endif + +static void receive_pool_free(struct net_buf *buf); +static void receive_ff_sf_pool_free(struct net_buf *buf); + +NET_BUF_POOL_DEFINE(isotp_rx_pool, CONFIG_ISOTP_RX_BUF_COUNT, + CONFIG_ISOTP_RX_BUF_SIZE, sizeof(u32_t), + receive_pool_free); + +NET_BUF_POOL_DEFINE(isotp_rx_sf_ff_pool, CONFIG_ISOTP_RX_SF_FF_BUF_COUNT, + ISOTP_CAN_DL, sizeof(u32_t), receive_ff_sf_pool_free); + +static struct isotp_global_ctx global_ctx = { + .alloc_list = SYS_SLIST_STATIC_INIT(&global_ctx.alloc_list), + .ff_sf_alloc_list = SYS_SLIST_STATIC_INIT(&global_ctx.ff_sf_alloc_list) +}; + +#ifdef CONFIG_ISOTP_USE_TX_BUF +NET_BUF_POOL_VAR_DEFINE(isotp_tx_pool, CONFIG_ISOTP_TX_BUF_COUNT, + CONFIG_ISOTP_BUF_TX_DATA_POOL_SIZE, NULL); +#endif + +K_THREAD_STACK_DEFINE(tx_stack, CONFIG_ISOTP_WORKQ_STACK_SIZE); +static struct k_work_q isotp_workq; + +static void receive_state_machine(struct isotp_recv_ctx *ctx); + +/* + * Wake every context that is waiting for a buffer + */ +static void receive_pool_free(struct net_buf *buf) +{ + struct isotp_recv_ctx *ctx; + sys_snode_t *ctx_node; + + net_buf_destroy(buf); + + SYS_SLIST_FOR_EACH_NODE(&global_ctx.alloc_list, ctx_node) { + ctx = CONTAINER_OF(ctx_node, struct isotp_recv_ctx, alloc_node); + k_work_submit(&ctx->work); + } +} + +static void receive_ff_sf_pool_free(struct net_buf *buf) +{ + struct isotp_recv_ctx *ctx; + sys_snode_t *ctx_node; + + net_buf_destroy(buf); + + SYS_SLIST_FOR_EACH_NODE(&global_ctx.ff_sf_alloc_list, ctx_node) { + ctx = CONTAINER_OF(ctx_node, struct isotp_recv_ctx, alloc_node); + k_work_submit(&ctx->work); + } +} + +static inline int _k_fifo_wait_non_empty(struct k_fifo *fifo, int32_t timeout) +{ + struct k_poll_event events[] = { + K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE, + K_POLL_MODE_NOTIFY_ONLY, fifo), + }; + + return k_poll(events, ARRAY_SIZE(events), timeout); +} + +static inline void receive_report_error(struct isotp_recv_ctx *ctx, int err) +{ + ctx->state = ISOTP_RX_STATE_ERR; + ctx->error_nr = err; +} + +void receive_can_tx_isr(u32_t err_flags, void *arg) +{ + struct isotp_recv_ctx *ctx = (struct isotp_recv_ctx *)arg; + + if (err_flags) { + LOG_ERR("Error sending FC frame (%d)", err_flags); + receive_report_error(ctx, ISOTP_N_ERROR); + k_work_submit(&ctx->work); + } +} + +static inline u32_t receive_get_ff_length(struct net_buf *buf) +{ + u32_t len; + u8_t pci = net_buf_pull_u8(buf); + + len = ((pci & ISOTP_PCI_FF_DL_UPPER_MASK) << 8) | net_buf_pull_u8(buf); + + /* Jumbo packet (32 bit length)*/ + if (!len) { + len = net_buf_pull_be32(buf); + } + + return len; +} + +static inline u32_t receive_get_sf_length(struct net_buf *buf) +{ + u8_t len = net_buf_pull_u8(buf) & ISOTP_PCI_SF_DL_MASK; + + /* Single frames > 16 bytes (CAN-FD only) */ + if (IS_ENABLED(ISOTP_USE_CAN_FD) && !len) { + len = net_buf_pull_u8(buf); + } + + return len; +} + +static void receive_send_fc(struct isotp_recv_ctx *ctx, u8_t fs) +{ + struct zcan_frame frame = { + .id_type = ctx->tx_addr.id_type, + .rtr = CAN_DATAFRAME, + .ext_id = ctx->tx_addr.ext_id + }; + u8_t *data = frame.data; + int ret; + + __ASSERT_NO_MSG(!(fs & ISOTP_PCI_TYPE_MASK)); + + if (ctx->tx_addr.use_ext_addr) { + *data++ = ctx->tx_addr.ext_addr; + } + + *data++ = ISOTP_PCI_TYPE_FC | fs; + *data++ = ctx->opts.bs; + *data++ = ctx->opts.stmin; + frame.dlc = data - frame.data; + + ret = can_send(ctx->can_dev, &frame, ISOTP_A, receive_can_tx_isr, ctx); + if (ret) { + LOG_ERR("Can't send FC, (%d)", ret); + receive_report_error(ctx, ISOTP_N_TIMEOUT_A); + receive_state_machine(ctx); + } +} + +static inline struct net_buf *receive_alloc_buffer_chain(u32_t len) +{ + struct net_buf *buf, *frag, *last; + u32_t remaining_len; + + LOG_DBG("Allocate %d bytes ", len); + buf = net_buf_alloc_fixed(&isotp_rx_pool, K_NO_WAIT); + if (!buf) { + return NULL; + } + + if (len <= CONFIG_ISOTP_RX_BUF_SIZE) { + return buf; + } + + remaining_len = len - CONFIG_ISOTP_RX_BUF_SIZE; + last = buf; + while (remaining_len) { + frag = net_buf_alloc_fixed(&isotp_rx_pool, K_NO_WAIT); + if (!frag) { + net_buf_unref(buf); + return NULL; + } + + net_buf_frag_insert(last, frag); + last = frag; + remaining_len = remaining_len > CONFIG_ISOTP_RX_BUF_SIZE ? + remaining_len - CONFIG_ISOTP_RX_BUF_SIZE : 0; + } + + return buf; +} + +static void receive_timeout_handler(struct _timeout *to) +{ + struct isotp_recv_ctx *ctx = CONTAINER_OF(to, struct isotp_recv_ctx, + timeout); + + switch (ctx->state) { + case ISOTP_RX_STATE_WAIT_CF: + LOG_ERR("Timeout while waiting for CF"); + receive_report_error(ctx, ISOTP_N_TIMEOUT_CR); + break; + + case ISOTP_RX_STATE_TRY_ALLOC: + ctx->state = ISOTP_RX_STATE_SEND_WAIT; + break; + } + + k_work_submit(&ctx->work); +} + +static int receive_alloc_buffer(struct isotp_recv_ctx *ctx) +{ + struct net_buf *buf = NULL; + + if (ctx->opts.bs == 0) { + /* Alloc all buffers because we can't wait during reception */ + buf = receive_alloc_buffer_chain(ctx->length); + } else { + buf = receive_alloc_buffer_chain(ctx->opts.bs * + (ISOTP_CAN_DL - 1)); + } + + if (!buf) { + z_add_timeout(&ctx->timeout, receive_timeout_handler, + k_ms_to_ticks_floor32(ISOTP_ALLOC_TIMEOUT)); + + if (ctx->wft == ISOTP_WFT_FIRST) { + LOG_DBG("Allocation failed. Append to alloc list"); + ctx->wft = 0; + sys_slist_append(&global_ctx.alloc_list, + &ctx->alloc_node); + } else { + LOG_DBG("Allocation failed. Send WAIT frame"); + ctx->state = ISOTP_RX_STATE_SEND_WAIT; + receive_state_machine(ctx); + } + + return -1; + } + + if (ctx->state == ISOTP_RX_STATE_TRY_ALLOC) { + z_abort_timeout(&ctx->timeout); + ctx->wft = ISOTP_WFT_FIRST; + sys_slist_find_and_remove(&global_ctx.alloc_list, + &ctx->alloc_node); + } + + if (ctx->opts.bs != 0) { + ctx->buf = buf; + } else { + net_buf_frag_insert(ctx->buf, buf); + } + + ctx->act_frag = buf; + return 0; +} + +static void receive_state_machine(struct isotp_recv_ctx *ctx) +{ + int ret; + u32_t *ud_rem_len; + + switch (ctx->state) { + case ISOTP_RX_STATE_PROCESS_SF: + ctx->buf->len = receive_get_sf_length(ctx->buf); + ud_rem_len = net_buf_user_data(ctx->buf); + *ud_rem_len = 0; + LOG_DBG("SM process SF of length %d", ctx->buf->len); + net_buf_put(&ctx->fifo, ctx->buf); + ctx->state = ISOTP_RX_STATE_RECYCLE; + receive_state_machine(ctx); + break; + + case ISOTP_RX_STATE_PROCESS_FF: + ctx->length = receive_get_ff_length(ctx->buf); + LOG_DBG("SM process FF. Length: %d", ctx->length); + ctx->length -= ctx->buf->len; + if (ctx->opts.bs == 0 && + ctx->length > CONFIG_ISOTP_RX_BUF_COUNT * + CONFIG_ISOTP_RX_BUF_SIZE) { + LOG_ERR("Pkt length is %d but buffer has only %d bytes", + ctx->length, + CONFIG_ISOTP_RX_BUF_COUNT * + CONFIG_ISOTP_RX_BUF_SIZE); + receive_report_error(ctx, ISOTP_N_BUFFER_OVERFLW); + receive_state_machine(ctx); + break; + } + + if (ctx->opts.bs) { + ctx->bs = ctx->opts.bs; + ud_rem_len = net_buf_user_data(ctx->buf); + *ud_rem_len = ctx->length; + net_buf_put(&ctx->fifo, ctx->buf); + } + + ctx->wft = ISOTP_WFT_FIRST; + ctx->state = ISOTP_RX_STATE_TRY_ALLOC; + /* FALLTHROUGH */ + case ISOTP_RX_STATE_TRY_ALLOC: + LOG_DBG("SM try to allocate"); + z_abort_timeout(&ctx->timeout); + ret = receive_alloc_buffer(ctx); + if (ret) { + LOG_DBG("SM allocation failed. Wait for free buffer"); + break; + } + + ctx->state = ISOTP_RX_STATE_SEND_FC; + /* FALLTHROUGH */ + case ISOTP_RX_STATE_SEND_FC: + LOG_DBG("SM send CTS FC frame"); + receive_send_fc(ctx, ISOTP_PCI_FS_CTS); + z_add_timeout(&ctx->timeout, receive_timeout_handler, + k_ms_to_ticks_ceil32(ISOTP_CR)); + ctx->state = ISOTP_RX_STATE_WAIT_CF; + break; + + case ISOTP_RX_STATE_SEND_WAIT: + if (++ctx->wft < CONFIG_ISOTP_WFTMAX) { + LOG_DBG("Send wait frame number %d", ctx->wft); + receive_send_fc(ctx, ISOTP_PCI_FS_WAIT); + z_add_timeout(&ctx->timeout, receive_timeout_handler, + k_ms_to_ticks_floor32(ISOTP_ALLOC_TIMEOUT)); + ctx->state = ISOTP_RX_STATE_TRY_ALLOC; + break; + } + + sys_slist_find_and_remove(&global_ctx.alloc_list, + &ctx->alloc_node); + LOG_ERR("Sent %d wait frames. Giving up to alloc now", + ctx->wft); + receive_report_error(ctx, ISOTP_N_BUFFER_OVERFLW); + /* FALLTHROUGH */ + case ISOTP_RX_STATE_ERR: + LOG_DBG("SM ERR state. err nr: %d", ctx->error_nr); + z_abort_timeout(&ctx->timeout); + + if (ctx->error_nr == ISOTP_N_BUFFER_OVERFLW) { + receive_send_fc(ctx, ISOTP_PCI_FS_OVFLW); + } + + k_fifo_cancel_wait(&ctx->fifo); + net_buf_unref(ctx->buf); + ctx->buf = NULL; + ctx->state = ISOTP_RX_STATE_RECYCLE; + /* FALLTHROUGH */ + case ISOTP_RX_STATE_RECYCLE: + LOG_DBG("SM recycle context for next message"); + ctx->buf = net_buf_alloc_fixed(&isotp_rx_sf_ff_pool, K_NO_WAIT); + if (!ctx->buf) { + LOG_DBG("No free context. Append to waiters list"); + sys_slist_append(&global_ctx.ff_sf_alloc_list, + &ctx->alloc_node); + break; + } + + sys_slist_find_and_remove(&global_ctx.ff_sf_alloc_list, + &ctx->alloc_node); + ctx->state = ISOTP_RX_STATE_WAIT_FF_SF; + /* FALLTHROUGH */ + case ISOTP_RX_STATE_UNBOUND: + break; + + default: + break; + } +} + +static void receive_work_handler(struct k_work *item) +{ + struct isotp_recv_ctx *ctx = CONTAINER_OF(item, struct isotp_recv_ctx, + work); + + receive_state_machine(ctx); +} + +static void process_ff_sf(struct isotp_recv_ctx *ctx, struct zcan_frame *frame) +{ + int index = 0; + + if (ctx->rx_addr.use_ext_addr) { + if (frame->data[index++] != ctx->rx_addr.ext_addr) { + return; + } + } + + switch (frame->data[index] & ISOTP_PCI_TYPE_MASK) { + case ISOTP_PCI_TYPE_FF: + LOG_DBG("Got FF IRQ"); + if (frame->dlc != ISOTP_CAN_DL) { + LOG_INF("FF DL does not match. Ignore"); + return; + } + + ctx->state = ISOTP_RX_STATE_PROCESS_FF; + ctx->sn_expected = 1; + break; + + case ISOTP_PCI_TYPE_SF: + LOG_DBG("Got SF IRQ"); + if ((frame->data[index] & ISOTP_PCI_FF_DL_UPPER_MASK) + + index + 1 != frame->dlc) { + LOG_INF("SF DL does not match. Ignore"); + return; + } + + ctx->state = ISOTP_RX_STATE_PROCESS_SF; + break; + + default: + LOG_INF("Got unexpected frame. Ignore"); + return; + } + + net_buf_add_mem(ctx->buf, &frame->data[index], frame->dlc - index); +} + +static inline void receive_add_mem(struct isotp_recv_ctx *ctx, u8_t *data, + size_t len) +{ + size_t tailroom = net_buf_tailroom(ctx->act_frag); + + if (tailroom >= len) { + net_buf_add_mem(ctx->act_frag, data, len); + return; + } + + /* Use next fragment that is already allocated*/ + net_buf_add_mem(ctx->act_frag, data, tailroom); + ctx->act_frag = ctx->act_frag->frags; + if (!ctx->act_frag) { + LOG_ERR("No fragmet left to append data"); + receive_report_error(ctx, ISOTP_N_BUFFER_OVERFLW); + } + + net_buf_add_mem(ctx->act_frag, data + tailroom, len - tailroom); +} + +static void process_cf(struct isotp_recv_ctx *ctx, struct zcan_frame *frame) +{ + u32_t *ud_rem_len = (u32_t *)net_buf_user_data(ctx->buf); + int index = 0; + + if (ctx->rx_addr.use_ext_addr) { + if (frame->data[index++] != ctx->rx_addr.ext_addr) { + return; + } + } + + if ((frame->data[index] & ISOTP_PCI_TYPE_MASK) != ISOTP_PCI_TYPE_CF) { + LOG_DBG("Waiting for CF but got something else (%d)", + frame->data[index] >> ISOTP_PCI_TYPE_POS); + receive_report_error(ctx, ISOTP_N_UNEXP_PDU); + k_work_submit(&ctx->work); + return; + } + + z_abort_timeout(&ctx->timeout); + z_add_timeout(&ctx->timeout, receive_timeout_handler, + k_ms_to_ticks_ceil32(ISOTP_CR)); + + if ((frame->data[index++] & ISOTP_PCI_SN_MASK) != ctx->sn_expected++) { + LOG_ERR("Sequence number missmatch"); + receive_report_error(ctx, ISOTP_N_WRONG_SN); + k_work_submit(&ctx->work); + return; + } + + if (frame->dlc - index > ctx->length) { + LOG_ERR("The frame contains more bytes than expected"); + receive_report_error(ctx, ISOTP_N_ERROR); + } + + LOG_DBG("Got CF irq. Appending data"); + receive_add_mem(ctx, &frame->data[index], frame->dlc - index); + ctx->length -= frame->dlc - index; + LOG_DBG("%d bytes remaining", ctx->length); + + if (ctx->length == 0) { + ctx->state = ISOTP_RX_STATE_RECYCLE; + *ud_rem_len = 0; + net_buf_put(&ctx->fifo, ctx->buf); + return; + } + + if (ctx->opts.bs && !--ctx->bs) { + LOG_DBG("Block is complete. Allocate new buffer"); + ctx->bs = ctx->opts.bs; + *ud_rem_len = ctx->length; + net_buf_put(&ctx->fifo, ctx->buf); + ctx->state = ISOTP_RX_STATE_TRY_ALLOC; + } +} + +static void receive_can_rx_isr(struct zcan_frame *frame, void *arg) +{ + struct isotp_recv_ctx *ctx = (struct isotp_recv_ctx *)arg; + + switch (ctx->state) { + case ISOTP_RX_STATE_WAIT_FF_SF: + __ASSERT_NO_MSG(ctx->buf); + process_ff_sf(ctx, frame); + break; + + case ISOTP_RX_STATE_WAIT_CF: + process_cf(ctx, frame); + /* still waiting for more CF */ + if (ctx->state == ISOTP_RX_STATE_WAIT_CF) { + return; + } + + break; + + case ISOTP_RX_STATE_RECYCLE: + LOG_ERR("Got a frame but was not yet ready for a new one"); + receive_report_error(ctx, ISOTP_N_BUFFER_OVERFLW); + break; + + default: + LOG_INF("Got a frame in a state where it is unexpected."); + } + + k_work_submit(&ctx->work); +} + +static inline int attach_ff_filter(struct isotp_recv_ctx *ctx) +{ + struct zcan_filter filter = { + .id_type = ctx->rx_addr.id_type, + .rtr = CAN_DATAFRAME, + .ext_id = ctx->rx_addr.ext_id, + .rtr_mask = 1, + .ext_id_mask = CAN_EXT_ID_MASK + }; + + ctx->filter_id = can_attach_isr(ctx->can_dev, receive_can_rx_isr, ctx, + &filter); + if (ctx->filter_id < 0) { + LOG_ERR("Error attaching FF filter [%d]", ctx->filter_id); + return ISOTP_NO_FREE_FILTER; + } + + return 0; +} + +int isotp_bind(struct isotp_recv_ctx *ctx, struct device *can_dev, + const struct isotp_msg_id *rx_addr, + const struct isotp_msg_id *tx_addr, + const struct isotp_fc_opts *opts, + s32_t timeout) +{ + int ret; + + __ASSERT(ctx, "ctx is NULL"); + __ASSERT(can_dev, "CAN device is NULL"); + __ASSERT(rx_addr && rx_addr, "RX or TX addr is NULL"); + __ASSERT(opts, "OPTS is NULL"); + + ctx->can_dev = can_dev; + ctx->rx_addr = *rx_addr; + ctx->tx_addr = *tx_addr; + k_fifo_init(&ctx->fifo); + + __ASSERT(opts->stmin < ISOTP_STMIN_MAX, "STmin limit"); + __ASSERT(opts->stmin <= ISOTP_STMIN_MS_MAX || + opts->stmin >= ISOTP_STMIN_US_BEGIN, "STmin reserved"); + + ctx->opts = *opts; + ctx->state = ISOTP_RX_STATE_WAIT_FF_SF; + + LOG_DBG("Binding to addr: 0x%x. Responding on 0x%x", + ctx->rx_addr.ext_id, ctx->tx_addr.ext_id); + + ctx->buf = net_buf_alloc_fixed(&isotp_rx_sf_ff_pool, timeout); + if (!ctx->buf) { + LOG_ERR("No buffer for FF left"); + return ISOTP_NO_NET_BUF_LEFT; + } + + ret = attach_ff_filter(ctx); + if (ret) { + LOG_ERR("Can't attach filter for binding"); + net_buf_unref(ctx->buf); + ctx->buf = NULL; + return ret; + } + + k_work_init(&ctx->work, receive_work_handler); + z_init_timeout(&ctx->timeout); + + return ISOTP_N_OK; +} + +void isotp_unbind(struct isotp_recv_ctx *ctx) +{ + struct net_buf *buf; + + if (ctx->filter_id >= 0 && ctx->can_dev) { + can_detach(ctx->can_dev, ctx->filter_id); + } + + z_abort_timeout(&ctx->timeout); + + sys_slist_find_and_remove(&global_ctx.ff_sf_alloc_list, + &ctx->alloc_node); + sys_slist_find_and_remove(&global_ctx.alloc_list, + &ctx->alloc_node); + + ctx->state = ISOTP_RX_STATE_UNBOUND; + + while ((buf = net_buf_get(&ctx->fifo, K_NO_WAIT))) { + net_buf_unref(buf); + } + + k_fifo_cancel_wait(&ctx->fifo); + + if (ctx->buf) { + net_buf_unref(ctx->buf); + } + + LOG_DBG("Unbound"); +} + +int isotp_recv_net(struct isotp_recv_ctx *ctx, struct net_buf **buffer, + s32_t timeout) +{ + struct net_buf *buf; + int ret; + + buf = net_buf_get(&ctx->fifo, timeout); + if (!buf) { + ret = ctx->error_nr ? ctx->error_nr : ISOTP_RECV_TIMEOUT; + ctx->error_nr = 0; + + return ret; + } + + *buffer = buf; + + return *(u32_t *)net_buf_user_data(buf); +} + +static inline void pull_frags(struct k_fifo *fifo, struct net_buf *buf, + size_t len) +{ + size_t rem_len = len; + struct net_buf *frag = buf; + + /* frags to be removed */ + while (frag && (frag->len <= rem_len)) { + rem_len -= frag->len; + frag = frag->frags; + k_fifo_get(fifo, K_NO_WAIT); + } + + if (frag) { + /* Start of frags to be preserved */ + net_buf_ref(frag); + net_buf_pull(frag, rem_len); + } + + net_buf_unref(buf); +} + +int isotp_recv(struct isotp_recv_ctx *ctx, u8_t *data, size_t len, + s32_t timeout) +{ + size_t num_copied, frags_len; + struct net_buf *buf; + int ret; + + ret = _k_fifo_wait_non_empty(&ctx->fifo, timeout); + if (ret) { + if (ctx->error_nr) { + ret = ctx->error_nr; + ctx->error_nr = 0; + return ret; + } + + if (ret == -EAGAIN) { + return ISOTP_RECV_TIMEOUT; + } + + return ISOTP_N_ERROR; + } + + buf = k_fifo_peek_head(&ctx->fifo); + + if (!buf) { + return ISOTP_N_ERROR; + } + + frags_len = net_buf_frags_len(buf); + num_copied = net_buf_linearize(data, len, buf, 0, len); + + pull_frags(&ctx->fifo, buf, num_copied); + + return num_copied; +} + +static inline void send_report_error(struct isotp_send_ctx *ctx, u32_t err) +{ + ctx->state = ISOTP_TX_ERR; + ctx->error_nr = err; +} + +static void send_can_tx_isr(u32_t err_flags, void *arg) +{ + struct isotp_send_ctx *ctx = (struct isotp_send_ctx *)arg; + + ctx->tx_backlog--; + + if (ctx->state == ISOTP_TX_WAIT_BACKLOG) { + if (ctx->tx_backlog > 0) { + return; + } + + ctx->state = ISOTP_TX_WAIT_FIN; + } + + k_work_submit(&ctx->work); +} + +static void send_timeout_handler(struct _timeout *to) +{ + struct isotp_send_ctx *ctx = CONTAINER_OF(to, struct isotp_send_ctx, + timeout); + + if (ctx->state != ISOTP_TX_SEND_CF) { + send_report_error(ctx, ISOTP_N_TIMEOUT_BS); + LOG_ERR("Reception of next FC has timed out"); + } + + k_work_submit(&ctx->work); +} + +static void send_process_fc(struct isotp_send_ctx *ctx, + struct zcan_frame *frame) +{ + u8_t *data = frame->data; + + if (ctx->rx_addr.use_ext_addr) { + if (ctx->rx_addr.ext_addr != *data++) { + return; + } + } + + if ((*data & ISOTP_PCI_TYPE_MASK) != ISOTP_PCI_TYPE_FC) { + LOG_ERR("Got unexpected PDU expected FC"); + send_report_error(ctx, ISOTP_N_UNEXP_PDU); + return; + } + + switch (*data++ & ISOTP_PCI_FS_MASK) { + case ISOTP_PCI_FS_CTS: + ctx->state = ISOTP_TX_SEND_CF; + ctx->wft = 0; + ctx->tx_backlog = 0; + ctx->opts.bs = *data++; + ctx->opts.stmin = *data++; + ctx->bs = ctx->opts.bs; + LOG_DBG("Got CTS. BS: %d, STmin: %d", ctx->opts.bs, + ctx->opts.stmin); + break; + + case ISOTP_PCI_FS_WAIT: + LOG_DBG("Got WAIT frame"); + z_abort_timeout(&ctx->timeout); + z_add_timeout(&ctx->timeout, send_timeout_handler, + k_ms_to_ticks_ceil32(ISOTP_BS)); + if (ctx->wft >= CONFIG_ISOTP_WFTMAX) { + LOG_INF("Got to many wait frames"); + send_report_error(ctx, ISOTP_N_WFT_OVRN); + } + + ctx->wft++; + break; + + case ISOTP_PCI_FS_OVFLW: + LOG_ERR("Got overflow FC frame"); + send_report_error(ctx, ISOTP_N_BUFFER_OVERFLW); + break; + + default: + send_report_error(ctx, ISOTP_N_INVALID_FS); + } +} + +static void send_can_rx_isr(struct zcan_frame *frame, void *arg) +{ + struct isotp_send_ctx *ctx = (struct isotp_send_ctx *)arg; + + if (ctx->state == ISOTP_TX_WAIT_FC) { + z_abort_timeout(&ctx->timeout); + send_process_fc(ctx, frame); + } else { + LOG_ERR("Got unexpected PDU"); + send_report_error(ctx, ISOTP_N_UNEXP_PDU); + } + + k_work_submit(&ctx->work); +} + +static size_t get_ctx_data_length(struct isotp_send_ctx *ctx) +{ + return ctx->is_net_buf ? net_buf_frags_len(ctx->buf) : ctx->len; +} + +static const u8_t *get_data_ctx(struct isotp_send_ctx *ctx) +{ + if (ctx->is_net_buf) { + return ctx->buf->data; + } else { + return ctx->data; + } +} + +static void pull_data_ctx(struct isotp_send_ctx *ctx, size_t len) +{ + if (ctx->is_net_buf) { + net_buf_pull_mem(ctx->buf, len); + } else { + ctx->data += len; + ctx->len -= len; + } +} + +static inline int send_sf(struct isotp_send_ctx *ctx) +{ + struct zcan_frame frame = { + .id_type = ctx->tx_addr.id_type, + .rtr = CAN_DATAFRAME, + .ext_id = ctx->tx_addr.ext_id + }; + size_t len = get_ctx_data_length(ctx); + int index = 0; + int ret; + const u8_t *data; + + data = get_data_ctx(ctx); + pull_data_ctx(ctx, len); + + if (ctx->tx_addr.use_ext_addr) { + frame.data[index++] = ctx->tx_addr.ext_addr; + } + + frame.data[index++] = ISOTP_PCI_TYPE_SF | len; + + __ASSERT_NO_MSG(len <= ISOTP_CAN_DL - index); + memcpy(&frame.data[index], data, len); + + frame.dlc = len + index; + + ctx->state = ISOTP_TX_SEND_SF; + ret = can_send(ctx->can_dev, &frame, ISOTP_A, send_can_tx_isr, ctx); + return ret; +} + +static inline int send_ff(struct isotp_send_ctx *ctx) +{ + struct zcan_frame frame = { + .id_type = ctx->tx_addr.id_type, + .rtr = CAN_DATAFRAME, + .ext_id = ctx->tx_addr.ext_id, + .dlc = ISOTP_CAN_DL + }; + int index = 0; + size_t len = get_ctx_data_length(ctx); + int ret; + const u8_t *data; + + if (ctx->tx_addr.use_ext_addr) { + frame.data[index++] = ctx->tx_addr.ext_addr; + } + + if (len > 0xFFF) { + frame.data[index++] = ISOTP_PCI_TYPE_FF; + frame.data[index++] = 0; + frame.data[index++] = (len >> 3 * 8) & 0xFF; + frame.data[index++] = (len >> 2 * 8) & 0xFF; + frame.data[index++] = (len >> 8) & 0xFF; + frame.data[index++] = len & 0xFF; + } else { + frame.data[index++] = ISOTP_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 + */ + ctx->sn = 1; + data = get_data_ctx(ctx); + pull_data_ctx(ctx, ISOTP_CAN_DL - index); + memcpy(&frame.data[index], data, ISOTP_CAN_DL - index); + + ret = can_send(ctx->can_dev, &frame, ISOTP_A, send_can_tx_isr, ctx); + return ret; +} + +static inline int send_cf(struct isotp_send_ctx *ctx) +{ + struct zcan_frame frame = { + .id_type = ctx->tx_addr.id_type, + .rtr = CAN_DATAFRAME, + .ext_id = ctx->tx_addr.ext_id, + }; + int index = 0; + int ret; + int len; + int rem_len; + const u8_t *data; + + if (ctx->tx_addr.use_ext_addr) { + frame.data[index++] = ctx->tx_addr.ext_addr; + } + + /*sn wraps around at 0xF automatically because it has a 4 bit size*/ + frame.data[index++] = ISOTP_PCI_TYPE_CF | ctx->sn; + + rem_len = get_ctx_data_length(ctx); + len = MIN(rem_len, ISOTP_CAN_DL - index); + rem_len -= len; + frame.dlc = len + index; + data = get_data_ctx(ctx); + memcpy(&frame.data[index], data, len); + + ret = can_send(ctx->can_dev, &frame, ISOTP_A, send_can_tx_isr, ctx); + if (ret == CAN_TX_OK) { + ctx->sn++; + pull_data_ctx(ctx, len); + ctx->bs--; + ctx->tx_backlog++; + } + + ret = ret ? ret : rem_len; + return ret; +} + +#ifdef CONFIG_ISOTP_ENABLE_CONTEXT_BUFFERS +static inline void free_send_ctx(struct isotp_send_ctx **ctx) +{ + if ((*ctx)->is_net_buf) { + net_buf_unref((*ctx)->buf); + (*ctx)->buf = NULL; + } + + if ((*ctx)->is_ctx_slab) { + k_mem_slab_free(&ctx_slab, (void **)ctx); + } +} + +static int alloc_ctx(struct isotp_send_ctx **ctx, s32_t timeout) +{ + int ret; + + ret = k_mem_slab_alloc(&ctx_slab, (void **)ctx, timeout); + if (ret) { + return ISOTP_NO_CTX_LEFT; + } + + (*ctx)->is_ctx_slab = 1; + + return 0; +} +#else +#define free_send_ctx(x) +#endif /*CONFIG_ISOTP_ENABLE_CONTEXT_BUFFERS*/ + +static s32_t stmin_to_ticks(u8_t stmin) +{ + /* According to ISO 15765-2 stmin should be 127ms if value is corrupt */ + if (stmin > ISOTP_STMIN_MAX || + (stmin > ISOTP_STMIN_MS_MAX && stmin < ISOTP_STMIN_US_BEGIN)) { + return k_ms_to_ticks_ceil32(K_MSEC(ISOTP_STMIN_MS_MAX)); + } + + if (stmin >= ISOTP_STMIN_US_BEGIN) { + return k_us_to_ticks_ceil32((stmin + 1 - ISOTP_STMIN_US_BEGIN) * + 100U); + } + + return k_ms_to_ticks_ceil32(K_MSEC(stmin)); +} + +static void send_state_machine(struct isotp_send_ctx *ctx) +{ + int ret; + + switch (ctx->state) { + + case ISOTP_TX_SEND_FF: + LOG_DBG("SM send FF"); + send_ff(ctx); + z_add_timeout(&ctx->timeout, send_timeout_handler, + k_ms_to_ticks_ceil32(ISOTP_BS)); + ctx->state = ISOTP_TX_WAIT_FC; + break; + + case ISOTP_TX_SEND_CF: + LOG_DBG("SM send CF"); + z_abort_timeout(&ctx->timeout); + do { + ret = send_cf(ctx); + if (!ret) { + ctx->state = ISOTP_TX_WAIT_BACKLOG; + break; + } + + if (ret < 0) { + LOG_ERR("Failed to send CF"); + send_report_error(ctx, ret == CAN_TIMEOUT ? + ISOTP_N_TIMEOUT_A : + ISOTP_N_ERROR); + break; + } + + if (ctx->opts.bs && !ctx->bs) { + LOG_DBG("BS reached. Wait for FC again"); + ctx->state = ISOTP_TX_WAIT_FC; + z_add_timeout(&ctx->timeout, + send_timeout_handler, + k_ms_to_ticks_ceil32(ISOTP_BS)); + break; + } else if (ctx->opts.stmin) { + ctx->state = ISOTP_TX_WAIT_ST; + break; + } + } while (ret > 0); + + break; + + case ISOTP_TX_WAIT_ST: + LOG_DBG("SM wait ST"); + z_add_timeout(&ctx->timeout, send_timeout_handler, + stmin_to_ticks(ctx->opts.stmin)); + ctx->state = ISOTP_TX_SEND_CF; + break; + + case ISOTP_TX_ERR: + LOG_DBG("SM error"); + /* FALLTHROUGH */ + case ISOTP_TX_WAIT_FIN: + if (ctx->filter_id >= 0) { + can_detach(ctx->can_dev, ctx->filter_id); + } + + LOG_DBG("SM finish"); + z_abort_timeout(&ctx->timeout); + + if (ctx->has_callback) { + ctx->fin_cb.cb(ctx->error_nr, ctx->fin_cb.arg); + free_send_ctx(&ctx); + } else { + k_sem_give(&ctx->fin_sem); + } + + ctx->state = ISOTP_TX_STATE_RESET; + break; + + default: + break; + } +} + +static void send_work_handler(struct k_work *item) +{ + struct isotp_send_ctx *ctx = CONTAINER_OF(item, struct isotp_send_ctx, + work); + + send_state_machine(ctx); +} + +static inline int attach_fc_filter(struct isotp_send_ctx *ctx) +{ + struct zcan_filter filter = { + .id_type = ctx->rx_addr.id_type, + .rtr = CAN_DATAFRAME, + .ext_id = ctx->rx_addr.ext_id, + .rtr_mask = 1, + .ext_id_mask = CAN_EXT_ID_MASK + }; + + ctx->filter_id = can_attach_isr(ctx->can_dev, send_can_rx_isr, ctx, + &filter); + if (ctx->filter_id < 0) { + LOG_ERR("Error attaching FC filter [%d]", ctx->filter_id); + return ISOTP_NO_FREE_FILTER; + } + + return 0; +} + +static int send(struct isotp_send_ctx *ctx, struct device *can_dev, + const struct isotp_msg_id *tx_addr, + const struct isotp_msg_id *rx_addr, + isotp_tx_callback_t complete_cb, void *cb_arg) +{ + size_t len; + int ret; + + __ASSERT_NO_MSG(ctx); + __ASSERT_NO_MSG(can_dev); + __ASSERT_NO_MSG(rx_addr && rx_addr); + + if (complete_cb) { + ctx->fin_cb.cb = complete_cb; + ctx->fin_cb.arg = cb_arg; + ctx->has_callback = 1; + } else { + k_sem_init(&ctx->fin_sem, 0, 1); + ctx->has_callback = 0; + } + + ctx->can_dev = can_dev; + ctx->tx_addr = *tx_addr; + ctx->rx_addr = *rx_addr; + ctx->error_nr = ISOTP_N_OK; + ctx->wft = 0; + k_work_init(&ctx->work, send_work_handler); + z_init_timeout(&ctx->timeout); + + len = get_ctx_data_length(ctx); + LOG_DBG("Send %d bytes to addr 0x%x and listen on 0x%x", len, + ctx->tx_addr.ext_id, ctx->rx_addr.ext_id); + if (len > ISOTP_CAN_DL - (tx_addr->use_ext_addr ? 2 : 1)) { + ret = attach_fc_filter(ctx); + if (ret) { + LOG_ERR("Can't attach fc filter: %d", ret); + return ret; + } + + LOG_DBG("Starting work to send FF"); + ctx->state = ISOTP_TX_SEND_FF; + k_work_submit(&ctx->work); + } else { + LOG_DBG("Sending single frame"); + ctx->filter_id = -1; + ret = send_sf(ctx); + ctx->state = ISOTP_TX_WAIT_FIN; + if (ret) { + return ret == CAN_TIMEOUT ? + ISOTP_N_TIMEOUT_A : ISOTP_N_ERROR; + } + } + + if (!complete_cb) { + k_sem_take(&ctx->fin_sem, K_FOREVER); + ret = ctx->error_nr; + free_send_ctx(&ctx); + return ret; + } + + return ISOTP_N_OK; +} + +int isotp_send(struct isotp_send_ctx *ctx, struct device *can_dev, + const u8_t *data, size_t len, + const struct isotp_msg_id *tx_addr, + const struct isotp_msg_id *rx_addr, + isotp_tx_callback_t complete_cb, void *cb_arg) +{ + ctx->data = data; + ctx->len = len; + ctx->is_ctx_slab = 0; + ctx->is_net_buf = 0; + + return send(ctx, can_dev, tx_addr, rx_addr, complete_cb, cb_arg); +} + +#ifdef CONFIG_ISOTP_ENABLE_CONTEXT_BUFFERS + +int isotp_send_ctx_buf(struct device *can_dev, + const u8_t *data, size_t len, + const struct isotp_msg_id *tx_addr, + const struct isotp_msg_id *rx_addr, + isotp_tx_callback_t complete_cb, void *cb_arg, + s32_t timeout) +{ + struct isotp_send_ctx *ctx; + int ret; + + __ASSERT_NO_MSG(data); + + ret = alloc_ctx(&ctx, timeout); + if (ret) { + return ret; + } + + ctx->data = data; + ctx->len = len; + ctx->is_net_buf = 0; + + return send(ctx, can_dev, tx_addr, rx_addr, complete_cb, cb_arg); +} + +int isotp_send_net_ctx_buf(struct device *can_dev, + struct net_buf *data, + const struct isotp_msg_id *tx_addr, + const struct isotp_msg_id *rx_addr, + isotp_tx_callback_t complete_cb, void *cb_arg, + s32_t timeout) +{ + struct isotp_send_ctx *ctx; + int ret; + + __ASSERT_NO_MSG(data); + + ret = alloc_ctx(&ctx, timeout); + if (ret) { + return ret; + } + + ctx->is_net_buf = 1; + ctx->buf = data; + + return send(ctx, can_dev, tx_addr, rx_addr, complete_cb, cb_arg); +} + +#ifdef CONFIG_ISOTP_USE_TX_BUF +int isotp_send_buf(struct device *can_dev, + const u8_t *data, size_t len, + const struct isotp_msg_id *tx_addr, + const struct isotp_msg_id *rx_addr, + isotp_tx_callback_t complete_cb, void *cb_arg, + s32_t timeout) +{ + struct isotp_send_ctx *ctx; + struct net_buf *buf; + int ret; + + __ASSERT_NO_MSG(data); + + ret = alloc_ctx(&ctx, timeout); + if (ret) { + return ret; + } + + buf = net_buf_alloc_len(&isotp_tx_pool, len, timeout); + if (!buf) { + k_mem_slab_free(&ctx_slab, (void **)&ctx); + return ISOTP_NO_BUF_DATA_LEFT; + } + + net_buf_add_mem(buf, data, len); + + ctx->is_net_buf = 1; + ctx->buf = buf; + + return send(ctx, can_dev, tx_addr, rx_addr, complete_cb, cb_arg); +} +#endif /*CONFIG_ISOTP_USE_TX_BUF*/ +#endif /*CONFIG_ISOTP_ENABLE_CONTEXT_BUFFERS*/ + +static int isotp_workq_init(struct device *dev) +{ + ARG_UNUSED(dev); + LOG_DBG("Starting workqueue"); + k_work_q_start(&isotp_workq, + tx_stack, + K_THREAD_STACK_SIZEOF(tx_stack), + CONFIG_ISOTP_WORKQUEUE_PRIO); + k_thread_name_set(&isotp_workq.thread, "isotp_work"); + + return 0; +} + +SYS_INIT(isotp_workq_init, POST_KERNEL, CONFIG_APPLICATION_INIT_PRIORITY); diff --git a/subsys/canbus/isotp/isotp_internal.h b/subsys/canbus/isotp/isotp_internal.h new file mode 100644 index 00000000000..e4a73cbf142 --- /dev/null +++ b/subsys/canbus/isotp/isotp_internal.h @@ -0,0 +1,124 @@ +/* + * Copyright (c) 2019 Alexander Wachter + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_SUBSYS_NET_CAN_ISOTP_INTERNAL_H_ +#define ZEPHYR_SUBSYS_NET_CAN_ISOTP_INTERNAL_H_ + + +#include +#include + +/* + * 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 + * SF Single Frame + * FS Flow Status + * AE Adders Extension + * SN Sequence Number + * ST Separation time + * PCI Process Control Information + */ + +/* This is for future use when we have CAN-FD */ +#ifdef ISOTP_USE_CAN_FD +/* #define ISOTP_CAN_DL CONFIG_ISOTP_TX_DL* */ +#define ISOTP_CAN_DL 8 +#else +#define ISOTP_CAN_DL 8 +#endif/*ISOTP_USE_CAN_FD*/ + +/* Protocol control information*/ +#define ISOTP_PCI_SF 0x00 /* Single frame*/ +#define ISOTP_PCI_FF 0x01 /* First frame */ +#define ISOTP_PCI_CF 0x02 /* Consecutive frame */ +#define ISOTP_PCI_FC 0x03 /* Flow control frame */ + +#define ISOTP_PCI_TYPE_BYTE 0 +#define ISOTP_PCI_TYPE_POS 4 +#define ISOTP_PCI_TYPE_MASK 0xF0 +#define ISOTP_PCI_TYPE_SF (ISOTP_PCI_SF << ISOTP_PCI_TYPE_POS) +#define ISOTP_PCI_TYPE_FF (ISOTP_PCI_FF << ISOTP_PCI_TYPE_POS) +#define ISOTP_PCI_TYPE_CF (ISOTP_PCI_CF << ISOTP_PCI_TYPE_POS) +#define ISOTP_PCI_TYPE_FC (ISOTP_PCI_FC << ISOTP_PCI_TYPE_POS) + +#define ISOTP_PCI_SF_DL_MASK 0x0F + +#define ISOTP_PCI_FF_DL_UPPER_BYTE 0 +#define ISOTP_PCI_FF_DL_UPPER_MASK 0x0F +#define ISOTP_PCI_FF_DL_LOWER_BYTE 1 + +#define ISOTP_PCI_FS_BYTE 0 +#define ISOTP_PCI_FS_MASK 0x0F +#define ISOTP_PCI_BS_BYTE 1 +#define ISOTP_PCI_ST_MIN_BYTE 2 + +#define ISOTP_PCI_FS_CTS 0x0 +#define ISOTP_PCI_FS_WAIT 0x1 +#define ISOTP_PCI_FS_OVFLW 0x2 + +#define ISOTP_PCI_SN_MASK 0x0F + +#define ISOTP_FF_DL_MIN (ISOTP_CAN_DL) + +#define ISOTP_STMIN_MAX 0xFA +#define ISOTP_STMIN_MS_MAX 0x7F +#define ISOTP_STMIN_US_BEGIN 0xF1 +#define ISOTP_STMIN_US_END 0xF9 + +#define ISOTP_WFT_FIRST 0xFF + +#define ISOTP_BS K_MSEC(CONFIG_ISOTP_BS_TIMEOUT) +#define ISOTP_A K_MSEC(CONFIG_ISOTP_A_TIMEOUT) +#define ISOTP_CR K_MSEC(CONFIG_ISOTP_CR_TIMEOUT) + +/* Just before the sender would time out*/ +#define ISOTP_ALLOC_TIMEOUT K_MSEC(CONFIG_ISOTP_A_TIMEOUT - 100) + +#ifdef __cplusplus +extern "C" { +#endif + +enum isotp_rx_state { + ISOTP_RX_STATE_WAIT_FF_SF, + ISOTP_RX_STATE_PROCESS_SF, + ISOTP_RX_STATE_PROCESS_FF, + ISOTP_RX_STATE_TRY_ALLOC, + ISOTP_RX_STATE_SEND_FC, + ISOTP_RX_STATE_WAIT_CF, + ISOTP_RX_STATE_SEND_WAIT, + ISOTP_RX_STATE_ERR, + ISOTP_RX_STATE_RECYCLE, + ISOTP_RX_STATE_UNBOUND +}; + +enum isotp_tx_state { + ISOTP_TX_STATE_RESET, + ISOTP_TX_SEND_SF, + ISOTP_TX_SEND_FF, + ISOTP_TX_WAIT_FC, + ISOTP_TX_SEND_CF, + ISOTP_TX_WAIT_ST, + ISOTP_TX_WAIT_BACKLOG, + ISOTP_TX_WAIT_FIN, + ISOTP_TX_ERR +}; + +struct isotp_global_ctx { + sys_slist_t alloc_list; + sys_slist_t ff_sf_alloc_list; +}; + +#ifdef __cplusplus +} +#endif + +#endif /* ZEPHYR_SUBSYS_NET_CAN_ISOTP_INTERNAL_H_ */