This is a relatively significant edit that brings the Arduino copy of

optiboot up-to-date with the optiboot source repository as of Jun-2011
(the last changes made in the optiboot repository were in Oct-2010)

This adds support for several plaforms, fixes the "30k bug", and
refactors the source to have separate stk500.h, boot.h, and pin_defs.h

These are the arduino opticode issues fixed:
http://code.google.com/p/arduino/issues/detail?id=380
    optiboot has problems upload sketches bigger than 30 KB
http://code.google.com/p/arduino/issues/detail?id=556
    update optiboot to the point of the latest optiboot project sources.

These are issues that had been solved in the optiboot source aready:
http://code.google.com/p/arduino/issues/detail?id=364
   optiboot leaves timer1 configured when starting app, breaks PWM on
   pin 9 and 10.  (fixed with a workaround in arduino core.)
   aka http://code.google.com/p/optiboot/source/detail?r=c778fbe72df6ac13ef730c25283358c3c970f73e
   Support for ATmega8 and mega88.
   Fix fuse settings for mega168 _ISP targets
   Additional new platforms (mega, sanguino)

http://code.google.com/p/optiboot/issues/detail?id=26
   Set R1 to 0  (already in arduino code)
http://code.google.com/p/optiboot/issues/detail?id=36&can=1
   Fails to build correctly for mega88

After this commit, the only differences between the Arduino optiboot.c
and the optiboot repository optiboot.c are cosmetic.
(cherry picked from commit e2812ef91cc1489527827dcd14c843f3b3244f36)

23 files changed, 4141 insertions, 2756 deletions

diff --git a/bootloaders/optiboot/Makefile b/bootloaders/optiboot/Makefile
index 0f8d09e..88cb9a2 100644
--- a/bootloaders/optiboot/Makefile
+++ b/bootloaders/optiboot/Makefile
@@ -123,12 +123,23 @@ OBJDUMP        = $(call fixpath,$(GCCROOT)avr-objdump)
 
 SIZE           = $(GCCROOT)avr-size
 
+# Test platforms
+# Virtual boot block test
+virboot328: TARGET = atmega328
+virboot328: MCU_TARGET = atmega328p
+virboot328: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' '-DVIRTUAL_BOOT'
+virboot328: AVR_FREQ = 16000000L
+virboot328: LDSECTION  = --section-start=.text=0x7e00
+virboot328: $(PROGRAM)_atmega328.hex
+virboot328: $(PROGRAM)_atmega328.lst
+
 # 20MHz clocked platforms
 #
 # These are capable of 230400 baud, or 115200 baud on PC (Arduino Avrdude issue)
 #
 
 pro20: TARGET = pro_20mhz
+pro20: MCU_TARGET = atmega168
 pro20: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200'
 pro20: AVR_FREQ = 20000000L
 pro20: $(PROGRAM)_pro_20mhz.hex
@@ -141,7 +152,7 @@ pro20_isp: HFUSE = DD
 # Full swing xtal (20MHz) 258CK/14CK+4.1ms
 pro20_isp: LFUSE = C6
 # 512 byte boot
-pro20_isp: EFUSE = 02
+pro20_isp: EFUSE = 04
 pro20_isp: isp
 
 # 16MHz clocked platforms
@@ -150,6 +161,7 @@ pro20_isp: isp
 #
 
 pro16: TARGET = pro_16MHz
+pro16: MCU_TARGET = atmega168
 pro16: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200'
 pro16: AVR_FREQ = 16000000L
 pro16: $(PROGRAM)_pro_16MHz.hex
@@ -162,12 +174,13 @@ pro16_isp: HFUSE = DD
 # Full swing xtal (20MHz) 258CK/14CK+4.1ms
 pro16_isp: LFUSE = C6
 # 512 byte boot
-pro16_isp: EFUSE = 02
+pro16_isp: EFUSE = 04
 pro16_isp: isp
 
 # Diecimila and NG use identical bootloaders
 #
 diecimila: TARGET = diecimila
+diecimila: MCU_TARGET = atmega168
 diecimila: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200'
 diecimila: AVR_FREQ = 16000000L 
 diecimila: $(PROGRAM)_diecimila.hex
@@ -180,7 +193,7 @@ diecimila_isp: HFUSE = DD
 # Low power xtal (16MHz) 16KCK/14CK+65ms
 diecimila_isp: LFUSE = FF
 # 512 byte boot
-diecimila_isp: EFUSE = 02
+diecimila_isp: EFUSE = 04
 diecimila_isp: isp
 
 atmega328: TARGET = atmega328
@@ -194,7 +207,7 @@ atmega328: $(PROGRAM)_atmega328.lst
 atmega328_isp: atmega328
 atmega328_isp: TARGET = atmega328
 atmega328_isp: MCU_TARGET = atmega328p
-# 512 byte boot
+# 512 byte boot, SPIEN
 atmega328_isp: HFUSE = DE
 # Low power xtal (16MHz) 16KCK/14CK+65ms
 atmega328_isp: LFUSE = FF
@@ -202,13 +215,96 @@ atmega328_isp: LFUSE = FF
 atmega328_isp: EFUSE = 05
 atmega328_isp: isp
 
+# Sanguino has a minimum boot size of 1024 bytes, so enable extra functions
+#
+sanguino: TARGET = atmega644p
+sanguino: MCU_TARGET = atmega644p
+sanguino: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' '-DBIGBOOT'
+sanguino: AVR_FREQ = 16000000L
+sanguino: LDSECTION  = --section-start=.text=0xfc00
+sanguino: $(PROGRAM)_atmega644p.hex
+sanguino: $(PROGRAM)_atmega644p.lst
+
+sanguino_isp: sanguino
+sanguino_isp: TARGET = atmega644p
+sanguino_isp: MCU_TARGET = atmega644p
+# 1024 byte boot
+sanguino_isp: HFUSE = DE
+# Low power xtal (16MHz) 16KCK/14CK+65ms
+sanguino_isp: LFUSE = FF
+# 2.7V brownout
+sanguino_isp: EFUSE = 05
+sanguino_isp: isp
+
+# Mega has a minimum boot size of 1024 bytes, so enable extra functions
+#mega: TARGET = atmega1280
+mega: MCU_TARGET = atmega1280
+mega: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' '-DBIGBOOT'
+mega: AVR_FREQ = 16000000L
+mega: LDSECTION  = --section-start=.text=0x1fc00
+mega: $(PROGRAM)_atmega1280.hex
+mega: $(PROGRAM)_atmega1280.lst
+
+mega_isp: mega
+mega_isp: TARGET = atmega1280
+mega_isp: MCU_TARGET = atmega1280
+# 1024 byte boot
+mega_isp: HFUSE = DE
+# Low power xtal (16MHz) 16KCK/14CK+65ms
+mega_isp: LFUSE = FF
+# 2.7V brownout
+mega_isp: EFUSE = 05
+mega_isp: isp
+
+# ATmega8
+#
+atmega8: TARGET = atmega8
+atmega8: MCU_TARGET = atmega8
+atmega8: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200'
+atmega8: AVR_FREQ = 16000000L 
+atmega8: LDSECTION  = --section-start=.text=0x1e00
+atmega8: $(PROGRAM)_atmega8.hex
+atmega8: $(PROGRAM)_atmega8.lst
+
+atmega8_isp: atmega8
+atmega8_isp: TARGET = atmega8
+atmega8_isp: MCU_TARGET = atmega8
+# SPIEN, CKOPT, Bootsize=512B
+atmega8_isp: HFUSE = CC
+# 2.7V brownout, Low power xtal (16MHz) 16KCK/14CK+65ms
+atmega8_isp: LFUSE = BF
+atmega8_isp: isp
+
+# ATmega88
+#
+atmega88: TARGET = atmega88
+atmega88: MCU_TARGET = atmega88
+atmega88: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200'
+atmega88: AVR_FREQ = 16000000L 
+atmega88: LDSECTION  = --section-start=.text=0x1e00
+atmega88: $(PROGRAM)_atmega88.hex
+atmega88: $(PROGRAM)_atmega88.lst
+
+atmega88_isp: atmega88
+atmega88_isp: TARGET = atmega88
+atmega88_isp: MCU_TARGET = atmega88
+# 2.7V brownout
+atmega88_isp: HFUSE = DD
+# Low power xtal (16MHz) 16KCK/14CK+65ms
+atemga88_isp: LFUSE = FF
+# 512 byte boot
+atmega88_isp: EFUSE = 04
+atmega88_isp: isp
+
+
 # 8MHz clocked platforms
 #
 # These are capable of 115200 baud
 #
 
 lilypad: TARGET = lilypad
-lilypad: CFLAGS += '-DLED_START_FLASHES=3' '-DSOFT_UART' '-DBAUD_RATE=115200'
+lilypad: MCU_TARGET = atmega168
+lilypad: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200'
 lilypad: AVR_FREQ = 8000000L
 lilypad: $(PROGRAM)_lilypad.hex
 lilypad: $(PROGRAM)_lilypad.lst
@@ -220,11 +316,12 @@ lilypad_isp: HFUSE = DD
 # Internal 8MHz osc (8MHz) Slow rising power
 lilypad_isp: LFUSE = E2
 # 512 byte boot
-lilypad_isp: EFUSE = 02
+lilypad_isp: EFUSE = 04
 lilypad_isp: isp
 
 lilypad_resonator: TARGET = lilypad_resonator
-lilypad_resonator: CFLAGS += '-DLED_START_FLASHES=3' '-DSOFT_UART' '-DBAUD_RATE=115200'
+lilypad_resonator: MCU_TARGET = atmega168
+lilypad_resonator: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200'
 lilypad_resonator: AVR_FREQ = 8000000L
 lilypad_resonator: $(PROGRAM)_lilypad_resonator.hex
 lilypad_resonator: $(PROGRAM)_lilypad_resonator.lst
@@ -236,11 +333,12 @@ lilypad_resonator_isp: HFUSE = DD
 # Full swing xtal (20MHz) 258CK/14CK+4.1ms
 lilypad_resonator_isp: LFUSE = C6
 # 512 byte boot
-lilypad_resonator_isp: EFUSE = 02
+lilypad_resonator_isp: EFUSE = 04
 lilypad_resonator_isp: isp
 
 pro8: TARGET = pro_8MHz
-pro8: CFLAGS += '-DLED_START_FLASHES=3' '-DSOFT_UART' '-DBAUD_RATE=115200'
+pro8: MCU_TARGET = atmega168
+pro8: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200'
 pro8: AVR_FREQ = 8000000L
 pro8: $(PROGRAM)_pro_8MHz.hex
 pro8: $(PROGRAM)_pro_8MHz.lst
@@ -252,7 +350,7 @@ pro8_isp: HFUSE = DD
 # Full swing xtal (20MHz) 258CK/14CK+4.1ms
 pro8_isp: LFUSE = C6
 # 512 byte boot
-pro8_isp: EFUSE = 02
+pro8_isp: EFUSE = 04
 pro8_isp: isp
 
 atmega328_pro8: TARGET = atmega328_pro_8MHz
@@ -266,7 +364,7 @@ atmega328_pro8: $(PROGRAM)_atmega328_pro_8MHz.lst
 atmega328_pro8_isp: atmega328_pro8
 atmega328_pro8_isp: TARGET = atmega328_pro_8MHz
 atmega328_pro8_isp: MCU_TARGET = atmega328p
-# 512 byte boot
+# 512 byte boot, SPIEN
 atmega328_pro8_isp: HFUSE = DE
 # Low power xtal (16MHz) 16KCK/14CK+65ms
 atmega328_pro8_isp: LFUSE = FF
@@ -299,6 +397,11 @@ luminet_isp: LFUSE = 62
 luminet_isp: EFUSE = FE
 luminet_isp: isp
 
+#
+# Generic build instructions
+#
+#
+
 isp: $(TARGET)
 	$(ISPFUSES)
 	$(ISPFLASH)
diff --git a/bootloaders/optiboot/boot.h b/bootloaders/optiboot/boot.h
new file mode 100644
index 0000000..2639cd8
--- /dev/null
+++ b/bootloaders/optiboot/boot.h
@@ -0,0 +1,848 @@
+/* Modified to use out for SPM access
+** Peter Knight, Optiboot project http://optiboot.googlecode.com
+**
+** Todo: Tidy up
+**
+** "_short" routines execute 1 cycle faster and use 1 less word of flash
+** by using "out" instruction instead of "sts".
+**
+** Additional elpm variants that trust the value of RAMPZ
+*/
+
+/* Copyright (c) 2002, 2003, 2004, 2005, 2006, 2007  Eric B. Weddington
+   All rights reserved.
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in
+     the documentation and/or other materials provided with the
+     distribution.
+   * Neither the name of the copyright holders nor the names of
+     contributors may be used to endorse or promote products derived
+     from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE. */
+
+/* $Id: boot.h,v 1.27.2.3 2008/09/30 13:58:48 arcanum Exp $ */
+
+#ifndef _AVR_BOOT_H_
+#define _AVR_BOOT_H_    1
+
+/** \file */
+/** \defgroup avr_boot <avr/boot.h>: Bootloader Support Utilities
+    \code
+    #include <avr/io.h>
+    #include <avr/boot.h>
+    \endcode
+
+    The macros in this module provide a C language interface to the
+    bootloader support functionality of certain AVR processors. These
+    macros are designed to work with all sizes of flash memory.
+
+    Global interrupts are not automatically disabled for these macros. It
+    is left up to the programmer to do this. See the code example below. 
+    Also see the processor datasheet for caveats on having global interrupts 
+    enabled during writing of the Flash.
+
+    \note Not all AVR processors provide bootloader support. See your
+    processor datasheet to see if it provides bootloader support.
+
+    \todo From email with Marek: On smaller devices (all except ATmega64/128),
+    __SPM_REG is in the I/O space, accessible with the shorter "in" and "out"
+    instructions - since the boot loader has a limited size, this could be an
+    important optimization.
+
+    \par API Usage Example
+    The following code shows typical usage of the boot API.
+
+    \code
+    #include <inttypes.h>
+    #include <avr/interrupt.h>
+    #include <avr/pgmspace.h>
+    
+    void boot_program_page (uint32_t page, uint8_t *buf)
+    {
+        uint16_t i;
+        uint8_t sreg;
+
+        // Disable interrupts.
+
+        sreg = SREG;
+        cli();
+    
+        eeprom_busy_wait ();
+
+        boot_page_erase (page);
+        boot_spm_busy_wait ();      // Wait until the memory is erased.
+
+        for (i=0; i<SPM_PAGESIZE; i+=2)
+        {
+            // Set up little-endian word.
+
+            uint16_t w = *buf++;
+            w += (*buf++) << 8;
+        
+            boot_page_fill (page + i, w);
+        }
+
+        boot_page_write (page);     // Store buffer in flash page.
+        boot_spm_busy_wait();       // Wait until the memory is written.
+
+        // Reenable RWW-section again. We need this if we want to jump back
+        // to the application after bootloading.
+
+        boot_rww_enable ();
+
+        // Re-enable interrupts (if they were ever enabled).
+
+        SREG = sreg;
+    }\endcode */
+
+#include <avr/eeprom.h>
+#include <avr/io.h>
+#include <inttypes.h>
+#include <limits.h>
+
+/* Check for SPM Control Register in processor. */
+#if defined (SPMCSR)
+#  define __SPM_REG    SPMCSR
+#elif defined (SPMCR)
+#  define __SPM_REG    SPMCR
+#else
+#  error AVR processor does not provide bootloader support!
+#endif
+
+
+/* Check for SPM Enable bit. */
+#if defined(SPMEN)
+#  define __SPM_ENABLE  SPMEN
+#elif defined(SELFPRGEN)
+#  define __SPM_ENABLE  SELFPRGEN
+#else
+#  error Cannot find SPM Enable bit definition!
+#endif
+
+/** \ingroup avr_boot
+    \def BOOTLOADER_SECTION
+
+    Used to declare a function or variable to be placed into a
+    new section called .bootloader. This section and its contents
+    can then be relocated to any address (such as the bootloader
+    NRWW area) at link-time. */
+
+#define BOOTLOADER_SECTION    __attribute__ ((section (".bootloader")))
+
+/* Create common bit definitions. */
+#ifdef ASB
+#define __COMMON_ASB    ASB
+#else
+#define __COMMON_ASB    RWWSB
+#endif
+
+#ifdef ASRE
+#define __COMMON_ASRE   ASRE
+#else
+#define __COMMON_ASRE   RWWSRE
+#endif
+
+/* Define the bit positions of the Boot Lock Bits. */
+
+#define BLB12           5
+#define BLB11           4
+#define BLB02           3
+#define BLB01           2
+
+/** \ingroup avr_boot
+    \def boot_spm_interrupt_enable()
+    Enable the SPM interrupt. */
+
+#define boot_spm_interrupt_enable()   (__SPM_REG |= (uint8_t)_BV(SPMIE))
+
+/** \ingroup avr_boot
+    \def boot_spm_interrupt_disable()
+    Disable the SPM interrupt. */
+
+#define boot_spm_interrupt_disable()  (__SPM_REG &= (uint8_t)~_BV(SPMIE))
+
+/** \ingroup avr_boot
+    \def boot_is_spm_interrupt()
+    Check if the SPM interrupt is enabled. */
+
+#define boot_is_spm_interrupt()       (__SPM_REG & (uint8_t)_BV(SPMIE))
+
+/** \ingroup avr_boot
+    \def boot_rww_busy()
+    Check if the RWW section is busy. */
+
+#define boot_rww_busy()          (__SPM_REG & (uint8_t)_BV(__COMMON_ASB))
+
+/** \ingroup avr_boot
+    \def boot_spm_busy()
+    Check if the SPM instruction is busy. */
+
+#define boot_spm_busy()               (__SPM_REG & (uint8_t)_BV(__SPM_ENABLE))
+
+/** \ingroup avr_boot
+    \def boot_spm_busy_wait()
+    Wait while the SPM instruction is busy. */
+
+#define boot_spm_busy_wait()          do{}while(boot_spm_busy())
+
+#define __BOOT_PAGE_ERASE         (_BV(__SPM_ENABLE) | _BV(PGERS))
+#define __BOOT_PAGE_WRITE         (_BV(__SPM_ENABLE) | _BV(PGWRT))
+#define __BOOT_PAGE_FILL          _BV(__SPM_ENABLE)
+#define __BOOT_RWW_ENABLE         (_BV(__SPM_ENABLE) | _BV(__COMMON_ASRE))
+#define __BOOT_LOCK_BITS_SET      (_BV(__SPM_ENABLE) | _BV(BLBSET))
+
+#define __boot_page_fill_short(address, data)   \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "movw  r0, %3\n\t"                       \
+        "out %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        "clr  r1\n\t"                            \
+        :                                        \
+        : "i" (_SFR_IO_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_PAGE_FILL),       \
+          "z" ((uint16_t)address),               \
+          "r" ((uint16_t)data)                   \
+        : "r0"                                   \
+    );                                           \
+}))
+
+#define __boot_page_fill_normal(address, data)   \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "movw  r0, %3\n\t"                       \
+        "sts %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        "clr  r1\n\t"                            \
+        :                                        \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_PAGE_FILL),       \
+          "z" ((uint16_t)address),               \
+          "r" ((uint16_t)data)                   \
+        : "r0"                                   \
+    );                                           \
+}))
+
+#define __boot_page_fill_alternate(address, data)\
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "movw  r0, %3\n\t"                       \
+        "sts %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        ".word 0xffff\n\t"                       \
+        "nop\n\t"                                \
+        "clr  r1\n\t"                            \
+        :                                        \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_PAGE_FILL),       \
+          "z" ((uint16_t)address),               \
+          "r" ((uint16_t)data)                   \
+        : "r0"                                   \
+    );                                           \
+}))
+
+#define __boot_page_fill_extended(address, data) \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "movw  r0, %4\n\t"                       \
+        "movw r30, %A3\n\t"                      \
+        "sts %1, %C3\n\t"                        \
+        "sts %0, %2\n\t"                         \
+        "spm\n\t"                                \
+        "clr  r1\n\t"                            \
+        :                                        \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),        \
+          "i" (_SFR_MEM_ADDR(RAMPZ)),            \
+          "r" ((uint8_t)__BOOT_PAGE_FILL),       \
+          "r" ((uint32_t)address),               \
+          "r" ((uint16_t)data)                   \
+        : "r0", "r30", "r31"                     \
+    );                                           \
+}))
+
+#define __boot_page_fill_extended_short(address, data) \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "movw  r0, %4\n\t"                       \
+        "movw r30, %A3\n\t"                      \
+        "out %1, %C3\n\t"                        \
+        "out %0, %2\n\t"                         \
+        "spm\n\t"                                \
+        "clr  r1\n\t"                            \
+        :                                        \
+        : "i" (_SFR_IO_ADDR(__SPM_REG)),        \
+          "i" (_SFR_IO_ADDR(RAMPZ)),            \
+          "r" ((uint8_t)__BOOT_PAGE_FILL),       \
+          "r" ((uint32_t)address),               \
+          "r" ((uint16_t)data)                   \
+        : "r0", "r30", "r31"                     \
+    );                                           \
+}))
+
+#define __boot_page_erase_short(address)        \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "out %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        :                                        \
+        : "i" (_SFR_IO_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_PAGE_ERASE),      \
+          "z" ((uint16_t)address)                \
+    );                                           \
+}))
+
+
+#define __boot_page_erase_normal(address)        \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "sts %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        :                                        \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_PAGE_ERASE),      \
+          "z" ((uint16_t)address)                \
+    );                                           \
+}))
+
+#define __boot_page_erase_alternate(address)     \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "sts %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        ".word 0xffff\n\t"                       \
+        "nop\n\t"                                \
+        :                                        \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_PAGE_ERASE),      \
+          "z" ((uint16_t)address)                \
+    );                                           \
+}))
+
+#define __boot_page_erase_extended(address)      \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "movw r30, %A3\n\t"                      \
+        "sts  %1, %C3\n\t"                       \
+        "sts %0, %2\n\t"                         \
+        "spm\n\t"                                \
+        :                                        \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),        \
+          "i" (_SFR_MEM_ADDR(RAMPZ)),            \
+          "r" ((uint8_t)__BOOT_PAGE_ERASE),      \
+          "r" ((uint32_t)address)                \
+        : "r30", "r31"                           \
+    );                                           \
+}))
+#define __boot_page_erase_extended_short(address)      \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "movw r30, %A3\n\t"                      \
+        "out  %1, %C3\n\t"                       \
+        "out %0, %2\n\t"                         \
+        "spm\n\t"                                \
+        :                                        \
+        : "i" (_SFR_IO_ADDR(__SPM_REG)),        \
+          "i" (_SFR_IO_ADDR(RAMPZ)),            \
+          "r" ((uint8_t)__BOOT_PAGE_ERASE),      \
+          "r" ((uint32_t)address)                \
+        : "r30", "r31"                           \
+    );                                           \
+}))
+
+#define __boot_page_write_short(address)        \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "out %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        :                                        \
+        : "i" (_SFR_IO_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_PAGE_WRITE),      \
+          "z" ((uint16_t)address)                \
+    );                                           \
+}))
+
+#define __boot_page_write_normal(address)        \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "sts %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        :                                        \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_PAGE_WRITE),      \
+          "z" ((uint16_t)address)                \
+    );                                           \
+}))
+
+#define __boot_page_write_alternate(address)     \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "sts %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        ".word 0xffff\n\t"                       \
+        "nop\n\t"                                \
+        :                                        \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_PAGE_WRITE),      \
+          "z" ((uint16_t)address)                \
+    );                                           \
+}))
+
+#define __boot_page_write_extended(address)      \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "movw r30, %A3\n\t"                      \
+        "sts %1, %C3\n\t"                        \
+        "sts %0, %2\n\t"                         \
+        "spm\n\t"                                \
+        :                                        \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),        \
+          "i" (_SFR_MEM_ADDR(RAMPZ)),            \
+          "r" ((uint8_t)__BOOT_PAGE_WRITE),      \
+          "r" ((uint32_t)address)                \
+        : "r30", "r31"                           \
+    );                                           \
+}))
+#define __boot_page_write_extended_short(address)      \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "movw r30, %A3\n\t"                      \
+        "out %1, %C3\n\t"                        \
+        "out %0, %2\n\t"                         \
+        "spm\n\t"                                \
+        :                                        \
+        : "i" (_SFR_IO_ADDR(__SPM_REG)),        \
+          "i" (_SFR_IO_ADDR(RAMPZ)),            \
+          "r" ((uint8_t)__BOOT_PAGE_WRITE),      \
+          "r" ((uint32_t)address)                \
+        : "r30", "r31"                           \
+    );                                           \
+}))
+
+#define __boot_rww_enable_short()                      \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "out %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        :                                        \
+        : "i" (_SFR_IO_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_RWW_ENABLE)       \
+    );                                           \
+}))
+
+#define __boot_rww_enable()                      \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "sts %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        :                                        \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_RWW_ENABLE)       \
+    );                                           \
+}))
+
+#define __boot_rww_enable_alternate()            \
+(__extension__({                                 \
+    __asm__ __volatile__                         \
+    (                                            \
+        "sts %0, %1\n\t"                         \
+        "spm\n\t"                                \
+        ".word 0xffff\n\t"                       \
+        "nop\n\t"                                \
+        :                                        \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t)__BOOT_RWW_ENABLE)       \
+    );                                           \
+}))
+
+/* From the mega16/mega128 data sheets (maybe others):
+
+     Bits by SPM To set the Boot Loader Lock bits, write the desired data to
+     R0, write "X0001001" to SPMCR and execute SPM within four clock cycles
+     after writing SPMCR. The only accessible Lock bits are the Boot Lock bits
+     that may prevent the Application and Boot Loader section from any
+     software update by the MCU.
+
+     If bits 5..2 in R0 are cleared (zero), the corresponding Boot Lock bit
+     will be programmed if an SPM instruction is executed within four cycles
+     after BLBSET and SPMEN (or SELFPRGEN) are set in SPMCR. The Z-pointer is 
+     don't care during this operation, but for future compatibility it is 
+     recommended to load the Z-pointer with $0001 (same as used for reading the 
+     Lock bits). For future compatibility It is also recommended to set bits 7, 
+     6, 1, and 0 in R0 to 1 when writing the Lock bits. When programming the 
+     Lock bits the entire Flash can be read during the operation. */
+
+#define __boot_lock_bits_set_short(lock_bits)                    \
+(__extension__({                                           \
+    uint8_t value = (uint8_t)(~(lock_bits));               \
+    __asm__ __volatile__                                   \
+    (                                                      \
+        "ldi r30, 1\n\t"                                   \
+        "ldi r31, 0\n\t"                                   \
+        "mov r0, %2\n\t"                                   \
+        "out %0, %1\n\t"                                   \
+        "spm\n\t"                                          \
+        :                                                  \
+        : "i" (_SFR_IO_ADDR(__SPM_REG)),                  \
+          "r" ((uint8_t)__BOOT_LOCK_BITS_SET),             \
+          "r" (value)                                      \
+        : "r0", "r30", "r31"                               \
+    );                                                     \
+}))
+
+#define __boot_lock_bits_set(lock_bits)                    \
+(__extension__({                                           \
+    uint8_t value = (uint8_t)(~(lock_bits));               \
+    __asm__ __volatile__                                   \
+    (                                                      \
+        "ldi r30, 1\n\t"                                   \
+        "ldi r31, 0\n\t"                                   \
+        "mov r0, %2\n\t"                                   \
+        "sts %0, %1\n\t"                                   \
+        "spm\n\t"                                          \
+        :                                                  \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),                  \
+          "r" ((uint8_t)__BOOT_LOCK_BITS_SET),             \
+          "r" (value)                                      \
+        : "r0", "r30", "r31"                               \
+    );                                                     \
+}))
+
+#define __boot_lock_bits_set_alternate(lock_bits)          \
+(__extension__({                                           \
+    uint8_t value = (uint8_t)(~(lock_bits));               \
+    __asm__ __volatile__                                   \
+    (                                                      \
+        "ldi r30, 1\n\t"                                   \
+        "ldi r31, 0\n\t"                                   \
+        "mov r0, %2\n\t"                                   \
+        "sts %0, %1\n\t"                                   \
+        "spm\n\t"                                          \
+        ".word 0xffff\n\t"                                 \
+        "nop\n\t"                                          \
+        :                                                  \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),                  \
+          "r" ((uint8_t)__BOOT_LOCK_BITS_SET),             \
+          "r" (value)                                      \
+        : "r0", "r30", "r31"                               \
+    );                                                     \
+}))
+
+/*
+   Reading lock and fuse bits:
+
+     Similarly to writing the lock bits above, set BLBSET and SPMEN (or 
+     SELFPRGEN) bits in __SPMREG, and then (within four clock cycles) issue an 
+     LPM instruction.
+
+     Z address:       contents:
+     0x0000           low fuse bits
+     0x0001           lock bits
+     0x0002           extended fuse bits
+     0x0003           high fuse bits
+
+     Sounds confusing, doesn't it?
+
+     Unlike the macros in pgmspace.h, no need to care for non-enhanced
+     cores here as these old cores do not provide SPM support anyway.
+ */
+
+/** \ingroup avr_boot
+    \def GET_LOW_FUSE_BITS
+    address to read the low fuse bits, using boot_lock_fuse_bits_get
+ */
+#define GET_LOW_FUSE_BITS           (0x0000)
+/** \ingroup avr_boot
+    \def GET_LOCK_BITS
+    address to read the lock bits, using boot_lock_fuse_bits_get
+ */
+#define GET_LOCK_BITS               (0x0001)
+/** \ingroup avr_boot
+    \def GET_EXTENDED_FUSE_BITS
+    address to read the extended fuse bits, using boot_lock_fuse_bits_get
+ */
+#define GET_EXTENDED_FUSE_BITS      (0x0002)
+/** \ingroup avr_boot
+    \def GET_HIGH_FUSE_BITS
+    address to read the high fuse bits, using boot_lock_fuse_bits_get
+ */
+#define GET_HIGH_FUSE_BITS          (0x0003)
+
+/** \ingroup avr_boot
+    \def boot_lock_fuse_bits_get(address)
+
+    Read the lock or fuse bits at \c address.
+
+    Parameter \c address can be any of GET_LOW_FUSE_BITS,
+    GET_LOCK_BITS, GET_EXTENDED_FUSE_BITS, or GET_HIGH_FUSE_BITS.
+
+    \note The lock and fuse bits returned are the physical values,
+    i.e. a bit returned as 0 means the corresponding fuse or lock bit
+    is programmed.
+ */
+#define boot_lock_fuse_bits_get_short(address)                   \
+(__extension__({                                           \
+    uint8_t __result;                                      \
+    __asm__ __volatile__                                   \
+    (                                                      \
+        "ldi r30, %3\n\t"                                  \
+        "ldi r31, 0\n\t"                                   \
+        "out %1, %2\n\t"                                   \
+        "lpm %0, Z\n\t"                                    \
+        : "=r" (__result)                                  \
+        : "i" (_SFR_IO_ADDR(__SPM_REG)),                  \
+          "r" ((uint8_t)__BOOT_LOCK_BITS_SET),             \
+          "M" (address)                                    \
+        : "r0", "r30", "r31"                               \
+    );                                                     \
+    __result;                                              \
+}))
+
+#define boot_lock_fuse_bits_get(address)                   \
+(__extension__({                                           \
+    uint8_t __result;                                      \
+    __asm__ __volatile__                                   \
+    (                                                      \
+        "ldi r30, %3\n\t"                                  \
+        "ldi r31, 0\n\t"                                   \
+        "sts %1, %2\n\t"                                   \
+        "lpm %0, Z\n\t"                                    \
+        : "=r" (__result)                                  \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),                  \
+          "r" ((uint8_t)__BOOT_LOCK_BITS_SET),             \
+          "M" (address)                                    \
+        : "r0", "r30", "r31"                               \
+    );                                                     \
+    __result;                                              \
+}))
+
+/** \ingroup avr_boot
+    \def boot_signature_byte_get(address)
+
+    Read the Signature Row byte at \c address.  For some MCU types,
+    this function can also retrieve the factory-stored oscillator
+    calibration bytes.
+
+    Parameter \c address can be 0-0x1f as documented by the datasheet.
+    \note The values are MCU type dependent.
+*/
+
+#define __BOOT_SIGROW_READ (_BV(__SPM_ENABLE) | _BV(SIGRD))
+
+#define boot_signature_byte_get_short(addr) \
+(__extension__({                      \
+      uint16_t __addr16 = (uint16_t)(addr);     \
+      uint8_t __result;                         \
+      __asm__ __volatile__                      \
+      (                                         \
+        "out %1, %2\n\t"                        \
+        "lpm %0, Z" "\n\t"                      \
+        : "=r" (__result)                       \
+        : "i" (_SFR_IO_ADDR(__SPM_REG)),        \
+          "r" ((uint8_t) __BOOT_SIGROW_READ),   \
+          "z" (__addr16)                        \
+      );                                        \
+      __result;                                 \
+}))
+
+#define boot_signature_byte_get(addr) \
+(__extension__({                      \
+      uint16_t __addr16 = (uint16_t)(addr);     \
+      uint8_t __result;                         \
+      __asm__ __volatile__                      \
+      (                                         \
+        "sts %1, %2\n\t"                        \
+        "lpm %0, Z" "\n\t"                      \
+        : "=r" (__result)                       \
+        : "i" (_SFR_MEM_ADDR(__SPM_REG)),       \
+          "r" ((uint8_t) __BOOT_SIGROW_READ),   \
+          "z" (__addr16)                        \
+      );                                        \
+      __result;                                 \
+}))
+
+/** \ingroup avr_boot
+    \def boot_page_fill(address, data)
+
+    Fill the bootloader temporary page buffer for flash 
+    address with data word. 
+
+    \note The address is a byte address. The data is a word. The AVR 
+    writes data to the buffer a word at a time, but addresses the buffer
+    per byte! So, increment your address by 2 between calls, and send 2
+    data bytes in a word format! The LSB of the data is written to the lower 
+    address; the MSB of the data is written to the higher address.*/
+
+/** \ingroup avr_boot
+    \def boot_page_erase(address)
+
+    Erase the flash page that contains address.
+
+    \note address is a byte address in flash, not a word address. */
+
+/** \ingroup avr_boot
+    \def boot_page_write(address)
+
+    Write the bootloader temporary page buffer 
+    to flash page that contains address.
+    
+    \note address is a byte address in flash, not a word address. */
+
+/** \ingroup avr_boot
+    \def boot_rww_enable()
+
+    Enable the Read-While-Write memory section. */
+
+/** \ingroup avr_boot
+    \def boot_lock_bits_set(lock_bits)
+
+    Set the bootloader lock bits.
+
+    \param lock_bits A mask of which Boot Loader Lock Bits to set.
+
+    \note In this context, a 'set bit' will be written to a zero value.
+    Note also that only BLBxx bits can be programmed by this command.
+
+    For example, to disallow the SPM instruction from writing to the Boot
+    Loader memory section of flash, you would use this macro as such:
+
+    \code
+    boot_lock_bits_set (_BV (BLB11));
+    \endcode
+
+    \note Like any lock bits, the Boot Loader Lock Bits, once set,
+    cannot be cleared again except by a chip erase which will in turn
+    also erase the boot loader itself. */
+
+/* Normal versions of the macros use 16-bit addresses.
+   Extended versions of the macros use 32-bit addresses.
+   Alternate versions of the macros use 16-bit addresses and require special
+   instruction sequences after LPM.
+
+   FLASHEND is defined in the ioXXXX.h file.
+   USHRT_MAX is defined in <limits.h>. */ 
+
+#if defined(__AVR_ATmega161__) || defined(__AVR_ATmega163__) \
+    || defined(__AVR_ATmega323__)
+
+/* Alternate: ATmega161/163/323 and 16 bit address */
+#define boot_page_fill(address, data) __boot_page_fill_alternate(address, data)
+#define boot_page_erase(address)      __boot_page_erase_alternate(address)
+#define boot_page_write(address)      __boot_page_write_alternate(address)
+#define boot_rww_enable()             __boot_rww_enable_alternate()
+#define boot_lock_bits_set(lock_bits) __boot_lock_bits_set_alternate(lock_bits)
+
+#elif (FLASHEND > USHRT_MAX)
+
+/* Extended: >16 bit address */
+#define boot_page_fill(address, data) __boot_page_fill_extended_short(address, data)
+#define boot_page_erase(address)      __boot_page_erase_extended_short(address)
+#define boot_page_write(address)      __boot_page_write_extended_short(address)
+#define boot_rww_enable()             __boot_rww_enable_short()
+#define boot_lock_bits_set(lock_bits) __boot_lock_bits_set_short(lock_bits)
+
+#else
+
+/* Normal: 16 bit address */
+#define boot_page_fill(address, data) __boot_page_fill_short(address, data)
+#define boot_page_erase(address)      __boot_page_erase_short(address)
+#define boot_page_write(address)      __boot_page_write_short(address)
+#define boot_rww_enable()             __boot_rww_enable_short()
+#define boot_lock_bits_set(lock_bits) __boot_lock_bits_set_short(lock_bits)
+
+#endif
+
+/** \ingroup avr_boot
+
+    Same as boot_page_fill() except it waits for eeprom and spm operations to
+    complete before filling the page. */
+
+#define boot_page_fill_safe(address, data) \
+do { \
+    boot_spm_busy_wait();                       \
+    eeprom_busy_wait();                         \
+    boot_page_fill(address, data);              \
+} while (0)
+
+/** \ingroup avr_boot
+
+    Same as boot_page_erase() except it waits for eeprom and spm operations to
+    complete before erasing the page. */
+
+#define boot_page_erase_safe(address) \
+do { \
+    boot_spm_busy_wait();                       \
+    eeprom_busy_wait();                         \
+    boot_page_erase (address);                  \
+} while (0)
+
+/** \ingroup avr_boot
+
+    Same as boot_page_write() except it waits for eeprom and spm operations to
+    complete before writing the page. */
+
+#define boot_page_write_safe(address) \
+do { \
+    boot_spm_busy_wait();                       \
+    eeprom_busy_wait();                         \
+    boot_page_write (address);                  \
+} while (0)
+
+/** \ingroup avr_boot
+
+    Same as boot_rww_enable() except waits for eeprom and spm operations to
+    complete before enabling the RWW mameory. */
+
+#define boot_rww_enable_safe() \
+do { \
+    boot_spm_busy_wait();                       \
+    eeprom_busy_wait();                         \
+    boot_rww_enable();                          \
+} while (0)
+
+/** \ingroup avr_boot
+
+    Same as boot_lock_bits_set() except waits for eeprom and spm operations to
+    complete before setting the lock bits. */
+
+#define boot_lock_bits_set_safe(lock_bits) \
+do { \
+    boot_spm_busy_wait();                       \
+    eeprom_busy_wait();                         \
+    boot_lock_bits_set (lock_bits);             \
+} while (0)
+
+#endif /* _AVR_BOOT_H_ */
diff --git a/bootloaders/optiboot/optiboot.c b/bootloaders/optiboot/optiboot.c
index c7d817a..4b4a10a 100644
--- a/bootloaders/optiboot/optiboot.c
+++ b/bootloaders/optiboot/optiboot.c
@@ -1,6 +1,11 @@
 /**********************************************************/
 /* Optiboot bootloader for Arduino                        */
 /*                                                        */
+/* http://optiboot.googlecode.com                         */
+/*                                                        */
+/* Arduino-maintained version : See README.TXT            */
+/* http://code.google.com/p/arduino/                      */
+/*                                                        */
 /* Heavily optimised bootloader that is faster and        */
 /* smaller than the Arduino standard bootloader           */
 /*                                                        */
@@ -10,6 +15,8 @@
 /*   Higher baud rate speeds up programming               */
 /*   Written almost entirely in C                         */
 /*   Customisable timeout with accurate timeconstant      */
+/*   Optional virtual UART. No hardware UART required.    */
+/*   Optional virtual boot partition for devices without. */
 /*                                                        */
 /* What you lose:                                         */
 /*   Implements a skeleton STK500 protocol which is       */
@@ -18,12 +25,19 @@
 /*   High baud rate breaks compatibility with standard    */
 /*     Arduino flash settings                             */
 /*                                                        */
-/* Currently supports:                                    */
-/*   ATmega168 based devices (Diecimila etc)              */
+/* Fully supported:                                       */
+/*   ATmega168 based devices  (Diecimila etc)             */
 /*   ATmega328P based devices (Duemilanove etc)           */
 /*                                                        */
+/* Alpha test                                             */
+/*   ATmega1280 based devices (Arduino Mega)              */
+/*                                                        */
+/* Work in progress:                                      */
+/*   ATmega644P based devices (Sanguino)                  */
+/*   ATtiny84 based devices (Luminet)                     */
+/*                                                        */
 /* Does not support:                                      */
-/*   ATmega1280 based devices (eg. Mega)                  */
+/*   USB based devices (eg. Teensy)                       */
 /*                                                        */
 /* Assumptions:                                           */
 /*   The code makes several assumptions that reduce the   */
@@ -64,102 +78,85 @@
 /*                                                        */
 /**********************************************************/
 
+
+/**********************************************************/
+/*                                                        */
+/* Optional defines:                                      */
+/*                                                        */
+/**********************************************************/
+/*                                                        */
+/* BIG_BOOT:                                              */
+/* Build a 1k bootloader, not 512 bytes. This turns on    */
+/* extra functionality.                                   */
+/*                                                        */
+/* BAUD_RATE:                                             */
+/* Set bootloader baud rate.                              */
+/*                                                        */
+/* LUDICROUS_SPEED:                                       */
+/* 230400 baud :-)                                        */
+/*                                                        */
+/* SOFT_UART:                                             */
+/* Use AVR305 soft-UART instead of hardware UART.         */
+/*                                                        */
+/* LED_START_FLASHES:                                     */
+/* Number of LED flashes on bootup.                       */
+/*                                                        */
+/* LED_DATA_FLASH:                                        */
+/* Flash LED when transferring data. For boards without   */
+/* TX or RX LEDs, or for people who like blinky lights.   */
+/*                                                        */
+/* SUPPORT_EEPROM:                                        */
+/* Support reading and writing from EEPROM. This is not   */
+/* used by Arduino, so off by default.                    */
+/*                                                        */
+/* TIMEOUT_MS:                                            */
+/* Bootloader timeout period, in milliseconds.            */
+/* 500,1000,2000,4000,8000 supported.                     */
+/*                                                        */
+/**********************************************************/
+
 #include <inttypes.h>
 #include <avr/io.h>
 #include <avr/pgmspace.h>
-#include <avr/boot.h>
 
-//#define LED_DATA_FLASH
+// <avr/boot.h> uses sts instructions, but this version uses out instructions
+// This saves cycles and program memory.
+#include "boot.h"
+
+
+// We don't use <avr/wdt.h> as those routines have interrupt overhead we don't need.
+
+#include "pin_defs.h"
+#include "stk500.h"
 
 #ifndef LED_START_FLASHES
 #define LED_START_FLASHES 0
 #endif
 
-/* Build-time variables */
-/* BAUD_RATE       Programming baud rate */
-/* LED_NO_FLASHES  Number of LED flashes on boot */
-/* FLASH_TIME_MS   Duration of each LED flash */
-/* BOOT_TIMEOUT_MS Serial port wait time before exiting bootloader */
+#ifdef LUDICROUS_SPEED
+#define BAUD_RATE 230400L
+#endif
 
-/* set the UART baud rate */
+/* set the UART baud rate defaults */
 #ifndef BAUD_RATE
-#define BAUD_RATE   19200
+#if F_CPU >= 8000000L
+#define BAUD_RATE   115200L // Highest rate Avrdude win32 will support
+#elsif F_CPU >= 1000000L
+#define BAUD_RATE   9600L   // 19200 also supported, but with significant error
+#elsif F_CPU >= 128000L
+#define BAUD_RATE   4800L   // Good for 128kHz internal RC
+#else
+#define BAUD_RATE 1200L     // Good even at 32768Hz
+#endif
 #endif
 
-#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__)
-/* Onboard LED is connected to pin PB5 in Arduino NG, Diecimila, and Duemilanove */ 
-#define LED_DDR     DDRB
-#define LED_PORT    PORTB
-#define LED_PIN     PINB
-#define LED         PINB5
-
-/* Ports for soft UART */
-#ifdef SOFT_UART
-#define UART_PORT   PORTD
-#define UART_PIN    PIND
-#define UART_DDR    DDRD
-#define UART_TX_BIT 1
-#define UART_RX_BIT 0
+/* Switch in soft UART for hard baud rates */
+#if (F_CPU/BAUD_RATE) > 280 // > 57600 for 16MHz
+#ifndef SOFT_UART
+#define SOFT_UART
 #endif
 #endif
 
-#if defined(__AVR_ATtiny84__)
-/* Onboard LED is connected to pin PB5 in Arduino NG, Diecimila, and Duemilanove */ 
-#define LED_DDR     DDRA
-#define LED_PORT    PORTA
-#define LED_PIN     PINA
-#define LED         PINA4
-
-/* Ports for soft UART - left port only for now*/
-#ifdef SOFT_UART
-#define UART_PORT   PORTA
-#define UART_PIN    PINA
-#define UART_DDR    DDRA
-#define UART_TX_BIT 2
-#define UART_RX_BIT 3
-#endif
-#endif
-
-/* STK500 constants list, from AVRDUDE */
-#define STK_OK              0x10
-#define STK_FAILED          0x11  // Not used
-#define STK_UNKNOWN         0x12  // Not used
-#define STK_NODEVICE        0x13  // Not used
-#define STK_INSYNC          0x14  // ' '
-#define STK_NOSYNC          0x15  // Not used
-#define ADC_CHANNEL_ERROR   0x16  // Not used
-#define ADC_MEASURE_OK      0x17  // Not used
-#define PWM_CHANNEL_ERROR   0x18  // Not used
-#define PWM_ADJUST_OK       0x19  // Not used
-#define CRC_EOP             0x20  // 'SPACE'
-#define STK_GET_SYNC        0x30  // '0'
-#define STK_GET_SIGN_ON     0x31  // '1'
-#define STK_SET_PARAMETER   0x40  // '@'
-#define STK_GET_PARAMETER   0x41  // 'A'
-#define STK_SET_DEVICE      0x42  // 'B'
-#define STK_SET_DEVICE_EXT  0x45  // 'E'
-#define STK_ENTER_PROGMODE  0x50  // 'P'
-#define STK_LEAVE_PROGMODE  0x51  // 'Q'
-#define STK_CHIP_ERASE      0x52  // 'R'
-#define STK_CHECK_AUTOINC   0x53  // 'S'
-#define STK_LOAD_ADDRESS    0x55  // 'U'
-#define STK_UNIVERSAL       0x56  // 'V'
-#define STK_PROG_FLASH      0x60  // '`'
-#define STK_PROG_DATA       0x61  // 'a'
-#define STK_PROG_FUSE       0x62  // 'b'
-#define STK_PROG_LOCK       0x63  // 'c'
-#define STK_PROG_PAGE       0x64  // 'd'
-#define STK_PROG_FUSE_EXT   0x65  // 'e'
-#define STK_READ_FLASH      0x70  // 'p'
-#define STK_READ_DATA       0x71  // 'q'
-#define STK_READ_FUSE       0x72  // 'r'
-#define STK_READ_LOCK       0x73  // 's'
-#define STK_READ_PAGE       0x74  // 't'
-#define STK_READ_SIGN       0x75  // 'u'
-#define STK_READ_OSCCAL     0x76  // 'v'
-#define STK_READ_FUSE_EXT   0x77  // 'w'
-#define STK_READ_OSCCAL_EXT 0x78  // 'x'
-
 /* Watchdog settings */
 #define WATCHDOG_OFF    (0)
 #define WATCHDOG_16MS   (_BV(WDE))
@@ -170,8 +167,10 @@
 #define WATCHDOG_500MS  (_BV(WDP2) | _BV(WDP0) | _BV(WDE))
 #define WATCHDOG_1S     (_BV(WDP2) | _BV(WDP1) | _BV(WDE))
 #define WATCHDOG_2S     (_BV(WDP2) | _BV(WDP1) | _BV(WDP0) | _BV(WDE))
+#ifndef __AVR_ATmega8__
 #define WATCHDOG_4S     (_BV(WDE3) | _BV(WDE))
 #define WATCHDOG_8S     (_BV(WDE3) | _BV(WDE0) | _BV(WDE))
+#endif
 
 /* Function Prototypes */
 /* The main function is in init9, which removes the interrupt vector table */
@@ -191,18 +190,41 @@ void uartDelay() __attribute__ ((naked));
 #endif
 void appStart() __attribute__ ((naked));
 
+#if defined(__AVR_ATmega168__)
+#define RAMSTART (0x100)
+#define NRWWSTART (0x3800)
+#elif defined(__AVR_ATmega328P__)
+#define RAMSTART (0x100)
+#define NRWWSTART (0x7000)
+#elif defined (__AVR_ATmega644P__)
+#define RAMSTART (0x100)
+#define NRWWSTART (0xE000)
+#elif defined(__AVR_ATtiny84__)
+#define RAMSTART (0x100)
+#define NRWWSTART (0x0000)
+#elif defined(__AVR_ATmega1280__)
+#define RAMSTART (0x200)
+#define NRWWSTART (0xE000)
+#elif defined(__AVR_ATmega8__) || defined(__AVR_ATmega88__)
+#define RAMSTART (0x100)
+#define NRWWSTART (0x1800)
+#endif
+
 /* C zero initialises all global variables. However, that requires */
 /* These definitions are NOT zero initialised, but that doesn't matter */
 /* This allows us to drop the zero init code, saving us memory */
-#define buff    ((uint8_t*)(0x100))
-#define address (*(uint16_t*)(0x200))
-#define length  (*(uint8_t*)(0x202))
+#define buff    ((uint8_t*)(RAMSTART))
+#define address (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2))
+#define length  (*(uint8_t*)(RAMSTART+SPM_PAGESIZE*2+2))
 #ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
+  uint8_t ch;
+
   // After the zero init loop, this is the first code to run.
   //
   // This code makes the following assumptions:
@@ -212,29 +234,36 @@ int main(void) {
   //
   // If not, uncomment the following instructions:
   // cli();
-  // SP=RAMEND;  // This is done by hardware reset
   asm volatile ("clr __zero_reg__");
+#ifdef __AVR_ATmega8__
+  SP=RAMEND;  // This is done by hardware reset
+#endif
 
-  uint8_t ch;
+  // Adaboot no-wait mod
+  ch = MCUSR;
+  MCUSR = 0;
+  if (!(ch & _BV(EXTRF))) appStart();
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
 #endif
 #ifndef SOFT_UART
+#ifdef __AVR_ATmega8__
+  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
   UCSR0A = _BV(U2X0); //Double speed mode USART0
   UCSR0B = _BV(RXEN0) | _BV(TXEN0);
   UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
   UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
 #endif
-
-  // Adaboot no-wait mod
-  ch = MCUSR;
-  MCUSR = 0;
-  if (!(ch & _BV(EXTRF))) appStart();
+#endif
 
   // Set up watchdog to trigger after 500ms
-  watchdogConfig(WATCHDOG_500MS);
+  watchdogConfig(WATCHDOG_1S);
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
@@ -269,9 +298,15 @@ int main(void) {
     }
     else if(ch == STK_LOAD_ADDRESS) {
       // LOAD ADDRESS
-      address = getch();
-      address = (address & 0xff) | (getch() << 8);
-      address += address; // Convert from word address to byte address
+      uint16_t newAddress;
+      newAddress = getch();
+      newAddress = (newAddress & 0xff) | (getch() << 8);
+#ifdef RAMPZ
+      // Transfer top bit to RAMPZ
+      RAMPZ = (newAddress & 0x8000) ? 1 : 0;
+#endif
+      newAddress += newAddress; // Convert from word address to byte address
+      address = newAddress;
       verifySpace();
     }
     else if(ch == STK_UNIVERSAL) {
@@ -279,7 +314,7 @@ int main(void) {
       getNch(4);
       putch(0x00);
     }
-    /* Write memory, length is big endian and is in bytes  */
+    /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
@@ -287,14 +322,18 @@ int main(void) {
 
       getLen();
 
-      // Immediately start page erase - this will 4.5ms
-      boot_page_erase((uint16_t)(void*)address);
-
+      // If we are in RWW section, immediately start page erase
+      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+      
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
       while (--length);
 
+      // If we are in NRWW section, page erase has to be delayed until now.
+      // Todo: Take RAMPZ into account
+      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+
       // Read command terminator, start reply
       verifySpace();
       
@@ -310,10 +349,10 @@ int main(void) {
         // Move RESET vector to WDT vector
         uint16_t vect = buff[0] | (buff[1]<<8);
         rstVect = vect;
-        wdtVect = buff[10] | (buff[11]<<8);
+        wdtVect = buff[8] | (buff[9]<<8);
         vect -= 4; // Instruction is a relative jump (rjmp), so recalculate.
-        buff[10] = vect & 0xff;
-        buff[11] = vect >> 8;
+        buff[8] = vect & 0xff;
+        buff[9] = vect >> 8;
 
         // Add jump to bootloader at RESET vector
         buff[0] = 0x7f;
@@ -329,12 +368,12 @@ int main(void) {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
+      __boot_page_write_short((uint16_t)(void*)address);
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
@@ -353,16 +392,28 @@ int main(void) {
         // Undo vector patch in bottom page so verify passes
         if (address == 0)       ch=rstVect & 0xff;
         else if (address == 1)  ch=rstVect >> 8;
-        else if (address == 10)  ch=wdtVect & 0xff;
-        else if (address == 11) ch=wdtVect >> 8;
+        else if (address == 8)  ch=wdtVect & 0xff;
+        else if (address == 9) ch=wdtVect >> 8;
         else ch = pgm_read_byte_near(address);
         address++;
         putch(ch);
       } while (--length);
 #else
+#ifdef __AVR_ATmega1280__
+//      do putch(pgm_read_byte_near(address++));
+//      while (--length);
+      do {
+        uint8_t result;
+        __asm__ ("elpm %0,Z\n":"=r"(result):"z"(address));
+        putch(result);
+        address++;
+      }
+      while (--length);
+#else
       do putch(pgm_read_byte_near(address++));
       while (--length);
 #endif
+#endif
     }
 
     /* Get device signature bytes  */
@@ -422,8 +473,12 @@ uint8_t getch(void) {
   watchdogReset();
 
 #ifdef LED_DATA_FLASH
+#ifdef __AVR_ATmega8__
+  LED_PORT ^= _BV(LED);
+#else
   LED_PIN |= _BV(LED);
 #endif
+#endif
 
 #ifdef SOFT_UART
   __asm__ __volatile__ (
@@ -455,14 +510,19 @@ uint8_t getch(void) {
 #endif
 
 #ifdef LED_DATA_FLASH
+#ifdef __AVR_ATmega8__
+  LED_PORT ^= _BV(LED);
+#else
   LED_PIN |= _BV(LED);
 #endif
+#endif
 
   return ch;
 }
 
 #ifdef SOFT_UART
-//#define UART_B_VALUE (((F_CPU/BAUD_RATE)-23)/6)
+// AVR350 equation: #define UART_B_VALUE (((F_CPU/BAUD_RATE)-23)/6)
+// Adding 3 to numerator simulates nearest rounding for more accurate baud rates
 #define UART_B_VALUE (((F_CPU/BAUD_RATE)-20)/6)
 #if UART_B_VALUE > 255
 #error Baud rate too slow for soft UART
@@ -495,7 +555,11 @@ void flash_led(uint8_t count) {
     TCNT1 = -(F_CPU/(1024*16));
     TIFR1 = _BV(TOV1);
     while(!(TIFR1 & _BV(TOV1)));
+#ifdef __AVR_ATmega8__
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
+#endif
     watchdogReset();
   } while (--count);
 }
@@ -524,7 +588,7 @@ void appStart() {
   __asm__ __volatile__ (
 #ifdef VIRTUAL_BOOT_PARTITION
     // Jump to WDT vector
-    "ldi r30,5\n"
+    "ldi r30,4\n"
     "clr r31\n"
 #else
     // Jump to RST vector
diff --git a/bootloaders/optiboot/optiboot_atmega328.hex b/bootloaders/optiboot/optiboot_atmega328.hex
index e8aa31c..72108ee 100644
--- a/bootloaders/optiboot/optiboot_atmega328.hex
+++ b/bootloaders/optiboot/optiboot_atmega328.hex
@@ -1,33 +1,34 @@
-:107E0000112485E08093810082E08093C00088E1A6

-:107E10008093C10086E08093C20080E18093C4001B

-:107E200084B714BE81FFD0D08DE0C8D0259A86E0FB

+:107E0000112484B714BE81FFE6D085E08093810001

+:107E100082E08093C00088E18093C10086E0809377

+:107E2000C20080E18093C4008EE0CFD0259A86E026

 :107E300020E33CEF91E0309385002093840096BBD3

 :107E4000B09BFECF1D9AA8958150A9F7DD24D3944D

-:107E5000A5E0EA2EF1E1FF2EA4D0813421F481E0E7

-:107E6000BED083E024C0823411F484E103C08534A1

-:107E700019F485E0B4D08AC08535A1F492D0082FDA

-:107E800010E010930102009300028BD090E0982F35

-:107E90008827802B912B880F991F90930102809344

-:107EA000000273C0863529F484E099D080E071D057

-:107EB0006DC0843609F043C07CD0E0910002F0919F

-:107EC000010283E080935700E895C0E0D1E069D0DB

-:107ED0008993809102028150809302028823B9F72E

-:107EE00078D007B600FCFDCF40910002509101020E

-:107EF000A0E0B1E02C9130E011968C91119790E0C8

-:107F0000982F8827822B932B1296FA010C01D0927E

-:107F10005700E89511244E5F5F4FF1E0A038BF078E

-:107F200049F7E0910002F0910102E0925700E895D4

-:107F300007B600FCFDCFF0925700E89527C08437C4

-:107F4000B9F437D046D0E0910002F09101023196A9

-:107F5000F0930102E09300023197E4918E2F19D043

-:107F600080910202815080930202882361F70EC043

-:107F7000853739F42ED08EE10CD085E90AD08FE018

-:107F800096CF813511F488E019D023D080E101D05B

-:107F900063CF982F8091C00085FFFCCF9093C600DF

-:107FA0000895A8958091C00087FFFCCF8091C600FE

-:107FB0000895F7DFF6DF80930202F3CFE0E6F0E00A

-:107FC00098E190838083089580E0F8DFEE27FF2713

-:107FD0000994E7DF803209F0F7DF84E1DACF1F93FD

-:0E7FE000182FDFDF1150E9F7F4DF1F9108952D

+:107E5000A5E0EA2EF1E1FF2EABD0813421F481E0E0

+:107E6000C5D083E020C0823411F484E103C085349E

+:107E700019F485E0BBD091C0853581F499D0082FE5

+:107E800010E096D090E0982F8827802B912B880FB8

+:107E9000991F90930102809300027EC0863529F4D9

+:107EA00084E0A4D080E07CD078C0843609F04EC055

+:107EB00087D0E0910002F091010280E7E030F807FE

+:107EC00018F483E087BFE895C0E0D1E071D08993D2

+:107ED000809102028150809302028823B9F7E091D9

+:107EE0000002F091010280E7E030F80718F083E02B

+:107EF00087BFE89575D007B600FCFDCF4091000222

+:107F000050910102A0E0B1E02C9130E011968C91EB

+:107F1000119790E0982F8827822B932B1296FA01C5

+:107F20000C01D7BEE89511244E5F5F4FF1E0A038F9

+:107F3000BF0751F7E0910002F0910102E7BEE8951A

+:107F400007B600FCFDCFF7BEE89527C08437B9F42B

+:107F500037D046D0E0910002F09101023196F093C3

+:107F60000102E09300023197E4918E2F19D08091A5

+:107F70000202815080930202882361F70EC0853788

+:107F800039F42ED08EE10CD085E90AD08FE08BCF6A

+:107F9000813511F488E019D023D080E101D05CCF85

+:107FA000982F8091C00085FFFCCF9093C600089564

+:107FB000A8958091C00087FFFCCF8091C6000895EE

+:107FC000F7DFF6DF80930202F3CFE0E6F0E098E11E

+:107FD00090838083089580E0F8DFEE27FF270994DF

+:107FE000E7DF803209F0F7DF84E1DACF1F93182F43

+:0C7FF000DFDF1150E9F7F4DF1F91089566

 :0400000300007E007B

 :00000001FF

diff --git a/bootloaders/optiboot/optiboot_atmega328.lst b/bootloaders/optiboot/optiboot_atmega328.lst
index 104799f..1af4a3c 100644
--- a/bootloaders/optiboot/optiboot_atmega328.lst
+++ b/bootloaders/optiboot/optiboot_atmega328.lst
@@ -3,74 +3,79 @@ optiboot_atmega328.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001ee  00007e00  00007e00  00000054  2**1
+  0 .text         000001fc  00007e00  00007e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .debug_aranges 00000028  00000000  00000000  00000242  2**0
+  1 .debug_aranges 00000028  00000000  00000000  00000250  2**0
                   CONTENTS, READONLY, DEBUGGING
-  2 .debug_pubnames 0000006a  00000000  00000000  0000026a  2**0
+  2 .debug_pubnames 0000006a  00000000  00000000  00000278  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_info   00000269  00000000  00000000  000002d4  2**0
+  3 .debug_info   00000284  00000000  00000000  000002e2  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_abbrev 00000196  00000000  00000000  0000053d  2**0
+  4 .debug_abbrev 000001ae  00000000  00000000  00000566  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_line   0000043f  00000000  00000000  000006d3  2**0
+  5 .debug_line   00000450  00000000  00000000  00000714  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_frame  00000090  00000000  00000000  00000b14  2**2
+  6 .debug_frame  00000090  00000000  00000000  00000b64  2**2
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_str    00000136  00000000  00000000  00000ba4  2**0
+  7 .debug_str    00000141  00000000  00000000  00000bf4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_loc    000001d1  00000000  00000000  00000cda  2**0
+  8 .debug_loc    000001e1  00000000  00000000  00000d35  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_ranges 00000068  00000000  00000000  00000eab  2**0
+  9 .debug_ranges 00000068  00000000  00000000  00000f16  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00007e00 <main>:
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
     7e00:	11 24       	eor	r1, r1
+#ifdef __AVR_ATmega8__
+  SP=RAMEND;  // This is done by hardware reset
+#endif
 
-  uint8_t ch;
+  // Adaboot no-wait mod
+  ch = MCUSR;
+    7e02:	84 b7       	in	r24, 0x34	; 52
+  MCUSR = 0;
+    7e04:	14 be       	out	0x34, r1	; 52
+  if (!(ch & _BV(EXTRF))) appStart();
+    7e06:	81 ff       	sbrs	r24, 1
+    7e08:	e6 d0       	rcall	.+460    	; 0x7fd6 <appStart>
 
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-    7e02:	85 e0       	ldi	r24, 0x05	; 5
-    7e04:	80 93 81 00 	sts	0x0081, r24
-#endif
-#ifndef SOFT_UART
+    7e0a:	85 e0       	ldi	r24, 0x05	; 5
+    7e0c:	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
   UCSR0A = _BV(U2X0); //Double speed mode USART0
-    7e08:	82 e0       	ldi	r24, 0x02	; 2
-    7e0a:	80 93 c0 00 	sts	0x00C0, r24
+    7e10:	82 e0       	ldi	r24, 0x02	; 2
+    7e12:	80 93 c0 00 	sts	0x00C0, r24
   UCSR0B = _BV(RXEN0) | _BV(TXEN0);
-    7e0e:	88 e1       	ldi	r24, 0x18	; 24
-    7e10:	80 93 c1 00 	sts	0x00C1, r24
+    7e16:	88 e1       	ldi	r24, 0x18	; 24
+    7e18:	80 93 c1 00 	sts	0x00C1, r24
   UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
-    7e14:	86 e0       	ldi	r24, 0x06	; 6
-    7e16:	80 93 c2 00 	sts	0x00C2, r24
+    7e1c:	86 e0       	ldi	r24, 0x06	; 6
+    7e1e:	80 93 c2 00 	sts	0x00C2, r24
   UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    7e1a:	80 e1       	ldi	r24, 0x10	; 16
-    7e1c:	80 93 c4 00 	sts	0x00C4, r24
+    7e22:	80 e1       	ldi	r24, 0x10	; 16
+    7e24:	80 93 c4 00 	sts	0x00C4, r24
+#endif
 #endif
-
-  // Adaboot no-wait mod
-  ch = MCUSR;
-    7e20:	84 b7       	in	r24, 0x34	; 52
-  MCUSR = 0;
-    7e22:	14 be       	out	0x34, r1	; 52
-  if (!(ch & _BV(EXTRF))) appStart();
-    7e24:	81 ff       	sbrs	r24, 1
-    7e26:	d0 d0       	rcall	.+416    	; 0x7fc8 <appStart>
 
   // Set up watchdog to trigger after 500ms
-  watchdogConfig(WATCHDOG_500MS);
-    7e28:	8d e0       	ldi	r24, 0x0D	; 13
-    7e2a:	c8 d0       	rcall	.+400    	; 0x7fbc <watchdogConfig>
+  watchdogConfig(WATCHDOG_1S);
+    7e28:	8e e0       	ldi	r24, 0x0E	; 14
+    7e2a:	cf d0       	rcall	.+414    	; 0x7fca <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
@@ -99,6 +104,9 @@ void flash_led(uint8_t count) {
     while(!(TIFR1 & _BV(TOV1)));
     7e40:	b0 9b       	sbis	0x16, 0	; 22
     7e42:	fe cf       	rjmp	.-4      	; 0x7e40 <main+0x40>
+#ifdef __AVR_ATmega8__
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
     7e44:	1d 9a       	sbi	0x03, 5	; 3
   return getch();
@@ -108,10 +116,10 @@ void flash_led(uint8_t count) {
 void watchdogReset() {
   __asm__ __volatile__ (
     7e46:	a8 95       	wdr
-    TCNT1 = -(F_CPU/(1024*16));
-    TIFR1 = _BV(TOV1);
-    while(!(TIFR1 & _BV(TOV1)));
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
+#endif
     watchdogReset();
   } while (--count);
     7e48:	81 50       	subi	r24, 0x01	; 1
@@ -124,12 +132,12 @@ void watchdogReset() {
       getNch(1);
     7e4c:	dd 24       	eor	r13, r13
     7e4e:	d3 94       	inc	r13
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
+      __boot_page_write_short((uint16_t)(void*)address);
     7e50:	a5 e0       	ldi	r26, 0x05	; 5
     7e52:	ea 2e       	mov	r14, r26
       boot_spm_busy_wait();
@@ -145,7 +153,7 @@ void watchdogReset() {
   for (;;) {
     /* get character from UART */
     ch = getch();
-    7e58:	a4 d0       	rcall	.+328    	; 0x7fa2 <getch>
+    7e58:	ab d0       	rcall	.+342    	; 0x7fb0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
     7e5a:	81 34       	cpi	r24, 0x41	; 65
@@ -153,10 +161,10 @@ void watchdogReset() {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
     7e5e:	81 e0       	ldi	r24, 0x01	; 1
-    7e60:	be d0       	rcall	.+380    	; 0x7fde <getNch>
+    7e60:	c5 d0       	rcall	.+394    	; 0x7fec <getNch>
       putch(0x03);
     7e62:	83 e0       	ldi	r24, 0x03	; 3
-    7e64:	24 c0       	rjmp	.+72     	; 0x7eae <main+0xae>
+    7e64:	20 c0       	rjmp	.+64     	; 0x7ea6 <main+0xa6>
     }
     else if(ch == STK_SET_DEVICE) {
     7e66:	82 34       	cpi	r24, 0x42	; 66
@@ -172,353 +180,375 @@ void watchdogReset() {
       // SET DEVICE EXT is ignored
       getNch(5);
     7e72:	85 e0       	ldi	r24, 0x05	; 5
-    7e74:	b4 d0       	rcall	.+360    	; 0x7fde <getNch>
-    7e76:	8a c0       	rjmp	.+276    	; 0x7f8c <main+0x18c>
+    7e74:	bb d0       	rcall	.+374    	; 0x7fec <getNch>
+    7e76:	91 c0       	rjmp	.+290    	; 0x7f9a <main+0x19a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
     7e78:	85 35       	cpi	r24, 0x55	; 85
-    7e7a:	a1 f4       	brne	.+40     	; 0x7ea4 <main+0xa4>
+    7e7a:	81 f4       	brne	.+32     	; 0x7e9c <main+0x9c>
       // LOAD ADDRESS
-      address = getch();
-    7e7c:	92 d0       	rcall	.+292    	; 0x7fa2 <getch>
+      uint16_t newAddress;
+      newAddress = getch();
+    7e7c:	99 d0       	rcall	.+306    	; 0x7fb0 <getch>
+      newAddress = (newAddress & 0xff) | (getch() << 8);
     7e7e:	08 2f       	mov	r16, r24
     7e80:	10 e0       	ldi	r17, 0x00	; 0
-    7e82:	10 93 01 02 	sts	0x0201, r17
-    7e86:	00 93 00 02 	sts	0x0200, r16
-      address = (address & 0xff) | (getch() << 8);
-    7e8a:	8b d0       	rcall	.+278    	; 0x7fa2 <getch>
-    7e8c:	90 e0       	ldi	r25, 0x00	; 0
-    7e8e:	98 2f       	mov	r25, r24
-    7e90:	88 27       	eor	r24, r24
-    7e92:	80 2b       	or	r24, r16
-    7e94:	91 2b       	or	r25, r17
-      address += address; // Convert from word address to byte address
-    7e96:	88 0f       	add	r24, r24
-    7e98:	99 1f       	adc	r25, r25
-    7e9a:	90 93 01 02 	sts	0x0201, r25
-    7e9e:	80 93 00 02 	sts	0x0200, r24
-    7ea2:	73 c0       	rjmp	.+230    	; 0x7f8a <main+0x18a>
+    7e82:	96 d0       	rcall	.+300    	; 0x7fb0 <getch>
+    7e84:	90 e0       	ldi	r25, 0x00	; 0
+    7e86:	98 2f       	mov	r25, r24
+    7e88:	88 27       	eor	r24, r24
+    7e8a:	80 2b       	or	r24, r16
+    7e8c:	91 2b       	or	r25, r17
+#ifdef RAMPZ
+      // Transfer top bit to RAMPZ
+      RAMPZ = (newAddress & 0x8000) ? 1 : 0;
+#endif
+      newAddress += newAddress; // Convert from word address to byte address
+    7e8e:	88 0f       	add	r24, r24
+    7e90:	99 1f       	adc	r25, r25
+      address = newAddress;
+    7e92:	90 93 01 02 	sts	0x0201, r25
+    7e96:	80 93 00 02 	sts	0x0200, r24
+    7e9a:	7e c0       	rjmp	.+252    	; 0x7f98 <main+0x198>
       verifySpace();
     }
     else if(ch == STK_UNIVERSAL) {
-    7ea4:	86 35       	cpi	r24, 0x56	; 86
-    7ea6:	29 f4       	brne	.+10     	; 0x7eb2 <main+0xb2>
+    7e9c:	86 35       	cpi	r24, 0x56	; 86
+    7e9e:	29 f4       	brne	.+10     	; 0x7eaa <main+0xaa>
       // UNIVERSAL command is ignored
       getNch(4);
-    7ea8:	84 e0       	ldi	r24, 0x04	; 4
-    7eaa:	99 d0       	rcall	.+306    	; 0x7fde <getNch>
+    7ea0:	84 e0       	ldi	r24, 0x04	; 4
+    7ea2:	a4 d0       	rcall	.+328    	; 0x7fec <getNch>
       putch(0x00);
-    7eac:	80 e0       	ldi	r24, 0x00	; 0
-    7eae:	71 d0       	rcall	.+226    	; 0x7f92 <putch>
-    7eb0:	6d c0       	rjmp	.+218    	; 0x7f8c <main+0x18c>
+    7ea4:	80 e0       	ldi	r24, 0x00	; 0
+    7ea6:	7c d0       	rcall	.+248    	; 0x7fa0 <putch>
+    7ea8:	78 c0       	rjmp	.+240    	; 0x7f9a <main+0x19a>
     }
-    /* Write memory, length is big endian and is in bytes  */
+    /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    7eb2:	84 36       	cpi	r24, 0x64	; 100
-    7eb4:	09 f0       	breq	.+2      	; 0x7eb8 <main+0xb8>
-    7eb6:	43 c0       	rjmp	.+134    	; 0x7f3e <main+0x13e>
+    7eaa:	84 36       	cpi	r24, 0x64	; 100
+    7eac:	09 f0       	breq	.+2      	; 0x7eb0 <main+0xb0>
+    7eae:	4e c0       	rjmp	.+156    	; 0x7f4c <main+0x14c>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
       getLen();
-    7eb8:	7c d0       	rcall	.+248    	; 0x7fb2 <getLen>
-
-      // Immediately start page erase - this will 4.5ms
-      boot_page_erase((uint16_t)(void*)address);
-    7eba:	e0 91 00 02 	lds	r30, 0x0200
-    7ebe:	f0 91 01 02 	lds	r31, 0x0201
+    7eb0:	87 d0       	rcall	.+270    	; 0x7fc0 <getLen>
+
+      // If we are in RWW section, immediately start page erase
+      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    7eb2:	e0 91 00 02 	lds	r30, 0x0200
+    7eb6:	f0 91 01 02 	lds	r31, 0x0201
+    7eba:	80 e7       	ldi	r24, 0x70	; 112
+    7ebc:	e0 30       	cpi	r30, 0x00	; 0
+    7ebe:	f8 07       	cpc	r31, r24
+    7ec0:	18 f4       	brcc	.+6      	; 0x7ec8 <main+0xc8>
     7ec2:	83 e0       	ldi	r24, 0x03	; 3
-    7ec4:	80 93 57 00 	sts	0x0057, r24
-    7ec8:	e8 95       	spm
-    7eca:	c0 e0       	ldi	r28, 0x00	; 0
-    7ecc:	d1 e0       	ldi	r29, 0x01	; 1
-
+    7ec4:	87 bf       	out	0x37, r24	; 55
+    7ec6:	e8 95       	spm
+    7ec8:	c0 e0       	ldi	r28, 0x00	; 0
+    7eca:	d1 e0       	ldi	r29, 0x01	; 1
+      
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    7ece:	69 d0       	rcall	.+210    	; 0x7fa2 <getch>
-    7ed0:	89 93       	st	Y+, r24
+    7ecc:	71 d0       	rcall	.+226    	; 0x7fb0 <getch>
+    7ece:	89 93       	st	Y+, r24
       while (--length);
-    7ed2:	80 91 02 02 	lds	r24, 0x0202
-    7ed6:	81 50       	subi	r24, 0x01	; 1
-    7ed8:	80 93 02 02 	sts	0x0202, r24
-    7edc:	88 23       	and	r24, r24
-    7ede:	b9 f7       	brne	.-18     	; 0x7ece <main+0xce>
+    7ed0:	80 91 02 02 	lds	r24, 0x0202
+    7ed4:	81 50       	subi	r24, 0x01	; 1
+    7ed6:	80 93 02 02 	sts	0x0202, r24
+    7eda:	88 23       	and	r24, r24
+    7edc:	b9 f7       	brne	.-18     	; 0x7ecc <main+0xcc>
+
+      // If we are in NRWW section, page erase has to be delayed until now.
+      // Todo: Take RAMPZ into account
+      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    7ede:	e0 91 00 02 	lds	r30, 0x0200
+    7ee2:	f0 91 01 02 	lds	r31, 0x0201
+    7ee6:	80 e7       	ldi	r24, 0x70	; 112
+    7ee8:	e0 30       	cpi	r30, 0x00	; 0
+    7eea:	f8 07       	cpc	r31, r24
+    7eec:	18 f0       	brcs	.+6      	; 0x7ef4 <main+0xf4>
+    7eee:	83 e0       	ldi	r24, 0x03	; 3
+    7ef0:	87 bf       	out	0x37, r24	; 55
+    7ef2:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    7ee0:	78 d0       	rcall	.+240    	; 0x7fd2 <verifySpace>
+    7ef4:	75 d0       	rcall	.+234    	; 0x7fe0 <verifySpace>
       
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    7ee2:	07 b6       	in	r0, 0x37	; 55
-    7ee4:	00 fc       	sbrc	r0, 0
-    7ee6:	fd cf       	rjmp	.-6      	; 0x7ee2 <main+0xe2>
+    7ef6:	07 b6       	in	r0, 0x37	; 55
+    7ef8:	00 fc       	sbrc	r0, 0
+    7efa:	fd cf       	rjmp	.-6      	; 0x7ef6 <main+0xf6>
       }
 #endif
 
       // Copy buffer into programming buffer
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    7ee8:	40 91 00 02 	lds	r20, 0x0200
-    7eec:	50 91 01 02 	lds	r21, 0x0201
-    7ef0:	a0 e0       	ldi	r26, 0x00	; 0
-    7ef2:	b1 e0       	ldi	r27, 0x01	; 1
+    7efc:	40 91 00 02 	lds	r20, 0x0200
+    7f00:	50 91 01 02 	lds	r21, 0x0201
+    7f04:	a0 e0       	ldi	r26, 0x00	; 0
+    7f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    7ef4:	2c 91       	ld	r18, X
-    7ef6:	30 e0       	ldi	r19, 0x00	; 0
+    7f08:	2c 91       	ld	r18, X
+    7f0a:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    7ef8:	11 96       	adiw	r26, 0x01	; 1
-    7efa:	8c 91       	ld	r24, X
-    7efc:	11 97       	sbiw	r26, 0x01	; 1
-    7efe:	90 e0       	ldi	r25, 0x00	; 0
-    7f00:	98 2f       	mov	r25, r24
-    7f02:	88 27       	eor	r24, r24
-    7f04:	82 2b       	or	r24, r18
-    7f06:	93 2b       	or	r25, r19
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+    7f0c:	11 96       	adiw	r26, 0x01	; 1
+    7f0e:	8c 91       	ld	r24, X
+    7f10:	11 97       	sbiw	r26, 0x01	; 1
+    7f12:	90 e0       	ldi	r25, 0x00	; 0
+    7f14:	98 2f       	mov	r25, r24
+    7f16:	88 27       	eor	r24, r24
+    7f18:	82 2b       	or	r24, r18
+    7f1a:	93 2b       	or	r25, r19
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    7f08:	12 96       	adiw	r26, 0x02	; 2
+    7f1c:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
-    7f0a:	fa 01       	movw	r30, r20
-    7f0c:	0c 01       	movw	r0, r24
-    7f0e:	d0 92 57 00 	sts	0x0057, r13
-    7f12:	e8 95       	spm
-    7f14:	11 24       	eor	r1, r1
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    7f1e:	fa 01       	movw	r30, r20
+    7f20:	0c 01       	movw	r0, r24
+    7f22:	d7 be       	out	0x37, r13	; 55
+    7f24:	e8 95       	spm
+    7f26:	11 24       	eor	r1, r1
         addrPtr += 2;
-    7f16:	4e 5f       	subi	r20, 0xFE	; 254
-    7f18:	5f 4f       	sbci	r21, 0xFF	; 255
+    7f28:	4e 5f       	subi	r20, 0xFE	; 254
+    7f2a:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    7f1a:	f1 e0       	ldi	r31, 0x01	; 1
-    7f1c:	a0 38       	cpi	r26, 0x80	; 128
-    7f1e:	bf 07       	cpc	r27, r31
-    7f20:	49 f7       	brne	.-46     	; 0x7ef4 <main+0xf4>
+    7f2c:	f1 e0       	ldi	r31, 0x01	; 1
+    7f2e:	a0 38       	cpi	r26, 0x80	; 128
+    7f30:	bf 07       	cpc	r27, r31
+    7f32:	51 f7       	brne	.-44     	; 0x7f08 <main+0x108>
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    7f22:	e0 91 00 02 	lds	r30, 0x0200
-    7f26:	f0 91 01 02 	lds	r31, 0x0201
-    7f2a:	e0 92 57 00 	sts	0x0057, r14
-    7f2e:	e8 95       	spm
+      __boot_page_write_short((uint16_t)(void*)address);
+    7f34:	e0 91 00 02 	lds	r30, 0x0200
+    7f38:	f0 91 01 02 	lds	r31, 0x0201
+    7f3c:	e7 be       	out	0x37, r14	; 55
+    7f3e:	e8 95       	spm
       boot_spm_busy_wait();
-    7f30:	07 b6       	in	r0, 0x37	; 55
-    7f32:	00 fc       	sbrc	r0, 0
-    7f34:	fd cf       	rjmp	.-6      	; 0x7f30 <main+0x130>
+    7f40:	07 b6       	in	r0, 0x37	; 55
+    7f42:	00 fc       	sbrc	r0, 0
+    7f44:	fd cf       	rjmp	.-6      	; 0x7f40 <main+0x140>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    7f36:	f0 92 57 00 	sts	0x0057, r15
-    7f3a:	e8 95       	spm
-    7f3c:	27 c0       	rjmp	.+78     	; 0x7f8c <main+0x18c>
+    7f46:	f7 be       	out	0x37, r15	; 55
+    7f48:	e8 95       	spm
+    7f4a:	27 c0       	rjmp	.+78     	; 0x7f9a <main+0x19a>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    7f3e:	84 37       	cpi	r24, 0x74	; 116
-    7f40:	b9 f4       	brne	.+46     	; 0x7f70 <main+0x170>
+    7f4c:	84 37       	cpi	r24, 0x74	; 116
+    7f4e:	b9 f4       	brne	.+46     	; 0x7f7e <main+0x17e>
       // READ PAGE - we only read flash
       getLen();
-    7f42:	37 d0       	rcall	.+110    	; 0x7fb2 <getLen>
+    7f50:	37 d0       	rcall	.+110    	; 0x7fc0 <getLen>
       verifySpace();
-    7f44:	46 d0       	rcall	.+140    	; 0x7fd2 <verifySpace>
-        else ch = pgm_read_byte_near(address);
+    7f52:	46 d0       	rcall	.+140    	; 0x7fe0 <verifySpace>
+        putch(result);
         address++;
-        putch(ch);
-      } while (--length);
+      }
+      while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    7f46:	e0 91 00 02 	lds	r30, 0x0200
-    7f4a:	f0 91 01 02 	lds	r31, 0x0201
-    7f4e:	31 96       	adiw	r30, 0x01	; 1
-    7f50:	f0 93 01 02 	sts	0x0201, r31
-    7f54:	e0 93 00 02 	sts	0x0200, r30
-    7f58:	31 97       	sbiw	r30, 0x01	; 1
-    7f5a:	e4 91       	lpm	r30, Z+
-    7f5c:	8e 2f       	mov	r24, r30
-    7f5e:	19 d0       	rcall	.+50     	; 0x7f92 <putch>
+    7f54:	e0 91 00 02 	lds	r30, 0x0200
+    7f58:	f0 91 01 02 	lds	r31, 0x0201
+    7f5c:	31 96       	adiw	r30, 0x01	; 1
+    7f5e:	f0 93 01 02 	sts	0x0201, r31
+    7f62:	e0 93 00 02 	sts	0x0200, r30
+    7f66:	31 97       	sbiw	r30, 0x01	; 1
+    7f68:	e4 91       	lpm	r30, Z+
+    7f6a:	8e 2f       	mov	r24, r30
+    7f6c:	19 d0       	rcall	.+50     	; 0x7fa0 <putch>
       while (--length);
-    7f60:	80 91 02 02 	lds	r24, 0x0202
-    7f64:	81 50       	subi	r24, 0x01	; 1
-    7f66:	80 93 02 02 	sts	0x0202, r24
-    7f6a:	88 23       	and	r24, r24
-    7f6c:	61 f7       	brne	.-40     	; 0x7f46 <main+0x146>
-    7f6e:	0e c0       	rjmp	.+28     	; 0x7f8c <main+0x18c>
+    7f6e:	80 91 02 02 	lds	r24, 0x0202
+    7f72:	81 50       	subi	r24, 0x01	; 1
+    7f74:	80 93 02 02 	sts	0x0202, r24
+    7f78:	88 23       	and	r24, r24
+    7f7a:	61 f7       	brne	.-40     	; 0x7f54 <main+0x154>
+    7f7c:	0e c0       	rjmp	.+28     	; 0x7f9a <main+0x19a>
+#endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    7f70:	85 37       	cpi	r24, 0x75	; 117
-    7f72:	39 f4       	brne	.+14     	; 0x7f82 <main+0x182>
+    7f7e:	85 37       	cpi	r24, 0x75	; 117
+    7f80:	39 f4       	brne	.+14     	; 0x7f90 <main+0x190>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    7f74:	2e d0       	rcall	.+92     	; 0x7fd2 <verifySpace>
+    7f82:	2e d0       	rcall	.+92     	; 0x7fe0 <verifySpace>
       putch(SIGNATURE_0);
-    7f76:	8e e1       	ldi	r24, 0x1E	; 30
-    7f78:	0c d0       	rcall	.+24     	; 0x7f92 <putch>
+    7f84:	8e e1       	ldi	r24, 0x1E	; 30
+    7f86:	0c d0       	rcall	.+24     	; 0x7fa0 <putch>
       putch(SIGNATURE_1);
-    7f7a:	85 e9       	ldi	r24, 0x95	; 149
-    7f7c:	0a d0       	rcall	.+20     	; 0x7f92 <putch>
+    7f88:	85 e9       	ldi	r24, 0x95	; 149
+    7f8a:	0a d0       	rcall	.+20     	; 0x7fa0 <putch>
       putch(SIGNATURE_2);
-    7f7e:	8f e0       	ldi	r24, 0x0F	; 15
-    7f80:	96 cf       	rjmp	.-212    	; 0x7eae <main+0xae>
+    7f8c:	8f e0       	ldi	r24, 0x0F	; 15
+    7f8e:	8b cf       	rjmp	.-234    	; 0x7ea6 <main+0xa6>
     }
     else if (ch == 'Q') {
-    7f82:	81 35       	cpi	r24, 0x51	; 81
-    7f84:	11 f4       	brne	.+4      	; 0x7f8a <main+0x18a>
+    7f90:	81 35       	cpi	r24, 0x51	; 81
+    7f92:	11 f4       	brne	.+4      	; 0x7f98 <main+0x198>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    7f86:	88 e0       	ldi	r24, 0x08	; 8
-    7f88:	19 d0       	rcall	.+50     	; 0x7fbc <watchdogConfig>
+    7f94:	88 e0       	ldi	r24, 0x08	; 8
+    7f96:	19 d0       	rcall	.+50     	; 0x7fca <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    7f8a:	23 d0       	rcall	.+70     	; 0x7fd2 <verifySpace>
+    7f98:	23 d0       	rcall	.+70     	; 0x7fe0 <verifySpace>
     }
     putch(STK_OK);
-    7f8c:	80 e1       	ldi	r24, 0x10	; 16
-    7f8e:	01 d0       	rcall	.+2      	; 0x7f92 <putch>
-    7f90:	63 cf       	rjmp	.-314    	; 0x7e58 <main+0x58>
+    7f9a:	80 e1       	ldi	r24, 0x10	; 16
+    7f9c:	01 d0       	rcall	.+2      	; 0x7fa0 <putch>
+    7f9e:	5c cf       	rjmp	.-328    	; 0x7e58 <main+0x58>
 
-00007f92 <putch>:
+00007fa0 <putch>:
   }
 }
 
 void putch(char ch) {
-    7f92:	98 2f       	mov	r25, r24
+    7fa0:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    7f94:	80 91 c0 00 	lds	r24, 0x00C0
-    7f98:	85 ff       	sbrs	r24, 5
-    7f9a:	fc cf       	rjmp	.-8      	; 0x7f94 <putch+0x2>
+    7fa2:	80 91 c0 00 	lds	r24, 0x00C0
+    7fa6:	85 ff       	sbrs	r24, 5
+    7fa8:	fc cf       	rjmp	.-8      	; 0x7fa2 <putch+0x2>
   UDR0 = ch;
-    7f9c:	90 93 c6 00 	sts	0x00C6, r25
+    7faa:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    7fa0:	08 95       	ret
+    7fae:	08 95       	ret
 
-00007fa2 <getch>:
+00007fb0 <getch>:
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    7fa2:	a8 95       	wdr
+    7fb0:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    7fa4:	80 91 c0 00 	lds	r24, 0x00C0
-    7fa8:	87 ff       	sbrs	r24, 7
-    7faa:	fc cf       	rjmp	.-8      	; 0x7fa4 <getch+0x2>
+    7fb2:	80 91 c0 00 	lds	r24, 0x00C0
+    7fb6:	87 ff       	sbrs	r24, 7
+    7fb8:	fc cf       	rjmp	.-8      	; 0x7fb2 <getch+0x2>
   ch = UDR0;
-    7fac:	80 91 c6 00 	lds	r24, 0x00C6
-#ifdef LED_DATA_FLASH
+    7fba:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
+#endif
 
   return ch;
 }
-    7fb0:	08 95       	ret
+    7fbe:	08 95       	ret
 
-00007fb2 <getLen>:
+00007fc0 <getLen>:
   } while (--count);
 }
 #endif
 
 uint8_t getLen() {
   getch();
-    7fb2:	f7 df       	rcall	.-18     	; 0x7fa2 <getch>
+    7fc0:	f7 df       	rcall	.-18     	; 0x7fb0 <getch>
   length = getch();
-    7fb4:	f6 df       	rcall	.-20     	; 0x7fa2 <getch>
-    7fb6:	80 93 02 02 	sts	0x0202, r24
+    7fc2:	f6 df       	rcall	.-20     	; 0x7fb0 <getch>
+    7fc4:	80 93 02 02 	sts	0x0202, r24
   return getch();
 }
-    7fba:	f3 cf       	rjmp	.-26     	; 0x7fa2 <getch>
+    7fc8:	f3 cf       	rjmp	.-26     	; 0x7fb0 <getch>
 
-00007fbc <watchdogConfig>:
+00007fca <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    7fbc:	e0 e6       	ldi	r30, 0x60	; 96
-    7fbe:	f0 e0       	ldi	r31, 0x00	; 0
-    7fc0:	98 e1       	ldi	r25, 0x18	; 24
-    7fc2:	90 83       	st	Z, r25
+    7fca:	e0 e6       	ldi	r30, 0x60	; 96
+    7fcc:	f0 e0       	ldi	r31, 0x00	; 0
+    7fce:	98 e1       	ldi	r25, 0x18	; 24
+    7fd0:	90 83       	st	Z, r25
   WDTCSR = x;
-    7fc4:	80 83       	st	Z, r24
+    7fd2:	80 83       	st	Z, r24
 }
-    7fc6:	08 95       	ret
+    7fd4:	08 95       	ret
 
-00007fc8 <appStart>:
+00007fd6 <appStart>:
 
 void appStart() {
   watchdogConfig(WATCHDOG_OFF);
-    7fc8:	80 e0       	ldi	r24, 0x00	; 0
-    7fca:	f8 df       	rcall	.-16     	; 0x7fbc <watchdogConfig>
+    7fd6:	80 e0       	ldi	r24, 0x00	; 0
+    7fd8:	f8 df       	rcall	.-16     	; 0x7fca <watchdogConfig>
   __asm__ __volatile__ (
-    7fcc:	ee 27       	eor	r30, r30
-    7fce:	ff 27       	eor	r31, r31
-    7fd0:	09 94       	ijmp
+    7fda:	ee 27       	eor	r30, r30
+    7fdc:	ff 27       	eor	r31, r31
+    7fde:	09 94       	ijmp
 
-00007fd2 <verifySpace>:
+00007fe0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) appStart();
-    7fd2:	e7 df       	rcall	.-50     	; 0x7fa2 <getch>
-    7fd4:	80 32       	cpi	r24, 0x20	; 32
-    7fd6:	09 f0       	breq	.+2      	; 0x7fda <verifySpace+0x8>
-    7fd8:	f7 df       	rcall	.-18     	; 0x7fc8 <appStart>
+    7fe0:	e7 df       	rcall	.-50     	; 0x7fb0 <getch>
+    7fe2:	80 32       	cpi	r24, 0x20	; 32
+    7fe4:	09 f0       	breq	.+2      	; 0x7fe8 <verifySpace+0x8>
+    7fe6:	f7 df       	rcall	.-18     	; 0x7fd6 <appStart>
   putch(STK_INSYNC);
-    7fda:	84 e1       	ldi	r24, 0x14	; 20
+    7fe8:	84 e1       	ldi	r24, 0x14	; 20
 }
-    7fdc:	da cf       	rjmp	.-76     	; 0x7f92 <putch>
+    7fea:	da cf       	rjmp	.-76     	; 0x7fa0 <putch>
 
-00007fde <getNch>:
+00007fec <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    7fde:	1f 93       	push	r17
-    7fe0:	18 2f       	mov	r17, r24
+    7fec:	1f 93       	push	r17
+    7fee:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    7fe2:	df df       	rcall	.-66     	; 0x7fa2 <getch>
-    7fe4:	11 50       	subi	r17, 0x01	; 1
-    7fe6:	e9 f7       	brne	.-6      	; 0x7fe2 <getNch+0x4>
+    7ff0:	df df       	rcall	.-66     	; 0x7fb0 <getch>
+    7ff2:	11 50       	subi	r17, 0x01	; 1
+    7ff4:	e9 f7       	brne	.-6      	; 0x7ff0 <getNch+0x4>
   verifySpace();
-    7fe8:	f4 df       	rcall	.-24     	; 0x7fd2 <verifySpace>
+    7ff6:	f4 df       	rcall	.-24     	; 0x7fe0 <verifySpace>
 }
-    7fea:	1f 91       	pop	r17
-    7fec:	08 95       	ret
+    7ff8:	1f 91       	pop	r17
+    7ffa:	08 95       	ret
diff --git a/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex b/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex
index d6ac145..f4a1514 100644
--- a/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex
+++ b/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex
@@ -1,33 +1,34 @@
-:107E000085E08093810082E08093C00088E18093C8

-:107E1000C10086E08093C20088E08093C40084B7EC

-:107E200014BE81FFD0D08DE0C8D0259A86E028E12D

-:107E30003EEF91E0309385002093840096BBB09B89

-:107E4000FECF1D9AA8958150A9F7DD24D394A5E013

-:107E5000EA2EF1E1FF2EA4D0813421F481E0BED0DE

-:107E600083E024C0823411F484E103C0853419F422

-:107E700085E0B4D08AC08535A1F492D0082F10E0F7

-:107E800010930102009300028BD090E0982F882776

-:107E9000802B912B880F991F9093010280930002F1

-:107EA00073C0863529F484E099D080E071D06DC02C

-:107EB000843609F043C07CD0E0910002F0910102C9

-:107EC00083E080935700E895C0E0D1E069D08993C2

-:107ED000809102028150809302028823B9F778D002

-:107EE00007B600FCFDCF4091000250910102A0E0D6

-:107EF000B1E02C9130E011968C91119790E0982F81

-:107F00008827822B932B1296FA010C01D0925700EE

-:107F1000E89511244E5F5F4FF1E0A038BF0749F7A5

-:107F2000E0910002F0910102E0925700E89507B657

-:107F300000FCFDCFF0925700E89527C08437B9F4D4

-:107F400037D046D0E0910002F09101023196F093D3

-:107F50000102E09300023197E4918E2F19D08091B5

-:107F60000202815080930202882361F70EC0853798

-:107F700039F42ED08EE10CD085E90AD08FE096CF6F

-:107F8000813511F488E019D023D080E101D063CF8E

-:107F9000982F8091C00085FFFCCF9093C600089574

-:107FA000A8958091C00087FFFCCF8091C6000895FE

-:107FB000F7DFF6DF80930202F3CFE0E6F0E098E12E

-:107FC00090838083089580E0F8DFEE27FF270994EF

-:107FD000E7DF803209F0F7DF84E1DACF1F93182F53

-:0C7FE000DFDF1150E9F7F4DF1F91089576

+:107E0000112484B714BE81FFE6D085E08093810001

+:107E100082E08093C00088E18093C10086E0809377

+:107E2000C20088E08093C4008EE0CFD0259A86E01F

+:107E300028E13EEF91E0309385002093840096BBCB

+:107E4000B09BFECF1D9AA8958150A9F7DD24D3944D

+:107E5000A5E0EA2EF1E1FF2EABD0813421F481E0E0

+:107E6000C5D083E020C0823411F484E103C085349E

+:107E700019F485E0BBD091C0853581F499D0082FE5

+:107E800010E096D090E0982F8827802B912B880FB8

+:107E9000991F90930102809300027EC0863529F4D9

+:107EA00084E0A4D080E07CD078C0843609F04EC055

+:107EB00087D0E0910002F091010280E7E030F807FE

+:107EC00018F483E087BFE895C0E0D1E071D08993D2

+:107ED000809102028150809302028823B9F7E091D9

+:107EE0000002F091010280E7E030F80718F083E02B

+:107EF00087BFE89575D007B600FCFDCF4091000222

+:107F000050910102A0E0B1E02C9130E011968C91EB

+:107F1000119790E0982F8827822B932B1296FA01C5

+:107F20000C01D7BEE89511244E5F5F4FF1E0A038F9

+:107F3000BF0751F7E0910002F0910102E7BEE8951A

+:107F400007B600FCFDCFF7BEE89527C08437B9F42B

+:107F500037D046D0E0910002F09101023196F093C3

+:107F60000102E09300023197E4918E2F19D08091A5

+:107F70000202815080930202882361F70EC0853788

+:107F800039F42ED08EE10CD085E90AD08FE08BCF6A

+:107F9000813511F488E019D023D080E101D05CCF85

+:107FA000982F8091C00085FFFCCF9093C600089564

+:107FB000A8958091C00087FFFCCF8091C6000895EE

+:107FC000F7DFF6DF80930202F3CFE0E6F0E098E11E

+:107FD00090838083089580E0F8DFEE27FF270994DF

+:107FE000E7DF803209F0F7DF84E1DACF1F93182F43

+:0C7FF000DFDF1150E9F7F4DF1F91089566

 :0400000300007E007B

 :00000001FF

diff --git a/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst b/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst
index 46eda68..4cbcf62 100644
--- a/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst
+++ b/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst
@@ -3,237 +3,257 @@ optiboot_atmega328_pro_8MHz.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001ec  00007e00  00007e00  00000054  2**1
+  0 .text         000001fc  00007e00  00007e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .debug_aranges 00000028  00000000  00000000  00000240  2**0
+  1 .debug_aranges 00000028  00000000  00000000  00000250  2**0
                   CONTENTS, READONLY, DEBUGGING
-  2 .debug_pubnames 0000006a  00000000  00000000  00000268  2**0
+  2 .debug_pubnames 0000006a  00000000  00000000  00000278  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_info   00000269  00000000  00000000  000002d2  2**0
+  3 .debug_info   00000284  00000000  00000000  000002e2  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_abbrev 00000196  00000000  00000000  0000053b  2**0
+  4 .debug_abbrev 000001ae  00000000  00000000  00000566  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_line   000003d3  00000000  00000000  000006d1  2**0
+  5 .debug_line   00000450  00000000  00000000  00000714  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_frame  00000090  00000000  00000000  00000aa4  2**2
+  6 .debug_frame  00000090  00000000  00000000  00000b64  2**2
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_str    00000135  00000000  00000000  00000b34  2**0
+  7 .debug_str    00000141  00000000  00000000  00000bf4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_loc    000001d1  00000000  00000000  00000c69  2**0
+  8 .debug_loc    000001e1  00000000  00000000  00000d35  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_ranges 00000068  00000000  00000000  00000e3a  2**0
+  9 .debug_ranges 00000068  00000000  00000000  00000f16  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00007e00 <main>:
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    7e00:	85 e0       	ldi	r24, 0x05	; 5
-    7e02:	80 93 81 00 	sts	0x0081, r24
+    7e00:	11 24       	eor	r1, r1
+#ifdef __AVR_ATmega8__
+  SP=RAMEND;  // This is done by hardware reset
+#endif
+
+  // Adaboot no-wait mod
+  ch = MCUSR;
+    7e02:	84 b7       	in	r24, 0x34	; 52
+  MCUSR = 0;
+    7e04:	14 be       	out	0x34, r1	; 52
+  if (!(ch & _BV(EXTRF))) appStart();
+    7e06:	81 ff       	sbrs	r24, 1
+    7e08:	e6 d0       	rcall	.+460    	; 0x7fd6 <appStart>
+
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-#endif
-#ifndef SOFT_UART
+    7e0a:	85 e0       	ldi	r24, 0x05	; 5
+    7e0c:	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
   UCSR0A = _BV(U2X0); //Double speed mode USART0
-    7e06:	82 e0       	ldi	r24, 0x02	; 2
-    7e08:	80 93 c0 00 	sts	0x00C0, r24
+    7e10:	82 e0       	ldi	r24, 0x02	; 2
+    7e12:	80 93 c0 00 	sts	0x00C0, r24
   UCSR0B = _BV(RXEN0) | _BV(TXEN0);
-    7e0c:	88 e1       	ldi	r24, 0x18	; 24
-    7e0e:	80 93 c1 00 	sts	0x00C1, r24
+    7e16:	88 e1       	ldi	r24, 0x18	; 24
+    7e18:	80 93 c1 00 	sts	0x00C1, r24
   UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
-    7e12:	86 e0       	ldi	r24, 0x06	; 6
-    7e14:	80 93 c2 00 	sts	0x00C2, r24
+    7e1c:	86 e0       	ldi	r24, 0x06	; 6
+    7e1e:	80 93 c2 00 	sts	0x00C2, r24
   UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    7e18:	88 e0       	ldi	r24, 0x08	; 8
-    7e1a:	80 93 c4 00 	sts	0x00C4, r24
+    7e22:	88 e0       	ldi	r24, 0x08	; 8
+    7e24:	80 93 c4 00 	sts	0x00C4, r24
+#endif
 #endif
-
-  // Adaboot no-wait mod
-  ch = MCUSR;
-    7e1e:	84 b7       	in	r24, 0x34	; 52
-  MCUSR = 0;
-    7e20:	14 be       	out	0x34, r1	; 52
-  if (!(ch & _BV(EXTRF))) appStart();
-    7e22:	81 ff       	sbrs	r24, 1
-    7e24:	d0 d0       	rcall	.+416    	; 0x7fc6 <appStart>
 
   // Set up watchdog to trigger after 500ms
-  watchdogConfig(WATCHDOG_500MS);
-    7e26:	8d e0       	ldi	r24, 0x0D	; 13
-    7e28:	c8 d0       	rcall	.+400    	; 0x7fba <watchdogConfig>
+  watchdogConfig(WATCHDOG_1S);
+    7e28:	8e e0       	ldi	r24, 0x0E	; 14
+    7e2a:	cf d0       	rcall	.+414    	; 0x7fca <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    7e2a:	25 9a       	sbi	0x04, 5	; 4
-    7e2c:	86 e0       	ldi	r24, 0x06	; 6
+    7e2c:	25 9a       	sbi	0x04, 5	; 4
+    7e2e:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    7e2e:	28 e1       	ldi	r18, 0x18	; 24
-    7e30:	3e ef       	ldi	r19, 0xFE	; 254
+    7e30:	28 e1       	ldi	r18, 0x18	; 24
+    7e32:	3e ef       	ldi	r19, 0xFE	; 254
     TIFR1 = _BV(TOV1);
-    7e32:	91 e0       	ldi	r25, 0x01	; 1
+    7e34:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    7e34:	30 93 85 00 	sts	0x0085, r19
-    7e38:	20 93 84 00 	sts	0x0084, r18
+    7e36:	30 93 85 00 	sts	0x0085, r19
+    7e3a:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    7e3c:	96 bb       	out	0x16, r25	; 22
+    7e3e:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    7e3e:	b0 9b       	sbis	0x16, 0	; 22
-    7e40:	fe cf       	rjmp	.-4      	; 0x7e3e <main+0x3e>
+    7e40:	b0 9b       	sbis	0x16, 0	; 22
+    7e42:	fe cf       	rjmp	.-4      	; 0x7e40 <main+0x40>
+#ifdef __AVR_ATmega8__
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
-    7e42:	1d 9a       	sbi	0x03, 5	; 3
+    7e44:	1d 9a       	sbi	0x03, 5	; 3
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    7e44:	a8 95       	wdr
-    TCNT1 = -(F_CPU/(1024*16));
-    TIFR1 = _BV(TOV1);
-    while(!(TIFR1 & _BV(TOV1)));
+    7e46:	a8 95       	wdr
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
+#endif
     watchdogReset();
   } while (--count);
-    7e46:	81 50       	subi	r24, 0x01	; 1
-    7e48:	a9 f7       	brne	.-22     	; 0x7e34 <main+0x34>
+    7e48:	81 50       	subi	r24, 0x01	; 1
+    7e4a:	a9 f7       	brne	.-22     	; 0x7e36 <main+0x36>
     /* get character from UART */
     ch = getch();
 
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    7e4a:	dd 24       	eor	r13, r13
-    7e4c:	d3 94       	inc	r13
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
+    7e4c:	dd 24       	eor	r13, r13
+    7e4e:	d3 94       	inc	r13
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    7e4e:	a5 e0       	ldi	r26, 0x05	; 5
-    7e50:	ea 2e       	mov	r14, r26
+      __boot_page_write_short((uint16_t)(void*)address);
+    7e50:	a5 e0       	ldi	r26, 0x05	; 5
+    7e52:	ea 2e       	mov	r14, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    7e52:	f1 e1       	ldi	r31, 0x11	; 17
-    7e54:	ff 2e       	mov	r15, r31
+    7e54:	f1 e1       	ldi	r31, 0x11	; 17
+    7e56:	ff 2e       	mov	r15, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    7e56:	a4 d0       	rcall	.+328    	; 0x7fa0 <getch>
+    7e58:	ab d0       	rcall	.+342    	; 0x7fb0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    7e58:	81 34       	cpi	r24, 0x41	; 65
-    7e5a:	21 f4       	brne	.+8      	; 0x7e64 <main+0x64>
+    7e5a:	81 34       	cpi	r24, 0x41	; 65
+    7e5c:	21 f4       	brne	.+8      	; 0x7e66 <main+0x66>
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    7e5c:	81 e0       	ldi	r24, 0x01	; 1
-    7e5e:	be d0       	rcall	.+380    	; 0x7fdc <verifySpace+0xc>
+    7e5e:	81 e0       	ldi	r24, 0x01	; 1
+    7e60:	c5 d0       	rcall	.+394    	; 0x7fec <getNch>
       putch(0x03);
-    7e60:	83 e0       	ldi	r24, 0x03	; 3
-    7e62:	24 c0       	rjmp	.+72     	; 0x7eac <main+0xac>
+    7e62:	83 e0       	ldi	r24, 0x03	; 3
+    7e64:	20 c0       	rjmp	.+64     	; 0x7ea6 <main+0xa6>
     }
     else if(ch == STK_SET_DEVICE) {
-    7e64:	82 34       	cpi	r24, 0x42	; 66
-    7e66:	11 f4       	brne	.+4      	; 0x7e6c <main+0x6c>
+    7e66:	82 34       	cpi	r24, 0x42	; 66
+    7e68:	11 f4       	brne	.+4      	; 0x7e6e <main+0x6e>
       // SET DEVICE is ignored
       getNch(20);
-    7e68:	84 e1       	ldi	r24, 0x14	; 20
-    7e6a:	03 c0       	rjmp	.+6      	; 0x7e72 <main+0x72>
+    7e6a:	84 e1       	ldi	r24, 0x14	; 20
+    7e6c:	03 c0       	rjmp	.+6      	; 0x7e74 <main+0x74>
     }
     else if(ch == STK_SET_DEVICE_EXT) {
-    7e6c:	85 34       	cpi	r24, 0x45	; 69
-    7e6e:	19 f4       	brne	.+6      	; 0x7e76 <main+0x76>
+    7e6e:	85 34       	cpi	r24, 0x45	; 69
+    7e70:	19 f4       	brne	.+6      	; 0x7e78 <main+0x78>
       // SET DEVICE EXT is ignored
       getNch(5);
-    7e70:	85 e0       	ldi	r24, 0x05	; 5
-    7e72:	b4 d0       	rcall	.+360    	; 0x7fdc <verifySpace+0xc>
-    7e74:	8a c0       	rjmp	.+276    	; 0x7f8a <main+0x18a>
+    7e72:	85 e0       	ldi	r24, 0x05	; 5
+    7e74:	bb d0       	rcall	.+374    	; 0x7fec <getNch>
+    7e76:	91 c0       	rjmp	.+290    	; 0x7f9a <main+0x19a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
-    7e76:	85 35       	cpi	r24, 0x55	; 85
-    7e78:	a1 f4       	brne	.+40     	; 0x7ea2 <main+0xa2>
+    7e78:	85 35       	cpi	r24, 0x55	; 85
+    7e7a:	81 f4       	brne	.+32     	; 0x7e9c <main+0x9c>
       // LOAD ADDRESS
-      address = getch();
-    7e7a:	92 d0       	rcall	.+292    	; 0x7fa0 <getch>
-    7e7c:	08 2f       	mov	r16, r24
-    7e7e:	10 e0       	ldi	r17, 0x00	; 0
-    7e80:	10 93 01 02 	sts	0x0201, r17
-    7e84:	00 93 00 02 	sts	0x0200, r16
-      address = (address & 0xff) | (getch() << 8);
-    7e88:	8b d0       	rcall	.+278    	; 0x7fa0 <getch>
-    7e8a:	90 e0       	ldi	r25, 0x00	; 0
-    7e8c:	98 2f       	mov	r25, r24
-    7e8e:	88 27       	eor	r24, r24
-    7e90:	80 2b       	or	r24, r16
-    7e92:	91 2b       	or	r25, r17
-      address += address; // Convert from word address to byte address
-    7e94:	88 0f       	add	r24, r24
-    7e96:	99 1f       	adc	r25, r25
-    7e98:	90 93 01 02 	sts	0x0201, r25
-    7e9c:	80 93 00 02 	sts	0x0200, r24
-    7ea0:	73 c0       	rjmp	.+230    	; 0x7f88 <main+0x188>
+      uint16_t newAddress;
+      newAddress = getch();
+    7e7c:	99 d0       	rcall	.+306    	; 0x7fb0 <getch>
+      newAddress = (newAddress & 0xff) | (getch() << 8);
+    7e7e:	08 2f       	mov	r16, r24
+    7e80:	10 e0       	ldi	r17, 0x00	; 0
+    7e82:	96 d0       	rcall	.+300    	; 0x7fb0 <getch>
+    7e84:	90 e0       	ldi	r25, 0x00	; 0
+    7e86:	98 2f       	mov	r25, r24
+    7e88:	88 27       	eor	r24, r24
+    7e8a:	80 2b       	or	r24, r16
+    7e8c:	91 2b       	or	r25, r17
+#ifdef RAMPZ
+      // Transfer top bit to RAMPZ
+      RAMPZ = (newAddress & 0x8000) ? 1 : 0;
+#endif
+      newAddress += newAddress; // Convert from word address to byte address
+    7e8e:	88 0f       	add	r24, r24
+    7e90:	99 1f       	adc	r25, r25
+      address = newAddress;
+    7e92:	90 93 01 02 	sts	0x0201, r25
+    7e96:	80 93 00 02 	sts	0x0200, r24
+    7e9a:	7e c0       	rjmp	.+252    	; 0x7f98 <main+0x198>
       verifySpace();
     }
     else if(ch == STK_UNIVERSAL) {
-    7ea2:	86 35       	cpi	r24, 0x56	; 86
-    7ea4:	29 f4       	brne	.+10     	; 0x7eb0 <main+0xb0>
+    7e9c:	86 35       	cpi	r24, 0x56	; 86
+    7e9e:	29 f4       	brne	.+10     	; 0x7eaa <main+0xaa>
       // UNIVERSAL command is ignored
       getNch(4);
-    7ea6:	84 e0       	ldi	r24, 0x04	; 4
-    7ea8:	99 d0       	rcall	.+306    	; 0x7fdc <verifySpace+0xc>
+    7ea0:	84 e0       	ldi	r24, 0x04	; 4
+    7ea2:	a4 d0       	rcall	.+328    	; 0x7fec <getNch>
       putch(0x00);
-    7eaa:	80 e0       	ldi	r24, 0x00	; 0
-    7eac:	71 d0       	rcall	.+226    	; 0x7f90 <putch>
-    7eae:	6d c0       	rjmp	.+218    	; 0x7f8a <main+0x18a>
+    7ea4:	80 e0       	ldi	r24, 0x00	; 0
+    7ea6:	7c d0       	rcall	.+248    	; 0x7fa0 <putch>
+    7ea8:	78 c0       	rjmp	.+240    	; 0x7f9a <main+0x19a>
     }
-    /* Write memory, length is big endian and is in bytes  */
+    /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    7eb0:	84 36       	cpi	r24, 0x64	; 100
-    7eb2:	09 f0       	breq	.+2      	; 0x7eb6 <main+0xb6>
-    7eb4:	43 c0       	rjmp	.+134    	; 0x7f3c <main+0x13c>
+    7eaa:	84 36       	cpi	r24, 0x64	; 100
+    7eac:	09 f0       	breq	.+2      	; 0x7eb0 <main+0xb0>
+    7eae:	4e c0       	rjmp	.+156    	; 0x7f4c <main+0x14c>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
       getLen();
-    7eb6:	7c d0       	rcall	.+248    	; 0x7fb0 <getLen>
-
-      // Immediately start page erase - this will 4.5ms
-      boot_page_erase((uint16_t)(void*)address);
-    7eb8:	e0 91 00 02 	lds	r30, 0x0200
-    7ebc:	f0 91 01 02 	lds	r31, 0x0201
-    7ec0:	83 e0       	ldi	r24, 0x03	; 3
-    7ec2:	80 93 57 00 	sts	0x0057, r24
+    7eb0:	87 d0       	rcall	.+270    	; 0x7fc0 <getLen>
+
+      // If we are in RWW section, immediately start page erase
+      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    7eb2:	e0 91 00 02 	lds	r30, 0x0200
+    7eb6:	f0 91 01 02 	lds	r31, 0x0201
+    7eba:	80 e7       	ldi	r24, 0x70	; 112
+    7ebc:	e0 30       	cpi	r30, 0x00	; 0
+    7ebe:	f8 07       	cpc	r31, r24
+    7ec0:	18 f4       	brcc	.+6      	; 0x7ec8 <main+0xc8>
+    7ec2:	83 e0       	ldi	r24, 0x03	; 3
+    7ec4:	87 bf       	out	0x37, r24	; 55
     7ec6:	e8 95       	spm
     7ec8:	c0 e0       	ldi	r28, 0x00	; 0
     7eca:	d1 e0       	ldi	r29, 0x01	; 1
-
+      
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    7ecc:	69 d0       	rcall	.+210    	; 0x7fa0 <getch>
+    7ecc:	71 d0       	rcall	.+226    	; 0x7fb0 <getch>
     7ece:	89 93       	st	Y+, r24
       while (--length);
     7ed0:	80 91 02 02 	lds	r24, 0x0202
@@ -242,279 +262,293 @@ void watchdogReset() {
     7eda:	88 23       	and	r24, r24
     7edc:	b9 f7       	brne	.-18     	; 0x7ecc <main+0xcc>
 
+      // If we are in NRWW section, page erase has to be delayed until now.
+      // Todo: Take RAMPZ into account
+      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    7ede:	e0 91 00 02 	lds	r30, 0x0200
+    7ee2:	f0 91 01 02 	lds	r31, 0x0201
+    7ee6:	80 e7       	ldi	r24, 0x70	; 112
+    7ee8:	e0 30       	cpi	r30, 0x00	; 0
+    7eea:	f8 07       	cpc	r31, r24
+    7eec:	18 f0       	brcs	.+6      	; 0x7ef4 <main+0xf4>
+    7eee:	83 e0       	ldi	r24, 0x03	; 3
+    7ef0:	87 bf       	out	0x37, r24	; 55
+    7ef2:	e8 95       	spm
+
       // Read command terminator, start reply
       verifySpace();
-    7ede:	78 d0       	rcall	.+240    	; 0x7fd0 <verifySpace>
+    7ef4:	75 d0       	rcall	.+234    	; 0x7fe0 <verifySpace>
       
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    7ee0:	07 b6       	in	r0, 0x37	; 55
-    7ee2:	00 fc       	sbrc	r0, 0
-    7ee4:	fd cf       	rjmp	.-6      	; 0x7ee0 <main+0xe0>
+    7ef6:	07 b6       	in	r0, 0x37	; 55
+    7ef8:	00 fc       	sbrc	r0, 0
+    7efa:	fd cf       	rjmp	.-6      	; 0x7ef6 <main+0xf6>
       }
 #endif
 
       // Copy buffer into programming buffer
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    7ee6:	40 91 00 02 	lds	r20, 0x0200
-    7eea:	50 91 01 02 	lds	r21, 0x0201
-    7eee:	a0 e0       	ldi	r26, 0x00	; 0
-    7ef0:	b1 e0       	ldi	r27, 0x01	; 1
+    7efc:	40 91 00 02 	lds	r20, 0x0200
+    7f00:	50 91 01 02 	lds	r21, 0x0201
+    7f04:	a0 e0       	ldi	r26, 0x00	; 0
+    7f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    7ef2:	2c 91       	ld	r18, X
-    7ef4:	30 e0       	ldi	r19, 0x00	; 0
+    7f08:	2c 91       	ld	r18, X
+    7f0a:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    7ef6:	11 96       	adiw	r26, 0x01	; 1
-    7ef8:	8c 91       	ld	r24, X
-    7efa:	11 97       	sbiw	r26, 0x01	; 1
-    7efc:	90 e0       	ldi	r25, 0x00	; 0
-    7efe:	98 2f       	mov	r25, r24
-    7f00:	88 27       	eor	r24, r24
-    7f02:	82 2b       	or	r24, r18
-    7f04:	93 2b       	or	r25, r19
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+    7f0c:	11 96       	adiw	r26, 0x01	; 1
+    7f0e:	8c 91       	ld	r24, X
+    7f10:	11 97       	sbiw	r26, 0x01	; 1
+    7f12:	90 e0       	ldi	r25, 0x00	; 0
+    7f14:	98 2f       	mov	r25, r24
+    7f16:	88 27       	eor	r24, r24
+    7f18:	82 2b       	or	r24, r18
+    7f1a:	93 2b       	or	r25, r19
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    7f06:	12 96       	adiw	r26, 0x02	; 2
+    7f1c:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
-    7f08:	fa 01       	movw	r30, r20
-    7f0a:	0c 01       	movw	r0, r24
-    7f0c:	d0 92 57 00 	sts	0x0057, r13
-    7f10:	e8 95       	spm
-    7f12:	11 24       	eor	r1, r1
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    7f1e:	fa 01       	movw	r30, r20
+    7f20:	0c 01       	movw	r0, r24
+    7f22:	d7 be       	out	0x37, r13	; 55
+    7f24:	e8 95       	spm
+    7f26:	11 24       	eor	r1, r1
         addrPtr += 2;
-    7f14:	4e 5f       	subi	r20, 0xFE	; 254
-    7f16:	5f 4f       	sbci	r21, 0xFF	; 255
+    7f28:	4e 5f       	subi	r20, 0xFE	; 254
+    7f2a:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    7f18:	f1 e0       	ldi	r31, 0x01	; 1
-    7f1a:	a0 38       	cpi	r26, 0x80	; 128
-    7f1c:	bf 07       	cpc	r27, r31
-    7f1e:	49 f7       	brne	.-46     	; 0x7ef2 <main+0xf2>
+    7f2c:	f1 e0       	ldi	r31, 0x01	; 1
+    7f2e:	a0 38       	cpi	r26, 0x80	; 128
+    7f30:	bf 07       	cpc	r27, r31
+    7f32:	51 f7       	brne	.-44     	; 0x7f08 <main+0x108>
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    7f20:	e0 91 00 02 	lds	r30, 0x0200
-    7f24:	f0 91 01 02 	lds	r31, 0x0201
-    7f28:	e0 92 57 00 	sts	0x0057, r14
-    7f2c:	e8 95       	spm
+      __boot_page_write_short((uint16_t)(void*)address);
+    7f34:	e0 91 00 02 	lds	r30, 0x0200
+    7f38:	f0 91 01 02 	lds	r31, 0x0201
+    7f3c:	e7 be       	out	0x37, r14	; 55
+    7f3e:	e8 95       	spm
       boot_spm_busy_wait();
-    7f2e:	07 b6       	in	r0, 0x37	; 55
-    7f30:	00 fc       	sbrc	r0, 0
-    7f32:	fd cf       	rjmp	.-6      	; 0x7f2e <main+0x12e>
+    7f40:	07 b6       	in	r0, 0x37	; 55
+    7f42:	00 fc       	sbrc	r0, 0
+    7f44:	fd cf       	rjmp	.-6      	; 0x7f40 <main+0x140>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    7f34:	f0 92 57 00 	sts	0x0057, r15
-    7f38:	e8 95       	spm
-    7f3a:	27 c0       	rjmp	.+78     	; 0x7f8a <main+0x18a>
+    7f46:	f7 be       	out	0x37, r15	; 55
+    7f48:	e8 95       	spm
+    7f4a:	27 c0       	rjmp	.+78     	; 0x7f9a <main+0x19a>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    7f3c:	84 37       	cpi	r24, 0x74	; 116
-    7f3e:	b9 f4       	brne	.+46     	; 0x7f6e <main+0x16e>
+    7f4c:	84 37       	cpi	r24, 0x74	; 116
+    7f4e:	b9 f4       	brne	.+46     	; 0x7f7e <main+0x17e>
       // READ PAGE - we only read flash
       getLen();
-    7f40:	37 d0       	rcall	.+110    	; 0x7fb0 <getLen>
+    7f50:	37 d0       	rcall	.+110    	; 0x7fc0 <getLen>
       verifySpace();
-    7f42:	46 d0       	rcall	.+140    	; 0x7fd0 <verifySpace>
-        else ch = pgm_read_byte_near(address);
+    7f52:	46 d0       	rcall	.+140    	; 0x7fe0 <verifySpace>
+        putch(result);
         address++;
-        putch(ch);
-      } while (--length);
+      }
+      while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    7f44:	e0 91 00 02 	lds	r30, 0x0200
-    7f48:	f0 91 01 02 	lds	r31, 0x0201
-    7f4c:	31 96       	adiw	r30, 0x01	; 1
-    7f4e:	f0 93 01 02 	sts	0x0201, r31
-    7f52:	e0 93 00 02 	sts	0x0200, r30
-    7f56:	31 97       	sbiw	r30, 0x01	; 1
-    7f58:	e4 91       	lpm	r30, Z+
-    7f5a:	8e 2f       	mov	r24, r30
-    7f5c:	19 d0       	rcall	.+50     	; 0x7f90 <putch>
+    7f54:	e0 91 00 02 	lds	r30, 0x0200
+    7f58:	f0 91 01 02 	lds	r31, 0x0201
+    7f5c:	31 96       	adiw	r30, 0x01	; 1
+    7f5e:	f0 93 01 02 	sts	0x0201, r31
+    7f62:	e0 93 00 02 	sts	0x0200, r30
+    7f66:	31 97       	sbiw	r30, 0x01	; 1
+    7f68:	e4 91       	lpm	r30, Z+
+    7f6a:	8e 2f       	mov	r24, r30
+    7f6c:	19 d0       	rcall	.+50     	; 0x7fa0 <putch>
       while (--length);
-    7f5e:	80 91 02 02 	lds	r24, 0x0202
-    7f62:	81 50       	subi	r24, 0x01	; 1
-    7f64:	80 93 02 02 	sts	0x0202, r24
-    7f68:	88 23       	and	r24, r24
-    7f6a:	61 f7       	brne	.-40     	; 0x7f44 <main+0x144>
-    7f6c:	0e c0       	rjmp	.+28     	; 0x7f8a <main+0x18a>
+    7f6e:	80 91 02 02 	lds	r24, 0x0202
+    7f72:	81 50       	subi	r24, 0x01	; 1
+    7f74:	80 93 02 02 	sts	0x0202, r24
+    7f78:	88 23       	and	r24, r24
+    7f7a:	61 f7       	brne	.-40     	; 0x7f54 <main+0x154>
+    7f7c:	0e c0       	rjmp	.+28     	; 0x7f9a <main+0x19a>
+#endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    7f6e:	85 37       	cpi	r24, 0x75	; 117
-    7f70:	39 f4       	brne	.+14     	; 0x7f80 <main+0x180>
+    7f7e:	85 37       	cpi	r24, 0x75	; 117
+    7f80:	39 f4       	brne	.+14     	; 0x7f90 <main+0x190>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    7f72:	2e d0       	rcall	.+92     	; 0x7fd0 <verifySpace>
+    7f82:	2e d0       	rcall	.+92     	; 0x7fe0 <verifySpace>
       putch(SIGNATURE_0);
-    7f74:	8e e1       	ldi	r24, 0x1E	; 30
-    7f76:	0c d0       	rcall	.+24     	; 0x7f90 <putch>
+    7f84:	8e e1       	ldi	r24, 0x1E	; 30
+    7f86:	0c d0       	rcall	.+24     	; 0x7fa0 <putch>
       putch(SIGNATURE_1);
-    7f78:	85 e9       	ldi	r24, 0x95	; 149
-    7f7a:	0a d0       	rcall	.+20     	; 0x7f90 <putch>
+    7f88:	85 e9       	ldi	r24, 0x95	; 149
+    7f8a:	0a d0       	rcall	.+20     	; 0x7fa0 <putch>
       putch(SIGNATURE_2);
-    7f7c:	8f e0       	ldi	r24, 0x0F	; 15
-    7f7e:	96 cf       	rjmp	.-212    	; 0x7eac <main+0xac>
+    7f8c:	8f e0       	ldi	r24, 0x0F	; 15
+    7f8e:	8b cf       	rjmp	.-234    	; 0x7ea6 <main+0xa6>
     }
     else if (ch == 'Q') {
-    7f80:	81 35       	cpi	r24, 0x51	; 81
-    7f82:	11 f4       	brne	.+4      	; 0x7f88 <main+0x188>
+    7f90:	81 35       	cpi	r24, 0x51	; 81
+    7f92:	11 f4       	brne	.+4      	; 0x7f98 <main+0x198>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    7f84:	88 e0       	ldi	r24, 0x08	; 8
-    7f86:	19 d0       	rcall	.+50     	; 0x7fba <watchdogConfig>
+    7f94:	88 e0       	ldi	r24, 0x08	; 8
+    7f96:	19 d0       	rcall	.+50     	; 0x7fca <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    7f88:	23 d0       	rcall	.+70     	; 0x7fd0 <verifySpace>
+    7f98:	23 d0       	rcall	.+70     	; 0x7fe0 <verifySpace>
     }
     putch(STK_OK);
-    7f8a:	80 e1       	ldi	r24, 0x10	; 16
-    7f8c:	01 d0       	rcall	.+2      	; 0x7f90 <putch>
-    7f8e:	63 cf       	rjmp	.-314    	; 0x7e56 <main+0x56>
+    7f9a:	80 e1       	ldi	r24, 0x10	; 16
+    7f9c:	01 d0       	rcall	.+2      	; 0x7fa0 <putch>
+    7f9e:	5c cf       	rjmp	.-328    	; 0x7e58 <main+0x58>
 
-00007f90 <putch>:
+00007fa0 <putch>:
   }
 }
 
 void putch(char ch) {
-    7f90:	98 2f       	mov	r25, r24
+    7fa0:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    7f92:	80 91 c0 00 	lds	r24, 0x00C0
-    7f96:	85 ff       	sbrs	r24, 5
-    7f98:	fc cf       	rjmp	.-8      	; 0x7f92 <putch+0x2>
+    7fa2:	80 91 c0 00 	lds	r24, 0x00C0
+    7fa6:	85 ff       	sbrs	r24, 5
+    7fa8:	fc cf       	rjmp	.-8      	; 0x7fa2 <putch+0x2>
   UDR0 = ch;
-    7f9a:	90 93 c6 00 	sts	0x00C6, r25
+    7faa:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    7f9e:	08 95       	ret
+    7fae:	08 95       	ret
 
-00007fa0 <getch>:
+00007fb0 <getch>:
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    7fa0:	a8 95       	wdr
+    7fb0:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    7fa2:	80 91 c0 00 	lds	r24, 0x00C0
-    7fa6:	87 ff       	sbrs	r24, 7
-    7fa8:	fc cf       	rjmp	.-8      	; 0x7fa2 <getch+0x2>
+    7fb2:	80 91 c0 00 	lds	r24, 0x00C0
+    7fb6:	87 ff       	sbrs	r24, 7
+    7fb8:	fc cf       	rjmp	.-8      	; 0x7fb2 <getch+0x2>
   ch = UDR0;
-    7faa:	80 91 c6 00 	lds	r24, 0x00C6
-#ifdef LED_DATA_FLASH
+    7fba:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
+#endif
 
   return ch;
 }
-    7fae:	08 95       	ret
+    7fbe:	08 95       	ret
 
-00007fb0 <getLen>:
+00007fc0 <getLen>:
   } while (--count);
 }
 #endif
 
 uint8_t getLen() {
   getch();
-    7fb0:	f7 df       	rcall	.-18     	; 0x7fa0 <getch>
+    7fc0:	f7 df       	rcall	.-18     	; 0x7fb0 <getch>
   length = getch();
-    7fb2:	f6 df       	rcall	.-20     	; 0x7fa0 <getch>
-    7fb4:	80 93 02 02 	sts	0x0202, r24
+    7fc2:	f6 df       	rcall	.-20     	; 0x7fb0 <getch>
+    7fc4:	80 93 02 02 	sts	0x0202, r24
   return getch();
 }
-    7fb8:	f3 cf       	rjmp	.-26     	; 0x7fa0 <getch>
+    7fc8:	f3 cf       	rjmp	.-26     	; 0x7fb0 <getch>
 
-00007fba <watchdogConfig>:
+00007fca <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    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
+    7fca:	e0 e6       	ldi	r30, 0x60	; 96
+    7fcc:	f0 e0       	ldi	r31, 0x00	; 0
+    7fce:	98 e1       	ldi	r25, 0x18	; 24
+    7fd0:	90 83       	st	Z, r25
   WDTCSR = x;
-    7fc2:	80 83       	st	Z, r24
+    7fd2:	80 83       	st	Z, r24
 }
-    7fc4:	08 95       	ret
+    7fd4:	08 95       	ret
 
-00007fc6 <appStart>:
+00007fd6 <appStart>:
 
 void appStart() {
   watchdogConfig(WATCHDOG_OFF);
-    7fc6:	80 e0       	ldi	r24, 0x00	; 0
-    7fc8:	f8 df       	rcall	.-16     	; 0x7fba <watchdogConfig>
+    7fd6:	80 e0       	ldi	r24, 0x00	; 0
+    7fd8:	f8 df       	rcall	.-16     	; 0x7fca <watchdogConfig>
   __asm__ __volatile__ (
-    7fca:	ee 27       	eor	r30, r30
-    7fcc:	ff 27       	eor	r31, r31
-    7fce:	09 94       	ijmp
+    7fda:	ee 27       	eor	r30, r30
+    7fdc:	ff 27       	eor	r31, r31
+    7fde:	09 94       	ijmp
 
-00007fd0 <verifySpace>:
+00007fe0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) appStart();
-    7fd0:	e7 df       	rcall	.-50     	; 0x7fa0 <getch>
-    7fd2:	80 32       	cpi	r24, 0x20	; 32
-    7fd4:	09 f0       	breq	.+2      	; 0x7fd8 <verifySpace+0x8>
-    7fd6:	f7 df       	rcall	.-18     	; 0x7fc6 <appStart>
+    7fe0:	e7 df       	rcall	.-50     	; 0x7fb0 <getch>
+    7fe2:	80 32       	cpi	r24, 0x20	; 32
+    7fe4:	09 f0       	breq	.+2      	; 0x7fe8 <verifySpace+0x8>
+    7fe6:	f7 df       	rcall	.-18     	; 0x7fd6 <appStart>
   putch(STK_INSYNC);
-    7fd8:	84 e1       	ldi	r24, 0x14	; 20
+    7fe8:	84 e1       	ldi	r24, 0x14	; 20
 }
-    7fda:	da cf       	rjmp	.-76     	; 0x7f90 <putch>
+    7fea:	da cf       	rjmp	.-76     	; 0x7fa0 <putch>
+
+00007fec <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    7fdc:	1f 93       	push	r17
-    7fde:	18 2f       	mov	r17, r24
-
-00007fe0 <getNch>:
+    7fec:	1f 93       	push	r17
+    7fee:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    7fe0:	df df       	rcall	.-66     	; 0x7fa0 <getch>
-    7fe2:	11 50       	subi	r17, 0x01	; 1
-    7fe4:	e9 f7       	brne	.-6      	; 0x7fe0 <getNch>
+    7ff0:	df df       	rcall	.-66     	; 0x7fb0 <getch>
+    7ff2:	11 50       	subi	r17, 0x01	; 1
+    7ff4:	e9 f7       	brne	.-6      	; 0x7ff0 <getNch+0x4>
   verifySpace();
-    7fe6:	f4 df       	rcall	.-24     	; 0x7fd0 <verifySpace>
+    7ff6:	f4 df       	rcall	.-24     	; 0x7fe0 <verifySpace>
 }
-    7fe8:	1f 91       	pop	r17
-    7fea:	08 95       	ret
+    7ff8:	1f 91       	pop	r17
+    7ffa:	08 95       	ret
diff --git a/bootloaders/optiboot/optiboot_diecimila.hex b/bootloaders/optiboot/optiboot_diecimila.hex
index 1e93414..2b4a582 100644
--- a/bootloaders/optiboot/optiboot_diecimila.hex
+++ b/bootloaders/optiboot/optiboot_diecimila.hex
@@ -1,33 +1,34 @@
-:103E000085E08093810082E08093C00088E1809308

-:103E1000C10086E08093C20080E18093C40084B733

-:103E200014BE81FFD0D08DE0C8D0259A86E020E373

-:103E30003CEF91E0309385002093840096BBB09BCB

-:103E4000FECF1D9AA8958150A9F7DD24D394A5E053

-:103E5000EA2EF1E1FF2EA4D0813421F481E0BED01E

-:103E600083E024C0823411F484E103C0853419F462

-:103E700085E0B4D08AC08535A1F492D0082F10E037

-:103E800010930102009300028BD090E0982F8827B6

-:103E9000802B912B880F991F909301028093000231

-:103EA00073C0863529F484E099D080E071D06DC06C

-:103EB000843609F043C07CD0E0910002F091010209

-:103EC00083E080935700E895C0E0D1E069D0899302

-:103ED000809102028150809302028823B9F778D042

-:103EE00007B600FCFDCF4091000250910102A0E016

-:103EF000B1E02C9130E011968C91119790E0982FC1

-:103F00008827822B932B1296FA010C01D09257002E

-:103F1000E89511244E5F5F4FF1E0A038BF0749F7E5

-:103F2000E0910002F0910102E0925700E89507B697

-:103F300000FCFDCFF0925700E89527C08437B9F414

-:103F400037D046D0E0910002F09101023196F09313

-:103F50000102E09300023197E4918E2F19D08091F5

-:103F60000202815080930202882361F70EC08537D8

-:103F700039F42ED08EE10CD084E90AD086E096CFB9

-:103F8000813511F488E019D023D080E101D063CFCE

-:103F9000982F8091C00085FFFCCF9093C6000895B4

-:103FA000A8958091C00087FFFCCF8091C60008953E

-:103FB000F7DFF6DF80930202F3CFE0E6F0E098E16E

-:103FC00090838083089580E0F8DFEE27FF2709942F

-:103FD000E7DF803209F0F7DF84E1DACF1F93182F93

-:0C3FE000DFDF1150E9F7F4DF1F910895B6

+:103E0000112484B714BE81FFE6D085E08093810041

+:103E100082E08093C00088E18093C10086E08093B7

+:103E2000C20080E18093C4008EE0CFD0259A86E066

+:103E300020E33CEF91E0309385002093840096BB13

+:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D

+:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020

+:103E6000C5D083E020C0823411F484E103C08534DE

+:103E700019F485E0BBD091C0853581F499D0082F25

+:103E800010E096D090E0982F8827802B912B880FF8

+:103E9000991F90930102809300027EC0863529F419

+:103EA00084E0A4D080E07CD078C0843609F04EC095

+:103EB00087D0E0910002F091010288E3E030F8073A

+:103EC00018F483E087BFE895C0E0D1E071D0899312

+:103ED000809102028150809302028823B9F7E09119

+:103EE0000002F091010288E3E030F80718F083E067

+:103EF00087BFE89575D007B600FCFDCF4091000262

+:103F000050910102A0E0B1E02C9130E011968C912B

+:103F1000119790E0982F8827822B932B1296FA0105

+:103F20000C01D7BEE89511244E5F5F4FF1E0A03839

+:103F3000BF0751F7E0910002F0910102E7BEE8955A

+:103F400007B600FCFDCFF7BEE89527C08437B9F46B

+:103F500037D046D0E0910002F09101023196F09303

+:103F60000102E09300023197E4918E2F19D08091E5

+:103F70000202815080930202882361F70EC08537C8

+:103F800039F42ED08EE10CD084E90AD086E08BCFB4

+:103F9000813511F488E019D023D080E101D05CCFC5

+:103FA000982F8091C00085FFFCCF9093C6000895A4

+:103FB000A8958091C00087FFFCCF8091C60008952E

+:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E

+:103FD00090838083089580E0F8DFEE27FF2709941F

+:103FE000E7DF803209F0F7DF84E1DACF1F93182F83

+:0C3FF000DFDF1150E9F7F4DF1F910895A6

 :0400000300003E00BB

 :00000001FF

diff --git a/bootloaders/optiboot/optiboot_diecimila.lst b/bootloaders/optiboot/optiboot_diecimila.lst
index 1121893..e0a1ecf 100644
--- a/bootloaders/optiboot/optiboot_diecimila.lst
+++ b/bootloaders/optiboot/optiboot_diecimila.lst
@@ -3,237 +3,257 @@ optiboot_diecimila.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001ec  00003e00  00003e00  00000054  2**1
+  0 .text         000001fc  00003e00  00003e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .debug_aranges 00000028  00000000  00000000  00000240  2**0
+  1 .debug_aranges 00000028  00000000  00000000  00000250  2**0
                   CONTENTS, READONLY, DEBUGGING
-  2 .debug_pubnames 0000006a  00000000  00000000  00000268  2**0
+  2 .debug_pubnames 0000006a  00000000  00000000  00000278  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_info   00000269  00000000  00000000  000002d2  2**0
+  3 .debug_info   00000284  00000000  00000000  000002e2  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_abbrev 00000196  00000000  00000000  0000053b  2**0
+  4 .debug_abbrev 000001ae  00000000  00000000  00000566  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_line   000003d3  00000000  00000000  000006d1  2**0
+  5 .debug_line   00000450  00000000  00000000  00000714  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_frame  00000090  00000000  00000000  00000aa4  2**2
+  6 .debug_frame  00000090  00000000  00000000  00000b64  2**2
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_str    00000135  00000000  00000000  00000b34  2**0
+  7 .debug_str    00000141  00000000  00000000  00000bf4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_loc    000001d1  00000000  00000000  00000c69  2**0
+  8 .debug_loc    000001e1  00000000  00000000  00000d35  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_ranges 00000068  00000000  00000000  00000e3a  2**0
+  9 .debug_ranges 00000068  00000000  00000000  00000f16  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00003e00 <main>:
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3e00:	85 e0       	ldi	r24, 0x05	; 5
-    3e02:	80 93 81 00 	sts	0x0081, r24
+    3e00:	11 24       	eor	r1, r1
+#ifdef __AVR_ATmega8__
+  SP=RAMEND;  // This is done by hardware reset
+#endif
+
+  // Adaboot no-wait mod
+  ch = MCUSR;
+    3e02:	84 b7       	in	r24, 0x34	; 52
+  MCUSR = 0;
+    3e04:	14 be       	out	0x34, r1	; 52
+  if (!(ch & _BV(EXTRF))) appStart();
+    3e06:	81 ff       	sbrs	r24, 1
+    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-#endif
-#ifndef SOFT_UART
+    3e0a:	85 e0       	ldi	r24, 0x05	; 5
+    3e0c:	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
   UCSR0A = _BV(U2X0); //Double speed mode USART0
-    3e06:	82 e0       	ldi	r24, 0x02	; 2
-    3e08:	80 93 c0 00 	sts	0x00C0, r24
+    3e10:	82 e0       	ldi	r24, 0x02	; 2
+    3e12:	80 93 c0 00 	sts	0x00C0, r24
   UCSR0B = _BV(RXEN0) | _BV(TXEN0);
-    3e0c:	88 e1       	ldi	r24, 0x18	; 24
-    3e0e:	80 93 c1 00 	sts	0x00C1, r24
+    3e16:	88 e1       	ldi	r24, 0x18	; 24
+    3e18:	80 93 c1 00 	sts	0x00C1, r24
   UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
-    3e12:	86 e0       	ldi	r24, 0x06	; 6
-    3e14:	80 93 c2 00 	sts	0x00C2, r24
+    3e1c:	86 e0       	ldi	r24, 0x06	; 6
+    3e1e:	80 93 c2 00 	sts	0x00C2, r24
   UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    3e18:	80 e1       	ldi	r24, 0x10	; 16
-    3e1a:	80 93 c4 00 	sts	0x00C4, r24
+    3e22:	80 e1       	ldi	r24, 0x10	; 16
+    3e24:	80 93 c4 00 	sts	0x00C4, r24
+#endif
 #endif
-
-  // Adaboot no-wait mod
-  ch = MCUSR;
-    3e1e:	84 b7       	in	r24, 0x34	; 52
-  MCUSR = 0;
-    3e20:	14 be       	out	0x34, r1	; 52
-  if (!(ch & _BV(EXTRF))) appStart();
-    3e22:	81 ff       	sbrs	r24, 1
-    3e24:	d0 d0       	rcall	.+416    	; 0x3fc6 <appStart>
 
   // Set up watchdog to trigger after 500ms
-  watchdogConfig(WATCHDOG_500MS);
-    3e26:	8d e0       	ldi	r24, 0x0D	; 13
-    3e28:	c8 d0       	rcall	.+400    	; 0x3fba <watchdogConfig>
+  watchdogConfig(WATCHDOG_1S);
+    3e28:	8e e0       	ldi	r24, 0x0E	; 14
+    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    3e2a:	25 9a       	sbi	0x04, 5	; 4
-    3e2c:	86 e0       	ldi	r24, 0x06	; 6
+    3e2c:	25 9a       	sbi	0x04, 5	; 4
+    3e2e:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e2e:	20 e3       	ldi	r18, 0x30	; 48
-    3e30:	3c ef       	ldi	r19, 0xFC	; 252
+    3e30:	20 e3       	ldi	r18, 0x30	; 48
+    3e32:	3c ef       	ldi	r19, 0xFC	; 252
     TIFR1 = _BV(TOV1);
-    3e32:	91 e0       	ldi	r25, 0x01	; 1
+    3e34:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e34:	30 93 85 00 	sts	0x0085, r19
-    3e38:	20 93 84 00 	sts	0x0084, r18
+    3e36:	30 93 85 00 	sts	0x0085, r19
+    3e3a:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    3e3c:	96 bb       	out	0x16, r25	; 22
+    3e3e:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    3e3e:	b0 9b       	sbis	0x16, 0	; 22
-    3e40:	fe cf       	rjmp	.-4      	; 0x3e3e <main+0x3e>
+    3e40:	b0 9b       	sbis	0x16, 0	; 22
+    3e42:	fe cf       	rjmp	.-4      	; 0x3e40 <main+0x40>
+#ifdef __AVR_ATmega8__
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
-    3e42:	1d 9a       	sbi	0x03, 5	; 3
+    3e44:	1d 9a       	sbi	0x03, 5	; 3
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3e44:	a8 95       	wdr
-    TCNT1 = -(F_CPU/(1024*16));
-    TIFR1 = _BV(TOV1);
-    while(!(TIFR1 & _BV(TOV1)));
+    3e46:	a8 95       	wdr
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
+#endif
     watchdogReset();
   } while (--count);
-    3e46:	81 50       	subi	r24, 0x01	; 1
-    3e48:	a9 f7       	brne	.-22     	; 0x3e34 <main+0x34>
+    3e48:	81 50       	subi	r24, 0x01	; 1
+    3e4a:	a9 f7       	brne	.-22     	; 0x3e36 <main+0x36>
     /* get character from UART */
     ch = getch();
 
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e4a:	dd 24       	eor	r13, r13
-    3e4c:	d3 94       	inc	r13
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
+    3e4c:	dd 24       	eor	r13, r13
+    3e4e:	d3 94       	inc	r13
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3e4e:	a5 e0       	ldi	r26, 0x05	; 5
-    3e50:	ea 2e       	mov	r14, r26
+      __boot_page_write_short((uint16_t)(void*)address);
+    3e50:	a5 e0       	ldi	r26, 0x05	; 5
+    3e52:	ea 2e       	mov	r14, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3e52:	f1 e1       	ldi	r31, 0x11	; 17
-    3e54:	ff 2e       	mov	r15, r31
+    3e54:	f1 e1       	ldi	r31, 0x11	; 17
+    3e56:	ff 2e       	mov	r15, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e56:	a4 d0       	rcall	.+328    	; 0x3fa0 <getch>
+    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    3e58:	81 34       	cpi	r24, 0x41	; 65
-    3e5a:	21 f4       	brne	.+8      	; 0x3e64 <main+0x64>
+    3e5a:	81 34       	cpi	r24, 0x41	; 65
+    3e5c:	21 f4       	brne	.+8      	; 0x3e66 <main+0x66>
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e5c:	81 e0       	ldi	r24, 0x01	; 1
-    3e5e:	be d0       	rcall	.+380    	; 0x3fdc <verifySpace+0xc>
+    3e5e:	81 e0       	ldi	r24, 0x01	; 1
+    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
       putch(0x03);
-    3e60:	83 e0       	ldi	r24, 0x03	; 3
-    3e62:	24 c0       	rjmp	.+72     	; 0x3eac <main+0xac>
+    3e62:	83 e0       	ldi	r24, 0x03	; 3
+    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
     }
     else if(ch == STK_SET_DEVICE) {
-    3e64:	82 34       	cpi	r24, 0x42	; 66
-    3e66:	11 f4       	brne	.+4      	; 0x3e6c <main+0x6c>
+    3e66:	82 34       	cpi	r24, 0x42	; 66
+    3e68:	11 f4       	brne	.+4      	; 0x3e6e <main+0x6e>
       // SET DEVICE is ignored
       getNch(20);
-    3e68:	84 e1       	ldi	r24, 0x14	; 20
-    3e6a:	03 c0       	rjmp	.+6      	; 0x3e72 <main+0x72>
+    3e6a:	84 e1       	ldi	r24, 0x14	; 20
+    3e6c:	03 c0       	rjmp	.+6      	; 0x3e74 <main+0x74>
     }
     else if(ch == STK_SET_DEVICE_EXT) {
-    3e6c:	85 34       	cpi	r24, 0x45	; 69
-    3e6e:	19 f4       	brne	.+6      	; 0x3e76 <main+0x76>
+    3e6e:	85 34       	cpi	r24, 0x45	; 69
+    3e70:	19 f4       	brne	.+6      	; 0x3e78 <main+0x78>
       // SET DEVICE EXT is ignored
       getNch(5);
-    3e70:	85 e0       	ldi	r24, 0x05	; 5
-    3e72:	b4 d0       	rcall	.+360    	; 0x3fdc <verifySpace+0xc>
-    3e74:	8a c0       	rjmp	.+276    	; 0x3f8a <main+0x18a>
+    3e72:	85 e0       	ldi	r24, 0x05	; 5
+    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
+    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
-    3e76:	85 35       	cpi	r24, 0x55	; 85
-    3e78:	a1 f4       	brne	.+40     	; 0x3ea2 <main+0xa2>
+    3e78:	85 35       	cpi	r24, 0x55	; 85
+    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
       // LOAD ADDRESS
-      address = getch();
-    3e7a:	92 d0       	rcall	.+292    	; 0x3fa0 <getch>
-    3e7c:	08 2f       	mov	r16, r24
-    3e7e:	10 e0       	ldi	r17, 0x00	; 0
-    3e80:	10 93 01 02 	sts	0x0201, r17
-    3e84:	00 93 00 02 	sts	0x0200, r16
-      address = (address & 0xff) | (getch() << 8);
-    3e88:	8b d0       	rcall	.+278    	; 0x3fa0 <getch>
-    3e8a:	90 e0       	ldi	r25, 0x00	; 0
-    3e8c:	98 2f       	mov	r25, r24
-    3e8e:	88 27       	eor	r24, r24
-    3e90:	80 2b       	or	r24, r16
-    3e92:	91 2b       	or	r25, r17
-      address += address; // Convert from word address to byte address
-    3e94:	88 0f       	add	r24, r24
-    3e96:	99 1f       	adc	r25, r25
-    3e98:	90 93 01 02 	sts	0x0201, r25
-    3e9c:	80 93 00 02 	sts	0x0200, r24
-    3ea0:	73 c0       	rjmp	.+230    	; 0x3f88 <main+0x188>
+      uint16_t newAddress;
+      newAddress = getch();
+    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+      newAddress = (newAddress & 0xff) | (getch() << 8);
+    3e7e:	08 2f       	mov	r16, r24
+    3e80:	10 e0       	ldi	r17, 0x00	; 0
+    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
+    3e84:	90 e0       	ldi	r25, 0x00	; 0
+    3e86:	98 2f       	mov	r25, r24
+    3e88:	88 27       	eor	r24, r24
+    3e8a:	80 2b       	or	r24, r16
+    3e8c:	91 2b       	or	r25, r17
+#ifdef RAMPZ
+      // Transfer top bit to RAMPZ
+      RAMPZ = (newAddress & 0x8000) ? 1 : 0;
+#endif
+      newAddress += newAddress; // Convert from word address to byte address
+    3e8e:	88 0f       	add	r24, r24
+    3e90:	99 1f       	adc	r25, r25
+      address = newAddress;
+    3e92:	90 93 01 02 	sts	0x0201, r25
+    3e96:	80 93 00 02 	sts	0x0200, r24
+    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
     }
     else if(ch == STK_UNIVERSAL) {
-    3ea2:	86 35       	cpi	r24, 0x56	; 86
-    3ea4:	29 f4       	brne	.+10     	; 0x3eb0 <main+0xb0>
+    3e9c:	86 35       	cpi	r24, 0x56	; 86
+    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
       // UNIVERSAL command is ignored
       getNch(4);
-    3ea6:	84 e0       	ldi	r24, 0x04	; 4
-    3ea8:	99 d0       	rcall	.+306    	; 0x3fdc <verifySpace+0xc>
+    3ea0:	84 e0       	ldi	r24, 0x04	; 4
+    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
       putch(0x00);
-    3eaa:	80 e0       	ldi	r24, 0x00	; 0
-    3eac:	71 d0       	rcall	.+226    	; 0x3f90 <putch>
-    3eae:	6d c0       	rjmp	.+218    	; 0x3f8a <main+0x18a>
+    3ea4:	80 e0       	ldi	r24, 0x00	; 0
+    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
+    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
     }
-    /* Write memory, length is big endian and is in bytes  */
+    /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3eb0:	84 36       	cpi	r24, 0x64	; 100
-    3eb2:	09 f0       	breq	.+2      	; 0x3eb6 <main+0xb6>
-    3eb4:	43 c0       	rjmp	.+134    	; 0x3f3c <main+0x13c>
+    3eaa:	84 36       	cpi	r24, 0x64	; 100
+    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
+    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
       getLen();
-    3eb6:	7c d0       	rcall	.+248    	; 0x3fb0 <getLen>
-
-      // Immediately start page erase - this will 4.5ms
-      boot_page_erase((uint16_t)(void*)address);
-    3eb8:	e0 91 00 02 	lds	r30, 0x0200
-    3ebc:	f0 91 01 02 	lds	r31, 0x0201
-    3ec0:	83 e0       	ldi	r24, 0x03	; 3
-    3ec2:	80 93 57 00 	sts	0x0057, r24
+    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+
+      // If we are in RWW section, immediately start page erase
+      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3eb2:	e0 91 00 02 	lds	r30, 0x0200
+    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eba:	88 e3       	ldi	r24, 0x38	; 56
+    3ebc:	e0 30       	cpi	r30, 0x00	; 0
+    3ebe:	f8 07       	cpc	r31, r24
+    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
+    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ec4:	87 bf       	out	0x37, r24	; 55
     3ec6:	e8 95       	spm
     3ec8:	c0 e0       	ldi	r28, 0x00	; 0
     3eca:	d1 e0       	ldi	r29, 0x01	; 1
-
+      
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ecc:	69 d0       	rcall	.+210    	; 0x3fa0 <getch>
+    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
     3ece:	89 93       	st	Y+, r24
       while (--length);
     3ed0:	80 91 02 02 	lds	r24, 0x0202
@@ -242,279 +262,293 @@ void watchdogReset() {
     3eda:	88 23       	and	r24, r24
     3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
 
+      // If we are in NRWW section, page erase has to be delayed until now.
+      // Todo: Take RAMPZ into account
+      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3ede:	e0 91 00 02 	lds	r30, 0x0200
+    3ee2:	f0 91 01 02 	lds	r31, 0x0201
+    3ee6:	88 e3       	ldi	r24, 0x38	; 56
+    3ee8:	e0 30       	cpi	r30, 0x00	; 0
+    3eea:	f8 07       	cpc	r31, r24
+    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
+    3eee:	83 e0       	ldi	r24, 0x03	; 3
+    3ef0:	87 bf       	out	0x37, r24	; 55
+    3ef2:	e8 95       	spm
+
       // Read command terminator, start reply
       verifySpace();
-    3ede:	78 d0       	rcall	.+240    	; 0x3fd0 <verifySpace>
+    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
       
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3ee0:	07 b6       	in	r0, 0x37	; 55
-    3ee2:	00 fc       	sbrc	r0, 0
-    3ee4:	fd cf       	rjmp	.-6      	; 0x3ee0 <main+0xe0>
+    3ef6:	07 b6       	in	r0, 0x37	; 55
+    3ef8:	00 fc       	sbrc	r0, 0
+    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
       }
 #endif
 
       // Copy buffer into programming buffer
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3ee6:	40 91 00 02 	lds	r20, 0x0200
-    3eea:	50 91 01 02 	lds	r21, 0x0201
-    3eee:	a0 e0       	ldi	r26, 0x00	; 0
-    3ef0:	b1 e0       	ldi	r27, 0x01	; 1
+    3efc:	40 91 00 02 	lds	r20, 0x0200
+    3f00:	50 91 01 02 	lds	r21, 0x0201
+    3f04:	a0 e0       	ldi	r26, 0x00	; 0
+    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3ef2:	2c 91       	ld	r18, X
-    3ef4:	30 e0       	ldi	r19, 0x00	; 0
+    3f08:	2c 91       	ld	r18, X
+    3f0a:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3ef6:	11 96       	adiw	r26, 0x01	; 1
-    3ef8:	8c 91       	ld	r24, X
-    3efa:	11 97       	sbiw	r26, 0x01	; 1
-    3efc:	90 e0       	ldi	r25, 0x00	; 0
-    3efe:	98 2f       	mov	r25, r24
-    3f00:	88 27       	eor	r24, r24
-    3f02:	82 2b       	or	r24, r18
-    3f04:	93 2b       	or	r25, r19
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+    3f0c:	11 96       	adiw	r26, 0x01	; 1
+    3f0e:	8c 91       	ld	r24, X
+    3f10:	11 97       	sbiw	r26, 0x01	; 1
+    3f12:	90 e0       	ldi	r25, 0x00	; 0
+    3f14:	98 2f       	mov	r25, r24
+    3f16:	88 27       	eor	r24, r24
+    3f18:	82 2b       	or	r24, r18
+    3f1a:	93 2b       	or	r25, r19
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3f06:	12 96       	adiw	r26, 0x02	; 2
+    3f1c:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
-    3f08:	fa 01       	movw	r30, r20
-    3f0a:	0c 01       	movw	r0, r24
-    3f0c:	d0 92 57 00 	sts	0x0057, r13
-    3f10:	e8 95       	spm
-    3f12:	11 24       	eor	r1, r1
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3f1e:	fa 01       	movw	r30, r20
+    3f20:	0c 01       	movw	r0, r24
+    3f22:	d7 be       	out	0x37, r13	; 55
+    3f24:	e8 95       	spm
+    3f26:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3f14:	4e 5f       	subi	r20, 0xFE	; 254
-    3f16:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f28:	4e 5f       	subi	r20, 0xFE	; 254
+    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f18:	f1 e0       	ldi	r31, 0x01	; 1
-    3f1a:	a0 38       	cpi	r26, 0x80	; 128
-    3f1c:	bf 07       	cpc	r27, r31
-    3f1e:	49 f7       	brne	.-46     	; 0x3ef2 <main+0xf2>
+    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
+    3f2e:	a0 38       	cpi	r26, 0x80	; 128
+    3f30:	bf 07       	cpc	r27, r31
+    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3f20:	e0 91 00 02 	lds	r30, 0x0200
-    3f24:	f0 91 01 02 	lds	r31, 0x0201
-    3f28:	e0 92 57 00 	sts	0x0057, r14
-    3f2c:	e8 95       	spm
+      __boot_page_write_short((uint16_t)(void*)address);
+    3f34:	e0 91 00 02 	lds	r30, 0x0200
+    3f38:	f0 91 01 02 	lds	r31, 0x0201
+    3f3c:	e7 be       	out	0x37, r14	; 55
+    3f3e:	e8 95       	spm
       boot_spm_busy_wait();
-    3f2e:	07 b6       	in	r0, 0x37	; 55
-    3f30:	00 fc       	sbrc	r0, 0
-    3f32:	fd cf       	rjmp	.-6      	; 0x3f2e <main+0x12e>
+    3f40:	07 b6       	in	r0, 0x37	; 55
+    3f42:	00 fc       	sbrc	r0, 0
+    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f34:	f0 92 57 00 	sts	0x0057, r15
-    3f38:	e8 95       	spm
-    3f3a:	27 c0       	rjmp	.+78     	; 0x3f8a <main+0x18a>
+    3f46:	f7 be       	out	0x37, r15	; 55
+    3f48:	e8 95       	spm
+    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f3c:	84 37       	cpi	r24, 0x74	; 116
-    3f3e:	b9 f4       	brne	.+46     	; 0x3f6e <main+0x16e>
+    3f4c:	84 37       	cpi	r24, 0x74	; 116
+    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
       // READ PAGE - we only read flash
       getLen();
-    3f40:	37 d0       	rcall	.+110    	; 0x3fb0 <getLen>
+    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
       verifySpace();
-    3f42:	46 d0       	rcall	.+140    	; 0x3fd0 <verifySpace>
-        else ch = pgm_read_byte_near(address);
+    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+        putch(result);
         address++;
-        putch(ch);
-      } while (--length);
+      }
+      while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f44:	e0 91 00 02 	lds	r30, 0x0200
-    3f48:	f0 91 01 02 	lds	r31, 0x0201
-    3f4c:	31 96       	adiw	r30, 0x01	; 1
-    3f4e:	f0 93 01 02 	sts	0x0201, r31
-    3f52:	e0 93 00 02 	sts	0x0200, r30
-    3f56:	31 97       	sbiw	r30, 0x01	; 1
-    3f58:	e4 91       	lpm	r30, Z+
-    3f5a:	8e 2f       	mov	r24, r30
-    3f5c:	19 d0       	rcall	.+50     	; 0x3f90 <putch>
+    3f54:	e0 91 00 02 	lds	r30, 0x0200
+    3f58:	f0 91 01 02 	lds	r31, 0x0201
+    3f5c:	31 96       	adiw	r30, 0x01	; 1
+    3f5e:	f0 93 01 02 	sts	0x0201, r31
+    3f62:	e0 93 00 02 	sts	0x0200, r30
+    3f66:	31 97       	sbiw	r30, 0x01	; 1
+    3f68:	e4 91       	lpm	r30, Z+
+    3f6a:	8e 2f       	mov	r24, r30
+    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
       while (--length);
-    3f5e:	80 91 02 02 	lds	r24, 0x0202
-    3f62:	81 50       	subi	r24, 0x01	; 1
-    3f64:	80 93 02 02 	sts	0x0202, r24
-    3f68:	88 23       	and	r24, r24
-    3f6a:	61 f7       	brne	.-40     	; 0x3f44 <main+0x144>
-    3f6c:	0e c0       	rjmp	.+28     	; 0x3f8a <main+0x18a>
+    3f6e:	80 91 02 02 	lds	r24, 0x0202
+    3f72:	81 50       	subi	r24, 0x01	; 1
+    3f74:	80 93 02 02 	sts	0x0202, r24
+    3f78:	88 23       	and	r24, r24
+    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
+    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+#endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f6e:	85 37       	cpi	r24, 0x75	; 117
-    3f70:	39 f4       	brne	.+14     	; 0x3f80 <main+0x180>
+    3f7e:	85 37       	cpi	r24, 0x75	; 117
+    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f72:	2e d0       	rcall	.+92     	; 0x3fd0 <verifySpace>
+    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
       putch(SIGNATURE_0);
-    3f74:	8e e1       	ldi	r24, 0x1E	; 30
-    3f76:	0c d0       	rcall	.+24     	; 0x3f90 <putch>
+    3f84:	8e e1       	ldi	r24, 0x1E	; 30
+    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
       putch(SIGNATURE_1);
-    3f78:	84 e9       	ldi	r24, 0x94	; 148
-    3f7a:	0a d0       	rcall	.+20     	; 0x3f90 <putch>
+    3f88:	84 e9       	ldi	r24, 0x94	; 148
+    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
       putch(SIGNATURE_2);
-    3f7c:	86 e0       	ldi	r24, 0x06	; 6
-    3f7e:	96 cf       	rjmp	.-212    	; 0x3eac <main+0xac>
+    3f8c:	86 e0       	ldi	r24, 0x06	; 6
+    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
     }
     else if (ch == 'Q') {
-    3f80:	81 35       	cpi	r24, 0x51	; 81
-    3f82:	11 f4       	brne	.+4      	; 0x3f88 <main+0x188>
+    3f90:	81 35       	cpi	r24, 0x51	; 81
+    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f84:	88 e0       	ldi	r24, 0x08	; 8
-    3f86:	19 d0       	rcall	.+50     	; 0x3fba <watchdogConfig>
+    3f94:	88 e0       	ldi	r24, 0x08	; 8
+    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f88:	23 d0       	rcall	.+70     	; 0x3fd0 <verifySpace>
+    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
     }
     putch(STK_OK);
-    3f8a:	80 e1       	ldi	r24, 0x10	; 16
-    3f8c:	01 d0       	rcall	.+2      	; 0x3f90 <putch>
-    3f8e:	63 cf       	rjmp	.-314    	; 0x3e56 <main+0x56>
+    3f9a:	80 e1       	ldi	r24, 0x10	; 16
+    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
+    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
 
-00003f90 <putch>:
+00003fa0 <putch>:
   }
 }
 
 void putch(char ch) {
-    3f90:	98 2f       	mov	r25, r24
+    3fa0:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    3f92:	80 91 c0 00 	lds	r24, 0x00C0
-    3f96:	85 ff       	sbrs	r24, 5
-    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <putch+0x2>
+    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fa6:	85 ff       	sbrs	r24, 5
+    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
   UDR0 = ch;
-    3f9a:	90 93 c6 00 	sts	0x00C6, r25
+    3faa:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3f9e:	08 95       	ret
+    3fae:	08 95       	ret
 
-00003fa0 <getch>:
+00003fb0 <getch>:
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fa0:	a8 95       	wdr
+    3fb0:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fa6:	87 ff       	sbrs	r24, 7
-    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <getch+0x2>
+    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fb6:	87 ff       	sbrs	r24, 7
+    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
   ch = UDR0;
-    3faa:	80 91 c6 00 	lds	r24, 0x00C6
-#ifdef LED_DATA_FLASH
+    3fba:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
+#endif
 
   return ch;
 }
-    3fae:	08 95       	ret
+    3fbe:	08 95       	ret
 
-00003fb0 <getLen>:
+00003fc0 <getLen>:
   } while (--count);
 }
 #endif
 
 uint8_t getLen() {
   getch();
-    3fb0:	f7 df       	rcall	.-18     	; 0x3fa0 <getch>
+    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
   length = getch();
-    3fb2:	f6 df       	rcall	.-20     	; 0x3fa0 <getch>
-    3fb4:	80 93 02 02 	sts	0x0202, r24
+    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
+    3fc4:	80 93 02 02 	sts	0x0202, r24
   return getch();
 }
-    3fb8:	f3 cf       	rjmp	.-26     	; 0x3fa0 <getch>
+    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
 
-00003fba <watchdogConfig>:
+00003fca <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3fba:	e0 e6       	ldi	r30, 0x60	; 96
-    3fbc:	f0 e0       	ldi	r31, 0x00	; 0
-    3fbe:	98 e1       	ldi	r25, 0x18	; 24
-    3fc0:	90 83       	st	Z, r25
+    3fca:	e0 e6       	ldi	r30, 0x60	; 96
+    3fcc:	f0 e0       	ldi	r31, 0x00	; 0
+    3fce:	98 e1       	ldi	r25, 0x18	; 24
+    3fd0:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fc2:	80 83       	st	Z, r24
+    3fd2:	80 83       	st	Z, r24
 }
-    3fc4:	08 95       	ret
+    3fd4:	08 95       	ret
 
-00003fc6 <appStart>:
+00003fd6 <appStart>:
 
 void appStart() {
   watchdogConfig(WATCHDOG_OFF);
-    3fc6:	80 e0       	ldi	r24, 0x00	; 0
-    3fc8:	f8 df       	rcall	.-16     	; 0x3fba <watchdogConfig>
+    3fd6:	80 e0       	ldi	r24, 0x00	; 0
+    3fd8:	f8 df       	rcall	.-16     	; 0x3fca <watchdogConfig>
   __asm__ __volatile__ (
-    3fca:	ee 27       	eor	r30, r30
-    3fcc:	ff 27       	eor	r31, r31
-    3fce:	09 94       	ijmp
+    3fda:	ee 27       	eor	r30, r30
+    3fdc:	ff 27       	eor	r31, r31
+    3fde:	09 94       	ijmp
 
-00003fd0 <verifySpace>:
+00003fe0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) appStart();
-    3fd0:	e7 df       	rcall	.-50     	; 0x3fa0 <getch>
-    3fd2:	80 32       	cpi	r24, 0x20	; 32
-    3fd4:	09 f0       	breq	.+2      	; 0x3fd8 <verifySpace+0x8>
-    3fd6:	f7 df       	rcall	.-18     	; 0x3fc6 <appStart>
+    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
+    3fe2:	80 32       	cpi	r24, 0x20	; 32
+    3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
+    3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
   putch(STK_INSYNC);
-    3fd8:	84 e1       	ldi	r24, 0x14	; 20
+    3fe8:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fda:	da cf       	rjmp	.-76     	; 0x3f90 <putch>
+    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+
+00003fec <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fdc:	1f 93       	push	r17
-    3fde:	18 2f       	mov	r17, r24
-
-00003fe0 <getNch>:
+    3fec:	1f 93       	push	r17
+    3fee:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3fe0:	df df       	rcall	.-66     	; 0x3fa0 <getch>
-    3fe2:	11 50       	subi	r17, 0x01	; 1
-    3fe4:	e9 f7       	brne	.-6      	; 0x3fe0 <getNch>
+    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
+    3ff2:	11 50       	subi	r17, 0x01	; 1
+    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
   verifySpace();
-    3fe6:	f4 df       	rcall	.-24     	; 0x3fd0 <verifySpace>
+    3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
 }
-    3fe8:	1f 91       	pop	r17
-    3fea:	08 95       	ret
+    3ff8:	1f 91       	pop	r17
+    3ffa:	08 95       	ret
diff --git a/bootloaders/optiboot/optiboot_lilypad.hex b/bootloaders/optiboot/optiboot_lilypad.hex
index 9d31a7a..7579286 100644
--- a/bootloaders/optiboot/optiboot_lilypad.hex
+++ b/bootloaders/optiboot/optiboot_lilypad.hex
@@ -1,34 +1,34 @@
-:103E000085E08093810084B714BE81FFE4D08DE00B

-:103E1000DCD0259A519A86E028E13EEF91E030937C

-:103E200085002093840096BBB09BFECF1D9AA89579

-:103E30008150A9F7DD24D394A5E0EA2EF1E1FF2E0D

-:103E4000ABD0813421F481E0D1D083E024C082342E

-:103E500011F484E103C0853419F485E0C7D08AC029

-:103E60008535A1F499D0082F10E01093010200933A

-:103E7000000292D090E0982F8827802B912B880FFA

-:103E8000991F909301028093000273C0863529F434

-:103E900084E0ACD080E071D06DC0843609F043C0BE

-:103EA0008FD0E0910002F091010283E080935700EF

-:103EB000E895C0E0D1E070D08993809102028150F2

-:103EC000809302028823B9F78BD007B600FCFDCFA0

-:103ED0004091000250910102A0E0B1E02C9130E04D

-:103EE00011968C91119790E0982F8827822B932B15

-:103EF0001296FA010C01D0925700E89511244E5FFA

-:103F00005F4FF1E0A038BF0749F7E0910002F09160

-:103F10000102E0925700E89507B600FCFDCFF09251

-:103F20005700E89527C08437B9F44AD059D0E091BA

-:103F30000002F09101023196F0930102E093000239

-:103F40003197E4918E2F19D0809102028150809395

-:103F50000202882361F70EC0853739F441D08EE123

-:103F60000CD084E90AD086E096CF813511F488E040

-:103F70002CD036D080E101D063CF2AE030E08095AC

-:103F8000089410F4599802C0599A000015D014D022

-:103F900086952A95B1F70895A89529E030E04899CB

-:103FA000FECF0AD009D008D08894489908942A9561

-:103FB00011F08795F7CF089598E09A95F1F7089555

-:103FC000EBDFEADF80930202E7CFE0E6F0E098E182

+:103E0000112484B714BE81FFE6D085E08093810041

+:103E100082E08093C00088E18093C10086E08093B7

+:103E2000C20088E08093C4008EE0CFD0259A86E05F

+:103E300028E13EEF91E0309385002093840096BB0B

+:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D

+:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020

+:103E6000C5D083E020C0823411F484E103C08534DE

+:103E700019F485E0BBD091C0853581F499D0082F25

+:103E800010E096D090E0982F8827802B912B880FF8

+:103E9000991F90930102809300027EC0863529F419

+:103EA00084E0A4D080E07CD078C0843609F04EC095

+:103EB00087D0E0910002F091010288E3E030F8073A

+:103EC00018F483E087BFE895C0E0D1E071D0899312

+:103ED000809102028150809302028823B9F7E09119

+:103EE0000002F091010288E3E030F80718F083E067

+:103EF00087BFE89575D007B600FCFDCF4091000262

+:103F000050910102A0E0B1E02C9130E011968C912B

+:103F1000119790E0982F8827822B932B1296FA0105

+:103F20000C01D7BEE89511244E5F5F4FF1E0A03839

+:103F3000BF0751F7E0910002F0910102E7BEE8955A

+:103F400007B600FCFDCFF7BEE89527C08437B9F46B

+:103F500037D046D0E0910002F09101023196F09303

+:103F60000102E09300023197E4918E2F19D08091E5

+:103F70000202815080930202882361F70EC08537C8

+:103F800039F42ED08EE10CD084E90AD086E08BCFB4

+:103F9000813511F488E019D023D080E101D05CCFC5

+:103FA000982F8091C00085FFFCCF9093C6000895A4

+:103FB000A8958091C00087FFFCCF8091C60008952E

+:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E

 :103FD00090838083089580E0F8DFEE27FF2709941F

-:103FE000DBDF803209F0F7DF84E1C7CF1F93182FA2

-:0C3FF000D3DF1150E9F7F4DF1F910895B2

+:103FE000E7DF803209F0F7DF84E1DACF1F93182F83

+:0C3FF000DFDF1150E9F7F4DF1F910895A6

 :0400000300003E00BB

 :00000001FF

diff --git a/bootloaders/optiboot/optiboot_lilypad.lst b/bootloaders/optiboot/optiboot_lilypad.lst
index 0e46bd1..cbae268 100644
--- a/bootloaders/optiboot/optiboot_lilypad.lst
+++ b/bootloaders/optiboot/optiboot_lilypad.lst
@@ -7,452 +7,473 @@ Idx Name          Size      VMA       LMA       File off  Algn
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
   1 .debug_aranges 00000028  00000000  00000000  00000250  2**0
                   CONTENTS, READONLY, DEBUGGING
-  2 .debug_pubnames 00000078  00000000  00000000  00000278  2**0
+  2 .debug_pubnames 0000006a  00000000  00000000  00000278  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_info   00000277  00000000  00000000  000002f0  2**0
+  3 .debug_info   00000284  00000000  00000000  000002e2  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_abbrev 00000194  00000000  00000000  00000567  2**0
+  4 .debug_abbrev 000001ae  00000000  00000000  00000566  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_line   000003bb  00000000  00000000  000006fb  2**0
+  5 .debug_line   00000450  00000000  00000000  00000714  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_frame  000000a0  00000000  00000000  00000ab8  2**2
+  6 .debug_frame  00000090  00000000  00000000  00000b64  2**2
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_str    0000013f  00000000  00000000  00000b58  2**0
+  7 .debug_str    00000141  00000000  00000000  00000bf4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_loc    000001a0  00000000  00000000  00000c97  2**0
+  8 .debug_loc    000001e1  00000000  00000000  00000d35  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_ranges 00000070  00000000  00000000  00000e37  2**0
+  9 .debug_ranges 00000068  00000000  00000000  00000f16  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00003e00 <main>:
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3e00:	85 e0       	ldi	r24, 0x05	; 5
-    3e02:	80 93 81 00 	sts	0x0081, r24
-  UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
-  UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
+    3e00:	11 24       	eor	r1, r1
+#ifdef __AVR_ATmega8__
+  SP=RAMEND;  // This is done by hardware reset
 #endif
 
   // Adaboot no-wait mod
   ch = MCUSR;
-    3e06:	84 b7       	in	r24, 0x34	; 52
+    3e02:	84 b7       	in	r24, 0x34	; 52
   MCUSR = 0;
-    3e08:	14 be       	out	0x34, r1	; 52
+    3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
-    3e0a:	81 ff       	sbrs	r24, 1
-    3e0c:	e4 d0       	rcall	.+456    	; 0x3fd6 <appStart>
+    3e06:	81 ff       	sbrs	r24, 1
+    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <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
+  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
+  UCSR0A = _BV(U2X0); //Double speed mode USART0
+    3e10:	82 e0       	ldi	r24, 0x02	; 2
+    3e12:	80 93 c0 00 	sts	0x00C0, r24
+  UCSR0B = _BV(RXEN0) | _BV(TXEN0);
+    3e16:	88 e1       	ldi	r24, 0x18	; 24
+    3e18:	80 93 c1 00 	sts	0x00C1, r24
+  UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
+    3e1c:	86 e0       	ldi	r24, 0x06	; 6
+    3e1e:	80 93 c2 00 	sts	0x00C2, r24
+  UBRR0L = (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
+#endif
+#endif
 
   // Set up watchdog to trigger after 500ms
-  watchdogConfig(WATCHDOG_500MS);
-    3e0e:	8d e0       	ldi	r24, 0x0D	; 13
-    3e10:	dc d0       	rcall	.+440    	; 0x3fca <watchdogConfig>
+  watchdogConfig(WATCHDOG_1S);
+    3e28:	8e e0       	ldi	r24, 0x0E	; 14
+    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    3e12:	25 9a       	sbi	0x04, 5	; 4
-
-#ifdef SOFT_UART
-  /* Set TX pin as output */
-  UART_DDR |= _BV(UART_TX_BIT);
-    3e14:	51 9a       	sbi	0x0a, 1	; 10
-    3e16:	86 e0       	ldi	r24, 0x06	; 6
+    3e2c:	25 9a       	sbi	0x04, 5	; 4
+    3e2e:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e18:	28 e1       	ldi	r18, 0x18	; 24
-    3e1a:	3e ef       	ldi	r19, 0xFE	; 254
+    3e30:	28 e1       	ldi	r18, 0x18	; 24
+    3e32:	3e ef       	ldi	r19, 0xFE	; 254
     TIFR1 = _BV(TOV1);
-    3e1c:	91 e0       	ldi	r25, 0x01	; 1
+    3e34:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e1e:	30 93 85 00 	sts	0x0085, r19
-    3e22:	20 93 84 00 	sts	0x0084, r18
+    3e36:	30 93 85 00 	sts	0x0085, r19
+    3e3a:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    3e26:	96 bb       	out	0x16, r25	; 22
+    3e3e:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    3e28:	b0 9b       	sbis	0x16, 0	; 22
-    3e2a:	fe cf       	rjmp	.-4      	; 0x3e28 <main+0x28>
+    3e40:	b0 9b       	sbis	0x16, 0	; 22
+    3e42:	fe cf       	rjmp	.-4      	; 0x3e40 <main+0x40>
+#ifdef __AVR_ATmega8__
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
-    3e2c:	1d 9a       	sbi	0x03, 5	; 3
+    3e44:	1d 9a       	sbi	0x03, 5	; 3
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3e2e:	a8 95       	wdr
-    TCNT1 = -(F_CPU/(1024*16));
-    TIFR1 = _BV(TOV1);
-    while(!(TIFR1 & _BV(TOV1)));
+    3e46:	a8 95       	wdr
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
+#endif
     watchdogReset();
   } while (--count);
-    3e30:	81 50       	subi	r24, 0x01	; 1
-    3e32:	a9 f7       	brne	.-22     	; 0x3e1e <main+0x1e>
+    3e48:	81 50       	subi	r24, 0x01	; 1
+    3e4a:	a9 f7       	brne	.-22     	; 0x3e36 <main+0x36>
     /* get character from UART */
     ch = getch();
 
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e34:	dd 24       	eor	r13, r13
-    3e36:	d3 94       	inc	r13
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
+    3e4c:	dd 24       	eor	r13, r13
+    3e4e:	d3 94       	inc	r13
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3e38:	a5 e0       	ldi	r26, 0x05	; 5
-    3e3a:	ea 2e       	mov	r14, r26
+      __boot_page_write_short((uint16_t)(void*)address);
+    3e50:	a5 e0       	ldi	r26, 0x05	; 5
+    3e52:	ea 2e       	mov	r14, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3e3c:	f1 e1       	ldi	r31, 0x11	; 17
-    3e3e:	ff 2e       	mov	r15, r31
+    3e54:	f1 e1       	ldi	r31, 0x11	; 17
+    3e56:	ff 2e       	mov	r15, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e40:	ab d0       	rcall	.+342    	; 0x3f98 <getch>
+    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    3e42:	81 34       	cpi	r24, 0x41	; 65
-    3e44:	21 f4       	brne	.+8      	; 0x3e4e <main+0x4e>
+    3e5a:	81 34       	cpi	r24, 0x41	; 65
+    3e5c:	21 f4       	brne	.+8      	; 0x3e66 <main+0x66>
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e46:	81 e0       	ldi	r24, 0x01	; 1
-    3e48:	d1 d0       	rcall	.+418    	; 0x3fec <verifySpace+0xc>
+    3e5e:	81 e0       	ldi	r24, 0x01	; 1
+    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
       putch(0x03);
-    3e4a:	83 e0       	ldi	r24, 0x03	; 3
-    3e4c:	24 c0       	rjmp	.+72     	; 0x3e96 <main+0x96>
+    3e62:	83 e0       	ldi	r24, 0x03	; 3
+    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
     }
     else if(ch == STK_SET_DEVICE) {
-    3e4e:	82 34       	cpi	r24, 0x42	; 66
-    3e50:	11 f4       	brne	.+4      	; 0x3e56 <main+0x56>
+    3e66:	82 34       	cpi	r24, 0x42	; 66
+    3e68:	11 f4       	brne	.+4      	; 0x3e6e <main+0x6e>
       // SET DEVICE is ignored
       getNch(20);
-    3e52:	84 e1       	ldi	r24, 0x14	; 20
-    3e54:	03 c0       	rjmp	.+6      	; 0x3e5c <main+0x5c>
+    3e6a:	84 e1       	ldi	r24, 0x14	; 20
+    3e6c:	03 c0       	rjmp	.+6      	; 0x3e74 <main+0x74>
     }
     else if(ch == STK_SET_DEVICE_EXT) {
-    3e56:	85 34       	cpi	r24, 0x45	; 69
-    3e58:	19 f4       	brne	.+6      	; 0x3e60 <main+0x60>
+    3e6e:	85 34       	cpi	r24, 0x45	; 69
+    3e70:	19 f4       	brne	.+6      	; 0x3e78 <main+0x78>
       // SET DEVICE EXT is ignored
       getNch(5);
-    3e5a:	85 e0       	ldi	r24, 0x05	; 5
-    3e5c:	c7 d0       	rcall	.+398    	; 0x3fec <verifySpace+0xc>
-    3e5e:	8a c0       	rjmp	.+276    	; 0x3f74 <main+0x174>
+    3e72:	85 e0       	ldi	r24, 0x05	; 5
+    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
+    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
-    3e60:	85 35       	cpi	r24, 0x55	; 85
-    3e62:	a1 f4       	brne	.+40     	; 0x3e8c <main+0x8c>
+    3e78:	85 35       	cpi	r24, 0x55	; 85
+    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
       // LOAD ADDRESS
-      address = getch();
-    3e64:	99 d0       	rcall	.+306    	; 0x3f98 <getch>
-    3e66:	08 2f       	mov	r16, r24
-    3e68:	10 e0       	ldi	r17, 0x00	; 0
-    3e6a:	10 93 01 02 	sts	0x0201, r17
-    3e6e:	00 93 00 02 	sts	0x0200, r16
-      address = (address & 0xff) | (getch() << 8);
-    3e72:	92 d0       	rcall	.+292    	; 0x3f98 <getch>
-    3e74:	90 e0       	ldi	r25, 0x00	; 0
-    3e76:	98 2f       	mov	r25, r24
-    3e78:	88 27       	eor	r24, r24
-    3e7a:	80 2b       	or	r24, r16
-    3e7c:	91 2b       	or	r25, r17
-      address += address; // Convert from word address to byte address
-    3e7e:	88 0f       	add	r24, r24
-    3e80:	99 1f       	adc	r25, r25
-    3e82:	90 93 01 02 	sts	0x0201, r25
-    3e86:	80 93 00 02 	sts	0x0200, r24
-    3e8a:	73 c0       	rjmp	.+230    	; 0x3f72 <main+0x172>
+      uint16_t newAddress;
+      newAddress = getch();
+    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+      newAddress = (newAddress & 0xff) | (getch() << 8);
+    3e7e:	08 2f       	mov	r16, r24
+    3e80:	10 e0       	ldi	r17, 0x00	; 0
+    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
+    3e84:	90 e0       	ldi	r25, 0x00	; 0
+    3e86:	98 2f       	mov	r25, r24
+    3e88:	88 27       	eor	r24, r24
+    3e8a:	80 2b       	or	r24, r16
+    3e8c:	91 2b       	or	r25, r17
+#ifdef RAMPZ
+      // Transfer top bit to RAMPZ
+      RAMPZ = (newAddress & 0x8000) ? 1 : 0;
+#endif
+      newAddress += newAddress; // Convert from word address to byte address
+    3e8e:	88 0f       	add	r24, r24
+    3e90:	99 1f       	adc	r25, r25
+      address = newAddress;
+    3e92:	90 93 01 02 	sts	0x0201, r25
+    3e96:	80 93 00 02 	sts	0x0200, r24
+    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
     }
     else if(ch == STK_UNIVERSAL) {
-    3e8c:	86 35       	cpi	r24, 0x56	; 86
-    3e8e:	29 f4       	brne	.+10     	; 0x3e9a <main+0x9a>
+    3e9c:	86 35       	cpi	r24, 0x56	; 86
+    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
       // UNIVERSAL command is ignored
       getNch(4);
-    3e90:	84 e0       	ldi	r24, 0x04	; 4
-    3e92:	ac d0       	rcall	.+344    	; 0x3fec <verifySpace+0xc>
+    3ea0:	84 e0       	ldi	r24, 0x04	; 4
+    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
       putch(0x00);
-    3e94:	80 e0       	ldi	r24, 0x00	; 0
-    3e96:	71 d0       	rcall	.+226    	; 0x3f7a <putch>
-    3e98:	6d c0       	rjmp	.+218    	; 0x3f74 <main+0x174>
+    3ea4:	80 e0       	ldi	r24, 0x00	; 0
+    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
+    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
     }
-    /* Write memory, length is big endian and is in bytes  */
+    /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3e9a:	84 36       	cpi	r24, 0x64	; 100
-    3e9c:	09 f0       	breq	.+2      	; 0x3ea0 <main+0xa0>
-    3e9e:	43 c0       	rjmp	.+134    	; 0x3f26 <main+0x126>
+    3eaa:	84 36       	cpi	r24, 0x64	; 100
+    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
+    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
       getLen();
-    3ea0:	8f d0       	rcall	.+286    	; 0x3fc0 <getLen>
-
-      // Immediately start page erase - this will 4.5ms
-      boot_page_erase((uint16_t)(void*)address);
-    3ea2:	e0 91 00 02 	lds	r30, 0x0200
-    3ea6:	f0 91 01 02 	lds	r31, 0x0201
-    3eaa:	83 e0       	ldi	r24, 0x03	; 3
-    3eac:	80 93 57 00 	sts	0x0057, r24
-    3eb0:	e8 95       	spm
-    3eb2:	c0 e0       	ldi	r28, 0x00	; 0
-    3eb4:	d1 e0       	ldi	r29, 0x01	; 1
-
+    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+
+      // If we are in RWW section, immediately start page erase
+      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3eb2:	e0 91 00 02 	lds	r30, 0x0200
+    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eba:	88 e3       	ldi	r24, 0x38	; 56
+    3ebc:	e0 30       	cpi	r30, 0x00	; 0
+    3ebe:	f8 07       	cpc	r31, r24
+    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
+    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ec4:	87 bf       	out	0x37, r24	; 55
+    3ec6:	e8 95       	spm
+    3ec8:	c0 e0       	ldi	r28, 0x00	; 0
+    3eca:	d1 e0       	ldi	r29, 0x01	; 1
+      
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3eb6:	70 d0       	rcall	.+224    	; 0x3f98 <getch>
-    3eb8:	89 93       	st	Y+, r24
+    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
+    3ece:	89 93       	st	Y+, r24
       while (--length);
-    3eba:	80 91 02 02 	lds	r24, 0x0202
-    3ebe:	81 50       	subi	r24, 0x01	; 1
-    3ec0:	80 93 02 02 	sts	0x0202, r24
-    3ec4:	88 23       	and	r24, r24
-    3ec6:	b9 f7       	brne	.-18     	; 0x3eb6 <main+0xb6>
+    3ed0:	80 91 02 02 	lds	r24, 0x0202
+    3ed4:	81 50       	subi	r24, 0x01	; 1
+    3ed6:	80 93 02 02 	sts	0x0202, r24
+    3eda:	88 23       	and	r24, r24
+    3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
+
+      // If we are in NRWW section, page erase has to be delayed until now.
+      // Todo: Take RAMPZ into account
+      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3ede:	e0 91 00 02 	lds	r30, 0x0200
+    3ee2:	f0 91 01 02 	lds	r31, 0x0201
+    3ee6:	88 e3       	ldi	r24, 0x38	; 56
+    3ee8:	e0 30       	cpi	r30, 0x00	; 0
+    3eea:	f8 07       	cpc	r31, r24
+    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
+    3eee:	83 e0       	ldi	r24, 0x03	; 3
+    3ef0:	87 bf       	out	0x37, r24	; 55
+    3ef2:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    3ec8:	8b d0       	rcall	.+278    	; 0x3fe0 <verifySpace>
+    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
       
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3eca:	07 b6       	in	r0, 0x37	; 55
-    3ecc:	00 fc       	sbrc	r0, 0
-    3ece:	fd cf       	rjmp	.-6      	; 0x3eca <main+0xca>
+    3ef6:	07 b6       	in	r0, 0x37	; 55
+    3ef8:	00 fc       	sbrc	r0, 0
+    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
       }
 #endif
 
       // Copy buffer into programming buffer
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3ed0:	40 91 00 02 	lds	r20, 0x0200
-    3ed4:	50 91 01 02 	lds	r21, 0x0201
-    3ed8:	a0 e0       	ldi	r26, 0x00	; 0
-    3eda:	b1 e0       	ldi	r27, 0x01	; 1
+    3efc:	40 91 00 02 	lds	r20, 0x0200
+    3f00:	50 91 01 02 	lds	r21, 0x0201
+    3f04:	a0 e0       	ldi	r26, 0x00	; 0
+    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3edc:	2c 91       	ld	r18, X
-    3ede:	30 e0       	ldi	r19, 0x00	; 0
+    3f08:	2c 91       	ld	r18, X
+    3f0a:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3ee0:	11 96       	adiw	r26, 0x01	; 1
-    3ee2:	8c 91       	ld	r24, X
-    3ee4:	11 97       	sbiw	r26, 0x01	; 1
-    3ee6:	90 e0       	ldi	r25, 0x00	; 0
-    3ee8:	98 2f       	mov	r25, r24
-    3eea:	88 27       	eor	r24, r24
-    3eec:	82 2b       	or	r24, r18
-    3eee:	93 2b       	or	r25, r19
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+    3f0c:	11 96       	adiw	r26, 0x01	; 1
+    3f0e:	8c 91       	ld	r24, X
+    3f10:	11 97       	sbiw	r26, 0x01	; 1
+    3f12:	90 e0       	ldi	r25, 0x00	; 0
+    3f14:	98 2f       	mov	r25, r24
+    3f16:	88 27       	eor	r24, r24
+    3f18:	82 2b       	or	r24, r18
+    3f1a:	93 2b       	or	r25, r19
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3ef0:	12 96       	adiw	r26, 0x02	; 2
+    3f1c:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
-    3ef2:	fa 01       	movw	r30, r20
-    3ef4:	0c 01       	movw	r0, r24
-    3ef6:	d0 92 57 00 	sts	0x0057, r13
-    3efa:	e8 95       	spm
-    3efc:	11 24       	eor	r1, r1
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3f1e:	fa 01       	movw	r30, r20
+    3f20:	0c 01       	movw	r0, r24
+    3f22:	d7 be       	out	0x37, r13	; 55
+    3f24:	e8 95       	spm
+    3f26:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3efe:	4e 5f       	subi	r20, 0xFE	; 254
-    3f00:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f28:	4e 5f       	subi	r20, 0xFE	; 254
+    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f02:	f1 e0       	ldi	r31, 0x01	; 1
-    3f04:	a0 38       	cpi	r26, 0x80	; 128
-    3f06:	bf 07       	cpc	r27, r31
-    3f08:	49 f7       	brne	.-46     	; 0x3edc <main+0xdc>
+    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
+    3f2e:	a0 38       	cpi	r26, 0x80	; 128
+    3f30:	bf 07       	cpc	r27, r31
+    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3f0a:	e0 91 00 02 	lds	r30, 0x0200
-    3f0e:	f0 91 01 02 	lds	r31, 0x0201
-    3f12:	e0 92 57 00 	sts	0x0057, r14
-    3f16:	e8 95       	spm
+      __boot_page_write_short((uint16_t)(void*)address);
+    3f34:	e0 91 00 02 	lds	r30, 0x0200
+    3f38:	f0 91 01 02 	lds	r31, 0x0201
+    3f3c:	e7 be       	out	0x37, r14	; 55
+    3f3e:	e8 95       	spm
       boot_spm_busy_wait();
-    3f18:	07 b6       	in	r0, 0x37	; 55
-    3f1a:	00 fc       	sbrc	r0, 0
-    3f1c:	fd cf       	rjmp	.-6      	; 0x3f18 <main+0x118>
+    3f40:	07 b6       	in	r0, 0x37	; 55
+    3f42:	00 fc       	sbrc	r0, 0
+    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f1e:	f0 92 57 00 	sts	0x0057, r15
-    3f22:	e8 95       	spm
-    3f24:	27 c0       	rjmp	.+78     	; 0x3f74 <main+0x174>
+    3f46:	f7 be       	out	0x37, r15	; 55
+    3f48:	e8 95       	spm
+    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f26:	84 37       	cpi	r24, 0x74	; 116
-    3f28:	b9 f4       	brne	.+46     	; 0x3f58 <main+0x158>
+    3f4c:	84 37       	cpi	r24, 0x74	; 116
+    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
       // READ PAGE - we only read flash
       getLen();
-    3f2a:	4a d0       	rcall	.+148    	; 0x3fc0 <getLen>
+    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
       verifySpace();
-    3f2c:	59 d0       	rcall	.+178    	; 0x3fe0 <verifySpace>
-        else ch = pgm_read_byte_near(address);
+    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+        putch(result);
         address++;
-        putch(ch);
-      } while (--length);
+      }
+      while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f2e:	e0 91 00 02 	lds	r30, 0x0200
-    3f32:	f0 91 01 02 	lds	r31, 0x0201
-    3f36:	31 96       	adiw	r30, 0x01	; 1
-    3f38:	f0 93 01 02 	sts	0x0201, r31
-    3f3c:	e0 93 00 02 	sts	0x0200, r30
-    3f40:	31 97       	sbiw	r30, 0x01	; 1
-    3f42:	e4 91       	lpm	r30, Z+
-    3f44:	8e 2f       	mov	r24, r30
-    3f46:	19 d0       	rcall	.+50     	; 0x3f7a <putch>
+    3f54:	e0 91 00 02 	lds	r30, 0x0200
+    3f58:	f0 91 01 02 	lds	r31, 0x0201
+    3f5c:	31 96       	adiw	r30, 0x01	; 1
+    3f5e:	f0 93 01 02 	sts	0x0201, r31
+    3f62:	e0 93 00 02 	sts	0x0200, r30
+    3f66:	31 97       	sbiw	r30, 0x01	; 1
+    3f68:	e4 91       	lpm	r30, Z+
+    3f6a:	8e 2f       	mov	r24, r30
+    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
       while (--length);
-    3f48:	80 91 02 02 	lds	r24, 0x0202
-    3f4c:	81 50       	subi	r24, 0x01	; 1
-    3f4e:	80 93 02 02 	sts	0x0202, r24
-    3f52:	88 23       	and	r24, r24
-    3f54:	61 f7       	brne	.-40     	; 0x3f2e <main+0x12e>
-    3f56:	0e c0       	rjmp	.+28     	; 0x3f74 <main+0x174>
+    3f6e:	80 91 02 02 	lds	r24, 0x0202
+    3f72:	81 50       	subi	r24, 0x01	; 1
+    3f74:	80 93 02 02 	sts	0x0202, r24
+    3f78:	88 23       	and	r24, r24
+    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
+    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+#endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f58:	85 37       	cpi	r24, 0x75	; 117
-    3f5a:	39 f4       	brne	.+14     	; 0x3f6a <main+0x16a>
+    3f7e:	85 37       	cpi	r24, 0x75	; 117
+    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f5c:	41 d0       	rcall	.+130    	; 0x3fe0 <verifySpace>
+    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
       putch(SIGNATURE_0);
-    3f5e:	8e e1       	ldi	r24, 0x1E	; 30
-    3f60:	0c d0       	rcall	.+24     	; 0x3f7a <putch>
+    3f84:	8e e1       	ldi	r24, 0x1E	; 30
+    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
       putch(SIGNATURE_1);
-    3f62:	84 e9       	ldi	r24, 0x94	; 148
-    3f64:	0a d0       	rcall	.+20     	; 0x3f7a <putch>
+    3f88:	84 e9       	ldi	r24, 0x94	; 148
+    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
       putch(SIGNATURE_2);
-    3f66:	86 e0       	ldi	r24, 0x06	; 6
-    3f68:	96 cf       	rjmp	.-212    	; 0x3e96 <main+0x96>
+    3f8c:	86 e0       	ldi	r24, 0x06	; 6
+    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
     }
     else if (ch == 'Q') {
-    3f6a:	81 35       	cpi	r24, 0x51	; 81
-    3f6c:	11 f4       	brne	.+4      	; 0x3f72 <main+0x172>
+    3f90:	81 35       	cpi	r24, 0x51	; 81
+    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f6e:	88 e0       	ldi	r24, 0x08	; 8
-    3f70:	2c d0       	rcall	.+88     	; 0x3fca <watchdogConfig>
+    3f94:	88 e0       	ldi	r24, 0x08	; 8
+    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f72:	36 d0       	rcall	.+108    	; 0x3fe0 <verifySpace>
+    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
     }
     putch(STK_OK);
-    3f74:	80 e1       	ldi	r24, 0x10	; 16
-    3f76:	01 d0       	rcall	.+2      	; 0x3f7a <putch>
-    3f78:	63 cf       	rjmp	.-314    	; 0x3e40 <main+0x40>
+    3f9a:	80 e1       	ldi	r24, 0x10	; 16
+    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
+    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
+
+00003fa0 <putch>:
+  }
+}
 
-00003f7a <putch>:
 void putch(char ch) {
+    3fa0:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
+    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fa6:	85 ff       	sbrs	r24, 5
+    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
   UDR0 = ch;
-#else
-  __asm__ __volatile__ (
-    3f7a:	2a e0       	ldi	r18, 0x0A	; 10
-    3f7c:	30 e0       	ldi	r19, 0x00	; 0
-    3f7e:	80 95       	com	r24
-    3f80:	08 94       	sec
-    3f82:	10 f4       	brcc	.+4      	; 0x3f88 <putch+0xe>
-    3f84:	59 98       	cbi	0x0b, 1	; 11
-    3f86:	02 c0       	rjmp	.+4      	; 0x3f8c <putch+0x12>
-    3f88:	59 9a       	sbi	0x0b, 1	; 11
-    3f8a:	00 00       	nop
-    3f8c:	15 d0       	rcall	.+42     	; 0x3fb8 <uartDelay>
-    3f8e:	14 d0       	rcall	.+40     	; 0x3fb8 <uartDelay>
-    3f90:	86 95       	lsr	r24
-    3f92:	2a 95       	dec	r18
-    3f94:	b1 f7       	brne	.-20     	; 0x3f82 <putch+0x8>
+    3faa:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3f96:	08 95       	ret
+    3fae:	08 95       	ret
 
-00003f98 <getch>:
+00003fb0 <getch>:
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3f98:	a8 95       	wdr
-#ifdef LED_DATA_FLASH
+    3fb0:	a8 95       	wdr
+      [uartBit] "I" (UART_RX_BIT)
+    :
+      "r25"
+);
+#else
+  while(!(UCSR0A & _BV(RXC0)));
+    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fb6:	87 ff       	sbrs	r24, 7
+    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
+  ch = UDR0;
+    3fba:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
+#endif
 
   return ch;
 }
-    3f9a:	29 e0       	ldi	r18, 0x09	; 9
-    3f9c:	30 e0       	ldi	r19, 0x00	; 0
-    3f9e:	48 99       	sbic	0x09, 0	; 9
-    3fa0:	fe cf       	rjmp	.-4      	; 0x3f9e <getch+0x6>
-    3fa2:	0a d0       	rcall	.+20     	; 0x3fb8 <uartDelay>
-    3fa4:	09 d0       	rcall	.+18     	; 0x3fb8 <uartDelay>
-    3fa6:	08 d0       	rcall	.+16     	; 0x3fb8 <uartDelay>
-    3fa8:	88 94       	clc
-    3faa:	48 99       	sbic	0x09, 0	; 9
-    3fac:	08 94       	sec
-    3fae:	2a 95       	dec	r18
-    3fb0:	11 f0       	breq	.+4      	; 0x3fb6 <getch+0x1e>
-    3fb2:	87 95       	ror	r24
-    3fb4:	f7 cf       	rjmp	.-18     	; 0x3fa4 <getch+0xc>
-    3fb6:	08 95       	ret
-
-00003fb8 <uartDelay>:
-#if UART_B_VALUE > 255
-#error Baud rate too slow for soft UART
-#endif
-
-void uartDelay() {
-  __asm__ __volatile__ (
-    3fb8:	98 e0       	ldi	r25, 0x08	; 8
-    3fba:	9a 95       	dec	r25
-    3fbc:	f1 f7       	brne	.-4      	; 0x3fba <uartDelay+0x2>
     3fbe:	08 95       	ret
 
 00003fc0 <getLen>:
@@ -462,13 +483,13 @@ void uartDelay() {
 
 uint8_t getLen() {
   getch();
-    3fc0:	eb df       	rcall	.-42     	; 0x3f98 <getch>
+    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
   length = getch();
-    3fc2:	ea df       	rcall	.-44     	; 0x3f98 <getch>
+    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
     3fc4:	80 93 02 02 	sts	0x0202, r24
   return getch();
 }
-    3fc8:	e7 cf       	rjmp	.-50     	; 0x3f98 <getch>
+    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
 
 00003fca <watchdogConfig>:
     "wdr\n"
@@ -504,14 +525,16 @@ void appStart() {
 
 void verifySpace() {
   if (getch() != CRC_EOP) appStart();
-    3fe0:	db df       	rcall	.-74     	; 0x3f98 <getch>
+    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
     3fe2:	80 32       	cpi	r24, 0x20	; 32
     3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
     3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
   putch(STK_INSYNC);
     3fe8:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fea:	c7 cf       	rjmp	.-114    	; 0x3f7a <putch>
+    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+
+00003fec <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
@@ -520,12 +543,10 @@ void verifySpace() {
 void getNch(uint8_t count) {
     3fec:	1f 93       	push	r17
     3fee:	18 2f       	mov	r17, r24
-
-00003ff0 <getNch>:
   do getch(); while (--count);
-    3ff0:	d3 df       	rcall	.-90     	; 0x3f98 <getch>
+    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
     3ff2:	11 50       	subi	r17, 0x01	; 1
-    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch>
+    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
   verifySpace();
     3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
 }
diff --git a/bootloaders/optiboot/optiboot_lilypad_resonator.hex b/bootloaders/optiboot/optiboot_lilypad_resonator.hex
index 9d31a7a..7579286 100644
--- a/bootloaders/optiboot/optiboot_lilypad_resonator.hex
+++ b/bootloaders/optiboot/optiboot_lilypad_resonator.hex
@@ -1,34 +1,34 @@
-:103E000085E08093810084B714BE81FFE4D08DE00B

-:103E1000DCD0259A519A86E028E13EEF91E030937C

-:103E200085002093840096BBB09BFECF1D9AA89579

-:103E30008150A9F7DD24D394A5E0EA2EF1E1FF2E0D

-:103E4000ABD0813421F481E0D1D083E024C082342E

-:103E500011F484E103C0853419F485E0C7D08AC029

-:103E60008535A1F499D0082F10E01093010200933A

-:103E7000000292D090E0982F8827802B912B880FFA

-:103E8000991F909301028093000273C0863529F434

-:103E900084E0ACD080E071D06DC0843609F043C0BE

-:103EA0008FD0E0910002F091010283E080935700EF

-:103EB000E895C0E0D1E070D08993809102028150F2

-:103EC000809302028823B9F78BD007B600FCFDCFA0

-:103ED0004091000250910102A0E0B1E02C9130E04D

-:103EE00011968C91119790E0982F8827822B932B15

-:103EF0001296FA010C01D0925700E89511244E5FFA

-:103F00005F4FF1E0A038BF0749F7E0910002F09160

-:103F10000102E0925700E89507B600FCFDCFF09251

-:103F20005700E89527C08437B9F44AD059D0E091BA

-:103F30000002F09101023196F0930102E093000239

-:103F40003197E4918E2F19D0809102028150809395

-:103F50000202882361F70EC0853739F441D08EE123

-:103F60000CD084E90AD086E096CF813511F488E040

-:103F70002CD036D080E101D063CF2AE030E08095AC

-:103F8000089410F4599802C0599A000015D014D022

-:103F900086952A95B1F70895A89529E030E04899CB

-:103FA000FECF0AD009D008D08894489908942A9561

-:103FB00011F08795F7CF089598E09A95F1F7089555

-:103FC000EBDFEADF80930202E7CFE0E6F0E098E182

+:103E0000112484B714BE81FFE6D085E08093810041

+:103E100082E08093C00088E18093C10086E08093B7

+:103E2000C20088E08093C4008EE0CFD0259A86E05F

+:103E300028E13EEF91E0309385002093840096BB0B

+:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D

+:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020

+:103E6000C5D083E020C0823411F484E103C08534DE

+:103E700019F485E0BBD091C0853581F499D0082F25

+:103E800010E096D090E0982F8827802B912B880FF8

+:103E9000991F90930102809300027EC0863529F419

+:103EA00084E0A4D080E07CD078C0843609F04EC095

+:103EB00087D0E0910002F091010288E3E030F8073A

+:103EC00018F483E087BFE895C0E0D1E071D0899312

+:103ED000809102028150809302028823B9F7E09119

+:103EE0000002F091010288E3E030F80718F083E067

+:103EF00087BFE89575D007B600FCFDCF4091000262

+:103F000050910102A0E0B1E02C9130E011968C912B

+:103F1000119790E0982F8827822B932B1296FA0105

+:103F20000C01D7BEE89511244E5F5F4FF1E0A03839

+:103F3000BF0751F7E0910002F0910102E7BEE8955A

+:103F400007B600FCFDCFF7BEE89527C08437B9F46B

+:103F500037D046D0E0910002F09101023196F09303

+:103F60000102E09300023197E4918E2F19D08091E5

+:103F70000202815080930202882361F70EC08537C8

+:103F800039F42ED08EE10CD084E90AD086E08BCFB4

+:103F9000813511F488E019D023D080E101D05CCFC5

+:103FA000982F8091C00085FFFCCF9093C6000895A4

+:103FB000A8958091C00087FFFCCF8091C60008952E

+:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E

 :103FD00090838083089580E0F8DFEE27FF2709941F

-:103FE000DBDF803209F0F7DF84E1C7CF1F93182FA2

-:0C3FF000D3DF1150E9F7F4DF1F910895B2

+:103FE000E7DF803209F0F7DF84E1DACF1F93182F83

+:0C3FF000DFDF1150E9F7F4DF1F910895A6

 :0400000300003E00BB

 :00000001FF

diff --git a/bootloaders/optiboot/optiboot_lilypad_resonator.lst b/bootloaders/optiboot/optiboot_lilypad_resonator.lst
index 80ecb83..c5d7710 100644
--- a/bootloaders/optiboot/optiboot_lilypad_resonator.lst
+++ b/bootloaders/optiboot/optiboot_lilypad_resonator.lst
@@ -7,452 +7,473 @@ Idx Name          Size      VMA       LMA       File off  Algn
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
   1 .debug_aranges 00000028  00000000  00000000  00000250  2**0
                   CONTENTS, READONLY, DEBUGGING
-  2 .debug_pubnames 00000078  00000000  00000000  00000278  2**0
+  2 .debug_pubnames 0000006a  00000000  00000000  00000278  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_info   00000277  00000000  00000000  000002f0  2**0
+  3 .debug_info   00000284  00000000  00000000  000002e2  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_abbrev 00000194  00000000  00000000  00000567  2**0
+  4 .debug_abbrev 000001ae  00000000  00000000  00000566  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_line   000003bb  00000000  00000000  000006fb  2**0
+  5 .debug_line   00000450  00000000  00000000  00000714  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_frame  000000a0  00000000  00000000  00000ab8  2**2
+  6 .debug_frame  00000090  00000000  00000000  00000b64  2**2
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_str    0000013f  00000000  00000000  00000b58  2**0
+  7 .debug_str    00000141  00000000  00000000  00000bf4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_loc    000001a0  00000000  00000000  00000c97  2**0
+  8 .debug_loc    000001e1  00000000  00000000  00000d35  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_ranges 00000070  00000000  00000000  00000e37  2**0
+  9 .debug_ranges 00000068  00000000  00000000  00000f16  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00003e00 <main>:
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3e00:	85 e0       	ldi	r24, 0x05	; 5
-    3e02:	80 93 81 00 	sts	0x0081, r24
-  UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
-  UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
+    3e00:	11 24       	eor	r1, r1
+#ifdef __AVR_ATmega8__
+  SP=RAMEND;  // This is done by hardware reset
 #endif
 
   // Adaboot no-wait mod
   ch = MCUSR;
-    3e06:	84 b7       	in	r24, 0x34	; 52
+    3e02:	84 b7       	in	r24, 0x34	; 52
   MCUSR = 0;
-    3e08:	14 be       	out	0x34, r1	; 52
+    3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
-    3e0a:	81 ff       	sbrs	r24, 1
-    3e0c:	e4 d0       	rcall	.+456    	; 0x3fd6 <appStart>
+    3e06:	81 ff       	sbrs	r24, 1
+    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <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
+  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
+  UCSR0A = _BV(U2X0); //Double speed mode USART0
+    3e10:	82 e0       	ldi	r24, 0x02	; 2
+    3e12:	80 93 c0 00 	sts	0x00C0, r24
+  UCSR0B = _BV(RXEN0) | _BV(TXEN0);
+    3e16:	88 e1       	ldi	r24, 0x18	; 24
+    3e18:	80 93 c1 00 	sts	0x00C1, r24
+  UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
+    3e1c:	86 e0       	ldi	r24, 0x06	; 6
+    3e1e:	80 93 c2 00 	sts	0x00C2, r24
+  UBRR0L = (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
+#endif
+#endif
 
   // Set up watchdog to trigger after 500ms
-  watchdogConfig(WATCHDOG_500MS);
-    3e0e:	8d e0       	ldi	r24, 0x0D	; 13
-    3e10:	dc d0       	rcall	.+440    	; 0x3fca <watchdogConfig>
+  watchdogConfig(WATCHDOG_1S);
+    3e28:	8e e0       	ldi	r24, 0x0E	; 14
+    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    3e12:	25 9a       	sbi	0x04, 5	; 4
-
-#ifdef SOFT_UART
-  /* Set TX pin as output */
-  UART_DDR |= _BV(UART_TX_BIT);
-    3e14:	51 9a       	sbi	0x0a, 1	; 10
-    3e16:	86 e0       	ldi	r24, 0x06	; 6
+    3e2c:	25 9a       	sbi	0x04, 5	; 4
+    3e2e:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e18:	28 e1       	ldi	r18, 0x18	; 24
-    3e1a:	3e ef       	ldi	r19, 0xFE	; 254
+    3e30:	28 e1       	ldi	r18, 0x18	; 24
+    3e32:	3e ef       	ldi	r19, 0xFE	; 254
     TIFR1 = _BV(TOV1);
-    3e1c:	91 e0       	ldi	r25, 0x01	; 1
+    3e34:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e1e:	30 93 85 00 	sts	0x0085, r19
-    3e22:	20 93 84 00 	sts	0x0084, r18
+    3e36:	30 93 85 00 	sts	0x0085, r19
+    3e3a:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    3e26:	96 bb       	out	0x16, r25	; 22
+    3e3e:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    3e28:	b0 9b       	sbis	0x16, 0	; 22
-    3e2a:	fe cf       	rjmp	.-4      	; 0x3e28 <main+0x28>
+    3e40:	b0 9b       	sbis	0x16, 0	; 22
+    3e42:	fe cf       	rjmp	.-4      	; 0x3e40 <main+0x40>
+#ifdef __AVR_ATmega8__
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
-    3e2c:	1d 9a       	sbi	0x03, 5	; 3
+    3e44:	1d 9a       	sbi	0x03, 5	; 3
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3e2e:	a8 95       	wdr
-    TCNT1 = -(F_CPU/(1024*16));
-    TIFR1 = _BV(TOV1);
-    while(!(TIFR1 & _BV(TOV1)));
+    3e46:	a8 95       	wdr
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
+#endif
     watchdogReset();
   } while (--count);
-    3e30:	81 50       	subi	r24, 0x01	; 1
-    3e32:	a9 f7       	brne	.-22     	; 0x3e1e <main+0x1e>
+    3e48:	81 50       	subi	r24, 0x01	; 1
+    3e4a:	a9 f7       	brne	.-22     	; 0x3e36 <main+0x36>
     /* get character from UART */
     ch = getch();
 
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e34:	dd 24       	eor	r13, r13
-    3e36:	d3 94       	inc	r13
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
+    3e4c:	dd 24       	eor	r13, r13
+    3e4e:	d3 94       	inc	r13
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3e38:	a5 e0       	ldi	r26, 0x05	; 5
-    3e3a:	ea 2e       	mov	r14, r26
+      __boot_page_write_short((uint16_t)(void*)address);
+    3e50:	a5 e0       	ldi	r26, 0x05	; 5
+    3e52:	ea 2e       	mov	r14, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3e3c:	f1 e1       	ldi	r31, 0x11	; 17
-    3e3e:	ff 2e       	mov	r15, r31
+    3e54:	f1 e1       	ldi	r31, 0x11	; 17
+    3e56:	ff 2e       	mov	r15, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e40:	ab d0       	rcall	.+342    	; 0x3f98 <getch>
+    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    3e42:	81 34       	cpi	r24, 0x41	; 65
-    3e44:	21 f4       	brne	.+8      	; 0x3e4e <main+0x4e>
+    3e5a:	81 34       	cpi	r24, 0x41	; 65
+    3e5c:	21 f4       	brne	.+8      	; 0x3e66 <main+0x66>
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e46:	81 e0       	ldi	r24, 0x01	; 1
-    3e48:	d1 d0       	rcall	.+418    	; 0x3fec <verifySpace+0xc>
+    3e5e:	81 e0       	ldi	r24, 0x01	; 1
+    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
       putch(0x03);
-    3e4a:	83 e0       	ldi	r24, 0x03	; 3
-    3e4c:	24 c0       	rjmp	.+72     	; 0x3e96 <main+0x96>
+    3e62:	83 e0       	ldi	r24, 0x03	; 3
+    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
     }
     else if(ch == STK_SET_DEVICE) {
-    3e4e:	82 34       	cpi	r24, 0x42	; 66
-    3e50:	11 f4       	brne	.+4      	; 0x3e56 <main+0x56>
+    3e66:	82 34       	cpi	r24, 0x42	; 66
+    3e68:	11 f4       	brne	.+4      	; 0x3e6e <main+0x6e>
       // SET DEVICE is ignored
       getNch(20);
-    3e52:	84 e1       	ldi	r24, 0x14	; 20
-    3e54:	03 c0       	rjmp	.+6      	; 0x3e5c <main+0x5c>
+    3e6a:	84 e1       	ldi	r24, 0x14	; 20
+    3e6c:	03 c0       	rjmp	.+6      	; 0x3e74 <main+0x74>
     }
     else if(ch == STK_SET_DEVICE_EXT) {
-    3e56:	85 34       	cpi	r24, 0x45	; 69
-    3e58:	19 f4       	brne	.+6      	; 0x3e60 <main+0x60>
+    3e6e:	85 34       	cpi	r24, 0x45	; 69
+    3e70:	19 f4       	brne	.+6      	; 0x3e78 <main+0x78>
       // SET DEVICE EXT is ignored
       getNch(5);
-    3e5a:	85 e0       	ldi	r24, 0x05	; 5
-    3e5c:	c7 d0       	rcall	.+398    	; 0x3fec <verifySpace+0xc>
-    3e5e:	8a c0       	rjmp	.+276    	; 0x3f74 <main+0x174>
+    3e72:	85 e0       	ldi	r24, 0x05	; 5
+    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
+    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
-    3e60:	85 35       	cpi	r24, 0x55	; 85
-    3e62:	a1 f4       	brne	.+40     	; 0x3e8c <main+0x8c>
+    3e78:	85 35       	cpi	r24, 0x55	; 85
+    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
       // LOAD ADDRESS
-      address = getch();
-    3e64:	99 d0       	rcall	.+306    	; 0x3f98 <getch>
-    3e66:	08 2f       	mov	r16, r24
-    3e68:	10 e0       	ldi	r17, 0x00	; 0
-    3e6a:	10 93 01 02 	sts	0x0201, r17
-    3e6e:	00 93 00 02 	sts	0x0200, r16
-      address = (address & 0xff) | (getch() << 8);
-    3e72:	92 d0       	rcall	.+292    	; 0x3f98 <getch>
-    3e74:	90 e0       	ldi	r25, 0x00	; 0
-    3e76:	98 2f       	mov	r25, r24
-    3e78:	88 27       	eor	r24, r24
-    3e7a:	80 2b       	or	r24, r16
-    3e7c:	91 2b       	or	r25, r17
-      address += address; // Convert from word address to byte address
-    3e7e:	88 0f       	add	r24, r24
-    3e80:	99 1f       	adc	r25, r25
-    3e82:	90 93 01 02 	sts	0x0201, r25
-    3e86:	80 93 00 02 	sts	0x0200, r24
-    3e8a:	73 c0       	rjmp	.+230    	; 0x3f72 <main+0x172>
+      uint16_t newAddress;
+      newAddress = getch();
+    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+      newAddress = (newAddress & 0xff) | (getch() << 8);
+    3e7e:	08 2f       	mov	r16, r24
+    3e80:	10 e0       	ldi	r17, 0x00	; 0
+    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
+    3e84:	90 e0       	ldi	r25, 0x00	; 0
+    3e86:	98 2f       	mov	r25, r24
+    3e88:	88 27       	eor	r24, r24
+    3e8a:	80 2b       	or	r24, r16
+    3e8c:	91 2b       	or	r25, r17
+#ifdef RAMPZ
+      // Transfer top bit to RAMPZ
+      RAMPZ = (newAddress & 0x8000) ? 1 : 0;
+#endif
+      newAddress += newAddress; // Convert from word address to byte address
+    3e8e:	88 0f       	add	r24, r24
+    3e90:	99 1f       	adc	r25, r25
+      address = newAddress;
+    3e92:	90 93 01 02 	sts	0x0201, r25
+    3e96:	80 93 00 02 	sts	0x0200, r24
+    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
     }
     else if(ch == STK_UNIVERSAL) {
-    3e8c:	86 35       	cpi	r24, 0x56	; 86
-    3e8e:	29 f4       	brne	.+10     	; 0x3e9a <main+0x9a>
+    3e9c:	86 35       	cpi	r24, 0x56	; 86
+    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
       // UNIVERSAL command is ignored
       getNch(4);
-    3e90:	84 e0       	ldi	r24, 0x04	; 4
-    3e92:	ac d0       	rcall	.+344    	; 0x3fec <verifySpace+0xc>
+    3ea0:	84 e0       	ldi	r24, 0x04	; 4
+    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
       putch(0x00);
-    3e94:	80 e0       	ldi	r24, 0x00	; 0
-    3e96:	71 d0       	rcall	.+226    	; 0x3f7a <putch>
-    3e98:	6d c0       	rjmp	.+218    	; 0x3f74 <main+0x174>
+    3ea4:	80 e0       	ldi	r24, 0x00	; 0
+    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
+    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
     }
-    /* Write memory, length is big endian and is in bytes  */
+    /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3e9a:	84 36       	cpi	r24, 0x64	; 100
-    3e9c:	09 f0       	breq	.+2      	; 0x3ea0 <main+0xa0>
-    3e9e:	43 c0       	rjmp	.+134    	; 0x3f26 <main+0x126>
+    3eaa:	84 36       	cpi	r24, 0x64	; 100
+    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
+    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
       getLen();
-    3ea0:	8f d0       	rcall	.+286    	; 0x3fc0 <getLen>
-
-      // Immediately start page erase - this will 4.5ms
-      boot_page_erase((uint16_t)(void*)address);
-    3ea2:	e0 91 00 02 	lds	r30, 0x0200
-    3ea6:	f0 91 01 02 	lds	r31, 0x0201
-    3eaa:	83 e0       	ldi	r24, 0x03	; 3
-    3eac:	80 93 57 00 	sts	0x0057, r24
-    3eb0:	e8 95       	spm
-    3eb2:	c0 e0       	ldi	r28, 0x00	; 0
-    3eb4:	d1 e0       	ldi	r29, 0x01	; 1
-
+    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+
+      // If we are in RWW section, immediately start page erase
+      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3eb2:	e0 91 00 02 	lds	r30, 0x0200
+    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eba:	88 e3       	ldi	r24, 0x38	; 56
+    3ebc:	e0 30       	cpi	r30, 0x00	; 0
+    3ebe:	f8 07       	cpc	r31, r24
+    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
+    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ec4:	87 bf       	out	0x37, r24	; 55
+    3ec6:	e8 95       	spm
+    3ec8:	c0 e0       	ldi	r28, 0x00	; 0
+    3eca:	d1 e0       	ldi	r29, 0x01	; 1
+      
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3eb6:	70 d0       	rcall	.+224    	; 0x3f98 <getch>
-    3eb8:	89 93       	st	Y+, r24
+    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
+    3ece:	89 93       	st	Y+, r24
       while (--length);
-    3eba:	80 91 02 02 	lds	r24, 0x0202
-    3ebe:	81 50       	subi	r24, 0x01	; 1
-    3ec0:	80 93 02 02 	sts	0x0202, r24
-    3ec4:	88 23       	and	r24, r24
-    3ec6:	b9 f7       	brne	.-18     	; 0x3eb6 <main+0xb6>
+    3ed0:	80 91 02 02 	lds	r24, 0x0202
+    3ed4:	81 50       	subi	r24, 0x01	; 1
+    3ed6:	80 93 02 02 	sts	0x0202, r24
+    3eda:	88 23       	and	r24, r24
+    3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
+
+      // If we are in NRWW section, page erase has to be delayed until now.
+      // Todo: Take RAMPZ into account
+      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3ede:	e0 91 00 02 	lds	r30, 0x0200
+    3ee2:	f0 91 01 02 	lds	r31, 0x0201
+    3ee6:	88 e3       	ldi	r24, 0x38	; 56
+    3ee8:	e0 30       	cpi	r30, 0x00	; 0
+    3eea:	f8 07       	cpc	r31, r24
+    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
+    3eee:	83 e0       	ldi	r24, 0x03	; 3
+    3ef0:	87 bf       	out	0x37, r24	; 55
+    3ef2:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    3ec8:	8b d0       	rcall	.+278    	; 0x3fe0 <verifySpace>
+    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
       
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3eca:	07 b6       	in	r0, 0x37	; 55
-    3ecc:	00 fc       	sbrc	r0, 0
-    3ece:	fd cf       	rjmp	.-6      	; 0x3eca <main+0xca>
+    3ef6:	07 b6       	in	r0, 0x37	; 55
+    3ef8:	00 fc       	sbrc	r0, 0
+    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
       }
 #endif
 
       // Copy buffer into programming buffer
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3ed0:	40 91 00 02 	lds	r20, 0x0200
-    3ed4:	50 91 01 02 	lds	r21, 0x0201
-    3ed8:	a0 e0       	ldi	r26, 0x00	; 0
-    3eda:	b1 e0       	ldi	r27, 0x01	; 1
+    3efc:	40 91 00 02 	lds	r20, 0x0200
+    3f00:	50 91 01 02 	lds	r21, 0x0201
+    3f04:	a0 e0       	ldi	r26, 0x00	; 0
+    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3edc:	2c 91       	ld	r18, X
-    3ede:	30 e0       	ldi	r19, 0x00	; 0
+    3f08:	2c 91       	ld	r18, X
+    3f0a:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3ee0:	11 96       	adiw	r26, 0x01	; 1
-    3ee2:	8c 91       	ld	r24, X
-    3ee4:	11 97       	sbiw	r26, 0x01	; 1
-    3ee6:	90 e0       	ldi	r25, 0x00	; 0
-    3ee8:	98 2f       	mov	r25, r24
-    3eea:	88 27       	eor	r24, r24
-    3eec:	82 2b       	or	r24, r18
-    3eee:	93 2b       	or	r25, r19
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+    3f0c:	11 96       	adiw	r26, 0x01	; 1
+    3f0e:	8c 91       	ld	r24, X
+    3f10:	11 97       	sbiw	r26, 0x01	; 1
+    3f12:	90 e0       	ldi	r25, 0x00	; 0
+    3f14:	98 2f       	mov	r25, r24
+    3f16:	88 27       	eor	r24, r24
+    3f18:	82 2b       	or	r24, r18
+    3f1a:	93 2b       	or	r25, r19
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3ef0:	12 96       	adiw	r26, 0x02	; 2
+    3f1c:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
-    3ef2:	fa 01       	movw	r30, r20
-    3ef4:	0c 01       	movw	r0, r24
-    3ef6:	d0 92 57 00 	sts	0x0057, r13
-    3efa:	e8 95       	spm
-    3efc:	11 24       	eor	r1, r1
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3f1e:	fa 01       	movw	r30, r20
+    3f20:	0c 01       	movw	r0, r24
+    3f22:	d7 be       	out	0x37, r13	; 55
+    3f24:	e8 95       	spm
+    3f26:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3efe:	4e 5f       	subi	r20, 0xFE	; 254
-    3f00:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f28:	4e 5f       	subi	r20, 0xFE	; 254
+    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f02:	f1 e0       	ldi	r31, 0x01	; 1
-    3f04:	a0 38       	cpi	r26, 0x80	; 128
-    3f06:	bf 07       	cpc	r27, r31
-    3f08:	49 f7       	brne	.-46     	; 0x3edc <main+0xdc>
+    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
+    3f2e:	a0 38       	cpi	r26, 0x80	; 128
+    3f30:	bf 07       	cpc	r27, r31
+    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3f0a:	e0 91 00 02 	lds	r30, 0x0200
-    3f0e:	f0 91 01 02 	lds	r31, 0x0201
-    3f12:	e0 92 57 00 	sts	0x0057, r14
-    3f16:	e8 95       	spm
+      __boot_page_write_short((uint16_t)(void*)address);
+    3f34:	e0 91 00 02 	lds	r30, 0x0200
+    3f38:	f0 91 01 02 	lds	r31, 0x0201
+    3f3c:	e7 be       	out	0x37, r14	; 55
+    3f3e:	e8 95       	spm
       boot_spm_busy_wait();
-    3f18:	07 b6       	in	r0, 0x37	; 55
-    3f1a:	00 fc       	sbrc	r0, 0
-    3f1c:	fd cf       	rjmp	.-6      	; 0x3f18 <main+0x118>
+    3f40:	07 b6       	in	r0, 0x37	; 55
+    3f42:	00 fc       	sbrc	r0, 0
+    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f1e:	f0 92 57 00 	sts	0x0057, r15
-    3f22:	e8 95       	spm
-    3f24:	27 c0       	rjmp	.+78     	; 0x3f74 <main+0x174>
+    3f46:	f7 be       	out	0x37, r15	; 55
+    3f48:	e8 95       	spm
+    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f26:	84 37       	cpi	r24, 0x74	; 116
-    3f28:	b9 f4       	brne	.+46     	; 0x3f58 <main+0x158>
+    3f4c:	84 37       	cpi	r24, 0x74	; 116
+    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
       // READ PAGE - we only read flash
       getLen();
-    3f2a:	4a d0       	rcall	.+148    	; 0x3fc0 <getLen>
+    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
       verifySpace();
-    3f2c:	59 d0       	rcall	.+178    	; 0x3fe0 <verifySpace>
-        else ch = pgm_read_byte_near(address);
+    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+        putch(result);
         address++;
-        putch(ch);
-      } while (--length);
+      }
+      while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f2e:	e0 91 00 02 	lds	r30, 0x0200
-    3f32:	f0 91 01 02 	lds	r31, 0x0201
-    3f36:	31 96       	adiw	r30, 0x01	; 1
-    3f38:	f0 93 01 02 	sts	0x0201, r31
-    3f3c:	e0 93 00 02 	sts	0x0200, r30
-    3f40:	31 97       	sbiw	r30, 0x01	; 1
-    3f42:	e4 91       	lpm	r30, Z+
-    3f44:	8e 2f       	mov	r24, r30
-    3f46:	19 d0       	rcall	.+50     	; 0x3f7a <putch>
+    3f54:	e0 91 00 02 	lds	r30, 0x0200
+    3f58:	f0 91 01 02 	lds	r31, 0x0201
+    3f5c:	31 96       	adiw	r30, 0x01	; 1
+    3f5e:	f0 93 01 02 	sts	0x0201, r31
+    3f62:	e0 93 00 02 	sts	0x0200, r30
+    3f66:	31 97       	sbiw	r30, 0x01	; 1
+    3f68:	e4 91       	lpm	r30, Z+
+    3f6a:	8e 2f       	mov	r24, r30
+    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
       while (--length);
-    3f48:	80 91 02 02 	lds	r24, 0x0202
-    3f4c:	81 50       	subi	r24, 0x01	; 1
-    3f4e:	80 93 02 02 	sts	0x0202, r24
-    3f52:	88 23       	and	r24, r24
-    3f54:	61 f7       	brne	.-40     	; 0x3f2e <main+0x12e>
-    3f56:	0e c0       	rjmp	.+28     	; 0x3f74 <main+0x174>
+    3f6e:	80 91 02 02 	lds	r24, 0x0202
+    3f72:	81 50       	subi	r24, 0x01	; 1
+    3f74:	80 93 02 02 	sts	0x0202, r24
+    3f78:	88 23       	and	r24, r24
+    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
+    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+#endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f58:	85 37       	cpi	r24, 0x75	; 117
-    3f5a:	39 f4       	brne	.+14     	; 0x3f6a <main+0x16a>
+    3f7e:	85 37       	cpi	r24, 0x75	; 117
+    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f5c:	41 d0       	rcall	.+130    	; 0x3fe0 <verifySpace>
+    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
       putch(SIGNATURE_0);
-    3f5e:	8e e1       	ldi	r24, 0x1E	; 30
-    3f60:	0c d0       	rcall	.+24     	; 0x3f7a <putch>
+    3f84:	8e e1       	ldi	r24, 0x1E	; 30
+    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
       putch(SIGNATURE_1);
-    3f62:	84 e9       	ldi	r24, 0x94	; 148
-    3f64:	0a d0       	rcall	.+20     	; 0x3f7a <putch>
+    3f88:	84 e9       	ldi	r24, 0x94	; 148
+    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
       putch(SIGNATURE_2);
-    3f66:	86 e0       	ldi	r24, 0x06	; 6
-    3f68:	96 cf       	rjmp	.-212    	; 0x3e96 <main+0x96>
+    3f8c:	86 e0       	ldi	r24, 0x06	; 6
+    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
     }
     else if (ch == 'Q') {
-    3f6a:	81 35       	cpi	r24, 0x51	; 81
-    3f6c:	11 f4       	brne	.+4      	; 0x3f72 <main+0x172>
+    3f90:	81 35       	cpi	r24, 0x51	; 81
+    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f6e:	88 e0       	ldi	r24, 0x08	; 8
-    3f70:	2c d0       	rcall	.+88     	; 0x3fca <watchdogConfig>
+    3f94:	88 e0       	ldi	r24, 0x08	; 8
+    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f72:	36 d0       	rcall	.+108    	; 0x3fe0 <verifySpace>
+    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
     }
     putch(STK_OK);
-    3f74:	80 e1       	ldi	r24, 0x10	; 16
-    3f76:	01 d0       	rcall	.+2      	; 0x3f7a <putch>
-    3f78:	63 cf       	rjmp	.-314    	; 0x3e40 <main+0x40>
+    3f9a:	80 e1       	ldi	r24, 0x10	; 16
+    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
+    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
+
+00003fa0 <putch>:
+  }
+}
 
-00003f7a <putch>:
 void putch(char ch) {
+    3fa0:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
+    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fa6:	85 ff       	sbrs	r24, 5
+    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
   UDR0 = ch;
-#else
-  __asm__ __volatile__ (
-    3f7a:	2a e0       	ldi	r18, 0x0A	; 10
-    3f7c:	30 e0       	ldi	r19, 0x00	; 0
-    3f7e:	80 95       	com	r24
-    3f80:	08 94       	sec
-    3f82:	10 f4       	brcc	.+4      	; 0x3f88 <putch+0xe>
-    3f84:	59 98       	cbi	0x0b, 1	; 11
-    3f86:	02 c0       	rjmp	.+4      	; 0x3f8c <putch+0x12>
-    3f88:	59 9a       	sbi	0x0b, 1	; 11
-    3f8a:	00 00       	nop
-    3f8c:	15 d0       	rcall	.+42     	; 0x3fb8 <uartDelay>
-    3f8e:	14 d0       	rcall	.+40     	; 0x3fb8 <uartDelay>
-    3f90:	86 95       	lsr	r24
-    3f92:	2a 95       	dec	r18
-    3f94:	b1 f7       	brne	.-20     	; 0x3f82 <putch+0x8>
+    3faa:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3f96:	08 95       	ret
+    3fae:	08 95       	ret
 
-00003f98 <getch>:
+00003fb0 <getch>:
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3f98:	a8 95       	wdr
-#ifdef LED_DATA_FLASH
+    3fb0:	a8 95       	wdr
+      [uartBit] "I" (UART_RX_BIT)
+    :
+      "r25"
+);
+#else
+  while(!(UCSR0A & _BV(RXC0)));
+    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fb6:	87 ff       	sbrs	r24, 7
+    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
+  ch = UDR0;
+    3fba:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
+#endif
 
   return ch;
 }
-    3f9a:	29 e0       	ldi	r18, 0x09	; 9
-    3f9c:	30 e0       	ldi	r19, 0x00	; 0
-    3f9e:	48 99       	sbic	0x09, 0	; 9
-    3fa0:	fe cf       	rjmp	.-4      	; 0x3f9e <getch+0x6>
-    3fa2:	0a d0       	rcall	.+20     	; 0x3fb8 <uartDelay>
-    3fa4:	09 d0       	rcall	.+18     	; 0x3fb8 <uartDelay>
-    3fa6:	08 d0       	rcall	.+16     	; 0x3fb8 <uartDelay>
-    3fa8:	88 94       	clc
-    3faa:	48 99       	sbic	0x09, 0	; 9
-    3fac:	08 94       	sec
-    3fae:	2a 95       	dec	r18
-    3fb0:	11 f0       	breq	.+4      	; 0x3fb6 <getch+0x1e>
-    3fb2:	87 95       	ror	r24
-    3fb4:	f7 cf       	rjmp	.-18     	; 0x3fa4 <getch+0xc>
-    3fb6:	08 95       	ret
-
-00003fb8 <uartDelay>:
-#if UART_B_VALUE > 255
-#error Baud rate too slow for soft UART
-#endif
-
-void uartDelay() {
-  __asm__ __volatile__ (
-    3fb8:	98 e0       	ldi	r25, 0x08	; 8
-    3fba:	9a 95       	dec	r25
-    3fbc:	f1 f7       	brne	.-4      	; 0x3fba <uartDelay+0x2>
     3fbe:	08 95       	ret
 
 00003fc0 <getLen>:
@@ -462,13 +483,13 @@ void uartDelay() {
 
 uint8_t getLen() {
   getch();
-    3fc0:	eb df       	rcall	.-42     	; 0x3f98 <getch>
+    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
   length = getch();
-    3fc2:	ea df       	rcall	.-44     	; 0x3f98 <getch>
+    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
     3fc4:	80 93 02 02 	sts	0x0202, r24
   return getch();
 }
-    3fc8:	e7 cf       	rjmp	.-50     	; 0x3f98 <getch>
+    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
 
 00003fca <watchdogConfig>:
     "wdr\n"
@@ -504,14 +525,16 @@ void appStart() {
 
 void verifySpace() {
   if (getch() != CRC_EOP) appStart();
-    3fe0:	db df       	rcall	.-74     	; 0x3f98 <getch>
+    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
     3fe2:	80 32       	cpi	r24, 0x20	; 32
     3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
     3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
   putch(STK_INSYNC);
     3fe8:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fea:	c7 cf       	rjmp	.-114    	; 0x3f7a <putch>
+    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+
+00003fec <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
@@ -520,12 +543,10 @@ void verifySpace() {
 void getNch(uint8_t count) {
     3fec:	1f 93       	push	r17
     3fee:	18 2f       	mov	r17, r24
-
-00003ff0 <getNch>:
   do getch(); while (--count);
-    3ff0:	d3 df       	rcall	.-90     	; 0x3f98 <getch>
+    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
     3ff2:	11 50       	subi	r17, 0x01	; 1
-    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch>
+    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
   verifySpace();
     3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
 }
diff --git a/bootloaders/optiboot/optiboot_luminet.hex b/bootloaders/optiboot/optiboot_luminet.hex
index 0e51124..3ad3c0a 100644
--- a/bootloaders/optiboot/optiboot_luminet.hex
+++ b/bootloaders/optiboot/optiboot_luminet.hex
@@ -1,42 +1,42 @@
-:101D000085E08EBD84B714BE81FF27D18DE021D13F

-:101D1000D49AD29A86E023EC3FEF91E03DBD2CBDF2

-:101D20009BB9589BFECFCC9AA8958150B9F7CC248B

-:101D3000C39485E0E82E0FE7D02E1EECF12EF0D0F4

-:101D4000813421F481E014D183E024C0823411F481

-:101D500084E103C0853419F485E00AD1CFC085350C

-:101D6000A1F4DED0082F10E01093010200930002CE

-:101D7000D7D090E0982F8827802B912B880F991F20

-:101D80009093010280930002B8C0863529F484E064

-:101D9000EFD080E0B6D0B2C0843609F06EC0D4D0A7

-:101DA000E0910002F091010283E080935700E895F2

-:101DB000C0E0D1E0B5D08993809102028150809338

-:101DC00002028823B9F7CED007B600FCFDCF809180

-:101DD000000290910102892B41F580910001209130

-:101DE000010130E0322F222790E0282B392B30934D

-:101DF00005022093040240910A0180910B0190E0BA

-:101E0000982F882750E0842B952B9093070280937E

-:101E100006022450304020930A01232F33272093B9

-:101E20000B01D0920001F09201014091000250910B

-:101E30000102A0E0B1E02C9130E011968C91119755

-:101E400090E0982F8827822B932B1296FA010C0191

-:101E5000C0925700E89511244E5F5F4FF1E0A03427

-:101E6000BF0749F7E0910002F0910102E0925700AC

-:101E7000E89507B600FCFDCF41C0843789F564D0F2

-:101E800071D0E0910002F0910102309719F4209195

-:101E9000040213C0E130F10519F4209105020DC0D0

-:101EA000EA30F10519F42091060207C0EB30F10584

-:101EB00019F42091070201C02491809100029091B1

-:101EC000010201969093010280930002822F19D0A3

-:101ED00080910202815080930202882391F60EC005

-:101EE000853739F43FD08EE10CD083E90AD08CE0FD

-:101EF00051CF813511F488E02CD034D080E101D06D

-:101F00001ECF2AE030E08095089410F4DA9802C0E1

-:101F1000DA9A000015D014D086952A95B1F7089565

-:101F2000A89529E030E0CB99FECF0AD009D008D09F

-:101F30008894CB9908942A9511F08795F7CF089546

-:101F40009EE09A95F1F70895EBDFEADF80930202B5

-:101F5000E7CF98E191BD81BD089580E0FADFE5E02B

-:101F6000FF270994DDDF803209F0F7DF84E1C9CF74

-:101F70001F93182FD5DF1150E9F7F4DF1F91089553

+:101D0000112484B714BE81FF22D185E08EBD8EE000

+:101D10001AD1D49AD29A86E023EC3FEF91E03DBDF0

+:101D20002CBD9BB9589BFECFCC9AA8958150B9F792

+:101D3000DD24D39485E0C82E0FE7F02E1EECE12EB3

+:101D4000E9D0813421F481E00DD183E020C08234D8

+:101D500011F484E103C0853419F485E003D1C8C0CF

+:101D6000853581F4D7D0082F10E0D4D090E0982F9B

+:101D70008827802B912B880F991F90938101809346

+:101D80008001B5C0863529F484E0ECD080E0B3D082

+:101D9000AFC0843609F06BC0D1D0C0E0D1E0BAD07A

+:101DA0008993809182018150809382018823B9F7C1

+:101DB000E0918001F091810183E087BFE895CCD06C

+:101DC00007B600FCFDCF8091800190918101892BA5

+:101DD00041F5809100012091010130E0322F22274E

+:101DE00090E0282B392B309385012093840140917A

+:101DF00008018091090190E0982F882750E0842BFA

+:101E0000952B909387018093860124503040209336

+:101E10000801232F332720930901F0920001E0925B

+:101E200001014091800150918101A0E0B1E02C912D

+:101E300030E011968C91119790E0982F8827822B93

+:101E4000932B1296FA010C01D7BEE89511244E5F30

+:101E50005F4FF1E0A034BF0751F7E0918001F091AE

+:101E60008101C7BEE89507B600FCFDCF41C08437AD

+:101E700089F564D071D0E0918001F09181013097B3

+:101E800019F42091840113C0E130F10519F4209177

+:101E900085010DC0E830F10519F42091860107C0D5

+:101EA000E930F10519F42091870101C02491809156

+:101EB000800190918101019690938101809380012E

+:101EC000822F19D0809182018150809382018823D2

+:101ED00091F60EC0853739F43FD08EE10CD083E9FE

+:101EE0000AD08CE054CF813511F488E02CD034D066

+:101EF00080E101D025CF2AE030E08095089410F4ED

+:101F0000DA9802C0DA9A000015D014D086952A9586

+:101F1000B1F70895A89529E030E0CB99FECF0AD01B

+:101F200009D008D08894CB9908942A9511F0879508

+:101F3000F7CF08959EE09A95F1F70895EBDFEADF79

+:101F400080938201E7CF98E191BD81BD089580E043

+:101F5000FADFE4E0FF270994DDDF803209F0F7DFE4

+:101F600084E1C9CF1F93182FD5DF1150E9F7F4DFB3

+:041F70001F91089520

 :0400000300001D00DC

 :00000001FF

diff --git a/bootloaders/optiboot/optiboot_luminet.lst b/bootloaders/optiboot/optiboot_luminet.lst
index 59468cb..737d2c5 100644
--- a/bootloaders/optiboot/optiboot_luminet.lst
+++ b/bootloaders/optiboot/optiboot_luminet.lst
@@ -3,602 +3,620 @@ optiboot_luminet.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         00000280  00001d00  00001d00  00000054  2**1
+  0 .text         00000274  00001d00  00001d00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .debug_aranges 00000028  00000000  00000000  000002d4  2**0
+  1 .debug_aranges 00000028  00000000  00000000  000002c8  2**0
                   CONTENTS, READONLY, DEBUGGING
-  2 .debug_pubnames 00000078  00000000  00000000  000002fc  2**0
+  2 .debug_pubnames 00000078  00000000  00000000  000002f0  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_info   00000289  00000000  00000000  00000374  2**0
+  3 .debug_info   000002a4  00000000  00000000  00000368  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_abbrev 000001a1  00000000  00000000  000005fd  2**0
+  4 .debug_abbrev 000001ac  00000000  00000000  0000060c  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_line   00000435  00000000  00000000  0000079e  2**0
+  5 .debug_line   000004a9  00000000  00000000  000007b8  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_frame  000000a0  00000000  00000000  00000bd4  2**2
+  6 .debug_frame  000000a0  00000000  00000000  00000c64  2**2
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_str    00000144  00000000  00000000  00000c74  2**0
+  7 .debug_str    00000150  00000000  00000000  00000d04  2**0
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_loc    00000194  00000000  00000000  00000db8  2**0
+  8 .debug_loc    00000194  00000000  00000000  00000e54  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_ranges 00000088  00000000  00000000  00000f4c  2**0
+  9 .debug_ranges 00000088  00000000  00000000  00000fe8  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00001d00 <main>:
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    1d00:	85 e0       	ldi	r24, 0x05	; 5
-    1d02:	8e bd       	out	0x2e, r24	; 46
-  UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
-  UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
+    1d00:	11 24       	eor	r1, r1
+#ifdef __AVR_ATmega8__
+  SP=RAMEND;  // This is done by hardware reset
 #endif
 
   // Adaboot no-wait mod
   ch = MCUSR;
-    1d04:	84 b7       	in	r24, 0x34	; 52
+    1d02:	84 b7       	in	r24, 0x34	; 52
   MCUSR = 0;
-    1d06:	14 be       	out	0x34, r1	; 52
+    1d04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
-    1d08:	81 ff       	sbrs	r24, 1
-    1d0a:	27 d1       	rcall	.+590    	; 0x1f5a <appStart>
+    1d06:	81 ff       	sbrs	r24, 1
+    1d08:	22 d1       	rcall	.+580    	; 0x1f4e <appStart>
+
+#if LED_START_FLASHES > 0
+  // Set up Timer 1 for timeout counter
+  TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
+    1d0a:	85 e0       	ldi	r24, 0x05	; 5
+    1d0c:	8e bd       	out	0x2e, r24	; 46
+  UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
+#endif
+#endif
 
   // Set up watchdog to trigger after 500ms
-  watchdogConfig(WATCHDOG_500MS);
-    1d0c:	8d e0       	ldi	r24, 0x0D	; 13
-    1d0e:	21 d1       	rcall	.+578    	; 0x1f52 <watchdogConfig>
+  watchdogConfig(WATCHDOG_1S);
+    1d0e:	8e e0       	ldi	r24, 0x0E	; 14
+    1d10:	1a d1       	rcall	.+564    	; 0x1f46 <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    1d10:	d4 9a       	sbi	0x1a, 4	; 26
+    1d12:	d4 9a       	sbi	0x1a, 4	; 26
 
 #ifdef SOFT_UART
   /* Set TX pin as output */
   UART_DDR |= _BV(UART_TX_BIT);
-    1d12:	d2 9a       	sbi	0x1a, 2	; 26
-    1d14:	86 e0       	ldi	r24, 0x06	; 6
+    1d14:	d2 9a       	sbi	0x1a, 2	; 26
+    1d16:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    1d16:	23 ec       	ldi	r18, 0xC3	; 195
-    1d18:	3f ef       	ldi	r19, 0xFF	; 255
+    1d18:	23 ec       	ldi	r18, 0xC3	; 195
+    1d1a:	3f ef       	ldi	r19, 0xFF	; 255
     TIFR1 = _BV(TOV1);
-    1d1a:	91 e0       	ldi	r25, 0x01	; 1
+    1d1c:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    1d1c:	3d bd       	out	0x2d, r19	; 45
-    1d1e:	2c bd       	out	0x2c, r18	; 44
+    1d1e:	3d bd       	out	0x2d, r19	; 45
+    1d20:	2c bd       	out	0x2c, r18	; 44
     TIFR1 = _BV(TOV1);
-    1d20:	9b b9       	out	0x0b, r25	; 11
+    1d22:	9b b9       	out	0x0b, r25	; 11
     while(!(TIFR1 & _BV(TOV1)));
-    1d22:	58 9b       	sbis	0x0b, 0	; 11
-    1d24:	fe cf       	rjmp	.-4      	; 0x1d22 <main+0x22>
+    1d24:	58 9b       	sbis	0x0b, 0	; 11
+    1d26:	fe cf       	rjmp	.-4      	; 0x1d24 <main+0x24>
+#ifdef __AVR_ATmega8__
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
-    1d26:	cc 9a       	sbi	0x19, 4	; 25
+    1d28:	cc 9a       	sbi	0x19, 4	; 25
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    1d28:	a8 95       	wdr
-    TCNT1 = -(F_CPU/(1024*16));
-    TIFR1 = _BV(TOV1);
-    while(!(TIFR1 & _BV(TOV1)));
+    1d2a:	a8 95       	wdr
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
+#endif
     watchdogReset();
   } while (--count);
-    1d2a:	81 50       	subi	r24, 0x01	; 1
-    1d2c:	b9 f7       	brne	.-18     	; 0x1d1c <main+0x1c>
+    1d2c:	81 50       	subi	r24, 0x01	; 1
+    1d2e:	b9 f7       	brne	.-18     	; 0x1d1e <main+0x1e>
     /* get character from UART */
     ch = getch();
 
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    1d2e:	cc 24       	eor	r12, r12
-    1d30:	c3 94       	inc	r12
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
+    1d30:	dd 24       	eor	r13, r13
+    1d32:	d3 94       	inc	r13
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    1d32:	85 e0       	ldi	r24, 0x05	; 5
-    1d34:	e8 2e       	mov	r14, r24
+      __boot_page_write_short((uint16_t)(void*)address);
+    1d34:	85 e0       	ldi	r24, 0x05	; 5
+    1d36:	c8 2e       	mov	r12, r24
         vect -= 4; // Instruction is a relative jump (rjmp), so recalculate.
-        buff[10] = vect & 0xff;
-        buff[11] = vect >> 8;
+        buff[8] = vect & 0xff;
+        buff[9] = vect >> 8;
 
         // Add jump to bootloader at RESET vector
         buff[0] = 0x7f;
-    1d36:	0f e7       	ldi	r16, 0x7F	; 127
-    1d38:	d0 2e       	mov	r13, r16
+    1d38:	0f e7       	ldi	r16, 0x7F	; 127
+    1d3a:	f0 2e       	mov	r15, r16
         buff[1] = 0xce; // rjmp 0x1d00 instruction
-    1d3a:	1e ec       	ldi	r17, 0xCE	; 206
-    1d3c:	f1 2e       	mov	r15, r17
+    1d3c:	1e ec       	ldi	r17, 0xCE	; 206
+    1d3e:	e1 2e       	mov	r14, r17
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    1d3e:	f0 d0       	rcall	.+480    	; 0x1f20 <getch>
+    1d40:	e9 d0       	rcall	.+466    	; 0x1f14 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    1d40:	81 34       	cpi	r24, 0x41	; 65
-    1d42:	21 f4       	brne	.+8      	; 0x1d4c <main+0x4c>
+    1d42:	81 34       	cpi	r24, 0x41	; 65
+    1d44:	21 f4       	brne	.+8      	; 0x1d4e <main+0x4e>
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    1d44:	81 e0       	ldi	r24, 0x01	; 1
-    1d46:	14 d1       	rcall	.+552    	; 0x1f70 <verifySpace+0xc>
+    1d46:	81 e0       	ldi	r24, 0x01	; 1
+    1d48:	0d d1       	rcall	.+538    	; 0x1f64 <getNch>
       putch(0x03);
-    1d48:	83 e0       	ldi	r24, 0x03	; 3
-    1d4a:	24 c0       	rjmp	.+72     	; 0x1d94 <main+0x94>
+    1d4a:	83 e0       	ldi	r24, 0x03	; 3
+    1d4c:	20 c0       	rjmp	.+64     	; 0x1d8e <main+0x8e>
     }
     else if(ch == STK_SET_DEVICE) {
-    1d4c:	82 34       	cpi	r24, 0x42	; 66
-    1d4e:	11 f4       	brne	.+4      	; 0x1d54 <main+0x54>
+    1d4e:	82 34       	cpi	r24, 0x42	; 66
+    1d50:	11 f4       	brne	.+4      	; 0x1d56 <main+0x56>
       // SET DEVICE is ignored
       getNch(20);
-    1d50:	84 e1       	ldi	r24, 0x14	; 20
-    1d52:	03 c0       	rjmp	.+6      	; 0x1d5a <main+0x5a>
+    1d52:	84 e1       	ldi	r24, 0x14	; 20
+    1d54:	03 c0       	rjmp	.+6      	; 0x1d5c <main+0x5c>
     }
     else if(ch == STK_SET_DEVICE_EXT) {
-    1d54:	85 34       	cpi	r24, 0x45	; 69
-    1d56:	19 f4       	brne	.+6      	; 0x1d5e <main+0x5e>
+    1d56:	85 34       	cpi	r24, 0x45	; 69
+    1d58:	19 f4       	brne	.+6      	; 0x1d60 <main+0x60>
       // SET DEVICE EXT is ignored
       getNch(5);
-    1d58:	85 e0       	ldi	r24, 0x05	; 5
-    1d5a:	0a d1       	rcall	.+532    	; 0x1f70 <verifySpace+0xc>
-    1d5c:	cf c0       	rjmp	.+414    	; 0x1efc <main+0x1fc>
+    1d5a:	85 e0       	ldi	r24, 0x05	; 5
+    1d5c:	03 d1       	rcall	.+518    	; 0x1f64 <getNch>
+    1d5e:	c8 c0       	rjmp	.+400    	; 0x1ef0 <main+0x1f0>
     }
     else if(ch == STK_LOAD_ADDRESS) {
-    1d5e:	85 35       	cpi	r24, 0x55	; 85
-    1d60:	a1 f4       	brne	.+40     	; 0x1d8a <main+0x8a>
+    1d60:	85 35       	cpi	r24, 0x55	; 85
+    1d62:	81 f4       	brne	.+32     	; 0x1d84 <main+0x84>
       // LOAD ADDRESS
-      address = getch();
-    1d62:	de d0       	rcall	.+444    	; 0x1f20 <getch>
-    1d64:	08 2f       	mov	r16, r24
-    1d66:	10 e0       	ldi	r17, 0x00	; 0
-    1d68:	10 93 01 02 	sts	0x0201, r17
-    1d6c:	00 93 00 02 	sts	0x0200, r16
-      address = (address & 0xff) | (getch() << 8);
-    1d70:	d7 d0       	rcall	.+430    	; 0x1f20 <getch>
-    1d72:	90 e0       	ldi	r25, 0x00	; 0
-    1d74:	98 2f       	mov	r25, r24
-    1d76:	88 27       	eor	r24, r24
-    1d78:	80 2b       	or	r24, r16
-    1d7a:	91 2b       	or	r25, r17
-      address += address; // Convert from word address to byte address
-    1d7c:	88 0f       	add	r24, r24
-    1d7e:	99 1f       	adc	r25, r25
-    1d80:	90 93 01 02 	sts	0x0201, r25
-    1d84:	80 93 00 02 	sts	0x0200, r24
-    1d88:	b8 c0       	rjmp	.+368    	; 0x1efa <main+0x1fa>
+      uint16_t newAddress;
+      newAddress = getch();
+    1d64:	d7 d0       	rcall	.+430    	; 0x1f14 <getch>
+      newAddress = (newAddress & 0xff) | (getch() << 8);
+    1d66:	08 2f       	mov	r16, r24
+    1d68:	10 e0       	ldi	r17, 0x00	; 0
+    1d6a:	d4 d0       	rcall	.+424    	; 0x1f14 <getch>
+    1d6c:	90 e0       	ldi	r25, 0x00	; 0
+    1d6e:	98 2f       	mov	r25, r24
+    1d70:	88 27       	eor	r24, r24
+    1d72:	80 2b       	or	r24, r16
+    1d74:	91 2b       	or	r25, r17
+#ifdef RAMPZ
+      // Transfer top bit to RAMPZ
+      RAMPZ = (newAddress & 0x8000) ? 1 : 0;
+#endif
+      newAddress += newAddress; // Convert from word address to byte address
+    1d76:	88 0f       	add	r24, r24
+    1d78:	99 1f       	adc	r25, r25
+      address = newAddress;
+    1d7a:	90 93 81 01 	sts	0x0181, r25
+    1d7e:	80 93 80 01 	sts	0x0180, r24
+    1d82:	b5 c0       	rjmp	.+362    	; 0x1eee <main+0x1ee>
       verifySpace();
     }
     else if(ch == STK_UNIVERSAL) {
-    1d8a:	86 35       	cpi	r24, 0x56	; 86
-    1d8c:	29 f4       	brne	.+10     	; 0x1d98 <main+0x98>
+    1d84:	86 35       	cpi	r24, 0x56	; 86
+    1d86:	29 f4       	brne	.+10     	; 0x1d92 <main+0x92>
       // UNIVERSAL command is ignored
       getNch(4);
-    1d8e:	84 e0       	ldi	r24, 0x04	; 4
-    1d90:	ef d0       	rcall	.+478    	; 0x1f70 <verifySpace+0xc>
+    1d88:	84 e0       	ldi	r24, 0x04	; 4
+    1d8a:	ec d0       	rcall	.+472    	; 0x1f64 <getNch>
       putch(0x00);
-    1d92:	80 e0       	ldi	r24, 0x00	; 0
-    1d94:	b6 d0       	rcall	.+364    	; 0x1f02 <putch>
-    1d96:	b2 c0       	rjmp	.+356    	; 0x1efc <main+0x1fc>
+    1d8c:	80 e0       	ldi	r24, 0x00	; 0
+    1d8e:	b3 d0       	rcall	.+358    	; 0x1ef6 <putch>
+    1d90:	af c0       	rjmp	.+350    	; 0x1ef0 <main+0x1f0>
     }
-    /* Write memory, length is big endian and is in bytes  */
+    /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    1d98:	84 36       	cpi	r24, 0x64	; 100
-    1d9a:	09 f0       	breq	.+2      	; 0x1d9e <main+0x9e>
-    1d9c:	6e c0       	rjmp	.+220    	; 0x1e7a <main+0x17a>
+    1d92:	84 36       	cpi	r24, 0x64	; 100
+    1d94:	09 f0       	breq	.+2      	; 0x1d98 <main+0x98>
+    1d96:	6b c0       	rjmp	.+214    	; 0x1e6e <main+0x16e>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
       getLen();
-    1d9e:	d4 d0       	rcall	.+424    	; 0x1f48 <getLen>
-
-      // Immediately start page erase - this will 4.5ms
-      boot_page_erase((uint16_t)(void*)address);
-    1da0:	e0 91 00 02 	lds	r30, 0x0200
-    1da4:	f0 91 01 02 	lds	r31, 0x0201
-    1da8:	83 e0       	ldi	r24, 0x03	; 3
-    1daa:	80 93 57 00 	sts	0x0057, r24
-    1dae:	e8 95       	spm
-    1db0:	c0 e0       	ldi	r28, 0x00	; 0
-    1db2:	d1 e0       	ldi	r29, 0x01	; 1
-
+    1d98:	d1 d0       	rcall	.+418    	; 0x1f3c <getLen>
+    1d9a:	c0 e0       	ldi	r28, 0x00	; 0
+    1d9c:	d1 e0       	ldi	r29, 0x01	; 1
+      // If we are in RWW section, immediately start page erase
+      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+      
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    1db4:	b5 d0       	rcall	.+362    	; 0x1f20 <getch>
-    1db6:	89 93       	st	Y+, r24
+    1d9e:	ba d0       	rcall	.+372    	; 0x1f14 <getch>
+    1da0:	89 93       	st	Y+, r24
       while (--length);
-    1db8:	80 91 02 02 	lds	r24, 0x0202
-    1dbc:	81 50       	subi	r24, 0x01	; 1
-    1dbe:	80 93 02 02 	sts	0x0202, r24
-    1dc2:	88 23       	and	r24, r24
-    1dc4:	b9 f7       	brne	.-18     	; 0x1db4 <main+0xb4>
+    1da2:	80 91 82 01 	lds	r24, 0x0182
+    1da6:	81 50       	subi	r24, 0x01	; 1
+    1da8:	80 93 82 01 	sts	0x0182, r24
+    1dac:	88 23       	and	r24, r24
+    1dae:	b9 f7       	brne	.-18     	; 0x1d9e <main+0x9e>
+
+      // If we are in NRWW section, page erase has to be delayed until now.
+      // Todo: Take RAMPZ into account
+      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    1db0:	e0 91 80 01 	lds	r30, 0x0180
+    1db4:	f0 91 81 01 	lds	r31, 0x0181
+    1db8:	83 e0       	ldi	r24, 0x03	; 3
+    1dba:	87 bf       	out	0x37, r24	; 55
+    1dbc:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    1dc6:	ce d0       	rcall	.+412    	; 0x1f64 <verifySpace>
+    1dbe:	cc d0       	rcall	.+408    	; 0x1f58 <verifySpace>
       
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    1dc8:	07 b6       	in	r0, 0x37	; 55
-    1dca:	00 fc       	sbrc	r0, 0
-    1dcc:	fd cf       	rjmp	.-6      	; 0x1dc8 <main+0xc8>
+    1dc0:	07 b6       	in	r0, 0x37	; 55
+    1dc2:	00 fc       	sbrc	r0, 0
+    1dc4:	fd cf       	rjmp	.-6      	; 0x1dc0 <main+0xc0>
 
 #ifdef VIRTUAL_BOOT_PARTITION
       if ((uint16_t)(void*)address == 0) {
-    1dce:	80 91 00 02 	lds	r24, 0x0200
-    1dd2:	90 91 01 02 	lds	r25, 0x0201
-    1dd6:	89 2b       	or	r24, r25
-    1dd8:	41 f5       	brne	.+80     	; 0x1e2a <main+0x12a>
+    1dc6:	80 91 80 01 	lds	r24, 0x0180
+    1dca:	90 91 81 01 	lds	r25, 0x0181
+    1dce:	89 2b       	or	r24, r25
+    1dd0:	41 f5       	brne	.+80     	; 0x1e22 <main+0x122>
         // This is the reset vector page. We need to live-patch the code so the
         // bootloader runs.
         //
         // Move RESET vector to WDT vector
         uint16_t vect = buff[0] | (buff[1]<<8);
-    1dda:	80 91 00 01 	lds	r24, 0x0100
-    1dde:	20 91 01 01 	lds	r18, 0x0101
-    1de2:	30 e0       	ldi	r19, 0x00	; 0
-    1de4:	32 2f       	mov	r19, r18
-    1de6:	22 27       	eor	r18, r18
-    1de8:	90 e0       	ldi	r25, 0x00	; 0
-    1dea:	28 2b       	or	r18, r24
-    1dec:	39 2b       	or	r19, r25
+    1dd2:	80 91 00 01 	lds	r24, 0x0100
+    1dd6:	20 91 01 01 	lds	r18, 0x0101
+    1dda:	30 e0       	ldi	r19, 0x00	; 0
+    1ddc:	32 2f       	mov	r19, r18
+    1dde:	22 27       	eor	r18, r18
+    1de0:	90 e0       	ldi	r25, 0x00	; 0
+    1de2:	28 2b       	or	r18, r24
+    1de4:	39 2b       	or	r19, r25
         rstVect = vect;
-    1dee:	30 93 05 02 	sts	0x0205, r19
-    1df2:	20 93 04 02 	sts	0x0204, r18
-        wdtVect = buff[10] | (buff[11]<<8);
-    1df6:	40 91 0a 01 	lds	r20, 0x010A
-    1dfa:	80 91 0b 01 	lds	r24, 0x010B
-    1dfe:	90 e0       	ldi	r25, 0x00	; 0
-    1e00:	98 2f       	mov	r25, r24
-    1e02:	88 27       	eor	r24, r24
-    1e04:	50 e0       	ldi	r21, 0x00	; 0
-    1e06:	84 2b       	or	r24, r20
-    1e08:	95 2b       	or	r25, r21
-    1e0a:	90 93 07 02 	sts	0x0207, r25
-    1e0e:	80 93 06 02 	sts	0x0206, r24
+    1de6:	30 93 85 01 	sts	0x0185, r19
+    1dea:	20 93 84 01 	sts	0x0184, r18
+        wdtVect = buff[8] | (buff[9]<<8);
+    1dee:	40 91 08 01 	lds	r20, 0x0108
+    1df2:	80 91 09 01 	lds	r24, 0x0109
+    1df6:	90 e0       	ldi	r25, 0x00	; 0
+    1df8:	98 2f       	mov	r25, r24
+    1dfa:	88 27       	eor	r24, r24
+    1dfc:	50 e0       	ldi	r21, 0x00	; 0
+    1dfe:	84 2b       	or	r24, r20
+    1e00:	95 2b       	or	r25, r21
+    1e02:	90 93 87 01 	sts	0x0187, r25
+    1e06:	80 93 86 01 	sts	0x0186, r24
         vect -= 4; // Instruction is a relative jump (rjmp), so recalculate.
-    1e12:	24 50       	subi	r18, 0x04	; 4
-    1e14:	30 40       	sbci	r19, 0x00	; 0
-        buff[10] = vect & 0xff;
-    1e16:	20 93 0a 01 	sts	0x010A, r18
-        buff[11] = vect >> 8;
-    1e1a:	23 2f       	mov	r18, r19
-    1e1c:	33 27       	eor	r19, r19
-    1e1e:	20 93 0b 01 	sts	0x010B, r18
+    1e0a:	24 50       	subi	r18, 0x04	; 4
+    1e0c:	30 40       	sbci	r19, 0x00	; 0
+        buff[8] = vect & 0xff;
+    1e0e:	20 93 08 01 	sts	0x0108, r18
+        buff[9] = vect >> 8;
+    1e12:	23 2f       	mov	r18, r19
+    1e14:	33 27       	eor	r19, r19
+    1e16:	20 93 09 01 	sts	0x0109, r18
 
         // Add jump to bootloader at RESET vector
         buff[0] = 0x7f;
-    1e22:	d0 92 00 01 	sts	0x0100, r13
+    1e1a:	f0 92 00 01 	sts	0x0100, r15
         buff[1] = 0xce; // rjmp 0x1d00 instruction
-    1e26:	f0 92 01 01 	sts	0x0101, r15
+    1e1e:	e0 92 01 01 	sts	0x0101, r14
       }
 #endif
 
       // Copy buffer into programming buffer
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    1e2a:	40 91 00 02 	lds	r20, 0x0200
-    1e2e:	50 91 01 02 	lds	r21, 0x0201
-    1e32:	a0 e0       	ldi	r26, 0x00	; 0
-    1e34:	b1 e0       	ldi	r27, 0x01	; 1
+    1e22:	40 91 80 01 	lds	r20, 0x0180
+    1e26:	50 91 81 01 	lds	r21, 0x0181
+    1e2a:	a0 e0       	ldi	r26, 0x00	; 0
+    1e2c:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    1e36:	2c 91       	ld	r18, X
-    1e38:	30 e0       	ldi	r19, 0x00	; 0
+    1e2e:	2c 91       	ld	r18, X
+    1e30:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    1e3a:	11 96       	adiw	r26, 0x01	; 1
-    1e3c:	8c 91       	ld	r24, X
-    1e3e:	11 97       	sbiw	r26, 0x01	; 1
-    1e40:	90 e0       	ldi	r25, 0x00	; 0
-    1e42:	98 2f       	mov	r25, r24
-    1e44:	88 27       	eor	r24, r24
-    1e46:	82 2b       	or	r24, r18
-    1e48:	93 2b       	or	r25, r19
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+    1e32:	11 96       	adiw	r26, 0x01	; 1
+    1e34:	8c 91       	ld	r24, X
+    1e36:	11 97       	sbiw	r26, 0x01	; 1
+    1e38:	90 e0       	ldi	r25, 0x00	; 0
+    1e3a:	98 2f       	mov	r25, r24
+    1e3c:	88 27       	eor	r24, r24
+    1e3e:	82 2b       	or	r24, r18
+    1e40:	93 2b       	or	r25, r19
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    1e4a:	12 96       	adiw	r26, 0x02	; 2
+    1e42:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
-    1e4c:	fa 01       	movw	r30, r20
-    1e4e:	0c 01       	movw	r0, r24
-    1e50:	c0 92 57 00 	sts	0x0057, r12
-    1e54:	e8 95       	spm
-    1e56:	11 24       	eor	r1, r1
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    1e44:	fa 01       	movw	r30, r20
+    1e46:	0c 01       	movw	r0, r24
+    1e48:	d7 be       	out	0x37, r13	; 55
+    1e4a:	e8 95       	spm
+    1e4c:	11 24       	eor	r1, r1
         addrPtr += 2;
-    1e58:	4e 5f       	subi	r20, 0xFE	; 254
-    1e5a:	5f 4f       	sbci	r21, 0xFF	; 255
+    1e4e:	4e 5f       	subi	r20, 0xFE	; 254
+    1e50:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    1e5c:	f1 e0       	ldi	r31, 0x01	; 1
-    1e5e:	a0 34       	cpi	r26, 0x40	; 64
-    1e60:	bf 07       	cpc	r27, r31
-    1e62:	49 f7       	brne	.-46     	; 0x1e36 <main+0x136>
+    1e52:	f1 e0       	ldi	r31, 0x01	; 1
+    1e54:	a0 34       	cpi	r26, 0x40	; 64
+    1e56:	bf 07       	cpc	r27, r31
+    1e58:	51 f7       	brne	.-44     	; 0x1e2e <main+0x12e>
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    1e64:	e0 91 00 02 	lds	r30, 0x0200
-    1e68:	f0 91 01 02 	lds	r31, 0x0201
-    1e6c:	e0 92 57 00 	sts	0x0057, r14
-    1e70:	e8 95       	spm
+      __boot_page_write_short((uint16_t)(void*)address);
+    1e5a:	e0 91 80 01 	lds	r30, 0x0180
+    1e5e:	f0 91 81 01 	lds	r31, 0x0181
+    1e62:	c7 be       	out	0x37, r12	; 55
+    1e64:	e8 95       	spm
       boot_spm_busy_wait();
-    1e72:	07 b6       	in	r0, 0x37	; 55
-    1e74:	00 fc       	sbrc	r0, 0
-    1e76:	fd cf       	rjmp	.-6      	; 0x1e72 <main+0x172>
-    1e78:	41 c0       	rjmp	.+130    	; 0x1efc <main+0x1fc>
+    1e66:	07 b6       	in	r0, 0x37	; 55
+    1e68:	00 fc       	sbrc	r0, 0
+    1e6a:	fd cf       	rjmp	.-6      	; 0x1e66 <main+0x166>
+    1e6c:	41 c0       	rjmp	.+130    	; 0x1ef0 <main+0x1f0>
       boot_rww_enable();
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    1e7a:	84 37       	cpi	r24, 0x74	; 116
-    1e7c:	89 f5       	brne	.+98     	; 0x1ee0 <main+0x1e0>
+    1e6e:	84 37       	cpi	r24, 0x74	; 116
+    1e70:	89 f5       	brne	.+98     	; 0x1ed4 <main+0x1d4>
       // READ PAGE - we only read flash
       getLen();
-    1e7e:	64 d0       	rcall	.+200    	; 0x1f48 <getLen>
+    1e72:	64 d0       	rcall	.+200    	; 0x1f3c <getLen>
       verifySpace();
-    1e80:	71 d0       	rcall	.+226    	; 0x1f64 <verifySpace>
+    1e74:	71 d0       	rcall	.+226    	; 0x1f58 <verifySpace>
 #ifdef VIRTUAL_BOOT_PARTITION
       do {
         // Undo vector patch in bottom page so verify passes
         if (address == 0)       ch=rstVect & 0xff;
-    1e82:	e0 91 00 02 	lds	r30, 0x0200
-    1e86:	f0 91 01 02 	lds	r31, 0x0201
-    1e8a:	30 97       	sbiw	r30, 0x00	; 0
-    1e8c:	19 f4       	brne	.+6      	; 0x1e94 <main+0x194>
-    1e8e:	20 91 04 02 	lds	r18, 0x0204
-    1e92:	13 c0       	rjmp	.+38     	; 0x1eba <main+0x1ba>
+    1e76:	e0 91 80 01 	lds	r30, 0x0180
+    1e7a:	f0 91 81 01 	lds	r31, 0x0181
+    1e7e:	30 97       	sbiw	r30, 0x00	; 0
+    1e80:	19 f4       	brne	.+6      	; 0x1e88 <main+0x188>
+    1e82:	20 91 84 01 	lds	r18, 0x0184
+    1e86:	13 c0       	rjmp	.+38     	; 0x1eae <main+0x1ae>
         else if (address == 1)  ch=rstVect >> 8;
-    1e94:	e1 30       	cpi	r30, 0x01	; 1
+    1e88:	e1 30       	cpi	r30, 0x01	; 1
+    1e8a:	f1 05       	cpc	r31, r1
+    1e8c:	19 f4       	brne	.+6      	; 0x1e94 <main+0x194>
+    1e8e:	20 91 85 01 	lds	r18, 0x0185
+    1e92:	0d c0       	rjmp	.+26     	; 0x1eae <main+0x1ae>
+        else if (address == 8)  ch=wdtVect & 0xff;
+    1e94:	e8 30       	cpi	r30, 0x08	; 8
     1e96:	f1 05       	cpc	r31, r1
     1e98:	19 f4       	brne	.+6      	; 0x1ea0 <main+0x1a0>
-    1e9a:	20 91 05 02 	lds	r18, 0x0205
-    1e9e:	0d c0       	rjmp	.+26     	; 0x1eba <main+0x1ba>
-        else if (address == 10)  ch=wdtVect & 0xff;
-    1ea0:	ea 30       	cpi	r30, 0x0A	; 10
+    1e9a:	20 91 86 01 	lds	r18, 0x0186
+    1e9e:	07 c0       	rjmp	.+14     	; 0x1eae <main+0x1ae>
+        else if (address == 9) ch=wdtVect >> 8;
+    1ea0:	e9 30       	cpi	r30, 0x09	; 9
     1ea2:	f1 05       	cpc	r31, r1
     1ea4:	19 f4       	brne	.+6      	; 0x1eac <main+0x1ac>
-    1ea6:	20 91 06 02 	lds	r18, 0x0206
-    1eaa:	07 c0       	rjmp	.+14     	; 0x1eba <main+0x1ba>
-        else if (address == 11) ch=wdtVect >> 8;
-    1eac:	eb 30       	cpi	r30, 0x0B	; 11
-    1eae:	f1 05       	cpc	r31, r1
-    1eb0:	19 f4       	brne	.+6      	; 0x1eb8 <main+0x1b8>
-    1eb2:	20 91 07 02 	lds	r18, 0x0207
-    1eb6:	01 c0       	rjmp	.+2      	; 0x1eba <main+0x1ba>
+    1ea6:	20 91 87 01 	lds	r18, 0x0187
+    1eaa:	01 c0       	rjmp	.+2      	; 0x1eae <main+0x1ae>
         else ch = pgm_read_byte_near(address);
-    1eb8:	24 91       	lpm	r18, Z+
+    1eac:	24 91       	lpm	r18, Z+
         address++;
-    1eba:	80 91 00 02 	lds	r24, 0x0200
-    1ebe:	90 91 01 02 	lds	r25, 0x0201
-    1ec2:	01 96       	adiw	r24, 0x01	; 1
-    1ec4:	90 93 01 02 	sts	0x0201, r25
-    1ec8:	80 93 00 02 	sts	0x0200, r24
+    1eae:	80 91 80 01 	lds	r24, 0x0180
+    1eb2:	90 91 81 01 	lds	r25, 0x0181
+    1eb6:	01 96       	adiw	r24, 0x01	; 1
+    1eb8:	90 93 81 01 	sts	0x0181, r25
+    1ebc:	80 93 80 01 	sts	0x0180, r24
         putch(ch);
-    1ecc:	82 2f       	mov	r24, r18
-    1ece:	19 d0       	rcall	.+50     	; 0x1f02 <putch>
+    1ec0:	82 2f       	mov	r24, r18
+    1ec2:	19 d0       	rcall	.+50     	; 0x1ef6 <putch>
       } while (--length);
-    1ed0:	80 91 02 02 	lds	r24, 0x0202
-    1ed4:	81 50       	subi	r24, 0x01	; 1
-    1ed6:	80 93 02 02 	sts	0x0202, r24
-    1eda:	88 23       	and	r24, r24
-    1edc:	91 f6       	brne	.-92     	; 0x1e82 <main+0x182>
-    1ede:	0e c0       	rjmp	.+28     	; 0x1efc <main+0x1fc>
-      while (--length);
+    1ec4:	80 91 82 01 	lds	r24, 0x0182
+    1ec8:	81 50       	subi	r24, 0x01	; 1
+    1eca:	80 93 82 01 	sts	0x0182, r24
+    1ece:	88 23       	and	r24, r24
+    1ed0:	91 f6       	brne	.-92     	; 0x1e76 <main+0x176>
+    1ed2:	0e c0       	rjmp	.+28     	; 0x1ef0 <main+0x1f0>
+#endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    1ee0:	85 37       	cpi	r24, 0x75	; 117
-    1ee2:	39 f4       	brne	.+14     	; 0x1ef2 <main+0x1f2>
+    1ed4:	85 37       	cpi	r24, 0x75	; 117
+    1ed6:	39 f4       	brne	.+14     	; 0x1ee6 <main+0x1e6>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    1ee4:	3f d0       	rcall	.+126    	; 0x1f64 <verifySpace>
+    1ed8:	3f d0       	rcall	.+126    	; 0x1f58 <verifySpace>
       putch(SIGNATURE_0);
-    1ee6:	8e e1       	ldi	r24, 0x1E	; 30
-    1ee8:	0c d0       	rcall	.+24     	; 0x1f02 <putch>
+    1eda:	8e e1       	ldi	r24, 0x1E	; 30
+    1edc:	0c d0       	rcall	.+24     	; 0x1ef6 <putch>
       putch(SIGNATURE_1);
-    1eea:	83 e9       	ldi	r24, 0x93	; 147
-    1eec:	0a d0       	rcall	.+20     	; 0x1f02 <putch>
+    1ede:	83 e9       	ldi	r24, 0x93	; 147
+    1ee0:	0a d0       	rcall	.+20     	; 0x1ef6 <putch>
       putch(SIGNATURE_2);
-    1eee:	8c e0       	ldi	r24, 0x0C	; 12
-    1ef0:	51 cf       	rjmp	.-350    	; 0x1d94 <main+0x94>
+    1ee2:	8c e0       	ldi	r24, 0x0C	; 12
+    1ee4:	54 cf       	rjmp	.-344    	; 0x1d8e <main+0x8e>
     }
     else if (ch == 'Q') {
-    1ef2:	81 35       	cpi	r24, 0x51	; 81
-    1ef4:	11 f4       	brne	.+4      	; 0x1efa <main+0x1fa>
+    1ee6:	81 35       	cpi	r24, 0x51	; 81
+    1ee8:	11 f4       	brne	.+4      	; 0x1eee <main+0x1ee>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    1ef6:	88 e0       	ldi	r24, 0x08	; 8
-    1ef8:	2c d0       	rcall	.+88     	; 0x1f52 <watchdogConfig>
+    1eea:	88 e0       	ldi	r24, 0x08	; 8
+    1eec:	2c d0       	rcall	.+88     	; 0x1f46 <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    1efa:	34 d0       	rcall	.+104    	; 0x1f64 <verifySpace>
+    1eee:	34 d0       	rcall	.+104    	; 0x1f58 <verifySpace>
     }
     putch(STK_OK);
-    1efc:	80 e1       	ldi	r24, 0x10	; 16
-    1efe:	01 d0       	rcall	.+2      	; 0x1f02 <putch>
-    1f00:	1e cf       	rjmp	.-452    	; 0x1d3e <main+0x3e>
+    1ef0:	80 e1       	ldi	r24, 0x10	; 16
+    1ef2:	01 d0       	rcall	.+2      	; 0x1ef6 <putch>
+    1ef4:	25 cf       	rjmp	.-438    	; 0x1d40 <main+0x40>
 
-00001f02 <putch>:
+00001ef6 <putch>:
 void putch(char ch) {
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
   UDR0 = ch;
 #else
   __asm__ __volatile__ (
-    1f02:	2a e0       	ldi	r18, 0x0A	; 10
-    1f04:	30 e0       	ldi	r19, 0x00	; 0
-    1f06:	80 95       	com	r24
-    1f08:	08 94       	sec
-    1f0a:	10 f4       	brcc	.+4      	; 0x1f10 <putch+0xe>
-    1f0c:	da 98       	cbi	0x1b, 2	; 27
-    1f0e:	02 c0       	rjmp	.+4      	; 0x1f14 <putch+0x12>
-    1f10:	da 9a       	sbi	0x1b, 2	; 27
-    1f12:	00 00       	nop
-    1f14:	15 d0       	rcall	.+42     	; 0x1f40 <uartDelay>
-    1f16:	14 d0       	rcall	.+40     	; 0x1f40 <uartDelay>
-    1f18:	86 95       	lsr	r24
-    1f1a:	2a 95       	dec	r18
-    1f1c:	b1 f7       	brne	.-20     	; 0x1f0a <putch+0x8>
+    1ef6:	2a e0       	ldi	r18, 0x0A	; 10
+    1ef8:	30 e0       	ldi	r19, 0x00	; 0
+    1efa:	80 95       	com	r24
+    1efc:	08 94       	sec
+    1efe:	10 f4       	brcc	.+4      	; 0x1f04 <putch+0xe>
+    1f00:	da 98       	cbi	0x1b, 2	; 27
+    1f02:	02 c0       	rjmp	.+4      	; 0x1f08 <putch+0x12>
+    1f04:	da 9a       	sbi	0x1b, 2	; 27
+    1f06:	00 00       	nop
+    1f08:	15 d0       	rcall	.+42     	; 0x1f34 <uartDelay>
+    1f0a:	14 d0       	rcall	.+40     	; 0x1f34 <uartDelay>
+    1f0c:	86 95       	lsr	r24
+    1f0e:	2a 95       	dec	r18
+    1f10:	b1 f7       	brne	.-20     	; 0x1efe <putch+0x8>
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    1f1e:	08 95       	ret
+    1f12:	08 95       	ret
 
-00001f20 <getch>:
+00001f14 <getch>:
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    1f20:	a8 95       	wdr
-#ifdef LED_DATA_FLASH
+    1f14:	a8 95       	wdr
   LED_PIN |= _BV(LED);
 #endif
+#endif
 
   return ch;
 }
-    1f22:	29 e0       	ldi	r18, 0x09	; 9
-    1f24:	30 e0       	ldi	r19, 0x00	; 0
+    1f16:	29 e0       	ldi	r18, 0x09	; 9
+    1f18:	30 e0       	ldi	r19, 0x00	; 0
+    1f1a:	cb 99       	sbic	0x19, 3	; 25
+    1f1c:	fe cf       	rjmp	.-4      	; 0x1f1a <getch+0x6>
+    1f1e:	0a d0       	rcall	.+20     	; 0x1f34 <uartDelay>
+    1f20:	09 d0       	rcall	.+18     	; 0x1f34 <uartDelay>
+    1f22:	08 d0       	rcall	.+16     	; 0x1f34 <uartDelay>
+    1f24:	88 94       	clc
     1f26:	cb 99       	sbic	0x19, 3	; 25
-    1f28:	fe cf       	rjmp	.-4      	; 0x1f26 <getch+0x6>
-    1f2a:	0a d0       	rcall	.+20     	; 0x1f40 <uartDelay>
-    1f2c:	09 d0       	rcall	.+18     	; 0x1f40 <uartDelay>
-    1f2e:	08 d0       	rcall	.+16     	; 0x1f40 <uartDelay>
-    1f30:	88 94       	clc
-    1f32:	cb 99       	sbic	0x19, 3	; 25
-    1f34:	08 94       	sec
-    1f36:	2a 95       	dec	r18
-    1f38:	11 f0       	breq	.+4      	; 0x1f3e <getch+0x1e>
-    1f3a:	87 95       	ror	r24
-    1f3c:	f7 cf       	rjmp	.-18     	; 0x1f2c <getch+0xc>
-    1f3e:	08 95       	ret
-
-00001f40 <uartDelay>:
+    1f28:	08 94       	sec
+    1f2a:	2a 95       	dec	r18
+    1f2c:	11 f0       	breq	.+4      	; 0x1f32 <getch+0x1e>
+    1f2e:	87 95       	ror	r24
+    1f30:	f7 cf       	rjmp	.-18     	; 0x1f20 <getch+0xc>
+    1f32:	08 95       	ret
+
+00001f34 <uartDelay>:
 #if UART_B_VALUE > 255
 #error Baud rate too slow for soft UART
 #endif
 
 void uartDelay() {
   __asm__ __volatile__ (
-    1f40:	9e e0       	ldi	r25, 0x0E	; 14
-    1f42:	9a 95       	dec	r25
-    1f44:	f1 f7       	brne	.-4      	; 0x1f42 <uartDelay+0x2>
-    1f46:	08 95       	ret
+    1f34:	9e e0       	ldi	r25, 0x0E	; 14
+    1f36:	9a 95       	dec	r25
+    1f38:	f1 f7       	brne	.-4      	; 0x1f36 <uartDelay+0x2>
+    1f3a:	08 95       	ret
 
-00001f48 <getLen>:
+00001f3c <getLen>:
   } while (--count);
 }
 #endif
 
 uint8_t getLen() {
   getch();
-    1f48:	eb df       	rcall	.-42     	; 0x1f20 <getch>
+    1f3c:	eb df       	rcall	.-42     	; 0x1f14 <getch>
   length = getch();
-    1f4a:	ea df       	rcall	.-44     	; 0x1f20 <getch>
-    1f4c:	80 93 02 02 	sts	0x0202, r24
+    1f3e:	ea df       	rcall	.-44     	; 0x1f14 <getch>
+    1f40:	80 93 82 01 	sts	0x0182, r24
   return getch();
 }
-    1f50:	e7 cf       	rjmp	.-50     	; 0x1f20 <getch>
+    1f44:	e7 cf       	rjmp	.-50     	; 0x1f14 <getch>
 
-00001f52 <watchdogConfig>:
+00001f46 <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    1f52:	98 e1       	ldi	r25, 0x18	; 24
-    1f54:	91 bd       	out	0x21, r25	; 33
+    1f46:	98 e1       	ldi	r25, 0x18	; 24
+    1f48:	91 bd       	out	0x21, r25	; 33
   WDTCSR = x;
-    1f56:	81 bd       	out	0x21, r24	; 33
+    1f4a:	81 bd       	out	0x21, r24	; 33
 }
-    1f58:	08 95       	ret
+    1f4c:	08 95       	ret
 
-00001f5a <appStart>:
+00001f4e <appStart>:
 
 void appStart() {
   watchdogConfig(WATCHDOG_OFF);
-    1f5a:	80 e0       	ldi	r24, 0x00	; 0
-    1f5c:	fa df       	rcall	.-12     	; 0x1f52 <watchdogConfig>
+    1f4e:	80 e0       	ldi	r24, 0x00	; 0
+    1f50:	fa df       	rcall	.-12     	; 0x1f46 <watchdogConfig>
   __asm__ __volatile__ (
-    1f5e:	e5 e0       	ldi	r30, 0x05	; 5
-    1f60:	ff 27       	eor	r31, r31
-    1f62:	09 94       	ijmp
+    1f52:	e4 e0       	ldi	r30, 0x04	; 4
+    1f54:	ff 27       	eor	r31, r31
+    1f56:	09 94       	ijmp
 
-00001f64 <verifySpace>:
+00001f58 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) appStart();
-    1f64:	dd df       	rcall	.-70     	; 0x1f20 <getch>
-    1f66:	80 32       	cpi	r24, 0x20	; 32
-    1f68:	09 f0       	breq	.+2      	; 0x1f6c <verifySpace+0x8>
-    1f6a:	f7 df       	rcall	.-18     	; 0x1f5a <appStart>
+    1f58:	dd df       	rcall	.-70     	; 0x1f14 <getch>
+    1f5a:	80 32       	cpi	r24, 0x20	; 32
+    1f5c:	09 f0       	breq	.+2      	; 0x1f60 <verifySpace+0x8>
+    1f5e:	f7 df       	rcall	.-18     	; 0x1f4e <appStart>
   putch(STK_INSYNC);
-    1f6c:	84 e1       	ldi	r24, 0x14	; 20
+    1f60:	84 e1       	ldi	r24, 0x14	; 20
 }
-    1f6e:	c9 cf       	rjmp	.-110    	; 0x1f02 <putch>
+    1f62:	c9 cf       	rjmp	.-110    	; 0x1ef6 <putch>
+
+00001f64 <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    1f70:	1f 93       	push	r17
-    1f72:	18 2f       	mov	r17, r24
-
-00001f74 <getNch>:
+    1f64:	1f 93       	push	r17
+    1f66:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    1f74:	d5 df       	rcall	.-86     	; 0x1f20 <getch>
-    1f76:	11 50       	subi	r17, 0x01	; 1
-    1f78:	e9 f7       	brne	.-6      	; 0x1f74 <getNch>
+    1f68:	d5 df       	rcall	.-86     	; 0x1f14 <getch>
+    1f6a:	11 50       	subi	r17, 0x01	; 1
+    1f6c:	e9 f7       	brne	.-6      	; 0x1f68 <getNch+0x4>
   verifySpace();
-    1f7a:	f4 df       	rcall	.-24     	; 0x1f64 <verifySpace>
+    1f6e:	f4 df       	rcall	.-24     	; 0x1f58 <verifySpace>
 }
-    1f7c:	1f 91       	pop	r17
-    1f7e:	08 95       	ret
+    1f70:	1f 91       	pop	r17
+    1f72:	08 95       	ret
diff --git a/bootloaders/optiboot/optiboot_pro_16MHz.hex b/bootloaders/optiboot/optiboot_pro_16MHz.hex
index 1e93414..2b4a582 100644
--- a/bootloaders/optiboot/optiboot_pro_16MHz.hex
+++ b/bootloaders/optiboot/optiboot_pro_16MHz.hex
@@ -1,33 +1,34 @@
-:103E000085E08093810082E08093C00088E1809308

-:103E1000C10086E08093C20080E18093C40084B733

-:103E200014BE81FFD0D08DE0C8D0259A86E020E373

-:103E30003CEF91E0309385002093840096BBB09BCB

-:103E4000FECF1D9AA8958150A9F7DD24D394A5E053

-:103E5000EA2EF1E1FF2EA4D0813421F481E0BED01E

-:103E600083E024C0823411F484E103C0853419F462

-:103E700085E0B4D08AC08535A1F492D0082F10E037

-:103E800010930102009300028BD090E0982F8827B6

-:103E9000802B912B880F991F909301028093000231

-:103EA00073C0863529F484E099D080E071D06DC06C

-:103EB000843609F043C07CD0E0910002F091010209

-:103EC00083E080935700E895C0E0D1E069D0899302

-:103ED000809102028150809302028823B9F778D042

-:103EE00007B600FCFDCF4091000250910102A0E016

-:103EF000B1E02C9130E011968C91119790E0982FC1

-:103F00008827822B932B1296FA010C01D09257002E

-:103F1000E89511244E5F5F4FF1E0A038BF0749F7E5

-:103F2000E0910002F0910102E0925700E89507B697

-:103F300000FCFDCFF0925700E89527C08437B9F414

-:103F400037D046D0E0910002F09101023196F09313

-:103F50000102E09300023197E4918E2F19D08091F5

-:103F60000202815080930202882361F70EC08537D8

-:103F700039F42ED08EE10CD084E90AD086E096CFB9

-:103F8000813511F488E019D023D080E101D063CFCE

-:103F9000982F8091C00085FFFCCF9093C6000895B4

-:103FA000A8958091C00087FFFCCF8091C60008953E

-:103FB000F7DFF6DF80930202F3CFE0E6F0E098E16E

-:103FC00090838083089580E0F8DFEE27FF2709942F

-:103FD000E7DF803209F0F7DF84E1DACF1F93182F93

-:0C3FE000DFDF1150E9F7F4DF1F910895B6

+:103E0000112484B714BE81FFE6D085E08093810041

+:103E100082E08093C00088E18093C10086E08093B7

+:103E2000C20080E18093C4008EE0CFD0259A86E066

+:103E300020E33CEF91E0309385002093840096BB13

+:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D

+:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020

+:103E6000C5D083E020C0823411F484E103C08534DE

+:103E700019F485E0BBD091C0853581F499D0082F25

+:103E800010E096D090E0982F8827802B912B880FF8

+:103E9000991F90930102809300027EC0863529F419

+:103EA00084E0A4D080E07CD078C0843609F04EC095

+:103EB00087D0E0910002F091010288E3E030F8073A

+:103EC00018F483E087BFE895C0E0D1E071D0899312

+:103ED000809102028150809302028823B9F7E09119

+:103EE0000002F091010288E3E030F80718F083E067

+:103EF00087BFE89575D007B600FCFDCF4091000262

+:103F000050910102A0E0B1E02C9130E011968C912B

+:103F1000119790E0982F8827822B932B1296FA0105

+:103F20000C01D7BEE89511244E5F5F4FF1E0A03839

+:103F3000BF0751F7E0910002F0910102E7BEE8955A

+:103F400007B600FCFDCFF7BEE89527C08437B9F46B

+:103F500037D046D0E0910002F09101023196F09303

+:103F60000102E09300023197E4918E2F19D08091E5

+:103F70000202815080930202882361F70EC08537C8

+:103F800039F42ED08EE10CD084E90AD086E08BCFB4

+:103F9000813511F488E019D023D080E101D05CCFC5

+:103FA000982F8091C00085FFFCCF9093C6000895A4

+:103FB000A8958091C00087FFFCCF8091C60008952E

+:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E

+:103FD00090838083089580E0F8DFEE27FF2709941F

+:103FE000E7DF803209F0F7DF84E1DACF1F93182F83

+:0C3FF000DFDF1150E9F7F4DF1F910895A6

 :0400000300003E00BB

 :00000001FF

diff --git a/bootloaders/optiboot/optiboot_pro_16MHz.lst b/bootloaders/optiboot/optiboot_pro_16MHz.lst
index 9920a76..25dc46b 100644
--- a/bootloaders/optiboot/optiboot_pro_16MHz.lst
+++ b/bootloaders/optiboot/optiboot_pro_16MHz.lst
@@ -3,237 +3,257 @@ optiboot_pro_16MHz.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001ec  00003e00  00003e00  00000054  2**1
+  0 .text         000001fc  00003e00  00003e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .debug_aranges 00000028  00000000  00000000  00000240  2**0
+  1 .debug_aranges 00000028  00000000  00000000  00000250  2**0
                   CONTENTS, READONLY, DEBUGGING
-  2 .debug_pubnames 0000006a  00000000  00000000  00000268  2**0
+  2 .debug_pubnames 0000006a  00000000  00000000  00000278  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_info   00000269  00000000  00000000  000002d2  2**0
+  3 .debug_info   00000284  00000000  00000000  000002e2  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_abbrev 00000196  00000000  00000000  0000053b  2**0
+  4 .debug_abbrev 000001ae  00000000  00000000  00000566  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_line   000003d3  00000000  00000000  000006d1  2**0
+  5 .debug_line   00000450  00000000  00000000  00000714  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_frame  00000090  00000000  00000000  00000aa4  2**2
+  6 .debug_frame  00000090  00000000  00000000  00000b64  2**2
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_str    00000135  00000000  00000000  00000b34  2**0
+  7 .debug_str    00000141  00000000  00000000  00000bf4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_loc    000001d1  00000000  00000000  00000c69  2**0
+  8 .debug_loc    000001e1  00000000  00000000  00000d35  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_ranges 00000068  00000000  00000000  00000e3a  2**0
+  9 .debug_ranges 00000068  00000000  00000000  00000f16  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00003e00 <main>:
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3e00:	85 e0       	ldi	r24, 0x05	; 5
-    3e02:	80 93 81 00 	sts	0x0081, r24
+    3e00:	11 24       	eor	r1, r1
+#ifdef __AVR_ATmega8__
+  SP=RAMEND;  // This is done by hardware reset
+#endif
+
+  // Adaboot no-wait mod
+  ch = MCUSR;
+    3e02:	84 b7       	in	r24, 0x34	; 52
+  MCUSR = 0;
+    3e04:	14 be       	out	0x34, r1	; 52
+  if (!(ch & _BV(EXTRF))) appStart();
+    3e06:	81 ff       	sbrs	r24, 1
+    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-#endif
-#ifndef SOFT_UART
+    3e0a:	85 e0       	ldi	r24, 0x05	; 5
+    3e0c:	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
   UCSR0A = _BV(U2X0); //Double speed mode USART0
-    3e06:	82 e0       	ldi	r24, 0x02	; 2
-    3e08:	80 93 c0 00 	sts	0x00C0, r24
+    3e10:	82 e0       	ldi	r24, 0x02	; 2
+    3e12:	80 93 c0 00 	sts	0x00C0, r24
   UCSR0B = _BV(RXEN0) | _BV(TXEN0);
-    3e0c:	88 e1       	ldi	r24, 0x18	; 24
-    3e0e:	80 93 c1 00 	sts	0x00C1, r24
+    3e16:	88 e1       	ldi	r24, 0x18	; 24
+    3e18:	80 93 c1 00 	sts	0x00C1, r24
   UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
-    3e12:	86 e0       	ldi	r24, 0x06	; 6
-    3e14:	80 93 c2 00 	sts	0x00C2, r24
+    3e1c:	86 e0       	ldi	r24, 0x06	; 6
+    3e1e:	80 93 c2 00 	sts	0x00C2, r24
   UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    3e18:	80 e1       	ldi	r24, 0x10	; 16
-    3e1a:	80 93 c4 00 	sts	0x00C4, r24
+    3e22:	80 e1       	ldi	r24, 0x10	; 16
+    3e24:	80 93 c4 00 	sts	0x00C4, r24
+#endif
 #endif
-
-  // Adaboot no-wait mod
-  ch = MCUSR;
-    3e1e:	84 b7       	in	r24, 0x34	; 52
-  MCUSR = 0;
-    3e20:	14 be       	out	0x34, r1	; 52
-  if (!(ch & _BV(EXTRF))) appStart();
-    3e22:	81 ff       	sbrs	r24, 1
-    3e24:	d0 d0       	rcall	.+416    	; 0x3fc6 <appStart>
 
   // Set up watchdog to trigger after 500ms
-  watchdogConfig(WATCHDOG_500MS);
-    3e26:	8d e0       	ldi	r24, 0x0D	; 13
-    3e28:	c8 d0       	rcall	.+400    	; 0x3fba <watchdogConfig>
+  watchdogConfig(WATCHDOG_1S);
+    3e28:	8e e0       	ldi	r24, 0x0E	; 14
+    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    3e2a:	25 9a       	sbi	0x04, 5	; 4
-    3e2c:	86 e0       	ldi	r24, 0x06	; 6
+    3e2c:	25 9a       	sbi	0x04, 5	; 4
+    3e2e:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e2e:	20 e3       	ldi	r18, 0x30	; 48
-    3e30:	3c ef       	ldi	r19, 0xFC	; 252
+    3e30:	20 e3       	ldi	r18, 0x30	; 48
+    3e32:	3c ef       	ldi	r19, 0xFC	; 252
     TIFR1 = _BV(TOV1);
-    3e32:	91 e0       	ldi	r25, 0x01	; 1
+    3e34:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e34:	30 93 85 00 	sts	0x0085, r19
-    3e38:	20 93 84 00 	sts	0x0084, r18
+    3e36:	30 93 85 00 	sts	0x0085, r19
+    3e3a:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    3e3c:	96 bb       	out	0x16, r25	; 22
+    3e3e:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    3e3e:	b0 9b       	sbis	0x16, 0	; 22
-    3e40:	fe cf       	rjmp	.-4      	; 0x3e3e <main+0x3e>
+    3e40:	b0 9b       	sbis	0x16, 0	; 22
+    3e42:	fe cf       	rjmp	.-4      	; 0x3e40 <main+0x40>
+#ifdef __AVR_ATmega8__
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
-    3e42:	1d 9a       	sbi	0x03, 5	; 3
+    3e44:	1d 9a       	sbi	0x03, 5	; 3
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3e44:	a8 95       	wdr
-    TCNT1 = -(F_CPU/(1024*16));
-    TIFR1 = _BV(TOV1);
-    while(!(TIFR1 & _BV(TOV1)));
+    3e46:	a8 95       	wdr
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
+#endif
     watchdogReset();
   } while (--count);
-    3e46:	81 50       	subi	r24, 0x01	; 1
-    3e48:	a9 f7       	brne	.-22     	; 0x3e34 <main+0x34>
+    3e48:	81 50       	subi	r24, 0x01	; 1
+    3e4a:	a9 f7       	brne	.-22     	; 0x3e36 <main+0x36>
     /* get character from UART */
     ch = getch();
 
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e4a:	dd 24       	eor	r13, r13
-    3e4c:	d3 94       	inc	r13
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
+    3e4c:	dd 24       	eor	r13, r13
+    3e4e:	d3 94       	inc	r13
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3e4e:	a5 e0       	ldi	r26, 0x05	; 5
-    3e50:	ea 2e       	mov	r14, r26
+      __boot_page_write_short((uint16_t)(void*)address);
+    3e50:	a5 e0       	ldi	r26, 0x05	; 5
+    3e52:	ea 2e       	mov	r14, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3e52:	f1 e1       	ldi	r31, 0x11	; 17
-    3e54:	ff 2e       	mov	r15, r31
+    3e54:	f1 e1       	ldi	r31, 0x11	; 17
+    3e56:	ff 2e       	mov	r15, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e56:	a4 d0       	rcall	.+328    	; 0x3fa0 <getch>
+    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    3e58:	81 34       	cpi	r24, 0x41	; 65
-    3e5a:	21 f4       	brne	.+8      	; 0x3e64 <main+0x64>
+    3e5a:	81 34       	cpi	r24, 0x41	; 65
+    3e5c:	21 f4       	brne	.+8      	; 0x3e66 <main+0x66>
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e5c:	81 e0       	ldi	r24, 0x01	; 1
-    3e5e:	be d0       	rcall	.+380    	; 0x3fdc <verifySpace+0xc>
+    3e5e:	81 e0       	ldi	r24, 0x01	; 1
+    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
       putch(0x03);
-    3e60:	83 e0       	ldi	r24, 0x03	; 3
-    3e62:	24 c0       	rjmp	.+72     	; 0x3eac <main+0xac>
+    3e62:	83 e0       	ldi	r24, 0x03	; 3
+    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
     }
     else if(ch == STK_SET_DEVICE) {
-    3e64:	82 34       	cpi	r24, 0x42	; 66
-    3e66:	11 f4       	brne	.+4      	; 0x3e6c <main+0x6c>
+    3e66:	82 34       	cpi	r24, 0x42	; 66
+    3e68:	11 f4       	brne	.+4      	; 0x3e6e <main+0x6e>
       // SET DEVICE is ignored
       getNch(20);
-    3e68:	84 e1       	ldi	r24, 0x14	; 20
-    3e6a:	03 c0       	rjmp	.+6      	; 0x3e72 <main+0x72>
+    3e6a:	84 e1       	ldi	r24, 0x14	; 20
+    3e6c:	03 c0       	rjmp	.+6      	; 0x3e74 <main+0x74>
     }
     else if(ch == STK_SET_DEVICE_EXT) {
-    3e6c:	85 34       	cpi	r24, 0x45	; 69
-    3e6e:	19 f4       	brne	.+6      	; 0x3e76 <main+0x76>
+    3e6e:	85 34       	cpi	r24, 0x45	; 69
+    3e70:	19 f4       	brne	.+6      	; 0x3e78 <main+0x78>
       // SET DEVICE EXT is ignored
       getNch(5);
-    3e70:	85 e0       	ldi	r24, 0x05	; 5
-    3e72:	b4 d0       	rcall	.+360    	; 0x3fdc <verifySpace+0xc>
-    3e74:	8a c0       	rjmp	.+276    	; 0x3f8a <main+0x18a>
+    3e72:	85 e0       	ldi	r24, 0x05	; 5
+    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
+    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
-    3e76:	85 35       	cpi	r24, 0x55	; 85
-    3e78:	a1 f4       	brne	.+40     	; 0x3ea2 <main+0xa2>
+    3e78:	85 35       	cpi	r24, 0x55	; 85
+    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
       // LOAD ADDRESS
-      address = getch();
-    3e7a:	92 d0       	rcall	.+292    	; 0x3fa0 <getch>
-    3e7c:	08 2f       	mov	r16, r24
-    3e7e:	10 e0       	ldi	r17, 0x00	; 0
-    3e80:	10 93 01 02 	sts	0x0201, r17
-    3e84:	00 93 00 02 	sts	0x0200, r16
-      address = (address & 0xff) | (getch() << 8);
-    3e88:	8b d0       	rcall	.+278    	; 0x3fa0 <getch>
-    3e8a:	90 e0       	ldi	r25, 0x00	; 0
-    3e8c:	98 2f       	mov	r25, r24
-    3e8e:	88 27       	eor	r24, r24
-    3e90:	80 2b       	or	r24, r16
-    3e92:	91 2b       	or	r25, r17
-      address += address; // Convert from word address to byte address
-    3e94:	88 0f       	add	r24, r24
-    3e96:	99 1f       	adc	r25, r25
-    3e98:	90 93 01 02 	sts	0x0201, r25
-    3e9c:	80 93 00 02 	sts	0x0200, r24
-    3ea0:	73 c0       	rjmp	.+230    	; 0x3f88 <main+0x188>
+      uint16_t newAddress;
+      newAddress = getch();
+    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+      newAddress = (newAddress & 0xff) | (getch() << 8);
+    3e7e:	08 2f       	mov	r16, r24
+    3e80:	10 e0       	ldi	r17, 0x00	; 0
+    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
+    3e84:	90 e0       	ldi	r25, 0x00	; 0
+    3e86:	98 2f       	mov	r25, r24
+    3e88:	88 27       	eor	r24, r24
+    3e8a:	80 2b       	or	r24, r16
+    3e8c:	91 2b       	or	r25, r17
+#ifdef RAMPZ
+      // Transfer top bit to RAMPZ
+      RAMPZ = (newAddress & 0x8000) ? 1 : 0;
+#endif
+      newAddress += newAddress; // Convert from word address to byte address
+    3e8e:	88 0f       	add	r24, r24
+    3e90:	99 1f       	adc	r25, r25
+      address = newAddress;
+    3e92:	90 93 01 02 	sts	0x0201, r25
+    3e96:	80 93 00 02 	sts	0x0200, r24
+    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
     }
     else if(ch == STK_UNIVERSAL) {
-    3ea2:	86 35       	cpi	r24, 0x56	; 86
-    3ea4:	29 f4       	brne	.+10     	; 0x3eb0 <main+0xb0>
+    3e9c:	86 35       	cpi	r24, 0x56	; 86
+    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
       // UNIVERSAL command is ignored
       getNch(4);
-    3ea6:	84 e0       	ldi	r24, 0x04	; 4
-    3ea8:	99 d0       	rcall	.+306    	; 0x3fdc <verifySpace+0xc>
+    3ea0:	84 e0       	ldi	r24, 0x04	; 4
+    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
       putch(0x00);
-    3eaa:	80 e0       	ldi	r24, 0x00	; 0
-    3eac:	71 d0       	rcall	.+226    	; 0x3f90 <putch>
-    3eae:	6d c0       	rjmp	.+218    	; 0x3f8a <main+0x18a>
+    3ea4:	80 e0       	ldi	r24, 0x00	; 0
+    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
+    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
     }
-    /* Write memory, length is big endian and is in bytes  */
+    /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3eb0:	84 36       	cpi	r24, 0x64	; 100
-    3eb2:	09 f0       	breq	.+2      	; 0x3eb6 <main+0xb6>
-    3eb4:	43 c0       	rjmp	.+134    	; 0x3f3c <main+0x13c>
+    3eaa:	84 36       	cpi	r24, 0x64	; 100
+    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
+    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
       getLen();
-    3eb6:	7c d0       	rcall	.+248    	; 0x3fb0 <getLen>
-
-      // Immediately start page erase - this will 4.5ms
-      boot_page_erase((uint16_t)(void*)address);
-    3eb8:	e0 91 00 02 	lds	r30, 0x0200
-    3ebc:	f0 91 01 02 	lds	r31, 0x0201
-    3ec0:	83 e0       	ldi	r24, 0x03	; 3
-    3ec2:	80 93 57 00 	sts	0x0057, r24
+    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+
+      // If we are in RWW section, immediately start page erase
+      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3eb2:	e0 91 00 02 	lds	r30, 0x0200
+    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eba:	88 e3       	ldi	r24, 0x38	; 56
+    3ebc:	e0 30       	cpi	r30, 0x00	; 0
+    3ebe:	f8 07       	cpc	r31, r24
+    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
+    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ec4:	87 bf       	out	0x37, r24	; 55
     3ec6:	e8 95       	spm
     3ec8:	c0 e0       	ldi	r28, 0x00	; 0
     3eca:	d1 e0       	ldi	r29, 0x01	; 1
-
+      
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ecc:	69 d0       	rcall	.+210    	; 0x3fa0 <getch>
+    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
     3ece:	89 93       	st	Y+, r24
       while (--length);
     3ed0:	80 91 02 02 	lds	r24, 0x0202
@@ -242,279 +262,293 @@ void watchdogReset() {
     3eda:	88 23       	and	r24, r24
     3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
 
+      // If we are in NRWW section, page erase has to be delayed until now.
+      // Todo: Take RAMPZ into account
+      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3ede:	e0 91 00 02 	lds	r30, 0x0200
+    3ee2:	f0 91 01 02 	lds	r31, 0x0201
+    3ee6:	88 e3       	ldi	r24, 0x38	; 56
+    3ee8:	e0 30       	cpi	r30, 0x00	; 0
+    3eea:	f8 07       	cpc	r31, r24
+    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
+    3eee:	83 e0       	ldi	r24, 0x03	; 3
+    3ef0:	87 bf       	out	0x37, r24	; 55
+    3ef2:	e8 95       	spm
+
       // Read command terminator, start reply
       verifySpace();
-    3ede:	78 d0       	rcall	.+240    	; 0x3fd0 <verifySpace>
+    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
       
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3ee0:	07 b6       	in	r0, 0x37	; 55
-    3ee2:	00 fc       	sbrc	r0, 0
-    3ee4:	fd cf       	rjmp	.-6      	; 0x3ee0 <main+0xe0>
+    3ef6:	07 b6       	in	r0, 0x37	; 55
+    3ef8:	00 fc       	sbrc	r0, 0
+    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
       }
 #endif
 
       // Copy buffer into programming buffer
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3ee6:	40 91 00 02 	lds	r20, 0x0200
-    3eea:	50 91 01 02 	lds	r21, 0x0201
-    3eee:	a0 e0       	ldi	r26, 0x00	; 0
-    3ef0:	b1 e0       	ldi	r27, 0x01	; 1
+    3efc:	40 91 00 02 	lds	r20, 0x0200
+    3f00:	50 91 01 02 	lds	r21, 0x0201
+    3f04:	a0 e0       	ldi	r26, 0x00	; 0
+    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3ef2:	2c 91       	ld	r18, X
-    3ef4:	30 e0       	ldi	r19, 0x00	; 0
+    3f08:	2c 91       	ld	r18, X
+    3f0a:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3ef6:	11 96       	adiw	r26, 0x01	; 1
-    3ef8:	8c 91       	ld	r24, X
-    3efa:	11 97       	sbiw	r26, 0x01	; 1
-    3efc:	90 e0       	ldi	r25, 0x00	; 0
-    3efe:	98 2f       	mov	r25, r24
-    3f00:	88 27       	eor	r24, r24
-    3f02:	82 2b       	or	r24, r18
-    3f04:	93 2b       	or	r25, r19
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+    3f0c:	11 96       	adiw	r26, 0x01	; 1
+    3f0e:	8c 91       	ld	r24, X
+    3f10:	11 97       	sbiw	r26, 0x01	; 1
+    3f12:	90 e0       	ldi	r25, 0x00	; 0
+    3f14:	98 2f       	mov	r25, r24
+    3f16:	88 27       	eor	r24, r24
+    3f18:	82 2b       	or	r24, r18
+    3f1a:	93 2b       	or	r25, r19
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3f06:	12 96       	adiw	r26, 0x02	; 2
+    3f1c:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
-    3f08:	fa 01       	movw	r30, r20
-    3f0a:	0c 01       	movw	r0, r24
-    3f0c:	d0 92 57 00 	sts	0x0057, r13
-    3f10:	e8 95       	spm
-    3f12:	11 24       	eor	r1, r1
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3f1e:	fa 01       	movw	r30, r20
+    3f20:	0c 01       	movw	r0, r24
+    3f22:	d7 be       	out	0x37, r13	; 55
+    3f24:	e8 95       	spm
+    3f26:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3f14:	4e 5f       	subi	r20, 0xFE	; 254
-    3f16:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f28:	4e 5f       	subi	r20, 0xFE	; 254
+    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f18:	f1 e0       	ldi	r31, 0x01	; 1
-    3f1a:	a0 38       	cpi	r26, 0x80	; 128
-    3f1c:	bf 07       	cpc	r27, r31
-    3f1e:	49 f7       	brne	.-46     	; 0x3ef2 <main+0xf2>
+    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
+    3f2e:	a0 38       	cpi	r26, 0x80	; 128
+    3f30:	bf 07       	cpc	r27, r31
+    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3f20:	e0 91 00 02 	lds	r30, 0x0200
-    3f24:	f0 91 01 02 	lds	r31, 0x0201
-    3f28:	e0 92 57 00 	sts	0x0057, r14
-    3f2c:	e8 95       	spm
+      __boot_page_write_short((uint16_t)(void*)address);
+    3f34:	e0 91 00 02 	lds	r30, 0x0200
+    3f38:	f0 91 01 02 	lds	r31, 0x0201
+    3f3c:	e7 be       	out	0x37, r14	; 55
+    3f3e:	e8 95       	spm
       boot_spm_busy_wait();
-    3f2e:	07 b6       	in	r0, 0x37	; 55
-    3f30:	00 fc       	sbrc	r0, 0
-    3f32:	fd cf       	rjmp	.-6      	; 0x3f2e <main+0x12e>
+    3f40:	07 b6       	in	r0, 0x37	; 55
+    3f42:	00 fc       	sbrc	r0, 0
+    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f34:	f0 92 57 00 	sts	0x0057, r15
-    3f38:	e8 95       	spm
-    3f3a:	27 c0       	rjmp	.+78     	; 0x3f8a <main+0x18a>
+    3f46:	f7 be       	out	0x37, r15	; 55
+    3f48:	e8 95       	spm
+    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f3c:	84 37       	cpi	r24, 0x74	; 116
-    3f3e:	b9 f4       	brne	.+46     	; 0x3f6e <main+0x16e>
+    3f4c:	84 37       	cpi	r24, 0x74	; 116
+    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
       // READ PAGE - we only read flash
       getLen();
-    3f40:	37 d0       	rcall	.+110    	; 0x3fb0 <getLen>
+    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
       verifySpace();
-    3f42:	46 d0       	rcall	.+140    	; 0x3fd0 <verifySpace>
-        else ch = pgm_read_byte_near(address);
+    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+        putch(result);
         address++;
-        putch(ch);
-      } while (--length);
+      }
+      while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f44:	e0 91 00 02 	lds	r30, 0x0200
-    3f48:	f0 91 01 02 	lds	r31, 0x0201
-    3f4c:	31 96       	adiw	r30, 0x01	; 1
-    3f4e:	f0 93 01 02 	sts	0x0201, r31
-    3f52:	e0 93 00 02 	sts	0x0200, r30
-    3f56:	31 97       	sbiw	r30, 0x01	; 1
-    3f58:	e4 91       	lpm	r30, Z+
-    3f5a:	8e 2f       	mov	r24, r30
-    3f5c:	19 d0       	rcall	.+50     	; 0x3f90 <putch>
+    3f54:	e0 91 00 02 	lds	r30, 0x0200
+    3f58:	f0 91 01 02 	lds	r31, 0x0201
+    3f5c:	31 96       	adiw	r30, 0x01	; 1
+    3f5e:	f0 93 01 02 	sts	0x0201, r31
+    3f62:	e0 93 00 02 	sts	0x0200, r30
+    3f66:	31 97       	sbiw	r30, 0x01	; 1
+    3f68:	e4 91       	lpm	r30, Z+
+    3f6a:	8e 2f       	mov	r24, r30
+    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
       while (--length);
-    3f5e:	80 91 02 02 	lds	r24, 0x0202
-    3f62:	81 50       	subi	r24, 0x01	; 1
-    3f64:	80 93 02 02 	sts	0x0202, r24
-    3f68:	88 23       	and	r24, r24
-    3f6a:	61 f7       	brne	.-40     	; 0x3f44 <main+0x144>
-    3f6c:	0e c0       	rjmp	.+28     	; 0x3f8a <main+0x18a>
+    3f6e:	80 91 02 02 	lds	r24, 0x0202
+    3f72:	81 50       	subi	r24, 0x01	; 1
+    3f74:	80 93 02 02 	sts	0x0202, r24
+    3f78:	88 23       	and	r24, r24
+    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
+    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+#endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f6e:	85 37       	cpi	r24, 0x75	; 117
-    3f70:	39 f4       	brne	.+14     	; 0x3f80 <main+0x180>
+    3f7e:	85 37       	cpi	r24, 0x75	; 117
+    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f72:	2e d0       	rcall	.+92     	; 0x3fd0 <verifySpace>
+    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
       putch(SIGNATURE_0);
-    3f74:	8e e1       	ldi	r24, 0x1E	; 30
-    3f76:	0c d0       	rcall	.+24     	; 0x3f90 <putch>
+    3f84:	8e e1       	ldi	r24, 0x1E	; 30
+    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
       putch(SIGNATURE_1);
-    3f78:	84 e9       	ldi	r24, 0x94	; 148
-    3f7a:	0a d0       	rcall	.+20     	; 0x3f90 <putch>
+    3f88:	84 e9       	ldi	r24, 0x94	; 148
+    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
       putch(SIGNATURE_2);
-    3f7c:	86 e0       	ldi	r24, 0x06	; 6
-    3f7e:	96 cf       	rjmp	.-212    	; 0x3eac <main+0xac>
+    3f8c:	86 e0       	ldi	r24, 0x06	; 6
+    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
     }
     else if (ch == 'Q') {
-    3f80:	81 35       	cpi	r24, 0x51	; 81
-    3f82:	11 f4       	brne	.+4      	; 0x3f88 <main+0x188>
+    3f90:	81 35       	cpi	r24, 0x51	; 81
+    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f84:	88 e0       	ldi	r24, 0x08	; 8
-    3f86:	19 d0       	rcall	.+50     	; 0x3fba <watchdogConfig>
+    3f94:	88 e0       	ldi	r24, 0x08	; 8
+    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f88:	23 d0       	rcall	.+70     	; 0x3fd0 <verifySpace>
+    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
     }
     putch(STK_OK);
-    3f8a:	80 e1       	ldi	r24, 0x10	; 16
-    3f8c:	01 d0       	rcall	.+2      	; 0x3f90 <putch>
-    3f8e:	63 cf       	rjmp	.-314    	; 0x3e56 <main+0x56>
+    3f9a:	80 e1       	ldi	r24, 0x10	; 16
+    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
+    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
 
-00003f90 <putch>:
+00003fa0 <putch>:
   }
 }
 
 void putch(char ch) {
-    3f90:	98 2f       	mov	r25, r24
+    3fa0:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    3f92:	80 91 c0 00 	lds	r24, 0x00C0
-    3f96:	85 ff       	sbrs	r24, 5
-    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <putch+0x2>
+    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fa6:	85 ff       	sbrs	r24, 5
+    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
   UDR0 = ch;
-    3f9a:	90 93 c6 00 	sts	0x00C6, r25
+    3faa:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3f9e:	08 95       	ret
+    3fae:	08 95       	ret
 
-00003fa0 <getch>:
+00003fb0 <getch>:
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fa0:	a8 95       	wdr
+    3fb0:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fa6:	87 ff       	sbrs	r24, 7
-    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <getch+0x2>
+    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fb6:	87 ff       	sbrs	r24, 7
+    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
   ch = UDR0;
-    3faa:	80 91 c6 00 	lds	r24, 0x00C6
-#ifdef LED_DATA_FLASH
+    3fba:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
+#endif
 
   return ch;
 }
-    3fae:	08 95       	ret
+    3fbe:	08 95       	ret
 
-00003fb0 <getLen>:
+00003fc0 <getLen>:
   } while (--count);
 }
 #endif
 
 uint8_t getLen() {
   getch();
-    3fb0:	f7 df       	rcall	.-18     	; 0x3fa0 <getch>
+    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
   length = getch();
-    3fb2:	f6 df       	rcall	.-20     	; 0x3fa0 <getch>
-    3fb4:	80 93 02 02 	sts	0x0202, r24
+    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
+    3fc4:	80 93 02 02 	sts	0x0202, r24
   return getch();
 }
-    3fb8:	f3 cf       	rjmp	.-26     	; 0x3fa0 <getch>
+    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
 
-00003fba <watchdogConfig>:
+00003fca <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3fba:	e0 e6       	ldi	r30, 0x60	; 96
-    3fbc:	f0 e0       	ldi	r31, 0x00	; 0
-    3fbe:	98 e1       	ldi	r25, 0x18	; 24
-    3fc0:	90 83       	st	Z, r25
+    3fca:	e0 e6       	ldi	r30, 0x60	; 96
+    3fcc:	f0 e0       	ldi	r31, 0x00	; 0
+    3fce:	98 e1       	ldi	r25, 0x18	; 24
+    3fd0:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fc2:	80 83       	st	Z, r24
+    3fd2:	80 83       	st	Z, r24
 }
-    3fc4:	08 95       	ret
+    3fd4:	08 95       	ret
 
-00003fc6 <appStart>:
+00003fd6 <appStart>:
 
 void appStart() {
   watchdogConfig(WATCHDOG_OFF);
-    3fc6:	80 e0       	ldi	r24, 0x00	; 0
-    3fc8:	f8 df       	rcall	.-16     	; 0x3fba <watchdogConfig>
+    3fd6:	80 e0       	ldi	r24, 0x00	; 0
+    3fd8:	f8 df       	rcall	.-16     	; 0x3fca <watchdogConfig>
   __asm__ __volatile__ (
-    3fca:	ee 27       	eor	r30, r30
-    3fcc:	ff 27       	eor	r31, r31
-    3fce:	09 94       	ijmp
+    3fda:	ee 27       	eor	r30, r30
+    3fdc:	ff 27       	eor	r31, r31
+    3fde:	09 94       	ijmp
 
-00003fd0 <verifySpace>:
+00003fe0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) appStart();
-    3fd0:	e7 df       	rcall	.-50     	; 0x3fa0 <getch>
-    3fd2:	80 32       	cpi	r24, 0x20	; 32
-    3fd4:	09 f0       	breq	.+2      	; 0x3fd8 <verifySpace+0x8>
-    3fd6:	f7 df       	rcall	.-18     	; 0x3fc6 <appStart>
+    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
+    3fe2:	80 32       	cpi	r24, 0x20	; 32
+    3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
+    3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
   putch(STK_INSYNC);
-    3fd8:	84 e1       	ldi	r24, 0x14	; 20
+    3fe8:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fda:	da cf       	rjmp	.-76     	; 0x3f90 <putch>
+    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+
+00003fec <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fdc:	1f 93       	push	r17
-    3fde:	18 2f       	mov	r17, r24
-
-00003fe0 <getNch>:
+    3fec:	1f 93       	push	r17
+    3fee:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3fe0:	df df       	rcall	.-66     	; 0x3fa0 <getch>
-    3fe2:	11 50       	subi	r17, 0x01	; 1
-    3fe4:	e9 f7       	brne	.-6      	; 0x3fe0 <getNch>
+    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
+    3ff2:	11 50       	subi	r17, 0x01	; 1
+    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
   verifySpace();
-    3fe6:	f4 df       	rcall	.-24     	; 0x3fd0 <verifySpace>
+    3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
 }
-    3fe8:	1f 91       	pop	r17
-    3fea:	08 95       	ret
+    3ff8:	1f 91       	pop	r17
+    3ffa:	08 95       	ret
diff --git a/bootloaders/optiboot/optiboot_pro_20mhz.hex b/bootloaders/optiboot/optiboot_pro_20mhz.hex
index 19c9ae4..d147f8f 100644
--- a/bootloaders/optiboot/optiboot_pro_20mhz.hex
+++ b/bootloaders/optiboot/optiboot_pro_20mhz.hex
@@ -1,33 +1,34 @@
-:103E000085E08093810082E08093C00088E1809308

-:103E1000C10086E08093C20085E18093C40084B72E

-:103E200014BE81FFD0D08DE0C8D0259A86E02CE367

-:103E30003BEF91E0309385002093840096BBB09BCC

-:103E4000FECF1D9AA8958150A9F7DD24D394A5E053

-:103E5000EA2EF1E1FF2EA4D0813421F481E0BED01E

-:103E600083E024C0823411F484E103C0853419F462

-:103E700085E0B4D08AC08535A1F492D0082F10E037

-:103E800010930102009300028BD090E0982F8827B6

-:103E9000802B912B880F991F909301028093000231

-:103EA00073C0863529F484E099D080E071D06DC06C

-:103EB000843609F043C07CD0E0910002F091010209

-:103EC00083E080935700E895C0E0D1E069D0899302

-:103ED000809102028150809302028823B9F778D042

-:103EE00007B600FCFDCF4091000250910102A0E016

-:103EF000B1E02C9130E011968C91119790E0982FC1

-:103F00008827822B932B1296FA010C01D09257002E

-:103F1000E89511244E5F5F4FF1E0A038BF0749F7E5

-:103F2000E0910002F0910102E0925700E89507B697

-:103F300000FCFDCFF0925700E89527C08437B9F414

-:103F400037D046D0E0910002F09101023196F09313

-:103F50000102E09300023197E4918E2F19D08091F5

-:103F60000202815080930202882361F70EC08537D8

-:103F700039F42ED08EE10CD084E90AD086E096CFB9

-:103F8000813511F488E019D023D080E101D063CFCE

-:103F9000982F8091C00085FFFCCF9093C6000895B4

-:103FA000A8958091C00087FFFCCF8091C60008953E

-:103FB000F7DFF6DF80930202F3CFE0E6F0E098E16E

-:103FC00090838083089580E0F8DFEE27FF2709942F

-:103FD000E7DF803209F0F7DF84E1DACF1F93182F93

-:0C3FE000DFDF1150E9F7F4DF1F910895B6

+:103E0000112484B714BE81FFE6D085E08093810041

+:103E100082E08093C00088E18093C10086E08093B7

+:103E2000C20085E18093C4008EE0CFD0259A86E061

+:103E30002CE33BEF91E0309385002093840096BB08

+:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D

+:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020

+:103E6000C5D083E020C0823411F484E103C08534DE

+:103E700019F485E0BBD091C0853581F499D0082F25

+:103E800010E096D090E0982F8827802B912B880FF8

+:103E9000991F90930102809300027EC0863529F419

+:103EA00084E0A4D080E07CD078C0843609F04EC095

+:103EB00087D0E0910002F091010288E3E030F8073A

+:103EC00018F483E087BFE895C0E0D1E071D0899312

+:103ED000809102028150809302028823B9F7E09119

+:103EE0000002F091010288E3E030F80718F083E067

+:103EF00087BFE89575D007B600FCFDCF4091000262

+:103F000050910102A0E0B1E02C9130E011968C912B

+:103F1000119790E0982F8827822B932B1296FA0105

+:103F20000C01D7BEE89511244E5F5F4FF1E0A03839

+:103F3000BF0751F7E0910002F0910102E7BEE8955A

+:103F400007B600FCFDCFF7BEE89527C08437B9F46B

+:103F500037D046D0E0910002F09101023196F09303

+:103F60000102E09300023197E4918E2F19D08091E5

+:103F70000202815080930202882361F70EC08537C8

+:103F800039F42ED08EE10CD084E90AD086E08BCFB4

+:103F9000813511F488E019D023D080E101D05CCFC5

+:103FA000982F8091C00085FFFCCF9093C6000895A4

+:103FB000A8958091C00087FFFCCF8091C60008952E

+:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E

+:103FD00090838083089580E0F8DFEE27FF2709941F

+:103FE000E7DF803209F0F7DF84E1DACF1F93182F83

+:0C3FF000DFDF1150E9F7F4DF1F910895A6

 :0400000300003E00BB

 :00000001FF

diff --git a/bootloaders/optiboot/optiboot_pro_20mhz.lst b/bootloaders/optiboot/optiboot_pro_20mhz.lst
index 62178d3..1868d16 100644
--- a/bootloaders/optiboot/optiboot_pro_20mhz.lst
+++ b/bootloaders/optiboot/optiboot_pro_20mhz.lst
@@ -3,237 +3,257 @@ optiboot_pro_20mhz.elf:     file format elf32-avr
 
 Sections:
 Idx Name          Size      VMA       LMA       File off  Algn
-  0 .text         000001ec  00003e00  00003e00  00000054  2**1
+  0 .text         000001fc  00003e00  00003e00  00000054  2**1
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
-  1 .debug_aranges 00000028  00000000  00000000  00000240  2**0
+  1 .debug_aranges 00000028  00000000  00000000  00000250  2**0
                   CONTENTS, READONLY, DEBUGGING
-  2 .debug_pubnames 0000006a  00000000  00000000  00000268  2**0
+  2 .debug_pubnames 0000006a  00000000  00000000  00000278  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_info   00000269  00000000  00000000  000002d2  2**0
+  3 .debug_info   00000284  00000000  00000000  000002e2  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_abbrev 00000196  00000000  00000000  0000053b  2**0
+  4 .debug_abbrev 000001ae  00000000  00000000  00000566  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_line   000003d3  00000000  00000000  000006d1  2**0
+  5 .debug_line   00000450  00000000  00000000  00000714  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_frame  00000090  00000000  00000000  00000aa4  2**2
+  6 .debug_frame  00000090  00000000  00000000  00000b64  2**2
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_str    00000135  00000000  00000000  00000b34  2**0
+  7 .debug_str    00000141  00000000  00000000  00000bf4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_loc    000001d1  00000000  00000000  00000c69  2**0
+  8 .debug_loc    000001e1  00000000  00000000  00000d35  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_ranges 00000068  00000000  00000000  00000e3a  2**0
+  9 .debug_ranges 00000068  00000000  00000000  00000f16  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00003e00 <main>:
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3e00:	85 e0       	ldi	r24, 0x05	; 5
-    3e02:	80 93 81 00 	sts	0x0081, r24
+    3e00:	11 24       	eor	r1, r1
+#ifdef __AVR_ATmega8__
+  SP=RAMEND;  // This is done by hardware reset
+#endif
+
+  // Adaboot no-wait mod
+  ch = MCUSR;
+    3e02:	84 b7       	in	r24, 0x34	; 52
+  MCUSR = 0;
+    3e04:	14 be       	out	0x34, r1	; 52
+  if (!(ch & _BV(EXTRF))) appStart();
+    3e06:	81 ff       	sbrs	r24, 1
+    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <appStart>
+
 #if LED_START_FLASHES > 0
   // Set up Timer 1 for timeout counter
   TCCR1B = _BV(CS12) | _BV(CS10); // div 1024
-#endif
-#ifndef SOFT_UART
+    3e0a:	85 e0       	ldi	r24, 0x05	; 5
+    3e0c:	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
   UCSR0A = _BV(U2X0); //Double speed mode USART0
-    3e06:	82 e0       	ldi	r24, 0x02	; 2
-    3e08:	80 93 c0 00 	sts	0x00C0, r24
+    3e10:	82 e0       	ldi	r24, 0x02	; 2
+    3e12:	80 93 c0 00 	sts	0x00C0, r24
   UCSR0B = _BV(RXEN0) | _BV(TXEN0);
-    3e0c:	88 e1       	ldi	r24, 0x18	; 24
-    3e0e:	80 93 c1 00 	sts	0x00C1, r24
+    3e16:	88 e1       	ldi	r24, 0x18	; 24
+    3e18:	80 93 c1 00 	sts	0x00C1, r24
   UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
-    3e12:	86 e0       	ldi	r24, 0x06	; 6
-    3e14:	80 93 c2 00 	sts	0x00C2, r24
+    3e1c:	86 e0       	ldi	r24, 0x06	; 6
+    3e1e:	80 93 c2 00 	sts	0x00C2, r24
   UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
-    3e18:	85 e1       	ldi	r24, 0x15	; 21
-    3e1a:	80 93 c4 00 	sts	0x00C4, r24
+    3e22:	85 e1       	ldi	r24, 0x15	; 21
+    3e24:	80 93 c4 00 	sts	0x00C4, r24
+#endif
 #endif
-
-  // Adaboot no-wait mod
-  ch = MCUSR;
-    3e1e:	84 b7       	in	r24, 0x34	; 52
-  MCUSR = 0;
-    3e20:	14 be       	out	0x34, r1	; 52
-  if (!(ch & _BV(EXTRF))) appStart();
-    3e22:	81 ff       	sbrs	r24, 1
-    3e24:	d0 d0       	rcall	.+416    	; 0x3fc6 <appStart>
 
   // Set up watchdog to trigger after 500ms
-  watchdogConfig(WATCHDOG_500MS);
-    3e26:	8d e0       	ldi	r24, 0x0D	; 13
-    3e28:	c8 d0       	rcall	.+400    	; 0x3fba <watchdogConfig>
+  watchdogConfig(WATCHDOG_1S);
+    3e28:	8e e0       	ldi	r24, 0x0E	; 14
+    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    3e2a:	25 9a       	sbi	0x04, 5	; 4
-    3e2c:	86 e0       	ldi	r24, 0x06	; 6
+    3e2c:	25 9a       	sbi	0x04, 5	; 4
+    3e2e:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e2e:	2c e3       	ldi	r18, 0x3C	; 60
-    3e30:	3b ef       	ldi	r19, 0xFB	; 251
+    3e30:	2c e3       	ldi	r18, 0x3C	; 60
+    3e32:	3b ef       	ldi	r19, 0xFB	; 251
     TIFR1 = _BV(TOV1);
-    3e32:	91 e0       	ldi	r25, 0x01	; 1
+    3e34:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e34:	30 93 85 00 	sts	0x0085, r19
-    3e38:	20 93 84 00 	sts	0x0084, r18
+    3e36:	30 93 85 00 	sts	0x0085, r19
+    3e3a:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    3e3c:	96 bb       	out	0x16, r25	; 22
+    3e3e:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    3e3e:	b0 9b       	sbis	0x16, 0	; 22
-    3e40:	fe cf       	rjmp	.-4      	; 0x3e3e <main+0x3e>
+    3e40:	b0 9b       	sbis	0x16, 0	; 22
+    3e42:	fe cf       	rjmp	.-4      	; 0x3e40 <main+0x40>
+#ifdef __AVR_ATmega8__
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
-    3e42:	1d 9a       	sbi	0x03, 5	; 3
+    3e44:	1d 9a       	sbi	0x03, 5	; 3
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3e44:	a8 95       	wdr
-    TCNT1 = -(F_CPU/(1024*16));
-    TIFR1 = _BV(TOV1);
-    while(!(TIFR1 & _BV(TOV1)));
+    3e46:	a8 95       	wdr
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
+#endif
     watchdogReset();
   } while (--count);
-    3e46:	81 50       	subi	r24, 0x01	; 1
-    3e48:	a9 f7       	brne	.-22     	; 0x3e34 <main+0x34>
+    3e48:	81 50       	subi	r24, 0x01	; 1
+    3e4a:	a9 f7       	brne	.-22     	; 0x3e36 <main+0x36>
     /* get character from UART */
     ch = getch();
 
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e4a:	dd 24       	eor	r13, r13
-    3e4c:	d3 94       	inc	r13
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
+    3e4c:	dd 24       	eor	r13, r13
+    3e4e:	d3 94       	inc	r13
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3e4e:	a5 e0       	ldi	r26, 0x05	; 5
-    3e50:	ea 2e       	mov	r14, r26
+      __boot_page_write_short((uint16_t)(void*)address);
+    3e50:	a5 e0       	ldi	r26, 0x05	; 5
+    3e52:	ea 2e       	mov	r14, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3e52:	f1 e1       	ldi	r31, 0x11	; 17
-    3e54:	ff 2e       	mov	r15, r31
+    3e54:	f1 e1       	ldi	r31, 0x11	; 17
+    3e56:	ff 2e       	mov	r15, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e56:	a4 d0       	rcall	.+328    	; 0x3fa0 <getch>
+    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    3e58:	81 34       	cpi	r24, 0x41	; 65
-    3e5a:	21 f4       	brne	.+8      	; 0x3e64 <main+0x64>
+    3e5a:	81 34       	cpi	r24, 0x41	; 65
+    3e5c:	21 f4       	brne	.+8      	; 0x3e66 <main+0x66>
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e5c:	81 e0       	ldi	r24, 0x01	; 1
-    3e5e:	be d0       	rcall	.+380    	; 0x3fdc <verifySpace+0xc>
+    3e5e:	81 e0       	ldi	r24, 0x01	; 1
+    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
       putch(0x03);
-    3e60:	83 e0       	ldi	r24, 0x03	; 3
-    3e62:	24 c0       	rjmp	.+72     	; 0x3eac <main+0xac>
+    3e62:	83 e0       	ldi	r24, 0x03	; 3
+    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
     }
     else if(ch == STK_SET_DEVICE) {
-    3e64:	82 34       	cpi	r24, 0x42	; 66
-    3e66:	11 f4       	brne	.+4      	; 0x3e6c <main+0x6c>
+    3e66:	82 34       	cpi	r24, 0x42	; 66
+    3e68:	11 f4       	brne	.+4      	; 0x3e6e <main+0x6e>
       // SET DEVICE is ignored
       getNch(20);
-    3e68:	84 e1       	ldi	r24, 0x14	; 20
-    3e6a:	03 c0       	rjmp	.+6      	; 0x3e72 <main+0x72>
+    3e6a:	84 e1       	ldi	r24, 0x14	; 20
+    3e6c:	03 c0       	rjmp	.+6      	; 0x3e74 <main+0x74>
     }
     else if(ch == STK_SET_DEVICE_EXT) {
-    3e6c:	85 34       	cpi	r24, 0x45	; 69
-    3e6e:	19 f4       	brne	.+6      	; 0x3e76 <main+0x76>
+    3e6e:	85 34       	cpi	r24, 0x45	; 69
+    3e70:	19 f4       	brne	.+6      	; 0x3e78 <main+0x78>
       // SET DEVICE EXT is ignored
       getNch(5);
-    3e70:	85 e0       	ldi	r24, 0x05	; 5
-    3e72:	b4 d0       	rcall	.+360    	; 0x3fdc <verifySpace+0xc>
-    3e74:	8a c0       	rjmp	.+276    	; 0x3f8a <main+0x18a>
+    3e72:	85 e0       	ldi	r24, 0x05	; 5
+    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
+    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
-    3e76:	85 35       	cpi	r24, 0x55	; 85
-    3e78:	a1 f4       	brne	.+40     	; 0x3ea2 <main+0xa2>
+    3e78:	85 35       	cpi	r24, 0x55	; 85
+    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
       // LOAD ADDRESS
-      address = getch();
-    3e7a:	92 d0       	rcall	.+292    	; 0x3fa0 <getch>
-    3e7c:	08 2f       	mov	r16, r24
-    3e7e:	10 e0       	ldi	r17, 0x00	; 0
-    3e80:	10 93 01 02 	sts	0x0201, r17
-    3e84:	00 93 00 02 	sts	0x0200, r16
-      address = (address & 0xff) | (getch() << 8);
-    3e88:	8b d0       	rcall	.+278    	; 0x3fa0 <getch>
-    3e8a:	90 e0       	ldi	r25, 0x00	; 0
-    3e8c:	98 2f       	mov	r25, r24
-    3e8e:	88 27       	eor	r24, r24
-    3e90:	80 2b       	or	r24, r16
-    3e92:	91 2b       	or	r25, r17
-      address += address; // Convert from word address to byte address
-    3e94:	88 0f       	add	r24, r24
-    3e96:	99 1f       	adc	r25, r25
-    3e98:	90 93 01 02 	sts	0x0201, r25
-    3e9c:	80 93 00 02 	sts	0x0200, r24
-    3ea0:	73 c0       	rjmp	.+230    	; 0x3f88 <main+0x188>
+      uint16_t newAddress;
+      newAddress = getch();
+    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+      newAddress = (newAddress & 0xff) | (getch() << 8);
+    3e7e:	08 2f       	mov	r16, r24
+    3e80:	10 e0       	ldi	r17, 0x00	; 0
+    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
+    3e84:	90 e0       	ldi	r25, 0x00	; 0
+    3e86:	98 2f       	mov	r25, r24
+    3e88:	88 27       	eor	r24, r24
+    3e8a:	80 2b       	or	r24, r16
+    3e8c:	91 2b       	or	r25, r17
+#ifdef RAMPZ
+      // Transfer top bit to RAMPZ
+      RAMPZ = (newAddress & 0x8000) ? 1 : 0;
+#endif
+      newAddress += newAddress; // Convert from word address to byte address
+    3e8e:	88 0f       	add	r24, r24
+    3e90:	99 1f       	adc	r25, r25
+      address = newAddress;
+    3e92:	90 93 01 02 	sts	0x0201, r25
+    3e96:	80 93 00 02 	sts	0x0200, r24
+    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
     }
     else if(ch == STK_UNIVERSAL) {
-    3ea2:	86 35       	cpi	r24, 0x56	; 86
-    3ea4:	29 f4       	brne	.+10     	; 0x3eb0 <main+0xb0>
+    3e9c:	86 35       	cpi	r24, 0x56	; 86
+    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
       // UNIVERSAL command is ignored
       getNch(4);
-    3ea6:	84 e0       	ldi	r24, 0x04	; 4
-    3ea8:	99 d0       	rcall	.+306    	; 0x3fdc <verifySpace+0xc>
+    3ea0:	84 e0       	ldi	r24, 0x04	; 4
+    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
       putch(0x00);
-    3eaa:	80 e0       	ldi	r24, 0x00	; 0
-    3eac:	71 d0       	rcall	.+226    	; 0x3f90 <putch>
-    3eae:	6d c0       	rjmp	.+218    	; 0x3f8a <main+0x18a>
+    3ea4:	80 e0       	ldi	r24, 0x00	; 0
+    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
+    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
     }
-    /* Write memory, length is big endian and is in bytes  */
+    /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3eb0:	84 36       	cpi	r24, 0x64	; 100
-    3eb2:	09 f0       	breq	.+2      	; 0x3eb6 <main+0xb6>
-    3eb4:	43 c0       	rjmp	.+134    	; 0x3f3c <main+0x13c>
+    3eaa:	84 36       	cpi	r24, 0x64	; 100
+    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
+    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
       getLen();
-    3eb6:	7c d0       	rcall	.+248    	; 0x3fb0 <getLen>
-
-      // Immediately start page erase - this will 4.5ms
-      boot_page_erase((uint16_t)(void*)address);
-    3eb8:	e0 91 00 02 	lds	r30, 0x0200
-    3ebc:	f0 91 01 02 	lds	r31, 0x0201
-    3ec0:	83 e0       	ldi	r24, 0x03	; 3
-    3ec2:	80 93 57 00 	sts	0x0057, r24
+    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+
+      // If we are in RWW section, immediately start page erase
+      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3eb2:	e0 91 00 02 	lds	r30, 0x0200
+    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eba:	88 e3       	ldi	r24, 0x38	; 56
+    3ebc:	e0 30       	cpi	r30, 0x00	; 0
+    3ebe:	f8 07       	cpc	r31, r24
+    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
+    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ec4:	87 bf       	out	0x37, r24	; 55
     3ec6:	e8 95       	spm
     3ec8:	c0 e0       	ldi	r28, 0x00	; 0
     3eca:	d1 e0       	ldi	r29, 0x01	; 1
-
+      
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3ecc:	69 d0       	rcall	.+210    	; 0x3fa0 <getch>
+    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
     3ece:	89 93       	st	Y+, r24
       while (--length);
     3ed0:	80 91 02 02 	lds	r24, 0x0202
@@ -242,279 +262,293 @@ void watchdogReset() {
     3eda:	88 23       	and	r24, r24
     3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
 
+      // If we are in NRWW section, page erase has to be delayed until now.
+      // Todo: Take RAMPZ into account
+      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3ede:	e0 91 00 02 	lds	r30, 0x0200
+    3ee2:	f0 91 01 02 	lds	r31, 0x0201
+    3ee6:	88 e3       	ldi	r24, 0x38	; 56
+    3ee8:	e0 30       	cpi	r30, 0x00	; 0
+    3eea:	f8 07       	cpc	r31, r24
+    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
+    3eee:	83 e0       	ldi	r24, 0x03	; 3
+    3ef0:	87 bf       	out	0x37, r24	; 55
+    3ef2:	e8 95       	spm
+
       // Read command terminator, start reply
       verifySpace();
-    3ede:	78 d0       	rcall	.+240    	; 0x3fd0 <verifySpace>
+    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
       
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3ee0:	07 b6       	in	r0, 0x37	; 55
-    3ee2:	00 fc       	sbrc	r0, 0
-    3ee4:	fd cf       	rjmp	.-6      	; 0x3ee0 <main+0xe0>
+    3ef6:	07 b6       	in	r0, 0x37	; 55
+    3ef8:	00 fc       	sbrc	r0, 0
+    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
       }
 #endif
 
       // Copy buffer into programming buffer
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3ee6:	40 91 00 02 	lds	r20, 0x0200
-    3eea:	50 91 01 02 	lds	r21, 0x0201
-    3eee:	a0 e0       	ldi	r26, 0x00	; 0
-    3ef0:	b1 e0       	ldi	r27, 0x01	; 1
+    3efc:	40 91 00 02 	lds	r20, 0x0200
+    3f00:	50 91 01 02 	lds	r21, 0x0201
+    3f04:	a0 e0       	ldi	r26, 0x00	; 0
+    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3ef2:	2c 91       	ld	r18, X
-    3ef4:	30 e0       	ldi	r19, 0x00	; 0
+    3f08:	2c 91       	ld	r18, X
+    3f0a:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3ef6:	11 96       	adiw	r26, 0x01	; 1
-    3ef8:	8c 91       	ld	r24, X
-    3efa:	11 97       	sbiw	r26, 0x01	; 1
-    3efc:	90 e0       	ldi	r25, 0x00	; 0
-    3efe:	98 2f       	mov	r25, r24
-    3f00:	88 27       	eor	r24, r24
-    3f02:	82 2b       	or	r24, r18
-    3f04:	93 2b       	or	r25, r19
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+    3f0c:	11 96       	adiw	r26, 0x01	; 1
+    3f0e:	8c 91       	ld	r24, X
+    3f10:	11 97       	sbiw	r26, 0x01	; 1
+    3f12:	90 e0       	ldi	r25, 0x00	; 0
+    3f14:	98 2f       	mov	r25, r24
+    3f16:	88 27       	eor	r24, r24
+    3f18:	82 2b       	or	r24, r18
+    3f1a:	93 2b       	or	r25, r19
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3f06:	12 96       	adiw	r26, 0x02	; 2
+    3f1c:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
-    3f08:	fa 01       	movw	r30, r20
-    3f0a:	0c 01       	movw	r0, r24
-    3f0c:	d0 92 57 00 	sts	0x0057, r13
-    3f10:	e8 95       	spm
-    3f12:	11 24       	eor	r1, r1
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3f1e:	fa 01       	movw	r30, r20
+    3f20:	0c 01       	movw	r0, r24
+    3f22:	d7 be       	out	0x37, r13	; 55
+    3f24:	e8 95       	spm
+    3f26:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3f14:	4e 5f       	subi	r20, 0xFE	; 254
-    3f16:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f28:	4e 5f       	subi	r20, 0xFE	; 254
+    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f18:	f1 e0       	ldi	r31, 0x01	; 1
-    3f1a:	a0 38       	cpi	r26, 0x80	; 128
-    3f1c:	bf 07       	cpc	r27, r31
-    3f1e:	49 f7       	brne	.-46     	; 0x3ef2 <main+0xf2>
+    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
+    3f2e:	a0 38       	cpi	r26, 0x80	; 128
+    3f30:	bf 07       	cpc	r27, r31
+    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3f20:	e0 91 00 02 	lds	r30, 0x0200
-    3f24:	f0 91 01 02 	lds	r31, 0x0201
-    3f28:	e0 92 57 00 	sts	0x0057, r14
-    3f2c:	e8 95       	spm
+      __boot_page_write_short((uint16_t)(void*)address);
+    3f34:	e0 91 00 02 	lds	r30, 0x0200
+    3f38:	f0 91 01 02 	lds	r31, 0x0201
+    3f3c:	e7 be       	out	0x37, r14	; 55
+    3f3e:	e8 95       	spm
       boot_spm_busy_wait();
-    3f2e:	07 b6       	in	r0, 0x37	; 55
-    3f30:	00 fc       	sbrc	r0, 0
-    3f32:	fd cf       	rjmp	.-6      	; 0x3f2e <main+0x12e>
+    3f40:	07 b6       	in	r0, 0x37	; 55
+    3f42:	00 fc       	sbrc	r0, 0
+    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f34:	f0 92 57 00 	sts	0x0057, r15
-    3f38:	e8 95       	spm
-    3f3a:	27 c0       	rjmp	.+78     	; 0x3f8a <main+0x18a>
+    3f46:	f7 be       	out	0x37, r15	; 55
+    3f48:	e8 95       	spm
+    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f3c:	84 37       	cpi	r24, 0x74	; 116
-    3f3e:	b9 f4       	brne	.+46     	; 0x3f6e <main+0x16e>
+    3f4c:	84 37       	cpi	r24, 0x74	; 116
+    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
       // READ PAGE - we only read flash
       getLen();
-    3f40:	37 d0       	rcall	.+110    	; 0x3fb0 <getLen>
+    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
       verifySpace();
-    3f42:	46 d0       	rcall	.+140    	; 0x3fd0 <verifySpace>
-        else ch = pgm_read_byte_near(address);
+    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+        putch(result);
         address++;
-        putch(ch);
-      } while (--length);
+      }
+      while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f44:	e0 91 00 02 	lds	r30, 0x0200
-    3f48:	f0 91 01 02 	lds	r31, 0x0201
-    3f4c:	31 96       	adiw	r30, 0x01	; 1
-    3f4e:	f0 93 01 02 	sts	0x0201, r31
-    3f52:	e0 93 00 02 	sts	0x0200, r30
-    3f56:	31 97       	sbiw	r30, 0x01	; 1
-    3f58:	e4 91       	lpm	r30, Z+
-    3f5a:	8e 2f       	mov	r24, r30
-    3f5c:	19 d0       	rcall	.+50     	; 0x3f90 <putch>
+    3f54:	e0 91 00 02 	lds	r30, 0x0200
+    3f58:	f0 91 01 02 	lds	r31, 0x0201
+    3f5c:	31 96       	adiw	r30, 0x01	; 1
+    3f5e:	f0 93 01 02 	sts	0x0201, r31
+    3f62:	e0 93 00 02 	sts	0x0200, r30
+    3f66:	31 97       	sbiw	r30, 0x01	; 1
+    3f68:	e4 91       	lpm	r30, Z+
+    3f6a:	8e 2f       	mov	r24, r30
+    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
       while (--length);
-    3f5e:	80 91 02 02 	lds	r24, 0x0202
-    3f62:	81 50       	subi	r24, 0x01	; 1
-    3f64:	80 93 02 02 	sts	0x0202, r24
-    3f68:	88 23       	and	r24, r24
-    3f6a:	61 f7       	brne	.-40     	; 0x3f44 <main+0x144>
-    3f6c:	0e c0       	rjmp	.+28     	; 0x3f8a <main+0x18a>
+    3f6e:	80 91 02 02 	lds	r24, 0x0202
+    3f72:	81 50       	subi	r24, 0x01	; 1
+    3f74:	80 93 02 02 	sts	0x0202, r24
+    3f78:	88 23       	and	r24, r24
+    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
+    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+#endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f6e:	85 37       	cpi	r24, 0x75	; 117
-    3f70:	39 f4       	brne	.+14     	; 0x3f80 <main+0x180>
+    3f7e:	85 37       	cpi	r24, 0x75	; 117
+    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f72:	2e d0       	rcall	.+92     	; 0x3fd0 <verifySpace>
+    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
       putch(SIGNATURE_0);
-    3f74:	8e e1       	ldi	r24, 0x1E	; 30
-    3f76:	0c d0       	rcall	.+24     	; 0x3f90 <putch>
+    3f84:	8e e1       	ldi	r24, 0x1E	; 30
+    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
       putch(SIGNATURE_1);
-    3f78:	84 e9       	ldi	r24, 0x94	; 148
-    3f7a:	0a d0       	rcall	.+20     	; 0x3f90 <putch>
+    3f88:	84 e9       	ldi	r24, 0x94	; 148
+    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
       putch(SIGNATURE_2);
-    3f7c:	86 e0       	ldi	r24, 0x06	; 6
-    3f7e:	96 cf       	rjmp	.-212    	; 0x3eac <main+0xac>
+    3f8c:	86 e0       	ldi	r24, 0x06	; 6
+    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
     }
     else if (ch == 'Q') {
-    3f80:	81 35       	cpi	r24, 0x51	; 81
-    3f82:	11 f4       	brne	.+4      	; 0x3f88 <main+0x188>
+    3f90:	81 35       	cpi	r24, 0x51	; 81
+    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f84:	88 e0       	ldi	r24, 0x08	; 8
-    3f86:	19 d0       	rcall	.+50     	; 0x3fba <watchdogConfig>
+    3f94:	88 e0       	ldi	r24, 0x08	; 8
+    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f88:	23 d0       	rcall	.+70     	; 0x3fd0 <verifySpace>
+    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
     }
     putch(STK_OK);
-    3f8a:	80 e1       	ldi	r24, 0x10	; 16
-    3f8c:	01 d0       	rcall	.+2      	; 0x3f90 <putch>
-    3f8e:	63 cf       	rjmp	.-314    	; 0x3e56 <main+0x56>
+    3f9a:	80 e1       	ldi	r24, 0x10	; 16
+    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
+    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
 
-00003f90 <putch>:
+00003fa0 <putch>:
   }
 }
 
 void putch(char ch) {
-    3f90:	98 2f       	mov	r25, r24
+    3fa0:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
-    3f92:	80 91 c0 00 	lds	r24, 0x00C0
-    3f96:	85 ff       	sbrs	r24, 5
-    3f98:	fc cf       	rjmp	.-8      	; 0x3f92 <putch+0x2>
+    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fa6:	85 ff       	sbrs	r24, 5
+    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
   UDR0 = ch;
-    3f9a:	90 93 c6 00 	sts	0x00C6, r25
+    3faa:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3f9e:	08 95       	ret
+    3fae:	08 95       	ret
 
-00003fa0 <getch>:
+00003fb0 <getch>:
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3fa0:	a8 95       	wdr
+    3fb0:	a8 95       	wdr
       [uartBit] "I" (UART_RX_BIT)
     :
       "r25"
 );
 #else
   while(!(UCSR0A & _BV(RXC0)));
-    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
-    3fa6:	87 ff       	sbrs	r24, 7
-    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <getch+0x2>
+    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fb6:	87 ff       	sbrs	r24, 7
+    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
   ch = UDR0;
-    3faa:	80 91 c6 00 	lds	r24, 0x00C6
-#ifdef LED_DATA_FLASH
+    3fba:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
+#endif
 
   return ch;
 }
-    3fae:	08 95       	ret
+    3fbe:	08 95       	ret
 
-00003fb0 <getLen>:
+00003fc0 <getLen>:
   } while (--count);
 }
 #endif
 
 uint8_t getLen() {
   getch();
-    3fb0:	f7 df       	rcall	.-18     	; 0x3fa0 <getch>
+    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
   length = getch();
-    3fb2:	f6 df       	rcall	.-20     	; 0x3fa0 <getch>
-    3fb4:	80 93 02 02 	sts	0x0202, r24
+    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
+    3fc4:	80 93 02 02 	sts	0x0202, r24
   return getch();
 }
-    3fb8:	f3 cf       	rjmp	.-26     	; 0x3fa0 <getch>
+    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
 
-00003fba <watchdogConfig>:
+00003fca <watchdogConfig>:
     "wdr\n"
   );
 }
 
 void watchdogConfig(uint8_t x) {
   WDTCSR = _BV(WDCE) | _BV(WDE);
-    3fba:	e0 e6       	ldi	r30, 0x60	; 96
-    3fbc:	f0 e0       	ldi	r31, 0x00	; 0
-    3fbe:	98 e1       	ldi	r25, 0x18	; 24
-    3fc0:	90 83       	st	Z, r25
+    3fca:	e0 e6       	ldi	r30, 0x60	; 96
+    3fcc:	f0 e0       	ldi	r31, 0x00	; 0
+    3fce:	98 e1       	ldi	r25, 0x18	; 24
+    3fd0:	90 83       	st	Z, r25
   WDTCSR = x;
-    3fc2:	80 83       	st	Z, r24
+    3fd2:	80 83       	st	Z, r24
 }
-    3fc4:	08 95       	ret
+    3fd4:	08 95       	ret
 
-00003fc6 <appStart>:
+00003fd6 <appStart>:
 
 void appStart() {
   watchdogConfig(WATCHDOG_OFF);
-    3fc6:	80 e0       	ldi	r24, 0x00	; 0
-    3fc8:	f8 df       	rcall	.-16     	; 0x3fba <watchdogConfig>
+    3fd6:	80 e0       	ldi	r24, 0x00	; 0
+    3fd8:	f8 df       	rcall	.-16     	; 0x3fca <watchdogConfig>
   __asm__ __volatile__ (
-    3fca:	ee 27       	eor	r30, r30
-    3fcc:	ff 27       	eor	r31, r31
-    3fce:	09 94       	ijmp
+    3fda:	ee 27       	eor	r30, r30
+    3fdc:	ff 27       	eor	r31, r31
+    3fde:	09 94       	ijmp
 
-00003fd0 <verifySpace>:
+00003fe0 <verifySpace>:
   do getch(); while (--count);
   verifySpace();
 }
 
 void verifySpace() {
   if (getch() != CRC_EOP) appStart();
-    3fd0:	e7 df       	rcall	.-50     	; 0x3fa0 <getch>
-    3fd2:	80 32       	cpi	r24, 0x20	; 32
-    3fd4:	09 f0       	breq	.+2      	; 0x3fd8 <verifySpace+0x8>
-    3fd6:	f7 df       	rcall	.-18     	; 0x3fc6 <appStart>
+    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
+    3fe2:	80 32       	cpi	r24, 0x20	; 32
+    3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
+    3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
   putch(STK_INSYNC);
-    3fd8:	84 e1       	ldi	r24, 0x14	; 20
+    3fe8:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fda:	da cf       	rjmp	.-76     	; 0x3f90 <putch>
+    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+
+00003fec <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
 #endif
 
 void getNch(uint8_t count) {
-    3fdc:	1f 93       	push	r17
-    3fde:	18 2f       	mov	r17, r24
-
-00003fe0 <getNch>:
+    3fec:	1f 93       	push	r17
+    3fee:	18 2f       	mov	r17, r24
   do getch(); while (--count);
-    3fe0:	df df       	rcall	.-66     	; 0x3fa0 <getch>
-    3fe2:	11 50       	subi	r17, 0x01	; 1
-    3fe4:	e9 f7       	brne	.-6      	; 0x3fe0 <getNch>
+    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
+    3ff2:	11 50       	subi	r17, 0x01	; 1
+    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
   verifySpace();
-    3fe6:	f4 df       	rcall	.-24     	; 0x3fd0 <verifySpace>
+    3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
 }
-    3fe8:	1f 91       	pop	r17
-    3fea:	08 95       	ret
+    3ff8:	1f 91       	pop	r17
+    3ffa:	08 95       	ret
diff --git a/bootloaders/optiboot/optiboot_pro_8MHz.hex b/bootloaders/optiboot/optiboot_pro_8MHz.hex
index 9d31a7a..7579286 100644
--- a/bootloaders/optiboot/optiboot_pro_8MHz.hex
+++ b/bootloaders/optiboot/optiboot_pro_8MHz.hex
@@ -1,34 +1,34 @@
-:103E000085E08093810084B714BE81FFE4D08DE00B

-:103E1000DCD0259A519A86E028E13EEF91E030937C

-:103E200085002093840096BBB09BFECF1D9AA89579

-:103E30008150A9F7DD24D394A5E0EA2EF1E1FF2E0D

-:103E4000ABD0813421F481E0D1D083E024C082342E

-:103E500011F484E103C0853419F485E0C7D08AC029

-:103E60008535A1F499D0082F10E01093010200933A

-:103E7000000292D090E0982F8827802B912B880FFA

-:103E8000991F909301028093000273C0863529F434

-:103E900084E0ACD080E071D06DC0843609F043C0BE

-:103EA0008FD0E0910002F091010283E080935700EF

-:103EB000E895C0E0D1E070D08993809102028150F2

-:103EC000809302028823B9F78BD007B600FCFDCFA0

-:103ED0004091000250910102A0E0B1E02C9130E04D

-:103EE00011968C91119790E0982F8827822B932B15

-:103EF0001296FA010C01D0925700E89511244E5FFA

-:103F00005F4FF1E0A038BF0749F7E0910002F09160

-:103F10000102E0925700E89507B600FCFDCFF09251

-:103F20005700E89527C08437B9F44AD059D0E091BA

-:103F30000002F09101023196F0930102E093000239

-:103F40003197E4918E2F19D0809102028150809395

-:103F50000202882361F70EC0853739F441D08EE123

-:103F60000CD084E90AD086E096CF813511F488E040

-:103F70002CD036D080E101D063CF2AE030E08095AC

-:103F8000089410F4599802C0599A000015D014D022

-:103F900086952A95B1F70895A89529E030E04899CB

-:103FA000FECF0AD009D008D08894489908942A9561

-:103FB00011F08795F7CF089598E09A95F1F7089555

-:103FC000EBDFEADF80930202E7CFE0E6F0E098E182

+:103E0000112484B714BE81FFE6D085E08093810041

+:103E100082E08093C00088E18093C10086E08093B7

+:103E2000C20088E08093C4008EE0CFD0259A86E05F

+:103E300028E13EEF91E0309385002093840096BB0B

+:103E4000B09BFECF1D9AA8958150A9F7DD24D3948D

+:103E5000A5E0EA2EF1E1FF2EABD0813421F481E020

+:103E6000C5D083E020C0823411F484E103C08534DE

+:103E700019F485E0BBD091C0853581F499D0082F25

+:103E800010E096D090E0982F8827802B912B880FF8

+:103E9000991F90930102809300027EC0863529F419

+:103EA00084E0A4D080E07CD078C0843609F04EC095

+:103EB00087D0E0910002F091010288E3E030F8073A

+:103EC00018F483E087BFE895C0E0D1E071D0899312

+:103ED000809102028150809302028823B9F7E09119

+:103EE0000002F091010288E3E030F80718F083E067

+:103EF00087BFE89575D007B600FCFDCF4091000262

+:103F000050910102A0E0B1E02C9130E011968C912B

+:103F1000119790E0982F8827822B932B1296FA0105

+:103F20000C01D7BEE89511244E5F5F4FF1E0A03839

+:103F3000BF0751F7E0910002F0910102E7BEE8955A

+:103F400007B600FCFDCFF7BEE89527C08437B9F46B

+:103F500037D046D0E0910002F09101023196F09303

+:103F60000102E09300023197E4918E2F19D08091E5

+:103F70000202815080930202882361F70EC08537C8

+:103F800039F42ED08EE10CD084E90AD086E08BCFB4

+:103F9000813511F488E019D023D080E101D05CCFC5

+:103FA000982F8091C00085FFFCCF9093C6000895A4

+:103FB000A8958091C00087FFFCCF8091C60008952E

+:103FC000F7DFF6DF80930202F3CFE0E6F0E098E15E

 :103FD00090838083089580E0F8DFEE27FF2709941F

-:103FE000DBDF803209F0F7DF84E1C7CF1F93182FA2

-:0C3FF000D3DF1150E9F7F4DF1F910895B2

+:103FE000E7DF803209F0F7DF84E1DACF1F93182F83

+:0C3FF000DFDF1150E9F7F4DF1F910895A6

 :0400000300003E00BB

 :00000001FF

diff --git a/bootloaders/optiboot/optiboot_pro_8MHz.lst b/bootloaders/optiboot/optiboot_pro_8MHz.lst
index 94603e2..25a4bd2 100644
--- a/bootloaders/optiboot/optiboot_pro_8MHz.lst
+++ b/bootloaders/optiboot/optiboot_pro_8MHz.lst
@@ -7,452 +7,473 @@ Idx Name          Size      VMA       LMA       File off  Algn
                   CONTENTS, ALLOC, LOAD, READONLY, CODE
   1 .debug_aranges 00000028  00000000  00000000  00000250  2**0
                   CONTENTS, READONLY, DEBUGGING
-  2 .debug_pubnames 00000078  00000000  00000000  00000278  2**0
+  2 .debug_pubnames 0000006a  00000000  00000000  00000278  2**0
                   CONTENTS, READONLY, DEBUGGING
-  3 .debug_info   00000277  00000000  00000000  000002f0  2**0
+  3 .debug_info   00000284  00000000  00000000  000002e2  2**0
                   CONTENTS, READONLY, DEBUGGING
-  4 .debug_abbrev 00000194  00000000  00000000  00000567  2**0
+  4 .debug_abbrev 000001ae  00000000  00000000  00000566  2**0
                   CONTENTS, READONLY, DEBUGGING
-  5 .debug_line   000003bb  00000000  00000000  000006fb  2**0
+  5 .debug_line   00000450  00000000  00000000  00000714  2**0
                   CONTENTS, READONLY, DEBUGGING
-  6 .debug_frame  000000a0  00000000  00000000  00000ab8  2**2
+  6 .debug_frame  00000090  00000000  00000000  00000b64  2**2
                   CONTENTS, READONLY, DEBUGGING
-  7 .debug_str    0000013f  00000000  00000000  00000b58  2**0
+  7 .debug_str    00000141  00000000  00000000  00000bf4  2**0
                   CONTENTS, READONLY, DEBUGGING
-  8 .debug_loc    000001a0  00000000  00000000  00000c97  2**0
+  8 .debug_loc    000001e1  00000000  00000000  00000d35  2**0
                   CONTENTS, READONLY, DEBUGGING
-  9 .debug_ranges 00000070  00000000  00000000  00000e37  2**0
+  9 .debug_ranges 00000068  00000000  00000000  00000f16  2**0
                   CONTENTS, READONLY, DEBUGGING
 
 Disassembly of section .text:
 
 00003e00 <main>:
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3e00:	85 e0       	ldi	r24, 0x05	; 5
-    3e02:	80 93 81 00 	sts	0x0081, r24
-  UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
-  UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 );
+    3e00:	11 24       	eor	r1, r1
+#ifdef __AVR_ATmega8__
+  SP=RAMEND;  // This is done by hardware reset
 #endif
 
   // Adaboot no-wait mod
   ch = MCUSR;
-    3e06:	84 b7       	in	r24, 0x34	; 52
+    3e02:	84 b7       	in	r24, 0x34	; 52
   MCUSR = 0;
-    3e08:	14 be       	out	0x34, r1	; 52
+    3e04:	14 be       	out	0x34, r1	; 52
   if (!(ch & _BV(EXTRF))) appStart();
-    3e0a:	81 ff       	sbrs	r24, 1
-    3e0c:	e4 d0       	rcall	.+456    	; 0x3fd6 <appStart>
+    3e06:	81 ff       	sbrs	r24, 1
+    3e08:	e6 d0       	rcall	.+460    	; 0x3fd6 <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
+  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
+  UCSR0A = _BV(U2X0); //Double speed mode USART0
+    3e10:	82 e0       	ldi	r24, 0x02	; 2
+    3e12:	80 93 c0 00 	sts	0x00C0, r24
+  UCSR0B = _BV(RXEN0) | _BV(TXEN0);
+    3e16:	88 e1       	ldi	r24, 0x18	; 24
+    3e18:	80 93 c1 00 	sts	0x00C1, r24
+  UCSR0C = _BV(UCSZ00) | _BV(UCSZ01);
+    3e1c:	86 e0       	ldi	r24, 0x06	; 6
+    3e1e:	80 93 c2 00 	sts	0x00C2, r24
+  UBRR0L = (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
+#endif
+#endif
 
   // Set up watchdog to trigger after 500ms
-  watchdogConfig(WATCHDOG_500MS);
-    3e0e:	8d e0       	ldi	r24, 0x0D	; 13
-    3e10:	dc d0       	rcall	.+440    	; 0x3fca <watchdogConfig>
+  watchdogConfig(WATCHDOG_1S);
+    3e28:	8e e0       	ldi	r24, 0x0E	; 14
+    3e2a:	cf d0       	rcall	.+414    	; 0x3fca <watchdogConfig>
 
   /* Set LED pin as output */
   LED_DDR |= _BV(LED);
-    3e12:	25 9a       	sbi	0x04, 5	; 4
-
-#ifdef SOFT_UART
-  /* Set TX pin as output */
-  UART_DDR |= _BV(UART_TX_BIT);
-    3e14:	51 9a       	sbi	0x0a, 1	; 10
-    3e16:	86 e0       	ldi	r24, 0x06	; 6
+    3e2c:	25 9a       	sbi	0x04, 5	; 4
+    3e2e:	86 e0       	ldi	r24, 0x06	; 6
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e18:	28 e1       	ldi	r18, 0x18	; 24
-    3e1a:	3e ef       	ldi	r19, 0xFE	; 254
+    3e30:	28 e1       	ldi	r18, 0x18	; 24
+    3e32:	3e ef       	ldi	r19, 0xFE	; 254
     TIFR1 = _BV(TOV1);
-    3e1c:	91 e0       	ldi	r25, 0x01	; 1
+    3e34:	91 e0       	ldi	r25, 0x01	; 1
 }
 
 #if LED_START_FLASHES > 0
 void flash_led(uint8_t count) {
   do {
     TCNT1 = -(F_CPU/(1024*16));
-    3e1e:	30 93 85 00 	sts	0x0085, r19
-    3e22:	20 93 84 00 	sts	0x0084, r18
+    3e36:	30 93 85 00 	sts	0x0085, r19
+    3e3a:	20 93 84 00 	sts	0x0084, r18
     TIFR1 = _BV(TOV1);
-    3e26:	96 bb       	out	0x16, r25	; 22
+    3e3e:	96 bb       	out	0x16, r25	; 22
     while(!(TIFR1 & _BV(TOV1)));
-    3e28:	b0 9b       	sbis	0x16, 0	; 22
-    3e2a:	fe cf       	rjmp	.-4      	; 0x3e28 <main+0x28>
+    3e40:	b0 9b       	sbis	0x16, 0	; 22
+    3e42:	fe cf       	rjmp	.-4      	; 0x3e40 <main+0x40>
+#ifdef __AVR_ATmega8__
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
-    3e2c:	1d 9a       	sbi	0x03, 5	; 3
+    3e44:	1d 9a       	sbi	0x03, 5	; 3
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3e2e:	a8 95       	wdr
-    TCNT1 = -(F_CPU/(1024*16));
-    TIFR1 = _BV(TOV1);
-    while(!(TIFR1 & _BV(TOV1)));
+    3e46:	a8 95       	wdr
+    LED_PORT ^= _BV(LED);
+#else
     LED_PIN |= _BV(LED);
+#endif
     watchdogReset();
   } while (--count);
-    3e30:	81 50       	subi	r24, 0x01	; 1
-    3e32:	a9 f7       	brne	.-22     	; 0x3e1e <main+0x1e>
+    3e48:	81 50       	subi	r24, 0x01	; 1
+    3e4a:	a9 f7       	brne	.-22     	; 0x3e36 <main+0x36>
     /* get character from UART */
     ch = getch();
 
     if(ch == STK_GET_PARAMETER) {
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e34:	dd 24       	eor	r13, r13
-    3e36:	d3 94       	inc	r13
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
+    3e4c:	dd 24       	eor	r13, r13
+    3e4e:	d3 94       	inc	r13
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
         addrPtr += 2;
       } while (--ch);
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3e38:	a5 e0       	ldi	r26, 0x05	; 5
-    3e3a:	ea 2e       	mov	r14, r26
+      __boot_page_write_short((uint16_t)(void*)address);
+    3e50:	a5 e0       	ldi	r26, 0x05	; 5
+    3e52:	ea 2e       	mov	r14, r26
       boot_spm_busy_wait();
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3e3c:	f1 e1       	ldi	r31, 0x11	; 17
-    3e3e:	ff 2e       	mov	r15, r31
+    3e54:	f1 e1       	ldi	r31, 0x11	; 17
+    3e56:	ff 2e       	mov	r15, r31
 #endif
 
   /* Forever loop */
   for (;;) {
     /* get character from UART */
     ch = getch();
-    3e40:	ab d0       	rcall	.+342    	; 0x3f98 <getch>
+    3e58:	ab d0       	rcall	.+342    	; 0x3fb0 <getch>
 
     if(ch == STK_GET_PARAMETER) {
-    3e42:	81 34       	cpi	r24, 0x41	; 65
-    3e44:	21 f4       	brne	.+8      	; 0x3e4e <main+0x4e>
+    3e5a:	81 34       	cpi	r24, 0x41	; 65
+    3e5c:	21 f4       	brne	.+8      	; 0x3e66 <main+0x66>
       // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy
       getNch(1);
-    3e46:	81 e0       	ldi	r24, 0x01	; 1
-    3e48:	d1 d0       	rcall	.+418    	; 0x3fec <verifySpace+0xc>
+    3e5e:	81 e0       	ldi	r24, 0x01	; 1
+    3e60:	c5 d0       	rcall	.+394    	; 0x3fec <getNch>
       putch(0x03);
-    3e4a:	83 e0       	ldi	r24, 0x03	; 3
-    3e4c:	24 c0       	rjmp	.+72     	; 0x3e96 <main+0x96>
+    3e62:	83 e0       	ldi	r24, 0x03	; 3
+    3e64:	20 c0       	rjmp	.+64     	; 0x3ea6 <main+0xa6>
     }
     else if(ch == STK_SET_DEVICE) {
-    3e4e:	82 34       	cpi	r24, 0x42	; 66
-    3e50:	11 f4       	brne	.+4      	; 0x3e56 <main+0x56>
+    3e66:	82 34       	cpi	r24, 0x42	; 66
+    3e68:	11 f4       	brne	.+4      	; 0x3e6e <main+0x6e>
       // SET DEVICE is ignored
       getNch(20);
-    3e52:	84 e1       	ldi	r24, 0x14	; 20
-    3e54:	03 c0       	rjmp	.+6      	; 0x3e5c <main+0x5c>
+    3e6a:	84 e1       	ldi	r24, 0x14	; 20
+    3e6c:	03 c0       	rjmp	.+6      	; 0x3e74 <main+0x74>
     }
     else if(ch == STK_SET_DEVICE_EXT) {
-    3e56:	85 34       	cpi	r24, 0x45	; 69
-    3e58:	19 f4       	brne	.+6      	; 0x3e60 <main+0x60>
+    3e6e:	85 34       	cpi	r24, 0x45	; 69
+    3e70:	19 f4       	brne	.+6      	; 0x3e78 <main+0x78>
       // SET DEVICE EXT is ignored
       getNch(5);
-    3e5a:	85 e0       	ldi	r24, 0x05	; 5
-    3e5c:	c7 d0       	rcall	.+398    	; 0x3fec <verifySpace+0xc>
-    3e5e:	8a c0       	rjmp	.+276    	; 0x3f74 <main+0x174>
+    3e72:	85 e0       	ldi	r24, 0x05	; 5
+    3e74:	bb d0       	rcall	.+374    	; 0x3fec <getNch>
+    3e76:	91 c0       	rjmp	.+290    	; 0x3f9a <main+0x19a>
     }
     else if(ch == STK_LOAD_ADDRESS) {
-    3e60:	85 35       	cpi	r24, 0x55	; 85
-    3e62:	a1 f4       	brne	.+40     	; 0x3e8c <main+0x8c>
+    3e78:	85 35       	cpi	r24, 0x55	; 85
+    3e7a:	81 f4       	brne	.+32     	; 0x3e9c <main+0x9c>
       // LOAD ADDRESS
-      address = getch();
-    3e64:	99 d0       	rcall	.+306    	; 0x3f98 <getch>
-    3e66:	08 2f       	mov	r16, r24
-    3e68:	10 e0       	ldi	r17, 0x00	; 0
-    3e6a:	10 93 01 02 	sts	0x0201, r17
-    3e6e:	00 93 00 02 	sts	0x0200, r16
-      address = (address & 0xff) | (getch() << 8);
-    3e72:	92 d0       	rcall	.+292    	; 0x3f98 <getch>
-    3e74:	90 e0       	ldi	r25, 0x00	; 0
-    3e76:	98 2f       	mov	r25, r24
-    3e78:	88 27       	eor	r24, r24
-    3e7a:	80 2b       	or	r24, r16
-    3e7c:	91 2b       	or	r25, r17
-      address += address; // Convert from word address to byte address
-    3e7e:	88 0f       	add	r24, r24
-    3e80:	99 1f       	adc	r25, r25
-    3e82:	90 93 01 02 	sts	0x0201, r25
-    3e86:	80 93 00 02 	sts	0x0200, r24
-    3e8a:	73 c0       	rjmp	.+230    	; 0x3f72 <main+0x172>
+      uint16_t newAddress;
+      newAddress = getch();
+    3e7c:	99 d0       	rcall	.+306    	; 0x3fb0 <getch>
+      newAddress = (newAddress & 0xff) | (getch() << 8);
+    3e7e:	08 2f       	mov	r16, r24
+    3e80:	10 e0       	ldi	r17, 0x00	; 0
+    3e82:	96 d0       	rcall	.+300    	; 0x3fb0 <getch>
+    3e84:	90 e0       	ldi	r25, 0x00	; 0
+    3e86:	98 2f       	mov	r25, r24
+    3e88:	88 27       	eor	r24, r24
+    3e8a:	80 2b       	or	r24, r16
+    3e8c:	91 2b       	or	r25, r17
+#ifdef RAMPZ
+      // Transfer top bit to RAMPZ
+      RAMPZ = (newAddress & 0x8000) ? 1 : 0;
+#endif
+      newAddress += newAddress; // Convert from word address to byte address
+    3e8e:	88 0f       	add	r24, r24
+    3e90:	99 1f       	adc	r25, r25
+      address = newAddress;
+    3e92:	90 93 01 02 	sts	0x0201, r25
+    3e96:	80 93 00 02 	sts	0x0200, r24
+    3e9a:	7e c0       	rjmp	.+252    	; 0x3f98 <main+0x198>
       verifySpace();
     }
     else if(ch == STK_UNIVERSAL) {
-    3e8c:	86 35       	cpi	r24, 0x56	; 86
-    3e8e:	29 f4       	brne	.+10     	; 0x3e9a <main+0x9a>
+    3e9c:	86 35       	cpi	r24, 0x56	; 86
+    3e9e:	29 f4       	brne	.+10     	; 0x3eaa <main+0xaa>
       // UNIVERSAL command is ignored
       getNch(4);
-    3e90:	84 e0       	ldi	r24, 0x04	; 4
-    3e92:	ac d0       	rcall	.+344    	; 0x3fec <verifySpace+0xc>
+    3ea0:	84 e0       	ldi	r24, 0x04	; 4
+    3ea2:	a4 d0       	rcall	.+328    	; 0x3fec <getNch>
       putch(0x00);
-    3e94:	80 e0       	ldi	r24, 0x00	; 0
-    3e96:	71 d0       	rcall	.+226    	; 0x3f7a <putch>
-    3e98:	6d c0       	rjmp	.+218    	; 0x3f74 <main+0x174>
+    3ea4:	80 e0       	ldi	r24, 0x00	; 0
+    3ea6:	7c d0       	rcall	.+248    	; 0x3fa0 <putch>
+    3ea8:	78 c0       	rjmp	.+240    	; 0x3f9a <main+0x19a>
     }
-    /* Write memory, length is big endian and is in bytes  */
+    /* Write memory, length is big endian and is in bytes */
     else if(ch == STK_PROG_PAGE) {
-    3e9a:	84 36       	cpi	r24, 0x64	; 100
-    3e9c:	09 f0       	breq	.+2      	; 0x3ea0 <main+0xa0>
-    3e9e:	43 c0       	rjmp	.+134    	; 0x3f26 <main+0x126>
+    3eaa:	84 36       	cpi	r24, 0x64	; 100
+    3eac:	09 f0       	breq	.+2      	; 0x3eb0 <main+0xb0>
+    3eae:	4e c0       	rjmp	.+156    	; 0x3f4c <main+0x14c>
       // PROGRAM PAGE - we support flash programming only, not EEPROM
       uint8_t *bufPtr;
       uint16_t addrPtr;
 
       getLen();
-    3ea0:	8f d0       	rcall	.+286    	; 0x3fc0 <getLen>
-
-      // Immediately start page erase - this will 4.5ms
-      boot_page_erase((uint16_t)(void*)address);
-    3ea2:	e0 91 00 02 	lds	r30, 0x0200
-    3ea6:	f0 91 01 02 	lds	r31, 0x0201
-    3eaa:	83 e0       	ldi	r24, 0x03	; 3
-    3eac:	80 93 57 00 	sts	0x0057, r24
-    3eb0:	e8 95       	spm
-    3eb2:	c0 e0       	ldi	r28, 0x00	; 0
-    3eb4:	d1 e0       	ldi	r29, 0x01	; 1
-
+    3eb0:	87 d0       	rcall	.+270    	; 0x3fc0 <getLen>
+
+      // If we are in RWW section, immediately start page erase
+      if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3eb2:	e0 91 00 02 	lds	r30, 0x0200
+    3eb6:	f0 91 01 02 	lds	r31, 0x0201
+    3eba:	88 e3       	ldi	r24, 0x38	; 56
+    3ebc:	e0 30       	cpi	r30, 0x00	; 0
+    3ebe:	f8 07       	cpc	r31, r24
+    3ec0:	18 f4       	brcc	.+6      	; 0x3ec8 <main+0xc8>
+    3ec2:	83 e0       	ldi	r24, 0x03	; 3
+    3ec4:	87 bf       	out	0x37, r24	; 55
+    3ec6:	e8 95       	spm
+    3ec8:	c0 e0       	ldi	r28, 0x00	; 0
+    3eca:	d1 e0       	ldi	r29, 0x01	; 1
+      
       // While that is going on, read in page contents
       bufPtr = buff;
       do *bufPtr++ = getch();
-    3eb6:	70 d0       	rcall	.+224    	; 0x3f98 <getch>
-    3eb8:	89 93       	st	Y+, r24
+    3ecc:	71 d0       	rcall	.+226    	; 0x3fb0 <getch>
+    3ece:	89 93       	st	Y+, r24
       while (--length);
-    3eba:	80 91 02 02 	lds	r24, 0x0202
-    3ebe:	81 50       	subi	r24, 0x01	; 1
-    3ec0:	80 93 02 02 	sts	0x0202, r24
-    3ec4:	88 23       	and	r24, r24
-    3ec6:	b9 f7       	brne	.-18     	; 0x3eb6 <main+0xb6>
+    3ed0:	80 91 02 02 	lds	r24, 0x0202
+    3ed4:	81 50       	subi	r24, 0x01	; 1
+    3ed6:	80 93 02 02 	sts	0x0202, r24
+    3eda:	88 23       	and	r24, r24
+    3edc:	b9 f7       	brne	.-18     	; 0x3ecc <main+0xcc>
+
+      // If we are in NRWW section, page erase has to be delayed until now.
+      // Todo: Take RAMPZ into account
+      if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address);
+    3ede:	e0 91 00 02 	lds	r30, 0x0200
+    3ee2:	f0 91 01 02 	lds	r31, 0x0201
+    3ee6:	88 e3       	ldi	r24, 0x38	; 56
+    3ee8:	e0 30       	cpi	r30, 0x00	; 0
+    3eea:	f8 07       	cpc	r31, r24
+    3eec:	18 f0       	brcs	.+6      	; 0x3ef4 <main+0xf4>
+    3eee:	83 e0       	ldi	r24, 0x03	; 3
+    3ef0:	87 bf       	out	0x37, r24	; 55
+    3ef2:	e8 95       	spm
 
       // Read command terminator, start reply
       verifySpace();
-    3ec8:	8b d0       	rcall	.+278    	; 0x3fe0 <verifySpace>
+    3ef4:	75 d0       	rcall	.+234    	; 0x3fe0 <verifySpace>
       
       // If only a partial page is to be programmed, the erase might not be complete.
       // So check that here
       boot_spm_busy_wait();
-    3eca:	07 b6       	in	r0, 0x37	; 55
-    3ecc:	00 fc       	sbrc	r0, 0
-    3ece:	fd cf       	rjmp	.-6      	; 0x3eca <main+0xca>
+    3ef6:	07 b6       	in	r0, 0x37	; 55
+    3ef8:	00 fc       	sbrc	r0, 0
+    3efa:	fd cf       	rjmp	.-6      	; 0x3ef6 <main+0xf6>
       }
 #endif
 
       // Copy buffer into programming buffer
       bufPtr = buff;
       addrPtr = (uint16_t)(void*)address;
-    3ed0:	40 91 00 02 	lds	r20, 0x0200
-    3ed4:	50 91 01 02 	lds	r21, 0x0201
-    3ed8:	a0 e0       	ldi	r26, 0x00	; 0
-    3eda:	b1 e0       	ldi	r27, 0x01	; 1
+    3efc:	40 91 00 02 	lds	r20, 0x0200
+    3f00:	50 91 01 02 	lds	r21, 0x0201
+    3f04:	a0 e0       	ldi	r26, 0x00	; 0
+    3f06:	b1 e0       	ldi	r27, 0x01	; 1
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
-    3edc:	2c 91       	ld	r18, X
-    3ede:	30 e0       	ldi	r19, 0x00	; 0
+    3f08:	2c 91       	ld	r18, X
+    3f0a:	30 e0       	ldi	r19, 0x00	; 0
         a |= (*bufPtr++) << 8;
-    3ee0:	11 96       	adiw	r26, 0x01	; 1
-    3ee2:	8c 91       	ld	r24, X
-    3ee4:	11 97       	sbiw	r26, 0x01	; 1
-    3ee6:	90 e0       	ldi	r25, 0x00	; 0
-    3ee8:	98 2f       	mov	r25, r24
-    3eea:	88 27       	eor	r24, r24
-    3eec:	82 2b       	or	r24, r18
-    3eee:	93 2b       	or	r25, r19
-#ifdef VIRTUAL_BOOT_PARTITION
-#define rstVect (*(uint16_t*)(0x204))
-#define wdtVect (*(uint16_t*)(0x206))
+    3f0c:	11 96       	adiw	r26, 0x01	; 1
+    3f0e:	8c 91       	ld	r24, X
+    3f10:	11 97       	sbiw	r26, 0x01	; 1
+    3f12:	90 e0       	ldi	r25, 0x00	; 0
+    3f14:	98 2f       	mov	r25, r24
+    3f16:	88 27       	eor	r24, r24
+    3f18:	82 2b       	or	r24, r18
+    3f1a:	93 2b       	or	r25, r19
+#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4))
+#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6))
 #endif
+
 /* main program starts here */
 int main(void) {
-    3ef0:	12 96       	adiw	r26, 0x02	; 2
+    3f1c:	12 96       	adiw	r26, 0x02	; 2
       ch = SPM_PAGESIZE / 2;
       do {
         uint16_t a;
         a = *bufPtr++;
         a |= (*bufPtr++) << 8;
-        boot_page_fill((uint16_t)(void*)addrPtr,a);
-    3ef2:	fa 01       	movw	r30, r20
-    3ef4:	0c 01       	movw	r0, r24
-    3ef6:	d0 92 57 00 	sts	0x0057, r13
-    3efa:	e8 95       	spm
-    3efc:	11 24       	eor	r1, r1
+        __boot_page_fill_short((uint16_t)(void*)addrPtr,a);
+    3f1e:	fa 01       	movw	r30, r20
+    3f20:	0c 01       	movw	r0, r24
+    3f22:	d7 be       	out	0x37, r13	; 55
+    3f24:	e8 95       	spm
+    3f26:	11 24       	eor	r1, r1
         addrPtr += 2;
-    3efe:	4e 5f       	subi	r20, 0xFE	; 254
-    3f00:	5f 4f       	sbci	r21, 0xFF	; 255
+    3f28:	4e 5f       	subi	r20, 0xFE	; 254
+    3f2a:	5f 4f       	sbci	r21, 0xFF	; 255
       } while (--ch);
-    3f02:	f1 e0       	ldi	r31, 0x01	; 1
-    3f04:	a0 38       	cpi	r26, 0x80	; 128
-    3f06:	bf 07       	cpc	r27, r31
-    3f08:	49 f7       	brne	.-46     	; 0x3edc <main+0xdc>
+    3f2c:	f1 e0       	ldi	r31, 0x01	; 1
+    3f2e:	a0 38       	cpi	r26, 0x80	; 128
+    3f30:	bf 07       	cpc	r27, r31
+    3f32:	51 f7       	brne	.-44     	; 0x3f08 <main+0x108>
       
       // Write from programming buffer
-      boot_page_write((uint16_t)(void*)address);
-    3f0a:	e0 91 00 02 	lds	r30, 0x0200
-    3f0e:	f0 91 01 02 	lds	r31, 0x0201
-    3f12:	e0 92 57 00 	sts	0x0057, r14
-    3f16:	e8 95       	spm
+      __boot_page_write_short((uint16_t)(void*)address);
+    3f34:	e0 91 00 02 	lds	r30, 0x0200
+    3f38:	f0 91 01 02 	lds	r31, 0x0201
+    3f3c:	e7 be       	out	0x37, r14	; 55
+    3f3e:	e8 95       	spm
       boot_spm_busy_wait();
-    3f18:	07 b6       	in	r0, 0x37	; 55
-    3f1a:	00 fc       	sbrc	r0, 0
-    3f1c:	fd cf       	rjmp	.-6      	; 0x3f18 <main+0x118>
+    3f40:	07 b6       	in	r0, 0x37	; 55
+    3f42:	00 fc       	sbrc	r0, 0
+    3f44:	fd cf       	rjmp	.-6      	; 0x3f40 <main+0x140>
 
 #if defined(RWWSRE)
       // Reenable read access to flash
       boot_rww_enable();
-    3f1e:	f0 92 57 00 	sts	0x0057, r15
-    3f22:	e8 95       	spm
-    3f24:	27 c0       	rjmp	.+78     	; 0x3f74 <main+0x174>
+    3f46:	f7 be       	out	0x37, r15	; 55
+    3f48:	e8 95       	spm
+    3f4a:	27 c0       	rjmp	.+78     	; 0x3f9a <main+0x19a>
 #endif
 
     }
     /* Read memory block mode, length is big endian.  */
     else if(ch == STK_READ_PAGE) {
-    3f26:	84 37       	cpi	r24, 0x74	; 116
-    3f28:	b9 f4       	brne	.+46     	; 0x3f58 <main+0x158>
+    3f4c:	84 37       	cpi	r24, 0x74	; 116
+    3f4e:	b9 f4       	brne	.+46     	; 0x3f7e <main+0x17e>
       // READ PAGE - we only read flash
       getLen();
-    3f2a:	4a d0       	rcall	.+148    	; 0x3fc0 <getLen>
+    3f50:	37 d0       	rcall	.+110    	; 0x3fc0 <getLen>
       verifySpace();
-    3f2c:	59 d0       	rcall	.+178    	; 0x3fe0 <verifySpace>
-        else ch = pgm_read_byte_near(address);
+    3f52:	46 d0       	rcall	.+140    	; 0x3fe0 <verifySpace>
+        putch(result);
         address++;
-        putch(ch);
-      } while (--length);
+      }
+      while (--length);
 #else
       do putch(pgm_read_byte_near(address++));
-    3f2e:	e0 91 00 02 	lds	r30, 0x0200
-    3f32:	f0 91 01 02 	lds	r31, 0x0201
-    3f36:	31 96       	adiw	r30, 0x01	; 1
-    3f38:	f0 93 01 02 	sts	0x0201, r31
-    3f3c:	e0 93 00 02 	sts	0x0200, r30
-    3f40:	31 97       	sbiw	r30, 0x01	; 1
-    3f42:	e4 91       	lpm	r30, Z+
-    3f44:	8e 2f       	mov	r24, r30
-    3f46:	19 d0       	rcall	.+50     	; 0x3f7a <putch>
+    3f54:	e0 91 00 02 	lds	r30, 0x0200
+    3f58:	f0 91 01 02 	lds	r31, 0x0201
+    3f5c:	31 96       	adiw	r30, 0x01	; 1
+    3f5e:	f0 93 01 02 	sts	0x0201, r31
+    3f62:	e0 93 00 02 	sts	0x0200, r30
+    3f66:	31 97       	sbiw	r30, 0x01	; 1
+    3f68:	e4 91       	lpm	r30, Z+
+    3f6a:	8e 2f       	mov	r24, r30
+    3f6c:	19 d0       	rcall	.+50     	; 0x3fa0 <putch>
       while (--length);
-    3f48:	80 91 02 02 	lds	r24, 0x0202
-    3f4c:	81 50       	subi	r24, 0x01	; 1
-    3f4e:	80 93 02 02 	sts	0x0202, r24
-    3f52:	88 23       	and	r24, r24
-    3f54:	61 f7       	brne	.-40     	; 0x3f2e <main+0x12e>
-    3f56:	0e c0       	rjmp	.+28     	; 0x3f74 <main+0x174>
+    3f6e:	80 91 02 02 	lds	r24, 0x0202
+    3f72:	81 50       	subi	r24, 0x01	; 1
+    3f74:	80 93 02 02 	sts	0x0202, r24
+    3f78:	88 23       	and	r24, r24
+    3f7a:	61 f7       	brne	.-40     	; 0x3f54 <main+0x154>
+    3f7c:	0e c0       	rjmp	.+28     	; 0x3f9a <main+0x19a>
+#endif
 #endif
     }
 
     /* Get device signature bytes  */
     else if(ch == STK_READ_SIGN) {
-    3f58:	85 37       	cpi	r24, 0x75	; 117
-    3f5a:	39 f4       	brne	.+14     	; 0x3f6a <main+0x16a>
+    3f7e:	85 37       	cpi	r24, 0x75	; 117
+    3f80:	39 f4       	brne	.+14     	; 0x3f90 <main+0x190>
       // READ SIGN - return what Avrdude wants to hear
       verifySpace();
-    3f5c:	41 d0       	rcall	.+130    	; 0x3fe0 <verifySpace>
+    3f82:	2e d0       	rcall	.+92     	; 0x3fe0 <verifySpace>
       putch(SIGNATURE_0);
-    3f5e:	8e e1       	ldi	r24, 0x1E	; 30
-    3f60:	0c d0       	rcall	.+24     	; 0x3f7a <putch>
+    3f84:	8e e1       	ldi	r24, 0x1E	; 30
+    3f86:	0c d0       	rcall	.+24     	; 0x3fa0 <putch>
       putch(SIGNATURE_1);
-    3f62:	84 e9       	ldi	r24, 0x94	; 148
-    3f64:	0a d0       	rcall	.+20     	; 0x3f7a <putch>
+    3f88:	84 e9       	ldi	r24, 0x94	; 148
+    3f8a:	0a d0       	rcall	.+20     	; 0x3fa0 <putch>
       putch(SIGNATURE_2);
-    3f66:	86 e0       	ldi	r24, 0x06	; 6
-    3f68:	96 cf       	rjmp	.-212    	; 0x3e96 <main+0x96>
+    3f8c:	86 e0       	ldi	r24, 0x06	; 6
+    3f8e:	8b cf       	rjmp	.-234    	; 0x3ea6 <main+0xa6>
     }
     else if (ch == 'Q') {
-    3f6a:	81 35       	cpi	r24, 0x51	; 81
-    3f6c:	11 f4       	brne	.+4      	; 0x3f72 <main+0x172>
+    3f90:	81 35       	cpi	r24, 0x51	; 81
+    3f92:	11 f4       	brne	.+4      	; 0x3f98 <main+0x198>
       // Adaboot no-wait mod
       watchdogConfig(WATCHDOG_16MS);
-    3f6e:	88 e0       	ldi	r24, 0x08	; 8
-    3f70:	2c d0       	rcall	.+88     	; 0x3fca <watchdogConfig>
+    3f94:	88 e0       	ldi	r24, 0x08	; 8
+    3f96:	19 d0       	rcall	.+50     	; 0x3fca <watchdogConfig>
       verifySpace();
     }
     else {
       // This covers the response to commands like STK_ENTER_PROGMODE
       verifySpace();
-    3f72:	36 d0       	rcall	.+108    	; 0x3fe0 <verifySpace>
+    3f98:	23 d0       	rcall	.+70     	; 0x3fe0 <verifySpace>
     }
     putch(STK_OK);
-    3f74:	80 e1       	ldi	r24, 0x10	; 16
-    3f76:	01 d0       	rcall	.+2      	; 0x3f7a <putch>
-    3f78:	63 cf       	rjmp	.-314    	; 0x3e40 <main+0x40>
+    3f9a:	80 e1       	ldi	r24, 0x10	; 16
+    3f9c:	01 d0       	rcall	.+2      	; 0x3fa0 <putch>
+    3f9e:	5c cf       	rjmp	.-328    	; 0x3e58 <main+0x58>
+
+00003fa0 <putch>:
+  }
+}
 
-00003f7a <putch>:
 void putch(char ch) {
+    3fa0:	98 2f       	mov	r25, r24
 #ifndef SOFT_UART
   while (!(UCSR0A & _BV(UDRE0)));
+    3fa2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fa6:	85 ff       	sbrs	r24, 5
+    3fa8:	fc cf       	rjmp	.-8      	; 0x3fa2 <putch+0x2>
   UDR0 = ch;
-#else
-  __asm__ __volatile__ (
-    3f7a:	2a e0       	ldi	r18, 0x0A	; 10
-    3f7c:	30 e0       	ldi	r19, 0x00	; 0
-    3f7e:	80 95       	com	r24
-    3f80:	08 94       	sec
-    3f82:	10 f4       	brcc	.+4      	; 0x3f88 <putch+0xe>
-    3f84:	59 98       	cbi	0x0b, 1	; 11
-    3f86:	02 c0       	rjmp	.+4      	; 0x3f8c <putch+0x12>
-    3f88:	59 9a       	sbi	0x0b, 1	; 11
-    3f8a:	00 00       	nop
-    3f8c:	15 d0       	rcall	.+42     	; 0x3fb8 <uartDelay>
-    3f8e:	14 d0       	rcall	.+40     	; 0x3fb8 <uartDelay>
-    3f90:	86 95       	lsr	r24
-    3f92:	2a 95       	dec	r18
-    3f94:	b1 f7       	brne	.-20     	; 0x3f82 <putch+0x8>
+    3faa:	90 93 c6 00 	sts	0x00C6, r25
       [uartBit] "I" (UART_TX_BIT)
     :
       "r25"
   );
 #endif
 }
-    3f96:	08 95       	ret
+    3fae:	08 95       	ret
 
-00003f98 <getch>:
+00003fb0 <getch>:
   return getch();
 }
 
 // Watchdog functions. These are only safe with interrupts turned off.
 void watchdogReset() {
   __asm__ __volatile__ (
-    3f98:	a8 95       	wdr
-#ifdef LED_DATA_FLASH
+    3fb0:	a8 95       	wdr
+      [uartBit] "I" (UART_RX_BIT)
+    :
+      "r25"
+);
+#else
+  while(!(UCSR0A & _BV(RXC0)));
+    3fb2:	80 91 c0 00 	lds	r24, 0x00C0
+    3fb6:	87 ff       	sbrs	r24, 7
+    3fb8:	fc cf       	rjmp	.-8      	; 0x3fb2 <getch+0x2>
+  ch = UDR0;
+    3fba:	80 91 c6 00 	lds	r24, 0x00C6
   LED_PIN |= _BV(LED);
 #endif
+#endif
 
   return ch;
 }
-    3f9a:	29 e0       	ldi	r18, 0x09	; 9
-    3f9c:	30 e0       	ldi	r19, 0x00	; 0
-    3f9e:	48 99       	sbic	0x09, 0	; 9
-    3fa0:	fe cf       	rjmp	.-4      	; 0x3f9e <getch+0x6>
-    3fa2:	0a d0       	rcall	.+20     	; 0x3fb8 <uartDelay>
-    3fa4:	09 d0       	rcall	.+18     	; 0x3fb8 <uartDelay>
-    3fa6:	08 d0       	rcall	.+16     	; 0x3fb8 <uartDelay>
-    3fa8:	88 94       	clc
-    3faa:	48 99       	sbic	0x09, 0	; 9
-    3fac:	08 94       	sec
-    3fae:	2a 95       	dec	r18
-    3fb0:	11 f0       	breq	.+4      	; 0x3fb6 <getch+0x1e>
-    3fb2:	87 95       	ror	r24
-    3fb4:	f7 cf       	rjmp	.-18     	; 0x3fa4 <getch+0xc>
-    3fb6:	08 95       	ret
-
-00003fb8 <uartDelay>:
-#if UART_B_VALUE > 255
-#error Baud rate too slow for soft UART
-#endif
-
-void uartDelay() {
-  __asm__ __volatile__ (
-    3fb8:	98 e0       	ldi	r25, 0x08	; 8
-    3fba:	9a 95       	dec	r25
-    3fbc:	f1 f7       	brne	.-4      	; 0x3fba <uartDelay+0x2>
     3fbe:	08 95       	ret
 
 00003fc0 <getLen>:
@@ -462,13 +483,13 @@ void uartDelay() {
 
 uint8_t getLen() {
   getch();
-    3fc0:	eb df       	rcall	.-42     	; 0x3f98 <getch>
+    3fc0:	f7 df       	rcall	.-18     	; 0x3fb0 <getch>
   length = getch();
-    3fc2:	ea df       	rcall	.-44     	; 0x3f98 <getch>
+    3fc2:	f6 df       	rcall	.-20     	; 0x3fb0 <getch>
     3fc4:	80 93 02 02 	sts	0x0202, r24
   return getch();
 }
-    3fc8:	e7 cf       	rjmp	.-50     	; 0x3f98 <getch>
+    3fc8:	f3 cf       	rjmp	.-26     	; 0x3fb0 <getch>
 
 00003fca <watchdogConfig>:
     "wdr\n"
@@ -504,14 +525,16 @@ void appStart() {
 
 void verifySpace() {
   if (getch() != CRC_EOP) appStart();
-    3fe0:	db df       	rcall	.-74     	; 0x3f98 <getch>
+    3fe0:	e7 df       	rcall	.-50     	; 0x3fb0 <getch>
     3fe2:	80 32       	cpi	r24, 0x20	; 32
     3fe4:	09 f0       	breq	.+2      	; 0x3fe8 <verifySpace+0x8>
     3fe6:	f7 df       	rcall	.-18     	; 0x3fd6 <appStart>
   putch(STK_INSYNC);
     3fe8:	84 e1       	ldi	r24, 0x14	; 20
 }
-    3fea:	c7 cf       	rjmp	.-114    	; 0x3f7a <putch>
+    3fea:	da cf       	rjmp	.-76     	; 0x3fa0 <putch>
+
+00003fec <getNch>:
     ::[count] "M" (UART_B_VALUE)
   );
 }
@@ -520,12 +543,10 @@ void verifySpace() {
 void getNch(uint8_t count) {
     3fec:	1f 93       	push	r17
     3fee:	18 2f       	mov	r17, r24
-
-00003ff0 <getNch>:
   do getch(); while (--count);
-    3ff0:	d3 df       	rcall	.-90     	; 0x3f98 <getch>
+    3ff0:	df df       	rcall	.-66     	; 0x3fb0 <getch>
     3ff2:	11 50       	subi	r17, 0x01	; 1
-    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch>
+    3ff4:	e9 f7       	brne	.-6      	; 0x3ff0 <getNch+0x4>
   verifySpace();
     3ff6:	f4 df       	rcall	.-24     	; 0x3fe0 <verifySpace>
 }
diff --git a/bootloaders/optiboot/pin_defs.h b/bootloaders/optiboot/pin_defs.h
new file mode 100644
index 0000000..313e453
--- /dev/null
+++ b/bootloaders/optiboot/pin_defs.h
@@ -0,0 +1,79 @@
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) || defined(__AVR_ATmega88) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega88__)
+/* Onboard LED is connected to pin PB5 in Arduino NG, Diecimila, and Duemilanove */ 
+#define LED_DDR     DDRB
+#define LED_PORT    PORTB
+#define LED_PIN     PINB
+#define LED         PINB5
+
+/* Ports for soft UART */
+#ifdef SOFT_UART
+#define UART_PORT   PORTD
+#define UART_PIN    PIND
+#define UART_DDR    DDRD
+#define UART_TX_BIT 1
+#define UART_RX_BIT 0
+#endif
+#endif
+
+#if defined(__AVR_ATmega8__)
+  //Name conversion R.Wiersma
+  #define UCSR0A	UCSRA
+  #define UDR0 		UDR
+  #define UDRE0 	UDRE
+  #define RXC0		RXC
+  #define TIFR1 	TIFR
+  #define WDTCSR	WDTCR
+#endif
+
+/* Luminet support */
+#if defined(__AVR_ATtiny84__)
+/* Red LED is connected to pin PA4 */ 
+#define LED_DDR     DDRA
+#define LED_PORT    PORTA
+#define LED_PIN     PINA
+#define LED         PINA4
+/* Ports for soft UART - left port only for now. TX/RX on PA2/PA3 */
+#ifdef SOFT_UART
+#define UART_PORT   PORTA
+#define UART_PIN    PINA
+#define UART_DDR    DDRA
+#define UART_TX_BIT 2
+#define UART_RX_BIT 3
+#endif
+#endif
+
+/* Sanguino support */
+#if defined(__AVR_ATmega644P__)
+/* Onboard LED is connected to pin PB0 on Sanguino */ 
+#define LED_DDR     DDRB
+#define LED_PORT    PORTB
+#define LED_PIN     PINB
+#define LED         PINB0
+
+/* Ports for soft UART */
+#ifdef SOFT_UART
+#define UART_PORT   PORTD
+#define UART_PIN    PIND
+#define UART_DDR    DDRD
+#define UART_TX_BIT 1
+#define UART_RX_BIT 0
+#endif
+#endif
+
+/* Mega support */
+#if defined(__AVR_ATmega1280__)
+/* Onboard LED is connected to pin PB7 on Arduino Mega */ 
+#define LED_DDR     DDRB
+#define LED_PORT    PORTB
+#define LED_PIN     PINB
+#define LED         PINB7
+
+/* Ports for soft UART */
+#ifdef SOFT_UART
+#define UART_PORT   PORTE
+#define UART_PIN    PINE
+#define UART_DDR    DDRE
+#define UART_TX_BIT 1
+#define UART_RX_BIT 0
+#endif
+#endif
diff --git a/bootloaders/optiboot/stk500.h b/bootloaders/optiboot/stk500.h
new file mode 100644
index 0000000..ca0dd91
--- /dev/null
+++ b/bootloaders/optiboot/stk500.h
@@ -0,0 +1,39 @@
+/* STK500 constants list, from AVRDUDE */
+#define STK_OK              0x10
+#define STK_FAILED          0x11  // Not used
+#define STK_UNKNOWN         0x12  // Not used
+#define STK_NODEVICE        0x13  // Not used
+#define STK_INSYNC          0x14  // ' '
+#define STK_NOSYNC          0x15  // Not used
+#define ADC_CHANNEL_ERROR   0x16  // Not used
+#define ADC_MEASURE_OK      0x17  // Not used
+#define PWM_CHANNEL_ERROR   0x18  // Not used
+#define PWM_ADJUST_OK       0x19  // Not used
+#define CRC_EOP             0x20  // 'SPACE'
+#define STK_GET_SYNC        0x30  // '0'
+#define STK_GET_SIGN_ON     0x31  // '1'
+#define STK_SET_PARAMETER   0x40  // '@'
+#define STK_GET_PARAMETER   0x41  // 'A'
+#define STK_SET_DEVICE      0x42  // 'B'
+#define STK_SET_DEVICE_EXT  0x45  // 'E'
+#define STK_ENTER_PROGMODE  0x50  // 'P'
+#define STK_LEAVE_PROGMODE  0x51  // 'Q'
+#define STK_CHIP_ERASE      0x52  // 'R'
+#define STK_CHECK_AUTOINC   0x53  // 'S'
+#define STK_LOAD_ADDRESS    0x55  // 'U'
+#define STK_UNIVERSAL       0x56  // 'V'
+#define STK_PROG_FLASH      0x60  // '`'
+#define STK_PROG_DATA       0x61  // 'a'
+#define STK_PROG_FUSE       0x62  // 'b'
+#define STK_PROG_LOCK       0x63  // 'c'
+#define STK_PROG_PAGE       0x64  // 'd'
+#define STK_PROG_FUSE_EXT   0x65  // 'e'
+#define STK_READ_FLASH      0x70  // 'p'
+#define STK_READ_DATA       0x71  // 'q'
+#define STK_READ_FUSE       0x72  // 'r'
+#define STK_READ_LOCK       0x73  // 's'
+#define STK_READ_PAGE       0x74  // 't'
+#define STK_READ_SIGN       0x75  // 'u'
+#define STK_READ_OSCCAL     0x76  // 'v'
+#define STK_READ_FUSE_EXT   0x77  // 'w'
+#define STK_READ_OSCCAL_EXT 0x78  // 'x'