Version 0.1.2rel-0.1.2

21 files changed, 848 insertions, 186 deletions

diff --git a/Makefile b/Makefile
index a704cd9a..f6019ace 100644
--- a/Makefile
+++ b/Makefile
@@ -12,7 +12,7 @@ SRC16=floppy.c disk.c system.c clock.c serial.c kbd.c mouse.c output.c boot.c
 SRC32=post.c output.c
 
 # Default compiler flags (note -march=armv4 is needed for 16 bit insns)
-CFLAGS = -Wall -Os -MD -m32 -march=i386 -mregparm=2 -ffreestanding
+CFLAGS = -Wall -g -Os -MD -m32 -march=i386 -mregparm=2 -ffreestanding
 CFLAGS16 = -Wall -Os -MD -m32 -DMODE16 -march=i386 -mregparm=2 -ffreestanding -fno-jump-tables
 
 all: $(OUT) $(OUT)rom.bin
diff --git a/src/biosvar.h b/src/biosvar.h
index a6c6fb3c..037cdab8 100644
--- a/src/biosvar.h
+++ b/src/biosvar.h
@@ -3,6 +3,8 @@
 // Copyright (C) 2008  Kevin O'Connor <kevin@koconnor.net>
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
+#ifndef __BIOSVAR_H
+#define __BIOSVAR_H
 
 #include "types.h" // u8
 #include "farptr.h" // SET_SEG
