Base code for dtplug : Core, drivers, lib and some external drivers.
[dtplug] / lib / vsprintf.c
1 /****************************************************************************
2  *  lib/vsprintf.c
3  *
4  * Code based on lib/vsprintf.c from linux kernel.
5  * 
6  * Copyright 2012 Nathael Pajani <nathael.pajani@ed3l.fr>
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
20  *
21  *************************************************************************** */
23 #include <stdarg.h>
24 #include <stdint.h>
25 #include "lib/string.h"
27 #define ZEROPAD   (1 <<  0)  /* pad with zero */
28 #define SIGNED    (1 <<  1)  /* unsigned/signed long */
29 #define SIGN      (1 <<  2)  /* show plus */
30 #define SPACE     (1 <<  3)  /* space if plus */
31 #define LEFT      (1 <<  4)  /* left justified */
32 #define LOWERCASE (1 <<  5)  /* use lowercase in hex (must be 32 == 0x20) */
33 #define SPECIAL   (1 <<  6)  /* prefix hex with "0x", octal with "0" */
34 #define HEXA      (1 <<  7)  /* output hexa-decimal */
36 /* In our case, anything that does not fit in 20 chars does not fit in an unsigned int. */
37 #define TMP_NUM_BUF_SIZE 20
38 static int convert(char* buf, char* end, uint32_t flags, uint32_t width, uint32_t num)
39 {
40         static const char digits[16] = "0123456789ABCDEF";
41         char tmp[TMP_NUM_BUF_SIZE];
42         int i = 0, length = 0;
43         char sign = 0;
45         if (width > TMP_NUM_BUF_SIZE) {
46                 width = TMP_NUM_BUF_SIZE;
47         }
49         /* Store sign, and convert to unsigned */
50         if (flags & SIGNED) {
51                 if (width) {
52                         width--;
53                 }
54                 if ((signed long)num < 0) {
55                         sign = '-';
56                         num = -(signed long)num;
57                 }
58         } /* Do we need to remove 2 to width in case of "SPECIAL" flag ? */
60         /* Generate full string in tmp[], in reverse order */
61         if (num == 0) {
62                 tmp[i++] = '0';
63         } else if (flags & HEXA) {
64                 /* low_case = 0 or 0x20. ORing digits or letters with 'low_case'
65                  * produces same digits or (maybe lowercased) letters */
66                 uint32_t low_case = (flags & LOWERCASE) ? 0x20 : 0;
67                 do {
68                         tmp[i++] = (digits[num & 0x0F] | low_case);
69                         num = (num >> 4);
70                 } while (num);
71         } else {
72                 while (num) {
73                         tmp[i++] = (num % 10) + '0';
74                         num = num / 10;
75                 }
76         }
78         /* Add sign, pad if reqiered */
79         if (flags & ZEROPAD) {
80                 while (i < width) {
81                         tmp[i++] = '0';
82                 }
83         }
84         if (sign) {
85                 tmp[i++] = sign;
86         } else if (flags & SIGN) {
87                 tmp[i++] = '+';
88         } else if (flags & SPACE) {
89                 tmp[i++] = ' ';
90         } else if (flags & SPECIAL) {
91                 tmp[i++] = 'x';
92                 tmp[i++] = '0';
93         }
94         while (i < width) {
95                 tmp[i++] = ' ';
96         }
98         /* And reverse string */
99         length = i;
100         while (i && (buf < end)) {
101                 i--;
102                 *(buf++) = tmp[i];
103         }
104         if (i)
105                 return -i;
107         return length;
110 int vsnprintf(char *buf, size_t size, const char *fmt, va_list args)
112     char* start = buf;
113     char* end = buf + size - 1; /* leave one char for terminating null byte */
115         /* Parse format string */
116         while ((buf < end) && *fmt) {
117         uint32_t flags = 0;
118         uint32_t width = 0;
120                 if (*fmt != '%') {
121                         *buf++ = *fmt++;
122                         continue;
123                 }
125                 /* Do we have a conversion specifier ? */
126                 fmt++;
127                 if (*fmt == '%') {
128                         *buf++ = *fmt++;
129                         continue;
130                 }
131                 /* We got a conversion specifier, any modifier ? */
132                 /* Note that '-' won't be handled */
133                 while (1) {
134                         int found = 1;
135                         switch (*fmt) {
136                                 case '-': flags |= LEFT;    break;
137                                 case '+': flags |= SIGN;    break;
138                                 case ' ': flags |= SPACE;   break;
139                                 case '#': flags |= SPECIAL; break;
140                                 case '0': flags |= ZEROPAD; break;
141                                 default:  found = 0;
142                         }
143                         if (!found)
144                                 break;
145                         fmt++;
146                 }
147                 /* Field width ? */
148                 while ((*fmt >= '0') && (*fmt <= '9')) {
149                         width = (width * 10) + (*fmt - '0');
150                         fmt++;
151                 }
152                 /* We do not handle floats, floats have nothing to do with embeded systems */
153                 /* Skip any precision */
154                 if (*fmt == '.') {
155                         do {
156                                 fmt++;
157                         } while ((*fmt >= '0') && (*fmt <= '9'));
158                 }
159                 /* Qualifier ? Should we really handle length modifiers ?
160                  * The while loop is here to skip these. */
161                 /* Handle conversion, if supported. Note that we may spend some time in here
162          * while waiting for the buffer to get available again  */
163                 while (*fmt) {
164                         int found = 1;
165                         switch (*fmt) {
166                                 /* signed */
167                                 case 'd':
168                                 case 'i':
169                                         flags |= SIGNED;
170                                         buf += convert(buf, end, flags, width, (uint32_t)va_arg(args, signed int));
171                                         break;
172                                 /* unsigned */
173                                 case 'x':
174                                         flags |= LOWERCASE;
175                                 case 'X':
176                                         flags |= HEXA;
177                                 case 'u':
178                                         buf += convert(buf, end, flags, width, (uint32_t)va_arg(args, unsigned int));
179                                         break;
180                                 /* string */
181                                 case 's': {
182                                         /* Copy string to buf */
183                                         char* tmp = va_arg(args, char *);
184                                         while ((buf < end) && *tmp) {
185                                                 *buf++ = *tmp++;
186                                         }
187                                         break;
188                                 }
189                                 /* character */
190                                 case 'c':
191                                         *buf++ = (char)va_arg(args, unsigned int);
192                                         break;
193                                 default:
194                                         found = 0;
195                         }
196                         fmt++;
197                         if (found)
198                                 break;
199                 }
200         }
201         *buf = '\0';
202         return (buf - start);
206 int snprintf(char* buf, size_t size, const char *format, ...)
208         va_list args;
209         int r;
211         va_start(args, format);
212     r = vsnprintf(buf, size, format, args);
213     va_end(args);
215     return r;