Merge remote-tracking branch 'upstream/master'

MYSBootloader/MYSBootloader.c

@@ -1,17 +1,15 @@
 /*
  MYSBootloader: OTA bootloader for Mysensor nodes (www.mysensors.org)
- Code based on OTA bootloader by ToSa
+ Original OTA bootloader code by ToSa
  Optimized and extended by tekka
- Version 1.0 / 20150206
+ Version 1.1 / 20150314
- Size: 1834 bytes
+ Size: 2002 bytes
  
- Tested with Atmega328P, 16Mhz 3.3V and 5V and MYSController 0.1.2.266 (goo.gl/9DCWNo)
+ Tested with MYSController 0.1.2.276 (goo.gl/9DCWNo)
  
- MCU: atmega328
+ MCU: Atmega328p
- clock: 16Mhz (prescaler: off/1)
  bootsz: 1024W
  
-
  fuses for ISP:
  EX = 0xFE (use 0x06 for Arduino IDE, boards.txt)
  HI = 0xDA
@@ -23,9 +21,20 @@
  
  Successfully tested with:
  
- 16Mhz extXTAL, 3.3V
+ 16Mhz extXTAL, 3.3V & 5V
- 1Mhz intRC (8Mhz + DIV8), 3.3V
+ 8Mhz intRC, 3.3V & 5V
- 128kHz intRC, 3.3V
+ 1Mhz intRC, 3.3 & 5V
+ 128kHz intRC, 3.3V & 5V
+ 
+ * Version 1.1
+ - use eeprom_update instead of eeprom_write to reduce wear out
+ - bootloader commands: erase eeprom, set node id
+ - verify incoming FW blocks for type & address
+ - communicate over static parent (if set and found) else broadcast to find nearest node
+ - adjusted timings 
+ 
+ * Version 1.0
+ Initial release
  
  This program is free software; you can redistribute it and/or
  modify it under the terms of the GNU General Public License
@@ -36,45 +45,63 @@
 
 
 #define MYSBOOTLOADER_MAJVER 1
-#define MYSBOOTLOADER_MINVER 0
+#define MYSBOOTLOADER_MINVER 1
 
 #define MYSBOOTLOADER_VERSION (MYSBOOTLOADER_MINVER * 256 + MYSBOOTLOADER_MAJVER)
 
-//#define USE_PRESCALER
 #define MAX_RESEND 5
 
 
 // procedures and functions
 
-int main(void) __attribute__ ((OS_main)) __attribute__ ((section (".init9")));
+static void programPage(uint16_t page, uint8_t *buf);
 static uint16_t calcCRCrom (const void* ptr, uint16_t len);
 static uint8_t IsFirmwareValid();
 static void reboot();
 static void startup();
-static void programPage(uint32_t page, uint8_t *buf);
 static boolean sendWrite(MyMessage message);
 static bool sendAndWait(uint8_t reqType, uint8_t resType);
 
+int main(void) __attribute__ ((OS_main)) __attribute__ ((section (".init9")));
+
+
+static void programPage(uint16_t page, uint8_t *buf) {
+	// these function calls use "out" commands: save some bytes and cycles :)
+	__boot_page_erase_short(page);
+	boot_spm_busy_wait();
+	for (uint16_t i = 0; i < SPM_PAGESIZE; i += 2) {
+		uint16_t data_word = *buf++;
+		data_word += (*buf++) << 8;
+		__boot_page_fill_short(page + i, data_word);
+	}
+	__boot_page_write_short(page);
+	boot_spm_busy_wait();
+	__boot_rww_enable_short();
+}
+
 
 static uint16_t calcCRCrom (const void* ptr, uint16_t len) {
 	// init 0xFFFF
 	uint16_t crc = ~0;
-	for (uint16_t i = 0; i < len; ++i) {
+	for (uint16_t i = 0; i < len; i++) {
 		crc = _crc16_update(crc, pgm_read_byte((uint16_t) ptr + i));
 	}
 	return crc;
 }
 
+
 static uint8_t IsFirmwareValid () {
 	return calcCRCrom(0, fc.blocks * FIRMWARE_BLOCK_SIZE) == fc.crc;
 }
 
+
+
 static void reboot() {
-	// any pending eeprom activities?
+	// wait for pending eeprom activities
 	eeprom_busy_wait();
 	// trigger watchdog ASAP
 	watchdogConfig(WATCHDOG_16MS);
-	// wait until WD triggers
+	// endless loop
 	while (1);
 }
 
@@ -82,13 +109,7 @@ static void startup() {
 	if (IsFirmwareValid()) {
 		// WD off
 		watchdogConfig(WATCHDOG_OFF);
-		
+		// run sketch
-		#ifdef USE_PRESCALER
-		// reset prescaler
-		clock_prescale_set(orgClockDiv);
-		#endif
-		
-		// start sketch
 		((void(*)()) 0)();
 				
 	} else {
@@ -96,20 +117,7 @@ static void startup() {
 	}
 }
 
-static void programPage(uint32_t page, uint8_t *buf) {
+
-	// using out commands, saves some bytes :)
-	__boot_page_erase_short(page);
-	boot_spm_busy_wait();	
-	for (uint16_t i = 0; i < SPM_PAGESIZE; i += 2) {
-		uint16_t w = *buf++;
-		// generate word
-		w += (*buf++) << 8;
-		__boot_page_fill_short(page + i, w);
-	}
-	__boot_page_write_short(page);
-	boot_spm_busy_wait();
-	__boot_rww_enable_short();
-}
 
 
 static boolean sendWrite(MyMessage message) {
@@ -121,38 +129,45 @@ static bool sendAndWait(uint8_t reqType, uint8_t resType) {
 	// outer loop, retries
 	for (uint8_t i = 0; i < MAX_RESEND; i++) {
 		sendWrite(outMsg);
-		// loop 20 times, wait time 0.1ms if no/wrong data => 2s
+		// loop 20 times, wait time 0.1s if no/wrong data => 2s
 		for (uint8_t j = 0; j < 20; j++) {
 			// loop 100 times, wait 1ms if no/wrong data => 0.1s
 			for (uint8_t k = 0; k < 100; k++) {
 				watchdogReset();
 				// Tx FIFO data available? (we don't care about the pipe here)
 				if (available(NULL)) {
-					// read message from FIFO
+					// read message from FIFO, skip if size = 0
-					readMessage(inMsg.array);
+					if (readMessage(inMsg.array) > 0) {
-					// protocol compatible? if not ignore msg
+						// protocol compatible? if not ignore msg
-					if ((mGetVersion(inMsg) != PROTOCOL_VERSION)) {
+						if ((mGetVersion(inMsg) != PROTOCOL_VERSION)) {
-						continue;
+							continue;
-					}
+						}
-					if (inMsg.destination == nc.nodeId) {
+						// msg for us?
-						// msg for us
+						if (inMsg.destination == nc.nodeId) {
-						if ((mGetCommand(inMsg) == C_INTERNAL) && (inMsg.type == I_FIND_PARENT_RESPONSE)) {
+							// internal command: find parent
-							if (inMsg.bValue < nc.distance - 1) {
+							if ((mGetCommand(inMsg) == C_INTERNAL) && (inMsg.type == I_FIND_PARENT_RESPONSE)) {
-								// got new routing info, update settings
+								// static parent found?
-								nc.distance = inMsg.bValue + 1;
+								if (configuredParentID == inMsg.sender) {
-								nc.parentNodeId = inMsg.sender;
+									configuredParentFound = true;
+								}
+								if ( ((inMsg.bValue < nc.distance - 1) && ( !configuredParentFound) ) || (configuredParentID == inMsg.sender)) {
+									// got new routing info, update settings
+									nc.distance = inMsg.bValue + 1;
+									nc.parentNodeId = inMsg.sender;
+								}
+							}
+							// did we receive expected reply?
+							if ((mGetCommand(inMsg) == mGetCommand(outMsg)) && (inMsg.type == resType)) {
+								return true;
 							}
-						}
-						// did we receive expected reply?
-						if ((mGetCommand(inMsg) == mGetCommand(outMsg)) && (inMsg.type == resType)) {
-							return true;
-						}
 						
+						}
 					}
-				} else {
+				} else { 
 					// wait 1ms if no data available
-					delaym(10);
+					_delay_ms(1);
 				}
+				
 			}
 		}
 	}
@@ -169,13 +184,6 @@ int main(void) {
 	// reset MCU status register	
 	MCUSR = 0;
 	
-	#ifdef USE_PRESCALER
-	// get current prescale
-	orgClockDiv = clock_prescale_get();
-	//switch to 4 MHz on 16Mhz crystal
-	clock_prescale_set(F_CPU_DIV);
-	#endif	
-		
 	// enable watchdog to avoid deadlock
 	watchdogConfig(WATCHDOG_8S);
 
@@ -190,11 +198,10 @@ int main(void) {
 	// Read firmware config from EEPROM, i.e. type, version, CRC, blocks
 	eeprom_read_block((void*)&fc, (void*)EEPROM_FIRMWARE_TYPE_ADDRESS, sizeof(NodeFirmwareConfig));
 	
-	// bootloader should find closest node during each reboot
+	// find nearest node during reboot: invalidate parent node settings, since we have to re-discover them for every single reboot
-
+	configuredParentID = nc.parentNodeId;
-	// invalidate parent node settings, since we have to re-discover them for every single reboot
+	// nc.parentNodeId = 0xFF;
 	nc.distance = 0xFF;
-	nc.parentNodeId = 0xFF;
 	
 	// prepare for I_FIND_PARENTS
 	outMsg.sender = nc.nodeId;
@@ -202,6 +209,7 @@ int main(void) {
 	outMsg.sensor = 0xFF;
 	outMsg.destination = BROADCAST_ADDRESS;
 	
+	// set header
 	mSetVersion(outMsg, PROTOCOL_VERSION);
 	mSetLength(outMsg, 0);
 	mSetCommand(outMsg, C_INTERNAL);
@@ -211,7 +219,7 @@ int main(void) {
 	// set reading & writing pipe address
 	setAddress(nc.nodeId);
 
-	// network up?, get neighbors, else startup
+	// network up? get neighbors, else startup
 	if (!sendAndWait(I_FIND_PARENT, I_FIND_PARENT_RESPONSE)) {
 		startup();
 	}
@@ -225,9 +233,9 @@ int main(void) {
 		openReadingPipe(CURRENT_NODE_PIPE, TO_ADDR(BROADCAST_ADDRESS));
 		if (sendAndWait(I_ID_REQUEST, I_ID_RESPONSE)) {
 			// save id to eeprom
-			eeprom_write_byte((uint8_t*)EEPROM_NODE_ID_ADDRESS, atoi(inMsg.data));
+			eeprom_update_byte((uint8_t*)EEPROM_NODE_ID_ADDRESS, atoi(inMsg.data));
 		}
-		// we could go on and set everything right here, but rebooting will take care of that - saves bytes :)
+		// we could go on and set everything right here, but rebooting will take care of that - and saves some bytes :)
 		reboot();				
 	}
 	
@@ -250,6 +258,23 @@ int main(void) {
 	
 	NodeFirmwareConfig *firmwareConfigResponse = (NodeFirmwareConfig *)inMsg.data;
 	
+	// bootloader commands
+	if (firmwareConfigResponse->blocks == 0) {
+		// verify flag
+		if (firmwareConfigResponse->crc == 0xDA7A){
+			// cmd 0x01 clear eeprom
+			if(firmwareConfigResponse->bl_command == 0x01) {
+				for(uint16_t i = 0; i < EEPROM_SIZE; i++) eeprom_update_byte((uint8_t *)i,0xFF);
+			} else 
+			// cmd 0x02 set id
+			if(firmwareConfigResponse->bl_command == 0x02) {
+				eeprom_update_byte((uint8_t*)EEPROM_NODE_ID_ADDRESS, (uint8_t)firmwareConfigResponse->bl_data);
+			}
+		}
+		// final step
+		reboot();
+	}
+	
 	// compare with current node configuration, if equal startup
 	if (!memcmp(&fc,firmwareConfigResponse,sizeof(NodeFirmwareConfig))) {
 		startup();
@@ -260,7 +285,7 @@ int main(void) {
 	// invalidate current CRC
 	fc.crc = 0xFFFF;
 	// write fetched type and version in case OTA fails (BL will reboot and re-request FW with stored settings)
-	eeprom_write_block(&fc, (void*)EEPROM_FIRMWARE_TYPE_ADDRESS,sizeof(NodeFirmwareConfig));
+	eeprom_update_block(&fc, (void*)EEPROM_FIRMWARE_TYPE_ADDRESS,sizeof(NodeFirmwareConfig));
 	
 	// copy new FW config
 	memcpy(&fc,firmwareConfigResponse,sizeof(NodeFirmwareConfig));
@@ -270,33 +295,39 @@ int main(void) {
 	firmwareRequest->type = fc.type;
 	firmwareRequest->version = fc.version;
 	
-	// request FW from controller
+	// request FW from controller, load FW counting backwards
-	for (uint16_t block = fc.blocks; block > 0; block--) {
+	uint16_t block = fc.blocks;
-		firmwareRequest->block = (block - 1);
+	do {
+		firmwareRequest->block = block - 1;
+		
 		// request FW block
 		if (!sendAndWait(ST_FIRMWARE_REQUEST, ST_FIRMWARE_RESPONSE)) {
 			reboot();
 		}
 		
 		ReplyFWBlock *firmwareResponse = (ReplyFWBlock *)inMsg.data;
-		// calculate page offset
+		
-		uint8_t offset = ((block - 1) * FIRMWARE_BLOCK_SIZE) % SPM_PAGESIZE;
+		// did we receive requested block?
-		// write to buffer
+		if (!memcmp(firmwareRequest,firmwareResponse,sizeof(RequestFWBlock))) {
-		memcpy(progBuf + offset, firmwareResponse->data, FIRMWARE_BLOCK_SIZE);
+			// calculate page offset
-		// program if page full
+			uint8_t offset = ((block - 1) * FIRMWARE_BLOCK_SIZE) % SPM_PAGESIZE;
-		if (offset == 0) {
+			// write to buffer
-			programPage(((block - 1) * FIRMWARE_BLOCK_SIZE), progBuf);
+			memcpy(progBuf + offset, firmwareResponse->data, FIRMWARE_BLOCK_SIZE);
-		}
+			// program if page full
-	}
+			if (offset == 0) {
-
+				programPage(((block - 1) * FIRMWARE_BLOCK_SIZE), progBuf);
+			}
+			block--;	
+		}	
+	} while (block);
+	
 	// wuff
 	watchdogReset();
 	
 	// all blocks transmitted, calc CRC and write to eeprom if valid	
 	if (IsFirmwareValid()) {
 		// if FW is valid, write settings to eeprom 
-		eeprom_write_block((void*)&fc, (void*)EEPROM_FIRMWARE_TYPE_ADDRESS, sizeof(NodeFirmwareConfig));
+		eeprom_update_block(&fc, (void*)EEPROM_FIRMWARE_TYPE_ADDRESS, sizeof(NodeFirmwareConfig));
-		// we could restart directly here, but eeprom may still be busy, thus reboot (we check for idle eeprom before rebooting) and save some bytes :)
 	} 
 	// final step
 	reboot();

MYSBootloader/MYSBootloader.h

@@ -5,6 +5,7 @@
 #include <stdint.h>
 #include <string.h>
 #include <util/crc16.h>
+#include <util/delay.h>
 
 #include "MyMessage.h"
 #include "MySensor.h"
@@ -13,11 +14,17 @@
 
 
 #define FIRMWARE_BLOCK_SIZE	16
+#define EEPROM_SIZE 1024			// 1024 bytes for ATMEGA328
 
-// FW config structure, stored in eeprom
 typedef struct {
-	uint16_t type;
+	union {
-	uint16_t version;
+		uint16_t type;
+		uint16_t bl_command;
+	};
+	union {
+		uint16_t version;
+		uint16_t bl_data
+	};
 	uint16_t blocks;
 	uint16_t crc;
 } __attribute__((packed)) NodeFirmwareConfig;
@@ -48,9 +55,10 @@ static NodeFirmwareConfig fc;
 static MyMessage outMsg;
 static MyMessage inMsg;
 
-#ifdef PRESCALE
+static boolean configuredParentFound = false;
-static clock_div_t orgClockDiv = 0;
+static uint8_t configuredParentID = 0xFF;
-#endif
+
+//static clock_div_t orgClockDiv = 0;
 
 static uint8_t progBuf[SPM_PAGESIZE];
 

MYSBootloader/MYSBootloader.hex

@@ -1,117 +1,128 @@
-:1078000021E0A0E0B1E001C01D92A53DB207E1F783
+:1078000011E0A0E0B1E0E0EDFFE702C005900D92CD
-:10781000CDB7DEB76897DEBFCDBF112414BE88E1B7
+:10781000A230B107D9F721E0A2E0B1E001C01D928A
-:107820008093600089E2809360008FEE84B963E00A
+:10782000A83DB207E1F7CDB7DEB76897DEBFCDBFA1
-:1078300083E201D26FE584E2FED16BE381E2FBD10A
+:10783000112414BE88E18093600089E28093600087
-:107840006CE485E2F8D167E286E2F5D10DD263E718
+:107840008FEE84B963E083E2D9D16FE584E2D6D1CB
-:1078500080E5F1D166E08DE3EED16FE38CE3EBD10F
+:107850006BE381E2D3D16CE485E2D0D167E286E2CA
-:107860006EEF80E2E8D185E053D143E050E060E084
+:10786000CDD12ED263E780E5C9D166E08DE3C6D1E4
-:1078700070E08AEC91E032D348E050E06BE171E0D7
+:107870006FE38CE3C3D16EEF80E2C0D18FE19EE471
-:1078800082EC91E02BD38FEF8093CC018093CB01DE
+:107880000197F1F700C0000043E050E060E070E0D5
-:107890002091CA012093A2012093A1018093A70106
+:107890008DEC91E06CD348E050E06BE171E085EC59
-:1078A0008093A30182E08093A4018091A501887058
+:1078A00091E065D38091CE01809300018FEF8093AA
-:1078B00083608093A5012F3FD1F130E040E050E09C
+:1078B000CF012091CD012093A5012093A4018093B5
-:1078C00060E070E080E090E034504E4157456745FD
+:1078C000AA018093A60182E08093A7018091A8017C
-:1078D0007F4F8F4F9F4F29013A014B015C01298B4C
+:1078D000887083608093A8012F3FD1F130E040E0B1
-:1078E0005A8A4B8B7C8A6D8B9E8A8F8BB88E85E0F3
+:1078E00050E060E070E080E090E034504E41574559
-:1078F000FE017196ADECB1E001900D928A95E1F731
+:1078F00067457F4F8F4F9F4F29013A014B015C0134
-:1079000045E0BE016F5E7F4F8AE27FD149865A868D
+:10790000298B5A8A4B8B7C8A6D8B9E8A8F8BB88E83
-:107910006B867C868D869E86AF86B88A45E0BE01E2
+:1079100085E0FE017196A0EDB1E001900D928A958F
-:10792000675F7F4F8BE271D163E082E284D168E0D0
+:10792000E1F745E0BE016F5E7F4F8AE255D149869F
-:1079300087E09FD1811101C049D11092A3018091AC
+:107930005A866B867C868D869E86AF86B88A45E0A1
-:10794000CA018F3FF1F489838CEF8A8381EE8B83A8
+:10794000BE01675F7F4F8BE247D163E082E256D191
-:1079500088EA8C838D831E821F82188645E0BE01D3
+:1079500068E087E0BAD1811101C09CD11092A601E4
-:107960006F5F7F4F8BE251D164E083E082D1882347
+:107960008091CD018F3FF1F489838CEF8A8381EE82
-:1079700039F087E891E080D2682F80E090E0C7D2AC
+:107970008B8388EA8C838D831E821F82188645E064
-:107980001CD1A8958091A401877080658093A40183
+:10798000BE016F5F7F4F8BE227D164E083E09DD122
-:107990008091A5018871846C8093A50188E0E2EC58
+:10799000882339F08AE891E0B8D2682F80E090E03F
-:1079A000F1E0A8EAB1E001900D928A95E1F781E05B
+:1079A00005D36FD1A8958091A701877080658093DA
-:1079B00090E09093B1018093B00161E080E059D1F3
+:1079B000A7018091A8018871846C8093A80188E058
-:1079C000811101C003D148E050E067E871E082EC2A
+:1079C000E5ECF1E0ABEAB1E001900D928A95E1F7C8
-:1079D00091E06FD2892B09F4F9D08FEF9FEF90934C
+:1079D00081E091E09093B4018093B30161E080E095
-:1079E000C9018093C80148E050E06BE171E082EC8E
+:1079E00074D1811101C056D180918E0190918F0187
-:1079F00091E084D288E0E7E8F1E0A2ECB1E0019008
+:1079F000892B09F580919001909191018A379A4DD8
-:107A00000D928A95E1F78091A4018770806380933D
+:107A000081F680918A0190918B018130910569F412
-:107A1000A4018091C2019091C3019093A901809328
+:107A1000A12CB12C6FEFC501C9D2FFEFAF1ABF0A7D
-:107A2000A8018091C4019091C5019093AB0180930E
+:107A2000A11424E0B206B1F7BCCF029709F0B9CF98
-:107A3000AA01A090C601B090C70185010150104F66
+:107A300060918C01B3CF48E050E06AE871E085ECDA
-:107A400064E0000F111F6A95E1F770E1972E88241A
+:107A400091E080D2892B09F425D18FEF9FEF90939D
-:107A50008394E5E07E2EF1E16F2EA114B10409F4C8
+:107A5000CC018093CB0148E050E06BE171E085EC14
-:107A60004AC081E0A81AB108B092AD01A092AC0161
+:107A600091E095D288E0EAE8F1E0A5ECB1E0019080
-:107A700063E082E0FED0882309F482CF802F8077F4
+:107A70000D928A95E1F78091A701877080638093CA
-:107A8000A82FB0E0A050BF4FEDE8F1E0992D019094
+:107A8000A7018091C5019091C6019093AC018093AC
-:107A90000D929A95E1F781112BC083E0F80187BF21
+:107A9000AB018091C7019091C8019093AE01809392
-:107AA000E89507B600FCFDCF4091000180E090E032
+:107AA000AD01A090C901B090CA0110E101E0E5E08C
-:107AB000DC01A050BF4FFC01E00FF11F11962C918B
+:107AB0007E2EF1E16F2E450181E0881A91089092A7
-:107AC000119730E0322F2227240F311D090187BE84
+:107AC000B0018092AF0163E082E0FFD0882309F427
-:107AD000E895112412964C9102968038910541F751
+:107AD00068CF46E050E06AE871E08BEA91E032D28C
-:107AE000F80177BEE89507B600FCFDCF67BEE895C4
+:107AE000892B09F045C091E0A91A90EFB90A8A2DB7
-:107AF00000511109B2CFA8952CD0882309F440CFAA
+:107AF00082958077A82FB0E0AE5FBE4FE0E9F1E05D
-:107B000048E050E06BE171E082EC91E0F7D138CFD2
+:107B0000912F01900D929A95E1F7811130C074E0A8
-:107B100090E0981751F02DE730E000002150310936
+:107B1000AA0CBB1C7A95E1F783E0F50187BFE895D5
-:107B200021153105D1F79F5FF4CF089598E087FFC5
+:107B200007B600FCFDCF4091020180E090E0DC014F
-:107B300002C02B9A01C02B9800002D9A000023B19F
+:107B3000AE5FBE4FFC01EA0DFB1D11962C91119713
-:107B4000880F22952170822B2D980000915079F793
+:107B400030E0322F2227240F311D090107BFE895AD
-:107B500008958091C6019091C70124E0880F991F74
+:107B5000112412964C9102968038910541F7F50157
-:107B60002A95E1F7E0E0F0E02FEF3FEFE817F907A3
+:107B600077BEE89507B600FCFDCF67BEE8955401E7
-:107B7000D1F044912427422F42954227042E469566
+:107B7000A114B10409F09FCFA8954FD0882309F430
-:107B800046954025042E469540254770022E232F0A
+:107B800010CF48E050E06BE171E085EC91E0FFD16F
-:107B9000469507944795302D242706944795302520
+:107B900008CF98E087FF02C02B9A01C02B982D9A3E
-:107BA00024273196E3CF81E04091C8015091C9016B
+:107BA00023B1880F22952170822B2D98915091F747
-:107BB0002417350709F080E00895F999FECF88E190
+:107BB00008951F93CF93DF93EB01142F2A98E9DFE9
-:107BC0008093600088E080936000FFCFC2DF88234D
+:107BC0001C0F1C1721F08FEFE4DF8993FACF2A9A5C
-:107BD00041F088E18093600010926000E0E0F0E006
+:107BD000DF91CF911F9108951F93CF93DF93EB0116
-:107BE0000994EBDF1F93CF93DF93EB01142F2A98B7
+:107BE000142F2A98D6DF1C0F1C1719F08991D1DFAA
-:107BF0009DDF1C0F1C1721F08FEF98DF8993FACFC0
+:107BF000FBCF2A9ADF91CF911F910895CF93DF9306
-:107C00002A9ADF91CF911F9108951F93CF93DF930D
+:107C00001F92CDB7DEB7698341E0BE016F5F7F4F42
-:107C1000EB01142F2A988ADF1C0F1C1719F0899189
+:107C1000E3DF0F90DF91CF9108958091C90190919A
-:107C200085DFFBCF2A9ADF91CF911F91089541E024
+:107C2000CA0124E0880F991F2A95E1F7E0E0F0E00F
-:107C300060E070E0D7CFCF93DF931F92CDB7DEB770
+:107C30002FEF3FEFE817F907D1F044912427422FA7
-:107C4000698341E0BE016F5F7F4FDFDF0F90DF91FF
+:107C400042954227042E469546954025042E46959A
-:107C5000CF91089540E060E070E082EED6CF40E042
+:107C500040254770022E232F469507944795302DD7
-:107C600060E070E081EED1CFF5DFF9DF60E787E219
+:107C6000242706944795302524273196E3CF81E0D9
-:107C7000E2CF6F927F928F929F92AF92BF92CF92FC
+:107C70004091CB015091CC012417350709F080E0E9
-:107C8000DF92EF92FF920F931F93CF93DF93CDB7C5
+:107C80000895F999FECF88E18093600088E08093A1
-:107C9000DEB7AA97DEBFCDBF762E8093A60105E0A2
+:107C90006000FFCFC2DF882341F088E1809360005D
-:107CA00041E2642EE1EAF1E0DE011196862D0190B9
+:107CA00010926000E0E0F0E00994EBDF40E060E07B
-:107CB0000D928A95E1F7FB801C8116951695169515
+:107CB00070E082EE91CF40E060E070E081EE8CCF2A
-:107CC000195F2091CB0129982AA7CEDF6EEF80E2C1
+:107CC000F5DFF9DF60E787E299CF6F927F928F92BD
-:107CD000B2DF2AA530E040E050E060E070E080E0F4
+:107CD0009F92AF92BF92CF92DF92EF92FF920F935B
-:107CE00090E034504E41574567457F4F8F4F9F4F2F
+:107CE0001F93CF93DF93CDB7DEB7AA97DEBFCDBF8B
-:107CF0002AA33BA34CA35DA36EA37FA388A799A748
+:107CF000762E8093A90105E051E2652EE4EAF1E0D9
-:107D000045E0BE016E5D7F4F8AE27FDF45E0BE0148
+:107D0000DE011196862D01900D928A95E1F7FB8098
-:107D10006E5D7F4F80E379DF8FEFF81202C080EB5A
+:107D10001C81169516951695195F2091CE0129980C
-:107D200001C080EA412FBE016F5F7F4F6EDF299A4D
+:107D20002AA7CEDF6EEF80E269DF2AA530E040E0CF
-:107D300081E0EEDE299840E060E070E08FEF52DFF6
+:107D300050E060E070E080E090E034504E41574504
-:107D40008073C9F360E787E276DF84FD88DF6FE048
+:107D400067457F4F8F4F9F4F2AA33BA34CA35DA353
-:107D500080E271DF89DF8090CD019090CE01A0900C
+:107D50006EA37FA388A799A745E0BE016E5D7F4F04
-:107D6000CF01B090D001C090D101D090D201E0906D
+:107D60008AE23ADF45E0BE016E5D7F4F80E334DF9B
-:107D7000D301F090D4019401A501B601C701A0E0A0
+:107D70008FEFF81202C080EB01C080EA412FBE01F4
-:107D80006FD029F045E06DEC71E08AE23EDF299A80
+:107D80006F5F7F4F29DF299A9AE69A95F1F700C035
-:107D900014E134E6F32EA89587E149DF80FD4BC05E
+:107D9000299840E060E070E08FEF0BDF8073C9F35B
-:107DA00080E645DF813210F055DF80E0482F60E843
+:107DA00060E787E22BDF84FD86DF6FE080E226DF7D
-:107DB00071E081E617DF60E787E23DDF80918301B4
+:107DB00087DF8090D0019090D101A090D201B09047
-:107DC0008770823059F5909182018091CA01981391
+:107DC000D301C090D401D090D501E090D601F090BD
-:107DD00025C040918401842F87708330B1F4809155
+:107DD000D7019401A501B601C701A0E08AD029F01E
-:107DE0008501883091F450918701252F30E08091F2
+:107DE00045E060ED71E08AE2F7DE299A44E1E42E95
-:107DF000CC0190E00197281739073CF45F5F50935E
+:107DF00034E6F32EA89541E060E070E087E1D9DE3B
-:107E0000CC01809181018093CB018091A5018427D1
+:107E000080FD5EC041E060E070E080E6D2DE182FC9
-:107E1000877021F480918501871559F0FA9409F053
+:107E1000813210F04BDF10E0412F63E871E081E622
-:107E2000BACF115009F0B5CF015009F03BCF80E037
+:107E2000C8DE60E787E2EADE1123E9F1809186018E
-:107E300005C081E003C08AE06BDEF0CFAA96DEBF0A
+:107E300087708230C1F5909185018091CD019813B2
-:107E4000CDBFDF91CF911F910F91FF90EF90DF9009
+:107E400032C050918701852F8770833019F580915A
-:107E5000CF90BF90AF909F908F907F906F9008953C
+:107E500088018830F9F47091000140918401741315
-:107E60000024A7FD00942A17300540055005600541
+:107E600003C081E08093820160918A01262F30E077
-:107E70007005800590050895FC0188279927E894EE
+:107E70008091CF0190E001972817390724F4809171
-:107E800021912032E9F3293010F02E30C8F32B3243
+:107E80008201882311F0741305C06F5F6093CF01E6
-:107E900039F02D3231F4689403C018D0820F911D4F
+:107E90004093CE018091A8018527877021F48091BD
-:107EA000219120532A30C8F31EF4909581959F4F5D
+:107EA0008801871559F0FA9409F0A4CFEA9409F0F3
-:107EB0000895FB01DC0104C08D910190801921F42B
+:107EB0009FCF015009F022CF80E009C081E007C0C8
-:107EC00041505040C8F7881B990B08957AE0979F5E
+:107EC0008FE99FE00197F1F700C00000ECCFAA9680
-:107ED000902D879F802D910D11240895DC01CB01F9
+:107ED000DEBFCDBFDF91CF911F910F91FF90EF904B
-:107EE000FC01F999FECF06C0F2BDE1BDF89A3196CA
+:107EE000DF90CF90BF90AF909F908F907F906F90DA
-:107EF00000B40D9241505040B8F70895DC01CB0119
+:107EF00008950024A7FD00942A1730054005500579
-:107F000002C02D9105D041505040D8F70895262F3A
+:107F000060057005800590050895FC018827992774
-:107F1000F999FECF1FBA92BD81BD20BD0FB6F8946E
+:107F1000E89421912032E9F3293010F02E30C8F393
-:0A7F2000FA9AF99A0FBE019608952F
+:107F20002B3239F02D3231F4689403C018D0820F0F
+:107F3000911D219120532A30C8F31EF4909581950C
+:107F40009F4F0895FB01DC0104C08D9101908019C1
+:107F500021F441505040C8F7881B990B08957AE0EE
+:107F6000979F902D879F802D910D11240895DC01FE
+:107F7000CB01FC01F999FECF06C0F2BDE1BDF89A34
+:107F8000319600B40D9241505040B8F70895DC018D
+:107F9000A40FB51F4150504040F0CB01840F951FF6
+:107FA0002E9105D041505040D8F70895262FF999C9
+:107FB000FECF92BD81BDF89A019700B4021639F048
+:107FC0001FBA20BD0FB6F894FA9AF99A0FBE089519
+:027FD000FF00B0
 :040000030000780081
 :00000001FF

MYSBootloader/MYSBootloaderHW.h

@@ -5,18 +5,10 @@
 
 // hardware
 
-
-
-#ifdef USE_PRESCALER
 #define F_CPU_DIV	clock_div_4
-#else
+#define F_CPU_REAL	F_CPU / (F_CPU_DIV)
-#define F_CPU_DIV	clock_div_1
+#define BAUD_SETTING (( (F_CPU_REAL + BAUD_RATE * 4L) / ((BAUD_RATE * 8L))) - 1 )
-#endif
+#define BAUD_ACTUAL (F_CPU_REAL/(8 * ((BAUD_SETTING)+1)))
-
-#define F_CPU_REAL	F_CPU / (1 << F_CPU_DIV)
-#define DELAY_M		125 * (1 << F_CPU_DIV)
-#define BAUD_SETTING (( (F_CPU + BAUD_RATE * 4L) / ((BAUD_RATE * 8L))) - 1 )
-#define BAUD_ACTUAL (F_CPU/(8 * ((BAUD_SETTING)+1)))
 #define BAUD_ERROR (( 100*(BAUD_RATE - BAUD_ACTUAL) ) / BAUD_RATE)
 #define UART_SRA UCSR0A
 #define UART_SRB UCSR0B
@@ -56,20 +48,6 @@ void watchdogConfig(uint8_t x) {
 	WDTCSR = x;
 }
 
-// delay procedures
-
-inline static void delayu() {
-	asm volatile ("nop");
-}
-
-static void delaym(uint8_t value) {
-	for (uint8_t outer = 0; outer < value; outer++) {
-		for (uint16_t inner = 0; inner < DELAY_M; inner++) {
-				delayu();	
-		}
-	}			
-}
-
 // SPI communication
 
 #define SPI_DDR		DDRB
@@ -80,10 +58,12 @@ static void delaym(uint8_t value) {
 #define	SPI_MOSI	3		// Arduino Pin 11 <-> Bit 3 of port B
 #define	SPI_CSN		2		// Arduino Pin 10 <-> Bit 2 of port B
 #define	SPI_CE		1		// Arduino Pin  9 <-> Bit 1 of port B
-#define csnlow() SPI_PORT &= ~_BV(SPI_CSN)//;delaym(1)			// not necessary
+#define CE_PULSE_LENGTH	20	// IMPORTANT: minimum CE pulse width 10us, see nRF24L01 specs. Set 20us to be on the safe side
-#define csnhigh() SPI_PORT |= _BV(SPI_CSN)//;delaym(1)			// not necessary
+
-#define celow() SPI_PORT &= ~_BV(SPI_CE)//;delaym(1)			// not necessary
+#define csnlow() SPI_PORT &= ~_BV(SPI_CSN)
-#define cehigh() SPI_PORT |= _BV(SPI_CE)//;delaym(1)			// minimum puls width 10us
+#define csnhigh() SPI_PORT |= _BV(SPI_CSN)
+#define celow() SPI_PORT &= ~_BV(SPI_CE)
+#define cehigh() SPI_PORT |= _BV(SPI_CE)
 
 static void SPIinit() {
 	// set pin mode: MOSI,SCLK,CE = OUTPUT, MISO = INPUT
@@ -91,15 +71,16 @@ static void SPIinit() {
 }
 
 static uint8_t SPItransfer(uint8_t value) {
-	for(uint8_t i = 0 ;i < 8;i++) {
+	// bit bang SPI
+	for(uint8_t i = 0; i<8; i++) {
 		if (value & 0x80) SPI_PORT |= _BV(SPI_MOSI); else SPI_PORT &= ~_BV(SPI_MOSI);
-		delayu();
+		//_delay_us(1);
 		value <<= 1;
 		SPI_PORT |= _BV(SPI_SCLK);
-		delayu();
+		//_delay_us(1);
 		value |= ((SPI_PIN >> SPI_MISO) & 0x01);
 		SPI_PORT &= ~_BV(SPI_SCLK);
-		delayu();
+		//_delay_us(1);
 	}
 	return value;	
 }

MYSBootloader/MYSBootloaderRF24.h

@@ -9,12 +9,11 @@ typedef enum { RF24_1MBPS = 0, RF24_2MBPS, RF24_250KBPS } rf24_datarate_e;
 
 #define _write_register(reg) ( (reg) | W_REGISTER)
 #define _read_register(reg) ( (reg) | R_REGISTER)
-
 #define addr_width 5
+
 uint64_t pipe0_reading_address = 0;
 
-
+static uint8_t BurstReadAddress(uint8_t addr, uint8_t* buf, uint8_t len) {
-uint8_t BurstReadAddress(uint8_t addr, uint8_t* buf, uint8_t len) {
 	csnlow();
 	uint8_t status = SPItransfer( addr );
 	while ( len-- ) {
@@ -25,7 +24,7 @@ uint8_t BurstReadAddress(uint8_t addr, uint8_t* buf, uint8_t len) {
 	return status;
 }
 
-uint8_t BurstWriteAddress(uint8_t addr, uint8_t* buf, uint8_t len) {
+static uint8_t BurstWriteAddress(uint8_t addr, uint8_t* buf, uint8_t len) {
 	csnlow();
 	uint8_t status = SPItransfer( addr );
 	while ( len-- ) {
@@ -35,30 +34,30 @@ uint8_t BurstWriteAddress(uint8_t addr, uint8_t* buf, uint8_t len) {
 	return status;
 }
 
-uint8_t ReadAddress(uint8_t addr) {
+static uint8_t ReadAddress(uint8_t addr) {
 	return BurstReadAddress(addr,NULL,1);
 }
 
 
-uint8_t WriteAddress(uint8_t addr, uint8_t val) {
+static uint8_t WriteAddress(uint8_t addr, uint8_t val) {
 	return BurstWriteAddress(addr,&val,1);
 }
 
-#define BurstWriteRegister(reg,buf,len) BurstWriteAddress(_write_register(reg),buf,len)
+#define BurstWriteRegister(reg,buf,len) BurstWriteAddress(_write_register(reg), buf, len)
-#define WriteRegister(reg,val) WriteAddress(_write_register(reg),val)
+#define WriteRegister(reg,val) WriteAddress(_write_register(reg), val)
-#define BurstReadRegister(reg,buf,len) BurstReadAddress(_read_register(reg),buf,len)
+#define BurstReadRegister(reg,buf,len) BurstReadAddress(_read_register(reg), buf, len)
 #define ReadRegister(reg) ReadAddress(_read_register(reg))
-#define get_status() BurstReadAddress(NOP,NULL,0)
+#define get_status() BurstReadAddress(NOP, NULL, 0)
 
 void Flush_RX(void) {
-	BurstWriteAddress(FLUSH_RX,NULL,0);
+	BurstWriteAddress(FLUSH_RX, NULL, 0);
 }
 
 void Flush_TX(void) {
-	BurstWriteAddress(FLUSH_TX,NULL,0);
+	BurstWriteAddress(FLUSH_TX, NULL, 0);
 }
 
-void Flush_RXTX_CLI(void) {
+static void Flush_RXTX_CLI(void) {
 	// flush RX and TX buffer
 	Flush_RX();
 	Flush_TX();
@@ -86,18 +85,18 @@ static void stopListening(void) {
 
 
 
-static bool write_buf(uint8_t* buf, uint8_t len, const bool multicast ) {
+static boolean write_buf(uint8_t* buf, uint8_t len, const bool multicast ) {
 	// write payload to FIFO
 	BurstWriteRegister( multicast ? W_TX_PAYLOAD_NO_ACK : W_TX_PAYLOAD, buf, len) ;
 	// CE pulse to start transmission
 	cehigh();
 	// IMPORTANT: minimum CE pulse width 10us, see nRF24L01 specs
-	delaym(1);
+	_delay_us(CE_PULSE_LENGTH);
 	// start transmitting
 	celow();
 	
 
-	// wait until sent, here we potentially have a deadlock when transmitter not connected/not working
+	// wait until sent or ACKed, here we potentially have a deadlock when transmitter not connected/not working => wdt recovers
 	while( ! ( get_status()  & ( _BV(TX_DS) | _BV(MAX_RT) ))) { }
 	// read interrupts and clear
 	uint8_t status = WriteRegister(STATUS,_BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT) );
@@ -119,7 +118,7 @@ static uint8_t getDynamicPayloadSize(void) {
 }
 
 
-static bool available(uint8_t* pipe_num) {
+static boolean available(uint8_t* pipe_num) {
 	if (!(ReadAddress(FIFO_STATUS) & _BV(RX_EMPTY) )){
 		if (pipe_num) {
 			uint8_t status = get_status();
@@ -131,11 +130,13 @@ static bool available(uint8_t* pipe_num) {
 }
 
 
-static void readMessage(uint8_t* buf) {
+static uint8_t readMessage(uint8_t* buf) {
 	// read payload
-	BurstReadRegister(R_RX_PAYLOAD,buf,getDynamicPayloadSize());
+	uint8_t PL_LEN = getDynamicPayloadSize();
+	BurstReadRegister(R_RX_PAYLOAD,buf,PL_LEN);
 	// reset interrupts
-	WriteRegister(STATUS,_BV(RX_DR) | _BV(MAX_RT) | _BV(TX_DS));
+	WriteRegister(STATUS,_BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT) );
+	return PL_LEN;
 }
 
 static void openWritingPipe(uint64_t value) {
@@ -162,7 +163,7 @@ static void openReadingPipe(uint8_t pipe, uint64_t address) {
 static boolean write(uint8_t destination, uint8_t* buf, uint8_t len, boolean multicast) {
 	stopListening();
 	openWritingPipe(TO_ADDR(destination));
-	bool result = write_buf(buf, len, multicast);
+	boolean result = write_buf(buf, len, multicast);
 	startListening();
 	return result;
 }
@@ -177,6 +178,8 @@ static void setAddress(uint8_t addr)
 	}
 }
 
+
+
 static void RFinit(void){
 	// set address width
 	WriteRegister(SETUP_AW, (addr_width-2) % 4);
@@ -190,7 +193,7 @@ static void RFinit(void){
 	WriteRegister(RF_SETUP, ((RF24_DATARATE & 0b00000010 ) << 4) | ((RF24_DATARATE & 0b00000001 ) << 3) | ((RF24_PA_LEVEL << 1) + 1));
 	// flush RX and TX FIFO, clear interrupts
 	Flush_RXTX_CLI();
-	// activate dynamic payload feature
+	// activate to unlock features
 	WriteAddress(ACTIVATE,0x73);
 	// enable payload with ACK and dynamic payload length
 	WriteRegister(FEATURE, _BV(EN_ACK_PAY) | _BV(EN_DPL) );
@@ -198,13 +201,8 @@ static void RFinit(void){
 	WriteRegister(DYNPD, _BV(DPL_P5) | _BV(DPL_P4) | _BV(DPL_P3) | _BV(DPL_P2) | _BV(DPL_P1) | _BV(DPL_P0));
 	//powerUp, enable 16bit CRC, no RX mode
 	WriteRegister(CONFIG, _BV(PWR_UP) | _BV(CRCO) | _BV(EN_CRC) | ~_BV(PRIM_RX));
-	// stabilize
+	// stabilize 5ms
-	delaym(5);
+	_delay_ms(5);
-	
-	// We are not listening to broadcast pipe since we do not reply to broadcasting messages (e.g. FIND_PARENT_RESPONSE messages)
-	// openReadingPipe(BROADCAST_PIPE, TO_ADDR(BROADCAST_ADDRESS));
-
-
 }
 
 #endif // MYSBootloaderRF24_H

MYSBootloader/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_ */

