; nastavte se Ucase=Lcase v preferencich
; tato rutina pochazi od Jofy (ktery ji nekde vybrakoval a okomentoval)
; doporucuji pred pouzitim zakazat preruseni
SECTION	MFM,CODE_C
	INCLUDE	"hardware/Custom.i"

WAITBLACK MACRO
 cmp.b #$ff,$dff006
 bne.b *-8
 cmp.b #$10,$dff006
 bne.b *-8
 ENDM

DSKPTH =$20
DSKPTL =$22
CUSTOM =$dff000


start
 bsr r_rd
 move.w #0,d0			;.w cislo drivu
 move.w #1800,d1		;.w prvni sektor
 move.w #2,d2			;.w pocet sektoru
 move.l #xx1,d3		        ;.l kam nahrat
 bsr MFM_ReadSectors
 moveq #0,d0
 bsr MFM_StopDrive

 rts
r_rd
 bsr MFM_LoadRegs
 moveq #0,d0
 bsr MFM_DriveIn
 moveq #$0,d0
 bsr MFM_ActiveDrive
r_jesteraz
 bsr MFM_DriveReady
 cmp.w #-1,d0
 beq r_jesteraz
 moveq #0,d0
 bsr MFM_GoTo00
 rts
*******************************************************************************
************************ JOFA's SECTOR MFM LOADER *****************************
*******************************************************************************

** MFM DATA **
MFM_Cylinders:	dc.b 0,1,2,3	;No. of moved head-cylinder
MFM_Time:	dc.w 2548	;wait among steps for CIA (cca 3 milisecs)
MFM_Step:	dc.w 1		;type of waiting among steps (0-3)
MFM_Tries:	dc.w 6		;No. of tries to read bad track-1
MFM_Buffer:	dc.l xx	;Loader-buffer for $3300 bytes (12*1024+$100)

;--------------------------------Short Info------------------------------------
;How To Handle:
;	-first move heads on track 00
;		example:	call	MFM_LoadRegs
;				wait	MFM_DriveIn (Yes/No)
;				call	MFM_ActiveDrive (DF0-3)
;				test	MFM_DriveReady
;				call	MFM_GoTo00
;	-later You can do what You wanna to...
;
;Routines:
;pozn.Nikdy se nemÅnÉ ÚÁdn× registry (kromÅ speciÁlnÉch vÙstupÊ).
;     Rutiny kter× nemajÉ na vstupu ÃÉslo drivu se vztahujÉ k prÁvÅ akt. drivu.
;
;		MFM_LoadRegs	-natÁhne potÒebn× registry (A4-A6)
;		MFM_DriveIn	-testuje, zda je v drivu disk,nemusÉ bÙt READY!
;				IN: D0.w=ÃÉslo drivu (0-3)
;				OUT:D0.w=0 if disk in drive else -1
;		MFM_StepIn	-krok ke stÒedu disku<-00 (neÃekÁ mezi kroky!)
;				PRE: active & ready drive (should be)
;		MFM_StepOut	-krok ke kraji disku->00 (neÃekÁ mezi kroky!)
;				PRE: active & ready drive (should be)
;		MFM_StepWait	-ÃekÁnÉ mezi kroky
;				PRE: active & ready drive
;				MFM_Step: dc.w =0 if wait 1 snÉmek
;					=1 if wait pomocÉ 60-ti ÒÁdek obrazovky
;					=2 if wait pomocÉ CIAA (nedoporuÃuji)
;					=3 if wait pomocÉ CIAB
;		MFM_GoTo00	- pÒechod na track 00
;				PRE: active & ready drive
;				IN: D0.W=ÃÉslo drivu(0-3)
;		MFM_MoveOn	- pÒechod na danÙ cylinder
;				PRE: active & ready drive
;				IN: D0.W=ÃÉslo drivu (0-3)
;				    D1.W=ÃÉslo sektoru (0-1759)
;				    (MFM_cylinders: 0,1,2,3)
;		MFM_DriveReady	-ÃekÁ vÉc jak 1sec na READY signÁl
;				PRE: active drive
;				OUT: D0.W=0 if Ready else -1
;		MFM_ActiveDrive	-vybere drive & zapne motor
;				IN: D0.W=ÃÉslo drivu (0-3)
;		MFM_StopDrive	-stopne drive
;				IN: D0.W=ÃÉslo drivu (0-3)
;		MFM_WaitFrames	-ÃekÁ urÃitÙ poÃet snÉmkÊ
;				IN: D0.W=poÃet snÉmkÊ-1
;		MFM_ReadSectors	-pÒesune hlavy & nalouduje sektory do pamÅti
;				PRE: active & ready drive
;				IN: D0.W=ÃÉslo drivu (0-3)
;				    D1.W=prvnÉ sektor (0-1759)
;				    (MFM_cylinders: 0,1,2,3)
;				    D2.W=poÃet sektorÊ
;				    D3.L=adresa uloÚenÉ	
;				OUT:D0.L = 0 if ok else -1
;				    (MFM_Cylinders=bad cylinder)
;------------------------------------------------------------------------------

