diff --git a/.gitmodules b/.gitmodules
index a04afc2..9b4b4d4 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,6 @@
 [submodule "vendor/serial"]
 	path = vendor/serial
 	url = https://github.com/wjwwood/serial.git
+[submodule "vendor/libblepp"]
+	path = vendor/libblepp
+	url = https://github.com/edrosten/libblepp.git
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 06adf6f..95b2a6c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,6 +4,23 @@ project(firmatacpp)
 set (CMAKE_CXX_STANDARD 11)
 
 option(FIRMATA_BUILD_EXAMPLES "Build firmata example programs" YES)
+option(FIRMATA_WITH_BLUETOOTH "Enable Bluetooth support" YES)
+
+if (FIRMATA_WITH_BLUETOOTH)
+    include (ExternalProject)
+
+    ExternalProject_Add(libblepp
+        SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/vendor/libblepp
+        CONFIGURE_COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/vendor/libblepp/configure --prefix=<INSTALL_DIR>
+        BUILD_COMMAND ${MAKE})
+    ExternalProject_Get_Property(libblepp install_dir)
+    set(FIRMATA_BLESRC src/firmble.cpp)
+    set(FIRMATA_BLEHDR include/firmble.h)
+else()
+    set(FIRMATA_BLESRC)
+    set(FIRMATA_BLEHDR)
+endif()
+
 
 include (GenerateExportHeader)
 
@@ -16,6 +33,10 @@ include_directories(
 	${CMAKE_CURRENT_SOURCE_DIR}/vendor/serial/include
 	)
 