libraries/MySensors/MyMQTT.cpp

@@ -13,7 +13,7 @@ version 2 as published by the Free Software Foundation.
 #include "utility/MsTimer2.h"
 
 char V_0[] PROGMEM = "TEMP";		//V_TEMP
-char V_1[] PROGMEM = "HUM";		//V_HUM
+char V_1[] PROGMEM = "HUM";			//V_HUM
 char V_2[] PROGMEM = "LIGHT";		//V_LIGHT
 char V_3[] PROGMEM = "DIMMER";		//V_DIMMER
 char V_4[] PROGMEM = "PRESSURE";	//V_PRESSURE
@@ -23,7 +23,7 @@ char V_7[] PROGMEM = "RAINRATE";	//V_RAINRATE
 char V_8[] PROGMEM = "WIND";		//V_WIND
 char V_9[] PROGMEM = "GUST";		//V_GUST
 char V_10[] PROGMEM = "DIRECTON";	//V_DIRECTON
-char V_11[] PROGMEM = "UV";		//V_UV
+char V_11[] PROGMEM = "UV";			//V_UV
 char V_12[] PROGMEM = "WEIGHT";		//V_WEIGHT
 char V_13[] PROGMEM = "DISTANCE";	//V_DISTANCE
 char V_14[] PROGMEM = "IMPEDANCE";	//V_IMPEDANCE
