multi-sim support verification

Signed-off-by: Loic Poulain <loic.poulain@linaro.org>

3 files changed, 115 insertions, 60 deletions

diff --git a/mhi-qmi-connect b/mhi-qmi-connect
index b7cf0c1..1c60a3a 100755
--- a/mhi-qmi-connect
+++ b/mhi-qmi-connect
diff --git a/mhi-qmi-connect.c b/mhi-qmi-connect.c
index bb968cf..43a069e 100644
--- a/mhi-qmi-connect.c
+++ b/mhi-qmi-connect.c
@@ -12,7 +12,6 @@
 #define MAX_QMAP_DATAGRAM_SIZE 16384
 #define MAX_QMAP_AGGREGATED_DATAGRAM 32
 #define IFACE_ID 4
-#define MUX_ID 1
 #define WWAN_IFACE "mhi_hwip0"
 
 #define MHI_CONFIG_DEFAULT "/etc/mhi-qmi-connect.conf"
@@ -33,20 +32,31 @@ static QmiClientUim *client_uim;
 static QmiWdsConnectionStatus connection_status;
 static guint32 packet_data_handle;
 
-static gchar *apn_str;
-static gchar *sim1_pin_str;
-static gboolean default_route;
-static gboolean set_ip;
-static guint32 mux_id = 1;
+struct config_struct {
+	gboolean sim1_enable;
+	gchar *sim1_pin_str;
+
+	gboolean sim2_enable;
+	gchar *sim2_pin_str;
+
+	gchar *apn_str;
+	guint32 mux_id;
+	gboolean set_ip;
+	gboolean default_route;
+};
+
+static struct config_struct config;
+
 static gchar *qmi_dev;
 static gboolean udev;
 static gboolean kmsg;
-static gchar *config;
+static gchar *config_path;
 
 static gchar ip4_addr[INET_ADDRSTRLEN];
 static gchar ip4_dns[INET_ADDRSTRLEN];
 static gboolean rmnet_setup_done;
 static gchar *ip_route_dump;
+static gboolean wda_allocated = FALSE;
 static gboolean ping;
 
 static int exit_reason;
@@ -102,7 +112,7 @@ static void stop_rmnet(void)
 		iptool = iptool_alt;
 
 	/* Set RMNET interface down */
-	g_snprintf(cmdline, sizeof(cmdline), "%s link set rmnet_data%u down", iptool, mux_id);
+	g_snprintf(cmdline, sizeof(cmdline), "%s link set rmnet_data%u down", iptool, config.mux_id);
 	if (!g_spawn_command_line_sync(cmdline, NULL, NULL, NULL, &error)) {
 		g_printerr("%s: %s\n", cmdline, error->message);
 		g_error_free(error);
@@ -118,14 +128,14 @@ static void stop_rmnet(void)
 	}
 	
 	/* del RMNET interface */
-	g_snprintf(cmdline, sizeof(cmdline), "%s link delete rmnet_data%u", iptool, mux_id);
+	g_snprintf(cmdline, sizeof(cmdline), "%s link delete rmnet_data%u", iptool, config.mux_id);
 	if (!g_spawn_command_line_sync(cmdline, NULL, NULL, NULL, &error)) {
 		g_printerr("%s: %s\n", cmdline, error->message);
 		g_error_free(error);
 		return;
 	}
 	
-	if (!default_route)
+	if (!config.default_route)
 		return;
 
 	g_snprintf(cmdline, sizeof(cmdline), "%s route del default",iptool);
@@ -292,19 +302,19 @@ static void setup_rmnet(void)
 
 	/* Add link */
 	g_snprintf(cmdline, sizeof(cmdline), "%s link add link %s name rmnet_data%u type rmnet mux_id %u",
-		   iptool, WWAN_IFACE, mux_id, mux_id);
+		   iptool, WWAN_IFACE, config.mux_id, config.mux_id);
 	if (!g_spawn_command_line_sync(cmdline, NULL, NULL, NULL, &error)) {
 		g_printerr("%s: %s\n", cmdline, error->message);
 		g_error_free(error);
 		op_shutdown(SHUTDOWN_ERROR);
 		return;
 	}
