diff options
Diffstat (limited to 'bootloader/format.c')
-rw-r--r-- | bootloader/format.c | 324 |
1 files changed, 324 insertions, 0 deletions
diff --git a/bootloader/format.c b/bootloader/format.c new file mode 100644 index 0000000000..8f28444b8c --- /dev/null +++ b/bootloader/format.c | |||
@@ -0,0 +1,324 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2002 by Gary Czvitkovicz | ||
11 | * Copyright (C) 2017 by William Wilgus | ||
12 | * | ||
13 | * This program is free software; you can redistribute it and/or | ||
14 | * modify it under the terms of the GNU General Public License | ||
15 | * as published by the Free Software Foundation; either version 2 | ||
16 | * of the License, or (at your option) any later version. | ||
17 | * | ||
18 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
19 | * KIND, either express or implied. | ||
20 | * | ||
21 | ****************************************************************************/ | ||
22 | |||
23 | |||
24 | #include <stdarg.h> | ||
25 | #include <stdbool.h> | ||
26 | #include <limits.h> | ||
27 | #include <string.h> | ||
28 | #include "file.h" | ||
29 | #include "format.h" | ||
30 | |||
31 | static const char hexdigit[] = "0123456789ABCDEF"; | ||
32 | |||
33 | /* smaller compressed binary without inline but 18% slower */ | ||
34 | #define FMT_DECL static inline | ||
35 | |||
36 | FMT_DECL int fmt_width_precision(int *ch, const char **fmt, char **str, va_list *ap) | ||
37 | { | ||
38 | int value = 0; | ||
39 | (void) str; | ||
40 | (void) ap; | ||
41 | |||
42 | while (*ch >= '0' && *ch <= '9') | ||
43 | { | ||
44 | value = 10 * value + (*ch - '0'); | ||
45 | *ch = *(*fmt)++; | ||
46 | } | ||
47 | return value; | ||
48 | } | ||
49 | |||
50 | FMT_DECL int fmt_integer_signed(int *ch, const char **fmt, char **str, va_list *ap) | ||
51 | { | ||
52 | int val, rem, sign; | ||
53 | (void) ch; | ||
54 | (void) fmt; | ||
55 | |||
56 | val = sign = va_arg(*ap, int); | ||
57 | if (val < 0) | ||
58 | val = -val; | ||
59 | do { | ||
60 | rem = val % 10; | ||
61 | val /= 10; | ||
62 | *--(*str) = rem + '0'; | ||
63 | |||
64 | } while (val > 0); | ||
65 | |||
66 | if (sign < 0) | ||
67 | *--(*str) = '-'; | ||
68 | return 0; | ||
69 | } | ||
70 | |||
71 | FMT_DECL int fmt_integer_unsigned(int *ch, const char **fmt, char **str, va_list *ap) | ||
72 | { | ||
73 | unsigned int uval, urem; | ||
74 | (void) ch; | ||
75 | (void) fmt; | ||
76 | |||
77 | uval = va_arg(*ap, unsigned int); | ||
78 | do { | ||
79 | urem = uval % 10; | ||
80 | uval /= 10; | ||
81 | *--(*str) = urem + '0'; | ||
82 | } while (uval > 0); | ||
83 | return 0; | ||
84 | } | ||
85 | |||
86 | FMT_DECL int fmt_long(int *ch, const char **fmt, char **str, va_list *ap) | ||
87 | { | ||
88 | int pad = 0; | ||
89 | long lval, lrem, lsign = 0; | ||
90 | unsigned long ulval, ulrem; | ||
91 | char ch_l = *ch; | ||
92 | |||
93 | *ch = *(*fmt)++; | ||
94 | if (*ch == 'd') { | ||
95 | lval = lsign = va_arg(*ap, long); | ||
96 | |||
97 | if (lval < 0) | ||
98 | lval = -lval; | ||
99 | do { | ||
100 | lrem = lval % 10; | ||
101 | lval /= 10; | ||
102 | *--(*str) = lrem + '0'; | ||
103 | } while (lval > 0); | ||
104 | |||
105 | if (lsign < 0) | ||
106 | *--(*str) = '-'; | ||
107 | } | ||
108 | else if (*ch == 'u') { | ||
109 | ulval = va_arg(*ap, unsigned long); | ||
110 | do { | ||
111 | ulrem = ulval % 10; | ||
112 | ulval /= 10; | ||
113 | *--(*str) = ulrem + '0'; | ||
114 | } while (ulval > 0); | ||
115 | } | ||
116 | else if (*ch == 'x' || *ch == 'X') { | ||
117 | pad++; | ||
118 | ulval = va_arg(*ap, long); | ||
119 | do { | ||
120 | *--(*str) = hexdigit[ulval & 0xf]; | ||
121 | ulval >>= 4; | ||
122 | } while (ulval > 0); | ||
123 | } | ||
124 | else { | ||
125 | *--(*str) = ch_l; | ||
126 | *--(*str) = *ch; | ||
127 | } | ||
128 | return pad; | ||
129 | } | ||
130 | |||
131 | FMT_DECL int fmt_character(int *ch, const char **fmt, char **str, va_list *ap) | ||
132 | { | ||
133 | (void) ch; | ||
134 | (void) fmt; | ||
135 | |||
136 | *--(*str) = va_arg(*ap, int); | ||
137 | return 0; | ||
138 | } | ||
139 | |||
140 | FMT_DECL int fmt_string(int *ch, const char **fmt, char **str, va_list *ap) | ||
141 | { | ||
142 | (void) ch; | ||
143 | (void) fmt; | ||
144 | |||
145 | *str = va_arg (*ap, char*); | ||
146 | return 0; | ||
147 | } | ||
148 | |||
149 | FMT_DECL int fmt_hex_unsigned(int *ch, const char **fmt, char **str, va_list *ap) | ||
150 | { | ||
151 | unsigned int uval; | ||
152 | (void) ch; | ||
153 | (void) fmt; | ||
154 | |||
155 | uval = va_arg(*ap, int); | ||
156 | do { | ||
157 | *--(*str) = hexdigit[uval & 0xf]; | ||
158 | uval >>= 4; | ||
159 | } while (uval > 0); | ||
160 | return 1; | ||
161 | } | ||
162 | |||
163 | FMT_DECL int fmt_pointer(int *ch, const char **fmt, char **str, va_list *ap) | ||
164 | { | ||
165 | int pad = fmt_hex_unsigned(ch, fmt, str, ap); | ||
166 | /* for pointers prepend 0x and act like 'X' */ | ||
167 | *--(*str) = 'x'; | ||
168 | *--(*str) = '0'; | ||
169 | return pad; | ||
170 | } | ||
171 | |||
172 | FMT_DECL int fmt_sizet(int *ch, const char **fmt, char **str, va_list *ap) | ||
173 | { | ||
174 | size_t uszval, uszrem; | ||
175 | ssize_t szval, szrem, szsign; | ||
176 | char ch_z = *ch; | ||
177 | *ch = *(*fmt)++; | ||
178 | |||
179 | if (*ch == 'd') { | ||
180 | szval = szsign = va_arg(*ap, ssize_t); | ||
181 | if (szval < 0) | ||
182 | szval = -szval; | ||
183 | do { | ||
184 | szrem = szval % 10; | ||
185 | szval /= 10; | ||
186 | *--(*str) = szrem + '0'; | ||
187 | } while (szval > 0); | ||
188 | |||
189 | if (szsign < 0) | ||
190 | *--(*str) = '-'; | ||
191 | } | ||
192 | else if (*ch == 'u') { | ||
193 | uszval = va_arg(*ap, size_t); | ||
194 | do { | ||
195 | uszrem = uszval % 10; | ||
196 | uszval /= 10; | ||
197 | *--(*str) = uszrem + '0'; | ||
198 | } while (uszval > 0); | ||
199 | } | ||
200 | else { | ||
201 | *--(*str) = ch_z; | ||
202 | *--(*str) = *ch; | ||
203 | } | ||
204 | return 0; | ||
205 | } | ||
206 | |||
207 | static inline int fmt_next_char(int *ch, const char **fmt, char **str, va_list *ap) | ||
208 | { | ||
209 | (void) fmt; | ||
210 | (void) ap; | ||
211 | |||
212 | *--(*str) = *ch; | ||
213 | return 0; | ||
214 | } | ||
215 | |||
216 | |||
217 | |||
218 | void format( | ||
219 | /* call 'push()' for each output letter */ | ||
220 | int (*push)(void *userp, unsigned char data), | ||
221 | void *userp, | ||
222 | const char *fmt, | ||
223 | va_list ap) | ||
224 | { | ||
225 | bool ok = true; | ||
226 | char *str; | ||
227 | char tmpbuf[12], pad; | ||
228 | int ch, width, precision, padded; | ||
229 | |||
230 | |||
231 | ch = *fmt++; | ||
232 | tmpbuf[sizeof tmpbuf - 1] = '\0'; | ||
233 | |||
234 | do | ||
235 | { | ||
236 | if (ch == '%') | ||
237 | { | ||
238 | str = tmpbuf + sizeof tmpbuf - 1; | ||
239 | ch = *fmt++; | ||
240 | padded = (ch == '0' ? 1 : 0); | ||
241 | width = fmt_width_precision(&ch, &fmt, &str, &ap); | ||
242 | |||
243 | precision = INT_MAX; | ||
244 | if(ch == '.') | ||
245 | { | ||
246 | ch = *fmt++; | ||
247 | precision = fmt_width_precision(&ch, &fmt, &str, &ap); | ||
248 | } | ||
249 | |||
250 | if (ch == 'd') | ||
251 | fmt_integer_signed(&ch, &fmt, &str, &ap); | ||
252 | else if (ch == 'u') | ||
253 | fmt_integer_unsigned(&ch, &fmt, &str, &ap); | ||
254 | else if (ch == 'l') | ||
255 | padded += fmt_long(&ch, &fmt, &str, &ap); | ||
256 | else if (ch == 'c') | ||
257 | fmt_character(&ch, &fmt, &str, &ap); | ||
258 | else if (ch == 's') | ||
259 | fmt_string(&ch, &fmt, &str, &ap); | ||
260 | else if (ch == 'x' || ch == 'X') | ||
261 | padded += fmt_hex_unsigned(&ch, &fmt, &str, &ap); | ||
262 | else if (ch == 'p' || ch == 'P') | ||
263 | padded += fmt_pointer(&ch, &fmt, &str, &ap); | ||
264 | #if 0 | ||
265 | else if (ch == 'z') | ||
266 | fmt_sizet(&ch, &fmt, &str, &ap); | ||
267 | #endif | ||
268 | else | ||
269 | fmt_next_char(&ch, &fmt, &str, &ap); | ||
270 | |||
271 | width -= strlen (str); | ||
272 | if (width > 0) | ||
273 | { | ||
274 | pad = (padded ? '0' : ' '); | ||
275 | while (width-- > 0 && ok) | ||
276 | ok=push(userp, pad); | ||
277 | } | ||
278 | while(*str != '\0' && ok && precision--) | ||
279 | ok=push(userp, *str++); | ||
280 | } | ||
281 | else | ||
282 | ok=push(userp, ch); | ||
283 | |||
284 | } while ((ch = *fmt++) != '\0' && ok); | ||
285 | } | ||
286 | |||
287 | struct for_fprintf { | ||
288 | int fd; /* where to store it */ | ||
289 | int bytes; /* amount stored */ | ||
290 | }; | ||
291 | |||
292 | static int fprfunc(void *pr, unsigned char letter) | ||
293 | { | ||
294 | struct for_fprintf *fpr = (struct for_fprintf *)pr; | ||
295 | int rc = write(fpr->fd, &letter, 1); | ||
296 | |||
297 | if(rc > 0) { | ||
298 | fpr->bytes++; /* count them */ | ||
299 | return true; /* we are ok */ | ||
300 | } | ||
301 | |||
302 | return false; /* failure */ | ||
303 | } | ||
304 | |||
305 | |||
306 | int fdprintf(int fd, const char *fmt, ...) | ||
307 | { | ||
308 | va_list ap; | ||
309 | struct for_fprintf fpr; | ||
310 | |||
311 | fpr.fd=fd; | ||
312 | fpr.bytes=0; | ||
313 | |||
314 | va_start(ap, fmt); | ||
315 | format(fprfunc, &fpr, fmt, ap); | ||
316 | va_end(ap); | ||
317 | |||
318 | return fpr.bytes; /* return 0 on error */ | ||
319 | } | ||
320 | |||
321 | void vuprintf(int (*push)(void *userp, unsigned char data), void *userp, const char *fmt, va_list ap) | ||
322 | { | ||
323 | format(push, userp, fmt, ap); | ||
324 | } | ||