From aff381579034006ece5c7df37633f2bca95f162b Mon Sep 17 00:00:00 2001
From: Kimmo Vaisanen <kimmo.vaisanen@arm.com>
Date: Fri, 15 Mar 2019 12:29:46 +0200
Subject: [PATCH] Cellular: Improve ATHandler AT traces

Original AT traces had several issue:
- Can be overwritten by other traces (printf/mbed-trace)
- No way to know which direction message was going (TX or RX)
- <cr> and <ln> characters were not visible in trace
etc.

This commit addresses those issues using mbed-trace and showing separately
each filehandle write and read.
---
 features/cellular/framework/AT/ATHandler.cpp | 56 ++++++++++++--------
 features/cellular/framework/AT/ATHandler.h   |  7 ++-
 2 files changed, 40 insertions(+), 23 deletions(-)

diff --git a/features/cellular/framework/AT/ATHandler.cpp b/features/cellular/framework/AT/ATHandler.cpp
index 8812408db3..e7d366ca38 100644
--- a/features/cellular/framework/AT/ATHandler.cpp
+++ b/features/cellular/framework/AT/ATHandler.cpp
@@ -368,7 +368,7 @@ bool ATHandler::fill_buffer(bool wait_for_timeout)
     // Reset buffer when full
     if (sizeof(_recv_buff) == _recv_len) {
         tr_error("AT overflow");
-        debug_print(_recv_buff, _recv_len);
+        debug_print(_recv_buff, _recv_len, AT_ERR);
         reset_buffer();
     }
 
@@ -379,7 +379,7 @@ bool ATHandler::fill_buffer(bool wait_for_timeout)
     if (count > 0 && (fhs.revents & POLLIN)) {
         ssize_t len = _fileHandle->read(_recv_buff + _recv_len, sizeof(_recv_buff) - _recv_len);
         if (len > 0) {
-            debug_print(_recv_buff + _recv_len, len);
+            debug_print(_recv_buff + _recv_len, len, AT_RX);
             _recv_len += len;
             return true;
         }
@@ -481,7 +481,6 @@ ssize_t ATHandler::read_bytes(uint8_t *buf, size_t len)
         }
         buf[read_len] = c;
         if (_debug_on && read_len >= DEBUG_MAXLEN) {
-            debug_print(DEBUG_END_MARK, sizeof(DEBUG_END_MARK) - 1);
             _debug_on = false;
         }
     }
@@ -586,7 +585,6 @@ ssize_t ATHandler::read_hex_string(char *buf, size_t size)
         int c = get_char();
 
         if (_debug_on && read_idx >= DEBUG_MAXLEN) {
-            debug_print(DEBUG_END_MARK, sizeof(DEBUG_END_MARK) - 1);
             _debug_on = false;
         }
 
@@ -1231,9 +1229,8 @@ size_t ATHandler::write(const void *data, size_t len)
         }
         if (_debug_on && write_len < DEBUG_MAXLEN) {
             if (write_len + ret < DEBUG_MAXLEN) {
-                debug_print((char *)data + write_len, ret);
+                debug_print((char *)data + write_len, ret, AT_TX);
             } else {
-                debug_print(DEBUG_END_MARK, sizeof(DEBUG_END_MARK) - 1);
                 _debug_on = false;
             }
         }
@@ -1287,30 +1284,45 @@ void ATHandler::flush()
     }
 }
 
-void ATHandler::debug_print(const char *p, int len)
+void ATHandler::debug_print(const char *p, int len, ATType type)
 {
 #if MBED_CONF_CELLULAR_DEBUG_AT
     if (_debug_on) {
-#if MBED_CONF_MBED_TRACE_ENABLE
-        mbed_cellular_trace::mutex_wait();
-#endif
-        char c;
-        for (ssize_t i = 0; i < len; i++) {
-            c = *p++;
-            if (!isprint(c)) {
-                if (c == '\r') {
-                    debug("\n");
+        const int buf_size = len * 4 + 1; // x4 -> reserve space for extra characters, +1 -> terminating null
+        char *buffer = (char *)malloc(buf_size);
+        if (buffer) {
+            memset(buffer, 0, buf_size);
+
+            char *pbuf = buffer;
+            for (ssize_t i = 0; i < len; i++) {
+                const char c = *p++;
+                if (isprint(c)) {
+                    *pbuf++ = c;
+                } else if (c == '\r') {
+                    sprintf(pbuf, "<cr>");
+                    pbuf += 4;
                 } else if (c == '\n') {
+                    sprintf(pbuf, "<ln>");
+                    pbuf += 4;
                 } else {
-                    debug("#%02x", c);
+                    sprintf(pbuf, "<%02X>", c);
+                    pbuf += 4;
                 }
-            } else {
-                debug("%c", c);
             }
+            MBED_ASSERT((int)(pbuf - buffer) <= buf_size); // Check for buffer overflow
+
+            if (type == AT_RX) {
+                tr_info("AT RX (%2d): %s", len, buffer);
+            } else if (type == AT_TX) {
+                tr_info("AT TX (%2d): %s", len, buffer);
+            } else {
+                tr_info("AT ERR (%2d): %s", len, buffer);
+            }
+
+            free(buffer);
+        } else {
+            tr_error("AT trace unable to allocate buffer!");
         }
-#if MBED_CONF_MBED_TRACE_ENABLE
-        mbed_cellular_trace::mutex_release();
-#endif
     }
 #endif // MBED_CONF_CELLULAR_DEBUG_AT
 }
diff --git a/features/cellular/framework/AT/ATHandler.h b/features/cellular/framework/AT/ATHandler.h
index 839d511dbc..efd436f967 100644
--- a/features/cellular/framework/AT/ATHandler.h
+++ b/features/cellular/framework/AT/ATHandler.h
@@ -571,7 +571,12 @@ private:
     bool find_urc_handler(const char *prefix);
 
     // print contents of a buffer to trace log
-    void debug_print(const char *p, int len);
+    enum ATType {
+        AT_ERR,
+        AT_RX,
+        AT_TX
+    };
+    void debug_print(const char *p, int len, ATType type);
 };
 
 } // namespace mbed