[avrcpm.git] / avrcpm / avr / dsk_fsys.asm

;    Filesystem functions for the Interaction with BIOS and Disks
;
;    Copyright (C) 2010 Frank Zoll
;
;    This file is part of avrcpm.
;
;    avrcpm is free software: you can redistribute it and/or modify it
;    under the terms of the GNU General Public License as published by
;    the Free Software Foundation, either version 3 of the License, or
;    (at your option) any later version.
;
;    avrcpm is distributed in the hope that it will be useful,
;    but WITHOUT ANY WARRANTY; without even the implied warranty of
;    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
;    GNU General Public License for more details.
;
;    You should have received a copy of the GNU General Public License
;    along with avrcpm.  If not, see <http://www.gnu.org/licenses/>.
;
;    $Id$
;


; ---------------- Defines for the Filesystem Interface -------


;*****************************************************
;*        Disk-Manager constants                     *
;*****************************************************
	.equ	dskType_None	= 0
	.equ	dskType_CPM		= 1
	.equ	dskType_FAT		= 2
	.equ	dskType_RAM		= 3

;*****************************************************
;*         CP/M to host disk constants               *
;*****************************************************
	.equ	MAXDISKS  = 4			;Max number of Disks (partitions)
	.equ    PARTENTRY_SIZE = 9		;Size of a Partitiontableentry
	.equ	blksize = 1024			;CP/M allocation size
	.equ	hostsize = 512			;host disk sector size
;	.equ	hostspt = 20			;host disk sectors/trk
	.equ	hostblk = hostsize/128	;CP/M sects/host buff
;	.equ	CPMSPT = hostblk*hostspt;CP/M sectors/track
	.equ	CPMSPT = 26		;
	.equ	SECMSK = hostblk-1		;sector mask
	.equ	SECSHF = log2(hostblk)	;sector shift

;*****************************************************
;*        BDOS constants on entry to write           *
;*****************************************************
	.equ	WRALL = 0		;write to allocated
	.equ	WRDIR = 1		;write to directory
	.equ	WRUAL = 2		;write to unallocated
	.equ	WRTMSK= 3		;write type mask
;----------------------------------------------- Start of Data Segment

	.dseg

ndisks:			.byte	1		;Number of CP/M disks

seekdsk:		.byte	1		;seek disk number
seektrk:		.byte	2		;seek track number
seeksec:		.byte	1		;seek sector number

unacnt:			.byte	1		;unalloc rec cnt
unadsk:			.byte	1		;last unalloc disk
unatrk:			.byte	2		;last unalloc track
unasec:			.byte	1		;last unalloc sector

erflag:			.byte	1		;error reporting
wrtype:			.byte	1		;write operation type
dmaadr:			.byte	2		;last dma address

hostbuf:		.byte	hostsize ;host buffer (from/to SD-card)
hostparttbl:	.byte	PARTENTRY_SIZE*MAXDISKS ;host partition table (type, start sector, sector count)
hostparttbltop:
hostdsk:		.byte	1		;host disk number
hosttype:		.byte	1		;host disk type (same entry as 1 parition entry)
hostlba:		.byte	3		;host sector number (relative to partition start)


; ------------------------------- Start of Code Segment
	.cseg

; ====================================================================
; Function: Get a Pointer to a Partitiontable entry
; ====================================================================
; Parameters
; --------------------------------------------------------------------
; Registers  : [w] z			Pointer to the Partitionentry
;              [r] xl			Number of Diskentry to Read
; --------------------------------------------------------------------
; Description:
; ====================================================================
dsk_getpartentry:
	
	ldiw	z,hostparttbl
	mov		temp,xl

dsk_getpartentryloop:
	cp		temp,_0
	breq	dsk_getpartentryloopend
	adiw	z,PARTENTRY_SIZE
	dec		temp
	rjmp		dsk_getpartentryloop
dsk_getpartentryloopend:
	ret

; ====================================================================
; Function: 
; ====================================================================
; Parameters
; --------------------------------------------------------------------
; Registers  : none
; Variables  : [r] seeksec		Sector to read
;              [r] seektrk		Track  to read
; --------------------------------------------------------------------
; Description:
; ====================================================================
dskDiskCheck:
	lds		temp2,seekdsk
	cpi		temp2,RAMDISKNR
	brsh	dsk_dchrd			;maybe ramdisk

; Check if selected disk # is less then # of disks.

	lds		temp,ndisks	
	tst		temp
	brne	dsk_dchpart1

; Need to init

	rcall	mgr_init_partitions
	cbr		temp,0x80
	lds		temp2,seekdsk
	
dsk_dchpart1:
	cp		temp2,temp
	brsh	dsk_dcher
	
dsk_dchend:
	ldi		temp,0
	ret

dsk_dchrd:
#if RAMDISKCNT
	cpi		temp,RAMDISKNR+RAMDISKCNT
	brlo	dsk_dchend
#endif
dsk_dcher:
	ldi		temp,0xff			;error return
	ret
	
dskErrorRet:
	lds		temp,erflag
	ret

dskDiskSel:
	sts 	seekdsk,temp
	ret

