Initial commit

1 files changed, 222 insertions, 0 deletions

diff --git a/src/drive.c b/src/drive.c
new file mode 100644
index 0000000..7afeaec
--- /dev/null
+++ b/src/drive.c
@@ -0,0 +1,222 @@
+#include "drive.h"
+
+#include <stdlib.h>
+
+const uint8_t SECTORS_PER_TRACK[40] = {
+	21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21,
+	19, 19, 19, 19, 19, 19, 19,
+	18, 18, 18, 18, 18, 18,
+	17, 17, 17, 17, 17, 17, 17, 17, 17, 17
+};
+
+const uint8_t BITRATE_PER_TRACK[40] = {
+	80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 80, 80,
+	73, 73, 73, 73, 73, 73, 73,
+	65, 65, 65, 65, 65, 65,
+	62 ,62, 62, 62, 62, 62, 62, 62, 62, 62
+};
+
+const uint8_t GCR_CONV[16] = {
+    0x0a, 0x0b, 0x12, 0x13,
+    0x0e, 0x0f, 0x16, 0x17,
+    0x09, 0x19, 0x1a, 0x1b,
+    0x0d, 0x1d, 0x1e, 0x15
+};
+
+void drive_init(drive_t *drive) {
+	drive->spinning = false;
+	drive->timer = 0;
+	drive->image = NULL;
+
+	drive->track = 17;
+	drive->halftrack = 0;
+	drive->image_ptr = 0x16500;
+	drive->sector = 0;
+	drive->byte = 0;
+
+	drive->gcr_word = 0;
+	drive->gcr_bits = 0;
+	drive->bit_counter = 0;
+
+	drive->gap_remain = 0;
+	drive->sync_remain = 4;
+	drive->header_remain = 8;
+	drive->data_remain = 0;
+	drive->data_checksum = 0;
+
+	drive->current_idx = 0;
+	drive->lookahead_idx = 0;
+
+	drive_cycle(drive, 0, false);
+}
+
+void drive_step_up(drive_t *drive) {
+	drive->halftrack = (drive->halftrack + 1) & 1;
+	if (drive->track < 39 && drive->halftrack == 0) {
+		drive->image_ptr += SECTORS_PER_TRACK[drive->track] << 8;
+		drive->track += 1;
+		drive->sector = drive->sector % SECTORS_PER_TRACK[drive->track];
+	}
+}
+
+void 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;
+		drive->track -= 1;
+		drive->sector = drive->sector % SECTORS_PER_TRACK[drive->track];
+	}
+}
+
+bool drive_cycle(drive_t *drive, uint8_t cycles, bool advance) {
+	bool byte_ready = false;
+	if (advance) {
+		for (uint8_t i = 0; i < cycles; i++) {
+			uint8_t j = (i + drive->current_idx) & (DRIVE_LOOKAHEAD - 1);
+			byte_ready |= drive->byte_ready[j];
+		}
+		drive->current_idx = (drive->current_idx + cycles) & (DRIVE_LOOKAHEAD - 1);
+	}
+
+	while (drive->lookahead_idx != ((drive->current_idx - 1) & (DRIVE_LOOKAHEAD - 1))) {
+		uint8_t j = (drive->lookahead_idx + 1) & (DRIVE_LOOKAHEAD - 1);
+		uint8_t timer_next = drive->timer + BITRATE_PER_TRACK[drive->track];
+
+		uint16_t prev = drive->unlatched[drive->lookahead_idx];
+		if (timer_next < drive->timer) {
+			if (drive->gcr_bits == 0) {
+				/*
+				if (drive->spinning && drive->image != NULL) {
+					if (!drive->sync_flag && (drive->byte == 0 || drive->byte == 2)) {
+						drive->gcr_word = 0x3ff;
+						drive->sync_flag = true;
+					} else {
+						uint8_t b = drive->image[drive->image_ptr + (drive->sector << 8) + drive->byte];
+						drive->gcr_word = (GCR_CONV[(b & 0xf0) >> 4] << 5) | GCR_CONV[b & 0x0f];
+						drive->sync_flag = false;
+
+						uint8_t byte_next = drive->byte + 1;
+						uint8_t sector_next = drive->sector;
+						if (byte_next == 0) {
+							sector_next += 1;
+						}
+						if (sector_next >= SECTORS_PER_TRACK[drive->track]) {
+							sector_next = 0;
+						}
+						drive->byte = byte_next;
+						drive->sector = sector_next;
+					}
+				}
+
+				drive->gcr_bits = 10;
+				*/
+				if (drive->gap_remain > 0) {
+					drive->gap_remain--;
+					drive->gcr_word = 0x55;
+					drive->gcr_bits = 8;
+				} else if (drive->sync_remain > 0) {
+					drive->sync_remain--;
+					drive->gcr_word = 0x3ff;
+					drive->gcr_bits = 10;
+				} else if (drive->header_remain > 0) {
+					drive->header_remain--;
+					uint8_t track_plus_one = drive->track + 1;
+					uint8_t checksum = drive->sector ^ track_plus_one;
+					switch (drive->header_remain) {
+					case 7:
+						drive->gcr_word = (GCR_CONV[0x0] << 5) | GCR_CONV[0x8];
+						break;
+					case 6:
+						drive->gcr_word = (GCR_CONV[(checksum & 0xf0) >> 4] << 5) | GCR_CONV[checksum & 0x0f];
+						break;
+					case 5:
+						drive->gcr_word = (GCR_CONV[(drive->sector & 0xf0) >> 4] << 5) | GCR_CONV[drive->sector & 0x0f];
+						break;
+					case 4:
+						drive->gcr_word = (GCR_CONV[(track_plus_one & 0xf0) >> 4] << 5) | GCR_CONV[track_plus_one & 0x0f];
+						break;
+					case 3:
+					case 2:
+						drive->gcr_word = (GCR_CONV[0x0] << 5) | GCR_CONV[0x0];
+						break;
+					case 1:
+					case 0:
+						drive->gcr_word = (GCR_CONV[0x0] << 5) | GCR_CONV[0xf];
+						break;
+					}
+					if (drive->header_remain == 0) {
+						drive->gap_remain = 9;
+						drive->sync_remain = 4;
+						drive->data_remain = 260;
+						drive->data_checksum = 0;
+					}
+					drive->gcr_bits = 10;
+				} else if (drive->data_remain > 0) {
+					drive->data_remain--;
+					switch (drive->data_remain) {
+					case 259:
+						drive->gcr_word = (GCR_CONV[0x0] << 5) | GCR_CONV[0x7];
+						break;
+					case 2:
+						drive->gcr_word = (GCR_CONV[(drive->data_checksum & 0xf0) >> 4] << 5) | GCR_CONV[drive->data_checksum & 0x0f];
+						break;
+					case 1:
+					case 0:
+						drive->gcr_word = (GCR_CONV[0x0] << 5) | GCR_CONV[0xf];
+						break;
+					default:
+						{
+							uint8_t b = drive->image[drive->image_ptr + (drive->sector << 8) + drive->byte];
+							drive->gcr_word = (GCR_CONV[(b & 0xf0) >> 4] << 5) | GCR_CONV[b & 0x0f];
+							drive->data_checksum ^= b;
+
+							uint8_t byte_next = drive->byte + 1;
+							uint8_t sector_next = drive->sector;
+							if (byte_next == 0) {
+								sector_next += 1;
+							}
+							if (sector_next >= SECTORS_PER_TRACK[drive->track]) {
+								sector_next = 0;
+							}
+							drive->byte = byte_next;
+							drive->sector = sector_next;
+						}
+						break;
+					}
+					if (drive->data_remain == 0) {
+						drive->gap_remain = 10;
+						drive->sync_remain = 4;
+						drive->header_remain = 8;
+					}
+					drive->gcr_bits = 10;
+				}
+			}
+
+			drive->unlatched[j] = ((prev << 1) | ((drive->gcr_word & 0x200) >> 9)) & 0x3ff;
+			drive->gcr_word <<= 1;
+			drive->gcr_bits--;
+			drive->bit_counter += 1;
+		} else {
+			drive->unlatched[j] = prev;
+		}
+
+		drive->byte_ready[j] = false;
+		drive->latched[j] = drive->latched[drive->lookahead_idx];
+		if ((drive->unlatched[j] & 0x3ff) == 0x3ff) {
+			drive->bit_counter = 0;
+			drive->sync[j] = true;
+		} else {
+			if (drive->bit_counter == 8) {
+				drive->bit_counter = 0;
+				drive->byte_ready[j] = true;
+				drive->latched[j] = drive->unlatched[j] & 0xff;
+			}
+			drive->sync[j] = false;
+		}
+
+		drive->lookahead_idx = j;
+		drive->timer = timer_next;
+	}
+
+	return byte_ready;
+}