diff --git a/components/BLEManager/BLEConfig.h b/components/BLEManager/BLEConfig.h index 33e7320..a7fed78 100644 --- a/components/BLEManager/BLEConfig.h +++ b/components/BLEManager/BLEConfig.h @@ -70,6 +70,8 @@ class BLEConfig static const uint32_t BLE_OUTBOUND_MSGS_IN_FLIGHT_TIMEOUT_MS = 500; static const uint32_t DEFAULT_OUTBOUND_QUEUE_RESERVE_FOR_NON_PUBLISH = 10; static const uint32_t DEFAULT_PUBLISH_QUEUE_SIZE = 30; + // About a second of link time with notifications + static const uint32_t DEFAULT_PUBLISH_QUEUE_MAX_BYTES = 32768; static const bool DEFAULT_PUBLISH_USE_INDICATE = false; static const uint32_t DEFAULT_CONN_INTERVAL_BLE_UNITS = 12; // 15ms static const uint32_t DEFAULT_CONN_LATENCY = 0; @@ -148,6 +150,7 @@ class BLEConfig outMsgsInFlightTimeoutMs = config.getLong("outMsgsInFlightMs", BLE_OUTBOUND_MSGS_IN_FLIGHT_TIMEOUT_MS); outQReserveForNonPublish = config.getLong("outQResvNonPub", DEFAULT_OUTBOUND_QUEUE_RESERVE_FOR_NON_PUBLISH); publishQueueSize = config.getLong("pubQSize", DEFAULT_PUBLISH_QUEUE_SIZE); + publishQueueMaxBytes = config.getLong("pubQMaxBytes", DEFAULT_PUBLISH_QUEUE_MAX_BYTES); publishUseIndication = config.getBool("pubUseInd", DEFAULT_PUBLISH_USE_INDICATE); // Task settings @@ -290,6 +293,7 @@ class BLEConfig uint32_t outMsgsInFlightTimeoutMs = BLE_OUTBOUND_MSGS_IN_FLIGHT_TIMEOUT_MS; uint16_t outQReserveForNonPublish = DEFAULT_OUTBOUND_QUEUE_RESERVE_FOR_NON_PUBLISH; uint16_t publishQueueSize = DEFAULT_PUBLISH_QUEUE_SIZE; + uint32_t publishQueueMaxBytes = DEFAULT_PUBLISH_QUEUE_MAX_BYTES; bool publishUseIndication:1 = DEFAULT_PUBLISH_USE_INDICATE; // Task settings diff --git a/components/BLEManager/BLEGattOutbound.cpp b/components/BLEManager/BLEGattOutbound.cpp index 4c3677d..f4bcbe8 100644 --- a/components/BLEManager/BLEGattOutbound.cpp +++ b/components/BLEManager/BLEGattOutbound.cpp @@ -56,6 +56,7 @@ bool BLEGattOutbound::setup(const BLEConfig& bleConfig) // Setup queues _commandQueue.setMaxLen(bleConfig.outboundQueueSize); _publishQueue.setMaxLen(bleConfig.publishQueueSize); + _publishQueueMaxBytes = bleConfig.publishQueueMaxBytes; // Check if a thread should be started for sending if (bleConfig.useTaskForSending) @@ -149,24 +150,37 @@ bool BLEGattOutbound::isReadyToSend(uint32_t channelID, CommsMsgTypeCode msgType { if (msgType == MSG_TYPE_PUBLISH) { - bool canSend = _publishQueue.count() < _publishQueue.maxLen(); + bool canSend = (_publishQueue.count() < _publishQueue.maxLen()) && + (_publishQueueBytes.load() < _publishQueueMaxBytes); if (!canSend) { + // A publisher asks again (each loop) while the queue is full, so a drop is counted once per spell of + // the queue being full, and the warning is limited to one a second (with the number of refusals) // A drop within PUBLISH_DROP_CONNECT_GRACE_MS of connection is the harmless startup burst // (link still ramping up, backlog flushing). A later drop is operational - the publish rate // is exceeding the BLE drain rate - and is the case worth flagging. uint32_t connStartMs = _connStartMs.load(); bool operationalDrop = (connStartMs != 0) && Raft::isTimeout(millis(), connStartMs, PUBLISH_DROP_CONNECT_GRACE_MS); - _bleStats.txPublishDropped(operationalDrop); + if (!_publishQueueFull) + _bleStats.txPublishDropped(operationalDrop); + _publishQueueFull = true; + _publishRefusedSinceWarn++; #ifdef WARN_ON_PUBLISH_QUEUE_FULL - if (operationalDrop) + if (operationalDrop && Raft::isTimeout(millis(), _publishFullLastWarnMs, PUBLISH_FULL_WARN_INTERVAL_MS)) { - LOG_W(MODULE_PREFIX, "isReadyToSend PUBLISH DROPPED (operational) qCount %d maxLen %d", - _publishQueue.count(), _publishQueue.maxLen()); + LOG_W(MODULE_PREFIX, "isReadyToSend PUBLISH QUEUE FULL (operational) qCount %d maxLen %d qBytes %d maxBytes %d refused %d", + _publishQueue.count(), _publishQueue.maxLen(), + (int)_publishQueueBytes.load(), (int)_publishQueueMaxBytes, (int)_publishRefusedSinceWarn); + _publishFullLastWarnMs = millis(); + _publishRefusedSinceWarn = 0; } #endif } + else + { + _publishQueueFull = false; + } return canSend; } @@ -200,25 +214,36 @@ bool BLEGattOutbound::sendMsg(CommsChannelMsg& msg) #endif #endif + // The message's buffer is taken if it may be (e.g. a message just encoded for sending), and copied otherwise, + // then moved into the queue + uint32_t msgLen = msg.getBufLen(); + ProtocolRawMsg bleOutMsg; + if (msg.isBufTakeable()) + bleOutMsg.takeBuffer(msg.getCmdVector()); + else + bleOutMsg.setFromBuffer(msg.getBuf(), msg.getBufLen()); + // Route to appropriate queue based on message type - ProtocolRawMsg bleOutMsg(msg.getBuf(), msg.getBufLen()); bool putOk = false; if (msg.getMsgTypeCode() == MSG_TYPE_PUBLISH) { - putOk = _publishQueue.put(bleOutMsg, OUTBOUND_QUEUE_MAX_MS_TO_WAIT); + // Counted before it is queued, as the sending side may take it off the queue at once + _publishQueueBytes += msgLen; + putOk = _publishQueue.put(std::move(bleOutMsg), OUTBOUND_QUEUE_MAX_MS_TO_WAIT); if (!putOk) { + _publishQueueBytes -= msgLen; LOG_W(MODULE_PREFIX, "sendBLEMsg PUBLISH FAILEDTOSEND totalLen %d qCount %d", - msg.getBufLen(), _publishQueue.count()); + (int)msgLen, _publishQueue.count()); } } else { - putOk = _commandQueue.put(bleOutMsg, OUTBOUND_QUEUE_MAX_MS_TO_WAIT); + putOk = _commandQueue.put(std::move(bleOutMsg), OUTBOUND_QUEUE_MAX_MS_TO_WAIT); if (!putOk) { LOG_W(MODULE_PREFIX, "sendBLEMsg CMD FAILEDTOSEND totalLen %d qCount %d", - msg.getBufLen(), _commandQueue.count()); + (int)msgLen, _commandQueue.count()); } } @@ -285,7 +310,7 @@ int32_t BLEGattOutbound::decrementMsgsInFlight() ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool BLEGattOutbound::handleSendFromQueue(ThreadSafeQueue& queue, - uint16_t& msgPos, bool useIndication, const char* queueName) + uint32_t& msgPos, bool useIndication, const char* queueName) { // If this queue uses indication, check if an indication is already in flight if (useIndication && isIndicationInFlight()) @@ -298,21 +323,22 @@ bool BLEGattOutbound::handleSendFromQueue(ThreadSafeQueue& queue return false; } - // Peek next message in queue - ProtocolRawMsg bleOutMsg; - if (!queue.peek(bleOutMsg, OUTBOUND_QUEUE_MAX_MS_TO_WAIT)) - return false; - - // Extract next section of message to send - uint32_t toSendLen = 0; - bool removeFromQueue = true; - if (bleOutMsg.getBufLen() > msgPos) - toSendLen = bleOutMsg.getBufLen() - msgPos; + // Copy the next section of the message at the front of the queue (copying the whole message for each + // section sent took many ms per section for a large message, and held the queue locked meanwhile) uint32_t maxLen = getMaxSendLen(); - if (toSendLen > maxLen) - toSendLen = maxLen; - if (bleOutMsg.getBufLen() > msgPos + toSendLen) - removeFromQueue = false; + uint32_t msgLen = 0; + uint32_t toSendLen = 0; + bool msgFound = queue.peekWith([&](const ProtocolRawMsg& bleOutMsg) { + msgLen = bleOutMsg.getBufLen(); + if (msgLen > msgPos) + toSendLen = msgLen - msgPos; + if (toSendLen > maxLen) + toSendLen = maxLen; + _sendChunk.assign(bleOutMsg.getBuf() + msgPos, bleOutMsg.getBuf() + msgPos + toSendLen); + }, OUTBOUND_QUEUE_MAX_MS_TO_WAIT); + if (!msgFound) + return false; + bool removeFromQueue = msgLen <= msgPos + toSendLen; BLEGattServerSendResult rslt = BLEGATT_SERVER_SEND_RESULT_TRY_AGAIN; if (toSendLen != 0) @@ -329,7 +355,7 @@ bool BLEGattOutbound::handleSendFromQueue(ThreadSafeQueue& queue _lastNotifySendMs = millis(); // Send to central - rslt = _gattServer.sendToCentral(bleOutMsg.getBuf() + msgPos, toSendLen, useIndication); + rslt = _gattServer.sendToCentral(_sendChunk.data(), toSendLen, useIndication); if (rslt == BLEGATT_SERVER_SEND_RESULT_OK) { _bleStats.txMsg(toSendLen, true); @@ -365,12 +391,14 @@ bool BLEGattOutbound::handleSendFromQueue(ThreadSafeQueue& queue if (queue.pop(OUTBOUND_QUEUE_MAX_MS_TO_WAIT)) { msgPos = 0; + if (&queue == &_publishQueue) + _publishQueueBytes -= msgLen; } else { // Failed to remove (queue mutex not obtained) so set the position to the end of the message to // ensure that no part of it is sent again - removal will be attempted again on the next call - msgPos = bleOutMsg.getBufLen() > UINT16_MAX ? UINT16_MAX : bleOutMsg.getBufLen(); + msgPos = msgLen; } } @@ -379,12 +407,12 @@ bool BLEGattOutbound::handleSendFromQueue(ThreadSafeQueue& queue { int32_t msgsInFlight = _outboundMsgsInFlight.load(); LOG_I(MODULE_PREFIX, "handleSendFromQ %s sendLen %d totalLen %d msgPos %d sendOk %d inFlight %d leftInQ %d removeFromQ %d", - queueName, toSendLen, bleOutMsg.getBufLen(), msgPos, rslt, msgsInFlight, queue.count(), removeFromQueue); + queueName, toSendLen, msgLen, msgPos, rslt, msgsInFlight, queue.count(), removeFromQueue); } else { LOG_I(MODULE_PREFIX, "handleSendFromQ %s sendLen %d totalLen %d msgPos %d sendOk %s leftInQ %d removeFromQ %d", - queueName, toSendLen, bleOutMsg.getBufLen(), msgPos, + queueName, toSendLen, msgLen, msgPos, rslt == BLEGATT_SERVER_SEND_RESULT_OK ? "OK" : rslt == BLEGATT_SERVER_SEND_RESULT_FAIL ? "FAIL" : "TRYAGAIN", queue.count(), removeFromQueue); } diff --git a/components/BLEManager/BLEGattOutbound.h b/components/BLEManager/BLEGattOutbound.h index 800b5dd..faed43e 100644 --- a/components/BLEManager/BLEGattOutbound.h +++ b/components/BLEManager/BLEGattOutbound.h @@ -70,18 +70,34 @@ class BLEGattOutbound // Command queue: typically uses indication (ACK'd, reliable) ThreadSafeQueue _commandQueue; std::atomic _commandUseIndication{true}; - uint16_t _commandMsgPos = 0; + uint32_t _commandMsgPos = 0; // Publish queue: typically uses notification (faster, no ACK wait) ThreadSafeQueue _publishQueue; std::atomic _publishUseIndication{false}; - uint16_t _publishMsgPos = 0; + uint32_t _publishMsgPos = 0; + + // Bytes in the publish queue, and the most there may be before more publishes are refused (one message may + // take it over the limit, so a message larger than the limit is still sent when the queue is empty). A limit + // on the count alone lets large messages (e.g. camera frames) pile up for many seconds of link time + std::atomic _publishQueueBytes{0}; + uint32_t _publishQueueMaxBytes = BLEConfig::DEFAULT_PUBLISH_QUEUE_MAX_BYTES; + + // The part of a message being sent, copied out of the queue (so the queue isn't locked during the send) + std::vector _sendChunk; // Time the current connection was established (0 if not connected) - used to distinguish // publish-queue drops during the post-connection burst from drops during normal operation std::atomic _connStartMs{0}; static const uint32_t PUBLISH_DROP_CONNECT_GRACE_MS = 5000; + // Publish queue full: whether it was full when last asked, publishes refused since the last warning, and the + // time of that warning (warnings are limited as a publisher asks again each loop while the queue is full) + bool _publishQueueFull = false; + uint32_t _publishRefusedSinceWarn = 0; + uint32_t _publishFullLastWarnMs = 0; + static const uint32_t PUBLISH_FULL_WARN_INTERVAL_MS = 1000; + // Max time to wait for the mutex on the outbound queues static const uint32_t OUTBOUND_QUEUE_MAX_MS_TO_WAIT = 10; @@ -117,7 +133,7 @@ class BLEGattOutbound // Outbound queue handling void serviceOutboundQueue(); - bool handleSendFromQueue(ThreadSafeQueue& queue, uint16_t& msgPos, + bool handleSendFromQueue(ThreadSafeQueue& queue, uint32_t& msgPos, bool useIndication, const char* queueName); void outboundMsgTask(); diff --git a/components/BLEManager/BLEGattServer.cpp b/components/BLEManager/BLEGattServer.cpp index d71073b..48c31da 100644 --- a/components/BLEManager/BLEGattServer.cpp +++ b/components/BLEManager/BLEGattServer.cpp @@ -16,6 +16,7 @@ #include "RaftArduino.h" #include "NamedValueProvider.h" #include "BLEGattServer.h" +#include "BLEManStats.h" #include "services/gap/ble_svc_gap.h" #include "services/gatt/ble_svc_gatt.h" @@ -71,6 +72,7 @@ ble_uuid128_t BLEGattServer::DEFAULT_MESSAGE_RESPONSE_UUID = ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// BLEGattServer::BLEGattServer(BLEGattServerAccessCBType callback, BLEManStats& bleStats) : + _bleStats(bleStats), _bleOutbound(*this, bleStats) { _accessCallback = callback; @@ -364,6 +366,15 @@ BLEGattServerSendResult BLEGattServer::sendToCentral(const uint8_t* pBuf, uint32 // Form buffer to send struct os_mbuf *om = ble_hs_mbuf_from_flat(pBuf, bufLen); + + // No buffer (NimBLE's pool is used up, e.g. by notifications sent faster than the link takes them): send + // it later. It mustn't be passed on: NimBLE takes a null buffer to mean "send the characteristic's value", + // which is empty here, and reports success - so the packet would be lost from the message + if (!om) + { + _bleStats.txNoBuf(); + return BLEGATT_SERVER_SEND_RESULT_TRY_AGAIN; + } #ifdef DEBUG_RESP_CHARACTERISTIC LOG_I(MODULE_PREFIX, "sendToCentral sending bufLen %d om %p", bufLen, om); #endif diff --git a/components/BLEManager/BLEGattServer.h b/components/BLEManager/BLEGattServer.h index 55251b1..b507e09 100644 --- a/components/BLEManager/BLEGattServer.h +++ b/components/BLEManager/BLEGattServer.h @@ -176,6 +176,9 @@ class BLEGattServer uint32_t _lastBLEErrorMsgMs = 0; uint32_t _lastBLEErrorMsgCode = 0; + // Stats + BLEManStats& _bleStats; + // Outbound handler BLEGattOutbound _bleOutbound; diff --git a/components/BLEManager/BLEManStats.h b/components/BLEManager/BLEManStats.h index c882e1e..e448f4d 100644 --- a/components/BLEManager/BLEManStats.h +++ b/components/BLEManager/BLEManStats.h @@ -35,6 +35,7 @@ class BLEManStats _rxTestFrameBytes = 0; _txPublishDropCount = 0; _txPublishDropOpCount = 0; + _txNoBufCount = 0; _rxRate.clear(); _txRate.clear(); _txErrRate.clear(); @@ -63,8 +64,9 @@ class BLEManStats _txErrRate.sample(_txErrCount); } - // A publish was dropped because the outbound publish queue was full. operational is true if the drop - // occurred during normal operation (not during the post-connection burst) - i.e. the case that matters + // Publishes were refused because the outbound publish queue was full (counted once each time it fills, as a + // publisher asks again while it is full). operational is true if this occurred during normal operation (not + // during the post-connection burst) - i.e. the case that matters void txPublishDropped(bool operational) { _txPublishDropCount++; @@ -72,6 +74,12 @@ class BLEManStats _txPublishDropOpCount++; } + // NimBLE had no buffer for a packet (it is sent again later) + void txNoBuf() + { + _txNoBufCount++; + } + void rxTestFrame(uint32_t msgSize, bool seqOK, bool dataOK) { _rxTestFrameCount++; @@ -114,6 +122,11 @@ class BLEManStats (int)_txPublishDropCount.load(), (int)_txPublishDropOpCount.load()); json += buf; } + if (_txNoBufCount.load() > 0) + { + snprintf(buf, sizeof(buf), R"(,"txNoBuf":%d)", (int)_txNoBufCount.load()); + json += buf; + } if (_rxTestFrameCount.load() > 0) { snprintf(buf, sizeof(buf), R"("tM":%d,"tB":%d,"tBPS":%.1f,"tSeqErrs":%d,"tDatErrs":%d)", @@ -156,6 +169,7 @@ class BLEManStats std::atomic _txErrCount{0}; std::atomic _txPublishDropCount{0}; std::atomic _txPublishDropOpCount{0}; + std::atomic _txNoBufCount{0}; #define MOVING_AVERAGE_WINDOW_SIZE 5 MovingRate _rxRate; MovingRate _txRate;