+if (FIRMATA_WITH_BLUETOOTH)
+    include_directories( ${install_dir}/include)
+endif()
+
 set(FIRMATACPP_SOURCES 
 	src/firmbase.cpp
 	src/firmi2c.cpp
@@ -27,19 +48,31 @@ set(FIRMATACPP_INCLUDES
 	include/firmata.h
 	include/firmbase.h
 	include/firmi2c.h
-	include/firmio.h 
+	include/firmio.h
 	include/firmserial.h 
 	${CMAKE_CURRENT_BINARY_DIR}/firmatacpp_export.h
 	)
 
-add_library(firmatacpp ${FIRMATACPP_SOURCES} ${FIRMATACPP_INCLUDES})
+SET( CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -g3" )
+
+add_library(firmatacpp ${FIRMATACPP_SOURCES} ${FIRMATA_BLESRC} ${FIRMATACPP_INCLUDES} ${FIRMATA_BLEHDR})
+add_dependencies(firmatacpp libblepp)
 generate_export_header(firmatacpp)
 set_target_properties(firmatacpp PROPERTIES
   COMPILE_FLAGS -DLIBSHARED_AND_STATIC_STATIC_DEFINE)
 
+link_directories(
+        ${install_dir}/lib
+)
+
 target_link_libraries(firmatacpp serial)
 
 if (FIRMATA_BUILD_EXAMPLES)
 	add_executable(simple_example examples/simple.cpp)
 	target_link_libraries(simple_example firmatacpp)
+
+        if (FIRMATA_WITH_BLUETOOTH)
+                add_executable(simple_exampleble examples/simpleble.cpp)
+                target_link_libraries(simple_exampleble firmatacpp ble++)
+        endif()
 endif()
diff --git a/README.md b/README.md
index 5e793dc..1bcb59a 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,16 @@
 #firmatacpp [![Donate](https://nourish.je/assets/images/donate.svg)](http://ko-fi.com/A250KJT)
 
 A C++ firmata client library. Currently implements the basic protocol and I2C extension, version 2.5.
+
+If you are having problems with messages like this:
+
+```
+CMake Error at vendor/serial/CMakeLists.txt:30 (add_library):
+  Cannot find source file:
+
+    src/serial.cc
+```
+
+then the magic commands are: `git submodule init && git submodule update`
+
+Packages required to build on Ubuntu are: bluetooth bluez libbluetooth-dev libboost-all-dev
diff --git a/examples/simpleble.cpp b/examples/simpleble.cpp
new file mode 100644
index 0000000..14bb6ad
--- /dev/null
+++ b/examples/simpleble.cpp
@@ -0,0 +1,119 @@
+#include <iostream>
+#include <signal.h>
+
+#include "firmata.h"
+#include "firmble.h"
+
+#ifndef WIN32
+#include "unistd.h"
+#endif
+
+bool stopping = false;
+
+void do_stop(int sig)
+{
+    std::cout << "Shutting down..." << std::endl;
+    stopping = true;
+}
+
+/*
+ * Detect first bluetooth device with a StandardFirmata interface
+ * Read analog inputs A0 and A1 and digital pin 2 (eg, a Playstation analog stick + button)
+ * as well as I2C address 8 (eg, the slave_sender example that comes with Arduino IDE)
+ * and print to stdout
+ */
+
+int main(int argc, const char* argv[])
+{
+    signal(SIGINT, do_stop);
+    signal(SIGTERM, do_stop);
+
+	std::vector<firmata::BlePortInfo> ports = firmata::FirmBle::listPorts(3);
+	firmata::Firmata<firmata::Base, firmata::I2C>* f = NULL;
+	firmata::FirmBle* bleio;
+
+	for (auto port : ports) {
+		std::cout << port.port << std::endl;
+
+		if (f != NULL) {
+			delete f;
+			f = NULL;
+		}
+
+		try {
+			bleio = new firmata::FirmBle(port.port.c_str());
+#ifndef WIN32
+			//if (bleio->available()) {
+				f = new firmata::Firmata<firmata::Base, firmata::I2C>(bleio);
+			//}
+#else
+			f = new firmata::Firmata<firmata::Base, firmata::I2C>(bleio);
+#endif
+		} 
+		catch(firmata::IOException e) {
+			std::cout << e.what() << std::endl;
+		}
+		catch(firmata::NotOpenException e) {
+			std::cout << e.what() << std::endl;
+		}
+		if (f != NULL && f->ready()) {
+			break;
+		}
+	}
+	if (f == NULL || !f->ready()) {
+            std::cout << "nothing there" <<std::endl;
+		return 1;
+	}
+
+        int sleep_usecs = 100000;
+	try {
+		f->setSamplingInterval(100);
+
+		std::cout << f->name << std::endl;
+		std::cout << f->major_version << std::endl;
+		std::cout << f->minor_version << std::endl;
+
+		f->pinMode(2, MODE_INPUT);
+
+		f->reportAnalog(0, 1);
+		f->reportAnalog(1, 1);
+
+		f->reportDigital(0, 1);
+		//f->configI2C(0);
+		//f->reportI2C(8, FIRMATA_I2C_REGISTER_NOT_SPECIFIED, 6);
+
+		while (true) {
+                        if (stopping)
+                        {
+                            break;
+                        }
+			f->parse();
+			int a0 = f->analogRead("A0");
+			int a1 = f->analogRead("A1");
+			int pin2 = f->digitalRead(2);
+			//std::vector<uint8_t> i2c = f->readI2C(8);
+			//std::string s = "";
+			//for (auto byte = i2c.begin(); byte < i2c.end(); ++byte) {
+				//s += (char)*byte;
+			//}
+
+			std::cout << a0 << ", " << a1 << ", " << pin2 << std::endl;
+                        f->digitalWrite(13,1);
+                        usleep(sleep_usecs);
+                        f->digitalWrite(13,0);
+                        usleep(sleep_usecs);
+		};
+
+                std::vector<unsigned char> r;
+                r.push_back(FIRMATA_SYSTEM_RESET);
+                f->standardCommand(r);
+		delete f;
+	}
+	catch (firmata::IOException e) {
+		std::cout << e.what() << std::endl;
+	}
+	catch (firmata::NotOpenException e) {
+		std::cout << e.what() << std::endl;
+	}
+}
+
diff --git a/include/firmata_constants.h b/include/firmata_constants.h
index 62126ea..2f60896 100644
--- a/include/firmata_constants.h
+++ b/include/firmata_constants.h
@@ -11,6 +11,11 @@
 #define MODE_SERVO	0x04
 #define MODE_SHIFT	0x05
 #define MODE_I2C	0x06
+#define MODE_ONEWIRE    0x07
+#define MODE_STEPPER    0x08
+#define MODE_ENCODER    0x09
+#define MODE_SERIAL     0x0a
+#define MODE_PULLUP     0x0b
 
 #define LOW			0
 #define HIGH		1
@@ -69,4 +74,4 @@ typedef struct		s_pin
 	std::vector<uint8_t>	resolutions;
 } t_pin;
 
-#endif
\ No newline at end of file
+#endif
diff --git a/include/firmbase.h b/include/firmbase.h
index f789455..5a7f116 100644
--- a/include/firmbase.h
+++ b/include/firmbase.h
@@ -27,8 +27,8 @@ namespace firmata {
 
 		void pinMode(uint8_t pin, uint8_t mode);
 		void digitalWrite(uint8_t pin, uint8_t value);
-		void analogWrite(uint8_t pin, uint32_t value);
-		void analogWrite(const std::string& channel, uint32_t value);
+		void analogWrite(uint8_t pin, uint32_t value); // pin = digital pin
+		void analogWrite(const std::string& channel, uint32_t value); // pin = "AN" where N is analog pin
 
 		uint8_t digitalRead(uint8_t pin);
 		uint32_t analogRead(uint8_t pin);
@@ -38,16 +38,25 @@ namespace firmata {
 		void sysexCommand(uint8_t sysex_command);
 		void sysexCommand(std::vector<uint8_t> sysex_command);
 
-		void reportAnalog(uint8_t channel, uint8_t enable = 1);
-		void reportDigital(uint8_t port, uint8_t enable = 1);
+		void reportAnalog(uint8_t channel, uint8_t enable = 1); // pin = analog pin
+		void reportDigital(uint8_t port, uint8_t enable = 1); // port = port group
+		void reportDigitalPin(uint8_t pin, uint8_t enable = 1);
 		void setSamplingInterval(uint32_t intervalms);
 
+                const uint8_t getNumPins() const { return m_numPins; }
+                const std::vector<uint8_t> & getPinCaps(uint8_t pin) const { return pins[pin].supported_modes; }
+                const std::vector<uint8_t> & getPinResolutions(uint8_t pin) const { return pins[pin].resolutions; }
+                uint8_t getPinCapResolution(uint8_t pin, uint8_t mode) const;
+                uint8_t getPinMode(uint8_t pin) { return pins[pin].mode; }
+                uint8_t getPinAnalogChannel(uint8_t pin) const { return pins[pin].analog_channel; } // Dpin -> Apin
+                uint8_t getPinFromAnalogChannel(uint8_t apin) const { return apins[apin]; } // Apin -> Dpin
+
 	protected:
 		virtual bool handleSysex(uint8_t command, std::vector<uint8_t> data);
 		virtual bool handleString(std::string data);
 
-		bool awaitResponse(uint8_t command, uint32_t timeout = 1000);
-		bool awaitSysexResponse(uint8_t sysexCommand, uint32_t timeout = 1000);
+		bool awaitResponse(uint8_t command, uint32_t timeout = 1000 /* ms */ );
+		bool awaitSysexResponse(uint8_t sysexCommand, uint32_t timeout = 1000 /* ms */ );
 
 	private:
 		void initPins();
@@ -64,7 +73,9 @@ namespace firmata {
 
 
 		FirmIO* m_firmIO;
+                uint8_t m_numPins;
 		t_pin pins[128];
+                uint8_t apins[128];
 	};
 
 }
diff --git a/include/firmble.h b/include/firmble.h
new file mode 100644
index 0000000..a4c7788
--- /dev/null
+++ b/include/firmble.h
@@ -0,0 +1,168 @@
+#ifndef		__FIRMBLE_H_
+#define		__FIRMBLE_H_
+
+#include <queue>
+#include <mutex>
+#include "firmio.h"
+#include "blepp/blestatemachine.h"
+#include "blepp/lescan.h"
+
+namespace firmata {
+
+	typedef struct BlePortInfo {
+		std::string port;
+		std::string description;
+		std::string hardware_id;
+	} BlePortInfo;
+
+	class FirmBle : public FirmIO {
+	public:
+		FirmBle(const std::string &port = ""); /* normal use */
+		FirmBle(int maxScanTime); /* used for initiating a scan */
+		virtual ~FirmBle();
+
+                /////////////////////////////////////////////////////////////
+                // Firmata methods
+
+                // firmata open connection
+                // this triggers the ble connect
+		virtual void open() override;
+                // firmata check if connection is open
+		virtual bool isOpen() override;
+                // firmata close
+                // this triggers ble disconnect
+		virtual void close() override;
+                // firmata queries how much data is queued
+		virtual size_t available() override;
+                // firmata read data
+		virtual std::vector<uint8_t> read(size_t size = 1) override;
+                // firmata transmit data
+		virtual size_t write(std::vector<uint8_t> bytes) override;
+                // firmata transmit data in batches
+                // call first with true then with false to release queued data
+		void write_batch(bool start);
+                // firmata retrieve list of ports
+		static std::vector<BlePortInfo> listPorts(int timeout=10 /*seconds*/, int maxDevices=0);
+                // enable ble debug
+                static void enableDebug(bool enable=true);
+
+	private:
+
+                /////////////////////////////////////////////////////////////
+                // members etc.
+
+                // state 
+                enum ble_job {
+                    st_idle, // idle
+                    st_scan, // running scan
+                    st_conn, // connecting
+                    st_disc, // disconnecting
+                    st_write, // send data
+                    st_stop,  // shutdown
+                };
+                ble_job m_job = st_idle;
+
+                // do we think we are connected?
+                bool m_connected = false;
+
+                // are we busy doing something
+                bool m_active = false;
+
+                // file descriptors for waking each side
+                int m_fdWakeBle = -1;
+                int m_fdWakeMain = -1;
+                int m_fdWakeMainRx = -1;
+
+                // memory buffer for sending data from
+                uint8_t * m_tx_buff = nullptr;
+                // sets of bytes to be sent
+                std::queue< std::vector<uint8_t> > m_tx_queue;
+                // access lock to transmit queue
+                std::mutex m_tx_lock;
+
+                // received PDUs
+                // stored like this to make it easy to limit queue length
+                // without overwriting partial messages
+                std::queue<BLEPP::PDUNotificationOrIndication> m_rx_buf;
+                // easier to count bytes here than calculate on demand
+                volatile size_t m_rx_bytes = 0;
+                // max number of messages in receive queue
+                static size_t s_max_queued_messages;
+                // access lock to receive queue
+                std::mutex m_rx_lock;
+                // is anyone waiting to read from the rx queue?
+                volatile bool m_rx_reader_waiting = false;
+
+                // BLE handles
+                BLEPP::BLEGATTStateMachine * m_gatt = nullptr;
+                BLEPP::Characteristic * m_tx = nullptr;
+                BLEPP::Characteristic * m_rx = nullptr;
+
+                // scan stuff
+                BLEPP::HCIScanner * m_scanner = nullptr;
+                int mScanMaxTime = 10;
+                // list of "ports" (remote devices) that we will report
+                // to main
+                std::vector<BlePortInfo> m_portList;
+
+                // worker thread handle
+                pthread_t m_thread;
+                
+                // results - empty for OK, message otherwise
+                std::string m_result;
+
+                // the port we have been asked to communicate with
+                std::string m_port;
+
+                // connect callback
+                std::function<void()> m_scancallback;
+
+                // are we batching writes
+                bool m_writebatch = false;
+
+                /////////////////////////////////////////////////////////////
+                // ble methods
+                // called in context of worker thread
+
+                // common constructor path
+                void initialise();
+
+                // scan results handler
+                void ble_handle_scan_result();
+                // check remote characteristics during connect
+                void ble_connect_scan_result ();
+                // device connected
+                void ble_connected ();
+                // device reporting interval is now set up
+                void ble_connect_reporting_set ();
+                // device disconnected
+                // could happen any time
+                void ble_disconnected (BLEPP::BLEGATTStateMachine::Disconnect);
+                // all data written
+                void ble_write_done ();
+                // data received from device
+                void ble_rx(const BLEPP::PDUNotificationOrIndication & p);
+                void ble_rx2(const BLEPP::Characteristic &, const BLEPP::PDUNotificationOrIndication & p);
+
+                /////////////////////////////////////////////////////////////
+                // threading methods
+
+                // main thread uses this to wake ble thread and wait for completion
+                void start_thread_and_wait(ble_job job, bool nothrow = false);
+                // ble thread sets flags as idle
+                void jobDone();
+                // ble thread completes a job and wakes something waiting for receive
+                void wakeRx();
+                // ble thread completes a job and wakes main
+                void wakeMain();
+                // ble thread write helper
+                void do_write();
+                // ble thread
+                static void * thread_main(void * b);
+                void ble_thread_main();
+	};
+
+}
+
+#endif
+
diff --git a/include/firmio.h b/include/firmio.h
index 22ea840..2f67b34 100644
--- a/include/firmio.h
+++ b/include/firmio.h
@@ -15,6 +15,7 @@ namespace firmata {
 		virtual size_t available() = 0;
 		virtual std::vector<uint8_t> read(size_t size = 1) = 0;
 		virtual size_t write(std::vector<uint8_t> bytes) = 0;
+                virtual ~FirmIO() {};
 	};
 
 	class IOException : public std::exception {
diff --git a/src/firmbase.cpp b/src/firmbase.cpp
index 4db3614..6cf7ff6 100644
--- a/src/firmbase.cpp
+++ b/src/firmbase.cpp
@@ -1,5 +1,7 @@
 #include "firmbase.h"
 
+#include <string.h>
+#include <unistd.h>
 #include <chrono>
 #include <string>
 #include <iostream>
@@ -30,6 +32,8 @@ namespace firmata {
 
 	void Base::init()
 	{
+                // TODO each of these should check the return from
+                // await[Sysex]Response and throw if necessary
 		reportFirmware();
 		initPins();
 		capabilityQuery();
@@ -50,6 +54,7 @@ namespace firmata {
 		standardCommand({ FIRMATA_SET_DIGITAL_PIN, pin, value });
 	}
 
+        // pin is digital pin ID
 	void Base::analogWrite(uint8_t pin, uint32_t value)
 	{
 		if (pin > 15 || value > FIRMATA_MAX) {
@@ -64,6 +69,7 @@ namespace firmata {
 		standardCommand({ analog_write_pin, lsb, msb });
 	}
 
+        // pin is digital pin ID
 	void Base::analogWriteExtended(uint8_t pin, uint32_t value)
 	{
 		pins[pin].value = value;
@@ -79,21 +85,22 @@ namespace firmata {
 		sysexCommand(bytes);
 	}
 
+        // arg is "AN" where N is analog pin ID
 	void Base::analogWrite(const std::string& channel, uint32_t value)
 	{
 		if (channel[0] != 'A') return;
-		for (uint8_t pin = 0; pin < 127; pin++) {
-			if (pins[pin].analog_channel + '0' == channel[1]) {
-				pins[pin].value = value;
-				analogWrite(pin, value);
-				return;
-			}
+                uint8_t apin = std::stoul(channel.substr(1));
+                uint8_t pin = apins[apin];
+                if (pin < 128) {
+                    pins[pin].value = value;
+                    analogWrite(pin, value);
+                    return;
 		}
 	}
 
 	uint8_t Base::digitalRead(uint8_t pin)
 	{
-		return pins[pin].value;
+		return pins[pin].value?1:0;
 	}
 
 	uint32_t Base::analogRead(uint8_t pin)
@@ -101,17 +108,19 @@ namespace firmata {
 		return pins[pin].value;
 	}
 
+        // arg is "AN" where N is analog pin ID
 	uint32_t Base::analogRead(const std::string& channel)
 	{
 		if (channel[0] != 'A') return 0;
-		for (uint8_t pin = 0; pin < 127; pin++) {
-			if (pins[pin].analog_channel + '0' == channel[1]) {
-				return pins[pin].value;
-			}
+                uint8_t apin = std::stoul(channel.substr(1));
+                uint8_t pin = apins[apin];
+                if (pin < 128) {
+                    return pins[pin].value;
 		}
 		return 0;
 	}
 
+        // arg is analog pin ID
 	void Base::reportAnalog(uint8_t channel, uint8_t enable)
 	{
 		uint8_t report_channel = FIRMATA_REPORT_ANALOG | channel;
@@ -124,6 +133,11 @@ namespace firmata {
 		standardCommand({ report_port, enable });
 	}
 
+	void Base::reportDigitalPin(uint8_t pin, uint8_t enable)
+	{
+            reportDigital(pin>>3, enable);
+	}
+
 	void Base::setSamplingInterval(uint32_t intervalms)
 	{
 
@@ -186,11 +200,9 @@ namespace firmata {
 					if (msb > 0x7F) continue; // Why do we sometimes get only 1 data byte?
 
 					value = FIRMATA_COMBINE_LSB_MSB(lsb, msb);
-					for (int pin = 0; pin < 128; pin++) {
-						if (pins[pin].analog_channel == channel) {
-							pins[pin].value = value;
-							break;
-						}
+                                        uint8_t pin = apins[channel];
+                                        if (pin < 128) {
+                                            pins[pin].value = value;
 					}
 					i += 2;
 					completed_commands++;
@@ -211,8 +223,9 @@ namespace firmata {
 
 					value = FIRMATA_COMBINE_LSB_MSB(lsb, msb);
 					for (int pin = 0; pin < 8; pin++) {
-						if (pins[port * 8 + pin].mode == MODE_INPUT) {
-							pins[port * 8 + pin].value = FIRMATA_NTH_BIT(value, pin);
+						int thepin = port * 8 + pin;
+						if ((pins[thepin].mode == MODE_INPUT) || (pins[thepin].mode == MODE_PULLUP)) {
+							pins[thepin].value = FIRMATA_NTH_BIT(value, pin);
 						}
 					}
 					i += 2;
@@ -244,12 +257,17 @@ namespace firmata {
 
 					std::vector<uint8_t> sysex_buffer;
 					for (i = i + 2; i < parse_buffer.size() && parse_buffer[i] != FIRMATA_END_SYSEX; i++) { // Copy sysex and skip to next command
+                                                if (parse_buffer[i] > 127) {
+                                                    // bad character, drop what we have so far and go back to parsing new command
+                                                    sysex_buffer.clear();
+                                                    break;
+                                                }
 						sysex_buffer.push_back(parse_buffer[i]);
 					}
 					if (i == parse_buffer.size()) {
 						interrupted_command = true;
 					}
-					else {
+                                        else if (!sysex_buffer.empty()) {
 						handleSysex(subcommand, sysex_buffer);
 						completed_commands++;
 						last_completed = (whole_command << 8) | subcommand;
@@ -293,18 +311,18 @@ namespace firmata {
 				pins[pin].resolutions = {};
 			}
 
-			pin = 0;
+			m_numPins = 0;
 			for (uint8_t byte : data) {
 				if (byte == 127) {
-					pin++;
+					m_numPins++;
 					is_mode_byte = true;
 				}
 				else if (is_mode_byte) {
-					pins[pin].supported_modes.push_back(byte);
+					pins[m_numPins].supported_modes.push_back(byte);
 					is_mode_byte = false;
 				}
 				else {
-					pins[pin].resolutions.push_back(byte);
+					pins[m_numPins].resolutions.push_back(byte);
 					is_mode_byte = true;
 				}
 			}
@@ -322,6 +340,7 @@ namespace firmata {
 		case(FIRMATA_ANALOG_MAPPING_RESPONSE) :
 			for (pin = 0; pin < data.size(); pin++) {
 				pins[pin].analog_channel = data[pin];
+                                apins[data[pin]] = pin;
 			}
 			return true;
 
@@ -350,8 +369,11 @@ namespace firmata {
 	std::string Base::stringFromBytes(std::vector<uint8_t>::iterator begin, std::vector<uint8_t>::iterator end)
 	{
 		std::string s;
-		for (auto byte = begin; byte < end - 1; ++byte) {
-			s += (*byte) | (*(++byte) << 7);
+		//for (auto byte = begin; byte < end - 1; ++byte) {
+			//s += (*byte) | (*(++byte) << 7);
+		//}
+		for (auto byte = begin; byte < end; ++byte) {
+                        s+=(*byte);
 		}
 		return s;
 	}
@@ -362,7 +384,8 @@ namespace firmata {
 		std::chrono::time_point<std::chrono::system_clock> start, current;
 		start = std::chrono::system_clock::now();
 		std::chrono::duration<std::chrono::system_clock::rep,
-			std::chrono::system_clock::period> timeoutDuration(timeout * 10000), elapsed;
+			std::chrono::system_clock::period> elapsed;
+                std::chrono::milliseconds timeoutDuration(timeout); 
 
 		uint8_t first_nibble = FIRMATA_FIRST_NIBBLE(command);
 		uint16_t result;
@@ -385,11 +408,13 @@ namespace firmata {
 		bool succeeded = true;
 		std::chrono::time_point<std::chrono::system_clock> start, current;
 		start = std::chrono::system_clock::now();
-		std::chrono::duration<std::chrono::system_clock::rep, 
-			std::chrono::system_clock::period> timeoutDuration(timeout * 10000), elapsed;
+		std::chrono::duration<std::chrono::system_clock::rep,
+			std::chrono::system_clock::period> elapsed;
+                std::chrono::milliseconds timeoutDuration(timeout); 
 
 		uint16_t result, result_sysex, result_command;
 		do {
+                        usleep(1000);
 			current = std::chrono::system_clock::now();
 			elapsed = current - start;
 			if (elapsed > timeoutDuration) {
@@ -407,6 +432,9 @@ namespace firmata {
 
 	void Base::initPins()
 	{
+                // clear analog pin mappings
+                memset(apins, sizeof(apins), 128);
+
 		for (int i = 0; i < 128; i++) {
 			pins[i].mode = 255;
 			pins[i].analog_channel = 127;
@@ -414,6 +442,7 @@ namespace firmata {
 			pins[i].resolutions = {};
 			pins[i].value = 0;
 		}
+                m_numPins = 0;
 	}
 
 	void Base::reportFirmware() {
@@ -446,4 +475,18 @@ namespace firmata {
 			}
 		}
 	}
+        uint8_t Base::getPinCapResolution(uint8_t pin, uint8_t mode) const {
+            std::vector<uint8_t>::const_iterator pm = pins[pin].supported_modes.begin();
+            std::vector<uint8_t>::const_iterator pr = pins[pin].resolutions.begin();
+            while (pm !=pins[pin].supported_modes.end())
+            {
+                if (*pm == mode)
+                {
+                    return *pr;
+                }
+                ++pm;
+                ++pr;
+            }
+            return 0;
+        }
 }
diff --git a/src/firmble.cpp b/src/firmble.cpp
new file mode 100644
index 0000000..037b5ed
--- /dev/null
+++ b/src/firmble.cpp
@@ -0,0 +1,1009 @@
+#include "firmble.h"
+
+#include <iostream>
+#include <sstream>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+
+#include <string.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/eventfd.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <sys/syscall.h>
+
+#include <cerrno>
+#include <array>
+#include <iomanip>
+#include <vector>
+#include <boost/optional.hpp>
+#include <functional>
+
+#include <signal.h>
+
+#include <stdexcept>
+
+#include <blepp/logging.h>
+#include <blepp/pretty_printers.h>
+#include <blepp/blestatemachine.h> //for UUID. FIXME mofo
+#include <blepp/lescan.h>
+
+#include <time.h>
+#include <chrono>
+
+using namespace std;
+using namespace chrono;
+using namespace BLEPP;
+
+#define DBG(__x...) \
+    if (s_debug) { \
+        std::ostringstream __s; \
+        struct timespec __t; \
+        std::ios __f(NULL); \
+        __f.copyfmt(__s); \
+        clock_gettime(CLOCK_REALTIME, &__t); \
+        __s << "DBG:" << syscall(SYS_gettid) << ":" \
+            << __t.tv_sec << "." << setfill('0') << setw(9) << __t.tv_nsec << ":"; \
+        __s.copyfmt(__f); \
+        __s << __FILE__ ":" << __LINE__ << ":" << __FUNCTION__ << ":" \
+            << __x << std::endl; \
+        std::cerr <<__s.str(); \
+    }
+
+#define DBGPKT(__d, __l) \
+    if (s_debug) { \
+        std::ostringstream __s; \
+        struct timespec __t; \
+        std::ios __f(NULL); \
+        __f.copyfmt(__s); \
+        clock_gettime(CLOCK_REALTIME, &__t); \
+        __s << "PKT:" << syscall(SYS_gettid) << ":" \
+            << __t.tv_sec << "." << setfill('0') << setw(9) << __t.tv_nsec << ":"; \
+        __s.copyfmt(__f); \
+        __s << __FILE__ ":" << __LINE__ << ":" << __FUNCTION__ << ":" \
+            << __l << ":" <<  hex << setw(2) ; \
+        auto __p = __d; \
+        size_t __n = __l; \
+        while (__n > 0) { \
+            __s << " " << (unsigned int)(*__p); \
+            --__n; \
+            ++__p; \
+        } \
+        __s << std::endl; \
+        std::cerr <<__s.str(); \
+    }
+
+namespace {
+    // debug integration
+    bool s_debug = 0;
+
+    // time to shut down
+    bool s_shutdown = false;
+
+    // local methods
+
+    // main thread waits for signal
+    void wait_complete(int fd)
+    {
+        // wait for the event completion signal to be raised
+        DBG("waiting for "<<fd);
+        int result = -1;
+        do
+        {
+            fd_set read_set;
+            FD_ZERO(&read_set);
+            FD_SET(fd,&read_set);
+            result = select(fd+1, &read_set, nullptr, nullptr, nullptr);
+        } while (result < 0);
+        uint64_t r;
+        ::read(fd, &r, 8);
+        DBG("completed on "<<fd<<" value "<<r);
+    }
+
+}
+
+namespace firmata {
+
+    // max number of queued messages
+    size_t FirmBle::s_max_queued_messages = 1000;
+
+    void FirmBle::enableDebug(bool enable)
+    {
+        s_debug = enable;
+    }
+    ///////////////////////////////////////////////////
+    // Firmata methods
+
+    // wrapper for starting thread
+    void * FirmBle::thread_main(void * b)
+    {
+        DBG("entry "<<b);
+        firmata::FirmBle * ble = reinterpret_cast<firmata::FirmBle*>(b);
+        ble->ble_thread_main();
+        DBG("exit "<<b);
+        return nullptr;
+    }
+
+    FirmBle::FirmBle(const std::string &port)
+    {
+        DBG("this "<<this<<" port "<<port);
+        // remember the device to connect to
+        m_port = port;
+        initialise();
+    }
+    FirmBle::FirmBle(int maxScanTime)
+    {
+        DBG("this "<<this<<" max scan time "<<maxScanTime);
+        mScanMaxTime = maxScanTime;
+        initialise();
+    }
+    void FirmBle::initialise()
+    {
+        // create sockets
+        m_fdWakeBle    = eventfd(0, EFD_CLOEXEC|EFD_SEMAPHORE|EFD_NONBLOCK);
+        m_fdWakeMain   = eventfd(0, EFD_CLOEXEC|EFD_SEMAPHORE|EFD_NONBLOCK);
+        m_fdWakeMainRx = eventfd(0, EFD_CLOEXEC|EFD_SEMAPHORE|EFD_NONBLOCK);
+        m_scancallback = std::bind(&firmata::FirmBle::ble_connect_scan_result,this);
+        DBG("fds: ble "<<m_fdWakeBle<<" main "<<m_fdWakeMain << " rx "<<m_fdWakeMainRx);
+        
+        // create the worker thread
+        pthread_create(&m_thread,
+                       nullptr, 
+                       &FirmBle::thread_main,
+                       this);
+        DBG("created thread");
+    }
+
+    FirmBle::~FirmBle()
+    {
+        DBG("entry "<<this);
+        // disconnect if required
+        close();
+
+        // tell thread to stop and wait for it
+        DBG("stopping thread");
+        start_thread_and_wait(st_stop, true);
+        DBG("stopped thread");
+        pthread_join(m_thread, nullptr);
+        DBG("joined thread");
+
+        // tidy up
+        ::close(m_fdWakeBle);
+        ::close(m_fdWakeMain);
+        ::close(m_fdWakeMainRx);
+        DBG("done");
+    }
+
+    // firmata open connection
+    // this triggers the ble connect
+    void FirmBle::open()
+    {
+        // check for already connected
+        if (m_connected) {
+            DBG("already connected");
+            throw firmata::IOException();
+        }
+        DBG("starting");
+        start_thread_and_wait(st_conn);
+        DBG("done");
+    }
+
+    // firmata check if connection is open
+    bool FirmBle::isOpen()
+    {
+        return m_connected;
+    }
+
+    // firmata close
+    // this triggers ble disconnect
+    void FirmBle::close()
+    {
+        DBG("entry");
+        start_thread_and_wait(st_disc, true);
+        DBG("done");
+    }
+
+    // firmata queries how much data is queued
+    size_t FirmBle::available()
+    {
+        DBG("entry");
+        std::lock_guard<std::mutex> lock(m_rx_lock);
+        DBG(m_rx_buf.size() << " entries");
+        return m_rx_bytes;
+    }
+
+    // firmata read data
+    std::vector<uint8_t> FirmBle::read(size_t size)
+    {
+        std::vector<uint8_t> bytes;
+        DBG("size "<<size);
+
+        pthread_yield();
+
+        {
+            std::lock_guard<std::mutex> lock(m_rx_lock);
+            DBG(m_rx_buf.size() << " entries");
+
+            if (!m_rx_buf.empty())
+            {
+                // concatenate all available packets
+                while (!m_rx_buf.empty())
+                {
+                    BLEPP::PDUNotificationOrIndication & p = m_rx_buf.front();
+                    const uint8_t* d = p.value().first;
+                    size_t n = p.value().second - p.value().first;
+                    if (n > size) {
+                        DBG("no space left "<<n<<" space left "<<size);
+                        break;
+                    }
+                    size-=n;
+                    DBGPKT(d,n);
+                    while (n > 0) {
+                        bytes.push_back(*d);
+                        --n;
+                        ++d;
+                    }
+                    m_rx_buf.pop();
+                }
+                m_rx_bytes = 0;
+            }
+        }
+        DBG("prepared result");
+
+        pthread_yield();
+
+        DBG("returning "<<bytes.size());
+        return bytes;
+    }
+
+    // firmata transmit data in batches
+    // call first with true then with false to release queued data
+    void FirmBle::write_batch(bool start)
+    {
+        if (start) {
+            m_writebatch = true;
+        } else {
+            m_writebatch = false;
+            bool queued = false;
+            {
+                std::lock_guard<std::mutex> lock(m_tx_lock);
+                if (!m_tx_queue.empty()) {
+                    queued = true;
+                }
+            }
+            // release any pending data
+            if (queued) {
+                DBG("waiting");
+                start_thread_and_wait(st_write);
+                pthread_yield();
+                DBG("waited");
+            }
+        }
+    }
+
+    // firmata transmit data
+    size_t FirmBle::write(std::vector<uint8_t> bytes)
+    {
+        // check for not connected
+        if (!m_connected) {
+            DBG("not connected");
+            throw firmata::NotOpenException();
+        }
+        DBGPKT(bytes.begin(),bytes.size());
+        {
+            std::lock_guard<std::mutex> lock(m_tx_lock);
+            m_tx_queue.push(bytes);
+        }
+        if (m_writebatch == false) {
+            DBG("waiting");
+            start_thread_and_wait(st_write);
+            pthread_yield();
+            DBG("waited");
+        } else {
+            DBG("batched mode");
+        }
+    }
+
+    // firmata retrieve list of ports
+    std::vector<BlePortInfo> FirmBle::listPorts(int maxTime /*seconds*/, int maxDevices)
+    {
+        firmata::FirmBle * ble = new firmata::FirmBle(maxTime);
+        DBG("entry, ble "<<ble);
+
+        // tell the object we want to do a scan
+        ble->start_thread_and_wait(st_scan);
+        DBG("waited, "<<ble->m_portList.size()<<" entries");
+
+        // all done
+        std::vector<BlePortInfo> ret = ble->m_portList;
+        delete ble;
+        DBG("done");
+        return ret;
+    }
+
+    //////////////////////////////////////////////////////////////
+    //
+    // response handlers
+
+    // scan results handler
+    void FirmBle::ble_handle_scan_result()
+    {
+        DBG("entry "<<this);
+        vector<AdvertisingResponse> ads = m_scanner->get_advertisements();
+
+        for(const auto& ad: ads)
+        {
+            // if device under consideration is connectable
+            DBG("device "<<ad.address);
+            if((ad.type == LeAdvertisingEventType::ADV_IND) ||
+               (ad.type == LeAdvertisingEventType::ADV_DIRECT_IND))
+            {
+                DBG("connectable");
+                for(const auto& uuid: ad.UUIDs)
+                {
+                    DBG("    uuid: "<<to_str(uuid));
+                    // and if it is advertising the firmata service
+                    if (to_str(uuid) == "6e400001-b5a3-f393-e0a9-e50e24dcca9e")
+                    {
+                        // we have a candidate
+                        m_portList.push_back(BlePortInfo({
+                            ad.address, (ad.local_name)?"":ad.local_name->name, ""
+                        }));
+                        break;
+                    }
+                }
+            } else {
+                DBG("not connectable");
+            }
+        }
+        DBG("valid devices: "<<m_portList.size());
+
+        // if port is empty then we are connecting to the first located device
+        if ((m_job != st_scan) && (m_port == ""))
+        {
+            if (m_portList.empty())
+            {
+                // normal client request, all done
+                m_result = "No suitable devices found";
+                wakeMain();
+                DBG("done");
+            }
+            else
+            {
+                // so we must remember the MAC
+                m_port = m_portList[0].port;
+                DBG("autoselected port "<<m_port);
+                // initiate the connect by finding the descriptors
+                m_gatt->connect_nonblocking(m_port);
+                DBG("started connect");
+            }
+        }
+        else
+        {
+            // normal client request, all done
+            //DBG("done");
+            //wakeMain();
+            // wait for timeout to complete scan
+        }
+    }
+
+    // check remote characteristics during connect
+    void FirmBle::ble_connect_scan_result ()
+    {
+        DBG("entry");
+        //pretty_print_tree(m_gatt);
+
+        // look thru reported services + characteristics to find
+        // firmata characteristics
+        for(auto& service: m_gatt->primary_services)
+        {
+            DBG("service");
+            for(auto& characteristic: service.characteristics)
+            {
+                DBG(" characteristic: "<<to_str(characteristic.uuid));
+                if(characteristic.uuid == UUID("6e400002-b5a3-f393-e0a9-e50e24dcca9e"))
+                {
+                    // for sending requests
+                    //cout << "got tx" << endl;
+                    m_tx = new Characteristic(characteristic);
+                    DBG("    found tx");
+                }
+
+                if(characteristic.uuid == UUID("6e400003-b5a3-f393-e0a9-e50e24dcca9e"))
+                {
+                    // for receiving responses
+                    //cout << "got rx" << endl;
+                    m_rx = new Characteristic(characteristic);
+                    DBG("    found rx");
+                }
+            }
+        }
+        if (m_tx == nullptr) {
+            m_result = "Failed to find TX characteristic";
+            wakeMain();
+        } else if (m_rx == nullptr) {
+            m_result = "Failed to find RX characteristic";
+            wakeMain();
+        } else {
+            // initiate the connect
+            DBG("initiating connect");
+            m_rx->cb_notify_or_indicate = std::bind(&firmata::FirmBle::ble_rx,this, std::placeholders::_1);
+            m_gatt->cb_notify_or_indicate = std::bind(&firmata::FirmBle::ble_rx2,this, std::placeholders::_1, std::placeholders::_2);
+            try
+            {
+                ble_connected();
+            }
+            catch (...)
+            {
+                DBG("caught");
+                m_result = "Cannot connect";
+                wakeMain();
+            }
+        }
+        DBG("done");
+    };
+
+    // device connected
+    void FirmBle::ble_connected ()
+    {
+        DBG("entry");
+        // enable notify
+        m_rx->set_notify_and_indicate(true, false);
+        DBG("done set_notify");
+        // then set reporting interval to something a little slower
+        // than is normally used on a serial link
+        m_gatt->cb_write_response = std::bind(&firmata::FirmBle::ble_connect_reporting_set,this);
+        static unsigned char set_reporting_interval[] = { 0xf0, 0x7a, 0, 8, 0xf7 };
+        try
+        {
+            DBG("sending set reporting interval");
+            m_tx->write_request(set_reporting_interval, 5);
+        }
+        catch (...)
+        {
+            DBG("caught");
+            m_result = "Failed to set reporting interval";
+            wakeMain();
+        }
+        DBG("done");
+    }
+
+    // device reporting interval is now set up
+    void FirmBle::ble_connect_reporting_set ()
+    {
+        DBG("entry");
+        // all done
+        // reinstate the write done handler
+        m_gatt->cb_write_response = std::bind(&firmata::FirmBle::ble_write_done,this);
+        m_result = "";
+        // wake the caller
+        m_connected = true;
+        wakeMain();
+        DBG("done");
+    }
+
+    // device disconnected
+    // could happen any time
+    void FirmBle::ble_disconnected (BLEPP::BLEGATTStateMachine::Disconnect d)
+    {
+        DBG("entry "<<BLEPP::BLEGATTStateMachine::get_disconnect_string(d)<< " job "<<m_job);
+        m_connected = false;
+        if (m_job == st_disc)
+        {
+            // disconnect is expected
+            m_result = "";
+            wakeMain();
+            DBG("done");
+        } else {
+            m_result = "Disconnected";
+            if (m_job != st_idle) {
+                // abandon anything else in progress
+                // TODO should we auto reconnect?
+                wakeMain();
+                // wake a reader using other fd
+                wakeRx();
+                DBG("done");
+            }
+        }
+    }
+
+    // all data written
+    void FirmBle::ble_write_done ()
+    {
+        DBG("entry, clearing buffer "<<reinterpret_cast<void*>(m_tx_buff));
+        if (m_tx_buff)
+        {
+            free(m_tx_buff);
+            m_tx_buff = nullptr;
+            wakeMain();
+        }
+        DBG("done");
+    }
+
+    // data received from device
+    void FirmBle::ble_rx(const PDUNotificationOrIndication & p) {
+        size_t n = p.value().second - p.value().first;
+        DBG("entry "<<n<< " bytes");
+        DBGPKT(p.value().first,p.value().second - p.value().first);
+        {
+            // receive data
+            std::lock_guard<std::mutex> lock(m_rx_lock);
+            // limit queue length
+            if (m_rx_buf.size() > s_max_queued_messages) {
+                // too many queued messages, drop the oldest
+                BLEPP::PDUNotificationOrIndication & p2 = m_rx_buf.front();
+                m_rx_bytes -= p2.value().second - p2.value().first;
+                m_rx_buf.pop();
+            }
+            m_rx_buf.push(p);
+            m_rx_bytes += n;
+            DBG("queue now "<<m_rx_buf.size()<<" entries, "<<m_rx_bytes<<" bytes");
+        }
+        wakeRx();
+        DBG("done");
+    }
+
+    void FirmBle::ble_rx2(const Characteristic &, const PDUNotificationOrIndication & p) {
+        ble_rx(p);
+    }
+
+
+    //////////////////////////////////////////////////////////////
+    //
+    // Thread methods
+
+    // main thread uses this to wake ble thread and wait for completion
+    void FirmBle::start_thread_and_wait(ble_job job, bool nothrow)
+    {
+        DBG("entry, job "<<job<<" nothrow "<<nothrow);
+        // record what to do
+        m_job = job;
+        // prepare the semaphore value
+        uint64_t r = 1;
+        // wake the caller
+        ::write(m_fdWakeBle, reinterpret_cast<const void *>(&r), 8);
+        // wait for tx ack
+        DBG("waiting");
+        wait_complete(m_fdWakeMain);
+        DBG("done");
+        if ((nothrow == false) && (m_result.size())) {
+            DBG("error "<<m_result);
+            throw firmata::IOException();
+        }
+    }
+
+    // ble thread flags as idle
+    void FirmBle::jobDone()
+    {
+        // mark ourselves as inactive
+        m_job = st_idle;
+        m_active = false;
+    }
+
+    // ble thread completes a job and wakes something waiting for receive
+    void FirmBle::wakeRx()
+    {
+        uint64_t r = 0;
+        DBG("entry");
+        {
+            std::lock_guard<std::mutex> lock(m_rx_lock);
+            DBG("waiting "<<m_rx_reader_waiting);
+            if (m_rx_reader_waiting)
+            {
+                // prepare the return value
+                r = 1;
+                m_rx_reader_waiting = false;
+            }
+        }
+
+        if (r != 0)
+        {
+            DBG("signalling main");
+            jobDone();
+            // wake the caller
+            ::write(m_fdWakeMainRx, reinterpret_cast<const void *>(&r), 8);
+            DBG("done");
+        }
+    }
+    // ble thread completes a job and wakes main
+    void FirmBle::wakeMain()
+    {
+        DBG("entry");
+        // prepare the return value
+        uint64_t r = 1;
+        jobDone();
+        // wake the caller
+        ::write(m_fdWakeMain, reinterpret_cast<const void *>(&r), 8);
+        DBG("done");
+    }
+
+    // ble thread write helper
+    void FirmBle::do_write()
+    {
+        DBG("entry");
+        int i=0;
+        {
+            size_t bufsiz = 256;
+            m_tx_buff = reinterpret_cast<uint8_t *>(malloc(bufsiz));
+            std::lock_guard<std::mutex> lock(m_tx_lock);
+            DBG(m_tx_queue.size()<<" entries to send");
+            while (!m_tx_queue.empty())
+            {
+                std::vector<uint8_t> & r = m_tx_queue.front();
+                DBG(r.size()<<" bytes in buffer");
+                for(auto& b: r)
+                {
+                    m_tx_buff[i++]=b;
+                    if (i > bufsiz)
+                    {
+                        DBG("reallocating at bufsiz");
+                        bufsiz *= 2;
+                        m_tx_buff = reinterpret_cast<uint8_t *>(realloc(m_tx_buff, bufsiz));
+                    }
+                }
+                DBG("now have "<<i<<" bytes");
+                m_tx_queue.pop();
+            }
+        }
+        DBG("sending "<<i<<" bytes from "<<reinterpret_cast<void*>(m_tx_buff));
+        DBGPKT(m_tx_buff,i);
+        m_tx->write_request(m_tx_buff, i);
+    }
+
+    // ble thread
+    void FirmBle::ble_thread_main()
+    {
+        DBG("entry");
+
+        fd_set write_set, read_set;
+        bool finished = false;
+        while (finished == false)
+        {
+            struct timeval tv;
+            int result;
+            do
+            {
+                FD_ZERO(&read_set);
+                FD_ZERO(&write_set);
+                int fd_max = -1;
+
+                if (m_gatt) {
+                    if (m_gatt->socket() != -1)
+                    {
+                        //Reads are always a possibility due to asynchronus notifications.
+                        DBG("adding gatt socket "<<m_gatt->socket()<<" to read");
+                        FD_SET(m_gatt->socket(), &read_set);
+                        if (m_gatt->socket() > fd_max) {
+                            fd_max = m_gatt->socket();
+                        }
+
+                        //Writes are usually available, so only check for them when the 
+                        //state machine wants to write.
+                        if(m_gatt->wait_on_write()) {
+                            DBG("adding gatt socket "<<m_gatt->socket()<<" to write");
+                            FD_SET(m_gatt->socket(), &write_set);
+                        }
+                    } else {
+                        DBG("no gatt socket");
+                    }
+                }
+
+                if (!m_active) {
+                    // add event fd only if idle
+                    DBG("adding main fd "<<m_fdWakeBle);
+                    FD_SET(m_fdWakeBle, &read_set);
+                    if (m_fdWakeBle > fd_max) { fd_max = m_fdWakeBle; }
+                }
+
+                if (m_scanner != nullptr) {
+                    // add scanner fd if required
+                    DBG("adding scanner fd "<<m_scanner->get_fd());
+                    FD_SET(m_scanner->get_fd(), &read_set);
+                    if (m_scanner->get_fd() > fd_max) { fd_max = m_scanner->get_fd(); }
+                }
+
+                // use the timer to limit scan completion
+                if (m_job == st_scan)
+                {
+                    // delay time was set up at scan initiation
+                } else {
+                    // wait for max 10 sec
+                    tv.tv_sec = 10;
+                    tv.tv_usec = 0;
+                }
+                result = select(fd_max+1, &read_set, &write_set, nullptr, &tv);
+                if (result >= 0)
+                {
+                    break;
+                }
+                DBG("trying select again after "<<errno);
+            } while(true);
+
+            // timeout
+            if (result == 0) {
+                if (m_job == st_scan) {
+                    // scan completed
+                    DBG("scan timed out");
+                    //ble_handle_scan_result();
+                    wakeMain();
+                }
+                continue;
+            }
+
+            if (m_gatt != nullptr) {
+                if (m_gatt->socket() != -1) {
+                    if(FD_ISSET(m_gatt->socket(), &write_set)) {
+                        DBG("gatt write");
+                        m_gatt->write_and_process_next();
+                    }
+
+                    if(FD_ISSET(m_gatt->socket(), &read_set)) {
+                        DBG("gatt read");
+                        m_gatt->read_and_process_next();
+                    }
+                } else {
+                    DBG("no gatt socket");
+                }
+            }
+
+            if((m_scanner != nullptr) && (FD_ISSET(m_scanner->get_fd(), &read_set))) {
+                DBG("scanner");
+                ble_handle_scan_result();
+            }
+
+            if(FD_ISSET(m_fdWakeBle, &read_set))
+            {
+                DBG("wake, job "<<m_job);
+                // flush the socket
+                wait_complete(m_fdWakeBle);
+                // determine what to do
+                switch (m_job) {
+                    case st_idle: // idle
+                        // err?
+                        break;
+
+                    case st_scan: // initiate scan
+                        try
+                        {
+                            m_scanner = new BLEPP::HCIScanner(true);
+                            m_active = true;
+                            // this will get counted down by successive select calls
+                            tv.tv_sec = mScanMaxTime;
+                            tv.tv_usec = 0;
+                        }
+                        catch (...)
+                        {
+                            DBG("caught");
+                            m_result = "scan failed";
+                            wakeMain();
+                        }
+                        break;
+
+                    case st_conn: // initiate connect
+                        try
+                        {
+                            if (m_gatt == nullptr)
+                            {
+                                // create new GATT object
+                                m_gatt = new BLEPP::BLEGATTStateMachine;
+                                m_gatt->cb_disconnected = std::bind(&firmata::FirmBle::ble_disconnected,this,std::placeholders::_1);
+                                m_gatt->cb_connected = std::bind(&firmata::FirmBle::ble_connected,this);
+                                m_gatt->setup_standard_scan(m_scancallback);
+                                DBG("m_gatt "<<m_gatt);
+                            }
+                            if (m_port == "")
+                            {
+                                // if no port specified, do a scan to find
+                                // the first device
+                                m_scanner = new BLEPP::HCIScanner(true);
+                                DBG("m_scanner "<<m_scanner);
+                            } else {
+                                // initiate the connect by finding the descriptors
+                                DBG("connect");
+                                m_gatt->connect_nonblocking(m_port);
+                            }
+                            m_active = true;
+                            DBG("done");
+                        }
+                        catch (...)
+                        {
+                            DBG("caught");
+                            // any error -> disconnect
+                            if (m_gatt)
+                            {
+                                try
+                                {
+                                    DBG("closing");
+                                    m_gatt->close();
+                                }
+                                catch (...)
+                                {
+                                    DBG("caught");
+                                }
+                                m_active = false;
+                            }
+                            wakeMain();
+                        }
+                        DBG("finished");
+                        break;
+
+                    case st_disc: // initiate disconnect
+                        // stop any scanner that is running
+                        if (m_scanner)
+                        {
+                            try
+                            {
+                                m_scanner->stop();
+                                DBG("stopped scanner");
+                            }
+                            catch (...)
+                            {
+                                DBG("caught");
+                            }
+                            delete m_scanner;
+                            m_scanner = nullptr;
+                        }
+
+                        if (m_gatt)
+                        {
+                            try
+                            {
+                                // TODO send reset or at least stop events
+                                DBG("closing");
+                                if (m_gatt != nullptr)
+                                {
+                                    m_gatt->close();
+                                }
+                            }
+                            catch (...)
+                            {
+                                DBG("caught");
+                            }
+                        }
+                        // disconnect appears instant
+                        if ((m_scanner == nullptr) && (m_gatt == nullptr || m_gatt->socket() == -1))
+                        {
+                            DBG("connection is now inactive");
+                            m_active = false;
+                            wakeMain();
+                        }
+                        DBG("done");
+                        break;
+
+                    case st_write: // send data
+                        try
+                        {
+                            do_write();
+                            m_active = true;
+                        }
+                        catch (...)
+                        {
+                            DBG("caught");
+                            wakeMain();
+                        }
+                        DBG("done");
+                        break;
+
+                    case st_stop: // all done
+                        m_active = false;
+                        if (m_scanner)
+                        {
+                            try
+                            {
+                                m_scanner->stop();
+                                DBG("stopped scanner");
+                            }
+                            catch (...)
+                            {
+                                DBG("caught");
+                            }
+                            delete m_scanner;
+                            m_scanner = nullptr;
+                        }
+                        if (m_gatt)
+                        {
+                            try
+                            {
+                                m_gatt->close();
+                                DBG("closed gatt");
+                            }
+                            catch (...)
+                            {
+                                DBG("caught");
+                            }
+                            delete m_gatt;
+                            m_gatt = nullptr;
+                        }
+                        if (m_tx_buff)
+                        {
+                            free(m_tx_buff);
+                            m_tx_buff = nullptr;
+                        }
+                        finished = true;
+                        DBG("done");
+                        wakeMain();
+                        break;
+                }
+            }
+
+        } // end of main loop
+    } // end of thread main
+} // end namespace
+
+
+
+#if 0
+//cout << "state "<<state<<endl;
+switch (state) {
+    case 0: // reading services
+        if (m_gatt.is_idle()) {
+            cout << endl;
+            ++state;
+        } else {
+            break;
+        }
+
+    case 1: // services read
+        if (rx == nullptr) {
+            cerr << "no receive service found" << endl;
+            state = 7;
+        } else {
+            ++state;
+            cout << "setup rx" << endl;
+            rx->cb_notify_or_indicate = notify_cb;
+            m_gatt.cb_notify_or_indicate = notify_cb2;
+            rx->set_notify_and_indicate(true, false);
+        }
+        break;
+
+    case 2: // setting notify
+        if (m_gatt.is_idle()) {
+            cout << endl;
+            ++state;
+        } else {
+            break;
+        }
+
+    case 3: // set reporting interval
+        {
+            unsigned char enable[] = { 0xf0, 0x7a, 0, 8, 0xf7 };
+            tx->write_request(enable, 5);
+        }
+        ++state;
+        break;
+
+    case 4: // setting reporting interval
+        if (m_gatt.is_idle()) {
+            cout << endl;
+            ++state;
+        } else {
+            break;
+        }
+
+    case 5: // notify set
+        {
+            unsigned char enable[2] = { 0xc0, 0x01 };
+            tx->write_request(enable, 2);
+        }
+        ++state;
+        break;
+
+    case 6: // enabling reporting
+        if (m_gatt.is_idle()) {
+            cout << endl;
+            ++state;
+        } else {
+            break;
+        }
+
+    case 7: // reporting enabled
+        if (tx == nullptr) {
+            cerr << "no tx characteristic found" << endl;
+            exit(1);
+        }
+        cout << endl;
+        ++state;
+}
+#endif
diff --git a/vendor/libblepp b/vendor/libblepp
new file mode 160000
index 0000000..7c88892
--- /dev/null
+++ b/vendor/libblepp
@@ -0,0 +1 @@
+Subproject commit 7c88892e254d9fe80e0ff004d2dde909bb189f8a