From d5fdcfdab8fa487687654cf07ccc370a66ae9834 Mon Sep 17 00:00:00 2001
From: Antti Kauppila <antti.kauppila@arm.com>
Date: Wed, 6 Nov 2019 09:47:01 +0200
Subject: [PATCH] busy s... fix (ONME-4352)

oob handling added for OK in busy scenario

Fixed spellcheck error

trace added for ok
---
 .../wifi/esp8266-driver/ESP8266/ESP8266.cpp   | 22 ++++++++++++++++++-
 .../wifi/esp8266-driver/ESP8266/ESP8266.h     |  2 ++
 platform/ATCmdParser.h                        |  6 +++++
 platform/source/ATCmdParser.cpp               | 19 ++++++++++++++++
 4 files changed, 48 insertions(+), 1 deletion(-)

diff --git a/components/wifi/esp8266-driver/ESP8266/ESP8266.cpp b/components/wifi/esp8266-driver/ESP8266/ESP8266.cpp
index 0427cdcefc..b7a8f2168c 100644
--- a/components/wifi/esp8266-driver/ESP8266/ESP8266.cpp
+++ b/components/wifi/esp8266-driver/ESP8266/ESP8266.cpp
@@ -626,6 +626,7 @@ nsapi_error_t ESP8266::send(int id, const void *data, uint32_t amount)
     }
 
     _smutex.lock();
+RETRY:
     set_timeout(ESP8266_SEND_TIMEOUT);
     _busy = false;
     _error = false;
@@ -634,11 +635,25 @@ nsapi_error_t ESP8266::send(int id, const void *data, uint32_t amount)
         goto END;
     }
 
+    //We might receive "busy s/p..." and "OK" from modem, so we need to check that also
+    _ok_received = false;
+    _parser.oob("OK", callback(this, &ESP8266::_oob_ok_received));
+
     if (!_parser.recv(">")) {
+        _parser.remove_oob("OK");
+        if (_busy) {
+            if (_ok_received) {
+                goto RETRY;
+            } else if (_parser.recv("OK")) {
+                goto RETRY;
+            }
+        }
         tr_debug("send(): Didn't get \">\"");
         ret = NSAPI_ERROR_WOULD_BLOCK;
         goto END;
     }
+    _ok_received = false;
+    _parser.remove_oob("OK");
 
     if (_parser.write((char *)data, (int)amount) >= 0 && _parser.recv("SEND OK")) {
         ret = NSAPI_ERROR_OK;
@@ -1071,7 +1086,6 @@ void ESP8266::_oob_busy()
                    "ESP8266::_oob_busy() AT timeout\n");
     }
     _busy = true;
-    _parser.abort();
 }
 
 void ESP8266::_oob_tcp_data_hdlr()
@@ -1205,6 +1219,12 @@ void ESP8266::_oob_connection_status()
     _conn_stat_cb();
 }
 
+void ESP8266::_oob_ok_received()
+{
+    tr_debug("_oob_ok_received called");
+    _ok_received = true;
+}
+
 int8_t ESP8266::default_wifi_mode()
 {
     int8_t mode;
diff --git a/components/wifi/esp8266-driver/ESP8266/ESP8266.h b/components/wifi/esp8266-driver/ESP8266/ESP8266.h
index a1c476f80e..7b18e10a0c 100644
--- a/components/wifi/esp8266-driver/ESP8266/ESP8266.h
+++ b/components/wifi/esp8266-driver/ESP8266/ESP8266.h
@@ -469,6 +469,7 @@ private:
     void _oob_tcp_data_hdlr();
     void _oob_ready();
     void _oob_scan_results();
+    void _oob_ok_received();
 
     // OOB state variables
     int _connect_error;
@@ -479,6 +480,7 @@ private:
     bool _error;
     bool _busy;
     bool _reset_done;
+    bool _ok_received;
 
     // Modem's address info
     char _ip_buffer[16];
diff --git a/platform/ATCmdParser.h b/platform/ATCmdParser.h
index 4e6ff3e6a0..48d82ac276 100644
--- a/platform/ATCmdParser.h
+++ b/platform/ATCmdParser.h
@@ -311,6 +311,12 @@ public:
      */
     void oob(const char *prefix, mbed::Callback<void()> func);
 
+    /**
+     * @brief remove_oob Removes oob callback handler
+     * @param prefix Prefix to identify oob to be removed.
+     */
+    void remove_oob(const char *prefix);
+
     /**
      * Flushes the underlying stream
      */
diff --git a/platform/source/ATCmdParser.cpp b/platform/source/ATCmdParser.cpp
index 724f5b9ade..90b66583d8 100644
--- a/platform/source/ATCmdParser.cpp
+++ b/platform/source/ATCmdParser.cpp
@@ -362,6 +362,25 @@ void ATCmdParser::oob(const char *prefix, Callback<void()> cb)
     _oobs = oob;
 }
 
+void ATCmdParser::remove_oob(const char *prefix)
+{
+    struct oob *prev = NULL;
+    struct oob *oob = _oobs;
+    while (oob) {
+        if (memcmp(oob->prefix, prefix, strlen(prefix)) == 0) {
+            if (prev) {
+                prev->next = oob->next;
+            } else {
+                _oobs = oob->next;
+            }
+            delete oob;
+            return;
+        }
+        prev = oob;
+        oob = oob->next;
+    }
+}
+
 void ATCmdParser::abort()
 {
     _aborted = true;