;************************************************************************
;*	Super Famivom FDC ( uPD72069 ) driver  Version 1.00		*
;*		    Programmed by  Y.Nishida				*
;*						[ Aug.08, 1991 ]	*
;************************************************************************

		native
		extend

		include		PPUregs.h
		include		CPUregs.h
		include		sfxdos.h

FDC_Driver	sect		rel
SFXDOS		group		FDC_Driver
		dpage		DOS_variable

;===============  Cross definition  =====================================

		global		FDC_Driver
		global		FDC_Interrupt

;===============  FDC (uPD72069) status bit  ============================

FDC_D0B		equ		00000001B		; FD0 Busy
FDC_D1B		equ		00000010B		; FD1 Busy
FDC_D2B		equ		00000100B		; FD2 Busy
FDC_D3B		equ		00001000B		; FD3 Busy
FDC_DB		equ		00001111B		; FD Busy
FDC_CB		equ		00010000B		; FDC Busy
FDC_NDM		equ		00100000B		; Non-DMA Mode
FDC_DIO		equ		01000000B		; Data input/Output
FDC_RQM		equ		10000000B		; Request For Master

;===============  FDC (uPD72069) command  ===============================

FDC_RESET	equ		00110110B		; FDC Software reset
FDC_SET_STDBY	equ		00110101B		; FDC Set standby
FDC_RESET_STDBY	equ		00110100B		; FDC Reset standby
FDC_START_CLOCK	equ		01000111B		; FDC Start clock
FDC_SNSINT	equ		00001000B		; FDC Sense interrupt status
FDC_SNSDEV	equ		00000100B		; FDC Sence device status
FDC_SPECIFY	equ		00000011B		; FDC Specify
FDC_INTERNAL250	equ		00001011B		; FDC Control internal mode ( 250k bps )
FDC_INTERNAL300	equ		11001011B		; FDC Control internal mode ( 300k bps )
FDC_INTERNAL500	equ		01001011B		; FDC Control internal mode ( 500k bps )
FDC_SELFORMAT	equ		01001111B		; FDC Select format
FDC_MOTORS	equ		11111110B		; FDC Enable motors
FDC_RECALIBRATE	equ		00000111B		; FDC Recalibtate
FDC_SEEK	equ		00001111B		; FDC Seek
FDC_READ	equ		11100110B		; FDC Read
FDC_WRITE	equ		11000101B		; FDC Write
FDC_WRITEID	equ		01001101B		; FDC Write ID

;************************************************************************
;*	Floppy Disk Controller ( uPD72069 )  Driver entry		*
;*									*
;*		ENTRY	X = Function number				*
;*			D = DOS_variable				*
;************************************************************************
 
		mem8
		idx8
FDC_Driver
		php
		sep		#00110000B		; Memory,Index 8 bit mode
;------------------------------------------------------------------------
		lda		<Disk_control
		asl		a
		tax
		jsr		(Jump_table$,x)
;------------------------------------------------------------------------
		plp
		rts

;===============  Jump Table  ===========================================

Jump_table$	word		FDC_Reset		; FDC reset and work initialize
		word		FDC_Device_stat		; Device status read
		word		FDC_Recalibrate		; Head recalibrate
		word		FDC_Seek		; Head seek
		word		FDC_Read		; Disk read
		word		FDC_Write		; Disk write
		word		FDC_Read		; Disk verify
		word		FDC_Format		; Disk format
		word		FDC_Reset_Stdby		; FDC Reset standby
		word		FDC_Set_Stdby		; FDC Set standby

;************************************************************************
;*	Reset Floppy Disk Controller					*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Reset
		jsr		FDC_Reset_Stdby
		lda		#FDC_RESET
		sta		>FDC_Stat_reg		; Output Reset command
		jsr		FDC_Ready		; Check FDC ready
		bcs		FDC_Reset

;=============== Initial FDC parameter ==================================
FDC_Reset_10
		ldy		#9
		lda		<Disk_control+1		; 2HD mode ?
		beq		Skip$			; yes.
		ldy		#19