@@ -23,8 +25,8 @@ struct bios_data_area_s {
     // 30:00
 //    u8 stack[256];
     // 40:00
-    u16 port_com1, port_com2, port_com3, port_com4;
-    u16 port_lpt1, port_lpt2, port_lpt3;
+    u16 port_com[4];
+    u16 port_lpt[3];
     u16 ebda_seg;
     // 40:10
     u16 equipment_list_flags;
@@ -52,7 +54,9 @@ struct bios_data_area_s {
     u32 timer_counter;
     // 40:70
     u8 timer_rollover;
-    u8 other4[0x0f];
+    u8 other4[0x07];
+    u8 lpt_timeout[4];
+    u8 com_timeout[4];
     // 40:80
     u16 kbd_buf_start_offset;
     u16 kbd_buf_end_offset;
@@ -123,9 +127,48 @@ struct extended_bios_data_area_s {
 #endif // BX_ELTORITO_BOOT
 };
 
+// Accessor functions
+#define GET_EBDA(var) \
+    GET_FARVAR(EBDA_SEG, ((struct extended_bios_data_area_s *)0)->var)
+#define SET_EBDA(var, val) \
+    SET_FARVAR(EBDA_SEG, ((struct extended_bios_data_area_s *)0)->var, (val))
+
 
 /****************************************************************
- * Extended Bios Data Area (EBDA)
+ * Initial Program Load (IPL)
+ ****************************************************************/
+
+// XXX - is this a standard, or just a bochs bios thing?
+
+struct ipl_entry_s {
+    u16 type;
+    u16 flags;
+    u32 vector;
+    u32 description;
+    u32 reserved;
+};
+
+struct ipl_s {
+    struct ipl_entry_s table[8];
+    u16 count;
+    u16 sequence;
+    u8 pad[124];
+};
+
+#define IPL_TYPE_FLOPPY      0x01
+#define IPL_TYPE_HARDDISK    0x02
+#define IPL_TYPE_CDROM       0x03
+#define IPL_TYPE_BEV         0x80
+
+// Accessor functions
+#define GET_IPL(var) \
+    GET_FARVAR(IPL_SEG, ((struct ipl_s *)0)->var)
+#define SET_IPL(var, val) \
+    SET_FARVAR(IPL_SEG, ((struct ipl_s *)0)->var, (val))
+
+
+/****************************************************************
+ * Registers saved/restored in romlayout.S
  ****************************************************************/
 
 #define UREG(ER, R, RH, RL) union { u32 ER; struct { u16 R; u16 R ## _hi; }; struct { u8 RL; u8 RH; u8 R ## _hilo; u8 R ## _hihi; }; }
@@ -179,5 +222,8 @@ extern struct bios_config_table_s BIOS_CONFIG_TABLE;
 #define SEG_BIOS     0xf000
 
 #define EBDA_SEG           0x9FC0
+#define IPL_SEG            0x9FF0
 #define EBDA_SIZE          1              // In KiB
 #define BASE_MEM_IN_K   (640 - EBDA_SIZE)
+
+#endif // __BIOSVAR_H
diff --git a/src/boot.c b/src/boot.c
index 828be145..af22e1eb 100644
--- a/src/boot.c
+++ b/src/boot.c
@@ -5,114 +5,178 @@
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
-#include "types.h" // VISIBLE
 #include "util.h" // irq_enable
 #include "biosvar.h" // struct bregs
-#include "farptr.h" // SET_SEG
+#include "config.h" // CONFIG_*
+#include "cmos.h" // inb_cmos
 
-static inline void
-__call_irq(u8 nr)
-{
-    asm volatile("int %0" : : "N" (nr));
-}
+//--------------------------------------------------------------------------
+// print_boot_device
+//   displays the boot device
+//--------------------------------------------------------------------------
 
-static inline u32
-call_irq(u8 nr, struct bregs *callregs)
+static const char drivetypes[][10]={
+    "", "Floppy","Hard Disk","CD-Rom", "Network"
+};
+
+static void
+print_boot_device(u16 type)
 {
-    u32 flags;
-    asm volatile(
-        // Save current registers
-        "pushal\n"
-        // Pull in calling registers.
-        "movl 0x04(%%eax), %%edi\n"
-        "movl 0x08(%%eax), %%esi\n"
-        "movl 0x0c(%%eax), %%ebp\n"
-        "movl 0x14(%%eax), %%ebx\n"
-        "movl 0x18(%%eax), %%edx\n"
-        "movl 0x1c(%%eax), %%ecx\n"
-        "movl 0x20(%%eax), %%eax\n"
-        // Invoke interrupt
-        "int %1\n"
-        // Restore registers
-        "popal\n"
-        // Exract flags
-        "pushfw\n"
-        "popl %%eax\n"
-        : "=a" (flags): "N" (nr), "a" (callregs), "m" (*callregs));
-    return flags;
+    /* NIC appears as type 0x80 */
+    if (type == IPL_TYPE_BEV)
+        type = 0x4;
+    if (type == 0 || type > 0x4)
+        BX_PANIC("Bad drive type\n");
+    printf("Booting from %s...\n", drivetypes[type]);
 }
 
+//--------------------------------------------------------------------------
+// print_boot_failure
+//   displays the reason why boot failed
+//--------------------------------------------------------------------------
 static void
-print_boot_failure()
+print_boot_failure(u16 type, u8 reason)
 {
-    bprintf(0, "Boot failed\n");
+    if (type == 0 || type > 0x3)
+        BX_PANIC("Bad drive type\n");
+
+    printf("Boot from %s failed", drivetypes[type]);
+    if (type < 4) {
+        /* Report the reason too */
+        if (reason==0)
+            printf(": not a bootable disk");
+        else
+            printf(": could not read the boot disk");
+    }
+    printf("\n");
 }
 
 static void
-try_boot()
+try_boot(u16 seq_nr)
 {
-    // XXX - assume floppy
-    u16 bootseg = 0x07c0;
+    SET_IPL(sequence, seq_nr);
+    u16 bootseg;
     u8 bootdrv = 0;
+    u16 bootdev, bootip;
+
+    if (CONFIG_ELTORITO_BOOT) {
+        bootdev = inb_cmos(CMOS_BIOS_BOOTFLAG2);
+        bootdev |= ((inb_cmos(CMOS_BIOS_BOOTFLAG1) & 0xf0) << 4);
+        bootdev >>= 4 * seq_nr;
+        bootdev &= 0xf;
+        if (bootdev == 0)
+            BX_PANIC("No bootable device.\n");
+
+        /* Translate from CMOS runes to an IPL table offset by subtracting 1 */
+        bootdev -= 1;
+    } else {
+        if (seq_nr ==2)
+            BX_PANIC("No more boot devices.");
+        if (!!(inb_cmos(CMOS_BIOS_CONFIG) & 0x20) ^ (seq_nr == 1))
+            /* Boot from floppy if the bit is set or it's the second boot */
+            bootdev = 0x00;
+        else
+            bootdev = 0x01;
+    }
+
+    if (bootdev >= GET_IPL(count)) {
+        BX_INFO("Invalid boot device (0x%x)\n", bootdev);
+        return;
+    }
+    u16 type = GET_IPL(table[bootdev].type);
+
+    /* Do the loading, and set up vector as a far pointer to the boot
+     * address, and bootdrv as the boot drive */
+    print_boot_device(type);
 
-    // Read sector
     struct bregs cr;
-    memset(&cr, 0, sizeof(cr));
-    cr.dl = bootdrv;
-    SET_SEG(ES, bootseg);
-    cr.bx = 0;
-    cr.ah = 2;
-    cr.al = 1;
-    cr.ch = 0;
-    cr.cl = 1;
-    cr.dh = 0;
-    u32 status = call_irq(0x13, &cr);
-
-    if (status & F_CF) {
-        print_boot_failure();
+    switch(type) {
+    case IPL_TYPE_FLOPPY: /* FDD */
+    case IPL_TYPE_HARDDISK: /* HDD */
+
+        bootdrv = (type == IPL_TYPE_HARDDISK) ? 0x80 : 0x00;
+        bootseg = 0x07c0;
+
+        // Read sector
+        memset(&cr, 0, sizeof(cr));
+        cr.dl = bootdrv;
+        cr.es = bootseg;
+        cr.ah = 2;
+        cr.al = 1;
+        cr.cl = 1;
+        call16_int(0x13, &cr);
+
+        if (cr.flags & F_CF) {
+            print_boot_failure(type, 1);
+            return;
+        }
+
+        /* Always check the signature on a HDD boot sector; on FDD,
+         * only do the check if the CMOS doesn't tell us to skip it */
+        if ((type != IPL_TYPE_FLOPPY)
+            || !((inb_cmos(CMOS_BIOS_BOOTFLAG1) & 0x01))) {
+            if (GET_FARVAR(bootseg, *(u16*)0x1fe) != 0xaa55) {
+                print_boot_failure(type, 0);
+                return;
+            }
+        }
+
+        /* Canonicalize bootseg:bootip */
+        bootip = (bootseg & 0x0fff) << 4;
+        bootseg &= 0xf000;
+        break;
+    case IPL_TYPE_CDROM: /* CD-ROM */
+        // XXX
         return;
+        break;
+    case IPL_TYPE_BEV: {
+        /* Expansion ROM with a Bootstrap Entry Vector (a far
+         * pointer) */
+        u32 vector = GET_IPL(table[bootdev].vector);
+        bootseg = vector >> 16;
+        bootip = vector & 0xffff;
+        break;
     }
+    default:
+        return;
+    }
+
+    memset(&cr, 0, sizeof(cr));
+    cr.ip = bootip;
+    cr.cs = bootseg;
+    // Set the magic number in ax and the boot drive in dl.
+    cr.dl = bootdrv;
+    cr.ax = 0xaa55;
+    call16(&cr);
 
-    u16 bootip = (bootseg & 0x0fff) << 4;
-    bootseg &= 0xf000;
-
-    u32 segoff = (bootseg << 16) | bootip;
-    asm volatile (
-        "pushf\n"
-        "pushl %0\n"
-        "movb %b1, %%dl\n"
-        // Set the magic number in ax and the boot drive in dl.
-        "movw $0xaa55, %%ax\n"
-        // Zero some of the other registers.
-        "xorw %%bx, %%bx\n"
-        "movw %%bx, %%ds\n"
-        "movw %%bx, %%es\n"
-        "movw %%bx, %%bp\n"
-        // Go!
-        "iretw\n"
-        : : "r" (segoff), "ri" (bootdrv));
+    // Boot failed: invoke the boot recovery function
+    memset(&cr, 0, sizeof(cr));
+    call16_int(0x18, &cr);
 }
 
 // Boot Failure recovery: try the next device.
 void VISIBLE
-handle_18(struct bregs *regs)
+handle_18()
 {
-    debug_enter(regs);
-    try_boot();
+    debug_enter(NULL);
+    u16 seq = GET_IPL(sequence) + 1;
+    try_boot(seq);
 }
 
 // INT 19h Boot Load Service Entry Point
 void VISIBLE
-handle_19(struct bregs *regs)
+handle_19()
 {
-    debug_enter(regs);
-    try_boot();
+    debug_enter(NULL);
+    try_boot(0);
 }
 
-// Callback from 32bit entry - start boot process
+// Called from 32bit code - start boot process
 void VISIBLE
 begin_boot()
 {
     irq_enable();
-    __call_irq(0x19);
+    struct bregs br;
+    memset(&br, 0, sizeof(br));
+    call16_int(0x19, &br);
 }
diff --git a/src/clock.c b/src/clock.c
index d45a8c70..f89b3917 100644
--- a/src/clock.c
+++ b/src/clock.c
@@ -250,7 +250,7 @@ handle_1a(struct bregs *regs)
 void VISIBLE
 handle_1c(struct bregs *regs)
 {
-    debug_enter(regs);
+    //debug_enter(regs);
 }
 
 // INT 08h System Timer ISR Entry Point
@@ -271,7 +271,12 @@ handle_08(struct bregs *regs)
     }
 
     SET_BDA(timer_counter, counter);
-    // XXX - int #0x1c
+
+    // chain to user timer tick INT #0x1c
+    struct bregs br;
+    memset(&br, 0, sizeof(br));
+    call16_int(0x1c, &br);
+
     eoi_master_pic();
 }
 
@@ -280,4 +285,47 @@ void VISIBLE
 handle_70(struct bregs *regs)
 {
     debug_enter(regs);
+
+    // Check which modes are enabled and have occurred.
+    u8 registerB = inb_cmos(CMOS_STATUS_B);
+    u8 registerC = inb_cmos(CMOS_STATUS_C);
+
+    if (!(registerB & 0x60))
+        goto done;
+    if (registerC & 0x20) {
+        // Handle Alarm Interrupt.
+        struct bregs br;
+        memset(&br, 0, sizeof(br));
+        call16_int(0x4a, &br);
+    }
+    if (!(registerC & 0x40))
+        goto done;
+
+    // Handle Periodic Interrupt.
+
+    if (!GET_BDA(rtc_wait_flag))
+        goto done;
+
+    // Wait Interval (Int 15, AH=83) active.
+    u32 time = GET_BDA(user_wait_timeout);  // Time left in microseconds.
+    if (time < 0x3D1) {
+        // Done waiting.
+        u32 segoff = GET_BDA(ptr_user_wait_complete_flag);
+        u16 segment = segoff >> 16;
+        u16 offset = segoff & 0xffff;
+        // Turn off status byte.
+        SET_BDA(rtc_wait_flag, 0);
+        // Clear the Periodic Interrupt.
+        outb_cmos(registerB & 0x37, CMOS_STATUS_B);
+        // Write to specified flag byte.
+        u8 oldval = GET_FARVAR(segment, *(u8*)(offset+0));
+        SET_FARVAR(segment, *(u8*)(offset+0), oldval | 0x80);
+    } else {
+        // Continue waiting.
+        time -= 0x3D1;
+        SET_BDA(user_wait_timeout, time);
+    }
+
+done:
+    eoi_both_pics();
 }
diff --git a/src/cmos.h b/src/cmos.h
index 0295fa3a..60ebe808 100644
--- a/src/cmos.h
+++ b/src/cmos.h
@@ -25,11 +25,14 @@
 #define CMOS_RESET_CODE          0x0f
 #define CMOS_FLOPPY_DRIVE_TYPE   0x10
 #define CMOS_EQUIPMENT_INFO      0x14
+#define CMOS_BIOS_CONFIG         0x2d
 #define CMOS_EXTMEM_LOW          0x30
 #define CMOS_EXTMEM_HIGH         0x31
 #define CMOS_CENTURY             0x32
 #define CMOS_EXTMEM2_LOW         0x34
 #define CMOS_EXTMEM2_HIGH        0x35
+#define CMOS_BIOS_BOOTFLAG1      0x38
+#define CMOS_BIOS_BOOTFLAG2      0x3d
 
 // CMOS_STATUS_B bitdefs
 #define CSB_EN_ALARM_IRQ (1<<5)
diff --git a/src/config.h b/src/config.h
index 53996b43..161dfcc9 100644
--- a/src/config.h
+++ b/src/config.h
@@ -3,6 +3,8 @@
 #define CONFIG_FLOPPY_SUPPORT 1
 #define CONFIG_PS2_MOUSE 0
 #define CONFIG_ATA 0
-#define CONFIG_STACK16_SEGMENT 0x00
-#define CONFIG_STACK16_OFFSET  0xfffe
-#define CONFIG_STACK32_OFFSET 0x80000
+#define CONFIG_KBD_CALL_INT15_4F 1
+#define CONFIG_ELTORITO_BOOT 0
+
+#define CONFIG_STACK_SEGMENT 0x00
+#define CONFIG_STACK_OFFSET  0xfffe
diff --git a/src/disk.c b/src/disk.c
index 144e42fa..675e9bf5 100644
--- a/src/disk.c
+++ b/src/disk.c
@@ -12,6 +12,7 @@
 static void
 disk_13(struct bregs *regs, u8 drive)
 {
+    // XXX
     set_cf(regs, 1);
 }
 
@@ -46,6 +47,7 @@ handle_13(struct bregs *regs)
 {
     //debug_enter(regs);
     u8 drive = regs->dl;
+    // XXX
 #if BX_ELTORITO_BOOT
     if (regs->ah >= 0x4a || regs->ah <= 0x4d) {
         int13_eltorito(regs);
diff --git a/src/farptr.h b/src/farptr.h
index 34c3ac2c..86179cce 100644
--- a/src/farptr.h
+++ b/src/farptr.h
@@ -29,6 +29,8 @@
     __asm__ __volatile__("movl %0, %%" #SEG ":%1"       \
                          : : "r"(value), "m"(var))
 
+extern void __force_link_error__unknown_type();
+
 #define __GET_VAR(seg, var) ({                                  \
     typeof(var) __val;                                          \
     if (__builtin_types_compatible_p(typeof(__val), u8))        \
@@ -37,6 +39,8 @@
         __val = READ16_SEG(seg, var);                           \
     else if (__builtin_types_compatible_p(typeof(__val), u32))  \
         __val = READ32_SEG(seg, var);                           \
+    else                                                        \
+        __force_link_error__unknown_type();                     \
     __val; })
 
 #define __SET_VAR(seg, var, val) do {                             \
@@ -46,6 +50,8 @@
             WRITE16_SEG(seg, var, (val));                         \
         else if (__builtin_types_compatible_p(typeof(var), u32))  \
             WRITE32_SEG(seg, var, (val));                         \
+        else                                                      \
+            __force_link_error__unknown_type();                   \
     } while (0)
 
 #define __SET_SEG(SEG, value)                                   \
