SECTION .text   align=0
[bits 32]

%include "eelstd.h"
%include "fdc.h"
%include "bios.h"
%include "irq.h"

		GLOBAL	fdc_irq
		GLOBAL	fdc_uflag
		GLOBAL	fdc_flag
		GLOBAL	fdc_motor
		GLOBAL	fdc_mtimer
		GLOBAL	fdc
		GLOBAL	read_sector
		GLOBAL	write_sector
		GLOBAL	fdc_sensei
		GLOBAL	fdc_init
		GLOBAL	fdc_seek
		GLOBAL	fdc_recalibrate
		GLOBAL	fdc_motor_on

fdc_irq:	pushfd
		jmp	short _fdc_irq

		dd	12345678h

                db      'FDC IRQ handler ',0

_fdc_irq:	pushad
                push    ds

                push    byte data32
		pop	ds

		cmp	byte [fdc_flag],0xff
		jne short .1
		inc	byte [fdc_uflag]
 
.1: 		mov	byte [fdc_flag],0ffh

		mov	al,20h
		out	20h,al

		pop	ds 
		popad
		popfd
		iretd


fdc_uflag	db	0		;unscheduled irq flag
fdc_flag	db	0
fdc_motor	db	0		;is fdc motor on?
fdc_mtimer	dd	0		;how long has the fdcmotor been on?

fdc	times   fdc_io_size	db 0
		dw	0

;
; Call with:
;
; esi -> fdc info block
;
; lba begining at 0
;

lba2chs:	xor	edx,edx
		mov	ax,[esi+fdc_io.lba]
		mov	bl,18*2
		div	bl

		mov	[esi+fdc_io.cylinder],al
		mov	al,ah
		mov	ah,0
		mov	bl,18
		div	bl
		mov	[esi+fdc_io.head],al

		mov	ax,[esi+fdc_io.lba]
		movzx	ebx,bl
		div	bx
		inc	dl
		mov	[esi+fdc_io.sector],dl

		ret

;
; Call with 
;
; esi -> fdc info block
;
; lba begining at 0
;

chs2lba:	xor	eax,eax
		mov	al,[esi+fdc_io.cylinder]
		mov	bl,2*18
		mul	bl
		mov	[esi+fdc_io.lba],ax

		xor	eax,eax
		mov	al,[esi+fdc_io.head]
		mov	bl,18
		mul	bl
		add	[esi+fdc_io.lba],ax
		mov	ah,0
		mov	al,[esi+fdc_io.sector]
		add	[esi+fdc_io.lba],ax

		ret

;
; Call with 
;
; esi -> fdc info block
;


read_sector:    sti
                pushad
		push	ds 
		push	es

                push    byte data32
		pop	ds

		mov	byte [esi+fdc_io.fd_command],FD_READ
		mov	byte [esi+fdc_io.dma_mode],DMA_READ

		call	RW_fd0
                jnc     rsec_ok

fd_err:         mov	word [esi+fdc_io.error],ax
                pop     es 
		pop	ds
                popad
                mov     ax,word [esi+fdc_io.error]
                popfd
                iretd

rsec_ok:        pop     es 
		pop	ds
		popad
                popfd
		iretd

fd_errc dw      0

write_sector:   sti
                pushad
                push    ds 
                push    es

                push    byte data32
		pop	ds

		mov	byte [esi+fdc_io.fd_command],FD_WRITE
		mov	byte [esi+fdc_io.dma_mode],DMA_WRITE

		call	RW_fd0
                jnc     rsec_ok

                jmp     fd_err

RW_fd0: 	mov	byte [esi+fdc_io.io_tries],0
		mov	byte [esi+fdc_io.hw_resets],0

		test	byte [esi+fdc_io.flags],FD_FLBA
		jz	.chs

		call	lba2chs			;convert lba -> chs

.chs:		mov	al,[esi+fdc_io.head]
		shl	al,2
		or	al,[esi+fdc_io.drive]
		mov	[esi+fdc_io.hdr1],al
                mov     byte [fdc_flag],0

.fd_init:	call	fdc_init
		jc near	fd_error

		call	fdc_seek
		jc near fd_error

.dma_init:	mov	byte [fdc_flag],0

