Working with multi-sector reads

src/arch/i386/prefix/hdprefix.S

@@ -10,29 +10,61 @@
 #include "bootpart.S"
 
 load_image:
-	movw	$_rom_size, %bp
-1:	/* Truncate read length to end of track */
-	movzwl	%bp, %eax
-			movw	$1, %ax
-	/* Read sectors */
+	/* Get disk geometry */
+	pushal
+	pushw	%es
+	movb	$0x08, %ah
+	int	$0x13
+	jc	load_failed
+	movb	%cl, max_sector
+	movb	%dh, max_head
+	popw	%es
+	popal
+	
+1:	/* Read to end of current track */
+	movb	%cl, %al
+	negb	%al
+	addb	max_sector, %al
+	incb	%al
+	andb	$0x3f, %al
+	movzbl	%al, %eax
 	call	*read_sectors
 	jc	load_failed
+	
 	/* Update %es */
 	movw	%es, %bx
 	shll	$5, %eax
 	addw	%ax, %bx
 	movw	%bx, %es
 	shrl	$5, %eax
+	
 	/* Update LBA address */
 	addl	%eax, %edi
 	adcl	$0, %esi
+	
 	/* Update CHS address */
-			// hmmmm
+	andb	$0xc0, %cl
+	orb	$0x01, %cl
+	incb	%dh
+	cmpb	max_head, %dh
+	jbe	2f
+	xorb	%dh, %dh
+	incb	%ch
+	jnc	2f
+	addb	$0xc0, %cl
+2:
 	/* Loop until whole image is read */
-	subw	%ax, %bp
-	jne	1b
+	subl	%eax, load_length
+	ja	1b
 	ljmp	$BOOT_SEG, $start_image
 
+load_length:
+	.long	_rom_size
+max_sector:
+	.byte	0
+max_head:
+	.byte	0
+
 load_failed:
 	movw	$10f, %si
 	jmp	boot_error