@@ -55,23 +61,39 @@
     __asm__ __volatile__("movw %%" #SEG ", %w0" : "=r"(__seg)); \
     __seg;})
 
+#define GET_FARVAR(seg, var) ({                 \
+    SET_SEG(ES, (seg));                         \
+    GET_VAR(ES, (var)); })
+#define SET_FARVAR(seg, var, val) do {          \
+        SET_SEG(ES, (seg));                     \
+        SET_VAR(ES, (var), val);                \
+    } while (0)
+
+#define PTR_TO_SEG(p) ((((u32)(p)) >> 4) & 0xf000)
+#define PTR_TO_OFFSET(p) (((u32)(p)) & 0xffff)
+
+#define __GET_FARPTR(ptr) ({                                            \
+    typeof (&(ptr)) __ptr;                                              \
+    GET_FARVAR(PTR_TO_SEG(__ptr), *(typeof __ptr)PTR_TO_OFFSET(__ptr)); })
+#define __SET_FARVAR(ptr, val) do {                                     \
+        typeof (&(ptr)) __ptr;                                          \
+        SET_FARVAR(PTR_TO_SEG(__ptr), *(typeof __ptr)PTR_TO_OFFSET(__ptr) \
+                   , (val));                                            \
+    } while (0)
+
 #ifdef MODE16
-#define GET_VAR(seg, var) __GET_VAR(seg, var)
-#define SET_VAR(seg, var, val) __SET_VAR(seg, var, val)
-#define SET_SEG(SEG, value) __SET_SEG(SEG, value)
+#define GET_VAR(seg, var) __GET_VAR(seg, (var))
+#define SET_VAR(seg, var, val) __SET_VAR(seg, (var), (val))
+#define SET_SEG(SEG, value) __SET_SEG(SEG, (value))
 #define GET_SEG(SEG) __GET_SEG(SEG)
