[gnoduino: 158/237] Shrink code by using registers for variables "length" and "address" http://code.google.com/p/optiboo



commit 0bee48464bc0087df0ceb3e199cbbb84541fabf9
Author: WestfW <westfw gmail com>
Date:   Fri Jun 10 23:02:25 2011 -0700

    Shrink code by using registers for variables "length" and "address"
    http://code.google.com/p/optiboot/issues/detail?id=33
    
    Fix high-value watchdog timeouts on ATmega8
    http://code.google.com/p/optiboot/issues/detail?id=38
    
    Change "start app on bad commands" code to start the app via the
    watchdog timer, so that the app is always started with the chip
    in fully reset state.
    http://code.google.com/p/optiboot/issues/detail?id=37
    (cherry picked from commit 6f7687b0f925cb07447cbbcc692766c51c02f700)

 arduino/bootloaders/optiboot/optiboot.c            |   34 +-
 .../bootloaders/optiboot/optiboot_atmega328.hex    |   58 +-
 .../bootloaders/optiboot/optiboot_atmega328.lst    |  439 ++++++++--------
 .../optiboot/optiboot_atmega328_pro_8MHz.hex       |   58 +-
 .../optiboot/optiboot_atmega328_pro_8MHz.lst       |  439 ++++++++--------
 .../bootloaders/optiboot/optiboot_diecimila.hex    |   58 +-
 .../bootloaders/optiboot/optiboot_diecimila.lst    |  439 ++++++++--------
 arduino/bootloaders/optiboot/optiboot_lilypad.hex  |   58 +-
 arduino/bootloaders/optiboot/optiboot_lilypad.lst  |  439 ++++++++--------
 .../optiboot/optiboot_lilypad_resonator.hex        |   58 +-
 .../optiboot/optiboot_lilypad_resonator.lst        |  439 ++++++++--------
 arduino/bootloaders/optiboot/optiboot_luminet.hex  |   75 ++--
 arduino/bootloaders/optiboot/optiboot_luminet.lst  |  574 ++++++++++----------
 .../bootloaders/optiboot/optiboot_pro_16MHz.hex    |   58 +-
 .../bootloaders/optiboot/optiboot_pro_16MHz.lst    |  439 ++++++++--------
 .../bootloaders/optiboot/optiboot_pro_20mhz.hex    |   58 +-
 .../bootloaders/optiboot/optiboot_pro_20mhz.lst    |  439 ++++++++--------
 arduino/bootloaders/optiboot/optiboot_pro_8MHz.hex |   58 +-
 arduino/bootloaders/optiboot/optiboot_pro_8MHz.lst |  439 ++++++++--------
 19 files changed, 2344 insertions(+), 2315 deletions(-)
