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 | |
---|
24 | static void putchar(char c); |
---|
25 | #define isdigit(c) ((c) >= '0' && (c) <= '9') |
---|
26 | |
---|
27 | void outb(uint16_t addr, uint8_t val) |
---|
28 | { |
---|
29 | __asm__ __volatile__ ( "outb %%al, %%dx" :: "d"(addr), "a"(val) ); |
---|
30 | } |
---|
31 | |
---|
32 | void outw(uint16_t addr, uint16_t val) |
---|
33 | { |
---|
34 | __asm__ __volatile__ ( "outw %%ax, %%dx" :: "d"(addr), "a"(val) ); |
---|
35 | } |
---|
36 | |
---|
37 | void outl(uint16_t addr, uint32_t val) |
---|
38 | { |
---|
39 | __asm__ __volatile__ ( "outl %%eax, %%dx" :: "d"(addr), "a"(val) ); |
---|
40 | } |
---|
41 | |
---|
42 | uint8_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 | |
---|
49 | uint16_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 | |
---|
56 | uint32_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 | |
---|
63 | char *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 | |
---|
83 | int 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 | |
---|
93 | int 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 | |
---|
102 | void *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 | |
---|
122 | void *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 | |
---|
140 | char * |
---|
141 | strcpy(char *dest, const char *src) |
---|
142 | { |
---|
143 | char *p = dest; |
---|
144 | while ( *src ) |
---|
145 | *p++ = *src++; |
---|
146 | *p = 0; |
---|
147 | return dest; |
---|
148 | } |
---|
149 | |
---|
150 | char * |
---|
151 | strncpy(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 | |
---|
174 | unsigned |
---|
175 | strlen(const char *s) |
---|
176 | { |
---|
177 | int i = 0; |
---|
178 | while ( *s++ ) |
---|
179 | i++; |
---|
180 | return i; |
---|
181 | } |
---|
182 | |
---|
183 | void * |
---|
184 | memset(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 | |
---|
194 | int |
---|
195 | memcmp(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 | |
---|
212 | void |
---|
213 | cpuid(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 */ |
---|
223 | void |
---|
224 | byte_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 */ |
---|
244 | void |
---|
245 | uuid_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 | |
---|
282 | static 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 | |
---|
293 | static 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 | |
---|
375 | static void putchar(char c) |
---|
376 | { |
---|
377 | outb(0xe9, c); |
---|
378 | } |
---|
379 | |
---|
380 | int 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 | |
---|
391 | int vprintf(const char *fmt, va_list ap) |
---|
392 | { |
---|
393 | _doprint(putchar, fmt, ap); |
---|
394 | return 0; |
---|
395 | } |
---|
396 | |
---|
397 | void 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 | } |
---|