Merge from Etherboot 5.4
[people/lynusvaz/gpxe.git] / src / core / vsprintf.c
1 #include <stdarg.h>
2 #include "if_ether.h" /* for ETH_ALEN */
3 #include "limits.h" /* for CHAR_BIT */
4 #include "console.h"
5 #include "errno.h"
6 #include "vsprintf.h"
7
8 #define LONG_SHIFT  ((int)((sizeof(unsigned long)*CHAR_BIT) - 4))
9 #define INT_SHIFT   ((int)((sizeof(unsigned int)*CHAR_BIT) - 4))
10 #define SHRT_SHIFT  ((int)((sizeof(unsigned short)*CHAR_BIT) - 4))
11 #define CHAR_SHIFT  ((int)((sizeof(unsigned char)*CHAR_BIT) - 4))
12
13 /** @file */
14
15 /**
16  * Write a formatted string to a buffer.
17  *
18  * @v buf               Buffer into which to write the string, or NULL
19  * @v fmt               Format string
20  * @v args              Arguments corresponding to the format string
21  * @ret len             Length of string written to buffer (if buf != NULL)
22  * @ret 0               (if buf == NULL)
23  * @err None            -
24  *
25  * If #buf==NULL, then the string will be written to the console
26  * directly using putchar().
27  *
28  */
29 static int vsprintf(char *buf, const char *fmt, va_list args)
30 {
31         const char *p;
32         char *s;
33         s = buf;
34         for ( ; *fmt != '\0'; ++fmt) {
35                 if (*fmt != '%') {
36                         buf ? *s++ = *fmt : putchar(*fmt);
37                         continue;
38                 }
39                 /* skip width specs */
40                 fmt++;
41                 while (*fmt >= '0' && *fmt <= '9')
42                         fmt++;
43                 if (*fmt == '.')
44                         fmt++;
45                 while (*fmt >= '0' && *fmt <= '9')
46                         fmt++;
47                 if (*fmt == 's') {
48                         for(p = va_arg(args, char *); *p != '\0'; p++)
49                                 buf ? *s++ = *p : putchar(*p);
50                 } else if (*fmt == 'm') {
51                         for(p = strerror(errno); *p != '\0'; p++)
52                                 buf ? *s++ = *p : putchar(*p);
53                 } else {        /* Length of item is bounded */
54                         char tmp[40], *q = tmp;
55                         int alt = 0;
56                         int shift = INT_SHIFT;
57                         if (*fmt == '#') {
58                                 alt = 1;
59                                 fmt++;
60                         }
61                         if (*fmt == 'l') {
62                                 shift = LONG_SHIFT;
63                                 fmt++;
64                         }
65                         else if (*fmt == 'h') {
66                                 shift = SHRT_SHIFT;
67                                 fmt++;
68                                 if (*fmt == 'h') {
69                                         shift = CHAR_SHIFT;
70                                         fmt++;
71                                 }
72                         }
73
74                         /*
75                          * Before each format q points to tmp buffer
76                          * After each format q points past end of item
77                          */
78                         if ((*fmt | 0x20) == 'x') {
79                                 /* With x86 gcc, sizeof(long) == sizeof(int) */
80                                 unsigned long h;
81                                 int ncase;
82                                 if (shift > INT_SHIFT) {
83                                         h = va_arg(args, unsigned long);
84                                 } else {
85                                         h = va_arg(args, unsigned int);
86                                 }
87                                 ncase = (*fmt & 0x20);
88                                 if (alt) {
89                                         *q++ = '0';
90                                         *q++ = 'X' | ncase;
91                                 }
92                                 for ( ; shift >= 0; shift -= 4)
93                                         *q++ = "0123456789ABCDEF"[(h >> shift) & 0xF] | ncase;
94                         }
95                         else if (*fmt == 'd') {
96                                 char *r, *t;
97                                 long i;
98                                 if (shift > INT_SHIFT) {
99                                         i = va_arg(args, long);
100                                 } else {
101                                         i = va_arg(args, int);
102                                 }
103                                 if (i < 0) {
104                                         *q++ = '-';
105                                         i = -i;
106                                 }
107                                 t = q;          /* save beginning of digits */
108                                 do {
109                                         *q++ = '0' + (i % 10);
110                                         i /= 10;
111                                 } while (i);
112                                 /* reverse digits, stop in middle */
113                                 r = q;          /* don't alter q */
114                                 while (--r > t) {
115                                         i = *r;
116                                         *r = *t;
117                                         *t++ = i;
118                                 }
119                         }
120                         else if (*fmt == '@') {
121                                 unsigned char *r;
122                                 union {
123                                         uint32_t        l;
124                                         unsigned char   c[4];
125                                 } u;
126                                 u.l = va_arg(args, uint32_t);
127                                 for (r = &u.c[0]; r < &u.c[4]; ++r)
128                                         q += sprintf(q, "%d.", *r);
129                                 --q;
130                         }
131                         else if (*fmt == '!') {
132                                 const char *r;
133                                 p = va_arg(args, char *);
134                                 for (r = p + ETH_ALEN; p < r; ++p)
135                                         q += sprintf(q, "%hhX:", *p);
136                                 --q;
137                         }
138                         else if (*fmt == 'c')
139                                 *q++ = va_arg(args, int);
140                         else
141                                 *q++ = *fmt;
142                         /* now output the saved string */
143                         for (p = tmp; p < q; ++p)
144                                 buf ? *s++ = *p : putchar(*p);
145                 }
146         }
147         if (buf)
148                 *s = '\0';
149         return (s - buf);
150 }
151
152 /**
153  * Write a formatted string to a buffer.
154  *
155  * @v buf               Buffer into which to write the string, or NULL
156  * @v fmt               Format string
157  * @v ...               Arguments corresponding to the format string
158  * @ret len             Length of string written to buffer (if buf != NULL)
159  * @ret 0               (if buf == NULL)
160  * @err None            -
161  *
162  * If #buf==NULL, then the string will be written to the console
163  * directly using putchar().
164  *
165  */
166 int sprintf(char *buf, const char *fmt, ...)
167 {
168         va_list args;
169         int i;
170         va_start(args, fmt);
171         i=vsprintf(buf, fmt, args);
172         va_end(args);
173         return i;
174 }
175
176 /**
177  * Write a formatted string to the console.
178  *
179  * @v fmt               Format string
180  * @v ...               Arguments corresponding to the format string
181  * @ret None            -
182  * @err None            -
183  *
184  */
185 int printf(const char *fmt, ...)
186 {
187         va_list args;
188         int i;
189         va_start(args, fmt);
190         i=vsprintf(0, fmt, args);
191         va_end(args);
192         return i;
193 }