int read_j1708_message(int serial_port, char* buf, pthread_mutex_t *lock){
  int chars = 0;
  int status = 0;
  int retval = 0;


  //inter-char timeout
  struct timespec timeout;
  timeout.tv_sec = 0;
  //CHANGE THIS FOR DETROIT DIESEL/CAT/ETC
  timeout.tv_nsec = TENTH_BIT_TIME * 93;
  
  fd_set fds;
  FD_ZERO (&fds);
  FD_SET (serial_port,&fds);
 
  status = pselect(serial_port + 1, &fds, NULL, NULL, NULL, NULL);//timeout=NULL => block indefinitely
  pthread_mutex_lock(lock);

  while(pselect(serial_port + 1, &fds, NULL, NULL, &timeout, NULL)){
    retval = read(serial_port,&buf[chars],1);
    chars++;
  }

  pthread_mutex_unlock(lock);
  return chars;

}

TEST(sys_select, pselect_smoke) {
  sigset_t ss;
  sigemptyset(&ss);
  sigaddset(&ss, SIGPIPE);

  fd_set r;
  FD_ZERO(&r);
  fd_set w;
  FD_ZERO(&w);
  fd_set e;
  FD_ZERO(&e);

  FD_SET(STDIN_FILENO, &r);
  FD_SET(STDOUT_FILENO, &w);
  FD_SET(STDERR_FILENO, &w);

  int max = STDERR_FILENO + 1;

  // Invalid max fd.
  ASSERT_EQ(-1, pselect(-1, &r, &w, &e, NULL, &ss));
  ASSERT_EQ(EINVAL, errno);

  // If there is data to be read on STDIN, then the number of
  // fds ready will be 3 instead of 2. Allow this case, but verify
  // every fd that is set.
  int num_fds = pselect(max, &r, &w, &e, NULL, &ss);
  ASSERT_TRUE(num_fds == 2 || num_fds == 3) << "Num fds returned " << num_fds;
  ASSERT_TRUE(FD_ISSET(STDOUT_FILENO, &w));
  ASSERT_TRUE(FD_ISSET(STDERR_FILENO, &w));
  if (num_fds == 3) {
    ASSERT_TRUE(FD_ISSET(STDIN_FILENO, &r));
  }

  // Invalid timeout.
  timespec tv;
  tv.tv_sec = -1;
  tv.tv_nsec = 0;
  ASSERT_EQ(-1, pselect(max, &r, &w, &e, &tv, &ss));
  ASSERT_EQ(EINVAL, errno);

  // Valid timeout...
  tv.tv_sec = 1;
  int pid, fd;
  DelayedWrite(&pid, &fd);

  FD_ZERO(&r);
  FD_SET(fd, &r);
  ASSERT_EQ(1, pselect(fd+1, &r, NULL, NULL, &tv, NULL));
  // Neither tv_sec nor tv_nsec should have been updated.
  ASSERT_EQ(1, tv.tv_sec);
  ASSERT_EQ(0, tv.tv_nsec);

  DelayedWriteCleanup(pid, fd);
}

int save_data_to_buffer(int descriptor, char* buffer, int length) {
        int get_length = 0;
        int read_length;
        
        fd_set server_fds;
        FD_ZERO(&server_fds);
        FD_SET(descriptor, &server_fds);
        
        struct timespec timeout;
        timeout.tv_sec = 10;
        timeout.tv_nsec = 0;
        int temp = 1;
        
        while (get_length < length) {
                read_length = recv(descriptor, buffer + get_length, length - get_length, 0);
                
                temp = pselect(descriptor + 1, NULL, &server_fds, NULL, &timeout, NULL);
                        
                if (temp == 0 || temp == -1){
                        close(descriptor);
                        print_error("Connection terminated\n", 666);
                }
                
                if (read_length == 0) {
                        break;
                } else if (read_length < 0) {
                        return -1;
                } else {
                        get_length += read_length;
                }
        }
        
        return get_length;
}

/* --------------------- */
int usockfd_check(int sockfd, const sigset_t *sigset)
{
    int fd;
    socklen_t size;
    fd_set readfds;
    int ret;
     
    FD_ZERO(&readfds);
    FD_SET(sockfd, &readfds);

    if ((ret = pselect(sockfd + 1, &readfds, NULL, NULL, NULL, sigset)) < 0) {
        if (errno == EINTR)
            return 0;
        LOG(log_error, logtype_cnid, "error in select: %s",
            strerror(errno));
        return -1;
    }

    if (ret) {
        size = 0;
        if ((fd = accept(sockfd, NULL, &size)) < 0) {
            if (errno == EINTR)
                return 0;
            LOG(log_error, logtype_cnid, "error in accept: %s", 
                strerror(errno));
            return -1;
        }
        return fd;
    } else
        return 0;
}

