ia64/xen-unstable

view tools/firmware/hvmloader/util.c @ 18398:6c50c7d089d9

hvmloader: Fix e820_malloc() after bug I introduced in c/s 18383
Signed-off-by: Keir Fraser <keir.fraser@citrix.com>
author Keir Fraser <keir.fraser@citrix.com>
date Wed Aug 27 15:16:13 2008 +0100 (2008-08-27)
parents dade7f0bdc8d
children 883d01b2fd72
line source
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 */
21 #include "util.h"
22 #include "config.h"
23 #include "e820.h"
24 #include "hypercall.h"
25 #include <stdint.h>
26 #include <xen/xen.h>
27 #include <xen/memory.h>
28 #include <xen/hvm/hvm_info_table.h>
30 void wrmsr(uint32_t idx, uint64_t v)
31 {
32 asm volatile (
33 "wrmsr"
34 : : "c" (idx), "a" ((uint32_t)v), "d" ((uint32_t)(v>>32)) );
35 }
37 uint64_t rdmsr(uint32_t idx)
38 {
39 uint32_t lo, hi;
41 asm volatile (
42 "rdmsr"
43 : "=a" (lo), "=d" (hi) : "c" (idx) );
45 return (lo | ((uint64_t)hi << 32));
46 }
48 void outb(uint16_t addr, uint8_t val)
49 {
50 asm volatile ( "outb %%al, %%dx" : : "d" (addr), "a" (val) );
51 }
53 void outw(uint16_t addr, uint16_t val)
54 {
55 asm volatile ( "outw %%ax, %%dx" : : "d" (addr), "a" (val) );
56 }
58 void outl(uint16_t addr, uint32_t val)
59 {
60 asm volatile ( "outl %%eax, %%dx" : : "d" (addr), "a" (val) );
61 }
63 uint8_t inb(uint16_t addr)
64 {
65 uint8_t val;
66 asm volatile ( "inb %%dx,%%al" : "=a" (val) : "d" (addr) );
67 return val;
68 }
70 uint16_t inw(uint16_t addr)
71 {
72 uint16_t val;
73 asm volatile ( "inw %%dx,%%ax" : "=a" (val) : "d" (addr) );
74 return val;
75 }
77 uint32_t inl(uint16_t addr)
78 {
79 uint32_t val;
80 asm volatile ( "inl %%dx,%%eax" : "=a" (val) : "d" (addr) );
81 return val;
82 }
84 uint8_t cmos_inb(uint8_t idx)
85 {
86 outb(0x70, idx);
87 return inb(0x71);
88 }
90 void cmos_outb(uint8_t idx, uint8_t val)
91 {
92 outb(0x70, idx);
93 outb(0x71, val);
94 }
96 char *itoa(char *a, unsigned int i)
97 {
98 unsigned int _i = i, x = 0;
100 do {
101 x++;
102 _i /= 10;
103 } while ( _i != 0 );
105 a += x;
106 *a-- = '\0';
108 do {
109 *a-- = (i % 10) + '0';
110 i /= 10;
111 } while ( i != 0 );
113 return a + 1;
114 }
116 int strcmp(const char *cs, const char *ct)
117 {
118 signed char res;
120 while ( ((res = *cs - *ct++) == 0) && (*cs++ != '\0') )
121 continue;
123 return res;
124 }
126 int strncmp(const char *s1, const char *s2, uint32_t n)
127 {
128 uint32_t ctr;
129 for (ctr = 0; ctr < n; ctr++)
130 if (s1[ctr] != s2[ctr])
131 return (int)(s1[ctr] - s2[ctr]);
132 return 0;
133 }
135 void *memcpy(void *dest, const void *src, unsigned n)
136 {
137 int t0, t1, t2;
139 asm volatile (
140 "cld\n"
141 "rep; movsl\n"
142 "testb $2,%b4\n"
143 "je 1f\n"
144 "movsw\n"
145 "1: testb $1,%b4\n"
146 "je 2f\n"
147 "movsb\n"
148 "2:"
149 : "=&c" (t0), "=&D" (t1), "=&S" (t2)
150 : "0" (n/4), "q" (n), "1" ((long) dest), "2" ((long) src)
151 : "memory" );
152 return dest;
153 }
155 void *memmove(void *dest, const void *src, unsigned n)
156 {
157 if ( (unsigned long)dest > (unsigned long)src )
158 while ( n-- != 0 )
159 ((char *)dest)[n] = ((char *)src)[n];
160 else
161 memcpy(dest, src, n);
162 return dest;
163 }
165 char *
166 strcpy(char *dest, const char *src)
167 {
168 char *p = dest;
169 while ( *src )
170 *p++ = *src++;
171 *p = 0;
172 return dest;
173 }
175 char *
176 strncpy(char *dest, const char *src, unsigned n)
177 {
178 int i = 0;
179 char *p = dest;
181 /* write non-NUL characters from src into dest until we run
182 out of room in dest or encounter a NUL in src */
183 while ( (i < n) && *src )
184 {
185 *p++ = *src++;
186 i++;
187 }
189 /* pad remaining bytes of dest with NUL bytes */
190 while ( i < n )
191 {
192 *p++ = 0;
193 i++;
194 }
196 return dest;
197 }
199 unsigned
200 strlen(const char *s)
201 {
202 int i = 0;
203 while ( *s++ )
204 i++;
205 return i;
206 }
208 void *
209 memset(void *s, int c, unsigned n)
210 {
211 uint8_t b = (uint8_t) c;
212 uint8_t *p = (uint8_t *)s;
213 int i;
214 for ( i = 0; i < n; i++ )
215 *p++ = b;
216 return s;
217 }
219 int
220 memcmp(const void *s1, const void *s2, unsigned n)
221 {
222 unsigned i;
223 uint8_t *p1 = (uint8_t *) s1;
224 uint8_t *p2 = (uint8_t *) s2;
226 for ( i = 0; i < n; i++ )
227 {
228 if ( p1[i] < p2[i] )
229 return -1;
230 else if ( p1[i] > p2[i] )
231 return 1;
232 }
234 return 0;
235 }
237 void
238 cpuid(uint32_t idx, uint32_t *eax, uint32_t *ebx, uint32_t *ecx, uint32_t *edx)
239 {
240 asm volatile (
241 "cpuid"
242 : "=a" (*eax), "=b" (*ebx), "=c" (*ecx), "=d" (*edx)
243 : "0" (idx) );
244 }
246 /* Write a two-character hex representation of 'byte' to digits[].
247 Pre-condition: sizeof(digits) >= 2 */
248 void
249 byte_to_hex(char *digits, uint8_t byte)
250 {
251 uint8_t nybbel = byte >> 4;
253 if ( nybbel > 9 )
254 digits[0] = 'a' + nybbel-10;
255 else
256 digits[0] = '0' + nybbel;
258 nybbel = byte & 0x0f;
259 if ( nybbel > 9 )
260 digits[1] = 'a' + nybbel-10;
261 else
262 digits[1] = '0' + nybbel;
263 }
265 /* Convert an array of 16 unsigned bytes to a DCE/OSF formatted UUID
266 string.
268 Pre-condition: sizeof(dest) >= 37 */
269 void
270 uuid_to_string(char *dest, uint8_t *uuid)
271 {
272 int i = 0;
273 char *p = dest;
275 for ( i = 0; i < 4; i++ )
276 {
277 byte_to_hex(p, uuid[i]);
278 p += 2;
279 }
280 *p++ = '-';
281 for ( i = 4; i < 6; i++ )
282 {
283 byte_to_hex(p, uuid[i]);
284 p += 2;
285 }
286 *p++ = '-';
287 for ( i = 6; i < 8; i++ )
288 {
289 byte_to_hex(p, uuid[i]);
290 p += 2;
291 }
292 *p++ = '-';
293 for ( i = 8; i < 10; i++ )
294 {
295 byte_to_hex(p, uuid[i]);
296 p += 2;
297 }
298 *p++ = '-';
299 for ( i = 10; i < 16; i++ )
300 {
301 byte_to_hex(p, uuid[i]);
302 p += 2;
303 }
304 *p = '\0';
305 }
307 static void e820_collapse(void)
308 {
309 int i = 0;
310 struct e820entry *ent = (struct e820entry *)HVM_E820;
312 while ( i < (*HVM_E820_NR-1) )
313 {
314 if ( (ent[i].type == ent[i+1].type) &&
315 ((ent[i].addr + ent[i].size) == ent[i+1].addr) )
316 {
317 ent[i].size += ent[i+1].size;
318 memcpy(&ent[i+1], &ent[i+2], (*HVM_E820_NR-i-2) * sizeof(*ent));
319 (*HVM_E820_NR)--;
320 }
321 else
322 {
323 i++;
324 }
325 }
326 }
328 uint32_t e820_malloc(uint32_t size, uint32_t align)
329 {
330 uint32_t addr;
331 int i;
332 struct e820entry *ent = (struct e820entry *)HVM_E820;
334 /* Align to at leats one kilobyte. */
335 if ( align < 1024 )
336 align = 1024;
338 for ( i = *HVM_E820_NR - 1; i >= 0; i-- )
339 {
340 addr = (ent[i].addr + ent[i].size - size) & ~(align-1);
341 if ( (ent[i].type != E820_RAM) || /* not ram? */
342 (addr < ent[i].addr) || /* too small or starts above 4gb? */
343 ((addr + size) < addr) ) /* ends above 4gb? */
344 continue;
346 if ( addr != ent[i].addr )
347 {
348 memmove(&ent[i+1], &ent[i], (*HVM_E820_NR-i) * sizeof(*ent));
349 (*HVM_E820_NR)++;
350 ent[i].size = addr - ent[i].addr;
351 ent[i+1].addr = addr;
352 ent[i+1].size -= ent[i].size;
353 i++;
354 }
356 ent[i].type = E820_RESERVED;
358 e820_collapse();
360 return addr;
361 }
363 return 0;
364 }
366 uint32_t ioapic_read(uint32_t reg)
367 {
368 *(volatile uint32_t *)(IOAPIC_BASE_ADDRESS + 0x00) = reg;
369 return *(volatile uint32_t *)(IOAPIC_BASE_ADDRESS + 0x10);
370 }
372 void ioapic_write(uint32_t reg, uint32_t val)
373 {
374 *(volatile uint32_t *)(IOAPIC_BASE_ADDRESS + 0x00) = reg;
375 *(volatile uint32_t *)(IOAPIC_BASE_ADDRESS + 0x10) = val;
376 }
378 uint32_t lapic_read(uint32_t reg)
379 {
380 return *(volatile uint32_t *)(LAPIC_BASE_ADDRESS + reg);
381 }
383 void lapic_write(uint32_t reg, uint32_t val)
384 {
385 *(volatile uint32_t *)(LAPIC_BASE_ADDRESS + reg) = val;
386 }
388 #define PCI_CONF1_ADDRESS(bus, devfn, reg) \
389 (0x80000000 | (bus << 16) | (devfn << 8) | (reg & ~3))
391 uint32_t pci_read(uint32_t devfn, uint32_t reg, uint32_t len)
392 {
393 outl(0xcf8, PCI_CONF1_ADDRESS(0, devfn, reg));
395 switch ( len )
396 {
397 case 1: return inb(0xcfc + (reg & 3));
398 case 2: return inw(0xcfc + (reg & 2));
399 }
401 return inl(0xcfc);
402 }
404 void pci_write(uint32_t devfn, uint32_t reg, uint32_t len, uint32_t val)
405 {
406 outl(0xcf8, PCI_CONF1_ADDRESS(0, devfn, reg));
408 switch ( len )
409 {
410 case 1: outb(0xcfc + (reg & 3), val); break;
411 case 2: outw(0xcfc + (reg & 2), val); break;
412 case 4: outl(0xcfc, val); break;
413 }
414 }
416 static char *printnum(char *p, unsigned long num, int base)
417 {
418 unsigned long n;
420 if ( (n = num/base) > 0 )
421 p = printnum(p, n, base);
422 *p++ = "0123456789abcdef"[(int)(num % base)];
423 *p = '\0';
424 return p;
425 }
427 static void _doprint(void (*put)(char), const char *fmt, va_list ap)
428 {
429 register char *str, c;
430 int lflag, zflag, nflag;
431 char buffer[17];
432 unsigned value;
433 int i, slen, pad;
435 for ( ; *fmt != '\0'; fmt++ )
436 {
437 if ( *fmt != '%' )
438 {
439 put(*fmt);
440 continue;
441 }
443 pad = zflag = nflag = lflag = 0;
444 c = *++fmt;
445 if ( (c == '-') || isdigit(c) )
446 {
447 if ( c == '-' )
448 {
449 nflag = 1;
450 c = *++fmt;
451 }
452 zflag = c == '0';
453 for ( pad = 0; isdigit(c); c = *++fmt )
454 pad = (pad * 10) + c - '0';
455 }
456 if ( c == 'l' ) /* long extension */
457 {
458 lflag = 1;
459 c = *++fmt;
460 }
461 if ( (c == 'd') || (c == 'u') || (c == 'o') || (c == 'x') )
462 {
463 if ( lflag )
464 value = va_arg(ap, unsigned);
465 else
466 value = (unsigned) va_arg(ap, unsigned int);
467 str = buffer;
468 printnum(str, value,
469 c == 'o' ? 8 : (c == 'x' ? 16 : 10));
470 goto printn;
471 }
472 else if ( (c == 'O') || (c == 'D') || (c == 'X') )
473 {
474 value = va_arg(ap, unsigned);
475 str = buffer;
476 printnum(str, value,
477 c == 'O' ? 8 : (c == 'X' ? 16 : 10));
478 printn:
479 slen = strlen(str);
480 for ( i = pad - slen; i > 0; i-- )
481 put(zflag ? '0' : ' ');
482 while ( *str )
483 put(*str++);
484 }
485 else if ( c == 's' )
486 {
487 str = va_arg(ap, char *);
488 slen = strlen(str);
489 if ( nflag == 0 )
490 for ( i = pad - slen; i > 0; i-- )
491 put(' ');
492 while ( *str )
493 put(*str++);
494 if ( nflag )
495 for ( i = pad - slen; i > 0; i-- )
496 put(' ');
497 }
498 else if ( c == 'c' )
499 {
500 put(va_arg(ap, int));
501 }
502 else
503 {
504 put(*fmt);
505 }
506 }
507 }
509 static void putchar(char c)
510 {
511 outb(0xe9, c);
512 }
514 int printf(const char *fmt, ...)
515 {
516 va_list ap;
518 va_start(ap, fmt);
519 _doprint(putchar, fmt, ap);
520 va_end(ap);
522 return 0;
523 }
525 int vprintf(const char *fmt, va_list ap)
526 {
527 _doprint(putchar, fmt, ap);
528 return 0;
529 }
531 void __assert_failed(char *assertion, char *file, int line)
532 {
533 printf("HVMLoader assertion '%s' failed at %s:%d\n",
534 assertion, file, line);
535 for ( ; ; )
536 asm volatile ( "ud2" );
537 }
539 void __bug(char *file, int line)
540 {
541 printf("HVMLoader bug at %s:%d\n", file, line);
542 for ( ; ; )
543 asm volatile ( "ud2" );
544 }
546 static int validate_hvm_info(struct hvm_info_table *t)
547 {
548 char signature[] = "HVM INFO";
549 uint8_t *ptr = (uint8_t *)t;
550 uint8_t sum = 0;
551 int i;
553 /* strncmp(t->signature, "HVM INFO", 8) */
554 for ( i = 0; i < 8; i++ )
555 {
556 if ( signature[i] != t->signature[i] )
557 {
558 printf("Bad hvm info signature\n");
559 return 0;
560 }
561 }
563 for ( i = 0; i < t->length; i++ )
564 sum += ptr[i];
566 return (sum == 0);
567 }
569 static struct hvm_info_table *get_hvm_info_table(void)
570 {
571 static struct hvm_info_table *table;
572 struct hvm_info_table *t;
574 if ( table != NULL )
575 return table;
577 t = (struct hvm_info_table *)HVM_INFO_PADDR;
579 if ( !validate_hvm_info(t) )
580 {
581 printf("Bad hvm info table\n");
582 return NULL;
583 }
585 table = t;
587 return table;
588 }
590 int get_vcpu_nr(void)
591 {
592 struct hvm_info_table *t = get_hvm_info_table();
593 return (t ? t->nr_vcpus : 1);
594 }
596 int get_acpi_enabled(void)
597 {
598 struct hvm_info_table *t = get_hvm_info_table();
599 return (t ? t->acpi_enabled : 1);
600 }
602 int get_apic_mode(void)
603 {
604 struct hvm_info_table *t = get_hvm_info_table();
605 return (t ? t->apic_mode : 1);
606 }
608 uint16_t get_cpu_mhz(void)
609 {
610 struct xen_add_to_physmap xatp;
611 struct shared_info *shared_info = (struct shared_info *)0xfffff000;
612 struct vcpu_time_info *info = &shared_info->vcpu_info[0].time;
613 uint64_t cpu_khz;
614 uint32_t tsc_to_nsec_mul, version;
615 int8_t tsc_shift;
617 static uint16_t cpu_mhz;
618 if ( cpu_mhz != 0 )
619 return cpu_mhz;
621 /* Map shared-info page. */
622 xatp.domid = DOMID_SELF;
623 xatp.space = XENMAPSPACE_shared_info;
624 xatp.idx = 0;
625 xatp.gpfn = (unsigned long)shared_info >> 12;
626 if ( hypercall_memory_op(XENMEM_add_to_physmap, &xatp) != 0 )
627 BUG();
629 /* Get a consistent snapshot of scale factor (multiplier and shift). */
630 do {
631 version = info->version;
632 rmb();
633 tsc_to_nsec_mul = info->tsc_to_system_mul;
634 tsc_shift = info->tsc_shift;
635 rmb();
636 } while ((version & 1) | (version ^ info->version));
638 /* Compute CPU speed in kHz. */
639 cpu_khz = 1000000ull << 32;
640 do_div(cpu_khz, tsc_to_nsec_mul);
641 if ( tsc_shift < 0 )
642 cpu_khz = cpu_khz << -tsc_shift;
643 else
644 cpu_khz = cpu_khz >> tsc_shift;
646 cpu_mhz = (uint16_t)(((uint32_t)cpu_khz + 500) / 1000);
647 return cpu_mhz;
648 }
650 /*
651 * Local variables:
652 * mode: C
653 * c-set-style: "BSD"
654 * c-basic-offset: 4
655 * tab-width: 4
656 * indent-tabs-mode: nil
657 * End:
658 */