***** Posun Hlav a NaÃtenÉ SektorÊ Do PamÅti *****
;
MFM_ReadSectors:
;IN: D0.W=ÃÉslo drivu (0-3)
;    D1.W=prvnÉ sektor (0-1759)
;    (MFM_cylinders: 0,1,2,3)
;    D2.W=poÃet sektorÊ (1,2,...n)
;    D3.L=adresa uloÚenÉ	
;OUT:D0.L = 0 if ok else -1
;    (MFM_Cylinders=bad cylinder)
;--------------------------------------
		movem.l	d1-d3/d5-d6/a0/a3,-(a7)
		tst.w	d2
		ble.w	MFM_RSgood		;poÃet sektorÊ<=0
		bsr.w	MFM_MoveOn		;posun hlav
* init *
		lea	MFM_Cylinders(pc),a3
		lea	(a3,d0.w),a3		;a3->akt. cylinder_counter
		lea	MFM_Current(pc),a0
		move.l	d3,(a0)			;->cur. addr. for data
** first cylinder **
		moveq	#0,d5
		move.w	d1,d5			;first sector
		divu	#22,d5
		swap	d5			;d5=first sector in cylinder
		cmpi.w	#10,d5
		bhi.s	MFM_Rside1
* side 0 *
		bset	#2,(a4)			;side0
		bsr.w	MFM_ReadTrack
		move.w	d5,d0
		add.w	d2,d0			;last sector
		subq.w	#1,d0			;correct 11->10 apod.
		cmpi.w	#10,d0
		bls.s	MFM_Rs02
		moveq	#10,d0
MFM_Rs02:	add.w	d5,d2
		sub.w	#11,d2			;d2=d2-(11-first)
		swap	d0
		move.w	d5,d0			;first sector
		move.l	d0,d5			;d5 je rezerva pro TryAgain
		bsr.w	MFM_Decode
		tst.l	d0
		beq.s	MFM_RStest1
		bsr.w	MFM_Rtryagain
		tst.l	d0
		bne.w	MFM_RSerr
MFM_RStest1:	tst.w	d2			;konec?
		ble.w	MFM_RSgood
		moveq	#11,d5			;finta...
* side 1 *
MFM_Rside1:
		bclr	#2,(a4)			;side1
		bsr.w	MFM_ReadTrack
		sub.w	#11,d5			;first sector in track
		move.w	d5,d0
		add.w	d2,d0			;last sector
		subq.w	#1,d0			;correct 11->10 apod.
		cmpi.w	#10,d0
		bls.s	MFM_Rs01
		moveq	#10,d0
MFM_Rs01:	add.w	d5,d2
		sub.w	#11,d2			;d2=d2-(11-first)
		swap	d0
		move.w	d5,d0			;first sector
		move.l	d0,d5			;d5 je rezerva pro TryAgain
		bsr.w	MFM_Decode
		tst.l	d0
		beq.s	MFM_RStest2
		bsr.b	MFM_Rtryagain
		tst.l	d0
		bne.b	MFM_RSerr
MFM_RStest2:
* next cylinder *
MFM_Rnextcyl:	tst.w	d2
		ble.s	MFM_RSgood
		bsr.w	MFM_StepIn
		addq.b	#1,(a3)			;cyl_counter+1
* side 0 *
		bset	#2,(a4)			;side0
		bsr.w	MFM_ReadTrack
		move.w	d2,d0			;last sector
		subq.w	#1,d0			;correct 11->10 apod.
		cmpi.w	#10,d0
		bls.s	MFM_Rs10
		moveq	#10,d0
