;    Various functions: init, (RAM) disk, mmc, timer
;    This file needs to get split up.
;
;    Copyright (C) 2010 Sprite_tm
;    Copyright (C) 2010 Leo C.
;
;    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$
;


; ------------------- DRAM Refresh Interrupt --------------------

	.cseg
; refresh interupt; exec 2 cbr cycles
refrint:			;4
	.org OC2Aaddr
	rjmp refrint		; tim2cmpa

	.org refrint
	sbis	P_RAS,ram_ras	;2
	reti
				;       CAS  RAS  
	cbi	P_CAS,ram_cas	;2       1|   1|  
				;        1|   1|  
	cbi	P_RAS,ram_ras	;2      |0    1|  
				;       |0    1|  
	nop			;1      |0   |0   
;	nop			;1      |0   |0   
	sbi	P_RAS,ram_ras	;2      |0   |0   
				;       |0   |0   
	dram_wait DRAM_WAITSTATES-1 ;   |    |
;	nop			;1      |0   |0   
	cbi	P_RAS,ram_ras	;2      |0    1|  
				;       |0    1|  
	sbi	P_CAS,ram_cas	;2      |0   |0   
				;       |0   |0   
	sbi	P_RAS,ram_ras	;2       1|  |0   
				;        1|   1|  
	reti			;4  --> 21 cycles




;Print a unsigned lonng value to the uart
; temp4:temp3:temp2:temp = value

print_ultoa:
	push	yh
	push	yl
	push	z_flags
	push	temp4
	push	temp3
	push	temp2
	push	temp
				
	clr	yl		;yl = stack level

ultoa1:	ldi	z_flags, 32	;yh = temp4:temp % 10
	clr	yh		;temp4:temp /= 10
ultoa2:	lsl	temp	
	rol	temp2	
	rol	temp3	
	rol	temp4	
	rol	yh	
	cpi	yh,10	
	brcs	ultoa3	
	subi	yh,10	
	inc	temp
ultoa3:	dec	z_flags	
	brne	ultoa2
	cpi	yh, 10	;yh is a numeral digit '0'-'9'
	subi	yh, -'0'
	push	yh		;Stack it
	inc	yl	
	cp	temp,_0		;Repeat until temp4:temp gets zero
	cpc	temp2,_0
	cpc	temp3,_0
	cpc	temp4,_0
	brne	ultoa1	
	
	ldi	temp, '0'
ultoa5:	cpi	yl,3		; at least 3 digits (ms)
	brge	ultoa6
	push	temp	
	inc	yl
	rjmp	ultoa5

ultoa6:	pop	temp		;Flush stacked digits
	rcall	uartputc
	dec	yl	
	brne	ultoa6	

	pop	temp
	pop	temp2
	pop	temp3
	pop	temp4
	pop	z_flags
	pop	yl
	pop	yh
	ret


;Prints temp2:temp in hex to the uart
printhexw:
	push	temp
	mov	temp,temp2
	rcall	printhex
	pop	temp
	;fall thru

;Prints temp in hex to the uart
printhex:
	swap temp
	rcall printhexn
	swap temp	
	;fall thru

;Prints the lower nibble
printhexn:
	push temp
	andi temp,0xf
	cpi temp,0xA
	brlo printhexn_isno
	subi temp,-7
printhexn_isno:
	subi temp,-'0'
	rcall uartputc
	pop temp
	ret

;Prints the zero-terminated string following the call statement. 

printstr:
	push	zh
	push	zl
	push	yh
	push	yl
	push	temp
	in	yh,sph
	in	yl,spl
	ldd	zl,y+7
	ldd	zh,y+6

	lsl zl
	rol zh
printstr_loop:
	lpm temp,z+
	cpi temp,0
	breq printstr_end
	rcall uartputc
	cpi temp,13
	brne printstr_loop
	ldi temp,10
	rcall uartputc
	rjmp printstr_loop

printstr_end:
	adiw zl,1		;rounding
	lsr zh
	ror zl

	std	y+7,zl
	std	y+6,zh
	pop	temp
	pop	yl
	pop	yh
	pop	zl
	pop	zh
	ret

