feat: Disk swapping HEAD main

1 files changed, 124 insertions, 71 deletions

diff --git a/src/pico1541.c b/src/pico1541.c
index 3bf52a8..cb0473b 100644
--- a/src/pico1541.c
+++ b/src/pico1541.c
@@ -1,4 +1,3 @@
-#include <hardware/regs/intctrl.h>
 #include <stdint.h>
 
 #include <hardware/clocks.h>
@@ -9,6 +8,7 @@
 #include <pico/multicore.h>
 
 #include <bsp/board_api.h>
+#include <stdlib.h>
 #include <tusb.h>
 
 #include "ff/ff.h"
@@ -22,28 +22,32 @@ const uint32_t GPIO_CLK = 2;
 const uint32_t GPIO_DATA = 3;
 const uint32_t GPIO_DATA2 = 5;
 const uint32_t GPIO_ATN = 4;
+const uint32_t GPIO_SWAP = 6;
 const uint32_t GPIO_LED = 25;
 
 const uint8_t DEVICE = 8;
 
 #define FS_BYTES 1572864
+#define MAX_DISKS 16
 static uint8_t __in_flash("fs") __attribute__((aligned(4096))) FS[FS_BYTES];
 
 static FATFS FAT_FS;
+static uint8_t n_disks;
+static FILINFO disks[MAX_DISKS];
 
 static uint8_t ram[2048];
-static uint8_t dos1541[16384];
-static uint8_t disk[174848];
+static volatile uint8_t dos1541[16384];
+static volatile uint8_t disk[174848];
 
-uint32_t ts;
-cpu_t cpu;
+static uint32_t ts;
+static cpu_t cpu;
 static via_t via1;
 static via_t via2;
-drive_t drive;
+static drive_t drive;
 
-int8_t track_step;
+static int8_t track_step;
 
