Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/boards.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/boards.txt	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/boards.txt	(revision 224)
@@ -1,95 +1,132 @@
+######################################################
 #ARDUINO M0 PRO (WITH) BOOTLOADER - PROGRAMMING PORT
-arduino_zero_pro_bl_dbg.name=Arduino M0 Pro (Programming Port)
-arduino_zero_pro_bl_dbg.vid.0=0x03eb
-arduino_zero_pro_bl_dbg.pid.0=0x2111
-arduino_zero_pro_bl_dbg.upload.tool=openocd
-arduino_zero_pro_bl_dbg.upload.protocol=sam-ba
-arduino_zero_pro_bl_dbg.upload.maximum_size=262144
-arduino_zero_pro_bl_dbg.upload.use_1200bps_touch=false
-arduino_zero_pro_bl_dbg.upload.wait_for_upload_port=false
-arduino_zero_pro_bl_dbg.upload.native_usb=false
-arduino_zero_pro_bl_dbg.build.mcu=cortex-m0plus
-arduino_zero_pro_bl_dbg.build.f_cpu=48000000L
-arduino_zero_pro_bl_dbg.build.usb_product="Arduino M0 Pro"
-arduino_zero_pro_bl_dbg.build.board=SAM_ZERO
-arduino_zero_pro_bl_dbg.build.core=arduino
-arduino_zero_pro_bl_dbg.build.extra_flags=-D__SAMD21G18A__ -mthumb {build.usb_flags}
-arduino_zero_pro_bl_dbg.build.ldscript=linker_scripts/gcc/flash_with_bootloader.ld
-arduino_zero_pro_bl_dbg.build.openocdscript=openocd_scripts/arduino_zero.cfg
-arduino_zero_pro_bl_dbg.build.variant=arduino_zero
-arduino_zero_pro_bl_dbg.build.variant_system_lib=
-arduino_zero_pro_bl_dbg.build.vid=0x2a03
-arduino_zero_pro_bl_dbg.build.pid=0x804f
-arduino_zero_pro_bl_dbg.build.section.start=0x4000
+mzero_pro_bl_dbg.name=Arduino M0 Pro (Programming Port)
+mzero_pro_bl_dbg.vid.0=0x03eb
+mzero_pro_bl_dbg.pid.0=0x2111
+mzero_pro_bl_dbg.upload.tool=openocd
+mzero_pro_bl_dbg.upload.protocol=sam-ba
+mzero_pro_bl_dbg.upload.maximum_size=262144
+mzero_pro_bl_dbg.upload.use_1200bps_touch=false
+mzero_pro_bl_dbg.upload.wait_for_upload_port=false
+mzero_pro_bl_dbg.upload.native_usb=false
+mzero_pro_bl_dbg.build.mcu=cortex-m0plus
+mzero_pro_bl_dbg.build.f_cpu=48000000L
+mzero_pro_bl_dbg.build.usb_product="Arduino M0 Pro"
+mzero_pro_bl_dbg.build.board=SAM_ZERO
+mzero_pro_bl_dbg.build.core=arduino
+mzero_pro_bl_dbg.build.extra_flags=-D__SAMD21G18A__ -mthumb {build.usb_flags}
+mzero_pro_bl_dbg.build.ldscript=linker_scripts/gcc/flash_with_bootloader.ld
+mzero_pro_bl_dbg.build.openocdscript=openocd_scripts/arduino_zero.cfg
+mzero_pro_bl_dbg.build.variant=arduino_zero
+mzero_pro_bl_dbg.build.variant_system_lib=
+mzero_pro_bl_dbg.build.vid=0x2a03
+mzero_pro_bl_dbg.build.pid=0x804f
+mzero_pro_bl_dbg.build.section.start=0x4000
 
-arduino_zero_pro_bl_dbg.bootloader.tool=openocd
-arduino_zero_pro_bl_dbg.bootloader.cmd_unprotect=at91samd bootloader 0
-arduino_zero_pro_bl_dbg.bootloader.cmd_protect=at91samd bootloader 16384
-arduino_zero_pro_bl_dbg.bootloader.cmd_protect_verify=at91samd bootloader
-arduino_zero_pro_bl_dbg.bootloader.file=zero/Bootloader_D21_M0_Pro_150427.hex
+mzero_pro_bl_dbg.bootloader.tool=openocd
+mzero_pro_bl_dbg.bootloader.cmd_unprotect=at91samd bootloader 0
+mzero_pro_bl_dbg.bootloader.cmd_protect=at91samd bootloader 16384
+mzero_pro_bl_dbg.bootloader.cmd_protect_verify=at91samd bootloader
+mzero_pro_bl_dbg.bootloader.file=zero/Bootloader_D21_M0_Pro_150427.hex
 
+######################################################
 #ARDUINO M0 PRO (WITH) BOOTLOADER - NATIVE PORT
-arduino_zero_pro_bl.name=Arduino M0 Pro (Native USB Port)
-arduino_zero_pro_bl.vid.0=0x2a03
-arduino_zero_pro_bl.pid.0=0x004d
-arduino_zero_pro_bl.vid.1=0x2a03
-arduino_zero_pro_bl.pid.1=0x804d
-arduino_zero_pro_bl.vid.2=0x2a03
-arduino_zero_pro_bl.pid.2=0x004f
-arduino_zero_pro_bl.vid.3=0x2a03
-arduino_zero_pro_bl.pid.3=0x804f
-arduino_zero_pro_bl.upload.tool=avrdude
-arduino_zero_pro_bl.upload.protocol=stk500v2
-arduino_zero_pro_bl.upload.maximum_size=262144
-arduino_zero_pro_bl.upload.use_1200bps_touch=true
-arduino_zero_pro_bl.upload.wait_for_upload_port=true
-arduino_zero_pro_bl.upload.native_usb=true
-arduino_zero_pro_bl.upload.speed=57600
-arduino_zero_pro_bl.build.mcu=cortex-m0plus
-arduino_zero_pro_bl.build.f_cpu=48000000L
-arduino_zero_pro_bl.build.usb_product="Arduino M0 Pro"
-arduino_zero_pro_bl.build.board=SAM_ZERO
-arduino_zero_pro_bl.build.core=arduino
-arduino_zero_pro_bl.build.extra_flags=-D__SAMD21G18A__ -mthumb {build.usb_flags}
-arduino_zero_pro_bl.build.ldscript=linker_scripts/gcc/flash_with_bootloader.ld
-arduino_zero_pro_bl.build.variant=arduino_zero
-arduino_zero_pro_bl.build.variant_system_lib=
-arduino_zero_pro_bl.build.vid=0x2a03
-arduino_zero_pro_bl.build.pid=0x804f
-arduino_zero_pro_bl.build.section.start=0x4000
-arduino_zero_pro_bl.build.emu.mcu=atmega2560
-arduino_zero_pro_bl.bootloader.tool=avrdude
-arduino_zero_pro_bl.bootloader.low_fuses=0xff
+mzero_pro_bl.name=Arduino M0 Pro (Native USB Port)
+mzero_pro_bl.vid.0=0x2a03
+mzero_pro_bl.pid.0=0x004d
+mzero_pro_bl.vid.1=0x2a03
+mzero_pro_bl.pid.1=0x804d
+mzero_pro_bl.vid.2=0x2a03
+mzero_pro_bl.pid.2=0x004f
+mzero_pro_bl.vid.3=0x2a03
+mzero_pro_bl.pid.3=0x804f
+mzero_pro_bl.upload.tool=avrdude
+mzero_pro_bl.upload.protocol=stk500v2
+mzero_pro_bl.upload.maximum_size=262144
+mzero_pro_bl.upload.use_1200bps_touch=true
+mzero_pro_bl.upload.wait_for_upload_port=true
+mzero_pro_bl.upload.native_usb=true
+mzero_pro_bl.upload.speed=57600
+mzero_pro_bl.build.mcu=cortex-m0plus
+mzero_pro_bl.build.f_cpu=48000000L
+mzero_pro_bl.build.usb_product="Arduino M0 Pro"
+mzero_pro_bl.build.board=SAM_ZERO
+mzero_pro_bl.build.core=arduino
+mzero_pro_bl.build.extra_flags=-D__SAMD21G18A__ -mthumb {build.usb_flags}
+mzero_pro_bl.build.ldscript=linker_scripts/gcc/flash_with_bootloader.ld
+mzero_pro_bl.build.variant=arduino_zero
+mzero_pro_bl.build.variant_system_lib=
+mzero_pro_bl.build.vid=0x2a03
+mzero_pro_bl.build.pid=0x804f
+mzero_pro_bl.build.section.start=0x4000
+mzero_pro_bl.build.emu.mcu=atmega2560
+mzero_pro_bl.bootloader.tool=avrdude
+mzero_pro_bl.bootloader.low_fuses=0xff
 
+######################################################
 #ARDUINO M0 (WITH) BOOTLOADER
-arduino_zero_bl.name=Arduino M0
-arduino_zero_bl.vid.0=0x2a03
-arduino_zero_bl.pid.0=0x004d
-arduino_zero_bl.vid.1=0x2a03
-arduino_zero_bl.pid.1=0x804d
-arduino_zero_bl.vid.2=0x2a03
-arduino_zero_bl.pid.2=0x004e
-arduino_zero_bl.vid.3=0x2a03
-arduino_zero_bl.pid.3=0x804e
-arduino_zero_bl.upload.tool=avrdude
-arduino_zero_bl.upload.protocol=stk500v2
-arduino_zero_bl.upload.maximum_size=262144
-arduino_zero_bl.upload.use_1200bps_touch=true
-arduino_zero_bl.upload.wait_for_upload_port=true
-arduino_zero_bl.upload.native_usb=true
-arduino_zero_bl.upload.speed=57600
-arduino_zero_bl.build.mcu=cortex-m0plus
-arduino_zero_bl.build.f_cpu=48000000L
-arduino_zero_bl.build.usb_product="Arduino M0"
-arduino_zero_bl.build.board=SAM_ZERO
-arduino_zero_bl.build.core=arduino
-arduino_zero_bl.build.extra_flags=-D__SAMD21G18A__ -mthumb {build.usb_flags}
-arduino_zero_bl.build.ldscript=linker_scripts/gcc/flash_with_bootloader.ld
-arduino_zero_bl.build.variant=arduino_zero
-arduino_zero_bl.build.variant_system_lib=
-arduino_zero_bl.build.vid=0x2a03
-arduino_zero_bl.build.pid=0x804e
-arduino_zero_bl.build.section.start=0x4000
-arduino_zero_bl.build.emu.mcu=atmega2560
-arduino_zero_bl.bootloader.tool=avrdude
-arduino_zero_bl.bootloader.low_fuses=0xff
+mzero_bl.name=Arduino M0
+mzero_bl.vid.0=0x2a03
+mzero_bl.pid.0=0x004d
+mzero_bl.vid.1=0x2a03
+mzero_bl.pid.1=0x804d
+mzero_bl.vid.2=0x2a03
+mzero_bl.pid.2=0x004e
+mzero_bl.vid.3=0x2a03
+mzero_bl.pid.3=0x804e
+mzero_bl.upload.tool=avrdude
+mzero_bl.upload.protocol=stk500v2
+mzero_bl.upload.maximum_size=262144
+mzero_bl.upload.use_1200bps_touch=true
+mzero_bl.upload.wait_for_upload_port=true
+mzero_bl.upload.native_usb=true
+mzero_bl.upload.speed=57600
+mzero_bl.build.mcu=cortex-m0plus
+mzero_bl.build.f_cpu=48000000L
+mzero_bl.build.usb_product="Arduino M0"
+mzero_bl.build.board=SAM_ZERO
+mzero_bl.build.core=arduino
+mzero_bl.build.extra_flags=-D__SAMD21G18A__ -mthumb {build.usb_flags}
+mzero_bl.build.ldscript=linker_scripts/gcc/flash_with_bootloader.ld
+mzero_bl.build.variant=arduino_zero
+mzero_bl.build.variant_system_lib=
+mzero_bl.build.vid=0x2a03
+mzero_bl.build.pid=0x804e
+mzero_bl.build.section.start=0x4000
+mzero_bl.build.emu.mcu=atmega2560
+mzero_bl.bootloader.tool=avrdude
+mzero_bl.bootloader.low_fuses=0xff
+
+######################################################
+#ARDUINO TIAN (WITH) BOOTLOADER
+
+tian.name=Arduino Tian
+tian.upload.via_ssh=true
+tian.vid.0=0x10C4
+tian.pid.0=0xEA70
+
+tian.upload.tool=avrdude
+#tian.upload.protocol=stk500v2
+tian.upload.protocol=wiring
+tian.upload.maximum_size=262144
+tian.upload.use_1200bps_touch=true
+tian.upload.wait_for_upload_port=true
+tian.upload.native_usb=true
+tian.upload.speed=57600
+tian.build.mcu=cortex-m0plus
+tian.build.f_cpu=48000000L
+tian.build.usb_product="Arduino Tian"
+tian.build.board=SAMD_TIAN
+tian.build.core=arduino
+tian.build.extra_flags=-D__SAMD21G18A__ -mthumb {build.usb_flags}
+tian.build.ldscript=linker_scripts/gcc/flash_with_bootloader.ld
+tian.build.variant=arduino_zero
+tian.build.variant_system_lib=
+tian.build.vid=0x2a03
+tian.build.pid=0x8052
+tian.build.section.start=0x4000
+tian.build.emu.mcu=atmega2560
+tian.bootloader.tool=avrdude
+tian.bootloader.low_fuses=0xff
+tian.bootloader.file=sofia/Sofia_Tian_151118.hex
+tian.drivers=SiliconLabs-CP2105/Silicon Labs VCP Driver.pkg
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/Print.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/Print.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/Print.cpp	(revision 224)
@@ -115,7 +115,5 @@
 size_t Print::println(void)
 {
-  size_t n = print('\r');
-  n += print('\n');
-  return n;
+   return write("\r\n");
 }
 
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/CDC.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/CDC.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/CDC.cpp	(revision 224)
@@ -1,16 +1,21 @@
-/* Copyright (c) 2011, Peter Barrett
-**
-** Permission to use, copy, modify, and/or distribute this software for
-** any purpose with or without fee is hereby granted, provided that the
-** above copyright notice and this permission notice appear in all copies.
-**
-** THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
-** WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
-** WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR
-** BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES
-** OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
-** WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION,
-** ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS
-** SOFTWARE.
+/*
+  Copyright (c) 2015 Arduino LLC.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+  See the GNU Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+/*
+** Modified 04/04/2016 by Arduino.org development team
 */
 
@@ -20,18 +25,10 @@
 
 // Include Atmel headers
-#include "Arduino.h"
-#include "sam.h"
-#include "wiring_constants.h"
-#include "USBCore.h"
-#include "USB/USB_device.h"
-#include "USBDesc.h"
-#include "USBAPI.h"
-
-#include "Reset.h"
-
+#include <Arduino.h>
+#include <Reset.h> 
 
 #ifdef CDC_ENABLED
 
-#define CDC_SERIAL_BUFFER_SIZE	64
+#define CDC_SERIAL_BUFFER_SIZE	256
 
 /* For information purpose only since RTS is not always handled by the terminal application */
@@ -41,66 +38,70 @@
 #define CDC_LINESTATE_READY		(CDC_LINESTATE_RTS | CDC_LINESTATE_DTR)
 
-struct ring_buffer
-{
+struct ring_buffer {
 	uint8_t buffer[CDC_SERIAL_BUFFER_SIZE];
 	volatile uint32_t head;
 	volatile uint32_t tail;
+	volatile bool full;
 };
-
-ring_buffer cdc_rx_buffer = { { 0 }, 0, 0};
-
-typedef struct
-{
-	uint32_t	dwDTERate;
-	uint8_t		bCharFormat;
-	uint8_t 	bParityType;
-	uint8_t 	bDataBits;
-	uint8_t		lineState;
+ring_buffer cdc_rx_buffer = {{0}, 0, 0, false};
+
+typedef struct {
+	uint32_t dwDTERate;
+	uint8_t bCharFormat;
+	uint8_t bParityType;
+	uint8_t bDataBits;
+	uint8_t lineState;
 } LineInfo;
 
 _Pragma("pack(1)")
-static volatile LineInfo _usbLineInfo = { 
-   115200, // dWDTERate
-    0x00,  // bCharFormat
-    0x00,  // bParityType
-    0x08,  // bDataBits
-    0x00   // lineState
+static volatile LineInfo _usbLineInfo = {
+	115200, // dWDTERate
+	0x00,   // bCharFormat
+	0x00,   // bParityType
+	0x08,   // bDataBits
+	0x00    // lineState
 };
 
-static const CDCDescriptor _cdcInterface =
-{
-#if (defined CDC_ENABLED) && defined(HID_ENABLED)
+static volatile int32_t breakValue = -1;
+
+static CDCDescriptor _cdcInterface = {
 	D_IAD(0, 2, CDC_COMMUNICATION_INTERFACE_CLASS, CDC_ABSTRACT_CONTROL_MODEL, 0),
-#endif
-	//	CDC communication interface
-	D_INTERFACE(CDC_ACM_INTERFACE,1,CDC_COMMUNICATION_INTERFACE_CLASS,CDC_ABSTRACT_CONTROL_MODEL,0),
-	D_CDCCS( CDC_HEADER, CDC_V1_10 & 0xFF, (CDC_V1_10>>8) & 0x0FF ),	// Header (1.10 bcd)
-	
-	D_CDCCS4(CDC_ABSTRACT_CONTROL_MANAGEMENT,6),				// SET_LINE_CODING, GET_LINE_CODING, SET_CONTROL_LINE_STATE supported
-	D_CDCCS(CDC_UNION,CDC_ACM_INTERFACE,CDC_DATA_INTERFACE),	// Communication interface is master, data interface is slave 0
-	D_CDCCS(CDC_CALL_MANAGEMENT,1,1),							// Device handles call management (not)
-	D_ENDPOINT(USB_ENDPOINT_IN (CDC_ENDPOINT_ACM),USB_ENDPOINT_TYPE_INTERRUPT,0x10, 0x10),
-
-	//	CDC data interface
-	D_INTERFACE(CDC_DATA_INTERFACE,2,CDC_DATA_INTERFACE_CLASS,0,0),
-	D_ENDPOINT(USB_ENDPOINT_OUT(CDC_ENDPOINT_OUT),USB_ENDPOINT_TYPE_BULK,EPX_SIZE,0),
-	D_ENDPOINT(USB_ENDPOINT_IN (CDC_ENDPOINT_IN ),USB_ENDPOINT_TYPE_BULK,EPX_SIZE,0)
+
+	// CDC communication interface
+	D_INTERFACE(CDC_ACM_INTERFACE, 1, CDC_COMMUNICATION_INTERFACE_CLASS, CDC_ABSTRACT_CONTROL_MODEL, 0),
+	D_CDCCS(CDC_HEADER, CDC_V1_10 & 0xFF, (CDC_V1_10>>8) & 0x0FF), // Header (1.10 bcd)
+
+	D_CDCCS4(CDC_ABSTRACT_CONTROL_MANAGEMENT, 6), // SET_LINE_CODING, GET_LINE_CODING, SET_CONTROL_LINE_STATE supported
+	D_CDCCS(CDC_UNION, CDC_ACM_INTERFACE, CDC_DATA_INTERFACE), // Communication interface is master, data interface is slave 0
+	D_CDCCS(CDC_CALL_MANAGEMENT, 1, 1), // Device handles call management (not)
+	D_ENDPOINT(USB_ENDPOINT_IN(CDC_ENDPOINT_ACM), USB_ENDPOINT_TYPE_INTERRUPT, 0x10, 0x10),
+
+	// CDC data interface
+	D_INTERFACE(CDC_DATA_INTERFACE, 2, CDC_DATA_INTERFACE_CLASS, 0, 0),
+	D_ENDPOINT(USB_ENDPOINT_OUT(CDC_ENDPOINT_OUT), USB_ENDPOINT_TYPE_BULK, EPX_SIZE, 0),
+	D_ENDPOINT(USB_ENDPOINT_IN (CDC_ENDPOINT_IN ), USB_ENDPOINT_TYPE_BULK, EPX_SIZE, 0)
 };
 _Pragma("pack()")
 
-const void* WEAK CDC_GetInterface(void)
-{
-	return  &_cdcInterface;
-}
-
-uint32_t WEAK CDC_GetInterfaceLength(void)
-{
-    return sizeof( _cdcInterface );
-}
-
-bool WEAK CDC_Setup(Setup& setup)
-{
-	uint8_t r = setup.bRequest;
-	uint8_t requestType = setup.bmRequestType;
+const void* _CDC_GetInterface(void)
+{
+	return &_cdcInterface;
+}
+
+uint32_t _CDC_GetInterfaceLength(void)
+{
+	return sizeof(_cdcInterface);
+}
+
+int CDC_GetInterface(uint8_t* interfaceNum)
+{
+	interfaceNum[0] += 2;	// uses 2
+	return USBD_SendControl(&_cdcInterface,sizeof(_cdcInterface));
+}
+
+bool CDC_Setup(Setup& pSetup)
+{
+	uint8_t requestType = pSetup.bmRequestType;
+	uint8_t r = pSetup.bRequest;
 
 	if (REQUEST_DEVICETOHOST_CLASS_INTERFACE == requestType)
@@ -108,5 +109,5 @@
 		if (CDC_GET_LINE_CODING == r)
 		{
-			USBD_SendControl(0,(void*)&_usbLineInfo,7);
+			USBD_SendControl((void*)&_usbLineInfo, 7);
 			return true;
 		}
@@ -117,27 +118,33 @@
 		if (CDC_SET_LINE_CODING == r)
 		{
-			while( UDD_FifoByteCount(EP0) <15);
-			//USBD_RecvControl((void*)&_usbLineInfo,7);
-			uint8_t* line = (uint8_t*)&_usbLineInfo;
-			for(uint8_t i = 0; i<7; i++)
-				line[i] = setup.data[i];
-			return false;
+			USBD_RecvControl((void*)&_usbLineInfo, 7);
 		}
 
 		if (CDC_SET_CONTROL_LINE_STATE == r)
 		{
-			_usbLineInfo.lineState = setup.wValueL;
+			_usbLineInfo.lineState = pSetup.wValueL;
+		}
+
+		if (r == CDC_SET_LINE_CODING || r == CDC_SET_CONTROL_LINE_STATE)
+		{
 			// auto-reset into the bootloader is triggered when the port, already
-			// open at 1200 bps, is closed.
-			if (1200 == _usbLineInfo.dwDTERate)
+			// open at 1200 bps, is closed. We check DTR state to determine if host 
+			// port is open (bit 0 of lineState).
+			if (_usbLineInfo.dwDTERate == 1200 && (_usbLineInfo.lineState & 0x01) == 0)
 			{
-				// We check DTR state to determine if host port is open (bit 0 of lineState).
-				if ((_usbLineInfo.lineState & 0x01) == 0)
-					initiateReset(250);
-				else
-					cancelReset();
+				initiateReset(250);
+			}
+			else
+			{
+				cancelReset();
 			}
 			return false;
 		}
+
+		if (r == CDC_SEND_BREAK)
+		{
+			breakValue = ((uint16_t)pSetup.wValueH << 8) | pSetup.wValueL;
+			return false;
+		}
 	}
 	return false;
@@ -159,51 +166,65 @@
 void Serial_::accept(void)
 {
+	uint8_t buffer[CDC_SERIAL_BUFFER_SIZE];
+	uint32_t len = USBD_Recv(CDC_ENDPOINT_OUT, &buffer, CDC_SERIAL_BUFFER_SIZE);
+
+	uint8_t enableInterrupts = ((__get_PRIMASK() & 0x1) == 0);
+	__disable_irq();
+
+	ring_buffer *ringBuffer = &cdc_rx_buffer;
+	uint32_t i = ringBuffer->head;
+
+	uint32_t k = 0;
+	while (len > 0 && !ringBuffer->full) {
+		len--;
+		ringBuffer->buffer[i++] = buffer[k++];
+		i %= CDC_SERIAL_BUFFER_SIZE;
+		if (i == ringBuffer->tail)
+			ringBuffer->full = true;
+	}
+	ringBuffer->head = i;
+	if (enableInterrupts) {
+		__enable_irq();
+	}
+}
+
+int Serial_::available(void)
+{
 	ring_buffer *buffer = &cdc_rx_buffer;
-	uint32_t i = (uint32_t)(buffer->head+1) % CDC_SERIAL_BUFFER_SIZE;
-
-	// if we should be storing the received character into the location
-	// just before the tail (meaning that the head would advance to the
-	// current location of the tail), we're about to overflow the buffer
-	// and so we don't write the character or advance the head.
-	while (i != buffer->tail) {
-		uint32_t c;
-		if (!USBD_Available(CDC_ENDPOINT_OUT)) {
-            UDD_ReleaseRX(CDC_ENDPOINT_OUT);
-			break;
-		}
-		c = USBD_Recv(CDC_ENDPOINT_OUT);
-		buffer->buffer[buffer->head] = c;
-		buffer->head = i;
-
-		i = (i + 1) % CDC_SERIAL_BUFFER_SIZE;
-	}
-}
-
-int Serial_::available(void)
+	if (buffer->full) {
+		return CDC_SERIAL_BUFFER_SIZE;
+	}
+	if (buffer->head == buffer->tail) {
+		USB->DEVICE.DeviceEndpoint[CDC_ENDPOINT_OUT].EPINTENSET.reg = USB_DEVICE_EPINTENCLR_TRCPT(1);
+	}
+	return (uint32_t)(CDC_SERIAL_BUFFER_SIZE + buffer->head - buffer->tail) % CDC_SERIAL_BUFFER_SIZE;
+}
+
+int Serial_::peek(void)
 {
 	ring_buffer *buffer = &cdc_rx_buffer;
-	return (uint32_t)(CDC_SERIAL_BUFFER_SIZE + buffer->head - buffer->tail) % CDC_SERIAL_BUFFER_SIZE;
-}
-
-int Serial_::peek(void)
+	if (buffer->head == buffer->tail && !buffer->full) {
+		return -1;
+	} else {
+		return buffer->buffer[buffer->tail];
+	}
+}
+
+
+// if the ringBuffer is empty: try to fill it
+// if it's still empty: return -1
+// else return the last char
+// so the buffer is filled only when needed
+int Serial_::read(void)
 {
 	ring_buffer *buffer = &cdc_rx_buffer;
 
-	if (buffer->head == buffer->tail)
-	{
-		return -1;
-	}
-	else
-	{
-		return buffer->buffer[buffer->tail];
-	}
-}
-
-int Serial_::read(void)
-{
-	ring_buffer *buffer = &cdc_rx_buffer;
-
 	// if the head isn't ahead of the tail, we don't have any characters
-	if (buffer->head == buffer->tail)
+	if (buffer->head == buffer->tail && !buffer->full)
+	{
+		if (USBD_Available(CDC_ENDPOINT_OUT))
+			accept();
+	}
+	if (buffer->head == buffer->tail && !buffer->full)
 	{
 		return -1;
@@ -213,6 +234,5 @@
 		unsigned char c = buffer->buffer[buffer->tail];
 		buffer->tail = (uint32_t)(buffer->tail + 1) % CDC_SERIAL_BUFFER_SIZE;
-		if (USBD_Available(CDC_ENDPOINT_OUT))
-			accept();
+		buffer->full = false;
 		return c;
 	}
@@ -231,17 +251,15 @@
 	 bytes sent before the user opens the connection or after
 	 the connection is closed are lost - just like with a UART. */
-
+	if (_usbLineInfo.lineState > 0)  // Problem with Windows(R)
 	{
 		uint32_t r = USBD_Send(CDC_ENDPOINT_IN, buffer, size);
 
-		if (r > 0)
-		{
+		if (r == 0) {
 			return r;
-		} else
-		{
+		} else {
 			setWriteError();
 			return 0;
 		}
-	}
+	 }
 	setWriteError();
 	return 0;
@@ -259,5 +277,4 @@
 // We add a short delay before returning to fix a bug observed by Federico
 // where the port is configured (lineState != 0) but not quite opened.
-
 Serial_::operator bool()
 {
@@ -277,5 +294,49 @@
 }
 
-Serial_ SerialUSB;
+int32_t Serial_::readBreak() {
+	uint8_t enableInterrupts = ((__get_PRIMASK() & 0x1) == 0);
+
+	// disable interrupts,
+	// to avoid clearing a breakValue that might occur 
+	// while processing the current break value
+	__disable_irq();
+
+	int32_t ret = breakValue;
+
+	breakValue = -1;
+
+	if (enableInterrupts) {
+		// re-enable the interrupts
+		__enable_irq();
+	}
+
+	return ret;
+}
+
+unsigned long Serial_::baud() {
+	return _usbLineInfo.dwDTERate;
+}
+
+uint8_t Serial_::stopbits() {
+	return _usbLineInfo.bCharFormat;
+}
+
+uint8_t Serial_::paritytype() {
+	return _usbLineInfo.bParityType;
+}
+
+uint8_t Serial_::numbits() {
+	return _usbLineInfo.bDataBits;
+}
+
+bool Serial_::dtr() {
+	return _usbLineInfo.lineState & 0x1;
+}
+
+bool Serial_::rts() {
+	return _usbLineInfo.lineState & 0x2;
+}
+
+Serial_ SerialUSB(USBDevice);
 
 #endif
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/HID.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/HID.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/HID.cpp	(revision 224)
@@ -21,5 +21,4 @@
 #include "USBDesc.h"
 #include "sam.h"
-#include "USB/USB_device.h"
 
 
@@ -45,5 +44,5 @@
 //	HID report descriptor
 _Pragma("pack(1)")
-extern const uint8_t _hidReportDescriptor[] = {
+extern const uint32_t _hidReportDescriptor[] = {
 	//	Mouse
     0x05, 0x01,                    //   USAGE_PAGE (Generic Desktop)	// 54
@@ -162,5 +161,5 @@
 uint32_t WEAK HID_GetDescriptor(void)
 {
-	return USBD_SendControl(0,_hidReportDescriptor,sizeof(_hidReportDescriptor));
+	return USBD_SendControl(_hidReportDescriptor,sizeof(_hidReportDescriptor));
 }
 
@@ -197,6 +196,6 @@
 		if (HID_GET_IDLE == r)
 		{
-			UDD_Send(0, &_hid_idle, 1);
-			UDD_ClearIN();
+			USBD_Send(0, &_hid_idle, 1);
+			USB->DEVICE.DeviceEndpoint[EP0].EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_BK1RDY;
 			return true;
 		}
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/SAMD21_USBDevice.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/SAMD21_USBDevice.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/SAMD21_USBDevice.h	(revision 224)
@@ -0,0 +1,200 @@
+/*
+  Copyright (c) 2015 Arduino LLC.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+  See the GNU Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+/*
+**Modified 04/04/2016 by Arduino.org development team
+*/ 
+
+#pragma once
+
+#include <Arduino.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+
+typedef uint8_t ep_t;
+
+class USBDevice_SAMD21G18x {
+public:
+	USBDevice_SAMD21G18x() : usb(USB->DEVICE) {
+		// Empty
+	}
+
+	// USB Device function mapping
+	// ---------------------------
+
+	// Reset USB Device
+	void reset();
+
+	// Enable
+	inline void enable()  { usb.CTRLA.bit.ENABLE = 1; }
+	inline void disable() { usb.CTRLA.bit.ENABLE = 0; }
+
+	// USB mode (device/host)
+	inline void setUSBDeviceMode() { usb.CTRLA.bit.MODE = USB_CTRLA_MODE_DEVICE_Val; }
+	inline void setUSBHostMode()   { usb.CTRLA.bit.MODE = USB_CTRLA_MODE_HOST_Val;   }
+
+	inline void runInStandby()   { usb.CTRLA.bit.RUNSTDBY = 1; }
+	inline void noRunInStandby() { usb.CTRLA.bit.RUNSTDBY = 0; }
+
+	// USB speed
+	inline void setFullSpeed()       { usb.CTRLB.bit.SPDCONF = USB_DEVICE_CTRLB_SPDCONF_0_Val;   }
+	inline void setLowSpeed()        { usb.CTRLB.bit.SPDCONF = USB_DEVICE_CTRLB_SPDCONF_1_Val;   }
+	inline void setHiSpeed()         { usb.CTRLB.bit.SPDCONF = USB_DEVICE_CTRLB_SPDCONF_2_Val;   }
+	inline void setHiSpeedTestMode() { usb.CTRLB.bit.SPDCONF = USB_DEVICE_CTRLB_SPDCONF_3_Val; }
+
+	// Authorize attach if Vbus is present
+	inline void attach() { usb.CTRLB.bit.DETACH = 0; }
+	inline void detach() { usb.CTRLB.bit.DETACH = 1; }
+
+	// USB Interrupts
+	inline bool isEndOfResetInterrupt()        { return usb.INTFLAG.bit.EORST; }
+	inline void ackEndOfResetInterrupt()       { usb.INTFLAG.reg = USB_DEVICE_INTFLAG_EORST; }
+	inline void enableEndOfResetInterrupt()    { usb.INTENSET.bit.EORST = 1; } 
+	inline void disableEndOfResetInterrupt()   { usb.INTENCLR.bit.EORST = 1; } 
+
+	inline bool isStartOfFrameInterrupt()      { return usb.INTFLAG.bit.SOF; }
+	inline void ackStartOfFrameInterrupt()     { usb.INTFLAG.reg = USB_DEVICE_INTFLAG_SOF; }
+	inline void enableStartOfFrameInterrupt()  { usb.INTENSET.bit.SOF = 1; }
+	inline void disableStartOfFrameInterrupt() { usb.INTENCLR.bit.SOF = 1; }
+
+	// USB Address
+	inline void setAddress(uint32_t addr)   { usb.DADD.bit.DADD = addr; usb.DADD.bit.ADDEN = 1; }
+	inline void unsetAddress()              { usb.DADD.bit.DADD = 0;    usb.DADD.bit.ADDEN = 0; }
+
+	// Frame number
+	inline uint16_t frameNumber() { return usb.FNUM.bit.FNUM; }
+
+	// Load calibration values
+	void calibrate();
+
+	// USB Device Endpoints function mapping
+	// -------------------------------------
+
+	// Config
+	inline void epBank0SetType(ep_t ep, uint8_t type) { usb.DeviceEndpoint[ep].EPCFG.bit.EPTYPE0 = type; }
+	inline void epBank1SetType(ep_t ep, uint8_t type) { usb.DeviceEndpoint[ep].EPCFG.bit.EPTYPE1 = type; }
+
+	// Interrupts
+	inline uint16_t epInterruptSummary() { return usb.EPINTSMRY.reg; }
+
+	inline bool epBank0IsSetupReceived(ep_t ep)     { return usb.DeviceEndpoint[ep].EPINTFLAG.bit.RXSTP; }
+	inline bool epBank0IsStalled(ep_t ep)           { return usb.DeviceEndpoint[ep].EPINTFLAG.bit.STALL0; }
+	inline bool epBank1IsStalled(ep_t ep)           { return usb.DeviceEndpoint[ep].EPINTFLAG.bit.STALL1; }
+	inline bool epBank0IsTransferComplete(ep_t ep)  { return usb.DeviceEndpoint[ep].EPINTFLAG.bit.TRCPT0; }
+	inline bool epBank1IsTransferComplete(ep_t ep)  { return usb.DeviceEndpoint[ep].EPINTFLAG.bit.TRCPT1; }
+
+	inline void epBank0AckSetupReceived(ep_t ep)    { usb.DeviceEndpoint[ep].EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_RXSTP; }
+	inline void epBank0AckStalled(ep_t ep)          { usb.DeviceEndpoint[ep].EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_STALL(1); }
+	inline void epBank1AckStalled(ep_t ep)          { usb.DeviceEndpoint[ep].EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_STALL(2); }
+	inline void epBank0AckTransferComplete(ep_t ep) { usb.DeviceEndpoint[ep].EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_TRCPT(1); }
+	inline void epBank1AckTransferComplete(ep_t ep) { usb.DeviceEndpoint[ep].EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_TRCPT(2); }
+
+	inline void epBank0EnableSetupReceived(ep_t ep)    { usb.DeviceEndpoint[ep].EPINTENSET.bit.RXSTP = 1; }
+	inline void epBank0EnableStalled(ep_t ep)          { usb.DeviceEndpoint[ep].EPINTENSET.bit.STALL0 = 1; }
+	inline void epBank1EnableStalled(ep_t ep)          { usb.DeviceEndpoint[ep].EPINTENSET.bit.STALL1 = 1; }
+	inline void epBank0EnableTransferComplete(ep_t ep) { usb.DeviceEndpoint[ep].EPINTENSET.bit.TRCPT0 = 1; }
+	inline void epBank1EnableTransferComplete(ep_t ep) { usb.DeviceEndpoint[ep].EPINTENSET.bit.TRCPT1 = 1; }
+
+	inline void epBank0DisableSetupReceived(ep_t ep)    { usb.DeviceEndpoint[ep].EPINTENCLR.bit.RXSTP = 1; }
+	inline void epBank0DisableStalled(ep_t ep)          { usb.DeviceEndpoint[ep].EPINTENCLR.bit.STALL0 = 1; }
+	inline void epBank1DisableStalled(ep_t ep)          { usb.DeviceEndpoint[ep].EPINTENCLR.bit.STALL1 = 1; }
+	inline void epBank0DisableTransferComplete(ep_t ep) { usb.DeviceEndpoint[ep].EPINTENCLR.bit.TRCPT0 = 1; }
+	inline void epBank1DisableTransferComplete(ep_t ep) { usb.DeviceEndpoint[ep].EPINTENCLR.bit.TRCPT1 = 1; }
+
+	// Status
+	inline bool epBank0IsReady(ep_t ep)    { return usb.DeviceEndpoint[ep].EPSTATUS.bit.BK0RDY; }
+	inline bool epBank1IsReady(ep_t ep)    { return usb.DeviceEndpoint[ep].EPSTATUS.bit.BK1RDY; }
+	inline void epBank0SetReady(ep_t ep)   { usb.DeviceEndpoint[ep].EPSTATUSSET.bit.BK0RDY = 1; }
+	inline void epBank1SetReady(ep_t ep)   { usb.DeviceEndpoint[ep].EPSTATUSSET.bit.BK1RDY = 1; }
+	inline void epBank0ResetReady(ep_t ep) { usb.DeviceEndpoint[ep].EPSTATUSCLR.bit.BK0RDY = 1; }
+	inline void epBank1ResetReady(ep_t ep) { usb.DeviceEndpoint[ep].EPSTATUSCLR.bit.BK1RDY = 1; }
+
+	inline void epBank0SetStallReq(ep_t ep)   { usb.DeviceEndpoint[ep].EPSTATUSSET.bit.STALLRQ0 = 1; }
+	inline void epBank1SetStallReq(ep_t ep)   { usb.DeviceEndpoint[ep].EPSTATUSSET.bit.STALLRQ1 = 1; }
+	inline void epBank0ResetStallReq(ep_t ep) { usb.DeviceEndpoint[ep].EPSTATUSCLR.bit.STALLRQ0 = 1; }
+	inline void epBank1ResetStallReq(ep_t ep) { usb.DeviceEndpoint[ep].EPSTATUSCLR.bit.STALLRQ1 = 1; }
+
+	// Packet
+	inline uint16_t epBank0ByteCount(ep_t ep) { return EP[ep].DeviceDescBank[0].PCKSIZE.bit.BYTE_COUNT; }
+	inline uint16_t epBank1ByteCount(ep_t ep) { return EP[ep].DeviceDescBank[1].PCKSIZE.bit.BYTE_COUNT; }
+	inline void epBank0SetByteCount(ep_t ep, uint16_t bc) { EP[ep].DeviceDescBank[0].PCKSIZE.bit.BYTE_COUNT = bc; }
+	inline void epBank1SetByteCount(ep_t ep, uint16_t bc) { EP[ep].DeviceDescBank[1].PCKSIZE.bit.BYTE_COUNT = bc; }
+	inline void epBank0SetMultiPacketSize(ep_t ep, uint16_t s) { EP[ep].DeviceDescBank[0].PCKSIZE.bit.MULTI_PACKET_SIZE = s; }
+	inline void epBank1SetMultiPacketSize(ep_t ep, uint16_t s) { EP[ep].DeviceDescBank[1].PCKSIZE.bit.MULTI_PACKET_SIZE = s; }
+
+	inline void epBank0SetAddress(ep_t ep, void *addr) { EP[ep].DeviceDescBank[0].ADDR.reg = (uint32_t)addr; }
+	inline void epBank1SetAddress(ep_t ep, void *addr) { EP[ep].DeviceDescBank[1].ADDR.reg = (uint32_t)addr; }
+	inline void epBank0SetSize(ep_t ep, uint16_t size) { EP[ep].DeviceDescBank[0].PCKSIZE.bit.SIZE = EP_PCKSIZE_SIZE(size); }
+	inline void epBank1SetSize(ep_t ep, uint16_t size) { EP[ep].DeviceDescBank[1].PCKSIZE.bit.SIZE = EP_PCKSIZE_SIZE(size); }
+	inline uint8_t EP_PCKSIZE_SIZE(uint16_t size) {
+		switch (size) {
+		case 8:    return 0;
+		case 16:   return 1;
+		case 32:   return 2;
+		case 64:   return 3;
+		case 128:  return 4;
+		case 256:  return 5;
+		case 512:  return 6;
+		case 1023: return 7;
+		default:   return 0;
+		}
+	}
+
+	inline void epBank0DisableAutoZLP(ep_t ep) { EP[ep].DeviceDescBank[0].PCKSIZE.bit.AUTO_ZLP = 0; }
+	inline void epBank1DisableAutoZLP(ep_t ep) { EP[ep].DeviceDescBank[1].PCKSIZE.bit.AUTO_ZLP = 0; }
+	inline void epBank0EnableAutoZLP(ep_t ep)  { EP[ep].DeviceDescBank[0].PCKSIZE.bit.AUTO_ZLP = 1; }
+	inline void epBank1EnableAutoZLP(ep_t ep)  { EP[ep].DeviceDescBank[1].PCKSIZE.bit.AUTO_ZLP = 1; }
+
+private:
+	// USB Device registers
+	UsbDevice &usb;
+
+	// Endpoints descriptors table
+	__attribute__((__aligned__(4)))	UsbDeviceDescriptor EP[USB_EPT_NUM];
+};
+
+void USBDevice_SAMD21G18x::reset() {
+	usb.CTRLA.bit.SWRST = 1;
+	memset(EP, 0, sizeof(EP));
+	while (usb.SYNCBUSY.bit.SWRST) {}
+	usb.DESCADD.reg = (uint32_t)(&EP);
+}
+
+void USBDevice_SAMD21G18x::calibrate() {
+	// Load Pad Calibration data from non-volatile memory
+	uint32_t *pad_transn_p = (uint32_t *) USB_FUSES_TRANSN_ADDR;
+	uint32_t *pad_transp_p = (uint32_t *) USB_FUSES_TRANSP_ADDR;
+	uint32_t *pad_trim_p   = (uint32_t *) USB_FUSES_TRIM_ADDR;
+
+	uint32_t pad_transn = (*pad_transn_p & USB_FUSES_TRANSN_Msk) >> USB_FUSES_TRANSN_Pos;
+	uint32_t pad_transp = (*pad_transp_p & USB_FUSES_TRANSP_Msk) >> USB_FUSES_TRANSP_Pos;
+	uint32_t pad_trim   = (*pad_trim_p   & USB_FUSES_TRIM_Msk  ) >> USB_FUSES_TRIM_Pos;
+
+	if (pad_transn == 0x1F)  // maximum value (31)
+		pad_transn = 5;
+	if (pad_transp == 0x1F)  // maximum value (31)
+		pad_transp = 29;
+	if (pad_trim == 0x7)     // maximum value (7)
+		pad_trim = 3;
+
+	usb.PADCAL.bit.TRANSN = pad_transn;
+	usb.PADCAL.bit.TRANSP = pad_transp;
+	usb.PADCAL.bit.TRIM   = pad_trim;
+}
+
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBAPI.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBAPI.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBAPI.h	(revision 224)
@@ -1,4 +1,4 @@
 /*
-  Copyright (c) 2012 Arduino.  All right reserved.
+  Copyright (c) 2015 Arduino LLC.  All right reserved.
 
   This library is free software; you can redistribute it and/or
@@ -16,18 +16,20 @@
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */
-
-#ifndef __USBAPI__
-#define __USBAPI__
-
-
-/* Define attribute */
-#if defined   ( __CC_ARM   ) /* Keil uVision 4 */
-    #define WEAK (__attribute__ ((weak)))
-#elif defined ( __ICCARM__ ) /* IAR Ewarm 5.41+ */
-    #define WEAK __weak
-#elif defined (  __GNUC__  ) /* GCC CS */
-    #define WEAK __attribute__ ((weak))
-#endif
-
+/*
+**Modified 04/04/2016 by Arduino.org development team
+*/
+
+#pragma once
+
+#define WEAK __attribute__ ((weak))
+
+#define HSTPIPCFG_PTYPE_BLK 1
+#define HSTPIPCFG_PTOKEN_IN 2
+#define HSTPIPCFG_PTOKEN_OUT 3
+#define HSTPIPCFG_PBK_1_BANK 4
+#define HSTPIPCFG_PTYPE_INTRPT 5
+
+#define EP0      0
+#define EPX_SIZE 64 // 64 for Full Speed, EPT size max is 1024
 
 #if defined __cplusplus
@@ -37,29 +39,49 @@
 
 //================================================================================
-//================================================================================
-//	USB
-
-class USBDevice_
-{
-public:
-	USBDevice_();
+// USB
+//================================================================================
+//================================================================================
+// Low level API
+typedef struct {
+	union {
+		uint8_t bmRequestType;
+		struct {
+			uint8_t direction : 5;
+			uint8_t type : 2;
+			uint8_t transferDirection : 1;
+		};
+	};
+	uint8_t bRequest;
+	uint8_t wValueL;
+	uint8_t wValueH;
+	uint16_t wIndex;
+	uint16_t wLength;
+} Setup;
+
+
+class USBDevice_ {
+public:
+	USBDevice_() {};
+
+	// USB Device API
+	void init();
+	bool attach();	// Serial port goes down too...
+	bool detach();
+
 	bool configured();
 
-	bool attach();
-	bool detach();	// Serial port goes down too...
-	void poll();
-	void init();
-};
+private:
+	bool initialized;
+};
+
 extern USBDevice_ USBDevice;
 
 //================================================================================
-//================================================================================
 //	Serial over CDC (Serial1 is the physical port)
 
 class Serial_ : public Stream
 {
-private:
-	RingBuffer *_cdc_rx_buffer;
-public:
+public:
+	Serial_(USBDevice_ &_usb) : usb(_usb) { }
 	void begin(uint32_t baud_count);
 	void begin(unsigned long, uint8_t);
@@ -75,4 +97,47 @@
 	using Print::write; // pull in write(str) from Print
 	operator bool();
+
+	// This method allows processing "SEND_BREAK" requests sent by
+	// the USB host. Those requests indicate that the host wants to
+	// send a BREAK signal and are accompanied by a single uint16_t
+	// value, specifying the duration of the break. The value 0
+	// means to end any current break, while the value 0xffff means
+	// to start an indefinite break.
+	// readBreak() will return the value of the most recent break
+	// request, but will return it at most once, returning -1 when
+	// readBreak() is called again (until another break request is
+	// received, which is again returned once).
+	// This also mean that if two break requests are received
+	// without readBreak() being called in between, the value of the
+	// first request is lost.
+	// Note that the value returned is a long, so it can return
+	// 0-0xffff as well as -1.
+	int32_t readBreak();
+
+	// These return the settings specified by the USB host for the
+	// serial port. These aren't really used, but are offered here
+	// in case a sketch wants to act on these settings.
+	uint32_t baud();
+	uint8_t stopbits();
+	uint8_t paritytype();
+	uint8_t numbits();
+	bool dtr();
+	bool rts();
+	enum {
+		ONE_STOP_BIT = 0,
+		ONE_AND_HALF_STOP_BIT = 1,
+		TWO_STOP_BITS = 2,
+	};
+	enum {
+		NO_PARITY = 0,
+		ODD_PARITY = 1,
+		EVEN_PARITY = 2,
+		MARK_PARITY = 3,
+		SPACE_PARITY = 4,
+	};
+
+private:
+	USBDevice_ &usb;
+	RingBuffer *_cdc_rx_buffer;
 };
 extern Serial_ SerialUSB;
@@ -171,19 +236,4 @@
 //================================================================================
 //================================================================================
-//	Low level API
-
-typedef struct
-{
-	uint8_t bmRequestType;
-	uint8_t bRequest;
-	uint8_t wValueL;
-	uint8_t wValueH;
-	uint16_t wIndex;
-	uint16_t wLength;
-	uint8_t data[8];
-} Setup;
-
-//================================================================================
-//================================================================================
 //	HID 'Driver'
 
@@ -202,5 +252,5 @@
 uint32_t		MSC_GetInterface(uint8_t* interfaceNum);
 uint32_t		MSC_GetDescriptor(uint32_t i);
-bool	MSC_Setup(Setup& setup);
+bool	MSC_Setup(Setup& pSetup);
 bool	MSC_Data(uint8_t rx,uint8_t tx);
 
@@ -209,15 +259,18 @@
 //	CDC 'Driver'
 
-const void* CDC_GetInterface(/*uint8_t* interfaceNum*/);
-uint32_t WEAK CDC_GetInterfaceLength(void);
+int CDC_GetInterface(uint8_t* interfaceNum);
+const void* _CDC_GetInterface(void);
+uint32_t _CDC_GetInterfaceLength(void);
 uint32_t		CDC_GetOtherInterface(uint8_t* interfaceNum);
 uint32_t		CDC_GetDescriptor(uint32_t i);
-bool	CDC_Setup(Setup& setup);
-
-//================================================================================
-//================================================================================
-
-uint32_t USBD_SendControl(uint8_t flags, const void* d, uint32_t len);
+bool	CDC_Setup(Setup& pSetup);
+
+
+//================================================================================
+//================================================================================
+
+uint32_t USBD_SendControl(const void* _data, uint32_t len);
 uint32_t USBD_RecvControl(void* d, uint32_t len);
+void USBD_Calibrate();
 uint32_t USBD_SendInterfaces(void);
 bool USBD_ClassInterfaceRequest(Setup& setup);
@@ -228,7 +281,7 @@
 uint32_t USBD_Recv(uint32_t ep, void* data, uint32_t len);		// non-blocking
 uint32_t USBD_Recv(uint32_t ep);							// non-blocking
+uint8_t USBD_armRecv(uint32_t ep);
 void USBD_Flush(uint32_t ep);
 uint32_t USBD_Connected(void);
 
 #endif  // __cplusplus
-#endif  // __USBAPI__
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBCore.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBCore.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBCore.cpp	(revision 224)
@@ -1,60 +1,39 @@
-// Copyright (c) 2010, Peter Barrett
 /*
-** Permission to use, copy, modify, and/or distribute this software for
-** any purpose with or without fee is hereby granted, provided that the
-** above copyright notice and this permission notice appear in all copies.
-**
-** THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
-** WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
-** WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR
-** BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES
-** OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
-** WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION,
-** ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS
-** SOFTWARE.
+  Copyright (c) 2015 Arduino LLC.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+  See the GNU Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */
+/*
+**Modified 04/04/2016 by Arduino.org development team
+*/
+
+#include <Arduino.h>
+
+#include "SAMD21_USBDevice.h"
+#include "USB_host.h"
 
 #include <stdlib.h>
 #include <stdio.h>
 #include <stdint.h>
-//#include "../Arduino.h"
-
-#include "sam.h"
-#include "wiring_constants.h"
-#include "USBCore.h"
-#include "USB/USB_device.h"   // needed for USB PID define
-#include "USBDesc.h"
-#include "USBAPI.h"
-
-#define TRACE_CORE(x)
-
-//==================================================================
-
-
-
-/*
-#if (defined CDC_ENABLED) && defined(HID_ENABLED)
-//#define USB_PID_ZERO       0x004B  // CDC and HID
-#define USB_PID_ZERO       0x804d  // CDC only
-#else
-#if (defined CDC_ENABLED)
-#define USB_PID_ZERO       0x804d  // CDC only  usbserial.name
-#else
-#define USB_PID_ZERO       0x804d  // HID only
-#endif
-#endif
-*/
-
-#define USB_PID_ZERO_PRO	0x804d
-
-// USB Device
-//#define USB_VID 			0x2a03 // arduino srl vid
-//#undef USB_PID
-//#define USB_PID            USB_PID_ZERO
-
-//==================================================================
+#include <limits.h>
+
+USBDevice_SAMD21G18x usbd;
 
 static char isRemoteWakeUpEnabled = 0;
 static char isEndpointHalt = 0;
+
+extern void (*gpf_isr)(void);
 
 
@@ -64,4 +43,6 @@
 };
 
+#define USB_PID_ZERO_PRO	0x804d
+#define USB_PRODUCT "Arduino M0 Pro"
 #ifndef USB_PRODUCT
 // Use a hardcoded product name if none is provided
@@ -88,6 +69,4 @@
 
 
-
-//	DEVICE DESCRIPTOR
 #if (defined CDC_ENABLED) && defined(HID_ENABLED)
 const DeviceDescriptor USB_DeviceDescriptor =
@@ -101,673 +80,771 @@
 #endif
 
+
 //==================================================================
 
 volatile uint32_t _usbConfiguration = 0;
-volatile uint32_t _usbInitialized = 0;
 volatile uint32_t _usbSetInterface = 0;
 
+static __attribute__((__aligned__(4))) //__attribute__((__section__(".bss_hram0")))
+uint8_t udd_ep_out_cache_buffer[7][64];
+
+static __attribute__((__aligned__(4))) //__attribute__((__section__(".bss_hram0")))
+uint8_t udd_ep_in_cache_buffer[7][64];
+
 //==================================================================
 
-
-//	Number of bytes, assumes a rx endpoint
-uint32_t USBD_Available(uint32_t ep)
-{
-	return UDD_FifoByteCount(ep);
-}
-
-//	Non Blocking receive
-//	Return number of bytes read
-uint32_t USBD_Recv(uint32_t ep, void* d, uint32_t len)
-{
-	if (!_usbConfiguration || len < 0)
+bool _dry_run = false;
+bool _pack_message = false;
+uint16_t _pack_size = 0;
+uint8_t _pack_buffer[256];
+
+
+void packMessages(bool val)
+{
+	if (val) {
+		_pack_message = true;
+		_pack_size = 0;
+	} else {
+		_pack_message = false;
+		USBD_SendControl(_pack_buffer, _pack_size);
+	}
+}
+
+// Blocking Send of data to an endpoint
+uint32_t USBD_Send(uint32_t ep, const void *data, uint32_t len)
+{
+	uint32_t length = 0;
+
+	if (!_usbConfiguration)
 		return -1;
-
-	uint32_t n = UDD_FifoByteCount(ep);
-	len = min(n,len);
-	n = len;
-	uint8_t* dst = (uint8_t*)d;
-	while (n--)
-		*dst++ = UDD_Recv8(ep);
-	
-	
-	
-	
-	if (len && !UDD_FifoByteCount(ep)) // release empty buffer
-		UDD_ReleaseRX(ep);
-		
-  //----- Tx & Rx led blinking during transmission ----- begin ----	
-	PORT->Group[1].OUTTGL.reg =0x00000008 ;  //RxLED
-	for(int i=0; i < 100000; i++)
-	{
-		asm("NOP");
-	}
-	PORT->Group[1].OUTTGL.reg =0x00000008 ;	
-  //----- Tx & Rx led blinking during transmission ----- end ----
+	if (len > 16384)
+		return -1;
+
+	PORT->Group[0].OUTCLR.reg =0x08000000 ; //TxLED on
+	// Flash area
+	while (len != 0)
+	{
+		if (len >= 64) {
+			length = 64;
+		} else {
+			length = len;
+		}
+
+		/* memcopy could be safer in multi threaded environment */
+		memcpy(&udd_ep_in_cache_buffer[ep], data, length);
+
+		usbd.epBank1SetAddress(ep, &udd_ep_in_cache_buffer[ep]);
+
+		usbd.epBank1SetByteCount(ep, length);
+
+		// Clear the transfer complete flag
+		usbd.epBank1AckTransferComplete(ep);
+
+		// RAM buffer is full, we can send data (IN)
+		usbd.epBank1SetReady(ep);
+
+		// Wait for transfer to complete
+		while (!usbd.epBank1IsTransferComplete(ep)) {
+			;  // need fire exit.
+		}
+		len -= length;
+		data = (char *)data + length;
+	}
 	return len;
 }
 
-//	Recv 1 byte if ready
-uint32_t USBD_Recv(uint32_t ep)
-{
-	uint8_t c;
-	if (USBD_Recv(ep, &c, 1) != 1)
-		return -1;
-	else
-		return c;
-}
-
-//	Blocking Send of data to an endpoint
-uint32_t USBD_Send(uint32_t ep, const void* d, uint32_t len)
-{
-	int r = len;
-	const uint8_t* data = (const uint8_t*)d;
-
-    if (!_usbConfiguration)
-    {
-    	TRACE_CORE(printf("pb conf\n\r");)
-		return -1;
-    }
-	UDD_Send(ep, data, len);
-
-	/* Clear the transfer complete flag  */
-	udd_clear_transf_cplt(ep);
-	/* Set the bank as ready */
-	udd_bk_rdy(ep);
-
-	/* Wait for transfer to complete */
-	while (! udd_is_transf_cplt(ep));  // need fire exit.
-	
-  //----- Tx & Rx led blinking during transmission ----- begin ----
-	PORT->Group[0].OUTTGL.reg =0x08000000 ; //TxLED
-	for(int i=0; i < 100000; i++)
-	{
-		asm("NOP");
-	}
-	PORT->Group[0].OUTTGL.reg =0x08000000 ;
-	/*for(int i=0; i < 100000; i++)
-	{
-		asm("NOP");
-	}*/
-  //----- Tx & Rx led blinking during transmission ----- end ----	
-	return r;
-}
-
-uint32_t USBD_SendControl(uint8_t flags, const void* d, uint32_t len)
-{
-	const uint8_t* data = (const uint8_t*)d;
+uint32_t USBD_armSend(uint32_t ep, const void* data, uint32_t len)
+{
+	memcpy(&udd_ep_in_cache_buffer[ep], data, len);
+
+	// Get endpoint configuration from setting register
+	usbd.epBank1SetAddress(ep, &udd_ep_in_cache_buffer[ep]);
+	usbd.epBank1SetMultiPacketSize(ep, 0);
+	usbd.epBank1SetByteCount(ep, len);
+
+	return len;
+}
+
+uint32_t USBD_SendControl(const void* _data, uint32_t len)
+{
+	const uint8_t *data = reinterpret_cast<const uint8_t *>(_data);
 	uint32_t length = len;
 	uint32_t sent = 0;
 	uint32_t pos = 0;
 
-	TRACE_CORE(printf("=> USBD_SendControl TOTAL len=%lu\r\n", len);)
-
- 		while (len > 0)
- 		{
-			sent = UDD_Send(EP0, data + pos, len);
-			TRACE_CORE(printf("=> USBD_SendControl sent=%lu\r\n", sent);)
-			pos += sent;
-			len -= sent;
- 		}
+	if (_dry_run == true)
+		return length;
+
+	if (_pack_message == true) {
+		memcpy(&_pack_buffer[_pack_size], data, len);
+		_pack_size += len;
+		return length;
+	}
+
+ 	while (len > 0)
+ 	{
+		sent = USBD_armSend(EP0, data + pos, len);
+		pos += sent;
+		len -= sent;
+ 	}
 
 	return length;
 }
 
-// Send a USB descriptor string. The string is stored as a
-// plain ASCII string but is sent out as UTF-16 with the
-// correct 2-byte prefix
-static bool USB_SendStringDescriptor(const uint8_t *string, int wLength)
-{
-	uint16_t buff[64];
+bool USBD_SendStringDescriptor(const uint8_t *string, uint8_t maxlen)
+{
+	if (maxlen < 2)
+		return false;
+
+	uint16_t buff[maxlen/2];
 	int l = 1;
 
-	wLength -= 2;
-	while (*string && wLength>0)
+	maxlen -= 2;
+	while (*string && maxlen>0)
 	{
 		buff[l++] = (uint8_t)(*string++);
-		wLength -= 2;
+		maxlen -= 2;
 	}
 	buff[0] = (3<<8) | (l*2);
 
-	return USBD_SendControl(0, (uint8_t*)buff, l*2);
-}
-
-uint32_t USBD_RecvControl(void* d, uint32_t len)
-{
-	udd_ack_out_received(0);
-
-	return len;
-}
-
-//	Handle CLASS_INTERFACE requests
-bool USBD_ClassInterfaceRequest(Setup& setup)
-{
-	uint8_t i = setup.wIndex;
-
-	TRACE_CORE(printf("=> USBD_ClassInterfaceRequest\r\n");)
-
-#ifdef CDC_ENABLED
-	if (CDC_ACM_INTERFACE == i)
-	{
-		if( CDC_Setup(setup) == false )
-		{
-			send_zlp();
-		}
+	return USBD_SendControl((uint8_t*)buff, l*2);
+}
+
+uint8_t USBD_SendInterfaces(uint32_t* total)
+{
+	uint8_t interfaces = 0;
+
+#if defined(CDC_ENABLED)
+	total[0] += CDC_GetInterface(&interfaces);
+#endif
+
+	return interfaces;
+}
+
+// Construct a dynamic configuration descriptor
+// This really needs dynamic endpoint allocation etc
+uint32_t USBD_SendConfiguration(uint32_t maxlen)
+{
+	uint32_t total = 0;
+	// Count and measure interfaces
+	_dry_run = true;
+	uint8_t interfaces = USBD_SendInterfaces(&total);
+
+	_Pragma("pack(1)")
+	ConfigDescriptor config = D_CONFIG((uint16_t)(total + sizeof(ConfigDescriptor)), interfaces);
+	_Pragma("pack()")
+
+	//	Now send them
+	_dry_run = false;
+
+	if (maxlen == sizeof(ConfigDescriptor)) {
+		USBD_SendControl(&config, sizeof(ConfigDescriptor));
 		return true;
 	}
-#endif
-
-#ifdef HID_ENABLED
-	if (HID_INTERFACE == i)
-	{
-		if( HID_Setup(setup) == true )
-		{
-			send_zlp();
-		}
-		return true;
-	}
-#endif
-
-	return false;
-}
-
-//	Construct a dynamic configuration descriptor
-//	This really needs dynamic endpoint allocation etc
-//	TODO
-static bool USBD_SendConfiguration(uint32_t maxlen)
-{
-	uint8_t cache_buffer[128];
-	uint8_t i;
-
-	const uint8_t* interfaces;
-	uint32_t interfaces_length = 0;
-	uint8_t num_interfaces[1];
-	
-	num_interfaces[0] = 0;
-
-#if (defined CDC_ENABLED) && defined(HID_ENABLED)
-    num_interfaces[0] += 3;
-    interfaces = (const uint8_t*) CDC_GetInterface();
-    interfaces_length = CDC_GetInterfaceLength() + HID_GetInterfaceLength();
-    if( maxlen > CDC_GetInterfaceLength() + HID_GetInterfaceLength() + sizeof(ConfigDescriptor) )
-    {
-	    maxlen = CDC_GetInterfaceLength() + HID_GetInterfaceLength() + sizeof(ConfigDescriptor);
-    }
-
-#else
-#ifdef CDC_ENABLED
-    num_interfaces[0] += 2;
-	interfaces = (const uint8_t*) CDC_GetInterface();
-	interfaces_length += CDC_GetInterfaceLength();
-	if( maxlen > CDC_GetInterfaceLength()+ sizeof(ConfigDescriptor) )
-	{
-		maxlen = CDC_GetInterfaceLength()+ sizeof(ConfigDescriptor);
-	}
-#endif
-
-#ifdef HID_ENABLED
-    num_interfaces[0] += 1;
-	interfaces = (const uint8_t*) HID_GetInterface();
-	interfaces_length += HID_GetInterfaceLength();
-	if( maxlen > HID_GetInterfaceLength()+ sizeof(ConfigDescriptor) )
-	{
-		maxlen = HID_GetInterfaceLength()+ sizeof(ConfigDescriptor);
-	}
-#endif
-#endif
-
-_Pragma("pack(1)")
-	ConfigDescriptor config = D_CONFIG((uint16_t)(interfaces_length + sizeof(ConfigDescriptor)),num_interfaces[0]);
-_Pragma("pack()")
-
-	memcpy( cache_buffer, &config, sizeof(ConfigDescriptor) );
-
-#if (defined CDC_ENABLED) && defined(HID_ENABLED)
-	for ( i=0; i<CDC_GetInterfaceLength(); i++)
-	{
-		cache_buffer[i+sizeof(ConfigDescriptor)] = interfaces[i];
-	}
-	interfaces = (const uint8_t*) HID_GetInterface();
-	for ( i=0; i<HID_GetInterfaceLength(); i++)
-	{
-		cache_buffer[i+sizeof(ConfigDescriptor)+CDC_GetInterfaceLength()] = interfaces[i];
-	}
-#else
-#ifdef HID_ENABLED
-	for ( i=0; i<interfaces_length; i++)
-	{
-		cache_buffer[i+sizeof(ConfigDescriptor)] = interfaces[i];
-	}
-#endif
-
-#ifdef CDC_ENABLED
-	for ( i=0; i<interfaces_length; i++)
-	{
-		cache_buffer[i+sizeof(ConfigDescriptor)] = interfaces[i];
-	}
-#endif	
-#endif
-
-	if (maxlen > sizeof(cache_buffer))
-	{
-		 maxlen = sizeof(cache_buffer);
-	}
-	USBD_SendControl(0,cache_buffer, maxlen );
+
+	total = 0;
+
+	packMessages(true);
+	USBD_SendControl(&config, sizeof(ConfigDescriptor));
+	USBD_SendInterfaces(&total);
+	packMessages(false);
+
 	return true;
 }
 
-static bool USBD_SendDescriptor(Setup* pSetup)
-{
-	uint8_t t = pSetup->wValueH;
+bool USBD_SendDescriptor(Setup &setup)
+{
+	uint8_t t = setup.wValueH;
 	uint8_t desc_length = 0;
-	const uint8_t* desc_addr = 0;
-
-	if (USB_CONFIGURATION_DESCRIPTOR_TYPE == t)
-	{
-		TRACE_CORE(printf("=> USBD_SendDescriptor : USB_CONFIGURATION_DESCRIPTOR_TYPE length=%d\r\n", setup.wLength);)
-		return USBD_SendConfiguration(pSetup->wLength);
-	}
-
-#ifdef HID_ENABLED
-	if (HID_REPORT_DESCRIPTOR_TYPE == t)
-	{
-		TRACE_CORE(puts("=> USBD_SendDescriptor : HID_REPORT_DESCRIPTOR_TYPE\r\n");)
-		return HID_GetDescriptor();
-	}
-	if (HID_HID_DESCRIPTOR_TYPE == t)
-	{
-		uint8_t tab[9] = D_HIDREPORT((uint8_t)HID_SizeReportDescriptor());
-
-		TRACE_CORE(puts("=> USBD_SendDescriptor : HID_HID_DESCRIPTOR_TYPE\r\n");)
-
-		return USBD_SendControl(0, tab, sizeof(tab));
-	}
-#endif
-
-	if (USB_DEVICE_DESCRIPTOR_TYPE == t)
-	{
-		TRACE_CORE(puts("=> USBD_SendDescriptor : USB_DEVICE_DESCRIPTOR_TYPE\r\n");)
-		desc_addr = (const uint8_t*)&USB_DeviceDescriptor;
-        if( *desc_addr > pSetup->wLength ) {
-            desc_length = pSetup->wLength;
-        }
+	bool _cdcComposite;
+	int ret;
+	const uint8_t *desc_addr = 0;
+
+	if (t == USB_CONFIGURATION_DESCRIPTOR_TYPE)
+	{
+		return USBD_SendConfiguration(setup.wLength);
+	}
+
+	if (t == USB_DEVICE_DESCRIPTOR_TYPE)
+	{
+		if (setup.wLength == 8)
+			_cdcComposite = 1;
+
+		desc_addr = _cdcComposite ?  (const uint8_t*)&USB_DeviceDescriptor : (const uint8_t*)&USB_DeviceDescriptor;
+
+		if (*desc_addr > setup.wLength) {
+			desc_length = setup.wLength;
+		}
 	}
 	else if (USB_STRING_DESCRIPTOR_TYPE == t)
 	{
-		TRACE_CORE(puts("=> USBD_SendDescriptor : USB_STRING_DESCRIPTOR_TYPE\r\n");)
-		if (pSetup->wValueL == 0) {
+		if (setup.wValueL == 0) {
 			desc_addr = (const uint8_t*)&STRING_LANGUAGE;
 		}
-		else if (pSetup->wValueL == IPRODUCT) {
-			return USB_SendStringDescriptor(STRING_PRODUCT, pSetup->wLength);
-		}
-		else if (pSetup->wValueL == IMANUFACTURER) {
-			return USB_SendStringDescriptor(STRING_MANUFACTURER, pSetup->wLength);
+		else if (setup.wValueL == IPRODUCT) {
+			return USBD_SendStringDescriptor(STRING_PRODUCT, setup.wLength);
+		}
+		else if (setup.wValueL == IMANUFACTURER) {
+			return USBD_SendStringDescriptor(STRING_MANUFACTURER, setup.wLength);
 		}
 		else {
 			return false;
 		}
-		if( *desc_addr > pSetup->wLength ) {
-			desc_length = pSetup->wLength;
-		}
-	}
-    else
-    {
-        TRACE_CORE(printf("Device ERROR");)
-    }
-
-	if (desc_addr == 0)
-	{
+		if (*desc_addr > setup.wLength) {
+			desc_length = setup.wLength;
+		}
+	}
+	else
+	{
+	}
+
+	if (desc_addr == 0) {
 		return false;
 	}
 
-	if (desc_length == 0)
-	{
+	if (desc_length == 0) {
 		desc_length = *desc_addr;
 	}
 
-	TRACE_CORE(printf("=> USBD_SendDescriptor : desc_addr=%p desc_length=%d\r\n", desc_addr, desc_length);)
-	USBD_SendControl(0, desc_addr, desc_length);
+	USBD_SendControl(desc_addr, desc_length);
 
 	return true;
 }
 
-
-void EndpointHandler(uint8_t bEndpoint)
-{
+void initEP(uint32_t ep, uint32_t config)
+{
+	if (config == (USB_ENDPOINT_TYPE_INTERRUPT | USB_ENDPOINT_IN(0)))
+	{
+		usbd.epBank1SetSize(ep, 64);
+		usbd.epBank1SetAddress(ep, &udd_ep_in_cache_buffer[ep]);
+		usbd.epBank1SetType(ep, 4); // INTERRUPT IN
+	}
+	else if (config == (USB_ENDPOINT_TYPE_BULK | USB_ENDPOINT_OUT(0)))
+	{
+		usbd.epBank0SetSize(ep, 64);
+		usbd.epBank0SetAddress(ep, &udd_ep_out_cache_buffer[ep]);
+		usbd.epBank0SetType(ep, 3); // BULK OUT
+
+		// Release OUT EP
+		usbd.epBank0SetMultiPacketSize(ep, 64);
+		usbd.epBank0SetByteCount(ep, 0);
+	}
+	else if (config == (USB_ENDPOINT_TYPE_BULK | USB_ENDPOINT_IN(0)))
+	{
+		usbd.epBank1SetSize(ep, 64);
+		usbd.epBank1SetAddress(ep, &udd_ep_in_cache_buffer[ep]);
+
+		// NAK on endpoint IN, the bank is not yet filled in.
+		usbd.epBank1ResetReady(ep);
+
+		usbd.epBank1SetType(ep, 3); // BULK IN
+	}
+	else if (config == USB_ENDPOINT_TYPE_CONTROL)
+	{
+		// XXX: Needed?
+// 		usbd.epBank0DisableAutoZLP(ep);
+// 		usbd.epBank1DisableAutoZLP(ep);
+
+		// Setup Control OUT
+		usbd.epBank0SetSize(ep, 64);
+		usbd.epBank0SetAddress(ep, &udd_ep_out_cache_buffer[ep]);
+		usbd.epBank0SetType(ep, 1); // CONTROL OUT / SETUP
+
+		// Setup Control IN
+		usbd.epBank1SetSize(ep, 64);
+		usbd.epBank1SetAddress(ep, &udd_ep_in_cache_buffer[0]);
+		usbd.epBank1SetType(ep, 1); // CONTROL IN
+
+		// Release OUT EP
+		usbd.epBank0SetMultiPacketSize(ep, 64);
+		usbd.epBank0SetByteCount(ep, 0);
+
+		// NAK on endpoint OUT, the bank is full.
+		usbd.epBank0SetReady(ep);
+	}
+}
+
+void sendZlp(uint32_t ep)
+{
+	// Set the byte count as zero
+	usbd.epBank1SetByteCount(ep, 0);
+}
+
+void setAddress(uint32_t addr)
+{
+	usbd.epBank1SetByteCount(0, 0);
+	usbd.epBank1AckTransferComplete(0);
+
+	// RAM buffer is full, we can send data (IN)
+	usbd.epBank1SetReady(0);
+
+	// Wait for transfer to complete
+	while (!usbd.epBank1IsTransferComplete(0)) {}
+
+	// Set USB address to addr
+	USB->DEVICE.DADD.bit.DADD = addr; // Address
+	USB->DEVICE.DADD.bit.ADDEN = 1; // Enable
+}
+
+uint32_t EndPoints[] =
+{
+	USB_ENDPOINT_TYPE_CONTROL,
+
 #ifdef CDC_ENABLED
-	if( bEndpoint == CDC_ENDPOINT_OUT )
-	{
-		udd_ack_out_received(CDC_ENDPOINT_OUT);
-
-		// Handle received bytes		
+	USB_ENDPOINT_TYPE_INTERRUPT | USB_ENDPOINT_IN(0),           // CDC_ENDPOINT_ACM
+	USB_ENDPOINT_TYPE_BULK      | USB_ENDPOINT_OUT(0),               // CDC_ENDPOINT_OUT
+	USB_ENDPOINT_TYPE_BULK | USB_ENDPOINT_IN(0),                // CDC_ENDPOINT_IN
+#endif
+
+};
+
+void initEndpoints() {
+	for (uint8_t i = 1; i < sizeof(EndPoints) && EndPoints[i] != 0; i++) {
+		initEP(i, EndPoints[i]);
+	}
+}
+
+bool handleStandardSetup(Setup &setup)
+{
+	switch (setup.bRequest) {
+	case GET_STATUS:
+		if (setup.bmRequestType == 0)  // device
+		{
+			// Send the device status
+			// TODO: Check current configuration for power mode (if device is configured)
+			// TODO: Check if remote wake-up is enabled
+			uint8_t buff[] = { 0, 0 };
+			USBD_armSend(0, buff, 2);
+			return true;
+		}
+		// if( setup.bmRequestType == 2 ) // Endpoint:
+		else
+		{
+			// Send the endpoint status
+			// Check if the endpoint if currently halted
+			uint8_t buff[] = { 0, 0 };
+			if (isEndpointHalt == 1)
+				buff[0] = 1;
+			USBD_armSend(0, buff, 2);
+			return true;
+		}
+
+	case CLEAR_FEATURE:
+		// Check which is the selected feature
+		if (setup.wValueL == 1) // DEVICEREMOTEWAKEUP
+		{
+			// Enable remote wake-up and send a ZLP
+			uint8_t buff[] = { 0, 0 };
+			if (isRemoteWakeUpEnabled == 1)
+				buff[0] = 1;
+			USBD_armSend(0, buff, 2);
+			return true;
+		}
+		else // if( setup.wValueL == 0) // ENDPOINTHALT
+		{
+			isEndpointHalt = 0;
+			sendZlp(0);
+			return true;
+		}
+
+	case SET_FEATURE:
+		// Check which is the selected feature
+		if (setup.wValueL == 1) // DEVICEREMOTEWAKEUP
+		{
+			// Enable remote wake-up and send a ZLP
+			isRemoteWakeUpEnabled = 1;
+			uint8_t buff[] = { 0 };
+			USBD_armSend(0, buff, 1);
+			return true;
+		}
+		if (setup.wValueL == 0) // ENDPOINTHALT
+		{
+			// Halt endpoint
+			isEndpointHalt = 1;
+			sendZlp(0);
+			return true;
+		}
+
+	case SET_ADDRESS:
+		setAddress(setup.wValueL);
+		return true;
+
+	case GET_DESCRIPTOR:
+		return USBD_SendDescriptor(setup);
+
+	case SET_DESCRIPTOR:
+		return false;
+
+	case GET_CONFIGURATION:
+		USBD_armSend(0, (void*)&_usbConfiguration, 1);
+		return true;
+
+	case SET_CONFIGURATION:
+		if (REQUEST_DEVICE == (setup.bmRequestType & REQUEST_RECIPIENT)) {
+
+			initEndpoints();
+			_usbConfiguration = setup.wValueL;
+
+			#if defined(CDC_ENABLED)
+			// Enable interrupt for CDC reception from host (OUT packet)
+			usbd.epBank1EnableTransferComplete(CDC_ENDPOINT_ACM);
+			usbd.epBank0EnableTransferComplete(CDC_ENDPOINT_OUT);
+			#endif
+
+			sendZlp(0);
+			return true;
+		} else {
+			return false;
+		}
+
+	case GET_INTERFACE:
+		USBD_armSend(0, (void*)&_usbSetInterface, 1);
+		return true;
+
+	case SET_INTERFACE:
+		_usbSetInterface = setup.wValueL;
+		sendZlp(0);
+		return true;
+
+	default:
+		return true;
+	}
+}
+
+bool handleClassInterfaceSetup(Setup& setup)
+{
+	uint8_t i = setup.wIndex;
+
+	#if defined(CDC_ENABLED)
+	if (CDC_ACM_INTERFACE == i)
+	{
+		if (CDC_Setup(setup) == false) {
+			sendZlp(0);
+		}
+		return true;
+	}
+	#endif
+
+	return false;
+}
+
+void stall(uint32_t ep)
+{
+	// TODO: test
+	// TODO: use .bit. notation
+
+	// Stall endpoint
+	USB->DEVICE.DeviceEndpoint[ep].EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_STALLRQ(2);
+}
+
+void handleEndpoint(uint8_t ep)
+{
+#if defined(CDC_ENABLED)
+	if (ep == CDC_ENDPOINT_OUT)
+	{
+		// The RAM Buffer is empty: we can receive data
+		//usbd.epBank0ResetReady(CDC_ENDPOINT_OUT);
+
+		// Handle received bytes
 		if (USBD_Available(CDC_ENDPOINT_OUT))
+			SerialUSB.accept();
+	}
+	if (ep == CDC_ENDPOINT_IN)
+	{
+		// NAK on endpoint IN, the bank is not yet filled in.
+		usbd.epBank1ResetReady(CDC_ENDPOINT_IN);
+		usbd.epBank1AckTransferComplete(CDC_ENDPOINT_IN);
+	}
+	if (ep == CDC_ENDPOINT_ACM)
+	{
+		// NAK on endpoint IN, the bank is not yet filled in.
+		usbd.epBank1ResetReady(CDC_ENDPOINT_ACM);
+		usbd.epBank1AckTransferComplete(CDC_ENDPOINT_ACM);
+	}
+#endif
+
+}
+
+void ISRHandler()
+{
+
+	if (_pack_message == true) {
+		return;
+	}
+	// End-Of-Reset
+	if (usbd.isEndOfResetInterrupt())
+	{
+		// Configure EP 0
+		initEP(0, USB_ENDPOINT_TYPE_CONTROL);
+
+		// Enable Setup-Received interrupt
+		usbd.epBank0EnableSetupReceived(0);
+
+		_usbConfiguration = 0;
+
+		usbd.ackEndOfResetInterrupt();
+	}
+
+	// Start-Of-Frame
+	if (usbd.isStartOfFrameInterrupt())
+	{
+		usbd.ackStartOfFrameInterrupt();
+	}
+
+	// Endpoint 0 Received Setup interrupt
+	if (usbd.epBank0IsSetupReceived(0))
+	{
+		usbd.epBank0AckSetupReceived(0);
+
+		Setup *setup = reinterpret_cast<Setup *>(udd_ep_out_cache_buffer[0]);
+
+		/* Clear the Bank 0 ready flag on Control OUT */
+		// The RAM Buffer is empty: we can receive data
+		usbd.epBank0ResetReady(0);
+
+		bool ok;
+		if (REQUEST_STANDARD == (setup->bmRequestType & REQUEST_TYPE)) {
+			// Standard Requests
+			ok = handleStandardSetup(*setup);
+		} else {
+			// Class Interface Requests
+			ok = handleClassInterfaceSetup(*setup);
+		}
+
+		if (ok) {
+			usbd.epBank1SetReady(0);
+		} else {
+			stall(0);
+		}
+
+		if (usbd.epBank1IsStalled(0))
 		{
-			SerialUSB.accept();
-		}
-	}
-	if( bEndpoint == CDC_ENDPOINT_IN )
-	{
-		udd_ack_in_received(CDC_ENDPOINT_IN);
-		/* Clear the transfer complete flag  */
-		udd_clear_transf_cplt(CDC_ENDPOINT_IN);
-		
-	}
-	if( bEndpoint == CDC_ENDPOINT_ACM )
-	{
-		udd_ack_in_received(CDC_ENDPOINT_ACM);
-		/* Clear the transfer complete flag  */
-		udd_clear_transf_cplt(CDC_ENDPOINT_ACM);
-	}
-#endif
-
-#ifdef HID_ENABLED
-	/* Nothing to do in our example */
-#endif
-}
-
-
-void USB_Handler(void)
-{
-	uint16_t flags;
-	uint8_t i;
-	uint8_t ept_int;
-
-	ept_int = udd_endpoint_interrupt();
+			usbd.epBank1AckStalled(0);
+
+			// Remove stall request
+			usbd.epBank1DisableStalled(0);
+		}
 	
-	/* Not endpoint interrupt */
-	if (0 == ept_int)
-	{
-		udd_clear_wakeup_interrupt();
-		udd_clear_eorsm_interrupt();
-		udd_clear_suspend_interrupt();
-
-		// End of bus reset
-		if (Is_udd_reset())
+	} // end Received Setup handler
+	PORT->Group[0].OUTSET.reg =0x08000000 ; //TxLED off
+	PORT->Group[1].OUTSET.reg =0x00000008 ; //RxLED off
+
+	uint8_t i=0;
+	uint8_t ept_int = usbd.epInterruptSummary() & 0xFE; // Remove endpoint number 0 (setup)
+	while (ept_int != 0)
+	{
+		// Check if endpoint has a pending interrupt
+		if ((ept_int & (1 << i)) != 0)
 		{
-			TRACE_CORE(printf(">>> End of Reset\r\n");)
-			// Reset USB address to 0
-			udd_configure_address(0);
-
-			// Configure EP 0
-			UDD_InitEP(0, USB_ENDPOINT_TYPE_CONTROL);
-			udd_enable_setup_received_interrupt(0);
-			_usbConfiguration = 0;
-			udd_ack_reset();
-		}
-
-		if (Is_udd_sof())
-		{
-			udd_ack_sof();
-		}
-
-	}
-	else 
-	{
-		// Endpoint interrupt
-		flags = udd_read_endpoint_flag(0);
-
-		// endpoint received setup interrupt
-		if (flags & USB_DEVICE_EPINTFLAG_RXSTP)
-		{
-			Setup *pSetupData;
-
-			/* Clear the Received Setup flag */
-			udd_read_endpoint_flag(0) = USB_DEVICE_EPINTFLAG_RXSTP;
-
-			UDD_Recv(EP0, (uint8_t**)&pSetupData);
-
-			/* Clear the Bank 0 ready flag on Control OUT */
-			udd_ack_out_received(0);
-
-			bool ok = true;
-			if (REQUEST_STANDARD == (pSetupData->bmRequestType & REQUEST_TYPE))
+			// Endpoint Transfer Complete (0/1) Interrupt
+			if (usbd.epBank0IsTransferComplete(i) ||
+			    usbd.epBank1IsTransferComplete(i))
 			{
-				unsigned char data_to_be_send[2];
-
-				// Standard Requests
-				uint8_t r = pSetupData->bRequest;
-				if (GET_STATUS == r)
-				{
-					if( pSetupData->bmRequestType == 0 )  // device
-					{
-						// Send the device status
-     					TRACE_CORE(puts(">>> EP0 Int: GET_STATUS\r\n");)
-						// Check current configuration for power mode (if device is configured)
-						
-						// Check if remote wake-up is enabled
-						
-						data_to_be_send[0]=0;
-						data_to_be_send[1]=0;
-						UDD_Send(0, data_to_be_send, 2);
-					}
-					// if( pSetupData->bmRequestType == 2 ) // Endpoint:
-					else
-					{
-						// Send the endpoint status
-						// Check if the endpoint if currently halted
- 						if( isEndpointHalt == 1 )
-							data_to_be_send[0]=1;
- 						else
-							data_to_be_send[0]=0;
-						data_to_be_send[1]=0;
-						UDD_Send(0, data_to_be_send, 2);
-					}
-				}
-				else if (CLEAR_FEATURE == r)
-				{
-				   // Check which is the selected feature
-					if( pSetupData->wValueL == 1) // DEVICEREMOTEWAKEUP
-					{
-						// Enable remote wake-up and send a ZLP
-						if( isRemoteWakeUpEnabled == 1 )
-							data_to_be_send[0]=1;
-						else
-							data_to_be_send[0]=0;
-						data_to_be_send[1]=0;
-						UDD_Send(0, data_to_be_send, 2);
-					}
-					else // if( pSetupData->wValueL == 0) // ENDPOINTHALT
-					{
-						isEndpointHalt = 0;
-						send_zlp();
-					}
- 				}
-				else if (SET_FEATURE == r)
-				{
-					// Check which is the selected feature
-					if( pSetupData->wValueL == 1) // DEVICEREMOTEWAKEUP
-					{
-						// Enable remote wake-up and send a ZLP
-						isRemoteWakeUpEnabled = 1;
-	    				data_to_be_send[0] = 0;
-						UDD_Send(0, data_to_be_send, 1);
-					}
-					if( pSetupData->wValueL == 0) // ENDPOINTHALT
-					{
-						// Halt endpoint
-						isEndpointHalt = 1;
-						send_zlp();
-					}
-				}
-				else if (SET_ADDRESS == r)
-				{
-					TRACE_CORE(puts(">>> EP0 Int: SET_ADDRESS\r\n");)
-					UDD_SetAddress(pSetupData->wValueL);
-				}
-				else if (GET_DESCRIPTOR == r)
-				{
-					TRACE_CORE(puts(">>> EP0 Int: GET_DESCRIPTOR\r\n");)
-					ok = USBD_SendDescriptor(pSetupData);
-				}
-				else if (SET_DESCRIPTOR == r)
-				{
-					TRACE_CORE(puts(">>> EP0 Int: SET_DESCRIPTOR\r\n");)
-					ok = false;
-				}
-				else if (GET_CONFIGURATION == r)
-				{
-					TRACE_CORE(puts(">>> EP0 Int: GET_CONFIGURATION\r\n");)
-					UDD_Send(0, (void*)&_usbConfiguration, 1);
-				}
-				else if (SET_CONFIGURATION == r)
-				{
-					if (REQUEST_DEVICE == (pSetupData->bmRequestType & REQUEST_RECIPIENT))
-					{
-						TRACE_CORE(printf(">>> EP0 Int: SET_CONFIGURATION REQUEST_DEVICE %d\r\n", pSetupData->wValueL);)
-#ifdef HID_ENABLED
-						UDD_InitEP( HID_ENDPOINT_INT, USB_ENDPOINT_TYPE_INTERRUPT | USB_ENDPOINT_IN(0));
-#endif
-
-#ifdef CDC_ENABLED
-						UDD_InitEP( CDC_ENDPOINT_ACM, USB_ENDPOINT_TYPE_BULK | USB_ENDPOINT_IN(0));
-						UDD_InitEP( CDC_ENDPOINT_OUT, USB_ENDPOINT_TYPE_BULK | USB_ENDPOINT_OUT(0));
-						UDD_InitEP( CDC_ENDPOINT_IN, USB_ENDPOINT_TYPE_INTERRUPT | USB_ENDPOINT_IN(0));
-#endif
-						_usbConfiguration = pSetupData->wValueL;
-
-#ifdef CDC_ENABLED
-						// Enable interrupt for CDC reception from host (OUT packet)
-						udd_ept_enable_it_transf_cplt_in(CDC_ENDPOINT_ACM);
-						udd_ept_enable_it_transf_cplt_out(CDC_ENDPOINT_OUT);
-#endif
-						send_zlp();
-					}
-					else
-					{
-						TRACE_CORE(puts(">>> EP0 Int: SET_CONFIGURATION failed!\r\n");)
-						ok = false;
-					}
-				}
-				else if (GET_INTERFACE == r)
-				{
-					TRACE_CORE(puts(">>> EP0 Int: GET_INTERFACE\r\n");)
-					UDD_Send(0, (void*)&_usbSetInterface, 1);
-				}
-				else if (SET_INTERFACE == r)
-				{
-					_usbSetInterface = pSetupData->wValueL;
-					TRACE_CORE(puts(">>> EP0 Int: SET_INTERFACE\r\n");)
-					send_zlp();
-				}
+				handleEndpoint(i);
 			}
-			else
-			{
-				TRACE_CORE(puts(">>> EP0 Int: ClassInterfaceRequest\r\n");)
-				ok =  USBD_ClassInterfaceRequest(*pSetupData);
-			}
-
-			if (ok)
-			{
-				TRACE_CORE(puts(">>> EP0 Int: Send packet\r\n");)
-				UDD_ClearIN();
-			}
-			else
-			{
-				TRACE_CORE(puts(">>> EP0 Int: Stall\r\n");)
-				UDD_Stall(0);
-			}
-
-			if( flags & USB_DEVICE_EPINTFLAG_STALL1 )
-			{
-				/* Clear the stall flag */
-				udd_clear_stall_request(0);
-
-				// Remove stall request
-				udd_remove_stall_request(0);
-			}
-		}  // end if USB_DEVICE_EPINTFLAG_RXSTP
-
-		i=0;
-		ept_int &= 0xFE;  // Remove endpoint number 0 (setup)
-		while (ept_int != 0)
-		{
-            // Check if endpoint has a pending interrupt
-            if ((ept_int & (1 << i)) != 0)
-			{
-				if( (udd_read_endpoint_flag(i) & USB_DEVICE_EPINTFLAG_TRCPT_Msk ) != 0 )
-				
-				{
-					EndpointHandler(i);
-				}
-                ept_int &= ~(1 << i);
-                
-                if (ept_int != 0)
-				{
-                
-                    TRACE_CORE("\n\r  - ");
-                }
-            }
-            i++;
-			if( i> USB_EPT_NUM) break;  // exit
-        }
-	}
-}
-
+			ept_int &= ~(1 << i);
+		}
+		i++;
+		if (i > USB_EPT_NUM)
+			break;  // fire exit
+	}
+}
+
+// USB_Handler ISR
+extern "C" void UDD_Handler(void) {
+	ISRHandler();
+}
 
 
 void USBD_Flush(uint32_t ep)
 {
-	if (UDD_FifoByteCount(ep))
-	{
-		UDD_ReleaseTX(ep);
-	}
-}
-
-//	Counting frames
-uint32_t USBD_Connected(void)
-{
-	uint8_t f = UDD_GetFrameNumber();
-	return f != UDD_GetFrameNumber();
-}
-
-
-//=======================================================================
-//=======================================================================
-
-USBDevice_ USBDevice;
-
-USBDevice_::USBDevice_()
-{
+	if (USBD_Available(ep)) {
+		// RAM buffer is full, we can send data (IN)
+		usbd.epBank1SetReady(ep);
+
+	 	// Clear the transfer complete flag
+		usbd.epBank1AckTransferComplete(ep);
+	}
+}
+
+
+bool connected()
+{
+	// Count frame numbers
+	uint8_t f = USB->DEVICE.FNUM.bit.FNUM;
+	//delay(3);
+	return f != USB->DEVICE.FNUM.bit.FNUM;
+}
+
+uint8_t armRecvCtrlOUT(uint32_t ep)
+{
+	// Get endpoint configuration from setting register
+	usbd.epBank0SetAddress(ep, &udd_ep_out_cache_buffer[ep]);
+	usbd.epBank0SetMultiPacketSize(ep, 8);
+	usbd.epBank0SetByteCount(ep, 0);
+
+	usbd.epBank0ResetReady(ep);
+
+	// Wait OUT
+	while (!usbd.epBank0IsReady(ep)) {}
+	while (!usbd.epBank0IsTransferComplete(ep)) {}
+	return usbd.epBank0ByteCount(ep);
+}
+
+
+uint32_t USBD_RecvControl(void *_data, uint32_t len)
+{
+	uint8_t *data = reinterpret_cast<uint8_t *>(_data);
+
+	// The RAM Buffer is empty: we can receive data
+	usbd.epBank0ResetReady(0);
+
+	//usbd.epBank0AckSetupReceived(0);
+	uint32_t read = armRecvCtrlOUT(0);
+	if (read > len)
+		read = len;
+	//while (!usbd.epBank0AckTransferComplete(0)) {}
+	uint8_t *buffer = udd_ep_out_cache_buffer[0];
+	for (uint32_t i=0; i<len; i++) {
+		data[i] = buffer[i];
+	}
+
+	return read;
+}
+
+// Number of bytes, assumes a rx endpoint
+uint32_t USBD_Available(uint32_t ep)
+{
+	return usbd.epBank0ByteCount(ep);
+}
+
+uint8_t armRecv(uint32_t ep)
+{
+	uint16_t count = usbd.epBank0ByteCount(ep);
+	if (count >= 64) {
+		usbd.epBank0SetByteCount(ep, count - 64);
+	} else {
+		usbd.epBank0SetByteCount(ep, 0);
+	}
+	return usbd.epBank0ByteCount(ep);
+}
+
+// Non Blocking receive
+// Return number of bytes read
+uint32_t USBD_Recv(uint32_t ep, void *_data, uint32_t len)
+{
+	if (!_usbConfiguration)
+		return -1;
+
+	PORT->Group[1].OUTCLR.reg =0x00000008 ; //RxLED on
+	if (USBD_Available(ep) < len)
+		len = USBD_Available(ep);
+
+	armRecv(ep);
+
+	usbd.epBank0DisableTransferComplete(ep);
+
+	memcpy(_data, udd_ep_out_cache_buffer[ep], len);
+
+	// release empty buffer
+	if (len && !USBD_Available(ep)) {
+		// The RAM Buffer is empty: we can receive data
+		usbd.epBank0ResetReady(ep);
+
+		// Clear Transfer complete 0 flag
+		usbd.epBank0AckTransferComplete(ep);
+	}
+
+	return len;
+}
+
+// Recv 1 byte if ready
+uint32_t USBD_Recv(uint32_t ep)
+{
+	uint8_t c;
+	if (USBD_Recv(ep, &c, 1) != 1) {
+		return -1;
+	} else {
+		return c;
+	}
+}
+
+
+
+//==================================================================
+
+void USBDevice_::init()
+{
+	// Enable USB clock
+	PM->APBBMASK.reg |= PM_APBBMASK_USB;
+
+	// Set up the USB DP/DN pins
+	PORT->Group[0].PINCFG[PIN_PA24G_USB_DM].bit.PMUXEN = 1;
+	PORT->Group[0].PMUX[PIN_PA24G_USB_DM/2].reg &= ~(0xF << (4 * (PIN_PA24G_USB_DM & 0x01u)));
+	PORT->Group[0].PMUX[PIN_PA24G_USB_DM/2].reg |= MUX_PA24G_USB_DM << (4 * (PIN_PA24G_USB_DM & 0x01u));
+	PORT->Group[0].PINCFG[PIN_PA25G_USB_DP].bit.PMUXEN = 1;
+	PORT->Group[0].PMUX[PIN_PA25G_USB_DP/2].reg &= ~(0xF << (4 * (PIN_PA25G_USB_DP & 0x01u)));
+	PORT->Group[0].PMUX[PIN_PA25G_USB_DP/2].reg |= MUX_PA25G_USB_DP << (4 * (PIN_PA25G_USB_DP & 0x01u));
+
+	// Put Generic Clock Generator 0 as source for Generic Clock Multiplexer 6 (USB reference)
+	GCLK->CLKCTRL.reg = GCLK_CLKCTRL_ID(6)     | // Generic Clock Multiplexer 6
+	                    GCLK_CLKCTRL_GEN_GCLK0 | // Generic Clock Generator 0 is source
+	                    GCLK_CLKCTRL_CLKEN;
+	while (GCLK->STATUS.bit.SYNCBUSY)
+		;
+
+	USB_SetHandler(&UDD_Handler);
+
+	// Reset USB Device
+	usbd.reset();
+
+	usbd.calibrate();
+	usbd.setUSBDeviceMode();
+	usbd.runInStandby();
+	usbd.setFullSpeed();
+
+	// Configure interrupts
+	NVIC_SetPriority((IRQn_Type) USB_IRQn, 0UL);
+	NVIC_EnableIRQ((IRQn_Type) USB_IRQn);
+
+	usbd.enable();
+
+	initialized = true;
 }
 
 bool USBDevice_::attach()
 {
-  if (_usbInitialized != 0UL)
-  {
-    UDD_Attach();
+	if (!initialized)
+		return false;
+
+	usbd.attach();
+	usbd.enableEndOfResetInterrupt();
+	usbd.enableStartOfFrameInterrupt();
+
 	_usbConfiguration = 0;
 	return true;
-  }
-  else
-  {
-    return false;
-  }
-}
+}
+
 
 bool USBDevice_::detach()
 {
-	if (_usbInitialized != 0UL)
-	{
-		UDD_Detach();
-		return true;
-	}
-	else
-	{
+	if (!initialized)
 		return false;
-	}
+	usbd.detach();
+	return true;
 }
 
 bool USBDevice_::configured()
 {
-	return _usbConfiguration;
-}
-
-void USBDevice_::poll()
-{
-}
-
-void USBDevice_::init()
-{
-	UDD_Init();
-	_usbInitialized=1UL;
-}
+	return _usbConfiguration != 0;
+}
+
+
+	
+/*
+ * USB Device instance
+ * -------------------
+ */
+
+// USBDevice class instance
+USBDevice_ USBDevice;
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBCore.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBCore.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBCore.h	(revision 224)
@@ -1,17 +1,23 @@
-// Copyright (c) 2010, Peter Barrett
 /*
-** Permission to use, copy, modify, and/or distribute this software for
-** any purpose with or without fee is hereby granted, provided that the
-** above copyright notice and this permission notice appear in all copies.
-**
-** THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
-** WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
-** WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR
-** BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES
-** OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
-** WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION,
-** ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS
-** SOFTWARE.
+  Copyright (c) 2014 Arduino LLC.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+  See the GNU Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */
+/*
+**Modified 04/04/2016 by Arduino.org development team
+*/
+
 
 #ifndef __USBCORE_H__
@@ -30,4 +36,16 @@
 #define SET_INTERFACE				11
 
+// bEndpointAddress in Endpoint Descriptor
+#define USB_ENDPOINT_DIRECTION_MASK            0x80
+#define USB_ENDPOINT_OUT(addr)                 ((addr) | 0x00)
+#define USB_ENDPOINT_IN(addr)                  ((addr) | 0x80)
+
+#define USB_ENDPOINTS                          7
+
+#define USB_ENDPOINT_TYPE_MASK                 0x03
+#define USB_ENDPOINT_TYPE_CONTROL              0x00
+#define USB_ENDPOINT_TYPE_ISOCHRONOUS          0x01
+#define USB_ENDPOINT_TYPE_BULK                 0x02
+#define USB_ENDPOINT_TYPE_INTERRUPT            0x03
 
 // bmRequestType
@@ -47,6 +65,7 @@
 #define REQUEST_RECIPIENT			0x1F
 
-#define REQUEST_DEVICETOHOST_CLASS_INTERFACE  (REQUEST_DEVICETOHOST + REQUEST_CLASS + REQUEST_INTERFACE)
-#define REQUEST_HOSTTODEVICE_CLASS_INTERFACE  (REQUEST_HOSTTODEVICE + REQUEST_CLASS + REQUEST_INTERFACE)
+#define REQUEST_DEVICETOHOST_CLASS_INTERFACE    (REQUEST_DEVICETOHOST | REQUEST_CLASS | REQUEST_INTERFACE)
+#define REQUEST_HOSTTODEVICE_CLASS_INTERFACE    (REQUEST_HOSTTODEVICE | REQUEST_CLASS | REQUEST_INTERFACE)
+#define REQUEST_DEVICETOHOST_STANDARD_INTERFACE (REQUEST_DEVICETOHOST | REQUEST_STANDARD | REQUEST_INTERFACE)
 
 //	Class requests
@@ -55,4 +74,5 @@
 #define CDC_GET_LINE_CODING			0x21
 #define CDC_SET_CONTROL_LINE_STATE	0x22
+#define CDC_SEND_BREAK				0x23
 
 #define MSC_RESET					0xFF
@@ -108,10 +128,4 @@
 #define MSC_SUBCLASS_SCSI						0x06
 #define MSC_PROTOCOL_BULK_ONLY					0x50
-
-#define HID_HID_DESCRIPTOR_TYPE					0x21
-#define HID_REPORT_DESCRIPTOR_TYPE				0x22
-#define HID_PHYSICAL_DESCRIPTOR_TYPE			0x23
-
-#define TX_RX_LED_PULSE_MS 100     //----- Tx & Rx led blinking during transmission (declaration)
 
 _Pragma("pack(1)")
@@ -225,8 +239,6 @@
 typedef struct
 {
-#if (defined CDC_ENABLED) && defined(HID_ENABLED)
 	//	IAD
 	IADDescriptor				iad;	// Only needed on compound device
-#endif
 	//	Control
 	InterfaceDescriptor			cif;
@@ -249,4 +261,5 @@
 	EndpointDescriptor			out;
 } MSCDescriptor;
+
 
 typedef struct
@@ -273,5 +286,5 @@
 
 #define D_DEVICE(_class,_subClass,_proto,_packetSize0,_vid,_pid,_version,_im,_ip,_is,_configs) \
-	{ 18, 1, 0x110, _class,_subClass,_proto,_packetSize0,_vid,_pid,_version,_im,_ip,_is,_configs }
+	{ 18, 1, 0x200, _class,_subClass,_proto,_packetSize0,_vid,_pid,_version,_im,_ip,_is,_configs }
 /* Table 9-8. Standard Device Descriptor
  * bLength, bDescriptorType, bcdUSB, bDeviceClass, bDeviceSubClass, bDeviceProtocol, bMaxPacketSize0,
@@ -279,9 +292,9 @@
 
 #define D_CONFIG(_totalLength,_interfaces) \
-	{ 9, 2, _totalLength,_interfaces, 1, 0, USB_CONFIG_SELF_POWERED, USB_CONFIG_POWER_MA(500) }
+	{ 9, 2, _totalLength,_interfaces, 1, 0, USB_CONFIG_BUS_POWERED, USB_CONFIG_POWER_MA(500) }
 /* Table 9-10. Standard Configuration Descriptor
  * bLength, bDescriptorType, wTotalLength, bNumInterfaces, bConfigurationValue, iConfiguration
  * bmAttributes, bMaxPower */
- 
+
 #define D_INTERFACE(_n,_numEndpoints,_class,_subClass,_protocol) \
 	{ 9, 4, _n, 0, _numEndpoints, _class,_subClass, _protocol, 0 }
@@ -289,5 +302,5 @@
  * bLength, bDescriptorType, bInterfaceNumber, bAlternateSetting, bNumEndpoints, bInterfaceClass,
  * bInterfaceSubClass, bInterfaceProtocol, iInterface */
- 
+
 #define D_ENDPOINT(_addr,_attr,_packetSize, _interval) \
 	{ 7, 5, _addr,_attr,_packetSize, _interval }
@@ -297,5 +310,5 @@
 #define D_IAD(_firstInterface, _count, _class, _subClass, _protocol) \
 	{ 8, 11, _firstInterface, _count, _class, _subClass, _protocol, 0 }
-/* iadclasscode_r10.pdf, Table 9Z. Standard Interface Association Descriptor
+/* iadclasscode_r10.pdf, Table 9\96Z. Standard Interface Association Descriptor
  * bLength, bDescriptorType, bFirstInterface, bInterfaceCount, bFunctionClass, bFunctionSubClass, bFunctionProtocol, iFunction */
 #define D_HIDREPORT(_descriptorLength) \
@@ -303,5 +316,5 @@
 /* HID1_11.pdf  E.8 HID Descriptor (Mouse)
  * bLength, bDescriptorType, bcdHID, bCountryCode, bNumDescriptors, bDescriptorType, wItemLength */
-
+ 
 // Functional Descriptor General Format
 #define D_CDCCS(_subtype,_d0,_d1)	{ 5, 0x24, _subtype, _d0, _d1 }
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBDesc.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBDesc.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USBDesc.h	(revision 224)
@@ -1,16 +1,21 @@
-// Copyright (c) 2010, Peter Barrett
 /*
-** Permission to use, copy, modify, and/or distribute this software for
-** any purpose with or without fee is hereby granted, provided that the
-** above copyright notice and this permission notice appear in all copies.
-**
-** THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
-** WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
-** WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR
-** BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES
-** OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
-** WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION,
-** ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS
-** SOFTWARE.
+  Copyright (c) 2014 Arduino LLC.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+  See the GNU Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+/*
+**Modified 04/04/2016 by Arduino.org development team
 */
 
@@ -22,10 +27,22 @@
 #define HID_ENABLED
 
+
+#ifdef CDC_ENABLED
+#define CDC_INTERFACE_COUNT 2
+#define CDC_ENPOINT_COUNT 3
+#endif
+
 // CDC
 #define CDC_ACM_INTERFACE	0	// CDC ACM
 #define CDC_DATA_INTERFACE	1	// CDC Data
+#define CDC_FIRST_ENDPOINT  1
 #define CDC_ENDPOINT_ACM	1
 #define CDC_ENDPOINT_OUT	2
 #define CDC_ENDPOINT_IN		3
+
+#ifdef CDC_ENABLED
+#define CDC_RX CDC_ENDPOINT_OUT
+#define CDC_TX CDC_ENDPOINT_IN
+#endif
 
 // HID
@@ -33,7 +50,11 @@
 #define HID_ENDPOINT_INT	4  
 
+
+#define ISERIAL_MAX_LEN        20
+
 // Defined string description
 #define IMANUFACTURER	1
-#define IPRODUCT		2
+#define IPRODUCT    2
+#define ISERIAL    3
 
 #endif /* __USBDESC_H__ */
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USB_device.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USB_device.h	(revision 223)
+++ 	(revision )
@@ -1,73 +1,0 @@
-/* ----------------------------------------------------------------------------
- *         SAM Software Package License
- * ----------------------------------------------------------------------------
- * Copyright (c) 2011-2012, Atmel Corporation
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following condition is met:
- *
- * - Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the disclaimer below.
- *
- * Atmel's name may not be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
- * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
- * DISCLAIMED. IN NO EVENT SHALL ATMEL 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.
- * ----------------------------------------------------------------------------
- */
-
-#ifndef USB_DEVICE_H_INCLUDED
-#define USB_DEVICE_H_INCLUDED
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <stdint.h>
-#include "USB/samd21_device.h"
-
-
-// bEndpointAddress in Endpoint Descriptor
-#define USB_ENDPOINT_DIRECTION_MASK            0x80
-#define USB_ENDPOINT_OUT(addr)                 ((addr) | 0x00)
-#define USB_ENDPOINT_IN(addr)                  ((addr) | 0x80)
-
-#define USB_ENDPOINT_TYPE_MASK                 0x03
-#define USB_ENDPOINT_TYPE_CONTROL              0x00
-#define USB_ENDPOINT_TYPE_ISOCHRONOUS          0x01
-#define USB_ENDPOINT_TYPE_BULK                 0x02
-#define USB_ENDPOINT_TYPE_INTERRUPT            0x03
-
-
-extern void UDD_ClearIN(void);
-extern uint32_t UDD_FifoByteCount(uint32_t ep);
-extern void UDD_ReleaseRX(uint32_t ep);
-extern void UDD_ReleaseTX(uint32_t ep);
-extern uint32_t UDD_Send(uint32_t ep, const void* data, uint32_t len);
-extern uint8_t UDD_Recv8(uint32_t ep);
-extern void UDD_Recv(uint32_t ep, uint8_t** data);
-extern void UDD_Init(void);
-extern void UDD_InitEP( uint32_t ul_ep, uint32_t ul_ep_cfg );
-extern void send_zlp (void);
-extern void UDD_Attach(void);
-extern void UDD_Detach(void);
-extern void UDD_SetAddress(uint32_t addr);
-extern void UDD_Stall(uint32_t ep);
-extern uint32_t UDD_GetFrameNumber(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* USB_DEVICE_H_INCLUDED */
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USB_host.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USB_host.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/USB_host.h	(revision 224)
@@ -1,30 +1,22 @@
-/* ----------------------------------------------------------------------------
- *         SAM Software Package License
- * ----------------------------------------------------------------------------
- * Copyright (c) 2011-2012, Atmel Corporation
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following condition is met:
- *
- * - Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the disclaimer below.
- *
- * Atmel's name may not be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
- * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
- * DISCLAIMED. IN NO EVENT SHALL ATMEL 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.
- * ----------------------------------------------------------------------------
- */
+/*
+  Copyright (c) 2014 Arduino LLC.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+  See the GNU Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+/*
+**Modified 04/04/2016 by Arduino.org development team
+*/
 
 #ifndef USB_HOST_H_INCLUDED
@@ -32,14 +24,14 @@
 
 #include <stdint.h>
-
+#include "samd21_host.h"
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-#define tokSETUP		UOTGHS_HSTPIPCFG_PTOKEN_SETUP
-#define tokIN			UOTGHS_HSTPIPCFG_PTOKEN_IN
-#define tokOUT			UOTGHS_HSTPIPCFG_PTOKEN_OUT
-#define tokINHS			UOTGHS_HSTPIPCFG_PTOKEN_IN
-#define tokOUTHS		UOTGHS_HSTPIPCFG_PTOKEN_OUT
+#define tokSETUP		USB_HOST_PCFG_PTOKEN_SETUP
+#define tokIN			USB_HOST_PCFG_PTOKEN_IN
+#define tokOUT			USB_HOST_PCFG_PTOKEN_OUT
+#define tokINHS			USB_HOST_PCFG_PTOKEN_IN
+#define tokOUTHS		USB_HOST_PCFG_PTOKEN_OUT
 
 //! \brief Device speed
@@ -58,12 +50,11 @@
 } uhd_vbus_state_t;
 
-//extern uhd_speed_t uhd_get_speed(void);
-
-extern void UHD_SetStack(void (*pf_isr)(void));
 extern void UHD_Init(void);
-extern void UHD_BusReset(void);
+extern void UHD_Handler(void);
+extern void USB_SetHandler(void (*pf_isr)(void));
 extern uhd_vbus_state_t UHD_GetVBUSState(void);
 extern uint32_t UHD_Pipe0_Alloc(uint32_t ul_add, uint32_t ul_ep_size);
 extern uint32_t UHD_Pipe_Alloc(uint32_t ul_dev_addr, uint32_t ul_dev_ep, uint32_t ul_type, uint32_t ul_dir, uint32_t ul_maxsize, uint32_t ul_interval, uint32_t ul_nb_bank);
+extern void UHD_Pipe_CountZero(uint32_t ul_pipe);
 extern void UHD_Pipe_Free(uint32_t ul_pipe);
 extern uint32_t UHD_Pipe_Read(uint32_t ul_pipe, uint32_t ul_size, uint8_t* data);
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/samd21_device.c
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/samd21_device.c	(revision 223)
+++ 	(revision )
@@ -1,348 +1,0 @@
-/* ----------------------------------------------------------------------------
- *         SAM Software Package License
- * ----------------------------------------------------------------------------
- * Copyright (c) 2011-2012, Atmel Corporation
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following condition is met:
- *
- * - Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the disclaimer below.
- *
- * Atmel's name may not be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
- * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
- * DISCLAIMED. IN NO EVENT SHALL ATMEL 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.
- * ----------------------------------------------------------------------------
- */
-
-#include <stdio.h>
-#include <stdint.h>
-#include <string.h>
-
-#include "variant.h"
-#include "USB/USB_device.h"
-#include "USB/samd21_device.h"
-#include "sam.h"
-
-#ifdef __cplusplus
-extern "C"{
-#endif // __cplusplus
-
-#ifdef SAMD_SERIES
-
-//#define TRACE_DEVICE(x)	x
-#define TRACE_DEVICE(x)
-
-__attribute__((__aligned__(4)))   __attribute__((__section__(".bss_hram0"))) uint8_t udd_ep_out_cache_buffer[4][64];
-__attribute__((__aligned__(4)))   __attribute__((__section__(".bss_hram0"))) uint8_t udd_ep_in_cache_buffer[4][128];
-
-/**
- * USB SRAM data containing pipe descriptor table
- * The content of the USB SRAM can be :
- * - modified by USB hardware interface to update pipe status.
- *   Thereby, it is read by software.
- * - modified by USB software to control pipe.
- *   Thereby, it is read by hardware.
- * This data section is volatile.
- */
- __attribute__((__aligned__(4))) UsbDeviceDescriptor usb_endpoint_table[USB_EPT_NUM];
-
-// NVM Software Calibration Area Mapping
-// USB TRANSN calibration value. Should be written to the USB PADCAL register.
-#define NVM_USB_PAD_TRANSN_POS  45
-#define NVM_USB_PAD_TRANSN_SIZE 5
-// USB TRANSP calibration value. Should be written to the USB PADCAL register.
-#define NVM_USB_PAD_TRANSP_POS  50
-#define NVM_USB_PAD_TRANSP_SIZE 5
-// USB TRIM calibration value. Should be written to the USB PADCAL register.
-#define NVM_USB_PAD_TRIM_POS  55
-#define NVM_USB_PAD_TRIM_SIZE 3
-
-void UDD_Init(void)
-{
-	uint32_t pad_transn;
-    uint32_t pad_transp;
-    uint32_t pad_trim;
-	uint32_t i;
-
-	/* Enable USB clock */
-	PM->APBBMASK.reg |= PM_APBBMASK_USB;
-
-	/* Set up the USB DP/DN pins */
-	PORT->Group[0].PINCFG[PIN_PA24G_USB_DM].bit.PMUXEN = 1;
-	PORT->Group[0].PMUX[PIN_PA24G_USB_DM/2].reg &= ~(0xF << (4 * (PIN_PA24G_USB_DM & 0x01u)));
-	PORT->Group[0].PMUX[PIN_PA24G_USB_DM/2].reg |= MUX_PA24G_USB_DM << (4 * (PIN_PA24G_USB_DM & 0x01u));
-	PORT->Group[0].PINCFG[PIN_PA25G_USB_DP].bit.PMUXEN = 1;
-	PORT->Group[0].PMUX[PIN_PA25G_USB_DP/2].reg &= ~(0xF << (4 * (PIN_PA25G_USB_DP & 0x01u)));
-	PORT->Group[0].PMUX[PIN_PA25G_USB_DP/2].reg |= MUX_PA25G_USB_DP << (4 * (PIN_PA25G_USB_DP & 0x01u));
-
-	/* ----------------------------------------------------------------------------------------------
-	 * Put Generic Clock Generator 0 as source for Generic Clock Multiplexer 6 (USB reference)
-	 */
-	GCLK->CLKCTRL.reg = GCLK_CLKCTRL_ID( 6 ) | // Generic Clock Multiplexer 6
-					GCLK_CLKCTRL_GEN_GCLK0 | // Generic Clock Generator 0 is source
-					GCLK_CLKCTRL_CLKEN ;
-
-	while ( GCLK->STATUS.reg & GCLK_STATUS_SYNCBUSY )
-	{
-	/* Wait for synchronization */
-	}
-
-	/* Reset */
-	USB->DEVICE.CTRLA.bit.SWRST = 1;
-	while (USB->DEVICE.SYNCBUSY.bit.SWRST) {
-		/* Sync wait */
-	}
-
-	udd_enable();
-
-	/* Load Pad Calibration */
-	pad_transn =( *((uint32_t *)(NVMCTRL_OTP4)  // Non-Volatile Memory Controller
-	+ (NVM_USB_PAD_TRANSN_POS / 32))
-	>> (NVM_USB_PAD_TRANSN_POS % 32))
-	& ((1 << NVM_USB_PAD_TRANSN_SIZE) - 1);
-
-	if (pad_transn == 0x1F) {  // maximum value (31)
-		pad_transn = 5;
-	}
-
-	USB->DEVICE.PADCAL.bit.TRANSN = pad_transn;
-
-	pad_transp =( *((uint32_t *)(NVMCTRL_OTP4)
-	+ (NVM_USB_PAD_TRANSP_POS / 32))
-	>> (NVM_USB_PAD_TRANSP_POS % 32))
-	& ((1 << NVM_USB_PAD_TRANSP_SIZE) - 1);
-
-	if (pad_transp == 0x1F) {  // maximum value (31)
-		pad_transp = 29;
-	}
-
-	USB->DEVICE.PADCAL.bit.TRANSP = pad_transp;
-
-	pad_trim =( *((uint32_t *)(NVMCTRL_OTP4)
-	+ (NVM_USB_PAD_TRIM_POS / 32))
-	>> (NVM_USB_PAD_TRIM_POS % 32))
-	& ((1 << NVM_USB_PAD_TRIM_SIZE) - 1);
-
-	if (pad_trim == 0x7) {  // maximum value (7)
-		pad_trim = 3;
-	}
-
-	USB->DEVICE.PADCAL.bit.TRIM = pad_trim;
-
-	/* Set the configuration */
-	udd_force_device_mode();
-	udd_device_run_in_standby();
-    // Set address of USB SRAM
-	USB->DEVICE.DESCADD.reg = (uint32_t)(&usb_endpoint_table[0]);
-	// For USB_SPEED_FULL 
-	udd_force_full_speed();
- 	for (i = 0; i < sizeof(usb_endpoint_table)/4; i++) {
-		(*(uint32_t *)((uint32_t *)(&usb_endpoint_table[0])+i)) = 0;
- 	}
-
-	// Configure interrupts
-	NVIC_SetPriority((IRQn_Type) USB_IRQn, 0UL);
-	NVIC_EnableIRQ((IRQn_Type) USB_IRQn);
-}
-
-void UDD_Attach(void)
-{
-	TRACE_DEVICE(printf("=> UDD_Attach\r\n");)
-
-    // Authorize attach if Vbus is present
-	udd_attach_device();
-
-	// Enable USB line events
-	udd_enable_reset_interrupt();
-
-    // usefull for debug
-    udd_enable_sof_interrupt();
-}
-
-void UDD_Detach(void)
-{
-	TRACE_DEVICE(printf("=> UDD_Detach\r\n");)
-    udd_detach_device();
-}
-
-void send_zlp(void)
-{
-	/* Set the byte count as zero */
-	usb_endpoint_table[0].DeviceDescBank[1].PCKSIZE.bit.BYTE_COUNT = 0;
-}
-
-
-void UDD_InitEP( uint32_t ul_ep_nb, uint32_t ul_ep_cfg )
-{
-	TRACE_DEVICE(printf("=> UDD_InitEP : init EP %lu\r\n", (unsigned long)ul_ep_nb);)
-
-	if( ul_ep_cfg == (USB_ENDPOINT_TYPE_INTERRUPT | USB_ENDPOINT_IN(0)) )
-	{
-		USB->DEVICE.DeviceEndpoint[ul_ep_nb].EPCFG.reg = USB_DEVICE_EPCFG_EPTYPE1(4);
-		/* Set maximum packet size as 8 bytes */
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[1].PCKSIZE.bit.SIZE = 0;  // 8 bytes
-		/* Configure the data buffer */
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[1].ADDR.reg = (uint32_t)&udd_ep_in_cache_buffer[ul_ep_nb];
-	}
-	else if( ul_ep_cfg == (USB_ENDPOINT_TYPE_BULK | USB_ENDPOINT_OUT(0)) )
-	{
-		/* Configure BULK OUT endpoint for CDC Data interface*/
-		USB->DEVICE.DeviceEndpoint[ul_ep_nb].EPCFG.reg = USB_DEVICE_EPCFG_EPTYPE0(3);
-		/* Set maximum packet size as 64 bytes */
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[0].PCKSIZE.bit.SIZE = 0x3;  // for 64 bytes
-		/* Configure the data buffer */
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[0].ADDR.reg = (uint32_t)&udd_ep_out_cache_buffer[ul_ep_nb];
-
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[0].PCKSIZE.bit.MULTI_PACKET_SIZE = 64;
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[0].PCKSIZE.bit.BYTE_COUNT = 0;
-		// NACK if not ready
-	    USB->DEVICE.DeviceEndpoint[ul_ep_nb].EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_BK0RDY;
-	}
-	else if( ul_ep_cfg == (USB_ENDPOINT_TYPE_BULK | USB_ENDPOINT_IN(0)) )
-	{
-		/* Configure BULK IN endpoint for CDC Data interface */
-		USB->DEVICE.DeviceEndpoint[ul_ep_nb].EPCFG.reg = USB_DEVICE_EPCFG_EPTYPE1(3);
-		/* Set maximum packet size as 64 bytes */
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[1].PCKSIZE.bit.SIZE = 0x3;  // for 64 bytes
-		/* Configure the data buffer */
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[1].ADDR.reg = (uint32_t)&udd_ep_in_cache_buffer[ul_ep_nb];
-		// NACK if not ready
-		USB->DEVICE.DeviceEndpoint[ul_ep_nb].EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_BK1RDY;
-	}
-	else if( ul_ep_cfg == USB_ENDPOINT_TYPE_CONTROL )
-	{
-		/* Configure CONTROL endpoint */
-		USB->DEVICE.DeviceEndpoint[ul_ep_nb].EPCFG.reg = USB_DEVICE_EPCFG_EPTYPE0(1) | USB_DEVICE_EPCFG_EPTYPE1(1);
-		USB->DEVICE.DeviceEndpoint[ul_ep_nb].EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_BK0RDY;
-		USB->DEVICE.DeviceEndpoint[ul_ep_nb].EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_BK1RDY;
-
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[0].PCKSIZE.reg &= ~USB_DEVICE_PCKSIZE_AUTO_ZLP;
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[1].PCKSIZE.reg &= ~USB_DEVICE_PCKSIZE_AUTO_ZLP;
-
-		/* Set maximum packet size as 64 bytes */
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[0].PCKSIZE.bit.SIZE = 0x3;  // for 64 bytes
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[1].PCKSIZE.bit.SIZE = 0x3;  // for 64 bytes
-
-		/* get endpoint configuration from setting register */
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[0].ADDR.reg = (uint32_t)&udd_ep_out_cache_buffer[0];
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[0].PCKSIZE.bit.MULTI_PACKET_SIZE = 16;
-		usb_endpoint_table[ul_ep_nb].DeviceDescBank[0].PCKSIZE.bit.BYTE_COUNT = 0;
-
-		// NACK if not ready
-		USB->DEVICE.DeviceEndpoint[ul_ep_nb].EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_BK0RDY;
-		USB->DEVICE.DeviceEndpoint[ul_ep_nb].EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_BK1RDY;
-	}
-}
-
-
-// Send packet.
-void UDD_ClearIN(void)
-{
-	USB->DEVICE.DeviceEndpoint[EP0].EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_BK1RDY;
-}
-
-
-
-uint32_t UDD_Send(uint32_t ep, const void* data, uint32_t len)
-{
-	memcpy(&udd_ep_in_cache_buffer[ep], data, len);
-
-	/* Get endpoint configuration from setting register */
-	usb_endpoint_table[ep].DeviceDescBank[1].ADDR.reg = (uint32_t)&udd_ep_in_cache_buffer[ep];
-	usb_endpoint_table[ep].DeviceDescBank[1].PCKSIZE.bit.MULTI_PACKET_SIZE = 0;
-	usb_endpoint_table[ep].DeviceDescBank[1].PCKSIZE.bit.BYTE_COUNT = len;
-
-	return len;
-}
-
-uint8_t UDD_Recv8(uint32_t ep)
-{
-	TRACE_DEVICE(printf("=> UDD_Recv8 : ep=%d\r\n", (char)ep);)
-
-    usb_endpoint_table[ep].DeviceDescBank[0].ADDR.reg = (uint32_t)&udd_ep_out_cache_buffer[ep];
-	usb_endpoint_table[ep].DeviceDescBank[0].PCKSIZE.bit.MULTI_PACKET_SIZE = 8;
-	usb_endpoint_table[ep].DeviceDescBank[0].PCKSIZE.bit.BYTE_COUNT = 0;
-	USB->DEVICE.DeviceEndpoint[ep].EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_BK0RDY;
-	TRACE_DEVICE(printf("=> UDD_Recv8 : data=%lu\r\n", (unsigned long)data);)
-
-	/* Wait for transfer to complete */
-	while (!( USB->DEVICE.DeviceEndpoint[ep].EPINTFLAG.reg & USB_DEVICE_EPINTFLAG_TRCPT0 ));
-	/* Clear Transfer complete 0 flag */
-	USB->DEVICE.DeviceEndpoint[ep].EPINTFLAG.bit.TRCPT0 = 1;
-
-	return udd_ep_out_cache_buffer[ep][0];
-}
-
-void UDD_Recv(uint32_t ep, uint8_t** ppData)
-{
-	*ppData = udd_ep_out_cache_buffer[ep];
-}
-
-void UDD_Stall(uint32_t ep)
-{
-	uint8_t ep_num = ep;
-
-	// Stall endpoint
-	USB->DEVICE.DeviceEndpoint[ep_num].EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_STALLRQ1;
-}
-
-uint32_t UDD_FifoByteCount(uint32_t ep)
-{
-	return ((uint16_t)(usb_endpoint_table[ep].DeviceDescBank[0].PCKSIZE.bit.BYTE_COUNT));
-}
-
-void UDD_ReleaseRX(uint32_t ep)
-{
-	TRACE_DEVICE(puts("=> UDD_ReleaseRX\r\n");)
-	USB->DEVICE.DeviceEndpoint[ep].EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_BK0RDY;
-    USB->DEVICE.DeviceEndpoint[ep].EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_TRCPT0;
-}
-
-void UDD_ReleaseTX(uint32_t ep)
-{
-	TRACE_DEVICE(printf("=> UDD_ReleaseTX ep=%lu\r\n", (unsigned long)ep);)
-    USB->DEVICE.DeviceEndpoint[ep].EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_BK1RDY;
-    USB->DEVICE.DeviceEndpoint[ep].EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_TRCPT1;
-}
-
-void UDD_SetAddress(uint32_t addr)
-{
-	TRACE_DEVICE(printf("=> UDD_SetAddress : setting address to %lu\r\n", (unsigned long)addr);)
-
-	/* Set the byte count as zero */
-	usb_endpoint_table[0].DeviceDescBank[1].PCKSIZE.bit.BYTE_COUNT = 0;
- 	/* Clear the transfer complete flag  */
- 	USB->DEVICE.DeviceEndpoint[0].EPINTFLAG.bit.TRCPT1 = 1;
-	/* Set the bank as ready */
-	USB->DEVICE.DeviceEndpoint[0].EPSTATUSSET.bit.BK1RDY = 1;
-
-	/* Wait for transfer to complete */
-	while (!( USB->DEVICE.DeviceEndpoint[0].EPINTFLAG.reg & USB_DEVICE_EPINTFLAG_TRCPT1 )) {}
-
-	udd_configure_address(addr);
-}
-
-uint32_t UDD_GetFrameNumber(void)
-{
-	return udd_frame_number();
-}
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* SAMD_SERIES */
-
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/samd21_device.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/samd21_device.h	(revision 223)
+++ 	(revision )
@@ -1,112 +1,0 @@
-/* ----------------------------------------------------------------------------
- *         SAM Software Package License
- * ----------------------------------------------------------------------------
- * Copyright (c) 2011-2012, Atmel Corporation
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following condition is met:
- *
- * - Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the disclaimer below.
- *
- * Atmel's name may not be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
- * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
- * DISCLAIMED. IN NO EVENT SHALL ATMEL 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.
- * ----------------------------------------------------------------------------
- */
-
-#ifndef SAMD21_DEVICE_H_INCLUDED
-#define SAMD21_DEVICE_H_INCLUDED
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#define EP0				0
-#define EPX_SIZE		64// 64 for Full Speed, EPT size max is 1024
-
-// Force device low speed mode
-#define udd_force_low_speed()               USB->DEVICE.CTRLB.reg &= ~USB_DEVICE_CTRLB_SPDCONF_Msk; USB->DEVICE.CTRLB.reg |= USB_DEVICE_CTRLB_SPDCONF_1_Val
-// Force full speed mode
-#define udd_force_full_speed()              USB->DEVICE.CTRLB.reg &= ~USB_DEVICE_CTRLB_SPDCONF_Msk
-
-// Attaches to USB bus
-#define udd_attach_device()                  USB->DEVICE.CTRLB.reg &= ~USB_DEVICE_CTRLB_DETACH
-#define udd_detach_device()                  USB->DEVICE.CTRLB.reg |= USB_DEVICE_CTRLB_DETACH
-
-// Manage reset event
-// Set when a USB "End of Reset" has been detected
-#define udd_enable_reset_interrupt()              USB->DEVICE.INTENSET.reg = USB_DEVICE_INTENSET_EORST
-#define udd_ack_reset()                           USB->DEVICE.INTFLAG.reg = USB_DEVICE_INTFLAG_EORST
-#define Is_udd_reset()                            (USB->DEVICE.INTFLAG.reg & USB_DEVICE_INTFLAG_EORST)
-
-// Manage start of frame (SOF) event
-#define udd_enable_sof_interrupt()                USB->DEVICE.INTENSET.reg = USB_DEVICE_INTENSET_SOF
-#define udd_ack_sof()                             USB->DEVICE.INTFLAG.reg = USB_DEVICE_INTFLAG_SOF
-#define Is_udd_sof()                              (USB->DEVICE.INTENSET.reg & USB_DEVICE_INTENSET_SOF)
-#define udd_frame_number()                        ((USB->DEVICE.FNUM.reg & USB_DEVICE_FNUM_FNUM_Msk) >> USB_DEVICE_FNUM_FNUM_Pos)
-
-// configures the USB device address and enable it.
-#define udd_configure_address(address)            USB->DEVICE.DADD.reg = USB_DEVICE_DADD_ADDEN | address
-
-// Enables SETUP received interrupt
-#define udd_enable_setup_received_interrupt(ep)   USB->DEVICE.DeviceEndpoint[ep].EPINTENSET.reg = USB_DEVICE_EPINTFLAG_RXSTP
-// ACKs OUT received
-#define udd_ack_out_received(ep)                  USB->DEVICE.DeviceEndpoint[ep].EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_BK0RDY
-// ACKs OUT received
-#define udd_ack_in_received(ep)                   USB->DEVICE.DeviceEndpoint[ep].EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_BK1RDY
-// Is transfert completed ?
-#define udd_is_transf_cplt(ep)                   (USB->DEVICE.DeviceEndpoint[ep].EPINTFLAG.reg & USB_DEVICE_EPINTFLAG_TRCPT1)
-// Clear the transfer complete flag
-#define udd_clear_transf_cplt(ep)                 USB->DEVICE.DeviceEndpoint[ep].EPINTFLAG.bit.TRCPT1 = 1
-// Set the bank as ready
-#define udd_bk_rdy(ep)                            USB->DEVICE.DeviceEndpoint[ep].EPSTATUSSET.bit.BK1RDY = 1
-#define udd_read_endpoint_flag(ep)                USB->DEVICE.DeviceEndpoint[ep].EPINTFLAG.reg
-// Enable interrupt transfer complete
-#define udd_ept_enable_it_transf_cplt_in(ep)      USB->DEVICE.DeviceEndpoint[ep].EPINTENSET.reg = USB_DEVICE_EPINTENSET_TRCPT1
-#define udd_ept_enable_it_transf_cplt_out(ep)     USB->DEVICE.DeviceEndpoint[ep].EPINTENSET.reg = USB_DEVICE_EPINTENSET_TRCPT0
-// Clear the stall flag
-#define udd_clear_stall_request(ep)			      USB->DEVICE.DeviceEndpoint[ep].EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_STALL1
-// Remove stall 
-#define udd_remove_stall_request(ep)		      USB->DEVICE.DeviceEndpoint[ep].EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_STALLRQ1
-
-// Endpoint Interrupt Summary
-#define udd_endpoint_interrupt()            USB->DEVICE.EPINTSMRY.reg
-
-#define udd_clear_wakeup_interrupt()        USB->DEVICE.INTENCLR.reg = USB_DEVICE_INTENCLR_WAKEUP
-#define udd_clear_eorsm_interrupt()         USB->DEVICE.INTENCLR.reg = USB_DEVICE_INTENCLR_EORSM
-#define udd_clear_suspend_interrupt()       USB->DEVICE.INTENCLR.reg = USB_DEVICE_INTENCLR_SUSPEND
-
-
-
-// Force device mode
-#define udd_force_device_mode()             USB->DEVICE.CTRLA.reg &= ~USB_CTRLA_MODE
-// Run in Standby
-#define udd_device_run_in_standby()         USB->DEVICE.CTRLA.reg |= USB_CTRLA_RUNSTDBY 
-// Force host mode
-#define udd_force_host_mode()               USB->DEVICE.CTRLA.reg |= USB_CTRLA_MODE
-
-
-// Enable USB macro
-#define udd_enable()                        USB->DEVICE.CTRLA.reg |= USB_CTRLA_ENABLE
-// Disable USB macro
-#define udd_disable()                       USB->DEVICE.CTRLA.reg &= ~USB_CTRLA_ENABLE
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* SAMD21_DEVICE_H_INCLUDED */
-
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/samd21_host.c
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/samd21_host.c	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/samd21_host.c	(revision 224)
@@ -1,32 +1,38 @@
-/* ----------------------------------------------------------------------------
- *         SAM Software Package License
- * ----------------------------------------------------------------------------
- * Copyright (c) 2011-2012, Atmel Corporation
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following condition is met:
- *
- * - Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the disclaimer below.
- *
- * Atmel's name may not be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
- * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
- * DISCLAIMED. IN NO EVENT SHALL ATMEL 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.
- * ----------------------------------------------------------------------------
- */
+/*
+  Copyright (c) 2014 Arduino LLC.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+  See the GNU Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+/*
+** Modified 04/04/2016 by Arduino.org development team
+*/
+
+
 
 #include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "../Arduino.h"
+#include "variant.h"
+#include "USB_host.h"
+#include "samd21_host.h"
+#include "sam.h"
+#include "wiring_digital.h"
+#include "wiring_private.h"
+
+#define HOST_DEFINED
 
 #ifdef HOST_DEFINED
@@ -35,22 +41,24 @@
 #define TRACE_UOTGHS_HOST(x)
 
-//extern void (*gpf_isr)(void);
-
 // Handle UOTGHS Host driver state
 static uhd_vbus_state_t uhd_state = UHD_STATE_NO_VBUS;
 
- __attribute__((__aligned__(4))) UsbHostDescriptor usb_pipe_table[USB_EPT_NUM];
+__attribute__((__aligned__(4))) volatile UsbHostDescriptor usb_pipe_table[USB_EPT_NUM];
+
+extern void (*gpf_isr)(void);
 
 
 // NVM Software Calibration Area Mapping
 // USB TRANSN calibration value. Should be written to the USB PADCAL register.
-#define NVM_USB_PAD_TRANSN_POS  45
-#define NVM_USB_PAD_TRANSN_SIZE 5
+#define NVM_USB_PAD_TRANSN_POS     45
+#define NVM_USB_PAD_TRANSN_SIZE    5
 // USB TRANSP calibration value. Should be written to the USB PADCAL register.
-#define NVM_USB_PAD_TRANSP_POS  50
-#define NVM_USB_PAD_TRANSP_SIZE 5
+#define NVM_USB_PAD_TRANSP_POS     50
+#define NVM_USB_PAD_TRANSP_SIZE    5
 // USB TRIM calibration value. Should be written to the USB PADCAL register.
-#define NVM_USB_PAD_TRIM_POS  55
-#define NVM_USB_PAD_TRIM_SIZE 3
+#define NVM_USB_PAD_TRIM_POS       55
+#define NVM_USB_PAD_TRIM_SIZE      3
+
+static void UHD_ISR(void);
 
 /**
@@ -60,45 +68,50 @@
 {
 	uint32_t pad_transn;
-    uint32_t pad_transp;
-    uint32_t pad_trim;
+	uint32_t pad_transp;
+	uint32_t pad_trim;
+	uint32_t i;
+
+//	USB_SetHandler(&USB_Handler);
+	USB_SetHandler(&UHD_ISR);
 
 	/* Enable USB clock */
 	PM->APBBMASK.reg |= PM_APBBMASK_USB;
 
-	/* Set up the USB DP/DN pins */
-	PORT->Group[0].PINCFG[PIN_PA24G_USB_DM].bit.PMUXEN = 1;
-	PORT->Group[0].PMUX[PIN_PA24G_USB_DM/2].reg &= ~(0xF << (4 * (PIN_PA24G_USB_DM & 0x01u)));
-	PORT->Group[0].PMUX[PIN_PA24G_USB_DM/2].reg |= MUX_PA24G_USB_DM << (4 * (PIN_PA24G_USB_DM & 0x01u));
-	PORT->Group[0].PINCFG[PIN_PA25G_USB_DP].bit.PMUXEN = 1;
-	PORT->Group[0].PMUX[PIN_PA25G_USB_DP/2].reg &= ~(0xF << (4 * (PIN_PA25G_USB_DP & 0x01u)));
-	PORT->Group[0].PMUX[PIN_PA25G_USB_DP/2].reg |= MUX_PA25G_USB_DP << (4 * (PIN_PA25G_USB_DP & 0x01u));
+	/* Set up the USB DP/DM pins */
+	pinPeripheral( 33ul, PIO_COM );
+	pinPeripheral( 34ul, PIO_COM );
 
 	/* ----------------------------------------------------------------------------------------------
-	 * Put Generic Clock Generator 0 as source for Generic Clock Multiplexer 6 (USB reference)
-	 */
-	GCLK->CLKCTRL.reg = GCLK_CLKCTRL_ID( 6 ) | // Generic Clock Multiplexer 6
-					GCLK_CLKCTRL_GEN_GCLK0 | // Generic Clock Generator 0 is source
-					GCLK_CLKCTRL_CLKEN ;
-
-	while ( GCLK->STATUS.reg & GCLK_STATUS_SYNCBUSY )
-	{
-	/* Wait for synchronization */
+	* Put Generic Clock Generator 0 as source for Generic Clock Multiplexer 6 (USB reference)
+	*/
+	GCLK->CLKCTRL.reg = GCLK_CLKCTRL_ID(6) |        // Generic Clock Multiplexer 6
+						GCLK_CLKCTRL_GEN_GCLK0 |    // Generic Clock Generator 0 is source
+						GCLK_CLKCTRL_CLKEN;
+
+	while (GCLK->STATUS.reg & GCLK_STATUS_SYNCBUSY)
+	{
+		/* Wait for synchronization */
 	}
 
 	/* Reset */
 	USB->HOST.CTRLA.bit.SWRST = 1;
-	while (USB->HOST.SYNCBUSY.bit.SWRST) {
+	while (USB->HOST.SYNCBUSY.bit.SWRST)
+	{
 		/* Sync wait */
 	}
 
-	udd_enable();
+	//  uhd_enable();
+	USB->DEVICE.CTRLA.reg |= USB_CTRLA_ENABLE | USB_CTRLA_MODE;
+	uhd_force_host_mode();
+	while (USB->HOST.SYNCBUSY.reg == USB_SYNCBUSY_ENABLE);
 
 	/* Load Pad Calibration */
-	pad_transn =( *((uint32_t *)(NVMCTRL_OTP4)  // Non-Volatile Memory Controller
-	+ (NVM_USB_PAD_TRANSN_POS / 32))
-	>> (NVM_USB_PAD_TRANSN_POS % 32))
-	& ((1 << NVM_USB_PAD_TRANSN_SIZE) - 1);
-
-	if (pad_transn == 0x1F) {  // maximum value (31)
+	pad_transn = (*((uint32_t *)(NVMCTRL_OTP4)       // Non-Volatile Memory Controller
+					+ (NVM_USB_PAD_TRANSN_POS / 32))
+					>> (NVM_USB_PAD_TRANSN_POS % 32))
+				& ((1 << NVM_USB_PAD_TRANSN_SIZE) - 1);
+
+	if (pad_transn == 0x1F)         // maximum value (31)
+	{
 		pad_transn = 5;
 	}
@@ -106,10 +119,11 @@
 	USB->HOST.PADCAL.bit.TRANSN = pad_transn;
 
-	pad_transp =( *((uint32_t *)(NVMCTRL_OTP4)
-	+ (NVM_USB_PAD_TRANSP_POS / 32))
-	>> (NVM_USB_PAD_TRANSP_POS % 32))
-	& ((1 << NVM_USB_PAD_TRANSP_SIZE) - 1);
-
-	if (pad_transp == 0x1F) {  // maximum value (31)
+	pad_transp = (*((uint32_t *)(NVMCTRL_OTP4)
+					+ (NVM_USB_PAD_TRANSP_POS / 32))
+					>> (NVM_USB_PAD_TRANSP_POS % 32))
+				& ((1 << NVM_USB_PAD_TRANSP_SIZE) - 1);
+
+	if (pad_transp == 0x1F)         // maximum value (31)
+	{
 		pad_transp = 29;
 	}
@@ -117,10 +131,11 @@
 	USB->HOST.PADCAL.bit.TRANSP = pad_transp;
 
-	pad_trim =( *((uint32_t *)(NVMCTRL_OTP4)
-	+ (NVM_USB_PAD_TRIM_POS / 32))
-	>> (NVM_USB_PAD_TRIM_POS % 32))
-	& ((1 << NVM_USB_PAD_TRIM_SIZE) - 1);
-
-	if (pad_trim == 0x7) {  // maximum value (7)
+	pad_trim = (*((uint32_t *)(NVMCTRL_OTP4)
+					+ (NVM_USB_PAD_TRIM_POS / 32))
+				>> (NVM_USB_PAD_TRIM_POS % 32))
+				& ((1 << NVM_USB_PAD_TRIM_SIZE) - 1);
+
+	if (pad_trim == 0x7)         // maximum value (7)
+	{
 		pad_trim = 3;
 	}
@@ -128,42 +143,43 @@
 	USB->HOST.PADCAL.bit.TRIM = pad_trim;
 
+
 	/* Set the configuration */
-	udd_force_host_mode();
-	udd_device_run_in_standby();
-    // Set address of USB SRAM
-	USB->HOST.DESCADD.reg = (uint32_t)(&usb_endpoint_table[0]);
-	// For USB_SPEED_FULL 
-	udd_force_full_speed();
- 	for (uint32_t i = 0; i < sizeof(usb_endpoint_table); i++) {
- 		(*(uint32_t *)(&usb_endpoint_table[0]+i)) = 0;
- 	}
+	uhd_run_in_standby();
+	// Set address of USB SRAM
+	USB->HOST.DESCADD.reg = (uint32_t)(&usb_pipe_table[0]);
+	// For USB_SPEED_FULL
+	uhd_force_full_speed();
+	for (i = 0; i < sizeof(usb_pipe_table); i++)
+	{
+		(*(uint32_t *)(&usb_pipe_table[0] + i)) = 0;
+	}
 
 	uhd_state = UHD_STATE_NO_VBUS;
 
+	// Put VBUS on USB port
+	pinMode( 32ul, OUTPUT );
+	digitalWrite( 32ul, HIGH );
+
+	uhd_enable_connection_int();
+
+	USB->HOST.INTENSET.reg = USB_HOST_INTENSET_DCONN;
+	USB->HOST.INTENSET.reg = USB_HOST_INTENSET_WAKEUP;
+	USB->HOST.INTENSET.reg = USB_HOST_INTENSET_DDISC;
+
 	USB->HOST.CTRLB.bit.VBUSOK = 1;
 
 	// Configure interrupts
-	NVIC_SetPriority((IRQn_Type) USB_IRQn, 0UL);
-	NVIC_EnableIRQ((IRQn_Type) USB_IRQn);
-}
-
-//static void UHD_ISR(void)
-void USB_Handler(void)
-{
-	uint16_t flags;
-	uint8_t i;
-	uint8_t ept_int;
-
-	ept_int = udd_endpoint_interrupt();
-	
-	/* Not endpoint interrupt */
-	if (0 == ept_int)
-	{
-
-
-	}
-	else
-	{
-		/* host interrupts */
+	NVIC_SetPriority((IRQn_Type)USB_IRQn, 0UL);
+	NVIC_EnableIRQ((IRQn_Type)USB_IRQn);
+}
+
+
+static void UHD_ISR(void)
+//void USB_Handler(void)
+{
+   uint16_t flags;
+
+	if (USB->HOST.CTRLA.bit.MODE) {
+		/*host mode ISR */
 
 		/* get interrupt flags */
@@ -171,54 +187,62 @@
 
 		/* host SOF interrupt */
-		if (flags & USB_HOST_INTFLAG_HSOF) {
+		if (flags & USB_HOST_INTFLAG_HSOF)
+		{
 			/* clear the flag */
 			USB->HOST.INTFLAG.reg = USB_HOST_INTFLAG_HSOF;
-			uhd_state = UHD_STATE_CONNECTED;
+			uhd_state             = UHD_STATE_CONNECTED;
 			return;
 		}
 
 		/* host reset interrupt */
-		if (flags & USB_HOST_INTFLAG_RST) {
+		if (flags & USB_HOST_INTFLAG_RST)
+		{
 			/* clear the flag */
 			USB->HOST.INTFLAG.reg = USB_HOST_INTFLAG_RST;
-			uhd_state = UHD_STATE_DISCONNECTED; //UHD_STATE_ERROR;
+			uhd_state             = UHD_STATE_DISCONNECTED;    //UHD_STATE_ERROR;
 			return;
 		}
 
 		/* host upstream resume interrupts */
-		if (flags & USB_HOST_INTFLAG_UPRSM) {
+		if (flags & USB_HOST_INTFLAG_UPRSM)
+		{
 			/* clear the flags */
 			USB->HOST.INTFLAG.reg = USB_HOST_INTFLAG_UPRSM;
-			uhd_state = UHD_STATE_DISCONNECTED; //UHD_STATE_ERROR;
+			uhd_state             = UHD_STATE_DISCONNECTED;    //UHD_STATE_ERROR;
 			return;
 		}
 
 		/* host downstream resume interrupts */
-		if (flags & USB_HOST_INTFLAG_DNRSM) {
+		if (flags & USB_HOST_INTFLAG_DNRSM)
+		{
 			/* clear the flags */
 			USB->HOST.INTFLAG.reg = USB_HOST_INTFLAG_DNRSM;
-			uhd_state = UHD_STATE_DISCONNECTED; //UHD_STATE_ERROR;
+			uhd_state             = UHD_STATE_DISCONNECTED;    //UHD_STATE_ERROR;
 			return;
 		}
 
 		/* host wakeup interrupts */
-		if (flags & USB_HOST_INTFLAG_WAKEUP) {
+		if (flags & USB_HOST_INTFLAG_WAKEUP)
+		{
 			/* clear the flags */
 			USB->HOST.INTFLAG.reg = USB_HOST_INTFLAG_WAKEUP;
-			uhd_state = UHD_STATE_CONNECTED; //UHD_STATE_ERROR;
+			uhd_state             = UHD_STATE_CONNECTED;    //UHD_STATE_ERROR;
 			return;
 		}
 
 		/* host ram access interrupt  */
-		if (flags & USB_HOST_INTFLAG_RAMACER) {
+		if (flags & USB_HOST_INTFLAG_RAMACER)
+		{
 			/* clear the flag */
 			USB->HOST.INTFLAG.reg = USB_HOST_INTFLAG_RAMACER;
-			uhd_state = UHD_STATE_DISCONNECTED; //UHD_STATE_ERROR;
+			uhd_state             = UHD_STATE_DISCONNECTED;    //UHD_STATE_ERROR;
 			return;
 		}
 
 		/* host connect interrupt */
-		if (flags & USB_HOST_INTFLAG_DCONN) {
-			TRACE_UOTGHS_HOST(printf(">>> UHD_ISR : Connection INT\r\n");)
+		if (flags & USB_HOST_INTFLAG_DCONN)
+		{
+			TRACE_UOTGHS_HOST(printf(">>> UHD_ISR : Connection INT\r\n");
+							)
 			/* clear the flag */
 			uhd_ack_connection();
@@ -231,7 +255,9 @@
 		}
 
-		/* host disconnect interrupt 	*/
-		if (flags & USB_HOST_INTFLAG_DDISC) {
-			TRACE_UOTGHS_HOST(printf(">>> UHD_ISR : Disconnection INT\r\n");)
+		/* host disconnect interrupt  */
+		if (flags & USB_HOST_INTFLAG_DDISC)
+		{
+			TRACE_UOTGHS_HOST(printf(">>> UHD_ISR : Disconnection INT\r\n");
+							)
 			/* clear the flag */
 			uhd_ack_disconnection();
@@ -246,19 +272,11 @@
 			return;
 		}
-
-	}
-
-}
-
-
-
-
-/**
- * \brief Trigger a USB bus reset.
- */
-void UHD_BusReset(void)
-{
-	USB->HOST.CTRLB.bit.BUSRESET = 1;;
-}
+	}
+	else {
+		while(1);
+	}
+}
+
+
 
 /**
@@ -269,7 +287,6 @@
 uhd_vbus_state_t UHD_GetVBUSState(void)
 {
-	return uhd_state;
-}
-
+   return uhd_state;
+}
 
 
@@ -283,31 +300,20 @@
  * \retval 1 error.
  */
-uint32_t UHD_Pipe0_Alloc(uint32_t ul_add, uint32_t ul_ep_size)
-{
-	struct usb_host_pipe_config cfg;
-
-	if (ep_size < 8)
-	{
-		return 0;
-	}
-
-	/* set pipe config */
-	USB->HOST.HostPipe[0].PCFG.bit.BK = 0;
-	USB->HOST.HostPipe[0].PCFG.bit.PTYPE = USB_HOST_PIPE_TYPE_CONTROL;
-	USB->HOST.HostPipe[0].BINTERVAL.reg = 0;
-	USB->HOST.HostPipe[0].PCFG.bit.PTOKEN = USB_HOST_PIPE_TOKEN_SETUP;
-
-	memset((uint8_t *)&usb_pipe_table[pipe_num], 0, sizeof(usb_pipe_table[0]));
-	usb_pipe_table[pipe_num].HostDescBank[0].CTRL_PIPE.bit.PDADDR = 0;
-	usb_pipe_table[pipe_num].HostDescBank[0].CTRL_PIPE.bit.PEPNUM = 0;
-	usb_pipe_table[pipe_num].HostDescBank[0].PCKSIZE.bit.SIZE = 0x03;  // 64 bytes
-
-	USB->HOST.HostPipe[pipe_num].PINTENSET.reg = USB_HOST_PINTENSET_TRCPT_Msk;
-	USB->HOST.HostPipe[pipe_num].PINTENSET.reg = USB_HOST_PINTENSET_TRFAIL | USB_HOST_PINTENSET_PERR;
-	USB->HOST.HostPipe[pipe_num].PINTENSET.reg = USB_HOST_PINTENSET_TXSTP;
-	USB->HOST.HostPipe[pipe_num].PINTENSET.reg = USB_HOST_PINTENSET_STALL;
-
-	return 1;
-}
+uint32_t UHD_Pipe0_Alloc(uint32_t ul_add , uint32_t ul_ep_size)
+{
+	(void)(ul_add); // Unused argument
+
+	if( USB->HOST.STATUS.reg & USB_HOST_STATUS_SPEED(1) )
+		ul_ep_size = USB_PCKSIZE_SIZE_8_BYTES;  // Low Speed
+	else
+		ul_ep_size = USB_PCKSIZE_SIZE_64_BYTES; // Full Speed
+
+	USB->HOST.HostPipe[0].PCFG.bit.PTYPE = 1; //USB_HOST_PCFG_PTYPE_CTRL;
+	usb_pipe_table[0].HostDescBank[0].CTRL_PIPE.bit.PEPNUM = 0;
+	usb_pipe_table[0].HostDescBank[0].PCKSIZE.bit.SIZE = ul_ep_size;
+
+	return 0;
+}
+
 
 /**
@@ -336,32 +342,39 @@
 {
 	/* set pipe config */
-	USB->HOST.HostPipe[ul_dev_ep].PCFG.bit.BK = ul_nb_bank;
-	USB->HOST.HostPipe[ul_dev_ep].PCFG.bit.PTYPE = ul_type;
-	USB->HOST.HostPipe[ul_dev_ep].BINTERVAL.reg = ul_interval;
-
-	if (ul_dir & USB_EP_DIR_IN)
-	{
-		USB->HOST.HostPipe[ul_dev_ep].PCFG.bit.PTOKEN = USB_HOST_PIPE_TOKEN_IN;
-		USB->HOST.HostPipe[ul_dev_ep].PSTATUSSET.reg = USB_HOST_PSTATUSSET_BK0RDY;
-	}
+	USB->HOST.HostPipe[ul_dev_ep].PCFG.bit.BK    = ul_nb_bank;
+	USB->HOST.HostPipe[ul_dev_ep].PCFG.reg &= ~USB_HOST_PCFG_MASK;  // USB->HOST.HostPipe[0].PCFG.bit.PTYPE = 1; //USB_HOST_PCFG_PTYPE_CTRL;
+	USB->HOST.HostPipe[ul_dev_ep].PCFG.reg |= ul_type;
+	USB->HOST.HostPipe[ul_dev_ep].BINTERVAL.reg  = ul_interval;
+
+   if (ul_dir & USB_EP_DIR_IN)
+   {
+      USB->HOST.HostPipe[ul_dev_ep].PCFG.bit.PTOKEN = USB_HOST_PCFG_PTOKEN_IN;
+      USB->HOST.HostPipe[ul_dev_ep].PSTATUSSET.reg  = USB_HOST_PSTATUSSET_BK0RDY;
+   }
+   else
+   {
+      USB->HOST.HostPipe[ul_dev_ep].PCFG.bit.PTOKEN = USB_HOST_PCFG_PTOKEN_OUT;
+      USB->HOST.HostPipe[ul_dev_ep].PSTATUSCLR.reg  = USB_HOST_PSTATUSCLR_BK0RDY;
+   }
+
+	if( USB->HOST.STATUS.reg & USB_HOST_STATUS_SPEED(1) )
+	   ul_maxsize = USB_PCKSIZE_SIZE_8_BYTES;  // Low Speed
 	else
-	{
-		USB->HOST.HostPipe[ul_dev_ep].PCFG.bit.PTOKEN = USB_HOST_PIPE_TOKEN_OUT;
-		USB->HOST.HostPipe[ul_dev_ep].PSTATUSCLR.reg =  USB_HOST_PSTATUSCLR_BK0RDY;
-	}
-
-	memset((uint8_t *)&usb_descriptor_table.usb_pipe_table[ul_dev_ep], 0, sizeof(usb_pipe_table[ul_dev_ep]));
-
-	usb_descriptor_table.usb_pipe_table[ul_dev_ep].HostDescBank[0].CTRL_PIPE.bit.PDADDR = ul_dev_addr;
-	usb_descriptor_table.usb_pipe_table[ul_dev_ep].HostDescBank[0].CTRL_PIPE.bit.PEPNUM = ul_dev_ep;
-	usb_descriptor_table.usb_pipe_table[ul_dev_ep].HostDescBank[0].PCKSIZE.bit.SIZE = 0x03;  // 64 bytes
-
-	USB->HOST.HostPipe[pipe_num].PINTENSET.reg = USB_HOST_PINTENSET_TRCPT_Msk;
-	USB->HOST.HostPipe[pipe_num].PINTENSET.reg = USB_HOST_PINTENSET_TRFAIL | USB_HOST_PINTENSET_PERR;
-	USB->HOST.HostPipe[pipe_num].PINTENSET.reg = USB_HOST_PINTENSET_STALL;
-
-	return 1;
-}
-
+	   ul_maxsize = USB_PCKSIZE_SIZE_64_BYTES; // Full Speed
+
+   memset((uint8_t *)&usb_pipe_table[ul_dev_ep], 0, sizeof(usb_pipe_table[ul_dev_ep]));
+
+   usb_pipe_table[ul_dev_ep].HostDescBank[0].CTRL_PIPE.bit.PDADDR = ul_dev_addr;
+   usb_pipe_table[ul_dev_ep].HostDescBank[0].CTRL_PIPE.bit.PEPNUM = ul_dev_ep;
+   usb_pipe_table[ul_dev_ep].HostDescBank[0].PCKSIZE.bit.SIZE     = ul_maxsize;
+
+   return 1;
+}
+
+
+void UHD_Pipe_CountZero(uint32_t ul_pipe)
+{
+	usb_pipe_table[ul_pipe].HostDescBank[0].PCKSIZE.bit.BYTE_COUNT = 0;
+}
 
 /**
@@ -372,12 +385,8 @@
 void UHD_Pipe_Free(uint32_t ul_pipe)
 {
-	// Unalloc pipe
-	uhd_disable_pipe(ul_pipe);
-	uhd_unallocate_memory(ul_pipe);
-	uhd_reset_pipe(ul_pipe);
-
-	// The Pipe is frozen and no additional requests will be sent to the device on this pipe address.
-	USB->HOST.HostPipe[pipe_num].PSTATUSSET.reg = USB_HOST_PSTATUSSET_PFREEZE;
-}
+   // The Pipe is frozen and no additional requests will be sent to the device on this pipe address.
+   USB->HOST.HostPipe[ul_pipe].PSTATUSSET.reg = USB_HOST_PSTATUSSET_PFREEZE;
+}
+
 
 /**
@@ -390,25 +399,26 @@
  * \return number of data read.
  */
-uint32_t UHD_Pipe_Read(uint32_t pipe_num, uint32_t buf_size, uint8_t* buf)
-{
-	if (USB->HOST.HostPipe[pipe_num].PCFG.bit.PTYPE == USB_HOST_PIPE_TYPE_DISABLE)
-	{
-		return 0;
-	}
-
-	/* get pipe config from setting register */
-	usb_descriptor_table.usb_pipe_table[pipe_num].HostDescBank[0].ADDR.reg = (uint32_t)buf;
-	usb_descriptor_table.usb_pipe_table[pipe_num].HostDescBank[0].PCKSIZE.bit.BYTE_COUNT = 0;
-	usb_descriptor_table.usb_pipe_table[pipe_num].HostDescBank[0].PCKSIZE.bit.MULTI_PACKET_SIZE = buf_size;
-	USB->HOST.HostPipe[pipe_num].PCFG.bit.PTOKEN = USB_HOST_PIPE_TOKEN_IN;
-
-	/* Start transfer */
-	USB->HOST.HostPipe[pipe_num].PSTATUSCLR.reg = USB_HOST_PSTATUSCLR_BK0RDY;
-
-	// Unfreeze pipe
-	USB->HOST.HostPipe[pipe_num].PSTATUSCLR.reg = USB_HOST_PSTATUSCLR_PFREEZE;
-
-	return buf_size;
-}
+uint32_t UHD_Pipe_Read(uint32_t pipe_num, uint32_t buf_size, uint8_t *buf)
+{
+   if (USB->HOST.HostPipe[pipe_num].PCFG.bit.PTYPE == USB_HOST_PTYPE_DIS)
+   {
+      return 0;
+   }
+
+   /* get pipe config from setting register */
+   usb_pipe_table[pipe_num].HostDescBank[0].ADDR.reg = (uint32_t)buf;
+   usb_pipe_table[pipe_num].HostDescBank[0].PCKSIZE.bit.BYTE_COUNT        = 0;
+   usb_pipe_table[pipe_num].HostDescBank[0].PCKSIZE.bit.MULTI_PACKET_SIZE = buf_size;
+   USB->HOST.HostPipe[pipe_num].PCFG.bit.PTOKEN = USB_HOST_PCFG_PTOKEN_IN;
+
+   /* Start transfer */
+   USB->HOST.HostPipe[pipe_num].PSTATUSCLR.reg = USB_HOST_PSTATUSCLR_BK0RDY;
+
+   // Unfreeze pipe
+   USB->HOST.HostPipe[pipe_num].PSTATUSCLR.reg = USB_HOST_PSTATUSCLR_PFREEZE;
+
+   return buf_size;
+}
+
 
 /**
@@ -419,20 +429,10 @@
  * \param data Buffer containing data to write.
  */
-void UHD_Pipe_Write(uint32_t ul_pipe, uint32_t ul_size, uint8_t* data)
-{
-
-	if (USB->HOST.HostPipe[pipe_num].PCFG.bit.PTYPE == USB_HOST_PIPE_TYPE_DISABLE) 
-	{
-		return 0;
-	}
-
-	/* get pipe config from setting register */
-	usb_descriptor_table.usb_pipe_table[pipe_num].HostDescBank[0].ADDR.reg = (uint32_t)buf;
-	usb_descriptor_table.usb_pipe_table[pipe_num].HostDescBank[0].PCKSIZE.bit.BYTE_COUNT = buf_size;
-	usb_descriptor_table.usb_pipe_table[pipe_num].HostDescBank[0].PCKSIZE.bit.MULTI_PACKET_SIZE = 0;
-	USB->HOST.HostPipe[pipe_num].PCFG.bit.PTOKEN = USB_HOST_PIPE_TOKEN_OUT;
-
-
-	return 1;
+void UHD_Pipe_Write(uint32_t ul_pipe, uint32_t ul_size, uint8_t *buf)
+{
+   /* get pipe config from setting register */
+   usb_pipe_table[ul_pipe].HostDescBank[0].ADDR.reg = (uint32_t)buf;
+   usb_pipe_table[ul_pipe].HostDescBank[0].PCKSIZE.bit.BYTE_COUNT = ul_size;
+   usb_pipe_table[ul_pipe].HostDescBank[0].PCKSIZE.bit.MULTI_PACKET_SIZE = 0;
 }
 
@@ -445,10 +445,29 @@
 void UHD_Pipe_Send(uint32_t ul_pipe, uint32_t ul_token_type)
 {
-	/* Start transfer */
-	USB->HOST.HostPipe[pipe_num].PSTATUSSET.reg = USB_HOST_PSTATUSSET_BK0RDY;
-
+	USB->HOST.HostPipe[ul_pipe].PCFG.bit.PTOKEN = ul_token_type;
+
+   /* Start transfer */
+	if(ul_token_type == USB_HOST_PCFG_PTOKEN_SETUP )
+	{
+		USB->HOST.HostPipe[ul_pipe].PINTFLAG.reg = USB_HOST_PINTFLAG_TXSTP;
+		USB->HOST.HostPipe[ul_pipe].PSTATUSSET.reg = USB_HOST_PSTATUSSET_BK0RDY;
+	}
+	else if(ul_token_type == USB_HOST_PCFG_PTOKEN_IN )
+	{
+		USB->HOST.HostPipe[ul_pipe].PSTATUSCLR.reg = USB_HOST_PSTATUSCLR_BK0RDY;
+	}
+	else
+	{
+		USB->HOST.HostPipe[ul_pipe].PINTFLAG.reg = USB_HOST_PINTFLAG_TRCPT(1);  // Transfer Complete 0
+		USB->HOST.HostPipe[ul_pipe].PSTATUSSET.reg = USB_HOST_PSTATUSSET_BK0RDY;
+	}
+   
 	// Unfreeze pipe
-	USB->HOST.HostPipe[pipe_num].PSTATUSCLR.reg = USB_HOST_PSTATUSCLR_PFREEZE;
-}
+    uhd_unfreeze_pipe(ul_pipe);
+}
+
+#define USB_HOST_PINTFLAG_TRCPT_Pos 0            /**< \brief (USB_HOST_PINTFLAG) Transfer Complete 0/1 Interrupt Flag */
+#define USB_HOST_PINTFLAG_TRCPT_Msk (0x3u << USB_HOST_PINTFLAG_TRCPT_Pos)
+#define USB_HOST_PINTFLAG_TRCPT(value) ((USB_HOST_PINTFLAG_TRCPT_Msk & ((value) << USB_HOST_PINTFLAG_TRCPT_Pos)))
 
 /**
@@ -463,24 +482,40 @@
 uint32_t UHD_Pipe_Is_Transfer_Complete(uint32_t ul_pipe, uint32_t ul_token_type)
 {
-
-	// Freeze pipe
-	USB->HOST.HostPipe[pipe_num].PSTATUSSET.reg = USB_HOST_PSTATUSSET_PFREEZE;
-	switch(uhd_ctrl_request_phase) {
-		case UHD_CTRL_REQ_PHASE_DATA_IN:
-		_uhd_ctrl_phase_data_in(p_callback_para->transfered_size);
-		break;
-		case UHD_CTRL_REQ_PHASE_ZLP_IN:
-		_uhd_ctrl_request_end(UHD_TRANS_NOERROR);
-		break;
-		case UHD_CTRL_REQ_PHASE_DATA_OUT:
-		_uhd_ctrl_phase_data_out();
-		break;
-		case UHD_CTRL_REQ_PHASE_ZLP_OUT:
-		_uhd_ctrl_request_end(UHD_TRANS_NOERROR);
-		break;
-	}
-	return 0;
-}
-
-#endif 
-
+   // Check for transfer completion depending on token type
+   switch (ul_token_type)
+   {
+      case USB_HOST_PCFG_PTOKEN_SETUP:
+         if (Is_uhd_setup_ready(ul_pipe))
+         {
+            uhd_ack_setup_ready(ul_pipe);
+            uhd_freeze_pipe(ul_pipe);
+            return 1;
+         }
+		 break;
+
+      case USB_HOST_PCFG_PTOKEN_IN:
+         if (Is_uhd_in_received(ul_pipe))
+         {
+            // IN packet received
+            uhd_ack_in_received(ul_pipe);
+			// Freeze will stop after the transfer
+			uhd_freeze_pipe(ul_pipe);
+            return 1;
+         }
+		 break;
+ 
+      case USB_HOST_PCFG_PTOKEN_OUT:
+         if (Is_uhd_out_ready(ul_pipe))
+         {
+            // OUT packet sent
+            uhd_ack_out_ready(ul_pipe);
+            uhd_freeze_pipe(ul_pipe);
+            return 1;
+         }
+		 break;
+   }
+
+   return 0;
+}
+
+#endif //  HOST_DEFINED
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/samd21_host.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/samd21_host.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/USB/samd21_host.h	(revision 224)
@@ -1,34 +1,25 @@
-/* ----------------------------------------------------------------------------
- *         SAM Software Package License
- * ----------------------------------------------------------------------------
- * Copyright (c) 2011-2012, Atmel Corporation
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following condition is met:
- *
- * - Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the disclaimer below.
- *
- * Atmel's name may not be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
- * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
- * DISCLAIMED. IN NO EVENT SHALL ATMEL 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.
- * ----------------------------------------------------------------------------
- */
+/*
+  Copyright (c) 2014 Arduino LLC.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+  See the GNU Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+/*
+**Modified 04/04/2016 by Arduino.org development team
+*/
 
 #ifndef UOTGHS_HOST_H_INCLUDED
 #define UOTGHS_HOST_H_INCLUDED
-
 
 #ifdef __cplusplus
@@ -36,360 +27,102 @@
 #endif
 
-//! \ingroup usb_host_group
-//! \defgroup uhd_group USB Host Driver (UHD)
-//! UOTGHS low-level driver for USB host mode
-//!
-//! @{
+extern __attribute__((__aligned__(4))) volatile UsbHostDescriptor usb_pipe_table[USB_EPT_NUM];
 
-//! @name UOTGHS Host IP properties
-//!
-//! @{
-//! Get maximal number of endpoints
-#define uhd_get_pipe_max_nbr()                (9)
-#define UOTGHS_EPT_NUM                        (uhd_get_pipe_max_nbr()+1)
-//! @}
+#define  USB_EP_DIR_IN        0x80  // USB_SETUP_DEVICE_TO_HOST
+#define  USB_EP_DIR_OUT       0x00  // USB_SETUP_HOST_TO_DEVICE
 
-//! @name Host Vbus line control
-//!
-//! VBOF is an optional output pin which allows to enable or disable
-//! the external VBus generator.
-//!
-//! @{
-//! Enables hardware control of USB_VBOF output pin when a Vbus error occur
-#define uhd_enable_vbus_error_hw_control()    (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBUSHWC))
-//! Disables hardware control of USB_VBOF output pin when a Vbus error occur
-#define uhd_disable_vbus_error_hw_control()   (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBUSHWC))
+#define USB_HOST_PTYPE_DIS     USB_HOST_PCFG_PTYPE(0x0) // Pipe is disabled
+#define USB_HOST_PTYPE_CTRL    USB_HOST_PCFG_PTYPE(0x1) // Pipe is enabled and configured as CONTROL
+#define USB_HOST_PTYPE_ISO     USB_HOST_PCFG_PTYPE(0x2) // Pipe is enabled and configured as ISO
+#define USB_HOST_PTYPE_BULK    USB_HOST_PCFG_PTYPE(0x3) // Pipe is enabled and configured as BULK
+#define USB_HOST_PTYPE_INT     USB_HOST_PCFG_PTYPE(0x4) // Pipe is enabled and configured as INTERRUPT
+#define USB_HOST_PTYPE_EXT     USB_HOST_PCFG_PTYPE(0x5) // Pipe is enabled and configured as EXTENDED
 
-//! Pin and function for USB_VBOF according to configuration from USB_VBOF
-#define USB_VBOF_PIN            USB_VBOF_GPIO
-#define USB_VBOF_FUNCTION       USB_VBOF_FLAGS
-//! Output USB_VBOF onto its pin
-#define uhd_output_vbof_pin() do {\
-	pio_configure_pin(USB_VBOF_PIN, USB_VBOF_FUNCTION); \
-} while (0)
+#define USB_HOST_NB_BK_1		1
 
-//! Set USB_VBOF output pin polarity
-#define uhd_set_vbof_active_high()            (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBUSPO))
-#define uhd_set_vbof_active_low()             (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBUSPO))
-//! Requests VBus activation
-#define uhd_enable_vbus()                     (Set_bits(UOTGHS->UOTGHS_SFR, UOTGHS_SR_VBUSRQ))
-//! Requests VBus deactivation
-#define uhd_disable_vbus()                    (Set_bits(UOTGHS->UOTGHS_SCR, UOTGHS_SR_VBUSRQ))
-//! Tests if VBus activation has been requested
-#define Is_uhd_vbus_enabled()                 (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_VBUSRQ))
-//! @}
+#define USB_HOST_PCFG_PTOKEN_SETUP  USB_HOST_PCFG_PTOKEN(0x0)
+#define USB_HOST_PCFG_PTOKEN_IN     USB_HOST_PCFG_PTOKEN(0x1)
+#define USB_HOST_PCFG_PTOKEN_OUT    USB_HOST_PCFG_PTOKEN(0x2)
 
-//! @name Host Vbus line monitoring
-//!
-//! The VBus level is always checked by USBC hardware.
-//!
-//! @{
-#define uhd_enable_vbus_error_interrupt()     (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBERRE))
-#define uhd_disable_vbus_error_interrupt()    (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBERRE))
-#define Is_uhd_vbus_error_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBERRE))
-#define uhd_ack_vbus_error_interrupt()        (Set_bits(UOTGHS->UOTGHS_SCR, UOTGHS_SCR_VBERRIC))
-#define Is_uhd_vbus_error_interrupt()         (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_VBERRI))
-//! @}
+#define USB_ERRORFLOW	      USB_HOST_STATUS_BK_ERRORFLOW
+#define USB_ERRORTIMEOUT      USB_HOST_STATUS_PIPE_TOUTER
+#define USB_ERROR_DATATOGGLE  USB_HOST_STATUS_PIPE_DTGLER
 
-#define uhd_ack_errors_interrupt()            (UOTGHS->UOTGHS_SCR = (UOTGHS_SCR_VBERRIC|UOTGHS_SCR_BCERRIC|UOTGHS_SCR_HNPERRIC|UOTGHS_SCR_STOIC))
-#define Is_uhd_errors_interrupt()             (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBERRE|UOTGHS_CTRL_BCERRE|UOTGHS_CTRL_HNPERRE|UOTGHS_CTRL_STOE))
-#define uhd_enable_suspend_error_interrupt()
-#define uhd_enable_hnp_error_interrupt()
-#define uhd_enable_bconn_error_interrupt()
+#define USB_PCKSIZE_SIZE_8_BYTES        0
+#define USB_PCKSIZE_SIZE_16_BYTES       1
+#define USB_PCKSIZE_SIZE_32_BYTES       2
+#define USB_PCKSIZE_SIZE_64_BYTES       3 
+#define USB_PCKSIZE_SIZE_128_BYTES      4
+#define USB_PCKSIZE_SIZE_256_BYTES      5
+#define USB_PCKSIZE_SIZE_512_BYTES      6
+#define USB_PCKSIZE_SIZE_1023_BYTES_FS  7   
+#define USB_PCKSIZE_SIZE_1024_BYTES_HS  7  
 
-//! @name USB device connection/disconnection monitoring
-//! @{
-#define uhd_enable_connection_int()           (UOTGHS->UOTGHS_HSTIER = UOTGHS_HSTIER_DCONNIES)
-#define uhd_disable_connection_int()          (UOTGHS->UOTGHS_HSTIDR = UOTGHS_HSTIDR_DCONNIEC)
-#define Is_uhd_connection_int_enabled()       (Tst_bits(UOTGHS->UOTGHS_HSTIMR, UOTGHS_HSTIMR_DCONNIE))
-#define uhd_ack_connection()                  (UOTGHS->UOTGHS_HSTICR = UOTGHS_HSTICR_DCONNIC)
-#define Is_uhd_connection()                   (Tst_bits(UOTGHS->UOTGHS_HSTISR, UOTGHS_HSTISR_DCONNI))
+#define USB_HOST_DTGL(p)               (USB->HOST.HostPipe[p].PSTATUS.reg & USB_HOST_PSTATUS_DTGL)>>USB_HOST_PSTATUS_DTGL_Pos
 
-#define uhd_enable_disconnection_int()        (UOTGHS->UOTGHS_HSTIER = UOTGHS_HSTIER_DDISCIES)
-#define uhd_disable_disconnection_int()       (UOTGHS->UOTGHS_HSTIDR = UOTGHS_HSTIDR_DDISCIEC)
-#define Is_uhd_disconnection_int_enabled()    (Tst_bits(UOTGHS->UOTGHS_HSTIMR, UOTGHS_HSTIMR_DDISCIE))
-#define uhd_ack_disconnection()               (UOTGHS->UOTGHS_HSTICR = UOTGHS_HSTICR_DDISCIC)
-#define Is_uhd_disconnection()                (Tst_bits(UOTGHS->UOTGHS_HSTISR, UOTGHS_HSTISR_DDISCI))
-//! @}
+// USB host connection/disconnection monitoring
+#define uhd_enable_connection_int()           USB->HOST.INTENSET.reg = USB_HOST_INTENSET_DCONN
+#define uhd_disable_connection_int()          USB->HOST.INTENCLR.reg = USB_HOST_INTENCLR_DCONN
+#define uhd_ack_connection()                  USB->HOST.INTFLAG.reg = USB_HOST_INTFLAG_DCONN
 
-//! @name USB device speed control
-//! @{
-#define uhd_get_speed_mode()                  (Rd_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_SPEED_Msk))
-#define Is_uhd_low_speed_mode()                 (uhd_get_speed_mode() == UOTGHS_SR_SPEED_LOW_SPEED)
-#define Is_uhd_full_speed_mode()                (uhd_get_speed_mode() == UOTGHS_SR_SPEED_FULL_SPEED)
-#define Is_uhd_high_speed_mode()                (uhd_get_speed_mode() == UOTGHS_SR_SPEED_HIGH_SPEED)
-//! Enable high speed mode
-# define uhd_enable_high_speed_mode()        (Wr_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_SPDCONF_Msk, UOTGHS_HSTCTRL_SPDCONF_HIGH_SPEED))
-//! Disable high speed mode
-# define uhd_disable_high_speed_mode()       (Wr_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_SPDCONF_Msk, UOTGHS_HSTCTRL_SPDCONF_FORCED_FS))
-//! @}
+#define uhd_enable_disconnection_int()        USB->HOST.INTENSET.reg = USB_HOST_INTENSET_DDISC
+#define uhd_disable_disconnection_int()       USB->HOST.INTENCLR.reg = USB_HOST_INTENCLR_DDISC
+#define uhd_ack_disconnection()               USB->HOST.INTFLAG.reg = USB_HOST_INTFLAG_DDISC
 
-//! @name Bus events control
-//! These macros manage the bus events: reset, SOF, resume, wakeup.
-//! @{
+// Initiates a USB register reset
+#define uhd_start_USB_reg_reset()             USB->HOST.CTRLA.bit.SWRST = 1;
 
-//! Initiates a reset event
-//! @{
-#define uhd_start_reset()                            (Set_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_RESET))
-#define Is_uhd_starting_reset()                      (Tst_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_RESET))
-#define uhd_stop_reset()                             (Clr_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_RESET))
+// Bus Reset
+#define Is_uhd_starting_reset()             (USB->HOST.CTRLB.bit.BUSRESET == 1)
+#define UHD_BusReset()                      USB->HOST.CTRLB.bit.BUSRESET = 1
+#define uhd_stop_reset()                    // nothing to do
 
-#define uhd_enable_reset_sent_interrupt()            (UOTGHS->UOTGHS_HSTIER = UOTGHS_HSTIER_RSTIES)
-#define uhd_disable_reset_sent_interrupt()           (UOTGHS->UOTGHS_HSTIDR = UOTGHS_HSTIDR_RSTIEC)
-#define Is_uhd_reset_sent_interrupt_enabled()        (Tst_bits(UOTGHS->UOTGHS_HSTIMR, UOTGHS_HSTIMR_RSTIE))
-#define uhd_ack_reset_sent()                         (UOTGHS->UOTGHS_HSTICR = UOTGHS_HSTICR_RSTIC)
-#define Is_uhd_reset_sent()                          (Tst_bits(UOTGHS->UOTGHS_HSTISR, UOTGHS_HSTISR_RSTI))
-//! @}
+#define uhd_ack_reset_sent()                         USB->HOST.INTFLAG.reg = USB_HOST_INTFLAG_RST
+#define Is_uhd_reset_sent()                          (USB->HOST.INTFLAG.reg & USB_HOST_INTFLAG_RST)
 
-//! Initiates a SOF events
-//! @{
-#define uhd_enable_sof()                             (Set_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_SOFE))
-#define uhd_disable_sof()                            (Clr_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_SOFE))
-#define Is_uhd_sof_enabled()                         (Tst_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_SOFE))
-#define uhd_get_sof_number()                         ((UOTGHS->UOTGHS_HSTFNUM&UOTGHS_HSTFNUM_FNUM_Msk)>>UOTGHS_HSTFNUM_FNUM_Pos)
-#define uhd_get_microsof_number()                    ((UOTGHS->UOTGHS_HSTFNUM&UOTGHS_HSTFNUM_MFNUM_Msk)>>UOTGHS_HSTFNUM_MFNUM_Pos)
-#define uhd_get_frame_position()                     (Rd_bits(UOTGHS->UOTGHS_HSTFNUM, UOTGHS_HSTFNUM_FLENHIGH_Msk))
-#define uhd_enable_sof_interrupt()                   (UOTGHS->UOTGHS_HSTIER = UOTGHS_HSTIER_HSOFIES)
-#define uhd_disable_sof_interrupt()                  (UOTGHS->UOTGHS_HSTIDR = UOTGHS_HSTIDR_HSOFIEC)
-#define Is_uhd_sof_interrupt_enabled()               (Tst_bits(UOTGHS->UOTGHS_HSTIMR, UOTGHS_HSTIMR_HSOFIE))
-#define uhd_ack_sof()                                (UOTGHS->UOTGHS_HSTICR = UOTGHS_HSTICR_HSOFIC)
-#define Is_uhd_sof()                                 (Tst_bits(UOTGHS->UOTGHS_HSTISR, UOTGHS_HSTISR_HSOFI))
-//! @}
+// Initiates a SOF events
+#define uhd_enable_sof()                             USB->HOST.CTRLB.bit.SOFE = 1
+#define uhd_disable_sof()                            USB->HOST.CTRLB.bit.SOFE = 0
+#define Is_uhd_sof_enabled()                         (USB->HOST.CTRLB & USB_HOST_CTRLB_SOFE)
+#define Is_uhd_sof()                                 (USB->HOST.INTFLAG.reg & USB_HOST_INTFLAG_HSOF)
 
-//! Initiates a resume event
-//! It is called downstream resume event.
-//! @{
-#define uhd_send_resume()                            (Set_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_RESUME))
-#define Is_uhd_sending_resume()                      (Tst_bits(UOTGHS->UOTGHS_HSTCTRL, UOTGHS_HSTCTRL_RESUME))
+// USB address of pipes
+#define uhd_configure_address(pipe_num, addr) usb_pipe_table[pipe_num].HostDescBank[0].CTRL_PIPE.bit.PDADDR = addr
+#define uhd_get_configured_address(pipe_num)  usb_pipe_table[pipe_num].HostDescBank[0].CTRL_PIPE.bit.PDADDR
 
-#define uhd_enable_downstream_resume_interrupt()     (UOTGHS->UOTGHS_HSTIER = UOTGHS_HSTIER_RSMEDIES)
-#define uhd_disable_downstream_resume_interrupt()    (UOTGHS->UOTGHS_HSTIDR = UOTGHS_HSTIDR_RSMEDIEC)
-#define Is_uhd_downstream_resume_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_HSTIMR, UOTGHS_HSTIMR_RSMEDIE))
-#define uhd_ack_downstream_resume()                  (UOTGHS->UOTGHS_HSTICR = UOTGHS_HSTICR_RSMEDIC)
-#define Is_uhd_downstream_resume()                   (Tst_bits(UOTGHS->UOTGHS_HSTISR, UOTGHS_HSTISR_RSMEDI))
-//! @}
+// Pipes
+#define uhd_freeze_pipe(p)                       USB->HOST.HostPipe[p].PSTATUSSET.reg = USB_HOST_PSTATUSSET_PFREEZE
+#define uhd_unfreeze_pipe(p)                     USB->HOST.HostPipe[p].PSTATUSCLR.reg = USB_HOST_PSTATUSCLR_PFREEZE
+#define Is_uhd_pipe_frozen(p)                    ((USB->HOST.HostPipe[p].PSTATUS.reg&USB_HOST_PSTATUS_PFREEZE)==USB_HOST_PSTATUS_PFREEZE)
 
-//! Detection of a wake-up event
-//! A wake-up event is received when the host controller is in the suspend mode:
-//! - and an upstream resume from the peripheral is detected.
-//! - and a peripheral disconnection is detected.
-//! @{
-#define uhd_enable_wakeup_interrupt()                (UOTGHS->UOTGHS_HSTIER = UOTGHS_HSTIER_HWUPIES)
-#define uhd_disable_wakeup_interrupt()               (UOTGHS->UOTGHS_HSTIDR = UOTGHS_HSTIDR_HWUPIEC)
-#define Is_uhd_wakeup_interrupt_enabled()            (Tst_bits(UOTGHS->UOTGHS_HSTIMR, UOTGHS_HSTIMR_HWUPIE))
-#define uhd_ack_wakeup()                             (UOTGHS->UOTGHS_HSTICR = UOTGHS_HSTICR_HWUPIC)
-#define Is_uhd_wakeup()                              (Tst_bits(UOTGHS->UOTGHS_HSTISR, UOTGHS_HSTISR_HWUPI))
+// Pipe configuration
+#define uhd_configure_pipe_token(p, token)       USB->HOST.HostPipe[p].PCFG.bit.PTOKEN = token
 
-#define uhd_enable_upstream_resume_interrupt()       (UOTGHS->UOTGHS_HSTIER = UOTGHS_HSTIER_RXRSMIES)
-#define uhd_disable_upstream_resume_interrupt()      (UOTGHS->UOTGHS_HSTIDR = UOTGHS_HSTIDR_RXRSMIEC)
-#define Is_uhd_upstream_resume_interrupt_enabled()   (Tst_bits(UOTGHS->UOTGHS_HSTIMR, UOTGHS_HSTIMR_RXRSMIE))
-#define uhd_ack_upstream_resume()                    (UOTGHS->UOTGHS_HSTICR = UOTGHS_HSTICR_RXRSMIC)
-#define Is_uhd_upstream_resume()                     (Tst_bits(UOTGHS->UOTGHS_HSTISR, UOTGHS_HSTISR_RXRSMI))
-//! @}
-//! @}
+// Pipe data management
+#define uhd_byte_count(p)                        usb_pipe_table[p].HostDescBank[0].PCKSIZE.bit.BYTE_COUNT
+#define uhd_ack_setup_ready(p)                   USB->HOST.HostPipe[p].PINTFLAG.reg = USB_HOST_PINTFLAG_TXSTP
+#define Is_uhd_setup_ready(p)                    ((USB->HOST.HostPipe[p].PINTFLAG.reg&USB_HOST_PINTFLAG_TXSTP) == USB_HOST_PINTFLAG_TXSTP)
+#define uhd_ack_in_received(p)                   USB->HOST.HostPipe[p].PINTFLAG.reg = USB_HOST_PINTFLAG_TRCPT(1)
+#define Is_uhd_in_received(p)                    ((USB->HOST.HostPipe[p].PINTFLAG.reg&USB_HOST_PINTFLAG_TRCPT(1)) == USB_HOST_PINTFLAG_TRCPT(1))
+#define uhd_ack_out_ready(p)                     USB->HOST.HostPipe[p].PINTFLAG.reg = USB_HOST_PINTFLAG_TRCPT(1)
+#define Is_uhd_out_ready(p)                      ((USB->HOST.HostPipe[p].PINTFLAG.reg&USB_HOST_PINTFLAG_TRCPT(1)) == USB_HOST_PINTFLAG_TRCPT(1))
+#define uhd_ack_nak_received(p)                  usb_pipe_table[p].HostDescBank[1].STATUS_BK.reg &= ~USB_HOST_STATUS_BK_ERRORFLOW
+#define Is_uhd_nak_received(p)                   (usb_pipe_table[p].HostDescBank[1].STATUS_BK.reg & USB_HOST_STATUS_BK_ERRORFLOW)
 
+// Endpoint Interrupt Summary
+#define uhd_endpoint_interrupt()            USB->HOST.PINTSMRY.reg
 
-//! @name Pipes management
-//! @{
+// Run in Standby
+#define uhd_run_in_standby()                USB->HOST.CTRLA.reg |= USB_CTRLA_RUNSTDBY
+// Force host mode
+#define uhd_force_host_mode()               USB->HOST.CTRLA.reg |= USB_CTRLA_MODE
 
-//! USB address of pipes
-//! @{
-#define uhd_configure_address(p, addr) \
-		(Wr_bitfield((&UOTGHS->UOTGHS_HSTADDR1)[(p)>>2], \
-		UOTGHS_HSTADDR1_HSTADDRP0_Msk << (((p)&0x03)<<3), addr))
-#define uhd_get_configured_address(p) \
-		(Rd_bitfield((&UOTGHS->UOTGHS_HSTADDR1)[(p)>>2], \
-		UOTGHS_HSTADDR1_HSTADDRP0_Msk << (((p)&0x03)<<3)))
-//! @}
+// Enable USB macro
+#define uhd_enable()                        USB->HOST.CTRLA.reg |= USB_CTRLA_ENABLE
+// Disable USB macro
+#define uhd_disable()                       USB->HOST.CTRLA.reg &= ~USB_CTRLA_ENABLE
 
-//! Pipe enable
-//! Enable, disable, reset, freeze
-//! @{
-#define uhd_enable_pipe(p) \
-		(Set_bits(UOTGHS->UOTGHS_HSTPIP, UOTGHS_HSTPIP_PEN0 << (p)))
-#define uhd_disable_pipe(p) \
-		(Clr_bits(UOTGHS->UOTGHS_HSTPIP, UOTGHS_HSTPIP_PEN0 << (p)))
-#define Is_uhd_pipe_enabled(p) \
-		(Tst_bits(UOTGHS->UOTGHS_HSTPIP, UOTGHS_HSTPIP_PEN0 << (p)))
-#define uhd_reset_pipe(p) \
-		(Set_bits(UOTGHS->UOTGHS_HSTPIP, UOTGHS_HSTPIP_PEN0 << (p))); \
-		(Clr_bits(UOTGHS->UOTGHS_HSTPIP, UOTGHS_HSTPIP_PEN0 << (p)))
-#define Is_uhd_resetting_pipe(p) \
-		(Tst_bits(UOTGHS->UOTGHS_HSTPIP, UOTGHS_HSTPIP_PEN0 << (p)))
-#define uhd_freeze_pipe(p)                       (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_PFREEZES)
-#define uhd_unfreeze_pipe(p)                     (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_PFREEZEC)
-#define Is_uhd_pipe_frozen(p)                    (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_PFREEZE))
-#define uhd_reset_data_toggle(p)                 (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_RSTDTS)
-#define Is_uhd_data_toggle_reset(p)              (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_RSTDT))
-//! @}
-
-//! Pipe configuration
-//! @{
-#define uhd_configure_pipe_int_req_freq(p,freq)  (Wr_bitfield(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_INTFRQ_Msk, (freq)))
-#define uhd_get_pipe_int_req_freq(p)             (Rd_bitfield(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_INTFRQ_Msk))
-#define uhd_configure_pipe_endpoint_number(p,ep) (Wr_bitfield(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_PEPNUM_Msk, (ep)))
-#define uhd_get_pipe_endpoint_address(p) \
-		(uhd_is_pipe_in(p) ?\
-			(uhd_get_pipe_endpoint_number(p) | USB_EP_DIR_IN) :\
-			(uhd_get_pipe_endpoint_number(p) | USB_EP_DIR_OUT))
-#define uhd_get_pipe_endpoint_number(p)          (Rd_bitfield(UOTGHS->UOTGHS_HSTPIPCFG[p], (UOTGHS_HSTPIPCFG_PEPNUM_Msk)))
-#define uhd_configure_pipe_type(p, type)         (Wr_bitfield(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_PTYPE_Msk, type))
-#define uhd_get_pipe_type(p)                     (Rd_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_PTYPE_Msk))
-#define uhd_enable_pipe_bank_autoswitch(p)       (Set_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_AUTOSW))
-#define uhd_disable_pipe_bank_autoswitch(p)      (Clr_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_AUTOSW))
-#define Is_uhd_pipe_bank_autoswitch_enabled(p)   (Tst_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_AUTOSW))
-#define uhd_configure_pipe_token(p, token)       (Wr_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_PTOKEN_Msk, token))
-#define uhd_get_pipe_token(p)                    (Rd_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_PTOKEN_Msk))
-#define uhd_is_pipe_in(p)                        (UOTGHS_HSTPIPCFG_PTOKEN_IN==uhd_get_pipe_token(p))
-#define uhd_is_pipe_out(p)                       (UOTGHS_HSTPIPCFG_PTOKEN_OUT==uhd_get_pipe_token(p))
-//! Bounds given integer size to allowed range and rounds it up to the nearest
-//! available greater size, then applies register format of UOTGHS controller
-//! for pipe size bit-field.
-#define uhd_format_pipe_size(size) \
-		(32 - clz(((uint32_t)min(max(size, 8), 1024) << 1) - 1) - 1 - 3)
-#define uhd_configure_pipe_size(p,size) \
-		(Wr_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_PSIZE_Msk, uhd_format_pipe_size(size)))
-#define uhd_get_pipe_size(p)                     (8<<((Rd_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], (UOTGHS_HSTPIPCFG_PSIZE_Msk)))>> UOTGHS_HSTPIPCFG_PSIZE_Pos))
-#define uhd_configure_pipe_bank(p,bank)          (Wr_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_PBK_Msk, (bank)))
-#define uhd_get_pipe_bank(p)                     (Rd_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_PBK_Msk))
-#define uhd_allocate_memory(p)                   (Set_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_ALLOC))
-#define uhd_unallocate_memory(p)                 (Clr_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_ALLOC))
-#define Is_uhd_memory_allocated(p)               (Tst_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_ALLOC))
-
-//! Enable PING management only available in HS mode
-#  define uhd_enable_ping(p)                     (Set_bits(UOTGHS->UOTGHS_HSTPIPCFG[p], UOTGHS_HSTPIPCFG_PINGEN))
-//#endif
-#define uhd_configure_pipe(p, freq, ep_num, type, token, size, bank, bank_switch) \
-	(UOTGHS->UOTGHS_HSTPIPCFG[p] = \
-		(bank)|\
-		((uhd_format_pipe_size(size)<<UOTGHS_HSTPIPCFG_PSIZE_Pos)&UOTGHS_HSTPIPCFG_PSIZE_Msk)|\
-		((token)&UOTGHS_HSTPIPCFG_PTOKEN_Msk)|\
-		((type)&UOTGHS_HSTPIPCFG_PTYPE_Msk)|\
-		(((ep_num)<<UOTGHS_HSTPIPCFG_PEPNUM_Pos)&UOTGHS_HSTPIPCFG_PEPNUM_Msk)|\
-		bank_switch |\
-		(((freq)<<UOTGHS_HSTPIPCFG_INTFRQ_Pos)&UOTGHS_HSTPIPCFG_INTFRQ_Msk))
-
-#define Is_uhd_pipe_configured(p)                (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_CFGOK))
-//! @}
-
-//! Pipe main interrupts management
-//! @{
-#define uhd_enable_pipe_interrupt(p) 		(UOTGHS->UOTGHS_HSTIER = (UOTGHS_HSTIER_PEP_0 << (p)))
-#define uhd_disable_pipe_interrupt(p) 		(UOTGHS->UOTGHS_HSTIDR = (UOTGHS_HSTIDR_PEP_0 << (p)))
-#define Is_uhd_pipe_interrupt_enabled(p) 	(Tst_bits(UOTGHS->UOTGHS_HSTIMR, UOTGHS_HSTIMR_PEP_0 << (p)))
-#define Is_uhd_pipe_interrupt(p) 		(Tst_bits(UOTGHS->UOTGHS_HSTISR, UOTGHS_HSTISR_PEP_0 << (p)))
-//! returns the lowest pipe number generating a pipe interrupt or UOTGHS_EPT_NUM if none
-#define uhd_get_interrupt_pipe_number() \
-	(ctz(((UOTGHS->UOTGHS_HSTISR >> 8) & (UOTGHS->UOTGHS_HSTIMR >> 8)) | (1 << UOTGHS_EPT_NUM)))
-//! @}
-
-//! Pipe overflow and underflow for isochronous and interrupt endpoints
-//! @{
-#define uhd_enable_overflow_interrupt(p)         (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_OVERFIES)
-#define uhd_disable_overflow_interrupt(p)        (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_OVERFIEC)
-#define Is_uhd_overflow_interrupt_enabled(p)     (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_OVERFIE))
-#define uhd_ack_overflow_interrupt(p)            (UOTGHS->UOTGHS_HSTPIPICR[p] = UOTGHS_HSTPIPICR_OVERFIC)
-#define Is_uhd_overflow(p)                       (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_OVERFI))
-
-#define uhd_enable_underflow_interrupt(p)        (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_UNDERFIES)
-#define uhd_disable_underflow_interrupt(p)       (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_UNDERFIEC)
-#define Is_uhd_underflow_interrupt_enabled(p)    (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_UNDERFIE))
-#define uhd_ack_underflow_interrupt(p)           (UOTGHS->UOTGHS_HSTPIPICR[p] = UOTGHS_HSTPIPICR_UNDERFIC)
-#define Is_uhd_underflow(p)                      (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_UNDERFI))
-//! @}
-
-//! USB packet errors management
-//! @{
-#define uhd_enable_stall_interrupt(p)            (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_RXSTALLDES)
-#define uhd_disable_stall_interrupt(p)           (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_RXSTALLDEC)
-#define Is_uhd_stall_interrupt_enabled(p)        (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_RXSTALLDE))
-#define uhd_ack_stall(p)                         (UOTGHS->UOTGHS_HSTPIPICR[p] = UOTGHS_HSTPIPICR_RXSTALLDIC)
-#define Is_uhd_stall(p)                          (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_RXSTALLDI))
-
-#define uhd_enable_pipe_error_interrupt(p)       (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_PERRES)
-#define uhd_disable_pipe_error_interrupt(p)      (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_PERREC)
-#define Is_uhd_pipe_error_interrupt_enabled(p)   (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_PERRE))
-#define uhd_ack_all_errors(p)                    (UOTGHS->UOTGHS_HSTPIPERR[p] = 0UL)
-#define Is_uhd_pipe_error(p)                     (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_PERRI))
-#define uhd_error_status(p)                      (UOTGHS->UOTGHS_HSTPIPERR[p])
-#define Is_uhd_bad_data_toggle(p)                (Tst_bits(UOTGHS->UOTGHS_HSTPIPERR[p], UOTGHS_HSTPIPERR_DATATGL))
-#define Is_uhd_data_pid_error(p)                 (Tst_bits(UOTGHS->UOTGHS_HSTPIPERR[p], UOTGHS_HSTPIPERR_DATAPID))
-#define Is_uhd_pid_error(p)                      (Tst_bits(UOTGHS->UOTGHS_HSTPIPERR[p], UOTGHS_HSTPIPERR_PID))
-#define Is_uhd_timeout_error(p)                  (Tst_bits(UOTGHS->UOTGHS_HSTPIPERR[p], UOTGHS_HSTPIPERR_TIMEOUT))
-#define Is_uhd_crc16_error(p)                    (Tst_bits(UOTGHS->UOTGHS_HSTPIPERR[p], UOTGHS_HSTPIPERR_CRC16))
-#define uhd_get_error_counter(p)                 (Rd_bits(UOTGHS->UOTGHS_HSTPIPERR[p], UOTGHS_HSTPIPERR_COUNTER))
-//! @}
-
-//! Pipe data management
-//! @{
-#define uhd_data_toggle(p)                       (Rd_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_DTSEQ))
-
-#define uhd_enable_bank_interrupt(p)             (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_NBUSYBKES)
-#define uhd_disable_bank_interrupt(p)            (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_NBUSYBKEC)
-#define Is_uhd_bank_interrupt_enabled(p)         (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_NBUSYBKE))
-#define uhd_nb_busy_bank(p)                      (Rd_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_NBUSYBK_Msk))
-#define uhd_current_bank(p)                      (Rd_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_CURRBK_Msk ))
-
-#define uhd_enable_short_packet_interrupt(p)     (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_SHORTPACKETES)
-#define uhd_disable_short_packet_interrupt(p)    (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_SHORTPACKETIEC)
-#define Is_uhd_short_packet_interrupt_enabled(p) (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_SHORTPACKETIE))  )
-#define uhd_ack_short_packet(p)                  (UOTGHS->UOTGHS_HSTPIPICR[p] = UOTGHS_HSTPIPICR_SHORTPACKETIC)
-#define Is_uhd_short_packet(p)                   (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_SHORTPACKETI))
-#define uhd_byte_count(p)                        (Rd_bitfield(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_PBYCT_Msk))
-
-#define Is_uhd_fifocon(p)                        (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_FIFOCON))
-#define uhd_ack_fifocon(p)                       (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_FIFOCONC)
-
-#define uhd_enable_setup_ready_interrupt(p)      (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_TXSTPES)
-#define uhd_disable_setup_ready_interrupt(p)     (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_TXSTPEC)
-#define Is_uhd_setup_ready_interrupt_enabled(p)  (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_TXSTPE))
-#define uhd_ack_setup_ready(p)                   (UOTGHS->UOTGHS_HSTPIPICR[p] = UOTGHS_HSTPIPICR_TXSTPIC)
-#define Is_uhd_setup_ready(p)                    (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_TXSTPI))
-
-#define uhd_enable_in_received_interrupt(p)      (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_RXINES)
-#define uhd_disable_in_received_interrupt(p)     (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_RXINEC)
-#define Is_uhd_in_received_interrupt_enabled(p)  (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_RXINE))
-#define uhd_ack_in_received(p)                   (UOTGHS->UOTGHS_HSTPIPICR[p] = UOTGHS_HSTPIPICR_RXINIC)
-#define Is_uhd_in_received(p)                    (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_RXINI))
-
-#define uhd_enable_out_ready_interrupt(p)        (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_TXOUTES)
-#define uhd_disable_out_ready_interrupt(p)       (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_TXOUTEC)
-#define Is_uhd_out_ready_interrupt_enabled(p)    (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_TXOUTE))
-#define uhd_ack_out_ready(p)                     (UOTGHS->UOTGHS_HSTPIPICR[p] = UOTGHS_HSTPIPICR_TXOUTIC)
-#define Is_uhd_out_ready(p)                      (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_TXOUTI))
-#define uhd_raise_out_ready(p)                   (UOTGHS->UOTGHS_HSTPIPIFR[p] = UOTGHS_HSTPIPIFR_TXOUTIS)
-
-#define uhd_enable_nak_received_interrupt(p)     (UOTGHS->UOTGHS_HSTPIPIER[p] = UOTGHS_HSTPIPIER_NAKEDES)
-#define uhd_disable_nak_received_interrupt(p)    (UOTGHS->UOTGHS_HSTPIPIDR[p] = UOTGHS_HSTPIPIDR_NAKEDEC)
-#define Is_uhd_nak_received_interrupt_enabled(p) (Tst_bits(UOTGHS->UOTGHS_HSTPIPIMR[p], UOTGHS_HSTPIPIMR_NAKEDE))
-#define uhd_ack_nak_received(p)                  (UOTGHS->UOTGHS_HSTPIPICR[p] = UOTGHS_HSTPIPICR_NAKEDIC)
-#define Is_uhd_nak_received(p)                   (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_NAKEDI))
-
-#define Is_uhd_read_enabled(p)                   (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_RWALL))
-#define Is_uhd_write_enabled(p)                  (Tst_bits(UOTGHS->UOTGHS_HSTPIPISR[p], UOTGHS_HSTPIPISR_RWALL ))
-
-#define uhd_enable_continuous_in_mode(p)         (Set_bits(UOTGHS->UOTGHS_HSTPIPINRQ[p], UOTGHS_HSTPIPINRQ_INMODE))
-#define uhd_disable_continuous_in_mode(p)        (Clr_bits(UOTGHS->UOTGHS_HSTPIPINRQ[p], UOTGHS_HSTPIPINRQ_INMODE))
-#define Is_uhd_continuous_in_mode_enabled(p)     (Tst_bits(UOTGHS->UOTGHS_HSTPIPINRQ[p], UOTGHS_HSTPIPINRQ_INMODE))
-
-#define uhd_in_request_number(p, in_num)         (Set_bits(UOTGHS->UOTGHS_HSTPIPINRQ[p], (in_num)-1))
-#define uhd_get_in_request_number(p)             (((Rd_bits(UOTGHS->UOTGHS_HSTPIPINRQ[p], UOTGHS_HSTPIPINRQ_INRQ_Msk))>>UOTGHS_HSTPIPINRQ_INRQ_Pos)+1)
-//! @}
-
-//! Maximum transfer size on USB DMA
-#define UHD_PIPE_MAX_TRANS 0x8000
-
-//! Get 64-, 32-, 16- or 8-bit access to FIFO data register of selected pipe.
-//! @param p      Target Pipe number
-//! @param scale  Data scale in bits: 64, 32, 16 or 8
-//! @return       Volatile 64-, 32-, 16- or 8-bit data pointer to FIFO data register
-//! @warning It is up to the user of this macro to make sure that all accesses
-//! are aligned with their natural boundaries except 64-bit accesses which
-//! require only 32-bit alignment.
-//! @warning It is up to the user of this macro to make sure that used HSB
-//! addresses are identical to the DPRAM internal pointer modulo 32 bits.
-#define uhd_get_pipe_fifo_access(p, scale) \
-	(((volatile TPASTE2(U, scale) (*)[UHD_PIPE_MAX_TRANS / ((scale) / 8)])UOTGHS_RAM_ADDR)[(p)])
-
+// Force full speed mode
+#define uhd_force_full_speed()              USB->HOST.CTRLB.reg &= ~USB_HOST_CTRLB_SPDCONF_Msk
 
 #ifdef __cplusplus
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/avr/dtostrf.c
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/avr/dtostrf.c	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/avr/dtostrf.c	(revision 224)
@@ -20,4 +20,5 @@
 
 char *dtostrf (double val, signed char width, unsigned char prec, char *sout) {
+  asm(".global _printf_float");
   char fmt[20];
   sprintf(fmt, "%%%d.%df", width, prec);
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/startup.c
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/startup.c	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/startup.c	(revision 224)
@@ -46,7 +46,7 @@
 void HardFault_Handler( void )
 {
-  while ( 1 )
-  {
-  }
+	 while ( 1 )
+  {
+	 }
 }
 
@@ -82,5 +82,5 @@
 void NVMCTRL_Handler         ( void ) __attribute__ ((weak, alias("Dummy_Handler")));
 void DMAC_Handler            ( void ) __attribute__ ((weak, alias("Dummy_Handler")));
-void USB_Handler             ( void ) __attribute__ ((weak, alias("Dummy_Handler")));
+void USB_Handler             ( void ) __attribute__ ((weak));
 void EVSYS_Handler           ( void ) __attribute__ ((weak, alias("Dummy_Handler")));
 void SERCOM0_Handler         ( void ) __attribute__ ((weak, alias("Dummy_Handler")));
@@ -406,4 +406,18 @@
   while ( 1 )
   {
-  }
-}
+ }
+}
+
+static void (*usb_isr)(void) = NULL;
+
+void USB_Handler(void)
+{
+  if (usb_isr)
+    usb_isr();
+}
+
+void USB_SetHandler(void (*new_usb_isr)(void))
+{
+  usb_isr = new_usb_isr;
+}
+
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/wiring_analog.c
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/wiring_analog.c	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/wiring_analog.c	(revision 224)
@@ -31,16 +31,9 @@
 static int DAC_RESOLUTION = 10;
 
-// Wait for synchronization of registers between the clock domains
-static __inline__ void syncDAC() __attribute__((always_inline, unused));
-static void syncDAC() {
-  while (DAC->STATUS.bit.SYNCBUSY == 1)
-    ;
-}
-
 void analogReadResolution(int res) {
 	_readResolution = res;
 	while( ADC->STATUS.bit.SYNCBUSY == 1 )
   {
-    // Waiting for synchroinization
+    // Waiting for synchronization
   }
 	if(res == 8) ADC->CTRLB.bit.RESSEL= ADC_CTRLB_RESSEL_8BIT_Val; 
@@ -69,5 +62,5 @@
   while( ADC->STATUS.bit.SYNCBUSY == 1 )
   {
-    // Waiting for synchroinization
+    // Waiting for synchronization
   }
   switch(ulMode)
@@ -91,5 +84,5 @@
   while( ADC->STATUS.bit.SYNCBUSY == 1 )
   {
-    // Waiting for synchroinization
+    // Waiting for synchronization
   }
 }
@@ -154,5 +147,5 @@
     	DAC->DATA.reg = ulValue & 0x3FF;  // Dac on 10 bits.
 		DAC->CTRLA.bit.ENABLE = 1; // DAC Enabled
-		syncDAC();
+		while (DAC->STATUS.bit.SYNCBUSY == 1); //wait for synchronization
       	return;
     }
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/wiring_digital.c
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/wiring_digital.c	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/cores/arduino/wiring_digital.c	(revision 224)
@@ -178,4 +178,5 @@
 
     default:
+	  PORT->Group[g_APinDescription[ulPin].ulPort].OUTSET.reg = (1ul << g_APinDescription[ulPin].ulPin) ;
     break ;
   }
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/TianMIPSAndMCUOnOff/TianMIPSAndMCUOnOff.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/TianMIPSAndMCUOnOff/TianMIPSAndMCUOnOff.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/TianMIPSAndMCUOnOff/TianMIPSAndMCUOnOff.ino	(revision 224)
@@ -0,0 +1,50 @@
+/*
+
+This sketch allows the users to manage the Tian and MCU power management.
+Users can power up or power down MIPS and MCU with an external interrupt.
+
+*/
+
+#include <EnergySaving.h>
+
+EnergySaving nrgSave;
+
+#define MIPS_PIN 32  //PA28 PIN 32
+
+void setup()
+{
+  pinMode(MIPS_PIN,OUTPUT);
+  digitalWrite(MIPS_PIN,HIGH);
+
+  nrgSave.begin(WAKE_EXT_INTERRUPT, 8, wakeUp);  //standby setup for external interrupts
+}
+
+void loop()
+{
+  doSomething();
+  sleep();
+}
+
+void doSomething(){
+  //blink for 90 seconds
+  for(int i=0; i<90; i++)
+  {
+    digitalWrite(13,HIGH);
+    delay(500);
+    digitalWrite(13,LOW);
+    delay(500);
+  }
+}
+
+void sleep(void){
+  digitalWrite(MIPS_PIN, LOW);
+  nrgSave.standby();  //now mcu goes in standby mode
+}
+
+void wakeUp(void)  //interrupt routine (isn't necessary to execute any tasks in this routine
+{
+  //mcu is waked-up by the interrupt
+
+  //wake up mips
+  digitalWrite(MIPS_PIN,HIGH);
+}
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/TianMIPSOnOff/TianMIPSOnOff.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/TianMIPSOnOff/TianMIPSOnOff.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/TianMIPSOnOff/TianMIPSOnOff.ino	(revision 224)
@@ -0,0 +1,63 @@
+/*
+
+This sketch allows the users to manage the Tian power management.
+Users can power up or power down the MIPS side from the MCU serial console.
+
+After upload the sketch, open the Serial Terminal and press the send button.
+
+COMMANDS
+----------------------
+h - help
+D - power down MIPS
+U - power up MIPS
+
+created March 2016
+  by andrea@arduino.org
+
+ */
+
+#define MIPS_PIN 32  //PA28 PIN 32
+
+char var;
+
+void setup() {
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+
+  pinMode(MIPS_PIN,OUTPUT);    //MIPS
+  digitalWrite(MIPS_PIN,HIGH);
+  while(!Serial.available());;
+  menu('h');
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  while (Serial.available()){
+    var = Serial.read();
+    menu(var);
+  }
+}
+
+void menu( char var){
+     switch (var) {
+      case 'D':
+        Serial.println("MIPS OFF");
+        Serial.println();
+        digitalWrite(MIPS_PIN,LOW);
+        break;
+      case 'U':
+        Serial.println("MIPS ON");
+        Serial.println();
+        digitalWrite(MIPS_PIN,HIGH);
+        break;
+      case 'h':
+        Serial.println("COMMANDS");
+        Serial.println("----------------------");
+        Serial.println("h - help");
+        Serial.println("D - power down MIPS");
+        Serial.println("U - power up MIPS");
+        Serial.println();
+        break;
+    }
+
+}
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/WakeUpOnExternalInterrupt/WakeUpOnExternalInterrupt.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/WakeUpOnExternalInterrupt/WakeUpOnExternalInterrupt.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/WakeUpOnExternalInterrupt/WakeUpOnExternalInterrupt.ino	(revision 224)
@@ -0,0 +1,36 @@
+/********************************************************************************************
+ This sketch demonstrates how sleep function using external interrupt to wake up the MCU.
+ This sketch work only cortex-m0+ boards.
+*********************************************************************************************/
+
+
+
+#include <EnergySaving.h>
+
+EnergySaving nrgSave;
+
+unsigned int i=0;
+
+void setup()
+{
+  nrgSave.begin(WAKE_EXT_INTERRUPT, 8, dummy);  //standby setup for external interrupts
+}
+
+void loop()
+{
+  for(i=0; i<20; i++)
+  {
+    digitalWrite(13,HIGH);
+    delay(500);
+    digitalWrite(13,LOW);
+    delay(500);
+  }
+
+  nrgSave.standby();  //now mcu goes in standby mode
+}
+
+
+void dummy(void)  //interrupt routine (isn't necessary to execute any tasks in this routine
+{
+
+}
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/WakeUpOnRTCInterrupt/WakeUpOnRTCInterrupt.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/WakeUpOnRTCInterrupt/WakeUpOnRTCInterrupt.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/examples/WakeUpOnRTCInterrupt/WakeUpOnRTCInterrupt.ino	(revision 224)
@@ -0,0 +1,53 @@
+/***********************************************************************************************
+ This sketch demonstrates how to use sleep features and wake-up MCU with RTC alarm interrupt.
+ This sketch works only on cortex-m0+ based boards.
+************************************************************************************************/
+
+#include <EnergySaving.h>
+#include <RTCInt.h>
+
+EnergySaving nrgSave;
+RTCInt rtc;
+
+unsigned int i=0;
+
+void setup()
+{
+
+  pinMode(30,OUTPUT);  //led RX
+  Serial5.begin(9600);   //serial settings
+  rtc.begin(TIME_H24);
+  rtc.setTime(12,48,0,0);
+  rtc.setDate(22,10,15);
+  rtc.enableAlarm(SEC,ALARM_INTERRUPT,rest_alarm_int);
+  rtc.local_time.hour=12;
+  rtc.local_time.minute=48;
+  rtc.local_time.second=30;
+  nrgSave.begin(WAKE_RTC_ALARM);  //standby setup for external interrupts
+  rtc.setAlarm();
+  SerialUSB.println("START");
+
+}
+
+void loop()
+{
+  for(i=0; i<20; i++)
+  {
+    digitalWrite(13,HIGH);
+    delay(500);
+    digitalWrite(13,LOW);
+    delay(500);
+  }
+
+  digitalWrite(30,LOW);
+  nrgSave.standby();  //now mcu go to standby
+
+}
+
+void rest_alarm_int(void)  //interrupt routine
+{
+  //PORT->Group[1].OUTSET.reg = 0x1u<<3;
+  digitalWrite(30,HIGH);
+
+  Serial5.println("HELLO");
+}
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/keywords.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/keywords.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/keywords.txt	(revision 224)
@@ -0,0 +1,24 @@
+#######################################
+# Syntax Coloring Map For Energy Saving
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+EnergySaving	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+begin	KEYWORD2
+standby	KEYWORD2
+
+
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+WAKE_EXT_INTERRUPT	LITERAL1
+WAKE_RTC_ALARM	LITERAL1
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/library.properties
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/library.properties	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/library.properties	(revision 224)
@@ -0,0 +1,9 @@
+name=Energy Saving
+version=1.0.0
+author=Arduino Srl
+maintainer=Arduino Srl <swdev@arduino.org>
+sentence=Energy Saving features for corte-m0+ Arduino Boards, using sleep features.
+paragraph=With this library you can manage the power of Arduino boards based on cortex-m0+ (like Tian and M0)
+category=Device Control
+url=http://labs.arduino.org/EnergySaving
+architectures=samd
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/src/EnergySaving.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/src/EnergySaving.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/src/EnergySaving.cpp	(revision 224)
@@ -0,0 +1,111 @@
+#include "EnergySaving.h"
+
+void EnergySaving::begin(unsigned int mode, unsigned int inter_pin, voidFuncPtr callback)
+{
+	if((mode == WAKE_EXT_INTERRUPT) && (inter_pin !=2) &&  (inter_pin!=0) && (inter_pin!=1))
+	{
+		 NVMCTRL->CTRLB.bit.SLEEPPRM = 3;
+
+		SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
+		pinMode(inter_pin,INPUT_PULLUP);
+
+		attachInterrupt(inter_pin,callback, CHANGE);
+		enable_eic_wake(inter_pin);
+		set_clk();
+
+	}
+	else return;
+}
+
+
+void EnergySaving::begin(unsigned int mode)
+{
+	if(mode == WAKE_RTC_ALARM)
+	{
+		//RTCInt.begin(TIME_H24);
+
+		GCLK->CLKCTRL.bit.CLKEN = 0; //disable GCLK module
+		while (GCLK->STATUS.bit.SYNCBUSY);
+
+		GCLK->GENCTRL.bit.RUNSTDBY = 1;  //GCLK6 run standby
+		while (GCLK->STATUS.reg & GCLK_STATUS_SYNCBUSY);
+
+		GCLK->CLKCTRL.bit.CLKEN = 1; //disable GCLK module
+		while (GCLK->STATUS.bit.SYNCBUSY);
+
+		NVMCTRL->CTRLB.bit.SLEEPPRM = 3;
+
+		SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
+
+	}
+	else return;
+
+}
+
+
+void EnergySaving::standby(void)
+{
+	__DSB();
+    __WFI();
+
+}
+
+
+void EnergySaving::set_clk(void)
+{
+	GCLK->CLKCTRL.bit.CLKEN = 0; //disable GCLK module
+		while (GCLK->STATUS.bit.SYNCBUSY);
+
+		GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK6 | GCLK_CLKCTRL_ID( GCM_EIC )) ;  //EIC clock switched on GCLK6
+		while (GCLK->STATUS.bit.SYNCBUSY);
+
+		GCLK->GENCTRL.reg = (GCLK_GENCTRL_GENEN | GCLK_GENCTRL_SRC_OSCULP32K | GCLK_GENCTRL_ID(6));  //source for GCLK6 is OSCULP32K
+		while (GCLK->STATUS.reg & GCLK_STATUS_SYNCBUSY);
+
+		GCLK->GENCTRL.bit.RUNSTDBY = 1;  //GCLK6 run standby
+		while (GCLK->STATUS.reg & GCLK_STATUS_SYNCBUSY);
+
+}
+
+
+void EnergySaving::enable_eic_wake(unsigned int inter_pin)
+{
+	switch(inter_pin)
+		{
+			case 3:
+				EIC->WAKEUP.bit.WAKEUPEN9 = 1;
+				break;
+			case 4:
+				EIC->WAKEUP.bit.WAKEUPEN14 = 1;
+				break;
+			case 5:
+				EIC->WAKEUP.bit.WAKEUPEN15 = 1;
+				break;
+			case 6:
+				//EIC->WAKEUP.bit.WAKEUPEN20 = 1;  //non si pu嘆 usare
+				break;
+			case 7:
+				//EIC->WAKEUP.bit.WAKEUPEN21 = 1;  //non si pu嘆 usare
+				break;
+			case 8:
+				EIC->WAKEUP.bit.WAKEUPEN6 = 1;
+				break;
+			case 9:
+				EIC->WAKEUP.bit.WAKEUPEN7 = 1;
+				break;
+			case 10:
+				//EIC->WAKEUP.bit.WAKEUPEN18 = 1;  //non si pu嘆 usare
+				break;
+			case 11:
+				//EIC->WAKEUP.bit.WAKEUPEN16 = 1;  //non c'竪
+				break;
+			case 12:
+				//EIC->WAKEUP.bit.WAKEUPEN19 = 1;  //non si pu嘆 usare
+				break;
+			case 13:
+				//EIC->WAKEUP.bit.WAKEUPEN17 = 1;  //non c'竪
+				break;
+			default:
+				break;
+		}
+}
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/src/EnergySaving.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/src/EnergySaving.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/EnergySaving/src/EnergySaving.h	(revision 224)
@@ -0,0 +1,26 @@
+#ifndef energy_saving_h
+#define energy_saving_h
+
+
+#include <sam.h>
+#include <Arduino.h>
+
+#define WAKE_EXT_INTERRUPT 1
+#define WAKE_RTC_ALARM 2
+
+typedef void (*voidFuncPtr)( void ) ;
+
+class EnergySaving {
+	public:
+		//sleep();
+		void begin(unsigned int mode, unsigned int inter_pin, voidFuncPtr callback);
+		void begin(unsigned int mode);
+		void standby(void);
+
+	private:
+		void set_clk(void);
+		void enable_eic_wake(unsigned int inter_pin);
+
+};
+
+#endif
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/SPI/SPI.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/SPI/SPI.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/SPI/SPI.cpp	(revision 224)
@@ -13,4 +13,5 @@
 #include "assert.h"
 #include "variant.h"
+#include <Arduino.h>
 
 SPIClass::SPIClass(SERCOM *p_sercom, uint8_t uc_pinMISO, uint8_t uc_pinSCK, uint8_t uc_pinMOSI)
@@ -38,8 +39,61 @@
 }
 
+
+void SPIClass::beginSlave()
+{
+  // PIO init
+  pinPeripheral(_uc_pinMiso, g_APinDescription[_uc_pinMiso].ulPinType);
+  pinPeripheral(_uc_pinSCK, g_APinDescription[_uc_pinSCK].ulPinType);
+  pinPeripheral(_uc_pinMosi, g_APinDescription[_uc_pinMosi].ulPinType);
+
+	// Default speed set to 4Mhz, SPI mode set to MODE 0 and Bit order set to MSB first.
+	_p_sercom->initSPIslave(SPI_PAD_2_SCK_3, SERCOM_RX_PAD_0, SPI_CHAR_SIZE_8_BITS, MSB_FIRST);
+	
+	_p_sercom->enableSPI();
+}
+
 void SPIClass::end()
 {
 	_p_sercom->resetSPI();
 }
+
+
+void SPIClass::beginTransaction(SPISettings settings)
+	{
+		SercomDataOrder bitOrder;
+		SercomSpiClockMode SpiClockMode;
+		
+		interruptSave = intStatus();
+		noInterrupts();
+		
+	
+		if(settings.bit_order==LSBFIRST) bitOrder = LSB_FIRST;
+		else if(settings.bit_order==MSBFIRST) bitOrder = MSB_FIRST;
+		else;
+		
+		if(settings.data_mode==SPI_MODE0) SpiClockMode = SERCOM_SPI_MODE_0;
+		else if(settings.data_mode==SPI_MODE1) SpiClockMode = SERCOM_SPI_MODE_1;
+		else if(settings.data_mode==SPI_MODE2) SpiClockMode = SERCOM_SPI_MODE_2;
+		else if(settings.data_mode==SPI_MODE3) SpiClockMode = SERCOM_SPI_MODE_3;
+		else;
+		
+		end();
+		
+		pinPeripheral(_uc_pinMiso, g_APinDescription[_uc_pinMiso].ulPinType);
+		pinPeripheral(_uc_pinSCK, g_APinDescription[_uc_pinSCK].ulPinType);
+		pinPeripheral(_uc_pinMosi, g_APinDescription[_uc_pinMosi].ulPinType);
+		
+		_p_sercom->initSPI(SPI_PAD_2_SCK_3, SERCOM_RX_PAD_0, SPI_CHAR_SIZE_8_BITS, bitOrder);
+		_p_sercom->initSPIClock(SpiClockMode, settings.interface_clock);
+	
+	_p_sercom->enableSPI();
+	inTransactionFlag=1;
+	}
+	
+void SPIClass::endTransaction(void)
+{
+	inTransactionFlag=0;
+	interrupts();
+}	
 
 void SPIClass::setBitOrder(BitOrder order)
@@ -76,5 +130,5 @@
 }
 
-void SPIClass::setClockDivider(uint8_t div)
+void SPIClass::setClockDivider(uint16_t div)
 {
 	_p_sercom->setBaudrateSPI(div);
@@ -90,4 +144,104 @@
 }
 
+byte SPIClass::read()
+{
+	return _p_sercom->readDataSPI();
+}
+
+void SPIClass::usingInterrupt(uint8_t intNum)
+{
+	uint8_t irestore;
+	uint32_t mask;
+	
+	irestore=intStatus();
+	noInterrupts();
+	if(intNum > 13)
+	{
+		//Interrupts();
+		return;
+	}
+	else
+	{
+		//Pio *pio=g_APinDescription[intNum].ulPort;
+		mask=g_APinDescription[intNum].ulPin;
+		interruptMode=1;
+		interruptMask=mask;
+	}
+	if (irestore) interrupts();		
+}
+
+void SPIClass::write(uint8_t data)
+{
+	//Writing the data
+	_p_sercom->writeDataSPI(data);
+}
+
+void SPIClass::transfer(void *data, size_t count)
+{
+	uint8_t *p;
+	//int i=0;
+		//Serial.println("dentro transfer");
+	if(count==0) return;
+	p=(uint8_t *)data;
+	_p_sercom->writeDataSPI(*p);  //scrittura primo dato
+	
+	while(--count > 0)
+	{
+		//Serial.println("dentro while");
+		uint8_t out=*(p+1); //copio in out il prossimo dato
+		uint8_t in = _p_sercom->readDataSPI();  //leggo dato da SPI
+		//Serial.println("UNO");
+		//while(!(SERCOM4->SPI.INTFLAG.bit.RXC));  // controllo trasferimento  //vedere se necessario
+		//Serial.println("DUE");
+		//Serial.println(out);
+		_p_sercom->writeDataSPI(out);   //scrivo il out su SPI
+		*p++=in; //metto in p il dato letto da spi
+	}
+	while(!(SERCOM4->SPI.INTFLAG.bit.TXC));  // controllo trasferimrnto
+	//Serial.print("*p: ");
+	//Serial.println(*p);
+	*p = _p_sercom->readDataSPI();
+	
+	
+		
+}
+
+uint16_t SPIClass::transfer16(uint16_t data)
+{
+	union{
+		uint16_t value;
+		struct{
+			uint8_t lsb;
+			uint8_t msb;
+		};
+	}in, out;
+	
+	in.value = data;
+	
+	if(SERCOM4->SPI.CTRLA.bit.DORD==0)
+	{
+		_p_sercom->writeDataSPI(in.msb);
+		while(!(SERCOM4->SPI.INTFLAG.bit.TXC));
+		out.msb = _p_sercom->readDataSPI();
+		//while(!(SERCOM4->SPI.INTFLAG.bit.RXC)); 
+		_p_sercom->writeDataSPI(in.lsb);
+		while(!(SERCOM4->SPI.INTFLAG.bit.TXC));
+		out.lsb = _p_sercom->readDataSPI();
+	}
+
+	else
+	{
+		_p_sercom->writeDataSPI(in.lsb);
+		while(!(SERCOM4->SPI.INTFLAG.bit.TXC));
+		out.lsb = _p_sercom->readDataSPI();
+		//while(!(SERCOM4->SPI.INTFLAG.bit.RXC)); 
+		_p_sercom->writeDataSPI(in.msb);
+		while(!(SERCOM4->SPI.INTFLAG.bit.TXC));
+		out.msb = _p_sercom->readDataSPI();
+	}
+
+		return out.value;
+}
+
 void SPIClass::attachInterrupt() {
 	// Should be enableInterrupt()
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/SPI/SPI.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/SPI/SPI.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/libraries/SPI/SPI.h	(revision 224)
@@ -14,4 +14,8 @@
 #include "variant.h"
 #include "wiring_constants.h"
+#include <Arduino.h>
+ 
+//#define LSBFIRST 0 //already defined in wiring_constants.h
+//#define MSBFIRST 1 
 
 #define SPI_MODE0 0x02
@@ -20,4 +24,64 @@
 #define SPI_MODE3 0x01
 
+
+#define SPI_CLOCK_DIV2 6       //8 MHz
+#define SPI_CLOCK_DIV4 12      //4 MHz
+#define SPI_CLOCK_DIV8 24      //2 MHz
+#define SPI_CLOCK_DIV16 48     //1 MHz
+#define SPI_CLOCK_DIV32 96     //500 KHz
+#define SPI_CLOCK_DIV64 192    //250 KHz
+#define SPI_CLOCK_DIV128 384   //125 KHz
+
+#ifndef intStatus
+#define intStatus() __intStatus()
+static inline unsigned char __intStatus(void) __attribute__((always_inline, unused));
+static inline unsigned char __intStatus(void) {
+	unsigned int primask, faultmask;
+	asm volatile ("mrs %0, primask" : "=r" (primask));
+	if (primask) return 0;
+	asm volatile ("mrs %0, faultmask" : "=r" (faultmask));
+	if (faultmask) return 0;
+	return 1;
+}
+#endif
+
+class SPISettings {
+	public:
+		SPISettings(uint32_t clock, uint8_t bitOrder, uint8_t dataMode)
+		{
+			interface_clock = clock;
+			bit_order = bitOrder;
+			data_mode = dataMode;
+			
+			/*Serial.print("clock: ");
+			Serial.println(interface_clock);
+			Serial.print("bit order: ");
+			Serial.println(bit_order);
+			Serial.print("data mode: ");
+			Serial.println(data_mode);*/
+		}
+		
+		SPISettings(void)
+		{
+			interface_clock = 4000000;
+			bit_order = MSBFIRST;
+			data_mode = SPI_MODE0;
+			
+			/*Serial.print("clock: ");
+			Serial.println(interface_clock);
+			Serial.print("bit order: ");
+			Serial.println(bit_order);
+			Serial.print("data mode: ");
+			Serial.println(data_mode);*/
+		}
+	private:
+		uint32_t interface_clock;
+		uint8_t bit_order;
+		uint8_t data_mode;
+	
+	friend class SPIClass;	
+};
+
+
 class SPIClass {
   public:
@@ -25,15 +89,24 @@
 
 	byte transfer(uint8_t data);
-
+	void transfer(void *data, size_t count);
+	byte read(void);
+	uint16_t transfer16(uint16_t data);
+	void write(uint8_t data);
 	// SPI Configuration methods
+	void beginTransaction(SPISettings settings);
+	void endTransaction(void);
+	
 	void attachInterrupt();
 	void detachInterrupt();
+	
+	void usingInterrupt(uint8_t interruptNumber);
 
 	void begin();
+	void beginSlave();
 	void end();
 	
 	void setBitOrder(BitOrder order);
 	void setDataMode(uint8_t uc_mode);
-	void setClockDivider(uint8_t uc_div);
+	void setClockDivider(uint16_t uc_div);
 
   private:
@@ -42,4 +115,8 @@
 	uint8_t _uc_pinMosi;
 	uint8_t _uc_pinSCK;
+	uint8_t interruptMode;
+	uint32_t interruptMask;
+	uint8_t interruptSave;
+	uint8_t inTransactionFlag;
 };
 
Index: rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/programmers.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/programmers.txt	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/hardware/arduino/samd/programmers.txt	(revision 224)
@@ -1,2 +1,2 @@
-zeroedbg.name=M0 Pro Programming Port
+zeroedbg.name=Atmel EDBG Programming Port
 zeroedbg.program.tool=openocd
Index: rtos_arduino/trunk/arduino_lib/libraries/Ciao/examples/CiaoRestClient/CiaoRestClient.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Ciao/examples/CiaoRestClient/CiaoRestClient.ino	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Ciao/examples/CiaoRestClient/CiaoRestClient.ino	(revision 224)
@@ -3,13 +3,16 @@
 
 #define CONNECTOR     "rest" 
-#define SERVER_ADDR   "192.168.0.100" // change ip address with your server ip address
+#define SERVER_ADDR   "192.168.1.1" // change ip address with your server ip address
 
-int buttonState = LOW; //this variable tracks the state of the button, low if not pressed, high if pressed
-int ledState = -1; //this variable tracks the state of the LED, negative if off, positive if on
+int buttonState; //this variable tracks the state of the button, low if not pressed, high if pressed
+int ledState = HIGH; //this variable tracks the state of the LED, negative if off, positive if on
 long lastDebounceTime = 0;  // the last time the output pin was toggled
 long debounceDelay = 50;    // the debounce time; increase if the output flickers
+String command = "/arduino/mode/13/output";
+int previous_value = LOW;
 
 void setup() {
   Ciao.begin();
+  Ciao.write(CONNECTOR, SERVER_ADDR, command);
   pinMode(2, INPUT);
   
@@ -20,32 +23,29 @@
   //sample the state of the button - is it pressed or not?
   buttonState = digitalRead(2);
- 
+  
   //filter out any noise by setting a time buffer
-  if ( (millis() - lastDebounceTime) > debounceDelay) {
- 
-    //if the button has been pressed, lets toggle the LED from "off to on" or "on to off"
-    if ( (buttonState == HIGH) && (ledState < 0) ) {
- 
-      CiaoData data = Ciao.write(CONNECTOR, SERVER_ADDR, "/arduino/digital/12/1"); //turn LED on
-      ledState = -ledState; //now the LED is on, we need to change the state
-      lastDebounceTime = millis(); //set the current time
+  if ( (buttonState == HIGH) && (previous_value == LOW) && (millis() - lastDebounceTime) > debounceDelay ) {
+    if (ledState == HIGH){
+      command = "/arduino/digital/13/0";
+      ledState = LOW;
     }
-    else if ( (buttonState == HIGH) && (ledState > 0) ) {
- 
-      CiaoData data = Ciao.write(CONNECTOR, SERVER_ADDR, "/arduino/digital/12/0"); //turn LED off
-      ledState = -ledState; //now the LED is off, we need to change the state
-      lastDebounceTime = millis(); //set the current time
-        
-      if (!data.isEmpty()){
-        Ciao.println( "State: " + String (data.get(1)) );
-        Ciao.println( "Response: " + String (data.get(2)) );
-      }
-      else{ 
-        Ciao.println ("Write Error");
-      }
-              
+    else{
+      command = "/arduino/digital/13/1";
+      ledState = HIGH;
     }
- 
+     
+    lastDebounceTime = millis(); //set the current time
+    CiaoData data = Ciao.write(CONNECTOR, SERVER_ADDR, command);  
+    if (!data.isEmpty()){
+      Ciao.println( "State: " + String (data.get(1)) );
+      Ciao.println( "Response: " + String (data.get(2)) );
+    }
+    else{ 
+      Ciao.println ("Write Error");
+    }
+
   }
  
+  previous_value = buttonState;
+ 
 }
Index: rtos_arduino/trunk/arduino_lib/libraries/Ciao/examples/CiaoRestServer/CiaoRestServer.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Ciao/examples/CiaoRestServer/CiaoRestServer.ino	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Ciao/examples/CiaoRestServer/CiaoRestServer.ino	(revision 224)
@@ -1,26 +1,31 @@
 /*
 
- This sketch uses the xmpp connector to receive command for the MCU from a xmpp client.
+ This sketch uses the restserver connector receive rest calls. It allows access to
+ the analog and digital pin of the board via rest calls.
+
+ supported boards: Yun,Tian.
 
  Possible commands to send from the xmpp client:
 
- * "digital/PIN"        -> to read a digital PIN
- * "digital/PIN/VALUE"  -> to write a digital PIN (VALUE: 1/0)
- * "analog/PIN/VALUE"   -> to write in a PWM PIN(VALUE range: 0 - 255);
- * "analog/PIN"         -> to read a analog PIN
- * "servo/PIN/VALUE"	  -> to write angle in a SERVO PIN(VALUE range: 0 - 180);
- * "mode/PIN/VALUE"     -> to set the PIN mode (VALUE: input / output)
- * "ledon"              -> turn on led 13
- * "ledoff"             -> turn off led 13
- * "ciao"               -> random answers in 5 different languages
- 
- NOTE: be sure to activate and configure xmpp connector on Linino OS
-       http://labs.arduino.org/Ciao
-   
+ * "/arduino/digital/PIN"       -> to read a digital PIN
+ * "/arduino/digital/PIN/VALUE" -> to write a digital PIN (VALUE: 1/0)
+ * "/arduino/analog/PIN/VALUE"  -> to write in a PWM PIN(VALUE range: 0 - 255);
+ * "/arduino/analog/PIN"        -> to read a analog PIN
+ * "/arduino/servo/PIN/VALUE"   -> to write angle in a SERVO PIN(VALUE range: 0 - 180);
+ * "/arduino/mode/PIN/VALUE"    -> to set the PIN mode (VALUE: input / output)
+
+ Example:
+ "/arduino/mode/13/output" -> pinMode(13, OUTPUT)
+ "/arduino/digital/13/1"     -> digitalWrite(13, HIGH)
+
+
+ NOTE: be sure to activate and configure restserver connector on Linino OS
+  http://labs.arduino.org/Ciao
+
  created September 2015
  by andrea[at]arduino[dot]org
- 
+
  */
- 
+
 #include <Ciao.h>
 #include <Servo.h>
@@ -35,14 +40,14 @@
 void loop() {
 
-  CiaoData data = Ciao.read("restserver");   
-  if(!data.isEmpty()){  
+  CiaoData data = Ciao.read("restserver");
+  if(!data.isEmpty()){
     String id = data.get(0);
     String sender = data.get(1);
     String message = data.get(2);
-    
+
     message.toUpperCase();
-    
+
     String command[3];
-          
+
     splitString(message,"/",command,3);
     execute(command,id);
@@ -71,7 +76,7 @@
 void servoCommand(String cmd[], String id){
    int pin, value;
-  
+
   pin = (cmd[1]).toInt();
-  
+
   if (cmd[2] != "-1") {
     value = (cmd[2]).toInt();
@@ -85,12 +90,12 @@
   }
   else
-    Ciao.writeResponse("restserver",id,"Invalid command");  
+    Ciao.writeResponse("restserver",id,"Invalid command");
 }
 
 void digitalCommand(String cmd[], String id) {
   int pin, value;
-  
+
   pin = (cmd[1]).toInt();
-  
+
   if (cmd[2] != "-1") {
     value = (cmd[2]).toInt();
@@ -99,5 +104,5 @@
       Ciao.writeResponse("restserver",id,"Pin D"+String(pin)+" ON");
     else if(value == 0)
-      Ciao.writeResponse("restserver",id,"Pin D"+String(pin)+" OFF");   
+      Ciao.writeResponse("restserver",id,"Pin D"+String(pin)+" OFF");
   }
   else if (cmd[2] == "-1") {
@@ -111,5 +116,5 @@
 
   pin = (cmd[1]).toInt();
-  
+
   if (cmd[2] != "-1") {
     value =(cmd[2]).toInt();
@@ -127,5 +132,5 @@
 
   pin = (cmd[1]).toInt();
-  
+
   if (cmd[2] == "INPUT") {
     pinMode(pin, INPUT);
Index: rtos_arduino/trunk/arduino_lib/libraries/Ciao/keywords.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Ciao/keywords.txt	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Ciao/keywords.txt	(revision 224)
@@ -7,8 +7,8 @@
 #######################################
 
-Ciao		KEYWORD3
-CiaoData	KEYWORD3
-Wifi		KEYWORD3
-WifiData	KEYWORD3
+Ciao  KEYWORD3
+CiaoData  KEYWORD3
+Wifi  KEYWORD3
+WifiData  KEYWORD3
 
 #######################################
@@ -16,9 +16,10 @@
 #######################################
 
-begin			KEYWORD2
-writeResponse	KEYWORD2
-available		KEYWORD2
-read			KEYWORD2
-write			KEYWORD2
-bool			KEYWORD2
+writeResponse KEYWORD2
+available KEYWORD2
+read  KEYWORD2
+write KEYWORD2
 
+#######################################
+# Constants (LITERAL1)
+#######################################
Index: rtos_arduino/trunk/arduino_lib/libraries/Ciao/library.properties
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Ciao/library.properties	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Ciao/library.properties	(revision 224)
@@ -1,4 +1,4 @@
 name=Ciao
-version=1.0
+version=0.0.3
 author=Arduino srl
 maintainer=Arduino srl<info@arduino.org>
Index: rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/Ciao.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/Ciao.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/Ciao.h	(revision 224)
@@ -30,11 +30,8 @@
 
 #include <Arduino.h>
+#include <Stream.h>
 #include "lib/CiaoData.h"
 #if defined(__AVR_ATmega328P__)
-#include "lib/rest.h"
 #include "lib/SC16IS750.h"
-#include "lib/espduino.h"
-#else
-#include <Stream.h>
 #endif
 
@@ -44,5 +41,5 @@
 #define BAUDRATE 250000
 #elif defined(ARDUINO_ARCH_SAMD)
-#define BAUDRATE 115200
+#define BAUDRATE 4000000
 #endif
 
@@ -55,18 +52,10 @@
 		CiaoData parse( String, String);
 		void println(String log){};
-		#if defined(__AVR_ATmega32U4__) 
 		CiaoClass(Stream &_stream);
-		#elif defined(ARDUINO_ARCH_SAMD)
-		CiaoClass(Serial_ stream);
-		#endif
 
 	private:
 		void dropAll();
 		bool started;
-		#if defined(__AVR_ATmega32U4__)
 		Stream &stream;
-		#elif defined(ARDUINO_ARCH_SAMD)
-		Serial_ stream;
-		#endif
 };
 
@@ -80,7 +69,7 @@
 		}
 		#elif defined(ARDUINO_ARCH_SAMD)
-		SerialCiaoClass(Serial_ serial)
-			: CiaoClass(serial){
-			// Empty	
+		SerialCiaoClass(Serial_ &_serial)
+			: CiaoClass(_serial), serial(_serial) {
+			// Empty
 		}
 		#endif
@@ -93,5 +82,5 @@
 		HardwareSerial &serial;
 		#elif defined(ARDUINO_ARCH_SAMD)
-		Serial_ serial;
+		Serial_ &serial;
 		#endif
 };
@@ -103,17 +92,21 @@
 #else
 
-// class CiaoData {
-// 	public:
-		
-// 		char* get(int index){
-// 			return msg_split[index];
-// 		}
-  
-// 	public:
-// 		char* msg_split[3];
-		
-// };
+class ArduinoWifiClass : public WifiData
+{
 
-class CiaoClass {
+	public:
+		void begin();		
+
+		boolean connected();
+		void connect(char* , char*);
+
+		void powerON();
+		void powerOFF();
+
+
+};
+
+class CiaoClass : public WifiData
+{
 	public:
 		void begin();
@@ -124,10 +117,10 @@
 		CiaoData write( char*, char*, String );            // ârestâ, âhostnameâ, âStringoneâ,		
 		CiaoData write( char*, char*, String, char*);      // ârestâ, âhostnameâ, âStringoneâ, âmethodâ
-		
-		void print(String str);
-		void println(String str);
 	
 };
+
+
 extern CiaoClass Ciao;
+extern ArduinoWifiClass Wifi;
 
 #endif
Index: rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/Ciao.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/Ciao.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/Ciao.cpp	(revision 224)
@@ -24,18 +24,13 @@
 */
 
-#include "Ciao.h"
 
 #if defined(__AVR_ATmega32U4__) || defined(ARDUINO_ARCH_SAMD)
 
-#if defined(__AVR_ATmega32U4__)
+#include "Ciao.h"
+
 CiaoClass::CiaoClass(Stream &_stream) :
 	stream(_stream), started(false) {
   // Empty
 }
-#elif defined(ARDUINO_ARCH_SAMD)
-CiaoClass::CiaoClass(Serial_ stream){
-  // Empty
-}
-#endif
 
 void CiaoClass::begin() {
@@ -60,8 +55,6 @@
     }
 	do{ 
-		#if defined(__AVR_ATmega32U4__)
 		stream.print(F("run-ciao\n"));				//start bridge python
 		stream.readStringUntil(END_TX_CHAR);
-		#endif
 		delay(3000);
 		stream.println("ciao;r;status");				//check if bridge python is running
Index: rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/SC16IS750.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/SC16IS750.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/SC16IS750.cpp	(revision 224)
@@ -27,13 +27,7 @@
 #define WIRE Wire
 
-
-WifiData::WifiData(uint8_t prtcl, uint8_t addr_sspin)
-{
-    protocol = prtcl;
-    if ( protocol == SC16IS750_PROTOCOL_I2C ) {
-        device_address_sspin = (addr_sspin>>1);
-    } else {
-        device_address_sspin = addr_sspin;
-    }
+WifiData::WifiData()
+{
+    device_address_sspin = (SC16IS750_ADDRESS_AA >> 1);
     peek_flag = 0;
 }
@@ -41,7 +35,6 @@
 void WifiData::begin(uint32_t baud)
 {
-    //Serial.println("1111111111111111");
+
     if ( protocol == SC16IS750_PROTOCOL_I2C) {
-    //Serial.println("22222222222222");
         WIRE.begin();
     } 
Index: rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/SC16IS750.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/SC16IS750.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/SC16IS750.h	(revision 224)
@@ -104,10 +104,11 @@
 #define     SC16IS750_PROTOCOL_I2C  (0)
 #define     SC16IS750_PROTOCOL_I2C  (0)
-#define     END     "<!--~-->"
+#define     DELIMITER     "<!--~-->"
+#define     EOL           '\0'
 
 class WifiData : public Stream
 { 
     public:
-        WifiData(uint8_t prtcl = SC16IS750_PROTOCOL_I2C, uint8_t addr = SC16IS750_ADDRESS_AD);
+	WifiData();
         void begin(uint32_t baud);                               
         int read();
Index: rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/espduino.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/espduino.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/espduino.cpp	(revision 224)
@@ -188,6 +188,6 @@
 }
 
-ESP::ESP(Stream *serial, int chip_pd):
-_serial(serial), _chip_pd(chip_pd)
+ESP::ESP(Stream *serial):
+_serial(serial)
 {
   _debugEn = false;
@@ -195,11 +195,5 @@
 }
 
-ESP::ESP(Stream *serial, Stream* debug, int chip_pd):
-_serial(serial), _debug(debug), _chip_pd(chip_pd)
-{
-    _debugEn = true;
-    //_serial = _debug;
-    init();
-}
+
 void ESP::enable()
 {
Index: rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/espduino.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/espduino.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/espduino.h	(revision 224)
@@ -92,6 +92,6 @@
 {
 public:
-  ESP(Stream *serial, Stream* debug, int chip_pd);
-  ESP(Stream *serial, int chip_pd);
+  //ESP(Stream *serial, Stream* debug, int chip_pd);
+  ESP(Stream *serial); //ok
   Stream *_debug;
 
Index: rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/wifi.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/wifi.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Ciao/src/lib/wifi.cpp	(revision 224)
@@ -0,0 +1,192 @@
+/*
+****************************************************************************
+* Copyright (c) 2015 Arduino srl. All right reserved.
+*
+* File : wifi.cpp
+* Date : 2016/02/16
+* Revision : 0.0.1 $
+* Author: adriano[at]arduino[dot]org
+*
+****************************************************************************
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+
+#include <stdarg.h>
+#include <stdio.h>
+
+#include "Ciao.h"
+#include "crc16.h"
+#include "rest.h"
+#include "espduino.h"
+#include "ringbuf.h"
+#include "FP.h"
+#include "string.h"
+
+#if defined(__AVR_ATmega328P__)
+
+WifiData espSerial;
+ESP esp(&espSerial);
+REST rest(&esp);
+
+boolean wifiConnected = false;
+
+ArduinoWifiClass Wifi;
+CiaoClass Ciao;
+
+void wifiCb(void* response)
+{
+	uint32_t status;
+	RESPONSE res(response);
+
+	if(res.getArgc() == 1) {
+		res.popArgs((uint8_t*)&status, 4);
+		if(status == STATION_GOT_IP){
+			espSerial.println("DBG: Internet Connected");
+			wifiConnected = true;
+		}
+		else {
+			wifiConnected = false;
+		}
+	}
+}
+
+void ArduinoWifiClass::powerON(){ 		
+}
+void ArduinoWifiClass::powerOFF(){ 		
+}
+
+void ArduinoWifiClass::connect(char* ssid,char* pwd){
+	esp.wifiConnect(ssid, pwd);
+}
+boolean ArduinoWifiClass::connected(){
+	return wifiConnected;
+}
+
+void WifiBegin(short isCiao) {
+	espSerial.begin(9600);
+	if(espSerial.ping()!=1) {
+		espSerial.println("DBG: esp not found");
+		while(1);
+	}
+	else {
+		//espSerial.println("device found");
+	}
+	//put GPIO control here !!!
+	esp.enable();
+	delay(1000);
+	esp.reset();
+	delay(1000);
+	while(!esp.ready());
+	if (isCiao == 0)
+		esp.wifiCb.attach(&wifiCb);
+
+	espSerial.println("\nDBG: UnoWiFi Start");
+}
+void ArduinoWifiClass::begin() {
+	WifiBegin(0);	
+}
+
+// -----------CIAO---------------------
+
+void CiaoClass::begin() {
+	
+        WifiBegin(1);
+	rest.begin("google.com");
+	rest.get("/");
+	delay(3000);
+}
+CiaoData responseREAD(){
+	CiaoData data;
+	delay(400);
+	char response[64] = "";
+	char cstr[8] = "";
+
+	int ret = rest.getResponse(response, 64);
+	//espSerial.println(ret);
+
+	snprintf(cstr,8,"%d",ret);
+
+		data.msg_split[0]="id";
+		data.msg_split[1]=cstr;
+		data.msg_split[2]=response;
+				
+	return data;
+}
+CiaoData requestPOST(char* hostname, String stringone){
+	rest.begin(hostname);
+	//delay(1000);	fix
+	//esp.process(); 
+	rest.post((const char*) stringone.c_str(),0);
+	return responseREAD(); 
+}
+
+CiaoData requestGET(char* hostname, String stringone){
+
+	rest.begin(hostname);
+	//delay(1000);   fix
+	//esp.process(); 
+	rest.get((const char*) stringone.c_str());
+	return responseREAD(); 
+}
+
+CiaoData PassThroughM(char* connector,char* hostname, String stringone, char* method){
+  
+	short mode = 0;
+	if (!strcmp(connector, "rest")){
+		mode = 0;
+	}
+	else if (!strcmp(connector, "mqtt")){
+		mode = 1;
+	}
+	else {
+		CiaoData data;				
+		data.msg_split[0]="";
+		data.msg_split[1]="";
+		data.msg_split[2]="Protocol Error";
+		return data;
+	}
+	
+	if (mode == 0){ 
+		if (!strcmp(method, "GET")){
+			return requestGET(hostname, stringone); 
+		}
+		else if (!strcmp(method, "POST")){
+			return requestPOST(hostname, stringone); 
+		}
+		else{
+			CiaoData data;				
+			data.msg_split[0]="";
+			data.msg_split[1]="";
+			data.msg_split[2]="Method Error";
+			return data;
+		}   
+	}
+}
+
+CiaoData CiaoClass::write(char* connector,char* hostname, String stringone, char* method) {
+	return PassThroughM(connector, hostname, stringone, method);
+}
+CiaoData CiaoClass::read(char* connector,char* hostname, String stringone, char* method) {
+	return PassThroughM(connector, hostname, stringone, method);
+}
+CiaoData CiaoClass::write(char* connector,char* hostname, String stringone) {
+	return PassThroughM(connector, hostname, stringone, "GET");
+}
+CiaoData CiaoClass::read(char* connector,char* hostname, String stringone) {
+	return PassThroughM(connector, hostname, stringone, "GET");
+}
+
+#endif
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/Boards.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/Boards.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/Boards.h	(revision 224)
@@ -0,0 +1,740 @@
+/*
+  Boards.h - Hardware Abstraction Layer for Firmata library
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2015 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated December 19th, 2015
+*/
+
+#ifndef Firmata_Boards_h
+#define Firmata_Boards_h
+
+#include <inttypes.h>
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"	// for digitalRead, digitalWrite, etc
+#else
+#include "WProgram.h"
+#endif
+
+// Normally Servo.h must be included before Firmata.h (which then includes
+// this file).  If Servo.h wasn't included, this allows the code to still
+// compile, but without support for any Servos.  Hopefully that's what the
+// user intended by not including Servo.h
+#ifndef MAX_SERVOS
+#define MAX_SERVOS 0
+#endif
+
+/*
+    Firmata Hardware Abstraction Layer
+
+Firmata is built on top of the hardware abstraction functions of Arduino,
+specifically digitalWrite, digitalRead, analogWrite, analogRead, and 
+pinMode.  While these functions offer simple integer pin numbers, Firmata
+needs more information than is provided by Arduino.  This file provides
+all other hardware specific details.  To make Firmata support a new board,
+only this file should require editing.
+
+The key concept is every "pin" implemented by Firmata may be mapped to
+any pin as implemented by Arduino.  Usually a simple 1-to-1 mapping is
+best, but such mapping should not be assumed.  This hardware abstraction
+layer allows Firmata to implement any number of pins which map onto the
+Arduino implemented pins in almost any arbitrary way.
+
+
+General Constants:
+
+These constants provide basic information Firmata requires.
+
+TOTAL_PINS: The total number of pins Firmata implemented by Firmata.
+    Usually this will match the number of pins the Arduino functions
+    implement, including any pins pins capable of analog or digital.
+    However, Firmata may implement any number of pins.  For example,
+    on Arduino Mini with 8 analog inputs, 6 of these may be used
+    for digital functions, and 2 are analog only.  On such boards,
+    Firmata can implement more pins than Arduino's pinMode()
+    function, in order to accommodate those special pins.  The
+    Firmata protocol supports a maximum of 128 pins, so this
+    constant must not exceed 128.
+
+TOTAL_ANALOG_PINS: The total number of analog input pins implemented.
+    The Firmata protocol allows up to 16 analog inputs, accessed
+    using offsets 0 to 15.  Because Firmata presents the analog
+    inputs using different offsets than the actual pin numbers
+    (a legacy of Arduino's analogRead function, and the way the
+    analog input capable pins are physically labeled on all
+    Arduino boards), the total number of analog input signals
+    must be specified.  16 is the maximum.
+
+VERSION_BLINK_PIN: When Firmata starts up, it will blink the version
+    number.  This constant is the Arduino pin number where a
+    LED is connected.
+
+
+Pin Mapping Macros:
+
+These macros provide the mapping between pins as implemented by
+Firmata protocol and the actual pin numbers used by the Arduino
+functions.  Even though such mappings are often simple, pin
+numbers received by Firmata protocol should always be used as
+input to these macros, and the result of the macro should be
+used with with any Arduino function.
+
+When Firmata is extended to support a new pin mode or feature,
+a pair of macros should be added and used for all hardware
+access.  For simple 1:1 mapping, these macros add no actual
+overhead, yet their consistent use allows source code which
+uses them consistently to be easily adapted to all other boards
+with different requirements.
+
+IS_PIN_XXXX(pin): The IS_PIN macros resolve to true or non-zero
+    if a pin as implemented by Firmata corresponds to a pin
+    that actually implements the named feature.
+
+PIN_TO_XXXX(pin): The PIN_TO macros translate pin numbers as
+    implemented by Firmata to the pin numbers needed as inputs
+    to the Arduino functions.  The corresponding IS_PIN macro
+    should always be tested before using a PIN_TO macro, so
+    these macros only need to handle valid Firmata pin
+    numbers for the named feature.
+
+
+Port Access Inline Funtions:
+
+For efficiency, Firmata protocol provides access to digital
+input and output pins grouped by 8 bit ports.  When these
+groups of 8 correspond to actual 8 bit ports as implemented
+by the hardware, these inline functions can provide high
+speed direct port access.  Otherwise, a default implementation
+using 8 calls to digitalWrite or digitalRead is used.
+
+When porting Firmata to a new board, it is recommended to
+use the default functions first and focus only on the constants
+and macros above.  When those are working, if optimized port
+access is desired, these inline functions may be extended.
+The recommended approach defines a symbol indicating which
+optimization to use, and then conditional complication is
+used within these functions.
+
+readPort(port, bitmask):  Read an 8 bit port, returning the value.
+   port:    The port number, Firmata pins port*8 to port*8+7
+   bitmask: The actual pins to read, indicated by 1 bits.
+
+writePort(port, value, bitmask):  Write an 8 bit port.
+   port:    The port number, Firmata pins port*8 to port*8+7
+   value:   The 8 bit value to write
+   bitmask: The actual pins to write, indicated by 1 bits.
+*/
+
+/*==============================================================================
+ * Board Specific Configuration
+ *============================================================================*/
+
+#ifndef digitalPinHasPWM
+#define digitalPinHasPWM(p)     IS_PIN_DIGITAL(p)
+#endif
+
+// Arduino Duemilanove, Diecimila, and NG
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__)
+#if defined(NUM_ANALOG_INPUTS) && NUM_ANALOG_INPUTS == 6
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              20 // 14 digital + 6 analog
+#else
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              22 // 14 digital + 8 analog
+#endif
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 19)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) < 14 + TOTAL_ANALOG_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+#define ARDUINO_PINOUT_OPTIMIZE 1
+
+
+// Wiring (and board)
+#elif defined(WIRING)
+#define VERSION_BLINK_PIN       WLED
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= FIRST_ANALOG_PIN && (p) < (FIRST_ANALOG_PIN+TOTAL_ANALOG_PINS))
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == SDA || (p) == SCL)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - FIRST_ANALOG_PIN)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p) 
+
+
+// old Arduinos
+#elif defined(__AVR_ATmega8__)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              20 // 14 digital + 6 analog
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 19)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 19)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+#define ARDUINO_PINOUT_OPTIMIZE 1
+
+
+// Arduino Mega
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+#define TOTAL_ANALOG_PINS       16
+#define TOTAL_PINS              70 // 54 digital + 16 analog
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          19
+#define PIN_SERIAL1_TX          18
+#define PIN_SERIAL2_RX          17
+#define PIN_SERIAL2_TX          16
+#define PIN_SERIAL3_RX          15
+#define PIN_SERIAL3_TX          14
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 54 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 2 && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 20 || (p) == 21)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) > 13 && (p) < 20)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 54)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// Arduino DUE
+#elif defined(__SAM3X8E__)
+#define TOTAL_ANALOG_PINS       12
+#define TOTAL_PINS              66 // 54 digital + 12 analog
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          19
+#define PIN_SERIAL1_TX          18
+#define PIN_SERIAL2_RX          17
+#define PIN_SERIAL2_TX          16
+#define PIN_SERIAL3_RX          15
+#define PIN_SERIAL3_TX          14
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 54 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 2 && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 20 || (p) == 21) // 70 71
+#define IS_PIN_SERIAL(p)        ((p) > 13 && (p) < 20)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 54)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// Arduino M0 / M0 pro
+#elif defined(__SAMD21G18A__)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              34 // 14 digital + 6 analog + 2 i2c + 3 spi. 34 is due to non linear enumeration of pins
+#define TOTAL_PORTS				3
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       (((p) >= 2 && (p) <= 13) || ((p)>=24 && (p)<=29))
+#define IS_PIN_ANALOG(p)        ((p) >= 24 && (p) <= 29)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 16 || (p) == 17)
+#define IS_PIN_SPI(p)			((p)== MOSI || (p) == MISO || (p == SCK))
+#define IS_PIN_SERIAL(p)		((p) == 0 || (p) == 1)
+#define PIN_SERIAL5_RX			0 //Serial5
+#define PIN_SERIAL5_TX			1 //Serial5
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 24)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy 1.0
+#elif defined(__AVR_AT90USB162__)
+#define TOTAL_ANALOG_PINS       0
+#define TOTAL_PINS              21 // 21 digital + no analog
+#define VERSION_BLINK_PIN       6
+#define PIN_SERIAL1_RX          2
+#define PIN_SERIAL1_TX          3
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        (0)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           (0)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 2 || (p) == 3)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (0)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy 2.0
+#elif defined(__AVR_ATmega32U4__) && defined(CORE_TEENSY)
+#define TOTAL_ANALOG_PINS       12
+#define TOTAL_PINS              25 // 11 digital + 12 analog
+#define VERSION_BLINK_PIN       11
+#define PIN_SERIAL1_RX          7
+#define PIN_SERIAL1_TX          8
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 11 && (p) <= 22)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 5 || (p) == 6)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 7 || (p) == 8)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (((p) < 22) ? 21 - (p) : 11)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy 3.0, 3.1 and 3.2
+#elif defined(__MK20DX128__) || defined(__MK20DX256__)
+#define TOTAL_ANALOG_PINS       14
+#define TOTAL_PINS              38 // 24 digital + 10 analog-digital + 4 analog
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          0
+#define PIN_SERIAL1_TX          1
+#define PIN_SERIAL2_RX          9
+#define PIN_SERIAL2_TX          10
+#define PIN_SERIAL3_RX          7
+#define PIN_SERIAL3_TX          8
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) <= 33)
+#define IS_PIN_ANALOG(p)        (((p) >= 14 && (p) <= 23) || ((p) >= 34 && (p) <= 38))
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define IS_PIN_SERIAL(p)        (((p) > 6 && (p) < 11) || ((p) == 0 || (p) == 1))
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (((p) <= 23) ? (p) - 14 : (p) - 24)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy-LC
+#elif defined(__MKL26Z64__)
+#define TOTAL_ANALOG_PINS       13
+#define TOTAL_PINS              27 // 27 digital + 13 analog-digital
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          0
+#define PIN_SERIAL1_TX          1
+#define PIN_SERIAL2_RX          9
+#define PIN_SERIAL2_TX          10
+#define PIN_SERIAL3_RX          7
+#define PIN_SERIAL3_TX          8
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) <= 26)
+#define IS_PIN_ANALOG(p)        ((p) >= 14)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define IS_PIN_SERIAL(p)        (((p) > 6 && (p) < 11) || ((p) == 0 || (p) == 1))
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy++ 1.0 and 2.0
+#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              46 // 38 digital + 8 analog
+#define VERSION_BLINK_PIN       6
+#define PIN_SERIAL1_RX          2
+#define PIN_SERIAL1_TX          3
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 38 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 0 || (p) == 1)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 2 || (p) == 3)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 38)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Leonardo
+#elif defined(__AVR_ATmega32U4__)
+#define TOTAL_ANALOG_PINS       12
+#define TOTAL_PINS              30 // 14 digital + 12 analog + 4 SPI (D14-D17 on ISP header)
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          0
+#define PIN_SERIAL1_TX          1
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 18 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           ((p) == 3 || (p) == 5 || (p) == 6 || (p) == 9 || (p) == 10 || (p) == 11 || (p) == 13)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 2 || (p) == 3)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 0 || (p) == 1)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (p) - 18
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Intel Galileo Board (gen 1 and 2) and Intel Edison
+#elif defined(ARDUINO_LINUX)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              20 // 14 digital + 6 analog
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          0
+#define PIN_SERIAL1_TX          1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 19)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 19)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == SDA || (p) == SCL)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 0 || (p) == 1)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// RedBearLab BLE Nano with factory switch settings (S1 - S10)
+#elif defined(BLE_NANO)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              15 // 9 digital + 3 analog
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 14)
+#define IS_PIN_ANALOG(p)        ((p) == 8 || (p) == 9 || (p) == 10 || (p) == 11 || (p) == 12 || (p) == 14) //A0~A5
+#define IS_PIN_PWM(p)           ((p) == 3 || (p) == 5 || (p) == 6)
+#define IS_PIN_SERVO(p)         ((p) >= 2 && (p) <= 7)
+#define IS_PIN_I2C(p)           ((p) == SDA || (p) == SCL)
+#define IS_PIN_SPI(p)           ((p) == CS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 8)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Sanguino
+#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              32 // 24 digital + 8 analog
+#define VERSION_BLINK_PIN       0
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 24 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 16 || (p) == 17)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 24)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// Illuminato
+#elif defined(__AVR_ATmega645__)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              42 // 36 digital + 6 analog
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 36 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 4 || (p) == 5)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 36)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// Pic32 chipKIT FubarinoSD
+#elif defined(_BOARD_FUBARINO_SD_)
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS  // 15
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 45, All pins can be digital
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       1
+#define IS_PIN_ANALOG(p)        ((p) >= 30 && (p) <= 44)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 1 || (p) == 2)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (14 - (p - 30))
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT FubarinoMini
+// Note, FubarinoMini analog pin 20 will not function in Firmata as analog input due to limitation in analog mapping
+#elif defined(_BOARD_FUBARINO_MINI_)
+#define TOTAL_ANALOG_PINS       14 // We have to fake this because of the poor analog pin mapping planning in FubarinoMini
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 33
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) != 14 && (p) != 15 && (p) != 31 && (p) != 32)
+#define IS_PIN_ANALOG(p)        ((p) == 0 || ((p) >= 3 && (p) <= 13))
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 25 || (p) == 26)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (p)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT UNO32
+#elif defined(_BOARD_UNO_) && defined(__PIC32)  // NOTE: no _BOARD_UNO32_ to use
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS // 12
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 47 All pins can be digital
+#define MAX_SERVOS              NUM_DIGITAL_PINS // All pins can be servo with SoftPWMservo
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 25)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 45 || (p) == 46)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT DP32
+#elif defined(_BOARD_DP32_)
+#define TOTAL_ANALOG_PINS       15  // Really only has 9, but have to override because of mistake in variant file
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 19
+#define MAX_SERVOS              NUM_DIGITAL_PINS // All pins can be servo with SoftPWMservo
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       (((p) != 1) && ((p) != 4) && ((p) != 5) && ((p) != 15) && ((p) != 16))
+#define IS_PIN_ANALOG(p)        ((p) >= 6 && (p) <= 14)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 2 || (p) == 3)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (p)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT uC32
+#elif defined(_BOARD_UC32_)
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS  // 12
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 47 All pins can be digital
+#define MAX_SERVOS              NUM_DIGITAL_PINS // All pins can be servo with SoftPWMservo
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 25)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 45 || (p) == 46)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT WF32
+#elif defined(_BOARD_WF32_)
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS
+#define TOTAL_PINS              NUM_DIGITAL_PINS
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 49)     // Accounts for SD and WiFi dedicated pins
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 25)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 34 || (p) == 35)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT WiFire
+#elif defined(_BOARD_WIFIRE_)
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS  // 14
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 71
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 47)     // Accounts for SD and WiFi dedicated pins
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 25)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 34 || (p) == 35)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) <= 25 ? ((p) - 14) : (p) - 36)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT MAX32
+#elif defined(_BOARD_MEGA_) && defined(__PIC32)  // NOTE: no _BOARD_MAX32_ to use
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS  // 16
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 87
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2)
+#define IS_PIN_ANALOG(p)        ((p) >= 54 && (p) <= 69)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 34 || (p) == 35)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 54)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT Pi
+#elif defined(_BOARD_CHIPKIT_PI_)
+#define TOTAL_ANALOG_PINS       16
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 19
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       (((p) >= 2) && ((p) <= 3) || (((p) >= 8) && ((p) <= 13)) || (((p) >= 14) && ((p) <= 17)))
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 17)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 16 || (p) == 17)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) <= 15 ? (p) - 14 : (p) - 12)
+//#define PIN_TO_ANALOG(p)        (((p) <= 16) ? ((p) - 14) : ((p) - 16))
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+// Pinoccio Scout
+// Note: digital pins 9-16 are usable but not labeled on the board numerically.
+// SS=9, MOSI=10, MISO=11, SCK=12, RX1=13, TX1=14, SCL=15, SDA=16
+#elif defined(ARDUINO_PINOCCIO)
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 32
+#define VERSION_BLINK_PIN       23
+#define PIN_SERIAL1_RX          13
+#define PIN_SERIAL1_TX          14
+#define IS_PIN_DIGITAL(p)       (((p) >= 2) && ((p) <= 16)) || (((p) >= 24) && ((p) <= 31))
+#define IS_PIN_ANALOG(p)        ((p) >= 24 && (p) <= 31)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == SCL || (p) == SDA)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 13 || (p) == 14)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 24)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+// anything else
+#else
+#error "Please edit Boards.h with a hardware abstraction for this board"
+#endif
+
+// as long this is not defined for all boards:
+#ifndef IS_PIN_SPI
+#define IS_PIN_SPI(p)           0
+#endif
+
+#ifndef IS_PIN_SERIAL
+#define IS_PIN_SERIAL(p)        0
+#endif
+
+/*==============================================================================
+ * readPort() - Read an 8 bit port
+ *============================================================================*/
+
+static inline unsigned char readPort(byte, byte) __attribute__((always_inline, unused));
+static inline unsigned char readPort(byte port, byte bitmask)
+{
+#if defined(ARDUINO_PINOUT_OPTIMIZE)
+	if (port == 0) return (PIND & 0xFC) & bitmask; // ignore Rx/Tx 0/1
+	if (port == 1) return ((PINB & 0x3F) | ((PINC & 0x03) << 6)) & bitmask;
+	if (port == 2) return ((PINC & 0x3C) >> 2) & bitmask;
+	return 0;
+#else
+	unsigned char out=0, pin=port*8;
+	if (IS_PIN_DIGITAL(pin+0) && (bitmask & 0x01) && digitalRead(PIN_TO_DIGITAL(pin+0))) out |= 0x01;
+	if (IS_PIN_DIGITAL(pin+1) && (bitmask & 0x02) && digitalRead(PIN_TO_DIGITAL(pin+1))) out |= 0x02;
+	if (IS_PIN_DIGITAL(pin+2) && (bitmask & 0x04) && digitalRead(PIN_TO_DIGITAL(pin+2))) out |= 0x04;
+	if (IS_PIN_DIGITAL(pin+3) && (bitmask & 0x08) && digitalRead(PIN_TO_DIGITAL(pin+3))) out |= 0x08;
+	if (IS_PIN_DIGITAL(pin+4) && (bitmask & 0x10) && digitalRead(PIN_TO_DIGITAL(pin+4))) out |= 0x10;
+	if (IS_PIN_DIGITAL(pin+5) && (bitmask & 0x20) && digitalRead(PIN_TO_DIGITAL(pin+5))) out |= 0x20;
+	if (IS_PIN_DIGITAL(pin+6) && (bitmask & 0x40) && digitalRead(PIN_TO_DIGITAL(pin+6))) out |= 0x40;
+	if (IS_PIN_DIGITAL(pin+7) && (bitmask & 0x80) && digitalRead(PIN_TO_DIGITAL(pin+7))) out |= 0x80;
+	return out;
+#endif
+}
+
+/*==============================================================================
+ * writePort() - Write an 8 bit port, only touch pins specified by a bitmask
+ *============================================================================*/
+
+static inline unsigned char writePort(byte, byte, byte) __attribute__((always_inline, unused));
+static inline unsigned char writePort(byte port, byte value, byte bitmask)
+{
+#if defined(ARDUINO_PINOUT_OPTIMIZE)
+	if (port == 0) {
+		bitmask = bitmask & 0xFC;  // do not touch Tx & Rx pins
+		byte valD = value & bitmask;
+		byte maskD = ~bitmask;
+		cli();
+		PORTD = (PORTD & maskD) | valD;
+		sei();
+	} else if (port == 1) {
+		byte valB = (value & bitmask) & 0x3F;
+		byte valC = (value & bitmask) >> 6;
+		byte maskB = ~(bitmask & 0x3F);
+		byte maskC = ~((bitmask & 0xC0) >> 6);
+		cli();
+		PORTB = (PORTB & maskB) | valB;
+		PORTC = (PORTC & maskC) | valC;
+		sei();
+	} else if (port == 2) {
+		bitmask = bitmask & 0x0F;
+		byte valC = (value & bitmask) << 2;
+		byte maskC = ~(bitmask << 2);
+		cli();
+		PORTC = (PORTC & maskC) | valC;
+		sei();
+	}
+#else
+	byte pin=port*8;
+	if ((bitmask & 0x01)) digitalWrite(PIN_TO_DIGITAL(pin+0), (value & 0x01));
+	if ((bitmask & 0x02)) digitalWrite(PIN_TO_DIGITAL(pin+1), (value & 0x02));
+	if ((bitmask & 0x04)) digitalWrite(PIN_TO_DIGITAL(pin+2), (value & 0x04));
+	if ((bitmask & 0x08)) digitalWrite(PIN_TO_DIGITAL(pin+3), (value & 0x08));
+	if ((bitmask & 0x10)) digitalWrite(PIN_TO_DIGITAL(pin+4), (value & 0x10));
+	if ((bitmask & 0x20)) digitalWrite(PIN_TO_DIGITAL(pin+5), (value & 0x20));
+	if ((bitmask & 0x40)) digitalWrite(PIN_TO_DIGITAL(pin+6), (value & 0x40));
+	if ((bitmask & 0x80)) digitalWrite(PIN_TO_DIGITAL(pin+7), (value & 0x80));
+
+#endif
+}
+
+
+
+
+#ifndef TOTAL_PORTS
+#define TOTAL_PORTS             ((TOTAL_PINS + 7) / 8)
+#endif
+
+
+#endif /* Firmata_Boards_h */
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.cpp	(revision 224)
@@ -0,0 +1,668 @@
+/*
+  Firmata.cpp - Firmata library v2.5.2 - 2016-2-15
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+//******************************************************************************
+//* Includes
+//******************************************************************************
+
+#include "Firmata.h"
+#include "HardwareSerial.h"
+
+extern "C" {
+#include <string.h>
+#include <stdlib.h>
+}
+
+//******************************************************************************
+//* Support Functions
+//******************************************************************************
+
+/**
+ * Split a 16-bit byte into two 7-bit values and write each value.
+ * @param value The 16-bit value to be split and written separately.
+ */
+void FirmataClass::sendValueAsTwo7bitBytes(int value)
+{
+  FirmataStream->write(value & 0x7F); // LSB
+  FirmataStream->write(value >> 7 & 0x7F); // MSB
+}
+
+/**
+ * A helper method to write the beginning of a Sysex message transmission.
+ */
+void FirmataClass::startSysex(void)
+{
+  FirmataStream->write(START_SYSEX);
+}
+
+/**
+ * A helper method to write the end of a Sysex message transmission.
+ */
+void FirmataClass::endSysex(void)
+{
+  FirmataStream->write(END_SYSEX);
+}
+
+//******************************************************************************
+//* Constructors
+//******************************************************************************
+
+/**
+ * The Firmata class.
+ * An instance named "Firmata" is created automatically for the user.
+ */
+FirmataClass::FirmataClass()
+{
+  firmwareVersionCount = 0;
+  firmwareVersionVector = 0;
+  systemReset();
+}
+
+//******************************************************************************
+//* Public Methods
+//******************************************************************************
+
+/**
+ * Initialize the default Serial transport at the default baud of 57600.
+ */
+void FirmataClass::begin(void)
+{
+  begin(57600);
+}
+
+/**
+ * Initialize the default Serial transport and override the default baud.
+ * Sends the protocol version to the host application followed by the firmware version and name.
+ * blinkVersion is also called. To skip the call to blinkVersion, call Firmata.disableBlinkVersion()
+ * before calling Firmata.begin(baud).
+ * @param speed The baud to use. 57600 baud is the default value.
+ */
+void FirmataClass::begin(long speed)
+{
+  Serial.begin(speed);
+  FirmataStream = &Serial;
+  blinkVersion();
+  printVersion();         // send the protocol version
+  printFirmwareVersion(); // send the firmware name and version
+}
+
+/**
+ * Reassign the Firmata stream transport.
+ * @param s A reference to the Stream transport object. This can be any type of
+ * transport that implements the Stream interface. Some examples include Ethernet, WiFi
+ * and other UARTs on the board (Serial1, Serial2, etc).
+ */
+void FirmataClass::begin(Stream &s)
+{
+  FirmataStream = &s;
+  // do not call blinkVersion() here because some hardware such as the
+  // Ethernet shield use pin 13
+  printVersion();
+  printFirmwareVersion();
+}
+
+/**
+ * Send the Firmata protocol version to the Firmata host application.
+ */
+void FirmataClass::printVersion(void)
+{	
+  FirmataStream->write(REPORT_VERSION);
+  FirmataStream->write(FIRMATA_PROTOCOL_MAJOR_VERSION);
+  FirmataStream->write(FIRMATA_PROTOCOL_MINOR_VERSION);
+}
+
+/**
+ * Blink the Firmata protocol version to the onboard LEDs (if the board has an onboard LED).
+ * If VERSION_BLINK_PIN is not defined in Boards.h for a particular board, then this method
+ * does nothing.
+ * The first series of flashes indicates the firmware major version (2 flashes = 2).
+ * The second series of flashes indicates the firmware minor version (5 flashes = 5).
+ */
+void FirmataClass::blinkVersion(void)
+{
+#if defined(VERSION_BLINK_PIN)
+  if (blinkVersionDisabled) return;
+  // flash the pin with the protocol version
+  pinMode(VERSION_BLINK_PIN, OUTPUT);
+  strobeBlinkPin(VERSION_BLINK_PIN, FIRMATA_FIRMWARE_MAJOR_VERSION, 40, 210);
+  delay(250);
+  strobeBlinkPin(VERSION_BLINK_PIN, FIRMATA_FIRMWARE_MINOR_VERSION, 40, 210);
+  delay(125);
+#endif
+}
+
+/**
+ * Provides a means to disable the version blink sequence on the onboard LED, trimming startup
+ * time by a couple of seconds.
+ * Call this before Firmata.begin(). It only applies when using the default Serial transport.
+ */
+void FirmataClass::disableBlinkVersion()
+{
+  blinkVersionDisabled = true;
+}
+
+/**
+ * Sends the firmware name and version to the Firmata host application. The major and minor version
+ * numbers are the first 2 bytes in the message. The following bytes are the characters of the
+ * firmware name.
+ */
+void FirmataClass::printFirmwareVersion(void)
+{
+  byte i;
+
+  if (firmwareVersionCount) { // make sure that the name has been set before reporting
+    startSysex();
+    FirmataStream->write(REPORT_FIRMWARE);
+    FirmataStream->write(firmwareVersionVector[0]); // major version number
+    FirmataStream->write(firmwareVersionVector[1]); // minor version number
+    for (i = 2; i < firmwareVersionCount; ++i) {
+      sendValueAsTwo7bitBytes(firmwareVersionVector[i]);
+    }
+    endSysex();
+  }
+}
+
+/**
+ * Sets the name and version of the firmware. This is not the same version as the Firmata protocol
+ * (although at times the firmware version and protocol version may be the same number).
+ * @param name A pointer to the name char array
+ * @param major The major version number
+ * @param minor The minor version number
+ */
+void FirmataClass::setFirmwareNameAndVersion(const char *name, byte major, byte minor)
+{
+  const char *firmwareName;
+  const char *extension;
+
+  // parse out ".cpp" and "applet/" that comes from using __FILE__
+  extension = strstr(name, ".cpp");
+  firmwareName = strrchr(name, '/');
+
+  if (!firmwareName) {
+    // windows
+    firmwareName = strrchr(name, '\\');
+  }
+  if (!firmwareName) {
+    // user passed firmware name
+    firmwareName = name;
+  } else {
+    firmwareName ++;
+  }
+
+  if (!extension) {
+    firmwareVersionCount = strlen(firmwareName) + 2;
+  } else {
+    firmwareVersionCount = extension - firmwareName + 2;
+  }
+
+  // in case anyone calls setFirmwareNameAndVersion more than once
+  free(firmwareVersionVector);
+
+  firmwareVersionVector = (byte *) malloc(firmwareVersionCount + 1);
+  firmwareVersionVector[firmwareVersionCount] = 0;
+  firmwareVersionVector[0] = major;
+  firmwareVersionVector[1] = minor;
+  strncpy((char *)firmwareVersionVector + 2, firmwareName, firmwareVersionCount - 2);
+}
+
+//------------------------------------------------------------------------------
+// Serial Receive Handling
+
+/**
+ * A wrapper for Stream::available()
+ * @return The number of bytes remaining in the input stream buffer.
+ */
+int FirmataClass::available(void)
+{
+  return FirmataStream->available();
+}
+
+/**
+ * Process incoming sysex messages. Handles REPORT_FIRMWARE and STRING_DATA internally.
+ * Calls callback function for STRING_DATA and all other sysex messages.
+ * @private
+ */
+void FirmataClass::processSysexMessage(void)
+{
+  switch (storedInputData[0]) { //first byte in buffer is command
+    case REPORT_FIRMWARE:
+      printFirmwareVersion();
+      break;
+    case STRING_DATA:
+      if (currentStringCallback) {
+        byte bufferLength = (sysexBytesRead - 1) / 2;
+        byte i = 1;
+        byte j = 0;
+        while (j < bufferLength) {
+          // The string length will only be at most half the size of the
+          // stored input buffer so we can decode the string within the buffer.
+          storedInputData[j] = storedInputData[i];
+          i++;
+          storedInputData[j] += (storedInputData[i] << 7);
+          i++;
+          j++;
+        }
+        // Make sure string is null terminated. This may be the case for data
+        // coming from client libraries in languages that don't null terminate
+        // strings.
+        if (storedInputData[j - 1] != '\0') {
+          storedInputData[j] = '\0';
+        }
+        (*currentStringCallback)((char *)&storedInputData[0]);
+      }
+      break;
+    default:
+      if (currentSysexCallback)
+        (*currentSysexCallback)(storedInputData[0], sysexBytesRead - 1, storedInputData + 1);
+  }
+}
+
+/**
+ * Read a single int from the input stream. If the value is not = -1, pass it on to parse(byte)
+ */
+void FirmataClass::processInput(void)
+{
+  int inputData = FirmataStream->read(); // this is 'int' to handle -1 when no data
+  if (inputData != -1) {
+    parse(inputData);
+  }
+}
+
+/**
+ * Parse data from the input stream.
+ * @param inputData A single byte to be added to the parser.
+ */
+void FirmataClass::parse(byte inputData)
+{
+  int command;
+  if (parsingSysex) {
+    if (inputData == END_SYSEX) {
+      //stop sysex byte
+      parsingSysex = false;
+      //fire off handler function
+      processSysexMessage();
+    } else {
+      //normal data byte - add to buffer
+      storedInputData[sysexBytesRead] = inputData;
+      sysexBytesRead++;
+		
+    }
+  } else if ( (waitForData > 0) && (inputData < 128) ) {
+    waitForData--;
+    storedInputData[waitForData] = inputData;
+    if ( (waitForData == 0) && executeMultiByteCommand ) { // got the whole message
+      switch (executeMultiByteCommand) {
+        case ANALOG_MESSAGE:
+          if (currentAnalogCallback) {
+            (*currentAnalogCallback)(multiByteChannel,
+                                     (storedInputData[0] << 7)
+                                     + storedInputData[1]);
+          }
+          break;
+        case DIGITAL_MESSAGE:
+          if (currentDigitalCallback) {
+            (*currentDigitalCallback)(multiByteChannel,
+                                      (storedInputData[0] << 7)
+                                      + storedInputData[1]);
+          }
+          break;
+         case SET_PIN_MODE:
+          if (currentPinModeCallback)
+            (*currentPinModeCallback)(storedInputData[1], storedInputData[0]);
+          break;
+        case SET_DIGITAL_PIN_VALUE:
+          if (currentPinValueCallback)
+            (*currentPinValueCallback)(storedInputData[1], storedInputData[0]);
+          break;
+        case REPORT_ANALOG:
+          if (currentReportAnalogCallback)
+            (*currentReportAnalogCallback)(multiByteChannel, storedInputData[0]);
+          break;
+        case REPORT_DIGITAL:
+          if (currentReportDigitalCallback)
+            (*currentReportDigitalCallback)(multiByteChannel, storedInputData[0]);
+          break;
+      }
+      executeMultiByteCommand = 0;
+    }
+  } else {
+    // remove channel info from command byte if less than 0xF0
+    if (inputData < 0xF0) {
+      command = inputData & 0xF0;
+      multiByteChannel = inputData & 0x0F;
+    } else {
+      command = inputData;
+      // commands in the 0xF* range don't use channel data
+    }
+    switch (command) {
+      case ANALOG_MESSAGE:
+      case DIGITAL_MESSAGE:
+      case SET_PIN_MODE:
+      case SET_DIGITAL_PIN_VALUE:
+        waitForData = 2; // two data bytes needed
+        executeMultiByteCommand = command;
+        break;
+      case REPORT_ANALOG:
+      case REPORT_DIGITAL:
+        waitForData = 1; // one data byte needed
+        executeMultiByteCommand = command;
+        break;
+      case START_SYSEX:
+        parsingSysex = true;
+        sysexBytesRead = 0;
+        break;
+      case SYSTEM_RESET:
+        systemReset();
+        break;
+      case REPORT_VERSION:
+        Firmata.printVersion();
+        break;
+    }
+  }
+}
+
+/**
+ * @return Returns true if the parser is actively parsing data.
+ */
+boolean FirmataClass::isParsingMessage(void)
+{
+  return (waitForData > 0 || parsingSysex);
+}
+
+//------------------------------------------------------------------------------
+// Output Stream Handling
+
+/**
+ * Send an analog message to the Firmata host application. The range of pins is limited to [0..15]
+ * when using the ANALOG_MESSAGE. The maximum value of the ANALOG_MESSAGE is limited to 14 bits
+ * (16384). To increase the pin range or value, see the documentation for the EXTENDED_ANALOG
+ * message.
+ * @param pin The analog pin to send the value of (limited to pins 0 - 15).
+ * @param value The value of the analog pin (0 - 1024 for 10-bit analog, 0 - 4096 for 12-bit, etc).
+ * The maximum value is 14-bits (16384).
+ */
+void FirmataClass::sendAnalog(byte pin, int value)
+{
+  // pin can only be 0-15, so chop higher bits
+  FirmataStream->write(ANALOG_MESSAGE | (pin & 0xF));
+  sendValueAsTwo7bitBytes(value);
+}
+
+/* (intentionally left out asterix here)
+ * STUB - NOT IMPLEMENTED
+ * Send a single digital pin value to the Firmata host application.
+ * @param pin The digital pin to send the value of.
+ * @param value The value of the pin.
+ */
+void FirmataClass::sendDigital(byte pin, int value)
+{
+  /* TODO add single pin digital messages to the protocol, this needs to
+   * track the last digital data sent so that it can be sure to change just
+   * one bit in the packet.  This is complicated by the fact that the
+   * numbering of the pins will probably differ on Arduino, Wiring, and
+   * other boards.
+   */
+
+  // TODO: the digital message should not be sent on the serial port every
+  // time sendDigital() is called.  Instead, it should add it to an int
+  // which will be sent on a schedule.  If a pin changes more than once
+  // before the digital message is sent on the serial port, it should send a
+  // digital message for each change.
+
+  //    if(value == 0)
+  //        sendDigitalPortPair();
+}
+
+
+/**
+ * Send an 8-bit port in a single digital message (protocol v2 and later).
+ * Send 14-bits in a single digital message (protocol v1).
+ * @param portNumber The port number to send. Note that this is not the same as a "port" on the
+ * physical microcontroller. Ports are defined in order per every 8 pins in ascending order
+ * of the Arduino digital pin numbering scheme. Port 0 = pins D0 - D7, port 1 = pins D8 - D15, etc.
+ * @param portData The value of the port. The value of each pin in the port is represented by a bit.
+ */
+void FirmataClass::sendDigitalPort(byte portNumber, int portData)
+{
+  FirmataStream->write(DIGITAL_MESSAGE | (portNumber & 0xF));
+  FirmataStream->write((byte)portData % 128); // Tx bits 0-6 (protocol v1 and higher)
+  FirmataStream->write(portData >> 7);  // Tx bits 7-13 (bit 7 only for protocol v2 and higher)
+}
+
+/**
+ * Send a sysex message where all values after the command byte are packet as 2 7-bit bytes
+ * (this is not always the case so this function is not always used to send sysex messages).
+ * @param command The sysex command byte.
+ * @param bytec The number of data bytes in the message (excludes start, command and end bytes).
+ * @param bytev A pointer to the array of data bytes to send in the message.
+ */
+void FirmataClass::sendSysex(byte command, byte bytec, byte *bytev)
+{
+  byte i;
+  startSysex();
+  FirmataStream->write(command);
+  for (i = 0; i < bytec; i++) {
+    sendValueAsTwo7bitBytes(bytev[i]);
+  }
+  endSysex();
+}
+
+/**
+ * Send a string to the Firmata host application.
+ * @param command Must be STRING_DATA
+ * @param string A pointer to the char string
+ */
+void FirmataClass::sendString(byte command, const char *string)
+{
+  if (command == STRING_DATA) {
+    sendSysex(command, strlen(string), (byte *)string);
+  }
+}
+
+/**
+ * Send a string to the Firmata host application.
+ * @param string A pointer to the char string
+ */
+void FirmataClass::sendString(const char *string)
+{
+  sendString(STRING_DATA, string);
+}
+
+/**
+ * A wrapper for Stream::available().
+ * Write a single byte to the output stream.
+ * @param c The byte to be written.
+ */
+void FirmataClass::write(byte c)
+{
+  FirmataStream->write(c);
+}
+
+/**
+ * Attach a generic sysex callback function to a command (options are: ANALOG_MESSAGE,
+ * DIGITAL_MESSAGE, REPORT_ANALOG, REPORT DIGITAL, SET_PIN_MODE and SET_DIGITAL_PIN_VALUE).
+ * @param command The ID of the command to attach a callback function to.
+ * @param newFunction A reference to the callback function to attach.
+ */
+void FirmataClass::attach(byte command, callbackFunction newFunction)
+{
+  switch (command) {
+    case ANALOG_MESSAGE: currentAnalogCallback = newFunction; break;
+    case DIGITAL_MESSAGE: currentDigitalCallback = newFunction; break;
+    case REPORT_ANALOG: currentReportAnalogCallback = newFunction; break;
+    case REPORT_DIGITAL: currentReportDigitalCallback = newFunction; break;
+    case SET_PIN_MODE: currentPinModeCallback = newFunction; break;
+    case SET_DIGITAL_PIN_VALUE: currentPinValueCallback = newFunction; break;
+  }
+}
+
+/**
+ * Attach a callback function for the SYSTEM_RESET command.
+ * @param command Must be set to SYSTEM_RESET or it will be ignored.
+ * @param newFunction A reference to the system reset callback function to attach.
+ */
+void FirmataClass::attach(byte command, systemResetCallbackFunction newFunction)
+{
+  switch (command) {
+    case SYSTEM_RESET: currentSystemResetCallback = newFunction; break;
+  }
+}
+
+/**
+ * Attach a callback function for the STRING_DATA command.
+ * @param command Must be set to STRING_DATA or it will be ignored.
+ * @param newFunction A reference to the string callback function to attach.
+ */
+void FirmataClass::attach(byte command, stringCallbackFunction newFunction)
+{
+  switch (command) {
+    case STRING_DATA: currentStringCallback = newFunction; break;
+  }
+}
+
+/**
+ * Attach a generic sysex callback function to sysex command.
+ * @param command The ID of the command to attach a callback function to.
+ * @param newFunction A reference to the sysex callback function to attach.
+ */
+void FirmataClass::attach(byte command, sysexCallbackFunction newFunction)
+{
+  currentSysexCallback = newFunction;
+}
+
+/**
+ * Detach a callback function for a specified command (such as SYSTEM_RESET, STRING_DATA,
+ * ANALOG_MESSAGE, DIGITAL_MESSAGE, etc).
+ * @param command The ID of the command to detatch the callback function from.
+ */
+void FirmataClass::detach(byte command)
+{
+  switch (command) {
+    case SYSTEM_RESET: currentSystemResetCallback = NULL; break;
+    case STRING_DATA: currentStringCallback = NULL; break;
+    case START_SYSEX: currentSysexCallback = NULL; break;
+    default:
+      attach(command, (callbackFunction)NULL);
+  }
+}
+
+/**
+ * @param pin The pin to get the configuration of.
+ * @return The configuration of the specified pin.
+ */
+byte FirmataClass::getPinMode(byte pin)
+{
+  return pinConfig[pin];
+}
+
+/**
+ * Set the pin mode/configuration. The pin configuration (or mode) in Firmata represents the
+ * current function of the pin. Examples are digital input or output, analog input, pwm, i2c,
+ * serial (uart), etc.
+ * @param pin The pin to configure.
+ * @param config The configuration value for the specified pin.
+ */
+void FirmataClass::setPinMode(byte pin, byte config)
+{
+  if (pinConfig[pin] == PIN_MODE_IGNORE)
+    return;
+
+  pinConfig[pin] = config;
+}
+
+/**
+ * @param pin The pin to get the state of.
+ * @return The state of the specified pin.
+ */
+int FirmataClass::getPinState(byte pin)
+{
+  return pinState[pin];
+}
+
+/**
+ * Set the pin state. The pin state of an output pin is the pin value. The state of an
+ * input pin is 0, unless the pin has it's internal pull up resistor enabled, then the value is 1.
+ * @param pin The pin to set the state of
+ * @param state Set the state of the specified pin
+ */
+void FirmataClass::setPinState(byte pin, int state)
+{
+  pinState[pin] = state;
+}
+
+// sysex callbacks
+/*
+ * this is too complicated for analogReceive, but maybe for Sysex?
+ void FirmataClass::attachSysex(sysexFunction newFunction)
+ {
+ byte i;
+ byte tmpCount = analogReceiveFunctionCount;
+ analogReceiveFunction* tmpArray = analogReceiveFunctionArray;
+ analogReceiveFunctionCount++;
+ analogReceiveFunctionArray = (analogReceiveFunction*) calloc(analogReceiveFunctionCount, sizeof(analogReceiveFunction));
+ for(i = 0; i < tmpCount; i++) {
+ analogReceiveFunctionArray[i] = tmpArray[i];
+ }
+ analogReceiveFunctionArray[tmpCount] = newFunction;
+ free(tmpArray);
+ }
+*/
+
+//******************************************************************************
+//* Private Methods
+//******************************************************************************
+
+/**
+ * Resets the system state upon a SYSTEM_RESET message from the host software.
+ * @private
+ */
+void FirmataClass::systemReset(void)
+{
+  byte i;
+
+  waitForData = 0; // this flag says the next serial input will be data
+  executeMultiByteCommand = 0; // execute this after getting multi-byte data
+  multiByteChannel = 0; // channel data for multiByteCommands
+
+  for (i = 0; i < MAX_DATA_BYTES; i++) {
+    storedInputData[i] = 0;
+  }
+
+  parsingSysex = false;
+  sysexBytesRead = 0;
+
+  if (currentSystemResetCallback)
+    (*currentSystemResetCallback)();
+}
+
+/**
+ * Flashing the pin for the version number
+ * @private
+ * @param pin The pin the LED is attached to.
+ * @param count The number of times to flash the LED.
+ * @param onInterval The number of milliseconds for the LED to be ON during each interval.
+ * @param offInterval The number of milliseconds for the LED to be OFF during each interval.
+ */
+void FirmataClass::strobeBlinkPin(byte pin, int count, int onInterval, int offInterval)
+{
+  byte i;
+  for (i = 0; i < count; i++) {
+    delay(offInterval);
+    digitalWrite(pin, HIGH);
+    delay(onInterval);
+    digitalWrite(pin, LOW);
+  }
+}
+
+// make one instance for the user to use
+FirmataClass Firmata;
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.h	(revision 224)
@@ -0,0 +1,224 @@
+/*
+  Firmata.h - Firmata library v2.5.2 - 2016-2-15
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2015 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+#ifndef Firmata_h
+#define Firmata_h
+
+#include "Boards.h"  /* Hardware Abstraction Layer + Wiring/Arduino */
+
+/* Version numbers for the protocol.  The protocol is still changing, so these
+ * version numbers are important.
+ * Query using the REPORT_VERSION message.
+ */
+#define FIRMATA_PROTOCOL_MAJOR_VERSION  2 // for non-compatible changes
+#define FIRMATA_PROTOCOL_MINOR_VERSION  5 // for backwards compatible changes
+#define FIRMATA_PROTOCOL_BUGFIX_VERSION 1 // for bugfix releases
+
+/* Version numbers for the Firmata library.
+ * The firmware version will not always equal the protocol version going forward.
+ * Query using the REPORT_FIRMWARE message.
+ */
+#define FIRMATA_FIRMWARE_MAJOR_VERSION  2
+#define FIRMATA_FIRMWARE_MINOR_VERSION  5
+#define FIRMATA_FIRMWARE_BUGFIX_VERSION 2
+
+/* DEPRECATED as of Firmata v2.5.1. As of 2.5.1 there are separate version numbers for
+ * the protocol version and the firmware version.
+ */
+#define FIRMATA_MAJOR_VERSION           2 // same as FIRMATA_PROTOCOL_MAJOR_VERSION
+#define FIRMATA_MINOR_VERSION           5 // same as FIRMATA_PROTOCOL_MINOR_VERSION
+#define FIRMATA_BUGFIX_VERSION          1 // same as FIRMATA_PROTOCOL_BUGFIX_VERSION
+
+#define MAX_DATA_BYTES                  64 // max number of data bytes in incoming messages
+
+// Arduino 101 also defines SET_PIN_MODE as a macro in scss_registers.h
+#ifdef SET_PIN_MODE
+#undef SET_PIN_MODE
+#endif
+
+// message command bytes (128-255/0x80-0xFF)
+#define DIGITAL_MESSAGE         0x90 // send data for a digital port (collection of 8 pins)
+#define ANALOG_MESSAGE          0xE0 // send data for an analog pin (or PWM)
+#define REPORT_ANALOG           0xC0 // enable analog input by pin #
+#define REPORT_DIGITAL          0xD0 // enable digital input by port pair
+//
+#define SET_PIN_MODE            0xF4 // set a pin to INPUT/OUTPUT/PWM/etc
+#define SET_DIGITAL_PIN_VALUE   0xF5 // set value of an individual digital pin
+//
+#define REPORT_VERSION          0xF9 // report protocol version
+#define SYSTEM_RESET            0xFF // reset from MIDI
+//
+#define START_SYSEX             0xF0 // start a MIDI Sysex message
+#define END_SYSEX               0xF7 // end a MIDI Sysex message
+
+// extended command set using sysex (0-127/0x00-0x7F)
+/* 0x00-0x0F reserved for user-defined commands */
+#define SERIAL_MESSAGE          0x60 // communicate with serial devices, including other boards
+#define ENCODER_DATA            0x61 // reply with encoders current positions
+#define SERVO_CONFIG            0x70 // set max angle, minPulse, maxPulse, freq
+#define STRING_DATA             0x71 // a string message with 14-bits per char
+#define STEPPER_DATA            0x72 // control a stepper motor
+#define ONEWIRE_DATA            0x73 // send an OneWire read/write/reset/select/skip/search request
+#define SHIFT_DATA              0x75 // a bitstream to/from a shift register
+#define I2C_REQUEST             0x76 // send an I2C read/write request
+#define I2C_REPLY               0x77 // a reply to an I2C read request
+#define I2C_CONFIG              0x78 // config I2C settings such as delay times and power pins
+#define EXTENDED_ANALOG         0x6F // analog write (PWM, Servo, etc) to any pin
+#define PIN_STATE_QUERY         0x6D // ask for a pin's current mode and value
+#define PIN_STATE_RESPONSE      0x6E // reply with pin's current mode and value
+#define CAPABILITY_QUERY        0x6B // ask for supported modes and resolution of all pins
+#define CAPABILITY_RESPONSE     0x6C // reply with supported modes and resolution
+#define ANALOG_MAPPING_QUERY    0x69 // ask for mapping of analog to pin numbers
+#define ANALOG_MAPPING_RESPONSE 0x6A // reply with mapping info
+#define REPORT_FIRMWARE         0x79 // report name and version of the firmware
+#define SAMPLING_INTERVAL       0x7A // set the poll rate of the main loop
+#define SCHEDULER_DATA          0x7B // send a createtask/deletetask/addtotask/schedule/querytasks/querytask request to the scheduler
+#define SYSEX_NON_REALTIME      0x7E // MIDI Reserved for non-realtime messages
+#define SYSEX_REALTIME          0x7F // MIDI Reserved for realtime messages
+// these are DEPRECATED to make the naming more consistent
+#define FIRMATA_STRING          0x71 // same as STRING_DATA
+#define SYSEX_I2C_REQUEST       0x76 // same as I2C_REQUEST
+#define SYSEX_I2C_REPLY         0x77 // same as I2C_REPLY
+#define SYSEX_SAMPLING_INTERVAL 0x7A // same as SAMPLING_INTERVAL
+
+// pin modes
+//#define INPUT                 0x00 // defined in Arduino.h
+//#define OUTPUT                0x01 // defined in Arduino.h
+#define PIN_MODE_ANALOG         0x02 // analog pin in analogInput mode
+#define PIN_MODE_PWM            0x03 // digital pin in PWM output mode
+#define PIN_MODE_SERVO          0x04 // digital pin in Servo output mode
+#define PIN_MODE_SHIFT          0x05 // shiftIn/shiftOut mode
+#define PIN_MODE_I2C            0x06 // pin included in I2C setup
+#define PIN_MODE_ONEWIRE        0x07 // pin configured for 1-wire
+#define PIN_MODE_STEPPER        0x08 // pin configured for stepper motor
+#define PIN_MODE_ENCODER        0x09 // pin configured for rotary encoders
+#define PIN_MODE_SERIAL         0x0A // pin configured for serial communication
+#define PIN_MODE_PULLUP         0x0B // enable internal pull-up resistor for pin
+#define PIN_MODE_IGNORE         0x7F // pin configured to be ignored by digitalWrite and capabilityResponse
+#define TOTAL_PIN_MODES         13
+// DEPRECATED as of Firmata v2.5
+#define ANALOG                  0x02 // same as PIN_MODE_ANALOG
+#define PWM                     0x03 // same as PIN_MODE_PWM
+#define SERVO                   0x04 // same as PIN_MODE_SERVO
+#define SHIFT                   0x05 // same as PIN_MODE_SHIFT
+#define I2C                     0x06 // same as PIN_MODE_I2C
+#define ONEWIRE                 0x07 // same as PIN_MODE_ONEWIRE
+#define STEPPER                 0x08 // same as PIN_MODE_STEPPER
+#define ENCODER                 0x09 // same as PIN_MODE_ENCODER
+#define IGNORE                  0x7F // same as PIN_MODE_IGNORE
+
+extern "C" {
+  // callback function types
+  typedef void (*callbackFunction)(byte, int);
+  typedef void (*systemResetCallbackFunction)(void);
+  typedef void (*stringCallbackFunction)(char *);
+  typedef void (*sysexCallbackFunction)(byte command, byte argc, byte *argv);
+}
+
+// TODO make it a subclass of a generic Serial/Stream base class
+class FirmataClass
+{
+  public:
+    FirmataClass();
+    /* Arduino constructors */
+    void begin();
+    void begin(long);
+    void begin(Stream &s);
+    /* querying functions */
+    void printVersion(void);
+    void blinkVersion(void);
+    void printFirmwareVersion(void);
+    //void setFirmwareVersion(byte major, byte minor);  // see macro below
+    void setFirmwareNameAndVersion(const char *name, byte major, byte minor);
+    void disableBlinkVersion();
+    /* serial receive handling */
+    int available(void);
+    void processInput(void);
+    void parse(unsigned char value);
+    boolean isParsingMessage(void);
+    /* serial send handling */
+    void sendAnalog(byte pin, int value);
+    void sendDigital(byte pin, int value); // TODO implement this
+    void sendDigitalPort(byte portNumber, int portData);
+    void sendString(const char *string);
+    void sendString(byte command, const char *string);
+    void sendSysex(byte command, byte bytec, byte *bytev);
+    void write(byte c);
+    /* attach & detach callback functions to messages */
+    void attach(byte command, callbackFunction newFunction);
+    void attach(byte command, systemResetCallbackFunction newFunction);
+    void attach(byte command, stringCallbackFunction newFunction);
+    void attach(byte command, sysexCallbackFunction newFunction);
+    void detach(byte command);
+
+    /* access pin state and config */
+    byte getPinMode(byte pin);
+    void setPinMode(byte pin, byte config);
+    /* access pin state */
+    int getPinState(byte pin);
+    void setPinState(byte pin, int state);
+
+    /* utility methods */
+    void sendValueAsTwo7bitBytes(int value);
+    void startSysex(void);
+    void endSysex(void);
+
+  private:
+    Stream *FirmataStream;
+    /* firmware name and version */
+    byte firmwareVersionCount;
+    byte *firmwareVersionVector;
+    /* input message handling */
+    byte waitForData; // this flag says the next serial input will be data
+    byte executeMultiByteCommand; // execute this after getting multi-byte data
+    byte multiByteChannel; // channel data for multiByteCommands
+    byte storedInputData[MAX_DATA_BYTES]; // multi-byte data
+    /* sysex */
+    boolean parsingSysex;
+    int sysexBytesRead;
+    /* pin configuration */
+    byte pinConfig[TOTAL_PINS];
+    int pinState[TOTAL_PINS];
+
+    /* callback functions */
+    callbackFunction currentAnalogCallback;
+    callbackFunction currentDigitalCallback;
+    callbackFunction currentReportAnalogCallback;
+    callbackFunction currentReportDigitalCallback;
+    callbackFunction currentPinModeCallback;
+    callbackFunction currentPinValueCallback;
+    systemResetCallbackFunction currentSystemResetCallback;
+    stringCallbackFunction currentStringCallback;
+    sysexCallbackFunction currentSysexCallback;
+
+    boolean blinkVersionDisabled = false;
+
+    /* private methods ------------------------------ */
+    void processSysexMessage(void);
+    void systemReset(void);
+    void strobeBlinkPin(byte pin, int count, int onInterval, int offInterval);
+};
+
+extern FirmataClass Firmata;
+
+/*==============================================================================
+ * MACROS
+ *============================================================================*/
+
+/* shortcut for setFirmwareNameAndVersion() that uses __FILE__ to set the
+ * firmware name.  It needs to be a macro so that __FILE__ is included in the
+ * firmware source file rather than the library source file.
+ */
+#define setFirmwareVersion(x, y)   setFirmwareNameAndVersion(__FILE__, x, y)
+
+#endif /* Firmata_h */
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AllInputsFirmata/AllInputsFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AllInputsFirmata/AllInputsFirmata.ino	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AllInputsFirmata/AllInputsFirmata.ino	(revision 224)
@@ -23,5 +23,5 @@
 int previousAnalogValues[TOTAL_ANALOG_PINS];
 
-byte portStatus[TOTAL_PORTS];	// each bit: 1=pin is digital input, 0=other/ignore
+byte portStatus[TOTAL_PORTS]; // each bit: 1=pin is digital input, 0=other/ignore
 byte previousPINs[TOTAL_PORTS];
 
@@ -46,5 +46,5 @@
   byte i, port, status;
 
-  Firmata.setFirmwareVersion(0, 1);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
 
   for (pin = 0; pin < TOTAL_PINS; pin++) {
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.ino	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.ino	(revision 224)
@@ -54,5 +54,5 @@
 {
   if (value == 0) {
-    analogInputsToReport = analogInputsToReport &~ (1 << pin);
+    analogInputsToReport = analogInputsToReport & ~ (1 << pin);
   }
   else { // everything but 0 enables reporting of that pin
@@ -67,5 +67,5 @@
 void setup()
 {
-  Firmata.setFirmwareVersion(0, 2);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
   Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
   Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/EchoString/EchoString.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/EchoString/EchoString.ino	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/EchoString/EchoString.ino	(revision 224)
@@ -22,5 +22,5 @@
 
 
-void sysexCallback(byte command, byte argc, byte*argv)
+void sysexCallback(byte command, byte argc, byte *argv)
 {
   Firmata.sendSysex(command, argc, argv);
@@ -29,5 +29,5 @@
 void setup()
 {
-  Firmata.setFirmwareVersion(0, 1);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
   Firmata.attach(STRING_DATA, stringCallback);
   Firmata.attach(START_SYSEX, sysexCallback);
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/OldStandardFirmata/OldStandardFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/OldStandardFirmata/OldStandardFirmata.ino	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/OldStandardFirmata/OldStandardFirmata.ino	(revision 224)
@@ -56,5 +56,5 @@
 void outputPort(byte portNumber, byte portValue)
 {
-  portValue = portValue &~ portStatus[portNumber];
+  portValue = portValue & ~ portStatus[portNumber];
   if (previousPINs[portNumber] != portValue) {
     Firmata.sendDigitalPort(portNumber, portValue);
@@ -73,5 +73,5 @@
     if (reportPINs[i]) {
       switch (i) {
-        case 0: outputPort(0, PIND &~ B00000011); break; // ignore Rx/Tx 0/1
+        case 0: outputPort(0, PIND & ~ B00000011); break; // ignore Rx/Tx 0/1
         case 1: outputPort(1, PINB); break;
         case 2: outputPort(2, PINC); break;
@@ -105,5 +105,5 @@
       case INPUT:
         pinMode(pin, INPUT);
-        portStatus[port] = portStatus[port] &~ (1 << (pin - offset));
+        portStatus[port] = portStatus[port] & ~ (1 << (pin - offset));
         break;
       case OUTPUT:
@@ -113,5 +113,5 @@
         portStatus[port] = portStatus[port] | (1 << (pin - offset));
         break;
-        //case ANALOG: // TODO figure this out
+      //case ANALOG: // TODO figure this out
       default:
         Firmata.sendString("");
@@ -123,5 +123,5 @@
 void analogWriteCallback(byte pin, int value)
 {
-  setPinModeCallback(pin, PWM);
+  setPinModeCallback(pin, PIN_MODE_PWM);
   analogWrite(pin, value);
 }
@@ -132,5 +132,5 @@
     case 0: // pins 2-7 (don't change Rx/Tx, pins 0 and 1)
       // 0xFF03 == B1111111100000011    0x03 == B00000011
-      PORTD = (value &~ 0xFF03) | (PORTD & 0x03);
+      PORTD = (value & ~ 0xFF03) | (PORTD & 0x03);
       break;
     case 1: // pins 8-13 (14,15 are disabled for the crystal)
@@ -151,5 +151,5 @@
 {
   if (value == 0) {
-    analogInputsToReport = analogInputsToReport &~ (1 << pin);
+    analogInputsToReport = analogInputsToReport & ~ (1 << pin);
   }
   else { // everything but 0 enables reporting of that pin
@@ -203,5 +203,5 @@
    * host computer, since once in the loop(), this firmware will only send
    * digital data on change. */
-  if (reportPINs[0]) outputPort(0, PIND &~ B00000011); // ignore Rx/Tx 0/1
+  if (reportPINs[0]) outputPort(0, PIND & ~ B00000011); // ignore Rx/Tx 0/1
   if (reportPINs[1]) outputPort(1, PINB);
   if (reportPINs[2]) outputPort(2, PINC);
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/ServoFirmata/ServoFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/ServoFirmata/ServoFirmata.ino	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/ServoFirmata/ServoFirmata.ino	(revision 224)
@@ -13,6 +13,4 @@
  * included in Arduino 0017
  *
- * TODO add message to configure minPulse/maxPulse/degrees
- *
  * This example code is in the public domain.
  */
@@ -22,10 +20,17 @@
 
 Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte servoCount = 0;
 
 void analogWriteCallback(byte pin, int value)
 {
-  if (IS_PIN_SERVO(pin)) {
-    servos[PIN_TO_SERVO(pin)].write(value);
+  if (IS_PIN_DIGITAL(pin)) {
+    servos[servoPinMap[pin]].write(value);
   }
+}
+
+void systemResetCallback()
+{
+  servoCount = 0;
 }
 
@@ -34,14 +39,22 @@
   byte pin;
 
-  Firmata.setFirmwareVersion(0, 2);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
   Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
 
+  Firmata.begin(57600);
+  systemResetCallback();
+
+  // attach servos from first digital pin up to max number of
+  // servos supported for the board
   for (pin = 0; pin < TOTAL_PINS; pin++) {
-    if (IS_PIN_SERVO(pin)) {
-      servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin));
+    if (IS_PIN_DIGITAL(pin)) {
+      if (servoCount < MAX_SERVOS) {
+        servoPinMap[pin] = servoCount;
+        servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+        servoCount++;
+      }
     }
   }
-
-  Firmata.begin(57600);
 }
 
@@ -51,3 +64,2 @@
     Firmata.processInput();
 }
-
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.ino	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.ino	(revision 224)
@@ -28,5 +28,5 @@
 void setup()
 {
-  Firmata.setFirmwareVersion(0, 1);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
   Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
   Firmata.begin(57600);
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.ino	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.ino	(revision 224)
@@ -53,5 +53,5 @@
 void setup()
 {
-  Firmata.setFirmwareVersion(0, 1);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
   Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
   Firmata.attach(SET_PIN_MODE, setPinModeCallback);
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmata/StandardFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmata/StandardFirmata.ino	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmata/StandardFirmata.ino	(revision 224)
@@ -1,18 +1,16 @@
 /*
- * Firmata is a generic protocol for communicating with microcontrollers
- * from software on a host computer. It is intended to work with
- * any host computer software package.
- *
- * To download a host software package, please clink on the following link
- * to open the download page in your default browser.
- *
- * http://firmata.org/wiki/Download
- */
-
-/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
   Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
   Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
   Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
-  Copyright (C) 2009-2011 Jeff Hoefs.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
 
   This library is free software; you can redistribute it and/or
@@ -23,10 +21,6 @@
   See file LICENSE.txt for further informations on licensing terms.
 
-  formatted using the GNU C formatting and indenting
+  Last updated by Jeff Hoefs: January 10th, 2016
 */
-
-/*
- * TODO: use Program Control to load stored profiles from EEPROM
- */
 
 #include <Servo.h>
@@ -34,16 +28,19 @@
 #include <Firmata.h>
 
-// move the following defines to Firmata.h?
-#define I2C_WRITE B00000000
-#define I2C_READ B00001000
-#define I2C_READ_CONTINUOUSLY B00010000
-#define I2C_STOP_READING B00011000
-#define I2C_READ_WRITE_MODE_MASK B00011000
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
 #define I2C_10BIT_ADDRESS_MODE_MASK B00100000
-
-#define MAX_QUERIES 8
-#define MINIMUM_SAMPLING_INTERVAL 10
-
-#define REGISTER_NOT_SPECIFIED -1
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
 
 /*==============================================================================
@@ -51,4 +48,8 @@
  *============================================================================*/
 
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
 /* analog inputs */
 int analogInputsToReport = 0; // bitwise array to store pin reporting
@@ -59,45 +60,106 @@
 
 /* pins configuration */
-byte pinConfig[TOTAL_PINS];         // configuration of every pin
 byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
-int pinState[TOTAL_PINS];           // any value that has been written
 
 /* timer variables */
 unsigned long currentMillis;        // store the current value from millis()
 unsigned long previousMillis;       // for comparison with currentMillis
-int samplingInterval = 19;          // how often to run the main loop (in ms)
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
 
 /* i2c data */
 struct i2c_device_info {
   byte addr;
-  byte reg;
+  int reg;
   byte bytes;
+  byte stopTX;
 };
 
 /* for i2c read continuous more */
-i2c_device_info query[MAX_QUERIES];
-
-byte i2cRxData[32];
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
 boolean isI2CEnabled = false;
 signed char queryIndex = -1;
-unsigned int i2cReadDelayTime = 0;  // default delay time between i2c read request and Wire.requestFrom()
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
 
 Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
 /*==============================================================================
  * FUNCTIONS
  *============================================================================*/
 
-void readAndReportData(byte address, int theRegister, byte numBytes) {
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
   // allow I2C requests that don't require a register read
   // for example, some devices using an interrupt pin to signify new data available
   // do not always require the register read so upon interrupt you call Wire.requestFrom()
-  if (theRegister != REGISTER_NOT_SPECIFIED) {
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
     Wire.beginTransmission(address);
-#if ARDUINO >= 100
-    Wire.write((byte)theRegister);
-#else
-    Wire.send((byte)theRegister);
-#endif
-    Wire.endTransmission();
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
     // do not set a value of 0
     if (i2cReadDelayTime > 0) {
@@ -112,21 +174,15 @@
 
   // check to be sure correct number of bytes were returned by slave
-  if (numBytes == Wire.available()) {
-    i2cRxData[0] = address;
-    i2cRxData[1] = theRegister;
-    for (int i = 0; i < numBytes; i++) {
-#if ARDUINO >= 100
-      i2cRxData[2 + i] = Wire.read();
-#else
-      i2cRxData[2 + i] = Wire.receive();
-#endif
-    }
-  }
-  else {
-    if (numBytes > Wire.available()) {
-      Firmata.sendString("I2C Read Error: Too many bytes received");
-    } else {
-      Firmata.sendString("I2C Read Error: Too few bytes received");
-    }
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
   }
 
@@ -173,4 +229,37 @@
 
 // -----------------------------------------------------------------------------
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+// -----------------------------------------------------------------------------
 /* sets the pin mode to the correct state and sets the relevant bits in the
  * two bit-arrays that track Digital I/O and PWM status
@@ -178,17 +267,22 @@
 void setPinModeCallback(byte pin, int mode)
 {
-  if (pinConfig[pin] == I2C && isI2CEnabled && mode != I2C) {
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
     // disable i2c so pins can be used for other functions
     // the following if statements should reconfigure the pins properly
     disableI2CPins();
   }
-  if (IS_PIN_SERVO(pin) && mode != SERVO && servos[PIN_TO_SERVO(pin)].attached()) {
-    servos[PIN_TO_SERVO(pin)].detach();
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
   }
   if (IS_PIN_ANALOG(pin)) {
-    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == ANALOG ? 1 : 0); // turn on/off reporting
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
   }
   if (IS_PIN_DIGITAL(pin)) {
-    if (mode == INPUT) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
       portConfigInputs[pin / 8] |= (1 << (pin & 7));
     } else {
@@ -196,20 +290,33 @@
     }
   }
-  pinState[pin] = 0;
+  Firmata.setPinState(pin, 0);
   switch (mode) {
-    case ANALOG:
+    case PIN_MODE_ANALOG:
       if (IS_PIN_ANALOG(pin)) {
         if (IS_PIN_DIGITAL(pin)) {
-          pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
           digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
-        }
-        pinConfig[pin] = ANALOG;
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
       }
       break;
     case INPUT:
       if (IS_PIN_DIGITAL(pin)) {
-        pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
         digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
-        pinConfig[pin] = INPUT;
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
       }
       break;
@@ -218,28 +325,35 @@
         digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
         pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
-        pinConfig[pin] = OUTPUT;
-      }
-      break;
-    case PWM:
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
       if (IS_PIN_PWM(pin)) {
         pinMode(PIN_TO_PWM(pin), OUTPUT);
         analogWrite(PIN_TO_PWM(pin), 0);
-        pinConfig[pin] = PWM;
-      }
-      break;
-    case SERVO:
-      if (IS_PIN_SERVO(pin)) {
-        pinConfig[pin] = SERVO;
-        if (!servos[PIN_TO_SERVO(pin)].attached()) {
-          servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin));
-        }
-      }
-      break;
-    case I2C:
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
       if (IS_PIN_I2C(pin)) {
         // mark the pin as i2c
         // the user must call I2C_CONFIG to enable I2C for a device
-        pinConfig[pin] = I2C;
-      }
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
       break;
     default:
@@ -249,17 +363,33 @@
 }
 
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
 void analogWriteCallback(byte pin, int value)
 {
   if (pin < TOTAL_PINS) {
-    switch (pinConfig[pin]) {
-      case SERVO:
-        if (IS_PIN_SERVO(pin))
-          servos[PIN_TO_SERVO(pin)].write(value);
-        pinState[pin] = value;
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
         break;
-      case PWM:
+      case PIN_MODE_PWM:
         if (IS_PIN_PWM(pin))
           analogWrite(PIN_TO_PWM(pin), value);
-        pinState[pin] = value;
+        Firmata.setPinState(pin, value);
         break;
     }
@@ -269,5 +399,5 @@
 void digitalWriteCallback(byte port, int value)
 {
-  byte pin, lastPin, mask = 1, pinWriteMask = 0;
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
 
   if (port < TOTAL_PORTS) {
@@ -278,9 +408,19 @@
       // do not disturb non-digital pins (eg, Rx & Tx)
       if (IS_PIN_DIGITAL(pin)) {
-        // only write to OUTPUT and INPUT (enables pullup)
         // do not touch pins in PWM, ANALOG, SERVO or other modes
-        if (pinConfig[pin] == OUTPUT || pinConfig[pin] == INPUT) {
-          pinWriteMask |= mask;
-          pinState[pin] = ((byte)value & mask) ? 1 : 0;
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
         }
       }
@@ -293,19 +433,4 @@
 
 // -----------------------------------------------------------------------------
-/* sets bits in a bit array (int) to toggle the reporting of the analogIns
- */
-//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
-//}
-void reportAnalogCallback(byte analogPin, int value)
-{
-  if (analogPin < TOTAL_ANALOG_PINS) {
-    if (value == 0) {
-      analogInputsToReport = analogInputsToReport &~ (1 << analogPin);
-    } else {
-      analogInputsToReport = analogInputsToReport | (1 << analogPin);
-    }
-  }
-  // TODO: save status to EEPROM here, if changed
-}
 
 void reportDigitalCallback(byte port, int value)
@@ -313,4 +438,8 @@
   if (port < TOTAL_PORTS) {
     reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
   }
   // do not disable analog reporting on these 8 pins, to allow some
@@ -322,4 +451,23 @@
 }
 
+// -----------------------------------------------------------------------------
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
 /*==============================================================================
  * SYSEX-BASED commands
@@ -329,7 +477,8 @@
 {
   byte mode;
+  byte stopTX;
   byte slaveAddress;
-  byte slaveRegister;
   byte data;
+  int slaveRegister;
   unsigned int delayTime;
 
@@ -338,9 +487,18 @@
       mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
       if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
-        Firmata.sendString("10-bit addressing mode is not yet supported");
+        Firmata.sendString("10-bit addressing not supported");
         return;
       }
       else {
         slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
       }
 
@@ -350,9 +508,5 @@
           for (byte i = 2; i < argc; i += 2) {
             data = argv[i] + (argv[i + 1] << 7);
-#if ARDUINO >= 100
-            Wire.write(data);
-#else
-            Wire.send(data);
-#endif
+            wireWrite(data);
           }
           Wire.endTransmission();
@@ -364,22 +518,33 @@
             slaveRegister = argv[2] + (argv[3] << 7);
             data = argv[4] + (argv[5] << 7);  // bytes to read
-            readAndReportData(slaveAddress, (int)slaveRegister, data);
           }
           else {
             // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
             data = argv[2] + (argv[3] << 7);  // bytes to read
-            readAndReportData(slaveAddress, (int)REGISTER_NOT_SPECIFIED, data);
           }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
           break;
         case I2C_READ_CONTINUOUSLY:
-          if ((queryIndex + 1) >= MAX_QUERIES) {
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
             // too many queries, just ignore
             Firmata.sendString("too many queries");
             break;
           }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
           queryIndex++;
           query[queryIndex].addr = slaveAddress;
-          query[queryIndex].reg = argv[2] + (argv[3] << 7);
-          query[queryIndex].bytes = argv[4] + (argv[5] << 7);
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
           break;
         case I2C_STOP_READING:
@@ -390,9 +555,10 @@
             queryIndex = -1;
           } else {
+            queryIndexToSkip = 0;
             // if read continuous mode is enabled for multiple devices,
             // determine which device to stop reading and remove it's data from
             // the array, shifiting other array data to fill the space
             for (byte i = 0; i < queryIndex + 1; i++) {
-              if (query[i].addr = slaveAddress) {
+              if (query[i].addr == slaveAddress) {
                 queryIndexToSkip = i;
                 break;
@@ -401,8 +567,9 @@
 
             for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
-              if (i < MAX_QUERIES) {
+              if (i < I2C_MAX_QUERIES) {
                 query[i].addr = query[i + 1].addr;
-                query[i].reg = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
                 query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
               }
             }
@@ -433,9 +600,10 @@
         int maxPulse = argv[3] + (argv[4] << 7);
 
-        if (IS_PIN_SERVO(pin)) {
-          if (servos[PIN_TO_SERVO(pin)].attached())
-            servos[PIN_TO_SERVO(pin)].detach();
-          servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
-          setPinModeCallback(pin, SERVO);
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
         }
       }
@@ -466,23 +634,28 @@
           Firmata.write((byte)INPUT);
           Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
           Firmata.write((byte)OUTPUT);
           Firmata.write(1);
         }
         if (IS_PIN_ANALOG(pin)) {
-          Firmata.write(ANALOG);
-          Firmata.write(10);
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
         }
         if (IS_PIN_PWM(pin)) {
-          Firmata.write(PWM);
-          Firmata.write(8);
-        }
-        if (IS_PIN_SERVO(pin)) {
-          Firmata.write(SERVO);
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
           Firmata.write(14);
         }
         if (IS_PIN_I2C(pin)) {
-          Firmata.write(I2C);
-          Firmata.write(1);  // to do: determine appropriate value
-        }
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
         Firmata.write(127);
       }
@@ -496,8 +669,8 @@
         Firmata.write(pin);
         if (pin < TOTAL_PINS) {
-          Firmata.write((byte)pinConfig[pin]);
-          Firmata.write((byte)pinState[pin] & 0x7F);
-          if (pinState[pin] & 0xFF80) Firmata.write((byte)(pinState[pin] >> 7) & 0x7F);
-          if (pinState[pin] & 0xC000) Firmata.write((byte)(pinState[pin] >> 14) & 0x7F);
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
         }
         Firmata.write(END_SYSEX);
@@ -512,33 +685,13 @@
       Firmata.write(END_SYSEX);
       break;
-  }
-}
-
-void enableI2CPins()
-{
-  byte i;
-  // is there a faster way to do this? would probaby require importing
-  // Arduino.h to get SCL and SDA pins
-  for (i = 0; i < TOTAL_PINS; i++) {
-    if (IS_PIN_I2C(i)) {
-      // mark pins as i2c so they are ignore in non i2c data requests
-      setPinModeCallback(i, I2C);
-    }
-  }
-
-  isI2CEnabled = true;
-
-  // is there enough time before the first I2C request to call this here?
-  Wire.begin();
-}
-
-/* disable the i2c pins so they can be used for other functions */
-void disableI2CPins() {
-  isI2CEnabled = false;
-  // disable read continuous mode for all devices
-  queryIndex = -1;
-  // uncomment the following if or when the end() method is added to Wire library
-  // Wire.end();
-}
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
 
 /*==============================================================================
@@ -548,27 +701,41 @@
 void systemResetCallback()
 {
+  isResetting = true;
+
   // initialize a defalt state
   // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
   if (isI2CEnabled) {
     disableI2CPins();
   }
+
   for (byte i = 0; i < TOTAL_PORTS; i++) {
-    reportPINs[i] = false;      // by default, reporting off
-    portConfigInputs[i] = 0;	// until activated
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
     previousPINs[i] = 0;
   }
-  // pins with analog capability default to analog input
-  // otherwise, pins default to digital output
+
   for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
     if (IS_PIN_ANALOG(i)) {
       // turns off pullup, configures everything
-      setPinModeCallback(i, ANALOG);
-    } else {
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
       // sets the output to 0, configures portConfigInputs
       setPinModeCallback(i, OUTPUT);
     }
+
+    servoPinMap[i] = 255;
   }
   // by default, do not report any analog inputs
   analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
 
   /* send digital inputs to set the initial state on the host computer,
@@ -581,9 +748,10 @@
   }
   */
+  isResetting = false;
 }
 
 void setup()
 {
-  Firmata.setFirmwareVersion(FIRMATA_MAJOR_VERSION, FIRMATA_MINOR_VERSION);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
 
   Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
@@ -592,8 +760,19 @@
   Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
   Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
   Firmata.attach(START_SYSEX, sysexCallback);
   Firmata.attach(SYSTEM_RESET, systemResetCallback);
 
+  // to use a port other than Serial, such as Serial1 on an Arduino Leonardo or Mega,
+  // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
+  // Serial1.begin(57600);
+  // Firmata.begin(Serial1);
+  // However do not do this if you are using SERIAL_MESSAGE
+
   Firmata.begin(57600);
+  while (!Serial) {
+    ; // wait for serial port to connect. Needed for ATmega32u4-based boards and Arduino 101
+  }
+
   systemResetCallback();  // reset to default config
 }
@@ -610,12 +789,10 @@
   checkDigitalInputs();
 
-  /* SERIALREAD - processing incoming messagse as soon as possible, while still
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
    * checking digital inputs.  */
   while (Firmata.available())
     Firmata.processInput();
 
-  /* SEND FTDI WRITE BUFFER - make sure that the FTDI buffer doesn't go over
-   * 60 bytes. use a timer to sending an event character every 4 ms to
-   * trigger the buffer to dump. */
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
 
   currentMillis = millis();
@@ -624,5 +801,5 @@
     /* ANALOGREAD - do all analogReads() at the configured sampling interval */
     for (pin = 0; pin < TOTAL_PINS; pin++) {
-      if (IS_PIN_ANALOG(pin) && pinConfig[pin] == ANALOG) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
         analogPin = PIN_TO_ANALOG(pin);
         if (analogInputsToReport & (1 << analogPin)) {
@@ -634,7 +811,11 @@
     if (queryIndex > -1) {
       for (byte i = 0; i < queryIndex + 1; i++) {
-        readAndReportData(query[i].addr, query[i].reg, query[i].bytes);
-      }
-    }
-  }
-}
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/StandardFirmataBLE.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/StandardFirmataBLE.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/StandardFirmataBLE.ino	(revision 224)
@@ -0,0 +1,850 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated March 13th, 2016
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+//#define SERIAL_DEBUG
+#include "utility/firmataDebug.h"
+
+/*
+ * Uncomment the following include to enable interfacing
+ * with Serial devices via hardware or software serial.
+ */
+//#include "utility/SerialFirmata.h"
+
+// follow the instructions in bleConfig.h to configure your BLE hardware
+#include "bleConfig.h"
+
+#define I2C_WRITE                   0x00 //B00000000
+#define I2C_READ                    0x08 //B00001000
+#define I2C_READ_CONTINUOUSLY       0x10 //B00010000
+#define I2C_STOP_READING            0x18 //B00011000
+#define I2C_READ_WRITE_MODE_MASK    0x18 //B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK 0x20 //B00100000
+#define I2C_END_TX_MASK             0x40 //B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+// min cannot be < 0x0006. Adjust max if necessary
+#define FIRMATA_BLE_MIN_INTERVAL    0x0006 // 7.5ms (7.5 / 1.25)
+#define FIRMATA_BLE_MAX_INTERVAL    0x0018 // 30ms (30 / 1.25)
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0; // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous more */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Serial output queue using Serial.print() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+// -----------------------------------------------------------------------------
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  isResetting = false;
+}
+
+void setup()
+{
+  DEBUG_BEGIN(9600);
+
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  stream.setLocalName(FIRMATA_BLE_LOCAL_NAME);
+
+// setConnectionInterval is not available in the CurieBLE library included in the
+// Intel Curie Boards package v1.0.5 (latest version via the Boards Manager). Without
+// setConnectionInterval, the BLE reporting rate for analog input and I2C read continuous mode
+// will be very slow (150ms instead of 30ms).
+// However, you can manually update CurieBLE to get the functionality now. Follow these steps:
+// 1. Install Intel Curie Boards v1.0.5 via the Arduino Boards manager (use Arduino 1.6.7 or newer)
+// 2. Download or clone corelibs-arduino101: https://github.com/01org/corelibs-arduino101
+// 3. Make a copy of the CurieBLE directory found in corelibs-arduino101/libraries/
+// 4. Find the Arduino15 directory on your computer:
+//    OS X:    ~/Library/Arduino15
+//    Windows: C:\Users\(username)\AppData\Local\Arduino15
+//    Linux:   ~/.arduino15
+// 5. From the Arduino15 directory, navigate to: /packages/Intel/hardware/arc32/1.0.5/libraries/
+// 6. Replace the CurieBLE library with the version you copied in step 3
+// 7. Comment out the #ifndef statement below and the following #endif statement
+#ifndef _VARIANT_ARDUINO_101_X_
+  // set the BLE connection interval - this is the fastest interval you can read inputs
+  stream.setConnectionInterval(FIRMATA_BLE_MIN_INTERVAL, FIRMATA_BLE_MAX_INTERVAL);
+  // set how often the BLE TX buffer is flushed (if not full)
+  stream.setFlushInterval(FIRMATA_BLE_MAX_INTERVAL);
+#endif
+
+#ifdef BLE_REQ
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    if (IS_IGNORE_BLE_PINS(i)) {
+      Firmata.setPinMode(i, PIN_MODE_IGNORE);
+    }
+  }
+#endif
+
+  stream.begin();
+  Firmata.begin(stream);
+
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  // do not process data if no BLE connection is established
+  // poll will send the TX buffer at the specified flush interval or when the buffer is full
+  if (!stream.poll()) return;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * Stream buffer using Stream.write()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis = currentMillis;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/bleConfig.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/bleConfig.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/bleConfig.h	(revision 224)
@@ -0,0 +1,106 @@
+/*==================================================================================================
+ * BLE CONFIGURATION
+ *
+ * If you are using an Arduino 101, you do not need to make any changes to this file (unless you
+ * need a unique ble local name (see below). If you are using another supported BLE board or shield,
+ * follow the instructions for the specific board or shield below.
+ *
+ * Supported boards and shields:
+ * - Arduino 101 (recommended)
+ * - RedBearLab BLE Shield (v2)  ** to be verified **
+ * - RedBearLab BLE Nano ** works with modifications **
+ *
+ *================================================================================================*/
+
+// change this to a unique name per board if running StandardFirmataBLE on multiple boards
+// within the same physical space
+#define FIRMATA_BLE_LOCAL_NAME "FIRMATA"
+
+/*
+ * RedBearLab BLE Shield
+ *
+ * If you are using a RedBearLab BLE shield, uncomment the define below.
+ * Also, change the define for BLE_RST if you have the jumper set to pin 7 rather than pin 4.
+ *
+ * You will need to use the shield with an Arduino Zero, Due, Mega, or other board with sufficient
+ * Flash and RAM. Arduino Uno, Leonardo and other ATmega328p and Atmega32u4 boards to not have
+ * enough memory to run StandardFirmataBLE.
+ *
+ * TODO: verify if this works and with which boards it works.
+ *
+ * Test script: https://gist.github.com/soundanalogous/927360b797574ed50e27
+ */
+//#define REDBEAR_BLE_SHIELD
+
+#ifdef REDBEAR_BLE_SHIELD
+#include <SPI.h>
+#include <BLEPeripheral.h>
+#include "utility/BLEStream.h"
+
+#define BLE_REQ  9
+#define BLE_RDY  8
+#define BLE_RST  4 // 4 or 7 via jumper on shield
+
+BLEStream stream(BLE_REQ, BLE_RDY, BLE_RST);
+#endif
+
+
+/*==================================================================================================
+ * END BLE CONFIGURATION - you should not need to change anything below this line
+ *================================================================================================*/
+
+/*
+ * Arduino 101
+ *
+ * Test script: https://gist.github.com/soundanalogous/927360b797574ed50e27
+ */
+#ifdef _VARIANT_ARDUINO_101_X_
+#include <CurieBLE.h>
+#include "utility/BLEStream.h"
+BLEStream stream;
+#endif
+
+
+/*
+ * RedBearLab BLE Nano (with default switch settings)
+ *
+ * Blocked on this issue: https://github.com/RedBearLab/nRF51822-Arduino/issues/46
+ * Works with modifications. See comments at top of the test script referenced below.
+ * When the RBL nRF51822-Arduino library issue is resolved, this should work witout
+ * any modifications.
+ *
+ * Test script: https://gist.github.com/soundanalogous/d39bb3eb36333a0906df
+ *
+ * Note: If you have changed the solder jumpers on the Nano you may encounter issues since
+ * the pins are currently mapped in Firmata only for the default (factory) jumper settings.
+ */
+// #ifdef BLE_NANO
+// #include <BLEPeripheral.h>
+// #include "utility/BLEStream.h"
+// BLEStream stream;
+// #endif
+
+
+/*
+ * RedBearLab Blend and Blend Micro
+ *
+ * StandardFirmataBLE requires too much Flash and RAM to run on the ATmega32u4-based Blend
+ * and Blend Micro boards. It may work with ConfigurableFirmata selecting only analog and/or
+ * digital I/O.
+ */
+// #if defined(BLEND_MICRO) || defined(BLEND)
+// #include <SPI.h>
+// #include <BLEPeripheral.h>
+// #include "utility/BLEStream.h"
+
+// #define BLE_REQ  6
+// #define BLE_RDY  7
+// #define BLE_RST  4
+
+// BLEStream stream(BLE_REQ, BLE_RDY, BLE_RST);
+// #endif
+
+
+#if defined(BLE_REQ) && defined(BLE_RDY) && defined(BLE_RST)
+#define IS_IGNORE_BLE_PINS(p) ((p) == BLE_REQ || (p) == BLE_RDY || (p) == BLE_RST)
+#endif
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/StandardFirmataChipKIT.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/StandardFirmataChipKIT.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/StandardFirmataChipKIT.ino	(revision 224)
@@ -0,0 +1,793 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+  Copyright (C) 2015 Brian Schmalz. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+#include <SoftPWMServo.h>  // Gives us PWM and Servo on every pin
+#include <Wire.h>
+#include <Firmata.h>
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+/* analog inputs */
+int analogInputsToReport = 0; // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous more */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+SoftServo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Serial output queue using Serial.print() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* Sets a pin that is in Servo mode to a particular output value
+ * (i.e. pulse width). Different boards may have different ways of
+ * setting servo values, so putting it in a function keeps things cleaner.
+ */
+void servoWrite(byte pin, int value)
+{
+  SoftPWMServoPWMWrite(PIN_TO_PWM(pin), value);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        servoWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          servoWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  /* For chipKIT Pi board, we need to use Serial1. All others just use Serial. */
+#if defined(_BOARD_CHIPKIT_PI_)
+  Serial1.begin(57600);
+  Firmata.begin(Serial1);
+#else
+  Firmata.begin(57600);
+#endif
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * FTDI buffer using Serial.print()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino	(revision 224)
@@ -0,0 +1,910 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+/*
+  README
+
+  StandardFirmataEthernet is a client implementation. You will need a Firmata client library with
+  a network transport that can act as a server in order to establish a connection between
+  StandardFirmataEthernet and the Firmata client application.
+
+  To use StandardFirmataEthernet you will need to have one of the following
+  boards or shields:
+
+  - Arduino Ethernet shield (or clone)
+  - Arduino Ethernet board (or clone)
+  - Arduino Yun
+
+  Follow the instructions in the ethernetConfig.h file (ethernetConfig.h tab in Arduino IDE) to
+  configure your particular hardware.
+
+  NOTE: If you are using an Arduino Ethernet shield you cannot use the following pins on
+  the following boards. Firmata will ignore any requests to use these pins:
+
+  - Arduino Uno or other ATMega328 boards: (D4, D10, D11, D12, D13)
+  - Arduino Mega: (D4, D10, D50, D51, D52, D53)
+  - Arduino Leonardo: (D4, D10)
+  - Arduino Due: (D4, D10)
+  - Arduino Zero: (D4, D10)
+
+  If you are using an ArduinoEthernet board, the following pins cannot be used (same as Uno):
+  - D4, D10, D11, D12, D13
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+/*
+ * Uncomment the #define SERIAL_DEBUG line below to receive serial output messages relating to your connection
+ * that may help in the event of connection issues. If defined, some boards may not begin executing this sketch
+ * until the Serial console is opened.
+ */
+//#define SERIAL_DEBUG
+#include "utility/firmataDebug.h"
+
+// follow the instructions in ethernetConfig.h to configure your particular hardware
+#include "ethernetConfig.h"
+#include "utility/EthernetClientStream.h"
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+/* network */
+#if defined remote_ip && !defined remote_host
+#ifdef local_ip
+EthernetClientStream stream(client, local_ip, remote_ip, NULL, remote_port);
+#else
+EthernetClientStream stream(client, IPAddress(0, 0, 0, 0), remote_ip, NULL, remote_port);
+#endif
+#endif
+
+#if !defined remote_ip && defined remote_host
+#ifdef local_ip
+EthernetClientStream stream(client, local_ip, IPAddress(0, 0, 0, 0), remote_host, remote_port);
+#else
+EthernetClientStream stream(client, IPAddress(0, 0, 0, 0), IPAddress(0, 0, 0, 0), remote_host, remote_port);
+#endif
+#endif
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0;      // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to sample analog inputs (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous mode */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Stream output queue using Stream.write() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  DEBUG_BEGIN(9600);
+
+#ifdef YUN_ETHERNET
+  Bridge.begin();
+#else
+#ifdef local_ip
+  Ethernet.begin((uint8_t *)mac, local_ip); //start ethernet
+#else
+  Ethernet.begin((uint8_t *)mac);           //start ethernet using dhcp
+#endif
+#endif
+
+  DEBUG_PRINTLN("connecting...");
+
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+#ifdef WIZ5100_ETHERNET
+  // StandardFirmataEthernet communicates with Ethernet shields over SPI. Therefore all
+  // SPI pins must be set to IGNORE. Otherwise Firmata would break SPI communication.
+  // add Pin 10 and configure pin 53 as output if using a MEGA with an Ethernet shield.
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    if (IS_IGNORE_ETHERNET_SHIELD(i)
+  #if defined(__AVR_ATmega32U4__)
+        || 24 == i // On Leonardo, pin 24 maps to D4 and pin 28 maps to D10
+        || 28 == i
+  #endif
+       ) {
+      Firmata.setPinMode(i, PIN_MODE_IGNORE);
+    }
+  }
+
+  // Arduino Ethernet and Arduino EthernetShield have SD SS wired to D4
+  pinMode(PIN_TO_DIGITAL(4), OUTPUT);    // switch off SD card bypassing Firmata
+  digitalWrite(PIN_TO_DIGITAL(4), HIGH); // SS is active low;
+#endif // WIZ5100_ETHERNET
+
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  pinMode(PIN_TO_DIGITAL(53), OUTPUT); // configure hardware SS as output on MEGA
+#endif
+
+  // start up Network Firmata:
+  Firmata.begin(stream);
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * Stream buffer using Stream.write()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+
+#if !defined local_ip && !defined YUN_ETHERNET
+  // only necessary when using DHCP, ensures local IP is updated appropriately if it changes
+  if (Ethernet.maintain()) {
+    stream.maintain(Ethernet.localIP());
+  }
+#endif
+
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/ethernetConfig.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/ethernetConfig.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/ethernetConfig.h	(revision 224)
@@ -0,0 +1,85 @@
+/*==============================================================================
+ * NETWORK CONFIGURATION
+ *
+ * You must configure your particular hardware. Follow the steps below.
+ *
+ * Currently StandardFirmataEthernet is configured as a client. An option to
+ * configure as a server may be added in the future.
+ *============================================================================*/
+
+// STEP 1 [REQUIRED]
+// Uncomment / comment the appropriate set of includes for your hardware (OPTION A or B)
+// Option A is enabled by default.
+
+/*
+ * OPTION A: Configure for Arduino Ethernet board or Arduino Ethernet shield (or clone)
+ *
+ * To configure StandardFirmataEthernet to use the original WIZ5100-based
+ * ethernet shield or Arduino Ethernet uncomment the WIZ5100_ETHERNET define below
+ */
+#define WIZ5100_ETHERNET
+
+#ifdef WIZ5100_ETHERNET
+#include <SPI.h>
+#include <Ethernet.h>
+EthernetClient client;
+#endif
+
+/*
+ * OPTION B: Configure for Arduin Yun
+ *
+ * The Ethernet port on the Arduino Yun board can be used with Firmata in this configuration.
+ *
+ * To execute StandardFirmataEthernet on Yun uncomment the YUN_ETHERNET define below and make
+ * sure the WIZ5100_ETHERNET define (above) is commented out.
+ *
+ * On Yun there's no need to configure local_ip and mac address as this is automatically
+ * configured on the linux-side of Yun.
+ */
+//#define YUN_ETHERNET
+
+#ifdef YUN_ETHERNET
+#include <Bridge.h>
+#include <YunClient.h>
+YunClient client;
+#endif
+
+
+// STEP 2 [REQUIRED for all boards and shields]
+// replace with IP of the server you want to connect to, comment out if using 'remote_host'
+#define remote_ip IPAddress(10, 0, 0, 3)
+// *** REMOTE HOST IS NOT YET WORKING ***
+// replace with hostname of server you want to connect to, comment out if using 'remote_ip'
+// #define remote_host "server.local"
+
+// STEP 3 [REQUIRED unless using Arduin Yun]
+// Replace with the port that your server is listening on
+#define remote_port 3030
+
+// STEP 4 [REQUIRED unless using Arduino Yun OR if not using DHCP]
+// Replace with your board or ethernet shield's IP address
+// Comment out if you want to use DHCP
+#define local_ip IPAddress(10, 0, 0, 15)
+
+// STEP 5 [REQUIRED unless using Arduino Yun]
+// replace with ethernet shield mac. Must be unique for your network
+const byte mac[] = {0x90, 0xA2, 0xDA, 0x00, 0x53, 0xE5};
+
+/*==============================================================================
+ * CONFIGURATION ERROR CHECK (don't change anything here)
+ *============================================================================*/
+
+#if !defined WIZ5100_ETHERNET && !defined YUN_ETHERNET
+#error "you must define either WIZ5100_ETHERNET or YUN_ETHERNET in ethernetConfig.h"
+#endif
+
+#if defined remote_ip && defined remote_host
+#error "cannot define both remote_ip and remote_host at the same time in ethernetConfig.h"
+#endif
+
+/*==============================================================================
+ * PIN IGNORE MACROS (don't change anything here)
+ *============================================================================*/
+
+// ignore SPI pins, pin 10 (Ethernet SS) and pin 4 (SS for SD-Card on Ethernet shield)
+#define IS_IGNORE_ETHERNET_SHIELD(p) ((IS_PIN_SPI(p) || (p) == 4) || (p) == 10)
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino	(revision 224)
@@ -0,0 +1,909 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+/*
+  README
+
+  StandardFirmataEthernetPlus is a client implementation. You will need a Firmata client library
+  with a network transport that can act as a server in order to establish a connection between
+  StandardFirmataEthernetPlus and the Firmata client application.
+
+  StandardFirmataEthernetPlus adds additional features that may exceed the Flash and
+  RAM sizes of Arduino boards such as ATMega328p (Uno) and ATMega32u4
+  (Leonardo, Micro, Yun, etc). It is best to use StandardFirmataPlus with a board that
+  has > 32k Flash and > 3k RAM such as: Arduino Mega, Arduino Due, Teensy 3.0/3.1/3.2, etc.
+
+  This sketch consumes too much Flash and RAM to run reliably on an
+  Arduino Leonardo, Yun, ATMega32u4-based board. Use StandardFirmataEthernet.ino instead
+  for those boards and other boards that do not meet the Flash and RAM requirements.
+
+  To use StandardFirmataEthernet you will need to have one of the following
+  boards or shields:
+
+  - Arduino Ethernet shield (or clone)
+  - Arduino Ethernet board (or clone)
+
+  Follow the instructions in the ethernetConfig.h file (ethernetConfig.h tab in Arduino IDE) to
+  configure your particular hardware.
+
+  NOTE: If you are using an Arduino Ethernet shield you cannot use the following pins on
+  the following boards. Firmata will ignore any requests to use these pins:
+
+  - Arduino Mega: (D4, D10, D50, D51, D52, D53)
+  - Arduino Due: (D4, D10)
+  - Arduino Zero: (D4, D10)
+  - Arduino Uno or other ATMega328p boards: (D4, D10, D11, D12, D13)
+
+  If you are using an ArduinoEthernet board, the following pins cannot be used (same as Uno):
+  - D4, D10, D11, D12, D13
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+/*
+ * Uncomment the #define SERIAL_DEBUG line below to receive serial output messages relating to your connection
+ * that may help in the event of connection issues. If defined, some boards may not begin executing this sketch
+ * until the Serial console is opened.
+ */
+//#define SERIAL_DEBUG
+#include "utility/firmataDebug.h"
+
+// follow the instructions in ethernetConfig.h to configure your particular hardware
+#include "ethernetConfig.h"
+#include "utility/EthernetClientStream.h"
+
+#include "utility/SerialFirmata.h"
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#if defined remote_ip && !defined remote_host
+#ifdef local_ip
+EthernetClientStream stream(client, local_ip, remote_ip, NULL, remote_port);
+#else
+EthernetClientStream stream(client, IPAddress(0, 0, 0, 0), remote_ip, NULL, remote_port);
+#endif
+#endif
+
+#if !defined remote_ip && defined remote_host
+#ifdef local_ip
+EthernetClientStream stream(client, local_ip, IPAddress(0, 0, 0, 0), remote_host, remote_port);
+#else
+EthernetClientStream stream(client, IPAddress(0, 0, 0, 0), IPAddress(0, 0, 0, 0), remote_host, remote_port);
+#endif
+#endif
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0;      // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to sample analog inputs (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous mode */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Stream output queue using Stream.write() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  DEBUG_BEGIN(9600);
+
+#ifdef local_ip
+  Ethernet.begin((uint8_t *)mac, local_ip); //start ethernet
+#else
+  Ethernet.begin((uint8_t *)mac);           //start ethernet using dhcp
+#endif
+
+  DEBUG_PRINTLN("connecting...");
+
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+#ifdef WIZ5100_ETHERNET
+  // StandardFirmataEthernetPlus communicates with Ethernet shields over SPI. Therefore all
+  // SPI pins must be set to IGNORE. Otherwise Firmata would break SPI communication.
+  // add Pin 10 and configure pin 53 as output if using a MEGA with an Ethernet shield.
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    if (IS_IGNORE_ETHERNET_SHIELD(i)) {
+      Firmata.setPinMode(i, PIN_MODE_IGNORE);
+    }
+  }
+
+  // Arduino Ethernet and Arduino EthernetShield have SD SS wired to D4
+  pinMode(PIN_TO_DIGITAL(4), OUTPUT);    // switch off SD card bypassing Firmata
+  digitalWrite(PIN_TO_DIGITAL(4), HIGH); // SS is active low;
+#endif // WIZ5100_ETHERNET
+
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  pinMode(PIN_TO_DIGITAL(53), OUTPUT); // configure hardware SS as output on MEGA
+#endif
+
+  // start up Network Firmata:
+  Firmata.begin(stream);
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * Stream buffer using Stream.write()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+
+#if !defined local_ip
+  // only necessary when using DHCP, ensures local IP is updated appropriately if it changes
+  if (Ethernet.maintain()) {
+    stream.maintain(Ethernet.localIP());
+  }
+#endif
+
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/ethernetConfig.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/ethernetConfig.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/ethernetConfig.h	(revision 224)
@@ -0,0 +1,55 @@
+/*==============================================================================
+ * NETWORK CONFIGURATION
+ *
+ * You must configure your particular hardware. Follow the steps below.
+ *
+ * Currently StandardFirmataEthernetPlus is configured as a client. An option to
+ * configure as a server may be added in the future.
+ *============================================================================*/
+
+/*
+ * Only WIZ5100-based shields and boards are currently supported for
+ * StandardFirmataEthernetPlus.
+ */
+#define WIZ5100_ETHERNET
+
+#ifdef WIZ5100_ETHERNET
+#include <SPI.h>
+#include <Ethernet.h>
+EthernetClient client;
+#endif
+
+// STEP 1 [REQUIRED for all boards and shields]
+// replace with IP of the server you want to connect to, comment out if using 'remote_host'
+#define remote_ip IPAddress(10, 0, 0, 3)
+// *** REMOTE HOST IS NOT YET WORKING ***
+// replace with hostname of server you want to connect to, comment out if using 'remote_ip'
+// #define remote_host "server.local"
+
+// STEP 2 [REQUIRED]
+// Replace with the port that your server is listening on
+#define remote_port 3030
+
+// STEP 3 [REQUIRED unless using DHCP]
+// Replace with your board or ethernet shield's IP address
+// Comment out if you want to use DHCP
+#define local_ip IPAddress(10, 0, 0, 15)
+
+// STEP 4 [REQUIRED]
+// replace with ethernet shield mac. Must be unique for your network
+const byte mac[] = {0x90, 0xA2, 0xDA, 0x00, 0x53, 0xE5};
+
+/*==============================================================================
+ * CONFIGURATION ERROR CHECK (don't change anything here)
+ *============================================================================*/
+
+#if defined remote_ip && defined remote_host
+#error "cannot define both remote_ip and remote_host at the same time in ethernetConfig.h"
+#endif
+
+/*==============================================================================
+ * PIN IGNORE MACROS (don't change anything here)
+ *============================================================================*/
+
+// ignore SPI pins, pin 10 (Ethernet SS) and pin 4 (SS for SD-Card on Ethernet shield)
+#define IS_IGNORE_ETHERNET_SHIELD(p) ((IS_PIN_SPI(p) || (p) == 4) || (p) == 10)
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/StandardFirmataPlus.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/StandardFirmataPlus.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/StandardFirmataPlus.ino	(revision 224)
@@ -0,0 +1,838 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+/*
+  README
+
+  StandardFirmataPlus adds additional features that may exceed the Flash and
+  RAM sizes of Arduino boards such as ATMega328p (Uno) and ATMega32u4
+  (Leonardo, Micro, Yun, etc). It is best to use StandardFirmataPlus with higher
+  memory boards such as the Arduino Mega, Arduino Due, Teensy 3.0/3.1/3.2.
+
+  All Firmata examples that are appended with "Plus" add the following features:
+
+  - Ability to interface with serial devices using UART, USART, or SoftwareSerial
+    depending on the capatilities of the board.
+
+  At the time of this writing, StandardFirmataPlus will still compile and run
+  on ATMega328p and ATMega32u4-based boards, but future versions of this sketch
+  may not as new features are added.
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+#include "utility/SerialFirmata.h"
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0; // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous more */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Serial output queue using Serial.print() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  // Save a couple of seconds by disabling the startup blink sequence.
+  Firmata.disableBlinkVersion();
+
+  // to use a port other than Serial, such as Serial1 on an Arduino Leonardo or Mega,
+  // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
+  // Serial1.begin(57600);
+  // Firmata.begin(Serial1);
+  // However do not do this if you are using SERIAL_MESSAGE
+
+  Firmata.begin(57600);
+  while (!Serial) {
+    ; // wait for serial port to connect. Needed for ATmega32u4-based boards and Arduino 101
+  }
+
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * FTDI buffer using Serial.print()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino	(revision 224)
@@ -0,0 +1,1023 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+  Copyright (C) 2015-2016 Jesse Frush. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+/*
+  README
+
+  StandardFirmataWiFi is a WiFi server application. You will need a Firmata client library with
+  a network transport in order to establish a connection with StandardFirmataWiFi.
+
+  To use StandardFirmataWiFi you will need to have one of the following
+  boards or shields:
+
+  - Arduino WiFi Shield (or clone)
+  - Arduino WiFi Shield 101
+  - Arduino MKR1000 board (built-in WiFi 101)
+  - Adafruit HUZZAH CC3000 WiFi Shield (support coming soon)
+
+  Follow the instructions in the wifiConfig.h file (wifiConfig.h tab in Arduino IDE) to
+  configure your particular hardware.
+
+  Dependencies:
+  - WiFi Shield 101 requires version 0.7.0 or higher of the WiFi101 library (available in Arduino
+    1.6.8 or higher, or update the library via the Arduino Library Manager or clone from source:
+    https://github.com/arduino-libraries/WiFi101)
+
+  In order to use the WiFi Shield 101 with Firmata you will need a board with at least
+  35k of Flash memory. This means you cannot use the WiFi Shield 101 with an Arduino Uno
+  or any other ATmega328p-based microcontroller or with an Arduino Leonardo or other
+  ATmega32u4-based microcontroller. Some boards that will work are:
+
+  - Arduino Zero
+  - Arduino Due
+  - Arduino 101
+  - Arduino Mega
+
+  NOTE: If you are using an Arduino WiFi (legacy) shield you cannot use the following pins on
+  the following boards. Firmata will ignore any requests to use these pins:
+
+  - Arduino Uno or other ATMega328 boards: (D4, D7, D10, D11, D12, D13)
+  - Arduino Mega: (D4, D7, D10, D50, D51, D52, D53)
+  - Arduino Due, Zero or Leonardo: (D4, D7, D10)
+
+  If you are using an Arduino WiFi 101 shield you cannot use the following pins on the following
+  boards:
+
+  - Arduino Due or Zero: (D5, D7, D10)
+  - Arduino Mega: (D5, D7, D10, D50, D52, D53)
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+/*
+ * Uncomment the #define SERIAL_DEBUG line below to receive serial output messages relating to your
+ * connection that may help in the event of connection issues. If defined, some boards may not begin
+ * executing this sketch until the Serial console is opened.
+ */
+//#define SERIAL_DEBUG
+#include "utility/firmataDebug.h"
+
+/*
+ * Uncomment the following include to enable interfacing with Serial devices via hardware or
+ * software serial. Note that if enabled, this sketch will likely consume too much memory to run on
+ * an Arduino Uno or Leonardo or other ATmega328p-based or ATmega32u4-based boards.
+ */
+//#include "utility/SerialFirmata.h"
+
+// follow the instructions in wifiConfig.h to configure your particular hardware
+#include "wifiConfig.h"
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+#define WIFI_MAX_CONN_ATTEMPTS      3
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+#ifdef STATIC_IP_ADDRESS
+IPAddress local_ip(STATIC_IP_ADDRESS);
+#endif
+
+int wifiConnectionAttemptCounter = 0;
+int wifiStatus = WL_IDLE_STATUS;
+
+/* analog inputs */
+int analogInputsToReport = 0;      // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to sample analog inputs (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous mode */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Stream output queue using Stream.write() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void printWifiStatus() {
+#if defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+  if ( WiFi.status() != WL_CONNECTED )
+  {
+    DEBUG_PRINT( "WiFi connection failed. Status value: " );
+    DEBUG_PRINTLN( WiFi.status() );
+  }
+  else
+#endif    //defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+  {
+    // print the SSID of the network you're attached to:
+    DEBUG_PRINT( "SSID: " );
+
+#if defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+    DEBUG_PRINTLN( WiFi.SSID() );
+#endif    //defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+
+    // print your WiFi shield's IP address:
+    DEBUG_PRINT( "IP Address: " );
+
+#if defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+    IPAddress ip = WiFi.localIP();
+    DEBUG_PRINTLN( ip );
+#endif    //defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+
+    // print the received signal strength:
+    DEBUG_PRINT( "signal strength (RSSI): " );
+
+#if defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+    long rssi = WiFi.RSSI();
+    DEBUG_PRINT( rssi );
+#endif    //defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+
+    DEBUG_PRINTLN( " dBm" );
+  }
+}
+
+void setup()
+{
+  /*
+   * WIFI SETUP
+   */
+  DEBUG_BEGIN(9600);
+
+  /*
+   * This statement will clarify how a connection is being made
+   */
+  DEBUG_PRINT( "StandardFirmataWiFi will attempt a WiFi connection " );
+#if defined(WIFI_101)
+  DEBUG_PRINTLN( "using the WiFi 101 library." );
+#elif defined(ARDUINO_WIFI_SHIELD)
+  DEBUG_PRINTLN( "using the legacy WiFi library." );
+#elif defined(HUZZAH_WIFI)
+  DEBUG_PRINTLN( "using the HUZZAH WiFi library." );
+  //else should never happen here as error-checking in wifiConfig.h will catch this
+#endif  //defined(WIFI_101)
+
+  /*
+   * Configure WiFi IP Address
+   */
+#ifdef STATIC_IP_ADDRESS
+  DEBUG_PRINT( "Using static IP: " );
+  DEBUG_PRINTLN( local_ip );
+  //you can also provide a static IP in the begin() functions, but this simplifies
+  //ifdef logic in this sketch due to support for all different encryption types.
+  stream.config( local_ip );
+#else
+  DEBUG_PRINTLN( "IP will be requested from DHCP ..." );
+#endif
+
+  /*
+   * Configure WiFi security
+   */
+#if defined(WIFI_WEP_SECURITY)
+  while (wifiStatus != WL_CONNECTED) {
+    DEBUG_PRINT( "Attempting to connect to WEP SSID: " );
+    DEBUG_PRINTLN(ssid);
+    wifiStatus = stream.begin( ssid, wep_index, wep_key, SERVER_PORT );
+    delay(5000); // TODO - determine minimum delay
+    if (++wifiConnectionAttemptCounter > WIFI_MAX_CONN_ATTEMPTS) break;
+  }
+
+#elif defined(WIFI_WPA_SECURITY)
+  while (wifiStatus != WL_CONNECTED) {
+    DEBUG_PRINT( "Attempting to connect to WPA SSID: " );
+    DEBUG_PRINTLN(ssid);
+    wifiStatus = stream.begin(ssid, wpa_passphrase, SERVER_PORT);
+    delay(5000); // TODO - determine minimum delay
+    if (++wifiConnectionAttemptCounter > WIFI_MAX_CONN_ATTEMPTS) break;
+  }
+
+#else                          //OPEN network
+  while (wifiStatus != WL_CONNECTED) {
+    DEBUG_PRINTLN( "Attempting to connect to open SSID: " );
+    DEBUG_PRINTLN(ssid);
+    wifiStatus = stream.begin(ssid, SERVER_PORT);
+    delay(5000); // TODO - determine minimum delay
+    if (++wifiConnectionAttemptCounter > WIFI_MAX_CONN_ATTEMPTS) break;
+  }
+#endif //defined(WIFI_WEP_SECURITY)
+
+  DEBUG_PRINTLN( "WiFi setup done" );
+  printWifiStatus();
+
+  /*
+   * FIRMATA SETUP
+   */
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  // StandardFirmataWiFi communicates with WiFi shields over SPI. Therefore all
+  // SPI pins must be set to IGNORE. Otherwise Firmata would break SPI communication.
+  // Additional pins may also need to be ignored depending on the particular board or
+  // shield in use.
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+#if defined(ARDUINO_WIFI_SHIELD)
+    if (IS_IGNORE_WIFI_SHIELD(i)
+  #if defined(__AVR_ATmega32U4__)
+        || 24 == i // On Leonardo, pin 24 maps to D4 and pin 28 maps to D10
+        || 28 == i
+  #endif  //defined(__AVR_ATmega32U4__)
+       ) {
+#elif defined (WIFI_101)
+    if (IS_IGNORE_WIFI101_SHIELD(i)) {
+#elif defined (HUZZAH_WIFI)
+    // TODO
+    if (false) {
+#else
+    if (false) {
+#endif
+      Firmata.setPinMode(i, PIN_MODE_IGNORE);
+    }
+  }
+
+  //Set up controls for the Arduino WiFi Shield SS for the SD Card
+#ifdef ARDUINO_WIFI_SHIELD
+  // Arduino WiFi, Arduino WiFi Shield and Arduino Yun all have SD SS wired to D4
+  pinMode(PIN_TO_DIGITAL(4), OUTPUT);    // switch off SD card bypassing Firmata
+  digitalWrite(PIN_TO_DIGITAL(4), HIGH); // SS is active low;
+
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  pinMode(PIN_TO_DIGITAL(53), OUTPUT); // configure hardware SS as output on MEGA
+#endif  //defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+
+#endif  //ARDUINO_WIFI_SHIELD
+
+  // start up Network Firmata:
+  Firmata.begin(stream);
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * Stream buffer using Stream.write()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available()) {
+    Firmata.processInput();
+  }
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+
+  stream.maintain();
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/wifiConfig.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/wifiConfig.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/wifiConfig.h	(revision 224)
@@ -0,0 +1,155 @@
+/*==============================================================================
+ * WIFI CONFIGURATION
+ *
+ * You must configure your particular hardware. Follow the steps below.
+ *
+ * Currently StandardFirmataWiFi is configured as a server. An option to
+ * configure as a client may be added in the future.
+ *============================================================================*/
+
+// STEP 1 [REQUIRED]
+// Uncomment / comment the appropriate set of includes for your hardware (OPTION A, B or C)
+// Option A is enabled by default.
+
+/*
+ * OPTION A: Configure for Arduino WiFi shield
+ *
+ * This will configure StandardFirmataWiFi to use the original WiFi library (deprecated) provided
+ * with the Arduino IDE. It is supported by the Arduino WiFi shield (a discontinued product) and
+ * is compatible with 802.11 B/G networks.
+ *
+ * To configure StandardFirmataWiFi to use the Arduino WiFi shield
+ * leave the #define below uncommented.
+ */
+#define ARDUINO_WIFI_SHIELD
+
+//do not modify these next 4 lines
+#ifdef ARDUINO_WIFI_SHIELD
+#include "utility/WiFiStream.h"
+WiFiStream stream;
+#endif
+
+/*
+ * OPTION B: Configure for WiFi 101
+ *
+ * This will configure StandardFirmataWiFi to use the WiFi101 library, which works with the Arduino WiFi101
+ * shield and devices that have the WiFi101 chip built in (such as the MKR1000). It is compatible
+ * with 802.11 B/G/N networks.
+ *
+ * To enable, uncomment the #define WIFI_101 below and verify the #define values under
+ * options A and C are commented out.
+ *
+ * IMPORTANT: You must have the WiFI 101 library installed. To easily install this library, opent the library manager via:
+ * Arduino IDE Menus: Sketch > Include Library > Manage Libraries > filter search for "WiFi101" > Select the result and click 'install'
+ */
+//#define WIFI_101
+
+//do not modify these next 4 lines
+#ifdef WIFI_101
+#include "utility/WiFi101Stream.h"
+WiFi101Stream stream;
+#endif
+
+/*
+ * OPTION C: Configure for HUZZAH
+ *
+ * HUZZAH is not yet supported, this will be added in a later revision to StandardFirmataWiFi
+ */
+
+//------------------------------
+// TODO
+//------------------------------
+//#define HUZZAH_WIFI
+
+
+// STEP 2 [REQUIRED for all boards and shields]
+// replace this with your wireless network SSID
+char ssid[] = "your_network_name";
+
+// STEP 3 [OPTIONAL for all boards and shields]
+// if you want to use a static IP (v4) address, uncomment the line below. You can also change the IP.
+// if this line is commented out, the WiFi shield will attempt to get an IP from the DHCP server
+// #define STATIC_IP_ADDRESS 192,168,1,113
+
+// STEP 4 [REQUIRED for all boards and shields]
+// define your port number here, you will need this to open a TCP connection to your Arduino
+#define SERVER_PORT 3030
+
+// STEP 5 [REQUIRED for all boards and shields]
+// determine your network security type (OPTION A, B, or C). Option A is the most common, and the default.
+
+
+/*
+ * OPTION A: WPA / WPA2
+ *
+ * WPA is the most common network security type. A passphrase is required to connect to this type.
+ *
+ * To enable, leave #define WIFI_WPA_SECURITY uncommented below, set your wpa_passphrase value appropriately,
+ * and do not uncomment the #define values under options B and C
+ */
+#define WIFI_WPA_SECURITY
+
+#ifdef WIFI_WPA_SECURITY
+char wpa_passphrase[] = "your_wpa_passphrase";
+#endif  //WIFI_WPA_SECURITY
+
+/*
+ * OPTION B: WEP
+ *
+ * WEP is a less common (and regarded as less safe) security type. A WEP key and its associated index are required
+ * to connect to this type.
+ *
+ * To enable, Uncomment the #define below, set your wep_index and wep_key values appropriately, and verify
+ * the #define values under options A and C are commented out.
+ */
+//#define WIFI_WEP_SECURITY
+
+#ifdef WIFI_WEP_SECURITY
+//The wep_index below is a zero-indexed value.
+//Valid indices are [0-3], even if your router/gateway numbers your keys [1-4].
+byte wep_index = 0;
+char wep_key[] = "your_wep_key";
+#endif  //WIFI_WEP_SECURITY
+
+
+/*
+ * OPTION C: Open network (no security)
+ *
+ * Open networks have no security, can be connected to by any device that knows the ssid, and are unsafe.
+ *
+ * To enable, uncomment #define WIFI_NO_SECURITY below and verify the #define values
+ * under options A and B are commented out.
+ */
+//#define WIFI_NO_SECURITY
+
+/*==============================================================================
+ * CONFIGURATION ERROR CHECK (don't change anything here)
+ *============================================================================*/
+
+#if ((defined(ARDUINO_WIFI_SHIELD) && (defined(WIFI_101) || defined(HUZZAH_WIFI))) || (defined(WIFI_101) && defined(HUZZAH_WIFI)))
+#error "you may not define more than one wifi device type in wifiConfig.h."
+#endif //WIFI device type check
+
+#if !(defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101) || defined(HUZZAH_WIFI))
+#error "you must define a wifi device type in wifiConfig.h."
+#endif
+
+#if ((defined(WIFI_NO_SECURITY) && (defined(WIFI_WEP_SECURITY) || defined(WIFI_WPA_SECURITY))) || (defined(WIFI_WEP_SECURITY) && defined(WIFI_WPA_SECURITY)))
+#error "you may not define more than one security type at the same time in wifiConfig.h."
+#endif  //WIFI_* security define check
+
+#if !(defined(WIFI_NO_SECURITY) || defined(WIFI_WEP_SECURITY) || defined(WIFI_WPA_SECURITY))
+#error "you must define a wifi security type in wifiConfig.h."
+#endif  //WIFI_* security define check
+
+/*==============================================================================
+ * PIN IGNORE MACROS (don't change anything here)
+ *============================================================================*/
+
+// ignore SPI pins, pin 5 (reset WiFi101 shield), pin 7 (WiFi handshake) and pin 10 (WiFi SS)
+// also don't ignore SS pin if it's not pin 10
+// TODO - need to differentiate between Arduino WiFi1 101 Shield and Arduino MKR1000
+#define IS_IGNORE_WIFI101_SHIELD(p)  ((p) == 10 || (IS_PIN_SPI(p) && (p) != SS) || (p) == 5 || (p) == 7)
+
+// ignore SPI pins, pin 4 (SS for SD-Card on WiFi-shield), pin 7 (WiFi handshake) and pin 10 (WiFi SS)
+#define IS_IGNORE_WIFI_SHIELD(p)     ((IS_PIN_SPI(p) || (p) == 4) || (p) == 7 || (p) == 10)
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/extras/revisions.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/extras/revisions.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/extras/revisions.txt	(revision 224)
@@ -0,0 +1,202 @@
+FIRMATA 2.5.2 - Feb 15, 2016
+
+[core library]
+* Added Wi-Fi transport (Jesse Frush)
+* Added support for Arduino MKR1000 (Jesse Frush)
+* Moved Serial feature to own class SerialFirmata
+* Moved pin config and pin state handling to Firmata.cpp
+* Added new method disableBlinkVersion to provide a way to optionally bypass startup blink sequence
+
+[StandardFirmata & variants]
+* Added StandardFirmataWiFi (Jesse Frush)
+* Added ethernetConfig.h for StandardFirmataEthernet and StandardFirmtaEthernetPlus
+* Removed serialUtils.h and using SerialFirmata class instead for Serial feature
+
+FIRMATA 2.5.1 - Dec 26, 2015
+
+[core library]
+* Added support for Arduino 101
+* Make VERSION_BLINK_PIN optional
+* Separate protocol version from firmware version.
+  Use FIRMATA_PROTOCOL_VERSION_[MAJOR/MINOR/BUGFIX] for protocol and use
+  FIRMATA_FIRMWARE_VERSION_[MAJOR/MINOR/BUGFIX] for firmware (library version).
+
+[StandardFirmata & variants]
+* Added ability to auto-restart I2C transmission by setting bit 6 of byte 3
+  of the I2C_REQUEST message.
+
+FIRMATA 2.5.0 - Nov 7, 2015
+
+[core library]
+* Added Serial feature for interfacing with serial devices via hardware
+  or software serial. See github.com/firmata/protocol/serial.md for details
+* Added ability to set the value of a pin by sending a single value instead
+  of a port value. See 'set digital pin value' in github.com/firmata/protocol/protocol.md
+  for details
+* Added support for Arduino Zero board
+* Added support for Teensy LC board (copied from Teensy Firmata lib)
+* Added support for Pinoccio Scout board (Pawel Szymczykowski)
+* Lowered minimun sampling interval from 10 to 1 millisecond
+* Added new pin mode (PIN_MODE_PULLUP) for setting the INPUT_PULLUP pin mode
+* Changed pin mode defines to safer names (old names still included but
+  deprecated) - see Firmata.h
+
+[StandardFirmata & variants]
+* Created new StandardFirmataPlus that implements the Serial feature
+  Note: The new Serial features is only implemented in the "Plus" versions of
+  StandardFirmata.
+* Created new StandardFirmataEthernetPlus that implements the Serial feature
+* Fixed issue where StandardFirmata was not compiling for Intel Galileo boards
+* Moved StandardFirmataYun to its own repo (github.com/firmata/StandardFirmataYun)
+
+FIRMATA 2.4.4 - Aug 9, 2015
+
+[core library]
+* Added support for chipKIT boards (Brian Schmalz, Rick Anderson and Keith Vogel)
+* Added support for ATmega328 boards (previously only ATmega328p was supported)
+
+[StandardFirmata]
+* Added StandardFirmataChipKIT for ChipKIT boards (Brian Schmalz, Rick Anderson and Keith Vogel)
+* Ensure Serial is ready on Leonardo and other ATMega32u4-based boards
+
+FIRMATA 2.4.3 - Apr 11, 2015
+
+[core library]
+* Added debug macros (utility/firmataDebug.h)
+* Added Norbert Truchsess' EthernetClientStream lib from the configurable branch
+
+[examples]
+* Added StandardFirmataEthernet to enable Firmata over Ethernet
+* Minor updates to StandardFirmata and StandardFirmataYun
+
+FIRMATA 2.4.2 - Mar 16, 2015
+
+[core library]
+* Add support for Teesy 3.1 (Olivier Louvignes)
+
+FIRMATA 2.4.1 - Feb 7, 2015
+
+[core library]
+* Fixed off-by-one bug in setFirmwareNameAndVersion (Brian Schmalz)
+
+[StandardFirmata]
+* Prevent analog values from being reported during system reset
+
+FIRMATA 2.4.0 - Dec 21, 2014
+
+Changes from 2.3.6 to 2.4 that may impact existing Firmata client implementations:
+
+* When sending a string from the client application to the board (STRING_DATA) a
+static buffer is now used for the incoming string in place of a dynamically allocated
+block of memory (see Firmata.cpp lines 181 - 205). In Firmata 2.3.6 and older,
+the dynamically allocated block was never freed, causing a memory leak. If your
+client library had freed this memory in the string callback method, that code
+will break in Firmata 2.4. If the string data needs to persist beyond the string
+callback, it should be copied within the string callback.
+
+* As of Firmata 2.4, when digital port reporting or analog pin reporting is enabled,
+the value of the port (digital) or pin (analog) is immediately sent back to the client
+application. This will likely not have a negative impact on existing client
+implementations, but may be unexpected. This feature was added to better support
+non-serial streams (such as Ethernet, Wi-Fi, Bluetooth, etc) that may lose
+connectivity and need a quick way to get the current state of the pins upon
+reestablishing a connection.
+
+[core library]
+* Changed sendValueAsTwo7bitBytes, startSysex and endSysex from private to
+  public methods.
+* Added Intel Galileo to Boards.h
+* Renamed FirmataSerial to FirmataStream
+* Updated to latest Arduino library format
+* writePort function in Boards.h now returns 1 (to suppress compiler warning)
+* Updated syntax highlighting (keywords.txt)
+* Fixed IS_PIN_SPI ifndef condition in boards.h
+* Added constants to Firmata.h to reserve configurable firmata features
+* Fixed issue where firmwareName was not reported correctly in Windows
+* Ensure incoming String via STRING_DATA command is null-terminated
+* Fixed memory leak when receiving String via STRING_DATA command
+  (note this change may break existing code if you were manually deallocating
+  the incoming string in your string callback function. See code for details)
+* Added ability for user to specify a filename when calling setFirmwareNameAndVersion
+* Increased input data buffer size from 32 to 64 bytes
+
+[StandardFirmata]
+* Updated I2C_READ_CONTINUOUSLY to work with or without slaveRegister (Rick Waldron)
+* Added Yun variant of StandardFirmata
+* When a digital port is enabled, its value is now immediately sent to the client
+* When an analog pin is enabled, its value is now immediately sent to the client
+* Changed the way servo pins are mapped to enable use of servos on
+  a wider range of pins, including analog pins.
+* Fixed management of unexpected sized I2C replies (Nahuel Greco)
+* Fixed a bug when removing a monitored device with I2C_STOP_Reading (Nahuel Greco)
+* Fixed conditional expression in I2C_STOP_READING case
+* Changed samplingInterval from type int to type unsigned int
+* Shortened error message strings to save a few bytes
+
+[examples]
+* Updated FirmataServo example to use new pin mapping technique
+* Removed makefiles from examples (because they were not being updated)
+* Updated all examples to set current firmware version
+
+FIRMATA 2.3.6 - Jun 18, 2013 (Version included with Arduino core libraries)
+
+[core library]
+* Fixed bug introduced in 2.3.5 that broke ability to use Ethernet.
+
+FIRMATA 2.3.5 - May 21, 2013
+
+[core library]
+* Added Arduino Due to Boards.h
+* Added Teensy 3.0 to Boards.h
+* Updated unit tests to use ArduinoUnit v2.0
+* Renamed pin13strobe to strobeBlinkPin
+* Removed blinkVersion method from begin method for non-serial streams
+* Fixed memory leak in setting firmware version (Matthew Murdoch)
+* Added unit tests for a few core functions (Matthew Murdoch)
+* Added IS_PIN_SPI macro to all board definitions in Board.h (Norbert Truchsess)
+
+FIRMATA 2.3.4 - Feb 11, 2013
+
+[core library]
+* Fixed Stream implementation so Firmata can be used with Streams other than
+  Serial (Norbert Truchsess)
+
+FIRMATA 2.3.3 - Oct 6, 2012
+
+[core library]
+* Added write method to expose FirmataSerial.write
+* Added Arduino Leonardo to Boards.h
+
+[StandardFirmata]
+* Changed all instances of Serial.write to Firmata.write
+* Fixed delayMicroseconds(0) bug in readAndReportData
+
+FIRMATA 2.3.0 - 2.3.2
+
+* Removed angle from servo config
+* Changed file extensions from .pde to .ino
+* Added MEGA2560 to Boards.h
+* Added I2C pins to Boards.h
+* Modified examples to be compatible with Arduino 0022 and 1.0 or greater
+* Removed I2CFirmata example
+* Changes to StandardFirmata
+  * Added I2C support
+  * Added system reset message to reset all pins to default config on sysex reset
+
+FIRMATA 2.2 (changes prior to Firmata 2.3.0 were not well documented)
+
+* changes undocumented
+
+FIRMATA 2.1
+
+* added support for changing the sampling interval
+* added Servo support
+
+FIRMATA 2.0
+
+* changed to 8-bit port-based digital messages to mirror ports from previous 14-bit ports modeled after the standard Arduino board.
+* switched order of version message so major version is reported first
+
+FIRMATA 1.0
+
+* switched to MIDI-compatible packet format (though the message interpretation differs)
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/keywords.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/keywords.txt	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/keywords.txt	(revision 224)
@@ -7,9 +7,9 @@
 #######################################
 
-Firmata	KEYWORD1
-callbackFunction	KEYWORD1
-systemResetCallbackFunction	KEYWORD1
-stringCallbackFunction	KEYWORD1
-sysexCallbackFunction	KEYWORD1
+Firmata				KEYWORD1	Firmata
+callbackFunction		KEYWORD1	callbackFunction
+systemResetCallbackFunction	KEYWORD1	systemResetCallbackFunction
+stringCallbackFunction		KEYWORD1	stringCallbackFunction
+sysexCallbackFunction		KEYWORD1	sysexCallbackFunction
 
 #######################################
@@ -17,23 +17,32 @@
 #######################################
 
-begin	KEYWORD2
-begin	KEYWORD2
-printVersion	KEYWORD2
-blinkVersion	KEYWORD2
-printFirmwareVersion	KEYWORD2
-setFirmwareVersion	KEYWORD2
+begin				KEYWORD2
+printVersion			KEYWORD2
+blinkVersion			KEYWORD2
+printFirmwareVersion		KEYWORD2
+setFirmwareVersion		KEYWORD2
 setFirmwareNameAndVersion	KEYWORD2
-available	KEYWORD2
-processInput	KEYWORD2
-sendAnalog	KEYWORD2
-sendDigital	KEYWORD2
-sendDigitalPortPair	KEYWORD2
-sendDigitalPort	KEYWORD2
-sendString	KEYWORD2
-sendString	KEYWORD2
-sendSysex	KEYWORD2
-attach	KEYWORD2
-detach	KEYWORD2
-flush	KEYWORD2
+available			KEYWORD2
+processInput			KEYWORD2
+isParsingMessage		KEYWORD2
+parse				KEYWORD2
+sendAnalog			KEYWORD2
+sendDigital			KEYWORD2
+sendDigitalPort			KEYWORD2
+sendString			KEYWORD2
+sendSysex			KEYWORD2
+getPinMode			KEYWORD2
+setPinMode			KEYWORD2
+getPinState			KEYWORD2
+setPinState			KEYWORD2
+attach				KEYWORD2
+detach				KEYWORD2
+write				KEYWORD2
+sendValueAsTwo7bitBytes	KEYWORD2
+startSysex			KEYWORD2
+endSysex			KEYWORD2
+writePort			KEYWORD2
+readPort			KEYWORD2
+disableBlinkVersion		KEYWORD2
 
 
@@ -42,21 +51,40 @@
 #######################################
 
-MAX_DATA_BYTES	LITERAL1
+FIRMATA_MAJOR_VERSION	LITERAL1
+FIRMATA_MINOR_VERSION	LITERAL1
+FIRMATA_BUGFIX_VERSION	LITERAL1
 
-DIGITAL_MESSAGE	LITERAL1
-ANALOG_MESSAGE	LITERAL1
-REPORT_ANALOG	LITERAL1
-REPORT_DIGITAL	LITERAL1
-REPORT_VERSION	LITERAL1
-SET_PIN_MODE	LITERAL1
-SYSTEM_RESET	LITERAL1
+MAX_DATA_BYTES		LITERAL1
 
-START_SYSEX	LITERAL1
-END_SYSEX	LITERAL1
+DIGITAL_MESSAGE		LITERAL1
+ANALOG_MESSAGE		LITERAL1
+REPORT_ANALOG		LITERAL1
+REPORT_DIGITAL		LITERAL1
+REPORT_VERSION		LITERAL1
+SET_PIN_MODE		LITERAL1
+SET_DIGITAL_PIN_VALUE	LITERAL1
+SYSTEM_RESET		LITERAL1
+START_SYSEX		LITERAL1
+END_SYSEX		LITERAL1
+REPORT_FIRMWARE		LITERAL1
+STRING_DATA		LITERAL1
 
-PWM	LITERAL1
+PIN_MODE_ANALOG		LITERAL1
+PIN_MODE_PWM		LITERAL1
+PIN_MODE_SERVO		LITERAL1
+PIN_MODE_SHIFT		LITERAL1
+PIN_MODE_I2C		LITERAL1
+PIN_MODE_ONEWIRE	LITERAL1
+PIN_MODE_STEPPER	LITERAL1
+PIN_MODE_ENCODER	LITERAL1
+PIN_MODE_SERIAL		LITERAL1
+PIN_MODE_PULLUP		LITERAL1
+PIN_MODE_IGNORE		LITERAL1
 
+TOTAL_PINS		LITERAL1
 TOTAL_ANALOG_PINS	LITERAL1
 TOTAL_DIGITAL_PINS	LITERAL1
-TOTAL_PORTS			LITERAL1
-ANALOG_PORT			LITERAL1
+TOTAL_PIN_MODES		LITERAL1
+TOTAL_PORTS		LITERAL1
+ANALOG_PORT		LITERAL1
+MAX_SERVOS		LITERAL1
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/library.properties
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/library.properties	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/library.properties	(revision 224)
@@ -1,9 +1,9 @@
 name=Firmata
-version=2.3.6
+version=2.5.2
 author=Firmata Developers
-maintainer=Firmata Developers <firmata-devel@lists.sourceforge.net>
+maintainer=https://github.com/firmata/arduino
 sentence=Enables the communication with computer apps using a standard serial protocol. For all Arduino boards.
 paragraph=The Firmata library implements the Firmata protocol for communicating with software on the host computer. This allows you to write custom firmware without having to create your own protocol and objects for the programming environment that you are using.
 category=Device Control
-url=http://firmata.org
+url=https://github.com/firmata/arduino
 architectures=*
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/readme.md
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/readme.md	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/readme.md	(revision 224)
@@ -0,0 +1,177 @@
+#Firmata
+
+[![Gitter](https://badges.gitter.im/Join Chat.svg)](https://gitter.im/firmata/arduino?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge)
+
+Firmata is a protocol for communicating with microcontrollers from software on a host computer. The [protocol](https://github.com/firmata/protocol) can be implemented in firmware on any microcontroller architecture as well as software on any host computer software package. The Arduino repository described here is a Firmata library for Arduino and Arduino-compatible devices. If you would like to contribute to Firmata, please see the [Contributing](#contributing) section below.
+
+##Usage
+
+There are two main models of usage of Firmata. In one model, the author of the Arduino sketch uses the various methods provided by the Firmata library to selectively send and receive data between the Arduino device and the software running on the host computer. For example, a user can send analog data to the host using ``` Firmata.sendAnalog(analogPin, analogRead(analogPin)) ``` or send data packed in a string using ``` Firmata.sendString(stringToSend) ```. See File -> Examples -> Firmata -> AnalogFirmata & EchoString respectively for examples.
+
+The second and more common model is to load a general purpose sketch called StandardFirmata (or one of the variants such as StandardFirmataPlus or StandardFirmataEthernet depending on your needs) on the Arduino board and then use the host computer exclusively to interact with the Arduino board. StandardFirmata is located in the Arduino IDE in File -> Examples -> Firmata.
+
+##Firmata Client Libraries
+Most of the time you will be interacting with Arduino with a client library on the host computers. Several Firmata client libraries have been implemented in a variety of popular programming languages:
+
+* processing
+  * [https://github.com/firmata/processing]
+  * [http://funnel.cc]
+* python
+  * [https://github.com/MrYsLab/pymata-aio]
+  * [https://github.com/MrYsLab/PyMata]
+  * [https://github.com/tino/pyFirmata]
+  * [https://github.com/lupeke/python-firmata]
+  * [https://github.com/firmata/pyduino]
+* perl
+  * [https://github.com/ntruchsess/perl-firmata]
+  * [https://github.com/rcaputo/rx-firmata]
+* ruby
+  * [https://github.com/hardbap/firmata]
+  * [https://github.com/PlasticLizard/rufinol]
+  * [http://funnel.cc]
+* clojure
+  * [https://github.com/nakkaya/clodiuno]
+  * [https://github.com/peterschwarz/clj-firmata]
+* javascript
+  * [https://github.com/jgautier/firmata]
+  * [https://github.com/rwldrn/johnny-five]
+  * [http://breakoutjs.com]
+* java
+  * [https://github.com/kurbatov/firmata4j]
+  * [https://github.com/4ntoine/Firmata]
+  * [https://github.com/reapzor/FiloFirmata]
+* .NET
+  * [https://github.com/SolidSoils/Arduino]
+  * [http://www.imagitronics.org/projects/firmatanet/]
+* Flash/AS3
+  * [http://funnel.cc]
+  * [http://code.google.com/p/as3glue/]
+* PHP
+  * [https://bitbucket.org/ThomasWeinert/carica-firmata]
+  * [https://github.com/oasynnoum/phpmake_firmata]
+* Haskell
+  * [http://hackage.haskell.org/package/hArduino]
+* iOS
+  * [https://github.com/jacobrosenthal/iosfirmata]
+* Dart
+  * [https://github.com/nfrancois/firmata]
+* Max/MSP
+  * [http://www.maxuino.org/]
+* Elixir
+  * [https://github.com/kfatehi/firmata]
+* Modelica
+  * [https://www.wolfram.com/system-modeler/libraries/model-plug/]
+* golang
+  * [https://github.com/kraman/go-firmata]
+
+Note: The above libraries may support various versions of the Firmata protocol and therefore may not support all features of the latest Firmata spec nor all Arduino and Arduino-compatible boards. Refer to the respective projects for details.
+
+##Updating Firmata in the Arduino IDE - Arduino 1.6.4 and higher
+
+If you want to update to the latest stable version:
+
+1. Open the Arduino IDE and navigate to: `Sketch > Include Library > Manage Libraries`
+2. Filter by "Firmata" and click on the "Firmata by Firmata Developers" item in the list of results.
+3. Click the `Select version` dropdown and select the most recent version (note you can also install previous versions)
+4. Click `Install`.
+
+###Cloning Firmata
+
+If you are contributing to Firmata or otherwise need a version newer than the latest tagged release, you can clone Firmata directly to your Arduino/libraries/ directory (where 3rd party libraries are installed). This only works for Arduino 1.6.4 and higher, for older versions you need to clone into the Arduino application directory (see section below titled "Using the Source code rather than release archive"). Be sure to change the name to Firmata as follows:
+
+```bash
+$ git clone git@github.com:firmata/arduino.git ~/Documents/Arduino/libraries/Firmata
+```
+
+*Update path above if you're using Windows or Linux or changed the default Arduino directory on OS X*
+
+
+##Updating Firmata in the Arduino IDE - older versions (<= 1.6.3 or 1.0.x)
+
+Download the latest [release](https://github.com/firmata/arduino/releases/tag/2.5.2) (for Arduino 1.0.x or Arduino 1.5.6 or higher) and replace the existing Firmata folder in your Arduino application. See the instructions below for your platform.
+
+*Note that Arduino 1.5.0 - 1.5.5 are not supported. Please use Arduino 1.5.6 or higher (or Arduino 1.0.5 or 1.0.6).*
+
+###Mac OSX:
+
+The Firmata library is contained within the Arduino package.
+
+1. Navigate to the Arduino application
+2. Right click on the application icon and select `Show Package Contents`
+3. Navigate to: `/Contents/Resources/Java/libraries/` and replace the existing
+`Firmata` folder with latest [Firmata release](https://github.com/firmata/arduino/releases/tag/2.5.2) (note there is a different download
+for Arduino 1.0.x vs 1.6.x)
+4. Restart the Arduino application and the latest version of Firmata will be available.
+
+*If you are using the Java 7 version of Arduino 1.5.7 or higher, the file path
+will differ slightly: `Contents/Java/libraries/Firmata` (no Resources directory).*
+
+###Windows:
+
+1. Navigate to `c:/Program\ Files/arduino-1.x/libraries/` and replace the existing
+`Firmata` folder with the latest [Firmata release](https://github.com/firmata/arduino/releases/tag/2.5.2) (note there is a different download
+for Arduino 1.0.x vs 1.6.x).
+2. Restart the Arduino application and the latest version of Firmata will be available.
+
+*Update the path and Arduino version as necessary*
+
+###Linux:
+
+1. Navigate to `~/arduino-1.x/libraries/` and replace the existing
+`Firmata` folder with the latest [Firmata release](https://github.com/firmata/arduino/releases/tag/2.5.2) (note there is a different download
+for Arduino 1.0.x vs 1.6.x).
+2. Restart the Arduino application and the latest version of Firmata will be available.
+
+*Update the path and Arduino version as necessary*
+
+###Using the Source code rather than release archive (only for versions older than Arduino 1.6.3)
+
+*It is recommended you update to Arduino 1.6.4 or higher if possible, that way you can clone directly into the external Arduino/libraries/ directory which persists between Arduino application updates. Otherwise you will need to move your clone each time you update to a newer version of the Arduino IDE.*
+
+If you're stuck with an older version of the IDE, then follow these keep reading otherwise jump up to the "Cloning Firmata section above".
+
+Clone this repo directly into the core Arduino application libraries directory. If you are using
+Arduino 1.5.x or <= 1.6.3, the repo directory structure will not match the Arduino
+library format, however it should still compile as long as you are using Arduino 1.5.7
+or higher.
+
+You will first need to remove the existing Firmata library, then clone firmata/arduino
+into an empty Firmata directory:
+
+```bash
+$ rm -r /Applications/Arduino.app/Contents/Resources/Java/libraries/Firmata
+$ git clone git@github.com:firmata/arduino.git /Applications/Arduino.app/Contents/Resources/Java/libraries/Firmata
+```
+
+*Update paths if you're using Windows or Linux*
+
+To generate properly formatted versions of Firmata (for Arduino 1.0.x and Arduino 1.6.x), run the
+`release.sh` script.
+
+
+<a name="contributing" />
+##Contributing
+
+If you discover a bug or would like to propose a new feature, please open a new [issue](https://github.com/firmata/arduino/issues?sort=created&state=open). Due to the limited memory of standard Arduino boards we cannot add every requested feature to StandardFirmata. Requests to add new features to StandardFirmata will be evaluated by the Firmata developers. However it is still possible to add new features to other Firmata implementations (Firmata is a protocol whereas StandardFirmata is just one of many possible implementations).
+
+To contribute, fork this repository and create a new topic branch for the bug, feature or other existing issue you are addressing. Submit the pull request against the *master* branch.
+
+If you would like to contribute but don't have a specific bugfix or new feature to contribute, you can take on an existing issue, see issues labeled "pull-request-encouraged". Add a comment to the issue to express your intent to begin work and/or to get any additional information about the issue.
+
+You must thoroughly test your contributed code. In your pull request, describe tests performed to ensure that no existing code is broken and that any changes maintain backwards compatibility with the existing api. Test on multiple Arduino board variants if possible. We hope to enable some form of automated (or at least semi-automated) testing in the future, but for now any tests will need to be executed manually by the contributor and reviewers.
+
+Use [Artistic Style](http://astyle.sourceforge.net/) (astyle) to format your code. Set the following rules for the astyle formatter:
+
+```
+style = ""
+indent-spaces = 2
+indent-classes = true
+indent-switches = true
+indent-cases = true
+indent-col1-comments = true
+pad-oper = true
+pad-header = true
+keep-one-line-statements = true
+```
+
+If you happen to use Sublime Text, [this astyle plugin](https://github.com/timonwong/SublimeAStyleFormatter) is helpful. Set the above rules in the user settings file.
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/release.sh
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/release.sh	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/release.sh	(revision 224)
@@ -0,0 +1,35 @@
+#!/bin/sh
+
+# use this script to package Firmata for distribution
+
+# package for Arduino 1.0.x
+mkdir -p temp/Firmata
+cp -r examples temp/Firmata
+cp -r extras temp/Firmata
+cp -r utility temp/Firmata
+cp Boards.h temp/Firmata
+cp Firmata.cpp temp/Firmata
+cp Firmata.h temp/Firmata
+cp keywords.txt temp/Firmata
+cp readme.md temp/Firmata
+cd temp
+find . -name "*.DS_Store" -type f -delete
+zip -r Firmata.zip ./Firmata/
+cd ..
+mv ./temp/Firmata.zip Firmata-2.5.2.zip
+
+#package for Arduino 1.6.x
+cp library.properties temp/Firmata
+cd temp/Firmata
+mv readme.md ./extras/
+mkdir src
+mv Boards.h ./src/
+mv Firmata.cpp ./src/
+mv Firmata.h ./src/
+mv utility ./src/
+cd ..
+find . -name "*.DS_Store" -type f -delete
+zip -r Firmata.zip ./Firmata/
+cd ..
+mv ./temp/Firmata.zip Arduino-1.6.x-Firmata-2.5.2.zip
+rm -r ./temp
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/firmata_test/firmata_test.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/firmata_test/firmata_test.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/firmata_test/firmata_test.ino	(revision 224)
@@ -0,0 +1,136 @@
+/*
+ * To run this test suite, you must first install the ArduinoUnit library
+ * to your Arduino/libraries/ directory.
+ * You can get ArduinoUnit here: https://github.com/mmurdoch/arduinounit
+ * Download version 2.0 or greater or install it via the Arduino library manager.
+ */
+
+#include <ArduinoUnit.h>
+#include <Firmata.h>
+
+void setup()
+{
+  Serial.begin(9600);
+}
+
+void loop()
+{
+  Test::run();
+}
+
+test(beginPrintsVersion)
+{
+  FakeStream stream;
+
+  Firmata.begin(stream);
+
+  char expected[] = {
+    REPORT_VERSION,
+    FIRMATA_PROTOCOL_MAJOR_VERSION,
+    FIRMATA_PROTOCOL_MINOR_VERSION,
+    0
+  };
+  assertEqual(expected, stream.bytesWritten());
+}
+
+void processMessage(const byte *message, size_t length)
+{
+  FakeStream stream;
+  Firmata.begin(stream);
+
+  for (size_t i = 0; i < length; i++) {
+    stream.nextByte(message[i]);
+    Firmata.processInput();
+  }
+}
+
+byte _digitalPort;
+int _digitalPortValue;
+void writeToDigitalPort(byte port, int value)
+{
+  _digitalPort = port;
+  _digitalPortValue = value;
+}
+
+void setupDigitalPort()
+{
+  _digitalPort = 0;
+  _digitalPortValue = 0;
+}
+
+test(processWriteDigital_0)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE, 0, 0 };
+  processMessage(message, 3);
+
+  assertEqual(0, _digitalPortValue);
+}
+
+test(processWriteDigital_127)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE, 127, 0 };
+  processMessage(message, 3);
+
+  assertEqual(127, _digitalPortValue);
+}
+
+test(processWriteDigital_128)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE, 0, 1 };
+  processMessage(message, 3);
+
+  assertEqual(128, _digitalPortValue);
+}
+
+test(processWriteLargestDigitalValue)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE, 0x7F, 0x7F };
+  processMessage(message, 3);
+
+  // Maximum of 14 bits can be set (B0011111111111111)
+  assertEqual(0x3FFF, _digitalPortValue);
+}
+
+test(defaultDigitalWritePortIsZero)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE, 0, 0 };
+  processMessage(message, 3);
+
+  assertEqual(0, _digitalPort);
+}
+
+test(specifiedDigitalWritePort)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE + 1, 0, 0 };
+  processMessage(message, 3);
+
+  assertEqual(1, _digitalPort);
+}
+
+test(setFirmwareVersionDoesNotLeakMemory)
+{
+  Firmata.setFirmwareVersion(1, 0);
+  int initialMemory = freeMemory();
+
+  Firmata.setFirmwareVersion(1, 0);
+
+  assertEqual(0, initialMemory - freeMemory());
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/readme.md
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/readme.md	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/readme.md	(revision 224)
@@ -0,0 +1,13 @@
+#Testing Firmata
+
+Tests tests are written using the [ArduinoUnit](https://github.com/mmurdoch/arduinounit) library (version 2.0).
+
+Follow the instructions in the [ArduinoUnit readme](https://github.com/mmurdoch/arduinounit/blob/master/readme.md) to install the library.
+
+Compile and upload the test sketch as you would any other sketch. Then open the
+Serial Monitor to view the test results.
+
+If you make changes to Firmata.cpp, run the tests in /test/ to ensure
+that your changes have not produced any unexpected errors.
+
+You should also perform manual tests against actual hardware.
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.cpp	(revision 224)
@@ -0,0 +1,3 @@
+/*
+ * Implementation is in BLEStream.h to avoid linker issues.
+ */
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.h	(revision 224)
@@ -0,0 +1,243 @@
+/*
+  BLEStream.h
+
+  Based on BLESerial.cpp by Voita Molda
+  https://github.com/sandeepmistry/arduino-BLEPeripheral/blob/master/examples/serial/BLESerial.h
+
+  Last updated April 04th, 2016
+ */
+
+#ifndef _BLE_STREAM_H_
+#define _BLE_STREAM_H_
+
+#include <Arduino.h>
+#if defined(_VARIANT_ARDUINO_101_X_)
+#include <CurieBLE.h>
+#define _MAX_ATTR_DATA_LEN_ BLE_MAX_ATTR_DATA_LEN
+#else
+#include <BLEPeripheral.h>
+#define _MAX_ATTR_DATA_LEN_ BLE_ATTRIBUTE_MAX_VALUE_LENGTH
+#endif
+
+#define BLESTREAM_TXBUFFER_FLUSH_INTERVAL 80
+#define BLESTREAM_MIN_FLUSH_INTERVAL 8 // minimum interval for flushing the TX buffer
+
+// #define BLE_SERIAL_DEBUG
+
+class BLEStream : public BLEPeripheral, public Stream
+{
+  public:
+    BLEStream(unsigned char req = 0, unsigned char rdy = 0, unsigned char rst = 0);
+
+    void begin(...);
+    bool poll();
+    void end();
+    void setFlushInterval(int);
+
+    virtual int available(void);
+    virtual int peek(void);
+    virtual int read(void);
+    virtual void flush(void);
+    virtual size_t write(uint8_t byte);
+    using Print::write;
+    virtual operator bool();
+
+  private:
+    bool _connected;
+    unsigned long _flushed;
+    int _flushInterval;
+    static BLEStream* _instance;
+
+    size_t _rxHead;
+    size_t _rxTail;
+    size_t _rxCount() const;
+    unsigned char _rxBuffer[256];
+    size_t _txCount;
+    unsigned char _txBuffer[_MAX_ATTR_DATA_LEN_];
+
+    BLEService _uartService = BLEService("6E400001-B5A3-F393-E0A9-E50E24DCCA9E");
+    BLEDescriptor _uartNameDescriptor = BLEDescriptor("2901", "UART");
+    BLECharacteristic _rxCharacteristic = BLECharacteristic("6E400002-B5A3-F393-E0A9-E50E24DCCA9E", BLEWriteWithoutResponse, _MAX_ATTR_DATA_LEN_);
+    BLEDescriptor _rxNameDescriptor = BLEDescriptor("2901", "RX - Receive Data (Write)");
+    BLECharacteristic _txCharacteristic = BLECharacteristic("6E400003-B5A3-F393-E0A9-E50E24DCCA9E", BLENotify, _MAX_ATTR_DATA_LEN_);
+    BLEDescriptor _txNameDescriptor = BLEDescriptor("2901", "TX - Transfer Data (Notify)");
+
+    void _received(const unsigned char* data, size_t size);
+    static void _received(BLECentral& /*central*/, BLECharacteristic& rxCharacteristic);
+};
+
+
+/*
+ * BLEStream.cpp
+ * Copied here as a hack to avoid having to install the BLEPeripheral libarary even if it's
+ * not needed.
+ */
+
+BLEStream* BLEStream::_instance = NULL;
+
+BLEStream::BLEStream(unsigned char req, unsigned char rdy, unsigned char rst) :
+#if defined(_VARIANT_ARDUINO_101_X_)
+  BLEPeripheral()
+#else
+  BLEPeripheral(req, rdy, rst)
+#endif
+{
+  this->_txCount = 0;
+  this->_rxHead = this->_rxTail = 0;
+  this->_flushed = 0;
+  this->_flushInterval = BLESTREAM_TXBUFFER_FLUSH_INTERVAL;
+  BLEStream::_instance = this;
+
+  addAttribute(this->_uartService);
+  addAttribute(this->_uartNameDescriptor);
+  setAdvertisedServiceUuid(this->_uartService.uuid());
+  addAttribute(this->_rxCharacteristic);
+  addAttribute(this->_rxNameDescriptor);
+  this->_rxCharacteristic.setEventHandler(BLEWritten, BLEStream::_received);
+  addAttribute(this->_txCharacteristic);
+  addAttribute(this->_txNameDescriptor);
+}
+
+void BLEStream::begin(...)
+{
+  BLEPeripheral::begin();
+#ifdef BLE_SERIAL_DEBUG
+  Serial.println(F("BLEStream::begin()"));
+#endif
+}
+
+bool BLEStream::poll()
+{
+  // BLEPeripheral::poll is called each time connected() is called
+  this->_connected = BLEPeripheral::connected();
+  if (millis() > this->_flushed + this->_flushInterval) {
+    flush();
+  }
+  return this->_connected;
+}
+
+void BLEStream::end()
+{
+  this->_rxCharacteristic.setEventHandler(BLEWritten, NULL);
+  this->_rxHead = this->_rxTail = 0;
+  flush();
+  BLEPeripheral::disconnect();
+}
+
+int BLEStream::available(void)
+{
+// BLEPeripheral::poll only calls delay(1) in CurieBLE so skipping it here to avoid the delay
+#ifndef _VARIANT_ARDUINO_101_X_
+  // TODO Need to do more testing to determine if all of these calls to BLEPeripheral::poll are
+  // actually necessary. Seems to run fine without them, but only minimal testing so far.
+  BLEPeripheral::poll();
+#endif
+  int retval = (this->_rxHead - this->_rxTail + sizeof(this->_rxBuffer)) % sizeof(this->_rxBuffer);
+#ifdef BLE_SERIAL_DEBUG
+  if (retval > 0) {
+    Serial.print(F("BLEStream::available() = "));
+    Serial.println(retval);
+  }
+#endif
+  return retval;
+}
+
+int BLEStream::peek(void)
+{
+#ifndef _VARIANT_ARDUINO_101_X_
+  BLEPeripheral::poll();
+#endif
+  if (this->_rxTail == this->_rxHead) return -1;
+  uint8_t byte = this->_rxBuffer[this->_rxTail];
+#ifdef BLE_SERIAL_DEBUG
+  Serial.print(F("BLEStream::peek() = 0x"));
+  Serial.println(byte, HEX);
+#endif
+  return byte;
+}
+
+int BLEStream::read(void)
+{
+#ifndef _VARIANT_ARDUINO_101_X_
+  BLEPeripheral::poll();
+#endif
+  if (this->_rxTail == this->_rxHead) return -1;
+  this->_rxTail = (this->_rxTail + 1) % sizeof(this->_rxBuffer);
+  uint8_t byte = this->_rxBuffer[this->_rxTail];
+#ifdef BLE_SERIAL_DEBUG
+  Serial.print(F("BLEStream::read() = 0x"));
+  Serial.println(byte, HEX);
+#endif
+  return byte;
+}
+
+void BLEStream::flush(void)
+{
+  if (this->_txCount == 0) return;
+#ifndef _VARIANT_ARDUINO_101_X_
+  // ensure there are available packets before sending
+  while(!this->_txCharacteristic.canNotify()) {
+    BLEPeripheral::poll();
+  }
+#endif
+  this->_txCharacteristic.setValue(this->_txBuffer, this->_txCount);
+  this->_flushed = millis();
+  this->_txCount = 0;
+#ifdef BLE_SERIAL_DEBUG
+  Serial.println(F("BLEStream::flush()"));
+#endif
+}
+
+size_t BLEStream::write(uint8_t byte)
+{
+#ifndef _VARIANT_ARDUINO_101_X_
+  BLEPeripheral::poll();
+#endif
+  if (this->_txCharacteristic.subscribed() == false) return 0;
+  this->_txBuffer[this->_txCount++] = byte;
+  if (this->_txCount == sizeof(this->_txBuffer)) flush();
+#ifdef BLE_SERIAL_DEBUG
+  Serial.print(F("BLEStream::write( 0x"));
+  Serial.print(byte, HEX);
+  Serial.println(F(") = 1"));
+#endif
+  return 1;
+}
+
+BLEStream::operator bool()
+{
+  bool retval = this->_connected = BLEPeripheral::connected();
+#ifdef BLE_SERIAL_DEBUG
+  Serial.print(F("BLEStream::operator bool() = "));
+  Serial.println(retval);
+#endif
+  return retval;
+}
+
+void BLEStream::setFlushInterval(int interval)
+{
+  if (interval > BLESTREAM_MIN_FLUSH_INTERVAL) {
+    this->_flushInterval = interval;
+  }
+}
+
+void BLEStream::_received(const unsigned char* data, size_t size)
+{
+  for (size_t i = 0; i < size; i++) {
+    this->_rxHead = (this->_rxHead + 1) % sizeof(this->_rxBuffer);
+    this->_rxBuffer[this->_rxHead] = data[i];
+  }
+#ifdef BLE_SERIAL_DEBUG
+  Serial.print(F("BLEStream::received("));
+  for (int i = 0; i < size; i++) Serial.print(data[i], HEX);
+  Serial.println(F(")"));
+#endif
+}
+
+void BLEStream::_received(BLECentral& /*central*/, BLECharacteristic& rxCharacteristic)
+{
+  BLEStream::_instance->_received(rxCharacteristic.value(), rxCharacteristic.valueLength());
+}
+
+
+#endif // _BLE_STREAM_H_
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.cpp	(revision 224)
@@ -0,0 +1,114 @@
+/*
+  EthernetClientStream.cpp
+  An Arduino-Stream that wraps an instance of Client reconnecting to
+  the remote-ip in a transparent way. A disconnected client may be
+  recognized by the returnvalues -1 from calls to peek or read and
+  a 0 from calls to write.
+
+  Copyright (C) 2013 Norbert Truchsess. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  formatted using the GNU C formatting and indenting
+ */
+
+#include "EthernetClientStream.h"
+#include <Arduino.h>
+
+//#define SERIAL_DEBUG
+#include "firmataDebug.h"
+
+#define MILLIS_RECONNECT 5000
+
+EthernetClientStream::EthernetClientStream(Client &client, IPAddress localip, IPAddress ip, const char* host, uint16_t port)
+: client(client),
+  localip(localip),
+  ip(ip),
+  host(host),
+  port(port),
+  connected(false)
+{
+}
+
+int
+EthernetClientStream::available()
+{
+  return maintain() ? client.available() : 0;
+}
+
+int
+EthernetClientStream::read()
+{
+  return maintain() ? client.read() : -1;
+}
+
+int
+EthernetClientStream::peek()
+{
+  return maintain() ? client.peek() : -1;
+}
+
+void EthernetClientStream::flush()
+{
+  if (maintain())
+    client.flush();
+}
+
+size_t
+EthernetClientStream::write(uint8_t c)
+{
+  return maintain() ? client.write(c) : 0;
+}
+
+void
+EthernetClientStream::maintain(IPAddress localip)
+{
+// temporary hack to Firmata to compile for Intel Galileo
+// the issue is documented here: https://github.com/firmata/arduino/issues/218
+#if !defined(ARDUINO_LINUX)
+  // ensure the local IP is updated in the case that it is changed by the DHCP server
+  if (this->localip != localip)
+    {
+      this->localip = localip;
+      if (connected)
+        stop();
+    }
+#endif
+}
+
+void
+EthernetClientStream::stop()
+{
+  client.stop();
+  connected = false;
+  time_connect = millis();
+}
+
+bool
+EthernetClientStream::maintain()
+{
+  if (client && client.connected())
+    return true;
+
+  if (connected)
+    {
+      stop();
+    }
+  // if the client is disconnected, attempt to reconnect every 5 seconds
+  else if (millis()-time_connect >= MILLIS_RECONNECT)
+    {
+      connected = host ? client.connect(host, port) : client.connect(ip, port);
+      if (!connected) {
+        time_connect = millis();
+        DEBUG_PRINTLN("connection failed. attempting to reconnect...");
+      } else {
+        DEBUG_PRINTLN("connected");
+      }
+    }
+  return connected;
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.h	(revision 224)
@@ -0,0 +1,52 @@
+/*
+  EthernetClientStream.h
+  An Arduino-Stream that wraps an instance of Client reconnecting to
+  the remote-ip in a transparent way. A disconnected client may be
+  recognized by the returnvalues -1 from calls to peek or read and
+  a 0 from calls to write.
+
+  Copyright (C) 2013 Norbert Truchsess. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  formatted using the GNU C formatting and indenting
+ */
+
+#ifndef ETHERNETCLIENTSTREAM_H
+#define ETHERNETCLIENTSTREAM_H
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <Stream.h>
+#include <Client.h>
+#include <IPAddress.h>
+
+class EthernetClientStream : public Stream
+{
+public:
+  EthernetClientStream(Client &client, IPAddress localip, IPAddress ip, const char* host, uint16_t port);
+  int available();
+  int read();
+  int peek();
+  void flush();
+  size_t write(uint8_t);
+  void maintain(IPAddress localip);
+
+private:
+  Client &client;
+  IPAddress localip;
+  IPAddress ip;
+  const char* host;
+  uint16_t port;
+  bool connected;
+  uint32_t time_connect;
+  bool maintain();
+  void stop();
+};
+
+#endif
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/FirmataFeature.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/FirmataFeature.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/FirmataFeature.h	(revision 224)
@@ -0,0 +1,38 @@
+/*
+  FirmataFeature.h
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  Interface for Firmata feature classes.
+
+  This version of FirmataFeature.h differs from the ConfigurableFirmata
+  version in the following ways:
+
+  - Imports Firmata.h rather than ConfigurableFirmata.h
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+#ifndef FirmataFeature_h
+#define FirmataFeature_h
+
+#include <Firmata.h>
+
+class FirmataFeature
+{
+  public:
+    virtual void handleCapability(byte pin) = 0;
+    virtual boolean handlePinMode(byte pin, int mode) = 0;
+    virtual boolean handleSysex(byte command, byte argc, byte* argv) = 0;
+    virtual void reset() = 0;
+};
+
+#endif
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.cpp	(revision 224)
@@ -0,0 +1,328 @@
+/*
+  SerialFirmata.cpp
+  Copyright (C) 2016 Jeff Hoefs. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  This version of SerialFirmata.cpp differs from the ConfigurableFirmata
+  version in the following ways:
+
+  - handlePinMode calls Firmata::setPinMode
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+#include "SerialFirmata.h"
+
+SerialFirmata::SerialFirmata()
+{
+  swSerial0 = NULL;
+  swSerial1 = NULL;
+  swSerial2 = NULL;
+  swSerial3 = NULL;
+
+  serialIndex = -1;
+}
+
+boolean SerialFirmata::handlePinMode(byte pin, int mode)
+{
+  // used for both HW and SW serial
+  if (mode == PIN_MODE_SERIAL) {
+    Firmata.setPinMode(pin, PIN_MODE_SERIAL);
+    return true;
+  }
+  return false;
+}
+
+void SerialFirmata::handleCapability(byte pin)
+{
+  if (IS_PIN_SERIAL(pin)) {
+    Firmata.write(PIN_MODE_SERIAL);
+    Firmata.write(getSerialPinType(pin));
+  }
+}
+
+boolean SerialFirmata::handleSysex(byte command, byte argc, byte *argv)
+{
+  if (command == SERIAL_MESSAGE) {
+
+    Stream *serialPort;
+    byte mode = argv[0] & SERIAL_MODE_MASK;
+    byte portId = argv[0] & SERIAL_PORT_ID_MASK;
+
+    switch (mode) {
+      case SERIAL_CONFIG:
+        {
+          long baud = (long)argv[1] | ((long)argv[2] << 7) | ((long)argv[3] << 14);
+          serial_pins pins;
+
+          if (portId < 8) {
+            serialPort = getPortFromId(portId);
+            if (serialPort != NULL) {
+              pins = getSerialPinNumbers(portId);
+              if (pins.rx != 0 && pins.tx != 0) {
+                Firmata.setPinMode(pins.rx, PIN_MODE_SERIAL);
+                Firmata.setPinMode(pins.tx, PIN_MODE_SERIAL);
+                // Fixes an issue where some serial devices would not work properly with Arduino Due
+                // because all Arduino pins are set to OUTPUT by default in StandardFirmata.
+                pinMode(pins.rx, INPUT);
+              }
+              ((HardwareSerial*)serialPort)->begin(baud);
+            }
+          } else {
+#if defined(SoftwareSerial_h)
+            byte swTxPin, swRxPin;
+            if (argc > 4) {
+              swRxPin = argv[4];
+              swTxPin = argv[5];
+            } else {
+              // RX and TX pins must be specified when using SW serial
+              Firmata.sendString("Specify serial RX and TX pins");
+              return false;
+            }
+            switch (portId) {
+              case SW_SERIAL0:
+                if (swSerial0 == NULL) {
+                  swSerial0 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+              case SW_SERIAL1:
+                if (swSerial1 == NULL) {
+                  swSerial1 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+              case SW_SERIAL2:
+                if (swSerial2 == NULL) {
+                  swSerial2 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+              case SW_SERIAL3:
+                if (swSerial3 == NULL) {
+                  swSerial3 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+            }
+            serialPort = getPortFromId(portId);
+            if (serialPort != NULL) {
+              Firmata.setPinMode(swRxPin, PIN_MODE_SERIAL);
+              Firmata.setPinMode(swTxPin, PIN_MODE_SERIAL);
+              ((SoftwareSerial*)serialPort)->begin(baud);
+            }
+#endif
+          }
+          break; // SERIAL_CONFIG
+        }
+      case SERIAL_WRITE:
+        {
+          byte data;
+          serialPort = getPortFromId(portId);
+          if (serialPort == NULL) {
+            break;
+          }
+          for (byte i = 1; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            serialPort->write(data);
+          }
+          break; // SERIAL_WRITE
+        }
+      case SERIAL_READ:
+        if (argv[1] == SERIAL_READ_CONTINUOUSLY) {
+          if (serialIndex + 1 >= MAX_SERIAL_PORTS) {
+            break;
+          }
+
+          if (argc > 2) {
+            // maximum number of bytes to read from buffer per iteration of loop()
+            serialBytesToRead[portId] = (int)argv[2] | ((int)argv[3] << 7);
+          } else {
+            // read all available bytes per iteration of loop()
+            serialBytesToRead[portId] = 0;
+          }
+          serialIndex++;
+          reportSerial[serialIndex] = portId;
+        } else if (argv[1] == SERIAL_STOP_READING) {
+          byte serialIndexToSkip = 0;
+          if (serialIndex <= 0) {
+            serialIndex = -1;
+          } else {
+            for (byte i = 0; i < serialIndex + 1; i++) {
+              if (reportSerial[i] == portId) {
+                serialIndexToSkip = i;
+                break;
+              }
+            }
+            // shift elements over to fill space left by removed element
+            for (byte i = serialIndexToSkip; i < serialIndex + 1; i++) {
+              if (i < MAX_SERIAL_PORTS) {
+                reportSerial[i] = reportSerial[i + 1];
+              }
+            }
+            serialIndex--;
+          }
+        }
+        break; // SERIAL_READ
+      case SERIAL_CLOSE:
+        serialPort = getPortFromId(portId);
+        if (serialPort != NULL) {
+          if (portId < 8) {
+            ((HardwareSerial*)serialPort)->end();
+          } else {
+#if defined(SoftwareSerial_h)
+            ((SoftwareSerial*)serialPort)->end();
+            if (serialPort != NULL) {
+              free(serialPort);
+              serialPort = NULL;
+            }
+#endif
+          }
+        }
+        break; // SERIAL_CLOSE
+      case SERIAL_FLUSH:
+        serialPort = getPortFromId(portId);
+        if (serialPort != NULL) {
+          getPortFromId(portId)->flush();
+        }
+        break; // SERIAL_FLUSH
+#if defined(SoftwareSerial_h)
+      case SERIAL_LISTEN:
+        // can only call listen() on software serial ports
+        if (portId > 7) {
+          serialPort = getPortFromId(portId);
+          if (serialPort != NULL) {
+            ((SoftwareSerial*)serialPort)->listen();
+          }
+        }
+        break; // SERIAL_LISTEN
+#endif
+    } // end switch
+    return true;
+  }
+  return false;
+}
+
+void SerialFirmata::update()
+{
+  checkSerial();
+}
+
+void SerialFirmata::reset()
+{
+#if defined(SoftwareSerial_h)
+  Stream *serialPort;
+  // free memory allocated for SoftwareSerial ports
+  for (byte i = SW_SERIAL0; i < SW_SERIAL3 + 1; i++) {
+    serialPort = getPortFromId(i);
+    if (serialPort != NULL) {
+      free(serialPort);
+      serialPort = NULL;
+    }
+  }
+#endif
+
+  serialIndex = -1;
+  for (byte i = 0; i < SERIAL_READ_ARR_LEN; i++) {
+    serialBytesToRead[i] = 0;
+  }
+}
+
+// get a pointer to the serial port associated with the specified port id
+Stream* SerialFirmata::getPortFromId(byte portId)
+{
+  switch (portId) {
+    case HW_SERIAL0:
+      // block use of Serial (typically pins 0 and 1) until ability to reclaim Serial is implemented
+      //return &Serial;
+      return NULL;
+#if defined(PIN_SERIAL1_RX)
+    case HW_SERIAL1:
+      return &Serial1;
+#endif
+#if defined(PIN_SERIAL2_RX)
+    case HW_SERIAL2:
+      return &Serial2;
+#endif
+#if defined(PIN_SERIAL3_RX)
+    case HW_SERIAL3:
+      return &Serial3;
+#endif
+#if defined(SoftwareSerial_h)
+    case SW_SERIAL0:
+      if (swSerial0 != NULL) {
+        // instances of SoftwareSerial are already pointers so simply return the instance
+        return swSerial0;
+      }
+      break;
+    case SW_SERIAL1:
+      if (swSerial1 != NULL) {
+        return swSerial1;
+      }
+      break;
+    case SW_SERIAL2:
+      if (swSerial2 != NULL) {
+        return swSerial2;
+      }
+      break;
+    case SW_SERIAL3:
+      if (swSerial3 != NULL) {
+        return swSerial3;
+      }
+      break;
+#endif
+  }
+  return NULL;
+}
+
+// Check serial ports that have READ_CONTINUOUS mode set and relay any data
+// for each port to the device attached to that port.
+void SerialFirmata::checkSerial()
+{
+  byte portId, serialData;
+  int bytesToRead = 0;
+  int numBytesToRead = 0;
+  Stream* serialPort;
+
+  if (serialIndex > -1) {
+
+    // loop through all reporting (READ_CONTINUOUS) serial ports
+    for (byte i = 0; i < serialIndex + 1; i++) {
+      portId = reportSerial[i];
+      bytesToRead = serialBytesToRead[portId];
+      serialPort = getPortFromId(portId);
+      if (serialPort == NULL) {
+        continue;
+      }
+#if defined(SoftwareSerial_h)
+      // only the SoftwareSerial port that is "listening" can read data
+      if (portId > 7 && !((SoftwareSerial*)serialPort)->isListening()) {
+        continue;
+      }
+#endif
+      if (serialPort->available() > 0) {
+        Firmata.write(START_SYSEX);
+        Firmata.write(SERIAL_MESSAGE);
+        Firmata.write(SERIAL_REPLY | portId);
+
+        if (bytesToRead == 0 || (serialPort->available() <= bytesToRead)) {
+          numBytesToRead = serialPort->available();
+        } else {
+          numBytesToRead = bytesToRead;
+        }
+
+        // relay serial data to the serial device
+        while (numBytesToRead > 0) {
+          serialData = serialPort->read();
+          Firmata.write(serialData & 0x7F);
+          Firmata.write((serialData >> 7) & 0x7F);
+          numBytesToRead--;
+        }
+        Firmata.write(END_SYSEX);
+      }
+
+    }
+  }
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.h	(revision 224)
@@ -0,0 +1,167 @@
+/*
+  SerialFirmata.h
+  Copyright (C) 2016 Jeff Hoefs. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  This version of SerialFirmata.h differs from the ConfigurableFirmata
+  version in the following ways:
+
+  - Defines FIRMATA_SERIAL_FEATURE (could add to Configurable version as well)
+  - Imports Firmata.h rather than ConfigurableFirmata.h
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+#ifndef SerialFirmata_h
+#define SerialFirmata_h
+
+#include <Firmata.h>
+#include "FirmataFeature.h"
+// SoftwareSerial is currently only supported for AVR-based boards and the Arduino 101
+// The third condition checks if the IDE is in the 1.0.x series, if so, include SoftwareSerial
+// since it should be available to all boards in that IDE.
+#if defined(ARDUINO_ARCH_AVR) || defined(ARDUINO_ARCH_ARC32) || (ARDUINO >= 100 && ARDUINO < 10500)
+#include <SoftwareSerial.h>
+#endif
+
+#define FIRMATA_SERIAL_FEATURE
+
+// Serial port Ids
+#define HW_SERIAL0                  0x00
+#define HW_SERIAL1                  0x01
+#define HW_SERIAL2                  0x02
+#define HW_SERIAL3                  0x03
+// extensible up to 0x07
+
+#define SW_SERIAL0                  0x08
+#define SW_SERIAL1                  0x09
+#define SW_SERIAL2                  0x0A
+#define SW_SERIAL3                  0x0B
+// extensible up to 0x0F
+
+#define SERIAL_PORT_ID_MASK         0x0F
+#define MAX_SERIAL_PORTS            8
+#define SERIAL_READ_ARR_LEN         12
+
+// map configuration query response resolution value to serial pin type
+#define RES_RX1                     0x02
+#define RES_TX1                     0x03
+#define RES_RX2                     0x04
+#define RES_TX2                     0x05
+#define RES_RX3                     0x06
+#define RES_TX3                     0x07
+
+// Serial command bytes
+#define SERIAL_CONFIG               0x10
+#define SERIAL_WRITE                0x20
+#define SERIAL_READ                 0x30
+#define SERIAL_REPLY                0x40
+#define SERIAL_CLOSE                0x50
+#define SERIAL_FLUSH                0x60
+#define SERIAL_LISTEN               0x70
+
+// Serial read modes
+#define SERIAL_READ_CONTINUOUSLY    0x00
+#define SERIAL_STOP_READING         0x01
+#define SERIAL_MODE_MASK            0xF0
+
+namespace {
+
+  struct serial_pins {
+    uint8_t rx;
+    uint8_t tx;
+  };
+
+  /*
+   * Get the serial serial pin type (RX1, TX1, RX2, TX2, etc) for the specified pin.
+   */
+  inline uint8_t getSerialPinType(uint8_t pin) {
+  #if defined(PIN_SERIAL_RX)
+    // TODO when use of HW_SERIAL0 is enabled
+  #endif
+  #if defined(PIN_SERIAL1_RX)
+    if (pin == PIN_SERIAL1_RX) return RES_RX1;
+    if (pin == PIN_SERIAL1_TX) return RES_TX1;
+  #endif
+  #if defined(PIN_SERIAL2_RX)
+    if (pin == PIN_SERIAL2_RX) return RES_RX2;
+    if (pin == PIN_SERIAL2_TX) return RES_TX2;
+  #endif
+  #if defined(PIN_SERIAL3_RX)
+    if (pin == PIN_SERIAL3_RX) return RES_RX3;
+    if (pin == PIN_SERIAL3_TX) return RES_TX3;
+  #endif
+    return 0;
+  }
+
+  /*
+   * Get the RX and TX pins numbers for the specified HW serial port.
+   */
+  inline serial_pins getSerialPinNumbers(uint8_t portId) {
+    serial_pins pins;
+    switch (portId) {
+  #if defined(PIN_SERIAL_RX)
+        // case HW_SERIAL0:
+        //   // TODO when use of HW_SERIAL0 is enabled
+        //   break;
+  #endif
+  #if defined(PIN_SERIAL1_RX)
+      case HW_SERIAL1:
+        pins.rx = PIN_SERIAL1_RX;
+        pins.tx = PIN_SERIAL1_TX;
+        break;
+  #endif
+  #if defined(PIN_SERIAL2_RX)
+      case HW_SERIAL2:
+        pins.rx = PIN_SERIAL2_RX;
+        pins.tx = PIN_SERIAL2_TX;
+        break;
+  #endif
+  #if defined(PIN_SERIAL3_RX)
+      case HW_SERIAL3:
+        pins.rx = PIN_SERIAL3_RX;
+        pins.tx = PIN_SERIAL3_TX;
+        break;
+  #endif
+      default:
+        pins.rx = 0;
+        pins.tx = 0;
+    }
+    return pins;
+  }
+
+} // end namespace
+
+
+class SerialFirmata: public FirmataFeature
+{
+  public:
+    SerialFirmata();
+    boolean handlePinMode(byte pin, int mode);
+    void handleCapability(byte pin);
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    void update();
+    void reset();
+    void checkSerial();
+
+  private:
+    byte reportSerial[MAX_SERIAL_PORTS];
+    int serialBytesToRead[SERIAL_READ_ARR_LEN];
+    signed char serialIndex;
+
+    Stream *swSerial0;
+    Stream *swSerial1;
+    Stream *swSerial2;
+    Stream *swSerial3;
+
+    Stream* getPortFromId(byte portId);
+
+};
+
+#endif /* SerialFirmata_h */
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.cpp	(revision 224)
@@ -0,0 +1,4 @@
+/*
+ * Implementation is in WiFi101Stream.h to avoid linker issues. Legacy WiFi and modern WiFi101 both define WiFiClass which
+ * will cause linker errors whenever Firmata.h is included.
+ */
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.h	(revision 224)
@@ -0,0 +1,259 @@
+/*
+  WiFi101Stream.h
+  An Arduino Stream that wraps an instance of a WiFi101 server. For use
+  with Arduino WiFi 101 shield, Arduino MKR1000 and other boards and
+  shields that are compatible with the Arduino WiFi101 library.
+
+  Copyright (C) 2015-2016 Jesse Frush. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+ */
+
+#ifndef WIFI101_STREAM_H
+#define WIFI101_STREAM_H
+
+#include <inttypes.h>
+#include <Stream.h>
+#include <WiFi101.h>
+
+
+class WiFi101Stream : public Stream
+{
+private:
+  WiFiServer _server = WiFiServer(23);
+  WiFiClient _client;
+
+  //configuration members
+  IPAddress _local_ip;
+  uint16_t _port = 0;
+  uint8_t _key_idx = 0;               //WEP
+  const char *_key = nullptr;         //WEP
+  const char *_passphrase = nullptr;  //WPA
+  char *_ssid = nullptr;
+
+  inline int connect_client()
+  {
+    if( !( _client && _client.connected() ) )
+    {
+      WiFiClient newClient = _server.available();
+      if( !newClient )
+      {
+        return 0;
+      }
+
+      _client = newClient;
+    }
+    return 1;
+  }
+
+  inline bool is_ready()
+  {
+    uint8_t status = WiFi.status();
+    return !( status == WL_NO_SHIELD || status == WL_CONNECTED );
+  }
+
+public:
+  WiFi101Stream() {};
+
+  // allows another way to configure a static IP before begin is called
+  inline void config(IPAddress local_ip)
+  {
+    _local_ip = local_ip;
+    WiFi.config( local_ip );
+  }
+
+  // get DCHP IP
+  inline IPAddress localIP()
+  {
+    return WiFi.localIP();
+  }
+
+  inline bool maintain()
+  {
+    if( connect_client() ) return true;
+
+    stop();
+    int result = 0;
+    if( WiFi.status() != WL_CONNECTED )
+    {
+      if( _local_ip )
+      {
+        WiFi.config( _local_ip );
+      }
+
+      if( _passphrase )
+      {
+        result = WiFi.begin( _ssid, _passphrase);
+      }
+      else if( _key_idx && _key )
+      {
+        result = WiFi.begin( _ssid, _key_idx, _key );
+      }
+      else
+      {
+        result = WiFi.begin( _ssid );
+      }
+    }
+    if( result == 0 ) return false;
+
+    _server = WiFiServer( _port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions with DHCP
+ ******************************************************************************/
+
+  //OPEN networks
+  inline int begin(char *ssid, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks
+  inline int begin(char *ssid, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _key_idx = key_idx;
+    _key = key;
+
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks
+  inline int begin(char *ssid, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _passphrase = passphrase;
+
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions without DHCP
+ ******************************************************************************/
+
+  //OPEN networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _key_idx = key_idx;
+    _key = key;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _passphrase = passphrase;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *             Stream implementations
+ ******************************************************************************/
+
+  inline int available()
+  {
+    return connect_client() ? _client.available() : 0;
+  }
+
+  inline void flush()
+  {
+    if( _client ) _client.flush();
+  }
+
+  inline int peek()
+  {
+    return connect_client() ? _client.peek(): 0;
+  }
+
+  inline int read()
+  {
+    return connect_client() ? _client.read() : -1;
+  }
+
+  inline void stop()
+  {
+    _client.stop();
+  }
+
+  inline size_t write(uint8_t byte)
+  {
+    if( connect_client() ) _client.write( byte );
+  }
+};
+
+#endif //WIFI101_STREAM_H
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.cpp	(revision 224)
@@ -0,0 +1,4 @@
+/*
+ * Implementation is in WiFiStream.h to avoid linker issues. Legacy WiFi and modern WiFi101 both define WiFiClass which
+ * will cause linker errors whenever Firmata.h is included.
+ */
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.h	(revision 224)
@@ -0,0 +1,258 @@
+/*
+  WiFiStream.h
+  An Arduino Stream that wraps an instance of a WiFi server. For use
+  with legacy Arduino WiFi shield and other boards and sheilds that
+  are compatible with the Arduino WiFi library.
+
+  Copyright (C) 2015-2016 Jesse Frush. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+ */
+
+#ifndef WIFI_STREAM_H
+#define WIFI_STREAM_H
+
+#include <inttypes.h>
+#include <Stream.h>
+#include <WiFi.h>
+
+class WiFiStream : public Stream
+{
+private:
+  WiFiServer _server = WiFiServer(23);
+  WiFiClient _client;
+
+  //configuration members
+  IPAddress _local_ip;
+  uint16_t _port = 0;
+  uint8_t _key_idx = 0;               //WEP
+  const char *_key = nullptr;         //WEP
+  const char *_passphrase = nullptr;  //WPA
+  char *_ssid = nullptr;
+
+  inline int connect_client()
+  {
+    if( !( _client && _client.connected() ) )
+    {
+      WiFiClient newClient = _server.available();
+      if( !newClient )
+      {
+        return 0;
+      }
+
+      _client = newClient;
+    }
+    return 1;
+  }
+
+  inline bool is_ready()
+  {
+    uint8_t status = WiFi.status();
+    return !( status == WL_NO_SHIELD || status == WL_CONNECTED );
+  }
+
+public:
+  WiFiStream() {};
+
+  // allows another way to configure a static IP before begin is called
+  inline void config(IPAddress local_ip)
+  {
+    _local_ip = local_ip;
+    WiFi.config( local_ip );
+  }
+
+  // get DCHP IP
+  inline IPAddress localIP()
+  {
+    return WiFi.localIP();
+  }
+
+  inline bool maintain()
+  {
+    if( connect_client() ) return true;
+
+    stop();
+    int result = 0;
+    if( WiFi.status() != WL_CONNECTED )
+    {
+      if( _local_ip )
+      {
+        WiFi.config( _local_ip );
+      }
+
+      if( _passphrase )
+      {
+        result = WiFi.begin( _ssid, _passphrase);
+      }
+      else if( _key_idx && _key )
+      {
+        result = WiFi.begin( _ssid, _key_idx, _key );
+      }
+      else
+      {
+        result = WiFi.begin( _ssid );
+      }
+    }
+    if( result == 0 ) return false;
+
+    _server = WiFiServer( _port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions with DHCP
+ ******************************************************************************/
+
+  //OPEN networks
+  inline int begin(char *ssid, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks
+  inline int begin(char *ssid, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _key_idx = key_idx;
+    _key = key;
+
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks
+  inline int begin(char *ssid, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _passphrase = passphrase;
+
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions without DHCP
+ ******************************************************************************/
+
+  //OPEN networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _key_idx = key_idx;
+    _key = key;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _passphrase = passphrase;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *             Stream implementations
+ ******************************************************************************/
+
+  inline int available()
+  {
+    return connect_client() ? _client.available() : 0;
+  }
+
+  inline void flush()
+  {
+    if( _client ) _client.flush();
+  }
+
+  inline int peek()
+  {
+    return connect_client() ? _client.peek(): 0;
+  }
+
+  inline int read()
+  {
+    return connect_client() ? _client.read() : -1;
+  }
+
+  inline void stop()
+  {
+    _client.stop();
+  }
+
+  inline size_t write(uint8_t byte)
+  {
+    if( connect_client() ) _client.write( byte );
+  }
+};
+
+#endif //WIFI_STREAM_H
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/firmataDebug.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/firmataDebug.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/firmataDebug.h	(revision 224)
@@ -0,0 +1,14 @@
+#ifndef FIRMATA_DEBUG_H
+#define FIRMATA_DEBUG_H
+
+#ifdef SERIAL_DEBUG
+  #define DEBUG_BEGIN(baud) Serial.begin(baud); while(!Serial) {;}
+  #define DEBUG_PRINTLN(x)  Serial.println (x)
+  #define DEBUG_PRINT(x)    Serial.print (x)
+#else
+  #define DEBUG_BEGIN(baud)
+  #define DEBUG_PRINTLN(x)
+  #define DEBUG_PRINT(x)
+#endif
+
+#endif /* FIRMATA_DEBUG_H */
Index: rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/examples/LuckyTest/LuckyTest.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/examples/LuckyTest/LuckyTest.ino	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/examples/LuckyTest/LuckyTest.ino	(revision 224)
@@ -79,4 +79,5 @@
 
   //read magnetometer sensor
+  lucky.magnetometer().read();
   Serial.print("Mx: ");
   Serial.print(lucky.magnetometer().x());
Index: rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/library.properties
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/library.properties	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/library.properties	(revision 224)
@@ -1,4 +1,4 @@
 name=Lucky Shield
-version=1.0.2
+version=1.0.3
 author=Arduino srl
 maintainer=Arduino srl <swdev@arduino.org>
Index: rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/Lucky.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/Lucky.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/Lucky.h	(revision 224)
@@ -40,4 +40,8 @@
 #include "Wire.h"
 
+#ifdef __SAM3X8E__
+#define Wire Wire1
+#endif
+
 class Lucky
 {
Index: rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/Adafruit_SSD1306.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/Adafruit_SSD1306.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/Adafruit_SSD1306.cpp	(revision 224)
@@ -32,4 +32,7 @@
 #include "Adafruit_SSD1306.h"
 
+#ifdef __SAM3X8E__
+#define Wire Wire1
+#endif
 // the memory buffer for the LCD
 
Index: rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/Adafruit_SSD1306.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/Adafruit_SSD1306.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/Adafruit_SSD1306.h	(revision 224)
@@ -164,7 +164,7 @@
   int8_t _i2caddr, _vccstate, sid, sclk, dc, rst, cs;
 
-  boolean hwSPI;
-  PortReg *mosiport, *clkport, *csport, *dcport;
-  PortMask mosipinmask, clkpinmask, cspinmask, dcpinmask;
+  //boolean hwSPI;
+  //PortReg *mosiport, *clkport, *csport, *dcport;
+  //PortMask mosipinmask, clkpinmask, cspinmask, dcpinmask;
 
   inline void drawFastVLineInternal(int16_t x, int16_t y, int16_t h, uint16_t color) __attribute__((always_inline));
Index: rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/BME280.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/BME280.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/BME280.cpp	(revision 224)
@@ -19,4 +19,7 @@
 #include "BME280.h"
 
+#ifdef __SAM3X8E__
+#define Wire Wire1
+#endif
 
 /***************************************************************************
Index: rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/CAT9555.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/CAT9555.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/CAT9555.cpp	(revision 224)
@@ -28,4 +28,8 @@
 #include "CAT9555.h"
 #include <Wire.h>
+
+#ifdef __SAM3X8E__
+#define Wire Wire1
+#endif
 
 // CONSTRUCTUR
Index: rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/MAG3110.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/MAG3110.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/MAG3110.cpp	(revision 224)
@@ -16,40 +16,38 @@
 
 //Constructor
-MAG3110::MAG3110()
-{
-  
-}
-
-
+// MAG3110::MAG3110()
+// {
+// }
 
 // Configure magnetometer
 void MAG3110::begin(void) {
-  //CTRL_REG1
-  //DR2|DR1|DR0|OS1|OS0|FastRead|Trigger|ActiveMode|
-  // 0 | 1 | 1 | 1 | 1 |    0   |   0   |    1     |=dec121
+
   Wire.beginTransmission(MAG_ADDR);// transmit to device 0x0E
-  Wire.write(0x10);                // cntrl register1
-  Wire.write(0x79); // Active Mode, 1.25 Hz datarate, 8x oversampling
-  Wire.endTransmission();          // stop transmitting
+  Wire.write(0x11);              // cntrl register2
+  Wire.write(0x80);              // send 0x80, enable auto resets
+  Wire.endTransmission();       // stop transmitting
   
   delay(15);
   
-  //CTRL_REG2: 
-  //AutoMagRst|---|Raw|Mag_Rst|---|---|---|---|
-  //    1     | 0 | 0 |   0   | 0 | 0 | 0 | 0 |
   Wire.beginTransmission(MAG_ADDR);// transmit to device 0x0E
-  Wire.write(0x11);                // cntrl register2
-  Wire.write(0x80);        // Auto resets before each measurement
-  Wire.endTransmission();          // stop transmitting
+  Wire.write(0x10);              // cntrl register1
+  Wire.write(1);                 // send 0x01, active mode
+  Wire.endTransmission();       // stop transmitting
 }
 
+void MAG3110::read()
+{
+  mag_x =read16Data(0x01,0x02);
+  mag_y =read16Data(0x03,0x04);
+  mag_z =read16Data(0x05,0x06);
+}
+// read X value
 
-// read X value
-int MAG3110::readx(void)
+int MAG3110::read16Data(byte MSB, byte LSB)
 {
   int xl, xh;  //define the MSB and LSB
   
   Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
-  Wire.write(0x01);              // x MSB reg
+  Wire.write(MSB);              // x MSB reg
   Wire.endTransmission();       // stop transmitting
  
@@ -65,5 +63,5 @@
   
   Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
-  Wire.write(0x02);              // x LSB reg
+  Wire.write(LSB);              // x LSB reg
   Wire.endTransmission();       // stop transmitting
  
@@ -76,77 +74,12 @@
   }
   
-  int xout = (xl|(xh << 8)); //concatenate the MSB and LSB
-  return xout;
+  int out = (xl|(xh << 8)); //concatenate the MSB and LSB 
+  if (out & 0b1000000000000000){
+     //float yout1 = ((~yout & 0b0111111111111111)+ 1)*(-1) ; 
+    return float ((~out & 0b0111111111111111)+ 1)*(-1) ;
+  }
+  return float (out);
 }
 
-//read Y value
-int MAG3110::ready(void)
-{
-  int yl, yh;  //define the MSB and LSB
-  
-  Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
-  Wire.write(0x03);              // y MSB reg
-  Wire.endTransmission();       // stop transmitting
- 
-  delayMicroseconds(2); //needs at least 1.3us free time between start and stop
-  
-  Wire.requestFrom(MAG_ADDR, 1); // request 1 byte
-  while(Wire.available())    // slave may send less than requested
-  { 
-    yh = Wire.read(); // receive the byte
-  }
-  
-  delayMicroseconds(2); //needs at least 1.3us free time between start and stop
-  
-  Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
-  Wire.write(0x04);              // y LSB reg
-  Wire.endTransmission();       // stop transmitting
- 
-  delayMicroseconds(2); //needs at least 1.3us free time between start and stop
-  
-  Wire.requestFrom(MAG_ADDR, 1); // request 1 byte
-  while(Wire.available())    // slave may send less than requested
-  { 
-    yl = Wire.read(); // receive the byte
-  }
-  
-  int yout = (yl|(yh << 8)); //concatenate the MSB and LSB
-  return yout;
-}
-
-// read Z value
-int MAG3110::readz(void)
-{
-  int zl, zh;  //define the MSB and LSB
-  
-  Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
-  Wire.write(0x05);              // z MSB reg
-  Wire.endTransmission();       // stop transmitting
- 
-  delayMicroseconds(2); //needs at least 1.3us free time between start and stop
-  
-  Wire.requestFrom(MAG_ADDR, 1); // request 1 byte
-  while(Wire.available())    // slave may send less than requested
-  { 
-    zh = Wire.read(); // receive the byte
-  }
-  
-  delayMicroseconds(2); //needs at least 1.3us free time between start and stop
-  
-  Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
-  Wire.write(0x06);              // z LSB reg
-  Wire.endTransmission();       // stop transmitting
- 
-  delayMicroseconds(2); //needs at least 1.3us free time between start and stop
-  
-  Wire.requestFrom(MAG_ADDR, 1); // request 1 byte
-  while(Wire.available())    // slave may send less than requested
-  { 
-    zl = Wire.read(); // receive the byte
-  }
-  
-  int zout = (zl|(zh << 8)); //concatenate the MSB and LSB
-  return zout;
-}
 
 MAG3110 mag3110;
Index: rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/MAG3110.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/MAG3110.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/MAG3110.h	(revision 224)
@@ -25,14 +25,23 @@
 {
   public:
-    MAG3110();
+    //constructor
+    //MAG3110();
+    
+    //public methods
     void begin();
-    int readx();
-    int ready();
-    int readz();
-	
-	int x(){return readx();};
-	int y(){return ready();};
-	int z(){return readz();};
+    void read(); 
+    int x(){return mag_x;};
+    int y(){return mag_y;};
+    int z(){return mag_z;};
+
   private:
+    //class-only methods:
+    int read16Data(byte MSB, byte LSB);
+    
+    //global variables
+    int mag_x;
+    int mag_y;
+    int mag_z;
+    
 };
 
Index: rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/MMA8491Q.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/MMA8491Q.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/LuckyShield/src/lib/MMA8491Q.cpp	(revision 224)
@@ -30,4 +30,8 @@
 #include "MMA8491Q.h"
 #include <Wire.h>
+
+#ifdef __SAM3X8E__
+#define Wire Wire1
+#endif
 
 // CONSTRUCTUR
Index: rtos_arduino/trunk/arduino_lib/libraries/Servo/src/Servo.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Servo/src/Servo.h	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Servo/src/Servo.h	(revision 224)
@@ -66,6 +66,8 @@
 #elif defined(ARDUINO_ARCH_SAMD)
 	//#include "samd/ServoTimersSamd.h"
-	#define MIN_PULSE_WIDTH_SAMD 500
-	#define MAX_PULSE_WIDTH_SAMD 2100
+	#define MIN_PULSE_WIDTH_SAMD_TCC 405
+	#define MAX_PULSE_WIDTH_SAMD_TCC 2300
+	#define MIN_PULSE_WIDTH_SAMD_TC 1700
+	#define MAX_PULSE_WIDTH_SAMD_TC 9200
 #else	
 	#error "This library only supports boards with an AVR,SAM or SAMD processor."
Index: rtos_arduino/trunk/arduino_lib/libraries/Servo/src/samd/Servo.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Servo/src/samd/Servo.cpp	(revision 223)
+++ rtos_arduino/trunk/arduino_lib/libraries/Servo/src/samd/Servo.cpp	(revision 224)
@@ -16,4 +16,5 @@
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */
+//Edited by Arduino Srl development team.
 
 #if defined(ARDUINO_ARCH_SAMD)
@@ -22,7 +23,10 @@
 #include <Servo.h>
 
+
 static servo_t servos[MAX_SERVOS];                          // static array of servo structures
 
 uint8_t ServoCount = 0;                                     // the total number of attached servos
+uint8_t isTC = 0 ;
+Tc* TCx ;
 Tcc* TCCx;
 uint8_t Channelx = 0;
@@ -30,6 +34,8 @@
 // convenience macros
 
-#define SERVO_MIN() (MIN_PULSE_WIDTH_SAMD)  // minimum value in uS for this servo
-#define SERVO_MAX() (MAX_PULSE_WIDTH_SAMD)  // maximum value in uS for this servo
+#define SERVO_MIN_TCC() (MIN_PULSE_WIDTH_SAMD_TCC)  // minimum value in uS for this servo if TCC timer is used
+#define SERVO_MAX_TCC() (MAX_PULSE_WIDTH_SAMD_TCC)  // maximum value in uS for this servo if TCC timer is used
+#define SERVO_MIN_TC() (MIN_PULSE_WIDTH_SAMD_TC)  // minimum value in uS for this servo if TC timer is used
+#define SERVO_MAX_TC() (MAX_PULSE_WIDTH_SAMD_TC)  // maximum value in uS for this servo if TC timer is used
 
 /************ static functions common to all instances ***********************/
@@ -46,6 +52,6 @@
   if (ServoCount < MAX_SERVOS) {
     this->servoIndex = ServoCount++;                    // assign a servo index to this instance
-  } else {                                                  //su questo costruttore forse si deve tornare
-    this->servoIndex = INVALID_SERVO;  // too many servos
+  } else {                                                 
+    this->servoIndex = INVALID_SERVO;  					// too many servos
   }
 }
@@ -53,5 +59,10 @@
 uint8_t Servo::attach(int pin)
 {
-  return this->attach(pin, MIN_PULSE_WIDTH_SAMD, MAX_PULSE_WIDTH_SAMD);
+  if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
+	return this->attach(pin, SERVO_MIN_TC(), SERVO_MAX_TC());
+  }
+  else{
+	return this->attach(pin, SERVO_MIN_TCC(), SERVO_MAX_TCC());
+  }
 }
 
@@ -63,6 +74,15 @@
     pinMode(pin, OUTPUT);                                   // set servo pin to output
     servos[this->servoIndex].Pin.nbr = pin;
-    if(min > MIN_PULSE_WIDTH_SAMD) min = MIN_PULSE_WIDTH_SAMD;
-	if (max > MAX_PULSE_WIDTH_SAMD) max = MAX_PULSE_WIDTH_SAMD;
+	int servo_min, servo_max;
+	if(pin==4 | pin==5 | pin==10 | pin==12){
+		servo_min=SERVO_MIN_TC();
+		servo_max=SERVO_MAX_TC();
+	}
+	else{
+		servo_min=SERVO_MIN_TCC();
+		servo_max=SERVO_MAX_TCC();
+	}
+    if(min > servo_min) min = servo_min;
+	if (max > servo_max) max = servo_max;
 	this->min  = min;
     this->max  = max;
@@ -75,4 +95,5 @@
 			TCCx=TCC0;
 			Channelx=0;
+			isTC=0;
 		}
 		break;
@@ -83,7 +104,27 @@
 			TCCx=TCC0;
 			Channelx=1;
-		}
-		break;
-		
+			isTC=0;
+		}
+		break;
+
+			case 4:
+		{
+			pinPeripheral(pin, g_APinDescription[pin].ulPinType);
+			TCx=TC3;
+			Channelx=0;
+			isTC=1;
+			 
+		}
+		break;
+
+		case 5:
+		{
+			pinPeripheral(pin, g_APinDescription[pin].ulPinType);
+			TCx=TC3;
+			Channelx=1;
+			isTC=1;
+			 
+		}
+		break;
 		case 6:
 		{
@@ -91,4 +132,5 @@
 			TCCx=TCC0;
 			Channelx=2;
+			isTC=0;
 		}
 		break;
@@ -99,4 +141,5 @@
 			TCCx=TCC0;
 			Channelx=3;
+			isTC=0;
 		}
 		break;
@@ -107,4 +150,5 @@
 			TCCx=TCC1;
 			Channelx=0;
+			isTC=0;
 		}
 		break;
@@ -115,4 +159,15 @@
 			TCCx=TCC1;
 			Channelx=1;
+			isTC=0;
+		}
+		break;
+		
+		case 10:
+		{
+			pinPeripheral(pin, g_APinDescription[pin].ulPinType);
+			TCx=TC3;
+			Channelx=0;
+			isTC=1;
+			 
 		}
 		break;
@@ -123,4 +178,15 @@
 			TCCx=TCC2;
 			Channelx=0;
+			isTC=0;
+		}
+		break;
+		
+		case 12:
+		{
+			pinPeripheral(pin, g_APinDescription[pin].ulPinType);
+			TCx=TC3;
+			Channelx=1;
+			isTC=1;
+			 
 		}
 		break;
@@ -131,4 +197,5 @@
 			TCCx=TCC2;
 			Channelx=1;
+			isTC=0;
 		}
 		break;
@@ -140,10 +207,31 @@
 	
 	if ((TCCx==TCC0) | (TCCx==TCC1)) GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC0_TCC1 )) ;
-	else if(TCCx==TCC2) GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC2_TC3 )) ;
+	else if((TCCx==TCC2) | (TCx==TC3 ))GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC2_TC3 )) ;
 	else;
-		
-	if(servos[this->servoIndex].Pin.isActive == false)
-	{	
-		TCCx->CTRLA.reg &=~(TCC_CTRLA_ENABLE);        //disable TCC module
+
+	if(servos[this->servoIndex].Pin.isActive == false){
+	// Set PORT
+    if ( isTC )
+    {
+      // -- Configure TC
+      //DISABLE TCx
+      TCx->COUNT16.CTRLA.reg &=~(TC_CTRLA_ENABLE);
+      //Set Timer counter Mode to 16 bits
+	  TCx->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16;
+	  //Set Prescaler to divide by 2
+	  TCx->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV2;
+      //Set TCx as normal PWM
+      TCx->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_NPWM;
+	  //default value for servo position
+      TCx->COUNT16.CC[Channelx].reg = 1500;
+	  //ENABLE TCx
+      TCx->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE;
+	  servos[this->servoIndex].Pin.isActive = true;
+    }
+    else
+    {
+      // -- Configure TCC
+	  
+	 TCCx->CTRLA.reg &=~(TCC_CTRLA_ENABLE);        //disable TCC module
 		TCCx->CTRLA.reg |=TCC_CTRLA_PRESCALER_DIV8;   //setting prescaler to divide by 8
 		TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM;      //Set TCCx as normal PWM
@@ -151,8 +239,8 @@
 		TCCx->PER.reg=20000;                          // setting servo frequency (50 hz) 
 		TCCx->CTRLA.reg |= TCC_CTRLA_ENABLE ;	      //ENABLE TCCx
-		servos[this->servoIndex].Pin.isActive = true; 
-	}	
-    
-    //servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
+		servos[this->servoIndex].Pin.isActive = true;  
+    }
+	
+	}
   }
   return this->servoIndex;
@@ -167,18 +255,39 @@
   else if((servos[this->servoIndex].Pin.nbr == 8) | (servos[this->servoIndex].Pin.nbr == 9)) TCC1->CTRLA.reg &=~(TCC_CTRLA_ENABLE);
   else if ((servos[this->servoIndex].Pin.nbr == 11) | (servos[this->servoIndex].Pin.nbr == 13)) TCC2->CTRLA.reg &=~(TCC_CTRLA_ENABLE);	
+  else if ((servos[this->servoIndex].Pin.nbr == 4 ) | (servos[this->servoIndex].Pin.nbr == 5 ) | (servos[this->servoIndex].Pin.nbr == 10 ) | (servos[this->servoIndex].Pin.nbr == 12 ))TC3->COUNT16.CTRLA.reg &=~(TC_CTRLA_ENABLE);
 }
 
 void Servo::write(int value)
-{
-  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
-  if (value < MIN_PULSE_WIDTH)
-  {
-    if (value < 0)
-      value = 0;
-    else if (value > 180)
-      value = 180;
-
-    value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
-  }
+{  
+  //select the right values for servo motor
+  int servo_min;
+  int servo_max;
+  if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
+	servo_min=SERVO_MIN_TC();
+	servo_max=SERVO_MAX_TC();
+	// treat values less than 1700 as angles in degrees (valid values in microseconds are handled as microseconds)
+	if (value < servo_min)
+	{
+		if (value < 0)
+			value = 0;
+		else if (value > 180)
+			value = 180;
+		value = map(value, 0, 180, servo_min, servo_max);
+	}
+  }
+  else{
+	servo_min=SERVO_MIN_TCC();
+	servo_max=SERVO_MAX_TCC();
+	// treat values less than 400 as angles in degrees (valid values in microseconds are handled as microseconds)
+	if (value < servo_min)
+	{
+		if (value < 0)
+			value = 0;
+		else if (value > 180)
+			value = 180;
+		value = map(value, 0, 180, servo_min, servo_max);
+	}
+  }
+    
   writeMicroseconds(value);
 }
@@ -190,9 +299,20 @@
   if( (channel < MAX_SERVOS) )   // ensure channel is valid
   {
-    if (value < SERVO_MIN())          // ensure pulse width is valid
-      value = SERVO_MIN();
-    else if (value > SERVO_MAX())
-      value = SERVO_MAX();
-    servos[this->servoIndex].ticks = value;   //da sistemare
+	//select the right values for servo motor
+	int servo_min;
+	int servo_max;
+	if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
+		servo_min=SERVO_MIN_TC();
+		servo_max=SERVO_MAX_TC();
+	}
+	else{
+		servo_min=SERVO_MIN_TCC();
+		servo_max=SERVO_MAX_TCC();
+	}
+    if (value < servo_min)          // ensure pulse width is valid
+      value = servo_min;
+    else if (value > servo_max)
+      value = servo_max;
+    servos[this->servoIndex].ticks = value; 
 	switch(servos[this->servoIndex].Pin.nbr)
 	{
@@ -205,4 +325,12 @@
 			break;
 			
+		case 4:
+			TC3->COUNT16.CC[0].reg = value;
+			break;	
+			
+		case 5:
+			TC3->COUNT16.CC[1].reg = value;
+			break;	
+			
 		case 6:
 			TCC0->CC[2].reg=value;
@@ -220,4 +348,8 @@
 			TCC1->CC[1].reg=value;
 			break;
+		
+		case 10:
+			TC3->COUNT16.CC[0].reg = value;
+			break;		
 			
 		case 11:
@@ -225,4 +357,8 @@
 			break;
 
+		case 12:
+			TC3->COUNT16.CC[1].reg = value;
+			break;	
+
 		case 13:
 			TCC2->CC[1].reg=value;
@@ -234,5 +370,5 @@
 	}
     
-    //servos[this->servoIndex].ticks = value;   //da sistemare
+    //servos[this->servoIndex].ticks = value;   //to be fixed
 	//servos[channel].ticks = value;
   }
@@ -241,5 +377,16 @@
 int Servo::read() // return the value as degrees
 {
-  return map(readMicroseconds(), SERVO_MIN(), SERVO_MAX(), 0, 180);
+  //select the right values for servo motor
+  int servo_min;
+  int servo_max;
+  if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){
+	servo_min=SERVO_MIN_TC();
+	servo_max=SERVO_MAX_TC();
+	}
+  else{
+	servo_min=SERVO_MIN_TCC();
+	servo_max=SERVO_MAX_TCC();
+	}
+  return map(readMicroseconds(), servo_min, servo_max, 0, 180);
 }
 
Index: rtos_arduino/trunk/lib/Makefile.rca
===================================================================
--- rtos_arduino/trunk/lib/Makefile.rca	(revision 223)
+++ rtos_arduino/trunk/lib/Makefile.rca	(revision 224)
@@ -10,5 +10,5 @@
               wiring.o wiring_digital.o wiring_analog.o wiring_shift.o \
               WInterrupts.o \
-              samd21_device.o samd21_host.o \
+              samd21_host.o \
               dtostrf.o \
               itoa.o \
@@ -110,5 +110,5 @@
 CDEFS += -Dprintf=iprintf \
          -DF_CPU=48000000L \
-         -DARDUINO=10708 \
+         -DARDUINO=10710 \
          -DARDUINO_SAM_ZERO \
          -DARDUINO_ARCH_SAMD \