; ---------------- Virtual peripherial interface ----------------

;The hw is modelled to make writing a CPM BIOS easier.
;Ports:
;0 	- Con status. Returns 0xFF if the UART has a byte, 0 otherwise.
;1 	- Console input, aka UDR.
;2 	- Console output
;3	- "UART" status: bit 0=rx, bit 1 = tx
;4	- "UART" data register, no wait
;15	- Disk select
;16,17 	- Track select
;18 	- Sector select
;20 	- Write addr l
;21 	- Write addr h
;22 	- Trigger - write  to read, to write a sector using the above info;
;			, write to allocated/dirctory/unallocated

	.equ	READ_FUNC  = 7
	.equ	WRITE_FUNC = 6
	.equ	BOOT_FUNC  = 5
	.equ	HOME_FUNC  = 4



;*****************************************************
;*         CP/M to host disk constants               *
;*****************************************************
	.equ	MAXDISKS  = 4		;Max number of Disks (partitions)
	.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


	.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

hostparttbl:	.byte	8*MAXDISKS ;host partition table (start sector, sector count)
hostparttbltop:
hostdsk:	.byte	1	;host disk number
hostlba:	.byte	3	;host sector number (relative to partition start)

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)


	.cseg
	
conStatus:
	lds	temp,rxcount
	cpse	temp,_0
	 ldi	temp,0xff
	ret

conInp:
	rjmp uartgetc

dbgOut:
	printnewline
	printstring "Debug: "
	rcall printhex
	ret

conOut:
	rjmp uartputc
	
uartstat:
	clr	temp
	lds	temp2,rxcount
	cpse	temp2,_0
	 sbr	temp,0x01		
	lds	temp2,txcount
	cpi	temp2,TXBUFSIZE
	breq	uartst_1
	 sbr	temp,0x02
uartst_1:
	ret

uartout:
	lds	temp2,txcount
	cpi	temp2,TXBUFSIZE
	breq	uartout_1
	rjmp uartputc
uartout_1:
	ret

uartin:
	clr	temp
	lds	temp2,rxcount
	cpse	temp2,_0
	 rjmp	uartgetc
	ret

;Called with port in temp2. Should return value in temp.
portRead:
	cpi temp2,0
	breq conStatus
	cpi temp2,1
	breq conInp
	cpi	temp2,3
	breq	uartstat
	cpi	temp2,4
	breq	uartin

	cpi	temp2,15
	breq	dskDiskCheck
	cpi	temp2,22
	breq	dskErrorRet

	cpi	temp2,TIMER_MSECS
	brlo	pr_noclock
	cpi	temp2,TIMER_MSECS+6
	brsh	pr_noclock
	rjmp	clockget

pr_noclock:
	ldi	temp,0xFF
	ret

;Called with port in temp2 and value in temp.
portWrite:
	cpi temp2,0
	breq dbgOut
	cpi temp2,2
	breq conOut
	cpi temp2,4
	breq uartout

	cpi temp2,15
	breq dskDiskSel
	cpi temp2,16
	breq dskTrackSel_l
	cpi temp2,17
	breq dskTrackSel_h
	cpi temp2,18
	breq dskSecSel
	cpi temp2,20
	breq dskDmaL
	cpi temp2,21
	breq dskDmaH

	cpi temp2,22
	breq dskDoIt
	
	cpi	temp2,TIMERPORT
	brlo	pw_noclock
	cpi	temp2,TIMER_MSECS+6
	brsh	pw_noclock
	rjmp	clockput

pw_noclock:
	ret


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	dsk_partinit
	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

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


dsk_read:

	sbi	flags,readop		;read operation
	;RAM disk?
	lds	temp2,seekdsk
#if RAMDISKCNT
	cpi	temp2,RAMDISKNR
	brlt	PC+2
	 rjmp	rdskDoIt
#endif
	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_write:
	;write the selected CP/M sector

	cbi	flags,readop		;not a read operation

	;RAM disk?
	lds	temp2,seekdsk
#if RAMDISKCNT
	cpi	temp2,RAMDISKNR
	brlt	PC+2
	 rjmp	rdskDoIt
#endif
	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

