1 files changed, 50 insertions, 21 deletions

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;
+//    }
 }
 
 }