Reorganizing the source code:

o Moving main to apps/
o Moving the library sources to ble/
o Creating cmake files for each piece.

1 files changed, 512 insertions, 0 deletions

diff --git a/ble/LinuxBluetooth.cpp b/ble/LinuxBluetooth.cpp
new file mode 100644
index 0000000..4b7f0e0
--- /dev/null
+++ b/ble/LinuxBluetooth.cpp
@@ -0,0 +1,512 @@
+#include "BluetoothImpl.h"
+
+#include <string.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/l2cap.h>
+#include <map>
+#include <sstream>
+#include <iomanip>
+#include <numeric>
+
+// Got to love magic constants. Taken from bluez.git/tools/btgatt-client.c
+#define ATT_CID 4
+
+#define MAX_MTU 256
+
+namespace trygvis {
+namespace bluetooth {
+namespace linux {
+
+typedef boost::uuids::uuid uuid_t;
+
+using namespace uuids;
+
+class LinuxBluetoothDevice;
+
+class LinuxBluetoothAdapter;
+
+class LinuxBluetoothManager;
+
+class LinuxBluetoothAdapter : public BluetoothAdapter {
+public:
+    LinuxBluetoothAdapter(int hciDeviceId);
+
+    ~LinuxBluetoothAdapter();
+
+    void runScan(void (*callback)(BluetoothDevice &device)) override;
+
+    BluetoothDevice &getDevice(Mac &mac) override;
+
+private:
+    void startScan() override;
+
+    void stopScan() override;
+
+    int hciDeviceId;
+    int hciSocket;
+    struct hci_filter hciFilter;
+    bool scanning;
+
+    map<Mac, LinuxBluetoothDevice *> devices;
+};
+
+class LinuxBluetoothDevice : public DefaultBluetoothDevice {
+public:
+    LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac mac);
+
+    Mac const &mac() override;
+
+    LinuxBluetoothAdapter &adapter() override;
+
+    void connect() override;
+
+    void disconnect() override;
+
+    void discoverServices() override;
+
+private:
+    vector<AttributeData> discoverServices(uint16_t startHandle);
+
+    vector<AttributeData> discoverCharacteristics(uint16_t startHandle, uint16_t endHandle);
+
+    ByteBuffer writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size);
+
+    uuid_t readUuid(const ByteBuffer &bytes) const;
+
+    LinuxBluetoothAdapter &_adapter;
+    Mac _mac;
+    int l2cap;
+};
+
+// Utilities
+
+string errnoAsString() {
+    return string(strerror(errno));
+};
+
+// -----------------------------------------------------------------------
+// Mac
+// -----------------------------------------------------------------------
+
+Mac parseMac(bdaddr_t &a) {
+    return Mac(a.b[0], a.b[1], a.b[2], a.b[3], a.b[4], a.b[5]);
+}
+
+// -----------------------------------------------------------------------
+// Device
+// -----------------------------------------------------------------------
+
+LinuxBluetoothDevice::LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac mac) :
+        DefaultBluetoothDevice(), _adapter(adapter), _mac(mac) {
+}
+
+Mac const &LinuxBluetoothDevice::mac() {
+    return _mac;
+}
+
+LinuxBluetoothAdapter &LinuxBluetoothDevice::adapter() {
+    return _adapter;
+}
+
+void LinuxBluetoothDevice::connect() {
+    struct sockaddr_l2 addr;
+
+    D << "connect: mac=" << _mac.str();
+
+    l2cap = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+    if (l2cap < 0) {
+        throw BluetoothException(this, "LinuxBluetoothDevice::connect(): socket(): " + errnoAsString());
+    }
+
+    memset(&addr, 0, sizeof(addr));
+    addr.l2_family = AF_BLUETOOTH;
+    addr.l2_bdaddr = {{0, 0, 0, 0, 0, 0}};
+    addr.l2_cid = htobs(ATT_CID);
+    addr.l2_bdaddr_type = BDADDR_LE_PUBLIC;
+
+    if (bind(l2cap, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+        close(l2cap);
+        throw BluetoothException(this, "LinuxBluetoothDevice::connect(): bind(): " + errnoAsString());
+    }
+
+    struct bt_security btsec;
+    memset(&btsec, 0, sizeof(btsec));
+    btsec.level = BT_SECURITY_LOW;
+    if (setsockopt(l2cap, SOL_BLUETOOTH, BT_SECURITY, &btsec, sizeof(btsec)) != 0) {
+        close(l2cap);
+        throw BluetoothException(this, "LinuxBluetoothDevice::connect(): setsockopt(): " + errnoAsString());
+    }
+
+    memset(&addr, 0, sizeof(addr));
+    addr.l2_family = AF_BLUETOOTH;
+    addr.l2_cid = htobs(ATT_CID);
+    addr.l2_bdaddr_type = BDADDR_LE_RANDOM;
+    _mac.copy(addr.l2_bdaddr.b[5],
+            addr.l2_bdaddr.b[4],
+            addr.l2_bdaddr.b[3],
+            addr.l2_bdaddr.b[2],
+            addr.l2_bdaddr.b[1],
+            addr.l2_bdaddr.b[0]);
+
+    if (::connect(l2cap, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+        close(l2cap);
+        throw BluetoothException(this, "LinuxBluetoothDevice::connect(): connect(): " + errnoAsString());
+    }
+}
+
+void LinuxBluetoothDevice::disconnect() {
+    DF << "mac = " << _mac.str();
+    close(l2cap);
+}
+
+uuid_t LinuxBluetoothDevice::readUuid(const ByteBuffer &bytes) const {
+    size_t bytesLeft = bytes.getBytesLeft();
+
+    uuid_t u;
+
+    if (bytesLeft == 2) {
+        uint8_t bs[16] = BLUETOOTH_UUID_INITIALIZER;
+        bs[2] = bytes.get8(1);
+        bs[3] = bytes.get8(0);
+        memcpy(&u, bs, 16);
+    } else if (bytesLeft == 16) {
+        uint8_t bs[16];
+        bs[15] = bytes.get8(0);
+        bs[14] = bytes.get8(1);
+        bs[13] = bytes.get8(2);
+        bs[12] = bytes.get8(3);
+        bs[11] = bytes.get8(4);
+        bs[10] = bytes.get8(5);
+        bs[9] = bytes.get8(6);
+        bs[8] = bytes.get8(7);
+        bs[7] = bytes.get8(8);
+        bs[6] = bytes.get8(9);
+        bs[5] = bytes.get8(10);
+        bs[4] = bytes.get8(11);
+        bs[3] = bytes.get8(12);
+        bs[2] = bytes.get8(13);
+        bs[1] = bytes.get8(14);
+        bs[0] = bytes.get8(15);
+        memcpy(&u, bs, 16);
+    } else {
+        throw BluetoothException(this, "Unexpected bytes left: " + to_string(bytesLeft));
+    }
+
+    return u;
+}
+
+void LinuxBluetoothDevice::discoverServices() {
+    uint16_t startHandle = 0x0001;
+
+    removeServices();
+
+    do {
+        vector<AttributeData> values = discoverServices(startHandle);
+
+        // Shouldn't happen, but you never know.
+        if (values.size() == 0) {
+            break;
+        }
+
+        uint16_t endGroupHandle;
+
+        for (auto &data: values) {
+            endGroupHandle = data.value.read16le();
+
+//            D << "handle: 0x" << hex << setw(4) << setfill('0') << data.handle <<
+//                        ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << endGroupHandle <<
+//                        ", value: " << data.value.toString();
+
+            uuid_t u = readUuid(data.value);
+
+            addService(new DefaultBluetoothGattService(*this, u, data.handle, endGroupHandle));
+        }
+
+        auto last = values.back();
+
+        startHandle = endGroupHandle;
+    } while (startHandle != 0xffff);
+
+//    for (auto &s : services) {
+//        D << "service: " << to_string(s->getUuid()) << ", handle: " << s->getHandle() << ", end group handle: " << s->getEndGroupHandle();
+//        D << "s= " << (uint64_t) s;
+//    }
+
+    auto it = services.begin(),
+            end = services.end();
+
+    if (it == end) {
+        return;
+    }
+
+    startHandle = 0x0001;
+    vector<AttributeData> chars;
+
+    auto s = *it;
+
+    do {
+        vector<AttributeData> values = discoverCharacteristics(startHandle, 0xffff);
+
+        if (values.size() == 0) {
+            break;
+        }
+
+        uint16_t lastHandle = 0xffff;
+
+        for (auto &c : values) {
+            while (c.handle > s->getEndGroupHandle() && it != end) {
+                s = *it++;
+//                D << "service: " << s->getHandle();
+            }
+
+            lastHandle = c.handle;
+
+            uint8_t properties = c.value.read8();
+            uint16_t valueHandle = c.value.read16le();
+            uuid_t uuid = readUuid(c.value);
+
+//            D << "characteristic: handle: " << setw(2) << setfill('0') << hex << (int) c.handle <<
+//                        ", properties: " << setw(2) << setfill('0') << hex << (int) properties <<
+//                        ", valueHandle: 0x" << setw(4) << setfill('0') << hex << (int) valueHandle <<
+//                        ", uuid: " << uuid;
+
+            s->addCharacteristic(new DefaultBluetoothGattCharacteristic(*s, c.handle, uuid, properties, valueHandle));
+        }
+
+        auto last = values.back();
+
+        startHandle = lastHandle + (uint8_t) 2;
+    } while (startHandle != 0xffff);
+}
+
+ByteBuffer LinuxBluetoothDevice::writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size) {
+    D << "pdu size=" << out.getCursor();
+    ssize_t written = write(l2cap, buffer.get(), out.getCursor());
+
+    D << "written=" << written;
+
+    ssize_t r = read(l2cap, buffer.get(), size);
+
+    if (r == -1) {
+        throw BluetoothException(this, "read(): " + errnoAsString());
+    }
+
+    auto in = ByteBuffer(buffer, (size_t) r);
+
+    D << "read: " << r << " bytes: " << in.toString();
+
+    return in;
+}
+
+vector<AttributeData> LinuxBluetoothDevice::discoverServices(uint16_t startHandle) {
+    DF;
+
+    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
+    ByteBuffer out = ByteBuffer(buffer, MAX_MTU);
+
+    AttPdu::makeReadByGroupType(out, startHandle, 0xffff, uuids::PRIMARY_SERVICE);
+
+    ByteBuffer in = writeAndRead(out, buffer, MAX_MTU);
+
+    vector<AttributeData> values = AttPdu::parseReadByGroupType(in);
+
+    D << "READ_BY_GROUP_TYPE response has " + to_string(values.size()) + " values";
+
+    return values;
+}
+
+vector<AttributeData> LinuxBluetoothDevice::discoverCharacteristics(uint16_t startHandle, uint16_t endHandle) {
+    DF;
+
+    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
+    ByteBuffer out = ByteBuffer(buffer, MAX_MTU);
+
+    AttPdu::makeReadByType(out, startHandle, endHandle, uuids::CHARACTERISTIC);
+
+    ByteBuffer in = writeAndRead(out, buffer, MAX_MTU);
+
+    vector<AttributeData> values = AttPdu::parseReadByType(in);
+
+    D << "READ_BY_TYPE response has " + to_string(values.size()) + " values";
+
+    return values;
+}
+
+// -----------------------------------------------------------------------
+// Adapter
+// -----------------------------------------------------------------------
+
+LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId) :
+        scanning(false) {
+    DF << "hciDeviceId=" << hciDeviceId;
+
+    this->hciDeviceId = hciDeviceId;
+    hciSocket = ::hci_open_dev(hciDeviceId);
+
+    D << "HCI socket: " << hciSocket;
+
+    if (hciSocket == -1) {
+        throw BluetoothException(this, "Could not open HCI device " + to_string(hciDeviceId));
+    }
+
+    hci_filter_clear(&hciFilter);
+    hci_filter_set_ptype(HCI_EVENT_PKT, &hciFilter);
+    hci_filter_set_event(EVT_LE_META_EVENT, &hciFilter);
+    hci_filter_set_event(EVT_LE_ADVERTISING_REPORT, &hciFilter);
+    setsockopt(hciSocket, SOL_HCI, HCI_FILTER, &hciFilter, sizeof(hciFilter));
+}
+
+LinuxBluetoothAdapter::~LinuxBluetoothAdapter() {
+    DF;
+
+    stopScan();
+
+    close(hciSocket);
+
+    for (auto &pair : devices) {
+        delete pair.second;
+    }
+}
+
+void LinuxBluetoothAdapter::startScan() {
+    DF;
+
+    struct hci_dev_info di;
+
+    if (hci_devinfo(hciDeviceId, &di) < 0) {
+        throw BluetoothException(this, "HCI adapter is not up: " + to_string(hciDeviceId));
+    }
+
+    D << "hciDeviceId.dev_id=" << di.dev_id;
+    D << "hciDeviceId.bdaddr=" << parseMac(di.bdaddr).str();
+    D << "hciDeviceId.flags=" << setw(8) << setfill('0') << hex << di.flags;
+    D << "hciDeviceId.flags RUNNING = " << hci_test_bit(HCI_RUNNING, &di.flags);
+    D << "hciDeviceId.flags UP      = " << hci_test_bit(HCI_UP, &di.flags);
+    D << "hciDeviceId.flags PSCAN   = " << hci_test_bit(HCI_PSCAN, &di.flags);
+    D << "hciDeviceId.flags ISCAN   = " << hci_test_bit(HCI_ISCAN, &di.flags);
+    D << "hciDeviceId.name=" << di.name;
+
+    int up = hci_test_bit(HCI_UP, &di.flags);
+
+    if (!up) {
+        throw BluetoothException(this, "HCI adapter is not up: " + to_string(hciDeviceId));
+    }
+
+    if (hci_le_set_scan_parameters(hciSocket, 0x01, htobs(0x0010), htobs(0x0010), 0x00, 0, 1000) < 0) {
+        throw BluetoothException(this, "hci_le_set_scan_parameters: " + errnoAsString());
+    }
+
+    if (hci_le_set_scan_enable(hciSocket, 1, 0, 1000)) {
+        throw BluetoothException(this, "Could not start scanning: other" + errnoAsString());
+    }
+
+    scanning = true;
+}
+
+void LinuxBluetoothAdapter::stopScan() {
+    DF;
+
+    if (!scanning) {
+        return;
+    }
+
+    scanning = false;
+
+    if (hci_le_set_scan_enable(hciSocket, 0, 0, 1000) < 0) {
+        W << "stopScan: hci_le_set_scan_enable: " << errnoAsString();
+    }
+}
+
+BluetoothDevice &LinuxBluetoothAdapter::getDevice(Mac &mac) {
+    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);
+    FD_SET(hciSocket, &rfds);
+
+    startScan();
+
+    while (scanning) {
+        // Linux can change tv, so it has to be reinitialized
+        struct timeval tv;
+        tv.tv_sec = 1;
+        tv.tv_usec = 0;
+
+        int selected = select(hciSocket + 1, &rfds, NULL, NULL, &tv);
+
+        if (selected == -1) {
+            throw BluetoothException(this, "select() failed");
+        }
+
+        if (selected == 0) {
+            D << "timeout";
+            // Timeout, just continue
+            continue;
+        }
+
+        unsigned char hciEventBuf[HCI_MAX_EVENT_SIZE];
+
+        ssize_t len = read(hciSocket, hciEventBuf, sizeof(hciEventBuf));
+        evt_le_meta_event *metaEvent = (evt_le_meta_event *) (hciEventBuf + (1 + HCI_EVENT_HDR_SIZE));
+        len -= (1 + HCI_EVENT_HDR_SIZE);
+
+        D << "metaEvent->subevent = " << std::hex << (int) metaEvent->subevent;
+
+        if (metaEvent->subevent == EVT_LE_ADVERTISING_REPORT) {
+            le_advertising_info *advertisingInfo = (le_advertising_info *) (metaEvent->data + 1);
+
+            Mac mac = parseMac(advertisingInfo->bdaddr);
+
+            BluetoothDevice &device = getDevice(mac);
+
+            callback(device);
+        }
+    }
+}
+
+}
+}
+}
+
+// -----------------------------------------------------------------------
+// Implementation of platform-specific method.
+// -----------------------------------------------------------------------
+
+namespace trygvis {
+namespace bluetooth {
+using namespace trygvis::bluetooth::linux;
+
+map<int, LinuxBluetoothAdapter *> adapters;
+
+BluetoothAdapter &getAdapterImpl(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;
+}
+
+void shutdownImpl() {
+    for (auto &pair: adapters) {
+        delete pair.second;
+    }
+}
+
+}
+}