int
ls_wakeup_fifo_wait(LSWakeupFifo *w, struct timespec timeout)
{
    if (w->fifo_fd >= 0) {
        FD_SET(w->fifo_fd, &w->fds);
    }

    const int r = pselect(
        w->fifo_fd >= 0 ? w->fifo_fd + 1 : 0,
        &w->fds, NULL, NULL,
        ls_timespec_is_invalid(timeout) ? NULL : &timeout,
        &w->sigmask);

    if (r < 0) {
        int saved_errno = errno;
        if (w->fifo_fd >= 0) {
            FD_CLR(w->fifo_fd, &w->fds);
        }
        close(w->fifo_fd);
        w->fifo_fd = -1;
        errno = saved_errno;
        return -1;

    } else if (r == 0) {
        return 0;

    } else {
        FD_CLR(w->fifo_fd, &w->fds);
        close(w->fifo_fd);
        w->fifo_fd = -1;
        return 1;
    }
}

void ms_sleep(long ms)
{
    struct timespec t;
    t.tv_sec = (ms / 1000);
    t.tv_nsec = (ms % 1000) * 1000000;
    pselect(0, NULL, NULL, NULL, &t, NULL);
}

static int
do_test (void)
{
	int rc;
	sigset_t wait_mask, mask_sigchld;
	struct sigaction act;

	// block SIGALRM. We want to handle it only when we're ready
	sigemptyset(&mask_sigchld);
	sigaddset(&mask_sigchld, SIGALRM);
	sigprocmask(SIG_BLOCK,  &mask_sigchld, &wait_mask);
	sigdelset(&wait_mask, SIGALRM);

	// register a signal handler so we can see when the signal arrives
	memset(&act, 0, sizeof(act));
	sigemptyset(&act.sa_mask); // just in case an empty set isn't all 0's (total paranoia)
	act.sa_handler = handler;
	sigaction(SIGALRM, &act, NULL);

	// send ourselves a SIGARLM. It will pend until we unblock that signal in pselect()
	printf("sending ourselves a signal\n");
	kill(getpid(), SIGALRM);

	printf("signal is pending; calling pselect()\n");
	rc = pselect(0, NULL, NULL, NULL, NULL, &wait_mask);
	if (rc != -1 || errno != EINTR) {
		int e = errno;
		printf("pselect() returned %d, errno %d (%s)\n", rc, e, strerror(e));
		exit(1);
	}

	return 0;
}

  int read_into(std::vector<readfd> &dests, const timespec *timeout,
                const sigset_t *sigmask) {
    while(true) {
      int maxfd = -1;
      fd_set fds;
      FD_ZERO(&fds);
      for(const auto &i : dests) {
        if(i.fd >= 0) {
          maxfd = std::max(maxfd, i.fd);
          FD_SET(i.fd, &fds);
        }
      }
      if(maxfd < 0)
        return 0;

      int rv = pselect(maxfd + 1, &fds, nullptr, nullptr, timeout, sigmask);
      if(rv <= 0)
        return rv;

      for(auto &i : dests) {
        if(i.fd >= 0 && FD_ISSET(i.fd, &fds)) {
          ssize_t size;
          char buf[BUFSIZ];

          if((size = read(i.fd, buf, sizeof(buf))) < 0)
            return size;
          if(size == 0)
            i.fd = -i.fd;
          else
            i.dest->append(buf, size);
        }
      }
    }
  }

void check_in_events(int fd)
{
    fd_set rfds;
    sigset_t mask;
    sigset_t orig_mask;

    sigemptyset (&mask);
    sigaddset (&mask, SIGTERM);

    if (sigprocmask(SIG_BLOCK, &mask, &orig_mask) < 0)
        debug_log("sigprocmask()");


    while(main_loop) {
        FD_ZERO(&rfds);
        FD_SET(fd, &rfds);

        if(pselect (FD_SETSIZE, &rfds, NULL, NULL, NULL, &orig_mask) == -1) {
            if(errno == EINTR)
                continue;

            fatal("select() failed\n");
        }

        get_in_events(fd);
    }
}

