diff --git a/Makefile b/Makefile
index 0f7e18b..4395460 100644
--- a/Makefile
+++ b/Makefile
@@ -46,9 +46,9 @@ PROJECT := rffe-fw
 ###############################################################################
 # Objects and Paths
 
-OBJECTS += ./src/Drivers/ADT7320.o
-OBJECTS += ./src/Drivers/LM71.o
 OBJECTS += ./src/Drivers/feram.o
+OBJECTS += ./src/Drivers/CDCE906.o
+OBJECTS += ./src/Drivers/ADT7320.o
 OBJECTS += ./src/Drivers/DAC7554.o
 OBJECTS += ./src/PID/PID.o
 OBJECTS += ./src/bsmp/bsmp.o
@@ -59,9 +59,14 @@ OBJECTS += ./src/cli.o
 OBJECTS += ./src/main.o
 OBJECTS += ./src/pcbnAPI.o
 OBJECTS += ./src/util.o
+OBJECTS += ./boot/IAP.o
+
+BOOT_OBJECTS += ./boot/boot.o
+BOOT_OBJECTS += ./boot/IAP.o
 
 INCLUDE_PATHS += -I../
 INCLUDE_PATHS += -I../.
+INCLUDE_PATHS += -I.././boot
 INCLUDE_PATHS += -I.././mbed-os
 INCLUDE_PATHS += -I.././mbed-os/cmsis
 INCLUDE_PATHS += -I.././mbed-os/cmsis/TARGET_CORTEX_M
@@ -143,9 +148,9 @@ INCLUDE_PATHS += -I.././src/PID
 INCLUDE_PATHS += -I.././src/bsmp
 INCLUDE_PATHS += -I.././src/bsmp/md5
 
-LIBRARY_PATHS := -L./
+LIBRARY_PATHS := -L./ -L../lib/
 LIBRARIES := -lmbed-os
-LINKER_SCRIPT ?= .././mbed-os/targets/TARGET_NXP/TARGET_LPC176X/device/TOOLCHAIN_GCC_ARM/LPC1768.ld
+LINKER_SCRIPT ?= .././linker/LPC1768.ld
 
 # Objects and Paths
 ###############################################################################
@@ -265,9 +270,11 @@ endif
 
 ifeq ($(TEMP_SENSOR),LM71)
 CXX_FLAGS += -DTEMP_SENSOR_LM71
+OBJECTS += ./src/Drivers/LM71.o
 else
 ifeq ($(TEMP_SENSOR),ADT7320)
 CXX_FLAGS += -DTEMP_SENSOR_ADT7320
+OBJECTS += ./src/Drivers/ADT7320.o
 endif
 endif
 
@@ -295,7 +302,7 @@ ASM_FLAGS += -DARM_MATH_CM3
 
 FW_VERSION := $(shell $(MACROS) -E -dM ../src/main.cpp $(CXX_FLAGS) $(INCLUDE_PATHS) | grep "\#define FW_VERSION " | grep -oP "(V\d\_\d\_\d)")
 
-LD_FLAGS :=-Wl,--gc-sections -Wl,--wrap,main -Wl,--wrap,_memalign_r -Wl,--wrap,exit -Wl,--wrap,atexit -Wl,-n -mcpu=cortex-m3 -mthumb -Wl,-Map=$(FW_VERSION).map
+LD_FLAGS :=-Wl,--gc-sections -Wl,--wrap,main -Wl,--wrap,_memalign_r -Wl,--wrap,exit -Wl,--wrap,atexit -Wl,-n -mcpu=cortex-m3 -mthumb
 LD_SYS_LIBS :=-Wl,--start-group -lstdc++ -lsupc++ -lm -lc -lgcc -lnosys -Wl,--end-group
 
 
@@ -303,7 +310,7 @@ LD_SYS_LIBS :=-Wl,--start-group -lstdc++ -lsupc++ -lm -lc -lgcc -lnosys -Wl,--en
 ###############################################################################
 # Rules
 
-all: $(FW_VERSION).bin
+all: $(FW_VERSION).bin bootloader.bin
 
 mbed-os:
 	@$(MAKE) -C ../ -f mbed-os.mk
@@ -336,9 +343,9 @@ mbed-os:
 $(FW_VERSION).link_script.ld: $(LINKER_SCRIPT)
 	@$(PREPROC) $< -o $@
 
-$(FW_VERSION).elf: mbed-os $(OBJECTS) $(SYS_OBJECTS) $(FW_VERSION).link_script.ld
+$(FW_VERSION).elf: $(OBJECTS) $(SYS_OBJECTS) $(FW_VERSION).link_script.ld
 	+@echo "link: $(notdir $@)"
-	@$(LD) $(LD_FLAGS) -T $(filter %.ld, $^) $(LIBRARY_PATHS) --output $@ $(filter %.o, $^) $(LIBRARIES) $(LD_SYS_LIBS)
+	$(LD) $(LD_FLAGS) -Wl,-Map=$(FW_VERSION).map -T $(filter %.ld, $^) $(LIBRARY_PATHS) --output $@ $(filter %.o, $^) $(LIBRARIES) $(LD_SYS_LIBS)
 
 $(FW_VERSION).bin: $(FW_VERSION).elf
 	$(ELF2BIN) -O binary $< $@
@@ -347,7 +354,20 @@ $(FW_VERSION).bin: $(FW_VERSION).elf
 $(FW_VERSION).hex: $(FW_VERSION).elf
 	$(ELF2BIN) -O ihex $< $@
 
-.PHONY: all mbed-os
+bootloader.elf: $(BOOT_OBJECTS) $(SYS_OBJECTS)
+	+@echo "link: $(notdir $@)"
+	$(LD) $(LD_FLAGS) -Wl,-Map=bootloader.map -T ../linker/bootloader.ld $(LIBRARY_PATHS) --output $@ $(filter %.o, $^) $(LIBRARIES) $(LD_SYS_LIBS)
+
+bootloader.bin: bootloader.elf
+	$(ELF2BIN) -O binary $< $@
+	+@echo "===== bin file ready to flash: $(OBJDIR)/$@ ====="
+
+full: bootloader.bin $(FW_VERSION).bin
+	+@dd if=/dev/zero bs=1 count=32768 | tr \"\\000\" \"\\377\" > bootloader_pad.bin
+	+@dd if=bootloader.bin of=bootloader_pad.bin conv=notrunc
+	+@cat bootloader_pad.bin $(FW_VERSION).bin > rffe_fw_full.bin
+
+.PHONY: all mbed-os full
 
 # Rules
 ###############################################################################