MFM_Rs10:	sub.w	#11,d2			;d2=d2-11
		swap	d0
		clr.w	d0			;first sector
		move.l	d0,d5
		bsr.b	MFM_Decode
		tst.l	d0
		beq.s	MFM_RStest3
		bsr.b	MFM_Rtryagain
		tst.l	d0
		bne.b	MFM_RSerr
MFM_RStest3:	tst.w	d2			;konec?
		ble.s	MFM_RSgood
* side 1 *
		bclr	#2,(a4)			;side0
		bsr.w	MFM_ReadTrack
		move.w	d2,d0			;last sector
		subq.w	#1,d0			;correct 11->10 apod.
		cmpi.w	#10,d0
		bls.s	MFM_Rs11
		moveq	#10,d0
MFM_Rs11:	sub.w	#11,d2			;d2=d2-11
		swap	d0
		clr.w	d0			;first sector
		move.l	d0,d5
		bsr.b	MFM_Decode
		tst.l	d0
		beq.s	MFM_RStest4
		bsr.b	MFM_Rtryagain
		tst.l	d0
		bne.b	MFM_RSerr
MFM_RStest4:	bra.s	MFM_Rnextcyl
* ok *
MFM_RSgood:	moveq	#0,d0
* nÁvrat *
MFM_RSend:	movem.l	(a7)+,d1-d3/d5-d6/a0/a3
		rts
* error *
MFM_RSerr:	moveq	#-1,d0
		bra.s	MFM_RSend
* try again *
MFM_Rtryagain:
		move.w	MFM_Tries(pc),d6
MFM_RSlop1:	bsr.w	MFM_ReadTrack
		move.l	d5,d0
		bsr.b	MFM_Decode
		tst.l	d0
		beq.s	MFM_RTok
		dbf	d6,MFM_RSlop1		;error-> try again
MFM_RTok:	rts

** Decode 1 Track **
MFM_Decode:			;MFM dekodovani
;IN: D0_low=first sector
;    D0_high=last sector
;OUT:D0.l=0 if ok else -1
;--------------------------------------
		movem.l	d1-d7/a0-a5,-(a7)
		move.l	#$55555555,d3		;mask off in d3
		move.l	MFM_Buffer(pc),a0	;a0->buffer
		lea	$3200(a0),a1		;store track table address !
		move.l	a1,a4
		move.w	#$4489,d6		;standard sync !!
		moveq	#10,d7			;11 sectors
MFM_Dloop0:	cmp.w	(a0)+,d6
		bne.s	MFM_Dloop0
		cmp.w	(a0),d6			;2nd sync ?
		beq.s	MFM_Dloop0		;found branch
MFM_Dloop2:	move.l	a0,a2
		movem.l	(a0),d1-d2		;info part
		and.l	d3,d1
		and.l	d3,d2
		add.l	d1,d1
		or.l	d2,d1			;decode block info
		lsr.w	#8,d1
		move.w	d1,(a1)+		;save sector
		lea	44(a0),a0		;block checksum
		move.l	(a0)+,d5		;block checksum in d5
		moveq	#9,d1			;byte count
MFM_Dbc:	move.l	(a2)+,d4
		eor.l	d4,d5			;eor with checksum
		dbra	d1,MFM_Dbc		;calculate header checksum
		and.l	d3,d5			;mask clock bits
		bne.s	MFM_Derr		;return on error
		addq.l	#4,a0			;align for data checksum
		move.l	(a0)+,(a1)+		;data checksum !!
		move.l	a0,(a1)+		;and address of data
		lea	1024(a0),a0		;next sector
		dbf	d7,MFM_Dloop0		;for all tracks

		move.l	MFM_Current(pc),a1	;address of decoded data
		move.l	a4,a0			;restore track table !
		moveq	#$A,d7			;11 sectors
MFM_Dloop3:	move.w	(a0)+,d1		;d1=sector <0-10>
* check sector *
;IN: D0_low=first sector, D0_high=last sector
		cmp.w	d0,d1
		blo.s	MFM_Dskip		;d1<first
		swap	d0
		cmp.w	d0,d1
		bhi.s	MFM_Dskip2		;d1>last
		swap	d0
		bra.s	MFM_Ddoit
MFM_Dskip2:	swap	d0			;restart d0
MFM_Dskip:	addq.l	#8,a0			;skip this sector
		bra.s	MFM_Dsumok		;next one	