Skip$		ldx		#9
;------------------------------------------------------------------------
Repeat$		lda		FDC_2HD_param,y
		sta		<FDC_parameter,x
		dey
		dex
		bpl		Repeat$
;------------------------------------------------------------------------
		lda		<Disk_control+1
		sta		<FDC_device_mode
		dec		a			; 2DD 8 sector mode ?
		bne		FDC_Reset_20		; no.
		dec		<FDC_EOT		; Decrement last sector No.

;=============== Initial FDC ============================================
FDC_Reset_20
		lda		#FDC_SPECIFY
		sta		<FDC_command+0
		lda		<FDC_SRT_HUT
		sta		<FDC_command+1
		lda		<FDC_HLT_ND
		sta		<FDC_command+2
		ldy		#3
		jsr		FDC_Output_comm		; Output Specify command
;------------------------------------------------------------------------
		ldx		#FDC_INTERNAL300
		lda		<Disk_control+1		; 2HD mode ?
		bne		Skip$			; no.
		ldx		#FDC_INTERNAL500
Skip$		jsr		FDC_Output_sub		; Set Trasfer rate
		jsr		FDC_Input_data		; Read result
;------------------------------------------------------------------------
		ldx		#FDC_SELFORMAT
		jsr		FDC_Output_sub		; Select IBM format
		jsr		FDC_Input_data		; Read result
		rts

;===============  2HD format parameters  ================================

FDC_2HD_param	byte		008H			; EOT	2HD Last sector number
		byte		035H			; GSL	2HD Gap skip length
		byte		0FFH			; DTL	2HD Data length
;------------------------------------------------------------------------
		byte		154			; TRK	2HD Number of tracks
		byte		004H			; LEN   2HD Record length ( *256 bytes )
;------------------------------------------------------------------------
		byte		008H			; SC	2HD Number of Sectors on 1 track
		byte		003H			; N	2HD Record length ( code  )
		byte		074H			; GPL	2HD Gap length
;------------------------------------------------------------------------
		byte		06EH			; SRT  10ms  HUT 224ms
		byte		065H			; HLT 100ms  Non-DMA

;===============  2DD format parameters  ================================

FDC_2DD_param	byte		009H			; EOT	2DD Last sector number
		byte		02AH			; GSL	2DD Gap skip length
		byte		0FFH			; DTL	2DD Data length
;------------------------------------------------------------------------
		byte		160			; TRK	2DD Number of tracks
		byte		002H			; LEN   2DD Record length ( bytes )
;------------------------------------------------------------------------
		byte		009H			; SC	2DD Number of Sectors on 1 track
		byte		002H			; N	2DD Record length ( code  )
		byte		050H			; GPL	2DD Gap length
;------------------------------------------------------------------------
		byte		0B7H			; SRT  10ms  HUT 224ms
		byte		033H			; HLT 100ms  Non-DMA

;************************************************************************
;*	Set standby							*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Set_Stdby
		lda		<FDC_device_mode	; FDC Standby ? 
		bmi		Exit$			; yes.
;------------------------------------------------------------------------
		lda		#FDC_SET_STDBY
		jsr		FDC_Output_sub		
		lda		#10000000B
		tsb		<FDC_device_mode
Exit$		rts

;************************************************************************
;*	Reset standby							*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Reset_Stdby
		lda		<FDC_device_mode	; FDC standby ?
		bpl		Exit$			; no.
;------------------------------------------------------------------------
		lda		#FDC_START_CLOCK
		jsr		FDC_Output_sub
		lda		#FDC_RESET_STDBY
		jsr		FDC_Output_sub
		jsr		FDC_Input_data
		lda		#10000000B
		trb		<FDC_device_mode
Exit$		rts

;************************************************************************
;*	Sense Device							*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Device_stat
		jsr		FDC_Ready
		bcs		Error$