dskTrackSel_l:
	sts 	seektrk,temp
	sts 	seektrk+1,_0
	ret

dskTrackSel_h:
	sts 	seektrk+1,temp
	ret

dskSecSel:
	sts 	seeksec,temp
	ret

dskDmal:
	sts 	dmaadr,temp
	ret

dskDmah:
	sts 	dmaadr+1,temp
	ret

; ====================================================================
; Function: Does a Disk interaction
; ====================================================================
; Parameters
; --------------------------------------------------------------------
; Registers  : none
; Variables  : [r] seeksec		Sector to read
;              [r] seektrk		Track  to read
; --------------------------------------------------------------------
; Description:
; ====================================================================
dskDoIt:
.if DISK_DEBUG
	push 	temp
	sbrc	temp,READ_FUNC
	rjmp	dskdbgr
	sbrc	temp,WRITE_FUNC
	rjmp	dskdbgw
	rjmp	dskdbge

dskdbgr:
	printnewline
	printstring "Disk read:  "
	rjmp	dskdbg1
dskdbgw:
	printnewline
	printstring "Disk write: "
dskdbg1:
	lds		temp,seekdsk
	subi	temp,-('A')
	rcall	uartputc
	printstring ": track "
	lds 	temp2,seektrk+1
	lds 	temp,seektrk
	rcall 	printhexw
	printstring ", sector "
	lds 	temp,seeksec
	rcall 	printhex
	printstring ", dma-addr "
	lds 	temp2,dmaadr+1
	lds 	temp,dmaadr
	rcall 	printhexw
	pop		temp
	push	temp
	sbrs	temp,WRITE_FUNC
	rjmp	dskdbge
	printstring " wrtype "
	andi	temp,3
	rcall printhex
dskdbge:
	pop temp
.endif
	;See what has to be done.
	sbrc	temp,READ_FUNC
	rjmp	dsk_read
	sbrc	temp,WRITE_FUNC
	rjmp	dsk_write
	sbrc	temp,HOME_FUNC
	rjmp	dsk_home
	sbrc	temp,BOOT_FUNC
	rjmp	dsk_boot

	printstring "DISK I/O: Invalid Function code: "
	rcall	printhex
	rjmp	haltinv

dsk_boot:
	sts		ndisks,_0			;no active partitions
dsk_cboot:
	cbi		flags,hostact		;host buffer inactive
	sts		unacnt,_0			;clear unalloc count
	ret

dsk_home:
	sbis	flags,hostwrt		;check for pending write
	cbi		flags,hostact		;clear host active flag
	ret


; ====================================================================
; Function: Does a Disk read operation
; ====================================================================
; Parameters
; --------------------------------------------------------------------
; Registers  : none
; Variables  : [r] seekdsk		Number of Disk to Read
;			   [r] seeksec		Sector to read
;              [r] seektrk		Track  to read
; --------------------------------------------------------------------
; Description:
; ====================================================================
dsk_read:
	sts		erflag,_0
	sbi		flags,readop		; Set read operation flag
	lds     xl,seekdsk
	rcall 	dsk_getpartentry	; Get Paritiontableentry
	ld    	temp,z				; Get Partitiontype

; Isn't it a Disk ?
	cpi		temp,dskType_None
	brne	PC+2
	rjmp	dsk_read_err
; Is it a RamDisk ?
	cpi		temp,dskType_RAM
	brne	PC+2
	rjmp	rdsk_read
; It must be a FAT16-Imagefile or CP/M Partition.
	sts		unacnt,_0
	sbi		flags,rsflag		;must read data
	ldi		temp,WRUAL			;write type
	sts		wrtype,temp			;treat as unalloc
	rjmp	dsk_rwoper			;to perform the read

dsk_read_err:
	ret

; ====================================================================
; Function: Does a Disk write operation
; ====================================================================
; Parameters
; --------------------------------------------------------------------
; Registers  : none
; Variables  : [r] seekdsk		Number of Disk to Read
;			   [r] seeksec		Sector to read
;              [r] seektrk		Track  to read
; --------------------------------------------------------------------
; Description:
; ====================================================================
dsk_write:
	;write the selected sector
	sts		erflag,_0
	cbi		flags,readop		; not a read operation
	lds     xl,seekdsk
	rcall 	dsk_getpartentry	; Get Paritiontableentry
	ld    	temp,z				; Get Partitiontype

; Isn't it a Disk ?
	cpi		temp,dskType_None
	brne	PC+2
	rjmp	dsk_write_err

; Is it a RamDisk ?
	cpi		temp,dskType_RAM
	brne	PC+2
	rjmp	rdsk_write

; It must be a FAT16-Imagefile or CP/M Partition.

	cbi		flags,readop		;not a read operation

	andi	temp,WRTMSK
	sts		wrtype,temp			;save write type

	cpi		temp,WRUAL			;write unallocated?
	brne	dsk_chkuna			;check for unalloc

