diskchooser.a65

; C65GS Disk Chooser
; Paul Gardner-Stephen, 2014.
;
; Purpose: 

	 .scope

	.data 
	.org $00fb
	.space zptempv 2
	.space zptempv2 2

	.org $ce00
	.space romslab 1
	.space screenrow 1

	.space checksum 4
	.space rom_sectorsread 2

	; structure for partition table
	.space part_startsector 4

	; structure for FAT32 file system
	.space fs_startfat 4
	.space fs_syssectors 4
	.space fs_reservedclusters 4
	.space fs_rootdircluster 4
	.space fs_datasectors 4
	.space fs_clustercount 4
	.space fs_sectorspercluster 1

	; structure for current directory entry
	.space fs_currentdircluster 4
	.space fs_direntry 32
	.space fs_direntrynumber 1
	; structure for current open file
	.space fs_clusternumber 4
	.space fs_sector_in_cluster 1

	; variables for testing of D81 boot image
	.space d81_clusternumber 4
	.space d81_clustersneeded 2
	.space d81_clustercount 2

	.space timeoutcounter 3

	.space sptemp 1

	.checkpc $CEFF

	.text

	.org $C000

		tsx
		stx sptemp

	 	 ; clear screen
	 	 lda #$93
	 	 jsr $ffd2

		sec
		jsr enhanced_io

		sei

		; Try to read the MBR from the SD card
		jsr print
		.byte "OPENING SDCARD...",13,0

tryreadmbr:

		jsr readmbr
		bcs gotmbr

		jmp tryreadmbr
		
gotmbr:
		jsr print
		.byte "LOADING PARTITION TABLE...",13,0

		jsr sdmapsectorbuffer

		; check for $55AA MBR signature
		lda $DFFE
		cmp #$55
		bne badfs
		lda $DFFF
		cmp #$AA
		bne badfs

		; read partition 1 details from $DFBE
		; $DFBE - $80 = bootable, $00 = inactive.  We ignore this
		; $DFC2 - Partition type
		; $DFC6 - Starting SECTOR number
		; $DFCA - Number of sectors in partition

		; overwrite $ chars in template with hex digits
		ldy #$00
		ldz $DFC9
		stz part_startsector+3
		ldz $DFC8
		stz part_startsector+2
		ldz $DFC7
		stz part_startsector+1
		ldz $DFC6
		stz part_startsector+0

		; load start sector of partition into SD card sector register
		ldx #$03
l9:		lda part_startsector,x
		sta $D681,x
		dex
		bpl l9
		jsr sd_fix_sectornumber
		jsr sd_readsector
		bcc sdcarderror		

		jsr print
		.byte "MOUNTING PARTITION...",13,0

	
		lda $DFFE
		cmp #$55
		bne badfs
		lda $DFFF
		cmp #$AA
		bne badfs

		; The information of relevance we have now is:
		; $DE0D - sectors per cluster
		; $DE0E-$DE0F - reserved sectors
		; $DE10 - number of copies of FAT
		; $DE11 - Root dir entry count (0 on FAT32)
		; $DE20-$DE23 - number of sectors in the file system
		; $DE24-$DE27 - number of sectors in one copy of the FAT
		; $DE2C-$DE2F - number of reserved clusters
		
		; We need to work out if this is a FAT32 partition by working out
		; the number of clusters.  To work that out, we need to work out
		; the number of data sectors.  To work that out, we need to work out
		; the number of reserved sectors.
		
		; first, filter out obviously FAT16/FAT12 file systems
		lda $DE11
		bne badfs

		; With root directory entries = 0, the reserved sector count
		; is the number of reserved sectors, plus (copies of fat) *
		; (sectors in one copy of the fat).
		; the first FAT begins immediately after the reserved sectors
		ldx #$03
l11:		lda $DE0E,x
		sta fs_syssectors,x
		sta fs_startfat,x
		lda $DE2C,x
		sta fs_reservedclusters,x
		dex
		bpl l11
		lda #$00
		sta fs_syssectors+2
		sta fs_syssectors+3
		sta fs_startfat+2
		sta fs_startfat+3
		ldy $DE10
		beq addedfatsectors