* init data for decode *
MFM_Ddoit:
		moveq	#9,d2
		lsl.w	d2,d1		;current sector*512
		lea	0(a1,d1.w),a3	;multiply by 1 sector and save address
		move.w	d0,d5
		lsl.w	d2,d5		;first sector*512
		sub.w	d5,a3		;a3->correct addr.

		move.l	(a0)+,d2	;checksum
		move.l	(a0)+,a2	;start of data
		lea	512(a2),a5	;next set of datas
* decode 1 sector *
		moveq	#512>>2-1,d4
MFM_Ddcode:	move.l	(a2)+,d5	;data
		move.l	(a5)+,d6	;data
		eor.l	d6,d2
		eor.l	d5,d2		;fix data checksum !!
		and.l	d3,d5
		and.l	d3,d6
		add.l	d5,d5
		or.l	d6,d5
		move.l	d5,(a3)+	;save data
		dbf	d4,MFM_Ddcode	;continue
		and.l	d3,d2		;mask checksum
		bne.s	MFM_Derr	;if sum is ok then branch
MFM_Dsumok:	dbf	d7,MFM_Dloop3
		move.l	d0,d1
		swap	d1
		sub.w	d0,d1
		addq.w	#1,d1		;d1=last-first_sector+1
		moveq	#9,d2
		lsl.w	d2,d1		;d1*512
		ext.l	d1
		lea	MFM_Current(pc),a0
		add.l	d1,(a0)		;addr. for next sector
		moveq	#0,d0		;clear error
MFM_Dend:	movem.l	(a7)+,d1-d7/a0-a5
		rts
MFM_Derr:	moveq	#-1,d0		;an error occured
		bra.s	MFM_Dend

MFM_Current:	dc.l 0

** Load 1 Track **
MFM_ReadTrack:
		movem.w	d1/d3/d6/d7,-(a7)
* init *
		move.w	#$7fff,ADKCON(a6)	;clear disk control !!
		move.w	#$9500,ADKCON(a6)	;set prefs for disk
		move.w	#$4489,DSKSYNC(a6)	;standard sync
* reading *
		move.w	#$8210,DMACON(a6)		;enable disk dma
		move.l	MFM_Buffer(pc),DSKPTH(a6)	;destination
		move.w	#$4000,d6
		move.w	d6,DSKLEN(a6)
		move.w	#$9900,d3
		move.w	d3,DSKLEN(a6)
		move.w	d3,DSKLEN(a6)			;read 6400 mfm words
		moveq	#$2,d7
		move.w	d7,INTREQ(a6) 		;clear DSKBLK interupt request
MFM_RTwait:	move.w	INTREQR(a6),d1
		and.w	d7,d1
		beq.s	MFM_RTwait		;test for DSKBLK
		move.w	d6,DSKLEN(a6)		;no more dma
		move.w	#$10,DMACON(a6)
		movem.w	(a7)+,d1/d3/d6/d7
		rts


***** PÒechod na danÙ cylinder *****
;
MFM_MoveOn:
;IN: D0.W=ÃÉslo drivu
;    D1.W=ÃÉslo sektoru (0-1759)
;---------------------------------------
		movem.l	d0-d1/a0,-(a7)
		ext.l	d1
		divu	#22,d1			;#cylinder
		lea	MFM_Cylinders(pc),a0
		lea	(a0,d0.w),a0
		clr.w	d0
		move.b	(a0),d0
		sub.w	d0,d1
		add.b	d1,(a0)			;new cylinder
		tst.w	d1
		beq.s	MFM_MOend		;no move
		bpl.s	MFM_MOcont		;smÅr IN (d0>0)
* smÅr OUT *
		neg.w	d1			;d1=poÃet cylindrÊ (stepÊ)
		moveq	#-1,d0			;smÅr OUT (d0<0)
* move *
MFM_MOcont:	subq.w	#1,d1
MFM_MOloop:	tst.w	d0
		bpl.s	MFM_MOin
		bsr.b	MFM_StepOut		;Out
		bra.s	MFM_MOwait
MFM_MOin:	bsr.b	MFM_StepIn		;In
MFM_MOwait:	bsr.b	MFM_StepWait
		dbf	d1,MFM_MOloop
MFM_MOend:	movem.l	(a7)+,d0-d1/a0
		rts

***** Je disketa v drivu? *****
;
MFM_DriveIn:
;IN:  D0.w=ÃÉslo drivu (0-3)
;OUT: D0.l=0 if it is else -1
;--------------------------------------
		bsr.w	MFM_ActiveDrive
		move.l	a0,-(a7)
		lea	MFM_DItest(pc),a0
		not.w	(a0)
		bne.s	MFM_DIout
		bsr.b	MFM_StepIn	;Step In
		bra.s	MFM_DIcont