;------------------------------------------------------------------------
		lda		#FDC_SNSDEV
		sta		<FDC_command+0
		lda		<Disk_control+1
		and		#00000011B
		sta		<FDC_command+1
		ldy		#2
		jsr		FDC_Output_comm
;------------------------------------------------------------------------
		jsr		FDC_Input_data
		sta		<FDC_result+0
		rts
;------------------------------------------------------------------------
Error$		lda		#10000000B
		sta		<FDC_result+0
		rts

;************************************************************************
;*	Recalibrate device						*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Recalibrate
		lda		<Disk_control+1		; Acc = Drive number
;------------------------------------------------------------------------
FDC_Recalib_sub	and		#00000011B
		sta		<FDC_command+1		; Set Drive number
		lda		#FDC_RECALIBRATE
		sta		<FDC_command+0		; Set REcalibrate command
		ldx		#4			; IX = Retry counter

;=============== Send Recalibrate command ===============================
Recalibrate_10
		phx
		ldy		#2			; IY = recalibrate command
		jsr		FDC_Issue_seek		; Recalibrate
		plx
		bcs		Recalibrate_20		; if ( device not ready )
;-----------------------------------------------------------------------
		lda		<FDC_result+0
		bit		#00001000B		; Device not ready ?
		bne		Recalibrate_20		; yes.
		bit		#00010000B		; Recalibrate ok ?
		bne		Retry$			; no.
		and		#11000000B		; Normal terminate ?
		bne		Retry$			; no.
		lda		<FDC_result+1		; Cylinder number == 0 ?
		beq		Recalibrate_30		; yes.
;------------------------------------------------------------------------
Retry$		dex
		bne		Recalibrate_10

;=============== Recalibrate failed =====================================
Recalibrate_20
		lda		#01010000B
		sta		<FDC_result+0
		rts

;=============== Recalibrate completed ==================================
Recalibrate_30
		stz		<FDC_result+0
		rts

;************************************************************************
;*	Seek device							*
;*									*									*
;************************************************************************

		mem8
		idx8
FDC_Seek
		lda		<Disk_control+1		; Acc = Drive number
		ldx		<Disk_control+2		; IX  = Cylinder number
;------------------------------------------------------------------------
FDC_Seek_sub	and		#00000011B
		sta		<FDC_command+1		; Set Drive number
		stx		<FDC_command+2		; Set Cylinder
		lda		#FDC_SEEK
		sta		<FDC_command+0		; Set Seek command
		ldx		#4			; IX = Retry counter

;===============  Send Seek command =====================================
FDC_Seek_10
		phx
		ldy		#3			; IY = seek command
		jsr		FDC_Issue_seek		; Seek
		plx
		bcs		FDC_Seek_20
;------------------------------------------------------------------------
		lda		<FDC_result+0
		bit		#00001000B		; Device not ready ?
		bne		FDC_Seek_20		; yes.
		bit		#00010000B		; Device fault ?
		bne		Retry$			; yes.
		and		#11000000B		; Normal terminate ?
		bne		Retry$			; no.
		lda		<FDC_result+1
		cmp		<FDC_command+2		; Cylinder number ok ?
		beq		FDC_Seek_30		; yes.
;------------------------------------------------------------------------
Retry$		dex
		bne		FDC_Seek_10

;=============== Seek failed ============================================
FDC_Seek_20
		lda		#01010000B
		sta		<FDC_result+0
		rts

;=============== Seek completed  ========================================
FDC_Seek_30
		stz		<FDC_result+0
		rts

;************************************************************************
;*	Send Seek and Recalibrate command to device			*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Issue_seek
		phy
		jsr		FDC_Ready		; FDC ready ?
		ply
		bcs		Issue_seek_30		; no.

;=============== Send command to device =================================
Issue_seek_10
		sei					; Disable IRQ
		jsr		FDC_Output_comm
Issue_seek_11	wai					; Wait IRQ

