diff --git a/firmware/Test/Makefile b/firmware/Test/Makefile
new file mode 100644
index 0000000..19b2534
--- /dev/null
+++ b/firmware/Test/Makefile
@@ -0,0 +1,27 @@
+BOARD=rp2040:rp2040:rpipico2
+#OPTS=PSRAM=enabled,FlashSize=4M,PartitionScheme=min_spiffs,CPUFreq=240
+SKETCH=Test
+PORT?=/dev/ttyACM0
+
+PIOASM=~/.arduino15/packages/rp2040/tools/pqt-pioasm/4.0.1-8ec9d6f/pioasm
+
+
+
+SUBDIR=$(subst :,.,$(BOARD))
+BIN=bin/$(SKETCH).ino.bin
+#IP=$(shell avahi-resolve -n $(HOST).local | cut -f2 -d'	')
+
+$(BIN): $(SKETCH).ino datastream.h
+	arduino-cli compile --fqbn $(BOARD) --board-options "freq=300,arch=riscv" --output-dir bin --build-path build
+
+upload: $(BIN)
+	arduino-cli upload --fqbn $(BOARD) -p $(PORT) --input-dir bin
+
+clean:
+	rm -rf build bin
+
+watch:
+	echo ${SKETCH}.ino | entr -c -s 'make'
+
+datastream.h: datastream.pio
+	${PIOASM} datastream.pio datastream.h
diff --git a/firmware/Test/Test.ino b/firmware/Test/Test.ino
new file mode 100644
index 0000000..0fd20bf
--- /dev/null
+++ b/firmware/Test/Test.ino
@@ -0,0 +1,173 @@
+/*
+ * PIO
+ *
+ *
+ * Fifo format
+ * 19 bits of address
+ * 8 bits of current data value
+ * 3 bits of head number
+ * 2 bit of pin data
+ *
+ * Shift 1 bit from OSR into pins
+ * Shift 11 bits from OSR into ISR
+ * Shift 3 bits from pins into ISR
+ *
+ * PULL block
+ * OUT pins 1
+ * OUT ISR 11
+ * IN pin 3
+ * PUSH noblock
+ *
+ * Software:
+ * Write to fifo data, head and current bit
+ * Read from fifo.
+ * Test write bit
+ * If set, set or clear affected bit
+ * Shift data to 8 bits and replace in buffer
+ */
+
+#include "datastream.h"
+
+static PIOProgram datastreamPgm(&datastream_program);
+
+PIO pio;
+int sm;
+int offset;
+
+uint16_t header_crc;
+
+#define POLY16 0x1021
+#define POLY32 0xa00805
+
+static inline void w() {
+	while(pio_sm_is_tx_fifo_full(pio, sm));
+}
+
+void bindump(uint16_t v) {
+	for (int i = 0; i < 16; i++) {
+		Serial.print((v & 0x8000) ? "1" : "0");
+		v <<= 1;
+	}
+}
+
+uint16_t crc16(uint8_t val, uint16_t crc)
+{
+
+	uint16_t xval = val;
+
+   int j;
+   crc = crc ^ (xval << 8);
+   for (j = 1; j <= 8; j++) {   // Assuming 8 bits per val
+      if (crc & 0x8000) {   // if leftmost (most significant) bit is set
+         crc = (crc << 1) ^ POLY16;
+      } else {
+         crc = crc << 1;
+      }
+   }
+   return crc;
+}
+
+uint32_t crc32(uint8_t val, uint32_t crc)
+{
+
+   int j;
+   crc = crc ^ (val << 24);
+   for (j = 1; j <= 8; j++) {   // Assuming 8 bits per val
+      if (crc & 0x80000000) {   // if leftmost (most significant) bit is set
+         crc = (crc << 1) ^ POLY32;
+      } else {
+         crc = crc << 1;
+      }
+   }
+   return crc;
+}
+
+
+uint8_t last_bit = 0;
+uint16_t mfm_encode_bit(uint8_t b) {
+
+
+	if (b == 0x80) {
+		last_bit = 1;
+		return 0b1000000000000000;
+	}
+
+
+	if (last_bit == 0) {
+		return 0b0100000000000000;
+	} else {
+		last_bit = 0;
+		return 0b00;
+	}
+}
+
+uint16_t mfm_encode(uint8_t b) {
+
+	uint16_t out = 0;
+
+	//Serial.print(">>> ");
+	//Serial.println(b, HEX);
+
+	for (int i = 0; i < 8; i++) {
+		out >>= 2;
+		//Serial.println(b, BIN);
+		out |= mfm_encode_bit(b & 0x80);
+		b <<= 1;
+	}
+
+	//Serial.print("Last bit: ");
+	//Serial.println(last_bit);
+
+	//Serial.print("Output: ");
+	//Serial.println(out, BIN);
+
+	return out;
+}
+
+void csend(uint8_t v) {
+	w();
+	pio->txf[sm] = mfm_encode(v);
+	header_crc = crc16(v, header_crc);
+}
+
+void setup() {
+    datastreamPgm.prepare(&pio, &sm, &offset);
+    datastream_program_init(pio, sm, offset);
+    pio_sm_set_enabled(pio, sm, true);
+
+	pio->txf[sm] = mfm_encode(0x00);
+
+	pinMode(13, OUTPUT);
+	digitalWrite(13, LOW);
+	Serial.begin(115200);
+}
+
+void loop() {
+	delay(1);
+
+	digitalWrite(13, HIGH);
+	digitalWrite(13, LOW);
+
+	last_bit = 0;
+	w();
+	pio->txf[sm] = mfm_encode(0x00);
+
+	header_crc = 0xFFFF;
+	header_crc = crc16(0xA1, header_crc);
+	uint16_t sync = mfm_encode(0xA1);
+	sync &= 0b1111101111111111;
+
+	pio->txf[sm] = sync;
+	csend(0xFE);
+	csend(0x00);
+	csend(0x00);
+	csend(0x03);
+	csend(0x02);
+	w();
+	pio->txf[sm] = mfm_encode(header_crc >> 8);
+	w();
+	pio->txf[sm] = mfm_encode(header_crc & 0xff);
+	w();
+	pio->txf[sm] = mfm_encode(0x00);
+
+}
diff --git a/firmware/Test/datastream.pio b/firmware/Test/datastream.pio
new file mode 100644
index 0000000..f48c6fb
--- /dev/null
+++ b/firmware/Test/datastream.pio
@@ -0,0 +1,41 @@
+.program datastream 
+
+public start:   
+
+	PULL block
+	MOV X, OSR
+
+.wrap_target
+	PULL noblock
+
+	SET Y,6
+loop:
+	OUT PINS,1 [14]
+	SET PINS,0 [14]
+	OUT PINS,1 [14]
+	SET PINS,0 [13]
+	JMP Y-- loop
+
+	OUT PINS,1 [14]
+	SET PINS,0 [14]
+	OUT PINS,1 [14]
+	SET PINS,0 [12]
+
+.wrap
+
+
+% c-sdk {
+static inline void datastream_program_init(PIO pio, uint sm, uint offset) {
+	pio_set_gpio_base(pio, 0);
+	pio_gpio_init(pio, 12);
+	pio_sm_set_consecutive_pindirs(pio, sm, 12, 1, true);
+	pio_sm_config c = datastream_program_get_default_config(offset);
+	sm_config_set_clkdiv(&c, 1.0);
+    sm_config_set_out_pin_base(&c, 12);
+    sm_config_set_out_pin_count(&c, 1);
+    sm_config_set_set_pin_base(&c, 12);
+    sm_config_set_set_pin_count(&c, 1);
+	pio_sm_init(pio, sm, offset, &c);
+}
+%}
+