new indentation style

[BOS.git] / kernel / fdc / fdc.asm

diff --git a/kernel/fdc/fdc.asm b/kernel/fdc/fdc.asm
index d90218d8990b7709c29850b3470682bfe8aa7ec7..ac3cf0089561a7ee2e89e433e9536608b79549eb 100755 (executable)
--- a/kernel/fdc/fdc.asm
+++ b/kernel/fdc/fdc.asm
@@ -1,39 +1,34 @@
 ;----------------------------------------------------------;\r
 ;----------------------------------------------------------;\r

-; BOS kernel                Christoffer Bubach, 2004-2005. ;\r
+; BOS kernel                Christoffer Bubach, 2004-2012. ;\r

 ;----------------------------------------------------------;\r
 ;                                                          ;\r
 ;----------------------------------------------------------;\r
 ;                                                          ;\r

-;  floppy disk driver originally by Craig Bamford, Dex4u.  ;\r
-;          adjustments and for BOS by bubach.              ;\r
+;               floppy disk driver.                        ;\r

 ;                                                          ;\r
 ;----------------------------------------------------------;\r
 \r
 ;                                                          ;\r
 ;----------------------------------------------------------;\r
 \r

-;  NOTE: I'm almost finished with a much improved version of\r
-;        this code, that supports 2 drives of multiple types.\r
-;        I'll wait with that until next version, so you won't\r
-;        have to see a half finished mess here.\r

 \r
 ;---------------------------;\r
 ;  variables and contants   ;\r
 ;---------------------------;\r
 \r
 ;---------------------------;\r
 ;  variables and contants   ;\r
 ;---------------------------;\r

-     fdd_int_done    dw   0                          ; is the IRQ done?\r
-     fdd_motor_on    db   0                          ; fdd motor on=1, off=0\r
-     fdd_head       db   0\r
-     fdd_driveno     db   0\r
-     fdd_track      db   0\r
-     fdd_sector      db   0\r
-\r
-     fdd_errorcode   db   0                          ; this the error code 00 = no error\r
-     fdd_tries      db   0                           ; no. of times to try, before error.\r
-\r
-     ResultST0      db   0\r
-     ResultST1      db   0                           ; or pcn\r
-     ResultST2      db   0\r
-     ResultST3      db   0\r
-     ResultC        db   0\r
-     ResultH        db   0\r
-     ResultR        db   0\r
-     ResultN        db   0\r
-     ResultExtra     db   0\r
+    fdd_int_done    dw   0                            ; is the IRQ done?\r
+    fdd_motor_on    db   0                            ; fdd motor on=1, off=0\r
+    fdd_drivehead   db   0                            ; ((head*4)|drive)\r
+    fdd_head        db   0\r
+    fdd_track       db   0\r
+    fdd_sector      db   0\r
+    fdd_error       db   0                            ; should we recalibrate\r
+                                                      ; at next read/write?\r
+    fdd_errorcode   db   0\r
+\r
+    result_ST0      db   0\r
+    result_ST1      db   0                            ; or pcn\r
+    result_ST2      db   0\r
+    result_ST3      db   0\r
+    result_C        db   0\r
+    result_H        db   0\r
+    result_R        db   0\r
+    result_N        db   0\r
+    result_extra    db   0\r

 \r
 \r
 \r
 \r
 \r
 \r


@@ -42,22 +37,38 @@
 ;                                                      ;\r
 ;------------------------------------------------------;\r
 fdc_init:\r
 ;                                                      ;\r
 ;------------------------------------------------------;\r
 fdc_init:\r

-         push    ecx\r
-         push    edi\r
+        push   eax\r
+        push   ebx\r
+        push   ecx\r
+        push   edi\r
+        push   esi\r
+\r
+        xor    eax, eax                               ; get configuration\r
+        mov    al, 0x10                               ; from CMOS.\r
+        out    0x70, al\r
+        in     al, 0x71\r

 \r
 \r

-         mov     cl, 0x26                            ; hook interrupt 0x26 (IRQ6)\r
-         mov     edi, floppy_irq\r
-         call    set_int\r
+        shr    al, 4\r
+        cmp    al, 4                                  ; a 1.44mb?\r
+        jnz    .no_floppy\r

 \r
 \r

-         mov     cl, 6                               ; enable IRQ6\r
-         call    enable_irq\r
+        mov    cl, 0x26                               ; hook IRQ 6\r
+        mov    dx, 0x8                                ; CS = 8\r
+        mov    edi, floppy_irq\r
+        call   set_int\r

 \r
 \r