R_API int r_cons_readchar() {
	void *bed;
	char buf[2];
	buf[0] = -1;
	if (readbuffer_length > 0) {
		int ch = *readbuffer;
		readbuffer_length--;
		memmove (readbuffer, readbuffer + 1, readbuffer_length);
		return ch;
	}
#if __WINDOWS__ && !__CYGWIN__ //&& !MINGW32
	#if 1   // if something goes wrong set this to 0. skuater.....
	return readchar_win(0);
	#endif
	BOOL ret;
	DWORD out;
	DWORD mode;
	HANDLE h = GetStdHandle (STD_INPUT_HANDLE);
	GetConsoleMode (h, &mode);
	SetConsoleMode (h, 0); // RAW
	bed = r_cons_sleep_begin ();
	ret = ReadConsole (h, buf, 1, &out, NULL);
	r_cons_sleep_end (bed);
	FlushConsoleInputBuffer (h);
	if (!ret) {
		return -1;
	}
	SetConsoleMode (h, mode);
#else
	r_cons_set_raw (1);
	bed = r_cons_sleep_begin ();

	// Blocks until either stdin has something to read or a signal happens.
	// This serves to check if the terminal window was resized. It avoids the race
	// condition that could happen if we did not use pselect or select in case SIGWINCH
	// was handled immediately before the blocking call (select or read). The race is
	// prevented from happening by having SIGWINCH blocked process-wide except for in
	// pselect (that is what pselect is for).
	fd_set readfds;
	sigset_t sigmask;
	FD_ZERO (&readfds);
	FD_SET (STDIN_FILENO, &readfds);
	r_signal_sigmask (0, NULL, &sigmask);
	sigdelset (&sigmask, SIGWINCH);
	pselect (STDIN_FILENO + 1, &readfds, NULL, NULL, NULL, &sigmask);
	if (sigwinchFlag != 0) {
		resizeWin ();
	}

	ssize_t ret = read (STDIN_FILENO, buf, 1);
	r_cons_sleep_end (bed);
	if (ret != 1) {
		return -1;
	}
	if (bufactive) {
		r_cons_set_raw (0);
	}
#endif
	return r_cons_controlz (buf[0]);
}

int
select(int nfds, fd_set *in0, fd_set *out0, fd_set *ex0, struct timeval *tv)
{
	timespec_t ts;
	timespec_t *tsp;

	if (tv == NULL)
		tsp = NULL;
	else {
		/* check timeval validity */
		if (tv->tv_usec < 0 || tv->tv_usec >= MICROSEC) {
			errno = EINVAL;
			return (-1);
		}
		/*
		 * Convert timeval to timespec.
		 * To preserve compatibility with past behavior,
		 * when select was built upon poll(2), which has a
		 * minimum non-zero timeout of 1 millisecond, force
		 * a minimum non-zero timeout of 500 microseconds.
		 */
		ts.tv_sec = tv->tv_sec;
		ts.tv_nsec = tv->tv_usec * 1000;
		if (ts.tv_nsec != 0 && ts.tv_nsec < 500000)
			ts.tv_nsec = 500000;
		tsp = &ts;
	}

	return (pselect(nfds, in0, out0, ex0, tsp, NULL));
}

int main(void) {
	int fd = STDIN_FILENO, i;
	char buffer[1024];
	fd_set zbior;
	struct sigaction act, oldact[32];
	sigset_t oldsigset;

	/* Block all signals -- we will recieve them during pselect(2) only */
	sigfillset(&act.sa_mask);
	sigdelset(&act.sa_mask, SIGKILL);
	sigdelset(&act.sa_mask, SIGSTOP);
	sigprocmask(SIG_SETMASK, &act.sa_mask, &oldsigset);

	/* Now catch all signals */
	act.sa_handler = sig_handler;
	act.sa_flags = 0;
#if SA_INTERRUPT
	act.sa_flags |= SA_INTERRUPT;
#endif
#if SA_RESTART
	act.sa_flags |= SA_RESTART;
#endif
	for (i = 1; i<32; ++i) {
		sigaction(i, &act, oldact + i);
	}

	/* Do the job */
	setvbuf(stdin, NULL, _IONBF, 0);
	alarm(5);
	for(;;) {
		FD_ZERO(&zbior);
		FD_SET(fd, &zbior);

		i = pselect(fd + 1, &zbior, 0, 0, 0, &oldsigset);
		if (i > 0) {
			fgets(buffer, sizeof buffer, stdin);
			printf("input: %s", buffer);
		} else if (i == 0) {
			/* dead code -- there is no timeout */
			fputs("select: returned 0\n", stderr);
			return 1;
		} else if (errno != EINTR) {
			perror("select");
			return 1;
		} else if (got_sig_num == SIGALRM) {
			alarm(5);
			puts("tick");
		} else {
			printf("signal: %d\n", (int)got_sig_num);
			if (got_sig_num == 15) break;
		}
	}

	/* Restore old signal handlers and mask */
	for (i = 1; i<32; ++i) {
		sigaction(i, oldact + i, 0);
	}
	sigprocmask(SIG_SETMASK, &oldsigset, 0);
	return 0;
}

