diff options
-rw-r--r-- | Bluetooth.cpp | 22 | ||||
-rw-r--r-- | Bluetooth.h | 17 | ||||
-rw-r--r-- | ByteBuffer.cpp | 86 | ||||
-rw-r--r-- | ByteBuffer.h | 62 | ||||
-rw-r--r-- | CMakeLists.txt | 7 | ||||
-rw-r--r-- | LinuxBluetooth.cpp | 23 | ||||
-rw-r--r-- | log.h | 17 | ||||
-rw-r--r-- | main.cpp | 16 | ||||
-rw-r--r-- | test/ByteBufferTest.cpp | 83 |
9 files changed, 220 insertions, 113 deletions
diff --git a/Bluetooth.cpp b/Bluetooth.cpp index e6d6d28..ebdd527 100644 --- a/Bluetooth.cpp +++ b/Bluetooth.cpp @@ -1,8 +1,9 @@ +#include "Bluetooth.h" +#include "BluetoothImpl.h" + #include <sstream> #include <iomanip> #include <string.h> -#include "Bluetooth.h" -#include "BluetoothImpl.h" namespace trygvis { namespace bluetooth { @@ -88,7 +89,7 @@ void AttPdu::checkType(ByteBuffer &bytes, AttPduType type) { AttPduType t = (AttPduType) bytes.get8(); if (t != type) { - throw BluetoothException("Unexpected type: " + t); + throw BluetoothException("Unexpected type: " + to_string(t)); } } @@ -98,18 +99,21 @@ vector<AttributeData> 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()); + throw BluetoothException("Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + to_string(bytes.getSize())); } uint8_t length = bytes.get8(); - DF << "length=" << (int) length; + D << "length=" << (int) length; size_t count = (bytes.getSize() - 2) / length; - DF << "count=" << count; + D << "count=" << count; vector<AttributeData> values; for (int i = 0; i < count; i++) { - values.push_back(AttributeData::fromByteBuffer(bytes, length)); + auto data = bytes.view(length); + D << "data, size=" << data.getSize() << ", bytes=" << data.toString(); + bytes.skip(length); + values.push_back(AttributeData::fromByteBuffer(data)); } return values; @@ -119,11 +123,11 @@ vector<AttributeData> AttPdu::parseReadByGroupType(ByteBuffer &bytes) { // AttributeData // ----------------------------------------------------------------------- -AttributeData AttributeData::fromByteBuffer(ByteBuffer &bytes, uint8_t length) { +AttributeData AttributeData::fromByteBuffer(ByteBuffer &bytes) { uint16_t handle = bytes.get16le(); uint16_t groupEndHandle = bytes.get16le(); - return AttributeData(handle, groupEndHandle, bytes.view(length - 4)); + return AttributeData(handle, groupEndHandle, bytes.view()); } AttributeData::AttributeData(uint16_t handle, uint16_t groupEndHandle, ByteBuffer value) : diff --git a/Bluetooth.h b/Bluetooth.h index 7ed7b8c..1b5d5f0 100644 --- a/Bluetooth.h +++ b/Bluetooth.h @@ -5,20 +5,9 @@ #include <stdexcept> // For now -#include <boost/log/core.hpp> -#include <boost/log/trivial.hpp> -#include "ByteBuffer.h" - -#define D BOOST_LOG_TRIVIAL(debug) -#define I BOOST_LOG_TRIVIAL(info) -#define W BOOST_LOG_TRIVIAL(warning) +#include "log.h" -#define DF BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ << ": " -#define IF BOOST_LOG_TRIVIAL(info) << __FUNCTION__ << ": " -#define WF BOOST_LOG_TRIVIAL(warning) << __FUNCTION__ << ": " - -#define UUID_PRIMARY_SERVICE 0x2800 -#define UUID_SECONDARY_SERVICE 0x2801 +#include "ByteBuffer.h" namespace trygvis { namespace bluetooth { @@ -130,7 +119,7 @@ class AttributeData { public: ~AttributeData(); - static AttributeData fromByteBuffer(ByteBuffer &value, uint8_t length); + static AttributeData fromByteBuffer(ByteBuffer &value); const uint16_t handle; const uint16_t groupEndHandle; diff --git a/ByteBuffer.cpp b/ByteBuffer.cpp index a33589e..2d4a258 100644 --- a/ByteBuffer.cpp +++ b/ByteBuffer.cpp @@ -1,82 +1,102 @@ #include "ByteBuffer.h" -#include <stdint-gcc.h> #include <string.h> #include <sstream> #include <iomanip> -#include <cassert> using namespace std; -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(const uint8_t *bytes, size_t capacity) : + bytes(bytes), capacity(capacity), zero(&bytes[0]), end(&bytes[capacity]) { + ptr = (uint8_t *) &bytes[0]; +} + +ByteBuffer::ByteBuffer(const uint8_t *bytes, size_t capacity, size_t zero, size_t size) : + bytes(bytes), capacity(capacity), zero(&bytes[0]), end(&bytes[size]) { assert(zero <= size); assert(size <= capacity); + ptr = (uint8_t *) &bytes[0]; +} + +ByteBuffer::ByteBuffer(const uint8_t *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::add8(uint8_t value) { - checkAndUpdateSize(1); - bytes[cursor++] = value; + checkAndUpdateEnd(1); + (*ptr++) = value; return *this; } ByteBuffer &ByteBuffer::add16le(uint16_t value) { - checkAndUpdateSize(2); - bytes[cursor++] = (uint8_t) (value & 0xff); - bytes[cursor++] = (uint8_t) ((value >> 8) & 0xff); + checkAndUpdateEnd(2); + (*ptr++) = (uint8_t) (value & 0xff); + (*ptr++) = (uint8_t) ((value >> 8) & 0xff); return *this; } uint8_t ByteBuffer::get8(size_t index) { - canAccessIndex(index); - return bytes[cursor]; + assertCanAccessRelative(index); + return *ptr++; } uint8_t ByteBuffer::get8() { - canAccessIndex(cursor); - return bytes[cursor++]; + assertCanAccessRelative(0); + return *ptr++; } uint16_t ByteBuffer::get16le() { - canAccessIndex(cursor + 1); + assertCanAccessRelative(0); uint16_t value; - value = bytes[cursor++]; - value |= ((uint16_t) bytes[cursor++]) << 8; + value = *ptr++; + value |= ((uint16_t) *ptr++) << 8; return value; } void ByteBuffer::copy(uint8_t *bytes, size_t length) { - canAccessIndex(length); + assertCanAccessRelative(length); - memcpy(bytes, &this->bytes[cursor], length); - cursor += length; + memcpy(bytes, ptr, length); + ptr += length; } -ByteBuffer ByteBuffer::view(size_t length) { - canAccessIndex(cursor + length); - size_t s = cursor + length; - return ByteBuffer(bytes, s, s, cursor); +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); } -void ByteBuffer::checkAndUpdateSize(size_t newBytes) { - size_t newSize = 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)); +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 *newPtr = ptr + newBytes; + if (newPtr >= end) { + if (newPtr >= &zero[capacity]) { + throw ByteBufferException(string("New size is too large! cursor=") + to_string(getCursor()) + ", size=" + to_string(getSize()) + ", capacity=" + to_string(capacity)); + } + end = newPtr; } +} - size = max(newSize, size); +void ByteBuffer::assertCanAccessRelative(size_t diff) const { + assertCanAccessIndex(ptr + diff); } -void ByteBuffer::canAccessIndex(size_t index) { - if (zero + index >= size) { - throw ByteBufferException(string("Out of bounds! zero=") + to_string(zero) + ", index=" + to_string(index) + ", size=" + to_string(size)); +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(size_t i = zero; i < size; i++) { - s << hex << setfill('0') << setw(2) << (int) bytes[i] << " "; + 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/ByteBuffer.h b/ByteBuffer.h index 828ee4b..d1d02bb 100644 --- a/ByteBuffer.h +++ b/ByteBuffer.h @@ -6,6 +6,9 @@ #include <string> #include <stdexcept> +// For now +#include "log.h" + class ByteBufferException : public std::runtime_error { public: ByteBufferException(std::string const &what) : std::runtime_error(what) { @@ -14,22 +17,37 @@ public: class ByteBuffer { public: - ByteBuffer(uint8_t *bytes, size_t capacity, size_t size, size_t zero = 0); - - inline size_t getSize() { - return size; + /** + * Wrapping constructor, the size will be equal to the capacity. + */ + ByteBuffer(const uint8_t *bytes, size_t capacity); + + /** + * Wrapping constructor. + */ + ByteBuffer(const uint8_t *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() { + inline size_t getCapacity() const { return capacity; } - inline size_t getCursor() { - return cursor; + inline size_t getCursor() const { + return ptr - zero; } inline void setCursor(size_t newCursor) { - cursor = newCursor; +// assertCanAccessRelative(newCursor); + ptr = (uint8_t *) &zero[newCursor]; + } + + inline void skip(size_t length) { + checkAndUpdateEnd(length); + ptr += length; } ByteBuffer &add8(uint8_t value); @@ -44,20 +62,32 @@ public: void copy(uint8_t *bytes, size_t length); - ByteBuffer view(size_t length); + /** + * 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: - void checkAndUpdateSize(size_t count); + ByteBuffer(const uint8_t *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 canAccessIndex(size_t count); + void assertCanAccessIndex(uint8_t *p) const; - uint8_t *bytes; - size_t zero; - size_t size; - size_t capacity; - size_t cursor; + const uint8_t *bytes; + const size_t capacity; + const uint8_t *zero; + const uint8_t *end; + uint8_t *ptr; }; #endif diff --git a/CMakeLists.txt b/CMakeLists.txt index 1098415..5c596e8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.4) project(ble_toys) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") set(Boost_USE_STATIC_LIBS OFF) set(Boost_USE_MULTITHREADED OFF) @@ -26,7 +26,7 @@ target_link_libraries(ble_toys ${Boost_LIBRARIES}) target_link_libraries(ble_toys ble) enable_testing() -find_package(Boost COMPONENTS unit_test_framework REQUIRED) +find_package(Boost COMPONENTS log unit_test_framework REQUIRED) # If we can change directory here add_definition and test-specific stuff could be moved to the test directory file(GLOB TEST_SRCS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} test/*.cpp) @@ -42,8 +42,9 @@ foreach(testSrc ${TEST_SRCS}) include_directories(${PROJECT_SOURCE_DIR}) #link to Boost libraries AND your targets and dependencies - target_link_libraries(${testName} ${Boost_LIBRARIES}) target_link_libraries(${testName} ble) + target_link_libraries(${testName} pthread) + target_link_libraries(${testName} ${Boost_LIBRARIES}) #I like to move testing binaries into a testBin directory set_target_properties(${testName} PROPERTIES diff --git a/LinuxBluetooth.cpp b/LinuxBluetooth.cpp index 5a00f53..3efa7e5 100644 --- a/LinuxBluetooth.cpp +++ b/LinuxBluetooth.cpp @@ -1,3 +1,5 @@ +#include "Bluetooth.h" + #include <string.h> #include <stropts.h> #include <sys/select.h> @@ -10,8 +12,6 @@ #include <sstream> #include <iomanip> -#include "Bluetooth.h" - // Got to love magic constants. Taken from bluez.git/tools/btgatt-client.c #define ATT_CID 4 @@ -33,7 +33,7 @@ public: ~LinuxBluetoothAdapter(); - void runScan(void (*callback)(BluetoothDevice &device)); + void runScan(void (*callback)(BluetoothDevice &device)) override; BluetoothDevice &getDevice(Mac &mac) override; @@ -155,7 +155,7 @@ void LinuxBluetoothDevice::discoverServices() { DF; uint8_t buffer[MAX_MTU]; - ByteBuffer out = ByteBuffer(buffer, MAX_MTU, 0); + ByteBuffer out = ByteBuffer(buffer, MAX_MTU); AttPdu::makeReadByGroupType(out, 0x0001, 0xffff, UUID_PRIMARY_SERVICE); @@ -165,9 +165,14 @@ void LinuxBluetoothDevice::discoverServices() { D << "written=" << written; ssize_t r = read(l2cap, buffer, MAX_MTU); - ByteBuffer in = ByteBuffer(buffer, MAX_MTU, r); - D << "read: " << r << " bytes"; + if (r == -1) { + throw BluetoothException(this, "read(): " + errnoAsString()); + } + + ByteBuffer in = ByteBuffer(buffer, r); + + D << "read: " << r << " bytes: " << in.toString(); vector<AttributeData> values = AttPdu::parseReadByGroupType(in); @@ -192,7 +197,7 @@ LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId) : D << "HCI socket: " << hciSocket; if (hciSocket == -1) { - throw BluetoothException(this, "Could not open HCI device " + hciDeviceId); + throw BluetoothException(this, "Could not open HCI device " + to_string(hciDeviceId)); } hci_filter_clear(&hciFilter); @@ -222,12 +227,12 @@ void LinuxBluetoothAdapter::startScan() { struct hci_dev_info di; if (hci_devinfo(hciDeviceId, &di) < 0) { - throw BluetoothException(this, "hci_devinfo: " + hciDeviceId); + 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=" << hex << setw(8) << setfill('0') << di.flags; + 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); @@ -0,0 +1,17 @@ +#ifndef LOG_H +#define LOG_H + +#include <boost/log/core.hpp> +#include <boost/log/trivial.hpp> +#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__ << ": " + +#define UUID_PRIMARY_SERVICE 0x2800 +#define UUID_SECONDARY_SERVICE 0x2801 + +#endif @@ -31,7 +31,7 @@ int main(int argc, char *argv[]) { } int e; - try { +// try { Mac mac = Mac::parseMac(argv[1]); targetMac = &mac; @@ -44,13 +44,13 @@ int main(int argc, char *argv[]) { // adapter->runScan(scan_callback); e = EXIT_SUCCESS; - } catch (std::runtime_error ex) { - W << "std::runtime_error: " << ex.what(); - e = EXIT_FAILURE; - } catch (std::exception ex) { - W << "std::exception: " << ex.what(); - e = EXIT_FAILURE; - } +// } catch (std::runtime_error ex) { +// W << "std::runtime_error: " << ex.what(); +// e = EXIT_FAILURE; +// } catch (std::exception ex) { +// W << "std::exception: " << ex.what(); +// e = EXIT_FAILURE; +// } shutdown(); return e; diff --git a/test/ByteBufferTest.cpp b/test/ByteBufferTest.cpp index 70a98ab..2a9bb8c 100644 --- a/test/ByteBufferTest.cpp +++ b/test/ByteBufferTest.cpp @@ -3,29 +3,40 @@ #define BOOST_TEST_MODULE "ByteBuffer" #include <boost/test/unit_test.hpp> +#include "Bluetooth.h" -class Utils { +#define checkBuffer(buffer, size, capacity, cursor) \ + D << "size=" << buffer.getSize() << ", capacity=" << buffer.getCapacity() << ", cursor=" << buffer.getCursor(); \ + BOOST_CHECK_EQUAL(buffer.getSize(), size); \ + BOOST_CHECK_EQUAL(buffer.getCapacity(), capacity); \ + BOOST_CHECK_EQUAL(buffer.getCursor(), cursor) + +using namespace std; + +class Bytes { public: - uint8_t bytes[1000]; + Bytes(size_t size) : capacity(size) { + _bytes = new uint8_t[size + 0x100]; + + bytes = (uint8_t *) (((uint64_t) &_bytes[0x100]) & 0xffffffffffffff00); - Utils() { - for (int i = 0; i < sizeof(bytes); i++) { + for (int i = 0; i < size; i++) { bytes[i] = (uint8_t) i; } } + ~Bytes() { + delete _bytes; + } + uint8_t *bytes; + uint8_t *_bytes; + size_t capacity; }; -void checkBuffer(ByteBuffer& buffer, size_t size, size_t capacity, size_t cursor) { - BOOST_CHECK(buffer.getSize() == size); - BOOST_CHECK(buffer.getCapacity() == capacity); - BOOST_CHECK(buffer.getCursor() == cursor); -} - BOOST_AUTO_TEST_CASE(empty_buffer) { - Utils u; - ByteBuffer buffer(u.bytes, sizeof(u.bytes), 0, 0); + Bytes b(0); + ByteBuffer buffer(b.bytes, b.capacity); - checkBuffer(buffer, 0, 1000, 0); + checkBuffer(buffer, 0, 0, 0); try { buffer.get8(); @@ -35,19 +46,49 @@ BOOST_AUTO_TEST_CASE(empty_buffer) { } BOOST_AUTO_TEST_CASE(basic) { - Utils u; - ByteBuffer buffer(u.bytes, sizeof(u.bytes), 10, 0); + Bytes b(1000); + ByteBuffer buffer(b.bytes, 1000, 0, 1000); + checkBuffer(buffer, 1000, 1000, 0); + + BOOST_CHECK_EQUAL(buffer.get8(), 0); + checkBuffer(buffer, 1000, 1000, 1); + + for (int i = 1; i < b.capacity; i++) { + BOOST_CHECK_EQUAL(buffer.get8(), b.bytes[i]); + } +} + +BOOST_AUTO_TEST_CASE(setCursor) { + Bytes b(1000); + ByteBuffer buffer(b.bytes, 1000, 0, 10); checkBuffer(buffer, 10, 1000, 0); - buffer.get8(); + BOOST_CHECK_EQUAL(buffer.get8(), 0); checkBuffer(buffer, 10, 1000, 1); -} -BOOST_AUTO_TEST_CASE(view) { - Utils u; - ByteBuffer buffer(u.bytes, sizeof(u.bytes), 10, 0); + buffer.setCursor(0); checkBuffer(buffer, 10, 1000, 0); - buffer.get8(); + BOOST_CHECK_EQUAL(buffer.get8(), 0); checkBuffer(buffer, 10, 1000, 1); + + buffer.setCursor(9); + checkBuffer(buffer, 10, 1000, 9); +} + +BOOST_AUTO_TEST_CASE(view) { + Bytes b(1000); + ByteBuffer buffer(b.bytes, b.capacity, 0, 10); + + BOOST_CHECK_EQUAL(buffer.get8(), 0); + ByteBuffer view1 = buffer.view(); + checkBuffer(view1, 9, 9, 0); + + BOOST_CHECK_EQUAL(view1.get8(), 1); + BOOST_CHECK_EQUAL(view1.get8(), 2); + + ByteBuffer view2 = view1.view(); + checkBuffer(view2, 7, 7, 0); + + BOOST_CHECK_EQUAL(view1.get8(), 3); } |