ble-scan:

o New tool to scan for devices. Requires root on linux :( Linux is also touchy if the program dies.
BluetoothAdapter:
o Adding getMac().

8 files changed, 163 insertions, 50 deletions

diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt
index c9eea9e..01252cf 100644
--- a/apps/CMakeLists.txt
+++ b/apps/CMakeLists.txt
@@ -1,4 +1,5 @@
 list(APPEND APPS ble-inspect-device)
+list(APPEND APPS ble-scan)
 list(APPEND APPS sample-add-timestamp)
 list(APPEND APPS sample-convert)
 list(APPEND APPS sample-select)
diff --git a/apps/ble-inspect-device.h b/apps/ble-inspect-device.h
index 0bb70aa..346f9be 100644
--- a/apps/ble-inspect-device.h
+++ b/apps/ble-inspect-device.h
@@ -56,9 +56,9 @@ public:
         try {
             Mac mac = Mac::parseMac(mac_str);
 
-            BluetoothAdapter &adapter = getAdapter(0);
+            shared_ptr<BluetoothAdapter> adapter = getAdapter(0);
 
-            BluetoothDevice &device = adapter.getDevice(mac);
+            BluetoothDevice &device = adapter->getDevice(mac);
 
             scan_callback(device);
 
diff --git a/apps/ble-scan.h b/apps/ble-scan.h
new file mode 100644
index 0000000..9a72388
--- /dev/null
+++ b/apps/ble-scan.h
@@ -0,0 +1,87 @@
+#include "ble/Bluetooth.h"
+#include "apps.h"
+
+#include <csignal>
+
+namespace trygvis {
+namespace apps {
+
+using namespace std;
+using namespace trygvis::bluetooth;
+using namespace trygvis::apps;
+
+namespace ble_scan_utils {
+
+static std::function<void(int)> onSignal;
+
+static void signal_handler(int signal) {
+    onSignal(signal);
+}
+
+}
+
+class ble_scan : public app {
+
+public:
+    ble_scan() : app("ble-scan") {
+    }
+
+    ~ble_scan() = default;
+
+    void add_options(po::options_description_easy_init &options) override {
+        options
+                ("adapter", po::value<int>()->default_value(0), "Which adapter to use.");
+    }
+
+    int main(app_execution &execution) override {
+        BluetoothSystem bluetoothSystem;
+        shared_ptr<BluetoothAdapter> adapter;
+
+        struct sigaction sigIntHandler;
+
+        ble_scan_utils::onSignal = [&](int signal) {
+            adapter->stopScan();
+        };
+
+        sigIntHandler.sa_handler = &ble_scan_utils::signal_handler;
+        sigemptyset(&sigIntHandler.sa_mask);
+        sigIntHandler.sa_flags = 0;
+
+        sigaction(SIGINT, &sigIntHandler, NULL);
+
+        try {
+            auto adapter_index = execution.vm["adapter"].as<int>();
+
+            adapter = getAdapter(adapter_index);
+
+            set<Mac> seen_devices;
+
+            cout << "Scanning with adapter #" << adapter_index << ", mac=" << adapter->getMac().str() << endl;
+
+            adapter->runScan([&](BluetoothDevice &device) {
+                auto mac = device.getMac();
+
+                cout << "Found: " << mac.str() << endl;
+
+                seen_devices.insert(mac);
+            });
+
+            cout << "Stopped. Found " << seen_devices.size() << " devices." << endl;
+
+            for_each(begin(seen_devices), end(seen_devices), [&](auto mac) {
+                cout << mac.str() << endl;
+            });
+
+            return EXIT_SUCCESS;
+        } catch (std::runtime_error ex) {
+            cout << "std::runtime_error: " << ex.what() << endl;
+            return EXIT_FAILURE;
+        } catch (std::exception ex) {
+            cout << "std::exception: " << ex.what() << endl;
+            return EXIT_FAILURE;
+        }
+    }
+};
+
+}
+}
diff --git a/apps/sm-get-value.h b/apps/sm-get-value.h
index 0073d7f..0535f68 100644
--- a/apps/sm-get-value.h
+++ b/apps/sm-get-value.h
@@ -71,9 +71,9 @@ public:
 
             Mac mac = Mac::parseMac(MAC);
 
-            auto &adapter = getAdapter(0);
+            auto adapter = getAdapter(0);
 
-            auto &device = adapter.getDevice(mac);
+            auto &device = adapter->getDevice(mac);
 
             loop = sleepTime > 0;
 
diff --git a/ble/Bluetooth.cpp b/ble/Bluetooth.cpp
index b672dba..14a8cda 100644
--- a/ble/Bluetooth.cpp
+++ b/ble/Bluetooth.cpp
@@ -208,23 +208,7 @@ BluetoothSystem::~BluetoothSystem() {
     shutdown();
 }
 
-/*
-map<int, LinuxBluetoothAdapter *> adapters;
-
-BluetoothAdapter &getAdapter(int hciDevice) {
-    map<int, LinuxBluetoothAdapter *>::iterator it = adapters.find(hciDevice);
-
-    if (it == adapters.end()) {
-        LinuxBluetoothAdapter *adapter = new LinuxBluetoothAdapter(hciDevice);
-        adapters[hciDevice] = adapter;
-        return *adapter;
-    }
-
-    return *it->second;
-}
-*/
-
-BluetoothAdapter &getAdapter(int hciDevice) {
+shared_ptr<BluetoothAdapter> getAdapter(int hciDevice) {
     return getAdapterImpl(hciDevice);
 }
 
diff --git a/ble/BluetoothImpl.h b/ble/BluetoothImpl.h
index 204b051..3f4615e 100644
--- a/ble/BluetoothImpl.h
+++ b/ble/BluetoothImpl.h
@@ -16,6 +16,7 @@
         0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb \
     };
 
+#define LOG_TRACE(body) LOG4CPLUS_TRACE(logger, body)
 #define LOG_DEBUG(body) LOG4CPLUS_DEBUG(logger, body)
 #define LOG_INFO(body) LOG4CPLUS_INFO(logger, body)
 #define LOG_WARN(body) LOG4CPLUS_WARN(logger, body)
@@ -44,8 +45,10 @@ protected:
 
 class DefaultBluetoothGattCharacteristic : LogSetup, public BluetoothGattCharacteristic {
 public:
-    DefaultBluetoothGattCharacteristic(BluetoothGattService &service, uint16_t handle, uuid_t uuid, uint8_t properties, uint16_t valueHandle)
-            : LogSetup("DefaultBluetoothGattCharacteristic"), service(service), handle(handle), uuid(uuid), properties(properties), valueHandle(valueHandle) {
+    DefaultBluetoothGattCharacteristic(BluetoothGattService &service, uint16_t handle, uuid_t uuid, uint8_t properties,
+                                       uint16_t valueHandle)
+            : LogSetup("DefaultBluetoothGattCharacteristic"), service(service), handle(handle), uuid(uuid),
+              properties(properties), valueHandle(valueHandle) {
     }
 
     virtual ~DefaultBluetoothGattCharacteristic() {
@@ -81,7 +84,8 @@ protected:
 
 class DefaultBluetoothGattService : public BluetoothGattService {
 public:
-    DefaultBluetoothGattService(BluetoothDevice &device, const uuid_t uuid, const uint16_t handle, const uint16_t endGroupHandle)
+    DefaultBluetoothGattService(BluetoothDevice &device, const uuid_t uuid, const uint16_t handle,
+                                const uint16_t endGroupHandle)
             : device(device), uuid(uuid), handle(handle), endGroupHandle(endGroupHandle) {
     }
 
@@ -218,12 +222,18 @@ protected:
 class DefaultBluetoothAdapter : protected LogSetup, public BluetoothAdapter {
 public:
 protected:
-    DefaultBluetoothAdapter() :
-            LogSetup("BluetoothAdapter") {
+    DefaultBluetoothAdapter(Mac &mac) :
+            LogSetup("BluetoothAdapter"), mac(mac) {
     }
+
+    Mac const &getMac() override {
+        return mac;
+    };
+
+    Mac &mac;
 };
 
-BluetoothAdapter &getAdapterImpl(int hciDevice);
+shared_ptr<BluetoothAdapter> getAdapterImpl(int hciDevice);
 
 void shutdownImpl();
 
diff --git a/ble/LinuxBluetooth.cpp b/ble/LinuxBluetooth.cpp
index c96d145..d48bfcd 100644
--- a/ble/LinuxBluetooth.cpp
+++ b/ble/LinuxBluetooth.cpp
@@ -28,11 +28,11 @@ class LinuxBluetoothManager;
 
 class LinuxBluetoothAdapter : public DefaultBluetoothAdapter {
 public:
-    LinuxBluetoothAdapter(int hciDeviceId);
+    LinuxBluetoothAdapter(int hciDeviceId, Mac &mac);
 
     ~LinuxBluetoothAdapter();
 
-    void runScan(void (*callback)(BluetoothDevice &device)) override;
+    void runScan(std::function<void(BluetoothDevice &device)>) override;
 
     BluetoothDevice &getDevice(Mac &mac) override;
 
@@ -352,15 +352,20 @@ void LinuxBluetoothGatt::discoverServices() {
             s->addCharacteristic(new DefaultBluetoothGattCharacteristic(*s, c.handle, uuid, properties, valueHandle));
         }
 
-        auto last = values.back();
-
         startHandle = lastHandle + (uint8_t) 2;
     } while (startHandle != 0xffff);
 }
 
 ByteBuffer LinuxBluetoothGatt::writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size) {
 //    LOG_DEBUG("pdu size=" << out.getCursor());
-    ssize_t written = write(l2cap, buffer.get(), out.getCursor());
+    auto to_be_written = out.getCursor();
+
+    ssize_t written = write(l2cap, buffer.get(), to_be_written);
+
+    if (to_be_written != written) {
+        throw BluetoothException(&device, "Expected to write " + to_string(to_be_written) + " but wrote only " +
+                to_string(written));
+    }
 
 //    LOG_DEBUG("written=" << written);
 
@@ -411,7 +416,7 @@ vector<AttributeData> LinuxBluetoothGatt::discoverCharacteristics(uint16_t start
 // Adapter
 // -----------------------------------------------------------------------
 
-LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId) :
+LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId, Mac &mac) : DefaultBluetoothAdapter(mac),
         scanning(false) {
     LOG_DEBUG("hciDeviceId=" << hciDeviceId);
 
@@ -432,6 +437,8 @@ LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId) :
 }
 
 LinuxBluetoothAdapter::~LinuxBluetoothAdapter() {
+    LOG_DEBUG("Stopping scan on device #" << hciDeviceId);
+
     stopScan();
 
     close(hciSocket);
@@ -445,9 +452,11 @@ void LinuxBluetoothAdapter::startScan() {
     struct hci_dev_info di;
 
     if (hci_devinfo(hciDeviceId, &di) < 0) {
-        throw BluetoothException(this, "HCI adapter is not up: " + to_string(hciDeviceId));
+        throw BluetoothException(this, "Could not query device info: " + errnoAsString());
     }
 
+    LOG_INFO("Starting scan on device #" << hciDeviceId);
+
     LOG_DEBUG("hciDeviceId.dev_id=" << di.dev_id);
     LOG_DEBUG("hciDeviceId.bdaddr=" << parseMac(di.bdaddr).str());
     LOG_DEBUG("hciDeviceId.flags=" << setw(8) << setfill('0') << hex << di.flags);
@@ -464,6 +473,11 @@ void LinuxBluetoothAdapter::startScan() {
     }
 
     if (hci_le_set_scan_parameters(hciSocket, 0x01, htobs(0x0010), htobs(0x0010), 0x00, 0, 1000) < 0) {
+        if (errno == EPERM) {
+            throw BluetoothException(this, "hci_le_set_scan_parameters: " + errnoAsString() +
+                                           ". root privileges are probably required");
+        }
+
         throw BluetoothException(this, "hci_le_set_scan_parameters: " + errnoAsString());
     }
 
@@ -479,6 +493,8 @@ void LinuxBluetoothAdapter::stopScan() {
         return;
     }
 
+    LOG_INFO("Stopping scan");
+
     scanning = false;
 
     if (hci_le_set_scan_enable(hciSocket, 0, 0, 1000) < 0) {
@@ -498,14 +514,15 @@ BluetoothDevice &LinuxBluetoothAdapter::getDevice(Mac &mac) {
     return *it->second;
 }
 
-void LinuxBluetoothAdapter::runScan(void (*callback)(BluetoothDevice &device)) {
+void LinuxBluetoothAdapter::runScan(std::function<void(BluetoothDevice &device)> callback) {
     fd_set rfds;
     FD_ZERO(&rfds);
-    FD_SET(hciSocket, &rfds);
 
     startScan();
 
     while (scanning) {
+        FD_SET(hciSocket, &rfds);
+
         // Linux can change tv, so it has to be reinitialized
         struct timeval tv;
         tv.tv_sec = 1;
@@ -514,11 +531,15 @@ void LinuxBluetoothAdapter::runScan(void (*callback)(BluetoothDevice &device)) {
         int selected = select(hciSocket + 1, &rfds, NULL, NULL, &tv);
 
         if (selected == -1) {
-            throw BluetoothException(this, "select() failed");
+            // Someone stopped the scan, so this is no problem
+            if (errno == EINTR && !scanning) {
+                break;
+            }
+            throw BluetoothException(this, "select() failed: " + errnoAsString());
         }
 
         if (selected == 0) {
-            LOG_DEBUG("timeout");
+            LOG_TRACE("select() timed out");
             // Timeout, just continue
             continue;
         }
@@ -541,6 +562,8 @@ void LinuxBluetoothAdapter::runScan(void (*callback)(BluetoothDevice &device)) {
             callback(device);
         }
     }
+
+    stopScan();
 }
 
 }
@@ -555,24 +578,30 @@ namespace trygvis {
 namespace bluetooth {
 using namespace trygvis::bluetooth::linux;
 
-map<int, LinuxBluetoothAdapter *> adapters;
+map<int, shared_ptr<LinuxBluetoothAdapter>> adapters;
 
-BluetoothAdapter &getAdapterImpl(int hciDevice) {
-    map<int, LinuxBluetoothAdapter *>::iterator it = adapters.find(hciDevice);
+shared_ptr<BluetoothAdapter> getAdapterImpl(int hciDevice) {
+    map<int, shared_ptr<LinuxBluetoothAdapter>>::iterator it = adapters.find(hciDevice);
 
     if (it == adapters.end()) {
-        LinuxBluetoothAdapter *adapter = new LinuxBluetoothAdapter(hciDevice);
-        adapters[hciDevice] = adapter;
-        return *adapter;
+        struct hci_dev_info di;
+        if (hci_devinfo(hciDevice, &di) < 0) {
+            throw BluetoothException("Could not query device info: " + errnoAsString());
+        }
+        auto mac = parseMac(di.bdaddr);
+
+        auto adapter = adapters[hciDevice] = make_shared<LinuxBluetoothAdapter>(hciDevice, mac);
+        return adapter;
     }
 
-    return *it->second;
+    return std::static_pointer_cast<BluetoothAdapter, LinuxBluetoothAdapter>(it->second);
 }
 
 void shutdownImpl() {
-    for (auto &pair: adapters) {
-        delete pair.second;
-    }
+    adapters.clear();
+//    for (auto &pair: adapters) {
+//        delete pair.second;
+//    }
 }
 
 }
diff --git a/include/ble/Bluetooth.h b/include/ble/Bluetooth.h
index 97e7cfe..6a974d5 100644
--- a/include/ble/Bluetooth.h
+++ b/include/ble/Bluetooth.h
@@ -157,11 +157,13 @@ public:
 
 class BluetoothAdapter {
 public:
+    virtual Mac const &getMac() = 0;
+
     virtual void startScan() = 0;
 
     virtual void stopScan() = 0;
 
-    virtual void runScan(void (callback)(BluetoothDevice &device)) = 0;
+    virtual void runScan(std::function<void(BluetoothDevice &device)>) = 0;
 
     virtual BluetoothDevice &getDevice(Mac &mac) = 0;
 
@@ -243,7 +245,7 @@ private:
     AttributeData(uint16_t handle, ByteBuffer value);
 };
 
-BluetoothAdapter &getAdapter(int hciDevice);
+shared_ptr<BluetoothAdapter> getAdapter(int hciDevice);
 
 void shutdown();