+#define GET_FARPTR(ptr) __GET_FARPTR(ptr)
+#define SET_FARPTR(ptr, val) __SET_FARPTR((ptr), (val))
 #else
 // In 32-bit mode there is no need to mess with the segments.
 #define GET_VAR(seg, var) (var)
-#define SET_VAR(seg, var, val) (var) = (val)
+#define SET_VAR(seg, var, val) do { (var) = (val); } while (0)
 #define SET_SEG(SEG, value) ((void)(value))
 #define GET_SEG(SEG) 0
+#define GET_FARPTR(ptr) (ptr)
+#define SET_FARPTR(ptr, val) do { (var) = (val); } while (0)
 #endif
-
-#define GET_FARVAR(seg, var) ({                 \
-    SET_SEG(ES, (seg));                         \
-    GET_VAR(ES, (var)); })
-#define SET_FARVAR(seg, var, val) do {          \
-        SET_SEG(ES, (seg));                     \
-        SET_VAR(ES, (var), val);                \
-    } while (0)
diff --git a/src/floppy.c b/src/floppy.c
index fb4e2b5e..98e18af8 100644
--- a/src/floppy.c
+++ b/src/floppy.c
@@ -14,7 +14,8 @@
 
 #define BX_FLOPPY_ON_CNT 37   /* 2 seconds */
 
-////.org 0xefc7
+// XXX - //.org 0xefc7
+
 // Since no provisions are made for multiple drive types, most
 // values in this table are ignored.  I set parameters for 1.44M
 // floppy here
diff --git a/src/ioport.h b/src/ioport.h
index 030a8ece..eed676be 100644
--- a/src/ioport.h
+++ b/src/ioport.h
@@ -33,6 +33,7 @@
 #define PORT_DMA2_MASK_REG     0x00d4
 #define PORT_DMA2_MODE_REG     0x00d6
 #define PORT_DMA2_MASTER_CLEAR 0x00da
+#define PORT_MATH_CLEAR        0x00f0
 #define PORT_FD_DOR            0x03f2
 #define PORT_FD_STATUS         0x03f4
 #define PORT_FD_DATA           0x03f5
diff --git a/src/kbd.c b/src/kbd.c
index 4115aa76..13e4a62e 100644
--- a/src/kbd.c
+++ b/src/kbd.c
@@ -7,6 +7,7 @@
 
 #include "biosvar.h" // struct bregs
 #include "util.h" // debug_enter
+#include "config.h" // CONFIG_*
 
 static u8
 enqueue_key(u8 scan_code, u8 ascii_code)
@@ -44,6 +45,7 @@ dequeue_key(u8 *scan_code, u8 *ascii_code, u8 incr)
             break;
         if (!incr)
             return 0;
+        nop();
     }
 
     *ascii_code = GET_FARVAR(0x0000, *(u8*)(buffer_head+0x400+0));
@@ -554,16 +556,24 @@ handle_09(struct bregs *regs)
     // read key from keyboard controller
     u8 key = inb(PORT_KBD_DATA);
     irq_enable();
-#if 0
     if (CONFIG_KBD_CALL_INT15_4F) {
-        // XXX
+        // allow for keyboard intercept
+        struct bregs tr;
+        memset(&tr, 0, sizeof(tr));
+        tr.al = key;
+        tr.ah = 0x4f;
+        tr.flags = F_CF;
+        call16_int(0x15, &tr);
+        if (!tr.flags & F_CF)
+            goto done;
+        key = tr.al;
     }
-#endif
     process_key(key);
 
     irq_disable();
     eoi_master_pic();
 
+done:
     // enable keyboard
     outb(0xae, PORT_KBD_STATUS);
 }
diff --git a/src/mouse.c b/src/mouse.c
index 6364c686..e99700a5 100644
--- a/src/mouse.c
+++ b/src/mouse.c
@@ -11,11 +11,13 @@
 void
 handle_15c2(struct bregs *regs)
 {
+    // XXX
 }
 
 static u8
 int74_function()
 {
+    // XXX
     return 0;
 }
 
diff --git a/src/output.c b/src/output.c
index efb64114..6c7f7198 100644
--- a/src/output.c
+++ b/src/output.c
@@ -11,9 +11,13 @@
 #include "biosvar.h" // struct bregs
 
 static void
-screenc(char c)
+screenc(u8 c)
 {
-    // XXX
+    struct bregs br;
+    memset(&br, 0, sizeof(br));
+    br.ah = 0x0e;
+    br.al = c;
+    call16_int(0x10, &br);
 }
 
 // XXX
@@ -23,8 +27,12 @@ screenc(char c)
 static void
 putc(u16 action, char c)
 {
-    screenc(c);
     outb(c, PORT_DEBUG);
+    if (action) {
+        if (c == '\n')
+            screenc('\r');
+        screenc(c);
+    }
 }
 
 // Write a string to the framebuffer.
@@ -140,13 +148,27 @@ bprintf(u16 action, const char *fmt, ...)
     va_end(args);
 }
 
+static void
+dump_regs(const char *fname, const char *type, struct bregs *regs)
+{
+    if (!regs) {
+        bprintf(0, "%s %s: NULL\n", type, fname);
+        return;
+    }
+    bprintf(0, "%s %s: a=%x b=%x c=%x d=%x si=%x di=%x\n"
+            , type, fname, regs->eax, regs->ebx, regs->ecx, regs->edx
+            , regs->esi, regs->edi);
+    bprintf(0, "  ds=%x es=%x bp=%x sp=%x ip=%x cs=%x f=%x\n"
+            , regs->ds, regs->es, regs->ebp, regs->esp
+            , regs->ip, regs->cs, regs->flags);
+}
+
 // Function called on handler startup.
 void
 __debug_enter(const char *fname, struct bregs *regs)
 {
-    bprintf(0, "enter %s: a=%x b=%x c=%x d=%x si=%x di=%x\n"
-            , fname, regs->eax, regs->ebx, regs->ecx, regs->edx
-            , regs->esi, regs->edi);
+    // XXX - implement run time suppression test
+    dump_regs(fname, "enter", regs);
 }
 
 void
@@ -154,7 +176,5 @@ __debug_exit(const char *fname, struct bregs *regs)
 {
     if (! (regs->flags & F_CF))
         return;
-    bprintf(0, "exit %s: a=%x b=%x c=%x d=%x s=%x i=%x\n"
-            , fname, regs->eax, regs->ebx, regs->ecx, regs->edx
-            , regs->esi, regs->edi);
+    dump_regs(fname, "exit", regs);
 }
diff --git a/src/post.c b/src/post.c
index a6dd424b..30490016 100644
--- a/src/post.c
+++ b/src/post.c
@@ -14,6 +14,7 @@
 
 #define bda ((struct bios_data_area_s *)0)
 #define ebda ((struct extended_bios_data_area_s *)(EBDA_SEG<<4))
