diff options
-rw-r--r-- | firmware/common/sscanf.c | 216 | ||||
-rw-r--r-- | firmware/include/sscanf.h | 30 |
2 files changed, 246 insertions, 0 deletions
diff --git a/firmware/common/sscanf.c b/firmware/common/sscanf.c new file mode 100644 index 0000000000..a63a384456 --- /dev/null +++ b/firmware/common/sscanf.c | |||
@@ -0,0 +1,216 @@ | |||
1 | #include <stdarg.h> | ||
2 | #include <string.h> | ||
3 | #include <stdbool.h> | ||
4 | |||
5 | static inline bool isspace(char c) | ||
6 | { | ||
7 | return (c == ' ') || (c == '\t') || (c == '\n'); | ||
8 | } | ||
9 | |||
10 | static inline bool isdigit(char c) | ||
11 | { | ||
12 | return (c >= '0') && (c <= '9'); | ||
13 | } | ||
14 | |||
15 | static inline bool isxdigit(char c) | ||
16 | { | ||
17 | return ((c >= '0') && (c <= '9')) | ||
18 | || ((c >= 'a') && (c <= 'f')) || ((c >= 'A') && (c <= 'F')); | ||
19 | } | ||
20 | |||
21 | static int parse_dec(int (*peek)(void *userp), | ||
22 | void (*pop)(void *userp), | ||
23 | void *userp, | ||
24 | long *vp) | ||
25 | { | ||
26 | long v = 0; | ||
27 | int n = 0; | ||
28 | int minus = 0; | ||
29 | char ch; | ||
30 | |||
31 | if ((*peek)(userp) == '-') | ||
32 | { | ||
33 | (*pop)(userp); | ||
34 | n++; | ||
35 | minus = 1; | ||
36 | } | ||
37 | |||
38 | ch = (*peek)(userp); | ||
39 | if (!isdigit(ch)) | ||
40 | return -1; | ||
41 | |||
42 | do | ||
43 | { | ||
44 | v = v * 10 + ch - '0'; | ||
45 | (*pop)(userp); | ||
46 | n++; | ||
47 | ch = (*peek)(userp); | ||
48 | } while (isdigit(ch)); | ||
49 | |||
50 | *vp = minus ? -v : v; | ||
51 | return n; | ||
52 | } | ||
53 | |||
54 | static int parse_hex(int (*peek)(void *userp), | ||
55 | void (*pop)(void *userp), | ||
56 | void *userp, | ||
57 | unsigned long *vp) | ||
58 | { | ||
59 | unsigned long v = 0; | ||
60 | int n = 0; | ||
61 | char ch; | ||
62 | |||
63 | ch = (*peek)(userp); | ||
64 | if (!isxdigit(ch)) | ||
65 | return -1; | ||
66 | |||
67 | do | ||
68 | { | ||
69 | if (ch >= 'a') | ||
70 | ch = ch - 'a' + 10; | ||
71 | else if (ch >= 'A') | ||
72 | ch = ch - 'A' + 10; | ||
73 | else | ||
74 | ch = ch - '0'; | ||
75 | v = v * 16 + ch; | ||
76 | (*pop)(userp); | ||
77 | n++; | ||
78 | ch = (*peek)(userp); | ||
79 | } while (isxdigit(ch)); | ||
80 | |||
81 | *vp = v; | ||
82 | return n; | ||
83 | } | ||
84 | |||
85 | static int scan(int (*peek)(void *userp), | ||
86 | void (*pop)(void *userp), | ||
87 | void *userp, | ||
88 | const char *fmt, | ||
89 | va_list ap) | ||
90 | { | ||
91 | char ch; | ||
92 | int n = 0; | ||
93 | int n_chars = 0; | ||
94 | int r; | ||
95 | long lval; | ||
96 | unsigned long ulval; | ||
97 | |||
98 | while ((ch = *fmt++) != '\0') | ||
99 | { | ||
100 | bool literal = false; | ||
101 | |||
102 | if (ch == '%') | ||
103 | { | ||
104 | ch = *fmt++; | ||
105 | |||
106 | while (isspace((*peek)(userp))) { | ||
107 | n_chars++; | ||
108 | (*pop)(userp); | ||
109 | } | ||
110 | |||
111 | switch (ch) | ||
112 | { | ||
113 | case 'x': | ||
114 | if ((r = parse_hex(peek, pop, userp, &ulval)) >= 0) | ||
115 | { | ||
116 | *(va_arg(ap, unsigned int *)) = ulval; | ||
117 | n++; | ||
118 | n_chars += r; | ||
119 | } | ||
120 | else | ||
121 | return n; | ||
122 | break; | ||
123 | case 'd': | ||
124 | if ((r = parse_dec(peek, pop, userp, &lval)) >= 0) | ||
125 | { | ||
126 | *(va_arg(ap, int *)) = lval; | ||
127 | n++; | ||
128 | n_chars += r; | ||
129 | } | ||
130 | else | ||
131 | return n; | ||
132 | break; | ||
133 | case 'n': | ||
134 | *(va_arg(ap, int *)) = n_chars; | ||
135 | n++; | ||
136 | break; | ||
137 | case 'l': | ||
138 | ch = *fmt++; | ||
139 | switch (ch) | ||
140 | { | ||
141 | case 'x': | ||
142 | if ((r = parse_hex(peek, pop, userp, &ulval)) >= 0) | ||
143 | { | ||
144 | *(va_arg(ap, unsigned long *)) = ulval; | ||
145 | n++; | ||
146 | n_chars += r; | ||
147 | } | ||
148 | else | ||
149 | return n; | ||
150 | break; | ||
151 | case 'd': | ||
152 | if ((r = parse_dec(peek, pop, userp, &lval)) >= 0) | ||
153 | { | ||
154 | *(va_arg(ap, long *)) = lval; | ||
155 | n++; | ||
156 | n_chars += r; | ||
157 | } | ||
158 | else | ||
159 | return n; | ||
160 | break; | ||
161 | case '\0': | ||
162 | return n; | ||
163 | default: | ||
164 | literal = true; | ||
165 | break; | ||
166 | } | ||
167 | break; | ||
168 | case '\0': | ||
169 | return n; | ||
170 | default: | ||
171 | literal = true; | ||
172 | break; | ||
173 | } | ||
174 | } else | ||
175 | literal = true; | ||
176 | |||
177 | if (literal) | ||
178 | { | ||
179 | while (isspace((*peek)(userp))) { | ||
180 | (*pop)(userp); | ||
181 | n_chars++; | ||
182 | } | ||
183 | if ((*peek)(userp) != ch) | ||
184 | return n; | ||
185 | else | ||
186 | { | ||
187 | (*pop)(userp); | ||
188 | n_chars++; | ||
189 | } | ||
190 | } | ||
191 | } | ||
192 | return n; | ||
193 | } | ||
194 | |||
195 | static int sspeek(void *userp) | ||
196 | { | ||
197 | return **((char **)userp); | ||
198 | } | ||
199 | |||
200 | static void sspop(void *userp) | ||
201 | { | ||
202 | (*((char **)userp))++; | ||
203 | } | ||
204 | |||
205 | int sscanf(const char *s, const char *fmt, ...) | ||
206 | { | ||
207 | int r; | ||
208 | va_list ap; | ||
209 | const char *p; | ||
210 | |||
211 | p = s; | ||
212 | va_start(ap, fmt); | ||
213 | r = scan(sspeek, sspop, &p, fmt, ap); | ||
214 | va_end(ap); | ||
215 | return r; | ||
216 | } | ||
diff --git a/firmware/include/sscanf.h b/firmware/include/sscanf.h new file mode 100644 index 0000000000..e2fd3a9aec --- /dev/null +++ b/firmware/include/sscanf.h | |||
@@ -0,0 +1,30 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2006 by Tomasz Malesinski | ||
11 | * | ||
12 | * All files in this archive are subject to the GNU General Public License. | ||
13 | * See the file COPYING in the source tree root for full license agreement. | ||
14 | * | ||
15 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
16 | * KIND, either express or implied. | ||
17 | * | ||
18 | ****************************************************************************/ | ||
19 | |||
20 | #ifndef __SSCANF_H__ | ||
21 | #define __SSCANF_H__ | ||
22 | |||
23 | #include <stddef.h> | ||
24 | #include <stdarg.h> | ||
25 | #include <_ansi.h> | ||
26 | |||
27 | int sscanf(const char *s, const char *fmt, ...) | ||
28 | ATTRIBUTE_SCANF(2, 3); | ||
29 | |||
30 | #endif /* __SSCANF_H__ */ | ||