;	write to unallocated, set parameters
	ldi		temp,blksize/128	;next unalloc recs
	sts		unacnt,temp
	lds		temp,seekdsk		;disk to seek
	sts		unadsk,temp			;unadsk = sekdsk
	lds		temp,seektrk
	sts		unatrk,temp			;unatrk = sectrk
	lds		temp,seektrk+1
	sts		unatrk+1,temp		;unatrk = sectrk
	lds		temp,seeksec
	sts		unasec,temp			;unasec = seksec
;
dsk_chkuna:
	;check for write to unallocated sector
	lds		temp,unacnt			;any unalloc remain?
	tst		temp
	breq	dsk_alloc			;skip if not

;	more unallocated records remain
	dec		temp				;unacnt = unacnt-1
	sts		unacnt,temp
	lds		temp,seekdsk		;same disk?
	lds		temp2,unadsk
	cp		temp,temp2			;seekdsk = unadsk?
	brne	dsk_alloc			;skip if not

;	disks are the same
	lds		temp,unatrk
	lds		temp2,unatrk+1
	lds		temp3,seektrk
	lds		temp4,seektrk+1
	cp		temp,temp3			;seektrk = unatrk?
	cpc		temp2,temp4
	brne	dsk_alloc			;skip if not

;	tracks are the same
	lds		temp,seeksec		;same sector?
	lds		temp2,unasec
	cp		temp,temp2			;seeksec = unasec?
	brne	dsk_alloc			;skip if not

;	match, move to next sector for future ref
	inc		temp2				;unasec = unasec+1
	sts		unasec,temp2
	cpi		temp2,CPMSPT		;end of track? (count CP/M sectors)
	brlo	dsk_noovf			;skip if no overflow

;	overflow to next track
	sts		unasec,_0			;unasec = 0
	lds		temp,unatrk
	lds		temp2,unatrk+1
	subi	temp, low(-1)		;unatrk = unatrk+1
	sbci	temp2,high(-1)
	sts		unatrk,temp
	sts		unatrk+1,temp2
;
dsk_noovf:
	cbi		flags,rsflag		;rsflag = 0
	rjmp	dsk_rwoper			;to perform the write
;
dsk_alloc:
	;not an unallocated record, requires pre-read
	sts		unacnt,_0			;unacnt = 0
	sbi		flags,rsflag		;rsflag = 1
	rjmp	dsk_rwoper

dsk_write_err:
	ret

; ====================================================================
; Function: Does a Disk read/write operation
; ====================================================================
; Parameters
; --------------------------------------------------------------------
; Registers  : none
; Variables  : [r] seekdsk		Number of Disk to Read
;			   [r] seeksec		Sector to read
;              [r] seektrk		Track  to read
; --------------------------------------------------------------------
; Description:
; ====================================================================
dsk_rwoper:
	;enter here to perform the read/write
.if DISK_DEBUG
	printstring ", flags: "
	in		temp,flags
	rcall	printhex
.endif
	sts		erflag,_0	;no errors (yet)

	;Convert track/sector to an LBA address (in 128byte blocks)

	lds		xl,seeksec			;
	ldi		xh,0				;
	ldi		yl,0				;
	lds		temp3,seektrk		;
	lds		temp4,seektrk+1		;
	ldi		temp,CPMSPT			;
	mul		temp3,temp			;
	add		xl,r0				;
	adc		xh,r1				;
	mul		temp4,temp			;
	add		xh,r0				;yl:xh:xl := sec + trk * SectorsPerTrack
	adc		yl,r1				;

	mov		temp,xl
	andi	temp,SECMSK			;mask buffer number
	push	temp				;save for later

	;Convert from CP/M LBA blocks to host LBA blocks
	ldi		temp,SECSHF
dsk_sh1:
	lsr		yl
	ror		xh
	ror		xl
	dec		temp
	brne	dsk_sh1
					;yl:xh:xl = host block to seek
;	active host sector?
	in		_tmp0,flags			;host active flag
	sbi		flags,hostact		;always becomes 1
	sbrs	_tmp0,hostact		;was it already?
	rjmp	dsk_filhst			;fill host if not

;	host buffer active, same as seek buffer?
	lds		temp,seekdsk
	lds		temp2,hostdsk		;same disk?
	cp		temp,temp2			;seekdsk = hostdsk?
	brne	dsk_nomatch

;	same disk, same block?
	lds		temp,hostlba
	lds		temp2,hostlba+1
	lds		temp3,hostlba+2
	cp		xl,temp
	cpc		xh,temp2
	cpc		yl,temp3
	breq	dsk_match
;
dsk_nomatch:
	;proper disk, but not correct sector
	sbis	flags,hostwrt		;host written?
	rjmp	dsk_filhst
	push	xl
	push	xh
	push	yl
	rcall	dsk_writehost		;clear host buff
	pop		yl
	pop		xh
	pop		xl

dsk_filhst:
	;may have to fill the host buffer
	lds		temp,seekdsk
	sts		hostdsk,temp
	sts		hostlba,xl
	sts		hostlba+1,xh
	sts		hostlba+2,yl

	sbic	flags,rsflag		;need to read?
	rcall	dsk_readhost		;yes, if 1
	cbi		flags,hostwrt		;no pending write

