feat: Disk swapping HEAD main

6 files changed, 146 insertions, 86 deletions

diff --git a/src/6502.c b/src/6502.c
index 09f0a04..0ed0773 100644
--- a/src/6502.c
+++ b/src/6502.c
@@ -1,3 +1,5 @@
+#include <hardware/flash.h>
+
 #include "6502.h"
 
 #define STACK_PUSH(v) \
@@ -529,7 +531,7 @@ void cpu_reset(cpu_t *cpu) {
 	FETCH_PC(0xfffc);
 }
 
-uint8_t cpu_step(cpu_t *cpu) {
+uint8_t __not_in_flash_func(cpu_step)(cpu_t *cpu) {
 	uint8_t cycles = 0;
 	if (cpu->jammed) {
 		return cycles;
diff --git a/src/6522.c b/src/6522.c
index 89bc102..76c8872 100644
--- a/src/6522.c
+++ b/src/6522.c
@@ -1,3 +1,5 @@
+#include <hardware/flash.h>
+
 #include "6522.h"
 
 void via_init(via_t *via) {
@@ -20,7 +22,7 @@ void via_init(via_t *via) {
 	via->tbv = true;
 }
 
-void via_read(via_t *via, uint8_t addr, uint8_t *v, uint8_t cycle) {
+void __not_in_flash_func(via_read)(via_t *via, uint8_t addr, uint8_t *v, uint8_t cycle) {
 	uint16_t tca = via->tca - cycle;
 	uint16_t tcb = via->tcb - cycle;
 
@@ -81,7 +83,7 @@ void via_read(via_t *via, uint8_t addr, uint8_t *v, uint8_t cycle) {
 	}
 }
 
-void via_write(via_t *via, uint8_t addr, uint8_t v, uint8_t cycle) {
+void __not_in_flash_func(via_write)(via_t *via, uint8_t addr, uint8_t v, uint8_t cycle) {
 	switch (addr) {
 		case 0:
 			via->orb = v;
@@ -153,7 +155,7 @@ void via_write(via_t *via, uint8_t addr, uint8_t v, uint8_t cycle) {
 	}
 }
 
-void via_cycle(via_t *via, uint8_t cycles) {
+void __not_in_flash_func(via_cycle)(via_t *via, uint8_t cycles) {
 	if (!via->tav) {
 		uint16_t tca = via->tca - cycles;
 
@@ -182,6 +184,6 @@ void via_cycle(via_t *via, uint8_t cycles) {
 	via->tcb = tcb;
 }
 
-bool via_irq(via_t *via) {
+bool __not_in_flash_func(via_irq)(via_t *via) {
 	return (via->ifr & via->ier) > 0;
 }
diff --git a/src/drive.c b/src/drive.c
index 9193608..8c06e69 100644
--- a/src/drive.c
+++ b/src/drive.c
@@ -1,7 +1,8 @@
-#include "drive.h"
-
+#include <hardware/flash.h>
 #include <stdlib.h>
 
+#include "drive.h"
+
 const uint8_t SECTORS_PER_TRACK[35] = {
 	21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21,
 	19, 19, 19, 19, 19, 19, 19,
@@ -46,7 +47,7 @@ void drive_init(drive_t *drive) {
 	drive_cycle(drive);
 }
 
-void drive_step_up(drive_t *drive) {
+void __not_in_flash_func(drive_step_up)(drive_t *drive) {
 	drive->halftrack = (drive->halftrack + 1) & 1;
 	if (drive->track < 34 && drive->halftrack == 0) {
 		drive->image_ptr += SECTORS_PER_TRACK[drive->track] << 8;
@@ -55,7 +56,7 @@ void drive_step_up(drive_t *drive) {
 	}
 }
 
-void drive_step_down(drive_t *drive) {
+void __not_in_flash_func(drive_step_down)(drive_t *drive) {
 	drive->halftrack = (drive->halftrack + 1) & 1;
 	if (drive->track > 0 && drive->halftrack == 0) {
 		drive->image_ptr -= SECTORS_PER_TRACK[drive->track - 1] << 8;
@@ -64,7 +65,7 @@ void drive_step_down(drive_t *drive) {
 	}
 }
 
-bool drive_advance(drive_t *drive, uint8_t cycles) {
+bool __not_in_flash_func(drive_advance)(drive_t *drive, uint8_t cycles) {
 	bool byte_ready = false;
 
 	for (uint8_t i = 0; i < cycles; i++) {
@@ -76,7 +77,7 @@ bool drive_advance(drive_t *drive, uint8_t cycles) {
 	return byte_ready;
 }
 
-void drive_cycle(drive_t *drive) {
+void __not_in_flash_func(drive_cycle)(drive_t *drive) {
 	while (drive->lookahead_idx != ((drive->current_idx - 1) & (DRIVE_LOOKAHEAD - 1))) {
 		uint8_t j = (drive->lookahead_idx + 1) & (DRIVE_LOOKAHEAD - 1);
 		uint16_t timer_next = drive->timer + BITRATES[drive->bitrate];
@@ -170,7 +171,9 @@ void drive_cycle(drive_t *drive) {
 
 			drive->unlatched[j] = ((prev << 1) | ((drive->gcr_word & 0x200) >> 9)) & 0x3ff;
 			drive->gcr_word <<= 1;
-			drive->gcr_bits--;
+			if (drive->gcr_bits > 0) {
+				drive->gcr_bits--;
+			}
 			drive->bit_counter += 1;
 		} else {
 			drive->unlatched[j] = prev;
diff --git a/src/drive.h b/src/drive.h
index a555f15..7d1daae 100644
--- a/src/drive.h
+++ b/src/drive.h
@@ -13,7 +13,7 @@ extern const uint8_t GCR_CONV[16];
 typedef struct drive_s {
 	volatile bool spinning;
 	uint16_t timer;
-	uint8_t *image;
+	volatile uint8_t *volatile image;
 
 	volatile uint8_t track;
 	volatile uint8_t halftrack;
diff --git a/src/ff/ffconf.h b/src/ff/ffconf.h
index c8370e6..c9c96e1 100644
--- a/src/ff/ffconf.h
+++ b/src/ff/ffconf.h
@@ -15,7 +15,7 @@
 /  and optional writing functions as well. */

 

 

-#define FF_FS_MINIMIZE	3

+#define FF_FS_MINIMIZE	1

 /* This option defines minimization level to remove some basic API functions.

 /

 /   0: Basic functions are fully enabled.

@@ -25,7 +25,7 @@
 /   3: f_lseek() function is removed in addition to 2. */

 

 

-#define FF_USE_FIND		0

+#define FF_USE_FIND		1

 /* This option switches filtered directory read functions, f_findfirst() and

 /  f_findnext(). (0:Disable, 1:Enable 2:Enable with matching altname[] too) */

 

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;
 }