diff options
Diffstat (limited to 'firmware')
-rw-r--r-- | firmware/common/sprintf.c | 119 |
1 files changed, 101 insertions, 18 deletions
diff --git a/firmware/common/sprintf.c b/firmware/common/sprintf.c index c18a390b44..ddbae9a473 100644 --- a/firmware/common/sprintf.c +++ b/firmware/common/sprintf.c | |||
@@ -26,21 +26,27 @@ | |||
26 | 26 | ||
27 | #include <stdarg.h> | 27 | #include <stdarg.h> |
28 | #include <string.h> | 28 | #include <string.h> |
29 | #include <stdbool.h> | ||
30 | |||
31 | #include "file.h" /* for write(), used in fprintf() */ | ||
29 | 32 | ||
30 | static const char hexdigit[] = "0123456789ABCDEF"; | 33 | static const char hexdigit[] = "0123456789ABCDEF"; |
31 | 34 | ||
32 | int vsnprintf (char *buf, int size, const char *fmt, va_list ap) | 35 | static int format( |
36 | /* call 'push()' for each output letter */ | ||
37 | int (*push)(void *userp, unsigned char data), | ||
38 | void *userp, | ||
39 | const char *fmt, | ||
40 | va_list ap) | ||
33 | { | 41 | { |
34 | char *bp = buf; | ||
35 | char *end = buf + size - 1; | ||
36 | |||
37 | char *str; | 42 | char *str; |
38 | char tmpbuf[12], pad; | 43 | char tmpbuf[12], pad; |
39 | int ch, width, val, sign; | 44 | int ch, width, val, sign; |
45 | bool ok = true; | ||
40 | 46 | ||
41 | tmpbuf[sizeof tmpbuf - 1] = '\0'; | 47 | tmpbuf[sizeof tmpbuf - 1] = '\0'; |
42 | 48 | ||
43 | while ((ch = *fmt++) != '\0' && bp < end) | 49 | while ((ch = *fmt++) != '\0' && ok) |
44 | { | 50 | { |
45 | if (ch == '%') | 51 | if (ch == '%') |
46 | { | 52 | { |
@@ -100,28 +106,105 @@ int vsnprintf (char *buf, int size, const char *fmt, va_list ap) | |||
100 | if (width > 0) | 106 | if (width > 0) |
101 | { | 107 | { |
102 | width -= strlen (str); | 108 | width -= strlen (str); |
103 | while (width-- > 0 && bp < end) | 109 | while (width-- > 0 && ok) |
104 | *bp++ = pad; | 110 | ok=push(userp, pad); |
105 | } | 111 | } |
106 | while (*str != '\0' && bp < end) | 112 | while (*str != '\0' && ok) |
107 | *bp++ = *str++; | 113 | ok=push(userp, *str++); |
108 | } | 114 | } |
109 | else | 115 | else |
110 | *bp++ = ch; | 116 | ok=push(userp, ch); |
117 | } | ||
118 | return ok; /* true means good */ | ||
119 | } | ||
120 | |||
121 | struct for_snprintf { | ||
122 | unsigned char *ptr; /* where to store it */ | ||
123 | int bytes; /* amount already stored */ | ||
124 | int max; /* max amount to store */ | ||
125 | }; | ||
126 | |||
127 | static int sprfunc(void *ptr, unsigned char letter) | ||
128 | { | ||
129 | struct for_snprintf *pr = (struct for_snprintf *)ptr; | ||
130 | if(pr->bytes < pr->max) { | ||
131 | *pr->ptr = letter; | ||
132 | pr->ptr++; | ||
133 | pr->bytes++; | ||
134 | return true; | ||
135 | } | ||
136 | return false; /* filled buffer */ | ||
137 | } | ||
138 | |||
139 | |||
140 | int snprintf(char *buf, int size, const char *fmt, ...) | ||
141 | { | ||
142 | bool ok; | ||
143 | va_list ap; | ||
144 | struct for_snprintf pr; | ||
145 | |||
146 | pr.ptr = buf; | ||
147 | pr.bytes = 0; | ||
148 | pr.max = size; | ||
149 | |||
150 | va_start(ap, fmt); | ||
151 | ok = format(sprfunc, &pr, fmt, ap); | ||
152 | va_end(ap); | ||
153 | |||
154 | /* make sure it ends with a trailing zero */ | ||
155 | pr.ptr[ok?0:-1]='\0'; | ||
156 | |||
157 | return pr.bytes; | ||
158 | } | ||
159 | |||
160 | int vsnprintf(char *buf, int size, const char *fmt, va_list ap) | ||
161 | { | ||
162 | bool ok; | ||
163 | struct for_snprintf pr; | ||
164 | |||
165 | pr.ptr = buf; | ||
166 | pr.bytes = 0; | ||
167 | pr.max = size; | ||
168 | |||
169 | ok = format(sprfunc, &pr, fmt, ap); | ||
170 | |||
171 | /* make sure it ends with a trailing zero */ | ||
172 | pr.ptr[ok?0:-1]='\0'; | ||
173 | |||
174 | return pr.bytes; | ||
175 | } | ||
176 | |||
177 | struct for_fprintf { | ||
178 | int fd; /* where to store it */ | ||
179 | int bytes; /* amount stored */ | ||
180 | }; | ||
181 | |||
182 | static int fprfunc(void *pr, unsigned char letter) | ||
183 | { | ||
184 | struct for_fprintf *fpr = (struct for_fprintf *)pr; | ||
185 | int rc = write(fpr->fd, &letter, 1); | ||
186 | |||
187 | if(rc > 0) { | ||
188 | fpr->bytes++; /* count them */ | ||
189 | return true; /* we are ok */ | ||
111 | } | 190 | } |
112 | 191 | ||
113 | *bp++ = '\0'; | 192 | return false; /* failure */ |
114 | return bp - buf - 1; | ||
115 | } | 193 | } |
116 | 194 | ||
117 | int snprintf (char *buf, int size, const char *fmt, ...) | 195 | |
196 | int fprintf(int fd, const char *fmt, ...) | ||
118 | { | 197 | { |
119 | int n; | 198 | bool ok; |
120 | va_list ap; | 199 | va_list ap; |
200 | struct for_fprintf fpr; | ||
201 | |||
202 | fpr.fd=fd; | ||
203 | fpr.bytes=0; | ||
121 | 204 | ||
122 | va_start (ap, fmt); | 205 | va_start(ap, fmt); |
123 | n = vsnprintf (buf, size, fmt, ap); | 206 | ok = format(fprfunc, &fpr, fmt, ap); |
124 | va_end (ap); | 207 | va_end(ap); |
125 | 208 | ||
126 | return n; | 209 | return fpr.bytes; /* return 0 on error */ |
127 | } | 210 | } |