From e44813dddbf5ba063d29ae1e40862e7a7cbb6f43 Mon Sep 17 00:00:00 2001 From: Trygve Laugstøl Date: Fri, 20 Feb 2015 22:56:22 +0100 Subject: Reorganizing the source code: o Moving main to apps/ o Moving the library sources to ble/ o Creating cmake files for each piece. --- ble/Bluetooth.cpp | 193 +++++++++++++++++++ ble/Bluetooth.h | 206 ++++++++++++++++++++ ble/BluetoothImpl.h | 160 ++++++++++++++++ ble/ByteBuffer.cpp | 102 ++++++++++ ble/ByteBuffer.h | 91 +++++++++ ble/CMakeLists.txt | 3 + ble/LinuxBluetooth.cpp | 512 +++++++++++++++++++++++++++++++++++++++++++++++++ ble/log.h | 14 ++ 8 files changed, 1281 insertions(+) create mode 100644 ble/Bluetooth.cpp create mode 100644 ble/Bluetooth.h create mode 100644 ble/BluetoothImpl.h create mode 100644 ble/ByteBuffer.cpp create mode 100644 ble/ByteBuffer.h create mode 100644 ble/CMakeLists.txt create mode 100644 ble/LinuxBluetooth.cpp create mode 100644 ble/log.h (limited to 'ble') 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 +#include +#include + +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 AttPdu::parse(ByteBuffer &bytes, AttPduType type) { + DF << "bytes: " << bytes.toString(); + + AttPduType t = (AttPduType) bytes.read8(); + + if (t == INVALID_HANDLE) { + return vector(); + } + + 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 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 AttPdu::parseReadByGroupType(ByteBuffer &bytes) { + return parse(bytes, READ_BY_GROUP_TYPE_RES); +} + +vector 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 adapters; + +BluetoothAdapter &getAdapter(int hciDevice) { + map::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 +#include +#include +#include + +#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 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 getServices() const = 0; + + virtual const boost::optional 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 parseReadByGroupType(ByteBuffer &bytes); + + static vector 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 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 +#include + +#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 +using o = boost::optional; + +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 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 characteristics; + + void removeCharacteristics() { + DF; + for (auto &c: characteristics) { + delete c; + } + characteristics.clear(); + } +}; + +class DefaultBluetoothDevice : public BluetoothDevice { +public: + virtual const vector getServices() const { + return services; + }; + + virtual const o findService(boost::uuids::uuid uuid) const { + for (auto s: services) { + if (memcmp(s->getUuid().data, uuid.data, 16) == 0) { + return o(s); + } + } + + return o(); + } + + 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 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 +#include +#include + +using namespace std; + +ByteBuffer::ByteBuffer(const std::shared_ptr 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 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 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 +#include +#include +#include + +// 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 bytes, size_t capacity); + + ByteBuffer(const std::shared_ptr 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 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 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 +#include +#include +#include +#include +#include +#include +#include +#include + +// 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 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 discoverServices(uint16_t startHandle); + + vector discoverCharacteristics(uint16_t startHandle, uint16_t endHandle); + + ByteBuffer writeAndRead(ByteBuffer &out, shared_ptr 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 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 chars; + + auto s = *it; + + do { + vector 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 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 LinuxBluetoothDevice::discoverServices(uint16_t startHandle) { + DF; + + shared_ptr 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 values = AttPdu::parseReadByGroupType(in); + + D << "READ_BY_GROUP_TYPE response has " + to_string(values.size()) + " values"; + + return values; +} + +vector LinuxBluetoothDevice::discoverCharacteristics(uint16_t startHandle, uint16_t endHandle) { + DF; + + shared_ptr 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 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::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 adapters; + +BluetoothAdapter &getAdapterImpl(int hciDevice) { + map::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 +#include +#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 -- cgit v1.2.3