;*****************************************************
;*	Common code for READ and WRITE follows       *
;*****************************************************

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

;*****************************************************

; hostdsk = host disk #,  (partition #)
; hostlba = host block #, relative to partition start 
; Read/Write "hostsize" bytes to/from hostbuf
	

dsk_hostparam:
	ldiw	z,hostparttbl
	lds	temp,hostdsk
.if HOSTRW_DEBUG
	push	temp
	subi	temp,-('A')
	rcall	uartputc
	printstring ": "
	pop	temp
.endif

	lsl	temp		
	lsl	temp		
	lsl	temp		
	add	zl,temp		
	adc	zh,_0		

	lds	temp,hostlba
	lds	temp2,hostlba+1
	lds	temp3,hostlba+2

.if HOSTRW_DEBUG
	printstring "lba: "
	clr	temp4
	rcall	print_ultoa
.endif

	ldd	xl,z+4
	ldd	xh,z+5
	ldd	yl,z+6
	
	cp	temp,xl
	cpc	temp2,xh
	cpc	temp3,yl
	brcs	dsk_hp1
	
.if HOSTRW_DEBUG
	printstring ", max: "
	push	temp4
	push	temp3
	push	temp2
	push	temp
	movw	temp,x
	mov	temp3,yl
	clr	temp4
	rcall	print_ultoa
	pop	temp
	pop	temp2
	pop	temp3
	pop	temp4
	printstring " "
.endif
	
	clr	temp
	ret

dsk_hp1:
	ldd	xl,z+0
	ldd	xh,z+1
	ldd	yl,z+2
	ldd	yh,z+3

	add	xl,temp
	adc	xh,temp2
	adc	yl,temp3
	adc	yh,_0
.if HOSTRW_DEBUG
	printstring ", abs:"
	push	temp4
	push	temp3
	push	temp2
	push	temp
	movw	temp,x
	movw	temp3,y
	rcall	print_ultoa
	pop	temp
	pop	temp2
	pop	temp3
	pop	temp4
	printstring " "
.endif
	ori	temp,255
dsk_hpex:
	ret

;*****************************************************
;*	WRITEhost performs the physical write to     *
;*	the host disk, READhost reads the physical   *
;*	disk.					     *
;*****************************************************

dsk_writehost:
.if HOSTRW_DEBUG
	printnewline
	printstring "host write "
.endif
	rcall	dsk_hostparam
	breq	dsk_rdwr_err
	
	rcall	mmcWriteSect
	tst	temp
	breq	dsk_rdwr_ok
	
	rcall	dsk_partinit
	cbr	temp,0x80
	breq	dsk_rdwr_err

	rcall	dsk_hostparam
	breq	dsk_rdwr_err
	rcall	mmcWriteSect
	tst	temp
	brne	dsk_rdwr_err
	rjmp	dsk_rdwr_ok


dsk_readhost:
.if HOSTRW_DEBUG
	printnewline
	printstring "host read  "
.endif
	rcall	dsk_hostparam
	breq	dsk_rdwr_err
	
	rcall	mmcReadSect
	tst	temp
	breq	dsk_rdwr_ok

	rcall	dsk_partinit
	cbr	temp,0x80
	breq	dsk_rdwr_err

	rcall	dsk_hostparam
	breq	dsk_rdwr_err
	rcall	mmcReadSect
	tst	temp
	brne	dsk_rdwr_err

dsk_rdwr_ok:
	sts	erflag,_0
	ret

dsk_rdwr_err:
	sts	erflag,_255
	ret

;***************************************************************************

#if RAMDISKCNT

; ----------------- RAM disk -----------------

	.dseg
rdskbuf:
	.byte	128
	
	.cseg
;----------------------------------------------

rdsk_adr:
	ldi	xl,0
	lds	xh,seeksec
	lds	temp2,seektrk
	
	lsr	xh
	ror	xl			;Col 0..7
	
	mov	temp,temp2
	andi	temp,0x0f
	swap	temp
	or	xh,temp			;Row  0..7
	
	ldiw	z,rdskbuf
	ldi	temp3,128
	DRAM_SETADDR xh, ~0,(1<<ram_ras), ~0,(1<<ram_a8)|(1<<ram_oe)
	cbi	P_RAS,ram_ras

