Working with multi-sector reads

diff --git a/src/arch/i386/prefix/hdprefix.S b/src/arch/i386/prefix/hdprefix.S
index 68392d2..ceab206 100644 (file)
--- a/src/arch/i386/prefix/hdprefix.S
+++ b/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