From b55df0b8e46f46fea4d69f98a729cd237d77a6ed Mon Sep 17 00:00:00 2001 From: Trygve Laugstøl Date: Tue, 10 Feb 2015 21:29:19 +0100 Subject: o wip. --- Bluetooth.cpp | 91 ++++++++++++++++++++++++++++++++++++++++++------------ Bluetooth.h | 38 ++++++++++++++--------- ByteBuffer.cpp | 54 +++++++++++++++++++++++--------- ByteBuffer.h | 38 ++++++++++++++++++----- LinuxBluetooth.cpp | 50 ++++++++++++++++++++---------- main.cpp | 21 ++++++++----- 6 files changed, 213 insertions(+), 79 deletions(-) diff --git a/Bluetooth.cpp b/Bluetooth.cpp index 43faf0a..737f04f 100644 --- a/Bluetooth.cpp +++ b/Bluetooth.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include "Bluetooth.h" namespace trygvis { @@ -24,9 +25,17 @@ namespace trygvis { return buf.str(); } - bool Mac::operator==(Mac &other) const { - return memcmp(bytes, other.bytes, sizeof(bytes)) == 0; + 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 { @@ -38,12 +47,16 @@ namespace trygvis { _5 = bytes[5]; } - Mac *Mac::parseMac(string s) { - uint8_t bytes[6]; - sscanf("%02x:%02x:%02x:%02x:%02x:%02x", s.c_str(), + 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]); - return new Mac(bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5]); + if (count != 6) { + throw BluetoothException("Unable to parse mac: " + s); + } + + return new 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) { @@ -57,26 +70,66 @@ namespace trygvis { return (AttPduType) bytes.get8(0); } - AttPdu AttPdu::parse(ByteBuffer & bytes) { - if(size == 0) { + void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, uint16_t uuid) { + bytes.setCursor(0); + bytes.add8(AttPduType::READ_BY_GROUP_TYPE_REQ); + bytes.add16le(startHandle); + bytes.add16le(endHandle); + bytes.add16le(uuid); + } + + void AttPdu::checkType(ByteBuffer &bytes, AttPduType type) { + if (bytes.getSize() == 0) { throw BluetoothException("PDU is too small"); } - AttPdu pdu = AttPdu(bytes, size); + bytes.setCursor(0); + AttPduType t = (AttPduType) bytes.get8(); - AttPduType type = pdu.getType(); + if (t != type) { + throw BluetoothException("Unexpected type: " + t); + } + } - switch (type) { - case READ_BY_GROUP_TYPE_RES: - if (size < 4) { - throw BluetoothException("Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + size); - } - return pdu; - default: - throw BluetoothException("Uknown PDU type: " + type); + vector* AttPdu::parseReadByGroupType(ByteBuffer &bytes) { + checkType(bytes, READ_BY_GROUP_TYPE_RES); + + if (bytes.getSize() < 4) { + throw BluetoothException("Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + bytes.getSize()); } - return pdu; + uint8_t length = bytes.get8(); + DF << "length=" << (int) length; + + size_t count = (bytes.getSize() - 2) / length; + DF << "count=" << count; + + vector *values = new vector; + for (int i = 0; i < count; i++) { + values->push_back(AttributeData::fromByteBuffer(bytes, length)); + } + + return values; + } + + // ----------------------------------------------------------------------- + // AttributeData + // ----------------------------------------------------------------------- + + AttributeData* AttributeData::fromByteBuffer(ByteBuffer &bytes, uint8_t length) { + uint16_t handle = bytes.get16le(); + + uint8_t* value = new uint8_t[length]; + bytes.copy(value, length); + return new AttributeData(handle, value); + } + + AttributeData::AttributeData(uint16_t handle, uint8_t *bytes) : handle(handle), bytes(bytes) { + + } + + AttributeData::~AttributeData() { + delete bytes; } // ----------------------------------------------------------------------- diff --git a/Bluetooth.h b/Bluetooth.h index 9496983..586ac66 100644 --- a/Bluetooth.h +++ b/Bluetooth.h @@ -30,15 +30,15 @@ namespace trygvis { class BluetoothException : public runtime_error { public: BluetoothException(const BluetoothAdapter *adapter, string const &what) : - adapter(adapter), device(nullptr), runtime_error(what) { + runtime_error(what), adapter(adapter), device(nullptr) { } BluetoothException(const BluetoothDevice *device, string const &what) : - adapter(nullptr), device(device), runtime_error(what) { + runtime_error(what), adapter(nullptr), device(device) { } BluetoothException(string const &what) : - adapter(nullptr), device(nullptr), runtime_error(what) { + runtime_error(what), adapter(nullptr), device(nullptr) { } const BluetoothAdapter *adapter; @@ -60,10 +60,13 @@ namespace trygvis { 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); + static Mac *parseMac(string s) throw(BluetoothException); + friend bool operator<(const Mac &a, const Mac &b); private: uint8_t bytes[6]; }; @@ -91,6 +94,8 @@ namespace trygvis { virtual void stopScan() = 0; virtual void runScan(void (callback)(BluetoothDevice &device)) = 0; + + virtual BluetoothDevice &getDevice(Mac& mac) = 0; }; enum AttPduType { @@ -99,6 +104,8 @@ namespace trygvis { READ_BY_GROUP_TYPE_RES = 0x11 }; + class AttributeData; + class AttPdu { public: AttPdu(ByteBuffer &bytes); @@ -107,29 +114,30 @@ namespace trygvis { AttPduType getType(); - static AttPdu parse(ByteBuffer & bytes); - private: - ByteBuffer &bytes; - }; + static vector* parseReadByGroupType(ByteBuffer &bytes); - class AttributeDataList { - public: - AttributeDataList(ByteBuffer &bytes); + static void makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, uint16_t uuid); private: + static void checkType(ByteBuffer &bytes, AttPduType type); + ByteBuffer &bytes; }; class AttributeData { public: - AttributeData(ByteBuffer &bytes); + static AttributeData* fromByteBuffer(ByteBuffer &bytes, uint8_t length); private: - ByteBuffer &bytes; + AttributeData(uint16_t handle, uint8_t* bytes); + ~AttributeData(); + + uint16_t handle; + uint8_t *bytes; }; -// BluetoothAdapter &getDevice(int hciDevice); - BluetoothAdapter *getDevice(int hciDevice); +// BluetoothAdapter &getAdapter(int hciDevice); + BluetoothAdapter *getAdapter(int hciDevice); } #endif diff --git a/ByteBuffer.cpp b/ByteBuffer.cpp index 9714c8c..70b9a8e 100644 --- a/ByteBuffer.cpp +++ b/ByteBuffer.cpp @@ -1,38 +1,62 @@ #include "ByteBuffer.h" -#include +#include +#include using namespace std; -ByteBuffer::ByteBuffer(uint8_t *bytes, ssize_t size, ssize_t zero) : - bytes(bytes), size(size), ptr(zero), zero(zero) { +ByteBuffer::ByteBuffer(uint8_t *bytes, size_t capacity, size_t size, size_t zero) : + bytes(bytes), capacity(capacity), size(size), cursor(zero), zero(zero) { } ByteBuffer &ByteBuffer::add8(uint8_t value) { - canAccessNextBytes(1); - bytes[zero + ptr++] = value; + checkAndUpdateSize(1); + bytes[zero + cursor++] = value; return *this; } ByteBuffer &ByteBuffer::add16le(uint16_t value) { - canAccessNextBytes(2); - bytes[zero + ptr++] = (uint8_t) (value & 0xff); - bytes[zero + ptr++] = (uint8_t) ((value >> 8) & 0xff); + checkAndUpdateSize(2); + bytes[zero + cursor++] = (uint8_t) (value & 0xff); + bytes[zero + cursor++] = (uint8_t) ((value >> 8) & 0xff); return *this; } -uint8_t ByteBuffer::get8(ssize_t index) { +uint8_t ByteBuffer::get8(size_t index) { canAccessIndex(index); - return bytes[zero + ptr]; + return bytes[zero + cursor]; } -void ByteBuffer::canAccessNextBytes(ssize_t newBytes) { - if (zero + ptr + newBytes >= size) { - throw exception(); +uint8_t ByteBuffer::get8() { + canAccessIndex(cursor); + return bytes[zero + cursor++]; +} + +uint16_t ByteBuffer::get16le() { + canAccessIndex(cursor + 1); + uint16_t value; + value = bytes[zero + cursor++]; + value = ((uint16_t) bytes[zero + cursor++]) << 8; + return value; +} + +void ByteBuffer::copy(uint8_t *bytes, size_t length) { + canAccessIndex(cursor + length); + + memcpy(bytes, &this->bytes[zero + cursor], length); + cursor += length; +} + +void ByteBuffer::checkAndUpdateSize(size_t newBytes) { + size_t newSize = zero + cursor + newBytes; + if (newSize >= capacity) { + throw ByteBufferException(string("Out of bounds! zero=") + to_string(zero) + ", cursor=" + to_string(cursor) + ", size=" + to_string(size) + ", capacity=" + to_string(capacity) + ", newSize=" + to_string(newSize)); } + + size = max(newSize, size); } -void ByteBuffer::canAccessIndex(ssize_t index) { +void ByteBuffer::canAccessIndex(size_t index) { if (zero + index >= size) { - throw exception(); + throw ByteBufferException(string("Out of bounds! zero=") + to_string(zero) + ", index=" + to_string(index) + ", size=" + to_string(size)); } } diff --git a/ByteBuffer.h b/ByteBuffer.h index a7e9f9f..444943d 100644 --- a/ByteBuffer.h +++ b/ByteBuffer.h @@ -3,25 +3,49 @@ #include #include +#include +#include + +class ByteBufferException : public std::runtime_error { +public: + ByteBufferException(std::string const &what) : std::runtime_error(what) { + } +}; class ByteBuffer { public: - ByteBuffer(uint8_t *bytes, ssize_t size, ssize_t zero = 0); + ByteBuffer(uint8_t *bytes, size_t capacity, size_t size, size_t zero = 0); + + inline size_t getSize() { + return size; + } + + inline void setCursor(size_t newCursor) { + cursor = newCursor; + } ByteBuffer &add8(uint8_t value); ByteBuffer &add16le(uint16_t value); - uint8_t get8(ssize_t index); + uint8_t get8(size_t index); + + uint8_t get8(); + + uint16_t get16le(); + + void copy(uint8_t *bytes, size_t length); private: - void canAccessNextBytes(ssize_t count); - void canAccessIndex(ssize_t count); + void checkAndUpdateSize(size_t count); + + void canAccessIndex(size_t count); uint8_t *bytes; - ssize_t zero; - ssize_t size; - ssize_t ptr; + size_t zero; + size_t size; + size_t capacity; + size_t cursor; }; #endif diff --git a/LinuxBluetooth.cpp b/LinuxBluetooth.cpp index 14407c5..d274f84 100644 --- a/LinuxBluetooth.cpp +++ b/LinuxBluetooth.cpp @@ -33,6 +33,8 @@ namespace trygvis { void runScan(void (*callback)(BluetoothDevice &device)); + BluetoothDevice &getDevice(Mac &mac) override; + private: void startScan(); @@ -149,31 +151,32 @@ namespace trygvis { DF; uint8_t buffer[MAX_MTU]; - ByteBuffer bytes = ByteBuffer(buffer, MAX_MTU); + ByteBuffer out = ByteBuffer(buffer, MAX_MTU, 0); - AttPdu pdu = AttPdu(bytes, AttPduType::READ_BY_GROUP_TYPE_REQ). - add16l(0x0001). - add16l(0x1234). - add16l(UUID_PRIMARY_SERVICE); + AttPdu::makeReadByGroupType(out, 0x0001, 0xffff, UUID_PRIMARY_SERVICE); - D << "pdu.size()=" << pdu.size(); - ssize_t written = write(l2cap, bytes, pdu.size()); + D << "pdu.size()=" << out.getSize(); + ssize_t written = write(l2cap, buffer, out.getSize()); D << "written=" << written; - uint8_t *buffer = new uint8_t[MAX_MTU]; - ssize_t r = read(l2cap, buffer, MAX_MTU); + ByteBuffer in = ByteBuffer(buffer, MAX_MTU, r); D << "read: " << r << " bytes"; - pdu = AttPdu::parse(buffer, r); - - D << "type= " << pdu.getType(); + vector* values = AttPdu::parseReadByGroupType(in); - if (pdu.getType() != AttPduType::READ_BY_GROUP_TYPE_REQ) { + D << "READ_BY_GROUP_TYPE response has " + (values->size()) + " values"; + for(auto& data: *values) { +// strstring s; +// s << hex << setfill('0') << setw(2) << ar[i] << " "; +// data.bytes +// D << data. } + + delete values; } // ----------------------------------------------------------------------- @@ -257,6 +260,20 @@ namespace trygvis { } } + BluetoothDevice &LinuxBluetoothAdapter::getDevice(Mac &mac) { + static map devices; + + 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); @@ -295,7 +312,7 @@ namespace trygvis { Mac mac = parseMac(advertisingInfo->bdaddr); - LinuxBluetoothDevice device = LinuxBluetoothDevice(*this, mac); + BluetoothDevice &device = getDevice(mac); callback(device); } @@ -309,7 +326,7 @@ namespace trygvis { /* map adapters; - BluetoothAdapter &getDevice(int hciDevice) { + BluetoothAdapter &getAdapter(int hciDevice) { map::iterator it = adapters.find(hciDevice); if (it == adapters.end()) { @@ -322,7 +339,8 @@ namespace trygvis { } */ - BluetoothAdapter *getDevice(int hciDevice) { + BluetoothAdapter *getAdapter(int hciDevice) { return new LinuxBluetoothAdapter(hciDevice); } + }; diff --git a/main.cpp b/main.cpp index ad713e8..7caa303 100644 --- a/main.cpp +++ b/main.cpp @@ -10,8 +10,8 @@ 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; + if (device.mac() != *targetMac) { + cout << "found device: " << device.mac().str() << ", but not the one we want " << targetMac->str() << endl; return; } @@ -30,18 +30,25 @@ int main(int argc, char *argv[]) { return EXIT_FAILURE; } - targetMac = Mac::parseMac(argv[1]); - BluetoothAdapter *adapter = nullptr; int e; try { - adapter = trygvis::getDevice(0); + targetMac = Mac::parseMac(argv[1]); + + adapter = trygvis::getAdapter(0); + + BluetoothDevice &device = adapter->getDevice(*targetMac); - adapter->runScan(scan_callback); + scan_callback(device); + +// adapter->runScan(scan_callback); e = EXIT_SUCCESS; } catch (BluetoothException ex) { - W << "Excpetion: " << ex.what(); + W << "BluetoothException: " << ex.what(); + e = EXIT_FAILURE; + } catch (std::exception ex) { + W << "std::exception: " << ex.what(); e = EXIT_FAILURE; } -- cgit v1.2.3