org #100

nolist

run start

.start
	di
	ld A,1
	call MOTOR_state
	call FDC_calibrate
	ld HL,#4000
	ld DE,#01C1
	call FDC_read

	xor A
	call MOTOR_State
.done
	jr done


.MOTOR_state

	ld BC,#FA7E
	out (C),A

	ld C,10 ;wait 1 sec
	call CR_Wait

	ret

;read score sector , D = track - E = sector - HL =address
.FDC_read
	push HL
	push DE

	ld A,D
	call FDC_track

;send command

        ld A,%01000110
        call FDC_send ;code
        ld A,0		
        call FDC_send ;idd
	pop DE
	ld A,D
	push DE
        call FDC_send ;track
        xor A
        call FDC_send ;adt
        pop DE
	ld A,E
	push AF
	call FDC_send ;ns

	ld A,2	
        call FDC_send ;ls

	pop AF
        call FDC_send ;fp

        ld   A,51                  
        call FDC_send ;GAP
        ld   A,#FF
        call FDC_send ;done

;read data
	ld BC,#FB7E
	pop HL
	ld DE,512

.FDC_read_loop1
        in   A,(C)      
        jp   P,FDC_read_loop1

        and  %00100000  
        jr   Z,FDC_read_done
;
        inc  C
        in   A,(C)
        ld   (HL),A
        inc  HL
;
        dec  C
 	dec de
 	ld a,e
 	or d

        jr nz,FDC_read_loop1

.FDC_read_loop2
        in   A,(C)                
 	jp   P,FDC_read_loop2
        and  %00100000  
        jr   Z,FDC_read_done
;
        inc  C
        in   A,(C)
        dec  C
        jr FDC_read_loop2

.FDC_read_done

	jr FDC_getresult

;get operation result
;return carry if success

.FDC_getresult
        call FDC_get
        ld (st0),A
        call FDC_get
        ld (st1),A
        call FDC_get
        ld (st2),A
;ignore other parms
        call FDC_get
        call FDC_get
        call FDC_get
        call FDC_get

	ld A,(st0)
	and %10011000
	jr nz,FDC_error

	ld A,(st1)
	and %00110111
	jr nz,FDC_error

	ld A,(st2)
	and %00110000
	jr nz,FDC_error

	scf
	ret

.FDC_error
	or A
	ret

;goto track A 
.FDC_track
        push AF
        ld   A,%00001111
        call FDC_send
        ;ld A,(current_drive)
	xor A
        call FDC_send
        pop AF
        call FDC_send
        call FDC_waitdone
	ret

;goto track 0 to recalibrate head
.FDC_calibrate         
	ld   A,%00000111
   	call FDC_send
  	;ld a,(current_drive)
	xor A
        call FDC_send
        call FDC_waitdone
        ret


;wait X seconds contained in C
CR_Wait:
	ld B,#F5
	ld E,5 ;minimal wait is 0.1 sec

;1sec loop
;wait start of VBL
.CR_waitLoop
	in A,(C)
	rra
	jr nc,CR_waitLoop

;wait end of VBL
.CR_waitCounterLoop
	in A,(C)
	rra
	jr c,CR_waitCounterLoop


	dec E
	jr nz,CR_waitLoop



;1 sec loop
	ld E,5
	dec C
	jr nz,CR_waitLoop
;C secs loop
	ret

;get ST0 status to see if operation ended
.FDC_waitdone
        ld   A,%00001000
        call FDC_send

        call FDC_get
        ld   (st0),A
        call FDC_get
        xor  A
        ld   (st1),A                   
        ld   (st2),A
        ld   A,(st0)

        bit  5,A                      
        jr z,FDC_waitdone 

	ret
	

;send data to FDC
.FDC_send
    ex af,af'
 	ld   BC,#FB7E
.FDC_sendwait 	
	in   A,(C)
    jp   P,FDC_sendwait
    ex af,af'
 	inc C
 	out  (C),A
        ret  

;get data from FDC
.FDC_get
 	ld   BC,#FB7E
.FDC_getwait
	in   A,(C)
 	jp   P,FDC_getwait
 	inc C
        in   A,(C)
        ret

.st0		db 0
.st1		db 0
.st2		db 0

list

;/EOF