Implement operator selection (AT+COPS) support

3 files changed, 86 insertions, 34 deletions

diff --git a/audio/headset.c b/audio/headset.c
index 6599e1d7..9c575fc0 100644
--- a/audio/headset.c
+++ b/audio/headset.c
@@ -529,6 +529,41 @@ static int sco_connect(struct audio_device *dev, headset_stream_cb_t cb,
 	return 0;
 }
 
+static int hfp_cmp(struct headset *hs)
+{
+	if (hs->hfp_active)
+		return 0;
+	else
+		return -1;
+}
+
+static void send_foreach_headset(GSList *devices,
+					int (*cmp)(struct headset *hs),
+					char *format, ...)
+{
+	GSList *l;
+	va_list ap;
+
+	for (l = devices; l != NULL; l = l->next) {
+		struct audio_device *device = l->data;
+		struct headset *hs = device->headset;
+		int ret;
+
+		assert(hs != NULL);
+
+		if (cmp && cmp(hs) != 0)
+			continue;
+
+		va_start(ap, format);
+		ret = headset_send_valist(hs, format, ap);
+		if (ret < 0)
+			error("Failed to send to headset: %s (%d)",
+					strerror(-ret), -ret);
+		va_end(ap);
+	}
+}
+
+
 static void hfp_slc_complete(struct audio_device *dev)
 {
 	struct headset *hs = dev->headset;
@@ -934,6 +969,41 @@ static int call_waiting_notify(struct audio_device *device, const char *buf)
 	return headset_send(hs, "\r\nOK\r\n");
 }
 
+int telephony_operator_selection_rsp(void *telephony_device, cme_error_t err)
+{
+	return telephony_generic_rsp(telephony_device, err);
+}
+
+int telephony_operator_selection_ind(int mode, const char *oper)
+{
+	if (!active_devices)
+		return -ENODEV;
+
+	send_foreach_headset(active_devices, hfp_cmp, "\r\n+COPS:%d,0,%s\r\n",
+				mode, oper);
+	return 0;
+}
+
+static int operator_selection(struct audio_device *device, const char *buf)
+{
+	struct headset *hs = device->headset;
+
+	if (strlen(buf) < 8)
+		return -EINVAL;
+
+	switch (buf[7]) {
+	case '?':
+		telephony_operator_selection_req(device);
+		break;
+	case '=':
+		return headset_send(hs, "\r\nOK\r\n");
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static struct event event_callbacks[] = {
 	{ "ATA", answer_call },
 	{ "ATD", dial_number },
@@ -952,6 +1022,7 @@ static struct event event_callbacks[] = {
 	{ "AT+CLCC", list_current_calls },
 	{ "AT+CMEE", extended_errors },
 	{ "AT+CCWA", call_waiting_notify },
+	{ "AT+COPS", operator_selection },
 	{ 0 }
 };
 
@@ -1393,40 +1464,6 @@ static DBusMessage *hs_connect(DBusConnection *conn, DBusMessage *msg,
 	return NULL;
 }
 
-static int hfp_cmp(struct headset *hs)
-{
-	if (hs->hfp_active)
-		return 0;
-	else
-		return -1;
-}
-
-static void send_foreach_headset(GSList *devices,
-					int (*cmp)(struct headset *hs),
-					char *format, ...)
-{
-	GSList *l;
-	va_list ap;
-
-	for (l = devices; l != NULL; l = l->next) {
-		struct audio_device *device = l->data;
-		struct headset *hs = device->headset;
-		int ret;
-
-		assert(hs != NULL);
-
-		if (cmp && cmp(hs) != 0)
-			continue;
-
-		va_start(ap, format);
-		ret = headset_send_valist(hs, format, ap);
-		if (ret < 0)
-			error("Failed to send to headset: %s (%d)",
-					strerror(-ret), -ret);
-		va_end(ap);
-	}
-}
-
 static int cli_cmp(struct headset *hs)
 {
 	if (!hs->hfp_active)
diff --git a/audio/telephony-dummy.c b/audio/telephony-dummy.c
index 976150f3..8b3f105d 100644
--- a/audio/telephony-dummy.c
+++ b/audio/telephony-dummy.c
@@ -177,6 +177,12 @@ void telephony_list_current_calls_req(void *telephony_device)
 	telephony_list_current_calls_rsp(telephony_device, CME_ERROR_NONE);
 }
 
+void telephony_operator_selection_req(void *telephony_device)
+{
+	telephony_operator_selection_ind(OPERATOR_MODE_AUTO, "DummyOperator");
+	telephony_operator_selection_rsp(telephony_device, CME_ERROR_NONE);
+}
+
 /* D-Bus method handlers */
 static DBusMessage *outgoing_call(DBusConnection *conn, DBusMessage *msg,
 					void *data)
diff --git a/audio/telephony.h b/audio/telephony.h
index 55d327b7..95434093 100644
--- a/audio/telephony.h
+++ b/audio/telephony.h
@@ -86,6 +86,12 @@
 #define SUBSCRIBER_SERVICE_VOICE	4
 #define SUBSCRIBER_SERVICE_FAX		5
 
+/* Operator selection mode values */
+#define OPERATOR_MODE_AUTO		0
+#define OPERATOR_MODE_MANUAL		1
+#define OPERATOR_MODE_DEREGISTER	2
+#define OPERATOR_MODE_MANUAL_AUTO	4
+
 /* Extended Audio Gateway Error Result Codes */
 typedef enum {
 	CME_ERROR_NONE			= -1,
@@ -137,6 +143,7 @@ void telephony_dial_number_req(void *telephony_device, const char *number);
 void telephony_transmit_dtmf_req(void *telephony_device, char tone);
 void telephony_subscriber_number_req(void *telephony_device);
 void telephony_list_current_calls_req(void *telephony_device);
+void telephony_operator_selection_req(void *telephony_device);
 
 /* AG responses to HF requests. These are implemented by headset.c */
 int telephony_event_reporting_rsp(void *telephony_device, cme_error_t err);
@@ -148,6 +155,7 @@ int telephony_dial_number_rsp(void *telephony_device, cme_error_t err);
 int telephony_transmit_dtmf_rsp(void *telephony_device, cme_error_t err);
 int telephony_subscriber_number_rsp(void *telephony_device, cme_error_t err);
 int telephony_list_current_calls_rsp(void *telephony_device, cme_error_t err);
+int telephony_operator_selection_rsp(void *telephony_device, cme_error_t err);
 
 /* Event indications by AG. These are implemented by headset.c */
 int telephony_event_ind(int index);
@@ -162,6 +170,7 @@ int telephony_list_current_call_ind(int idx, int dir, int status, int mode,
 int telephony_subscriber_number_ind(const char *number, int type,
 					int service);
 int telephony_call_waiting_ind(const char *number, int type);
+int telephony_operator_selection_ind(int mode, const char *oper);
 
 /* Helper function for quick indicator updates */
 static inline int telephony_update_indicator(struct indicator *indicators,