dsk_match:

	;copy data to or from buffer
	ldiw	z,hostbuf
	ldi		temp,128
	pop		temp2				;get buffer number (which part of hostbuf)
	mul		temp2,temp
	add		zl,r0				;offset in hostbuf
	adc		zh,r1

.if DISK_DEBUG > 2
	push	r0
	push	r1
	printstring "; host buf adr: "
	pop		temp2
	pop		temp
	rcall 	printhexw
.endif

	lds		xl,dmaadr
	lds		xh,dmaadr+1
	ldi		temp3,128			;length of move
	sbic	flags,readop		;which way?
	rjmp	dsk_rmove			;skip if read

;	mark write operation
	sbi		flags,hostwrt		;hostwrt = 1
dsk_wmove:
	mem_read
	st		z+,temp
	adiw	xl,1
	dec		temp3
	brne 	dsk_wmove
	rjmp	dsk_rwmfin
	
dsk_rmove:
	ld		temp,z+
	mem_write
	adiw	xl,1
	dec		temp3
	brne	dsk_rmove
dsk_rwmfin:
;	data has been moved to/from host buffer
	lds		temp,wrtype			;write type
	cpi		temp,WRDIR			;to directory?
	breq	dsk_wdir
	ret							;no further processing
dsk_wdir:
;	clear host buffer for directory write
	lds		temp,erflag
	tst		temp				;errors?
	breq	dsk_wdir1
	ret							;skip if so
dsk_wdir1:
	rcall	dsk_writehost		;clear host buff
	cbi		flags,hostwrt		;buffer written
	ret

; ====================================================================
; Function: Does a Disk write operation
; ====================================================================
; Parameters
; --------------------------------------------------------------------
; Registers  : none
; Variables  : [r] seekdsk		Number of Disk to Read
;			   [r] seeksec		Sector to read
;              [r] seektrk		Track  to read
; --------------------------------------------------------------------
; Description:
; ====================================================================
dsk_writehost:
	lds    xl,hostdsk
	rcall  dsk_getpartentry
	ld     temp,z

#if FAT16_SUPPORT
; Is it a FAT16 Diskimage ?
	cpi		temp,dskType_FAT
	brne	PC+2
	rjmp	fat_writehost
#endif

; Is it a CP/M Partition ?
	cpi		temp,dskType_CPM
	brne	PC+2
	rjmp	cpm_writehost
; Disktype not supported -> Return 
	ret

; ====================================================================
; Function: Does a Disk read operation
; ====================================================================
; Parameters
; --------------------------------------------------------------------
; Registers  : none
; Variables  : [r] seekdsk		Number of Disk to Read
;			   [r] seeksec		Sector to read
;              [r] seektrk		Track  to read
; --------------------------------------------------------------------
; Description:
; ====================================================================
dsk_readhost:
	lds    xl,hostdsk
	rcall  dsk_getpartentry
	ld     temp,z

#if FAT16_SUPPORT
; Is it a FAT16 Diskimage ?
	cpi		temp,dskType_FAT
	brne	PC+2
	rjmp	fat_readhost
#endif

; Is it a CP/M Partition ?
	cpi		temp,dskType_CPM
	brne	PC+2
	rjmp	cpm_readhost
; Disktype not supported -> Return 
	ret
Commit	Line	Data
64219415 FZ	1	; Filesystem functions for the Interaction with BIOS and Disks
	2	;
	3	; Copyright (C) 2010 Frank Zoll
	4	;
	5	; This file is part of avrcpm.
	6	;
	7	; avrcpm is free software: you can redistribute it and/or modify it
	8	; under the terms of the GNU General Public License as published by
	9	; the Free Software Foundation, either version 3 of the License, or
	10	; (at your option) any later version.
	11	;
	12	; avrcpm is distributed in the hope that it will be useful,
	13	; but WITHOUT ANY WARRANTY; without even the implied warranty of
	14	; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	15	; GNU General Public License for more details.
	16	;
	17	; You should have received a copy of the GNU General Public License
	18	; along with avrcpm. If not, see <http://www.gnu.org/licenses/>.
	19	;
	20	; $Id$
b741422e L	21	;
	22
	23
64219415 FZ	24	; ---------------- Defines for the Filesystem Interface -------
64219415 FZ	25
b741422e	26
64219415 FZ	27	;*****************************************************
	28	;* Disk-Manager constants *
	29	;*****************************************************
b741422e L	30	.equ dskType_None = 0
	31	.equ dskType_CPM = 1
	32	.equ dskType_FAT = 2
	33	.equ dskType_RAM = 3
64219415 FZ	34
	35	;*****************************************************
	36	;* CP/M to host disk constants *
	37	;*****************************************************
