[comboot] Implement INT 22h AX=001Bh (Cleanup, shuffle, and boot to real mode)

2 files changed, 95 insertions, 1 deletions

diff --git a/src/arch/i386/include/comboot.h b/src/arch/i386/include/comboot.h
index b5b1f8a0..56661a80 100644
--- a/src/arch/i386/include/comboot.h
+++ b/src/arch/i386/include/comboot.h
@@ -58,6 +58,40 @@ typedef struct {
 } com32sys_t;
 
 typedef struct {
+	uint32_t eax;               /* Offset  0 */
+	uint32_t ecx;               /* Offset  4 */
+	uint32_t edx;               /* Offset  8 */
+	uint32_t ebx;               /* Offset 12 */
+	uint32_t esp;               /* Offset 16 */
+	uint32_t ebp;               /* Offset 20 */
+	uint32_t esi;               /* Offset 24 */
+	uint32_t edi;               /* Offset 28 */
+
+	uint32_t eip;               /* Offset 32 */
+} syslinux_pm_regs;
+
+typedef struct {
+	uint16_t es;                /* Offset  0 */
+	uint16_t _unused_cs;        /* Offset  2 */
+	uint16_t ds;                /* Offset  4 */
+	uint16_t ss;                /* Offset  6 */
+	uint16_t fs;                /* Offset  8 */
+	uint16_t gs;                /* Offset 10 */
+
+	uint32_t eax;                /* Offset 12 */
+	uint32_t ecx;                /* Offset 16 */
+	uint32_t edx;                /* Offset 20 */
+	uint32_t ebx;                /* Offset 24 */
+	uint32_t esp;                /* Offset 28 */
+	uint32_t ebp;                /* Offset 32 */
+	uint32_t esi;                /* Offset 36 */
+	uint32_t edi;                /* Offset 40 */
+
+	uint16_t ip;                /* Offset 44 */
+	uint16_t cs;                /* Offset 46 */
+} syslinux_rm_regs;
+
+typedef struct {
 	uint32_t dest;
 	uint32_t src;
 	uint32_t len;
diff --git a/src/arch/i386/interface/syslinux/comboot_call.c b/src/arch/i386/interface/syslinux/comboot_call.c
index 290d94ac..bf6c4c66 100644
--- a/src/arch/i386/interface/syslinux/comboot_call.c
+++ b/src/arch/i386/interface/syslinux/comboot_call.c
@@ -53,6 +53,14 @@ static char __data16_array ( syslinux_configuration_file, [] ) = "";
 static uint8_t __data16 ( comboot_feature_flags ) = COMBOOT_FEATURE_IDLE_LOOP;
 #define comboot_feature_flags __use_data16 ( comboot_feature_flags )
 
+typedef union {
+	syslinux_pm_regs pm; syslinux_rm_regs rm;
+} syslinux_regs;
+
+/** Initial register values for INT 22h AX=1Ah and 1Bh */
+static syslinux_regs __text16 ( comboot_initial_regs );
+#define comboot_initial_regs __use_text16 ( comboot_initial_regs )
+
 static struct segoff __text16 ( int20_vector );
 #define int20_vector __use_text16 ( int20_vector )
 
@@ -316,7 +324,7 @@ static __asmcall void int22 ( struct i386_all_regs *ix86 ) {
 	case 0x0001: /* Get Version */
 
 		/* Number of INT 22h API functions available */
-		ix86->regs.ax = 0x0018;
+		ix86->regs.ax = 0x001B;
 
 		/* SYSLINUX version number */
 		ix86->regs.ch = 0; /* major */
@@ -569,6 +577,58 @@ static __asmcall void int22 ( struct i386_all_regs *ix86 ) {
 		ix86->flags &= ~CF;
 		break;
 
+	case 0x001B: /* Cleanup, shuffle and boot to real mode */
+		if ( ix86->regs.cx > COMBOOT_MAX_SHUFFLE_DESCRIPTORS )
+			break;
+
+		/* Perform final cleanup */
+		shutdown ( SHUTDOWN_BOOT );
+
+		/* Perform sequence of copies */
+		shuffle ( ix86->segs.es, ix86->regs.di, ix86->regs.cx );
+
+		/* Copy initial register values to .text16 */
+		memcpy_user ( real_to_user ( rm_cs, (unsigned) __from_text16 ( &comboot_initial_regs ) ), 0,
+		              real_to_user ( ix86->segs.ds, ix86->regs.si ), 0,
+		              sizeof(syslinux_rm_regs) );
+
+		/* Load initial register values */
+		__asm__ __volatile__ (
+			REAL_CODE (
+				/* Point SS:SP at the register value structure */
+				"pushw %%cs\n\t"
+				"popw %%ss\n\t"
+				"movw $comboot_initial_regs, %%sp\n\t"
+
+				/* Segment registers */
+				"popw %%es\n\t"
+				"popw %%ax\n\t" /* Skip CS */
+				"popw %%ds\n\t"
+				"popw %%ax\n\t" /* Skip SS for now */
+				"popw %%fs\n\t"
+				"popw %%gs\n\t"
+
+				/* GP registers */
+				"popl %%eax\n\t"
+				"popl %%ecx\n\t"
+				"popl %%edx\n\t"
+				"popl %%ebx\n\t"
+				"popl %%ebp\n\t" /* Skip ESP for now */
+				"popl %%ebp\n\t"
+				"popl %%esi\n\t"
+				"popl %%edi\n\t"
+
+				/* Load correct SS:ESP */
+				"movw $(comboot_initial_regs + 6), %%sp\n\t"
+				"popw %%ss\n\t"
+				"movl %%cs:(comboot_initial_regs + 28), %%esp\n\t"
+
+				"ljmp *%%cs:(comboot_initial_regs + 44)\n\t"
+			)
+			: : );
+
+		break;
+
 	default:
 		DBG ( "COMBOOT unknown int22 function %04x\n", ix86->regs.ax );
 		break;