addnextfatsectors:
		ldx #$00
		clc
		php
l12:		plp
		lda fs_syssectors,x
		adc $DE24,x
		sta fs_syssectors,x
		php
		inx
		cpx #$04
		bne l12
		plp
		dey
		bne addnextfatsectors
addedfatsectors:

		; also calculate number of data sectors
		sec
		ldx #$03
l13:		lda $DE20,x
		sbc fs_syssectors,x
		sta fs_datasectors,x
		sta fs_clustercount,x
		dex
		bpl l13		

		; from number of data sectors and sectors per cluster,
		; we can work out the number of clusters.
		lda $DE0D
		sta fs_sectorspercluster
		tay
		and #$fe
		beq gotclustercount
l14:
		ldx #$03
		clc
l15:		lda fs_clustercount,x
		ror
		sta fs_clustercount,x
		dex
		bpl l15
		tya
		lsr
		tay
		and #$fe
		bne l14
gotclustercount:

		; filter out non-FAT32 filesystems
		; NOTE: FAT32 can have as few as 65525 clusters, but we do not support
		; such file systems, which should be rare, anyway.
		lda fs_clustercount+3
		ora fs_clustercount+2
		beq badfs
	
		; Store cluster number of root directory
		ldx #$03
l16:		lda $DE2C,x
		sta fs_rootdircluster,x
		sta fs_clusternumber,x
		dex
		bpl l16

		jsr print
		.byte "SEARCHING DIRECTORY...",13,0

		; Look for C65GS.D81 to mount for F011 emulation
		
		; for now indicate that there is no disk in drive
		lda #$00
		sta $d68b

		jsr fs_cd_rootdir
		jsr fs_opendir
		bcc sdcarderror

		; iterate through directory entries looking for
		; ordinary files with .D81 extension 
nextdirectoryentry2:
		; XXX this is needed here otherwise printing the file
		; name goes strange.  This does not seem to make sense!	
		nop  

		jsr fs_readdir
		bcc nod81

		; Look for .D81 extension
		ldx #$00
		lda fs_direntry+8
		jsr toupper
		cmp #'D
		bne nextdirectoryentry2
		lda fs_direntry+9
		jsr toupper
		cmp #'8
		bne nextdirectoryentry2
		lda fs_direntry+10
		jsr toupper
		cmp #'1
		bne nextdirectoryentry2

		jsr print
		.byte "MOUNT ",0

		jsr sdunmapsectorbuffer

		ldx #$00
l242:		lda fs_direntry,x
		phx
		jsr $ffd2
		plx
		inx
		cpx #$08
		bne l242
		jsr print
		.byte "? ",0
		
		jsr sdunmapsectorbuffer
		cli

l243:		jsr $ffe4
		cmp #$00
		beq l243
		
		pha

		lda #$0d
		jsr $ffd2

		sei
		jsr sdmapsectorbuffer
		pla

		cmp #'Y
		bne nextdirectoryentry2


		; now we need to check that the file is long enough,
		; and also that the cluster are contiguous.

		; get cluster number from directory entry.
		jsr fs_setfile

		; get the starting sector number in case it is good
		jsr fs_clustertosector
		jsr sd_fix_sectornumber
		; copy to FDC starting sector register (but don't mark
		; disk as available yet)
		ldx #$03
l91:		lda $d681,x
		sta $d68c,x
		dex
		bpl l91
	
		; now check that file is long enough and has
		; contiguous clusters
		jsr fs_setfile
		ldx #$03
l92:		lda fs_clusternumber,x
		sta d81_clusternumber,x
		dex
		bpl l92

		; work out how many clusters we need
		; We need 1600 sectors, so halve for every zero tail
		; bit in sectors per cluster.  we can do this because
		; clusters in FAT must be 2^n sectors.
		lda #$00
		sta d81_clustercount 
		sta d81_clustercount+1
		lda #<1600
		sta d81_clustersneeded
		lda #>1600
		sta d81_clustersneeded+1
		ldz fs_sectorspercluster