b741422e	38	.equ MAXDISKS = 4 ;Max number of Disks (partitions)
64219415 FZ	39	.equ PARTENTRY_SIZE = 9 ;Size of a Partitiontableentry
	40	.equ blksize = 1024 ;CP/M allocation size
	41	.equ hostsize = 512 ;host disk sector size
	42	; .equ hostspt = 20 ;host disk sectors/trk
	43	.equ hostblk = hostsize/128 ;CP/M sects/host buff
	44	; .equ CPMSPT = hostblk*hostspt;CP/M sectors/track
	45	.equ CPMSPT = 26 ;
	46	.equ SECMSK = hostblk-1 ;sector mask
	47	.equ SECSHF = log2(hostblk) ;sector shift
	48
	49	;*****************************************************
	50	;* BDOS constants on entry to write *
	51	;*****************************************************
	52	.equ WRALL = 0 ;write to allocated
	53	.equ WRDIR = 1 ;write to directory
	54	.equ WRUAL = 2 ;write to unallocated
b741422e	55	.equ WRTMSK= 3 ;write type mask
64219415 FZ	56	;----------------------------------------------- Start of Data Segment
64219415 FZ	57
b741422e L	58	.dseg
b741422e L	59
64219415 FZ	60	ndisks: .byte 1 ;Number of CP/M disks
	61
	62	seekdsk: .byte 1 ;seek disk number
	63	seektrk: .byte 2 ;seek track number
	64	seeksec: .byte 1 ;seek sector number
	65
	66	unacnt: .byte 1 ;unalloc rec cnt
	67	unadsk: .byte 1 ;last unalloc disk
	68	unatrk: .byte 2 ;last unalloc track
	69	unasec: .byte 1 ;last unalloc sector
	70
	71	erflag: .byte 1 ;error reporting
	72	wrtype: .byte 1 ;write operation type
b741422e	73	dmaadr: .byte 2 ;last dma address
64219415	74
b741422e	75	hostbuf: .byte hostsize ;host buffer (from/to SD-card)
64219415 FZ	76	hostparttbl: .byte PARTENTRY_SIZE*MAXDISKS ;host partition table (type, start sector, sector count)
	77	hostparttbltop:
	78	hostdsk: .byte 1 ;host disk number
b741422e L	79	hosttype: .byte 1 ;host disk type (same entry as 1 parition entry)
	80	hostlba: .byte 3 ;host sector number (relative to partition start)
	81
	82
64219415	83	; ------------------------------- Start of Code Segment
b741422e L	84	.cseg
	85
	86	; ====================================================================
64219415	87	; Function: Get a Pointer to a Partitiontable entry
b741422e L	88	; ====================================================================
	89	; Parameters
	90	; --------------------------------------------------------------------
	91	; Registers : [w] z Pointer to the Partitionentry
	92	; [r] xl Number of Diskentry to Read
	93	; --------------------------------------------------------------------
	94	; Description:
	95	; ====================================================================
	96	dsk_getpartentry:
	97
	98	ldiw z,hostparttbl
	99	mov temp,xl
	100
	101	dsk_getpartentryloop:
	102	cp temp,_0
	103	breq dsk_getpartentryloopend
	104	adiw z,PARTENTRY_SIZE
	105	dec temp
	106	rjmp dsk_getpartentryloop
	107	dsk_getpartentryloopend:
64219415	108	ret
b741422e L	109
b741422e L	110	; ====================================================================
64219415	111	; Function:
b741422e L	112	; ====================================================================
	113	; Parameters
	114	; --------------------------------------------------------------------
	115	; Registers : none
	116	; Variables : [r] seeksec Sector to read
	117	; [r] seektrk Track to read
	118	; --------------------------------------------------------------------
	119	; Description:
	120	; ====================================================================
64219415 FZ	121	dskDiskCheck:
	122	lds temp2,seekdsk
	123	cpi temp2,RAMDISKNR
	124	brsh dsk_dchrd ;maybe ramdisk
	125
	126	; Check if selected disk # is less then # of disks.
	127
	128	lds temp,ndisks
	129	tst temp
	130	brne dsk_dchpart1
	131
	132	; Need to init
	133
	134	rcall mgr_init_partitions
	135	cbr temp,0x80
	136	lds temp2,seekdsk
	137
	138	dsk_dchpart1:
	139	cp temp2,temp
	140	brsh dsk_dcher
	141
	142	dsk_dchend:
	143	ldi temp,0
	144	ret
	145
	146	dsk_dchrd:
	147	#if RAMDISKCNT
	148	cpi temp,RAMDISKNR+RAMDISKCNT
	149	brlo dsk_dchend
	150	#endif
	151	dsk_dcher:
	152	ldi temp,0xff ;error return
	153	ret
	154
	155	dskErrorRet:
	156	lds temp,erflag
	157	ret
	158
	159	dskDiskSel:
	160	sts seekdsk,temp
	161	ret
	162
	163	dskTrackSel_l:
	164	sts seektrk,temp
	165	sts seektrk+1,_0
	166	ret
	167
	168	dskTrackSel_h:
	169	sts seektrk+1,temp
	170	ret
	171
	172	dskSecSel:
	173	sts seeksec,temp
	174	ret
	175
	176	dskDmal:
	177	sts dmaadr,temp
	178	ret
	179
	180	dskDmah:
	181	sts dmaadr+1,temp
