diff options
Diffstat (limited to 'ble/LinuxBluetooth.cpp')
| -rw-r--r-- | ble/LinuxBluetooth.cpp | 512 |
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; + } +} + +} +} |