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