diff --git a/optiboot/bootloaders/optiboot/optiboot_atmega1280.hex b/optiboot/bootloaders/optiboot/optiboot_atmega1280.hex
index 04d137c..accc1ff 100644
--- a/optiboot/bootloaders/optiboot/optiboot_atmega1280.hex
+++ b/optiboot/bootloaders/optiboot/optiboot_atmega1280.hex
@@ -1,35 +1,42 @@
 :020000021000EC
-:10FC0000112484B714BE81FFE8D085E08093810081
-:10FC100082E08093C00088E18093C10086E08093F9
-:10FC2000C20080E18093C4008EE0C1D0279A86E0B4
-:10FC300020E33CEF91E0309385002093840096BB55
-:10FC4000B09BFECF1F9AA8958150A9F7EE24FF2400
-:10FC5000BB24B394A5E0CA2EF1E1DF2E9CD0813401
-:10FC600061F499D0082FA9D0023841F1013811F47C
-:10FC700085E001C083E087D083C0823411F484E141
-:10FC800003C0853419F485E0A0D07AC0853591F49D
-:10FC900082D0082F10E07FD0E82EFF24FE2CEE2427
-:10FCA000E02AF12A8F2D881F8827881F8BBFEE0C32
-:10FCB000FF1C65C0863521F484E087D080E0DBCF6F
-:10FCC000843609F042C067D066D0082F64D080E047
-:10FCD000E81680EEF80620F483E0F70187BFE89588
-:10FCE000C0E0D2E058D089930C17E1F7F0E0EF16AE
-:10FCF000F0EEFF0620F083E0F70187BFE8955DD0C6
-:10FD000007B600FCFDCFA701A0E0B2E02C9130E0E7
-:10FD100011968C91119790E0982F8827822B932B26
-:10FD20001296FA010C01B7BEE89511244E5F5F4FA1
-:10FD3000F3E0A030BF0751F7F701C7BEE89507B65B
-:10FD400000FCFDCFD7BEE8951BC0843761F423D0FB
-:10FD500022D0082F20D031D0F70187917F0113D016
-:10FD60000150D1F70DC0853731F427D08EE10BD08B
-:10FD700087E909D07FCF813511F488E018D01DD0F4
-:10FD800080E101D06BCF982F8091C00085FFFCCF20
-:10FD90009093C60008958091C00087FFFCCF8091AA
-:10FDA000C00084FD01C0A8958091C6000895E0E6DA
-:10FDB000F0E098E1908380830895EDDF803219F0C0
-:10FDC00088E0F5DFFFCF84E1DECF1F93182FE3DF5C
-:10FDD0001150E9F7F2DF1F910895282E80E0E7DF48
-:06FDE000EE27FF27099445
-:02FFFE000005FC
+:10FC0000112484B714BE81FD01C00CD185E080931E
+:10FC1000810082E08093C00088E18093C10086E08B
+:10FC20008093C20080E18093C4008EE0E5D0279AE3
+:10FC300086E020E33CEF91E0309385002093840040
+:10FC400096BBB09BFECF1F9AA8958150A9F7CC24F4
+:10FC5000DD248824839425E0A22E91E1992EC0D042
+:10FC6000813461F4BDD0182FCDD0123851F1113844
+:10FC700011F486E001C083E0ABD0A7C0823411F458
+:10FC800084E103C0853419F485E0C4D09EC0853575
+:10FC9000A1F4A6D0082F10E0A3D0E82EFF24FE2C5C
+:10FCA000EE24E02AF12A8F2D881F8827881F8BBF1A
+:10FCB000EE0CFF1CA7D0670188C0863521F484E0D4
+:10FCC000A9D080E0D9CF843609F052C089D090E025
+:10FCD000182F002785D090E0082B192B81D0B82E43
+:10FCE000E801E12CA2E0FA2E7BD0F70181937F019D
+:10FCF0002197D1F787D0F5E4BF1689F4E601E12C0E
+:10FD0000F2E0FF2E08C0CE012196F70161917F013C
+:10FD100097D00150104001151105A9F756C083E096
+:10FD2000F60187BFE89507B600FCFDCFB801A60134
+:10FD3000A0E0B2E02C9130E011968C91119790E008
+:10FD4000982F8827822B932B1296FA010C0187BEDD
+:10FD5000E89511244E5F5F4F6250704059F7F601ED
+:10FD6000A7BEE89507B600FCFDCF97BEE8952DC06D
+:10FD70008437F1F435D090E0D82FCC2731D090E003
+:10FD8000C82BD92B2DD0182F3DD0153449F486011E
+:10FD9000C8010F5F1F4F4CD01BD02197C9F715C06A
+:10FDA0008601F80187918F0113D02197D1F70DC0FB
+:10FDB000853731F427D08EE10BD087E909D05BCFAE
+:10FDC000813511F488E018D01DD080E101D047CFF3
+:10FDD000982F8091C00085FFFCCF9093C6000895B6
+:10FDE0008091C00087FFFCCF8091C00084FD01C0DE
+:10FDF000A8958091C6000895E0E6F0E098E1908330
+:10FE000080830895EDDF803219F088E0F5DFFFCFC1
+:10FE100084E1DECF1F93182FE3DF1150E9F7F2DF03
+:10FE20001F910895282E80E0E7DFEE27FF27099431
+:10FE3000F999FECF92BD81BDF89A992780B50895B2
+:10FE4000262FF999FECF1FBA92BD81BD20BD0FB6F6
+:0CFE5000F894FA9AF99A0FBE01960895F2
+:02FFFE000006FB
 :040000031000FC00ED
 :00000001FF
diff --git a/optiboot/bootloaders/optiboot/optiboot_atmega1280.lst b/optiboot/bootloaders/optiboot/optiboot_atmega1280.lst
index 2e7ecbd..632db55 100644
--- a/optiboot/bootloaders/optiboot/optiboot_atmega1280.lst
+++ b/optiboot/bootloaders/optiboot/optiboot_atmega1280.lst
@@ -3,36 +3,40 @@ optiboot_atmega1280.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001e6  0001fc00  0001fc00  00000054  2**1
+  0 .text         0000025c  0001fc00  0001fc00  00000074  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  0001fffe  0001fffe  0000023a  2**0
-                  CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  0000023c  2**0
+  1 .version      00000002  0001fffe  0001fffe  000002d0  2**0
+                  CONTENTS, ALLOC, LOAD, DATA
+  2 .stab         00000180  00000000  00000000  000002d4  2**2
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000005f  00000000  00000000  00000264  2**0
+  3 .stabstr      000000bb  00000000  00000000  00000454  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000294  00000000  00000000  000002c3  2**0
+  4 .debug_aranges 00000028  00000000  00000000  0000050f  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000016b  00000000  00000000  00000557  2**0
+  5 .debug_pubnames 00000074  00000000  00000000  00000537  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   0000040f  00000000  00000000  000006c2  2**0
+  6 .debug_info   000003df  00000000  00000000  000005ab  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000080  00000000  00000000  00000ad4  2**2
+  7 .debug_abbrev 000001ea  00000000  00000000  0000098a  2**0
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000131  00000000  00000000  00000b54  2**0
+  8 .debug_line   0000044b  00000000  00000000  00000b74  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    00000256  00000000  00000000  00000c85  2**0
+  9 .debug_frame  00000080  00000000  00000000  00000fc0  2**2
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000078  00000000  00000000  00000edb  2**0
+ 10 .debug_str    00000172  00000000  00000000  00001040  2**0
+                  CONTENTS, READONLY, DEBUGGING
+ 11 .debug_loc    000003b3  00000000  00000000  000011b2  2**0
+                  CONTENTS, READONLY, DEBUGGING
+ 12 .debug_ranges 000000d0  00000000  00000000  00001565  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 0001fc00 <main>:
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
    1fc00:	11 24       	eor	r1, r1
@@ -46,537 +50,630 @@ int main(void) {
   MCUSR = 0;
    1fc04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart(ch);
-   1fc06:	81 ff       	sbrs	r24, 1
-   1fc08:	e8 d0       	rcall	.+464    	; 0x1fdda <appStart>
+   1fc06:	81 fd       	sbrc	r24, 1
+   1fc08:	01 c0       	rjmp	.+2      	; 0x1fc0c <main+0xc>
+   1fc0a:	0c d1       	rcall	.+536    	; 0x1fe24 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-   1fc0a:	85 e0       	ldi	r24, 0x05	; 5
-   1fc0c:	80 93 81 00 	sts	0x0081, r24
+   1fc0c:	85 e0       	ldi	r24, 0x05	; 5
+   1fc0e:	80 93 81 00 	sts	0x0081, r24
   UCSRA = _BV(U2X); //Double speed mode USART
   UCSRB = _BV(RXEN) | _BV(TXEN);  // enable Rx & Tx
   UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0);  // config USART; 8N1
   UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
 #else
   UART_SRA = _BV(U2X0); //Double speed mode USART0
-   1fc10:	82 e0       	ldi	r24, 0x02	; 2
-   1fc12:	80 93 c0 00 	sts	0x00C0, r24
+   1fc12:	82 e0       	ldi	r24, 0x02	; 2
+   1fc14:	80 93 c0 00 	sts	0x00C0, r24
   UART_SRB = _BV(RXEN0) | _BV(TXEN0);
-   1fc16:	88 e1       	ldi	r24, 0x18	; 24
-   1fc18:	80 93 c1 00 	sts	0x00C1, r24
+   1fc18:	88 e1       	ldi	r24, 0x18	; 24
+   1fc1a:	80 93 c1 00 	sts	0x00C1, r24
   UART_SRC = _BV(UCSZ00) | _BV(UCSZ01);
-   1fc1c:	86 e0       	ldi	r24, 0x06	; 6
-   1fc1e:	80 93 c2 00 	sts	0x00C2, r24
+   1fc1e:	86 e0       	ldi	r24, 0x06	; 6
+   1fc20:	80 93 c2 00 	sts	0x00C2, r24
   UART_SRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-   1fc22:	80 e1       	ldi	r24, 0x10	; 16
-   1fc24:	80 93 c4 00 	sts	0x00C4, r24
+   1fc24:	80 e1       	ldi	r24, 0x10	; 16
+   1fc26:	80 93 c4 00 	sts	0x00C4, r24
 #endif
 #endif
 
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
-   1fc28:	8e e0       	ldi	r24, 0x0E	; 14
-   1fc2a:	c1 d0       	rcall	.+386    	; 0x1fdae <watchdogConfig>
+   1fc2a:	8e e0       	ldi	r24, 0x0E	; 14
+   1fc2c:	e5 d0       	rcall	.+458    	; 0x1fdf8 <watchdogConfig>
 
 #if (LED_START_FLASHES > 0) || defined(LED_DATA_FLASH)
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-   1fc2c:	27 9a       	sbi	0x04, 7	; 4
-   1fc2e:	86 e0       	ldi	r24, 0x06	; 6
+   1fc2e:	27 9a       	sbi	0x04, 7	; 4
+   1fc30:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-   1fc30:	20 e3       	ldi	r18, 0x30	; 48
-   1fc32:	3c ef       	ldi	r19, 0xFC	; 252
+   1fc32:	20 e3       	ldi	r18, 0x30	; 48
+   1fc34:	3c ef       	ldi	r19, 0xFC	; 252
     TIFR1 = _BV(TOV1);
-   1fc34:	91 e0       	ldi	r25, 0x01	; 1
+   1fc36:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-   1fc36:	30 93 85 00 	sts	0x0085, r19
-   1fc3a:	20 93 84 00 	sts	0x0084, r18
+   1fc38:	30 93 85 00 	sts	0x0085, r19
+   1fc3c:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-   1fc3e:	96 bb       	out	0x16, r25	; 22
+   1fc40:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-   1fc40:	b0 9b       	sbis	0x16, 0	; 22
-   1fc42:	fe cf       	rjmp	.-4      	; 0x1fc40 <main+0x40>
+   1fc42:	b0 9b       	sbis	0x16, 0	; 22
+   1fc44:	fe cf       	rjmp	.-4      	; 0x1fc42 <main+0x42>
 #if defined(__AVR_ATmega8__)  || defined (__AVR_ATmega32__)
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
-   1fc44:	1f 9a       	sbi	0x03, 7	; 3
+   1fc46:	1f 9a       	sbi	0x03, 7	; 3
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-   1fc46:	a8 95       	wdr
+   1fc48:	a8 95       	wdr
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
 #endif
     watchdogReset();
   } while (--count);
-   1fc48:	81 50       	subi	r24, 0x01	; 1
-   1fc4a:	a9 f7       	brne	.-22     	; 0x1fc36 <main+0x36>
-   1fc4c:	ee 24       	eor	r14, r14
-   1fc4e:	ff 24       	eor	r15, r15
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-        a |= (*bufPtr++) << 8;
-        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-   1fc50:	bb 24       	eor	r11, r11
-   1fc52:	b3 94       	inc	r11
-        addrPtr += 2;
-      } while (--ch);
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-   1fc54:	a5 e0       	ldi	r26, 0x05	; 5
-   1fc56:	ca 2e       	mov	r12, r26
-      boot_spm_busy_wait();
+   1fc4a:	81 50       	subi	r24, 0x01	; 1
+   1fc4c:	a9 f7       	brne	.-22     	; 0x1fc38 <main+0x38>
+   1fc4e:	cc 24       	eor	r12, r12
+   1fc50:	dd 24       	eor	r13, r13
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+   1fc52:	88 24       	eor	r8, r8
+   1fc54:	83 94       	inc	r8
+	    } while (len -= 2);
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+   1fc56:	25 e0       	ldi	r18, 0x05	; 5
+   1fc58:	a2 2e       	mov	r10, r18
+	    boot_spm_busy_wait();
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-   1fc58:	f1 e1       	ldi	r31, 0x11	; 17
-   1fc5a:	df 2e       	mov	r13, r31
+	    // Reenable read access to flash
+	    boot_rww_enable();
+   1fc5a:	91 e1       	ldi	r25, 0x11	; 17
+   1fc5c:	99 2e       	mov	r9, r25
 #endif
 
-  /* Forever loop */
+  /* Forever loop: exits by causing WDT reset */
   for (;;) {
     /* get character from UART */
     ch = getch();
-   1fc5c:	9c d0       	rcall	.+312    	; 0x1fd96 <getch>
+   1fc5e:	c0 d0       	rcall	.+384    	; 0x1fde0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-   1fc5e:	81 34       	cpi	r24, 0x41	; 65
-   1fc60:	61 f4       	brne	.+24     	; 0x1fc7a <main+0x7a>
+   1fc60:	81 34       	cpi	r24, 0x41	; 65
+   1fc62:	61 f4       	brne	.+24     	; 0x1fc7c <main+0x7c>
       unsigned char which = getch();
-   1fc62:	99 d0       	rcall	.+306    	; 0x1fd96 <getch>
-   1fc64:	08 2f       	mov	r16, r24
+   1fc64:	bd d0       	rcall	.+378    	; 0x1fde0 <getch>
+   1fc66:	18 2f       	mov	r17, r24
       verifySpace();
-   1fc66:	a9 d0       	rcall	.+338    	; 0x1fdba <verifySpace>
+   1fc68:	cd d0       	rcall	.+410    	; 0x1fe04 <verifySpace>
       if (which == 0x82) {
-   1fc68:	02 38       	cpi	r16, 0x82	; 130
-   1fc6a:	41 f1       	breq	.+80     	; 0x1fcbc <main+0xbc>
+   1fc6a:	12 38       	cpi	r17, 0x82	; 130
+   1fc6c:	51 f1       	breq	.+84     	; 0x1fcc2 <main+0xc2>
 	/*
 	 * Send optiboot version as "minor SW version"
 	 */
 	putch(OPTIBOOT_MINVER);
       } else if (which == 0x81) {
-   1fc6c:	01 38       	cpi	r16, 0x81	; 129
-   1fc6e:	11 f4       	brne	.+4      	; 0x1fc74 <main+0x74>
+   1fc6e:	11 38       	cpi	r17, 0x81	; 129
+   1fc70:	11 f4       	brne	.+4      	; 0x1fc76 <main+0x76>
 	  putch(OPTIBOOT_MAJVER);
-   1fc70:	85 e0       	ldi	r24, 0x05	; 5
-   1fc72:	01 c0       	rjmp	.+2      	; 0x1fc76 <main+0x76>
+   1fc72:	86 e0       	ldi	r24, 0x06	; 6
+   1fc74:	01 c0       	rjmp	.+2      	; 0x1fc78 <main+0x78>
       } else {
 	/*
 	 * GET PARAMETER returns a generic 0x03 reply for
          * other parameters - enough to keep Avrdude happy
 	 */
 	putch(0x03);
-   1fc74:	83 e0       	ldi	r24, 0x03	; 3
-   1fc76:	87 d0       	rcall	.+270    	; 0x1fd86 <putch>
-   1fc78:	83 c0       	rjmp	.+262    	; 0x1fd80 <main+0x180>
+   1fc76:	83 e0       	ldi	r24, 0x03	; 3
+   1fc78:	ab d0       	rcall	.+342    	; 0x1fdd0 <putch>
+   1fc7a:	a7 c0       	rjmp	.+334    	; 0x1fdca <main+0x1ca>
       }
     }
     else if(ch == STK_SET_DEVICE) {
-   1fc7a:	82 34       	cpi	r24, 0x42	; 66
-   1fc7c:	11 f4       	brne	.+4      	; 0x1fc82 <main+0x82>
+   1fc7c:	82 34       	cpi	r24, 0x42	; 66
+   1fc7e:	11 f4       	brne	.+4      	; 0x1fc84 <main+0x84>
       // SET DEVICE is ignored
       getNch(20);
-   1fc7e:	84 e1       	ldi	r24, 0x14	; 20
-   1fc80:	03 c0       	rjmp	.+6      	; 0x1fc88 <main+0x88>
+   1fc80:	84 e1       	ldi	r24, 0x14	; 20
+   1fc82:	03 c0       	rjmp	.+6      	; 0x1fc8a <main+0x8a>
     }
     else if(ch == STK_SET_DEVICE_EXT) {
-   1fc82:	85 34       	cpi	r24, 0x45	; 69
-   1fc84:	19 f4       	brne	.+6      	; 0x1fc8c <main+0x8c>
+   1fc84:	85 34       	cpi	r24, 0x45	; 69
+   1fc86:	19 f4       	brne	.+6      	; 0x1fc8e <main+0x8e>
       // SET DEVICE EXT is ignored
       getNch(5);
-   1fc86:	85 e0       	ldi	r24, 0x05	; 5
-   1fc88:	a0 d0       	rcall	.+320    	; 0x1fdca <getNch>
-   1fc8a:	7a c0       	rjmp	.+244    	; 0x1fd80 <main+0x180>
+   1fc88:	85 e0       	ldi	r24, 0x05	; 5
+   1fc8a:	c4 d0       	rcall	.+392    	; 0x1fe14 <getNch>
+   1fc8c:	9e c0       	rjmp	.+316    	; 0x1fdca <main+0x1ca>
     }
     else if(ch == STK_LOAD_ADDRESS) {
-   1fc8c:	85 35       	cpi	r24, 0x55	; 85
-   1fc8e:	91 f4       	brne	.+36     	; 0x1fcb4 <main+0xb4>
+   1fc8e:	85 35       	cpi	r24, 0x55	; 85
+   1fc90:	a1 f4       	brne	.+40     	; 0x1fcba <main+0xba>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-   1fc90:	82 d0       	rcall	.+260    	; 0x1fd96 <getch>
+   1fc92:	a6 d0       	rcall	.+332    	; 0x1fde0 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-   1fc92:	08 2f       	mov	r16, r24
-   1fc94:	10 e0       	ldi	r17, 0x00	; 0
-   1fc96:	7f d0       	rcall	.+254    	; 0x1fd96 <getch>
-   1fc98:	e8 2e       	mov	r14, r24
-   1fc9a:	ff 24       	eor	r15, r15
-   1fc9c:	fe 2c       	mov	r15, r14
-   1fc9e:	ee 24       	eor	r14, r14
-   1fca0:	e0 2a       	or	r14, r16
-   1fca2:	f1 2a       	or	r15, r17
+   1fc94:	08 2f       	mov	r16, r24
+   1fc96:	10 e0       	ldi	r17, 0x00	; 0
+   1fc98:	a3 d0       	rcall	.+326    	; 0x1fde0 <getch>
+   1fc9a:	e8 2e       	mov	r14, r24
+   1fc9c:	ff 24       	eor	r15, r15
+   1fc9e:	fe 2c       	mov	r15, r14
+   1fca0:	ee 24       	eor	r14, r14
+   1fca2:	e0 2a       	or	r14, r16
+   1fca4:	f1 2a       	or	r15, r17
 #ifdef RAMPZ
       // Transfer top bit to RAMPZ
       RAMPZ = (newAddress & 0x8000) ? 1 : 0;
-   1fca4:	8f 2d       	mov	r24, r15
-   1fca6:	88 1f       	adc	r24, r24
-   1fca8:	88 27       	eor	r24, r24
-   1fcaa:	88 1f       	adc	r24, r24
-   1fcac:	8b bf       	out	0x3b, r24	; 59
+   1fca6:	8f 2d       	mov	r24, r15
+   1fca8:	88 1f       	adc	r24, r24
+   1fcaa:	88 27       	eor	r24, r24
+   1fcac:	88 1f       	adc	r24, r24
+   1fcae:	8b bf       	out	0x3b, r24	; 59
 #endif
       newAddress += newAddress; // Convert from word address to byte address
-   1fcae:	ee 0c       	add	r14, r14
-   1fcb0:	ff 1c       	adc	r15, r15
-   1fcb2:	65 c0       	rjmp	.+202    	; 0x1fd7e <main+0x17e>
+   1fcb0:	ee 0c       	add	r14, r14
+   1fcb2:	ff 1c       	adc	r15, r15
       address = newAddress;
       verifySpace();
+   1fcb4:	a7 d0       	rcall	.+334    	; 0x1fe04 <verifySpace>
+   1fcb6:	67 01       	movw	r12, r14
+   1fcb8:	88 c0       	rjmp	.+272    	; 0x1fdca <main+0x1ca>
     }
     else if(ch == STK_UNIVERSAL) {
-   1fcb4:	86 35       	cpi	r24, 0x56	; 86
-   1fcb6:	21 f4       	brne	.+8      	; 0x1fcc0 <main+0xc0>
+   1fcba:	86 35       	cpi	r24, 0x56	; 86
+   1fcbc:	21 f4       	brne	.+8      	; 0x1fcc6 <main+0xc6>
       // UNIVERSAL command is ignored
       getNch(4);
-   1fcb8:	84 e0       	ldi	r24, 0x04	; 4
-   1fcba:	87 d0       	rcall	.+270    	; 0x1fdca <getNch>
+   1fcbe:	84 e0       	ldi	r24, 0x04	; 4
+   1fcc0:	a9 d0       	rcall	.+338    	; 0x1fe14 <getNch>
       putch(0x00);
-   1fcbc:	80 e0       	ldi	r24, 0x00	; 0
-   1fcbe:	db cf       	rjmp	.-74     	; 0x1fc76 <main+0x76>
+   1fcc2:	80 e0       	ldi	r24, 0x00	; 0
+   1fcc4:	d9 cf       	rjmp	.-78     	; 0x1fc78 <main+0x78>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-   1fcc0:	84 36       	cpi	r24, 0x64	; 100
-   1fcc2:	09 f0       	breq	.+2      	; 0x1fcc6 <main+0xc6>
-   1fcc4:	42 c0       	rjmp	.+132    	; 0x1fd4a <main+0x14a>
+   1fcc6:	84 36       	cpi	r24, 0x64	; 100
+   1fcc8:	09 f0       	breq	.+2      	; 0x1fccc <main+0xcc>
+   1fcca:	52 c0       	rjmp	.+164    	; 0x1fd70 <main+0x170>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
+      uint8_t desttype;
       uint8_t *bufPtr;
-      uint16_t addrPtr;
+      uint16_t savelength;
 
-      getch();			/* getlen() */
-   1fcc6:	67 d0       	rcall	.+206    	; 0x1fd96 <getch>
-      length = getch();
-   1fcc8:	66 d0       	rcall	.+204    	; 0x1fd96 <getch>
-   1fcca:	08 2f       	mov	r16, r24
-      getch();
-   1fccc:	64 d0       	rcall	.+200    	; 0x1fd96 <getch>
+      length = getch()<<8;			/* getlen() */
+   1fccc:	89 d0       	rcall	.+274    	; 0x1fde0 <getch>
+   1fcce:	90 e0       	ldi	r25, 0x00	; 0
+   1fcd0:	18 2f       	mov	r17, r24
+   1fcd2:	00 27       	eor	r16, r16
+      length |= getch();
+   1fcd4:	85 d0       	rcall	.+266    	; 0x1fde0 <getch>
+   1fcd6:	90 e0       	ldi	r25, 0x00	; 0
+   1fcd8:	08 2b       	or	r16, r24
+   1fcda:	19 2b       	or	r17, r25
+      savelength = length;
+      desttype = getch();
+   1fcdc:	81 d0       	rcall	.+258    	; 0x1fde0 <getch>
+   1fcde:	b8 2e       	mov	r11, r24
+   1fce0:	e8 01       	movw	r28, r16
+   1fce2:	e1 2c       	mov	r14, r1
+   1fce4:	a2 e0       	ldi	r26, 0x02	; 2
+   1fce6:	fa 2e       	mov	r15, r26
 
-      // If we are in RWW section, immediately start page erase
-      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-   1fcce:	80 e0       	ldi	r24, 0x00	; 0
-   1fcd0:	e8 16       	cp	r14, r24
-   1fcd2:	80 ee       	ldi	r24, 0xE0	; 224
-   1fcd4:	f8 06       	cpc	r15, r24
-   1fcd6:	20 f4       	brcc	.+8      	; 0x1fce0 <main+0xe0>
-   1fcd8:	83 e0       	ldi	r24, 0x03	; 3
-   1fcda:	f7 01       	movw	r30, r14
-   1fcdc:	87 bf       	out	0x37, r24	; 55
-   1fcde:	e8 95       	spm
-   1fce0:	c0 e0       	ldi	r28, 0x00	; 0
-   1fce2:	d2 e0       	ldi	r29, 0x02	; 2
-
-      // While that is going on, read in page contents
+      // read a page worth of contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-   1fce4:	58 d0       	rcall	.+176    	; 0x1fd96 <getch>
-   1fce6:	89 93       	st	Y+, r24
+   1fce8:	7b d0       	rcall	.+246    	; 0x1fde0 <getch>
+   1fcea:	f7 01       	movw	r30, r14
+   1fcec:	81 93       	st	Z+, r24
+   1fcee:	7f 01       	movw	r14, r30
       while (--length);
-   1fce8:	0c 17       	cp	r16, r28
-   1fcea:	e1 f7       	brne	.-8      	; 0x1fce4 <main+0xe4>
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-   1fcec:	f0 e0       	ldi	r31, 0x00	; 0
-   1fcee:	ef 16       	cp	r14, r31
-   1fcf0:	f0 ee       	ldi	r31, 0xE0	; 224
-   1fcf2:	ff 06       	cpc	r15, r31
-   1fcf4:	20 f0       	brcs	.+8      	; 0x1fcfe <main+0xfe>
-   1fcf6:	83 e0       	ldi	r24, 0x03	; 3
-   1fcf8:	f7 01       	movw	r30, r14
-   1fcfa:	87 bf       	out	0x37, r24	; 55
-   1fcfc:	e8 95       	spm
+   1fcf0:	21 97       	sbiw	r28, 0x01	; 1
+   1fcf2:	d1 f7       	brne	.-12     	; 0x1fce8 <main+0xe8>
 
       // Read command terminator, start reply
       verifySpace();
-   1fcfe:	5d d0       	rcall	.+186    	; 0x1fdba <verifySpace>
-
-      // If only a partial page is to be programmed, the erase might not be complete.
-      // So check that here
-      boot_spm_busy_wait();
-   1fd00:	07 b6       	in	r0, 0x37	; 55
-   1fd02:	00 fc       	sbrc	r0, 0
-   1fd04:	fd cf       	rjmp	.-6      	; 0x1fd00 <main+0x100>
-   1fd06:	a7 01       	movw	r20, r14
-   1fd08:	a0 e0       	ldi	r26, 0x00	; 0
-   1fd0a:	b2 e0       	ldi	r27, 0x02	; 2
-      bufPtr = buff;
-      addrPtr = (uint16_t)(void*)address;
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-   1fd0c:	2c 91       	ld	r18, X
-   1fd0e:	30 e0       	ldi	r19, 0x00	; 0
-        a |= (*bufPtr++) << 8;
-   1fd10:	11 96       	adiw	r26, 0x01	; 1
-   1fd12:	8c 91       	ld	r24, X
-   1fd14:	11 97       	sbiw	r26, 0x01	; 1
-   1fd16:	90 e0       	ldi	r25, 0x00	; 0
-   1fd18:	98 2f       	mov	r25, r24
-   1fd1a:	88 27       	eor	r24, r24
-   1fd1c:	82 2b       	or	r24, r18
-   1fd1e:	93 2b       	or	r25, r19
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+   1fcf4:	87 d0       	rcall	.+270    	; 0x1fe04 <verifySpace>
+ * void writebuffer(memtype, buffer, address, length)
+ */
+static inline void writebuffer(int8_t memtype, uint8_t *mybuff,
+			       uint16_t address, uint16_t len)
+{
+    switch (memtype) {
+   1fcf6:	f5 e4       	ldi	r31, 0x45	; 69
+   1fcf8:	bf 16       	cp	r11, r31
+   1fcfa:	89 f4       	brne	.+34     	; 0x1fd1e <main+0x11e>
+   1fcfc:	e6 01       	movw	r28, r12
+   1fcfe:	e1 2c       	mov	r14, r1
+   1fd00:	f2 e0       	ldi	r31, 0x02	; 2
+   1fd02:	ff 2e       	mov	r15, r31
+   1fd04:	08 c0       	rjmp	.+16     	; 0x1fd16 <main+0x116>
+   1fd06:	ce 01       	movw	r24, r28
+    case 'E': // EEPROM
+#if defined(SUPPORT_EEPROM) || defined(BIGBOOT)
+        while(len--) {
+	    eeprom_write_byte((uint8_t *)(address++), *mybuff++);
+   1fd08:	21 96       	adiw	r28, 0x01	; 1
+   1fd0a:	f7 01       	movw	r30, r14
+   1fd0c:	61 91       	ld	r22, Z+
+   1fd0e:	7f 01       	movw	r14, r30
+   1fd10:	97 d0       	rcall	.+302    	; 0x1fe40 <__eewr_byte_m1280>
+   1fd12:	01 50       	subi	r16, 0x01	; 1
+   1fd14:	10 40       	sbci	r17, 0x00	; 0
+			       uint16_t address, uint16_t len)
+{
+    switch (memtype) {
+    case 'E': // EEPROM
+#if defined(SUPPORT_EEPROM) || defined(BIGBOOT)
+        while(len--) {
+   1fd16:	01 15       	cp	r16, r1
+   1fd18:	11 05       	cpc	r17, r1
+   1fd1a:	a9 f7       	brne	.-22     	; 0x1fd06 <main+0x106>
+   1fd1c:	56 c0       	rjmp	.+172    	; 0x1fdca <main+0x1ca>
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+   1fd1e:	83 e0       	ldi	r24, 0x03	; 3
+   1fd20:	f6 01       	movw	r30, r12
+   1fd22:	87 bf       	out	0x37, r24	; 55
+   1fd24:	e8 95       	spm
+	    boot_spm_busy_wait();
+   1fd26:	07 b6       	in	r0, 0x37	; 55
+   1fd28:	00 fc       	sbrc	r0, 0
+   1fd2a:	fd cf       	rjmp	.-6      	; 0x1fd26 <main+0x126>
+   1fd2c:	b8 01       	movw	r22, r16
+   1fd2e:	a6 01       	movw	r20, r12
+   1fd30:	a0 e0       	ldi	r26, 0x00	; 0
+   1fd32:	b2 e0       	ldi	r27, 0x02	; 2
+	    /*
+	     * Copy data from the buffer into the flash write buffer.
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+   1fd34:	2c 91       	ld	r18, X
+   1fd36:	30 e0       	ldi	r19, 0x00	; 0
+		a |= (*bufPtr++) << 8;
+   1fd38:	11 96       	adiw	r26, 0x01	; 1
+   1fd3a:	8c 91       	ld	r24, X
+   1fd3c:	11 97       	sbiw	r26, 0x01	; 1
+   1fd3e:	90 e0       	ldi	r25, 0x00	; 0
+   1fd40:	98 2f       	mov	r25, r24
+   1fd42:	88 27       	eor	r24, r24
+   1fd44:	82 2b       	or	r24, r18
+   1fd46:	93 2b       	or	r25, r19
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
-   1fd20:	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);
-   1fd22:	fa 01       	movw	r30, r20
-   1fd24:	0c 01       	movw	r0, r24
-   1fd26:	b7 be       	out	0x37, r11	; 55
-   1fd28:	e8 95       	spm
-   1fd2a:	11 24       	eor	r1, r1
-        addrPtr += 2;
-   1fd2c:	4e 5f       	subi	r20, 0xFE	; 254
-   1fd2e:	5f 4f       	sbci	r21, 0xFF	; 255
-      } while (--ch);
-   1fd30:	f3 e0       	ldi	r31, 0x03	; 3
-   1fd32:	a0 30       	cpi	r26, 0x00	; 0
-   1fd34:	bf 07       	cpc	r27, r31
-   1fd36:	51 f7       	brne	.-44     	; 0x1fd0c <main+0x10c>
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-   1fd38:	f7 01       	movw	r30, r14
-   1fd3a:	c7 be       	out	0x37, r12	; 55
-   1fd3c:	e8 95       	spm
-      boot_spm_busy_wait();
-   1fd3e:	07 b6       	in	r0, 0x37	; 55
-   1fd40:	00 fc       	sbrc	r0, 0
-   1fd42:	fd cf       	rjmp	.-6      	; 0x1fd3e <main+0x13e>
+   1fd48:	12 96       	adiw	r26, 0x02	; 2
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+   1fd4a:	fa 01       	movw	r30, r20
+   1fd4c:	0c 01       	movw	r0, r24
+   1fd4e:	87 be       	out	0x37, r8	; 55
+   1fd50:	e8 95       	spm
+   1fd52:	11 24       	eor	r1, r1
+		addrPtr += 2;
+   1fd54:	4e 5f       	subi	r20, 0xFE	; 254
+   1fd56:	5f 4f       	sbci	r21, 0xFF	; 255
+	    } while (len -= 2);
+   1fd58:	62 50       	subi	r22, 0x02	; 2
+   1fd5a:	70 40       	sbci	r23, 0x00	; 0
+   1fd5c:	59 f7       	brne	.-42     	; 0x1fd34 <main+0x134>
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+   1fd5e:	f6 01       	movw	r30, r12
+   1fd60:	a7 be       	out	0x37, r10	; 55
+   1fd62:	e8 95       	spm
+	    boot_spm_busy_wait();
+   1fd64:	07 b6       	in	r0, 0x37	; 55
+   1fd66:	00 fc       	sbrc	r0, 0
+   1fd68:	fd cf       	rjmp	.-6      	; 0x1fd64 <main+0x164>
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-   1fd44:	d7 be       	out	0x37, r13	; 55
-   1fd46:	e8 95       	spm
-   1fd48:	1b c0       	rjmp	.+54     	; 0x1fd80 <main+0x180>
-#endif
+	    // Reenable read access to flash
+	    boot_rww_enable();
+   1fd6a:	97 be       	out	0x37, r9	; 55
+   1fd6c:	e8 95       	spm
+   1fd6e:	2d c0       	rjmp	.+90     	; 0x1fdca <main+0x1ca>
+      writebuffer(desttype, buff, address, savelength);
+
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-   1fd4a:	84 37       	cpi	r24, 0x74	; 116
-   1fd4c:	61 f4       	brne	.+24     	; 0x1fd66 <main+0x166>
-      // READ PAGE - we only read flash
-      getch();			/* getlen() */
-   1fd4e:	23 d0       	rcall	.+70     	; 0x1fd96 <getch>
-      length = getch();
-   1fd50:	22 d0       	rcall	.+68     	; 0x1fd96 <getch>
-   1fd52:	08 2f       	mov	r16, r24
-      getch();
-   1fd54:	20 d0       	rcall	.+64     	; 0x1fd96 <getch>
+   1fd70:	84 37       	cpi	r24, 0x74	; 116
+   1fd72:	f1 f4       	brne	.+60     	; 0x1fdb0 <main+0x1b0>
+      uint8_t desttype;
+      length = getch()<<8;			/* getlen() */
+   1fd74:	35 d0       	rcall	.+106    	; 0x1fde0 <getch>
+   1fd76:	90 e0       	ldi	r25, 0x00	; 0
+   1fd78:	d8 2f       	mov	r29, r24
+   1fd7a:	cc 27       	eor	r28, r28
+      length |= getch();
+   1fd7c:	31 d0       	rcall	.+98     	; 0x1fde0 <getch>
+   1fd7e:	90 e0       	ldi	r25, 0x00	; 0
+   1fd80:	c8 2b       	or	r28, r24
+   1fd82:	d9 2b       	or	r29, r25
+      desttype = getch();
+   1fd84:	2d d0       	rcall	.+90     	; 0x1fde0 <getch>
+   1fd86:	18 2f       	mov	r17, r24
 
       verifySpace();
-   1fd56:	31 d0       	rcall	.+98     	; 0x1fdba <verifySpace>
-        __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+   1fd88:	3d d0       	rcall	.+122    	; 0x1fe04 <verifySpace>
+
+static inline void read_mem(uint8_t memtype, uint16_t address, uint16_t length)
+{
+    uint8_t ch;
+
+    switch (memtype) {
+   1fd8a:	15 34       	cpi	r17, 0x45	; 69
+   1fd8c:	49 f4       	brne	.+18     	; 0x1fda0 <main+0x1a0>
+   1fd8e:	86 01       	movw	r16, r12
+   1fd90:	c8 01       	movw	r24, r16
+
+#if defined(SUPPORT_EEPROM) || defined(BIGBOOT)
+    case 'E': // EEPROM
+	do {
+	    putch(eeprom_read_byte((uint8_t *)(address++)));
+   1fd92:	0f 5f       	subi	r16, 0xFF	; 255
+   1fd94:	1f 4f       	sbci	r17, 0xFF	; 255
+   1fd96:	4c d0       	rcall	.+152    	; 0x1fe30 <__eerd_byte_m1280>
+   1fd98:	1b d0       	rcall	.+54     	; 0x1fdd0 <putch>
+	} while (--length);
+   1fd9a:	21 97       	sbiw	r28, 0x01	; 1
+   1fd9c:	c9 f7       	brne	.-14     	; 0x1fd90 <main+0x190>
+   1fd9e:	15 c0       	rjmp	.+42     	; 0x1fdca <main+0x1ca>
+   1fda0:	86 01       	movw	r16, r12
+	    __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #else
-        // read a Flash byte and increment the address
-        __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+	    // read a Flash byte and increment the address
+	    __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #endif
-        putch(ch);
-   1fd58:	f7 01       	movw	r30, r14
-   1fd5a:	87 91       	elpm	r24, Z+
-   1fd5c:	7f 01       	movw	r14, r30
-   1fd5e:	13 d0       	rcall	.+38     	; 0x1fd86 <putch>
-      } while (--length);
-   1fd60:	01 50       	subi	r16, 0x01	; 1
-   1fd62:	d1 f7       	brne	.-12     	; 0x1fd58 <main+0x158>
-   1fd64:	0d c0       	rjmp	.+26     	; 0x1fd80 <main+0x180>
+	    putch(ch);
+   1fda2:	f8 01       	movw	r30, r16
+   1fda4:	87 91       	elpm	r24, Z+
+   1fda6:	8f 01       	movw	r16, r30
+   1fda8:	13 d0       	rcall	.+38     	; 0x1fdd0 <putch>
+	} while (--length);
+   1fdaa:	21 97       	sbiw	r28, 0x01	; 1
+   1fdac:	d1 f7       	brne	.-12     	; 0x1fda2 <main+0x1a2>
+   1fdae:	0d c0       	rjmp	.+26     	; 0x1fdca <main+0x1ca>
+	  
+      read_mem(desttype, address, length);
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-   1fd66:	85 37       	cpi	r24, 0x75	; 117
-   1fd68:	31 f4       	brne	.+12     	; 0x1fd76 <main+0x176>
+   1fdb0:	85 37       	cpi	r24, 0x75	; 117
+   1fdb2:	31 f4       	brne	.+12     	; 0x1fdc0 <main+0x1c0>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-   1fd6a:	27 d0       	rcall	.+78     	; 0x1fdba <verifySpace>
+   1fdb4:	27 d0       	rcall	.+78     	; 0x1fe04 <verifySpace>
       putch(SIGNATURE_0);
-   1fd6c:	8e e1       	ldi	r24, 0x1E	; 30
-   1fd6e:	0b d0       	rcall	.+22     	; 0x1fd86 <putch>
+   1fdb6:	8e e1       	ldi	r24, 0x1E	; 30
+   1fdb8:	0b d0       	rcall	.+22     	; 0x1fdd0 <putch>
       putch(SIGNATURE_1);
-   1fd70:	87 e9       	ldi	r24, 0x97	; 151
-   1fd72:	09 d0       	rcall	.+18     	; 0x1fd86 <putch>
-   1fd74:	7f cf       	rjmp	.-258    	; 0x1fc74 <main+0x74>
+   1fdba:	87 e9       	ldi	r24, 0x97	; 151
+   1fdbc:	09 d0       	rcall	.+18     	; 0x1fdd0 <putch>
+   1fdbe:	5b cf       	rjmp	.-330    	; 0x1fc76 <main+0x76>
       putch(SIGNATURE_2);
     }
     else if (ch == STK_LEAVE_PROGMODE) { /* 'Q' */
-   1fd76:	81 35       	cpi	r24, 0x51	; 81
-   1fd78:	11 f4       	brne	.+4      	; 0x1fd7e <main+0x17e>
+   1fdc0:	81 35       	cpi	r24, 0x51	; 81
+   1fdc2:	11 f4       	brne	.+4      	; 0x1fdc8 <main+0x1c8>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-   1fd7a:	88 e0       	ldi	r24, 0x08	; 8
-   1fd7c:	18 d0       	rcall	.+48     	; 0x1fdae <watchdogConfig>
+   1fdc4:	88 e0       	ldi	r24, 0x08	; 8
+   1fdc6:	18 d0       	rcall	.+48     	; 0x1fdf8 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-   1fd7e:	1d d0       	rcall	.+58     	; 0x1fdba <verifySpace>
+   1fdc8:	1d d0       	rcall	.+58     	; 0x1fe04 <verifySpace>
     }
     putch(STK_OK);
-   1fd80:	80 e1       	ldi	r24, 0x10	; 16
-   1fd82:	01 d0       	rcall	.+2      	; 0x1fd86 <putch>
-   1fd84:	6b cf       	rjmp	.-298    	; 0x1fc5c <main+0x5c>
+   1fdca:	80 e1       	ldi	r24, 0x10	; 16
+   1fdcc:	01 d0       	rcall	.+2      	; 0x1fdd0 <putch>
+   1fdce:	47 cf       	rjmp	.-370    	; 0x1fc5e <main+0x5e>
 
-0001fd86 <putch>:
+0001fdd0 <putch>:
   }
 }
 
 void putch(char ch) {
-   1fd86:	98 2f       	mov	r25, r24
+   1fdd0:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UART_SRA & _BV(UDRE0)));
-   1fd88:	80 91 c0 00 	lds	r24, 0x00C0
-   1fd8c:	85 ff       	sbrs	r24, 5
-   1fd8e:	fc cf       	rjmp	.-8      	; 0x1fd88 <putch+0x2>
+   1fdd2:	80 91 c0 00 	lds	r24, 0x00C0
+   1fdd6:	85 ff       	sbrs	r24, 5
+   1fdd8:	fc cf       	rjmp	.-8      	; 0x1fdd2 <putch+0x2>
   UART_UDR = ch;
-   1fd90:	90 93 c6 00 	sts	0x00C6, r25
+   1fdda:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-   1fd94:	08 95       	ret
+   1fdde:	08 95       	ret
 
-0001fd96 <getch>:
+0001fde0 <getch>:
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UART_SRA & _BV(RXC0)))
-   1fd96:	80 91 c0 00 	lds	r24, 0x00C0
-   1fd9a:	87 ff       	sbrs	r24, 7
-   1fd9c:	fc cf       	rjmp	.-8      	; 0x1fd96 <getch>
+   1fde0:	80 91 c0 00 	lds	r24, 0x00C0
+   1fde4:	87 ff       	sbrs	r24, 7
+   1fde6:	fc cf       	rjmp	.-8      	; 0x1fde0 <getch>
     ;
   if (!(UART_SRA & _BV(FE0))) {
-   1fd9e:	80 91 c0 00 	lds	r24, 0x00C0
-   1fda2:	84 fd       	sbrc	r24, 4
-   1fda4:	01 c0       	rjmp	.+2      	; 0x1fda8 <getch+0x12>
+   1fde8:	80 91 c0 00 	lds	r24, 0x00C0
+   1fdec:	84 fd       	sbrc	r24, 4
+   1fdee:	01 c0       	rjmp	.+2      	; 0x1fdf2 <getch+0x12>
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-   1fda6:	a8 95       	wdr
+   1fdf0:	a8 95       	wdr
        * don't care that an invalid char is returned...)
        */
     watchdogReset();
   }
   
   ch = UART_UDR;
-   1fda8:	80 91 c6 00 	lds	r24, 0x00C6
+   1fdf2:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-   1fdac:	08 95       	ret
+   1fdf6:	08 95       	ret
 
-0001fdae <watchdogConfig>:
+0001fdf8 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-   1fdae:	e0 e6       	ldi	r30, 0x60	; 96
-   1fdb0:	f0 e0       	ldi	r31, 0x00	; 0
-   1fdb2:	98 e1       	ldi	r25, 0x18	; 24
-   1fdb4:	90 83       	st	Z, r25
+   1fdf8:	e0 e6       	ldi	r30, 0x60	; 96
+   1fdfa:	f0 e0       	ldi	r31, 0x00	; 0
+   1fdfc:	98 e1       	ldi	r25, 0x18	; 24
+   1fdfe:	90 83       	st	Z, r25
   WDTCSR = x;
-   1fdb6:	80 83       	st	Z, r24
+   1fe00:	80 83       	st	Z, r24
 }
-   1fdb8:	08 95       	ret
+   1fe02:	08 95       	ret
 
-0001fdba <verifySpace>:
+0001fe04 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) {
-   1fdba:	ed df       	rcall	.-38     	; 0x1fd96 <getch>
-   1fdbc:	80 32       	cpi	r24, 0x20	; 32
-   1fdbe:	19 f0       	breq	.+6      	; 0x1fdc6 <verifySpace+0xc>
+   1fe04:	ed df       	rcall	.-38     	; 0x1fde0 <getch>
+   1fe06:	80 32       	cpi	r24, 0x20	; 32
+   1fe08:	19 f0       	breq	.+6      	; 0x1fe10 <verifySpace+0xc>
     watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
-   1fdc0:	88 e0       	ldi	r24, 0x08	; 8
-   1fdc2:	f5 df       	rcall	.-22     	; 0x1fdae <watchdogConfig>
-   1fdc4:	ff cf       	rjmp	.-2      	; 0x1fdc4 <verifySpace+0xa>
+   1fe0a:	88 e0       	ldi	r24, 0x08	; 8
+   1fe0c:	f5 df       	rcall	.-22     	; 0x1fdf8 <watchdogConfig>
+   1fe0e:	ff cf       	rjmp	.-2      	; 0x1fe0e <verifySpace+0xa>
     while (1)			      // and busy-loop so that WD causes
       ;				      //  a reset and app start.
   }
   putch(STK_INSYNC);
-   1fdc6:	84 e1       	ldi	r24, 0x14	; 20
+   1fe10:	84 e1       	ldi	r24, 0x14	; 20
 }
-   1fdc8:	de cf       	rjmp	.-68     	; 0x1fd86 <putch>
+   1fe12:	de cf       	rjmp	.-68     	; 0x1fdd0 <putch>
 
-0001fdca <getNch>:
+0001fe14 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-   1fdca:	1f 93       	push	r17
-   1fdcc:	18 2f       	mov	r17, r24
+   1fe14:	1f 93       	push	r17
+   1fe16:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-   1fdce:	e3 df       	rcall	.-58     	; 0x1fd96 <getch>
-   1fdd0:	11 50       	subi	r17, 0x01	; 1
-   1fdd2:	e9 f7       	brne	.-6      	; 0x1fdce <getNch+0x4>
+   1fe18:	e3 df       	rcall	.-58     	; 0x1fde0 <getch>
+   1fe1a:	11 50       	subi	r17, 0x01	; 1
+   1fe1c:	e9 f7       	brne	.-6      	; 0x1fe18 <getNch+0x4>
   verifySpace();
-   1fdd4:	f2 df       	rcall	.-28     	; 0x1fdba <verifySpace>
+   1fe1e:	f2 df       	rcall	.-28     	; 0x1fe04 <verifySpace>
 }
-   1fdd6:	1f 91       	pop	r17
-   1fdd8:	08 95       	ret
+   1fe20:	1f 91       	pop	r17
+   1fe22:	08 95       	ret
 
-0001fdda <appStart>:
+0001fe24 <appStart>:
 
 void appStart(uint8_t rstFlags) {
   // save the reset flags in the designated register
   //  This can be saved in a main program by putting code in .init0 (which
   //  executes before normal c init code) to save R2 to a global variable.
   __asm__ __volatile__ ("mov r2, %0\n" :: "r" (rstFlags));
-   1fdda:	28 2e       	mov	r2, r24
+   1fe24:	28 2e       	mov	r2, r24
 
   watchdogConfig(WATCHDOG_OFF);
-   1fddc:	80 e0       	ldi	r24, 0x00	; 0
-   1fdde:	e7 df       	rcall	.-50     	; 0x1fdae <watchdogConfig>
+   1fe26:	80 e0       	ldi	r24, 0x00	; 0
+   1fe28:	e7 df       	rcall	.-50     	; 0x1fdf8 <watchdogConfig>
   __asm__ __volatile__ (
-   1fde0:	ee 27       	eor	r30, r30
-   1fde2:	ff 27       	eor	r31, r31
-   1fde4:	09 94       	ijmp
+   1fe2a:	ee 27       	eor	r30, r30
+   1fe2c:	ff 27       	eor	r31, r31
+   1fe2e:	09 94       	ijmp
+
+0001fe30 <__eerd_byte_m1280>:
+   1fe30:	f9 99       	sbic	0x1f, 1	; 31
+   1fe32:	fe cf       	rjmp	.-4      	; 0x1fe30 <__eerd_byte_m1280>
+   1fe34:	92 bd       	out	0x22, r25	; 34
+   1fe36:	81 bd       	out	0x21, r24	; 33
+   1fe38:	f8 9a       	sbi	0x1f, 0	; 31
+   1fe3a:	99 27       	eor	r25, r25
+   1fe3c:	80 b5       	in	r24, 0x20	; 32
+   1fe3e:	08 95       	ret
+
+0001fe40 <__eewr_byte_m1280>:
+   1fe40:	26 2f       	mov	r18, r22
+
+0001fe42 <__eewr_r18_m1280>:
+   1fe42:	f9 99       	sbic	0x1f, 1	; 31
+   1fe44:	fe cf       	rjmp	.-4      	; 0x1fe42 <__eewr_r18_m1280>
+   1fe46:	1f ba       	out	0x1f, r1	; 31
+   1fe48:	92 bd       	out	0x22, r25	; 34
+   1fe4a:	81 bd       	out	0x21, r24	; 33
+   1fe4c:	20 bd       	out	0x20, r18	; 32
+   1fe4e:	0f b6       	in	r0, 0x3f	; 63
+   1fe50:	f8 94       	cli
+   1fe52:	fa 9a       	sbi	0x1f, 2	; 31
+   1fe54:	f9 9a       	sbi	0x1f, 1	; 31
+   1fe56:	0f be       	out	0x3f, r0	; 63
+   1fe58:	01 96       	adiw	r24, 0x01	; 1
+   1fe5a:	08 95       	ret
diff --git a/optiboot/bootloaders/optiboot/optiboot_atmega328.hex b/optiboot/bootloaders/optiboot/optiboot_atmega328.hex
index 8917cb8..e5105c2 100644
--- a/optiboot/bootloaders/optiboot/optiboot_atmega328.hex
+++ b/optiboot/bootloaders/optiboot/optiboot_atmega328.hex
@@ -1,34 +1,35 @@
-:107E0000112484B714BE81FFE6D085E08093810001
-:107E100082E08093C00088E18093C10086E0809377
-:107E2000C20080E18093C4008EE0BFD0259A86E036
-:107E300020E33CEF91E0309385002093840096BBD3
-:107E4000B09BFECF1D9AA8958150A9F7EE24FF2480
-:107E5000AA24A394B5E0CB2EA1E1BA2EF3E0DF2E45
-:107E600098D0813461F495D0082FA5D0023829F13B
-:107E7000013811F485E001C083E083D07FC08234F3
-:107E800011F484E103C0853419F485E09CD076C0F8
-:107E9000853579F47ED0E82EFF247BD0082F10E0C2
-:107EA000102F00270E291F29000F111F84D07801E1
-:107EB00065C0863521F484E086D080E0DECF84364C
-:107EC00009F040C066D065D0082F63D080E0E81686
-:107ED00080E7F80618F4F701D7BEE895C0E0D1E0D6
-:107EE00058D089930C17E1F7F0E0EF16F0E7FF06A2
-:107EF00018F0F701D7BEE8955ED007B600FCFDCFBD
-:107F0000A701A0E0B1E02C9130E011968C9111977F
-:107F100090E0982F8827822B932B1296FA010C0160
-:107F2000A7BEE89511244E5F5F4FF1E0A038BF0770
-:107F300051F7F701C7BEE89507B600FCFDCFB7BE05
-:107F4000E8951CC0843761F424D023D0082F21D0B9
-:107F500032D0F70185917F0114D00150D1F70EC0C6
-:107F6000853739F428D08EE10CD085E90AD08FE02E
-:107F700084CF813511F488E018D01DD080E101D084
-:107F80006FCF982F8091C00085FFFCCF9093C600E3
-:107F900008958091C00087FFFCCF8091C00084FDD0
-:107FA00001C0A8958091C6000895E0E6F0E098E150
-:107FB000908380830895EDDF803219F088E0F5DF4B
-:107FC000FFCF84E1DECF1F93182FE3DF1150E9F7D5
-:107FD000F2DF1F910895282E80E0E7DFEE27FF27CC
-:027FE000099402
-:027FFE0000057C
+:107E0000112484B714BE81FD01C0EDD085E08093BC
+:107E1000810082E08093C00088E18093C10086E009
+:107E20008093C20080E18093C4008EE0C6D0259A82
+:107E300086E020E33CEF91E03093850020938400BE
+:107E400096BBB09BFECF1D9AA8958150A9F7AA2496
+:107E5000BB2433E0832E7724739425E0922E91E1A6
+:107E6000C92E9FD0813461F49CD0182FACD0123829
+:107E700029F1113811F486E001C083E08AD086C070
+:107E8000823411F484E103C0853419F485E0A3D071
+:107E90007DC0853579F485D0E82EFF2482D0082F67
+:107EA00010E0102F00270E291F29000F111F8BD063
+:107EB00058016CC0863521F484E08DD080E0DECF9F
+:107EC000843609F041C06DD090E0182F002769D0AA
+:107ED00090E0082B192B65D0D82EE801E12CF1E0B9
+:107EE000FF2E5FD0F70181937F012197D1F76BD0EF
+:107EF000F5E4DF1609F4FFCFF50187BEE89507B674
+:107F000000FCFDCFB501A801A0E0B1E02C9130E06C
+:107F100011968C91119790E0982F8827822B932BA4
+:107F20001296FB010C0177BEE89511246E5F7F4F1E
+:107F30004250504059F7F50197BEE89507B600FC4E
+:107F4000FDCFC7BEE89522C0843791F42AD090E0D7
+:107F5000D82FCC2726D090E0C82BD92B22D033D0D5
+:107F60008501F80185918F0114D02197D1F70EC0BA
+:107F7000853739F428D08EE10CD085E90AD08FE01E
+:107F80007DCF813511F488E018D01DD080E101D07B
+:107F900068CF982F8091C00085FFFCCF9093C600DA
+:107FA00008958091C00087FFFCCF8091C00084FDC0
+:107FB00001C0A8958091C6000895E0E6F0E098E140
+:107FC000908380830895EDDF803219F088E0F5DF3B
+:107FD000FFCF84E1DECF1F93182FE3DF1150E9F7C5
+:107FE000F2DF1F910895282E80E0E7DFEE27FF27BC
+:027FF0000994F2
+:027FFE0000067B
 :0400000300007E007B
 :00000001FF
diff --git a/optiboot/bootloaders/optiboot/optiboot_atmega328.lst b/optiboot/bootloaders/optiboot/optiboot_atmega328.lst
index 87e56a9..72c7735 100644
--- a/optiboot/bootloaders/optiboot/optiboot_atmega328.lst
+++ b/optiboot/bootloaders/optiboot/optiboot_atmega328.lst
@@ -3,36 +3,36 @@ optiboot_atmega328.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001e2  00007e00  00007e00  00000054  2**1
+  0 .text         000001f2  00007e00  00007e00  00000074  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00007ffe  00007ffe  00000236  2**0
-                  CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000238  2**0
+  1 .version      00000002  00007ffe  00007ffe  00000266  2**0
+                  CONTENTS, ALLOC, LOAD, DATA
+  2 .debug_aranges 00000028  00000000  00000000  00000268  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000005f  00000000  00000000  00000260  2**0
+  3 .debug_pubnames 00000074  00000000  00000000  00000290  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000294  00000000  00000000  000002bf  2**0
+  4 .debug_info   000003db  00000000  00000000  00000304  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000016b  00000000  00000000  00000553  2**0
+  5 .debug_abbrev 000001ea  00000000  00000000  000006df  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000421  00000000  00000000  000006be  2**0
+  6 .debug_line   0000042b  00000000  00000000  000008c9  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000080  00000000  00000000  00000ae0  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000cf4  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000131  00000000  00000000  00000b60  2**0
+  8 .debug_str    00000172  00000000  00000000  00000d74  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    00000266  00000000  00000000  00000c91  2**0
+  9 .debug_loc    000002d7  00000000  00000000  00000ee6  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000078  00000000  00000000  00000ef7  2**0
+ 10 .debug_ranges 000000b8  00000000  00000000  000011bd  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00007e00 <main>:
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
     7e00:	11 24       	eor	r1, r1
@@ -46,541 +46,555 @@ int main(void) {
   MCUSR = 0;
     7e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart(ch);
-    7e06:	81 ff       	sbrs	r24, 1
-    7e08:	e6 d0       	rcall	.+460    	; 0x7fd6 <appStart>
+    7e06:	81 fd       	sbrc	r24, 1
+    7e08:	01 c0       	rjmp	.+2      	; 0x7e0c <main+0xc>
+    7e0a:	ed d0       	rcall	.+474    	; 0x7fe6 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-    7e0a:	85 e0       	ldi	r24, 0x05	; 5
-    7e0c:	80 93 81 00 	sts	0x0081, r24
+    7e0c:	85 e0       	ldi	r24, 0x05	; 5
+    7e0e:	80 93 81 00 	sts	0x0081, r24
   UCSRA = _BV(U2X); //Double speed mode USART
   UCSRB = _BV(RXEN) | _BV(TXEN);  // enable Rx & Tx
   UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0);  // config USART; 8N1
   UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
 #else
   UART_SRA = _BV(U2X0); //Double speed mode USART0
-    7e10:	82 e0       	ldi	r24, 0x02	; 2
-    7e12:	80 93 c0 00 	sts	0x00C0, r24
+    7e12:	82 e0       	ldi	r24, 0x02	; 2
+    7e14:	80 93 c0 00 	sts	0x00C0, r24
   UART_SRB = _BV(RXEN0) | _BV(TXEN0);
-    7e16:	88 e1       	ldi	r24, 0x18	; 24
-    7e18:	80 93 c1 00 	sts	0x00C1, r24
+    7e18:	88 e1       	ldi	r24, 0x18	; 24
+    7e1a:	80 93 c1 00 	sts	0x00C1, r24
   UART_SRC = _BV(UCSZ00) | _BV(UCSZ01);
-    7e1c:	86 e0       	ldi	r24, 0x06	; 6
-    7e1e:	80 93 c2 00 	sts	0x00C2, r24
+    7e1e:	86 e0       	ldi	r24, 0x06	; 6
+    7e20:	80 93 c2 00 	sts	0x00C2, r24
   UART_SRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    7e22:	80 e1       	ldi	r24, 0x10	; 16
-    7e24:	80 93 c4 00 	sts	0x00C4, r24
+    7e24:	80 e1       	ldi	r24, 0x10	; 16
+    7e26:	80 93 c4 00 	sts	0x00C4, r24
 #endif
 #endif
 
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
-    7e28:	8e e0       	ldi	r24, 0x0E	; 14
-    7e2a:	bf d0       	rcall	.+382    	; 0x7faa <watchdogConfig>
+    7e2a:	8e e0       	ldi	r24, 0x0E	; 14
+    7e2c:	c6 d0       	rcall	.+396    	; 0x7fba <watchdogConfig>
 
 #if (LED_START_FLASHES > 0) || defined(LED_DATA_FLASH)
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    7e2c:	25 9a       	sbi	0x04, 5	; 4
-    7e2e:	86 e0       	ldi	r24, 0x06	; 6
+    7e2e:	25 9a       	sbi	0x04, 5	; 4
+    7e30:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    7e30:	20 e3       	ldi	r18, 0x30	; 48
-    7e32:	3c ef       	ldi	r19, 0xFC	; 252
+    7e32:	20 e3       	ldi	r18, 0x30	; 48
+    7e34:	3c ef       	ldi	r19, 0xFC	; 252
     TIFR1 = _BV(TOV1);
-    7e34:	91 e0       	ldi	r25, 0x01	; 1
+    7e36:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    7e36:	30 93 85 00 	sts	0x0085, r19
-    7e3a:	20 93 84 00 	sts	0x0084, r18
+    7e38:	30 93 85 00 	sts	0x0085, r19
+    7e3c:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    7e3e:	96 bb       	out	0x16, r25	; 22
+    7e40:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    7e40:	b0 9b       	sbis	0x16, 0	; 22
-    7e42:	fe cf       	rjmp	.-4      	; 0x7e40 <main+0x40>
+    7e42:	b0 9b       	sbis	0x16, 0	; 22
+    7e44:	fe cf       	rjmp	.-4      	; 0x7e42 <main+0x42>
 #if defined(__AVR_ATmega8__)  || defined (__AVR_ATmega32__)
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
-    7e44:	1d 9a       	sbi	0x03, 5	; 3
+    7e46:	1d 9a       	sbi	0x03, 5	; 3
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    7e46:	a8 95       	wdr
+    7e48:	a8 95       	wdr
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
 #endif
     watchdogReset();
   } while (--count);
-    7e48:	81 50       	subi	r24, 0x01	; 1
-    7e4a:	a9 f7       	brne	.-22     	; 0x7e36 <main+0x36>
-    7e4c:	ee 24       	eor	r14, r14
-    7e4e:	ff 24       	eor	r15, r15
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-        a |= (*bufPtr++) << 8;
-        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    7e50:	aa 24       	eor	r10, r10
-    7e52:	a3 94       	inc	r10
-        addrPtr += 2;
-      } while (--ch);
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    7e54:	b5 e0       	ldi	r27, 0x05	; 5
-    7e56:	cb 2e       	mov	r12, r27
-      boot_spm_busy_wait();
+    7e4a:	81 50       	subi	r24, 0x01	; 1
+    7e4c:	a9 f7       	brne	.-22     	; 0x7e38 <main+0x38>
+    7e4e:	aa 24       	eor	r10, r10
+    7e50:	bb 24       	eor	r11, r11
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    7e52:	33 e0       	ldi	r19, 0x03	; 3
+    7e54:	83 2e       	mov	r8, r19
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    7e56:	77 24       	eor	r7, r7
+    7e58:	73 94       	inc	r7
+	    } while (len -= 2);
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    7e5a:	25 e0       	ldi	r18, 0x05	; 5
+    7e5c:	92 2e       	mov	r9, r18
+	    boot_spm_busy_wait();
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    7e58:	a1 e1       	ldi	r26, 0x11	; 17
-    7e5a:	ba 2e       	mov	r11, r26
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    7e5c:	f3 e0       	ldi	r31, 0x03	; 3
-    7e5e:	df 2e       	mov	r13, r31
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    7e5e:	91 e1       	ldi	r25, 0x11	; 17
+    7e60:	c9 2e       	mov	r12, r25
 #endif
 
-  /* Forever loop */
+  /* Forever loop: exits by causing WDT reset */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    7e60:	98 d0       	rcall	.+304    	; 0x7f92 <getch>
+    7e62:	9f d0       	rcall	.+318    	; 0x7fa2 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    7e62:	81 34       	cpi	r24, 0x41	; 65
-    7e64:	61 f4       	brne	.+24     	; 0x7e7e <main+0x7e>
+    7e64:	81 34       	cpi	r24, 0x41	; 65
+    7e66:	61 f4       	brne	.+24     	; 0x7e80 <main+0x80>
       unsigned char which = getch();
-    7e66:	95 d0       	rcall	.+298    	; 0x7f92 <getch>
-    7e68:	08 2f       	mov	r16, r24
+    7e68:	9c d0       	rcall	.+312    	; 0x7fa2 <getch>
+    7e6a:	18 2f       	mov	r17, r24
       verifySpace();
-    7e6a:	a5 d0       	rcall	.+330    	; 0x7fb6 <verifySpace>
+    7e6c:	ac d0       	rcall	.+344    	; 0x7fc6 <verifySpace>
       if (which == 0x82) {
-    7e6c:	02 38       	cpi	r16, 0x82	; 130
-    7e6e:	29 f1       	breq	.+74     	; 0x7eba <main+0xba>
+    7e6e:	12 38       	cpi	r17, 0x82	; 130
+    7e70:	29 f1       	breq	.+74     	; 0x7ebc <main+0xbc>
 	/*
 	 * Send optiboot version as "minor SW version"
 	 */
 	putch(OPTIBOOT_MINVER);
       } else if (which == 0x81) {
-    7e70:	01 38       	cpi	r16, 0x81	; 129
-    7e72:	11 f4       	brne	.+4      	; 0x7e78 <main+0x78>
+    7e72:	11 38       	cpi	r17, 0x81	; 129
+    7e74:	11 f4       	brne	.+4      	; 0x7e7a <main+0x7a>
 	  putch(OPTIBOOT_MAJVER);
-    7e74:	85 e0       	ldi	r24, 0x05	; 5
-    7e76:	01 c0       	rjmp	.+2      	; 0x7e7a <main+0x7a>
+    7e76:	86 e0       	ldi	r24, 0x06	; 6
+    7e78:	01 c0       	rjmp	.+2      	; 0x7e7c <main+0x7c>
       } else {
 	/*
 	 * GET PARAMETER returns a generic 0x03 reply for
          * other parameters - enough to keep Avrdude happy
 	 */
 	putch(0x03);
-    7e78:	83 e0       	ldi	r24, 0x03	; 3
-    7e7a:	83 d0       	rcall	.+262    	; 0x7f82 <putch>
-    7e7c:	7f c0       	rjmp	.+254    	; 0x7f7c <main+0x17c>
+    7e7a:	83 e0       	ldi	r24, 0x03	; 3
+    7e7c:	8a d0       	rcall	.+276    	; 0x7f92 <putch>
+    7e7e:	86 c0       	rjmp	.+268    	; 0x7f8c <main+0x18c>
       }
     }
     else if(ch == STK_SET_DEVICE) {
-    7e7e:	82 34       	cpi	r24, 0x42	; 66
-    7e80:	11 f4       	brne	.+4      	; 0x7e86 <main+0x86>
+    7e80:	82 34       	cpi	r24, 0x42	; 66
+    7e82:	11 f4       	brne	.+4      	; 0x7e88 <main+0x88>
       // SET DEVICE is ignored
       getNch(20);
-    7e82:	84 e1       	ldi	r24, 0x14	; 20
-    7e84:	03 c0       	rjmp	.+6      	; 0x7e8c <main+0x8c>
+    7e84:	84 e1       	ldi	r24, 0x14	; 20
+    7e86:	03 c0       	rjmp	.+6      	; 0x7e8e <main+0x8e>
     }
     else if(ch == STK_SET_DEVICE_EXT) {
-    7e86:	85 34       	cpi	r24, 0x45	; 69
-    7e88:	19 f4       	brne	.+6      	; 0x7e90 <main+0x90>
+    7e88:	85 34       	cpi	r24, 0x45	; 69
+    7e8a:	19 f4       	brne	.+6      	; 0x7e92 <main+0x92>
       // SET DEVICE EXT is ignored
       getNch(5);
-    7e8a:	85 e0       	ldi	r24, 0x05	; 5
-    7e8c:	9c d0       	rcall	.+312    	; 0x7fc6 <getNch>
-    7e8e:	76 c0       	rjmp	.+236    	; 0x7f7c <main+0x17c>
+    7e8c:	85 e0       	ldi	r24, 0x05	; 5
+    7e8e:	a3 d0       	rcall	.+326    	; 0x7fd6 <getNch>
+    7e90:	7d c0       	rjmp	.+250    	; 0x7f8c <main+0x18c>
     }
     else if(ch == STK_LOAD_ADDRESS) {
-    7e90:	85 35       	cpi	r24, 0x55	; 85
-    7e92:	79 f4       	brne	.+30     	; 0x7eb2 <main+0xb2>
+    7e92:	85 35       	cpi	r24, 0x55	; 85
+    7e94:	79 f4       	brne	.+30     	; 0x7eb4 <main+0xb4>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    7e94:	7e d0       	rcall	.+252    	; 0x7f92 <getch>
+    7e96:	85 d0       	rcall	.+266    	; 0x7fa2 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-    7e96:	e8 2e       	mov	r14, r24
-    7e98:	ff 24       	eor	r15, r15
-    7e9a:	7b d0       	rcall	.+246    	; 0x7f92 <getch>
-    7e9c:	08 2f       	mov	r16, r24
-    7e9e:	10 e0       	ldi	r17, 0x00	; 0
-    7ea0:	10 2f       	mov	r17, r16
-    7ea2:	00 27       	eor	r16, r16
-    7ea4:	0e 29       	or	r16, r14
-    7ea6:	1f 29       	or	r17, r15
+    7e98:	e8 2e       	mov	r14, r24
+    7e9a:	ff 24       	eor	r15, r15
+    7e9c:	82 d0       	rcall	.+260    	; 0x7fa2 <getch>
+    7e9e:	08 2f       	mov	r16, r24
+    7ea0:	10 e0       	ldi	r17, 0x00	; 0
+    7ea2:	10 2f       	mov	r17, r16
+    7ea4:	00 27       	eor	r16, r16
+    7ea6:	0e 29       	or	r16, r14
+    7ea8:	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
-    7ea8:	00 0f       	add	r16, r16
-    7eaa:	11 1f       	adc	r17, r17
+    7eaa:	00 0f       	add	r16, r16
+    7eac:	11 1f       	adc	r17, r17
       address = newAddress;
       verifySpace();
-    7eac:	84 d0       	rcall	.+264    	; 0x7fb6 <verifySpace>
-    7eae:	78 01       	movw	r14, r16
-    7eb0:	65 c0       	rjmp	.+202    	; 0x7f7c <main+0x17c>
+    7eae:	8b d0       	rcall	.+278    	; 0x7fc6 <verifySpace>
+    7eb0:	58 01       	movw	r10, r16
+    7eb2:	6c c0       	rjmp	.+216    	; 0x7f8c <main+0x18c>
     }
     else if(ch == STK_UNIVERSAL) {
-    7eb2:	86 35       	cpi	r24, 0x56	; 86
-    7eb4:	21 f4       	brne	.+8      	; 0x7ebe <main+0xbe>
+    7eb4:	86 35       	cpi	r24, 0x56	; 86
+    7eb6:	21 f4       	brne	.+8      	; 0x7ec0 <main+0xc0>
       // UNIVERSAL command is ignored
       getNch(4);
-    7eb6:	84 e0       	ldi	r24, 0x04	; 4
-    7eb8:	86 d0       	rcall	.+268    	; 0x7fc6 <getNch>
+    7eb8:	84 e0       	ldi	r24, 0x04	; 4
+    7eba:	8d d0       	rcall	.+282    	; 0x7fd6 <getNch>
       putch(0x00);
-    7eba:	80 e0       	ldi	r24, 0x00	; 0
-    7ebc:	de cf       	rjmp	.-68     	; 0x7e7a <main+0x7a>
+    7ebc:	80 e0       	ldi	r24, 0x00	; 0
+    7ebe:	de cf       	rjmp	.-68     	; 0x7e7c <main+0x7c>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    7ebe:	84 36       	cpi	r24, 0x64	; 100
-    7ec0:	09 f0       	breq	.+2      	; 0x7ec4 <main+0xc4>
-    7ec2:	40 c0       	rjmp	.+128    	; 0x7f44 <main+0x144>
+    7ec0:	84 36       	cpi	r24, 0x64	; 100
+    7ec2:	09 f0       	breq	.+2      	; 0x7ec6 <main+0xc6>
+    7ec4:	41 c0       	rjmp	.+130    	; 0x7f48 <main+0x148>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
+      uint8_t desttype;
       uint8_t *bufPtr;
-      uint16_t addrPtr;
+      uint16_t savelength;
 
-      getch();			/* getlen() */
-    7ec4:	66 d0       	rcall	.+204    	; 0x7f92 <getch>
-      length = getch();
-    7ec6:	65 d0       	rcall	.+202    	; 0x7f92 <getch>
-    7ec8:	08 2f       	mov	r16, r24
-      getch();
-    7eca:	63 d0       	rcall	.+198    	; 0x7f92 <getch>
+      length = getch()<<8;			/* getlen() */
+    7ec6:	6d d0       	rcall	.+218    	; 0x7fa2 <getch>
+    7ec8:	90 e0       	ldi	r25, 0x00	; 0
+    7eca:	18 2f       	mov	r17, r24
+    7ecc:	00 27       	eor	r16, r16
+      length |= getch();
+    7ece:	69 d0       	rcall	.+210    	; 0x7fa2 <getch>
+    7ed0:	90 e0       	ldi	r25, 0x00	; 0
+    7ed2:	08 2b       	or	r16, r24
+    7ed4:	19 2b       	or	r17, r25
+      savelength = length;
+      desttype = getch();
+    7ed6:	65 d0       	rcall	.+202    	; 0x7fa2 <getch>
+    7ed8:	d8 2e       	mov	r13, r24
+    7eda:	e8 01       	movw	r28, r16
+    7edc:	e1 2c       	mov	r14, r1
+    7ede:	f1 e0       	ldi	r31, 0x01	; 1
+    7ee0:	ff 2e       	mov	r15, r31
 
-      // If we are in RWW section, immediately start page erase
-      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    7ecc:	80 e0       	ldi	r24, 0x00	; 0
-    7ece:	e8 16       	cp	r14, r24
-    7ed0:	80 e7       	ldi	r24, 0x70	; 112
-    7ed2:	f8 06       	cpc	r15, r24
-    7ed4:	18 f4       	brcc	.+6      	; 0x7edc <main+0xdc>
-    7ed6:	f7 01       	movw	r30, r14
-    7ed8:	d7 be       	out	0x37, r13	; 55
-    7eda:	e8 95       	spm
-    7edc:	c0 e0       	ldi	r28, 0x00	; 0
-    7ede:	d1 e0       	ldi	r29, 0x01	; 1
-
-      // While that is going on, read in page contents
+      // read a page worth of contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    7ee0:	58 d0       	rcall	.+176    	; 0x7f92 <getch>
-    7ee2:	89 93       	st	Y+, r24
+    7ee2:	5f d0       	rcall	.+190    	; 0x7fa2 <getch>
+    7ee4:	f7 01       	movw	r30, r14
+    7ee6:	81 93       	st	Z+, r24
+    7ee8:	7f 01       	movw	r14, r30
       while (--length);
-    7ee4:	0c 17       	cp	r16, r28
-    7ee6:	e1 f7       	brne	.-8      	; 0x7ee0 <main+0xe0>
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    7ee8:	f0 e0       	ldi	r31, 0x00	; 0
-    7eea:	ef 16       	cp	r14, r31
-    7eec:	f0 e7       	ldi	r31, 0x70	; 112
-    7eee:	ff 06       	cpc	r15, r31
-    7ef0:	18 f0       	brcs	.+6      	; 0x7ef8 <main+0xf8>
-    7ef2:	f7 01       	movw	r30, r14
-    7ef4:	d7 be       	out	0x37, r13	; 55
-    7ef6:	e8 95       	spm
+    7eea:	21 97       	sbiw	r28, 0x01	; 1
+    7eec:	d1 f7       	brne	.-12     	; 0x7ee2 <main+0xe2>
 
       // Read command terminator, start reply
       verifySpace();
-    7ef8:	5e d0       	rcall	.+188    	; 0x7fb6 <verifySpace>
-
-      // If only a partial page is to be programmed, the erase might not be complete.
-      // So check that here
-      boot_spm_busy_wait();
-    7efa:	07 b6       	in	r0, 0x37	; 55
-    7efc:	00 fc       	sbrc	r0, 0
-    7efe:	fd cf       	rjmp	.-6      	; 0x7efa <main+0xfa>
-    7f00:	a7 01       	movw	r20, r14
-    7f02:	a0 e0       	ldi	r26, 0x00	; 0
-    7f04:	b1 e0       	ldi	r27, 0x01	; 1
-      bufPtr = buff;
-      addrPtr = (uint16_t)(void*)address;
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-    7f06:	2c 91       	ld	r18, X
-    7f08:	30 e0       	ldi	r19, 0x00	; 0
-        a |= (*bufPtr++) << 8;
-    7f0a:	11 96       	adiw	r26, 0x01	; 1
-    7f0c:	8c 91       	ld	r24, X
-    7f0e:	11 97       	sbiw	r26, 0x01	; 1
-    7f10:	90 e0       	ldi	r25, 0x00	; 0
-    7f12:	98 2f       	mov	r25, r24
-    7f14:	88 27       	eor	r24, r24
-    7f16:	82 2b       	or	r24, r18
-    7f18:	93 2b       	or	r25, r19
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+    7eee:	6b d0       	rcall	.+214    	; 0x7fc6 <verifySpace>
+ * void writebuffer(memtype, buffer, address, length)
+ */
+static inline void writebuffer(int8_t memtype, uint8_t *mybuff,
+			       uint16_t address, uint16_t len)
+{
+    switch (memtype) {
+    7ef0:	f5 e4       	ldi	r31, 0x45	; 69
+    7ef2:	df 16       	cp	r13, r31
+    7ef4:	09 f4       	brne	.+2      	; 0x7ef8 <main+0xf8>
+    7ef6:	ff cf       	rjmp	.-2      	; 0x7ef6 <main+0xf6>
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    7ef8:	f5 01       	movw	r30, r10
+    7efa:	87 be       	out	0x37, r8	; 55
+    7efc:	e8 95       	spm
+	    boot_spm_busy_wait();
+    7efe:	07 b6       	in	r0, 0x37	; 55
+    7f00:	00 fc       	sbrc	r0, 0
+    7f02:	fd cf       	rjmp	.-6      	; 0x7efe <main+0xfe>
+    7f04:	b5 01       	movw	r22, r10
+    7f06:	a8 01       	movw	r20, r16
+    7f08:	a0 e0       	ldi	r26, 0x00	; 0
+    7f0a:	b1 e0       	ldi	r27, 0x01	; 1
+	    /*
+	     * Copy data from the buffer into the flash write buffer.
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+    7f0c:	2c 91       	ld	r18, X
+    7f0e:	30 e0       	ldi	r19, 0x00	; 0
+		a |= (*bufPtr++) << 8;
+    7f10:	11 96       	adiw	r26, 0x01	; 1
+    7f12:	8c 91       	ld	r24, X
+    7f14:	11 97       	sbiw	r26, 0x01	; 1
+    7f16:	90 e0       	ldi	r25, 0x00	; 0
+    7f18:	98 2f       	mov	r25, r24
+    7f1a:	88 27       	eor	r24, r24
+    7f1c:	82 2b       	or	r24, r18
+    7f1e:	93 2b       	or	r25, r19
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
-    7f1a:	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);
-    7f1c:	fa 01       	movw	r30, r20
-    7f1e:	0c 01       	movw	r0, r24
-    7f20:	a7 be       	out	0x37, r10	; 55
-    7f22:	e8 95       	spm
-    7f24:	11 24       	eor	r1, r1
-        addrPtr += 2;
-    7f26:	4e 5f       	subi	r20, 0xFE	; 254
-    7f28:	5f 4f       	sbci	r21, 0xFF	; 255
-      } while (--ch);
-    7f2a:	f1 e0       	ldi	r31, 0x01	; 1
-    7f2c:	a0 38       	cpi	r26, 0x80	; 128
-    7f2e:	bf 07       	cpc	r27, r31
-    7f30:	51 f7       	brne	.-44     	; 0x7f06 <main+0x106>
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    7f32:	f7 01       	movw	r30, r14
-    7f34:	c7 be       	out	0x37, r12	; 55
-    7f36:	e8 95       	spm
-      boot_spm_busy_wait();
-    7f38:	07 b6       	in	r0, 0x37	; 55
-    7f3a:	00 fc       	sbrc	r0, 0
-    7f3c:	fd cf       	rjmp	.-6      	; 0x7f38 <main+0x138>
+    7f20:	12 96       	adiw	r26, 0x02	; 2
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    7f22:	fb 01       	movw	r30, r22
+    7f24:	0c 01       	movw	r0, r24
+    7f26:	77 be       	out	0x37, r7	; 55
+    7f28:	e8 95       	spm
+    7f2a:	11 24       	eor	r1, r1
+		addrPtr += 2;
+    7f2c:	6e 5f       	subi	r22, 0xFE	; 254
+    7f2e:	7f 4f       	sbci	r23, 0xFF	; 255
+	    } while (len -= 2);
+    7f30:	42 50       	subi	r20, 0x02	; 2
+    7f32:	50 40       	sbci	r21, 0x00	; 0
+    7f34:	59 f7       	brne	.-42     	; 0x7f0c <main+0x10c>
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    7f36:	f5 01       	movw	r30, r10
+    7f38:	97 be       	out	0x37, r9	; 55
+    7f3a:	e8 95       	spm
+	    boot_spm_busy_wait();
+    7f3c:	07 b6       	in	r0, 0x37	; 55
+    7f3e:	00 fc       	sbrc	r0, 0
+    7f40:	fd cf       	rjmp	.-6      	; 0x7f3c <main+0x13c>
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    7f3e:	b7 be       	out	0x37, r11	; 55
-    7f40:	e8 95       	spm
-    7f42:	1c c0       	rjmp	.+56     	; 0x7f7c <main+0x17c>
-#endif
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    7f42:	c7 be       	out	0x37, r12	; 55
+    7f44:	e8 95       	spm
+    7f46:	22 c0       	rjmp	.+68     	; 0x7f8c <main+0x18c>
+      writebuffer(desttype, buff, address, savelength);
+
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    7f44:	84 37       	cpi	r24, 0x74	; 116
-    7f46:	61 f4       	brne	.+24     	; 0x7f60 <main+0x160>
-      // READ PAGE - we only read flash
-      getch();			/* getlen() */
-    7f48:	24 d0       	rcall	.+72     	; 0x7f92 <getch>
-      length = getch();
-    7f4a:	23 d0       	rcall	.+70     	; 0x7f92 <getch>
-    7f4c:	08 2f       	mov	r16, r24
-      getch();
-    7f4e:	21 d0       	rcall	.+66     	; 0x7f92 <getch>
+    7f48:	84 37       	cpi	r24, 0x74	; 116
+    7f4a:	91 f4       	brne	.+36     	; 0x7f70 <main+0x170>
+      uint8_t desttype;
+      length = getch()<<8;			/* getlen() */
+    7f4c:	2a d0       	rcall	.+84     	; 0x7fa2 <getch>
+    7f4e:	90 e0       	ldi	r25, 0x00	; 0
+    7f50:	d8 2f       	mov	r29, r24
+    7f52:	cc 27       	eor	r28, r28
+      length |= getch();
+    7f54:	26 d0       	rcall	.+76     	; 0x7fa2 <getch>
+    7f56:	90 e0       	ldi	r25, 0x00	; 0
+    7f58:	c8 2b       	or	r28, r24
+    7f5a:	d9 2b       	or	r29, r25
+      desttype = getch();
+    7f5c:	22 d0       	rcall	.+68     	; 0x7fa2 <getch>
 
       verifySpace();
-    7f50:	32 d0       	rcall	.+100    	; 0x7fb6 <verifySpace>
-        __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+    7f5e:	33 d0       	rcall	.+102    	; 0x7fc6 <verifySpace>
+    7f60:	85 01       	movw	r16, r10
+	    __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #else
-        // read a Flash byte and increment the address
-        __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+	    // read a Flash byte and increment the address
+	    __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #endif
-        putch(ch);
-    7f52:	f7 01       	movw	r30, r14
-    7f54:	85 91       	lpm	r24, Z+
-    7f56:	7f 01       	movw	r14, r30
-    7f58:	14 d0       	rcall	.+40     	; 0x7f82 <putch>
-      } while (--length);
-    7f5a:	01 50       	subi	r16, 0x01	; 1
-    7f5c:	d1 f7       	brne	.-12     	; 0x7f52 <main+0x152>
-    7f5e:	0e c0       	rjmp	.+28     	; 0x7f7c <main+0x17c>
+	    putch(ch);
+    7f62:	f8 01       	movw	r30, r16
+    7f64:	85 91       	lpm	r24, Z+
+    7f66:	8f 01       	movw	r16, r30
+    7f68:	14 d0       	rcall	.+40     	; 0x7f92 <putch>
+	} while (--length);
+    7f6a:	21 97       	sbiw	r28, 0x01	; 1
+    7f6c:	d1 f7       	brne	.-12     	; 0x7f62 <main+0x162>
+    7f6e:	0e c0       	rjmp	.+28     	; 0x7f8c <main+0x18c>
+	  
+      read_mem(desttype, address, length);
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    7f60:	85 37       	cpi	r24, 0x75	; 117
-    7f62:	39 f4       	brne	.+14     	; 0x7f72 <main+0x172>
+    7f70:	85 37       	cpi	r24, 0x75	; 117
+    7f72:	39 f4       	brne	.+14     	; 0x7f82 <main+0x182>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    7f64:	28 d0       	rcall	.+80     	; 0x7fb6 <verifySpace>
+    7f74:	28 d0       	rcall	.+80     	; 0x7fc6 <verifySpace>
       putch(SIGNATURE_0);
-    7f66:	8e e1       	ldi	r24, 0x1E	; 30
-    7f68:	0c d0       	rcall	.+24     	; 0x7f82 <putch>
+    7f76:	8e e1       	ldi	r24, 0x1E	; 30
+    7f78:	0c d0       	rcall	.+24     	; 0x7f92 <putch>
       putch(SIGNATURE_1);
-    7f6a:	85 e9       	ldi	r24, 0x95	; 149
-    7f6c:	0a d0       	rcall	.+20     	; 0x7f82 <putch>
+    7f7a:	85 e9       	ldi	r24, 0x95	; 149
+    7f7c:	0a d0       	rcall	.+20     	; 0x7f92 <putch>
       putch(SIGNATURE_2);
-    7f6e:	8f e0       	ldi	r24, 0x0F	; 15
-    7f70:	84 cf       	rjmp	.-248    	; 0x7e7a <main+0x7a>
+    7f7e:	8f e0       	ldi	r24, 0x0F	; 15
+    7f80:	7d cf       	rjmp	.-262    	; 0x7e7c <main+0x7c>
     }
     else if (ch == STK_LEAVE_PROGMODE) { /* 'Q' */
-    7f72:	81 35       	cpi	r24, 0x51	; 81
-    7f74:	11 f4       	brne	.+4      	; 0x7f7a <main+0x17a>
+    7f82:	81 35       	cpi	r24, 0x51	; 81
+    7f84:	11 f4       	brne	.+4      	; 0x7f8a <main+0x18a>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    7f76:	88 e0       	ldi	r24, 0x08	; 8
-    7f78:	18 d0       	rcall	.+48     	; 0x7faa <watchdogConfig>
+    7f86:	88 e0       	ldi	r24, 0x08	; 8
+    7f88:	18 d0       	rcall	.+48     	; 0x7fba <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    7f7a:	1d d0       	rcall	.+58     	; 0x7fb6 <verifySpace>
+    7f8a:	1d d0       	rcall	.+58     	; 0x7fc6 <verifySpace>
     }
     putch(STK_OK);
-    7f7c:	80 e1       	ldi	r24, 0x10	; 16
-    7f7e:	01 d0       	rcall	.+2      	; 0x7f82 <putch>
-    7f80:	6f cf       	rjmp	.-290    	; 0x7e60 <main+0x60>
+    7f8c:	80 e1       	ldi	r24, 0x10	; 16
+    7f8e:	01 d0       	rcall	.+2      	; 0x7f92 <putch>
+    7f90:	68 cf       	rjmp	.-304    	; 0x7e62 <main+0x62>
 
-00007f82 <putch>:
+00007f92 <putch>:
   }
 }
 
 void putch(char ch) {
-    7f82:	98 2f       	mov	r25, r24
+    7f92:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UART_SRA & _BV(UDRE0)));
-    7f84:	80 91 c0 00 	lds	r24, 0x00C0
-    7f88:	85 ff       	sbrs	r24, 5
-    7f8a:	fc cf       	rjmp	.-8      	; 0x7f84 <putch+0x2>
+    7f94:	80 91 c0 00 	lds	r24, 0x00C0
+    7f98:	85 ff       	sbrs	r24, 5
+    7f9a:	fc cf       	rjmp	.-8      	; 0x7f94 <putch+0x2>
   UART_UDR = ch;
-    7f8c:	90 93 c6 00 	sts	0x00C6, r25
+    7f9c:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    7f90:	08 95       	ret
+    7fa0:	08 95       	ret
 
-00007f92 <getch>:
+00007fa2 <getch>:
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UART_SRA & _BV(RXC0)))
-    7f92:	80 91 c0 00 	lds	r24, 0x00C0
-    7f96:	87 ff       	sbrs	r24, 7
-    7f98:	fc cf       	rjmp	.-8      	; 0x7f92 <getch>
+    7fa2:	80 91 c0 00 	lds	r24, 0x00C0
+    7fa6:	87 ff       	sbrs	r24, 7
+    7fa8:	fc cf       	rjmp	.-8      	; 0x7fa2 <getch>
     ;
   if (!(UART_SRA & _BV(FE0))) {
-    7f9a:	80 91 c0 00 	lds	r24, 0x00C0
-    7f9e:	84 fd       	sbrc	r24, 4
-    7fa0:	01 c0       	rjmp	.+2      	; 0x7fa4 <getch+0x12>
+    7faa:	80 91 c0 00 	lds	r24, 0x00C0
+    7fae:	84 fd       	sbrc	r24, 4
+    7fb0:	01 c0       	rjmp	.+2      	; 0x7fb4 <getch+0x12>
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    7fa2:	a8 95       	wdr
+    7fb2:	a8 95       	wdr
        * don't care that an invalid char is returned...)
        */
     watchdogReset();
   }
   
   ch = UART_UDR;
-    7fa4:	80 91 c6 00 	lds	r24, 0x00C6
+    7fb4:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    7fa8:	08 95       	ret
+    7fb8:	08 95       	ret
 
-00007faa <watchdogConfig>:
+00007fba <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    7faa:	e0 e6       	ldi	r30, 0x60	; 96
-    7fac:	f0 e0       	ldi	r31, 0x00	; 0
-    7fae:	98 e1       	ldi	r25, 0x18	; 24
-    7fb0:	90 83       	st	Z, r25
+    7fba:	e0 e6       	ldi	r30, 0x60	; 96
+    7fbc:	f0 e0       	ldi	r31, 0x00	; 0
+    7fbe:	98 e1       	ldi	r25, 0x18	; 24
+    7fc0:	90 83       	st	Z, r25
   WDTCSR = x;
-    7fb2:	80 83       	st	Z, r24
+    7fc2:	80 83       	st	Z, r24
 }
-    7fb4:	08 95       	ret
+    7fc4:	08 95       	ret
 
-00007fb6 <verifySpace>:
+00007fc6 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) {
-    7fb6:	ed df       	rcall	.-38     	; 0x7f92 <getch>
-    7fb8:	80 32       	cpi	r24, 0x20	; 32
-    7fba:	19 f0       	breq	.+6      	; 0x7fc2 <verifySpace+0xc>
+    7fc6:	ed df       	rcall	.-38     	; 0x7fa2 <getch>
+    7fc8:	80 32       	cpi	r24, 0x20	; 32
+    7fca:	19 f0       	breq	.+6      	; 0x7fd2 <verifySpace+0xc>
     watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
-    7fbc:	88 e0       	ldi	r24, 0x08	; 8
-    7fbe:	f5 df       	rcall	.-22     	; 0x7faa <watchdogConfig>
-    7fc0:	ff cf       	rjmp	.-2      	; 0x7fc0 <verifySpace+0xa>
+    7fcc:	88 e0       	ldi	r24, 0x08	; 8
+    7fce:	f5 df       	rcall	.-22     	; 0x7fba <watchdogConfig>
+    7fd0:	ff cf       	rjmp	.-2      	; 0x7fd0 <verifySpace+0xa>
     while (1)			      // and busy-loop so that WD causes
       ;				      //  a reset and app start.
   }
   putch(STK_INSYNC);
-    7fc2:	84 e1       	ldi	r24, 0x14	; 20
+    7fd2:	84 e1       	ldi	r24, 0x14	; 20
 }
-    7fc4:	de cf       	rjmp	.-68     	; 0x7f82 <putch>
+    7fd4:	de cf       	rjmp	.-68     	; 0x7f92 <putch>
 
-00007fc6 <getNch>:
+00007fd6 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    7fc6:	1f 93       	push	r17
-    7fc8:	18 2f       	mov	r17, r24
+    7fd6:	1f 93       	push	r17
+    7fd8:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    7fca:	e3 df       	rcall	.-58     	; 0x7f92 <getch>
-    7fcc:	11 50       	subi	r17, 0x01	; 1
-    7fce:	e9 f7       	brne	.-6      	; 0x7fca <getNch+0x4>
+    7fda:	e3 df       	rcall	.-58     	; 0x7fa2 <getch>
+    7fdc:	11 50       	subi	r17, 0x01	; 1
+    7fde:	e9 f7       	brne	.-6      	; 0x7fda <getNch+0x4>
   verifySpace();
-    7fd0:	f2 df       	rcall	.-28     	; 0x7fb6 <verifySpace>
+    7fe0:	f2 df       	rcall	.-28     	; 0x7fc6 <verifySpace>
 }
-    7fd2:	1f 91       	pop	r17
-    7fd4:	08 95       	ret
+    7fe2:	1f 91       	pop	r17
+    7fe4:	08 95       	ret
 
-00007fd6 <appStart>:
+00007fe6 <appStart>:
 
 void appStart(uint8_t rstFlags) {
   // save the reset flags in the designated register
   //  This can be saved in a main program by putting code in .init0 (which
   //  executes before normal c init code) to save R2 to a global variable.
   __asm__ __volatile__ ("mov r2, %0\n" :: "r" (rstFlags));
-    7fd6:	28 2e       	mov	r2, r24
+    7fe6:	28 2e       	mov	r2, r24
 
   watchdogConfig(WATCHDOG_OFF);
-    7fd8:	80 e0       	ldi	r24, 0x00	; 0
-    7fda:	e7 df       	rcall	.-50     	; 0x7faa <watchdogConfig>
+    7fe8:	80 e0       	ldi	r24, 0x00	; 0
+    7fea:	e7 df       	rcall	.-50     	; 0x7fba <watchdogConfig>
   __asm__ __volatile__ (
-    7fdc:	ee 27       	eor	r30, r30
-    7fde:	ff 27       	eor	r31, r31
-    7fe0:	09 94       	ijmp
+    7fec:	ee 27       	eor	r30, r30
+    7fee:	ff 27       	eor	r31, r31
+    7ff0:	09 94       	ijmp
diff --git a/optiboot/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex b/optiboot/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex
index 06969f4..33eebf7 100644
--- a/optiboot/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex
+++ b/optiboot/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex
@@ -1,34 +1,35 @@
-:107E0000112484B714BE81FFE6D085E08093810001
-:107E100082E08093C00088E18093C10086E0809377
-:107E2000C20088E08093C4008EE0BFD0259A86E02F
-:107E300028E13EEF91E0309385002093840096BBCB
-:107E4000B09BFECF1D9AA8958150A9F7EE24FF2480
-:107E5000AA24A394B5E0CB2EA1E1BA2EF3E0DF2E45
-:107E600098D0813461F495D0082FA5D0023829F13B
-:107E7000013811F485E001C083E083D07FC08234F3
-:107E800011F484E103C0853419F485E09CD076C0F8
-:107E9000853579F47ED0E82EFF247BD0082F10E0C2
-:107EA000102F00270E291F29000F111F84D07801E1
-:107EB00065C0863521F484E086D080E0DECF84364C
-:107EC00009F040C066D065D0082F63D080E0E81686
-:107ED00080E7F80618F4F701D7BEE895C0E0D1E0D6
-:107EE00058D089930C17E1F7F0E0EF16F0E7FF06A2
-:107EF00018F0F701D7BEE8955ED007B600FCFDCFBD
-:107F0000A701A0E0B1E02C9130E011968C9111977F
-:107F100090E0982F8827822B932B1296FA010C0160
-:107F2000A7BEE89511244E5F5F4FF1E0A038BF0770
-:107F300051F7F701C7BEE89507B600FCFDCFB7BE05
-:107F4000E8951CC0843761F424D023D0082F21D0B9
-:107F500032D0F70185917F0114D00150D1F70EC0C6
-:107F6000853739F428D08EE10CD085E90AD08FE02E
-:107F700084CF813511F488E018D01DD080E101D084
-:107F80006FCF982F8091C00085FFFCCF9093C600E3
-:107F900008958091C00087FFFCCF8091C00084FDD0
-:107FA00001C0A8958091C6000895E0E6F0E098E150
-:107FB000908380830895EDDF803219F088E0F5DF4B
-:107FC000FFCF84E1DECF1F93182FE3DF1150E9F7D5
-:107FD000F2DF1F910895282E80E0E7DFEE27FF27CC
-:027FE000099402
-:027FFE0000057C
+:107E0000112484B714BE81FD01C0EDD085E08093BC
+:107E1000810082E08093C00088E18093C10086E009
+:107E20008093C20088E08093C4008EE0C6D0259A7B
+:107E300086E028E13EEF91E03093850020938400B6
+:107E400096BBB09BFECF1D9AA8958150A9F7AA2496
+:107E5000BB2433E0832E7724739425E0922E91E1A6
+:107E6000C92E9FD0813461F49CD0182FACD0123829
+:107E700029F1113811F486E001C083E08AD086C070
+:107E8000823411F484E103C0853419F485E0A3D071
+:107E90007DC0853579F485D0E82EFF2482D0082F67
+:107EA00010E0102F00270E291F29000F111F8BD063
+:107EB00058016CC0863521F484E08DD080E0DECF9F
+:107EC000843609F041C06DD090E0182F002769D0AA
+:107ED00090E0082B192B65D0D82EE801E12CF1E0B9
+:107EE000FF2E5FD0F70181937F012197D1F76BD0EF
+:107EF000F5E4DF1609F4FFCFF50187BEE89507B674
+:107F000000FCFDCFB501A801A0E0B1E02C9130E06C
+:107F100011968C91119790E0982F8827822B932BA4
+:107F20001296FB010C0177BEE89511246E5F7F4F1E
+:107F30004250504059F7F50197BEE89507B600FC4E
+:107F4000FDCFC7BEE89522C0843791F42AD090E0D7
+:107F5000D82FCC2726D090E0C82BD92B22D033D0D5
+:107F60008501F80185918F0114D02197D1F70EC0BA
+:107F7000853739F428D08EE10CD085E90AD08FE01E
+:107F80007DCF813511F488E018D01DD080E101D07B
+:107F900068CF982F8091C00085FFFCCF9093C600DA
+:107FA00008958091C00087FFFCCF8091C00084FDC0
+:107FB00001C0A8958091C6000895E0E6F0E098E140
+:107FC000908380830895EDDF803219F088E0F5DF3B
+:107FD000FFCF84E1DECF1F93182FE3DF1150E9F7C5
+:107FE000F2DF1F910895282E80E0E7DFEE27FF27BC
+:027FF0000994F2
+:027FFE0000067B
 :0400000300007E007B
 :00000001FF
diff --git a/optiboot/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst b/optiboot/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst
index 782b2a2..a59b9aa 100644
--- a/optiboot/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst
+++ b/optiboot/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst
@@ -3,36 +3,36 @@ optiboot_atmega328.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001e2  00007e00  00007e00  00000054  2**1
+  0 .text         000001f2  00007e00  00007e00  00000074  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00007ffe  00007ffe  00000236  2**0
-                  CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000238  2**0
+  1 .version      00000002  00007ffe  00007ffe  00000266  2**0
+                  CONTENTS, ALLOC, LOAD, DATA
+  2 .debug_aranges 00000028  00000000  00000000  00000268  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000005f  00000000  00000000  00000260  2**0
+  3 .debug_pubnames 00000074  00000000  00000000  00000290  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000294  00000000  00000000  000002bf  2**0
+  4 .debug_info   000003db  00000000  00000000  00000304  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000016b  00000000  00000000  00000553  2**0
+  5 .debug_abbrev 000001ea  00000000  00000000  000006df  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000421  00000000  00000000  000006be  2**0
+  6 .debug_line   0000042b  00000000  00000000  000008c9  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000080  00000000  00000000  00000ae0  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000cf4  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000131  00000000  00000000  00000b60  2**0
+  8 .debug_str    00000172  00000000  00000000  00000d74  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    00000266  00000000  00000000  00000c91  2**0
+  9 .debug_loc    000002d7  00000000  00000000  00000ee6  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000078  00000000  00000000  00000ef7  2**0
+ 10 .debug_ranges 000000b8  00000000  00000000  000011bd  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00007e00 <main>:
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
     7e00:	11 24       	eor	r1, r1
@@ -46,541 +46,555 @@ int main(void) {
   MCUSR = 0;
     7e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart(ch);
-    7e06:	81 ff       	sbrs	r24, 1
-    7e08:	e6 d0       	rcall	.+460    	; 0x7fd6 <appStart>
+    7e06:	81 fd       	sbrc	r24, 1
+    7e08:	01 c0       	rjmp	.+2      	; 0x7e0c <main+0xc>
+    7e0a:	ed d0       	rcall	.+474    	; 0x7fe6 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-    7e0a:	85 e0       	ldi	r24, 0x05	; 5
-    7e0c:	80 93 81 00 	sts	0x0081, r24
+    7e0c:	85 e0       	ldi	r24, 0x05	; 5
+    7e0e:	80 93 81 00 	sts	0x0081, r24
   UCSRA = _BV(U2X); //Double speed mode USART
   UCSRB = _BV(RXEN) | _BV(TXEN);  // enable Rx & Tx
   UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0);  // config USART; 8N1
   UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
 #else
   UART_SRA = _BV(U2X0); //Double speed mode USART0
-    7e10:	82 e0       	ldi	r24, 0x02	; 2
-    7e12:	80 93 c0 00 	sts	0x00C0, r24
+    7e12:	82 e0       	ldi	r24, 0x02	; 2
+    7e14:	80 93 c0 00 	sts	0x00C0, r24
   UART_SRB = _BV(RXEN0) | _BV(TXEN0);
-    7e16:	88 e1       	ldi	r24, 0x18	; 24
-    7e18:	80 93 c1 00 	sts	0x00C1, r24
+    7e18:	88 e1       	ldi	r24, 0x18	; 24
+    7e1a:	80 93 c1 00 	sts	0x00C1, r24
   UART_SRC = _BV(UCSZ00) | _BV(UCSZ01);
-    7e1c:	86 e0       	ldi	r24, 0x06	; 6
-    7e1e:	80 93 c2 00 	sts	0x00C2, r24
+    7e1e:	86 e0       	ldi	r24, 0x06	; 6
+    7e20:	80 93 c2 00 	sts	0x00C2, r24
   UART_SRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    7e22:	88 e0       	ldi	r24, 0x08	; 8
-    7e24:	80 93 c4 00 	sts	0x00C4, r24
+    7e24:	88 e0       	ldi	r24, 0x08	; 8
+    7e26:	80 93 c4 00 	sts	0x00C4, r24
 #endif
 #endif
 
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
-    7e28:	8e e0       	ldi	r24, 0x0E	; 14
-    7e2a:	bf d0       	rcall	.+382    	; 0x7faa <watchdogConfig>
+    7e2a:	8e e0       	ldi	r24, 0x0E	; 14
+    7e2c:	c6 d0       	rcall	.+396    	; 0x7fba <watchdogConfig>
 
 #if (LED_START_FLASHES > 0) || defined(LED_DATA_FLASH)
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    7e2c:	25 9a       	sbi	0x04, 5	; 4
-    7e2e:	86 e0       	ldi	r24, 0x06	; 6
+    7e2e:	25 9a       	sbi	0x04, 5	; 4
+    7e30:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    7e30:	28 e1       	ldi	r18, 0x18	; 24
-    7e32:	3e ef       	ldi	r19, 0xFE	; 254
+    7e32:	28 e1       	ldi	r18, 0x18	; 24
+    7e34:	3e ef       	ldi	r19, 0xFE	; 254
     TIFR1 = _BV(TOV1);
-    7e34:	91 e0       	ldi	r25, 0x01	; 1
+    7e36:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    7e36:	30 93 85 00 	sts	0x0085, r19
-    7e3a:	20 93 84 00 	sts	0x0084, r18
+    7e38:	30 93 85 00 	sts	0x0085, r19
+    7e3c:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    7e3e:	96 bb       	out	0x16, r25	; 22
+    7e40:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    7e40:	b0 9b       	sbis	0x16, 0	; 22
-    7e42:	fe cf       	rjmp	.-4      	; 0x7e40 <main+0x40>
+    7e42:	b0 9b       	sbis	0x16, 0	; 22
+    7e44:	fe cf       	rjmp	.-4      	; 0x7e42 <main+0x42>
 #if defined(__AVR_ATmega8__)  || defined (__AVR_ATmega32__)
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
-    7e44:	1d 9a       	sbi	0x03, 5	; 3
+    7e46:	1d 9a       	sbi	0x03, 5	; 3
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    7e46:	a8 95       	wdr
+    7e48:	a8 95       	wdr
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
 #endif
     watchdogReset();
   } while (--count);
-    7e48:	81 50       	subi	r24, 0x01	; 1
-    7e4a:	a9 f7       	brne	.-22     	; 0x7e36 <main+0x36>
-    7e4c:	ee 24       	eor	r14, r14
-    7e4e:	ff 24       	eor	r15, r15
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-        a |= (*bufPtr++) << 8;
-        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    7e50:	aa 24       	eor	r10, r10
-    7e52:	a3 94       	inc	r10
-        addrPtr += 2;
-      } while (--ch);
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    7e54:	b5 e0       	ldi	r27, 0x05	; 5
-    7e56:	cb 2e       	mov	r12, r27
-      boot_spm_busy_wait();
+    7e4a:	81 50       	subi	r24, 0x01	; 1
+    7e4c:	a9 f7       	brne	.-22     	; 0x7e38 <main+0x38>
+    7e4e:	aa 24       	eor	r10, r10
+    7e50:	bb 24       	eor	r11, r11
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    7e52:	33 e0       	ldi	r19, 0x03	; 3
+    7e54:	83 2e       	mov	r8, r19
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    7e56:	77 24       	eor	r7, r7
+    7e58:	73 94       	inc	r7
+	    } while (len -= 2);
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    7e5a:	25 e0       	ldi	r18, 0x05	; 5
+    7e5c:	92 2e       	mov	r9, r18
+	    boot_spm_busy_wait();
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    7e58:	a1 e1       	ldi	r26, 0x11	; 17
-    7e5a:	ba 2e       	mov	r11, r26
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    7e5c:	f3 e0       	ldi	r31, 0x03	; 3
-    7e5e:	df 2e       	mov	r13, r31
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    7e5e:	91 e1       	ldi	r25, 0x11	; 17
+    7e60:	c9 2e       	mov	r12, r25
 #endif
 
-  /* Forever loop */
+  /* Forever loop: exits by causing WDT reset */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    7e60:	98 d0       	rcall	.+304    	; 0x7f92 <getch>
+    7e62:	9f d0       	rcall	.+318    	; 0x7fa2 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    7e62:	81 34       	cpi	r24, 0x41	; 65
-    7e64:	61 f4       	brne	.+24     	; 0x7e7e <main+0x7e>
+    7e64:	81 34       	cpi	r24, 0x41	; 65
+    7e66:	61 f4       	brne	.+24     	; 0x7e80 <main+0x80>
       unsigned char which = getch();
-    7e66:	95 d0       	rcall	.+298    	; 0x7f92 <getch>
-    7e68:	08 2f       	mov	r16, r24
+    7e68:	9c d0       	rcall	.+312    	; 0x7fa2 <getch>
+    7e6a:	18 2f       	mov	r17, r24
       verifySpace();
-    7e6a:	a5 d0       	rcall	.+330    	; 0x7fb6 <verifySpace>
+    7e6c:	ac d0       	rcall	.+344    	; 0x7fc6 <verifySpace>
       if (which == 0x82) {
-    7e6c:	02 38       	cpi	r16, 0x82	; 130
-    7e6e:	29 f1       	breq	.+74     	; 0x7eba <main+0xba>
+    7e6e:	12 38       	cpi	r17, 0x82	; 130
+    7e70:	29 f1       	breq	.+74     	; 0x7ebc <main+0xbc>
 	/*
 	 * Send optiboot version as "minor SW version"
 	 */
 	putch(OPTIBOOT_MINVER);
       } else if (which == 0x81) {
-    7e70:	01 38       	cpi	r16, 0x81	; 129
-    7e72:	11 f4       	brne	.+4      	; 0x7e78 <main+0x78>
+    7e72:	11 38       	cpi	r17, 0x81	; 129
+    7e74:	11 f4       	brne	.+4      	; 0x7e7a <main+0x7a>
 	  putch(OPTIBOOT_MAJVER);
-    7e74:	85 e0       	ldi	r24, 0x05	; 5
-    7e76:	01 c0       	rjmp	.+2      	; 0x7e7a <main+0x7a>
+    7e76:	86 e0       	ldi	r24, 0x06	; 6
+    7e78:	01 c0       	rjmp	.+2      	; 0x7e7c <main+0x7c>
       } else {
 	/*
 	 * GET PARAMETER returns a generic 0x03 reply for
          * other parameters - enough to keep Avrdude happy
 	 */
 	putch(0x03);
-    7e78:	83 e0       	ldi	r24, 0x03	; 3
-    7e7a:	83 d0       	rcall	.+262    	; 0x7f82 <putch>
-    7e7c:	7f c0       	rjmp	.+254    	; 0x7f7c <main+0x17c>
+    7e7a:	83 e0       	ldi	r24, 0x03	; 3
+    7e7c:	8a d0       	rcall	.+276    	; 0x7f92 <putch>
+    7e7e:	86 c0       	rjmp	.+268    	; 0x7f8c <main+0x18c>
       }
     }
     else if(ch == STK_SET_DEVICE) {
-    7e7e:	82 34       	cpi	r24, 0x42	; 66
-    7e80:	11 f4       	brne	.+4      	; 0x7e86 <main+0x86>
+    7e80:	82 34       	cpi	r24, 0x42	; 66
+    7e82:	11 f4       	brne	.+4      	; 0x7e88 <main+0x88>
       // SET DEVICE is ignored
       getNch(20);
-    7e82:	84 e1       	ldi	r24, 0x14	; 20
-    7e84:	03 c0       	rjmp	.+6      	; 0x7e8c <main+0x8c>
+    7e84:	84 e1       	ldi	r24, 0x14	; 20
+    7e86:	03 c0       	rjmp	.+6      	; 0x7e8e <main+0x8e>
     }
     else if(ch == STK_SET_DEVICE_EXT) {
-    7e86:	85 34       	cpi	r24, 0x45	; 69
-    7e88:	19 f4       	brne	.+6      	; 0x7e90 <main+0x90>
+    7e88:	85 34       	cpi	r24, 0x45	; 69
+    7e8a:	19 f4       	brne	.+6      	; 0x7e92 <main+0x92>
       // SET DEVICE EXT is ignored
       getNch(5);
-    7e8a:	85 e0       	ldi	r24, 0x05	; 5
-    7e8c:	9c d0       	rcall	.+312    	; 0x7fc6 <getNch>
-    7e8e:	76 c0       	rjmp	.+236    	; 0x7f7c <main+0x17c>
+    7e8c:	85 e0       	ldi	r24, 0x05	; 5
+    7e8e:	a3 d0       	rcall	.+326    	; 0x7fd6 <getNch>
+    7e90:	7d c0       	rjmp	.+250    	; 0x7f8c <main+0x18c>
     }
     else if(ch == STK_LOAD_ADDRESS) {
-    7e90:	85 35       	cpi	r24, 0x55	; 85
-    7e92:	79 f4       	brne	.+30     	; 0x7eb2 <main+0xb2>
+    7e92:	85 35       	cpi	r24, 0x55	; 85
+    7e94:	79 f4       	brne	.+30     	; 0x7eb4 <main+0xb4>
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    7e94:	7e d0       	rcall	.+252    	; 0x7f92 <getch>
+    7e96:	85 d0       	rcall	.+266    	; 0x7fa2 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
-    7e96:	e8 2e       	mov	r14, r24
-    7e98:	ff 24       	eor	r15, r15
-    7e9a:	7b d0       	rcall	.+246    	; 0x7f92 <getch>
-    7e9c:	08 2f       	mov	r16, r24
-    7e9e:	10 e0       	ldi	r17, 0x00	; 0
-    7ea0:	10 2f       	mov	r17, r16
-    7ea2:	00 27       	eor	r16, r16
-    7ea4:	0e 29       	or	r16, r14
-    7ea6:	1f 29       	or	r17, r15
+    7e98:	e8 2e       	mov	r14, r24
+    7e9a:	ff 24       	eor	r15, r15
+    7e9c:	82 d0       	rcall	.+260    	; 0x7fa2 <getch>
+    7e9e:	08 2f       	mov	r16, r24
+    7ea0:	10 e0       	ldi	r17, 0x00	; 0
+    7ea2:	10 2f       	mov	r17, r16
+    7ea4:	00 27       	eor	r16, r16
+    7ea6:	0e 29       	or	r16, r14
+    7ea8:	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
-    7ea8:	00 0f       	add	r16, r16
-    7eaa:	11 1f       	adc	r17, r17
+    7eaa:	00 0f       	add	r16, r16
+    7eac:	11 1f       	adc	r17, r17
       address = newAddress;
       verifySpace();
-    7eac:	84 d0       	rcall	.+264    	; 0x7fb6 <verifySpace>
-    7eae:	78 01       	movw	r14, r16
-    7eb0:	65 c0       	rjmp	.+202    	; 0x7f7c <main+0x17c>
+    7eae:	8b d0       	rcall	.+278    	; 0x7fc6 <verifySpace>
+    7eb0:	58 01       	movw	r10, r16
+    7eb2:	6c c0       	rjmp	.+216    	; 0x7f8c <main+0x18c>
     }
     else if(ch == STK_UNIVERSAL) {
-    7eb2:	86 35       	cpi	r24, 0x56	; 86
-    7eb4:	21 f4       	brne	.+8      	; 0x7ebe <main+0xbe>
+    7eb4:	86 35       	cpi	r24, 0x56	; 86
+    7eb6:	21 f4       	brne	.+8      	; 0x7ec0 <main+0xc0>
       // UNIVERSAL command is ignored
       getNch(4);
-    7eb6:	84 e0       	ldi	r24, 0x04	; 4
-    7eb8:	86 d0       	rcall	.+268    	; 0x7fc6 <getNch>
+    7eb8:	84 e0       	ldi	r24, 0x04	; 4
+    7eba:	8d d0       	rcall	.+282    	; 0x7fd6 <getNch>
       putch(0x00);
-    7eba:	80 e0       	ldi	r24, 0x00	; 0
-    7ebc:	de cf       	rjmp	.-68     	; 0x7e7a <main+0x7a>
+    7ebc:	80 e0       	ldi	r24, 0x00	; 0
+    7ebe:	de cf       	rjmp	.-68     	; 0x7e7c <main+0x7c>
     }
     /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    7ebe:	84 36       	cpi	r24, 0x64	; 100
-    7ec0:	09 f0       	breq	.+2      	; 0x7ec4 <main+0xc4>
-    7ec2:	40 c0       	rjmp	.+128    	; 0x7f44 <main+0x144>
+    7ec0:	84 36       	cpi	r24, 0x64	; 100
+    7ec2:	09 f0       	breq	.+2      	; 0x7ec6 <main+0xc6>
+    7ec4:	41 c0       	rjmp	.+130    	; 0x7f48 <main+0x148>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
+      uint8_t desttype;
       uint8_t *bufPtr;
-      uint16_t addrPtr;
+      uint16_t savelength;
 
-      getch();			/* getlen() */
-    7ec4:	66 d0       	rcall	.+204    	; 0x7f92 <getch>
-      length = getch();
-    7ec6:	65 d0       	rcall	.+202    	; 0x7f92 <getch>
-    7ec8:	08 2f       	mov	r16, r24
-      getch();
-    7eca:	63 d0       	rcall	.+198    	; 0x7f92 <getch>
+      length = getch()<<8;			/* getlen() */
+    7ec6:	6d d0       	rcall	.+218    	; 0x7fa2 <getch>
+    7ec8:	90 e0       	ldi	r25, 0x00	; 0
+    7eca:	18 2f       	mov	r17, r24
+    7ecc:	00 27       	eor	r16, r16
+      length |= getch();
+    7ece:	69 d0       	rcall	.+210    	; 0x7fa2 <getch>
+    7ed0:	90 e0       	ldi	r25, 0x00	; 0
+    7ed2:	08 2b       	or	r16, r24
+    7ed4:	19 2b       	or	r17, r25
+      savelength = length;
+      desttype = getch();
+    7ed6:	65 d0       	rcall	.+202    	; 0x7fa2 <getch>
+    7ed8:	d8 2e       	mov	r13, r24
+    7eda:	e8 01       	movw	r28, r16
+    7edc:	e1 2c       	mov	r14, r1
+    7ede:	f1 e0       	ldi	r31, 0x01	; 1
+    7ee0:	ff 2e       	mov	r15, r31
 
-      // If we are in RWW section, immediately start page erase
-      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    7ecc:	80 e0       	ldi	r24, 0x00	; 0
-    7ece:	e8 16       	cp	r14, r24
-    7ed0:	80 e7       	ldi	r24, 0x70	; 112
-    7ed2:	f8 06       	cpc	r15, r24
-    7ed4:	18 f4       	brcc	.+6      	; 0x7edc <main+0xdc>
-    7ed6:	f7 01       	movw	r30, r14
-    7ed8:	d7 be       	out	0x37, r13	; 55
-    7eda:	e8 95       	spm
-    7edc:	c0 e0       	ldi	r28, 0x00	; 0
-    7ede:	d1 e0       	ldi	r29, 0x01	; 1
-
-      // While that is going on, read in page contents
+      // read a page worth of contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    7ee0:	58 d0       	rcall	.+176    	; 0x7f92 <getch>
-    7ee2:	89 93       	st	Y+, r24
+    7ee2:	5f d0       	rcall	.+190    	; 0x7fa2 <getch>
+    7ee4:	f7 01       	movw	r30, r14
+    7ee6:	81 93       	st	Z+, r24
+    7ee8:	7f 01       	movw	r14, r30
       while (--length);
-    7ee4:	0c 17       	cp	r16, r28
-    7ee6:	e1 f7       	brne	.-8      	; 0x7ee0 <main+0xe0>
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    7ee8:	f0 e0       	ldi	r31, 0x00	; 0
-    7eea:	ef 16       	cp	r14, r31
-    7eec:	f0 e7       	ldi	r31, 0x70	; 112
-    7eee:	ff 06       	cpc	r15, r31
-    7ef0:	18 f0       	brcs	.+6      	; 0x7ef8 <main+0xf8>
-    7ef2:	f7 01       	movw	r30, r14
-    7ef4:	d7 be       	out	0x37, r13	; 55
-    7ef6:	e8 95       	spm
+    7eea:	21 97       	sbiw	r28, 0x01	; 1
+    7eec:	d1 f7       	brne	.-12     	; 0x7ee2 <main+0xe2>
 
       // Read command terminator, start reply
       verifySpace();
-    7ef8:	5e d0       	rcall	.+188    	; 0x7fb6 <verifySpace>
-
-      // If only a partial page is to be programmed, the erase might not be complete.
-      // So check that here
-      boot_spm_busy_wait();
-    7efa:	07 b6       	in	r0, 0x37	; 55
-    7efc:	00 fc       	sbrc	r0, 0
-    7efe:	fd cf       	rjmp	.-6      	; 0x7efa <main+0xfa>
-    7f00:	a7 01       	movw	r20, r14
-    7f02:	a0 e0       	ldi	r26, 0x00	; 0
-    7f04:	b1 e0       	ldi	r27, 0x01	; 1
-      bufPtr = buff;
-      addrPtr = (uint16_t)(void*)address;
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-    7f06:	2c 91       	ld	r18, X
-    7f08:	30 e0       	ldi	r19, 0x00	; 0
-        a |= (*bufPtr++) << 8;
-    7f0a:	11 96       	adiw	r26, 0x01	; 1
-    7f0c:	8c 91       	ld	r24, X
-    7f0e:	11 97       	sbiw	r26, 0x01	; 1
-    7f10:	90 e0       	ldi	r25, 0x00	; 0
-    7f12:	98 2f       	mov	r25, r24
-    7f14:	88 27       	eor	r24, r24
-    7f16:	82 2b       	or	r24, r18
-    7f18:	93 2b       	or	r25, r19
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+    7eee:	6b d0       	rcall	.+214    	; 0x7fc6 <verifySpace>
+ * void writebuffer(memtype, buffer, address, length)
+ */
+static inline void writebuffer(int8_t memtype, uint8_t *mybuff,
+			       uint16_t address, uint16_t len)
+{
+    switch (memtype) {
+    7ef0:	f5 e4       	ldi	r31, 0x45	; 69
+    7ef2:	df 16       	cp	r13, r31
+    7ef4:	09 f4       	brne	.+2      	; 0x7ef8 <main+0xf8>
+    7ef6:	ff cf       	rjmp	.-2      	; 0x7ef6 <main+0xf6>
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    7ef8:	f5 01       	movw	r30, r10
+    7efa:	87 be       	out	0x37, r8	; 55
+    7efc:	e8 95       	spm
+	    boot_spm_busy_wait();
+    7efe:	07 b6       	in	r0, 0x37	; 55
+    7f00:	00 fc       	sbrc	r0, 0
+    7f02:	fd cf       	rjmp	.-6      	; 0x7efe <main+0xfe>
+    7f04:	b5 01       	movw	r22, r10
+    7f06:	a8 01       	movw	r20, r16
+    7f08:	a0 e0       	ldi	r26, 0x00	; 0
+    7f0a:	b1 e0       	ldi	r27, 0x01	; 1
+	    /*
+	     * Copy data from the buffer into the flash write buffer.
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+    7f0c:	2c 91       	ld	r18, X
+    7f0e:	30 e0       	ldi	r19, 0x00	; 0
+		a |= (*bufPtr++) << 8;
+    7f10:	11 96       	adiw	r26, 0x01	; 1
+    7f12:	8c 91       	ld	r24, X
+    7f14:	11 97       	sbiw	r26, 0x01	; 1
+    7f16:	90 e0       	ldi	r25, 0x00	; 0
+    7f18:	98 2f       	mov	r25, r24
+    7f1a:	88 27       	eor	r24, r24
+    7f1c:	82 2b       	or	r24, r18
+    7f1e:	93 2b       	or	r25, r19
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
-    7f1a:	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);
-    7f1c:	fa 01       	movw	r30, r20
-    7f1e:	0c 01       	movw	r0, r24
-    7f20:	a7 be       	out	0x37, r10	; 55
-    7f22:	e8 95       	spm
-    7f24:	11 24       	eor	r1, r1
-        addrPtr += 2;
-    7f26:	4e 5f       	subi	r20, 0xFE	; 254
-    7f28:	5f 4f       	sbci	r21, 0xFF	; 255
-      } while (--ch);
-    7f2a:	f1 e0       	ldi	r31, 0x01	; 1
-    7f2c:	a0 38       	cpi	r26, 0x80	; 128
-    7f2e:	bf 07       	cpc	r27, r31
-    7f30:	51 f7       	brne	.-44     	; 0x7f06 <main+0x106>
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    7f32:	f7 01       	movw	r30, r14
-    7f34:	c7 be       	out	0x37, r12	; 55
-    7f36:	e8 95       	spm
-      boot_spm_busy_wait();
-    7f38:	07 b6       	in	r0, 0x37	; 55
-    7f3a:	00 fc       	sbrc	r0, 0
-    7f3c:	fd cf       	rjmp	.-6      	; 0x7f38 <main+0x138>
+    7f20:	12 96       	adiw	r26, 0x02	; 2
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    7f22:	fb 01       	movw	r30, r22
+    7f24:	0c 01       	movw	r0, r24
+    7f26:	77 be       	out	0x37, r7	; 55
+    7f28:	e8 95       	spm
+    7f2a:	11 24       	eor	r1, r1
+		addrPtr += 2;
+    7f2c:	6e 5f       	subi	r22, 0xFE	; 254
+    7f2e:	7f 4f       	sbci	r23, 0xFF	; 255
+	    } while (len -= 2);
+    7f30:	42 50       	subi	r20, 0x02	; 2
+    7f32:	50 40       	sbci	r21, 0x00	; 0
+    7f34:	59 f7       	brne	.-42     	; 0x7f0c <main+0x10c>
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    7f36:	f5 01       	movw	r30, r10
+    7f38:	97 be       	out	0x37, r9	; 55
+    7f3a:	e8 95       	spm
+	    boot_spm_busy_wait();
+    7f3c:	07 b6       	in	r0, 0x37	; 55
+    7f3e:	00 fc       	sbrc	r0, 0
+    7f40:	fd cf       	rjmp	.-6      	; 0x7f3c <main+0x13c>
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    7f3e:	b7 be       	out	0x37, r11	; 55
-    7f40:	e8 95       	spm
-    7f42:	1c c0       	rjmp	.+56     	; 0x7f7c <main+0x17c>
-#endif
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    7f42:	c7 be       	out	0x37, r12	; 55
+    7f44:	e8 95       	spm
+    7f46:	22 c0       	rjmp	.+68     	; 0x7f8c <main+0x18c>
+      writebuffer(desttype, buff, address, savelength);
+
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    7f44:	84 37       	cpi	r24, 0x74	; 116
-    7f46:	61 f4       	brne	.+24     	; 0x7f60 <main+0x160>
-      // READ PAGE - we only read flash
-      getch();			/* getlen() */
-    7f48:	24 d0       	rcall	.+72     	; 0x7f92 <getch>
-      length = getch();
-    7f4a:	23 d0       	rcall	.+70     	; 0x7f92 <getch>
-    7f4c:	08 2f       	mov	r16, r24
-      getch();
-    7f4e:	21 d0       	rcall	.+66     	; 0x7f92 <getch>
+    7f48:	84 37       	cpi	r24, 0x74	; 116
+    7f4a:	91 f4       	brne	.+36     	; 0x7f70 <main+0x170>
+      uint8_t desttype;
+      length = getch()<<8;			/* getlen() */
+    7f4c:	2a d0       	rcall	.+84     	; 0x7fa2 <getch>
+    7f4e:	90 e0       	ldi	r25, 0x00	; 0
+    7f50:	d8 2f       	mov	r29, r24
+    7f52:	cc 27       	eor	r28, r28
+      length |= getch();
+    7f54:	26 d0       	rcall	.+76     	; 0x7fa2 <getch>
+    7f56:	90 e0       	ldi	r25, 0x00	; 0
+    7f58:	c8 2b       	or	r28, r24
+    7f5a:	d9 2b       	or	r29, r25
+      desttype = getch();
+    7f5c:	22 d0       	rcall	.+68     	; 0x7fa2 <getch>
 
       verifySpace();
-    7f50:	32 d0       	rcall	.+100    	; 0x7fb6 <verifySpace>
-        __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+    7f5e:	33 d0       	rcall	.+102    	; 0x7fc6 <verifySpace>
+    7f60:	85 01       	movw	r16, r10
+	    __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #else
-        // read a Flash byte and increment the address
-        __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+	    // read a Flash byte and increment the address
+	    __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #endif
-        putch(ch);
-    7f52:	f7 01       	movw	r30, r14
-    7f54:	85 91       	lpm	r24, Z+
-    7f56:	7f 01       	movw	r14, r30
-    7f58:	14 d0       	rcall	.+40     	; 0x7f82 <putch>
-      } while (--length);
-    7f5a:	01 50       	subi	r16, 0x01	; 1
-    7f5c:	d1 f7       	brne	.-12     	; 0x7f52 <main+0x152>
-    7f5e:	0e c0       	rjmp	.+28     	; 0x7f7c <main+0x17c>
+	    putch(ch);
+    7f62:	f8 01       	movw	r30, r16
+    7f64:	85 91       	lpm	r24, Z+
+    7f66:	8f 01       	movw	r16, r30
+    7f68:	14 d0       	rcall	.+40     	; 0x7f92 <putch>
+	} while (--length);
+    7f6a:	21 97       	sbiw	r28, 0x01	; 1
+    7f6c:	d1 f7       	brne	.-12     	; 0x7f62 <main+0x162>
+    7f6e:	0e c0       	rjmp	.+28     	; 0x7f8c <main+0x18c>
+	  
+      read_mem(desttype, address, length);
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    7f60:	85 37       	cpi	r24, 0x75	; 117
-    7f62:	39 f4       	brne	.+14     	; 0x7f72 <main+0x172>
+    7f70:	85 37       	cpi	r24, 0x75	; 117
+    7f72:	39 f4       	brne	.+14     	; 0x7f82 <main+0x182>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    7f64:	28 d0       	rcall	.+80     	; 0x7fb6 <verifySpace>
+    7f74:	28 d0       	rcall	.+80     	; 0x7fc6 <verifySpace>
       putch(SIGNATURE_0);
-    7f66:	8e e1       	ldi	r24, 0x1E	; 30
-    7f68:	0c d0       	rcall	.+24     	; 0x7f82 <putch>
+    7f76:	8e e1       	ldi	r24, 0x1E	; 30
+    7f78:	0c d0       	rcall	.+24     	; 0x7f92 <putch>
       putch(SIGNATURE_1);
-    7f6a:	85 e9       	ldi	r24, 0x95	; 149
-    7f6c:	0a d0       	rcall	.+20     	; 0x7f82 <putch>
+    7f7a:	85 e9       	ldi	r24, 0x95	; 149
+    7f7c:	0a d0       	rcall	.+20     	; 0x7f92 <putch>
       putch(SIGNATURE_2);
-    7f6e:	8f e0       	ldi	r24, 0x0F	; 15
-    7f70:	84 cf       	rjmp	.-248    	; 0x7e7a <main+0x7a>
+    7f7e:	8f e0       	ldi	r24, 0x0F	; 15
+    7f80:	7d cf       	rjmp	.-262    	; 0x7e7c <main+0x7c>
     }
     else if (ch == STK_LEAVE_PROGMODE) { /* 'Q' */
-    7f72:	81 35       	cpi	r24, 0x51	; 81
-    7f74:	11 f4       	brne	.+4      	; 0x7f7a <main+0x17a>
+    7f82:	81 35       	cpi	r24, 0x51	; 81
+    7f84:	11 f4       	brne	.+4      	; 0x7f8a <main+0x18a>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    7f76:	88 e0       	ldi	r24, 0x08	; 8
-    7f78:	18 d0       	rcall	.+48     	; 0x7faa <watchdogConfig>
+    7f86:	88 e0       	ldi	r24, 0x08	; 8
+    7f88:	18 d0       	rcall	.+48     	; 0x7fba <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    7f7a:	1d d0       	rcall	.+58     	; 0x7fb6 <verifySpace>
+    7f8a:	1d d0       	rcall	.+58     	; 0x7fc6 <verifySpace>
     }
     putch(STK_OK);
-    7f7c:	80 e1       	ldi	r24, 0x10	; 16
-    7f7e:	01 d0       	rcall	.+2      	; 0x7f82 <putch>
-    7f80:	6f cf       	rjmp	.-290    	; 0x7e60 <main+0x60>
+    7f8c:	80 e1       	ldi	r24, 0x10	; 16
+    7f8e:	01 d0       	rcall	.+2      	; 0x7f92 <putch>
+    7f90:	68 cf       	rjmp	.-304    	; 0x7e62 <main+0x62>
 
-00007f82 <putch>:
+00007f92 <putch>:
   }
 }
 
 void putch(char ch) {
-    7f82:	98 2f       	mov	r25, r24
+    7f92:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UART_SRA & _BV(UDRE0)));
-    7f84:	80 91 c0 00 	lds	r24, 0x00C0
-    7f88:	85 ff       	sbrs	r24, 5
-    7f8a:	fc cf       	rjmp	.-8      	; 0x7f84 <putch+0x2>
+    7f94:	80 91 c0 00 	lds	r24, 0x00C0
+    7f98:	85 ff       	sbrs	r24, 5
+    7f9a:	fc cf       	rjmp	.-8      	; 0x7f94 <putch+0x2>
   UART_UDR = ch;
-    7f8c:	90 93 c6 00 	sts	0x00C6, r25
+    7f9c:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    7f90:	08 95       	ret
+    7fa0:	08 95       	ret
 
-00007f92 <getch>:
+00007fa2 <getch>:
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UART_SRA & _BV(RXC0)))
-    7f92:	80 91 c0 00 	lds	r24, 0x00C0
-    7f96:	87 ff       	sbrs	r24, 7
-    7f98:	fc cf       	rjmp	.-8      	; 0x7f92 <getch>
+    7fa2:	80 91 c0 00 	lds	r24, 0x00C0
+    7fa6:	87 ff       	sbrs	r24, 7
+    7fa8:	fc cf       	rjmp	.-8      	; 0x7fa2 <getch>
     ;
   if (!(UART_SRA & _BV(FE0))) {
-    7f9a:	80 91 c0 00 	lds	r24, 0x00C0
-    7f9e:	84 fd       	sbrc	r24, 4
-    7fa0:	01 c0       	rjmp	.+2      	; 0x7fa4 <getch+0x12>
+    7faa:	80 91 c0 00 	lds	r24, 0x00C0
+    7fae:	84 fd       	sbrc	r24, 4
+    7fb0:	01 c0       	rjmp	.+2      	; 0x7fb4 <getch+0x12>
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    7fa2:	a8 95       	wdr
+    7fb2:	a8 95       	wdr
        * don't care that an invalid char is returned...)
        */
     watchdogReset();
   }
   
   ch = UART_UDR;
-    7fa4:	80 91 c6 00 	lds	r24, 0x00C6
+    7fb4:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    7fa8:	08 95       	ret
+    7fb8:	08 95       	ret
 
-00007faa <watchdogConfig>:
+00007fba <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    7faa:	e0 e6       	ldi	r30, 0x60	; 96
-    7fac:	f0 e0       	ldi	r31, 0x00	; 0
-    7fae:	98 e1       	ldi	r25, 0x18	; 24
-    7fb0:	90 83       	st	Z, r25
+    7fba:	e0 e6       	ldi	r30, 0x60	; 96
+    7fbc:	f0 e0       	ldi	r31, 0x00	; 0
+    7fbe:	98 e1       	ldi	r25, 0x18	; 24
+    7fc0:	90 83       	st	Z, r25
   WDTCSR = x;
-    7fb2:	80 83       	st	Z, r24
+    7fc2:	80 83       	st	Z, r24
 }
-    7fb4:	08 95       	ret
+    7fc4:	08 95       	ret
 
-00007fb6 <verifySpace>:
+00007fc6 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) {
-    7fb6:	ed df       	rcall	.-38     	; 0x7f92 <getch>
-    7fb8:	80 32       	cpi	r24, 0x20	; 32
-    7fba:	19 f0       	breq	.+6      	; 0x7fc2 <verifySpace+0xc>
+    7fc6:	ed df       	rcall	.-38     	; 0x7fa2 <getch>
+    7fc8:	80 32       	cpi	r24, 0x20	; 32
+    7fca:	19 f0       	breq	.+6      	; 0x7fd2 <verifySpace+0xc>
     watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
-    7fbc:	88 e0       	ldi	r24, 0x08	; 8
-    7fbe:	f5 df       	rcall	.-22     	; 0x7faa <watchdogConfig>
-    7fc0:	ff cf       	rjmp	.-2      	; 0x7fc0 <verifySpace+0xa>
+    7fcc:	88 e0       	ldi	r24, 0x08	; 8
+    7fce:	f5 df       	rcall	.-22     	; 0x7fba <watchdogConfig>
+    7fd0:	ff cf       	rjmp	.-2      	; 0x7fd0 <verifySpace+0xa>
     while (1)			      // and busy-loop so that WD causes
       ;				      //  a reset and app start.
   }
   putch(STK_INSYNC);
-    7fc2:	84 e1       	ldi	r24, 0x14	; 20
+    7fd2:	84 e1       	ldi	r24, 0x14	; 20
 }
-    7fc4:	de cf       	rjmp	.-68     	; 0x7f82 <putch>
+    7fd4:	de cf       	rjmp	.-68     	; 0x7f92 <putch>
 
-00007fc6 <getNch>:
+00007fd6 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    7fc6:	1f 93       	push	r17
-    7fc8:	18 2f       	mov	r17, r24
+    7fd6:	1f 93       	push	r17
+    7fd8:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    7fca:	e3 df       	rcall	.-58     	; 0x7f92 <getch>
-    7fcc:	11 50       	subi	r17, 0x01	; 1
-    7fce:	e9 f7       	brne	.-6      	; 0x7fca <getNch+0x4>
+    7fda:	e3 df       	rcall	.-58     	; 0x7fa2 <getch>
+    7fdc:	11 50       	subi	r17, 0x01	; 1
+    7fde:	e9 f7       	brne	.-6      	; 0x7fda <getNch+0x4>
   verifySpace();
-    7fd0:	f2 df       	rcall	.-28     	; 0x7fb6 <verifySpace>
+    7fe0:	f2 df       	rcall	.-28     	; 0x7fc6 <verifySpace>
 }
-    7fd2:	1f 91       	pop	r17
-    7fd4:	08 95       	ret
+    7fe2:	1f 91       	pop	r17
+    7fe4:	08 95       	ret
 
-00007fd6 <appStart>:
+00007fe6 <appStart>:
 
 void appStart(uint8_t rstFlags) {
   // save the reset flags in the designated register
   //  This can be saved in a main program by putting code in .init0 (which
   //  executes before normal c init code) to save R2 to a global variable.
   __asm__ __volatile__ ("mov r2, %0\n" :: "r" (rstFlags));
-    7fd6:	28 2e       	mov	r2, r24
+    7fe6:	28 2e       	mov	r2, r24
 
   watchdogConfig(WATCHDOG_OFF);
-    7fd8:	80 e0       	ldi	r24, 0x00	; 0
-    7fda:	e7 df       	rcall	.-50     	; 0x7faa <watchdogConfig>
+    7fe8:	80 e0       	ldi	r24, 0x00	; 0
+    7fea:	e7 df       	rcall	.-50     	; 0x7fba <watchdogConfig>
   __asm__ __volatile__ (
-    7fdc:	ee 27       	eor	r30, r30
-    7fde:	ff 27       	eor	r31, r31
-    7fe0:	09 94       	ijmp
+    7fec:	ee 27       	eor	r30, r30
+    7fee:	ff 27       	eor	r31, r31
+    7ff0:	09 94       	ijmp
diff --git a/optiboot/bootloaders/optiboot/optiboot_diecimila.lst b/optiboot/bootloaders/optiboot/optiboot_diecimila.lst
index 6f31a80..c2b08ef 100644
--- a/optiboot/bootloaders/optiboot/optiboot_diecimila.lst
+++ b/optiboot/bootloaders/optiboot/optiboot_diecimila.lst
@@ -3,36 +3,36 @@ optiboot_atmega168.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001e2  00003e00  00003e00  00000054  2**1
+  0 .text         000001f0  00003e00  00003e00  00000074  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00003ffe  00003ffe  00000236  2**0
-                  CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000238  2**0
+  1 .version      00000002  00003ffe  00003ffe  00000264  2**0
+                  CONTENTS, ALLOC, LOAD, DATA
+  2 .debug_aranges 00000028  00000000  00000000  00000266  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000005f  00000000  00000000  00000260  2**0
+  3 .debug_pubnames 00000074  00000000  00000000  0000028e  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000294  00000000  00000000  000002bf  2**0
+  4 .debug_info   000003db  00000000  00000000  00000302  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000016b  00000000  00000000  00000553  2**0
+  5 .debug_abbrev 000001ea  00000000  00000000  000006dd  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000421  00000000  00000000  000006be  2**0
+  6 .debug_line   00000423  00000000  00000000  000008c7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000080  00000000  00000000  00000ae0  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000cec  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000131  00000000  00000000  00000b60  2**0
+  8 .debug_str    00000172  00000000  00000000  00000d6c  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    00000266  00000000  00000000  00000c91  2**0
+  9 .debug_loc    000002d7  00000000  00000000  00000ede  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000078  00000000  00000000  00000ef7  2**0
+ 10 .debug_ranges 000000b8  00000000  00000000  000011b5  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00003e00 <main>:
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
     3e00:	11 24       	eor	r1, r1
@@ -46,148 +46,147 @@ int main(void) {
   MCUSR = 0;
     3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart(ch);
-    3e06:	81 ff       	sbrs	r24, 1
-    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+    3e06:	81 fd       	sbrc	r24, 1
+    3e08:	01 c0       	rjmp	.+2      	; 0x3e0c <main+0xc>
+    3e0a:	ec d0       	rcall	.+472    	; 0x3fe4 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-    3e0a:	85 e0       	ldi	r24, 0x05	; 5
-    3e0c:	80 93 81 00 	sts	0x0081, r24
+    3e0c:	85 e0       	ldi	r24, 0x05	; 5
+    3e0e:	80 93 81 00 	sts	0x0081, r24
   UCSRA = _BV(U2X); //Double speed mode USART
   UCSRB = _BV(RXEN) | _BV(TXEN);  // enable Rx & Tx
   UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0);  // config USART; 8N1
   UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
 #else
   UART_SRA = _BV(U2X0); //Double speed mode USART0
-    3e10:	82 e0       	ldi	r24, 0x02	; 2
-    3e12:	80 93 c0 00 	sts	0x00C0, r24
+    3e12:	82 e0       	ldi	r24, 0x02	; 2
+    3e14:	80 93 c0 00 	sts	0x00C0, r24
   UART_SRB = _BV(RXEN0) | _BV(TXEN0);
-    3e16:	88 e1       	ldi	r24, 0x18	; 24
-    3e18:	80 93 c1 00 	sts	0x00C1, r24
+    3e18:	88 e1       	ldi	r24, 0x18	; 24
+    3e1a:	80 93 c1 00 	sts	0x00C1, r24
   UART_SRC = _BV(UCSZ00) | _BV(UCSZ01);
-    3e1c:	86 e0       	ldi	r24, 0x06	; 6
-    3e1e:	80 93 c2 00 	sts	0x00C2, r24
+    3e1e:	86 e0       	ldi	r24, 0x06	; 6
+    3e20:	80 93 c2 00 	sts	0x00C2, r24
   UART_SRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    3e22:	80 e1       	ldi	r24, 0x10	; 16
-    3e24:	80 93 c4 00 	sts	0x00C4, r24
+    3e24:	80 e1       	ldi	r24, 0x10	; 16
+    3e26:	80 93 c4 00 	sts	0x00C4, r24
 #endif
 #endif
 
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
-    3e28:	8e e0       	ldi	r24, 0x0E	; 14
-    3e2a:	bf d0       	rcall	.+382    	; 0x3faa <watchdogConfig>
+    3e2a:	8e e0       	ldi	r24, 0x0E	; 14
+    3e2c:	c5 d0       	rcall	.+394    	; 0x3fb8 <watchdogConfig>
 
 #if (LED_START_FLASHES > 0) || defined(LED_DATA_FLASH)
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    3e2c:	25 9a       	sbi	0x04, 5	; 4
-    3e2e:	86 e0       	ldi	r24, 0x06	; 6
+    3e2e:	25 9a       	sbi	0x04, 5	; 4
+    3e30:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e30:	20 e3       	ldi	r18, 0x30	; 48
-    3e32:	3c ef       	ldi	r19, 0xFC	; 252
+    3e32:	20 e3       	ldi	r18, 0x30	; 48
+    3e34:	3c ef       	ldi	r19, 0xFC	; 252
     TIFR1 = _BV(TOV1);
-    3e34:	91 e0       	ldi	r25, 0x01	; 1
+    3e36:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e36:	30 93 85 00 	sts	0x0085, r19
-    3e3a:	20 93 84 00 	sts	0x0084, r18
+    3e38:	30 93 85 00 	sts	0x0085, r19
+    3e3c:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    3e3e:	96 bb       	out	0x16, r25	; 22
+    3e40:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    3e40:	b0 9b       	sbis	0x16, 0	; 22
-    3e42:	fe cf       	rjmp	.-4      	; 0x3e40 <main+0x40>
+    3e42:	b0 9b       	sbis	0x16, 0	; 22
+    3e44:	fe cf       	rjmp	.-4      	; 0x3e42 <main+0x42>
 #if defined(__AVR_ATmega8__)  || defined (__AVR_ATmega32__)
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
-    3e44:	1d 9a       	sbi	0x03, 5	; 3
+    3e46:	1d 9a       	sbi	0x03, 5	; 3
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3e46:	a8 95       	wdr
+    3e48:	a8 95       	wdr
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
 #endif
     watchdogReset();
   } while (--count);
-    3e48:	81 50       	subi	r24, 0x01	; 1
-    3e4a:	a9 f7       	brne	.-22     	; 0x3e36 <main+0x36>
-    3e4c:	ee 24       	eor	r14, r14
-    3e4e:	ff 24       	eor	r15, r15
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-        a |= (*bufPtr++) << 8;
-        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    3e50:	aa 24       	eor	r10, r10
-    3e52:	a3 94       	inc	r10
-        addrPtr += 2;
-      } while (--ch);
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    3e54:	b5 e0       	ldi	r27, 0x05	; 5
-    3e56:	cb 2e       	mov	r12, r27
-      boot_spm_busy_wait();
+    3e4a:	81 50       	subi	r24, 0x01	; 1
+    3e4c:	a9 f7       	brne	.-22     	; 0x3e38 <main+0x38>
+    3e4e:	aa 24       	eor	r10, r10
+    3e50:	bb 24       	eor	r11, r11
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    3e52:	33 e0       	ldi	r19, 0x03	; 3
+    3e54:	83 2e       	mov	r8, r19
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3e56:	77 24       	eor	r7, r7
+    3e58:	73 94       	inc	r7
+	    } while (len -= 2);
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    3e5a:	25 e0       	ldi	r18, 0x05	; 5
+    3e5c:	92 2e       	mov	r9, r18
+	    boot_spm_busy_wait();
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    3e58:	a1 e1       	ldi	r26, 0x11	; 17
-    3e5a:	ba 2e       	mov	r11, r26
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3e5c:	f3 e0       	ldi	r31, 0x03	; 3
-    3e5e:	df 2e       	mov	r13, r31
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    3e5e:	91 e1       	ldi	r25, 0x11	; 17
+    3e60:	c9 2e       	mov	r12, r25
 #endif
 
-  /* Forever loop */
+  /* Forever loop: exits by causing WDT reset */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e60:	98 d0       	rcall	.+304    	; 0x3f92 <getch>
+    3e62:	9e d0       	rcall	.+316    	; 0x3fa0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    3e62:	81 34       	cpi	r24, 0x41	; 65
-    3e64:	61 f4       	brne	.+24     	; 0x3e7e <main+0x7e>
+    3e64:	81 34       	cpi	r24, 0x41	; 65
+    3e66:	59 f4       	brne	.+22     	; 0x3e7e <main+0x7e>
       unsigned char which = getch();
-    3e66:	95 d0       	rcall	.+298    	; 0x3f92 <getch>
-    3e68:	08 2f       	mov	r16, r24
+    3e68:	9b d0       	rcall	.+310    	; 0x3fa0 <getch>
+    3e6a:	18 2f       	mov	r17, r24
       verifySpace();
-    3e6a:	a5 d0       	rcall	.+330    	; 0x3fb6 <verifySpace>
+    3e6c:	ab d0       	rcall	.+342    	; 0x3fc4 <verifySpace>
       if (which == 0x82) {
-    3e6c:	02 38       	cpi	r16, 0x82	; 130
-    3e6e:	29 f1       	breq	.+74     	; 0x3eba <main+0xba>
+    3e6e:	12 38       	cpi	r17, 0x82	; 130
+    3e70:	21 f1       	breq	.+72     	; 0x3eba <main+0xba>
 	/*
 	 * Send optiboot version as "minor SW version"
 	 */
 	putch(OPTIBOOT_MINVER);
       } else if (which == 0x81) {
-    3e70:	01 38       	cpi	r16, 0x81	; 129
-    3e72:	11 f4       	brne	.+4      	; 0x3e78 <main+0x78>
-	  putch(OPTIBOOT_MAJVER);
-    3e74:	85 e0       	ldi	r24, 0x05	; 5
-    3e76:	01 c0       	rjmp	.+2      	; 0x3e7a <main+0x7a>
+    3e72:	11 38       	cpi	r17, 0x81	; 129
+    3e74:	09 f4       	brne	.+2      	; 0x3e78 <main+0x78>
+    3e76:	82 c0       	rjmp	.+260    	; 0x3f7c <main+0x17c>
       } else {
 	/*
 	 * GET PARAMETER returns a generic 0x03 reply for
@@ -195,8 +194,8 @@ void watchdogReset() {
 	 */
 	putch(0x03);
     3e78:	83 e0       	ldi	r24, 0x03	; 3
-    3e7a:	83 d0       	rcall	.+262    	; 0x3f82 <putch>
-    3e7c:	7f c0       	rjmp	.+254    	; 0x3f7c <main+0x17c>
+    3e7a:	8a d0       	rcall	.+276    	; 0x3f90 <putch>
+    3e7c:	86 c0       	rjmp	.+268    	; 0x3f8a <main+0x18a>
       }
     }
     else if(ch == STK_SET_DEVICE) {
@@ -213,8 +212,8 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     3e8a:	85 e0       	ldi	r24, 0x05	; 5
-    3e8c:	9c d0       	rcall	.+312    	; 0x3fc6 <getNch>
-    3e8e:	76 c0       	rjmp	.+236    	; 0x3f7c <main+0x17c>
+    3e8c:	a3 d0       	rcall	.+326    	; 0x3fd4 <getNch>
+    3e8e:	7d c0       	rjmp	.+250    	; 0x3f8a <main+0x18a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     3e90:	85 35       	cpi	r24, 0x55	; 85
@@ -222,11 +221,11 @@ void watchdogReset() {
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    3e94:	7e d0       	rcall	.+252    	; 0x3f92 <getch>
+    3e94:	85 d0       	rcall	.+266    	; 0x3fa0 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
     3e96:	e8 2e       	mov	r14, r24
     3e98:	ff 24       	eor	r15, r15
-    3e9a:	7b d0       	rcall	.+246    	; 0x3f92 <getch>
+    3e9a:	82 d0       	rcall	.+260    	; 0x3fa0 <getch>
     3e9c:	08 2f       	mov	r16, r24
     3e9e:	10 e0       	ldi	r17, 0x00	; 0
     3ea0:	10 2f       	mov	r17, r16
@@ -242,9 +241,9 @@ void watchdogReset() {
     3eaa:	11 1f       	adc	r17, r17
       address = newAddress;
       verifySpace();
-    3eac:	84 d0       	rcall	.+264    	; 0x3fb6 <verifySpace>
-    3eae:	78 01       	movw	r14, r16
-    3eb0:	65 c0       	rjmp	.+202    	; 0x3f7c <main+0x17c>
+    3eac:	8b d0       	rcall	.+278    	; 0x3fc4 <verifySpace>
+    3eae:	58 01       	movw	r10, r16
+    3eb0:	6c c0       	rjmp	.+216    	; 0x3f8a <main+0x18a>
     }
     else if(ch == STK_UNIVERSAL) {
     3eb2:	86 35       	cpi	r24, 0x56	; 86
@@ -252,7 +251,7 @@ void watchdogReset() {
       // UNIVERSAL command is ignored
       getNch(4);
     3eb6:	84 e0       	ldi	r24, 0x04	; 4
-    3eb8:	86 d0       	rcall	.+268    	; 0x3fc6 <getNch>
+    3eb8:	8d d0       	rcall	.+282    	; 0x3fd4 <getNch>
       putch(0x00);
     3eba:	80 e0       	ldi	r24, 0x00	; 0
     3ebc:	de cf       	rjmp	.-68     	; 0x3e7a <main+0x7a>
@@ -261,326 +260,339 @@ void watchdogReset() {
     else if(ch == STK_PROG_PAGE) {
     3ebe:	84 36       	cpi	r24, 0x64	; 100
     3ec0:	09 f0       	breq	.+2      	; 0x3ec4 <main+0xc4>
-    3ec2:	40 c0       	rjmp	.+128    	; 0x3f44 <main+0x144>
+    3ec2:	41 c0       	rjmp	.+130    	; 0x3f46 <main+0x146>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
+      uint8_t desttype;
       uint8_t *bufPtr;
-      uint16_t addrPtr;
+      uint16_t savelength;
 
-      getch();			/* getlen() */
-    3ec4:	66 d0       	rcall	.+204    	; 0x3f92 <getch>
-      length = getch();
-    3ec6:	65 d0       	rcall	.+202    	; 0x3f92 <getch>
-    3ec8:	08 2f       	mov	r16, r24
-      getch();
-    3eca:	63 d0       	rcall	.+198    	; 0x3f92 <getch>
+      length = getch()<<8;			/* getlen() */
+    3ec4:	6d d0       	rcall	.+218    	; 0x3fa0 <getch>
+    3ec6:	90 e0       	ldi	r25, 0x00	; 0
+    3ec8:	18 2f       	mov	r17, r24
+    3eca:	00 27       	eor	r16, r16
+      length |= getch();
+    3ecc:	69 d0       	rcall	.+210    	; 0x3fa0 <getch>
+    3ece:	90 e0       	ldi	r25, 0x00	; 0
+    3ed0:	08 2b       	or	r16, r24
+    3ed2:	19 2b       	or	r17, r25
+      savelength = length;
+      desttype = getch();
+    3ed4:	65 d0       	rcall	.+202    	; 0x3fa0 <getch>
+    3ed6:	d8 2e       	mov	r13, r24
+    3ed8:	e8 01       	movw	r28, r16
+    3eda:	e1 2c       	mov	r14, r1
+    3edc:	f1 e0       	ldi	r31, 0x01	; 1
+    3ede:	ff 2e       	mov	r15, r31
 
-      // If we are in RWW section, immediately start page erase
-      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ecc:	80 e0       	ldi	r24, 0x00	; 0
-    3ece:	e8 16       	cp	r14, r24
-    3ed0:	88 e3       	ldi	r24, 0x38	; 56
-    3ed2:	f8 06       	cpc	r15, r24
-    3ed4:	18 f4       	brcc	.+6      	; 0x3edc <main+0xdc>
-    3ed6:	f7 01       	movw	r30, r14
-    3ed8:	d7 be       	out	0x37, r13	; 55
-    3eda:	e8 95       	spm
-    3edc:	c0 e0       	ldi	r28, 0x00	; 0
-    3ede:	d1 e0       	ldi	r29, 0x01	; 1
-
-      // While that is going on, read in page contents
+      // read a page worth of contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ee0:	58 d0       	rcall	.+176    	; 0x3f92 <getch>
-    3ee2:	89 93       	st	Y+, r24
+    3ee0:	5f d0       	rcall	.+190    	; 0x3fa0 <getch>
+    3ee2:	f7 01       	movw	r30, r14
+    3ee4:	81 93       	st	Z+, r24
+    3ee6:	7f 01       	movw	r14, r30
       while (--length);
-    3ee4:	0c 17       	cp	r16, r28
-    3ee6:	e1 f7       	brne	.-8      	; 0x3ee0 <main+0xe0>
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ee8:	f0 e0       	ldi	r31, 0x00	; 0
-    3eea:	ef 16       	cp	r14, r31
-    3eec:	f8 e3       	ldi	r31, 0x38	; 56
-    3eee:	ff 06       	cpc	r15, r31
-    3ef0:	18 f0       	brcs	.+6      	; 0x3ef8 <main+0xf8>
-    3ef2:	f7 01       	movw	r30, r14
-    3ef4:	d7 be       	out	0x37, r13	; 55
-    3ef6:	e8 95       	spm
+    3ee8:	21 97       	sbiw	r28, 0x01	; 1
+    3eea:	d1 f7       	brne	.-12     	; 0x3ee0 <main+0xe0>
 
       // Read command terminator, start reply
       verifySpace();
-    3ef8:	5e d0       	rcall	.+188    	; 0x3fb6 <verifySpace>
-
-      // If only a partial page is to be programmed, the erase might not be complete.
-      // So check that here
-      boot_spm_busy_wait();
-    3efa:	07 b6       	in	r0, 0x37	; 55
-    3efc:	00 fc       	sbrc	r0, 0
-    3efe:	fd cf       	rjmp	.-6      	; 0x3efa <main+0xfa>
-    3f00:	a7 01       	movw	r20, r14
-    3f02:	a0 e0       	ldi	r26, 0x00	; 0
-    3f04:	b1 e0       	ldi	r27, 0x01	; 1
-      bufPtr = buff;
-      addrPtr = (uint16_t)(void*)address;
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-    3f06:	2c 91       	ld	r18, X
-    3f08:	30 e0       	ldi	r19, 0x00	; 0
-        a |= (*bufPtr++) << 8;
-    3f0a:	11 96       	adiw	r26, 0x01	; 1
-    3f0c:	8c 91       	ld	r24, X
-    3f0e:	11 97       	sbiw	r26, 0x01	; 1
-    3f10:	90 e0       	ldi	r25, 0x00	; 0
-    3f12:	98 2f       	mov	r25, r24
-    3f14:	88 27       	eor	r24, r24
-    3f16:	82 2b       	or	r24, r18
-    3f18:	93 2b       	or	r25, r19
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+    3eec:	6b d0       	rcall	.+214    	; 0x3fc4 <verifySpace>
+ * void writebuffer(memtype, buffer, address, length)
+ */
+static inline void writebuffer(int8_t memtype, uint8_t *mybuff,
+			       uint16_t address, uint16_t len)
+{
+    switch (memtype) {
+    3eee:	f5 e4       	ldi	r31, 0x45	; 69
+    3ef0:	df 16       	cp	r13, r31
+    3ef2:	09 f4       	brne	.+2      	; 0x3ef6 <main+0xf6>
+    3ef4:	ff cf       	rjmp	.-2      	; 0x3ef4 <main+0xf4>
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    3ef6:	f5 01       	movw	r30, r10
+    3ef8:	87 be       	out	0x37, r8	; 55
+    3efa:	e8 95       	spm
+	    boot_spm_busy_wait();
+    3efc:	07 b6       	in	r0, 0x37	; 55
+    3efe:	00 fc       	sbrc	r0, 0
+    3f00:	fd cf       	rjmp	.-6      	; 0x3efc <main+0xfc>
+    3f02:	b5 01       	movw	r22, r10
+    3f04:	a8 01       	movw	r20, r16
+    3f06:	a0 e0       	ldi	r26, 0x00	; 0
+    3f08:	b1 e0       	ldi	r27, 0x01	; 1
+	    /*
+	     * Copy data from the buffer into the flash write buffer.
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+    3f0a:	2c 91       	ld	r18, X
+    3f0c:	30 e0       	ldi	r19, 0x00	; 0
+		a |= (*bufPtr++) << 8;
+    3f0e:	11 96       	adiw	r26, 0x01	; 1
+    3f10:	8c 91       	ld	r24, X
+    3f12:	11 97       	sbiw	r26, 0x01	; 1
+    3f14:	90 e0       	ldi	r25, 0x00	; 0
+    3f16:	98 2f       	mov	r25, r24
+    3f18:	88 27       	eor	r24, r24
+    3f1a:	82 2b       	or	r24, r18
+    3f1c:	93 2b       	or	r25, r19
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
-    3f1a:	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);
-    3f1c:	fa 01       	movw	r30, r20
-    3f1e:	0c 01       	movw	r0, r24
-    3f20:	a7 be       	out	0x37, r10	; 55
-    3f22:	e8 95       	spm
-    3f24:	11 24       	eor	r1, r1
-        addrPtr += 2;
-    3f26:	4e 5f       	subi	r20, 0xFE	; 254
-    3f28:	5f 4f       	sbci	r21, 0xFF	; 255
-      } while (--ch);
-    3f2a:	f1 e0       	ldi	r31, 0x01	; 1
-    3f2c:	a0 38       	cpi	r26, 0x80	; 128
-    3f2e:	bf 07       	cpc	r27, r31
-    3f30:	51 f7       	brne	.-44     	; 0x3f06 <main+0x106>
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    3f32:	f7 01       	movw	r30, r14
-    3f34:	c7 be       	out	0x37, r12	; 55
-    3f36:	e8 95       	spm
-      boot_spm_busy_wait();
-    3f38:	07 b6       	in	r0, 0x37	; 55
-    3f3a:	00 fc       	sbrc	r0, 0
-    3f3c:	fd cf       	rjmp	.-6      	; 0x3f38 <main+0x138>
+    3f1e:	12 96       	adiw	r26, 0x02	; 2
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3f20:	fb 01       	movw	r30, r22
+    3f22:	0c 01       	movw	r0, r24
+    3f24:	77 be       	out	0x37, r7	; 55
+    3f26:	e8 95       	spm
+    3f28:	11 24       	eor	r1, r1
+		addrPtr += 2;
+    3f2a:	6e 5f       	subi	r22, 0xFE	; 254
+    3f2c:	7f 4f       	sbci	r23, 0xFF	; 255
+	    } while (len -= 2);
+    3f2e:	42 50       	subi	r20, 0x02	; 2
+    3f30:	50 40       	sbci	r21, 0x00	; 0
+    3f32:	59 f7       	brne	.-42     	; 0x3f0a <main+0x10a>
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    3f34:	f5 01       	movw	r30, r10
+    3f36:	97 be       	out	0x37, r9	; 55
+    3f38:	e8 95       	spm
+	    boot_spm_busy_wait();
+    3f3a:	07 b6       	in	r0, 0x37	; 55
+    3f3c:	00 fc       	sbrc	r0, 0
+    3f3e:	fd cf       	rjmp	.-6      	; 0x3f3a <main+0x13a>
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    3f3e:	b7 be       	out	0x37, r11	; 55
-    3f40:	e8 95       	spm
-    3f42:	1c c0       	rjmp	.+56     	; 0x3f7c <main+0x17c>
-#endif
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    3f40:	c7 be       	out	0x37, r12	; 55
+    3f42:	e8 95       	spm
+    3f44:	22 c0       	rjmp	.+68     	; 0x3f8a <main+0x18a>
+      writebuffer(desttype, buff, address, savelength);
+
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f44:	84 37       	cpi	r24, 0x74	; 116
-    3f46:	61 f4       	brne	.+24     	; 0x3f60 <main+0x160>
-      // READ PAGE - we only read flash
-      getch();			/* getlen() */
-    3f48:	24 d0       	rcall	.+72     	; 0x3f92 <getch>
-      length = getch();
-    3f4a:	23 d0       	rcall	.+70     	; 0x3f92 <getch>
-    3f4c:	08 2f       	mov	r16, r24
-      getch();
-    3f4e:	21 d0       	rcall	.+66     	; 0x3f92 <getch>
+    3f46:	84 37       	cpi	r24, 0x74	; 116
+    3f48:	91 f4       	brne	.+36     	; 0x3f6e <main+0x16e>
+      uint8_t desttype;
+      length = getch()<<8;			/* getlen() */
+    3f4a:	2a d0       	rcall	.+84     	; 0x3fa0 <getch>
+    3f4c:	90 e0       	ldi	r25, 0x00	; 0
+    3f4e:	d8 2f       	mov	r29, r24
+    3f50:	cc 27       	eor	r28, r28
+      length |= getch();
+    3f52:	26 d0       	rcall	.+76     	; 0x3fa0 <getch>
+    3f54:	90 e0       	ldi	r25, 0x00	; 0
+    3f56:	c8 2b       	or	r28, r24
+    3f58:	d9 2b       	or	r29, r25
+      desttype = getch();
+    3f5a:	22 d0       	rcall	.+68     	; 0x3fa0 <getch>
 
       verifySpace();
-    3f50:	32 d0       	rcall	.+100    	; 0x3fb6 <verifySpace>
-        __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+    3f5c:	33 d0       	rcall	.+102    	; 0x3fc4 <verifySpace>
+    3f5e:	85 01       	movw	r16, r10
+	    __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #else
-        // read a Flash byte and increment the address
-        __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+	    // read a Flash byte and increment the address
+	    __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #endif
-        putch(ch);
-    3f52:	f7 01       	movw	r30, r14
-    3f54:	85 91       	lpm	r24, Z+
-    3f56:	7f 01       	movw	r14, r30
-    3f58:	14 d0       	rcall	.+40     	; 0x3f82 <putch>
-      } while (--length);
-    3f5a:	01 50       	subi	r16, 0x01	; 1
-    3f5c:	d1 f7       	brne	.-12     	; 0x3f52 <main+0x152>
-    3f5e:	0e c0       	rjmp	.+28     	; 0x3f7c <main+0x17c>
+	    putch(ch);
+    3f60:	f8 01       	movw	r30, r16
+    3f62:	85 91       	lpm	r24, Z+
+    3f64:	8f 01       	movw	r16, r30
+    3f66:	14 d0       	rcall	.+40     	; 0x3f90 <putch>
+	} while (--length);
+    3f68:	21 97       	sbiw	r28, 0x01	; 1
+    3f6a:	d1 f7       	brne	.-12     	; 0x3f60 <main+0x160>
+    3f6c:	0e c0       	rjmp	.+28     	; 0x3f8a <main+0x18a>
+	  
+      read_mem(desttype, address, length);
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f60:	85 37       	cpi	r24, 0x75	; 117
-    3f62:	39 f4       	brne	.+14     	; 0x3f72 <main+0x172>
+    3f6e:	85 37       	cpi	r24, 0x75	; 117
+    3f70:	39 f4       	brne	.+14     	; 0x3f80 <main+0x180>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f64:	28 d0       	rcall	.+80     	; 0x3fb6 <verifySpace>
+    3f72:	28 d0       	rcall	.+80     	; 0x3fc4 <verifySpace>
       putch(SIGNATURE_0);
-    3f66:	8e e1       	ldi	r24, 0x1E	; 30
-    3f68:	0c d0       	rcall	.+24     	; 0x3f82 <putch>
+    3f74:	8e e1       	ldi	r24, 0x1E	; 30
+    3f76:	0c d0       	rcall	.+24     	; 0x3f90 <putch>
       putch(SIGNATURE_1);
-    3f6a:	84 e9       	ldi	r24, 0x94	; 148
-    3f6c:	0a d0       	rcall	.+20     	; 0x3f82 <putch>
+    3f78:	84 e9       	ldi	r24, 0x94	; 148
+    3f7a:	0a d0       	rcall	.+20     	; 0x3f90 <putch>
       putch(SIGNATURE_2);
-    3f6e:	86 e0       	ldi	r24, 0x06	; 6
-    3f70:	84 cf       	rjmp	.-248    	; 0x3e7a <main+0x7a>
+    3f7c:	86 e0       	ldi	r24, 0x06	; 6
+    3f7e:	7d cf       	rjmp	.-262    	; 0x3e7a <main+0x7a>
     }
     else if (ch == STK_LEAVE_PROGMODE) { /* 'Q' */
-    3f72:	81 35       	cpi	r24, 0x51	; 81
-    3f74:	11 f4       	brne	.+4      	; 0x3f7a <main+0x17a>
+    3f80:	81 35       	cpi	r24, 0x51	; 81
+    3f82:	11 f4       	brne	.+4      	; 0x3f88 <main+0x188>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f76:	88 e0       	ldi	r24, 0x08	; 8
-    3f78:	18 d0       	rcall	.+48     	; 0x3faa <watchdogConfig>
+    3f84:	88 e0       	ldi	r24, 0x08	; 8
+    3f86:	18 d0       	rcall	.+48     	; 0x3fb8 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f7a:	1d d0       	rcall	.+58     	; 0x3fb6 <verifySpace>
+    3f88:	1d d0       	rcall	.+58     	; 0x3fc4 <verifySpace>
     }
     putch(STK_OK);
-    3f7c:	80 e1       	ldi	r24, 0x10	; 16
-    3f7e:	01 d0       	rcall	.+2      	; 0x3f82 <putch>
-    3f80:	6f cf       	rjmp	.-290    	; 0x3e60 <main+0x60>
+    3f8a:	80 e1       	ldi	r24, 0x10	; 16
+    3f8c:	01 d0       	rcall	.+2      	; 0x3f90 <putch>
+    3f8e:	69 cf       	rjmp	.-302    	; 0x3e62 <main+0x62>
 
-00003f82 <putch>:
+00003f90 <putch>:
   }
 }
 
 void putch(char ch) {
-    3f82:	98 2f       	mov	r25, r24
+    3f90:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UART_SRA & _BV(UDRE0)));
-    3f84:	80 91 c0 00 	lds	r24, 0x00C0
-    3f88:	85 ff       	sbrs	r24, 5
-    3f8a:	fc cf       	rjmp	.-8      	; 0x3f84 <putch+0x2>
+    3f92:	80 91 c0 00 	lds	r24, 0x00C0
+    3f96:	85 ff       	sbrs	r24, 5
+    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <putch+0x2>
   UART_UDR = ch;
-    3f8c:	90 93 c6 00 	sts	0x00C6, r25
+    3f9a:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3f90:	08 95       	ret
+    3f9e:	08 95       	ret
 
-00003f92 <getch>:
+00003fa0 <getch>:
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UART_SRA & _BV(RXC0)))
-    3f92:	80 91 c0 00 	lds	r24, 0x00C0
-    3f96:	87 ff       	sbrs	r24, 7
-    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <getch>
+    3fa0:	80 91 c0 00 	lds	r24, 0x00C0
+    3fa4:	87 ff       	sbrs	r24, 7
+    3fa6:	fc cf       	rjmp	.-8      	; 0x3fa0 <getch>
     ;
   if (!(UART_SRA & _BV(FE0))) {
-    3f9a:	80 91 c0 00 	lds	r24, 0x00C0
-    3f9e:	84 fd       	sbrc	r24, 4
-    3fa0:	01 c0       	rjmp	.+2      	; 0x3fa4 <getch+0x12>
+    3fa8:	80 91 c0 00 	lds	r24, 0x00C0
+    3fac:	84 fd       	sbrc	r24, 4
+    3fae:	01 c0       	rjmp	.+2      	; 0x3fb2 <getch+0x12>
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fa2:	a8 95       	wdr
+    3fb0:	a8 95       	wdr
        * don't care that an invalid char is returned...)
        */
     watchdogReset();
   }
   
   ch = UART_UDR;
-    3fa4:	80 91 c6 00 	lds	r24, 0x00C6
+    3fb2:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    3fa8:	08 95       	ret
+    3fb6:	08 95       	ret
 
-00003faa <watchdogConfig>:
+00003fb8 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3faa:	e0 e6       	ldi	r30, 0x60	; 96
-    3fac:	f0 e0       	ldi	r31, 0x00	; 0
-    3fae:	98 e1       	ldi	r25, 0x18	; 24
-    3fb0:	90 83       	st	Z, r25
+    3fb8:	e0 e6       	ldi	r30, 0x60	; 96
+    3fba:	f0 e0       	ldi	r31, 0x00	; 0
+    3fbc:	98 e1       	ldi	r25, 0x18	; 24
+    3fbe:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fb2:	80 83       	st	Z, r24
+    3fc0:	80 83       	st	Z, r24
 }
-    3fb4:	08 95       	ret
+    3fc2:	08 95       	ret
 
-00003fb6 <verifySpace>:
+00003fc4 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) {
-    3fb6:	ed df       	rcall	.-38     	; 0x3f92 <getch>
-    3fb8:	80 32       	cpi	r24, 0x20	; 32
-    3fba:	19 f0       	breq	.+6      	; 0x3fc2 <verifySpace+0xc>
+    3fc4:	ed df       	rcall	.-38     	; 0x3fa0 <getch>
+    3fc6:	80 32       	cpi	r24, 0x20	; 32
+    3fc8:	19 f0       	breq	.+6      	; 0x3fd0 <verifySpace+0xc>
     watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
-    3fbc:	88 e0       	ldi	r24, 0x08	; 8
-    3fbe:	f5 df       	rcall	.-22     	; 0x3faa <watchdogConfig>
-    3fc0:	ff cf       	rjmp	.-2      	; 0x3fc0 <verifySpace+0xa>
+    3fca:	88 e0       	ldi	r24, 0x08	; 8
+    3fcc:	f5 df       	rcall	.-22     	; 0x3fb8 <watchdogConfig>
+    3fce:	ff cf       	rjmp	.-2      	; 0x3fce <verifySpace+0xa>
     while (1)			      // and busy-loop so that WD causes
       ;				      //  a reset and app start.
   }
   putch(STK_INSYNC);
-    3fc2:	84 e1       	ldi	r24, 0x14	; 20
+    3fd0:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fc4:	de cf       	rjmp	.-68     	; 0x3f82 <putch>
+    3fd2:	de cf       	rjmp	.-68     	; 0x3f90 <putch>
 
-00003fc6 <getNch>:
+00003fd4 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fc6:	1f 93       	push	r17
-    3fc8:	18 2f       	mov	r17, r24
+    3fd4:	1f 93       	push	r17
+    3fd6:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3fca:	e3 df       	rcall	.-58     	; 0x3f92 <getch>
-    3fcc:	11 50       	subi	r17, 0x01	; 1
-    3fce:	e9 f7       	brne	.-6      	; 0x3fca <getNch+0x4>
+    3fd8:	e3 df       	rcall	.-58     	; 0x3fa0 <getch>
+    3fda:	11 50       	subi	r17, 0x01	; 1
+    3fdc:	e9 f7       	brne	.-6      	; 0x3fd8 <getNch+0x4>
   verifySpace();
-    3fd0:	f2 df       	rcall	.-28     	; 0x3fb6 <verifySpace>
+    3fde:	f2 df       	rcall	.-28     	; 0x3fc4 <verifySpace>
 }
-    3fd2:	1f 91       	pop	r17
-    3fd4:	08 95       	ret
+    3fe0:	1f 91       	pop	r17
+    3fe2:	08 95       	ret
 
-00003fd6 <appStart>:
+00003fe4 <appStart>:
 
 void appStart(uint8_t rstFlags) {
   // save the reset flags in the designated register
   //  This can be saved in a main program by putting code in .init0 (which
   //  executes before normal c init code) to save R2 to a global variable.
   __asm__ __volatile__ ("mov r2, %0\n" :: "r" (rstFlags));
-    3fd6:	28 2e       	mov	r2, r24
+    3fe4:	28 2e       	mov	r2, r24
 
   watchdogConfig(WATCHDOG_OFF);
-    3fd8:	80 e0       	ldi	r24, 0x00	; 0
-    3fda:	e7 df       	rcall	.-50     	; 0x3faa <watchdogConfig>
+    3fe6:	80 e0       	ldi	r24, 0x00	; 0
+    3fe8:	e7 df       	rcall	.-50     	; 0x3fb8 <watchdogConfig>
   __asm__ __volatile__ (
-    3fdc:	ee 27       	eor	r30, r30
-    3fde:	ff 27       	eor	r31, r31
-    3fe0:	09 94       	ijmp
+    3fea:	ee 27       	eor	r30, r30
+    3fec:	ff 27       	eor	r31, r31
+    3fee:	09 94       	ijmp
diff --git a/optiboot/bootloaders/optiboot/optiboot_lilypad.hex b/optiboot/bootloaders/optiboot/optiboot_lilypad.hex
index b43d1ed..83f5e2b 100644
--- a/optiboot/bootloaders/optiboot/optiboot_lilypad.hex
+++ b/optiboot/bootloaders/optiboot/optiboot_lilypad.hex
@@ -1,34 +1,34 @@
-:103E0000112484B714BE81FFE6D085E08093810041
-:103E100082E08093C00088E18093C10086E08093B7
-:103E2000C20088E08093C4008EE0BFD0259A86E06F
-:103E300028E13EEF91E0309385002093840096BB0B
-:103E4000B09BFECF1D9AA8958150A9F7EE24FF24C0
-:103E5000AA24A394B5E0CB2EA1E1BA2EF3E0DF2E85
-:103E600098D0813461F495D0082FA5D0023829F17B
-:103E7000013811F485E001C083E083D07FC0823433
-:103E800011F484E103C0853419F485E09CD076C038
-:103E9000853579F47ED0E82EFF247BD0082F10E002
-:103EA000102F00270E291F29000F111F84D0780121
-:103EB00065C0863521F484E086D080E0DECF84368C
-:103EC00009F040C066D065D0082F63D080E0E816C6
-:103ED00088E3F80618F4F701D7BEE895C0E0D1E012
-:103EE00058D089930C17E1F7F0E0EF16F8E3FF06DE
-:103EF00018F0F701D7BEE8955ED007B600FCFDCFFD
-:103F0000A701A0E0B1E02C9130E011968C911197BF
-:103F100090E0982F8827822B932B1296FA010C01A0
-:103F2000A7BEE89511244E5F5F4FF1E0A038BF07B0
-:103F300051F7F701C7BEE89507B600FCFDCFB7BE45
-:103F4000E8951CC0843761F424D023D0082F21D0F9
-:103F500032D0F70185917F0114D00150D1F70EC006
-:103F6000853739F428D08EE10CD084E90AD086E078
-:103F700084CF813511F488E018D01DD080E101D0C4
-:103F80006FCF982F8091C00085FFFCCF9093C60023
-:103F900008958091C00087FFFCCF8091C00084FD10
-:103FA00001C0A8958091C6000895E0E6F0E098E190
-:103FB000908380830895EDDF803219F088E0F5DF8B
-:103FC000FFCF84E1DECF1F93182FE3DF1150E9F715
-:103FD000F2DF1F910895282E80E0E7DFEE27FF270C
-:023FE000099442
-:023FFE000005BC
+:103E0000112484B714BE81FD01C0ECD085E08093FD
+:103E1000810082E08093C00088E18093C10086E049
+:103E20008093C20088E08093C4008EE0C5D0259ABC
+:103E300086E028E13EEF91E03093850020938400F6
+:103E400096BBB09BFECF1D9AA8958150A9F7AA24D6
+:103E5000BB2433E0832E7724739425E0922E91E1E6
+:103E6000C92E9ED0813459F49BD0182FABD0123874
+:103E700021F1113809F482C083E08AD086C08234EF
+:103E800011F484E103C0853419F485E0A3D07DC02A
+:103E9000853579F485D0E82EFF2482D0082F10E0F4
+:103EA000102F00270E291F29000F111F8BD058013A
+:103EB0006CC0863521F484E08DD080E0DECF84367E
+:103EC00009F041C06DD090E0182F002769D090E034
+:103ED000082B192B65D0D82EE801E12CF1E0FF2E3C
+:103EE0005FD0F70181937F012197D1F76BD0F5E483
+:103EF000DF1609F4FFCFF50187BEE89507B600FC91
+:103F0000FDCFB501A801A0E0B1E02C9130E0119601
+:103F10008C91119790E0982F8827822B932B1296E3
+:103F2000FB010C0177BEE89511246E5F7F4F425074
+:103F3000504059F7F50197BEE89507B600FCFDCF54
+:103F4000C7BEE89522C0843791F42AD090E0D82FDC
+:103F5000CC2726D090E0C82BD92B22D033D0850196
+:103F6000F80185918F0114D02197D1F70EC08537C4
+:103F700039F428D08EE10CD084E90AD086E07DCFD8
+:103F8000813511F488E018D01DD080E101D069CFCF
+:103F9000982F8091C00085FFFCCF9093C6000895B4
+:103FA0008091C00087FFFCCF8091C00084FD01C0DC
+:103FB000A8958091C6000895E0E6F0E098E190832E
+:103FC00080830895EDDF803219F088E0F5DFFFCFC0
+:103FD00084E1DECF1F93182FE3DF1150E9F7F2DF02
+:103FE0001F910895282E80E0E7DFEE27FF27099430
+:023FFE000006BB
 :0400000300003E00BB
 :00000001FF
diff --git a/optiboot/bootloaders/optiboot/optiboot_lilypad.lst b/optiboot/bootloaders/optiboot/optiboot_lilypad.lst
index d2d73c8..7f6a486 100644
--- a/optiboot/bootloaders/optiboot/optiboot_lilypad.lst
+++ b/optiboot/bootloaders/optiboot/optiboot_lilypad.lst
@@ -3,36 +3,36 @@ optiboot_atmega168.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001e2  00003e00  00003e00  00000054  2**1
+  0 .text         000001f0  00003e00  00003e00  00000074  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00003ffe  00003ffe  00000236  2**0
-                  CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000238  2**0
+  1 .version      00000002  00003ffe  00003ffe  00000264  2**0
+                  CONTENTS, ALLOC, LOAD, DATA
+  2 .debug_aranges 00000028  00000000  00000000  00000266  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000005f  00000000  00000000  00000260  2**0
+  3 .debug_pubnames 00000074  00000000  00000000  0000028e  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000294  00000000  00000000  000002bf  2**0
+  4 .debug_info   000003db  00000000  00000000  00000302  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000016b  00000000  00000000  00000553  2**0
+  5 .debug_abbrev 000001ea  00000000  00000000  000006dd  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000421  00000000  00000000  000006be  2**0
+  6 .debug_line   00000423  00000000  00000000  000008c7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000080  00000000  00000000  00000ae0  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000cec  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000131  00000000  00000000  00000b60  2**0
+  8 .debug_str    00000172  00000000  00000000  00000d6c  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    00000266  00000000  00000000  00000c91  2**0
+  9 .debug_loc    000002d7  00000000  00000000  00000ede  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000078  00000000  00000000  00000ef7  2**0
+ 10 .debug_ranges 000000b8  00000000  00000000  000011b5  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00003e00 <main>:
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
     3e00:	11 24       	eor	r1, r1
@@ -46,148 +46,147 @@ int main(void) {
   MCUSR = 0;
     3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart(ch);
-    3e06:	81 ff       	sbrs	r24, 1
-    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+    3e06:	81 fd       	sbrc	r24, 1
+    3e08:	01 c0       	rjmp	.+2      	; 0x3e0c <main+0xc>
+    3e0a:	ec d0       	rcall	.+472    	; 0x3fe4 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-    3e0a:	85 e0       	ldi	r24, 0x05	; 5
-    3e0c:	80 93 81 00 	sts	0x0081, r24
+    3e0c:	85 e0       	ldi	r24, 0x05	; 5
+    3e0e:	80 93 81 00 	sts	0x0081, r24
   UCSRA = _BV(U2X); //Double speed mode USART
   UCSRB = _BV(RXEN) | _BV(TXEN);  // enable Rx & Tx
   UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0);  // config USART; 8N1
   UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
 #else
   UART_SRA = _BV(U2X0); //Double speed mode USART0
-    3e10:	82 e0       	ldi	r24, 0x02	; 2
-    3e12:	80 93 c0 00 	sts	0x00C0, r24
+    3e12:	82 e0       	ldi	r24, 0x02	; 2
+    3e14:	80 93 c0 00 	sts	0x00C0, r24
   UART_SRB = _BV(RXEN0) | _BV(TXEN0);
-    3e16:	88 e1       	ldi	r24, 0x18	; 24
-    3e18:	80 93 c1 00 	sts	0x00C1, r24
+    3e18:	88 e1       	ldi	r24, 0x18	; 24
+    3e1a:	80 93 c1 00 	sts	0x00C1, r24
   UART_SRC = _BV(UCSZ00) | _BV(UCSZ01);
-    3e1c:	86 e0       	ldi	r24, 0x06	; 6
-    3e1e:	80 93 c2 00 	sts	0x00C2, r24
+    3e1e:	86 e0       	ldi	r24, 0x06	; 6
+    3e20:	80 93 c2 00 	sts	0x00C2, r24
   UART_SRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    3e22:	88 e0       	ldi	r24, 0x08	; 8
-    3e24:	80 93 c4 00 	sts	0x00C4, r24
+    3e24:	88 e0       	ldi	r24, 0x08	; 8
+    3e26:	80 93 c4 00 	sts	0x00C4, r24
 #endif
 #endif
 
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
-    3e28:	8e e0       	ldi	r24, 0x0E	; 14
-    3e2a:	bf d0       	rcall	.+382    	; 0x3faa <watchdogConfig>
+    3e2a:	8e e0       	ldi	r24, 0x0E	; 14
+    3e2c:	c5 d0       	rcall	.+394    	; 0x3fb8 <watchdogConfig>
 
 #if (LED_START_FLASHES > 0) || defined(LED_DATA_FLASH)
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    3e2c:	25 9a       	sbi	0x04, 5	; 4
-    3e2e:	86 e0       	ldi	r24, 0x06	; 6
+    3e2e:	25 9a       	sbi	0x04, 5	; 4
+    3e30:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e30:	28 e1       	ldi	r18, 0x18	; 24
-    3e32:	3e ef       	ldi	r19, 0xFE	; 254
+    3e32:	28 e1       	ldi	r18, 0x18	; 24
+    3e34:	3e ef       	ldi	r19, 0xFE	; 254
     TIFR1 = _BV(TOV1);
-    3e34:	91 e0       	ldi	r25, 0x01	; 1
+    3e36:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e36:	30 93 85 00 	sts	0x0085, r19
-    3e3a:	20 93 84 00 	sts	0x0084, r18
+    3e38:	30 93 85 00 	sts	0x0085, r19
+    3e3c:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    3e3e:	96 bb       	out	0x16, r25	; 22
+    3e40:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    3e40:	b0 9b       	sbis	0x16, 0	; 22
-    3e42:	fe cf       	rjmp	.-4      	; 0x3e40 <main+0x40>
+    3e42:	b0 9b       	sbis	0x16, 0	; 22
+    3e44:	fe cf       	rjmp	.-4      	; 0x3e42 <main+0x42>
 #if defined(__AVR_ATmega8__)  || defined (__AVR_ATmega32__)
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
-    3e44:	1d 9a       	sbi	0x03, 5	; 3
+    3e46:	1d 9a       	sbi	0x03, 5	; 3
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3e46:	a8 95       	wdr
+    3e48:	a8 95       	wdr
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
 #endif
     watchdogReset();
   } while (--count);
-    3e48:	81 50       	subi	r24, 0x01	; 1
-    3e4a:	a9 f7       	brne	.-22     	; 0x3e36 <main+0x36>
-    3e4c:	ee 24       	eor	r14, r14
-    3e4e:	ff 24       	eor	r15, r15
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-        a |= (*bufPtr++) << 8;
-        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    3e50:	aa 24       	eor	r10, r10
-    3e52:	a3 94       	inc	r10
-        addrPtr += 2;
-      } while (--ch);
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    3e54:	b5 e0       	ldi	r27, 0x05	; 5
-    3e56:	cb 2e       	mov	r12, r27
-      boot_spm_busy_wait();
+    3e4a:	81 50       	subi	r24, 0x01	; 1
+    3e4c:	a9 f7       	brne	.-22     	; 0x3e38 <main+0x38>
+    3e4e:	aa 24       	eor	r10, r10
+    3e50:	bb 24       	eor	r11, r11
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    3e52:	33 e0       	ldi	r19, 0x03	; 3
+    3e54:	83 2e       	mov	r8, r19
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3e56:	77 24       	eor	r7, r7
+    3e58:	73 94       	inc	r7
+	    } while (len -= 2);
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    3e5a:	25 e0       	ldi	r18, 0x05	; 5
+    3e5c:	92 2e       	mov	r9, r18
+	    boot_spm_busy_wait();
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    3e58:	a1 e1       	ldi	r26, 0x11	; 17
-    3e5a:	ba 2e       	mov	r11, r26
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3e5c:	f3 e0       	ldi	r31, 0x03	; 3
-    3e5e:	df 2e       	mov	r13, r31
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    3e5e:	91 e1       	ldi	r25, 0x11	; 17
+    3e60:	c9 2e       	mov	r12, r25
 #endif
 
-  /* Forever loop */
+  /* Forever loop: exits by causing WDT reset */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e60:	98 d0       	rcall	.+304    	; 0x3f92 <getch>
+    3e62:	9e d0       	rcall	.+316    	; 0x3fa0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    3e62:	81 34       	cpi	r24, 0x41	; 65
-    3e64:	61 f4       	brne	.+24     	; 0x3e7e <main+0x7e>
+    3e64:	81 34       	cpi	r24, 0x41	; 65
+    3e66:	59 f4       	brne	.+22     	; 0x3e7e <main+0x7e>
       unsigned char which = getch();
-    3e66:	95 d0       	rcall	.+298    	; 0x3f92 <getch>
-    3e68:	08 2f       	mov	r16, r24
+    3e68:	9b d0       	rcall	.+310    	; 0x3fa0 <getch>
+    3e6a:	18 2f       	mov	r17, r24
       verifySpace();
-    3e6a:	a5 d0       	rcall	.+330    	; 0x3fb6 <verifySpace>
+    3e6c:	ab d0       	rcall	.+342    	; 0x3fc4 <verifySpace>
       if (which == 0x82) {
-    3e6c:	02 38       	cpi	r16, 0x82	; 130
-    3e6e:	29 f1       	breq	.+74     	; 0x3eba <main+0xba>
+    3e6e:	12 38       	cpi	r17, 0x82	; 130
+    3e70:	21 f1       	breq	.+72     	; 0x3eba <main+0xba>
 	/*
 	 * Send optiboot version as "minor SW version"
 	 */
 	putch(OPTIBOOT_MINVER);
       } else if (which == 0x81) {
-    3e70:	01 38       	cpi	r16, 0x81	; 129
-    3e72:	11 f4       	brne	.+4      	; 0x3e78 <main+0x78>
-	  putch(OPTIBOOT_MAJVER);
-    3e74:	85 e0       	ldi	r24, 0x05	; 5
-    3e76:	01 c0       	rjmp	.+2      	; 0x3e7a <main+0x7a>
+    3e72:	11 38       	cpi	r17, 0x81	; 129
+    3e74:	09 f4       	brne	.+2      	; 0x3e78 <main+0x78>
+    3e76:	82 c0       	rjmp	.+260    	; 0x3f7c <main+0x17c>
       } else {
 	/*
 	 * GET PARAMETER returns a generic 0x03 reply for
@@ -195,8 +194,8 @@ void watchdogReset() {
 	 */
 	putch(0x03);
     3e78:	83 e0       	ldi	r24, 0x03	; 3
-    3e7a:	83 d0       	rcall	.+262    	; 0x3f82 <putch>
-    3e7c:	7f c0       	rjmp	.+254    	; 0x3f7c <main+0x17c>
+    3e7a:	8a d0       	rcall	.+276    	; 0x3f90 <putch>
+    3e7c:	86 c0       	rjmp	.+268    	; 0x3f8a <main+0x18a>
       }
     }
     else if(ch == STK_SET_DEVICE) {
@@ -213,8 +212,8 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     3e8a:	85 e0       	ldi	r24, 0x05	; 5
-    3e8c:	9c d0       	rcall	.+312    	; 0x3fc6 <getNch>
-    3e8e:	76 c0       	rjmp	.+236    	; 0x3f7c <main+0x17c>
+    3e8c:	a3 d0       	rcall	.+326    	; 0x3fd4 <getNch>
+    3e8e:	7d c0       	rjmp	.+250    	; 0x3f8a <main+0x18a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     3e90:	85 35       	cpi	r24, 0x55	; 85
@@ -222,11 +221,11 @@ void watchdogReset() {
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    3e94:	7e d0       	rcall	.+252    	; 0x3f92 <getch>
+    3e94:	85 d0       	rcall	.+266    	; 0x3fa0 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
     3e96:	e8 2e       	mov	r14, r24
     3e98:	ff 24       	eor	r15, r15
-    3e9a:	7b d0       	rcall	.+246    	; 0x3f92 <getch>
+    3e9a:	82 d0       	rcall	.+260    	; 0x3fa0 <getch>
     3e9c:	08 2f       	mov	r16, r24
     3e9e:	10 e0       	ldi	r17, 0x00	; 0
     3ea0:	10 2f       	mov	r17, r16
@@ -242,9 +241,9 @@ void watchdogReset() {
     3eaa:	11 1f       	adc	r17, r17
       address = newAddress;
       verifySpace();
-    3eac:	84 d0       	rcall	.+264    	; 0x3fb6 <verifySpace>
-    3eae:	78 01       	movw	r14, r16
-    3eb0:	65 c0       	rjmp	.+202    	; 0x3f7c <main+0x17c>
+    3eac:	8b d0       	rcall	.+278    	; 0x3fc4 <verifySpace>
+    3eae:	58 01       	movw	r10, r16
+    3eb0:	6c c0       	rjmp	.+216    	; 0x3f8a <main+0x18a>
     }
     else if(ch == STK_UNIVERSAL) {
     3eb2:	86 35       	cpi	r24, 0x56	; 86
@@ -252,7 +251,7 @@ void watchdogReset() {
       // UNIVERSAL command is ignored
       getNch(4);
     3eb6:	84 e0       	ldi	r24, 0x04	; 4
-    3eb8:	86 d0       	rcall	.+268    	; 0x3fc6 <getNch>
+    3eb8:	8d d0       	rcall	.+282    	; 0x3fd4 <getNch>
       putch(0x00);
     3eba:	80 e0       	ldi	r24, 0x00	; 0
     3ebc:	de cf       	rjmp	.-68     	; 0x3e7a <main+0x7a>
@@ -261,326 +260,339 @@ void watchdogReset() {
     else if(ch == STK_PROG_PAGE) {
     3ebe:	84 36       	cpi	r24, 0x64	; 100
     3ec0:	09 f0       	breq	.+2      	; 0x3ec4 <main+0xc4>
-    3ec2:	40 c0       	rjmp	.+128    	; 0x3f44 <main+0x144>
+    3ec2:	41 c0       	rjmp	.+130    	; 0x3f46 <main+0x146>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
+      uint8_t desttype;
       uint8_t *bufPtr;
-      uint16_t addrPtr;
+      uint16_t savelength;
 
-      getch();			/* getlen() */
-    3ec4:	66 d0       	rcall	.+204    	; 0x3f92 <getch>
-      length = getch();
-    3ec6:	65 d0       	rcall	.+202    	; 0x3f92 <getch>
-    3ec8:	08 2f       	mov	r16, r24
-      getch();
-    3eca:	63 d0       	rcall	.+198    	; 0x3f92 <getch>
+      length = getch()<<8;			/* getlen() */
+    3ec4:	6d d0       	rcall	.+218    	; 0x3fa0 <getch>
+    3ec6:	90 e0       	ldi	r25, 0x00	; 0
+    3ec8:	18 2f       	mov	r17, r24
+    3eca:	00 27       	eor	r16, r16
+      length |= getch();
+    3ecc:	69 d0       	rcall	.+210    	; 0x3fa0 <getch>
+    3ece:	90 e0       	ldi	r25, 0x00	; 0
+    3ed0:	08 2b       	or	r16, r24
+    3ed2:	19 2b       	or	r17, r25
+      savelength = length;
+      desttype = getch();
+    3ed4:	65 d0       	rcall	.+202    	; 0x3fa0 <getch>
+    3ed6:	d8 2e       	mov	r13, r24
+    3ed8:	e8 01       	movw	r28, r16
+    3eda:	e1 2c       	mov	r14, r1
+    3edc:	f1 e0       	ldi	r31, 0x01	; 1
+    3ede:	ff 2e       	mov	r15, r31
 
-      // If we are in RWW section, immediately start page erase
-      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ecc:	80 e0       	ldi	r24, 0x00	; 0
-    3ece:	e8 16       	cp	r14, r24
-    3ed0:	88 e3       	ldi	r24, 0x38	; 56
-    3ed2:	f8 06       	cpc	r15, r24
-    3ed4:	18 f4       	brcc	.+6      	; 0x3edc <main+0xdc>
-    3ed6:	f7 01       	movw	r30, r14
-    3ed8:	d7 be       	out	0x37, r13	; 55
-    3eda:	e8 95       	spm
-    3edc:	c0 e0       	ldi	r28, 0x00	; 0
-    3ede:	d1 e0       	ldi	r29, 0x01	; 1
-
-      // While that is going on, read in page contents
+      // read a page worth of contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ee0:	58 d0       	rcall	.+176    	; 0x3f92 <getch>
-    3ee2:	89 93       	st	Y+, r24
+    3ee0:	5f d0       	rcall	.+190    	; 0x3fa0 <getch>
+    3ee2:	f7 01       	movw	r30, r14
+    3ee4:	81 93       	st	Z+, r24
+    3ee6:	7f 01       	movw	r14, r30
       while (--length);
-    3ee4:	0c 17       	cp	r16, r28
-    3ee6:	e1 f7       	brne	.-8      	; 0x3ee0 <main+0xe0>
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ee8:	f0 e0       	ldi	r31, 0x00	; 0
-    3eea:	ef 16       	cp	r14, r31
-    3eec:	f8 e3       	ldi	r31, 0x38	; 56
-    3eee:	ff 06       	cpc	r15, r31
-    3ef0:	18 f0       	brcs	.+6      	; 0x3ef8 <main+0xf8>
-    3ef2:	f7 01       	movw	r30, r14
-    3ef4:	d7 be       	out	0x37, r13	; 55
-    3ef6:	e8 95       	spm
+    3ee8:	21 97       	sbiw	r28, 0x01	; 1
+    3eea:	d1 f7       	brne	.-12     	; 0x3ee0 <main+0xe0>
 
       // Read command terminator, start reply
       verifySpace();
-    3ef8:	5e d0       	rcall	.+188    	; 0x3fb6 <verifySpace>
-
-      // If only a partial page is to be programmed, the erase might not be complete.
-      // So check that here
-      boot_spm_busy_wait();
-    3efa:	07 b6       	in	r0, 0x37	; 55
-    3efc:	00 fc       	sbrc	r0, 0
-    3efe:	fd cf       	rjmp	.-6      	; 0x3efa <main+0xfa>
-    3f00:	a7 01       	movw	r20, r14
-    3f02:	a0 e0       	ldi	r26, 0x00	; 0
-    3f04:	b1 e0       	ldi	r27, 0x01	; 1
-      bufPtr = buff;
-      addrPtr = (uint16_t)(void*)address;
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-    3f06:	2c 91       	ld	r18, X
-    3f08:	30 e0       	ldi	r19, 0x00	; 0
-        a |= (*bufPtr++) << 8;
-    3f0a:	11 96       	adiw	r26, 0x01	; 1
-    3f0c:	8c 91       	ld	r24, X
-    3f0e:	11 97       	sbiw	r26, 0x01	; 1
-    3f10:	90 e0       	ldi	r25, 0x00	; 0
-    3f12:	98 2f       	mov	r25, r24
-    3f14:	88 27       	eor	r24, r24
-    3f16:	82 2b       	or	r24, r18
-    3f18:	93 2b       	or	r25, r19
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+    3eec:	6b d0       	rcall	.+214    	; 0x3fc4 <verifySpace>
+ * void writebuffer(memtype, buffer, address, length)
+ */
+static inline void writebuffer(int8_t memtype, uint8_t *mybuff,
+			       uint16_t address, uint16_t len)
+{
+    switch (memtype) {
+    3eee:	f5 e4       	ldi	r31, 0x45	; 69
+    3ef0:	df 16       	cp	r13, r31
+    3ef2:	09 f4       	brne	.+2      	; 0x3ef6 <main+0xf6>
+    3ef4:	ff cf       	rjmp	.-2      	; 0x3ef4 <main+0xf4>
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    3ef6:	f5 01       	movw	r30, r10
+    3ef8:	87 be       	out	0x37, r8	; 55
+    3efa:	e8 95       	spm
+	    boot_spm_busy_wait();
+    3efc:	07 b6       	in	r0, 0x37	; 55
+    3efe:	00 fc       	sbrc	r0, 0
+    3f00:	fd cf       	rjmp	.-6      	; 0x3efc <main+0xfc>
+    3f02:	b5 01       	movw	r22, r10
+    3f04:	a8 01       	movw	r20, r16
+    3f06:	a0 e0       	ldi	r26, 0x00	; 0
+    3f08:	b1 e0       	ldi	r27, 0x01	; 1
+	    /*
+	     * Copy data from the buffer into the flash write buffer.
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+    3f0a:	2c 91       	ld	r18, X
+    3f0c:	30 e0       	ldi	r19, 0x00	; 0
+		a |= (*bufPtr++) << 8;
+    3f0e:	11 96       	adiw	r26, 0x01	; 1
+    3f10:	8c 91       	ld	r24, X
+    3f12:	11 97       	sbiw	r26, 0x01	; 1
+    3f14:	90 e0       	ldi	r25, 0x00	; 0
+    3f16:	98 2f       	mov	r25, r24
+    3f18:	88 27       	eor	r24, r24
+    3f1a:	82 2b       	or	r24, r18
+    3f1c:	93 2b       	or	r25, r19
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
-    3f1a:	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);
-    3f1c:	fa 01       	movw	r30, r20
-    3f1e:	0c 01       	movw	r0, r24
-    3f20:	a7 be       	out	0x37, r10	; 55
-    3f22:	e8 95       	spm
-    3f24:	11 24       	eor	r1, r1
-        addrPtr += 2;
-    3f26:	4e 5f       	subi	r20, 0xFE	; 254
-    3f28:	5f 4f       	sbci	r21, 0xFF	; 255
-      } while (--ch);
-    3f2a:	f1 e0       	ldi	r31, 0x01	; 1
-    3f2c:	a0 38       	cpi	r26, 0x80	; 128
-    3f2e:	bf 07       	cpc	r27, r31
-    3f30:	51 f7       	brne	.-44     	; 0x3f06 <main+0x106>
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    3f32:	f7 01       	movw	r30, r14
-    3f34:	c7 be       	out	0x37, r12	; 55
-    3f36:	e8 95       	spm
-      boot_spm_busy_wait();
-    3f38:	07 b6       	in	r0, 0x37	; 55
-    3f3a:	00 fc       	sbrc	r0, 0
-    3f3c:	fd cf       	rjmp	.-6      	; 0x3f38 <main+0x138>
+    3f1e:	12 96       	adiw	r26, 0x02	; 2
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3f20:	fb 01       	movw	r30, r22
+    3f22:	0c 01       	movw	r0, r24
+    3f24:	77 be       	out	0x37, r7	; 55
+    3f26:	e8 95       	spm
+    3f28:	11 24       	eor	r1, r1
+		addrPtr += 2;
+    3f2a:	6e 5f       	subi	r22, 0xFE	; 254
+    3f2c:	7f 4f       	sbci	r23, 0xFF	; 255
+	    } while (len -= 2);
+    3f2e:	42 50       	subi	r20, 0x02	; 2
+    3f30:	50 40       	sbci	r21, 0x00	; 0
+    3f32:	59 f7       	brne	.-42     	; 0x3f0a <main+0x10a>
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    3f34:	f5 01       	movw	r30, r10
+    3f36:	97 be       	out	0x37, r9	; 55
+    3f38:	e8 95       	spm
+	    boot_spm_busy_wait();
+    3f3a:	07 b6       	in	r0, 0x37	; 55
+    3f3c:	00 fc       	sbrc	r0, 0
+    3f3e:	fd cf       	rjmp	.-6      	; 0x3f3a <main+0x13a>
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    3f3e:	b7 be       	out	0x37, r11	; 55
-    3f40:	e8 95       	spm
-    3f42:	1c c0       	rjmp	.+56     	; 0x3f7c <main+0x17c>
-#endif
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    3f40:	c7 be       	out	0x37, r12	; 55
+    3f42:	e8 95       	spm
+    3f44:	22 c0       	rjmp	.+68     	; 0x3f8a <main+0x18a>
+      writebuffer(desttype, buff, address, savelength);
+
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f44:	84 37       	cpi	r24, 0x74	; 116
-    3f46:	61 f4       	brne	.+24     	; 0x3f60 <main+0x160>
-      // READ PAGE - we only read flash
-      getch();			/* getlen() */
-    3f48:	24 d0       	rcall	.+72     	; 0x3f92 <getch>
-      length = getch();
-    3f4a:	23 d0       	rcall	.+70     	; 0x3f92 <getch>
-    3f4c:	08 2f       	mov	r16, r24
-      getch();
-    3f4e:	21 d0       	rcall	.+66     	; 0x3f92 <getch>
+    3f46:	84 37       	cpi	r24, 0x74	; 116
+    3f48:	91 f4       	brne	.+36     	; 0x3f6e <main+0x16e>
+      uint8_t desttype;
+      length = getch()<<8;			/* getlen() */
+    3f4a:	2a d0       	rcall	.+84     	; 0x3fa0 <getch>
+    3f4c:	90 e0       	ldi	r25, 0x00	; 0
+    3f4e:	d8 2f       	mov	r29, r24
+    3f50:	cc 27       	eor	r28, r28
+      length |= getch();
+    3f52:	26 d0       	rcall	.+76     	; 0x3fa0 <getch>
+    3f54:	90 e0       	ldi	r25, 0x00	; 0
+    3f56:	c8 2b       	or	r28, r24
+    3f58:	d9 2b       	or	r29, r25
+      desttype = getch();
+    3f5a:	22 d0       	rcall	.+68     	; 0x3fa0 <getch>
 
       verifySpace();
-    3f50:	32 d0       	rcall	.+100    	; 0x3fb6 <verifySpace>
-        __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+    3f5c:	33 d0       	rcall	.+102    	; 0x3fc4 <verifySpace>
+    3f5e:	85 01       	movw	r16, r10
+	    __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #else
-        // read a Flash byte and increment the address
-        __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+	    // read a Flash byte and increment the address
+	    __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #endif
-        putch(ch);
-    3f52:	f7 01       	movw	r30, r14
-    3f54:	85 91       	lpm	r24, Z+
-    3f56:	7f 01       	movw	r14, r30
-    3f58:	14 d0       	rcall	.+40     	; 0x3f82 <putch>
-      } while (--length);
-    3f5a:	01 50       	subi	r16, 0x01	; 1
-    3f5c:	d1 f7       	brne	.-12     	; 0x3f52 <main+0x152>
-    3f5e:	0e c0       	rjmp	.+28     	; 0x3f7c <main+0x17c>
+	    putch(ch);
+    3f60:	f8 01       	movw	r30, r16
+    3f62:	85 91       	lpm	r24, Z+
+    3f64:	8f 01       	movw	r16, r30
+    3f66:	14 d0       	rcall	.+40     	; 0x3f90 <putch>
+	} while (--length);
+    3f68:	21 97       	sbiw	r28, 0x01	; 1
+    3f6a:	d1 f7       	brne	.-12     	; 0x3f60 <main+0x160>
+    3f6c:	0e c0       	rjmp	.+28     	; 0x3f8a <main+0x18a>
+	  
+      read_mem(desttype, address, length);
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f60:	85 37       	cpi	r24, 0x75	; 117
-    3f62:	39 f4       	brne	.+14     	; 0x3f72 <main+0x172>
+    3f6e:	85 37       	cpi	r24, 0x75	; 117
+    3f70:	39 f4       	brne	.+14     	; 0x3f80 <main+0x180>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f64:	28 d0       	rcall	.+80     	; 0x3fb6 <verifySpace>
+    3f72:	28 d0       	rcall	.+80     	; 0x3fc4 <verifySpace>
       putch(SIGNATURE_0);
-    3f66:	8e e1       	ldi	r24, 0x1E	; 30
-    3f68:	0c d0       	rcall	.+24     	; 0x3f82 <putch>
+    3f74:	8e e1       	ldi	r24, 0x1E	; 30
+    3f76:	0c d0       	rcall	.+24     	; 0x3f90 <putch>
       putch(SIGNATURE_1);
-    3f6a:	84 e9       	ldi	r24, 0x94	; 148
-    3f6c:	0a d0       	rcall	.+20     	; 0x3f82 <putch>
+    3f78:	84 e9       	ldi	r24, 0x94	; 148
+    3f7a:	0a d0       	rcall	.+20     	; 0x3f90 <putch>
       putch(SIGNATURE_2);
-    3f6e:	86 e0       	ldi	r24, 0x06	; 6
-    3f70:	84 cf       	rjmp	.-248    	; 0x3e7a <main+0x7a>
+    3f7c:	86 e0       	ldi	r24, 0x06	; 6
+    3f7e:	7d cf       	rjmp	.-262    	; 0x3e7a <main+0x7a>
     }
     else if (ch == STK_LEAVE_PROGMODE) { /* 'Q' */
-    3f72:	81 35       	cpi	r24, 0x51	; 81
-    3f74:	11 f4       	brne	.+4      	; 0x3f7a <main+0x17a>
+    3f80:	81 35       	cpi	r24, 0x51	; 81
+    3f82:	11 f4       	brne	.+4      	; 0x3f88 <main+0x188>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f76:	88 e0       	ldi	r24, 0x08	; 8
-    3f78:	18 d0       	rcall	.+48     	; 0x3faa <watchdogConfig>
+    3f84:	88 e0       	ldi	r24, 0x08	; 8
+    3f86:	18 d0       	rcall	.+48     	; 0x3fb8 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f7a:	1d d0       	rcall	.+58     	; 0x3fb6 <verifySpace>
+    3f88:	1d d0       	rcall	.+58     	; 0x3fc4 <verifySpace>
     }
     putch(STK_OK);
-    3f7c:	80 e1       	ldi	r24, 0x10	; 16
-    3f7e:	01 d0       	rcall	.+2      	; 0x3f82 <putch>
-    3f80:	6f cf       	rjmp	.-290    	; 0x3e60 <main+0x60>
+    3f8a:	80 e1       	ldi	r24, 0x10	; 16
+    3f8c:	01 d0       	rcall	.+2      	; 0x3f90 <putch>
+    3f8e:	69 cf       	rjmp	.-302    	; 0x3e62 <main+0x62>
 
-00003f82 <putch>:
+00003f90 <putch>:
   }
 }
 
 void putch(char ch) {
-    3f82:	98 2f       	mov	r25, r24
+    3f90:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UART_SRA & _BV(UDRE0)));
-    3f84:	80 91 c0 00 	lds	r24, 0x00C0
-    3f88:	85 ff       	sbrs	r24, 5
-    3f8a:	fc cf       	rjmp	.-8      	; 0x3f84 <putch+0x2>
+    3f92:	80 91 c0 00 	lds	r24, 0x00C0
+    3f96:	85 ff       	sbrs	r24, 5
+    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <putch+0x2>
   UART_UDR = ch;
-    3f8c:	90 93 c6 00 	sts	0x00C6, r25
+    3f9a:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3f90:	08 95       	ret
+    3f9e:	08 95       	ret
 
-00003f92 <getch>:
+00003fa0 <getch>:
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UART_SRA & _BV(RXC0)))
-    3f92:	80 91 c0 00 	lds	r24, 0x00C0
-    3f96:	87 ff       	sbrs	r24, 7
-    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <getch>
+    3fa0:	80 91 c0 00 	lds	r24, 0x00C0
+    3fa4:	87 ff       	sbrs	r24, 7
+    3fa6:	fc cf       	rjmp	.-8      	; 0x3fa0 <getch>
     ;
   if (!(UART_SRA & _BV(FE0))) {
-    3f9a:	80 91 c0 00 	lds	r24, 0x00C0
-    3f9e:	84 fd       	sbrc	r24, 4
-    3fa0:	01 c0       	rjmp	.+2      	; 0x3fa4 <getch+0x12>
+    3fa8:	80 91 c0 00 	lds	r24, 0x00C0
+    3fac:	84 fd       	sbrc	r24, 4
+    3fae:	01 c0       	rjmp	.+2      	; 0x3fb2 <getch+0x12>
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fa2:	a8 95       	wdr
+    3fb0:	a8 95       	wdr
        * don't care that an invalid char is returned...)
        */
     watchdogReset();
   }
   
   ch = UART_UDR;
-    3fa4:	80 91 c6 00 	lds	r24, 0x00C6
+    3fb2:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    3fa8:	08 95       	ret
+    3fb6:	08 95       	ret
 
-00003faa <watchdogConfig>:
+00003fb8 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3faa:	e0 e6       	ldi	r30, 0x60	; 96
-    3fac:	f0 e0       	ldi	r31, 0x00	; 0
-    3fae:	98 e1       	ldi	r25, 0x18	; 24
-    3fb0:	90 83       	st	Z, r25
+    3fb8:	e0 e6       	ldi	r30, 0x60	; 96
+    3fba:	f0 e0       	ldi	r31, 0x00	; 0
+    3fbc:	98 e1       	ldi	r25, 0x18	; 24
+    3fbe:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fb2:	80 83       	st	Z, r24
+    3fc0:	80 83       	st	Z, r24
 }
-    3fb4:	08 95       	ret
+    3fc2:	08 95       	ret
 
-00003fb6 <verifySpace>:
+00003fc4 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) {
-    3fb6:	ed df       	rcall	.-38     	; 0x3f92 <getch>
-    3fb8:	80 32       	cpi	r24, 0x20	; 32
-    3fba:	19 f0       	breq	.+6      	; 0x3fc2 <verifySpace+0xc>
+    3fc4:	ed df       	rcall	.-38     	; 0x3fa0 <getch>
+    3fc6:	80 32       	cpi	r24, 0x20	; 32
+    3fc8:	19 f0       	breq	.+6      	; 0x3fd0 <verifySpace+0xc>
     watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
-    3fbc:	88 e0       	ldi	r24, 0x08	; 8
-    3fbe:	f5 df       	rcall	.-22     	; 0x3faa <watchdogConfig>
-    3fc0:	ff cf       	rjmp	.-2      	; 0x3fc0 <verifySpace+0xa>
+    3fca:	88 e0       	ldi	r24, 0x08	; 8
+    3fcc:	f5 df       	rcall	.-22     	; 0x3fb8 <watchdogConfig>
+    3fce:	ff cf       	rjmp	.-2      	; 0x3fce <verifySpace+0xa>
     while (1)			      // and busy-loop so that WD causes
       ;				      //  a reset and app start.
   }
   putch(STK_INSYNC);
-    3fc2:	84 e1       	ldi	r24, 0x14	; 20
+    3fd0:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fc4:	de cf       	rjmp	.-68     	; 0x3f82 <putch>
+    3fd2:	de cf       	rjmp	.-68     	; 0x3f90 <putch>
 
-00003fc6 <getNch>:
+00003fd4 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fc6:	1f 93       	push	r17
-    3fc8:	18 2f       	mov	r17, r24
+    3fd4:	1f 93       	push	r17
+    3fd6:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3fca:	e3 df       	rcall	.-58     	; 0x3f92 <getch>
-    3fcc:	11 50       	subi	r17, 0x01	; 1
-    3fce:	e9 f7       	brne	.-6      	; 0x3fca <getNch+0x4>
+    3fd8:	e3 df       	rcall	.-58     	; 0x3fa0 <getch>
+    3fda:	11 50       	subi	r17, 0x01	; 1
+    3fdc:	e9 f7       	brne	.-6      	; 0x3fd8 <getNch+0x4>
   verifySpace();
-    3fd0:	f2 df       	rcall	.-28     	; 0x3fb6 <verifySpace>
+    3fde:	f2 df       	rcall	.-28     	; 0x3fc4 <verifySpace>
 }
-    3fd2:	1f 91       	pop	r17
-    3fd4:	08 95       	ret
+    3fe0:	1f 91       	pop	r17
+    3fe2:	08 95       	ret
 
-00003fd6 <appStart>:
+00003fe4 <appStart>:
 
 void appStart(uint8_t rstFlags) {
   // save the reset flags in the designated register
   //  This can be saved in a main program by putting code in .init0 (which
   //  executes before normal c init code) to save R2 to a global variable.
   __asm__ __volatile__ ("mov r2, %0\n" :: "r" (rstFlags));
-    3fd6:	28 2e       	mov	r2, r24
+    3fe4:	28 2e       	mov	r2, r24
 
   watchdogConfig(WATCHDOG_OFF);
-    3fd8:	80 e0       	ldi	r24, 0x00	; 0
-    3fda:	e7 df       	rcall	.-50     	; 0x3faa <watchdogConfig>
+    3fe6:	80 e0       	ldi	r24, 0x00	; 0
+    3fe8:	e7 df       	rcall	.-50     	; 0x3fb8 <watchdogConfig>
   __asm__ __volatile__ (
-    3fdc:	ee 27       	eor	r30, r30
-    3fde:	ff 27       	eor	r31, r31
-    3fe0:	09 94       	ijmp
+    3fea:	ee 27       	eor	r30, r30
+    3fec:	ff 27       	eor	r31, r31
+    3fee:	09 94       	ijmp
diff --git a/optiboot/bootloaders/optiboot/optiboot_pro_16MHz.hex b/optiboot/bootloaders/optiboot/optiboot_pro_16MHz.hex
index e4457f6..a151198 100644
--- a/optiboot/bootloaders/optiboot/optiboot_pro_16MHz.hex
+++ b/optiboot/bootloaders/optiboot/optiboot_pro_16MHz.hex
@@ -1,34 +1,34 @@
-:103E0000112484B714BE81FFE6D085E08093810041
-:103E100082E08093C00088E18093C10086E08093B7
-:103E2000C20080E18093C4008EE0BFD0259A86E076
-:103E300020E33CEF91E0309385002093840096BB13
-:103E4000B09BFECF1D9AA8958150A9F7EE24FF24C0
-:103E5000AA24A394B5E0CB2EA1E1BA2EF3E0DF2E85
-:103E600098D0813461F495D0082FA5D0023829F17B
-:103E7000013811F485E001C083E083D07FC0823433
-:103E800011F484E103C0853419F485E09CD076C038
-:103E9000853579F47ED0E82EFF247BD0082F10E002
-:103EA000102F00270E291F29000F111F84D0780121
-:103EB00065C0863521F484E086D080E0DECF84368C
-:103EC00009F040C066D065D0082F63D080E0E816C6
-:103ED00088E3F80618F4F701D7BEE895C0E0D1E012
-:103EE00058D089930C17E1F7F0E0EF16F8E3FF06DE
-:103EF00018F0F701D7BEE8955ED007B600FCFDCFFD
-:103F0000A701A0E0B1E02C9130E011968C911197BF
-:103F100090E0982F8827822B932B1296FA010C01A0
-:103F2000A7BEE89511244E5F5F4FF1E0A038BF07B0
-:103F300051F7F701C7BEE89507B600FCFDCFB7BE45
-:103F4000E8951CC0843761F424D023D0082F21D0F9
-:103F500032D0F70185917F0114D00150D1F70EC006
-:103F6000853739F428D08EE10CD084E90AD086E078
-:103F700084CF813511F488E018D01DD080E101D0C4
-:103F80006FCF982F8091C00085FFFCCF9093C60023
-:103F900008958091C00087FFFCCF8091C00084FD10
-:103FA00001C0A8958091C6000895E0E6F0E098E190
-:103FB000908380830895EDDF803219F088E0F5DF8B
-:103FC000FFCF84E1DECF1F93182FE3DF1150E9F715
-:103FD000F2DF1F910895282E80E0E7DFEE27FF270C
-:023FE000099442
-:023FFE000005BC
+:103E0000112484B714BE81FD01C0ECD085E08093FD
+:103E1000810082E08093C00088E18093C10086E049
+:103E20008093C20080E18093C4008EE0C5D0259AC3
+:103E300086E020E33CEF91E03093850020938400FE
+:103E400096BBB09BFECF1D9AA8958150A9F7AA24D6
+:103E5000BB2433E0832E7724739425E0922E91E1E6
+:103E6000C92E9ED0813459F49BD0182FABD0123874
+:103E700021F1113809F482C083E08AD086C08234EF
+:103E800011F484E103C0853419F485E0A3D07DC02A
+:103E9000853579F485D0E82EFF2482D0082F10E0F4
+:103EA000102F00270E291F29000F111F8BD058013A
+:103EB0006CC0863521F484E08DD080E0DECF84367E
+:103EC00009F041C06DD090E0182F002769D090E034
+:103ED000082B192B65D0D82EE801E12CF1E0FF2E3C
+:103EE0005FD0F70181937F012197D1F76BD0F5E483
+:103EF000DF1609F4FFCFF50187BEE89507B600FC91
+:103F0000FDCFB501A801A0E0B1E02C9130E0119601
+:103F10008C91119790E0982F8827822B932B1296E3
+:103F2000FB010C0177BEE89511246E5F7F4F425074
+:103F3000504059F7F50197BEE89507B600FCFDCF54
+:103F4000C7BEE89522C0843791F42AD090E0D82FDC
+:103F5000CC2726D090E0C82BD92B22D033D0850196
+:103F6000F80185918F0114D02197D1F70EC08537C4
+:103F700039F428D08EE10CD084E90AD086E07DCFD8
+:103F8000813511F488E018D01DD080E101D069CFCF
+:103F9000982F8091C00085FFFCCF9093C6000895B4
+:103FA0008091C00087FFFCCF8091C00084FD01C0DC
+:103FB000A8958091C6000895E0E6F0E098E190832E
+:103FC00080830895EDDF803219F088E0F5DFFFCFC0
+:103FD00084E1DECF1F93182FE3DF1150E9F7F2DF02
+:103FE0001F910895282E80E0E7DFEE27FF27099430
+:023FFE000006BB
 :0400000300003E00BB
 :00000001FF
diff --git a/optiboot/bootloaders/optiboot/optiboot_pro_16MHz.lst b/optiboot/bootloaders/optiboot/optiboot_pro_16MHz.lst
index 6f31a80..c2b08ef 100644
--- a/optiboot/bootloaders/optiboot/optiboot_pro_16MHz.lst
+++ b/optiboot/bootloaders/optiboot/optiboot_pro_16MHz.lst
@@ -3,36 +3,36 @@ optiboot_atmega168.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001e2  00003e00  00003e00  00000054  2**1
+  0 .text         000001f0  00003e00  00003e00  00000074  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00003ffe  00003ffe  00000236  2**0
-                  CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000238  2**0
+  1 .version      00000002  00003ffe  00003ffe  00000264  2**0
+                  CONTENTS, ALLOC, LOAD, DATA
+  2 .debug_aranges 00000028  00000000  00000000  00000266  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000005f  00000000  00000000  00000260  2**0
+  3 .debug_pubnames 00000074  00000000  00000000  0000028e  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000294  00000000  00000000  000002bf  2**0
+  4 .debug_info   000003db  00000000  00000000  00000302  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000016b  00000000  00000000  00000553  2**0
+  5 .debug_abbrev 000001ea  00000000  00000000  000006dd  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000421  00000000  00000000  000006be  2**0
+  6 .debug_line   00000423  00000000  00000000  000008c7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000080  00000000  00000000  00000ae0  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000cec  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000131  00000000  00000000  00000b60  2**0
+  8 .debug_str    00000172  00000000  00000000  00000d6c  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    00000266  00000000  00000000  00000c91  2**0
+  9 .debug_loc    000002d7  00000000  00000000  00000ede  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000078  00000000  00000000  00000ef7  2**0
+ 10 .debug_ranges 000000b8  00000000  00000000  000011b5  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00003e00 <main>:
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
     3e00:	11 24       	eor	r1, r1
@@ -46,148 +46,147 @@ int main(void) {
   MCUSR = 0;
     3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart(ch);
-    3e06:	81 ff       	sbrs	r24, 1
-    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+    3e06:	81 fd       	sbrc	r24, 1
+    3e08:	01 c0       	rjmp	.+2      	; 0x3e0c <main+0xc>
+    3e0a:	ec d0       	rcall	.+472    	; 0x3fe4 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-    3e0a:	85 e0       	ldi	r24, 0x05	; 5
-    3e0c:	80 93 81 00 	sts	0x0081, r24
+    3e0c:	85 e0       	ldi	r24, 0x05	; 5
+    3e0e:	80 93 81 00 	sts	0x0081, r24
   UCSRA = _BV(U2X); //Double speed mode USART
   UCSRB = _BV(RXEN) | _BV(TXEN);  // enable Rx & Tx
   UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0);  // config USART; 8N1
   UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
 #else
   UART_SRA = _BV(U2X0); //Double speed mode USART0
-    3e10:	82 e0       	ldi	r24, 0x02	; 2
-    3e12:	80 93 c0 00 	sts	0x00C0, r24
+    3e12:	82 e0       	ldi	r24, 0x02	; 2
+    3e14:	80 93 c0 00 	sts	0x00C0, r24
   UART_SRB = _BV(RXEN0) | _BV(TXEN0);
-    3e16:	88 e1       	ldi	r24, 0x18	; 24
-    3e18:	80 93 c1 00 	sts	0x00C1, r24
+    3e18:	88 e1       	ldi	r24, 0x18	; 24
+    3e1a:	80 93 c1 00 	sts	0x00C1, r24
   UART_SRC = _BV(UCSZ00) | _BV(UCSZ01);
-    3e1c:	86 e0       	ldi	r24, 0x06	; 6
-    3e1e:	80 93 c2 00 	sts	0x00C2, r24
+    3e1e:	86 e0       	ldi	r24, 0x06	; 6
+    3e20:	80 93 c2 00 	sts	0x00C2, r24
   UART_SRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    3e22:	80 e1       	ldi	r24, 0x10	; 16
-    3e24:	80 93 c4 00 	sts	0x00C4, r24
+    3e24:	80 e1       	ldi	r24, 0x10	; 16
+    3e26:	80 93 c4 00 	sts	0x00C4, r24
 #endif
 #endif
 
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
-    3e28:	8e e0       	ldi	r24, 0x0E	; 14
-    3e2a:	bf d0       	rcall	.+382    	; 0x3faa <watchdogConfig>
+    3e2a:	8e e0       	ldi	r24, 0x0E	; 14
+    3e2c:	c5 d0       	rcall	.+394    	; 0x3fb8 <watchdogConfig>
 
 #if (LED_START_FLASHES > 0) || defined(LED_DATA_FLASH)
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    3e2c:	25 9a       	sbi	0x04, 5	; 4
-    3e2e:	86 e0       	ldi	r24, 0x06	; 6
+    3e2e:	25 9a       	sbi	0x04, 5	; 4
+    3e30:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e30:	20 e3       	ldi	r18, 0x30	; 48
-    3e32:	3c ef       	ldi	r19, 0xFC	; 252
+    3e32:	20 e3       	ldi	r18, 0x30	; 48
+    3e34:	3c ef       	ldi	r19, 0xFC	; 252
     TIFR1 = _BV(TOV1);
-    3e34:	91 e0       	ldi	r25, 0x01	; 1
+    3e36:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e36:	30 93 85 00 	sts	0x0085, r19
-    3e3a:	20 93 84 00 	sts	0x0084, r18
+    3e38:	30 93 85 00 	sts	0x0085, r19
+    3e3c:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    3e3e:	96 bb       	out	0x16, r25	; 22
+    3e40:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    3e40:	b0 9b       	sbis	0x16, 0	; 22
-    3e42:	fe cf       	rjmp	.-4      	; 0x3e40 <main+0x40>
+    3e42:	b0 9b       	sbis	0x16, 0	; 22
+    3e44:	fe cf       	rjmp	.-4      	; 0x3e42 <main+0x42>
 #if defined(__AVR_ATmega8__)  || defined (__AVR_ATmega32__)
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
-    3e44:	1d 9a       	sbi	0x03, 5	; 3
+    3e46:	1d 9a       	sbi	0x03, 5	; 3
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3e46:	a8 95       	wdr
+    3e48:	a8 95       	wdr
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
 #endif
     watchdogReset();
   } while (--count);
-    3e48:	81 50       	subi	r24, 0x01	; 1
-    3e4a:	a9 f7       	brne	.-22     	; 0x3e36 <main+0x36>
-    3e4c:	ee 24       	eor	r14, r14
-    3e4e:	ff 24       	eor	r15, r15
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-        a |= (*bufPtr++) << 8;
-        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    3e50:	aa 24       	eor	r10, r10
-    3e52:	a3 94       	inc	r10
-        addrPtr += 2;
-      } while (--ch);
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    3e54:	b5 e0       	ldi	r27, 0x05	; 5
-    3e56:	cb 2e       	mov	r12, r27
-      boot_spm_busy_wait();
+    3e4a:	81 50       	subi	r24, 0x01	; 1
+    3e4c:	a9 f7       	brne	.-22     	; 0x3e38 <main+0x38>
+    3e4e:	aa 24       	eor	r10, r10
+    3e50:	bb 24       	eor	r11, r11
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    3e52:	33 e0       	ldi	r19, 0x03	; 3
+    3e54:	83 2e       	mov	r8, r19
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3e56:	77 24       	eor	r7, r7
+    3e58:	73 94       	inc	r7
+	    } while (len -= 2);
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    3e5a:	25 e0       	ldi	r18, 0x05	; 5
+    3e5c:	92 2e       	mov	r9, r18
+	    boot_spm_busy_wait();
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    3e58:	a1 e1       	ldi	r26, 0x11	; 17
-    3e5a:	ba 2e       	mov	r11, r26
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3e5c:	f3 e0       	ldi	r31, 0x03	; 3
-    3e5e:	df 2e       	mov	r13, r31
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    3e5e:	91 e1       	ldi	r25, 0x11	; 17
+    3e60:	c9 2e       	mov	r12, r25
 #endif
 
-  /* Forever loop */
+  /* Forever loop: exits by causing WDT reset */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e60:	98 d0       	rcall	.+304    	; 0x3f92 <getch>
+    3e62:	9e d0       	rcall	.+316    	; 0x3fa0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    3e62:	81 34       	cpi	r24, 0x41	; 65
-    3e64:	61 f4       	brne	.+24     	; 0x3e7e <main+0x7e>
+    3e64:	81 34       	cpi	r24, 0x41	; 65
+    3e66:	59 f4       	brne	.+22     	; 0x3e7e <main+0x7e>
       unsigned char which = getch();
-    3e66:	95 d0       	rcall	.+298    	; 0x3f92 <getch>
-    3e68:	08 2f       	mov	r16, r24
+    3e68:	9b d0       	rcall	.+310    	; 0x3fa0 <getch>
+    3e6a:	18 2f       	mov	r17, r24
       verifySpace();
-    3e6a:	a5 d0       	rcall	.+330    	; 0x3fb6 <verifySpace>
+    3e6c:	ab d0       	rcall	.+342    	; 0x3fc4 <verifySpace>
       if (which == 0x82) {
-    3e6c:	02 38       	cpi	r16, 0x82	; 130
-    3e6e:	29 f1       	breq	.+74     	; 0x3eba <main+0xba>
+    3e6e:	12 38       	cpi	r17, 0x82	; 130
+    3e70:	21 f1       	breq	.+72     	; 0x3eba <main+0xba>
 	/*
 	 * Send optiboot version as "minor SW version"
 	 */
 	putch(OPTIBOOT_MINVER);
       } else if (which == 0x81) {
-    3e70:	01 38       	cpi	r16, 0x81	; 129
-    3e72:	11 f4       	brne	.+4      	; 0x3e78 <main+0x78>
-	  putch(OPTIBOOT_MAJVER);
-    3e74:	85 e0       	ldi	r24, 0x05	; 5
-    3e76:	01 c0       	rjmp	.+2      	; 0x3e7a <main+0x7a>
+    3e72:	11 38       	cpi	r17, 0x81	; 129
+    3e74:	09 f4       	brne	.+2      	; 0x3e78 <main+0x78>
+    3e76:	82 c0       	rjmp	.+260    	; 0x3f7c <main+0x17c>
       } else {
 	/*
 	 * GET PARAMETER returns a generic 0x03 reply for
@@ -195,8 +194,8 @@ void watchdogReset() {
 	 */
 	putch(0x03);
     3e78:	83 e0       	ldi	r24, 0x03	; 3
-    3e7a:	83 d0       	rcall	.+262    	; 0x3f82 <putch>
-    3e7c:	7f c0       	rjmp	.+254    	; 0x3f7c <main+0x17c>
+    3e7a:	8a d0       	rcall	.+276    	; 0x3f90 <putch>
+    3e7c:	86 c0       	rjmp	.+268    	; 0x3f8a <main+0x18a>
       }
     }
     else if(ch == STK_SET_DEVICE) {
@@ -213,8 +212,8 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     3e8a:	85 e0       	ldi	r24, 0x05	; 5
-    3e8c:	9c d0       	rcall	.+312    	; 0x3fc6 <getNch>
-    3e8e:	76 c0       	rjmp	.+236    	; 0x3f7c <main+0x17c>
+    3e8c:	a3 d0       	rcall	.+326    	; 0x3fd4 <getNch>
+    3e8e:	7d c0       	rjmp	.+250    	; 0x3f8a <main+0x18a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     3e90:	85 35       	cpi	r24, 0x55	; 85
@@ -222,11 +221,11 @@ void watchdogReset() {
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    3e94:	7e d0       	rcall	.+252    	; 0x3f92 <getch>
+    3e94:	85 d0       	rcall	.+266    	; 0x3fa0 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
     3e96:	e8 2e       	mov	r14, r24
     3e98:	ff 24       	eor	r15, r15
-    3e9a:	7b d0       	rcall	.+246    	; 0x3f92 <getch>
+    3e9a:	82 d0       	rcall	.+260    	; 0x3fa0 <getch>
     3e9c:	08 2f       	mov	r16, r24
     3e9e:	10 e0       	ldi	r17, 0x00	; 0
     3ea0:	10 2f       	mov	r17, r16
@@ -242,9 +241,9 @@ void watchdogReset() {
     3eaa:	11 1f       	adc	r17, r17
       address = newAddress;
       verifySpace();
-    3eac:	84 d0       	rcall	.+264    	; 0x3fb6 <verifySpace>
-    3eae:	78 01       	movw	r14, r16
-    3eb0:	65 c0       	rjmp	.+202    	; 0x3f7c <main+0x17c>
+    3eac:	8b d0       	rcall	.+278    	; 0x3fc4 <verifySpace>
+    3eae:	58 01       	movw	r10, r16
+    3eb0:	6c c0       	rjmp	.+216    	; 0x3f8a <main+0x18a>
     }
     else if(ch == STK_UNIVERSAL) {
     3eb2:	86 35       	cpi	r24, 0x56	; 86
@@ -252,7 +251,7 @@ void watchdogReset() {
       // UNIVERSAL command is ignored
       getNch(4);
     3eb6:	84 e0       	ldi	r24, 0x04	; 4
-    3eb8:	86 d0       	rcall	.+268    	; 0x3fc6 <getNch>
+    3eb8:	8d d0       	rcall	.+282    	; 0x3fd4 <getNch>
       putch(0x00);
     3eba:	80 e0       	ldi	r24, 0x00	; 0
     3ebc:	de cf       	rjmp	.-68     	; 0x3e7a <main+0x7a>
@@ -261,326 +260,339 @@ void watchdogReset() {
     else if(ch == STK_PROG_PAGE) {
     3ebe:	84 36       	cpi	r24, 0x64	; 100
     3ec0:	09 f0       	breq	.+2      	; 0x3ec4 <main+0xc4>
-    3ec2:	40 c0       	rjmp	.+128    	; 0x3f44 <main+0x144>
+    3ec2:	41 c0       	rjmp	.+130    	; 0x3f46 <main+0x146>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
+      uint8_t desttype;
       uint8_t *bufPtr;
-      uint16_t addrPtr;
+      uint16_t savelength;
 
-      getch();			/* getlen() */
-    3ec4:	66 d0       	rcall	.+204    	; 0x3f92 <getch>
-      length = getch();
-    3ec6:	65 d0       	rcall	.+202    	; 0x3f92 <getch>
-    3ec8:	08 2f       	mov	r16, r24
-      getch();
-    3eca:	63 d0       	rcall	.+198    	; 0x3f92 <getch>
+      length = getch()<<8;			/* getlen() */
+    3ec4:	6d d0       	rcall	.+218    	; 0x3fa0 <getch>
+    3ec6:	90 e0       	ldi	r25, 0x00	; 0
+    3ec8:	18 2f       	mov	r17, r24
+    3eca:	00 27       	eor	r16, r16
+      length |= getch();
+    3ecc:	69 d0       	rcall	.+210    	; 0x3fa0 <getch>
+    3ece:	90 e0       	ldi	r25, 0x00	; 0
+    3ed0:	08 2b       	or	r16, r24
+    3ed2:	19 2b       	or	r17, r25
+      savelength = length;
+      desttype = getch();
+    3ed4:	65 d0       	rcall	.+202    	; 0x3fa0 <getch>
+    3ed6:	d8 2e       	mov	r13, r24
+    3ed8:	e8 01       	movw	r28, r16
+    3eda:	e1 2c       	mov	r14, r1
+    3edc:	f1 e0       	ldi	r31, 0x01	; 1
+    3ede:	ff 2e       	mov	r15, r31
 
-      // If we are in RWW section, immediately start page erase
-      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ecc:	80 e0       	ldi	r24, 0x00	; 0
-    3ece:	e8 16       	cp	r14, r24
-    3ed0:	88 e3       	ldi	r24, 0x38	; 56
-    3ed2:	f8 06       	cpc	r15, r24
-    3ed4:	18 f4       	brcc	.+6      	; 0x3edc <main+0xdc>
-    3ed6:	f7 01       	movw	r30, r14
-    3ed8:	d7 be       	out	0x37, r13	; 55
-    3eda:	e8 95       	spm
-    3edc:	c0 e0       	ldi	r28, 0x00	; 0
-    3ede:	d1 e0       	ldi	r29, 0x01	; 1
-
-      // While that is going on, read in page contents
+      // read a page worth of contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ee0:	58 d0       	rcall	.+176    	; 0x3f92 <getch>
-    3ee2:	89 93       	st	Y+, r24
+    3ee0:	5f d0       	rcall	.+190    	; 0x3fa0 <getch>
+    3ee2:	f7 01       	movw	r30, r14
+    3ee4:	81 93       	st	Z+, r24
+    3ee6:	7f 01       	movw	r14, r30
       while (--length);
-    3ee4:	0c 17       	cp	r16, r28
-    3ee6:	e1 f7       	brne	.-8      	; 0x3ee0 <main+0xe0>
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ee8:	f0 e0       	ldi	r31, 0x00	; 0
-    3eea:	ef 16       	cp	r14, r31
-    3eec:	f8 e3       	ldi	r31, 0x38	; 56
-    3eee:	ff 06       	cpc	r15, r31
-    3ef0:	18 f0       	brcs	.+6      	; 0x3ef8 <main+0xf8>
-    3ef2:	f7 01       	movw	r30, r14
-    3ef4:	d7 be       	out	0x37, r13	; 55
-    3ef6:	e8 95       	spm
+    3ee8:	21 97       	sbiw	r28, 0x01	; 1
+    3eea:	d1 f7       	brne	.-12     	; 0x3ee0 <main+0xe0>
 
       // Read command terminator, start reply
       verifySpace();
-    3ef8:	5e d0       	rcall	.+188    	; 0x3fb6 <verifySpace>
-
-      // If only a partial page is to be programmed, the erase might not be complete.
-      // So check that here
-      boot_spm_busy_wait();
-    3efa:	07 b6       	in	r0, 0x37	; 55
-    3efc:	00 fc       	sbrc	r0, 0
-    3efe:	fd cf       	rjmp	.-6      	; 0x3efa <main+0xfa>
-    3f00:	a7 01       	movw	r20, r14
-    3f02:	a0 e0       	ldi	r26, 0x00	; 0
-    3f04:	b1 e0       	ldi	r27, 0x01	; 1
-      bufPtr = buff;
-      addrPtr = (uint16_t)(void*)address;
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-    3f06:	2c 91       	ld	r18, X
-    3f08:	30 e0       	ldi	r19, 0x00	; 0
-        a |= (*bufPtr++) << 8;
-    3f0a:	11 96       	adiw	r26, 0x01	; 1
-    3f0c:	8c 91       	ld	r24, X
-    3f0e:	11 97       	sbiw	r26, 0x01	; 1
-    3f10:	90 e0       	ldi	r25, 0x00	; 0
-    3f12:	98 2f       	mov	r25, r24
-    3f14:	88 27       	eor	r24, r24
-    3f16:	82 2b       	or	r24, r18
-    3f18:	93 2b       	or	r25, r19
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+    3eec:	6b d0       	rcall	.+214    	; 0x3fc4 <verifySpace>
+ * void writebuffer(memtype, buffer, address, length)
+ */
+static inline void writebuffer(int8_t memtype, uint8_t *mybuff,
+			       uint16_t address, uint16_t len)
+{
+    switch (memtype) {
+    3eee:	f5 e4       	ldi	r31, 0x45	; 69
+    3ef0:	df 16       	cp	r13, r31
+    3ef2:	09 f4       	brne	.+2      	; 0x3ef6 <main+0xf6>
+    3ef4:	ff cf       	rjmp	.-2      	; 0x3ef4 <main+0xf4>
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    3ef6:	f5 01       	movw	r30, r10
+    3ef8:	87 be       	out	0x37, r8	; 55
+    3efa:	e8 95       	spm
+	    boot_spm_busy_wait();
+    3efc:	07 b6       	in	r0, 0x37	; 55
+    3efe:	00 fc       	sbrc	r0, 0
+    3f00:	fd cf       	rjmp	.-6      	; 0x3efc <main+0xfc>
+    3f02:	b5 01       	movw	r22, r10
+    3f04:	a8 01       	movw	r20, r16
+    3f06:	a0 e0       	ldi	r26, 0x00	; 0
+    3f08:	b1 e0       	ldi	r27, 0x01	; 1
+	    /*
+	     * Copy data from the buffer into the flash write buffer.
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+    3f0a:	2c 91       	ld	r18, X
+    3f0c:	30 e0       	ldi	r19, 0x00	; 0
+		a |= (*bufPtr++) << 8;
+    3f0e:	11 96       	adiw	r26, 0x01	; 1
+    3f10:	8c 91       	ld	r24, X
+    3f12:	11 97       	sbiw	r26, 0x01	; 1
+    3f14:	90 e0       	ldi	r25, 0x00	; 0
+    3f16:	98 2f       	mov	r25, r24
+    3f18:	88 27       	eor	r24, r24
+    3f1a:	82 2b       	or	r24, r18
+    3f1c:	93 2b       	or	r25, r19
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
-    3f1a:	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);
-    3f1c:	fa 01       	movw	r30, r20
-    3f1e:	0c 01       	movw	r0, r24
-    3f20:	a7 be       	out	0x37, r10	; 55
-    3f22:	e8 95       	spm
-    3f24:	11 24       	eor	r1, r1
-        addrPtr += 2;
-    3f26:	4e 5f       	subi	r20, 0xFE	; 254
-    3f28:	5f 4f       	sbci	r21, 0xFF	; 255
-      } while (--ch);
-    3f2a:	f1 e0       	ldi	r31, 0x01	; 1
-    3f2c:	a0 38       	cpi	r26, 0x80	; 128
-    3f2e:	bf 07       	cpc	r27, r31
-    3f30:	51 f7       	brne	.-44     	; 0x3f06 <main+0x106>
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    3f32:	f7 01       	movw	r30, r14
-    3f34:	c7 be       	out	0x37, r12	; 55
-    3f36:	e8 95       	spm
-      boot_spm_busy_wait();
-    3f38:	07 b6       	in	r0, 0x37	; 55
-    3f3a:	00 fc       	sbrc	r0, 0
-    3f3c:	fd cf       	rjmp	.-6      	; 0x3f38 <main+0x138>
+    3f1e:	12 96       	adiw	r26, 0x02	; 2
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3f20:	fb 01       	movw	r30, r22
+    3f22:	0c 01       	movw	r0, r24
+    3f24:	77 be       	out	0x37, r7	; 55
+    3f26:	e8 95       	spm
+    3f28:	11 24       	eor	r1, r1
+		addrPtr += 2;
+    3f2a:	6e 5f       	subi	r22, 0xFE	; 254
+    3f2c:	7f 4f       	sbci	r23, 0xFF	; 255
+	    } while (len -= 2);
+    3f2e:	42 50       	subi	r20, 0x02	; 2
+    3f30:	50 40       	sbci	r21, 0x00	; 0
+    3f32:	59 f7       	brne	.-42     	; 0x3f0a <main+0x10a>
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    3f34:	f5 01       	movw	r30, r10
+    3f36:	97 be       	out	0x37, r9	; 55
+    3f38:	e8 95       	spm
+	    boot_spm_busy_wait();
+    3f3a:	07 b6       	in	r0, 0x37	; 55
+    3f3c:	00 fc       	sbrc	r0, 0
+    3f3e:	fd cf       	rjmp	.-6      	; 0x3f3a <main+0x13a>
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    3f3e:	b7 be       	out	0x37, r11	; 55
-    3f40:	e8 95       	spm
-    3f42:	1c c0       	rjmp	.+56     	; 0x3f7c <main+0x17c>
-#endif
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    3f40:	c7 be       	out	0x37, r12	; 55
+    3f42:	e8 95       	spm
+    3f44:	22 c0       	rjmp	.+68     	; 0x3f8a <main+0x18a>
+      writebuffer(desttype, buff, address, savelength);
+
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f44:	84 37       	cpi	r24, 0x74	; 116
-    3f46:	61 f4       	brne	.+24     	; 0x3f60 <main+0x160>
-      // READ PAGE - we only read flash
-      getch();			/* getlen() */
-    3f48:	24 d0       	rcall	.+72     	; 0x3f92 <getch>
-      length = getch();
-    3f4a:	23 d0       	rcall	.+70     	; 0x3f92 <getch>
-    3f4c:	08 2f       	mov	r16, r24
-      getch();
-    3f4e:	21 d0       	rcall	.+66     	; 0x3f92 <getch>
+    3f46:	84 37       	cpi	r24, 0x74	; 116
+    3f48:	91 f4       	brne	.+36     	; 0x3f6e <main+0x16e>
+      uint8_t desttype;
+      length = getch()<<8;			/* getlen() */
+    3f4a:	2a d0       	rcall	.+84     	; 0x3fa0 <getch>
+    3f4c:	90 e0       	ldi	r25, 0x00	; 0
+    3f4e:	d8 2f       	mov	r29, r24
+    3f50:	cc 27       	eor	r28, r28
+      length |= getch();
+    3f52:	26 d0       	rcall	.+76     	; 0x3fa0 <getch>
+    3f54:	90 e0       	ldi	r25, 0x00	; 0
+    3f56:	c8 2b       	or	r28, r24
+    3f58:	d9 2b       	or	r29, r25
+      desttype = getch();
+    3f5a:	22 d0       	rcall	.+68     	; 0x3fa0 <getch>
 
       verifySpace();
-    3f50:	32 d0       	rcall	.+100    	; 0x3fb6 <verifySpace>
-        __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+    3f5c:	33 d0       	rcall	.+102    	; 0x3fc4 <verifySpace>
+    3f5e:	85 01       	movw	r16, r10
+	    __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #else
-        // read a Flash byte and increment the address
-        __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+	    // read a Flash byte and increment the address
+	    __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #endif
-        putch(ch);
-    3f52:	f7 01       	movw	r30, r14
-    3f54:	85 91       	lpm	r24, Z+
-    3f56:	7f 01       	movw	r14, r30
-    3f58:	14 d0       	rcall	.+40     	; 0x3f82 <putch>
-      } while (--length);
-    3f5a:	01 50       	subi	r16, 0x01	; 1
-    3f5c:	d1 f7       	brne	.-12     	; 0x3f52 <main+0x152>
-    3f5e:	0e c0       	rjmp	.+28     	; 0x3f7c <main+0x17c>
+	    putch(ch);
+    3f60:	f8 01       	movw	r30, r16
+    3f62:	85 91       	lpm	r24, Z+
+    3f64:	8f 01       	movw	r16, r30
+    3f66:	14 d0       	rcall	.+40     	; 0x3f90 <putch>
+	} while (--length);
+    3f68:	21 97       	sbiw	r28, 0x01	; 1
+    3f6a:	d1 f7       	brne	.-12     	; 0x3f60 <main+0x160>
+    3f6c:	0e c0       	rjmp	.+28     	; 0x3f8a <main+0x18a>
+	  
+      read_mem(desttype, address, length);
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f60:	85 37       	cpi	r24, 0x75	; 117
-    3f62:	39 f4       	brne	.+14     	; 0x3f72 <main+0x172>
+    3f6e:	85 37       	cpi	r24, 0x75	; 117
+    3f70:	39 f4       	brne	.+14     	; 0x3f80 <main+0x180>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f64:	28 d0       	rcall	.+80     	; 0x3fb6 <verifySpace>
+    3f72:	28 d0       	rcall	.+80     	; 0x3fc4 <verifySpace>
       putch(SIGNATURE_0);
-    3f66:	8e e1       	ldi	r24, 0x1E	; 30
-    3f68:	0c d0       	rcall	.+24     	; 0x3f82 <putch>
+    3f74:	8e e1       	ldi	r24, 0x1E	; 30
+    3f76:	0c d0       	rcall	.+24     	; 0x3f90 <putch>
       putch(SIGNATURE_1);
-    3f6a:	84 e9       	ldi	r24, 0x94	; 148
-    3f6c:	0a d0       	rcall	.+20     	; 0x3f82 <putch>
+    3f78:	84 e9       	ldi	r24, 0x94	; 148
+    3f7a:	0a d0       	rcall	.+20     	; 0x3f90 <putch>
       putch(SIGNATURE_2);
-    3f6e:	86 e0       	ldi	r24, 0x06	; 6
-    3f70:	84 cf       	rjmp	.-248    	; 0x3e7a <main+0x7a>
+    3f7c:	86 e0       	ldi	r24, 0x06	; 6
+    3f7e:	7d cf       	rjmp	.-262    	; 0x3e7a <main+0x7a>
     }
     else if (ch == STK_LEAVE_PROGMODE) { /* 'Q' */
-    3f72:	81 35       	cpi	r24, 0x51	; 81
-    3f74:	11 f4       	brne	.+4      	; 0x3f7a <main+0x17a>
+    3f80:	81 35       	cpi	r24, 0x51	; 81
+    3f82:	11 f4       	brne	.+4      	; 0x3f88 <main+0x188>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f76:	88 e0       	ldi	r24, 0x08	; 8
-    3f78:	18 d0       	rcall	.+48     	; 0x3faa <watchdogConfig>
+    3f84:	88 e0       	ldi	r24, 0x08	; 8
+    3f86:	18 d0       	rcall	.+48     	; 0x3fb8 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f7a:	1d d0       	rcall	.+58     	; 0x3fb6 <verifySpace>
+    3f88:	1d d0       	rcall	.+58     	; 0x3fc4 <verifySpace>
     }
     putch(STK_OK);
-    3f7c:	80 e1       	ldi	r24, 0x10	; 16
-    3f7e:	01 d0       	rcall	.+2      	; 0x3f82 <putch>
-    3f80:	6f cf       	rjmp	.-290    	; 0x3e60 <main+0x60>
+    3f8a:	80 e1       	ldi	r24, 0x10	; 16
+    3f8c:	01 d0       	rcall	.+2      	; 0x3f90 <putch>
+    3f8e:	69 cf       	rjmp	.-302    	; 0x3e62 <main+0x62>
 
-00003f82 <putch>:
+00003f90 <putch>:
   }
 }
 
 void putch(char ch) {
-    3f82:	98 2f       	mov	r25, r24
+    3f90:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UART_SRA & _BV(UDRE0)));
-    3f84:	80 91 c0 00 	lds	r24, 0x00C0
-    3f88:	85 ff       	sbrs	r24, 5
-    3f8a:	fc cf       	rjmp	.-8      	; 0x3f84 <putch+0x2>
+    3f92:	80 91 c0 00 	lds	r24, 0x00C0
+    3f96:	85 ff       	sbrs	r24, 5
+    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <putch+0x2>
   UART_UDR = ch;
-    3f8c:	90 93 c6 00 	sts	0x00C6, r25
+    3f9a:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3f90:	08 95       	ret
+    3f9e:	08 95       	ret
 
-00003f92 <getch>:
+00003fa0 <getch>:
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UART_SRA & _BV(RXC0)))
-    3f92:	80 91 c0 00 	lds	r24, 0x00C0
-    3f96:	87 ff       	sbrs	r24, 7
-    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <getch>
+    3fa0:	80 91 c0 00 	lds	r24, 0x00C0
+    3fa4:	87 ff       	sbrs	r24, 7
+    3fa6:	fc cf       	rjmp	.-8      	; 0x3fa0 <getch>
     ;
   if (!(UART_SRA & _BV(FE0))) {
-    3f9a:	80 91 c0 00 	lds	r24, 0x00C0
-    3f9e:	84 fd       	sbrc	r24, 4
-    3fa0:	01 c0       	rjmp	.+2      	; 0x3fa4 <getch+0x12>
+    3fa8:	80 91 c0 00 	lds	r24, 0x00C0
+    3fac:	84 fd       	sbrc	r24, 4
+    3fae:	01 c0       	rjmp	.+2      	; 0x3fb2 <getch+0x12>
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fa2:	a8 95       	wdr
+    3fb0:	a8 95       	wdr
        * don't care that an invalid char is returned...)
        */
     watchdogReset();
   }
   
   ch = UART_UDR;
-    3fa4:	80 91 c6 00 	lds	r24, 0x00C6
+    3fb2:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    3fa8:	08 95       	ret
+    3fb6:	08 95       	ret
 
-00003faa <watchdogConfig>:
+00003fb8 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3faa:	e0 e6       	ldi	r30, 0x60	; 96
-    3fac:	f0 e0       	ldi	r31, 0x00	; 0
-    3fae:	98 e1       	ldi	r25, 0x18	; 24
-    3fb0:	90 83       	st	Z, r25
+    3fb8:	e0 e6       	ldi	r30, 0x60	; 96
+    3fba:	f0 e0       	ldi	r31, 0x00	; 0
+    3fbc:	98 e1       	ldi	r25, 0x18	; 24
+    3fbe:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fb2:	80 83       	st	Z, r24
+    3fc0:	80 83       	st	Z, r24
 }
-    3fb4:	08 95       	ret
+    3fc2:	08 95       	ret
 
-00003fb6 <verifySpace>:
+00003fc4 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) {
-    3fb6:	ed df       	rcall	.-38     	; 0x3f92 <getch>
-    3fb8:	80 32       	cpi	r24, 0x20	; 32
-    3fba:	19 f0       	breq	.+6      	; 0x3fc2 <verifySpace+0xc>
+    3fc4:	ed df       	rcall	.-38     	; 0x3fa0 <getch>
+    3fc6:	80 32       	cpi	r24, 0x20	; 32
+    3fc8:	19 f0       	breq	.+6      	; 0x3fd0 <verifySpace+0xc>
     watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
-    3fbc:	88 e0       	ldi	r24, 0x08	; 8
-    3fbe:	f5 df       	rcall	.-22     	; 0x3faa <watchdogConfig>
-    3fc0:	ff cf       	rjmp	.-2      	; 0x3fc0 <verifySpace+0xa>
+    3fca:	88 e0       	ldi	r24, 0x08	; 8
+    3fcc:	f5 df       	rcall	.-22     	; 0x3fb8 <watchdogConfig>
+    3fce:	ff cf       	rjmp	.-2      	; 0x3fce <verifySpace+0xa>
     while (1)			      // and busy-loop so that WD causes
       ;				      //  a reset and app start.
   }
   putch(STK_INSYNC);
-    3fc2:	84 e1       	ldi	r24, 0x14	; 20
+    3fd0:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fc4:	de cf       	rjmp	.-68     	; 0x3f82 <putch>
+    3fd2:	de cf       	rjmp	.-68     	; 0x3f90 <putch>
 
-00003fc6 <getNch>:
+00003fd4 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fc6:	1f 93       	push	r17
-    3fc8:	18 2f       	mov	r17, r24
+    3fd4:	1f 93       	push	r17
+    3fd6:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3fca:	e3 df       	rcall	.-58     	; 0x3f92 <getch>
-    3fcc:	11 50       	subi	r17, 0x01	; 1
-    3fce:	e9 f7       	brne	.-6      	; 0x3fca <getNch+0x4>
+    3fd8:	e3 df       	rcall	.-58     	; 0x3fa0 <getch>
+    3fda:	11 50       	subi	r17, 0x01	; 1
+    3fdc:	e9 f7       	brne	.-6      	; 0x3fd8 <getNch+0x4>
   verifySpace();
-    3fd0:	f2 df       	rcall	.-28     	; 0x3fb6 <verifySpace>
+    3fde:	f2 df       	rcall	.-28     	; 0x3fc4 <verifySpace>
 }
-    3fd2:	1f 91       	pop	r17
-    3fd4:	08 95       	ret
+    3fe0:	1f 91       	pop	r17
+    3fe2:	08 95       	ret
 
-00003fd6 <appStart>:
+00003fe4 <appStart>:
 
 void appStart(uint8_t rstFlags) {
   // save the reset flags in the designated register
   //  This can be saved in a main program by putting code in .init0 (which
   //  executes before normal c init code) to save R2 to a global variable.
   __asm__ __volatile__ ("mov r2, %0\n" :: "r" (rstFlags));
-    3fd6:	28 2e       	mov	r2, r24
+    3fe4:	28 2e       	mov	r2, r24
 
   watchdogConfig(WATCHDOG_OFF);
-    3fd8:	80 e0       	ldi	r24, 0x00	; 0
-    3fda:	e7 df       	rcall	.-50     	; 0x3faa <watchdogConfig>
+    3fe6:	80 e0       	ldi	r24, 0x00	; 0
+    3fe8:	e7 df       	rcall	.-50     	; 0x3fb8 <watchdogConfig>
   __asm__ __volatile__ (
-    3fdc:	ee 27       	eor	r30, r30
-    3fde:	ff 27       	eor	r31, r31
-    3fe0:	09 94       	ijmp
+    3fea:	ee 27       	eor	r30, r30
+    3fec:	ff 27       	eor	r31, r31
+    3fee:	09 94       	ijmp
diff --git a/optiboot/bootloaders/optiboot/optiboot_pro_20mhz.hex b/optiboot/bootloaders/optiboot/optiboot_pro_20mhz.hex
index 4d5e8ba..8330c62 100644
--- a/optiboot/bootloaders/optiboot/optiboot_pro_20mhz.hex
+++ b/optiboot/bootloaders/optiboot/optiboot_pro_20mhz.hex
@@ -1,34 +1,34 @@
-:103E0000112484B714BE81FFE6D085E08093810041
-:103E100082E08093C00088E18093C10086E08093B7
-:103E2000C20085E18093C4008EE0BFD0259A86E071
-:103E30002CE33BEF91E0309385002093840096BB08
-:103E4000B09BFECF1D9AA8958150A9F7EE24FF24C0
-:103E5000AA24A394B5E0CB2EA1E1BA2EF3E0DF2E85
-:103E600098D0813461F495D0082FA5D0023829F17B
-:103E7000013811F485E001C083E083D07FC0823433
-:103E800011F484E103C0853419F485E09CD076C038
-:103E9000853579F47ED0E82EFF247BD0082F10E002
-:103EA000102F00270E291F29000F111F84D0780121
-:103EB00065C0863521F484E086D080E0DECF84368C
-:103EC00009F040C066D065D0082F63D080E0E816C6
-:103ED00088E3F80618F4F701D7BEE895C0E0D1E012
-:103EE00058D089930C17E1F7F0E0EF16F8E3FF06DE
-:103EF00018F0F701D7BEE8955ED007B600FCFDCFFD
-:103F0000A701A0E0B1E02C9130E011968C911197BF
-:103F100090E0982F8827822B932B1296FA010C01A0
-:103F2000A7BEE89511244E5F5F4FF1E0A038BF07B0
-:103F300051F7F701C7BEE89507B600FCFDCFB7BE45
-:103F4000E8951CC0843761F424D023D0082F21D0F9
-:103F500032D0F70185917F0114D00150D1F70EC006
-:103F6000853739F428D08EE10CD084E90AD086E078
-:103F700084CF813511F488E018D01DD080E101D0C4
-:103F80006FCF982F8091C00085FFFCCF9093C60023
-:103F900008958091C00087FFFCCF8091C00084FD10
-:103FA00001C0A8958091C6000895E0E6F0E098E190
-:103FB000908380830895EDDF803219F088E0F5DF8B
-:103FC000FFCF84E1DECF1F93182FE3DF1150E9F715
-:103FD000F2DF1F910895282E80E0E7DFEE27FF270C
-:023FE000099442
-:023FFE000005BC
+:103E0000112484B714BE81FD01C0ECD085E08093FD
+:103E1000810082E08093C00088E18093C10086E049
+:103E20008093C20085E18093C4008EE0C5D0259ABE
+:103E300086E02CE33BEF91E03093850020938400F3
+:103E400096BBB09BFECF1D9AA8958150A9F7AA24D6
+:103E5000BB2433E0832E7724739425E0922E91E1E6
+:103E6000C92E9ED0813459F49BD0182FABD0123874
+:103E700021F1113809F482C083E08AD086C08234EF
+:103E800011F484E103C0853419F485E0A3D07DC02A
+:103E9000853579F485D0E82EFF2482D0082F10E0F4
+:103EA000102F00270E291F29000F111F8BD058013A
+:103EB0006CC0863521F484E08DD080E0DECF84367E
+:103EC00009F041C06DD090E0182F002769D090E034
+:103ED000082B192B65D0D82EE801E12CF1E0FF2E3C
+:103EE0005FD0F70181937F012197D1F76BD0F5E483
+:103EF000DF1609F4FFCFF50187BEE89507B600FC91
+:103F0000FDCFB501A801A0E0B1E02C9130E0119601
+:103F10008C91119790E0982F8827822B932B1296E3
+:103F2000FB010C0177BEE89511246E5F7F4F425074
+:103F3000504059F7F50197BEE89507B600FCFDCF54
+:103F4000C7BEE89522C0843791F42AD090E0D82FDC
+:103F5000CC2726D090E0C82BD92B22D033D0850196
+:103F6000F80185918F0114D02197D1F70EC08537C4
+:103F700039F428D08EE10CD084E90AD086E07DCFD8
+:103F8000813511F488E018D01DD080E101D069CFCF
+:103F9000982F8091C00085FFFCCF9093C6000895B4
+:103FA0008091C00087FFFCCF8091C00084FD01C0DC
+:103FB000A8958091C6000895E0E6F0E098E190832E
+:103FC00080830895EDDF803219F088E0F5DFFFCFC0
+:103FD00084E1DECF1F93182FE3DF1150E9F7F2DF02
+:103FE0001F910895282E80E0E7DFEE27FF27099430
+:023FFE000006BB
 :0400000300003E00BB
 :00000001FF
diff --git a/optiboot/bootloaders/optiboot/optiboot_pro_20mhz.lst b/optiboot/bootloaders/optiboot/optiboot_pro_20mhz.lst
index 644471e..6a9a4f7 100644
--- a/optiboot/bootloaders/optiboot/optiboot_pro_20mhz.lst
+++ b/optiboot/bootloaders/optiboot/optiboot_pro_20mhz.lst
@@ -3,36 +3,36 @@ optiboot_atmega168.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001e2  00003e00  00003e00  00000054  2**1
+  0 .text         000001f0  00003e00  00003e00  00000074  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00003ffe  00003ffe  00000236  2**0
-                  CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000238  2**0
+  1 .version      00000002  00003ffe  00003ffe  00000264  2**0
+                  CONTENTS, ALLOC, LOAD, DATA
+  2 .debug_aranges 00000028  00000000  00000000  00000266  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000005f  00000000  00000000  00000260  2**0
+  3 .debug_pubnames 00000074  00000000  00000000  0000028e  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000294  00000000  00000000  000002bf  2**0
+  4 .debug_info   000003db  00000000  00000000  00000302  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000016b  00000000  00000000  00000553  2**0
+  5 .debug_abbrev 000001ea  00000000  00000000  000006dd  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000421  00000000  00000000  000006be  2**0
+  6 .debug_line   00000423  00000000  00000000  000008c7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000080  00000000  00000000  00000ae0  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000cec  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000131  00000000  00000000  00000b60  2**0
+  8 .debug_str    00000172  00000000  00000000  00000d6c  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    00000266  00000000  00000000  00000c91  2**0
+  9 .debug_loc    000002d7  00000000  00000000  00000ede  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000078  00000000  00000000  00000ef7  2**0
+ 10 .debug_ranges 000000b8  00000000  00000000  000011b5  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00003e00 <main>:
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
     3e00:	11 24       	eor	r1, r1
@@ -46,148 +46,147 @@ int main(void) {
   MCUSR = 0;
     3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart(ch);
-    3e06:	81 ff       	sbrs	r24, 1
-    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+    3e06:	81 fd       	sbrc	r24, 1
+    3e08:	01 c0       	rjmp	.+2      	; 0x3e0c <main+0xc>
+    3e0a:	ec d0       	rcall	.+472    	; 0x3fe4 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-    3e0a:	85 e0       	ldi	r24, 0x05	; 5
-    3e0c:	80 93 81 00 	sts	0x0081, r24
+    3e0c:	85 e0       	ldi	r24, 0x05	; 5
+    3e0e:	80 93 81 00 	sts	0x0081, r24
   UCSRA = _BV(U2X); //Double speed mode USART
   UCSRB = _BV(RXEN) | _BV(TXEN);  // enable Rx & Tx
   UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0);  // config USART; 8N1
   UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
 #else
   UART_SRA = _BV(U2X0); //Double speed mode USART0
-    3e10:	82 e0       	ldi	r24, 0x02	; 2
-    3e12:	80 93 c0 00 	sts	0x00C0, r24
+    3e12:	82 e0       	ldi	r24, 0x02	; 2
+    3e14:	80 93 c0 00 	sts	0x00C0, r24
   UART_SRB = _BV(RXEN0) | _BV(TXEN0);
-    3e16:	88 e1       	ldi	r24, 0x18	; 24
-    3e18:	80 93 c1 00 	sts	0x00C1, r24
+    3e18:	88 e1       	ldi	r24, 0x18	; 24
+    3e1a:	80 93 c1 00 	sts	0x00C1, r24
   UART_SRC = _BV(UCSZ00) | _BV(UCSZ01);
-    3e1c:	86 e0       	ldi	r24, 0x06	; 6
-    3e1e:	80 93 c2 00 	sts	0x00C2, r24
+    3e1e:	86 e0       	ldi	r24, 0x06	; 6
+    3e20:	80 93 c2 00 	sts	0x00C2, r24
   UART_SRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    3e22:	85 e1       	ldi	r24, 0x15	; 21
-    3e24:	80 93 c4 00 	sts	0x00C4, r24
+    3e24:	85 e1       	ldi	r24, 0x15	; 21
+    3e26:	80 93 c4 00 	sts	0x00C4, r24
 #endif
 #endif
 
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
-    3e28:	8e e0       	ldi	r24, 0x0E	; 14
-    3e2a:	bf d0       	rcall	.+382    	; 0x3faa <watchdogConfig>
+    3e2a:	8e e0       	ldi	r24, 0x0E	; 14
+    3e2c:	c5 d0       	rcall	.+394    	; 0x3fb8 <watchdogConfig>
 
 #if (LED_START_FLASHES > 0) || defined(LED_DATA_FLASH)
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    3e2c:	25 9a       	sbi	0x04, 5	; 4
-    3e2e:	86 e0       	ldi	r24, 0x06	; 6
+    3e2e:	25 9a       	sbi	0x04, 5	; 4
+    3e30:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e30:	2c e3       	ldi	r18, 0x3C	; 60
-    3e32:	3b ef       	ldi	r19, 0xFB	; 251
+    3e32:	2c e3       	ldi	r18, 0x3C	; 60
+    3e34:	3b ef       	ldi	r19, 0xFB	; 251
     TIFR1 = _BV(TOV1);
-    3e34:	91 e0       	ldi	r25, 0x01	; 1
+    3e36:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e36:	30 93 85 00 	sts	0x0085, r19
-    3e3a:	20 93 84 00 	sts	0x0084, r18
+    3e38:	30 93 85 00 	sts	0x0085, r19
+    3e3c:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    3e3e:	96 bb       	out	0x16, r25	; 22
+    3e40:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    3e40:	b0 9b       	sbis	0x16, 0	; 22
-    3e42:	fe cf       	rjmp	.-4      	; 0x3e40 <main+0x40>
+    3e42:	b0 9b       	sbis	0x16, 0	; 22
+    3e44:	fe cf       	rjmp	.-4      	; 0x3e42 <main+0x42>
 #if defined(__AVR_ATmega8__)  || defined (__AVR_ATmega32__)
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
-    3e44:	1d 9a       	sbi	0x03, 5	; 3
+    3e46:	1d 9a       	sbi	0x03, 5	; 3
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3e46:	a8 95       	wdr
+    3e48:	a8 95       	wdr
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
 #endif
     watchdogReset();
   } while (--count);
-    3e48:	81 50       	subi	r24, 0x01	; 1
-    3e4a:	a9 f7       	brne	.-22     	; 0x3e36 <main+0x36>
-    3e4c:	ee 24       	eor	r14, r14
-    3e4e:	ff 24       	eor	r15, r15
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-        a |= (*bufPtr++) << 8;
-        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    3e50:	aa 24       	eor	r10, r10
-    3e52:	a3 94       	inc	r10
-        addrPtr += 2;
-      } while (--ch);
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    3e54:	b5 e0       	ldi	r27, 0x05	; 5
-    3e56:	cb 2e       	mov	r12, r27
-      boot_spm_busy_wait();
+    3e4a:	81 50       	subi	r24, 0x01	; 1
+    3e4c:	a9 f7       	brne	.-22     	; 0x3e38 <main+0x38>
+    3e4e:	aa 24       	eor	r10, r10
+    3e50:	bb 24       	eor	r11, r11
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    3e52:	33 e0       	ldi	r19, 0x03	; 3
+    3e54:	83 2e       	mov	r8, r19
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3e56:	77 24       	eor	r7, r7
+    3e58:	73 94       	inc	r7
+	    } while (len -= 2);
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    3e5a:	25 e0       	ldi	r18, 0x05	; 5
+    3e5c:	92 2e       	mov	r9, r18
+	    boot_spm_busy_wait();
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    3e58:	a1 e1       	ldi	r26, 0x11	; 17
-    3e5a:	ba 2e       	mov	r11, r26
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3e5c:	f3 e0       	ldi	r31, 0x03	; 3
-    3e5e:	df 2e       	mov	r13, r31
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    3e5e:	91 e1       	ldi	r25, 0x11	; 17
+    3e60:	c9 2e       	mov	r12, r25
 #endif
 
-  /* Forever loop */
+  /* Forever loop: exits by causing WDT reset */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e60:	98 d0       	rcall	.+304    	; 0x3f92 <getch>
+    3e62:	9e d0       	rcall	.+316    	; 0x3fa0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    3e62:	81 34       	cpi	r24, 0x41	; 65
-    3e64:	61 f4       	brne	.+24     	; 0x3e7e <main+0x7e>
+    3e64:	81 34       	cpi	r24, 0x41	; 65
+    3e66:	59 f4       	brne	.+22     	; 0x3e7e <main+0x7e>
       unsigned char which = getch();
-    3e66:	95 d0       	rcall	.+298    	; 0x3f92 <getch>
-    3e68:	08 2f       	mov	r16, r24
+    3e68:	9b d0       	rcall	.+310    	; 0x3fa0 <getch>
+    3e6a:	18 2f       	mov	r17, r24
       verifySpace();
-    3e6a:	a5 d0       	rcall	.+330    	; 0x3fb6 <verifySpace>
+    3e6c:	ab d0       	rcall	.+342    	; 0x3fc4 <verifySpace>
       if (which == 0x82) {
-    3e6c:	02 38       	cpi	r16, 0x82	; 130
-    3e6e:	29 f1       	breq	.+74     	; 0x3eba <main+0xba>
+    3e6e:	12 38       	cpi	r17, 0x82	; 130
+    3e70:	21 f1       	breq	.+72     	; 0x3eba <main+0xba>
 	/*
 	 * Send optiboot version as "minor SW version"
 	 */
 	putch(OPTIBOOT_MINVER);
       } else if (which == 0x81) {
-    3e70:	01 38       	cpi	r16, 0x81	; 129
-    3e72:	11 f4       	brne	.+4      	; 0x3e78 <main+0x78>
-	  putch(OPTIBOOT_MAJVER);
-    3e74:	85 e0       	ldi	r24, 0x05	; 5
-    3e76:	01 c0       	rjmp	.+2      	; 0x3e7a <main+0x7a>
+    3e72:	11 38       	cpi	r17, 0x81	; 129
+    3e74:	09 f4       	brne	.+2      	; 0x3e78 <main+0x78>
+    3e76:	82 c0       	rjmp	.+260    	; 0x3f7c <main+0x17c>
       } else {
 	/*
 	 * GET PARAMETER returns a generic 0x03 reply for
@@ -195,8 +194,8 @@ void watchdogReset() {
 	 */
 	putch(0x03);
     3e78:	83 e0       	ldi	r24, 0x03	; 3
-    3e7a:	83 d0       	rcall	.+262    	; 0x3f82 <putch>
-    3e7c:	7f c0       	rjmp	.+254    	; 0x3f7c <main+0x17c>
+    3e7a:	8a d0       	rcall	.+276    	; 0x3f90 <putch>
+    3e7c:	86 c0       	rjmp	.+268    	; 0x3f8a <main+0x18a>
       }
     }
     else if(ch == STK_SET_DEVICE) {
@@ -213,8 +212,8 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     3e8a:	85 e0       	ldi	r24, 0x05	; 5
-    3e8c:	9c d0       	rcall	.+312    	; 0x3fc6 <getNch>
-    3e8e:	76 c0       	rjmp	.+236    	; 0x3f7c <main+0x17c>
+    3e8c:	a3 d0       	rcall	.+326    	; 0x3fd4 <getNch>
+    3e8e:	7d c0       	rjmp	.+250    	; 0x3f8a <main+0x18a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     3e90:	85 35       	cpi	r24, 0x55	; 85
@@ -222,11 +221,11 @@ void watchdogReset() {
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    3e94:	7e d0       	rcall	.+252    	; 0x3f92 <getch>
+    3e94:	85 d0       	rcall	.+266    	; 0x3fa0 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
     3e96:	e8 2e       	mov	r14, r24
     3e98:	ff 24       	eor	r15, r15
-    3e9a:	7b d0       	rcall	.+246    	; 0x3f92 <getch>
+    3e9a:	82 d0       	rcall	.+260    	; 0x3fa0 <getch>
     3e9c:	08 2f       	mov	r16, r24
     3e9e:	10 e0       	ldi	r17, 0x00	; 0
     3ea0:	10 2f       	mov	r17, r16
@@ -242,9 +241,9 @@ void watchdogReset() {
     3eaa:	11 1f       	adc	r17, r17
       address = newAddress;
       verifySpace();
-    3eac:	84 d0       	rcall	.+264    	; 0x3fb6 <verifySpace>
-    3eae:	78 01       	movw	r14, r16
-    3eb0:	65 c0       	rjmp	.+202    	; 0x3f7c <main+0x17c>
+    3eac:	8b d0       	rcall	.+278    	; 0x3fc4 <verifySpace>
+    3eae:	58 01       	movw	r10, r16
+    3eb0:	6c c0       	rjmp	.+216    	; 0x3f8a <main+0x18a>
     }
     else if(ch == STK_UNIVERSAL) {
     3eb2:	86 35       	cpi	r24, 0x56	; 86
@@ -252,7 +251,7 @@ void watchdogReset() {
       // UNIVERSAL command is ignored
       getNch(4);
     3eb6:	84 e0       	ldi	r24, 0x04	; 4
-    3eb8:	86 d0       	rcall	.+268    	; 0x3fc6 <getNch>
+    3eb8:	8d d0       	rcall	.+282    	; 0x3fd4 <getNch>
       putch(0x00);
     3eba:	80 e0       	ldi	r24, 0x00	; 0
     3ebc:	de cf       	rjmp	.-68     	; 0x3e7a <main+0x7a>
@@ -261,326 +260,339 @@ void watchdogReset() {
     else if(ch == STK_PROG_PAGE) {
     3ebe:	84 36       	cpi	r24, 0x64	; 100
     3ec0:	09 f0       	breq	.+2      	; 0x3ec4 <main+0xc4>
-    3ec2:	40 c0       	rjmp	.+128    	; 0x3f44 <main+0x144>
+    3ec2:	41 c0       	rjmp	.+130    	; 0x3f46 <main+0x146>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
+      uint8_t desttype;
       uint8_t *bufPtr;
-      uint16_t addrPtr;
+      uint16_t savelength;
 
-      getch();			/* getlen() */
-    3ec4:	66 d0       	rcall	.+204    	; 0x3f92 <getch>
-      length = getch();
-    3ec6:	65 d0       	rcall	.+202    	; 0x3f92 <getch>
-    3ec8:	08 2f       	mov	r16, r24
-      getch();
-    3eca:	63 d0       	rcall	.+198    	; 0x3f92 <getch>
+      length = getch()<<8;			/* getlen() */
+    3ec4:	6d d0       	rcall	.+218    	; 0x3fa0 <getch>
+    3ec6:	90 e0       	ldi	r25, 0x00	; 0
+    3ec8:	18 2f       	mov	r17, r24
+    3eca:	00 27       	eor	r16, r16
+      length |= getch();
+    3ecc:	69 d0       	rcall	.+210    	; 0x3fa0 <getch>
+    3ece:	90 e0       	ldi	r25, 0x00	; 0
+    3ed0:	08 2b       	or	r16, r24
+    3ed2:	19 2b       	or	r17, r25
+      savelength = length;
+      desttype = getch();
+    3ed4:	65 d0       	rcall	.+202    	; 0x3fa0 <getch>
+    3ed6:	d8 2e       	mov	r13, r24
+    3ed8:	e8 01       	movw	r28, r16
+    3eda:	e1 2c       	mov	r14, r1
+    3edc:	f1 e0       	ldi	r31, 0x01	; 1
+    3ede:	ff 2e       	mov	r15, r31
 
-      // If we are in RWW section, immediately start page erase
-      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ecc:	80 e0       	ldi	r24, 0x00	; 0
-    3ece:	e8 16       	cp	r14, r24
-    3ed0:	88 e3       	ldi	r24, 0x38	; 56
-    3ed2:	f8 06       	cpc	r15, r24
-    3ed4:	18 f4       	brcc	.+6      	; 0x3edc <main+0xdc>
-    3ed6:	f7 01       	movw	r30, r14
-    3ed8:	d7 be       	out	0x37, r13	; 55
-    3eda:	e8 95       	spm
-    3edc:	c0 e0       	ldi	r28, 0x00	; 0
-    3ede:	d1 e0       	ldi	r29, 0x01	; 1
-
-      // While that is going on, read in page contents
+      // read a page worth of contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ee0:	58 d0       	rcall	.+176    	; 0x3f92 <getch>
-    3ee2:	89 93       	st	Y+, r24
+    3ee0:	5f d0       	rcall	.+190    	; 0x3fa0 <getch>
+    3ee2:	f7 01       	movw	r30, r14
+    3ee4:	81 93       	st	Z+, r24
+    3ee6:	7f 01       	movw	r14, r30
       while (--length);
-    3ee4:	0c 17       	cp	r16, r28
-    3ee6:	e1 f7       	brne	.-8      	; 0x3ee0 <main+0xe0>
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ee8:	f0 e0       	ldi	r31, 0x00	; 0
-    3eea:	ef 16       	cp	r14, r31
-    3eec:	f8 e3       	ldi	r31, 0x38	; 56
-    3eee:	ff 06       	cpc	r15, r31
-    3ef0:	18 f0       	brcs	.+6      	; 0x3ef8 <main+0xf8>
-    3ef2:	f7 01       	movw	r30, r14
-    3ef4:	d7 be       	out	0x37, r13	; 55
-    3ef6:	e8 95       	spm
+    3ee8:	21 97       	sbiw	r28, 0x01	; 1
+    3eea:	d1 f7       	brne	.-12     	; 0x3ee0 <main+0xe0>
 
       // Read command terminator, start reply
       verifySpace();
-    3ef8:	5e d0       	rcall	.+188    	; 0x3fb6 <verifySpace>
-
-      // If only a partial page is to be programmed, the erase might not be complete.
-      // So check that here
-      boot_spm_busy_wait();
-    3efa:	07 b6       	in	r0, 0x37	; 55
-    3efc:	00 fc       	sbrc	r0, 0
-    3efe:	fd cf       	rjmp	.-6      	; 0x3efa <main+0xfa>
-    3f00:	a7 01       	movw	r20, r14
-    3f02:	a0 e0       	ldi	r26, 0x00	; 0
-    3f04:	b1 e0       	ldi	r27, 0x01	; 1
-      bufPtr = buff;
-      addrPtr = (uint16_t)(void*)address;
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-    3f06:	2c 91       	ld	r18, X
-    3f08:	30 e0       	ldi	r19, 0x00	; 0
-        a |= (*bufPtr++) << 8;
-    3f0a:	11 96       	adiw	r26, 0x01	; 1
-    3f0c:	8c 91       	ld	r24, X
-    3f0e:	11 97       	sbiw	r26, 0x01	; 1
-    3f10:	90 e0       	ldi	r25, 0x00	; 0
-    3f12:	98 2f       	mov	r25, r24
-    3f14:	88 27       	eor	r24, r24
-    3f16:	82 2b       	or	r24, r18
-    3f18:	93 2b       	or	r25, r19
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+    3eec:	6b d0       	rcall	.+214    	; 0x3fc4 <verifySpace>
+ * void writebuffer(memtype, buffer, address, length)
+ */
+static inline void writebuffer(int8_t memtype, uint8_t *mybuff,
+			       uint16_t address, uint16_t len)
+{
+    switch (memtype) {
+    3eee:	f5 e4       	ldi	r31, 0x45	; 69
+    3ef0:	df 16       	cp	r13, r31
+    3ef2:	09 f4       	brne	.+2      	; 0x3ef6 <main+0xf6>
+    3ef4:	ff cf       	rjmp	.-2      	; 0x3ef4 <main+0xf4>
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    3ef6:	f5 01       	movw	r30, r10
+    3ef8:	87 be       	out	0x37, r8	; 55
+    3efa:	e8 95       	spm
+	    boot_spm_busy_wait();
+    3efc:	07 b6       	in	r0, 0x37	; 55
+    3efe:	00 fc       	sbrc	r0, 0
+    3f00:	fd cf       	rjmp	.-6      	; 0x3efc <main+0xfc>
+    3f02:	b5 01       	movw	r22, r10
+    3f04:	a8 01       	movw	r20, r16
+    3f06:	a0 e0       	ldi	r26, 0x00	; 0
+    3f08:	b1 e0       	ldi	r27, 0x01	; 1
+	    /*
+	     * Copy data from the buffer into the flash write buffer.
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+    3f0a:	2c 91       	ld	r18, X
+    3f0c:	30 e0       	ldi	r19, 0x00	; 0
+		a |= (*bufPtr++) << 8;
+    3f0e:	11 96       	adiw	r26, 0x01	; 1
+    3f10:	8c 91       	ld	r24, X
+    3f12:	11 97       	sbiw	r26, 0x01	; 1
+    3f14:	90 e0       	ldi	r25, 0x00	; 0
+    3f16:	98 2f       	mov	r25, r24
+    3f18:	88 27       	eor	r24, r24
+    3f1a:	82 2b       	or	r24, r18
+    3f1c:	93 2b       	or	r25, r19
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
-    3f1a:	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);
-    3f1c:	fa 01       	movw	r30, r20
-    3f1e:	0c 01       	movw	r0, r24
-    3f20:	a7 be       	out	0x37, r10	; 55
-    3f22:	e8 95       	spm
-    3f24:	11 24       	eor	r1, r1
-        addrPtr += 2;
-    3f26:	4e 5f       	subi	r20, 0xFE	; 254
-    3f28:	5f 4f       	sbci	r21, 0xFF	; 255
-      } while (--ch);
-    3f2a:	f1 e0       	ldi	r31, 0x01	; 1
-    3f2c:	a0 38       	cpi	r26, 0x80	; 128
-    3f2e:	bf 07       	cpc	r27, r31
-    3f30:	51 f7       	brne	.-44     	; 0x3f06 <main+0x106>
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    3f32:	f7 01       	movw	r30, r14
-    3f34:	c7 be       	out	0x37, r12	; 55
-    3f36:	e8 95       	spm
-      boot_spm_busy_wait();
-    3f38:	07 b6       	in	r0, 0x37	; 55
-    3f3a:	00 fc       	sbrc	r0, 0
-    3f3c:	fd cf       	rjmp	.-6      	; 0x3f38 <main+0x138>
+    3f1e:	12 96       	adiw	r26, 0x02	; 2
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3f20:	fb 01       	movw	r30, r22
+    3f22:	0c 01       	movw	r0, r24
+    3f24:	77 be       	out	0x37, r7	; 55
+    3f26:	e8 95       	spm
+    3f28:	11 24       	eor	r1, r1
+		addrPtr += 2;
+    3f2a:	6e 5f       	subi	r22, 0xFE	; 254
+    3f2c:	7f 4f       	sbci	r23, 0xFF	; 255
+	    } while (len -= 2);
+    3f2e:	42 50       	subi	r20, 0x02	; 2
+    3f30:	50 40       	sbci	r21, 0x00	; 0
+    3f32:	59 f7       	brne	.-42     	; 0x3f0a <main+0x10a>
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    3f34:	f5 01       	movw	r30, r10
+    3f36:	97 be       	out	0x37, r9	; 55
+    3f38:	e8 95       	spm
+	    boot_spm_busy_wait();
+    3f3a:	07 b6       	in	r0, 0x37	; 55
+    3f3c:	00 fc       	sbrc	r0, 0
+    3f3e:	fd cf       	rjmp	.-6      	; 0x3f3a <main+0x13a>
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    3f3e:	b7 be       	out	0x37, r11	; 55
-    3f40:	e8 95       	spm
-    3f42:	1c c0       	rjmp	.+56     	; 0x3f7c <main+0x17c>
-#endif
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    3f40:	c7 be       	out	0x37, r12	; 55
+    3f42:	e8 95       	spm
+    3f44:	22 c0       	rjmp	.+68     	; 0x3f8a <main+0x18a>
+      writebuffer(desttype, buff, address, savelength);
+
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f44:	84 37       	cpi	r24, 0x74	; 116
-    3f46:	61 f4       	brne	.+24     	; 0x3f60 <main+0x160>
-      // READ PAGE - we only read flash
-      getch();			/* getlen() */
-    3f48:	24 d0       	rcall	.+72     	; 0x3f92 <getch>
-      length = getch();
-    3f4a:	23 d0       	rcall	.+70     	; 0x3f92 <getch>
-    3f4c:	08 2f       	mov	r16, r24
-      getch();
-    3f4e:	21 d0       	rcall	.+66     	; 0x3f92 <getch>
+    3f46:	84 37       	cpi	r24, 0x74	; 116
+    3f48:	91 f4       	brne	.+36     	; 0x3f6e <main+0x16e>
+      uint8_t desttype;
+      length = getch()<<8;			/* getlen() */
+    3f4a:	2a d0       	rcall	.+84     	; 0x3fa0 <getch>
+    3f4c:	90 e0       	ldi	r25, 0x00	; 0
+    3f4e:	d8 2f       	mov	r29, r24
+    3f50:	cc 27       	eor	r28, r28
+      length |= getch();
+    3f52:	26 d0       	rcall	.+76     	; 0x3fa0 <getch>
+    3f54:	90 e0       	ldi	r25, 0x00	; 0
+    3f56:	c8 2b       	or	r28, r24
+    3f58:	d9 2b       	or	r29, r25
+      desttype = getch();
+    3f5a:	22 d0       	rcall	.+68     	; 0x3fa0 <getch>
 
       verifySpace();
-    3f50:	32 d0       	rcall	.+100    	; 0x3fb6 <verifySpace>
-        __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+    3f5c:	33 d0       	rcall	.+102    	; 0x3fc4 <verifySpace>
+    3f5e:	85 01       	movw	r16, r10
+	    __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #else
-        // read a Flash byte and increment the address
-        __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+	    // read a Flash byte and increment the address
+	    __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #endif
-        putch(ch);
-    3f52:	f7 01       	movw	r30, r14
-    3f54:	85 91       	lpm	r24, Z+
-    3f56:	7f 01       	movw	r14, r30
-    3f58:	14 d0       	rcall	.+40     	; 0x3f82 <putch>
-      } while (--length);
-    3f5a:	01 50       	subi	r16, 0x01	; 1
-    3f5c:	d1 f7       	brne	.-12     	; 0x3f52 <main+0x152>
-    3f5e:	0e c0       	rjmp	.+28     	; 0x3f7c <main+0x17c>
+	    putch(ch);
+    3f60:	f8 01       	movw	r30, r16
+    3f62:	85 91       	lpm	r24, Z+
+    3f64:	8f 01       	movw	r16, r30
+    3f66:	14 d0       	rcall	.+40     	; 0x3f90 <putch>
+	} while (--length);
+    3f68:	21 97       	sbiw	r28, 0x01	; 1
+    3f6a:	d1 f7       	brne	.-12     	; 0x3f60 <main+0x160>
+    3f6c:	0e c0       	rjmp	.+28     	; 0x3f8a <main+0x18a>
+	  
+      read_mem(desttype, address, length);
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f60:	85 37       	cpi	r24, 0x75	; 117
-    3f62:	39 f4       	brne	.+14     	; 0x3f72 <main+0x172>
+    3f6e:	85 37       	cpi	r24, 0x75	; 117
+    3f70:	39 f4       	brne	.+14     	; 0x3f80 <main+0x180>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f64:	28 d0       	rcall	.+80     	; 0x3fb6 <verifySpace>
+    3f72:	28 d0       	rcall	.+80     	; 0x3fc4 <verifySpace>
       putch(SIGNATURE_0);
-    3f66:	8e e1       	ldi	r24, 0x1E	; 30
-    3f68:	0c d0       	rcall	.+24     	; 0x3f82 <putch>
+    3f74:	8e e1       	ldi	r24, 0x1E	; 30
+    3f76:	0c d0       	rcall	.+24     	; 0x3f90 <putch>
       putch(SIGNATURE_1);
-    3f6a:	84 e9       	ldi	r24, 0x94	; 148
-    3f6c:	0a d0       	rcall	.+20     	; 0x3f82 <putch>
+    3f78:	84 e9       	ldi	r24, 0x94	; 148
+    3f7a:	0a d0       	rcall	.+20     	; 0x3f90 <putch>
       putch(SIGNATURE_2);
-    3f6e:	86 e0       	ldi	r24, 0x06	; 6
-    3f70:	84 cf       	rjmp	.-248    	; 0x3e7a <main+0x7a>
+    3f7c:	86 e0       	ldi	r24, 0x06	; 6
+    3f7e:	7d cf       	rjmp	.-262    	; 0x3e7a <main+0x7a>
     }
     else if (ch == STK_LEAVE_PROGMODE) { /* 'Q' */
-    3f72:	81 35       	cpi	r24, 0x51	; 81
-    3f74:	11 f4       	brne	.+4      	; 0x3f7a <main+0x17a>
+    3f80:	81 35       	cpi	r24, 0x51	; 81
+    3f82:	11 f4       	brne	.+4      	; 0x3f88 <main+0x188>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f76:	88 e0       	ldi	r24, 0x08	; 8
-    3f78:	18 d0       	rcall	.+48     	; 0x3faa <watchdogConfig>
+    3f84:	88 e0       	ldi	r24, 0x08	; 8
+    3f86:	18 d0       	rcall	.+48     	; 0x3fb8 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f7a:	1d d0       	rcall	.+58     	; 0x3fb6 <verifySpace>
+    3f88:	1d d0       	rcall	.+58     	; 0x3fc4 <verifySpace>
     }
     putch(STK_OK);
-    3f7c:	80 e1       	ldi	r24, 0x10	; 16
-    3f7e:	01 d0       	rcall	.+2      	; 0x3f82 <putch>
-    3f80:	6f cf       	rjmp	.-290    	; 0x3e60 <main+0x60>
+    3f8a:	80 e1       	ldi	r24, 0x10	; 16
+    3f8c:	01 d0       	rcall	.+2      	; 0x3f90 <putch>
+    3f8e:	69 cf       	rjmp	.-302    	; 0x3e62 <main+0x62>
 
-00003f82 <putch>:
+00003f90 <putch>:
   }
 }
 
 void putch(char ch) {
-    3f82:	98 2f       	mov	r25, r24
+    3f90:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UART_SRA & _BV(UDRE0)));
-    3f84:	80 91 c0 00 	lds	r24, 0x00C0
-    3f88:	85 ff       	sbrs	r24, 5
-    3f8a:	fc cf       	rjmp	.-8      	; 0x3f84 <putch+0x2>
+    3f92:	80 91 c0 00 	lds	r24, 0x00C0
+    3f96:	85 ff       	sbrs	r24, 5
+    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <putch+0x2>
   UART_UDR = ch;
-    3f8c:	90 93 c6 00 	sts	0x00C6, r25
+    3f9a:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3f90:	08 95       	ret
+    3f9e:	08 95       	ret
 
-00003f92 <getch>:
+00003fa0 <getch>:
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UART_SRA & _BV(RXC0)))
-    3f92:	80 91 c0 00 	lds	r24, 0x00C0
-    3f96:	87 ff       	sbrs	r24, 7
-    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <getch>
+    3fa0:	80 91 c0 00 	lds	r24, 0x00C0
+    3fa4:	87 ff       	sbrs	r24, 7
+    3fa6:	fc cf       	rjmp	.-8      	; 0x3fa0 <getch>
     ;
   if (!(UART_SRA & _BV(FE0))) {
-    3f9a:	80 91 c0 00 	lds	r24, 0x00C0
-    3f9e:	84 fd       	sbrc	r24, 4
-    3fa0:	01 c0       	rjmp	.+2      	; 0x3fa4 <getch+0x12>
+    3fa8:	80 91 c0 00 	lds	r24, 0x00C0
+    3fac:	84 fd       	sbrc	r24, 4
+    3fae:	01 c0       	rjmp	.+2      	; 0x3fb2 <getch+0x12>
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fa2:	a8 95       	wdr
+    3fb0:	a8 95       	wdr
        * don't care that an invalid char is returned...)
        */
     watchdogReset();
   }
   
   ch = UART_UDR;
-    3fa4:	80 91 c6 00 	lds	r24, 0x00C6
+    3fb2:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    3fa8:	08 95       	ret
+    3fb6:	08 95       	ret
 
-00003faa <watchdogConfig>:
+00003fb8 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3faa:	e0 e6       	ldi	r30, 0x60	; 96
-    3fac:	f0 e0       	ldi	r31, 0x00	; 0
-    3fae:	98 e1       	ldi	r25, 0x18	; 24
-    3fb0:	90 83       	st	Z, r25
+    3fb8:	e0 e6       	ldi	r30, 0x60	; 96
+    3fba:	f0 e0       	ldi	r31, 0x00	; 0
+    3fbc:	98 e1       	ldi	r25, 0x18	; 24
+    3fbe:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fb2:	80 83       	st	Z, r24
+    3fc0:	80 83       	st	Z, r24
 }
-    3fb4:	08 95       	ret
+    3fc2:	08 95       	ret
 
-00003fb6 <verifySpace>:
+00003fc4 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) {
-    3fb6:	ed df       	rcall	.-38     	; 0x3f92 <getch>
-    3fb8:	80 32       	cpi	r24, 0x20	; 32
-    3fba:	19 f0       	breq	.+6      	; 0x3fc2 <verifySpace+0xc>
+    3fc4:	ed df       	rcall	.-38     	; 0x3fa0 <getch>
+    3fc6:	80 32       	cpi	r24, 0x20	; 32
+    3fc8:	19 f0       	breq	.+6      	; 0x3fd0 <verifySpace+0xc>
     watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
-    3fbc:	88 e0       	ldi	r24, 0x08	; 8
-    3fbe:	f5 df       	rcall	.-22     	; 0x3faa <watchdogConfig>
-    3fc0:	ff cf       	rjmp	.-2      	; 0x3fc0 <verifySpace+0xa>
+    3fca:	88 e0       	ldi	r24, 0x08	; 8
+    3fcc:	f5 df       	rcall	.-22     	; 0x3fb8 <watchdogConfig>
+    3fce:	ff cf       	rjmp	.-2      	; 0x3fce <verifySpace+0xa>
     while (1)			      // and busy-loop so that WD causes
       ;				      //  a reset and app start.
   }
   putch(STK_INSYNC);
-    3fc2:	84 e1       	ldi	r24, 0x14	; 20
+    3fd0:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fc4:	de cf       	rjmp	.-68     	; 0x3f82 <putch>
+    3fd2:	de cf       	rjmp	.-68     	; 0x3f90 <putch>
 
-00003fc6 <getNch>:
+00003fd4 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fc6:	1f 93       	push	r17
-    3fc8:	18 2f       	mov	r17, r24
+    3fd4:	1f 93       	push	r17
+    3fd6:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3fca:	e3 df       	rcall	.-58     	; 0x3f92 <getch>
-    3fcc:	11 50       	subi	r17, 0x01	; 1
-    3fce:	e9 f7       	brne	.-6      	; 0x3fca <getNch+0x4>
+    3fd8:	e3 df       	rcall	.-58     	; 0x3fa0 <getch>
+    3fda:	11 50       	subi	r17, 0x01	; 1
+    3fdc:	e9 f7       	brne	.-6      	; 0x3fd8 <getNch+0x4>
   verifySpace();
-    3fd0:	f2 df       	rcall	.-28     	; 0x3fb6 <verifySpace>
+    3fde:	f2 df       	rcall	.-28     	; 0x3fc4 <verifySpace>
 }
-    3fd2:	1f 91       	pop	r17
-    3fd4:	08 95       	ret
+    3fe0:	1f 91       	pop	r17
+    3fe2:	08 95       	ret
 
-00003fd6 <appStart>:
+00003fe4 <appStart>:
 
 void appStart(uint8_t rstFlags) {
   // save the reset flags in the designated register
   //  This can be saved in a main program by putting code in .init0 (which
   //  executes before normal c init code) to save R2 to a global variable.
   __asm__ __volatile__ ("mov r2, %0\n" :: "r" (rstFlags));
-    3fd6:	28 2e       	mov	r2, r24
+    3fe4:	28 2e       	mov	r2, r24
 
   watchdogConfig(WATCHDOG_OFF);
-    3fd8:	80 e0       	ldi	r24, 0x00	; 0
-    3fda:	e7 df       	rcall	.-50     	; 0x3faa <watchdogConfig>
+    3fe6:	80 e0       	ldi	r24, 0x00	; 0
+    3fe8:	e7 df       	rcall	.-50     	; 0x3fb8 <watchdogConfig>
   __asm__ __volatile__ (
-    3fdc:	ee 27       	eor	r30, r30
-    3fde:	ff 27       	eor	r31, r31
-    3fe0:	09 94       	ijmp
+    3fea:	ee 27       	eor	r30, r30
+    3fec:	ff 27       	eor	r31, r31
+    3fee:	09 94       	ijmp
diff --git a/optiboot/bootloaders/optiboot/optiboot_pro_8MHz.hex b/optiboot/bootloaders/optiboot/optiboot_pro_8MHz.hex
index b43d1ed..83f5e2b 100644
--- a/optiboot/bootloaders/optiboot/optiboot_pro_8MHz.hex
+++ b/optiboot/bootloaders/optiboot/optiboot_pro_8MHz.hex
@@ -1,34 +1,34 @@
-:103E0000112484B714BE81FFE6D085E08093810041
-:103E100082E08093C00088E18093C10086E08093B7
-:103E2000C20088E08093C4008EE0BFD0259A86E06F
-:103E300028E13EEF91E0309385002093840096BB0B
-:103E4000B09BFECF1D9AA8958150A9F7EE24FF24C0
-:103E5000AA24A394B5E0CB2EA1E1BA2EF3E0DF2E85
-:103E600098D0813461F495D0082FA5D0023829F17B
-:103E7000013811F485E001C083E083D07FC0823433
-:103E800011F484E103C0853419F485E09CD076C038
-:103E9000853579F47ED0E82EFF247BD0082F10E002
-:103EA000102F00270E291F29000F111F84D0780121
-:103EB00065C0863521F484E086D080E0DECF84368C
-:103EC00009F040C066D065D0082F63D080E0E816C6
-:103ED00088E3F80618F4F701D7BEE895C0E0D1E012
-:103EE00058D089930C17E1F7F0E0EF16F8E3FF06DE
-:103EF00018F0F701D7BEE8955ED007B600FCFDCFFD
-:103F0000A701A0E0B1E02C9130E011968C911197BF
-:103F100090E0982F8827822B932B1296FA010C01A0
-:103F2000A7BEE89511244E5F5F4FF1E0A038BF07B0
-:103F300051F7F701C7BEE89507B600FCFDCFB7BE45
-:103F4000E8951CC0843761F424D023D0082F21D0F9
-:103F500032D0F70185917F0114D00150D1F70EC006
-:103F6000853739F428D08EE10CD084E90AD086E078
-:103F700084CF813511F488E018D01DD080E101D0C4
-:103F80006FCF982F8091C00085FFFCCF9093C60023
-:103F900008958091C00087FFFCCF8091C00084FD10
-:103FA00001C0A8958091C6000895E0E6F0E098E190
-:103FB000908380830895EDDF803219F088E0F5DF8B
-:103FC000FFCF84E1DECF1F93182FE3DF1150E9F715
-:103FD000F2DF1F910895282E80E0E7DFEE27FF270C
-:023FE000099442
-:023FFE000005BC
+:103E0000112484B714BE81FD01C0ECD085E08093FD
+:103E1000810082E08093C00088E18093C10086E049
+:103E20008093C20088E08093C4008EE0C5D0259ABC
+:103E300086E028E13EEF91E03093850020938400F6
+:103E400096BBB09BFECF1D9AA8958150A9F7AA24D6
+:103E5000BB2433E0832E7724739425E0922E91E1E6
+:103E6000C92E9ED0813459F49BD0182FABD0123874
+:103E700021F1113809F482C083E08AD086C08234EF
+:103E800011F484E103C0853419F485E0A3D07DC02A
+:103E9000853579F485D0E82EFF2482D0082F10E0F4
+:103EA000102F00270E291F29000F111F8BD058013A
+:103EB0006CC0863521F484E08DD080E0DECF84367E
+:103EC00009F041C06DD090E0182F002769D090E034
+:103ED000082B192B65D0D82EE801E12CF1E0FF2E3C
+:103EE0005FD0F70181937F012197D1F76BD0F5E483
+:103EF000DF1609F4FFCFF50187BEE89507B600FC91
+:103F0000FDCFB501A801A0E0B1E02C9130E0119601
+:103F10008C91119790E0982F8827822B932B1296E3
+:103F2000FB010C0177BEE89511246E5F7F4F425074
+:103F3000504059F7F50197BEE89507B600FCFDCF54
+:103F4000C7BEE89522C0843791F42AD090E0D82FDC
+:103F5000CC2726D090E0C82BD92B22D033D0850196
+:103F6000F80185918F0114D02197D1F70EC08537C4
+:103F700039F428D08EE10CD084E90AD086E07DCFD8
+:103F8000813511F488E018D01DD080E101D069CFCF
+:103F9000982F8091C00085FFFCCF9093C6000895B4
+:103FA0008091C00087FFFCCF8091C00084FD01C0DC
+:103FB000A8958091C6000895E0E6F0E098E190832E
+:103FC00080830895EDDF803219F088E0F5DFFFCFC0
+:103FD00084E1DECF1F93182FE3DF1150E9F7F2DF02
+:103FE0001F910895282E80E0E7DFEE27FF27099430
+:023FFE000006BB
 :0400000300003E00BB
 :00000001FF
diff --git a/optiboot/bootloaders/optiboot/optiboot_pro_8MHz.lst b/optiboot/bootloaders/optiboot/optiboot_pro_8MHz.lst
index d2d73c8..7f6a486 100644
--- a/optiboot/bootloaders/optiboot/optiboot_pro_8MHz.lst
+++ b/optiboot/bootloaders/optiboot/optiboot_pro_8MHz.lst
@@ -3,36 +3,36 @@ optiboot_atmega168.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001e2  00003e00  00003e00  00000054  2**1
+  0 .text         000001f0  00003e00  00003e00  00000074  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .version      00000002  00003ffe  00003ffe  00000236  2**0
-                  CONTENTS, READONLY
-  2 .debug_aranges 00000028  00000000  00000000  00000238  2**0
+  1 .version      00000002  00003ffe  00003ffe  00000264  2**0
+                  CONTENTS, ALLOC, LOAD, DATA
+  2 .debug_aranges 00000028  00000000  00000000  00000266  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_pubnames 0000005f  00000000  00000000  00000260  2**0
+  3 .debug_pubnames 00000074  00000000  00000000  0000028e  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_info   00000294  00000000  00000000  000002bf  2**0
+  4 .debug_info   000003db  00000000  00000000  00000302  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_abbrev 0000016b  00000000  00000000  00000553  2**0
+  5 .debug_abbrev 000001ea  00000000  00000000  000006dd  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_line   00000421  00000000  00000000  000006be  2**0
+  6 .debug_line   00000423  00000000  00000000  000008c7  2**0
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_frame  00000080  00000000  00000000  00000ae0  2**2
+  7 .debug_frame  00000080  00000000  00000000  00000cec  2**2
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_str    00000131  00000000  00000000  00000b60  2**0
+  8 .debug_str    00000172  00000000  00000000  00000d6c  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_loc    00000266  00000000  00000000  00000c91  2**0
+  9 .debug_loc    000002d7  00000000  00000000  00000ede  2**0
                   CONTENTS, READONLY, DEBUGGING
- 10 .debug_ranges 00000078  00000000  00000000  00000ef7  2**0
+ 10 .debug_ranges 000000b8  00000000  00000000  000011b5  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00003e00 <main>:
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
     3e00:	11 24       	eor	r1, r1
@@ -46,148 +46,147 @@ int main(void) {
   MCUSR = 0;
     3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart(ch);
-    3e06:	81 ff       	sbrs	r24, 1
-    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+    3e06:	81 fd       	sbrc	r24, 1
+    3e08:	01 c0       	rjmp	.+2      	; 0x3e0c <main+0xc>
+    3e0a:	ec d0       	rcall	.+472    	; 0x3fe4 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-    3e0a:	85 e0       	ldi	r24, 0x05	; 5
-    3e0c:	80 93 81 00 	sts	0x0081, r24
+    3e0c:	85 e0       	ldi	r24, 0x05	; 5
+    3e0e:	80 93 81 00 	sts	0x0081, r24
   UCSRA = _BV(U2X); //Double speed mode USART
   UCSRB = _BV(RXEN) | _BV(TXEN);  // enable Rx & Tx
   UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0);  // config USART; 8N1
   UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
 #else
   UART_SRA = _BV(U2X0); //Double speed mode USART0
-    3e10:	82 e0       	ldi	r24, 0x02	; 2
-    3e12:	80 93 c0 00 	sts	0x00C0, r24
+    3e12:	82 e0       	ldi	r24, 0x02	; 2
+    3e14:	80 93 c0 00 	sts	0x00C0, r24
   UART_SRB = _BV(RXEN0) | _BV(TXEN0);
-    3e16:	88 e1       	ldi	r24, 0x18	; 24
-    3e18:	80 93 c1 00 	sts	0x00C1, r24
+    3e18:	88 e1       	ldi	r24, 0x18	; 24
+    3e1a:	80 93 c1 00 	sts	0x00C1, r24
   UART_SRC = _BV(UCSZ00) | _BV(UCSZ01);
-    3e1c:	86 e0       	ldi	r24, 0x06	; 6
-    3e1e:	80 93 c2 00 	sts	0x00C2, r24
+    3e1e:	86 e0       	ldi	r24, 0x06	; 6
+    3e20:	80 93 c2 00 	sts	0x00C2, r24
   UART_SRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    3e22:	88 e0       	ldi	r24, 0x08	; 8
-    3e24:	80 93 c4 00 	sts	0x00C4, r24
+    3e24:	88 e0       	ldi	r24, 0x08	; 8
+    3e26:	80 93 c4 00 	sts	0x00C4, r24
 #endif
 #endif
 
   // Set up watchdog to trigger after 500ms
   watchdogConfig(WATCHDOG_1S);
-    3e28:	8e e0       	ldi	r24, 0x0E	; 14
-    3e2a:	bf d0       	rcall	.+382    	; 0x3faa <watchdogConfig>
+    3e2a:	8e e0       	ldi	r24, 0x0E	; 14
+    3e2c:	c5 d0       	rcall	.+394    	; 0x3fb8 <watchdogConfig>
 
 #if (LED_START_FLASHES > 0) || defined(LED_DATA_FLASH)
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    3e2c:	25 9a       	sbi	0x04, 5	; 4
-    3e2e:	86 e0       	ldi	r24, 0x06	; 6
+    3e2e:	25 9a       	sbi	0x04, 5	; 4
+    3e30:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e30:	28 e1       	ldi	r18, 0x18	; 24
-    3e32:	3e ef       	ldi	r19, 0xFE	; 254
+    3e32:	28 e1       	ldi	r18, 0x18	; 24
+    3e34:	3e ef       	ldi	r19, 0xFE	; 254
     TIFR1 = _BV(TOV1);
-    3e34:	91 e0       	ldi	r25, 0x01	; 1
+    3e36:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e36:	30 93 85 00 	sts	0x0085, r19
-    3e3a:	20 93 84 00 	sts	0x0084, r18
+    3e38:	30 93 85 00 	sts	0x0085, r19
+    3e3c:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    3e3e:	96 bb       	out	0x16, r25	; 22
+    3e40:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    3e40:	b0 9b       	sbis	0x16, 0	; 22
-    3e42:	fe cf       	rjmp	.-4      	; 0x3e40 <main+0x40>
+    3e42:	b0 9b       	sbis	0x16, 0	; 22
+    3e44:	fe cf       	rjmp	.-4      	; 0x3e42 <main+0x42>
 #if defined(__AVR_ATmega8__)  || defined (__AVR_ATmega32__)
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
-    3e44:	1d 9a       	sbi	0x03, 5	; 3
+    3e46:	1d 9a       	sbi	0x03, 5	; 3
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3e46:	a8 95       	wdr
+    3e48:	a8 95       	wdr
     LED_PORT ^= _BV(LED);
 #else
     LED_PIN |= _BV(LED);
 #endif
     watchdogReset();
   } while (--count);
-    3e48:	81 50       	subi	r24, 0x01	; 1
-    3e4a:	a9 f7       	brne	.-22     	; 0x3e36 <main+0x36>
-    3e4c:	ee 24       	eor	r14, r14
-    3e4e:	ff 24       	eor	r15, r15
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-        a |= (*bufPtr++) << 8;
-        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
-    3e50:	aa 24       	eor	r10, r10
-    3e52:	a3 94       	inc	r10
-        addrPtr += 2;
-      } while (--ch);
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    3e54:	b5 e0       	ldi	r27, 0x05	; 5
-    3e56:	cb 2e       	mov	r12, r27
-      boot_spm_busy_wait();
+    3e4a:	81 50       	subi	r24, 0x01	; 1
+    3e4c:	a9 f7       	brne	.-22     	; 0x3e38 <main+0x38>
+    3e4e:	aa 24       	eor	r10, r10
+    3e50:	bb 24       	eor	r11, r11
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    3e52:	33 e0       	ldi	r19, 0x03	; 3
+    3e54:	83 2e       	mov	r8, r19
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3e56:	77 24       	eor	r7, r7
+    3e58:	73 94       	inc	r7
+	    } while (len -= 2);
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    3e5a:	25 e0       	ldi	r18, 0x05	; 5
+    3e5c:	92 2e       	mov	r9, r18
+	    boot_spm_busy_wait();
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    3e58:	a1 e1       	ldi	r26, 0x11	; 17
-    3e5a:	ba 2e       	mov	r11, r26
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3e5c:	f3 e0       	ldi	r31, 0x03	; 3
-    3e5e:	df 2e       	mov	r13, r31
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    3e5e:	91 e1       	ldi	r25, 0x11	; 17
+    3e60:	c9 2e       	mov	r12, r25
 #endif
 
-  /* Forever loop */
+  /* Forever loop: exits by causing WDT reset */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e60:	98 d0       	rcall	.+304    	; 0x3f92 <getch>
+    3e62:	9e d0       	rcall	.+316    	; 0x3fa0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    3e62:	81 34       	cpi	r24, 0x41	; 65
-    3e64:	61 f4       	brne	.+24     	; 0x3e7e <main+0x7e>
+    3e64:	81 34       	cpi	r24, 0x41	; 65
+    3e66:	59 f4       	brne	.+22     	; 0x3e7e <main+0x7e>
       unsigned char which = getch();
-    3e66:	95 d0       	rcall	.+298    	; 0x3f92 <getch>
-    3e68:	08 2f       	mov	r16, r24
+    3e68:	9b d0       	rcall	.+310    	; 0x3fa0 <getch>
+    3e6a:	18 2f       	mov	r17, r24
       verifySpace();
-    3e6a:	a5 d0       	rcall	.+330    	; 0x3fb6 <verifySpace>
+    3e6c:	ab d0       	rcall	.+342    	; 0x3fc4 <verifySpace>
       if (which == 0x82) {
-    3e6c:	02 38       	cpi	r16, 0x82	; 130
-    3e6e:	29 f1       	breq	.+74     	; 0x3eba <main+0xba>
+    3e6e:	12 38       	cpi	r17, 0x82	; 130
+    3e70:	21 f1       	breq	.+72     	; 0x3eba <main+0xba>
 	/*
 	 * Send optiboot version as "minor SW version"
 	 */
 	putch(OPTIBOOT_MINVER);
       } else if (which == 0x81) {
-    3e70:	01 38       	cpi	r16, 0x81	; 129
-    3e72:	11 f4       	brne	.+4      	; 0x3e78 <main+0x78>
-	  putch(OPTIBOOT_MAJVER);
-    3e74:	85 e0       	ldi	r24, 0x05	; 5
-    3e76:	01 c0       	rjmp	.+2      	; 0x3e7a <main+0x7a>
+    3e72:	11 38       	cpi	r17, 0x81	; 129
+    3e74:	09 f4       	brne	.+2      	; 0x3e78 <main+0x78>
+    3e76:	82 c0       	rjmp	.+260    	; 0x3f7c <main+0x17c>
       } else {
 	/*
 	 * GET PARAMETER returns a generic 0x03 reply for
@@ -195,8 +194,8 @@ void watchdogReset() {
 	 */
 	putch(0x03);
     3e78:	83 e0       	ldi	r24, 0x03	; 3
-    3e7a:	83 d0       	rcall	.+262    	; 0x3f82 <putch>
-    3e7c:	7f c0       	rjmp	.+254    	; 0x3f7c <main+0x17c>
+    3e7a:	8a d0       	rcall	.+276    	; 0x3f90 <putch>
+    3e7c:	86 c0       	rjmp	.+268    	; 0x3f8a <main+0x18a>
       }
     }
     else if(ch == STK_SET_DEVICE) {
@@ -213,8 +212,8 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     3e8a:	85 e0       	ldi	r24, 0x05	; 5
-    3e8c:	9c d0       	rcall	.+312    	; 0x3fc6 <getNch>
-    3e8e:	76 c0       	rjmp	.+236    	; 0x3f7c <main+0x17c>
+    3e8c:	a3 d0       	rcall	.+326    	; 0x3fd4 <getNch>
+    3e8e:	7d c0       	rjmp	.+250    	; 0x3f8a <main+0x18a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     3e90:	85 35       	cpi	r24, 0x55	; 85
@@ -222,11 +221,11 @@ void watchdogReset() {
       // LOAD ADDRESS
       uint16_t newAddress;
       newAddress = getch();
-    3e94:	7e d0       	rcall	.+252    	; 0x3f92 <getch>
+    3e94:	85 d0       	rcall	.+266    	; 0x3fa0 <getch>
       newAddress = (newAddress & 0xff) | (getch() << 8);
     3e96:	e8 2e       	mov	r14, r24
     3e98:	ff 24       	eor	r15, r15
-    3e9a:	7b d0       	rcall	.+246    	; 0x3f92 <getch>
+    3e9a:	82 d0       	rcall	.+260    	; 0x3fa0 <getch>
     3e9c:	08 2f       	mov	r16, r24
     3e9e:	10 e0       	ldi	r17, 0x00	; 0
     3ea0:	10 2f       	mov	r17, r16
@@ -242,9 +241,9 @@ void watchdogReset() {
     3eaa:	11 1f       	adc	r17, r17
       address = newAddress;
       verifySpace();
-    3eac:	84 d0       	rcall	.+264    	; 0x3fb6 <verifySpace>
-    3eae:	78 01       	movw	r14, r16
-    3eb0:	65 c0       	rjmp	.+202    	; 0x3f7c <main+0x17c>
+    3eac:	8b d0       	rcall	.+278    	; 0x3fc4 <verifySpace>
+    3eae:	58 01       	movw	r10, r16
+    3eb0:	6c c0       	rjmp	.+216    	; 0x3f8a <main+0x18a>
     }
     else if(ch == STK_UNIVERSAL) {
     3eb2:	86 35       	cpi	r24, 0x56	; 86
@@ -252,7 +251,7 @@ void watchdogReset() {
       // UNIVERSAL command is ignored
       getNch(4);
     3eb6:	84 e0       	ldi	r24, 0x04	; 4
-    3eb8:	86 d0       	rcall	.+268    	; 0x3fc6 <getNch>
+    3eb8:	8d d0       	rcall	.+282    	; 0x3fd4 <getNch>
       putch(0x00);
     3eba:	80 e0       	ldi	r24, 0x00	; 0
     3ebc:	de cf       	rjmp	.-68     	; 0x3e7a <main+0x7a>
@@ -261,326 +260,339 @@ void watchdogReset() {
     else if(ch == STK_PROG_PAGE) {
     3ebe:	84 36       	cpi	r24, 0x64	; 100
     3ec0:	09 f0       	breq	.+2      	; 0x3ec4 <main+0xc4>
-    3ec2:	40 c0       	rjmp	.+128    	; 0x3f44 <main+0x144>
+    3ec2:	41 c0       	rjmp	.+130    	; 0x3f46 <main+0x146>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
+      uint8_t desttype;
       uint8_t *bufPtr;
-      uint16_t addrPtr;
+      uint16_t savelength;
 
-      getch();			/* getlen() */
-    3ec4:	66 d0       	rcall	.+204    	; 0x3f92 <getch>
-      length = getch();
-    3ec6:	65 d0       	rcall	.+202    	; 0x3f92 <getch>
-    3ec8:	08 2f       	mov	r16, r24
-      getch();
-    3eca:	63 d0       	rcall	.+198    	; 0x3f92 <getch>
+      length = getch()<<8;			/* getlen() */
+    3ec4:	6d d0       	rcall	.+218    	; 0x3fa0 <getch>
+    3ec6:	90 e0       	ldi	r25, 0x00	; 0
+    3ec8:	18 2f       	mov	r17, r24
+    3eca:	00 27       	eor	r16, r16
+      length |= getch();
+    3ecc:	69 d0       	rcall	.+210    	; 0x3fa0 <getch>
+    3ece:	90 e0       	ldi	r25, 0x00	; 0
+    3ed0:	08 2b       	or	r16, r24
+    3ed2:	19 2b       	or	r17, r25
+      savelength = length;
+      desttype = getch();
+    3ed4:	65 d0       	rcall	.+202    	; 0x3fa0 <getch>
+    3ed6:	d8 2e       	mov	r13, r24
+    3ed8:	e8 01       	movw	r28, r16
+    3eda:	e1 2c       	mov	r14, r1
+    3edc:	f1 e0       	ldi	r31, 0x01	; 1
+    3ede:	ff 2e       	mov	r15, r31
 
-      // If we are in RWW section, immediately start page erase
-      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ecc:	80 e0       	ldi	r24, 0x00	; 0
-    3ece:	e8 16       	cp	r14, r24
-    3ed0:	88 e3       	ldi	r24, 0x38	; 56
-    3ed2:	f8 06       	cpc	r15, r24
-    3ed4:	18 f4       	brcc	.+6      	; 0x3edc <main+0xdc>
-    3ed6:	f7 01       	movw	r30, r14
-    3ed8:	d7 be       	out	0x37, r13	; 55
-    3eda:	e8 95       	spm
-    3edc:	c0 e0       	ldi	r28, 0x00	; 0
-    3ede:	d1 e0       	ldi	r29, 0x01	; 1
-
-      // While that is going on, read in page contents
+      // read a page worth of contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ee0:	58 d0       	rcall	.+176    	; 0x3f92 <getch>
-    3ee2:	89 93       	st	Y+, r24
+    3ee0:	5f d0       	rcall	.+190    	; 0x3fa0 <getch>
+    3ee2:	f7 01       	movw	r30, r14
+    3ee4:	81 93       	st	Z+, r24
+    3ee6:	7f 01       	movw	r14, r30
       while (--length);
-    3ee4:	0c 17       	cp	r16, r28
-    3ee6:	e1 f7       	brne	.-8      	; 0x3ee0 <main+0xe0>
-
-      // If we are in NRWW section, page erase has to be delayed until now.
-      // Todo: Take RAMPZ into account (not doing so just means that we will
-      //  treat the top of both "pages" of flash as NRWW, for a slight speed
-      //  decrease, so fixing this is not urgent.)
-      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
-    3ee8:	f0 e0       	ldi	r31, 0x00	; 0
-    3eea:	ef 16       	cp	r14, r31
-    3eec:	f8 e3       	ldi	r31, 0x38	; 56
-    3eee:	ff 06       	cpc	r15, r31
-    3ef0:	18 f0       	brcs	.+6      	; 0x3ef8 <main+0xf8>
-    3ef2:	f7 01       	movw	r30, r14
-    3ef4:	d7 be       	out	0x37, r13	; 55
-    3ef6:	e8 95       	spm
+    3ee8:	21 97       	sbiw	r28, 0x01	; 1
+    3eea:	d1 f7       	brne	.-12     	; 0x3ee0 <main+0xe0>
 
       // Read command terminator, start reply
       verifySpace();
-    3ef8:	5e d0       	rcall	.+188    	; 0x3fb6 <verifySpace>
-
-      // If only a partial page is to be programmed, the erase might not be complete.
-      // So check that here
-      boot_spm_busy_wait();
-    3efa:	07 b6       	in	r0, 0x37	; 55
-    3efc:	00 fc       	sbrc	r0, 0
-    3efe:	fd cf       	rjmp	.-6      	; 0x3efa <main+0xfa>
-    3f00:	a7 01       	movw	r20, r14
-    3f02:	a0 e0       	ldi	r26, 0x00	; 0
-    3f04:	b1 e0       	ldi	r27, 0x01	; 1
-      bufPtr = buff;
-      addrPtr = (uint16_t)(void*)address;
-      ch = SPM_PAGESIZE / 2;
-      do {
-        uint16_t a;
-        a = *bufPtr++;
-    3f06:	2c 91       	ld	r18, X
-    3f08:	30 e0       	ldi	r19, 0x00	; 0
-        a |= (*bufPtr++) << 8;
-    3f0a:	11 96       	adiw	r26, 0x01	; 1
-    3f0c:	8c 91       	ld	r24, X
-    3f0e:	11 97       	sbiw	r26, 0x01	; 1
-    3f10:	90 e0       	ldi	r25, 0x00	; 0
-    3f12:	98 2f       	mov	r25, r24
-    3f14:	88 27       	eor	r24, r24
-    3f16:	82 2b       	or	r24, r18
-    3f18:	93 2b       	or	r25, r19
-# define UART_SRL UBRR3L
-# define UART_UDR UDR3
+    3eec:	6b d0       	rcall	.+214    	; 0x3fc4 <verifySpace>
+ * void writebuffer(memtype, buffer, address, length)
+ */
+static inline void writebuffer(int8_t memtype, uint8_t *mybuff,
+			       uint16_t address, uint16_t len)
+{
+    switch (memtype) {
+    3eee:	f5 e4       	ldi	r31, 0x45	; 69
+    3ef0:	df 16       	cp	r13, r31
+    3ef2:	09 f4       	brne	.+2      	; 0x3ef6 <main+0xf6>
+    3ef4:	ff cf       	rjmp	.-2      	; 0x3ef4 <main+0xf4>
+	     * Start the page erase and wait for it to finish.  There
+	     * used to be code to do this while receiving the data over
+	     * the serial link, but the performance improvement was slight,
+	     * and we needed the space back.
+	     */
+	    __boot_page_erase_short((uint16_t)(void*)address);
+    3ef6:	f5 01       	movw	r30, r10
+    3ef8:	87 be       	out	0x37, r8	; 55
+    3efa:	e8 95       	spm
+	    boot_spm_busy_wait();
+    3efc:	07 b6       	in	r0, 0x37	; 55
+    3efe:	00 fc       	sbrc	r0, 0
+    3f00:	fd cf       	rjmp	.-6      	; 0x3efc <main+0xfc>
+    3f02:	b5 01       	movw	r22, r10
+    3f04:	a8 01       	movw	r20, r16
+    3f06:	a0 e0       	ldi	r26, 0x00	; 0
+    3f08:	b1 e0       	ldi	r27, 0x01	; 1
+	    /*
+	     * Copy data from the buffer into the flash write buffer.
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+    3f0a:	2c 91       	ld	r18, X
+    3f0c:	30 e0       	ldi	r19, 0x00	; 0
+		a |= (*bufPtr++) << 8;
+    3f0e:	11 96       	adiw	r26, 0x01	; 1
+    3f10:	8c 91       	ld	r24, X
+    3f12:	11 97       	sbiw	r26, 0x01	; 1
+    3f14:	90 e0       	ldi	r25, 0x00	; 0
+    3f16:	98 2f       	mov	r25, r24
+    3f18:	88 27       	eor	r24, r24
+    3f1a:	82 2b       	or	r24, r18
+    3f1c:	93 2b       	or	r25, r19
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
 
+
 /* main program starts here */
 int main(void) {
-    3f1a:	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);
-    3f1c:	fa 01       	movw	r30, r20
-    3f1e:	0c 01       	movw	r0, r24
-    3f20:	a7 be       	out	0x37, r10	; 55
-    3f22:	e8 95       	spm
-    3f24:	11 24       	eor	r1, r1
-        addrPtr += 2;
-    3f26:	4e 5f       	subi	r20, 0xFE	; 254
-    3f28:	5f 4f       	sbci	r21, 0xFF	; 255
-      } while (--ch);
-    3f2a:	f1 e0       	ldi	r31, 0x01	; 1
-    3f2c:	a0 38       	cpi	r26, 0x80	; 128
-    3f2e:	bf 07       	cpc	r27, r31
-    3f30:	51 f7       	brne	.-44     	; 0x3f06 <main+0x106>
-
-      // Write from programming buffer
-      __boot_page_write_short((uint16_t)(void*)address);
-    3f32:	f7 01       	movw	r30, r14
-    3f34:	c7 be       	out	0x37, r12	; 55
-    3f36:	e8 95       	spm
-      boot_spm_busy_wait();
-    3f38:	07 b6       	in	r0, 0x37	; 55
-    3f3a:	00 fc       	sbrc	r0, 0
-    3f3c:	fd cf       	rjmp	.-6      	; 0x3f38 <main+0x138>
+    3f1e:	12 96       	adiw	r26, 0x02	; 2
+	     */
+	    do {
+		uint16_t a;
+		a = *bufPtr++;
+		a |= (*bufPtr++) << 8;
+		__boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3f20:	fb 01       	movw	r30, r22
+    3f22:	0c 01       	movw	r0, r24
+    3f24:	77 be       	out	0x37, r7	; 55
+    3f26:	e8 95       	spm
+    3f28:	11 24       	eor	r1, r1
+		addrPtr += 2;
+    3f2a:	6e 5f       	subi	r22, 0xFE	; 254
+    3f2c:	7f 4f       	sbci	r23, 0xFF	; 255
+	    } while (len -= 2);
+    3f2e:	42 50       	subi	r20, 0x02	; 2
+    3f30:	50 40       	sbci	r21, 0x00	; 0
+    3f32:	59 f7       	brne	.-42     	; 0x3f0a <main+0x10a>
 
+	    /*
+	     * Actually Write the buffer to flash (and wait for it to finish.)
+	     */
+	    __boot_page_write_short((uint16_t)(void*)address);
+    3f34:	f5 01       	movw	r30, r10
+    3f36:	97 be       	out	0x37, r9	; 55
+    3f38:	e8 95       	spm
+	    boot_spm_busy_wait();
+    3f3a:	07 b6       	in	r0, 0x37	; 55
+    3f3c:	00 fc       	sbrc	r0, 0
+    3f3e:	fd cf       	rjmp	.-6      	; 0x3f3a <main+0x13a>
 #if defined(RWWSRE)
-      // Reenable read access to flash
-      boot_rww_enable();
-    3f3e:	b7 be       	out	0x37, r11	; 55
-    3f40:	e8 95       	spm
-    3f42:	1c c0       	rjmp	.+56     	; 0x3f7c <main+0x17c>
-#endif
+	    // Reenable read access to flash
+	    boot_rww_enable();
+    3f40:	c7 be       	out	0x37, r12	; 55
+    3f42:	e8 95       	spm
+    3f44:	22 c0       	rjmp	.+68     	; 0x3f8a <main+0x18a>
+      writebuffer(desttype, buff, address, savelength);
+
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f44:	84 37       	cpi	r24, 0x74	; 116
-    3f46:	61 f4       	brne	.+24     	; 0x3f60 <main+0x160>
-      // READ PAGE - we only read flash
-      getch();			/* getlen() */
-    3f48:	24 d0       	rcall	.+72     	; 0x3f92 <getch>
-      length = getch();
-    3f4a:	23 d0       	rcall	.+70     	; 0x3f92 <getch>
-    3f4c:	08 2f       	mov	r16, r24
-      getch();
-    3f4e:	21 d0       	rcall	.+66     	; 0x3f92 <getch>
+    3f46:	84 37       	cpi	r24, 0x74	; 116
+    3f48:	91 f4       	brne	.+36     	; 0x3f6e <main+0x16e>
+      uint8_t desttype;
+      length = getch()<<8;			/* getlen() */
+    3f4a:	2a d0       	rcall	.+84     	; 0x3fa0 <getch>
+    3f4c:	90 e0       	ldi	r25, 0x00	; 0
+    3f4e:	d8 2f       	mov	r29, r24
+    3f50:	cc 27       	eor	r28, r28
+      length |= getch();
+    3f52:	26 d0       	rcall	.+76     	; 0x3fa0 <getch>
+    3f54:	90 e0       	ldi	r25, 0x00	; 0
+    3f56:	c8 2b       	or	r28, r24
+    3f58:	d9 2b       	or	r29, r25
+      desttype = getch();
+    3f5a:	22 d0       	rcall	.+68     	; 0x3fa0 <getch>
 
       verifySpace();
-    3f50:	32 d0       	rcall	.+100    	; 0x3fb6 <verifySpace>
-        __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+    3f5c:	33 d0       	rcall	.+102    	; 0x3fc4 <verifySpace>
+    3f5e:	85 01       	movw	r16, r10
+	    __asm__ ("elpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #else
-        // read a Flash byte and increment the address
-        __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
+	    // read a Flash byte and increment the address
+	    __asm__ ("lpm %0,Z+\n" : "=r" (ch), "=z" (address): "1" (address));
 #endif
-        putch(ch);
-    3f52:	f7 01       	movw	r30, r14
-    3f54:	85 91       	lpm	r24, Z+
-    3f56:	7f 01       	movw	r14, r30
-    3f58:	14 d0       	rcall	.+40     	; 0x3f82 <putch>
-      } while (--length);
-    3f5a:	01 50       	subi	r16, 0x01	; 1
-    3f5c:	d1 f7       	brne	.-12     	; 0x3f52 <main+0x152>
-    3f5e:	0e c0       	rjmp	.+28     	; 0x3f7c <main+0x17c>
+	    putch(ch);
+    3f60:	f8 01       	movw	r30, r16
+    3f62:	85 91       	lpm	r24, Z+
+    3f64:	8f 01       	movw	r16, r30
+    3f66:	14 d0       	rcall	.+40     	; 0x3f90 <putch>
+	} while (--length);
+    3f68:	21 97       	sbiw	r28, 0x01	; 1
+    3f6a:	d1 f7       	brne	.-12     	; 0x3f60 <main+0x160>
+    3f6c:	0e c0       	rjmp	.+28     	; 0x3f8a <main+0x18a>
+	  
+      read_mem(desttype, address, length);
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f60:	85 37       	cpi	r24, 0x75	; 117
-    3f62:	39 f4       	brne	.+14     	; 0x3f72 <main+0x172>
+    3f6e:	85 37       	cpi	r24, 0x75	; 117
+    3f70:	39 f4       	brne	.+14     	; 0x3f80 <main+0x180>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f64:	28 d0       	rcall	.+80     	; 0x3fb6 <verifySpace>
+    3f72:	28 d0       	rcall	.+80     	; 0x3fc4 <verifySpace>
       putch(SIGNATURE_0);
-    3f66:	8e e1       	ldi	r24, 0x1E	; 30
-    3f68:	0c d0       	rcall	.+24     	; 0x3f82 <putch>
+    3f74:	8e e1       	ldi	r24, 0x1E	; 30
+    3f76:	0c d0       	rcall	.+24     	; 0x3f90 <putch>
       putch(SIGNATURE_1);
-    3f6a:	84 e9       	ldi	r24, 0x94	; 148
-    3f6c:	0a d0       	rcall	.+20     	; 0x3f82 <putch>
+    3f78:	84 e9       	ldi	r24, 0x94	; 148
+    3f7a:	0a d0       	rcall	.+20     	; 0x3f90 <putch>
       putch(SIGNATURE_2);
-    3f6e:	86 e0       	ldi	r24, 0x06	; 6
-    3f70:	84 cf       	rjmp	.-248    	; 0x3e7a <main+0x7a>
+    3f7c:	86 e0       	ldi	r24, 0x06	; 6
+    3f7e:	7d cf       	rjmp	.-262    	; 0x3e7a <main+0x7a>
     }
     else if (ch == STK_LEAVE_PROGMODE) { /* 'Q' */
-    3f72:	81 35       	cpi	r24, 0x51	; 81
-    3f74:	11 f4       	brne	.+4      	; 0x3f7a <main+0x17a>
+    3f80:	81 35       	cpi	r24, 0x51	; 81
+    3f82:	11 f4       	brne	.+4      	; 0x3f88 <main+0x188>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f76:	88 e0       	ldi	r24, 0x08	; 8
-    3f78:	18 d0       	rcall	.+48     	; 0x3faa <watchdogConfig>
+    3f84:	88 e0       	ldi	r24, 0x08	; 8
+    3f86:	18 d0       	rcall	.+48     	; 0x3fb8 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f7a:	1d d0       	rcall	.+58     	; 0x3fb6 <verifySpace>
+    3f88:	1d d0       	rcall	.+58     	; 0x3fc4 <verifySpace>
     }
     putch(STK_OK);
-    3f7c:	80 e1       	ldi	r24, 0x10	; 16
-    3f7e:	01 d0       	rcall	.+2      	; 0x3f82 <putch>
-    3f80:	6f cf       	rjmp	.-290    	; 0x3e60 <main+0x60>
+    3f8a:	80 e1       	ldi	r24, 0x10	; 16
+    3f8c:	01 d0       	rcall	.+2      	; 0x3f90 <putch>
+    3f8e:	69 cf       	rjmp	.-302    	; 0x3e62 <main+0x62>
 
-00003f82 <putch>:
+00003f90 <putch>:
   }
 }
 
 void putch(char ch) {
-    3f82:	98 2f       	mov	r25, r24
+    3f90:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UART_SRA & _BV(UDRE0)));
-    3f84:	80 91 c0 00 	lds	r24, 0x00C0
-    3f88:	85 ff       	sbrs	r24, 5
-    3f8a:	fc cf       	rjmp	.-8      	; 0x3f84 <putch+0x2>
+    3f92:	80 91 c0 00 	lds	r24, 0x00C0
+    3f96:	85 ff       	sbrs	r24, 5
+    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <putch+0x2>
   UART_UDR = ch;
-    3f8c:	90 93 c6 00 	sts	0x00C6, r25
+    3f9a:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3f90:	08 95       	ret
+    3f9e:	08 95       	ret
 
-00003f92 <getch>:
+00003fa0 <getch>:
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UART_SRA & _BV(RXC0)))
-    3f92:	80 91 c0 00 	lds	r24, 0x00C0
-    3f96:	87 ff       	sbrs	r24, 7
-    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <getch>
+    3fa0:	80 91 c0 00 	lds	r24, 0x00C0
+    3fa4:	87 ff       	sbrs	r24, 7
+    3fa6:	fc cf       	rjmp	.-8      	; 0x3fa0 <getch>
     ;
   if (!(UART_SRA & _BV(FE0))) {
-    3f9a:	80 91 c0 00 	lds	r24, 0x00C0
-    3f9e:	84 fd       	sbrc	r24, 4
-    3fa0:	01 c0       	rjmp	.+2      	; 0x3fa4 <getch+0x12>
+    3fa8:	80 91 c0 00 	lds	r24, 0x00C0
+    3fac:	84 fd       	sbrc	r24, 4
+    3fae:	01 c0       	rjmp	.+2      	; 0x3fb2 <getch+0x12>
 }
 #endif
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fa2:	a8 95       	wdr
+    3fb0:	a8 95       	wdr
        * don't care that an invalid char is returned...)
        */
     watchdogReset();
   }
   
   ch = UART_UDR;
-    3fa4:	80 91 c6 00 	lds	r24, 0x00C6
+    3fb2:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
 #endif
 
   return ch;
 }
-    3fa8:	08 95       	ret
+    3fb6:	08 95       	ret
 
-00003faa <watchdogConfig>:
+00003fb8 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3faa:	e0 e6       	ldi	r30, 0x60	; 96
-    3fac:	f0 e0       	ldi	r31, 0x00	; 0
-    3fae:	98 e1       	ldi	r25, 0x18	; 24
-    3fb0:	90 83       	st	Z, r25
+    3fb8:	e0 e6       	ldi	r30, 0x60	; 96
+    3fba:	f0 e0       	ldi	r31, 0x00	; 0
+    3fbc:	98 e1       	ldi	r25, 0x18	; 24
+    3fbe:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fb2:	80 83       	st	Z, r24
+    3fc0:	80 83       	st	Z, r24
 }
-    3fb4:	08 95       	ret
+    3fc2:	08 95       	ret
 
-00003fb6 <verifySpace>:
+00003fc4 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) {
-    3fb6:	ed df       	rcall	.-38     	; 0x3f92 <getch>
-    3fb8:	80 32       	cpi	r24, 0x20	; 32
-    3fba:	19 f0       	breq	.+6      	; 0x3fc2 <verifySpace+0xc>
+    3fc4:	ed df       	rcall	.-38     	; 0x3fa0 <getch>
+    3fc6:	80 32       	cpi	r24, 0x20	; 32
+    3fc8:	19 f0       	breq	.+6      	; 0x3fd0 <verifySpace+0xc>
     watchdogConfig(WATCHDOG_16MS);    // shorten WD timeout
-    3fbc:	88 e0       	ldi	r24, 0x08	; 8
-    3fbe:	f5 df       	rcall	.-22     	; 0x3faa <watchdogConfig>
-    3fc0:	ff cf       	rjmp	.-2      	; 0x3fc0 <verifySpace+0xa>
+    3fca:	88 e0       	ldi	r24, 0x08	; 8
+    3fcc:	f5 df       	rcall	.-22     	; 0x3fb8 <watchdogConfig>
+    3fce:	ff cf       	rjmp	.-2      	; 0x3fce <verifySpace+0xa>
     while (1)			      // and busy-loop so that WD causes
       ;				      //  a reset and app start.
   }
   putch(STK_INSYNC);
-    3fc2:	84 e1       	ldi	r24, 0x14	; 20
+    3fd0:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fc4:	de cf       	rjmp	.-68     	; 0x3f82 <putch>
+    3fd2:	de cf       	rjmp	.-68     	; 0x3f90 <putch>
 
-00003fc6 <getNch>:
+00003fd4 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fc6:	1f 93       	push	r17
-    3fc8:	18 2f       	mov	r17, r24
+    3fd4:	1f 93       	push	r17
+    3fd6:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3fca:	e3 df       	rcall	.-58     	; 0x3f92 <getch>
-    3fcc:	11 50       	subi	r17, 0x01	; 1
-    3fce:	e9 f7       	brne	.-6      	; 0x3fca <getNch+0x4>
+    3fd8:	e3 df       	rcall	.-58     	; 0x3fa0 <getch>
+    3fda:	11 50       	subi	r17, 0x01	; 1
+    3fdc:	e9 f7       	brne	.-6      	; 0x3fd8 <getNch+0x4>
   verifySpace();
-    3fd0:	f2 df       	rcall	.-28     	; 0x3fb6 <verifySpace>
+    3fde:	f2 df       	rcall	.-28     	; 0x3fc4 <verifySpace>
 }
-    3fd2:	1f 91       	pop	r17
-    3fd4:	08 95       	ret
+    3fe0:	1f 91       	pop	r17
+    3fe2:	08 95       	ret
 
-00003fd6 <appStart>:
+00003fe4 <appStart>:
 
 void appStart(uint8_t rstFlags) {
   // save the reset flags in the designated register
   //  This can be saved in a main program by putting code in .init0 (which
   //  executes before normal c init code) to save R2 to a global variable.
   __asm__ __volatile__ ("mov r2, %0\n" :: "r" (rstFlags));
-    3fd6:	28 2e       	mov	r2, r24
+    3fe4:	28 2e       	mov	r2, r24
 
   watchdogConfig(WATCHDOG_OFF);
-    3fd8:	80 e0       	ldi	r24, 0x00	; 0
-    3fda:	e7 df       	rcall	.-50     	; 0x3faa <watchdogConfig>
+    3fe6:	80 e0       	ldi	r24, 0x00	; 0
+    3fe8:	e7 df       	rcall	.-50     	; 0x3fb8 <watchdogConfig>
   __asm__ __volatile__ (
-    3fdc:	ee 27       	eor	r30, r30
-    3fde:	ff 27       	eor	r31, r31
-    3fe0:	09 94       	ijmp
+    3fea:	ee 27       	eor	r30, r30
+    3fec:	ff 27       	eor	r31, r31
+    3fee:	09 94       	ijmp