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