+#define ipl ((struct ipl_s *)(IPL_SEG<<4))
 
 static void
 init_bda()
@@ -62,12 +63,13 @@ init_handlers()
 static void
 init_ebda()
 {
+    memset(ebda, 0, sizeof(*ebda));
     ebda->size = EBDA_SIZE;
     bda->ebda_seg = EBDA_SEG;
     bda->ivecs[0x41].seg = EBDA_SEG;
-    bda->ivecs[0x41].offset = 0x3d; // XXX
+    bda->ivecs[0x41].offset = offsetof(struct extended_bios_data_area_s, fdpt0);
     bda->ivecs[0x46].seg = EBDA_SEG;
-    bda->ivecs[0x46].offset = 0x4d; // XXX
+    bda->ivecs[0x41].offset = offsetof(struct extended_bios_data_area_s, fdpt1);
 }
 
 static void
@@ -222,22 +224,64 @@ kbd_setup()
            - 0x400);
     keyboard_init();
 
-    // XXX
+    // mov CMOS Equipment Byte to BDA Equipment Word
     u16 eqb = bda->equipment_list_flags;
-    eqb = (eqb & 0xff00) | inb_cmos(CMOS_EQUIPMENT_INFO);
-    bda->equipment_list_flags = eqb;
+    bda->equipment_list_flags = (eqb & 0xff00) | inb_cmos(CMOS_EQUIPMENT_INFO);
+}
+
+static u16
+detect_parport(u16 port, u8 timeout, u8 count)
+{
+    // clear input mode
+    outb(inb(port+2) & 0xdf, port+2);
+
+    outb(0xaa, port);
+    if (inb(port) != 0xaa)
+        // Not present
+        return 0;
+    bda->port_lpt[count] = port;
+    bda->lpt_timeout[count] = timeout;
+    return 1;
 }
 
 static void
 lpt_setup()
 {
-    // XXX
+    u16 count = 0;
+    count += detect_parport(0x378, 0x14, count);
+    count += detect_parport(0x278, 0x14, count);
+
+    // Equipment word bits 14..15 determing # parallel ports
+    u16 eqb = bda->equipment_list_flags;
+    bda->equipment_list_flags = (eqb & 0x3fff) | (count << 14);
+}
+
+static u16
+detect_serial(u16 port, u8 timeout, u8 count)
+{
+    outb(0x02, port+1);
+    if (inb(port+1) != 0x02)
+        return 0;
+    if (inb(port+2) != 0x02)
+        return 0;
+    outb(0x00, port+1);
+    bda->port_com[count] = port;
+    bda->com_timeout[count] = timeout;
+    return 1;
 }
 
 static void
 serial_setup()
 {
-    // XXX
+    u16 count = 0;
+    count += detect_serial(0x3f8, 0x0a, count);
+    count += detect_serial(0x2f8, 0x0a, count);
+    count += detect_serial(0x3e8, 0x0a, count);
+    count += detect_serial(0x2e8, 0x0a, count);
+
+    // Equipment word bits 9..11 determing # serial ports
+    u16 eqb = bda->equipment_list_flags;
+    bda->equipment_list_flags = (eqb & 0xf1ff) | (count << 9);
 }
 
 static u32
@@ -295,40 +339,64 @@ floppy_drive_post()
 static void
 cdemu_init()
 {
+    // XXX
     //ebda->cdemu.active = 0;
 }
 
 static void
 ata_init()
 {
+    // XXX
 }
 
 static void
 ata_detect()
 {
+    // XXX
 }
 
 static void
 hard_drive_post()
 {
+    // XXX
 }
 
+
 static void
 init_boot_vectors()
 {
+    // Clear out the IPL table.
+    memset(ipl, 0, sizeof(*ipl));
+
+    // Floppy drive
+    struct ipl_entry_s *ip = &ipl->table[0];
+    ip->type = IPL_TYPE_FLOPPY;
+    ip++;
+
+    // First HDD
+    ip->type = IPL_TYPE_HARDDISK;
+    ip++;
+
+    // CDROM
+    if (CONFIG_ELTORITO_BOOT) {
+        ip->type = IPL_TYPE_CDROM;
+        ip++;
+    }
+
+    ipl->count = ip - ipl->table;
+    ipl->sequence = 0xffff;
 }
 
-static void __attribute__((noinline))
-call16(u16 seg, u16 offset)
+static void
+callrom(u16 seg, u16 offset)
 {
-    u32 segoff = (seg << 16) | offset;
-    asm volatile(
-        "pushal\n"  // Save registers
-        "ljmp $0x20, %0\n" // Jump to 16bit transition code
-        ".globl call16_resume\n"
-        "call16_resume:\n"  // point of return
-        "popal\n"   // restore registers
-        : : "Z" (OFFSET_call16), "b" (segoff));
+    struct bregs br;
+    memset(&br, 0, sizeof(br));
+    br.es = 0xf000;
+    br.di = OFFSET_pnp_string;
+    br.cs = seg;
+    br.ip = offset;
+    call16(&br);
 }
 
 static int
@@ -341,9 +409,6 @@ checksum(u8 *p, u32 len)
     return sum;
 }
 
-#define PTR_TO_SEG(p) ((((u32)(p)) >> 4) & 0xf000)
-#define PTR_TO_OFFSET(p) (((u32)(p)) & 0xffff)
-
 static void
 rom_scan()
 {
@@ -356,7 +421,7 @@ rom_scan()
         if (checksum(rom, len) != 0)
             continue;
         p = (u8*)(((u32)p + len) / 2048 * 2048);
-        call16(PTR_TO_SEG(rom), PTR_TO_OFFSET(rom + 3));
+        callrom(PTR_TO_SEG(rom), PTR_TO_OFFSET(rom + 3));
 
         // Look at the ROM's PnP Expansion header.  Properly, we're supposed
         // to init all the ROMs and then go back and build an IPL table of
@@ -372,13 +437,25 @@ rom_scan()
         // Found a device that thinks it can boot the system.  Record
         // its BEV and product name string.
 
-        // XXX
+        if (ipl->count >= ARRAY_SIZE(ipl->table))
+            continue;
+
+        struct ipl_entry_s *ip = &ipl->table[ipl->count];
+        ip->type = IPL_TYPE_BEV;
+        ip->vector = (PTR_TO_SEG(rom) << 16) | entry;
+
+        u16 desc = *(u16*)&rom[0x1a+0x10];
+        if (desc)
+            ip->description = (PTR_TO_SEG(rom) << 16) | desc;
+
+        ipl->count++;
     }
 }
 
 static void
 status_restart(u8 status)
 {
+    // XXX
 #if 0
     if (status == 0x05)
         eoi_jmp_post();
@@ -417,6 +494,7 @@ post()
     serial_setup();
     timer_setup();
     pic_setup();
+    // XXX - need to do pci stuff
     //pci_setup();
     init_boot_vectors();
     rom_scan();
@@ -430,7 +508,7 @@ post()
         ata_detect();
     }
     cdemu_init();