;		 call	check_valid_dma_address

		mov	al,4|2
		out	10,al
		
		mov	al,0
		out	0ch,al

		mov	al,[esi+fdc_io.dma_mode]
		out	12,al

		jmp short $+2
		jmp short $+2

		out	11,al

		mov	eax,DMA_BUF	;[esi+fdc_io.ptr]
		out	4,al

		shr	eax,8
		out	4,al

		shr	eax,8
		out	0x81,al

		mov	ax,[esi+fdc_io.cnt]
		dec	ax
		out	5,al	
		mov	al,ah
		out	5,al

		mov	al,2
		out	10,al		

;                mov     esi,[pointer]
;                mov     bl,[dma_mode]
;                xor     ecx,ecx
;                mov     cx,[rw_bytes]
;
;                call    req_dmal
;                jc near pageerror

                mov     ah,byte [esi+fdc_io.fd_command]
		call	send_fd0
		jc near	fd_error

		mov	ah,[esi+fdc_io.hdr1]
		call	send_fd0
		jc near	fd_error

                mov     ah,[esi+fdc_io.cylinder]
		call	send_fd0
		jc near	fd_error

                mov     ah,[esi+fdc_io.head]
                call    send_fd0
		jc near	fd_error

                mov     ah,[esi+fdc_io.sector]
		call	send_fd0
		jc near	fd_error

                mov     ah,2            ; (0 - User spec., 1 - 256, 2 - 512, 3 - 1024)
		call	send_fd0
		jc near	fd_error

                mov     ah,18           ; sectors / track
		call	send_fd0
		jc near	fd_error

                mov     ah,1bh          ; gap 3 length (1.44M)
		call	send_fd0
		jc	fd_error

                mov     ah,0ffh         ; skummo.. hmm..?
		call	send_fd0
		jc	fd_error

		call	wait_int
		jc      fd_error

                mov     ebx,esi
		add	ebx,byte fdc_io.status
		mov	ecx,7
.next7:		push	ecx
		push	ebx
		call	get_fdc_stat
		pop	ebx
		pop	ecx
		mov	[ebx],al
		inc	ebx
		loop	.next7

;		mov	dx,FD_DOR
;		mov	al,0xc
;		out	dx,al

                mov     al,[esi+fdc_io.status]
                test    al,0c0h
                jz      .ok

		inc	byte [esi+fdc_io.io_tries]
		cmp	byte [esi+fdc_io.io_tries],byte 3
		jb near	.dma_init

		inc	byte [esi+fdc_io.hw_resets]
		cmp	byte [esi+fdc_io.hw_resets],byte 3
		jae	.stat_error

		call	fdc_recalibrate

		jmp	.fd_init

.ok             clc
                ret

.stat_error:    mov     ax,0ffffh
                stc
                ret

fd_error:	or	al,FD_IO_ERROR
		push	eax
		call	fdc_sensei
		pop	eax

		xchg	al,ah
		mov	al,0xff
		stc
		ret

fdc_clear_read:	call	get_fdc_stat
		jnc	fdc_clear_read

		ret


; 'sense' the fdc interrupt
;


fdc_sensei:	mov	ah,FD_SENSEI
		call	send_fd0
		jc short .1

		call	get_fdc_stat
		jc short .1
		mov	[esi+fdc_io.status],al
		call	get_fdc_stat
		jc short .1
		mov	[esi+fdc_io.status+1],al

.1:		ret

fdc_init: 	xor	eax,eax
		mov	[fdc_mtimer],eax

;		mov	dx,FD_DOR
;		mov	al,0
;		mov	[fdc_flag],al
;		out	dx,al		;disable irq+dma
;
;		mov	ecx,82
;		call	fdc_wait_s
;
;                mov     dx,FD_DCR
;                mov     al,0            ;500000 bit/sec mode
;                out     dx,al
;
;		mov	ecx,82
;		call	fdc_wait_s
;
;		mov	dx,FD_DOR
;		mov	al,0x0c
;		out	dx,al
;
;		mov	byte [fdc_flag],0xff
;		call	wait_int	;wait for fdc irq
;		jc	.2
;
;		mov	ecx,4096
;		call	fdc_wait_s
;
;		call	fdc_sensei	;'sense' irq
;		jc	.2
;
;		mov	ah,FD_SPECIFY
;		call	send_fd0
;		jc short .2
;
;		mov	ah,0xdf		;SRT = 3ms, HUT = 240ms
;		call	send_fd0
;		jc short .2
;
;		mov	ah,0x02		;HLT = 16ms, ND = 0
;		call	send_fd0
;		jc short .2

		call	fdc_motor_on

		call	fdc_seek
		jc	.2

		call	fdc_recalibrate
		jc	.2

		clc