-	g_print("[%s] [RMNET] <rmnet_data%u> interface created\n", qmi_device_get_path_display(device), mux_id);
+	g_print("[%s] [RMNET] <rmnet_data%u> interface created\n", qmi_device_get_path_display(device), config.mux_id);
 
-	if (set_ip) {
+	if (config.set_ip) {
 		/* Set RMNET IP address */
 		g_snprintf(cmdline, sizeof(cmdline), "%s addr add %s/27 dev rmnet_data%u",
-			   iptool, ip4_addr, mux_id);
+			   iptool, ip4_addr, config.mux_id);
 		if (!g_spawn_command_line_sync(cmdline, NULL, NULL, NULL, &error)) {
 			g_printerr("%s: %s\n", cmdline, error->message);
 			g_error_free(error);
@@ -312,12 +322,12 @@ static void setup_rmnet(void)
 			return;
 		}
 		g_print("[%s] [RMNET] <rmnet_data%u> address set to %s\n",
-			qmi_device_get_path_display(device), mux_id, ip4_addr);
+			qmi_device_get_path_display(device), config.mux_id, ip4_addr);
 	}
 	
 	/* Set RMNET MTU */
 	g_snprintf(cmdline, sizeof(cmdline), "%s link set rmnet_data%u mtu %u",
-		   iptool, mux_id, MAX_QMAP_DATAGRAM_SIZE);
+		   iptool, config.mux_id, MAX_QMAP_DATAGRAM_SIZE);
 	if (!g_spawn_command_line_sync(cmdline, NULL, NULL, NULL, &error)) {
 		g_printerr("%s: %s\n", cmdline, error->message);
 		g_error_free(error);
@@ -335,7 +345,7 @@ static void setup_rmnet(void)
 	}
 
 	/* Set RMNET interface up */
-	g_snprintf(cmdline, sizeof(cmdline), "%s link set rmnet_data%u up", iptool, mux_id);
+	g_snprintf(cmdline, sizeof(cmdline), "%s link set rmnet_data%u up", iptool, config.mux_id);
 	if (!g_spawn_command_line_sync(cmdline, NULL, NULL, NULL, &error)) {
 		g_printerr("%s: %s\n", cmdline, error->message);
 		g_error_free(error);
@@ -345,11 +355,11 @@ static void setup_rmnet(void)
 
 	rmnet_setup_done = TRUE;
 
-	if (!default_route)
+	if (!config.default_route)
 		return;
 
 	g_print("[%s] [RMNET] <rmnet_data%u> as default route\n",
-			qmi_device_get_path_display(device), mux_id);
+			qmi_device_get_path_display(device), config.mux_id);
 
 	/* Save routing */
 	g_snprintf(cmdline, sizeof(cmdline), "%s route save", iptool);
@@ -463,7 +473,7 @@ static void start_network_ready(QmiClientWds *client, GAsyncResult *res)
 	qmi_message_wds_start_network_output_get_packet_data_handle(output, &packet_data_handle, NULL);
 	qmi_message_wds_start_network_output_unref(output);
 
-	g_print("[%s] [WDS] Network Started!\n", qmi_device_get_path_display(device));
+	g_print("[%s] [WDS] Network Started! (handle: %u)\n", qmi_device_get_path_display(device), packet_data_handle);
 
 	input = qmi_message_wds_get_current_settings_input_new();
 	qmi_message_wds_get_current_settings_input_set_requested_settings(input,
@@ -490,11 +500,11 @@ static gboolean start_network(gpointer user_data)
 {
 	QmiMessageWdsStartNetworkInput *input;
 
-	g_print("[%s] [WDS] Starting Network...\n", qmi_device_get_path_display(device));
+	g_print("[%s] [WDS] Starting network connection to %s...\n", qmi_device_get_path_display(device), config.apn_str);
 
 	input = qmi_message_wds_start_network_input_new();
 
-	qmi_message_wds_start_network_input_set_apn(input, apn_str, NULL);
+	qmi_message_wds_start_network_input_set_apn(input, config.apn_str, NULL);
 
 	qmi_client_wds_start_network(client_wds, input, 45, cancellable,
 								 (GAsyncReadyCallback)start_network_ready, NULL);
@@ -553,7 +563,7 @@ static void allocate_wds_client_ready(QmiDevice *dev, GAsyncResult *res)
 	qmi_message_wds_bind_mux_data_port_input_set_endpoint_info(
 		input, QMI_DATA_ENDPOINT_TYPE_PCIE, IFACE_ID, &error);
 	qmi_message_wds_bind_mux_data_port_input_set_mux_id(
-		input, MUX_ID, &error);
+		input, config.mux_id, &error);
 
 	qmi_client_wds_bind_mux_data_port(client_wds, input, 10, cancellable,
 					   (GAsyncReadyCallback) bind_mux_data_port_ready, NULL);
@@ -665,16 +675,22 @@ static void verify_pin_ready(QmiClientUim *client, GAsyncResult *res)
 	g_print("[%s] [UIM] PIN verified successfully\n", qmi_device_get_path_display(device));
 
 	qmi_message_uim_verify_pin_output_unref(output);
-    
+	
+	if (!wda_allocated)
+		wda_allocated = TRUE;
+	else
+		return;
+
 	qmi_device_allocate_client(device, service_wda, QMI_CID_NONE, 10, cancellable,
 				(GAsyncReadyCallback)allocate_wda_client_ready, NULL);
 }
 
 /* 6.2 - SIM powered on, Set pin code */
