1
2
3
4
5
6
7#include <string.h>
8#include <div64.h>
9#include <console/vtxprintf.h>
10
11
12#define isdigit(c) ((c) >= '0' && (c) <= '9')
13#define is_digit isdigit
14#define isxdigit(c) (((c) >= '0' && (c) <= '9') || ((c) >= 'a' && (c) <= 'f') || ((c) >= 'A' && (c) <= 'F'))
15
16static int skip_atoi(const char **s)
17{
18 int i=0;
19
20 while (is_digit(**s))
21 i = i*10 + *((*s)++) - '0';
22 return i;
23}
24
25#define ZEROPAD 1
26#define SIGN 2
27#define PLUS 4
28#define SPACE 8
29#define LEFT 16
30#define SPECIAL 32
31#define LARGE 64
32
33static int number(void (*tx_byte)(unsigned char byte),
34 unsigned long long num, int base, int size, int precision, int type)
35{
36 char c,sign,tmp[66];
37 const char *digits="0123456789abcdefghijklmnopqrstuvwxyz";
38 int i;
39 int count = 0;
40
41 if (type & LARGE)
42 digits = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
43 if (type & LEFT)
44 type &= ~ZEROPAD;
45 if (base < 2 || base > 36)
46 return 0;
47 c = (type & ZEROPAD) ? '0' : ' ';
48 sign = 0;
49 if (type & SIGN) {
50 if ((signed long long)num < 0) {
51 sign = '-';
52 num = -num;
53 size--;
54 } else if (type & PLUS) {
55 sign = '+';
56 size--;
57 } else if (type & SPACE) {
58 sign = ' ';
59 size--;
60 }
61 }
62 if (type & SPECIAL) {
63 if (base == 16)
64 size -= 2;
65 else if (base == 8)
66 size--;
67 }
68 i = 0;
69 if (num == 0)
70 tmp[i++]='0';
71 else while (num != 0)
72 tmp[i++] = digits[do_div(num,base)];
73 if (i > precision)
74 precision = i;
75 size -= precision;
76 if (!(type&(ZEROPAD+LEFT)))
77 while(size-->0)
78 tx_byte(' '), count++;
79 if (sign)
80 tx_byte(sign), count++;
81 if (type & SPECIAL) {
82 if (base==8)
83 tx_byte('0'), count++;
84 else if (base==16) {
85 tx_byte('0'), count++;
86 tx_byte(digits[33]), count++;
87 }
88 }
89 if (!(type & LEFT))
90 while (size-- > 0)
91 tx_byte(c), count++;
92 while (i < precision--)
93 tx_byte('0'), count++;
94 while (i-- > 0)
95 tx_byte(tmp[i]), count++;
96 while (size-- > 0)
97 tx_byte(' '), count++;
98 return count;
99}
100
101
102int vtxprintf(void (*tx_byte)(unsigned char byte), const char *fmt, va_list args)
103{
104 int len;
105 unsigned long long num;
106 int i, base;
107 const char *s;
108
109 int flags;
110
111 int field_width;
112 int precision;
113
114 int qualifier;
115
116 int count;
117
118 for (count=0; *fmt ; ++fmt) {
119 if (*fmt != '%') {
120 tx_byte(*fmt), count++;
121 continue;
122 }
123
124
125 flags = 0;
126 repeat:
127 ++fmt;
128 switch (*fmt) {
129 case '-': flags |= LEFT; goto repeat;
130 case '+': flags |= PLUS; goto repeat;
131 case ' ': flags |= SPACE; goto repeat;
132 case '#': flags |= SPECIAL; goto repeat;
133 case '0': flags |= ZEROPAD; goto repeat;
134 }
135
136
137 field_width = -1;
138 if (is_digit(*fmt))
139 field_width = skip_atoi(&fmt);
140 else if (*fmt == '*') {
141 ++fmt;
142
143 field_width = va_arg(args, int);
144 if (field_width < 0) {
145 field_width = -field_width;
146 flags |= LEFT;
147 }
148 }
149
150
151 precision = -1;
152 if (*fmt == '.') {
153 ++fmt;
154 if (is_digit(*fmt))
155 precision = skip_atoi(&fmt);
156 else if (*fmt == '*') {
157 ++fmt;
158
159 precision = va_arg(args, int);
160 }
161 if (precision < 0)
162 precision = 0;
163 }
164
165
166 qualifier = -1;
167 if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L') {
168 qualifier = *fmt;
169 ++fmt;
170 if (*fmt == 'l') {
171 qualifier = 'L';
172 ++fmt;
173 }
174 }
175
176
177 base = 10;
178
179 switch (*fmt) {
180 case 'c':
181 if (!(flags & LEFT))
182 while (--field_width > 0)
183 tx_byte(' '), count++;
184 tx_byte((unsigned char) va_arg(args, int)), count++;
185 while (--field_width > 0)
186 tx_byte(' '), count++;
187 continue;
188
189 case 's':
190 s = va_arg(args, char *);
191 if (!s)
192 s = "<NULL>";
193
194 len = strnlen(s, precision);
195
196 if (!(flags & LEFT))
197 while (len < field_width--)
198 tx_byte(' '), count++;
199 for (i = 0; i < len; ++i)
200 tx_byte(*s++), count++;
201 while (len < field_width--)
202 tx_byte(' '), count++;
203 continue;
204
205 case 'p':
206 if (field_width == -1) {
207 field_width = 2*sizeof(void *);
208 flags |= ZEROPAD;
209 }
210 count += number(tx_byte,
211 (unsigned long) va_arg(args, void *), 16,
212 field_width, precision, flags);
213 continue;
214
215
216 case 'n':
217 if (qualifier == 'L') {
218 long long *ip = va_arg(args, long long *);
219 *ip = count;
220 } else if (qualifier == 'l') {
221 long * ip = va_arg(args, long *);
222 *ip = count;
223 } else {
224 int * ip = va_arg(args, int *);
225 *ip = count;
226 }
227 continue;
228
229 case '%':
230 tx_byte('%'), count++;
231 continue;
232
233
234 case 'o':
235 base = 8;
236 break;
237
238 case 'X':
239 flags |= LARGE;
240 case 'x':
241 base = 16;
242 break;
243
244 case 'd':
245 case 'i':
246 flags |= SIGN;
247 case 'u':
248 break;
249
250 default:
251 tx_byte('%'), count++;
252 if (*fmt)
253 tx_byte(*fmt), count++;
254 else
255 --fmt;
256 continue;
257 }
258 if (qualifier == 'L') {
259 num = va_arg(args, unsigned long long);
260 } else if (qualifier == 'l') {
261 num = va_arg(args, unsigned long);
262 } else if (qualifier == 'h') {
263 num = (unsigned short) va_arg(args, int);
264 if (flags & SIGN)
265 num = (short) num;
266 } else if (flags & SIGN) {
267 num = va_arg(args, int);
268 } else {
269 num = va_arg(args, unsigned int);
270 }
271 count += number(tx_byte, num, base, field_width, precision, flags);
272 }
273 return count;
274}
275
276