.1:		ret

.2:		or	al,FD_INIT_ERROR
		stc
		ret

; IN:	hdr1, _cx_
;

fdc_seek:	mov     byte [fdc_flag],0

		mov     ah,FD_SEEK
                call    send_fd0
		jc short .1

                mov     ah,[esi+fdc_io.hdr1]
                call    send_fd0
		jc short .1

                mov     ah,[esi+fdc_io.cylinder]
                call    send_fd0
		jc short .1

                mov	ecx,130
		call	fdc_wait_s			;let head settle

		call    wait_int
		jc	.1
		call	fdc_sensei

		mov	ah,[esi+fdc_io.status+1]	;check if it worked
		cmp	ah,[esi+fdc_io.cylinder]
		jnz	.2

		clc
.1:		ret

.2:		or	al,FD_SEEK_ERROR
		stc
		ret		

; IN:   [hdr1] = the drive, head is ignored
;

fdc_recalibrate:
		call	fdc_motor_on

		mov	byte [fdc_flag],0

		mov     ah,FD_RECALIBRATE
                call    send_fd0
		jc	.2

		mov     ah,[esi+fdc_io.drive]
                and     ah,3
                call    send_fd0
		jc	.2

		call	wait_int
		jc	.2

                mov     byte [fdc_flag],0

		call	fdc_sensei
		jc	.2

		clc
.1:             ret

.2:		or	al,FD_RECAL_ERROR
		stc
		ret

fdc_motor_on:	cmp	byte [fdc_motor],0xff
		je	.motor_is_on

		mov     dx,FD_DOR
		mov	al,00011100b
                or      al,[esi+fdc_io.drive]
                out     dx,al

                mov     dx,FD_DCR
                mov     al,0            ;500000 bit/sec mode
                out     dx,al

		call	wait_motor

.motor_is_on:	xor	eax,eax
		mov	[fdc_mtimer],eax
		not	eax
		mov	[fdc_motor],al

		ret

send_fd0:       mov     ecx,[cmos_count]
                add     ecx,2048                ;timeout == 1/4 of a sec
                mov     dx,FD_STATUS

.tryagain:      cmp     [cmos_count],ecx
                ja      .timeout

                in      al,dx
		and	al,0c0h
		cmp	al,0x80
                jne     .tryagain

                inc     dx
		mov	al,ah
		out	dx,al

                clc
                ret

.timeout:       mov	al,FD_TOS_ERROR
		stc
                ret

wait_motor:     mov     ecx,[cmos_count]
                add     ecx,4096                ;timeout == 1/2 of a sec.
.1:             cmp     [cmos_count],ecx
                jna     .1

                ret

fdc_wait_s:	mov	eax,[cmos_count]
		add	eax,ecx
.1:		cmp	[cmos_count],eax
		jb	.1
		ret

wait_int:       mov     eax,[cmos_count]
                add     eax,4096                ;1/2 second timeout
.again:         cmp     [cmos_count],eax
                ja      .timeout
		cmp	byte [fdc_flag],0ffh
                jnz     .again
		mov	byte [fdc_flag],0
		clc
		ret

.timeout:       mov	al,FD_INT_ERROR
		stc
		ret

get_fdc_stat:   mov     ecx,[cmos_count]
                add     ecx,2048                ;timeout == 1/4 of a sec
                mov     dx,FD_STATUS

.tryagain:      cmp     [cmos_count],ecx
                ja      .timeout

                in      al,dx
		and	al,0c0h
		cmp	al,0c0h
                jne     .tryagain

                inc     dx
		in	al,dx

                clc
		ret

.timeout:       mov	al,FD_TOR_ERROR
		stc
                ret