Merge branch 'master' into memdisk-iso
diff --git a/doc/memdisk.txt b/doc/memdisk.txt
index 58ec748..8a308f1 100644
--- a/doc/memdisk.txt
+++ b/doc/memdisk.txt
@@ -75,6 +75,7 @@
    s=#		Specify number of sectors (max 63)
    floppy[=#]	The image is a floppy image[**]
    harddisk[=#]	The image is a hard disk image[**]
+   iso		The image is an El Torito ISO9660 image (drive 0xE0)
 
    # represents a decimal number.
 
diff --git a/memdisk/Makefile b/memdisk/Makefile
index d185d87..09f17c8 100644
--- a/memdisk/Makefile
+++ b/memdisk/Makefile
@@ -38,11 +38,15 @@
 # Important: init.o16 must be first!!
 OBJS16   = init.o16 init32.o
 OBJS32   = start32.o setup.o msetup.o e820func.o conio.o memcpy.o memset.o \
-	   memmove.o unzip.o memdisk_chs.o memdisk_edd.o
+	   memmove.o unzip.o dskprobe.o eltorito.o \
+	   memdisk_chs_512.o memdisk_edd_512.o \
+	   memdisk_iso_512.o memdisk_iso_2048.o
 
-CSRC     = setup.c msetup.c e820func.c conio.c unzip.c
+CSRC     = setup.c msetup.c e820func.c conio.c unzip.c dskprobe.c eltorito.c
 SSRC     = start32.S memcpy.S memset.S memmove.S
-NASMSRC  = memdisk_chs.asm memdisk_edd.asm memdisk16.asm
+NASMSRC  = memdisk_chs_512.asm memdisk_edd_512.asm \
+	   memdisk_iso_512.asm memdisk_iso_2048.asm \
+	   memdisk16.asm
 
 all: memdisk # e820test
 
diff --git a/memdisk/bda.h b/memdisk/bda.h
new file mode 100644
index 0000000..cfac441
--- /dev/null
+++ b/memdisk/bda.h
@@ -0,0 +1,56 @@
+/* ----------------------------------------------------------------------- *
+ *
+ *   Copyright 2001-2009 H. Peter Anvin - All Rights Reserved
+ *   Copyright 2009 Intel Corporation; author: H. Peter Anvin
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation, Inc., 53 Temple Place Ste 330,
+ *   Boston MA 02111-1307, USA; either version 2 of the License, or
+ *   (at your option) any later version; incorporated herein by reference.
+ *
+ * ----------------------------------------------------------------------- */
+
+#include <stdint.h>
+
+/* Addresses in the zero page */
+#define BIOS_INT13	(0x13*4)	/* INT 13h vector */
+#define BIOS_INT15	(0x15*4)	/* INT 15h vector */
+#define BIOS_INT1E	(0x1E*4)	/* INT 1Eh vector */
+#define BIOS_INT40	(0x40*4)	/* INT 13h vector */
+#define BIOS_INT41	(0x41*4)	/* INT 41h vector */
+#define BIOS_INT46	(0x46*4)	/* INT 46h vector */
+#define BIOS_BASEMEM	0x413		/* Amount of DOS memory */
+#define BIOS_EQUIP	0x410		/* BIOS equipment list */
+#define BIOS_HD_COUNT	0x475		/* Number of hard drives present */
+
+/* Access to objects in the zero page */
+static inline void wrz_8(uint32_t addr, uint8_t data)
+{
+    *((uint8_t *) addr) = data;
+}
+
+static inline void wrz_16(uint32_t addr, uint16_t data)
+{
+    *((uint16_t *) addr) = data;
+}
+
+static inline void wrz_32(uint32_t addr, uint32_t data)
+{
+    *((uint32_t *) addr) = data;
+}
+
+static inline uint8_t rdz_8(uint32_t addr)
+{
+    return *((uint8_t *) addr);
+}
+
+static inline uint16_t rdz_16(uint32_t addr)
+{
+    return *((uint16_t *) addr);
+}
+
+static inline uint32_t rdz_32(uint32_t addr)
+{
+    return *((uint32_t *) addr);
+}
diff --git a/memdisk/dskprobe.c b/memdisk/dskprobe.c
new file mode 100644
index 0000000..de858bb
--- /dev/null
+++ b/memdisk/dskprobe.c
@@ -0,0 +1,114 @@
+/* ----------------------------------------------------------------------- *
+ *
+ *   Copyright 2009 Shao Miller - All Rights Reserved
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation, Inc., 53 Temple Place Ste 330,
+ *   Boston MA 02111-1307, USA; either version 2 of the License, or
+ *   (at your option) any later version; incorporated herein by reference.
+ *
+ * ----------------------------------------------------------------------- */
+
+/*
+ * dskprobe.c
+ *
+ * Routines for probing BIOS disk drives
+ */
+
+/*
+ * Uncomment for debugging
+ *
+ * #define DBG_DSKPROBE 1
+ */
+
+#include <stdint.h>
+#include "memdisk.h"
+#include "bda.h"
+#include "conio.h"
+
+/*
+ * We will probe a BIOS drive numer using INT 13h, AH=probe
+ * and will pass along that call's success or failure
+ */
+int probe_int13_ah(uint8_t drive, uint8_t probe)
+{
+    int err;
+    com32sys_t regs;
+
+    memset(&regs, 0, sizeof regs);
+
+    regs.eax.b[1] = probe;	/* AH = probe                 */
+    regs.edx.b[0] = drive;	/* DL = drive number to probe */
+    intcall(0x13, &regs, &regs);
+
+    err = !(regs.eflags.l & 1);
+#ifdef DBG_DSKPROBE
+    printf("probe_int13_ah(0x%02x, 0x%02x) == %d\n", drive, probe, err);
+#endif
+    return err;
+}
+
+/*
+ * We will probe the BIOS Data Area and count the drives found there.
+ * This heuristic then assumes that all drives of 'drive's type are
+ * found in a contiguous range, and returns 1 if the probed drive
+ * is less than or equal to the BDA count.
+ * This particular function's code is derived from code in setup.c by
+ * H. Peter Anvin.  Please respect that file's copyright for this function
+ */
+int probe_bda_drive(uint8_t drive)
+{
+    int bios_drives;
+    int err;
+
+    if (drive & 0x80) {
+	bios_drives = rdz_8(BIOS_HD_COUNT);	/* HDD count */
+    } else {
+	uint8_t equip = rdz_8(BIOS_EQUIP);
+	if (equip & 1)
+	    bios_drives = (equip >> 6) + 1;	/* Floppy count */
+	else
+	    bios_drives = 0;
+    }
+    err = (drive - (drive & 0x80)) >= bios_drives ? 0 : 1;
+#ifdef DBG_DSKPROBE
+    printf("probe_bda_drive(0x%02x) == %d, count: %d\n",
+	   drive, err, bios_drives);
+#endif
+    return err;
+}
+
+/*
+ * We will probe a drive with a few different methods, returning
+ * the count of succesful probes
+ */
+int probe_drive(uint8_t drive)
+{
+    int c = 0;
+    /* Only probe the BDA for floppies */
+    if (drive & 0x80) {
+	c += probe_int13_ah(drive, 0x08);
+	c += probe_int13_ah(drive, 0x15);
+	c += probe_int13_ah(drive, 0x41);
+    }
+    c += probe_bda_drive(drive);
+    return c;
+}
+
+/*
+ * We will probe a contiguous range of BIOS drive, starting with drive
+ * number 'start'.  We probe with a few different methods, and return
+ * the first drive which doesn't respond to any of the probes.
+ */
+uint8_t probe_drive_range(uint8_t start)
+{
+    uint8_t drive = start;
+    while (probe_drive(drive)) {
+	drive++;
+	/* Check for passing the floppy/HDD boundary */
+	if ((drive & 0x7F) == 0)
+	    break;
+    }
+    return drive;
+}
diff --git a/memdisk/dskprobe.h b/memdisk/dskprobe.h
new file mode 100644
index 0000000..99bfa66
--- /dev/null
+++ b/memdisk/dskprobe.h
@@ -0,0 +1,21 @@
+/* ----------------------------------------------------------------------- *
+ *
+ *   Copyright 2009 Shao Miller - All Rights Reserved
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation, Inc., 53 Temple Place Ste 330,
+ *   Boston MA 02111-1307, USA; either version 2 of the License, or
+ *   (at your option) any later version; incorporated herein by reference.
+ *
+ * ----------------------------------------------------------------------- */
+
+/*
+ * dskprobe.h
+ *
+ * Routines for probing BIOS disk drives
+ */
+
+#include <stdint.h>
+
+extern uint8_t probe_drive_range(uint8_t);
diff --git a/memdisk/eltorito.c b/memdisk/eltorito.c
new file mode 100644
index 0000000..7e0ba89
--- /dev/null
+++ b/memdisk/eltorito.c
@@ -0,0 +1,58 @@
+/* ----------------------------------------------------------------------- *
+ *
+ *   Copyright 2009 Shao Miller - All Rights Reserved
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation, Inc., 53 Temple Place Ste 330,
+ *   Boston MA 02111-1307, USA; either version 2 of the License, or
+ *   (at your option) any later version; incorporated herein by reference.
+ *
+ * ----------------------------------------------------------------------- */
+
+/*
+ * eltorito.c
+ *
+ * EDD-4 El Torito structures and debugging routines
+ */
+
+#include <stdint.h>
+#include "memdisk.h"
+#include "conio.h"
+#include "eltorito.h"
+
+#ifdef DBG_ELTORITO
+void eltorito_dump(uint32_t image)
+{
+    printf("-- El Torito dump --\n", image);
+
+    /* BVD starts at sector 17. */
+    struct edd4_bvd *bvd = (struct edd4_bvd *)(image + 17 * 2048);
+
+    printf("bvd.boot_rec_ind: 0x%02x\n", bvd->boot_rec_ind);
+    printf("bvd.iso9660_id: %c%c%c%c%c\n", bvd->iso9660_id[0],
+	   bvd->iso9660_id[1], bvd->iso9660_id[2], bvd->iso9660_id[3],
+	   bvd->iso9660_id[4]);
+    printf("bvd.ver: 0x%02x\n", bvd->ver);
+    printf("bvd.eltorito: %s\n", bvd->eltorito);
+    printf("bvd.boot_cat: 0x%08x\n", bvd->boot_cat);
+
+    struct edd4_bootcat *boot_cat =
+	(struct edd4_bootcat *)(image + bvd->boot_cat * 2048);
+
+    printf("boot_cat.validation_entry\n");
+    printf("  .header_id: 0x%02x\n", boot_cat->validation_entry.header_id);
+    printf("  .platform_id: 0x%02x\n", boot_cat->validation_entry.platform_id);
+    printf("  .id_string: %s\n", boot_cat->validation_entry.id_string);
+    printf("  .checksum: 0x%04x\n", boot_cat->validation_entry.checksum);
+    printf("  .key55: 0x%02x\n", boot_cat->validation_entry.key55);
+    printf("  .keyAA: 0x%02x\n", boot_cat->validation_entry.keyAA);
+    printf("boot_cat.initial_entry\n");
+    printf("  .header_id: 0x%02x\n", boot_cat->initial_entry.header_id);
+    printf("  .media_type: 0x%02x\n", boot_cat->initial_entry.media_type);
+    printf("  .load_seg: 0x%04x\n", boot_cat->initial_entry.load_seg);
+    printf("  .system_type: 0x%02x\n", boot_cat->initial_entry.system_type);
+    printf("  .sect_count: %d\n", boot_cat->initial_entry.sect_count);
+    printf("  .load_block: 0x%08x\n", boot_cat->initial_entry.load_block);
+}
+#endif
diff --git a/memdisk/eltorito.h b/memdisk/eltorito.h
new file mode 100644
index 0000000..7d46e1d
--- /dev/null
+++ b/memdisk/eltorito.h
@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------- *
+ *
+ *   Copyright 2009 Shao Miller - All Rights Reserved
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation, Inc., 53 Temple Place Ste 330,
+ *   Boston MA 02111-1307, USA; either version 2 of the License, or
+ *   (at your option) any later version; incorporated herein by reference.
+ *
+ * ----------------------------------------------------------------------- */
+
+/*
+ * eltorito.h
+ *
+ * EDD-4 El Torito structures and debugging routines
+ */
+
+#include <stdint.h>
+
+/*
+ * Uncomment for El Torito debugging
+ *
+ * #define DBG_ELTORITO 1
+ */
+
+#ifdef DBG_ELTORITO
+extern void eltorito_dump(uint32_t);
+#endif
+
+/* EDD-4 Bootable Optical Disc Drive Boot Volume Descriptor */
+struct edd4_bvd {
+    uint8_t boot_rec_ind;	/* Boot Record Indicator */
+    uint8_t iso9660_id[5];	/* ISO9660 ID            */
+    uint8_t ver;		/* Descriptor Version    */
+    uint8_t eltorito[32];	/* "EL TORITO" etc.      */
+    uint8_t res1[32];		/* Reserved              */
+    uint32_t boot_cat;		/* Boot catalog sector   */
+    uint8_t res2[1973];		/* Reserved              */
+} __attribute__ ((packed));
+
+struct validation_entry {
+    uint8_t header_id;		/* Header ID                      */
+    uint8_t platform_id;	/* Platform ID                    */
+    uint16_t res1;		/* Reserved                       */
+    uint8_t id_string[24];	/* Manufacturer                   */
+    uint16_t checksum;		/* Sums with whole record to zero */
+    uint8_t key55;		/* Key byte 0x55                  */
+    uint8_t keyAA;		/* Key byte 0xAA                  */
+} __attribute__ ((packed));
+
+struct initial_entry {
+    uint8_t header_id;		/* Header ID                */
+    uint8_t media_type;		/* Media type               */
+    uint16_t load_seg;		/* Load segment             */
+    uint8_t system_type;	/* (Filesystem ID)          */
+    uint8_t res1;		/* Reserved                 */
+    uint16_t sect_count;	/* Emulated sectors to load */
+    uint32_t load_block;	/* Starting sector of image */
+    uint8_t res2[4];		/* Reserved                 */
+} __attribute__ ((packed));
+
+/* EDD-4 Bootable Optical Disc Drive Boot Catalog (fixed-size portions) */
+struct edd4_bootcat {
+    struct validation_entry validation_entry;
+    struct initial_entry initial_entry;
+} __attribute__ ((packed));
+
+/* EDD-4 CD Specification Packet */
+struct edd4_cd_pkt {
+    uint8_t size;		/* Packet size                     */
+    uint8_t type;		/* Boot media type (flags)         */
+    uint8_t driveno;		/* INT 13h drive number            */
+    uint8_t controller;		/* Controller index                */
+    uint32_t start;		/* Starting LBA of image           */
+    uint16_t devno;		/* Device number                   */
+    uint16_t userbuf;		/* User buffer segment             */
+    uint16_t load_seg;		/* Load segment                    */
+    uint16_t sect_count;	/* Emulated sectors to load        */
+    uint8_t geom1;		/* Cylinders bits 0 thru 7         */
+    uint8_t geom2;		/* Sects/track 0 thru 5, cyls 8, 9 */
+    uint8_t geom3;		/* Heads                           */
+} __attribute__ ((packed));
diff --git a/memdisk/memdisk.inc b/memdisk/memdisk.inc
index 3c79b62..98ad52b 100644
--- a/memdisk/memdisk.inc
+++ b/memdisk/memdisk.inc
@@ -8,6 +8,7 @@
 ;
 ;   Copyright 2001-2009 H. Peter Anvin - All Rights Reserved
 ;   Copyright 2009 Intel Corporation; author: H. Peter Anvin
+;   Portions copyright 2009 Shao Miller [El Torito code]
 ;
 ;  This program is free software; you can redistribute it and/or modify
 ;  it under the terms of the GNU General Public License as published by
@@ -79,7 +80,6 @@
 
 		org 0h
 
-%define SECTORSIZE_LG2	9		; log2(sector size)
 %define	SECTORSIZE	(1 << SECTORSIZE_LG2)
 
 		; Parameter registers definition; this is the definition
@@ -136,6 +136,10 @@
 		mov ss,ax
 		mov sp,[cs:MyStack]
 
+%if ELTORITO
+		cmp word [cs:SavedAX],4a00h	; El Torito function?
+		jae our_drive			; We grab it
+%endif
 		; See if DL points to our class of device (FD, HD)
 		push dx
 		push dx
@@ -147,6 +151,12 @@
 		jz our_drive		; If ZF=1, we have an exact match
 		cmp dl,[cs:DriveNo]
 		jb .nomatch		; Drive < Our drive
+		cmp dl,[cs:DriveShiftLimit]
+		jae .nomatch		; Drive > The maximum drive
+					; number that we will shift for.
+					; This leaves any higher-up BIOS
+					; drives alone, such as an optical
+					; disc drive 0xA0 or 0xE0
 		dec dl			; Drive > Our drive, adjust drive #
 .nomatch:
 		TRACER '!'
@@ -200,6 +210,14 @@
 		; Note: AX == P_AX here
 		cmp ah,Int13FuncsCnt-1
 		ja Invalid_jump
+%if ELTORITO
+		mov al,[CD_PKT.type]	; Check if we are in
+		cmp al,0		; El Torito no emulation mode
+		ja .emulation		; No.  We support the function
+		cmp ah,3fh		; Yes.  We must not support functions
+		jbe Invalid_jump	; 0 through 3Fh.  Check and decide
+.emulation:
+%endif
 		xor al,al		; AL = 0 is standard entry condition
 		mov di,ax
 		shr di,7		; Convert AH to an offset in DI
@@ -388,8 +406,9 @@
 		jne Invalid
 		mov P_BX,0AA55h		; EDD signature
 		mov P_AX,03000h		; EDD 3.0
-		mov P_CX,0003h		; Bit 0 - Fixed disk access subset
+		mov P_CX,0007h		; Bit 0 - Fixed disk access subset
 					; Bit 1 - Locking and ejecting subset
+					; Bit 2 - EDD subset
 		pop ax			; Drop return address
 		xor ax,ax		; Success
 		jmp DoneWeird		; Success, but AH != 0, sigh...
@@ -570,10 +589,31 @@
 EDDEject:
 		mov ax,0B200h		; Volume Not Removable
 		ret
-
+%if ELTORITO
+ElToritoTerminate:
+		TRACER 'T'
+		mov ax,[cs:SavedAX]
+		cmp al,1		; We only support query, not terminate
+		jne ElToritoErr		; Fail
+		mov es,P_DS		; Caller's DS:SI pointed to packet
+		mov di,P_SI		; We'll use ES:DI
+		mov si,CD_PKT.size	; First byte is packet size
+		xor cx,0		; Empty our count
+		;mov cl,[ds:si]		; We'll copy that many bytes
+		mov cl,13h
+		rep movsb		; Copy until CX is zero
+		mov ax,0		; Success
+		ret
+ElToritoEmulate:
+ElToritoBoot:
+ElToritoCatalog:
+ElToritoErr:
+		TRACER '!'
+		mov ax,100h		; Invalid parameter
+		ret
+%endif ; ELTORITO
 %endif ; EDD
 
-
 ;
 ; INT 15h intercept routines
 ;
@@ -955,7 +995,14 @@
 		dw EDDSeek		; 47h - EDD SEEK
 		dw EDDGetParms		; 48h - EDD GET PARAMETERS
 		dw EDDDetectChange	; 49h - EDD MEDIA CHANGE STATUS
-%endif
+%if ELTORITO				; EDD El Torito Functions
+					; ELTORITO _must_ also have EDD
+		dw ElToritoEmulate	; 4Ah - Initiate Disk Emulation
+		dw ElToritoTerminate	; 4Bh - Terminate Disk Emulation
+		dw ElToritoBoot		; 4Ch - Initiate Disk Emu. and Reboot
+		dw ElToritoCatalog	; 4Dh - Return Boot Catalog
+%endif ; ELTORITO
+%endif ; EDD
 
 Int13FuncsEnd	equ $
 Int13FuncsCnt	equ (Int13FuncsEnd-Int13Funcs) >> 1
@@ -1013,6 +1060,13 @@
 MDI_Len		equ $-MemDisk_Info
 
 ; ---- MDI structure ends here ---
+DriveShiftLimit	db 0ffh			; Installer will [soon] probe for
+					; a range of contiguous drives.
+					; Any BIOS drives above this region
+					; shall not be impacted by our
+					; shifting behaviour
+		db 0			; pad to a DWORD
+		dw 0			; pad to a QWORD
 MemInt1588	dw 0			; 1MB-65MB memory amount (1K)
 
 Cylinders	dw 0			; Cylinder count
@@ -1058,7 +1112,24 @@
 .res3		db 0			; Reserved
 .chksum		db 0			; DPI checksum
 
-%endif
+%if ELTORITO
+; El Torito CD Specification Packet - mostly filled in by installer
+CD_PKT:
+.size		db 13h	; Packet size (19 bytes)
+.type		db 0	; Boot media type (flags)
+.driveno	db 0E0h	; INT 13h drive number
+.controller	db 0	; Controller index
+.start		dd 0	; Starting LBA of image
+.devno		dw 0	; Device number
+.user_buf	dw 0	; User buffer segment
+.load_seg	dw 0	; Load segment
+.sect_count	dw 0	; Emulated sectors to load
+.geom1		db 0	; Cylinders bits 0 thru 7
+.geom2		db 0	; Sects/track 0 thru 5, cyls 8, 9
+.geom3		db 0	; Heads
+%endif ; ELTORITO
+
+%endif ; EDD
 
 		; End patch area
 		alignb 4, db 0
diff --git a/memdisk/memdisk_chs.asm b/memdisk/memdisk_chs.asm
deleted file mode 100644
index 94dad9d..0000000
--- a/memdisk/memdisk_chs.asm
+++ /dev/null
@@ -1,3 +0,0 @@
-	[map all memdisk_chs.map]
-%define EDD 0
-%include "memdisk.inc"
diff --git a/memdisk/memdisk_chs_512.asm b/memdisk/memdisk_chs_512.asm
new file mode 100644
index 0000000..bb436f3
--- /dev/null
+++ b/memdisk/memdisk_chs_512.asm
@@ -0,0 +1,5 @@
+	[map all memdisk_chs_512.map]
+%define EDD 0
+%define ELTORITO 0
+%define SECTORSIZE_LG2 9	; log2(sector size)
+%include "memdisk.inc"
diff --git a/memdisk/memdisk_edd.asm b/memdisk/memdisk_edd.asm
deleted file mode 100644
index 6f909d7..0000000
--- a/memdisk/memdisk_edd.asm
+++ /dev/null
@@ -1,3 +0,0 @@
-	[map all memdisk_edd.map]
-%define EDD 1
-%include "memdisk.inc"
diff --git a/memdisk/memdisk_edd_512.asm b/memdisk/memdisk_edd_512.asm
new file mode 100644
index 0000000..3a6d5ca
--- /dev/null
+++ b/memdisk/memdisk_edd_512.asm
@@ -0,0 +1,5 @@
+	[map all memdisk_edd_512.map]
+%define EDD 1
+%define ELTORITO 0
+%define SECTORSIZE_LG2 9	; log2(sector size)
+%include "memdisk.inc"
diff --git a/memdisk/memdisk_iso_2048.asm b/memdisk/memdisk_iso_2048.asm
new file mode 100644
index 0000000..0c8ffee
--- /dev/null
+++ b/memdisk/memdisk_iso_2048.asm
@@ -0,0 +1,5 @@
+	[map all memdisk_iso_2048.map]
+%define EDD 1
+%define ELTORITO 1
+%define SECTORSIZE_LG2 11	; log2(sector size)
+%include "memdisk.inc"
diff --git a/memdisk/memdisk_iso_512.asm b/memdisk/memdisk_iso_512.asm
new file mode 100644
index 0000000..1555b77
--- /dev/null
+++ b/memdisk/memdisk_iso_512.asm
@@ -0,0 +1,5 @@
+	[map all memdisk_iso_512.map]
+%define EDD 1
+%define ELTORITO 1
+%define SECTORSIZE_LG2 9	; log2(sector size)
+%include "memdisk.inc"
diff --git a/memdisk/setup.c b/memdisk/setup.c
index 5278169..40f3f8c 100644
--- a/memdisk/setup.c
+++ b/memdisk/setup.c
@@ -2,6 +2,7 @@
  *
  *   Copyright 2001-2009 H. Peter Anvin - All Rights Reserved
  *   Copyright 2009 Intel Corporation; author: H. Peter Anvin
+ *   Portions copyright 2009 Shao Miller [El Torito code]
  *
  *   This program is free software; you can redistribute it and/or modify
  *   it under the terms of the GNU General Public License as published by
@@ -12,7 +13,10 @@
  * ----------------------------------------------------------------------- */
 
 #include <stdint.h>
+#include "bda.h"
+#include "dskprobe.h"
 #include "e820.h"
+#include "eltorito.h"
 #include "conio.h"
 #include "version.h"
 #include "memdisk.h"
@@ -22,12 +26,18 @@
 const char copyright[] =
     "Copyright " FIRSTYEAR "-" YEAR_STR " H. Peter Anvin et al";
 
-extern const char _binary_memdisk_chs_bin_start[];
-extern const char _binary_memdisk_chs_bin_end[];
-extern const char _binary_memdisk_chs_bin_size[];
-extern const char _binary_memdisk_edd_bin_start[];
-extern const char _binary_memdisk_edd_bin_end[];
-extern const char _binary_memdisk_edd_bin_size[];
+extern const char _binary_memdisk_chs_512_bin_start[];
+extern const char _binary_memdisk_chs_512_bin_end[];
+extern const char _binary_memdisk_chs_512_bin_size[];
+extern const char _binary_memdisk_edd_512_bin_start[];
+extern const char _binary_memdisk_edd_512_bin_end[];
+extern const char _binary_memdisk_edd_512_bin_size[];
+extern const char _binary_memdisk_iso_512_bin_start[];
+extern const char _binary_memdisk_iso_512_bin_end[];
+extern const char _binary_memdisk_iso_512_bin_size[];
+extern const char _binary_memdisk_iso_2048_bin_start[];
+extern const char _binary_memdisk_iso_2048_bin_end[];
+extern const char _binary_memdisk_iso_2048_bin_size[];
 
 struct memdisk_header {
     uint16_t int13_offs;
@@ -106,6 +116,10 @@
 
     uint16_t dpt_ptr;
     /* End of the official MemDisk_Info */
+    uint8_t driveshiftlimit;	/* Do not shift drives above this region */
+    uint8_t _pad2;		/* Pad to DWORD */
+    uint16_t _pad3;		/* Pad to QWORD */
+
     uint16_t memint1588;
 
     uint16_t cylinders;
@@ -131,51 +145,18 @@
 
     dpt_t dpt;
     struct edd_dpt edd_dpt;
+    struct edd4_cd_pkt cd_pkt; /* Only really in a memdisk_iso_* hook */
 } __attribute__((packed));
 
-/* Access to high memory */
-
-/* Access to objects in the zero page */
-static inline void wrz_8(uint32_t addr, uint8_t data)
-{
-    *((uint8_t *) addr) = data;
-}
-
-static inline void wrz_16(uint32_t addr, uint16_t data)
-{
-    *((uint16_t *) addr) = data;
-}
-
-static inline void wrz_32(uint32_t addr, uint32_t data)
-{
-    *((uint32_t *) addr) = data;
-}
-
-static inline uint8_t rdz_8(uint32_t addr)
-{
-    return *((uint8_t *) addr);
-}
-
-static inline uint16_t rdz_16(uint32_t addr)
-{
-    return *((uint16_t *) addr);
-}
-
-static inline uint32_t rdz_32(uint32_t addr)
-{
-    return *((uint32_t *) addr);
-}
-
-/* Addresses in the zero page */
-#define BIOS_INT13	(0x13*4)	/* INT 13h vector */
-#define BIOS_INT15	(0x15*4)	/* INT 15h vector */
-#define BIOS_INT1E      (0x1E*4)	/* INT 1Eh vector */
-#define BIOS_INT40	(0x40*4)	/* INT 13h vector */
-#define BIOS_INT41      (0x41*4)	/* INT 41h vector */
-#define BIOS_INT46      (0x46*4)	/* INT 46h vector */
-#define BIOS_BASEMEM	0x413	/* Amount of DOS memory */
-#define BIOS_EQUIP	0x410	/* BIOS equipment list */
-#define BIOS_HD_COUNT   0x475	/* Number of hard drives present */
+/* An EDD disk packet */
+struct edd_dsk_pkt {
+    uint8_t size;		/* Packet size        */
+    uint8_t res1;		/* Reserved           */
+    uint16_t count;		/* Count to transfer  */
+    uint32_t buf;		/* Buffer pointer     */
+    uint64_t start;		/* LBA to start from  */
+    uint64_t buf64;		/* 64-bit buf pointer */
+} __attribute__ ((packed));
 
 /*
  * Routine to seek for a command-line item and return a pointer
@@ -339,7 +320,7 @@
 
 	if (!okmem)
 	    die("Not enough memory to decompress image (need 0x%08x bytes)\n",
-		 gzdatasize);
+		gzdatasize);
 
 	printf("gzip image: decompressed addr 0x%08x, len 0x%08x: ",
 	       target, gzdatasize);
@@ -354,11 +335,12 @@
  * Figure out the "geometry" of the disk in question
  */
 struct geometry {
-    uint32_t sectors;		/* 512-byte sector count */
+    uint32_t sectors;		/* Sector count */
     uint32_t c, h, s;		/* C/H/S geometry */
     uint32_t offset;		/* Byte offset for disk */
     uint8_t type;		/* Type byte for INT 13h AH=08h */
     uint8_t driveno;		/* Drive no */
+    uint16_t sector_size;	/* Sector size in bytes (512 vs. 2048) */
     const char *hsrc, *ssrc;	/* Origins of H and S geometries */
 };
 
@@ -426,7 +408,8 @@
 
 #define FOUR(a,b,c,d) (((a) << 24)|((b) << 16)|((c) << 8)|(d))
 
-static const struct geometry *get_disk_image_geometry(uint32_t where, uint32_t size)
+static const struct geometry *get_disk_image_geometry(uint32_t where,
+						      uint32_t size)
 {
     static struct geometry hd_geometry;
     struct dosemu_header dosemu;
@@ -437,6 +420,8 @@
 
     printf("command line: %s\n", shdr->cmdline);
 
+    hd_geometry.sector_size = 512;	/* Assume floppy/HDD at first */
+
     offset = 0;
     if (CMD_HASDATA(p = getcmditem("offset")) && (v = atou(p)))
 	offset = v;
@@ -448,6 +433,71 @@
     hd_geometry.sectors = sectors;
     hd_geometry.offset = offset;
 
+    if ((p = getcmditem("iso")) != CMD_NOTFOUND) {
+#ifdef DBG_ELTORITO
+	eltorito_dump(where);
+#endif
+	struct edd4_bvd *bvd = (struct edd4_bvd *)(where + 17 * 2048);
+	/* Tiny sanity check */
+	if ((bvd->boot_rec_ind != 0) || (bvd->ver != 1))
+	    printf("El Torito BVD sanity check failed.\n");
+	struct edd4_bootcat *boot_cat =
+	    (struct edd4_bootcat *)(where + bvd->boot_cat * 2048);
+	/* Another tiny sanity check */
+	if ((boot_cat->validation_entry.platform_id != 0) ||
+	    (boot_cat->validation_entry.key55 != 0x55) ||
+	    (boot_cat->validation_entry.keyAA != 0xAA))
+	    printf("El Torito boot catalog sanity check failed.\n");
+	/* If we have an emulation mode, set the offset to the image */
+	if (boot_cat->initial_entry.media_type)
+	    hd_geometry.offset += boot_cat->initial_entry.load_block * 2048;
+	if (boot_cat->initial_entry.media_type < 4) {
+	    /* We're a floppy emulation mode or our params will be
+	     * overwritten by the no emulation mode case
+	     */
+	    hd_geometry.driveno = 0x00;
+	    hd_geometry.c = 80;
+	    hd_geometry.h = 2;
+	}
+	switch (boot_cat->initial_entry.media_type) {
+	case 0:		/* No emulation   */
+	    hd_geometry.driveno = 0xE0;
+	    hd_geometry.type = 10;	/* ATAPI removable media device */
+	    hd_geometry.c = 65535;
+	    hd_geometry.h = 255;
+	    hd_geometry.s = 15;
+	    /* 2048-byte sectors, so adjust the size and count */
+	    hd_geometry.sector_size = 2048;
+	    sectors = (size - hd_geometry.offset) >> 11;
+	    break;
+	case 1:		/* 1.2 MB floppy  */
+	    hd_geometry.s = 15;
+	    hd_geometry.type = 2;
+	    sectors = 2400;
+	    break;
+	case 2:		/* 1.44 MB floppy */
+	    hd_geometry.s = 18;
+	    hd_geometry.type = 4;
+	    sectors = 2880;
+	    break;
+	case 3:		/* 2.88 MB floppy */
+	    hd_geometry.s = 36;
+	    hd_geometry.type = 6;
+	    sectors = 5760;
+	    break;
+	case 4:
+	    hd_geometry.driveno = 0x80;
+	    hd_geometry.type = 0;
+	    sectors = (size - hd_geometry.offset) >> 9;
+	    break;
+	}
+	/* For HDD emulation, we figure out the geometry later. Otherwise: */
+	if (hd_geometry.s) {
+	    hd_geometry.hsrc = hd_geometry.ssrc = "El Torito";
+	}
+	hd_geometry.sectors = sectors;
+    }
+
     /* Do we have a DOSEMU header? */
     memcpy(&dosemu, (char *)where + hd_geometry.offset, sizeof dosemu);
     if (!memcmp("DOSEMU", dosemu.magic, 7)) {
@@ -511,7 +561,7 @@
 
 	if (!(max_h | max_s)) {
 	    /* No FAT filesystem found to steal geometry from... */
-	    if (sectors < 4096 * 2) {
+	    if ((sectors < 4096 * 2) && (hd_geometry.sector_size == 512)) {
 		int ok = 0;
 		unsigned int xsectors = sectors;
 
@@ -572,7 +622,9 @@
 		const struct ptab_entry *ptab = (const struct ptab_entry *)
 		    ((char *)where + hd_geometry.offset + (512 - 2 - 4 * 16));
 
-		hd_geometry.driveno = 0x80;	/* Assume hard disk */
+		/* Assume hard disk */
+		if (!hd_geometry.driveno)
+		    hd_geometry.driveno = 0x80;
 
 		if (*(uint16_t *) ((char *)where + 512 - 2) == 0xaa55) {
 		    for (i = 0; i < 4; i++) {
@@ -674,46 +726,46 @@
 struct gdt_ptr {
     uint16_t limit;
     uint32_t base;
-} __attribute__((packed));
+} __attribute__ ((packed));
 
 static void set_seg_base(uint32_t gdt_base, int seg, uint32_t v)
 {
-    *(uint16_t *)(gdt_base + seg + 2) = v;
-    *(uint8_t *)(gdt_base + seg + 4) = v >> 16;
-    *(uint8_t *)(gdt_base + seg + 7) = v >> 24;
+    *(uint16_t *) (gdt_base + seg + 2) = v;
+    *(uint8_t *) (gdt_base + seg + 4) = v >> 16;
+    *(uint8_t *) (gdt_base + seg + 7) = v >> 24;
 }
 
 static void relocate_rm_code(uint32_t newbase)
 {
     uint32_t gdt_base;
     uint32_t oldbase = rm_args.rm_base;
-    uint32_t delta   = newbase - oldbase;
+    uint32_t delta = newbase - oldbase;
 
     cli();
     memmove((void *)newbase, (void *)oldbase, rm_args.rm_size);
 
-    rm_args.rm_return  		+= delta;
-    rm_args.rm_intcall 		+= delta;
-    rm_args.rm_bounce  		+= delta;
-    rm_args.rm_base    		+= delta;
-    rm_args.rm_gdt              += delta;
-    rm_args.rm_pmjmp		+= delta;
-    rm_args.rm_rmjmp		+= delta;
+    rm_args.rm_return += delta;
+    rm_args.rm_intcall += delta;
+    rm_args.rm_bounce += delta;
+    rm_args.rm_base += delta;
+    rm_args.rm_gdt += delta;
+    rm_args.rm_pmjmp += delta;
+    rm_args.rm_rmjmp += delta;
 
     gdt_base = rm_args.rm_gdt;
 
-    *(uint32_t *)(gdt_base+2)    = gdt_base;	/* GDT self-pointer */
+    *(uint32_t *) (gdt_base + 2) = gdt_base;	/* GDT self-pointer */
 
     /* Segments 0x10 and 0x18 are real-mode-based */
     set_seg_base(gdt_base, 0x10, rm_args.rm_base);
     set_seg_base(gdt_base, 0x18, rm_args.rm_base);
 
-    asm volatile("lgdtl %0" : : "m" (*(char *)gdt_base));
+    asm volatile ("lgdtl %0"::"m" (*(char *)gdt_base));
 
-    *(uint32_t *)rm_args.rm_pmjmp += delta;
-    *(uint16_t *)rm_args.rm_rmjmp += delta >> 4;
+    *(uint32_t *) rm_args.rm_pmjmp += delta;
+    *(uint16_t *) rm_args.rm_rmjmp += delta >> 4;
 
-    rm_args.rm_handle_interrupt	+= delta;
+    rm_args.rm_handle_interrupt += delta;
 
     sti();
 }
@@ -768,11 +820,14 @@
     const struct geometry *geometry;
     unsigned int total_size;
     unsigned int cmdline_len, stack_len, e820_len;
+    const struct edd4_bvd *bvd;
+    const struct edd4_bootcat *boot_cat = 0;
     com32sys_t regs;
     uint32_t ramdisk_image, ramdisk_size;
     uint32_t boot_base, rm_base;
     int bios_drives;
     int do_edd = 1;		/* 0 = no, 1 = yes, default is yes */
+    int do_eltorito = 0;	/* default is no */
     int no_bpt;			/* No valid BPT presented */
     uint32_t boot_seg = 0;	/* Meaning 0000:7C00 */
     uint32_t boot_len = 512;	/* One sector */
@@ -811,13 +866,28 @@
     else
 	do_edd = (geometry->driveno & 0x80) ? 1 : 0;
 
+    if (getcmditem("iso") != CMD_NOTFOUND) {
+	do_eltorito = 1;
+	do_edd = 1;		/* Mandatory */
+    }
+
     /* Choose the appropriate installable memdisk hook */
-    if (do_edd) {
-	bin_size = (int)&_binary_memdisk_edd_bin_size;
-	memdisk_hook = (char *)&_binary_memdisk_edd_bin_start;
+    if (do_eltorito) {
+	if (geometry->sector_size == 2048) {
+	    bin_size = (int)&_binary_memdisk_iso_2048_bin_size;
+	    memdisk_hook = (char *)&_binary_memdisk_iso_2048_bin_start;
+	} else {
+	    bin_size = (int)&_binary_memdisk_iso_512_bin_size;
+	    memdisk_hook = (char *)&_binary_memdisk_iso_512_bin_start;
+	}
     } else {
-	bin_size = (int)&_binary_memdisk_chs_bin_size;
-	memdisk_hook = (char *)&_binary_memdisk_chs_bin_start;
+	if (do_edd) {
+	    bin_size = (int)&_binary_memdisk_edd_512_bin_size;
+	    memdisk_hook = (char *)&_binary_memdisk_edd_512_bin_start;
+	} else {
+	    bin_size = (int)&_binary_memdisk_chs_512_bin_size;
+	    memdisk_hook = (char *)&_binary_memdisk_chs_512_bin_start;
+	}
     }
 
     /* Reserve the ramdisk memory */
@@ -837,7 +907,7 @@
 
     pptr->driveno = geometry->driveno;
     pptr->drivetype = geometry->type;
-    pptr->cylinders = geometry->c;
+    pptr->cylinders = geometry->c;	/* Possible precision loss */
     pptr->heads = geometry->h;
     pptr->sectors = geometry->s;
     pptr->disksize = geometry->sectors;
@@ -933,16 +1003,36 @@
 	    pptr->edd_dpt.c = geometry->c;
 	    pptr->edd_dpt.h = geometry->h;
 	    pptr->edd_dpt.s = geometry->s;
-	    pptr->edd_dpt.flags |= 0x0002;	/* Geometry valid */
+	    /* EDD-4 states that invalid geometry should be returned
+	     * for INT 0x13, AH=0x48 "EDD Get Disk Parameters" call on an
+	     * El Torito ODD.  Check for 2048-byte sector size
+	     */
+	    if (geometry->sector_size != 2048)
+		pptr->edd_dpt.flags |= 0x0002;	/* Geometry valid */
 	}
 	if (!(geometry->driveno & 0x80)) {
 	    /* Floppy drive.  Mark it as a removable device with
 	       media change notification; media is present. */
 	    pptr->edd_dpt.flags |= 0x0014;
 	}
-	
+
 	pptr->edd_dpt.devpath[0] = pptr->diskbuf;
-	pptr->edd_dpt.chksum  = -checksum_buf(&pptr->edd_dpt.dpikey, 73-30);
+	pptr->edd_dpt.chksum = -checksum_buf(&pptr->edd_dpt.dpikey, 73 - 30);
+    }
+
+    if (do_eltorito) {
+	bvd = (struct edd4_bvd *)(ramdisk_image + 17 * 2048);
+	boot_cat =
+	    (struct edd4_bootcat *)(ramdisk_image + bvd->boot_cat * 2048);
+	pptr->cd_pkt.type = boot_cat->initial_entry.media_type;	/* Cheat */
+	pptr->cd_pkt.driveno = geometry->driveno;
+	pptr->cd_pkt.start = boot_cat->initial_entry.load_block;
+	pptr->cd_pkt.load_seg = boot_cat->initial_entry.load_seg;
+	pptr->cd_pkt.sect_count = boot_cat->initial_entry.sect_count;
+	boot_len = pptr->cd_pkt.sect_count * 2048;
+	pptr->cd_pkt.geom1 = (uint8_t)(pptr->cylinders) & 0xFF;
+	pptr->cd_pkt.geom2 = (uint8_t)(pptr->sectors) | (uint8_t)((pptr->cylinders >> 2) & 0xC0);
+	pptr->cd_pkt.geom3 = (uint8_t)(pptr->heads);
     }
 
     /* The size is given by hptr->total_size plus the size of the E820
@@ -1059,6 +1149,13 @@
     if (pptr->drivecnt <= (geometry->driveno & 0x7f))
 	pptr->drivecnt = (geometry->driveno & 0x7f) + 1;
 
+    /* Probe for contiguous range of BIOS drives starting with driveno */
+    pptr->driveshiftlimit = probe_drive_range(geometry->driveno) + 1;
+    if ((pptr->driveshiftlimit & 0x80) != (geometry->driveno & 0x80))
+	printf("We lost the last drive in our class of drives.\n");
+    printf("Drive probing gives drive shift limit: 0x%02x\n",
+	pptr->driveshiftlimit);
+
     /* Pointer to the command line */
     pptr->cmdline_off = bin_size + (nranges + 1) * sizeof(ranges[0]);
     pptr->cmdline_seg = driverseg;
@@ -1087,7 +1184,7 @@
 	if (nhd > 128)
 	    nhd = 128;
 
-	wrz_8(BIOS_HD_COUNT, nhd);
+	if (!do_eltorito) wrz_8(BIOS_HD_COUNT, nhd);
     } else {
 	/* Update BIOS floppy disk count */
 	uint8_t equip = rdz_8(BIOS_EQUIP);
@@ -1124,11 +1221,11 @@
 
     /* Figure out entry point */
     if (!boot_seg) {
-	boot_base  = 0x7c00;
-	shdr->sssp = 0x7c00; 
-	shdr->csip = 0x7c00; 
+	boot_base = 0x7c00;
+	shdr->sssp = 0x7c00;
+	shdr->csip = 0x7c00;
     } else {
-	boot_base  = boot_seg << 4;
+	boot_base = boot_seg << 4;
 	shdr->sssp = boot_seg << 16;
 	shdr->csip = boot_seg << 16;
     }
@@ -1143,7 +1240,11 @@
     /* Reboot into the new "disk" */
     puts("Loading boot sector... ");
 
-    memcpy((void *)boot_base, (char *)pptr->diskbuf + boot_lba*512, boot_len);
+    if (do_eltorito) {
+	/* 4 times as many 512-byte sectors in a 2048-byte sector */
+	boot_lba = pptr->cd_pkt.start * 4;
+    }
+    memcpy((void *)boot_base, (char *)pptr->diskbuf + boot_lba * 512, boot_len);
 
     if (getcmditem("pause") != CMD_NOTFOUND) {
 	puts("press any key to boot... ");
@@ -1155,5 +1256,5 @@
 
     /* On return the assembly code will jump to the boot vector */
     shdr->esdi = pnp_install_check();
-    shdr->edx  = geometry->driveno;
+    shdr->edx = geometry->driveno;
 }