modem-helpers: new +CESQ response parser

(cherry picked from commit df6f6d1f37dfeb27c27cf54e6181df9eaf162322)

3 files changed, 179 insertions, 0 deletions

diff --git a/src/mm-modem-helpers.c b/src/mm-modem-helpers.c
index 01d0c7902..46274fd21 100644
--- a/src/mm-modem-helpers.c
+++ b/src/mm-modem-helpers.c
@@ -1393,6 +1393,98 @@ mm_3gpp_parse_crsm_response (const gchar *reply,
 }
 
 /*************************************************************************/
+/* +CESQ response parser */
+
+gboolean
+mm_3gpp_parse_cesq_response (const gchar  *response,
+                             guint        *out_rxlev,
+                             guint        *out_ber,
+                             guint        *out_rscp,
+                             guint        *out_ecn0,
+                             guint        *out_rsrq,
+                             guint        *out_rsrp,
+                             GError      **error)
+{
+    GRegex     *r;
+    GMatchInfo *match_info;
+    GError     *inner_error = NULL;
+    guint       rxlev = 0;
+    guint       ber = 0;
+    guint       rscp = 0;
+    guint       ecn0 = 0;
+    guint       rsrq = 0;
+    guint       rsrp = 0;
+    gboolean    success = FALSE;
+
+    g_assert (out_rxlev);
+    g_assert (out_ber);
+    g_assert (out_rscp);
+    g_assert (out_ecn0);
+    g_assert (out_rsrq);
+    g_assert (out_rsrp);
+
+    /* Response may be e.g.:
+     * +CESQ: 99,99,255,255,20,80
+     */
+    r = g_regex_new ("\\+CESQ: (\\d+),(\\d+),(\\d+),(\\d+),(\\d+),(\\d+)(?:\\r\\n)?", 0, 0, NULL);
+    g_assert (r != NULL);
+
+    g_regex_match_full (r, response, strlen (response), 0, 0, &match_info, &inner_error);
+    if (!inner_error && g_match_info_matches (match_info)) {
+        if (!mm_get_uint_from_match_info (match_info, 1, &rxlev)) {
+            inner_error = g_error_new (MM_CORE_ERROR, MM_CORE_ERROR_FAILED, "Couldn't read RXLEV");
+            goto out;
+        }
+        if (!mm_get_uint_from_match_info (match_info, 2, &ber)) {
+            inner_error = g_error_new (MM_CORE_ERROR, MM_CORE_ERROR_FAILED, "Couldn't read BER");
+            goto out;
+        }
+        if (!mm_get_uint_from_match_info (match_info, 3, &rscp)) {
+            inner_error = g_error_new (MM_CORE_ERROR, MM_CORE_ERROR_FAILED, "Couldn't read RSCP");
+            goto out;
+        }
+        if (!mm_get_uint_from_match_info (match_info, 4, &ecn0)) {
+            inner_error = g_error_new (MM_CORE_ERROR, MM_CORE_ERROR_FAILED, "Couldn't read Ec/N0");
+            goto out;
+        }
+        if (!mm_get_uint_from_match_info (match_info, 5, &rsrq)) {
+            inner_error = g_error_new (MM_CORE_ERROR, MM_CORE_ERROR_FAILED, "Couldn't read RSRQ");
+            goto out;
+        }
+        if (!mm_get_uint_from_match_info (match_info, 6, &rsrp)) {
+            inner_error = g_error_new (MM_CORE_ERROR, MM_CORE_ERROR_FAILED, "Couldn't read RSRP");
+            goto out;
+        }
+        success = TRUE;
+    }
+
+out:
+
+    if (match_info)
+        g_match_info_free (match_info);
+    g_regex_unref (r);
+
+    if (inner_error) {
+        g_propagate_error (error, inner_error);
+        return FALSE;
+    }
+
+    if (!success) {
+        g_set_error (error, MM_CORE_ERROR, MM_CORE_ERROR_FAILED,
+                     "Couldn't parse +CESQ response: %s", response);
+        return FALSE;
+    }
+
+    *out_rxlev = rxlev;
+    *out_ber = ber;
+    *out_rscp = rscp;
+    *out_ecn0 = ecn0;
+    *out_rsrq = rsrq;
+    *out_rsrp = rsrp;
+    return TRUE;
+}
+
+/*************************************************************************/
 
 static MMSmsStorage
 storage_from_str (const gchar *str)