l94:		tza
		and #$01
		bne d81nextcluster
		tza
		lsr
		taz
		lsr d81_clustersneeded+1
		ror d81_clustersneeded
		jmp l94				

d81nextcluster:
		; Now read through clusters and make sure that all is
		; well.
		
		; we start with the first cluster already read.

		; check that it matches expected cluster number
		ldx #$03
l95:		lda d81_clusternumber,x
		cmp fs_clusternumber,x
		bne d81isfragged
		dex
		bpl l95
		
		; increment number of expected clusters found.
		inc d81_clustercount
		bne l96
		inc d81_clustercount+1
l96:

		; increment expected cluster number
		clc
		lda d81_clusternumber
		adc #$01
		sta d81_clusternumber
		lda d81_clusternumber+1
		adc #$00
		sta d81_clusternumber+1
		lda d81_clusternumber+2
		adc #$00
		sta d81_clusternumber+2
		lda d81_clusternumber+3
		adc #$00
		sta d81_clusternumber+3

		jsr fs_getnextclusteroffile
		bcs d81nextcluster

		; read to end of D81 file, and it is contiguous
		; now check that it is the right length

		lda d81_clustersneeded
		cmp d81_clustercount
		bne d81wronglength
		
		lda d81_clustersneeded+1
		cmp d81_clustercount+1
		bne d81wronglength

		; D81 is good, so set flag 
		lda #$07
		sta $d68b

		; all done, move on to loading the ROM
		clc
		jsr enhanced_io
		jsr print
		.byte "DISK MOUNTED",13,0
		jmp alldone

d81wronglength:
		jsr print
		.byte ".D81 FILE HAS WRONG LENGTH",13,0
		jmp alldone

d81isfragged:
		jsr print
		.byte "THAT DISK IMAGE IS FRAGMENTED.",13
		.byte "DE-FRAG DISK IMAGE BEFORE MOUNTING",13,0
		
		jmp alldone
nod81:
		; we couldn't find the D81 file, so tell the user
		jsr print
		.byte "NO MORE DISK IMAGES. DRIVE MARKED EMPTY.",13,0
		jmp alldone

sdcarderror:	
		jsr print
		.byte "SD-CARD ERROR",13,0
		jmp alldone

badfs:		jsr print
		.byte "INVALID OR UNSUPPORTED FILE SYSTEM.",13
		.byte "(SHOULD BE FAT32)",13,0
		jmp alldone

;;; ----------------------------------------------------------------------------
;;; FAT file system routines
;;; ----------------------------------------------------------------------------
toupper:	; convert ASCII character to upper case
		cmp #$60
		bcc tu1
		cmp #$7a
		bcs tu1
		and #$5f
tu1:		rts

fs_cd_rootdir:
		ldx #$00
cd1:		lda fs_rootdircluster,x
		sta fs_currentdircluster,x
		inx
		cpx #$04
		bne cd1
		sec
		rts

fs_opendir:
		; assume cluster for directory is already set
		ldx #$00
od1:		lda fs_currentdircluster,x
		sta fs_clusternumber,x
		inx
		cpx #$04
		bne od1
		lda #$00
		sta fs_direntrynumber
		jmp fs_openfile

fs_readdir:
		; read next entry from directory
		lda fs_direntrynumber
		cmp #$10

		bcc rd2

		; reached end of sector, so load next sector
		lda #$00
		sta fs_direntrynumber
		jsr fs_readnextsector
		; return with carry clear if we have readed the end of the directory
		bcs rd2
		rts

rd2:		; have sector, so can copy entry
		ldy #$00
		lda fs_direntrynumber
		and #$08
		bne rd3
		lda fs_direntrynumber
		asl
		asl
		asl
		asl
		asl
		tax
rd1:		lda $DE00,x
		sta fs_direntry,y
		inx
		iny
		cpy #$20
		bne rd1
rddone:		inc fs_direntrynumber
		sec		
		rts
rd3:		lda fs_direntrynumber
		asl
		asl
		asl
		asl
		asl
		tax
rd4:		lda $DF00,x
		sta fs_direntry,y
		inx
		iny
		cpy #$20
		bne rd4
		inc fs_direntrynumber
		sec
		rts		