@@ -35,20 +35,20 @@ char V_19[] PROGMEM = "SCENE_ON";	//V_SCENE_ON
 char V_20[] PROGMEM = "SCENE_OFF";	//V_SCENE_OFF
 char V_21[] PROGMEM = "HEATER";		//V_HEATER
 char V_22[] PROGMEM = "HEATER_SW";	//V_HEATER_SW
-char V_23[] PROGMEM = "LIGHT_LEVEL";	//V_LIGHT_LEVEL
+char V_23[] PROGMEM = "LIGHT_LEVEL";//V_LIGHT_LEVEL
 char V_24[] PROGMEM = "VAR1";		//V_VAR1
 char V_25[] PROGMEM = "VAR2";		//V_VAR2
 char V_26[] PROGMEM = "VAR3";		//V_VAR3
 char V_27[] PROGMEM = "VAR4";		//V_VAR4
 char V_28[] PROGMEM = "VAR5";		//V_VAR5
-char V_29[] PROGMEM = "UP";		//V_UP
+char V_29[] PROGMEM = "UP";			//V_UP
 char V_30[] PROGMEM = "DOWN";		//V_DOWN
 char V_31[] PROGMEM = "STOP";		//V_STOP
 char V_32[] PROGMEM = "IR_SEND";	//V_IR_SEND
 char V_33[] PROGMEM = "IR_RECEIVE";	//V_IR_RECEIVE
 char V_34[] PROGMEM = "FLOW";		//V_FLOW
 char V_35[] PROGMEM = "VOLUME";		//V_VOLUME
