diff --git a/src/NimBLEAttValue.cpp b/src/NimBLEAttValue.cpp index be769595..86055f30 100644 --- a/src/NimBLEAttValue.cpp +++ b/src/NimBLEAttValue.cpp @@ -18,16 +18,49 @@ #include "NimBLEAttValue.h" #if CONFIG_BT_NIMBLE_ENABLED -# ifdef USING_NIMBLE_ARDUINO_HEADERS -# include "nimble/nimble/include/nimble/nimble_npl.h" -# else -# include "nimble/nimble_npl.h" -# endif - # include "NimBLEUtils.h" # include "NimBLELog.h" +# if defined(__ARM_ARCH_6M__) +// Cortex-M0/M0+ has no LDREX/STREX, so GCC emits libcalls for 4 byte atomic read-modify-write +// operations that are not provided by libgcc. Implement them by briefly masking interrupts. +extern "C" { +unsigned int __atomic_fetch_add_4(volatile void* ptr, unsigned int val, int) { + volatile unsigned int* p = static_cast(ptr); + unsigned int primask; + __asm volatile("mrs %0, primask\n\tcpsid i" : "=r"(primask)::"memory"); + unsigned int old = *p; + *p = old + val; + __asm volatile("msr primask, %0" ::"r"(primask) : "memory"); + return old; +} + +unsigned int __atomic_fetch_sub_4(volatile void* ptr, unsigned int val, int) { + volatile unsigned int* p = static_cast(ptr); + unsigned int primask; + __asm volatile("mrs %0, primask\n\tcpsid i" : "=r"(primask)::"memory"); + unsigned int old = *p; + *p = old - val; + __asm volatile("msr primask, %0" ::"r"(primask) : "memory"); + return old; +} + +unsigned char __atomic_exchange_1(volatile void* ptr, unsigned char val, int) { + volatile unsigned char* p = static_cast(ptr); + unsigned int primask; + __asm volatile("mrs %0, primask\n\tcpsid i" : "=r"(primask)::"memory"); + unsigned char old = *p; + *p = val; + __asm volatile("msr primask, %0" ::"r"(primask) : "memory"); + return old; +} +} +# endif + static const char* LOG_TAG = "NimBLEAttValue"; +ble_npl_mutex NimBLEAttValue::Lock::m_mutex{}; +std::atomic NimBLEAttValue::Lock::m_initialized{false}; +std::atomic NimBLEAttValue::Lock::m_activeCount{0}; // Default constructor implementation. NimBLEAttValue::NimBLEAttValue(uint16_t init_len, uint16_t max_len) @@ -64,6 +97,7 @@ NimBLEAttValue::~NimBLEAttValue() { // Move assignment operator implementation. NimBLEAttValue& NimBLEAttValue::operator=(NimBLEAttValue&& source) { if (this != &source) { + Lock lock; free(m_attr_value); m_attr_value = source.m_attr_value; m_attr_max_len = source.m_attr_max_len; @@ -85,26 +119,27 @@ NimBLEAttValue& NimBLEAttValue::operator=(const NimBLEAttValue& source) { } // Copy all the data from the source object to this object, including allocated space. -void NimBLEAttValue::deepCopy(const NimBLEAttValue& source) { +bool NimBLEAttValue::deepCopy(const NimBLEAttValue& source) { + Lock lock; uint8_t* res = static_cast(realloc(m_attr_value, source.m_capacity + 1)); NIMBLE_CPP_DEBUG_ASSERT(res); if (res == nullptr) { NIMBLE_LOGE(LOG_TAG, "Failed to realloc deepCopy"); - return; + return false; } - ble_npl_hw_enter_critical(); m_attr_value = res; m_attr_max_len = source.m_attr_max_len; m_attr_len = source.m_attr_len; m_capacity = source.m_capacity; setTimeStamp(source.getTimeStamp()); memcpy(m_attr_value, source.m_attr_value, m_attr_len + 1); - ble_npl_hw_exit_critical(0); + return true; } // Set the value of the attribute. bool NimBLEAttValue::setValue(const uint8_t* value, uint16_t len) { + Lock lock; m_attr_len = 0; // Just set the value length to 0 and append instead of repeating code. m_attr_value[0] = '\0'; // Set the first byte to 0 incase the len of the new value is 0. append(value, len); @@ -113,6 +148,7 @@ bool NimBLEAttValue::setValue(const uint8_t* value, uint16_t len) { // Append the new data, allocate as necessary. NimBLEAttValue& NimBLEAttValue::append(const uint8_t* value, uint16_t len) { + Lock lock; if (len == 0) { return *this; } @@ -144,18 +180,17 @@ NimBLEAttValue& NimBLEAttValue::append(const uint8_t* value, uint16_t len) { time_t t = 0; # endif - ble_npl_hw_enter_critical(); memcpy(res + m_attr_len, value, len); m_attr_value = res; m_attr_len = new_len; m_attr_value[m_attr_len] = '\0'; setTimeStamp(t); - ble_npl_hw_exit_critical(0); return *this; } uint8_t NimBLEAttValue::operator[](int pos) const { + Lock lock; NIMBLE_CPP_DEBUG_ASSERT(pos < m_attr_len); if (pos >= m_attr_len) { NIMBLE_LOGE(LOG_TAG, "pos >= len, pos=%u, len=%u", pos, m_attr_len); diff --git a/src/NimBLEAttValue.h b/src/NimBLEAttValue.h index 9ef92d52..a3571b53 100644 --- a/src/NimBLEAttValue.h +++ b/src/NimBLEAttValue.h @@ -21,6 +21,12 @@ #include "syscfg/syscfg.h" #if CONFIG_BT_NIMBLE_ENABLED +# ifdef USING_NIMBLE_ARDUINO_HEADERS +# include "nimble/nimble/include/nimble/nimble_npl.h" +# else +# include "nimble/nimble_npl.h" +# endif + /* Enables the use of Arduino String class for attribute values */ # ifndef NIMBLE_CPP_ARDUINO_STRING_AVAILABLE # define NIMBLE_CPP_ARDUINO_STRING_AVAILABLE (__has_include()) @@ -35,6 +41,7 @@ # include # include # include +# include # ifndef MYNEWT_VAL_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED # ifndef CONFIG_NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED @@ -100,9 +107,69 @@ class NimBLEAttValue { # if MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED) time_t m_timestamp{}; # endif - void deepCopy(const NimBLEAttValue& source); + bool deepCopy(const NimBLEAttValue& source); public: + class Lock { + static ble_npl_mutex m_mutex; + static std::atomic m_initialized; + static std::atomic m_activeCount; + bool m_locked{false}; + + public: + Lock() { + // Increment active count to signal that a constructor is running + m_activeCount.fetch_add(1); + + if (m_initialized.load()) { + m_locked = (ble_npl_mutex_pend(&m_mutex, BLE_NPL_TIME_FOREVER) == BLE_NPL_OK); + + // Post-lock check: If deinit flipped the flag while we were waiting in line, + // we must abort, give it back, and act un-locked. + if (!m_initialized.load() && m_locked) { + ble_npl_mutex_release(&m_mutex); + m_locked = false; + } + } + + // Decrement active count to signal that the constructor has finished processing + m_activeCount.fetch_sub(1); + } + + ~Lock() { + if (m_locked) { + ble_npl_mutex_release(&m_mutex); + } + } + + static bool init() { + if (!m_initialized.load()) { + if (ble_npl_mutex_init(&m_mutex) == BLE_NPL_OK) { + m_initialized.store(true); + } + } + return m_initialized.load(); + } + + static void deinit() { + if (m_initialized.load()) { + // Prevent any new instances from acquiring the lock while we deinitialize + m_initialized.store(false); + + // Wait for any active users to finish + while (m_activeCount.load() > 0) { + ble_npl_time_delay(1); + } + + // Acquire the mutex to ensure no other threads are holding it before deinitializing. + if (ble_npl_mutex_pend(&m_mutex, BLE_NPL_TIME_FOREVER) == BLE_NPL_OK) { + // Deinitialize the mutex now that we have exclusive ownership and no active users. + ble_npl_mutex_deinit(&m_mutex); + } + } + } + }; + /** * @brief Default constructor. * @param[in] init_len The initial size in bytes. @@ -181,16 +248,22 @@ class NimBLEAttValue { /** @brief Returns the current size of the value in bytes */ uint16_t size() const { return m_attr_len; } - /** @brief Returns a pointer to the internal buffer of the value */ + /** + * @brief Returns a pointer to the internal buffer of the value + * @note The pointer is not protected by the lock and can be invalidated by a concurrent update. + * Only use the pointer getters (`data()`, `c_str()`, `begin()`, `end()`) on a copy of the value. + */ const uint8_t* data() const { return m_attr_value; } - /** @brief Returns a pointer to the internal buffer of the value as a const char* */ + /** @brief Returns a pointer to the internal buffer of the value as a const char* + * @note See data() for thread safety restrictions. + */ const char* c_str() const { return reinterpret_cast(m_attr_value); } - /** @brief Iterator begin */ + /** @brief Iterator begin; see data() for thread safety restrictions. */ const uint8_t* begin() const { return m_attr_value; } - /** @brief Iterator end */ + /** @brief Iterator end; see data() for thread safety restrictions. */ const uint8_t* end() const { return m_attr_value + m_attr_len; } # if MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED) @@ -250,6 +323,13 @@ class NimBLEAttValue { */ NimBLEAttValue& append(const uint8_t* value, uint16_t len); + /** + * @brief Copy the value from another NimBLEAttValue instance. + * @param[in] source The NimBLEAttValue instance to copy from. + * @returns True if the copy was successful. + */ + bool copyFrom(const NimBLEAttValue& source) { return deepCopy(source); } + /*********************** Template Functions ************************/ # if __cplusplus < 201703L @@ -262,9 +342,7 @@ class NimBLEAttValue { # ifdef _DOXYGEN_ bool # else - typename std::enable_if::value && - std::is_same::type, char>::value, - bool>::type + typename std::enable_if::value && std::is_same::type, char>::value, bool>::type # endif setValue(const T& s) { return setValue(reinterpret_cast(s), strnlen(s, sizeof(T))); @@ -281,8 +359,7 @@ class NimBLEAttValue { bool # else typename std::enable_if::value && !Has_c_str_length::value && !Has_data_size::value && - !(std::is_array::value && - std::is_same::type, char>::value), + !(std::is_array::value && std::is_same::type, char>::value), bool>::type # endif setValue(const T& v) { @@ -354,8 +431,7 @@ class NimBLEAttValue { } } else if constexpr (Has_c_str_length::value) { return setValue(reinterpret_cast(s.c_str()), s.length()); - } else if constexpr (std::is_array::value && - std::is_same::type, char>::value) { + } else if constexpr (std::is_array::value && std::is_same::type, char>::value) { return setValue(reinterpret_cast(s), strnlen(s, sizeof(s))); } else { return setValue(reinterpret_cast(&s), sizeof(s)); @@ -375,6 +451,7 @@ class NimBLEAttValue { */ template T getValue(time_t* timestamp = nullptr, bool skipSizeCheck = false) const { + Lock lock; if (timestamp != nullptr) { # if MYNEWT_VAL(NIMBLE_CPP_ATT_VALUE_TIMESTAMP_ENABLED) *timestamp = m_timestamp; @@ -395,16 +472,30 @@ class NimBLEAttValue { uint8_t operator[](int pos) const; /** @brief Operator; Get the value as a std::vector. */ - operator std::vector() const { return std::vector(m_attr_value, m_attr_value + m_attr_len); } + operator std::vector() const { + Lock lock; + return std::vector(m_attr_value, m_attr_value + m_attr_len); + } /** @brief Operator; Get the value as a std::string. */ - operator std::string() const { return std::string(reinterpret_cast(m_attr_value), m_attr_len); } + operator std::string() const { + Lock lock; + return std::string(reinterpret_cast(m_attr_value), m_attr_len); + } /** @brief Operator; Get the value as a const uint8_t*. */ operator const uint8_t*() const { return m_attr_value; } /** @brief Operator; Append another NimBLEAttValue. */ - NimBLEAttValue& operator+=(const NimBLEAttValue& source) { return append(source.data(), source.size()); } + NimBLEAttValue& operator+=(const NimBLEAttValue& source) { + Lock lock; // Recursive; keeps the source data and size consistent while appending + if (&source == this) { + // append() may reallocate the buffer, so append from a copy of our own data + std::vector copy(m_attr_value, m_attr_value + m_attr_len); + return append(copy.data(), copy.size()); + } + return append(source.data(), source.size()); + } /** @brief Operator; Set the value from a std::string source. */ NimBLEAttValue& operator=(const std::string& source) { @@ -420,6 +511,7 @@ class NimBLEAttValue { /** @brief Equality operator */ bool operator==(const NimBLEAttValue& source) const { + Lock lock; return (m_attr_len == source.size()) ? memcmp(m_attr_value, source.data(), m_attr_len) == 0 : false; } @@ -428,7 +520,10 @@ class NimBLEAttValue { # if NIMBLE_CPP_ARDUINO_STRING_AVAILABLE /** @brief Operator; Get the value as an Arduino String value. */ - operator String() const { return String(reinterpret_cast(m_attr_value)); } + operator String() const { + Lock lock; + return String(reinterpret_cast(m_attr_value)); + } # endif }; diff --git a/src/NimBLEClient.cpp b/src/NimBLEClient.cpp index 2d219a73..470dbf2c 100644 --- a/src/NimBLEClient.cpp +++ b/src/NimBLEClient.cpp @@ -1179,34 +1179,32 @@ int NimBLEClient::handleGapEvent(struct ble_gap_event* event, void* arg) { return BLE_ATT_ERR_INVALID_HANDLE; } - auto len = event->notify_rx.om->om_len; - if (pChr->m_value.setValue(event->notify_rx.om->om_data, len)) { - os_mbuf* next; - next = SLIST_NEXT(event->notify_rx.om, om_next); - while (next != NULL) { - pChr->m_value.append(next->om_data, next->om_len); - if (pChr->m_value.length() != len + next->om_len) { - rc = BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; - break; - } - len += next->om_len; - next = SLIST_NEXT(next, om_next); - } - } else { - rc = BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + // Build the value locally so the update of the characteristic value is atomic + // and the callback gets a stable copy without holding the value lock. + const auto totalLen = OS_MBUF_PKTLEN(event->notify_rx.om); + if (totalLen > BLE_ATT_ATTR_MAX_LEN) { + NIMBLE_LOGE(LOG_TAG, "notification value error; exceeds limit"); + return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; } - if (rc != 0) { // This should never happen - NIMBLE_LOGE(LOG_TAG, "notification value error; exceeds limit"); - return rc; + NimBLEAttValue value(totalLen, BLE_ATT_ATTR_MAX_LEN); + if (value.data() == nullptr) { + NIMBLE_LOGE(LOG_TAG, "Failed to allocate notification value"); + return BLE_ATT_ERR_INSUFFICIENT_RES; + } + + for (os_mbuf* segment = event->notify_rx.om; segment != nullptr; segment = SLIST_NEXT(segment, om_next)) { + value.append(segment->om_data, segment->om_len); + } + + if (!pChr->m_value.copyFrom(value)) { + NIMBLE_LOGE(LOG_TAG, "Failed to copy value to characteristic"); + return BLE_ATT_ERR_INSUFFICIENT_RES; } if (pChr->m_notifyCallback != nullptr) { // TODO: change this callback to use the NimBLEAttValue class instead of raw data and length - pChr->m_notifyCallback(pChr, - const_cast(pChr->m_value.getValue().data()), - pChr->m_value.length(), - !event->notify_rx.indication); + pChr->m_notifyCallback(pChr, const_cast(value.data()), value.length(), !event->notify_rx.indication); } return 0; diff --git a/src/NimBLEDevice.cpp b/src/NimBLEDevice.cpp index e125aeb9..15b9e5ec 100644 --- a/src/NimBLEDevice.cpp +++ b/src/NimBLEDevice.cpp @@ -94,9 +94,9 @@ NimBLEAdvertising* NimBLEDevice::m_bleAdvertising = nullptr; std::array NimBLEDevice::m_pClients{}; # endif -bool NimBLEDevice::m_initialized{false}; +std::atomic NimBLEDevice::m_initialized{false}; uint32_t NimBLEDevice::m_passkey{123456}; -bool NimBLEDevice::m_synced{false}; +std::atomic NimBLEDevice::m_synced{false}; ble_gap_event_listener NimBLEDevice::m_listener{}; std::vector NimBLEDevice::m_whiteList{}; uint8_t NimBLEDevice::m_ownAddrType{BLE_OWN_ADDR_PUBLIC}; @@ -257,7 +257,7 @@ NimBLEScan* NimBLEDevice::getScan() { * @details Must only be called before calling NimBLEDevice::init. */ void NimBLEDevice::setScanDuplicateCacheSize(uint16_t size) { - if (m_initialized) { + if (isInitialized()) { NIMBLE_LOGE(LOG_TAG, "Cannot change scan cache size while initialized"); return; } else { @@ -286,7 +286,7 @@ void NimBLEDevice::setScanDuplicateCacheSize(uint16_t size) { * @details Must only be called before calling NimBLEDevice::init. */ void NimBLEDevice::setScanFilterMode(uint8_t mode) { - if (m_initialized) { + if (isInitialized()) { NIMBLE_LOGE(LOG_TAG, "Cannot change scan duplicate type while initialized"); return; } else if (mode > 2) { @@ -304,7 +304,7 @@ void NimBLEDevice::setScanFilterMode(uint8_t mode) { * even if already seen in the current scan. If set to 0 the cache will never be reset. */ void NimBLEDevice::setScanDuplicateCacheResetTime(uint16_t time) { - if (m_initialized) { + if (isInitialized()) { NIMBLE_LOGE(LOG_TAG, "Cannot change scan cache reset time while initialized"); return; } else if (time > 1000) { @@ -820,11 +820,11 @@ bool NimBLEDevice::setDefaultPhy(uint8_t txPhyMask, uint8_t rxPhyMask) { * @param [in] reason The reason code for the reset. */ void NimBLEDevice::onReset(int reason) { - if (!m_synced) { + if (!m_synced.load(std::memory_order_acquire)) { return; } - m_synced = false; + m_synced.store(false, std::memory_order_release); NIMBLE_LOGE(LOG_TAG, "Host reset; reason=%d, %s", reason, NimBLEUtils::returnCodeToString(reason)); } // onReset @@ -833,10 +833,10 @@ void NimBLEDevice::onReset(int reason) { * @brief Host synced with controller, all clear to make calls to the stack. */ void NimBLEDevice::onSync(void) { - NIMBLE_LOGI(LOG_TAG, "NimBle host synced."); + NIMBLE_LOGI(LOG_TAG, "NimBLE host synced."); // This check is needed due to potentially being called multiple times in succession // If this happens, the call to scan start may get stuck or cause an advertising fault. - if (m_synced) { + if (m_synced.load(std::memory_order_acquire)) { return; } @@ -861,9 +861,9 @@ void NimBLEDevice::onSync(void) { // Occasionally triggers exception without. ble_npl_time_delay(1); - m_synced = true; + m_synced.store(true, std::memory_order_release); - if (m_initialized) { + if (isInitialized()) { # if MYNEWT_VAL(BLE_ROLE_OBSERVER) if (m_pScan != nullptr) { m_pScan->onHostSync(); @@ -892,7 +892,7 @@ void NimBLEDevice::host_task(void* param) { * @param [in] deviceName The device name of the device. */ bool NimBLEDevice::init(const std::string& deviceName) { - if (!m_initialized) { + if (!isInitialized()) { NIMBLE_LOGD(LOG_TAG, "Starting %s", getVersion()); # ifdef ESP_PLATFORM @@ -1004,18 +1004,24 @@ bool NimBLEDevice::init(const std::string& deviceName) { ble_hs_cfg.sm_their_key_dist |= BLE_SM_PAIR_KEY_DIST_ID; # endif + if (!NimBLEAttValue::Lock::init()) { + NIMBLE_LOGE(LOG_TAG, "Failed to initialize value lock"); + nimble_port_deinit(); + return false; + } + setDeviceName(deviceName); ble_store_config_init(); nimble_port_freertos_init(NimBLEDevice::host_task); } // Wait for host and controller to sync before returning and accepting new tasks - while (!m_synced) { + while (!m_synced.load(std::memory_order_acquire)) { ble_npl_time_delay(1); } - m_initialized = true; // Set the initialization flag to ensure we are only initialized once. - NIMBLE_LOGD(LOG_TAG, "Initialized"); + m_initialized.store(true); // Set the initialization flag to ensure we are only initialized once. + NIMBLE_LOGD(LOG_TAG, "Initialized BLE stack"); return true; } // init @@ -1028,14 +1034,18 @@ bool NimBLEDevice::init(const std::string& deviceName) { */ bool NimBLEDevice::deinit(bool clearAll) { int rc = 0; - if (m_initialized) { + if (isInitialized()) { rc = nimble_port_stop(); if (rc == 0) { + m_initialized.store(false); + m_synced.store(false, std::memory_order_release); # if MYNEWT_VAL(BLE_ROLE_OBSERVER) if (NimBLEDevice::m_pScan != nullptr) { NimBLEDevice::m_pScan->onHostDeinit(); } # endif + NimBLEAttValue::Lock::deinit(); + NimBLEUtils::deinit(); nimble_port_deinit(); # ifndef USING_NIMBLE_ARDUINO_HEADERS # if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) @@ -1045,8 +1055,6 @@ bool NimBLEDevice::deinit(bool clearAll) { } # endif # endif - m_initialized = false; - m_synced = false; } else { NIMBLE_LOGE(LOG_TAG, "nimble_port_stop() failed with error: %d", rc); return false; @@ -1086,8 +1094,6 @@ bool NimBLEDevice::deinit(bool clearAll) { deleteClient(clt); } # endif - - NimBLEUtils::deleteTaskSems(); } return rc == 0; @@ -1098,7 +1104,7 @@ bool NimBLEDevice::deinit(bool clearAll) { * @return true if initialized. */ bool NimBLEDevice::isInitialized() { - return m_initialized; + return m_initialized.load(); } // getInitialized /* -------------------------------------------------------------------------- */ diff --git a/src/NimBLEDevice.h b/src/NimBLEDevice.h index fb64c83d..fc247bf7 100644 --- a/src/NimBLEDevice.h +++ b/src/NimBLEDevice.h @@ -42,6 +42,7 @@ # include # include +# include # if MYNEWT_VAL(BLE_ROLE_CENTRAL) # include @@ -214,8 +215,8 @@ class NimBLEDevice { # endif private: - static bool m_synced; - static bool m_initialized; + static std::atomic m_synced; + static std::atomic m_initialized; static uint32_t m_passkey; static ble_gap_event_listener m_listener; static uint8_t m_ownAddrType; diff --git a/src/NimBLELocalValueAttribute.h b/src/NimBLELocalValueAttribute.h index 5fd37e55..0c4f3909 100644 --- a/src/NimBLELocalValueAttribute.h +++ b/src/NimBLELocalValueAttribute.h @@ -125,8 +125,8 @@ class NimBLELocalValueAttribute : public NimBLELocalAttribute, public NimBLEValu virtual void writeEvent(const uint8_t* val, uint16_t len, NimBLEConnInfo& connInfo) = 0; /** - * @brief Get a pointer to value of the attribute. - * @return A pointer to the value of the attribute. + * @brief Get a reference to value of the attribute. + * @return A reference to the value of the attribute. * @details This function is used by NimBLEServer when handling read/write requests. */ const NimBLEAttValue& getAttVal() const { return m_value; } diff --git a/src/NimBLEServer.cpp b/src/NimBLEServer.cpp index 660a0313..3129d113 100644 --- a/src/NimBLEServer.cpp +++ b/src/NimBLEServer.cpp @@ -760,9 +760,8 @@ int NimBLEServer::handleGattEvent(uint16_t connHandle, uint16_t attrHandle, ble_ pAtt->readEvent(peerInfo); } - ble_npl_hw_enter_critical(); + NimBLEAttValue::Lock lock; int rc = os_mbuf_append(ctxt->om, val.data(), val.size()); - ble_npl_hw_exit_critical(0); return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; } diff --git a/src/NimBLEUtils.cpp b/src/NimBLEUtils.cpp index 02f5b0a9..683a05a6 100644 --- a/src/NimBLEUtils.cpp +++ b/src/NimBLEUtils.cpp @@ -18,7 +18,7 @@ #include "NimBLEUtils.h" #if CONFIG_BT_NIMBLE_ENABLED -# include "NimBLEAddress.h" +# include "NimBLEDevice.h" # include "NimBLELog.h" # ifdef USING_NIMBLE_ARDUINO_HEADERS @@ -62,6 +62,8 @@ static const char* LOG_TAG = "NimBLEUtils"; std::vector NimBLEUtils::m_taskSemEntries{}; +std::atomic NimBLEUtils::m_semUserCount{0}; +std::atomic_flag NimBLEUtils::m_taskSemLock = ATOMIC_FLAG_INIT; /** * @brief Construct a NimBLEUtils::TaskData instance. @@ -79,6 +81,15 @@ NimBLEUtils::TaskData::TaskData(void* pInstance, int flags, void* buf) * @return True if the task completed, false if the timeout was reached. */ bool NimBLEUtils::taskWait(const TaskData& taskData, uint32_t timeout) { + m_semUserCount.fetch_add(1); + + if (!NimBLEDevice::isInitialized()) { + NIMBLE_LOGE(LOG_TAG, "NimBLE stack is not initialized"); + taskData.m_flags = BLE_HS_EOS; + m_semUserCount.fetch_sub(1); + return false; + } + ble_npl_time_t ticks; if (timeout == BLE_NPL_TIME_FOREVER) { ticks = BLE_NPL_TIME_FOREVER; @@ -86,11 +97,16 @@ bool NimBLEUtils::taskWait(const TaskData& taskData, uint32_t timeout) { ble_npl_time_ms_to_ticks(timeout, &ticks); } + taskData.m_pSem = nullptr; + // find a semaphore that is not currently in use, or create a new one - for (auto& entry : m_taskSemEntries) { - if (!entry->inUse) { - taskData.m_pSem = entry; - break; + { + SpinGuard guard(m_taskSemLock); + for (auto& entry : m_taskSemEntries) { + if (!entry->inUse.exchange(true, std::memory_order_acq_rel)) { + taskData.m_pSem = entry; + break; + } } } @@ -99,20 +115,31 @@ bool NimBLEUtils::taskWait(const TaskData& taskData, uint32_t timeout) { if (ble_npl_sem_init(sem, 0) != BLE_NPL_OK) { NIMBLE_LOGE(LOG_TAG, "Failed to initialize semaphore for taskWait"); delete sem; + m_semUserCount.fetch_sub(1); return false; } TaskSemEntry* entry = new TaskSemEntry; entry->sem = sem; entry->inUse = true; - m_taskSemEntries.push_back(entry); + { + SpinGuard guard(m_taskSemLock); + m_taskSemEntries.push_back(entry); + } taskData.m_pSem = entry; - NIMBLE_LOGD(LOG_TAG, "Created new semaphore for taskWait, total semaphores: %d\n", (int)m_taskSemEntries.size()); } + taskData.m_pSem->inUse.store(true); + m_semUserCount.fetch_sub(1); NIMBLE_LOGD(LOG_TAG, "Task waiting with timeout %" PRIu32 "ms", timeout); - taskData.m_pSem->inUse = true; - return ble_npl_sem_pend(taskData.m_pSem->sem, ticks) == BLE_NPL_OK; + if (ble_npl_sem_pend(taskData.m_pSem->sem, ticks) == BLE_NPL_OK) { + taskData.m_pSem->inUse.store(false); + return true; + } + + NIMBLE_LOGE(LOG_TAG, "Task wait timed out after %" PRIu32 "ms", timeout); + taskData.m_pSem->inUse.store(false); + return false; } // taskWait /** @@ -123,11 +150,9 @@ bool NimBLEUtils::taskWait(const TaskData& taskData, uint32_t timeout) { void NimBLEUtils::taskRelease(const TaskData& taskData, int flags) { taskData.m_flags = flags; if (taskData.m_pSem == nullptr) { - NIMBLE_LOGE(LOG_TAG, "taskRelease called with null semaphore"); return; } - taskData.m_pSem->inUse = false; auto rc = ble_npl_sem_release(taskData.m_pSem->sem); if (rc != BLE_NPL_OK) { NIMBLE_LOGE(LOG_TAG, "Failed to release semaphore: rc=%d %s", rc, returnCodeToString(rc)); @@ -136,13 +161,24 @@ void NimBLEUtils::taskRelease(const TaskData& taskData, int flags) { } // taskRelease /** - * @brief Deletes all semaphores used for task waiting. + * @brief Deinitializes the NimBLEUtils class and deletes all semaphores used for task waiting. * @details This should be called when the NimBLE stack is deinitialized to clean up any resources used by waiting tasks. */ -void NimBLEUtils::deleteTaskSems() { - for (auto& entry : m_taskSemEntries) { - ble_npl_sem_release(entry->sem); - ble_npl_time_delay(10); // give time for any pending tasks to be released +void NimBLEUtils::deinit() { + // wait for any active users of the semaphore vector to finish + while (m_semUserCount.load() > 0) { + ble_npl_time_delay(1); + } + + for (auto entry : m_taskSemEntries) { + if (entry->inUse.load()) { + ble_npl_sem_release(entry->sem); + // wait for the pend to complete before deinitializing the semaphore + while (entry->inUse.load()) { + ble_npl_time_delay(1); + } + } + ble_npl_sem_deinit(entry->sem); delete entry->sem; delete entry; @@ -599,4 +635,24 @@ NimBLEAddress NimBLEUtils::generateAddr(bool nrpa) { return NimBLEAddress{addr}; } // generateAddr +/** + * @brief Construct a SpinGuard and acquire the lock. + * @param [in] flag The atomic_flag to guard. + * @details Spins until the lock is acquired. + */ +NimBLEUtils::SpinGuard::SpinGuard(std::atomic_flag& flag) : m_flag(flag) { + while (m_flag.test_and_set(std::memory_order_acquire)) { + // spin until the lock is acquired + ble_npl_time_delay(1); + } +} + +/** + * @brief Destruct a SpinGuard and release the lock. + * @details Releases the lock acquired by the constructor. + */ +NimBLEUtils::SpinGuard::~SpinGuard() { + m_flag.clear(std::memory_order_release); +} + #endif // CONFIG_BT_NIMBLE_ENABLED diff --git a/src/NimBLEUtils.h b/src/NimBLEUtils.h index fafb02c1..0bf780c2 100644 --- a/src/NimBLEUtils.h +++ b/src/NimBLEUtils.h @@ -42,6 +42,7 @@ void nimble_cpp_assert(const char* file, unsigned line) __attribute((weak, noret # include # include +# include class NimBLEAddress; struct ble_npl_sem; @@ -51,11 +52,24 @@ struct ble_npl_sem; */ class NimBLEUtils { struct TaskSemEntry { - ble_npl_sem* sem; - bool inUse{false}; + ble_npl_sem* sem; + std::atomic inUse{false}; }; public: + /** + * @brief A simple spinlock guard for atomic_flag. + * @details Acquires the lock on construction and releases it on destruction. + */ + class SpinGuard { + public: + explicit SpinGuard(std::atomic_flag& flag); + ~SpinGuard(); + + private: + std::atomic_flag& m_flag; + }; + /** * @brief A structure to hold data for a task that is waiting for a response. * @details This structure is used in conjunction with NimBLEUtils::taskWait() and NimBLEUtils::taskRelease(). @@ -83,8 +97,10 @@ class NimBLEUtils { private: friend class NimBLEDevice; - static void deleteTaskSems(); + static void deinit(); static std::vector m_taskSemEntries; + static std::atomic m_semUserCount; + static std::atomic_flag m_taskSemLock; }; #endif // CONFIG_BT_NIMBLE_ENABLED