fs_setfile:
		; get cluster from directory entry and store in cluster number
		lda fs_direntry+20
		sta fs_clusternumber+2
		lda fs_direntry+21
		sta fs_clusternumber+3
		lda fs_direntry+26
		sta fs_clusternumber+0
		lda fs_direntry+27
		sta fs_clusternumber+1
		sec
		rts		

fs_openfile:
		; zero sector number in cluster
		lda #$00
		sta fs_sector_in_cluster

		; Read first sector of first cluster
		; (no need to add fs_sector_in_cluster since it is zero here)
		jsr fs_clustertosector
		bcs of1
		rts
of1:		
		jsr sd_fix_sectornumber
		jmp sd_readsector

fs_readnextsector:
		; load next sector of file
		jsr sd_inc_fixedsectornumber
		inc fs_sector_in_cluster
		lda fs_sector_in_cluster
		cmp fs_sectorspercluster
		bne rs6
		; need to advance cluster number
		lda #$00
		sta fs_sector_in_cluster
		jsr fs_getnextclusteroffile
		bcs rs5
		; end of file or other error
		rts
rs5:		; cluster number is right
		jsr fs_clustertosector
		jsr sd_fix_sectornumber

rs6:		
		jmp sd_readsector

fs_getnextclusteroffile:
		; read chained cluster number for fs_clusternumber

		; FAT32 uses 32-bit cluster numbers.
		; 512 / 4 = 128 cluster numbers per sector.
		; To get the sector of the FAT containin a particular
		; cluster entry, we thus need to shift the cluster number
		; right 7 bits.  Then we add the start sector number of the FAT.

		; get cluster number into sector number registers
		ldx #$00
g1:		lda fs_clusternumber,x
		sta $D681,x
		inx
		cpx #$04
		bne g1

		; shift right 7 times
		ldy #$07
g31:		clc		
		ror $d684
		ror $d683
		ror $d682
		ror $d681
		dey
		bne g31

		; add start of partition offset
		ldx #$00
		clc
		php
g33:		plp
		lda $d681,x
		adc part_startsector,x
		sta $d681,x
		php
		inx
		cpx #$04
		bne g33
		plp

		; add start of fat offset
		ldx #$00
		clc
		php
g3:		plp
		lda $d681,x
		adc fs_startfat,x
		sta $d681,x
		php
		inx
		cpx #$04
		bne g3
		plp

		; turn sector number into byte address for non-SDHC cards
		jsr sd_fix_sectornumber

		; read FAT sector
		jsr sd_readsector
		bcc gfail

		; now read the right four bytes out.
		; cluster number needs to be shifted left 2 bits.
		; we only need the lowest order byte.
		lda fs_clusternumber
		asl
		asl
		tax
		ldy #$00
		lda fs_clusternumber
		and #$40
		bne ghigh
g6:		lda $de00,x
		sta fs_clusternumber,y
		inx
		iny
		cpy #$04
		bne g6
		bra gcheck
ghigh:		lda $df00,x
		sta fs_clusternumber,y
		inx
		iny
		cpy #$04
		bne ghigh
gcheck:		; check that resulting cluster number is valid.

		; First, only the lower 28-bits are valid
		lda fs_clusternumber+3
		and #$0f
		sta fs_clusternumber+3

		; Now check for special values:
		; cluster 0 is invalid
		lda fs_clusternumber+3
		ora fs_clusternumber+2
		ora fs_clusternumber+1
		ora fs_clusternumber
		cmp #$00
		beq gfail
		; $FFFFFF7 = bad cluster
		; $FFFFFFF = end of file
		lda fs_clusternumber+3
		cmp #$0f
		bne gok
		lda fs_clusternumber+2
		cmp #$ff
		bne gok
		lda fs_clusternumber+1
		cmp #$ff
		bne gok
		lda fs_clusternumber
		cmp #$ff
		beq gfail
		cmp #$f7
		beq gfail

gok:		; cluster number is okay
		sec
		rts

gfail:		
		clc
		rts
		

fs_clustertosector:
	; convert a cluster number in fs_clustertosector into a sector 
	; number loaded into $D681-$D684.

	  	; put cluster number into sector number	
		ldx #$03
