n900: add support for kernel PMHEADmaster

New kernels can be configured for kernel based
modem power management. In this case the kernel
will handle the modem gpios itself. It will power
up the modem, once the phonet interface is brought
up and vice versa.

1 files changed, 156 insertions, 13 deletions

diff --git a/plugins/n900.c b/plugins/n900.c
index 956a6fca..76d58646 100644
--- a/plugins/n900.c
+++ b/plugins/n900.c
@@ -29,6 +29,8 @@
 #include <string.h>
 #include <glib.h>
 
+#include <sys/stat.h>
+
 #include <gisi/modem.h>
 #include <gisi/netlink.h>
 #include <gisi/client.h>
@@ -66,6 +68,7 @@ struct isi_data {
 	const char *ifname;
 	GIsiModem *modem;
 	GIsiClient *client;
+	GIsiPhonetNetlink *link;
 	struct isi_infoserver *infoserver;
 	ofono_bool_t enabled;
 	ofono_bool_t online;
@@ -330,6 +333,130 @@ static void n900_power_cb(enum power_state state, void *data)
 		isi->mtc_state = MTC_STATE_NONE;
 }
 
+static gboolean n900_has_kernel_modem_pm(struct ofono_modem *modem)
+{
+	struct isi_data *isi = ofono_modem_get_data(modem);
+	char cmtdev[256];
+	char engpiofile[256];
+	struct stat st;
+
+	snprintf(cmtdev, sizeof cmtdev, "/sys/class/net/%s/device/nokia-modem",
+				isi->ifname);
+	snprintf(engpiofile, sizeof engpiofile, "%s/cmt_en", cmtdev);
+
+	/* kernel too old, nokia-modem symlink is missing */
+	if (stat(cmtdev, &st) != 0)
+		return FALSE;
+
+	/* gpios are exported, so no kernel based pm */
+	if (stat(engpiofile, &st) == 0)
+		return FALSE;
+
+	return TRUE;
+}
+
+static int n900_kernel_pm_rapuyama_version(struct ofono_modem *modem)
+{
+	struct isi_data *isi = ofono_modem_get_data(modem);
+	char filename[256];
+	char buf[32];
+	FILE *f;
+
+	snprintf(filename, sizeof filename, "/sys/class/net/%s/device/rapuyama_version",
+				isi->ifname);
+
+	f = fopen(filename, "r");
+	if (f == NULL) {
+		DBG("%s: %s (%d)", filename, strerror(errno), errno);
+		return 0;
+	}
+
+	if (fgets(buf, sizeof buf, f) == NULL) {
+		fclose(f);
+		return 0;
+	}
+
+	fclose(f);
+
+	return atoi(buf);
+}
+
+static void phonet_status_cb(GIsiModem *idx, enum GIsiPhonetLinkState state,
+				char const *ifname, void *data)
+{
+	struct ofono_modem *modem = data;
+
+	DBG("Link %s (%u) is %s",
+		ifname, g_isi_modem_index(idx),
+		state == PN_LINK_REMOVED ? "removed" :
+		state == PN_LINK_DOWN ? "down" : "up");
+
+	if (state == PN_LINK_UP)
+		n900_power_cb(POWER_STATE_ON, modem);
+	else
+		n900_power_cb(POWER_STATE_OFF, modem);
+}
+
+
+static int n900_pm_enable(struct ofono_modem *modem)
+{
+	struct isi_data *isi = ofono_modem_get_data(modem);
+	unsigned address = ofono_modem_get_integer(modem, "Address");
+	GMainContext *ctx = g_main_context_default();
+	int error, i;
+
+	if (g_isi_pn_netlink_by_modem(isi->modem)) {
+		DBG("Phonet link %p: %s", isi->modem, strerror(EBUSY));
+		return -(errno = EBUSY);
+	}
+
+	isi->link = g_isi_pn_netlink_start(isi->modem, phonet_status_cb, modem);
+	if (isi->link == NULL) {
+		return -errno;
+	}
+
+	if (address) {
+		error = g_isi_pn_netlink_set_address(isi->modem, address);
+		if (error && error != -EEXIST)
+			DBG("g_isi_netlink_set_address: %s", strerror(-error));
+	}
+
+	/* wait for modem power up event */
+	for (i = 0; i < 100; i++) {
+		if (isi->power_state == POWER_STATE_ON)
+			break;
+
+		g_main_context_iteration(ctx, FALSE);
+		g_usleep(100000);
+	}
+
+	if (isi->power_state != POWER_STATE_ON) {
+		ofono_error("Could not enable modem!");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int n900_pm_disable(struct ofono_modem *modem)
+{
+	struct isi_data *isi = ofono_modem_get_data(modem);
+	GMainContext *ctx = g_main_context_default();
+
+	n900_power_cb(POWER_STATE_OFF_STARTED, modem);
+
+	/* wait for poweroff message being sent before taking the interface down */
+	g_main_context_iteration(ctx, FALSE);
+	g_usleep(100000);
+	g_main_context_iteration(ctx, FALSE);
+
+	g_isi_pn_netlink_stop(isi->link);
+
+	n900_power_cb(POWER_STATE_OFF, modem);
+
+	return 0;
+}
+
 static int n900_probe(struct ofono_modem *modem)
 {
 	char const *ifname = ofono_modem_get_string(modem, "Interface");
@@ -359,11 +486,6 @@ static int n900_probe(struct ofono_modem *modem)
 	if (getenv("OFONO_ISI_TRACE"))
 		g_isi_modem_set_trace(isimodem, isi_trace);
 
-	if (gpio_probe(isimodem, address, n900_power_cb, modem) != 0) {
-		DBG("gpio for %s: %s", ifname, strerror(errno));
-		goto error;
-	}
-
 	isi = g_try_new0(struct isi_data, 1);
 	if (isi == NULL) {
 		errno = ENOMEM;
@@ -378,15 +500,29 @@ static int n900_probe(struct ofono_modem *modem)
 	isi->ifname = ifname;
 	isi->client = client;
 
-	isi->rapu_version = gpio_rapuyama_version(modem);
+	ofono_modem_set_data(modem, isi);
+
+	if (n900_has_kernel_modem_pm(modem)) {
+		DBG("kernel modem PM detected!");
+	} else if (gpio_probe(isimodem, address, n900_power_cb, modem) != 0) {
+		DBG("gpio for %s: %s", ifname, strerror(errno));
+		goto error;
+	}
+
+	if (n900_has_kernel_modem_pm(modem)) {
+		isi->rapu_version = n900_kernel_pm_rapuyama_version(modem);
+	} else {
+		isi->rapu_version = gpio_rapuyama_version(modem);
+	}
+
 	DBG("RAPUYAMA version: %d", isi->rapu_version);
 
-	ofono_modem_set_data(modem, isi);
 	return 0;
 
 error:
 	g_isi_modem_destroy(isimodem);
-	gpio_remove(modem);
+	if (!n900_has_kernel_modem_pm(modem))
+		gpio_remove(modem);
 	g_free(isi);
 
 	return -errno;
@@ -396,12 +532,13 @@ static void n900_remove(struct ofono_modem *modem)
 {
 	struct isi_data *isi = ofono_modem_get_data(modem);
 
-	ofono_modem_set_data(modem, NULL);
-
 	if (!isi)
 		return;
 
-	gpio_remove(modem);
+	if (!n900_has_kernel_modem_pm(modem))
+		gpio_remove(modem);
+
+	ofono_modem_set_data(modem, NULL);
 
 	if (isi->timeout)
 		g_source_remove(isi->timeout);
@@ -530,7 +667,10 @@ static int n900_enable(struct ofono_modem *modem)
 
 	isi->enabled = TRUE;
 
-	return gpio_enable(modem);
+	if (n900_has_kernel_modem_pm(modem))
+		return n900_pm_enable(modem);
+	else
+		return gpio_enable(modem);
 }
 
 static int n900_disable(struct ofono_modem *modem)
@@ -541,7 +681,10 @@ static int n900_disable(struct ofono_modem *modem)
 
 	isi->enabled = FALSE;
 
-	return gpio_disable(modem);
+	if (n900_has_kernel_modem_pm(modem))
+		return n900_pm_disable(modem);
+	else
+		return gpio_disable(modem);
 }
 
 static struct ofono_modem_driver n900_driver = {