source: trunk/packages/xen-3.1/xen-3.1/tools/firmware/rombios/32bit/util.c @ 34

Last change on this file since 34 was 34, checked in by hartmans, 18 years ago

Add xen and xen-common

File size: 8.5 KB
Line 
1/*
2 * util.c: Helper library functions for HVMLoader.
3 *
4 * Leendert van Doorn, leendert@watson.ibm.com
5 * Copyright (c) 2005, International Business Machines Corporation.
6 *
7 * This program is free software; you can redistribute it and/or modify it
8 * under the terms and conditions of the GNU General Public License,
9 * version 2, as published by the Free Software Foundation.
10 *
11 * This program is distributed in the hope it will be useful, but WITHOUT
12 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
13 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
14 * more details.
15 *
16 * You should have received a copy of the GNU General Public License along with
17 * this program; if not, write to the Free Software Foundation, Inc., 59 Temple
18 * Place - Suite 330, Boston, MA 02111-1307 USA.
19 */
20#include <stdarg.h>
21#include <stdint.h>
22#include "util.h"
23
24static void putchar(char c);
25#define isdigit(c) ((c) >= '0' && (c) <= '9')
26
27void outb(uint16_t addr, uint8_t val)
28{
29    __asm__ __volatile__ ( "outb %%al, %%dx" :: "d"(addr), "a"(val) );
30}
31
32void outw(uint16_t addr, uint16_t val)
33{
34    __asm__ __volatile__ ( "outw %%ax, %%dx" :: "d"(addr), "a"(val) );
35}
36
37void outl(uint16_t addr, uint32_t val)
38{
39    __asm__ __volatile__ ( "outl %%eax, %%dx" :: "d"(addr), "a"(val) );
40}
41
42uint8_t inb(uint16_t addr)
43{
44    uint8_t val;
45    __asm__ __volatile__ ( "inb %%dx,%%al" : "=a" (val) : "d" (addr) );
46    return val;
47}
48
49uint16_t inw(uint16_t addr)
50{
51    uint16_t val;
52    __asm__ __volatile__ ( "inw %%dx,%%ax" : "=a" (val) : "d" (addr) );
53    return val;
54}
55
56uint32_t inl(uint16_t addr)
57{
58    uint32_t val;
59    __asm__ __volatile__ ( "inl %%dx,%%eax" : "=a" (val) : "d" (addr) );
60    return val;
61}
62
63char *itoa(char *a, unsigned int i)
64{
65    unsigned int _i = i, x = 0;
66
67    do {
68        x++;
69        _i /= 10;
70    } while ( _i != 0 );
71
72    a += x;
73    *a-- = '\0';
74
75    do {
76        *a-- = (i % 10) + '0';
77        i /= 10;
78    } while ( i != 0 );
79
80    return a + 1;
81}
82
83int strcmp(const char *cs, const char *ct)
84{
85    signed char res;
86
87    while ( ((res = *cs - *ct++) == 0) && (*cs++ != '\0') )
88        continue;
89
90    return res;
91}
92
93int strncmp(const char *s1, const char *s2, uint32_t n)
94{
95        uint32_t ctr;
96        for (ctr = 0; ctr < n; ctr++)
97                if (s1[ctr] != s2[ctr])
98                        return (int)(s1[ctr] - s2[ctr]);
99        return 0;
100}
101
102void *memcpy(void *dest, const void *src, unsigned n)
103{
104    int t0, t1, t2;
105
106    __asm__ __volatile__ (
107        "cld\n"
108        "rep; movsl\n"
109        "testb $2,%b4\n"
110        "je 1f\n"
111        "movsw\n"
112        "1: testb $1,%b4\n"
113        "je 2f\n"
114        "movsb\n"
115        "2:"
116        : "=&c" (t0), "=&D" (t1), "=&S" (t2)
117        : "0" (n/4), "q" (n), "1" ((long) dest), "2" ((long) src)
118        : "memory" );
119    return dest;
120}
121
122void *memmove(void *dest, const void *src, unsigned n)
123{
124    if ( (long)dest > (long)src )
125    {
126        n--;
127        while ( n > 0 )
128        {
129            ((char *)dest)[n] = ((char *)src)[n];
130            n--;
131        }
132    }
133    else
134    {
135        memcpy(dest, src, n);
136    }
137    return dest;
138}
139
140char *
141strcpy(char *dest, const char *src)
142{
143    char *p = dest;
144    while ( *src )
145        *p++ = *src++;
146    *p = 0;
147    return dest;
148}
149
150char *
151strncpy(char *dest, const char *src, unsigned n)
152{
153    int i = 0;
154    char *p = dest;
155
156    /* write non-NUL characters from src into dest until we run
157       out of room in dest or encounter a NUL in src */
158    while ( (i < n) && *src )
159    {
160        *p++ = *src++;
161        i++;
162    }
163
164    /* pad remaining bytes of dest with NUL bytes */
165    while ( i < n )
166    {
167        *p++ = 0;
168        i++;
169    }
170
171    return dest;
172}
173
174unsigned
175strlen(const char *s)
176{
177    int i = 0;
178    while ( *s++ )
179        i++;
180    return i;
181}
182
183void *
184memset(void *s, int c, unsigned n)
185{
186    uint8_t b = (uint8_t) c;
187    uint8_t *p = (uint8_t *)s;
188    int i;
189    for ( i = 0; i < n; i++ )
190        *p++ = b;
191    return s;
192}
193
194int
195memcmp(const void *s1, const void *s2, unsigned n)
196{
197    unsigned i;
198    uint8_t *p1 = (uint8_t *) s1;
199    uint8_t *p2 = (uint8_t *) s2;
200
201    for ( i = 0; i < n; i++ )
202    {
203        if ( p1[i] < p2[i] )
204            return -1;
205        else if ( p1[i] > p2[i] )
206            return 1;
207    }
208
209    return 0;
210}
211
212void
213cpuid(uint32_t idx, uint32_t *eax, uint32_t *ebx, uint32_t *ecx, uint32_t *edx)
214{
215    __asm__ __volatile__ (
216        "cpuid"
217        : "=a" (*eax), "=b" (*ebx), "=c" (*ecx), "=d" (*edx)
218        : "0" (idx) );
219}
220
221/* Write a two-character hex representation of 'byte' to digits[].
222   Pre-condition: sizeof(digits) >= 2 */
223void
224byte_to_hex(char *digits, uint8_t byte)
225{
226    uint8_t nybbel = byte >> 4;
227
228    if ( nybbel > 9 )
229        digits[0] = 'a' + nybbel-10;
230    else
231        digits[0] = '0' + nybbel;
232
233    nybbel = byte & 0x0f;
234    if ( nybbel > 9 )
235        digits[1] = 'a' + nybbel-10;
236    else
237        digits[1] = '0' + nybbel;
238}
239
240/* Convert an array of 16 unsigned bytes to a DCE/OSF formatted UUID
241   string.
242
243   Pre-condition: sizeof(dest) >= 37 */
244void
245uuid_to_string(char *dest, uint8_t *uuid)
246{
247    int i = 0;
248    char *p = dest;
249
250    for ( i = 0; i < 4; i++ )
251    {
252        byte_to_hex(p, uuid[i]);
253        p += 2;
254    }
255    *p++ = '-';
256    for ( i = 4; i < 6; i++ )
257    {
258        byte_to_hex(p, uuid[i]);
259        p += 2;
260    }
261    *p++ = '-';
262    for ( i = 6; i < 8; i++ )
263    {
264        byte_to_hex(p, uuid[i]);
265        p += 2;
266    }
267    *p++ = '-';
268    for ( i = 8; i < 10; i++ )
269    {
270        byte_to_hex(p, uuid[i]);
271        p += 2;
272    }
273    *p++ = '-';
274    for ( i = 10; i < 16; i++ )
275    {
276        byte_to_hex(p, uuid[i]);
277        p += 2;
278    }
279    *p = '\0';
280}
281
282static char *printnum(char *p, unsigned long num, int base)
283{
284    unsigned long n;
285
286    if ( (n = num/base) > 0 )
287        p = printnum(p, n, base);
288    *p++ = "0123456789abcdef"[(int)(num % base)];
289    *p = '\0';
290    return p;
291}
292
293static void _doprint(void (*put)(char), char const *fmt, va_list ap)
294{
295    register char *str, c;
296    int lflag, zflag, nflag;
297    char buffer[17];
298    unsigned value;
299    int i, slen, pad;
300
301    for ( ; *fmt != '\0'; fmt++ )
302    {
303        if ( *fmt != '%' )
304        {
305            put(*fmt);
306            continue;
307        }
308
309        pad = zflag = nflag = lflag = 0;
310        c = *++fmt;
311        if ( (c == '-') || isdigit(c) )
312        {
313            if ( c == '-' )
314            {
315                nflag = 1;
316                c = *++fmt;
317            }
318            zflag = c == '0';
319            for ( pad = 0; isdigit(c); c = *++fmt )
320                pad = (pad * 10) + c - '0';
321        }
322        if ( c == 'l' ) /* long extension */
323        {
324            lflag = 1;
325            c = *++fmt;
326        }
327        if ( (c == 'd') || (c == 'u') || (c == 'o') || (c == 'x') )
328        {
329            if ( lflag )
330                value = va_arg(ap, unsigned);
331            else
332                value = (unsigned) va_arg(ap, unsigned int);
333            str = buffer;
334            printnum(str, value,
335                     c == 'o' ? 8 : (c == 'x' ? 16 : 10));
336            goto printn;
337        }
338        else if ( (c == 'O') || (c == 'D') || (c == 'X') )
339        {
340            value = va_arg(ap, unsigned);
341            str = buffer;
342            printnum(str, value,
343                     c == 'O' ? 8 : (c == 'X' ? 16 : 10));
344        printn:
345            slen = strlen(str);
346            for ( i = pad - slen; i > 0; i-- )
347                put(zflag ? '0' : ' ');
348            while ( *str )
349                put(*str++);
350        }
351        else if ( c == 's' )
352        {
353            str = va_arg(ap, char *);
354            slen = strlen(str);
355            if ( nflag == 0 )
356                for ( i = pad - slen; i > 0; i-- )
357                    put(' ');
358            while ( *str )
359                put(*str++);
360            if ( nflag )
361                for ( i = pad - slen; i > 0; i-- )
362                    put(' ');
363        }
364        else if ( c == 'c' )
365        {
366            put(va_arg(ap, int));
367        }
368        else
369        {
370            put(*fmt);
371        }
372    }
373}
374
375static void putchar(char c)
376{
377    outb(0xe9, c);
378}
379
380int printf(const char *fmt, ...)
381{
382    va_list ap;
383
384    va_start(ap, fmt);
385    _doprint(putchar, fmt, ap);
386    va_end(ap);
387
388    return 0;
389}
390
391int vprintf(const char *fmt, va_list ap)
392{
393    _doprint(putchar, fmt, ap);
394    return 0;
395}
396
397void mssleep(uint32_t waittime)
398{
399    uint32_t i;
400    uint8_t  x, y = inb(0x61) & 0x10;
401
402    /* Poll the DRAM refresh timer: I/O port 61h, bit 4 toggles every 15us. */
403    waittime *= 67; /* Convert milliseconds to multiples of 15us. */
404    for ( i = 0; i < waittime; i++ )
405    {
406        while ( (x = inb(0x61) & 0x10) == y )
407            continue;
408        y = x;
409    }
410}
Note: See TracBrowser for help on using the repository browser.