n900: modem driver plugin for Nokia N900

This patch adds a modem driver plugin for the Nokia N900 internal
modem. It controls the modem using the appropriate GPIO lines and thus
works without the Maemo userspace.

This plugin can run natively on the N900 with either Maemo or Meego
kernels. However, it conflicts with the Maemo userspace, for which
isigen should be used instead.

4 files changed, 1451 insertions, 0 deletions

diff --git a/Makefile.am b/Makefile.am
index 51225067..66fecf7d 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -133,6 +133,9 @@ builtin_sources += $(gisi_sources) \
 builtin_modules += isigen
 builtin_sources += plugins/isigen.c
 
+builtin_modules += n900
+builtin_sources += plugins/n900.c plugins/nokia-gpio.h plugins/nokia-gpio.c
+
 builtin_modules += usbpnmodem
 builtin_sources += plugins/usbpnmodem.c
 endif
diff --git a/plugins/n900.c b/plugins/n900.c
new file mode 100644
index 00000000..bc40443d
--- /dev/null
+++ b/plugins/n900.c
@@ -0,0 +1,566 @@
+/*
+ * This file is part of oFono - Open Source Telephony
+ *
+ * Copyright (C) 2010  Nokia Corporation and/or its subsidiary(-ies).
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <string.h>
+#include <glib.h>
+
+#include <gisi/modem.h>
+#include <gisi/netlink.h>
+#include <gisi/client.h>
+
+#define OFONO_API_SUBJECT_TO_CHANGE
+#include <ofono/plugin.h>
+#include <ofono/log.h>
+#include <ofono/modem.h>
+#include <ofono/devinfo.h>
+#include <ofono/phonebook.h>
+#include <ofono/netreg.h>
+#include <ofono/voicecall.h>
+#include <ofono/sms.h>
+#include <ofono/cbs.h>
+#include <ofono/sim.h>
+#include <ofono/ussd.h>
+#include <ofono/ssn.h>
+#include <ofono/call-forwarding.h>
+#include <ofono/call-settings.h>
+#include <ofono/call-barring.h>
+#include <ofono/call-meter.h>
+#include <ofono/radio-settings.h>
+#include <ofono/gprs.h>
+#include <ofono/gprs-context.h>
+
+#include "drivers/isimodem/isimodem.h"
+#include "drivers/isimodem/isiutil.h"
+#include "drivers/isimodem/infoserver.h"
+#include "drivers/isimodem/mtc.h"
+#include "drivers/isimodem/debug.h"
+
+#include "nokia-gpio.h"
+
+struct isi_data {
+	struct ofono_modem *modem;
+	char const *ifname;
+	GIsiModem *idx;
+	GIsiClient *client;
+	struct isi_infoserver *infoserver;
+	ofono_bool_t enabled;
+	ofono_bool_t online;
+	ofono_bool_t reported;
+	enum power_state power_state;
+	int mtc_state;
+	guint timeout;
+	struct isi_cb_data *online_cbd;
+};
+
+static void set_power_by_mtc_state(struct isi_data *isi, int state);
+static void mtc_power_off(struct isi_data *isi);
+static gboolean mtc_power_off_poll(gpointer user);
+
+static void report_powered(struct isi_data *isi, ofono_bool_t powered)
+{
+	if (powered == isi->reported)
+		return;
+
+	DBG("%s", powered ? "Powered on"
+		: isi->enabled ? "Reset"
+		: "Powered off");
+
+	isi->reported = powered;
+
+	ofono_modem_set_powered(isi->modem, powered);
+}
+
+static void report_online(struct isi_data *isi, ofono_bool_t online)
+{
+	struct isi_cb_data *cbd = isi->online_cbd;
+	ofono_modem_online_cb_t cb = cbd->cb;
+
+	isi->online_cbd = NULL;
+
+	if (isi->online == online)
+		CALLBACK_WITH_SUCCESS(cb, cbd->data);
+	else
+		CALLBACK_WITH_FAILURE(cb, cbd->data);
+
+	g_free(cbd);
+}
+
+static void set_power_by_mtc_state(struct isi_data *isi, int mtc_state)
+{
+	isi->mtc_state = mtc_state;
+
+	if (isi->online_cbd)
+		report_online(isi, mtc_state == MTC_NORMAL);
+
+	switch (mtc_state) {
+	case MTC_STATE_NONE:
+	case MTC_POWER_OFF:
+	case MTC_CHARGING:
+	case MTC_SELFTEST_FAIL:
+		report_powered(isi, 0);
+		break;
+
+	case MTC_RF_INACTIVE:
+	case MTC_NORMAL:
+	default:
+		report_powered(isi, 1);
+	}
+}
+
+static void mtc_state_ind_cb(GIsiClient *client, const void *restrict data,
+				size_t len, uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct isi_data *isi = opaque;
+
+	if (!msg) {
+		DBG("ISI client error: %d", g_isi_client_error(client));
+		return;
+	}
+
+	if (len < 3 || msg[0] != MTC_STATE_INFO_IND)
+		return;
+
+	if (msg[2] == MTC_START) {
+		DBG("target modem state: %s (0x%02X)",
+			mtc_modem_state_name(msg[1]), msg[1]);
+
+		if (msg[1] == MTC_POWER_OFF) {
+			isi->power_state = POWER_STATE_OFF_STARTED;
+			mtc_power_off_poll(isi);
+		}
+	} else if (msg[2] == MTC_READY) {
+		DBG("current modem state: %s (0x%02X)",
+			mtc_modem_state_name(msg[1]), msg[1]);
+
+		set_power_by_mtc_state(isi, msg[1]);
+	}
+}
+
+static gboolean mtc_state_cb(GIsiClient *client,
+				const void *restrict data, size_t len,
+				uint16_t object, void *opaque)
+{
+	struct isi_cb_data *cbd = opaque;
+	struct ofono_modem *modem = cbd->user;
+	ofono_modem_online_cb_t cb = cbd->cb;
+	struct isi_data *isi = ofono_modem_get_data(modem);
+	const unsigned char *msg = data;
+
+	if (!msg) {
+		DBG("ISI client error: %d", g_isi_client_error(client));
+		goto err;
+	}
+
+	if (len < 3 || msg[0] != MTC_STATE_RESP)
+		return FALSE;
+
+	DBG("cause: %s (0x%02X)", mtc_isi_cause_name(msg[1]), msg[1]);
+
+	if (msg[1] == MTC_OK) {
+		/* Wait until MTC_READY indication */
+		isi->online_cbd = cbd;
+		return TRUE;
+	}
+
+err:
+	if (msg && msg[1] == MTC_ALREADY_ACTIVE)
+		CALLBACK_WITH_SUCCESS(cb, cbd->data);
+	else
+		CALLBACK_WITH_FAILURE(cb, cbd->data);
+
+	g_free(cbd);
+
+	return TRUE;
+}
+
+static GIsiRequest *mtc_state(struct isi_data *isi, uint8_t state,
+				struct isi_cb_data *cbd)
+{
+	const unsigned char req[3] = {
+		MTC_STATE_REQ, state
+	};
+
+	return g_isi_send(isi->client, req, sizeof(req), MTC_TIMEOUT,
+				mtc_state_cb, cbd, NULL);
+}
+
+static gboolean mtc_startup_synq_cb(GIsiClient *client,
+					const void *restrict data, size_t len,
+					uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+
+	if (!msg) {
+		DBG("%s: %s", "MTC_STARTUP_SYNQ",
+			strerror(-g_isi_client_error(client)));
+		return TRUE;
+	}
+
+	if (len < 3 || msg[0] != MTC_STARTUP_SYNQ_RESP)
+		return FALSE;
+
+	return TRUE;
+}
+
+static void mtc_startup_synq(struct isi_data *isi)
+{
+	static const unsigned char msg[3] = {
+		MTC_STARTUP_SYNQ_REQ,
+	};
+
+	g_isi_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT,
+			mtc_startup_synq_cb, NULL, NULL);
+}
+
+static gboolean mtc_state_query_cb(GIsiClient *client,
+					const void *restrict data, size_t len,
+					uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct isi_data *isi = opaque;
+
+	if (!msg) {
+		DBG("ISI client error: %d", g_isi_client_error(client));
+		return TRUE;
+	}
+
+	if (len < 3 || msg[0] != MTC_STATE_QUERY_RESP)
+		return FALSE;
+
+	DBG("current: %s (0x%02X)", mtc_modem_state_name(msg[1]), msg[1]);
+	DBG("target: %s (0x%02X)", mtc_modem_state_name(msg[2]), msg[2]);
+
+	set_power_by_mtc_state(isi, msg[1]);
+
+	mtc_startup_synq(isi);
+
+	return TRUE;
+}
+
+static void mtc_state_query(struct isi_data *isi)
+{
+	static const unsigned char msg[3] = {
+		MTC_STATE_QUERY_REQ,
+	};
+
+	g_isi_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT,
+			mtc_state_query_cb, isi, NULL);
+}
+
+static void mtc_reachable_cb(GIsiClient *client, gboolean alive,
+				uint16_t object, void *opaque)
+{
+	struct isi_data *isi = opaque;
+
+	if (!alive) {
+		DBG("MTC client: %s", strerror(-g_isi_client_error(client)));
+		/* enable is terminated eventually by timeout */
+		return;
+	}
+
+	DBG("%s (v.%03d.%03d) reachable",
+		pn_resource_name(g_isi_client_resource(client)),
+		g_isi_version_major(client),
+		g_isi_version_minor(client));
+
+	g_isi_subscribe(client, MTC_STATE_INFO_IND, mtc_state_ind_cb, opaque);
+
+	mtc_state_query(isi);
+}
+
+static void mtc_shutdown_sync(struct isi_data *isi)
+{
+	const unsigned char msg[3] = {
+		MTC_SHUTDOWN_SYNC_REQ,
+	};
+
+	g_isi_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT,
+			NULL, NULL, NULL);
+}
+
+
+static gboolean mtc_power_off_poll(gpointer user)
+{
+	struct isi_data *isi = user;
+
+	isi->timeout = 0;
+
+	if (isi->power_state == POWER_STATE_ON_STARTED
+			|| isi->power_state == POWER_STATE_OFF
+			|| isi->power_state == POWER_STATE_OFF_WAITING)
+		return FALSE;
+
+	mtc_shutdown_sync(isi);
+
+	isi->timeout = g_timeout_add(200, mtc_power_off_poll, user);
+
+	return FALSE;
+}
+
+static gboolean mtc_power_off_cb(GIsiClient *client,
+					const void *restrict data, size_t len,
+					uint16_t object, void *opaque)
+{
+	struct isi_data *isi = opaque;
+	const unsigned char *msg = data;
+
+	if (!msg) {
+		DBG("%s: %s", "MTC_POWER_OFF_RESP",
+			strerror(-g_isi_client_error(client)));
+
+		if (isi->power_state == POWER_STATE_OFF_STARTED)
+			mtc_power_off(isi);
+
+		return TRUE;
+	}
+
+	if (len < 3 || msg[0] != MTC_POWER_OFF_RESP)
+		return FALSE;
+
+	/* power off poll is started by mtc_state_ind_cb() */
+
+	return TRUE;
+}
+
+static void mtc_power_off(struct isi_data *isi)
+{
+	static const unsigned char msg[3] = {
+		MTC_POWER_OFF_REQ,
+	};
+
+	g_isi_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT,
+			mtc_power_off_cb, isi, NULL);
+}
+
+static void n900_power_cb(enum power_state state, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct isi_data *isi = ofono_modem_get_data(modem);
+
+	DBG("power state %s", gpio_power_state_name(state));
+
+	isi->power_state = state;
+
+	if (state == POWER_STATE_OFF_STARTED)
+		mtc_power_off(isi);
+	else if (isi->timeout)
+		g_source_remove(isi->timeout);
+
+	if (state == POWER_STATE_ON)
+		g_isi_verify(isi->client, mtc_reachable_cb, isi);
+	else if (isi->enabled)
+		/* If enabled, report modem crash */
+		set_power_by_mtc_state(isi, MTC_STATE_NONE);
+	else if (state == POWER_STATE_OFF || state == POWER_STATE_ON_FAILED)
+		/* If being disabled, report powered off only when safe */
+		report_powered(isi, 0);
+	else
+		isi->mtc_state = MTC_STATE_NONE;
+}
+
+static int n900_probe(struct ofono_modem *modem)
+{
+	char const *ifname = ofono_modem_get_string(modem, "Interface");
+	unsigned address = ofono_modem_get_integer(modem, "Address");
+	GIsiModem *idx;
+	struct isi_data *isi;
+
+	if (ifname == NULL)
+		ifname = "phonet0";
+
+	DBG("(%p) with %s", modem, ifname);
+
+	idx = g_isi_modem_by_name(ifname);
+	if (!idx) {
+		DBG("Interface=%s: %s", ifname, strerror(errno));
+		return -errno;
+	}
+
+	if (gpio_probe(idx, address, n900_power_cb, modem) != 0) {
+		DBG("gpio for %s: %s", ifname, strerror(errno));
+		return -errno;
+	}
+
+	isi = g_new0(struct isi_data, 1);
+	if (!isi) {
+		gpio_remove(modem);
+		return -ENOMEM;
+	}
+
+	ofono_modem_set_data(isi->modem = modem, isi);
+
+	isi->idx = idx;
+	isi->ifname = ifname;
+	isi->client = g_isi_client_create(isi->idx, PN_MTC);
+
+	return 0;
+}
+
+static void n900_remove(struct ofono_modem *modem)
+{
+	struct isi_data *isi = ofono_modem_get_data(modem);
+
+	DBG("");
+
+	if (isi == NULL)
+		return;
+
+	gpio_remove(modem);
+
+	if (isi->timeout)
+		g_source_remove(isi->timeout);
+
+	g_isi_client_destroy(isi->client);
+
+	g_free(isi);
+}
+
+static void n900_set_online(struct ofono_modem *modem,
+					ofono_bool_t online,
+					ofono_modem_online_cb_t cb, void *data)
+{
+	struct isi_data *isi = ofono_modem_get_data(modem);
+	struct isi_cb_data *cbd = isi_cb_data_new(modem, cb, data);
+
+	DBG("(%p) with %s", modem, isi->ifname);
+
+	if (!cbd)
+		goto error;
+
+	if (isi->power_state != POWER_STATE_ON)
+		goto error;
+
+	if (isi->mtc_state == MTC_SELFTEST_FAIL)
+		goto error;
+
+	if (mtc_state(isi, online ? MTC_NORMAL : MTC_RF_INACTIVE, cbd)) {
+		isi->online = online;
+		return;
+	}
+
+error:
+	g_free(cbd);
+	CALLBACK_WITH_FAILURE(cb, data);
+}
+
+static void n900_pre_sim(struct ofono_modem *modem)
+{
+	struct isi_data *isi = ofono_modem_get_data(modem);
+
+	DBG("");
+
+	isi->infoserver = isi_infoserver_create(isi->modem, isi->idx);
+
+	ofono_sim_create(isi->modem, 0, "isimodem", isi->idx);
+	ofono_devinfo_create(isi->modem, 0, "isimodem", isi->idx);
+	ofono_voicecall_create(isi->modem, 0, "isimodem", isi->idx);
+}
+
+static void n900_post_sim(struct ofono_modem *modem)
+{
+	struct isi_data *isi = ofono_modem_get_data(modem);
+
+	DBG("");
+
+	ofono_phonebook_create(isi->modem, 0, "isimodem", isi->idx);
+}
+
+static void n900_post_online(struct ofono_modem *modem)
+{
+	struct isi_data *isi = ofono_modem_get_data(modem);
+	struct ofono_gprs *gprs;
+	struct ofono_gprs_context *gc;
+
+	DBG("");
+
+	ofono_netreg_create(isi->modem, 0, "isimodem", isi->idx);
+	ofono_sms_create(isi->modem, 0, "isimodem", isi->idx);
+	ofono_cbs_create(isi->modem, 0, "isimodem", isi->idx);
+	ofono_ssn_create(isi->modem, 0, "isimodem", isi->idx);
+	ofono_ussd_create(isi->modem, 0, "isimodem", isi->idx);
+	ofono_call_forwarding_create(isi->modem, 0, "isimodem", isi->idx);
+	ofono_call_settings_create(isi->modem, 0, "isimodem", isi->idx);
+	ofono_call_barring_create(isi->modem, 0, "isimodem", isi->idx);
+	ofono_call_meter_create(isi->modem, 0, "isimodem", isi->idx);
+	ofono_radio_settings_create(isi->modem, 0, "isimodem", isi->idx);
+	gprs = ofono_gprs_create(isi->modem, 0, "isimodem", isi->idx);
+	gc = ofono_gprs_context_create(isi->modem, 0, "isimodem", isi->idx);
+
+	if (gprs && gc)
+		ofono_gprs_add_context(gprs, gc);
+	else
+		DBG("Failed to add context");
+}
+
+static int n900_enable(struct ofono_modem *modem)
+{
+	struct isi_data *isi = ofono_modem_get_data(modem);
+
+	DBG("modem=%p with %p", modem, isi ? isi->ifname : NULL);
+
+	isi->enabled = TRUE;
+
+	return gpio_enable(modem);
+}
+
+static int n900_disable(struct ofono_modem *modem)
+{
+	struct isi_data *isi = ofono_modem_get_data(modem);
+
+	DBG("modem=%p with %p", modem, isi ? isi->ifname : NULL);
+
+	isi->enabled = FALSE;
+
+	return gpio_disable(modem);
+}
+
+static struct ofono_modem_driver n900_driver = {
+	.name = "n900",
+	.probe = n900_probe,
+	.remove = n900_remove,
+	.enable = n900_enable,
+	.disable = n900_disable,
+	.set_online = n900_set_online,
+	.pre_sim = n900_pre_sim,
+	.post_sim = n900_post_sim,
+	.post_online = n900_post_online,
+};
+
+static int n900_init(void)
+{
+	return ofono_modem_driver_register(&n900_driver);
+}
+
+static void n900_exit(void)
+{
+	ofono_modem_driver_unregister(&n900_driver);
+}
+
+OFONO_PLUGIN_DEFINE(n900, "Nokia N900 modem driver", VERSION,
+			OFONO_PLUGIN_PRIORITY_DEFAULT, n900_init, n900_exit)
diff --git a/plugins/nokia-gpio.c b/plugins/nokia-gpio.c
new file mode 100644
index 00000000..f4a5ec2e
--- /dev/null
+++ b/plugins/nokia-gpio.c
@@ -0,0 +1,829 @@
+/*
+ * This file is part of oFono - Open Source Telephony
+ *
+ * Copyright (C) 2010  Nokia Corporation and/or its subsidiary(-ies).
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <errno.h>
+
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <gisi/netlink.h>
+#include <glib.h>
+
+#define OFONO_API_SUBJECT_TO_CHANGE
+#include <ofono/plugin.h>
+#include <ofono/modem.h>
+#include <ofono/log.h>
+
+#include <drivers/isimodem/debug.h>
+#include "nokia-gpio.h"
+
+#define GPIO_SWITCH	"/sys/devices/platform/gpio-switch"
+#define DEV_CMT		"/dev/cmt"
+
+enum rapu_type {
+	RAPU_TYPE_1,
+	RAPU_TYPE_2,
+};
+
+enum retry_count {
+	RETRY_COUNT_RESET = 5,
+	RETRY_COUNT_POWER_ON = 10,
+};
+
+enum phonet_link {
+	PHONET_LINK_NONE = 0,
+	PHONET_LINK_DOWN,
+	PHONET_LINK_UP,
+};
+
+enum power_event {
+	POWER_EVENT_PHONET_LINK_UP = 1,
+	POWER_EVENT_PHONET_LINK_DOWN,
+	POWER_EVENT_ON,
+	POWER_EVENT_ON_TIMEOUT,
+	POWER_EVENT_REBOOT_TIMEOUT,
+	POWER_EVENT_OFF,
+	POWER_EVENT_OFF_IMMEDIATELY,
+	POWER_EVENT_OFF_TIMEOUT,
+	POWER_EVENT_OFF_COMPLETE,
+};
+
+struct gpio_data {
+	GPhonetNetlink *link;
+	gpio_finished_cb_t callback;
+	void *data;
+
+	enum power_state state;
+	enum phonet_link current;
+	enum phonet_link target;
+	enum power_event timer_event;
+	enum rapu_type rapu;
+
+	guint timeout_source;
+
+	unsigned retry_count;
+	unsigned have_gpio_switch:1;
+	unsigned have_cmt_en:1;
+	unsigned have_cmt_rst_rq:1;
+	unsigned have_cmt_rst:1;
+	unsigned have_cmt_bsi:1;
+	unsigned have_cmt_apeslpx:1;
+	unsigned reset_in_progress:1;
+	unsigned startup_in_progress:1;
+};
+
+static struct gpio_data self;
+
+#define _(X) case X: return #X
+
+static inline char const *gpio_power_event_name(enum power_event value)
+{
+	switch (value) {
+		_(POWER_EVENT_PHONET_LINK_UP);
+		_(POWER_EVENT_PHONET_LINK_DOWN);
+		_(POWER_EVENT_ON);
+		_(POWER_EVENT_ON_TIMEOUT);
+		_(POWER_EVENT_REBOOT_TIMEOUT);
+		_(POWER_EVENT_OFF);
+		_(POWER_EVENT_OFF_IMMEDIATELY);
+		_(POWER_EVENT_OFF_TIMEOUT);
+		_(POWER_EVENT_OFF_COMPLETE);
+	}
+	return "<UNKNOWN>";
+}
+
+char const *gpio_power_state_name(enum power_state value)
+{
+	switch (value) {
+		_(POWER_STATE_NONE);
+		_(POWER_STATE_ON_STARTED);
+		_(POWER_STATE_ON);
+		_(POWER_STATE_ON_RESET);
+		_(POWER_STATE_ON_FAILED);
+		_(POWER_STATE_OFF_STARTED);
+		_(POWER_STATE_OFF_WAITING);
+		_(POWER_STATE_OFF);
+	}
+	return "<UNKNOWN>";
+}
+
+#undef _
+
+static void gpio_power_state_machine(enum power_event event);
+static void gpio_power_set_state(enum power_state new_state);
+
+static int file_exists(char const *filename)
+{
+	struct stat st;
+
+	return stat(filename, &st) == 0;
+}
+
+static int dir_exists(char const *filename)
+{
+	struct stat st;
+
+	return stat(filename, &st) == 0 && S_ISDIR(st.st_mode);
+}
+
+static int file_write(char const *filename, char const *output)
+{
+	FILE *f;
+
+	f = fopen(filename, "r+");
+	if (!f) {
+		DBG("%s: %s (%d)", filename, strerror(errno), errno);
+		return -1;
+	}
+
+	fputs(output, f);
+
+	return fclose(f);
+}
+
+static int gpio_write(char *line, int value)
+{
+	char filename[256];
+
+	DBG("(\"%s\", \"%s\")", line, value ? "active" : "inactive");
+
+	if (self.have_gpio_switch) {
+		snprintf(filename, sizeof filename, "%s/%s/%s",
+				GPIO_SWITCH, line, "state");
+		return file_write(filename, value ? "active" : "inactive");
+	} else {
+		snprintf(filename, sizeof filename, "%s/%s/%s",
+				DEV_CMT, line, "value");
+		return file_write(filename, value ? "1" : "0");
+	}
+}
+
+#define GPIO_WRITE(line, value) \
+	(self.have_ ## line ? gpio_write(#line, value) : 0)
+
+static int gpio_line_probe(char const *line)
+{
+	char filename[256];
+	int result;
+
+	if (self.have_gpio_switch)
+		snprintf(filename, sizeof filename,
+				"%s/%s/state", GPIO_SWITCH, line);
+	else
+		snprintf(filename, sizeof filename,
+				"%s/%s/value", DEV_CMT, line);
+
+	result = file_exists(filename);
+
+	DBG("%s: %s", line, result ? "found" : "not found");
+
+	return result;
+}
+
+/*
+ * Modem start up function
+ *
+ * Sets all lines down and leaves "power key" pressed (power key must
+ * be released after some time)
+ */
+static void gpio_start_modem_power_on(void)
+{
+	DBG("");
+
+	if (self.startup_in_progress)
+		return;
+	self.startup_in_progress = 1;
+
+	GPIO_WRITE(cmt_apeslpx, 0);	/* skip flash mode */
+	GPIO_WRITE(cmt_rst_rq, 0);	/* prevent current drain */
+
+	switch (self.rapu) {
+	case RAPU_TYPE_2:
+		GPIO_WRITE(cmt_en, 0);
+		/* 15 ms needed for ASIC poweroff */
+		usleep(20000);
+		GPIO_WRITE(cmt_en, 1);
+		break;
+
+	case RAPU_TYPE_1:
+		GPIO_WRITE(cmt_en, 0);
+		GPIO_WRITE(cmt_bsi, 0);	/* toggle BSI visible to modem */
+		GPIO_WRITE(cmt_rst, 0);	/* Assert PURX */
+		GPIO_WRITE(cmt_en, 1);	/* Press "power key" */
+		GPIO_WRITE(cmt_rst, 1);	/* Release CMT to boot */
+		break;
+	}
+
+	GPIO_WRITE(cmt_rst_rq, 1);
+}
+
+static void gpio_finish_modem_power_on(void)
+{
+	DBG("");
+
+	if (!self.startup_in_progress)
+		return;
+
+	self.startup_in_progress = 0;
+
+	switch (self.rapu) {
+	case RAPU_TYPE_2:
+		break;
+
+	case RAPU_TYPE_1:
+		GPIO_WRITE(cmt_en, 0);	/* release "power key" */
+		break;
+	}
+}
+
+static void gpio_start_modem_reset(void)
+{
+	DBG("");
+
+	if (self.reset_in_progress)
+		return;
+	self.reset_in_progress = 1;
+
+	if (self.have_cmt_rst_rq) {
+		GPIO_WRITE(cmt_rst_rq, 0); /* Just in case */
+		GPIO_WRITE(cmt_rst_rq, 1);
+	} else {
+		gpio_start_modem_power_on();
+	}
+}
+
+static void gpio_finish_modem_reset(void)
+{
+	DBG("");
+
+	if (!self.reset_in_progress)
+		return;
+
+	self.reset_in_progress = 0;
+	gpio_finish_modem_power_on();
+}
+
+static void gpio_finish_modem_power_off(void)
+{
+	DBG("");
+
+	if (self.reset_in_progress)
+		gpio_finish_modem_reset();
+
+	if (self.startup_in_progress)
+		gpio_finish_modem_power_on();
+
+	GPIO_WRITE(cmt_apeslpx, 0);	/* skip flash mode */
+	GPIO_WRITE(cmt_rst_rq, 0);	/* prevent current drain */
+
+	switch (self.rapu) {
+	case RAPU_TYPE_2:
+		GPIO_WRITE(cmt_en, 0);	/* Power off */
+		break;
+
+	case RAPU_TYPE_1:
+		GPIO_WRITE(cmt_en, 0);	/* release "power key" */
+		GPIO_WRITE(cmt_rst, 0);	/* force modem to reset state */
+		GPIO_WRITE(cmt_rst, 1);	/* release modem to be powered
+							off by bootloader */
+		break;
+	}
+}
+
+static gboolean gpio_power_timer_cb(gpointer user)
+{
+	self.timeout_source = 0;
+
+	if (self.timer_event)
+		gpio_power_state_machine(self.timer_event);
+
+	return FALSE;
+}
+
+
+static void gpio_power_state_machine(enum power_event event)
+{
+	enum power_state new_state;
+
+	DBG("(%s) @ state %s",
+		gpio_power_event_name(event),
+		gpio_power_state_name(self.state));
+
+	switch (event) {
+	case POWER_EVENT_ON:
+		self.target = POWER_EVENT_PHONET_LINK_UP;
+
+		if (self.current == PHONET_LINK_NONE)
+			return;
+
+		switch (self.state) {
+		case POWER_STATE_ON_STARTED:
+		case POWER_STATE_ON_RESET:
+		case POWER_STATE_ON:
+			/* Do nothing */
+			break;
+
+		case POWER_STATE_OFF_STARTED:
+			/* Do nothing */
+			break;
+
+		case POWER_STATE_NONE:
+		case POWER_STATE_OFF_WAITING:
+		case POWER_STATE_OFF:
+		case POWER_STATE_ON_FAILED:
+			gpio_power_set_state(POWER_STATE_ON_STARTED);
+			break;
+		}
+		return;
+
+	case POWER_EVENT_PHONET_LINK_DOWN:
+
+		switch (self.target) {
+		case PHONET_LINK_UP:
+			break;
+
+		case PHONET_LINK_DOWN:
+		case PHONET_LINK_NONE:
+		default:
+			if (self.state == POWER_STATE_OFF ||
+					self.state == POWER_STATE_NONE)
+				new_state = POWER_STATE_OFF;
+			else
+				new_state = POWER_STATE_OFF_WAITING;
+
+			gpio_power_set_state(new_state);
+			return;
+		}
+
+		switch (self.state) {
+		case POWER_STATE_NONE:
+			/* first connection down event => start modem */
+			gpio_power_set_state(POWER_STATE_ON_STARTED);
+			break;
+
+		case POWER_STATE_ON_STARTED:
+		case POWER_STATE_ON_RESET:
+			break;
+
+		default:
+			self.retry_count = 0;
+			gpio_power_set_state(POWER_STATE_ON_RESET);
+			break;
+		}
+		return;
+
+	case POWER_EVENT_ON_TIMEOUT:
+
+		if (self.target == PHONET_LINK_DOWN)
+			new_state = POWER_STATE_OFF_STARTED;
+		else if (self.retry_count <= RETRY_COUNT_POWER_ON)
+			new_state = POWER_STATE_ON_STARTED;
+		else
+			new_state = POWER_STATE_ON_FAILED;
+
+		gpio_power_set_state(new_state);
+		return;
+
+	case POWER_EVENT_REBOOT_TIMEOUT:
+		/* Modem not rebooting - try to powercycle */
+		if (self.target == PHONET_LINK_DOWN)
+			new_state = POWER_STATE_OFF_STARTED;
+		else if (self.retry_count <= RETRY_COUNT_RESET)
+			new_state = POWER_STATE_ON_RESET;
+		else
+			new_state = POWER_STATE_ON_STARTED;
+
+		gpio_power_set_state(new_state);
+		return;
+
+	case POWER_EVENT_PHONET_LINK_UP:
+
+		switch (self.state) {
+		case POWER_STATE_NONE:
+			return;
+
+		case POWER_STATE_ON_STARTED:
+		case POWER_STATE_ON_RESET:
+			break;
+
+		case POWER_STATE_ON:
+			return;
+
+		case POWER_STATE_OFF_STARTED:
+		case POWER_STATE_OFF_WAITING:
+		case POWER_STATE_OFF:
+		case POWER_STATE_ON_FAILED:
+			DBG("LINK_UP event while modem should be powered off");
+			/* should never come here */
+			break;
+		}
+
+		if (self.target == PHONET_LINK_DOWN)
+			gpio_power_set_state(POWER_STATE_OFF_STARTED);
+		else
+			gpio_power_set_state(POWER_STATE_ON);
+		return;
+
+	case POWER_EVENT_OFF:
+		self.target = PHONET_LINK_DOWN;
+
+		switch (self.state) {
+		case POWER_STATE_ON_STARTED:
+		case POWER_STATE_ON_RESET:
+			/* Do nothing until a timer expires */
+			break;
+
+		case POWER_STATE_ON:
+			gpio_power_set_state(POWER_STATE_OFF_STARTED);
+			break;
+
+		case POWER_STATE_OFF_STARTED:
+		case POWER_STATE_OFF_WAITING:
+		case POWER_STATE_OFF:
+			/* Do nothing */
+			break;
+
+		case POWER_STATE_NONE:
+		case POWER_STATE_ON_FAILED:
+			gpio_power_set_state(POWER_STATE_OFF);
+			break;
+		}
+		return;
+
+	case POWER_EVENT_OFF_IMMEDIATELY:
+		gpio_power_set_state(POWER_STATE_OFF);
+		return;
+
+	case POWER_EVENT_OFF_TIMEOUT:
+		DBG("Modem power off timed out");
+		gpio_power_set_state(POWER_STATE_OFF);
+		return;
+
+	case POWER_EVENT_OFF_COMPLETE:
+		if (self.state == POWER_STATE_OFF_WAITING) {
+			DBG("Modem shutdown complete");
+			gpio_power_set_state(POWER_STATE_OFF);
+		}
+		return;
+	}
+
+	DBG("Event %s (%d) not handled", gpio_power_event_name(event), event);
+}
+
+
+static void gpio_power_set_state(enum power_state new_state)
+{
+	enum power_state old_state = self.state;
+	unsigned timeout = 0;
+	enum power_event timer_event;
+
+	DBG("(%s) at (%s)%s",
+		gpio_power_state_name(new_state),
+		gpio_power_state_name(old_state),
+		new_state == old_state ? " - already" : "");
+
+	switch (old_state) {
+	case POWER_STATE_ON_STARTED:
+		gpio_finish_modem_power_on();
+		break;
+
+	case POWER_STATE_ON_RESET:
+		gpio_finish_modem_reset();
+		break;
+
+	default:
+		break;
+	}
+
+	if (self.timeout_source) {
+		g_source_remove(self.timeout_source);
+		self.timeout_source = 0;
+		self.timer_event = 0;
+	}
+
+	if (old_state == new_state
+			&& new_state != POWER_STATE_ON_STARTED
+			&& new_state != POWER_STATE_ON_RESET)
+		return;
+
+	self.state = new_state;
+
+	switch (self.state) {
+	case POWER_STATE_NONE:
+		break;
+
+	case POWER_STATE_ON_STARTED:
+		self.retry_count++;
+
+		/* Maximum time modem power on procedure on can take */
+		timeout = 5000;
+		timer_event = POWER_EVENT_ON_TIMEOUT;
+		gpio_start_modem_power_on();
+		break;
+
+	case POWER_STATE_ON_RESET:
+		DBG("Starting modem restart timeout");
+
+		/* Time allowed for modem to restart after crash */
+		timeout = 5000;
+		timer_event = POWER_EVENT_REBOOT_TIMEOUT;
+
+		if (self.retry_count++ > 0)
+			gpio_start_modem_reset();
+		break;
+
+	case POWER_STATE_ON:
+		DBG("Power on");
+		self.retry_count = 0;
+		break;
+
+	case POWER_STATE_OFF_STARTED:
+		DBG("Starting power off");
+
+		/* Maximum time modem power_off can take */
+		timeout = 6150;
+		timer_event = POWER_EVENT_OFF_TIMEOUT;
+		break;
+
+	case POWER_STATE_OFF_WAITING:
+		gpio_finish_modem_power_off();
+		DBG("Waiting for modem to settle down");
+
+		/* Cooling time after power off */
+		timeout = 1000;
+		timer_event = POWER_EVENT_OFF_COMPLETE;
+		break;
+
+	case POWER_STATE_OFF:
+
+		if (old_state != POWER_STATE_OFF_WAITING
+				&& old_state != POWER_STATE_ON_FAILED)
+			gpio_finish_modem_power_off();
+		break;
+
+	case POWER_STATE_ON_FAILED:
+		DBG("Link to modem cannot be established, giving up");
+		gpio_finish_modem_power_off();
+		break;
+	}
+
+	if (timeout) {
+		self.timer_event = timer_event;
+		self.timeout_source = g_timeout_add(timeout,
+					gpio_power_timer_cb, NULL);
+	}
+
+	self.callback(new_state, self.data);
+}
+
+static void phonet_status_cb(GIsiModem *idx, GPhonetLinkState state,
+				char const *ifname, void *dummy)
+{
+	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) {
+
+		if (self.current == PHONET_LINK_UP)
+			return;
+
+		self.current = PHONET_LINK_UP;
+
+		/* link is up - we can lower cmt_rst_rq */
+		GPIO_WRITE(cmt_rst_rq, 0);
+
+		gpio_power_state_machine(POWER_EVENT_PHONET_LINK_UP);
+	} else {
+
+		if (self.current == PHONET_LINK_DOWN)
+			return;
+
+		self.current = PHONET_LINK_DOWN;
+
+		gpio_power_state_machine(POWER_EVENT_PHONET_LINK_DOWN);
+	}
+}
+
+static int gpio_probe_links(void)
+{
+	char const *gpiodir = "/sys/class/gpio";
+	char const *cmtdir = "/dev/cmt";
+	DIR *gpio;
+	struct dirent *d, entry[1];
+
+	if (file_exists(cmtdir)) {
+		DBG("Using %s", cmtdir);
+		return 0;
+	}
+
+	DBG("Using %s: trying to make links to %s", gpiodir, cmtdir);
+
+	if (!dir_exists(cmtdir)) {
+
+		if (mkdir(cmtdir, 0755) == -1) {
+			DBG("%s: %s", cmtdir, strerror(errno));
+			return -(errno = ENODEV);
+		}
+	}
+
+	gpio = opendir(gpiodir);
+	if (gpio == NULL) {
+		DBG("%s: %s", "gpiodir", strerror(errno));
+		return -(errno = ENODEV);
+	}
+
+	while (readdir_r(gpio, entry, &d) == 0) {
+		char nn[PATH_MAX], name[PATH_MAX], from[PATH_MAX], to[PATH_MAX];
+		FILE *nf;
+		size_t len;
+
+		if (d == NULL)
+			return 0;
+
+		snprintf(nn, sizeof nn, "%s/%s/name", gpiodir, d->d_name);
+
+		nf = fopen(nn, "rb");
+		if (nf == NULL) {
+			DBG("%s: %s", nn, strerror(errno));
+			continue;
+		}
+
+		len = fread(name, sizeof name, 1, nf);
+
+		if (ferror(nf)) {
+			DBG("read from %s: %s", nn, strerror(errno));
+			fclose(nf);
+			continue;
+		}
+
+		fclose(nf);
+
+		if (len < 4)
+			continue;
+
+		name[--len] = '\0';
+
+		if (strncmp(name, "cmt_", 4))
+			continue;
+
+		snprintf(from, sizeof from, "%s/%s", gpiodir, d->d_name);
+		snprintf(to, sizeof to, "%s/%s", cmtdir, name);
+
+		if (symlink(from, to) == -1)
+			DBG("%s: %s", to, strerror(errno));
+	}
+
+	DBG("%s: %s", "/sys/class/gpio", strerror(errno));
+
+	return -(errno = ENODEV);
+}
+
+
+int gpio_probe(GIsiModem *idx, unsigned addr, gpio_finished_cb_t cb, void *data)
+{
+	int error;
+
+	if (!cb) {
+		DBG("gpio: No callback given");
+		return -(errno = EFAULT);
+	}
+
+	if (self.callback) {
+		DBG("gpio: %s", strerror(EBUSY));
+		return -(errno = EBUSY);
+	}
+
+	if (g_pn_netlink_by_modem(idx)) {
+		DBG("Phonet link %p: %s", idx, strerror(EBUSY));
+		return -(errno = EBUSY);
+	}
+
+	self.target = PHONET_LINK_NONE;
+	self.have_gpio_switch = file_exists(GPIO_SWITCH);
+
+	if (self.have_gpio_switch) {
+		DBG("Using GPIO switch");
+	} else {
+		error = gpio_probe_links();
+		if (error)
+			return error;
+	}
+
+	/* GPIO lines availability depends on HW and SW versions */
+	self.have_cmt_en = gpio_line_probe("cmt_en");
+	self.have_cmt_rst_rq = gpio_line_probe("cmt_rst_rq");
+	self.have_cmt_rst = gpio_line_probe("cmt_rst");
+	self.have_cmt_bsi = gpio_line_probe("cmt_bsi");
+	self.have_cmt_apeslpx = gpio_line_probe("cmt_apeslpx");
+
+	if (!self.have_cmt_en) {
+		DBG("Modem control GPIO lines are not available");
+		memset(&self, 0, sizeof self);
+		return -(errno = ENODEV);
+	}
+
+	if (self.have_cmt_bsi)
+		self.rapu = RAPU_TYPE_1;
+	else
+		self.rapu = RAPU_TYPE_2;
+
+	self.link = g_pn_netlink_start(idx, phonet_status_cb, NULL);
+	if (!self.link) {
+		memset(&self, 0, sizeof self);
+		return -errno;
+	}
+
+	self.callback = cb;
+	self.data = data;
+
+	if (addr) {
+		error = g_pn_netlink_set_address(idx, addr);
+		if (error && error != -EEXIST)
+			DBG("g_pn_netlink_set_address: %s", strerror(-error));
+	}
+
+#if notyet
+	if (route) {
+		error = g_pn_netlink_add_route(idx, PN_DEV_HOST);
+		/* We get ENOTSUP on Maemo 5 kernel */
+		if (error && error != -ENOTSUP)
+			DBG("g_pn_netlink_add_route: %s", strerror(-error));
+	}
+#endif
+
+	return 0;
+}
+
+int gpio_remove(void *data)
+{
+	if (self.data != data)
+		return -EINVAL;
+
+	if (self.link)
+		g_pn_netlink_stop(self.link);
+
+	if (self.timeout_source) {
+		g_source_remove(self.timeout_source);
+		self.timeout_source = 0;
+	}
+
+	memset(&self, 0, sizeof self);
+
+	return 0;
+}
+
+int gpio_enable(void *data)
+{
+	if (self.data != data)
+		return -EINVAL;
+
+	if (self.state == POWER_STATE_ON)
+		return 0;
+
+	gpio_power_state_machine(POWER_EVENT_ON);
+
+	return -EINPROGRESS;
+}
+
+int gpio_disable(void *data)
+{
+	if (self.data != data)
+		return -EINVAL;
+
+	if (self.state == POWER_STATE_OFF
+			|| self.state == POWER_STATE_ON_FAILED)
+		return 0;
+
+	gpio_power_state_machine(POWER_EVENT_OFF);
+
+	return -EINPROGRESS;
+}
diff --git a/plugins/nokia-gpio.h b/plugins/nokia-gpio.h
new file mode 100644
index 00000000..d0e1b0c0
--- /dev/null
+++ b/plugins/nokia-gpio.h
@@ -0,0 +1,53 @@
+/*
+ * This file is part of oFono - Open Source Telephony
+ *
+ * Copyright (C) 2010  Nokia Corporation and/or its subsidiary(-ies).
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __NOKIA_GPIO_H
+#define __NOKIA_GPIO_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+enum power_state {
+	POWER_STATE_NONE,
+	POWER_STATE_ON_STARTED,
+	POWER_STATE_ON,
+	POWER_STATE_ON_RESET,
+	POWER_STATE_ON_FAILED,
+	POWER_STATE_OFF_STARTED,
+	POWER_STATE_OFF_WAITING,
+	POWER_STATE_OFF,
+};
+
+typedef void (*gpio_finished_cb_t)(enum power_state value, void *opaque);
+
+int gpio_probe(GIsiModem *idx, unsigned addr, gpio_finished_cb_t cb, void *data);
+int gpio_enable(void *opaque);
+int gpio_disable(void *opaque);
+int gpio_remove(void *opaque);
+
+char const *gpio_power_state_name(enum power_state value);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif /* __NOKIA_GPIO_H */