-static void power_on_sim_ready(QmiClientUim *client, GAsyncResult *res)
+static void power_on_sim_ready(QmiClientUim *client, GAsyncResult *res, gpointer user_data)
 {
 	QmiMessageUimVerifyPinInput *input = NULL;
 	QmiMessageUimPowerOnSimOutput *output;
+	guint32 slot = user_data;
 	GError *error = NULL;
 	GArray *dummy_aid;
 
@@ -688,7 +704,7 @@ static void power_on_sim_ready(QmiClientUim *client, GAsyncResult *res)
 
 	if (!qmi_message_uim_power_on_sim_output_get_result(output, &error) &&
 	    error->code != QMI_PROTOCOL_ERROR_NO_EFFECT) {
-		g_printerr("error: could not power on SIM: %s\n", error->message);
+		g_printerr("error: could not power on SIM%u: %s\n", slot, error->message);
 		g_error_free(error);
 		qmi_message_uim_power_on_sim_output_unref(output);
 		op_shutdown(SHUTDOWN_ERROR);
@@ -697,15 +713,30 @@ static void power_on_sim_ready(QmiClientUim *client, GAsyncResult *res)
 	
 	qmi_message_uim_power_on_sim_output_unref(output);
 	
-	g_print("[%s] [UIM] Verifying SIM1 PIN...\n", qmi_device_get_path_display(device));
+	g_print("[%s] [UIM] Verifying SIM%u PIN...\n", qmi_device_get_path_display(device), slot);
 	
 	input = qmi_message_uim_verify_pin_input_new();
 	
 	dummy_aid = g_array_new(FALSE, FALSE, sizeof(guint8));
 
-	qmi_message_uim_verify_pin_input_set_info(input, 1, sim1_pin_str, &error);
-	qmi_message_uim_verify_pin_input_set_session(input, QMI_UIM_SESSION_TYPE_CARD_SLOT_1,
+	if (slot == 1 && config.sim1_pin_str) {
+		qmi_message_uim_verify_pin_input_set_info(input, 1, config.sim1_pin_str, &error);
+		qmi_message_uim_verify_pin_input_set_session(input, QMI_UIM_SESSION_TYPE_CARD_SLOT_1,
 						       dummy_aid, /* ignored */ &error);
+	} else if (slot == 2 && config.sim2_pin_str) {
+		qmi_message_uim_verify_pin_input_set_info(input, 1, config.sim2_pin_str, &error);
+		qmi_message_uim_verify_pin_input_set_session(input, QMI_UIM_SESSION_TYPE_CARD_SLOT_2,
+						       dummy_aid, /* ignored */ &error);
+	} else if (!config.sim1_pin_str && !config.sim2_pin_str) {
+		g_array_unref(dummy_aid);
+		qmi_message_uim_verify_pin_input_unref(input);
+		if (!wda_allocated) {
+			wda_allocated = TRUE;
+			qmi_device_allocate_client(device, service_wda, QMI_CID_NONE, 10, cancellable,
+							(GAsyncReadyCallback)allocate_wda_client_ready, NULL);
+		}
+		return;
+	}
 
 	qmi_client_uim_verify_pin(client, input, 10, cancellable,
 				   (GAsyncReadyCallback)verify_pin_ready, NULL);
@@ -719,7 +750,6 @@ static void allocate_uim_client_ready(QmiDevice *dev, GAsyncResult *res)
 {
 	QmiMessageUimPowerOnSimInput *input;
 	GError *error = NULL;
-	guint slot = 1;
 
 	client_uim = (QmiClientUim *)qmi_device_allocate_client_finish(dev, res, &error);
 	if (!client_uim) {
@@ -729,15 +759,22 @@ static void allocate_uim_client_ready(QmiDevice *dev, GAsyncResult *res)
 		op_shutdown(SHUTDOWN_ERROR);
 		return;
 	}
-	
-	g_print("[%s] [UIM] Powering SIM1...\n", qmi_device_get_path_display(device));
 
 	input = qmi_message_uim_power_on_sim_input_new();
-	
-	qmi_message_uim_power_on_sim_input_set_slot(input, slot, &error);
 
-	qmi_client_uim_power_on_sim(client_uim, input, 10, cancellable,
-				     (GAsyncReadyCallback)power_on_sim_ready, NULL);
+	if (config.sim1_enable) {
+		g_print("[%s] [UIM] Powering SIM1...\n", qmi_device_get_path_display(device));
+		qmi_message_uim_power_on_sim_input_set_slot(input, 1, &error);
+		qmi_client_uim_power_on_sim(client_uim, input, 10, cancellable,
+				     	    (GAsyncReadyCallback)power_on_sim_ready, (gpointer)1);
+	}
+
+	if (config.sim2_enable) {
+		g_print("[%s] [UIM] Powering SIM2...\n", qmi_device_get_path_display(device));
+		qmi_message_uim_power_on_sim_input_set_slot(input, 2, &error);
+		qmi_client_uim_power_on_sim(client_uim, input, 10, cancellable,
+				     	    (GAsyncReadyCallback)power_on_sim_ready, (gpointer)2);
+	}
 
 	qmi_message_uim_power_on_sim_input_unref(input);
 }
@@ -777,12 +814,12 @@ static void get_operating_mode_ready(QmiClientDms *client, GAsyncResult *res)
 	
 	g_print("[%s] [DMS] Modem is online\n", qmi_device_get_path_display(device));
 	
-	if (sim1_pin_str) {
+	if (config.sim1_enable || config.sim2_enable) {
 		qmi_device_allocate_client(device, service_uim, QMI_CID_NONE, 10, cancellable,
 					   (GAsyncReadyCallback)allocate_uim_client_ready, NULL);
 	} else {
 		qmi_device_allocate_client(device, service_wda, QMI_CID_NONE, 10, cancellable,
-					   (GAsyncReadyCallback)allocate_wda_client_ready, NULL);
+						   (GAsyncReadyCallback)allocate_wda_client_ready, NULL);
 	}
 }
 
@@ -903,25 +940,25 @@ static gboolean signals_handler(void)
 }
 
 static GOptionEntry main_entries[] = {
-	{ "config", 'c', 0, G_OPTION_ARG_STRING, &config,
+	{ "config", 'c', 0, G_OPTION_ARG_STRING, &config_path,
           "Configuration file to use (not working)", "[CONFIG_FILE]"
         },
-	{ "apn", 'a', 0, G_OPTION_ARG_STRING, &apn_str,
+	{ "apn", 'a', 0, G_OPTION_ARG_STRING, &config.apn_str,
 	  "Specify device APN name", "[APN]"
 	},
-	{ "sim1-pin", 'p', 0, G_OPTION_ARG_STRING, &sim1_pin_str,
+	{ "sim1-pin", 'p', 0, G_OPTION_ARG_STRING, &config.sim1_pin_str,
 	  "Specify SIM1 pin code", "[SIM1-PIN]"
 	},
 	{ "device", 'd', 0, G_OPTION_ARG_STRING, &qmi_dev,
 	  "Use passed MHI QMI device (instead of IPCR)", "[DEVICE]"
 	},
-	{ "mux-id", 0, 0, G_OPTION_ARG_INT, &mux_id,
+	{ "mux-id", 0, 0, G_OPTION_ARG_INT, &config.mux_id,
 	  "QMAP/RMNET mux-id of the connection (default is 1)", "[ID]"
 	},
-	{ "set-ip", 0, 0, G_OPTION_ARG_NONE, &set_ip,
+	{ "set-ip", 0, 0, G_OPTION_ARG_NONE, &config.set_ip,
 	  "Setup rmnet IP address and DNS from Bearer context info", NULL
 	},
-	{ "default-route", 0, 0, G_OPTION_ARG_NONE, &default_route,
+	{ "default-route", 0, 0, G_OPTION_ARG_NONE, &config.default_route,
 	  "Set rmnet interface as the defaut network route", NULL
 	},
 	{ "udev", 0, 0, G_OPTION_ARG_NONE, &udev,
@@ -978,20 +1015,26 @@ int mainloop(void)
 	return exit_reason;
 }
 
-static int read_config(const gchar *config)
+static int read_config(const gchar *path)
 {
 	GKeyFile *key_file = g_key_file_new();
 
-	if (!g_key_file_load_from_file(key_file, config, 0, NULL)) {
-		g_printerr("error: Unable to load %s\n", config);
+	if (!g_key_file_load_from_file(key_file, path, 0, NULL)) {
+		g_printerr("error: Unable to load %s\n", path);
 	}
 
 	/* cleanup this */
 
-	apn_str = g_key_file_get_string(key_file, "APN", "name", NULL);
-	sim1_pin_str = g_key_file_get_string(key_file, "SIM", "pin", NULL);
-	set_ip = g_key_file_get_boolean(key_file, "RMNET", "ip_from_bearer", NULL);
-	default_route = g_key_file_get_boolean(key_file, "RMNET", "default_route", NULL);
+	config.apn_str = g_key_file_get_string(key_file, "APN", "name", NULL);
+	config.mux_id = g_key_file_get_integer(key_file, "APN", "mux_id", NULL);
+	config.sim1_enable = g_key_file_get_boolean(key_file, "SIM1", "enable", NULL);
+	config.sim1_pin_str = g_key_file_get_string(key_file, "SIM1", "pin", NULL);
+	config.sim2_enable = g_key_file_get_boolean(key_file, "SIM2", "enable", NULL);
+	config.sim2_pin_str = g_key_file_get_string(key_file, "SIM2", "pin", NULL);
+	config.set_ip = g_key_file_get_boolean(key_file, "RMNET", "ip_from_bearer", NULL);
+	config.default_route = g_key_file_get_boolean(key_file, "RMNET", "default_route", NULL);
+
+	g_key_file_unref(key_file);
 
 	return 0;
 }
@@ -1011,25 +1054,30 @@ int main(int argc, char **argv)
 	}
 	g_option_context_free(context);
 
+	config.sim1_enable = TRUE;
+
 	if (udev) {
 		qmi_dev = g_strdup(g_getenv("DEVNAME"));
-		config = MHI_CONFIG_DEFAULT;
+		config_path = MHI_CONFIG_DEFAULT;
 		// todo g_free()
 	}
 
-	if (config) {
-		read_config(config);
+	if (config_path) {
+		read_config(config_path);
 	}
 
-	if (!apn_str) {
+	if (!config.apn_str) {
 		g_printerr("error: no APN name\n");
 		exit(EXIT_FAILURE);
 	}
 
-	if (default_route && !set_ip) {
+	if (config.default_route && !config.set_ip) {
 		g_printerr("error: Cannot set --default-route without --set-ip option\n");
 		exit(EXIT_FAILURE);
 	}
+	
+	if (!config.mux_id)
+		config.mux_id = 1;
 
 	if (kmsg) {
 		/*/ Wait for the modem to be ready */
diff --git a/mhi-qmi-connect.conf b/mhi-qmi-connect.conf
index a72de2e..259536c 100644
--- a/mhi-qmi-connect.conf
+++ b/mhi-qmi-connect.conf
@@ -1,12 +1,19 @@
 [APN]
 name=free
+mux_id=1
 
-[SIM]
+[SIM1]
+enable=true
+#pin=1234
+
+[SIM2]
+#enable=true
 #pin=1234
 
 [RMNET]
-# Retrieve network IP/DNS from bearer (no dhcp)
+## Retrieve network IP/DNS from bearer (no dhcp)
 ip_from_bearer=true
-# Network as default route
-default_route=true
+
+## Network as default route
+default_route=false