o wip.

6 files changed, 213 insertions, 79 deletions

diff --git a/Bluetooth.cpp b/Bluetooth.cpp
index 43faf0a..737f04f 100644
--- a/Bluetooth.cpp
+++ b/Bluetooth.cpp
@@ -1,6 +1,7 @@
 #include <sstream>
 #include <iomanip>
 #include <string.h>
+#include <hwloc.h>
 #include "Bluetooth.h"
 
 namespace trygvis {
@@ -24,9 +25,17 @@ namespace trygvis {
         return buf.str();
     }
 
-
     bool Mac::operator==(Mac &other) const {
-        return memcmp(bytes, other.bytes, sizeof(bytes)) == 0;
+        const uint8_t* b = bytes;
+        return memcmp(b, other.bytes, sizeof(bytes)) == 0;
+    }
+
+    bool Mac::operator!=(Mac &other) const {
+        return !operator==(other);
+    }
+
+    bool operator<(const Mac &a, const Mac &b) {
+        return memcmp(a.bytes, b.bytes, sizeof(a.bytes)) < 0;
     }
 
     void Mac::copy(uint8_t &_0, uint8_t &_1, uint8_t &_2, uint8_t &_3, uint8_t &_4, uint8_t &_5) const {
@@ -38,12 +47,16 @@ namespace trygvis {
         _5 = bytes[5];
     }
 
-    Mac *Mac::parseMac(string s) {
-        uint8_t bytes[6];
-        sscanf("%02x:%02x:%02x:%02x:%02x:%02x", s.c_str(),
+    Mac *Mac::parseMac(string s) throw(BluetoothException) {
+        unsigned int bytes[6];
+        int count = sscanf(s.c_str(), "%02x:%02x:%02x:%02x:%02x:%02x",
                 &bytes[0], &bytes[1], &bytes[2], &bytes[3], &bytes[4], &bytes[5]);
 
-        return new Mac(bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5]);
+        if (count != 6) {
+            throw BluetoothException("Unable to parse mac: " + s);
+        }
+
+        return new Mac((uint8_t) bytes[0], (uint8_t) bytes[1], (uint8_t) bytes[2], (uint8_t) bytes[3], (uint8_t) bytes[4], (uint8_t) bytes[5]);
     }
 
     AttPdu::AttPdu(ByteBuffer &bytes) : bytes(bytes) {
@@ -57,26 +70,66 @@ namespace trygvis {
         return (AttPduType) bytes.get8(0);
     }
 
-    AttPdu AttPdu::parse(ByteBuffer & bytes) {
-        if(size == 0) {
+    void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, uint16_t uuid) {
+        bytes.setCursor(0);
+        bytes.add8(AttPduType::READ_BY_GROUP_TYPE_REQ);
+        bytes.add16le(startHandle);
+        bytes.add16le(endHandle);
+        bytes.add16le(uuid);
+    }
+
+    void AttPdu::checkType(ByteBuffer &bytes, AttPduType type) {
+        if (bytes.getSize() == 0) {
             throw BluetoothException("PDU is too small");
         }
 
-        AttPdu pdu = AttPdu(bytes, size);
+        bytes.setCursor(0);
+        AttPduType t = (AttPduType) bytes.get8();
 
-        AttPduType type = pdu.getType();
+        if (t != type) {
+            throw BluetoothException("Unexpected type: " + t);
+        }
+    }
 
-        switch (type) {
-            case READ_BY_GROUP_TYPE_RES:
-                if (size < 4) {
-                    throw BluetoothException("Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + size);
-                }
-                return pdu;
-            default:
-                throw BluetoothException("Uknown PDU type: " + type);
+    vector<AttributeData*>* AttPdu::parseReadByGroupType(ByteBuffer &bytes) {
+        checkType(bytes, READ_BY_GROUP_TYPE_RES);
+
+        if (bytes.getSize() < 4) {
+            throw BluetoothException("Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + bytes.getSize());
         }
 
-        return pdu;
+        uint8_t length = bytes.get8();
+        DF << "length=" << (int) length;
+
+        size_t count = (bytes.getSize() - 2) / length;
+        DF << "count=" << count;
+
+        vector<AttributeData*> *values = new vector<AttributeData*>;
+        for (int i = 0; i < count; i++) {
+            values->push_back(AttributeData::fromByteBuffer(bytes, length));
+        }
+
+        return values;
+    }
+
+    // -----------------------------------------------------------------------
+    // AttributeData
+    // -----------------------------------------------------------------------
+
+    AttributeData* AttributeData::fromByteBuffer(ByteBuffer &bytes, uint8_t length) {
+        uint16_t handle = bytes.get16le();
+
+        uint8_t* value = new uint8_t[length];
+        bytes.copy(value, length);
+        return new AttributeData(handle, value);
+    }
+
+    AttributeData::AttributeData(uint16_t handle, uint8_t *bytes) : handle(handle), bytes(bytes) {
+
+    }
+
+    AttributeData::~AttributeData() {
+        delete bytes;
     }
 
     // -----------------------------------------------------------------------
diff --git a/Bluetooth.h b/Bluetooth.h
index 9496983..586ac66 100644
--- a/Bluetooth.h
+++ b/Bluetooth.h
@@ -30,15 +30,15 @@ namespace trygvis {
     class BluetoothException : public runtime_error {
     public:
         BluetoothException(const BluetoothAdapter *adapter, string const &what) :
-                adapter(adapter), device(nullptr), runtime_error(what) {
+                runtime_error(what), adapter(adapter), device(nullptr) {
         }
 
         BluetoothException(const BluetoothDevice *device, string const &what) :
-                adapter(nullptr), device(device), runtime_error(what) {
+                runtime_error(what), adapter(nullptr), device(device) {
         }
 
         BluetoothException(string const &what) :
-                adapter(nullptr), device(nullptr), runtime_error(what) {
+                runtime_error(what), adapter(nullptr), device(nullptr) {
         }
 
         const BluetoothAdapter *adapter;
@@ -60,10 +60,13 @@ namespace trygvis {
 
         bool operator==(Mac &other) const;
 
+        bool operator!=(Mac &other) const;
+
         void copy(uint8_t &_0, uint8_t &_1, uint8_t &_2, uint8_t &_3, uint8_t &_4, uint8_t &_5) const;
 
-        static Mac *parseMac(string s);
+        static Mac *parseMac(string s) throw(BluetoothException);
 
+        friend bool operator<(const Mac &a, const Mac &b);
     private:
         uint8_t bytes[6];
     };
@@ -91,6 +94,8 @@ namespace trygvis {
         virtual void stopScan() = 0;
 
         virtual void runScan(void (callback)(BluetoothDevice &device)) = 0;
+
+        virtual BluetoothDevice &getDevice(Mac& mac) = 0;
     };
 
     enum AttPduType {
@@ -99,6 +104,8 @@ namespace trygvis {
         READ_BY_GROUP_TYPE_RES = 0x11
     };
 
+    class AttributeData;
+
     class AttPdu {
     public:
         AttPdu(ByteBuffer &bytes);
@@ -107,29 +114,30 @@ namespace trygvis {
 
         AttPduType getType();
 
-        static AttPdu parse(ByteBuffer & bytes);
-    private:
-        ByteBuffer &bytes;
-    };
+        static vector<AttributeData*>* parseReadByGroupType(ByteBuffer &bytes);
 
-    class AttributeDataList {
-    public:
-        AttributeDataList(ByteBuffer &bytes);
+        static void makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, uint16_t uuid);
 
     private:
+        static void checkType(ByteBuffer &bytes, AttPduType type);
+
         ByteBuffer &bytes;
     };
 
     class AttributeData {
     public:
-        AttributeData(ByteBuffer &bytes);
+        static AttributeData* fromByteBuffer(ByteBuffer &bytes, uint8_t length);
 
     private:
-        ByteBuffer &bytes;
+        AttributeData(uint16_t handle, uint8_t* bytes);
+        ~AttributeData();
+
+        uint16_t handle;
+        uint8_t *bytes;
     };
 
-//    BluetoothAdapter &getDevice(int hciDevice);
-    BluetoothAdapter *getDevice(int hciDevice);
+//    BluetoothAdapter &getAdapter(int hciDevice);
+    BluetoothAdapter *getAdapter(int hciDevice);
 }
 
 #endif
diff --git a/ByteBuffer.cpp b/ByteBuffer.cpp
index 9714c8c..70b9a8e 100644
--- a/ByteBuffer.cpp
+++ b/ByteBuffer.cpp
@@ -1,38 +1,62 @@
 #include "ByteBuffer.h"
-#include <stdexcept>
+#include <stdint-gcc.h>
+#include <string.h>
 
 using namespace std;
 
-ByteBuffer::ByteBuffer(uint8_t *bytes, ssize_t size, ssize_t zero) :
-        bytes(bytes), size(size), ptr(zero), zero(zero) {
+ByteBuffer::ByteBuffer(uint8_t *bytes, size_t capacity, size_t size, size_t zero) :
+        bytes(bytes), capacity(capacity), size(size), cursor(zero), zero(zero) {
 }
 
 ByteBuffer &ByteBuffer::add8(uint8_t value) {
-    canAccessNextBytes(1);
-    bytes[zero + ptr++] = value;
+    checkAndUpdateSize(1);
+    bytes[zero + cursor++] = value;
     return *this;
 }
 
 ByteBuffer &ByteBuffer::add16le(uint16_t value) {
-    canAccessNextBytes(2);
-    bytes[zero + ptr++] = (uint8_t) (value & 0xff);
-    bytes[zero + ptr++] = (uint8_t) ((value >> 8) & 0xff);
+    checkAndUpdateSize(2);
+    bytes[zero + cursor++] = (uint8_t) (value & 0xff);
+    bytes[zero + cursor++] = (uint8_t) ((value >> 8) & 0xff);
     return *this;
 }
 
-uint8_t ByteBuffer::get8(ssize_t index) {
+uint8_t ByteBuffer::get8(size_t index) {
     canAccessIndex(index);
-    return bytes[zero + ptr];
+    return bytes[zero + cursor];
 }
 
-void ByteBuffer::canAccessNextBytes(ssize_t newBytes) {
-    if (zero + ptr + newBytes >= size) {
-        throw exception();
+uint8_t ByteBuffer::get8() {
+    canAccessIndex(cursor);
+    return bytes[zero + cursor++];
+}
+
+uint16_t ByteBuffer::get16le() {
+    canAccessIndex(cursor + 1);
+    uint16_t value;
+    value = bytes[zero + cursor++];
+    value = ((uint16_t) bytes[zero + cursor++]) << 8;
+    return value;
+}
+
+void ByteBuffer::copy(uint8_t *bytes, size_t length) {
+    canAccessIndex(cursor + length);
+
+    memcpy(bytes, &this->bytes[zero + cursor], length);
+    cursor += length;
+}
+
+void ByteBuffer::checkAndUpdateSize(size_t newBytes) {
+    size_t newSize = zero + cursor + newBytes;
+    if (newSize >= capacity) {
+        throw ByteBufferException(string("Out of bounds! zero=") + to_string(zero) + ", cursor=" + to_string(cursor) + ", size=" + to_string(size) + ", capacity=" + to_string(capacity) + ", newSize=" + to_string(newSize));
     }
+
+    size = max(newSize, size);
 }
 
-void ByteBuffer::canAccessIndex(ssize_t index) {
+void ByteBuffer::canAccessIndex(size_t index) {
     if (zero + index >= size) {
-        throw exception();
+        throw ByteBufferException(string("Out of bounds! zero=") + to_string(zero) + ", index=" + to_string(index) + ", size=" + to_string(size));
     }
 }
diff --git a/ByteBuffer.h b/ByteBuffer.h
index a7e9f9f..444943d 100644
--- a/ByteBuffer.h
+++ b/ByteBuffer.h
@@ -3,25 +3,49 @@
 
 #include <cstdint>
 #include <cstdlib>
+#include <string>
+#include <stdexcept>
+
+class ByteBufferException : public std::runtime_error {
+public:
+    ByteBufferException(std::string const &what) : std::runtime_error(what) {
+    }
+};
 
 class ByteBuffer {
 public:
-    ByteBuffer(uint8_t *bytes, ssize_t size, ssize_t zero = 0);
+    ByteBuffer(uint8_t *bytes, size_t capacity, size_t size, size_t zero = 0);
+
+    inline size_t getSize() {
+        return size;
+    }
+
+    inline void setCursor(size_t newCursor) {
+        cursor = newCursor;
+    }
 
     ByteBuffer &add8(uint8_t value);
 
     ByteBuffer &add16le(uint16_t value);
 
-    uint8_t get8(ssize_t index);
+    uint8_t get8(size_t index);
+
+    uint8_t get8();
+
+    uint16_t get16le();
+
+    void copy(uint8_t *bytes, size_t length);
 
 private:
-    void canAccessNextBytes(ssize_t count);
-    void canAccessIndex(ssize_t count);
+    void checkAndUpdateSize(size_t count);
+
+    void canAccessIndex(size_t count);
 
     uint8_t *bytes;
-    ssize_t zero;
-    ssize_t size;
-    ssize_t ptr;
+    size_t zero;
+    size_t size;
+    size_t capacity;
+    size_t cursor;
 };
 
 #endif
diff --git a/LinuxBluetooth.cpp b/LinuxBluetooth.cpp
index 14407c5..d274f84 100644
--- a/LinuxBluetooth.cpp
+++ b/LinuxBluetooth.cpp
@@ -33,6 +33,8 @@ namespace trygvis {
 
         void runScan(void (*callback)(BluetoothDevice &device));
 
+        BluetoothDevice &getDevice(Mac &mac) override;
+
     private:
         void startScan();
 
@@ -149,31 +151,32 @@ namespace trygvis {
         DF;
 
         uint8_t buffer[MAX_MTU];
-        ByteBuffer bytes = ByteBuffer(buffer, MAX_MTU);
+        ByteBuffer out = ByteBuffer(buffer, MAX_MTU, 0);
 
-        AttPdu pdu = AttPdu(bytes, AttPduType::READ_BY_GROUP_TYPE_REQ).
-                add16l(0x0001).
-                add16l(0x1234).
-                add16l(UUID_PRIMARY_SERVICE);
+        AttPdu::makeReadByGroupType(out, 0x0001, 0xffff, UUID_PRIMARY_SERVICE);
 
-        D << "pdu.size()=" << pdu.size();
-        ssize_t written = write(l2cap, bytes, pdu.size());
+        D << "pdu.size()=" << out.getSize();
+        ssize_t written = write(l2cap, buffer, out.getSize());
 
         D << "written=" << written;
 
-        uint8_t *buffer = new uint8_t[MAX_MTU];
-
         ssize_t r = read(l2cap, buffer, MAX_MTU);
+        ByteBuffer in = ByteBuffer(buffer, MAX_MTU, r);
 
         D << "read: " << r << " bytes";
 
-        pdu = AttPdu::parse(buffer, r);
-
-        D << "type= " << pdu.getType();
+        vector<AttributeData*>* values = AttPdu::parseReadByGroupType(in);
 
-        if (pdu.getType() != AttPduType::READ_BY_GROUP_TYPE_REQ) {
+        D << "READ_BY_GROUP_TYPE response has " + (values->size()) + " values";
 
+        for(auto& data: *values) {
+//            strstring s;
+//            s << hex << setfill('0') << setw(2) << ar[i] << " ";
+//            data.bytes
+//            D << data.
         }
+
+        delete values;
     }
 
     // -----------------------------------------------------------------------
@@ -257,6 +260,20 @@ namespace trygvis {
         }
     }
 
+    BluetoothDevice &LinuxBluetoothAdapter::getDevice(Mac &mac) {
+        static map<Mac, LinuxBluetoothDevice *> devices;
+
+        map<Mac, LinuxBluetoothDevice *>::iterator it = devices.find(mac);
+
+        if (it == devices.end()) {
+            LinuxBluetoothDevice *device = new LinuxBluetoothDevice(*this, mac);
+            devices[mac] = device;
+            return *device;
+        }
+
+        return *it->second;
+    }
+
     void LinuxBluetoothAdapter::runScan(void (*callback)(BluetoothDevice &device)) {
         fd_set rfds;
         FD_ZERO(&rfds);
@@ -295,7 +312,7 @@ namespace trygvis {
 
                 Mac mac = parseMac(advertisingInfo->bdaddr);
 
-                LinuxBluetoothDevice device = LinuxBluetoothDevice(*this, mac);
+                BluetoothDevice &device = getDevice(mac);
 
                 callback(device);
             }
@@ -309,7 +326,7 @@ namespace trygvis {
     /*
     map<int, LinuxBluetoothAdapter *> adapters;
 
-    BluetoothAdapter &getDevice(int hciDevice) {
+    BluetoothAdapter &getAdapter(int hciDevice) {
         map<int, LinuxBluetoothAdapter *>::iterator it = adapters.find(hciDevice);
 
         if (it == adapters.end()) {
@@ -322,7 +339,8 @@ namespace trygvis {
     }
     */
 
-    BluetoothAdapter *getDevice(int hciDevice) {
+    BluetoothAdapter *getAdapter(int hciDevice) {
         return new LinuxBluetoothAdapter(hciDevice);
     }
+
 };
diff --git a/main.cpp b/main.cpp
index ad713e8..7caa303 100644
--- a/main.cpp
+++ b/main.cpp
@@ -10,8 +10,8 @@ static Mac *targetMac;
 void scan_callback(BluetoothDevice &device) {
     device.adapter().stopScan();
 
-    if (device.mac() == *targetMac) {
-        cout << "found device: " << device.mac().str() << ", but not the one we want" << endl;
+    if (device.mac() != *targetMac) {
+        cout << "found device: " << device.mac().str() << ", but not the one we want " << targetMac->str() << endl;
         return;
     }
 
@@ -30,18 +30,25 @@ int main(int argc, char *argv[]) {
         return EXIT_FAILURE;
     }
 
-    targetMac = Mac::parseMac(argv[1]);
-
     BluetoothAdapter *adapter = nullptr;
     int e;
     try {
-        adapter = trygvis::getDevice(0);
+        targetMac = Mac::parseMac(argv[1]);
+
+        adapter = trygvis::getAdapter(0);
+
+        BluetoothDevice &device = adapter->getDevice(*targetMac);
 
-        adapter->runScan(scan_callback);
+        scan_callback(device);
+
+//        adapter->runScan(scan_callback);
 
         e = EXIT_SUCCESS;
     } catch (BluetoothException ex) {
-        W << "Excpetion: " << ex.what();
+        W << "BluetoothException: " << ex.what();
+        e = EXIT_FAILURE;
+    } catch (std::exception ex) {
+        W << "std::exception: " << ex.what();
         e = EXIT_FAILURE;
     }