;=============== Read result status =====================================
Issue_seek_20
		ldx		#FDC_SNSINT
		jsr		FDC_Output_data
;------------------------------------------------------------------------
		jsr		FDC_Input_data
		sta		<FDC_result+0
		cmp		#10000000B		; FDC_Invalid
		beq		Issue_seek_11
		jsr		FDC_Input_data
		sta		<FDC_result+1
;------------------------------------------------------------------------
		lda		<FDC_result+0
		and		#00000011B
		cmp		<FDC_command+1
		bne		Issue_seek_20		
;------------------------------------------------------------------------
		lda		<FDC_result+0
		and		#00100000B		; Seek or Recalibrate end ?
		beq		Issue_seek_20		; no.

;=============== Seek,Recalibrate completed =============================
Issue_seek_21
		cli					; Enable IRQ
		clc					; CY = Disable
		rts
;=============== Seek,Recalibrate failed ================================
Issue_seek_30
		cli					; Enable IRQ
		sec					; CY = Enable
		rts

;************************************************************************
;*	Read/Write device						*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Read
		lda		#FDC_READ
		bra		FDC_Data_RW
;------------------------------------------------------------------------
FDC_Write	lda		#FDC_WRITE

;=============== Seek head ==============================================
FDC_Data_RW
		pha
		jsr		FDC_Head_seek		; Head seek
		bcs		Error$			; if ( seek error )
		jsr		FDC_Ready		; FDC ready ?
		bcc		FDC_Data_RW_10		; yes.
;------------------------------------------------------------------------
Error$		pla
		rts

;=============== Send parameter to FDC ==================================
FDC_Data_RW_10
		pla
		sta		<FDC_command+0		; Set Read/Write command
;------------------------------------------------------------------------
		lda		<Disk_control+1
		and		#00000011B
		sta		<FDC_command+1		; Set Dirve No.
;------------------------------------------------------------------------
		lda		<Disk_control+2
		and		#00000001B
		sta		<FDC_command+3		; Set Head No.
;------------------------------------------------------------------------
		asl		a
		asl		a
		ora		<FDC_command+1
		sta		<FDC_command+1		; Set Head select
;------------------------------------------------------------------------
		lda		<Disk_control+2
		lsr		a
		sta		<FDC_command+2		; Set Cylinder No.
;------------------------------------------------------------------------
		lda		<Disk_control+3
		sta		<FDC_command+4		; Set sector No.
;------------------------------------------------------------------------
		lda		<FDC_N
		sta		<FDC_command+5		; Set Record length
;------------------------------------------------------------------------
		ldy		#08H			; IY = Retry counter
		lda		<Disk_control+0
		cmp		#06H			; Verify mode ?
		bne		FDC_Data_RW_20		; no.
		ldy		#04H			; No retry !

;=============== Set R/W address & numbetr of datas =====================
FDC_Data_RW_20
		phy
		ldy		#8			; IY = command length - 1
		sei					; Disable IRQ
		jsr		FDC_Output_comm
;------------------------------------------------------------------------
		phb
		lda		<Disk_control+7
		pha
		plb					; Set Data bank register
;------------------------------------------------------------------------
		rep		#00010000B		; Index 16 bit mode
		ldx		<Disk_control+5		; IX = Buffer address
;------------------------------------------------------------------------
		lda		<FDC_LEN		; Sector length
		sta		>Multiplicand
		lda		<Disk_control+4		; The number of sectors
		sta		>Multiplier
		nop
		nop
		nop
		nop
		lda		>Multiply+0
		xba
		lda		#00000000B
		tay					; IY = Loop counter

;=============== Branch verify,Read,Write ===============================
FDC_Data_RW_30
		lda		<Disk_control+0
		sec
		sbc		#04H
		beq		FDC_read_data
		dec		a
		beq		FDC_write_data

;=============== Vefiry disk ============================================
FDC_verify_data
		lda		>FDC_Stat_reg		; RQM == 1 ?
		bpl		FDC_verify_data		; no.
		lda		<FDC_command+8
		sta		>FDC_Data_reg		; Output last parameter