l21:		lda fs_clusternumber,x
		sta $D681,x
		dex
		bpl l21

		; subtract number of reserved clusters
		ldx #$03
		sec
		php
l17:		plp
		lda $D681,x
		sbc fs_reservedclusters,x
		sta $D681,x
		php
		dex
		bpl l17
		plp

		; now shift it left according to fs_sectorspercluster
		lda fs_sectorspercluster
		tay
		and #$fe
		beq multipliedclusternumber
l22:		clc
		rol $D681
		rol $D682
		rol $D683
		rol $D684
		tya
		lsr
		tay
		and #$fe
		bne l22
multipliedclusternumber:
		ldx #$00
mmm1:		lda $d681,x
		inx
		cpx #$04
		bne mmm1

		; skip over filesystem reserved and FAT sectors
		ldx #$00
		clc
		php
l23:		plp
		lda $D681,x
		adc fs_syssectors,x
		sta $D681,x
		php
		inx
		cpx #$04
		bne l23
		plp	

		; add start sector of partition
		ldx #$00
		clc
		php
l24:		plp
		lda $D681,x
		adc part_startsector,x
		sta $D681,x
		php
		inx
		cpx #$04
		bne l24
		plp
		
		; return success
		sec
		rts


;;; ----------------------------------------------------------------------------
;;; MBP / partition routines
;;; ----------------------------------------------------------------------------

; read master boot record. Does not sanity check anything.
readmbr:
		; MBR is sector 0	
		lda #$00
		sta $D681
		sta $d682
		sta $d683
		sta $d684

		jmp sd_readsector

;;; ----------------------------------------------------------------------------
;;; SD Card access routines
;;; ----------------------------------------------------------------------------
sdreset:
		; try SD-HC mode first
;		lda #$41
;		sta $D680
;		jsr sd_resetsequence
;		bcc sdr1
;		rts
sdr1:		; fall back to SD mode
		lda #$42
		sta $d680
		; fall through		

sd_resetsequence:
		; write $00 to $D680 to start reset
		lda #$00
		sta $D680
		jsr sdtimeoutreset
re1:		jsr sdreadytest
		bcs re1done	; success, so return
		bne re1		; not timed out, so keep trying
		rts 		; timeout, so return
re1done:	
		; write $01 to $D680 to complete reset
		lda #$01
		sta $D680
		jsr sdtimeoutreset
re2:		jsr sdreadytest
		bcs re2done	; success, so return
		bne re2		; not timed out, so keep trying
		rts 		; timeout, so return
re2done:
		; give card some time to settle after reset, otherwise 
		; reading sectors will fail
		jsr sdwaitawhile

		jsr sdmapsectorbuffer

		; write $02 to $D680 to read MBR as a test of reset
		lda #$02
		sta $D680
		jsr sdtimeoutreset
		
re3:		; allow time for read to happen
		lda $d680
		jsr sdreadytest
		bcs redone	; success, so return
		bne re3		; not timed out, so keep trying
		; timeout -- so call reset again
		rts
redone:		
		sec
		rts

sdwaitawhile:	jsr sdtimeoutreset
sw1:		inc timeoutcounter
		bne sw1
		inc timeoutcounter+1
		bne sw1
		inc timeoutcounter+2
		bne sw1
		rts

		; count to timeout value when trying to read from SD card
sdtimeoutreset:
		lda #$00
		sta timeoutcounter
		sta timeoutcounter+1
		lda #$e0
		sta timeoutcounter+2
		rts

sdreadytest:	; check if SD card is ready, or if timeout has occurred
		; C is set if ready.
		; Z is set if timeout has occurred.
		lda $d680
		and #$03
		beq sdisready
		inc timeoutcounter
		bne sr1
		inc timeoutcounter+1
		bne sr1
		inc timeoutcounter+2
		bne sr1

		; timeout
		lda #$00 ; set Z

sr1:		clc
		rts
sdisready:	sec
		rts

sdmapsectorbuffer:
		lda #$81
		sta $D680
		sec
		rts

