diff --git a/examples/L2CAP/L2CAP_Client/main/idf_component.yml b/examples/L2CAP/L2CAP_Client/main/idf_component.yml index 66f47cad..383b7f31 100644 --- a/examples/L2CAP/L2CAP_Client/main/idf_component.yml +++ b/examples/L2CAP/L2CAP_Client/main/idf_component.yml @@ -1,3 +1,6 @@ dependencies: local/esp-nimble-cpp: path: ../../../../../esp-nimble-cpp/ + mickeyl/esp-hpl: + git: https://github.com/mickeyl/esp-hpl.git + version: "1.1.0" diff --git a/examples/L2CAP/L2CAP_Client/main/main.cpp b/examples/L2CAP/L2CAP_Client/main/main.cpp index e4f21913..574ab411 100644 --- a/examples/L2CAP/L2CAP_Client/main/main.cpp +++ b/examples/L2CAP/L2CAP_Client/main/main.cpp @@ -1,15 +1,12 @@ #include +#include +#include -// See the following for generating UUIDs: -// https://www.uuidgenerator.net/ - -// The remote service we wish to connect to. -static BLEUUID serviceUUID("dcbc7255-1e9e-49a0-a360-b0430b6c6905"); -// The characteristic of the remote service we are interested in. -static BLEUUID charUUID("371a55c8-f251-4ad2-90b3-c7c195b049be"); - -#define L2CAP_CHANNEL 150 +#define L2CAP_PSM 192 #define L2CAP_MTU 5000 +#define INITIAL_PAYLOAD_SIZE 64 +#define BLOCKS_BEFORE_DOUBLE 50 +#define MAX_PAYLOAD_SIZE 4900 const BLEAdvertisedDevice* theDevice = NULL; BLEClient* theClient = NULL; @@ -17,6 +14,15 @@ BLEL2CAPChannel* theChannel = NULL; size_t bytesSent = 0; size_t bytesReceived = 0; +size_t currentPayloadSize = INITIAL_PAYLOAD_SIZE; +uint32_t blocksSent = 0; +uint64_t startTime = 0; + +// Heap monitoring +size_t initialHeap = 0; +size_t lastHeap = 0; +size_t heapDecreaseCount = 0; +const size_t HEAP_LEAK_THRESHOLD = 10; // Warn after 10 consecutive decreases class L2CAPChannelCallbacks: public BLEL2CAPChannelCallbacks { @@ -43,7 +49,7 @@ class MyClientCallbacks: public BLEClientCallbacks { printf("GAP connected\n"); pClient->setDataLen(251); - theChannel = BLEL2CAPChannel::connect(pClient, L2CAP_CHANNEL, L2CAP_MTU, new L2CAPChannelCallbacks()); + theChannel = BLEL2CAPChannel::connect(pClient, L2CAP_PSM, L2CAP_MTU, new L2CAPChannelCallbacks()); } void onDisconnect(BLEClient* pClient, int reason) { @@ -61,23 +67,68 @@ class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks { if (theDevice) { return; } printf("BLE Advertised Device found: %s\n", advertisedDevice->toString().c_str()); - if (!advertisedDevice->haveServiceUUID()) { return; } - if (!advertisedDevice->isAdvertisingService(serviceUUID)) { return; } + // Look for device named "l2cap" + if (advertisedDevice->haveName() && advertisedDevice->getName() == "l2cap") { + printf("Found l2cap device!\n"); + BLEDevice::getScan()->stop(); + theDevice = advertisedDevice; + } + } +}; + +void statusTask(void *pvParameters) { + while (true) { + vTaskDelay(1000 / portTICK_PERIOD_MS); - printf("Found the device we're interested in!\n"); - BLEDevice::getScan()->stop(); + if (startTime > 0 && blocksSent > 0) { + uint64_t currentTime = esp_timer_get_time(); + double elapsedSeconds = (currentTime - startTime) / 1000000.0; + double bytesPerSecond = bytesSent / elapsedSeconds; + double kbPerSecond = bytesPerSecond / 1024.0; - // Hand over the device to the other task - theDevice = advertisedDevice; + // Heap monitoring + size_t currentHeap = esp_get_free_heap_size(); + size_t minHeap = esp_get_minimum_free_heap_size(); + + // Track heap for leak detection + if (initialHeap == 0) { + initialHeap = currentHeap; + lastHeap = currentHeap; + } + + // Check for consistent heap decrease + if (currentHeap < lastHeap) { + heapDecreaseCount++; + if (heapDecreaseCount >= HEAP_LEAK_THRESHOLD) { + printf("\n⚠️ WARNING: POSSIBLE MEMORY LEAK DETECTED! ⚠️\n"); + printf("Heap has decreased %zu times in a row\n", heapDecreaseCount); + printf("Initial heap: %zu, Current heap: %zu, Lost: %zu bytes\n", + initialHeap, currentHeap, initialHeap - currentHeap); + } + } else if (currentHeap >= lastHeap) { + heapDecreaseCount = 0; // Reset counter if heap stabilizes or increases + } + lastHeap = currentHeap; + + printf("\n=== STATUS UPDATE ===\n"); + printf("Blocks sent: %lu\n", (unsigned long)blocksSent); + printf("Total bytes sent: %zu\n", bytesSent); + printf("Current payload size: %zu bytes\n", currentPayloadSize); + printf("Elapsed time: %.1f seconds\n", elapsedSeconds); + printf("Bandwidth: %.2f KB/s (%.2f Mbps)\n", kbPerSecond, (bytesPerSecond * 8) / 1000000.0); + printf("Heap: %zu free (min: %zu), Used since start: %zu\n", + currentHeap, minHeap, initialHeap > 0 ? initialHeap - currentHeap : 0); + printf("==================\n\n"); + } } -}; +} void connectTask(void *pvParameters) { uint8_t sequenceNumber = 0; while (true) { - + if (!theDevice) { vTaskDelay(1000 / portTICK_PERIOD_MS); continue; @@ -96,7 +147,7 @@ void connectTask(void *pvParameters) { break; } vTaskDelay(2000 / portTICK_PERIOD_MS); - continue; + continue; } if (!theChannel) { @@ -112,22 +163,58 @@ void connectTask(void *pvParameters) { } while (theChannel->isConnected()) { + // Create framed packet: [seqno 8bit] [16bit payload length] [payload] + std::vector packet; + packet.reserve(3 + currentPayloadSize); - /* - static auto initialDelay = true; - if (initialDelay) { - printf("Waiting gracefully 3 seconds before sending data\n"); - vTaskDelay(3000 / portTICK_PERIOD_MS); - initialDelay = false; - }; -*/ - std::vector data(5000, sequenceNumber++); - if (theChannel->write(data)) { - bytesSent += data.size(); + // Add sequence number (8 bits) + packet.push_back(sequenceNumber); + + // Add payload length (16 bits, big endian - network byte order) + uint16_t payloadLen = currentPayloadSize; + packet.push_back((payloadLen >> 8) & 0xFF); // High byte first + packet.push_back(payloadLen & 0xFF); // Low byte second + + // Add payload + for (size_t i = 0; i < currentPayloadSize; i++) { + packet.push_back(i & 0xFF); + } + + if (theChannel->write(packet)) { + if (startTime == 0) { + startTime = esp_timer_get_time(); + } + bytesSent += packet.size(); + blocksSent++; + + // Print every block since we're sending slowly now + printf("Sent block %lu (seq=%d, payload=%zu bytes, frame_size=%zu)\n", + (unsigned long)blocksSent, sequenceNumber, currentPayloadSize, packet.size()); + + sequenceNumber++; + + // After every 50 blocks, double payload size + if (blocksSent % BLOCKS_BEFORE_DOUBLE == 0) { + size_t newSize = currentPayloadSize * 2; + + // Cap at maximum safe payload size + if (newSize > MAX_PAYLOAD_SIZE) { + if (currentPayloadSize < MAX_PAYLOAD_SIZE) { + currentPayloadSize = MAX_PAYLOAD_SIZE; + printf("\n=== Reached maximum payload size of %zu bytes after %lu blocks ===\n", currentPayloadSize, (unsigned long)blocksSent); + } + // Already at max, don't increase further + } else { + currentPayloadSize = newSize; + printf("\n=== Doubling payload size to %zu bytes after %lu blocks ===\n", currentPayloadSize, (unsigned long)blocksSent); + } + } } else { printf("failed to send!\n"); - abort(); + abort(); } + + // No delay - send as fast as possible } vTaskDelay(1000 / portTICK_PERIOD_MS); @@ -136,9 +223,13 @@ void connectTask(void *pvParameters) { extern "C" void app_main(void) { + // Install high performance logging before any output + esp_hpl::HighPerformanceLogger::init(); + printf("Starting L2CAP client example\n"); xTaskCreate(connectTask, "connectTask", 5000, NULL, 1, NULL); + xTaskCreate(statusTask, "statusTask", 3000, NULL, 1, NULL); BLEDevice::init("L2CAP-Client"); BLEDevice::setMTU(BLE_ATT_MTU_MAX); @@ -151,15 +242,8 @@ void app_main(void) { scan->setActiveScan(true); scan->start(25 * 1000, false); - int numberOfSeconds = 0; - - while (bytesSent == 0) { - vTaskDelay(10 / portTICK_PERIOD_MS); - } - + // Main task just waits while (true) { vTaskDelay(1000 / portTICK_PERIOD_MS); - int bytesSentPerSeconds = bytesSent / ++numberOfSeconds; - printf("Bandwidth: %d b/sec = %d KB/sec\n", bytesSentPerSeconds, bytesSentPerSeconds / 1024); } } diff --git a/examples/L2CAP/L2CAP_Server/main/idf_component.yml b/examples/L2CAP/L2CAP_Server/main/idf_component.yml index 66f47cad..383b7f31 100644 --- a/examples/L2CAP/L2CAP_Server/main/idf_component.yml +++ b/examples/L2CAP/L2CAP_Server/main/idf_component.yml @@ -1,3 +1,6 @@ dependencies: local/esp-nimble-cpp: path: ../../../../../esp-nimble-cpp/ + mickeyl/esp-hpl: + git: https://github.com/mickeyl/esp-hpl.git + version: "1.1.0" diff --git a/examples/L2CAP/L2CAP_Server/main/main.cpp b/examples/L2CAP/L2CAP_Server/main/main.cpp index 47639072..9d161536 100644 --- a/examples/L2CAP/L2CAP_Server/main/main.cpp +++ b/examples/L2CAP/L2CAP_Server/main/main.cpp @@ -1,13 +1,16 @@ #include +#include +#include -// See the following for generating UUIDs: -// https://www.uuidgenerator.net/ - -#define SERVICE_UUID "dcbc7255-1e9e-49a0-a360-b0430b6c6905" -#define CHARACTERISTIC_UUID "371a55c8-f251-4ad2-90b3-c7c195b049be" -#define L2CAP_CHANNEL 150 +#define L2CAP_PSM 192 #define L2CAP_MTU 5000 +// Heap monitoring +size_t initialHeap = 0; +size_t lastHeap = 0; +size_t heapDecreaseCount = 0; +const size_t HEAP_LEAK_THRESHOLD = 10; // Warn after 10 consecutive decreases + class GATTCallbacks: public BLEServerCallbacks { public: @@ -23,68 +26,179 @@ class L2CAPChannelCallbacks: public BLEL2CAPChannelCallbacks { public: bool connected = false; - size_t numberOfReceivedBytes; - uint8_t nextSequenceNumber; + size_t totalBytesReceived = 0; + size_t totalFramesReceived = 0; + size_t totalPayloadBytes = 0; + uint8_t expectedSequenceNumber = 0; + size_t sequenceErrors = 0; + size_t frameErrors = 0; + uint64_t startTime = 0; + std::vector buffer; // Buffer for incomplete frames public: void onConnect(NimBLEL2CAPChannel* channel) { - printf("L2CAP connection established\n"); + printf("L2CAP connection established on PSM %d\n", L2CAP_PSM); connected = true; - numberOfReceivedBytes = nextSequenceNumber = 0; + totalBytesReceived = 0; + totalFramesReceived = 0; + totalPayloadBytes = 0; + expectedSequenceNumber = 0; + sequenceErrors = 0; + frameErrors = 0; + startTime = esp_timer_get_time(); + buffer.clear(); } void onRead(NimBLEL2CAPChannel* channel, std::vector& data) { - numberOfReceivedBytes += data.size(); - size_t sequenceNumber = data[0]; - printf("L2CAP read %d bytes w/ sequence number %d", data.size(), sequenceNumber); - if (sequenceNumber != nextSequenceNumber) { - printf("(wrong sequence number %d, expected %d)\n", sequenceNumber, nextSequenceNumber); - } else { - printf("\n"); - nextSequenceNumber++; + // Append new data to buffer + buffer.insert(buffer.end(), data.begin(), data.end()); + totalBytesReceived += data.size(); + if (startTime == 0) { + startTime = esp_timer_get_time(); // start measuring once data flows + } + + // Process complete frames from buffer + while (buffer.size() >= 3) { // Minimum frame size: seqno(1) + len(2) + // Parse frame header + uint8_t seqno = buffer[0]; + uint16_t payloadLen = (buffer[1] << 8) | buffer[2]; // Big-endian + + size_t frameSize = 3 + payloadLen; + + // Check if we have complete frame + if (buffer.size() < frameSize) { + break; // Wait for more data + } + + // Validate and process frame + totalFramesReceived++; + totalPayloadBytes += payloadLen; + + // Check sequence number + if (seqno != expectedSequenceNumber) { + sequenceErrors++; + printf("Frame %zu: Sequence error - got %d, expected %d (payload=%d bytes)\n", + totalFramesReceived, seqno, expectedSequenceNumber, payloadLen); + } + + // Update expected sequence number (wraps at 256) + expectedSequenceNumber = (seqno + 1) & 0xFF; + + // Remove processed frame from buffer + buffer.erase(buffer.begin(), buffer.begin() + frameSize); + + // Print progress every 100 frames + if (totalFramesReceived % 100 == 0) { + double elapsedSeconds = (esp_timer_get_time() - startTime) / 1000000.0; + double bytesPerSecond = elapsedSeconds > 0 ? totalBytesReceived / elapsedSeconds : 0.0; + printf("Received %zu frames (%zu payload bytes) - Bandwidth: %.2f KB/s (%.2f Mbps)\n", + totalFramesReceived, totalPayloadBytes, + bytesPerSecond / 1024.0, (bytesPerSecond * 8) / 1000000.0); + } } } + void onDisconnect(NimBLEL2CAPChannel* channel) { - printf("L2CAP disconnected\n"); + printf("\nL2CAP disconnected\n"); + double elapsedSeconds = startTime > 0 ? (esp_timer_get_time() - startTime) / 1000000.0 : 0.0; + double bytesPerSecond = elapsedSeconds > 0 ? totalBytesReceived / elapsedSeconds : 0.0; + + printf("Final statistics:\n"); + printf(" Total frames: %zu\n", totalFramesReceived); + printf(" Total bytes: %zu\n", totalBytesReceived); + printf(" Payload bytes: %zu\n", totalPayloadBytes); + printf(" Sequence errors: %zu\n", sequenceErrors); + printf(" Frame errors: %zu\n", frameErrors); + printf(" Bandwidth: %.2f KB/s (%.2f Mbps)\n", bytesPerSecond / 1024.0, (bytesPerSecond * 8) / 1000000.0); + + // Reset state for the next connection + buffer.clear(); + totalBytesReceived = 0; + totalFramesReceived = 0; + totalPayloadBytes = 0; + expectedSequenceNumber = 0; + sequenceErrors = 0; + frameErrors = 0; + startTime = 0; connected = false; + + // Restart advertising so another client can connect + BLEDevice::startAdvertising(); } }; extern "C" void app_main(void) { + // Install high performance logging before any other output + esp_hpl::HighPerformanceLogger::init(); + printf("Starting L2CAP server example [%lu free] [%lu min]\n", esp_get_free_heap_size(), esp_get_minimum_free_heap_size()); - BLEDevice::init("L2CAP-Server"); + BLEDevice::init("l2cap"); // Match the name the client is looking for BLEDevice::setMTU(BLE_ATT_MTU_MAX); auto cocServer = BLEDevice::createL2CAPServer(); auto l2capChannelCallbacks = new L2CAPChannelCallbacks(); - auto channel = cocServer->createService(L2CAP_CHANNEL, L2CAP_MTU, l2capChannelCallbacks); - + auto channel = cocServer->createService(L2CAP_PSM, L2CAP_MTU, l2capChannelCallbacks); + (void)channel; // prevent unused warning + auto server = BLEDevice::createServer(); server->setCallbacks(new GATTCallbacks()); - auto service = server->createService(SERVICE_UUID); - auto characteristic = service->createCharacteristic(CHARACTERISTIC_UUID, NIMBLE_PROPERTY::READ); - characteristic->setValue(L2CAP_CHANNEL); - service->start(); + auto advertising = BLEDevice::getAdvertising(); - advertising->addServiceUUID(SERVICE_UUID); - advertising->enableScanResponse(true); + NimBLEAdvertisementData scanData; + scanData.setName("l2cap"); + advertising->setScanResponseData(scanData); BLEDevice::startAdvertising(); printf("Server waiting for connection requests [%lu free] [%lu min]\n", esp_get_free_heap_size(), esp_get_minimum_free_heap_size()); - // Wait until transfer actually starts... - while (!l2capChannelCallbacks->numberOfReceivedBytes) { - vTaskDelay(10 / portTICK_PERIOD_MS); - } - printf("\n\n\n"); - int numberOfSeconds = 0; - + // Status reporting loop while (true) { vTaskDelay(1000 / portTICK_PERIOD_MS); - if (!l2capChannelCallbacks->connected) { continue; } - int bps = l2capChannelCallbacks->numberOfReceivedBytes / ++numberOfSeconds; - printf("Bandwidth: %d b/sec = %d KB/sec [%lu free] [%lu min]\n", bps, bps / 1024, esp_get_free_heap_size(), esp_get_minimum_free_heap_size()); + + if (l2capChannelCallbacks->connected && l2capChannelCallbacks->totalBytesReceived > 0) { + uint64_t currentTime = esp_timer_get_time(); + double elapsedSeconds = (currentTime - l2capChannelCallbacks->startTime) / 1000000.0; + + if (elapsedSeconds > 0) { + double bytesPerSecond = l2capChannelCallbacks->totalBytesReceived / elapsedSeconds; + double framesPerSecond = l2capChannelCallbacks->totalFramesReceived / elapsedSeconds; + + // Heap monitoring + size_t currentHeap = esp_get_free_heap_size(); + size_t minHeap = esp_get_minimum_free_heap_size(); + + // Track heap for leak detection + if (initialHeap == 0) { + initialHeap = currentHeap; + lastHeap = currentHeap; + } + + // Check for consistent heap decrease + if (currentHeap < lastHeap) { + heapDecreaseCount++; + if (heapDecreaseCount >= HEAP_LEAK_THRESHOLD) { + printf("\n⚠️ WARNING: POSSIBLE MEMORY LEAK DETECTED! ⚠️\n"); + printf("Heap has decreased %zu times in a row\n", heapDecreaseCount); + printf("Initial heap: %zu, Current heap: %zu, Lost: %zu bytes\n", + initialHeap, currentHeap, initialHeap - currentHeap); + } + } else if (currentHeap >= lastHeap) { + heapDecreaseCount = 0; // Reset counter if heap stabilizes or increases + } + lastHeap = currentHeap; + + printf("\n=== STATUS UPDATE ===\n"); + printf("Frames received: %zu (%.1f fps)\n", l2capChannelCallbacks->totalFramesReceived, framesPerSecond); + printf("Total bytes: %zu\n", l2capChannelCallbacks->totalBytesReceived); + printf("Payload bytes: %zu\n", l2capChannelCallbacks->totalPayloadBytes); + printf("Bandwidth: %.2f KB/s (%.2f Mbps)\n", bytesPerSecond / 1024.0, (bytesPerSecond * 8) / 1000000.0); + printf("Sequence errors: %zu\n", l2capChannelCallbacks->sequenceErrors); + printf("Heap: %zu free (min: %zu), Used since start: %zu\n", + currentHeap, minHeap, initialHeap > 0 ? initialHeap - currentHeap : 0); + printf("==================\n"); + } + } } } diff --git a/src/NimBLEL2CAPChannel.cpp b/src/NimBLEL2CAPChannel.cpp index 4a155860..a1631e7e 100644 --- a/src/NimBLEL2CAPChannel.cpp +++ b/src/NimBLEL2CAPChannel.cpp @@ -134,8 +134,14 @@ int NimBLEL2CAPChannel::writeFragment(std::vector::const_iterator begin case BLE_HS_ENOMEM: case BLE_HS_EAGAIN: + /* ble_l2cap_send already consumed and freed txd on these errors */ + NIMBLE_LOGD(LOG_TAG, "ble_l2cap_send returned %d (consumed buffer). Retrying shortly...", res); + ble_npl_time_delay(ble_npl_time_ms_to_ticks32(RetryTimeout)); + continue; + case BLE_HS_EBUSY: - NIMBLE_LOGD(LOG_TAG, "ble_l2cap_send returned %d. Retrying shortly...", res); + /* Channel busy; txd not consumed */ + NIMBLE_LOGD(LOG_TAG, "ble_l2cap_send returned %d (busy). Retrying shortly...", res); os_mbuf_free_chain(txd); ble_npl_time_delay(ble_npl_time_ms_to_ticks32(RetryTimeout)); continue; @@ -197,6 +203,25 @@ bool NimBLEL2CAPChannel::write(const std::vector& bytes) { return true; } +bool NimBLEL2CAPChannel::disconnect() { + if (!this->channel) { + NIMBLE_LOGW(LOG_TAG, "L2CAP Channel not open"); + return false; + } + + int rc = ble_l2cap_disconnect(this->channel); + if (rc != 0 && rc != BLE_HS_ENOTCONN && rc != BLE_HS_EALREADY) { + NIMBLE_LOGE(LOG_TAG, "ble_l2cap_disconnect failed: rc=%d %s", rc, NimBLEUtils::returnCodeToString(rc)); + return false; + } + + return true; +} + +uint16_t NimBLEL2CAPChannel::getConnHandle() const { + return ble_l2cap_get_conn_handle(this->channel); +} + // private int NimBLEL2CAPChannel::handleConnectionEvent(struct ble_l2cap_event* event) { channel = event->connect.chan; diff --git a/src/NimBLEL2CAPChannel.h b/src/NimBLEL2CAPChannel.h index b7fafa4a..b0a8be3e 100644 --- a/src/NimBLEL2CAPChannel.h +++ b/src/NimBLEL2CAPChannel.h @@ -56,6 +56,14 @@ class NimBLEL2CAPChannel { /// NOTE: This function will block until the data has been sent or an error occurred. bool write(const std::vector& bytes); + /// @brief Disconnect this L2CAP channel. + /// @return true on success, false on failure. + bool disconnect(); + + /// @brief Get the connection handle associated with this channel. + /// @return Connection handle, or BLE_HS_CONN_HANDLE_NONE if not connected. + uint16_t getConnHandle() const; + /// @return True, if the channel is connected. False, otherwise. bool isConnected() const { return !!channel; }