diff options
Diffstat (limited to 'contrib/motosend.c')
-rw-r--r-- | contrib/motosend.c | 209 |
1 files changed, 209 insertions, 0 deletions
diff --git a/contrib/motosend.c b/contrib/motosend.c new file mode 100644 index 00000000..e990338d --- /dev/null +++ b/contrib/motosend.c @@ -0,0 +1,209 @@ +/* $Id$ */ +#include <sys/types.h> +#include <machine/endian.h> +#include <sys/limits.h> +#include <err.h> +#include <errno.h> +#include <fcntl.h> +#include <stdarg.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <termios.h> +#include <unistd.h> + +/* + * @@Cj - receiver ID + * @@Be 0 - almanac dump + */ + +static int moto_send(int , char *, char *); +static char moto_gen_checksum(char *, int); +char *gpsd_hexdump(char *, size_t); +int gpsd_hexpack(char *, char *, int); +int hex2bin(char *s); + +#define BSIZ 64 +int main(int argc, char **argv) { + int speed, l, fd, n; + struct termios term; + char buf[BSIZ]; + time_t s, t; + + if (argc != 5){ + fprintf(stderr, "usage: motosend <speed> <port> msgtype moto-body-hex\n"); + return 1; + } + + if ((l = strlen(argv[4])) > 2*USHRT_MAX){ + fprintf(stderr, "oversized message\n"); + return 1; + } + + if (l % 2) { + fprintf(stderr, "body must have an even number of hex digits\n"); + return 1; + } + + speed = atoi(argv[1]); + switch (speed) { + case 230400: + case 115200: + case 57600: + case 38400: + case 28800: + case 14400: + case 9600: + case 4800: + break; + default: + fprintf(stderr, "invalid speed\n"); + return 1; + } + + if ((fd = open(argv[2], O_RDWR | O_NONBLOCK | O_NOCTTY, 0644)) == -1) + err(1, "open"); + + tcgetattr(fd, &term); + cfmakeraw(&term); + cfsetospeed(&term, speed); + cfsetispeed(&term, speed); + term.c_cc[VMIN] = 8; + term.c_cc[VTIME] = 1; + term.c_cflag &= ~(PARENB | PARODD | CRTSCTS); + term.c_cflag |= CREAD | CLOCAL; + term.c_iflag = term.c_oflag = term.c_lflag = (tcflag_t) 0; + + if (tcsetattr(fd, TCSANOW | TCSAFLUSH, &term) == -1) + err(1, "tcsetattr"); + + tcflush(fd, TCIOFLUSH); + t = 0; n = 0; + while (1){ + usleep(1000); + bzero(buf, BSIZ); + if ((l = read(fd, buf, BSIZ)) == -1) + if (!(EINTR == errno || EAGAIN == errno)) + err(1, "read"); + + if (l > 0){ + printf("%s", gpsd_hexdump(buf, l)); + fflush(stdout); + } + /* allow for up to "n" resends, once per second */ + if (((s = time(NULL)) > t) && (n < 1)){ + t = s; + n++; + moto_send(fd, argv[3], argv[4]); + } + } + return 0; +} + +char moto_gen_checksum(char *buf, int len){ + int n; + char ck = '\0'; + + for (n = 0; n < len; n++) + ck ^= buf[n]; + return ck; + +} + +static int moto_send(int fd, char *type, char *body ) { + size_t status; + char *buf; + unsigned short l, n, ck; + + l = strlen(body) / 2; + if ((buf = malloc(l+7)) == NULL) + return -1; + + bzero(buf, l+7); + buf[0] = '@'; buf[1] = '@'; + buf[2] = type[0]; buf[3] = type[1]; + + if (l) + if (gpsd_hexpack(body, buf+4, l) == -1){ + free(buf); + return -1; + } + + buf[l+4] = moto_gen_checksum(buf+2, l+2); + buf[l+5] = '\r'; buf[l+6] = '\n'; + + status = write(fd, buf, l+7); + if (status == -1) + perror("moto_send"); + return (int)status; +} + +static char last; +char *gpsd_hexdump(char *binbuf, size_t binbuflen) +{ + static char hexbuf[USHRT_MAX*2+10+2]; + size_t i, j = 0; + size_t len = (size_t)binbuflen; + const char *ibuf = (const char *)binbuf; + const char *hexchar = "0123456789abcdef"; + + for (i = 0; i < len; i++) { + if (ibuf[i] == '@' && (ibuf[i+1] == '@' || last == '@')){ + hexbuf[j++] = '\n'; + hexbuf[j++] = ibuf[i++]; + hexbuf[j++] = ibuf[i++]; + hexbuf[j++] = ibuf[i++]; + hexbuf[j++] = ibuf[i++]; + } else { + hexbuf[j++] = hexchar[ (ibuf[i]&0xf0)>>4 ]; + hexbuf[j++] = hexchar[ ibuf[i]&0x0f ]; + } + last = ibuf[i]; + } + hexbuf[j] ='\0'; + return hexbuf; +} + +int gpsd_hexpack(char *src, char *dst, int len){ + int i, k, l; + + l = (int)(strlen(src) / 2); + if ((l < 1) || (l > len)) + return -1; + + bzero(dst, len); + for (i = 0; i < l; i++) + if ((k = hex2bin(src+i*2)) != -1) + dst[i] = (char)(k & 0xff); + else + return -1; + return l; +} + +int hex2bin(char *s) +{ + int a, b; + + a = s[0] & 0xff; + b = s[1] & 0xff; + + if ((a >= 'a') && (a <= 'f')) + a = a + 10 - 'a'; + else if ((a >= 'A') && (a <= 'F')) + a = a + 10 - 'A'; + else if ((a >= '0') && (a <= '9')) + a -= '0'; + else + return -1; + + if ((b >= 'a') && (b <= 'f')) + b = b + 10 - 'a'; + else if ((b >= 'A') && (b <= 'F')) + b = b + 10 - 'A'; + else if ((b >= '0') && (b <= '9')) + b -= '0'; + else + return -1; + + return ((a<<4) + b); +} |