MFM_DIout:	bsr.b	MFM_StepOut	;Step Out
MFM_DIcont:	btst	#2,(a5)
		beq.s	MFM_DIerr
		moveq	#0,d0		;Disk In Drive
		bra.s	MFM_DIend
MFM_DIerr:	moveq	#-1,d0		;Disk Not In Drive
MFM_DIend:	move.l	(a7)+,a0
		rts
MFM_DItest:	dc.w 0


***** KrokovÁnÉ hlav *****
;
MFM_StepIn:
		bclr	#1,(a4)
		bclr	#0,(a4)
		bset	#0,(a4)
		rts
MFM_StepOut:
		bset	#1,(a4)
		bclr	#0,(a4)
		bset	#0,(a4)
		rts


***** ãekej Mezi Kroky *****
;
MFM_StepWait:
;MFM_Step=0 if wait 1 snÉmek
;	 =1 if wait pomocÉ 60-ti ÒÁdek obrazovky
;	 =2 if wait pomocÉ CIAA
;	 =3 if wait pomocÉ CIAB
;--------------------------------------
		movem.l	d0-d1/a0,-(a7)
		move.w	MFM_Step(pc),d0
* 1 frame *
MFM_SWframe:	WAITBLACK
MFM_SWend:	movem.l	(a7)+,d0-d1/a0
		rts


***** Go to Track 00 *****
;
MFM_GoTo00:
;IN: D0.W = ÃÉslo drivu (0-3)
;--------------------------------------
		move.l	a0,-(a7)
		lea	MFM_Cylinders(pc),a0	;clear #Cylinder of drive
		clr.b	(a0,d0.w)
MFM_GTloop:
		btst	#4,(a5)
		beq.s	MFM_GTok
		bsr.w	MFM_StepOut
		bsr.w	MFM_StepWait
		bra.s	MFM_GTloop
MFM_GTok:	move.l	(a7)+,a0
		rts


***** ãekej aÚ je disk pÒipraven *****
;
MFM_DriveReady:				;ÃekÁ vÉc jak 1sec na READY signÁl
;OUT: D0=0 if OK else -1
;--------------------------------------
		moveq	#70,d0
MFM_WDloop:	btst	#5,(a5)
		beq.s	MFM_WDok
		WAITBLACK
		dbf	d0,MFM_WDloop
		moveq	#-1,d0			;disk was removed from drive
		rts
MFM_WDok:	moveq	#0,d0			;ok disk is ready
		rts


***** NaLoaduj Registry *****
;
MFM_LoadRegs:	lea	$bfd100,a4
		lea	$f01(a4),a5
		lea	CUSTOM,a6
		rts


***** Active Drivers DF0-3 *****
;
MFM_ActiveDrive:
;IN: D0.w=ÃÉslo drivu (0-3)
;--------------------------------------
		movem.w	d0-d1,-(a7)
		moveq	#%00001000,d1
		lsl.b	d0,d1			;d1=bit na drive
		moveq	#%01111001,d0
		move.b	d0,(a4)
		eor.b	d1,d0
		move.b	d0,(a4)
		movem.w	(a7)+,d0-d1
		rts


***** Stop Drivers DF0-3 *****
;
MFM_StopDrive:
;IN: D0.w=ÃÉslo drivu (0-3)
;--------------------------------------
		movem.w	d0-d1,-(a7)
		moveq	#%00001000,d1
		lsl.b	d0,d1			;d1=bit na drive
		moveq	#-7,d0			;=%11111001
		move.b	d0,(a4)
		eor.b	d1,d0
		move.b	d0,(a4)
		movem.w	(a7)+,d0-d1
		rts


***** ãekej urÃitÙ poÃet Screen-snÉmkÊ *****
;
MFM_WaitFrames:
;IN: D0.W=poÃet snÉmkÊ-1 (niÃÉ d0)
;--------------------------------------
		move.w	d0,-(a7)
MFM_WFloop:	WAITBLACK
		dbf	d0,MFM_WFloop
		move.w	(a7)+,d0
		rts

;/* end of sector loader */----------------------------------------------------
copper dc.w $180,0,$182,0,$ffff,$fffe

end
xx  ds.b $3300
xx1 ds.b 100000