diff options
Diffstat (limited to 'firmware/common/format.c')
-rw-r--r-- | firmware/common/format.c | 227 |
1 files changed, 227 insertions, 0 deletions
diff --git a/firmware/common/format.c b/firmware/common/format.c new file mode 100644 index 0000000000..987af417da --- /dev/null +++ b/firmware/common/format.c | |||
@@ -0,0 +1,227 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2002 by Gary Czvitkovicz | ||
11 | * | ||
12 | * This program is free software; you can redistribute it and/or | ||
13 | * modify it under the terms of the GNU General Public License | ||
14 | * as published by the Free Software Foundation; either version 2 | ||
15 | * of the License, or (at your option) any later version. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | |||
22 | |||
23 | #include <stdarg.h> | ||
24 | #include <stdbool.h> | ||
25 | #include <limits.h> | ||
26 | #include <string.h> | ||
27 | #include "file.h" | ||
28 | |||
29 | static const char hexdigit[] = "0123456789ABCDEF"; | ||
30 | |||
31 | int format( | ||
32 | /* call 'push()' for each output letter */ | ||
33 | int (*push)(void *userp, unsigned char data), | ||
34 | void *userp, | ||
35 | const char *fmt, | ||
36 | va_list ap) | ||
37 | { | ||
38 | char *str; | ||
39 | char tmpbuf[12], pad; | ||
40 | int ch, width, val, sign, precision; | ||
41 | long lval, lsign; | ||
42 | unsigned int uval; | ||
43 | unsigned long ulval; | ||
44 | bool ok = true; | ||
45 | |||
46 | tmpbuf[sizeof tmpbuf - 1] = '\0'; | ||
47 | |||
48 | while ((ch = *fmt++) != '\0' && ok) | ||
49 | { | ||
50 | if (ch == '%') | ||
51 | { | ||
52 | ch = *fmt++; | ||
53 | pad = ' '; | ||
54 | if (ch == '0') | ||
55 | pad = '0'; | ||
56 | |||
57 | width = 0; | ||
58 | while (ch >= '0' && ch <= '9') | ||
59 | { | ||
60 | width = 10*width + ch - '0'; | ||
61 | ch = *fmt++; | ||
62 | } | ||
63 | |||
64 | precision = 0; | ||
65 | if(ch == '.') | ||
66 | { | ||
67 | ch = *fmt++; | ||
68 | while (ch >= '0' && ch <= '9') | ||
69 | { | ||
70 | precision = 10*precision + ch - '0'; | ||
71 | ch = *fmt++; | ||
72 | } | ||
73 | } else { | ||
74 | precision = INT_MAX; | ||
75 | } | ||
76 | |||
77 | str = tmpbuf + sizeof tmpbuf - 1; | ||
78 | switch (ch) | ||
79 | { | ||
80 | case 'c': | ||
81 | *--str = va_arg (ap, int); | ||
82 | break; | ||
83 | |||
84 | case 's': | ||
85 | str = va_arg (ap, char*); | ||
86 | break; | ||
87 | |||
88 | case 'd': | ||
89 | val = sign = va_arg (ap, int); | ||
90 | if (val < 0) | ||
91 | val = -val; | ||
92 | do | ||
93 | { | ||
94 | *--str = (val % 10) + '0'; | ||
95 | val /= 10; | ||
96 | } | ||
97 | while (val > 0); | ||
98 | if (sign < 0) | ||
99 | *--str = '-'; | ||
100 | break; | ||
101 | |||
102 | case 'u': | ||
103 | uval = va_arg(ap, unsigned int); | ||
104 | do | ||
105 | { | ||
106 | *--str = (uval % 10) + '0'; | ||
107 | uval /= 10; | ||
108 | } | ||
109 | while (uval > 0); | ||
110 | break; | ||
111 | |||
112 | case 'x': | ||
113 | case 'X': | ||
114 | pad='0'; | ||
115 | uval = va_arg (ap, int); | ||
116 | do | ||
117 | { | ||
118 | *--str = hexdigit[uval & 0xf]; | ||
119 | uval >>= 4; | ||
120 | } | ||
121 | while (uval); | ||
122 | break; | ||
123 | |||
124 | case 'l': | ||
125 | ch = *fmt++; | ||
126 | switch(ch) { | ||
127 | case 'x': | ||
128 | case 'X': | ||
129 | pad='0'; | ||
130 | ulval = va_arg (ap, long); | ||
131 | do | ||
132 | { | ||
133 | *--str = hexdigit[ulval & 0xf]; | ||
134 | ulval >>= 4; | ||
135 | } | ||
136 | while (ulval); | ||
137 | break; | ||
138 | case 'd': | ||
139 | lval = lsign = va_arg (ap, long); | ||
140 | if (lval < 0) | ||
141 | lval = -lval; | ||
142 | do | ||
143 | { | ||
144 | *--str = (lval % 10) + '0'; | ||
145 | lval /= 10; | ||
146 | } | ||
147 | while (lval > 0); | ||
148 | if (lsign < 0) | ||
149 | *--str = '-'; | ||
150 | break; | ||
151 | |||
152 | case 'u': | ||
153 | ulval = va_arg(ap, unsigned long); | ||
154 | do | ||
155 | { | ||
156 | *--str = (ulval % 10) + '0'; | ||
157 | ulval /= 10; | ||
158 | } | ||
159 | while (ulval > 0); | ||
160 | break; | ||
161 | |||
162 | default: | ||
163 | *--str = 'l'; | ||
164 | *--str = ch; | ||
165 | } | ||
166 | |||
167 | break; | ||
168 | |||
169 | default: | ||
170 | *--str = ch; | ||
171 | break; | ||
172 | } | ||
173 | |||
174 | if (width > 0) | ||
175 | { | ||
176 | width -= strlen (str); | ||
177 | while (width-- > 0 && ok) | ||
178 | ok=push(userp, pad); | ||
179 | } | ||
180 | while (*str != '\0' && ok && precision--) | ||
181 | ok=push(userp, *str++); | ||
182 | } | ||
183 | else | ||
184 | ok=push(userp, ch); | ||
185 | } | ||
186 | return ok; /* true means good */ | ||
187 | } | ||
188 | |||
189 | struct for_fprintf { | ||
190 | int fd; /* where to store it */ | ||
191 | int bytes; /* amount stored */ | ||
192 | }; | ||
193 | |||
194 | static int fprfunc(void *pr, unsigned char letter) | ||
195 | { | ||
196 | struct for_fprintf *fpr = (struct for_fprintf *)pr; | ||
197 | int rc = write(fpr->fd, &letter, 1); | ||
198 | |||
199 | if(rc > 0) { | ||
200 | fpr->bytes++; /* count them */ | ||
201 | return true; /* we are ok */ | ||
202 | } | ||
203 | |||
204 | return false; /* failure */ | ||
205 | } | ||
206 | |||
207 | |||
208 | int fdprintf(int fd, const char *fmt, ...) | ||
209 | { | ||
210 | bool ok; | ||
211 | va_list ap; | ||
212 | struct for_fprintf fpr; | ||
213 | |||
214 | fpr.fd=fd; | ||
215 | fpr.bytes=0; | ||
216 | |||
217 | va_start(ap, fmt); | ||
218 | ok = format(fprfunc, &fpr, fmt, ap); | ||
219 | va_end(ap); | ||
220 | |||
221 | return fpr.bytes; /* return 0 on error */ | ||
222 | } | ||
223 | |||
224 | int vuprintf(int (*push)(void *userp, unsigned char data), void *userp, const char *fmt, va_list ap) | ||
225 | { | ||
226 | return format(push, userp, fmt, ap); | ||
227 | } | ||