rendered paste body#include <fcntl.h>#include <sys/ioctl.h>#include <stdio.h>#include <stdlib.h> /* exit */#include <termios.h>#include <unistd.h>#include <ctype.h>#include <sys/time.h> /* timeval */#include <readline/readline.h>#include <readline/history.h>#define GPS_DEV "/dev/ttySAC1"#define GPS_SYNC "\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80"#define GPS_INIT "\xff\x00\xfd\xc0\x00\xfc"/* turn a gta01-pm-gps.0 pin on or off */int sysfs_gps_pin(char *name, int value){ static char device_path[512] = "/sys/bus/platform/devices/gta01-pm-gps.0/"; strcpy(&device_path[41], name); int fd; fd = open(device_path, O_WRONLY); if (fd < 0) return fd; write(fd, value ? "1" : "0", 2); close(fd);}/* power on sequence for hammerhead on GTA01 */void power_on_seq(void){ // reset 0 // pwron 0 sysfs_gps_pin("power_lp_io_3v3", 1); sysfs_gps_pin("power_lp_io_3v3", 1); sysfs_gps_pin("reset", 1); usleep(1000); sysfs_gps_pin("power_pll_core_2v5", 1); sysfs_gps_pin("power_avdd_3v", 1); sysfs_gps_pin("power_vdd_core_1v5", 1); // power_core_1v5 usleep(1000); sysfs_gps_pin("pwron", 1); usleep(15900);}/* power off sequence for hammerhead on GTA01 */void power_off_seq(void){ sysfs_gps_pin("power_pll_core_2v5", 0); sysfs_gps_pin("power_avdd_3v", 0); sysfs_gps_pin("power_vdd_core_1v5", 0); // power_core_1v5 sysfs_gps_pin("pwron", 0);}/* return the byte stream in 8bit hex representation */char *hexstr(unsigned char *data, int size){ static char *hex = "0123456789abcdef"; unsigned char *s = malloc(size*3); int i = 0; while (i < size) { *s++ = hex[data[i]>>4]; *s++ = hex[data[i]&0xf]; *s++ = ' '; i++; } *--s = '\0';}/* return the number of hex digits until the first occurence of a non-digit or end of string */int digits(char *s){ int i = 0; while isxdigit(*s++) i++; return i;}/* return the decimal value of a hex char */int unhexchar(char c){ if (c < '0') return -1; if (c <= '9') return c-'0'; if (c < 'A') return -1; if (c <= 'F') return c-'A'+10; if (c < 'a') return -1; if (c <= 'f') return c-'a'+10; return -1;}/* convert a hex string into a byte stream. each word (separated by whitespace) is parsed as a little-endian number of 8,16,24 or 32bit */char *unhexstr(char *s, int *size){ int len = 0; char *ret; char *p = s; if (!size) return NULL; while (*p) { int d = digits(p); if (d == 0) { if (!isblank(*p++)) return NULL; } else if (d <= 8) { len += (d+1)>>1; p += d; } else return NULL; } *size = len; p = ret = malloc(len); while (*s) { if (isblank(*s)) s++; else { int d = digits(s); switch (d) { case 1: *p++ = unhexchar(s[0]); break; case 2: *p++ = unhexchar(s[0])<<4|unhexchar(s[1]); break; case 3: *p++ = unhexchar(s[1])<<4|unhexchar(s[2]); *p++ = unhexchar(s[0]); break; case 4: *p++ = unhexchar(s[2])<<4|unhexchar(s[3]); *p++ = unhexchar(s[0])<<4|unhexchar(s[1]); break; case 5: *p++ = unhexchar(s[3])<<4|unhexchar(s[4]); *p++ = unhexchar(s[1])<<4|unhexchar(s[2]); *p++ = unhexchar(s[0]); break; case 6: *p++ = unhexchar(s[4])<<4|unhexchar(s[5]); *p++ = unhexchar(s[2])<<4|unhexchar(s[3]); *p++ = unhexchar(s[0])<<4|unhexchar(s[1]); break; case 7: *p++ = unhexchar(s[5])<<4|unhexchar(s[6]); *p++ = unhexchar(s[3])<<4|unhexchar(s[4]); *p++ = unhexchar(s[1])<<4|unhexchar(s[2]); *p++ = unhexchar(s[0]); case 8: *p++ = unhexchar(s[6])<<4|unhexchar(s[7]); *p++ = unhexchar(s[4])<<4|unhexchar(s[5]); *p++ = unhexchar(s[2])<<4|unhexchar(s[3]); *p++ = unhexchar(s[0])<<4|unhexchar(s[1]); } s += d; } } return ret;}/* change the output of decode.py's SEND lines into a packet (string of hex digits that can be consumed by unhexstr) */char *parse_sendcmd(char *cmd){ char *p; if (!strncmp(cmd,"SEND",4)) { cmd[0] = 'f'; cmd[1] = 'e'; //} else if (!strncmp(cmd, "CMD ",4)) { // cmd[0] = 'f'; // cmd[1] = 'f'; } else { printf("only SEND supported\n"); return NULL; } cmd[2] = ' '; cmd[3] = ' '; if (!((cmd[5] == 'L') && isxdigit(cmd[7]) && isxdigit(cmd[8]))) { printf("L parameter missing\n"); return NULL; } cmd[5] = ' '; if (!((cmd[10] == 'C' && cmd[11] == 'H') && isxdigit(cmd[13]))) { printf("CH parameter missing\n"); return NULL; } cmd[10] = 'f'; cmd[11] = 'd'; if (!((cmd[15] == 'F' && cmd[16] == 'L') && isxdigit(cmd[18]))) { printf("FL parameter missing\n"); return NULL; } cmd[15] = cmd[18]; cmd[16] = cmd[13]; cmd[13] = ' '; cmd[18] = ' '; if (!((cmd[20] == 'C' && cmd[21] == 'M' && cmd[22] == 'D') && isxdigit(cmd[24]) && isxdigit(cmd[25]))) { printf("CMD parameter missing\n"); return NULL; } cmd[20] = cmd[24]; cmd[21] = cmd[25]; cmd[22] = ' '; if (cmd[26] != ':') { printf("':' missing\n"); return NULL; } p = cmd+23; while (p[4]) { *p++ = *(p+4); } *p++ = ' '; *p++ = 'f'; *p++ = 'c'; *p = 0; return cmd;}/* rudimentary packet validation */int check_packet(char *p, int len){ int L, end; if (len < 6) { printf("too small\n"); return 0; } if ((p[0]|1) != 0xff) { printf("must start with fe or ff\n"); return 0; } if (p[2] != 0xfd) { printf("third byte must be fd\n"); return 0; } L = p[1]; if (p[0] & 1) end = 5; else end = 5+4+(L<<2); if (end >= len) { printf("packet with L=%d must be %d bytes", L, end+1); return 0; } if (p[end] != 0xfc) { printf("must end with fc (at %d)\n", end); return 0; } return 1;}/* send a packet over the uart and dump it to stdout */void send(int fd, char *data, int size){ char *s = hexstr(data, size); int n; printf("SEND(%d) %s\n", size, s); free(s); n = write(fd, data, size);}int recv_packet(int fd, char *data, int size){ int n; fd_set readset; struct timeval tv; FD_ZERO(&readset); FD_SET(fd, &readset); tv.tv_sec = 0; tv.tv_usec = 100000; select(fd+1, &readset, NULL, NULL, &tv); n = read(fd, data, size); if (n < 0) return n; while (n < 6) { select(fd+1, &readset, NULL, NULL, NULL); n += read(fd, data+n, size-n); } while (n < (10+data[1]*4)) { select(fd+1, &readset, NULL, NULL, NULL); n += read(fd, data+n, size-n); } return n;}int main(int argc, char *argv[]){ int fd; struct termios t; fd_set readset; unsigned char buf[512]; unsigned char *p; int n; FD_ZERO(&readset); printf("power on\n"); power_on_seq(); fd = open(GPS_DEV, O_RDWR | O_NONBLOCK); if (fd < 0) { printf("Couldn't open %s\n", GPS_DEV); exit(1); } FD_SET(fd, &readset); ioctl(fd, TCGETS, &t); cfsetispeed(&t, B115200); cfsetospeed(&t, B115200); cfmakeraw(&t); ioctl(fd, TCSETS, &t); send(fd, GPS_SYNC, 16); send(fd, GPS_INIT, 6); n = select(fd+1, &readset, NULL, NULL, NULL); n = read(fd, buf, sizeof(buf)); printf("RECV(%d) ", n); p = buf; while (n-- > 0) printf("%02x ", *p++); printf("\n"); while (1) { char *packet; char *cmd = readline("> "); if (!cmd) break; add_history(cmd); if (!strncmp(cmd, "SEND ", 5)) { packet = parse_sendcmd(cmd); //printf("'%s'\n", packet); packet = unhexstr(packet, &n); } else packet = unhexstr(cmd, &n); if (packet) { if (check_packet(packet, n)) { send(fd, packet, n); /* dump received packets */ n = recv_packet(fd, buf, sizeof(buf)); if (n >= 0) { printf("RECV(%d) ", n); p = buf; while (n-- > 0) printf("%02x ", *p++); printf("\n"); } else printf("no reply\n"); } else printf("invalid packet\n"); free(packet); } else printf("parse error\n"); free(cmd); } printf("power off\n"); power_off_seq();}