diff options
Diffstat (limited to 'Bluetooth.cpp')
| -rw-r--r-- | Bluetooth.cpp | 91 |
1 files changed, 72 insertions, 19 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; } // ----------------------------------------------------------------------- |