diff --git a/etc/config.mk b/etc/config.mk index 9d7ba7add..a86f6a5b3 100644 --- a/etc/config.mk +++ b/etc/config.mk @@ -2,9 +2,9 @@ ifndef MAKE_INC_CONFIG_MK MAKE_INC_CONFIG_MK := 1 ifneq ($(TARGET),bare.pruv3) -CORELIBS := console utils executor os dcc openlcb withrottle +CORELIBS := console utils executor os dcc openlcb withrottle ble -LINKCORELIBS = -lconsole -lopenlcb -lwithrottle -ldcc -lexecutor -lutils -lexecutor -los +LINKCORELIBS = -lconsole -lopenlcb -lwithrottle -ldcc -lexecutor -lutils -lexecutor -los -lble endif endif # MAKE_INC_CONFIG_MK diff --git a/src/ble/Advertisement.cxx b/src/ble/Advertisement.cxx new file mode 100644 index 000000000..a2e7f4c77 --- /dev/null +++ b/src/ble/Advertisement.cxx @@ -0,0 +1,168 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file Advertisement.cxx + * + * Advertisement definition. + * + * @author Stuart Baker + * @date 2 March 2024 + */ + +#include "ble/Advertisement.hxx" + +namespace ble +{ + +// +// Advertisement::concat_service_data_128() +// +std::basic_string Advertisement::concat_service_data_128( + const uint8_t uuid[16], const void *buf, size_t size) +{ + const uint8_t *data = static_cast(buf); + std::basic_string result(uuid, uuid + 16); + result.append(data, size); + return result; +} + +// +// Advertisement::prepend() +// +int Advertisement::prepend( + Field field, Defs::AdvType type, const void *buf, size_t size, bool clip) +{ + const uint8_t *data = static_cast(buf); + std::basic_string *d; + size_t max; + + switch (field) + { + default: + case Field::DATA: + d = &data_; + max = extended_ ? + MAX_EXT_DATA_PAYLOAD_SIZE : MAX_SCAN_DATA_PAYLOAD_SIZE; + break; + case Field::SCAN_DATA: + HASSERT(!extended_); + d = &scanData_; + max = MAX_DATA_PAYLOAD_SIZE; + break; + } + + size_t space = std::min((size + 2), (max - d->size())); + if (space < (size + 2) && clip == false) + { + // Data doesn't fit and clipping is not allowed. + return -1; + } + d->insert(0, 1, space - 1); + d->insert(1, 1, static_cast(type)); + d->insert(2, data, space - 2); + return space; +} + +// +// Advertisement::append() +// +int Advertisement::append( + Field field, Defs::AdvType type, const void *buf, size_t size, bool clip) +{ + const uint8_t *data = static_cast(buf); + std::basic_string *d; + size_t max; + + switch (field) + { + default: + case Field::DATA: + d = &data_; + max = extended_ ? + MAX_EXT_DATA_PAYLOAD_SIZE : MAX_SCAN_DATA_PAYLOAD_SIZE; + break; + case Field::SCAN_DATA: + HASSERT(!extended_); + d = &scanData_; + max = MAX_DATA_PAYLOAD_SIZE; + break; + } + + size_t space = std::min((size + 2), (max - d->size())); + if (space < (size + 2) && clip == false) + { + // Data doesn't fit and clipping is not allowed. + return -1; + } + d->push_back(space - 1); + d->push_back(static_cast(type)); + d->append(data, space - 2); + return space; +} + +// +// Advertisement::update() +// +int Advertisement::update(Field field, Defs::AdvType type, const void *buf, + size_t size, unsigned instance, bool exact_size, + bool clip) +{ + const uint8_t *data = static_cast(buf); + std::basic_string *d; + size_t max; + + switch (field) + { + default: + case Field::DATA: + d = &data_; + max = extended_ ? + MAX_EXT_DATA_PAYLOAD_SIZE : MAX_SCAN_DATA_PAYLOAD_SIZE; + break; + case Field::SCAN_DATA: + HASSERT(!extended_); + d = &scanData_; + max = MAX_DATA_PAYLOAD_SIZE; + break; + } + + uint8_t len; + ssize_t pos = Defs::adv_find_data(*d, type, &len, instance); + if (pos < 0) + { + // No matching advertising data found. + return -1; + } + if (exact_size && len != size) + { + // Found the data, but it is the wrong size. + return -1; + } + size_t space = std::min((size + 2), (max - d->size()) + (len + 2)); + if (space < size && clip == false) + { + // Data doesn't fit and clipping is not allowed. + return -1; + } + d->at(pos) = space - 1; + d->replace(pos + 2, len, data, space - 2); + return space; +} + +} // namespace ble diff --git a/src/ble/Advertisement.cxxtest b/src/ble/Advertisement.cxxtest new file mode 100644 index 000000000..a6035ea7a --- /dev/null +++ b/src/ble/Advertisement.cxxtest @@ -0,0 +1,580 @@ +#include "utils/test_main.hxx" +#include "ble/Advertisement.hxx" + +using namespace ble; + +// A test UUID to be uses as a test advertisement service date type field. +static uint8_t testUUID[] = +{ + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F +}; + +// A test node ID to be used as a test advertisement service data type field. +static uint8_t testNodeID[] = {0x06, 0x05, 0x04, 0x03, 0x02, 0x01}; + +// A test node ID to be used as a test advertisement service data type field. +static uint8_t testNodeID2[] = {0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x07}; + +// +// Test all the constructor versions and initial state. +// +TEST(AdvertisementTest, Create) +{ + Advertisement adv; + Advertisement adv_ext(true); + Advertisement adv_reserve(20, 21); + Advertisement adv_reserve_ext(20, 21, true); + + EXPECT_EQ(0U, adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + EXPECT_FALSE(adv.is_extended()); + EXPECT_LE(0U, adv.test_get_data().capacity()); + EXPECT_LE(0U, adv.test_get_scan_data().capacity()); + + EXPECT_EQ(0U, adv_ext.get_data_size()); + EXPECT_EQ(0U, adv_ext.get_scan_data_size()); + EXPECT_LE(0U, adv_ext.test_get_data().capacity()); + EXPECT_LE(0U, adv_ext.test_get_scan_data().capacity()); + EXPECT_TRUE(adv_ext.is_extended()); + + EXPECT_EQ(0U, adv_reserve.get_data_size()); + EXPECT_EQ(0U, adv_reserve.get_scan_data_size()); + EXPECT_LE(20U, adv_reserve.test_get_data().capacity()); + EXPECT_LE(21U, adv_reserve.test_get_scan_data().capacity()); + EXPECT_FALSE(adv_reserve.is_extended()); + + EXPECT_EQ(0U, adv_reserve_ext.get_data_size()); + EXPECT_EQ(0U, adv_reserve_ext.get_scan_data_size()); + EXPECT_LE(20U, adv_reserve_ext.test_get_data().capacity()); + EXPECT_LE(0U, adv_reserve_ext.test_get_scan_data().capacity()); + EXPECT_TRUE(adv_reserve_ext.is_extended()); +} + +// +// Test all the consturctor versions that reserve, and possibly clip, capacity. +// +TEST(AdvertisementTest, CreateReserveClipSize) +{ + Advertisement adv_reserve(70, 71); + Advertisement adv_reserve_ext(370, 71, true); + + EXPECT_EQ(0U, adv_reserve.get_data_size()); + EXPECT_EQ(0U, adv_reserve.get_scan_data_size()); + EXPECT_EQ(31U, adv_reserve.test_get_data().capacity()); + EXPECT_EQ(31U, adv_reserve.test_get_scan_data().capacity()); + EXPECT_FALSE(adv_reserve.is_extended()); + + EXPECT_EQ(0U, adv_reserve_ext.get_data_size()); + EXPECT_EQ(0U, adv_reserve_ext.get_scan_data_size()); + EXPECT_LE(254U, adv_reserve_ext.test_get_data().capacity()); + EXPECT_LE(0U, adv_reserve_ext.test_get_scan_data().capacity()); + EXPECT_TRUE(adv_reserve_ext.is_extended()); +} + +// +// Test the building up of legacy advertisement data. +// +TEST(AdvertisementTest, BuildData) +{ + int result; + Advertisement adv; + + // Add 128-bit UUID + 6 bytes of "data". + std::basic_string uuid_128_data = adv.concat_service_data_128( + testUUID, testNodeID, sizeof(testNodeID)); + result = adv.append(Advertisement::Field::DATA, + Defs::AdvType::SERVICE_DATA_128, uuid_128_data); + EXPECT_EQ(2 + 22, result); + EXPECT_EQ(2U + 22U, adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01 + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Prepend flags to the data. + uint8_t flags = (uint8_t)Advertisement::Flags::LE_ONLY_GENERAL_DISC_MODE; + result = adv.prepend(Advertisement::Field::DATA, Defs::AdvType::FLAGS, + &flags, sizeof(flags)); + EXPECT_EQ(2 + 1, result); + EXPECT_EQ((2U + 1U) + (2U + 22U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 2, 1, 0x06, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01 + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Try to append name, without clipping. + std::string name("Long Name"); + result = adv.append(Advertisement::Field::DATA, + Defs::AdvType::NAME_COMPLETE, name.data(), name.size()); + EXPECT_EQ(-1, result); + EXPECT_EQ((2U + 1U) + (2U + 22U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + + // Try to prepend name, without clipping. + result = adv.prepend(Advertisement::Field::DATA, + Defs::AdvType::NAME_COMPLETE, name.data(), name.size()); + EXPECT_EQ(-1, result); + EXPECT_EQ((2U + 1U) + (2U + 22U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + + // Append name, with clipping. + result = adv.append(Advertisement::Field::DATA, + Defs::AdvType::NAME_COMPLETE, name.data(), name.size(), true); + EXPECT_EQ(2 + 2, result); + EXPECT_EQ((2U + 1U) + (2U + 22U) + (2U + 2U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 2, 1, 0x06, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, + 3, 0x09, 'L', 'o' + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Try to update a data type that does not exist. + name = "new name"; + result = adv.update(Advertisement::Field::DATA, + Defs::AdvType::NAME_SHORT, uuid_128_data); + EXPECT_EQ(-1, result); + EXPECT_EQ((2U + 1U) + (2U + 22U) + (2U + 2U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 2, 1, 0x06, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, + 3, 0x09, 'L', 'o' + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Try to update a data type that does exist, exact, but wrong size. + name = "new short name"; + result = adv.update(Advertisement::Field::DATA, + Defs::AdvType::NAME_COMPLETE, uuid_128_data); + EXPECT_EQ(-1, result); + EXPECT_EQ((2U + 1U) + (2U + 22U) + (2U + 2U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 2, 1, 0x06, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, + 3, 0x09, 'L', 'o' + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Try to update a data type that does exist, but oversized without clipping. + name = "new short name"; + result = adv.update(Advertisement::Field::DATA, + Defs::AdvType::NAME_COMPLETE, uuid_128_data, 1, false); + EXPECT_EQ(-1, result); + EXPECT_EQ((2U + 1U) + (2U + 22U) + (2U + 2U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 2, 1, 0x06, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, + 3, 0x09, 'L', 'o' + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Update the service data field. + uuid_128_data = adv.concat_service_data_128( + testUUID, testNodeID2, sizeof(testNodeID2)); + result = adv.update(Advertisement::Field::DATA, + Defs::AdvType::SERVICE_DATA_128, uuid_128_data); + EXPECT_EQ(2 + 22, result); + EXPECT_EQ((2U + 1U) + (2U + 22U) + (2U + 2U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 2, 1, 0x06, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x07, + 3, 0x09, 'L', 'o' + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } +} + +// +// Test the building up of legacy advertisement scan data. +// +TEST(AdvertisementTest, BuildScanData) +{ + int result; + Advertisement adv; + + // Add 128-bit UUID + 6 bytes of "data". + std::basic_string uuid_128_data = adv.concat_service_data_128( + testUUID, testNodeID, sizeof(testNodeID)); + result = adv.append(Advertisement::Field::SCAN_DATA, + Defs::AdvType::SERVICE_DATA_128, uuid_128_data); + EXPECT_EQ(2 + 22, result); + EXPECT_EQ(0U, adv.get_data_size()); + EXPECT_EQ(2U + 22U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01 + }; + uint8_t *actual = adv.get_scan_data(); + + ASSERT_THAT( + std::vector(actual, actual + adv.get_scan_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Prepend flags to the data. + uint8_t flags = (uint8_t)Advertisement::Flags::LE_ONLY_GENERAL_DISC_MODE; + result = adv.prepend(Advertisement::Field::SCAN_DATA, Defs::AdvType::FLAGS, + &flags, sizeof(flags)); + EXPECT_EQ(2 + 1, result); + EXPECT_EQ(0U, adv.get_data_size()); + EXPECT_EQ((2U + 1U) + (2U + 22U), adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 2, 1, 0x06, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01 + }; + uint8_t *actual = adv.get_scan_data(); + + ASSERT_THAT( + std::vector(actual, actual + adv.get_scan_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Try to append name, without clipping. + std::string name("Long Name"); + result = adv.append(Advertisement::Field::SCAN_DATA, + Defs::AdvType::NAME_COMPLETE, name.data(), name.size()); + EXPECT_EQ(-1, result); + EXPECT_EQ(0U, adv.get_data_size()); + EXPECT_EQ((2U + 1U) + (2U + 22U), adv.get_scan_data_size()); + + // Try to prepend name, without clipping. + result = adv.prepend(Advertisement::Field::SCAN_DATA, + Defs::AdvType::NAME_COMPLETE, name.data(), name.size()); + EXPECT_EQ(-1, result); + EXPECT_EQ(0U, adv.get_data_size()); + EXPECT_EQ((2U + 1U) + (2U + 22U), adv.get_scan_data_size()); + + // Append name, with clipping. + result = adv.append(Advertisement::Field::SCAN_DATA, + Defs::AdvType::NAME_COMPLETE, name.data(), name.size(), true); + EXPECT_EQ(2 + 2, result); + EXPECT_EQ(0U, adv.get_data_size()); + EXPECT_EQ((2U + 1U) + (2U + 22U) + (2U + 2U), adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 2, 1, 0x06, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, + 3, 0x09, 'L', 'o' + }; + uint8_t *actual = adv.get_scan_data(); + + ASSERT_THAT( + std::vector(actual, actual + adv.get_scan_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Update the service data field. + uuid_128_data = adv.concat_service_data_128( + testUUID, testNodeID2, sizeof(testNodeID2)); + result = adv.update(Advertisement::Field::SCAN_DATA, + Defs::AdvType::SERVICE_DATA_128, uuid_128_data); + EXPECT_EQ(2 + 22, result); + EXPECT_EQ(0U, adv.get_data_size()); + EXPECT_EQ((2U + 1U) + (2U + 22U) + (2U + 2U), adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 2, 1, 0x06, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x07, + 3, 0x09, 'L', 'o' + }; + uint8_t *actual = adv.get_scan_data(); + + ASSERT_THAT( + std::vector(actual, actual + adv.get_scan_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } +} + +// +// Test the building up of extended advertisement data. +// +TEST(AdvertisementTest, BuildDataExt) +{ + int result; + Advertisement adv(true); + + // Add 128-bit UUID + 6 bytes of "data". + std::basic_string uuid_128_data = adv.concat_service_data_128( + testUUID, testNodeID, sizeof(testNodeID)); + result = adv.append(Advertisement::Field::DATA, + Defs::AdvType::SERVICE_DATA_128, uuid_128_data); + EXPECT_EQ(2 + 22, result); + EXPECT_EQ(2U + 22U, adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01 + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Prepend flags to the data, as a basic_string + std::basic_string flags = + { + (uint8_t)Advertisement::Flags::LE_ONLY_GENERAL_DISC_MODE + }; + result = adv.prepend(Advertisement::Field::DATA, Defs::AdvType::FLAGS, + flags); + EXPECT_EQ(2 + 1, result); + EXPECT_EQ((2U + 1U) + (2U + 22U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 2, 1, 0x06, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01 + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Append name, without clipping. + std::string name("Long Name"); + result = adv.append(Advertisement::Field::DATA, + Defs::AdvType::NAME_COMPLETE, name.data(), name.size()); + EXPECT_EQ(2 + 9, result); + EXPECT_EQ((2U + 1U) + (2U + 22U) + (2U + 9U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 2, 1, 0x06, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, + 10, 0x09, 'L', 'o', 'n', 'g', ' ', 'N', 'a', 'm', 'e' + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } +} + +// +// Test the building up of advertiements using the name API. +// +TEST(AdvertisementTest, BuildName) +{ + int result; + Advertisement adv; + + // Append name, without clipping. + std::string name("Long Name"); + result = adv.append_name(Advertisement::Field::DATA, name); + EXPECT_EQ(2 + 9, result); + EXPECT_EQ((2U + 9U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 10, 0x09, 'L', 'o', 'n', 'g', ' ', 'N', 'a', 'm', 'e' + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + // Append name, with clipping. + std::string long_name("Really Really Long Name"); + result = adv.append_name(Advertisement::Field::DATA, long_name); + EXPECT_EQ(2 + 18, result); + EXPECT_EQ((2 + 18) + (2U + 9U), adv.get_data_size()); + EXPECT_EQ(0U, adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 10, 0x09, 'L', 'o', 'n', 'g', ' ', 'N', 'a', 'm', 'e', + 19, 0x08, 'R', 'e', 'a', 'l', 'l', 'y', ' ', 'R', 'e', 'a', 'l', + 'l', 'y', ' ', 'L', 'o', 'n', 'g' + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + + // Scan data, prepend name, without clipping. + result = adv.prepend_name(Advertisement::Field::SCAN_DATA, name); + EXPECT_EQ(2 + 9, result); + EXPECT_EQ((2 + 18) + (2U + 9U), adv.get_data_size()); + EXPECT_EQ((2U + 9U), adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 10, 0x09, 'L', 'o', 'n', 'g', ' ', 'N', 'a', 'm', 'e' + }; + uint8_t *actual = adv.get_scan_data(); + + ASSERT_THAT( + std::vector(actual, actual + adv.get_scan_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + // Scan data, prepend name, with clipping. + result = adv.prepend_name(Advertisement::Field::SCAN_DATA, long_name); + EXPECT_EQ(2 + 18, result); + EXPECT_EQ((2 + 18) + (2U + 9U), adv.get_data_size()); + EXPECT_EQ((2 + 18) + (2U + 9U), adv.get_scan_data_size()); + { + uint8_t expect[] = + { + 19, 0x08, 'R', 'e', 'a', 'l', 'l', 'y', ' ', 'R', 'e', 'a', 'l', + 'l', 'y', ' ', 'L', 'o', 'n', 'g', + 10, 0x09, 'L', 'o', 'n', 'g', ' ', 'N', 'a', 'm', 'e' + }; + uint8_t *actual = adv.get_scan_data(); + + ASSERT_THAT( + std::vector(actual, actual + adv.get_scan_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } +} diff --git a/src/ble/Advertisement.hxx b/src/ble/Advertisement.hxx new file mode 100644 index 000000000..e162f68f2 --- /dev/null +++ b/src/ble/Advertisement.hxx @@ -0,0 +1,297 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file Advertisement.hxx + * + * Advertisement definition. + * + * @author Stuart Baker + * @date 2 March 2024 + */ + +#ifndef _BLE_ADVERTISEMENT_HXX_ +#define _BLE_ADVERTISEMENT_HXX_ + +#include "ble/Defs.hxx" +#include "utils/macros.h" + +namespace ble +{ + +/// Object helper to define an advertisement +class Advertisement +{ +public: + /// Maximum payload size of data. + static constexpr size_t MAX_DATA_PAYLOAD_SIZE = 31; + + /// Maximum payload size of scan data. + static constexpr size_t MAX_SCAN_DATA_PAYLOAD_SIZE = 31; + + /// Maximum payload size of extended data + static constexpr size_t MAX_EXT_DATA_PAYLOAD_SIZE = 254; + + /// Data fields that make up an advertisement. + enum Field + { + DATA, ///< main data + SCAN_DATA, ///< scan data + }; + + /// Flag values. + enum class Flags : uint8_t + { + LE_LIMITED_DISC_MODE = 0x01, ///< BLE limited discovery mode + LE_GENERAL_DISC_MODE = 0x02, ///< BLE general discovery mode + BR_EDR_NOT_SUPPORTED = 0x04, ///< BR/EDR (classic) not supported + LE_BR_EDR_CONTROLLER = 0x08, ///< BLE + BR/EDR controller + LE_BR_EDR_HOST = 0x10, ///< BLE + BR/EDR Host + LE_ONLY_LIMITED_DISC_MODE = 0x05, ///< BLE only limited discovery mode + LE_ONLY_GENERAL_DISC_MODE = 0x06, ///< BEE only general discovery mode + }; + + /// Constructor. + /// @param extended true if an extended advertisement, else false + Advertisement(bool extended = false) + : extended_(extended) + { + } + + /// Constructor which reserves data and scan data space. This cannot + /// be and extended advertisement because it is assumed to have scan data. + /// @param data_reserve size in bytes to reserve for the data + /// @param scan_data_reserve size in bytes to reserve for the scan data + Advertisement(size_t data_reserve, size_t scan_data_reserve) + : extended_(false) + { + if (data_reserve > MAX_DATA_PAYLOAD_SIZE) + { + data_reserve = MAX_DATA_PAYLOAD_SIZE; + } + if (scan_data_reserve > MAX_SCAN_DATA_PAYLOAD_SIZE) + { + scan_data_reserve = MAX_SCAN_DATA_PAYLOAD_SIZE; + } + data_.reserve(data_reserve); + scanData_.reserve(scan_data_reserve); + } + + /// Constructor which reserves data space. + /// @param data_reserve size in bytes to reserve for the data + /// @param dummy not used, present to remove ambiguity + /// @param extended true if an extended advertisement, else false + Advertisement(size_t data_reserve, size_t dummy, bool extended) + : extended_(extended) + { + size_t max = + extended_ ? MAX_EXT_DATA_PAYLOAD_SIZE : MAX_DATA_PAYLOAD_SIZE; + if (data_reserve > max) + { + data_reserve = max; + } + data_.reserve(data_reserve); + } + + /// Concatenate a 128-bit (16-byte) UUID with provided data. + /// @param uuid 128-bit UUID + /// @param data data to concatenate + /// @param size size of data in bytes to concatenate + /// @return resulting string + std::basic_string concat_service_data_128( + const uint8_t uuid[16], const void *buf, size_t size); + + /// Concatenate a 128-bit (16-byte) UUID with provided data. + /// @param uuid 128-bit UUID + /// @param data data to concatenate + /// @return resulting string + std::basic_string concat_service_data_128( + const uint8_t uuid[16], std::basic_string &buf) + { + return concat_service_data_128(uuid, buf.data(), buf.size()); + } + + /// Add to the beginning of the advertisement. + /// @param f field to place the advertisement into + /// @param type type of data to add + /// @param buf data to add + /// @param size size of data in bytes + /// @param clip if the data does not all fit, clip the end of it off + /// @return number of bytes added, else -1 upon error + int prepend(Field field, Defs::AdvType type, const void *buf, size_t size, + bool clip = false); + + /// Add to the beginning of the advertisement. + /// @param field field to place the data into + /// @param type type of data to add + /// @param buf data to add + /// @param clip if the data does not all fit, clip the end of it off + /// @return number of bytes added, else -1 upon error + int prepend(Field field, Defs::AdvType type, + std::basic_string &buf, bool clip = false) + { + return prepend(field, type, buf.data(), buf.size(), clip); + } + + /// Add name to the beginning of the advertisement. Will use type + /// NAME_COMPLETE if it fits. Will use NAME_SHORT if it does not fit. + /// @param field field to place the data into + /// @param name name to add + /// @return number of bytes added, else -1 upon error + int prepend_name(Field field, std::string &name) + { + int result = prepend( + field, Defs::AdvType::NAME_COMPLETE, name.data(), name.size()); + if (result > 0) + { + return result; + } + return prepend( + field, Defs::AdvType::NAME_SHORT, name.data(), name.size(), true); + } + + /// Add to the end of the advertisement. + /// @param field field to place the data into + /// @param type type of data to add + /// @param buf data to add + /// @param size size of data in bytes + /// @param clip if the data does not all fit, clip the end of it off + /// @return number of bytes added, else -1 upon error + int append(Field field, Defs::AdvType type, const void *buf, size_t size, + bool clip = false); + + /// Add to the end of the advertisement. + /// @param field field to place the data into + /// @param type type of data to add + /// @param buf data to add + /// @param clip if the data does not all fit, clip the end of it off + /// @return number of bytes added, else -1 upon error + int append(Field field, Defs::AdvType type, std::basic_string &buf, + bool clip = false) + { + return append(field, type, buf.data(), buf.size(), clip); + } + + /// Add name to the end of the advertisement. Will use type + /// NAME_COMPLETE if it fits. Will use NAME_SHORT if it does not fit. + /// @param field field to place the data into + /// @param name name to add + /// @return number of bytes added, else -1 upon error + int append_name(Field field, std::string &name) + { + int result = append( + field, Defs::AdvType::NAME_COMPLETE, name.data(), name.size()); + if (result > 0) + { + return result; + } + return append( + field, Defs::AdvType::NAME_SHORT, name.data(), name.size(), true); + } + + /// Update existing advertisement. + /// @param field field to place the data into + /// @param type type of data to update + /// @param buf data to update + /// @param size size of data in bytes + /// @param instance the instance occurance to update + /// @param exact_size the new data size must match the old data size + /// @param clip if the data does not all fit, clip the end of it off + /// @return number of bytes updated, else -1 upon error + int update(Field field, Defs::AdvType type, const void *buf, size_t size, + unsigned instance = 1, bool exact_size = true, + bool clip = false); + + /// Update existing advertisement. + /// @param field field to place the data into + /// @param type type of data to update + /// @param buf data to update + /// @param size size of data in bytes + /// @param instance the instance occurance to update + /// @param exact_size the new data size must match the old data size + /// @param clip if the data does not all fit, clip the end of it off + /// @return number of bytes updated, else -1 upon error + int update(Field field, Defs::AdvType type, std::basic_string &buf, + unsigned instance = 1, bool exact_size = true, + bool clip = false) + { + return update( + field, type, buf.data(), buf.size(), instance, exact_size, clip); + } + + /// Test if extended advertisement or not. + /// @return true if extended advertisement, else false + bool is_extended() + { + return extended_; + } + + /// Get the advertisement data. + /// @return pointer to the advertisement data + uint8_t *get_data() + { + return (uint8_t*)data_.data(); + } + + /// Get the advertisement data size in bytes. + /// @return size in bytes + size_t get_data_size() + { + return data_.size(); + } + + /// Get the advertisement data. + /// @return pointer to the advertisement data + uint8_t *get_scan_data() + { + HASSERT(!extended_); + return (uint8_t*)scanData_.data(); + } + + /// Get the advertisement data size in bytes. + /// @return size in bytes + size_t get_scan_data_size() + { + return extended_ ? 0 : scanData_.size(); + } + +#if defined(GTEST) + std::basic_string &test_get_data() + { + return data_; + } + + std::basic_string &test_get_scan_data() + { + return scanData_; + } +#endif + +private: + /// advertising data, also used for extended advertising + std::basic_string data_; + + ///< advertising scan data + std::basic_string scanData_; + + bool extended_; ///< true extended advertisement, else false +}; + +} // namespace ble + +#endif // _BLE_ADVERTISEMENT_HXX_ diff --git a/src/ble/Connection.hxx b/src/ble/Connection.hxx new file mode 100644 index 000000000..397fa7b8f --- /dev/null +++ b/src/ble/Connection.hxx @@ -0,0 +1,217 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file Connection.hxx + * + * Information about active connections. + * + * @author Stuart Baker + * @date 18 March 2024 + */ + +#ifndef _BLE_CONNECTION_HXX_ +#define _BLE_CONNECTION_HXX_ + +#include +#include +#include + +#include "ble/Defs.hxx" +#include "utils/Singleton.hxx" + +namespace ble +{ + +// Forward declaration. +class Connections; + +/// Connection instance metadata. +class Connection +{ +public: + /// Get the connection handle. + /// @return connection handle + Defs::ConnHandle get_handle() + { + return handle_; + } + + /// Get the connection address. + /// @return connection address + void get_addr(Defs::Addr addr, Defs::AddrType *addr_type) + { + memcpy(addr, addr_, Defs::ADDR_LEN); + if (addr_type) + { + *addr_type = addrType_; + } + } + + /// Destructor. This happens when there is a disconnect. + ~Connection() + { + } + +private: + /// Constructor. + /// @param handle handle to the connection + /// @param addr peer address + /// @param addr_type peer address type + /// @param central true if central role, else peripheral role + Connection(Defs::ConnHandle handle, Defs::Addr addr, + Defs::AddrType addr_type, bool central) + : handle_(handle) + , addrType_(addr_type) + , central_(central) + { + memcpy(addr_, addr, Defs::ADDR_LEN); + } + + Defs::ConnHandle handle_; ///< handle to the connection + Defs::Addr addr_; ///< peer address + Defs::AddrType addrType_; ///< peer address type + bool central_; ///< true if central role, else peripheral role + + /// Allow private access from Connections container object. + friend class Connections; +}; + +/// Singleton container of all the active connections. +class Connections : public Singleton +{ +public: + /// Constructor. + Connections() + { + } + + /// Register an active connection. + /// @param handle handle to the connection + /// @param addr peer address + /// @param addr_type peer address type + /// @param central true if central role, else peripheral role + void register_connection(Defs::ConnHandle handle, Defs::Addr addr, + Defs::AddrType addr_type, bool central) + { + connections_.emplace_back(Connection(handle, addr, addr_type, central)); + } + + /// Register a callback at disconnection. + /// @param callback callback to call at disconnection + void register_disconnect_callback( + std::function callback) + { + callbacks_.emplace_back(callback); + } + + /// Called when a connection is disconnected. Results in the connection + /// being unregistered. + /// @param conn connection to disconnect + void disconnect(Connection *conn) + { + // Execute all of the registered disconnect callbacks. + for (auto &e : callbacks_) + { + e(conn); + } + // Remove the connection instance from the container. + for (auto it = connections_.begin(); it != connections_.end(); ++it) + { + if (&(*it) == conn) + { + connections_.erase(it); + // Should only be one entry per connection in the container. + return; + } + } + } + + /// Called when a connection is disconnected. Results in the connection + /// being unregistered. + /// @param handle handle belonging to connection to disconnect + void disconnect(Defs::ConnHandle handle) + { + // Remove the connection instance from the container. + for (auto it = connections_.begin(); it != connections_.end(); ++it) + { + if (it->get_handle() == handle) + { + // Execute all of the registered disconnect callbacks. + for (auto &e : callbacks_) + { + e(&(*it)); + } + connections_.erase(it); + // Should only be one entry per connection in the container. + return; + } + } + } + + /// Find a connection by its peer address. + /// @param addr peer address + /// @param addr_type peer address type + /// @return connection instance pointer, else nullptr if not found + Connection *lookup_by_address(Defs::Addr addr, Defs::AddrType addr_type) + { + for (auto &e : connections_) + { + if (!memcmp(e.addr_, addr, Defs::ADDR_LEN) && + e.addrType_ == addr_type) + { + // Match. + return &e; + } + } + return nullptr; + } + + /// Find a connection by its connection handle. + /// @param handle connection handle + /// @return connection instance pointer, else nullptr if not found + Connection *lookup_by_handle(Defs::ConnHandle handle) + { + for (auto &e : connections_) + { + if (e.handle_ == handle) + { + // Match. + return &e; + } + } + return nullptr; + } + + /// Get the number of active connections being tracked by this object. + /// @return number of active connections + size_t get_active_count() + { + return connections_.size(); + } + +private: + /// All the connections managed by the container. + std::vector connections_; + /// Callbacks to be called upon disconnection. + std::vector> callbacks_; +}; + +} // namespace ble + +#endif // _BLE_CONNECTION_HXX_ diff --git a/src/ble/Defs.cxx b/src/ble/Defs.cxx new file mode 100644 index 000000000..e7c05f8c1 --- /dev/null +++ b/src/ble/Defs.cxx @@ -0,0 +1,152 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file Defs.cxx + * + * BLE specific definitions. + * + * @author Stuart Baker + * @date 2 March 2024 + */ + +#include "ble/Defs.hxx" + +namespace ble +{ + +const uint8_t Defs::PRIMARY_SERVICE_UUID[2] = {0x00, 0x28}; +const uint8_t Defs::SECONDARY_SERVICE_UUID[2] = {0x01, 0x28}; +const uint8_t Defs::CHAR_DECLARATOIN_UUID[2] = {0x03, 0x28}; +const uint8_t Defs::CHAR_CLIENT_CONFIG_UUID[2] = {0x02, 0x29}; + +const uint8_t Defs::CHAR_PROP_READ_WRITE_NOTIFY[1] = +{ + (1 << 1) | // read + (1 << 3) | // write with response + (1 << 4) // notify +}; + +const uint8_t Defs::CHAR_PROP_READ_NOTIFY_ACK[1] = +{ + (1 << 1) | // read + (1 << 5) // notify with ack +}; + +const uint8_t Defs::CHAR_PROP_WRITE[1] = +{ + (1 << 3) // write +}; + +// +// Defs::adv_find_data() +// +ssize_t Defs::adv_find_data(std::basic_string &adv, + AdvType type, uint8_t *size, unsigned instance) +{ + char t = static_cast(type); + + for (size_t idx = 1; instance && idx < adv.size(); ++idx) + { + uint8_t len = adv[idx - 1]; + if (adv[idx] == t) + { + if (size) + { + *size = len - 1; + } + if (--instance == 0) + { + return idx - 1; + } + } + // One additional added by the four loop to skip over next length. + idx += len; + } + + return -1; +} + +// +// Defs::adv_find_name_short() +// +std::string Defs::adv_find_name_short( + std::basic_string &adv, unsigned instance) +{ + ssize_t pos; + uint8_t size; + + pos = adv_find_data(adv, AdvType::NAME_SHORT, &size, instance); + + return (pos < 0) ? + std::string() : + std::string((const char*)(adv.data() + pos + 2), size); +} + +// +// Defs::adv_find_name_complete() +// +std::string Defs::adv_find_name_complete( + std::basic_string &adv, unsigned instance) +{ + ssize_t pos; + uint8_t size; + + pos = adv_find_data(adv, AdvType::NAME_COMPLETE, &size, instance); + + return (pos < 0) ? + std::string() : + std::string((const char*)(adv.data() + pos + 2), size); +} + +// +// Defs::adv_find_service_data_128() +// +std::basic_string Defs::adv_find_service_data_128( + std::basic_string &adv, const uint8_t service_uuid[16], + unsigned instance) +{ + ssize_t pos; + unsigned inst = 1; + uint8_t size; + + for ( ; /* forever */ ; ) + { + pos = adv_find_data(adv, AdvType::SERVICE_DATA_128, &size, inst); + if (pos < 0) + { + // Not found. + break; + } + if (adv.compare(pos + 2, 16, service_uuid, 16) == 0) + { + // Match. + if (inst == instance) + { + // Found and instance count matches. + return std::basic_string(adv, pos + 2 + 16, size - 16); + } + } + ++inst; + } + + // Not found. + return std::basic_string(); +} + +} // namespace ble diff --git a/src/ble/Defs.cxxtest b/src/ble/Defs.cxxtest new file mode 100644 index 000000000..f5712a6e0 --- /dev/null +++ b/src/ble/Defs.cxxtest @@ -0,0 +1,108 @@ +#include "utils/test_main.hxx" +#include "ble/Defs.hxx" + +namespace ble +{ + +// A test UUID to be uses as a test advertisement service date type field. +static uint8_t testUUID[] = +{ + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F +}; + +// A test node ID to be used as a test advertisement service data type field. +static uint8_t testNodeID[] = {0x06, 0x05, 0x04, 0x03, 0x02, 0x01}; + +// A test node ID to be used as a test advertisement service data type field. +static uint8_t testNodeID2[] = {0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x07}; + +// +// Find advertisement names short +// +TEST(DefsTest, AdvFindNameShort) +{ + std::string name; + std::basic_string adv( + { + 6, 0x08, + 'N', 'a', 'm', 'e', '1', + 6, 0x08, + 'N', 'a', 'm', 'e', '2', + }); + + name = Defs::adv_find_name_short(adv); + EXPECT_EQ("Name1", name); + EXPECT_EQ(5U, name.size()); + + name = Defs::adv_find_name_short(adv, 2); + EXPECT_EQ("Name2", name); + EXPECT_EQ(5U, name.size()); +} + +// +// Find advertisement names long +// +TEST(DefsTest, AdvFindNameComplete) +{ + std::string name; + std::basic_string adv( + { + 6, 0x09, + 'N', 'a', 'm', 'e', '1', + 6, 0x09, + 'N', 'a', 'm', 'e', '2', + }); + + name = Defs::adv_find_name_complete(adv); + EXPECT_EQ("Name1", name); + EXPECT_EQ(5U, name.size()); + + name = Defs::adv_find_name_complete(adv, 2); + EXPECT_EQ("Name2", name); + EXPECT_EQ(5U, name.size()); +} + +// +// Find advertisement service data 128 +// +TEST(DefsTest, AdvFindServiceData128) +{ + std::basic_string data; + std::basic_string adv( + { + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, + 23, 0x21, + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, + 0x06, 0x07, + 0x08, 0x09, + 0x0A, 0x0B, + 0x0C, 0x0D, 0x0E, 0x0F, + 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x07, + }); + + data = Defs::adv_find_service_data_128(adv, testUUID); + EXPECT_EQ(0, data.compare(0, data.size(), testNodeID, sizeof(testNodeID))); + EXPECT_EQ(sizeof(testNodeID), data.size()); + + data = Defs::adv_find_service_data_128(adv, testUUID, 2); + EXPECT_EQ( + 0, data.compare(0, data.size(), testNodeID2, sizeof(testNodeID2))); + EXPECT_EQ(sizeof(testNodeID2), data.size()); + + // Not found. + data = Defs::adv_find_service_data_128(adv, testUUID, 3); + EXPECT_EQ(0U, data.size()); +} + + +} // namespace ble diff --git a/src/ble/Defs.hxx b/src/ble/Defs.hxx new file mode 100644 index 000000000..87aa9482f --- /dev/null +++ b/src/ble/Defs.hxx @@ -0,0 +1,169 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file Defs.hxx + * + * BLE specific definitions. + * + * @author Stuart Baker + * @date 2 March 2024 + */ + +#ifndef _BLE_DEFS_HXX_ +#define _BLE_DEFS_HXX_ + +#include // ssize_t on some platforms +#include +#include + +namespace ble +{ + +/// Miscellaneous BLE definitions. +class Defs +{ +public: + /// The value of an invalid or unitialized attribute handle. + static constexpr uint16_t ATTR_HANDLE_INVALID = 0; + + /// The value of an invalid or unitialized connection handle. + static constexpr uint16_t CONN_HANDLE_INVALID = 0xFFFF; + + /// The length of an address. + static constexpr uint8_t ADDR_LEN = 6; + + /// Primary service UUID. + static const uint8_t PRIMARY_SERVICE_UUID[2]; + + /// Secondary service UUID. + static const uint8_t SECONDARY_SERVICE_UUID[2]; + + /// Characteristic UUID. + static const uint8_t CHAR_DECLARATOIN_UUID[2]; + + /// Characterisitic Client Config Descriptor (CCCD) UUID. + static const uint8_t CHAR_CLIENT_CONFIG_UUID[2]; + + /// Characteristic read/write/notify property. + static const uint8_t CHAR_PROP_READ_WRITE_NOTIFY[1]; + + /// Characteristic read/write/notify property. + static const uint8_t CHAR_PROP_READ_NOTIFY_ACK[1]; + + /// Characteristic read/write/notify property. + static const uint8_t CHAR_PROP_WRITE[1]; + + /// BLE address. + typedef uint8_t Addr[ADDR_LEN]; + + /// Address Type. + typedef uint8_t AddrType; + + /// Connection handle. + typedef uint16_t ConnHandle; + + typedef uint16_t AttHandle; + + /// GATT Permisions. + enum class GATTPerm : uint8_t + { + READ = 0x01, ///< attribute is readable + WRITE = 0x02, ///< attribute is writable + AUTHEN_READ = 0x04, ///< read requires authentication + AUTHEN_WRITE = 0x08, ///< write requires authentication + AUTHOR_READ = 0x10, ///< read requires authorization + AUTHOR_WRITE = 0x20, ///< write requires authorization + ENCRYPT_READ = 0x40, ///< read requires encryption + ENCRYPT_WRITE = 0x80, ///< write requires encryption + }; + + /// UUID Sizes. + enum + { + UUID_LEN_16 = 2, ///< length in bytes of a 16-bit UUID + UUID_LEN_32 = 4, ///< length in bytes of a 32-bit UUID + UUID_LEN_128 = 16, ///< length in bytes of a 128-bit UUID + }; + + /// Advertising types. + enum class AdvType : uint8_t + { + FLAGS = 0x01, ///< GAP discovery modes + NAME_SHORT = 0x08, ///< shortened local name + NAME_COMPLETE = 0x09, ///< complete local name + SERVICE_DATA_128 = 0x21, ///< 128-bit service UUID folloed by data + }; + + /// Find an advertisment data within an advertisement set. + /// @param adv contents of the advertisement set + /// @param type the data type to find + /// @param size location to place the data size, size excludes type byte + /// @param instance which instance, starting from the front, to find + /// @return starting position of the length byte, else -1 if not found + static ssize_t adv_find_data(std::basic_string &adv, AdvType type, + uint8_t *size, unsigned instance = 1); + + /// Find an advertisment name short type within an advertisement set. + /// @param adv contents of the advertisement set + /// @param instance which instance, starting from the front, to find + /// @return string containing the name, empty string if not found. + static std::string adv_find_name_short( + std::basic_string &adv, unsigned instance = 1); + + /// Find an advertisment name complete type within an advertisement set. + /// @param adv contents of the advertisement set + /// @param instance which instance, starting from the front, to find + /// @return string containing the name, empty string if not found. + static std::string adv_find_name_complete( + std::basic_string &adv, unsigned instance = 1); + + /// Find an advertisment service data 128 type within an advertisement set. + /// @param adv contents of the advertisement set + /// @param service_uuid 128-bit UUID of the service to extract data from + /// @param instance which instance, starting from the front, to find + /// @return basic_string containing the data, empty basic_string if not + /// found. + static std::basic_string adv_find_service_data_128( + std::basic_string &adv, const uint8_t service_uuid[16], + unsigned instance = 1); +}; + +/// '|' operator for GATTPerm. +/// @param a left hand operand +/// @param b right hand operand +inline constexpr Defs::GATTPerm operator|( + const Defs::GATTPerm &a, const Defs::GATTPerm &b) +{ + return static_cast( + static_cast(a) | static_cast(b)); +} + +/// '&' operator for GATTPerm. +/// @param a left hand operand +/// @param b right hand operand +inline constexpr bool operator&( + const Defs::GATTPerm &a, const Defs::GATTPerm &b) +{ + return static_cast( + static_cast(a) & static_cast(b)); +} + +} // namespace ble + +#endif // _BLE_DEFS_HXX_ diff --git a/src/ble/Service.hxx b/src/ble/Service.hxx new file mode 100644 index 000000000..b84354cc0 --- /dev/null +++ b/src/ble/Service.hxx @@ -0,0 +1,54 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file Service.hxx + * + * BLE Service definition interface. + * + * @author Stuart Baker + * @date 2 March 2024 + */ + +#ifndef _BLE_SERVICE_HXX_ +#define _BLE_SERVICE_HXX_ + +#include + +#include "ble/Defs.hxx" + +namespace ble +{ + +/// BLE service definition interface +class Service +{ +public: + struct GATTAttribute + { + const uint8_t uuidLen_; ///< UUID_LEN_16, UUID_LEN_32, or UUID_LEN_16 + const uint8_t *uuid_; ///< 16-bit, 32-bit, or 128-bit UUID pointer + const Defs::GATTPerm perm_; ///< permissions + const uint16_t size_; ///< size in bytes + const uint8_t *value_; ///< value pointer + }; +}; + +} + +#endif // _BLE_SERVICE_HXX_ diff --git a/src/freertos_drivers/esp32/Esp32HardwareTwai.cxx b/src/freertos_drivers/esp32/Esp32HardwareTwai.cxx index 04d5c6b82..62bac6b1b 100644 --- a/src/freertos_drivers/esp32/Esp32HardwareTwai.cxx +++ b/src/freertos_drivers/esp32/Esp32HardwareTwai.cxx @@ -87,7 +87,7 @@ static constexpr int TWAI_VFS_FD = 0; static constexpr BaseType_t WATCHDOG_TASK_PRIORITY = ESP_TASK_TCPIP_PRIO - 1; /// Stack size (bytes) to use for the ESP32 TWAI status reporting task. -static constexpr BaseType_t WATCHDOG_TASK_STACK = 2048; +static constexpr BaseType_t WATCHDOG_TASK_STACK = 2548; /// Interval at which to print the ESP32 TWAI bus status. static constexpr TickType_t STATUS_PRINT_INTERVAL = pdMS_TO_TICKS(10000); diff --git a/src/openlcb/BLEAdvertisement.cxx b/src/openlcb/BLEAdvertisement.cxx new file mode 100644 index 000000000..bdd702e03 --- /dev/null +++ b/src/openlcb/BLEAdvertisement.cxx @@ -0,0 +1,66 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file BLEAdvertisement.cxx + * + * OpenLCB BLE Advertisement definition. + * + * @author Stuart Baker + * @date 9 March 2024 + */ + +#include "openlcb/BLEAdvertisement.hxx" + +#include + +#include "openlcb/BLEService.hxx" + +namespace openlcb +{ + +// +// BLEAdvertisement::BLEAdvertisement() +// +BLEAdvertisement::BLEAdvertisement( + NodeID node_id, const char *node_name, uint32_t pip) + : Advertisement(ble::Advertisement::MAX_DATA_PAYLOAD_SIZE, + ble::Advertisement::MAX_SCAN_DATA_PAYLOAD_SIZE) +{ + // Add flags to data. + uint8_t flags = static_cast(Flags::LE_ONLY_GENERAL_DISC_MODE); + append(Field::DATA, ble::Defs::AdvType::FLAGS, &flags, 1); + // Add name to data. + if (node_name && strlen(node_name) != 0) + { + std::string name(node_name); + append_name(Field::DATA, name); + } + + // Add OpenLCB service data 128 to scan data. + pip = htole32(pip); + node_id = htole64(node_id); + std::basic_string payload = + std::basic_string((uint8_t*)&node_id, 6) + + std::basic_string((uint8_t*)&pip, sizeof(pip)); + std::basic_string srvc_data = concat_service_data_128( + BLEService::GATT_SERVICE_UUID_OPENLCB, payload); + append(Field::SCAN_DATA, ble::Defs::AdvType::SERVICE_DATA_128, srvc_data); +} + +} // namespace openlcb diff --git a/src/openlcb/BLEAdvertisement.cxxtest b/src/openlcb/BLEAdvertisement.cxxtest new file mode 100644 index 000000000..479fde0f9 --- /dev/null +++ b/src/openlcb/BLEAdvertisement.cxxtest @@ -0,0 +1,93 @@ +#include "utils/test_main.hxx" +#include "openlcb/BLEAdvertisement.hxx" + +namespace openlcb +{ + +NodeID nodeIdTest = 0x060504030201UL; + +uint32_t pipTest = 0xF45EF000; + +// +// Advertisement where the node name fits. +// +TEST(BLEAdvertisementTest, CreateShortName) +{ + BLEAdvertisement adv(nodeIdTest, "Short Name", pipTest); + + { + uint8_t expect[] = + { + 2, 1, 0x06, // flags + 11, 0x09, // name complete + 'S', 'h', 'o', 'r', 't', ' ', 'N', 'a', 'm', 'e', + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + { + uint8_t expect[] = + { + 27, 0x21, // Service Data 128 + 0x51, 0x18, 0x8D, 0x82, 0x4C, 0xDA, // UUID + 0xE7, 0x83, + 0xAF, 0x4D, + 0xA9, 0x84, + 0x20, 0x52, 0xF4, 0x0F, + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, // Node ID + 0x00, 0xF0, 0x5E, 0xf4, // PIP + }; + uint8_t *actual = adv.get_scan_data(); + + ASSERT_THAT( + std::vector(actual, actual + adv.get_scan_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + +} + +// +// Advertisement where the node name does not fit. +// +TEST(BLEAdvertisementTest, CreateLongName) +{ + BLEAdvertisement adv(nodeIdTest, "Long Long Long Long Name 12", pipTest); + + { + uint8_t expect[] = + { + 2, 1, 0x06, // flags + 27, 0x08, // name short + 'L', 'o', 'n', 'g', ' ', 'L', 'o', 'n', 'g', ' ', + 'L', 'o', 'n', 'g', ' ', 'L', 'o', 'n', 'g', ' ', + 'N', 'a', 'm', 'e', ' ', '1', + }; + uint8_t *actual = adv.get_data(); + + ASSERT_THAT(std::vector(actual, actual + adv.get_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + { + uint8_t expect[] = + { + 27, 0x21, // Service Data 128 + 0x51, 0x18, 0x8D, 0x82, 0x4C, 0xDA, // UUID + 0xE7, 0x83, + 0xAF, 0x4D, + 0xA9, 0x84, + 0x20, 0x52, 0xF4, 0x0F, + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, // Node ID + 0x00, 0xF0, 0x5E, 0xf4, // PIP + }; + uint8_t *actual = adv.get_scan_data(); + + ASSERT_THAT( + std::vector(actual, actual + adv.get_scan_data_size()), + testing::ElementsAreArray(expect, sizeof(expect))); + } + +} + +} // namespace openlcb diff --git a/src/openlcb/BLEAdvertisement.hxx b/src/openlcb/BLEAdvertisement.hxx new file mode 100644 index 000000000..a5ded2374 --- /dev/null +++ b/src/openlcb/BLEAdvertisement.hxx @@ -0,0 +1,50 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file BLEAdvertisement.hxx + * + * OpenLCB BLE Advertisement definition. + * + * @author Stuart Baker + * @date 9 March 2024 + */ + +#ifndef _OPENLCB_BLEADVERTISEMENT_HXX_ +#define _OPENLCB_BLEADVERTISEMENT_HXX_ + +#include "ble/Advertisement.hxx" +#include "openlcb/Defs.hxx" + +namespace openlcb +{ + +/// OpenLCB BLE advertisement defintion. +class BLEAdvertisement : public ble::Advertisement +{ +public: + /// Constructor. + /// @param node_id Device node ID that will be used in the advertisement. + /// @param node_name node name that will be used in the advertisement. + /// @param pip supported OpenLCB protocols + BLEAdvertisement(NodeID node_id, const char *node_name, uint32_t pip); +}; + +} // namespace openlcb + +#endif // _OPENLCB_BLEADVERTISEMENT_HXX_ diff --git a/src/openlcb/BLEDefs.hxx b/src/openlcb/BLEDefs.hxx new file mode 100644 index 000000000..bcbd7f37d --- /dev/null +++ b/src/openlcb/BLEDefs.hxx @@ -0,0 +1,84 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file BLEDefs.hxx + * + * OpenLCB BLE specific definitions. + * + * @author Stuart Baker + * @date 10 March 2024 + */ + +#ifndef _OPENLCB_BLEDEFS_HXX_ +#define _OPENLCB_BLEDEFS_HXX_ + +#include + +#include "utils/Destructable.hxx" + +namespace openlcb +{ + +#include + +/// Shared base class for protocol implementation on a per-BLE-connection +/// basis. +class BLEProtocolEngine : public Destructable { +public: + /// Notifies the protocol engine that the connection has been + /// terminated. The implementation must (eventually) call `delete this`. + virtual void disconnect_and_delete() = 0; + + struct Deleter { + void operator()(BLEProtocolEngine* e) { + e->disconnect_and_delete(); + } + }; +}; + +using BLEProtocolEnginePtr = std::unique_ptr; + + +/// Miscellaneous BLE definitions. +class BLEDefs +{ +public: + /// Create a valid BLE random address from an OpenLCB Node ID. + /// @param id Node ID to use + /// @return random address + static std::basic_string random_address_from_node_id(NodeID id) + { + // Random addresses have 2 most significant bits set. + id |= 0x0000C00000000000ULL; + + return std::basic_string( + { + static_cast((id >> 40) & 0xFF), + static_cast((id >> 32) & 0xFF), + static_cast((id >> 24) & 0xFF), + static_cast((id >> 16) & 0xFF), + static_cast((id >> 8) & 0xFF), + static_cast((id >> 0) & 0xFF) + }); + } +}; + +} // namespace openlcb + +#endif // _OPENLCB_BLEDEFS_HXX_ diff --git a/src/openlcb/BLEGattClient.hxx b/src/openlcb/BLEGattClient.hxx new file mode 100644 index 000000000..2958c6dd8 --- /dev/null +++ b/src/openlcb/BLEGattClient.hxx @@ -0,0 +1,198 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file BLEGattClient.hxx + * + * Information about OpenLCB BLE Gatt Clients. + * + * @author Stuart Baker + * @date 20 March 2024 + */ + +#ifndef _OPENLCB_BLEGATTCLIENT_HXX_ +#define _OPENLCB_BLEGATTCLIENT_HXX_ + +#include + +#include "ble/Connection.hxx" +#include "utils/Singleton.hxx" +#include "openlcb/BLEDefs.hxx" + +namespace openlcb +{ + +/// Metadata for a BLE Gatt Client instance. +class BLEGattClient +{ +public: + /// Destructor. + ~BLEGattClient() + { + } + + /// Get the out bound data characteristic handle. + /// @return out bound data characteristic handle + ble::Defs::AttHandle get_out_bound() + { + return outBound_; + } + + /// Get the in bound data characteristic handle. + /// @return in bound data characteristic handle + ble::Defs::AttHandle get_in_bound() + { + return inBound_; + } + + /// Get the client connection. + /// @return client connection + ble::Connection *connection() + { + return conn_; + } + + /// Save a protocol engine into this object's ownership. The new protocol + /// engine will be deleted when this client is disconnected. One + /// connection can set only one protocol engine. Any previous one will be + /// deleted in this call. + /// @param protocol the newly created protocol engine. Will take ownership + /// and delete it via abandon_and_delete. + void set_protocol_engine(BLEProtocolEnginePtr protocol) + { + protocol_ = std::move(protocol); + } + + /// @return the protocol engine + BLEProtocolEngine* protocol_engine() { + return protocol_.get(); + } + +private: + /// Constructor. + /// @param conn BLE device connection + /// @param out_bound handle to out bound data characteristic + /// @param in_bound handle to in bound data characteristic + BLEGattClient(ble::Connection *conn, ble::Defs::AttHandle out_bound, + ble::Defs::AttHandle in_bound) + : conn_(conn) + , outBound_(out_bound) + , inBound_(in_bound) + { } + + ble::Connection *conn_; ///< BLE device connection + ble::Defs::AttHandle outBound_; ///< handle to out bound data characteristic + ble::Defs::AttHandle inBound_; ///< handle to in bound data characteristic + /// This object implements the protocol over this connection. It is used + /// only for onwership and notifying it upon disconnect. + BLEProtocolEnginePtr protocol_; + + /// Allow private access from BLEGattClients container object. + friend class BLEGattClients; +}; + +/// Singleton container of all the BLE Gatt Clients. +class BLEGattClients : public Singleton +{ +public: + /// Constructor. + BLEGattClients() + { + ble::Connections::instance()->register_disconnect_callback(std::bind( + &BLEGattClients::disconnect_callback, this, std::placeholders::_1)); + } + + /// Register an active client. + /// @param conn BLE device connection + /// @param out_bound handle to out bound data characteristic + /// @param in_bound handle to in bound data characteristic + /// @return newly created client instance. Ownership is retained, and this + /// object will be deleted at an unspecified time. + BLEGattClient *register_client(ble::Connection *conn, + ble::Defs::AttHandle out_bound, ble::Defs::AttHandle in_bound) + { + auto *c = + new BLEGattClient(conn, out_bound, in_bound); + clients_.emplace_back(c); + return c; + } + + /// @return the first client for the given connection ID. + BLEGattClient *find_client_by_out( + ble::Connection *conn, ble::Defs::AttHandle out_handle) + { + for (auto it = clients_.begin(); it != clients_.end(); ++it) + { + if (it->get()->connection() == conn && + it->get()->get_out_bound() == out_handle) + { + return it->get(); + } + } + return nullptr; + } + + /// @return the first client for the given connection ID. + BLEGattClient *find_client_by_in( + ble::Connection *conn, ble::Defs::AttHandle in_handle) + { + for (auto it = clients_.begin(); it != clients_.end(); ++it) + { + if (it->get()->connection() == conn && + it->get()->get_in_bound() == in_handle) + { + return it->get(); + } + } + return nullptr; + } + + /// Invoke a callback method on each of the registered clients. + /// @param callback callback to invoke. + void for_each_call(std::function callback) + { + for (auto it = clients_.begin(); it != clients_.end(); ++it) + { + callback(it->get()); + } + } + +private: + /// Callback for when a connection disconnect occurs. + /// @param conn BLE device connection + void disconnect_callback(ble::Connection *conn) + { + /// Remove any clients using the given connection from the container. + for (int i = 0; i < (int)clients_.size(); ++i) + { + if (clients_[i]->conn_ == conn) + { + clients_.erase(clients_.begin() + i); + --i; + continue; + } + } + } + + /// All the BLE Gatt clients managed by the container. + std::vector> clients_; +}; + +} // namespace openlcb + +#endif // _OPENLCB_BLEGATTCLIENT_HXX_ diff --git a/src/openlcb/BLEHubPort.hxx b/src/openlcb/BLEHubPort.hxx new file mode 100644 index 000000000..913e1092b --- /dev/null +++ b/src/openlcb/BLEHubPort.hxx @@ -0,0 +1,562 @@ +/** \copyright + * Copyright (c) 2024, Balazs Racz + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \file BLEHubPort.hxx + * + * DirectHubPort for sending/receiving traffic through a BLE stack. + * + * @author Balazs Racz + * @date 7 Jul 2024 + */ + +#ifndef _OPENLCB_BLEHUBPORT_HXX_ +#define _OPENLCB_BLEHUBPORT_HXX_ + +#include + +#include "executor/StateFlow.hxx" +#include "openlcb/BLEDefs.hxx" +#include "os/OS.hxx" +#include "utils/DirectHub.hxx" + +extern DataBufferPool g_direct_hub_data_pool; + +namespace openlcb +{ + +class BLEHubPort : public DirectHubPort, + private StateFlowBase, + public BLEProtocolEngine +{ +public: + /// This function needs to be implemented by the application that has the + /// specific BLE stack. Performs the actual send. Synchronous, with the + /// expectation being that once this function returns, the data is copied + /// away from the buffer and is sent or will eventually be sent. + /// + /// @param data data payload to send + /// @param len number of bytes to send + /// + using SendFunction = std::function; + + /// How big can a single attribute write be? ESP's BLE implementation says + /// 600 bytes. We keep some buffer. + static constexpr size_t MAX_BYTES_PER_WRITE = 220; + + /// Constructor + /// + /// @param hub pointer to the direct hub instance. Externally owned. + /// @param segmenter Segments incoming data into messages. Ownership will be + /// taken. Typically created using creat_gc_message_segmenter(). + /// @param ble_write_service Thread on which the send_function will be + /// invoked. + /// @param send_function Function that performs the actual send. See {\link + /// SendFunction} + /// @param on_error Will be notified upon disconnect. + /// + BLEHubPort(DirectHubInterface *hub, + std::unique_ptr segmenter, Service *ble_write_service, + SendFunction send_function, Notifiable *on_error = nullptr) + : StateFlowBase(ble_write_service) + , pendingShutdown_(false) + , waitingForAck_(false) + , sendFunction_(std::move(send_function)) + , onError_(on_error) + , input_(this, hub, std::move(segmenter)) + { + // Sets the initial state of the write flow to the stage where we read + // the next entry from the queue. + wait_and_call(STATE(read_queue)); + notRunning_ = true; + + hub->register_port(this); + } + + void disconnect_and_delete() override + { + { + AtomicHolder l(lock()); + HASSERT(!pendingShutdown_); + input_.hub_->unregister_port(this); + // After this, the next notify will eventually reach the shutdown + // and delete state. + pendingShutdown_ = true; + if (notRunning_) + { + notRunning_ = 0; + notify(); + } + } + // Synchronization point that ensures that there is no currently + // running Executable on this executor. This ensures that is no + // currently pending nor will there be any future invocations of + // sendFunction_ after this function returns. + service()->executor()->sync_run([]() {}); + } + + // ===== API from the BLE stack connection ===== + + /// Called by the BLE stack, when a send function is completed. + void ack() + { + ack_helper(); + LOG(ALWAYS, "BLE ack, pend %d", sendPending_); + } + + /// Called by the BLE stack, when a send has failed. + void nack() + { + ack_helper(); + LOG(ALWAYS, "BLE nack, pend %d", sendPending_); + } + + /// Called by the BLE stack when input data arrives from this remote + /// endpoint. + /// + /// @param data payload that arrived. The data will be copied inline, and + /// does not need to exist beyond when this function returns. + /// @param len number of bytes in the data payload. + /// + void input_data(const uint8_t *data, size_t len) + { + input_.input_data(data, len); + } + + /// Synchronous output routine called by the hub. + void send(MessageAccessor *msg) override + { + if (pendingShutdown_) + { + // Port already closed. Ignore data to send. + return; + } + { + AtomicHolder h(lock()); + if (pendingTail_ && pendingTail_->buf_.try_append_from(msg->buf_)) + { + totalPendingSize_ += msg->buf_.size(); + // Successfully enqueued the bytes into the tail of the queue. + // Nothing else to do here. + return; + } + } + + /// @todo we should try to collect the bytes into a buffer first before + /// enqueueing them. + BufferType *b; + mainBufferPool->alloc(&b); + b->data()->buf_.reset(msg->buf_); + if (msg->done_) + { + b->set_done(msg->done_->new_child()); + } + // Checks if we need to wake up the flow. + { + AtomicHolder h(lock()); + if (pendingShutdown_) + { + // Catch race condition when port is already closed. + b->unref(); + return; + } + pendingQueue_.insert_locked(b); + totalPendingSize_ += msg->buf_.size(); + pendingTail_ = b->data(); + if (notRunning_) + { + notRunning_ = 0; + // When we have exactly one buffer at hand, we release the done + // notify of it to allow the upstream stack to generate more + // data quicker. + b->set_done(nullptr); + } + else + { + // flow already running. Skip notify. + return; + } + } + notify(); + } + + /// Entry point to the flow, when an outgoing message got into the queue + /// and we are woken up. + Action read_queue() + { + if (pendingShutdown_) + { + return call_immediately(STATE(shutdown_and_exit)); + } + BufferType *head; + { + AtomicHolder h(lock()); + head = static_cast(pendingQueue_.next_locked().item); + if (!head) + { + notRunning_ = true; + return wait(); + } + HASSERT(head); + if (head->data() == pendingTail_) + { + pendingTail_ = nullptr; + } + totalPendingSize_ -= head->data()->buf_.size(); + } + + currentHead_.reset(head); + return call_immediately(STATE(do_write)); + } + + Action do_write() + { + if (pendingShutdown_) + { + return call_immediately(STATE(shutdown_and_exit)); + } + + const uint8_t *read_ptr; + size_t num_bytes; + auto &b = currentHead_->data()->buf_; + read_ptr = b.data_read_pointer(&num_bytes); + if (num_bytes > MAX_BYTES_PER_WRITE) + { + num_bytes = MAX_BYTES_PER_WRITE; + } + { + AtomicHolder h(lock()); + ++sendPending_; + } + LOG(INFO, "BLE send %d bytes pendcount %d queuesize %d/%d", (int)num_bytes, sendPending_, (int)totalPendingSize_, pendingQueue_.pending()); + sendFunction_(read_ptr, num_bytes); + b.data_read_advance(num_bytes); + if (b.size()) + { + return yield(); + } + else + { + currentHead_.reset(); + AtomicHolder h(lock()); + if (pendingShutdown_) + { + return call_immediately(STATE(shutdown_and_exit)); + } + if (sendPending_ > 1) + { + waitingForAck_ = 1; + return wait_and_call(STATE(read_queue)); + } + else if (pendingQueue_.empty()) + { + // go back to sleep + notRunning_ = 1; + return wait_and_call(STATE(read_queue)); + } + else + { + return yield_and_call(STATE(read_queue)); + } + } + } + + /// Invoked after pendingShutdown == true. At this point nothing gets added + /// to the pending queue. + Action shutdown_and_exit() + { + // Synchronization with other threads that might have written + // pendingShutdown == true. + { + AtomicHolder h(lock()); + } + + // Releases all buffers. + currentHead_.reset(); + while (auto *h = static_cast(pendingQueue_.next().item)) + { + h->unref(); + } + return delete_this(); + } + +protected: + void ack_helper() + { + { + AtomicHolder h(lock()); + --sendPending_; + if (waitingForAck_ && sendPending_ <= 1) + { + waitingForAck_ = false; + notify(); + } + } + } + + /// State flow that handles data arriving from this bluetooth connection. + class InputFlow : public StateFlowBase, private Atomic + { + public: + InputFlow(BLEHubPort* parent, DirectHubInterface *hub, + std::unique_ptr segmenter) + : StateFlowBase(hub->get_service()) + , parent_(parent) + , segmenter_(std::move(segmenter)) + , hub_(hub) + { + segmenter_->clear(); + flowWaiting_ = true; + wait_and_call(STATE(take_input)); + } + + /// Called by the BLE stack when input data arrives from this remote + /// endpoint. + /// + /// @param data payload that arrived. The data will be copied inline, + /// and does not need to exist beyond when this function returns. + /// @param len number of bytes in the data payload. + /// + void input_data(const uint8_t *data, size_t len) + { + while (len) + { + uint8_t *dst = nullptr; + size_t free = 0; + if (!appendBuf_.free()) + { + DataBuffer *p; + g_direct_hub_data_pool.alloc(&p); + appendBuf_.append_empty_buffer(p); + } + dst = appendBuf_.data_write_pointer(); + free = appendBuf_.free(); + if (len < free) { + free = len; + } + memcpy(dst, data, free); + len -= free; + data += free; + appendBuf_.data_write_advance(free); + } + { + AtomicHolder h(this); + HASSERT(transferBuf_.try_append_from(appendBuf_, true)); + if (flowWaiting_) { + flowWaiting_ = false; + notify(); + } + } + appendBuf_.data_read_advance(appendBuf_.size()); + } + + /// Moves over data from the other thread which is in the + /// transferBuf_. Continues on to segment and send them as messages to + /// the hub. + Action take_input() { + AtomicHolder h(this); + HASSERT(segmentBuf_.try_append_from(transferBuf_, true)); + transferBuf_.data_read_advance(transferBuf_.size()); + return call_immediately(STATE(segment_head)); + } + + /// Takes the head of segmentBuf_, and performs the message + /// segmentation on it. Puts the resulting byte offsets into the + /// outputBuf_. + Action segment_head() { + size_t len = 0; + const uint8_t* ptr = segmentBuf_.data_read_pointer(&len); + if (!len) { + AtomicHolder h(this); + if (transferBuf_.size()) { + return call_immediately(STATE(take_input)); + } + flowWaiting_ = true; + return wait_and_call(STATE(take_input)); + } + ssize_t segment_size = segmenter_->segment_message(ptr, len); + size_t xfer = len; + if (segment_size) { + xfer = segment_size - outputBuf_.size(); + } + LinkedDataBufferPtr p; + p.reset(segmentBuf_, xfer); + segmentBuf_.data_read_advance(xfer); + HASSERT(outputBuf_.try_append_from(p, true)); + if (segment_size) { + segmenter_->clear(); + // completed data + return call_immediately(STATE(send_output)); + } else { + // Need to segment more data. + return again(); + } + } + + /// Called when one full message is segmented into outputBuf_. Sends + /// this to the hub. + Action send_output() { + // We expect either an inline call to our run() method or + // later a callback on the executor. This sequence of calls + // prepares for both of those options. + wait_and_call(STATE(send_callback)); + inlineCall_ = 1; + sendComplete_ = 0; + hub_->enqueue_send(this); // causes the callback + inlineCall_ = 0; + if (sendComplete_) + { + return send_done(); + } + return wait(); + } + + /// This is the callback state that is invoked inline by the hub. Since + /// the hub invokes this->run(), a standard StateFlow will execute + /// whatever state is current. We have set STATE(send_callback) as the + /// current state above, hence the code continues in this function. + Action send_callback() + { + auto *m = hub_->mutable_message(); + /// @todo do we need to add barriernotifiables here? + //m->set_done(buf_.tail()->new_child()); + m->source_ = parent_; + // This call transfers the chained head of the current buffers, + // taking additional references where necessary or transferring the + // existing reference. It adjusts the skip_ and size_ arguments in + // buf_ to continue from where we left off. + m->buf_ = std::move(outputBuf_); + hub_->do_send(); + sendComplete_ = 1; + if (inlineCall_) + { + // do not disturb current state. + return wait(); + } + else + { + // we were called queued; go back to running the flow on the + // main executor. + return yield_and_call(STATE(send_done)); + } + } + + Action send_done() + { + // Goes back to looking at more data from the transfered buffers. + return call_immediately(STATE(segment_head)); + } + + private: + friend class BLEHubPort; + + /// Owning outside flow. + BLEHubPort* parent_; + + /// True if the flow is paused and waiting for more data to + /// arrive. Guarded by atomic *this. + bool flowWaiting_; + /// 1 if we got the send callback inline from the read_done. + uint8_t inlineCall_ : 1; + /// 1 if the run callback actually happened inline. + uint8_t sendComplete_ : 1; + + /// Current buffer for the input data. Owned by the input thread. + LinkedDataBufferPtr appendBuf_; + /// Buffer for transferring data rfrom the input thread to the service + /// thread. Guarded by atomic *this. + LinkedDataBufferPtr transferBuf_; + /// Buffer for the data being segmented. Owned by (only manipulated on) + /// the Service executor. + LinkedDataBufferPtr segmentBuf_; + /// Buffer for one sent message. This is the output of the segmenter. + LinkedDataBufferPtr outputBuf_; + /// Implementation (and state) of the business logic that segments + /// incoming bytes into messages that shall be given to the hub. + std::unique_ptr segmenter_; + /// Parent hub where output data is coming from. + DirectHubInterface *hub_; + + }; + + /// Holds the necessary information we need to keep in the queue about a + /// single output entry. Automatically unrefs the buffer whose pointer we + /// are holding when released. + struct OutputDataEntry + { + LinkedDataBufferPtr buf_; + }; + /// Type of buffers we are enqueuing for output. + typedef Buffer BufferType; + /// Type of the queue used to keep the output buffer queue. + typedef Q QueueType; + + /// @return lock usable for the write flow and the port altogether. + Atomic *lock() + { + return pendingQueue_.lock(); + } + + /// True if we have an error and we are trying to shut down. Causes all + /// outgoing data to be thrown away. + bool pendingShutdown_ : 1; + /// 1 if the state flow is paused, waiting for the notification. + bool notRunning_ : 1; + /// true if the write flow is paused waiting for the BLE stack to ack the + /// data. Should be notified when the acknowledgements make sendPending_ <= + /// 1. + bool waitingForAck_ : 1; + + /// Contains buffers of OutputDataEntries to write. lock() is the internal + /// lock of this object. + QueueType pendingQueue_; + /// Last tail pointer in the pendingQueue. If queue is empty, + /// nullptr. Protected by pendingQueue_.lock(). + OutputDataEntry *pendingTail_ = nullptr; + /// Total number of bytes in the pendingQueue. This does not include data + /// that is in currentHead_. Protected by lock(). + size_t totalPendingSize_ = 0; + + /// The buffer that is taken out of the queue while flushing. This variable + /// is not locked, because it is owned by the state flow states. + BufferPtr currentHead_; + + /// Function object used to send out actual data. This is synchronously + /// operating, meaning it makes a copy of the data to the stack for sending + /// it out. + SendFunction sendFunction_; + /// This notifiable will be called before exiting. + Notifiable *onError_ = nullptr; + + /// Number of in-flight messages sent but not acknowledged. + int sendPending_ {0}; + + InputFlow input_; + +}; // class BLEHubPort + +} // namespace openlcb + +#endif // _OPENLCB_BLEHUBPORT_HXX_ diff --git a/src/openlcb/BLEService.cxx b/src/openlcb/BLEService.cxx new file mode 100644 index 000000000..dc76ef546 --- /dev/null +++ b/src/openlcb/BLEService.cxx @@ -0,0 +1,69 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file BLEService.hxx + * + * OpenLCB BLE Service definition. + * + * @author Stuart Baker + * @date 2 March 2024 + */ + +#include "openlcb/BLEService.hxx" + +namespace openlcb +{ + +// 0ff45220-84a9-4daf-83e7-da4c828d1851 +const uint8_t BLEService::GATT_SERVICE_UUID_OPENLCB[16] = +{ + 0x51, 0x18, 0x8D, 0x82, 0x4C, 0xDA, + 0xE7, 0x83, + 0xAF, 0x4D, + 0xA9, 0x84, + 0x20, 0x52, 0xF4, 0x0F, +}; + +// b029e1ba-47bc-4d8a-a28e-fbe167fc06cb +const uint8_t BLEService::GATT_CHAR_UUID_DATA_IN[16] = +{ + 0xCB, 0x06, 0xfC, 0x67, 0xE1, 0xFB, + 0x8E, 0xA2, + 0x8A, 0x4D, + 0xBC, 0x47, + 0xBA, 0xE1, 0x29, 0xB0, +}; + +// c4fed917-55d9-485d-bf30-2eb3c095a90f +const uint8_t BLEService::GATT_CHAR_UUID_DATA_OUT[16] = +{ + 0x0F, 0xA9, 0x95, 0xC0, 0xB3, 0x2E, + 0x30, 0xBF, + 0x5D, 0x48, + 0xD9, 0x55, + 0x17, 0xD9, 0xFE, 0xC4, +}; + +uint8_t BLEService::dataIn_[200]; +uint8_t BLEService::dataOut_[200]; +uint8_t BLEService::dataInCCCD_[2] = {0x00, 0x00}; +uint8_t BLEService::dataOutCCCD_[2] = {0x00, 0x00}; + + +} // openlcb diff --git a/src/openlcb/BLEService.hxx b/src/openlcb/BLEService.hxx new file mode 100644 index 000000000..b0d3b3eb6 --- /dev/null +++ b/src/openlcb/BLEService.hxx @@ -0,0 +1,143 @@ +/** @copyright + * Copyright (c) 2024, Stuart Baker + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are strictly prohibited without written consent. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file BLEService.hxx + * + * OpenLCB BLE Service definition. + * + * @author Stuart Baker + * @date 2 March 2024 + */ + +#ifndef _OPENLCB_BLESERVICE_HXX_ +#define _OPENLCB_BLESERVICE_HXX_ + +#include + +#include "ble/Service.hxx" +#include "utils/macros.h" + +namespace openlcb +{ + +/// OpenLCB BLE service definition. +class BLEService : public ble::Service +{ +public: + /// OpenLCB GATT service UUID. + static const uint8_t GATT_SERVICE_UUID_OPENLCB[16]; + + /// OpenLCB GATT data in characterisitic UUID. + static const uint8_t GATT_CHAR_UUID_DATA_IN[16]; + + /// OpenLCB GATT data out characterisitic UUID. + static const uint8_t GATT_CHAR_UUID_DATA_OUT[16]; + +private: + static uint8_t dataIn_[200]; ///< data incoming to the server + static uint8_t dataOut_[200]; ///< data outgoing from the server + + static uint8_t dataInCCCD_[2]; ///< configuration descriptor for input + static uint8_t dataOutCCCD_[2]; ///< configuration descriptor for output + +public: + /// The entry indexes for the GATT_ATTRIBUTES table. + enum GATTAttributeIndexs + { + GATT_SERVICE_INDEX = 0, ///< GATT Primary Service index + GATT_DATA_IN_DECL_INDEX, ///< GATT data in declaration index + GATT_DATA_IN_VALUE_INDEX, ///< GATT data in value index + GATT_DATA_IN_CCCD_INDEX, ///< GATT data in CCCD index + GATT_DATA_OUT_DECL_INDEX, ///< GATT data out declaration index + GATT_DATA_OUT_VALUE_INDEX, ///< GATT data out value index + GATT_DATA_OUT_CCCD_INDEX, ///< GATT data out CCCD index + }; + + /// GATT Attribute table properties for this service. + static constexpr GATTAttribute GATT_ATTRIBUTES[] = + { + // Primary service. + { + ble::Defs::UUID_LEN_16, + ble::Defs::PRIMARY_SERVICE_UUID, + ble::Defs::GATTPerm::READ, + sizeof(GATT_SERVICE_UUID_OPENLCB), + GATT_SERVICE_UUID_OPENLCB + }, + // Characteristic declaration for data in. + { + ble::Defs::UUID_LEN_16, + ble::Defs::CHAR_DECLARATOIN_UUID, + ble::Defs::GATTPerm::READ, + sizeof(ble::Defs::CHAR_PROP_WRITE), + ble::Defs::CHAR_PROP_WRITE + }, + // Characteristic value for data in. + { + ble::Defs::UUID_LEN_128, + GATT_CHAR_UUID_DATA_IN, + ble::Defs::GATTPerm::WRITE, + sizeof(dataIn_), + dataIn_ + }, + // Client characteristic configuration descriptor (CCCD) for data in. + { + ble::Defs::UUID_LEN_16, + ble::Defs::CHAR_CLIENT_CONFIG_UUID, + ble::Defs::GATTPerm::READ | ble::Defs::GATTPerm::WRITE, + sizeof(dataInCCCD_), + dataInCCCD_ + }, + // Characteristic declaration for data out. + { + ble::Defs::UUID_LEN_16, + ble::Defs::CHAR_DECLARATOIN_UUID, + ble::Defs::GATTPerm::READ, + sizeof(ble::Defs::CHAR_PROP_READ_NOTIFY_ACK), + ble::Defs::CHAR_PROP_READ_NOTIFY_ACK + }, + // Characteristic value for data out. + { + ble::Defs::UUID_LEN_128, + GATT_CHAR_UUID_DATA_OUT, + ble::Defs::GATTPerm::READ, + sizeof(dataOut_), + dataOut_ + }, + // Client characteristic configuration descriptor (CCCD) for data out. + { + ble::Defs::UUID_LEN_16, + ble::Defs::CHAR_CLIENT_CONFIG_UUID, + ble::Defs::GATTPerm::READ | ble::Defs::GATTPerm::WRITE, + sizeof(dataOutCCCD_), + dataOutCCCD_ + }, + }; + + /// Get the size in number of elements (Attributes). + /// @return number of elements + static constexpr size_t size() + { + return ARRAYSIZE(GATT_ATTRIBUTES); + } +}; + +} // namespace ble + +#endif // _OPENLCB_BLESERVICE_HXX_ diff --git a/src/openlcb/SimpleStack.hxx b/src/openlcb/SimpleStack.hxx index c5e7281c8..cf3498dfd 100644 --- a/src/openlcb/SimpleStack.hxx +++ b/src/openlcb/SimpleStack.hxx @@ -256,6 +256,13 @@ public: node()->iface()->addressed_message_write_flow()->send(b); } + /// Get the PIP response value. + /// @return PIP value + virtual uint64_t get_pip() + { + return 0; + } + protected: /// Call this function once after the actual IO ports are set up. Calling /// before the executor starts looping is okay. @@ -624,6 +631,13 @@ private: default_start_node(); } + /// Get the PIP response value. + /// @return PIP value + uint64_t get_pip() override + { + return PIP_RESPONSE; + } + /// The actual node. DefaultNode node_; /// Handles PIP requests. @@ -654,6 +668,13 @@ private: default_start_node(); } + /// Get the PIP response value. + /// @return PIP value + uint64_t get_pip() override + { + return PIP_RESPONSE; + } + /// The actual node. DefaultNode node_; /// Handles PIP requests. @@ -689,6 +710,13 @@ private: void start_node() override; + /// Get the PIP response value. + /// @return PIP value + uint64_t get_pip() override + { + return PIP_RESPONSE; + } + TrainService tractionService_ {iface()}; /// The actual node. TrainNodeWithId trainNode_; diff --git a/src/openlcb/sources b/src/openlcb/sources index 92ffba824..59035f0ad 100644 --- a/src/openlcb/sources +++ b/src/openlcb/sources @@ -5,6 +5,8 @@ CSRCS += CXXSRCS += \ AliasAllocator.cxx \ AliasCache.cxx \ + BLEAdvertisement.cxx \ + BLEService.cxx \ BroadcastTime.cxx \ BroadcastTimeDefs.cxx \ BroadcastTimeClient.cxx \ diff --git a/src/utils/DataBuffer.cxxtest b/src/utils/DataBuffer.cxxtest index ab52ca84b..48b242d36 100644 --- a/src/utils/DataBuffer.cxxtest +++ b/src/utils/DataBuffer.cxxtest @@ -3,6 +3,7 @@ #include "utils/test_main.hxx" DataBufferPool g_pool(64); +DataBufferPool g_pool10(10); class DataBufferTest : public ::testing::Test { @@ -390,3 +391,263 @@ TEST_F(DataBufferTest, lnk_multi) // The barriers will verify upon destruction time that they were correctly // notified. } + +class DataBufferFuzzTest : public ::testing::Test +{ +protected: + DataBufferFuzzTest() + { + for (int i = 0; i < NUM_OP; ++i) { + freq_[i] = 0; + } + freq_[0] = 1; + freq_[NUM_OP] = 0; + } + + enum Op { + OP_APPEND, + OP_READ, + OP_XFERMID, + OP_READMID, + OP_XFEREND, + OP_READEND, + NUM_OP + }; + + int freq_[NUM_OP + 1]; + + /// @return a pseudorandom number uniformly distributed between 0 and max - + /// 1. + /// @param max distribution parameter. + unsigned get_random_uni(unsigned max) + { + return rand_r(&randSeed_) % max; + } + + /// Setup a fuzz test scenario where we append a given LinkedDataBufferPtr + /// and then read from the same one. + void setup_basic_readwrite() { + freq_[OP_APPEND] = 1; + freq_[OP_READ] = 1; + } + + /// Setup a fuzz test scenario where we append one LinkedDataBufferPtr, + /// then move data to a middle one, then read that middle one. + void setup_write_transfer_read() { + freq_[OP_APPEND] = 1; + freq_[OP_XFERMID] = 1; + freq_[OP_READMID] = 1; + } + + /// Setup a fuzz test scenario where we append one LinkedDataBufferPtr, + /// then move data to a middle one, then move data to a third one, then + /// read that last. + void setup_write_transfer_read_transfer_read() { + freq_[OP_APPEND] = 1; + freq_[OP_XFERMID] = 1; + freq_[OP_XFEREND] = 1; + freq_[OP_READEND] = 1; + } + + void prep_fuzz() { + int sum = 0; + for (int i = 0; i <= NUM_OP; ++i) { + sum += freq_[i]; + freq_[i] = sum; + } + } + + void run_fuzz(unsigned iter) { + prep_fuzz(); + size_t idx = 0; + while (--iter && !HasFatalFailure()) + { + int oper = get_random_uni(freq_[NUM_OP]); + for (int i = 0; i < NUM_OP; ++i) { + if (freq_[i] > oper) { + SCOPED_TRACE(idx); + run_op((Op)i); + ++idx; + break; + } + } + } + } + + void run_op(Op op) { + switch(op) { + case OP_APPEND: { + int len = get_random_uni(22); + append_helper(&lnk_, len); + break; + } + case OP_READ: { + int len = get_random_uni(22); + consume_helper(&lnk_, len); + break; + } + case OP_XFERMID: { + int len = get_random_uni(22); + xfer_helper(&lnk_, &mid_, len); + break; + } + case OP_READMID: { + int len = get_random_uni(22); + consume_helper(&mid_, len); + break; + } + case OP_XFEREND: { + int len = get_random_uni(22); + xfer_helper(&mid_, &end_, len); + break; + } + case OP_READEND: { + int len = get_random_uni(22); + consume_helper(&end_, len); + break; + } + default: + return; + } + } + + std::string flatten(const LinkedDataBufferPtr &p) + { + std::string ret; + p.append_to(&ret); + return ret; + } + + /// Appends a certain number of characters to a ptr. Characters are always + /// taken in the input sequence. + void append_helper(LinkedDataBufferPtr *p, size_t len) + { + while (len) + { + int free = p->free(); + if (!free) + { + DataBuffer *c; + g_pool10.alloc(&c); + p->append_empty_buffer(c); + continue; + } + auto *rp = p->data_write_pointer(); + int count = 0; + while (free > 0 && len > 0) + { + *rp++ = generate(); + --free; + --len; + ++count; + } + p->data_write_advance(count); + } + } + + /// Appends a certain number of characters to a ptr. Characters are always + /// taken in the input sequence. + void xfer_helper( + LinkedDataBufferPtr *from, LinkedDataBufferPtr *to, size_t len) + { + LinkedDataBufferPtr tmp; + len = std::min(len, (size_t)from->size()); + tmp.reset(*from, len); + from->data_read_advance(len); + ASSERT_TRUE(to->try_append_from(tmp, true)); + } + + /// Consumes (reads) a certain number of characters from a ptr. Characters + /// are compared to the expected output sequence. + void consume_helper(LinkedDataBufferPtr *p, size_t len) + { + while (len > 0 && p->size() > 0) + { + size_t avail; + const uint8_t *ptr = p->data_read_pointer(&avail); + if (avail > len) + { + avail = len; + } + int count = 0; + while (avail) + { + consume(*(ptr++)); + ++count; + --avail; + --len; + } + p->data_read_advance(count); + } + } + + /// @return the next byte of the generated sequence. + uint8_t generate() { + return nextByte_++; + } + + /// Take in the next byte that came out at the end. Verifies that it is the + /// correct byte value. + void consume(uint8_t next_byte) { + EXPECT_EQ(nextByteRead_, next_byte); + ++nextByteRead_; + } + + DataBuffer *b_; + unsigned lastFree_; + unsigned int randSeed_{83012475}; + uint8_t nextByte_{0}; + uint8_t nextByteRead_{0}; + + BarrierNotifiable bn_; + BarrierNotifiable bn2_; + LinkedDataBufferPtr lnk_; + LinkedDataBufferPtr mid_; + LinkedDataBufferPtr end_; + std::vector > bns_; +}; + +TEST_F(DataBufferFuzzTest, small_fuzz) { + setup_basic_readwrite(); + run_fuzz(10); +} + +TEST_F(DataBufferFuzzTest, medium_fuzz) { + setup_basic_readwrite(); + run_fuzz(1000); +} + +TEST_F(DataBufferFuzzTest, large_fuzz) { + setup_basic_readwrite(); + run_fuzz(100000); +} + +TEST_F(DataBufferFuzzTest, small_duo) { + setup_write_transfer_read(); + run_fuzz(10); +} + +TEST_F(DataBufferFuzzTest, medium_duo) { + setup_write_transfer_read(); + run_fuzz(1000); +} + +TEST_F(DataBufferFuzzTest, large_duo) { + setup_write_transfer_read(); + run_fuzz(100000); +} + +TEST_F(DataBufferFuzzTest, small_tri) { + setup_write_transfer_read_transfer_read(); + run_fuzz(10); +} + +TEST_F(DataBufferFuzzTest, medium_tri) { + setup_write_transfer_read_transfer_read(); + run_fuzz(1000); +} + +TEST_F(DataBufferFuzzTest, large_tri) { + setup_write_transfer_read_transfer_read(); + run_fuzz(100000); +} diff --git a/src/utils/DataBuffer.hxx b/src/utils/DataBuffer.hxx index d7b940715..07ad64f4d 100644 --- a/src/utils/DataBuffer.hxx +++ b/src/utils/DataBuffer.hxx @@ -37,9 +37,22 @@ #define _UTILS_DATABUFFER_HXX_ #include "utils/Buffer.hxx" +#include "utils/LinkedObject.hxx" +#include "utils/macros.h" + + +#ifdef GTEST +//#define DEBUG_DATA_BUFFER_FREE +#endif class DataBufferPool; + +#ifdef DEBUG_DATA_BUFFER_FREE +class DataBuffer; +static void check_db_ownership(DataBuffer* p); +#endif + /// Specialization of the Buffer class that is designed for storing untyped /// data arrays. Adds the ability to treat the next pointers as links to /// consecutive data bytes, ref'ing and unref'ing a sequence of buffers in one @@ -118,7 +131,8 @@ public: } /// Releases one reference to all blocks of this buffer. This includes one - /// reference to the last block which may be a partially filled buffer. + /// reference to the last block which may be a partially filled + /// buffer. Calling with zero length will call release on the head block. /// @param total_size the number of bytes starting from the beginning of /// *this. void unref_all(unsigned total_size) @@ -165,6 +179,15 @@ public: return curr->next(); } +#ifdef DEBUG_DATA_BUFFER_FREE + void unref() { + if (references() == 1) { + check_db_ownership(this); + } + Buffer::unref(); + } +#endif + private: friend class DataBufferPool; @@ -176,8 +199,13 @@ private: using DataBufferPtr = std::unique_ptr>; +class LinkedDataBufferPtr; + /// A class that keeps ownership of a chain of linked DataBuffer references. class LinkedDataBufferPtr +#ifdef DEBUG_DATA_BUFFER_FREE + : public LinkedObject +#endif { public: LinkedDataBufferPtr() @@ -231,10 +259,19 @@ public: { size = o.size_; } + if ((size_t)size > o.size_) + { + size = o.size_; + } + if (!size) + { + // Nothing to copy, this will be an empty buffer. + return; + } skip_ = o.skip_; size_ = size; // Takes references, keeping the tail and tail size. - unsigned tail_size; + unsigned tail_size = 0; head_ = o.head_->ref_all(o.skip_ + size, &tail_, &tail_size); HASSERT(tail_size > 0); free_ = -tail_size; @@ -278,7 +315,7 @@ public: reset(buf); return; } - HASSERT(free_ >= 0); + HASSERT(free_ >= 0); // appendable HASSERT(tail_); // Note: if free_ was > 0, there were some unused bytes in the tail // buffer. However, as part of the append operation, we lose these @@ -297,9 +334,16 @@ public: { if (head_) { - head_->unref_all(size_ + skip_); + auto *h = head_; + size_t len = size_ + skip_; + clear(); + h->unref_all(len); + return; + } + else + { + clear(); } - clear(); } /// @return the pointer where data can be appended into the tail of this @@ -324,33 +368,64 @@ public: size_ += len; } + /// Retrieves a pointer where data can be read out of the buffer. + /// @param len will be filled in with the number of available bytes to read + /// at this point. + /// @return the read pointer, or nullptr if there is no data in this + /// buffer. + const uint8_t* data_read_pointer(size_t* len) + { + if (!head_ || !size_) + { + *len = 0; + return nullptr; + } + unsigned avail = 0; + uint8_t *p; + head_->get_read_pointer(skip_, &p, &avail); + if (avail > size_) + { + avail = size_; + } + *len = avail; + return p; + } + /// Advances the head pointer. Typically used after a successful read /// happened. /// @param len how many bytes to advance the read pointer. void data_read_advance(size_t len) { HASSERT(len <= size()); - while (len > 0) + skip_ += len; + size_ -= len; + while (head_ && skip_ >= head_->size()) { - uint8_t *p; - unsigned available; - DataBuffer *next_head = - head_->get_read_pointer(skip_, &p, &available); - if ((len > available) || (len == available && len < size_)) + if (head_ == tail_) { - head_->unref(); - head_ = next_head; - skip_ = 0; - size_ -= available; - len -= available; + if (free() > 0) + { + // We can still write into this buffer, do not unref it. + break; + } + else + { + // We're ending up with an empty linkedbuffer. + auto *b = head_; + clear(); + b->unref(); + return; + } } - else + skip_ -= head_->size(); + auto *b = head_; + auto *next_head = head_->next(); + head_ = next_head; + if (!head_) { - skip_ += len; - size_ -= len; - len = 0; - break; + tail_ = nullptr; } + b->unref(); } } @@ -456,37 +531,70 @@ public: /// this. This tries to do `*this += o`. It will succeed if o.head() == /// this->tail() and the bytes in these buffers are back to back. /// @param o a LinkedDataBuffer with data payload. + /// @param add_link creates a tail-to-head link if none exist yet between + /// *this and o.head_. This is fundamentally dangerous, do it only if there + /// is no shared ownership of this->tail_. /// @return true if append succeeded. If false, nothing was changed. - bool try_append_from(const LinkedDataBufferPtr &o) + bool try_append_from(const LinkedDataBufferPtr &o, bool add_link = false) { if (!o.size()) { return true; // zero bytes, nothing to do. } + if (!size_) { + // We are empty, so anything can be appended. + reset(o); + return true; + } if (free_ >= 0) { // writeable buffer, cannot append. return false; } HASSERT(o.head()); - if (o.head() != tail_) + if (o.head() != tail_) // Buffer does not start in the same chain where + // we end. { - // Buffer does not start in the same chain where we end. - return false; + HASSERT(tail_); // else we went into the !size_ branch above + + // Checks if the end of the tail buffer is already reached. This + // means that we don't depend on the value of free_ anymore for + // correctness. We also check that o starts at the beginning of the + // head buffer. + if (tail_->size() == (size_t)-free_ && o.skip() == 0) { + if (tail_->next() == o.head()) { + // link already exists + } else if (add_link && tail_->next() == nullptr) { + tail_->set_next(o.head()); + } else { + return false; + } + } + else + { + return false; + } } - if (-free_ != (int)o.skip()) + else if (-free_ != (int)o.skip()) { // Not back-to-back. return false; } // Now we're good, so take over the extra buffers. - tail_ = o.tail_; - free_ = o.free_; - size_ += o.size_; // Acquire extra references o.head_->ref_all(o.skip() + o.size()); - // Release duplicate reference between the two chains. - o.head_->unref(); + if (tail_ == o.head()) { + HASSERT(o.head_->references() > 1); + // Release duplicate reference between the two chains. + o.head_->unref(); + } + tail_ = o.tail_; + if (o.free_ < 0) { + free_ = o.free_; + } else { + free_ = -tail_->size(); + } + size_ += o.size_; return true; } @@ -516,6 +624,20 @@ private: int16_t free_ {0}; }; +#ifdef DEBUG_DATA_BUFFER_FREE +void check_db_ownership(DataBuffer* b) { + AtomicHolder h(LinkedDataBufferPtr::head_mu()); + for (LinkedDataBufferPtr* l = LinkedDataBufferPtr::link_head(); l; l = l->link_next()) { + ssize_t total = l->skip() + l->size(); + for (DataBuffer* curr = l->head(); total > 0;) { + HASSERT(curr != b); + total -= curr->size(); + curr = curr->next(); + } + } +} +#endif + /// Proxy Pool that can allocate DataBuffer objects of a certain size. All /// memory comes from the mainBufferPool. class DataBufferPool : public Pool diff --git a/src/utils/macros.h b/src/utils/macros.h index f06cbf763..20a4eee23 100644 --- a/src/utils/macros.h +++ b/src/utils/macros.h @@ -86,10 +86,9 @@ extern const char* g_death_file; */ #define HASSERT(x) do { if (!(x)) { RECORD_DEATH(); abort(); } } while(0) - #define DIE(MSG) abort() -#elif defined(ESP32) +#elif defined(ESP32) || defined(ESP_PLATFORM) #include #include diff --git a/targets/cov/ble/Makefile b/targets/cov/ble/Makefile new file mode 100644 index 000000000..5e5bf973e --- /dev/null +++ b/targets/cov/ble/Makefile @@ -0,0 +1 @@ +include $(OPENMRNPATH)/etc/lib.mk diff --git a/targets/test/ble/Makefile b/targets/test/ble/Makefile new file mode 100644 index 000000000..5e5bf973e --- /dev/null +++ b/targets/test/ble/Makefile @@ -0,0 +1 @@ +include $(OPENMRNPATH)/etc/lib.mk