;------------------------------------------------------------------------
Verify_repeat$	wai					; 3 cycle
		lda		>FDC_Stat_reg		; 5 cycle
		and		#FDC_NDM		; 2 cycle
		beq		Verify_abort$		; 2 cycle / 3 cycle
		lda		>FDC_Data_reg		; 5 cycle
		dey					; 2 cycle
		bne		Verify_repeat$		; 3 cycye
;------------------------------------------------------------------------
Verify_abort$	lda		>FDC_term_cnt		; Disk read terminate
		bra		FDC_Data_RW_40

;=============== Read disk ==============================================
FDC_read_data
		lda		>FDC_Stat_reg		; RQM == 1 ?
		bpl		FDC_read_data		; no.
		lda		<FDC_command+8
		sta		>FDC_Data_reg		; Output last parameter
;------------------------------------------------------------------------
Read_repeat$	wai					; 3 cycle
		lda		>FDC_Stat_reg		; 5 cycle
		and		#FDC_NDM		; 2 cycle
		beq		Read_abort$		; 2 cycle / 3 cycle
		lda		>FDC_Data_reg		; 5 cycle
		sta		!0000H,x		; 5 cycle
		inx					; 2 cycle
		dey					; 2 cycle
		bne		Read_repeat$		; 4 cycye --> 30 cycle ( 11.194 us )
;------------------------------------------------------------------------
Read_abort$	lda		>FDC_term_cnt		; Disk read terminate
		bra		FDC_Data_RW_40

;=============== Write disk =============================================
FDC_write_data
		lda		>FDC_Stat_reg		; RQM == 1 ?
		bpl		FDC_write_data		; no.
		lda		<FDC_command+8
		sta		>FDC_Data_reg		; Output last parameter
;------------------------------------------------------------------------
Write_repeat$	wai					; 3 cycle
		lda		>FDC_Stat_reg		; 5 cycle
		and		#FDC_NDM		; 2 cycle
		beq		Write_abort$		; 2 cycle / 3 cycle
		lda		!0000H,x		; 5 cycle
		sta		>FDC_Data_reg		; 5 cycle
		inx					; 2 cycle
		dey					; 2 cycle
		bne		Write_repeat$		; 4 cycye --> 31 cycle ( 11.194 us )
;------------------------------------------------------------------------
Write_abort$	lda		>FDC_term_cnt		; Disk read terminate

;=============== Read result status =====================================
FDC_Data_RW_40
		sep		#00010000B		; Index 8 bit mode
		plb					; Restore Data Bank Register
		ply
		ldx		#00H
;------------------------------------------------------------------------
Repeat$		jsr		FDC_Input_data
		sta		<FDC_result,x
		inx
		cpx		#07H
		bne		Repeat$

;=============== Check retry time =======================================
FDC_Data_RW_50
		cli					; Enable IRQ
		lda		<FDC_result+0
		and		#11000000B		; Normal terminate ?
		beq		Exit$			; yes.
;------------------------------------------------------------------------
		dey					; Retry ok ?
		beq		Exit$			; yes.
		jmp		FDC_Data_RW_20
;------------------------------------------------------------------------
Exit$		rts

;************************************************************************
;*	Format device							*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Format
		stz		<Disk_control+2		; Set cylinder
		lda		<Disk_control+1		; Acc = Drive number
		jsr		FDC_Recalib_sub		; Recalibrate
		lda		<FDC_result+0
		and		#11000000B
		bne		FDC_Format_err

;=============== Seek head ==============================================
FDC_Format_10
		jsr		FDC_Head_seek		; Head seek
		bcs		FDC_Format_err		; if ( seek error )
		jsr		FDC_Ready		; FDC ready ?
		bcc		FDC_Format_20		; yes.
;------------------------------------------------------------------------
FDC_Format_err	rts

