Branch data Line data Source code
1 : : /*
2 : : * Copyright Tim (xtimor@gmail.com)
3 : : *
4 : : * NMEA library is free software; you can redistribute it and/or modify
5 : : * it under the terms of the GNU Lesser General Public License as published by
6 : : * the Free Software Foundation; either version 2 of the License, or
7 : : * (at your option) any later version.
8 : : *
9 : : * This program 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
12 : : * GNU Lesser General Public License for more details.
13 : : *
14 : : * You should have received a copy of the GNU Lesser General Public License
15 : : * along with this program. If not, see <http://www.gnu.org/licenses/>
16 : : */
17 : : /*
18 : : *
19 : : * NMEA library
20 : : * URL: http://nmea.sourceforge.net
21 : : * Author: Tim (xtimor@gmail.com)
22 : : * Licence: http://www.gnu.org/licenses/lgpl.html
23 : : * $Id: tok.c 17 2008-03-11 11:56:11Z xtimor $
24 : : *
25 : : */
26 : :
27 : : //! \file tok.h
28 : :
29 : : #if defined(__clang__)
30 : : #define FALLTHROUGH //[[clang::fallthrough]]
31 : : #elif defined(__GNUC__) && __GNUC__ >= 7
32 : : #define FALLTHROUGH __attribute__((fallthrough));
33 : : #else
34 : : #define FALLTHROUGH
35 : : #endif
36 : :
37 : : #include "tok.h"
38 : :
39 : : #include <stdarg.h>
40 : : #include <stdlib.h>
41 : : #include <stdio.h>
42 : : #include <ctype.h>
43 : : #include <string.h>
44 : : #include <limits.h>
45 : : #include <locale.h>
46 : : #include <math.h>
47 : :
48 : : #define NMEA_TOKS_COMPARE (1)
49 : : #define NMEA_TOKS_PERCENT (2)
50 : : #define NMEA_TOKS_WIDTH (3)
51 : : #define NMEA_TOKS_TYPE (4)
52 : :
53 : : /**
54 : : * \brief Calculate control sum of binary buffer
55 : : */
56 : 0 : int nmea_calc_crc( const char *buff, int buff_sz )
57 : : {
58 : 0 : int chsum = 0,
59 : : it;
60 : :
61 : 0 : for ( it = 0; it < buff_sz; ++it )
62 : 0 : chsum ^= ( int )buff[it];
63 : :
64 : 0 : return chsum;
65 : : }
66 : :
67 : : /**
68 : : * \brief Convert string to number
69 : : */
70 : 0 : int nmea_atoi( const char *str, size_t str_sz, int radix )
71 : : {
72 : 0 : char *tmp_ptr = 0;
73 : : char buff[NMEA_CONVSTR_BUF];
74 : 0 : int res = 0;
75 : :
76 : 0 : if ( str_sz < NMEA_CONVSTR_BUF )
77 : : {
78 : 0 : memcpy( &buff[0], str, str_sz );
79 : 0 : buff[str_sz] = '\0';
80 : 0 : res = strtol( &buff[0], &tmp_ptr, radix );
81 : 0 : }
82 : :
83 : 0 : return res;
84 : : }
85 : :
86 : : /**
87 : : * \brief Convert string to fraction number
88 : : */
89 : 0 : double nmea_atof( const char *str, int str_sz )
90 : : {
91 : 0 : char *tmp_ptr = 0;
92 : : char buff[NMEA_CONVSTR_BUF];
93 : 0 : double res = 0;
94 : :
95 : 0 : if ( str_sz < NMEA_CONVSTR_BUF )
96 : : {
97 : 0 : const char *oldlocale = setlocale( LC_NUMERIC, NULL );
98 : :
99 : 0 : memcpy( &buff[0], str, str_sz );
100 : 0 : buff[str_sz] = '\0';
101 : 0 : setlocale( LC_NUMERIC, "C" );
102 : 0 : res = strtod( &buff[0], &tmp_ptr );
103 : 0 : setlocale( LC_NUMERIC, oldlocale );
104 : 0 : }
105 : :
106 : 0 : return res;
107 : : }
108 : :
109 : : /**
110 : : * \brief Formatting string (like standard printf) with CRC tail (*CRC)
111 : : */
112 : 0 : int nmea_printf( char *buff, int buff_sz, const char *format, ... )
113 : : {
114 : 0 : int retval, add = 0;
115 : : va_list arg_ptr;
116 : :
117 : 0 : if ( buff_sz <= 0 )
118 : 0 : return 0;
119 : :
120 : 0 : va_start( arg_ptr, format );
121 : :
122 : 0 : retval = NMEA_POSIX( vsnprintf )( buff, buff_sz, format, arg_ptr );
123 : :
124 : 0 : if ( retval > 0 )
125 : : {
126 : 0 : add = NMEA_POSIX( snprintf )(
127 : 0 : buff + retval, buff_sz - retval, "*%02x\r\n",
128 : 0 : nmea_calc_crc( buff + 1, retval - 1 ) );
129 : 0 : }
130 : :
131 : 0 : retval += add;
132 : :
133 : 0 : if ( retval < 0 || retval > buff_sz )
134 : : {
135 : 0 : memset( buff, ' ', buff_sz );
136 : 0 : retval = buff_sz;
137 : 0 : }
138 : :
139 : 0 : va_end( arg_ptr );
140 : :
141 : 0 : return retval;
142 : 0 : }
143 : :
144 : : /**
145 : : * \brief Analyze string (specificate for NMEA sentences)
146 : : */
147 : 0 : int nmea_scanf( const char *buff, int buff_sz, const char *format, ... )
148 : : {
149 : 0 : const char *beg_tok = 0;
150 : 0 : const char *end_buf = buff + buff_sz;
151 : :
152 : : va_list arg_ptr;
153 : 0 : int tok_type = NMEA_TOKS_COMPARE;
154 : 0 : int width = 0;
155 : 0 : const char *beg_fmt = 0;
156 : 0 : int snum = 0, unum = 0;
157 : :
158 : 0 : int tok_count = 0;
159 : 0 : void *parg_target = 0;
160 : :
161 : 0 : va_start( arg_ptr, format );
162 : :
163 : 0 : for ( ; *format && buff < end_buf; ++format )
164 : : {
165 : 0 : switch ( tok_type )
166 : : {
167 : : case NMEA_TOKS_COMPARE:
168 : 0 : if ( '%' == *format )
169 : 0 : tok_type = NMEA_TOKS_PERCENT;
170 : 0 : else if ( *buff++ != *format )
171 : 0 : goto fail;
172 : 0 : break;
173 : : case NMEA_TOKS_PERCENT:
174 : 0 : width = 0;
175 : 0 : beg_fmt = format;
176 : 0 : tok_type = NMEA_TOKS_WIDTH;
177 : :
178 : : FALLTHROUGH
179 : :
180 : : case NMEA_TOKS_WIDTH:
181 : : {
182 : 0 : if ( isdigit( *format ) )
183 : 0 : break;
184 : :
185 : 0 : tok_type = NMEA_TOKS_TYPE;
186 : 0 : if ( format > beg_fmt )
187 : 0 : width = nmea_atoi( beg_fmt, ( int )( format - beg_fmt ), 10 );
188 : :
189 : : FALLTHROUGH
190 : 0 : }
191 : :
192 : : case NMEA_TOKS_TYPE:
193 : 0 : beg_tok = buff;
194 : :
195 : 0 : if ( !width && ( 'c' == *format || 'C' == *format ) && *buff != format[1] )
196 : 0 : width = 1;
197 : :
198 : 0 : if ( width )
199 : : {
200 : 0 : if ( buff + width <= end_buf )
201 : 0 : buff += width;
202 : : else
203 : 0 : goto fail;
204 : 0 : }
205 : : else
206 : : {
207 : 0 : if ( !format[1] || ( 0 == ( buff = ( char * )memchr( buff, format[1], end_buf - buff ) ) ) )
208 : 0 : buff = end_buf;
209 : : }
210 : :
211 : 0 : if ( buff > end_buf )
212 : 0 : goto fail;
213 : :
214 : 0 : tok_type = NMEA_TOKS_COMPARE;
215 : 0 : tok_count++;
216 : :
217 : 0 : parg_target = 0;
218 : 0 : width = ( int )( buff - beg_tok );
219 : :
220 : 0 : switch ( *format )
221 : : {
222 : : case 'c':
223 : : case 'C':
224 : 0 : parg_target = ( void * )va_arg( arg_ptr, char * );
225 : 0 : if ( width && 0 != ( parg_target ) )
226 : 0 : *( ( char * )parg_target ) = *beg_tok;
227 : 0 : break;
228 : : case 's':
229 : : case 'S':
230 : 0 : parg_target = ( void * )va_arg( arg_ptr, char * );
231 : 0 : if ( width && 0 != ( parg_target ) )
232 : : {
233 : 0 : memcpy( parg_target, beg_tok, width );
234 : 0 : ( ( char * )parg_target )[width] = '\0';
235 : 0 : }
236 : 0 : break;
237 : : case 'f':
238 : : case 'g':
239 : : case 'G':
240 : : case 'e':
241 : : case 'E':
242 : 0 : parg_target = ( void * )va_arg( arg_ptr, double * );
243 : 0 : if ( width && 0 != ( parg_target ) )
244 : 0 : *( ( double * )parg_target ) = nmea_atof( beg_tok, width );
245 : 0 : else if ( width == 0 && 0 != ( parg_target ) )
246 : : {
247 : 0 : *( ( double * )parg_target ) = NAN;
248 : 0 : }
249 : 0 : break;
250 : : };
251 : :
252 : 0 : if ( parg_target )
253 : 0 : break;
254 : 0 : if ( 0 == ( parg_target = ( void * )va_arg( arg_ptr, int * ) ) )
255 : 0 : break;
256 : 0 : if ( !width )
257 : 0 : break;
258 : :
259 : 0 : switch ( *format )
260 : : {
261 : : case 'd':
262 : : case 'i':
263 : 0 : snum = nmea_atoi( beg_tok, width, 10 );
264 : 0 : memcpy( parg_target, &snum, sizeof( int ) );
265 : 0 : break;
266 : : case 'u':
267 : 0 : unum = nmea_atoi( beg_tok, width, 10 );
268 : 0 : memcpy( parg_target, &unum, sizeof( unsigned int ) );
269 : 0 : break;
270 : : case 'x':
271 : : case 'X':
272 : 0 : unum = nmea_atoi( beg_tok, width, 16 );
273 : 0 : memcpy( parg_target, &unum, sizeof( unsigned int ) );
274 : 0 : break;
275 : : case 'o':
276 : 0 : unum = nmea_atoi( beg_tok, width, 8 );
277 : 0 : memcpy( parg_target, &unum, sizeof( unsigned int ) );
278 : 0 : break;
279 : : default:
280 : 0 : goto fail;
281 : : };
282 : :
283 : 0 : break;
284 : : };
285 : 0 : }
286 : :
287 : : fail:
288 : :
289 : 0 : va_end( arg_ptr );
290 : :
291 : 0 : return tok_count;
292 : : }
|
