From 3a80867854a163779a213c38e488c1b7c892794a Mon Sep 17 00:00:00 2001
From: FeralChild64 <you@example.com>
Date: Wed, 12 May 2021 21:48:09 +0200
Subject: [PATCH] added MEGA65 floppy engine handling

---
 src/dos_m65/lowlevel/lowlevel_helpers.s    | 49 ++++++++++++++++++++++
 src/dos_m65/lowlevel/lowlevel_readsector.s | 40 ++++--------------
 2 files changed, 58 insertions(+), 31 deletions(-)
 create mode 100644 src/dos_m65/lowlevel/lowlevel_helpers.s

diff --git a/src/dos_m65/lowlevel/lowlevel_helpers.s b/src/dos_m65/lowlevel/lowlevel_helpers.s
new file mode 100644
index 00000000..cd81ec98
--- /dev/null
+++ b/src/dos_m65/lowlevel/lowlevel_helpers.s
@@ -0,0 +1,49 @@
+
+; Helper routines for low-level disk operations
+
+
+lowlevel_ensure_presence:
+
+	lda FDC_STATUS_B ; $D083           ; check if disk is inserted    XXX this is probably not enough!
+	and #$08
+	beq @not_present
+	rts
+
+@not_present:
+
+	pla
+	pla
+
+	jsr lowlevel_motor_off             ; if disk not present, disable drive motor
+
+	; XXX set error code, invalidate buffers, etc.
+
+	sec
+	rts
+
+lowlevel_motor_on:
+
+	lda #%00100000                     ; enable drive motor and LED
+	tsb FDC_CONTROL  ; $D080
+	bra lowlevel_wait_clr_BUSY
+
+lowlevel_motor_off:
+
+	lda #%00100000                     ; disable drive motor and LED
+	trb FDC_CONTROL  ; $D080
+
+	; FALLTROUGH
+
+lowlevel_wait_clr_BUSY:
+
+	bit FDC_STATUS_A ; $D082
+	bmi lowlevel_wait_clr_BUSY
+
+	rts
+
+lowlevel_wait_set_RDREQ:
+
+	bit FDC_STATUS_B ; $D083
+	bpl lowlevel_wait_set_RDREQ
+
+	rts
diff --git a/src/dos_m65/lowlevel/lowlevel_readsector.s b/src/dos_m65/lowlevel/lowlevel_readsector.s
index 8f539a82..8d2de407 100644
--- a/src/dos_m65/lowlevel/lowlevel_readsector.s
+++ b/src/dos_m65/lowlevel/lowlevel_readsector.s
@@ -4,21 +4,12 @@
 
 lowlevel_readsector:
 
-	; Set drive, check if disk is present
+	; Set drive, ensure disk is present
 
 	lda #%00001111                     ; set drive 0 (internal) and side 0
 	trb FDC_CONTROL  ; $D080
 
-	lda FDC_STATUS_B ; $D083           ; check if disk is inserted    XXX this is probably not enough!
-	and #$08
-	bne @disk_present
-
-	; XXX set error code
-
-	sec
-	rts
-
-@disk_present:
+	jsr lowlevel_ensure_presence
 
 	; Check if buffer contains data from given track
 	; XXX compare device and unit too
@@ -41,6 +32,9 @@ lowlevel_readsector:
 
 lowlevel_readsector_force:
 
+	jsr lowlevel_motor_on              ; enable drive motor and LED
+	jsr lowlevel_ensure_presence
+
 	lda #%10000000                     ; select floppy buffer
 	trb SD_BUFCTL    ; $D689
 
@@ -85,11 +79,13 @@ lowlevel_readsector_force:
 
 	; Wait for DRQ and EQ flags to go high
 
-@lp3:
+@lp1:
 	lda FDC_STATUS_A ; $D082
 	and #%01100000
 	cmp #%01000000   ; XXX is this correct? .A seems to stay at $40 at this point
-	bne @lp3
+	bne @lp1
+
+	jsr lowlevel_motor_off             ; disable drive motor
 
 	; Copy sector to buffer in RAM
 
@@ -116,21 +112,3 @@ lowlevel_readsector_force:
 
 	clc
 	rts
-
-
-
-; XXX consider moving these to a separate file
-
-lowlevel_wait_clr_BUSY:
-
-	bit FDC_STATUS_A ; $D082
-	bmi lowlevel_wait_clr_BUSY
-
-	rts
-
-lowlevel_wait_set_RDREQ:
-
-	bit FDC_STATUS_B ; $D083
-	bpl lowlevel_wait_set_RDREQ
-
-	rts