/*
   * 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     sys_can_isotp
   * @{
   * @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;
  }