b741422e	182	ret
64219415	183
b741422e	184	; ====================================================================
64219415	185	; Function: Does a Disk interaction
b741422e L	186	; ====================================================================
	187	; Parameters
	188	; --------------------------------------------------------------------
	189	; Registers : none
	190	; Variables : [r] seeksec Sector to read
	191	; [r] seektrk Track to read
	192	; --------------------------------------------------------------------
	193	; Description:
64219415 FZ	194	; ====================================================================
	195	dskDoIt:
	196	.if DISK_DEBUG
	197	push temp
	198	sbrc temp,READ_FUNC
	199	rjmp dskdbgr
	200	sbrc temp,WRITE_FUNC
	201	rjmp dskdbgw
	202	rjmp dskdbge
	203
	204	dskdbgr:
	205	printnewline
	206	printstring "Disk read: "
	207	rjmp dskdbg1
	208	dskdbgw:
	209	printnewline
	210	printstring "Disk write: "
	211	dskdbg1:
	212	lds temp,seekdsk
	213	subi temp,-('A')
	214	rcall uartputc
	215	printstring ": track "
	216	lds temp2,seektrk+1
	217	lds temp,seektrk
	218	rcall printhexw
	219	printstring ", sector "
	220	lds temp,seeksec
	221	rcall printhex
	222	printstring ", dma-addr "
	223	lds temp2,dmaadr+1
	224	lds temp,dmaadr
	225	rcall printhexw
	226	pop temp
	227	push temp
	228	sbrs temp,WRITE_FUNC
	229	rjmp dskdbge
	230	printstring " wrtype "
	231	andi temp,3
	232	rcall printhex
	233	dskdbge:
	234	pop temp
	235	.endif
	236	;See what has to be done.
	237	sbrc temp,READ_FUNC
	238	rjmp dsk_read
	239	sbrc temp,WRITE_FUNC
	240	rjmp dsk_write
	241	sbrc temp,HOME_FUNC
	242	rjmp dsk_home
	243	sbrc temp,BOOT_FUNC
	244	rjmp dsk_boot
	245
	246	printstring "DISK I/O: Invalid Function code: "
	247	rcall printhex
	248	rjmp haltinv
	249
	250	dsk_boot:
	251	sts ndisks,_0 ;no active partitions
	252	dsk_cboot:
	253	cbi flags,hostact ;host buffer inactive
	254	sts unacnt,_0 ;clear unalloc count
	255	ret
	256
	257	dsk_home:
258	sbis flags,hostwrt ;check for pending write
259	cbi flags,hostact ;clear host active flag
260	ret
64219415	261
b741422e L	262
	263
	264	; ====================================================================
64219415	265	; Function: Does a Disk read operation
b741422e L	266	; ====================================================================
	267	; Parameters
	268	; --------------------------------------------------------------------
	269	; Registers : none
	270	; Variables : [r] seekdsk Number of Disk to Read
	271	; [r] seeksec Sector to read
	272	; [r] seektrk Track to read
	273	; --------------------------------------------------------------------
	274	; Description:
64219415 FZ	275	; ====================================================================
	276	dsk_read:
	277	sts erflag,_0
b741422e	278	sbi flags,readop ; Set read operation flag
64219415	279	lds xl,seekdsk
b741422e L	280	rcall dsk_getpartentry ; Get Paritiontableentry
	281	ld temp,z ; Get Partitiontype
	282
	283	; Isn't it a Disk ?
	284	cpi temp,dskType_None
	285	brne PC+2
	286	rjmp dsk_read_err
64219415	287	; Is it a RamDisk ?
b741422e L	288	cpi temp,dskType_RAM
	289	brne PC+2
	290	rjmp rdsk_read
	291	; It must be a FAT16-Imagefile or CP/M Partition.
64219415 FZ	292	sts unacnt,_0
	293	sbi flags,rsflag ;must read data
	294	ldi temp,WRUAL ;write type
	295	sts wrtype,temp ;treat as unalloc
b741422e	296	rjmp dsk_rwoper ;to perform the read
64219415	297
b741422e L	298	dsk_read_err:
b741422e L	299	ret
64219415	300
b741422e	301	; ====================================================================
64219415	302	; Function: Does a Disk write operation
b741422e L	303	; ====================================================================
	304	; Parameters
	305	; --------------------------------------------------------------------
	306	; Registers : none
	307	; Variables : [r] seekdsk Number of Disk to Read
	308	; [r] seeksec Sector to read
	309	; [r] seektrk Track to read
	310	; --------------------------------------------------------------------
	311	; Description:
64219415 FZ	312	; ====================================================================
	313	dsk_write:
	314	;write the selected sector
	315	sts erflag,_0
b741422e	316	cbi flags,readop ; not a read operation
64219415	317	lds xl,seekdsk
b741422e L	318	rcall dsk_getpartentry ; Get Paritiontableentry
	319	ld temp,z ; Get Partitiontype
	320
	321	; Isn't it a Disk ?
	322	cpi temp,dskType_None
	323	brne PC+2
	324	rjmp dsk_write_err
	325
64219415	326	; Is it a RamDisk ?
b741422e L	327	cpi temp,dskType_RAM
	328	brne PC+2
	329	rjmp rdsk_write
	330
	331	; It must be a FAT16-Imagefile or CP/M Partition.
