aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Bluetooth.cpp91
-rw-r--r--Bluetooth.h38
-rw-r--r--ByteBuffer.cpp54
-rw-r--r--ByteBuffer.h38
-rw-r--r--LinuxBluetooth.cpp50
-rw-r--r--main.cpp21
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 <sstream>
#include <iomanip>
#include <string.h>
+#include <hwloc.h>
#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<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());
}
- return pdu;
+ uint8_t length = bytes.get8();
+ DF << "length=" << (int) length;
+
+ size_t count = (bytes.getSize() - 2) / length;
+ DF << "count=" << count;
+
+ vector<AttributeData*> *values = new vector<AttributeData*>;
+ 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<AttributeData*>* 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 <stdexcept>
+#include <stdint-gcc.h>
+#include <string.h>
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 <cstdint>
#include <cstdlib>
+#include <string>
+#include <stdexcept>
+
+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<AttributeData*>* 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<Mac, LinuxBluetoothDevice *> devices;
+
+ 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);
@@ -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<int, LinuxBluetoothAdapter *> adapters;
- BluetoothAdapter &getDevice(int hciDevice) {
+ BluetoothAdapter &getAdapter(int hciDevice) {
map<int, LinuxBluetoothAdapter *>::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;
}
generated by cgit v1.2.3 (git 2.46.0) at 2024-11-11 09:35:48 +0000