-char V_36[] PROGMEM = "LOCK_STATUS";	//V_LOCK_STATUS
+char V_36[] PROGMEM = "LOCK_STATUS";//V_LOCK_STATUS
 char V_37[] PROGMEM = "DUST_LEVEL";	//V_DUST_LEVEL
 char V_38[] PROGMEM = "VOLTAGE";	//V_VOLTAGE
 char V_39[] PROGMEM = "CURRENT";	//V_CURRENT
@@ -72,10 +72,10 @@ char V_56[] PROGMEM = "";		//
 char V_57[] PROGMEM = "";		//
 char V_58[] PROGMEM = "";		//
 char V_59[] PROGMEM = "";		//
-char V_60[] PROGMEM = "Started!\n";	//Custom for MQTTGateway
+char V_60[] PROGMEM = "Started!\n";		//Custom for MQTTGateway
 char V_61[] PROGMEM = "SKETCH_NAME";	//Custom for MQTTGateway
 char V_62[] PROGMEM = "SKETCH_VERSION"; //Custom for MQTTGateway
-char V_63[] PROGMEM = "UNKNOWN"; 	//Custom for MQTTGateway
+char V_63[] PROGMEM = "UNKNOWN"; 		//Custom for MQTTGateway
 
 //////////////////////////////////////////////////////////////////
 
