diff --git a/Makefile.dep b/Makefile.dep index 267a121c8da0..d4706ae62024 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 e94cddbb8094..c5f9dfa1fcbe 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 48422e909a47..449d4878caca 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 000000000000..9f32f4c6e47c --- /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 000000000000..8ee8c72b2d17 --- /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 + */ + +#include +#include + +#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 000000000000..2fe456385ea7 --- /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 + */ + +#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 */ +/** @} */