aboutsummaryrefslogtreecommitdiff
path: root/ble
diff options
context:
space:
mode:
Diffstat (limited to 'ble')
-rw-r--r--ble/Bluetooth.cpp193
-rw-r--r--ble/Bluetooth.h206
-rw-r--r--ble/BluetoothImpl.h160
-rw-r--r--ble/ByteBuffer.cpp102
-rw-r--r--ble/ByteBuffer.h91
-rw-r--r--ble/CMakeLists.txt3
-rw-r--r--ble/LinuxBluetooth.cpp512
-rw-r--r--ble/log.h14
8 files changed, 1281 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();
+}
+
+}
+};
diff --git a/ble/Bluetooth.h b/ble/Bluetooth.h
new file mode 100644
index 0000000..e47ff3e
--- /dev/null
+++ b/ble/Bluetooth.h
@@ -0,0 +1,206 @@
+#ifndef BLUETOOTH_H
+#define BLUETOOTH_H
+
+#include <string>
+#include <stdexcept>
+#include <boost/uuid/uuid.hpp>
+#include <boost/optional.hpp>
+
+#include "ByteBuffer.h"
+
+namespace trygvis {
+namespace bluetooth {
+
+using namespace std;
+
+struct SpecUuid {
+public:
+ SpecUuid(uint16_t value) : value(value) {}
+ uint16_t value;
+};
+
+namespace uuids {
+static const SpecUuid PRIMARY_SERVICE = SpecUuid(0x2800);
+static const SpecUuid SECONDARY_SERVICE = SpecUuid(0x2801);
+static const SpecUuid CHARACTERISTIC = SpecUuid(0x2803);
+}
+
+class BluetoothAdapter;
+
+class BluetoothDevice;
+
+class BluetoothGattService;
+
+class BluetoothGattCharacteristic;
+
+class BluetoothException : public runtime_error {
+public:
+ BluetoothException(const BluetoothAdapter *adapter, string const &what) :
+ runtime_error(what), adapter(adapter), device(nullptr) {
+ }
+
+ BluetoothException(const BluetoothDevice *device, string const &what) :
+ runtime_error(what), adapter(nullptr), device(device) {
+ }
+
+ BluetoothException(string const &what) :
+ runtime_error(what), adapter(nullptr), device(nullptr) {
+ }
+
+ 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;
+
+ 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) throw(BluetoothException);
+
+ friend bool operator<(const Mac &a, const Mac &b);
+
+private:
+ uint8_t bytes[6];
+};
+
+class BluetoothGattCharacteristic {
+public:
+ virtual ~BluetoothGattCharacteristic() {
+ };
+
+ virtual BluetoothGattService &getService() const = 0;
+
+ virtual uint16_t getHandle() const = 0;
+
+ virtual const boost::uuids::uuid getUuid() const = 0;
+
+ virtual uint8_t getProperties() const = 0;
+
+ virtual uint16_t getValueHandle() const = 0;
+};
+
+class BluetoothGattService {
+public:
+ virtual ~BluetoothGattService() {
+ };
+
+ virtual BluetoothDevice &getDevice() const = 0;
+
+ virtual boost::uuids::uuid getUuid() const = 0;
+
+ virtual uint16_t getHandle() const = 0;
+
+ virtual uint16_t getEndGroupHandle() const = 0;
+
+ virtual const vector<BluetoothGattCharacteristic *> getCharacteristics() const = 0;
+
+ virtual void addCharacteristic(BluetoothGattCharacteristic* characteristic) = 0;
+};
+
+class BluetoothDevice {
+public:
+ BluetoothDevice();
+
+ virtual ~BluetoothDevice();
+
+ virtual Mac const &mac() = 0;
+
+ virtual BluetoothAdapter &adapter() = 0;
+
+ virtual void connect() = 0;
+
+ virtual void disconnect() = 0;
+
+ virtual void discoverServices() = 0;
+
+ virtual const vector<BluetoothGattService *> getServices() const = 0;
+
+ virtual const boost::optional<BluetoothGattService*> findService(boost::uuids::uuid uuid) const = 0;
+};
+
+class BluetoothAdapter {
+public:
+ virtual void startScan() = 0;
+
+ virtual void stopScan() = 0;
+
+ virtual void runScan(void (callback)(BluetoothDevice &device)) = 0;
+
+ virtual BluetoothDevice &getDevice(Mac &mac) = 0;
+
+protected:
+ BluetoothAdapter();
+
+ virtual ~BluetoothAdapter();
+};
+
+enum AttPduType {
+ ERROR = 0x00,
+ INVALID_HANDLE = 0x01,
+ READ_BY_TYPE_REQ = 0x08,
+ READ_BY_TYPE_RES = 0x09,
+ READ_BY_GROUP_TYPE_REQ = 0x10,
+ READ_BY_GROUP_TYPE_RES = 0x11,
+};
+
+class AttributeData;
+
+class AttPdu {
+public:
+ AttPdu(ByteBuffer &bytes);
+
+ AttPdu(ByteBuffer &bytes, AttPduType type);
+
+ AttPduType getType();
+
+ static vector<AttributeData> parseReadByGroupType(ByteBuffer &bytes);
+
+ static vector<AttributeData> parseReadByType(ByteBuffer &bytes);
+
+ static void makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, SpecUuid uuid);
+
+ static void makeReadByType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, SpecUuid uuid);
+
+private:
+ static void checkType(ByteBuffer &bytes, AttPduType type);
+ static vector<AttributeData> parse(ByteBuffer &bytes, AttPduType type);
+
+ ByteBuffer &bytes;
+};
+
+class AttributeData {
+public:
+ ~AttributeData();
+
+ static AttributeData fromByteBuffer(ByteBuffer &value);
+
+ const uint16_t handle;
+ ByteBuffer value;
+
+private:
+ AttributeData(uint16_t handle, ByteBuffer value);
+};
+
+BluetoothAdapter &getAdapter(int hciDevice);
+
+void shutdown();
+
+}
+}
+
+#endif
diff --git a/ble/BluetoothImpl.h b/ble/BluetoothImpl.h
new file mode 100644
index 0000000..69fac6e
--- /dev/null
+++ b/ble/BluetoothImpl.h
@@ -0,0 +1,160 @@
+#ifndef BLUETOOTH_IMPL_H
+#define BLUETOOTH_IMPL_H
+
+#include "Bluetooth.h"
+#include <boost/uuid/uuid_io.hpp>
+#include <cstring>
+
+#define BLUETOOTH_UUID_INITIALIZER \
+ { \
+ 0x00, 0x00, 0x00, 0x00, \
+ 0x00, 0x00, \
+ 0x10, 0x00, \
+ 0x80, 0x00, \
+ 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb \
+ };
+
+namespace trygvis {
+namespace bluetooth {
+
+typedef boost::uuids::uuid uuid_t;
+template <class t>
+using o = boost::optional<t>;
+
+class DefaultBluetoothGattCharacteristic : public BluetoothGattCharacteristic {
+public:
+ DefaultBluetoothGattCharacteristic(BluetoothGattService &service, uint16_t handle, uuid_t uuid, uint8_t properties, uint16_t valueHandle)
+ : service(service), handle(handle), uuid(uuid), properties(properties), valueHandle(valueHandle) {
+ }
+
+ virtual ~DefaultBluetoothGattCharacteristic() {
+ }
+
+ BluetoothGattService &getService() const {
+ return service;
+ }
+
+ uint16_t getHandle() const {
+ return handle;
+ }
+
+ const uuid_t getUuid() const {
+ return uuid;
+ }
+
+ uint8_t getProperties() const {
+ return properties;
+ }
+
+ uint16_t getValueHandle() const {
+ return valueHandle;
+ }
+
+protected:
+ BluetoothGattService &service;
+ uint16_t handle;
+ uuid_t uuid;
+ uint8_t properties;
+ uint16_t valueHandle;
+};
+
+class DefaultBluetoothGattService : public BluetoothGattService {
+public:
+ DefaultBluetoothGattService(BluetoothDevice &device, const uuid_t uuid, const uint16_t handle, const uint16_t endGroupHandle)
+ : device(device), uuid(uuid), handle(handle), endGroupHandle(endGroupHandle) {
+ DF;
+ }
+
+ virtual ~DefaultBluetoothGattService() {
+ DF;
+ removeCharacteristics();
+ }
+
+ virtual BluetoothDevice &getDevice() const {
+ return device;
+ }
+
+ virtual uuid_t getUuid() const {
+ return uuid;
+ }
+
+ virtual uint16_t getHandle() const {
+ return handle;
+ }
+
+ virtual uint16_t getEndGroupHandle() const {
+ return endGroupHandle;
+ }
+
+ virtual const vector<BluetoothGattCharacteristic *> getCharacteristics() const {
+ return characteristics;
+ }
+
+ virtual void addCharacteristic(BluetoothGattCharacteristic *characteristic) {
+ characteristics.push_back(characteristic);
+ }
+
+protected:
+ BluetoothDevice &device;
+ const uuid_t uuid;
+ const uint16_t handle;
+ const uint16_t endGroupHandle;
+ vector<BluetoothGattCharacteristic *> characteristics;
+
+ void removeCharacteristics() {
+ DF;
+ for (auto &c: characteristics) {
+ delete c;
+ }
+ characteristics.clear();
+ }
+};
+
+class DefaultBluetoothDevice : public BluetoothDevice {
+public:
+ virtual const vector<BluetoothGattService *> getServices() const {
+ return services;
+ };
+
+ virtual const o<BluetoothGattService*> findService(boost::uuids::uuid uuid) const {
+ for (auto s: services) {
+ if (memcmp(s->getUuid().data, uuid.data, 16) == 0) {
+ return o<BluetoothGattService*>(s);
+ }
+ }
+
+ return o<BluetoothGattService*>();
+ }
+
+ virtual void addService(BluetoothGattService *service) {
+ services.push_back(service);
+ }
+
+protected:
+ DefaultBluetoothDevice() {
+ DF;
+ }
+
+ virtual ~DefaultBluetoothDevice() {
+ DF;
+ removeServices();
+ }
+
+ void removeServices() {
+ for (auto s: services) {
+ delete s;
+ }
+ services.clear();
+ }
+
+ vector<BluetoothGattService *> services;
+};
+
+BluetoothAdapter &getAdapterImpl(int hciDevice);
+
+void shutdownImpl();
+
+}
+};
+
+#endif
diff --git a/ble/ByteBuffer.cpp b/ble/ByteBuffer.cpp
new file mode 100644
index 0000000..820c638
--- /dev/null
+++ b/ble/ByteBuffer.cpp
@@ -0,0 +1,102 @@
+#include "ByteBuffer.h"
+#include <string.h>
+#include <sstream>
+#include <iomanip>
+
+using namespace std;
+
+ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity) :
+ bytes(bytes), capacity(capacity), zero(bytes.get()), end(&bytes.get()[capacity]) {
+ ptr = (uint8_t *) zero;
+}
+
+ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t zero, size_t size) :
+ bytes(bytes), capacity(capacity), zero(&bytes.get()[zero]), end(&bytes.get()[size]) {
+ assert(zero <= size);
+ assert(size <= capacity);
+ ptr = (uint8_t *) zero;
+}
+
+ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, const uint8_t *zero, const uint8_t *end) :
+ bytes(bytes), capacity(capacity), zero(zero), end(end), ptr((uint8_t *) zero) {
+}
+
+ByteBuffer &ByteBuffer::write8(uint8_t value) {
+ checkAndUpdateEnd(1);
+ (*ptr++) = value;
+ return *this;
+}
+
+ByteBuffer &ByteBuffer::write16le(uint16_t value) {
+ checkAndUpdateEnd(2);
+ (*ptr++) = (uint8_t) (value & 0xff);
+ (*ptr++) = (uint8_t) ((value >> 8) & 0xff);
+ return *this;
+}
+
+uint8_t ByteBuffer::get8(size_t index) const {
+ assertCanAccessRelative(index);
+ return ptr[index];
+}
+
+uint8_t ByteBuffer::read8() {
+ assertCanAccessRelative(0);
+ return *ptr++;
+}
+
+uint16_t ByteBuffer::read16le() {
+ assertCanAccessRelative(0);
+ uint16_t value;
+ value = *ptr++;
+ value |= ((uint16_t) *ptr++) << 8;
+ return value;
+}
+
+void ByteBuffer::copy(uint8_t *bytes, size_t length) const {
+ assertCanAccessRelative(length - 1);
+
+ memcpy(bytes, ptr, length);
+}
+
+ByteBuffer ByteBuffer::view() const {
+// DF << "cursor=" << getCursor() << ", size=" << getSize() << ", new size=" << end - ptr << ", ptr=" << (uint64_t) ptr << ", zero=" << (uint64_t) zero;
+ return view(ptr, end);
+}
+
+ByteBuffer ByteBuffer::view(size_t length) const {
+ return ByteBuffer(bytes, length, ptr, ptr + length);
+}
+
+ByteBuffer ByteBuffer::view(uint8_t *ptr, const uint8_t *end) const {
+ return ByteBuffer(bytes, end - ptr, ptr, end);
+}
+
+void ByteBuffer::checkAndUpdateEnd(size_t newBytes) {
+ uint8_t *newEnd = ptr + newBytes;
+ if (newEnd >= end) {
+ if (newEnd >= &zero[capacity]) {
+ throw ByteBufferException(string("New size is too large! cursor=") + to_string(getCursor()) + ", size=" + to_string(getSize()) + ", capacity=" + to_string(capacity) + ", new bytes=" + to_string(newBytes));
+ }
+ end = newEnd;
+ }
+}
+
+void ByteBuffer::assertCanAccessRelative(size_t diff) const {
+ assertCanAccessIndex(ptr + diff);
+}
+
+void ByteBuffer::assertCanAccessIndex(uint8_t *p) const {
+ if (p >= end || p < zero) {
+ throw ByteBufferException(string("Out of bounds! size=") + to_string(getSize()) + ", index=" + to_string(p - zero));
+ }
+}
+
+std::string ByteBuffer::toString() const {
+ stringstream s;
+
+ for (uint8_t *i = (uint8_t *) zero; i < end; i++) {
+ s << hex << setfill('0') << setw(2) << (int) *i << " ";
+ }
+
+ return string(s.str());
+}
diff --git a/ble/ByteBuffer.h b/ble/ByteBuffer.h
new file mode 100644
index 0000000..3836966
--- /dev/null
+++ b/ble/ByteBuffer.h
@@ -0,0 +1,91 @@
+#ifndef BYTE_STREAM_WRAPPER_H
+#define BYTE_STREAM_WRAPPER_H
+
+#include <cstdint>
+#include <cstdlib>
+#include <string>
+#include <stdexcept>
+
+// For now
+#include "log.h"
+
+class ByteBufferException : public std::runtime_error {
+public:
+ ByteBufferException(std::string const &what) : std::runtime_error(what) {
+ }
+};
+
+class ByteBuffer {
+public:
+ ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity);
+
+ ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t zero, size_t size);
+
+ inline size_t getSize() const {
+// DF << "end=" << (uint64_t)end << ", zero=" << (uint64_t)zero << ", size=" << (end - zero);
+ return end - zero;
+ }
+
+ inline size_t getCapacity() const {
+ return capacity;
+ }
+
+ inline size_t getCursor() const {
+ return ptr - zero;
+ }
+
+ inline size_t getBytesLeft() const {
+ return end - ptr;
+ }
+
+ inline void setCursor(size_t newCursor) {
+// assertCanAccessRelative(newCursor);
+ ptr = (uint8_t *) &zero[newCursor];
+ }
+
+ inline void skip(size_t length) {
+// checkAndUpdateEnd(length);
+ ptr += length;
+ }
+
+ ByteBuffer &write8(uint8_t value);
+
+ ByteBuffer &write16le(uint16_t value);
+
+ uint8_t get8(size_t index) const;
+
+ uint8_t read8();
+
+ uint16_t read16le();
+
+ void copy(uint8_t *bytes, size_t length) const;
+
+ /**
+ * Creates a view from cursor to size.
+ */
+ ByteBuffer view() const;
+
+ // TODO: should return const
+ ByteBuffer view(size_t length) const;
+
+ std::string toString() const;
+
+private:
+ ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, const uint8_t *zero, const uint8_t *end);
+
+ ByteBuffer view(uint8_t *ptr, const uint8_t *end) const;
+
+ void checkAndUpdateEnd(size_t count);
+
+ void assertCanAccessRelative(size_t diff) const;
+
+ void assertCanAccessIndex(uint8_t *p) const;
+
+ const std::shared_ptr<uint8_t> bytes;
+ const size_t capacity;
+ const uint8_t *zero;
+ const uint8_t *end;
+ uint8_t *ptr;
+};
+
+#endif
diff --git a/ble/CMakeLists.txt b/ble/CMakeLists.txt
new file mode 100644
index 0000000..1ecfb6c
--- /dev/null
+++ b/ble/CMakeLists.txt
@@ -0,0 +1,3 @@
+file(GLOB SOURCE_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.cpp)
+
+add_library(ble ${SOURCE_FILES})
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;
+ }
+}
+
+}
+}
diff --git a/ble/log.h b/ble/log.h
new file mode 100644
index 0000000..1d62121
--- /dev/null
+++ b/ble/log.h
@@ -0,0 +1,14 @@
+#ifndef LOG_H
+#define LOG_H
+
+#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)
+
+#define DF BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ << ": "
+#define IF BOOST_LOG_TRIVIAL(info) << __FUNCTION__ << ": "
+#define WF BOOST_LOG_TRIVIAL(warning) << __FUNCTION__ << ": "
+
+#endif
generated by cgit v1.2.3 (git 2.46.0) at 2024-12-22 21:06:01 +0000