PUD: include nmealib v0.6.7
[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 /**
22  * \file parser.h
23  */
24
25 #include <nmea/parser.h>
26
27 #include <stdlib.h>
28 #include <stddef.h>
29 #include <string.h>
30
31 #include <nmea/config.h>
32 #include <nmea/context.h>
33 #include <nmea/parse.h>
34 #include <nmea/sentence.h>
35
36 typedef struct _nmeaParserNODE
37 {
38     int packType;
39     void *pack;
40     struct _nmeaParserNODE *next_node;
41
42 } nmeaParserNODE;
43
44 /*
45  * high level
46  */
47
48 /**
49  * \brief Initialization of parser object
50  * @return true (1) - success or false (0) - fail
51  */
52 int nmea_parser_init(nmeaPARSER *parser)
53 {
54     int resv = 0;
55     int buff_size = nmea_property()->parse_buff_size;
56
57     NMEA_ASSERT(parser);
58
59     if(buff_size < NMEA_MIN_PARSEBUFF)
60         buff_size = NMEA_MIN_PARSEBUFF;
61
62     memset(parser, 0, sizeof(nmeaPARSER));
63
64     if(0 == (parser->buffer = malloc(buff_size)))
65         nmea_error("Insufficient memory!");
66     else
67     {
68         parser->buff_size = buff_size;
69         resv = 1;
70     }    
71
72     return resv;
73 }
74
75 /**
76  * \brief Destroy parser object
77  */
78 void nmea_parser_destroy(nmeaPARSER *parser)
79 {
80     NMEA_ASSERT(parser);
81     if (parser->buffer) {
82         free(parser->buffer);
83         parser->buffer = NULL;
84     }
85     nmea_parser_queue_clear(parser);
86     memset(parser, 0, sizeof(nmeaPARSER));
87 }
88
89 /**
90  * \brief Analysis of buffer and put results to information structure
91  * @return Number of packets wos parsed
92  */
93 int nmea_parse(    
94     nmeaPARSER *parser,
95     const char *buff, int buff_sz,
96     nmeaINFO *info
97     )
98 {
99     int ptype, nread = 0;
100     void *pack = 0;
101
102     NMEA_ASSERT(parser && parser->buffer);
103
104     nmea_parser_push(parser, buff, buff_sz);
105
106     while(GPNON != (ptype = nmea_parser_pop(parser, &pack)))
107     {
108         nread++;
109
110         switch(ptype)
111         {
112         case GPGGA:
113             nmea_GPGGA2info((nmeaGPGGA *)pack, info);
114             break;
115         case GPGSA:
116             nmea_GPGSA2info((nmeaGPGSA *)pack, info);
117             break;
118         case GPGSV:
119             nmea_GPGSV2info((nmeaGPGSV *)pack, info);
120             break;
121         case GPRMC:
122             nmea_GPRMC2info((nmeaGPRMC *)pack, info);
123             break;
124         case GPVTG:
125             nmea_GPVTG2info((nmeaGPVTG *)pack, info);
126             break;
127         default:
128             break;
129         };
130
131         free(pack);
132     }
133
134     return nread;
135 }
136
137 /*
138  * low level
139  */
140
141 int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz)
142 {
143     int nparsed = 0, crc, sen_sz, ptype;
144     nmeaParserNODE *node = 0;
145
146     NMEA_ASSERT(parser && parser->buffer);
147
148     /* clear unuse buffer (for debug) */
149     /*
150     memset(
151         parser->buffer + parser->buff_use, 0,
152         parser->buff_size - parser->buff_use
153         );
154         */
155
156     /* add */
157     if(parser->buff_use + buff_sz >= parser->buff_size)
158         nmea_parser_buff_clear(parser);
159
160     memcpy(parser->buffer + parser->buff_use, buff, buff_sz);
161     parser->buff_use += buff_sz;
162
163     /* parse */
164     for(;;node = 0)
165     {
166         sen_sz = nmea_find_tail(
167             (const char *)parser->buffer + nparsed,
168             (int)parser->buff_use - nparsed, &crc);
169
170         if(!sen_sz)
171         {
172             if(nparsed)
173                 memcpy(
174                 parser->buffer,
175                 parser->buffer + nparsed,
176                 parser->buff_use -= nparsed);
177             break;
178         }
179         else if(crc >= 0)
180         {
181             ptype = nmea_pack_type(
182                 (const char *)parser->buffer + nparsed + 1,
183                 parser->buff_use - nparsed - 1);
184
185             if(0 == (node = malloc(sizeof(nmeaParserNODE))))
186                 goto mem_fail;
187
188             node->pack = 0;
189
190             switch(ptype)
191             {
192             case GPGGA:
193                 if(0 == (node->pack = malloc(sizeof(nmeaGPGGA))))
194                     goto mem_fail;
195                 node->packType = GPGGA;
196                 if(!nmea_parse_GPGGA(
197                     (const char *)parser->buffer + nparsed,
198                     sen_sz, (nmeaGPGGA *)node->pack))
199                 {
200                     free(node);
201                     node = 0;
202                 }
203                 break;
204             case GPGSA:
205                 if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
206                     goto mem_fail;
207                 node->packType = GPGSA;
208                 if(!nmea_parse_GPGSA(
209                     (const char *)parser->buffer + nparsed,
210                     sen_sz, (nmeaGPGSA *)node->pack))
211                 {
212                     free(node);
213                     node = 0;
214                 }
215                 break;
216             case GPGSV:
217                 if(0 == (node->pack = malloc(sizeof(nmeaGPGSV))))
218                     goto mem_fail;
219                 node->packType = GPGSV;
220                 if(!nmea_parse_GPGSV(
221                     (const char *)parser->buffer + nparsed,
222                     sen_sz, (nmeaGPGSV *)node->pack))
223                 {
224                     free(node);
225                     node = 0;
226                 }
227                 break;
228             case GPRMC:
229                 if(0 == (node->pack = malloc(sizeof(nmeaGPRMC))))
230                     goto mem_fail;
231                 node->packType = GPRMC;
232                 if(!nmea_parse_GPRMC(
233                     (const char *)parser->buffer + nparsed,
234                     sen_sz, (nmeaGPRMC *)node->pack))
235                 {
236                     free(node);
237                     node = 0;
238                 }
239                 break;
240             case GPVTG:
241                 if(0 == (node->pack = malloc(sizeof(nmeaGPVTG))))
242                     goto mem_fail;
243                 node->packType = GPVTG;
244                 if(!nmea_parse_GPVTG(
245                     (const char *)parser->buffer + nparsed,
246                     sen_sz, (nmeaGPVTG *)node->pack))
247                 {
248                     free(node);
249                     node = 0;
250                 }
251                 break;
252             default:
253                 free(node);
254                 node = 0;
255                 break;
256             };
257
258             if(node)
259             {
260                 if(parser->end_node)
261                     ((nmeaParserNODE *)parser->end_node)->next_node = node;
262                 parser->end_node = node;
263                 if(!parser->top_node)
264                     parser->top_node = node;
265                 node->next_node = 0;
266             }
267         }
268
269         nparsed += sen_sz;
270     }
271
272     return nparsed;
273
274 mem_fail:
275     if(node)
276         free(node);
277
278     nmea_error("Insufficient memory!");
279
280     return -1;
281 }
282
283 /**
284  * \brief Analysis of buffer and keep results into parser
285  * @return Number of bytes wos parsed from buffer
286  */
287 int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz)
288 {
289     int nparse, nparsed = 0;
290
291     do
292     {
293         if(buff_sz > parser->buff_size)
294             nparse = parser->buff_size;
295         else
296             nparse = buff_sz;
297
298         nparsed += nmea_parser_real_push(
299             parser, buff, nparse);
300
301         buff_sz -= nparse;
302
303     } while(buff_sz);
304
305     return nparsed;
306 }
307
308 /**
309  * \brief Get type of top packet keeped into parser
310  * @return Type of packet
311  * @see nmeaPACKTYPE
312  */
313 int nmea_parser_top(nmeaPARSER *parser)
314 {
315     int retval = GPNON;
316     nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
317
318     NMEA_ASSERT(parser && parser->buffer);
319
320     if(node)
321         retval = node->packType;
322
323     return retval;
324 }
325
326 /**
327  * \brief Withdraw top packet from parser
328  * @return Received packet type
329  * @see nmeaPACKTYPE
330  */
331 int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr)
332 {
333     int retval = GPNON;
334     nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
335
336     NMEA_ASSERT(parser && parser->buffer);
337
338     if(node)
339     {
340         *pack_ptr = node->pack;
341         retval = node->packType;
342         parser->top_node = node->next_node;
343         if(!parser->top_node)
344             parser->end_node = 0;
345         free(node);
346     }
347
348     return retval;
349 }
350
351 /**
352  * \brief Get top packet from parser without withdraw
353  * @return Received packet type
354  * @see nmeaPACKTYPE
355  */
356 int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr)
357 {
358     int retval = GPNON;
359     nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
360
361     NMEA_ASSERT(parser && parser->buffer);
362
363     if(node)
364     {
365         *pack_ptr = node->pack;
366         retval = node->packType;
367     }
368
369     return retval;
370 }
371
372 /**
373  * \brief Delete top packet from parser
374  * @return Deleted packet type
375  * @see nmeaPACKTYPE
376  */
377 int nmea_parser_drop(nmeaPARSER *parser)
378 {
379     int retval = GPNON;
380     nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
381
382     NMEA_ASSERT(parser && parser->buffer);
383
384     if(node)
385     {
386         if(node->pack)
387             free(node->pack);
388         retval = node->packType;
389         parser->top_node = node->next_node;
390         if(!parser->top_node)
391             parser->end_node = 0;
392         free(node);
393     }
394
395     return retval;
396 }
397
398 /**
399  * \brief Clear cache of parser
400  * @return true (1) - success
401  */
402 int nmea_parser_buff_clear(nmeaPARSER *parser)
403 {
404     NMEA_ASSERT(parser && parser->buffer);
405     parser->buff_use = 0;
406     return 1;
407 }
408
409 /**
410  * \brief Clear packets queue into parser
411  * @return true (1) - success
412  */
413 int nmea_parser_queue_clear(nmeaPARSER *parser)
414 {
415     NMEA_ASSERT(parser);
416     while(parser->top_node)
417         nmea_parser_drop(parser);
418     return 1;
419 }