sdunmapsectorbuffer:
		lda #$82
		sta $D680
		sec
		rts

sd_readsector:
		; Assumes fixed sector number (or byte address in case of SD cards)
		; is loaded into $D681 - $D684		

		; check if sd card is busy
		lda $d680
		and #$01
		bne rsbusyfail

		; try reading sector fast the first time
		jmp rs4

redoread:	
		inc $d020
		; when retrying, introduce a delay.  This seems to be needed often
		; when reading the first sector after SD card reset.  
		ldx #$f0
		ldy #$00
		ldz #$00
r1:		inz
		bne r1
		iny
		bne r1
		inx
		bne r1

rs4:
		; ask for sector to be read
		lda #$02
		sta $d680

		; wait for sector to be read
		jsr sdtimeoutreset
rs3:		
		jsr sdreadytest
		bcs rsread
		bne rs3
		jmp rereadsector

rsread:
		lda $d680
		and #$01
		bne rs3

		; check that we read 512 bytes
		lda $d688
		lda $d689

		cmp #$02
		bne redoread

		sec
		rts

		; reset sd card and try again
rereadsector:
		jsr sdreset		
		jmp rs4

rsbusyfail:     ; fail
		clc 
		rts

sd_inc_fixedsectornumber:
		; increment fixed sector number.
		; if SDHC, then increment is simple.
		; if SD, then we add $200
		lda $D680
		and #$10
		bne sdhc1
		; sd card mode: add $200
		lda $d682
		clc
		adc #$02
		sta $d682
		lda $d683
		adc #$00
		sta $d683
		lda $d684
		adc #$00
		sta $d684
		rts

sdhc1:		; SDHC card mode: add 1
		inc $d681
		bcc s1
		inc $d682
s2:		bcc s1
		inc $d683
		bcc s1
		inc $d684
s1:		
		rts

sd_fix_sectornumber:
		; assumes sector number has been loaded into 
		; $D681 - $D684
		; We need to check if the card is SDHC or SD.
		; If SD, we need to shift sector number left 9 bits,
		; as SD cards address by byte, not sector.
		; If card is SD, then shift it all left nine bits
		; We detect SD versus SDHC by reading whether SD controller is
		; in SDHC or SD mode.
		lda $D680
		and #$10
		beq sdcardmode
		; SDHC mode, so nothing to do
		rts
sdcardmode:     
		; shift left 8 bits first
		lda $D683
		sta $D684
		lda $D682
		sta $D683
		lda $D681
		sta $D682
		lda #$00
		sta $D681
		; now shift it all left one bit
		lda $D682
		asl
		sta $D682
		lda $D683
		rol
		sta $D683
		lda $D684
		rol
		sta $D684
		rts

;;; ----------------------------------------------------------------------------
;;; Display and basic IO routines
;;; ----------------------------------------------------------------------------

print:	 jsr sdunmapsectorbuffer
	 cli
	 pla
         sta print0+1
         pla
         sta print0+2
         ldx #1
print0:  lda $ffff,x    ; address gets overwritten
         beq print1
         jsr $ffd2
         inx
         bne print0
print1:  sec
         txa
         adc print0+1
         sta print2+1
         lda #0
         adc print0+2
         sta print2+2
	 sei
	 jsr sdmapsectorbuffer
print2:  jmp $ffff      ; address gets overwritten

; If C=1, enable enhanced IO bank, else return to
; C64 standard IO map.
enhanced_io:
		bcs l1
		; Return to VIC-II / C64 IO
		lda #$00
		sta $D02F
		rts
l1:		; Enable VIC-IV / C65GS IO
		lda #$47
		sta $D02F
		lda #$53
		sta $D02F
		RTS

alldone:
		jsr sdunmapsectorbuffer
		clc
		jsr enhanced_io		
		ldx sptemp
		txs
		lda #$00
		cli
		clc
		rts

		.scend

		.checkpc $CE00
		; $CE00 - $CEFF is reserved for hypervisor/filesystem scratch space
		; (see kickstart.a65 for the allocations in this space)

		.checkpc $CF06
		.advance $CF06
		.incbin "etherload.prg"

		.outfile "diskchooser"