20105ca7ec61d05b647ee5c870362d87684dff9b
[gps] / sirf.c
1 /*
2   Author : Benoit Papillault <benoit.papillault@free.fr>
3   Creation : 2011-07-03
4   Goal : Understanding SiRF protocol for GPS devices
5 */
6
7 #include <stdio.h>
8 #include <sys/select.h>
9 #include <sys/types.h>
10 #include <sys/stat.h>
11 #include <fcntl.h>
12 #include <termios.h>
13 #include <unistd.h>
14 #include <string.h>
15
16 /* return 1 if message is a valid SiRF message, 0 if invalid */
17
18 int check_sirf_msg(unsigned char *buf, int n) {
19   int length, crc, computed_crc, i;
20
21   /* check size (at least 8 bytes) */
22   if (n<8) {
23     printf("msg too small (%d bytes)\n", n);
24     return 0;
25   }
26
27   /* check start pattern */
28   if ((buf[0] != 0xa0) || (buf[1] != 0xa2)) {
29     printf("invalid start pattern : 0x%02x 0x%02x\n", buf[0], buf[1]);
30     return 0;
31   }
32   
33   /* check length */
34   length = ((buf[2]<<8) | (buf[3]));
35   if (length & 0x8000) {
36     printf("invalid length : 0x%04x bytes\n", length);
37     return 0;
38   }
39
40   if (length > 912) {
41     printf("warning : length (%d bytes) is above 912\n", length);
42   }
43
44   if (4 + length + 4 != n) {
45     printf("invalid length : %d bytes, buf buffer is %d bytes\n", length, n);
46     return 0;
47   }
48   
49   /* check checksum */
50   crc = ((buf[n-4]<<8) | (buf[n-3]));
51   if (crc & 0x8000) {
52     printf("invalid crc : 0x%04x\n", crc);
53     return 0;
54   }
55
56   computed_crc = 0;
57   for (i=4; i<n-4; i++) {
58     computed_crc += buf[i];
59   }
60   computed_crc &= 0x7fff;
61
62   if (computed_crc != crc) {
63     printf("invalid crc : 0x%04x computed crc : 0x%04x\n", crc, computed_crc);
64     return 0;
65   }
66   
67   /* check stop pattern */
68   if ((buf[n-2] != 0xb0) || (buf[n-1] != 0xb3)) {
69     printf("invalid stop pattern : 0x%02x 0x%02x\n", buf[n-2], buf[n-1]);
70     return 0;
71   }
72
73   printf("valid message (%d bytes)\n", length);
74   return 1;
75 }
76
77 int do_read(int fd) {
78   unsigned char buffer[912]; /* SiRF message size is limited to 912 bytes */
79   int i, j, n;
80
81   static unsigned char sbuffer[912];
82   static int p = 0;
83
84   sleep(1);
85   n = read(fd, buffer, sizeof(buffer));
86   if (n > 0) {
87     printf("%d bytes read :", n);
88     for (i=0; i<n; i++) {
89       printf(" 0x%02x", buffer[i]);
90     }
91     printf("\n");
92
93     if (p + n < sizeof(sbuffer)) {
94       memcpy(sbuffer + p, buffer, n);
95       p += n;
96     } else {
97       printf("too much data received! (p=%d, n=%d)\n", p, n);
98     }
99
100     /*
101     printf("%d bytes received :", p);
102     for (i=0; i<p; i++) {
103       printf(" 0x%02x", sbuffer[i]);
104     }
105     printf("\n");
106     */
107     /* small parsing :
108        - start pattern = 0xa0 0xa2
109        - stop  pattern = 0xb0 0xb3
110
111        SiRF message format is :
112        <0xa0 0xa2><length (2 bytes)><length bytes><CRC (2 byte)><0xb0 0xb3>
113     */
114
115     for (;;) {
116       int start_found = 0, stop_found = 0;
117
118       /* search for start pattern */
119       for (i=0; i<p-1; i++) {
120         if ((sbuffer[i] == 0xa0) && (sbuffer[i+1] == 0xa2)) {
121           start_found = 1;
122           printf("start pattern found at offset %d\n", i);
123
124           if (i>0) {
125             printf("message: %d bytes skipped\n", i);
126             memmove(sbuffer, sbuffer+i, p-i);
127             p -= i;
128           }
129           break;
130         }
131       }
132
133       /* search for stop pattern */
134       for (i=0; i<p-1; i++) {
135         if ((sbuffer[i] == 0xb0) && (sbuffer[i+1] == 0xb3)) {
136           stop_found = 1;
137           printf("stop pattern found at offset %d\n", i);
138           
139           printf("%d bytes message :", i+2);
140           for (j=0; j<i+2; j++) {
141             printf(" 0x%02x", sbuffer[j]);
142           }
143           printf("\n");
144
145           if (start_found && stop_found) {
146             check_sirf_msg(sbuffer, i+2);
147           }
148           
149           memmove(sbuffer, sbuffer+i+2, i+2);
150           p -= i+2;
151         }
152       }
153
154       if (!stop_found)
155         break;
156     }
157   }
158
159   return 0;
160 }
161
162 int main(int argc, const char *argv[])
163 {
164   const char device[] = "/dev/ttyUSB0";
165   int fd;
166   struct termios term;
167
168   fd = open(device, O_RDWR | O_NONBLOCK);
169   if (fd < 0) {
170     perror(device);
171     return -1;
172   }
173
174   if (tcgetattr(fd, &term) != 0) {
175     perror("tcgetattr");
176   }
177   cfsetispeed(&term, B4800);
178   cfsetospeed(&term, B4800);
179   if (tcsetattr(fd, TCSANOW, &term) != 0) {
180     perror("tcsetattr");
181   }
182
183   for (;;) {
184     fd_set fdr;
185     int r;
186
187     FD_ZERO(&fdr);
188     FD_SET(fd, &fdr);
189
190     r = select(FD_SETSIZE, &fdr, NULL, NULL, NULL);
191     if (FD_ISSET(fd, &fdr)) {
192       do_read(fd);
193     }
194   }
195
196   close (fd);
197   return 0;
198 }