bios/vmxassist/util.c

   1 /*
   2  * util.c: Commonly used utility functions.
   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
  22 #include "util.h"
  23 #include "machine.h"
  24
  25 #define isdigit(c)      ((c) >= '0' && (c) <= '9')
  26 #define min(a, b)       ((a) < (b) ? (a) : (b))
  27
  28 static void putchar(int);
  29 static char *printnum(char *, unsigned long, int);
  30 static void _doprint(void (*)(int), const char *, va_list);
  31
  32 void
  33 cpuid_addr_value(uint64_t addr, uint64_t *value)
  34 {
  35         uint32_t addr_low   = (uint32_t)addr;
  36         uint32_t addr_high  = (uint32_t)(addr >> 32);
  37         uint32_t value_low, value_high;
  38         static unsigned int addr_leaf;
  39
  40         if (!addr_leaf) {
  41                 unsigned int eax, ebx, ecx, edx;
  42                 __asm__ __volatile__(
  43                         "cpuid"
  44                         : "=a" (eax), "=b" (ebx), "=c" (ecx), "=d" (edx)
  45                         : "0" (0x40000000));
  46                 addr_leaf = eax + 1;
  47         }
  48
  49         __asm__ __volatile__(
  50                 "cpuid"
  51                 : "=c" (value_low), "=d" (value_high)
  52                 : "a" (addr_leaf), "0" (addr_low), "1" (addr_high)
  53                 : "ebx");
  54
  55         *value = (uint64_t)value_high << 32 | value_low;
  56 }
  57
  58 void
  59 dump_regs(struct regs *regs)
  60 {
  61         printf("eax    %8x ecx    %8x edx    %8x ebx    %8x\n",
  62                 regs->eax, regs->ecx, regs->edx, regs->ebx);
  63         printf("esp    %8x ebp    %8x esi    %8x edi    %8x\n",
  64                 regs->esp, regs->ebp, regs->esi, regs->edi);
  65         printf("trapno %8x errno  %8x\n", regs->trapno, regs->errno);
  66         printf("eip    %8x cs     %8x eflags %8x\n",
  67                 regs->eip, regs->cs, regs->eflags);
  68         printf("uesp   %8x uss    %8x\n",
  69                 regs->uesp, regs->uss);
  70         printf("ves    %8x vds    %8x vfs    %8x vgs    %8x\n",
  71                 regs->ves, regs->vds, regs->vfs, regs->vgs);
  72
  73         printf("cr0    %8lx cr2    %8x cr3    %8lx cr4    %8lx\n\n",
  74                 (long)oldctx.cr0, get_cr2(),
  75                 (long)oldctx.cr3, (long)oldctx.cr4);
  76 }
  77
  78 #ifdef DEBUG
  79 void
  80 hexdump(unsigned char *data, int sz)
  81 {
  82         unsigned char *d;
  83         int i;
  84
  85         for (d = data; sz > 0; d += 16, sz -= 16) {
  86                 int n = sz > 16 ? 16 : sz;
  87
  88                 printf("%08x: ", (unsigned)d);
  89                 for (i = 0; i < n; i++)
  90                         printf("%02x%c", d[i], i == 7 ? '-' : ' ');
  91                 for (; i < 16; i++)
  92                         printf("  %c", i == 7 ? '-' : ' ');
  93                 printf("   ");
  94                 for (i = 0; i < n; i++)
  95                         printf("%c", d[i] >= ' ' && d[i] <= '~' ? d[i] : '.');
  96                 printf("\n");
  97         }
  98 }
  99
 100 void
 101 print_e820_map(struct e820entry *map, int entries)
 102 {
 103         struct e820entry *m;
 104
 105         if (entries > 32)
 106                 entries = 32;
 107
 108         for (m = map; m < &map[entries]; m++) {
 109                 printf("%08lx%08lx - %08lx%08lx ",
 110                         (unsigned long) (m->addr >> 32),
 111                         (unsigned long) (m->addr),
 112                         (unsigned long) ((m->addr+m->size) >> 32),
 113                         (unsigned long) ((m->addr+m->size)));
 114
 115                 switch (m->type) {
 116                 case E820_RAM:
 117                         printf("(RAM)\n"); break;
 118                 case E820_RESERVED:
 119                         printf("(Reserved)\n"); break;
 120                 case E820_ACPI:
 121                         printf("(ACPI Data)\n"); break;
 122                 case E820_NVS:
 123                         printf("(ACPI NVS)\n"); break;
 124                 default:
 125                         printf("(Type %ld)\n", m->type); break;
 126                 }
 127         }
 128 }
 129
 130 void
 131 dump_dtr(unsigned long addr, unsigned long size)
 132 {
 133         unsigned long long entry;
 134         unsigned long base, limit;
 135         int i;
 136
 137         for (i = 0; i < size; i += 8) {
 138                 entry = ((unsigned long long *) addr)[i >> 3];
 139                 base = (((entry >> (56-24)) & 0xFF000000) |
 140                         ((entry >> (32-16)) & 0x00FF0000) |
 141                         ((entry >> (   16)) & 0x0000FFFF));
 142                 limit = (((entry >> (48-16)) & 0x000F0000) |
 143                          ((entry           ) & 0x0000FFFF));
 144                 if (entry & (1ULL << (23+32))) /* G */
 145                         limit = (limit << 12) | 0xFFF;
 146
 147                 printf("[0x%x] = 0x%08x%08x, base 0x%lx, limit 0x%lx\n", i,
 148                         (unsigned)(entry >> 32), (unsigned)(entry),
 149                         base, limit);
 150         }
 151 }
 152
 153 void
 154 dump_vmx_context(struct vmx_assist_context *c)
 155 {
 156         printf("eip 0x%lx, esp 0x%lx, eflags 0x%lx\n",
 157                 (long) c->eip, (long) c->esp, (long) c->eflags);
 158
 159         printf("cr0 0x%lx, cr3 0x%lx, cr4 0x%lx\n",
 160                 (long)c->cr0, (long)c->cr3, (long)c->cr4);
 161
 162         printf("idtr: limit 0x%lx, base 0x%lx\n",
 163                 (long)c->idtr_limit, (long)c->idtr_base);
 164
 165         printf("gdtr: limit 0x%lx, base 0x%lx\n",
 166                 (long)c->gdtr_limit, (long)c->gdtr_base);
 167
 168         printf("cs: sel 0x%lx, limit 0x%lx, base 0x%lx\n",
 169                 (long)c->cs_sel, (long)c->cs_limit, (long)c->cs_base);
 170         printf("\ttype %d, s %d, dpl %d, p %d, avl %d, ops %d, g %d, nul %d\n",
 171                 c->cs_arbytes.fields.seg_type,
 172                 c->cs_arbytes.fields.s,
 173                 c->cs_arbytes.fields.dpl,
 174                 c->cs_arbytes.fields.p,
 175                 c->cs_arbytes.fields.avl,
 176                 c->cs_arbytes.fields.default_ops_size,
 177                 c->cs_arbytes.fields.g,
 178                 c->cs_arbytes.fields.null_bit);
 179
 180         printf("ds: sel 0x%lx, limit 0x%lx, base 0x%lx\n",
 181                 (long)c->ds_sel, (long)c->ds_limit, (long)c->ds_base);
 182         printf("\ttype %d, s %d, dpl %d, p %d, avl %d, ops %d, g %d, nul %d\n",
 183                 c->ds_arbytes.fields.seg_type,
 184                 c->ds_arbytes.fields.s,
 185                 c->ds_arbytes.fields.dpl,
 186                 c->ds_arbytes.fields.p,
 187                 c->ds_arbytes.fields.avl,
 188                 c->ds_arbytes.fields.default_ops_size,
 189                 c->ds_arbytes.fields.g,
 190                 c->ds_arbytes.fields.null_bit);
 191
 192         printf("es: sel 0x%lx, limit 0x%lx, base 0x%lx\n",
 193                 (long)c->es_sel, (long)c->es_limit, (long)c->es_base);
 194         printf("\ttype %d, s %d, dpl %d, p %d, avl %d, ops %d, g %d, nul %d\n",
 195                 c->es_arbytes.fields.seg_type,
 196                 c->es_arbytes.fields.s,
 197                 c->es_arbytes.fields.dpl,
 198                 c->es_arbytes.fields.p,
 199                 c->es_arbytes.fields.avl,
 200                 c->es_arbytes.fields.default_ops_size,
 201                 c->es_arbytes.fields.g,
 202                 c->es_arbytes.fields.null_bit);
 203
 204         printf("ss: sel 0x%lx, limit 0x%lx, base 0x%lx\n",
 205                 (long)c->ss_sel, (long)c->ss_limit, (long)c->ss_base);
 206         printf("\ttype %d, s %d, dpl %d, p %d, avl %d, ops %d, g %d, nul %d\n",
 207                 c->ss_arbytes.fields.seg_type,
 208                 c->ss_arbytes.fields.s,
 209                 c->ss_arbytes.fields.dpl,
 210                 c->ss_arbytes.fields.p,
 211                 c->ss_arbytes.fields.avl,
 212                 c->ss_arbytes.fields.default_ops_size,
 213                 c->ss_arbytes.fields.g,
 214                 c->ss_arbytes.fields.null_bit);
 215
 216         printf("fs: sel 0x%lx, limit 0x%lx, base 0x%lx\n",
 217                 (long)c->fs_sel, (long)c->fs_limit, (long)c->fs_base);
 218         printf("\ttype %d, s %d, dpl %d, p %d, avl %d, ops %d, g %d, nul %d\n",
 219                 c->fs_arbytes.fields.seg_type,
 220                 c->fs_arbytes.fields.s,
 221                 c->fs_arbytes.fields.dpl,
 222                 c->fs_arbytes.fields.p,
 223                 c->fs_arbytes.fields.avl,
 224                 c->fs_arbytes.fields.default_ops_size,
 225                 c->fs_arbytes.fields.g,
 226                 c->fs_arbytes.fields.null_bit);
 227
 228         printf("gs: sel 0x%lx, limit 0x%lx, base 0x%lx\n",
 229                 (long)c->gs_sel, (long)c->gs_limit, (long)c->gs_base);
 230         printf("\ttype %d, s %d, dpl %d, p %d, avl %d, ops %d, g %d, nul %d\n",
 231                 c->gs_arbytes.fields.seg_type,
 232                 c->gs_arbytes.fields.s,
 233                 c->gs_arbytes.fields.dpl,
 234                 c->gs_arbytes.fields.p,
 235                 c->gs_arbytes.fields.avl,
 236                 c->gs_arbytes.fields.default_ops_size,
 237                 c->gs_arbytes.fields.g,
 238                 c->gs_arbytes.fields.null_bit);
 239
 240         printf("tr: sel 0x%lx, limit 0x%lx, base 0x%lx\n",
 241                 (long)c->tr_sel, (long)c->tr_limit, (long)c->tr_base);
 242         printf("\ttype %d, s %d, dpl %d, p %d, avl %d, ops %d, g %d, nul %d\n",
 243                 c->tr_arbytes.fields.seg_type,
 244                 c->tr_arbytes.fields.s,
 245                 c->tr_arbytes.fields.dpl,
 246                 c->tr_arbytes.fields.p,
 247                 c->tr_arbytes.fields.avl,
 248                 c->tr_arbytes.fields.default_ops_size,
 249                 c->tr_arbytes.fields.g,
 250                 c->tr_arbytes.fields.null_bit);
 251
 252         printf("ldtr: sel 0x%lx, limit 0x%lx, base 0x%lx\n",
 253                 (long)c->ldtr_sel, (long)c->ldtr_limit, (long)c->ldtr_base);
 254         printf("\ttype %d, s %d, dpl %d, p %d, avl %d, ops %d, g %d, nul %d\n",
 255                 c->ldtr_arbytes.fields.seg_type,
 256                 c->ldtr_arbytes.fields.s,
 257                 c->ldtr_arbytes.fields.dpl,
 258                 c->ldtr_arbytes.fields.p,
 259                 c->ldtr_arbytes.fields.avl,
 260                 c->ldtr_arbytes.fields.default_ops_size,
 261                 c->ldtr_arbytes.fields.g,
 262                 c->ldtr_arbytes.fields.null_bit);
 263
 264         printf("GDTR <0x%lx,0x%lx>:\n",
 265                 (long)c->gdtr_base, (long)c->gdtr_limit);
 266         dump_dtr(c->gdtr_base, c->gdtr_limit);
 267 }
 268 #endif /* DEBUG */
 269
 270 /*
 271  * Lightweight printf that doesn't drag in everything under the sun.
 272  */
 273 int
 274 printf(const char *fmt, ...)
 275 {
 276         va_list ap;
 277
 278         va_start(ap, fmt);
 279         _doprint(putchar, fmt, ap);
 280         va_end(ap);
 281         return 0; /* for gcc compat */
 282 }
 283
 284 int
 285 vprintf(const char *fmt, va_list ap)
 286 {
 287         _doprint(putchar, fmt, ap);
 288         return 0; /* for gcc compat */
 289 }
 290
 291 void
 292 panic(const char *fmt, ...)
 293 {
 294         va_list ap;
 295
 296         va_start(ap, fmt);
 297         _doprint(putchar, fmt, ap);
 298         putchar('\n');
 299         va_end(ap);
 300         halt();
 301 }
 302
 303 unsigned
 304 strlen(const char *s)
 305 {
 306         const char *q = s;
 307
 308         while (*s++)
 309                 /* void */;
 310         return s - q - 1;
 311 }
 312
 313 static void
 314 putchar(int ch)
 315 {
 316         outb(0xE9, ch);
 317 }
 318
 319 /*
 320  * A stripped down version of doprint,
 321  * but still powerful enough for most tasks.
 322  */
 323 static void
 324 _doprint(void (*put)(int), const char *fmt, va_list ap)
 325 {
 326         register char *str, c;
 327         int lflag, zflag, nflag;
 328         char buffer[17];
 329         unsigned value;
 330         int i, slen, pad;
 331
 332         for ( ; *fmt != '\0'; fmt++) {
 333                 pad = zflag = nflag = lflag = 0;
 334                 if (*fmt == '%') {
 335                         c = *++fmt;
 336                         if (c == '-' || isdigit(c)) {
 337                                 if (c == '-') {
 338                                         nflag = 1;
 339                                         c = *++fmt;
 340                                 }
 341                                 zflag = c == '0';
 342                                 for (pad = 0; isdigit(c); c = *++fmt)
 343                                         pad = (pad * 10) + c - '0';
 344                         }
 345                         if (c == 'l') { /* long extension */
 346                                 lflag = 1;
 347                                 c = *++fmt;
 348                         }
 349                         if (c == 'd' || c == 'u' || c == 'o' || c == 'x') {
 350                                 if (lflag)
 351                                         value = va_arg(ap, unsigned);
 352                                 else
 353                                         value = (unsigned) va_arg(ap, unsigned int);
 354                                 str = buffer;
 355                                 printnum(str, value,
 356                                         c == 'o' ? 8 : (c == 'x' ? 16 : 10));
 357                                 goto printn;
 358                         } else if (c == 'O' || c == 'D' || c == 'X') {
 359                                 value = va_arg(ap, unsigned);
 360                                 str = buffer;
 361                                 printnum(str, value,
 362                                         c == 'O' ? 8 : (c == 'X' ? 16 : 10));
 363                         printn:
 364                                 slen = strlen(str);
 365                                 for (i = pad - slen; i > 0; i--)
 366                                         put(zflag ? '0' : ' ');
 367                                 while (*str) put(*str++);
 368                         } else if (c == 's') {
 369                                 str = va_arg(ap, char *);
 370                                 slen = strlen(str);
 371                                 if (nflag == 0)
 372                                         for (i = pad - slen; i > 0; i--) put(' ');
 373                                 while (*str) put(*str++);
 374                                 if (nflag)
 375                                         for (i = pad - slen; i > 0; i--) put(' ');
 376                         } else if (c == 'c')
 377                                 put(va_arg(ap, int));
 378                         else
 379                                 put(*fmt);
 380                 } else
 381                         put(*fmt);
 382         }
 383 }
 384
 385 static char *
 386 printnum(char *p, unsigned long num, int base)
 387 {
 388         unsigned long n;
 389
 390         if ((n = num/base) > 0)
 391                 p = printnum(p, n, base);
 392         *p++ = "0123456789ABCDEF"[(int)(num % base)];
 393         *p = '\0';
 394         return p;
 395 }
 396
 397 void *
 398 memset(void *s, int c, unsigned n)
 399 {
 400         int t0, t1;
 401
 402         __asm__ __volatile__ ("cld; rep; stosb"
 403                 : "=&c" (t0), "=&D" (t1)
 404                 : "a" (c), "1" (s), "0" (n)
 405                 : "memory");
 406         return s;
 407 }
 408
 409 void *
 410 memcpy(void *dest, const void *src, unsigned n)
 411 {
 412         int t0, t1, t2;
 413
 414         __asm__ __volatile__(
 415                 "cld\n"
 416                 "rep; movsl\n"
 417                 "testb $2,%b4\n"
 418                 "je 1f\n"
 419                 "movsw\n"
 420                 "1: testb $1,%b4\n"
 421                 "je 2f\n"
 422                 "movsb\n"
 423                 "2:"
 424                 : "=&c" (t0), "=&D" (t1), "=&S" (t2)
 425                 : "0" (n/4), "q" (n), "1" ((long) dest), "2" ((long) src)
 426                 : "memory"
 427         );
 428         return dest;
 429 }
 430