First version
[3rdparty/ote_partner/tlk.git] / lib / monitor / common / 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 size_t strlen(char const *s)
182 {
183         size_t i;
184
185         i = 0;
186         while(s[i])
187                 i += 1;
188         return i;
189 }
190
191 int _printf_engine(_printf_engine_output_func out, void *state, const char *fmt, va_list ap)
192 {
193         char c;
194         unsigned char uc;
195         const char *s;
196         unsigned long long n;
197         void *ptr;
198         int flags;
199         unsigned int format_num;
200         size_t chars_written = 0;
201         char num_buffer[32];
202
203 #define OUTPUT_CHAR(c) do { chars_written++; if (out(c, state) <= 1) goto done; } while(0)
204 #define OUTPUT_CHAR_NOLENCHECK(c) do { out(c, state); } while(0)
205
206         for(;;) {
207                 /* handle regular chars that aren't format related */
208                 while((c = *fmt++) != 0) {
209                         if(c == '%')
210                                 break; /* we saw a '%', break and start parsing format */
211                         OUTPUT_CHAR(c);
212                 }
213
214                 /* make sure we haven't just hit the end of the string */
215                 if(c == 0)
216                         break;
217
218                 /* reset the format state */
219                 flags = 0;
220                 format_num = 0;
221
222 next_format:
223                 /* grab the next format character */
224                 c = *fmt++;
225                 if(c == 0)
226                         break;
227
228                 switch(c) {
229                         case '0'...'9':
230                                 if (c == '0' && format_num == 0)
231                                         flags |= LEADZEROFLAG;
232                                 format_num *= 10;
233                                 format_num += c - '0';
234                                 goto next_format;
235                         case '.':
236                                 /* XXX for now eat numeric formatting */
237                                 goto next_format;
238                         case '%':
239                                 OUTPUT_CHAR('%');
240                                 break;
241                         case 'c':
242                                 uc = va_arg(ap, unsigned int);
243                                 OUTPUT_CHAR(uc);
244                                 break;
245                         case 's':
246                                 s = va_arg(ap, const char *);
247                                 if(s == 0)
248                                         s = "<null>";
249                                 goto _output_string;
250                         case '-':
251                                 flags |= LEFTFORMATFLAG;
252                                 goto next_format;
253                         case '+':
254                                 flags |= SHOWSIGNFLAG;
255                                 goto next_format;
256                         case '#':
257                                 flags |= ALTFLAG;
258                                 goto next_format;
259                         case 'l':
260                                 if(flags & LONGFLAG)
261                                         flags |= LONGLONGFLAG;
262                                 flags |= LONGFLAG;
263                                 goto next_format;
264                         case 'h':
265                                 if(flags & HALFFLAG)
266                                         flags |= HALFHALFFLAG;
267                                 flags |= HALFFLAG;
268                                 goto next_format;
269                     case 'z':
270                                 flags |= SIZETFLAG;
271                                 goto next_format;
272                         case 'D':
273                                 flags |= LONGFLAG;
274                                 /* fallthrough */
275                         case 'i':
276                         case 'd':
277                                 n = (flags & LONGLONGFLAG) ? va_arg(ap, long long) :
278                                         (flags & LONGFLAG) ? va_arg(ap, long) :
279                                         (flags & HALFHALFFLAG) ? (signed char)va_arg(ap, int) :
280                                         (flags & HALFFLAG) ? (short)va_arg(ap, int) :
281                                         (flags & SIZETFLAG) ? va_arg(ap, ssize_t) :
282                                         va_arg(ap, int);
283                                 flags |= SIGNEDFLAG;
284                                 s = longlong_to_string(num_buffer, n, sizeof(num_buffer), flags);
285                                 goto _output_string;
286                         case 'U':
287                                 flags |= LONGFLAG;
288                                 /* fallthrough */
289                         case 'u':
290                                 n = (flags & LONGLONGFLAG) ? va_arg(ap, unsigned long long) :
291                                         (flags & LONGFLAG) ? va_arg(ap, unsigned long) :
292                                         (flags & HALFHALFFLAG) ? (unsigned char)va_arg(ap, unsigned int) :
293                                         (flags & HALFFLAG) ? (unsigned short)va_arg(ap, unsigned int) :
294                                         (flags & SIZETFLAG) ? va_arg(ap, size_t) :
295                                         va_arg(ap, unsigned int);
296                                 s = longlong_to_string(num_buffer, n, sizeof(num_buffer), flags);
297                                 goto _output_string;
298                         case 'p':
299                                 flags |= LONGFLAG | ALTFLAG;
300                                 goto hex;
301                         case 'X':
302                                 flags |= CAPSFLAG;
303                                 /* fallthrough */
304 hex:
305                         case 'x':
306                                 n = (flags & LONGLONGFLAG) ? va_arg(ap, unsigned long long) :
307                                     (flags & LONGFLAG) ? va_arg(ap, unsigned long) :
308                                         (flags & HALFHALFFLAG) ? (unsigned char)va_arg(ap, unsigned int) :
309                                         (flags & HALFFLAG) ? (unsigned short)va_arg(ap, unsigned int) :
310                                         (flags & SIZETFLAG) ? va_arg(ap, size_t) :
311                                         va_arg(ap, unsigned int);
312                                 s = longlong_to_hexstring(num_buffer, n, sizeof(num_buffer), flags);
313                                 if(flags & ALTFLAG) {
314                                         OUTPUT_CHAR('0');
315                                         OUTPUT_CHAR((flags & CAPSFLAG) ? 'X': 'x');
316                                 }
317                                 goto _output_string;
318                         case 'n':
319                                 ptr = va_arg(ap, void *);
320                                 if(flags & LONGLONGFLAG)
321                                         *(long long *)ptr = chars_written;
322                                 else if(flags & LONGFLAG)
323                                         *(long *)ptr = chars_written;
324                                 else if(flags & HALFHALFFLAG)
325                                         *(signed char *)ptr = chars_written;
326                                 else if(flags & HALFFLAG)
327                                         *(short *)ptr = chars_written;
328                                 else if(flags & SIZETFLAG)
329                                         *(size_t *)ptr = chars_written;
330                                 else
331                                         *(int *)ptr = chars_written;
332                                 break;
333                         default:
334                                 OUTPUT_CHAR('%');
335                                 OUTPUT_CHAR(c);
336                                 break;
337                 }
338
339                 /* move on to the next field */
340                 continue;
341
342                 /* shared output code */
343 _output_string:
344                 if (flags & LEFTFORMATFLAG) {
345                         /* left justify the text */
346                         uint count = 0;
347                         while(*s != 0) {
348                                 OUTPUT_CHAR(*s++);
349                                 count++;
350                         }
351
352                         /* pad to the right (if necessary) */
353                         for (; format_num > count; format_num--)
354                                 OUTPUT_CHAR(' ');
355                 } else {
356                         /* right justify the text (digits) */
357                         size_t string_len = strlen(s);
358                         char outchar = (flags & LEADZEROFLAG) ? '0' : ' ';
359                         for (; format_num > string_len; format_num--)
360                                 OUTPUT_CHAR(outchar);
361
362                         /* output the string */
363                         while(*s != 0)
364                                 OUTPUT_CHAR(*s++);
365                 }
366                 continue;
367         }
368
369 done:
370         /* null terminate */
371         OUTPUT_CHAR_NOLENCHECK('\0');
372
373 #undef OUTPUT_CHAR
374 #undef OUTPUT_CHAR_NOLENCHECK
375
376         return chars_written;
377 }