-         call    fdd_reset                           ; reset FDC\r
-         call    fdd_recalibrate\r
+        mov    cl, 6                                  ; enable IRQ6\r
+        call   enable_irq\r

 \r
 \r

-         pop     edi\r
-         pop     ecx\r
-         ret\r
+        call   fdd_reset                              ; reset FDC\r
+\r
+    .no_floppy:\r
+        pop    esi\r
+        pop    edi\r
+        pop    ecx\r
+        pop    ebx\r
+        pop    eax\r
+        ret\r

 \r
 \r
 \r
 \r
 \r
 \r


@@ -66,24 +77,21 @@ fdc_init:
 ;                                                      ;\r
 ;------------------------------------------------------;\r
 floppy_irq:\r
 ;                                                      ;\r
 ;------------------------------------------------------;\r
 floppy_irq:\r

-         pusha\r
-         push    gs\r
-         push    fs\r
-         push    ds\r
-         push    es\r
-         push    esi\r
+        push   ds\r
+        push   eax\r
+        push   ebx\r
+\r
+        mov    ax, 0x10\r
+        mov    ds, ax\r

 \r
 \r

-         or      [fdd_int_done], 1\r
-         mov     al, 0x20\r
-         out     0x20, al\r
+        mov    [fdd_int_done], 1\r
+        mov    al, 0x20\r
+        out    0x20, al\r

 \r
 \r

-         pop     esi\r
-         pop     es\r
-         pop     ds\r
-         pop     fs\r
-         pop     gs\r
-         popa\r
-         iret\r
+        pop    ebx\r
+        pop    eax\r
+        pop    ds\r
+        iret\r

 \r
 \r
 \r
 \r
 \r
 \r


@@ -92,26 +100,26 @@ floppy_irq:
 ;                           out:  cf  = 1 if timeout   ;\r
 ;------------------------------------------------------;\r
 wait_int:\r
 ;                           out:  cf  = 1 if timeout   ;\r
 ;------------------------------------------------------;\r
 wait_int:\r

-         push    eax\r
-         push    ecx\r
-\r
-         mov     ecx, 150                            ; 150 = 1.5 seconds.\r
-         call    active_delay                        ; timer.inc\r
-     .l1:\r
-         cmp     dword [ecx], 0                      ; timeup?\r
-         je      .error\r
-         mov     ax, [fdd_int_done]                  ; if not we check for int.\r
-         or      ax, ax\r
-         jz      .l1\r
-         pop     ecx\r
-         pop     eax\r
-         clc\r
-         ret\r
-     .error:\r
-         pop     ecx\r
-         pop     eax\r
-         stc\r
-         ret\r
+        push   eax\r
+        push   ecx\r
+\r
+        mov    ecx, 150                               ; 50 = 0.5 seconds.\r
+        call   active_delay                           ; timer.inc\r
+    .l1:\r
+        cmp    dword [ecx], 0                         ; timeup?\r
+        je     .error\r
+        mov    ax, [fdd_int_done]                     ; if not we check for int.\r
+        or     ax, ax\r
+        jz     .l1\r
+\r
+        clc\r
+        jmp    .end\r
+    .error:\r
+        stc\r
+    .end:\r
+        pop    ecx\r
+        pop    eax\r
+        ret\r

 \r
 \r
 \r
 \r
 \r
 \r


@@ -120,18 +128,20 @@ wait_int:
 ;                           out:  nothing              ;\r
 ;------------------------------------------------------;\r
 fdd_off:\r
 ;                           out:  nothing              ;\r
 ;------------------------------------------------------;\r
 fdd_off:\r

-         push    eax\r
-         push    edx\r
-\r
-         mov     dx, 0x3F2\r
-         mov     al, 0                               ; all motors off..\r
-         out     dx, al\r
-         mov     [fdd_motor_on], 0\r
+        cmp    [fdd_motor_on], 0\r
+        je     .end\r
+        push   eax\r
+        push   edx\r

 \r
 \r

-         pop     edx\r
-         pop     eax\r
-         ret\r
+        mov    dx, 0x3F2\r
+        mov    al, 0x0c                               ; motor off\r
+        out    dx, al\r
+        mov    [fdd_motor_on], 0\r

 \r
 \r

+        pop    edx\r
+        pop    eax\r
+    .end:\r
+         ret\r

 \r
 \r
 \r
 \r
 \r
 \r


