o wip.

1 files changed, 72 insertions, 19 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;
     }
 
     // -----------------------------------------------------------------------