1 files changed, 193 insertions, 0 deletions
diff --git a/ble/Bluetooth.cpp b/ble/Bluetooth.cpp
new file mode 100644
index 0000000..b135282
--- /dev/null
+++ b/ble/Bluetooth.cpp
@@ -0,0 +1,193 @@
+#include "Bluetooth.h"
+#include "BluetoothImpl.h"
+
+#include <sstream>
+#include <iomanip>
+#include <string.h>
+
+namespace trygvis {
+namespace bluetooth {
+using namespace std;
+
+// -----------------------------------------------------------------------
+// Mac
+// -----------------------------------------------------------------------
+
+string Mac::str() const {
+    std::ostringstream buf;
+
+    buf
+            << setw(2) << hex << setfill('0') << (int) bytes[0] << ":"
+            << setw(2) << hex << setfill('0') << (int) bytes[1] << ":"
+            << setw(2) << hex << setfill('0') << (int) bytes[2] << ":"
+            << setw(2) << hex << setfill('0') << (int) bytes[3] << ":"
+            << setw(2) << hex << setfill('0') << (int) bytes[4] << ":"
+            << setw(2) << hex << setfill('0') << (int) bytes[5];
+
+    return buf.str();
+}
+
+bool Mac::operator==(Mac &other) const {
+    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 {
+    _0 = bytes[0];
+    _1 = bytes[1];
+    _2 = bytes[2];
+    _3 = bytes[3];
+    _4 = bytes[4];
+    _5 = bytes[5];
+}
+
+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]);
+
+    if (count != 6) {
+        throw BluetoothException("Unable to parse mac: " + s);
+    }
+
+    return 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) {
+}
+
+AttPdu::AttPdu(ByteBuffer &bytes, AttPduType type) : bytes(bytes) {
+    bytes.write8(type);
+}
+
+AttPduType AttPdu::getType() {
+    return (AttPduType) bytes.get8(0);
+}
+
+void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, SpecUuid uuid) {
+    bytes.write8(AttPduType::READ_BY_GROUP_TYPE_REQ);
+    bytes.write16le(startHandle);
+    bytes.write16le(endHandle);
+    bytes.write16le(uuid.value);
+}
+
+void AttPdu::makeReadByType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, SpecUuid uuid) {
+    bytes.write8(AttPduType::READ_BY_TYPE_REQ);
+    bytes.write16le(startHandle);
+    bytes.write16le(endHandle);
+    bytes.write16le(uuid.value);
+}
+
+vector<AttributeData> AttPdu::parse(ByteBuffer &bytes, AttPduType type) {
+    DF << "bytes: " << bytes.toString();
+
+    AttPduType t = (AttPduType) bytes.read8();
+
+    if (t == INVALID_HANDLE) {
+        return vector<AttributeData>();
+    }
+
+    if (t != type) {
+        throw BluetoothException("Unexpected type: " + to_string(t));
+    }
+
+    if (bytes.getSize() < 4) {
+        throw BluetoothException("Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + to_string(bytes.getSize()));
+    }
+
+    uint8_t length = bytes.read8();
+    D << "length=" << (int) length;
+
+    size_t count = (bytes.getSize() - 2) / length;
+    D << "count=" << count;
+
+    vector<AttributeData> values;
+    for (int i = 0; i < count; i++) {
+        auto data = bytes.view(length);
+        D << "data, size=" << data.getSize() << ", bytes=" << data.toString();
+        bytes.skip(length);
+        values.push_back(AttributeData::fromByteBuffer(data));
+    }
+
+    return values;
+}
+
+vector<AttributeData> AttPdu::parseReadByGroupType(ByteBuffer &bytes) {
+    return parse(bytes, READ_BY_GROUP_TYPE_RES);
+}
+
+vector<AttributeData> AttPdu::parseReadByType(ByteBuffer &bytes) {
+    return parse(bytes, READ_BY_TYPE_RES);
+}
+
+// -----------------------------------------------------------------------
+// AttributeData
+// -----------------------------------------------------------------------
+
+AttributeData AttributeData::fromByteBuffer(ByteBuffer &bytes) {
+    uint16_t handle = bytes.read16le();
+
+    return AttributeData(handle, bytes.view());
+}
+
+AttributeData::AttributeData(uint16_t handle, ByteBuffer value) :
+        handle(handle), value(value) {
+}
+
+AttributeData::~AttributeData() {
+}
+
+// -----------------------------------------------------------------------
+// Device
+// -----------------------------------------------------------------------
+
+BluetoothDevice::BluetoothDevice() {
+}
+
+BluetoothDevice::~BluetoothDevice() {
+}
+
+// -----------------------------------------------------------------------
+// Adapter
+// -----------------------------------------------------------------------
+
+BluetoothAdapter::BluetoothAdapter() {
+}
+
+BluetoothAdapter::~BluetoothAdapter() {
+}
+
+/*
+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) {
+    return getAdapterImpl(hciDevice);
+}
+
+void shutdown() {
+    shutdownImpl();
+}
+
+}
+};