diff --git a/src/mm-modem-helpers.h b/src/mm-modem-helpers.h
index 476b31567..af0520415 100644
--- a/src/mm-modem-helpers.h
+++ b/src/mm-modem-helpers.h
@@ -221,6 +221,16 @@ gboolean mm_3gpp_parse_crsm_response (const gchar *reply,
                                       gchar **hex,
                                       GError **error);
 
+/* +CESQ response parser */
+gboolean mm_3gpp_parse_cesq_response (const gchar  *response,
+                                      guint        *out_rxlev,
+                                      guint        *out_ber,
+                                      guint        *out_rscp,
+                                      guint        *out_ecn0,
+                                      guint        *out_rsrq,
+                                      guint        *out_rsrp,
+                                      GError      **error);
+
 /* Additional 3GPP-specific helpers */
 
 MMModem3gppFacility mm_3gpp_acronym_to_facility (const gchar *str);
diff --git a/src/tests/test-modem-helpers.c b/src/tests/test-modem-helpers.c
index 4c2d6db6c..84bc07f93 100644
--- a/src/tests/test-modem-helpers.c
+++ b/src/tests/test-modem-helpers.c
@@ -2733,6 +2733,81 @@ test_crsm_response (void)
 }
 
 /*****************************************************************************/
+/* Test +CESQ responses */
+
+typedef struct {
+    const gchar *str;
+    guint rxlev;
+    guint ber;
+    guint rscp;
+    guint ecn0;
+    guint rsrq;
+    guint rsrp;
+} CesqResponseTest;
+
+static const CesqResponseTest cesq_response_tests[] = {
+    {
+        .str   = "+CESQ: 99,99,255,255,20,80",
+        .rxlev = 99,
+        .ber   = 99,
+        .rscp  = 255,
+        .ecn0  = 255,
+        .rsrq  = 20,
+        .rsrp  = 80
+    },
+    {
+        .str   = "+CESQ: 99,99,95,40,255,255",
+        .rxlev = 99,
+        .ber   = 99,
+        .rscp  = 95,
+        .ecn0  = 40,
+        .rsrq  = 255,
+        .rsrp  = 255
+    },
+    {
+        .str   = "+CESQ: 10,6,255,255,255,255",
+        .rxlev = 10,
+        .ber   = 6,
+        .rscp  = 255,
+        .ecn0  = 255,
+        .rsrq  = 255,
+        .rsrp  = 255
+    }
+};
+
+static void
+test_cesq_response (void)
+{
+    guint i;
+
+    for (i = 0; i < G_N_ELEMENTS (cesq_response_tests); i++) {
+        GError   *error = NULL;
+        gboolean  success;
+        guint rxlev = G_MAXUINT;
+        guint ber = G_MAXUINT;
+        guint rscp = G_MAXUINT;
+        guint ecn0 = G_MAXUINT;
+        guint rsrq = G_MAXUINT;
+        guint rsrp = G_MAXUINT;
+
+        success = mm_3gpp_parse_cesq_response (cesq_response_tests[i].str,
+                                               &rxlev, &ber,
+                                               &rscp, &ecn0,
+                                               &rsrq, &rsrp,
+                                               &error);
+        g_assert_no_error (error);
+        g_assert (success);
+
+        g_assert_cmpuint (cesq_response_tests[i].rxlev, ==, rxlev);
+        g_assert_cmpuint (cesq_response_tests[i].ber,   ==, ber);
+        g_assert_cmpuint (cesq_response_tests[i].rscp,  ==, rscp);
+        g_assert_cmpuint (cesq_response_tests[i].ecn0,  ==, ecn0);
+        g_assert_cmpuint (cesq_response_tests[i].rsrq,  ==, rsrq);
+        g_assert_cmpuint (cesq_response_tests[i].rsrp,  ==, rsrp);
+    }
+}
+
+/*****************************************************************************/
 
 void
 _mm_log (const char *loc,
@@ -2910,6 +2985,8 @@ int main (int argc, char **argv)
 
     g_test_suite_add (suite, TESTCASE (test_crsm_response, NULL));
 
+    g_test_suite_add (suite, TESTCASE (test_cesq_response, NULL));
+
     result = g_test_run ();
 
     reg_test_data_free (reg_data);