You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

serial-util.c 4.5 KiB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202
  1. /*
  2. * Serial I/O helper routines
  3. *
  4. * Jim Paris <jim@jtan.com>
  5. * $Id$
  6. */
  7. #include <stdio.h>
  8. #include <stdlib.h>
  9. #include <termio.h>
  10. #include <sys/types.h>
  11. #include <sys/ioctl.h>
  12. #include <errno.h>
  13. #include <unistd.h>
  14. #include <string.h>
  15. #include <fcntl.h>
  16. #include <err.h>
  17. #include <linux/serial.h>
  18. #include <poll.h>
  19. #include <stdarg.h>
  20. #include "serial-util.h"
  21. static int rate_to_constant(int baudrate) {
  22. #define B(x) case x: return B##x
  23. switch(baudrate) {
  24. B(50); B(75); B(110); B(134); B(150);
  25. B(200); B(300); B(600); B(1200); B(1800);
  26. B(2400); B(4800); B(9600); B(19200); B(38400);
  27. B(57600); B(115200); B(230400); B(460800); B(500000);
  28. B(576000); B(921600); B(1000000);B(1152000);B(1500000);
  29. default: return 0;
  30. }
  31. #undef B
  32. }
  33. /* Open serial port in raw mode, with custom baudrate if necessary */
  34. int serial_open(const char *device, int rate)
  35. {
  36. struct termios options;
  37. struct serial_struct serinfo;
  38. int fd;
  39. int speed = 0;
  40. /* Open and configure serial port */
  41. if ((fd = open(device,O_RDWR|O_NOCTTY)) == -1)
  42. return -1;
  43. speed = rate_to_constant(rate);
  44. if (speed == 0) {
  45. serinfo.reserved_char[0] = 0;
  46. if (ioctl(fd, TIOCGSERIAL, &serinfo) < 0)
  47. return -1;
  48. serinfo.flags &= ~ASYNC_SPD_MASK;
  49. serinfo.flags |= ASYNC_SPD_CUST;
  50. serinfo.custom_divisor = (serinfo.baud_base + (rate / 2)) / rate;
  51. if (serinfo.custom_divisor < 1)
  52. serinfo.custom_divisor = 1;
  53. if (ioctl(fd, TIOCSSERIAL, &serinfo) < 0)
  54. return -1;
  55. if (ioctl(fd, TIOCGSERIAL, &serinfo) < 0)
  56. return -1;
  57. if (serinfo.custom_divisor * rate != serinfo.baud_base) {
  58. warnx("actual baudrate is %d / %d = %f",
  59. serinfo.baud_base, serinfo.custom_divisor,
  60. (float)serinfo.baud_base / serinfo.custom_divisor);
  61. }
  62. }
  63. fcntl(fd, F_SETFL, 0);
  64. tcgetattr(fd, &options);
  65. cfsetispeed(&options, speed ?: B38400);
  66. cfsetospeed(&options, speed ?: B38400);
  67. cfmakeraw(&options);
  68. options.c_cflag |= (CLOCAL | CREAD);
  69. options.c_cflag &= ~CRTSCTS;
  70. if (tcsetattr(fd, TCSANOW, &options) != 0)
  71. return -1;
  72. return fd;
  73. }
  74. /* Like read(), but restarts after EINTR, and reads until count bytes
  75. are received or a timeout occurs. */
  76. int saferead_timeout(int fd, void *buf, size_t count, int timeout_ms)
  77. {
  78. struct pollfd pfd;
  79. int r;
  80. size_t nread = 0;
  81. while (count > 0) {
  82. pfd.fd = fd;
  83. pfd.events = POLLIN;
  84. r = poll(&pfd, 1, timeout_ms);
  85. if (r < 0 && errno == EINTR) /* retry */
  86. continue;
  87. else if (r == 0) /* timeout */
  88. return nread;
  89. else if (r == 1 && (pfd.revents & POLLIN)) { /* readable */
  90. r = read(fd, buf, count);
  91. if (r < 0 && errno == EINTR) /* retry */
  92. continue;
  93. if (r < 0) /* error */
  94. return r;
  95. if (r == 0) /* EOF */
  96. return nread;
  97. buf = (char *) buf + r;
  98. count -= r;
  99. nread += r;
  100. } else {
  101. /* error */
  102. return -1;
  103. }
  104. }
  105. return nread;
  106. }
  107. /* Like write(), but restarts after EINTR */
  108. ssize_t safewrite(int fd, const void *buf, size_t count)
  109. {
  110. size_t nwritten = 0;
  111. while (count > 0) {
  112. ssize_t r = write(fd, buf, count);
  113. if (r < 0 && errno == EINTR)
  114. continue;
  115. if (r < 0)
  116. return r;
  117. if (r == 0)
  118. return nwritten;
  119. buf = (const char *)buf + r;
  120. count -= r;
  121. nwritten += r;
  122. }
  123. return nwritten;
  124. }
  125. /* Read bytes until no more are available for specified time */
  126. int drain_timeout(int fd, int msec)
  127. {
  128. char buf[1024];
  129. int ret;
  130. while (1) {
  131. ret = saferead_timeout(fd, buf, sizeof(buf), msec);
  132. if (ret <= 0)
  133. return ret;
  134. }
  135. }
  136. /* Like fprintf, but to a fd, using safewrite. */
  137. static int vfdprintf(int fd, const char *fmt, va_list args)
  138. {
  139. static char buf[1024];
  140. vsnprintf(buf, sizeof(buf), fmt, args);
  141. return safewrite(fd, buf, strlen(buf));
  142. }
  143. int fdprintf(int fd, const char *fmt, ...)
  144. {
  145. int ret;
  146. va_list args;
  147. va_start(args, fmt);
  148. ret = vfdprintf(fd, fmt, args);
  149. va_end(args);
  150. return ret;
  151. }
  152. /* Like fgets, but from a fd, using saferead_timeout. */
  153. char *fdgets(char *s, int size, int fd, int timeout_ms)
  154. {
  155. int ret;
  156. int nread = 0;
  157. /* Not very efficient; needs to read one char at a time to
  158. * avoid buffering */
  159. while (nread < (size - 1)) {
  160. ret = saferead_timeout(fd, &s[nread], 1, timeout_ms);
  161. if (ret <= 0) {
  162. s[nread] = '\0';
  163. return NULL;
  164. }
  165. if (ret == 1) {
  166. nread++;
  167. /* found terminator? */
  168. if (s[nread-1] == '\n')
  169. break;
  170. }
  171. }
  172. s[nread] = '\0';
  173. return s;
  174. }
  175. /* Like perl chomp. */
  176. void chomp(char *s)
  177. {
  178. int len = strlen(s);
  179. /* do it twice to remove \r\n as well */
  180. if (len > 1 && (s[len - 1] == '\r' || s[len - 1] == '\n'))
  181. s[--len] = '\0';
  182. if (len > 1 && (s[len - 1] == '\r' || s[len - 1] == '\n'))
  183. s[--len] = '\0';
  184. }