int mainLoop(int fd, sigset_t* sigMask)
{
  /*Endless loop for all commands*/
  while(1)
    {
      /*Preparing for pselect() call*/
      fd_set fds;
      FD_ZERO(&fds);
      FD_SET(fd, &fds);
      /*Calling pselect()*/
      if (pselect(fd + 1, &fds, NULL, NULL, NULL, sigMask) == -1)
	{
	  /*pselect() has returned -1, what the problem*/
	  int errorCode = errno;
	  if (errorCode == EINTR)
	    {
	      /*Now we must handle every signal we caught*/
	      handleSignals();
	      /*Trying to call pselect() one more time*/
	      continue;
	    }
	  /*it is an unexpected system call error, we must stop processing*/
	  onSystemCallError("pselect()", errorCode);
	} /*if (pselect() == -1)*/
      assert(FD_ISSET(fd, &fds));
      if (!processInputCommand(fd)) /* function returns 0 if fd was closed */
	return 0;
    } /*while(1)*/
}

int
xmlrpc_pselect(int                     const n,
               fd_set *                const readfdsP,
               fd_set *                const writefdsP,
               fd_set *                const exceptfdsP,
               const xmlrpc_timespec * const timeoutP,
               sigset_t *              const sigmaskP) {

    int retval;

#if HAVE_PSELECT
#if !HAVE_TIMESPEC
  #error "Impossible configuration -- has pselect(), but not struct timespec"
#else
    retval = pselect(n, readfdsP, writefdsP, exceptfdsP, timeoutP, sigmaskP);
#endif
#else /* HAVE_PSELECT */
    struct timeval timeout;
    
    timeout.tv_sec  = timeoutP->tv_sec;
    timeout.tv_usec = timeoutP->tv_nsec/1000;
#ifdef WIN32
    retval = select(n, readfdsP, writefdsP, exceptfdsP, &timeout);
#else
    {
       sigset_t origmask;
       sigprocmask(SIG_SETMASK, sigmaskP, &origmask);
       retval = select(n, readfdsP, writefdsP, exceptfdsP, &timeout);
       sigprocmask(SIG_SETMASK, &origmask, NULL);
    }
#endif
#endif

    return retval;
}

/* 
 * Wait for an appropriate response packet for 'secs' seconds.
 * Appropriateness is checked by calling callback(), which must return
 * a non-zero value to cancel the select loop.
 *
 * Returns 0 on success, -1 on failure (timeout).
 */
int ts_packet_wait(struct tunsess *tun, int secs, int (*callback)(struct tunsess *))
{
  fd_set rfds;
  struct timespec ts_wait;
  struct timespec ts_end;
  struct timespec ts_current;
  int r;

  FD_ZERO(&rfds);
  FD_SET(tun->sockfd, &rfds);

  clock_gettime(CLOCK_MONOTONIC, &ts_end);
  ts_end.tv_sec += secs;

 do_select:
  clock_gettime(CLOCK_MONOTONIC, &ts_current);
  current = tun;

  if (timespec_subtract(&ts_wait, &ts_end, &ts_current) == 1)
    goto timed_out;

  r = pselect(tun->sockfd+1, &rfds, NULL, NULL, &ts_wait, NULL);
  if (r==1) {
    ts_recv_packet(tun);
    if (callback(tun) == 0)
      return 0;
  }

  if (!tun->close_flag)
    goto do_select;

 timed_out:
  return -1;
}

int peisk_waitForWrite(int fd,double t) {
    fd_set readSet,writeSet,excpSet;
    int n;

#ifdef GUMSTIX
    struct timeval timeout;
    timeout.tv_sec = (int) t;
    timeout.tv_usec = (int) (fmod(t,1.0)*1e6);
#else
    struct timespec timeout;
    timeout.tv_sec = (int) t;
    timeout.tv_nsec = (int) (fmod(t,1.0)*1e9);
#endif

    n=fd;
    FD_ZERO(&readSet);
    FD_ZERO(&writeSet);
    FD_ZERO(&excpSet);
    FD_SET(fd,&writeSet);

#ifdef GUMSTIX
    if(select(n,&readSet,&writeSet,&excpSet,&timeout))
      return 1;
    else
      return 0;
#else
    if(pselect(n,&readSet,&writeSet,&excpSet,&timeout,NULL))
        return 1;
    else
        return 0;
#endif
}

