text: /*
   * Helper functions for implementing CANopen threads in Linux
   *
   * @file        Linux_threads.c
   * @author      Janez Paternoster
   * @author      Martin Wagner
   * @copyright   2004 - 2015 Janez Paternoster
   * @copyright   2018 - 2020 Neuberger Gebaeudeautomation GmbH
   *
   *
   * This file is part of CANopenNode, an opensource CANopen Stack.
   * Project home page is <https://github.com/CANopenNode/CANopenNode>.
   * For more information on CANopen see <http://www.can-cia.org/>.
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   *     http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  
  /* following macro is necessary for accept4() function call (sockets) */
  #define _GNU_SOURCE
  
  #include "CANopen.h"
  #include "CO_Linux_threads.h"
  
  #include <stdlib.h>
  #include <unistd.h>
  #include <syslog.h>
  #include <time.h>
  #include <sys/epoll.h>
  #include <sys/eventfd.h>
  #include <sys/timerfd.h>
  #include <fcntl.h>
  #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
  #include <stdio.h>
  #include <ctype.h>
  #include <limits.h>
  #include <sys/socket.h>
  #include <sys/un.h>
  #include <netinet/in.h>
  #endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
  
  
  #ifndef LISTEN_BACKLOG
  #define LISTEN_BACKLOG 50
  #endif
  
  
  /* Helper function - get monotonic clock time in microseconds */
  static inline uint64_t CO_LinuxThreads_clock_gettime_us(void)
  {
      struct timespec ts;
  
      (void)clock_gettime(CLOCK_MONOTONIC, &ts);
      return ts.tv_sec * 1000000 + ts.tv_nsec / 1000;
  }
  
  
  #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
  /* write response string from gateway-ascii object */
  static size_t gtwa_write_response(void *object, const char *buf, size_t count) {
      int* fd = (int *)object;
      /* nWritten = count -> in case of error (non-existing fd) data are purged */
      size_t nWritten = count;
  
      if (fd != NULL && *fd >= 0) {
          ssize_t n = write(*fd, (const void *)buf, count);
          if (n >= 0) {
              nWritten = (size_t)n;
          }
          else {
              log_printf(LOG_DEBUG, DBG_ERRNO, "write(gtwa_response)");
          }
      }
      return nWritten;
  }
  #endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
  
  
  /* Mainline thread - Blocking (threadMainWait) ********************************/
  static struct {
      uint64_t start;   /* time value CO_process() was called last time in us */
      int epoll_fd;         /* epoll file descriptor */
      int event_fd;         /* notification event file descriptor */
      int timer_fd;         /* interval timer file descriptor */
      uint32_t interval_us; /* interval for threadMainWait_process */
      struct itimerspec tm; /* structure for timer configuration */
  #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
      int32_t commandInterface; /* command interface type or tcp port */
      uint32_t socketTimeout_us;
      uint32_t socketTimeoutTmr_us;
      char *localSocketPath;/* path in case of local socket */
      int gtwa_fdSocket;    /* gateway socket file descriptor */
      int gtwa_fd;          /* gateway io stream file descriptor */
      bool_t freshCommand;
  #endif
  } tmw;
  
  static void threadMainWait_callback(void *object)
  {
      (void)object;
      /* send event to wake threadMainWait_process() */
      uint64_t u = 1;
      ssize_t s;
      s = write(tmw.event_fd, &u, sizeof(uint64_t));
      if (s != sizeof(uint64_t)) {
          log_printf(LOG_DEBUG, DBG_ERRNO, "write()");
      }
  }
  
  void threadMainWait_init(bool_t CANopenConfiguredOK)
  {
      /* Configure LSS slave callback function */
  #if (CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE
      CO_LSSslave_initCallbackPre(CO->LSSslave, NULL, threadMainWait_callback);
  #endif
  
      /* Initial value for time calculation */
      tmw.start = CO_LinuxThreads_clock_gettime_us();
  
      if (!CANopenConfiguredOK)
          return;
  
      /* Configure callback functions */
      CO_NMT_initCallbackPre(CO->NMT, NULL, threadMainWait_callback);
      CO_SDO_initCallbackPre(CO->SDO[0], NULL, threadMainWait_callback);
      CO_EM_initCallbackPre(CO->em, NULL, threadMainWait_callback);
      CO_HBconsumer_initCallbackPre(CO->HBcons, NULL, threadMainWait_callback);
  #if CO_NO_SDO_CLIENT != 0
      CO_SDOclient_initCallbackPre(CO->SDOclient[0], NULL,
                                   threadMainWait_callback);
  #endif
  #if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE
      CO_TIME_initCallbackPre(CO->TIME, NULL, threadMainWait_callback);
  #endif
  #if (CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE
  #if (CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER
      CO_LSSmaster_initCallbackPre(CO->LSSmaster, NULL, threadMainWait_callback);
  #endif
  #endif
  
  #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
      CO_GTWA_initRead(CO->gtwa, gtwa_write_response, (void *)&tmw.gtwa_fd);
      tmw.freshCommand = true;
  #endif
  }
  
  void threadMainWait_initOnce(uint32_t interval_us,
                               int32_t commandInterface,
                               uint32_t socketTimeout_ms,
                               char *localSocketPath)
  {
      int ret;
      struct epoll_event ev;
  
      /* Configure epoll for mainline */
      tmw.epoll_fd = epoll_create(1);
      if (tmw.epoll_fd < 0) {
          log_printf(LOG_CRIT, DBG_ERRNO, "epoll_create()");
          exit(EXIT_FAILURE);
      }
  
      /* Configure eventfd for notifications and add it to epoll */
      tmw.event_fd = eventfd(0, EFD_NONBLOCK);
      if (tmw.event_fd < 0) {
          log_printf(LOG_CRIT, DBG_ERRNO, "eventfd()");
          exit(EXIT_FAILURE);
      }
      ev.events = EPOLLIN;
      ev.data.fd = tmw.event_fd;
      ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
      if (ret < 0) {
          log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(event_fd)");
          exit(EXIT_FAILURE);
      }
  
      /* Configure timer for interval_us and add it to epoll */
      tmw.timer_fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK);
      if (tmw.timer_fd < 0) {
          log_printf(LOG_CRIT, DBG_ERRNO, "timerfd_create()");
          exit(EXIT_FAILURE);
      }
      tmw.interval_us = interval_us;
      tmw.tm.it_interval.tv_sec = interval_us / 1000000;
      tmw.tm.it_interval.tv_nsec = (interval_us % 1000000) * 1000;
      tmw.tm.it_value.tv_sec = 0;
      tmw.tm.it_value.tv_nsec = 1;
      ret = timerfd_settime(tmw.timer_fd, 0, &tmw.tm, NULL);
      if (ret < 0) {
          log_printf(LOG_CRIT, DBG_ERRNO, "timerfd_settime");
          exit(EXIT_FAILURE);
      }
      ev.events = EPOLLIN;
      ev.data.fd = tmw.timer_fd;
      ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
      if (ret < 0) {
          log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(timer_fd)");
          exit(EXIT_FAILURE);
      }
  
  #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
      /* Configure gateway-ascii command interface (CiA309-3) */
      tmw.commandInterface = commandInterface;
      tmw.socketTimeout_us = (socketTimeout_ms < (UINT_MAX / 1000 - 1000000)) ?
                             socketTimeout_ms * 1000 : (UINT_MAX - 1000000);
      tmw.gtwa_fdSocket = -1;
      tmw.gtwa_fd = -1;
  
      if (commandInterface == CO_COMMAND_IF_STDIO) {
          tmw.gtwa_fd = STDIN_FILENO;
          log_printf(LOG_INFO, DBG_COMMAND_STDIO_INFO);
      }
      else if (commandInterface == CO_COMMAND_IF_LOCAL_SOCKET) {
          struct sockaddr_un addr;
          tmw.localSocketPath = localSocketPath;
  
          /* Create, bind and listen local socket */
          tmw.gtwa_fdSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
          if(tmw.gtwa_fdSocket < 0) {
              log_printf(LOG_CRIT, DBG_ERRNO, "socket(local)");
              exit(EXIT_FAILURE);
          }
  
          memset(&addr, 0, sizeof(struct sockaddr_un));
          addr.sun_family = AF_UNIX;
          strncpy(addr.sun_path, localSocketPath, sizeof(addr.sun_path) - 1);
          ret = bind(tmw.gtwa_fdSocket, (struct sockaddr *) &addr,
                     sizeof(struct sockaddr_un));
          if(ret < 0) {
              log_printf(LOG_CRIT, DBG_COMMAND_LOCAL_BIND, localSocketPath);
              exit(EXIT_FAILURE);
          }
  
          ret = listen(tmw.gtwa_fdSocket, LISTEN_BACKLOG);
          if(ret < 0) {
              log_printf(LOG_CRIT, DBG_ERRNO, "listen(local)");
              exit(EXIT_FAILURE);
          }
  
          log_printf(LOG_INFO, DBG_COMMAND_LOCAL_INFO, localSocketPath);
      }
      else if (commandInterface >= CO_COMMAND_IF_TCP_SOCKET_MIN &&
               commandInterface <= CO_COMMAND_IF_TCP_SOCKET_MAX
      ) {
          struct sockaddr_in addr;
          const int yes = 1;
  
          /* Create, bind and listen socket */
          tmw.gtwa_fdSocket = socket(AF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0);
          if(tmw.gtwa_fdSocket < 0) {
              log_printf(LOG_CRIT, DBG_ERRNO, "socket(tcp)");
              exit(EXIT_FAILURE);
          }
  
          setsockopt(tmw.gtwa_fdSocket, SOL_SOCKET, SO_REUSEADDR,
                     &yes, sizeof(int));
  
          memset(&addr, 0, sizeof(struct sockaddr_in));
          addr.sin_family = AF_INET;
          addr.sin_port = htons(commandInterface);
          addr.sin_addr.s_addr = INADDR_ANY;
  
          ret = bind(tmw.gtwa_fdSocket, (struct sockaddr *) &addr,
                     sizeof(struct sockaddr_in));
          if(ret < 0) {
              log_printf(LOG_CRIT, DBG_COMMAND_TCP_BIND, commandInterface);
              exit(EXIT_FAILURE);
          }
  
          ret = listen(tmw.gtwa_fdSocket, LISTEN_BACKLOG);
          if(ret < 0) {
              log_printf(LOG_CRIT, DBG_ERRNO, "listen(tcp)");
              exit(EXIT_FAILURE);
          }
  
          log_printf(LOG_INFO, DBG_COMMAND_TCP_INFO, commandInterface);
      }
      else {
          tmw.commandInterface = CO_COMMAND_IF_DISABLED;
      }
  
      if (tmw.gtwa_fd >= 0) {
          ev.events = EPOLLIN;
          ev.data.fd = tmw.gtwa_fd;
          ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
          if (ret < 0) {
              log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(gtwa_fd)");
              exit(EXIT_FAILURE);
          }
      }
      if (tmw.gtwa_fdSocket >= 0) {
          /* prepare epool for listening for new socket connection. After
           * connection will be accepted, fd for io operation will be defined. */
          ev.events = EPOLLIN | EPOLLONESHOT;
          ev.data.fd = tmw.gtwa_fdSocket;
          ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_ADD, ev.data.fd, &ev);
          if (ret < 0) {
              log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(gtwa_fdSocket)");
              exit(EXIT_FAILURE);
          }
      }
  #endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
  }
  
  void threadMainWait_close(void)
  {
      close(tmw.epoll_fd);
      tmw.epoll_fd = -1;
  
  #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
      if (tmw.commandInterface == CO_COMMAND_IF_LOCAL_SOCKET) {
          if (tmw.gtwa_fd > 0) {
              close(tmw.gtwa_fd);
          }
          close(tmw.gtwa_fdSocket);
          /* Remove local socket file from filesystem. */
          if(remove(tmw.localSocketPath) < 0) {
              log_printf(LOG_CRIT, DBG_ERRNO, "remove(local)");
          }
      }
      else if (tmw.commandInterface >= CO_COMMAND_IF_TCP_SOCKET_MIN) {
          if (tmw.gtwa_fd > 0) {
              close(tmw.gtwa_fd);
          }
          close(tmw.gtwa_fdSocket);
      }
      tmw.gtwa_fd = -1;
      tmw.gtwa_fdSocket = -1;
  #endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
  
      close(tmw.event_fd);
      tmw.event_fd = -1;
  
      close(tmw.timer_fd);
      tmw.timer_fd = -1;
  }
  
  #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
  static inline void socetAcceptEnableForEpoll(void) {
      struct epoll_event ev;
      int ret;
  
      ev.events = EPOLLIN | EPOLLONESHOT;
      ev.data.fd = tmw.gtwa_fdSocket;
      ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_MOD, ev.data.fd, &ev);
      if (ret < 0) {
          log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(gtwa_fdSocket)");
      }
  }
  #endif
  
  uint32_t threadMainWait_process(CO_NMT_reset_cmd_t *reset)
  {
      int ready;
      struct epoll_event ev;
      uint64_t ull;
      ssize_t s;
      uint32_t diff, timerNext_us;
  
      /* wait for event or timer expiration and read data from file descriptors */
      ready = epoll_wait(tmw.epoll_fd, &ev, 1, -1);
      if (ready != 1 && errno != EINTR) {
          log_printf(LOG_DEBUG, DBG_ERRNO, "epoll_wait");
      }
      else if (ev.data.fd == tmw.event_fd) {
          s = read(tmw.event_fd, &ull, sizeof(uint64_t));
          if (s != sizeof(uint64_t)) {
              log_printf(LOG_DEBUG, DBG_ERRNO, "read(event_fd)");
          }
      }
      else if (ev.data.fd == tmw.timer_fd) {
          s = read(tmw.timer_fd, &ull, sizeof(uint64_t));
          if (s != sizeof(uint64_t) && errno != EAGAIN) {
              log_printf(LOG_DEBUG, DBG_ERRNO, "read(timer_fd)");
          }
      }
  #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
      else if (ev.data.fd == tmw.gtwa_fdSocket) {
          bool_t fail = false;
  
          tmw.gtwa_fd = accept4(tmw.gtwa_fdSocket, NULL, NULL, SOCK_NONBLOCK);
          if (tmw.gtwa_fd < 0) {
              fail = true;
              if (errno != EAGAIN && errno != EWOULDBLOCK) {
                  log_printf(LOG_CRIT, DBG_ERRNO, "accept(gtwa_fdSocket)");
              }
          }
          else {
              int ret;
              /* add fd to epoll */
              struct epoll_event ev2;
              ev2.events = EPOLLIN;
              ev2.data.fd = tmw.gtwa_fd;
              ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_ADD, ev2.data.fd, &ev2);
              if (ret < 0) {
                  fail = true;
                  log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(add, gtwa_fd)");
              }
              tmw.socketTimeoutTmr_us = 0;
          }
  
          if (fail) {
              socetAcceptEnableForEpoll();
          }
      }
      else if (ev.data.fd == tmw.gtwa_fd) {
          char buf[CO_CONFIG_GTWA_COMM_BUF_SIZE];
          size_t space = CO->nodeIdUnconfigured ?
                         CO_CONFIG_GTWA_COMM_BUF_SIZE :
                         CO_GTWA_write_getSpace(CO->gtwa);
  
          s = read(tmw.gtwa_fd, buf, space);
  
          if (CO->nodeIdUnconfigured) {
              /* purge data */
          }
          else if (s < 0 &&  errno != EAGAIN) {
              log_printf(LOG_DEBUG, DBG_ERRNO, "read(gtwa_fd)");
          }
          else if (s >= 0) {
              if (tmw.commandInterface == CO_COMMAND_IF_STDIO) {
                  /* simplify command interface on stdio, make hard to type
                   * sequence optional, prepend "[0] " to string, if missing */
                  const char sequence[] = "[0] ";
                  bool_t closed = (buf[s-1] == '\n'); /* is command closed? */
  
                  if (buf[0] != '[' && (space - s) >= strlen(sequence)
                      && isgraph(buf[0]) && buf[0] != '#'
                      && closed && tmw.freshCommand
                  ) {
                      CO_GTWA_write(CO->gtwa, sequence, strlen(sequence));
                  }
                  tmw.freshCommand = closed;
                  CO_GTWA_write(CO->gtwa, buf, s);
              }
              else { /* socket, local or tcp */
                  if (s == 0) {
                      int ret;
                      /* EOF received, close connection and enable socket accept*/
                      ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_DEL,
                                      tmw.gtwa_fd, NULL);
                      if (ret < 0) {
                          log_printf(LOG_CRIT, DBG_ERRNO,
                                     "epoll_ctl(del, gtwa_fd)");
                      }
                      if (close(tmw.gtwa_fd) < 0) {
                          log_printf(LOG_CRIT, DBG_ERRNO, "close(gtwa_fd)");
                      }
                      tmw.gtwa_fd = -1;
                      socetAcceptEnableForEpoll();
                  }
                  else {
                      CO_GTWA_write(CO->gtwa, buf, s);
                  }
              }
          }
          tmw.socketTimeoutTmr_us = 0;
      }
  #endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
  
      /* calculate time difference since last call */
      ull = CO_LinuxThreads_clock_gettime_us();
      diff = (uint32_t)(ull - tmw.start);
      tmw.start = ull;
  
  #if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
      /* if socket connection is established, verify timeout */
      if (tmw.socketTimeout_us > 0 && tmw.gtwa_fdSocket > 0 && tmw.gtwa_fd > 0) {
          if (tmw.socketTimeoutTmr_us > tmw.socketTimeout_us) {
              int ret;
              /* timout expired, close current connection and accept next */
              ret = epoll_ctl(tmw.epoll_fd, EPOLL_CTL_DEL, tmw.gtwa_fd, NULL);
              if (ret < 0) {
                  log_printf(LOG_CRIT, DBG_ERRNO, "epoll_ctl(del, gtwa_fd), tmo");
              }
              if (close(tmw.gtwa_fd) < 0) {
                  log_printf(LOG_CRIT, DBG_ERRNO, "close(gtwa_fd), tmo");
              }
              tmw.gtwa_fd = -1;
              socetAcceptEnableForEpoll();
          }
          else {
              tmw.socketTimeoutTmr_us += diff;
          }
      }
  #endif
  
      /* stack will lower this, if necessary */
      timerNext_us = tmw.interval_us;
  
      /* process CANopen objects */
      *reset = CO_process(CO, diff, &timerNext_us);
  
      /* lower next timer interval if necessary */
      if (timerNext_us < tmw.interval_us) {
          int ret;
          /* add one microsecond extra delay and make sure it is not zero */
          timerNext_us += 1;
          if (tmw.interval_us < 1000000) {
              tmw.tm.it_value.tv_nsec = timerNext_us * 1000;
          }
          else {
              tmw.tm.it_value.tv_sec = timerNext_us / 1000000;
              tmw.tm.it_value.tv_nsec = (timerNext_us % 1000000) * 1000;
          }
          ret = timerfd_settime(tmw.timer_fd, 0, &tmw.tm, NULL);
          if (ret < 0) {
              log_printf(LOG_DEBUG, DBG_ERRNO, "timerfd_settime");
          }
      }
  
      return diff;
  }
  
  
  /* Realtime thread (threadRT) *************************************************/
  static struct {
      uint32_t us_interval;         /* configured interval in us */
      int interval_fd;              /* timer fd */
  } threadRT;
  
  void CANrx_threadTmr_init(uint32_t interval_us)
  {
      struct itimerspec itval;
  
      threadRT.us_interval = interval_us;
      /* set up non-blocking interval timer */
      threadRT.interval_fd = timerfd_create(CLOCK_MONOTONIC, 0);
      (void)fcntl(threadRT.interval_fd, F_SETFL, O_NONBLOCK);
      itval.it_interval.tv_sec = 0;
      itval.it_interval.tv_nsec = interval_us * 1000;
      itval.it_value = itval.it_interval;
      (void)timerfd_settime(threadRT.interval_fd, 0, &itval, NULL);
  }
  
  void CANrx_threadTmr_close(void)
  {
      (void)close(threadRT.interval_fd);
      threadRT.interval_fd = -1;
  }
  
  uint32_t CANrx_threadTmr_process(void)
  {
      int32_t result;
      uint64_t i;
      bool_t syncWas;
      uint64_t missed = 0;
  
      result = CO_CANrxWait(CO->CANmodule[0], threadRT.interval_fd, NULL);
      if (result < 0) {
          result = read(threadRT.interval_fd, &missed, sizeof(missed));
          if (result > 0) {
          /* at least one timer interval occurred */
          CO_LOCK_OD();
  
          if(CO->CANmodule[0]->CANnormal) {
  
              for (i = 0; i <= missed; i++) {
  
  #if CO_NO_SYNC == 1
              /* Process Sync */
              syncWas = CO_process_SYNC(CO, threadRT.us_interval, NULL);
  #else
              syncWas = false;
  #endif
              /* Read inputs */
              CO_process_RPDO(CO, syncWas);
  
              /* Write outputs */
              CO_process_TPDO(CO, syncWas, threadRT.us_interval, NULL);
              }
          }
  
          CO_UNLOCK_OD();
          }
      }
  
      return (uint32_t) missed;
  }
  