From 5cb96fd96f51e949c5311db3080c58d851b7c2e1 Mon Sep 17 00:00:00 2001 From: Trygve Laugstøl Date: Sun, 8 Feb 2015 21:29:47 +0100 Subject: o Initial import of Linux code for talking to Bluetooth devices. --- .gitignore | 3 + Bluetooth.cpp | 55 ++++++++++ Bluetooth.h | 77 ++++++++++++++ CMakeLists.txt | 19 ++++ LinuxBluetooth.cpp | 292 +++++++++++++++++++++++++++++++++++++++++++++++++++++ main.cpp | 44 ++++++++ 6 files changed, 490 insertions(+) create mode 100644 .gitignore create mode 100644 Bluetooth.cpp create mode 100644 Bluetooth.h create mode 100644 CMakeLists.txt create mode 100644 LinuxBluetooth.cpp create mode 100644 main.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5331b61 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +noble +.idea +build diff --git a/Bluetooth.cpp b/Bluetooth.cpp new file mode 100644 index 0000000..22f67ec --- /dev/null +++ b/Bluetooth.cpp @@ -0,0 +1,55 @@ +#include +#include +#include +#include "Bluetooth.h" + +namespace trygvis { + using namespace std; + + // ----------------------------------------------------------------------- + // Mac + // ----------------------------------------------------------------------- + + string Mac::str() const { + std::ostringstream buf; + + buf + << setw(2) << hex << setfill('0') << (int)bytes[5] << ":" + << setw(2) << hex << setfill('0') << (int)bytes[4] << ":" + << setw(2) << hex << setfill('0') << (int)bytes[3] << ":" + << setw(2) << hex << setfill('0') << (int)bytes[2] << ":" + << setw(2) << hex << setfill('0') << (int)bytes[1] << ":" + << setw(2) << hex << setfill('0') << (int)bytes[0]; + + return buf.str(); + } + + + bool Mac::operator==(Mac &other) const { + return memcmp(bytes, other.bytes, sizeof(bytes)) == 0; + } + + void Mac::copy(uint8_t &_0, uint8_t &_1, uint8_t &_2, uint8_t &_3, uint8_t &_4, uint8_t &_5) const { + _0 = bytes[0]; + _1 = bytes[1]; + _2 = bytes[2]; + _3 = bytes[3]; + _4 = bytes[4]; + _5 = bytes[5]; + } + + Mac *Mac::parseMac(string s) { + uint8_t bytes[6]; + sscanf("%02x:%02x:%02x:%02x:%02x:%02x", s.c_str(), + &bytes[0], &bytes[1], &bytes[2], &bytes[3], &bytes[4], &bytes[5]); + + return new Mac(bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5]); + } + + // ----------------------------------------------------------------------- + // Adapter + // ----------------------------------------------------------------------- + + BluetoothAdapter::~BluetoothAdapter() { + } +}; diff --git a/Bluetooth.h b/Bluetooth.h new file mode 100644 index 0000000..2e2f2ef --- /dev/null +++ b/Bluetooth.h @@ -0,0 +1,77 @@ +#ifndef BLUETOOTH_H +#define BLUETOOTH_H + +#include +#include + +// For now +#include +#include + +#define D BOOST_LOG_TRIVIAL(debug) +#define I BOOST_LOG_TRIVIAL(info) +#define W BOOST_LOG_TRIVIAL(warning) + +namespace trygvis { + using namespace std; + + class BluetoothAdapter; + class BluetoothDevice; + + class BluetoothException : public runtime_error { + public: + BluetoothException(const BluetoothAdapter *adapter, string const &what) : + adapter(adapter), device(nullptr), runtime_error(what) { + } + BluetoothException(const BluetoothAdapter *adapter, const BluetoothDevice *device, string const &what) : + adapter(adapter), device(device), runtime_error(what) { + } + + const BluetoothAdapter *adapter; + const BluetoothDevice *device; + }; + + class Mac { + public: + Mac(uint8_t _0, uint8_t _1, uint8_t _2, uint8_t _3, uint8_t _4, uint8_t _5) { + bytes[0] = _0; + bytes[1] = _1; + bytes[2] = _2; + bytes[3] = _3; + bytes[4] = _4; + bytes[5] = _5; + }; + + string str() const; + + bool operator==(Mac& other) const; + + void copy(uint8_t &_0, uint8_t &_1, uint8_t &_2, uint8_t &_3, uint8_t &_4, uint8_t &_5) const; + static Mac *parseMac(string s); + + private: + uint8_t bytes[6]; + }; + + class BluetoothDevice { + public: + virtual Mac const &mac() = 0; + virtual void connect() = 0; + virtual void disconnect() = 0; + virtual BluetoothAdapter& adapter() = 0; + }; + + class BluetoothAdapter { + public: + BluetoothAdapter() {}; + virtual ~BluetoothAdapter(); + + virtual void stopScan() = 0; + virtual void runScan(void (callback)(BluetoothDevice &device)) = 0; + }; + +// BluetoothAdapter &getDevice(int hciDevice); + BluetoothAdapter *getDevice(int hciDevice); +} + +#endif diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..8a43b50 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.8.4) +project(ble_toys) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + +set(Boost_USE_STATIC_LIBS OFF) +set(Boost_USE_MULTITHREADED OFF) +set(Boost_USE_STATIC_RUNTIME OFF) + +add_definitions(-DBOOST_ALL_DYN_LINK) + +find_package(Boost REQUIRED COMPONENTS system log thread) + +set(SOURCE_FILES main.cpp Bluetooth.cpp LinuxBluetooth.cpp) +add_executable(ble_toys ${SOURCE_FILES}) + +target_link_libraries(ble_toys bluetooth) +target_link_libraries(ble_toys pthread) +target_link_libraries(ble_toys ${Boost_LIBRARIES}) diff --git a/LinuxBluetooth.cpp b/LinuxBluetooth.cpp new file mode 100644 index 0000000..ee93dee --- /dev/null +++ b/LinuxBluetooth.cpp @@ -0,0 +1,292 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Bluetooth.h" + +// Got to love magic constants. Taken from bluez.git/tools/btgatt-client.c +#define ATT_CID 4 + +namespace trygvis { + + class LinuxBluetoothDevice; + + class LinuxBluetoothAdapter; + + class LinuxBluetoothManager; + + class LinuxBluetoothAdapter : public BluetoothAdapter { + public: + LinuxBluetoothAdapter(int hciDeviceId); + + ~LinuxBluetoothAdapter(); + + void runScan(void (*callback)(BluetoothDevice &device)); + + private: + void startScan(); + + void stopScan(); + + int hciDeviceId; + int hciSocket; + struct hci_filter hciFilter; + bool scanning; + }; + + class LinuxBluetoothDevice : public BluetoothDevice { + public: + LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac mac); + + Mac const &mac() override; + + void connect() override; + + void disconnect() override; + + virtual LinuxBluetoothAdapter &adapter(); + + private: + LinuxBluetoothAdapter &_adapter; + Mac _mac; + int l2cap; + }; + + // Utilities + + string errnoAsString() { + return string(strerror(errno)); + }; + + // ----------------------------------------------------------------------- + // Mac + // ----------------------------------------------------------------------- + + Mac parseMac(bdaddr_t &a) { + return Mac(a.b[0], a.b[1], a.b[2], a.b[3], a.b[4], a.b[5]); + } + + // ----------------------------------------------------------------------- + // Device + // ----------------------------------------------------------------------- + + LinuxBluetoothDevice::LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac mac) : + _adapter(adapter), _mac(mac) { + } + + Mac const &LinuxBluetoothDevice::mac() { + return _mac; + } + + LinuxBluetoothAdapter &LinuxBluetoothDevice::adapter() { + return _adapter; + } + + void LinuxBluetoothDevice::connect() { + struct sockaddr_l2 addr; + + D << "connect: mac=" << _mac.str(); + + l2cap = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP); + if(l2cap < 0) { + throw BluetoothException(&_adapter, this, "LinuxBluetoothDevice::connect(): socket(): " + errnoAsString()); + } + + memset(&addr, 0, sizeof(addr)); + addr.l2_family = AF_BLUETOOTH; + addr.l2_bdaddr = {{0, 0, 0, 0, 0, 0}}; + addr.l2_cid = htobs(ATT_CID); + addr.l2_bdaddr_type = BDADDR_LE_PUBLIC; + + if (bind(l2cap, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + close(l2cap); + throw BluetoothException(&_adapter, this, "LinuxBluetoothDevice::connect(): bind(): " + errnoAsString()); + } + + struct bt_security btsec; + memset(&btsec, 0, sizeof(btsec)); + btsec.level = BT_SECURITY_LOW; + if (setsockopt(l2cap, SOL_BLUETOOTH, BT_SECURITY, &btsec, sizeof(btsec)) != 0) { + close(l2cap); + throw BluetoothException(&_adapter, this, "LinuxBluetoothDevice::connect(): setsockopt(): " + errnoAsString()); + } + + memset(&addr, 0, sizeof(addr)); + addr.l2_family = AF_BLUETOOTH; + addr.l2_cid = htobs(ATT_CID); + addr.l2_bdaddr_type = BDADDR_LE_RANDOM; + _mac.copy(addr.l2_bdaddr.b[0], + addr.l2_bdaddr.b[1], + addr.l2_bdaddr.b[2], + addr.l2_bdaddr.b[3], + addr.l2_bdaddr.b[4], + addr.l2_bdaddr.b[5]); + + if (::connect(l2cap, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + close(l2cap); + throw BluetoothException(&_adapter, this, "LinuxBluetoothDevice::connect(): connect(): " + errnoAsString()); + } + } + + void LinuxBluetoothDevice::disconnect() { + + } + + // ----------------------------------------------------------------------- + // Adapter + // ----------------------------------------------------------------------- + + + LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId) { + D << "LinuxBluetoothAdapter(), hciDeviceId " << hciDeviceId; + + this->hciDeviceId = hciDeviceId; + hciSocket = ::hci_open_dev(hciDeviceId); + + D << "HCI socket: " << hciSocket; + + if (hciSocket == -1) { + throw BluetoothException(this, "Could not open HCI device " + hciDeviceId); + } + + hci_filter_clear(&hciFilter); + hci_filter_set_ptype(HCI_EVENT_PKT, &hciFilter); + hci_filter_set_event(EVT_LE_META_EVENT, &hciFilter); + hci_filter_set_event(EVT_LE_ADVERTISING_REPORT, &hciFilter); + setsockopt(hciSocket, SOL_HCI, HCI_FILTER, &hciFilter, sizeof(hciFilter)); + } + + LinuxBluetoothAdapter::~LinuxBluetoothAdapter() { + D << "~LinuxBluetoothAdapter()"; + + stopScan(); + + close(hciSocket); + } + + void LinuxBluetoothAdapter::startScan() { + D << "startScan()"; + + struct hci_dev_info di; + + if (hci_devinfo(hciDeviceId, &di) < 0) { + throw BluetoothException(this, "hci_devinfo: " + hciDeviceId); + } + + D << "hciDeviceId.dev_id=" << di.dev_id; + D << "hciDeviceId.bdaddr=" << parseMac(di.bdaddr).str(); + D << "hciDeviceId.flags=" << hex << setw(8) << setfill('0') << di.flags; + D << "hciDeviceId.flags RUNNING = " << hci_test_bit(HCI_RUNNING, &di.flags); + D << "hciDeviceId.flags UP = " << hci_test_bit(HCI_UP, &di.flags); + D << "hciDeviceId.flags PSCAN = " << hci_test_bit(HCI_PSCAN, &di.flags); + D << "hciDeviceId.flags ISCAN = " << hci_test_bit(HCI_ISCAN, &di.flags); + D << "hciDeviceId.name=" << di.name; + + int up = hci_test_bit(HCI_UP, &di.flags); + + if (!up) { + throw BluetoothException(this, "HCI adapter is not up: " + hciDeviceId); + } + + if (hci_le_set_scan_parameters(hciSocket, 0x01, htobs(0x0010), htobs(0x0010), 0x00, 0, 1000) < 0) { + throw BluetoothException(this, "hci_le_set_scan_parameters: " + errnoAsString()); + } + + if (hci_le_set_scan_enable(hciSocket, 1, 0, 1000)) { + throw BluetoothException(this, "Could not start scanning: other" + errnoAsString()); + } + + scanning = true; + } + + void LinuxBluetoothAdapter::stopScan() { + D << "stopScan()"; + + if (!scanning) { + return; + } + + scanning = false; + + if (hci_le_set_scan_enable(hciSocket, 0, 0, 1000) < 0) { + W << "stopScan: hci_le_set_scan_enable: " << errnoAsString(); + } + } + + void LinuxBluetoothAdapter::runScan(void (*callback)(BluetoothDevice &device)) { + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(hciSocket, &rfds); + + startScan(); + + while (scanning) { + // Linux can change tv, so it has to be reinitialized + struct timeval tv; + tv.tv_sec = 1; + tv.tv_usec = 0; + + int selected = select(hciSocket + 1, &rfds, NULL, NULL, &tv); + + if (selected == -1) { + throw BluetoothException(this, "select() failed"); + } + + if (selected == 0) { + D << "timeout"; + // Timeout, just continue + continue; + } + + unsigned char hciEventBuf[HCI_MAX_EVENT_SIZE]; + + ssize_t len = read(hciSocket, hciEventBuf, sizeof(hciEventBuf)); + evt_le_meta_event *metaEvent = (evt_le_meta_event *) (hciEventBuf + (1 + HCI_EVENT_HDR_SIZE)); + len -= (1 + HCI_EVENT_HDR_SIZE); + + D << "metaEvent->subevent = " << std::hex << (int) metaEvent->subevent; + + if (metaEvent->subevent == EVT_LE_ADVERTISING_REPORT) { + le_advertising_info *advertisingInfo = (le_advertising_info *) (metaEvent->data + 1); + + Mac mac = parseMac(advertisingInfo->bdaddr); + + LinuxBluetoothDevice device = LinuxBluetoothDevice(*this, mac); + + callback(device); + } + } + } + + // ----------------------------------------------------------------------- + // + // ----------------------------------------------------------------------- + + /* + map adapters; + + BluetoothAdapter &getDevice(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 *getDevice(int hciDevice) { + return new LinuxBluetoothAdapter(hciDevice); + } +}; diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..ab1a9b0 --- /dev/null +++ b/main.cpp @@ -0,0 +1,44 @@ +#include +#include +#include "Bluetooth.h" + +using namespace std; +using namespace trygvis; + +static Mac *targetMac; + +void scan_callback(BluetoothDevice &device) { + device.adapter().stopScan(); + + if (device.mac() == *targetMac) { + cout << "found device: " << device.mac().str() << ", but not the one we want" << endl; + return; + } + + cout << "found device: " << device.mac().str() << endl; + + device.connect(); + device.disconnect(); +} + +int main(int argc, char *argv[]) { + if (argc != 2) { + cerr << "usage: " << argv[0] << " [mac]" << endl; + return EXIT_FAILURE; + } + + targetMac = Mac::parseMac(argv[1]); + + try { + BluetoothAdapter *adapter = trygvis::getDevice(0); + + adapter->runScan(scan_callback); + + delete adapter; + + return EXIT_SUCCESS; + } catch (BluetoothException ex) { + W << "Excpetion: " << ex.what(); + return EXIT_FAILURE; + } +} -- cgit v1.2.3