@@ -140,20 +150,23 @@ fdd_off:
 ;                           out: nothing               ;\r
 ;------------------------------------------------------;\r
 fdd_on:\r
 ;                           out: nothing               ;\r
 ;------------------------------------------------------;\r
 fdd_on:\r

-         push    eax\r
-         push    edx\r
+        cmp    [fdd_motor_on], 1\r
+        je     .end\r
+        push   eax\r
+        push   edx\r

 \r
 \r

-         mov     dx, 0x3F2                           ; motor 0 on..\r
-         mov     al, 0x1C\r
-         out     dx, al\r
+        mov    dx, 0x3F2                              ; motor 0 on..\r
+        mov    al, 0x1C\r
+        out    dx, al\r

 \r
 \r

-         mov     ecx, 20                             ; 1/5 of a sec. to speed up\r
-         call    delay                               ; in timer.inc\r
-         mov     [fdd_motor_on], 1\r
+        mov    ecx, 20                                ; 1/5 of a sec. to speed up\r
+        call   delay                                  ; in timer.inc\r
+        mov    [fdd_motor_on], 1\r

 \r
 \r

-         pop     edx\r
-         pop     eax\r
-         ret\r
+        pop    edx\r
+        pop    eax\r
+    .end:\r
+        ret\r

 \r
 \r
 \r
 \r
 \r
 \r


@@ -163,34 +176,35 @@ fdd_on:
 ;                                in:  al  = data byte  ;\r
 ;------------------------------------------------------;\r
 fdc_sendbyte:\r
 ;                                in:  al  = data byte  ;\r
 ;------------------------------------------------------;\r
 fdc_sendbyte:\r

-         push    edx\r
-         push    ecx\r
-         push    eax\r
-\r
-         mov     ecx, 150                            ; 150 = 1.5 seconds.\r
-         call    active_delay                        ; timer.inc\r
-     .l1:\r
-         cmp     dword [ecx], 0                      ; timeup?\r
-         je      .error\r
-         mov     dx, 0x3f4                           ; check status reg\r
-         in      al, dx\r
-         and     al, 11000000b\r
-         cmp     al, 10000000b                       ; are we ok to write\r
-         jnz     .l1\r
-\r
-         pop     eax\r
-         pop     ecx\r
-         mov     dx, 0x3F5                           ; send byte\r
-         out     dx, al\r
-         pop     edx\r
-         clc\r
-         ret\r
-     .error:\r
-         pop     eax\r
-         pop     ecx\r
-         pop     edx\r
-         stc\r
-         ret\r
+        push   edx\r
+        push   ecx\r
+        push   eax\r
+\r
+        mov    ecx, 50                             ; 50 = 0.5 seconds.\r
+        call   active_delay                        ; timer.inc\r
+    .l1:\r
+        cmp    dword [ecx], 0                      ; timeup?\r
+        je     .error\r
+        mov    dx, 0x3f4                           ; check status reg\r
+        in     al, dx\r
+        and    al, 0xC0\r
+        cmp    al, 0x80                            ; ok to write?\r
+        jnz    .l1\r
+\r
+        pop    eax\r
+        pop    ecx\r
+        mov    dx, 0x3F5                           ; send byte\r
+        out    dx, al\r
+        pop    edx\r
+        clc\r
+        ret\r
+    .error:\r
+        pop    eax\r
+        pop    ecx\r
+        pop    edx\r
+        stc\r
+        ret\r
+\r

 \r
 \r
 ;------------------------------------------------------;\r
 \r
 \r
 ;------------------------------------------------------;\r


@@ -199,303 +213,310 @@ fdc_sendbyte:
 ;                             out: al  = data byte     ;\r
 ;------------------------------------------------------;\r
 fdc_getbyte:\r
 ;                             out: al  = data byte     ;\r
 ;------------------------------------------------------;\r
 fdc_getbyte:\r

