First version
[3rdparty/ote_partner/tlk.git] / lib / libc / printf.c
1 /*
2  * Copyright (c) 2008-2012 Travis Geiselbrecht
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining
5  * a copy of this software and associated documentation files
6  * (the "Software"), to deal in the Software without restriction,
7  * including without limitation the rights to use, copy, modify, merge,
8  * publish, distribute, sublicense, and/or sell copies of the Software,
9  * and to permit persons to whom the Software is furnished to do so,
10  * subject to the following conditions:
11  *
12  * The above copyright notice and this permission notice shall be
13  * included in all copies or substantial portions of the Software.
14  *
15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
18  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
19  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
20  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
21  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22  */
23 #include <debug.h>
24 #include <stdarg.h>
25 #include <sys/types.h>
26 #include <printf.h>
27 #include <string.h>
28 #include <platform/debug.h>
29
30 void putc(char c)
31 {
32         return _dputc(c);
33 }
34
35 int puts(const char *str)
36 {
37         return _dputs(str);
38 }
39
40 int getc(char *c)
41 {
42         return platform_dgetc(c, true);
43 }
44
45 int _printf(const char *fmt, ...)
46 {
47         int err;
48
49         va_list ap;
50         va_start(ap, fmt);
51         err = _dvprintf(fmt, ap);
52         va_end(ap);
53
54         return err;
55 }
56
57 int sprintf(char *str, const char *fmt, ...)
58 {
59         int err;
60
61         va_list ap;
62         va_start(ap, fmt);
63         err = vsprintf(str, fmt, ap);
64         va_end(ap);
65
66         return err;
67 }
68
69 int snprintf(char *str, size_t len, const char *fmt, ...)
70 {
71         int err;
72
73         va_list ap;
74         va_start(ap, fmt);
75         err = vsnprintf(str, len, fmt, ap);
76         va_end(ap);
77
78         return err;
79 }
80
81
82 #define LONGFLAG     0x00000001
83 #define LONGLONGFLAG 0x00000002
84 #define HALFFLAG     0x00000004
85 #define HALFHALFFLAG 0x00000008
86 #define SIZETFLAG    0x00000010
87 #define ALTFLAG      0x00000020
88 #define CAPSFLAG     0x00000040
89 #define SHOWSIGNFLAG 0x00000080
90 #define SIGNEDFLAG   0x00000100
91 #define LEFTFORMATFLAG 0x00000200
92 #define LEADZEROFLAG 0x00000400
93
94 static char *longlong_to_string(char *buf, unsigned long long n, int len, uint flag)
95 {
96         int pos = len;
97         int negative = 0;
98
99         if((flag & SIGNEDFLAG) && (long long)n < 0) {
100                 negative = 1;
101                 n = -n;
102         }
103
104         buf[--pos] = 0;
105
106         /* only do the math if the number is >= 10 */
107         while(n >= 10) {
108                 int digit = n % 10;
109
110                 n /= 10;
111
112                 buf[--pos] = digit + '0';
113         }
114         buf[--pos] = n + '0';
115
116         if(negative)
117                 buf[--pos] = '-';
118         else if((flag & SHOWSIGNFLAG))
119                 buf[--pos] = '+';
120
121         return &buf[pos];
122 }
123
124 static char *longlong_to_hexstring(char *buf, unsigned long long u, int len, uint flag)
125 {
126         int pos = len;
127         static const char hextable[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' };
128         static const char hextable_caps[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
129         const char *table;
130
131         if((flag & CAPSFLAG))
132                 table = hextable_caps;
133         else
134                 table = hextable;
135
136         buf[--pos] = 0;
137         do {
138                 unsigned int digit = u % 16;
139                 u /= 16;
140
141                 buf[--pos] = table[digit];
142         } while(u != 0);
143
144         return &buf[pos];
145 }
146
147 int vsprintf(char *str, const char *fmt, va_list ap)
148 {
149         return vsnprintf(str, INT_MAX, fmt, ap);
150 }
151
152 struct _output_args {
153         char *outstr;
154         size_t len;
155         size_t pos;
156 };
157
158 static int _vsnprintf_output(char c, void *state)
159 {
160         struct _output_args *args = state;
161
162         if (args->pos >= args->len)
163                 return 0;
164
165         args->outstr[args->pos++] = c;
166
167         return args->len - args->pos;
168 }
169
170 int vsnprintf(char *str, size_t len, const char *fmt, va_list ap)
171 {
172         struct _output_args args;
173
174         args.outstr = str;
175         args.len = len;
176         args.pos = 0;
177
178         return _printf_engine(&_vsnprintf_output, (void *)&args, fmt, ap);
179 }
180
181 int _printf_engine(_printf_engine_output_func out, void *state, const char *fmt, va_list ap)
182 {
183         char c;
184         unsigned char uc;
185         const char *s;
186         unsigned long long n;
187         void *ptr;
188         int flags;
189         unsigned int format_num;
190         size_t chars_written = 0;
191         char num_buffer[32];
192
193 #define OUTPUT_CHAR(c) do { chars_written++; if (out(c, state) <= 1) goto done; } while(0)
194 #define OUTPUT_CHAR_NOLENCHECK(c) do { out(c, state); } while(0)
195
196         for(;;) {
197                 /* handle regular chars that aren't format related */
198                 while((c = *fmt++) != 0) {
199                         if(c == '%')
200                                 break; /* we saw a '%', break and start parsing format */
201                         OUTPUT_CHAR(c);
202                 }
203
204                 /* make sure we haven't just hit the end of the string */
205                 if(c == 0)
206                         break;
207
208                 /* reset the format state */
209                 flags = 0;
210                 format_num = 0;
211
212 next_format:
213                 /* grab the next format character */
214                 c = *fmt++;
215                 if(c == 0)
216                         break;
217
218                 switch(c) {
219                         case '0'...'9':
220                                 if (c == '0' && format_num == 0)
221                                         flags |= LEADZEROFLAG;
222                                 format_num *= 10;
223                                 format_num += c - '0';
224                                 goto next_format;
225                         case '.':
226                                 /* XXX for now eat numeric formatting */
227                                 goto next_format;
228                         case '%':
229                                 OUTPUT_CHAR('%');
230                                 break;
231                         case 'c':
232                                 uc = va_arg(ap, unsigned int);
233                                 OUTPUT_CHAR(uc);
234                                 break;
235                         case 's':
236                                 s = va_arg(ap, const char *);
237                                 if(s == 0)
238                                         s = "<null>";
239                                 goto _output_string;
240                         case '-':
241                                 flags |= LEFTFORMATFLAG;
242                                 goto next_format;
243                         case '+':
244                                 flags |= SHOWSIGNFLAG;
245                                 goto next_format;
246                         case '#':
247                                 flags |= ALTFLAG;
248                                 goto next_format;
249                         case 'l':
250                                 if(flags & LONGFLAG)
251                                         flags |= LONGLONGFLAG;
252                                 flags |= LONGFLAG;
253                                 goto next_format;
254                         case 'h':
255                                 if(flags & HALFFLAG)
256                                         flags |= HALFHALFFLAG;
257                                 flags |= HALFFLAG;
258                                 goto next_format;
259                     case 'z':
260                                 flags |= SIZETFLAG;
261                                 goto next_format;
262                         case 'D':
263                                 flags |= LONGFLAG;
264                                 /* fallthrough */
265                         case 'i':
266                         case 'd':
267                                 n = (flags & LONGLONGFLAG) ? va_arg(ap, long long) :
268                                         (flags & LONGFLAG) ? va_arg(ap, long) :
269                                         (flags & HALFHALFFLAG) ? (signed char)va_arg(ap, int) :
270                                         (flags & HALFFLAG) ? (short)va_arg(ap, int) :
271                                         (flags & SIZETFLAG) ? va_arg(ap, ssize_t) :
272                                         va_arg(ap, int);
273                                 flags |= SIGNEDFLAG;
274                                 s = longlong_to_string(num_buffer, n, sizeof(num_buffer), flags);
275                                 goto _output_string;
276                         case 'U':
277                                 flags |= LONGFLAG;
278                                 /* fallthrough */
279                         case 'u':
280                                 n = (flags & LONGLONGFLAG) ? va_arg(ap, unsigned long long) :
281                                         (flags & LONGFLAG) ? va_arg(ap, unsigned long) :
282                                         (flags & HALFHALFFLAG) ? (unsigned char)va_arg(ap, unsigned int) :
283                                         (flags & HALFFLAG) ? (unsigned short)va_arg(ap, unsigned int) :
284                                         (flags & SIZETFLAG) ? va_arg(ap, size_t) :
285                                         va_arg(ap, unsigned int);
286                                 s = longlong_to_string(num_buffer, n, sizeof(num_buffer), flags);
287                                 goto _output_string;
288                         case 'p':
289                                 flags |= LONGFLAG | ALTFLAG;
290                                 goto hex;
291                         case 'X':
292                                 flags |= CAPSFLAG;
293                                 /* fallthrough */
294 hex:
295                         case 'x':
296                                 n = (flags & LONGLONGFLAG) ? va_arg(ap, unsigned long long) :
297                                     (flags & LONGFLAG) ? va_arg(ap, unsigned long) :
298                                         (flags & HALFHALFFLAG) ? (unsigned char)va_arg(ap, unsigned int) :
299                                         (flags & HALFFLAG) ? (unsigned short)va_arg(ap, unsigned int) :
300                                         (flags & SIZETFLAG) ? va_arg(ap, size_t) :
301                                         va_arg(ap, unsigned int);
302                                 s = longlong_to_hexstring(num_buffer, n, sizeof(num_buffer), flags);
303                                 if(flags & ALTFLAG) {
304                                         OUTPUT_CHAR('0');
305                                         OUTPUT_CHAR((flags & CAPSFLAG) ? 'X': 'x');
306                                 }
307                                 goto _output_string;
308                         case 'n':
309                                 ptr = va_arg(ap, void *);
310                                 if(flags & LONGLONGFLAG)
311                                         *(long long *)ptr = chars_written;
312                                 else if(flags & LONGFLAG)
313                                         *(long *)ptr = chars_written;
314                                 else if(flags & HALFHALFFLAG)
315                                         *(signed char *)ptr = chars_written;
316                                 else if(flags & HALFFLAG)
317                                         *(short *)ptr = chars_written;
318                                 else if(flags & SIZETFLAG)
319                                         *(size_t *)ptr = chars_written;
320                                 else
321                                         *(int *)ptr = chars_written;
322                                 break;
323                         default:
324                                 OUTPUT_CHAR('%');
325                                 OUTPUT_CHAR(c);
326                                 break;
327                 }
328
329                 /* move on to the next field */
330                 continue;
331
332                 /* shared output code */
333 _output_string:
334                 if (flags & LEFTFORMATFLAG) {
335                         /* left justify the text */
336                         uint count = 0;
337                         while(*s != 0) {
338                                 OUTPUT_CHAR(*s++);
339                                 count++;
340                         }
341
342                         /* pad to the right (if necessary) */
343                         for (; format_num > count; format_num--)
344                                 OUTPUT_CHAR(' ');
345                 } else {
346                         /* right justify the text (digits) */
347                         size_t string_len = strlen(s);
348                         char outchar = (flags & LEADZEROFLAG) ? '0' : ' ';
349                         for (; format_num > string_len; format_num--)
350                                 OUTPUT_CHAR(outchar);
351
352                         /* output the string */
353                         while(*s != 0)
354                                 OUTPUT_CHAR(*s++);
355                 }
356                 continue;
357         }
358
359 done:
360         /* null terminate */
361         OUTPUT_CHAR_NOLENCHECK('\0');
362
363 #undef OUTPUT_CHAR
364 #undef OUTPUT_CHAR_NOLENCHECK
365
366         return chars_written;
367 }
368
369