o Initial import of Linux code for talking to Bluetooth devices.

6 files changed, 490 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..5331b61
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+noble
+.idea
+build
diff --git a/Bluetooth.cpp b/Bluetooth.cpp
new file mode 100644
index 0000000..22f67ec
--- /dev/null
+++ b/Bluetooth.cpp
@@ -0,0 +1,55 @@
+#include <sstream>
+#include <iomanip>
+#include <string.h>
+#include "Bluetooth.h"
+
+namespace trygvis {
+    using namespace std;
+
+    // -----------------------------------------------------------------------
+    // Mac
+    // -----------------------------------------------------------------------
+
+    string Mac::str() const {
+        std::ostringstream buf;
+
+        buf
+                << setw(2) << hex << setfill('0') << (int)bytes[5] << ":"
+                << setw(2) << hex << setfill('0') << (int)bytes[4] << ":"
+                << setw(2) << hex << setfill('0') << (int)bytes[3] << ":"
+                << setw(2) << hex << setfill('0') << (int)bytes[2] << ":"
+                << setw(2) << hex << setfill('0') << (int)bytes[1] << ":"
+                << setw(2) << hex << setfill('0') << (int)bytes[0];
+
+        return buf.str();
+    }
+
+
+    bool Mac::operator==(Mac &other) const {
+        return memcmp(bytes, other.bytes, sizeof(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) {
+        uint8_t bytes[6];
+        sscanf("%02x:%02x:%02x:%02x:%02x:%02x", s.c_str(),
+                &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]);
+    }
+
+    // -----------------------------------------------------------------------
+    // Adapter
+    // -----------------------------------------------------------------------
+
+    BluetoothAdapter::~BluetoothAdapter() {
+    }
+};
diff --git a/Bluetooth.h b/Bluetooth.h
new file mode 100644
index 0000000..2e2f2ef
--- /dev/null
+++ b/Bluetooth.h
@@ -0,0 +1,77 @@
+#ifndef BLUETOOTH_H
+#define BLUETOOTH_H
+
+#include <string>
+#include <stdexcept>
+
+// For now
+#include <boost/log/core.hpp>
+#include <boost/log/trivial.hpp>
+
+#define D BOOST_LOG_TRIVIAL(debug)
+#define I BOOST_LOG_TRIVIAL(info)
+#define W BOOST_LOG_TRIVIAL(warning)
+
+namespace trygvis {
+    using namespace std;
+
+    class BluetoothAdapter;
+    class BluetoothDevice;
+
+    class BluetoothException : public runtime_error {
+    public:
+        BluetoothException(const BluetoothAdapter *adapter, string const &what) :
+                adapter(adapter), device(nullptr), runtime_error(what) {
+        }
+        BluetoothException(const BluetoothAdapter *adapter, const BluetoothDevice *device, string const &what) :
+                adapter(adapter), device(device), runtime_error(what) {
+        }
+
+        const BluetoothAdapter *adapter;
+        const BluetoothDevice *device;
+    };
+
+    class Mac {
+    public:
+        Mac(uint8_t _0, uint8_t _1, uint8_t _2, uint8_t _3, uint8_t _4, uint8_t _5) {
+            bytes[0] = _0;
+            bytes[1] = _1;
+            bytes[2] = _2;
+            bytes[3] = _3;
+            bytes[4] = _4;
+            bytes[5] = _5;
+        };
+
+        string str() 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);
+
+    private:
+        uint8_t bytes[6];
+    };
+
+    class BluetoothDevice {
+    public:
+        virtual Mac const &mac() = 0;
+        virtual void connect() = 0;
+        virtual void disconnect() = 0;
+        virtual BluetoothAdapter& adapter() = 0;
+    };
+
+    class BluetoothAdapter {
+    public:
+        BluetoothAdapter() {};
+        virtual ~BluetoothAdapter();
+
+        virtual void stopScan() = 0;
+        virtual void runScan(void (callback)(BluetoothDevice &device)) = 0;
+    };
+
+//    BluetoothAdapter &getDevice(int hciDevice);
+    BluetoothAdapter *getDevice(int hciDevice);
+}
+
+#endif
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..8a43b50
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 2.8.4)
+project(ble_toys)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+
+set(Boost_USE_STATIC_LIBS OFF)
+set(Boost_USE_MULTITHREADED OFF)
+set(Boost_USE_STATIC_RUNTIME OFF)
+
+add_definitions(-DBOOST_ALL_DYN_LINK)
+
+find_package(Boost REQUIRED COMPONENTS system log thread)
+
+set(SOURCE_FILES main.cpp Bluetooth.cpp LinuxBluetooth.cpp)
+add_executable(ble_toys ${SOURCE_FILES})
+
+target_link_libraries(ble_toys bluetooth)
+target_link_libraries(ble_toys pthread)
+target_link_libraries(ble_toys ${Boost_LIBRARIES})
diff --git a/LinuxBluetooth.cpp b/LinuxBluetooth.cpp
new file mode 100644
index 0000000..ee93dee
--- /dev/null
+++ b/LinuxBluetooth.cpp
@@ -0,0 +1,292 @@
+#include <string.h>
+#include <stropts.h>
+#include <sys/select.h>
+#include <unistd.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 "Bluetooth.h"
+
+// Got to love magic constants. Taken from bluez.git/tools/btgatt-client.c
+#define ATT_CID 4
+
+namespace trygvis {
+
+    class LinuxBluetoothDevice;
+
+    class LinuxBluetoothAdapter;
+
+    class LinuxBluetoothManager;
+
+    class LinuxBluetoothAdapter : public BluetoothAdapter {
+    public:
+        LinuxBluetoothAdapter(int hciDeviceId);
+
+        ~LinuxBluetoothAdapter();
+
+        void runScan(void (*callback)(BluetoothDevice &device));
+
+    private:
+        void startScan();
+
+        void stopScan();
+
+        int hciDeviceId;
+        int hciSocket;
+        struct hci_filter hciFilter;
+        bool scanning;
+    };
+
+    class LinuxBluetoothDevice : public BluetoothDevice {
+    public:
+        LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac mac);
+
+        Mac const &mac() override;
+
+        void connect() override;
+
+        void disconnect() override;
+
+        virtual LinuxBluetoothAdapter &adapter();
+
+    private:
+        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) :
+            _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(&_adapter, 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(&_adapter, 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(&_adapter, 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[0],
+                addr.l2_bdaddr.b[1],
+                addr.l2_bdaddr.b[2],
+                addr.l2_bdaddr.b[3],
+                addr.l2_bdaddr.b[4],
+                addr.l2_bdaddr.b[5]);
+
+        if (::connect(l2cap, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+            close(l2cap);
+            throw BluetoothException(&_adapter, this, "LinuxBluetoothDevice::connect(): connect(): " + errnoAsString());
+        }
+    }
+
+    void LinuxBluetoothDevice::disconnect() {
+
+    }
+
+    // -----------------------------------------------------------------------
+    // Adapter
+    // -----------------------------------------------------------------------
+
+
+    LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId) {
+        D << "LinuxBluetoothAdapter(), 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 " + 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() {
+        D << "~LinuxBluetoothAdapter()";
+
+        stopScan();
+
+        close(hciSocket);
+    }
+
+    void LinuxBluetoothAdapter::startScan() {
+        D << "startScan()";
+
+        struct hci_dev_info di;
+
+        if (hci_devinfo(hciDeviceId, &di) < 0) {
+            throw BluetoothException(this, "hci_devinfo: " + hciDeviceId);
+        }
+
+        D << "hciDeviceId.dev_id=" << di.dev_id;
+        D << "hciDeviceId.bdaddr=" << parseMac(di.bdaddr).str();
+        D << "hciDeviceId.flags=" << hex << setw(8) << setfill('0') << 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: " + 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() {
+        D << "stopScan()";
+
+        if (!scanning) {
+            return;
+        }
+
+        scanning = false;
+
+        if (hci_le_set_scan_enable(hciSocket, 0, 0, 1000) < 0) {
+            W << "stopScan: hci_le_set_scan_enable: " << errnoAsString();
+        }
+    }
+
+    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);
+
+                LinuxBluetoothDevice device = LinuxBluetoothDevice(*this, mac);
+
+                callback(device);
+            }
+        }
+    }
+
+    // -----------------------------------------------------------------------
+    //
+    // -----------------------------------------------------------------------
+
+    /*
+    map<int, LinuxBluetoothAdapter *> adapters;
+
+    BluetoothAdapter &getDevice(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 *getDevice(int hciDevice) {
+        return new LinuxBluetoothAdapter(hciDevice);
+    }
+};
diff --git a/main.cpp b/main.cpp
new file mode 100644
index 0000000..ab1a9b0
--- /dev/null
+++ b/main.cpp
@@ -0,0 +1,44 @@
+#include <exception>
+#include <iostream>
+#include "Bluetooth.h"
+
+using namespace std;
+using namespace trygvis;
+
+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;
+        return;
+    }
+
+    cout << "found device: " << device.mac().str() << endl;
+
+    device.connect();
+    device.disconnect();
+}
+
+int main(int argc, char *argv[]) {
+    if (argc != 2) {
+        cerr << "usage: " << argv[0] << " [mac]" << endl;
+        return EXIT_FAILURE;
+    }
+
+    targetMac = Mac::parseMac(argv[1]);
+
+    try {
+        BluetoothAdapter *adapter = trygvis::getDevice(0);
+
+        adapter->runScan(scan_callback);
+
+        delete adapter;
+
+        return EXIT_SUCCESS;
+    } catch (BluetoothException ex) {
+        W << "Excpetion: " << ex.what();
+        return EXIT_FAILURE;
+    }
+}