-void via_pa_read(via_t *via, uint8_t cycle) {
+void __not_in_flash_func(via_pa_read)(via_t *via, uint8_t cycle) {
 	if (via == &via2) {
 		via->ira = drive.latched[(drive.current_idx + cycle) & (DRIVE_LOOKAHEAD - 1)];
 	} else {
@@ -51,7 +55,7 @@ void via_pa_read(via_t *via, uint8_t cycle) {
 	}
 }
 
-void via_pb_read(via_t *via, uint8_t cycle) {
+void __not_in_flash_func(via_pb_read)(via_t *via, uint8_t cycle) {
 	if (via == &via1) {
 		while (ts + cycle > time_us_32()) {}
 		uint8_t clk = gpio_get(GPIO_CLK) ? 0 : 0b00000100;
@@ -69,10 +73,10 @@ void via_pb_read(via_t *via, uint8_t cycle) {
 	}
 }
 
-void via_pa_write(via_t *via, uint8_t cycle) {
+void __not_in_flash_func(via_pa_write)(via_t *via, uint8_t cycle) {
 }
 
-void via_pb_write(via_t *via, uint8_t cycle) {
+void __not_in_flash_func(via_pb_write)(via_t *via, uint8_t cycle) {
 	if (via == &via1) {
 		while (ts + cycle > time_us_32()) {}
 		gpio_set_dir(GPIO_CLK, (via->orb & 0b00001000) > 0 && (via->ddrb & 0b00001000) > 0);
@@ -102,7 +106,7 @@ void via_pb_write(via_t *via, uint8_t cycle) {
 	} 
 }
 
-uint8_t bus_read(uint16_t addr, uint8_t cycle) {
+uint8_t __not_in_flash_func(bus_read)(uint16_t addr, uint8_t cycle) {
 	if (addr < 2048) {
 		return ram[addr];
 	} else if (addr >= 0xc000) {
@@ -120,7 +124,7 @@ uint8_t bus_read(uint16_t addr, uint8_t cycle) {
 	}
 }
 
-void bus_write(uint16_t addr, uint8_t v, uint8_t cycle) {
+void __not_in_flash_func(bus_write)(uint16_t addr, uint8_t v, uint8_t cycle) {
 	if (addr < 2048) {
 		ram[addr] = v;
 	} else if (addr >= 0x1800 && addr < 0x1810) {
@@ -130,7 +134,7 @@ void bus_write(uint16_t addr, uint8_t v, uint8_t cycle) {
 	}
 }
 
-void atn_irq_callback(uint pin, uint32_t flags) {
+void __not_in_flash_func(atn_irq_callback)(uint pin, uint32_t flags) {
 	if (flags & GPIO_IRQ_EDGE_RISE) {
 		if (!(via1.pcr & 1)) {
 			via1.ifr |= 0b10000010;
@@ -274,79 +278,125 @@ DRESULT disk_ioctl(BYTE pdrv, BYTE cmd, void *buff) {
 	return RES_PARERR;
 }
 
-void drive_irq() {
-	multicore_fifo_pop_blocking();
-	drive_cycle(&drive);
+int file_cmp(const void *lhs, const void *rhs) {
+	return strcmp(((FILINFO *)lhs)->fname, ((FILINFO *)rhs)->fname);
+}
+
+void __not_in_flash_func(drive_irq)() {
+	while (multicore_fifo_rvalid()) {
+		multicore_fifo_pop_blocking();
+		drive_cycle(&drive);
+	}
+
 	multicore_fifo_clear_irq();
 }
 
 void drive_thread() {
 	drive_init(&drive);
-	drive.image = disk;
 
-	irq_set_exclusive_handler(SIO_IRQ_PROC1, drive_irq);
-	irq_set_priority(SIO_IRQ_PROC1, 0);
-	irq_set_enabled(SIO_IRQ_PROC1, true);
+	multicore_fifo_clear_irq();
+	irq_set_exclusive_handler(SIO_FIFO_IRQ_NUM(1), drive_irq);
+	irq_set_priority(SIO_FIFO_IRQ_NUM(1), 0);
+	irq_set_enabled(SIO_FIFO_IRQ_NUM(1), true);
 	
-	multicore_fifo_push_blocking(1);
-}
-
-int main() {
-	board_init();
-	tud_init(BOARD_TUD_RHPORT);
-	if (board_init_after_tusb) {
-		board_init_after_tusb();
-	}
-
-	while (!ejected) {
-		tud_task();
-	}
-	tud_disconnect();
-
 	f_mount(&FAT_FS, "0:", 0);
 	
 	FIL file;
 	FRESULT res;
 	uint32_t btr;
 	uint32_t br;
+	uint8_t fread_buf[4096];
 
 	res = f_open(&file, "0:dos1541.bin", FA_READ);
-	if (res) {
-		return 0;
-	}
-	btr = 16384;
-	br = 0;
-	while (btr > 0) {
-		UINT brr;
-		if (btr < 4096) {
-			f_read(&file, dos1541 + br, btr, &brr);
-		} else {
-			f_read(&file, dos1541 + br, 4096, &brr);
+	if (!res) {
+		btr = 16384;
+		br = 0;
+		while (btr > 0) {
+			UINT brr;
+			if (btr < 4096) {
+				f_read(&file, fread_buf, btr, &brr);
+			} else {
+				f_read(&file, fread_buf, 4096, &brr);
+			}
+			for (uint32_t i = 0; i < brr; i++) {
+				dos1541[br + i] = fread_buf[i];
+			}
+			br += brr;
+			btr -= brr;
 		}
-		br += brr;
-		btr -= brr;
+		f_close(&file);
 	}
-	f_close(&file);
 
-	res = f_open(&file, "0:image.d64", FA_READ);
-	if (res) {
-		return 0;
+	n_disks = 0;
+	DIR d;
+	res = f_findfirst(&d, &disks[0], "0:", "*.d64");
+	while (
+		!res
+		&& disks[n_disks].fname[0]
+		&& n_disks < MAX_DISKS - 1
+	) {
+		if (disks[n_disks].fsize == 174848) {
+			n_disks++;
+		}
+		res = f_findnext(&d, &disks[n_disks]);
 	}
-	btr = 174848;
-	br = 0;
-	while (btr > 0) {
-		UINT brr;
-		if (btr < 4096) {
-			f_read(&file, disk + br, btr, &brr);
-		} else {
-			f_read(&file, disk + br, 4096, &brr);
+	f_closedir(&d);
+
+	qsort(disks, n_disks, sizeof(FILINFO), file_cmp);
+
+	multicore_fifo_push_blocking(1);
+
+	uint8_t cur_disk = 0;
+	while (true) {
+		drive.image = NULL;
+
+		if (n_disks > 0) {
+			char im_path[16];
+			sprintf(im_path, "0:%s", disks[cur_disk].fname);
+			
+			res = f_open(&file, im_path, FA_READ);
+			if (!res) {
+				btr = 174848;
+				br = 0;
+				while (btr > 0) {
+					UINT brr;
+					if (btr < 4096) {
+						f_read(&file, fread_buf, btr, &brr);
+					} else {
+						f_read(&file, fread_buf, 4096, &brr);
+					}
+					for (uint32_t i = 0; i < brr; i++) {
+						disk[br + i] = fread_buf[i];
+					}
+					br += brr;
+					btr -= brr;
+				}
+				f_close(&file);
+			}
+
+			drive.image = disk;
+		}
+
+		while (gpio_get(GPIO_SWAP)) {}
+
+		cur_disk++;
+		if (cur_disk >= MAX_DISKS) {
+			cur_disk = 0;
 		}
-		br += brr;
-		btr -= brr;
 	}
-	f_close(&file);
+}
+
+int __not_in_flash_func(main)() {
+	board_init();
+	tud_init(BOARD_TUD_RHPORT);
+	if (board_init_after_tusb) {
+		board_init_after_tusb();
+	}
 
-	f_unmount("0:");
+	while (!ejected) {
+		tud_task();
+	}
+	tud_disconnect();
 	
 	set_sys_clock_khz(250000, true);
 	multicore_launch_core1(drive_thread);
@@ -355,7 +405,8 @@ int main() {
 	gpio_init(GPIO_DATA);
 	gpio_init(GPIO_ATN);
 	gpio_init(GPIO_DATA2);
-	gpio_init(25);
+	gpio_init(GPIO_SWAP);
+	gpio_init(GPIO_LED);
 
 	gpio_put(GPIO_CLK, 0);
 	gpio_put(GPIO_DATA, 0);
@@ -366,12 +417,14 @@ int main() {
 	gpio_set_dir(GPIO_DATA, GPIO_IN);
 	gpio_set_dir(GPIO_ATN, GPIO_IN);
 	gpio_set_dir(GPIO_LED, GPIO_OUT);
+	gpio_set_dir(GPIO_SWAP, GPIO_IN);
 	gpio_set_dir(GPIO_DATA2, GPIO_IN);
 
 	gpio_set_pulls(GPIO_CLK, true, false);
 	gpio_set_pulls(GPIO_DATA, true, false);
 	gpio_set_pulls(GPIO_DATA2, true, false);
 	gpio_set_pulls(GPIO_ATN, true, false);
+	gpio_set_pulls(GPIO_SWAP, true, false);
 
 	gpio_set_irq_enabled_with_callback(GPIO_ATN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &atn_irq_callback);
 
@@ -383,25 +436,25 @@ int main() {
 	via_init(&via2);
 
 	ts = time_us_32();
-	uint16_t osc2 = 0xffff;
 	while (!cpu.jammed) {
 		if (ts + 100 < time_us_32()) {
 			break;
 		}
 
 		uint8_t cycles = cpu_step(&cpu);
+		via_cycle(&via1, cycles);
+		via_cycle(&via2, cycles);
+		cpu.irq = via_irq(&via1) | via_irq(&via2);
+
 		if (drive_advance(&drive, cycles) && (via2.pcr & 0b1110) == 0b1110) {
 			cpu.flags |= 1 << FLAG_V_BIT;
 		}
 		multicore_fifo_push_blocking_inline(1);
 
-		via_cycle(&via1, cycles);
-		via_cycle(&via2, cycles);
-		cpu.irq = via_irq(&via1) | via_irq(&via2);
-
 		ts += cycles;
 		while (ts > time_us_32()) {}
 	}
 
+
 	return 0;
 }