1 files changed, 41 insertions, 32 deletions

diff --git a/fw/esp32/components/eos/cell_ussd.c b/fw/esp32/components/eos/cell_ussd.c
index fb77b67..002391e 100644
--- a/fw/esp32/components/eos/cell_ussd.c
+++ b/fw/esp32/components/eos/cell_ussd.c
@@ -13,21 +13,26 @@ static const char *TAG = "EOS CELL USSD";
 
 extern char *at_cmd_buf;
 
-void eos_cell_ussd_handler(unsigned char mtype, unsigned char *buffer, uint16_t buf_len) {
+void eos_cell_ussd_handler(unsigned char _mtype, EOSMessage *msg, uint16_t len) {
+    unsigned char mtype;
+    unsigned char *buffer = msg->buffer;
     int rv;
 
+    if (len < 1) return;
+
+    mtype = buffer[0] & ~EOS_CELL_MTYPE_MASK;
     buffer++;
-    buf_len--;
+    len--;
+
     switch (mtype) {
         case EOS_CELL_MTYPE_USSD_REQUEST: {
-            if (buf_len > EOS_CELL_USSD_SIZE_REQ) return;
+            if (len > EOS_CELL_USSD_SIZE_REQ) break;
 
-            buffer[buf_len] = '\0';
-            rv = snprintf(at_cmd_buf, AT_SIZE_CMD_BUF, "AT+CUSD=1,\"%s\",15\r", buffer);
-            if ((rv < 0) || (rv >= AT_SIZE_CMD_BUF)) return;
+            rv = snprintf(at_cmd_buf, AT_SIZE_CMD_BUF, "AT+CUSD=1,\"%.*s\",15\r", len, buffer);
+            if ((rv < 0) || (rv >= AT_SIZE_CMD_BUF)) break;
 
             rv = eos_modem_take(1000);
-            if (rv) return;
+            if (rv) break;
 
             at_cmd(at_cmd_buf);
             rv = at_expect("^OK", "^ERROR", 1000);
@@ -38,7 +43,7 @@ void eos_cell_ussd_handler(unsigned char mtype, unsigned char *buffer, uint16_t
 
         case EOS_CELL_MTYPE_USSD_CANCEL: {
             rv = eos_modem_take(1000);
-            if (rv) return;
+            if (rv) break;
 
             at_cmd("AT+CUSD=2\r");
             rv = at_expect("^OK", "^ERROR", 1000);
@@ -50,59 +55,63 @@ void eos_cell_ussd_handler(unsigned char mtype, unsigned char *buffer, uint16_t
 }
 
 static void ussd_reply_handler(char *urc, regmatch_t m[]) {
-    int rep, rv;
-    unsigned char *buf;
+    EOSMessage msg;
     uint16_t len;
-    char *_buf;
-    size_t _len;
+    char *rbuf;
+    size_t rlen;
+    int rep, rv;
 
     sscanf(urc + m[1].rm_so, "%1d", &rep);
 
-    buf = eos_net_alloc();
-    buf[0] = EOS_CELL_MTYPE_USSD | EOS_CELL_MTYPE_USSD_REPLY;
-    buf[1] = rep;
+    eos_net_alloc(&msg);
+    if (msg.size < 2) {
+        eos_net_free(&msg);
+        return;
+    }
+    msg.buffer[0] = EOS_CELL_MTYPE_USSD | EOS_CELL_MTYPE_USSD_REPLY;
+    msg.buffer[1] = rep;
     len = 2;
 
     if (m[2].rm_so == -1) {
-        rv = eos_net_send(EOS_NET_MTYPE_CELL, buf, len);
+        rv = eos_net_send(EOS_NET_MTYPE_CELL, &msg, len);
         if (rv) ESP_LOGE(TAG, "NET SEND ERR:%d", rv);
         return;
     }
 
-    _buf = (char *)buf + len;
-    _len = m[2].rm_eo - m[2].rm_so - 2;
-    if (_len > EOS_NET_SIZE_BUF - len) {
-        eos_net_free(buf);
+    rbuf = (char *)msg.buffer + len;
+    rlen = m[2].rm_eo - m[2].rm_so - 2;
+    if (msg.size < rlen + len + 1) {
+        eos_net_free(&msg);
         return;
     }
-    memcpy(_buf, urc + m[2].rm_so + 2, _len);
-    len += _len;
+    memcpy(rbuf, urc + m[2].rm_so + 2, rlen);
+    rbuf[rlen] = '\0';
 
     rv = EOS_OK;
     do {
         char *end;
 
-        end = strchr(_buf, '"');
+        end = strchr(rbuf, '"');
         if (end) {
-            len += end - _buf;
+            len += end - rbuf;
             break;
         } else {
-            _len = strlen(_buf);
-            _buf[_len] = '\n';
-            _buf += _len + 1;
-            len += _len + 1;
+            rlen = strlen(rbuf);
+            rbuf[rlen] = '\n';
+            rbuf += rlen + 1;
+            len += rlen + 1;
         }
-        rv = eos_modem_readln(_buf, EOS_NET_SIZE_BUF - len, 1000);
+        rv = eos_modem_readln(rbuf, msg.size - len, 1000);
         if (rv) break;
     } while (1);
 
     if (rv) {
-        ESP_LOGE(TAG, "error");
-        eos_net_free(buf);
+        ESP_LOGE(TAG, "ERROR");
+        eos_net_free(&msg);
         return;
     }
 
-    rv = eos_net_send(EOS_NET_MTYPE_CELL, buf, len);
+    rv = eos_net_send(EOS_NET_MTYPE_CELL, &msg, len);
     if (rv) ESP_LOGE(TAG, "NET SEND ERR:%d", rv);
 }