From 5b845272ed5357a3ef4ca0d118a21e995d69b047 Mon Sep 17 00:00:00 2001
From: me-no-dev <hristo@espressif.com>
Date: Tue, 16 Mar 2021 02:11:59 +0200
Subject: [PATCH] Fix BluetoothSerial TX Stall

Fixes: https://github.com/espressif/arduino-esp32/issues/4949
---
 .../BluetoothSerial/src/BluetoothSerial.cpp   | 36 ++++++++++++++-----
 1 file changed, 27 insertions(+), 9 deletions(-)

diff --git a/libraries/BluetoothSerial/src/BluetoothSerial.cpp b/libraries/BluetoothSerial/src/BluetoothSerial.cpp
index adc3c331..4cbf548b 100755
--- a/libraries/BluetoothSerial/src/BluetoothSerial.cpp
+++ b/libraries/BluetoothSerial/src/BluetoothSerial.cpp
@@ -42,6 +42,10 @@ const char * _spp_server_name = "ESP32SPP";
 
 #define RX_QUEUE_SIZE 512
 #define TX_QUEUE_SIZE 32
+#define SPP_TX_QUEUE_TIMEOUT 1000
+#define SPP_TX_DONE_TIMEOUT 1000
+#define SPP_CONGESTED_TIMEOUT 1000
+
 static uint32_t _spp_client = 0;
 static xQueueHandle _spp_rx_queue = NULL;
 static xQueueHandle _spp_tx_queue = NULL;
@@ -143,7 +147,7 @@ static esp_err_t _spp_queue_packet(uint8_t *data, size_t len){
     }
     packet->len = len;
     memcpy(packet->data, data, len);
-    if (xQueueSend(_spp_tx_queue, &packet, portMAX_DELAY) != pdPASS) {
+    if (!_spp_tx_queue || xQueueSend(_spp_tx_queue, &packet, SPP_TX_QUEUE_TIMEOUT) != pdPASS) {
         log_e("SPP TX Queue Send Failed!");
         free(packet);
         return ESP_FAIL;
@@ -156,19 +160,25 @@ static uint8_t _spp_tx_buffer[SPP_TX_MAX];
 static uint16_t _spp_tx_buffer_len = 0;
 
 static bool _spp_send_buffer(){
-    if((xEventGroupWaitBits(_spp_event_group, SPP_CONGESTED, pdFALSE, pdTRUE, portMAX_DELAY) & SPP_CONGESTED) != 0){
+    if((xEventGroupWaitBits(_spp_event_group, SPP_CONGESTED, pdFALSE, pdTRUE, SPP_CONGESTED_TIMEOUT) & SPP_CONGESTED) != 0){
+        if(!_spp_client){
+            log_v("SPP Client Gone!");
+            return false;
+        }
+        log_v("SPP Write %u", _spp_tx_buffer_len);
         esp_err_t err = esp_spp_write(_spp_client, _spp_tx_buffer_len, _spp_tx_buffer);
         if(err != ESP_OK){
             log_e("SPP Write Failed! [0x%X]", err);
             return false;
         }
         _spp_tx_buffer_len = 0;
-        if(xSemaphoreTake(_spp_tx_done, portMAX_DELAY) != pdTRUE){
+        if(xSemaphoreTake(_spp_tx_done, SPP_TX_DONE_TIMEOUT) != pdTRUE){
             log_e("SPP Ack Failed!");
             return false;
         }
         return true;
     }
+    log_e("SPP Write Congested!");
     return false;
 }
 
@@ -194,13 +204,18 @@ static void _spp_tx_task(void * arg){
                 _spp_tx_buffer_len = SPP_TX_MAX;
                 data += to_send;
                 len -= to_send;
-                _spp_send_buffer();
+                if(!_spp_send_buffer()){
+                    len = 0;
+                }
                 while(len >= SPP_TX_MAX){
                     memcpy(_spp_tx_buffer, data, SPP_TX_MAX);
                     _spp_tx_buffer_len = SPP_TX_MAX;
                     data += SPP_TX_MAX;
                     len -= SPP_TX_MAX;
-                    _spp_send_buffer();
+                    if(!_spp_send_buffer()){
+                        len = 0;
+                        break;
+                    }
                 }
                 if(len){
                     memcpy(_spp_tx_buffer, data, len);
@@ -236,9 +251,10 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
 
     case ESP_SPP_SRV_OPEN_EVT://Server connection open
         if (param->srv_open.status == ESP_SPP_SUCCESS) {
-            log_i("ESP_SPP_SRV_OPEN_EVT");
+            log_i("ESP_SPP_SRV_OPEN_EVT: %u", _spp_client);
             if (!_spp_client){
                 _spp_client = param->srv_open.handle;
+                _spp_tx_buffer_len = 0;
             } else {
                 secondConnectionAttempt = true;
                 esp_spp_disconnect(param->srv_open.handle);
@@ -252,12 +268,13 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
 
     case ESP_SPP_CLOSE_EVT://Client connection closed
         if ((param->close.async == false && param->close.status == ESP_SPP_SUCCESS) || param->close.async) {
-            log_i("ESP_SPP_CLOSE_EVT");
+            log_i("ESP_SPP_CLOSE_EVT: %u", secondConnectionAttempt);
             if(secondConnectionAttempt) {
                 secondConnectionAttempt = false;
             } else {
                 _spp_client = 0;
                 xEventGroupSetBits(_spp_event_group, SPP_DISCONNECTED);
+                xEventGroupSetBits(_spp_event_group, SPP_CONGESTED);
             }        
             xEventGroupClearBits(_spp_event_group, SPP_CONNECTED);
         } else {
@@ -279,11 +296,11 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
             if(param->write.cong){
                 xEventGroupClearBits(_spp_event_group, SPP_CONGESTED);
             }
-            xSemaphoreGive(_spp_tx_done);//we can try to send another packet
             log_v("ESP_SPP_WRITE_EVT: %u %s", param->write.len, param->write.cong?"CONGESTED":"");
         } else {
             log_e("ESP_SPP_WRITE_EVT failed!, status:%d", param->write.status);
         }
+        xSemaphoreGive(_spp_tx_done);//we can try to send another packet
         break;
 
     case ESP_SPP_DATA_IND_EVT://connection received data
@@ -323,6 +340,7 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
         }
         xEventGroupClearBits(_spp_event_group, SPP_DISCONNECTED);
         xEventGroupSetBits(_spp_event_group, SPP_CONNECTED);
+        xEventGroupSetBits(_spp_event_group, SPP_CONGESTED);
         break;
 
     case ESP_SPP_START_EVT://server started
@@ -693,7 +711,7 @@ void BluetoothSerial::flush()
 {
     if (_spp_tx_queue != NULL){
         while(uxQueueMessagesWaiting(_spp_tx_queue) > 0){
-	    delay(5);
+           delay(100);
         }
     }
 }