;=============== Send parameter to FDC ==================================
FDC_Format_20
		lda		#FDC_WRITEID
		sta		<FDC_command+0		; Set Write ID command
;------------------------------------------------------------------------
		lda		<Disk_control+1
		and		#00000011B
		sta		<FDC_command+1		; Set Dirve No.
;------------------------------------------------------------------------
		lda		<Disk_control+2
		asl		a
		asl		a
		ora		<FDC_command+1
		sta		<FDC_command+1		; Set Head select
;------------------------------------------------------------------------
		lda		<FDC_N
		sta		<FDC_command+2		; Set Record length 
		lda		<FDC_SC
		sta		<FDC_command+3		; Set Number of Sectors
		lda		<FDC_GPL
		sta		<FDC_command+4		; Set Gap Length
		lda		#0E5H
		sta		<FDC_command+5		; Set Format data
;------------------------------------------------------------------------
		ldy		#04H			; IY = Retry counter

;=============== format 1 track =========================================
FDC_Format_30
		phy
		ldy		#6			; IY = command length - 1
		sei					; Disable IRQ
		jsr		FDC_Output_comm
		ldx		#00H			; IX = Sector number
;------------------------------------------------------------------------
Cylinder$	wai					; 3 cycle
		lda		>FDC_Stat_reg		; 5 cycle
		and		#FDC_NDM		; 2 cycle
		beq		FDC_Format_40		; 2 cycle / 3 cycle
		lda		<Disk_control+2		; 3 cycle
		lsr		a			; 2 cycle
		sta		>FDC_Data_reg		; 5 cycle
;------------------------------------------------------------------------
Head$		wai					; 3 cycle
		lda		>FDC_Stat_reg		; 5 cycle
		and		#FDC_NDM		; 2 cycle
		beq		FDC_Format_40		; 2 cycle / 3 cycle
		lda		<Disk_control+2		; 3 cycle
		and		#00000001B		; 2 cycle
		sta		>FDC_Data_reg		; 5 cycle
;------------------------------------------------------------------------
Sector$		wai					; 3 cycle
		lda		>FDC_Stat_reg		; 5 cycle
		and		#FDC_NDM		; 2 cycle
		beq		FDC_Format_40		; 2 cycle / 3 cycle
		inx					; 2 cycle
		txa					; 2 cycle
		sta		>FDC_Data_reg		; 5 cycle
;------------------------------------------------------------------------
Length$		wai					; 3 cycle
		lda		>FDC_Stat_reg		; 5 cycle
		and		#FDC_NDM		; 2 cycle
		beq		FDC_Format_40		; 2 cycle / 3 cycle
		lda		<FDC_N			; 2 cycle
		sta		>FDC_Data_reg		; 5 cycle
		cpx		<FDC_SC			; 2 cycle
		bne		Cylinder$		; 4 cycye

;=============== Read Result status =====================================
FDC_Format_40
		ply
		ldx		#00H
;------------------------------------------------------------------------
Repeat$		jsr		FDC_Input_data
		sta		<FDC_result,x
		inx
		cpx		#07H
		bne		Repeat$
;------------------------------------------------------------------------
		lda		<FDC_result+0
		and		#11000000B		; Normal terminate ?
		beq		FDC_Format_50		; yes.
;------------------------------------------------------------------------
		dey					; Retry ok ?
		bne		FDC_Format_30		; yes.
;------------------------------------------------------------------------
		cli					; Enable IRQ
FDC_Format_end	rts

;=============== Set next track number ==================================
FDC_Format_50
		cli
		inc		<Disk_control+2
		lda		<Disk_control+2
		cmp		<FDC_TRK
		beq		FDC_Format_end
		jmp		FDC_Format_10

;************************************************************************
;*	Head seek							*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Head_seek
		ldx		#4
		phx
		bra		Seek$
;------------------------------------------------------------------------
Retry$		phx
		lda		<Disk_control+1		; Acc = Drive number
		jsr		FDC_Recalib_sub		; Recalibrate
