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