PUD: nmealib: get rid of NMEA_ASSERT
[olsrd.git] / lib / pud / nmealib / src / tok.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 /*! \file tok.h */
22
23 #include <nmea/tok.h>
24
25 #include <ctype.h>
26 #include <stdarg.h>
27 #include <stdio.h>
28 #include <stdlib.h>
29 #include <string.h>
30
31 #define NMEA_TOKS_COMPARE   (1)
32 #define NMEA_TOKS_PERCENT   (2)
33 #define NMEA_TOKS_WIDTH     (3)
34 #define NMEA_TOKS_TYPE      (4)
35
36 #define NMEA_CONVSTR_BUF    (256)
37
38 /**
39  * \brief Calculate control sum of binary buffer
40  */
41 int nmea_calc_crc(const char *buff, int buff_sz)
42 {
43     int chsum = 0,
44         it;
45
46     for(it = 0; it < buff_sz; ++it)
47         chsum ^= (int)buff[it];
48
49     return chsum;
50 }
51
52 /**
53  * \brief Convert string to number
54  */
55 int nmea_atoi(const char *str, int str_sz, int radix)
56 {
57     char *tmp_ptr;
58     char buff[NMEA_CONVSTR_BUF];
59     int res = 0;
60
61     if(str_sz < NMEA_CONVSTR_BUF)
62     {
63         memcpy(&buff[0], str, str_sz);
64         buff[str_sz] = '\0';
65         res = strtol(&buff[0], &tmp_ptr, radix);
66     }
67
68     return res;
69 }
70
71 /**
72  * \brief Convert string to fraction number
73  */
74 double nmea_atof(const char *str, int str_sz)
75 {
76     char *tmp_ptr;
77     char buff[NMEA_CONVSTR_BUF];
78     double res = 0;
79
80     if(str_sz < NMEA_CONVSTR_BUF)
81     {
82         memcpy(&buff[0], str, str_sz);
83         buff[str_sz] = '\0';
84         res = strtod(&buff[0], &tmp_ptr);
85     }
86
87     return res;
88 }
89
90 /**
91  * \brief Formating string (like standart printf) with CRC tail (*CRC)
92  */
93 int nmea_printf(char *buff, int buff_sz, const char *format, ...)
94 {
95     int retval, add = 0;
96     va_list arg_ptr;
97
98     if(buff_sz <= 0)
99         return 0;
100
101     va_start(arg_ptr, format);
102
103     retval = vsnprintf(buff, buff_sz, format, arg_ptr);
104
105     if(retval > 0)
106     {
107         add = snprintf(
108             buff + retval, buff_sz - retval, "*%02x\r\n",
109             nmea_calc_crc(buff + 1, retval - 1));
110     }
111
112     retval += add;
113
114     if(retval < 0 || retval > buff_sz)
115     {
116         memset(buff, ' ', buff_sz);
117         retval = buff_sz;
118     }
119
120     va_end(arg_ptr);
121
122     return retval;
123 }
124
125 /**
126  * \brief Analyse string (specificate for NMEA sentences)
127  */
128 int nmea_scanf(const char *buff, int buff_sz, const char *format, ...)
129 {
130     const char *beg_tok;
131     const char *end_buf = buff + buff_sz;
132
133     va_list arg_ptr;
134     int tok_type = NMEA_TOKS_COMPARE;
135     int width = 0;
136     const char *beg_fmt = 0;
137     int snum = 0, unum = 0;
138
139     int tok_count = 0;
140     void *parg_target;
141
142     va_start(arg_ptr, format);
143     
144     for(; *format && buff < end_buf; ++format)
145     {
146         switch(tok_type)
147         {
148         case NMEA_TOKS_COMPARE:
149             if('%' == *format)
150                 tok_type = NMEA_TOKS_PERCENT;
151             else if(*buff++ != *format)
152                 goto fail;
153             break;
154         case NMEA_TOKS_PERCENT:
155             width = 0;
156             beg_fmt = format;
157             tok_type = NMEA_TOKS_WIDTH;
158             /* no break */
159         case NMEA_TOKS_WIDTH:
160             if(isdigit(*format))
161                 break;
162             {
163                 tok_type = NMEA_TOKS_TYPE;
164                 if(format > beg_fmt)
165                     width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10);
166             }
167             /* no break */
168         case NMEA_TOKS_TYPE:
169             beg_tok = buff;
170
171             if(!width && ('c' == *format || 'C' == *format) && *buff != format[1])
172                 width = 1;
173
174             if(width)
175             {
176                 if(buff + width <= end_buf)
177                     buff += width;
178                 else
179                     goto fail;
180             }
181             else
182             {
183                 if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff))))
184                     buff = end_buf;
185             }
186
187             if(buff > end_buf)
188                 goto fail;
189
190             tok_type = NMEA_TOKS_COMPARE;
191             tok_count++;
192
193             parg_target = 0; width = (int)(buff - beg_tok);
194
195             switch(*format)
196             {
197             case 'c':
198             case 'C':
199                 parg_target = (void *)va_arg(arg_ptr, char *);
200                 if(width && 0 != (parg_target))
201                     *((char *)parg_target) = *beg_tok;
202                 break;
203             case 's':
204             case 'S':
205                 parg_target = (void *)va_arg(arg_ptr, char *);
206                 if(width && 0 != (parg_target))
207                 {
208                     memcpy(parg_target, beg_tok, width);
209                     ((char *)parg_target)[width] = '\0';
210                 }
211                 break;
212             case 'f':
213             case 'g':
214             case 'G':
215             case 'e':
216             case 'E':
217                 parg_target = (void *)va_arg(arg_ptr, double *);
218                 if(width && 0 != (parg_target))
219                     *((double *)parg_target) = nmea_atof(beg_tok, width);
220                 break;
221             default:
222                 break;
223             };
224
225             if(parg_target)
226                 break;
227             if(0 == (parg_target = (void *)va_arg(arg_ptr, int *)))
228                 break;
229             if(!width)
230                 break;
231
232             switch(*format)
233             {
234             case 'd':
235             case 'i':
236                 snum = nmea_atoi(beg_tok, width, 10);
237                 memcpy(parg_target, &snum, sizeof(int));
238                 break;
239             case 'u':
240                 unum = nmea_atoi(beg_tok, width, 10);
241                 memcpy(parg_target, &unum, sizeof(unsigned int));
242                 break;
243             case 'x':
244             case 'X':
245                 unum = nmea_atoi(beg_tok, width, 16);
246                 memcpy(parg_target, &unum, sizeof(unsigned int));
247                 break;
248             case 'o':
249                 unum = nmea_atoi(beg_tok, width, 8);
250                 memcpy(parg_target, &unum, sizeof(unsigned int));
251                 break;
252             default:
253                 goto fail;
254             };
255
256             break;
257
258         default:
259             break;
260         };
261     }
262
263 fail:
264
265     va_end(arg_ptr);
266
267     return tok_count;
268 }