esp32 cellular modem driver fix

1 files changed, 197 insertions, 155 deletions

diff --git a/fw/esp32/components/eos/gsm.c b/fw/esp32/components/eos/gsm.c
index 920e121..551dc2c 100644
--- a/fw/esp32/components/eos/gsm.c
+++ b/fw/esp32/components/eos/gsm.c
@@ -1,36 +1,30 @@
-#include <stdlib.h>
-#include <string.h>
 #include <stdio.h>
 
 #include "gsm.h"
 
 #define DIVC(x,y)                   ((x) / (y) + ((x) % (y) != 0))
 
-uint8_t pdu_getc(char *pdu) {
-    int ch;
-
-    sscanf(pdu, "%2X", &ch);
-    return ch;
-}
-
-void pdu_putc(uint8_t ch, char *pdu) {
+size_t pdu_putc(char *pdu, uint8_t ch) {
     char b[3];
 
     sprintf(b, "%.2X", ch);
     *pdu = b[0];
     *(pdu + 1) = b[1];
+
+    return 2;
 }
 
-void pdu_gets(char *pdu, uint8_t *s, int s_len) {
-    int i, ch;
+ssize_t pdu_getc(char *pdu, uint8_t *ch) {
+    int rv, _ch;
 
-    for (i=0; i<s_len; i++) {
-        sscanf(pdu + 2 * i, "%2X", &ch);
-        s[i] = ch;
-    }
+    rv = sscanf(pdu, "%2X", &_ch);
+    if (rv != 1) return GSM_ERR;
+
+    *ch = _ch;
+    return 2;
 }
 
-void pdu_puts(uint8_t *s, int s_len, char *pdu) {
+size_t pdu_puts(char *pdu, uint8_t *s, size_t s_len) {
     int i;
     char b[3];
 
@@ -39,6 +33,108 @@ void pdu_puts(uint8_t *s, int s_len, char *pdu) {
         *(pdu + 2 * i) = b[0];
         *(pdu + 2 * i + 1) = b[1];
     }
+
+    return s_len * 2;
+}
+
+ssize_t pdu_gets(char *pdu, uint8_t *s, size_t s_len) {
+    int i, rv, _ch;
+
+    for (i=0; i<s_len; i++) {
+        rv = sscanf(pdu + 2 * i, "%2X", &_ch);
+        if (rv != 1) return GSM_ERR;
+        s[i] = _ch;
+    }
+
+    return s_len * 2;
+}
+
+size_t pdu_7bit_enc(char *pdu, char *text, size_t text_len, int padb) {
+    uint8_t carry = 0;
+    size_t pdu_len = 0;
+    int i = 0, shc = 0;
+
+    if (!text_len) return 0;
+
+    if (padb) {
+        shc = 7 - padb;
+    } else {
+        carry = *text;
+        i++;
+    }
+
+    while (i < text_len) {
+        pdu_putc(pdu + pdu_len, carry | (*(text + i) << (7 - shc)));
+        pdu_len += 2;
+
+        shc++;
+        shc = shc % 7;
+        if (!shc) {
+            i++;
+            if (i == text_len) return pdu_len;
+        }
+
+        carry = *(text + i) >> shc;
+        i++;
+    }
+    pdu_putc(pdu + pdu_len, carry);
+    pdu_len += 2;
+
+    return pdu_len;
+}
+
+ssize_t pdu_7bit_dec(char *pdu, char *text, size_t text_len, int padb) {
+    uint8_t ch;
+    uint8_t carry = 0;
+    int i = 0, shc = 0;
+    size_t pdu_len = 0;
+    ssize_t rv;
+
+    if (!text_len) return 0;
+
+    if (padb) {
+        rv = pdu_getc(pdu, &ch);
+        if (rv < 0) return rv;
+        pdu_len += 2;
+
+        if (padb == 1) {
+            *text = ch >> 1;
+            i++;
+        } else {
+            carry = ch >> padb;
+            shc = 8 - padb;
+        }
+    }
+
+    while (i < text_len) {
+        rv = pdu_getc(pdu + pdu_len, &ch);
+        if (rv < 0) return rv;
+        pdu_len += 2;
+
+        *(text + i) = ((ch << shc) | carry) & 0x7f;
+        carry = ch >> (7 - shc);
+        i++;
+
+        shc++;
+        shc = shc % 7;
+        if (!shc && (i < text_len)) {
+            *(text + i) = carry;
+            carry = 0;
+            i++;
+        }
+    }
+
+    return pdu_len;
+}
+
+void gsm_dcs_enc(uint8_t enc, uint16_t flags, uint8_t *dcs) {
+    *dcs = enc;
+    if (flags & GSM_FLAG_CLASS) {
+        *dcs |= GSM_DCS_CLASS_IND;
+        *dcs |= (flags >> 8) & GSM_DCS_CLASS;
+    }
+    if (flags & GSM_FLAG_COMPRESS) *dcs |= GSM_DCS_COMPRESS_IND;
+    if (flags & GSM_FLAG_DELETE) *dcs |= GSM_DCS_DELETE_IND;
 }
 
 void gsm_dcs_dec(uint8_t dcs, uint8_t *enc, uint16_t *flags) {
@@ -78,17 +174,7 @@ void gsm_dcs_dec(uint8_t dcs, uint8_t *enc, uint16_t *flags) {
     }
 }
 
-void gsm_dcs_enc(uint8_t enc, uint16_t flags, uint8_t *dcs) {
-    *dcs = enc;
-    if (flags & GSM_FLAG_CLASS) {
-        *dcs |= GSM_DCS_CLASS_IND;
-        *dcs |= (flags >> 8) & GSM_DCS_CLASS;
-    }
-    if (flags & GSM_FLAG_COMPRESS) *dcs |= GSM_DCS_COMPRESS_IND;
-    if (flags & GSM_FLAG_DELETE) *dcs |= GSM_DCS_DELETE_IND;
-}
-
-int gsm_ts_enc(char *ts, char *pdu, int pdu_size) {
+ssize_t gsm_ts_enc(char *pdu, size_t pdu_size, char *ts) {
     uint8_t tz;
     int tz_hh, tz_mm;
 
@@ -117,13 +203,14 @@ int gsm_ts_enc(char *ts, char *pdu, int pdu_size) {
     tz = (tz / 10) | ((tz % 10) << 4);
     if (ts[19] == '-') tz |= 0x08;
 
-    pdu_putc(tz, pdu + 12);
+    pdu_putc(pdu + 12, tz);
 
     return 14;
 }
 
-int gsm_ts_dec(char *pdu, int pdu_len, char *ts) {
+ssize_t gsm_ts_dec(char *pdu, size_t pdu_len, char *ts) {
     uint8_t tz;
+    ssize_t rv;
 
     if (pdu_len < 14) return GSM_ERR_SIZE;
 
@@ -147,7 +234,9 @@ int gsm_ts_dec(char *pdu, int pdu_len, char *ts) {
     ts[17] = pdu[11];   // ss
     ts[18] = pdu[10];
 
-    tz = pdu_getc(pdu + 12);
+    rv = pdu_getc(pdu + 12, &tz);
+    if (rv < 0) return rv;
+
     if (tz & 0x08) {
         ts[19] = '-';
         tz = tz & ~0x08;
@@ -160,80 +249,8 @@ int gsm_ts_dec(char *pdu, int pdu_len, char *ts) {
     return 14;
 }
 
-int gsm_7bit_enc(char *text, int text_len, char *pdu, int padb) {
-    uint8_t carry = 0;
-    int i = 0, pdu_len = 0, shc = 0;
-
-    if (!text_len) return 0;
-
-    if (padb) {
-        shc = 7 - padb;
-    } else {
-        carry = *text;
-        i++;
-    }
-
-    while (i < text_len) {
-        pdu_putc(carry | (*(text + i) << (7 - shc)), pdu + pdu_len);
-        pdu_len += 2;
-
-        shc++;
-        shc = shc % 7;
-        if (!shc) {
-            i++;
-            if (i == text_len) return pdu_len;
-        }
-
-        carry = *(text + i) >> shc;
-        i++;
-    }
-    pdu_putc(carry, pdu + pdu_len);
-    pdu_len += 2;
-
-    return pdu_len;
-}
-
-int gsm_7bit_dec(char *pdu, char *text, int text_len, int padb) {
-    uint8_t ch;
-    uint8_t carry = 0;
-    int i = 0, pdu_len = 0, shc = 0;
-
-    if (!text_len) return 0;
-
-    if (padb) {
-        ch = pdu_getc(pdu);
-        pdu_len += 2;
-        if (padb == 1) {
-            *text = ch >> 1;
-            i++;
-        } else {
-            carry = ch >> padb;
-            shc = 8 - padb;
-        }
-    }
-
-    while (i < text_len) {
-        ch = pdu_getc(pdu + pdu_len);
-        pdu_len += 2;
-
-        *(text + i) = ((ch << shc) | carry) & 0x7f;
-        carry = ch >> (7 - shc);
-        i++;
-
-        shc++;
-        shc = shc % 7;
-        if (!shc && (i < text_len)) {
-            *(text + i) = carry;
-            carry = 0;
-            i++;
-        }
-    }
-
-    return pdu_len;
-}
-
-int gsm_addr_enc(char *addr, int addr_len, uint8_t addr_type, char *pdu, int pdu_size) {
-    int _pdu_len;
+ssize_t gsm_addr_enc(char *pdu, size_t pdu_size, char *addr, size_t addr_len, uint8_t addr_type) {
+    size_t _pdu_len;
 
     addr_type |= GSM_EXT;
 
@@ -243,17 +260,17 @@ int gsm_addr_enc(char *addr, int addr_len, uint8_t addr_type, char *pdu, int pdu
         _pdu_len = 4 + DIVC(_addr_len, 2) * 2;
         if (pdu_size < _pdu_len) return GSM_ERR_SIZE;
 
-        pdu_putc(_addr_len, pdu);
-        pdu_putc(addr_type, pdu + 2);
-        gsm_7bit_enc(addr, addr_len, pdu, 0);
+        pdu_putc(pdu, _addr_len);
+        pdu_putc(pdu + 2, addr_type);
+        pdu_7bit_enc(pdu + 4, addr, addr_len, 0);
     } else {
         int i;
 
         _pdu_len = 4 + DIVC(addr_len, 2) * 2;
         if (pdu_size < _pdu_len) return GSM_ERR_SIZE;
 
-        pdu_putc(addr_len, pdu);
-        pdu_putc(addr_type, pdu + 2);
+        pdu_putc(pdu, addr_len);
+        pdu_putc(pdu + 2, addr_type);
         for (i=0; i<addr_len / 2; i++) {
             pdu[4 + 2 * i] = addr[2 * i + 1];
             pdu[4 + 2 * i + 1] = addr[2 * i];
@@ -267,16 +284,22 @@ int gsm_addr_enc(char *addr, int addr_len, uint8_t addr_type, char *pdu, int pdu
     return _pdu_len;
 }
 
-int gsm_addr_dec(char *pdu, int pdu_len, char *addr, int *addr_len, uint8_t *addr_type) {
-    int _pdu_len;
+ssize_t gsm_addr_dec(char *pdu, size_t pdu_len, char *addr, size_t *addr_len, uint8_t *addr_type) {
+    uint8_t pdu_ch;
+    size_t _pdu_len;
+    ssize_t rv;
 
     if (pdu_len < 4) return GSM_ERR_SIZE;
 
-    *addr_len = pdu_getc(pdu);
-    *addr_type = pdu_getc(pdu + 2);
+    rv = pdu_getc(pdu, &pdu_ch);
+    if (rv < 0) return rv;
+    rv = pdu_getc(pdu + 2, addr_type);
+    if (rv < 0) return rv;
+
+    *addr_len = pdu_ch;
     if (*addr_len > GSM_ADDR_SIZE) return GSM_ERR_SIZE;
 
-    if (!(*addr_type & GSM_EXT)) return GSM_ERR;
+    if (!(*addr_type & GSM_EXT)) return GSM_ERR_NOTSUPPORTED;
 
     _pdu_len = 4 + DIVC(*addr_len, 2) * 2;
     if (pdu_len < _pdu_len) return GSM_ERR_SIZE;
@@ -284,7 +307,8 @@ int gsm_addr_dec(char *pdu, int pdu_len, char *addr, int *addr_len, uint8_t *add
     if ((*addr_type & GSM_TON) == GSM_TON_ALPHANUMERIC) {
         *addr_len = (*addr_len * 4) / 7;
 
-        gsm_7bit_dec(pdu + 4, addr, *addr_len, 0);
+        rv = pdu_7bit_dec(pdu + 4, addr, *addr_len, 0);
+        if (rv < 0) return rv;
     } else {
         int i;
 
@@ -300,13 +324,14 @@ int gsm_addr_dec(char *pdu, int pdu_len, char *addr, int *addr_len, uint8_t *add
     return _pdu_len;
 }
 
-int gsm_sms_enc(char *addr, int addr_len, uint8_t addr_type, uint8_t *udh, int udh_len, uint8_t *msg, int msg_len, uint8_t enc, uint16_t flags, char *pdu, int pdu_size) {
-    int rv, _pdu_len = 0;
+ssize_t gsm_sms_enc(char *pdu, size_t pdu_size, char *addr, size_t addr_len, uint8_t addr_type, uint8_t *udh, size_t udh_len, uint8_t *txt, size_t txt_len, uint8_t enc, uint16_t flags) {
+    size_t _pdu_len = 0;
     uint8_t mti;
     uint8_t mr;
     uint8_t pid;
     uint8_t dcs;
     uint8_t udl;
+    ssize_t rv;
 
     mti = GSM_MTI_SUBMIT;
     if (udh_len) mti |= GSM_UDHI;
@@ -315,11 +340,11 @@ int gsm_sms_enc(char *addr, int addr_len, uint8_t addr_type, uint8_t *udh, int u
     mr = 0;
 
     if (pdu_size < 4) return GSM_ERR_SIZE;
-    pdu_putc(mti, pdu);
-    pdu_putc(mr, pdu + 2);
+    pdu_putc(pdu, mti);
+    pdu_putc(pdu + 2, mr);
     _pdu_len += 4;
 
-    rv = gsm_addr_enc(addr, addr_len, addr_type, pdu + _pdu_len, pdu_size - _pdu_len);
+    rv = gsm_addr_enc(pdu + _pdu_len, pdu_size - _pdu_len, addr, addr_len, addr_type);
     if (rv < 0) return rv;
     _pdu_len += rv;
 
@@ -329,10 +354,10 @@ int gsm_sms_enc(char *addr, int addr_len, uint8_t addr_type, uint8_t *udh, int u
     } else {
         pid = GSM_PID_DEFAULT;
     }
-    pid = 37;
-    pdu_putc(pid, pdu + _pdu_len);
     gsm_dcs_enc(enc, flags, &dcs);
-    pdu_putc(dcs, pdu + _pdu_len + 2);
+
+    pdu_putc(pdu + _pdu_len, pid);
+    pdu_putc(pdu + _pdu_len + 2, dcs);
     _pdu_len += 4;
 
     if (enc == GSM_ENC_7BIT) {
@@ -343,55 +368,57 @@ int gsm_sms_enc(char *addr, int addr_len, uint8_t addr_type, uint8_t *udh, int u
             udh_blen = 8 * (udh_len + 1);
             padb = DIVC(udh_blen, 7) * 7 - udh_blen;
         }
-        udl = DIVC(udh_blen, 7) + msg_len;
+        udl = DIVC(udh_blen, 7) + txt_len;
 
         if (pdu_size < _pdu_len + (DIVC(udl * 7, 8) + 1) * 2) return GSM_ERR_SIZE;
-        pdu_putc(udl, pdu + _pdu_len);
+        pdu_putc(pdu + _pdu_len, udl);
         _pdu_len += 2;
 
         if (udh_len) {
-            pdu_putc(udh_len, pdu + _pdu_len);
-            pdu_puts(udh, udh_len, pdu + _pdu_len + 2);
+            pdu_putc(pdu + _pdu_len, udh_len);
+            pdu_puts(pdu + _pdu_len + 2, udh, udh_len);
             _pdu_len += (udh_len + 1) * 2;
         }
 
-        rv = gsm_7bit_enc((char *)msg, msg_len, pdu + _pdu_len, padb);
+        rv = pdu_7bit_enc(pdu + _pdu_len, (char *)txt, txt_len, padb);
         if (rv < 0) return rv;
         _pdu_len += rv;
     } else {
-        udl = msg_len + (udh_len ? udh_len + 1 : 0);
+        udl = txt_len + (udh_len ? udh_len + 1 : 0);
 
         if (pdu_size < _pdu_len + (udl + 1) * 2) return GSM_ERR_SIZE;
-        pdu_putc(udl, pdu + _pdu_len);
+        pdu_putc(pdu + _pdu_len, udl);
         _pdu_len += 2;
 
         if (udh_len) {
-            pdu_putc(udh_len, pdu + _pdu_len);
-            pdu_puts(udh, udh_len, pdu + _pdu_len + 2);
+            pdu_putc(pdu + _pdu_len, udh_len);
+            pdu_puts(pdu + _pdu_len + 2, udh, udh_len);
             _pdu_len += (udh_len + 1) * 2;
         }
 
-        pdu_puts(msg, msg_len, pdu + _pdu_len);
-        _pdu_len += msg_len * 2;
+        pdu_puts(pdu + _pdu_len, txt, txt_len);
+        _pdu_len += txt_len * 2;
     }
 
     return _pdu_len;
 }
 
-int gsm_sms_dec(char *pdu, int pdu_len, char *addr, int *addr_len, uint8_t *addr_type, uint8_t *udh, int *udh_len, uint8_t *msg, int *msg_len, char *ts, uint8_t *enc, uint16_t *flags) {
-    int rv, _pdu_len = 0;
+ssize_t gsm_sms_dec(char *pdu, size_t pdu_len, char *addr, size_t *addr_len, uint8_t *addr_type, uint8_t *udh, size_t *udh_len, uint8_t *txt, size_t *txt_len, uint8_t *enc, char *ts, uint16_t *flags) {
+    size_t _pdu_len = 0;
     uint8_t mti;
     uint8_t pid;
     uint8_t dcs;
     uint8_t udl;
+    ssize_t rv;
 
     *enc = 0xff;
     *flags = 0;
     if (pdu_len < 2) return GSM_ERR_SIZE;
-    mti = pdu_getc(pdu);
+    rv = pdu_getc(pdu, &mti);
+    if (rv < 0) return rv;
     _pdu_len += 2;
 
-    if ((mti & GSM_MTI) != GSM_MTI_DELIVER) return GSM_ERR_NOT_SUPPORTED;
+    if ((mti & GSM_MTI) != GSM_MTI_DELIVER) return GSM_ERR_NOTSUPPORTED;
     if (mti & GSM_SRI) *flags |= GSM_FLAG_STATUS_REPORT;
     if (mti & GSM_RP) *flags |= GSM_FLAG_REPLY_PATH;
 
@@ -400,13 +427,15 @@ int gsm_sms_dec(char *pdu, int pdu_len, char *addr, int *addr_len, uint8_t *addr
     _pdu_len += rv;
 
     if (pdu_len < _pdu_len + 4) return GSM_ERR_SIZE;
-    pid = pdu_getc(pdu + _pdu_len);
+    rv = pdu_getc(pdu + _pdu_len, &pid);
+    if (rv < 0) return rv;
     if (pid == GSM_PID_TYPE0) {
         *flags |= GSM_FLAG_TYPE0;
     } else if (pid != GSM_PID_DEFAULT) {
-        return GSM_ERR_NOT_SUPPORTED;
+        return GSM_ERR_NOTSUPPORTED;
     }
-    dcs = pdu_getc(pdu + _pdu_len + 2);
+    rv = pdu_getc(pdu + _pdu_len + 2, &dcs);
+    if (rv < 0) return rv;
     gsm_dcs_dec(dcs, enc, flags);
     _pdu_len += 4;
 
@@ -415,7 +444,8 @@ int gsm_sms_dec(char *pdu, int pdu_len, char *addr, int *addr_len, uint8_t *addr
     _pdu_len += rv;
 
     if (pdu_len < _pdu_len + 2) return GSM_ERR_SIZE;
-    udl = pdu_getc(pdu + _pdu_len);
+    rv = pdu_getc(pdu + _pdu_len, &udl);
+    if (rv < 0) return rv;
     _pdu_len += 2;
 
     if ((mti & GSM_UDHI) && (udl == 0)) return GSM_ERR;
@@ -428,46 +458,58 @@ int gsm_sms_dec(char *pdu, int pdu_len, char *addr, int *addr_len, uint8_t *addr
         if (pdu_len < _pdu_len + DIVC(udl * 7, 8) * 2) return GSM_ERR_SIZE;
 
         if (mti & GSM_UDHI) {
-            *udh_len = pdu_getc(pdu + _pdu_len);
+            uint8_t pdu_ch;
+
+            rv = pdu_getc(pdu + _pdu_len, &pdu_ch);
+            if (rv < 0) return rv;
+
+            *udh_len = pdu_ch;
             udh_blen = 8 * (*udh_len + 1);
             padb = DIVC(udh_blen, 7) * 7 - udh_blen;
 
             if (udl * 7 < udh_blen) return GSM_ERR;
             if (*udh_len > GSM_UDH_SIZE) return GSM_ERR_SIZE;
 
-            pdu_gets(pdu + _pdu_len + 2, udh, *udh_len);
+            rv = pdu_gets(pdu + _pdu_len + 2, udh, *udh_len);
+            if (rv < 0) return rv;
             _pdu_len += (*udh_len + 1) * 2;
         } else {
             *udh_len = 0;
         }
 
-        *msg_len = udl - DIVC(udh_blen, 7);
-        if (*msg_len > GSM_UD_SIZE) return GSM_ERR_SIZE;
+        *txt_len = udl - DIVC(udh_blen, 7);
+        if (*txt_len > GSM_UD_SIZE) return GSM_ERR_SIZE;
 
-        rv = gsm_7bit_dec(pdu + _pdu_len, (char *)msg, *msg_len, padb);
+        rv = pdu_7bit_dec(pdu + _pdu_len, (char *)txt, *txt_len, padb);
         if (rv < 0) return rv;
         _pdu_len += rv;
     } else {
         if (pdu_len < _pdu_len + udl * 2) return GSM_ERR_SIZE;
 
         if (mti & GSM_UDHI) {
-            *udh_len = pdu_getc(pdu + _pdu_len);
+            uint8_t pdu_ch;
 
+            rv = pdu_getc(pdu + _pdu_len, &pdu_ch);
+            if (rv < 0) return rv;
+
+            *udh_len = pdu_ch;
             if (udl < *udh_len + 1) return GSM_ERR;
             if (*udh_len > GSM_UDH_SIZE) return GSM_ERR_SIZE;
 
-            pdu_gets(pdu + _pdu_len + 2, udh, *udh_len);
+            rv = pdu_gets(pdu + _pdu_len + 2, udh, *udh_len);
+            if (rv < 0) return rv;
             _pdu_len += (*udh_len + 1) * 2;
         } else {
             *udh_len = 0;
         }
 
-        *msg_len = udl - (*udh_len ? *udh_len + 1 : 0);
-        if (*msg_len > GSM_UD_SIZE) return GSM_ERR_SIZE;
-        if ((*enc == GSM_ENC_UCS2) && ((*msg_len % 2) != 0)) return GSM_ERR;
+        *txt_len = udl - (*udh_len ? *udh_len + 1 : 0);
+        if (*txt_len > GSM_UD_SIZE) return GSM_ERR_SIZE;
+        if ((*enc == GSM_ENC_UCS2) && ((*txt_len % 2) != 0)) return GSM_ERR;
 
-        pdu_gets(pdu + _pdu_len, msg, *msg_len);
-        _pdu_len += *msg_len * 2;
+        rv = pdu_gets(pdu + _pdu_len, txt, *txt_len);
+        if (rv < 0) return rv;
+        _pdu_len += *txt_len * 2;
     }
 
     return _pdu_len;