Initial commit: functional for newer bzImage kernels
[wraplinux.git] / reloc / conio.c
1 /* ----------------------------------------------------------------------- *
2  *
3  *   Copyright 2001-2008 H. Peter Anvin - All Rights Reserved
4  *
5  *   This program is free software; you can redistribute it and/or modify
6  *   it under the terms of the GNU General Public License as published by
7  *   the Free Software Foundation, Inc., 53 Temple Place Ste 330,
8  *   Boston MA 02111-1307, USA; either version 2 of the License, or
9  *   (at your option) any later version; incorporated herein by reference.
10  *
11  * ----------------------------------------------------------------------- */
12
13 /*
14  * conio.c
15  *
16  * Output to the screen
17  */
18
19 #include <stdint.h>
20 #include "reloc.h"
21 #include "conio.h"
22
23 int putchar(int ch)
24 {
25   com32sys_t regs;
26   memset(&regs, 0, sizeof regs);
27
28   if ( ch == '\n' ) {
29     /* \n -> \r\n */
30     putchar('\r');
31   }
32
33   regs.eax.w[0] = 0x0e00|(ch&0xff);
34   intcall(0x10, &regs, NULL);
35
36   return ch;
37 }
38
39 int puts(const char *s)
40 {
41   int count = 0;
42
43   while ( *s ) {
44     putchar(*s);
45     count++;
46     s++;
47   }
48
49   return count;
50 }
51
52 /*
53  * Oh, it's a waste of space, but oh-so-yummy for debugging.  It's just
54  * initialization code anyway, so it doesn't take up space when we're
55  * actually running.  This version of printf() does not include 64-bit
56  * support.  "Live with it."
57  *
58  * Most of this code was shamelessly snarfed from the Linux kernel, then
59  * modified.
60  */
61
62 static inline int
63 isdigit(int ch)
64 {
65   return (ch >= '0') && (ch <= '9');
66 }
67
68 static int skip_atoi(const char **s)
69 {
70   int i=0;
71
72   while (isdigit(**s))
73     i = i*10 + *((*s)++) - '0';
74   return i;
75 }
76
77 unsigned int atou(const char *s)
78 {
79   unsigned int i = 0;
80   while (isdigit(*s))
81     i = i*10 + (*s++ - '0');
82   return i;
83 }
84
85 static int strnlen(const char *s, int maxlen)
86 {
87   const char *es = s;
88   while ( *es && maxlen ) {
89     es++; maxlen--;
90   }
91
92   return (es-s);
93 }
94
95 #define ZEROPAD 1               /* pad with zero */
96 #define SIGN    2               /* unsigned/signed long */
97 #define PLUS    4               /* show plus */
98 #define SPACE   8               /* space if plus */
99 #define LEFT    16              /* left justified */
100 #define SPECIAL 32              /* 0x */
101 #define LARGE   64              /* use 'ABCDEF' instead of 'abcdef' */
102
103 #define do_div(n,base) ({ \
104 int __res; \
105 __res = ((unsigned long) n) % (unsigned) base; \
106 n = ((unsigned long) n) / (unsigned) base; \
107 __res; })
108
109 static char * number(char * str, long num, int base, int size, int precision
110         ,int type)
111 {
112   char c,sign,tmp[66];
113   const char *digits="0123456789abcdefghijklmnopqrstuvwxyz";
114   int i;
115
116   if (type & LARGE)
117     digits = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
118   if (type & LEFT)
119     type &= ~ZEROPAD;
120   if (base < 2 || base > 36)
121     return 0;
122   c = (type & ZEROPAD) ? '0' : ' ';
123   sign = 0;
124   if (type & SIGN) {
125     if (num < 0) {
126       sign = '-';
127       num = -num;
128       size--;
129     } else if (type & PLUS) {
130       sign = '+';
131       size--;
132     } else if (type & SPACE) {
133       sign = ' ';
134       size--;
135     }
136   }
137   if (type & SPECIAL) {
138     if (base == 16)
139       size -= 2;
140     else if (base == 8)
141       size--;
142   }
143   i = 0;
144   if (num == 0)
145     tmp[i++]='0';
146   else while (num != 0)
147     tmp[i++] = digits[do_div(num,base)];
148   if (i > precision)
149     precision = i;
150   size -= precision;
151   if (!(type&(ZEROPAD+LEFT)))
152     while(size-->0)
153       *str++ = ' ';
154   if (sign)
155     *str++ = sign;
156   if (type & SPECIAL) {
157     if (base==8)
158       *str++ = '0';
159     else if (base==16) {
160       *str++ = '0';
161       *str++ = digits[33];
162     }
163   }
164   if (!(type & LEFT))
165     while (size-- > 0)
166       *str++ = c;
167   while (i < precision--)
168     *str++ = '0';
169   while (i-- > 0)
170     *str++ = tmp[i];
171   while (size-- > 0)
172     *str++ = ' ';
173   return str;
174 }
175
176 /* Forward decl. needed for IP address printing stuff... */
177 int sprintf(char * buf, const char *fmt, ...);
178
179 int vsprintf(char *buf, const char *fmt, va_list args)
180 {
181   int len;
182   unsigned long num;
183   int i, base;
184   char * str;
185   const char *s;
186
187   int flags;            /* flags to number() */
188
189   int field_width;      /* width of output field */
190   int precision;                /* min. # of digits for integers; max
191                                    number of chars for from string */
192   int qualifier;                /* 'h', 'l', or 'L' for integer fields */
193
194   for (str=buf ; *fmt ; ++fmt) {
195     if (*fmt != '%') {
196       *str++ = *fmt;
197       continue;
198     }
199
200     /* process flags */
201     flags = 0;
202   repeat:
203     ++fmt;              /* this also skips first '%' */
204     switch (*fmt) {
205     case '-': flags |= LEFT; goto repeat;
206     case '+': flags |= PLUS; goto repeat;
207     case ' ': flags |= SPACE; goto repeat;
208     case '#': flags |= SPECIAL; goto repeat;
209     case '0': flags |= ZEROPAD; goto repeat;
210     }
211
212     /* get field width */
213     field_width = -1;
214     if (isdigit(*fmt))
215       field_width = skip_atoi(&fmt);
216     else if (*fmt == '*') {
217       ++fmt;
218       /* it's the next argument */
219       field_width = va_arg(args, int);
220       if (field_width < 0) {
221         field_width = -field_width;
222         flags |= LEFT;
223       }
224     }
225
226     /* get the precision */
227     precision = -1;
228     if (*fmt == '.') {
229       ++fmt;
230       if (isdigit(*fmt))
231         precision = skip_atoi(&fmt);
232       else if (*fmt == '*') {
233         ++fmt;
234         /* it's the next argument */
235         precision = va_arg(args, int);
236       }
237       if (precision < 0)
238         precision = 0;
239     }
240
241     /* get the conversion qualifier */
242     qualifier = -1;
243     if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L') {
244       qualifier = *fmt;
245       ++fmt;
246     }
247
248     /* default base */
249     base = 10;
250
251     switch (*fmt) {
252     case 'c':
253       if (!(flags & LEFT))
254         while (--field_width > 0)
255           *str++ = ' ';
256       *str++ = (unsigned char) va_arg(args, int);
257       while (--field_width > 0)
258         *str++ = ' ';
259       continue;
260
261     case 's':
262       s = va_arg(args, char *);
263       len = strnlen(s, precision);
264
265       if (!(flags & LEFT))
266         while (len < field_width--)
267           *str++ = ' ';
268       for (i = 0; i < len; ++i)
269         *str++ = *s++;
270       while (len < field_width--)
271         *str++ = ' ';
272       continue;
273
274     case 'p':
275       if (field_width == -1) {
276         field_width = 2*sizeof(void *);
277         flags |= ZEROPAD;
278       }
279       str = number(str,
280                    (unsigned long) va_arg(args, void *), 16,
281                    field_width, precision, flags);
282       continue;
283
284
285     case 'n':
286       if (qualifier == 'l') {
287         long * ip = va_arg(args, long *);
288         *ip = (str - buf);
289       } else {
290         int * ip = va_arg(args, int *);
291         *ip = (str - buf);
292       }
293       continue;
294
295     case '%':
296       *str++ = '%';
297       continue;
298
299       /* integer number formats - set up the flags and "break" */
300     case 'o':
301       base = 8;
302       break;
303
304     case 'X':
305       flags |= LARGE;
306     case 'x':
307       base = 16;
308       break;
309
310     case 'd':
311     case 'i':
312       flags |= SIGN;
313     case 'u':
314       break;
315
316     default:
317       *str++ = '%';
318       if (*fmt)
319         *str++ = *fmt;
320       else
321         --fmt;
322       continue;
323     }
324     if (qualifier == 'l')
325       num = va_arg(args, unsigned long);
326     else if (qualifier == 'h') {
327       num = (unsigned short) va_arg(args, int);
328       if (flags & SIGN)
329         num = (short) num;
330     } else if (flags & SIGN)
331       num = va_arg(args, int);
332     else
333       num = va_arg(args, unsigned int);
334     str = number(str, num, base, field_width, precision, flags);
335   }
336   *str = '\0';
337   return str-buf;
338 }
339
340 int sprintf(char * buf, const char *fmt, ...)
341 {
342   va_list args;
343   int i;
344
345   va_start(args, fmt);
346   i=vsprintf(buf,fmt,args);
347   va_end(args);
348   return i;
349 }
350
351 int printf(const char *fmt, ...)
352 {
353   char printf_buf[1024];
354   va_list args;
355   int printed;
356
357   va_start(args, fmt);
358   printed = vsprintf(printf_buf, fmt, args);
359   va_end(args);
360
361   puts(printf_buf);
362
363   return printed;
364 }