1 files changed, 102 insertions, 0 deletions
diff --git a/ble/ByteBuffer.cpp b/ble/ByteBuffer.cpp
new file mode 100644
index 0000000..820c638
--- /dev/null
+++ b/ble/ByteBuffer.cpp
@@ -0,0 +1,102 @@
+#include "ByteBuffer.h"
+#include <string.h>
+#include <sstream>
+#include <iomanip>
+
+using namespace std;
+
+ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity) :
+    bytes(bytes), capacity(capacity), zero(bytes.get()), end(&bytes.get()[capacity]) {
+    ptr = (uint8_t *) zero;
+}
+
+ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t zero, size_t size) :
+        bytes(bytes), capacity(capacity), zero(&bytes.get()[zero]), end(&bytes.get()[size]) {
+    assert(zero <= size);
+    assert(size <= capacity);
+    ptr = (uint8_t *) zero;
+}
+
+ByteBuffer::ByteBuffer(const std::shared_ptr<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::write8(uint8_t value) {
+    checkAndUpdateEnd(1);
+    (*ptr++) = value;
+    return *this;
+}
+
+ByteBuffer &ByteBuffer::write16le(uint16_t value) {
+    checkAndUpdateEnd(2);
+    (*ptr++) = (uint8_t) (value & 0xff);
+    (*ptr++) = (uint8_t) ((value >> 8) & 0xff);
+    return *this;
+}
+
+uint8_t ByteBuffer::get8(size_t index) const {
+    assertCanAccessRelative(index);
+    return ptr[index];
+}
+
+uint8_t ByteBuffer::read8() {
+    assertCanAccessRelative(0);
+    return *ptr++;
+}
+
+uint16_t ByteBuffer::read16le() {
+    assertCanAccessRelative(0);
+    uint16_t value;
+    value = *ptr++;
+    value |= ((uint16_t) *ptr++) << 8;
+    return value;
+}
+
+void ByteBuffer::copy(uint8_t *bytes, size_t length) const {
+    assertCanAccessRelative(length - 1);
+
+    memcpy(bytes, ptr, length);
+}
+
+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);
+}
+
+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 *newEnd = ptr + newBytes;
+    if (newEnd >= end) {
+        if (newEnd >= &zero[capacity]) {
+            throw ByteBufferException(string("New size is too large! cursor=") + to_string(getCursor()) + ", size=" + to_string(getSize()) + ", capacity=" + to_string(capacity) + ", new bytes=" + to_string(newBytes));
+        }
+        end = newEnd;
+    }
+}
+
+void ByteBuffer::assertCanAccessRelative(size_t diff) const {
+    assertCanAccessIndex(ptr + diff);
+}
+
+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 (uint8_t *i = (uint8_t *) zero; i < end; i++) {
+        s << hex << setfill('0') << setw(2) << (int) *i << " ";
+    }
+
+    return string(s.str());
+}