PUD: import nmealib v0.6.9
[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 static 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->pack);
201                     free(node);
202                     node = 0;
203                 }
204                 break;
205             case GPGSA:
206                 if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
207                     goto mem_fail;
208                 node->packType = GPGSA;
209                 if(!nmea_parse_GPGSA(
210                     (const char *)parser->buffer + nparsed,
211                     sen_sz, (nmeaGPGSA *)node->pack))
212                 {
213                     free(node->pack);
214                     free(node);
215                     node = 0;
216                 }
217                 break;
218             case GPGSV:
219                 if(0 == (node->pack = malloc(sizeof(nmeaGPGSV))))
220                     goto mem_fail;
221                 node->packType = GPGSV;
222                 if(!nmea_parse_GPGSV(
223                     (const char *)parser->buffer + nparsed,
224                     sen_sz, (nmeaGPGSV *)node->pack))
225                 {
226                     free(node->pack);
227                     free(node);
228                     node = 0;
229                 }
230                 break;
231             case GPRMC:
232                 if(0 == (node->pack = malloc(sizeof(nmeaGPRMC))))
233                     goto mem_fail;
234                 node->packType = GPRMC;
235                 if(!nmea_parse_GPRMC(
236                     (const char *)parser->buffer + nparsed,
237                     sen_sz, (nmeaGPRMC *)node->pack))
238                 {
239                     free(node->pack);
240                     free(node);
241                     node = 0;
242                 }
243                 break;
244             case GPVTG:
245                 if(0 == (node->pack = malloc(sizeof(nmeaGPVTG))))
246                     goto mem_fail;
247                 node->packType = GPVTG;
248                 if(!nmea_parse_GPVTG(
249                     (const char *)parser->buffer + nparsed,
250                     sen_sz, (nmeaGPVTG *)node->pack))
251                 {
252                     free(node->pack);
253                     free(node);
254                     node = 0;
255                 }
256                 break;
257             default:
258                 free(node);
259                 node = 0;
260                 break;
261             };
262
263             if(node)
264             {
265                 if(parser->end_node)
266                     ((nmeaParserNODE *)parser->end_node)->next_node = node;
267                 parser->end_node = node;
268                 if(!parser->top_node)
269                     parser->top_node = node;
270                 node->next_node = 0;
271             }
272         }
273
274         nparsed += sen_sz;
275     }
276
277     return nparsed;
278
279 mem_fail:
280     if(node)
281         free(node);
282
283     nmea_error("Insufficient memory!");
284
285     return -1;
286 }
287
288 /**
289  * \brief Analysis of buffer and keep results into parser
290  * @return Number of bytes wos parsed from buffer
291  */
292 int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz)
293 {
294     int nparse, nparsed = 0;
295
296     do
297     {
298         if(buff_sz > parser->buff_size)
299             nparse = parser->buff_size;
300         else
301             nparse = buff_sz;
302
303         nparsed += nmea_parser_real_push(
304             parser, buff, nparse);
305
306         buff_sz -= nparse;
307
308     } while(buff_sz);
309
310     return nparsed;
311 }
312
313 /**
314  * \brief Get type of top packet keeped into parser
315  * @return Type of packet
316  * @see nmeaPACKTYPE
317  */
318 int nmea_parser_top(nmeaPARSER *parser)
319 {
320     int retval = GPNON;
321     nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
322
323     NMEA_ASSERT(parser && parser->buffer);
324
325     if(node)
326         retval = node->packType;
327
328     return retval;
329 }
330
331 /**
332  * \brief Withdraw top packet from parser
333  * @return Received packet type
334  * @see nmeaPACKTYPE
335  */
336 int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr)
337 {
338     int retval = GPNON;
339     nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
340
341     NMEA_ASSERT(parser && parser->buffer);
342
343     if(node)
344     {
345         *pack_ptr = node->pack;
346         retval = node->packType;
347         parser->top_node = node->next_node;
348         if(!parser->top_node)
349             parser->end_node = 0;
350         free(node);
351     }
352
353     return retval;
354 }
355
356 /**
357  * \brief Get top packet from parser without withdraw
358  * @return Received packet type
359  * @see nmeaPACKTYPE
360  */
361 int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr)
362 {
363     int retval = GPNON;
364     nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
365
366     NMEA_ASSERT(parser && parser->buffer);
367
368     if(node)
369     {
370         *pack_ptr = node->pack;
371         retval = node->packType;
372     }
373
374     return retval;
375 }
376
377 /**
378  * \brief Delete top packet from parser
379  * @return Deleted packet type
380  * @see nmeaPACKTYPE
381  */
382 int nmea_parser_drop(nmeaPARSER *parser)
383 {
384     int retval = GPNON;
385     nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
386
387     NMEA_ASSERT(parser && parser->buffer);
388
389     if(node)
390     {
391         if(node->pack)
392             free(node->pack);
393         retval = node->packType;
394         parser->top_node = node->next_node;
395         if(!parser->top_node)
396             parser->end_node = 0;
397         free(node);
398     }
399
400     return retval;
401 }
402
403 /**
404  * \brief Clear cache of parser
405  * @return true (1) - success
406  */
407 int nmea_parser_buff_clear(nmeaPARSER *parser)
408 {
409     NMEA_ASSERT(parser && parser->buffer);
410     parser->buff_use = 0;
411     return 1;
412 }
413
414 /**
415  * \brief Clear packets queue into parser
416  * @return true (1) - success
417  */
418 int nmea_parser_queue_clear(nmeaPARSER *parser)
419 {
420     NMEA_ASSERT(parser);
421     while(parser->top_node)
422         nmea_parser_drop(parser);
423     return 1;
424 }