1 files changed, 37 insertions, 25 deletions

diff --git a/ble/LinuxBluetooth.cpp b/ble/LinuxBluetooth.cpp
index ae8f984..6e3c9da 100644
--- a/ble/LinuxBluetooth.cpp
+++ b/ble/LinuxBluetooth.cpp
@@ -5,7 +5,6 @@
 #include <bluetooth/hci_lib.h>
 #include <bluetooth/l2cap.h>
 #include <map>
-#include <sstream>
 #include <iomanip>
 
 // Got to love magic constants. Taken from bluez.git/tools/btgatt-client.c
@@ -56,10 +55,10 @@ public:
 
     ~LinuxBluetoothDevice();
 
-    BluetoothGatt &connectGatt() override;
+    shared_ptr<BluetoothGatt> connectGatt() override;
 
 private:
-    LinuxBluetoothGatt *gatt;
+    weak_ptr<LinuxBluetoothGatt> gatt;
     int l2cap;
 };
 
@@ -69,9 +68,7 @@ public:
 
     ~LinuxBluetoothGatt();
 
-    void connect() override;
-
-    void disconnect() override;
+    bool isConnected() const override;
 
     void discoverServices() override;
 
@@ -80,6 +77,10 @@ public:
     ByteBuffer readValue(const BluetoothGattCharacteristic &c) override;
 
 private:
+    void connect();
+
+    void disconnect();
+
     vector<AttributeData> discoverServices(uint16_t startHandle);
 
     vector<AttributeData> discoverCharacteristics(uint16_t startHandle, uint16_t endHandle);
@@ -87,6 +88,8 @@ private:
     ByteBuffer writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size);
 
     int l2cap;
+
+    bool connected;
 };
 
 // Utilities
@@ -108,23 +111,24 @@ Mac parseMac(bdaddr_t &a) {
 // -----------------------------------------------------------------------
 
 LinuxBluetoothDevice::LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac &mac) :
-        DefaultBluetoothDevice(adapter, mac), gatt(nullptr) {
+        DefaultBluetoothDevice(adapter, mac) {
 }
 
 LinuxBluetoothDevice::~LinuxBluetoothDevice() {
-    if (gatt) {
-        delete gatt;
-    }
+//    if (gatt) {
+//        delete gatt;
+//    }
 };
 
-BluetoothGatt &LinuxBluetoothDevice::connectGatt() {
-    if (!gatt) {
-        gatt = new LinuxBluetoothGatt(*this, l2cap);
+shared_ptr<BluetoothGatt> LinuxBluetoothDevice::connectGatt() {
+    if (auto p = gatt.lock()) {
+        return p;
     }
+    auto ref = make_shared<LinuxBluetoothGatt>(*this, l2cap);
 
-    gatt->connect();
+    gatt = ref;
 
-    return *gatt;
+    return ref;
 }
 
 // -----------------------------------------------------------------------
@@ -133,9 +137,15 @@ BluetoothGatt &LinuxBluetoothDevice::connectGatt() {
 
 LinuxBluetoothGatt::LinuxBluetoothGatt(LinuxBluetoothDevice &device, int l2cap) :
         DefaultBluetoothGatt(device), l2cap(l2cap) {
+    connect();
 }
 
 LinuxBluetoothGatt::~LinuxBluetoothGatt() {
+    disconnect();
+}
+
+bool LinuxBluetoothGatt::isConnected() const {
+    return connected;
 }
 
 void LinuxBluetoothGatt::connect() {
@@ -145,7 +155,7 @@ void LinuxBluetoothGatt::connect() {
 
     l2cap = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
     if (l2cap < 0) {
-        throw BluetoothException(&device, "LinuxBluetoothGatt::connect(): socket(): " + errnoAsString());
+        throw BluetoothException(&device, "connect(): socket(): " + errnoAsString());
     }
 
     memset(&addr, 0, sizeof(addr));
@@ -156,7 +166,7 @@ void LinuxBluetoothGatt::connect() {
 
     if (bind(l2cap, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
         close(l2cap);
-        throw BluetoothException(&device, "LinuxBluetoothGatt::connect(): bind(): " + errnoAsString());
+        throw BluetoothException(&device, "bind(): " + errnoAsString());
     }
 
     struct bt_security btsec;
@@ -164,7 +174,7 @@ void LinuxBluetoothGatt::connect() {
     btsec.level = BT_SECURITY_LOW;
     if (setsockopt(l2cap, SOL_BLUETOOTH, BT_SECURITY, &btsec, sizeof(btsec)) != 0) {
         close(l2cap);
-        throw BluetoothException(&device, "LinuxBluetoothGatt::connect(): setsockopt(): " + errnoAsString());
+        throw BluetoothException(&device, "setsockopt(): " + errnoAsString());
     }
 
     memset(&addr, 0, sizeof(addr));
@@ -181,11 +191,14 @@ void LinuxBluetoothGatt::connect() {
 
     if (::connect(l2cap, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
         close(l2cap);
-        throw BluetoothException(&device, "LinuxBluetoothGatt::connect(): connect(): " + errnoAsString());
+        throw BluetoothException(&device, "connect(): " + errnoAsString());
     }
 }
 
 void LinuxBluetoothGatt::disconnect() {
+    if (!connected) {
+        return;
+    }
     LOG_DEBUG("mac = " << device.getMac().str());
     close(l2cap);
 }
@@ -271,7 +284,7 @@ void LinuxBluetoothGatt::discoverServices() {
             break;
         }
 
-        uint16_t endGroupHandle;
+        uint16_t endGroupHandle = startHandle;
 
         for (auto &data: values) {
             endGroupHandle = data.value.read16le();
@@ -285,7 +298,7 @@ void LinuxBluetoothGatt::discoverServices() {
             addService(new DefaultBluetoothGattService(device, u, data.handle, endGroupHandle));
         }
 
-        auto last = values.back();
+//        auto last = values.back();
 
         startHandle = endGroupHandle;
     } while (startHandle != 0xffff);
@@ -303,7 +316,6 @@ void LinuxBluetoothGatt::discoverServices() {
     }
 
     startHandle = 0x0001;
-    vector<AttributeData> chars;
 
     auto s = *it;
 
@@ -343,10 +355,10 @@ void LinuxBluetoothGatt::discoverServices() {
 }
 
 ByteBuffer LinuxBluetoothGatt::writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size) {
-    LOG_DEBUG("pdu size=" << out.getCursor());
+//    LOG_DEBUG("pdu size=" << out.getCursor());
     ssize_t written = write(l2cap, buffer.get(), out.getCursor());
 
-    LOG_DEBUG("written=" << written);
+//    LOG_DEBUG("written=" << written);
 
     ssize_t r = read(l2cap, buffer.get(), size);
 
@@ -356,7 +368,7 @@ ByteBuffer LinuxBluetoothGatt::writeAndRead(ByteBuffer &out, shared_ptr<uint8_t>
 
     auto in = ByteBuffer(buffer, (size_t) r, (size_t) r);
 
-    LOG_DEBUG("read: " << r << " bytes: " << in.toString());
+//    LOG_DEBUG("read: " << r << " bytes: " << in.toString());
 
     return in;
 }