diff --git a/boot/IAP.cpp b/boot/IAP.cpp
new file mode 100644
index 0000000..e047aed
--- /dev/null
+++ b/boot/IAP.cpp
@@ -0,0 +1,234 @@
+/**    IAP : internal Flash memory access library
+ *
+ *        The internal Flash memory access is described in the LPC1768 and LPC11U24 usermanual.
+ *            http://www.nxp.com/documents/user_manual/UM10360.pdf
+ *            http://www.nxp.com/documents/user_manual/UM10462.pdf
+ *
+ *               LPC1768 --
+ *                    Chapter  2: "LPC17xx Memory map"
+ *                    Chapter 32: "LPC17xx Flash memory interface and programming"
+ *                    refering Rev. 01 - 4 January 2010
+ *
+ *               LPC11U24 --
+ *                    Chapter  2: "LPC11Uxx Memory mapping"
+ *                    Chapter 20: "LPC11Uxx Flash programming firmware"
+ *                    refering Rev. 03 - 16 July 2012
+ *
+ *        Released under the MIT License: http://mbed.org/license/mit
+ *
+ *        revision 1.0  09-Mar-2010   1st release
+ *        revision 1.1  12-Mar-2010   chaged: to make possible to reserve flash area for user
+ *                                            it can be set by USER_FLASH_AREA_START and USER_FLASH_AREA_SIZE in IAP.h
+ *        revision 2.0  26-Nov-2012   LPC11U24 code added
+ *        revision 2.1  26-Nov-2012   EEPROM access code imported from Suga koubou san's (http://mbed.org/users/okini3939/) library
+ *                                            http://mbed.org/users/okini3939/code/M0_EEPROM_test/
+ *        revision 3.0  09-Jan-2015   LPC812 and LPC824 support added
+ *        revision 3.1  13-Jan-2015   LPC1114 support added
+ *        revision 3.1.1 16-Jan-2015  Target MCU name changed for better compatibility across the platforms
+ */
+
+#include    "mbed.h"
+#include    "IAP.h"
+
+/*
+ *  Reserve of flash area is explained by Igor. Please refer next URL
+ *    http://mbed.org/users/okano/notebook/iap-in-application-programming-internal-flash-eras/?page=1#comment-271
+ */
+
+//unsigned char user_area[ size ] __attribute__((section(".ARM.__at_0x78000"), zero_init));
+
+/*
+ *  IAP command codes
+ *  Table 589. "IAP Command Summary", Chapter 8. "IAP commands", usermanual
+ */
+
+enum command_code {
+    IAPCommand_Prepare_sector_for_write_operation    = 50,
+    IAPCommand_Copy_RAM_to_Flash,
+    IAPCommand_Erase_sector,
+    IAPCommand_Blank_check_sector,
+    IAPCommand_Read_part_ID,
+    IAPCommand_Read_Boot_Code_version,
+    IAPCommand_Compare,
+    IAPCommand_Reinvoke_ISP,
+    IAPCommand_Read_device_serial_number,
+#if defined(TARGET_LPC11UXX)
+    IAPCommand_EEPROM_Write = 61,
+    IAPCommand_EEPROM_Read,
+#elif defined(TARGET_LPC81X) || defined(TARGET_LPC82X)
+    IAPCommand_Erase_page = 59,
+#endif
+};
+
+int IAP::reinvoke_isp( void ) {
+    __disable_irq();
+
+    IAP_command[ 0 ]    = IAPCommand_Reinvoke_ISP;
+
+    iap_entry( IAP_command, IAP_result );
+
+    return ( (int)IAP_result[ 0 ] );
+}
+
+/** Read part identification number
+ *
+ *  @return    device ID
+ *  @see       read_serial()
+ */
+int IAP::read_ID( void )
+{
+    IAP_command[ 0 ]    = IAPCommand_Read_part_ID;
+
+    iap_entry( IAP_command, IAP_result );
+
+    return ( (int)IAP_result[ 1 ] );    //  to return the number itself (this command always returns CMD_SUCCESS)
+}
+
+int *IAP::read_serial( void ) {
+    IAP_command[ 0 ]    = IAPCommand_Read_device_serial_number;
+
+    iap_entry( IAP_command, IAP_result );
+
+    return ( (int *)&IAP_result[ 1 ] );    //  to return the number itself (this command always returns CMD_SUCCESS)
+}
+
+int IAP::blank_check( int start, int end )
+{
+    IAP_command[ 0 ]    = IAPCommand_Blank_check_sector;
+    IAP_command[ 1 ]    = (unsigned int)start;  //  Start Sector Number
+    IAP_command[ 2 ]    = (unsigned int)end;    //  End Sector Number (should be greater than or equal to start sector number)
+
+    iap_entry( IAP_command, IAP_result );
+
+    return ( (int)IAP_result[ 0 ] );
+}
+
+int IAP::erase( int start, int end )
+{
+    int err = prepare(start, end);
+
+    if (err != IAP_CMD_SUCCESS) {
+        return err;
+    }
+
+    IAP_command[ 0 ]    = IAPCommand_Erase_sector;
+    IAP_command[ 1 ]    = (unsigned int)start;  //  Start Sector Number
+    IAP_command[ 2 ]    = (unsigned int)end;    //  End Sector Number (should be greater than or equal to start sector number)
+    IAP_command[ 3 ]    = cclk_kHz;             //  CPU Clock Frequency (CCLK) in kHz
+
+    iap_entry( IAP_command, IAP_result );
+
+    return ( (int)IAP_result[ 0 ] );
+}
+
+int IAP::prepare( int start, int end )
+{
+    IAP_command[ 0 ]    = IAPCommand_Prepare_sector_for_write_operation;
+    IAP_command[ 1 ]    = (unsigned int)start;  //  Start Sector Number
+    IAP_command[ 2 ]    = (unsigned int)end;    //  End Sector Number (should be greater than or equal to start sector number).
+
+    iap_entry( IAP_command, IAP_result );
+
+    return ( (int)IAP_result[ 0 ] );
+}
+
+int IAP::write( uint32_t *source_addr, uint32_t *target_addr, int size )
+{
+    uint32_t sector_start = find_sector((uint32_t)target_addr);
+    uint32_t sector_end = find_sector(((uint32_t)target_addr)+size);
+
+    if (size % 256) {
+        /* Data should be a 256 byte boundary */
+        return IAP_COUNT_ERROR;
+    }
+
+    int err = prepare(sector_start, sector_end);
+
+    if (err != IAP_CMD_SUCCESS) {
+        return err;
+    }
+
+    IAP_command[ 0 ]    = IAPCommand_Copy_RAM_to_Flash;
+    IAP_command[ 1 ]    = (unsigned int)target_addr;    //  Destination flash address where data bytes are to be written. This address should be a 256 byte boundary.
+    IAP_command[ 2 ]    = (unsigned int)source_addr;    //  Source RAM address from which data bytes are to be read. This address should be a word boundary.
+    IAP_command[ 3 ]    = size;                         //  Number of bytes to be written. Should be 256 | 512 | 1024 | 4096.
+    IAP_command[ 4 ]    = cclk_kHz;                     //  CPU Clock Frequency (CCLK) in kHz.
+
+    iap_entry( IAP_command, IAP_result );
+
+    return ( (int)IAP_result[ 0 ] );
+}
+
+int IAP::compare( uint32_t *source_addr, uint32_t *target_addr, int size )
+{
+    IAP_command[ 0 ]    = IAPCommand_Compare;
+    IAP_command[ 1 ]    = (unsigned int)target_addr;    //  Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
+    IAP_command[ 2 ]    = (unsigned int)source_addr;    //  Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
+    IAP_command[ 3 ]    = size;                         //  Number of bytes to be compared; should be a multiple of 4.
+
+    iap_entry( IAP_command, IAP_result );
+
+    return ( (int)IAP_result[ 0 ] );
+}
+
+int IAP::read_BootVer(void)
+{
+    IAP_command[0] = IAPCommand_Read_Boot_Code_version;
+    IAP_result[1] = 0; // not sure if in high or low bits.
+    iap_entry(IAP_command, IAP_result);
+    return ((int)IAP_result[1]);
+}
+
+int IAP::find_sector( uint32_t addr )
+{
+    if (addr <= 0xFFFF) {
+        return ((addr >> 12) & 0xF);
+    } else {
+        return ((addr/32768)-2+16);
+    }
+}
+
+#if defined(TARGET_LPC11UXX)
+
+int IAP::write_eeprom( uint32_t *source_addr, uint32_t *target_addr, int size )
+{
+    IAP_command[ 0 ]    = IAPCommand_EEPROM_Write;
+    IAP_command[ 1 ]    = (unsigned int)target_addr;    //  Destination EEPROM address where data bytes are to be written. This address should be a 256 byte boundary.
+    IAP_command[ 2 ]    = (unsigned int)source_addr;    //  Source RAM address from which data bytes are to be read. This address should be a word boundary.
+    IAP_command[ 3 ]    = size;                         //  Number of bytes to be written. Should be 256 | 512 | 1024 | 4096.
+    IAP_command[ 4 ]    = cclk_kHz;                     //  CPU Clock Frequency (CCLK) in kHz.
+
+    iap_entry( IAP_command, IAP_result );
+
+    return ( (int)IAP_result[ 0 ] );
+}
+
+int IAP::read_eeprom( uint32_t *source_addr, uint32_t *target_addr, int size )
+{
+    IAP_command[ 0 ]    = IAPCommand_EEPROM_Read;
+    IAP_command[ 1 ]    = (unsigned int)source_addr;    //  Source EEPROM address from which data bytes are to be read. This address should be a word boundary.
+    IAP_command[ 2 ]    = (unsigned int)target_addr;    //  Destination RAM address where data bytes are to be written. This address should be a 256 byte boundary.
+    IAP_command[ 3 ]    = size;                         //  Number of bytes to be written. Should be 256 | 512 | 1024 | 4096.
+    IAP_command[ 4 ]    = cclk_kHz;                     //  CPU Clock Frequency (CCLK) in kHz.
+
+    iap_entry( IAP_command, IAP_result );
+
+    return ( (int)IAP_result[ 0 ] );
+}
+
+#elif defined(TARGET_LPC81X) || defined(TARGET_LPC82X)
+
+int IAP::erase_page( int start, int end )
+{
+    IAP_command[ 0 ]    = IAPCommand_Erase_page;
+    IAP_command[ 1 ]    = (unsigned int)start;  //  Start Sector Number
+    IAP_command[ 2 ]    = (unsigned int)end;    //  End Sector Number (should be greater than or equal to start sector number)
+    IAP_command[ 3 ]    = cclk_kHz;             //  CPU Clock Frequency (CCLK) in kHz
+
+    iap_entry( IAP_command, IAP_result );
+
+    return ( (int)IAP_result[ 0 ] );
+}
+
+#endif
+
diff --git a/boot/IAP.h b/boot/IAP.h
new file mode 100644
index 0000000..0797b43
--- /dev/null
+++ b/boot/IAP.h
@@ -0,0 +1,494 @@
+/**    IAP : internal Flash memory access library
+ *
+ *        The internal Flash memory access is described in the LPC1768 and LPC11U24 usermanual.
+ *            http://www.nxp.com/documents/user_manual/UM10360.pdf
+ *            http://www.nxp.com/documents/user_manual/UM10462.pdf
+ *
+ *               LPC1768 --
+ *                    Chapter  2: "LPC17xx Memory map"
+ *                    Chapter 32: "LPC17xx Flash memory interface and programming"
+ *                    refering Rev. 01 - 4 January 2010
+ *
+ *               LPC11U24 --
+ *                    Chapter  2: "LPC11Uxx Memory mapping"
+ *                    Chapter 20: "LPC11Uxx Flash programming firmware"
+ *                    refering Rev. 03 - 16 July 2012
+ *
+ *        Released under the MIT License: http://mbed.org/license/mit
+ *
+ *        revision 1.0  09-Mar-2010   1st release
+ *        revision 1.1  12-Mar-2010   chaged: to make possible to reserve flash area for user
+ *                                            it can be set by USER_FLASH_AREA_START and USER_FLASH_AREA_SIZE in IAP.h
+ *        revision 2.0  26-Nov-2012   LPC11U24 code added
+ *        revision 2.1  26-Nov-2012   EEPROM access code imported from Suga koubou san's (http://mbed.org/users/okini3939/) library
+ *                                            http://mbed.org/users/okini3939/code/M0_EEPROM_test/
+ *        revision 3.0  09-Jan-2015   LPC812 and LPC824 support added
+ *        revision 3.1  13-Jan-2015   LPC1114 support added
+ *        revision 3.1.1 16-Jan-2015  Target MCU name changed for better compatibility across the platforms
+ */
+
+
+#ifndef        MBED_IAP
+#define        MBED_IAP
+
+#include    "mbed.h"
+
+#if defined(TARGET_LPC176X)
+
+/*
+ *  memory map information is available in next URL also.
+ *    http://mbed.org/projects/libraries/svn/mbed/trunk/LPC1768/LPC17xx.h
+ */
+
+/**    Table for start adress of sectors
+ *
+ *        LPC1768 internal flash memory sector numbers and addresses
+ *
+ *        LPC1768 flash memory are and sector number/size
+ *        Table 568 "Sectors in a LPC17xx device", Section 5. "Sector numbers", usermanual
+ *
+ *        0x00000000 - 0x0007FFFF        flash (29 sectors)
+ *
+ *      Sector0:     0x00000000 - 0x00000FFF        4K
+ *      Sector1:     0x00001000 - 0x00001FFF        4K
+ *      Sector2:     0x00002000 - 0x00002FFF        4K
+ *      Sector3:     0x00003000 - 0x00003FFF        4K
+ *      Sector4:     0x00004000 - 0x00004FFF        4K
+ *      Sector5:     0x00005000 - 0x00005FFF        4K
+ *      Sector6:     0x00006000 - 0x00006FFF        4K
+ *      Sector7:     0x00007000 - 0x00007FFF        4K
+ *      Sector8:     0x00008000 - 0x00008FFF        4K
+ *      Sector9:     0x00009000 - 0x00009FFF        4K
+ *      Sector10:    0x0000A000 - 0x0000AFFF        4K
+ *      Sector11:    0x0000B000 - 0x0000BFFF        4K
+ *      Sector12:    0x0000C000 - 0x0000CFFF        4K
+ *      Sector13:    0x0000D000 - 0x0000DFFF        4K
+ *      Sector14:    0x0000E000 - 0x0000EFFF        4K
+ *      Sector15:    0x0000F000 - 0x0000FFFF        4K
+ *
+ *      Sector16:    0x00010000 - 0x00017FFF        32K
+ *      Sector17:    0x00018000 - 0x0001FFFF        32K
+ *      Sector18:    0x00020000 - 0x00027FFF        32K
+ *      Sector19:    0x00028000 - 0x0002FFFF        32K
+ *      Sector20:    0x00030000 - 0x00037FFF        32K
+ *      Sector21:    0x00038000 - 0x0003FFFF        32K
+ *      Sector22:    0x00040000 - 0x00047FFF        32K
+ *      Sector23:    0x00048000 - 0x0004FFFF        32K
+ *      Sector24:    0x00050000 - 0x00057FFF        32K
+ *      Sector25:    0x00058000 - 0x0005FFFF        32K
+ *      Sector26:    0x00060000 - 0x00067FFF        32K
+ *      Sector27:    0x00068000 - 0x0006FFFF        32K
+ *      Sector28:    0x00070000 - 0x00077FFF        32K
+ *      Sector29:    0x00078000 - 0x0007FFFF        32K
+ */
+
+#define     FLASH_SECTOR_0       0x00000000
+#define     FLASH_SECTOR_1       0x00001000
+#define     FLASH_SECTOR_2       0x00002000
+#define     FLASH_SECTOR_3       0x00003000
+#define     FLASH_SECTOR_4       0x00004000
+#define     FLASH_SECTOR_5       0x00005000
+#define     FLASH_SECTOR_6       0x00006000
+#define     FLASH_SECTOR_7       0x00007000
+#define     FLASH_SECTOR_8       0x00008000
+#define     FLASH_SECTOR_9       0x00009000
+#define     FLASH_SECTOR_10      0x0000A000
+#define     FLASH_SECTOR_11      0x0000B000
+#define     FLASH_SECTOR_12      0x0000C000
+#define     FLASH_SECTOR_13      0x0000D000
+#define     FLASH_SECTOR_14      0x0000E000
+#define     FLASH_SECTOR_15      0x0000F000
+#define     FLASH_SECTOR_16      0x00010000
+#define     FLASH_SECTOR_17      0x00018000
+#define     FLASH_SECTOR_18      0x00020000
+#define     FLASH_SECTOR_19      0x00028000
+#define     FLASH_SECTOR_20      0x00030000
+#define     FLASH_SECTOR_21      0x00038000
+#define     FLASH_SECTOR_22      0x00040000
+#define     FLASH_SECTOR_23      0x00048000
+#define     FLASH_SECTOR_24      0x00050000
+#define     FLASH_SECTOR_25      0x00058000
+#define     FLASH_SECTOR_26      0x00060000
+#define     FLASH_SECTOR_27      0x00068000
+#define     FLASH_SECTOR_28      0x00070000
+#define     FLASH_SECTOR_29      0x00078000
+#define     FLASH_SECTOR_SIZE_0_TO_15    ( 4 * 1024)
+#define     FLASH_SECTOR_SIZE_16_TO_29   (32 * 1024)
+
+#if 0
+static char * sector_start_adress[]    = {
+    (char *)FLASH_SECTOR_0,
+    (char *)FLASH_SECTOR_1,
+    (char *)FLASH_SECTOR_2,
+    (char *)FLASH_SECTOR_3,
+    (char *)FLASH_SECTOR_4,
+    (char *)FLASH_SECTOR_5,
+    (char *)FLASH_SECTOR_6,
+    (char *)FLASH_SECTOR_7,
+    (char *)FLASH_SECTOR_8,
+    (char *)FLASH_SECTOR_9,
+    (char *)FLASH_SECTOR_10,
+    (char *)FLASH_SECTOR_11,
+    (char *)FLASH_SECTOR_12,
+    (char *)FLASH_SECTOR_13,
+    (char *)FLASH_SECTOR_14,
+    (char *)FLASH_SECTOR_15,
+    (char *)FLASH_SECTOR_16,
+    (char *)FLASH_SECTOR_17,
+    (char *)FLASH_SECTOR_18,
+    (char *)FLASH_SECTOR_19,
+    (char *)FLASH_SECTOR_20,
+    (char *)FLASH_SECTOR_21,
+    (char *)FLASH_SECTOR_22,
+    (char *)FLASH_SECTOR_23,
+    (char *)FLASH_SECTOR_24,
+    (char *)FLASH_SECTOR_25,
+    (char *)FLASH_SECTOR_26,
+    (char *)FLASH_SECTOR_27,
+    (char *)FLASH_SECTOR_28,
+    (char *)FLASH_SECTOR_29
+};
+#endif
+
+#elif defined(TARGET_LPC11UXX) || defined(TARGET_LPC11XX)
+
+#define     USER_FLASH_AREA_START   FLASH_SECTOR_7
+#define     USER_FLASH_AREA_SIZE    (FLASH_SECTOR_SIZE * 1)
+
+/**    Table for start adress of sectors
+ *
+ *        LPC11U24 internal flash memory sector numbers and addresses
+ *
+ *        LPC11U24 flash memory are and sector number/size
+ *        Table 334 "LPC11U1x/2x flash sectors", Section 20. "Sector numbers", usermanual
+ *
+ *        0x00000000 - 0x00007FFF        flash (8 sectors)
+ *
+ *      Sector0:     0x00000000 - 0x00000FFF        4K
+ *      Sector1:     0x00001000 - 0x00001FFF        4K
+ *      Sector2:     0x00002000 - 0x00002FFF        4K
+ *      Sector3:     0x00003000 - 0x00003FFF        4K
+ *      Sector4:     0x00004000 - 0x00004FFF        4K
+ *      Sector5:     0x00005000 - 0x00005FFF        4K
+ *      Sector6:     0x00006000 - 0x00006FFF        4K
+ *      Sector7:     0x00007000 - 0x00007FFF        4K
+ */
+
+#define     FLASH_SECTOR_0       0x00000000
+#define     FLASH_SECTOR_1       0x00001000
+#define     FLASH_SECTOR_2       0x00002000
+#define     FLASH_SECTOR_3       0x00003000
+#define     FLASH_SECTOR_4       0x00004000
+#define     FLASH_SECTOR_5       0x00005000
+#define     FLASH_SECTOR_6       0x00006000
+#define     FLASH_SECTOR_7       0x00007000
+#define     FLASH_SECTOR_SIZE    (4 * 1024)
+
+static char * sector_start_adress[]    = {
+    (char *)FLASH_SECTOR_0,
+    (char *)FLASH_SECTOR_1,
+    (char *)FLASH_SECTOR_2,
+    (char *)FLASH_SECTOR_3,
+    (char *)FLASH_SECTOR_4,
+    (char *)FLASH_SECTOR_5,
+    (char *)FLASH_SECTOR_6,
+    (char *)FLASH_SECTOR_7,
+};
+
+#elif defined(TARGET_LPC81X) || defined(TARGET_LPC82X)
+
+#define     USER_FLASH_AREA_START   FLASH_SECTOR_15
+#define     USER_FLASH_AREA_SIZE    (FLASH_SECTOR_SIZE * 1)
+
+/**    Table for start adress of sectors
+ *
+ *        LPC812/LPC824 internal flash memory sector numbers and addresses
+ *
+ *        0x00000000 - 0x00003FFF        flash (16 sectors for LPC812)
+ *        0x00000000 - 0x00007FFF        flash (32 sectors for LPC824)
+ *
+ *      Sector0:     0x00000000 - 0x000003FF        1K
+ *      Sector1:     0x00000400 - 0x000007FF        1K
+ *      Sector2:     0x00000800 - 0x00000BFF        1K
+ *      Sector3:     0x00000C00 - 0x00000FFF        1K
+ *      Sector4:     0x00001000 - 0x000013FF        1K
+ *      Sector5:     0x00001400 - 0x000017FF        1K
+ *      Sector6:     0x00001800 - 0x00001BFF        1K
+ *      Sector7:     0x00001C00 - 0x00001FFF        1K
+ *      Sector8:     0x00002000 - 0x000023FF        1K
+ *      Sector9:     0x00002400 - 0x000027FF        1K
+ *      Sector10:    0x00002800 - 0x00002BFF        1K
+ *      Sector11:    0x00002C00 - 0x00002FFF        1K
+ *      Sector12:    0x00003000 - 0x000033FF        1K
+ *      Sector13:    0x00003400 - 0x000037FF        1K
+ *      Sector14:    0x00003800 - 0x00003BFF        1K
+ *      Sector15:    0x00003C00 - 0x00003FFF        1K
+ *      Sector16:    0x00004000 - 0x000043FF        1K  (LPC824 only)
+ *      Sector17:    0x00004400 - 0x000047FF        1K  (LPC824 only)
+ *      Sector18:    0x00004800 - 0x00004BFF        1K  (LPC824 only)
+ *      Sector19:    0x00004C00 - 0x00004FFF        1K  (LPC824 only)
+ *      Sector20:    0x00005000 - 0x000053FF        1K  (LPC824 only)
+ *      Sector21:    0x00005400 - 0x000057FF        1K  (LPC824 only)
+ *      Sector22:    0x00005800 - 0x00005BFF        1K  (LPC824 only)
+ *      Sector23:    0x00005C00 - 0x00005FFF        1K  (LPC824 only)
+ *      Sector24:    0x00006000 - 0x000063FF        1K  (LPC824 only)
+ *      Sector25:    0x00006400 - 0x000067FF        1K  (LPC824 only)
+ *      Sector26:    0x00006800 - 0x00006BFF        1K  (LPC824 only)
+ *      Sector27:    0x00006C00 - 0x00006FFF        1K  (LPC824 only)
+ *      Sector28:    0x00007000 - 0x000073FF        1K  (LPC824 only)
+ *      Sector29:    0x00007400 - 0x000077FF        1K  (LPC824 only)
+ *      Sector30:    0x00007800 - 0x00007BFF        1K  (LPC824 only)
+ *      Sector31:    0x00007C00 - 0x00007FFF        1K  (LPC824 only)
+ */
+
+#define     FLASH_SECTOR_0       0x00000000
+#define     FLASH_SECTOR_1       0x00000400
+#define     FLASH_SECTOR_2       0x00000800
+#define     FLASH_SECTOR_3       0x00000C00
+#define     FLASH_SECTOR_4       0x00001000
+#define     FLASH_SECTOR_5       0x00001400
+#define     FLASH_SECTOR_6       0x00001800
+#define     FLASH_SECTOR_7       0x00001C00
+#define     FLASH_SECTOR_8       0x00002000
+#define     FLASH_SECTOR_9       0x00002400
+#define     FLASH_SECTOR_10      0x00002800
+#define     FLASH_SECTOR_11      0x00002C00
+#define     FLASH_SECTOR_12      0x00003000
+#define     FLASH_SECTOR_13      0x00003400
+#define     FLASH_SECTOR_14      0x00003800
+#define     FLASH_SECTOR_15      0x00003C00
+#define     FLASH_SECTOR_16      0x00004000     //  for LPC824 only
+#define     FLASH_SECTOR_17      0x00004400     //  for LPC824 only
+#define     FLASH_SECTOR_18      0x00004800     //  for LPC824 only
+#define     FLASH_SECTOR_19      0x00004C00     //  for LPC824 only
+#define     FLASH_SECTOR_20      0x00005000     //  for LPC824 only
+#define     FLASH_SECTOR_21      0x00005400     //  for LPC824 only
+#define     FLASH_SECTOR_22      0x00005800     //  for LPC824 only
+#define     FLASH_SECTOR_23      0x00005C00     //  for LPC824 only
+#define     FLASH_SECTOR_24      0x00006000     //  for LPC824 only
+#define     FLASH_SECTOR_25      0x00006400     //  for LPC824 only
+#define     FLASH_SECTOR_26      0x00006800     //  for LPC824 only
+#define     FLASH_SECTOR_27      0x00006C00     //  for LPC824 only
+#define     FLASH_SECTOR_28      0x00007000     //  for LPC824 only
+#define     FLASH_SECTOR_29      0x00007400     //  for LPC824 only
+#define     FLASH_SECTOR_30      0x00007800     //  for LPC824 only
+#define     FLASH_SECTOR_31      0x00007C00     //  for LPC824 only
+#define     FLASH_SECTOR_SIZE    (1 * 1024)
+
+static char * sector_start_adress[]    = {
+    (char *)FLASH_SECTOR_0,
+    (char *)FLASH_SECTOR_1,
+    (char *)FLASH_SECTOR_2,
+    (char *)FLASH_SECTOR_3,
+    (char *)FLASH_SECTOR_4,
+    (char *)FLASH_SECTOR_5,
+    (char *)FLASH_SECTOR_6,
+    (char *)FLASH_SECTOR_7,
+    (char *)FLASH_SECTOR_8,
+    (char *)FLASH_SECTOR_9,
+    (char *)FLASH_SECTOR_10,
+    (char *)FLASH_SECTOR_11,
+    (char *)FLASH_SECTOR_12,
+    (char *)FLASH_SECTOR_13,
+    (char *)FLASH_SECTOR_14,
+    (char *)FLASH_SECTOR_15,
+    (char *)FLASH_SECTOR_16,     //  for LPC824 only
+    (char *)FLASH_SECTOR_17,     //  for LPC824 only
+    (char *)FLASH_SECTOR_18,     //  for LPC824 only
+    (char *)FLASH_SECTOR_19,     //  for LPC824 only
+    (char *)FLASH_SECTOR_20,     //  for LPC824 only
+    (char *)FLASH_SECTOR_21,     //  for LPC824 only
+    (char *)FLASH_SECTOR_22,     //  for LPC824 only
+    (char *)FLASH_SECTOR_23,     //  for LPC824 only
+    (char *)FLASH_SECTOR_24,     //  for LPC824 only
+    (char *)FLASH_SECTOR_25,     //  for LPC824 only
+    (char *)FLASH_SECTOR_26,     //  for LPC824 only
+    (char *)FLASH_SECTOR_27,     //  for LPC824 only
+    (char *)FLASH_SECTOR_28,     //  for LPC824 only
+    (char *)FLASH_SECTOR_29,     //  for LPC824 only
+    (char *)FLASH_SECTOR_30,     //  for LPC824 only
+    (char *)FLASH_SECTOR_31     //  for LPC824 only
+};
+
+#endif
+
+/**    Error code by IAP routine
+ *
+ *        Table 588 "ISP Return Codes Summary", Section 7.15 "ISP Return Codes", usermanual
+ */
+
+enum error_code {
+    IAP_CMD_SUCCESS,
+    IAP_INVALID_COMMAND,
+    IAP_SRC_ADDR_ERROR,
+    IAP_DST_ADDR_ERROR,
+    IAP_SRC_ADDR_NOT_MAPPED,
+    IAP_DST_ADDR_NOT_MAPPED,
+    IAP_COUNT_ERROR,
+    IAP_INVALID_SECTOR,
+    IAP_SECTOR_NOT_BLANK,
+    IAP_SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION,
+    IAP_COMPARE_ERROR,
+    IAP_BUSY,
+    IAP_PARAM_ERROR,
+    IAP_ADDR_ERROR,
+    IAP_ADDR_NOT_MAPPED,
+    IAP_CMD_LOCKED,
+    IAP_INVALID_CODE,
+    IAP_INVALID_BAUD_RATE,
+    IAP_INVALID_STOP_BIT,
+    IAP_CODE_READ_PROTECTION_ENABLED
+};
+
+
+/*
+ *  IAP routine entry
+ *
+ *        "IAP commands"
+ */
+
+#define     IAP_LOCATION    0x1fff1ff1
+typedef     void (*IAP_call)(unsigned int [], unsigned int []);
+
+
+/**    IAP class
+ *
+ *        Interface for internal flash memory access
+ */
+
+class IAP
+{
+public:
+
+    /**    Constructor for IAP
+     *
+     */
+    IAP() : iap_entry( reinterpret_cast<IAP_call>(IAP_LOCATION) ), cclk_kHz( SystemCoreClock / 1000 ) {}
+
+    /** Reinvoke ISP
+     *
+     *  @return    error code
+     */
+    int reinvoke_isp( void );
+
+    /** Read part identification number
+     *
+     *  @return    device ID
+     *  @see       read_serial()
+     */
+    int read_ID( void );
+
+    /** Read device serial number
+     *
+     *  @return    device serial number
+     *  @see       read_ID()
+     */
+    int *read_serial( void );
+
+    /** Blank check sector(s)
+     *
+     *  @param    start    a Start Sector Number
+     *  @param    end      an End Sector Number (should be greater than or equal to start sector number).
+     *  @return error code: CMD_SUCCESS | BUSY | SECTOR_NOT_BLANK | INVALID_SECTOR
+     */
+    int blank_check( int start, int end );
+
+    /** Erase Sector(s)
+     *
+     *  @param    start    a Start Sector Number
+     *  @param    end      an End Sector Number (should be greater than or equal to start sector number).
+     *  @return   error code: CMD_SUCCESS | BUSY | SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION | INVALID_SECTOR
+     */
+    int erase( int start, int end );
+
+    /** Prepare sector(s) for write operation
+     *
+     *  @param    start    a Start Sector Number
+     *  @param    end      an End Sector Number (should be greater than or equal to start sector number).
+     *  @return   error code: CMD_SUCCESS | BUSY | INVALID_SECTOR
+     */
+    int prepare( int start, int end );
+
+    /** Copy RAM to Flash
+     *
+     *  @param    source_addr    Source RAM address from which data bytes are to be read. This address should be a word boundary.
+     *  @param    target_addr    Destination flash address where data bytes are to be written. This address should be a 256 byte boundary.
+     *  @param    size           Number of bytes to be written. Should be 256 | 512 | 1024 | 4096.
+     *  @return   error code: CMD_SUCCESS | SRC_ADDR_ERROR (Address not a word boundary) | DST_ADDR_ERROR (Address not on correct boundary) | SRC_ADDR_NOT_MAPPED | DST_ADDR_NOT_MAPPED | COUNT_ERROR (Byte count is not 256 | 512 | 1024 | 4096) | SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION | BUSY
+     */
+    int write( uint32_t *source_addr, uint32_t *target_addr, int size );
+
+    /** Compare <address1> <address2> <no of bytes>
+     *
+     *  @param    source_addr Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
+     *  @param    target_addr Starting flash or RAM address of data bytes to be compared. This address should be a word boundary.
+     *  @param    size         Number of bytes to be compared; should be a multiple of 4.
+     *  @return   error code: CMD_SUCCESS | COMPARE_ERROR | COUNT_ERROR (Byte count is not a multiple of 4) | ADDR_ERROR | ADDR_NOT_MAPPED
+     */
+    int compare( uint32_t *source_addr, uint32_t *target_addr, int size );
+
+    /** Read Boot code version number
+     *
+     *  @return   2 bytes of boot code version number
+     */
+    int read_BootVer( void );
+
+    /** Get user reserved flash start address
+     *
+     *  @return    start address of user reserved flash memory
+     *  @see       reserved_flash_area_size()
+     */
+
+    uint32_t *reserved_flash_area_start( void );
+
+    /** Get user reserved flash size
+     *
+     *  @return    size of user reserved flash memory
+     *  @see       reserved_flash_area_start()
+     */
+    int   reserved_flash_area_size( void );
+
+    int   find_sector( uint32_t addr );
+#if defined(TARGET_LPC11UXX)
+
+    /** Copy RAM to EEPROM (LPC11U24)
+     *
+     *  @param    source_addr    Source RAM address from which data bytes are to be read.
+     *  @param    target_addr    Destination EEPROM address where data bytes are to be written.
+     *  @param    size           Number of bytes to be written.
+     *  @return   error code: CMD_SUCCESS | SRC_ADDR_NOT_MAPPED | DST_ADDR_NOT_MAPPED
+     *  Remark: The top 64 bytes of the EEPROM memory are reserved and cannot be written to.
+     */
+    int write_eeprom( uint32_t *source_addr, uint32_t *target_addr, int size );
+
+    /** Copy EEPROM to RAM (LPC11U24)
+     *
+     *  @param    source_addr    Source EEPROM address from which data bytes are to be read.
+     *  @param    target_addr    Destination RAM address where data bytes are to be written.
+     *  @param    size           Number of bytes to be written.
+     *  @return   error code: CMD_SUCCESS | SRC_ADDR_NOT_MAPPED | DST_ADDR_NOT_MAPPED
+     *  Remark: The top 64 bytes of the EEPROM memory are reserved and cannot be written to.
+     */
+    int read_eeprom( uint32_t *source_addr, uint32_t *target_addr, int size );
+
+#elif defined(TARGET_LPC81X) || defined(TARGET_LPC82X)
+
+    /** Erase page(s) (LPC812, LPC824)
+     *
+     *  @param    start    Start page number.
+     *  @param    end      End page number (should be greater than or equal to start page).
+     *  @return   error code: CMD_SUCCESS | BUSY | SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION | INVALID_SECTOR
+     */
+    int erase_page( int start, int end );
+
+#endif
+
+private:
+    IAP_call        iap_entry;
+    unsigned int    IAP_command[ 5 ];
+    unsigned int    IAP_result[ 5 ];
+    int             cclk_kHz;
+}
+;
+
+#endif    //  #ifndef  MBED_IAP
diff --git a/linker/LPC1768.ld b/linker/LPC1768.ld
new file mode 100644
index 0000000..2635d65
--- /dev/null
+++ b/linker/LPC1768.ld
@@ -0,0 +1,99 @@
+MEMORY
+{
+  FLASH (rx) : ORIGIN = 0x00008000, LENGTH = 200K
+  RAM (rwx) : ORIGIN = 0x100000C8, LENGTH = (32K - 0xC8 - 32)
+  USB_RAM(rwx) : ORIGIN = 0x2007C000, LENGTH = 16K
+  ETH_RAM(rwx) : ORIGIN = 0x20080000, LENGTH = 16K
+}
+ENTRY(Reset_Handler)
+SECTIONS
+{
+    .text :
+    {
+        KEEP(*(.isr_vector))
+        *(.text*)
+        KEEP(*(.init))
+        KEEP(*(.fini))
+        *crtbegin.o(.ctors)
+        *crtbegin?.o(.ctors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+        *(SORT(.ctors.*))
+        *(.ctors)
+        *crtbegin.o(.dtors)
+        *crtbegin?.o(.dtors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+        *(SORT(.dtors.*))
+        *(.dtors)
+        *(.rodata*)
+        KEEP(*(.eh_frame*))
+    } > FLASH
+    .ARM.extab :
+    {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } > FLASH
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > FLASH
+    __exidx_end = .;
+    __etext = .;
+    .data : AT (__etext)
+    {
+        __data_start__ = .;
+        Image$$RW_IRAM1$$Base = .;
+        *(vtable)
+        *(.data*)
+        . = ALIGN(4);
+        PROVIDE (__preinit_array_start = .);
+        KEEP(*(.preinit_array))
+        PROVIDE (__preinit_array_end = .);
+        . = ALIGN(4);
+        PROVIDE (__init_array_start = .);
+        KEEP(*(SORT(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE (__init_array_end = .);
+        . = ALIGN(4);
+        PROVIDE (__fini_array_start = .);
+        KEEP(*(SORT(.fini_array.*)))
+        KEEP(*(.fini_array))
+        PROVIDE (__fini_array_end = .);
+        . = ALIGN(4);
+        __data_end__ = .;
+    } > RAM
+    .bss :
+    {
+        __bss_start__ = .;
+        *(.bss*)
+        *(COMMON)
+        __bss_end__ = .;
+        Image$$RW_IRAM1$$ZI$$Limit = . ;
+    } > RAM
+    .heap :
+    {
+        __end__ = .;
+        end = __end__;
+        *(.heap*)
+        __HeapLimit = .;
+    } > RAM
+    .stack_dummy :
+    {
+        *(.stack)
+    } > RAM
+    __StackTop = ORIGIN(RAM) + LENGTH(RAM);
+    __StackLimit = __StackTop - SIZEOF(.stack_dummy);
+    PROVIDE(__stack = __StackTop);
+    ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed with stack")
+    .AHBSRAM0 (NOLOAD):
+    {
+        Image$$RW_IRAM2$$Base = . ;
+        *(AHBSRAM0)
+        Image$$RW_IRAM2$$ZI$$Limit = .;
+    } > USB_RAM
+    .AHBSRAM1 (NOLOAD):
+    {
+        Image$$RW_IRAM3$$Base = . ;
+        *(AHBSRAM1)
+        Image$$RW_IRAM3$$ZI$$Limit = .;
+    } > ETH_RAM
+}
diff --git a/linker/bootloader.ld b/linker/bootloader.ld
new file mode 100644
index 0000000..1dcd122
--- /dev/null
+++ b/linker/bootloader.ld
@@ -0,0 +1,99 @@
+MEMORY
+{
+  FLASH (rx) : ORIGIN = 0x00000000, LENGTH = 32K
+  RAM (rwx) : ORIGIN = 0x100000C8, LENGTH = (32K - 0xC8 - 32)
+  USB_RAM(rwx) : ORIGIN = 0x2007C000, LENGTH = 16K
+  ETH_RAM(rwx) : ORIGIN = 0x20080000, LENGTH = 16K
+}
+ENTRY(Reset_Handler)
+SECTIONS
+{
+    .text :
+    {
+        KEEP(*(.isr_vector))
+        *(.text*)
+        KEEP(*(.init))
+        KEEP(*(.fini))
+        *crtbegin.o(.ctors)
+        *crtbegin?.o(.ctors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+        *(SORT(.ctors.*))
+        *(.ctors)
+        *crtbegin.o(.dtors)
+        *crtbegin?.o(.dtors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+        *(SORT(.dtors.*))
+        *(.dtors)
+        *(.rodata*)
+        KEEP(*(.eh_frame*))
+    } > FLASH
+    .ARM.extab :
+    {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } > FLASH
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > FLASH
+    __exidx_end = .;
+    __etext = .;
+    .data : AT (__etext)
+    {
+        __data_start__ = .;
+        Image$$RW_IRAM1$$Base = .;
+        *(vtable)
+        *(.data*)
+        . = ALIGN(4);
+        PROVIDE (__preinit_array_start = .);
+        KEEP(*(.preinit_array))
+        PROVIDE (__preinit_array_end = .);
+        . = ALIGN(4);
+        PROVIDE (__init_array_start = .);
+        KEEP(*(SORT(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE (__init_array_end = .);
+        . = ALIGN(4);
+        PROVIDE (__fini_array_start = .);
+        KEEP(*(SORT(.fini_array.*)))
+        KEEP(*(.fini_array))
+        PROVIDE (__fini_array_end = .);
+        . = ALIGN(4);
+        __data_end__ = .;
+    } > RAM
+    .bss :
+    {
+        __bss_start__ = .;
+        *(.bss*)
+        *(COMMON)
+        __bss_end__ = .;
+        Image$$RW_IRAM1$$ZI$$Limit = . ;
+    } > RAM
+    .heap :
+    {
+        __end__ = .;
+        end = __end__;
+        *(.heap*)
+        __HeapLimit = .;
+    } > RAM
+    .stack_dummy :
+    {
+        *(.stack)
+    } > RAM
+    __StackTop = ORIGIN(RAM) + LENGTH(RAM);
+    __StackLimit = __StackTop - SIZEOF(.stack_dummy);
+    PROVIDE(__stack = __StackTop);
+    ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed with stack")
+    .AHBSRAM0 (NOLOAD):
+    {
+        Image$$RW_IRAM2$$Base = . ;
+        *(AHBSRAM0)
+        Image$$RW_IRAM2$$ZI$$Limit = .;
+    } > USB_RAM
+    .AHBSRAM1 (NOLOAD):
+    {
+        Image$$RW_IRAM3$$Base = . ;
+        *(AHBSRAM1)
+        Image$$RW_IRAM3$$ZI$$Limit = .;
+    } > ETH_RAM
+}
diff --git a/src/Drivers/CDCE906.cpp b/src/Drivers/CDCE906.cpp
new file mode 100644
index 0000000..7b8fda5
--- /dev/null
+++ b/src/Drivers/CDCE906.cpp
@@ -0,0 +1,68 @@
+#include "CDCE906.h"
+
+int CDCE906::cfg_eth( void )
+{
+    char cfg[26] = {0};
+    int err = 0;
+
+    cfg[0] = 0x1;
+    cfg[1] = 9;   /* PLL 1 M */
+    cfg[2] = 25;   /* PLL 1 N */
+    cfg[3] = 0x0;
+    cfg[4] = 9;   /* PLL 2 M */
+    cfg[5] = 25; /* PLL 2 N */
+    cfg[6] = 0xE0;
+    cfg[7] = 9;   /* PLL 3 M */
+    cfg[8] = 25;   /* PLL 3 N */
+    cfg[9] = (3<<5);
+    cfg[10] = 0x0;
+    cfg[11] = 0x0;
+    cfg[12] = 0x0;
+    cfg[13] = 0x2; /* P0 div */
+    cfg[14] = 0x1;  /* P1 div */
+    cfg[15] = 0x1; /* P2 div */
+    cfg[16] = 0x1; /* P3 div */
+    cfg[17] = 0x1; /* P4 div */
+    cfg[18] = 0x1; /* P5 div */
+    cfg[19] = 0x38; /* Y0 */
+    cfg[20] = 0;    /* Y1 */
+    cfg[21] = 0;    /* Y2 */
+    cfg[22] = 0;    /* Y3 */
+    cfg[23] = 0;    /* Y4 */
+    cfg[24] = 0;    /* Y5 */
+    cfg[25] = (3<<4)|0xB;
+
+    err = _write(0, cfg, sizeof(cfg));
+
+    return err;
+}
+
+int CDCE906::_write(uint8_t addr, char *buffer, size_t len)
+{
+    int err;
+    char *data = (char *) malloc(len+2);
+
+    data[0] = addr;
+    data[1] = len;
+    memcpy(&data[2], buffer, len);
+    err = _i2c.write(_sladdr, &data[0], len+2);
+
+    free(data);
+
+    return err;
+}
+
+int CDCE906::_read(uint8_t addr, char *buffer, size_t len)
+{
+    int err;
+    char data[1];
+
+    data[0] = addr;
+    err = _i2c.write(_sladdr, data, 1, true);
+
+    if (err == 0) {
+        err = _i2c.read(_sladdr, buffer, len);
+    }
+
+    return err;
+}
diff --git a/src/Drivers/CDCE906.h b/src/Drivers/CDCE906.h
new file mode 100644
index 0000000..ad596b5
--- /dev/null
+++ b/src/Drivers/CDCE906.h
@@ -0,0 +1,22 @@
+#ifndef CDCE906_H_
+#define CDCE906_H_
+
+#include "mbed.h"
+#include "rtos.h"
+
+class CDCE906
+{
+public:
+
+    CDCE906( mbed::I2C& i2c, uint8_t addr )
+    : _i2c(i2c), _sladdr(addr) {
+    }
+
+    int cfg_eth();
+private:
+    int _read(uint8_t addr, char *buffer, size_t len);
+    int _write(uint8_t addr, char *buffer, size_t len);
+    I2C& _i2c;
+    int _sladdr;
+};
+#endif
diff --git a/src/Drivers/Drivers.h b/src/Drivers/Drivers.h
index 92d1db6..7d62c1f 100644
--- a/src/Drivers/Drivers.h
+++ b/src/Drivers/Drivers.h
@@ -4,6 +4,7 @@
 #include "ADT7320.h"
 #include "DAC7554.h"
 #include "LM71.h"
+#include "CDCE906.h"
 #include "feram.h"
 
 #endif
diff --git a/src/Drivers/LM71.cpp b/src/Drivers/LM71.cpp
index d583f6a..0498e67 100644
--- a/src/Drivers/LM71.cpp
+++ b/src/Drivers/LM71.cpp
@@ -6,7 +6,7 @@ double LM71::Read()
     double temp = 0;
 
     if (_mode != LM71_MODE_CONVERSION) {
-    	LM71::Config(_freq, LM71_MODE_CONVERSION);
+        LM71::Config(_freq, LM71_MODE_CONVERSION);
     }
 
     _spi.format(16,0);
diff --git a/src/Drivers/feram.cpp b/src/Drivers/feram.cpp
index 3f45a13..c79e616 100644
--- a/src/Drivers/feram.cpp
+++ b/src/Drivers/feram.cpp
@@ -1,6 +1,6 @@
 #include "feram.h"
 
-int FeRAM::write(uint16_t addr, char *buffer, size_t len)
+int FeRAM::write(uint16_t addr, uint8_t *buffer, size_t len)
 {
     int err;
     char *data = (char *) malloc(len+1);
@@ -11,7 +11,7 @@ int FeRAM::write(uint16_t addr, char *buffer, size_t len)
 
     _wp = 0;
     /* Upper 3 bits from addr are the bits [3:1] from the slave address */
-    err = _i2c.write(page_sladdr, &data[0], len+1);
+    err = _i2c.write(page_sladdr, (char *) &data[0], len+1);
     _wp = 1;
 
     free(data);
@@ -19,7 +19,7 @@ int FeRAM::write(uint16_t addr, char *buffer, size_t len)
     return err;
 }
 
-int FeRAM::read(uint16_t addr, char *buffer, size_t len)
+int FeRAM::read(uint16_t addr, uint8_t *buffer, size_t len)
 {
     int err;
     char data[1];
@@ -30,32 +30,59 @@ int FeRAM::read(uint16_t addr, char *buffer, size_t len)
     err = _i2c.write(page_sladdr, data, 1, true);
 
     if (err == 0) {
-        err = _i2c.read(page_sladdr, buffer, len);
+        err = _i2c.read(page_sladdr, (char *) buffer, len);
     }
 
     return err;
 }
 
-int FeRAM::get_mac_addr(char *mac_str)
+int FeRAM::get_mac_addr(char *mac_str, char* mac_buf)
 {
     int err;
-    char mac[6] = {0};
+    uint8_t mac[6] = {0};
 
-    err = FeRAM::read(0, mac, 6);
+    err = FeRAM::read(FERAM_MAC_ADDR_OFFSET, mac, sizeof(mac));
 
     snprintf(mac_str, 18, "%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
 
+    if (mac_buf) {
+        memcpy(mac_buf, mac, 6);
+    }
+    return err;
+}
+
+int FeRAM::set_mac_addr(char *mac_str)
+{
+    int err;
+    uint8_t mac[6] = {0};
+
+    sscanf(mac_str, "%02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx", &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5]);
+
+    err = FeRAM::write(FERAM_MAC_ADDR_OFFSET, mac, sizeof(mac));
+
     return err;
 }
 
 int FeRAM::get_ip_addr(char *ip_str)
 {
     int err;
-    char ip[4] = {0};
+    uint8_t ip[4] = {0};
+
+    err = FeRAM::read(FERAM_IP_ADDR_OFFSET, ip, sizeof(ip));
+
+    snprintf(ip_str, 16, "%hhu.%hhu.%hhu.%hhu", ip[0], ip[1], ip[2], ip[3]);
+
+    return err;
+}
+
+int FeRAM::set_ip_addr(char *ip_str)
+{
+    int err;
+    uint8_t ip[4] = {0};
 
-    err = FeRAM::read(0x10, ip, 4);
+    sscanf(ip_str, "%hhu.%hhu.%hhu.%hhu", &ip[0], &ip[1], &ip[2], &ip[3]);
 
-    snprintf(ip_str, 16, "%d.%d.%d.%d", ip[0], ip[1], ip[2], ip[3]);
+    err = FeRAM::write(FERAM_IP_ADDR_OFFSET, ip, sizeof(ip));
 
     return err;
 }
@@ -63,24 +90,47 @@ int FeRAM::get_ip_addr(char *ip_str)
 int FeRAM::get_mask_addr(char *mask_str)
 {
     int err;
-    char mask[4] = {0};
+    uint8_t mask[4] = {0};
 
-    err = FeRAM::read(0x20, mask, 4);
+    err = FeRAM::read(FERAM_MASK_ADDR_OFFSET, mask, sizeof(mask));
 
-    snprintf(mask_str, 16, "%d.%d.%d.%d", mask[0], mask[1], mask[2], mask[3]);
+    snprintf(mask_str, 16, "%hhu.%hhu.%hhu.%hhu", mask[0], mask[1], mask[2], mask[3]);
 
     return err;
 }
 
+int FeRAM::set_mask_addr(char *mask_str)
+{
+    int err;
+    uint8_t mask[4] = {0};
+
+    sscanf(mask_str,"%hhu.%hhu.%hhu.%hhu", &mask[0], &mask[1], &mask[2], &mask[3]);
+
+    err = FeRAM::write(FERAM_MASK_ADDR_OFFSET, mask, sizeof(mask));
+
+    return err;
+}
 
 int FeRAM::get_gateway_addr(char *gateway_str)
 {
     int err;
-    char gateway[4] = {0};
+    uint8_t gateway[4] = {0};
+
+    err = FeRAM::read(FERAM_GATEWAY_ADDR_OFFSET, gateway, sizeof(gateway));
+
+    snprintf(gateway_str, 16, "%hhu.%hhu.%hhu.%hhu", gateway[0], gateway[1], gateway[2], gateway[3]);
+
+    return err;
+}
+
+int FeRAM::set_gateway_addr(char *gateway_str)
+{
+    int err;
+    uint8_t gateway[4] = {0};
 
-    err = FeRAM::read(0x30, gateway, 4);
+    sscanf(gateway_str, "%hhu.%hhu.%hhu.%hhu", &gateway[0], &gateway[1], &gateway[2], &gateway[3]);
 
-    snprintf(gateway_str, 16, "%d.%d.%d.%d", gateway[0], gateway[1], gateway[2], gateway[3]);
+    err = FeRAM::write(FERAM_GATEWAY_ADDR_OFFSET, gateway, sizeof(gateway));
 
     return err;
 }
diff --git a/src/Drivers/feram.h b/src/Drivers/feram.h
index 84ce4f3..45fbd22 100644
--- a/src/Drivers/feram.h
+++ b/src/Drivers/feram.h
@@ -4,6 +4,11 @@
 #include "mbed.h"
 #include "rtos.h"
 
+#define FERAM_MAC_ADDR_OFFSET     0
+#define FERAM_IP_ADDR_OFFSET      0x10
+#define FERAM_MASK_ADDR_OFFSET    0x20
+#define FERAM_GATEWAY_ADDR_OFFSET 0x30
+
 class FeRAM
 {
 public:
@@ -12,13 +17,17 @@ class FeRAM
     : _i2c(i2c), _wp(wp), _sladdr(sladdr) {
     }
 
-    int read(uint16_t addr, char *buffer, size_t len); /* 11bit address */
-    int write(uint16_t addr, char *buffer, size_t len);
+    int read(uint16_t addr, uint8_t *buffer, size_t len); /* 11bit address */
+    int write(uint16_t addr, uint8_t *buffer, size_t len);
 
-    int get_mac_addr(char *mac_str);
+    int get_mac_addr(char *mac_str, char *mac_buf);
+    int set_mac_addr(char *mac_str);
     int get_ip_addr(char *ip_str);
+    int set_ip_addr(char *ip_str);
     int get_mask_addr(char *mask_str);
+    int set_mask_addr(char *mask_str);
     int get_gateway_addr(char *gateway_str);
+    int set_gateway_addr(char *gateway_str);
 private:
     I2C& _i2c;
     mbed::DigitalOut& _wp;
diff --git a/src/cli.cpp b/src/cli.cpp
index a27c64d..473d037 100644
--- a/src/cli.cpp
+++ b/src/cli.cpp
@@ -21,6 +21,7 @@
 */
 
 #include "cli.h"
+#include "ctype.h"
 
 struct SCMD *sc_listeners[SCMD_MAX_LISTENERS]; // set up to support upto 10 listeners
 
@@ -89,6 +90,7 @@ struct SCMD *scMake(RawSerial *sio, void (*callback)(char *, void *), void *call
     sc->callbackExtra = callbackExtra;
     sc_add_listener(sc);
     sc->sio->attach(&sc_rx_interrupt, RawSerial::RxIrq);
+    sc->in_ndx = 0;
     return sc;
 }
 
@@ -104,8 +106,12 @@ void sc_rx_process(struct SCMD *wrk) {
 
     while ((wrk->sio->readable())) {
         char cin = wrk->sio->getc();
-        wrk->sio->putc(cin);
-        if ((cin == 10) || (cin == 13)) { // found CR or LF
+
+        switch (cin) {
+
+            /* Carriage Return or Line Feed (CRLF) */
+        case 10:
+        case 13:
             if (wrk->in_ndx > 0) {
                 // commands must be at least 1 byte long
                 wrk->buff[wrk->in_ndx] = 0;    // add terminating null
@@ -113,23 +119,43 @@ void sc_rx_process(struct SCMD *wrk) {
             }
             wrk->in_ndx = 0;  // reset for next cycle;
             wrk->buff[0] = 0; // add null terminator
-        }
-        else {
-            // not a CR or LF so must be a valid character
-            wrk->buff[wrk->in_ndx] = cin; // add character to the buffer
-            wrk->buff[wrk->in_ndx + 1] = 0; // add null terminator just in case
+            break;
+
+            /* Backspace */
+        case 8:
+                /* Print a space over the last char and rewind the cursor */
+                wrk->sio->putc(cin);
+                wrk->sio->putc(' ');
+                wrk->sio->putc(cin);
+                if ( wrk->in_ndx > 0 ) {
+                    wrk->in_ndx--;
+                }
+                wrk->buff[wrk->in_ndx] = 0;
+
+            break;
+
+        default:
+            /* Check if input char is printable (valid) */
+            if (!isprint(cin)) {
+                break;
+            }
+            /* Echo back */
+            wrk->sio->putc(cin);
+            /* Add character to the buffer */
+            wrk->buff[wrk->in_ndx] = cin;
+            /* Add null terminator just in case */
+            wrk->buff[wrk->in_ndx + 1] = 0;
             wrk->in_ndx++;
-            //printf("wrk->buff=%s in_ndx=%d\n", wrk->buff, wrk->in_ndx);
+
             if (wrk->in_ndx >= SCMD_MAX_CMD_LEN) {
-                // buffer is full so treat as command
+                /* Buffer is full so treat as command */
                 wrk->callback(wrk->buff, wrk->callbackExtra);
                 wrk->buff[0] = 0;
                 wrk->in_ndx = 0;
-                // add callback here
             }
+            break;
         }
     }
-    return;
 }
 
 // Process all characters available in all the
diff --git a/src/main.cpp b/src/main.cpp
index 38ec197..2e8b0f1 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -5,6 +5,7 @@
 #include "TCPServer.h"
 #include "TCPSocket.h"
 #include "lpc_phy.h"
+#include "watchdog.h"
 
 /* IAP includes */
 #include "boot.h"
@@ -37,12 +38,7 @@ extern "C" {
 #define FILE_DATASIZE   128
 
 /* Firmware version macros */
-#define FW_VERSION      "V1_0_0"
-
-/* MBED Reset function */
-void mbed_reset( void ) {
-    NVIC_SystemReset();
-}
+#define FW_VERSION      "V1_1_0"
 
 // BSMP Variables arrays
 double Att[1];
@@ -68,6 +64,8 @@ char Mask_Addr[16];
 char Gateway_Addr[16];
 char MAC_Addr[18];
 
+char mac_buffer[6];
+
 #define READ_ONLY  0
 #define READ_WRITE 1
 
@@ -75,28 +73,41 @@ char MAC_Addr[18];
 /* The index in this table will coincide with the index on the server list, since it registrates the variables sequentially */
 
 struct bsmp_var rffe_vars[] = {
-    RFFE_VAR( Att,            READ_WRITE ), // Attenuators
-    RFFE_VAR( TempAC,         READ_ONLY ), // TempAC
-    RFFE_VAR( TempBD,         READ_ONLY ), // TempBD
-    RFFE_VAR( Set_PointAC,    READ_WRITE ), // Set_PointAC
-    RFFE_VAR( Set_PointBD,    READ_WRITE ), // Set_PointBD
-    RFFE_VAR( Temp_Control,   READ_WRITE ), // Temp_Control
-    RFFE_VAR( HeaterAC,       READ_WRITE ), // HeaterAC
-    RFFE_VAR( HeaterBD,       READ_WRITE ), // HeaterBD
-    RFFE_VAR( Reset,          READ_WRITE ), // Reset
-    RFFE_VAR( Reprogramming,  READ_WRITE ), // Reprogramming
-    RFFE_VAR( Data,           READ_WRITE ), // Data
-    RFFE_VAR( Version,        READ_ONLY ), // Version
-    RFFE_VAR( PID_AC_Kc,      READ_WRITE ), // PID_AC_Kc
-    RFFE_VAR( PID_AC_tauI,    READ_WRITE ), // PID_AC_tauI
-    RFFE_VAR( PID_AC_tauD,    READ_WRITE ), // PID_AC_tauD
-    RFFE_VAR( PID_BD_Kc,      READ_WRITE ), // PID_BD_Kc
-    RFFE_VAR( PID_BD_tauI,    READ_WRITE ), // PID_BD_tauI
-    RFFE_VAR( PID_BD_tauD,    READ_WRITE ), // PID_BD_tauD
-    RFFE_VAR( IP_Addr,        READ_WRITE ), // Ip Address
-    RFFE_VAR( MAC_Addr,       READ_WRITE ), // MAC Address
+    /* [0] = */  RFFE_VAR( Att,            READ_WRITE ), // Attenuators
+    /* [1] = */  RFFE_VAR( TempAC,         READ_ONLY ), // TempAC
+    /* [2] = */  RFFE_VAR( TempBD,         READ_ONLY ), // TempBD
+    /* [3] = */  RFFE_VAR( Set_PointAC,    READ_WRITE ), // Set_PointAC
+    /* [4] = */  RFFE_VAR( Set_PointBD,    READ_WRITE ), // Set_PointBD
+    /* [5] = */  RFFE_VAR( Temp_Control,   READ_WRITE ), // Temp_Control
+    /* [6] = */  RFFE_VAR( HeaterAC,       READ_WRITE ), // HeaterAC
+    /* [7] = */  RFFE_VAR( HeaterBD,       READ_WRITE ), // HeaterBD
+    /* [8] = */  RFFE_VAR( Reset,          READ_WRITE ), // Reset
+    /* [9] = */  RFFE_VAR( Reprogramming,  READ_WRITE ), // Reprogramming
+    /* [10] = */ RFFE_VAR( Data,           READ_WRITE ), // Data
+    /* [11] = */ RFFE_VAR( Version,        READ_ONLY ), // Version
+    /* [12] = */ RFFE_VAR( PID_AC_Kc,      READ_WRITE ), // PID_AC_Kc
+    /* [13] = */ RFFE_VAR( PID_AC_tauI,    READ_WRITE ), // PID_AC_tauI
+    /* [14] = */ RFFE_VAR( PID_AC_tauD,    READ_WRITE ), // PID_AC_tauD
+    /* [15] = */ RFFE_VAR( PID_BD_Kc,      READ_WRITE ), // PID_BD_Kc
+    /* [16] = */ RFFE_VAR( PID_BD_tauI,    READ_WRITE ), // PID_BD_tauI
+    /* [17] = */ RFFE_VAR( PID_BD_tauD,    READ_WRITE ), // PID_BD_tauD
+    /* [18] = */ RFFE_VAR( IP_Addr,        READ_WRITE ), // Ip Address
+    /* [19] = */ RFFE_VAR( MAC_Addr,       READ_WRITE ), // MAC Address
+    /* [20] = */ RFFE_VAR( Gateway_Addr,   READ_WRITE ), // Gateway Address
+    /* [21] = */ RFFE_VAR( Mask_Addr,      READ_WRITE ), // Mask Address
 };
 
+typedef struct {
+    struct bsmp_raw_packet msg;
+    Mail<struct bsmp_raw_packet, 5> *response_mail_box;
+} bsmp_mail_t;
+
+Mail<bsmp_mail_t, 5> bsmp_mail_box;
+Mail<struct bsmp_raw_packet, 5> eth_mail_box;
+
+/* Setup the watchdog timer */
+Watchdog wdt;
+
 /* BSMP server */
 bsmp_server_t *bsmp;
 
@@ -104,6 +115,7 @@ bsmp_server_t *bsmp;
 Thread Temp_Control_thread(osPriorityNormal, 1200, NULL, "TEMP");
 Thread Attenuators_thread(osPriorityNormal, 800, NULL, "ATT");
 Thread CLI_Proccess_Thread(osPriorityNormal, 1024, NULL, "CLI");
+Thread BSMP_Thread(osPriorityNormal, 800, NULL, "BSMP");
 
 // Hardware Initialization - MBED
 
@@ -129,10 +141,26 @@ DigitalOut CS_dac(P0_24); // Chip select for DAC. LVTTL, low = Selected, init =
 RawSerial pc(P0_2, P0_3); // Serial USB port. (NOTE: All printf() calls are redirected to this port)
 SPI spi1(P0_9,P0_8,P0_7); //SPI Interface - spi(mosi, miso, sclk)
 
+I2C pll_i2c(P0_27, P0_28);
+CDCE906 pll(pll_i2c, 0b11010010);
 I2C feram_i2c(P0_19, P0_20);
 DigitalOut feram_wp(P0_21);
 FeRAM feram(feram_i2c, feram_wp);
 
+/* MBED functions replacements */
+extern "C" void mbed_mac_address(char *s)
+{
+    memcpy(s, mac_buffer, 6);
+}
+
+/* MBED Reset function */
+void mbed_reset( void )
+{
+    wdt.kick(0.1);
+    /* Lock the firmware and wait for the overflow */
+    while(1);
+}
+
 bool get_eth_link_status(void)
 {
     return (lpc_mii_read_data() & DP8_VALID_LINK) ? true : false;
@@ -166,7 +194,7 @@ void Temp_Feedback_Control( void )
     int state = 2;
     int pid_state = MANUAL;
 
-    printf("Initializing Temp Control thread\n");
+    printf("Initializing Temp Control thread\n\r");
 
     /* Create PIDs with generic tuning constants (they will be updated as soon as the control loop starts) */
     pidAC.SetSampleTime( PID_RATE*1000 );
@@ -183,7 +211,7 @@ void Temp_Feedback_Control( void )
 
     while (1) {
         if (state != get_value8(Temp_Control)) {
-            printf ("Temperature control in %s mode!\n", (get_value8(Temp_Control) == AUTOMATIC) ? "AUTOMATIC":"MANUAL");
+            printf ("Temperature control in %s mode!\n\r", (get_value8(Temp_Control) == AUTOMATIC) ? "AUTOMATIC":"MANUAL");
             state = get_value8(Temp_Control);
 
             pid_state = (state != MANUAL) ? AUTOMATIC : MANUAL;
@@ -205,12 +233,12 @@ void Temp_Feedback_Control( void )
         SetP_BD = get_value64(Set_PointBD);
 
 #ifdef DEBUG_PRINTF
-        printf( "AC_Temp = %f \n", ProcessValueAC );
-        printf( "BD_Temp = %f \n", ProcessValueBD );
-        printf( "PID_AC Params:\n");
-        printf( "\tKc:%f\ttauI:%f\ttauD:%f\n", get_value64(PID_AC_Kc), get_value64(PID_AC_tauI), get_value64(PID_AC_tauD));
-        printf( "PID_BD Params:\n");
-        printf( "\tKc:%f\ttauI:%f\ttauD:%f\n", get_value64(PID_BD_Kc), get_value64(PID_BD_tauI), get_value64(PID_BD_tauD));
+        printf( "AC_Temp = %f \n\r", ProcessValueAC );
+        printf( "BD_Temp = %f \n\r", ProcessValueBD );
+        printf( "PID_AC Params:\n\r");
+        printf( "\tKc:%f\ttauI:%f\ttauD:%f\n\r", get_value64(PID_AC_Kc), get_value64(PID_AC_tauI), get_value64(PID_AC_tauD));
+        printf( "PID_BD Params:\n\r");
+        printf( "\tKc:%f\ttauI:%f\ttauD:%f\n\r", get_value64(PID_BD_Kc), get_value64(PID_BD_tauI), get_value64(PID_BD_tauD));
 #endif
 
         // Update PID tuning values
@@ -252,7 +280,7 @@ void Temp_Feedback_Control( void )
         BD_Heater_DAC.Write( voutBD );
 
 #ifdef DEBUG_PRINTF
-        printf("Heater output AC: %f \t BD: %f\n", voutAC, voutBD);
+        printf("Heater output AC: %f \t BD: %f\n\r", voutAC, voutBD);
 #endif
         Thread::wait(100);
     }
@@ -263,7 +291,7 @@ void Attenuators_Control( void )
     double prev_att1 = 0;
     bool attVec1[6];
 
-    printf("Initializing Attenuators thread\n");
+    printf("Initializing Attenuators thread\n\r");
 
     while (1) {
         // Attenuators set
@@ -272,7 +300,7 @@ void Attenuators_Control( void )
             // Checking and setting attenuators value to fisable values
             set_value(Att,(float)(int(get_value64(Att)*2))/2);
 #ifdef DEBUG_PRINTF
-            printf("\nAtt values updated from: %f to %f\n", prev_att1, get_value64(Att));
+            printf("\n\rAtt values updated from: %f to %f\n\r", prev_att1, get_value64(Att));
 #endif
             // Updating previous values
             prev_att1 = get_value64(Att);
@@ -316,8 +344,9 @@ void CLI_Proccess( void )
 {
     char *cmd, *save_ptr;
     char *arg[2];
+    uint8_t msg_buffer[30] = {0};
 
-    printf("Initializing CLI_Proccess thread\n");
+    printf("Initializing CLI_Proccess thread\n\r");
 
     for( ; ; ) {
         Thread::signal_wait(0x01);
@@ -327,68 +356,97 @@ void CLI_Proccess( void )
             arg[i] = strtok_r( NULL, " ", &save_ptr);
         }
         printf("\r\n");
-        if (strcmp( cmd, "dump" ) == 0) {
-            printf("RFFE Vars dump:\n");
-            printf("\t[0]  Att: %f\n", get_value64(Att));
-            printf("\t[1]  Temperature AC: %f\n", get_value64(TempAC));
-            printf("\t[2]  Temperature BD: %f\n", get_value64(TempBD));
-            printf("\t[3]  Set PointAC: %f\n", get_value64(Set_PointAC));
-            printf("\t[4]  Set PointBD: %f\n", get_value64(Set_PointBD));
-            printf("\t[5]  Temperature Control PID: %s\n", get_value8(Temp_Control) ? "AUTOMATIC":"MANUAL");
-            printf("\t[6]  Heater AC: %f\n", get_value64(HeaterAC));
-            printf("\t[7]  Heater BD: %f\n", get_value64(HeaterBD));
-            printf("\t[8]  Reset: %d\n", get_value8(Reset));
-            printf("\t[9]  Reprogramming: %d\n", get_value8(Reprogramming));
-            printf("\t[10] New FW Data\n");
-            printf("\t[11] Firmware version: %s\n", FW_VERSION);
-            printf("\t[12] PID_AC_Kc: %f\n", get_value64(PID_AC_Kc));
-            printf("\t[13] PID_AC_tauI: %f\n", get_value64(PID_AC_tauI));
-            printf("\t[14] PID_AC_tauD: %f\n", get_value64(PID_AC_tauD));
-            printf("\t[15] PID_BD_Kc: %f\n", get_value64(PID_BD_Kc));
-            printf("\t[16] PID_BD_tauI: %f\n", get_value64(PID_BD_tauI));
-            printf("\t[17] PID_BD_tauD: %f\n", get_value64(PID_BD_tauD));
-            printf("\t[18] IP-Address: %s\n", IP_Addr);
-            printf("\t[19] MAC-Address: %s\n", MAC_Addr);
-            printf("\n");
-        } else if (strcmp( cmd, "set" ) == 0) {
+        if (strncmp( cmd, "dump", 5 ) == 0) {
+            printf("RFFE Vars dump:\n\r");
+            printf("\t[0]  Att: %f\n\r", get_value64(Att));
+            printf("\t[1]  Temperature AC: %f\n\r", get_value64(TempAC));
+            printf("\t[2]  Temperature BD: %f\n\r", get_value64(TempBD));
+            printf("\t[3]  Set PointAC: %f\n\r", get_value64(Set_PointAC));
+            printf("\t[4]  Set PointBD: %f\n\r", get_value64(Set_PointBD));
+            printf("\t[5]  Temperature Control PID: %s\n\r", get_value8(Temp_Control) ? "AUTOMATIC":"MANUAL");
+            printf("\t[6]  Heater AC: %f\n\r", get_value64(HeaterAC));
+            printf("\t[7]  Heater BD: %f\n\r", get_value64(HeaterBD));
+            printf("\t[8]  Reset: %d\n\r", get_value8(Reset));
+            printf("\t[9]  Reprogramming: %d\n\r", get_value8(Reprogramming));
+            printf("\t[10] New FW Data\n\r");
+            printf("\t[11] Firmware version: %s\n\r", FW_VERSION);
+            printf("\t[12] PID_AC_Kc: %f\n\r", get_value64(PID_AC_Kc));
+            printf("\t[13] PID_AC_tauI: %f\n\r", get_value64(PID_AC_tauI));
+            printf("\t[14] PID_AC_tauD: %f\n\r", get_value64(PID_AC_tauD));
+            printf("\t[15] PID_BD_Kc: %f\n\r", get_value64(PID_BD_Kc));
+            printf("\t[16] PID_BD_tauI: %f\n\r", get_value64(PID_BD_tauI));
+            printf("\t[17] PID_BD_tauD: %f\n\r", get_value64(PID_BD_tauD));
+            printf("\t[18] IP-Address: %s\n\r", IP_Addr);
+            printf("\t[19] MAC-Address: %s\n\r", MAC_Addr);
+            printf("\t[20] Gateway-Address: %s\n\r", Gateway_Addr);
+            printf("\t[21] Mask-Address: %s\n\r", Mask_Addr);
+            printf("\n\r");
+        } else if (strncmp( cmd, "set", 4 ) == 0) {
             if ((arg[0] == NULL) || (arg[1] == NULL)) {
-                printf("Command \"set\" used but no arguments given! Type \"help\" to see its correct usage.\n");
+                printf("Command \"set\" used but no arguments given! Type \"help\" to see its correct usage.\n\r");
                 continue;
             }
             uint8_t var_index = strtol( arg[0], NULL, 10);
 
             if (rffe_vars[var_index].info.writable == READ_ONLY) {
-                printf("The requested variable is READ_ONLY!\n");
+                printf("The requested variable is READ_ONLY!\n\r");
                 continue;
             }
 
-            /* Special case for reset */
-            if (var_index == 8) {
-                printf("Resetting MBED...\n");
-                mbed_reset();
-            }
+            /* Clear request msg buffer */
+            memset(msg_buffer, 0, sizeof(msg_buffer));
+
+            bsmp_mail_t *mail = bsmp_mail_box.alloc();
 
-            if (rffe_vars[var_index].info.size == sizeof(int)){
+            mail->response_mail_box = NULL;
+            mail->msg.data = msg_buffer;
+            mail->msg.data[0] = 0x20; /* CMD_VAR_WRITE */
+            mail->msg.data[3] = var_index; /* Payload[0]: Var id */
+
+            uint16_t payload_size = 0;
+
+            switch(rffe_vars[var_index].info.size) {
+            case sizeof(int):
+            {
                 int arg_int = strtol( arg[1], NULL, 10);
-                set_value( (int *)rffe_vars[var_index].data, arg_int);
-            } else if ( (rffe_vars[var_index].info.size == sizeof(double)) ) {
+                memcpy(&(mail->msg.data[4]), &arg_int, sizeof(arg_int));
+                payload_size = sizeof(int);
+                break;
+            }
+            case sizeof(double):
+            {
                 double arg_dbl = strtod( arg[1], NULL);
-                set_value( (double *)rffe_vars[var_index].data, arg_dbl);
-            } else if ( (rffe_vars[var_index].info.size == sizeof(uint8_t)) ) {
-                uint8_t arg_dbl = strtoul( arg[1], NULL, 10);
-                set_value( (uint8_t *)rffe_vars[var_index].data, arg_dbl);
-            } else {
-                printf("Unknown data type to set!\n");
+                memcpy(&(mail->msg.data[4]), &arg_dbl, sizeof(arg_dbl));
+                payload_size = sizeof(double);
+                break;
+            }
+            case sizeof(uint8_t):
+            {
+                uint8_t arg_byte = strtoul( arg[1], NULL, 10);
+                memcpy(&(mail->msg.data[4]), &arg_byte, sizeof(arg_byte));
+                payload_size = sizeof(uint8_t);
+                break;
+            }
+            default:
+                strncpy((char *)&(mail->msg.data[4]), arg[1], rffe_vars[var_index].info.size);
+                payload_size = rffe_vars[var_index].info.size;
             }
 
-        } else if ((strcmp( cmd, "help" ) == 0) || (strcmp( cmd, "?" ) == 0) ) {
-            printf("RFFE Firmware help. Available commands:\n");
-            printf("\tCMD\t[arg1]\t[arg2]\n");
-            printf("\tdump\t\t\tList all variables available and their current status\n");
-            printf("\tset\t[VAR]\t[VALUE]\tSet value to a variable in the list\n");
-            printf("\thelp\t\t\tShow this help menu\n");
+            payload_size++; /* Var id counts as payload */
+            mail->msg.data[1] = (payload_size >> 8) & 0xFF; /* Payload size >> 8 */
+            mail->msg.data[2] = payload_size & 0xFF; /* Payload size */
+            mail->msg.len = payload_size + 3;
+
+            bsmp_mail_box.put(mail);
+
+        } else if ((strncmp( cmd, "help", 5 ) == 0) || (strncmp( cmd, "?", 2 ) == 0) ) {
+            printf("RFFE Firmware help. Available commands:\n\r");
+            printf("\tCMD\t[arg1]\t[arg2]\n\r");
+            printf("\tdump\t\t\tList all variables available and their current status\n\r");
+            printf("\tset\t[VAR]\t[VALUE]\tSet value to a variable in the list\n\r");
+            printf("\thelp\t\t\tShow this help menu\n\r");
         } else {
-            printf("Command \"%s\" not recognized! Please use the command \"help\" to check the CLI usage\n", cli_cmd);
+            printf("Command \"%s\" not recognized! Please use the command \"help\" to check the CLI usage\n\r", cli_cmd);
         }
     }
 }
@@ -402,18 +460,82 @@ void bsmp_hook_signal_threads(enum bsmp_operation op, struct bsmp_var **list)
 
     for (i = 0; list[i] != NULL; i++) {
         var = list[i];
-        if (var->info.id == 0) {
-            // Attenuator value changed
+        /* Special cases */
+        switch( var->info.id ) {
+        case 0:
+            /* Attenuators */
             Attenuators_thread.signal_set(0x01);
+            break;
+        case 8:
+            /* Reset */
+            printf("Resetting MBED...\n\r");
+            mbed_reset();
+            break;
+        case 18:
+            /* IP Address */
+            printf("Updating IP address on FeRAM to %s ...\n\r", IP_Addr);
+            feram.set_ip_addr(IP_Addr);
+            break;
+        case 19:
+            /* MAC Address */
+            printf("Updating MAC address on FeRAM to %s ...\n\r", MAC_Addr);
+            feram.set_mac_addr(MAC_Addr);
+            break;
+        case 20:
+            /* Gateway Address */
+            printf("Updating Gateway address on FeRAM to %s ...\n\r", Gateway_Addr);
+            feram.set_gateway_addr(Gateway_Addr);
+            break;
+        case 21:
+            /* Mask Address */
+            printf("Updating Mask address on FeRAM to %s ...\n\r", Mask_Addr);
+            feram.set_mask_addr(Mask_Addr);
+            break;
         }
     }
 }
 
+void bsmp_dispatcher( void )
+{
+    struct bsmp_raw_packet mock_response;
+
+    while(1) {
+        /*  Wait for a new message */
+        osEvent evt = bsmp_mail_box.get();
+
+        if (evt.status != osEventMail) {
+            /* Quietly ignore errors for now */
+            continue;
+        }
+
+        bsmp_mail_t *mail = (bsmp_mail_t*)evt.value.p;
+
+        mock_response.data = (uint8_t *) malloc(sizeof(uint8_t)*30);
+
+        /* Proccess BSMP request */
+        bsmp_process_packet(bsmp, &mail->msg, &mock_response);
+
+        /* Only respond if there's a valid mailbox to put the response in */
+        if (mail->response_mail_box) {
+            struct bsmp_raw_packet *response = mail->response_mail_box->alloc();
+            response->data = mock_response.data;
+            response->len = mock_response.len;
+            mail->response_mail_box->put(response);
+        }
+
+        bsmp_mail_box.free(mail);
+    }
+}
+
 int main( void )
 {
+    wdt.clear_overflow_flag();
+
     //Init serial port for info printf
     pc.baud(115200);
 
+    printf("Starting RFFEuC firmware "FW_VERSION" !\n\n\r");
+
     bsmp = bsmp_server_new();
 
     bsmp_register_hook(bsmp, bsmp_hook_signal_threads);
@@ -457,17 +579,16 @@ int main( void )
     //PID_BD tauI parameter
     set_value(PID_BD_tauD, 2);
 
-    printf("Getting ETH configuration from FeRAM...\n\r");
     feram.get_ip_addr(IP_Addr);
-    feram.get_mac_addr(MAC_Addr);
     feram.get_gateway_addr(Gateway_Addr);
     feram.get_mask_addr(Mask_Addr);
-
-    printf("Values from FeRAM:\n\r");
-    printf("IP : %s\n\r", IP_Addr);
-    printf("Mask : %s\n\r", Mask_Addr);
-    printf("Gateway : %s\n\r", Gateway_Addr);
-    printf("MAC : %s\n\n\r", MAC_Addr);
+    feram.get_mac_addr(MAC_Addr, mac_buffer);
+    printf("Ethernet configuration from FeRAM:\n\r");
+    printf("\tIP : %s\n\r", IP_Addr);
+    printf("\tMask : %s\n\r", Mask_Addr);
+    printf("\tGateway : %s\n\r", Gateway_Addr);
+    printf("\tMAC : %s\n\r", MAC_Addr);
+    printf("\n\r");
 
     for ( uint8_t i = 0; i < sizeof(rffe_vars)/sizeof(rffe_vars[0]); i++) {
         rffe_vars[i].info.id = i;
@@ -478,12 +599,11 @@ int main( void )
     Attenuators_thread.start(Attenuators_Control);
     Temp_Control_thread.start(Temp_Feedback_Control);
     CLI_Proccess_Thread.start(CLI_Proccess);
+    BSMP_Thread.start(bsmp_dispatcher);
 
     // Instantiate our command processor for the  USB serial line.
     scMake(&pc, commandCallback, NULL);
 
-    printf("\nRFFE Firmware Version: %s\n", FW_VERSION);
-
     IAP iap;
     uint8_t *fw_buffer;
     uint8_t state = 0;
@@ -491,10 +611,11 @@ int main( void )
     bool full_page = false;
     uint8_t v_major = 0, v_minor = 0, v_patch = 0;
 
-    struct bsmp_raw_packet request;
-    struct bsmp_raw_packet response;
     uint8_t buf[BUFSIZE];
-    uint8_t bufresponse[BUFSIZE];
+
+    led4 = !pll.cfg_eth();
+
+    Thread::wait(100);
 
     // Ethernet initialization
     EthernetInterface net;
@@ -515,29 +636,29 @@ int main( void )
 #endif
 
     while (true) {
-        printf("Trying to bring up ethernet connection...\n");
+        printf("Trying to bring up ethernet connection...\n\r");
         while (net.connect() != 0) {
-            printf("Attempt failed. Trying again in 0.5s... \n");
+            printf("Attempt failed. Trying again in 0.5s... \n\r");
             Thread::wait(500);
         }
-        printf("Success! RFFE eth server is up!\n");
+        printf("Success! RFFE eth server is up!\n\r");
 
-        printf("RFFE IP: %s\n", IP_Addr);
-        printf("RFFE MAC Address: %s\n", MAC_Addr);
+        printf("RFFE IP: %s\n\r", IP_Addr);
+        printf("RFFE MAC Address: %s\n\r", MAC_Addr);
 
-        printf("Listening on port %d...\n", SERVER_PORT);
+        printf("Listening on port %d...\n\r", SERVER_PORT);
 
         server.open(&net);
         server.bind(net.get_ip_address(), SERVER_PORT);
         server.listen();
 
         while (true) {
-            printf(" Waiting for new client connection...\n");
+            printf("Waiting for new client connection...\n\r");
 
             server.accept(&client, &client_addr);
             client.set_blocking(1500);
 
-            printf("Connection from client: %s\n", client_addr.get_ip_address());
+            printf("Connection from client: %s\n\r", client_addr.get_ip_address());
 
             while ( get_eth_link_status() ) {
 
@@ -566,27 +687,39 @@ int main( void )
                 for (int i = 0; i < recv_sz; i++) {
                     printf("0x%X ",buf[i]);
                 }
-                printf("\n");
+                printf("\n\r");
 #endif
-                request.data = buf;
-                request.len = recv_sz;
+                bsmp_mail_t *mail = bsmp_mail_box.alloc();
+
+                mail->response_mail_box = &eth_mail_box;
+                mail->msg.data = buf;
+                mail->msg.len = recv_sz;
+
+                bsmp_mail_box.put(mail);
 
-                response.data = bufresponse;
+                osEvent evt = eth_mail_box.get();
+
+                if (evt.status != osEventMail) {
+                    /* Quietly ignore errors for now */
+                    continue;
+                }
 
-                bsmp_process_packet(bsmp, &request, &response);
+                struct bsmp_raw_packet *response_mail = (struct bsmp_raw_packet *)evt.value.p;
 
-                sent_sz = client.send((char*)response.data, response.len);
+                sent_sz = client.send((char*)response_mail->data, response_mail->len);
 
 #ifdef DEBUG_PRINTF
                 printf("Sending message of %d bytes: ", sent_sz);
                 for (int i = 0; i < sent_sz; i++) {
-                    printf("0x%X ",response.data[i]);
+                    printf("0x%X ",response_mail->data[i]);
                 }
-                printf("\n");
+                printf("\n\r");
 #endif
+                free(response_mail->data);
+                eth_mail_box.free(response_mail);
 
                 if (sent_sz <= 0) {
-                    printf("ERROR while writing to socket!\n");
+                    printf("ERROR while writing to socket!\n\r");
                     continue;
                 }
 
@@ -645,13 +778,13 @@ int main( void )
                 }
 
                 if (get_value8(Reset) == 1) {
-                    printf("Resetting MBED!\n");
+                    printf("Resetting MBED!\n\r");
                     mbed_reset();
                 }
             }
 
             client.close();
-            printf("Client Disconnected!\n");
+            printf("Client Disconnected!\n\r");
 
             if (get_eth_link_status() == 0) {
                 /* Eth link is down, clean-up server connection */
diff --git a/src/watchdog.h b/src/watchdog.h
new file mode 100644
index 0000000..e3abcfa
--- /dev/null
+++ b/src/watchdog.h
@@ -0,0 +1,32 @@
+#ifndef WATCHDOG_H
+#define WATCHDOG_H
+
+#include "mbed.h"
+
+// Based on Simon's Watchdog code from
+// http://mbed.org/forum/mbed/topic/508/
+
+class Watchdog {
+public:
+// Load timeout value in watchdog timer and enable
+    void kick(float s) {
+        LPC_WDT->WDCLKSEL = 0x1;                // Set CLK src to PCLK
+        uint32_t clk = SystemCoreClock / 16;    // WD has a fixed /4 prescaler, PCLK default is /4
+        LPC_WDT->WDTC = s * (float)clk;
+        LPC_WDT->WDMOD = 0x3;                   // Enabled and Reset
+        kick();
+    }
+// "kick" or "feed" the dog - reset the watchdog timer
+// by writing this required bit pattern
+    void kick() {
+        LPC_WDT->WDFEED = 0xAA;
+        LPC_WDT->WDFEED = 0x55;
+    }
+    int check_overflow_flag() {
+        return (LPC_WDT->WDMOD >> 2) & 1;
+    }
+    void clear_overflow_flag() {
+        LPC_WDT->WDMOD &= ~(1 << 2);
+    }
+};
+#endif