-    call16(0xf000, OFFSET_begin_boot);
+    callrom(0xf000, OFFSET_begin_boot);
 }
 
 void VISIBLE
diff --git a/src/rombios32.lds.S b/src/rombios32.lds.S
index dae62d8b..18b8ab03 100644
--- a/src/rombios32.lds.S
+++ b/src/rombios32.lds.S
@@ -23,6 +23,7 @@ SECTIONS
         __bss_start = . ;
         .bss      : { *(.bss) *(COMMON) }
         _end = . ;
+        __call16 = (0xf0000 | OFFSET___call16_from32) ;
         /DISCARD/ : { *(.stab)
                      *(.stabstr)
                      *(.comment)
diff --git a/src/romlayout.S b/src/romlayout.S
index 4d648bf2..76de2eb5 100644
--- a/src/romlayout.S
+++ b/src/romlayout.S
@@ -8,7 +8,12 @@
 #include "config.h"
 
         .code16gcc
-        .text
+
+
+/****************************************************************
+ * Include of 16bit C code
+ ****************************************************************/
+
         .globl bios16c_start, bios16c_end
 bios16c_start:
 .include "out/blob.proc.16.s"
@@ -16,21 +21,35 @@ bios16c_start:
 bios16c_end:
 
 
+/****************************************************************
+ * POST handler
+ ****************************************************************/
+
         .org 0xe05b
         .globl _start
 _start:
         .globl post16
 post16:
+        // init the stack pointer
+        xorw %ax, %ax
+        movw %ax, %ss
+        movl $ CONFIG_STACK_OFFSET , %esp
 
-        // Set entry point of rombios32 code - the actual instruction
+        // Set entry point of rombios32 code - the actual address
 	// is altered later in the build process.
         .globl set_entry32
 set_entry32:
-        mov $0xf0000000, %ebx
+        pushl $0xf0000000
 
-        // init the stack pointer
-        movl $ CONFIG_STACK32_OFFSET , %esp
+        // Fall through to transition32 function below
+
+
+/****************************************************************
+ * Call trampolines
+ ****************************************************************/
 
+// Place CPU into 32bit mode from 16bit mode.
+// Clobbers: %eax, flags, stack registers, cr0, idt/gdt
 transition32:
         // Disable irqs
         cli
@@ -65,23 +84,20 @@ transition32:
 
         cld
 
-        jmp *%ebx
+        retl
 
-        .code16gcc
-
-// We need a copy of this string, but we are not actually a PnP BIOS,
-// so make sure it is *not* aligned, so OSes will not see it if they
-// scan.
-        .align 2
-        .byte 0
-pnp_string:
-        .ascii "$PnP"
+// Call a 16bit function from 32bit mode.
+// 4(%esp) = address of struct bregs
+// Clobbers: all gp registers, flags, stack registers, cr0, idt/gdt
+        .globl __call16_from32
+__call16_from32:
+        pushl %eax
 
-// Return from 32bit code to 16bit code - must pass in destination
-// code segment,offset (%ebx) and the return stack position (%esp).
+        // Jump to 16bit mode
+        ljmp $0x20, $1f
 
-        .globl call16
-call16:
+        .code16gcc
+1:
         // restore data segment limits to 0xffff
         movw $0x28, %ax
         movw %ax, %ds
@@ -96,43 +112,79 @@ call16:
         movl %eax, %cr0
 
         // far jump to flush CPU queue after transition to real mode
-        ljmpw $0xf000, $1f
-1:
+        ljmpw $0xf000, $2f
+2:
         // restore IDT to normal real-mode defaults
         lidt %cs:rmode_IDT_info
 
-        // Setup segment registers
+        // Clear segment registers
         xorw %ax, %ax
-        movw %ax, %ds
         movw %ax, %fs
         movw %ax, %gs
-        movw $0xf000, %ax
         movw %ax, %es
-        lea pnp_string, %di
-        movw $ CONFIG_STACK16_SEGMENT , %ax
-        movw %ax, %ss
-        movl %esp, %eax
-        movl $ CONFIG_STACK16_OFFSET , %esp
+        movw %ax, %ds
+        movw %ax, %ss  // Assume stack is in segment 0
 
-        // Save info
-        pushl %eax
-        pushl %ebx
-        movl %esp, %ebp
+        popl %eax
+        pushl $transition32
 
-        lcallw %ss:*(%bp)
+        // Fall through to __call16
 
-        // Restore stack and jump back to 32bit mode.
+
+// Call a 16bit function with a specified cpu register state
+// %eax = address of struct bregs
+// Clobbers: all gp registers, es
+        .globl __call16
+__call16:
+        // Save eax
+        pushl %eax
+
+        // Setup for iretw call
+        pushw $0xf000
+        pushw $1f               // return point
+        pushw 0x28(%eax)        // flags
+        pushl 0x24(%eax)        // CS:IP
+
+        // Load calling registers.
+        movl 0x04(%eax), %edi
+        movl 0x08(%eax), %esi
+        movl 0x0c(%eax), %ebp
+        movl 0x14(%eax), %ebx
+        movl 0x18(%eax), %edx
+        movl 0x1c(%eax), %ecx
+        movw 0x02(%eax), %es    // XXX - should load %ds too
+        movl 0x20(%eax), %eax
+
+        // Invoke call
+        iretw                   // XXX - just do a lcalll
+1:
+        // Store flags, eax, ecx
+        pushfw
+        pushl %eax
+        movl 0x06(%esp), %eax
+        movl %ecx, 0x1c(%eax)   // Save %ecx
+        popl %ecx
+        movl %ecx, 0x20(%eax)   // Save %eax
+        popw %cx
+        movw %cx, 0x28(%eax)    // Save flags
+
+        // Store remaining registers
+        movw %es, 0x02(%eax)
+        movl %edi, 0x04(%eax)
+        movl %esi, 0x08(%eax)
+        movl %ebp, 0x0c(%eax)
+        movl %ebx, 0x14(%eax)
+        movl %edx, 0x18(%eax)
+
+        // Remove %eax
         popl %eax
-        popl %esp
 
-        // Set resume point of rombios32 code - the actual instruction
-	// is altered later in the build process.
-        .globl set_resume32
-set_resume32:
-        mov $0xf0000000, %ebx
+        retl
 
-        jmp transition32
 
+/****************************************************************
+ * GDT and IDT tables
+ ****************************************************************/
 
 // Protected mode IDT descriptor
 //
@@ -171,15 +223,30 @@ rombios32_gdt:
         .word 0xffff, 0, 0x9b0f, 0x0000 // 16 bit code segment base=0xf0000 limit=0xffff
         .word 0xffff, 0, 0x9300, 0x0000 // 16 bit data segment base=0x0 limit=0xffff
 
+// We need a copy of this string, but we are not actually a PnP BIOS,
+// so make sure it is *not* aligned, so OSes will not see it if they
+// scan.
+        .align 2
+        .byte 0
+        .globl pnp_string
+pnp_string:
+        .ascii "$PnP"
+
+
+/****************************************************************
+ * Interrupt entry points
+ ****************************************************************/
 
         .macro ENTRY cfunc
+        cli         // In case something far-calls insted of using "int"
         pushal
         pushw %es
         pushw %ds
         movw %ss, %ax
         movw %ax, %ds
-        mov %esp, %eax
-        call \cfunc
+        movzwl %sp, %esp
+        movl %esp, %eax
+        calll \cfunc
         popw %ds
         popw %es
         popal
@@ -192,21 +259,50 @@ rombios32_gdt:
         iretw
         .endm
 
+        .macro IRQ_TRAMPOLINE num
+        .globl irq_trampoline_0x\num
+        irq_trampoline_0x\num :
+        int $0x\num
+        lretw
+        .endm
 
         .org 0xe2c3
         IRQ_ENTRY nmi
 
         IRQ_ENTRY 13
-        IRQ_ENTRY 19
         IRQ_ENTRY 12
         IRQ_ENTRY 11
         IRQ_ENTRY 76
-        IRQ_ENTRY 18
         IRQ_ENTRY 1c
         IRQ_ENTRY 70
         IRQ_ENTRY 74
         IRQ_ENTRY 75
 
+        .globl entry_19
+entry_19:
+        // init the stack pointer
+        xorw %ax, %ax
+        movw %ax, %ss
+        movl $ CONFIG_STACK_OFFSET , %esp
+        calll handle_19
+
+        .globl entry_18
+entry_18:
+        // init the stack pointer
+        xorw %ax, %ax
+        movw %ax, %ss
+        movl $ CONFIG_STACK_OFFSET , %esp
+        calll handle_18
+
+        IRQ_TRAMPOLINE 02
+        IRQ_TRAMPOLINE 10
+        IRQ_TRAMPOLINE 13
+        IRQ_TRAMPOLINE 15
+        IRQ_TRAMPOLINE 18
+        IRQ_TRAMPOLINE 19
+        IRQ_TRAMPOLINE 1c
+        IRQ_TRAMPOLINE 4a
+
         .org 0xe3fe
         jmp entry_13
 
@@ -214,6 +310,7 @@ rombios32_gdt:
         // XXX - Fixed Disk Parameter Table
 
         .org 0xe6f2
+        // XXX - should reset ss and sp
         jmp entry_19
 
         .org 0xe6f5
@@ -255,7 +352,11 @@ rombios32_gdt:
         // XXX int 1D
         iretw
 
+        .globl freespace2_start, freespace2_end
+freespace2_start:
+
         .org 0xf841
+freespace2_end:
         jmp entry_12
 
         .org 0xf84d
diff --git a/src/serial.c b/src/serial.c
index 55410890..63d17666 100644
--- a/src/serial.c
+++ b/src/serial.c
@@ -8,11 +8,210 @@
 #include "biosvar.h" // struct bregs
 #include "util.h" // debug_enter
 
+
+/****************************************************************
+ * COM ports
+ ****************************************************************/
+
+static u16
+getComAddr(struct bregs *regs)
+{
+    if (regs->dx >= 4) {
+        set_cf(regs, 1);
+        return 0;
+    }
+    u16 addr = GET_BDA(port_com[regs->dx]);
+    if (! addr)
+        set_cf(regs, 1);
+    return addr;
+}
+
+static void
+handle_1400(struct bregs *regs)
+{
+    u16 addr = getComAddr(regs);
+    if (!addr)
+        return;
+    outb(inb(addr+3) | 0x80, addr+3);
+    if ((regs->al & 0xE0) == 0) {
+        outb(0x17, addr);
+        outb(0x04, addr+1);
+    } else {
+        u16 val16 = 0x600 >> ((regs->al & 0xE0) >> 5);
+        outb(val16 & 0xFF, addr);
+        outb(val16 >> 8, addr+1);
+    }
+    outb(regs->al & 0x1F, addr+3);
+    regs->ah = inb(addr+5);
+    regs->al = inb(addr+6);
+    set_cf(regs, 0);
+}
+
+static void
+handle_1401(struct bregs *regs)
+{
+    u16 addr = getComAddr(regs);
+    if (!addr)
+        return;
+    u16 timer = GET_BDA(timer_counter);
+    u16 timeout = GET_BDA(com_timeout[regs->dx]);
+    while (((inb(addr+5) & 0x60) != 0x60) && (timeout)) {
+        u16 val16 = GET_BDA(timer_counter);
+        if (val16 != timer) {
+            timer = val16;
+            timeout--;
+        }
+    }
+    if (timeout)
+        outb(regs->al, addr);
+    regs->ah = inb(addr+5);
+    if (!timeout)
+        regs->ah |= 0x80;
+    set_cf(regs, 0);
+}
+
+static void
+handle_1402(struct bregs *regs)
+{
+    u16 addr = getComAddr(regs);
+    if (!addr)
+        return;
+    u16 timer = GET_BDA(timer_counter);
+    u16 timeout = GET_BDA(com_timeout[regs->dx]);
+    while (((inb(addr+5) & 0x01) == 0) && (timeout)) {
+        u16 val16 = GET_BDA(timer_counter);
+        if (val16 != timer) {
+            timer = val16;
+            timeout--;
+        }
+    }
+    if (timeout) {
+        regs->ah = 0;
+        regs->al = inb(addr);
+    } else {
+        regs->ah = inb(addr+5);
+    }
+    set_cf(regs, 0);
+}
+
+static void
+handle_1403(struct bregs *regs)
+{
+    u16 addr = getComAddr(regs);
+    if (!addr)
+        return;
+    regs->ah = inb(addr+5);
+    regs->al = inb(addr+6);
+    set_cf(regs, 0);
+}
+
+static void
+handle_14XX(struct bregs *regs)
+{
+    // Unsupported
+    set_cf(regs, 1);
+}
+
 // INT 14h Serial Communications Service Entry Point
 void VISIBLE
 handle_14(struct bregs *regs)
 {
     debug_enter(regs);
+
+    irq_enable();
+
+    switch (regs->ah) {
+    case 0x00: handle_1400(regs); break;
+    case 0x01: handle_1401(regs); break;
+    case 0x02: handle_1402(regs); break;
+    case 0x03: handle_1403(regs); break;
+    default:   handle_14XX(regs); break;
+    }
+    debug_exit(regs);
+}
+
+
+/****************************************************************
+ * LPT ports
+ ****************************************************************/
+
+static u16
+getLptAddr(struct bregs *regs)
+{
+    if (regs->dx >= 3) {
+        set_cf(regs, 1);
+        return 0;
+    }
+    u16 addr = GET_BDA(port_lpt[regs->dx]);
+    if (! addr)
+        set_cf(regs, 1);
+    return addr;
+}
+
+static void
+lpt_ret(struct bregs *regs, u16 addr, u16 timeout)
+{
+    u8 val8 = inb(addr+1);
+    regs->ah = (val8 ^ 0x48);
+    if (!timeout)
+        regs->ah |= 0x01;
+    set_cf(regs, 0);
+}
+
+// INT 17 - PRINTER - WRITE CHARACTER
+static void
+handle_1700(struct bregs *regs)
+{
+    u16 addr = getLptAddr(regs);
+    if (!addr)
+        return;
+    u16 timeout = GET_BDA(lpt_timeout[regs->dx]) << 8;
+
+    outb(regs->al, addr);
+    u8 val8 = inb(addr+2);
+    outb(val8 | 0x01, addr+2); // send strobe
+    nop();
+    outb(val8 & ~0x01, addr+2);
+    while (((inb(addr+1) & 0x40) == 0x40) && (timeout))
+        timeout--;
+
+    lpt_ret(regs, addr, timeout);
+}
+
+// INT 17 - PRINTER - INITIALIZE PORT
+static void
+handle_1701(struct bregs *regs)
+{
+    u16 addr = getLptAddr(regs);
+    if (!addr)
+        return;
+    u16 timeout = GET_BDA(lpt_timeout[regs->dx]) << 8;
+
+    u8 val8 = inb(addr+2);
+    outb(val8 & ~0x04, addr+2); // send init
+    nop();
+    outb(val8 | 0x04, addr+2);
+
+    lpt_ret(regs, addr, timeout);
+}
+
+// INT 17 - PRINTER - GET STATUS
+static void
+handle_1702(struct bregs *regs)
+{
+    u16 addr = getLptAddr(regs);
+    if (!addr)
+        return;
+    u16 timeout = GET_BDA(lpt_timeout[regs->dx]) << 8;
+
+    lpt_ret(regs, addr, timeout);
+}
+
+static void
+handle_17XX(struct bregs *regs)
+{
+    // Unsupported
+    set_cf(regs, 1);
 }
 
 // INT17h : Printer Service Entry Point
@@ -20,4 +219,14 @@ void VISIBLE
 handle_17(struct bregs *regs)
 {
     debug_enter(regs);
+
+    irq_enable();
+
+    switch (regs->ah) {
+    case 0x00: handle_1700(regs); break;
+    case 0x01: handle_1701(regs); break;
+    case 0x02: handle_1702(regs); break;
+    default:   handle_17XX(regs); break;
+    }
+    debug_exit(regs);
 }
diff --git a/src/system.c b/src/system.c
index 3967fc49..f54f9308 100644
--- a/src/system.c
+++ b/src/system.c
@@ -282,6 +282,13 @@ handle_1591(struct bregs *regs)
 {
 }
 
+// keyboard intercept
+static void
+handle_154f(struct bregs *regs)
+{
+    set_cf(regs, 1);
+}
+
 static void
 handle_15c0(struct bregs *regs)
 {
@@ -453,6 +460,7 @@ handle_15(struct bregs *regs)
         default:   handle_1524XX(regs); break;
         }
         break;
+    case 0x4f: handle_154f(regs); break;
     case 0x52: handle_1552(regs); break;
     case 0x83:
         switch (regs->al) {
@@ -526,4 +534,13 @@ void VISIBLE
 handle_75(struct bregs *regs)
 {
     debug_enter(regs);
+
+    // clear irq13
+    outb(0, PORT_MATH_CLEAR);
+    // clear interrupt
+    eoi_both_pics();
+    // legacy nmi call
+    struct bregs br;
+    memset(&br, 0, sizeof(br));
+    call16_int(0x02, &br);
 }
diff --git a/src/types.h b/src/types.h
index ea245bf9..2705dac4 100644
--- a/src/types.h
+++ b/src/types.h
@@ -17,5 +17,8 @@ typedef u32 size_t;
 #define VISIBLE __attribute__((externally_visible))
 
 #define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER)
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
+
+#define NULL ((void *)0)
 
 #endif // types.h
diff --git a/src/util.h b/src/util.h
index d83820e4..d33f2acc 100644
--- a/src/util.h
+++ b/src/util.h
@@ -5,6 +5,7 @@
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
 #include "ioport.h" // outb
+#include "biosvar.h" // struct bregs
 
 static inline void irq_disable(void) {
         asm volatile("cli": : :"memory");
@@ -27,6 +28,11 @@ static inline void irq_restore(unsigned long flags)
     asm volatile("pushl %0 ; popfl" : : "g" (flags) : "memory", "cc");
 }
 
+static inline void nop(void)
+{
+    asm volatile("nop");
+}
+
 #define DEBUGF(fmt, args...)
 #define BX_PANIC(fmt, args...)
 #define BX_INFO(fmt, args...)
@@ -35,7 +41,7 @@ static inline void
 memset(void *s, int c, size_t n)
 {
     while (n)
-        ((char *)s)[n--] = c;
+        ((char *)s)[--n] = c;
 }
 
 static inline void
@@ -51,6 +57,37 @@ eoi_both_pics()
     eoi_master_pic();
 }
 
+static inline
+void call16(struct bregs *callregs)
+{
+    asm volatile(
+        "pushfl\n"   // Save flags
+        "pushal\n"   // Save registers
+        "calll __call16\n"
+        "popal\n"
+        "popfl\n"
+        : : "a" (callregs), "m" (*callregs));
+}
+
+// XXX - this is ugly.
+#ifdef MODE16
+#define call16_int(nr, callregs) do {           \
+        struct bregs *__br = (callregs);        \
+        extern void irq_trampoline_ ##nr ();    \
+        __br->cs = 0xf000;                      \
+        __br->ip = (u16)&irq_trampoline_ ##nr;  \
+        call16(__br);                           \
+    } while (0)
+#else
+#include "../out/rom16.offset.auto.h"
+#define call16_int(nr, callregs) do {           \
+        struct bregs *__br = (callregs);        \
+        __br->cs = 0xf000;                      \
+        __br->ip = OFFSET_irq_trampoline_ ##nr; \
+        call16(__br);                           \
+    } while (0)
+#endif
+
 // output.c
 void bprintf(u16 action, const char *fmt, ...)
     __attribute__ ((format (printf, 2, 3)));
@@ -62,7 +99,7 @@ void __debug_exit(const char *fname, struct bregs *regs);
 #define debug_exit(regs) \
     __debug_exit(__func__, regs)
 #define printf(fmt, args...)                     \
-    bprintf(0, fmt , ##args )
+    bprintf(1, fmt , ##args )
 
 // kbd.c
 void handle_15c2(struct bregs *regs);
diff --git a/tools/buildrom.py b/tools/buildrom.py
index beb51ac2..c82bb2b4 100755
--- a/tools/buildrom.py
+++ b/tools/buildrom.py
@@ -64,11 +64,6 @@ def main():
     start32 = int(o32['OFFSET__start'], 16)
     outrom = alteraddr(outrom, jmppos+2, start32)
 
-    # Fixup resume from 16 jump to 32 bit code
-    jmppos = int(o16['OFFSET_set_resume32'], 16)
-    resume32 = int(o32['OFFSET_call16_resume'], 16)
-    outrom = alteraddr(outrom, jmppos+2, resume32)
-
     # Write output rom
     f = open(OUT, 'wb')
     f.write(outrom)