.include "atari.inc"

.export sio_handler

.data
	; Pokey update divisor + 1
	speed_byte:	.byte	0

.bss
	; Slot to save old dcb when querying the speed byte
	old_dcb:	.res	8

	; Receive flag
	receive_flag = old_dcb
	
.code

; ****************************************************************************
; High speed SIO entry point
; ****************************************************************************

.proc sio_handler
.rodata
	speed_query_dcb:
		.byte	'?'		; Speed query command code
		.byte	GETDAT		; Receive bytes
		.addr	speed_byte	; Buffer to read the speed byte
		.byte	1		; Timeout
		.byte	0		; Unused
		.word	1		; Transfer 1 byte
.code
	; Check the speed byte
	ldx	speed_byte
	bne	speed_query_done	; We already initialized the speed byte

	; Save old DCB and set up DCB for speed query
swap:
	lda	DCOMND,x
	sta	old_dcb,x
	lda	speed_query_dcb,x
	sta	DCOMND,x
	inx
	cpx	#8
	bne	swap

	; Do the speed query
	jsr	SIOV
	bpl	got_speed

	; An error has occured, assume standard speed
	lda	#$28		; Standard speed
	sta	speed_byte

got_speed:
	; Add 1 to speed byte so 0 doesn't cause problems
	inc	speed_byte

	; Restore old DCB
	ldx	#7
restore:
	lda	old_dcb,x
	sta	DCOMND,x
	dex
	bpl	restore

speed_query_done:

	; Do 14 command retries like the OS SIO routines
	lda	#14
	sta	CRETRY

	; Disable IRQs
	sei

	; Tell the OS to cut the VBI handler short
	lda	#1
	sta	CRITIC

	; Calculate the actual device code
	lda	DDEVIC
	clc
	adc	DUNIT
	adc	#$FF
	sta	CDEVIC

	; Set up the command buffer
	lda	DCOMND
	sta	CCOMND
	lda	DAUX1
	sta	CAUX1
	lda	DAUX2
	sta	CAUX2

	; Save stack pointer
	tsx
	stx	STACKP

retry:
	; Get the speed byte in X
	ldx	speed_byte

	; Decrement X to get the actual POKEY update divisor
	dex

	; Set the I/O speed
	stx	AUDF3

	lda	#0
	sta	AUDF4

	; Clear STATUS byte
	sta	STATUS
	
	; Set the number of bytes to transfer
	sta	BFENHI
	lda	#4
	sta	BFENLO

	; Set the buffer address
	lda	#<CDEVIC
	sta	BUFRLO
	lda	#>CDEVIC
	sta	BUFRHI

	; Assert the COMMAND line
	lda	#$34
	sta	PBCTL

	; Setup POKEY parameters
	lda	#$28
	sta	AUDCTL
	
	; Adjust the I/O sound volume
	ldx	#$A0
	lda	SOUNDR
	beq	no_sound
	ldx	#$A8
no_sound:
	stx	AUDC4

	; Send command frame
	jsr	send_frame

	; Clear timer
	jsr	clear_timer

	; Set up data frame buffer parameters
	lda	DBUFLO
	sta	BUFRLO
	lda	DBUFHI
	sta	BUFRHI
	lda	DBYTLO
	sta	BFENLO
	lda	DBYTHI
	sta	BFENHI

	; Check if we should send a data frame
	lda	DSTATS
	bpl	no_send

	; Send data frame, no more retries
	lda	#1
	sta	CRETRY
	jsr	send_frame

no_send:
	; Wait for COMPLETE byte
	jsr	wait_complete

	; Check if we should receive a data frame
	bit	DSTATS
	bvc	end

receive:
	; Receive data frame
	jsr	receive_frame

end:
	; Clean up and check error status
	jsr	cleanup
	ldy	STATUS
	beq	exit

	; Check if we should retry
	dec	CRETRY
	beq	exit
	jmp	retry

exit:
	lda	#0
	sta	CRITIC
	lda	POKMSK
	sta	IRQEN
	ldy	STATUS
	bne	:+
	iny
:	sty	STATUS
	cli
	rts
.endproc

.proc	send_byte
	tax

	; Wait until SEROUT is ready
	lda	#$10
:	bit	IRQST
	bne	:-

	; Clear SEROUT
	lda	#$ef
	sta	IRQEN
	lda	#$90
	sta	IRQEN

	; Write the byte
	txa
	sta	SEROUT

	; Update the checksum
	clc
	adc	CHKSUM
	adc	#0
	sta	CHKSUM

	; Done
	rts
