6e82aeb1924ff5f0ef7434e7322ef15c5abd4534
[olsrd.git] / lib / pud / nmealib / src / parser.c
1 /*
2  * This file is part of nmealib.
3  *
4  * Copyright (c) 2008 Timur Sinitsyn
5  * Copyright (c) 2011 Ferry Huberts
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public
9  * License as published by the Free Software Foundation; either
10  * version 2.1 of the License, or (at your option) any later version.
11  *
12  * This library is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15  * Lesser General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program. If not, see <http://www.gnu.org/licenses/>.
19  */
20
21 #include <nmea/parser.h>
22
23 #include <nmea/context.h>
24 #include <nmea/parse.h>
25 #include <nmea/sentence.h>
26 #include <nmea/conversions.h>
27
28 #include <stdlib.h>
29 #include <string.h>
30 #include <assert.h>
31
32 /*
33  * high level
34  */
35
36 /**
37  * Initialise the parser.
38  * Allocates a buffer.
39  *
40  * @param parser a pointer to the parser
41  * @return true (1) - success or false (0) - fail
42  */
43 int nmea_parser_init(nmeaPARSER *parser) {
44         int resv = 0;
45         int buff_size = nmea_context_get_buffer_size();
46
47         assert(parser);
48
49         memset(parser, 0, sizeof(nmeaPARSER));
50
51         if (!(parser->buffer = malloc(buff_size)))
52                 nmea_error("nmea_parser_init: insufficient memory");
53         else {
54                 parser->buff_size = buff_size;
55                 resv = 1;
56         }
57
58         return resv;
59 }
60
61 /**
62  * Destroy the parser.
63  * Frees a buffer.
64  *
65  * @param parser a pointer to the parser
66  */
67 void nmea_parser_destroy(nmeaPARSER *parser) {
68         assert(parser);
69
70         if (parser->buffer) {
71                 free(parser->buffer);
72                 parser->buffer = NULL;
73         }
74         nmea_parser_queue_clear(parser);
75         memset(parser, 0, sizeof(nmeaPARSER));
76 }
77
78 /**
79  * Parse a string and store the results in the nmeaINFO structure
80  *
81  * @param parser a pointer to the parser
82  * @param s the string
83  * @param len the length of the string
84  * @param info a pointer to the nmeaINFO structure
85  * @return the number of packets that were parsed
86  */
87 int nmea_parse(nmeaPARSER *parser, const char *s, const int len, nmeaINFO *info) {
88         int packetType;
89         int packetsParsed = 0;
90         void *packet = 0;
91
92         assert(parser);
93
94         nmea_parser_push(parser, s, len);
95
96         while (GPNON != (packetType = nmea_parser_pop(parser, &packet))) {
97                 packetsParsed++;
98
99                 switch (packetType) {
100                 case GPGGA:
101                         nmea_GPGGA2info((nmeaGPGGA *) packet, info);
102                         break;
103                 case GPGSA:
104                         nmea_GPGSA2info((nmeaGPGSA *) packet, info);
105                         break;
106                 case GPGSV:
107                         nmea_GPGSV2info((nmeaGPGSV *) packet, info);
108                         break;
109                 case GPRMC:
110                         nmea_GPRMC2info((nmeaGPRMC *) packet, info);
111                         break;
112                 case GPVTG:
113                         nmea_GPVTG2info((nmeaGPVTG *) packet, info);
114                         break;
115                 default:
116                         break;
117                 };
118
119                 free(packet);
120         }
121
122         return packetsParsed;
123 }
124
125 /*
126  * low level
127  */
128
129 /**
130  * Do the actual parsing of a string and store the results in the parser.
131  * This function is used to parse (broken up parts) of a complete string.
132  *
133  * @param parser a pointer to the parser
134  * @param s the string
135  * @param len the length of the string
136  * @return the number of bytes that were parsed, -1 on error
137  */
138 static int nmea_parser_real_push(nmeaPARSER *parser, const char *s, int len) {
139         int charsParsed = 0;
140         int crc;
141         int sentenceLength;
142         int sentenceType;
143         nmeaParserNODE *node = NULL;
144
145         assert(parser);
146         assert(parser->buffer);
147
148         if (!s || !len)
149                 return 0;
150
151         /* clear the buffer if the string is too large */
152         if ((parser->buff_use + len) >= parser->buff_size)
153                 nmea_parser_buff_clear(parser);
154
155         /* check that the string will fit in the buffer */
156         if ((parser->buff_use + len) >= parser->buff_size) {
157                 nmea_error("nmea_parser_real_push: string too long to fit in parser buffer");
158                 return 0;
159         }
160
161         /* put the string in the buffer */
162         memcpy(parser->buffer + parser->buff_use, s, len);
163         parser->buff_use += len;
164
165         /* parse */
166         for (;; node = NULL) {
167                 sentenceLength = nmea_parse_get_sentence_length(parser->buffer + charsParsed, parser->buff_use - charsParsed,
168                                 &crc);
169
170                 if (!sentenceLength) {
171                         if (charsParsed)
172                                 memmove(parser->buffer, parser->buffer + charsParsed, parser->buff_use -= charsParsed);
173                         break;
174                 } else if (crc >= 0) {
175                         sentenceType = nmea_parse_get_sentence_type(parser->buffer + charsParsed + 1,
176                                         parser->buff_use - charsParsed - 1);
177
178                         if (!(node = malloc(sizeof(nmeaParserNODE))))
179                                 goto mem_fail;
180
181                         node->pack = NULL;
182
183                         switch (sentenceType) {
184                         case GPGGA:
185                                 if (!(node->pack = malloc(sizeof(nmeaGPGGA))))
186                                         goto mem_fail;
187                                 node->packType = GPGGA;
188                                 if (!nmea_parse_GPGGA(parser->buffer + charsParsed, sentenceLength, (nmeaGPGGA *) node->pack)) {
189                                         free(node->pack);
190                                         free(node);
191                                         node = NULL;
192                                 }
193                                 break;
194                         case GPGSA:
195                                 if (!(node->pack = malloc(sizeof(nmeaGPGSA))))
196                                         goto mem_fail;
197                                 node->packType = GPGSA;
198                                 if (!nmea_parse_GPGSA(parser->buffer + charsParsed, sentenceLength, (nmeaGPGSA *) node->pack)) {
199                                         free(node->pack);
200                                         free(node);
201                                         node = NULL;
202                                 }
203                                 break;
204                         case GPGSV:
205                                 if (!(node->pack = malloc(sizeof(nmeaGPGSV))))
206                                         goto mem_fail;
207                                 node->packType = GPGSV;
208                                 if (!nmea_parse_GPGSV(parser->buffer + charsParsed, sentenceLength, (nmeaGPGSV *) node->pack)) {
209                                         free(node->pack);
210                                         free(node);
211                                         node = NULL;
212                                 }
213                                 break;
214                         case GPRMC:
215                                 if (!(node->pack = malloc(sizeof(nmeaGPRMC))))
216                                         goto mem_fail;
217                                 node->packType = GPRMC;
218                                 if (!nmea_parse_GPRMC(parser->buffer + charsParsed, sentenceLength, (nmeaGPRMC *) node->pack)) {
219                                         free(node->pack);
220                                         free(node);
221                                         node = NULL;
222                                 }
223                                 break;
224                         case GPVTG:
225                                 if (!(node->pack = malloc(sizeof(nmeaGPVTG))))
226                                         goto mem_fail;
227                                 node->packType = GPVTG;
228                                 if (!nmea_parse_GPVTG(parser->buffer + charsParsed, sentenceLength, (nmeaGPVTG *) node->pack)) {
229                                         free(node->pack);
230                                         free(node);
231                                         node = NULL;
232                                 }
233                                 break;
234                         default:
235                                 free(node);
236                                 node = NULL;
237                                 break;
238                         };
239
240                         if (node) {
241                                 if (parser->end_node)
242                                         parser->end_node->next_node = node;
243                                 parser->end_node = node;
244                                 if (!parser->top_node)
245                                         parser->top_node = node;
246                                 node->next_node = NULL;
247                         }
248                 }
249
250                 charsParsed += sentenceLength;
251         }
252
253         return charsParsed;
254
255         mem_fail: if (node)
256                 free(node);
257         nmea_error("Insufficient memory!");
258
259         return -1;
260 }
261
262 /**
263  * Parse a string and store the results in the parser
264  *
265  * @param parser a pointer to the parser
266  * @param s the string
267  * @param len the length of the string
268  * @return the number of bytes that were parsed
269  */
270 int nmea_parser_push(nmeaPARSER *parser, const char *s, int len) {
271         int charsParsed = 0;
272
273         assert(parser);
274
275         if (!s || !len)
276                 return 0;
277
278         do {
279                 int charsToParse;
280
281                 if (len > parser->buff_size)
282                         charsToParse = parser->buff_size;
283                 else
284                         charsToParse = len;
285
286                 charsParsed += nmea_parser_real_push(parser, s, charsToParse);
287                 len -= charsToParse;
288         } while (len);
289
290         return charsParsed;
291 }
292
293 /**
294  * Get the type of top packet
295  *
296  * @param parser a pointer to the parser
297  * @return the type of the top packet (see nmeaPACKTYPE)
298  */
299 int nmea_parser_top(const nmeaPARSER *parser) {
300         int retval = GPNON;
301         nmeaParserNODE *node;
302
303         assert(parser);
304
305         node = parser->top_node;
306         if (node)
307                 retval = node->packType;
308
309         return retval;
310 }
311
312 /**
313  * Remove the top packet from the parser
314  *
315  * @param parser a pointer to the parser
316  * @param pack_ptr a pointer to the location where to store a pointer to the packet
317  * @return the type of the top packet (see nmeaPACKTYPE)
318  */
319 int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr) {
320         int retval = GPNON;
321         nmeaParserNODE *node;
322
323         assert(parser);
324
325         node = parser->top_node;
326         if (node) {
327                 retval = node->packType;
328                 if (pack_ptr)
329                         *pack_ptr = node->pack;
330                 parser->top_node = node->next_node;
331                 if (!parser->top_node)
332                         parser->end_node = 0;
333                 free(node);
334         }
335
336         return retval;
337 }
338
339 /**
340  * Get the top packet from the parser without removing it
341  *
342  * @param parser a pointer to the parser
343  * @param pack_ptr a pointer to the location where to store a pointer to the packet
344  * @return the type of the top packet (see nmeaPACKTYPE)
345  */
346 int nmea_parser_peek(const nmeaPARSER *parser, void **pack_ptr) {
347         int retval = GPNON;
348         nmeaParserNODE *node;
349
350         assert(parser);
351
352         node = parser->top_node;
353         if (node) {
354                 retval = node->packType;
355                 if (pack_ptr)
356                         *pack_ptr = node->pack;
357         }
358
359         return retval;
360 }
361
362 /**
363  * Remove the top packet from the parser
364  *
365  * @param parser a pointer to the parser
366  * @return the type of the removed packet (see nmeaPACKTYPE)
367  */
368 int nmea_parser_drop(nmeaPARSER *parser) {
369         int retval = GPNON;
370         nmeaParserNODE *node;
371
372         assert(parser);
373
374         node = parser->top_node;
375         if (node) {
376                 retval = node->packType;
377                 if (node->pack)
378                         free(node->pack);
379                 parser->top_node = node->next_node;
380                 if (!parser->top_node)
381                         parser->end_node = NULL;
382                 free(node);
383         }
384
385         return retval;
386 }
387
388 /**
389  * Clear the cache of the parser
390  *
391  * @param parser a pointer to the parser
392  */
393 void nmea_parser_buff_clear(nmeaPARSER *parser) {
394         assert(parser);
395         parser->buff_use = 0;
396 }
397
398 /**
399  * Clear the packets queue in the parser
400  *
401  * @param parser a pointer to the parser
402  */
403 void nmea_parser_queue_clear(nmeaPARSER *parser) {
404         assert(parser);
405         while (parser->top_node)
406                 nmea_parser_drop(parser);
407 }