From 16b6c853fe337b882dec2c867afef149605c520b Mon Sep 17 00:00:00 2001
From: Volodymyr Shymanskyy <vshymanskyi@gmail.com>
Date: Tue, 12 Sep 2017 01:30:40 +0300
Subject: [PATCH] decodeHex16bit support for sendUSSD

---
 TinyGsmClientA6.h     | 29 +++++++++++++++++++++---
 TinyGsmClientSIM800.h | 51 ++++++++++++++++++++++++++++++++++++++-----
 2 files changed, 72 insertions(+), 8 deletions(-)

diff --git a/TinyGsmClientA6.h b/TinyGsmClientA6.h
index 69f04f9..9af5bb6 100644
--- a/TinyGsmClientA6.h
+++ b/TinyGsmClientA6.h
@@ -446,7 +446,9 @@ public:
    */
 
   String sendUSSD(const String& code) {
-    sendAT(GF("+CSCS=HEX"));
+    sendAT(GF("+CMGF=1"));
+    waitResponse();
+    sendAT(GF("+CSCS=\"HEX\""));
     waitResponse();
     sendAT(GF("+CUSD=1,\""), code, GF("\",15"));
     if (waitResponse(10000L) != 1) {
@@ -462,6 +464,8 @@ public:
 
     if (dcs == 15) {
       return decodeHex7bit(hex);
+    } else if (dcs == 72) {
+      return decodeHex16bit(hex);
     } else {
       return hex;
     }
@@ -539,8 +543,8 @@ private:
     return len;
   }
 
-  bool modemGetConnected(uint8_t mux) { //TODO mux?
-    sendAT(GF("+CIPSTATUS"));
+  bool modemGetConnected(uint8_t mux) {
+    sendAT(GF("+CIPSTATUS")); //TODO mux?
     int res = waitResponse(GF(",\"CONNECTED\""), GF(",\"CLOSED\""), GF(",\"CLOSING\""), GF(",\"INITIAL\""));
     waitResponse();
     return 1 == res;
@@ -571,6 +575,25 @@ private:
     return result;
   }
 
+  static String decodeHex16bit(String &instr) {
+    String result;
+    for (unsigned i=0; i<instr.length(); i+=4) {
+      char buf[4] = { 0, };
+      buf[0] = instr[i];
+      buf[1] = instr[i+1];
+      char b = strtol(buf, NULL, 16);
+      if (b) { // If high byte is non-zero, we can't handle it ;(
+        b = '?';
+      } else {
+        buf[0] = instr[i+2];
+        buf[1] = instr[i+3];
+        b = strtol(buf, NULL, 16);
+      }
+      result += b;
+    }
+    return result;
+  }
+
 public:
 
   /* Utilities */
diff --git a/TinyGsmClientSIM800.h b/TinyGsmClientSIM800.h
index 978fc37..6b50293 100644
--- a/TinyGsmClientSIM800.h
+++ b/TinyGsmClientSIM800.h
@@ -257,7 +257,7 @@ public:
       return "";
     }
     res.replace(GSM_NL "OK" GSM_NL, "");
-    res.replace(GSM_NL, "");
+    res.replace(GSM_NL, " ");
     res.trim();
     return res;
   }
@@ -537,7 +537,9 @@ public:
    */
 
   String sendUSSD(const String& code) {
-    sendAT(GF("+CSCS=\"8859-1\""));
+    sendAT(GF("+CMGF=1"));
+    waitResponse();
+    sendAT(GF("+CSCS=\"HEX\""));
     waitResponse();
     sendAT(GF("+CUSD=1,\""), code, GF("\""));
     if (waitResponse() != 1) {
@@ -547,9 +549,17 @@ public:
       return "";
     }
     stream.readStringUntil('"');
-    String res = stream.readStringUntil('"');
-    stream.readStringUntil('\n');
-    return res;
+    String hex = stream.readStringUntil('"');
+    stream.readStringUntil(',');
+    int dcs = stream.readStringUntil('\n').toInt();
+
+    if (dcs == 15) {
+      return decodeHex8bit(hex);
+    } else if (dcs == 72) {
+      return decodeHex16bit(hex);
+    } else {
+      return hex;
+    }
   }
 
   bool sendSMS(const String& number, const String& text) {
@@ -717,6 +727,37 @@ private:
     return 1 == res;
   }
 
+  static String decodeHex8bit(String &instr) {
+    String result;
+    for (unsigned i=0; i<instr.length(); i+=2) {
+      char buf[4] = { 0, };
+      buf[0] = instr[i];
+      buf[1] = instr[i+1];
+      char b = strtol(buf, NULL, 16);
+      result += b;
+    }
+    return result;
+  }
+
+  static String decodeHex16bit(String &instr) {
+    String result;
+    for (unsigned i=0; i<instr.length(); i+=4) {
+      char buf[4] = { 0, };
+      buf[0] = instr[i];
+      buf[1] = instr[i+1];
+      char b = strtol(buf, NULL, 16);
+      if (b) { // If high byte is non-zero, we can't handle it ;(
+        b = '?';
+      } else {
+        buf[0] = instr[i+2];
+        buf[1] = instr[i+3];
+        b = strtol(buf, NULL, 16);
+      }
+      result += b;
+    }
+    return result;
+  }
+
 public:
 
   /* Utilities */