From 43febf13a47421f810c65550c125d5bd2895ad6d Mon Sep 17 00:00:00 2001
From: Alexander Couzens <lynxis@fe80.eu>
Date: Tue, 25 Jul 2017 20:36:00 +0200
Subject: [PATCH] qmimodem: sync the modem on enable

The qmi sync call release all previous resources.
qmi/discovery: only do the sync call when ctl service version >= 1.5
---
 drivers/qmimodem/qmi.c | 47 ++++++++++++++++++++++++++++++++++++++++++
 drivers/qmimodem/qmi.h |  5 ++++-
 plugins/gobi.c         | 32 ++++++++++++++++++++++++++--
 3 files changed, 81 insertions(+), 3 deletions(-)

diff --git a/drivers/qmimodem/qmi.c b/drivers/qmimodem/qmi.c
index 96c70d17..08dbc3df 100644
--- a/drivers/qmimodem/qmi.c
+++ b/drivers/qmimodem/qmi.c
@@ -1316,6 +1316,53 @@ bool qmi_device_shutdown(struct qmi_device *device, qmi_shutdown_func_t func,
 	return true;
 }
 
+struct sync_data {
+	qmi_sync_func_t func;
+	void *user_data;
+};
+
+static void qmi_device_sync_callback(uint16_t message, uint16_t length,
+				     const void *buffer, void *user_data)
+{
+	struct sync_data *data = user_data;
+
+	if(data->func)
+		data->func(data->user_data);
+
+	g_free(data);
+}
+
+/* sync will release all previous clients */
+bool qmi_device_sync(struct qmi_device *device,
+		     qmi_sync_func_t func, void *user_data)
+{
+	struct qmi_request *req;
+	struct qmi_control_hdr *hdr;
+	struct sync_data *func_data;
+
+	if (!device)
+		return false;
+
+	func_data = g_new0(struct sync_data, 1);
+	func_data->func = func;
+	func_data->user_data = user_data;
+
+	req = __request_alloc(QMI_SERVICE_CONTROL, 0x00,
+			QMI_CTL_SYNC, QMI_CONTROL_HDR_SIZE,
+			NULL, 0,
+			qmi_device_sync_callback, func_data, (void **) &hdr);
+
+	if (device->next_control_tid < 1)
+		device->next_control_tid = 1;
+
+	hdr->type = 0x00;
+	hdr->transaction = device->next_control_tid++;
+
+	__request_submit(device, req, hdr->transaction);
+
+	return true;
+}
+
 static bool get_device_file_name(struct qmi_device *device,
 					char *file_name, int size)
 {
diff --git a/drivers/qmimodem/qmi.h b/drivers/qmimodem/qmi.h
index 71d9fc88..1229ab13 100644
--- a/drivers/qmimodem/qmi.h
+++ b/drivers/qmimodem/qmi.h
@@ -79,7 +79,7 @@ typedef void (*qmi_destroy_func_t)(void *user_data);
 struct qmi_device;
 
 typedef void (*qmi_debug_func_t)(const char *str, void *user_data);
-
+typedef void (*qmi_sync_func_t)(void *user_data);
 typedef void (*qmi_shutdown_func_t)(void *user_data);
 typedef void (*qmi_discover_func_t)(uint8_t count,
 			const struct qmi_version *list, void *user_data);
@@ -99,6 +99,9 @@ bool qmi_device_discover(struct qmi_device *device, qmi_discover_func_t func,
 bool qmi_device_shutdown(struct qmi_device *device, qmi_shutdown_func_t func,
 				void *user_data, qmi_destroy_func_t destroy);
 
+bool qmi_device_sync(struct qmi_device *device,
+		     qmi_sync_func_t func, void *user_data);
+
 enum qmi_device_expected_data_format qmi_device_get_expected_data_format(
 						struct qmi_device *device);
 bool qmi_device_set_expected_data_format(struct qmi_device *device,
diff --git a/plugins/gobi.c b/plugins/gobi.c
index a4985990..0c621ca2 100644
--- a/plugins/gobi.c
+++ b/plugins/gobi.c
@@ -252,6 +252,15 @@ error:
 	shutdown_device(modem);
 }
 
+static void create_shared_dms(void *user_data)
+{
+	struct ofono_modem *modem = user_data;
+	struct gobi_data *data = ofono_modem_get_data(modem);
+
+	qmi_service_create_shared(data->device, QMI_SERVICE_DMS,
+				  create_dms_cb, modem, NULL);
+}
+
 static void discover_cb(uint8_t count, const struct qmi_version *list,
 							void *user_data)
 {
@@ -266,6 +275,13 @@ static void discover_cb(uint8_t count, const struct qmi_version *list,
 				list[i].type);
 
 		switch (list[i].type) {
+		case QMI_SERVICE_CONTROL:
+			/* sync command resets the QMI state */
+			if (list[i].major > 1 || list[i].minor >= 5)
+				ofono_modem_set_boolean(modem,
+							"SupportQMISync",
+							TRUE);
+			break;
 		case QMI_SERVICE_DMS:
 			data->features |= GOBI_DMS;
 			break;
@@ -314,8 +330,20 @@ static void discover_cb(uint8_t count, const struct qmi_version *list,
 		return;
 	}
 
-	qmi_service_create_shared(data->device, QMI_SERVICE_DMS,
-						create_dms_cb, modem, NULL);
+	if (ofono_modem_get_boolean(modem, "SupportQMISync") == TRUE)
+		qmi_device_sync(data->device, create_shared_dms, modem);
+	else
+		create_shared_dms(modem);
+}
+
+static void sync_cb(void *user_data)
+{
+	struct ofono_modem *modem = user_data;
+	struct gobi_data *data = ofono_modem_get_data(modem);
+
+	DBG("modem in sync");
+
+	qmi_device_discover(data->device, discover_cb, modem, NULL);
 }
 
 static int gobi_enable(struct ofono_modem *modem)