.if DISK_DEBUG > 1
	mov	temp,xh
	rcall	printhex
	printstring " "
	mov	temp,xl
	rcall	printhex
	printstring " "
.endif
	ret

;----------------------------------------------

rdskDoIt:
	sts	erflag,_0
	sbis	flags,readop
	 rjmp	rdsk_wr
	
.if DISK_DEBUG > 1
	printnewline
	printstring "rd-adr: "
.endif
	rcall	rdsk_adr
rdsk_rdl:
	DRAM_SETADDR xl, ~(1<<ram_ras),0, ~((1<<ram_oe)), (1<<ram_a8)
	cbi	P_CAS,ram_cas
	cbi	P_A8,ram_a8
	inc	xl
	dram_wait DRAM_WAITSTATES	;
	in	temp,P_DQ-2		; PIN
	sbi	P_CAS,ram_cas

	cbi	P_CAS,ram_cas
	andi	temp,0x0f
	swap	temp
	dram_wait DRAM_WAITSTATES	;
	in	temp2,P_DQ-2		; PIN
	andi	temp2,0x0f
	or	temp,temp2

	sbi	P_OE,ram_oe
	sbi	P_CAS,ram_cas
	dec	temp3
	st	z+,temp
	brne	rdsk_rdl

	sbi	P_RAS,ram_ras
	ldiw	z,rdskbuf
	lds	xl,dmaadr
	lds	xh,dmaadr+1
	ldi	temp3,128	
rdsk_rdstl:
	ld	temp,z+
	mem_write
	adiw	x,1
	dec	temp3
	brne	rdsk_rdstl
	ret
	

rdsk_wr:
.if DISK_DEBUG > 1
	printnewline
	printstring "wr-adr: "
.endif	
	lds	xl,dmaadr
	lds	xh,dmaadr+1
	ldiw	z,rdskbuf
	ldi	temp3,128	
rdsk_wrldl:
	mem_read
	st	z+,temp
	adiw	x,1
	dec	temp3
	brne	rdsk_wrldl	

	ldi	temp2,RAM_DQ_MASK | (1<<ram_w) | (1<<ram_cas)
	out	DDRC,temp2
	rcall	rdsk_adr
rdsk_wrl:
	ld	temp,z+
	mov	temp2,temp
	andi	temp,RAM_DQ_MASK & ~(1<<ram_w)
	ori	temp,(1<<ram_cas)
	out	PORTC,temp
	DRAM_SETADDR xl, ~(1<<ram_ras),0, ~((1<<ram_a8)),(1<<ram_oe)
	cbi	PORTC,ram_cas
	sbi	PORTD,ram_a8
	sbi	PORTC,ram_cas
	swap	temp2
	andi	temp2,RAM_DQ_MASK & ~(1<<ram_w)
	ori	temp2,(1<<ram_cas)
	out	PORTC,temp2
	cbi	PORTC,ram_cas
	inc	xl
	sbi	PORTC,ram_cas
	dec	temp3
	brne	rdsk_wrl

	sbi	P_RAS,ram_ras
	ldi	temp,~RAM_DQ_MASK | (1<<ram_w) | (1<<ram_cas)
	out	DDRC,temp
	out	PORTC,temp
	ret


#endif /* RAMDISKCNT */

;---------------------------------------------------------------------

; Partition table offsets:
#define PART_TYPE   4
#define PART_START  8
#define PART_SIZE  12

	.dseg
tmp_tbl:
	.byte	8*MAXDISKS

	.cseg
dsk_partinit:
	sts	ndisks,_0
	rcall	mmcInit
	andi	temp,MMCST_NOINIT & MMCST_NODISK
	brne	dsk_pierr
	
;Load first sector from MMC (boot sector)
	ldiw	y,0			; Sector 0
	movw	x,y
	rcall	mmcReadSect
	tst	temp
	breq	dsk_pi1

dsk_pierr:
	clr	temp
	ret

dsk_pi1:
	ldiw	y,tmp_tbl
	ldi	temp2,8*MAXDISKS
