|
- /*
- * 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';
- }
|