lib/printf.c: making serial_printf return vsprintf ret value
[x-loader:x-loader.git] / lib / printf.c
1 /*
2  * (C) Copyright 2004 Texas Instruments
3  *
4  * Based on the following file:
5  *  linux/lib/vsprintf.c
6  *
7  *  Copyright (C) 1991, 1992  Linus Torvalds
8  */
9
10 /* vsprintf.c -- Lars Wirzenius & Linus Torvalds. */
11 /*
12  * Wirzenius wrote this portably, Torvalds fucked it up :-)
13  */
14
15 #include <stdarg.h>
16 #include <common.h>
17
18 #ifdef CFG_PRINTF
19  
20 /* we use this so that we can do without the ctype library */
21 #define is_digit(c)     ((c) >= '0' && (c) <= '9')
22
23 size_t strnlen(const char * s, size_t count)
24 {
25         const char *sc;
26
27         for (sc = s; count-- && *sc != '\0'; ++sc)
28                 /* nothing */;
29         return sc - s;
30 }
31
32 static int skip_atoi(const char **s)
33 {
34         int i=0;
35
36         while (is_digit(**s))
37                 i = i*10 + *((*s)++) - '0';
38         return i;
39 }
40
41 #define ZEROPAD 1               /* pad with zero */
42 #define SIGN    2               /* unsigned/signed long */
43 #define PLUS    4               /* show plus */
44 #define SPACE   8               /* space if plus */
45 #define LEFT    16              /* left justified */
46 #define SPECIAL 32              /* 0x */
47 #define LARGE   64              /* use 'ABCDEF' instead of 'abcdef' */
48
49 #define do_div(n,base) ({ \
50 int __res; \
51 __res = ((unsigned long) n) % (unsigned) base; \
52 n = ((unsigned long) n) / (unsigned) base; \
53 __res; })
54
55 static char * number(char * str, long num, int base, int size, int precision
56         ,int type)
57 {
58         char c,sign,tmp[66];
59         const char *digits="0123456789abcdefghijklmnopqrstuvwxyz";
60         int i;
61
62         if (type & LARGE)
63                 digits = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
64         if (type & LEFT)
65                 type &= ~ZEROPAD;
66         if (base < 2 || base > 36)
67                 return 0;
68         c = (type & ZEROPAD) ? '0' : ' ';
69         sign = 0;
70         if (type & SIGN) {
71                 if (num < 0) {
72                         sign = '-';
73                         num = -num;
74                         size--;
75                 } else if (type & PLUS) {
76                         sign = '+';
77                         size--;
78                 } else if (type & SPACE) {
79                         sign = ' ';
80                         size--;
81                 }
82         }
83         if (type & SPECIAL) {
84                 if (base == 16)
85                         size -= 2;
86                 else if (base == 8)
87                         size--;
88         }
89         i = 0;
90         if (num == 0)
91                 tmp[i++]='0';
92         else while (num != 0)
93                 tmp[i++] = digits[do_div(num,base)];
94         if (i > precision)
95                 precision = i;
96         size -= precision;
97         if (!(type&(ZEROPAD+LEFT)))
98                 while(size-->0)
99                         *str++ = ' ';
100         if (sign)
101                 *str++ = sign;
102         if (type & SPECIAL) {
103                 if (base==8)
104                         *str++ = '0';
105                 else if (base==16) {
106                         *str++ = '0';
107                         *str++ = digits[33];
108                 }
109         }
110         if (!(type & LEFT))
111                 while (size-- > 0)
112                         *str++ = c;
113         while (i < precision--)
114                 *str++ = '0';
115         while (i-- > 0)
116                 *str++ = tmp[i];
117         while (size-- > 0)
118                 *str++ = ' ';
119         return str;
120 }
121
122  
123 static int vsprintf(char *buf, const char *fmt, va_list args)
124 {
125         int len;
126         unsigned long num;
127         int i, base;
128         char * str;
129         const char *s;
130
131         int flags;              /* flags to number() */
132
133         int field_width;        /* width of output field */
134         int precision;          /* min. # of digits for integers; max
135                                    number of chars for from string */
136         int qualifier;          /* 'h', 'l', or 'L' for integer fields */
137
138         for (str=buf ; *fmt ; ++fmt) {
139                 if (*fmt != '%') {
140                         *str++ = *fmt;
141                         continue;
142                 }
143
144                 /* process flags */
145                 flags = 0;
146                 repeat:
147                         ++fmt;          /* this also skips first '%' */
148                         switch (*fmt) {
149                                 case '-': flags |= LEFT; goto repeat;
150                                 case '+': flags |= PLUS; goto repeat;
151                                 case ' ': flags |= SPACE; goto repeat;
152                                 case '#': flags |= SPECIAL; goto repeat;
153                                 case '0': flags |= ZEROPAD; goto repeat;
154                                 }
155
156                 /* get field width */
157                 field_width = -1;
158                 if (is_digit(*fmt))
159                         field_width = skip_atoi(&fmt);
160                 else if (*fmt == '*') {
161                         ++fmt;
162                         /* it's the next argument */
163                         field_width = va_arg(args, int);
164                         if (field_width < 0) {
165                                 field_width = -field_width;
166                                 flags |= LEFT;
167                         }
168                 }
169
170                 /* get the precision */
171                 precision = -1;
172                 if (*fmt == '.') {
173                         ++fmt;
174                         if (is_digit(*fmt))
175                                 precision = skip_atoi(&fmt);
176                         else if (*fmt == '*') {
177                                 ++fmt;
178                                 /* it's the next argument */
179                                 precision = va_arg(args, int);
180                         }
181                         if (precision < 0)
182                                 precision = 0;
183                 }
184
185                 /* get the conversion qualifier */
186                 qualifier = -1;
187                 if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L') {
188                         qualifier = *fmt;
189                         ++fmt;
190                 }
191
192                 /* default base */
193                 base = 10;
194
195                 switch (*fmt) {
196                 case 'c':
197                         if (!(flags & LEFT))
198                                 while (--field_width > 0)
199                                         *str++ = ' ';
200                         *str++ = (unsigned char) va_arg(args, int);
201                         while (--field_width > 0)
202                                 *str++ = ' ';
203                         continue;
204
205                 case 's':
206                         s = va_arg(args, char *);
207                         if (!s)
208                                 s = "<NULL>";
209
210                         len = strnlen(s, precision);
211
212                         if (!(flags & LEFT))
213                                 while (len < field_width--)
214                                         *str++ = ' ';
215                         for (i = 0; i < len; ++i)
216                                 *str++ = *s++;
217                         while (len < field_width--)
218                                 *str++ = ' ';
219                         continue;
220
221                 case 'p':
222                         if (field_width == -1) {
223                                 field_width = 2*sizeof(void *);
224                                 flags |= ZEROPAD;
225                         }
226                         str = number(str,
227                                 (unsigned long) va_arg(args, void *), 16,
228                                 field_width, precision, flags);
229                         continue;
230
231
232                 case 'n':
233                         if (qualifier == 'l') {
234                                 long * ip = va_arg(args, long *);
235                                 *ip = (str - buf);
236                         } else {
237                                 int * ip = va_arg(args, int *);
238                                 *ip = (str - buf);
239                         }
240                         continue;
241
242                 case '%':
243                         *str++ = '%';
244                         continue;
245
246                 /* integer number formats - set up the flags and "break" */
247                 case 'o':
248                         base = 8;
249                         break;
250
251                 case 'X':
252                         flags |= LARGE;
253                 case 'x':
254                         base = 16;
255                         break;
256
257                 case 'd':
258                 case 'i':
259                         flags |= SIGN;
260                 case 'u':
261                         break;
262
263                 default:
264                         *str++ = '%';
265                         if (*fmt)
266                                 *str++ = *fmt;
267                         else
268                                 --fmt;
269                         continue;
270                 }
271                 if (qualifier == 'l')
272                         num = va_arg(args, unsigned long);
273                 else if (qualifier == 'h') {
274                         num = (unsigned short) va_arg(args, int);
275                         if (flags & SIGN)
276                                 num = (short) num;
277                 } else if (flags & SIGN)
278                         num = va_arg(args, int);
279                 else
280                         num = va_arg(args, unsigned int);
281                 str = number(str, num, base, field_width, precision, flags);
282         }
283         *str = '\0';
284         return str-buf;
285 }
286
287 int serial_printf (const char *fmt, ...)
288 {
289         va_list args;
290         uint i;
291         char printbuffer[CFG_PBSIZE];
292
293         va_start (args, fmt);
294
295         /* For this to work, printbuffer must be larger than
296          * anything we ever want to print.
297          */
298         i = vsprintf (printbuffer, fmt, args);
299         va_end (args);
300
301         /* Print the string */
302         serial_puts (printbuffer);
303         return i;
304 }
305 #endif