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, see <http://www.gnu.org/licenses/>.
18 */
19 #include <stdarg.h>
20 #include <stdint.h>
21 #include "rombios_compat.h"
22 #include "util.h"
23
24 static void putchar(char c);
25 #define isdigit(c) ((c) >= '0' && (c) <= '9')
26
outb(uint16_t addr,uint8_t val)27 void outb(uint16_t addr, uint8_t val)
28 {
29 __asm__ __volatile__ ( "outb %%al, %%dx" :: "d"(addr), "a"(val) );
30 }
31
outw(uint16_t addr,uint16_t val)32 void outw(uint16_t addr, uint16_t val)
33 {
34 __asm__ __volatile__ ( "outw %%ax, %%dx" :: "d"(addr), "a"(val) );
35 }
36
outl(uint16_t addr,uint32_t val)37 void outl(uint16_t addr, uint32_t val)
38 {
39 __asm__ __volatile__ ( "outl %%eax, %%dx" :: "d"(addr), "a"(val) );
40 }
41
inb(uint16_t addr)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
inw(uint16_t addr)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
inl(uint16_t addr)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
itoa(char * a,unsigned int i)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
strcmp(const char * cs,const char * ct)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
strncmp(const char * s1,const char * s2,uint32_t n)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
memcpy(void * dest,const void * src,unsigned n)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
memmove(void * dest,const void * src,unsigned n)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 *
strcpy(char * dest,const char * src)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 *
strncpy(char * dest,const char * src,unsigned n)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
strlen(const char * s)175 strlen(const char *s)
176 {
177 int i = 0;
178 while ( *s++ )
179 i++;
180 return i;
181 }
182
183 void *
memset(void * s,int c,unsigned n)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
memcmp(const void * s1,const void * s2,unsigned n)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
cpuid(uint32_t idx,uint32_t * eax,uint32_t * ebx,uint32_t * ecx,uint32_t * edx)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
byte_to_hex(char * digits,uint8_t byte)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
uuid_to_string(char * dest,uint8_t * uuid)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
printnum(char * p,unsigned long num,int base)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
_doprint(void (* put)(char),const char * fmt,va_list ap)293 static void _doprint(void (*put)(char), const char *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
putchar(char c)375 static void putchar(char c)
376 {
377 outb(0xe9, c);
378 }
379
printf(const char * fmt,...)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
mssleep(uint32_t waittime)391 void mssleep(uint32_t waittime)
392 {
393 uint32_t i;
394 uint8_t x, y = inb(0x61) & 0x10;
395
396 /* Poll the DRAM refresh timer: I/O port 61h, bit 4 toggles every 15us. */
397 waittime *= 67; /* Convert milliseconds to multiples of 15us. */
398 for ( i = 0; i < waittime; i++ )
399 {
400 while ( (x = inb(0x61) & 0x10) == y )
401 continue;
402 y = x;
403 }
404 }
405
406 /*
407 * Search for the RSDP ACPI table in the memory starting at addr and
408 * ending at addr + len - 1.
409 */
__find_rsdp(const void * start,unsigned int len)410 static struct acpi_20_rsdp *__find_rsdp(const void *start, unsigned int len)
411 {
412 char *rsdp = (char *)start;
413 char *end = rsdp + len;
414 /* scan memory in steps of 16 bytes */
415 while (rsdp < end) {
416 /* check for expected string */
417 if (!strncmp(rsdp, "RSD PTR ", 8))
418 return (struct acpi_20_rsdp *)rsdp;
419 rsdp += 0x10;
420 }
421 return 0;
422 }
423
find_rsdp(void)424 struct acpi_20_rsdp *find_rsdp(void)
425 {
426 struct acpi_20_rsdp *rsdp;
427 uint16_t ebda_seg;
428
429 ebda_seg = *(uint16_t *)ADDR_FROM_SEG_OFF(0x40, 0xe);
430 rsdp = __find_rsdp((void *)(ebda_seg << 16), 1024);
431 if (!rsdp)
432 rsdp = __find_rsdp((void *)0xE0000, 0x20000);
433
434 return rsdp;
435 }
436
get_s3_waking_vector(void)437 uint32_t get_s3_waking_vector(void)
438 {
439 struct acpi_20_rsdp *rsdp = find_rsdp();
440 struct acpi_20_xsdt *xsdt;
441 struct acpi_fadt *fadt;
442 struct acpi_20_facs *facs;
443 uint32_t vector;
444
445 if (!rsdp)
446 return 0;
447
448 xsdt = (struct acpi_20_xsdt *)(long)rsdp->xsdt_address;
449 if (!xsdt)
450 return 0;
451
452 fadt = (struct acpi_fadt *)(long)xsdt->entry[0];
453 if (!fadt || (fadt->header.signature != ACPI_FADT_SIGNATURE))
454 return 0;
455
456 facs = (struct acpi_20_facs *)(long)fadt->x_firmware_ctrl;
457 if (!facs)
458 return 0;
459
460 vector = facs->x_firmware_waking_vector;
461 if (!vector)
462 vector = facs->firmware_waking_vector;
463
464 return vector;
465 }
466