Devuan fork of gpsd
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.
 
 
 
 
 
 

222 lines
4.2 KiB

  1. /*
  2. * This file is Copyright (c) 2010-2018 by the GPSD project
  3. * SPDX-License-Identifier: BSD-2-clause
  4. */
  5. #include <err.h>
  6. #include <errno.h>
  7. #include <fcntl.h>
  8. #include <limits.h>
  9. #include <stdarg.h>
  10. #include <stdio.h>
  11. #include <stdlib.h>
  12. #include <string.h>
  13. #include <sys/types.h>
  14. #include <termios.h>
  15. #include <time.h> /* For nanosleep() and time() */
  16. #include <unistd.h>
  17. /*
  18. * @@Cj - receiver ID
  19. * @@Be 0 - almanac dump
  20. */
  21. static int moto_send(int , char *, char *);
  22. static char moto_gen_checksum(char *, int);
  23. char *gpsd_hexdump(char *, size_t);
  24. int gpsd_hexpack(char *, char *, int);
  25. int hex2bin(char *s);
  26. #define BSIZ 64
  27. int main(int argc, char **argv) {
  28. int speed, l, fd, n;
  29. struct termios term;
  30. char buf[BSIZ];
  31. time_t t;
  32. struct timespec delay;
  33. if (argc != 5){
  34. fprintf(stderr, "usage: motosend <speed> <port> msgtype moto-body-hex\n");
  35. return 1;
  36. }
  37. if ((l = strlen(argv[4])) > 2*USHRT_MAX){
  38. fprintf(stderr, "oversized message\n");
  39. return 1;
  40. }
  41. if (l % 2) {
  42. fprintf(stderr, "body must have an even number of hex digits\n");
  43. return 1;
  44. }
  45. speed = atoi(argv[1]);
  46. switch (speed) {
  47. case 230400:
  48. case 115200:
  49. case 57600:
  50. case 38400:
  51. case 28800:
  52. case 14400:
  53. case 9600:
  54. case 4800:
  55. break;
  56. default:
  57. fprintf(stderr, "invalid speed\n");
  58. return 1;
  59. }
  60. if ((fd = open(argv[2], O_RDWR | O_NONBLOCK | O_NOCTTY, 0644)) == -1)
  61. err(1, "open");
  62. tcgetattr(fd, &term);
  63. cfmakeraw(&term);
  64. cfsetospeed(&term, speed);
  65. cfsetispeed(&term, speed);
  66. term.c_cc[VMIN] = 8;
  67. term.c_cc[VTIME] = 1;
  68. term.c_cflag &= ~(PARENB | PARODD | CRTSCTS);
  69. term.c_cflag |= CREAD | CLOCAL;
  70. term.c_iflag = term.c_oflag = term.c_lflag = (tcflag_t) 0;
  71. if (tcsetattr(fd, TCSANOW | TCSAFLUSH, &term) == -1)
  72. err(1, "tcsetattr");
  73. tcflush(fd, TCIOFLUSH);
  74. t = 0; n = 0;
  75. while (1){
  76. time_t s;
  77. /* wait 1,000 uSec */
  78. delay.tv_sec = 0;
  79. delay.tv_nsec = 1000000L;
  80. nanosleep(&delay, NULL);
  81. memset(buf, 0, BSIZ);
  82. if ((l = read(fd, buf, BSIZ)) == -1)
  83. if (!(EINTR == errno || EAGAIN == errno))
  84. err(1, "read");
  85. if (l > 0){
  86. printf("%s", gpsd_hexdump(buf, l));
  87. fflush(stdout);
  88. }
  89. /* allow for up to "n" resends, once per second */
  90. if (((s = time(NULL)) > t) && (n < 1)){
  91. t = s;
  92. n++;
  93. moto_send(fd, argv[3], argv[4]);
  94. }
  95. }
  96. return 0;
  97. }
  98. char moto_gen_checksum(char *buf, int len){
  99. int n;
  100. char ck = '\0';
  101. for (n = 0; n < len; n++)
  102. ck ^= buf[n];
  103. return ck;
  104. }
  105. static int moto_send(int fd, char *type, char *body ) {
  106. size_t status;
  107. char *buf;
  108. unsigned short l;
  109. l = strlen(body) / 2;
  110. if ((buf = malloc(l+7)) == NULL)
  111. return -1;
  112. memset(buf, 0, l+7);
  113. buf[0] = '@'; buf[1] = '@';
  114. buf[2] = type[0]; buf[3] = type[1];
  115. if (l)
  116. if (gpsd_hexpack(body, buf+4, l) == -1){
  117. free(buf);
  118. return -1;
  119. }
  120. buf[l+4] = moto_gen_checksum(buf+2, l+2);
  121. buf[l+5] = '\r'; buf[l+6] = '\n';
  122. status = write(fd, buf, l+7);
  123. if (status == -1)
  124. perror("moto_send");
  125. return (int)status;
  126. }
  127. static char last;
  128. char *gpsd_hexdump(char *binbuf, size_t binbuflen)
  129. {
  130. static char hexbuf[USHRT_MAX*2+10+2];
  131. size_t i, j = 0;
  132. size_t len = (size_t)binbuflen;
  133. const char *ibuf = (const char *)binbuf;
  134. const char *hexchar = "0123456789abcdef";
  135. for (i = 0; i < len; i++) {
  136. if (ibuf[i] == '@' && (ibuf[i+1] == '@' || last == '@')){
  137. hexbuf[j++] = '\n';
  138. hexbuf[j++] = ibuf[i++];
  139. hexbuf[j++] = ibuf[i++];
  140. hexbuf[j++] = ibuf[i++];
  141. hexbuf[j++] = ibuf[i++];
  142. } else {
  143. hexbuf[j++] = hexchar[ (ibuf[i]&0xf0)>>4 ];
  144. hexbuf[j++] = hexchar[ ibuf[i]&0x0f ];
  145. }
  146. last = ibuf[i];
  147. }
  148. hexbuf[j] ='\0';
  149. return hexbuf;
  150. }
  151. int gpsd_hexpack(char *src, char *dst, int len)
  152. {
  153. int i, l;
  154. l = (int)(strlen(src) / 2);
  155. if ((l < 1) || (l > len))
  156. return -1;
  157. memset(dst, 0, len);
  158. for (i = 0; i < l; i++) {
  159. int k;
  160. if ((k = hex2bin(src+i*2)) != -1)
  161. dst[i] = (char)(k & 0xff);
  162. else
  163. return -1;
  164. }
  165. return l;
  166. }
  167. int hex2bin(char *s)
  168. {
  169. int a, b;
  170. a = s[0] & 0xff;
  171. b = s[1] & 0xff;
  172. if ((a >= 'a') && (a <= 'f'))
  173. a = a + 10 - 'a';
  174. else if ((a >= 'A') && (a <= 'F'))
  175. a = a + 10 - 'A';
  176. else if ((a >= '0') && (a <= '9'))
  177. a -= '0';
  178. else
  179. return -1;
  180. if ((b >= 'a') && (b <= 'f'))
  181. b = b + 10 - 'a';
  182. else if ((b >= 'A') && (b <= 'F'))
  183. b = b + 10 - 'A';
  184. else if ((b >= '0') && (b <= '9'))
  185. b -= '0';
  186. else
  187. return -1;
  188. return ((a<<4) + b);
  189. }