7bb5a86e187d441f78bb2f5415b5f7754ebc2f82
[openwrt/openwrt.git] / target / linux / lantiq / image / lzma-loader / src / printf.c
1 /*
2 * Copyright (C) 2001 MontaVista Software Inc.
3 * Author: Jun Sun, jsun@mvista.com or jsun@junsun.net
4 *
5 * This program is free software; you can redistribute it and/or modify it
6 * under the terms of the GNU General Public License as published by the
7 * Free Software Foundation; either version 2 of the License, or (at your
8 * option) any later version.
9 *
10 */
11
12 #include "printf.h"
13
14 extern void board_putc(int ch);
15
16 /* this is the maximum width for a variable */
17 #define LP_MAX_BUF 256
18
19 /* macros */
20 #define IsDigit(x) ( ((x) >= '0') && ((x) <= '9') )
21 #define Ctod(x) ( (x) - '0')
22
23 /* forward declaration */
24 static int PrintChar(char *, char, int, int);
25 static int PrintString(char *, char *, int, int);
26 static int PrintNum(char *, unsigned long, int, int, int, int, char, int);
27
28 /* private variable */
29 static const char theFatalMsg[] = "fatal error in lp_Print!";
30
31 /* -*-
32 * A low level printf() function.
33 */
34 static void
35 lp_Print(void (*output)(void *, char *, int),
36 void * arg,
37 char *fmt,
38 va_list ap)
39 {
40
41 #define OUTPUT(arg, s, l) \
42 { if (((l) < 0) || ((l) > LP_MAX_BUF)) { \
43 (*output)(arg, (char*)theFatalMsg, sizeof(theFatalMsg)-1); for(;;); \
44 } else { \
45 (*output)(arg, s, l); \
46 } \
47 }
48
49 char buf[LP_MAX_BUF];
50
51 char c;
52 char *s;
53 long int num;
54
55 int longFlag;
56 int negFlag;
57 int width;
58 int prec;
59 int ladjust;
60 char padc;
61
62 int length;
63
64 for(;;) {
65 {
66 /* scan for the next '%' */
67 char *fmtStart = fmt;
68 while ( (*fmt != '\0') && (*fmt != '%')) {
69 fmt ++;
70 }
71
72 /* flush the string found so far */
73 OUTPUT(arg, fmtStart, fmt-fmtStart);
74
75 /* are we hitting the end? */
76 if (*fmt == '\0') break;
77 }
78
79 /* we found a '%' */
80 fmt ++;
81
82 /* check for long */
83 if (*fmt == 'l') {
84 longFlag = 1;
85 fmt ++;
86 } else {
87 longFlag = 0;
88 }
89
90 /* check for other prefixes */
91 width = 0;
92 prec = -1;
93 ladjust = 0;
94 padc = ' ';
95
96 if (*fmt == '-') {
97 ladjust = 1;
98 fmt ++;
99 }
100
101 if (*fmt == '0') {
102 padc = '0';
103 fmt++;
104 }
105
106 if (IsDigit(*fmt)) {
107 while (IsDigit(*fmt)) {
108 width = 10 * width + Ctod(*fmt++);
109 }
110 }
111
112 if (*fmt == '.') {
113 fmt ++;
114 if (IsDigit(*fmt)) {
115 prec = 0;
116 while (IsDigit(*fmt)) {
117 prec = prec*10 + Ctod(*fmt++);
118 }
119 }
120 }
121
122
123 /* check format flag */
124 negFlag = 0;
125 switch (*fmt) {
126 case 'b':
127 if (longFlag) {
128 num = va_arg(ap, long int);
129 } else {
130 num = va_arg(ap, int);
131 }
132 length = PrintNum(buf, num, 2, 0, width, ladjust, padc, 0);
133 OUTPUT(arg, buf, length);
134 break;
135
136 case 'd':
137 case 'D':
138 if (longFlag) {
139 num = va_arg(ap, long int);
140 } else {
141 num = va_arg(ap, int);
142 }
143 if (num < 0) {
144 num = - num;
145 negFlag = 1;
146 }
147 length = PrintNum(buf, num, 10, negFlag, width, ladjust, padc, 0);
148 OUTPUT(arg, buf, length);
149 break;
150
151 case 'o':
152 case 'O':
153 if (longFlag) {
154 num = va_arg(ap, long int);
155 } else {
156 num = va_arg(ap, int);
157 }
158 length = PrintNum(buf, num, 8, 0, width, ladjust, padc, 0);
159 OUTPUT(arg, buf, length);
160 break;
161
162 case 'u':
163 case 'U':
164 if (longFlag) {
165 num = va_arg(ap, long int);
166 } else {
167 num = va_arg(ap, int);
168 }
169 length = PrintNum(buf, num, 10, 0, width, ladjust, padc, 0);
170 OUTPUT(arg, buf, length);
171 break;
172
173 case 'x':
174 if (longFlag) {
175 num = va_arg(ap, long int);
176 } else {
177 num = va_arg(ap, int);
178 }
179 length = PrintNum(buf, num, 16, 0, width, ladjust, padc, 0);
180 OUTPUT(arg, buf, length);
181 break;
182
183 case 'X':
184 if (longFlag) {
185 num = va_arg(ap, long int);
186 } else {
187 num = va_arg(ap, int);
188 }
189 length = PrintNum(buf, num, 16, 0, width, ladjust, padc, 1);
190 OUTPUT(arg, buf, length);
191 break;
192
193 case 'c':
194 c = (char)va_arg(ap, int);
195 length = PrintChar(buf, c, width, ladjust);
196 OUTPUT(arg, buf, length);
197 break;
198
199 case 's':
200 s = (char*)va_arg(ap, char *);
201 length = PrintString(buf, s, width, ladjust);
202 OUTPUT(arg, buf, length);
203 break;
204
205 case '\0':
206 fmt --;
207 break;
208
209 default:
210 /* output this char as it is */
211 OUTPUT(arg, fmt, 1);
212 } /* switch (*fmt) */
213
214 fmt ++;
215 } /* for(;;) */
216
217 /* special termination call */
218 OUTPUT(arg, "\0", 1);
219 }
220
221
222 /* --------------- local help functions --------------------- */
223 static int
224 PrintChar(char * buf, char c, int length, int ladjust)
225 {
226 int i;
227
228 if (length < 1) length = 1;
229 if (ladjust) {
230 *buf = c;
231 for (i=1; i< length; i++) buf[i] = ' ';
232 } else {
233 for (i=0; i< length-1; i++) buf[i] = ' ';
234 buf[length - 1] = c;
235 }
236 return length;
237 }
238
239 static int
240 PrintString(char * buf, char* s, int length, int ladjust)
241 {
242 int i;
243 int len=0;
244 char* s1 = s;
245 while (*s1++) len++;
246 if (length < len) length = len;
247
248 if (ladjust) {
249 for (i=0; i< len; i++) buf[i] = s[i];
250 for (i=len; i< length; i++) buf[i] = ' ';
251 } else {
252 for (i=0; i< length-len; i++) buf[i] = ' ';
253 for (i=length-len; i < length; i++) buf[i] = s[i-length+len];
254 }
255 return length;
256 }
257
258 static int
259 PrintNum(char * buf, unsigned long u, int base, int negFlag,
260 int length, int ladjust, char padc, int upcase)
261 {
262 /* algorithm :
263 * 1. prints the number from left to right in reverse form.
264 * 2. fill the remaining spaces with padc if length is longer than
265 * the actual length
266 * TRICKY : if left adjusted, no "0" padding.
267 * if negtive, insert "0" padding between "0" and number.
268 * 3. if (!ladjust) we reverse the whole string including paddings
269 * 4. otherwise we only reverse the actual string representing the num.
270 */
271
272 int actualLength =0;
273 char *p = buf;
274 int i;
275
276 do {
277 int tmp = u %base;
278 if (tmp <= 9) {
279 *p++ = '0' + tmp;
280 } else if (upcase) {
281 *p++ = 'A' + tmp - 10;
282 } else {
283 *p++ = 'a' + tmp - 10;
284 }
285 u /= base;
286 } while (u != 0);
287
288 if (negFlag) {
289 *p++ = '-';
290 }
291
292 /* figure out actual length and adjust the maximum length */
293 actualLength = p - buf;
294 if (length < actualLength) length = actualLength;
295
296 /* add padding */
297 if (ladjust) {
298 padc = ' ';
299 }
300 if (negFlag && !ladjust && (padc == '0')) {
301 for (i = actualLength-1; i< length-1; i++) buf[i] = padc;
302 buf[length -1] = '-';
303 } else {
304 for (i = actualLength; i< length; i++) buf[i] = padc;
305 }
306
307
308 /* prepare to reverse the string */
309 {
310 int begin = 0;
311 int end;
312 if (ladjust) {
313 end = actualLength - 1;
314 } else {
315 end = length -1;
316 }
317
318 while (end > begin) {
319 char tmp = buf[begin];
320 buf[begin] = buf[end];
321 buf[end] = tmp;
322 begin ++;
323 end --;
324 }
325 }
326
327 /* adjust the string pointer */
328 return length;
329 }
330
331 static void printf_output(void *arg, char *s, int l)
332 {
333 int i;
334
335 // special termination call
336 if ((l==1) && (s[0] == '\0')) return;
337
338 for (i=0; i< l; i++) {
339 board_putc(s[i]);
340 if (s[i] == '\n') board_putc('\r');
341 }
342 }
343
344 void printf(char *fmt, ...)
345 {
346 va_list ap;
347 va_start(ap, fmt);
348 lp_Print(printf_output, 0, fmt, ap);
349 va_end(ap);
350 }