xmm: add +XCESQ parser

3 files changed, 375 insertions, 0 deletions

diff --git a/plugins/xmm/mm-modem-helpers-xmm.c b/plugins/xmm/mm-modem-helpers-xmm.c
index 1c2280af0..6b2e98d36 100644
--- a/plugins/xmm/mm-modem-helpers-xmm.c
+++ b/plugins/xmm/mm-modem-helpers-xmm.c
@@ -18,6 +18,7 @@
 #include "mm-log.h"
 #include "mm-modem-helpers.h"
 #include "mm-modem-helpers-xmm.h"
+#include "mm-signal.h"
 
 /*****************************************************************************/
 /* XACT common config */
@@ -593,3 +594,217 @@ mm_xmm_get_modem_mode_any (const GArray *combinations)
     g_assert (any != MM_MODEM_MODE_NONE);
     return any;
 }
+
+/*****************************************************************************/
+/* +XCESQ? response parser */
+gboolean
+mm_xmm_parse_xcesq_query_response  (const gchar  *response,
+                                    guint        *out_rxlev,
+                                    guint        *out_ber,
+                                    guint        *out_rscp,
+                                    guint        *out_ecn0,
+                                    guint        *out_rsrq,
+                                    guint        *out_rsrp,
+                                    gint         *out_rssnr,
+                                    GError      **error)
+{
+    GRegex     *r;
+    GMatchInfo *match_info;
+    GError     *inner_error = NULL;
+    guint       rxlev = 99;
+    guint       ber = 99;
+    guint       rscp = 255;
+    guint       ecn0 = 255;
+    guint       rsrq = 255;
+    guint       rsrp = 255;
+    gint        rssnr = 255;
+    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);
+    g_assert (out_rssnr);
+
+    /* Response may be e.g.:
+     * +XCESQ: 0,99,99,255,255,24,51,18
+     * +XCESQ: 0,99,99,46,31,255,255,255
+     * +XCESQ: 0,99,99,255,255,17,45,-2
+     */
+    r = g_regex_new ("\\+XCESQ: (\\d+),(\\d+),(\\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)) {
+        /* Ignore "n" value */
+        if (!mm_get_uint_from_match_info (match_info, 2, &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, 3, &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, 4, &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, 5, &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, 6, &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, 7, &rsrp)) {
+            inner_error = g_error_new (MM_CORE_ERROR, MM_CORE_ERROR_FAILED, "Couldn't read RSRP");
+            goto out;
+        }
+        if (!mm_get_int_from_match_info (match_info, 8, &rssnr)) {
+            inner_error = g_error_new (MM_CORE_ERROR, MM_CORE_ERROR_FAILED, "Couldn't read RSSNR");
+            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 +XCESQ response: %s", response);
+        return FALSE;
+    }
+
+    *out_rxlev = rxlev;
+    *out_ber = ber;
+    *out_rscp = rscp;
+    *out_ecn0 = ecn0;
+    *out_rsrq = rsrq;
+    *out_rsrp = rsrp;
+    *out_rssnr = rssnr;
+    return TRUE;
+}
+
+static gboolean
+rssnr_level_to_rssnr (gint     rssnr_level,
+                      gdouble *out_rssnr)
+{
+    if (rssnr_level <= 100 &&
+        rssnr_level >= -100) {
+        *out_rssnr = rssnr_level / 2.0;
+        return TRUE;
+    }
+
+    if (rssnr_level != 255)
+        mm_warn ("unexpected RSSNR level: %u", rssnr_level);
+    return FALSE;
+}
+
+
+/*****************************************************************************/
+/* Get extended signal information */
+gboolean
+mm_xmm_xcesq_response_to_signal_info      (const gchar  *response,
+                                           MMSignal    **out_gsm,
+                                           MMSignal    **out_umts,
+                                           MMSignal    **out_lte,
+                                           GError      **error)
+{
+    guint     rxlev = 0;
+    guint     ber = 0;
+    guint     rscp_level = 0;
+    guint     ecn0_level = 0;
+    guint     rsrq_level = 0;
+    guint     rsrp_level = 0;
+    gint      rssnr_level = 0;
+    gdouble   rssi = MM_SIGNAL_UNKNOWN;
+    gdouble   rscp = MM_SIGNAL_UNKNOWN;
+    gdouble   ecio = MM_SIGNAL_UNKNOWN;
+    gdouble   rsrq = MM_SIGNAL_UNKNOWN;
+    gdouble   rsrp = MM_SIGNAL_UNKNOWN;
+    gdouble   rssnr = MM_SIGNAL_UNKNOWN;
+    MMSignal *gsm = NULL;
+    MMSignal *umts = NULL;
+    MMSignal *lte = NULL;
+
+    if (!mm_xmm_parse_xcesq_query_response (response,
+                                            &rxlev, &ber,
+                                            &rscp_level, &ecn0_level,
+                                            &rsrq_level, &rsrp_level,
+                                            &rssnr_level, error))
+        return FALSE;
+
+    /* GERAN RSSI */
+    if (mm_3gpp_rxlev_to_rssi (rxlev, &rssi)) {
+        gsm = mm_signal_new ();
+        mm_signal_set_rssi (gsm, rssi);
+    }
+
+    /* ignore BER */
+
+    /* UMTS RSCP */
+    if (mm_3gpp_rscp_level_to_rscp (rscp_level, &rscp)) {
+        umts = mm_signal_new ();
+        mm_signal_set_rscp (umts, rscp);
+    }
+
+    /* UMTS EcIo (assumed EcN0) */
+    if (mm_3gpp_ecn0_level_to_ecio (ecn0_level, &ecio)) {
+        if (!umts)
+            umts = mm_signal_new ();
+        mm_signal_set_ecio (umts, ecio);
+    }
+
+    /* Calculate RSSI if we have ecio and rscp */
+    if (umts && ecio != -G_MAXDOUBLE && rscp != -G_MAXDOUBLE)
+    {
+        mm_signal_set_rssi (umts, rscp - ecio);
+    }
+
+    /* LTE RSRQ */
+    if (mm_3gpp_rsrq_level_to_rsrq (rsrq_level, &rsrq)) {
+        lte = mm_signal_new ();
+        mm_signal_set_rsrq (lte, rsrq);
+    }
+
+    /* LTE RSRP */
+    if (mm_3gpp_rsrp_level_to_rsrp (rsrp_level, &rsrp)) {
+        if (!lte)
+            lte = mm_signal_new ();
+        mm_signal_set_rsrp (lte, rsrp);
+    }
+
+    /* LTE RSSNR */
+    if (rssnr_level_to_rssnr (rssnr_level, &rssnr)) {
+        if (!lte)
+            lte = mm_signal_new ();
+        mm_signal_set_snr (lte, rssnr);
+    }
+
+    if (!gsm && !umts && !lte) {
+        g_set_error (error, MM_CORE_ERROR, MM_CORE_ERROR_FAILED,
+                     "Couldn't build detailed signal info");
+        return FALSE;
+    }
+
+    if (gsm)
+        *out_gsm = gsm;
+    if (umts)
+        *out_umts = umts;
+    if (lte)
+        *out_lte = lte;
+
+    return TRUE;
+}
diff --git a/plugins/xmm/mm-modem-helpers-xmm.h b/plugins/xmm/mm-modem-helpers-xmm.h
index abe11905d..8d7157ecd 100644
--- a/plugins/xmm/mm-modem-helpers-xmm.h
+++ b/plugins/xmm/mm-modem-helpers-xmm.h
@@ -39,4 +39,20 @@ gchar *mm_xmm_build_xact_set_command (const MMModemModeCombination  *mode,
 /* Mode to apply when ANY */
 MMModemMode mm_xmm_get_modem_mode_any (const GArray *combinations);
 
+gboolean mm_xmm_parse_xcesq_query_response (const gchar  *response,
+                                            guint        *out_rxlev,
+                                            guint        *out_ber,
+                                            guint        *out_rscp,
+                                            guint        *out_ecn0,
+                                            guint        *out_rsrq,
+                                            guint        *out_rsrp,
+                                            gint         *out_rssnr,
+                                            GError      **error);
+
+gboolean mm_xmm_xcesq_response_to_signal_info  (const gchar  *response,
+                                                MMSignal    **out_gsm,
+                                                MMSignal    **out_umts,
+                                                MMSignal    **out_lte,
+                                                GError      **error);
+
 #endif  /* MM_MODEM_HELPERS_XMM_H */
diff --git a/plugins/xmm/tests/test-modem-helpers-xmm.c b/plugins/xmm/tests/test-modem-helpers-xmm.c
index f560401a7..4c2bbf334 100644
--- a/plugins/xmm/tests/test-modem-helpers-xmm.c
+++ b/plugins/xmm/tests/test-modem-helpers-xmm.c
@@ -16,6 +16,7 @@
 #include <glib.h>
 #include <glib-object.h>
 #include <locale.h>
+#include <math.h>
 
 #include <ModemManager.h>
 #define _LIBMM_INSIDE_MM
@@ -25,6 +26,9 @@
 #include "mm-modem-helpers.h"
 #include "mm-modem-helpers-xmm.h"
 
+#define g_assert_cmpfloat_tolerance(val1, val2, tolerance)  \
+    g_assert_cmpfloat (fabs (val1 - val2), <, tolerance)
+
 /*****************************************************************************/
 /* Test XACT=? responses */
 
@@ -508,6 +512,143 @@ test_xact_set (void)
 }
 
 /*****************************************************************************/
