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