Initial commit

13 files changed, 1616 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..00c49ee
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,4 @@
+.DS_Store
+testbin/
+build/
+
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..59c6d70
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.13...3.27)
+
+include(pico_sdk_import.cmake)
+
+project(pico1541 VERSION 0.1.0)
+pico_sdk_init()
+
+add_executable(pico1541 src/pico1541.c src/6502.c src/6522.c src/drive.c testbin/1541.o testbin/testimage.o)
+pico_set_binary_type(pico1541 copy_to_ram)
+target_link_libraries(pico1541 pico_runtime hardware_timer)
+pico_add_extra_outputs(pico1541)
\ No newline at end of file
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..6594374
--- /dev/null
+++ b/README.md
@@ -0,0 +1,3 @@
+# pico1541
+
+Work in progress. No documentation yet, check back soon.
diff --git a/flake.lock b/flake.lock
new file mode 100644
index 0000000..49bf1bc
--- /dev/null
+++ b/flake.lock
@@ -0,0 +1,27 @@
+{
+  "nodes": {
+    "nixpkgs": {
+      "locked": {
+        "lastModified": 1728492678,
+        "narHash": "sha256-9UTxR8eukdg+XZeHgxW5hQA9fIKHsKCdOIUycTryeVw=",
+        "owner": "nixos",
+        "repo": "nixpkgs",
+        "rev": "5633bcff0c6162b9e4b5f1264264611e950c8ec7",
+        "type": "github"
+      },
+      "original": {
+        "owner": "nixos",
+        "ref": "nixos-unstable",
+        "repo": "nixpkgs",
+        "type": "github"
+      }
+    },
+    "root": {
+      "inputs": {
+        "nixpkgs": "nixpkgs"
+      }
+    }
+  },
+  "root": "root",
+  "version": 7
+}
diff --git a/flake.nix b/flake.nix
new file mode 100644
index 0000000..4c97fc4
--- /dev/null
+++ b/flake.nix
@@ -0,0 +1,27 @@
+{
+  inputs = {
+    nixpkgs.url = "github:nixos/nixpkgs?ref=nixos-unstable";
+  };
+
+  outputs = { self, nixpkgs }: let
+    system = "aarch64-darwin";
+    pkgs = nixpkgs.legacyPackages.${system};
+
+    pico1541 = { stdenvNoCC, cmake, ninja, python3, gcc-arm-embedded-13, pico-sdk, picotool }: stdenvNoCC.mkDerivation {
+      pname = "pico1541";
+      version = "0.1.0";
+      src = ./.;
+
+      nativeBuildInputs = [ ninja cmake python3 picotool ];
+      buildInputs = [ gcc-arm-embedded-13 ];
+      PICO_SDK_PATH = "${pico-sdk.override { withSubmodules = true; }}/lib/pico-sdk";
+    };
+  in rec {
+    packages.${system}.default = pkgs.callPackage pico1541 {};
+    devShells.${system}.default = pkgs.mkShell {
+      packages = [ pkgs.clang-tools pkgs.openocd ];
+      inputsFrom = [ packages.${system}.default ];
+      PICO_SDK_PATH = "${pkgs.pico-sdk.override { withSubmodules = true; }}/lib/pico-sdk";
+    };
+  };
+}
diff --git a/pico_sdk_import.cmake b/pico_sdk_import.cmake
new file mode 100644
index 0000000..a0721d0
--- /dev/null
+++ b/pico_sdk_import.cmake
@@ -0,0 +1,84 @@
+# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
+
+# This can be dropped into an external project to help locate this SDK
+# It should be include()ed prior to project()
+
+if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
+    set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
+    message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
+    set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
+    message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
+    set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
+    message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_TAG} AND (NOT PICO_SDK_FETCH_FROM_GIT_TAG))
+    set(PICO_SDK_FETCH_FROM_GIT_TAG $ENV{PICO_SDK_FETCH_FROM_GIT_TAG})
+    message("Using PICO_SDK_FETCH_FROM_GIT_TAG from environment ('${PICO_SDK_FETCH_FROM_GIT_TAG}')")
+endif ()
+
+if (PICO_SDK_FETCH_FROM_GIT AND NOT PICO_SDK_FETCH_FROM_GIT_TAG)
+  set(PICO_SDK_FETCH_FROM_GIT_TAG "master")
+  message("Using master as default value for PICO_SDK_FETCH_FROM_GIT_TAG")
+endif()
+
+set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
+set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
+set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
+set(PICO_SDK_FETCH_FROM_GIT_TAG "${PICO_SDK_FETCH_FROM_GIT_TAG}" CACHE FILEPATH "release tag for SDK")
+
+if (NOT PICO_SDK_PATH)
+    if (PICO_SDK_FETCH_FROM_GIT)
+        include(FetchContent)
+        set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
+        if (PICO_SDK_FETCH_FROM_GIT_PATH)
+            get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
+        endif ()
+        # GIT_SUBMODULES_RECURSE was added in 3.17
+        if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
+            FetchContent_Declare(
+                    pico_sdk
+                    GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                    GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
+                    GIT_SUBMODULES_RECURSE FALSE
+            )
+        else ()
+            FetchContent_Declare(
+                    pico_sdk
+                    GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                    GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
+            )
+        endif ()
+
+        if (NOT pico_sdk)
+            message("Downloading Raspberry Pi Pico SDK")
+            FetchContent_Populate(pico_sdk)
+            set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
+        endif ()
+        set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
+    else ()
+        message(FATAL_ERROR
+                "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
+                )
+    endif ()
+endif ()
+
+get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
+if (NOT EXISTS ${PICO_SDK_PATH})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
+endif ()
+
+set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
+if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
+endif ()
+
+set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
+
+include(${PICO_SDK_INIT_CMAKE_FILE})
diff --git a/src/6502.c b/src/6502.c
new file mode 100644
index 0000000..06ba198
--- /dev/null
+++ b/src/6502.c
@@ -0,0 +1,731 @@
+#include "6502.h"
+
+#define STACK_PUSH(v) \
+	do { \
+		bus_write(0x100 + cpu->sp, v, cycles); \
+		cpu->sp--; \
+		cycles++; \
+	} while (0)
+
+#define STACK_PULL(r, mask) \
+	do { \
+		bus_read(0x100 + cpu->sp, cycles); \
+		cpu->sp++; \
+		cycles++; \
+		cpu->r = (cpu->r & ~mask) | (bus_read(0x100 + cpu->sp, cycles) & mask); \
+		cycles++; \
+	} while (0)
+
+#define PUSH_STATE() \
+	do { \
+		STACK_PUSH((cpu->pc & 0xff00) >> 8); \
+		STACK_PUSH(cpu->pc & 0xff); \
+		STACK_PUSH(cpu->flags); \
+	} while (0)
+
+#define FETCH_PC(addr) \
+	do { \
+		cpu->pc = 0; \
+		cpu->pc |= bus_read(addr, cycles); \
+		cycles++; \
+		cpu->pc |= bus_read(addr + 1, cycles) << 8; \
+		cycles++; \
+	} while (0)
+
+#define OP(reg, src, op) \
+	do { \
+		uint8_t r = cpu->reg; \
+		uint16_t ea = 0; \
+		uint8_t a; \
+		src(); \
+		op(); \
+		FLAG_SET(N, (r & 0x80) >> 7); \
+		FLAG_SET(Z, (r == 0) ? 1 : 0); \
+		cpu->reg = r; \
+	} while (0); break
+
+#define CMP(reg, src) \
+	do { \
+		uint8_t r = cpu->reg; \
+		uint16_t ea = 0; \
+		uint8_t a; \
+		src(); \
+		uint16_t res = r - a; \
+		FLAG_SET(C, (~res & 0x100) >> 8); \
+		FLAG_SET(N, (res & 0x80) >> 7); \
+		FLAG_SET(Z, ((res & 0xff) == 0) ? 1 : 0); \
+	} while (0); break
+
+#define BIT(src) \
+	do { \
+		uint16_t ea = 0; \
+		uint8_t a; \
+		src(); \
+		FLAG_SET(V, (a & 0x40) >> 6); \
+		FLAG_SET(N, (a & 0x80) >> 7); \
+		FLAG_SET(Z, ((a & cpu->a) == 0) ? 1 : 0); \
+	} while (0); break
+
+#define RMW(src, op, dst) \
+	do { \
+		uint8_t a; \
+		uint16_t ea = 0; \
+		src(); \
+		dst(); \
+		op(); \
+		FLAG_SET(N, (a & 0x80) >> 7); \
+		FLAG_SET(Z, (a == 0) ? 1 : 0); \
+		dst(); \
+	} while (0); break
+
+#define STORE(src, dst) \
+	do { \
+		uint8_t a; \
+		src(); \
+		dst(); \
+	} while (0); break
+
+#define B(f, v) \
+	do { \
+		uint8_t a; \
+		R_IMM(); \
+		if (FLAG_GET(f) == v) { \
+			uint16_t ea = (cpu->pc & 0xff) + (int8_t)a; \
+			uint16_t carry = ea & 0xff00; \
+			ea &= 0xff; \
+			ea |= cpu->pc & 0xff00; \
+			bus_read(ea, cycles); \
+			cycles++; \
+			if (carry > 0) { \
+				bus_read(ea, cycles); \
+				ea += carry; \
+				cycles++; \
+			} \
+			cpu->pc = ea; \
+		} \
+	} while (0); break
+
+#define R_IMP()
+
+#define R_REG(r) \
+	do { \
+		a = cpu->r; \
+	} while (0)
+#define R_A() R_REG(a)
+#define R_X() R_REG(x)
+#define R_Y() R_REG(y)
+
+#define R_IMM() \
+	do { \
+		cpu->pc++; \
+		a = operand; \
+	} while (0)
+
+#define R_ZP() \
+	do { \
+		cpu->pc++; \
+		a = bus_read(operand, cycles); \
+		ea = operand; \
+		cycles++; \
+	} while (0)
+
+#define R_ZPX() \
+	do { \
+		cpu->pc++; \
+		bus_read(operand, cycles); \
+		cycles++; \
+		a = bus_read((operand + cpu->x) & 0xff, cycles); \
+		ea = (operand + cpu->x) & 0xff; \
+		cycles++; \
+	} while (0)
+
+#define R_ZPY() \
+	do { \
+		cpu->pc++; \
+		bus_read(operand, cycles); \
+		cycles++; \
+		a = bus_read((operand + cpu->y) & 0xff, cycles); \
+		ea = (operand + cpu->y) & 0xff; \
+		cycles++; \
+	} while (0)
+
+#define R_ABS() \
+	do { \
+		cpu->pc++; \
+		ea = operand; \
+		ea |= bus_read(cpu->pc, cycles) << 8; \
+		cpu->pc++; \
+		cycles++; \
+		a = bus_read(ea, cycles); \
+		cycles++; \
+	} while (0)
+
+#define R_ABSI(i) \
+	do { \
+		cpu->pc++; \
+		ea = operand; \
+		ea += cpu->i; \
+		uint16_t carry = ea & 0x0100; \
+		ea &= 0xff; \
+		ea |= bus_read(cpu->pc, cycles) << 8; \
+		cpu->pc++; \
+		cycles++; \
+		a = bus_read(ea, cycles); \
+		cycles++; \
+		if (carry) { \
+			ea += 0x100; \
+			a = bus_read(ea, cycles); \
+			cycles++; \
+		} \
+	} while (0)
+
+#define R_ABSX() R_ABSI(x)
+#define R_ABSY() R_ABSI(y)
+
+#define R_INDX() \
+	do { \
+		cpu->pc++; \
+		bus_read(operand, cycles); \
+		cycles++; \
+		ea = bus_read((operand + cpu->x) & 0xff, cycles); \
+		cycles++; \
+		ea |= bus_read((operand + cpu->x + 1) & 0xff, cycles) << 8; \
+		cycles++; \
+		a = bus_read(ea, cycles); \
+		cycles++; \
+	} while (0)
+
+#define R_INDY() \
+	do { \
+		cpu->pc++; \
+		ea = bus_read(operand, cycles); \
+		cycles++; \
+		ea += cpu->y; \
+		uint16_t carry = ea & 0x0100; \
+		ea &= 0xff; \
+		ea |= bus_read((operand + 1) & 0xff, cycles) << 8; \
+		cycles++; \
+		a = bus_read(ea, cycles); \
+		cycles++; \
+		if (carry) { \
+			ea += 0x100; \
+			a = bus_read(ea, cycles); \
+			cycles++; \
+		} \
+	} while (0)
+
+#define W_R(r) \
+	do { \
+		cpu->r = a; \
+	} while (0)
+#define W_A() W_R(a)
+#define W_X() W_R(x)
+#define W_Y() W_R(y)
+
+#define W_ZP() \
+	do { \
+		cpu->pc++; \
+		bus_write(operand, a, cycles); \
+		cycles++; \
+	} while (0)
+
+#define W_ZPX() \
+	do { \
+		cpu->pc++; \
+		bus_read(operand, cycles); \
+		cycles++; \
+		bus_write((operand + cpu->x) & 0xff, a, cycles); \
+		cycles++; \
+	} while (0)
+
+#define W_ZPY() \
+	do { \
+		cpu->pc++; \
+		bus_read(operand, cycles); \
+		cycles++; \
+		bus_write((operand + cpu->y) & 0xff, a, cycles); \
+		cycles++; \
+	} while (0)
+
+#define W_ABS() \
+	do { \
+		cpu->pc++; \
+		uint16_t ea = operand; \
+		ea |= bus_read(cpu->pc, cycles) << 8; \
+		cpu->pc++; \
+		cycles++; \
+		bus_write(ea, a, cycles); \
+		cycles++; \
+	} while (0)
+
+#define W_ABSI(i) \
+	do { \
+		cpu->pc++; \
+		uint16_t ea = operand; \
+		ea += cpu->i; \
+		uint16_t carry = ea & 0x0100; \
+		ea &= 0xff; \
+		ea |= bus_read(cpu->pc, cycles) << 8; \
+		cpu->pc++; \
+		cycles++; \
+		if (!carry) { \
+			bus_write(ea, a, cycles); \
+			cycles++; \
+		} else { \
+			bus_read(ea, cycles); \
+			cycles++; \
+			ea += 0x100; \
+			bus_write(ea, a, cycles); \
+			cycles++; \
+		} \
+	} while (0)
+
+#define W_ABSX() W_ABSI(x)
+#define W_ABSY() W_ABSI(y)
+
+#define W_INDX() \
+	do { \
+		cpu->pc++; \
+		bus_read(operand, cycles); \
+		cycles++; \
+		uint16_t ea = bus_read((operand + cpu->x) & 0xff, cycles); \
+		cycles++; \
+		ea |= bus_read((operand + cpu->x + 1) & 0xff, cycles) << 8; \
+		cycles++; \
+		bus_write(ea, a, cycles); \
+		cycles++; \
+	} while (0)
+
+#define W_INDY() \
+	do { \
+		cpu->pc++; \
+		uint16_t ea = bus_read(operand, cycles); \
+		cycles++; \
+		ea += cpu->y; \
+		uint16_t carry = ea & 0x0100; \
+		ea &= 0xff; \
+		ea |= bus_read((operand + 1) & 0xff, cycles) << 8; \
+		cycles++; \
+		if (!carry) { \
+			bus_write(ea, a, cycles); \
+			cycles++; \
+		} else { \
+			bus_read(ea, cycles); \
+			cycles++; \
+			ea += 0x100; \
+			bus_write(ea, a, cycles); \
+			cycles++; \
+		} \
+	} while (0)
+
+#define W_EA() \
+	do { \
+		bus_write(ea, a, cycles); \
+		cycles++; \
+	} while (0)
+
+#define ADC() \
+	do { \
+		uint16_t res = r + a + FLAG_GET(C); \
+		FLAG_SET(C, (res & 0x100) >> 8); \
+		FLAG_SET(V, ((r ^ res) & (a ^ res) & 0x80) >> 7); \
+		r = res & 0xff; \
+	} while (0)
+
+#define SBC() \
+	do { \
+		a = ~a; \
+		ADC(); \
+	} while (0)
+
+#define ASL() \
+	do { \
+		FLAG_SET(C, (a & 0x80) >> 7); \
+		a = a << 1; \
+	} while (0)
+
+#define LSR() \
+	do { \
+		FLAG_SET(C, a & 1); \
+		a = a >> 1; \
+	} while (0)
+
+#define ROL() \
+	do { \
+		uint8_t c = FLAG_GET(C); \
+		FLAG_SET(C, (a & 0x80) >> 7); \
+		a = (a << 1) | c; \
+	} while (0)
+
+#define ROR() \
+	do { \
+		uint8_t c = FLAG_GET(C); \
+		FLAG_SET(C, a & 1); \
+		a = (a >> 1) | (c << 7); \
+	} while (0)
+
+#define AND() r = r & a
+#define EOR() r = r ^ a
+#define ORA() r = r | a
+
+#define INC() a = a + 1
+#define DEC() a = a - 1
+
+#define LOAD() r = a
+
+void cpu_reset(cpu_t *cpu) {
+	cpu->a = 0;
+	cpu->x = 0;
+	cpu->y = 0;
+	cpu->sp = 0xff;
+	cpu->flags = 0;
+	cpu->irq = false;
+	cpu->nmi = false;
+	cpu->jammed = false;
+
+	uint8_t cycles = 0;
+	FETCH_PC(0xfffc);
+}
+
+uint8_t cpu_step(cpu_t *cpu) {
+	uint8_t cycles = 0;
+	if (cpu->jammed) {
+		return cycles;
+	}
+
+	// Service interrupts
+	if (cpu->nmi) {
+		cycles += 2; // ?
+		PUSH_STATE();
+		FETCH_PC(0xfffa);
+		FLAG_SET(I, 1);
+	} else if (!FLAG_GET(I) && cpu->irq) {
+		cycles += 2; // ?
+		PUSH_STATE();
+		FETCH_PC(0xfffe);
+		FLAG_SET(I, 1);
+	}
+
+	// Fetch opcode
+	uint8_t opcode = bus_read(cpu->pc, cycles);
+	cpu->pc++;
+	cycles++;
+
+	// Always fetch next byte
+	uint8_t operand = bus_read(cpu->pc, cycles);
+	cycles++;
+
+	// Execute opcode
+	switch (opcode) {
+		// LDA
+		case 0xa9: OP(a, R_IMM, LOAD);
+		case 0xa5: OP(a, R_ZP, LOAD);
+		case 0xb5: OP(a, R_ZPX, LOAD);
+		case 0xad: OP(a, R_ABS, LOAD);
+		case 0xbd: OP(a, R_ABSX, LOAD);
+		case 0xb9: OP(a, R_ABSY, LOAD);
+		case 0xa1: OP(a, R_INDX, LOAD);
+		case 0xb1: OP(a, R_INDY, LOAD);
+
+		// LDX
+		case 0xa2: OP(x, R_IMM, LOAD);
+		case 0xa6: OP(x, R_ZP, LOAD);
+		case 0xb6: OP(x, R_ZPY, LOAD);
+		case 0xae: OP(x, R_ABS, LOAD);
+		case 0xbe: OP(x, R_ABSY, LOAD);
+
+		// LDY
+		case 0xa0: OP(y, R_IMM, LOAD);
+		case 0xa4: OP(y, R_ZP, LOAD);
+		case 0xb4: OP(y, R_ZPX, LOAD);
+		case 0xac: OP(y, R_ABS, LOAD);
+		case 0xbc: OP(y, R_ABSX, LOAD);
+
+		// STA
+		case 0x85: STORE(R_A, W_ZP);
+		case 0x95: STORE(R_A, W_ZPX);
+		case 0x8d: STORE(R_A, W_ABS);
+		case 0x9d: STORE(R_A, W_ABSX);
+		case 0x99: STORE(R_A, W_ABSY);
+		case 0x81: STORE(R_A, W_INDX);
+		case 0x91: STORE(R_A, W_INDY);
+
+		// STX
+		case 0x86: STORE(R_X, W_ZP);
+		case 0x96: STORE(R_X, W_ZPY);
+		case 0x8e: STORE(R_X, W_ABS);
+
+		// STY
+		case 0x84: STORE(R_Y, W_ZP);
+		case 0x94: STORE(R_Y, W_ZPX);
+		case 0x8c: STORE(R_Y, W_ABS);
+
+		// ADC
+		case 0x69: OP(a, R_IMM, ADC);
+		case 0x65: OP(a, R_ZP, ADC);
+		case 0x75: OP(a, R_ZPX, ADC);
+		case 0x6d: OP(a, R_ABS, ADC);
+		case 0x7d: OP(a, R_ABSX, ADC);
+		case 0x79: OP(a, R_ABSY, ADC);
+		case 0x61: OP(a, R_INDX, ADC);
+		case 0x71: OP(a, R_INDY, ADC);
+
+		// SBC
+		case 0xe9: OP(a, R_IMM, SBC);
+		case 0xe5: OP(a, R_ZP, SBC);
+		case 0xf5: OP(a, R_ZPX, SBC);
+		case 0xed: OP(a, R_ABS, SBC);
+		case 0xfd: OP(a, R_ABSX, SBC);
+		case 0xf9: OP(a, R_ABSY, SBC);
+		case 0xe1: OP(a, R_INDX, SBC);
+		case 0xf1: OP(a, R_INDY, SBC);
+
+		// AND
+		case 0x29: OP(a, R_IMM, AND);
+		case 0x25: OP(a, R_ZP, AND);
+		case 0x35: OP(a, R_ZPX, AND);
+		case 0x2d: OP(a, R_ABS, AND);
+		case 0x3d: OP(a, R_ABSX, AND);
+		case 0x39: OP(a, R_ABSY, AND);
+		case 0x21: OP(a, R_INDX, AND);
+		case 0x31: OP(a, R_INDY, AND);
+
+		// EOR
+		case 0x49: OP(a, R_IMM, EOR);
+		case 0x45: OP(a, R_ZP, EOR);
+		case 0x55: OP(a, R_ZPX, EOR);
+		case 0x4d: OP(a, R_ABS, EOR);
+		case 0x5d: OP(a, R_ABSX, EOR);
+		case 0x59: OP(a, R_ABSY, EOR);
+		case 0x41: OP(a, R_INDX, EOR);
+		case 0x51: OP(a, R_INDY, EOR);
+
+		// ORA
+		case 0x09: OP(a, R_IMM, ORA);
+		case 0x05: OP(a, R_ZP, ORA);
+		case 0x15: OP(a, R_ZPX, ORA);
+		case 0x0d: OP(a, R_ABS, ORA);
+		case 0x1d: OP(a, R_ABSX, ORA);
+		case 0x19: OP(a, R_ABSY, ORA);
+		case 0x01: OP(a, R_INDX, ORA);
+		case 0x11: OP(a, R_INDY, ORA);
+
+		// CMP
+		case 0xc9: CMP(a, R_IMM);
+		case 0xc5: CMP(a, R_ZP);
+		case 0xd5: CMP(a, R_ZPX);
+		case 0xcd: CMP(a, R_ABS);
+		case 0xdd: CMP(a, R_ABSX);
+		case 0xd9: CMP(a, R_ABSY);
+		case 0xc1: CMP(a, R_INDX);
+		case 0xd1: CMP(a, R_INDY);
+
+		// CPX
+		case 0xe0: CMP(x, R_IMM);
+		case 0xe4: CMP(x, R_ZP);
+		case 0xec: CMP(x, R_ABS);
+
+		// CPY
+		case 0xc0: CMP(y, R_IMM);
+		case 0xc4: CMP(y, R_ZP);
+		case 0xcc: CMP(y, R_ABS);
+
+		// JMP
+		case 0x4c:
+			do {
+				uint8_t a;
+				R_IMM();
+				uint8_t pch = bus_read(cpu->pc, cycles);
+				cycles++;
+				cpu->pc = a;
+				cpu->pc |= pch << 8;
+			} while (0);
+			break;
+
+		case 0x6c:
+			do {
+				uint8_t a;
+				R_IMM();
+				uint16_t ea = a | (bus_read(cpu->pc, cycles) << 8);
+				cycles++;
+				cpu->pc = bus_read(ea, cycles);
+				cycles++;
+				uint8_t ea2 = (ea & 0xff) + 1;
+				cpu->pc |= bus_read((ea & 0xff00) | ea2, cycles) << 8;
+				cycles++;
+			} while (0);
+			break;
+
+		// JSR
+		case 0x20:
+			do {
+				uint8_t a;
+				R_IMM();
+				bus_read(0x100 + cpu->sp, cycles);
+				cycles++;
+				STACK_PUSH((cpu->pc & 0xff00) >> 8); \
+				STACK_PUSH(cpu->pc & 0xff); \
+				uint16_t ea = a | (bus_read(cpu->pc, cycles) << 8);
+				cycles++;
+				cpu->pc = ea;
+			} while (0);
+			break;
+
+		// RTI
+		case 0x40:
+			do {
+				cpu->sp++;
+				cycles++;
+				cpu->flags = (cpu->flags & ~0b11001111) | (bus_read(0x100 + cpu->sp, cycles) & 0b11001111);
+				cpu->sp++;
+				cycles++;
+				cpu->pc = bus_read(0x100 + cpu->sp, cycles);
+				cpu->sp++;
+				cycles++;
+				cpu->pc |= bus_read(0x100 + cpu->sp, cycles) << 8;
+				cycles++;
+			} while(0);
+			break;
+
+		// RTS
+		case 0x60:
+			do {
+				cpu->sp++;
+				cycles++;
+				cpu->pc = bus_read(0x100 + cpu->sp, cycles);
+				cpu->sp++;
+				cycles++;
+				cpu->pc |= bus_read(0x100 + cpu->sp, cycles) << 8;
+				cycles++;
+				cpu->pc++;
+				cycles++;
+
+			} while(0);
+			break;
+
+		// Branches
+		case 0x50: B(V, 0);
+		case 0x70: B(V, 1);
+		case 0x10: B(N, 0);
+		case 0x30: B(N, 1);
+		case 0xd0: B(Z, 0);
+		case 0xf0: B(Z, 1);
+		case 0x90: B(C, 0);
+		case 0xb0: B(C, 1);
+
+		// BRK
+		case 0x00:
+			cpu->pc++;
+			FLAG_SET(B, 1);
+			FLAG_SET(U, 1);
+			PUSH_STATE();
+			FLAG_SET(I, 1);
+			FETCH_PC(0xfffe);
+			break;
+
+		// Flag manipulation
+		case 0x18: FLAG_SET(C, 0); break;
+		case 0x38: FLAG_SET(C, 1); break;
+		case 0x58: FLAG_SET(I, 0); break;
+		case 0x78: FLAG_SET(I, 1); break;
+		case 0xd8: FLAG_SET(D, 0); break;
+		case 0xf8: FLAG_SET(D, 1); break;
+		case 0xb8: FLAG_SET(V, 0); break;
+
+		// ASL
+		case 0x0a: RMW(R_A, ASL, W_A);
+		case 0x06: RMW(R_ZP, ASL, W_EA);
+		case 0x16: RMW(R_ZPX, ASL, W_EA);
+		case 0x0e: RMW(R_ABS, ASL, W_EA);
+		case 0x1e: RMW(R_ABSX, ASL, W_EA);
+
+		// LSR
+		case 0x4a: RMW(R_A, LSR, W_A);
+		case 0x46: RMW(R_ZP, LSR, W_EA);
+		case 0x56: RMW(R_ZPX, LSR, W_EA);
+		case 0x4e: RMW(R_ABS, LSR, W_EA);
+		case 0x5e: RMW(R_ABSX, LSR, W_EA);
+
+		// ROL
+		case 0x2a: RMW(R_A, ROL, W_A);
+		case 0x26: RMW(R_ZP, ROL, W_EA);
+		case 0x36: RMW(R_ZPX, ROL, W_EA);
+		case 0x2e: RMW(R_ABS, ROL, W_EA);
+		case 0x3e: RMW(R_ABSX, ROL, W_EA);
+
+		// ROR
+		case 0x6a: RMW(R_A, ROR, W_A);
+		case 0x66: RMW(R_ZP, ROR, W_EA);
+		case 0x76: RMW(R_ZPX, ROR, W_EA);
+		case 0x6e: RMW(R_ABS, ROR, W_EA);
+		case 0x7e: RMW(R_ABSX, ROR, W_EA);
+
+		// Increment / decrement
+		case 0xe6: RMW(R_ZP, INC, W_EA);
+		case 0xf6: RMW(R_ZPX, INC, W_EA);
+		case 0xee: RMW(R_ABS, INC, W_EA);
+		case 0xfe: RMW(R_ABSX, INC, W_EA);
+		case 0xe8: RMW(R_X, INC, W_X);
+		case 0xc8: RMW(R_Y, INC, W_Y);
+
+		case 0xc6: RMW(R_ZP, DEC, W_EA);
+		case 0xd6: RMW(R_ZPX, DEC, W_EA);
+		case 0xce: RMW(R_ABS, DEC, W_EA);
+		case 0xde: RMW(R_ABSX, DEC, W_EA);
+		case 0xca: RMW(R_X, DEC, W_X);
+		case 0x88: RMW(R_Y, DEC, W_Y);
+
+		// Stack operations
+		case 0x48: STACK_PUSH(cpu->a); break;
+		case 0x08: STACK_PUSH(cpu->flags | 0b00110000); break;
+		case 0x68: 
+			STACK_PULL(a, 0xff);
+			FLAG_SET(N, (cpu->a & 0x80) >> 7);
+			FLAG_SET(Z, (cpu->a == 0) ? 1 : 0);
+			break;
+		case 0x28: STACK_PULL(flags, 0b11001111); break;
+
+		// Transfers
+		case 0xaa:
+			cpu->x = cpu->a;
+			FLAG_SET(N, (cpu->a & 0x80) >> 7);
+			FLAG_SET(Z, (cpu->a == 0) ? 1 : 0);
+			break;
+		case 0xa8:
+			cpu->y = cpu->a;
+			FLAG_SET(N, (cpu->a & 0x80) >> 7);
+			FLAG_SET(Z, (cpu->a == 0) ? 1 : 0);
+			break;
+		case 0x8a:
+			cpu->a = cpu->x;
+			FLAG_SET(N, (cpu->a & 0x80) >> 7);
+			FLAG_SET(Z, (cpu->a == 0) ? 1 : 0);
+			break;
+		case 0x98:
+			cpu->a = cpu->y;
+			FLAG_SET(N, (cpu->a & 0x80) >> 7);
+			FLAG_SET(Z, (cpu->a == 0) ? 1 : 0);
+			break;
+		case 0xba:
+			cpu->x = cpu->sp;
+			FLAG_SET(N, (cpu->x & 0x80) >> 7);
+			FLAG_SET(Z, (cpu->x == 0) ? 1 : 0);
+			break;
+
+		case 0x9a: cpu->sp = cpu->x; break;
+
+		// NOP
+		case 0xea: break;
+
+		// BIT
+		case 0x24: BIT(R_ZP);
+		case 0x2C: BIT(R_ABS);
+
+		// Jam on everything else
+		default:
+			cpu->jammed = true;
+			break;
+	}
+
+	return cycles;
+}
diff --git a/src/6502.h b/src/6502.h
new file mode 100644
index 0000000..1333623
--- /dev/null
+++ b/src/6502.h
@@ -0,0 +1,38 @@
+#ifndef MOS6502_H
+#define MOS6502_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#define FLAG_C_BIT 0
+#define FLAG_Z_BIT 1
+#define FLAG_I_BIT 2
+#define FLAG_D_BIT 3
+#define FLAG_B_BIT 4
+#define FLAG_U_BIT 5
+#define FLAG_N_BIT 6
+#define FLAG_V_BIT 7
+
+#define FLAG_GET(f) ((cpu->flags & (1 << FLAG_##f##_BIT)) >> FLAG_##f##_BIT)
+#define FLAG_SET(f, v) (cpu->flags = (cpu->flags & ~(1 << FLAG_##f##_BIT)) | ((v) << FLAG_##f##_BIT))
+
+extern uint8_t bus_read(uint16_t addr, uint8_t cycle);
+extern void bus_write(uint16_t addr, uint8_t v, uint8_t cycle);
+
+typedef struct cpu_s {
+	uint16_t pc;
+	uint8_t a;
+	uint8_t x;
+	uint8_t y;
+	uint8_t sp;
+	uint8_t flags;
+
+	bool irq;
+	bool nmi;
+	bool jammed;
+} cpu_t;
+
+void cpu_reset(cpu_t *cpu);
+uint8_t cpu_step(cpu_t *cpu);
+
+#endif
\ No newline at end of file
diff --git a/src/6522.c b/src/6522.c
new file mode 100644
index 0000000..89bc102
--- /dev/null
+++ b/src/6522.c
@@ -0,0 +1,187 @@
+#include "6522.h"
+
+void via_init(via_t *via) {
+	via->ira = 0;
+	via->irb = 0;
+	via->ora = 0;
+	via->orb = 0;
+	via->ddra = 0;
+	via->ddrb = 0;
+	via->tca = 0;
+	via->tcb = 0;
+	via->tla = 0;
+	via->tlb = 0;
+	via->acr = 0;
+	via->pcr = 0;
+	via->ifr = 0;
+	via->ier = 0;
+
+	via->tav = true;
+	via->tbv = true;
+}
+
+void 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;
+
+	switch (addr) {
+		case 0:
+			via_pb_read(via, cycle);
+			*v = (via->irb & ~via->ddrb) | (via->orb & via->ddrb);
+			via->ifr &= 0b11100111;
+			if (via->ifr == 0x80) via->ifr = 0;
+			break;
+		case 1:
+			via_pa_read(via, cycle);
+			*v = via->ira;
+			via->ifr &= 0b11111100;
+			if (via->ifr == 0x80) via->ifr = 0;
+			break;
+		case 2:
+			*v = via->ddrb;
+			break;
+		case 3:
+			*v = via->ddra;
+			break;
+		case 4:
+			*v = tca & 0xff;
+			via->ifr &= 0b10111111;
+			if (via->ifr == 0x80) via->ifr = 0;
+			break;
+		case 5:
+			*v = (tca & 0xff00) >> 8;
+			break;
+		case 6:
+			*v = via->tla & 0xff;
+			break;
+		case 7:
+			*v = (via->tla & 0xff00) >> 8;
+			break;
+		case 8:
+			*v = tcb & 0xff;
+			via->ifr &= 0b11011111;
+			if (via->ifr == 0x80) via->ifr = 0;
+			break;
+		case 9:
+			*v = (tcb & 0xff00) >> 8;
+		case 11:
+			*v = via->acr;
+			break;
+		case 12:
+			*v = via->pcr;
+			break;
+		case 13:
+			*v = via->ifr;
+			break;
+		case 14:
+			*v = via->ier | 0x80;
+			break;
+		default:
+			break;
+	}
+}
+
+void via_write(via_t *via, uint8_t addr, uint8_t v, uint8_t cycle) {
+	switch (addr) {
+		case 0:
+			via->orb = v;
+			via_pb_write(via, cycle);
+			via->ifr &= 0b11100111;
+			if (via->ifr == 0x80) via->ifr = 0;
+			break;
+		case 1:
+			via->ora = v;
+			via_pa_write(via, cycle);
+			via->ifr &= 0b11111100;
+			if (via->ifr == 0x80) via->ifr = 0;
+			break;
+		case 2:
+			via->ddrb = v;
+			via_pb_write(via, cycle);
+			break;
+		case 3:
+			via->ddra = v;
+			via_pa_write(via, cycle);
+			break;
+		case 4:
+			via->tla = (via->tla & 0xff00) | v;
+			break;
+		case 5:
+			via->tla = (via->tla & 0xff) | (v << 8);
+			via->tca = via->tla + cycle + 1;
+			via->ifr &= 0b10111111;
+			if (via->ifr == 0x80) via->ifr = 0;
+			via->tav = false;
+			break;
+		case 6:
+			via->tla = (via->tla & 0xff00) | v;
+			break;
+		case 7:
+			via->tla = (via->tla & 0xff) | (v << 8);
+			via->ifr &= 0b10111111;
+			if (via->ifr == 0x80) via->ifr = 0;
+			break;
+		case 8:
+			via->tlb = (via->tlb & 0xff00) | v;
+			break;
+		case 9:
+			via->tlb = (via->tlb & 0xff) | (v << 8);
+			via->tcb = via->tlb + cycle + 1;
+			via->tbv = false;
+			via->ifr &= 0b11011111;
+			if (via->ifr == 0x80) via->ifr = 0;
+			break;
+		case 11:
+			via->acr = v;
+			break;
+		case 12:
+			via->pcr = v;
+			break;
+		case 13:
+			via->ifr &= ~(v & 0x7f);
+			if (via->ifr == 0x80) via->ifr = 0;
+			break;
+		case 14:
+			if (v & 0x80) {
+				via->ier |= v & 0x7f;
+			} else {
+				via->ier &= ~v;
+			}
+			break;
+		default:
+			break;
+	}
+}
+
+void via_cycle(via_t *via, uint8_t cycles) {
+	if (!via->tav) {
+		uint16_t tca = via->tca - cycles;
+
+		if (tca > via->tca) {
+			via->ifr |= 0b11000000;
+			if ((via->acr & 0b01000000) == 0) {
+				tca = via->tla;
+				via->tav = true;
+			} else {
+				tca = via->tla + tca + 1;
+			}
+		}
+
+		via->tca = tca;
+	}
+
+	uint16_t tcb = via->tcb - cycles;
+
+	if (tcb > via->tcb) {
+		if (!via->tbv) {
+			via->ifr |= 0b10100000;
+		}
+		via->tbv = true;
+	}
+
+	via->tcb = tcb;
+}
+
+bool via_irq(via_t *via) {
+	return (via->ifr & via->ier) > 0;
+}
diff --git a/src/6522.h b/src/6522.h
new file mode 100644
index 0000000..5582ba3
--- /dev/null
+++ b/src/6522.h
@@ -0,0 +1,40 @@
+#ifndef MOS6522_H
+#define MOS6522_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+typedef struct via_s {
+	uint8_t ira;
+	uint8_t irb;
+	uint8_t ora;
+	uint8_t orb;
+	uint8_t ddra;
+	uint8_t ddrb;
+	uint16_t tca;
+	uint16_t tcb;
+	uint16_t tla;
+	uint16_t tlb;
+	uint8_t acr;
+	uint8_t pcr;
+	uint8_t ifr;
+	uint8_t ier;
+	uint8_t latch_a;
+	uint8_t latch_b;
+
+	bool tav;
+	bool tbv;
+} via_t;
+
+extern void via_pa_read(via_t *via, uint8_t cycle);
+extern void via_pb_read(via_t *via, uint8_t cycle);
+extern void via_pa_write(via_t *via, uint8_t cycle);
+extern void via_pb_write(via_t *via, uint8_t cycle);
+
+void via_init(via_t *via);
+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 via_cycle(via_t *via, uint8_t cycles);
+bool via_irq(via_t *via);
+
+#endif
\ No newline at end of file
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;
+}
diff --git a/src/drive.h b/src/drive.h
new file mode 100644
index 0000000..b9fb1f2
--- /dev/null
+++ b/src/drive.h
@@ -0,0 +1,47 @@
+#ifndef DRIVE_H
+#define DRIVE_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define DRIVE_LOOKAHEAD 16
+
+extern const uint8_t SECTORS_PER_TRACK[40];
+extern const uint8_t BITRATE_PER_TRACK[40];
+extern const uint8_t GCR_CONV[16];
+
+typedef struct drive_s {
+	bool spinning;
+	uint8_t timer;
+	uint8_t *image;
+
+	uint8_t track;
+	uint8_t halftrack;
+	uint32_t image_ptr;
+	uint8_t sector;
+	uint8_t byte;
+
+	uint16_t gcr_word;
+	uint8_t gcr_bits;
+	uint8_t bit_counter;
+
+	uint8_t gap_remain;
+	uint8_t sync_remain;
+	uint8_t header_remain;
+	uint16_t data_remain;
+	uint8_t data_checksum;
+
+	uint8_t current_idx;
+	uint8_t lookahead_idx;
+	uint16_t unlatched[DRIVE_LOOKAHEAD];
+	uint16_t latched[DRIVE_LOOKAHEAD];
+	bool sync[DRIVE_LOOKAHEAD];
+	bool byte_ready[DRIVE_LOOKAHEAD];
+} drive_t;
+
+void drive_init(drive_t *drive);
+void drive_step_up(drive_t *drive);
+void drive_step_down(drive_t *drive);
+bool drive_cycle(drive_t *drive, uint8_t cycles, bool advance);
+
+#endif
\ No newline at end of file
diff --git a/src/pico1541.c b/src/pico1541.c
new file mode 100644
index 0000000..5c16e0a
--- /dev/null
+++ b/src/pico1541.c
@@ -0,0 +1,195 @@
+#include <hardware/clocks.h>
+#include <hardware/gpio.h>
+#include <hardware/timer.h>
+
+#include "6502.h"
+#include "6522.h"
+#include "drive.h"
+
+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_LED = 25;
+
+const uint8_t DEVICE = 8;
+
+static uint8_t ram[2048];
+extern uint8_t _binary__Users_max_Documents_c64_roms_dos1541_start[16384];
+extern uint8_t _binary__Users_max_Downloads_blackbird_1_2_Blackbird_1_2_d64_start[174848];
+// extern uint8_t _binary__Users_max_Downloads_3sira_still_human_by_arise_3sira_by_arise_A_d64_start[174848];
+
+static uint32_t ts;
+static cpu_t cpu;
+static via_t via1;
+static via_t via2;
+static drive_t drive;
+
+int8_t track_step;
+
+void via_pa_read(via_t *via, uint8_t cycle) {
+	if (via == &via2) {
+		via->ira = drive.latched[(drive.current_idx + cycle) & (DRIVE_LOOKAHEAD - 1)];
+	} else {
+		via->ira = via->ora;
+	}
+}
+
+void 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;
+		uint8_t data = gpio_get(GPIO_DATA) ? 0 : 0b00000001;
+		uint8_t atn = gpio_get(GPIO_ATN) ? 0 : 0b10000000;
+		uint8_t dev = (DEVICE - 8) << 5;
+		uint8_t atna = via->orb & 0b00010000;
+
+		via->irb = clk | data | atn | dev | atna;
+	} else {
+		uint8_t low = via->orb & 0b00001111;
+		uint8_t sync = drive.sync[(drive.current_idx + cycle) & (DRIVE_LOOKAHEAD - 1)] ? 0 : 0b10000000;
+
+		via->irb = low | sync | 0b00010000;
+	}
+}
+
+void via_pa_write(via_t *via, uint8_t cycle) {
+}
+
+void 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);
+		gpio_set_dir(GPIO_DATA, (via->orb & 0b00000010) > 0 && (via->ddrb & 0b00000010) > 0);
+		if ((via->orb & 0b00010000) && (via->ddrb & 0b00010000)) {
+			gpio_set_dir(GPIO_DATA2, gpio_get(GPIO_ATN));
+		} else {
+			gpio_set_dir(GPIO_DATA2, !gpio_get(GPIO_ATN));
+		}
+	} else if (via == &via2) {
+		if ((via->ddrb & 3) == 3) {
+			int8_t step = via->orb & 3;
+			if (step - track_step > 0) {
+				drive_step_up(&drive);
+			} else if (step - track_step < 0) {
+				drive_step_down(&drive);
+			}
+			track_step = step;
+		}
+
+		drive.spinning = ((via->ddrb & via->orb) & 4) > 0;
+
+		gpio_put(GPIO_LED, (via->orb & 0b00001000) > 0 && (via->ddrb & 0b00001000) > 0);
+	}
+}
+
+uint8_t bus_read(uint16_t addr, uint8_t cycle) {
+	if (addr < 2048) {
+		return ram[addr];
+	} else if (addr >= 0xc000) {
+		return _binary__Users_max_Documents_c64_roms_dos1541_start[addr - 0xc000];
+	} else if (addr >= 0x1800 && addr < 0x1810) {
+		uint8_t v = 0;
+		via_read(&via1, addr - 0x1800, &v, cycle);
+		return v;
+	} else if (addr >= 0x1c00 && addr < 0x1c10) {
+		uint8_t v = 0;
+		via_read(&via2, addr - 0x1c00, &v, cycle);
+		return v;
+	} else {
+		return 0;
+	}
+}
+
+void bus_write(uint16_t addr, uint8_t v, uint8_t cycle) {
+	if (addr < 2048) {
+		ram[addr] = v;
+	} else if (addr >= 0x1800 && addr < 0x1810) {
+		via_write(&via1, addr - 0x1800, v, cycle);
+	} else if (addr >= 0x1c00 && addr < 0x1c10) {
+		via_write(&via2, addr - 0x1c00, v, cycle);
+	}
+}
+
+void atn_irq_callback(uint pin, uint32_t flags) {
+	if (flags & GPIO_IRQ_EDGE_RISE) {
+		if (!(via1.pcr & 1)) {
+			via1.ifr |= 0b10000010;
+		}
+		if ((via1.orb & 0b00010000) && (via1.ddrb & 0b00010000)) {
+			gpio_set_dir(GPIO_DATA2, 1);
+		} else {
+			gpio_set_dir(GPIO_DATA2, 0);
+		}
+	} else {
+		if (via1.pcr & 1) {
+			via1.ifr |= 0b10000010;
+		}
+		if ((via1.orb & 0b00010000) && (via1.ddrb & 0b00010000)) {
+			gpio_set_dir(GPIO_DATA2, 0);
+		} else {
+			gpio_set_dir(GPIO_DATA2, 1);
+		}
+	}
+}
+
+int main() {
+	set_sys_clock_khz(250000, true);
+
+	gpio_init(GPIO_CLK);
+	gpio_init(GPIO_DATA);
+	gpio_init(GPIO_ATN);
+	gpio_init(GPIO_DATA2);
+	gpio_init(25);
+
+	gpio_put(GPIO_CLK, 0);
+	gpio_put(GPIO_DATA, 0);
+	gpio_put(GPIO_LED, 0);
+	gpio_put(GPIO_DATA2, 0);
+
+	gpio_set_dir(GPIO_CLK, GPIO_IN);
+	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_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_irq_enabled_with_callback(GPIO_ATN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &atn_irq_callback);
+
+	track_step = 0;
+	drive_init(&drive);
+	drive.image = _binary__Users_max_Downloads_blackbird_1_2_Blackbird_1_2_d64_start;
+
+	cpu_reset(&cpu);
+	via_init(&via1);
+	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_cycle(&drive, cycles, true) && (via2.pcr & 0b1110) == 0b1110) {
+			cpu.flags |= 1 << FLAG_V_BIT;
+		}
+
+		ts += cycles;
+		while (ts > time_us_32()) {}
+	}
+
+	gpio_put(GPIO_LED, 1);
+
+	return 0;
+}