diff --git a/atusb/fw/flash.c b/atusb/fw/flash.c
index fd426f3..cd29b5b 100644
--- a/atusb/fw/flash.c
+++ b/atusb/fw/flash.c
@@ -13,30 +13,55 @@
 
 #include <stdint.h>
 
-#include <avr/io.h>
-#include <avr/interrupt.h>
-
-#define F_CPU   8000000UL
-#include <util/delay.h>
+#include <avr/boot.h>
 
 #include "dfu.h"
 #include "board.h"
 
 
+static uint32_t payload;
+
 
 void flash_start(void)
 {
+	payload = 0;
 }
 
 
 int flash_can_write(uint16_t size)
 {
-	return 0;
+	return 1;
 }
 
 
 void flash_write(const uint8_t *buf, uint16_t size)
 {
+	static uint8_t last;
+        const uint8_t *p;
+
+	eeprom_busy_wait();
+	for (p = buf; p != buf+size; p++) {
+		if (!(payload & (SPM_PAGESIZE-1))) {
+			if (payload) {
+				boot_page_write(payload-SPM_PAGESIZE);
+				boot_spm_busy_wait();
+			}
+			boot_page_erase(payload);
+			boot_spm_busy_wait();
+		}
+		if (payload & 1)
+			boot_page_fill(payload, last | (*p << 0));
+		else
+			last = *p;
+		payload++;
+        }
+
+	if (!(payload & (SPM_PAGESIZE-1))) {
+		boot_page_write(payload);
+		boot_spm_busy_wait();
+	}
+
+	boot_rww_enable();
 }