64219415 FZ	332
	333	cbi flags,readop ;not a read operation
	334
	335	andi temp,WRTMSK
	336	sts wrtype,temp ;save write type
	337
	338	cpi temp,WRUAL ;write unallocated?
	339	brne dsk_chkuna ;check for unalloc
	340
	341	; write to unallocated, set parameters
	342	ldi temp,blksize/128 ;next unalloc recs
	343	sts unacnt,temp
	344	lds temp,seekdsk ;disk to seek
	345	sts unadsk,temp ;unadsk = sekdsk
	346	lds temp,seektrk
	347	sts unatrk,temp ;unatrk = sectrk
	348	lds temp,seektrk+1
	349	sts unatrk+1,temp ;unatrk = sectrk
	350	lds temp,seeksec
	351	sts unasec,temp ;unasec = seksec
	352	;
	353	dsk_chkuna:
	354	;check for write to unallocated sector
	355	lds temp,unacnt ;any unalloc remain?
	356	tst temp
	357	breq dsk_alloc ;skip if not
	358
	359	; more unallocated records remain
	360	dec temp ;unacnt = unacnt-1
	361	sts unacnt,temp
	362	lds temp,seekdsk ;same disk?
	363	lds temp2,unadsk
	364	cp temp,temp2 ;seekdsk = unadsk?
	365	brne dsk_alloc ;skip if not
	366
	367	; disks are the same
	368	lds temp,unatrk
	369	lds temp2,unatrk+1
	370	lds temp3,seektrk
	371	lds temp4,seektrk+1
	372	cp temp,temp3 ;seektrk = unatrk?
	373	cpc temp2,temp4
	374	brne dsk_alloc ;skip if not
	375
	376	; tracks are the same
	377	lds temp,seeksec ;same sector?
	378	lds temp2,unasec
	379	cp temp,temp2 ;seeksec = unasec?
	380	brne dsk_alloc ;skip if not
	381
	382	; match, move to next sector for future ref
	383	inc temp2 ;unasec = unasec+1
	384	sts unasec,temp2
	385	cpi temp2,CPMSPT ;end of track? (count CP/M sectors)
	386	brlo dsk_noovf ;skip if no overflow
	387
	388	; overflow to next track
	389	sts unasec,_0 ;unasec = 0
	390	lds temp,unatrk
	391	lds temp2,unatrk+1
	392	subi temp, low(-1) ;unatrk = unatrk+1
	393	sbci temp2,high(-1)
	394	sts unatrk,temp
	395	sts unatrk+1,temp2
396	;
397	dsk_noovf:
398	cbi flags,rsflag ;rsflag = 0
399	rjmp dsk_rwoper ;to perform the write
400	;
401	dsk_alloc:
402	;not an unallocated record, requires pre-read
403	sts unacnt,_0 ;unacnt = 0
404	sbi flags,rsflag ;rsflag = 1
b741422e L	405	rjmp dsk_rwoper
	406
	407	dsk_write_err:
	408	ret
	409
	410	; ====================================================================
64219415	411	; Function: Does a Disk read/write operation
b741422e L	412	; ====================================================================
	413	; Parameters
	414	; --------------------------------------------------------------------
	415	; Registers : none
	416	; Variables : [r] seekdsk Number of Disk to Read
	417	; [r] seeksec Sector to read
	418	; [r] seektrk Track to read
	419	; --------------------------------------------------------------------
	420	; Description:
	421	; ====================================================================
	422	dsk_rwoper:
64219415 FZ	423	;enter here to perform the read/write
	424	.if DISK_DEBUG
	425	printstring ", flags: "
	426	in temp,flags
	427	rcall printhex
	428	.endif
	429	sts erflag,_0 ;no errors (yet)
	430
	431	;Convert track/sector to an LBA address (in 128byte blocks)
	432
	433	lds xl,seeksec ;
	434	ldi xh,0 ;
	435	ldi yl,0 ;
	436	lds temp3,seektrk ;
	437	lds temp4,seektrk+1 ;
	438	ldi temp,CPMSPT ;
	439	mul temp3,temp ;
	440	add xl,r0 ;
	441	adc xh,r1 ;
	442	mul temp4,temp ;
	443	add xh,r0 ;yl:xh:xl := sec + trk * SectorsPerTrack
	444	adc yl,r1 ;
	445
	446	mov temp,xl
	447	andi temp,SECMSK ;mask buffer number
	448	push temp ;save for later
	449
	450	;Convert from CP/M LBA blocks to host LBA blocks
	451	ldi temp,SECSHF
	452	dsk_sh1:
	453	lsr yl
	454	ror xh
	455	ror xl
	456	dec temp
	457	brne dsk_sh1
	458	;yl:xh:xl = host block to seek
	459	; active host sector?
	460	in _tmp0,flags ;host active flag
	461	sbi flags,hostact ;always becomes 1
	462	sbrs _tmp0,hostact ;was it already?
	463	rjmp dsk_filhst ;fill host if not
	464
	465	; host buffer active, same as seek buffer?
	466	lds temp,seekdsk
	467	lds temp2,hostdsk ;same disk?
	468	cp temp,temp2 ;seekdsk = hostdsk?
	469	brne dsk_nomatch
	470
	471	; same disk, same block?
	472	lds temp,hostlba
	473	lds temp2,hostlba+1
	474	lds temp3,hostlba+2
	475	cp xl,temp
	476	cpc xh,temp2
	477	cpc yl,temp3
	478	breq dsk_match
	479	;
	480	dsk_nomatch:
	481	;proper disk, but not correct sector
	482	sbis flags,hostwrt ;host written?
	483	rjmp dsk_filhst
	484	push xl
	485	push xh
	486	push yl