.endproc

; ****************************************************************************
; Send frame
; ****************************************************************************

.proc send_frame
	; Wait some time
	ldy	#0
:	iny
	bne	:-

	; Set POKEY to synchronous send mode
	lda	#$23
	sta	SKCTL
	
	; Reset POKEY serial status
	sta	SKRES

	; Enable BREAK key and SEROUT ready interrupts, disable all others
	lda	#$90
	sta	IRQEN
	
	; Send the first byte
	lda	(BUFRLO),y
	sta	CHKSUM
	sta	SEROUT
	iny
	bne	send2		; Always

send1:	lda	(BUFRLO),y
	jsr	send_byte
	iny
	bne	send2
	inc	BUFRHI
	dec	BFENHI

send2:	cpy	BFENLO
	bne	send1
	lda	BFENHI
	bne	send1

	lda	CHKSUM
	jsr	send_byte

	; Wait for output complete interrupt
	lda	#$08
:	bit	IRQST
	bne	:-

	; Wait for the ACK code
	ldy	#2
	jsr	set_timeout
	lda	#$A0
	sta	IRQEN
	jsr	read_ack
	bne	device_nak_error
.endproc

.proc	done
	rts
.endproc

.proc	wait_complete
	lda	DTIMLO
	ror
	ror
	tay
	and	#$3f
	tax
	tya
	ror
	and	#$c0
	tay
	jsr	set_timeout_2
.endproc

.proc	read_ack
	; Set POKEY to asynchronous receive mode
	lda	#$13
	sta	SKCTL

	; Clear POKEY status
	sta	SKRES

	; Clear command line
	lda	#$3c
	sta	PBCTL

	; Receive a single byte
	ldy	#$80
	jsr	receive_byte

	cmp	#'A'
	beq	done

noack:
	cmp	#'C'
	beq	done

	cmp	#'E'
	beq	read_error
.endproc

.proc	device_nak_error
	lda	#DNACK
	.byte	$2c
.endproc

.proc	device_framing_error
	lda	#FRMERR
	.byte	$2c
.endproc

.proc	device_data_input_overrun_error
	lda	#OVRRUN
	.byte	$2c
.endproc

.proc	device_timeout_error
	lda	#TIMOUT
.endproc

.proc	general_error
	sta	STATUS
	ldx	STACKP
	txs
	jmp	sio_handler::end
.endproc

.proc	read_error
	lda	#DERROR
	sta	STATUS
	rts
.endproc

.proc cleanup
	; Reset POKEY
	lda	#$a0
	sta	AUDC4
	; Clear command line
	lda	#$3c
	sta	PBCTL
	; Run into clear_timer
.endproc

.proc clear_timer
	ldy	#0
	; Run into set_timeout
.endproc

.proc set_timeout
	ldx	#0
	; Run into set_timeout_2
.endproc

.proc set_timeout_2
	lda	#1
	jsr	SETVBV
	lda	#<device_timeout_error
	sta	CDTMA1
	lda	#>device_timeout_error
	sta	CDTMA1 + 1
	lda	#0
	rts
.endproc

.proc	receive_frame
	ldy	#0
	sty	CHKSUM
.endproc

.proc	receive_byte
	sty	receive_flag

loop:	lda	#$20

:	bit	IRQST
	bpl	break_error
	bne	:-

	ldx	SERIN

	lda	#$df
	sta	IRQEN

	lda	#$a0
	sta	IRQEN

	lda	#$20
	bit	SKSTAT
	sta	SKRES

	bpl	device_framing_error
	beq	device_data_input_overrun_error

	txa
	bit	receive_flag
	bmi	exit

	sta	(BUFRLO),y
	
	clc
	adc	CHKSUM
	adc	#0
	sta	CHKSUM

	iny
	bne	@1

	inc	BUFRHI
	dec	BFENHI

@1:	cpy	BFENLO
	bne	loop
	
	lda	BFENHI
	bne	loop

	ldy	#$80
	jsr	receive_byte

	cmp	CHKSUM
	beq	exit
	lda	#CHKERR
	jmp	general_error

exit:	rts
.endproc

.proc	break_error
	lda	#$7f
	sta	IRQEN
	lda	POKMSK
	sta	IRQEN
	ldx	STACKP
	txs
	jsr	cleanup
	ldy	#BRKABT
	sty	STATUS
	jmp	sio_handler::exit
.endproc
