diff options
Diffstat (limited to 'LinuxBluetooth.cpp')
| -rw-r--r-- | LinuxBluetooth.cpp | 512 |
1 files changed, 0 insertions, 512 deletions
diff --git a/LinuxBluetooth.cpp b/LinuxBluetooth.cpp deleted file mode 100644 index 4b7f0e0..0000000 --- a/LinuxBluetooth.cpp +++ /dev/null @@ -1,512 +0,0 @@ -#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; - } -} - -} -} |