+/* Test +XCESQ responses */
+
+typedef struct {
+    const gchar *str;
+
+    gboolean gsm_info;
+    guint    rxlev;
+    gdouble  rssi;
+    guint    ber;
+
+    gboolean umts_info;
+    guint    rscp_level;
+    gdouble  rscp;
+    guint    ecn0_level;
+    gdouble  ecio;
+
+    gboolean lte_info;
+    guint    rsrq_level;
+    gdouble  rsrq;
+    guint    rsrp_level;
+    gdouble  rsrp;
+    gint     rssnr_level;
+    gdouble  rssnr;
+} XCesqResponseTest;
+
+static const XCesqResponseTest xcesq_response_tests[] = {
+    {
+        .str       = "+XCESQ: 0,99,99,255,255,19,46,32",
+        .gsm_info  = FALSE, .rxlev = 99, .ber = 99,
+        .umts_info = FALSE, .rscp_level = 255, .ecn0_level = 255,
+        .lte_info  = TRUE,  .rsrq_level = 19, .rsrq = -10.5, .rsrp_level = 46, .rsrp = -95.0, .rssnr_level = 32, .rssnr = 16.0
+    },
+    {
+        .str       = "+XCESQ: 0,99,99,255,255,19,46,-32",
+        .gsm_info  = FALSE, .rxlev = 99, .ber = 99,
+        .umts_info = FALSE, .rscp_level = 255, .ecn0_level = 255,
+        .lte_info  = TRUE,  .rsrq_level = 19, .rsrq = -10.5, .rsrp_level = 46, .rsrp = -95.0, .rssnr_level = -32, .rssnr = -16.0
+    },
+    {
+        .str       = "+XCESQ: 0,99,99,255,255,16,47,28",
+        .gsm_info  = FALSE, .rxlev = 99, .ber = 99,
+        .umts_info = FALSE, .rscp_level = 255, .ecn0_level = 255,
+        .lte_info  = TRUE,  .rsrq_level = 16, .rsrq = -12.0, .rsrp_level = 47, .rsrp = -94.0, .rssnr_level = 28, .rssnr = 14.0
+    },
+    {
+        .str       = "+XCESQ: 0,99,99,41,29,255,255,255",
+        .gsm_info  = FALSE, .rxlev = 99, .ber = 99,
+        .umts_info = TRUE,  .rscp_level = 41, .rscp = -80.0, .ecn0_level = 29, .ecio = -10.0,
+        .lte_info  = FALSE, .rsrq_level = 255, .rsrp_level = 255, .rssnr_level = 255
+    },
+    {
+        .str       = "+XCESQ: 0,10,6,255,255,255,255,255",
+        .gsm_info  = TRUE,  .rxlev = 10, .rssi = -101.0, .ber = 6,
+        .umts_info = FALSE, .rscp_level = 255, .ecn0_level = 255,
+        .lte_info  = FALSE, .rsrq_level = 255, .rsrp_level = 255, .rssnr_level = 255
+    }
+};
+
+static void
+test_xcesq_response (void)
+{
+    guint i;
+
+    for (i = 0; i < G_N_ELEMENTS (xcesq_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;
+        gint rssnr = G_MAXUINT;
+
+        success = mm_xmm_parse_xcesq_query_response (xcesq_response_tests[i].str,
+                                                    &rxlev, &ber,
+                                                    &rscp, &ecn0,
+                                                    &rsrq, &rsrp,
+                                                    &rssnr, &error);
+        g_assert_no_error (error);
+        g_assert (success);
+
+        g_assert_cmpuint (xcesq_response_tests[i].rxlev,      ==, rxlev);
+        g_assert_cmpuint (xcesq_response_tests[i].ber,        ==, ber);
+        g_assert_cmpuint (xcesq_response_tests[i].rscp_level, ==, rscp);
+        g_assert_cmpuint (xcesq_response_tests[i].ecn0_level, ==, ecn0);
+        g_assert_cmpuint (xcesq_response_tests[i].rsrq_level, ==, rsrq);
+        g_assert_cmpuint (xcesq_response_tests[i].rsrp_level, ==, rsrp);
+        g_assert_cmpuint (xcesq_response_tests[i].rssnr_level, ==, rssnr);
+    }
+}
+
+static void
+test_xcesq_response_to_signal (void)
+{
+    guint i;
+
+    for (i = 0; i < G_N_ELEMENTS (xcesq_response_tests); i++) {
+        GError   *error = NULL;
+        gboolean  success;
+        MMSignal *gsm  = NULL;
+        MMSignal *umts = NULL;
+        MMSignal *lte  = NULL;
+
+        success = mm_xmm_xcesq_response_to_signal_info (xcesq_response_tests[i].str,
+                                                        &gsm, &umts, &lte,
+                                                        &error);
+        g_assert_no_error (error);
+        g_assert (success);
+
+        if (xcesq_response_tests[i].gsm_info) {
+            g_assert (gsm);
+            g_assert_cmpfloat_tolerance (mm_signal_get_rssi (gsm), xcesq_response_tests[i].rssi, 0.1);
+            g_object_unref (gsm);
+        } else
+            g_assert (!gsm);
+
+        if (xcesq_response_tests[i].umts_info) {
+            g_assert (umts);
+            g_assert_cmpfloat_tolerance (mm_signal_get_rscp (umts), xcesq_response_tests[i].rscp, 0.1);
+            g_assert_cmpfloat_tolerance (mm_signal_get_ecio (umts), xcesq_response_tests[i].ecio, 0.1);
+            g_object_unref (umts);
+        } else
+            g_assert (!umts);
+
+        if (xcesq_response_tests[i].lte_info) {
+            g_assert (lte);
+            g_assert_cmpfloat_tolerance (mm_signal_get_rsrq (lte), xcesq_response_tests[i].rsrq, 0.1);
+            g_assert_cmpfloat_tolerance (mm_signal_get_rsrp (lte), xcesq_response_tests[i].rsrp, 0.1);
+            g_assert_cmpfloat_tolerance (mm_signal_get_snr (lte), xcesq_response_tests[i].rssnr, 0.1);
+            g_object_unref (lte);
+        } else
+            g_assert (!lte);
+    }
+}
+
+/*****************************************************************************/
 
 void
 _mm_log (const char *loc,
@@ -544,5 +685,8 @@ int main (int argc, char **argv)
 
     g_test_add_func ("/MM/xmm/xact/set", test_xact_set);
 
+    g_test_add_func ("/MM/xmm/xcesq/query_response", test_xcesq_response);
+    g_test_add_func ("/MM/xmm/xcesq/query_response_to_signal", test_xcesq_response_to_signal);
+
     return g_test_run ();
 }