/*
   * Copyright (C) 2016 Kaspar Schleiser <kaspar@schleiser.de>
   *
   * This file is subject to the terms and conditions of the GNU General Public
   * License v2. See the file LICENSE for more details.
   */
  
  #include <errno.h>
  #include <stdio.h>
  #include <string.h>
  #include <unistd.h>
  
  #include <netinet/in.h>
  #include <linux/if.h>
  #include <linux/if_tun.h>
  #include <sys/types.h>
  #include <sys/ioctl.h>
  #include <sys/stat.h>
  #include <fcntl.h>
  #include <arpa/inet.h>
  #include <sys/select.h>
  #include <sys/time.h>
  #include <stdlib.h>
  
  #include <netdb.h>
  
  #include <termios.h>
  
  #define MTU 9000
  
  #define TRACE(x)
  #define TTY_TIMEOUT_MS (500)
  
  #define case_baudrate(val)    \
      case val:                 \
          *baudrate = B ## val; \
          break
  
  #define BAUDRATE_DEFAULT B115200
  
  #define TCP_DEV "tcp:"
  #define IOTLAB_TCP_PORT "20000"
  
  /* Size of serial write buffer */
  #define SERIAL_BUFFER_SIZE 64
  
  static void usage(void)
  {
      fprintf(stderr, "Usage: ethos <tap> <serial> [baudrate]\n");
      fprintf(stderr, "       ethos <tap> tcp:<host> [port]\n");
  }
  
  static void checked_write(int handle, void *buffer, int nbyte)
  {
      while (nbyte > 0) {
          ssize_t res = write(handle, buffer, nbyte);
          if (res <= 0) {
              fprintf(stderr, "write to fd %i failed: %s\n", handle, strerror(errno));
              return;
          }
          nbyte -= res;
      }
  }
  
  int set_serial_attribs(int fd, int speed, int parity)
  {
      struct termios tty;
      memset (&tty, 0, sizeof tty);
      if (tcgetattr (fd, &tty) != 0)
      {
          perror ("error in tcgetattr");
          return -1;
      }
  
      cfsetospeed (&tty, speed);
      cfsetispeed (&tty, speed);
  
      tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; /* 8-bit chars*/
                                          /* disable IGNBRK for mismatched speed
                                           * tests; otherwise receive break*/
                                          /* as \000 chars*/
      tty.c_iflag &= ~IGNBRK;             /* disable break processing*/
      tty.c_lflag = 0;                    /* no signaling chars, no echo,*/
                                          /* no canonical processing*/
      tty.c_oflag = 0;                    /* no remapping, no delays*/
      tty.c_cc[VMIN]  = 0;                /* read doesn't block*/
      tty.c_cc[VTIME] = TTY_TIMEOUT_MS / 100; /* 0.5 seconds read timeout*/
                                              /* in tenths of a second*/
  
      tty.c_iflag &= ~(IXON | IXOFF | IXANY); /* shut off xon/xoff ctrl*/
  
      tty.c_cflag |= (CLOCAL | CREAD);    /* ignore modem controls,*/
                                          /* enable reading*/
      tty.c_cflag &= ~(PARENB | PARODD);  /* shut off parity*/
      tty.c_cflag |= parity;
      tty.c_cflag &= ~CSTOPB;
      tty.c_cflag &= ~CRTSCTS;
      cfmakeraw(&tty);
  
      if (tcsetattr (fd, TCSANOW, &tty) != 0)
      {
          perror("error from tcsetattr");
          return -1;
      }
      return 0;
  }
  
  void set_blocking(int fd, int should_block)
  {
      struct termios tty;
      memset (&tty, 0, sizeof tty);
      if (tcgetattr (fd, &tty) != 0)
      {
          perror("error from tggetattr");
          return;
      }
  
      tty.c_cc[VMIN]  = should_block ? 1 : 0;
      tty.c_cc[VTIME] = TTY_TIMEOUT_MS / 100; /* 0.5 seconds read timeout*/
                                              /* in tenths of a second*/
  
      if (tcsetattr (fd, TCSANOW, &tty) != 0)
          perror("error setting term attributes");
  }
  
  /**
   * @brief Escape char definitions
   * @{
   */
  #define LINE_FRAME_DELIMITER        (0x7E)
  #define LINE_ESC_CHAR               (0x7D)
  #define LINE_FRAME_TYPE_DATA        (0x00)
  #define LINE_FRAME_TYPE_TEXT        (0x01)
  #define LINE_FRAME_TYPE_HELLO       (0x02)
  #define LINE_FRAME_TYPE_HELLO_REPLY (0x03)
  /** @} */
  
  typedef enum {
      WAIT_FRAMESTART,
      IN_FRAME,
      IN_ESCAPE
  } line_state_t;
  
  typedef struct {
      line_state_t state;
      int frametype;
      size_t framebytes;
      char frame[MTU];
      char local_l2_addr[6];
      char remote_l2_addr[6];
  } serial_t;
  
  /**************************************************************************
   * tun_alloc: allocates or reconnects to a tun/tap device. The caller     *
   *            needs to reserve enough space in *dev.                      *
   **************************************************************************/
  int tun_alloc(char *dev, int flags) {
  
    struct ifreq ifr;
    int fd, err;
  
    if( (fd = open("/dev/net/tun", O_RDWR)) < 0 ) {
      perror("Opening /dev/net/tun");
      return fd;
    }
  
    memset(&ifr, 0, sizeof(ifr));
  
    ifr.ifr_flags = flags;
  
    if (*dev) {
      strncpy(ifr.ifr_name, dev, IFNAMSIZ);
    }
  
    if( (err = ioctl(fd, TUNSETIFF, (void *)&ifr)) < 0 ) {
      perror("ioctl(TUNSETIFF)");
      close(fd);
      return err;
    }
  
    strcpy(dev, ifr.ifr_name);
  
    return fd;
  }
  
  static void _handle_char(serial_t *serial, char c)
  {
      serial->frame[serial->framebytes] = c;
      serial->framebytes++;
  }
  
  static int _serial_handle_byte(serial_t *serial, char c)
  {
      switch (serial->state) {
          case WAIT_FRAMESTART:
              if (c == LINE_FRAME_DELIMITER) {
                  TRACE("frame start");
                  serial->state = IN_FRAME;
                  serial->frametype = 0;
                  serial->framebytes = 0;
              }
              break;
          case IN_FRAME:
              if (c == LINE_ESC_CHAR) {
                  TRACE("i esc");
                  serial->state = IN_ESCAPE;
              }
              else if (c == LINE_FRAME_DELIMITER) {
                  if (!serial->framebytes) {
                      TRACE("i del unexp");
                  } else {
                      TRACE("i del");
                      serial->state = WAIT_FRAMESTART;
                      return serial->framebytes;
                  }
              }
              else {
                  _handle_char(serial, c);
              }
              break;
          case IN_ESCAPE:
              if (c == (LINE_FRAME_DELIMITER ^ 0x20)) {
                  _handle_char(serial, LINE_FRAME_DELIMITER);
              }
              else if (c == (LINE_ESC_CHAR ^ 0x20)) {
                  _handle_char(serial, LINE_ESC_CHAR);
              }
              else if (c == (LINE_FRAME_TYPE_TEXT ^ 0x20)) {
                  TRACE("esc txt");
                  serial->frametype = LINE_FRAME_TYPE_TEXT;
              }
              else if (c == (LINE_FRAME_TYPE_HELLO ^ 0x20)) {
                  TRACE("esc hello");
                  serial->frametype = LINE_FRAME_TYPE_HELLO;
              }
              else if (c == (LINE_FRAME_TYPE_HELLO_REPLY ^ 0x20)) {
                  TRACE("esc hello_reply");
                  serial->frametype = LINE_FRAME_TYPE_HELLO_REPLY;
              }
              else if (c == LINE_FRAME_DELIMITER) {
                  TRACE("esc -del");
              }
              serial->state = IN_FRAME;
              break;
      }
  
      return 0;
  }
  
  static void _write_escaped(int fd, char* buf, ssize_t n)
  {
      /*
       * Certain USB-to-UART adapters/drivers will immediately send a USB packet
       * with a single byte instead of buffering internally when the application
       * does writes one byte at a time. Since USB Full Speed can only send 1
       * packet per 1 ms, this causes huge latencies for the network, because each
       * byte of data will then add at least 1 ms on the latency.
       * Observed on NXP OpenSDAv2 (Kinetis FRDM boards), both CMSIS/mbed DAPlink
       * and Segger Jlink firmware are affected.
       */
      /* Our workaround is to prepare the data to send in a local buffer and then
       * call write() on the buffer instead of one char at a time */
      uint8_t out[SERIAL_BUFFER_SIZE];
      size_t escaped = 0;
      size_t buffered = 0;
  
      while(n--) {
          unsigned char c = *buf++;
          if (c == LINE_FRAME_DELIMITER || c == LINE_ESC_CHAR) {
              out[buffered++] = LINE_ESC_CHAR;
              c ^= 0x20;
              ++escaped;
              if (buffered >= SERIAL_BUFFER_SIZE) {
                  checked_write(fd, out, buffered);
                  buffered = 0;
              }
          }
          out[buffered++] = c;
          if (buffered >= SERIAL_BUFFER_SIZE) {
              checked_write(fd, out, buffered);
              buffered = 0;
          }
      }
      if (buffered) {
          checked_write(fd, out, buffered);
      }
  }
  
  static void _send_hello(int serial_fd, serial_t *serial, unsigned type)
  {
      char delim = LINE_FRAME_DELIMITER;
      char head[] = { LINE_FRAME_DELIMITER, LINE_ESC_CHAR, (type ^ 0x20) };
      checked_write(serial_fd, head, sizeof(head));
      checked_write(serial_fd, serial->local_l2_addr, 6);
      checked_write(serial_fd, &delim, 1);
  }
  
  static void _clear_neighbor_cache(const char *ifname)
  {
      char tmp[20 + IFNAMSIZ];
      snprintf(tmp, sizeof(tmp), "ip neigh flush dev %s", ifname);
      if (system(tmp) < 0) {
          fprintf(stderr, "error while flushing device neighbor cache\n");
      }
  }
  
  static int _parse_baudrate(const char *arg, unsigned *baudrate)
  {
      if (arg == NULL) {
          *baudrate = BAUDRATE_DEFAULT;
          return 0;
      }
  
      switch(strtol(arg, (char**)NULL, 10)) {
      case 9600:
          *baudrate = B9600;
          break;
      case 19200:
          *baudrate = B19200;
          break;
      case 38400:
          *baudrate = B38400;
          break;
      case 57600:
          *baudrate = B57600;
          break;
      case 115200:
          *baudrate = B115200;
          break;
      /* the following baudrates might not be available on all platforms */
      #ifdef B234000
          case_baudrate(230400);
      #endif
      #ifdef B460800
          case_baudrate(460800);
      #endif
      #ifdef B500000
          case_baudrate(500000);
      #endif
      #ifdef B576000
          case_baudrate(576000);
      #endif
      #ifdef B921600
          case_baudrate(921600);
      #endif
      #ifdef B1000000
          case_baudrate(1000000);
      #endif
      #ifdef B1152000
          case_baudrate(1152000);
      #endif
      #ifdef B1500000
          case_baudrate(1500000);
      #endif
      #ifdef B2000000
          case_baudrate(2000000);
      #endif
      #ifdef B2500000
          case_baudrate(2500000);
      #endif
      #ifdef B3000000
          case_baudrate(3000000);
      #endif
      #ifdef B3500000
          case_baudrate(3500000);
      #endif
      #ifdef B4000000
          case_baudrate(4000000);
      #endif
      default:
          return -1;
      }
  
      return 0;
  }
  
  int _parse_tcp_arg(char *name, char *port_arg, char **host, char **port)
  {
      /* Remove 'tcp:' */
      name = &name[sizeof(TCP_DEV) - 1];
  
      /* Set default if NULL */
      if (!port_arg) {
          port_arg = IOTLAB_TCP_PORT;
      }
  
      *host = name;
      *port = port_arg;
  
      return 0;
  }
  
  /* Adapted from 'getaddrinfo' manpage example */
  int _tcp_connect(char *host, char *port)
  {
      int sfd = -1;
      struct addrinfo hints, *result, *rp;
  
      memset(&hints, 0, sizeof(struct addrinfo));
      hints.ai_family = AF_UNSPEC;
      hints.ai_socktype = SOCK_STREAM;
  
      int s = getaddrinfo(host, port, &hints, &result);
      if (s) {
          fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(s));
          return -1;
      }
  
      /* getaddrinfo() returns a list of address structures.
         Try each address until we successfully connect(2).
         If socket(2) (or connect(2)) fails, we (close the socket
         and) try the next address. */
      for (rp = result; rp != NULL; rp = rp->ai_next) {
          sfd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
          if (sfd == -1)
              continue;
  
          if (connect(sfd, rp->ai_addr, rp->ai_addrlen) != -1)
              break;
  
          close(sfd);
      }
  
      freeaddrinfo(result);
  
      if (rp == NULL) {
          fprintf(stderr, "Could not connect to '%s:%s'\n", host, port);
          return -1;
      }
  
      return sfd;
  }
  
  int _set_socket_timeout(int sfd)
  {
      struct timeval timeout = {
          .tv_sec = 0,
          .tv_usec = TTY_TIMEOUT_MS * 1000,
      };
  
      if (setsockopt(sfd, SOL_SOCKET, SO_RCVTIMEO,
                     (char *)&timeout, sizeof(timeout)) == -1) {
          perror("setsockopt failed\n");
          return 1;
      }
  
      if (setsockopt(sfd, SOL_SOCKET, SO_SNDTIMEO,
                     (char *)&timeout, sizeof(timeout)) == -1) {
          perror("setsockopt failed\n");
          return 1;
      }
      return 0;
  }
  
  int _open_tcp_connection(char *name, char *port_arg)
  {
      char *host;
      char *port;
  
      int ret = _parse_tcp_arg(name, port_arg, &host, &port);
      if (ret) {
          fprintf(stderr, "Error while parsing tcp arguments\n");
          return -1;
      }
  
      int sfd = _tcp_connect(host, port);
      if (_set_socket_timeout(sfd)) {
          fprintf(stderr, "Error while setting socket options\n");
          return -1;
      }
      return sfd;
  }
  
  int _open_serial_connection(char *name, char *baudrate_arg)
  {
      unsigned baudrate = 0;
      if (_parse_baudrate(baudrate_arg, &baudrate) == -1) {
          fprintf(stderr, "Invalid baudrate specified: %s\n", baudrate_arg);
          return 1;
      }
  
      int serial_fd = open(name, O_RDWR | O_NOCTTY | O_SYNC);
  
      if (serial_fd < 0) {
          fprintf(stderr, "Error opening serial device %s\n", name);
          return -1;
      }
  
      set_serial_attribs(serial_fd, baudrate, 0);
      set_blocking(serial_fd, 1);
  
      return serial_fd;
  }
  
  int _open_connection(char *name, char* option)
  {
      if (strncmp(name, TCP_DEV, strlen(TCP_DEV)) == 0) {
          return _open_tcp_connection(name, option);
      } else {
          return _open_serial_connection(name, option);
      }
  }
  
  int main(int argc, char *argv[])
  {
      char inbuf[MTU];
      char *serial_option = NULL;
  
      serial_t serial = {0};
  
      if (argc < 3) {
          usage();
          return 1;
      }
  
      if (argc >= 4) {
          serial_option = argv[3];
      }
  
      char ifname[IFNAMSIZ];
      strncpy(ifname, argv[1], IFNAMSIZ);
      int tap_fd = tun_alloc(ifname, IFF_TAP | IFF_NO_PI);
  
      if (tap_fd < 0) {
          return 1;
      }
  
  
      int serial_fd = _open_connection(argv[2], serial_option);
      if (serial_fd < 0) {
          fprintf(stderr, "Error opening serial device %s\n", argv[2]);
          return 1;
      }
  
      fd_set readfds;
      int max_fd = (serial_fd > tap_fd) ? serial_fd : tap_fd;
  
      fprintf(stderr, "----> ethos: sending hello.\n");
      _send_hello(serial_fd, &serial, LINE_FRAME_TYPE_HELLO);
  
      fprintf(stderr, "----> ethos: activating serial pass through.\n");
      _send_hello(serial_fd, &serial, LINE_FRAME_TYPE_HELLO);
      while(1) {
          int activity;
          FD_ZERO(&readfds);
          FD_SET(STDIN_FILENO, &readfds);
          FD_SET(tap_fd, &readfds);
          FD_SET(serial_fd, &readfds);
          activity = select( max_fd + 1 , &readfds , NULL , NULL , NULL);
  
          if ((activity < 0) && (errno != EINTR))
          {
              perror("select error");
          }
  
          if (FD_ISSET(serial_fd, &readfds)) {
              ssize_t n = read(serial_fd, inbuf, sizeof(inbuf));
              if (n > 0) {
                  char *ptr = inbuf;
                  while (n--) {
                      size_t res = _serial_handle_byte(&serial, *ptr++);
                      if (res) {
                          switch (serial.frametype) {
                              case LINE_FRAME_TYPE_DATA:
                                  checked_write(tap_fd, serial.frame, serial.framebytes);
                                  break;
                              case LINE_FRAME_TYPE_TEXT:
                                  checked_write(STDOUT_FILENO, serial.frame, serial.framebytes);
                                  break;
                              case LINE_FRAME_TYPE_HELLO:
                              case LINE_FRAME_TYPE_HELLO_REPLY:
                                  if (serial.framebytes == 6) {
                                      memcpy(serial.remote_l2_addr, serial.frame, 6);
                                      if (serial.frametype == LINE_FRAME_TYPE_HELLO) {
                                          fprintf(stderr, "----> ethos: hello received\n");
                                          _send_hello(serial_fd, &serial, LINE_FRAME_TYPE_HELLO_REPLY);
                                      } else {
                                          fprintf(stderr, "----> ethos: hello reply received\n");
                                      }
                                      _clear_neighbor_cache(ifname);
                                  }
                                  break;
                          }
                          memset(&serial, '\0', sizeof(serial));
                      }
                  }
              }
              else {
                  fprintf(stderr, "lost serial connection.\n");
                  exit(1);
              }
          }
  
          if (FD_ISSET(tap_fd, &readfds)) {
              ssize_t res = read(tap_fd, inbuf, sizeof(inbuf));
              if (res <= 0) {
                  fprintf(stderr, "error reading from tap device. res=%zi\n", res);
                  continue;
              }
              char delim = LINE_FRAME_DELIMITER;
              checked_write(serial_fd, &delim, 1);
              _write_escaped(serial_fd, inbuf, res);
              checked_write(serial_fd, &delim, 1);
          }
  
          if (FD_ISSET(STDIN_FILENO, &readfds)) {
              ssize_t res = read(STDIN_FILENO, inbuf, sizeof(inbuf));
              if (res <= 0) {
                  fprintf(stderr, "error reading from stdio. res=%zi\n", res);
                  continue;
              }
  
              if (res) {
                  char delim = LINE_FRAME_DELIMITER;
                  char head[] = { LINE_FRAME_DELIMITER, LINE_ESC_CHAR, (LINE_FRAME_TYPE_TEXT ^ 0x20) };
                  checked_write(serial_fd, head, sizeof(head));
                  _write_escaped(serial_fd, inbuf, res);
                  checked_write(serial_fd, &delim, 1);
              }
          }
      }
  
      return 0;
  }