zoom/pc/serial-util.c

/*
 * Serial I/O helper routines
 *
 * Jim Paris <jim@jtan.com>
 * $Id$
 */

#include <stdio.h>
#include <stdlib.h>
#include <termio.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <unistd.h>
#include <string.h>
#include <fcntl.h>
#include <err.h>
#include <linux/serial.h>
#include <poll.h>
#include <stdarg.h>
#include "serial-util.h"

static int rate_to_constant(int baudrate) {
#define B(x) case x: return B##x
	switch(baudrate) {
		B(50);     B(75);     B(110);    B(134);    B(150);
		B(200);    B(300);    B(600);    B(1200);   B(1800);
		B(2400);   B(4800);   B(9600);   B(19200);  B(38400);
		B(57600);  B(115200); B(230400); B(460800); B(500000); 
		B(576000); B(921600); B(1000000);B(1152000);B(1500000); 
	default: return 0;
	}
#undef B
}    

/* Open serial port in raw mode, with custom baudrate if necessary */
int serial_open(const char *device, int rate)
{
	struct termios options;
	struct serial_struct serinfo;
	int fd;
	int speed = 0;

	/* Open and configure serial port */
	if ((fd = open(device,O_RDWR|O_NOCTTY)) == -1)
		return -1;

	speed = rate_to_constant(rate);

	if (speed == 0) {
		serinfo.reserved_char[0] = 0;
		if (ioctl(fd, TIOCGSERIAL, &serinfo) < 0)
			return -1;
		serinfo.flags &= ~ASYNC_SPD_MASK;
		serinfo.flags |= ASYNC_SPD_CUST;
		serinfo.custom_divisor = (serinfo.baud_base + (rate / 2)) / rate;
		if (serinfo.custom_divisor < 1) 
			serinfo.custom_divisor = 1;
		if (ioctl(fd, TIOCSSERIAL, &serinfo) < 0)
			return -1;
		if (ioctl(fd, TIOCGSERIAL, &serinfo) < 0)
			return -1;
		if (serinfo.custom_divisor * rate != serinfo.baud_base) {
			warnx("actual baudrate is %d / %d = %f",
			      serinfo.baud_base, serinfo.custom_divisor,
			      (float)serinfo.baud_base / serinfo.custom_divisor);
		}
	}

	fcntl(fd, F_SETFL, 0);
	tcgetattr(fd, &options);
	cfsetispeed(&options, speed ?: B38400);
	cfsetospeed(&options, speed ?: B38400);
	cfmakeraw(&options);
	options.c_cflag |= (CLOCAL | CREAD);
	options.c_cflag &= ~CRTSCTS;
	if (tcsetattr(fd, TCSANOW, &options) != 0)
		return -1;

	return fd;
}

/* Like read(), but restarts after EINTR, and reads until count bytes
   are received or a timeout occurs. */
int saferead_timeout(int fd, void *buf, size_t count, int timeout_ms)
{
	struct pollfd pfd;
	int r;
	size_t nread = 0;

	while (count > 0) {
		pfd.fd = fd;
		pfd.events = POLLIN;
		r = poll(&pfd, 1, timeout_ms);
		if (r < 0 && errno == EINTR) /* retry */
			continue;
		else if (r == 0) /* timeout */
			return nread;
		else if (r == 1 && (pfd.revents & POLLIN)) { /* readable */
			r = read(fd, buf, count);
			if (r < 0 && errno == EINTR) /* retry */
				continue;
			if (r < 0) /* error */
				return r;
			if (r == 0) /* EOF */
				return nread;
			buf = (char *) buf + r;
			count -= r;
			nread += r;
		} else {
			/* error */
			return -1;
		}
	}
	return nread;
}

/* Like write(), but restarts after EINTR */
ssize_t safewrite(int fd, const void *buf, size_t count)
{
	size_t nwritten = 0;
	while (count > 0) {
		ssize_t r = write(fd, buf, count);
 
		if (r < 0 && errno == EINTR)
			continue;
		if (r < 0)
			return r;
		if (r == 0)
			return nwritten;
		buf = (const char *)buf + r;
		count -= r;
		nwritten += r;
	}
	return nwritten;
}

/* Read bytes until no more are available for specified time */
int drain_timeout(int fd, int msec)
{
	char buf[1024];
	int ret;
	while (1) {
		ret = saferead_timeout(fd, buf, sizeof(buf), msec);
		if (ret <= 0) 
			return ret;
	}	
}

/* Like fprintf, but to a fd, using safewrite. */
static int vfdprintf(int fd, const char *fmt, va_list args)
{
	static char buf[1024];
	vsnprintf(buf, sizeof(buf), fmt, args);
	return safewrite(fd, buf, strlen(buf));
}
int fdprintf(int fd, const char *fmt, ...)
{
	int ret;
	va_list args;
	va_start(args, fmt);
	ret = vfdprintf(fd, fmt, args);
	va_end(args);
	return ret;
}

/* Like fgets, but from a fd, using saferead_timeout. */
char *fdgets(char *s, int size, int fd, int timeout_ms)
{
	int ret;
	int nread = 0;

	/* Not very efficient, needs to read one char at a time to avoid buffering */
	while (nread < (size - 1)) {
		ret = saferead_timeout(fd, &s[nread], 1, timeout_ms);
		if (ret <= 0) {
			s[nread] = '\0';
			return NULL;
		}
		if (ret == 1) {
			nread++;

			/* found terminator? */
			if (s[nread-1] == '\n')
				break;
		}
	}
	s[nread] = '\0';
	return s;
}

/* Like perl chomp. */
void chomp(char *s)
{
	int len = strlen(s);
	/* do it twice to remove \r\n as well */
	if (len > 1 && (s[len - 1] == '\r' || s[len - 1] == '\n')) s[--len] = '\0';
	if (len > 1 && (s[len - 1] == '\r' || s[len - 1] == '\n')) s[--len] = '\0';
}