487	rcall dsk_writehost ;clear host buff
488	pop yl
489	pop xh
490	pop xl
491
492	dsk_filhst:
493	;may have to fill the host buffer
494	lds temp,seekdsk
495	sts hostdsk,temp
496	sts hostlba,xl
497	sts hostlba+1,xh
498	sts hostlba+2,yl
499
500	sbic flags,rsflag ;need to read?
501	rcall dsk_readhost ;yes, if 1
502	cbi flags,hostwrt ;no pending write
503
504	dsk_match:
505
506	;copy data to or from buffer
507	ldiw z,hostbuf
508	ldi temp,128
509	pop temp2 ;get buffer number (which part of hostbuf)
510	mul temp2,temp
511	add zl,r0 ;offset in hostbuf
b741422e	512	adc zh,r1
64219415 FZ	513
	514	.if DISK_DEBUG > 2
	515	push r0
	516	push r1
	517	printstring "; host buf adr: "
	518	pop temp2
	519	pop temp
	520	rcall printhexw
	521	.endif
	522
	523	lds xl,dmaadr
	524	lds xh,dmaadr+1
	525	ldi temp3,128 ;length of move
	526	sbic flags,readop ;which way?
	527	rjmp dsk_rmove ;skip if read
	528
	529	; mark write operation
	530	sbi flags,hostwrt ;hostwrt = 1
	531	dsk_wmove:
	532	mem_read
	533	st z+,temp
	534	adiw xl,1
	535	dec temp3
	536	brne dsk_wmove
	537	rjmp dsk_rwmfin
	538
	539	dsk_rmove:
	540	ld temp,z+
	541	mem_write
	542	adiw xl,1
	543	dec temp3
	544	brne dsk_rmove
	545	dsk_rwmfin:
	546	; data has been moved to/from host buffer
	547	lds temp,wrtype ;write type
	548	cpi temp,WRDIR ;to directory?
	549	breq dsk_wdir
	550	ret ;no further processing
	551	dsk_wdir:
	552	; clear host buffer for directory write
	553	lds temp,erflag
	554	tst temp ;errors?
	555	breq dsk_wdir1
	556	ret ;skip if so
	557	dsk_wdir1:
	558	rcall dsk_writehost ;clear host buff
	559	cbi flags,hostwrt ;buffer written
b741422e L	560	ret
	561
	562	; ====================================================================
64219415	563	; Function: Does a Disk write operation
b741422e L	564	; ====================================================================
	565	; Parameters
	566	; --------------------------------------------------------------------
	567	; Registers : none
	568	; Variables : [r] seekdsk Number of Disk to Read
	569	; [r] seeksec Sector to read
	570	; [r] seektrk Track to read
	571	; --------------------------------------------------------------------
	572	; Description:
	573	; ====================================================================
	574	dsk_writehost:
	575	lds xl,hostdsk
	576	rcall dsk_getpartentry
	577	ld temp,z
	578
	579	#if FAT16_SUPPORT
64219415	580	; Is it a FAT16 Diskimage ?
b741422e L	581	cpi temp,dskType_FAT
	582	brne PC+2
	583	rjmp fat_writehost
	584	#endif
	585
64219415	586	; Is it a CP/M Partition ?
b741422e L	587	cpi temp,dskType_CPM
	588	brne PC+2
	589	rjmp cpm_writehost
	590	; Disktype not supported -> Return
	591	ret
	592
	593	; ====================================================================
64219415	594	; Function: Does a Disk read operation
b741422e L	595	; ====================================================================
	596	; Parameters
	597	; --------------------------------------------------------------------
	598	; Registers : none
	599	; Variables : [r] seekdsk Number of Disk to Read
	600	; [r] seeksec Sector to read
	601	; [r] seektrk Track to read
	602	; --------------------------------------------------------------------
	603	; Description:
	604	; ====================================================================
	605	dsk_readhost:
	606	lds xl,hostdsk
	607	rcall dsk_getpartentry
	608	ld temp,z
	609
	610	#if FAT16_SUPPORT
64219415	611	; Is it a FAT16 Diskimage ?
b741422e L	612	cpi temp,dskType_FAT
	613	brne PC+2
	614	rjmp fat_readhost
	615	#endif
	616
64219415	617	; Is it a CP/M Partition ?
b741422e L	618	cpi temp,dskType_CPM
	619	brne PC+2
	620	rjmp cpm_readhost
	621	; Disktype not supported -> Return
	622	ret
	623