src/util.c

   1 // Misc utility functions.
   2 //
   3 // Copyright (C) 2008,2009  Kevin O'Connor <kevin@koconnor.net>
   4 //
   5 // This file may be distributed under the terms of the GNU LGPLv3 license.
   6
   7 #include "util.h" // call16
   8 #include "bregs.h" // struct bregs
   9 #include "farptr.h" // GET_FLATPTR
  10 #include "biosvar.h" // get_ebda_seg
  11
  12 static inline u32 getesp() {
  13     u32 esp;
  14     asm("movl %%esp, %0" : "=rm"(esp));
  15     return esp;
  16 }
  17
  18
  19 /****************************************************************
  20  * 16bit calls
  21  ****************************************************************/
  22
  23 // Call a function with a specified register state.  Note that on
  24 // return, the interrupt enable/disable flag may be altered.
  25 inline void
  26 call16(struct bregs *callregs)
  27 {
  28     if (!MODE16 && getesp() > BUILD_STACK_ADDR)
  29         panic("call16 with invalid stack\n");
  30     asm volatile(
  31 #if MODE16 == 1
  32         "calll __call16\n"
  33         "cli\n"
  34         "cld"
  35 #else
  36         "calll __call16_from32"
  37 #endif
  38         : "+a" (callregs), "+m" (*callregs)
  39         :
  40         : "ebx", "ecx", "edx", "esi", "edi", "cc", "memory");
  41 }
  42
  43 inline void
  44 call16big(struct bregs *callregs)
  45 {
  46     ASSERT32();
  47     if (getesp() > BUILD_STACK_ADDR)
  48         panic("call16 with invalid stack\n");
  49     asm volatile(
  50         "calll __call16big_from32"
  51         : "+a" (callregs), "+m" (*callregs)
  52         :
  53         : "ebx", "ecx", "edx", "esi", "edi", "cc", "memory");
  54 }
  55
  56 inline void
  57 __call16_int(struct bregs *callregs, u16 offset)
  58 {
  59     if (MODE16)
  60         callregs->code.seg = GET_SEG(CS);
  61     else
  62         callregs->code.seg = SEG_BIOS;
  63     callregs->code.offset = offset;
  64     call16(callregs);
  65 }
  66
  67 // Switch to the extra stack in ebda and call a function.
  68 inline u32
  69 stack_hop(u32 eax, u32 edx, u32 ecx, void *func)
  70 {
  71     ASSERT16();
  72     u16 ebda_seg = get_ebda_seg(), bkup_ss;
  73     u32 bkup_esp;
  74     asm volatile(
  75         // Backup current %ss/%esp values.
  76         "movw %%ss, %w3\n"
  77         "movl %%esp, %4\n"
  78         // Copy ebda seg to %ds/%ss and set %esp
  79         "movw %w6, %%ds\n"
  80         "movw %w6, %%ss\n"
  81         "movl %5, %%esp\n"
  82         // Call func
  83         "calll %7\n"
  84         // Restore segments and stack
  85         "movw %w3, %%ds\n"
  86         "movw %w3, %%ss\n"
  87         "movl %4, %%esp"
  88         : "+a" (eax), "+d" (edx), "+c" (ecx), "=&r" (bkup_ss), "=&r" (bkup_esp)
  89         : "i" (EBDA_OFFSET_TOP_STACK), "r" (ebda_seg), "m" (*(u8*)func)
  90         : "cc", "memory");
  91     return eax;
  92 }
  93
  94 // 16bit trampoline for enabling irqs from 32bit mode.
  95 ASM16(
  96     "  .global trampoline_checkirqs\n"
  97     "trampoline_checkirqs:\n"
  98     "  rep ; nop\n"
  99     "  lretw"
 100     );
 101
 102 static void
 103 check_irqs32()
 104 {
 105     extern void trampoline_checkirqs();
 106     struct bregs br;
 107     br.flags = F_IF;
 108     br.code.seg = SEG_BIOS;
 109     br.code.offset = (u32)&trampoline_checkirqs;
 110     call16big(&br);
 111 }
 112
 113 static void
 114 check_irqs16()
 115 {
 116     asm volatile(
 117         "sti\n"
 118         "nop\n"
 119         "rep ; nop\n"
 120         "cli\n"
 121         "cld\n"
 122         : : :"memory");
 123 }
 124
 125
 126 /****************************************************************
 127  * Threads
 128  ****************************************************************/
 129
 130 #define THREADSTACKSIZE 4096
 131
 132 struct thread_info {
 133     struct thread_info *next;
 134     void *stackpos;
 135 };
 136
 137 static struct thread_info MainThread = {&MainThread, NULL};
 138
 139 static struct thread_info *
 140 getCurThread()
 141 {
 142     u32 esp = getesp();
 143     if (esp <= BUILD_STACK_ADDR)
 144         return &MainThread;
 145     return (void*)ALIGN_DOWN(esp, THREADSTACKSIZE);
 146 }
 147
 148 // Briefly permit irqs to occur.
 149 void
 150 yield()
 151 {
 152     if (MODE16) {
 153         // In 16bit mode, just directly check irqs.
 154         check_irqs16();
 155         return;
 156     }
 157     if (! CONFIG_THREADS) {
 158         check_irqs32();
 159         return;
 160     }
 161     struct thread_info *cur = getCurThread();
 162     if (cur == &MainThread)
 163         // Permit irqs to fire
 164         check_irqs32();
 165
 166     // Switch to the next thread
 167     struct thread_info *next = cur->next;
 168     asm volatile(
 169         "  pushl $1f\n"                 // store return pc
 170         "  pushl %%ebp\n"               // backup %ebp
 171         "  movl %%esp, 4(%%eax)\n"      // cur->stackpos = %esp
 172         "  movl 4(%%ecx), %%esp\n"      // %esp = next->stackpos
 173         "  popl %%ebp\n"                // restore %ebp
 174         "  retl\n"                      // restore pc
 175         "1:\n"
 176         : "+a"(cur), "+c"(next)
 177         :
 178         : "ebx", "edx", "esi", "edi", "cc", "memory");
 179 }
 180
 181 // Last thing called from a thread (called on "next" stack).
 182 static void
 183 __end_thread(struct thread_info *old)
 184 {
 185     struct thread_info *pos = &MainThread;
 186     while (pos->next != old)
 187         pos = pos->next;
 188     pos->next = old->next;
 189     free(old);
 190     dprintf(2, "=========== end thread %p\n", old);
 191 }
 192
 193 void
 194 run_thread(void (*func)(void*), void *data)
 195 {
 196     ASSERT32();
 197     if (! CONFIG_THREADS)
 198         goto fail;
 199     struct thread_info *thread;
 200     thread = memalign_tmphigh(THREADSTACKSIZE, THREADSTACKSIZE);
 201     if (!thread)
 202         goto fail;
 203
 204     thread->stackpos = (void*)thread + THREADSTACKSIZE;
 205     struct thread_info *cur = getCurThread();
 206     thread->next = cur->next;
 207     cur->next = thread;
 208
 209     dprintf(2, "=========== start thread %p\n", thread);
 210     asm volatile(
 211         // Start thread
 212         "  pushl $1f\n"                 // store return pc
 213         "  pushl %%ebp\n"               // backup %ebp
 214         "  movl %%esp, 4(%%edx)\n"      // cur->stackpos = %esp
 215         "  movl 4(%%ebx), %%esp\n"      // %esp = thread->stackpos
 216         "  calll *%%ecx\n"              // Call func
 217
 218         // End thread
 219         "  movl (%%ebx), %%ecx\n"       // %ecx = thread->next
 220         "  movl 4(%%ecx), %%esp\n"      // %esp = next->stackpos
 221         "  movl %%ebx, %%eax\n"
 222         "  calll %4\n"                  // call __end_thread(thread)
 223         "  popl %%ebp\n"                // restore %ebp
 224         "  retl\n"                      // restore pc
 225         "1:\n"
 226         : "+a"(data), "+c"(func), "+b"(thread), "+d"(cur)
 227         : "m"(*(u8*)__end_thread)
 228         : "esi", "edi", "cc", "memory");
 229     return;
 230
 231 fail:
 232     func(data);
 233 }
 234
 235 void
 236 wait_threads()
 237 {
 238     ASSERT32();
 239     if (! CONFIG_THREADS)
 240         return;
 241     while (MainThread.next != &MainThread)
 242         yield();
 243 }
 244
 245
 246 /****************************************************************
 247  * String ops
 248  ****************************************************************/
 249
 250 // Sum the bytes in the specified area.
 251 u8
 252 checksum_far(u16 buf_seg, void *buf_far, u32 len)
 253 {
 254     SET_SEG(ES, buf_seg);
 255     u32 i;
 256     u8 sum = 0;
 257     for (i=0; i<len; i++)
 258         sum += GET_VAR(ES, ((u8*)buf_far)[i]);
 259     return sum;
 260 }
 261
 262 u8
 263 checksum(void *buf, u32 len)
 264 {
 265     return checksum_far(GET_SEG(SS), buf, len);
 266 }
 267
 268 size_t
 269 strlen(const char *s)
 270 {
 271     if (__builtin_constant_p(s))
 272         return __builtin_strlen(s);
 273     const char *p = s;
 274     while (*p)
 275         p++;
 276     return p-s;
 277 }
 278
 279 // Compare two areas of memory.
 280 int
 281 memcmp(const void *s1, const void *s2, size_t n)
 282 {
 283     while (n) {
 284         if (*(u8*)s1 != *(u8*)s2)
 285             return *(u8*)s1 < *(u8*)s2 ? -1 : 1;
 286         s1++;
 287         s2++;
 288         n--;
 289     }
 290     return 0;
 291 }
 292
 293 // Compare two strings.
 294 int
 295 strcmp(const char *s1, const char *s2)
 296 {
 297     for (;;) {
 298         if (*s1 != *s2)
 299             return *s1 < *s2 ? -1 : 1;
 300         if (! *s1)
 301             return 0;
 302         s1++;
 303         s2++;
 304     }
 305 }
 306
 307 inline void
 308 memset_far(u16 d_seg, void *d_far, u8 c, size_t len)
 309 {
 310     SET_SEG(ES, d_seg);
 311     asm volatile(
 312         "rep stosb %%es:(%%di)"
 313         : "+c"(len), "+D"(d_far)
 314         : "a"(c)
 315         : "cc", "memory");
 316 }
 317
 318 inline void
 319 memset16_far(u16 d_seg, void *d_far, u16 c, size_t len)
 320 {
 321     len /= 2;
 322     SET_SEG(ES, d_seg);
 323     asm volatile(
 324         "rep stosw %%es:(%%di)"
 325         : "+c"(len), "+D"(d_far)
 326         : "a"(c)
 327         : "cc", "memory");
 328 }
 329
 330 void *
 331 memset(void *s, int c, size_t n)
 332 {
 333     while (n)
 334         ((char *)s)[--n] = c;
 335     return s;
 336 }
 337
 338 inline void
 339 memcpy_far(u16 d_seg, void *d_far, u16 s_seg, const void *s_far, size_t len)
 340 {
 341     SET_SEG(ES, d_seg);
 342     u16 bkup_ds;
 343     asm volatile(
 344         "movw %%ds, %w0\n"
 345         "movw %w4, %%ds\n"
 346         "rep movsb (%%si),%%es:(%%di)\n"
 347         "movw %w0, %%ds"
 348         : "=&r"(bkup_ds), "+c"(len), "+S"(s_far), "+D"(d_far)
 349         : "r"(s_seg)
 350         : "cc", "memory");
 351 }
 352
 353 void *
 354 #undef memcpy
 355 memcpy(void *d1, const void *s1, size_t len)
 356 #if MODE16 == 0
 357 #define memcpy __builtin_memcpy
 358 #endif
 359 {
 360     SET_SEG(ES, GET_SEG(SS));
 361     void *d = d1;
 362     if (((u32)d1 | (u32)s1 | len) & 3) {
 363         // non-aligned memcpy
 364         asm volatile(
 365             "rep movsb (%%esi),%%es:(%%edi)"
 366             : "+c"(len), "+S"(s1), "+D"(d)
 367             : : "cc", "memory");
 368         return d1;
 369     }
 370     // Common case - use 4-byte copy
 371     len /= 4;
 372     asm volatile(
 373         "rep movsl (%%esi),%%es:(%%edi)"
 374         : "+c"(len), "+S"(s1), "+D"(d)
 375         : : "cc", "memory");
 376     return d1;
 377 }
 378
 379 void *
 380 memmove(void *d, const void *s, size_t len)
 381 {
 382     if (s >= d)
 383         return memcpy(d, s, len);
 384
 385     d += len-1;
 386     s += len-1;
 387     while (len--) {
 388         *(char*)d = *(char*)s;
 389         d--;
 390         s--;
 391     }
 392
 393     return d;
 394 }
 395
 396 // Copy a string - truncating it if necessary.
 397 char *
 398 strtcpy(char *dest, const char *src, size_t len)
 399 {
 400     char *d = dest;
 401     while (len-- && *src != '\0')
 402         *d++ = *src++;
 403     *d = '\0';
 404     return dest;
 405 }
 406
 407
 408 /****************************************************************
 409  * Keyboard calls
 410  ****************************************************************/
 411
 412 // Wait for 'usec' microseconds using (with irqs enabled) using int 1586.
 413 void
 414 biosusleep(u32 usec)
 415 {
 416     struct bregs br;
 417     memset(&br, 0, sizeof(br));
 418     br.flags = F_IF;
 419     br.ah = 0x86;
 420     br.cx = usec >> 16;
 421     br.dx = usec;
 422     call16_int(0x15, &br);
 423 }
 424
 425 // See if a keystroke is pending in the keyboard buffer.
 426 static int
 427 check_for_keystroke()
 428 {
 429     struct bregs br;
 430     memset(&br, 0, sizeof(br));
 431     br.flags = F_IF;
 432     br.ah = 1;
 433     call16_int(0x16, &br);
 434     return !(br.flags & F_ZF);
 435 }
 436
 437 // Return a keystroke - waiting forever if necessary.
 438 static int
 439 get_raw_keystroke()
 440 {
 441     struct bregs br;
 442     memset(&br, 0, sizeof(br));
 443     br.flags = F_IF;
 444     call16_int(0x16, &br);
 445     return br.ah;
 446 }
 447
 448 // Read a keystroke - waiting up to 'msec' milliseconds.
 449 int
 450 get_keystroke(int msec)
 451 {
 452     for (;;) {
 453         if (check_for_keystroke())
 454             return get_raw_keystroke();
 455         if (msec <= 0)
 456             return -1;
 457         biosusleep(50*1000);
 458         msec -= 50;
 459     }
 460 }