Rewrote printf and friends to better support standard C semantics.
[people/xl0/gpxe.git] / src / core / vsprintf.c
1 #include <stddef.h>
2 #include <stdarg.h>
3 #include <console.h>
4 #include <errno.h>
5 #include <vsprintf.h>
6
7 #define CHAR_LEN        1
8 #define SHORT_LEN       2
9 #define INT_LEN         3
10 #define LONG_LEN        4
11 #define LONGLONG_LEN    5
12 #define SIZE_T_LEN      6
13
14 static uint8_t type_sizes[] = {
15         [CHAR_LEN]      = sizeof ( char ),
16         [SHORT_LEN]     = sizeof ( short ),
17         [INT_LEN]       = sizeof ( int ),
18         [LONG_LEN]      = sizeof ( long ),
19         [LONGLONG_LEN]  = sizeof ( long long ),
20         [SIZE_T_LEN]    = sizeof ( size_t ),
21 };
22
23 /** @file */
24
25 /**
26  * A printf context
27  *
28  * Contexts are used in order to be able to share code between
29  * vprintf() and vsnprintf(), without requiring the allocation of a
30  * buffer for vprintf().
31  */
32 struct printf_context {
33         /**
34          * Character handler
35          *
36          * @v ctx       Context
37          * @v c         Character
38          *
39          * This method is called for each character written to the
40          * formatted string.  It must increment @len.
41          */
42         void ( * handler ) ( struct printf_context *ctx, unsigned int c );
43         /** Length of formatted string */
44         size_t len;
45         /** Buffer for formatted string (used by printf_sputc()) */
46         char *buf;
47         /** Buffer length (used by printf_sputc()) */
48         size_t max_len;
49 };
50
51 #define LCASE 0x20
52 #define ALT_FORM 0x02
53
54 static char * format_hex ( char *buf, unsigned long long num, int width,
55                            int flags ) {
56         char *ptr = buf;
57         int case_mod;
58
59         /* Generate the number */
60         case_mod = flags & LCASE;
61         do {
62                 *ptr++ = "0123456789ABCDEF"[ num & 0xf ] | case_mod;
63                 num >>= 4;
64         } while ( num );
65
66         /* Zero-pad to width */
67         while ( ( ptr - buf ) < width )
68                 *ptr++ = '0';
69
70         /* Add "0x" or "0X" if alternate form specified */
71         if ( flags & ALT_FORM ) {
72                 *ptr++ = 'X' | case_mod;
73                 *ptr++ = '0';
74         }
75
76         return ptr;
77 }
78
79 static char * format_decimal ( char *buf, signed long num, int width ) {
80         char *ptr = buf;
81         int negative = 0;
82
83         /* Generate the number */
84         if ( num < 0 ) {
85                 negative = 1;
86                 num = -num;
87         }
88         do {
89                 *ptr++ = '0' + ( num % 10 );
90                 num /= 10;
91         } while ( num );
92
93         /* Add "-" if necessary */
94         if ( negative )
95                 *ptr++ = '-';
96
97         /* Space-pad to width */
98         while ( ( ptr - buf ) < width )
99                 *ptr++ = ' ';
100
101         return ptr;
102 }
103
104
105 /**
106  * Write a formatted string to a printf context
107  *
108  * @v ctx               Context
109  * @v fmt               Format string
110  * @v args              Arguments corresponding to the format string
111  * @ret len             Length of formatted string
112  */
113 int vcprintf ( struct printf_context *ctx, const char *fmt, va_list args ) {
114         int flags;
115         int width;
116         uint8_t *length;
117         int character;
118         unsigned long long hex;
119         signed long decimal;
120         char num_buf[32];
121         char *ptr;
122
123         /* Initialise context */
124         ctx->len = 0;
125
126         for ( ; *fmt ; fmt++ ) {
127                 /* Pass through ordinary characters */
128                 if ( *fmt != '%' ) {
129                         ctx->handler ( ctx, *fmt );
130                         continue;
131                 }
132                 fmt++;
133                 /* Process flag characters */
134                 flags = 0;
135                 for ( ; ; fmt++ ) {
136                         if ( *fmt == '#' ) {
137                                 flags |= ALT_FORM;
138                         } else if ( *fmt == '0' ) {
139                                 /* We always 0-pad hex and space-pad decimal */
140                         } else {
141                                 /* End of flag characters */
142                                 break;
143                         }
144                 }
145                 /* Process field width */
146                 width = 0;
147                 for ( ; ; fmt++ ) {
148                         if ( ( ( unsigned ) ( *fmt - '0' ) ) < 10 ) {
149                                 width = ( width * 10 ) + ( *fmt - '0' );
150                         } else {
151                                 break;
152                         }
153                 }
154                 /* We don't do floating point */
155                 /* Process length modifier */
156                 length = &type_sizes[INT_LEN];
157                 for ( ; ; fmt++ ) {
158                         if ( *fmt == 'h' ) {
159                                 length--;
160                         } else if ( *fmt == 'l' ) {
161                                 length++;
162                         } else if ( *fmt == 'z' ) {
163                                 length = &type_sizes[SIZE_T_LEN];
164                         } else {
165                                 break;
166                         }
167                 }
168                 /* Process conversion specifier */
169                 if ( *fmt == 'c' ) {
170                         character = va_arg ( args, unsigned int );
171                         ctx->handler ( ctx, character );
172                 } else if ( *fmt == 's' ) {
173                         ptr = va_arg ( args, char * );
174                         for ( ; *ptr ; ptr++ ) {
175                                 ctx->handler ( ctx, *ptr );
176                         }
177                 } else if ( *fmt == 'p' ) {
178                         hex = ( intptr_t ) va_arg ( args, void * );
179                         ptr = format_hex ( num_buf, hex, width,
180                                            ( ALT_FORM | LCASE ) );
181                         do {
182                                 ctx->handler ( ctx, *(--ptr) );
183                         } while ( ptr != num_buf );
184                 } else if ( ( *fmt & ~0x20 ) == 'X' ) {
185                         flags |= ( *fmt & 0x20 ); /* LCASE */
186                         if ( *length >= sizeof ( unsigned long long ) ) {
187                                 hex = va_arg ( args, unsigned long long );
188                         } else if ( *length >= sizeof ( unsigned long ) ) {
189                                 hex = va_arg ( args, unsigned long );
190                         } else {
191                                 hex = va_arg ( args, unsigned int );
192                         }
193                         ptr = format_hex ( num_buf, hex, width, flags );
194                         do {
195                                 ctx->handler ( ctx, *(--ptr) );
196                         } while ( ptr != num_buf );
197                 } else if ( *fmt == 'd' ) {
198                         if ( *length >= sizeof ( signed long ) ) {
199                                 decimal = va_arg ( args, signed long );
200                         } else {
201                                 decimal = va_arg ( args, signed int );
202                         }
203                         ptr = format_decimal ( num_buf, decimal, width );
204                         do {
205                                 ctx->handler ( ctx, *(--ptr) );
206                         } while ( ptr != num_buf );
207                 } else {
208                         ctx->handler ( ctx, *fmt );
209                 }
210         }
211
212         return ctx->len;
213 }
214
215 /**
216  * Write character to buffer
217  *
218  * @v ctx               Context
219  * @v c                 Character
220  */
221 static void printf_sputc ( struct printf_context *ctx, unsigned int c ) {
222         if ( ++ctx->len < ctx->max_len )
223                 ctx->buf[ctx->len-1] = c;
224 }
225
226 /**
227  * Write a formatted string to a buffer
228  *
229  * @v buf               Buffer into which to write the string
230  * @v size              Size of buffer
231  * @v fmt               Format string
232  * @v args              Arguments corresponding to the format string
233  * @ret len             Length of formatted string
234  *
235  * If the buffer is too small to contain the string, the returned
236  * length is the length that would have been written had enough space
237  * been available.
238  */
239 int vsnprintf ( char *buf, size_t size, const char *fmt, va_list args ) {
240         struct printf_context ctx;
241         int len;
242
243         /* Ensure last byte is NUL if a size is specified.  This
244          * catches the case of the buffer being too small, in which
245          * case a trailing NUL would not otherwise be added.
246          */
247         if ( size != PRINTF_NO_LENGTH )
248                 buf[size-1] = '\0';
249
250         /* Hand off to vcprintf */
251         ctx.handler = printf_sputc;
252         ctx.buf = buf;
253         ctx.max_len = size;
254         len = vcprintf ( &ctx, fmt, args );
255
256         /* Add trailing NUL */
257         printf_sputc ( &ctx, '\0' );
258
259         return len;
260 }
261
262 /**
263  * Write a formatted string to a buffer
264  *
265  * @v buf               Buffer into which to write the string
266  * @v size              Size of buffer
267  * @v fmt               Format string
268  * @v ...               Arguments corresponding to the format string
269  * @ret len             Length of formatted string
270  */
271 int snprintf ( char *buf, size_t size, const char *fmt, ... ) {
272         va_list args;
273         int i;
274
275         va_start ( args, fmt );
276         i = vsnprintf ( buf, size, fmt, args );
277         va_end ( args );
278         return i;
279 }
280
281 /**
282  * Write character to console
283  *
284  * @v ctx               Context
285  * @v c                 Character
286  */
287 static void printf_putchar ( struct printf_context *ctx, unsigned int c ) {
288         ++ctx->len;
289         putchar ( c );
290 }
291
292 /**
293  * Write a formatted string to the console
294  *
295  * @v fmt               Format string
296  * @v args              Arguments corresponding to the format string
297  * @ret len             Length of formatted string
298  */
299 int vprintf ( const char *fmt, va_list args ) {
300         struct printf_context ctx;
301
302         /* Hand off to vcprintf */
303         ctx.handler = printf_putchar;   
304         return vcprintf ( &ctx, fmt, args );    
305 }
306
307 /**
308  * Write a formatted string to the console.
309  *
310  * @v fmt               Format string
311  * @v ...               Arguments corresponding to the format string
312  * @ret len             Length of formatted string
313  */
314 int printf ( const char *fmt, ... ) {
315         va_list args;
316         int i;
317
318         va_start ( args, fmt );
319         i = vprintf ( fmt, args );
320         va_end ( args );
321         return i;
322 }