/*------------------------------------------------------------------------------
 *  Check wether read() would return anything
 *----------------------------------------------------------------------------*/
bool
TcpSocket :: canWrite (    unsigned int    sec,
                           unsigned int    usec )      throw ( Exception )
{
    fd_set              fdset;
    struct timespec     timespec;
    sigset_t            sigset;
    int                 ret;

    if ( !isOpen() ) {
        return false;
    }

    FD_ZERO( &fdset);
    FD_SET( sockfd, &fdset);

    timespec.tv_sec  = sec;
    timespec.tv_nsec = usec * 1000L;

    // mask out SIGUSR1, as we're expecting that signal for other reasons
    sigemptyset(&sigset);
    sigaddset(&sigset, SIGUSR1);

    ret = pselect( sockfd + 1, NULL, &fdset, NULL, &timespec, &sigset);
    
    if ( ret == -1 ) {
	::close( sockfd);
	sockfd = 0;
        throw Exception( __FILE__, __LINE__, "select error");
    }

    return ret > 0;
}

TEST(sys_select, pselect_smoke) {
  sigset_t ss;
  sigemptyset(&ss);
  sigaddset(&ss, SIGPIPE);

  fd_set r;
  FD_ZERO(&r);
  fd_set w;
  FD_ZERO(&w);
  fd_set e;
  FD_ZERO(&e);

  FD_SET(STDIN_FILENO, &r);
  FD_SET(STDOUT_FILENO, &w);
  FD_SET(STDERR_FILENO, &w);

  int max = STDERR_FILENO + 1;

  // Invalid max fd.
  ASSERT_EQ(-1, pselect(-1, &r, &w, &e, NULL, &ss));
  ASSERT_EQ(EINVAL, errno);

  ASSERT_EQ(2, pselect(max, &r, &w, &e, NULL, &ss));

  // Invalid timeout.
  timespec tv;
  tv.tv_sec = -1;
  tv.tv_nsec = 0;
  ASSERT_EQ(-1, pselect(max, &r, &w, &e, &tv, &ss));
  ASSERT_EQ(EINVAL, errno);

  // Valid timeout...
  tv.tv_sec = 1;
  int pid, fd;
  DelayedWrite(&pid, &fd);

  FD_ZERO(&r);
  FD_SET(fd, &r);
  ASSERT_EQ(1, pselect(fd+1, &r, NULL, NULL, &tv, NULL));
  // Neither tv_sec nor tv_nsec should have been updated.
  ASSERT_EQ(1, tv.tv_sec);
  ASSERT_EQ(0, tv.tv_nsec);

  DelayedWriteCleanup(pid, fd);
}

int Pselect(int n, fd_set *readfds, fd_set *writefds, fd_set *exceptfds,
            const struct timespec *timeout, const sigset_t *sigmask) {
  int rc;

  if ((rc = pselect(n, readfds, writefds, exceptfds, timeout, sigmask)) < 0) {
    unix_error("Pselect error");
  }
  return rc;
}

int SerialPort::writeData(const char *data, int length, bool block)
{
    if (!mIsOpen)
    {
        qCritical() << "Serial port not open.";
        return -2;
    }
    
    int res = 0;
    int written = 0;
    fd_set set;
    timespec timeout;
    
    if (block)
    {
        while (length)
        {
            FD_ZERO(&set); /* clear the set */
            FD_SET(mFd, &set); /* add our file descriptor to the set */
            timeout.tv_sec = 0;
            timeout.tv_nsec = 1000000;
            res = pselect(mFd + 1, NULL, &set, NULL, &timeout, NULL);
            
            if(res < 0)
            {
                qCritical().nospace() << "PSelect failed in writeData (" << res << "), ignoring";
                //return res;
            } else if(res == 0)
            {
                // Timeout
            } else
            {
                res = write(mFd, data + written, length);
                if (res >= 0)
                {
                    length -= res;
                    written += res;
                } else
                {
                    qCritical().nospace() << "Writing to serial port failed (" << res << "), ignoring";
                    //return res;
                }
            }
            
            if (!mIsOpen)
            {
                qCritical() << "Serial port closed during write";
                return -2;
            }
        }
        return written;
    } else
    {
        return write(mFd, data, length);
    }
}