-         push    edx\r
-         push    ecx\r
-         push    eax\r
-\r
-         mov     ecx, 150                            ; 150 = 1.5 seconds.\r
-         call    active_delay                        ; timer.inc\r
-     .l1:\r
-         cmp     dword [ecx], 0                      ; timeup?\r
-         je      .error\r
-         mov     dx, 0x3f4                           ; check status reg\r
-         in      al, dx\r
-         and     al, 11000000b\r
-         cmp     al, 11000000b                       ; are we ok to read\r
-         jnz     .l1\r
-\r
-         pop     eax\r
-         pop     ecx\r
-         mov     dx, 0x3F5                           ; get the byte\r
-         in      al, dx\r
-         pop     edx\r
-         clc\r
-         ret\r
-     .error:\r
-         pop     eax\r
-         pop     ecx\r
-         pop     edx\r
-         stc\r
-         ret\r
-\r
+        push   edx\r
+        push   ecx\r
+        push   eax\r
+\r
+        mov    ecx, 50                                ; 50 = 0.5 seconds.\r
+        call   active_delay                           ; timer.inc\r
+    .l1:\r
+        cmp    dword [ecx], 0                         ; timeup?\r
+        je     .error\r
+        mov    dx, 0x3f4                              ; check status reg\r
+        in     al, dx\r
+        and    al, 0xD0\r
+        cmp    al, 0xD0                               ; ok to read?\r
+        jnz    .l1\r
+\r
+        pop    eax\r
+        pop    ecx\r
+        mov    dx, 0x3F5                              ; get the byte\r
+        in     al, dx\r
+        pop    edx\r
+        clc\r
+        ret\r
+    .error:\r
+        pop    eax\r
+        pop    ecx\r
+        pop    edx\r
+        stc\r
+        ret\r

 \r
 \r
 \r
 ;------------------------------------------------------;\r
 \r
 \r
 \r
 ;------------------------------------------------------;\r

-;    reset controller                                  ;\r
+;   sense interrupt status command                     ;\r

 ;                                                      ;\r
 ;------------------------------------------------------;\r
 ;                                                      ;\r
 ;------------------------------------------------------;\r

-fdd_reset:\r
-         push    eax\r
-         push    ecx\r
-         push    edx\r
-\r
-         mov     dx, 0x3F2\r
-         mov     al, 00001000b                       ; no motors enabled, DMA Enabled, held FDC at reset\r
-         out     dx, al                              ; drive A selected.\r
-\r
-         mov     ecx, 5\r
-         call    delay                               ; in timer.inc\r
-\r
-         mov     dx, 0x3F7\r
-         mov     al, 00000000b                       ; 500Kb/sec mode\r
-         out     dx, al\r
-\r
-         mov     dx, 0x3F2\r
-         or      al, 00001100b                       ; FDC completed reset, al =0x0c.\r
-         out     dx, al\r
-\r
-         mov     [fdd_int_done], 0                   ; we need to wait\r
-         call    wait_int                            ; for floppy int.\r
-         jc      .error                              ; jump to error exit, if timeout.\r
-\r
-         mov     cx, 0x04                            ; after a reset, the interrupts must be\r
-     .status:                                        ; cleared by (four) dummy-reads.\r
-         mov     al, 0x08                            ; sense interrupt status command\r
-         call    fdc_sendbyte\r
-         call    fdc_getbyte\r
-         call    fdc_getbyte\r
-         loop    .status\r
-\r
-         mov     al, 0x03                            ; specify command (includes next two bytes sent)\r
-         call    fdc_sendbyte\r
-         mov     al, 0xDF                            ; SRT = 3ms, HUT = 240ms\r
-         call    fdc_sendbyte\r
-         mov     al, 0x02                            ; HLT = 16ms, ND = 0\r
-         call    fdc_sendbyte\r
-     .ok:\r
-         pop     edx\r
-         pop     ecx\r
-         pop     eax\r
-         clc\r
-         ret\r
-     .error:\r
-         pop     edx\r
-         pop     ecx\r
-         pop     eax\r
-         stc\r
-         ret\r
+sensei:\r
+        push   eax\r

 \r
 \r

+        mov    al, 0x08                               ; fdc command\r
+        call   fdc_sendbyte\r
+        call   fdc_getbyte\r
+        mov    ah, al                                 ; save ST0 in ah\r
+        call   fdc_getbyte                            ; read PCN\r
+        clc\r
+        test   ah, 0x80                               ; test for error:\r
+        jz     .end                                   ; "invalid command"\r
+        stc\r
+    .end:\r
+        pop    eax\r
+        ret\r

 \r
 \r
 \r
 ;------------------------------------------------------;\r
 \r
 \r
 \r
 ;------------------------------------------------------;\r

-;     fdd seek                                         ;\r
+;    reset controller                                  ;\r

 ;                                                      ;\r
 ;------------------------------------------------------;\r
 ;                                                      ;\r
 ;------------------------------------------------------;\r