dsk_picl:
	st	y+,_0
	dec	temp2
	brne	dsk_picl
	sbiw	y,8*MAXDISKS

;Test, if it has a valid MBR

	ldiw	z,hostbuf+510-1	;Point to last byte of partition table

	ldi	temp3,0			;temp3 holds number of found disks (paritions)
	ldd	temp,z+1		;MBR signature (0xAA55)  at and of sector?
	ldd	temp2,z+2
	ldi	temp4,0xAA
	cpi	temp,0x55		
	cpc	temp2,temp4
	breq	dsk_part

;No MBR, no partition table ...
	inc	temp3			;pretend we have one.
	ldi	temp,high((1<<16) * 128/512)
	std	y+0,_0			;start at beginning of card
	std	y+1,_0
	std	y+2,_0
	std	y+3,_0
	std	y+4,_0			;max CP/M 2.2 disk size
	std	y+5,temp		;
	std	y+6,_0
	std	y+7,_0
	rjmp	dsk_pend
		
;Search Partition Table for CP/M partitions
dsk_part:
	sbiw	z,63			;Now at first byte of partition table
	ldi	temp4,high(hostbuf+510)
dsk_ploop:
	ldd	temp,z+PART_TYPE
	cpi	temp,PARTID
	brne	dsk_nextp
	
; Found a CP/M partition
	
	ldd	temp,z+PART_START
	st	y+,temp
	ldd	temp,z+PART_START+1
	st	y+,temp
	ldd	temp,z+PART_START+2
	st	y+,temp
	ldd	temp,z+PART_START+3
	st	y+,temp
	
	ldd	temp,z+PART_SIZE
	st	y+,temp
	ldd	temp,z+PART_SIZE+1
	st	y+,temp
	ldd	temp,z+PART_SIZE+2
	st	y+,temp
	ldd	temp,z+PART_SIZE+3
	st	y+,temp

	inc	temp3
	cpi	temp3,MAXDISKS
	breq	dsk_pend	
dsk_nextp:
	adiw	zl,16
	cpi	zl,low(hostbuf+510)
	cpc	zh,temp4
	brlo	dsk_ploop

dsk_pend:

;Store new partitions and check if the SD card has been changed.

	ldiw	y,tmp_tbl
	ldiw	z,hostparttbl
	ldi	temp4,8*MAXDISKS
	clt
dsk_pcpl:
	ld	temp,y+
	ld	temp2,z
	st	z+,temp
	cpse	temp,temp2
	 set
	dec	temp4
	brne	dsk_pcpl

	mov	temp,temp3
	sts	ndisks,temp
	brtc	dsk_pcpe

	tst	temp
	breq	dsk_pcpe

; SD card changed.
	sbr	temp,0x80

dsk_pcpe:
	ret

;---------------------------------------------------------------------
; Print partition table info

prnt_parttbl:
	ldiw	z,hostparttbl
pprl:
	ldd	xl,z+4			;Get partition size
	ldd	xh,z+5
	ldd	yl,z+6
	ldd	yh,z+7
	cp	xl,_0			;If zero ...
	cpc	xh,_0
	cpc	yl,_0
	cpc	yh,_0
	breq	pppre		;... No more partitions.

	ldd	temp,z+0		;Get partition start
	ldd	temp2,z+1
	ldd	temp3,z+2
	ldd	temp4,z+3
	printnewline
	cp	temp,_0			;If zero ...
	cpc	temp2,_0
	cpc	temp3,_0
	cpc	temp4,_0
	breq	prnop		;... no partition table at 0

	rcall	prstr_table
	rjmp	pprsz
prnop:
	rcall	prstr_image
pprsz:
	rcall	print_ultoa
	printstring ", size: "
	movw	temp,x
	movw	temp3,y

	lsr	temp4
	ror	temp3
	ror	temp2
	ror	temp
	rcall	print_ultoa
	printstring "KB."
	
	adiw	z,8
	ldi	temp,high(hostparttbltop)
	cpi	zl,  low (hostparttbltop)
	cpc	zh,temp
	brlo	pprl
pppre:
	ret
	
prstr_table:
	printstring "CP/M partition at: "
	ret
