pud: nmealib: include latest fixes
[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.has_checksum = false;
40   parser->sentence_parser.state = new_state;
41 }
42
43 static inline bool isHexChar(char c) {
44   switch (tolower(c)) {
45     case '0':
46     case '1':
47     case '2':
48     case '3':
49     case '4':
50     case '5':
51     case '6':
52     case '7':
53     case '8':
54     case '9':
55     case 'a':
56     case 'b':
57     case 'c':
58     case 'd':
59     case 'e':
60     case 'f':
61       return true;
62
63     default:
64       break;
65   }
66
67   return false;
68 }
69
70 /**
71  * Initialise the parser.
72  * Allocates a buffer.
73  *
74  * @param parser a pointer to the parser
75  * @return true (1) - success or false (0) - fail
76  */
77 int nmea_parser_init(nmeaPARSER *parser) {
78   assert(parser);
79   memset(&parser->sentence, 0, sizeof(parser->sentence));
80   reset_sentence_parser(parser, SKIP_UNTIL_START);
81   return 0;
82 }
83
84 static bool nmea_parse_sentence_character(nmeaPARSER *parser, const char * c) {
85   assert(parser);
86
87   /* always reset when we encounter a start-of-sentence character */
88   if (*c == '$') {
89     reset_sentence_parser(parser, READ_SENTENCE);
90     parser->buffer.buffer[parser->buffer.length++] = *c;
91     return false;
92   }
93
94   /* just return when we haven't encountered a start-of-sentence character yet */
95   if (parser->sentence_parser.state == SKIP_UNTIL_START) {
96     return false;
97   }
98
99   /* this character belongs to the sentence */
100
101   /* check whether the sentence still fits in the buffer */
102   if (parser->buffer.length >= SENTENCE_SIZE) {
103     reset_sentence_parser(parser, SKIP_UNTIL_START);
104     return false;
105   }
106
107   parser->buffer.buffer[parser->buffer.length++] = *c;
108
109   switch (parser->sentence_parser.state) {
110     case READ_SENTENCE:
111       if (*c == '*') {
112         parser->sentence_parser.state = READ_CHECKSUM;
113         parser->sentence_parser.sentence_checksum_chars_count = 0;
114       } else if (*c == first_eol_char) {
115         parser->sentence_parser.state = READ_EOL;
116         parser->sentence_parser.sentence_eol_chars_count = 1;
117       } else if (isInvalidNMEACharacter(c)) {
118         reset_sentence_parser(parser, SKIP_UNTIL_START);
119       } else {
120         parser->sentence_parser.calculated_checksum ^= (int) *c;
121       }
122       break;
123
124     case READ_CHECKSUM:
125       if (!isHexChar(*c)) {
126         reset_sentence_parser(parser, SKIP_UNTIL_START);
127       } else {
128         switch (parser->sentence_parser.sentence_checksum_chars_count) {
129           case 0:
130             parser->sentence_parser.sentence_checksum_chars[0] = *c;
131             parser->sentence_parser.sentence_checksum_chars[1] = 0;
132             parser->sentence_parser.sentence_checksum_chars_count = 1;
133             break;
134
135           case 1:
136             parser->sentence_parser.sentence_checksum_chars[1] = *c;
137             parser->sentence_parser.sentence_checksum_chars_count = 2;
138             parser->sentence_parser.sentence_checksum = nmea_atoi(parser->sentence_parser.sentence_checksum_chars, 2, 16);
139             parser->sentence_parser.has_checksum = true;
140             parser->sentence_parser.state = READ_EOL;
141             break;
142
143           default:
144             reset_sentence_parser(parser, SKIP_UNTIL_START);
145             break;
146           }
147       }
148       break;
149
150
151     case READ_EOL:
152       switch (parser->sentence_parser.sentence_eol_chars_count) {
153         case 0:
154           if (*c != first_eol_char) {
155             reset_sentence_parser(parser, SKIP_UNTIL_START);
156           } else {
157             parser->sentence_parser.sentence_eol_chars_count = 1;
158           }
159           break;
160
161         case 1:
162           if (*c != second_eol_char) {
163             reset_sentence_parser(parser, SKIP_UNTIL_START);
164           } else {
165             parser->sentence_parser.state = SKIP_UNTIL_START;
166             return (!parser->sentence_parser.sentence_checksum_chars_count
167                 || (parser->sentence_parser.sentence_checksum_chars_count
168                     && (parser->sentence_parser.sentence_checksum == parser->sentence_parser.calculated_checksum)));
169           }
170           break;
171
172         default:
173           reset_sentence_parser(parser, SKIP_UNTIL_START);
174           break;
175       }
176       break;
177
178       /* can't occur, but keep compiler happy */
179       case SKIP_UNTIL_START:
180       default:
181         break;
182
183   }
184
185   return false;
186 }
187
188 /**
189  * Parse a string and store the results in the nmeaINFO structure
190  *
191  * @param parser a pointer to the parser
192  * @param s the string
193  * @param len the length of the string
194  * @param info a pointer to the nmeaINFO structure
195  * @return the number of packets that were parsed
196  */
197 int nmea_parse(nmeaPARSER * parser, const char * s, int len, nmeaINFO * info) {
198   int sentences_count = 0;
199   int charIndex = 0;
200
201   assert(parser);
202   assert(s);
203   assert(info);
204
205   for (charIndex = 0; charIndex < len; charIndex++) {
206     bool sentence_read_successfully = nmea_parse_sentence_character(parser, &s[charIndex]);
207     if (sentence_read_successfully) {
208       enum nmeaPACKTYPE sentence_type = nmea_parse_get_sentence_type(&parser->buffer.buffer[1], parser->buffer.length - 1);
209       switch (sentence_type) {
210         case GPGGA:
211           if (nmea_parse_GPGGA(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpgga)) {
212             sentences_count++;
213             nmea_GPGGA2info(&parser->sentence.gpgga, info);
214           }
215           break;
216
217         case GPGSA:
218           if (nmea_parse_GPGSA(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpgsa)) {
219             sentences_count++;
220             nmea_GPGSA2info(&parser->sentence.gpgsa, info);
221           }
222           break;
223
224         case GPGSV:
225           if (nmea_parse_GPGSV(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpgsv)) {
226             sentences_count++;
227             nmea_GPGSV2info(&parser->sentence.gpgsv, info);
228           }
229           break;
230
231         case GPRMC:
232           if (nmea_parse_GPRMC(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gprmc)) {
233             sentences_count++;
234             nmea_GPRMC2info(&parser->sentence.gprmc, info);
235           }
236           break;
237
238         case GPVTG:
239           if (nmea_parse_GPVTG(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpvtg)) {
240             sentences_count++;
241             nmea_GPVTG2info(&parser->sentence.gpvtg, info);
242           }
243           break;
244
245         case GPNON:
246         default:
247           break;
248       }
249     }
250   }
251
252   return sentences_count;
253 }