81f1998754362d13ea82643cc78b768e33861440
[olsrd.git] / lib / pud / nmealib / src / parser.c
1 /*
2  * This file is part of nmealib.
3  *
4  * This library is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU Lesser General Public
6  * License as published by the Free Software Foundation; either
7  * version 2.1 of the License, or (at your option) any later version.
8  *
9  * This library is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12  * Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17
18 #include <nmea/parser.h>
19
20 #include <nmea/parse.h>
21 #include <nmea/sentence.h>
22 #include <nmea/conversions.h>
23 #include <nmea/tok.h>
24
25 #include <stdlib.h>
26 #include <stdbool.h>
27 #include <string.h>
28 #include <assert.h>
29 #include <ctype.h>
30
31 #define first_eol_char  ('\r')
32 #define second_eol_char ('\n')
33
34 static void reset_sentence_parser(nmeaPARSER * parser, sentence_parser_state new_state) {
35   assert(parser);
36   memset(&parser->sentence_parser, 0, sizeof(parser->sentence_parser));
37   parser->buffer.buffer[0] = '\0';
38   parser->buffer.length = 0;
39   parser->sentence_parser.state = new_state;
40 }
41
42 static inline bool isHexChar(char c) {
43   switch (tolower(c)) {
44     case '0':
45     case '1':
46     case '2':
47     case '3':
48     case '4':
49     case '5':
50     case '6':
51     case '7':
52     case '8':
53     case '9':
54     case 'a':
55     case 'b':
56     case 'c':
57     case 'd':
58     case 'e':
59     case 'f':
60       return true;
61
62     default:
63       break;
64   }
65
66   return false;
67 }
68
69 /**
70  * Initialise the parser.
71  * Allocates a buffer.
72  *
73  * @param parser a pointer to the parser
74  * @return true (1) - success or false (0) - fail
75  */
76 int nmea_parser_init(nmeaPARSER *parser) {
77   assert(parser);
78   memset(&parser->sentence, 0, sizeof(parser->sentence));
79   reset_sentence_parser(parser, SKIP_UNTIL_START);
80   return 0;
81 }
82
83 static bool nmea_parse_sentence_character(nmeaPARSER *parser, const char * c) {
84   assert(parser);
85
86   /* always reset when we encounter a start-of-sentence character */
87   if (*c == '$') {
88     reset_sentence_parser(parser, READ_SENTENCE);
89     parser->buffer.buffer[parser->buffer.length++] = *c;
90     return false;
91   }
92
93   /* just return when we haven't encountered a start-of-sentence character yet */
94   if (parser->sentence_parser.state == SKIP_UNTIL_START) {
95     return false;
96   }
97
98   /* this character belongs to the sentence */
99
100   /* check whether the sentence still fits in the buffer */
101   if (parser->buffer.length >= SENTENCE_SIZE) {
102     reset_sentence_parser(parser, SKIP_UNTIL_START);
103     return false;
104   }
105
106   parser->buffer.buffer[parser->buffer.length++] = *c;
107
108   switch (parser->sentence_parser.state) {
109     case READ_SENTENCE:
110       if (*c == '*') {
111         parser->sentence_parser.state = READ_CHECKSUM;
112         parser->sentence_parser.sentence_checksum_chars_count = 0;
113       } else if (*c == first_eol_char) {
114         parser->sentence_parser.state = READ_EOL;
115         parser->sentence_parser.sentence_eol_chars_count = 0;
116       } else if (isInvalidNMEACharacter(c)) {
117         reset_sentence_parser(parser, SKIP_UNTIL_START);
118       } else {
119         parser->sentence_parser.calculated_checksum ^= (int) *c;
120       }
121       break;
122
123     case READ_CHECKSUM:
124       if (!isHexChar(*c)) {
125         reset_sentence_parser(parser, SKIP_UNTIL_START);
126       } else {
127         switch (parser->sentence_parser.sentence_checksum_chars_count) {
128           case 0:
129             parser->sentence_parser.sentence_checksum_chars[0] = *c;
130             parser->sentence_parser.sentence_checksum_chars[1] = 0;
131             parser->sentence_parser.sentence_checksum_chars_count = 1;
132             break;
133
134           case 1:
135             parser->sentence_parser.sentence_checksum_chars[1] = *c;
136             parser->sentence_parser.sentence_checksum_chars_count = 2;
137             parser->sentence_parser.sentence_checksum = nmea_atoi(parser->sentence_parser.sentence_checksum_chars, 2, 16);
138             parser->sentence_parser.state = READ_EOL;
139             break;
140
141           default:
142             reset_sentence_parser(parser, SKIP_UNTIL_START);
143             break;
144           }
145       }
146       break;
147
148
149     case READ_EOL:
150       switch (parser->sentence_parser.sentence_eol_chars_count) {
151         case 0:
152           if (*c != first_eol_char) {
153             reset_sentence_parser(parser, SKIP_UNTIL_START);
154           } else {
155             parser->sentence_parser.sentence_eol_chars_count = 1;
156           }
157           break;
158
159         case 1:
160           if (*c != second_eol_char) {
161             reset_sentence_parser(parser, SKIP_UNTIL_START);
162           } else {
163             parser->sentence_parser.state = SKIP_UNTIL_START;
164             return (!parser->sentence_parser.sentence_checksum_chars_count
165                 || (parser->sentence_parser.sentence_checksum_chars_count
166                     && (parser->sentence_parser.sentence_checksum == parser->sentence_parser.calculated_checksum)));
167           }
168           break;
169
170         default:
171           reset_sentence_parser(parser, SKIP_UNTIL_START);
172           break;
173       }
174       break;
175
176       /* can't occur, but keep compiler happy */
177       case SKIP_UNTIL_START:
178       default:
179         break;
180
181   }
182
183   return false;
184 }
185
186 /**
187  * Parse a string and store the results in the nmeaINFO structure
188  *
189  * @param parser a pointer to the parser
190  * @param s the string
191  * @param len the length of the string
192  * @param info a pointer to the nmeaINFO structure
193  * @return the number of packets that were parsed
194  */
195 int nmea_parse(nmeaPARSER * parser, const char * s, int len, nmeaINFO * info) {
196   int sentences_count = 0;
197   int charIndex = 0;
198
199   assert(parser);
200   assert(s);
201   assert(info);
202
203   for (charIndex = 0; charIndex < len; charIndex++) {
204     bool sentence_read_successfully = nmea_parse_sentence_character(parser, &s[charIndex]);
205     if (sentence_read_successfully) {
206       enum nmeaPACKTYPE sentence_type = nmea_parse_get_sentence_type(&parser->buffer.buffer[1], parser->buffer.length - 1);
207       switch (sentence_type) {
208         case GPGGA:
209           if (nmea_parse_GPGGA(parser->buffer.buffer, parser->buffer.length, &parser->sentence.gpgga)) {
210             sentences_count++;
211             nmea_GPGGA2info(&parser->sentence.gpgga, info);
212           }
213           break;
214
215         case GPGSA:
216           if (nmea_parse_GPGSA(parser->buffer.buffer, parser->buffer.length, &parser->sentence.gpgsa)) {
217             sentences_count++;
218             nmea_GPGSA2info(&parser->sentence.gpgsa, info);
219           }
220           break;
221
222         case GPGSV:
223           if (nmea_parse_GPGSV(parser->buffer.buffer, parser->buffer.length, &parser->sentence.gpgsv)) {
224             sentences_count++;
225             nmea_GPGSV2info(&parser->sentence.gpgsv, info);
226           }
227           break;
228
229         case GPRMC:
230           if (nmea_parse_GPRMC(parser->buffer.buffer, parser->buffer.length, &parser->sentence.gprmc)) {
231             sentences_count++;
232             nmea_GPRMC2info(&parser->sentence.gprmc, info);
233           }
234           break;
235
236         case GPVTG:
237           if (nmea_parse_GPVTG(parser->buffer.buffer, parser->buffer.length, &parser->sentence.gpvtg)) {
238             sentences_count++;
239             nmea_GPVTG2info(&parser->sentence.gpvtg, info);
240           }
241           break;
242
243         case GPNON:
244         default:
245           break;
246       }
247     }
248   }
249
250   return sentences_count;
251 }