prstr_image:
	printstring "Assuming CP/M image at: "
	ret
	
; ****************************************************************************

; ------------- system timer 1ms ---------------

	.dseg

delay_timer1:
	.byte	1
delay_timer2:
	.byte	1
timer_base:
timer_ms:
	.byte	2
timer_s:
	.byte	4
; don't change order here, clock put/get depends on it.
cntms_out:		; register for ms
	.byte	2
utime_io:		; register for uptime. 
	.byte	4	
cnt_1ms:
	.byte	2
uptime:
	.byte	4
timer_top:
.equ timer_size	= timer_top - timer_base
	
.equ clkofs	= cnt_1ms-cntms_out
.equ timerofs	= cnt_1ms-timer_ms
 

	.cseg	
sysclockint:
	.org OC1Baddr  		; Timer/Counter1 Compare Match B
    	rjmp sysclockint	; 1ms system timer

	.org sysclockint
	push    zl
	in      zl,SREG
	push    zl
	push	zh
	inm8	zl,OCR1BL
	inm8	zh,OCR1BH
	addiw	z,F_CPU/1000
	outm8	OCR1BH,zh
	outm8	OCR1BL,zl
	
#if DRAM_8BIT	/* Implies software uart */
	lds	zl,srx_char_to
	subi	zl,1
	brcs	syscl0
	sts	srx_char_to,zl
	brne	syscl0
	rcall	srx_to
syscl0:
#endif
	lds	zl,delay_timer1
	subi	zl,1
	brcs	syscl_t1n
	sts	delay_timer1,zl
syscl_t1n:	
	lds	zl,delay_timer2
	subi	zl,1
	brcs	syscl_t2n
	sts	delay_timer2,zl
syscl_t2n:	
	lds     zl,cnt_1ms
	lds     zh,cnt_1ms+1
	adiw	z,1
	
	sts	cnt_1ms,zl
	sts	cnt_1ms+1,zh
	cpi	zl,low(1000)
	ldi	zl,high(1000)		;doesn't change flags
	cpc	zh,zl
	brlo	syscl_end
	
	sts	cnt_1ms,_0
	sts	cnt_1ms+1,_0

	lds	zl,uptime+0
	inc	zl
	sts	uptime+0,zl
	brne	syscl_end
	lds	zl,uptime+1
	inc	zl
	sts	uptime+1,zl
	brne	syscl_end
	lds	zl,uptime+2
	inc	zl
	sts	uptime+2,zl
	brne	syscl_end
	lds	zl,uptime+3
	inc	zl
	sts	uptime+3,zl
	
syscl_end:
	pop	zh
	pop     zl
	out     SREG,zl
	pop     zl
	reti

; wait for temp ms

delay_ms:
	sts	delay_timer1,temp
dly_loop:
	lds	temp,delay_timer1
	cpi	temp,0
	brne	dly_loop
	ret

; 

clockget:
	ldi	temp,0xFF
	subi	temp2,TIMER_MSECS
	brcs	clkget_end		;Port number in range?
	ldiw	z,cntms_out
	breq	clkget_copy		;lowest byte requestet, latch clock
	cpi	temp2,6
	brsh	clkget_end		;Port number to high?
	
	add	zl,temp2
	brcc	PC+2
	 inc	zh
	ld	temp,z
clkget_end:
	ret
	
	
		
clkget_copy:
	ldi	temp2,6
	cli
clkget_l:
	ldd	temp,z+clkofs
	st	z+,temp
	dec	temp2
	brne	clkget_l
	sei
	lds	temp,cntms_out
					;req. byte in temp
	ret

clockput:
	subi	temp2,TIMERPORT
	brcs	clkput_end		;Port number in range?
	brne	clkput_1
	
	; clock control

	cpi	temp,starttimercmd
	breq	timer_start
	cpi	temp,quitTimerCmd
	breq	timer_quit
	cpi	temp,printTimerCmd
	breq	timer_print
	cpi	temp,uptimeCmd
	brne	cp_ex
	rjmp	uptime_print
cp_ex:
	ret	
	
timer_quit:
	rcall	timer_print
	rjmp	timer_start

