qemu/roms/seabios/src/stacks.c

   1 // Code for manipulating stack locations.
   2 //
   3 // Copyright (C) 2009-2014  Kevin O'Connor <kevin@koconnor.net>
   4 //
   5 // This file may be distributed under the terms of the GNU LGPLv3 license.
   6
   7 #include "biosvar.h" // GET_GLOBAL
   8 #include "bregs.h" // CR0_PE
   9 #include "fw/paravirt.h" // PORT_SMI_CMD
  10 #include "hw/rtc.h" // rtc_use
  11 #include "list.h" // hlist_node
  12 #include "malloc.h" // free
  13 #include "output.h" // dprintf
  14 #include "romfile.h" // romfile_loadint
  15 #include "stacks.h" // struct mutex_s
  16 #include "util.h" // useRTC
  17
  18 #define MAIN_STACK_MAX (1024*1024)
  19
  20
  21 /****************************************************************
  22  * 16bit / 32bit calling
  23  ****************************************************************/
  24
  25 struct {
  26     u8 method;
  27     u8 cmosindex;
  28     u8 a20;
  29     u16 ss, fs, gs;
  30     struct descloc_s gdt;
  31 } Call32Data VARLOW;
  32
  33 #define C32_SLOPPY 1
  34 #define C32_SMM    2
  35
  36 int HaveSmmCall32 VARFSEG;
  37
  38 // Backup state in preparation for call32_smm()
  39 static void
  40 call32_smm_prep(void)
  41 {
  42     // Backup cmos index register and disable nmi
  43     u8 cmosindex = inb(PORT_CMOS_INDEX);
  44     outb(cmosindex | NMI_DISABLE_BIT, PORT_CMOS_INDEX);
  45     inb(PORT_CMOS_DATA);
  46     SET_LOW(Call32Data.cmosindex, cmosindex);
  47
  48     // Backup ss
  49     SET_LOW(Call32Data.ss, GET_SEG(SS));
  50
  51     SET_LOW(Call32Data.method, C32_SMM);
  52 }
  53
  54 // Restore state backed up during call32_smm()
  55 static void
  56 call32_smm_post(void)
  57 {
  58     SET_LOW(Call32Data.method, 0);
  59     SET_LOW(Call32Data.ss, 0);
  60
  61     // Restore cmos index register
  62     outb(GET_LOW(Call32Data.cmosindex), PORT_CMOS_INDEX);
  63     inb(PORT_CMOS_DATA);
  64 }
  65
  66 #define ASM32_SWITCH16 "  .pushsection .text.32fseg." UNIQSEC "\n  .code16\n"
  67 #define ASM32_BACK32   "  .popsection\n  .code32\n"
  68 #define ASM16_SWITCH32 "  .code32\n"
  69 #define ASM16_BACK16   "  .code16gcc\n"
  70
  71 // Call a SeaBIOS C function in 32bit mode using smm trampoline
  72 static u32
  73 call32_smm(void *func, u32 eax)
  74 {
  75     ASSERT16();
  76     dprintf(9, "call32_smm %p %x\n", func, eax);
  77     call32_smm_prep();
  78     u32 bkup_esp;
  79     asm volatile(
  80         // Backup esp / set esp to flat stack location
  81         "  movl %%esp, %0\n"
  82         "  movl %%ss, %%eax\n"
  83         "  shll $4, %%eax\n"
  84         "  addl %%eax, %%esp\n"
  85
  86         // Transition to 32bit mode, call func, return to 16bit
  87         "  movl $" __stringify(CALL32SMM_CMDID) ", %%eax\n"
  88         "  movl $" __stringify(CALL32SMM_ENTERID) ", %%ecx\n"
  89         "  movl $(" __stringify(BUILD_BIOS_ADDR) " + 1f), %%ebx\n"
  90         "  outb %%al, $" __stringify(PORT_SMI_CMD) "\n"
  91         "  rep; nop\n"
  92         "  hlt\n"
  93
  94         ASM16_SWITCH32
  95         "1:movl %1, %%eax\n"
  96         "  calll *%2\n"
  97         "  movl %%eax, %1\n"
  98
  99         "  movl $" __stringify(CALL32SMM_CMDID) ", %%eax\n"
 100         "  movl $" __stringify(CALL32SMM_RETURNID) ", %%ecx\n"
 101         "  movl $2f, %%ebx\n"
 102         "  outb %%al, $" __stringify(PORT_SMI_CMD) "\n"
 103         "  rep; nop\n"
 104         "  hlt\n"
 105
 106         // Restore esp
 107         ASM16_BACK16
 108         "2:movl %0, %%esp\n"
 109         : "=&r" (bkup_esp), "+r" (eax)
 110         : "r" (func)
 111         : "eax", "ecx", "edx", "ebx", "cc", "memory");
 112     call32_smm_post();
 113
 114     dprintf(9, "call32_smm done %p %x\n", func, eax);
 115     return eax;
 116 }
 117
 118 // 16bit handler code called from call16_smm()
 119 u32 VISIBLE16
 120 call16_smm_helper(u32 eax, u32 edx, u32 (*func)(u32 eax, u32 edx))
 121 {
 122     if (!CONFIG_CALL32_SMM)
 123         return eax;
 124     call32_smm_post();
 125     u32 ret = func(eax, edx);
 126     call32_smm_prep();
 127     return ret;
 128 }
 129
 130 static u32
 131 call16_smm(u32 eax, u32 edx, void *func)
 132 {
 133     ASSERT32FLAT();
 134     if (!CONFIG_CALL32_SMM)
 135         return eax;
 136     func -= BUILD_BIOS_ADDR;
 137     dprintf(9, "call16_smm %p %x %x\n", func, eax, edx);
 138     u32 stackoffset = Call32Data.ss << 4;
 139     asm volatile(
 140         // Restore esp
 141         "  subl %0, %%esp\n"
 142
 143         // Transition to 16bit mode, call func, return to 32bit
 144         "  movl $" __stringify(CALL32SMM_CMDID) ", %%eax\n"
 145         "  movl $" __stringify(CALL32SMM_RETURNID) ", %%ecx\n"
 146         "  movl $(1f - " __stringify(BUILD_BIOS_ADDR) "), %%ebx\n"
 147         "  outb %%al, $" __stringify(PORT_SMI_CMD) "\n"
 148         "  rep; nop\n"
 149         "  hlt\n"
 150
 151         ASM32_SWITCH16
 152         "1:movl %1, %%eax\n"
 153         "  movl %3, %%ecx\n"
 154         "  calll _cfunc16_call16_smm_helper\n"
 155         "  movl %%eax, %1\n"
 156
 157         "  movl $" __stringify(CALL32SMM_CMDID) ", %%eax\n"
 158         "  movl $" __stringify(CALL32SMM_ENTERID) ", %%ecx\n"
 159         "  movl $2f, %%ebx\n"
 160         "  outb %%al, $" __stringify(PORT_SMI_CMD) "\n"
 161         "  rep; nop\n"
 162         "  hlt\n"
 163
 164         // Set esp to flat stack location
 165         ASM32_BACK32
 166         "2:addl %0, %%esp\n"
 167         : "+r" (stackoffset), "+r" (eax), "+d" (edx)
 168         : "r" (func)
 169         : "eax", "ecx", "ebx", "cc", "memory");
 170     return eax;
 171 }
 172
 173 // Backup state in preparation for call32_sloppy()
 174 static void
 175 call32_sloppy_prep(void)
 176 {
 177     // Backup cmos index register and disable nmi
 178     u8 cmosindex = inb(PORT_CMOS_INDEX);
 179     outb(cmosindex | NMI_DISABLE_BIT, PORT_CMOS_INDEX);
 180     inb(PORT_CMOS_DATA);
 181     SET_LOW(Call32Data.cmosindex, cmosindex);
 182
 183     // Enable a20 and backup it's previous state
 184     SET_LOW(Call32Data.a20, set_a20(1));
 185
 186     // Backup ss/fs/gs and gdt
 187     SET_LOW(Call32Data.ss, GET_SEG(SS));
 188     SET_LOW(Call32Data.fs, GET_SEG(FS));
 189     SET_LOW(Call32Data.gs, GET_SEG(GS));
 190     struct descloc_s gdt;
 191     sgdt(&gdt);
 192     SET_LOW(Call32Data.gdt.length, gdt.length);
 193     SET_LOW(Call32Data.gdt.addr, gdt.addr);
 194
 195     SET_LOW(Call32Data.method, C32_SLOPPY);
 196 }
 197
 198 // Restore state backed up during call32_sloppy()
 199 static void
 200 call32_sloppy_post(void)
 201 {
 202     SET_LOW(Call32Data.method, 0);
 203     SET_LOW(Call32Data.ss, 0);
 204
 205     // Restore gdt and fs/gs
 206     struct descloc_s gdt;
 207     gdt.length = GET_LOW(Call32Data.gdt.length);
 208     gdt.addr = GET_LOW(Call32Data.gdt.addr);
 209     lgdt(&gdt);
 210     SET_SEG(FS, GET_LOW(Call32Data.fs));
 211     SET_SEG(GS, GET_LOW(Call32Data.gs));
 212
 213     // Restore a20
 214     set_a20(GET_LOW(Call32Data.a20));
 215
 216     // Restore cmos index register
 217     outb(GET_LOW(Call32Data.cmosindex), PORT_CMOS_INDEX);
 218     inb(PORT_CMOS_DATA);
 219 }
 220
 221 // Call a C function in 32bit mode.  This clobbers the 16bit segment
 222 // selector registers.
 223 static u32
 224 call32_sloppy(void *func, u32 eax)
 225 {
 226     ASSERT16();
 227     call32_sloppy_prep();
 228     u32 bkup_ss, bkup_esp;
 229     asm volatile(
 230         // Backup ss/esp / set esp to flat stack location
 231         "  movl %%ss, %0\n"
 232         "  movl %%esp, %1\n"
 233         "  shll $4, %0\n"
 234         "  addl %0, %%esp\n"
 235         "  shrl $4, %0\n"
 236
 237         // Transition to 32bit mode, call func, return to 16bit
 238         "  movl $(" __stringify(BUILD_BIOS_ADDR) " + 1f), %%edx\n"
 239         "  jmp transition32\n"
 240         ASM16_SWITCH32
 241         "1:calll *%3\n"
 242         "  movl $2f, %%edx\n"
 243         "  jmp transition16big\n"
 244
 245         // Restore ds/ss/esp
 246         ASM16_BACK16
 247         "2:movl %0, %%ds\n"
 248         "  movl %0, %%ss\n"
 249         "  movl %1, %%esp\n"
 250         : "=&r" (bkup_ss), "=&r" (bkup_esp), "+a" (eax)
 251         : "r" (func)
 252         : "ecx", "edx", "cc", "memory");
 253     call32_sloppy_post();
 254     return eax;
 255 }
 256
 257 // 16bit handler code called from call16_sloppy()
 258 u32 VISIBLE16
 259 call16_sloppy_helper(u32 eax, u32 edx, u32 (*func)(u32 eax, u32 edx))
 260 {
 261     call32_sloppy_post();
 262     u32 ret = func(eax, edx);
 263     call32_sloppy_prep();
 264     return ret;
 265 }
 266
 267 // Jump back to 16bit mode while in 32bit mode from call32_sloppy()
 268 static u32
 269 call16_sloppy(u32 eax, u32 edx, void *func)
 270 {
 271     ASSERT32FLAT();
 272     if (getesp() > MAIN_STACK_MAX)
 273         panic("call16_sloppy with invalid stack\n");
 274     func -= BUILD_BIOS_ADDR;
 275     u32 stackseg = Call32Data.ss;
 276     asm volatile(
 277         // Transition to 16bit mode
 278         "  movl $(1f - " __stringify(BUILD_BIOS_ADDR) "), %%edx\n"
 279         "  jmp transition16big\n"
 280         // Setup ss/esp and call func
 281         ASM32_SWITCH16
 282         "1:movl %3, %%ecx\n"
 283         "  shll $4, %3\n"
 284         "  movw %%cx, %%ss\n"
 285         "  subl %3, %%esp\n"
 286         "  movw %%cx, %%ds\n"
 287         "  movl %2, %%edx\n"
 288         "  movl %1, %%ecx\n"
 289         "  calll _cfunc16_call16_sloppy_helper\n"
 290         // Return to 32bit and restore esp
 291         "  movl $2f, %%edx\n"
 292         "  jmp transition32\n"
 293         ASM32_BACK32
 294         "2:addl %3, %%esp\n"
 295         : "+a" (eax)
 296         : "r" (func), "r" (edx), "r" (stackseg)
 297         : "edx", "ecx", "cc", "memory");
 298     return eax;
 299 }
 300
 301 // Call a 32bit SeaBIOS function from a 16bit SeaBIOS function.
 302 u32 VISIBLE16
 303 call32(void *func, u32 eax, u32 errret)
 304 {
 305     ASSERT16();
 306     if (CONFIG_CALL32_SMM && GET_GLOBAL(HaveSmmCall32))
 307         return call32_smm(func, eax);
 308     u32 cr0 = getcr0();
 309     if (cr0 & CR0_PE)
 310         // Called in 16bit protected mode?!
 311         return errret;
 312     return call32_sloppy(func, eax);
 313 }
 314
 315 // Call a 16bit SeaBIOS function from a 32bit SeaBIOS function.
 316 static u32
 317 call16(u32 eax, u32 edx, void *func)
 318 {
 319     ASSERT32FLAT();
 320     if (getesp() > BUILD_STACK_ADDR)
 321         panic("call16 with invalid stack\n");
 322     func -= BUILD_BIOS_ADDR;
 323     asm volatile(
 324         // Transition to 16bit mode
 325         "  movl $(1f - " __stringify(BUILD_BIOS_ADDR) "), %%edx\n"
 326         "  jmp transition16\n"
 327         // Call func
 328         ASM32_SWITCH16
 329         "1:movl %2, %%edx\n"
 330         "  calll *%1\n"
 331         // Return to 32bit
 332         "  movl $2f, %%edx\n"
 333         "  jmp transition32\n"
 334         ASM32_BACK32
 335         "2:\n"
 336         : "+a" (eax)
 337         : "r" (func), "r" (edx)
 338         : "edx", "ecx", "cc", "memory");
 339     return eax;
 340 }
 341
 342 // Call a 16bit SeaBIOS function in "big real" mode.
 343 static u32
 344 call16big(u32 eax, u32 edx, void *func)
 345 {
 346     ASSERT32FLAT();
 347     if (getesp() > BUILD_STACK_ADDR)
 348         panic("call16big with invalid stack\n");
 349     func -= BUILD_BIOS_ADDR;
 350     asm volatile(
 351         // Transition to 16bit mode
 352         "  movl $(1f - " __stringify(BUILD_BIOS_ADDR) "), %%edx\n"
 353         "  jmp transition16big\n"
 354         // Call func
 355         ASM32_SWITCH16
 356         "1:movl %2, %%edx\n"
 357         "  calll *%1\n"
 358         // Return to 32bit
 359         "  movl $2f, %%edx\n"
 360         "  jmp transition32\n"
 361         ASM32_BACK32
 362         "2:\n"
 363         : "+a" (eax)
 364         : "r" (func), "r" (edx)
 365         : "edx", "ecx", "cc", "memory");
 366     return eax;
 367 }
 368
 369 // Call a 16bit SeaBIOS function, restoring the mode from last call32().
 370 static u32
 371 call16_back(u32 eax, u32 edx, void *func)
 372 {
 373     ASSERT32FLAT();
 374     if (CONFIG_CALL32_SMM && Call32Data.method == C32_SMM)
 375         return call16_smm(eax, edx, func);
 376     if (Call32Data.method == C32_SLOPPY)
 377         return call16_sloppy(eax, edx, func);
 378     if (in_post())
 379         return call16big(eax, edx, func);
 380     return call16(eax, edx, func);
 381 }
 382
 383
 384 /****************************************************************
 385  * Extra 16bit stack
 386  ****************************************************************/
 387
 388 // Space for a stack for 16bit code.
 389 u8 ExtraStack[BUILD_EXTRA_STACK_SIZE+1] VARLOW __aligned(8);
 390 u8 *StackPos VARLOW;
 391
 392 // Test if currently on the extra stack
 393 int
 394 on_extra_stack(void)
 395 {
 396     return MODE16 && GET_SEG(SS) == SEG_LOW && getesp() > (u32)ExtraStack;
 397 }
 398
 399 // Switch to the extra stack and call a function.
 400 u32
 401 stack_hop(u32 eax, u32 edx, void *func)
 402 {
 403     if (on_extra_stack())
 404         return ((u32 (*)(u32, u32))func)(eax, edx);
 405     ASSERT16();
 406     u16 stack_seg = SEG_LOW;
 407     u32 bkup_ss, bkup_esp;
 408     asm volatile(
 409         // Backup current %ss/%esp values.
 410         "movw %%ss, %w3\n"
 411         "movl %%esp, %4\n"
 412         // Copy stack seg to %ds/%ss and set %esp
 413         "movw %w6, %%ds\n"
 414         "movw %w6, %%ss\n"
 415         "movl %5, %%esp\n"
 416         "pushl %3\n"
 417         "pushl %4\n"
 418         // Call func
 419         "calll *%2\n"
 420         "popl %4\n"
 421         "popl %3\n"
 422         // Restore segments and stack
 423         "movw %w3, %%ds\n"
 424         "movw %w3, %%ss\n"
 425         "movl %4, %%esp"
 426         : "+a" (eax), "+d" (edx), "+c" (func), "=&r" (bkup_ss), "=&r" (bkup_esp)
 427         : "m" (StackPos), "r" (stack_seg)
 428         : "cc", "memory");
 429     return eax;
 430 }
 431
 432 // Switch back to original caller's stack and call a function.
 433 u32
 434 stack_hop_back(u32 eax, u32 edx, void *func)
 435 {
 436     if (!MODESEGMENT)
 437         return call16_back(eax, edx, func);
 438     if (!MODE16 || !on_extra_stack())
 439         return ((u32 (*)(u32, u32))func)(eax, edx);
 440     ASSERT16();
 441     u16 bkup_ss;
 442     u32 bkup_stack_pos, temp;
 443     asm volatile(
 444         // Backup stack_pos and current %ss/%esp
 445         "movl %6, %4\n"
 446         "movw %%ss, %w3\n"
 447         "movl %%esp, %6\n"
 448         // Restore original callers' %ss/%esp
 449         "movl -4(%4), %5\n"
 450         "movl %5, %%ss\n"
 451         "movw %%ds:-8(%4), %%sp\n"
 452         "movl %5, %%ds\n"
 453         // Call func
 454         "calll *%2\n"
 455         // Restore %ss/%esp and stack_pos
 456         "movw %w3, %%ds\n"
 457         "movw %w3, %%ss\n"
 458         "movl %6, %%esp\n"
 459         "movl %4, %6"
 460         : "+a" (eax), "+d" (edx), "+c" (func), "=&r" (bkup_ss)
 461           , "=&r" (bkup_stack_pos), "=&r" (temp), "+m" (StackPos)
 462         :
 463         : "cc", "memory");
 464     return eax;
 465 }
 466
 467
 468 /****************************************************************
 469  * External 16bit interface calling
 470  ****************************************************************/
 471
 472 // Far call 16bit code with a specified register state.
 473 void VISIBLE16
 474 _farcall16(struct bregs *callregs, u16 callregseg)
 475 {
 476     if (need_hop_back()) {
 477         extern void _cfunc16__farcall16(void);
 478         stack_hop_back((u32)callregs, callregseg, _cfunc16__farcall16);
 479         return;
 480     }
 481     ASSERT16();
 482     asm volatile(
 483         "calll __farcall16\n"
 484         : "+a" (callregs), "+m" (*callregs), "+d" (callregseg)
 485         :
 486         : "ebx", "ecx", "esi", "edi", "cc", "memory");
 487 }
 488
 489 void
 490 farcall16(struct bregs *callregs)
 491 {
 492     extern void _cfunc16__farcall16(void);
 493     call16((u32)callregs, 0, _cfunc16__farcall16);
 494 }
 495
 496 void
 497 farcall16big(struct bregs *callregs)
 498 {
 499     extern void _cfunc16__farcall16(void);
 500     call16big((u32)callregs, 0, _cfunc16__farcall16);
 501 }
 502
 503 // Invoke a 16bit software interrupt.
 504 void
 505 __call16_int(struct bregs *callregs, u16 offset)
 506 {
 507     callregs->code.offset = offset;
 508     if (!MODESEGMENT) {
 509         callregs->code.seg = SEG_BIOS;
 510         _farcall16((void*)callregs - Call32Data.ss * 16, Call32Data.ss);
 511         return;
 512     }
 513     callregs->code.seg = GET_SEG(CS);
 514     _farcall16(callregs, GET_SEG(SS));
 515 }
 516
 517 // Reset the machine
 518 void
 519 reset(void)
 520 {
 521     extern void reset_vector(void) __noreturn;
 522     if (!MODE16)
 523         call16_back(0, 0, reset_vector);
 524     reset_vector();
 525 }
 526
 527
 528 /****************************************************************
 529  * Threads
 530  ****************************************************************/
 531
 532 // Thread info - stored at bottom of each thread stack - don't change
 533 // without also updating the inline assembler below.
 534 struct thread_info {
 535     void *stackpos;
 536     struct hlist_node node;
 537 };
 538 struct thread_info MainThread VARFSEG = {
 539     NULL, { &MainThread.node, &MainThread.node.next }
 540 };
 541 #define THREADSTACKSIZE 4096
 542
 543 // Check if any threads are running.
 544 static int
 545 have_threads(void)
 546 {
 547     return (CONFIG_THREADS
 548             && GET_FLATPTR(MainThread.node.next) != &MainThread.node);
 549 }
 550
 551 // Return the 'struct thread_info' for the currently running thread.
 552 struct thread_info *
 553 getCurThread(void)
 554 {
 555     u32 esp = getesp();
 556     if (esp <= MAIN_STACK_MAX)
 557         return &MainThread;
 558     return (void*)ALIGN_DOWN(esp, THREADSTACKSIZE);
 559 }
 560
 561 static int ThreadControl;
 562
 563 // Initialize the support for internal threads.
 564 void
 565 thread_init(void)
 566 {
 567     if (! CONFIG_THREADS)
 568         return;
 569     ThreadControl = romfile_loadint("etc/threads", 1);
 570 }
 571
 572 // Should hardware initialization threads run during optionrom execution.
 573 int
 574 threads_during_optionroms(void)
 575 {
 576     return CONFIG_THREADS && ThreadControl == 2 && in_post();
 577 }
 578
 579 // Switch to next thread stack.
 580 static void
 581 switch_next(struct thread_info *cur)
 582 {
 583     struct thread_info *next = container_of(
 584         cur->node.next, struct thread_info, node);
 585     if (cur == next)
 586         // Nothing to do.
 587         return;
 588     asm volatile(
 589         "  pushl $1f\n"                 // store return pc
 590         "  pushl %%ebp\n"               // backup %ebp
 591         "  movl %%esp, (%%eax)\n"       // cur->stackpos = %esp
 592         "  movl (%%ecx), %%esp\n"       // %esp = next->stackpos
 593         "  popl %%ebp\n"                // restore %ebp
 594         "  retl\n"                      // restore pc
 595         "1:\n"
 596         : "+a"(cur), "+c"(next)
 597         :
 598         : "ebx", "edx", "esi", "edi", "cc", "memory");
 599 }
 600
 601 // Last thing called from a thread (called on MainThread stack).
 602 static void
 603 __end_thread(struct thread_info *old)
 604 {
 605     hlist_del(&old->node);
 606     dprintf(DEBUG_thread, "\\%08x/ End thread\n", (u32)old);
 607     free(old);
 608     if (!have_threads())
 609         dprintf(1, "All threads complete.\n");
 610 }
 611
 612 // Create a new thread and start executing 'func' in it.
 613 void
 614 run_thread(void (*func)(void*), void *data)
 615 {
 616     ASSERT32FLAT();
 617     if (! CONFIG_THREADS || ! ThreadControl)
 618         goto fail;
 619     struct thread_info *thread;
 620     thread = memalign_tmphigh(THREADSTACKSIZE, THREADSTACKSIZE);
 621     if (!thread)
 622         goto fail;
 623
 624     dprintf(DEBUG_thread, "/%08x\\ Start thread\n", (u32)thread);
 625     thread->stackpos = (void*)thread + THREADSTACKSIZE;
 626     struct thread_info *cur = getCurThread();
 627     hlist_add_after(&thread->node, &cur->node);
 628     asm volatile(
 629         // Start thread
 630         "  pushl $1f\n"                 // store return pc
 631         "  pushl %%ebp\n"               // backup %ebp
 632         "  movl %%esp, (%%edx)\n"       // cur->stackpos = %esp
 633         "  movl (%%ebx), %%esp\n"       // %esp = thread->stackpos
 634         "  calll *%%ecx\n"              // Call func
 635
 636         // End thread
 637         "  movl %%ebx, %%eax\n"         // %eax = thread
 638         "  movl 4(%%ebx), %%ebx\n"      // %ebx = thread->node.next
 639         "  movl (%5), %%esp\n"          // %esp = MainThread.stackpos
 640         "  calll %4\n"                  // call __end_thread(thread)
 641         "  movl -4(%%ebx), %%esp\n"     // %esp = next->stackpos
 642         "  popl %%ebp\n"                // restore %ebp
 643         "  retl\n"                      // restore pc
 644         "1:\n"
 645         : "+a"(data), "+c"(func), "+b"(thread), "+d"(cur)
 646         : "m"(*(u8*)__end_thread), "m"(MainThread)
 647         : "esi", "edi", "cc", "memory");
 648     return;
 649
 650 fail:
 651     func(data);
 652 }
 653
 654
 655 /****************************************************************
 656  * Thread helpers
 657  ****************************************************************/
 658
 659 // Low-level irq enable.
 660 void VISIBLE16
 661 check_irqs(void)
 662 {
 663     if (need_hop_back()) {
 664         extern void _cfunc16_check_irqs(void);
 665         stack_hop_back(0, 0, _cfunc16_check_irqs);
 666         return;
 667     }
 668     asm volatile("sti ; nop ; rep ; nop ; cli ; cld" : : :"memory");
 669 }
 670
 671 // Briefly permit irqs to occur.
 672 void
 673 yield(void)
 674 {
 675     if (MODESEGMENT || !CONFIG_THREADS) {
 676         check_irqs();
 677         return;
 678     }
 679     struct thread_info *cur = getCurThread();
 680     if (cur == &MainThread)
 681         // Permit irqs to fire
 682         check_irqs();
 683
 684     // Switch to the next thread
 685     switch_next(cur);
 686 }
 687
 688 void VISIBLE16
 689 wait_irq(void)
 690 {
 691     if (need_hop_back()) {
 692         extern void _cfunc16_wait_irq(void);
 693         stack_hop_back(0, 0, _cfunc16_wait_irq);
 694         return;
 695     }
 696     asm volatile("sti ; hlt ; cli ; cld": : :"memory");
 697 }
 698
 699 // Wait for next irq to occur.
 700 void
 701 yield_toirq(void)
 702 {
 703     if (!MODESEGMENT && have_threads()) {
 704         // Threads still active - do a yield instead.
 705         yield();
 706         return;
 707     }
 708     wait_irq();
 709 }
 710
 711 // Wait for all threads (other than the main thread) to complete.
 712 void
 713 wait_threads(void)
 714 {
 715     ASSERT32FLAT();
 716     while (have_threads())
 717         yield();
 718 }
 719
 720 void
 721 mutex_lock(struct mutex_s *mutex)
 722 {
 723     ASSERT32FLAT();
 724     if (! CONFIG_THREADS)
 725         return;
 726     while (mutex->isLocked)
 727         yield();
 728     mutex->isLocked = 1;
 729 }
 730
 731 void
 732 mutex_unlock(struct mutex_s *mutex)
 733 {
 734     ASSERT32FLAT();
 735     if (! CONFIG_THREADS)
 736         return;
 737     mutex->isLocked = 0;
 738 }
 739
 740
 741 /****************************************************************
 742  * Thread preemption
 743  ****************************************************************/
 744
 745 int CanPreempt VARFSEG;
 746 static u32 PreemptCount;
 747
 748 // Turn on RTC irqs and arrange for them to check the 32bit threads.
 749 void
 750 start_preempt(void)
 751 {
 752     if (! threads_during_optionroms())
 753         return;
 754     CanPreempt = 1;
 755     PreemptCount = 0;
 756     rtc_use();
 757 }
 758
 759 // Turn off RTC irqs / stop checking for thread execution.
 760 void
 761 finish_preempt(void)
 762 {
 763     if (! threads_during_optionroms()) {
 764         yield();
 765         return;
 766     }
 767     CanPreempt = 0;
 768     rtc_release();
 769     dprintf(9, "Done preempt - %d checks\n", PreemptCount);
 770     yield();
 771 }
 772
 773 // Check if preemption is on, and wait for it to complete if so.
 774 int
 775 wait_preempt(void)
 776 {
 777     if (MODESEGMENT || !CONFIG_THREADS || !CanPreempt
 778         || getesp() < MAIN_STACK_MAX)
 779         return 0;
 780     while (CanPreempt)
 781         yield();
 782     return 1;
 783 }
 784
 785 // Try to execute 32bit threads.
 786 void VISIBLE32INIT
 787 yield_preempt(void)
 788 {
 789     PreemptCount++;
 790     switch_next(&MainThread);
 791 }
 792
 793 // 16bit code that checks if threads are pending and executes them if so.
 794 void
 795 check_preempt(void)
 796 {
 797     extern void _cfunc32flat_yield_preempt(void);
 798     if (CONFIG_THREADS && GET_GLOBAL(CanPreempt) && have_threads())
 799         call32(_cfunc32flat_yield_preempt, 0, 0);
 800 }
 801
 802
 803 /****************************************************************
 804  * call32 helper
 805  ****************************************************************/
 806
 807 struct call32_params_s {
 808     void *func;
 809     u32 eax, edx, ecx;
 810 };
 811
 812 u32 VISIBLE32FLAT
 813 call32_params_helper(struct call32_params_s *params)
 814 {
 815     return ((u32 (*)(u32, u32, u32))params->func)(
 816         params->eax, params->edx, params->ecx);
 817 }
 818
 819 u32
 820 call32_params(void *func, u32 eax, u32 edx, u32 ecx, u32 errret)
 821 {
 822     ASSERT16();
 823     struct call32_params_s params = {func, eax, edx, ecx};
 824     extern void _cfunc32flat_call32_params_helper(void);
 825     return call32(_cfunc32flat_call32_params_helper
 826                   , (u32)MAKE_FLATPTR(GET_SEG(SS), &params), errret);
 827 }