From 63ca443b05ee3bd3dc3eef4e48e487a7e14cea45 Mon Sep 17 00:00:00 2001
From: Vincent Dupont <vincent@otakeys.com>
Date: Wed, 23 Nov 2016 19:08:30 +0100
Subject: [PATCH] can: add ISO-TP support
ISO-TP is ISO15765 transport protocol over CAN.
Upper interface is located in sys/include/can/isotp.h.
---
Makefile.dep | 4 +
sys/auto_init/can/auto_init_can.c | 20 +
sys/can/Makefile | 5 +
sys/can/isotp/Makefile | 3 +
sys/can/isotp/isotp.c | 898 ++++++++++++++++++++++++++++++
sys/include/can/isotp.h | 196 +++++++
6 files changed, 1126 insertions(+)
create mode 100644 sys/can/isotp/Makefile
create mode 100644 sys/can/isotp/isotp.c
create mode 100644 sys/include/can/isotp.h
diff --git a/Makefile.dep b/Makefile.dep
index 267a121c8d..d4706ae620 100644
--- a/Makefile.dep
+++ b/Makefile.dep
@@ -579,6 +579,10 @@ ifneq (,$(filter can,$(USEMODULE)))
USEMODULE += gnrc_pktbuf_static
endif
+ifneq (,$(filter can_isotp,$(USEMODULE)))
+ USEMODULE += xtimer
+endif
+
ifneq (,$(filter random,$(USEMODULE)))
# select default prng
ifeq (,$(filter prng_%,$(USEMODULE)))
diff --git a/sys/auto_init/can/auto_init_can.c b/sys/auto_init/can/auto_init_can.c
index e94cddbb80..c5f9dfa1fc 100644
--- a/sys/auto_init/can/auto_init_can.c
+++ b/sys/auto_init/can/auto_init_can.c
@@ -25,10 +25,30 @@
#include "can/dll.h"
+#ifdef MODULE_CAN_ISOTP
+#include "can/isotp.h"
+
+#ifndef ISOTP_STACK_SIZE
+#define ISOTP_STACK_SIZE (THREAD_STACKSIZE_DEFAULT + THREAD_EXTRA_STACKSIZE_PRINTF)
+#endif
+
+#ifndef ISOTP_PRIORITY
+#define ISOTP_PRIORITY (THREAD_PRIORITY_MAIN - 2)
+#endif
+
+static char isotp_stack[ISOTP_STACK_SIZE];
+#endif
+
void auto_init_candev(void)
{
DEBUG("auto_init_can: init dll\n");
can_dll_init();
+
+#ifdef MODULE_CAN_ISOTP
+ DEBUG("auto_init_can: init isotp\n");
+ isotp_init(isotp_stack, ISOTP_STACK_SIZE, ISOTP_PRIORITY, "isotp");
+#endif
+
#ifdef MODULE_CAN_LINUX
extern void auto_init_can_native(void);
auto_init_can_native();
diff --git a/sys/can/Makefile b/sys/can/Makefile
index 48422e909a..449d4878ca 100644
--- a/sys/can/Makefile
+++ b/sys/can/Makefile
@@ -1 +1,6 @@
+
+ifneq (,$(filter can_isotp,$(USEMODULE)))
+ DIRS += isotp
+endif
+
include $(RIOTBASE)/Makefile.base
diff --git a/sys/can/isotp/Makefile b/sys/can/isotp/Makefile
new file mode 100644
index 0000000000..9f32f4c6e4
--- /dev/null
+++ b/sys/can/isotp/Makefile
@@ -0,0 +1,3 @@
+MODULE = can_isotp
+
+include $(RIOTBASE)/Makefile.base
diff --git a/sys/can/isotp/isotp.c b/sys/can/isotp/isotp.c
new file mode 100644
index 0000000000..8ee8c72b2d
--- /dev/null
+++ b/sys/can/isotp/isotp.c
@@ -0,0 +1,898 @@
+/*
+ * Copyright (C) 2016 OTA keys S.A.
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @file
+ * @brief ISO TP high level interface
+ *
+ * @author Vincent Dupont <vincent@otakeys.com>
+ */
+
+#include <errno.h>
+#include <string.h>
+
+#include "net/gnrc/pktbuf.h"
+
+#include "can/isotp.h"
+#include "can/common.h"
+#include "can/raw.h"
+#include "can/router.h"
+#include "thread.h"
+#include "mutex.h"
+#include "timex.h"
+#include "utlist.h"
+
+#define ENABLE_DEBUG (0)
+#include "debug.h"
+
+#ifndef CAN_ISOTP_BS
+#define CAN_ISOTP_BS 10
+#endif
+
+#ifndef CAN_ISOTP_STMIN
+#define CAN_ISOTP_STMIN 5
+#endif
+
+#ifndef CAN_ISOTP_WFTMAX
+#define CAN_ISOTP_WFTMAX 0
+#endif
+
+#ifndef CAN_ISOTP_MSG_QUEUE_SIZE
+#define CAN_ISOTP_MSG_QUEUE_SIZE 64
+#endif
+
+#ifndef CAN_ISOTP_TIMEOUT_N_As
+#define CAN_ISOTP_TIMEOUT_N_As (1 * US_PER_SEC)
+#endif
+
+#ifndef CAN_ISOTP_TIMEOUT_N_Bs
+#define CAN_ISOTP_TIMEOUT_N_Bs (1 * US_PER_SEC)
+#endif
+
+#ifndef CAN_ISOTP_TIMEOUT_N_Ar
+#define CAN_ISOTP_TIMEOUT_N_Ar (1 * US_PER_SEC)
+#endif
+
+#ifndef CAN_ISOTP_TIMEOUT_N_Cr
+#define CAN_ISOTP_TIMEOUT_N_Cr (1 * US_PER_SEC)
+#endif
+
+enum {
+ ISOTP_IDLE = 0,
+ ISOTP_WAIT_FC,
+ ISOTP_WAIT_CF,
+ ISOTP_SENDING_SF,
+ ISOTP_SENDING_FF,
+ ISOTP_SENDING_CF,
+ ISOTP_SENDING_FC,
+ ISOTP_SENDING_NEXT_CF,
+};
+
+#define MAX_MSG_LENGTH 4095
+
+/* N_PCI type values in bits 7-4 of N_PCI bytes */
+#define N_PCI_SF 0x00 /* single frame */
+#define N_PCI_FF 0x10 /* first frame */
+#define N_PCI_CF 0x20 /* consecutive frame */
+#define N_PCI_FC 0x30 /* flow control */
+
+#define N_PCI_SZ 1 /* size of the PCI byte #1 */
+#define SF_PCI_SZ 1 /* size of SingleFrame PCI including 4 bit SF_DL */
+#define FF_PCI_SZ 2 /* size of FirstFrame PCI including 12 bit FF_DL */
+#define FC_CONTENT_SZ 3 /* flow control content size in byte (FS/BS/STmin) */
+
+/* Flow Status given in FC frame */
+#define ISOTP_FC_CTS 0 /* clear to send */
+#define ISOTP_FC_WT 1 /* wait */
+#define ISOTP_FC_OVFLW 2 /* overflow */
+
+static kernel_pid_t isotp_pid = KERNEL_PID_UNDEF;
+static struct isotp *isotp_list = NULL;
+static mutex_t lock = MUTEX_INIT;
+
+#define MIN(a, b) (((a) < (b)) ? (a) : (b))
+
+static void _rx_timeout(void *arg);
+static int _isotp_send_fc(struct isotp *isotp, int ae, uint8_t status);
+static int _isotp_tx_send(struct isotp *isotp, struct can_frame *frame);
+
+static int _send_msg(msg_t *msg, can_reg_entry_t *entry)
+{
+#ifdef MODULE_CAN_MBOX
+ switch (entry->type) {
+ case CAN_TYPE_DEFAULT:
+ return msg_try_send(msg, entry->target.pid);
+ case CAN_TYPE_MBOX:
+ DEBUG("_send_msg: sending msg=%p to mbox=%p\n", (void *)msg, (void *)entry->target.mbox);
+ return mbox_try_put(entry->target.mbox, msg);
+ default:
+ return -ENOTSUP;
+ }
+#else
+ return msg_try_send(msg, entry->target.pid);
+#endif
+}
+
+static int _isotp_dispatch_rx(struct isotp *isotp)
+{
+ msg_t msg;
+ int ret = 0;
+ can_rx_data_t *data;
+
+ msg.type = CAN_MSG_RX_INDICATION;
+ data = can_pkt_alloc_rx_data(isotp->rx.snip,
+ isotp->rx.snip->size + sizeof(*isotp->rx.snip),
+ isotp->arg);
+
+ if (!data) {
+ return -ENOMEM;
+ }
+
+ msg.content.ptr = data;
+ if (_send_msg(&msg, &isotp->entry) < 1) {
+ DEBUG("_isotp_dispatch_rx: msg lost, freeing rx buf\n");
+ gnrc_pktbuf_release(((gnrc_pktsnip_t *)data->data.iov_base));
+ can_pkt_free_rx_data(data);
+ ret = -EOVERFLOW;
+ }
+
+ isotp->rx.snip = NULL;
+
+ return ret;
+}
+
+static int _isotp_dispatch_tx(struct isotp *isotp, int err)
+{
+ msg_t msg;
+
+ gnrc_pktbuf_release(isotp->tx.snip);
+ isotp->tx.snip = NULL;
+
+ if (isotp->opt.flags & CAN_ISOTP_TX_DONT_WAIT) {
+ return 0;
+ }
+
+ if (!err) {
+ msg.type = CAN_MSG_TX_CONFIRMATION;
+ }
+ else {
+ msg.type = CAN_MSG_TX_ERROR;
+ }
+
+ msg.content.ptr = isotp->arg;
+
+ if (_send_msg(&msg, &isotp->entry) < 1) {
+ DEBUG("_isotp_dispatch_tx: msg lost\n");
+ return -EOVERFLOW;
+ }
+
+ return 0;
+}
+
+static void _rx_timeout(void *arg)
+{
+ msg_t msg;
+
+ DEBUG("_rx_timeout: arg=%p\n", arg);
+
+ msg.type = CAN_MSG_ISOTP_RX_TIMEOUT;
+ msg.content.ptr = arg;
+
+ msg_send(&msg, isotp_pid);
+}
+
+static void _tx_timeout(void *arg)
+{
+ msg_t msg;
+
+ DEBUG("_tx_timeout: arg=%p\n", arg);
+
+ msg.type = CAN_MSG_ISOTP_TX_TIMEOUT;
+ msg.content.ptr = arg;
+
+ msg_send(&msg, isotp_pid);
+}
+
+static int _isotp_rcv_fc(struct isotp *isotp, struct can_frame *frame, int ae)
+{
+ if (isotp->tx.state != ISOTP_WAIT_FC) {
+ return 0;
+ }
+
+ xtimer_remove(&isotp->tx_timer);
+
+ if (frame->can_dlc < ae + FC_CONTENT_SZ) {
+ /* Invalid length */
+ isotp->tx.state = ISOTP_IDLE;
+ return 1;
+ }
+
+ isotp->txfc.bs = frame->data[ae + 1];
+ isotp->txfc.stmin = frame->data[ae + 2];
+
+ DEBUG("_isotp_rcv_fc: first FC: bs=0x%" PRIx8 ", stmin=0x%" PRIx8 "\n",
+ isotp->txfc.bs, isotp->txfc.stmin);
+
+ if ((isotp->txfc.stmin > 0x7F) &&
+ ((isotp->txfc.stmin < 0xF1) || (isotp->txfc.stmin > 0xF9))) {
+ /* according to ISO15765-2 8.5.5.6 */
+ isotp->txfc.stmin = 0x7F;
+ }
+ /* ISO15765-2 8.5.5.5 */
+ /* Range 0x0 - 0x7F -> 0 ms - 127 ms */
+ if (isotp->txfc.stmin < 0x80) {
+ isotp->tx_gap = isotp->txfc.stmin * US_PER_MS;
+ }
+ /* Range 0xF1 - 0xF9 -> 100 us - 900 us */
+ else {
+ isotp->tx_gap = (isotp->txfc.stmin - 0xF0) * 100;
+ }
+
+ switch (frame->data[ae] & 0xF) {
+ case ISOTP_FC_CTS:
+ isotp->tx_wft = 0;
+ isotp->tx.bs = 0;
+ isotp->tx.state = ISOTP_SENDING_NEXT_CF;
+ xtimer_set(&isotp->tx_timer, isotp->tx_gap);
+ break;
+
+ case ISOTP_FC_WT:
+ if (isotp->tx_wft++ >= isotp->txfc.wftmax) {
+ isotp->tx.state = ISOTP_IDLE;
+ _isotp_dispatch_tx(isotp, ETIMEDOUT);
+ return 1;
+ }
+ /* BS and STmin shall be ignored */
+ xtimer_set(&isotp->tx_timer, CAN_ISOTP_TIMEOUT_N_Bs);
+ break;
+
+ case ISOTP_FC_OVFLW:
+ /* overflow on receiver side -> error */
+
+ default:
+ isotp->tx.state = ISOTP_IDLE;
+ _isotp_dispatch_tx(isotp, EOVERFLOW);
+ break;
+ }
+
+ return 0;
+}
+
+static int _isotp_rcv_sf(struct isotp *isotp, struct can_frame *frame, int ae)
+{
+ xtimer_remove(&isotp->rx_timer);
+ isotp->rx.state = ISOTP_IDLE;
+
+ int len = (frame->data[ae] & 0x0F);
+ if (len > frame->can_dlc - (SF_PCI_SZ + ae)) {
+ return 1;
+ }
+
+ gnrc_pktsnip_t *snip = gnrc_pktbuf_add(NULL, NULL, len, GNRC_NETTYPE_UNDEF);
+ if (!snip) {
+ return 1;
+ }
+ isotp->rx.snip = snip;
+
+ isotp->rx.idx = 0;
+ for (size_t i = SF_PCI_SZ + ae; i < isotp->rx.snip->size + ae + SF_PCI_SZ; i++) {
+ ((uint8_t *)isotp->rx.snip->data)[isotp->rx.idx++] = frame->data[i];
+ }
+
+ return _isotp_dispatch_rx(isotp);
+}
+
+static int _isotp_rcv_ff(struct isotp *isotp, struct can_frame *frame, int ae)
+{
+ isotp->rx.state = ISOTP_IDLE;
+
+ int len = (frame->data[ae] & 0x0F) << 8;
+ len += frame->data[ae + 1];
+
+ if (isotp->rx.snip) {
+ DEBUG("_isotp_rcv_ff: freeing previous rx buf\n");
+ gnrc_pktbuf_release(isotp->rx.snip);
+ }
+
+ if (len > MAX_MSG_LENGTH) {
+ if (!(isotp->opt.flags & CAN_ISOTP_LISTEN_MODE)) {
+ _isotp_send_fc(isotp, ae, ISOTP_FC_OVFLW);
+ }
+ return 1;
+ }
+
+ gnrc_pktsnip_t *snip = gnrc_pktbuf_add(NULL, NULL, len, GNRC_NETTYPE_UNDEF);
+ if (!snip) {
+ if (!(isotp->opt.flags & CAN_ISOTP_LISTEN_MODE)) {
+ _isotp_send_fc(isotp, ae, ISOTP_FC_OVFLW);
+ }
+ return 1;
+ }
+ isotp->rx.snip = snip;
+
+ isotp->rx.idx = 0;
+ for (int i = ae + FF_PCI_SZ; i < frame->can_dlc; i++) {
+ ((uint8_t *)isotp->rx.snip->data)[isotp->rx.idx++] = frame->data[i];
+ }
+
+#if ENABLE_DEBUG
+ DEBUG("_isotp_rcv_ff: rx.buf=");
+ for (unsigned i = 0; i < isotp->rx.idx; i++) {
+ DEBUG("%02hhx", ((uint8_t *)isotp->rx.snip->data)[i]);
+ }
+ DEBUG("\n");
+#endif
+
+ isotp->rx.sn = 1;
+
+ if (isotp->opt.flags & CAN_ISOTP_LISTEN_MODE) {
+ isotp->rx.state = ISOTP_WAIT_CF;
+ return 0;
+ }
+
+ isotp->rx.state = ISOTP_SENDING_FC;
+ _isotp_send_fc(isotp, ae, ISOTP_FC_CTS);
+
+ return 0;
+}
+
+static int _isotp_rcv_cf(struct isotp *isotp, struct can_frame *frame, int ae)
+{
+ DEBUG("_isotp_rcv_cf: state=%d\n", isotp->rx.state);
+
+ if (isotp->rx.state != ISOTP_WAIT_CF) {
+ return 1;
+ }
+
+ xtimer_remove(&isotp->rx_timer);
+
+ if ((frame->data[ae] & 0x0F) != isotp->rx.sn) {
+ DEBUG("_isotp_rcv_cf: wrong seq number %d, expected %d\n", frame->data[ae] & 0x0F, isotp->rx.sn);
+ isotp->rx.state = ISOTP_IDLE;
+ gnrc_pktbuf_release(isotp->rx.snip);
+ isotp->rx.snip = NULL;
+ return 1;
+ }
+ isotp->rx.sn++;
+ isotp->rx.sn %= 16;
+
+ for (int i = ae + N_PCI_SZ; i < frame->can_dlc; i++) {
+ ((uint8_t *)isotp->rx.snip->data)[isotp->rx.idx++] = frame->data[i];
+ if (isotp->rx.idx >= isotp->rx.snip->size) {
+ break;
+ }
+ }
+
+#if ENABLE_DEBUG
+ DEBUG("_isotp_rcv_cf: rx.buf=");
+ for (unsigned i = 0; i < isotp->rx.idx; i++) {
+ DEBUG("%02hhx", ((uint8_t *)isotp->rx.snip->data)[i]);
+ }
+ DEBUG("\n");
+#endif
+
+ if (isotp->rx.idx >= isotp->rx.snip->size) {
+ isotp->rx.state = ISOTP_IDLE;
+ return _isotp_dispatch_rx(isotp);
+ }
+
+ if (isotp->opt.flags & CAN_ISOTP_LISTEN_MODE) {
+ return 0;
+ }
+
+ DEBUG("_isotp_rcv_cf: rxfc.bs=%" PRIx8 " rx.bs=%" PRIx8 "\n", isotp->rxfc.bs, isotp->rx.bs);
+
+ if (!isotp->rxfc.bs || (++isotp->rx.bs < isotp->rxfc.bs)) {
+ xtimer_set(&isotp->rx_timer, CAN_ISOTP_TIMEOUT_N_Cr);
+ return 0;
+ }
+
+ return _isotp_send_fc(isotp, ae, ISOTP_FC_CTS);
+}
+
+static int _isotp_rcv(struct isotp *isotp, struct can_frame *frame)
+{
+ int ae = (isotp->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
+ uint8_t n_pci_type;
+
+#if ENABLE_DEBUG
+ DEBUG("_isotp_rcv: id=%" PRIx32 " data=", frame->can_id);
+ for (int i = 0; i < frame->can_dlc; i++) {
+ DEBUG("%02hhx", frame->data[i]);
+ }
+ DEBUG("\n");
+#endif
+
+ if (ae && frame->data[0] != isotp->opt.rx_ext_address) {
+ return 1;
+ }
+
+ n_pci_type = frame->data[ae] & 0xF0;
+
+ switch (n_pci_type) {
+ case N_PCI_FC:
+ return _isotp_rcv_fc(isotp, frame, ae);
+
+ case N_PCI_SF:
+ return _isotp_rcv_sf(isotp, frame, ae);
+
+ case N_PCI_FF:
+ return _isotp_rcv_ff(isotp, frame, ae);
+
+ case N_PCI_CF:
+ return _isotp_rcv_cf(isotp, frame, ae);
+
+ }
+
+ return 1;
+}
+
+static int _isotp_send_fc(struct isotp *isotp, int ae, uint8_t status)
+{
+ struct can_frame fc;
+
+ fc.can_id = isotp->opt.tx_id;
+
+ if (isotp->opt.flags & CAN_ISOTP_TX_PADDING) {
+ memset(fc.data, isotp->opt.txpad_content, CAN_MAX_DLEN);
+ fc.can_dlc = CAN_MAX_DLEN;
+ }
+ else {
+ fc.can_dlc = ae + FC_CONTENT_SZ;
+ }
+
+ fc.data[ae] = N_PCI_FC | status;
+ fc.data[ae + 1] = isotp->rxfc.bs;
+ fc.data[ae + 2] = isotp->rxfc.stmin;
+
+ if (ae) {
+ fc.data[0] = isotp->opt.ext_address;
+ }
+
+ isotp->rx.bs = 0;
+
+#if ENABLE_DEBUG
+ DEBUG("_isotp_send_fc: id=%" PRIx32 " data=", fc.can_id);
+ for (int i = 0; i < fc.can_dlc; i++) {
+ DEBUG("%02hhx", fc.data[i]);
+ }
+ DEBUG("\n");
+#endif
+
+ xtimer_set(&isotp->rx_timer, CAN_ISOTP_TIMEOUT_N_Ar);
+ isotp->rx.tx_handle = raw_can_send(isotp->entry.ifnum, &fc, isotp_pid);
+
+ if (isotp->rx.tx_handle >= 0) {
+ return 0;
+ }
+ else {
+ isotp->rx.state = ISOTP_IDLE;
+ xtimer_remove(&isotp->rx_timer);
+ return isotp->rx.tx_handle;
+ }
+}
+
+static void _isotp_create_ff(struct isotp *isotp, struct can_frame *frame, int ae)
+{
+
+ frame->can_id = isotp->opt.tx_id;
+ frame->can_dlc = CAN_MAX_DLEN;
+
+ if (ae) {
+ frame->data[0] = isotp->opt.ext_address;
+ }
+
+ frame->data[ae] = (uint8_t)(isotp->tx.snip->size >> 8) | N_PCI_FF;
+ frame->data[ae + 1] = (uint8_t) isotp->tx.snip->size & 0xFFU;
+
+ for (int i = ae + FF_PCI_SZ; i < CAN_MAX_DLEN; i++) {
+ frame->data[i] = ((uint8_t *)isotp->tx.snip->data)[isotp->tx.idx++];
+ }
+
+ isotp->tx.sn = 1;
+}
+
+static void _isotp_fill_dataframe(struct isotp *isotp, struct can_frame *frame, int ae)
+{
+ size_t pci_len = N_PCI_SZ + ae;
+ size_t space = CAN_MAX_DLEN - pci_len;
+ size_t num_bytes = MIN(space, isotp->tx.snip->size - isotp->tx.idx);
+
+ frame->can_id = isotp->opt.tx_id;
+ frame->can_dlc = num_bytes + pci_len;
+
+ DEBUG("_isotp_fill_dataframe: num_bytes=%d, pci_len=%d\n", num_bytes, pci_len);
+
+ if (num_bytes < space) {
+ if (isotp->opt.flags & CAN_ISOTP_TX_PADDING) {
+ frame->can_dlc = CAN_MAX_DLEN;
+ memset(frame->data, isotp->opt.txpad_content, frame->can_dlc);
+ }
+ }
+
+ for (size_t i = 0; i < num_bytes; i++) {
+ frame->data[pci_len + i] = ((uint8_t *)isotp->tx.snip->data)[isotp->tx.idx++];
+ }
+
+ if (ae) {
+ frame->data[0] = isotp->opt.ext_address;
+ }
+
+}
+
+static void _isotp_tx_timeout_task(struct isotp *isotp)
+{
+ int ae = (isotp->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
+ struct can_frame frame;
+
+ DEBUG("_isotp_tx_timeout_task: state=%d\n", isotp->tx.state);
+
+ switch (isotp->tx.state) {
+ case ISOTP_WAIT_FC:
+ DEBUG("_isotp_tx_timeout_task: FC not received on time\n");
+ isotp->tx.state = ISOTP_IDLE;
+ _isotp_dispatch_tx(isotp, ETIMEDOUT);
+ break;
+
+ case ISOTP_SENDING_NEXT_CF:
+ DEBUG("_isotp_tx_timeout_task: sending next CF\n");
+ _isotp_fill_dataframe(isotp, &frame, ae);
+ frame.data[ae] = N_PCI_CF | isotp->tx.sn++;
+ isotp->tx.sn %= 16;
+ isotp->tx.bs++;
+
+ isotp->tx.state = ISOTP_SENDING_CF;
+ _isotp_tx_send(isotp, &frame);
+ break;
+
+ case ISOTP_SENDING_CF:
+ case ISOTP_SENDING_FF:
+ case ISOTP_SENDING_SF:
+ DEBUG("_isotp_tx_timeout_task: timeout on DLL\n");
+ isotp->tx.state = ISOTP_IDLE;
+ raw_can_abort(isotp->entry.ifnum, isotp->tx.tx_handle);
+ _isotp_dispatch_tx(isotp, ETIMEDOUT);
+ break;
+ }
+}
+
+static void _isotp_tx_tx_conf(struct isotp *isotp)
+{
+ xtimer_remove(&isotp->tx_timer);
+ isotp->tx.tx_handle = 0;
+
+ DEBUG("_isotp_tx_tx_conf: state=%d\n", isotp->tx.state);
+
+ switch (isotp->tx.state) {
+ case ISOTP_SENDING_SF:
+ isotp->tx.state = ISOTP_IDLE;
+ _isotp_dispatch_tx(isotp, 0);
+ break;
+
+ case ISOTP_SENDING_FF:
+ isotp->tx.state = ISOTP_WAIT_FC;
+ xtimer_set(&isotp->tx_timer, CAN_ISOTP_TIMEOUT_N_Bs);
+ break;
+
+ case ISOTP_SENDING_CF:
+ if (isotp->tx.idx >= isotp->tx.snip->size) {
+ /* Finished */
+ isotp->tx.state = ISOTP_IDLE;
+ _isotp_dispatch_tx(isotp, 0);
+ break;
+ }
+
+ if (isotp->txfc.bs && (isotp->tx.bs >= isotp->txfc.bs)) {
+ /* wait for FC */
+ isotp->tx.state = ISOTP_WAIT_FC;
+ xtimer_set(&isotp->tx_timer, CAN_ISOTP_TIMEOUT_N_Bs);
+ break;
+ }
+
+ isotp->tx.state = ISOTP_SENDING_NEXT_CF;
+ xtimer_set(&isotp->tx_timer, isotp->tx_gap);
+ break;
+ }
+}
+
+static void _isotp_rx_timeout_task(struct isotp *isotp)
+{
+ switch (isotp->rx.state) {
+ case ISOTP_SENDING_FC:
+ DEBUG("_isotp_rx_timeout_task: FC tx conf timeout\n");
+ raw_can_abort(isotp->entry.ifnum, isotp->rx.tx_handle);
+ case ISOTP_WAIT_CF:
+ DEBUG("_isotp_rx_timeout_task: free rx buf\n");
+ gnrc_pktbuf_release(isotp->rx.snip);
+ isotp->rx.snip = NULL;
+ isotp->rx.state = ISOTP_IDLE;
+ /* TODO dispatch rx error ? */
+ break;
+ }
+}
+
+static void _isotp_rx_tx_conf(struct isotp *isotp)
+{
+ xtimer_remove(&isotp->rx_timer);
+ isotp->rx.tx_handle = 0;
+
+ DEBUG("_isotp_rx_tx_conf: state=%d\n", isotp->rx.state);
+
+ switch (isotp->rx.state) {
+ case ISOTP_SENDING_FC:
+ isotp->rx.state = ISOTP_WAIT_CF;
+ xtimer_set(&isotp->rx_timer, CAN_ISOTP_TIMEOUT_N_Cr);
+ break;
+ }
+}
+
+static int _isotp_tx_send(struct isotp *isotp, struct can_frame *frame)
+{
+ xtimer_set(&isotp->tx_timer, CAN_ISOTP_TIMEOUT_N_As);
+ isotp->tx.tx_handle = raw_can_send(isotp->entry.ifnum, frame, isotp_pid);
+ DEBUG("isotp_send: FF/SF/CF sent handle=%d\n", isotp->tx.tx_handle);
+ if (isotp->tx.tx_handle < 0) {
+ xtimer_remove(&isotp->tx_timer);
+ isotp->tx.state = ISOTP_IDLE;
+ return _isotp_dispatch_tx(isotp, isotp->tx.tx_handle);
+ }
+
+ return 0;
+}
+
+static int _isotp_send_sf_ff(struct isotp *isotp)
+{
+ struct can_frame frame;
+ unsigned ae = (isotp->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
+
+ if (isotp->tx.snip->size <= CAN_MAX_DLEN - SF_PCI_SZ - ae) {
+ /* Fits into a single frame */
+ _isotp_fill_dataframe(isotp, &frame, ae);
+
+ frame.data[ae] = N_PCI_SF;
+ frame.data[ae] |= isotp->tx.snip->size;
+
+ isotp->tx.state = ISOTP_SENDING_SF;
+ }
+ else {
+ isotp->tx.state = ISOTP_SENDING_FF;
+ /* Must send a First frame */
+ _isotp_create_ff(isotp, &frame, ae);
+ }
+
+ return _isotp_tx_send(isotp, &frame);
+}
+
+static void *_isotp_thread(void *args)
+{
+ (void)args;
+ msg_t msg, msg_queue[CAN_ISOTP_MSG_QUEUE_SIZE];
+ struct can_rx_data *rx_frame;
+ struct isotp *isotp;
+
+ /* setup the device layers message queue */
+ msg_init_queue(msg_queue, CAN_ISOTP_MSG_QUEUE_SIZE);
+
+ isotp_pid = sched_active_pid;
+
+ while (1) {
+ msg_receive(&msg);
+ switch (msg.type) {
+ case CAN_MSG_SEND_FRAME:
+ _isotp_send_sf_ff(msg.content.ptr);
+ break;
+ case CAN_MSG_RX_INDICATION:
+ rx_frame = msg.content.ptr;
+ if (!rx_frame) {
+ DEBUG("_isotp_thread: CAN_MSG_RX_INDICATION with NULL ptr\n");
+ break;
+ }
+ DEBUG("_isotp_thread: CAN_MSG_RX_INDICATION, frame=%p, data=%p\n",
+ (void *)rx_frame->data.iov_base, rx_frame->arg);
+ _isotp_rcv((struct isotp *)rx_frame->arg, rx_frame->data.iov_base);
+ raw_can_free_frame(rx_frame);
+ break;
+ case CAN_MSG_TX_CONFIRMATION:
+ DEBUG("_isotp_thread: CAN_MSG_TX_CONFIRMATION, handle=%d\n", (int)msg.content.value);
+ mutex_lock(&lock);
+ LL_FOREACH(isotp_list, isotp) {
+ if (isotp->tx.tx_handle == (int)msg.content.value) {
+ mutex_unlock(&lock);
+ _isotp_tx_tx_conf(isotp);
+ break;
+ }
+ else if (isotp->rx.tx_handle == (int)msg.content.value) {
+ mutex_unlock(&lock);
+ _isotp_rx_tx_conf(isotp);
+ break;
+ }
+ }
+ if (isotp == NULL) {
+ mutex_unlock(&lock);
+ }
+ break;
+ case CAN_MSG_ISOTP_RX_TIMEOUT:
+ isotp = msg.content.ptr;
+ DEBUG("_isotp_thread: RX TIMEOUT arg=%p\n", (void *)isotp);
+ _isotp_rx_timeout_task(isotp);
+ break;
+ case CAN_MSG_ISOTP_TX_TIMEOUT:
+ isotp = msg.content.ptr;
+ DEBUG("_isotp_thread: TX_TIMEOUT arg=%p\n", (void *)isotp);
+ _isotp_tx_timeout_task(isotp);
+ break;
+ }
+ }
+
+ return NULL;
+}
+
+kernel_pid_t isotp_init(char *stack, int stacksize, char priority, const char *name)
+{
+ kernel_pid_t res;
+
+ DEBUG("isotp_init\n");
+
+ /* create new can device thread */
+ res = thread_create(stack, stacksize, priority, THREAD_CREATE_STACKTEST,
+ _isotp_thread, NULL, name);
+ if (res <= 0) {
+ return -EINVAL;
+ }
+
+ return res;
+}
+
+int isotp_send(struct isotp *isotp, const void *buf, int len, int flags)
+{
+ assert(isotp != NULL);
+#ifdef MODULE_CAN_MBOX
+ assert((isotp->entry.type == CAN_TYPE_DEFAULT && pid_is_valid(isotp->entry.target.pid)) ||
+ (isotp->entry.type == CAN_TYPE_MBOX && isotp->entry.target.mbox != NULL));
+#else
+ assert(isotp->entry.target.pid != KERNEL_PID_UNDEF);
+#endif
+ assert (len && len <= MAX_MSG_LENGTH);
+
+ if (isotp->tx.state != ISOTP_IDLE) {
+ return -EBUSY;
+ }
+
+ if (flags) {
+ isotp->opt.flags &= CAN_ISOTP_RX_FLAGS_MASK;
+ isotp->opt.flags |= (flags & CAN_ISOTP_TX_FLAGS_MASK);
+ }
+
+ gnrc_pktsnip_t *snip = gnrc_pktbuf_add(NULL, NULL, len, GNRC_NETTYPE_UNDEF);
+ if (!snip) {
+ return -ENOMEM;
+ }
+ isotp->tx.snip = snip;
+
+ memcpy(isotp->tx.snip->data, buf, len);
+
+ isotp->tx.idx = 0;
+
+ isotp->tx_wft = 0;
+
+ msg_t msg;
+ msg.type = CAN_MSG_SEND_FRAME;
+ msg.content.ptr = isotp;
+ msg_send(&msg, isotp_pid);
+
+ return len;
+}
+
+int isotp_bind(struct isotp *isotp, can_reg_entry_t *entry, void *arg)
+{
+ int ret;
+
+ assert(isotp != NULL);
+#ifdef MODULE_CAN_MBOX
+ assert((entry->type == CAN_TYPE_DEFAULT && pid_is_valid(entry->target.pid)) ||
+ (entry->type == CAN_TYPE_MBOX && entry->target.mbox != NULL));
+#else
+ assert(pid_is_valid(entry->target.pid));
+#endif
+ assert(isotp->opt.tx_id != isotp->opt.rx_id);
+ assert(!((isotp->opt.tx_id | isotp->opt.rx_id) & (CAN_RTR_FLAG | CAN_ERR_FLAG)));
+ assert(entry->ifnum < CAN_DLL_NUMOF);
+
+ isotp->rx_timer.callback = _rx_timeout;
+ isotp->rx_timer.arg = isotp;
+
+ isotp->tx_timer.callback = _tx_timeout;
+ isotp->tx_timer.arg = isotp;
+
+ memset(&isotp->rx, 0, sizeof(struct tpcon));
+ memset(&isotp->tx, 0, sizeof(struct tpcon));
+
+ isotp->rxfc.bs = CAN_ISOTP_BS;
+ isotp->rxfc.stmin = CAN_ISOTP_STMIN;
+ isotp->rxfc.wftmax = 0;
+
+ isotp->txfc.bs = 0;
+ isotp->txfc.stmin = 0;
+ isotp->txfc.wftmax = CAN_ISOTP_WFTMAX;
+
+ isotp->entry.ifnum = entry->ifnum;
+#ifdef MODULE_CAN_MBOX
+ isotp->entry.type = entry->type;
+ isotp->entry.target.mbox = entry->target.mbox;
+#else
+ isotp->entry.target.pid = entry->target.pid;
+#endif
+ isotp->arg = arg;
+ isotp->next = NULL;
+
+ DEBUG("isotp_bind: ifnum=%d, txid=%" PRIx32 ", rxid=%" PRIx32 ", flags=0x%" PRIx16 "\n",
+ isotp->entry.ifnum, isotp->opt.tx_id, isotp->opt.rx_id, isotp->opt.flags);
+ DEBUG("isotp_bind: pid=%" PRIkernel_pid "\n", entry->target.pid);
+
+ struct can_filter filter = {
+ .can_id = isotp->opt.rx_id,
+ .can_mask = 0xFFFFFFFF,
+ };
+ ret = raw_can_subscribe_rx(isotp->entry.ifnum, &filter, isotp_pid, isotp);
+ if (ret < 0) {
+ return ret;
+ }
+
+ mutex_lock(&lock);
+ LL_APPEND(isotp_list, isotp);
+ mutex_unlock(&lock);
+
+ return 0;
+}
+
+void isotp_free_rx(can_rx_data_t *rx)
+{
+ DEBUG("isotp_free_rx: rx=%p\n", (void *)rx);
+ gnrc_pktbuf_release(rx->data.iov_base);
+ can_pkt_free_rx_data(rx);
+}
+
+int isotp_release(struct isotp *isotp)
+{
+ assert(isotp != NULL);
+#ifdef MODULE_CAN_MBOX
+ assert((isotp->entry.type == CAN_TYPE_DEFAULT && pid_is_valid(isotp->entry.target.pid)) ||
+ (isotp->entry.type == CAN_TYPE_MBOX && isotp->entry.target.mbox != NULL));
+#else
+ assert(isotp->entry.target.pid != KERNEL_PID_UNDEF);
+#endif
+
+ DEBUG("isotp_release: isotp=%p\n", (void *)isotp);
+
+ struct can_filter filter = {
+ .can_id = isotp->opt.rx_id,
+ .can_mask = 0xFFFFFFFF,
+ };
+ raw_can_unsubscribe_rx(isotp->entry.ifnum, &filter, isotp_pid, isotp);
+
+ if (isotp->rx.snip) {
+ DEBUG("isotp_release: freeing rx buf\n");
+ gnrc_pktbuf_release(isotp->rx.snip);
+ isotp->rx.snip = NULL;
+ }
+ isotp->rx.state = ISOTP_IDLE;
+ isotp->entry.target.pid = KERNEL_PID_UNDEF;
+
+ mutex_lock(&lock);
+ LL_DELETE(isotp_list, isotp);
+ mutex_unlock(&lock);
+
+ if (isotp->tx.snip) {
+ DEBUG("isotp_release: freeing rx buf\n");
+ gnrc_pktbuf_release(isotp->tx.snip);
+ isotp->tx.snip = NULL;
+ }
+ isotp->tx.state = ISOTP_IDLE;
+
+ return 0;
+}
diff --git a/sys/include/can/isotp.h b/sys/include/can/isotp.h
new file mode 100644
index 0000000000..2fe456385e
--- /dev/null
+++ b/sys/include/can/isotp.h
@@ -0,0 +1,196 @@
+/*
+ * Copyright (C) 2016 OTA keys S.A.
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+/**
+ * @ingroup can
+ * @defgroup isotp ISOTP
+ * @brief ISO transport protocol over CAN (ISO15765)
+ * @{
+ *
+ *
+ * @file
+ * @brief ISO TP high level interface
+ *
+ * @author Vincent Dupont <vincent@otakeys.com>
+ */
+
+#ifndef CAN_ISOTP_H
+#define CAN_ISOTP_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "can/can.h"
+#include "can/common.h"
+#include "thread.h"
+#include "xtimer.h"
+#include "net/gnrc/pktbuf.h"
+
+
+/**
+ * @brief The isotp_fc_options struct
+ *
+ * It describes the flow control options
+ */
+struct isotp_fc_options {
+ uint8_t bs; /**< blocksize provided in FC frame, 0 = off */
+
+ /** separation time provided in FC frame
+ * 0x00 - 0x7F : 0 - 127 ms
+ * 0x80 - 0xF0 : reserved
+ * 0xF1 - 0xF9 : 100 us - 900 us
+ * 0xFA - 0xFF : reserved */
+ uint8_t stmin;
+
+ uint8_t wftmax; /**< max. number of wait frame transmiss., 0 = ignored */
+};
+
+/**
+ * @brief The isotp_options struct
+ *
+ * It describes the ISO-TP options
+ */
+struct isotp_options {
+ canid_t tx_id; /**< transmit CAN ID */
+ canid_t rx_id; /**< Receive CAN ID */
+ uint16_t flags; /**< set flags for isotp behaviour. */
+ uint8_t ext_address; /**< set address for extended addressing */
+ uint8_t txpad_content; /**< set content of padding byte (tx) */
+ uint8_t rx_ext_address; /**< set address for extended addressing */
+};
+
+/**
+ * @brief The tpcon struct
+ *
+ * It describes the current connection status
+ */
+struct tpcon {
+ unsigned idx; /**< current index in @p buf */
+ uint8_t state; /**< the protocol state */
+ uint8_t bs; /**< block size */
+ uint8_t sn; /**< current sequence number */
+ int tx_handle; /**< handle of the last sent frame */
+ gnrc_pktsnip_t *snip; /**< allocated snip containing data buffer */
+};
+
+/**
+ * @brief The isotp struct
+ *
+ * This is the main struct used by an ISO-TP channel
+ */
+struct isotp {
+ struct isotp *next; /**< next bound channel */
+ struct isotp_options opt; /**< channel options */
+ struct isotp_fc_options rxfc; /**< rx flow control options (defined locally) */
+ struct isotp_fc_options txfc; /**< tx flow control options (defined remotely) */
+ struct tpcon tx; /**< transmit state */
+ struct tpcon rx; /**< receive state */
+ xtimer_t tx_timer; /**< timer for tx operations */
+ xtimer_t rx_timer; /**< timer for rx operations */
+ can_reg_entry_t entry; /**< entry containing ifnum and upper layer msg system */
+ uint32_t tx_gap; /**< transmit gap from fc (in us) */
+ uint8_t tx_wft; /**< transmit wait counter */
+ void *arg; /**< upper layer private arg */
+};
+
+/**
+ * @name flags for isotp behaviour
+ * @{
+ */
+#define CAN_ISOTP_RX_FLAGS_MASK 0x0000FFFF /**< rx flags mask */
+#define CAN_ISOTP_LISTEN_MODE 0x0001 /**< listen only flag (do not send FC) */
+#define CAN_ISOTP_EXTEND_ADDR 0x0002 /**< enable extended addressing */
+#define CAN_ISOTP_TX_PADDING 0x0004 /**< enable CAN frame padding tx path */
+#define CAN_ISOTP_HALF_DUPLEX 0x0040 /**< half duplex error state handling */
+#define CAN_ISOTP_RX_EXT_ADDR 0x0200 /**< different rx extended addressing */
+
+#define CAN_ISOTP_TX_FLAGS_MASK 0xFFFF0000 /**< tx flags mask */
+#define CAN_ISOTP_TX_DONT_WAIT 0x00010000 /**< do not send a tx confirmation msg */
+/** @} */
+
+/**
+ * @name default configuration values
+ * @{
+ */
+#define CAN_ISOTP_DEFAULT_FLAGS 0
+#define CAN_ISOTP_DEFAULT_EXT_ADDRESS 0x00
+#define CAN_ISOTP_DEFAULT_PAD_CONTENT 0xCC /* prevent bit-stuffing */
+#define CAN_ISOTP_DEFAULT_FRAME_TXTIME 0
+#define CAN_ISOTP_DEFAULT_RECV_BS 0
+#define CAN_ISOTP_DEFAULT_RECV_STMIN 0x00
+#define CAN_ISOTP_DEFAULT_RECV_WFTMAX 0
+/** @} */
+
+/**
+ * @brief Initialize the isotp layer
+ *
+ * @param stack stack for the isotp thread
+ * @param stacksize size of @p stack
+ * @param priority priority of the isotp thread
+ * @param name name of the isotp thread
+ *
+ * @return the pid of the isotp thread
+ */
+kernel_pid_t isotp_init(char *stack, int stacksize, char priority, const char *name);
+
+/**
+ * @brief Send data through an isotp channel
+ *
+ * @param isotp the channel to use
+ * @param buf the data to send
+ * @param len length of the data to send
+ * @param flags flags for sending
+ *
+ * @return the number of bytes sent
+ * @return < 0 if an error occured (-EBUSY, -ENOMEM)
+ */
+int isotp_send(struct isotp *isotp, const void *buf, int len, int flags);
+
+/**
+ * @brief Bind an isotp channel
+ *
+ * Initialize the channel, set the filter on the DLL and add the
+ * channel to the list of bound channels
+ *
+ * @param isotp the channel to bind
+ * @param entry entry identifying the CAN ifnum and the upper layer
+ * either by its pid or its mailbox
+ * @param arg upper layer private parameter
+ *
+ * @return 0 on success, < 0 on error
+ */
+int isotp_bind(struct isotp *isotp, can_reg_entry_t *entry, void *arg);
+
+/**
+ * @brief Release a bound isotp channel
+ *
+ * Unset the filter on the DLL and remove the channel from the list
+ * of bound channels
+ *
+ * @param isotp the channel to relase
+ *
+ * @return 0 on success, < 0 on error
+ */
+int isotp_release(struct isotp *isotp);
+
+/**
+ * @brief Free a received buffer
+ *
+ * This MUST be called by the upper layer when the received data are read
+ *
+ * @param rx the received data
+ */
+void isotp_free_rx(can_rx_data_t *rx);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CAN_ISOTP_H */
+/** @} */
--
GitLab