-fdd_seek:\r
-         push    eax\r
-\r
-         mov     al, [ResultC]                       ; put what track/cylinder we are at in al\r
-         cmp     [fdd_track], al                     ; is it the same as we want\r
-         je      .ok                                 ; yes, then leave.\r
-\r
-         mov     al, 0x0F                            ; seek command\r
-         call    fdc_sendbyte\r
-         mov     al, [fdd_driveno]                   ; drive # (00 = A)\r
-         call    fdc_sendbyte\r
-         mov     al, [fdd_track]                     ; cylinder #\r
-         call    fdc_sendbyte\r
-\r
-         mov     [fdd_int_done], 0\r
-         call    wait_int\r
-         jc      .error\r
-\r
-         mov     al, 0x08                            ; sense interrupt status command\r
-         call    fdc_sendbyte\r
-         call    fdc_getbyte\r
-         mov     ah, al                              ; save ST0 in ah\r
-         call    fdc_getbyte                         ; should read PCN\r
-\r
-         test    ah, 00100000b                       ; test sr0 is 0x20\r
-         jz      .error                              ; if not we have a error :-(.\r
-         test    ah,10000000b                        ; test sr0 is 0x80\r
-         jnz     .error                              ; if not we have a error :-(.\r
-     .ok:                                            ; succsess.. :-)\r
-         pop     eax\r
-         clc\r
-         ret\r
-     .error:\r
-         pop     eax\r
-         stc\r
-         ret\r
+fdd_reset:\r
+        push   eax\r
+        push   ecx\r
+        push   edx\r
+\r
+        mov    byte [fdd_motor_on], 0\r
+\r
+        mov    dx, 0x3f2\r
+        mov    al, 8                                  ; off with all motors,\r
+        out    dx, al                                 ; dma,irq etc..\r
+\r
+        mov    ecx, 5\r
+        call   delay                                  ; in timer.inc\r
+\r
+        mov    dx, 0x3f7\r
+        mov    al, 0\r
+        out    dx, al                                 ; work at 500 kbit/s\r
+\r
+        mov    dx, 0x3f2\r
+        mov    al, 0x0c\r
+        out    dx, al                                 ; reenable interrupts\r
+\r
+        mov    [fdd_int_done], 0\r
+        call   wait_int                               ; wait for floppy int.\r
+        jc     .error                                 ; timeout?\r
+\r
+        mov    cx, 0x04\r
+    .status:                                          ; 4 dummy-reads.\r
+        call   sensei\r
+        loop   .status\r
+\r
+        mov    al, 0x03                               ; specify command\r
+        call   fdc_sendbyte\r
+        mov    al, 0xDF                               ; SRT, HUT\r
+        call   fdc_sendbyte\r
+        mov    al, 0x02                               ; HLT, ND\r
+        call   fdc_sendbyte\r
+\r
+        mov    al, 1\r
+        call   fdd_recal_seek\r
+        jc     .error\r
+        call   fdd_off\r
+        clc\r
+        jmp    .end\r
+    .error:\r
+        call   fdd_off\r
+        stc\r
+    .end:\r
+        pop    edx\r
+        pop    ecx\r
+        pop    eax\r
+        ret\r

 \r
 \r
 \r
 \r

+\r
+;------------------------------------------------------;\r
+;  fdd_recal_seek  -   fdd recalibrate/seek            ;\r

 ;------------------------------------------------------;\r
 ;------------------------------------------------------;\r

-;   fdd recalibrate                                    ;\r
+;                                                      ;\r
+;   in:  al  = 0 on seek, 1 on recalibrate             ;\r
+;        bl  = (at seek) track                         ;\r
+;        bh  = (at seek) ((head*4)|drive)              ;\r

 ;                                                      ;\r
 ;------------------------------------------------------;\r
 ;                                                      ;\r
 ;------------------------------------------------------;\r

-fdd_recalibrate:\r
-         push    eax\r
-\r
-         test    [fdd_motor_on], 1\r
-         jnz     .l1\r
-         call    fdd_on\r
-     .l1:\r
-         mov     al, 0x07                            ; recalibrate command\r
-         call    fdc_sendbyte\r
-         mov     al, 0x00                            ; selects drive a:\r
-         call    fdc_sendbyte\r
-\r
-         mov     [fdd_int_done], 0\r
-         call    wait_int\r
-         jc      .error\r
-\r
-         mov     al, 0x08                            ; sense interrupt status command\r
-         call    fdc_sendbyte\r
-         call    fdc_getbyte                         ; read STO\r
-         mov     ah, al                              ; save ST0 in ah\r
-         call    fdc_getbyte\r
-\r
-         test    ah, 00100000b                       ; test sr0 is 0x20\r
-         jz      .error\r
-         test    ah, 00010000b                       ; test sr0 is 0x80\r
-         jnz     .error\r
+fdd_recal_seek:\r
+        push   eax\r
+\r
+        call   fdd_on                                 ; turn motor on\r
+        cmp    al, 0\r
+        jne    .recalibrate\r
+        clc\r
+        cmp    bl, [result_C]                         ; are we there yet? :D\r
+        je     .ok\r
+        mov    al, 0x0F                               ; seek command\r
+        call   fdc_sendbyte\r
+        mov    al, bh                                 ; ((head*4)|drive)\r
+        call   fdc_sendbyte\r
+        mov    al, bl                                 ; track\r
+        call   fdc_sendbyte\r
+        mov    [result_C], bl                         ; now on..?\r
+        jmp    .get_int\r
+    .recalibrate:\r
+        mov    al, 0x07                               ; recalibrate command\r
+        call   fdc_sendbyte\r
+        mov    al, 0                                  ; drive number\r
+        call   fdc_sendbyte\r
+        mov    [result_C], 0                          ; now on track 0\r
+    .get_int:\r
+        mov    [fdd_int_done], 0\r
+        call   wait_int\r
+        jc     .error\r
+\r
+        call   sensei                                 ; sense interrupt status\r
+        jc     .error\r

      .ok:\r
      .ok:\r

-         mov     [ResultC], 0\r
-         pop     eax\r
-         clc\r
-         ret\r
-     .error:\r
-         pop     eax\r
-         stc\r
-         ret\r
+        clc\r
+        jmp    .end\r
+    .error:\r
+        stc\r
+    .end:\r
+        pop    eax\r
+        ret\r

 \r
 \r
 \r
 ;------------------------------------------------------;\r
 \r
 \r
 \r
 ;------------------------------------------------------;\r

-;  fdd read/write                                      ;\r
+;  fdd_read_write   -  fdd read/write                  ;\r
+;------------------------------------------------------;\r

 ;   input:  bl  = 0 read, 1 write                      ;\r
 ;           ch  = track/cylinder                       ;\r
 ;           cl  = sector                               ;\r
 ;           dh  = head                                 ;\r
 ;   input:  bl  = 0 read, 1 write                      ;\r
 ;           ch  = track/cylinder                       ;\r
 ;           cl  = sector                               ;\r
 ;           dh  = head                                 ;\r

-;           dl  = drive (only A: drive used, 00 )      ;\r
-;   output: ah  = status                               ;\r
-;           al  = sector number read                   ;\r
+;           edi = address to store or read the data    ;\r
+;   output: al  = status                               ;\r

 ;           cf  = 0 if ok, 1 if error                  ;\r
 ;------------------------------------------------------;\r
 fdd_read_write:\r
 ;           cf  = 0 if ok, 1 if error                  ;\r
 ;------------------------------------------------------;\r
 fdd_read_write:\r

-         pushad\r
-         and     dh, 00000001b                       ; is it set to fdd_head 0 or 1?\r
-         mov     [fdd_head], dh                      ; store it.\r
-         shl     dh, 2                               ; make the first 2 bits = 00 for A:.\r
-         mov     [fdd_driveno], dh                   ; store it.\r
-\r
-         mov     [fdd_errorcode], 0x04               ; put error code in ah, just incase\r
-         cmp     ch, 0x51                            ; cmp if track number, is above 80 decimal (51 hex)?\r
-         jae     .error                              ; if above jump error.\r
-         mov     [fdd_track], ch                     ; if not above,store it.\r
-\r
-         cmp     cl, 0x13                            ; cmp if fdd_sector number, is above 18 decimal (12 hex) ?.\r
-         jae     .error                              ; if above jump error.\r
-         mov     [fdd_sector], cl                    ; if not above,store it.\r
-\r
-         test    [fdd_motor_on], 1\r
-         jnz     .l1\r
-         call    fdd_on\r
-     .l1:\r
-         mov     dx, 0x3F7\r
-         mov     al, 00000000b                       ; 500Kb/sec mode\r
-         out     dx, al\r
-         mov     [fdd_errorcode], 0x80               ; put basic error code, just in case.\r
-\r
-         xor     ecx, ecx\r
-         mov     cx, 3                               ; we want to try seek 3 times\r
-     .l2:\r
-         call    fdd_seek                            ; we need to move to the right track.\r
-         jnc     .l3                                 ; we should be on the right track.\r
-         loop    .l2\r
-         jmp     .error                              ; timeout.\r
-     .l3:\r
-         mov     dx, 0x3f4                           ; check status reg (to see if DMA bit set)\r
-         in      al, dx\r
-         test    al, 00100000b                       ; test sr0 is 0x80\r
-         jnz     .error\r
-\r
-         cmp     bl, 0\r
-         je      .read_fdd\r
-\r
-     .write_fdd:\r
-         mov     bl, 2                               ; channel 2\r
-         mov     esi, 512                            ; bytes to write\r
-         mov     ecx, 0x80000                        ; page & offset\r
-         mov     bh, 1                               ; write floppy, "read DMA"\r
-         call    dma_transfer\r
-\r
-         mov     al, 0xC5                            ; write fdd_sector command\r
-         call    fdc_sendbyte\r
-         jmp     .cont\r
-\r
-     .read_fdd:\r
-         mov     bl, 2                               ; channel 2\r
-         mov     esi, 512                            ; bytes to read\r
-         mov     ecx, 0x80000                        ; page & offset\r
-         mov     bh, 0                               ; read floppy, "write DMA"\r
-         call    dma_transfer\r
-\r
-         mov     al, 0xE6                            ; read fdd_sector command\r
-         call    fdc_sendbyte\r
-\r
-     .cont:\r
-         mov     al, [fdd_driveno]                   ; fdd_head no. 0, drive A:\r
-         call    fdc_sendbyte\r
-         mov     al, [fdd_track]                     ; cylinder\r
-         call    fdc_sendbyte\r
-\r
-         mov     al, [fdd_head]                      ; head/side 0 or 1\r
-         call    fdc_sendbyte\r
-         mov     al, [fdd_sector]                    ; sector number,starts at 1\r
-         call    fdc_sendbyte\r
-         mov     al, 0x02                            ; sector size - 512 bytes\r
-         call    fdc_sendbyte\r
-\r
-         mov     al, 0x12                            ; 18 decimal sectors to a track\r
-         call    fdc_sendbyte\r
-         mov     al, 0x1B                            ; 27 should be the gap length for a 3.5" 1.44Mb\r
-         call    fdc_sendbyte\r
-         mov     al, 0xFF                            ; not used data length, because sector size has been filled\r
-         call    fdc_sendbyte\r
-\r
-         mov     [fdd_int_done], 0\r
-         call    wait_int\r
-         jc      .error\r
-\r
-         call    fdc_getbyte\r
-         mov     [ResultST0], al                     ; save result of ST0 in var\r
-         call    fdc_getbyte\r
-         mov     [ResultST1], al                     ; save result of ST1 in var\r
-         call    fdc_getbyte\r
-         mov     [ResultST2], al                     ; save result of ST2 in var\r
-         call    fdc_getbyte\r
-         mov     [ResultC], al                       ; save result of cylinder\r
-         call    fdc_getbyte\r
-         mov     [ResultH], al                       ; save result of head\r
-         call    fdc_getbyte\r
-         mov     [ResultR], al                       ; save result of sector number.\r
-         call    fdc_getbyte\r
-         mov     [ResultN], al                       ; save result of sector size\r
-\r
-         test    [ResultST0], 11000000b              ; test sr0 is 0xC0\r
-         jnz     .error\r
-         mov     [fdd_errorcode], 0x00\r
-     .ok:\r
-         popad\r
-         mov     ah, [fdd_errorcode]                 ; move error status into ah\r
-         mov     al, [ResultR]\r
-         clc\r
-         ret\r
-     .error:\r
-         popad\r
-         mov     ah, [fdd_errorcode]                 ; move error status into ah\r
-         stc\r
-         ret
\ No newline at end of file
+        pushad\r
+\r
+        and    dh, 1                                  ; head 0 or 1?\r
+        mov    [fdd_head], dh                         ; store it.\r
+        shl    dh, 2\r
+        or     dh, 0                                  ; drive 0, fd0\r
+        mov    [fdd_drivehead], dh                    ; dh = ((head*4)|drive)\r
+        mov    [fdd_errorcode], 0x04                  ; basic error code\r
+        cmp    ch, 0x51                               ; check for allowed\r
+        jae    .error                                 ; track number.\r
+        mov    [fdd_track], ch\r
+        cmp    cl, 0x13                               ; check for allowed\r
+        jae    .error                                 ; sector number.\r
+        mov    [fdd_sector], cl\r
+\r
+        cmp    [fdd_error], 1\r
+        jne    .no_previous_error\r
+        mov    al, 1\r
+        call   fdd_recal_seek\r
+    .no_previous_error:\r
+        call   fdd_on\r
+\r
+        mov    dx, 0x3F7\r
+        mov    al, 0                                  ; 500Kb/sec mode\r
+        out    dx, al\r
+        mov    [fdd_errorcode], 0x80                  ; error code\r
+\r
+        xor    ecx, ecx\r
+        mov    cx, 3                                  ; try seek 3 times\r
+    .l2:\r
+        mov    al, 0\r
+        push   ebx\r
+        mov    bl, [fdd_track]\r
+        mov    bh, [fdd_drivehead]                    ; ((head*4)|drive)\r
+        call   fdd_recal_seek\r
+        pop    ebx\r
+        jnc    .l3                                    ; ok, continue.\r
+        loop   .l2\r
+        jmp    .error                                 ; timeout.\r
+    .l3:\r
+        push   ebx\r
+        cmp    bl, 0\r
+        je     .read_fdd\r
+\r
+    .write_fdd:\r
+        push   edi\r
+        mov    esi, edi\r
+        mov    edi, 0x80000                           ; copy the stuff we will\r
+        mov    ecx, 128                               ; write to the DMA buffer\r
+        rep    movsd                                  ; 128*4=512\r
+        pop    edi\r
+\r
+        mov    bl, 2                                  ; channel 2\r
+        mov    esi, 512                               ; bytes to write\r
+        mov    ecx, 0x80000                           ; page & offset\r
+        mov    bh, 1                                  ; write floppy, read DMA\r
+        call   dma_transfer\r
+\r
+        mov    al, 0xC5                               ; write sector command\r
+        call   fdc_sendbyte\r
+        jmp    .cont\r
+\r
+    .read_fdd:\r
+        mov    bl, 2                                  ; channel 2\r
+        mov    esi, 512                               ; bytes to read\r
+        mov    ecx, 0x80000                           ; page & offset\r
+        mov    bh, 0                                  ; read floppy, write DMA\r
+        call   dma_transfer\r
+\r
+        mov    al, 0xE6                               ; read sector command\r
+        call   fdc_sendbyte\r
+\r
+    .cont:\r
+        pop    ebx\r
+        mov    al, [fdd_drivehead]                    ; ((head*4)|drive)\r
+        call   fdc_sendbyte\r
+        mov    al, [fdd_track]                        ; track/cylinder\r
+        call   fdc_sendbyte\r
+\r
+        mov    al, [fdd_head]                         ; head/side 0 or 1\r
+        call   fdc_sendbyte\r
+        mov    al, [fdd_sector]                       ; sector number\r
+        call   fdc_sendbyte\r
+        mov    al, 0x02                               ; sector size, 512 bytes\r
+        call   fdc_sendbyte\r
+\r
+        mov    al, 0x12                               ; sectors to a track\r
+        call   fdc_sendbyte\r
+        mov    al, 0x1B                               ; gap length\r
+        call   fdc_sendbyte\r
+        mov    al, 0xFF                               ; data length\r
+        call   fdc_sendbyte\r
+\r
+        mov    [fdd_int_done], 0\r
+        call   wait_int\r
+        jc     .error\r
+\r
+        call   fdc_getbyte\r
+        mov    [result_ST0], al                       ; ST0\r
+        call   fdc_getbyte\r
+        mov    [result_ST1], al                       ; ST1\r
+        call   fdc_getbyte\r
+        mov    [result_ST2], al                       ; ST2\r
+        call   fdc_getbyte\r
+        mov    [result_C], al                         ; cylinder\r
+        call   fdc_getbyte\r
+        mov    [result_H], al                         ; head\r
+        call   fdc_getbyte\r
+        mov    [result_R], al                         ; sector number.\r
+        call   fdc_getbyte\r
+        mov    [result_N], al                         ; sector size\r
+\r
+        test   [result_ST0], 0xc0                     ; test if sr0 is 0xC0\r
+        jnz    .error\r
+        cmp    bl, 1                                  ; did we write the disk?\r
+        je     .ok\r
+\r
+        mov    ecx, 512                               ; sector size in bytes\r
+        mov    esi, 0x80000                           ; copy to dest. in edi\r
+        rep    movsb\r
+    .ok:\r
+        mov    [fdd_errorcode], 0                     ; no error.. :D\r
+        mov    [fdd_error], 0                         ; no recal. next time\r
+        clc\r
+        jmp    .end\r
+    .error:\r
+        mov    [fdd_error], 1                         ; recalibrate next time\r
+        stc\r
+    .end:\r
+        call   fdd_off                                ; floppy motor off\r
+        popad\r
+        mov    al, [fdd_errorcode]                    ; error status\r
+        ret
\ No newline at end of file