netreg: Query and select supported CMER modes

1 files changed, 151 insertions, 2 deletions

diff --git a/drivers/atmodem/network-registration.c b/drivers/atmodem/network-registration.c
index 3d099135..71606794 100644
--- a/drivers/atmodem/network-registration.c
+++ b/drivers/atmodem/network-registration.c
@@ -47,6 +47,7 @@ static const char *creg_prefix[] = { "+CREG:", NULL };
 static const char *cops_prefix[] = { "+COPS:", NULL };
 static const char *csq_prefix[] = { "+CSQ:", NULL };
 static const char *cind_prefix[] = { "+CIND:", NULL };
+static const char *cmer_prefix[] = { "+CMER:", NULL };
 static const char *zpas_prefix[] = { "+ZPAS:", NULL };
 static const char *option_tech_prefix[] = { "_OCTI:", "_OUWCTI:", NULL };
 
@@ -1395,6 +1396,154 @@ notify:
 	ofono_netreg_status_notify(netreg, status, lac, ci, tech);
 }
 
+static void at_cmer_not_supported(struct ofono_netreg *netreg)
+{
+	ofono_error("+CMER not supported by this modem.  If this is an error"
+			" please submit patches to support this hardware");
+}
+
+static void at_cmer_set_cb(gboolean ok, GAtResult *result, gpointer user_data)
+{
+	struct ofono_netreg *netreg = user_data;
+
+	if (!ok)
+		at_cmer_not_supported(netreg);
+}
+
+static inline char wanted_cmer(int supported, const char *pref)
+{
+	while (*pref) {
+		if (supported & (1 << (*pref - '0')))
+			return *pref;
+
+		pref++;
+	}
+
+	return '\0';
+}
+
+static inline ofono_bool_t append_cmer_element(char *buf, int *len, int cap,
+						const char *wanted,
+						ofono_bool_t last)
+{
+	char setting = wanted_cmer(cap, wanted);
+
+	if (!setting)
+		return FALSE;
+
+	buf[*len] = setting;
+
+	if (last)
+		buf[*len + 1] = '\0';
+	else
+		buf[*len + 1] = ',';
+
+	*len += 2;
+
+	return TRUE;
+}
+
+static ofono_bool_t build_cmer_string(char *buf, int *cmer_opts,
+					struct netreg_data *nd)
+{
+	const char *mode;
+	int len = sprintf(buf, "AT+CMER=");
+
+	DBG("");
+
+	/*
+	 * Forward unsolicited result codes directly to the TE;
+	 * TA‑TE link specific inband technique used to embed result codes and
+	 * data when TA is in on‑line data mode
+	 */
+	if (!append_cmer_element(buf, &len, cmer_opts[0], "3", FALSE))
+		return FALSE;
+
+	/* No keypad event reporting */
+	if (!append_cmer_element(buf, &len, cmer_opts[1], "0", FALSE))
+		return FALSE;
+
+	/* No display event reporting */
+	if (!append_cmer_element(buf, &len, cmer_opts[2], "0", FALSE))
+		return FALSE;
+
+	switch (nd->vendor) {
+	case OFONO_VENDOR_TELIT:
+		/*
+		 * Telit does not support mode 1.
+		 * All indicator events shall be directed from TA to TE.
+		 */
+		mode = "2";
+		break;
+	default:
+		/*
+		 * Only those indicator events, which are not caused by +CIND
+		 * shall be indicated by the TA to the TE.
+		 */
+		mode = "1";
+		break;
+	}
+
+	/*
+	 * Indicator event reporting using URC +CIEV: <ind>,<value>.
+	 * <ind> indicates the indicator order number (as specified for +CIND)
+	 * and <value> is the new value of indicator.
+	 */
+	if (!append_cmer_element(buf, &len, cmer_opts[3], mode, TRUE))
+		return FALSE;
+
+	return TRUE;
+}
+
+static void at_cmer_query_cb(ofono_bool_t ok, GAtResult *result,
+				gpointer user_data)
+{
+	struct ofono_netreg *netreg = user_data;
+	struct netreg_data *nd = ofono_netreg_get_data(netreg);
+	GAtResultIter iter;
+	int cmer_opts_cnt = 5; /* See 27.007 Section 8.10 */
+	int cmer_opts[cmer_opts_cnt];
+	int opt;
+	int mode;
+	char buf[128];
+
+	if (!ok)
+		goto error;
+
+	memset(cmer_opts, 0, sizeof(cmer_opts));
+
+	g_at_result_iter_init(&iter, result);
+
+	if (!g_at_result_iter_next(&iter, "+CMER:"))
+		goto error;
+
+	for (opt = 0; opt < cmer_opts_cnt; opt++) {
+		int min, max;
+
+		if (!g_at_result_iter_open_list(&iter))
+			goto error;
+
+		while (g_at_result_iter_next_range(&iter, &min, &max)) {
+			for (mode = min; mode <= max; mode++)
+				cmer_opts[opt] |= 1 << mode;
+		}
+
+		if (!g_at_result_iter_close_list(&iter))
+			goto error;
+	}
+
+	if (build_cmer_string(buf, cmer_opts, nd) == FALSE)
+		goto error;
+
+	g_at_chat_send(nd->chat, buf, cmer_prefix,
+			at_cmer_set_cb, netreg, NULL);
+
+	return;
+
+error:
+	at_cmer_not_supported(netreg);
+}
+
 static void cind_support_cb(gboolean ok, GAtResult *result, gpointer user_data)
 {
 	struct ofono_netreg *netreg = user_data;
@@ -1465,8 +1614,8 @@ static void cind_support_cb(gboolean ok, GAtResult *result, gpointer user_data)
 	if (nd->signal_index == 0)
 		goto error;
 
-	g_at_chat_send(nd->chat, "AT+CMER=3,0,0,1", NULL,
-			NULL, NULL, NULL);
+	g_at_chat_send(nd->chat, "AT+CMER=?", cmer_prefix,
+				at_cmer_query_cb, netreg, NULL);
 	g_at_chat_register(nd->chat, "+CIEV:",
 				ciev_notify, FALSE, netreg, NULL);
 	g_at_chat_register(nd->chat, "+CREG:",