clkput_1:
	dec	temp2
	ldiw	z,cntms_out
	breq	clkput_copy		;lowest byte requestet, latch clock
	cpi	temp2,6
	brsh	clkput_end		;Port number to high?
	
	add	zl,temp2
	 brcc	PC+2
	inc	zh
	st	z,temp
clkput_end:
	ret
		
clkput_copy:
	st	z,temp
	adiw	z,5
	ldi	temp2,6
	cli
clkput_l:
	ldd	temp,z+clkofs
	st	z+,temp
	dec	temp2
	brne	clkput_l
	sei
	ret

; start/reset timer
;
timer_start:
	ldiw	z,timer_ms
	ldi	temp2,6
	cli
ts_loop:
	ldd	temp,z+timerofs
	st	z+,temp
	dec	temp2
	brne	ts_loop
	sei
	ret


; print timer
;
	
timer_print:
	push	yh
	push	yl
	ldiw	z,timer_ms

; put ms on stack (16 bit)

	cli
	ldd	yl,z+timerofs
	ld	temp2,z+
	sub	yl,temp2
	ldd	yh,z+timerofs
	ld	temp2,z+
	sbc	yh,temp2
	brsh	tp_s
	
	addiw	y,1000
	sec	
tp_s:
	push	yh
	push	yl

	ldd	temp,z+timerofs
	ld	yl,z+
	sbc	temp,yl

	ldd	temp2,z+timerofs
	ld	yh,z+
	sbc	temp2,yh

	ldd	temp3,z+timerofs
	ld	yl,z+
	sbc	temp3,yl

	sei
	ldd	temp4,z+timerofs
	ld	yh,z+
	sbc	temp4,yh
	
	printnewline
	printstring "Timer running. Elapsed: "
	rcall	print_ultoa

	printstring "."
	pop	temp
	pop	temp2
	ldi	temp3,0
	ldi	temp4,0
	rcall	print_ultoa
	printstring "s."

	pop	yl
	pop	yh
	ret
	
uptime_print:

	ldiw	z,cnt_1ms
	
	cli
	ld	temp,z+
	push	temp
	ld	temp,z+
	push	temp
	
	ld	temp,z+
	ld	temp2,z+
	ld	temp3,z+
	sei
	ld	temp4,z+
	
	printnewline
	printstring "Uptime: "
	
	rcall	print_ultoa
	printstring ","

	ldi	temp3,0
	ldi	temp4,0
	pop	temp2
	pop	temp
	rcall print_ultoa
	printstring "s."

	ret


	
; --------------- Debugging stuff ---------------
; Print a line with the Z80 main registers

;.if INS_DEBUG

zflags_to_ch:
	.db	"SZ H PNC",0,0
	
printregs:
	printnewline

	push	zl
	push	zh
	ldiw	z,zflags_to_ch*2
	mov	temp2,z_flags
pr_zfl_next:
	lpm	temp,z+
	tst	temp
	breq	pr_zfl_end
	cpi	temp,' '			; Test if no flag
	breq	pr_zfl_noflag
	sbrs	temp2,7			; 
	 ldi	temp,' '			; Flag not set
	rcall	uartputc
pr_zfl_noflag:
	rol	temp2
	rjmp	pr_zfl_next
pr_zfl_end:
	pop	zh
	pop	zl	

	printstring "  A ="
	mov	temp,z_a
	rcall	printhex	
	printstring " BC ="
	lds	temp2,z_b
	lds	temp, z_c
	rcall	printhexw
	printstring " DE ="
	lds	temp2,z_d
	lds	temp, z_e
	rcall	printhexw
	printstring " HL ="
	lds	temp2,z_h
	lds	temp, z_l
	rcall	printhexw
	printstring " SP ="
	movw	temp, z_spl
	rcall	printhexw
	printstring " PC ="
	movw	temp, z_pcl
	rcall	printhexw
	printstring "       "
	movw 	xl,z_pcl
	mem_read
	rcall	printhex
	printstring " "
	adiw	x,1
	mem_read
	rcall	printhex
	printstring " "
	adiw	x,1
	mem_read
	rcall	printhex
	printstring " "
	ret
;.endif