---
diff --git a/arduino/bootloaders/optiboot/optiboot.c b/arduino/bootloaders/optiboot/optiboot.c
index ece8b43..8940359 100644
--- a/arduino/bootloaders/optiboot/optiboot.c
+++ b/arduino/bootloaders/optiboot/optiboot.c
@@ -198,8 +198,8 @@ asm("  .section .version\n"
 #define WATCHDOG_1S     (_BV(WDP2) | _BV(WDP1) | _BV(WDE))
 #define WATCHDOG_2S     (_BV(WDP2) | _BV(WDP1) | _BV(WDP0) | _BV(WDE))
 #ifndef __AVR_ATmega8__
-#define WATCHDOG_4S     (_BV(WDE3) | _BV(WDE))
-#define WATCHDOG_8S     (_BV(WDE3) | _BV(WDE0) | _BV(WDE))
+#define WATCHDOG_4S     (_BV(WDP3) | _BV(WDE))
+#define WATCHDOG_8S     (_BV(WDP3) | _BV(WDP0) | _BV(WDE))
 #endif
 
 /* Function Prototypes */
@@ -244,8 +244,6 @@ void appStart() __attribute__ ((naked));
 /* These definitions are NOT zero initialised, but that doesn't matter */
 /* This allows us to drop the zero init code, saving us memory */
 #define buff    ((uint8_t*)(RAMSTART))
-#define address (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2))
-#define length  (*(uint8_t*)(RAMSTART+SPM_PAGESIZE*2+2))
 #ifdef VIRTUAL_BOOT_PARTITION
 #define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
 #define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
@@ -255,6 +253,13 @@ void appStart() __attribute__ ((naked));
 int main(void) {
   uint8_t ch;
 
+  /*
+   * Making these local and in registers prevents the need for initializing
+   * them, and also saves space because code no longer stores to memory.
+   */
+  register uint16_t address;
+  register uint8_t  length;
+
   // After the zero init loop, this is the first code to run.
   //
   // This code makes the following assumptions:
@@ -350,7 +355,9 @@ int main(void) {
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
-      getLen();
+      getch();			/* getlen() */
+      length = getch();
+      getch();
 
       // If we are in RWW section, immediately start page erase
       if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
@@ -415,7 +422,10 @@ int main(void) {
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
       // READ PAGE - we only read flash
-      getLen();
+      getch();			/* getlen() */
+      length = getch();
+      getch();
+
       verifySpace();
 #ifdef VIRTUAL_BOOT_PARTITION
       do {
@@ -575,7 +585,11 @@ void getNch(uint8_t count) {
 }
 
 void verifySpace() {
-  if (getch() != CRC_EOP) appStart();
+  if (getch() != CRC_EOP) {
+    watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
+    while (1)			      // and busy-loop so that WD causes
+      ;				      //  a reset and app start.
+  }
   putch(STK_INSYNC);
 }
 
@@ -595,12 +609,6 @@ void flash_led(uint8_t count) {
 }
 #endif
 
-uint8_t getLen() {
-  getch();
-  length = getch();
-  return getch();
-}
-
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
diff --git a/arduino/bootloaders/optiboot/optiboot_atmega328.hex b/arduino/bootloaders/optiboot/optiboot_atmega328.hex
index 7286e0d..11819d6 100644
--- a/arduino/bootloaders/optiboot/optiboot_atmega328.hex
+++ b/arduino/bootloaders/optiboot/optiboot_atmega328.hex
@@ -1,35 +1,33 @@
-:107E0000112484B714BE81FFE6D085E08093810001
+:107E0000112484B714BE81FFE3D085E08093810004
 :107E100082E08093C00088E18093C10086E0809377
-:107E2000C20080E18093C4008EE0CFD0259A86E026
+:107E2000C20080E18093C4008EE0BCD0259A86E039
 :107E300020E33CEF91E0309385002093840096BBD3
-:107E4000B09BFECF1D9AA8958150A9F7DD24D3944D
-:107E5000A5E0EA2EF1E1FF2EABD0813421F481E0E0
-:107E6000C5D083E020C0823411F484E103C085349E
-:107E700019F485E0BBD091C0853581F499D0082FE5
-:107E800010E096D090E0982F8827802B912B880FB8
-:107E9000991F90930102809300027EC0863529F4D9
-:107EA00084E0A4D080E07CD078C0843609F04EC055
-:107EB00087D0E0910002F091010280E7E030F807FE
-:107EC00018F483E087BFE895C0E0D1E071D08993D2
-:107ED000809102028150809302028823B9F7E091D9
-:107EE0000002F091010280E7E030F80718F083E02B
-:107EF00087BFE89575D007B600FCFDCF4091000222
-:107F000050910102A0E0B1E02C9130E011968C91EB
-:107F1000119790E0982F8827822B932B1296FA01C5
-:107F20000C01D7BEE89511244E5F5F4FF1E0A038F9
-:107F3000BF0751F7E0910002F0910102E7BEE8951A
-:107F400007B600FCFDCFF7BEE89527C08437B9F42B
-:107F500037D046D0E0910002F09101023196F093C3
-:107F60000102E09300023197E4918E2F19D08091A5
-:107F70000202815080930202882361F70EC0853788
-:107F800039F42ED08EE10CD085E90AD08FE08BCF6A
-:107F9000813511F488E019D023D080E101D05CCF85
-:107FA000982F8091C00085FFFCCF9093C600089564
-:107FB000A8958091C00087FFFCCF8091C6000895EE
-:107FC000F7DFF6DF80930202F3CFE0E6F0E098E11E
-:107FD00090838083089580E0F8DFEE27FF270994DF
-:107FE000E7DF803209F0F7DF84E1DACF1F93182F43
-:0C7FF000DFDF1150E9F7F4DF1F91089566
+:107E4000B09BFECF1D9AA8958150A9F799249394D1
+:107E5000A5E0AA2EF1E1BF2E9DD0813421F481E06E
+:107E6000AFD083E01FC0823411F484E103C08534B5
+:107E700019F485E0A5D083C0853579F48BD0E82E40
+:107E8000FF2488D0082F10E0102F00270E291F296B
+:107E9000000F111F8DD0680172C0863529F484E06F
+:107EA0008FD080E06FD06BC0843609F042C072D0B2
+:107EB00071D0082F6FD080E0C81680E7D80620F474
+:107EC00083E0F60187BFE895C0E0D1E063D08993F5
+:107ED0000C17E1F7F0E0CF16F0E7DF0620F083E0C3
+:107EE000F60187BFE89564D007B600FCFDCFA60178
+:107EF000A0E0B1E02C9130E011968C91119790E0C8
+:107F0000982F8827822B932B1296FA010C0197BE8B
+:107F1000E89511244E5F5F4FF1E0A038BF0751F79D
+:107F2000F601A7BEE89507B600FCFDCFB7BEE89501
+:107F300026C08437B1F42ED02DD0F82E2BD038D0D7
+:107F4000F601EF2C8F010F5F1F4F84911BD0EA9435
+:107F5000F801C1F70894C11CD11CFA94CF0CD11CB4
+:107F60000EC0853739F424D08EE10CD085E90AD0D3
+:107F70008FE098CF813511F488E014D019D080E1DA
+:107F800001D06ACF982F8091C00085FFFCCF9093DD
+:107F9000C6000895A8958091C00087FFFCCF80910E
+:107FA000C6000895E0E6F0E098E1908380830895AC
+:107FB000F1DF803219F088E0F5DFFFCF84E1E2CF16
+:107FC0001F93182FE7DF1150E9F7F2DF1F91089593
+:0A7FD00080E0E8DFEE27FF270994A8
 :027FFE0001047C
 :0400000300007E007B
 :00000001FF
diff --git a/arduino/bootloaders/optiboot/optiboot_atmega328.lst b/arduino/bootloaders/optiboot/optiboot_atmega328.lst
index 0da8aaf..7876b96 100644
--- a/arduino/bootloaders/optiboot/optiboot_atmega328.lst
+++ b/arduino/bootloaders/optiboot/optiboot_atmega328.lst
@@ -3,27 +3,27 @@ optiboot_atmega328.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001fc  00007e00  00007e00  00000054  2**1
+  0 .text         000001da  00007e00  00007e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00007ffe  00007ffe  00000250  2**0
+  1 .version      00000002  00007ffe  00007ffe  0000022e  2**0
                   CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000252  2**0
+  2 .debug_aranges 00000028  00000000  00000000  00000230  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000006a  00000000  00000000  0000027a  2**0
+  3 .debug_pubnames 0000005f  00000000  00000000  00000258  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000285  00000000  00000000  000002e4  2**0
+  4 .debug_info   0000028c  00000000  00000000  000002b7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000019f  00000000  00000000  00000569  2**0
+  5 .debug_abbrev 00000199  00000000  00000000  00000543  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000453  00000000  00000000  00000708  2**0
+  6 .debug_line   00000456  00000000  00000000  000006dc  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000090  00000000  00000000  00000b5c  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000b34  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000141  00000000  00000000  00000bec  2**0
+  8 .debug_str    00000149  00000000  00000000  00000bb4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    000001e1  00000000  00000000  00000d2d  2**0
+  9 .debug_loc    0000027e  00000000  00000000  00000cfd  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000068  00000000  00000000  00000f0e  2**0
+ 10 .debug_ranges 00000060  00000000  00000000  00000f7b  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
@@ -47,7 +47,7 @@ int main(void) {
     7e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
     7e06:	81 ff       	sbrs	r24, 1
-    7e08:	e6 d0       	rcall	.+460    	; 0x7fd6 <appStart>
+    7e08:	e3 d0       	rcall	.+454    	; 0x7fd0 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
@@ -77,7 +77,7 @@ int main(void) {
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
     7e28:	8e e0       	ldi	r24, 0x0E	; 14
-    7e2a:	cf d0       	rcall	.+414    	; 0x7fca <watchdogConfig>
+    7e2a:	bc d0       	rcall	.+376    	; 0x7fa4 <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
@@ -111,8 +111,8 @@ void flash_led(uint8_t count) {
 #else
     LED_PIN |= _BV(LED);
     7e44:	1d 9a       	sbi	0x03, 5	; 3
-  return getch();
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
@@ -132,8 +132,8 @@ void watchdogReset() {
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    7e4c:	dd 24       	eor	r13, r13
-    7e4e:	d3 94       	inc	r13
+    7e4c:	99 24       	eor	r9, r9
+    7e4e:	93 94       	inc	r9
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
@@ -141,21 +141,21 @@ void watchdogReset() {
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
     7e50:	a5 e0       	ldi	r26, 0x05	; 5
-    7e52:	ea 2e       	mov	r14, r26
+    7e52:	aa 2e       	mov	r10, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
     7e54:	f1 e1       	ldi	r31, 0x11	; 17
-    7e56:	ff 2e       	mov	r15, r31
+    7e56:	bf 2e       	mov	r11, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    7e58:	ab d0       	rcall	.+342    	; 0x7fb0 <getch>
+    7e58:	9d d0       	rcall	.+314    	; 0x7f94 <getch>
 
     if(ch == STK_GET_PARAMETER) {
     7e5a:	81 34       	cpi	r24, 0x41	; 65
@@ -163,10 +163,10 @@ void watchdogReset() {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
     7e5e:	81 e0       	ldi	r24, 0x01	; 1
-    7e60:	c5 d0       	rcall	.+394    	; 0x7fec <getNch>
+    7e60:	af d0       	rcall	.+350    	; 0x7fc0 <getNch>
       putch(0x03);
     7e62:	83 e0       	ldi	r24, 0x03	; 3
-    7e64:	20 c0       	rjmp	.+64     	; 0x7ea6 <main+0xa6>
+    7e64:	1f c0       	rjmp	.+62     	; 0x7ea4 <main+0xa4>
     }
     else if(ch == STK_SET_DEVICE) {
     7e66:	82 34       	cpi	r24, 0x42	; 66
@@ -182,71 +182,77 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     7e72:	85 e0       	ldi	r24, 0x05	; 5
-    7e74:	bb d0       	rcall	.+374    	; 0x7fec <getNch>
-    7e76:	91 c0       	rjmp	.+290    	; 0x7f9a <main+0x19a>
+    7e74:	a5 d0       	rcall	.+330    	; 0x7fc0 <getNch>
+    7e76:	83 c0       	rjmp	.+262    	; 0x7f7e <main+0x17e>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     7e78:	85 35       	cpi	r24, 0x55	; 85
-    7e7a:	81 f4       	brne	.+32     	; 0x7e9c <main+0x9c>
+    7e7a:	79 f4       	brne	.+30     	; 0x7e9a <main+0x9a>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    7e7c:	99 d0       	rcall	.+306    	; 0x7fb0 <getch>
+    7e7c:	8b d0       	rcall	.+278    	; 0x7f94 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-    7e7e:	08 2f       	mov	r16, r24
-    7e80:	10 e0       	ldi	r17, 0x00	; 0
-    7e82:	96 d0       	rcall	.+300    	; 0x7fb0 <getch>
-    7e84:	90 e0       	ldi	r25, 0x00	; 0
-    7e86:	98 2f       	mov	r25, r24
-    7e88:	88 27       	eor	r24, r24
-    7e8a:	80 2b       	or	r24, r16
-    7e8c:	91 2b       	or	r25, r17
+    7e7e:	e8 2e       	mov	r14, r24
+    7e80:	ff 24       	eor	r15, r15
+    7e82:	88 d0       	rcall	.+272    	; 0x7f94 <getch>
+    7e84:	08 2f       	mov	r16, r24
+    7e86:	10 e0       	ldi	r17, 0x00	; 0
+    7e88:	10 2f       	mov	r17, r16
+    7e8a:	00 27       	eor	r16, r16
+    7e8c:	0e 29       	or	r16, r14
+    7e8e:	1f 29       	or	r17, r15
 #ifdef RAMPZ
       // Transfer top bit to RAMPZ
       RAMPZ = (newAddress & 0x8000) ? 1 : 0;
 #endif
       newAddress += newAddress; // Convert from word address to byte address
-    7e8e:	88 0f       	add	r24, r24
-    7e90:	99 1f       	adc	r25, r25
+    7e90:	00 0f       	add	r16, r16
+    7e92:	11 1f       	adc	r17, r17
       address = newAddress;
-    7e92:	90 93 01 02 	sts	0x0201, r25
-    7e96:	80 93 00 02 	sts	0x0200, r24
-    7e9a:	7e c0       	rjmp	.+252    	; 0x7f98 <main+0x198>
       verifySpace();
+    7e94:	8d d0       	rcall	.+282    	; 0x7fb0 <verifySpace>
+    7e96:	68 01       	movw	r12, r16
+    7e98:	72 c0       	rjmp	.+228    	; 0x7f7e <main+0x17e>
     }
     else if(ch == STK_UNIVERSAL) {
-    7e9c:	86 35       	cpi	r24, 0x56	; 86
-    7e9e:	29 f4       	brne	.+10     	; 0x7eaa <main+0xaa>
+    7e9a:	86 35       	cpi	r24, 0x56	; 86
+    7e9c:	29 f4       	brne	.+10     	; 0x7ea8 <main+0xa8>
       // UNIVERSAL command is ignored
       getNch(4);
-    7ea0:	84 e0       	ldi	r24, 0x04	; 4
-    7ea2:	a4 d0       	rcall	.+328    	; 0x7fec <getNch>
+    7e9e:	84 e0       	ldi	r24, 0x04	; 4
+    7ea0:	8f d0       	rcall	.+286    	; 0x7fc0 <getNch>
       putch(0x00);
-    7ea4:	80 e0       	ldi	r24, 0x00	; 0
-    7ea6:	7c d0       	rcall	.+248    	; 0x7fa0 <putch>
-    7ea8:	78 c0       	rjmp	.+240    	; 0x7f9a <main+0x19a>
+    7ea2:	80 e0       	ldi	r24, 0x00	; 0
+    7ea4:	6f d0       	rcall	.+222    	; 0x7f84 <putch>
+    7ea6:	6b c0       	rjmp	.+214    	; 0x7f7e <main+0x17e>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    7eaa:	84 36       	cpi	r24, 0x64	; 100
-    7eac:	09 f0       	breq	.+2      	; 0x7eb0 <main+0xb0>
-    7eae:	4e c0       	rjmp	.+156    	; 0x7f4c <main+0x14c>
+    7ea8:	84 36       	cpi	r24, 0x64	; 100
+    7eaa:	09 f0       	breq	.+2      	; 0x7eae <main+0xae>
+    7eac:	42 c0       	rjmp	.+132    	; 0x7f32 <main+0x132>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
-      getLen();
-    7eb0:	87 d0       	rcall	.+270    	; 0x7fc0 <getLen>
+      getch();			/* getlen() */
+    7eae:	72 d0       	rcall	.+228    	; 0x7f94 <getch>
+      length = getch();
+    7eb0:	71 d0       	rcall	.+226    	; 0x7f94 <getch>
+    7eb2:	08 2f       	mov	r16, r24
+      getch();
+    7eb4:	6f d0       	rcall	.+222    	; 0x7f94 <getch>
 
       // If we are in RWW section, immediately start page erase
       if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    7eb2:	e0 91 00 02 	lds	r30, 0x0200
-    7eb6:	f0 91 01 02 	lds	r31, 0x0201
+    7eb6:	80 e0       	ldi	r24, 0x00	; 0
+    7eb8:	c8 16       	cp	r12, r24
     7eba:	80 e7       	ldi	r24, 0x70	; 112
-    7ebc:	e0 30       	cpi	r30, 0x00	; 0
-    7ebe:	f8 07       	cpc	r31, r24
-    7ec0:	18 f4       	brcc	.+6      	; 0x7ec8 <main+0xc8>
-    7ec2:	83 e0       	ldi	r24, 0x03	; 3
+    7ebc:	d8 06       	cpc	r13, r24
+    7ebe:	20 f4       	brcc	.+8      	; 0x7ec8 <main+0xc8>
+    7ec0:	83 e0       	ldi	r24, 0x03	; 3
+    7ec2:	f6 01       	movw	r30, r12
     7ec4:	87 bf       	out	0x37, r24	; 55
     7ec6:	e8 95       	spm
     7ec8:	c0 e0       	ldi	r28, 0x00	; 0
@@ -255,302 +261,301 @@ void watchdogReset() {
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    7ecc:	71 d0       	rcall	.+226    	; 0x7fb0 <getch>
+    7ecc:	63 d0       	rcall	.+198    	; 0x7f94 <getch>
     7ece:	89 93       	st	Y+, r24
       while (--length);
-    7ed0:	80 91 02 02 	lds	r24, 0x0202
-    7ed4:	81 50       	subi	r24, 0x01	; 1
-    7ed6:	80 93 02 02 	sts	0x0202, r24
-    7eda:	88 23       	and	r24, r24
-    7edc:	b9 f7       	brne	.-18     	; 0x7ecc <main+0xcc>
+    7ed0:	0c 17       	cp	r16, r28
+    7ed2:	e1 f7       	brne	.-8      	; 0x7ecc <main+0xcc>
 
       // If we are in NRWW section, page erase has to be delayed until now.
       // Todo: Take RAMPZ into account
       if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    7ede:	e0 91 00 02 	lds	r30, 0x0200
-    7ee2:	f0 91 01 02 	lds	r31, 0x0201
-    7ee6:	80 e7       	ldi	r24, 0x70	; 112
-    7ee8:	e0 30       	cpi	r30, 0x00	; 0
-    7eea:	f8 07       	cpc	r31, r24
-    7eec:	18 f0       	brcs	.+6      	; 0x7ef4 <main+0xf4>
-    7eee:	83 e0       	ldi	r24, 0x03	; 3
-    7ef0:	87 bf       	out	0x37, r24	; 55
-    7ef2:	e8 95       	spm
+    7ed4:	f0 e0       	ldi	r31, 0x00	; 0
+    7ed6:	cf 16       	cp	r12, r31
+    7ed8:	f0 e7       	ldi	r31, 0x70	; 112
+    7eda:	df 06       	cpc	r13, r31
+    7edc:	20 f0       	brcs	.+8      	; 0x7ee6 <main+0xe6>
+    7ede:	83 e0       	ldi	r24, 0x03	; 3
+    7ee0:	f6 01       	movw	r30, r12
+    7ee2:	87 bf       	out	0x37, r24	; 55
+    7ee4:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    7ef4:	75 d0       	rcall	.+234    	; 0x7fe0 <verifySpace>
+    7ee6:	64 d0       	rcall	.+200    	; 0x7fb0 <verifySpace>
 
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    7ef6:	07 b6       	in	r0, 0x37	; 55
-    7ef8:	00 fc       	sbrc	r0, 0
-    7efa:	fd cf       	rjmp	.-6      	; 0x7ef6 <main+0xf6>
-      }
-#endif
-
-      // Copy buffer into programming buffer
+    7ee8:	07 b6       	in	r0, 0x37	; 55
+    7eea:	00 fc       	sbrc	r0, 0
+    7eec:	fd cf       	rjmp	.-6      	; 0x7ee8 <main+0xe8>
+    7eee:	a6 01       	movw	r20, r12
+    7ef0:	a0 e0       	ldi	r26, 0x00	; 0
+    7ef2:	b1 e0       	ldi	r27, 0x01	; 1
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    7efc:	40 91 00 02 	lds	r20, 0x0200
-    7f00:	50 91 01 02 	lds	r21, 0x0201
-    7f04:	a0 e0       	ldi	r26, 0x00	; 0
-    7f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    7f08:	2c 91       	ld	r18, X
-    7f0a:	30 e0       	ldi	r19, 0x00	; 0
+    7ef4:	2c 91       	ld	r18, X
+    7ef6:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    7f0c:	11 96       	adiw	r26, 0x01	; 1
-    7f0e:	8c 91       	ld	r24, X
-    7f10:	11 97       	sbiw	r26, 0x01	; 1
-    7f12:	90 e0       	ldi	r25, 0x00	; 0
-    7f14:	98 2f       	mov	r25, r24
-    7f16:	88 27       	eor	r24, r24
-    7f18:	82 2b       	or	r24, r18
-    7f1a:	93 2b       	or	r25, r19
+    7ef8:	11 96       	adiw	r26, 0x01	; 1
+    7efa:	8c 91       	ld	r24, X
+    7efc:	11 97       	sbiw	r26, 0x01	; 1
+    7efe:	90 e0       	ldi	r25, 0x00	; 0
+    7f00:	98 2f       	mov	r25, r24
+    7f02:	88 27       	eor	r24, r24
+    7f04:	82 2b       	or	r24, r18
+    7f06:	93 2b       	or	r25, r19
 #define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
 #define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
 /* main program starts here */
 int main(void) {
-    7f1c:	12 96       	adiw	r26, 0x02	; 2
+    7f08:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    7f1e:	fa 01       	movw	r30, r20
-    7f20:	0c 01       	movw	r0, r24
-    7f22:	d7 be       	out	0x37, r13	; 55
-    7f24:	e8 95       	spm
-    7f26:	11 24       	eor	r1, r1
+    7f0a:	fa 01       	movw	r30, r20
+    7f0c:	0c 01       	movw	r0, r24
+    7f0e:	97 be       	out	0x37, r9	; 55
+    7f10:	e8 95       	spm
+    7f12:	11 24       	eor	r1, r1
         addrPtr += 2;
-    7f28:	4e 5f       	subi	r20, 0xFE	; 254
-    7f2a:	5f 4f       	sbci	r21, 0xFF	; 255
+    7f14:	4e 5f       	subi	r20, 0xFE	; 254
+    7f16:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    7f2c:	f1 e0       	ldi	r31, 0x01	; 1
-    7f2e:	a0 38       	cpi	r26, 0x80	; 128
-    7f30:	bf 07       	cpc	r27, r31
-    7f32:	51 f7       	brne	.-44     	; 0x7f08 <main+0x108>
+    7f18:	f1 e0       	ldi	r31, 0x01	; 1
+    7f1a:	a0 38       	cpi	r26, 0x80	; 128
+    7f1c:	bf 07       	cpc	r27, r31
+    7f1e:	51 f7       	brne	.-44     	; 0x7ef4 <main+0xf4>
 
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
-    7f34:	e0 91 00 02 	lds	r30, 0x0200
-    7f38:	f0 91 01 02 	lds	r31, 0x0201
-    7f3c:	e7 be       	out	0x37, r14	; 55
-    7f3e:	e8 95       	spm
+    7f20:	f6 01       	movw	r30, r12
+    7f22:	a7 be       	out	0x37, r10	; 55
+    7f24:	e8 95       	spm
       boot_spm_busy_wait();
-    7f40:	07 b6       	in	r0, 0x37	; 55
-    7f42:	00 fc       	sbrc	r0, 0
-    7f44:	fd cf       	rjmp	.-6      	; 0x7f40 <main+0x140>
+    7f26:	07 b6       	in	r0, 0x37	; 55
+    7f28:	00 fc       	sbrc	r0, 0
+    7f2a:	fd cf       	rjmp	.-6      	; 0x7f26 <main+0x126>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    7f46:	f7 be       	out	0x37, r15	; 55
-    7f48:	e8 95       	spm
-    7f4a:	27 c0       	rjmp	.+78     	; 0x7f9a <main+0x19a>
+    7f2c:	b7 be       	out	0x37, r11	; 55
+    7f2e:	e8 95       	spm
+    7f30:	26 c0       	rjmp	.+76     	; 0x7f7e <main+0x17e>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    7f4c:	84 37       	cpi	r24, 0x74	; 116
-    7f4e:	b9 f4       	brne	.+46     	; 0x7f7e <main+0x17e>
+    7f32:	84 37       	cpi	r24, 0x74	; 116
+    7f34:	b1 f4       	brne	.+44     	; 0x7f62 <main+0x162>
       // READ PAGE - we only read flash
-      getLen();
-    7f50:	37 d0       	rcall	.+110    	; 0x7fc0 <getLen>
+      getch();			/* getlen() */
+    7f36:	2e d0       	rcall	.+92     	; 0x7f94 <getch>
+      length = getch();
+    7f38:	2d d0       	rcall	.+90     	; 0x7f94 <getch>
+    7f3a:	f8 2e       	mov	r15, r24
+      getch();
+    7f3c:	2b d0       	rcall	.+86     	; 0x7f94 <getch>
+
       verifySpace();
-    7f52:	46 d0       	rcall	.+140    	; 0x7fe0 <verifySpace>
+    7f3e:	38 d0       	rcall	.+112    	; 0x7fb0 <verifySpace>
+    7f40:	f6 01       	movw	r30, r12
+    7f42:	ef 2c       	mov	r14, r15
         putch(result);
         address++;
       }
       while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    7f54:	e0 91 00 02 	lds	r30, 0x0200
-    7f58:	f0 91 01 02 	lds	r31, 0x0201
-    7f5c:	31 96       	adiw	r30, 0x01	; 1
-    7f5e:	f0 93 01 02 	sts	0x0201, r31
-    7f62:	e0 93 00 02 	sts	0x0200, r30
-    7f66:	31 97       	sbiw	r30, 0x01	; 1
-    7f68:	e4 91       	lpm	r30, Z+
-    7f6a:	8e 2f       	mov	r24, r30
-    7f6c:	19 d0       	rcall	.+50     	; 0x7fa0 <putch>
+    7f44:	8f 01       	movw	r16, r30
+    7f46:	0f 5f       	subi	r16, 0xFF	; 255
+    7f48:	1f 4f       	sbci	r17, 0xFF	; 255
+    7f4a:	84 91       	lpm	r24, Z+
+    7f4c:	1b d0       	rcall	.+54     	; 0x7f84 <putch>
       while (--length);
-    7f6e:	80 91 02 02 	lds	r24, 0x0202
-    7f72:	81 50       	subi	r24, 0x01	; 1
-    7f74:	80 93 02 02 	sts	0x0202, r24
-    7f78:	88 23       	and	r24, r24
-    7f7a:	61 f7       	brne	.-40     	; 0x7f54 <main+0x154>
-    7f7c:	0e c0       	rjmp	.+28     	; 0x7f9a <main+0x19a>
+    7f4e:	ea 94       	dec	r14
+    7f50:	f8 01       	movw	r30, r16
+    7f52:	c1 f7       	brne	.-16     	; 0x7f44 <main+0x144>
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
+#endif
+
+/* main program starts here */
+int main(void) {
+    7f54:	08 94       	sec
+    7f56:	c1 1c       	adc	r12, r1
+    7f58:	d1 1c       	adc	r13, r1
+    7f5a:	fa 94       	dec	r15
+    7f5c:	cf 0c       	add	r12, r15
+    7f5e:	d1 1c       	adc	r13, r1
+    7f60:	0e c0       	rjmp	.+28     	; 0x7f7e <main+0x17e>
 #endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    7f7e:	85 37       	cpi	r24, 0x75	; 117
-    7f80:	39 f4       	brne	.+14     	; 0x7f90 <main+0x190>
+    7f62:	85 37       	cpi	r24, 0x75	; 117
+    7f64:	39 f4       	brne	.+14     	; 0x7f74 <main+0x174>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    7f82:	2e d0       	rcall	.+92     	; 0x7fe0 <verifySpace>
+    7f66:	24 d0       	rcall	.+72     	; 0x7fb0 <verifySpace>
       putch(SIGNATURE_0);
-    7f84:	8e e1       	ldi	r24, 0x1E	; 30
-    7f86:	0c d0       	rcall	.+24     	; 0x7fa0 <putch>
+    7f68:	8e e1       	ldi	r24, 0x1E	; 30
+    7f6a:	0c d0       	rcall	.+24     	; 0x7f84 <putch>
       putch(SIGNATURE_1);
-    7f88:	85 e9       	ldi	r24, 0x95	; 149
-    7f8a:	0a d0       	rcall	.+20     	; 0x7fa0 <putch>
+    7f6c:	85 e9       	ldi	r24, 0x95	; 149
+    7f6e:	0a d0       	rcall	.+20     	; 0x7f84 <putch>
       putch(SIGNATURE_2);
-    7f8c:	8f e0       	ldi	r24, 0x0F	; 15
-    7f8e:	8b cf       	rjmp	.-234    	; 0x7ea6 <main+0xa6>
+    7f70:	8f e0       	ldi	r24, 0x0F	; 15
+    7f72:	98 cf       	rjmp	.-208    	; 0x7ea4 <main+0xa4>
     }
     else if (ch == 'Q') {
-    7f90:	81 35       	cpi	r24, 0x51	; 81
-    7f92:	11 f4       	brne	.+4      	; 0x7f98 <main+0x198>
+    7f74:	81 35       	cpi	r24, 0x51	; 81
+    7f76:	11 f4       	brne	.+4      	; 0x7f7c <main+0x17c>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    7f94:	88 e0       	ldi	r24, 0x08	; 8
-    7f96:	19 d0       	rcall	.+50     	; 0x7fca <watchdogConfig>
+    7f78:	88 e0       	ldi	r24, 0x08	; 8
+    7f7a:	14 d0       	rcall	.+40     	; 0x7fa4 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    7f98:	23 d0       	rcall	.+70     	; 0x7fe0 <verifySpace>
+    7f7c:	19 d0       	rcall	.+50     	; 0x7fb0 <verifySpace>
     }
     putch(STK_OK);
-    7f9a:	80 e1       	ldi	r24, 0x10	; 16
-    7f9c:	01 d0       	rcall	.+2      	; 0x7fa0 <putch>
-    7f9e:	5c cf       	rjmp	.-328    	; 0x7e58 <main+0x58>
+    7f7e:	80 e1       	ldi	r24, 0x10	; 16
+    7f80:	01 d0       	rcall	.+2      	; 0x7f84 <putch>
+    7f82:	6a cf       	rjmp	.-300    	; 0x7e58 <main+0x58>
 
-00007fa0 <putch>:
+00007f84 <putch>:
   }
 }
 
 void putch(char ch) {
-    7fa0:	98 2f       	mov	r25, r24
+    7f84:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    7fa2:	80 91 c0 00 	lds	r24, 0x00C0
-    7fa6:	85 ff       	sbrs	r24, 5
-    7fa8:	fc cf       	rjmp	.-8      	; 0x7fa2 <putch+0x2>
+    7f86:	80 91 c0 00 	lds	r24, 0x00C0
+    7f8a:	85 ff       	sbrs	r24, 5
+    7f8c:	fc cf       	rjmp	.-8      	; 0x7f86 <putch+0x2>
   UDR0 = ch;
-    7faa:	90 93 c6 00 	sts	0x00C6, r25
+    7f8e:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    7fae:	08 95       	ret
+    7f92:	08 95       	ret
 
-00007fb0 <getch>:
-  return getch();
+00007f94 <getch>:
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    7fb0:	a8 95       	wdr
+    7f94:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    7fb2:	80 91 c0 00 	lds	r24, 0x00C0
-    7fb6:	87 ff       	sbrs	r24, 7
-    7fb8:	fc cf       	rjmp	.-8      	; 0x7fb2 <getch+0x2>
+    7f96:	80 91 c0 00 	lds	r24, 0x00C0
+    7f9a:	87 ff       	sbrs	r24, 7
+    7f9c:	fc cf       	rjmp	.-8      	; 0x7f96 <getch+0x2>
   ch = UDR0;
-    7fba:	80 91 c6 00 	lds	r24, 0x00C6
+    7f9e:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    7fbe:	08 95       	ret
-
-00007fc0 <getLen>:
-  } while (--count);
-}
-#endif
-
-uint8_t getLen() {
-  getch();
-    7fc0:	f7 df       	rcall	.-18     	; 0x7fb0 <getch>
-  length = getch();
-    7fc2:	f6 df       	rcall	.-20     	; 0x7fb0 <getch>
-    7fc4:	80 93 02 02 	sts	0x0202, r24
-  return getch();
-}
-    7fc8:	f3 cf       	rjmp	.-26     	; 0x7fb0 <getch>
+    7fa2:	08 95       	ret
 
-00007fca <watchdogConfig>:
+00007fa4 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    7fca:	e0 e6       	ldi	r30, 0x60	; 96
-    7fcc:	f0 e0       	ldi	r31, 0x00	; 0
-    7fce:	98 e1       	ldi	r25, 0x18	; 24
-    7fd0:	90 83       	st	Z, r25
+    7fa4:	e0 e6       	ldi	r30, 0x60	; 96
+    7fa6:	f0 e0       	ldi	r31, 0x00	; 0
+    7fa8:	98 e1       	ldi	r25, 0x18	; 24
+    7faa:	90 83       	st	Z, r25
   WDTCSR = x;
-    7fd2:	80 83       	st	Z, r24
+    7fac:	80 83       	st	Z, r24
 }
-    7fd4:	08 95       	ret
-
-00007fd6 <appStart>:
-
-void appStart() {
-  watchdogConfig(WATCHDOG_OFF);
-    7fd6:	80 e0       	ldi	r24, 0x00	; 0
-    7fd8:	f8 df       	rcall	.-16     	; 0x7fca <watchdogConfig>
-  __asm__ __volatile__ (
-    7fda:	ee 27       	eor	r30, r30
-    7fdc:	ff 27       	eor	r31, r31
-    7fde:	09 94       	ijmp
+    7fae:	08 95       	ret
 
-00007fe0 <verifySpace>:
+00007fb0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
-  if (getch() != CRC_EOP) appStart();
-    7fe0:	e7 df       	rcall	.-50     	; 0x7fb0 <getch>
-    7fe2:	80 32       	cpi	r24, 0x20	; 32
-    7fe4:	09 f0       	breq	.+2      	; 0x7fe8 <verifySpace+0x8>
-    7fe6:	f7 df       	rcall	.-18     	; 0x7fd6 <appStart>
+  if (getch() != CRC_EOP) {
+    7fb0:	f1 df       	rcall	.-30     	; 0x7f94 <getch>
+    7fb2:	80 32       	cpi	r24, 0x20	; 32
+    7fb4:	19 f0       	breq	.+6      	; 0x7fbc <verifySpace+0xc>
+    watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
+    7fb6:	88 e0       	ldi	r24, 0x08	; 8
+    7fb8:	f5 df       	rcall	.-22     	; 0x7fa4 <watchdogConfig>
+    7fba:	ff cf       	rjmp	.-2      	; 0x7fba <verifySpace+0xa>
+    while (1)			      // and busy-loop so that WD causes
+      ;				      //  a reset and app start.
+  }
   putch(STK_INSYNC);
-    7fe8:	84 e1       	ldi	r24, 0x14	; 20
+    7fbc:	84 e1       	ldi	r24, 0x14	; 20
 }
-    7fea:	da cf       	rjmp	.-76     	; 0x7fa0 <putch>
+    7fbe:	e2 cf       	rjmp	.-60     	; 0x7f84 <putch>
 
-00007fec <getNch>:
+00007fc0 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    7fec:	1f 93       	push	r17
-    7fee:	18 2f       	mov	r17, r24
+    7fc0:	1f 93       	push	r17
+    7fc2:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    7ff0:	df df       	rcall	.-66     	; 0x7fb0 <getch>
-    7ff2:	11 50       	subi	r17, 0x01	; 1
-    7ff4:	e9 f7       	brne	.-6      	; 0x7ff0 <getNch+0x4>
+    7fc4:	e7 df       	rcall	.-50     	; 0x7f94 <getch>
+    7fc6:	11 50       	subi	r17, 0x01	; 1
+    7fc8:	e9 f7       	brne	.-6      	; 0x7fc4 <getNch+0x4>
   verifySpace();
-    7ff6:	f4 df       	rcall	.-24     	; 0x7fe0 <verifySpace>
+    7fca:	f2 df       	rcall	.-28     	; 0x7fb0 <verifySpace>
+}
+    7fcc:	1f 91       	pop	r17
+    7fce:	08 95       	ret
+
+00007fd0 <appStart>:
+  WDTCSR = _BV(WDCE) | _BV(WDE);
+  WDTCSR = x;
 }
-    7ff8:	1f 91       	pop	r17
-    7ffa:	08 95       	ret
+
+void appStart() {
+  watchdogConfig(WATCHDOG_OFF);
+    7fd0:	80 e0       	ldi	r24, 0x00	; 0
+    7fd2:	e8 df       	rcall	.-48     	; 0x7fa4 <watchdogConfig>
+  __asm__ __volatile__ (
+    7fd4:	ee 27       	eor	r30, r30
+    7fd6:	ff 27       	eor	r31, r31
+    7fd8:	09 94       	ijmp
diff --git a/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex b/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex
index 1e56839..a181f0b 100644
--- a/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex
+++ b/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex
@@ -1,34 +1,32 @@
-:10000000112484B714BE81FFE6D085E0809381007F
+:10000000112484B714BE81FFE3D085E08093810082
 :1000100082E08093C00088E18093C10086E08093F5
-:10002000C20088E08093C4008EE0CFD0259A86E09D
+:10002000C20088E08093C4008EE0BCD0259A86E0B0
 :1000300028E13EEF91E0309385002093840096BB49
-:10004000B09BFECF1D9AA8958150A9F7DD24D394CB
-:10005000A5E0EA2EF1E1FF2EABD0813421F481E05E
-:10006000C5D083E020C0823411F484E103C085341C
-:1000700019F485E0BBD091C0853581F499D0082F63
-:1000800010E096D090E0982F8827802B912B880F36
-:10009000991F90930102809300027EC0863529F457
-:1000A00084E0A4D080E07CD078C0843609F04EC0D3
-:1000B00087D0E0910002F091010280E7E030F8077C
-:1000C00018F483E087BFE895C0E0D1E071D0899350
-:1000D000809102028150809302028823B9F7E09157
-:1000E0000002F091010280E7E030F80718F083E0A9
-:1000F00087BFE89575D007B600FCFDCF40910002A0
-:1001000050910102A0E0B1E02C9130E011968C9169
-:10011000119790E0982F8827822B932B1296FA0143
-:100120000C01D7BEE89511244E5F5F4FF1E0A03877
-:10013000BF0751F7E0910002F0910102E7BEE89598
-:1001400007B600FCFDCFF7BEE89527C08437B9F4A9
-:1001500037D046D0E0910002F09101023196F09341
-:100160000102E09300023197E4918E2F19D0809123
-:100170000202815080930202882361F70EC0853706
-:1001800039F42ED08EE10CD085E90AD08FE08BCFE8
-:10019000813511F488E019D023D080E101D05CCF03
-:1001A000982F8091C00085FFFCCF9093C6000895E2
-:1001B000A8958091C00087FFFCCF8091C60008956C
-:1001C000F7DFF6DF80930202F3CFE0E6F0E098E19C
-:1001D00090838083089580E0F8DFEE27FF2709945D
-:1001E000E7DF803209F0F7DF84E1DACF1F93182FC1
-:0C01F000DFDF1150E9F7F4DF1F910895E4
+:10004000B09BFECF1D9AA8958150A9F7992493944F
+:10005000A5E0AA2EF1E1BF2E9DD0813421F481E0EC
+:10006000AFD083E01FC0823411F484E103C0853433
+:1000700019F485E0A5D083C0853579F48BD0E82EBE
+:10008000FF2488D0082F10E0102F00270E291F29E9
+:10009000000F111F8DD0680172C0863529F484E0ED
+:1000A0008FD080E06FD06BC0843609F042C072D030
+:1000B00071D0082F6FD080E0C81680E7D80620F4F2
+:1000C00083E0F60187BFE895C0E0D1E063D0899373
+:1000D0000C17E1F7F0E0CF16F0E7DF0620F083E041
+:1000E000F60187BFE89564D007B600FCFDCFA601F6
+:1000F000A0E0B1E02C9130E011968C91119790E046
+:10010000982F8827822B932B1296FA010C0197BE09
+:10011000E89511244E5F5F4FF1E0A038BF0751F71B
+:10012000F601A7BEE89507B600FCFDCFB7BEE8957F
+:1001300026C08437B1F42ED02DD0F82E2BD038D055
+:10014000F601EF2C8F010F5F1F4F84911BD0EA94B3
+:10015000F801C1F70894C11CD11CFA94CF0CD11C32
+:100160000EC0853739F424D08EE10CD085E90AD051
+:100170008FE098CF813511F488E014D019D080E158
+:1001800001D06ACF982F8091C00085FFFCCF90935B
+:10019000C6000895A8958091C00087FFFCCF80918C
+:1001A000C6000895E0E6F0E098E19083808308952A
+:1001B000F1DF803219F088E0F5DFFFCF84E1E2CF94
+:1001C0001F93182FE7DF1150E9F7F2DF1F91089511
+:0A01D00080E0E8DFEE27FF27099426
 :027FFE0001047C
 :00000001FF
diff --git a/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst b/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst
index d7ce991..a4da506 100644
--- a/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst
+++ b/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst
@@ -3,27 +3,27 @@ optiboot_atmega328_pro_8MHz.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001fc  00000000  00000000  00000054  2**1
+  0 .text         000001da  00000000  00000000  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00007ffe  00007ffe  00000250  2**0
+  1 .version      00000002  00007ffe  00007ffe  0000022e  2**0
                   CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000252  2**0
+  2 .debug_aranges 00000028  00000000  00000000  00000230  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000006a  00000000  00000000  0000027a  2**0
+  3 .debug_pubnames 0000005f  00000000  00000000  00000258  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000285  00000000  00000000  000002e4  2**0
+  4 .debug_info   0000028c  00000000  00000000  000002b7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000019f  00000000  00000000  00000569  2**0
+  5 .debug_abbrev 00000199  00000000  00000000  00000543  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000453  00000000  00000000  00000708  2**0
+  6 .debug_line   00000456  00000000  00000000  000006dc  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000090  00000000  00000000  00000b5c  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000b34  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000141  00000000  00000000  00000bec  2**0
+  8 .debug_str    00000149  00000000  00000000  00000bb4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    000001e1  00000000  00000000  00000d2d  2**0
+  9 .debug_loc    0000027e  00000000  00000000  00000cfd  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000068  00000000  00000000  00000f0e  2**0
+ 10 .debug_ranges 00000060  00000000  00000000  00000f7b  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
@@ -47,7 +47,7 @@ int main(void) {
    4:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
    6:	81 ff       	sbrs	r24, 1
-   8:	e6 d0       	rcall	.+460    	; 0x1d6 <appStart>
+   8:	e3 d0       	rcall	.+454    	; 0x1d0 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
@@ -77,7 +77,7 @@ int main(void) {
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
   28:	8e e0       	ldi	r24, 0x0E	; 14
-  2a:	cf d0       	rcall	.+414    	; 0x1ca <watchdogConfig>
+  2a:	bc d0       	rcall	.+376    	; 0x1a4 <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
@@ -111,8 +111,8 @@ void flash_led(uint8_t count) {
 #else
     LED_PIN |= _BV(LED);
   44:	1d 9a       	sbi	0x03, 5	; 3
-  return getch();
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
@@ -132,8 +132,8 @@ void watchdogReset() {
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-  4c:	dd 24       	eor	r13, r13
-  4e:	d3 94       	inc	r13
+  4c:	99 24       	eor	r9, r9
+  4e:	93 94       	inc	r9
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
@@ -141,21 +141,21 @@ void watchdogReset() {
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
   50:	a5 e0       	ldi	r26, 0x05	; 5
-  52:	ea 2e       	mov	r14, r26
+  52:	aa 2e       	mov	r10, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
   54:	f1 e1       	ldi	r31, 0x11	; 17
-  56:	ff 2e       	mov	r15, r31
+  56:	bf 2e       	mov	r11, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-  58:	ab d0       	rcall	.+342    	; 0x1b0 <getch>
+  58:	9d d0       	rcall	.+314    	; 0x194 <getch>
 
     if(ch == STK_GET_PARAMETER) {
   5a:	81 34       	cpi	r24, 0x41	; 65
@@ -163,10 +163,10 @@ void watchdogReset() {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
   5e:	81 e0       	ldi	r24, 0x01	; 1
-  60:	c5 d0       	rcall	.+394    	; 0x1ec <getNch>
+  60:	af d0       	rcall	.+350    	; 0x1c0 <getNch>
       putch(0x03);
   62:	83 e0       	ldi	r24, 0x03	; 3
-  64:	20 c0       	rjmp	.+64     	; 0xa6 <__SREG__+0x67>
+  64:	1f c0       	rjmp	.+62     	; 0xa4 <__SREG__+0x65>
     }
     else if(ch == STK_SET_DEVICE) {
   66:	82 34       	cpi	r24, 0x42	; 66
@@ -182,71 +182,77 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
   72:	85 e0       	ldi	r24, 0x05	; 5
-  74:	bb d0       	rcall	.+374    	; 0x1ec <getNch>
-  76:	91 c0       	rjmp	.+290    	; 0x19a <__SREG__+0x15b>
+  74:	a5 d0       	rcall	.+330    	; 0x1c0 <getNch>
+  76:	83 c0       	rjmp	.+262    	; 0x17e <__SREG__+0x13f>
     }
     else if(ch == STK_LOAD_ADDRESS) {
   78:	85 35       	cpi	r24, 0x55	; 85
-  7a:	81 f4       	brne	.+32     	; 0x9c <__SREG__+0x5d>
+  7a:	79 f4       	brne	.+30     	; 0x9a <__SREG__+0x5b>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-  7c:	99 d0       	rcall	.+306    	; 0x1b0 <getch>
+  7c:	8b d0       	rcall	.+278    	; 0x194 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-  7e:	08 2f       	mov	r16, r24
-  80:	10 e0       	ldi	r17, 0x00	; 0
-  82:	96 d0       	rcall	.+300    	; 0x1b0 <getch>
-  84:	90 e0       	ldi	r25, 0x00	; 0
-  86:	98 2f       	mov	r25, r24
-  88:	88 27       	eor	r24, r24
-  8a:	80 2b       	or	r24, r16
-  8c:	91 2b       	or	r25, r17
+  7e:	e8 2e       	mov	r14, r24
+  80:	ff 24       	eor	r15, r15
+  82:	88 d0       	rcall	.+272    	; 0x194 <getch>
+  84:	08 2f       	mov	r16, r24
+  86:	10 e0       	ldi	r17, 0x00	; 0
+  88:	10 2f       	mov	r17, r16
+  8a:	00 27       	eor	r16, r16
+  8c:	0e 29       	or	r16, r14
+  8e:	1f 29       	or	r17, r15
 #ifdef RAMPZ
       // Transfer top bit to RAMPZ
       RAMPZ = (newAddress & 0x8000) ? 1 : 0;
 #endif
       newAddress += newAddress; // Convert from word address to byte address
-  8e:	88 0f       	add	r24, r24
-  90:	99 1f       	adc	r25, r25
+  90:	00 0f       	add	r16, r16
+  92:	11 1f       	adc	r17, r17
       address = newAddress;
-  92:	90 93 01 02 	sts	0x0201, r25
-  96:	80 93 00 02 	sts	0x0200, r24
-  9a:	7e c0       	rjmp	.+252    	; 0x198 <__SREG__+0x159>
       verifySpace();
+  94:	8d d0       	rcall	.+282    	; 0x1b0 <verifySpace>
+  96:	68 01       	movw	r12, r16
+  98:	72 c0       	rjmp	.+228    	; 0x17e <__SREG__+0x13f>
     }
     else if(ch == STK_UNIVERSAL) {
-  9c:	86 35       	cpi	r24, 0x56	; 86
-  9e:	29 f4       	brne	.+10     	; 0xaa <__SREG__+0x6b>
+  9a:	86 35       	cpi	r24, 0x56	; 86
+  9c:	29 f4       	brne	.+10     	; 0xa8 <__SREG__+0x69>
       // UNIVERSAL command is ignored
       getNch(4);
-  a0:	84 e0       	ldi	r24, 0x04	; 4
-  a2:	a4 d0       	rcall	.+328    	; 0x1ec <getNch>
+  9e:	84 e0       	ldi	r24, 0x04	; 4
+  a0:	8f d0       	rcall	.+286    	; 0x1c0 <getNch>
       putch(0x00);
-  a4:	80 e0       	ldi	r24, 0x00	; 0
-  a6:	7c d0       	rcall	.+248    	; 0x1a0 <putch>
-  a8:	78 c0       	rjmp	.+240    	; 0x19a <__SREG__+0x15b>
+  a2:	80 e0       	ldi	r24, 0x00	; 0
+  a4:	6f d0       	rcall	.+222    	; 0x184 <putch>
+  a6:	6b c0       	rjmp	.+214    	; 0x17e <__SREG__+0x13f>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-  aa:	84 36       	cpi	r24, 0x64	; 100
-  ac:	09 f0       	breq	.+2      	; 0xb0 <__SREG__+0x71>
-  ae:	4e c0       	rjmp	.+156    	; 0x14c <__SREG__+0x10d>
+  a8:	84 36       	cpi	r24, 0x64	; 100
+  aa:	09 f0       	breq	.+2      	; 0xae <__SREG__+0x6f>
+  ac:	42 c0       	rjmp	.+132    	; 0x132 <__SREG__+0xf3>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
-      getLen();
-  b0:	87 d0       	rcall	.+270    	; 0x1c0 <getLen>
+      getch();			/* getlen() */
+  ae:	72 d0       	rcall	.+228    	; 0x194 <getch>
+      length = getch();
+  b0:	71 d0       	rcall	.+226    	; 0x194 <getch>
+  b2:	08 2f       	mov	r16, r24
+      getch();
+  b4:	6f d0       	rcall	.+222    	; 0x194 <getch>
 
       // If we are in RWW section, immediately start page erase
       if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-  b2:	e0 91 00 02 	lds	r30, 0x0200
-  b6:	f0 91 01 02 	lds	r31, 0x0201
+  b6:	80 e0       	ldi	r24, 0x00	; 0
+  b8:	c8 16       	cp	r12, r24
   ba:	80 e7       	ldi	r24, 0x70	; 112
-  bc:	e0 30       	cpi	r30, 0x00	; 0
-  be:	f8 07       	cpc	r31, r24
-  c0:	18 f4       	brcc	.+6      	; 0xc8 <__SREG__+0x89>
-  c2:	83 e0       	ldi	r24, 0x03	; 3
+  bc:	d8 06       	cpc	r13, r24
+  be:	20 f4       	brcc	.+8      	; 0xc8 <__SREG__+0x89>
+  c0:	83 e0       	ldi	r24, 0x03	; 3
+  c2:	f6 01       	movw	r30, r12
   c4:	87 bf       	out	0x37, r24	; 55
   c6:	e8 95       	spm
   c8:	c0 e0       	ldi	r28, 0x00	; 0
@@ -255,302 +261,301 @@ void watchdogReset() {
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-  cc:	71 d0       	rcall	.+226    	; 0x1b0 <getch>
+  cc:	63 d0       	rcall	.+198    	; 0x194 <getch>
   ce:	89 93       	st	Y+, r24
       while (--length);
-  d0:	80 91 02 02 	lds	r24, 0x0202
-  d4:	81 50       	subi	r24, 0x01	; 1
-  d6:	80 93 02 02 	sts	0x0202, r24
-  da:	88 23       	and	r24, r24
-  dc:	b9 f7       	brne	.-18     	; 0xcc <__SREG__+0x8d>
+  d0:	0c 17       	cp	r16, r28
+  d2:	e1 f7       	brne	.-8      	; 0xcc <__SREG__+0x8d>
 
       // If we are in NRWW section, page erase has to be delayed until now.
       // Todo: Take RAMPZ into account
       if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-  de:	e0 91 00 02 	lds	r30, 0x0200
-  e2:	f0 91 01 02 	lds	r31, 0x0201
-  e6:	80 e7       	ldi	r24, 0x70	; 112
-  e8:	e0 30       	cpi	r30, 0x00	; 0
-  ea:	f8 07       	cpc	r31, r24
-  ec:	18 f0       	brcs	.+6      	; 0xf4 <__SREG__+0xb5>
-  ee:	83 e0       	ldi	r24, 0x03	; 3
-  f0:	87 bf       	out	0x37, r24	; 55
-  f2:	e8 95       	spm
+  d4:	f0 e0       	ldi	r31, 0x00	; 0
+  d6:	cf 16       	cp	r12, r31
+  d8:	f0 e7       	ldi	r31, 0x70	; 112
+  da:	df 06       	cpc	r13, r31
+  dc:	20 f0       	brcs	.+8      	; 0xe6 <__SREG__+0xa7>
+  de:	83 e0       	ldi	r24, 0x03	; 3
+  e0:	f6 01       	movw	r30, r12
+  e2:	87 bf       	out	0x37, r24	; 55
+  e4:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-  f4:	75 d0       	rcall	.+234    	; 0x1e0 <verifySpace>
+  e6:	64 d0       	rcall	.+200    	; 0x1b0 <verifySpace>
 
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-  f6:	07 b6       	in	r0, 0x37	; 55
-  f8:	00 fc       	sbrc	r0, 0
-  fa:	fd cf       	rjmp	.-6      	; 0xf6 <__SREG__+0xb7>
-      }
-#endif
-
-      // Copy buffer into programming buffer
+  e8:	07 b6       	in	r0, 0x37	; 55
+  ea:	00 fc       	sbrc	r0, 0
+  ec:	fd cf       	rjmp	.-6      	; 0xe8 <__SREG__+0xa9>
+  ee:	a6 01       	movw	r20, r12
+  f0:	a0 e0       	ldi	r26, 0x00	; 0
+  f2:	b1 e0       	ldi	r27, 0x01	; 1
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-  fc:	40 91 00 02 	lds	r20, 0x0200
- 100:	50 91 01 02 	lds	r21, 0x0201
- 104:	a0 e0       	ldi	r26, 0x00	; 0
- 106:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
- 108:	2c 91       	ld	r18, X
- 10a:	30 e0       	ldi	r19, 0x00	; 0
+  f4:	2c 91       	ld	r18, X
+  f6:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
- 10c:	11 96       	adiw	r26, 0x01	; 1
- 10e:	8c 91       	ld	r24, X
- 110:	11 97       	sbiw	r26, 0x01	; 1
- 112:	90 e0       	ldi	r25, 0x00	; 0
- 114:	98 2f       	mov	r25, r24
- 116:	88 27       	eor	r24, r24
- 118:	82 2b       	or	r24, r18
- 11a:	93 2b       	or	r25, r19
+  f8:	11 96       	adiw	r26, 0x01	; 1
+  fa:	8c 91       	ld	r24, X
+  fc:	11 97       	sbiw	r26, 0x01	; 1
+  fe:	90 e0       	ldi	r25, 0x00	; 0
+ 100:	98 2f       	mov	r25, r24
+ 102:	88 27       	eor	r24, r24
+ 104:	82 2b       	or	r24, r18
+ 106:	93 2b       	or	r25, r19
 #define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
 #define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
 /* main program starts here */
 int main(void) {
- 11c:	12 96       	adiw	r26, 0x02	; 2
+ 108:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
- 11e:	fa 01       	movw	r30, r20
- 120:	0c 01       	movw	r0, r24
- 122:	d7 be       	out	0x37, r13	; 55
- 124:	e8 95       	spm
- 126:	11 24       	eor	r1, r1
+ 10a:	fa 01       	movw	r30, r20
+ 10c:	0c 01       	movw	r0, r24
+ 10e:	97 be       	out	0x37, r9	; 55
+ 110:	e8 95       	spm
+ 112:	11 24       	eor	r1, r1
         addrPtr += 2;
- 128:	4e 5f       	subi	r20, 0xFE	; 254
- 12a:	5f 4f       	sbci	r21, 0xFF	; 255
+ 114:	4e 5f       	subi	r20, 0xFE	; 254
+ 116:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
- 12c:	f1 e0       	ldi	r31, 0x01	; 1
- 12e:	a0 38       	cpi	r26, 0x80	; 128
- 130:	bf 07       	cpc	r27, r31
- 132:	51 f7       	brne	.-44     	; 0x108 <__SREG__+0xc9>
+ 118:	f1 e0       	ldi	r31, 0x01	; 1
+ 11a:	a0 38       	cpi	r26, 0x80	; 128
+ 11c:	bf 07       	cpc	r27, r31
+ 11e:	51 f7       	brne	.-44     	; 0xf4 <__SREG__+0xb5>
 
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
- 134:	e0 91 00 02 	lds	r30, 0x0200
- 138:	f0 91 01 02 	lds	r31, 0x0201
- 13c:	e7 be       	out	0x37, r14	; 55
- 13e:	e8 95       	spm
+ 120:	f6 01       	movw	r30, r12
+ 122:	a7 be       	out	0x37, r10	; 55
+ 124:	e8 95       	spm
       boot_spm_busy_wait();
- 140:	07 b6       	in	r0, 0x37	; 55
- 142:	00 fc       	sbrc	r0, 0
- 144:	fd cf       	rjmp	.-6      	; 0x140 <__SREG__+0x101>
+ 126:	07 b6       	in	r0, 0x37	; 55
+ 128:	00 fc       	sbrc	r0, 0
+ 12a:	fd cf       	rjmp	.-6      	; 0x126 <__SREG__+0xe7>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
- 146:	f7 be       	out	0x37, r15	; 55
- 148:	e8 95       	spm
- 14a:	27 c0       	rjmp	.+78     	; 0x19a <__SREG__+0x15b>
+ 12c:	b7 be       	out	0x37, r11	; 55
+ 12e:	e8 95       	spm
+ 130:	26 c0       	rjmp	.+76     	; 0x17e <__SREG__+0x13f>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
- 14c:	84 37       	cpi	r24, 0x74	; 116
- 14e:	b9 f4       	brne	.+46     	; 0x17e <__SREG__+0x13f>
+ 132:	84 37       	cpi	r24, 0x74	; 116
+ 134:	b1 f4       	brne	.+44     	; 0x162 <__SREG__+0x123>
       // READ PAGE - we only read flash
-      getLen();
- 150:	37 d0       	rcall	.+110    	; 0x1c0 <getLen>
+      getch();			/* getlen() */
+ 136:	2e d0       	rcall	.+92     	; 0x194 <getch>
+      length = getch();
+ 138:	2d d0       	rcall	.+90     	; 0x194 <getch>
+ 13a:	f8 2e       	mov	r15, r24
+      getch();
+ 13c:	2b d0       	rcall	.+86     	; 0x194 <getch>
+
       verifySpace();
- 152:	46 d0       	rcall	.+140    	; 0x1e0 <verifySpace>
+ 13e:	38 d0       	rcall	.+112    	; 0x1b0 <verifySpace>
+ 140:	f6 01       	movw	r30, r12
+ 142:	ef 2c       	mov	r14, r15
         putch(result);
         address++;
       }
       while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
- 154:	e0 91 00 02 	lds	r30, 0x0200
- 158:	f0 91 01 02 	lds	r31, 0x0201
- 15c:	31 96       	adiw	r30, 0x01	; 1
- 15e:	f0 93 01 02 	sts	0x0201, r31
- 162:	e0 93 00 02 	sts	0x0200, r30
- 166:	31 97       	sbiw	r30, 0x01	; 1
- 168:	e4 91       	lpm	r30, Z+
- 16a:	8e 2f       	mov	r24, r30
- 16c:	19 d0       	rcall	.+50     	; 0x1a0 <putch>
+ 144:	8f 01       	movw	r16, r30
+ 146:	0f 5f       	subi	r16, 0xFF	; 255
+ 148:	1f 4f       	sbci	r17, 0xFF	; 255
+ 14a:	84 91       	lpm	r24, Z+
+ 14c:	1b d0       	rcall	.+54     	; 0x184 <putch>
       while (--length);
- 16e:	80 91 02 02 	lds	r24, 0x0202
- 172:	81 50       	subi	r24, 0x01	; 1
- 174:	80 93 02 02 	sts	0x0202, r24
- 178:	88 23       	and	r24, r24
- 17a:	61 f7       	brne	.-40     	; 0x154 <__SREG__+0x115>
- 17c:	0e c0       	rjmp	.+28     	; 0x19a <__SREG__+0x15b>
+ 14e:	ea 94       	dec	r14
+ 150:	f8 01       	movw	r30, r16
+ 152:	c1 f7       	brne	.-16     	; 0x144 <__SREG__+0x105>
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
+#endif
+
+/* main program starts here */
+int main(void) {
+ 154:	08 94       	sec
+ 156:	c1 1c       	adc	r12, r1
+ 158:	d1 1c       	adc	r13, r1
+ 15a:	fa 94       	dec	r15
+ 15c:	cf 0c       	add	r12, r15
+ 15e:	d1 1c       	adc	r13, r1
+ 160:	0e c0       	rjmp	.+28     	; 0x17e <__SREG__+0x13f>
 #endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
- 17e:	85 37       	cpi	r24, 0x75	; 117
- 180:	39 f4       	brne	.+14     	; 0x190 <__SREG__+0x151>
+ 162:	85 37       	cpi	r24, 0x75	; 117
+ 164:	39 f4       	brne	.+14     	; 0x174 <__SREG__+0x135>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
- 182:	2e d0       	rcall	.+92     	; 0x1e0 <verifySpace>
+ 166:	24 d0       	rcall	.+72     	; 0x1b0 <verifySpace>
       putch(SIGNATURE_0);
- 184:	8e e1       	ldi	r24, 0x1E	; 30
- 186:	0c d0       	rcall	.+24     	; 0x1a0 <putch>
+ 168:	8e e1       	ldi	r24, 0x1E	; 30
+ 16a:	0c d0       	rcall	.+24     	; 0x184 <putch>
       putch(SIGNATURE_1);
- 188:	85 e9       	ldi	r24, 0x95	; 149
- 18a:	0a d0       	rcall	.+20     	; 0x1a0 <putch>
+ 16c:	85 e9       	ldi	r24, 0x95	; 149
+ 16e:	0a d0       	rcall	.+20     	; 0x184 <putch>
       putch(SIGNATURE_2);
- 18c:	8f e0       	ldi	r24, 0x0F	; 15
- 18e:	8b cf       	rjmp	.-234    	; 0xa6 <__SREG__+0x67>
+ 170:	8f e0       	ldi	r24, 0x0F	; 15
+ 172:	98 cf       	rjmp	.-208    	; 0xa4 <__SREG__+0x65>
     }
     else if (ch == 'Q') {
- 190:	81 35       	cpi	r24, 0x51	; 81
- 192:	11 f4       	brne	.+4      	; 0x198 <__SREG__+0x159>
+ 174:	81 35       	cpi	r24, 0x51	; 81
+ 176:	11 f4       	brne	.+4      	; 0x17c <__SREG__+0x13d>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
- 194:	88 e0       	ldi	r24, 0x08	; 8
- 196:	19 d0       	rcall	.+50     	; 0x1ca <watchdogConfig>
+ 178:	88 e0       	ldi	r24, 0x08	; 8
+ 17a:	14 d0       	rcall	.+40     	; 0x1a4 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
- 198:	23 d0       	rcall	.+70     	; 0x1e0 <verifySpace>
+ 17c:	19 d0       	rcall	.+50     	; 0x1b0 <verifySpace>
     }
     putch(STK_OK);
- 19a:	80 e1       	ldi	r24, 0x10	; 16
- 19c:	01 d0       	rcall	.+2      	; 0x1a0 <putch>
- 19e:	5c cf       	rjmp	.-328    	; 0x58 <__SREG__+0x19>
+ 17e:	80 e1       	ldi	r24, 0x10	; 16
+ 180:	01 d0       	rcall	.+2      	; 0x184 <putch>
+ 182:	6a cf       	rjmp	.-300    	; 0x58 <__SREG__+0x19>
 
-000001a0 <putch>:
+00000184 <putch>:
   }
 }
 
 void putch(char ch) {
- 1a0:	98 2f       	mov	r25, r24
+ 184:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
- 1a2:	80 91 c0 00 	lds	r24, 0x00C0
- 1a6:	85 ff       	sbrs	r24, 5
- 1a8:	fc cf       	rjmp	.-8      	; 0x1a2 <putch+0x2>
+ 186:	80 91 c0 00 	lds	r24, 0x00C0
+ 18a:	85 ff       	sbrs	r24, 5
+ 18c:	fc cf       	rjmp	.-8      	; 0x186 <putch+0x2>
   UDR0 = ch;
- 1aa:	90 93 c6 00 	sts	0x00C6, r25
+ 18e:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
- 1ae:	08 95       	ret
+ 192:	08 95       	ret
 
-000001b0 <getch>:
-  return getch();
+00000194 <getch>:
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
- 1b0:	a8 95       	wdr
+ 194:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
- 1b2:	80 91 c0 00 	lds	r24, 0x00C0
- 1b6:	87 ff       	sbrs	r24, 7
- 1b8:	fc cf       	rjmp	.-8      	; 0x1b2 <getch+0x2>
+ 196:	80 91 c0 00 	lds	r24, 0x00C0
+ 19a:	87 ff       	sbrs	r24, 7
+ 19c:	fc cf       	rjmp	.-8      	; 0x196 <getch+0x2>
   ch = UDR0;
- 1ba:	80 91 c6 00 	lds	r24, 0x00C6
+ 19e:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
- 1be:	08 95       	ret
-
-000001c0 <getLen>:
-  } while (--count);
-}
-#endif
-
-uint8_t getLen() {
-  getch();
- 1c0:	f7 df       	rcall	.-18     	; 0x1b0 <getch>
-  length = getch();
- 1c2:	f6 df       	rcall	.-20     	; 0x1b0 <getch>
- 1c4:	80 93 02 02 	sts	0x0202, r24
-  return getch();
-}
- 1c8:	f3 cf       	rjmp	.-26     	; 0x1b0 <getch>
+ 1a2:	08 95       	ret
 
-000001ca <watchdogConfig>:
+000001a4 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
- 1ca:	e0 e6       	ldi	r30, 0x60	; 96
- 1cc:	f0 e0       	ldi	r31, 0x00	; 0
- 1ce:	98 e1       	ldi	r25, 0x18	; 24
- 1d0:	90 83       	st	Z, r25
+ 1a4:	e0 e6       	ldi	r30, 0x60	; 96
+ 1a6:	f0 e0       	ldi	r31, 0x00	; 0
+ 1a8:	98 e1       	ldi	r25, 0x18	; 24
+ 1aa:	90 83       	st	Z, r25
   WDTCSR = x;
- 1d2:	80 83       	st	Z, r24
+ 1ac:	80 83       	st	Z, r24
 }
- 1d4:	08 95       	ret
-
-000001d6 <appStart>:
-
-void appStart() {
-  watchdogConfig(WATCHDOG_OFF);
- 1d6:	80 e0       	ldi	r24, 0x00	; 0
- 1d8:	f8 df       	rcall	.-16     	; 0x1ca <watchdogConfig>
-  __asm__ __volatile__ (
- 1da:	ee 27       	eor	r30, r30
- 1dc:	ff 27       	eor	r31, r31
- 1de:	09 94       	ijmp
+ 1ae:	08 95       	ret
 
-000001e0 <verifySpace>:
+000001b0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
-  if (getch() != CRC_EOP) appStart();
- 1e0:	e7 df       	rcall	.-50     	; 0x1b0 <getch>
- 1e2:	80 32       	cpi	r24, 0x20	; 32
- 1e4:	09 f0       	breq	.+2      	; 0x1e8 <verifySpace+0x8>
- 1e6:	f7 df       	rcall	.-18     	; 0x1d6 <appStart>
+  if (getch() != CRC_EOP) {
+ 1b0:	f1 df       	rcall	.-30     	; 0x194 <getch>
+ 1b2:	80 32       	cpi	r24, 0x20	; 32
+ 1b4:	19 f0       	breq	.+6      	; 0x1bc <verifySpace+0xc>
+    watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
+ 1b6:	88 e0       	ldi	r24, 0x08	; 8
+ 1b8:	f5 df       	rcall	.-22     	; 0x1a4 <watchdogConfig>
+ 1ba:	ff cf       	rjmp	.-2      	; 0x1ba <verifySpace+0xa>
+    while (1)			      // and busy-loop so that WD causes
+      ;				      //  a reset and app start.
+  }
   putch(STK_INSYNC);
- 1e8:	84 e1       	ldi	r24, 0x14	; 20
+ 1bc:	84 e1       	ldi	r24, 0x14	; 20
 }
- 1ea:	da cf       	rjmp	.-76     	; 0x1a0 <putch>
+ 1be:	e2 cf       	rjmp	.-60     	; 0x184 <putch>
 
-000001ec <getNch>:
+000001c0 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
- 1ec:	1f 93       	push	r17
- 1ee:	18 2f       	mov	r17, r24
+ 1c0:	1f 93       	push	r17
+ 1c2:	18 2f       	mov	r17, r24
   do getch(); while (--count);
- 1f0:	df df       	rcall	.-66     	; 0x1b0 <getch>
- 1f2:	11 50       	subi	r17, 0x01	; 1
- 1f4:	e9 f7       	brne	.-6      	; 0x1f0 <getNch+0x4>
+ 1c4:	e7 df       	rcall	.-50     	; 0x194 <getch>
+ 1c6:	11 50       	subi	r17, 0x01	; 1
+ 1c8:	e9 f7       	brne	.-6      	; 0x1c4 <getNch+0x4>
   verifySpace();
- 1f6:	f4 df       	rcall	.-24     	; 0x1e0 <verifySpace>
+ 1ca:	f2 df       	rcall	.-28     	; 0x1b0 <verifySpace>
+}
+ 1cc:	1f 91       	pop	r17
+ 1ce:	08 95       	ret
+
+000001d0 <appStart>:
+  WDTCSR = _BV(WDCE) | _BV(WDE);
+  WDTCSR = x;
 }
- 1f8:	1f 91       	pop	r17
- 1fa:	08 95       	ret
+
+void appStart() {
+  watchdogConfig(WATCHDOG_OFF);
+ 1d0:	80 e0       	ldi	r24, 0x00	; 0
+ 1d2:	e8 df       	rcall	.-48     	; 0x1a4 <watchdogConfig>
+  __asm__ __volatile__ (
+ 1d4:	ee 27       	eor	r30, r30
+ 1d6:	ff 27       	eor	r31, r31
+ 1d8:	09 94       	ijmp
diff --git a/arduino/bootloaders/optiboot/optiboot_diecimila.hex b/arduino/bootloaders/optiboot/optiboot_diecimila.hex
index 3ac0de2..733a139 100644
--- a/arduino/bootloaders/optiboot/optiboot_diecimila.hex
+++ b/arduino/bootloaders/optiboot/optiboot_diecimila.hex
@@ -1,35 +1,33 @@
-:103E0000112484B714BE81FFE6D085E08093810041
+:103E0000112484B714BE81FFE3D085E08093810044
 :103E100082E08093C00088E18093C10086E08093B7
-:103E2000C20080E18093C4008EE0CFD0259A86E066
+:103E2000C20080E18093C4008EE0BCD0259A86E079
 :103E300020E33CEF91E0309385002093840096BB13
-:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D
-:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020
-:103E6000C5D083E020C0823411F484E103C08534DE
-:103E700019F485E0BBD091C0853581F499D0082F25
-:103E800010E096D090E0982F8827802B912B880FF8
-:103E9000991F90930102809300027EC0863529F419
-:103EA00084E0A4D080E07CD078C0843609F04EC095
-:103EB00087D0E0910002F091010288E3E030F8073A
-:103EC00018F483E087BFE895C0E0D1E071D0899312
-:103ED000809102028150809302028823B9F7E09119
-:103EE0000002F091010288E3E030F80718F083E067
-:103EF00087BFE89575D007B600FCFDCF4091000262
-:103F000050910102A0E0B1E02C9130E011968C912B
-:103F1000119790E0982F8827822B932B1296FA0105
-:103F20000C01D7BEE89511244E5F5F4FF1E0A03839
-:103F3000BF0751F7E0910002F0910102E7BEE8955A
-:103F400007B600FCFDCFF7BEE89527C08437B9F46B
-:103F500037D046D0E0910002F09101023196F09303
-:103F60000102E09300023197E4918E2F19D08091E5
-:103F70000202815080930202882361F70EC08537C8
-:103F800039F42ED08EE10CD084E90AD086E08BCFB4
-:103F9000813511F488E019D023D080E101D05CCFC5
-:103FA000982F8091C00085FFFCCF9093C6000895A4
-:103FB000A8958091C00087FFFCCF8091C60008952E
-:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E
-:103FD00090838083089580E0F8DFEE27FF2709941F
-:103FE000E7DF803209F0F7DF84E1DACF1F93182F83
-:0C3FF000DFDF1150E9F7F4DF1F910895A6
+:103E4000B09BFECF1D9AA8958150A9F79924939411
+:103E5000A5E0AA2EF1E1BF2E9DD0813421F481E0AE
+:103E6000AFD083E01FC0823411F484E103C08534F5
+:103E700019F485E0A5D083C0853579F48BD0E82E80
+:103E8000FF2488D0082F10E0102F00270E291F29AB
+:103E9000000F111F8DD0680172C0863529F484E0AF
+:103EA0008FD080E06FD06BC0843609F042C072D0F2
+:103EB00071D0082F6FD080E0C81688E3D80620F4B0
+:103EC00083E0F60187BFE895C0E0D1E063D0899335
+:103ED0000C17E1F7F0E0CF16F8E3DF0620F083E0FF
+:103EE000F60187BFE89564D007B600FCFDCFA601B8
+:103EF000A0E0B1E02C9130E011968C91119790E008
+:103F0000982F8827822B932B1296FA010C0197BECB
+:103F1000E89511244E5F5F4FF1E0A038BF0751F7DD
+:103F2000F601A7BEE89507B600FCFDCFB7BEE89541
+:103F300026C08437B1F42ED02DD0F82E2BD038D017
+:103F4000F601EF2C8F010F5F1F4F84911BD0EA9475
+:103F5000F801C1F70894C11CD11CFA94CF0CD11CF4
+:103F60000EC0853739F424D08EE10CD084E90AD014
+:103F700086E098CF813511F488E014D019D080E123
+:103F800001D06ACF982F8091C00085FFFCCF90931D
+:103F9000C6000895A8958091C00087FFFCCF80914E
+:103FA000C6000895E0E6F0E098E1908380830895EC
+:103FB000F1DF803219F088E0F5DFFFCF84E1E2CF56
+:103FC0001F93182FE7DF1150E9F7F2DF1F910895D3
+:0A3FD00080E0E8DFEE27FF270994E8
 :023FFE000104BC
 :0400000300003E00BB
 :00000001FF
diff --git a/arduino/bootloaders/optiboot/optiboot_diecimila.lst b/arduino/bootloaders/optiboot/optiboot_diecimila.lst
index 8b17368..edd9cb3 100644
--- a/arduino/bootloaders/optiboot/optiboot_diecimila.lst
+++ b/arduino/bootloaders/optiboot/optiboot_diecimila.lst
@@ -3,27 +3,27 @@ optiboot_diecimila.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001fc  00003e00  00003e00  00000054  2**1
+  0 .text         000001da  00003e00  00003e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00003ffe  00003ffe  00000250  2**0
+  1 .version      00000002  00003ffe  00003ffe  0000022e  2**0
                   CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000252  2**0
+  2 .debug_aranges 00000028  00000000  00000000  00000230  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000006a  00000000  00000000  0000027a  2**0
+  3 .debug_pubnames 0000005f  00000000  00000000  00000258  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000285  00000000  00000000  000002e4  2**0
+  4 .debug_info   0000028c  00000000  00000000  000002b7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000019f  00000000  00000000  00000569  2**0
+  5 .debug_abbrev 00000199  00000000  00000000  00000543  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000453  00000000  00000000  00000708  2**0
+  6 .debug_line   00000456  00000000  00000000  000006dc  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000090  00000000  00000000  00000b5c  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000b34  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000141  00000000  00000000  00000bec  2**0
+  8 .debug_str    00000149  00000000  00000000  00000bb4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    000001e1  00000000  00000000  00000d2d  2**0
+  9 .debug_loc    0000027e  00000000  00000000  00000cfd  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000068  00000000  00000000  00000f0e  2**0
+ 10 .debug_ranges 00000060  00000000  00000000  00000f7b  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
@@ -47,7 +47,7 @@ int main(void) {
     3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
     3e06:	81 ff       	sbrs	r24, 1
-    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+    3e08:	e3 d0       	rcall	.+454    	; 0x3fd0 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
@@ -77,7 +77,7 @@ int main(void) {
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
     3e28:	8e e0       	ldi	r24, 0x0E	; 14
-    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
+    3e2a:	bc d0       	rcall	.+376    	; 0x3fa4 <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
@@ -111,8 +111,8 @@ void flash_led(uint8_t count) {
 #else
     LED_PIN |= _BV(LED);
     3e44:	1d 9a       	sbi	0x03, 5	; 3
-  return getch();
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
@@ -132,8 +132,8 @@ void watchdogReset() {
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e4c:	dd 24       	eor	r13, r13
-    3e4e:	d3 94       	inc	r13
+    3e4c:	99 24       	eor	r9, r9
+    3e4e:	93 94       	inc	r9
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
@@ -141,21 +141,21 @@ void watchdogReset() {
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
     3e50:	a5 e0       	ldi	r26, 0x05	; 5
-    3e52:	ea 2e       	mov	r14, r26
+    3e52:	aa 2e       	mov	r10, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
     3e54:	f1 e1       	ldi	r31, 0x11	; 17
-    3e56:	ff 2e       	mov	r15, r31
+    3e56:	bf 2e       	mov	r11, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
+    3e58:	9d d0       	rcall	.+314    	; 0x3f94 <getch>
 
     if(ch == STK_GET_PARAMETER) {
     3e5a:	81 34       	cpi	r24, 0x41	; 65
@@ -163,10 +163,10 @@ void watchdogReset() {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
     3e5e:	81 e0       	ldi	r24, 0x01	; 1
-    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
+    3e60:	af d0       	rcall	.+350    	; 0x3fc0 <getNch>
       putch(0x03);
     3e62:	83 e0       	ldi	r24, 0x03	; 3
-    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
+    3e64:	1f c0       	rjmp	.+62     	; 0x3ea4 <main+0xa4>
     }
     else if(ch == STK_SET_DEVICE) {
     3e66:	82 34       	cpi	r24, 0x42	; 66
@@ -182,71 +182,77 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     3e72:	85 e0       	ldi	r24, 0x05	; 5
-    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
-    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
+    3e74:	a5 d0       	rcall	.+330    	; 0x3fc0 <getNch>
+    3e76:	83 c0       	rjmp	.+262    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     3e78:	85 35       	cpi	r24, 0x55	; 85
-    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
+    3e7a:	79 f4       	brne	.+30     	; 0x3e9a <main+0x9a>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+    3e7c:	8b d0       	rcall	.+278    	; 0x3f94 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-    3e7e:	08 2f       	mov	r16, r24
-    3e80:	10 e0       	ldi	r17, 0x00	; 0
-    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
-    3e84:	90 e0       	ldi	r25, 0x00	; 0
-    3e86:	98 2f       	mov	r25, r24
-    3e88:	88 27       	eor	r24, r24
-    3e8a:	80 2b       	or	r24, r16
-    3e8c:	91 2b       	or	r25, r17
+    3e7e:	e8 2e       	mov	r14, r24
+    3e80:	ff 24       	eor	r15, r15
+    3e82:	88 d0       	rcall	.+272    	; 0x3f94 <getch>
+    3e84:	08 2f       	mov	r16, r24
+    3e86:	10 e0       	ldi	r17, 0x00	; 0
+    3e88:	10 2f       	mov	r17, r16
+    3e8a:	00 27       	eor	r16, r16
+    3e8c:	0e 29       	or	r16, r14
+    3e8e:	1f 29       	or	r17, r15
 #ifdef RAMPZ
       // Transfer top bit to RAMPZ
       RAMPZ = (newAddress & 0x8000) ? 1 : 0;
 #endif
       newAddress += newAddress; // Convert from word address to byte address
-    3e8e:	88 0f       	add	r24, r24
-    3e90:	99 1f       	adc	r25, r25
+    3e90:	00 0f       	add	r16, r16
+    3e92:	11 1f       	adc	r17, r17
       address = newAddress;
-    3e92:	90 93 01 02 	sts	0x0201, r25
-    3e96:	80 93 00 02 	sts	0x0200, r24
-    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
+    3e94:	8d d0       	rcall	.+282    	; 0x3fb0 <verifySpace>
+    3e96:	68 01       	movw	r12, r16
+    3e98:	72 c0       	rjmp	.+228    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_UNIVERSAL) {
-    3e9c:	86 35       	cpi	r24, 0x56	; 86
-    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
+    3e9a:	86 35       	cpi	r24, 0x56	; 86
+    3e9c:	29 f4       	brne	.+10     	; 0x3ea8 <main+0xa8>
       // UNIVERSAL command is ignored
       getNch(4);
-    3ea0:	84 e0       	ldi	r24, 0x04	; 4
-    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
+    3e9e:	84 e0       	ldi	r24, 0x04	; 4
+    3ea0:	8f d0       	rcall	.+286    	; 0x3fc0 <getNch>
       putch(0x00);
-    3ea4:	80 e0       	ldi	r24, 0x00	; 0
-    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
-    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
+    3ea2:	80 e0       	ldi	r24, 0x00	; 0
+    3ea4:	6f d0       	rcall	.+222    	; 0x3f84 <putch>
+    3ea6:	6b c0       	rjmp	.+214    	; 0x3f7e <main+0x17e>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3eaa:	84 36       	cpi	r24, 0x64	; 100
-    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
-    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
+    3ea8:	84 36       	cpi	r24, 0x64	; 100
+    3eaa:	09 f0       	breq	.+2      	; 0x3eae <main+0xae>
+    3eac:	42 c0       	rjmp	.+132    	; 0x3f32 <main+0x132>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
-      getLen();
-    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3eae:	72 d0       	rcall	.+228    	; 0x3f94 <getch>
+      length = getch();
+    3eb0:	71 d0       	rcall	.+226    	; 0x3f94 <getch>
+    3eb2:	08 2f       	mov	r16, r24
+      getch();
+    3eb4:	6f d0       	rcall	.+222    	; 0x3f94 <getch>
 
       // If we are in RWW section, immediately start page erase
       if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3eb2:	e0 91 00 02 	lds	r30, 0x0200
-    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eb6:	80 e0       	ldi	r24, 0x00	; 0
+    3eb8:	c8 16       	cp	r12, r24
     3eba:	88 e3       	ldi	r24, 0x38	; 56
-    3ebc:	e0 30       	cpi	r30, 0x00	; 0
-    3ebe:	f8 07       	cpc	r31, r24
-    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
-    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ebc:	d8 06       	cpc	r13, r24
+    3ebe:	20 f4       	brcc	.+8      	; 0x3ec8 <main+0xc8>
+    3ec0:	83 e0       	ldi	r24, 0x03	; 3
+    3ec2:	f6 01       	movw	r30, r12
     3ec4:	87 bf       	out	0x37, r24	; 55
     3ec6:	e8 95       	spm
     3ec8:	c0 e0       	ldi	r28, 0x00	; 0
@@ -255,302 +261,301 @@ void watchdogReset() {
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
+    3ecc:	63 d0       	rcall	.+198    	; 0x3f94 <getch>
     3ece:	89 93       	st	Y+, r24
       while (--length);
-    3ed0:	80 91 02 02 	lds	r24, 0x0202
-    3ed4:	81 50       	subi	r24, 0x01	; 1
-    3ed6:	80 93 02 02 	sts	0x0202, r24
-    3eda:	88 23       	and	r24, r24
-    3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
+    3ed0:	0c 17       	cp	r16, r28
+    3ed2:	e1 f7       	brne	.-8      	; 0x3ecc <main+0xcc>
 
       // If we are in NRWW section, page erase has to be delayed until now.
       // Todo: Take RAMPZ into account
       if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ede:	e0 91 00 02 	lds	r30, 0x0200
-    3ee2:	f0 91 01 02 	lds	r31, 0x0201
-    3ee6:	88 e3       	ldi	r24, 0x38	; 56
-    3ee8:	e0 30       	cpi	r30, 0x00	; 0
-    3eea:	f8 07       	cpc	r31, r24
-    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
-    3eee:	83 e0       	ldi	r24, 0x03	; 3
-    3ef0:	87 bf       	out	0x37, r24	; 55
-    3ef2:	e8 95       	spm
+    3ed4:	f0 e0       	ldi	r31, 0x00	; 0
+    3ed6:	cf 16       	cp	r12, r31
+    3ed8:	f8 e3       	ldi	r31, 0x38	; 56
+    3eda:	df 06       	cpc	r13, r31
+    3edc:	20 f0       	brcs	.+8      	; 0x3ee6 <main+0xe6>
+    3ede:	83 e0       	ldi	r24, 0x03	; 3
+    3ee0:	f6 01       	movw	r30, r12
+    3ee2:	87 bf       	out	0x37, r24	; 55
+    3ee4:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
+    3ee6:	64 d0       	rcall	.+200    	; 0x3fb0 <verifySpace>
 
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3ef6:	07 b6       	in	r0, 0x37	; 55
-    3ef8:	00 fc       	sbrc	r0, 0
-    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
-      }
-#endif
-
-      // Copy buffer into programming buffer
+    3ee8:	07 b6       	in	r0, 0x37	; 55
+    3eea:	00 fc       	sbrc	r0, 0
+    3eec:	fd cf       	rjmp	.-6      	; 0x3ee8 <main+0xe8>
+    3eee:	a6 01       	movw	r20, r12
+    3ef0:	a0 e0       	ldi	r26, 0x00	; 0
+    3ef2:	b1 e0       	ldi	r27, 0x01	; 1
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3efc:	40 91 00 02 	lds	r20, 0x0200
-    3f00:	50 91 01 02 	lds	r21, 0x0201
-    3f04:	a0 e0       	ldi	r26, 0x00	; 0
-    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3f08:	2c 91       	ld	r18, X
-    3f0a:	30 e0       	ldi	r19, 0x00	; 0
+    3ef4:	2c 91       	ld	r18, X
+    3ef6:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3f0c:	11 96       	adiw	r26, 0x01	; 1
-    3f0e:	8c 91       	ld	r24, X
-    3f10:	11 97       	sbiw	r26, 0x01	; 1
-    3f12:	90 e0       	ldi	r25, 0x00	; 0
-    3f14:	98 2f       	mov	r25, r24
-    3f16:	88 27       	eor	r24, r24
-    3f18:	82 2b       	or	r24, r18
-    3f1a:	93 2b       	or	r25, r19
+    3ef8:	11 96       	adiw	r26, 0x01	; 1
+    3efa:	8c 91       	ld	r24, X
+    3efc:	11 97       	sbiw	r26, 0x01	; 1
+    3efe:	90 e0       	ldi	r25, 0x00	; 0
+    3f00:	98 2f       	mov	r25, r24
+    3f02:	88 27       	eor	r24, r24
+    3f04:	82 2b       	or	r24, r18
+    3f06:	93 2b       	or	r25, r19
 #define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
 #define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
 /* main program starts here */
 int main(void) {
-    3f1c:	12 96       	adiw	r26, 0x02	; 2
+    3f08:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    3f1e:	fa 01       	movw	r30, r20
-    3f20:	0c 01       	movw	r0, r24
-    3f22:	d7 be       	out	0x37, r13	; 55
-    3f24:	e8 95       	spm
-    3f26:	11 24       	eor	r1, r1
+    3f0a:	fa 01       	movw	r30, r20
+    3f0c:	0c 01       	movw	r0, r24
+    3f0e:	97 be       	out	0x37, r9	; 55
+    3f10:	e8 95       	spm
+    3f12:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3f28:	4e 5f       	subi	r20, 0xFE	; 254
-    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f14:	4e 5f       	subi	r20, 0xFE	; 254
+    3f16:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
-    3f2e:	a0 38       	cpi	r26, 0x80	; 128
-    3f30:	bf 07       	cpc	r27, r31
-    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
+    3f18:	f1 e0       	ldi	r31, 0x01	; 1
+    3f1a:	a0 38       	cpi	r26, 0x80	; 128
+    3f1c:	bf 07       	cpc	r27, r31
+    3f1e:	51 f7       	brne	.-44     	; 0x3ef4 <main+0xf4>
 
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
-    3f34:	e0 91 00 02 	lds	r30, 0x0200
-    3f38:	f0 91 01 02 	lds	r31, 0x0201
-    3f3c:	e7 be       	out	0x37, r14	; 55
-    3f3e:	e8 95       	spm
+    3f20:	f6 01       	movw	r30, r12
+    3f22:	a7 be       	out	0x37, r10	; 55
+    3f24:	e8 95       	spm
       boot_spm_busy_wait();
-    3f40:	07 b6       	in	r0, 0x37	; 55
-    3f42:	00 fc       	sbrc	r0, 0
-    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
+    3f26:	07 b6       	in	r0, 0x37	; 55
+    3f28:	00 fc       	sbrc	r0, 0
+    3f2a:	fd cf       	rjmp	.-6      	; 0x3f26 <main+0x126>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f46:	f7 be       	out	0x37, r15	; 55
-    3f48:	e8 95       	spm
-    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
+    3f2c:	b7 be       	out	0x37, r11	; 55
+    3f2e:	e8 95       	spm
+    3f30:	26 c0       	rjmp	.+76     	; 0x3f7e <main+0x17e>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f4c:	84 37       	cpi	r24, 0x74	; 116
-    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
+    3f32:	84 37       	cpi	r24, 0x74	; 116
+    3f34:	b1 f4       	brne	.+44     	; 0x3f62 <main+0x162>
       // READ PAGE - we only read flash
-      getLen();
-    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3f36:	2e d0       	rcall	.+92     	; 0x3f94 <getch>
+      length = getch();
+    3f38:	2d d0       	rcall	.+90     	; 0x3f94 <getch>
+    3f3a:	f8 2e       	mov	r15, r24
+      getch();
+    3f3c:	2b d0       	rcall	.+86     	; 0x3f94 <getch>
+
       verifySpace();
-    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+    3f3e:	38 d0       	rcall	.+112    	; 0x3fb0 <verifySpace>
+    3f40:	f6 01       	movw	r30, r12
+    3f42:	ef 2c       	mov	r14, r15
         putch(result);
         address++;
       }
       while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f54:	e0 91 00 02 	lds	r30, 0x0200
-    3f58:	f0 91 01 02 	lds	r31, 0x0201
-    3f5c:	31 96       	adiw	r30, 0x01	; 1
-    3f5e:	f0 93 01 02 	sts	0x0201, r31
-    3f62:	e0 93 00 02 	sts	0x0200, r30
-    3f66:	31 97       	sbiw	r30, 0x01	; 1
-    3f68:	e4 91       	lpm	r30, Z+
-    3f6a:	8e 2f       	mov	r24, r30
-    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
+    3f44:	8f 01       	movw	r16, r30
+    3f46:	0f 5f       	subi	r16, 0xFF	; 255
+    3f48:	1f 4f       	sbci	r17, 0xFF	; 255
+    3f4a:	84 91       	lpm	r24, Z+
+    3f4c:	1b d0       	rcall	.+54     	; 0x3f84 <putch>
       while (--length);
-    3f6e:	80 91 02 02 	lds	r24, 0x0202
-    3f72:	81 50       	subi	r24, 0x01	; 1
-    3f74:	80 93 02 02 	sts	0x0202, r24
-    3f78:	88 23       	and	r24, r24
-    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
-    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+    3f4e:	ea 94       	dec	r14
+    3f50:	f8 01       	movw	r30, r16
+    3f52:	c1 f7       	brne	.-16     	; 0x3f44 <main+0x144>
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
+#endif
+
+/* main program starts here */
+int main(void) {
+    3f54:	08 94       	sec
+    3f56:	c1 1c       	adc	r12, r1
+    3f58:	d1 1c       	adc	r13, r1
+    3f5a:	fa 94       	dec	r15
+    3f5c:	cf 0c       	add	r12, r15
+    3f5e:	d1 1c       	adc	r13, r1
+    3f60:	0e c0       	rjmp	.+28     	; 0x3f7e <main+0x17e>
 #endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f7e:	85 37       	cpi	r24, 0x75	; 117
-    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
+    3f62:	85 37       	cpi	r24, 0x75	; 117
+    3f64:	39 f4       	brne	.+14     	; 0x3f74 <main+0x174>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
+    3f66:	24 d0       	rcall	.+72     	; 0x3fb0 <verifySpace>
       putch(SIGNATURE_0);
-    3f84:	8e e1       	ldi	r24, 0x1E	; 30
-    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
+    3f68:	8e e1       	ldi	r24, 0x1E	; 30
+    3f6a:	0c d0       	rcall	.+24     	; 0x3f84 <putch>
       putch(SIGNATURE_1);
-    3f88:	84 e9       	ldi	r24, 0x94	; 148
-    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
+    3f6c:	84 e9       	ldi	r24, 0x94	; 148
+    3f6e:	0a d0       	rcall	.+20     	; 0x3f84 <putch>
       putch(SIGNATURE_2);
-    3f8c:	86 e0       	ldi	r24, 0x06	; 6
-    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
+    3f70:	86 e0       	ldi	r24, 0x06	; 6
+    3f72:	98 cf       	rjmp	.-208    	; 0x3ea4 <main+0xa4>
     }
     else if (ch == 'Q') {
-    3f90:	81 35       	cpi	r24, 0x51	; 81
-    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
+    3f74:	81 35       	cpi	r24, 0x51	; 81
+    3f76:	11 f4       	brne	.+4      	; 0x3f7c <main+0x17c>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f94:	88 e0       	ldi	r24, 0x08	; 8
-    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
+    3f78:	88 e0       	ldi	r24, 0x08	; 8
+    3f7a:	14 d0       	rcall	.+40     	; 0x3fa4 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
+    3f7c:	19 d0       	rcall	.+50     	; 0x3fb0 <verifySpace>
     }
     putch(STK_OK);
-    3f9a:	80 e1       	ldi	r24, 0x10	; 16
-    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
-    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
+    3f7e:	80 e1       	ldi	r24, 0x10	; 16
+    3f80:	01 d0       	rcall	.+2      	; 0x3f84 <putch>
+    3f82:	6a cf       	rjmp	.-300    	; 0x3e58 <main+0x58>
 
-00003fa0 <putch>:
+00003f84 <putch>:
   }
 }
 
 void putch(char ch) {
-    3fa0:	98 2f       	mov	r25, r24
+    3f84:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fa6:	85 ff       	sbrs	r24, 5
-    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
+    3f86:	80 91 c0 00 	lds	r24, 0x00C0
+    3f8a:	85 ff       	sbrs	r24, 5
+    3f8c:	fc cf       	rjmp	.-8      	; 0x3f86 <putch+0x2>
   UDR0 = ch;
-    3faa:	90 93 c6 00 	sts	0x00C6, r25
+    3f8e:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3fae:	08 95       	ret
+    3f92:	08 95       	ret
 
-00003fb0 <getch>:
-  return getch();
+00003f94 <getch>:
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fb0:	a8 95       	wdr
+    3f94:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fb6:	87 ff       	sbrs	r24, 7
-    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
+    3f96:	80 91 c0 00 	lds	r24, 0x00C0
+    3f9a:	87 ff       	sbrs	r24, 7
+    3f9c:	fc cf       	rjmp	.-8      	; 0x3f96 <getch+0x2>
   ch = UDR0;
-    3fba:	80 91 c6 00 	lds	r24, 0x00C6
+    3f9e:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    3fbe:	08 95       	ret
-
-00003fc0 <getLen>:
-  } while (--count);
-}
-#endif
-
-uint8_t getLen() {
-  getch();
-    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
-  length = getch();
-    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
-    3fc4:	80 93 02 02 	sts	0x0202, r24
-  return getch();
-}
-    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
+    3fa2:	08 95       	ret
 
-00003fca <watchdogConfig>:
+00003fa4 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3fca:	e0 e6       	ldi	r30, 0x60	; 96
-    3fcc:	f0 e0       	ldi	r31, 0x00	; 0
-    3fce:	98 e1       	ldi	r25, 0x18	; 24
-    3fd0:	90 83       	st	Z, r25
+    3fa4:	e0 e6       	ldi	r30, 0x60	; 96
+    3fa6:	f0 e0       	ldi	r31, 0x00	; 0
+    3fa8:	98 e1       	ldi	r25, 0x18	; 24
+    3faa:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fd2:	80 83       	st	Z, r24
+    3fac:	80 83       	st	Z, r24
 }
-    3fd4:	08 95       	ret
-
-00003fd6 <appStart>:
-
-void appStart() {
-  watchdogConfig(WATCHDOG_OFF);
-    3fd6:	80 e0       	ldi	r24, 0x00	; 0
-    3fd8:	f8 df       	rcall	.-16     	; 0x3fca <watchdogConfig>
-  __asm__ __volatile__ (
-    3fda:	ee 27       	eor	r30, r30
-    3fdc:	ff 27       	eor	r31, r31
-    3fde:	09 94       	ijmp
+    3fae:	08 95       	ret
 
-00003fe0 <verifySpace>:
+00003fb0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
-  if (getch() != CRC_EOP) appStart();
-    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
-    3fe2:	80 32       	cpi	r24, 0x20	; 32
-    3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
-    3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
+  if (getch() != CRC_EOP) {
+    3fb0:	f1 df       	rcall	.-30     	; 0x3f94 <getch>
+    3fb2:	80 32       	cpi	r24, 0x20	; 32
+    3fb4:	19 f0       	breq	.+6      	; 0x3fbc <verifySpace+0xc>
+    watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
+    3fb6:	88 e0       	ldi	r24, 0x08	; 8
+    3fb8:	f5 df       	rcall	.-22     	; 0x3fa4 <watchdogConfig>
+    3fba:	ff cf       	rjmp	.-2      	; 0x3fba <verifySpace+0xa>
+    while (1)			      // and busy-loop so that WD causes
+      ;				      //  a reset and app start.
+  }
   putch(STK_INSYNC);
-    3fe8:	84 e1       	ldi	r24, 0x14	; 20
+    3fbc:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+    3fbe:	e2 cf       	rjmp	.-60     	; 0x3f84 <putch>
 
-00003fec <getNch>:
+00003fc0 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fec:	1f 93       	push	r17
-    3fee:	18 2f       	mov	r17, r24
+    3fc0:	1f 93       	push	r17
+    3fc2:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
-    3ff2:	11 50       	subi	r17, 0x01	; 1
-    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
+    3fc4:	e7 df       	rcall	.-50     	; 0x3f94 <getch>
+    3fc6:	11 50       	subi	r17, 0x01	; 1
+    3fc8:	e9 f7       	brne	.-6      	; 0x3fc4 <getNch+0x4>
   verifySpace();
-    3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
+    3fca:	f2 df       	rcall	.-28     	; 0x3fb0 <verifySpace>
+}
+    3fcc:	1f 91       	pop	r17
+    3fce:	08 95       	ret
+
+00003fd0 <appStart>:
+  WDTCSR = _BV(WDCE) | _BV(WDE);
+  WDTCSR = x;
 }
-    3ff8:	1f 91       	pop	r17
-    3ffa:	08 95       	ret
+
+void appStart() {
+  watchdogConfig(WATCHDOG_OFF);
+    3fd0:	80 e0       	ldi	r24, 0x00	; 0
+    3fd2:	e8 df       	rcall	.-48     	; 0x3fa4 <watchdogConfig>
+  __asm__ __volatile__ (
+    3fd4:	ee 27       	eor	r30, r30
+    3fd6:	ff 27       	eor	r31, r31
+    3fd8:	09 94       	ijmp
diff --git a/arduino/bootloaders/optiboot/optiboot_lilypad.hex b/arduino/bootloaders/optiboot/optiboot_lilypad.hex
index 77897e7..dcdbbb4 100644
--- a/arduino/bootloaders/optiboot/optiboot_lilypad.hex
+++ b/arduino/bootloaders/optiboot/optiboot_lilypad.hex
@@ -1,35 +1,33 @@
-:103E0000112484B714BE81FFE6D085E08093810041
+:103E0000112484B714BE81FFE3D085E08093810044
 :103E100082E08093C00088E18093C10086E08093B7
-:103E2000C20088E08093C4008EE0CFD0259A86E05F
+:103E2000C20088E08093C4008EE0BCD0259A86E072
 :103E300028E13EEF91E0309385002093840096BB0B
-:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D
-:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020
-:103E6000C5D083E020C0823411F484E103C08534DE
-:103E700019F485E0BBD091C0853581F499D0082F25
-:103E800010E096D090E0982F8827802B912B880FF8
-:103E9000991F90930102809300027EC0863529F419
-:103EA00084E0A4D080E07CD078C0843609F04EC095
-:103EB00087D0E0910002F091010288E3E030F8073A
-:103EC00018F483E087BFE895C0E0D1E071D0899312
-:103ED000809102028150809302028823B9F7E09119
-:103EE0000002F091010288E3E030F80718F083E067
-:103EF00087BFE89575D007B600FCFDCF4091000262
-:103F000050910102A0E0B1E02C9130E011968C912B
-:103F1000119790E0982F8827822B932B1296FA0105
-:103F20000C01D7BEE89511244E5F5F4FF1E0A03839
-:103F3000BF0751F7E0910002F0910102E7BEE8955A
-:103F400007B600FCFDCFF7BEE89527C08437B9F46B
-:103F500037D046D0E0910002F09101023196F09303
-:103F60000102E09300023197E4918E2F19D08091E5
-:103F70000202815080930202882361F70EC08537C8
-:103F800039F42ED08EE10CD084E90AD086E08BCFB4
-:103F9000813511F488E019D023D080E101D05CCFC5
-:103FA000982F8091C00085FFFCCF9093C6000895A4
-:103FB000A8958091C00087FFFCCF8091C60008952E
-:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E
-:103FD00090838083089580E0F8DFEE27FF2709941F
-:103FE000E7DF803209F0F7DF84E1DACF1F93182F83
-:0C3FF000DFDF1150E9F7F4DF1F910895A6
+:103E4000B09BFECF1D9AA8958150A9F79924939411
+:103E5000A5E0AA2EF1E1BF2E9DD0813421F481E0AE
+:103E6000AFD083E01FC0823411F484E103C08534F5
+:103E700019F485E0A5D083C0853579F48BD0E82E80
+:103E8000FF2488D0082F10E0102F00270E291F29AB
+:103E9000000F111F8DD0680172C0863529F484E0AF
+:103EA0008FD080E06FD06BC0843609F042C072D0F2
+:103EB00071D0082F6FD080E0C81688E3D80620F4B0
+:103EC00083E0F60187BFE895C0E0D1E063D0899335
+:103ED0000C17E1F7F0E0CF16F8E3DF0620F083E0FF
+:103EE000F60187BFE89564D007B600FCFDCFA601B8
+:103EF000A0E0B1E02C9130E011968C91119790E008
+:103F0000982F8827822B932B1296FA010C0197BECB
+:103F1000E89511244E5F5F4FF1E0A038BF0751F7DD
+:103F2000F601A7BEE89507B600FCFDCFB7BEE89541
+:103F300026C08437B1F42ED02DD0F82E2BD038D017
+:103F4000F601EF2C8F010F5F1F4F84911BD0EA9475
+:103F5000F801C1F70894C11CD11CFA94CF0CD11CF4
+:103F60000EC0853739F424D08EE10CD084E90AD014
+:103F700086E098CF813511F488E014D019D080E123
+:103F800001D06ACF982F8091C00085FFFCCF90931D
+:103F9000C6000895A8958091C00087FFFCCF80914E
+:103FA000C6000895E0E6F0E098E1908380830895EC
+:103FB000F1DF803219F088E0F5DFFFCF84E1E2CF56
+:103FC0001F93182FE7DF1150E9F7F2DF1F910895D3
+:0A3FD00080E0E8DFEE27FF270994E8
 :023FFE000104BC
 :0400000300003E00BB
 :00000001FF
diff --git a/arduino/bootloaders/optiboot/optiboot_lilypad.lst b/arduino/bootloaders/optiboot/optiboot_lilypad.lst
index 75180bc..a70713a 100644
--- a/arduino/bootloaders/optiboot/optiboot_lilypad.lst
+++ b/arduino/bootloaders/optiboot/optiboot_lilypad.lst
@@ -3,27 +3,27 @@ optiboot_lilypad.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001fc  00003e00  00003e00  00000054  2**1
+  0 .text         000001da  00003e00  00003e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00003ffe  00003ffe  00000250  2**0
+  1 .version      00000002  00003ffe  00003ffe  0000022e  2**0
                   CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000252  2**0
+  2 .debug_aranges 00000028  00000000  00000000  00000230  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000006a  00000000  00000000  0000027a  2**0
+  3 .debug_pubnames 0000005f  00000000  00000000  00000258  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000285  00000000  00000000  000002e4  2**0
+  4 .debug_info   0000028c  00000000  00000000  000002b7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000019f  00000000  00000000  00000569  2**0
+  5 .debug_abbrev 00000199  00000000  00000000  00000543  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000453  00000000  00000000  00000708  2**0
+  6 .debug_line   00000456  00000000  00000000  000006dc  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000090  00000000  00000000  00000b5c  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000b34  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000141  00000000  00000000  00000bec  2**0
+  8 .debug_str    00000149  00000000  00000000  00000bb4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    000001e1  00000000  00000000  00000d2d  2**0
+  9 .debug_loc    0000027e  00000000  00000000  00000cfd  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000068  00000000  00000000  00000f0e  2**0
+ 10 .debug_ranges 00000060  00000000  00000000  00000f7b  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
@@ -47,7 +47,7 @@ int main(void) {
     3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
     3e06:	81 ff       	sbrs	r24, 1
-    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+    3e08:	e3 d0       	rcall	.+454    	; 0x3fd0 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
@@ -77,7 +77,7 @@ int main(void) {
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
     3e28:	8e e0       	ldi	r24, 0x0E	; 14
-    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
+    3e2a:	bc d0       	rcall	.+376    	; 0x3fa4 <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
@@ -111,8 +111,8 @@ void flash_led(uint8_t count) {
 #else
     LED_PIN |= _BV(LED);
     3e44:	1d 9a       	sbi	0x03, 5	; 3
-  return getch();
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
@@ -132,8 +132,8 @@ void watchdogReset() {
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e4c:	dd 24       	eor	r13, r13
-    3e4e:	d3 94       	inc	r13
+    3e4c:	99 24       	eor	r9, r9
+    3e4e:	93 94       	inc	r9
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
@@ -141,21 +141,21 @@ void watchdogReset() {
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
     3e50:	a5 e0       	ldi	r26, 0x05	; 5
-    3e52:	ea 2e       	mov	r14, r26
+    3e52:	aa 2e       	mov	r10, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
     3e54:	f1 e1       	ldi	r31, 0x11	; 17
-    3e56:	ff 2e       	mov	r15, r31
+    3e56:	bf 2e       	mov	r11, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
+    3e58:	9d d0       	rcall	.+314    	; 0x3f94 <getch>
 
     if(ch == STK_GET_PARAMETER) {
     3e5a:	81 34       	cpi	r24, 0x41	; 65
@@ -163,10 +163,10 @@ void watchdogReset() {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
     3e5e:	81 e0       	ldi	r24, 0x01	; 1
-    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
+    3e60:	af d0       	rcall	.+350    	; 0x3fc0 <getNch>
       putch(0x03);
     3e62:	83 e0       	ldi	r24, 0x03	; 3
-    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
+    3e64:	1f c0       	rjmp	.+62     	; 0x3ea4 <main+0xa4>
     }
     else if(ch == STK_SET_DEVICE) {
     3e66:	82 34       	cpi	r24, 0x42	; 66
@@ -182,71 +182,77 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     3e72:	85 e0       	ldi	r24, 0x05	; 5
-    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
-    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
+    3e74:	a5 d0       	rcall	.+330    	; 0x3fc0 <getNch>
+    3e76:	83 c0       	rjmp	.+262    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     3e78:	85 35       	cpi	r24, 0x55	; 85
-    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
+    3e7a:	79 f4       	brne	.+30     	; 0x3e9a <main+0x9a>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+    3e7c:	8b d0       	rcall	.+278    	; 0x3f94 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-    3e7e:	08 2f       	mov	r16, r24
-    3e80:	10 e0       	ldi	r17, 0x00	; 0
-    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
-    3e84:	90 e0       	ldi	r25, 0x00	; 0
-    3e86:	98 2f       	mov	r25, r24
-    3e88:	88 27       	eor	r24, r24
-    3e8a:	80 2b       	or	r24, r16
-    3e8c:	91 2b       	or	r25, r17
+    3e7e:	e8 2e       	mov	r14, r24
+    3e80:	ff 24       	eor	r15, r15
+    3e82:	88 d0       	rcall	.+272    	; 0x3f94 <getch>
+    3e84:	08 2f       	mov	r16, r24
+    3e86:	10 e0       	ldi	r17, 0x00	; 0
+    3e88:	10 2f       	mov	r17, r16
+    3e8a:	00 27       	eor	r16, r16
+    3e8c:	0e 29       	or	r16, r14
+    3e8e:	1f 29       	or	r17, r15
 #ifdef RAMPZ
       // Transfer top bit to RAMPZ
       RAMPZ = (newAddress & 0x8000) ? 1 : 0;
 #endif
       newAddress += newAddress; // Convert from word address to byte address
-    3e8e:	88 0f       	add	r24, r24
-    3e90:	99 1f       	adc	r25, r25
+    3e90:	00 0f       	add	r16, r16
+    3e92:	11 1f       	adc	r17, r17
       address = newAddress;
-    3e92:	90 93 01 02 	sts	0x0201, r25
-    3e96:	80 93 00 02 	sts	0x0200, r24
-    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
+    3e94:	8d d0       	rcall	.+282    	; 0x3fb0 <verifySpace>
+    3e96:	68 01       	movw	r12, r16
+    3e98:	72 c0       	rjmp	.+228    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_UNIVERSAL) {
-    3e9c:	86 35       	cpi	r24, 0x56	; 86
-    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
+    3e9a:	86 35       	cpi	r24, 0x56	; 86
+    3e9c:	29 f4       	brne	.+10     	; 0x3ea8 <main+0xa8>
       // UNIVERSAL command is ignored
       getNch(4);
-    3ea0:	84 e0       	ldi	r24, 0x04	; 4
-    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
+    3e9e:	84 e0       	ldi	r24, 0x04	; 4
+    3ea0:	8f d0       	rcall	.+286    	; 0x3fc0 <getNch>
       putch(0x00);
-    3ea4:	80 e0       	ldi	r24, 0x00	; 0
-    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
-    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
+    3ea2:	80 e0       	ldi	r24, 0x00	; 0
+    3ea4:	6f d0       	rcall	.+222    	; 0x3f84 <putch>
+    3ea6:	6b c0       	rjmp	.+214    	; 0x3f7e <main+0x17e>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3eaa:	84 36       	cpi	r24, 0x64	; 100
-    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
-    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
+    3ea8:	84 36       	cpi	r24, 0x64	; 100
+    3eaa:	09 f0       	breq	.+2      	; 0x3eae <main+0xae>
+    3eac:	42 c0       	rjmp	.+132    	; 0x3f32 <main+0x132>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
-      getLen();
-    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3eae:	72 d0       	rcall	.+228    	; 0x3f94 <getch>
+      length = getch();
+    3eb0:	71 d0       	rcall	.+226    	; 0x3f94 <getch>
+    3eb2:	08 2f       	mov	r16, r24
+      getch();
+    3eb4:	6f d0       	rcall	.+222    	; 0x3f94 <getch>
 
       // If we are in RWW section, immediately start page erase
       if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3eb2:	e0 91 00 02 	lds	r30, 0x0200
-    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eb6:	80 e0       	ldi	r24, 0x00	; 0
+    3eb8:	c8 16       	cp	r12, r24
     3eba:	88 e3       	ldi	r24, 0x38	; 56
-    3ebc:	e0 30       	cpi	r30, 0x00	; 0
-    3ebe:	f8 07       	cpc	r31, r24
-    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
-    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ebc:	d8 06       	cpc	r13, r24
+    3ebe:	20 f4       	brcc	.+8      	; 0x3ec8 <main+0xc8>
+    3ec0:	83 e0       	ldi	r24, 0x03	; 3
+    3ec2:	f6 01       	movw	r30, r12
     3ec4:	87 bf       	out	0x37, r24	; 55
     3ec6:	e8 95       	spm
     3ec8:	c0 e0       	ldi	r28, 0x00	; 0
@@ -255,302 +261,301 @@ void watchdogReset() {
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
+    3ecc:	63 d0       	rcall	.+198    	; 0x3f94 <getch>
     3ece:	89 93       	st	Y+, r24
       while (--length);
-    3ed0:	80 91 02 02 	lds	r24, 0x0202
-    3ed4:	81 50       	subi	r24, 0x01	; 1
-    3ed6:	80 93 02 02 	sts	0x0202, r24
-    3eda:	88 23       	and	r24, r24
-    3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
+    3ed0:	0c 17       	cp	r16, r28
+    3ed2:	e1 f7       	brne	.-8      	; 0x3ecc <main+0xcc>
 
       // If we are in NRWW section, page erase has to be delayed until now.
       // Todo: Take RAMPZ into account
       if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ede:	e0 91 00 02 	lds	r30, 0x0200
-    3ee2:	f0 91 01 02 	lds	r31, 0x0201
-    3ee6:	88 e3       	ldi	r24, 0x38	; 56
-    3ee8:	e0 30       	cpi	r30, 0x00	; 0
-    3eea:	f8 07       	cpc	r31, r24
-    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
-    3eee:	83 e0       	ldi	r24, 0x03	; 3
-    3ef0:	87 bf       	out	0x37, r24	; 55
-    3ef2:	e8 95       	spm
+    3ed4:	f0 e0       	ldi	r31, 0x00	; 0
+    3ed6:	cf 16       	cp	r12, r31
+    3ed8:	f8 e3       	ldi	r31, 0x38	; 56
+    3eda:	df 06       	cpc	r13, r31
+    3edc:	20 f0       	brcs	.+8      	; 0x3ee6 <main+0xe6>
+    3ede:	83 e0       	ldi	r24, 0x03	; 3
+    3ee0:	f6 01       	movw	r30, r12
+    3ee2:	87 bf       	out	0x37, r24	; 55
+    3ee4:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
+    3ee6:	64 d0       	rcall	.+200    	; 0x3fb0 <verifySpace>
 
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3ef6:	07 b6       	in	r0, 0x37	; 55
-    3ef8:	00 fc       	sbrc	r0, 0
-    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
-      }
-#endif
-
-      // Copy buffer into programming buffer
+    3ee8:	07 b6       	in	r0, 0x37	; 55
+    3eea:	00 fc       	sbrc	r0, 0
+    3eec:	fd cf       	rjmp	.-6      	; 0x3ee8 <main+0xe8>
+    3eee:	a6 01       	movw	r20, r12
+    3ef0:	a0 e0       	ldi	r26, 0x00	; 0
+    3ef2:	b1 e0       	ldi	r27, 0x01	; 1
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3efc:	40 91 00 02 	lds	r20, 0x0200
-    3f00:	50 91 01 02 	lds	r21, 0x0201
-    3f04:	a0 e0       	ldi	r26, 0x00	; 0
-    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3f08:	2c 91       	ld	r18, X
-    3f0a:	30 e0       	ldi	r19, 0x00	; 0
+    3ef4:	2c 91       	ld	r18, X
+    3ef6:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3f0c:	11 96       	adiw	r26, 0x01	; 1
-    3f0e:	8c 91       	ld	r24, X
-    3f10:	11 97       	sbiw	r26, 0x01	; 1
-    3f12:	90 e0       	ldi	r25, 0x00	; 0
-    3f14:	98 2f       	mov	r25, r24
-    3f16:	88 27       	eor	r24, r24
-    3f18:	82 2b       	or	r24, r18
-    3f1a:	93 2b       	or	r25, r19
+    3ef8:	11 96       	adiw	r26, 0x01	; 1
+    3efa:	8c 91       	ld	r24, X
+    3efc:	11 97       	sbiw	r26, 0x01	; 1
+    3efe:	90 e0       	ldi	r25, 0x00	; 0
+    3f00:	98 2f       	mov	r25, r24
+    3f02:	88 27       	eor	r24, r24
+    3f04:	82 2b       	or	r24, r18
+    3f06:	93 2b       	or	r25, r19
 #define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
 #define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
 /* main program starts here */
 int main(void) {
-    3f1c:	12 96       	adiw	r26, 0x02	; 2
+    3f08:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    3f1e:	fa 01       	movw	r30, r20
-    3f20:	0c 01       	movw	r0, r24
-    3f22:	d7 be       	out	0x37, r13	; 55
-    3f24:	e8 95       	spm
-    3f26:	11 24       	eor	r1, r1
+    3f0a:	fa 01       	movw	r30, r20
+    3f0c:	0c 01       	movw	r0, r24
+    3f0e:	97 be       	out	0x37, r9	; 55
+    3f10:	e8 95       	spm
+    3f12:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3f28:	4e 5f       	subi	r20, 0xFE	; 254
-    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f14:	4e 5f       	subi	r20, 0xFE	; 254
+    3f16:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
-    3f2e:	a0 38       	cpi	r26, 0x80	; 128
-    3f30:	bf 07       	cpc	r27, r31
-    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
+    3f18:	f1 e0       	ldi	r31, 0x01	; 1
+    3f1a:	a0 38       	cpi	r26, 0x80	; 128
+    3f1c:	bf 07       	cpc	r27, r31
+    3f1e:	51 f7       	brne	.-44     	; 0x3ef4 <main+0xf4>
 
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
-    3f34:	e0 91 00 02 	lds	r30, 0x0200
-    3f38:	f0 91 01 02 	lds	r31, 0x0201
-    3f3c:	e7 be       	out	0x37, r14	; 55
-    3f3e:	e8 95       	spm
+    3f20:	f6 01       	movw	r30, r12
+    3f22:	a7 be       	out	0x37, r10	; 55
+    3f24:	e8 95       	spm
       boot_spm_busy_wait();
-    3f40:	07 b6       	in	r0, 0x37	; 55
-    3f42:	00 fc       	sbrc	r0, 0
-    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
+    3f26:	07 b6       	in	r0, 0x37	; 55
+    3f28:	00 fc       	sbrc	r0, 0
+    3f2a:	fd cf       	rjmp	.-6      	; 0x3f26 <main+0x126>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f46:	f7 be       	out	0x37, r15	; 55
-    3f48:	e8 95       	spm
-    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
+    3f2c:	b7 be       	out	0x37, r11	; 55
+    3f2e:	e8 95       	spm
+    3f30:	26 c0       	rjmp	.+76     	; 0x3f7e <main+0x17e>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f4c:	84 37       	cpi	r24, 0x74	; 116
-    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
+    3f32:	84 37       	cpi	r24, 0x74	; 116
+    3f34:	b1 f4       	brne	.+44     	; 0x3f62 <main+0x162>
       // READ PAGE - we only read flash
-      getLen();
-    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3f36:	2e d0       	rcall	.+92     	; 0x3f94 <getch>
+      length = getch();
+    3f38:	2d d0       	rcall	.+90     	; 0x3f94 <getch>
+    3f3a:	f8 2e       	mov	r15, r24
+      getch();
+    3f3c:	2b d0       	rcall	.+86     	; 0x3f94 <getch>
+
       verifySpace();
-    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+    3f3e:	38 d0       	rcall	.+112    	; 0x3fb0 <verifySpace>
+    3f40:	f6 01       	movw	r30, r12
+    3f42:	ef 2c       	mov	r14, r15
         putch(result);
         address++;
       }
       while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f54:	e0 91 00 02 	lds	r30, 0x0200
-    3f58:	f0 91 01 02 	lds	r31, 0x0201
-    3f5c:	31 96       	adiw	r30, 0x01	; 1
-    3f5e:	f0 93 01 02 	sts	0x0201, r31
-    3f62:	e0 93 00 02 	sts	0x0200, r30
-    3f66:	31 97       	sbiw	r30, 0x01	; 1
-    3f68:	e4 91       	lpm	r30, Z+
-    3f6a:	8e 2f       	mov	r24, r30
-    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
+    3f44:	8f 01       	movw	r16, r30
+    3f46:	0f 5f       	subi	r16, 0xFF	; 255
+    3f48:	1f 4f       	sbci	r17, 0xFF	; 255
+    3f4a:	84 91       	lpm	r24, Z+
+    3f4c:	1b d0       	rcall	.+54     	; 0x3f84 <putch>
       while (--length);
-    3f6e:	80 91 02 02 	lds	r24, 0x0202
-    3f72:	81 50       	subi	r24, 0x01	; 1
-    3f74:	80 93 02 02 	sts	0x0202, r24
-    3f78:	88 23       	and	r24, r24
-    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
-    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+    3f4e:	ea 94       	dec	r14
+    3f50:	f8 01       	movw	r30, r16
+    3f52:	c1 f7       	brne	.-16     	; 0x3f44 <main+0x144>
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
+#endif
+
+/* main program starts here */
+int main(void) {
+    3f54:	08 94       	sec
+    3f56:	c1 1c       	adc	r12, r1
+    3f58:	d1 1c       	adc	r13, r1
+    3f5a:	fa 94       	dec	r15
+    3f5c:	cf 0c       	add	r12, r15
+    3f5e:	d1 1c       	adc	r13, r1
+    3f60:	0e c0       	rjmp	.+28     	; 0x3f7e <main+0x17e>
 #endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f7e:	85 37       	cpi	r24, 0x75	; 117
-    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
+    3f62:	85 37       	cpi	r24, 0x75	; 117
+    3f64:	39 f4       	brne	.+14     	; 0x3f74 <main+0x174>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
+    3f66:	24 d0       	rcall	.+72     	; 0x3fb0 <verifySpace>
       putch(SIGNATURE_0);
-    3f84:	8e e1       	ldi	r24, 0x1E	; 30
-    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
+    3f68:	8e e1       	ldi	r24, 0x1E	; 30
+    3f6a:	0c d0       	rcall	.+24     	; 0x3f84 <putch>
       putch(SIGNATURE_1);
-    3f88:	84 e9       	ldi	r24, 0x94	; 148
-    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
+    3f6c:	84 e9       	ldi	r24, 0x94	; 148
+    3f6e:	0a d0       	rcall	.+20     	; 0x3f84 <putch>
       putch(SIGNATURE_2);
-    3f8c:	86 e0       	ldi	r24, 0x06	; 6
-    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
+    3f70:	86 e0       	ldi	r24, 0x06	; 6
+    3f72:	98 cf       	rjmp	.-208    	; 0x3ea4 <main+0xa4>
     }
     else if (ch == 'Q') {
-    3f90:	81 35       	cpi	r24, 0x51	; 81
-    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
+    3f74:	81 35       	cpi	r24, 0x51	; 81
+    3f76:	11 f4       	brne	.+4      	; 0x3f7c <main+0x17c>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f94:	88 e0       	ldi	r24, 0x08	; 8
-    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
+    3f78:	88 e0       	ldi	r24, 0x08	; 8
+    3f7a:	14 d0       	rcall	.+40     	; 0x3fa4 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
+    3f7c:	19 d0       	rcall	.+50     	; 0x3fb0 <verifySpace>
     }
     putch(STK_OK);
-    3f9a:	80 e1       	ldi	r24, 0x10	; 16
-    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
-    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
+    3f7e:	80 e1       	ldi	r24, 0x10	; 16
+    3f80:	01 d0       	rcall	.+2      	; 0x3f84 <putch>
+    3f82:	6a cf       	rjmp	.-300    	; 0x3e58 <main+0x58>
 
-00003fa0 <putch>:
+00003f84 <putch>:
   }
 }
 
 void putch(char ch) {
-    3fa0:	98 2f       	mov	r25, r24
+    3f84:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fa6:	85 ff       	sbrs	r24, 5
-    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
+    3f86:	80 91 c0 00 	lds	r24, 0x00C0
+    3f8a:	85 ff       	sbrs	r24, 5
+    3f8c:	fc cf       	rjmp	.-8      	; 0x3f86 <putch+0x2>
   UDR0 = ch;
-    3faa:	90 93 c6 00 	sts	0x00C6, r25
+    3f8e:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3fae:	08 95       	ret
+    3f92:	08 95       	ret
 
-00003fb0 <getch>:
-  return getch();
+00003f94 <getch>:
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fb0:	a8 95       	wdr
+    3f94:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fb6:	87 ff       	sbrs	r24, 7
-    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
+    3f96:	80 91 c0 00 	lds	r24, 0x00C0
+    3f9a:	87 ff       	sbrs	r24, 7
+    3f9c:	fc cf       	rjmp	.-8      	; 0x3f96 <getch+0x2>
   ch = UDR0;
-    3fba:	80 91 c6 00 	lds	r24, 0x00C6
+    3f9e:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    3fbe:	08 95       	ret
-
-00003fc0 <getLen>:
-  } while (--count);
-}
-#endif
-
-uint8_t getLen() {
-  getch();
-    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
-  length = getch();
-    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
-    3fc4:	80 93 02 02 	sts	0x0202, r24
-  return getch();
-}
-    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
+    3fa2:	08 95       	ret
 
-00003fca <watchdogConfig>:
+00003fa4 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3fca:	e0 e6       	ldi	r30, 0x60	; 96
-    3fcc:	f0 e0       	ldi	r31, 0x00	; 0
-    3fce:	98 e1       	ldi	r25, 0x18	; 24
-    3fd0:	90 83       	st	Z, r25
+    3fa4:	e0 e6       	ldi	r30, 0x60	; 96
+    3fa6:	f0 e0       	ldi	r31, 0x00	; 0
+    3fa8:	98 e1       	ldi	r25, 0x18	; 24
+    3faa:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fd2:	80 83       	st	Z, r24
+    3fac:	80 83       	st	Z, r24
 }
-    3fd4:	08 95       	ret
-
-00003fd6 <appStart>:
-
-void appStart() {
-  watchdogConfig(WATCHDOG_OFF);
-    3fd6:	80 e0       	ldi	r24, 0x00	; 0
-    3fd8:	f8 df       	rcall	.-16     	; 0x3fca <watchdogConfig>
-  __asm__ __volatile__ (
-    3fda:	ee 27       	eor	r30, r30
-    3fdc:	ff 27       	eor	r31, r31
-    3fde:	09 94       	ijmp
+    3fae:	08 95       	ret
 
-00003fe0 <verifySpace>:
+00003fb0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
-  if (getch() != CRC_EOP) appStart();
-    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
-    3fe2:	80 32       	cpi	r24, 0x20	; 32
-    3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
-    3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
+  if (getch() != CRC_EOP) {
+    3fb0:	f1 df       	rcall	.-30     	; 0x3f94 <getch>
+    3fb2:	80 32       	cpi	r24, 0x20	; 32
+    3fb4:	19 f0       	breq	.+6      	; 0x3fbc <verifySpace+0xc>
+    watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
+    3fb6:	88 e0       	ldi	r24, 0x08	; 8
+    3fb8:	f5 df       	rcall	.-22     	; 0x3fa4 <watchdogConfig>
+    3fba:	ff cf       	rjmp	.-2      	; 0x3fba <verifySpace+0xa>
+    while (1)			      // and busy-loop so that WD causes
+      ;				      //  a reset and app start.
+  }
   putch(STK_INSYNC);
-    3fe8:	84 e1       	ldi	r24, 0x14	; 20
+    3fbc:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+    3fbe:	e2 cf       	rjmp	.-60     	; 0x3f84 <putch>
 
-00003fec <getNch>:
+00003fc0 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fec:	1f 93       	push	r17
-    3fee:	18 2f       	mov	r17, r24
+    3fc0:	1f 93       	push	r17
+    3fc2:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
-    3ff2:	11 50       	subi	r17, 0x01	; 1
-    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
+    3fc4:	e7 df       	rcall	.-50     	; 0x3f94 <getch>
+    3fc6:	11 50       	subi	r17, 0x01	; 1
+    3fc8:	e9 f7       	brne	.-6      	; 0x3fc4 <getNch+0x4>
   verifySpace();
-    3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
+    3fca:	f2 df       	rcall	.-28     	; 0x3fb0 <verifySpace>
+}
+    3fcc:	1f 91       	pop	r17
+    3fce:	08 95       	ret
+
+00003fd0 <appStart>:
+  WDTCSR = _BV(WDCE) | _BV(WDE);
+  WDTCSR = x;
 }
-    3ff8:	1f 91       	pop	r17
-    3ffa:	08 95       	ret
+
+void appStart() {
+  watchdogConfig(WATCHDOG_OFF);
+    3fd0:	80 e0       	ldi	r24, 0x00	; 0
+    3fd2:	e8 df       	rcall	.-48     	; 0x3fa4 <watchdogConfig>
+  __asm__ __volatile__ (
+    3fd4:	ee 27       	eor	r30, r30
+    3fd6:	ff 27       	eor	r31, r31
+    3fd8:	09 94       	ijmp
diff --git a/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.hex b/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.hex
index 77897e7..dcdbbb4 100644
--- a/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.hex
+++ b/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.hex
@@ -1,35 +1,33 @@
-:103E0000112484B714BE81FFE6D085E08093810041
+:103E0000112484B714BE81FFE3D085E08093810044
 :103E100082E08093C00088E18093C10086E08093B7
-:103E2000C20088E08093C4008EE0CFD0259A86E05F
+:103E2000C20088E08093C4008EE0BCD0259A86E072
 :103E300028E13EEF91E0309385002093840096BB0B
-:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D
-:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020
-:103E6000C5D083E020C0823411F484E103C08534DE
-:103E700019F485E0BBD091C0853581F499D0082F25
-:103E800010E096D090E0982F8827802B912B880FF8
-:103E9000991F90930102809300027EC0863529F419
-:103EA00084E0A4D080E07CD078C0843609F04EC095
-:103EB00087D0E0910002F091010288E3E030F8073A
-:103EC00018F483E087BFE895C0E0D1E071D0899312
-:103ED000809102028150809302028823B9F7E09119
-:103EE0000002F091010288E3E030F80718F083E067
-:103EF00087BFE89575D007B600FCFDCF4091000262
-:103F000050910102A0E0B1E02C9130E011968C912B
-:103F1000119790E0982F8827822B932B1296FA0105
-:103F20000C01D7BEE89511244E5F5F4FF1E0A03839
-:103F3000BF0751F7E0910002F0910102E7BEE8955A
-:103F400007B600FCFDCFF7BEE89527C08437B9F46B
-:103F500037D046D0E0910002F09101023196F09303
-:103F60000102E09300023197E4918E2F19D08091E5
-:103F70000202815080930202882361F70EC08537C8
-:103F800039F42ED08EE10CD084E90AD086E08BCFB4
-:103F9000813511F488E019D023D080E101D05CCFC5
-:103FA000982F8091C00085FFFCCF9093C6000895A4
-:103FB000A8958091C00087FFFCCF8091C60008952E
-:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E
-:103FD00090838083089580E0F8DFEE27FF2709941F
-:103FE000E7DF803209F0F7DF84E1DACF1F93182F83
-:0C3FF000DFDF1150E9F7F4DF1F910895A6
+:103E4000B09BFECF1D9AA8958150A9F79924939411
+:103E5000A5E0AA2EF1E1BF2E9DD0813421F481E0AE
+:103E6000AFD083E01FC0823411F484E103C08534F5
+:103E700019F485E0A5D083C0853579F48BD0E82E80
+:103E8000FF2488D0082F10E0102F00270E291F29AB
+:103E9000000F111F8DD0680172C0863529F484E0AF
+:103EA0008FD080E06FD06BC0843609F042C072D0F2
+:103EB00071D0082F6FD080E0C81688E3D80620F4B0
+:103EC00083E0F60187BFE895C0E0D1E063D0899335
+:103ED0000C17E1F7F0E0CF16F8E3DF0620F083E0FF
+:103EE000F60187BFE89564D007B600FCFDCFA601B8
+:103EF000A0E0B1E02C9130E011968C91119790E008
+:103F0000982F8827822B932B1296FA010C0197BECB
+:103F1000E89511244E5F5F4FF1E0A038BF0751F7DD
+:103F2000F601A7BEE89507B600FCFDCFB7BEE89541
+:103F300026C08437B1F42ED02DD0F82E2BD038D017
+:103F4000F601EF2C8F010F5F1F4F84911BD0EA9475
+:103F5000F801C1F70894C11CD11CFA94CF0CD11CF4
+:103F60000EC0853739F424D08EE10CD084E90AD014
+:103F700086E098CF813511F488E014D019D080E123
+:103F800001D06ACF982F8091C00085FFFCCF90931D
+:103F9000C6000895A8958091C00087FFFCCF80914E
+:103FA000C6000895E0E6F0E098E1908380830895EC
+:103FB000F1DF803219F088E0F5DFFFCF84E1E2CF56
+:103FC0001F93182FE7DF1150E9F7F2DF1F910895D3
+:0A3FD00080E0E8DFEE27FF270994E8
 :023FFE000104BC
 :0400000300003E00BB
 :00000001FF
diff --git a/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.lst b/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.lst
index 48c2778..2676838 100644
--- a/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.lst
+++ b/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.lst
@@ -3,27 +3,27 @@ optiboot_lilypad_resonator.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001fc  00003e00  00003e00  00000054  2**1
+  0 .text         000001da  00003e00  00003e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00003ffe  00003ffe  00000250  2**0
+  1 .version      00000002  00003ffe  00003ffe  0000022e  2**0
                   CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000252  2**0
+  2 .debug_aranges 00000028  00000000  00000000  00000230  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000006a  00000000  00000000  0000027a  2**0
+  3 .debug_pubnames 0000005f  00000000  00000000  00000258  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000285  00000000  00000000  000002e4  2**0
+  4 .debug_info   0000028c  00000000  00000000  000002b7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000019f  00000000  00000000  00000569  2**0
+  5 .debug_abbrev 00000199  00000000  00000000  00000543  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000453  00000000  00000000  00000708  2**0
+  6 .debug_line   00000456  00000000  00000000  000006dc  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000090  00000000  00000000  00000b5c  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000b34  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000141  00000000  00000000  00000bec  2**0
+  8 .debug_str    00000149  00000000  00000000  00000bb4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    000001e1  00000000  00000000  00000d2d  2**0
+  9 .debug_loc    0000027e  00000000  00000000  00000cfd  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000068  00000000  00000000  00000f0e  2**0
+ 10 .debug_ranges 00000060  00000000  00000000  00000f7b  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
@@ -47,7 +47,7 @@ int main(void) {
     3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
     3e06:	81 ff       	sbrs	r24, 1
-    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+    3e08:	e3 d0       	rcall	.+454    	; 0x3fd0 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
@@ -77,7 +77,7 @@ int main(void) {
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
     3e28:	8e e0       	ldi	r24, 0x0E	; 14
-    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
+    3e2a:	bc d0       	rcall	.+376    	; 0x3fa4 <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
@@ -111,8 +111,8 @@ void flash_led(uint8_t count) {
 #else
     LED_PIN |= _BV(LED);
     3e44:	1d 9a       	sbi	0x03, 5	; 3
-  return getch();
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
@@ -132,8 +132,8 @@ void watchdogReset() {
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e4c:	dd 24       	eor	r13, r13
-    3e4e:	d3 94       	inc	r13
+    3e4c:	99 24       	eor	r9, r9
+    3e4e:	93 94       	inc	r9
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
@@ -141,21 +141,21 @@ void watchdogReset() {
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
     3e50:	a5 e0       	ldi	r26, 0x05	; 5
-    3e52:	ea 2e       	mov	r14, r26
+    3e52:	aa 2e       	mov	r10, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
     3e54:	f1 e1       	ldi	r31, 0x11	; 17
-    3e56:	ff 2e       	mov	r15, r31
+    3e56:	bf 2e       	mov	r11, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
+    3e58:	9d d0       	rcall	.+314    	; 0x3f94 <getch>
 
     if(ch == STK_GET_PARAMETER) {
     3e5a:	81 34       	cpi	r24, 0x41	; 65
@@ -163,10 +163,10 @@ void watchdogReset() {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
     3e5e:	81 e0       	ldi	r24, 0x01	; 1
-    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
+    3e60:	af d0       	rcall	.+350    	; 0x3fc0 <getNch>
       putch(0x03);
     3e62:	83 e0       	ldi	r24, 0x03	; 3
-    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
+    3e64:	1f c0       	rjmp	.+62     	; 0x3ea4 <main+0xa4>
     }
     else if(ch == STK_SET_DEVICE) {
     3e66:	82 34       	cpi	r24, 0x42	; 66
@@ -182,71 +182,77 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     3e72:	85 e0       	ldi	r24, 0x05	; 5
-    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
-    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
+    3e74:	a5 d0       	rcall	.+330    	; 0x3fc0 <getNch>
+    3e76:	83 c0       	rjmp	.+262    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     3e78:	85 35       	cpi	r24, 0x55	; 85
-    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
+    3e7a:	79 f4       	brne	.+30     	; 0x3e9a <main+0x9a>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+    3e7c:	8b d0       	rcall	.+278    	; 0x3f94 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-    3e7e:	08 2f       	mov	r16, r24
-    3e80:	10 e0       	ldi	r17, 0x00	; 0
-    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
-    3e84:	90 e0       	ldi	r25, 0x00	; 0
-    3e86:	98 2f       	mov	r25, r24
-    3e88:	88 27       	eor	r24, r24
-    3e8a:	80 2b       	or	r24, r16
-    3e8c:	91 2b       	or	r25, r17
+    3e7e:	e8 2e       	mov	r14, r24
+    3e80:	ff 24       	eor	r15, r15
+    3e82:	88 d0       	rcall	.+272    	; 0x3f94 <getch>
+    3e84:	08 2f       	mov	r16, r24
+    3e86:	10 e0       	ldi	r17, 0x00	; 0
+    3e88:	10 2f       	mov	r17, r16
+    3e8a:	00 27       	eor	r16, r16
+    3e8c:	0e 29       	or	r16, r14
+    3e8e:	1f 29       	or	r17, r15
 #ifdef RAMPZ
       // Transfer top bit to RAMPZ
       RAMPZ = (newAddress & 0x8000) ? 1 : 0;
 #endif
       newAddress += newAddress; // Convert from word address to byte address
-    3e8e:	88 0f       	add	r24, r24
-    3e90:	99 1f       	adc	r25, r25
+    3e90:	00 0f       	add	r16, r16
+    3e92:	11 1f       	adc	r17, r17
       address = newAddress;
-    3e92:	90 93 01 02 	sts	0x0201, r25
-    3e96:	80 93 00 02 	sts	0x0200, r24
-    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
+    3e94:	8d d0       	rcall	.+282    	; 0x3fb0 <verifySpace>
+    3e96:	68 01       	movw	r12, r16
+    3e98:	72 c0       	rjmp	.+228    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_UNIVERSAL) {
-    3e9c:	86 35       	cpi	r24, 0x56	; 86
-    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
+    3e9a:	86 35       	cpi	r24, 0x56	; 86
+    3e9c:	29 f4       	brne	.+10     	; 0x3ea8 <main+0xa8>
       // UNIVERSAL command is ignored
       getNch(4);
-    3ea0:	84 e0       	ldi	r24, 0x04	; 4
-    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
+    3e9e:	84 e0       	ldi	r24, 0x04	; 4
+    3ea0:	8f d0       	rcall	.+286    	; 0x3fc0 <getNch>
       putch(0x00);
-    3ea4:	80 e0       	ldi	r24, 0x00	; 0
-    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
-    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
+    3ea2:	80 e0       	ldi	r24, 0x00	; 0
+    3ea4:	6f d0       	rcall	.+222    	; 0x3f84 <putch>
+    3ea6:	6b c0       	rjmp	.+214    	; 0x3f7e <main+0x17e>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3eaa:	84 36       	cpi	r24, 0x64	; 100
-    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
-    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
+    3ea8:	84 36       	cpi	r24, 0x64	; 100
+    3eaa:	09 f0       	breq	.+2      	; 0x3eae <main+0xae>
+    3eac:	42 c0       	rjmp	.+132    	; 0x3f32 <main+0x132>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
-      getLen();
-    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3eae:	72 d0       	rcall	.+228    	; 0x3f94 <getch>
+      length = getch();
+    3eb0:	71 d0       	rcall	.+226    	; 0x3f94 <getch>
+    3eb2:	08 2f       	mov	r16, r24
+      getch();
+    3eb4:	6f d0       	rcall	.+222    	; 0x3f94 <getch>
 
       // If we are in RWW section, immediately start page erase
       if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3eb2:	e0 91 00 02 	lds	r30, 0x0200
-    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eb6:	80 e0       	ldi	r24, 0x00	; 0
+    3eb8:	c8 16       	cp	r12, r24
     3eba:	88 e3       	ldi	r24, 0x38	; 56
-    3ebc:	e0 30       	cpi	r30, 0x00	; 0
-    3ebe:	f8 07       	cpc	r31, r24
-    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
-    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ebc:	d8 06       	cpc	r13, r24
+    3ebe:	20 f4       	brcc	.+8      	; 0x3ec8 <main+0xc8>
+    3ec0:	83 e0       	ldi	r24, 0x03	; 3
+    3ec2:	f6 01       	movw	r30, r12
     3ec4:	87 bf       	out	0x37, r24	; 55
     3ec6:	e8 95       	spm
     3ec8:	c0 e0       	ldi	r28, 0x00	; 0
@@ -255,302 +261,301 @@ void watchdogReset() {
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
+    3ecc:	63 d0       	rcall	.+198    	; 0x3f94 <getch>
     3ece:	89 93       	st	Y+, r24
       while (--length);
-    3ed0:	80 91 02 02 	lds	r24, 0x0202
-    3ed4:	81 50       	subi	r24, 0x01	; 1
-    3ed6:	80 93 02 02 	sts	0x0202, r24
-    3eda:	88 23       	and	r24, r24
-    3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
+    3ed0:	0c 17       	cp	r16, r28
+    3ed2:	e1 f7       	brne	.-8      	; 0x3ecc <main+0xcc>
 
       // If we are in NRWW section, page erase has to be delayed until now.
       // Todo: Take RAMPZ into account
       if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ede:	e0 91 00 02 	lds	r30, 0x0200
-    3ee2:	f0 91 01 02 	lds	r31, 0x0201
-    3ee6:	88 e3       	ldi	r24, 0x38	; 56
-    3ee8:	e0 30       	cpi	r30, 0x00	; 0
-    3eea:	f8 07       	cpc	r31, r24
-    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
-    3eee:	83 e0       	ldi	r24, 0x03	; 3
-    3ef0:	87 bf       	out	0x37, r24	; 55
-    3ef2:	e8 95       	spm
+    3ed4:	f0 e0       	ldi	r31, 0x00	; 0
+    3ed6:	cf 16       	cp	r12, r31
+    3ed8:	f8 e3       	ldi	r31, 0x38	; 56
+    3eda:	df 06       	cpc	r13, r31
+    3edc:	20 f0       	brcs	.+8      	; 0x3ee6 <main+0xe6>
+    3ede:	83 e0       	ldi	r24, 0x03	; 3
+    3ee0:	f6 01       	movw	r30, r12
+    3ee2:	87 bf       	out	0x37, r24	; 55
+    3ee4:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
+    3ee6:	64 d0       	rcall	.+200    	; 0x3fb0 <verifySpace>
 
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3ef6:	07 b6       	in	r0, 0x37	; 55
-    3ef8:	00 fc       	sbrc	r0, 0
-    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
-      }
-#endif
-
-      // Copy buffer into programming buffer
+    3ee8:	07 b6       	in	r0, 0x37	; 55
+    3eea:	00 fc       	sbrc	r0, 0
+    3eec:	fd cf       	rjmp	.-6      	; 0x3ee8 <main+0xe8>
+    3eee:	a6 01       	movw	r20, r12
+    3ef0:	a0 e0       	ldi	r26, 0x00	; 0
+    3ef2:	b1 e0       	ldi	r27, 0x01	; 1
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3efc:	40 91 00 02 	lds	r20, 0x0200
-    3f00:	50 91 01 02 	lds	r21, 0x0201
-    3f04:	a0 e0       	ldi	r26, 0x00	; 0
-    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3f08:	2c 91       	ld	r18, X
-    3f0a:	30 e0       	ldi	r19, 0x00	; 0
+    3ef4:	2c 91       	ld	r18, X
+    3ef6:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3f0c:	11 96       	adiw	r26, 0x01	; 1
-    3f0e:	8c 91       	ld	r24, X
-    3f10:	11 97       	sbiw	r26, 0x01	; 1
-    3f12:	90 e0       	ldi	r25, 0x00	; 0
-    3f14:	98 2f       	mov	r25, r24
-    3f16:	88 27       	eor	r24, r24
-    3f18:	82 2b       	or	r24, r18
-    3f1a:	93 2b       	or	r25, r19
+    3ef8:	11 96       	adiw	r26, 0x01	; 1
+    3efa:	8c 91       	ld	r24, X
+    3efc:	11 97       	sbiw	r26, 0x01	; 1
+    3efe:	90 e0       	ldi	r25, 0x00	; 0
+    3f00:	98 2f       	mov	r25, r24
+    3f02:	88 27       	eor	r24, r24
+    3f04:	82 2b       	or	r24, r18
+    3f06:	93 2b       	or	r25, r19
 #define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
 #define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
 /* main program starts here */
 int main(void) {
-    3f1c:	12 96       	adiw	r26, 0x02	; 2
+    3f08:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    3f1e:	fa 01       	movw	r30, r20
-    3f20:	0c 01       	movw	r0, r24
-    3f22:	d7 be       	out	0x37, r13	; 55
-    3f24:	e8 95       	spm
-    3f26:	11 24       	eor	r1, r1
+    3f0a:	fa 01       	movw	r30, r20
+    3f0c:	0c 01       	movw	r0, r24
+    3f0e:	97 be       	out	0x37, r9	; 55
+    3f10:	e8 95       	spm
+    3f12:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3f28:	4e 5f       	subi	r20, 0xFE	; 254
-    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f14:	4e 5f       	subi	r20, 0xFE	; 254
+    3f16:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
-    3f2e:	a0 38       	cpi	r26, 0x80	; 128
-    3f30:	bf 07       	cpc	r27, r31
-    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
+    3f18:	f1 e0       	ldi	r31, 0x01	; 1
+    3f1a:	a0 38       	cpi	r26, 0x80	; 128
+    3f1c:	bf 07       	cpc	r27, r31
+    3f1e:	51 f7       	brne	.-44     	; 0x3ef4 <main+0xf4>
 
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
-    3f34:	e0 91 00 02 	lds	r30, 0x0200
-    3f38:	f0 91 01 02 	lds	r31, 0x0201
-    3f3c:	e7 be       	out	0x37, r14	; 55
-    3f3e:	e8 95       	spm
+    3f20:	f6 01       	movw	r30, r12
+    3f22:	a7 be       	out	0x37, r10	; 55
+    3f24:	e8 95       	spm
       boot_spm_busy_wait();
-    3f40:	07 b6       	in	r0, 0x37	; 55
-    3f42:	00 fc       	sbrc	r0, 0
-    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
+    3f26:	07 b6       	in	r0, 0x37	; 55
+    3f28:	00 fc       	sbrc	r0, 0
+    3f2a:	fd cf       	rjmp	.-6      	; 0x3f26 <main+0x126>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f46:	f7 be       	out	0x37, r15	; 55
-    3f48:	e8 95       	spm
-    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
+    3f2c:	b7 be       	out	0x37, r11	; 55
+    3f2e:	e8 95       	spm
+    3f30:	26 c0       	rjmp	.+76     	; 0x3f7e <main+0x17e>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f4c:	84 37       	cpi	r24, 0x74	; 116
-    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
+    3f32:	84 37       	cpi	r24, 0x74	; 116
+    3f34:	b1 f4       	brne	.+44     	; 0x3f62 <main+0x162>
       // READ PAGE - we only read flash
-      getLen();
-    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3f36:	2e d0       	rcall	.+92     	; 0x3f94 <getch>
+      length = getch();
+    3f38:	2d d0       	rcall	.+90     	; 0x3f94 <getch>
+    3f3a:	f8 2e       	mov	r15, r24
+      getch();
+    3f3c:	2b d0       	rcall	.+86     	; 0x3f94 <getch>
+
       verifySpace();
-    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+    3f3e:	38 d0       	rcall	.+112    	; 0x3fb0 <verifySpace>
+    3f40:	f6 01       	movw	r30, r12
+    3f42:	ef 2c       	mov	r14, r15
         putch(result);
         address++;
       }
       while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f54:	e0 91 00 02 	lds	r30, 0x0200
-    3f58:	f0 91 01 02 	lds	r31, 0x0201
-    3f5c:	31 96       	adiw	r30, 0x01	; 1
-    3f5e:	f0 93 01 02 	sts	0x0201, r31
-    3f62:	e0 93 00 02 	sts	0x0200, r30
-    3f66:	31 97       	sbiw	r30, 0x01	; 1
-    3f68:	e4 91       	lpm	r30, Z+
-    3f6a:	8e 2f       	mov	r24, r30
-    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
+    3f44:	8f 01       	movw	r16, r30
+    3f46:	0f 5f       	subi	r16, 0xFF	; 255
+    3f48:	1f 4f       	sbci	r17, 0xFF	; 255
+    3f4a:	84 91       	lpm	r24, Z+
+    3f4c:	1b d0       	rcall	.+54     	; 0x3f84 <putch>
       while (--length);
-    3f6e:	80 91 02 02 	lds	r24, 0x0202
-    3f72:	81 50       	subi	r24, 0x01	; 1
-    3f74:	80 93 02 02 	sts	0x0202, r24
-    3f78:	88 23       	and	r24, r24
-    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
-    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+    3f4e:	ea 94       	dec	r14
+    3f50:	f8 01       	movw	r30, r16
+    3f52:	c1 f7       	brne	.-16     	; 0x3f44 <main+0x144>
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
+#endif
+
+/* main program starts here */
+int main(void) {
+    3f54:	08 94       	sec
+    3f56:	c1 1c       	adc	r12, r1
+    3f58:	d1 1c       	adc	r13, r1
+    3f5a:	fa 94       	dec	r15
+    3f5c:	cf 0c       	add	r12, r15
+    3f5e:	d1 1c       	adc	r13, r1
+    3f60:	0e c0       	rjmp	.+28     	; 0x3f7e <main+0x17e>
 #endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f7e:	85 37       	cpi	r24, 0x75	; 117
-    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
+    3f62:	85 37       	cpi	r24, 0x75	; 117
+    3f64:	39 f4       	brne	.+14     	; 0x3f74 <main+0x174>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
+    3f66:	24 d0       	rcall	.+72     	; 0x3fb0 <verifySpace>
       putch(SIGNATURE_0);
-    3f84:	8e e1       	ldi	r24, 0x1E	; 30
-    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
+    3f68:	8e e1       	ldi	r24, 0x1E	; 30
+    3f6a:	0c d0       	rcall	.+24     	; 0x3f84 <putch>
       putch(SIGNATURE_1);
-    3f88:	84 e9       	ldi	r24, 0x94	; 148
-    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
+    3f6c:	84 e9       	ldi	r24, 0x94	; 148
+    3f6e:	0a d0       	rcall	.+20     	; 0x3f84 <putch>
       putch(SIGNATURE_2);
-    3f8c:	86 e0       	ldi	r24, 0x06	; 6
-    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
+    3f70:	86 e0       	ldi	r24, 0x06	; 6
+    3f72:	98 cf       	rjmp	.-208    	; 0x3ea4 <main+0xa4>
     }
     else if (ch == 'Q') {
-    3f90:	81 35       	cpi	r24, 0x51	; 81
-    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
+    3f74:	81 35       	cpi	r24, 0x51	; 81
+    3f76:	11 f4       	brne	.+4      	; 0x3f7c <main+0x17c>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f94:	88 e0       	ldi	r24, 0x08	; 8
-    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
+    3f78:	88 e0       	ldi	r24, 0x08	; 8
+    3f7a:	14 d0       	rcall	.+40     	; 0x3fa4 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
+    3f7c:	19 d0       	rcall	.+50     	; 0x3fb0 <verifySpace>
     }
     putch(STK_OK);
-    3f9a:	80 e1       	ldi	r24, 0x10	; 16
-    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
-    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
+    3f7e:	80 e1       	ldi	r24, 0x10	; 16
+    3f80:	01 d0       	rcall	.+2      	; 0x3f84 <putch>
+    3f82:	6a cf       	rjmp	.-300    	; 0x3e58 <main+0x58>
 
-00003fa0 <putch>:
+00003f84 <putch>:
   }
 }
 
 void putch(char ch) {
-    3fa0:	98 2f       	mov	r25, r24
+    3f84:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fa6:	85 ff       	sbrs	r24, 5
-    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
+    3f86:	80 91 c0 00 	lds	r24, 0x00C0
+    3f8a:	85 ff       	sbrs	r24, 5
+    3f8c:	fc cf       	rjmp	.-8      	; 0x3f86 <putch+0x2>
   UDR0 = ch;
-    3faa:	90 93 c6 00 	sts	0x00C6, r25
+    3f8e:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3fae:	08 95       	ret
+    3f92:	08 95       	ret
 
-00003fb0 <getch>:
-  return getch();
+00003f94 <getch>:
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fb0:	a8 95       	wdr
+    3f94:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fb6:	87 ff       	sbrs	r24, 7
-    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
+    3f96:	80 91 c0 00 	lds	r24, 0x00C0
+    3f9a:	87 ff       	sbrs	r24, 7
+    3f9c:	fc cf       	rjmp	.-8      	; 0x3f96 <getch+0x2>
   ch = UDR0;
-    3fba:	80 91 c6 00 	lds	r24, 0x00C6
+    3f9e:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    3fbe:	08 95       	ret
-
-00003fc0 <getLen>:
-  } while (--count);
-}
-#endif
-
-uint8_t getLen() {
-  getch();
-    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
-  length = getch();
-    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
-    3fc4:	80 93 02 02 	sts	0x0202, r24
-  return getch();
-}
-    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
+    3fa2:	08 95       	ret
 
-00003fca <watchdogConfig>:
+00003fa4 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3fca:	e0 e6       	ldi	r30, 0x60	; 96
-    3fcc:	f0 e0       	ldi	r31, 0x00	; 0
-    3fce:	98 e1       	ldi	r25, 0x18	; 24
-    3fd0:	90 83       	st	Z, r25
+    3fa4:	e0 e6       	ldi	r30, 0x60	; 96
+    3fa6:	f0 e0       	ldi	r31, 0x00	; 0
+    3fa8:	98 e1       	ldi	r25, 0x18	; 24
+    3faa:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fd2:	80 83       	st	Z, r24
+    3fac:	80 83       	st	Z, r24
 }
-    3fd4:	08 95       	ret
-
-00003fd6 <appStart>:
-
-void appStart() {
-  watchdogConfig(WATCHDOG_OFF);
-    3fd6:	80 e0       	ldi	r24, 0x00	; 0
-    3fd8:	f8 df       	rcall	.-16     	; 0x3fca <watchdogConfig>
-  __asm__ __volatile__ (
-    3fda:	ee 27       	eor	r30, r30
-    3fdc:	ff 27       	eor	r31, r31
-    3fde:	09 94       	ijmp
+    3fae:	08 95       	ret
 
-00003fe0 <verifySpace>:
+00003fb0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
-  if (getch() != CRC_EOP) appStart();
-    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
-    3fe2:	80 32       	cpi	r24, 0x20	; 32
-    3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
-    3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
+  if (getch() != CRC_EOP) {
+    3fb0:	f1 df       	rcall	.-30     	; 0x3f94 <getch>
+    3fb2:	80 32       	cpi	r24, 0x20	; 32
+    3fb4:	19 f0       	breq	.+6      	; 0x3fbc <verifySpace+0xc>
+    watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
+    3fb6:	88 e0       	ldi	r24, 0x08	; 8
+    3fb8:	f5 df       	rcall	.-22     	; 0x3fa4 <watchdogConfig>
+    3fba:	ff cf       	rjmp	.-2      	; 0x3fba <verifySpace+0xa>
+    while (1)			      // and busy-loop so that WD causes
+      ;				      //  a reset and app start.
+  }
   putch(STK_INSYNC);
-    3fe8:	84 e1       	ldi	r24, 0x14	; 20
+    3fbc:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+    3fbe:	e2 cf       	rjmp	.-60     	; 0x3f84 <putch>
 
-00003fec <getNch>:
+00003fc0 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fec:	1f 93       	push	r17
-    3fee:	18 2f       	mov	r17, r24
+    3fc0:	1f 93       	push	r17
+    3fc2:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
-    3ff2:	11 50       	subi	r17, 0x01	; 1
-    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
+    3fc4:	e7 df       	rcall	.-50     	; 0x3f94 <getch>
+    3fc6:	11 50       	subi	r17, 0x01	; 1
+    3fc8:	e9 f7       	brne	.-6      	; 0x3fc4 <getNch+0x4>
   verifySpace();
-    3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
+    3fca:	f2 df       	rcall	.-28     	; 0x3fb0 <verifySpace>
+}
+    3fcc:	1f 91       	pop	r17
+    3fce:	08 95       	ret
+
+00003fd0 <appStart>:
+  WDTCSR = _BV(WDCE) | _BV(WDE);
+  WDTCSR = x;
 }
-    3ff8:	1f 91       	pop	r17
-    3ffa:	08 95       	ret
+
+void appStart() {
+  watchdogConfig(WATCHDOG_OFF);
+    3fd0:	80 e0       	ldi	r24, 0x00	; 0
+    3fd2:	e8 df       	rcall	.-48     	; 0x3fa4 <watchdogConfig>
+  __asm__ __volatile__ (
+    3fd4:	ee 27       	eor	r30, r30
+    3fd6:	ff 27       	eor	r31, r31
+    3fd8:	09 94       	ijmp
diff --git a/arduino/bootloaders/optiboot/optiboot_luminet.hex b/arduino/bootloaders/optiboot/optiboot_luminet.hex
index 0f901f4..843b2ef 100644
--- a/arduino/bootloaders/optiboot/optiboot_luminet.hex
+++ b/arduino/bootloaders/optiboot/optiboot_luminet.hex
@@ -1,42 +1,39 @@
-:10000000112484B714BE81FF22D185E08EBD8EE01D
-:100010001AD1D49AD29A86E023EC3FEF91E03DBD0D
+:10000000112484B714BE81FF18D185E08EBD8EE027
+:1000100000D1D49AD29A86E023EC3FEF91E03DBD27
 :100020002CBD9BB9589BFECFCC9AA8958150B9F7AF
-:10003000DD24D39485E0C82E0FE7F02E1EECE12ED0
-:10004000E9D0813421F481E00DD183E020C08234F5
-:1000500011F484E103C0853419F485E003D1C8C0EC
-:10006000853581F4D7D0082F10E0D4D090E0982FB8
-:100070008827802B912B880F991F90938101809363
-:100080008001B5C0863529F484E0ECD080E0B3D09F
-:10009000AFC0843609F06BC0D1D0C0E0D1E0BAD097
-:1000A0008993809182018150809382018823B9F7DE
-:1000B000E0918001F091810183E087BFE895CCD089
-:1000C00007B600FCFDCF8091800190918101892BC2
-:1000D00041F5809100012091010130E0322F22276B
-:1000E00090E0282B392B3093850120938401409197
-:1000F00008018091090190E0982F882750E0842B17
-:10010000952B909387018093860124503040209353
-:100110000801232F332720930901F0920001E09278
-:1001200001014091800150918101A0E0B1E02C914A
-:1001300030E011968C91119790E0982F8827822BB0
-:10014000932B1296FA010C01D7BEE89511244E5F4D
-:100150005F4FF1E0A034BF0751F7E0918001F091CB
-:100160008101C7BEE89507B600FCFDCF41C08437CA
-:1001700089F564D071D0E0918001F09181013097D0
-:1001800019F42091840113C0E130F10519F4209194
-:1001900085010DC0E830F10519F42091860107C0F2
-:1001A000E930F10519F42091870101C02491809173
-:1001B000800190918101019690938101809380014B
-:1001C000822F19D0809182018150809382018823EF
-:1001D00091F60EC0853739F43FD08EE10CD083E91B
-:1001E0000AD08CE054CF813511F488E02CD034D083
-:1001F00080E101D025CF2AE030E08095089410F40A
-:10020000DA9802C0DA9A000015D014D086952A95A3
-:10021000B1F70895A89529E030E0CB99FECF0AD038
-:1002200009D008D08894CB9908942A9511F0879525
-:10023000F7CF08959EE09A95F1F70895EBDFEADF96
-:1002400080938201E7CF98E191BD81BD089580E060
-:10025000FADFE4E0FF270994DDDF803209F0F7DF01
-:1002600084E1C9CF1F93182FD5DF1150E9F7F4DFD0
-:040270001F9108953D
+:10003000BB24B39425E0A22E9FE7D92E8EECC82EC8
+:10004000D4D0813421F481E0F0D083E0B5C0823493
+:1000500011F484E103C0853419F485E0E6D0B3C01F
+:10006000853569F4C2D0E82EFF24BFD0082F10E0F8
+:10007000102F00270E291F29000F111FA3C086353E
+:1000800021F484E0D2D080E097C0843609F060C0CB
+:10009000ACD0ABD0F82EA9D0C0E0D1E0A6D08993E7
+:1000A000FC16E1F783E0F80187BFE895B6D007B604
+:1000B00000FCFDCF0115110511F0A8012AC08091A7
+:1000C00000012091010130E0322F222790E0282BFF
+:1000D000392B309385012093840140910801809150
+:1000E000090190E0982F882750E0842B952B90935E
+:1000F0008701809386012450304020930801232FEC
+:10010000332720930901D0920001C092010140E001
+:1001100050E0A0E0B1E02C9130E011968C91119765
+:1001200090E0982F8827822B932B1296FA010C01CE
+:10013000B7BEE89511244E5F5F4FF1E0A034BF07D2
+:1001400051F7F801A7BEE89507B600FCFDCF3BC00C
+:10015000843751F54AD049D0F82E47D05ED0E80117
+:10016000EF2C209719F48091840114C0C130D1057F
+:1001700019F4809185010EC0C830D10519F4809121
+:10018000860108C0C930D10519F48091870102C0E9
+:10019000FE01849121961AD0EA9419F70F5F1F4F40
+:1001A000FA940F0D111D0FC0853741F436D08EE142
+:1001B0000DD083E90BD08CE009D005C0813511F456
+:1001C00088E027D02AD080E101D03ACF2AE030E081
+:1001D0008095089410F4DA9802C0DA9A000015D0DD
+:1001E00014D086952A95B1F70895A89529E030E0B6
+:1001F000CB99FECF0AD009D008D08894CB99089427
+:100200002A9511F08795F7CF08959EE09A95F1F71A
+:10021000089598E191BD81BD0895E7DF803219F01E
+:1002200088E0F7DFFFCF84E1D1CF1F93182FDDDF08
+:100230001150E9F7F2DF1F91089580E0EADFE4E072
+:04024000FF270994F7
 :021EFE000104DD
 :00000001FF
diff --git a/arduino/bootloaders/optiboot/optiboot_luminet.lst b/arduino/bootloaders/optiboot/optiboot_luminet.lst
index 1be7e4f..8333670 100644
--- a/arduino/bootloaders/optiboot/optiboot_luminet.lst
+++ b/arduino/bootloaders/optiboot/optiboot_luminet.lst
@@ -3,27 +3,27 @@ optiboot_luminet.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .version      00000002  00001efe  00001efe  000002c8  2**0
+  0 .version      00000002  00001efe  00001efe  00000298  2**0
                   CONTENTS, READONLY
-  1 .text         00000274  00000000  00000000  00000054  2**1
+  1 .text         00000244  00000000  00000000  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  2 .debug_aranges 00000028  00000000  00000000  000002ca  2**0
+  2 .debug_aranges 00000028  00000000  00000000  0000029a  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 00000078  00000000  00000000  000002f2  2**0
+  3 .debug_pubnames 0000006d  00000000  00000000  000002c2  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   000002a5  00000000  00000000  0000036a  2**0
+  4 .debug_info   000002b0  00000000  00000000  0000032f  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000019d  00000000  00000000  0000060f  2**0
+  5 .debug_abbrev 00000197  00000000  00000000  000005df  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   000004ac  00000000  00000000  000007ac  2**0
+  6 .debug_line   000004a7  00000000  00000000  00000776  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  000000a0  00000000  00000000  00000c58  2**2
+  7 .debug_frame  00000090  00000000  00000000  00000c20  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000150  00000000  00000000  00000cf8  2**0
+  8 .debug_str    00000158  00000000  00000000  00000cb0  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    00000194  00000000  00000000  00000e48  2**0
+  9 .debug_loc    00000268  00000000  00000000  00000e08  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000088  00000000  00000000  00000fdc  2**0
+ 10 .debug_ranges 00000080  00000000  00000000  00001070  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
@@ -47,7 +47,7 @@ int main(void) {
    4:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
    6:	81 ff       	sbrs	r24, 1
-   8:	22 d1       	rcall	.+580    	; 0x24e <appStart>
+   8:	18 d1       	rcall	.+560    	; 0x23a <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
@@ -61,7 +61,7 @@ int main(void) {
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
    e:	8e e0       	ldi	r24, 0x0E	; 14
-  10:	1a d1       	rcall	.+564    	; 0x246 <watchdogConfig>
+  10:	00 d1       	rcall	.+512    	; 0x212 <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
@@ -100,8 +100,8 @@ void flash_led(uint8_t count) {
 #else
     LED_PIN |= _BV(LED);
   28:	cc 9a       	sbi	0x19, 4	; 25
-  return getch();
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
@@ -121,34 +121,34 @@ void watchdogReset() {
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-  30:	dd 24       	eor	r13, r13
-  32:	d3 94       	inc	r13
+  30:	bb 24       	eor	r11, r11
+  32:	b3 94       	inc	r11
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
 
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
-  34:	85 e0       	ldi	r24, 0x05	; 5
-  36:	c8 2e       	mov	r12, r24
+  34:	25 e0       	ldi	r18, 0x05	; 5
+  36:	a2 2e       	mov	r10, r18
         vect -= 4; // Instruction is a relative jump (rjmp), so recalculate.
         buff[8] = vect & 0xff;
         buff[9] = vect >> 8;
 
         // Add jump to bootloader at RESET vector
         buff[0] = 0x7f;
-  38:	0f e7       	ldi	r16, 0x7F	; 127
-  3a:	f0 2e       	mov	r15, r16
+  38:	9f e7       	ldi	r25, 0x7F	; 127
+  3a:	d9 2e       	mov	r13, r25
         buff[1] = 0xce; // rjmp 0x1d00 instruction
-  3c:	1e ec       	ldi	r17, 0xCE	; 206
-  3e:	e1 2e       	mov	r14, r17
+  3c:	8e ec       	ldi	r24, 0xCE	; 206
+  3e:	c8 2e       	mov	r12, r24
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-  40:	e9 d0       	rcall	.+466    	; 0x214 <getch>
+  40:	d4 d0       	rcall	.+424    	; 0x1ea <getch>
 
     if(ch == STK_GET_PARAMETER) {
   42:	81 34       	cpi	r24, 0x41	; 65
@@ -156,10 +156,10 @@ void watchdogReset() {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
   46:	81 e0       	ldi	r24, 0x01	; 1
-  48:	0d d1       	rcall	.+538    	; 0x264 <getNch>
+  48:	f0 d0       	rcall	.+480    	; 0x22a <getNch>
       putch(0x03);
   4a:	83 e0       	ldi	r24, 0x03	; 3
-  4c:	20 c0       	rjmp	.+64     	; 0x8e <__SREG__+0x4f>
+  4c:	b5 c0       	rjmp	.+362    	; 0x1b8 <__SREG__+0x179>
     }
     else if(ch == STK_SET_DEVICE) {
   4e:	82 34       	cpi	r24, 0x42	; 66
@@ -175,450 +175,450 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
   5a:	85 e0       	ldi	r24, 0x05	; 5
-  5c:	03 d1       	rcall	.+518    	; 0x264 <getNch>
-  5e:	c8 c0       	rjmp	.+400    	; 0x1f0 <__SREG__+0x1b1>
+  5c:	e6 d0       	rcall	.+460    	; 0x22a <getNch>
+  5e:	b3 c0       	rjmp	.+358    	; 0x1c6 <__SREG__+0x187>
     }
     else if(ch == STK_LOAD_ADDRESS) {
   60:	85 35       	cpi	r24, 0x55	; 85
-  62:	81 f4       	brne	.+32     	; 0x84 <__SREG__+0x45>
+  62:	69 f4       	brne	.+26     	; 0x7e <__SREG__+0x3f>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-  64:	d7 d0       	rcall	.+430    	; 0x214 <getch>
+  64:	c2 d0       	rcall	.+388    	; 0x1ea <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-  66:	08 2f       	mov	r16, r24
-  68:	10 e0       	ldi	r17, 0x00	; 0
-  6a:	d4 d0       	rcall	.+424    	; 0x214 <getch>
-  6c:	90 e0       	ldi	r25, 0x00	; 0
-  6e:	98 2f       	mov	r25, r24
-  70:	88 27       	eor	r24, r24
-  72:	80 2b       	or	r24, r16
-  74:	91 2b       	or	r25, r17
+  66:	e8 2e       	mov	r14, r24
+  68:	ff 24       	eor	r15, r15
+  6a:	bf d0       	rcall	.+382    	; 0x1ea <getch>
+  6c:	08 2f       	mov	r16, r24
+  6e:	10 e0       	ldi	r17, 0x00	; 0
+  70:	10 2f       	mov	r17, r16
+  72:	00 27       	eor	r16, r16
+  74:	0e 29       	or	r16, r14
+  76:	1f 29       	or	r17, r15
 #ifdef RAMPZ
       // Transfer top bit to RAMPZ
       RAMPZ = (newAddress & 0x8000) ? 1 : 0;
 #endif
       newAddress += newAddress; // Convert from word address to byte address
-  76:	88 0f       	add	r24, r24
-  78:	99 1f       	adc	r25, r25
+  78:	00 0f       	add	r16, r16
+  7a:	11 1f       	adc	r17, r17
+  7c:	a3 c0       	rjmp	.+326    	; 0x1c4 <__SREG__+0x185>
       address = newAddress;
-  7a:	90 93 81 01 	sts	0x0181, r25
-  7e:	80 93 80 01 	sts	0x0180, r24
-  82:	b5 c0       	rjmp	.+362    	; 0x1ee <__SREG__+0x1af>
       verifySpace();
     }
     else if(ch == STK_UNIVERSAL) {
-  84:	86 35       	cpi	r24, 0x56	; 86
-  86:	29 f4       	brne	.+10     	; 0x92 <__SREG__+0x53>
+  7e:	86 35       	cpi	r24, 0x56	; 86
+  80:	21 f4       	brne	.+8      	; 0x8a <__SREG__+0x4b>
       // UNIVERSAL command is ignored
       getNch(4);
-  88:	84 e0       	ldi	r24, 0x04	; 4
-  8a:	ec d0       	rcall	.+472    	; 0x264 <getNch>
+  82:	84 e0       	ldi	r24, 0x04	; 4
+  84:	d2 d0       	rcall	.+420    	; 0x22a <getNch>
       putch(0x00);
-  8c:	80 e0       	ldi	r24, 0x00	; 0
-  8e:	b3 d0       	rcall	.+358    	; 0x1f6 <putch>
-  90:	af c0       	rjmp	.+350    	; 0x1f0 <__SREG__+0x1b1>
+  86:	80 e0       	ldi	r24, 0x00	; 0
+  88:	97 c0       	rjmp	.+302    	; 0x1b8 <__SREG__+0x179>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-  92:	84 36       	cpi	r24, 0x64	; 100
-  94:	09 f0       	breq	.+2      	; 0x98 <__SREG__+0x59>
-  96:	6b c0       	rjmp	.+214    	; 0x16e <__SREG__+0x12f>
+  8a:	84 36       	cpi	r24, 0x64	; 100
+  8c:	09 f0       	breq	.+2      	; 0x90 <__SREG__+0x51>
+  8e:	60 c0       	rjmp	.+192    	; 0x150 <__SREG__+0x111>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
-      getLen();
-  98:	d1 d0       	rcall	.+418    	; 0x23c <getLen>
-  9a:	c0 e0       	ldi	r28, 0x00	; 0
-  9c:	d1 e0       	ldi	r29, 0x01	; 1
+      getch();			/* getlen() */
+  90:	ac d0       	rcall	.+344    	; 0x1ea <getch>
+      length = getch();
+  92:	ab d0       	rcall	.+342    	; 0x1ea <getch>
+  94:	f8 2e       	mov	r15, r24
+      getch();
+  96:	a9 d0       	rcall	.+338    	; 0x1ea <getch>
+  98:	c0 e0       	ldi	r28, 0x00	; 0
+  9a:	d1 e0       	ldi	r29, 0x01	; 1
       // If we are in RWW section, immediately start page erase
       if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
 
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-  9e:	ba d0       	rcall	.+372    	; 0x214 <getch>
-  a0:	89 93       	st	Y+, r24
+  9c:	a6 d0       	rcall	.+332    	; 0x1ea <getch>
+  9e:	89 93       	st	Y+, r24
       while (--length);
-  a2:	80 91 82 01 	lds	r24, 0x0182
-  a6:	81 50       	subi	r24, 0x01	; 1
-  a8:	80 93 82 01 	sts	0x0182, r24
-  ac:	88 23       	and	r24, r24
-  ae:	b9 f7       	brne	.-18     	; 0x9e <__SREG__+0x5f>
+  a0:	fc 16       	cp	r15, r28
+  a2:	e1 f7       	brne	.-8      	; 0x9c <__SREG__+0x5d>
 
       // If we are in NRWW section, page erase has to be delayed until now.
       // Todo: Take RAMPZ into account
       if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-  b0:	e0 91 80 01 	lds	r30, 0x0180
-  b4:	f0 91 81 01 	lds	r31, 0x0181
-  b8:	83 e0       	ldi	r24, 0x03	; 3
-  ba:	87 bf       	out	0x37, r24	; 55
-  bc:	e8 95       	spm
+  a4:	83 e0       	ldi	r24, 0x03	; 3
+  a6:	f8 01       	movw	r30, r16
+  a8:	87 bf       	out	0x37, r24	; 55
+  aa:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-  be:	cc d0       	rcall	.+408    	; 0x258 <verifySpace>
+  ac:	b6 d0       	rcall	.+364    	; 0x21a <verifySpace>
 
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-  c0:	07 b6       	in	r0, 0x37	; 55
-  c2:	00 fc       	sbrc	r0, 0
-  c4:	fd cf       	rjmp	.-6      	; 0xc0 <__SREG__+0x81>
+  ae:	07 b6       	in	r0, 0x37	; 55
+  b0:	00 fc       	sbrc	r0, 0
+  b2:	fd cf       	rjmp	.-6      	; 0xae <__SREG__+0x6f>
 
 #ifdef VIRTUAL_BOOT_PARTITION
       if ((uint16_t)(void*)address == 0) {
-  c6:	80 91 80 01 	lds	r24, 0x0180
-  ca:	90 91 81 01 	lds	r25, 0x0181
-  ce:	89 2b       	or	r24, r25
-  d0:	41 f5       	brne	.+80     	; 0x122 <__SREG__+0xe3>
+  b4:	01 15       	cp	r16, r1
+  b6:	11 05       	cpc	r17, r1
+  b8:	11 f0       	breq	.+4      	; 0xbe <__SREG__+0x7f>
+  ba:	a8 01       	movw	r20, r16
+  bc:	2a c0       	rjmp	.+84     	; 0x112 <__SREG__+0xd3>
         // This is the reset vector page. We need to live-patch the code so the
         // bootloader runs.
         //
         // Move RESET vector to WDT vector
         uint16_t vect = buff[0] | (buff[1]<<8);
-  d2:	80 91 00 01 	lds	r24, 0x0100
-  d6:	20 91 01 01 	lds	r18, 0x0101
-  da:	30 e0       	ldi	r19, 0x00	; 0
-  dc:	32 2f       	mov	r19, r18
-  de:	22 27       	eor	r18, r18
-  e0:	90 e0       	ldi	r25, 0x00	; 0
-  e2:	28 2b       	or	r18, r24
-  e4:	39 2b       	or	r19, r25
+  be:	80 91 00 01 	lds	r24, 0x0100
+  c2:	20 91 01 01 	lds	r18, 0x0101
+  c6:	30 e0       	ldi	r19, 0x00	; 0
+  c8:	32 2f       	mov	r19, r18
+  ca:	22 27       	eor	r18, r18
+  cc:	90 e0       	ldi	r25, 0x00	; 0
+  ce:	28 2b       	or	r18, r24
+  d0:	39 2b       	or	r19, r25
         rstVect = vect;
-  e6:	30 93 85 01 	sts	0x0185, r19
-  ea:	20 93 84 01 	sts	0x0184, r18
+  d2:	30 93 85 01 	sts	0x0185, r19
+  d6:	20 93 84 01 	sts	0x0184, r18
         wdtVect = buff[8] | (buff[9]<<8);
-  ee:	40 91 08 01 	lds	r20, 0x0108
-  f2:	80 91 09 01 	lds	r24, 0x0109
-  f6:	90 e0       	ldi	r25, 0x00	; 0
-  f8:	98 2f       	mov	r25, r24
-  fa:	88 27       	eor	r24, r24
-  fc:	50 e0       	ldi	r21, 0x00	; 0
-  fe:	84 2b       	or	r24, r20
- 100:	95 2b       	or	r25, r21
- 102:	90 93 87 01 	sts	0x0187, r25
- 106:	80 93 86 01 	sts	0x0186, r24
+  da:	40 91 08 01 	lds	r20, 0x0108
+  de:	80 91 09 01 	lds	r24, 0x0109
+  e2:	90 e0       	ldi	r25, 0x00	; 0
+  e4:	98 2f       	mov	r25, r24
+  e6:	88 27       	eor	r24, r24
+  e8:	50 e0       	ldi	r21, 0x00	; 0
+  ea:	84 2b       	or	r24, r20
+  ec:	95 2b       	or	r25, r21
+  ee:	90 93 87 01 	sts	0x0187, r25
+  f2:	80 93 86 01 	sts	0x0186, r24
         vect -= 4; // Instruction is a relative jump (rjmp), so recalculate.
- 10a:	24 50       	subi	r18, 0x04	; 4
- 10c:	30 40       	sbci	r19, 0x00	; 0
+  f6:	24 50       	subi	r18, 0x04	; 4
+  f8:	30 40       	sbci	r19, 0x00	; 0
         buff[8] = vect & 0xff;
- 10e:	20 93 08 01 	sts	0x0108, r18
+  fa:	20 93 08 01 	sts	0x0108, r18
         buff[9] = vect >> 8;
- 112:	23 2f       	mov	r18, r19
- 114:	33 27       	eor	r19, r19
- 116:	20 93 09 01 	sts	0x0109, r18
+  fe:	23 2f       	mov	r18, r19
+ 100:	33 27       	eor	r19, r19
+ 102:	20 93 09 01 	sts	0x0109, r18
 
         // Add jump to bootloader at RESET vector
         buff[0] = 0x7f;
- 11a:	f0 92 00 01 	sts	0x0100, r15
+ 106:	d0 92 00 01 	sts	0x0100, r13
         buff[1] = 0xce; // rjmp 0x1d00 instruction
- 11e:	e0 92 01 01 	sts	0x0101, r14
-      }
-#endif
-
-      // Copy buffer into programming buffer
+ 10a:	c0 92 01 01 	sts	0x0101, r12
+ 10e:	40 e0       	ldi	r20, 0x00	; 0
+ 110:	50 e0       	ldi	r21, 0x00	; 0
+ 112:	a0 e0       	ldi	r26, 0x00	; 0
+ 114:	b1 e0       	ldi	r27, 0x01	; 1
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
- 122:	40 91 80 01 	lds	r20, 0x0180
- 126:	50 91 81 01 	lds	r21, 0x0181
- 12a:	a0 e0       	ldi	r26, 0x00	; 0
- 12c:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
- 12e:	2c 91       	ld	r18, X
- 130:	30 e0       	ldi	r19, 0x00	; 0
+ 116:	2c 91       	ld	r18, X
+ 118:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
- 132:	11 96       	adiw	r26, 0x01	; 1
- 134:	8c 91       	ld	r24, X
- 136:	11 97       	sbiw	r26, 0x01	; 1
- 138:	90 e0       	ldi	r25, 0x00	; 0
- 13a:	98 2f       	mov	r25, r24
- 13c:	88 27       	eor	r24, r24
- 13e:	82 2b       	or	r24, r18
- 140:	93 2b       	or	r25, r19
+ 11a:	11 96       	adiw	r26, 0x01	; 1
+ 11c:	8c 91       	ld	r24, X
+ 11e:	11 97       	sbiw	r26, 0x01	; 1
+ 120:	90 e0       	ldi	r25, 0x00	; 0
+ 122:	98 2f       	mov	r25, r24
+ 124:	88 27       	eor	r24, r24
+ 126:	82 2b       	or	r24, r18
+ 128:	93 2b       	or	r25, r19
 #define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
 #define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
 /* main program starts here */
 int main(void) {
- 142:	12 96       	adiw	r26, 0x02	; 2
+ 12a:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
- 144:	fa 01       	movw	r30, r20
- 146:	0c 01       	movw	r0, r24
- 148:	d7 be       	out	0x37, r13	; 55
- 14a:	e8 95       	spm
- 14c:	11 24       	eor	r1, r1
+ 12c:	fa 01       	movw	r30, r20
+ 12e:	0c 01       	movw	r0, r24
+ 130:	b7 be       	out	0x37, r11	; 55
+ 132:	e8 95       	spm
+ 134:	11 24       	eor	r1, r1
         addrPtr += 2;
- 14e:	4e 5f       	subi	r20, 0xFE	; 254
- 150:	5f 4f       	sbci	r21, 0xFF	; 255
+ 136:	4e 5f       	subi	r20, 0xFE	; 254
+ 138:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
- 152:	f1 e0       	ldi	r31, 0x01	; 1
- 154:	a0 34       	cpi	r26, 0x40	; 64
- 156:	bf 07       	cpc	r27, r31
- 158:	51 f7       	brne	.-44     	; 0x12e <__SREG__+0xef>
+ 13a:	f1 e0       	ldi	r31, 0x01	; 1
+ 13c:	a0 34       	cpi	r26, 0x40	; 64
+ 13e:	bf 07       	cpc	r27, r31
+ 140:	51 f7       	brne	.-44     	; 0x116 <__SREG__+0xd7>
 
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
- 15a:	e0 91 80 01 	lds	r30, 0x0180
- 15e:	f0 91 81 01 	lds	r31, 0x0181
- 162:	c7 be       	out	0x37, r12	; 55
- 164:	e8 95       	spm
+ 142:	f8 01       	movw	r30, r16
+ 144:	a7 be       	out	0x37, r10	; 55
+ 146:	e8 95       	spm
       boot_spm_busy_wait();
- 166:	07 b6       	in	r0, 0x37	; 55
- 168:	00 fc       	sbrc	r0, 0
- 16a:	fd cf       	rjmp	.-6      	; 0x166 <__SREG__+0x127>
- 16c:	41 c0       	rjmp	.+130    	; 0x1f0 <__SREG__+0x1b1>
+ 148:	07 b6       	in	r0, 0x37	; 55
+ 14a:	00 fc       	sbrc	r0, 0
+ 14c:	fd cf       	rjmp	.-6      	; 0x148 <__SREG__+0x109>
+ 14e:	3b c0       	rjmp	.+118    	; 0x1c6 <__SREG__+0x187>
       boot_rww_enable();
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
- 16e:	84 37       	cpi	r24, 0x74	; 116
- 170:	89 f5       	brne	.+98     	; 0x1d4 <__SREG__+0x195>
+ 150:	84 37       	cpi	r24, 0x74	; 116
+ 152:	51 f5       	brne	.+84     	; 0x1a8 <__SREG__+0x169>
       // READ PAGE - we only read flash
-      getLen();
- 172:	64 d0       	rcall	.+200    	; 0x23c <getLen>
+      getch();			/* getlen() */
+ 154:	4a d0       	rcall	.+148    	; 0x1ea <getch>
+      length = getch();
+ 156:	49 d0       	rcall	.+146    	; 0x1ea <getch>
+ 158:	f8 2e       	mov	r15, r24
+      getch();
+ 15a:	47 d0       	rcall	.+142    	; 0x1ea <getch>
+
       verifySpace();
- 174:	71 d0       	rcall	.+226    	; 0x258 <verifySpace>
+ 15c:	5e d0       	rcall	.+188    	; 0x21a <verifySpace>
+ 15e:	e8 01       	movw	r28, r16
+ 160:	ef 2c       	mov	r14, r15
 #ifdef VIRTUAL_BOOT_PARTITION
       do {
         // Undo vector patch in bottom page so verify passes
         if (address == 0)       ch=rstVect & 0xff;
- 176:	e0 91 80 01 	lds	r30, 0x0180
- 17a:	f0 91 81 01 	lds	r31, 0x0181
- 17e:	30 97       	sbiw	r30, 0x00	; 0
- 180:	19 f4       	brne	.+6      	; 0x188 <__SREG__+0x149>
- 182:	20 91 84 01 	lds	r18, 0x0184
- 186:	13 c0       	rjmp	.+38     	; 0x1ae <__SREG__+0x16f>
+ 162:	20 97       	sbiw	r28, 0x00	; 0
+ 164:	19 f4       	brne	.+6      	; 0x16c <__SREG__+0x12d>
+ 166:	80 91 84 01 	lds	r24, 0x0184
+ 16a:	14 c0       	rjmp	.+40     	; 0x194 <__SREG__+0x155>
         else if (address == 1)  ch=rstVect >> 8;
- 188:	e1 30       	cpi	r30, 0x01	; 1
- 18a:	f1 05       	cpc	r31, r1
- 18c:	19 f4       	brne	.+6      	; 0x194 <__SREG__+0x155>
- 18e:	20 91 85 01 	lds	r18, 0x0185
- 192:	0d c0       	rjmp	.+26     	; 0x1ae <__SREG__+0x16f>
+ 16c:	c1 30       	cpi	r28, 0x01	; 1
+ 16e:	d1 05       	cpc	r29, r1
+ 170:	19 f4       	brne	.+6      	; 0x178 <__SREG__+0x139>
+ 172:	80 91 85 01 	lds	r24, 0x0185
+ 176:	0e c0       	rjmp	.+28     	; 0x194 <__SREG__+0x155>
         else if (address == 8)  ch=wdtVect & 0xff;
- 194:	e8 30       	cpi	r30, 0x08	; 8
- 196:	f1 05       	cpc	r31, r1
- 198:	19 f4       	brne	.+6      	; 0x1a0 <__SREG__+0x161>
- 19a:	20 91 86 01 	lds	r18, 0x0186
- 19e:	07 c0       	rjmp	.+14     	; 0x1ae <__SREG__+0x16f>
+ 178:	c8 30       	cpi	r28, 0x08	; 8
+ 17a:	d1 05       	cpc	r29, r1
+ 17c:	19 f4       	brne	.+6      	; 0x184 <__SREG__+0x145>
+ 17e:	80 91 86 01 	lds	r24, 0x0186
+ 182:	08 c0       	rjmp	.+16     	; 0x194 <__SREG__+0x155>
         else if (address == 9) ch=wdtVect >> 8;
- 1a0:	e9 30       	cpi	r30, 0x09	; 9
- 1a2:	f1 05       	cpc	r31, r1
- 1a4:	19 f4       	brne	.+6      	; 0x1ac <__SREG__+0x16d>
- 1a6:	20 91 87 01 	lds	r18, 0x0187
- 1aa:	01 c0       	rjmp	.+2      	; 0x1ae <__SREG__+0x16f>
+ 184:	c9 30       	cpi	r28, 0x09	; 9
+ 186:	d1 05       	cpc	r29, r1
+ 188:	19 f4       	brne	.+6      	; 0x190 <__SREG__+0x151>
+ 18a:	80 91 87 01 	lds	r24, 0x0187
+ 18e:	02 c0       	rjmp	.+4      	; 0x194 <__SREG__+0x155>
         else ch = pgm_read_byte_near(address);
- 1ac:	24 91       	lpm	r18, Z+
+ 190:	fe 01       	movw	r30, r28
+ 192:	84 91       	lpm	r24, Z+
         address++;
- 1ae:	80 91 80 01 	lds	r24, 0x0180
- 1b2:	90 91 81 01 	lds	r25, 0x0181
- 1b6:	01 96       	adiw	r24, 0x01	; 1
- 1b8:	90 93 81 01 	sts	0x0181, r25
- 1bc:	80 93 80 01 	sts	0x0180, r24
+ 194:	21 96       	adiw	r28, 0x01	; 1
         putch(ch);
- 1c0:	82 2f       	mov	r24, r18
- 1c2:	19 d0       	rcall	.+50     	; 0x1f6 <putch>
+ 196:	1a d0       	rcall	.+52     	; 0x1cc <putch>
       } while (--length);
- 1c4:	80 91 82 01 	lds	r24, 0x0182
- 1c8:	81 50       	subi	r24, 0x01	; 1
- 1ca:	80 93 82 01 	sts	0x0182, r24
- 1ce:	88 23       	and	r24, r24
- 1d0:	91 f6       	brne	.-92     	; 0x176 <__SREG__+0x137>
- 1d2:	0e c0       	rjmp	.+28     	; 0x1f0 <__SREG__+0x1b1>
+ 198:	ea 94       	dec	r14
+ 19a:	19 f7       	brne	.-58     	; 0x162 <__SREG__+0x123>
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
+#endif
+
+/* main program starts here */
+int main(void) {
+ 19c:	0f 5f       	subi	r16, 0xFF	; 255
+ 19e:	1f 4f       	sbci	r17, 0xFF	; 255
+ 1a0:	fa 94       	dec	r15
+ 1a2:	0f 0d       	add	r16, r15
+ 1a4:	11 1d       	adc	r17, r1
+ 1a6:	0f c0       	rjmp	.+30     	; 0x1c6 <__SREG__+0x187>
 #endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
- 1d4:	85 37       	cpi	r24, 0x75	; 117
- 1d6:	39 f4       	brne	.+14     	; 0x1e6 <__SREG__+0x1a7>
+ 1a8:	85 37       	cpi	r24, 0x75	; 117
+ 1aa:	41 f4       	brne	.+16     	; 0x1bc <__SREG__+0x17d>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
- 1d8:	3f d0       	rcall	.+126    	; 0x258 <verifySpace>
+ 1ac:	36 d0       	rcall	.+108    	; 0x21a <verifySpace>
       putch(SIGNATURE_0);
- 1da:	8e e1       	ldi	r24, 0x1E	; 30
- 1dc:	0c d0       	rcall	.+24     	; 0x1f6 <putch>
+ 1ae:	8e e1       	ldi	r24, 0x1E	; 30
+ 1b0:	0d d0       	rcall	.+26     	; 0x1cc <putch>
       putch(SIGNATURE_1);
- 1de:	83 e9       	ldi	r24, 0x93	; 147
- 1e0:	0a d0       	rcall	.+20     	; 0x1f6 <putch>
+ 1b2:	83 e9       	ldi	r24, 0x93	; 147
+ 1b4:	0b d0       	rcall	.+22     	; 0x1cc <putch>
       putch(SIGNATURE_2);
- 1e2:	8c e0       	ldi	r24, 0x0C	; 12
- 1e4:	54 cf       	rjmp	.-344    	; 0x8e <__SREG__+0x4f>
+ 1b6:	8c e0       	ldi	r24, 0x0C	; 12
+ 1b8:	09 d0       	rcall	.+18     	; 0x1cc <putch>
+ 1ba:	05 c0       	rjmp	.+10     	; 0x1c6 <__SREG__+0x187>
     }
     else if (ch == 'Q') {
- 1e6:	81 35       	cpi	r24, 0x51	; 81
- 1e8:	11 f4       	brne	.+4      	; 0x1ee <__SREG__+0x1af>
+ 1bc:	81 35       	cpi	r24, 0x51	; 81
+ 1be:	11 f4       	brne	.+4      	; 0x1c4 <__SREG__+0x185>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
- 1ea:	88 e0       	ldi	r24, 0x08	; 8
- 1ec:	2c d0       	rcall	.+88     	; 0x246 <watchdogConfig>
+ 1c0:	88 e0       	ldi	r24, 0x08	; 8
+ 1c2:	27 d0       	rcall	.+78     	; 0x212 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
- 1ee:	34 d0       	rcall	.+104    	; 0x258 <verifySpace>
+ 1c4:	2a d0       	rcall	.+84     	; 0x21a <verifySpace>
     }
     putch(STK_OK);
- 1f0:	80 e1       	ldi	r24, 0x10	; 16
- 1f2:	01 d0       	rcall	.+2      	; 0x1f6 <putch>
- 1f4:	25 cf       	rjmp	.-438    	; 0x40 <__SREG__+0x1>
+ 1c6:	80 e1       	ldi	r24, 0x10	; 16
+ 1c8:	01 d0       	rcall	.+2      	; 0x1cc <putch>
+ 1ca:	3a cf       	rjmp	.-396    	; 0x40 <__SREG__+0x1>
 
-000001f6 <putch>:
+000001cc <putch>:
 void putch(char ch) {
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
   UDR0 = ch;
 #else
   __asm__ __volatile__ (
- 1f6:	2a e0       	ldi	r18, 0x0A	; 10
- 1f8:	30 e0       	ldi	r19, 0x00	; 0
- 1fa:	80 95       	com	r24
- 1fc:	08 94       	sec
- 1fe:	10 f4       	brcc	.+4      	; 0x204 <putch+0xe>
- 200:	da 98       	cbi	0x1b, 2	; 27
- 202:	02 c0       	rjmp	.+4      	; 0x208 <putch+0x12>
- 204:	da 9a       	sbi	0x1b, 2	; 27
- 206:	00 00       	nop
- 208:	15 d0       	rcall	.+42     	; 0x234 <uartDelay>
- 20a:	14 d0       	rcall	.+40     	; 0x234 <uartDelay>
- 20c:	86 95       	lsr	r24
- 20e:	2a 95       	dec	r18
- 210:	b1 f7       	brne	.-20     	; 0x1fe <putch+0x8>
+ 1cc:	2a e0       	ldi	r18, 0x0A	; 10
+ 1ce:	30 e0       	ldi	r19, 0x00	; 0
+ 1d0:	80 95       	com	r24
+ 1d2:	08 94       	sec
+ 1d4:	10 f4       	brcc	.+4      	; 0x1da <putch+0xe>
+ 1d6:	da 98       	cbi	0x1b, 2	; 27
+ 1d8:	02 c0       	rjmp	.+4      	; 0x1de <putch+0x12>
+ 1da:	da 9a       	sbi	0x1b, 2	; 27
+ 1dc:	00 00       	nop
+ 1de:	15 d0       	rcall	.+42     	; 0x20a <uartDelay>
+ 1e0:	14 d0       	rcall	.+40     	; 0x20a <uartDelay>
+ 1e2:	86 95       	lsr	r24
+ 1e4:	2a 95       	dec	r18
+ 1e6:	b1 f7       	brne	.-20     	; 0x1d4 <putch+0x8>
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
- 212:	08 95       	ret
+ 1e8:	08 95       	ret
 
-00000214 <getch>:
-  return getch();
+000001ea <getch>:
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
- 214:	a8 95       	wdr
+ 1ea:	a8 95       	wdr
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
- 216:	29 e0       	ldi	r18, 0x09	; 9
- 218:	30 e0       	ldi	r19, 0x00	; 0
- 21a:	cb 99       	sbic	0x19, 3	; 25
- 21c:	fe cf       	rjmp	.-4      	; 0x21a <getch+0x6>
- 21e:	0a d0       	rcall	.+20     	; 0x234 <uartDelay>
- 220:	09 d0       	rcall	.+18     	; 0x234 <uartDelay>
- 222:	08 d0       	rcall	.+16     	; 0x234 <uartDelay>
- 224:	88 94       	clc
- 226:	cb 99       	sbic	0x19, 3	; 25
- 228:	08 94       	sec
- 22a:	2a 95       	dec	r18
- 22c:	11 f0       	breq	.+4      	; 0x232 <getch+0x1e>
- 22e:	87 95       	ror	r24
- 230:	f7 cf       	rjmp	.-18     	; 0x220 <getch+0xc>
- 232:	08 95       	ret
-
-00000234 <uartDelay>:
+ 1ec:	29 e0       	ldi	r18, 0x09	; 9
+ 1ee:	30 e0       	ldi	r19, 0x00	; 0
+ 1f0:	cb 99       	sbic	0x19, 3	; 25
+ 1f2:	fe cf       	rjmp	.-4      	; 0x1f0 <getch+0x6>
+ 1f4:	0a d0       	rcall	.+20     	; 0x20a <uartDelay>
+ 1f6:	09 d0       	rcall	.+18     	; 0x20a <uartDelay>
+ 1f8:	08 d0       	rcall	.+16     	; 0x20a <uartDelay>
+ 1fa:	88 94       	clc
+ 1fc:	cb 99       	sbic	0x19, 3	; 25
+ 1fe:	08 94       	sec
+ 200:	2a 95       	dec	r18
+ 202:	11 f0       	breq	.+4      	; 0x208 <getch+0x1e>
+ 204:	87 95       	ror	r24
+ 206:	f7 cf       	rjmp	.-18     	; 0x1f6 <getch+0xc>
+ 208:	08 95       	ret
+
+0000020a <uartDelay>:
 #if UART_B_VALUE > 255
 #error Baud rate too slow for soft UART
 #endif
 
 void uartDelay() {
   __asm__ __volatile__ (
- 234:	9e e0       	ldi	r25, 0x0E	; 14
- 236:	9a 95       	dec	r25
- 238:	f1 f7       	brne	.-4      	; 0x236 <uartDelay+0x2>
- 23a:	08 95       	ret
-
-0000023c <getLen>:
-  } while (--count);
-}
-#endif
+ 20a:	9e e0       	ldi	r25, 0x0E	; 14
+ 20c:	9a 95       	dec	r25
+ 20e:	f1 f7       	brne	.-4      	; 0x20c <uartDelay+0x2>
+ 210:	08 95       	ret
 
-uint8_t getLen() {
-  getch();
- 23c:	eb df       	rcall	.-42     	; 0x214 <getch>
-  length = getch();
- 23e:	ea df       	rcall	.-44     	; 0x214 <getch>
- 240:	80 93 82 01 	sts	0x0182, r24
-  return getch();
-}
- 244:	e7 cf       	rjmp	.-50     	; 0x214 <getch>
-
-00000246 <watchdogConfig>:
+00000212 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
- 246:	98 e1       	ldi	r25, 0x18	; 24
- 248:	91 bd       	out	0x21, r25	; 33
+ 212:	98 e1       	ldi	r25, 0x18	; 24
+ 214:	91 bd       	out	0x21, r25	; 33
   WDTCSR = x;
- 24a:	81 bd       	out	0x21, r24	; 33
+ 216:	81 bd       	out	0x21, r24	; 33
 }
- 24c:	08 95       	ret
-
-0000024e <appStart>:
-
-void appStart() {
-  watchdogConfig(WATCHDOG_OFF);
- 24e:	80 e0       	ldi	r24, 0x00	; 0
- 250:	fa df       	rcall	.-12     	; 0x246 <watchdogConfig>
-  __asm__ __volatile__ (
- 252:	e4 e0       	ldi	r30, 0x04	; 4
- 254:	ff 27       	eor	r31, r31
- 256:	09 94       	ijmp
+ 218:	08 95       	ret
 
-00000258 <verifySpace>:
+0000021a <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
-  if (getch() != CRC_EOP) appStart();
- 258:	dd df       	rcall	.-70     	; 0x214 <getch>
- 25a:	80 32       	cpi	r24, 0x20	; 32
- 25c:	09 f0       	breq	.+2      	; 0x260 <verifySpace+0x8>
- 25e:	f7 df       	rcall	.-18     	; 0x24e <appStart>
+  if (getch() != CRC_EOP) {
+ 21a:	e7 df       	rcall	.-50     	; 0x1ea <getch>
+ 21c:	80 32       	cpi	r24, 0x20	; 32
+ 21e:	19 f0       	breq	.+6      	; 0x226 <verifySpace+0xc>
+    watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
+ 220:	88 e0       	ldi	r24, 0x08	; 8
+ 222:	f7 df       	rcall	.-18     	; 0x212 <watchdogConfig>
+ 224:	ff cf       	rjmp	.-2      	; 0x224 <verifySpace+0xa>
+    while (1)			      // and busy-loop so that WD causes
+      ;				      //  a reset and app start.
+  }
   putch(STK_INSYNC);
- 260:	84 e1       	ldi	r24, 0x14	; 20
+ 226:	84 e1       	ldi	r24, 0x14	; 20
 }
- 262:	c9 cf       	rjmp	.-110    	; 0x1f6 <putch>
+ 228:	d1 cf       	rjmp	.-94     	; 0x1cc <putch>
 
-00000264 <getNch>:
+0000022a <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
- 264:	1f 93       	push	r17
- 266:	18 2f       	mov	r17, r24
+ 22a:	1f 93       	push	r17
+ 22c:	18 2f       	mov	r17, r24
   do getch(); while (--count);
- 268:	d5 df       	rcall	.-86     	; 0x214 <getch>
- 26a:	11 50       	subi	r17, 0x01	; 1
- 26c:	e9 f7       	brne	.-6      	; 0x268 <getNch+0x4>
+ 22e:	dd df       	rcall	.-70     	; 0x1ea <getch>
+ 230:	11 50       	subi	r17, 0x01	; 1
+ 232:	e9 f7       	brne	.-6      	; 0x22e <getNch+0x4>
   verifySpace();
- 26e:	f4 df       	rcall	.-24     	; 0x258 <verifySpace>
+ 234:	f2 df       	rcall	.-28     	; 0x21a <verifySpace>
 }
- 270:	1f 91       	pop	r17
- 272:	08 95       	ret
+ 236:	1f 91       	pop	r17
+ 238:	08 95       	ret
+
+0000023a <appStart>:
+  WDTCSR = _BV(WDCE) | _BV(WDE);
+  WDTCSR = x;
+}
+
+void appStart() {
+  watchdogConfig(WATCHDOG_OFF);
+ 23a:	80 e0       	ldi	r24, 0x00	; 0
+ 23c:	ea df       	rcall	.-44     	; 0x212 <watchdogConfig>
+  __asm__ __volatile__ (
+ 23e:	e4 e0       	ldi	r30, 0x04	; 4
+ 240:	ff 27       	eor	r31, r31
+ 242:	09 94       	ijmp
diff --git a/arduino/bootloaders/optiboot/optiboot_pro_16MHz.hex b/arduino/bootloaders/optiboot/optiboot_pro_16MHz.hex
index 3ac0de2..733a139 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_16MHz.hex
+++ b/arduino/bootloaders/optiboot/optiboot_pro_16MHz.hex
@@ -1,35 +1,33 @@
-:103E0000112484B714BE81FFE6D085E08093810041
+:103E0000112484B714BE81FFE3D085E08093810044
 :103E100082E08093C00088E18093C10086E08093B7
-:103E2000C20080E18093C4008EE0CFD0259A86E066
+:103E2000C20080E18093C4008EE0BCD0259A86E079
 :103E300020E33CEF91E0309385002093840096BB13
-:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D
-:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020
-:103E6000C5D083E020C0823411F484E103C08534DE
-:103E700019F485E0BBD091C0853581F499D0082F25
-:103E800010E096D090E0982F8827802B912B880FF8
-:103E9000991F90930102809300027EC0863529F419
-:103EA00084E0A4D080E07CD078C0843609F04EC095
-:103EB00087D0E0910002F091010288E3E030F8073A
-:103EC00018F483E087BFE895C0E0D1E071D0899312
-:103ED000809102028150809302028823B9F7E09119
-:103EE0000002F091010288E3E030F80718F083E067
-:103EF00087BFE89575D007B600FCFDCF4091000262
-:103F000050910102A0E0B1E02C9130E011968C912B
-:103F1000119790E0982F8827822B932B1296FA0105
-:103F20000C01D7BEE89511244E5F5F4FF1E0A03839
-:103F3000BF0751F7E0910002F0910102E7BEE8955A
-:103F400007B600FCFDCFF7BEE89527C08437B9F46B
-:103F500037D046D0E0910002F09101023196F09303
-:103F60000102E09300023197E4918E2F19D08091E5
-:103F70000202815080930202882361F70EC08537C8
-:103F800039F42ED08EE10CD084E90AD086E08BCFB4
-:103F9000813511F488E019D023D080E101D05CCFC5
-:103FA000982F8091C00085FFFCCF9093C6000895A4
-:103FB000A8958091C00087FFFCCF8091C60008952E
-:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E
-:103FD00090838083089580E0F8DFEE27FF2709941F
-:103FE000E7DF803209F0F7DF84E1DACF1F93182F83
-:0C3FF000DFDF1150E9F7F4DF1F910895A6
+:103E4000B09BFECF1D9AA8958150A9F79924939411
+:103E5000A5E0AA2EF1E1BF2E9DD0813421F481E0AE
+:103E6000AFD083E01FC0823411F484E103C08534F5
+:103E700019F485E0A5D083C0853579F48BD0E82E80
+:103E8000FF2488D0082F10E0102F00270E291F29AB
+:103E9000000F111F8DD0680172C0863529F484E0AF
+:103EA0008FD080E06FD06BC0843609F042C072D0F2
+:103EB00071D0082F6FD080E0C81688E3D80620F4B0
+:103EC00083E0F60187BFE895C0E0D1E063D0899335
+:103ED0000C17E1F7F0E0CF16F8E3DF0620F083E0FF
+:103EE000F60187BFE89564D007B600FCFDCFA601B8
+:103EF000A0E0B1E02C9130E011968C91119790E008
+:103F0000982F8827822B932B1296FA010C0197BECB
+:103F1000E89511244E5F5F4FF1E0A038BF0751F7DD
+:103F2000F601A7BEE89507B600FCFDCFB7BEE89541
+:103F300026C08437B1F42ED02DD0F82E2BD038D017
+:103F4000F601EF2C8F010F5F1F4F84911BD0EA9475
+:103F5000F801C1F70894C11CD11CFA94CF0CD11CF4
+:103F60000EC0853739F424D08EE10CD084E90AD014
+:103F700086E098CF813511F488E014D019D080E123
+:103F800001D06ACF982F8091C00085FFFCCF90931D
+:103F9000C6000895A8958091C00087FFFCCF80914E
+:103FA000C6000895E0E6F0E098E1908380830895EC
+:103FB000F1DF803219F088E0F5DFFFCF84E1E2CF56
+:103FC0001F93182FE7DF1150E9F7F2DF1F910895D3
+:0A3FD00080E0E8DFEE27FF270994E8
 :023FFE000104BC
 :0400000300003E00BB
 :00000001FF
diff --git a/arduino/bootloaders/optiboot/optiboot_pro_16MHz.lst b/arduino/bootloaders/optiboot/optiboot_pro_16MHz.lst
index 516bed8..b1d8c1b 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_16MHz.lst
+++ b/arduino/bootloaders/optiboot/optiboot_pro_16MHz.lst
@@ -3,27 +3,27 @@ optiboot_pro_16MHz.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001fc  00003e00  00003e00  00000054  2**1
+  0 .text         000001da  00003e00  00003e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00003ffe  00003ffe  00000250  2**0
+  1 .version      00000002  00003ffe  00003ffe  0000022e  2**0
                   CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000252  2**0
+  2 .debug_aranges 00000028  00000000  00000000  00000230  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000006a  00000000  00000000  0000027a  2**0
+  3 .debug_pubnames 0000005f  00000000  00000000  00000258  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000285  00000000  00000000  000002e4  2**0
+  4 .debug_info   0000028c  00000000  00000000  000002b7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000019f  00000000  00000000  00000569  2**0
+  5 .debug_abbrev 00000199  00000000  00000000  00000543  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000453  00000000  00000000  00000708  2**0
+  6 .debug_line   00000456  00000000  00000000  000006dc  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000090  00000000  00000000  00000b5c  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000b34  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000141  00000000  00000000  00000bec  2**0
+  8 .debug_str    00000149  00000000  00000000  00000bb4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    000001e1  00000000  00000000  00000d2d  2**0
+  9 .debug_loc    0000027e  00000000  00000000  00000cfd  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000068  00000000  00000000  00000f0e  2**0
+ 10 .debug_ranges 00000060  00000000  00000000  00000f7b  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
@@ -47,7 +47,7 @@ int main(void) {
     3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
     3e06:	81 ff       	sbrs	r24, 1
-    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+    3e08:	e3 d0       	rcall	.+454    	; 0x3fd0 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
@@ -77,7 +77,7 @@ int main(void) {
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
     3e28:	8e e0       	ldi	r24, 0x0E	; 14
-    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
+    3e2a:	bc d0       	rcall	.+376    	; 0x3fa4 <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
@@ -111,8 +111,8 @@ void flash_led(uint8_t count) {
 #else
     LED_PIN |= _BV(LED);
     3e44:	1d 9a       	sbi	0x03, 5	; 3
-  return getch();
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
@@ -132,8 +132,8 @@ void watchdogReset() {
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e4c:	dd 24       	eor	r13, r13
-    3e4e:	d3 94       	inc	r13
+    3e4c:	99 24       	eor	r9, r9
+    3e4e:	93 94       	inc	r9
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
@@ -141,21 +141,21 @@ void watchdogReset() {
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
     3e50:	a5 e0       	ldi	r26, 0x05	; 5
-    3e52:	ea 2e       	mov	r14, r26
+    3e52:	aa 2e       	mov	r10, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
     3e54:	f1 e1       	ldi	r31, 0x11	; 17
-    3e56:	ff 2e       	mov	r15, r31
+    3e56:	bf 2e       	mov	r11, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
+    3e58:	9d d0       	rcall	.+314    	; 0x3f94 <getch>
 
     if(ch == STK_GET_PARAMETER) {
     3e5a:	81 34       	cpi	r24, 0x41	; 65
@@ -163,10 +163,10 @@ void watchdogReset() {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
     3e5e:	81 e0       	ldi	r24, 0x01	; 1
-    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
+    3e60:	af d0       	rcall	.+350    	; 0x3fc0 <getNch>
       putch(0x03);
     3e62:	83 e0       	ldi	r24, 0x03	; 3
-    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
+    3e64:	1f c0       	rjmp	.+62     	; 0x3ea4 <main+0xa4>
     }
     else if(ch == STK_SET_DEVICE) {
     3e66:	82 34       	cpi	r24, 0x42	; 66
@@ -182,71 +182,77 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     3e72:	85 e0       	ldi	r24, 0x05	; 5
-    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
-    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
+    3e74:	a5 d0       	rcall	.+330    	; 0x3fc0 <getNch>
+    3e76:	83 c0       	rjmp	.+262    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     3e78:	85 35       	cpi	r24, 0x55	; 85
-    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
+    3e7a:	79 f4       	brne	.+30     	; 0x3e9a <main+0x9a>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+    3e7c:	8b d0       	rcall	.+278    	; 0x3f94 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-    3e7e:	08 2f       	mov	r16, r24
-    3e80:	10 e0       	ldi	r17, 0x00	; 0
-    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
-    3e84:	90 e0       	ldi	r25, 0x00	; 0
-    3e86:	98 2f       	mov	r25, r24
-    3e88:	88 27       	eor	r24, r24
-    3e8a:	80 2b       	or	r24, r16
-    3e8c:	91 2b       	or	r25, r17
+    3e7e:	e8 2e       	mov	r14, r24
+    3e80:	ff 24       	eor	r15, r15
+    3e82:	88 d0       	rcall	.+272    	; 0x3f94 <getch>
+    3e84:	08 2f       	mov	r16, r24
+    3e86:	10 e0       	ldi	r17, 0x00	; 0
+    3e88:	10 2f       	mov	r17, r16
+    3e8a:	00 27       	eor	r16, r16
+    3e8c:	0e 29       	or	r16, r14
+    3e8e:	1f 29       	or	r17, r15
 #ifdef RAMPZ
       // Transfer top bit to RAMPZ
       RAMPZ = (newAddress & 0x8000) ? 1 : 0;
 #endif
       newAddress += newAddress; // Convert from word address to byte address
-    3e8e:	88 0f       	add	r24, r24
-    3e90:	99 1f       	adc	r25, r25
+    3e90:	00 0f       	add	r16, r16
+    3e92:	11 1f       	adc	r17, r17
       address = newAddress;
-    3e92:	90 93 01 02 	sts	0x0201, r25
-    3e96:	80 93 00 02 	sts	0x0200, r24
-    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
+    3e94:	8d d0       	rcall	.+282    	; 0x3fb0 <verifySpace>
+    3e96:	68 01       	movw	r12, r16
+    3e98:	72 c0       	rjmp	.+228    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_UNIVERSAL) {
-    3e9c:	86 35       	cpi	r24, 0x56	; 86
-    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
+    3e9a:	86 35       	cpi	r24, 0x56	; 86
+    3e9c:	29 f4       	brne	.+10     	; 0x3ea8 <main+0xa8>
       // UNIVERSAL command is ignored
       getNch(4);
-    3ea0:	84 e0       	ldi	r24, 0x04	; 4
-    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
+    3e9e:	84 e0       	ldi	r24, 0x04	; 4
+    3ea0:	8f d0       	rcall	.+286    	; 0x3fc0 <getNch>
       putch(0x00);
-    3ea4:	80 e0       	ldi	r24, 0x00	; 0
-    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
-    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
+    3ea2:	80 e0       	ldi	r24, 0x00	; 0
+    3ea4:	6f d0       	rcall	.+222    	; 0x3f84 <putch>
+    3ea6:	6b c0       	rjmp	.+214    	; 0x3f7e <main+0x17e>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3eaa:	84 36       	cpi	r24, 0x64	; 100
-    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
-    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
+    3ea8:	84 36       	cpi	r24, 0x64	; 100
+    3eaa:	09 f0       	breq	.+2      	; 0x3eae <main+0xae>
+    3eac:	42 c0       	rjmp	.+132    	; 0x3f32 <main+0x132>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
-      getLen();
-    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3eae:	72 d0       	rcall	.+228    	; 0x3f94 <getch>
+      length = getch();
+    3eb0:	71 d0       	rcall	.+226    	; 0x3f94 <getch>
+    3eb2:	08 2f       	mov	r16, r24
+      getch();
+    3eb4:	6f d0       	rcall	.+222    	; 0x3f94 <getch>
 
       // If we are in RWW section, immediately start page erase
       if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3eb2:	e0 91 00 02 	lds	r30, 0x0200
-    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eb6:	80 e0       	ldi	r24, 0x00	; 0
+    3eb8:	c8 16       	cp	r12, r24
     3eba:	88 e3       	ldi	r24, 0x38	; 56
-    3ebc:	e0 30       	cpi	r30, 0x00	; 0
-    3ebe:	f8 07       	cpc	r31, r24
-    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
-    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ebc:	d8 06       	cpc	r13, r24
+    3ebe:	20 f4       	brcc	.+8      	; 0x3ec8 <main+0xc8>
+    3ec0:	83 e0       	ldi	r24, 0x03	; 3
+    3ec2:	f6 01       	movw	r30, r12
     3ec4:	87 bf       	out	0x37, r24	; 55
     3ec6:	e8 95       	spm
     3ec8:	c0 e0       	ldi	r28, 0x00	; 0
@@ -255,302 +261,301 @@ void watchdogReset() {
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
+    3ecc:	63 d0       	rcall	.+198    	; 0x3f94 <getch>
     3ece:	89 93       	st	Y+, r24
       while (--length);
-    3ed0:	80 91 02 02 	lds	r24, 0x0202
-    3ed4:	81 50       	subi	r24, 0x01	; 1
-    3ed6:	80 93 02 02 	sts	0x0202, r24
-    3eda:	88 23       	and	r24, r24
-    3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
+    3ed0:	0c 17       	cp	r16, r28
+    3ed2:	e1 f7       	brne	.-8      	; 0x3ecc <main+0xcc>
 
       // If we are in NRWW section, page erase has to be delayed until now.
       // Todo: Take RAMPZ into account
       if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ede:	e0 91 00 02 	lds	r30, 0x0200
-    3ee2:	f0 91 01 02 	lds	r31, 0x0201
-    3ee6:	88 e3       	ldi	r24, 0x38	; 56
-    3ee8:	e0 30       	cpi	r30, 0x00	; 0
-    3eea:	f8 07       	cpc	r31, r24
-    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
-    3eee:	83 e0       	ldi	r24, 0x03	; 3
-    3ef0:	87 bf       	out	0x37, r24	; 55
-    3ef2:	e8 95       	spm
+    3ed4:	f0 e0       	ldi	r31, 0x00	; 0
+    3ed6:	cf 16       	cp	r12, r31
+    3ed8:	f8 e3       	ldi	r31, 0x38	; 56
+    3eda:	df 06       	cpc	r13, r31
+    3edc:	20 f0       	brcs	.+8      	; 0x3ee6 <main+0xe6>
+    3ede:	83 e0       	ldi	r24, 0x03	; 3
+    3ee0:	f6 01       	movw	r30, r12
+    3ee2:	87 bf       	out	0x37, r24	; 55
+    3ee4:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
+    3ee6:	64 d0       	rcall	.+200    	; 0x3fb0 <verifySpace>
 
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3ef6:	07 b6       	in	r0, 0x37	; 55
-    3ef8:	00 fc       	sbrc	r0, 0
-    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
-      }
-#endif
-
-      // Copy buffer into programming buffer
+    3ee8:	07 b6       	in	r0, 0x37	; 55
+    3eea:	00 fc       	sbrc	r0, 0
+    3eec:	fd cf       	rjmp	.-6      	; 0x3ee8 <main+0xe8>
+    3eee:	a6 01       	movw	r20, r12
+    3ef0:	a0 e0       	ldi	r26, 0x00	; 0
+    3ef2:	b1 e0       	ldi	r27, 0x01	; 1
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3efc:	40 91 00 02 	lds	r20, 0x0200
-    3f00:	50 91 01 02 	lds	r21, 0x0201
-    3f04:	a0 e0       	ldi	r26, 0x00	; 0
-    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3f08:	2c 91       	ld	r18, X
-    3f0a:	30 e0       	ldi	r19, 0x00	; 0
+    3ef4:	2c 91       	ld	r18, X
+    3ef6:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3f0c:	11 96       	adiw	r26, 0x01	; 1
-    3f0e:	8c 91       	ld	r24, X
-    3f10:	11 97       	sbiw	r26, 0x01	; 1
-    3f12:	90 e0       	ldi	r25, 0x00	; 0
-    3f14:	98 2f       	mov	r25, r24
-    3f16:	88 27       	eor	r24, r24
-    3f18:	82 2b       	or	r24, r18
-    3f1a:	93 2b       	or	r25, r19
+    3ef8:	11 96       	adiw	r26, 0x01	; 1
+    3efa:	8c 91       	ld	r24, X
+    3efc:	11 97       	sbiw	r26, 0x01	; 1
+    3efe:	90 e0       	ldi	r25, 0x00	; 0
+    3f00:	98 2f       	mov	r25, r24
+    3f02:	88 27       	eor	r24, r24
+    3f04:	82 2b       	or	r24, r18
+    3f06:	93 2b       	or	r25, r19
 #define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
 #define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
 /* main program starts here */
 int main(void) {
-    3f1c:	12 96       	adiw	r26, 0x02	; 2
+    3f08:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    3f1e:	fa 01       	movw	r30, r20
-    3f20:	0c 01       	movw	r0, r24
-    3f22:	d7 be       	out	0x37, r13	; 55
-    3f24:	e8 95       	spm
-    3f26:	11 24       	eor	r1, r1
+    3f0a:	fa 01       	movw	r30, r20
+    3f0c:	0c 01       	movw	r0, r24
+    3f0e:	97 be       	out	0x37, r9	; 55
+    3f10:	e8 95       	spm
+    3f12:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3f28:	4e 5f       	subi	r20, 0xFE	; 254
-    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f14:	4e 5f       	subi	r20, 0xFE	; 254
+    3f16:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
-    3f2e:	a0 38       	cpi	r26, 0x80	; 128
-    3f30:	bf 07       	cpc	r27, r31
-    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
+    3f18:	f1 e0       	ldi	r31, 0x01	; 1
+    3f1a:	a0 38       	cpi	r26, 0x80	; 128
+    3f1c:	bf 07       	cpc	r27, r31
+    3f1e:	51 f7       	brne	.-44     	; 0x3ef4 <main+0xf4>
 
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
-    3f34:	e0 91 00 02 	lds	r30, 0x0200
-    3f38:	f0 91 01 02 	lds	r31, 0x0201
-    3f3c:	e7 be       	out	0x37, r14	; 55
-    3f3e:	e8 95       	spm
+    3f20:	f6 01       	movw	r30, r12
+    3f22:	a7 be       	out	0x37, r10	; 55
+    3f24:	e8 95       	spm
       boot_spm_busy_wait();
-    3f40:	07 b6       	in	r0, 0x37	; 55
-    3f42:	00 fc       	sbrc	r0, 0
-    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
+    3f26:	07 b6       	in	r0, 0x37	; 55
+    3f28:	00 fc       	sbrc	r0, 0
+    3f2a:	fd cf       	rjmp	.-6      	; 0x3f26 <main+0x126>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f46:	f7 be       	out	0x37, r15	; 55
-    3f48:	e8 95       	spm
-    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
+    3f2c:	b7 be       	out	0x37, r11	; 55
+    3f2e:	e8 95       	spm
+    3f30:	26 c0       	rjmp	.+76     	; 0x3f7e <main+0x17e>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f4c:	84 37       	cpi	r24, 0x74	; 116
-    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
+    3f32:	84 37       	cpi	r24, 0x74	; 116
+    3f34:	b1 f4       	brne	.+44     	; 0x3f62 <main+0x162>
       // READ PAGE - we only read flash
-      getLen();
-    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3f36:	2e d0       	rcall	.+92     	; 0x3f94 <getch>
+      length = getch();
+    3f38:	2d d0       	rcall	.+90     	; 0x3f94 <getch>
+    3f3a:	f8 2e       	mov	r15, r24
+      getch();
+    3f3c:	2b d0       	rcall	.+86     	; 0x3f94 <getch>
+
       verifySpace();
-    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+    3f3e:	38 d0       	rcall	.+112    	; 0x3fb0 <verifySpace>
+    3f40:	f6 01       	movw	r30, r12
+    3f42:	ef 2c       	mov	r14, r15
         putch(result);
         address++;
       }
       while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f54:	e0 91 00 02 	lds	r30, 0x0200
-    3f58:	f0 91 01 02 	lds	r31, 0x0201
-    3f5c:	31 96       	adiw	r30, 0x01	; 1
-    3f5e:	f0 93 01 02 	sts	0x0201, r31
-    3f62:	e0 93 00 02 	sts	0x0200, r30
-    3f66:	31 97       	sbiw	r30, 0x01	; 1
-    3f68:	e4 91       	lpm	r30, Z+
-    3f6a:	8e 2f       	mov	r24, r30
-    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
+    3f44:	8f 01       	movw	r16, r30
+    3f46:	0f 5f       	subi	r16, 0xFF	; 255
+    3f48:	1f 4f       	sbci	r17, 0xFF	; 255
+    3f4a:	84 91       	lpm	r24, Z+
+    3f4c:	1b d0       	rcall	.+54     	; 0x3f84 <putch>
       while (--length);
-    3f6e:	80 91 02 02 	lds	r24, 0x0202
-    3f72:	81 50       	subi	r24, 0x01	; 1
-    3f74:	80 93 02 02 	sts	0x0202, r24
-    3f78:	88 23       	and	r24, r24
-    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
-    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+    3f4e:	ea 94       	dec	r14
+    3f50:	f8 01       	movw	r30, r16
+    3f52:	c1 f7       	brne	.-16     	; 0x3f44 <main+0x144>
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
+#endif
+
+/* main program starts here */
+int main(void) {
+    3f54:	08 94       	sec
+    3f56:	c1 1c       	adc	r12, r1
+    3f58:	d1 1c       	adc	r13, r1
+    3f5a:	fa 94       	dec	r15
+    3f5c:	cf 0c       	add	r12, r15
+    3f5e:	d1 1c       	adc	r13, r1
+    3f60:	0e c0       	rjmp	.+28     	; 0x3f7e <main+0x17e>
 #endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f7e:	85 37       	cpi	r24, 0x75	; 117
-    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
+    3f62:	85 37       	cpi	r24, 0x75	; 117
+    3f64:	39 f4       	brne	.+14     	; 0x3f74 <main+0x174>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
+    3f66:	24 d0       	rcall	.+72     	; 0x3fb0 <verifySpace>
       putch(SIGNATURE_0);
-    3f84:	8e e1       	ldi	r24, 0x1E	; 30
-    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
+    3f68:	8e e1       	ldi	r24, 0x1E	; 30
+    3f6a:	0c d0       	rcall	.+24     	; 0x3f84 <putch>
       putch(SIGNATURE_1);
-    3f88:	84 e9       	ldi	r24, 0x94	; 148
-    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
+    3f6c:	84 e9       	ldi	r24, 0x94	; 148
+    3f6e:	0a d0       	rcall	.+20     	; 0x3f84 <putch>
       putch(SIGNATURE_2);
-    3f8c:	86 e0       	ldi	r24, 0x06	; 6
-    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
+    3f70:	86 e0       	ldi	r24, 0x06	; 6
+    3f72:	98 cf       	rjmp	.-208    	; 0x3ea4 <main+0xa4>
     }
     else if (ch == 'Q') {
-    3f90:	81 35       	cpi	r24, 0x51	; 81
-    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
+    3f74:	81 35       	cpi	r24, 0x51	; 81
+    3f76:	11 f4       	brne	.+4      	; 0x3f7c <main+0x17c>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f94:	88 e0       	ldi	r24, 0x08	; 8
-    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
+    3f78:	88 e0       	ldi	r24, 0x08	; 8
+    3f7a:	14 d0       	rcall	.+40     	; 0x3fa4 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
+    3f7c:	19 d0       	rcall	.+50     	; 0x3fb0 <verifySpace>
     }
     putch(STK_OK);
-    3f9a:	80 e1       	ldi	r24, 0x10	; 16
-    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
-    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
+    3f7e:	80 e1       	ldi	r24, 0x10	; 16
+    3f80:	01 d0       	rcall	.+2      	; 0x3f84 <putch>
+    3f82:	6a cf       	rjmp	.-300    	; 0x3e58 <main+0x58>
 
-00003fa0 <putch>:
+00003f84 <putch>:
   }
 }
 
 void putch(char ch) {
-    3fa0:	98 2f       	mov	r25, r24
+    3f84:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fa6:	85 ff       	sbrs	r24, 5
-    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
+    3f86:	80 91 c0 00 	lds	r24, 0x00C0
+    3f8a:	85 ff       	sbrs	r24, 5
+    3f8c:	fc cf       	rjmp	.-8      	; 0x3f86 <putch+0x2>
   UDR0 = ch;
-    3faa:	90 93 c6 00 	sts	0x00C6, r25
+    3f8e:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3fae:	08 95       	ret
+    3f92:	08 95       	ret
 
-00003fb0 <getch>:
-  return getch();
+00003f94 <getch>:
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fb0:	a8 95       	wdr
+    3f94:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fb6:	87 ff       	sbrs	r24, 7
-    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
+    3f96:	80 91 c0 00 	lds	r24, 0x00C0
+    3f9a:	87 ff       	sbrs	r24, 7
+    3f9c:	fc cf       	rjmp	.-8      	; 0x3f96 <getch+0x2>
   ch = UDR0;
-    3fba:	80 91 c6 00 	lds	r24, 0x00C6
+    3f9e:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    3fbe:	08 95       	ret
-
-00003fc0 <getLen>:
-  } while (--count);
-}
-#endif
-
-uint8_t getLen() {
-  getch();
-    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
-  length = getch();
-    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
-    3fc4:	80 93 02 02 	sts	0x0202, r24
-  return getch();
-}
-    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
+    3fa2:	08 95       	ret
 
-00003fca <watchdogConfig>:
+00003fa4 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3fca:	e0 e6       	ldi	r30, 0x60	; 96
-    3fcc:	f0 e0       	ldi	r31, 0x00	; 0
-    3fce:	98 e1       	ldi	r25, 0x18	; 24
-    3fd0:	90 83       	st	Z, r25
+    3fa4:	e0 e6       	ldi	r30, 0x60	; 96
+    3fa6:	f0 e0       	ldi	r31, 0x00	; 0
+    3fa8:	98 e1       	ldi	r25, 0x18	; 24
+    3faa:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fd2:	80 83       	st	Z, r24
+    3fac:	80 83       	st	Z, r24
 }
-    3fd4:	08 95       	ret
-
-00003fd6 <appStart>:
-
-void appStart() {
-  watchdogConfig(WATCHDOG_OFF);
-    3fd6:	80 e0       	ldi	r24, 0x00	; 0
-    3fd8:	f8 df       	rcall	.-16     	; 0x3fca <watchdogConfig>
-  __asm__ __volatile__ (
-    3fda:	ee 27       	eor	r30, r30
-    3fdc:	ff 27       	eor	r31, r31
-    3fde:	09 94       	ijmp
+    3fae:	08 95       	ret
 
-00003fe0 <verifySpace>:
+00003fb0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
-  if (getch() != CRC_EOP) appStart();
-    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
-    3fe2:	80 32       	cpi	r24, 0x20	; 32
-    3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
-    3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
+  if (getch() != CRC_EOP) {
+    3fb0:	f1 df       	rcall	.-30     	; 0x3f94 <getch>
+    3fb2:	80 32       	cpi	r24, 0x20	; 32
+    3fb4:	19 f0       	breq	.+6      	; 0x3fbc <verifySpace+0xc>
+    watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
+    3fb6:	88 e0       	ldi	r24, 0x08	; 8
+    3fb8:	f5 df       	rcall	.-22     	; 0x3fa4 <watchdogConfig>
+    3fba:	ff cf       	rjmp	.-2      	; 0x3fba <verifySpace+0xa>
+    while (1)			      // and busy-loop so that WD causes
+      ;				      //  a reset and app start.
+  }
   putch(STK_INSYNC);
-    3fe8:	84 e1       	ldi	r24, 0x14	; 20
+    3fbc:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+    3fbe:	e2 cf       	rjmp	.-60     	; 0x3f84 <putch>
 
-00003fec <getNch>:
+00003fc0 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fec:	1f 93       	push	r17
-    3fee:	18 2f       	mov	r17, r24
+    3fc0:	1f 93       	push	r17
+    3fc2:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
-    3ff2:	11 50       	subi	r17, 0x01	; 1
-    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
+    3fc4:	e7 df       	rcall	.-50     	; 0x3f94 <getch>
+    3fc6:	11 50       	subi	r17, 0x01	; 1
+    3fc8:	e9 f7       	brne	.-6      	; 0x3fc4 <getNch+0x4>
   verifySpace();
-    3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
+    3fca:	f2 df       	rcall	.-28     	; 0x3fb0 <verifySpace>
+}
+    3fcc:	1f 91       	pop	r17
+    3fce:	08 95       	ret
+
+00003fd0 <appStart>:
+  WDTCSR = _BV(WDCE) | _BV(WDE);
+  WDTCSR = x;
 }
-    3ff8:	1f 91       	pop	r17
-    3ffa:	08 95       	ret
+
+void appStart() {
+  watchdogConfig(WATCHDOG_OFF);
+    3fd0:	80 e0       	ldi	r24, 0x00	; 0
+    3fd2:	e8 df       	rcall	.-48     	; 0x3fa4 <watchdogConfig>
+  __asm__ __volatile__ (
+    3fd4:	ee 27       	eor	r30, r30
+    3fd6:	ff 27       	eor	r31, r31
+    3fd8:	09 94       	ijmp
diff --git a/arduino/bootloaders/optiboot/optiboot_pro_20mhz.hex b/arduino/bootloaders/optiboot/optiboot_pro_20mhz.hex
index 01c974c..58ed1dc 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_20mhz.hex
+++ b/arduino/bootloaders/optiboot/optiboot_pro_20mhz.hex
@@ -1,35 +1,33 @@
-:103E0000112484B714BE81FFE6D085E08093810041
+:103E0000112484B714BE81FFE3D085E08093810044
 :103E100082E08093C00088E18093C10086E08093B7
-:103E2000C20085E18093C4008EE0CFD0259A86E061
+:103E2000C20085E18093C4008EE0BCD0259A86E074
 :103E30002CE33BEF91E0309385002093840096BB08
-:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D
-:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020
-:103E6000C5D083E020C0823411F484E103C08534DE
-:103E700019F485E0BBD091C0853581F499D0082F25
-:103E800010E096D090E0982F8827802B912B880FF8
-:103E9000991F90930102809300027EC0863529F419
-:103EA00084E0A4D080E07CD078C0843609F04EC095
-:103EB00087D0E0910002F091010288E3E030F8073A
-:103EC00018F483E087BFE895C0E0D1E071D0899312
-:103ED000809102028150809302028823B9F7E09119
-:103EE0000002F091010288E3E030F80718F083E067
-:103EF00087BFE89575D007B600FCFDCF4091000262
-:103F000050910102A0E0B1E02C9130E011968C912B
-:103F1000119790E0982F8827822B932B1296FA0105
-:103F20000C01D7BEE89511244E5F5F4FF1E0A03839
-:103F3000BF0751F7E0910002F0910102E7BEE8955A
-:103F400007B600FCFDCFF7BEE89527C08437B9F46B
-:103F500037D046D0E0910002F09101023196F09303
-:103F60000102E09300023197E4918E2F19D08091E5
-:103F70000202815080930202882361F70EC08537C8
-:103F800039F42ED08EE10CD084E90AD086E08BCFB4
-:103F9000813511F488E019D023D080E101D05CCFC5
-:103FA000982F8091C00085FFFCCF9093C6000895A4
-:103FB000A8958091C00087FFFCCF8091C60008952E
-:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E
-:103FD00090838083089580E0F8DFEE27FF2709941F
-:103FE000E7DF803209F0F7DF84E1DACF1F93182F83
-:0C3FF000DFDF1150E9F7F4DF1F910895A6
+:103E4000B09BFECF1D9AA8958150A9F79924939411
+:103E5000A5E0AA2EF1E1BF2E9DD0813421F481E0AE
+:103E6000AFD083E01FC0823411F484E103C08534F5
+:103E700019F485E0A5D083C0853579F48BD0E82E80
+:103E8000FF2488D0082F10E0102F00270E291F29AB
+:103E9000000F111F8DD0680172C0863529F484E0AF
+:103EA0008FD080E06FD06BC0843609F042C072D0F2
+:103EB00071D0082F6FD080E0C81688E3D80620F4B0
+:103EC00083E0F60187BFE895C0E0D1E063D0899335
+:103ED0000C17E1F7F0E0CF16F8E3DF0620F083E0FF
+:103EE000F60187BFE89564D007B600FCFDCFA601B8
+:103EF000A0E0B1E02C9130E011968C91119790E008
+:103F0000982F8827822B932B1296FA010C0197BECB
+:103F1000E89511244E5F5F4FF1E0A038BF0751F7DD
+:103F2000F601A7BEE89507B600FCFDCFB7BEE89541
+:103F300026C08437B1F42ED02DD0F82E2BD038D017
+:103F4000F601EF2C8F010F5F1F4F84911BD0EA9475
+:103F5000F801C1F70894C11CD11CFA94CF0CD11CF4
+:103F60000EC0853739F424D08EE10CD084E90AD014
+:103F700086E098CF813511F488E014D019D080E123
+:103F800001D06ACF982F8091C00085FFFCCF90931D
+:103F9000C6000895A8958091C00087FFFCCF80914E
+:103FA000C6000895E0E6F0E098E1908380830895EC
+:103FB000F1DF803219F088E0F5DFFFCF84E1E2CF56
+:103FC0001F93182FE7DF1150E9F7F2DF1F910895D3
+:0A3FD00080E0E8DFEE27FF270994E8
 :023FFE000104BC
 :0400000300003E00BB
 :00000001FF
diff --git a/arduino/bootloaders/optiboot/optiboot_pro_20mhz.lst b/arduino/bootloaders/optiboot/optiboot_pro_20mhz.lst
index ba7b6b6..cd2d66a 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_20mhz.lst
+++ b/arduino/bootloaders/optiboot/optiboot_pro_20mhz.lst
@@ -3,27 +3,27 @@ optiboot_pro_20mhz.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001fc  00003e00  00003e00  00000054  2**1
+  0 .text         000001da  00003e00  00003e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00003ffe  00003ffe  00000250  2**0
+  1 .version      00000002  00003ffe  00003ffe  0000022e  2**0
                   CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000252  2**0
+  2 .debug_aranges 00000028  00000000  00000000  00000230  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000006a  00000000  00000000  0000027a  2**0
+  3 .debug_pubnames 0000005f  00000000  00000000  00000258  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000285  00000000  00000000  000002e4  2**0
+  4 .debug_info   0000028c  00000000  00000000  000002b7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000019f  00000000  00000000  00000569  2**0
+  5 .debug_abbrev 00000199  00000000  00000000  00000543  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000453  00000000  00000000  00000708  2**0
+  6 .debug_line   00000456  00000000  00000000  000006dc  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000090  00000000  00000000  00000b5c  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000b34  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000141  00000000  00000000  00000bec  2**0
+  8 .debug_str    00000149  00000000  00000000  00000bb4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    000001e1  00000000  00000000  00000d2d  2**0
+  9 .debug_loc    0000027e  00000000  00000000  00000cfd  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000068  00000000  00000000  00000f0e  2**0
+ 10 .debug_ranges 00000060  00000000  00000000  00000f7b  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
@@ -47,7 +47,7 @@ int main(void) {
     3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
     3e06:	81 ff       	sbrs	r24, 1
-    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+    3e08:	e3 d0       	rcall	.+454    	; 0x3fd0 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
@@ -77,7 +77,7 @@ int main(void) {
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
     3e28:	8e e0       	ldi	r24, 0x0E	; 14
-    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
+    3e2a:	bc d0       	rcall	.+376    	; 0x3fa4 <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
@@ -111,8 +111,8 @@ void flash_led(uint8_t count) {
 #else
     LED_PIN |= _BV(LED);
     3e44:	1d 9a       	sbi	0x03, 5	; 3
-  return getch();
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
@@ -132,8 +132,8 @@ void watchdogReset() {
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e4c:	dd 24       	eor	r13, r13
-    3e4e:	d3 94       	inc	r13
+    3e4c:	99 24       	eor	r9, r9
+    3e4e:	93 94       	inc	r9
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
@@ -141,21 +141,21 @@ void watchdogReset() {
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
     3e50:	a5 e0       	ldi	r26, 0x05	; 5
-    3e52:	ea 2e       	mov	r14, r26
+    3e52:	aa 2e       	mov	r10, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
     3e54:	f1 e1       	ldi	r31, 0x11	; 17
-    3e56:	ff 2e       	mov	r15, r31
+    3e56:	bf 2e       	mov	r11, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
+    3e58:	9d d0       	rcall	.+314    	; 0x3f94 <getch>
 
     if(ch == STK_GET_PARAMETER) {
     3e5a:	81 34       	cpi	r24, 0x41	; 65
@@ -163,10 +163,10 @@ void watchdogReset() {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
     3e5e:	81 e0       	ldi	r24, 0x01	; 1
-    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
+    3e60:	af d0       	rcall	.+350    	; 0x3fc0 <getNch>
       putch(0x03);
     3e62:	83 e0       	ldi	r24, 0x03	; 3
-    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
+    3e64:	1f c0       	rjmp	.+62     	; 0x3ea4 <main+0xa4>
     }
     else if(ch == STK_SET_DEVICE) {
     3e66:	82 34       	cpi	r24, 0x42	; 66
@@ -182,71 +182,77 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     3e72:	85 e0       	ldi	r24, 0x05	; 5
-    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
-    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
+    3e74:	a5 d0       	rcall	.+330    	; 0x3fc0 <getNch>
+    3e76:	83 c0       	rjmp	.+262    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     3e78:	85 35       	cpi	r24, 0x55	; 85
-    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
+    3e7a:	79 f4       	brne	.+30     	; 0x3e9a <main+0x9a>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+    3e7c:	8b d0       	rcall	.+278    	; 0x3f94 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-    3e7e:	08 2f       	mov	r16, r24
-    3e80:	10 e0       	ldi	r17, 0x00	; 0
-    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
-    3e84:	90 e0       	ldi	r25, 0x00	; 0
-    3e86:	98 2f       	mov	r25, r24
-    3e88:	88 27       	eor	r24, r24
-    3e8a:	80 2b       	or	r24, r16
-    3e8c:	91 2b       	or	r25, r17
+    3e7e:	e8 2e       	mov	r14, r24
+    3e80:	ff 24       	eor	r15, r15
+    3e82:	88 d0       	rcall	.+272    	; 0x3f94 <getch>
+    3e84:	08 2f       	mov	r16, r24
+    3e86:	10 e0       	ldi	r17, 0x00	; 0
+    3e88:	10 2f       	mov	r17, r16
+    3e8a:	00 27       	eor	r16, r16
+    3e8c:	0e 29       	or	r16, r14
+    3e8e:	1f 29       	or	r17, r15
 #ifdef RAMPZ
       // Transfer top bit to RAMPZ
       RAMPZ = (newAddress & 0x8000) ? 1 : 0;
 #endif
       newAddress += newAddress; // Convert from word address to byte address
-    3e8e:	88 0f       	add	r24, r24
-    3e90:	99 1f       	adc	r25, r25
+    3e90:	00 0f       	add	r16, r16
+    3e92:	11 1f       	adc	r17, r17
       address = newAddress;
-    3e92:	90 93 01 02 	sts	0x0201, r25
-    3e96:	80 93 00 02 	sts	0x0200, r24
-    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
+    3e94:	8d d0       	rcall	.+282    	; 0x3fb0 <verifySpace>
+    3e96:	68 01       	movw	r12, r16
+    3e98:	72 c0       	rjmp	.+228    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_UNIVERSAL) {
-    3e9c:	86 35       	cpi	r24, 0x56	; 86
-    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
+    3e9a:	86 35       	cpi	r24, 0x56	; 86
+    3e9c:	29 f4       	brne	.+10     	; 0x3ea8 <main+0xa8>
       // UNIVERSAL command is ignored
       getNch(4);
-    3ea0:	84 e0       	ldi	r24, 0x04	; 4
-    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
+    3e9e:	84 e0       	ldi	r24, 0x04	; 4
+    3ea0:	8f d0       	rcall	.+286    	; 0x3fc0 <getNch>
       putch(0x00);
-    3ea4:	80 e0       	ldi	r24, 0x00	; 0
-    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
-    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
+    3ea2:	80 e0       	ldi	r24, 0x00	; 0
+    3ea4:	6f d0       	rcall	.+222    	; 0x3f84 <putch>
+    3ea6:	6b c0       	rjmp	.+214    	; 0x3f7e <main+0x17e>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3eaa:	84 36       	cpi	r24, 0x64	; 100
-    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
-    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
+    3ea8:	84 36       	cpi	r24, 0x64	; 100
+    3eaa:	09 f0       	breq	.+2      	; 0x3eae <main+0xae>
+    3eac:	42 c0       	rjmp	.+132    	; 0x3f32 <main+0x132>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
-      getLen();
-    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3eae:	72 d0       	rcall	.+228    	; 0x3f94 <getch>
+      length = getch();
+    3eb0:	71 d0       	rcall	.+226    	; 0x3f94 <getch>
+    3eb2:	08 2f       	mov	r16, r24
+      getch();
+    3eb4:	6f d0       	rcall	.+222    	; 0x3f94 <getch>
 
       // If we are in RWW section, immediately start page erase
       if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3eb2:	e0 91 00 02 	lds	r30, 0x0200
-    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eb6:	80 e0       	ldi	r24, 0x00	; 0
+    3eb8:	c8 16       	cp	r12, r24
     3eba:	88 e3       	ldi	r24, 0x38	; 56
-    3ebc:	e0 30       	cpi	r30, 0x00	; 0
-    3ebe:	f8 07       	cpc	r31, r24
-    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
-    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ebc:	d8 06       	cpc	r13, r24
+    3ebe:	20 f4       	brcc	.+8      	; 0x3ec8 <main+0xc8>
+    3ec0:	83 e0       	ldi	r24, 0x03	; 3
+    3ec2:	f6 01       	movw	r30, r12
     3ec4:	87 bf       	out	0x37, r24	; 55
     3ec6:	e8 95       	spm
     3ec8:	c0 e0       	ldi	r28, 0x00	; 0
@@ -255,302 +261,301 @@ void watchdogReset() {
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
+    3ecc:	63 d0       	rcall	.+198    	; 0x3f94 <getch>
     3ece:	89 93       	st	Y+, r24
       while (--length);
-    3ed0:	80 91 02 02 	lds	r24, 0x0202
-    3ed4:	81 50       	subi	r24, 0x01	; 1
-    3ed6:	80 93 02 02 	sts	0x0202, r24
-    3eda:	88 23       	and	r24, r24
-    3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
+    3ed0:	0c 17       	cp	r16, r28
+    3ed2:	e1 f7       	brne	.-8      	; 0x3ecc <main+0xcc>
 
       // If we are in NRWW section, page erase has to be delayed until now.
       // Todo: Take RAMPZ into account
       if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ede:	e0 91 00 02 	lds	r30, 0x0200
-    3ee2:	f0 91 01 02 	lds	r31, 0x0201
-    3ee6:	88 e3       	ldi	r24, 0x38	; 56
-    3ee8:	e0 30       	cpi	r30, 0x00	; 0
-    3eea:	f8 07       	cpc	r31, r24
-    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
-    3eee:	83 e0       	ldi	r24, 0x03	; 3
-    3ef0:	87 bf       	out	0x37, r24	; 55
-    3ef2:	e8 95       	spm
+    3ed4:	f0 e0       	ldi	r31, 0x00	; 0
+    3ed6:	cf 16       	cp	r12, r31
+    3ed8:	f8 e3       	ldi	r31, 0x38	; 56
+    3eda:	df 06       	cpc	r13, r31
+    3edc:	20 f0       	brcs	.+8      	; 0x3ee6 <main+0xe6>
+    3ede:	83 e0       	ldi	r24, 0x03	; 3
+    3ee0:	f6 01       	movw	r30, r12
+    3ee2:	87 bf       	out	0x37, r24	; 55
+    3ee4:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
+    3ee6:	64 d0       	rcall	.+200    	; 0x3fb0 <verifySpace>
 
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3ef6:	07 b6       	in	r0, 0x37	; 55
-    3ef8:	00 fc       	sbrc	r0, 0
-    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
-      }
-#endif
-
-      // Copy buffer into programming buffer
+    3ee8:	07 b6       	in	r0, 0x37	; 55
+    3eea:	00 fc       	sbrc	r0, 0
+    3eec:	fd cf       	rjmp	.-6      	; 0x3ee8 <main+0xe8>
+    3eee:	a6 01       	movw	r20, r12
+    3ef0:	a0 e0       	ldi	r26, 0x00	; 0
+    3ef2:	b1 e0       	ldi	r27, 0x01	; 1
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3efc:	40 91 00 02 	lds	r20, 0x0200
-    3f00:	50 91 01 02 	lds	r21, 0x0201
-    3f04:	a0 e0       	ldi	r26, 0x00	; 0
-    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3f08:	2c 91       	ld	r18, X
-    3f0a:	30 e0       	ldi	r19, 0x00	; 0
+    3ef4:	2c 91       	ld	r18, X
+    3ef6:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3f0c:	11 96       	adiw	r26, 0x01	; 1
-    3f0e:	8c 91       	ld	r24, X
-    3f10:	11 97       	sbiw	r26, 0x01	; 1
-    3f12:	90 e0       	ldi	r25, 0x00	; 0
-    3f14:	98 2f       	mov	r25, r24
-    3f16:	88 27       	eor	r24, r24
-    3f18:	82 2b       	or	r24, r18
-    3f1a:	93 2b       	or	r25, r19
+    3ef8:	11 96       	adiw	r26, 0x01	; 1
+    3efa:	8c 91       	ld	r24, X
+    3efc:	11 97       	sbiw	r26, 0x01	; 1
+    3efe:	90 e0       	ldi	r25, 0x00	; 0
+    3f00:	98 2f       	mov	r25, r24
+    3f02:	88 27       	eor	r24, r24
+    3f04:	82 2b       	or	r24, r18
+    3f06:	93 2b       	or	r25, r19
 #define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
 #define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
 /* main program starts here */
 int main(void) {
-    3f1c:	12 96       	adiw	r26, 0x02	; 2
+    3f08:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    3f1e:	fa 01       	movw	r30, r20
-    3f20:	0c 01       	movw	r0, r24
-    3f22:	d7 be       	out	0x37, r13	; 55
-    3f24:	e8 95       	spm
-    3f26:	11 24       	eor	r1, r1
+    3f0a:	fa 01       	movw	r30, r20
+    3f0c:	0c 01       	movw	r0, r24
+    3f0e:	97 be       	out	0x37, r9	; 55
+    3f10:	e8 95       	spm
+    3f12:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3f28:	4e 5f       	subi	r20, 0xFE	; 254
-    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f14:	4e 5f       	subi	r20, 0xFE	; 254
+    3f16:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
-    3f2e:	a0 38       	cpi	r26, 0x80	; 128
-    3f30:	bf 07       	cpc	r27, r31
-    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
+    3f18:	f1 e0       	ldi	r31, 0x01	; 1
+    3f1a:	a0 38       	cpi	r26, 0x80	; 128
+    3f1c:	bf 07       	cpc	r27, r31
+    3f1e:	51 f7       	brne	.-44     	; 0x3ef4 <main+0xf4>
 
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
-    3f34:	e0 91 00 02 	lds	r30, 0x0200
-    3f38:	f0 91 01 02 	lds	r31, 0x0201
-    3f3c:	e7 be       	out	0x37, r14	; 55
-    3f3e:	e8 95       	spm
+    3f20:	f6 01       	movw	r30, r12
+    3f22:	a7 be       	out	0x37, r10	; 55
+    3f24:	e8 95       	spm
       boot_spm_busy_wait();
-    3f40:	07 b6       	in	r0, 0x37	; 55
-    3f42:	00 fc       	sbrc	r0, 0
-    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
+    3f26:	07 b6       	in	r0, 0x37	; 55
+    3f28:	00 fc       	sbrc	r0, 0
+    3f2a:	fd cf       	rjmp	.-6      	; 0x3f26 <main+0x126>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f46:	f7 be       	out	0x37, r15	; 55
-    3f48:	e8 95       	spm
-    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
+    3f2c:	b7 be       	out	0x37, r11	; 55
+    3f2e:	e8 95       	spm
+    3f30:	26 c0       	rjmp	.+76     	; 0x3f7e <main+0x17e>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f4c:	84 37       	cpi	r24, 0x74	; 116
-    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
+    3f32:	84 37       	cpi	r24, 0x74	; 116
+    3f34:	b1 f4       	brne	.+44     	; 0x3f62 <main+0x162>
       // READ PAGE - we only read flash
-      getLen();
-    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3f36:	2e d0       	rcall	.+92     	; 0x3f94 <getch>
+      length = getch();
+    3f38:	2d d0       	rcall	.+90     	; 0x3f94 <getch>
+    3f3a:	f8 2e       	mov	r15, r24
+      getch();
+    3f3c:	2b d0       	rcall	.+86     	; 0x3f94 <getch>
+
       verifySpace();
-    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+    3f3e:	38 d0       	rcall	.+112    	; 0x3fb0 <verifySpace>
+    3f40:	f6 01       	movw	r30, r12
+    3f42:	ef 2c       	mov	r14, r15
         putch(result);
         address++;
       }
       while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f54:	e0 91 00 02 	lds	r30, 0x0200
-    3f58:	f0 91 01 02 	lds	r31, 0x0201
-    3f5c:	31 96       	adiw	r30, 0x01	; 1
-    3f5e:	f0 93 01 02 	sts	0x0201, r31
-    3f62:	e0 93 00 02 	sts	0x0200, r30
-    3f66:	31 97       	sbiw	r30, 0x01	; 1
-    3f68:	e4 91       	lpm	r30, Z+
-    3f6a:	8e 2f       	mov	r24, r30
-    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
+    3f44:	8f 01       	movw	r16, r30
+    3f46:	0f 5f       	subi	r16, 0xFF	; 255
+    3f48:	1f 4f       	sbci	r17, 0xFF	; 255
+    3f4a:	84 91       	lpm	r24, Z+
+    3f4c:	1b d0       	rcall	.+54     	; 0x3f84 <putch>
       while (--length);
-    3f6e:	80 91 02 02 	lds	r24, 0x0202
-    3f72:	81 50       	subi	r24, 0x01	; 1
-    3f74:	80 93 02 02 	sts	0x0202, r24
-    3f78:	88 23       	and	r24, r24
-    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
-    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+    3f4e:	ea 94       	dec	r14
+    3f50:	f8 01       	movw	r30, r16
+    3f52:	c1 f7       	brne	.-16     	; 0x3f44 <main+0x144>
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
+#endif
+
+/* main program starts here */
+int main(void) {
+    3f54:	08 94       	sec
+    3f56:	c1 1c       	adc	r12, r1
+    3f58:	d1 1c       	adc	r13, r1
+    3f5a:	fa 94       	dec	r15
+    3f5c:	cf 0c       	add	r12, r15
+    3f5e:	d1 1c       	adc	r13, r1
+    3f60:	0e c0       	rjmp	.+28     	; 0x3f7e <main+0x17e>
 #endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f7e:	85 37       	cpi	r24, 0x75	; 117
-    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
+    3f62:	85 37       	cpi	r24, 0x75	; 117
+    3f64:	39 f4       	brne	.+14     	; 0x3f74 <main+0x174>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
+    3f66:	24 d0       	rcall	.+72     	; 0x3fb0 <verifySpace>
       putch(SIGNATURE_0);
-    3f84:	8e e1       	ldi	r24, 0x1E	; 30
-    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
+    3f68:	8e e1       	ldi	r24, 0x1E	; 30
+    3f6a:	0c d0       	rcall	.+24     	; 0x3f84 <putch>
       putch(SIGNATURE_1);
-    3f88:	84 e9       	ldi	r24, 0x94	; 148
-    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
+    3f6c:	84 e9       	ldi	r24, 0x94	; 148
+    3f6e:	0a d0       	rcall	.+20     	; 0x3f84 <putch>
       putch(SIGNATURE_2);
-    3f8c:	86 e0       	ldi	r24, 0x06	; 6
-    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
+    3f70:	86 e0       	ldi	r24, 0x06	; 6
+    3f72:	98 cf       	rjmp	.-208    	; 0x3ea4 <main+0xa4>
     }
     else if (ch == 'Q') {
-    3f90:	81 35       	cpi	r24, 0x51	; 81
-    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
+    3f74:	81 35       	cpi	r24, 0x51	; 81
+    3f76:	11 f4       	brne	.+4      	; 0x3f7c <main+0x17c>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f94:	88 e0       	ldi	r24, 0x08	; 8
-    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
+    3f78:	88 e0       	ldi	r24, 0x08	; 8
+    3f7a:	14 d0       	rcall	.+40     	; 0x3fa4 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
+    3f7c:	19 d0       	rcall	.+50     	; 0x3fb0 <verifySpace>
     }
     putch(STK_OK);
-    3f9a:	80 e1       	ldi	r24, 0x10	; 16
-    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
-    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
+    3f7e:	80 e1       	ldi	r24, 0x10	; 16
+    3f80:	01 d0       	rcall	.+2      	; 0x3f84 <putch>
+    3f82:	6a cf       	rjmp	.-300    	; 0x3e58 <main+0x58>
 
-00003fa0 <putch>:
+00003f84 <putch>:
   }
 }
 
 void putch(char ch) {
-    3fa0:	98 2f       	mov	r25, r24
+    3f84:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fa6:	85 ff       	sbrs	r24, 5
-    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
+    3f86:	80 91 c0 00 	lds	r24, 0x00C0
+    3f8a:	85 ff       	sbrs	r24, 5
+    3f8c:	fc cf       	rjmp	.-8      	; 0x3f86 <putch+0x2>
   UDR0 = ch;
-    3faa:	90 93 c6 00 	sts	0x00C6, r25
+    3f8e:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3fae:	08 95       	ret
+    3f92:	08 95       	ret
 
-00003fb0 <getch>:
-  return getch();
+00003f94 <getch>:
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fb0:	a8 95       	wdr
+    3f94:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fb6:	87 ff       	sbrs	r24, 7
-    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
+    3f96:	80 91 c0 00 	lds	r24, 0x00C0
+    3f9a:	87 ff       	sbrs	r24, 7
+    3f9c:	fc cf       	rjmp	.-8      	; 0x3f96 <getch+0x2>
   ch = UDR0;
-    3fba:	80 91 c6 00 	lds	r24, 0x00C6
+    3f9e:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    3fbe:	08 95       	ret
-
-00003fc0 <getLen>:
-  } while (--count);
-}
-#endif
-
-uint8_t getLen() {
-  getch();
-    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
-  length = getch();
-    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
-    3fc4:	80 93 02 02 	sts	0x0202, r24
-  return getch();
-}
-    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
+    3fa2:	08 95       	ret
 
-00003fca <watchdogConfig>:
+00003fa4 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3fca:	e0 e6       	ldi	r30, 0x60	; 96
-    3fcc:	f0 e0       	ldi	r31, 0x00	; 0
-    3fce:	98 e1       	ldi	r25, 0x18	; 24
-    3fd0:	90 83       	st	Z, r25
+    3fa4:	e0 e6       	ldi	r30, 0x60	; 96
+    3fa6:	f0 e0       	ldi	r31, 0x00	; 0
+    3fa8:	98 e1       	ldi	r25, 0x18	; 24
+    3faa:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fd2:	80 83       	st	Z, r24
+    3fac:	80 83       	st	Z, r24
 }
-    3fd4:	08 95       	ret
-
-00003fd6 <appStart>:
-
-void appStart() {
-  watchdogConfig(WATCHDOG_OFF);
-    3fd6:	80 e0       	ldi	r24, 0x00	; 0
-    3fd8:	f8 df       	rcall	.-16     	; 0x3fca <watchdogConfig>
-  __asm__ __volatile__ (
-    3fda:	ee 27       	eor	r30, r30
-    3fdc:	ff 27       	eor	r31, r31
-    3fde:	09 94       	ijmp
+    3fae:	08 95       	ret
 
-00003fe0 <verifySpace>:
+00003fb0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
-  if (getch() != CRC_EOP) appStart();
-    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
-    3fe2:	80 32       	cpi	r24, 0x20	; 32
-    3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
-    3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
+  if (getch() != CRC_EOP) {
+    3fb0:	f1 df       	rcall	.-30     	; 0x3f94 <getch>
+    3fb2:	80 32       	cpi	r24, 0x20	; 32
+    3fb4:	19 f0       	breq	.+6      	; 0x3fbc <verifySpace+0xc>
+    watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
+    3fb6:	88 e0       	ldi	r24, 0x08	; 8
+    3fb8:	f5 df       	rcall	.-22     	; 0x3fa4 <watchdogConfig>
+    3fba:	ff cf       	rjmp	.-2      	; 0x3fba <verifySpace+0xa>
+    while (1)			      // and busy-loop so that WD causes
+      ;				      //  a reset and app start.
+  }
   putch(STK_INSYNC);
-    3fe8:	84 e1       	ldi	r24, 0x14	; 20
+    3fbc:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+    3fbe:	e2 cf       	rjmp	.-60     	; 0x3f84 <putch>
 
-00003fec <getNch>:
+00003fc0 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fec:	1f 93       	push	r17
-    3fee:	18 2f       	mov	r17, r24
+    3fc0:	1f 93       	push	r17
+    3fc2:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
-    3ff2:	11 50       	subi	r17, 0x01	; 1
-    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
+    3fc4:	e7 df       	rcall	.-50     	; 0x3f94 <getch>
+    3fc6:	11 50       	subi	r17, 0x01	; 1
+    3fc8:	e9 f7       	brne	.-6      	; 0x3fc4 <getNch+0x4>
   verifySpace();
-    3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
+    3fca:	f2 df       	rcall	.-28     	; 0x3fb0 <verifySpace>
+}
+    3fcc:	1f 91       	pop	r17
+    3fce:	08 95       	ret
+
+00003fd0 <appStart>:
+  WDTCSR = _BV(WDCE) | _BV(WDE);
+  WDTCSR = x;
 }
-    3ff8:	1f 91       	pop	r17
-    3ffa:	08 95       	ret
+
+void appStart() {
+  watchdogConfig(WATCHDOG_OFF);
+    3fd0:	80 e0       	ldi	r24, 0x00	; 0
+    3fd2:	e8 df       	rcall	.-48     	; 0x3fa4 <watchdogConfig>
+  __asm__ __volatile__ (
+    3fd4:	ee 27       	eor	r30, r30
+    3fd6:	ff 27       	eor	r31, r31
+    3fd8:	09 94       	ijmp
diff --git a/arduino/bootloaders/optiboot/optiboot_pro_8MHz.hex b/arduino/bootloaders/optiboot/optiboot_pro_8MHz.hex
index 77897e7..dcdbbb4 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_8MHz.hex
+++ b/arduino/bootloaders/optiboot/optiboot_pro_8MHz.hex
@@ -1,35 +1,33 @@
-:103E0000112484B714BE81FFE6D085E08093810041
+:103E0000112484B714BE81FFE3D085E08093810044
 :103E100082E08093C00088E18093C10086E08093B7
-:103E2000C20088E08093C4008EE0CFD0259A86E05F
+:103E2000C20088E08093C4008EE0BCD0259A86E072
 :103E300028E13EEF91E0309385002093840096BB0B
-:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D
-:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020
-:103E6000C5D083E020C0823411F484E103C08534DE
-:103E700019F485E0BBD091C0853581F499D0082F25
-:103E800010E096D090E0982F8827802B912B880FF8
-:103E9000991F90930102809300027EC0863529F419
-:103EA00084E0A4D080E07CD078C0843609F04EC095
-:103EB00087D0E0910002F091010288E3E030F8073A
-:103EC00018F483E087BFE895C0E0D1E071D0899312
-:103ED000809102028150809302028823B9F7E09119
-:103EE0000002F091010288E3E030F80718F083E067
-:103EF00087BFE89575D007B600FCFDCF4091000262
-:103F000050910102A0E0B1E02C9130E011968C912B
-:103F1000119790E0982F8827822B932B1296FA0105
-:103F20000C01D7BEE89511244E5F5F4FF1E0A03839
-:103F3000BF0751F7E0910002F0910102E7BEE8955A
-:103F400007B600FCFDCFF7BEE89527C08437B9F46B
-:103F500037D046D0E0910002F09101023196F09303
-:103F60000102E09300023197E4918E2F19D08091E5
-:103F70000202815080930202882361F70EC08537C8
-:103F800039F42ED08EE10CD084E90AD086E08BCFB4
-:103F9000813511F488E019D023D080E101D05CCFC5
-:103FA000982F8091C00085FFFCCF9093C6000895A4
-:103FB000A8958091C00087FFFCCF8091C60008952E
-:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E
-:103FD00090838083089580E0F8DFEE27FF2709941F
-:103FE000E7DF803209F0F7DF84E1DACF1F93182F83
-:0C3FF000DFDF1150E9F7F4DF1F910895A6
+:103E4000B09BFECF1D9AA8958150A9F79924939411
+:103E5000A5E0AA2EF1E1BF2E9DD0813421F481E0AE
+:103E6000AFD083E01FC0823411F484E103C08534F5
+:103E700019F485E0A5D083C0853579F48BD0E82E80
+:103E8000FF2488D0082F10E0102F00270E291F29AB
+:103E9000000F111F8DD0680172C0863529F484E0AF
+:103EA0008FD080E06FD06BC0843609F042C072D0F2
+:103EB00071D0082F6FD080E0C81688E3D80620F4B0
+:103EC00083E0F60187BFE895C0E0D1E063D0899335
+:103ED0000C17E1F7F0E0CF16F8E3DF0620F083E0FF
+:103EE000F60187BFE89564D007B600FCFDCFA601B8
+:103EF000A0E0B1E02C9130E011968C91119790E008
+:103F0000982F8827822B932B1296FA010C0197BECB
+:103F1000E89511244E5F5F4FF1E0A038BF0751F7DD
+:103F2000F601A7BEE89507B600FCFDCFB7BEE89541
+:103F300026C08437B1F42ED02DD0F82E2BD038D017
+:103F4000F601EF2C8F010F5F1F4F84911BD0EA9475
+:103F5000F801C1F70894C11CD11CFA94CF0CD11CF4
+:103F60000EC0853739F424D08EE10CD084E90AD014
+:103F700086E098CF813511F488E014D019D080E123
+:103F800001D06ACF982F8091C00085FFFCCF90931D
+:103F9000C6000895A8958091C00087FFFCCF80914E
+:103FA000C6000895E0E6F0E098E1908380830895EC
+:103FB000F1DF803219F088E0F5DFFFCF84E1E2CF56
+:103FC0001F93182FE7DF1150E9F7F2DF1F910895D3
+:0A3FD00080E0E8DFEE27FF270994E8
 :023FFE000104BC
 :0400000300003E00BB
 :00000001FF
diff --git a/arduino/bootloaders/optiboot/optiboot_pro_8MHz.lst b/arduino/bootloaders/optiboot/optiboot_pro_8MHz.lst
index 23d5e1a..d3d760e 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_8MHz.lst
+++ b/arduino/bootloaders/optiboot/optiboot_pro_8MHz.lst
@@ -3,27 +3,27 @@ optiboot_pro_8MHz.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001fc  00003e00  00003e00  00000054  2**1
+  0 .text         000001da  00003e00  00003e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00003ffe  00003ffe  00000250  2**0
+  1 .version      00000002  00003ffe  00003ffe  0000022e  2**0
                   CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000252  2**0
+  2 .debug_aranges 00000028  00000000  00000000  00000230  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000006a  00000000  00000000  0000027a  2**0
+  3 .debug_pubnames 0000005f  00000000  00000000  00000258  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000285  00000000  00000000  000002e4  2**0
+  4 .debug_info   0000028c  00000000  00000000  000002b7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000019f  00000000  00000000  00000569  2**0
+  5 .debug_abbrev 00000199  00000000  00000000  00000543  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000453  00000000  00000000  00000708  2**0
+  6 .debug_line   00000456  00000000  00000000  000006dc  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000090  00000000  00000000  00000b5c  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000b34  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000141  00000000  00000000  00000bec  2**0
+  8 .debug_str    00000149  00000000  00000000  00000bb4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    000001e1  00000000  00000000  00000d2d  2**0
+  9 .debug_loc    0000027e  00000000  00000000  00000cfd  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000068  00000000  00000000  00000f0e  2**0
+ 10 .debug_ranges 00000060  00000000  00000000  00000f7b  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
@@ -47,7 +47,7 @@ int main(void) {
     3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
     3e06:	81 ff       	sbrs	r24, 1
-    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+    3e08:	e3 d0       	rcall	.+454    	; 0x3fd0 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
@@ -77,7 +77,7 @@ int main(void) {
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
     3e28:	8e e0       	ldi	r24, 0x0E	; 14
-    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
+    3e2a:	bc d0       	rcall	.+376    	; 0x3fa4 <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
@@ -111,8 +111,8 @@ void flash_led(uint8_t count) {
 #else
     LED_PIN |= _BV(LED);
     3e44:	1d 9a       	sbi	0x03, 5	; 3
-  return getch();
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
@@ -132,8 +132,8 @@ void watchdogReset() {
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e4c:	dd 24       	eor	r13, r13
-    3e4e:	d3 94       	inc	r13
+    3e4c:	99 24       	eor	r9, r9
+    3e4e:	93 94       	inc	r9
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
@@ -141,21 +141,21 @@ void watchdogReset() {
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
     3e50:	a5 e0       	ldi	r26, 0x05	; 5
-    3e52:	ea 2e       	mov	r14, r26
+    3e52:	aa 2e       	mov	r10, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
     3e54:	f1 e1       	ldi	r31, 0x11	; 17
-    3e56:	ff 2e       	mov	r15, r31
+    3e56:	bf 2e       	mov	r11, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
+    3e58:	9d d0       	rcall	.+314    	; 0x3f94 <getch>
 
     if(ch == STK_GET_PARAMETER) {
     3e5a:	81 34       	cpi	r24, 0x41	; 65
@@ -163,10 +163,10 @@ void watchdogReset() {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
     3e5e:	81 e0       	ldi	r24, 0x01	; 1
-    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
+    3e60:	af d0       	rcall	.+350    	; 0x3fc0 <getNch>
       putch(0x03);
     3e62:	83 e0       	ldi	r24, 0x03	; 3
-    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
+    3e64:	1f c0       	rjmp	.+62     	; 0x3ea4 <main+0xa4>
     }
     else if(ch == STK_SET_DEVICE) {
     3e66:	82 34       	cpi	r24, 0x42	; 66
@@ -182,71 +182,77 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     3e72:	85 e0       	ldi	r24, 0x05	; 5
-    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
-    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
+    3e74:	a5 d0       	rcall	.+330    	; 0x3fc0 <getNch>
+    3e76:	83 c0       	rjmp	.+262    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     3e78:	85 35       	cpi	r24, 0x55	; 85
-    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
+    3e7a:	79 f4       	brne	.+30     	; 0x3e9a <main+0x9a>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+    3e7c:	8b d0       	rcall	.+278    	; 0x3f94 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-    3e7e:	08 2f       	mov	r16, r24
-    3e80:	10 e0       	ldi	r17, 0x00	; 0
-    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
-    3e84:	90 e0       	ldi	r25, 0x00	; 0
-    3e86:	98 2f       	mov	r25, r24
-    3e88:	88 27       	eor	r24, r24
-    3e8a:	80 2b       	or	r24, r16
-    3e8c:	91 2b       	or	r25, r17
+    3e7e:	e8 2e       	mov	r14, r24
+    3e80:	ff 24       	eor	r15, r15
+    3e82:	88 d0       	rcall	.+272    	; 0x3f94 <getch>
+    3e84:	08 2f       	mov	r16, r24
+    3e86:	10 e0       	ldi	r17, 0x00	; 0
+    3e88:	10 2f       	mov	r17, r16
+    3e8a:	00 27       	eor	r16, r16
+    3e8c:	0e 29       	or	r16, r14
+    3e8e:	1f 29       	or	r17, r15
 #ifdef RAMPZ
       // Transfer top bit to RAMPZ
       RAMPZ = (newAddress & 0x8000) ? 1 : 0;
 #endif
       newAddress += newAddress; // Convert from word address to byte address
-    3e8e:	88 0f       	add	r24, r24
-    3e90:	99 1f       	adc	r25, r25
+    3e90:	00 0f       	add	r16, r16
+    3e92:	11 1f       	adc	r17, r17
       address = newAddress;
-    3e92:	90 93 01 02 	sts	0x0201, r25
-    3e96:	80 93 00 02 	sts	0x0200, r24
-    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
+    3e94:	8d d0       	rcall	.+282    	; 0x3fb0 <verifySpace>
+    3e96:	68 01       	movw	r12, r16
+    3e98:	72 c0       	rjmp	.+228    	; 0x3f7e <main+0x17e>
     }
     else if(ch == STK_UNIVERSAL) {
-    3e9c:	86 35       	cpi	r24, 0x56	; 86
-    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
+    3e9a:	86 35       	cpi	r24, 0x56	; 86
+    3e9c:	29 f4       	brne	.+10     	; 0x3ea8 <main+0xa8>
       // UNIVERSAL command is ignored
       getNch(4);
-    3ea0:	84 e0       	ldi	r24, 0x04	; 4
-    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
+    3e9e:	84 e0       	ldi	r24, 0x04	; 4
+    3ea0:	8f d0       	rcall	.+286    	; 0x3fc0 <getNch>
       putch(0x00);
-    3ea4:	80 e0       	ldi	r24, 0x00	; 0
-    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
-    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
+    3ea2:	80 e0       	ldi	r24, 0x00	; 0
+    3ea4:	6f d0       	rcall	.+222    	; 0x3f84 <putch>
+    3ea6:	6b c0       	rjmp	.+214    	; 0x3f7e <main+0x17e>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3eaa:	84 36       	cpi	r24, 0x64	; 100
-    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
-    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
+    3ea8:	84 36       	cpi	r24, 0x64	; 100
+    3eaa:	09 f0       	breq	.+2      	; 0x3eae <main+0xae>
+    3eac:	42 c0       	rjmp	.+132    	; 0x3f32 <main+0x132>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
-      getLen();
-    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3eae:	72 d0       	rcall	.+228    	; 0x3f94 <getch>
+      length = getch();
+    3eb0:	71 d0       	rcall	.+226    	; 0x3f94 <getch>
+    3eb2:	08 2f       	mov	r16, r24
+      getch();
+    3eb4:	6f d0       	rcall	.+222    	; 0x3f94 <getch>
 
       // If we are in RWW section, immediately start page erase
       if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3eb2:	e0 91 00 02 	lds	r30, 0x0200
-    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eb6:	80 e0       	ldi	r24, 0x00	; 0
+    3eb8:	c8 16       	cp	r12, r24
     3eba:	88 e3       	ldi	r24, 0x38	; 56
-    3ebc:	e0 30       	cpi	r30, 0x00	; 0
-    3ebe:	f8 07       	cpc	r31, r24
-    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
-    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ebc:	d8 06       	cpc	r13, r24
+    3ebe:	20 f4       	brcc	.+8      	; 0x3ec8 <main+0xc8>
+    3ec0:	83 e0       	ldi	r24, 0x03	; 3
+    3ec2:	f6 01       	movw	r30, r12
     3ec4:	87 bf       	out	0x37, r24	; 55
     3ec6:	e8 95       	spm
     3ec8:	c0 e0       	ldi	r28, 0x00	; 0
@@ -255,302 +261,301 @@ void watchdogReset() {
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
+    3ecc:	63 d0       	rcall	.+198    	; 0x3f94 <getch>
     3ece:	89 93       	st	Y+, r24
       while (--length);
-    3ed0:	80 91 02 02 	lds	r24, 0x0202
-    3ed4:	81 50       	subi	r24, 0x01	; 1
-    3ed6:	80 93 02 02 	sts	0x0202, r24
-    3eda:	88 23       	and	r24, r24
-    3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
+    3ed0:	0c 17       	cp	r16, r28
+    3ed2:	e1 f7       	brne	.-8      	; 0x3ecc <main+0xcc>
 
       // If we are in NRWW section, page erase has to be delayed until now.
       // Todo: Take RAMPZ into account
       if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ede:	e0 91 00 02 	lds	r30, 0x0200
-    3ee2:	f0 91 01 02 	lds	r31, 0x0201
-    3ee6:	88 e3       	ldi	r24, 0x38	; 56
-    3ee8:	e0 30       	cpi	r30, 0x00	; 0
-    3eea:	f8 07       	cpc	r31, r24
-    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
-    3eee:	83 e0       	ldi	r24, 0x03	; 3
-    3ef0:	87 bf       	out	0x37, r24	; 55
-    3ef2:	e8 95       	spm
+    3ed4:	f0 e0       	ldi	r31, 0x00	; 0
+    3ed6:	cf 16       	cp	r12, r31
+    3ed8:	f8 e3       	ldi	r31, 0x38	; 56
+    3eda:	df 06       	cpc	r13, r31
+    3edc:	20 f0       	brcs	.+8      	; 0x3ee6 <main+0xe6>
+    3ede:	83 e0       	ldi	r24, 0x03	; 3
+    3ee0:	f6 01       	movw	r30, r12
+    3ee2:	87 bf       	out	0x37, r24	; 55
+    3ee4:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
+    3ee6:	64 d0       	rcall	.+200    	; 0x3fb0 <verifySpace>
 
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3ef6:	07 b6       	in	r0, 0x37	; 55
-    3ef8:	00 fc       	sbrc	r0, 0
-    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
-      }
-#endif
-
-      // Copy buffer into programming buffer
+    3ee8:	07 b6       	in	r0, 0x37	; 55
+    3eea:	00 fc       	sbrc	r0, 0
+    3eec:	fd cf       	rjmp	.-6      	; 0x3ee8 <main+0xe8>
+    3eee:	a6 01       	movw	r20, r12
+    3ef0:	a0 e0       	ldi	r26, 0x00	; 0
+    3ef2:	b1 e0       	ldi	r27, 0x01	; 1
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3efc:	40 91 00 02 	lds	r20, 0x0200
-    3f00:	50 91 01 02 	lds	r21, 0x0201
-    3f04:	a0 e0       	ldi	r26, 0x00	; 0
-    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3f08:	2c 91       	ld	r18, X
-    3f0a:	30 e0       	ldi	r19, 0x00	; 0
+    3ef4:	2c 91       	ld	r18, X
+    3ef6:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3f0c:	11 96       	adiw	r26, 0x01	; 1
-    3f0e:	8c 91       	ld	r24, X
-    3f10:	11 97       	sbiw	r26, 0x01	; 1
-    3f12:	90 e0       	ldi	r25, 0x00	; 0
-    3f14:	98 2f       	mov	r25, r24
-    3f16:	88 27       	eor	r24, r24
-    3f18:	82 2b       	or	r24, r18
-    3f1a:	93 2b       	or	r25, r19
+    3ef8:	11 96       	adiw	r26, 0x01	; 1
+    3efa:	8c 91       	ld	r24, X
+    3efc:	11 97       	sbiw	r26, 0x01	; 1
+    3efe:	90 e0       	ldi	r25, 0x00	; 0
+    3f00:	98 2f       	mov	r25, r24
+    3f02:	88 27       	eor	r24, r24
+    3f04:	82 2b       	or	r24, r18
+    3f06:	93 2b       	or	r25, r19
 #define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
 #define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
 /* main program starts here */
 int main(void) {
-    3f1c:	12 96       	adiw	r26, 0x02	; 2
+    3f08:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
         __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    3f1e:	fa 01       	movw	r30, r20
-    3f20:	0c 01       	movw	r0, r24
-    3f22:	d7 be       	out	0x37, r13	; 55
-    3f24:	e8 95       	spm
-    3f26:	11 24       	eor	r1, r1
+    3f0a:	fa 01       	movw	r30, r20
+    3f0c:	0c 01       	movw	r0, r24
+    3f0e:	97 be       	out	0x37, r9	; 55
+    3f10:	e8 95       	spm
+    3f12:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3f28:	4e 5f       	subi	r20, 0xFE	; 254
-    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f14:	4e 5f       	subi	r20, 0xFE	; 254
+    3f16:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
-    3f2e:	a0 38       	cpi	r26, 0x80	; 128
-    3f30:	bf 07       	cpc	r27, r31
-    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
+    3f18:	f1 e0       	ldi	r31, 0x01	; 1
+    3f1a:	a0 38       	cpi	r26, 0x80	; 128
+    3f1c:	bf 07       	cpc	r27, r31
+    3f1e:	51 f7       	brne	.-44     	; 0x3ef4 <main+0xf4>
 
       // Write from programming buffer
       __boot_page_write_short((uint16_t)(void*)address);
-    3f34:	e0 91 00 02 	lds	r30, 0x0200
-    3f38:	f0 91 01 02 	lds	r31, 0x0201
-    3f3c:	e7 be       	out	0x37, r14	; 55
-    3f3e:	e8 95       	spm
+    3f20:	f6 01       	movw	r30, r12
+    3f22:	a7 be       	out	0x37, r10	; 55
+    3f24:	e8 95       	spm
       boot_spm_busy_wait();
-    3f40:	07 b6       	in	r0, 0x37	; 55
-    3f42:	00 fc       	sbrc	r0, 0
-    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
+    3f26:	07 b6       	in	r0, 0x37	; 55
+    3f28:	00 fc       	sbrc	r0, 0
+    3f2a:	fd cf       	rjmp	.-6      	; 0x3f26 <main+0x126>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f46:	f7 be       	out	0x37, r15	; 55
-    3f48:	e8 95       	spm
-    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
+    3f2c:	b7 be       	out	0x37, r11	; 55
+    3f2e:	e8 95       	spm
+    3f30:	26 c0       	rjmp	.+76     	; 0x3f7e <main+0x17e>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f4c:	84 37       	cpi	r24, 0x74	; 116
-    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
+    3f32:	84 37       	cpi	r24, 0x74	; 116
+    3f34:	b1 f4       	brne	.+44     	; 0x3f62 <main+0x162>
       // READ PAGE - we only read flash
-      getLen();
-    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
+      getch();			/* getlen() */
+    3f36:	2e d0       	rcall	.+92     	; 0x3f94 <getch>
+      length = getch();
+    3f38:	2d d0       	rcall	.+90     	; 0x3f94 <getch>
+    3f3a:	f8 2e       	mov	r15, r24
+      getch();
+    3f3c:	2b d0       	rcall	.+86     	; 0x3f94 <getch>
+
       verifySpace();
-    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+    3f3e:	38 d0       	rcall	.+112    	; 0x3fb0 <verifySpace>
+    3f40:	f6 01       	movw	r30, r12
+    3f42:	ef 2c       	mov	r14, r15
         putch(result);
         address++;
       }
       while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f54:	e0 91 00 02 	lds	r30, 0x0200
-    3f58:	f0 91 01 02 	lds	r31, 0x0201
-    3f5c:	31 96       	adiw	r30, 0x01	; 1
-    3f5e:	f0 93 01 02 	sts	0x0201, r31
-    3f62:	e0 93 00 02 	sts	0x0200, r30
-    3f66:	31 97       	sbiw	r30, 0x01	; 1
-    3f68:	e4 91       	lpm	r30, Z+
-    3f6a:	8e 2f       	mov	r24, r30
-    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
+    3f44:	8f 01       	movw	r16, r30
+    3f46:	0f 5f       	subi	r16, 0xFF	; 255
+    3f48:	1f 4f       	sbci	r17, 0xFF	; 255
+    3f4a:	84 91       	lpm	r24, Z+
+    3f4c:	1b d0       	rcall	.+54     	; 0x3f84 <putch>
       while (--length);
-    3f6e:	80 91 02 02 	lds	r24, 0x0202
-    3f72:	81 50       	subi	r24, 0x01	; 1
-    3f74:	80 93 02 02 	sts	0x0202, r24
-    3f78:	88 23       	and	r24, r24
-    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
-    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+    3f4e:	ea 94       	dec	r14
+    3f50:	f8 01       	movw	r30, r16
+    3f52:	c1 f7       	brne	.-16     	; 0x3f44 <main+0x144>
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
+#endif
+
+/* main program starts here */
+int main(void) {
+    3f54:	08 94       	sec
+    3f56:	c1 1c       	adc	r12, r1
+    3f58:	d1 1c       	adc	r13, r1
+    3f5a:	fa 94       	dec	r15
+    3f5c:	cf 0c       	add	r12, r15
+    3f5e:	d1 1c       	adc	r13, r1
+    3f60:	0e c0       	rjmp	.+28     	; 0x3f7e <main+0x17e>
 #endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f7e:	85 37       	cpi	r24, 0x75	; 117
-    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
+    3f62:	85 37       	cpi	r24, 0x75	; 117
+    3f64:	39 f4       	brne	.+14     	; 0x3f74 <main+0x174>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
+    3f66:	24 d0       	rcall	.+72     	; 0x3fb0 <verifySpace>
       putch(SIGNATURE_0);
-    3f84:	8e e1       	ldi	r24, 0x1E	; 30
-    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
+    3f68:	8e e1       	ldi	r24, 0x1E	; 30
+    3f6a:	0c d0       	rcall	.+24     	; 0x3f84 <putch>
       putch(SIGNATURE_1);
-    3f88:	84 e9       	ldi	r24, 0x94	; 148
-    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
+    3f6c:	84 e9       	ldi	r24, 0x94	; 148
+    3f6e:	0a d0       	rcall	.+20     	; 0x3f84 <putch>
       putch(SIGNATURE_2);
-    3f8c:	86 e0       	ldi	r24, 0x06	; 6
-    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
+    3f70:	86 e0       	ldi	r24, 0x06	; 6
+    3f72:	98 cf       	rjmp	.-208    	; 0x3ea4 <main+0xa4>
     }
     else if (ch == 'Q') {
-    3f90:	81 35       	cpi	r24, 0x51	; 81
-    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
+    3f74:	81 35       	cpi	r24, 0x51	; 81
+    3f76:	11 f4       	brne	.+4      	; 0x3f7c <main+0x17c>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f94:	88 e0       	ldi	r24, 0x08	; 8
-    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
+    3f78:	88 e0       	ldi	r24, 0x08	; 8
+    3f7a:	14 d0       	rcall	.+40     	; 0x3fa4 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
+    3f7c:	19 d0       	rcall	.+50     	; 0x3fb0 <verifySpace>
     }
     putch(STK_OK);
-    3f9a:	80 e1       	ldi	r24, 0x10	; 16
-    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
-    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
+    3f7e:	80 e1       	ldi	r24, 0x10	; 16
+    3f80:	01 d0       	rcall	.+2      	; 0x3f84 <putch>
+    3f82:	6a cf       	rjmp	.-300    	; 0x3e58 <main+0x58>
 
-00003fa0 <putch>:
+00003f84 <putch>:
   }
 }
 
 void putch(char ch) {
-    3fa0:	98 2f       	mov	r25, r24
+    3f84:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fa6:	85 ff       	sbrs	r24, 5
-    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
+    3f86:	80 91 c0 00 	lds	r24, 0x00C0
+    3f8a:	85 ff       	sbrs	r24, 5
+    3f8c:	fc cf       	rjmp	.-8      	; 0x3f86 <putch+0x2>
   UDR0 = ch;
-    3faa:	90 93 c6 00 	sts	0x00C6, r25
+    3f8e:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3fae:	08 95       	ret
+    3f92:	08 95       	ret
 
-00003fb0 <getch>:
-  return getch();
+00003f94 <getch>:
 }
+#endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fb0:	a8 95       	wdr
+    3f94:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fb6:	87 ff       	sbrs	r24, 7
-    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
+    3f96:	80 91 c0 00 	lds	r24, 0x00C0
+    3f9a:	87 ff       	sbrs	r24, 7
+    3f9c:	fc cf       	rjmp	.-8      	; 0x3f96 <getch+0x2>
   ch = UDR0;
-    3fba:	80 91 c6 00 	lds	r24, 0x00C6
+    3f9e:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    3fbe:	08 95       	ret
-
-00003fc0 <getLen>:
-  } while (--count);
-}
-#endif
-
-uint8_t getLen() {
-  getch();
-    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
-  length = getch();
-    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
-    3fc4:	80 93 02 02 	sts	0x0202, r24
-  return getch();
-}
-    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
+    3fa2:	08 95       	ret
 
-00003fca <watchdogConfig>:
+00003fa4 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3fca:	e0 e6       	ldi	r30, 0x60	; 96
-    3fcc:	f0 e0       	ldi	r31, 0x00	; 0
-    3fce:	98 e1       	ldi	r25, 0x18	; 24
-    3fd0:	90 83       	st	Z, r25
+    3fa4:	e0 e6       	ldi	r30, 0x60	; 96
+    3fa6:	f0 e0       	ldi	r31, 0x00	; 0
+    3fa8:	98 e1       	ldi	r25, 0x18	; 24
+    3faa:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fd2:	80 83       	st	Z, r24
+    3fac:	80 83       	st	Z, r24
 }
-    3fd4:	08 95       	ret
-
-00003fd6 <appStart>:
-
-void appStart() {
-  watchdogConfig(WATCHDOG_OFF);
-    3fd6:	80 e0       	ldi	r24, 0x00	; 0
-    3fd8:	f8 df       	rcall	.-16     	; 0x3fca <watchdogConfig>
-  __asm__ __volatile__ (
-    3fda:	ee 27       	eor	r30, r30
-    3fdc:	ff 27       	eor	r31, r31
-    3fde:	09 94       	ijmp
+    3fae:	08 95       	ret
 
-00003fe0 <verifySpace>:
+00003fb0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
-  if (getch() != CRC_EOP) appStart();
-    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
-    3fe2:	80 32       	cpi	r24, 0x20	; 32
-    3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
-    3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
+  if (getch() != CRC_EOP) {
+    3fb0:	f1 df       	rcall	.-30     	; 0x3f94 <getch>
+    3fb2:	80 32       	cpi	r24, 0x20	; 32
+    3fb4:	19 f0       	breq	.+6      	; 0x3fbc <verifySpace+0xc>
+    watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
+    3fb6:	88 e0       	ldi	r24, 0x08	; 8
+    3fb8:	f5 df       	rcall	.-22     	; 0x3fa4 <watchdogConfig>
+    3fba:	ff cf       	rjmp	.-2      	; 0x3fba <verifySpace+0xa>
+    while (1)			      // and busy-loop so that WD causes
+      ;				      //  a reset and app start.
+  }
   putch(STK_INSYNC);
-    3fe8:	84 e1       	ldi	r24, 0x14	; 20
+    3fbc:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+    3fbe:	e2 cf       	rjmp	.-60     	; 0x3f84 <putch>
 
-00003fec <getNch>:
+00003fc0 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fec:	1f 93       	push	r17
-    3fee:	18 2f       	mov	r17, r24
+    3fc0:	1f 93       	push	r17
+    3fc2:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
-    3ff2:	11 50       	subi	r17, 0x01	; 1
-    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
+    3fc4:	e7 df       	rcall	.-50     	; 0x3f94 <getch>
+    3fc6:	11 50       	subi	r17, 0x01	; 1
+    3fc8:	e9 f7       	brne	.-6      	; 0x3fc4 <getNch+0x4>
   verifySpace();
-    3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
+    3fca:	f2 df       	rcall	.-28     	; 0x3fb0 <verifySpace>
+}
+    3fcc:	1f 91       	pop	r17
+    3fce:	08 95       	ret
+
+00003fd0 <appStart>:
+  WDTCSR = _BV(WDCE) | _BV(WDE);
+  WDTCSR = x;
 }
-    3ff8:	1f 91       	pop	r17
-    3ffa:	08 95       	ret
+
+void appStart() {
+  watchdogConfig(WATCHDOG_OFF);
+    3fd0:	80 e0       	ldi	r24, 0x00	; 0
+    3fd2:	e8 df       	rcall	.-48     	; 0x3fa4 <watchdogConfig>
+  __asm__ __volatile__ (
+    3fd4:	ee 27       	eor	r30, r30
+    3fd6:	ff 27       	eor	r31, r31
+    3fd8:	09 94       	ijmp



[Date Prev][Date Next]   [Thread Prev][Thread Next]   [Thread Index] [Date Index] [Author Index]