@@ -89,7 +89,6 @@ PROGMEM const char *vType[] = {
 	V_61, V_62, V_63
 };
 
-
 char broker[] PROGMEM = MQTT_BROKER_PREFIX;
 
 #define S_FIRSTCUSTOM 60
@@ -132,7 +131,7 @@ void MyMQTT::begin(rf24_pa_dbm_e paLevel, uint8_t channel, rf24_datarate_e dataR
 	Serial.begin(BAUD_RATE);
 	repeaterMode = true;
 	isGateway = true;
-	MQTTClientConnected = false;
+	MQTTClients = 0;
 
 	setupRepeaterMode();
 	dataCallback = inDataCallback;
@@ -163,77 +162,120 @@ void MyMQTT::processRadioMessage() {
 	if (process()) {
 		// A new message was received from one of the sensors
 		MyMessage message = getLastMessage();
-		// Pass along the message from sensors to serial line
 		rxBlink(1);
-		SendMQTT(message);
-	}
 
+		if (msg.isAck()) {
+			Serial.println("msg is ack!");
+			if (msg.sender == 255 && mGetCommand(msg) == C_INTERNAL && msg.type == I_ID_REQUEST) {
+				// TODO: sending ACK request on id_response fucks node up. doesn't work.
+				// The idea was to confirm id and save to EEPROM_LATEST_NODE_ADDRESS.
+			}
+		} else {
+			// we have to check every message if its a newly assigned id or not.
+			// Ack on I_ID_RESPONSE does not work, and checking on C_PRESENTATION isn't reliable.
+			uint8_t newNodeID = loadState(EEPROM_LATEST_NODE_ADDRESS)+1;
+			if (newNodeID <= MQTT_FIRST_SENSORID) {
+				newNodeID = MQTT_FIRST_SENSORID;
+			}
+			if (msg.sender == newNodeID) {
+				saveState(EEPROM_LATEST_NODE_ADDRESS,newNodeID);
+			}
+
+			if (mGetCommand(msg) == C_INTERNAL) {
+				if (msg.type == I_CONFIG) {
+					txBlink(1);
+					if (!sendRoute(build(msg, GATEWAY_ADDRESS, msg.sender, 255, C_INTERNAL, I_CONFIG, 0).set("M"))) {
+						errBlink(1);
+					}
+				} else if (msg.type == I_ID_REQUEST && msg.sender == 255) {
+					uint8_t newNodeID = loadState(EEPROM_LATEST_NODE_ADDRESS)+1;
+					if (newNodeID <= MQTT_FIRST_SENSORID) {
+						newNodeID = MQTT_FIRST_SENSORID;
+					}
+					if (newNodeID >= MQTT_LAST_SENSORID) {
+						// Sorry no more id's left :(
+						newNodeID = AUTO;
+					}
+					txBlink(1);
+					if (!sendRoute(build(msg, GATEWAY_ADDRESS, msg.sender, 255, C_INTERNAL, I_ID_RESPONSE, 0).set(newNodeID))) {
+						errBlink(1);
+					}
+				}
+			} else if (mGetCommand(msg)!= C_PRESENTATION) {
+				// Pass along the message from sensors to MQTT
+				SendMQTT(message);
+			}
+		}
+	}
 }
 
 void MyMQTT::processMQTTMessage(char *inputString, uint8_t inputPos) {
-	char *str, *p;
+	char *str, *p, *payload=NULL;
 	uint8_t i = 0;
 	buffer[0]= 0;
 	buffsize = 0;
+	uint8_t mqttMsgType = (uint8_t)inputString[0] >> 4;
 
-	if ((uint8_t)inputString[0] >> 4 == MQTTCONNECT) {
+	if (mqttMsgType == MQTTCONNECT) {
 		buffer[buffsize++] = MQTTCONNACK << 4;
 		buffer[buffsize++] = 0x02;			// Remaining length
 		buffer[buffsize++] = 0x00;			// Connection accepted
 		buffer[buffsize++] = 0x00;			// Reserved
-		MQTTClientConnected=true;			// We have connection!
+		MQTTClients++;						// We have a new client connected!
-	}
+	} else if (mqttMsgType == MQTTPINGREQ) {
-	if ((uint8_t)inputString[0] >> 4 == MQTTPINGREQ) {
 		buffer[buffsize++] = MQTTPINGRESP << 4;
 		buffer[buffsize++] = 0x00;
-	}
+	} else if (mqttMsgType == MQTTSUBSCRIBE) {
-	if ((uint8_t)inputString[0] >> 4 == MQTTSUBSCRIBE) {
+		buffer[buffsize++] = MQTTSUBACK << 4;			// Just ack everything, we actually dont really care!
-		buffer[buffsize++] = MQTTSUBACK << 4;		// Just ack everything, we actually dont really care!
+		buffer[buffsize++] = 0x03;						// Remaining length
-		buffer[buffsize++] = 0x03;			// Remaining length
 		buffer[buffsize++] = (uint8_t)inputString[2];	// Message ID MSB
 		buffer[buffsize++] = (uint8_t)inputString[3];	// Message ID LSB
-		buffer[buffsize++] = MQTTQOS0;			// QOS level
+		buffer[buffsize++] = MQTTQOS0;					// QOS level
-	}
+	} else if (mqttMsgType == MQTTUNSUBSCRIBE) {
-	if ((uint8_t)inputString[0] >> 4 == MQTTUNSUBSCRIBE) {
 		buffer[buffsize++] = MQTTUNSUBACK << 4;
-		buffer[buffsize++] = 0x02;			// Remaining length
+		buffer[buffsize++] = 0x02;						// Remaining length
 		buffer[buffsize++] = (uint8_t)inputString[2];	// Message ID MSB
 		buffer[buffsize++] = (uint8_t)inputString[3];	// Message ID LSB
+	} else if (mqttMsgType == MQTTDISCONNECT) {
+		MQTTClients--;		// Client disconnected!
 	}
-	if ((uint8_t)inputString[0] >> 4 == MQTTDISCONNECT) {
+
-		MQTTClientConnected=false;			// We lost connection!
-	}
 	if (buffsize > 0) {
-		dataCallback(buffer,&buffsize);
+		dataCallback(buffer, &buffsize);
 	}
 
 	// We publish everything we get, we dont care if its subscribed or not!
-	if ((uint8_t)inputString[0] >> 4 == MQTTPUBLISH || (MQTT_SEND_SUBSCRIPTION && (uint8_t)inputString[0] >> 4 == MQTTSUBSCRIBE)) {
+	if (mqttMsgType == MQTTPUBLISH || (MQTT_SEND_SUBSCRIPTION && mqttMsgType == MQTTSUBSCRIBE)) {
-		buffer[0]= 0;
+		buffer[0] = 0;
 		buffsize = 0;
 		// Cut out address and payload depending on message type.
-		if ((uint8_t)inputString[0] >> 4 == MQTTSUBSCRIBE) {
+		if (mqttMsgType == MQTTSUBSCRIBE) {
-			strncat(buffer,inputString+6,inputString[5]);
+			strncat(buffer, inputString+6, inputString[5]);
 		} else {
-			strncat(buffer,inputString+4,inputString[3]);
+			strncat(buffer, inputString+4, inputString[3]);
 		}
 
 		// TODO: Check if we should send ack or not.
-		for (str = strtok_r(buffer,"/",&p) ; str && i<4 ; str = strtok_r(NULL,"/",&p)) {
+		for (str = strtok_r(buffer, "/", &p) ; str && i<4 ; str = strtok_r(NULL, "/", &p)) {
 			if (i == 0) {
-				if (strcmp_P(str,broker)!=0) {	//look for MQTT_BROKER_PREFIX
+				//look for MQTT_BROKER_PREFIX
-					return;			//Message not for us or malformatted!
+				if (strcmp_P(str, broker) != 0) {
+					//Message not for us or malformatted!
+					return;
 				}
 			} else if (i==1) {
-				msg.destination = atoi(str);	//NodeID
+				//NodeID
+				msg.destination = atoi(str);
 			} else if (i==2) {
-				msg.sensor = atoi(str);		//SensorID
+				//SensorID
+				msg.sensor = atoi(str);
 			} else if (i==3) {
-				char match=0;			//SensorType
+				//SensorType
-				//strcpy(str,(char*)&str[2]);  //Strip V_
+				char match=0;
 
 				for (uint8_t j=0; strcpy_P(convBuf, (char*)pgm_read_word(&(vType[j]))) ; j++) {
-					if (strcmp((char*)&str[2],convBuf)==0) { //Strip V_ and compare
+					//Strip V_ and compare
+					if (strcmp((char*)&str[2], convBuf)==0) {
 						match=j;
 						break;
 					}
@@ -245,70 +287,58 @@ void MyMQTT::processMQTTMessage(char *inputString, uint8_t inputPos) {
 				msg.type = match;
 			}
 			i++;
-		}						//Check if packge has payload
-		if ((uint8_t)inputString[1] > (uint8_t)(inputString[3]+2) && !((uint8_t)inputString[0] >> 4 == MQTTSUBSCRIBE)) {
-			strcpy(convBuf,inputString+(inputString[3]+4));
-			msg.set(convBuf);			//Payload
-		} else {
-			msg.set("");				//No payload
 		}
+
+		// Check if package has payload
+		if (mqttMsgType == MQTTPUBLISH) {
+			uint8_t length = (uint8_t)inputString[1] - (uint8_t)(inputString[3]+2);
+			if (length && length < MAX_PAYLOAD*2) {
+				// Payload
+				memcpy(convBuf, inputString+(inputString[3]+4), length);
+				convBuf[length] = 0;
+				payload = convBuf;
+			}
+		}
+		msg.set(payload);
+
 		txBlink(1);
 		if (!sendRoute(build(msg, GATEWAY_ADDRESS, msg.destination, msg.sensor, C_SET, msg.type, 0))) errBlink(1);
-
 	}
 }
 
 void MyMQTT::SendMQTT(MyMessage &msg) {
 	buffsize = 0;
-	if (!MQTTClientConnected) return;			//We have no connections - return
+	if (!MQTTClients) {
-	if (msg.isAck()) {
+		//We have no clients connected - return
-		Serial.println("msg is ack!");
+		return;
-		if (msg.sender==255 && mGetCommand(msg)==C_INTERNAL && msg.type==I_ID_REQUEST) {
-			// TODO: sending ACK request on id_response fucks node up. doesn't work.
-			// The idea was to confirm id and save to EEPROM_LATEST_NODE_ADDRESS.
-		}
-	} else {
-		// we have to check every message if its a newly assigned id or not.
-		// Ack on I_ID_RESPONSE does not work, and checking on C_PRESENTATION isn't reliable.
-		uint8_t newNodeID = loadState(EEPROM_LATEST_NODE_ADDRESS)+1;
-		if (newNodeID <= MQTT_FIRST_SENSORID) newNodeID = MQTT_FIRST_SENSORID;
-		if (msg.sender==newNodeID) {
-			saveState(EEPROM_LATEST_NODE_ADDRESS,newNodeID);
-		}
-		if (mGetCommand(msg)==C_INTERNAL) {
-			if (msg.type==I_CONFIG) {
-				txBlink(1);
-				if (!sendRoute(build(msg, GATEWAY_ADDRESS, msg.sender, 255, C_INTERNAL, I_CONFIG, 0).set("M"))) errBlink(1);
-			} else if (msg.type==I_ID_REQUEST && msg.sender==255) {
-				uint8_t newNodeID = loadState(EEPROM_LATEST_NODE_ADDRESS)+1;
-				if (newNodeID <= MQTT_FIRST_SENSORID) newNodeID = MQTT_FIRST_SENSORID;
-				if (newNodeID >= MQTT_LAST_SENSORID) return; // Sorry no more id's left :(
-				txBlink(1);
-				if (!sendRoute(build(msg, GATEWAY_ADDRESS, msg.sender, 255, C_INTERNAL, I_ID_RESPONSE, 0).set(newNodeID))) errBlink(1);
-			}
-		} else if (mGetCommand(msg)!=0) {
-			if (mGetCommand(msg)==3) msg.type=msg.type+(S_FIRSTCUSTOM-10);	//Special message
-
-			buffer[buffsize++] = MQTTPUBLISH << 4;	// 0:
-			buffer[buffsize++] = 0x09;		// 1: Remaining length with no payload, we'll set this later to correct value, buffsize -2
-			buffer[buffsize++] = 0x00;		// 2: Length MSB (Remaing length can never exceed ff,so MSB must be 0!)
-			buffer[buffsize++] = 0x08;		// 3: Length LSB (ADDR), We'll set this later
-			if (msg.type > V_TOTAL) msg.type=V_TOTAL;// If type > defined types set to unknown.
-			strcpy_P(buffer+4, broker);
-			buffsize+=strlen_P(broker);
-			buffsize+=sprintf(&buffer[buffsize],"/%i/%i/V_%s",msg.sender,msg.sensor,getType(convBuf, &vType[msg.type]));
-			buffer[3]=buffsize-4;			// Set correct address length on byte 4.
-#ifdef DEBUG
-			Serial.println((char*)&buffer[4]);
-#endif
-			msg.getString(convBuf);
-			for (uint8_t a=0; a<strlen(convBuf); a++) {// Payload
-				buffer[buffsize++] = convBuf[a];
-			}
-			buffer[1]=buffsize-2;			// Set correct Remaining length on byte 2.
-			dataCallback(buffer,&buffsize);
-		}
 	}
+
+	if (mGetCommand(msg) == C_INTERNAL) {
+		//Special message
+		msg.type = msg.type+(S_FIRSTCUSTOM-10);
+	}
+
+	buffer[buffsize++] = MQTTPUBLISH << 4;	// 0:
+	buffer[buffsize++] = 0x09;		// 1: Remaining length with no payload, we'll set this later to correct value, buffsize -2
+	buffer[buffsize++] = 0x00;		// 2: Length MSB (Remaing length can never exceed ff,so MSB must be 0!)
+	buffer[buffsize++] = 0x08;		// 3: Length LSB (ADDR), We'll set this later
+	if (msg.type > V_TOTAL) {
+		// If type > defined types set to unknown.
+		msg.type=V_TOTAL;
+	}
+	strcpy_P(buffer+4, broker);
+	buffsize+=strlen_P(broker);
+	buffsize+=sprintf(&buffer[buffsize], "/%i/%i/V_%s", msg.sender, msg.sensor, getType(convBuf, &vType[msg.type]));
+	buffer[3]=buffsize-4;			// Set correct address length on byte 4.
+#ifdef DEBUG
+	Serial.println((char*)&buffer[4]);
+#endif
+	msg.getString(convBuf);
+	for (uint8_t a=0; a<strlen(convBuf); a++) {	// Payload
+		buffer[buffsize++] = convBuf[a];
+	}
+	buffer[1]=buffsize-2;			// Set correct Remaining length on byte 2.
+	dataCallback(buffer, &buffsize);
 }
 
 void MyMQTT::rxBlink(uint8_t cnt) {
@@ -322,5 +352,3 @@ void MyMQTT::txBlink(uint8_t cnt) {
 void MyMQTT::errBlink(uint8_t cnt) {
 	if(countErr == 255) { countErr = cnt; }
 }
-
-

libraries/MySensors/MyMQTT.h

@@ -71,7 +71,7 @@ private:
 	void txBlink(uint8_t cnt);
 	void errBlink(uint8_t cnt);
 
-	bool MQTTClientConnected;
+	uint8_t MQTTClients;
 	char buffer[MQTT_MAX_PACKET_SIZE];
 	char convBuf[MAX_PAYLOAD*2+1];
 	uint8_t buffsize;

libraries/MySensors/examples/MQTTGateway/MQTTGateway.ino

@@ -56,19 +56,20 @@ http://forum.mysensors.org/topic/303/mqtt-broker-gateway
 
 */
 
+#include <DigitalIO.h>
 #include <SPI.h>
 #include <MySensor.h>
 #include <MyMQTT.h>
 #include <Ethernet.h>
 
-
 // * Use this for IBOARD modded to use standard MISO/MOSI/SCK, see note *1 above!
 /*
 #define RADIO_CE_PIN        3			// radio chip enable
 #define RADIO_SPI_SS_PIN    8			// radio SPI serial select
 #define RADIO_ERROR_LED_PIN A2  		// Error led pin
 #define RADIO_RX_LED_PIN    A1  		// Receive led pin
-#define RADIO_TX_LED_PIN    A0  		// the PCB, on board LED*/
+#define RADIO_TX_LED_PIN    A0  		// the PCB, on board LED
+*/
 
 // * Use this for default configured pro mini / nano etc :
 ///*
@@ -80,50 +81,71 @@ http://forum.mysensors.org/topic/303/mqtt-broker-gateway
 
 #define TCP_PORT 1883						// Set your MQTT Broker Listening port.
 IPAddress TCP_IP ( 192, 168, 0, 234 );				// Configure your static ip-address here
-uint8_t TCP_MAC[] = { 0x02, 0xDE, 0xAD, 0x00, 0x00, 0x42 };	// Mac-address - You should change this! see note *2 above!
+byte TCP_MAC[] = { 0x02, 0xDE, 0xAD, 0x00, 0x00, 0x42 };	// Mac-address - You should change this! see note *2 above!
 
 //////////////////////////////////////////////////////////////////
 
 EthernetServer server = EthernetServer(TCP_PORT);
+EthernetClient *currentClient = NULL;
 MyMQTT gw(RADIO_CE_PIN, RADIO_SPI_SS_PIN);
 
 void processEthernetMessages() {
-	char inputString[MQTT_MAX_PACKET_SIZE] = "";
+  char inputString[MQTT_MAX_PACKET_SIZE] = "";
-	uint8_t inputSize = 0;
+  byte inputSize = 0;
-	EthernetClient client = server.available();
+  byte readCnt = 0;
-	if (client) {
+  byte length = 0;
-		while (client.available()) {
+
-			char inChar = client.read();
+  EthernetClient client = server.available();
-			inputString[inputSize] = inChar;
+  if (client) {
-			inputSize++;
+    while (client.available()) {
-		}
+      // Save the current client we are talking with
+      currentClient = &client;
+      byte inByte = client.read();
+      readCnt++;
+
+      if (inputSize < MQTT_MAX_PACKET_SIZE) {
+        inputString[inputSize] = (char)inByte;
+        inputSize++;
+      }
+
+      if (readCnt == 2) {
+        length = (inByte & 127) * 1;
+      }
+      if (readCnt == (length+2)) {
+        break;
+      }
+    }
 #ifdef TCPDUMP
-		Serial.print("<<");
+    Serial.print("<<");
-		char buf[4];
+    char buf[4];
-		for (uint8_t a=0; a<inputSize; a++) { sprintf(buf,"%02X ", (uint8_t)inputString[a]); Serial.print(buf); } Serial.println("");
+    for (byte a=0; a<inputSize; a++) { sprintf(buf, "%02X ", (byte)inputString[a]); Serial.print(buf); } Serial.println();
 #endif
-		gw.processMQTTMessage(inputString, inputSize);
+    gw.processMQTTMessage(inputString, inputSize);
-	}
+    currentClient = NULL;
+  }
 }
 
-void writeEthernet(const char *writeBuffer, uint8_t *writeSize) {
+void writeEthernet(const char *writeBuffer, byte *writeSize) {
 #ifdef TCPDUMP
-	Serial.print(">>");
+  Serial.print(">>");
-	char buf[4];
+  char buf[4];
-	for (uint8_t a=0; a<*writeSize; a++) { sprintf(buf,"%02X ",(uint8_t)writeBuffer[a]);  Serial.print(buf); } Serial.println("");
+  for (byte a=0; a<*writeSize; a++) { sprintf(buf,"%02X ",(byte)writeBuffer[a]); Serial.print(buf); } Serial.println();
 #endif
-	server.write((const uint8_t *)writeBuffer, *writeSize);
+  // Check whether to respond to a single client or write to all
+  if (currentClient != NULL)
+    currentClient->write((const byte *)writeBuffer, *writeSize);
+  else
+    server.write((const byte *)writeBuffer, *writeSize);
 }
 
-
 int main(void) {
-	init();
+  init();
-	Ethernet.begin(TCP_MAC, TCP_IP);
+  Ethernet.begin(TCP_MAC, TCP_IP);
-	delay(1000);   // Wait for Ethernet to get configured.
+  delay(1000);   // Wait for Ethernet to get configured.
-	gw.begin(RF24_PA_LEVEL_GW, RF24_CHANNEL, RF24_DATARATE, writeEthernet, RADIO_RX_LED_PIN, RADIO_TX_LED_PIN, RADIO_ERROR_LED_PIN);
+  gw.begin(RF24_PA_LEVEL_GW, RF24_CHANNEL, RF24_DATARATE, writeEthernet, RADIO_RX_LED_PIN, RADIO_TX_LED_PIN, RADIO_ERROR_LED_PIN);
-	server.begin();
+  server.begin();
-	while (1) {
+  while (1) {
-		processEthernetMessages();
+    processEthernetMessages();
-		gw.processRadioMessage();
+    gw.processRadioMessage();
-	}
+  }
 }