[gnoduino: 155/237] This is a relatively significant edit that brings the Arduino copy of optiboot up-to-date with the o



commit 46a6c9f3fec4c23397345e6006ff0aea9e6a18ec
Author: WestfW <westfw gmail com>
Date:   Fri Jun 10 16:17:13 2011 -0700

    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)

 arduino/bootloaders/optiboot/Makefile              |  125 +++-
 arduino/bootloaders/optiboot/boot.h                |  848 ++++++++++++++++++++
 arduino/bootloaders/optiboot/optiboot.c            |  288 ++++---
 .../bootloaders/optiboot/optiboot_atmega328.hex    |   59 +-
 .../bootloaders/optiboot/optiboot_atmega328.lst    |  486 ++++++------
 .../optiboot/optiboot_atmega328_pro_8MHz.hex       |   63 +-
 .../optiboot/optiboot_atmega328_pro_8MHz.lst       |  542 +++++++------
 .../bootloaders/optiboot/optiboot_diecimila.hex    |   63 +-
 .../bootloaders/optiboot/optiboot_diecimila.lst    |  542 +++++++------
 arduino/bootloaders/optiboot/optiboot_lilypad.hex  |   62 +-
 arduino/bootloaders/optiboot/optiboot_lilypad.lst  |  539 +++++++------
 .../optiboot/optiboot_lilypad_resonator.hex        |   62 +-
 .../optiboot/optiboot_lilypad_resonator.lst        |  539 +++++++------
 arduino/bootloaders/optiboot/optiboot_luminet.hex  |   80 +-
 arduino/bootloaders/optiboot/optiboot_luminet.lst  |  670 ++++++++--------
 .../bootloaders/optiboot/optiboot_pro_16MHz.hex    |   63 +-
 .../bootloaders/optiboot/optiboot_pro_16MHz.lst    |  542 +++++++------
 .../bootloaders/optiboot/optiboot_pro_20mhz.hex    |   63 +-
 .../bootloaders/optiboot/optiboot_pro_20mhz.lst    |  542 +++++++------
 arduino/bootloaders/optiboot/optiboot_pro_8MHz.hex |   62 +-
 arduino/bootloaders/optiboot/optiboot_pro_8MHz.lst |  539 +++++++------
 arduino/bootloaders/optiboot/pin_defs.h            |   79 ++
 arduino/bootloaders/optiboot/stk500.h              |   39 +
 23 files changed, 4141 insertions(+), 2756 deletions(-)
---
diff --git a/arduino/bootloaders/optiboot/Makefile b/arduino/bootloaders/optiboot/Makefile
index 0f8d09e..88cb9a2 100644
--- a/arduino/bootloaders/optiboot/Makefile
+++ b/arduino/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/arduino/bootloaders/optiboot/boot.h b/arduino/bootloaders/optiboot/boot.h
new file mode 100644
index 0000000..2639cd8
--- /dev/null
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot.c b/arduino/bootloaders/optiboot/optiboot.c
index c7d817a..4b4a10a 100644
--- a/arduino/bootloaders/optiboot/optiboot.c
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_atmega328.hex b/arduino/bootloaders/optiboot/optiboot_atmega328.hex
index e8aa31c..72108ee 100644
--- a/arduino/bootloaders/optiboot/optiboot_atmega328.hex
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_atmega328.lst b/arduino/bootloaders/optiboot/optiboot_atmega328.lst
index 104799f..1af4a3c 100644
--- a/arduino/bootloaders/optiboot/optiboot_atmega328.lst
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex b/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex
index d6ac145..f4a1514 100644
--- a/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.hex
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst b/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst
index 46eda68..4cbcf62 100644
--- a/arduino/bootloaders/optiboot/optiboot_atmega328_pro_8MHz.lst
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_diecimila.hex b/arduino/bootloaders/optiboot/optiboot_diecimila.hex
index 1e93414..2b4a582 100644
--- a/arduino/bootloaders/optiboot/optiboot_diecimila.hex
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_diecimila.lst b/arduino/bootloaders/optiboot/optiboot_diecimila.lst
index 1121893..e0a1ecf 100644
--- a/arduino/bootloaders/optiboot/optiboot_diecimila.lst
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_lilypad.hex b/arduino/bootloaders/optiboot/optiboot_lilypad.hex
index 9d31a7a..7579286 100644
--- a/arduino/bootloaders/optiboot/optiboot_lilypad.hex
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_lilypad.lst b/arduino/bootloaders/optiboot/optiboot_lilypad.lst
index 0e46bd1..cbae268 100644
--- a/arduino/bootloaders/optiboot/optiboot_lilypad.lst
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.hex b/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.hex
index 9d31a7a..7579286 100644
--- a/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.hex
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.lst b/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.lst
index 80ecb83..c5d7710 100644
--- a/arduino/bootloaders/optiboot/optiboot_lilypad_resonator.lst
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_luminet.hex b/arduino/bootloaders/optiboot/optiboot_luminet.hex
index 0e51124..3ad3c0a 100644
--- a/arduino/bootloaders/optiboot/optiboot_luminet.hex
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_luminet.lst b/arduino/bootloaders/optiboot/optiboot_luminet.lst
index 59468cb..737d2c5 100644
--- a/arduino/bootloaders/optiboot/optiboot_luminet.lst
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_pro_16MHz.hex b/arduino/bootloaders/optiboot/optiboot_pro_16MHz.hex
index 1e93414..2b4a582 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_16MHz.hex
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_pro_16MHz.lst b/arduino/bootloaders/optiboot/optiboot_pro_16MHz.lst
index 9920a76..25dc46b 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_16MHz.lst
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_pro_20mhz.hex b/arduino/bootloaders/optiboot/optiboot_pro_20mhz.hex
index 19c9ae4..d147f8f 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_20mhz.hex
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_pro_20mhz.lst b/arduino/bootloaders/optiboot/optiboot_pro_20mhz.lst
index 62178d3..1868d16 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_20mhz.lst
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_pro_8MHz.hex b/arduino/bootloaders/optiboot/optiboot_pro_8MHz.hex
index 9d31a7a..7579286 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_8MHz.hex
+++ b/arduino/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/arduino/bootloaders/optiboot/optiboot_pro_8MHz.lst b/arduino/bootloaders/optiboot/optiboot_pro_8MHz.lst
index 94603e2..25a4bd2 100644
--- a/arduino/bootloaders/optiboot/optiboot_pro_8MHz.lst
+++ b/arduino/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/arduino/bootloaders/optiboot/pin_defs.h b/arduino/bootloaders/optiboot/pin_defs.h
new file mode 100644
index 0000000..313e453
--- /dev/null
+++ b/arduino/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/arduino/bootloaders/optiboot/stk500.h b/arduino/bootloaders/optiboot/stk500.h
new file mode 100644
index 0000000..ca0dd91
--- /dev/null
+++ b/arduino/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'



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