;------------------------------------------------------------------------
Seek$		lda		<Disk_control+2
		lsr		a
		tax					; IX  = Cylinder number
		lda		<Disk_control+1		; Acc = Drive number
		jsr		FDC_Seek_sub		; Seek
		lda		<FDC_result+0
		and		#11000000B
		beq		Complete$
		plx
		dex
		bne		Retry$
;------------------------------------------------------------------------
Error$		sec
		rts
;------------------------------------------------------------------------
Complete$	plx
		clc
		rts

;************************************************************************
;*	Check FDC ready							*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Ready
		ldy		#00H			; IX = Retry counter
;------------------------------------------------------------------------
FDC_Ready_retry	lda		>FDC_Stat_reg		; Get status
		bit		#FDC_DIO		; Data Input or Output ?
		bne		FDC_Ready_200		; if ( Data input )

;=============== Data output mode ( Host -> FDC ) =======================
FDC_Ready_100
		bit		#FDC_CB			; FDC Busy ?
		bne		FDC_Ready_error		; yes.
		bit		#FDC_DB			; Drive Busy ?
		beq		FDC_Ready_ok		; no.
;------------------------------------------------------------------------
		ldy		#FDC_SNSINT
		jsr		>FDC_Output_data
		jsr		>FDC_Input_data
		jsr		>FDC_Input_data
		bra		FDC_Ready_100

;=============== Data input mode ( FDC -> Host ) ========================
FDC_Ready_200
		bpl		FDC_Ready_100		; if ( data not avairable )
		lda		>FDC_Data_reg		; Dummy read
		dey					; Decrement retry counter
		bne		FDC_Ready_retry		; Retry !

;=============== FDC Not ready ==========================================
FDC_Ready_error
		lda		#01001000B
		sta		<FDC_result+0
		sec
		rts

;=============== FDC ready ==============================================
FDC_Ready_ok
		stz		<FDC_result+0
		clc
		rts

;************************************************************************
;*	Write command to FDC						*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Output_comm
		ldx		#00H
Repeat$		lda		>FDC_Stat_reg		; RQM == 1 ? 
		bpl		Repeat$			; no.
;-----------------------------------------------------------------------
		lda		<FDC_command,x
		sta		>FDC_Data_reg
		inx
		dey
		bne		Repeat$
		rts

;************************************************************************
;*	Write Sub command to FDC					*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Output_sub
		lda		>FDC_Stat_reg		; RQM == 1 ? 
		bpl		FDC_Output_sub		; no.
;-----------------------------------------------------------------------
		txa
		sta		>FDC_Stat_reg
		rts

;************************************************************************
;*	Read data from FDC						*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Input_data
		lda		>FDC_Stat_reg
		and		#FDC_RQM+FDC_DIO
		cmp		#FDC_RQM+FDC_DIO
		bne		FDC_Input_data
;-----------------------------------------------------------------------
		lda		>FDC_Data_reg
		rts

;************************************************************************
;*	Write data to FDC						*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Output_data
		lda		>FDC_Stat_reg		; RQM == 1 ? 
		bpl		FDC_Output_data		; no.
		txa
		sta		>FDC_Data_reg
		rts

;************************************************************************
;*	FDC Interrupt routine						*
;*									*
;************************************************************************

		mem8
		idx8
FDC_Interrupt
		lda		FDC_Stat_reg
		and		#FDC_DIO
		bne		FDC_Interrupt_2

;=============== Check interrupt factor ================================
FDC_Interrupt_1
		ldx		#FDC_SNSINT
		jsr		FDC_Output_data
;-----------------------------------------------------------------------
		jsr		FDC_Input_data
		cmp		#10000000B		; Status == Invalid ?
		beq		FDC_Interrupt_2		; yes.
;----------------------------------------------------------------------
		jsr		FDC_Input_data
		bra		FDC_Interrupt_1
;----------------------------------------------------------------------
FDC_Interrupt_2	rts

		end