Add implementation of telephony daemon

21 files changed, 9881 insertions, 1 deletions

diff --git a/src/Makefile.am b/src/Makefile.am
index 9394aa98..0c0c8a5b 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -7,7 +7,11 @@ endif
 
 sbin_PROGRAMS = ofonod
 
-ofonod_SOURCES = main.c ofono.h log.c plugin.c
+ofonod_SOURCES = main.c ofono.h log.c plugin.c \
+		driver.h modem.h modem.c common.h common.c \
+		manager.c dbus-gsm.h dbus-gsm.c util.h util.c \
+		network.c voicecall.c ussd.h ussd.c \
+		call-settings.c call-waiting.c call-forwarding.c call-meter.c
 
 ofonod_LDADD = @GDBUS_LIBS@ @GLIB_LIBS@ @GTHREAD_LIBS@ -ldl
 
diff --git a/src/call-forwarding.c b/src/call-forwarding.c
new file mode 100644
index 00000000..b7b100e1
--- /dev/null
+++ b/src/call-forwarding.c
@@ -0,0 +1,1431 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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 <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <dbus/dbus.h>
+#include <glib.h>
+#include <gdbus.h>
+
+#include "ofono.h"
+
+#include "dbus-gsm.h"
+#include "modem.h"
+#include "driver.h"
+#include "common.h"
+#include "ussd.h"
+
+#define CALL_FORWARDING_INTERFACE "org.ofono.CallForwarding"
+
+#define CALL_FORWARDING_FLAG_CACHED 0x1
+
+/* According to 27.007 Spec */
+#define DEFAULT_NO_REPLY_TIMEOUT 20
+
+struct call_forwarding_data {
+	struct ofono_call_forwarding_ops *ops;
+	GSList *cf_conditions[4];
+	int flags;
+	DBusMessage *pending;
+	struct cf_ss_request *ss_req;
+};
+
+static void cf_busy_callback(const struct ofono_error *error, int total,
+				const struct ofono_cf_condition *list, void *data);
+static void cf_unconditional_callback(const struct ofono_error *error, int total,
+				const struct ofono_cf_condition *list, void *data);
+
+static void cf_register_ss_controls(struct ofono_modem *modem);
+static void cf_unregister_ss_controls(struct ofono_modem *modem);
+
+struct set_cf_request {
+	struct ofono_modem *modem;
+	int type;
+	int cls;
+	char number[OFONO_MAX_PHONE_NUMBER_LENGTH + 1];
+	int number_type;
+	int timeout;
+};
+
+struct cf_ss_request {
+	int ss_type;
+	int cf_type;
+	int cls;
+	GSList *cf_list[4];
+};
+
+static gint cf_condition_compare(gconstpointer a, gconstpointer b)
+{
+	const struct ofono_cf_condition *ca = a;
+	const struct ofono_cf_condition *cb = b;
+
+	if (ca->cls < cb->cls)
+		return -1;
+
+	if (ca->cls > cb->cls)
+		return 1;
+
+	return 0;
+}
+
+static gint cf_condition_find_with_cls(gconstpointer a, gconstpointer b)
+{
+	const struct ofono_cf_condition *c = a;
+	int cls = GPOINTER_TO_INT(b);
+
+	if (c->cls < cls)
+		return -1;
+
+	if (c->cls > cls)
+		return 1;
+
+	return 0;
+}
+
+static int cf_find_timeout(GSList *cf_list, int cls)
+{
+	GSList *l;
+	struct ofono_cf_condition *c;
+
+	l = g_slist_find_custom(cf_list, GINT_TO_POINTER(cls),
+		cf_condition_find_with_cls);
+
+	if (!l)
+		return DEFAULT_NO_REPLY_TIMEOUT;
+
+	c = l->data;
+
+	return c->time;
+}
+
+static void cf_cond_list_print(GSList *list)
+{
+	GSList *l;
+	struct ofono_cf_condition *cond;
+
+	for (l = list; l; l = l->next) {
+		cond = l->data;
+
+		ofono_debug("CF Condition status: %d, class: %d, number: %s,"
+			" number_type: %d, time: %d",
+			cond->status, cond->cls, cond->phone_number,
+			cond->number_type, cond->time);
+	}
+}
+
+static GSList *cf_cond_list_create(int total,
+					const struct ofono_cf_condition *list)
+{
+	GSList *l = NULL;
+	int i;
+	int j;
+	struct ofono_cf_condition *cond;
+
+	/* Specification is not really clear how the results are reported,
+	 * so assume both multiple list items & compound values of class
+	 * are possible
+	 */
+	for (i = 0; i < total; i++) {
+		for (j = 1; j <= BEARER_CLASS_PAD; j = j << 1) {
+			if (!(list[i].cls & j))
+				continue;
+
+			if (list[i].status == 0)
+				continue;
+
+			cond = g_try_new0(struct ofono_cf_condition, 1);
+			if (!cond)
+				continue;
+
+			memcpy(cond, &list[i], sizeof(struct ofono_cf_condition));
+			cond->cls = j;
+
+			l = g_slist_insert_sorted(l, cond,
+							cf_condition_compare);
+		}
+	}
+
+	return l;
+}
+
+static inline void cf_list_clear(GSList *cf_list)
+{
+	GSList *l;
+
+	for (l = cf_list; l; l = l->next)
+		g_free(l->data);
+
+	g_slist_free(cf_list);
+}
+
+static inline void cf_clear_all(struct call_forwarding_data *cf)
+{
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		cf_list_clear(cf->cf_conditions[i]);
+		cf->cf_conditions[i] = NULL;
+	}
+}
+
+static struct call_forwarding_data *call_forwarding_create()
+{
+	struct call_forwarding_data *r;
+
+	r = g_try_new0(struct call_forwarding_data, 1);
+
+	if (!r)
+		return r;
+
+	return r;
+}
+
+static void call_forwarding_destroy(gpointer data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+
+	cf_clear_all(cf);
+
+	cf_unregister_ss_controls(modem);
+
+	g_free(cf);
+}
+
+static const char *cf_type_lut[] = {
+	"Unconditional",
+	"Busy",
+	"NoReply",
+	"NotReachable",
+	"All",
+	"AllConditional"
+};
+
+static void set_new_cond_list(struct ofono_modem *modem, int type, GSList *list)
+{
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	GSList *old = cf->cf_conditions[type];
+	DBusConnection *conn = dbus_gsm_connection();
+	GSList *l;
+	GSList *o;
+	struct ofono_cf_condition *lc;
+	struct ofono_cf_condition *oc;
+	const char *number;
+	dbus_uint16_t timeout;
+	char attr[64];
+	char tattr[64];
+
+	for (l = list; l; l = l->next) {
+		lc = l->data;
+
+		/* New condition lists might have attributes we don't care about
+		 * triggered by e.g. ss control magic strings just skip them
+		 * here
+		 */
+		if (lc->cls > BEARER_CLASS_SMS)
+			continue;
+
+		timeout = lc->time;
+		number = phone_number_to_string(lc->phone_number,
+						lc->number_type);
+
+		sprintf(attr, "%s%s", bearer_class_to_string(lc->cls),
+					cf_type_lut[type]);
+
+		if (type == CALL_FORWARDING_TYPE_NO_REPLY)
+			sprintf(tattr, "%sTimeout", attr);
+
+		o = g_slist_find_custom(old, GINT_TO_POINTER(lc->cls),
+					cf_condition_find_with_cls);
+
+		if (o) { /* On the old list, must be active */
+			oc = o->data;
+
+			if (oc->number_type != lc->number_type ||
+				strcmp(oc->phone_number, lc->phone_number))
+				dbus_gsm_signal_property_changed(conn,
+						modem->path,
+						CALL_FORWARDING_INTERFACE,
+						attr, DBUS_TYPE_STRING,
+						&number);
+
+			if (type == CALL_FORWARDING_TYPE_NO_REPLY &&
+				oc->time != lc->time)
+				dbus_gsm_signal_property_changed(conn,
+						modem->path,
+						CALL_FORWARDING_INTERFACE,
+						tattr, DBUS_TYPE_UINT16,
+						&timeout);
+
+			/* Remove from the old list */
+			g_free(o->data);
+			old = g_slist_remove(old, o->data);
+		} else {
+			number = phone_number_to_string(lc->phone_number,
+							lc->number_type);
+
+			dbus_gsm_signal_property_changed(conn, modem->path,
+						CALL_FORWARDING_INTERFACE,
+						attr, DBUS_TYPE_STRING,
+						&number);
+
+			if (type == CALL_FORWARDING_TYPE_NO_REPLY &&
+				lc->time != DEFAULT_NO_REPLY_TIMEOUT)
+				dbus_gsm_signal_property_changed(conn,
+						modem->path,
+						CALL_FORWARDING_INTERFACE,
+						tattr, DBUS_TYPE_UINT16,
+						&timeout);
+		}
+	}
+
+	timeout = DEFAULT_NO_REPLY_TIMEOUT;
+	number = "";
+
+	for (o = old; o; o = o->next) {
+		oc = o->data;
+
+		sprintf(attr, "%s%s", bearer_class_to_string(oc->cls),
+					cf_type_lut[type]);
+
+		if (type == CALL_FORWARDING_TYPE_NO_REPLY)
+			sprintf(tattr, "%sTimeout", attr);
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+					CALL_FORWARDING_INTERFACE, attr,
+					DBUS_TYPE_STRING, &number);
+
+		if (type == CALL_FORWARDING_TYPE_NO_REPLY &&
+			oc->time != DEFAULT_NO_REPLY_TIMEOUT)
+			dbus_gsm_signal_property_changed(conn, modem->path,
+						CALL_FORWARDING_INTERFACE,
+						tattr, DBUS_TYPE_UINT16,
+						&timeout);
+	}
+
+	cf_list_clear(old);
+	cf->cf_conditions[type] = list;
+}
+
+static inline void property_append_cf_condition(DBusMessageIter *dict, int cls,
+						const char *postfix,
+						const char *value,
+						dbus_uint16_t timeout)
+{
+	char attr[64];
+	char tattr[64];
+	int addt = !strcmp(postfix, "NoReply");
+
+	sprintf(attr, "%s%s", bearer_class_to_string(cls), postfix);
+
+	if (addt)
+		sprintf(tattr, "%s%sTimeout", bearer_class_to_string(cls),
+			postfix);
+
+	dbus_gsm_dict_append(dict, attr, DBUS_TYPE_STRING, &value);
+
+	if (addt)
+		dbus_gsm_dict_append(dict, tattr, DBUS_TYPE_UINT16, &timeout);
+}
+
+static void property_append_cf_conditions(DBusMessageIter *dict,
+						GSList *cf_list, int mask,
+						const char *postfix)
+{
+	GSList *l;
+	int i;
+	struct ofono_cf_condition *cf;
+	const char *number;
+
+	for (i = 1, l = cf_list; i <= BEARER_CLASS_PAD; i = i << 1) {
+		if (!(mask & i))
+			continue;
+
+		while (l && (cf = l->data) && (cf->cls < i))
+				l = l->next;
+
+		if (!l || cf->cls != i) {
+			property_append_cf_condition(dict, i, postfix, "",
+						DEFAULT_NO_REPLY_TIMEOUT);
+			continue;
+		}
+
+		number = phone_number_to_string(cf->phone_number,
+						cf->number_type);
+
+		property_append_cf_condition(dict, i, postfix, number,
+						cf->time);
+	}
+}
+
+static DBusMessage *cf_get_properties_reply(DBusMessage *msg,
+						struct call_forwarding_data *cf)
+{
+	DBusMessage *reply;
+	DBusMessageIter iter;
+	DBusMessageIter dict;
+	int i;
+
+	reply = dbus_message_new_method_return(msg);
+
+	if (!reply)
+		return NULL;
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+						PROPERTIES_ARRAY_SIGNATURE,
+						&dict);
+
+	for (i = 0; i < 4; i++)
+		property_append_cf_conditions(&dict,
+				cf->cf_conditions[i],
+				BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS,
+				cf_type_lut[i]);
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+static DBusMessage *cf_get_properties(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+
+	if (cf->flags & CALL_FORWARDING_FLAG_CACHED)
+		return cf_get_properties_reply(msg, cf);
+
+	/* We kicked off the query during interface creation, wait for it */
+	cf->pending = dbus_message_ref(msg);
+
+	return NULL;
+}
+
+static gboolean cf_condition_enabled_property(struct call_forwarding_data *cf,
+			const char *property, int *out_type, int *out_cls)
+{
+	int i;
+	int j;
+	int len;
+	const char *prefix;
+
+	/* We check the 4 bearer classes here, e.g. voice, data, fax, sms */
+	for (i = 0; i < 4; i++) {
+		prefix = bearer_class_to_string(1 << i);
+
+		len = strlen(prefix);
+
+		if (strncmp(property, prefix, len))
+			continue;
+
+		/* We check the 4 call forwarding types, e.g.
+		 * unconditional, busy, no reply, not reachable
+		 */
+		for (j = 0; j < 4; j++)
+			if (!strcmp(property+len, cf_type_lut[j])) {
+				*out_type = j;
+				*out_cls = 1 << i;
+				return TRUE;
+			}
+	}
+
+	return FALSE;
+}
+
+static gboolean cf_condition_timeout_property(const char *property,
+						int *out_cls)
+{
+	int i;
+	int len;
+	const char *prefix;
+
+	for (i = 0; i < 4; i++) {
+		prefix = bearer_class_to_string(1 << i);
+
+		len = strlen(prefix);
+
+		if (strncmp(property, prefix, len))
+			continue;
+
+		if (!strcmp(property+len, "NoReplyTimeout")) {
+			*out_cls = 1 << i;
+			return TRUE;
+		}
+	}
+
+	return FALSE;
+}
+
+static void cf_condition_manual_set(struct set_cf_request *req)
+{
+	struct ofono_modem *modem = req->modem;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	DBusConnection *conn = dbus_gsm_connection();
+	int status = req->number[0] == '\0' ? 0 : 1;
+	GSList *l;
+	struct ofono_cf_condition *c;
+	char attr[64];
+	char tattr[64];
+	const char *number = "";
+	dbus_uint16_t timeout;
+
+	l = g_slist_find_custom(cf->cf_conditions[req->type],
+					GINT_TO_POINTER(req->cls),
+					cf_condition_find_with_cls);
+
+	ofono_debug("L is: %p, status is: %d", l, status);
+
+	if (!l && !status)
+		return;
+
+	sprintf(attr, "%s%s", bearer_class_to_string(req->cls),
+					cf_type_lut[req->type]);
+
+	if (req->type == CALL_FORWARDING_TYPE_NO_REPLY)
+		sprintf(tattr, "%sTimeout", attr);
+
+	if (l && !status) {
+		c = l->data;
+		timeout = DEFAULT_NO_REPLY_TIMEOUT;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+			CALL_FORWARDING_INTERFACE,
+			attr, DBUS_TYPE_STRING, &number);
+
+		if (req->type == CALL_FORWARDING_TYPE_NO_REPLY &&
+			c->time != DEFAULT_NO_REPLY_TIMEOUT)
+			dbus_gsm_signal_property_changed(conn, modem->path,
+				CALL_FORWARDING_INTERFACE, tattr,
+				DBUS_TYPE_UINT16, &timeout);
+
+		ofono_debug("Removing condition");
+
+		g_free(c);
+		cf->cf_conditions[req->type] =
+			g_slist_remove(cf->cf_conditions[req->type], c);
+
+		return;
+	}
+
+	if (l)
+		c = l->data;
+	else {
+		c = g_try_new0(struct ofono_cf_condition, 1);
+
+		if (!c)
+			return;
+
+		c->status = 1;
+		c->cls = req->cls;
+		c->phone_number[0] = '\0';
+		c->number_type = 129;
+		c->time = DEFAULT_NO_REPLY_TIMEOUT;
+
+		ofono_debug("Inserting condition");
+		cf->cf_conditions[req->type] =
+			g_slist_insert_sorted(cf->cf_conditions[req->type],
+						c, cf_condition_compare);
+	}
+
+	if (c->number_type != req->number_type ||
+		strcmp(req->number, c->phone_number)) {
+		strcpy(c->phone_number, req->number);
+		c->number_type = req->number_type;
+
+		number = phone_number_to_string(req->number, req->number_type);
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+				CALL_FORWARDING_INTERFACE,
+				attr, DBUS_TYPE_STRING, &number);
+	}
+
+	if (req->type == CALL_FORWARDING_TYPE_NO_REPLY &&
+		c->time != req->timeout) {
+		c->time = req->timeout;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+				CALL_FORWARDING_INTERFACE,
+				tattr, DBUS_TYPE_UINT16, &req->timeout);
+	}
+}
+
+static void pending_msg_error(struct call_forwarding_data *cf,
+				const struct ofono_error *error)
+{
+	DBusMessage *reply;
+	DBusConnection *conn = dbus_gsm_connection();
+
+	reply = dbus_gsm_failed(cf->pending);
+	g_dbus_send_message(conn, reply);
+
+	dbus_message_unref(cf->pending);
+	cf->pending = NULL;
+}
+
+static void property_set_query_callback(const struct ofono_error *error, int total,
+					const struct ofono_cf_condition *list,
+					void *data)
+{
+	struct set_cf_request *req = data;
+	struct ofono_modem *modem = req->modem;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	//DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *reply;
+	GSList *new_cf_list;
+
+	reply = dbus_message_new_method_return(cf->pending);
+	dbus_gsm_pending_reply(&cf->pending, reply);
+
+	/* Strange, set succeeded but query failed, fallback to direct method */
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error occurred during query");
+		cf_condition_manual_set(req);
+		goto out;
+	}
+
+	new_cf_list = cf_cond_list_create(total, list);
+
+	ofono_debug("Query ran successfully");
+	cf_cond_list_print(new_cf_list);
+
+	set_new_cond_list(modem, req->type, new_cf_list);
+
+out:
+	g_free(req);
+}
+
+static void set_property_callback(const struct ofono_error *error, void *data)
+{
+	struct set_cf_request *req = data;
+	struct ofono_modem *modem = req->modem;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error occurred during set/erasure");
+
+		pending_msg_error(cf, error);
+		g_free(req);
+
+		return;
+	}
+
+	/* Successfully set, query the entire set just in case */
+	cf->ops->query(modem, req->type,
+			BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS,
+			property_set_query_callback, req);
+}
+
+static DBusMessage *set_property_request(struct ofono_modem *modem,
+						DBusMessage *msg,
+						int type, int cls,
+						const char *number,
+						int number_type, int timeout)
+{
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	struct set_cf_request *req;
+
+	if (number[0] != '\0' && cf->ops->registration == NULL)
+		return dbus_gsm_not_implemented(msg);
+
+	if (number[0] == '\0' && cf->ops->erasure == NULL)
+		return dbus_gsm_not_implemented(msg);
+
+	req = g_try_new0(struct set_cf_request, 1);
+
+	if (!req)
+		return dbus_gsm_failed(msg);
+
+	req->modem = modem;
+	req->type = type;
+	req->cls = cls;
+	strcpy(req->number, number);
+	req->number_type = number_type;
+	req->timeout = timeout;
+
+	cf->pending = dbus_message_ref(msg);
+
+	ofono_debug("Farming off request, will be erasure: %d", number[0] == 0);
+
+	if (number[0] != '\0')
+		cf->ops->registration(modem, type, cls, number, number_type,
+				timeout, set_property_callback, req);
+	else
+		cf->ops->erasure(modem, type, cls, set_property_callback, req);
+
+	return NULL;
+}
+
+static DBusMessage *cf_set_property(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	DBusMessageIter iter;
+	DBusMessageIter var;
+	const char *property;
+	int cls;
+	int type;
+
+	if (cf->pending)
+		return dbus_gsm_busy(msg);
+
+	if (!dbus_message_iter_init(msg, &iter))
+		return dbus_gsm_invalid_args(msg);
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_STRING)
+		return dbus_gsm_invalid_args(msg);
+
+	dbus_message_iter_get_basic(&iter, &property);
+	dbus_message_iter_next(&iter);
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_VARIANT)
+		return dbus_gsm_invalid_args(msg);
+
+	dbus_message_iter_recurse(&iter, &var);
+
+	if (cf_condition_timeout_property(property, &cls)) {
+		dbus_uint16_t timeout;
+		GSList *l;
+		struct ofono_cf_condition *c;
+
+		type = CALL_FORWARDING_TYPE_NO_REPLY;
+
+		if (dbus_message_iter_get_arg_type(&var) != DBUS_TYPE_UINT16)
+			return dbus_gsm_invalid_args(msg);
+
+		dbus_message_iter_get_basic(&var, &timeout);
+
+		if (timeout < 1 || timeout > 30)
+			return dbus_gsm_invalid_format(msg);
+
+		l = g_slist_find_custom(cf->cf_conditions[type],
+				GINT_TO_POINTER(cls),
+				cf_condition_find_with_cls);
+
+		if (!l)
+			return dbus_gsm_failed(msg);
+
+		c = l->data;
+
+		return set_property_request(modem, msg, type, cls,
+						c->phone_number,
+						c->number_type, timeout);
+	} else if (cf_condition_enabled_property(cf, property, &type, &cls)) {
+		const char *number;
+		int number_type;
+		int timeout;
+
+		if (dbus_message_iter_get_arg_type(&var) != DBUS_TYPE_STRING)
+			return dbus_gsm_invalid_args(msg);
+
+		dbus_message_iter_get_basic(&var, &number);
+
+		if (strlen(number) > 0 && !valid_phone_number_format(number))
+			return dbus_gsm_invalid_format(msg);
+
+		if (number[0] != '\0')
+			string_to_phone_number(number, &number_type, &number);
+		else
+			number_type = 129;
+
+		timeout = cf_find_timeout(cf->cf_conditions[type], cls);
+
+		return set_property_request(modem, msg, type, cls, number,
+					number_type, timeout);
+	}
+
+	return dbus_gsm_invalid_args(msg);
+}
+
+static void disable_conditional_callback(const struct ofono_error *error,
+						void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	//DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *reply;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error occurred during conditional erasure");
+
+		pending_msg_error(cf, error);
+
+		return;
+	}
+
+	reply = dbus_message_new_method_return(cf->pending);
+	dbus_gsm_pending_reply(&cf->pending, reply);
+
+	set_new_cond_list(modem, CALL_FORWARDING_TYPE_NO_REPLY, NULL);
+	set_new_cond_list(modem, CALL_FORWARDING_TYPE_NOT_REACHABLE, NULL);
+	set_new_cond_list(modem, CALL_FORWARDING_TYPE_BUSY, NULL);
+
+	cf->ops->query(modem, CALL_FORWARDING_TYPE_BUSY,
+			BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS,
+			cf_busy_callback, modem);
+}
+
+static void disable_all_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	//DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *reply;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error occurred during erasure of all");
+
+		pending_msg_error(cf, error);
+
+		return;
+	}
+
+	reply = dbus_message_new_method_return(cf->pending);
+	dbus_gsm_pending_reply(&cf->pending, reply);
+
+	set_new_cond_list(modem, CALL_FORWARDING_TYPE_UNCONDITIONAL, NULL);
+	set_new_cond_list(modem, CALL_FORWARDING_TYPE_NO_REPLY, NULL);
+	set_new_cond_list(modem, CALL_FORWARDING_TYPE_NOT_REACHABLE, NULL);
+	set_new_cond_list(modem, CALL_FORWARDING_TYPE_BUSY, NULL);
+
+	cf->ops->query(modem, CALL_FORWARDING_TYPE_UNCONDITIONAL,
+			BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS,
+			cf_unconditional_callback, modem);
+}
+
+static DBusMessage *cf_disable_all(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	const char *strtype;
+	int type;
+
+	if (cf->pending)
+		return dbus_gsm_busy(msg);
+
+	if (!cf->ops->erasure)
+		return dbus_gsm_not_implemented(msg);
+
+	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_STRING, &strtype,
+					DBUS_TYPE_INVALID) == FALSE)
+		return dbus_gsm_invalid_args(msg);
+
+	if (!strcmp(strtype, "all") || !strcmp(strtype, ""))
+		type = CALL_FORWARDING_TYPE_ALL;
+	else if (!strcmp(strtype, "conditional"))
+		type = CALL_FORWARDING_TYPE_ALL_CONDITIONAL;
+	else
+		return dbus_gsm_invalid_format(msg);
+
+	cf->pending = dbus_message_ref(msg);
+
+	if (type == CALL_FORWARDING_TYPE_ALL)
+		cf->ops->erasure(modem, type,
+				BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS,
+				disable_all_callback, modem);
+	else
+		cf->ops->erasure(modem, type,
+				BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS,
+				disable_conditional_callback, modem);
+
+	return NULL;
+}
+
+static GDBusMethodTable cf_methods[] = {
+	{ "GetProperties",	"",	"a{sv}",	cf_get_properties },
+	{ "SetProperty",	"sv",	"",		cf_set_property,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "DisableAll",		"s",	"",		cf_disable_all,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ }
+};
+
+static GDBusSignalTable cf_signals[] = {
+	{ "PropertyChanged",	"sv" },
+	{ }
+};
+
+static void cf_ss_control_reply(struct ofono_modem *modem,
+					struct cf_ss_request *req)
+{
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	const char *context = "CallForwarding";
+	const char *sig = "(ssa{sv})";
+	const char *ss_type = ss_control_type_to_string(req->ss_type);
+	const char *cf_type = cf_type_lut[req->cf_type];
+	DBusConnection *conn = dbus_gsm_connection();
+	DBusMessageIter iter;
+	DBusMessageIter variant;
+	DBusMessageIter vstruct;
+	DBusMessageIter dict;
+	DBusMessage *reply;
+
+	reply = dbus_message_new_method_return(cf->pending);
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_append_basic(&iter, DBUS_TYPE_STRING, &context);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_VARIANT, sig,
+						&variant);
+
+	dbus_message_iter_open_container(&variant, DBUS_TYPE_STRUCT, NULL,
+						&vstruct);
+
+	dbus_message_iter_append_basic(&vstruct, DBUS_TYPE_STRING,
+					&ss_type);
+
+	dbus_message_iter_append_basic(&vstruct, DBUS_TYPE_STRING,
+					&cf_type);
+
+	dbus_message_iter_open_container(&vstruct, DBUS_TYPE_ARRAY,
+					PROPERTIES_ARRAY_SIGNATURE, &dict);
+
+	if (req->cf_type == CALL_FORWARDING_TYPE_UNCONDITIONAL ||
+		req->cf_type == CALL_FORWARDING_TYPE_ALL)
+		property_append_cf_conditions(&dict,
+			req->cf_list[CALL_FORWARDING_TYPE_UNCONDITIONAL],
+			req->cls,
+			cf_type_lut[CALL_FORWARDING_TYPE_UNCONDITIONAL]);
+
+	if (req->cf_type == CALL_FORWARDING_TYPE_NO_REPLY ||
+		req->cf_type == CALL_FORWARDING_TYPE_ALL ||
+		req->cf_type == CALL_FORWARDING_TYPE_ALL_CONDITIONAL)
+		property_append_cf_conditions(&dict,
+			req->cf_list[CALL_FORWARDING_TYPE_NO_REPLY],
+			req->cls, cf_type_lut[CALL_FORWARDING_TYPE_NO_REPLY]);
+
+	if (req->cf_type == CALL_FORWARDING_TYPE_NOT_REACHABLE ||
+		req->cf_type == CALL_FORWARDING_TYPE_ALL ||
+		req->cf_type == CALL_FORWARDING_TYPE_ALL_CONDITIONAL)
+		property_append_cf_conditions(&dict,
+			req->cf_list[CALL_FORWARDING_TYPE_NOT_REACHABLE],
+			req->cls,
+			cf_type_lut[CALL_FORWARDING_TYPE_NOT_REACHABLE]);
+
+	if (req->cf_type == CALL_FORWARDING_TYPE_BUSY ||
+		req->cf_type == CALL_FORWARDING_TYPE_ALL ||
+		req->cf_type == CALL_FORWARDING_TYPE_ALL_CONDITIONAL)
+		property_append_cf_conditions(&dict,
+			req->cf_list[CALL_FORWARDING_TYPE_BUSY],
+			req->cls, cf_type_lut[CALL_FORWARDING_TYPE_BUSY]);
+
+	dbus_message_iter_close_container(&vstruct, &dict);
+
+	dbus_message_iter_close_container(&variant, &vstruct);
+
+	dbus_message_iter_close_container(&iter, &variant);
+
+	g_dbus_send_message(conn, reply);
+}
+
+static void cf_ss_control_query_callback(const struct ofono_error *error,
+					int total,
+					const struct ofono_cf_condition *list,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	//DBusConnection *conn = dbus_gsm_connection();
+	//DBusMessage *reply;
+	GSList *new_cf_list;
+
+	/* Strange, set succeeded but query failed, fallback to direct method */
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error occurred during cf ss query");
+
+		pending_msg_error(cf, error);
+
+		return;
+	}
+
+	new_cf_list = cf_cond_list_create(total, list);
+
+	ofono_debug("Query ran successfully");
+	cf_cond_list_print(new_cf_list);
+
+	cf->ss_req->cf_list[cf->ss_req->cf_type] = new_cf_list;
+
+	set_new_cond_list(modem, cf->ss_req->cf_type, new_cf_list);
+
+	cf_ss_control_reply(modem, cf->ss_req);
+
+	dbus_message_unref(cf->pending);
+	cf->pending = NULL;
+
+	g_free(cf->ss_req);
+	cf->ss_req = NULL;
+}
+
+static void cf_ss_control_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	int cls;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error occurred during cf ss control set/erasure");
+
+		pending_msg_error(cf, error);
+
+		return;
+	}
+
+	cls = BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS | cf->ss_req->cls;
+
+	/* Successfully set, query the entire set just in case */
+	if (cf->ss_req->cf_type == CALL_FORWARDING_TYPE_ALL)
+		cf->ops->query(modem, CALL_FORWARDING_TYPE_UNCONDITIONAL,
+				cls, cf_unconditional_callback, modem);
+	else if (cf->ss_req->cf_type == CALL_FORWARDING_TYPE_ALL_CONDITIONAL)
+		cf->ops->query(modem, CALL_FORWARDING_TYPE_BUSY,
+				cls, cf_busy_callback, modem);
+	else
+		cf->ops->query(modem, cf->ss_req->cf_type, cls,
+				cf_ss_control_query_callback, modem);
+}
+
+static gboolean cf_ss_control(struct ofono_modem *modem, int type, const char *sc,
+				const char *sia, const char *sib,
+				const char *sic, const char *dn,
+				DBusMessage *msg)
+{
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	DBusConnection *conn = dbus_gsm_connection();
+	int cls = BEARER_CLASS_DEFAULT;
+	int timeout = DEFAULT_NO_REPLY_TIMEOUT;
+	int cf_type;
+	DBusMessage *reply;
+	const char *number;
+	int number_type;
+	void *operation;
+
+	/* Before we do anything, make sure we're actually initialized */
+	if (!cf)
+		return FALSE;
+
+	if (cf->pending) {
+		reply = dbus_gsm_busy(msg);
+		g_dbus_send_message(conn, reply);
+
+		return TRUE;
+	}
+
+	ofono_debug("Received call forwarding ss control request");
+
+	ofono_debug("type: %d, sc: %s, sia: %s, sib: %s, sic: %s, dn: %s",
+			type, sc, sia, sib, sic, dn);
+
+	if (!strcmp(sc, "21"))
+		cf_type = CALL_FORWARDING_TYPE_UNCONDITIONAL;
+	else if (!strcmp(sc, "67"))
+		cf_type = CALL_FORWARDING_TYPE_BUSY;
+	else if (!strcmp(sc, "61"))
+		cf_type = CALL_FORWARDING_TYPE_NO_REPLY;
+	else if (!strcmp(sc, "62"))
+		cf_type = CALL_FORWARDING_TYPE_NOT_REACHABLE;
+	else if (!strcmp(sc, "002"))
+		cf_type = CALL_FORWARDING_TYPE_ALL;
+	else if (!strcmp(sc, "004"))
+		cf_type = CALL_FORWARDING_TYPE_ALL_CONDITIONAL;
+	else
+		return FALSE;
+
+	if (strlen(sia) &&
+		(type == SS_CONTROL_TYPE_QUERY ||
+		type == SS_CONTROL_TYPE_ERASURE ||
+		type == SS_CONTROL_TYPE_DEACTIVATION))
+		goto error;
+
+	/* Activation / Registration is figured context specific according to
+	 * 22.030 Section 6.5.2 "The UE shall determine from the context
+	 * whether, an entry of a single *, activation or registration
+	 * was intended."
+	 */
+	if (type == SS_CONTROL_TYPE_ACTIVATION && strlen(sia) > 0)
+		type = SS_CONTROL_TYPE_REGISTRATION;
+
+	if (type == SS_CONTROL_TYPE_REGISTRATION &&
+		!valid_phone_number_format(sia))
+		goto error;
+
+	if (strlen(sib) > 0) {
+		long service_code;
+		char *end;
+
+		service_code = strtoul(sib, &end, 10);
+
+		if (end == sib || *end != '\0')
+			goto error;
+
+		cls = mmi_service_code_to_bearer_class(service_code);
+
+		if (cls == 0)
+			goto error;
+	}
+
+	if (strlen(sic) > 0) {
+		char *end;
+
+		if  (type != SS_CONTROL_TYPE_REGISTRATION)
+			goto error;
+
+		if (cf_type != CALL_FORWARDING_TYPE_ALL &&
+			cf_type != CALL_FORWARDING_TYPE_ALL_CONDITIONAL &&
+			cf_type != CALL_FORWARDING_TYPE_NO_REPLY)
+			goto error;
+
+		timeout = strtoul(sic, &end, 10);
+
+		if (end == sic || *end != '\0')
+			goto error;
+
+		if (timeout < 1 || timeout > 30)
+			goto error;
+	}
+
+	switch (type) {
+	case SS_CONTROL_TYPE_REGISTRATION:
+		operation = cf->ops->registration;
+		break;
+	case SS_CONTROL_TYPE_ACTIVATION:
+		operation = cf->ops->activation;
+		break;
+	case SS_CONTROL_TYPE_DEACTIVATION:
+		operation = cf->ops->deactivation;
+		break;
+	case SS_CONTROL_TYPE_ERASURE:
+		operation = cf->ops->erasure;
+		break;
+	case SS_CONTROL_TYPE_QUERY:
+		operation = cf->ops->query;
+		break;
+	}
+
+	if (!operation) {
+		reply = dbus_gsm_not_implemented(msg);
+		g_dbus_send_message(conn, reply);
+
+		return TRUE;
+	}
+
+	cf->ss_req = g_try_new0(struct cf_ss_request, 1);
+
+	if (!cf->ss_req) {
+		reply = dbus_gsm_failed(msg);
+		g_dbus_send_message(conn, reply);
+
+		return TRUE;
+	}
+
+	cf->ss_req->ss_type = type;
+	cf->ss_req->cf_type = cf_type;
+	cf->ss_req->cls = cls;
+
+	cf->pending = dbus_message_ref(msg);
+
+	switch (cf->ss_req->ss_type) {
+	case SS_CONTROL_TYPE_REGISTRATION:
+		string_to_phone_number(sia, &number_type, &number);
+		cf->ops->registration(modem, cf_type, cls, number, number_type,
+					timeout, cf_ss_control_callback,
+					modem);
+		break;
+	case SS_CONTROL_TYPE_ACTIVATION:
+		cf->ops->activation(modem, cf_type, cls, cf_ss_control_callback,
+					modem);
+		break;
+	case SS_CONTROL_TYPE_DEACTIVATION:
+		cf->ops->deactivation(modem, cf_type, cls,
+					cf_ss_control_callback, modem);
+		break;
+	case SS_CONTROL_TYPE_ERASURE:
+		cf->ops->erasure(modem, cf_type, cls, cf_ss_control_callback,
+					modem);
+		break;
+	case SS_CONTROL_TYPE_QUERY:
+		cls |= BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS;
+		if (cf_type == CALL_FORWARDING_TYPE_ALL)
+			cf->ops->query(modem,
+					CALL_FORWARDING_TYPE_UNCONDITIONAL,
+					cls, cf_unconditional_callback, modem);
+		else if (cf_type == CALL_FORWARDING_TYPE_ALL_CONDITIONAL)
+			cf->ops->query(modem, CALL_FORWARDING_TYPE_BUSY,
+					cls, cf_busy_callback, modem);
+		else
+			cf->ops->query(modem, cf_type, cls,
+					cf_ss_control_query_callback, modem);
+		break;
+	}
+
+	return TRUE;
+
+error:
+	reply = dbus_gsm_invalid_format(msg);
+	g_dbus_send_message(conn, reply);
+	return TRUE;
+}
+
+static void cf_register_ss_controls(struct ofono_modem *modem)
+{
+	ss_control_register(modem, "21", cf_ss_control);
+	ss_control_register(modem, "67", cf_ss_control);
+	ss_control_register(modem, "61", cf_ss_control);
+	ss_control_register(modem, "62", cf_ss_control);
+
+	ss_control_register(modem, "002", cf_ss_control);
+	ss_control_register(modem, "004", cf_ss_control);
+}
+
+static void cf_unregister_ss_controls(struct ofono_modem *modem)
+{
+	ss_control_unregister(modem, "21", cf_ss_control);
+	ss_control_unregister(modem, "67", cf_ss_control);
+	ss_control_unregister(modem, "61", cf_ss_control);
+	ss_control_unregister(modem, "62", cf_ss_control);
+
+	ss_control_unregister(modem, "002", cf_ss_control);
+	ss_control_unregister(modem, "004", cf_ss_control);
+}
+
+static void cf_not_reachable_callback(const struct ofono_error *error, int total,
+				const struct ofono_cf_condition *list, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	GSList *l = NULL;
+	//DBusConnection *conn = dbus_gsm_connection();
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error during not reachable CF query");
+		goto out;
+	}
+
+	l = cf_cond_list_create(total, list);
+
+	set_new_cond_list(modem, CALL_FORWARDING_TYPE_NOT_REACHABLE, l);
+
+	ofono_debug("Not Reachable conditions:");
+	cf_cond_list_print(l);
+
+out:
+
+	cf->flags |= CALL_FORWARDING_FLAG_CACHED;
+
+	if (cf->pending) {
+		if (cf->ss_req) {
+			cf->ss_req->cf_list[CALL_FORWARDING_TYPE_NOT_REACHABLE] = l;
+			cf_ss_control_reply(modem, cf->ss_req);
+			g_free(cf->ss_req);
+			cf->ss_req = NULL;
+		} else {
+			DBusConnection *conn = dbus_gsm_connection();
+			DBusMessage *reply =
+				cf_get_properties_reply(cf->pending, cf);
+
+			g_dbus_send_message(conn, reply);
+		}
+
+		dbus_message_unref(cf->pending);
+		cf->pending = NULL;
+	}
+}
+
+static void cf_no_reply_callback(const struct ofono_error *error, int total,
+				const struct ofono_cf_condition *list, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	GSList *l = NULL;
+	int cls = BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error during no reply CF query");
+		goto out;
+	}
+
+	l = cf_cond_list_create(total, list);
+
+	set_new_cond_list(modem, CALL_FORWARDING_TYPE_NO_REPLY, l);
+
+	ofono_debug("No Reply conditions:");
+	cf_cond_list_print(l);
+
+out:
+	if (cf->ss_req) {
+		cls |= cf->ss_req->cls;
+		cf->ss_req->cf_list[CALL_FORWARDING_TYPE_NO_REPLY] = l;
+	}
+
+	cf->ops->query(modem, CALL_FORWARDING_TYPE_NOT_REACHABLE,
+			cls, cf_not_reachable_callback, modem);
+}
+
+static void cf_busy_callback(const struct ofono_error *error, int total,
+				const struct ofono_cf_condition *list, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	GSList *l = NULL;
+	int cls = BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error during busy CF query");
+		goto out;
+	}
+
+	l = cf_cond_list_create(total, list);
+
+	set_new_cond_list(modem, CALL_FORWARDING_TYPE_BUSY, l);
+
+	ofono_debug("On Busy conditions:");
+	cf_cond_list_print(l);
+
+out:
+	if (cf->ss_req) {
+		cls |= cf->ss_req->cls;
+		cf->ss_req->cf_list[CALL_FORWARDING_TYPE_BUSY] = l;
+	}
+
+	cf->ops->query(modem, CALL_FORWARDING_TYPE_NO_REPLY,
+			cls, cf_no_reply_callback, modem);
+}
+
+static void cf_unconditional_callback(const struct ofono_error *error, int total,
+					const struct ofono_cf_condition *list,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	GSList *l = NULL;
+	int cls = BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error during unconditional CF query");
+		goto out;
+	}
+
+	l = cf_cond_list_create(total, list);
+
+	set_new_cond_list(modem, CALL_FORWARDING_TYPE_UNCONDITIONAL, l);
+
+	ofono_debug("Unconditional conditions:");
+	cf_cond_list_print(l);
+
+out:
+	if (cf->ss_req) {
+		cls |= cf->ss_req->cls;
+		cf->ss_req->cf_list[CALL_FORWARDING_TYPE_UNCONDITIONAL] = l;
+	}
+
+	cf->ops->query(modem, CALL_FORWARDING_TYPE_BUSY,
+			cls, cf_busy_callback, modem);
+}
+
+static gboolean initiate_settings_request(void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_forwarding_data *call_forwarding = modem->call_forwarding;
+
+	/* We can't get all settings at the same time according to 22.004:
+	 * "Interrogation of groups of Supplementary Services is not supported."
+	 * so we do it piecemeal, unconditional, busy, no reply, not reachable
+	 */
+
+	if (call_forwarding->ops->query)
+		call_forwarding->ops->query(modem,
+					CALL_FORWARDING_TYPE_UNCONDITIONAL,
+					BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS,
+					cf_unconditional_callback, modem);
+
+	return FALSE;
+}
+
+static void request_settings(struct ofono_modem *modem)
+{
+	g_timeout_add(0, initiate_settings_request, modem);
+}
+
+int ofono_call_forwarding_register(struct ofono_modem *modem,
+				struct ofono_call_forwarding_ops *ops)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (modem == NULL)
+		return -1;
+
+	if (ops == NULL)
+		return -1;
+
+	if (ops->query == NULL)
+		return -1;
+
+	modem->call_forwarding = call_forwarding_create();
+
+	if (modem->call_forwarding == NULL)
+		return -1;
+
+	modem->call_forwarding->ops = ops;
+
+	if (!g_dbus_register_interface(conn, modem->path,
+					CALL_FORWARDING_INTERFACE,
+					cf_methods, cf_signals, NULL,
+					modem, call_forwarding_destroy)) {
+		ofono_error("Could not register CallForwarding %s", modem->path);
+		call_forwarding_destroy(modem);
+
+		return -1;
+	}
+
+	ofono_debug("Registered call forwarding interface");
+
+	cf_register_ss_controls(modem);
+
+	modem_add_interface(modem, CALL_FORWARDING_INTERFACE);
+
+	request_settings(modem);
+
+	return 0;
+}
+
+void ofono_call_forwarding_unregister(struct ofono_modem *modem)
+{
+	struct call_forwarding_data *cf = modem->call_forwarding;
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (!cf)
+		return;
+
+	modem_remove_interface(modem, CALL_FORWARDING_INTERFACE);
+	g_dbus_unregister_interface(conn, modem->path,
+					CALL_FORWARDING_INTERFACE);
+
+	modem->call_forwarding = NULL;
+}
diff --git a/src/call-meter.c b/src/call-meter.c
new file mode 100644
index 00000000..8a965ed2
--- /dev/null
+++ b/src/call-meter.c
@@ -0,0 +1,764 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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 <string.h>
+#include <stdio.h>
+#include <time.h>
+
+#include <dbus/dbus.h>
+#include <glib.h>
+#include <gdbus.h>
+
+#include "ofono.h"
+
+#include "driver.h"
+#include "common.h"
+#include "dbus-gsm.h"
+#include "modem.h"
+
+#define CALL_METER_INTERFACE "org.ofono.CallMeter"
+
+#define CALL_METER_FLAG_CACHED 0x1
+#define CALL_METER_FLAG_HAVE_PUCT 0x2
+
+struct call_meter_data {
+	struct ofono_call_meter_ops *ops;
+	int flags;
+	DBusMessage *pending;
+
+	int call_meter;
+	int acm;
+	int acm_max;
+	double ppu;
+	char currency[4];
+	char *passwd;
+};
+
+static struct call_meter_data *call_meter_create(void)
+{
+	struct call_meter_data *cm = g_try_new0(struct call_meter_data, 1);
+
+	return cm;
+}
+
+static void call_meter_destroy(gpointer userdata)
+{
+	struct ofono_modem *modem = userdata;
+	struct call_meter_data *cm = modem->call_meter;
+
+	g_free(cm);
+
+	modem->call_meter = NULL;
+}
+
+static void set_call_meter(struct ofono_modem *modem, int value)
+{
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (cm->call_meter != value) {
+		DBusConnection *conn = dbus_gsm_connection();
+
+		cm->call_meter = value;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+							CALL_METER_INTERFACE,
+							"CallMeter",
+							DBUS_TYPE_UINT32,
+							&cm->call_meter);
+	}
+}
+
+static void set_acm(struct ofono_modem *modem, int value)
+{
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (cm->acm != value) {
+		DBusConnection *conn = dbus_gsm_connection();
+
+		cm->acm = value;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+							CALL_METER_INTERFACE,
+							"AccumulatedCallMeter",
+							DBUS_TYPE_UINT32,
+							&cm->acm);
+	}
+}
+
+static void set_acm_max(struct ofono_modem *modem, int value)
+{
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (cm->acm_max != value) {
+		DBusConnection *conn = dbus_gsm_connection();
+
+		cm->acm_max = value;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+							CALL_METER_INTERFACE,
+							"AccumulatedCallMeterMaximum",
+							DBUS_TYPE_UINT32,
+							&cm->acm_max);
+	}
+}
+
+static void set_ppu(struct ofono_modem *modem, double value)
+{
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (cm->ppu != value) {
+		DBusConnection *conn = dbus_gsm_connection();
+
+		cm->ppu = value;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+							CALL_METER_INTERFACE,
+							"PricePerUnit",
+							DBUS_TYPE_DOUBLE,
+							&cm->ppu);
+	}
+}
+
+static void set_currency(struct ofono_modem *modem, const char *value)
+{
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (strlen(value) > 3) {
+		ofono_error("Currency reported with size > 3: %s", value);
+		return;
+	}
+
+	if (strcmp(cm->currency, value)) {
+		DBusConnection *conn = dbus_gsm_connection();
+		const char *dbusval = cm->currency;
+
+		strncpy(cm->currency, value, 3);
+		cm->currency[3] = '\0';
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+							CALL_METER_INTERFACE,
+							"Currency",
+							DBUS_TYPE_STRING,
+							&dbusval);
+	}
+}
+
+static void cm_get_properties_reply(struct ofono_modem *modem)
+{
+	struct call_meter_data *cm = modem->call_meter;
+	//struct call_meter_property *property;
+	DBusMessage *reply;
+	DBusMessageIter iter, dict;
+	const char *currency = cm->currency;
+
+	reply = dbus_message_new_method_return(cm->pending);
+	if (!reply)
+		return;
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+			PROPERTIES_ARRAY_SIGNATURE, &dict);
+
+	dbus_gsm_dict_append(&dict, "CallMeter", DBUS_TYPE_UINT32,
+				&cm->call_meter);
+
+	dbus_gsm_dict_append(&dict, "AccumulatedCallMeter", DBUS_TYPE_UINT32,
+				&cm->acm);
+
+	dbus_gsm_dict_append(&dict, "AccumulatedCallMeterMaximum",
+				DBUS_TYPE_UINT32, &cm->acm_max);
+
+	dbus_gsm_dict_append(&dict, "PricePerUnit", DBUS_TYPE_DOUBLE, &cm->ppu);
+
+	dbus_gsm_dict_append(&dict, "Currency", DBUS_TYPE_STRING, &currency);
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	dbus_gsm_pending_reply(&cm->pending, reply);
+}
+
+static void query_call_meter_callback(const struct ofono_error *error, int value,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		set_call_meter(modem, value);
+
+	if (cm->pending)
+		cm_get_properties_reply(modem);
+}
+
+static gboolean query_call_meter(gpointer user)
+{
+	struct ofono_modem *modem = user;
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (!cm->ops->call_meter_query) {
+		if (cm->pending)
+			cm_get_properties_reply(modem);
+
+		return FALSE;
+	}
+
+	cm->ops->call_meter_query(modem, query_call_meter_callback, modem);
+
+	return FALSE;
+}
+
+static void query_acm_callback(const struct ofono_error *error, int value,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	//struct call_meter_data *cm = modem->call_meter;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		set_acm(modem, value);
+
+	g_timeout_add(0, query_call_meter, modem);
+}
+
+static gboolean query_acm(gpointer user)
+{
+	struct ofono_modem *modem = user;
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (!cm->ops->acm_query) {
+		query_call_meter(modem);
+		return FALSE;
+	}
+
+	cm->ops->acm_query(modem, query_acm_callback, modem);
+
+	return FALSE;
+}
+
+static void query_acm_max_callback(const struct ofono_error *error, int value,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		set_acm_max(modem, value);
+
+	cm->flags |= CALL_METER_FLAG_CACHED;
+
+	g_timeout_add(0, query_acm, modem);
+}
+
+static gboolean query_acm_max(gpointer user)
+{
+	struct ofono_modem *modem = user;
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (!cm->ops->acm_max_query) {
+		cm->flags |= CALL_METER_FLAG_CACHED;
+
+		query_acm(modem);
+		return FALSE;
+	}
+
+	cm->ops->acm_max_query(modem, query_acm_max_callback, modem);
+
+	return FALSE;
+}
+
+static void query_puct_callback(const struct ofono_error *error,
+				const char *currency, double ppu, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR) {
+		cm->flags |= CALL_METER_FLAG_HAVE_PUCT;
+		set_currency(modem, currency);
+		set_ppu(modem, ppu);
+	}
+
+	g_timeout_add(0, query_acm_max, modem);
+}
+
+static gboolean query_puct(gpointer user)
+{
+	struct ofono_modem *modem = user;
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (!cm->ops->puct_query) {
+		query_acm_max(modem);
+		return FALSE;
+	}
+
+	cm->ops->puct_query(modem, query_puct_callback, modem);
+
+	return FALSE;
+}
+
+static DBusMessage *cm_get_properties(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (cm->pending)
+		return dbus_gsm_busy(msg);
+
+	cm->pending = dbus_message_ref(msg);
+
+	/* We don't need to query ppu, currency & acm_max every time
+	 * Not sure if we have to query acm & call_meter every time
+	 * so lets play on the safe side and query them.  They should be
+	 * fast to query anyway
+	 */
+	if (cm->flags & CALL_METER_FLAG_CACHED)
+		query_acm(modem);
+	else
+		query_puct(modem);
+
+	return NULL;
+}
+
+static void set_acm_max_query_callback(const struct ofono_error *error, int value,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+	DBusMessage *reply;
+
+	if (!cm->pending)
+		return;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_error("Setting acm_max successful, but query was not");
+
+		cm->flags &= ~CALL_METER_FLAG_CACHED;
+
+		dbus_gsm_pending_reply(&cm->pending,
+					dbus_gsm_failed(cm->pending));
+		return;
+	}
+
+	reply = dbus_message_new_method_return(cm->pending);
+	dbus_gsm_pending_reply(&cm->pending, reply);
+
+	set_acm_max(modem, value);
+}
+
+static void set_acm_max_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Setting acm_max failed");
+		dbus_gsm_pending_reply(&cm->pending,
+					dbus_gsm_failed(cm->pending));
+		return;
+	}
+
+	/* Assume if we have acm_reset, we have acm_query */
+	cm->ops->acm_max_query(modem, set_acm_max_query_callback, modem);
+}
+
+static DBusMessage *prop_set_acm_max(DBusMessage *msg, struct ofono_modem *modem,
+					DBusMessageIter *dbus_value,
+					const char *pin2)
+{
+	struct call_meter_data *cm = modem->call_meter;
+	dbus_uint32_t value;
+
+	if (!cm->ops->acm_max_set)
+		return dbus_gsm_not_implemented(msg);
+
+	dbus_message_iter_get_basic(dbus_value, &value);
+
+	cm->pending = dbus_message_ref(msg);
+
+	cm->ops->acm_max_set(modem, value, pin2, set_acm_max_callback, modem);
+
+	return NULL;
+}
+
+static void set_puct_query_callback(const struct ofono_error *error,
+					const char *currency, double ppu,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+	DBusMessage *reply;
+
+	if (!cm->pending)
+		return;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_error("Setting PUCT successful, but query was not");
+
+		cm->flags &= ~CALL_METER_FLAG_CACHED;
+
+		dbus_gsm_pending_reply(&cm->pending,
+					dbus_gsm_failed(cm->pending));
+		return;
+	}
+
+	reply = dbus_message_new_method_return(cm->pending);
+	dbus_gsm_pending_reply(&cm->pending, reply);
+
+	set_currency(modem, currency);
+	set_ppu(modem, ppu);
+}
+
+static void set_puct_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("setting puct failed");
+		dbus_gsm_pending_reply(&cm->pending,
+					dbus_gsm_failed(cm->pending));
+		return;
+	}
+
+	/* Assume if we have puct_set, we have puct_query */
+	cm->ops->puct_query(modem, set_puct_query_callback, modem);
+}
+
+/* This function is for the really bizarre case of someone trying to call
+ * SetProperty before GetProperties.  But we must handle it...
+ */
+static void set_puct_initial_query_callback(const struct ofono_error *error,
+						const char *currency,
+						double ppu, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+	DBusMessageIter iter;
+	DBusMessageIter var;
+	const char *name;
+	const char *pin2;
+
+	if (!cm->pending)
+		return;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		dbus_gsm_pending_reply(&cm->pending,
+					dbus_gsm_failed(cm->pending));
+		return;
+	}
+
+	set_currency(modem, currency);
+	set_ppu(modem, ppu);
+
+	cm->flags |= CALL_METER_FLAG_HAVE_PUCT;
+
+	dbus_message_iter_init(cm->pending, &iter);
+	dbus_message_iter_get_basic(&iter, &name);
+	dbus_message_iter_next(&iter);
+	dbus_message_iter_recurse(&iter, &var);
+	dbus_message_iter_next(&iter);
+	dbus_message_iter_get_basic(&iter, &pin2);
+
+	if (!strcmp(name, "PricePerUnit"))
+		dbus_message_iter_get_basic(&var, &ppu);
+	else
+		dbus_message_iter_get_basic(&var, &currency);
+
+	cm->ops->puct_set(modem, currency, ppu, pin2,
+				set_puct_callback, modem);
+}
+
+static DBusMessage *prop_set_ppu(DBusMessage *msg, struct ofono_modem *modem,
+				DBusMessageIter *var, const char *pin2)
+{
+	struct call_meter_data *cm = modem->call_meter;
+	double ppu;
+
+	if (!cm->ops->puct_set || !cm->ops->puct_query)
+		return dbus_gsm_not_implemented(msg);
+
+	dbus_message_iter_get_basic(var, &ppu);
+
+	if (ppu < 0.0)
+		return dbus_gsm_invalid_format(msg);
+
+	cm->pending = dbus_message_ref(msg);
+
+	if (cm->flags & CALL_METER_FLAG_HAVE_PUCT)
+		cm->ops->puct_set(modem, cm->currency, ppu, pin2,
+					set_puct_callback, modem);
+	else
+		cm->ops->puct_query(modem, set_puct_initial_query_callback,
+					modem);
+
+	return NULL;
+}
+
+static DBusMessage *prop_set_cur(DBusMessage *msg, struct ofono_modem *modem,
+				DBusMessageIter *var, const char *pin2)
+{
+	struct call_meter_data *cm = modem->call_meter;
+	const char *value;
+
+	if (!cm->ops->puct_set || !cm->ops->puct_query)
+		return dbus_gsm_not_implemented(msg);
+
+	dbus_message_iter_get_basic(var, &value);
+
+	if (strlen(value) > 3)
+		return dbus_gsm_invalid_format(msg);
+
+	cm->pending = dbus_message_ref(msg);
+
+	if (cm->flags & CALL_METER_FLAG_HAVE_PUCT)
+		cm->ops->puct_set(modem, value, cm->ppu, pin2,
+					set_puct_callback, modem);
+	else
+		cm->ops->puct_query(modem, set_puct_initial_query_callback,
+					modem);
+
+	return NULL;
+}
+
+struct call_meter_property {
+	const char *name;
+	int type;
+	DBusMessage* (*set)(DBusMessage *msg, struct ofono_modem *modem,
+				DBusMessageIter *var, const char *pin2);
+};
+
+static struct call_meter_property cm_properties[] = {
+	{ "AccumulatedCallMeterMaximum",DBUS_TYPE_UINT32,	prop_set_acm_max },
+	{ "PricePerUnit",		DBUS_TYPE_DOUBLE,	prop_set_ppu },
+	{ "Currency",			DBUS_TYPE_STRING,	prop_set_cur },
+	{ NULL, 0, 0 },
+};
+
+static DBusMessage *cm_set_property(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+	DBusMessageIter iter;
+	DBusMessageIter var;
+	const char *name, *passwd = "";
+	struct call_meter_property *property;
+
+	if (cm->pending)
+		return dbus_gsm_busy(msg);
+
+	if (!dbus_message_iter_init(msg, &iter))
+		return dbus_gsm_invalid_args(msg);
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_STRING)
+		return dbus_gsm_invalid_args(msg);
+
+	dbus_message_iter_get_basic(&iter, &name);
+
+	dbus_message_iter_next(&iter);
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_VARIANT)
+		return dbus_gsm_invalid_args(msg);
+
+	dbus_message_iter_recurse(&iter, &var);
+
+	if (!dbus_message_iter_next(&iter))
+		return dbus_gsm_invalid_args(msg);
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_STRING)
+			return dbus_gsm_invalid_args(msg);
+
+	dbus_message_iter_get_basic(&iter, &passwd);
+
+	if (!is_valid_pin(passwd))
+		return dbus_gsm_invalid_format(msg);
+
+	for (property = cm_properties; property->name; property++) {
+		if (strcmp(name, property->name))
+			continue;
+
+		if (dbus_message_iter_get_arg_type(&var) != property->type)
+			return dbus_gsm_invalid_format(msg);
+
+		return property->set(msg, modem, &var, passwd);
+	}
+
+	return dbus_gsm_invalid_args(msg);
+}
+
+static void reset_acm_query_callback(const struct ofono_error *error, int value,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+	DBusMessage *reply;
+
+	if (!cm->pending)
+		return;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_error("Reseting ACM successful, but query was not");
+
+		cm->flags &= ~CALL_METER_FLAG_CACHED;
+
+		dbus_gsm_pending_reply(&cm->pending,
+					dbus_gsm_failed(cm->pending));
+		return;
+	}
+
+	reply = dbus_message_new_method_return(cm->pending);
+	dbus_gsm_pending_reply(&cm->pending, reply);
+
+	set_acm(modem, value);
+}
+
+static void acm_reset_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("reseting acm failed");
+		dbus_gsm_pending_reply(&cm->pending,
+					dbus_gsm_failed(cm->pending));
+		return;
+	}
+
+	/* Assume if we have acm_reset, we have acm_query */
+	cm->ops->acm_query(modem, reset_acm_query_callback, modem);
+}
+
+static DBusMessage *cm_acm_reset(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_meter_data *cm = modem->call_meter;
+	DBusMessageIter iter;
+	const char *pin2;
+
+	if (cm->pending)
+		return dbus_gsm_busy(msg);
+
+	if (!dbus_message_iter_init(msg, &iter))
+		return dbus_gsm_invalid_args(msg);
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_STRING)
+		return dbus_gsm_invalid_args(msg);
+
+	dbus_message_iter_get_basic(&iter, &pin2);
+
+	if (!is_valid_pin(pin2))
+		return dbus_gsm_invalid_format(msg);
+
+	if (!cm->ops->acm_reset)
+		return dbus_gsm_not_implemented(msg);
+
+	cm->pending = dbus_message_ref(msg);
+
+	cm->ops->acm_reset(modem, pin2, acm_reset_callback, modem);
+
+	return NULL;
+}
+
+static GDBusMethodTable cm_methods[] = {
+	{ "GetProperties",	"",	"a{sv}",	cm_get_properties,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "SetProperty",	"svs",	"",		cm_set_property,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "Reset", 		"s",	"",		cm_acm_reset,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ }
+};
+
+static GDBusSignalTable cm_signals[] = {
+	{ "PropertyChanged",	"sv" },
+	{ "NearMaximumWarning",	"" },
+	{ }
+};
+
+void ofono_call_meter_changed_notify(struct ofono_modem *modem, int new_value)
+{
+	set_call_meter(modem, new_value);
+}
+
+void ofono_call_meter_maximum_notify(struct ofono_modem *modem)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *signal;
+
+	signal = dbus_message_new_signal(modem->path,
+			CALL_METER_INTERFACE, "NearMaximumWarning");
+	if (!signal) {
+		ofono_error("Unable to allocate new %s.NearMaximumWarning "
+				"signal", CALL_METER_INTERFACE);
+		return;
+	}
+
+	g_dbus_send_message(conn, signal);
+}
+
+int ofono_call_meter_register(struct ofono_modem *modem,
+				struct ofono_call_meter_ops *ops)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (!modem || !ops)
+		return -1;
+
+	modem->call_meter = call_meter_create();
+
+	if (!modem->call_meter)
+		return -1;
+
+	modem->call_meter->ops = ops;
+
+	if (!g_dbus_register_interface(conn, modem->path, CALL_METER_INTERFACE,
+					cm_methods, cm_signals, NULL, modem,
+					call_meter_destroy)) {
+		ofono_error("Could not create %s interface",
+				CALL_METER_INTERFACE);
+		call_meter_destroy(modem);
+
+		return -1;
+	}
+
+	modem_add_interface(modem, CALL_METER_INTERFACE);
+
+	return 0;
+}
+
+void ofono_call_meter_unregister(struct ofono_modem *modem)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (!modem->call_meter)
+		return;
+
+	modem_remove_interface(modem, CALL_METER_INTERFACE);
+	g_dbus_unregister_interface(conn, modem->path, CALL_METER_INTERFACE);
+
+	modem->call_meter = NULL;
+}
diff --git a/src/call-settings.c b/src/call-settings.c
new file mode 100644
index 00000000..197400b2
--- /dev/null
+++ b/src/call-settings.c
@@ -0,0 +1,900 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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 <string.h>
+#include <stdio.h>
+
+#include <dbus/dbus.h>
+#include <glib.h>
+#include <gdbus.h>
+
+#include "ofono.h"
+
+#include "driver.h"
+#include "common.h"
+#include "dbus-gsm.h"
+#include "modem.h"
+#include "ussd.h"
+
+#define CALL_SETTINGS_INTERFACE "org.ofono.CallSettings"
+
+#define CALL_SETTINGS_FLAG_CACHED 0x1
+
+struct call_settings_data {
+	struct ofono_call_settings_ops *ops;
+	int clir;
+	int colr;
+	int clip;
+	int colp;
+	int clir_setting;
+	int flags;
+	DBusMessage *pending;
+	int ss_req_type;
+	int call_setting_type;
+};
+
+enum call_setting_type {
+	CALL_SETTING_TYPE_CLIP = 0,
+	CALL_SETTING_TYPE_COLP,
+	CALL_SETTING_TYPE_COLR,
+	CALL_SETTING_TYPE_CLIR
+};
+
+static void cs_register_ss_controls(struct ofono_modem *modem);
+static void cs_unregister_ss_controls(struct ofono_modem *modem);
+
+static const char *clip_status_to_string(int status)
+{
+	switch (status) {
+	case CLIP_STATUS_NOT_PROVISIONED:
+		return "disabled";
+	case CLIP_STATUS_PROVISIONED:
+		return "enabled";
+	default:
+		return "unknown";
+	}
+}
+
+static const char *colp_status_to_string(int status)
+{
+	switch (status) {
+	case COLP_STATUS_NOT_PROVISIONED:
+		return "disabled";
+	case COLP_STATUS_PROVISIONED:
+		return "enabled";
+	default:
+		return "unknown";
+	}
+}
+
+static const char *colr_status_to_string(int status)
+{
+	switch (status) {
+	case COLR_STATUS_NOT_PROVISIONED:
+		return "disabled";
+	case COLR_STATUS_PROVISIONED:
+		return "enabled";
+	default:
+		return "unknown";
+	}
+}
+
+static const char *hide_callerid_to_string(int status)
+{
+	switch (status) {
+	case OFONO_CLIR_OPTION_DEFAULT:
+		return "default";
+	case OFONO_CLIR_OPTION_INVOCATION:
+		return "enabled";
+	case OFONO_CLIR_OPTION_SUPPRESSION:
+		return "disabled";
+	default:
+		return "default";
+	}
+}
+
+static const char *clir_status_to_string(int status)
+{
+	switch (status) {
+	case CLIR_STATUS_NOT_PROVISIONED:
+		return "disabled";
+	case CLIR_STATUS_PROVISIONED_PERMANENT:
+		return "permanent";
+	case CLIR_STATUS_TEMPORARY_RESTRICTED:
+		return "on";
+	case CLIR_STATUS_TEMPORARY_ALLOWED:
+		return "off";
+	default:
+		return "unknown";
+	}
+}
+
+static void set_clir_network(struct ofono_modem *modem, int clir)
+{
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (cs->clir != clir) {
+		DBusConnection *conn = dbus_gsm_connection();
+		const char *str = clir_status_to_string(clir);
+
+		cs->clir = clir;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+				CALL_SETTINGS_INTERFACE,
+				"CallingLineRestriction",
+				DBUS_TYPE_STRING, &str);
+	}
+}
+
+static void set_clir_override(struct ofono_modem *modem, int override)
+{
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (cs->clir_setting != override) {
+		DBusConnection *conn = dbus_gsm_connection();
+		const char *str = hide_callerid_to_string(override);
+
+		cs->clir_setting = override;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+				CALL_SETTINGS_INTERFACE,
+				"HideCallerId", DBUS_TYPE_STRING, &str);
+	}
+}
+
+static void set_clip(struct ofono_modem *modem, int clip)
+{
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (cs->clip != clip) {
+		DBusConnection *conn = dbus_gsm_connection();
+		const char *str = clip_status_to_string(clip);
+
+		cs->clip = clip;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+				CALL_SETTINGS_INTERFACE,
+				"CallingLinePresentation",
+				DBUS_TYPE_STRING, &str);
+	}
+}
+
+static void set_colp(struct ofono_modem *modem, int colp)
+{
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (cs->colp != colp) {
+		DBusConnection *conn = dbus_gsm_connection();
+		const char *str = colp_status_to_string(colp);
+
+		cs->colp = colp;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+				CALL_SETTINGS_INTERFACE,
+				"CalledLinePresentation",
+				DBUS_TYPE_STRING, &str);
+	}
+}
+
+static void set_colr(struct ofono_modem *modem, int colr)
+{
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (cs->colr != colr) {
+		DBusConnection *conn = dbus_gsm_connection();
+		const char *str = colr_status_to_string(colr);
+
+		cs->colr = colr;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+				CALL_SETTINGS_INTERFACE,
+				"CalledLineRestriction",
+				DBUS_TYPE_STRING, &str);
+	}
+}
+
+static struct call_settings_data *call_settings_create()
+{
+	struct call_settings_data *r;
+
+	r = g_try_new0(struct call_settings_data, 1);
+
+	if (!r)
+		return r;
+
+	/* Set all the settings to unknown state */
+	r->clip = 2;
+	r->clir = 2;
+	r->colp = 2;
+	r->colr = 2;
+
+	return r;
+}
+
+static void call_settings_destroy(gpointer data)
+{
+	struct ofono_modem *modem = data;
+	struct call_settings_data *cs = modem->call_settings;
+
+	cs_unregister_ss_controls(modem);
+
+	g_free(cs);
+}
+
+static void generate_ss_query_reply(struct ofono_modem *modem,
+					const char *context, const char *value)
+{
+	struct call_settings_data *cs = modem->call_settings;
+	const char *sig = "(ss)";
+	const char *ss_type = ss_control_type_to_string(cs->ss_req_type);
+	DBusMessageIter iter;
+	DBusMessageIter var;
+	DBusMessageIter vstruct;
+	DBusMessage *reply;
+
+	reply = dbus_message_new_method_return(cs->pending);
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_append_basic(&iter, DBUS_TYPE_STRING, &context);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_VARIANT, sig, &var);
+
+	dbus_message_iter_open_container(&var, DBUS_TYPE_STRUCT, NULL,
+						&vstruct);
+
+	dbus_message_iter_append_basic(&vstruct, DBUS_TYPE_STRING,
+					&ss_type);
+
+	dbus_message_iter_append_basic(&vstruct, DBUS_TYPE_STRING, &value);
+
+	dbus_message_iter_close_container(&var, &vstruct);
+
+	dbus_message_iter_close_container(&iter, &var);
+
+	dbus_gsm_pending_reply(&cs->pending, reply);
+}
+
+static void clip_colp_colr_ss_query_cb(const struct ofono_error *error,
+					int status, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_settings_data *cs = modem->call_settings;
+	const char *context;
+	const char *value;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error occurred during ss control query");
+		dbus_gsm_pending_reply(&cs->pending,
+					dbus_gsm_failed(cs->pending));
+
+		return;
+	}
+
+	switch (cs->call_setting_type) {
+	case CALL_SETTING_TYPE_CLIP:
+		set_clip(modem, status);
+		value = clip_status_to_string(status);
+		context = "CallingLinePresentation";
+		break;
+
+	case CALL_SETTING_TYPE_COLP:
+		set_colp(modem, status);
+		value = colp_status_to_string(status);
+		context = "CalledLinePresentation";
+		break;
+
+	case CALL_SETTING_TYPE_COLR:
+		set_colr(modem, status);
+		value = colr_status_to_string(status);
+		context = "CallingLineRestriction";
+		break;
+
+	default:
+		dbus_gsm_pending_reply(&cs->pending,
+				dbus_gsm_failed(cs->pending));
+		ofono_error("Unknown type during COLR/COLP/CLIP ss");
+		return;
+	};
+
+	generate_ss_query_reply(modem, context, value);
+}
+
+static gboolean clip_colp_colr_ss(struct ofono_modem *modem, int type,
+				const char *sc, const char *sia,
+				const char *sib, const char *sic,
+				const char *dn, DBusMessage *msg)
+{
+	struct call_settings_data *cs = modem->call_settings;
+	DBusConnection *conn = dbus_gsm_connection();
+	void (*query_op)(struct ofono_modem *modem, ofono_call_setting_status_cb_t cb,
+				void *data);
+
+	if (!cs)
+		return FALSE;
+
+	if (cs->pending) {
+		DBusMessage *reply = dbus_gsm_busy(msg);
+		g_dbus_send_message(conn, reply);
+
+		return TRUE;
+	}
+
+	if (!strcmp(sc, "30")) {
+		cs->call_setting_type = CALL_SETTING_TYPE_CLIP;
+		query_op = cs->ops->clip_query;
+	} else if (!strcmp(sc, "76")) {
+		cs->call_setting_type = CALL_SETTING_TYPE_COLP;
+		query_op = cs->ops->colp_query;
+	} else if (!strcmp(sc, "77")) {
+		cs->call_setting_type = CALL_SETTING_TYPE_COLR;
+		query_op = cs->ops->colr_query;
+	} else
+		return FALSE;
+
+	if (type != SS_CONTROL_TYPE_QUERY || strlen(sia) || strlen(sib) ||
+		strlen(sic) || strlen(dn)) {
+		DBusMessage *reply = dbus_gsm_invalid_format(msg);
+		g_dbus_send_message(conn, reply);
+
+		return TRUE;
+	}
+
+	if (!query_op) {
+		DBusMessage *reply = dbus_gsm_not_implemented(msg);
+		g_dbus_send_message(conn, reply);
+
+		return TRUE;
+	}
+
+	ofono_debug("Received CLIP/COLR/COLP query ss control");
+
+	cs->pending = dbus_message_ref(msg);
+
+	query_op(modem, clip_colp_colr_ss_query_cb, modem);
+
+	return TRUE;
+}
+
+static void clir_ss_query_callback(const struct ofono_error *error,
+					int override, int network, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_settings_data *cs = modem->call_settings;
+	const char *value;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("setting clir via SS failed");
+		dbus_gsm_pending_reply(&cs->pending,
+					dbus_gsm_failed(cs->pending));
+
+		return;
+	}
+
+	switch (network) {
+	case CLIR_STATUS_UNKNOWN:
+		value = "uknown";
+		break;
+
+	case CLIR_STATUS_PROVISIONED_PERMANENT:
+		value = "enabled";
+		break;
+
+	case CLIR_STATUS_NOT_PROVISIONED:
+		value = "disabled";
+		break;
+
+	case CLIR_STATUS_TEMPORARY_RESTRICTED:
+		if (override == OFONO_CLIR_OPTION_SUPPRESSION)
+			value = "enabled";
+		else
+			value = "disabled";
+		break;
+
+	case CLIR_STATUS_TEMPORARY_ALLOWED:
+		if (override == OFONO_CLIR_OPTION_INVOCATION)
+			value = "enabled";
+		else
+			value = "disabled";
+		break;
+	};
+
+	generate_ss_query_reply(modem, "CallingLineRestriction", value);
+
+	set_clir_network(modem, network);
+	set_clir_override(modem, override);
+}
+
+static void clir_ss_set_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("setting clir via SS failed");
+		dbus_gsm_pending_reply(&cs->pending,
+					dbus_gsm_failed(cs->pending));
+
+		return;
+	}
+
+	cs->ops->clir_query(modem, clir_ss_query_callback, modem);
+}
+
+static gboolean clir_ss_control(struct ofono_modem *modem, int type,
+				const char *sc, const char *sia,
+				const char *sib, const char *sic,
+				const char *dn, DBusMessage *msg)
+{
+	struct call_settings_data *cs = modem->call_settings;
+	DBusConnection *conn = dbus_gsm_connection();
+	//void *op;
+
+	if (!cs)
+		return FALSE;
+
+	if (strcmp(sc, "31"))
+		return FALSE;
+
+	if (cs->pending) {
+		DBusMessage *reply = dbus_gsm_busy(msg);
+		g_dbus_send_message(conn, reply);
+
+		return TRUE;
+	}
+
+	/* This is the temporary form of CLIR, handled in voicecalls */
+	if (!strlen(sia) && !strlen(sib) & !strlen(sic) &&
+		strlen(dn) && type != SS_CONTROL_TYPE_QUERY)
+		return FALSE;
+
+	if (strlen(sia) || strlen(sib) || strlen(sic) || strlen(dn)) {
+		DBusMessage *reply = dbus_gsm_invalid_format(msg);
+		g_dbus_send_message(conn, reply);
+
+		return TRUE;
+	}
+
+	if ((type == SS_CONTROL_TYPE_QUERY && !cs->ops->clir_query) ||
+		(type != SS_CONTROL_TYPE_QUERY && !cs->ops->clir_set)) {
+		DBusMessage *reply = dbus_gsm_not_implemented(msg);
+		g_dbus_send_message(conn, reply);
+
+		return TRUE;
+	}
+
+	cs->call_setting_type = CALL_SETTING_TYPE_CLIR;
+	cs->pending = dbus_message_ref(msg);
+
+	switch (type) {
+	case SS_CONTROL_TYPE_REGISTRATION:
+	case SS_CONTROL_TYPE_ACTIVATION:
+		cs->ss_req_type = SS_CONTROL_TYPE_ACTIVATION;
+		cs->ops->clir_set(modem, OFONO_CLIR_OPTION_INVOCATION,
+					clir_ss_set_callback, modem);
+		break;
+
+	case SS_CONTROL_TYPE_QUERY:
+		cs->ss_req_type = SS_CONTROL_TYPE_QUERY;
+		cs->ops->clir_query(modem, clir_ss_query_callback,
+					modem);
+		break;
+
+	case SS_CONTROL_TYPE_DEACTIVATION:
+	case SS_CONTROL_TYPE_ERASURE:
+		cs->ss_req_type = SS_CONTROL_TYPE_DEACTIVATION;
+		cs->ops->clir_set(modem, OFONO_CLIR_OPTION_SUPPRESSION,
+					clir_ss_set_callback, modem);
+		break;
+	};
+
+	return TRUE;
+}
+
+static void cs_register_ss_controls(struct ofono_modem *modem)
+{
+	struct call_settings_data *cs = modem->call_settings;
+
+	ss_control_register(modem, "30", clip_colp_colr_ss);
+	ss_control_register(modem, "31", clir_ss_control);
+	ss_control_register(modem, "76", clip_colp_colr_ss);
+
+	if (cs->ops->colr_query)
+		ss_control_register(modem, "77", clip_colp_colr_ss);
+}
+
+static void cs_unregister_ss_controls(struct ofono_modem *modem)
+{
+	struct call_settings_data *cs = modem->call_settings;
+
+	ss_control_unregister(modem, "30", clip_colp_colr_ss);
+	ss_control_unregister(modem, "31", clir_ss_control);
+	ss_control_unregister(modem, "76", clip_colp_colr_ss);
+
+	if (cs->ops->colr_query)
+		ss_control_unregister(modem, "77", clip_colp_colr_ss);
+}
+
+static DBusMessage *generate_get_properties_reply(struct ofono_modem *modem,
+							DBusMessage *msg)
+{
+	struct call_settings_data *cs = modem->call_settings;
+	DBusMessage *reply;
+	DBusMessageIter iter;
+	DBusMessageIter dict;
+	const char *str;
+
+	reply = dbus_message_new_method_return(msg);
+
+	if (!reply)
+		return NULL;
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+						PROPERTIES_ARRAY_SIGNATURE,
+						&dict);
+
+	str = clip_status_to_string(cs->clip);
+	dbus_gsm_dict_append(&dict, "CallingLinePresentation",
+				DBUS_TYPE_STRING, &str);
+
+	str = colp_status_to_string(cs->colp);
+	dbus_gsm_dict_append(&dict, "CalledLinePresentation",
+				DBUS_TYPE_STRING, &str);
+
+	str = colr_status_to_string(cs->colr);
+	dbus_gsm_dict_append(&dict, "CalledLineRestriction",
+				DBUS_TYPE_STRING, &str);
+
+	str = clir_status_to_string(cs->clir);
+	dbus_gsm_dict_append(&dict, "CallingLineRestriction",
+				DBUS_TYPE_STRING, &str);
+
+	str = hide_callerid_to_string(cs->clir_setting);
+	dbus_gsm_dict_append(&dict, "HideCallerId", DBUS_TYPE_STRING, &str);
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+static void cs_clir_callback(const struct ofono_error *error,
+				int override_setting, int network_setting,
+				void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_settings_data *cs = modem->call_settings;
+	//DBusConnection *conn = dbus_gsm_connection();
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR)
+		goto out;
+
+	set_clir_network(modem, network_setting);
+	set_clir_override(modem, override_setting);
+
+	cs->flags |= CALL_SETTINGS_FLAG_CACHED;
+
+out:
+	if (cs->pending) {
+		DBusMessage *reply = generate_get_properties_reply(modem,
+								cs->pending);
+		dbus_gsm_pending_reply(&cs->pending, reply);
+	}
+}
+
+static gboolean query_clir(gpointer user)
+{
+	struct ofono_modem *modem = user;
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (!cs->ops->clir_query) {
+		if (cs->pending) {
+			DBusMessage *reply =
+				generate_get_properties_reply(modem,
+								cs->pending);
+			dbus_gsm_pending_reply(&cs->pending, reply);
+		}
+
+		return FALSE;
+	}
+
+	cs->ops->clir_query(modem, cs_clir_callback, modem);
+
+	return FALSE;
+}
+
+static void cs_clip_callback(const struct ofono_error *error,
+				int state, void *data)
+{
+	struct ofono_modem *modem = data;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		set_clip(modem, state);
+
+	g_timeout_add(0, query_clir, modem);
+}
+
+static gboolean query_clip(gpointer user)
+{
+	struct ofono_modem *modem = user;
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (!cs->ops->clip_query) {
+		query_clir(modem);
+		return FALSE;
+	}
+
+	cs->ops->clip_query(modem, cs_clip_callback, modem);
+
+	return FALSE;
+}
+
+static void cs_colp_callback(const struct ofono_error *error,
+				int state, void *data)
+{
+	struct ofono_modem *modem = data;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		set_colp(modem, state);
+
+	g_timeout_add(0, query_clip, modem);
+}
+
+static gboolean query_colp(gpointer user)
+{
+	struct ofono_modem *modem = user;
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (!cs->ops->colp_query) {
+		query_clip(modem);
+		return FALSE;
+	}
+
+	cs->ops->colp_query(modem, cs_colp_callback, modem);
+
+	return FALSE;
+}
+
+static void cs_colr_callback(const struct ofono_error *error,
+				int state, void *data)
+{
+	struct ofono_modem *modem = data;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		set_colr(modem, state);
+
+	g_timeout_add(0, query_colp, modem);
+}
+
+static gboolean query_colr(gpointer user)
+{
+	struct ofono_modem *modem = user;
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (!cs->ops->colr_query) {
+		query_colp(modem);
+		return FALSE;
+	}
+
+	cs->ops->colr_query(modem, cs_colr_callback, modem);
+
+	return FALSE;
+}
+
+static DBusMessage *cs_get_properties(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (cs->pending)
+		return dbus_gsm_busy(msg);
+
+	if (cs->flags & CALL_SETTINGS_FLAG_CACHED)
+		return generate_get_properties_reply(modem, msg);
+
+	/* Query the settings and report back */
+	cs->pending = dbus_message_ref(msg);
+
+	query_colr(modem);
+
+	return NULL;
+}
+
+static void clir_set_query_callback(const struct ofono_error *error,
+					int override_setting,
+					int network_setting, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_settings_data *cs = modem->call_settings;
+	DBusMessage *reply;
+
+	if (!cs->pending)
+		return;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_error("setting clir was successful, but the query was not");
+
+		cs->flags &= ~CALL_SETTINGS_FLAG_CACHED;
+
+		reply = dbus_gsm_failed(cs->pending);
+		dbus_gsm_pending_reply(&cs->pending, reply);
+		return;
+	}
+
+	reply = dbus_message_new_method_return(cs->pending);
+	dbus_gsm_pending_reply(&cs->pending, reply);
+
+	set_clir_override(modem, override_setting);
+	set_clir_network(modem, network_setting);
+}
+
+static void clir_set_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_settings_data *cs = modem->call_settings;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("setting clir failed");
+		dbus_gsm_pending_reply(&cs->pending,
+					dbus_gsm_failed(cs->pending));
+
+		return;
+	}
+
+	/* Assume that if we have clir_set, we have clir_query */
+	cs->ops->clir_query(modem, clir_set_query_callback, modem);
+}
+
+static DBusMessage *set_clir(DBusMessage *msg, struct ofono_modem *modem,
+				const char *setting)
+{
+	struct call_settings_data *cs = modem->call_settings;
+	int clir = -1;
+
+	if (cs->ops->clir_set == NULL)
+		return dbus_gsm_not_implemented(msg);
+
+	if (!strcmp(setting, "default"))
+		clir = 0;
+	else if (!strcmp(setting, "enabled"))
+		clir = 1;
+	else if (!strcmp(setting, "disabled"))
+		clir = 2;
+
+	if (clir == -1)
+		return dbus_gsm_invalid_format(msg);
+
+	cs->pending = dbus_message_ref(msg);
+
+	cs->ops->clir_set(modem, clir, clir_set_callback, modem);
+
+	return NULL;
+}
+
+static DBusMessage *cs_set_property(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_settings_data *cs = modem->call_settings;
+	DBusMessageIter iter;
+	DBusMessageIter var;
+	const char *property;
+
+	if (cs->pending)
+		return dbus_gsm_busy(msg);
+
+	if (!dbus_message_iter_init(msg, &iter))
+		return dbus_gsm_invalid_args(msg);
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_STRING)
+		return dbus_gsm_invalid_args(msg);
+
+	dbus_message_iter_get_basic(&iter, &property);
+	dbus_message_iter_next(&iter);
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_VARIANT)
+		return dbus_gsm_invalid_args(msg);
+
+	dbus_message_iter_recurse(&iter, &var);
+
+	if (!strcmp(property, "HideCallerId")) {
+		const char *setting;
+
+		if (dbus_message_iter_get_arg_type(&var) != DBUS_TYPE_STRING)
+			return dbus_gsm_invalid_format(msg);
+
+		dbus_message_iter_get_basic(&var, &setting);
+
+		return set_clir(msg, modem, setting);
+	}
+
+	return dbus_gsm_invalid_args(msg);
+}
+
+static GDBusMethodTable cs_methods[] = {
+	{ "GetProperties",	"",	"a{sv}",	cs_get_properties,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "SetProperty",	"sv",	"",		cs_set_property,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ }
+};
+
+static GDBusSignalTable cs_signals[] = {
+	{ "PropertyChanged",	"sv" },
+	{ }
+};
+
+int ofono_call_settings_register(struct ofono_modem *modem,
+				struct ofono_call_settings_ops *ops)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (modem == NULL)
+		return -1;
+
+	if (ops == NULL)
+		return -1;
+
+	modem->call_settings = call_settings_create();
+
+	if (!modem->call_settings)
+		return -1;
+
+	modem->call_settings->ops = ops;
+
+	if (!g_dbus_register_interface(conn, modem->path,
+					CALL_SETTINGS_INTERFACE,
+					cs_methods, cs_signals, NULL,
+					modem, call_settings_destroy)) {
+		ofono_error("Could not register CallSettings %s", modem->path);
+		call_settings_destroy(modem);
+
+		return -1;
+	}
+
+	ofono_debug("Registered call settings interface");
+
+	cs_register_ss_controls(modem);
+
+	modem_add_interface(modem, CALL_SETTINGS_INTERFACE);
+	return 0;
+}
+
+void ofono_call_settings_unregister(struct ofono_modem *modem)
+{
+	struct call_settings_data *cs = modem->call_settings;
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (!cs)
+		return;
+
+	modem_remove_interface(modem, CALL_SETTINGS_INTERFACE);
+	g_dbus_unregister_interface(conn, modem->path,
+					CALL_SETTINGS_INTERFACE);
+
+	modem->call_settings = NULL;
+}
diff --git a/src/call-waiting.c b/src/call-waiting.c
new file mode 100644
index 00000000..2b3e5d9b
--- /dev/null
+++ b/src/call-waiting.c
@@ -0,0 +1,648 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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 <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+#include <dbus/dbus.h>
+#include <glib.h>
+#include <gdbus.h>
+
+#include "ofono.h"
+
+#include "driver.h"
+#include "common.h"
+#include "dbus-gsm.h"
+#include "modem.h"
+#include "ussd.h"
+
+#define CALL_WAITING_INTERFACE "org.ofono.CallWaiting"
+
+#define CALL_WAITING_FLAG_CACHED 0x1
+
+struct call_waiting_data {
+	struct ofono_call_waiting_ops *ops;
+	int flags;
+	DBusMessage *pending;
+	GSList *cw_list;
+	int ss_req_type;
+	int ss_req_cls;
+};
+
+static const char *enabled = "enabled";
+static const char *disabled = "disabled";
+
+static void cw_register_ss_controls(struct ofono_modem *modem);
+static void cw_unregister_ss_controls(struct ofono_modem *modem);
+
+static gint cw_condition_compare(gconstpointer a, gconstpointer b)
+{
+	const struct ofono_cw_condition *ca = a;
+	const struct ofono_cw_condition *cb = b;
+
+	if (ca->cls < cb->cls)
+		return -1;
+
+	if (ca->cls > cb->cls)
+		return 1;
+
+	return 0;
+}
+
+static gint cw_condition_find_with_cls(gconstpointer a, gconstpointer b)
+{
+	const struct ofono_cw_condition *c = a;
+	int cls = GPOINTER_TO_INT(b);
+
+	if (c->cls < cls)
+		return -1;
+
+	if (c->cls > cls)
+		return 1;
+
+	return 0;
+}
+
+static struct call_waiting_data *call_waiting_create()
+{
+	struct call_waiting_data *r;
+
+	r = g_try_new0(struct call_waiting_data, 1);
+
+	if (!r)
+		return r;
+
+	return r;
+}
+
+static void call_waiting_destroy(gpointer data)
+{
+	struct ofono_modem *modem = data;
+	struct call_waiting_data *cw = modem->call_waiting;
+
+	cw_unregister_ss_controls(modem);
+
+	g_slist_foreach(cw->cw_list, (GFunc)g_free, NULL);
+	g_slist_free(cw->cw_list);
+
+	g_free(cw);
+}
+
+static void cw_cond_list_print(GSList *list)
+{
+	GSList *l;
+	struct ofono_cw_condition *cond;
+
+	for (l = list; l; l = l->next) {
+		cond = l->data;
+
+		ofono_debug("CW condition status: %d, class: %d",
+				cond->status, cond->cls);
+	}
+}
+
+static GSList *cw_cond_list_create(int total,
+					const struct ofono_cw_condition *list)
+{
+	GSList *l = NULL;
+	int i;
+	int j;
+	struct ofono_cw_condition *cond;
+
+	/* Specification is not really clear on how the results are reported,
+	 * most modems report it as multiple list items, one for each class
+	 * however, specification does leave room for a single compound value
+	 * to be reported
+	 */
+	for (i = 0; i < total; i++) {
+		for (j = 1; j <= BEARER_CLASS_PAD; j = j << 1) {
+			if (!(list[i].cls & j))
+				continue;
+
+			if (list[i].status == 0)
+				continue;
+
+			cond = g_new0(struct ofono_cw_condition, 1);
+
+			memcpy(cond, &list[i], sizeof(struct ofono_cw_condition));
+			cond->cls = j;
+
+			l = g_slist_insert_sorted(l, cond,
+							cw_condition_compare);
+		}
+	}
+
+	return l;
+}
+
+static void set_new_cond_list(struct ofono_modem *modem, GSList *new_cw_list)
+{
+	struct call_waiting_data *cw = modem->call_waiting;
+	DBusConnection *conn = dbus_gsm_connection();
+	GSList *n;
+	GSList *o;
+	struct ofono_cw_condition *nc;
+	struct ofono_cw_condition *oc;
+	char buf[64];
+
+	for (n = new_cw_list; n; n = n->next) {
+		nc = n->data;
+
+		if (nc->cls > BEARER_CLASS_FAX)
+			continue;
+
+		sprintf(buf, "%s", bearer_class_to_string(nc->cls));
+
+		o = g_slist_find_custom(cw->cw_list, GINT_TO_POINTER(nc->cls),
+					cw_condition_find_with_cls);
+
+		if (o) {
+			g_free(o->data);
+			cw->cw_list = g_slist_remove(cw->cw_list, o->data);
+		} else {
+			dbus_gsm_signal_property_changed(conn, modem->path,
+							CALL_WAITING_INTERFACE,
+							buf, DBUS_TYPE_STRING,
+							&enabled);
+		}
+	}
+
+	for (o = cw->cw_list; o; o = o->next) {
+		oc = o->data;
+
+		sprintf(buf, "%s", bearer_class_to_string(oc->cls));
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+							CALL_WAITING_INTERFACE,
+							buf, DBUS_TYPE_STRING,
+							&disabled);
+	}
+
+	g_slist_foreach(cw->cw_list, (GFunc)g_free, NULL);
+	g_slist_free(cw->cw_list);
+
+	cw->cw_list = new_cw_list;
+}
+
+static void property_append_cw_conditions(DBusMessageIter *dict,
+						GSList *cw_list, int mask)
+{
+	GSList *l;
+	int i;
+	struct ofono_cw_condition *cw;
+	const char *prop;
+
+	for (i = 1, l = cw_list; i <= BEARER_CLASS_PAD; i = i << 1) {
+		if (!(mask & i))
+			continue;
+
+		prop = bearer_class_to_string(i);
+
+		while (l && (cw = l->data) && (cw->cls < i))
+			l = l->next;
+
+		if (!l || cw->cls != i) {
+			dbus_gsm_dict_append(dict, prop, DBUS_TYPE_STRING,
+						&disabled);
+			continue;
+		}
+
+		dbus_gsm_dict_append(dict, prop, DBUS_TYPE_STRING, &enabled);
+	}
+}
+
+static void generate_ss_query_reply(struct ofono_modem *modem)
+{
+	struct call_waiting_data *cw = modem->call_waiting;
+	const char *sig = "(sa{sv})";
+	const char *ss_type = ss_control_type_to_string(cw->ss_req_type);
+	const char *context = "CallWaiting";
+	DBusMessageIter iter;
+	DBusMessageIter var;
+	DBusMessageIter vstruct;
+	DBusMessageIter dict;
+	DBusMessage *reply;
+
+	reply = dbus_message_new_method_return(cw->pending);
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_append_basic(&iter, DBUS_TYPE_STRING, &context);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_VARIANT, sig, &var);
+
+	dbus_message_iter_open_container(&var, DBUS_TYPE_STRUCT, NULL,
+						&vstruct);
+
+	dbus_message_iter_append_basic(&vstruct, DBUS_TYPE_STRING,
+					&ss_type);
+
+	dbus_message_iter_open_container(&vstruct, DBUS_TYPE_ARRAY,
+					PROPERTIES_ARRAY_SIGNATURE, &dict);
+
+	property_append_cw_conditions(&dict, cw->cw_list, cw->ss_req_cls);
+
+	dbus_message_iter_close_container(&vstruct, &dict);
+
+	dbus_message_iter_close_container(&var, &vstruct);
+
+	dbus_message_iter_close_container(&iter, &var);
+
+	dbus_gsm_pending_reply(&cw->pending, reply);
+}
+
+static void cw_ss_query_callback(const struct ofono_error *error, int num,
+					struct ofono_cw_condition *cond_list,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_waiting_data *cw = modem->call_waiting;
+	GSList *l;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("setting CW via SS failed");
+
+		cw->flags &= ~CALL_WAITING_FLAG_CACHED;
+		dbus_gsm_pending_reply(&cw->pending,
+					dbus_gsm_failed(cw->pending));
+
+		return;
+	}
+
+	l = cw_cond_list_create(num, cond_list);
+
+	cw_cond_list_print(l);
+
+	set_new_cond_list(modem, l);
+	cw->flags |= CALL_WAITING_FLAG_CACHED;
+
+	generate_ss_query_reply(modem);
+}
+
+static void cw_ss_set_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_waiting_data *cw = modem->call_waiting;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("setting CW via SS failed");
+		dbus_gsm_pending_reply(&cw->pending,
+					dbus_gsm_failed(cw->pending));
+
+		return;
+	}
+
+	cw->ops->query(modem, cw->ss_req_cls, cw_ss_query_callback, modem);
+}
+
+static gboolean cw_ss_control(struct ofono_modem *modem, int type,
+				const char *sc, const char *sia,
+				const char *sib, const char *sic,
+				const char *dn, DBusMessage *msg)
+{
+	struct call_waiting_data *cw = modem->call_waiting;
+	DBusConnection *conn = dbus_gsm_connection();
+	int cls = BEARER_CLASS_DEFAULT;
+	DBusMessage *reply;
+	//void *op;
+
+	if (!cw)
+		return FALSE;
+
+	if (strcmp(sc, "43"))
+		return FALSE;
+
+	if (cw->pending) {
+		reply = dbus_gsm_busy(msg);
+		goto error;
+	}
+
+	if (strlen(sib) || strlen(sib) || strlen(dn))
+		goto bad_format;
+
+	if ((type == SS_CONTROL_TYPE_QUERY && !cw->ops->query) ||
+		(type != SS_CONTROL_TYPE_QUERY && !cw->ops->set)) {
+		reply = dbus_gsm_not_implemented(msg);
+		goto error;
+	}
+
+	if (strlen(sia) > 0) {
+		long service_code;
+		char *end;
+
+		service_code = strtoul(sia, &end, 10);
+
+		if (end == sia || *end != '\0')
+			goto bad_format;
+
+		cls = mmi_service_code_to_bearer_class(service_code);
+		if (cls == 0)
+			goto bad_format;
+	}
+
+	cw->ss_req_cls = cls;
+	cw->pending = dbus_message_ref(msg);
+
+	switch (type) {
+	case SS_CONTROL_TYPE_REGISTRATION:
+	case SS_CONTROL_TYPE_ACTIVATION:
+		cw->ss_req_type = SS_CONTROL_TYPE_ACTIVATION;
+		cw->ops->set(modem, 1, cls, cw_ss_set_callback, modem);
+		break;
+
+	case SS_CONTROL_TYPE_QUERY:
+		cw->ss_req_type = SS_CONTROL_TYPE_QUERY;
+		cw->ops->query(modem, cls, cw_ss_query_callback, modem);
+		break;
+
+	case SS_CONTROL_TYPE_DEACTIVATION:
+	case SS_CONTROL_TYPE_ERASURE:
+		cw->ss_req_type = SS_CONTROL_TYPE_DEACTIVATION;
+		cw->ops->set(modem, 0, cls, cw_ss_set_callback, modem);
+		break;
+	}
+
+	return TRUE;
+
+bad_format:
+	reply = dbus_gsm_invalid_format(msg);
+error:
+	g_dbus_send_message(conn, reply);
+	return TRUE;
+}
+
+static void cw_register_ss_controls(struct ofono_modem *modem)
+{
+	ss_control_register(modem, "43", cw_ss_control);
+}
+
+static void cw_unregister_ss_controls(struct ofono_modem *modem)
+{
+	ss_control_unregister(modem, "43", cw_ss_control);
+}
+
+static DBusMessage *generate_get_properties_reply(struct ofono_modem *modem,
+							DBusMessage *msg)
+{
+	struct call_waiting_data *cw = modem->call_waiting;
+	DBusMessage *reply;
+	DBusMessageIter iter;
+	DBusMessageIter dict;
+	//int i;
+	//GSList *l;
+
+	reply = dbus_message_new_method_return(msg);
+
+	if (!reply)
+		return NULL;
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+						PROPERTIES_ARRAY_SIGNATURE,
+						&dict);
+
+	property_append_cw_conditions(&dict, cw->cw_list, BEARER_CLASS_DEFAULT);
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+static void cw_query_callback(const struct ofono_error *error, int num,
+				struct ofono_cw_condition *cond_list, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_waiting_data *cw = modem->call_waiting;
+	GSList *l = NULL;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error during cw query");
+		goto out;
+	}
+
+	l = cw_cond_list_create(num, cond_list);
+
+	cw_cond_list_print(l);
+
+	set_new_cond_list(modem, l);
+	cw->flags |= CALL_WAITING_FLAG_CACHED;
+
+out:
+	if (cw->pending) {
+		DBusMessage *reply;
+
+		reply = generate_get_properties_reply(modem, cw->pending);
+		dbus_gsm_pending_reply(&cw->pending, reply);
+	}
+}
+
+static DBusMessage *cw_get_properties(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_waiting_data *cw = modem->call_waiting;
+
+	if (cw->pending)
+		return dbus_gsm_busy(msg);
+
+	if (!cw->ops->query)
+		return dbus_gsm_not_implemented(msg);
+
+	if (cw->flags & CALL_WAITING_FLAG_CACHED)
+		return generate_get_properties_reply(modem, msg);
+
+	cw->pending = dbus_message_ref(msg);
+
+	cw->ops->query(modem, BEARER_CLASS_DEFAULT, cw_query_callback, modem);
+
+	return NULL;
+}
+
+static void set_query_callback(const struct ofono_error *error, int num,
+				struct ofono_cw_condition *cond_list, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_waiting_data *cw = modem->call_waiting;
+	GSList *l = NULL;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_error("CW set succeeded, but query failed!");
+		cw->flags &= ~CALL_WAITING_FLAG_CACHED;
+
+		dbus_gsm_pending_reply(&cw->pending,
+					dbus_gsm_failed(cw->pending));
+		return;
+	}
+
+	dbus_gsm_pending_reply(&cw->pending,
+				dbus_message_new_method_return(cw->pending));
+
+	l = cw_cond_list_create(num, cond_list);
+
+	cw_cond_list_print(l);
+
+	set_new_cond_list(modem, l);
+}
+
+static void set_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_waiting_data *cw = modem->call_waiting;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error occurred during CW set");
+
+		dbus_gsm_pending_reply(&cw->pending,
+					dbus_gsm_failed(cw->pending));
+
+		return;
+	}
+
+	cw->ops->query(modem, BEARER_CLASS_DEFAULT, set_query_callback, modem);
+}
+
+static DBusMessage *cw_set_property(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct call_waiting_data *cw = modem->call_waiting;
+	DBusMessageIter iter;
+	DBusMessageIter var;
+	const char *property;
+	int i;
+
+	if (cw->pending)
+		return dbus_gsm_busy(msg);
+
+	if (!cw->ops->set)
+		return dbus_gsm_not_implemented(msg);
+
+	if (!dbus_message_iter_init(msg, &iter))
+		return dbus_gsm_invalid_args(msg);
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_STRING)
+		return dbus_gsm_invalid_args(msg);
+
+	dbus_message_iter_get_basic(&iter, &property);
+	dbus_message_iter_next(&iter);
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_VARIANT)
+		return dbus_gsm_invalid_args(msg);
+
+	dbus_message_iter_recurse(&iter, &var);
+
+	for (i = 1; i < BEARER_CLASS_SMS; i = i << 1)
+		if (!strcmp(property, bearer_class_to_string(i)))
+			break;
+
+	if (i < BEARER_CLASS_SMS) {
+		const char *value;
+		int status;
+
+		if (dbus_message_iter_get_arg_type(&var) != DBUS_TYPE_STRING)
+			return dbus_gsm_invalid_format(msg);
+
+		dbus_message_iter_get_basic(&var, &value);
+
+		if (!strcmp(value, "enabled"))
+			status = 1;
+		else if (!strcmp(value, "disabled"))
+			status = 0;
+		else
+			return dbus_gsm_invalid_format(msg);
+
+		cw->pending = dbus_message_ref(msg);
+
+		cw->ops->set(modem, status, i, set_callback, modem);
+	}
+
+	return dbus_gsm_invalid_args(msg);
+}
+
+static GDBusMethodTable cw_methods[] = {
+	{ "GetProperties",	"",	"a{sv}",	cw_get_properties,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "SetProperty",	"sv",	"",		cw_set_property,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ }
+};
+
+static GDBusSignalTable cw_signals[] = {
+	{ "PropertyChanged",	"sv" },
+	{ }
+};
+
+int ofono_call_waiting_register(struct ofono_modem *modem,
+				struct ofono_call_waiting_ops *ops)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (modem == NULL)
+		return -1;
+
+	if (ops == NULL)
+		return -1;
+
+	modem->call_waiting = call_waiting_create();
+
+	if (!modem->call_waiting)
+		return -1;
+
+	modem->call_waiting->ops = ops;
+
+	if (!g_dbus_register_interface(conn, modem->path,
+					CALL_WAITING_INTERFACE,
+					cw_methods, cw_signals, NULL,
+					modem, call_waiting_destroy)) {
+		ofono_error("Could not register CallWaiting %s", modem->path);
+		call_waiting_destroy(modem);
+
+		return -1;
+	}
+
+	ofono_debug("Registered call waiting interface");
+
+	cw_register_ss_controls(modem);
+
+	modem_add_interface(modem, CALL_WAITING_INTERFACE);
+	return 0;
+}
+
+void ofono_call_waiting_unregister(struct ofono_modem *modem)
+{
+	struct call_waiting_data *cw = modem->call_waiting;
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (!cw)
+		return;
+
+	modem_remove_interface(modem, CALL_WAITING_INTERFACE);
+	g_dbus_unregister_interface(conn, modem->path,
+					CALL_WAITING_INTERFACE);
+
+	modem->call_waiting = NULL;
+}
diff --git a/src/common.c b/src/common.c
new file mode 100644
index 00000000..f3ae3f63
--- /dev/null
+++ b/src/common.c
@@ -0,0 +1,576 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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
+
+#define _GNU_SOURCE
+#include <string.h>
+#include <ctype.h>
+
+#include <glib.h>
+
+#include "driver.h"
+#include "common.h"
+
+struct error_entry {
+	int error;
+	const char *str;
+};
+
+/* 0-127 from 24.011 Annex E2
+ * 127-255 23.040 Section 9.2.3.22
+ * Rest are from 27.005 Section 3.2.5
+ */
+struct error_entry cms_errors[] = {
+	{ 1,	"Unassigned number" },
+	{ 8,	"Operator determined barring" },
+	{ 10,	"Call barred" },
+	{ 21,	"Short message transfer rejected" },
+	{ 27,	"Destination out of service" },
+	{ 28,	"Unindentified subscriber" },
+	{ 29,	"Facility rejected" },
+	{ 30,	"Unknown subscriber" },
+	{ 38,	"Network out of order" },
+	{ 41,	"Temporary failure" },
+	{ 42,	"Congestion" },
+	{ 47,	"Recources unavailable" },
+	{ 50,	"Requested facility not subscribed" },
+	{ 69,	"Requested facility not implemented" },
+	{ 81,	"Invalid short message transfer reference value" },
+	{ 95,	"Invalid message, unspecified" },
+	{ 96,	"Invalid mandatory information" },
+	{ 97,	"Message type non existent or not implemented" },
+	{ 98,	"Message not compatible with short message protocol state" },
+	{ 99,	"Information element non-existent or not implemented" },
+	{ 111,	"Protocol error, unspecified" },
+	{ 127,	"Internetworking error, unspecified" },
+	{ 128,	"Telematic internetworking not supported" },
+	{ 129,	"Short message type 0 not supported" },
+	{ 130,	"Cannot replace short message" },
+	{ 143,	"Unspecified TP-PID error" },
+	{ 144,	"Data code scheme not supported" },
+	{ 145,	"Message class not supported" },
+	{ 159,	"Unspecified TP-DCS error" },
+	{ 160,	"Command cannot be actioned" },
+	{ 161,	"Command unsupported" },
+	{ 175,	"Unspecified TP-Command error" },
+	{ 176,	"TPDU not supported" },
+	{ 192,	"SC busy" },
+	{ 193,	"No SC subscription" },
+	{ 194,	"SC System failure" },
+	{ 195,	"Invalid SME address" },
+	{ 196,	"Destination SME barred" },
+	{ 197,	"SM Rejected-Duplicate SM" },
+	{ 198,	"TP-VPF not supported" },
+	{ 199,	"TP-VP not supported" },
+	{ 208,	"(U)SIM SMS Storage full" },
+	{ 209,	"No SMS Storage capability in SIM" },
+	{ 210,	"Error in MS" },
+	{ 211,	"Memory capacity exceeded" },
+	{ 212,	"Sim application toolkit busy" },
+	{ 213,	"SIM data download error" },
+	{ 255,	"Unspecified error cause" },
+	{ 300,	"ME Failure" },
+	{ 301,	"SMS service of ME reserved" },
+	{ 302,	"Operation not allowed" },
+	{ 303,	"Operation not supported" },
+	{ 304,	"Invalid PDU mode parameter" },
+	{ 305,	"Invalid Text mode parameter" },
+	{ 310,	"(U)SIM not inserted" },
+	{ 311,	"(U)SIM PIN required" },
+	{ 312,	"PH-(U)SIM PIN required" },
+	{ 313,	"(U)SIM failure" },
+	{ 314,	"(U)SIM busy" },
+	{ 315,	"(U)SIM wrong" },
+	{ 316,	"(U)SIM PUK required" },
+	{ 317,	"(U)SIM PIN2 required" },
+	{ 318,	"(U)SIM PUK2 required" },
+	{ 320,	"Memory failure" },
+	{ 321,	"Invalid memory index" },
+	{ 322,	"Memory full" },
+	{ 330,	"SMSC address unknown" },
+	{ 331,	"No network service" },
+	{ 332,	"Network timeout" },
+	{ 340,	"No +CNMA expected" },
+	{ 500,	"Unknown error" },
+};
+
+/* 27.007, Section 9 */
+struct error_entry cme_errors[] = {
+	{ 0,	"Phone failure" },
+	{ 1,	"No connection to phone" },
+	{ 2,	"Phone adapter link reserved" },
+	{ 3,	"Operation not allowed" },
+	{ 4,	"Operation not supported" },
+	{ 5,	"PH_SIM PIN required" },
+	{ 6,	"PH_FSIM PIN required" },
+	{ 7,	"PH_FSIM PUK required" },
+	{ 10,	"SIM not inserted" },
+	{ 11,	"SIM PIN required" },
+	{ 12,	"SIM PUK required" },
+	{ 13,	"SIM failure" },
+	{ 14,	"SIM busy" },
+	{ 15,	"SIM wrong" },
+	{ 16,	"Incorrect password" },
+	{ 17,	"SIM PIN2 required" },
+	{ 18,	"SIM PUK2 required" },
+	{ 20,	"Memory full" },
+	{ 21,	"Invalid index" },
+	{ 22,	"Not found" },
+	{ 23,	"Memory failure" },
+	{ 24,	"Text string too long" },
+	{ 25,	"Invalid characters in text string" },
+	{ 26,	"Dial string too long" },
+	{ 27,	"Invalid characters in dial string" },
+	{ 30,	"No network service" },
+	{ 31,	"Network timeout" },
+	{ 32,	"Network not allowed, emergency calls only" },
+	{ 40,	"Network personalization PIN required" },
+	{ 41,	"Network personalization PUK required" },
+	{ 42,	"Network subset personalization PIN required" },
+	{ 43,	"Network subset personalization PUK required" },
+	{ 44,	"Service provider personalization PIN required" },
+	{ 45,	"Service provider personalization PUK required" },
+	{ 46,	"Corporate personalization PIN required" },
+	{ 47,	"Corporate personalization PUK required" },
+	{ 48,	"PH-SIM PUK required" },
+	{ 100,	"Unknown error" },
+	{ 103,	"Illegal MS" },
+	{ 106,	"Illegal ME" },
+	{ 107,	"GPRS services not allowed" },
+	{ 111,	"PLMN not allowed" },
+	{ 112,	"Location area not allowed" },
+	{ 113,	"Roaming not allowed in this location area" },
+	{ 126,	"Operation temporary not allowed" },
+	{ 132,	"Service operation not supported" },
+	{ 133,	"Requested service option not subscribed" },
+	{ 134,	"Service option temporary out of order" },
+	{ 148,	"Unspecified GPRS error" },
+	{ 149,	"PDP authentication failure" },
+	{ 150,	"Invalid mobile class" },
+	{ 256,	"Operation temporarily not allowed" },
+	{ 257,	"Call barred" },
+	{ 258,	"Phone is busy" },
+	{ 259,	"User abort" },
+	{ 260,	"Invalid dial string" },
+	{ 261,	"SS not executed" },
+	{ 262,	"SIM Blocked" },
+	{ 263,	"Invalid block" },
+	{ 772,	"SIM powered down" },
+};
+
+/* 24.008 Annex H */
+struct error_entry ceer_errors[] = {
+	{ 1,	"Unassigned number" },
+	{ 3,	"No route to destination" },
+	{ 6,	"Channel unacceptable" },
+	{ 8,	"Operator determined barring" },
+	{ 16,	"Normal call clearing" },
+	{ 17,	"User busy" },
+	{ 18,	"No user responding" },
+	{ 19,	"User alerting, no answer" },
+	{ 21,	"Call rejected" },
+	{ 22,	"Number changed" },
+	{ 25,	"Pre-emption" },
+	{ 26,	"Non-selected user clearing" },
+	{ 27,	"Destination out of order" },
+	{ 28,	"Invalid number format (incomplete number)" },
+	{ 29,	"Facility rejected" },
+	{ 30,	"Response to STATUS ENQUIRY" },
+	{ 31,	"Normal, unspecified" },
+	{ 34,	"No circuit/channel available" },
+	{ 38,	"Network out of order" },
+	{ 41,	"Temporary failure" },
+	{ 42,	"Switching equipment congestion" },
+	{ 43,	"Access information discared" },
+	{ 44,	"Requested circuit/channel not available" },
+	{ 47,	"Resource unavailable (unspecified)" },
+	{ 49,	"Quality of service unavailable" },
+	{ 50,	"Requested facility not subscribed" },
+	{ 55,	"Incoming calls barred within the CUG" },
+	{ 57,	"Bearer capability not authorized" },
+	{ 58,	"Bearar capability not presently available" },
+	{ 63,	"Service or option not available, unspecified" },
+	{ 65,	"Bearer service not implemented" },
+	{ 68,	"ACM equal to or greater than ACMmax" },
+	{ 69,	"Requested facility not implemented" },
+	{ 70,	"Only restricted digital information bearer capability is available" },
+	{ 79,	"Service or option not implemented, unspecified" },
+	{ 81,	"Invalid transaction identifier value" },
+	{ 87,	"User not member of CUG" },
+	{ 88,	"Incompatible destination" },
+	{ 91,	"Invalid transit network selection" },
+	{ 95,	"Semantically incorrect message" },
+	{ 96,	"Invalid mandatory information"},
+	{ 97,	"Message type non-existent or not implemented" },
+	{ 98,	"Message type not compatible with protocol state" },
+	{ 99,	"Information element non-existent or not implemented" },
+	{ 100,	"Conditional IE error" },
+	{ 101,	"Message not compatible with protocol state" },
+	{ 102,	"Recovery on timer expirty" },
+	{ 111,	"Protocol error, unspecified" },
+	{ 127,	"Interworking, unspecified" },
+};
+
+gboolean valid_phone_number_format(const char *number)
+{
+	int len = strlen(number);
+	int begin = 0;
+	int i;
+
+	if (!len)
+		return FALSE;
+
+	if (len > OFONO_MAX_PHONE_NUMBER_LENGTH)
+		return FALSE;
+
+	if (number[0] == '+')
+		begin = 1;
+
+	for (i = begin; i < len; i++) {
+		if (number[i] >= '0' && number[i] <= '9')
+			continue;
+
+		if (number[i] == '*' || number[i] == '#')
+			continue;
+
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+const char *telephony_error_to_str(const struct ofono_error *error)
+{
+	struct error_entry *e;
+	int maxentries;
+	int i;
+
+	switch (error->type) {
+	case OFONO_ERROR_TYPE_CME:
+		e = cme_errors;
+		maxentries = sizeof(cme_errors) / sizeof(struct error_entry);
+		break;
+	case OFONO_ERROR_TYPE_CMS:
+		e = cms_errors;
+		maxentries = sizeof(cme_errors) / sizeof(struct error_entry);
+		break;
+	case OFONO_ERROR_TYPE_CEER:
+		e = ceer_errors;
+		maxentries = sizeof(ceer_errors) / sizeof(struct error_entry);
+		break;
+	default:
+		return 0;
+	}
+
+	for (i = 0; i < maxentries; i++)
+		if (e[i].error == error->error)
+			return e[i].str;
+
+	return 0;
+}
+
+int mmi_service_code_to_bearer_class(int code)
+{
+	int cls = 0;
+
+	switch (code) {
+	case 10:
+		cls = BEARER_CLASS_DEFAULT | BEARER_CLASS_SMS;
+		break;
+	case 11:
+		cls = BEARER_CLASS_VOICE;
+		break;
+	case 12:
+		cls = BEARER_CLASS_DATA;
+		break;
+	case 13:
+		cls = BEARER_CLASS_FAX;
+		break;
+	case 16:
+		cls = BEARER_CLASS_SMS;
+		break;
+	/* TODO: Voice Group Call & Broadcast VGCS & VBS */
+	case 17:
+	case 18:
+		break;
+	case 19:
+		cls = BEARER_CLASS_DEFAULT;
+		break;
+
+	/* Funny, according to 22.030, 20 implies BS 7-11 */
+	/* 22.004 only defines BS 7 (Data Sync) & BS 8 (Data Async) */
+	case 20:
+		cls = BEARER_CLASS_DATA_SYNC | BEARER_CLASS_DATA_ASYNC;
+		break;
+	/* According to 22.030: All Async */
+	case 21:
+	/* According to 22.030: All Data Async */
+	case 25:
+		cls = BEARER_CLASS_DATA_ASYNC;
+		break;
+	/* According to 22.030: All Sync */
+	case 22:
+	/* According to 22.030: All Data Sync */
+	case 24:
+		cls = BEARER_CLASS_DATA_SYNC;
+		break;
+	/* According to 22.030: Telephony & All Sync services */
+	case 26:
+		cls = BEARER_CLASS_VOICE | BEARER_CLASS_DATA_SYNC;
+		break;
+	default:
+		break;
+	}
+
+	return cls;
+}
+
+const char *phone_number_to_string(const char *number, int type)
+{
+	static char buffer[64];
+
+	if (type == 145 && (strlen(number) > 0) && number[0] != '+') {
+		buffer[0] = '+';
+		strncpy(buffer + 1, number, 62);
+		buffer[63] = '\0';
+	} else {
+		strncpy(buffer, number, 63);
+		buffer[63] = '\0';
+	}
+
+	return buffer;
+}
+
+void string_to_phone_number(const char *str, int *type, const char **number)
+{
+	if (strlen(str) && str[0] == '+') {
+		*number = &str[1];
+		*type = 145;	/* International */
+	} else {
+		*number = &str[0];
+		*type = 129;	/* Local */
+	}
+}
+
+int valid_ussd_string(const char *str)
+{
+	int len = strlen(str);
+
+	if (!len)
+		return FALSE;
+
+	/* It is hard to understand exactly what constitutes a valid USSD string
+	 * According to 22.090:
+	 * Case a - 1, 2 or 3 digits from the set (*, #) followed by 1X(Y),
+	 * where X=any number 0‑4, Y=any number 0‑9, then, optionally "*
+	 * followed by any number of any characters", and concluding with #SEND
+	 *
+	 * Case b - 1, 2 or 3 digits from the set (*, #) followed by 1X(Y),
+	 * where X=any number 5‑9, Y=any number 0‑9, then, optionally "*
+	 * followed by any number of any characters", and concluding with #SEND
+	 *
+	 * Case c - 7(Y) SEND, where Y=any number 0‑9
+	 *
+	 * Case d - All other formats
+	 *
+	 * According to 22.030 Figure 3.5.3.2 USSD strings can be:
+	 *
+	 * Supplementary service control
+	 * SIM control
+	 * Manufacturer defined
+	 * Terminated by '#'
+	 * Short String - This can be any 2 digit short string.  If the string
+	 *                starts with a '1' and no calls are in progress then
+	 *                this string is treated as a call setup request
+	 *
+	 * Everything else is not a valid USSD string
+	 */
+
+	if (len != 2 && str[len-1] != '#')
+		return FALSE;
+
+	return TRUE;
+}
+
+const char *ss_control_type_to_string(enum ss_control_type type)
+{
+	switch (type) {
+	case SS_CONTROL_TYPE_ACTIVATION:
+		return "acivation";
+	case SS_CONTROL_TYPE_REGISTRATION:
+		return "registration";
+	case SS_CONTROL_TYPE_QUERY:
+		return "interrogation";
+	case SS_CONTROL_TYPE_DEACTIVATION:
+		return "deactivation";
+	case SS_CONTROL_TYPE_ERASURE:
+		return "erasure";
+	}
+
+	return NULL;
+}
+
+#define NEXT_FIELD(str, dest)			\
+	do {					\
+		dest = str;			\
+						\
+		str = strchrnul(str, '*');	\
+		if (*str) {			\
+			*str = '\0';		\
+			str += 1;		\
+		}				\
+	} while (0)				\
+
+/* Note: The str will be modified, so in case of error you should
+ * throw it away and start over
+ */
+gboolean parse_ss_control_string(char *str, int *ss_type,
+					char **sc, char **sia,
+					char **sib, char **sic,
+					char **dn)
+{
+	int len = strlen(str);
+	int cur = 0;
+	char *c;
+	unsigned int i;
+	gboolean ret = FALSE;
+
+	/* Minimum is {*,#}SC# */
+	if (len < 4)
+		goto out;
+
+	if (str[0] != '*' && str[0] != '#')
+		goto out;
+
+	cur = 1;
+
+	if (str[1] != '*' && str[1] != '#' && str[1] > '9' && str[1] < '0')
+		goto out;
+
+	if (str[0] == '#' && str[1] == '*')
+		goto out;
+
+	if (str[1] == '#' || str[1] == '*')
+		cur = 2;
+
+	if (str[0] == '*' && str[1] == '*')
+		*ss_type = SS_CONTROL_TYPE_REGISTRATION;
+	else if (str[0] == '#' && str[1] == '#')
+		*ss_type = SS_CONTROL_TYPE_ERASURE;
+	else if (str[0] == '*' && str[1] == '#')
+		*ss_type = SS_CONTROL_TYPE_QUERY;
+	else if (str[0] == '*')
+		*ss_type = SS_CONTROL_TYPE_ACTIVATION;
+	else
+		*ss_type = SS_CONTROL_TYPE_DEACTIVATION;
+
+	/* Must have at least one other '#' */
+	c = strrchr(str+cur, '#');
+
+	if (!c)
+		goto out;
+
+	*dn = c+1;
+	*c = '\0';
+
+	if (strlen(*dn) > 0 && !valid_phone_number_format(*dn))
+		goto out;
+
+	c = str+cur;
+
+	NEXT_FIELD(c, *sc);
+
+	/* According to 22.030 SC is 2 or 3 digits, there can be
+	 * an optional digit 'n' if this is a call setup string,
+	 * however 22.030 does not define any SC of length 3
+	 * with an 'n' present
+	 */
+	if (strlen(*sc) < 2 || strlen(*sc) > 3)
+		goto out;
+
+	for (i = 0; i < strlen(*sc); i++)
+		if (!isdigit((*sc)[i]))
+			goto out;
+
+	NEXT_FIELD(c, *sia);
+	NEXT_FIELD(c, *sib);
+	NEXT_FIELD(c, *sic);
+
+	if (*c == '\0')
+		ret = TRUE;
+
+out:
+	return ret;
+}
+
+static const char *bearer_class_lut[] = {
+	"Voice",
+	"Data",
+	"Fax",
+	"Sms",
+	"DataSync",
+	"DataAsync",
+	"DataPad",
+	"DataPacket"
+};
+
+const char *bearer_class_to_string(enum bearer_class cls)
+{
+	switch (cls) {
+	case BEARER_CLASS_VOICE:
+		return bearer_class_lut[0];
+	case BEARER_CLASS_DATA:
+		return bearer_class_lut[1];
+	case BEARER_CLASS_FAX:
+		return bearer_class_lut[2];
+	case BEARER_CLASS_SMS:
+		return bearer_class_lut[3];
+	case BEARER_CLASS_DATA_SYNC:
+		return bearer_class_lut[4];
+	case BEARER_CLASS_DATA_ASYNC:
+		return bearer_class_lut[5];
+	case BEARER_CLASS_PACKET:
+		return bearer_class_lut[6];
+	case BEARER_CLASS_PAD:
+		return bearer_class_lut[7];
+	case BEARER_CLASS_DEFAULT:
+		break;
+	};
+
+	return NULL;
+}
+
+gboolean is_valid_pin(const char *pin)
+{
+	unsigned int i;
+
+	for (i = 0; i < strlen(pin); i++)
+		if (pin[i] < '0' || pin[i] > '9')
+			return FALSE;
+
+	if (i > 8)
+		return FALSE;
+
+	return TRUE;
+}
diff --git a/src/common.h b/src/common.h
new file mode 100644
index 00000000..d865eafe
--- /dev/null
+++ b/src/common.h
@@ -0,0 +1,164 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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
+ *
+ */
+
+/* 27.007 Section 7.3 <stat> */
+enum operator_status {
+	OPERATOR_STATUS_UNKNOWN = 0,
+	OPERATOR_STATUS_AVAILABLE = 1,
+	OPERATOR_STATUS_CURRENT = 2,
+	OPERATOR_STATUS_FORBIDDEN = 3
+};
+
+/* 27.007 Section 7.3 <AcT> */
+enum access_technology {
+	ACCESS_TECHNOLOGY_GSM = 0,
+	ACCESS_TECHNOLOGY_GSM_COMPACT = 1,
+	ACCESS_TECHNOLOGY_UTRAN = 2,
+	ACCESS_TECHNOLOGY_GSM_EGPRS = 3,
+	ACCESS_TECHNOLOGY_UTRAN_HSDPA = 4,
+	ACCESS_TECHNOLOGY_UTRAN_HSUPA = 5,
+	ACCESS_TECHNOLOGY_UTRAN_HSDPA_HSUPA = 6
+};
+
+/* 27.007 Section 7.2 <stat> */
+enum network_registration_status {
+	NETWORK_REGISTRATION_STATUS_NOT_REGISTERED = 0,
+	NETWORK_REGISTRATION_STATUS_REGISTERED = 1,
+	NETWORK_REGISTRATION_STATUS_SEARCHING = 2,
+	NETWORK_REGISTRATION_STATUS_DENIED = 3,
+	NETWORK_REGISTRATION_STATUS_UNKNOWN = 4,
+	NETWORK_REGISTRATION_STATUS_ROAMING = 5
+};
+
+/* 27.007 Section 7.7 */
+enum clir_status {
+	CLIR_STATUS_NOT_PROVISIONED = 0,
+	CLIR_STATUS_PROVISIONED_PERMANENT,
+	CLIR_STATUS_UNKNOWN,
+	CLIR_STATUS_TEMPORARY_RESTRICTED,
+	CLIR_STATUS_TEMPORARY_ALLOWED
+};
+
+/* 27.007 Section 7.6 */
+enum clip_status {
+	CLIP_STATUS_NOT_PROVISIONED = 0,
+	CLIP_STATUS_PROVISIONED,
+	CLIP_STATUS_UNKNOWN
+};
+
+/* 27.007 Section 7.6 */
+enum clip_validity {
+	CLIP_VALIDITY_VALID = 0,
+	CLIP_VALIDITY_WITHHELD = 1,
+	CLIP_VALIDITY_NOT_AVAILABLE = 2
+};
+
+/* 27.007 Section 7.8 */
+enum colp_status {
+	COLP_STATUS_NOT_PROVISIONED = 0,
+	COLP_STATUS_PROVISIONED = 1,
+	COLP_STATUS_UNKNOWN = 2
+};
+
+/* This is not defined in 27.007, but presumably the same as CLIP/COLP */
+enum colr_status {
+	COLR_STATUS_NOT_PROVISIONED = 0,
+	COLR_STATUS_PROVISIONED = 1,
+	COLR_STATUS_UNKNOWN = 2
+};
+
+/* 27.007 Section 7.18 */
+enum call_status {
+	CALL_STATUS_ACTIVE = 0,
+	CALL_STATUS_HELD = 1,
+	CALL_STATUS_DIALING = 2,
+	CALL_STATUS_ALERTING = 3,
+	CALL_STATUS_INCOMING = 4,
+	CALL_STATUS_WAITING = 5,
+	CALL_STATUS_DISCONNECTED
+};
+
+/* 27.007 Section 7.18 */
+enum call_direction {
+	CALL_DIRECTION_MOBILE_ORIGINATED = 0,
+	CALL_DIRECTION_MOBILE_TERMINATED = 1
+};
+
+/* 27.007 Section 7.11 */
+enum bearer_class {
+	BEARER_CLASS_VOICE = 1,
+	BEARER_CLASS_DATA = 2,
+	BEARER_CLASS_FAX = 4,
+	BEARER_CLASS_DEFAULT = 7,
+	BEARER_CLASS_SMS = 8,
+	BEARER_CLASS_DATA_SYNC = 16,
+	BEARER_CLASS_DATA_ASYNC = 32,
+	BEARER_CLASS_PACKET = 64,
+	BEARER_CLASS_PAD = 128
+};
+
+enum call_forwarding_type {
+	CALL_FORWARDING_TYPE_UNCONDITIONAL = 0,
+	CALL_FORWARDING_TYPE_BUSY = 1,
+	CALL_FORWARDING_TYPE_NO_REPLY = 2,
+	CALL_FORWARDING_TYPE_NOT_REACHABLE = 3,
+	CALL_FORWARDING_TYPE_ALL = 4,
+	CALL_FORWARDING_TYPE_ALL_CONDITIONAL = 5
+};
+
+enum ussd_status {
+	USSD_STATUS_NOTIFY = 0,
+	USSD_STATUS_ACTION_REQUIRED = 1,
+	USSD_STATUS_TERMINATED = 2,
+	USSD_STATUS_LOCAL_CLIENT_RESPONDED = 3,
+	USSD_STATUS_NOT_SUPPORTED = 4,
+	USSD_STATUS_TIMED_OUT = 5,
+};
+
+/* 22.030 Section 6.5.2 */
+enum ss_control_type {
+	SS_CONTROL_TYPE_ACTIVATION,
+	SS_CONTROL_TYPE_DEACTIVATION,
+	SS_CONTROL_TYPE_QUERY,
+	SS_CONTROL_TYPE_REGISTRATION,
+	SS_CONTROL_TYPE_ERASURE,
+};
+
+const char *telephony_error_to_str(const struct ofono_error *error);
+
+gboolean valid_phone_number_format(const char *number);
+const char *phone_number_to_string(const char *number, int type);
+void string_to_phone_number(const char *str, int *type, const char **number);
+
+int mmi_service_code_to_bearer_class(int code);
+
+gboolean valid_ussd_string(const char *str);
+
+gboolean parse_ss_control_string(char *str, int *ss_type,
+					char **sc, char **sia,
+					char **sib, char **sic,
+					char **dn);
+
+const char *ss_control_type_to_string(enum ss_control_type type);
+
+const char *bearer_class_to_string(enum bearer_class cls);
+
+gboolean is_valid_pin(const char *pin);
diff --git a/src/dbus-gsm.c b/src/dbus-gsm.c
new file mode 100644
index 00000000..f191032d
--- /dev/null
+++ b/src/dbus-gsm.c
@@ -0,0 +1,261 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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 <glib.h>
+#include <dbus/dbus.h>
+#include <gdbus.h>
+
+#include "ofono.h"
+
+#include "dbus-gsm.h"
+
+#define SERVICE_NAME	"org.ofono"
+
+#define RECONNECT_RETRY_TIMEOUT 2000
+
+static DBusConnection *g_connection;
+
+void dbus_gsm_free_string_array(char **array)
+{
+	int i;
+
+	if (!array)
+		return;
+
+	for (i = 0; array[i]; i++)
+		g_free(array[i]);
+
+	g_free(array);
+}
+
+void dbus_gsm_append_variant(DBusMessageIter *iter,
+				int type, void *value)
+{
+	char sig[2];
+	DBusMessageIter valueiter;
+
+	sig[0] = type;
+	sig[1] = 0;
+
+	dbus_message_iter_open_container(iter, DBUS_TYPE_VARIANT,
+						sig, &valueiter);
+
+	dbus_message_iter_append_basic(&valueiter, type, value);
+
+	dbus_message_iter_close_container(iter, &valueiter);
+}
+
+void dbus_gsm_dict_append(DBusMessageIter *dict,
+			const char *key, int type, void *value)
+{
+	DBusMessageIter keyiter;
+
+	if (type == DBUS_TYPE_STRING) {
+		const char *str = *((const char **) value);
+		if (str == NULL)
+			return;
+	}
+
+	dbus_message_iter_open_container(dict, DBUS_TYPE_DICT_ENTRY,
+							NULL, &keyiter);
+
+	dbus_message_iter_append_basic(&keyiter, DBUS_TYPE_STRING, &key);
+
+	dbus_gsm_append_variant(&keyiter, type, value);
+
+	dbus_message_iter_close_container(dict, &keyiter);
+}
+
+void dbus_gsm_append_array_variant(DBusMessageIter *iter, int type, void *val)
+{
+	DBusMessageIter variant, array;
+	char typesig[2];
+	char arraysig[3];
+	const char **str_array = *(const char ***)val;
+	int i;
+
+	arraysig[0] = DBUS_TYPE_ARRAY;
+	arraysig[1] = typesig[0] = type;
+	arraysig[2] = typesig[1] = '\0';
+
+	dbus_message_iter_open_container(iter, DBUS_TYPE_VARIANT,
+						arraysig, &variant);
+
+	dbus_message_iter_open_container(&variant, DBUS_TYPE_ARRAY,
+						typesig, &array);
+
+	for (i = 0; str_array[i]; i++)
+		dbus_message_iter_append_basic(&array, type,
+						&(str_array[i]));
+
+	dbus_message_iter_close_container(&variant, &array);
+
+	dbus_message_iter_close_container(iter, &variant);
+}
+
+void dbus_gsm_dict_append_array(DBusMessageIter *dict, const char *key,
+				int type, void *val)
+{
+	DBusMessageIter entry;
+
+	dbus_message_iter_open_container(dict, DBUS_TYPE_DICT_ENTRY,
+						NULL, &entry);
+
+	dbus_message_iter_append_basic(&entry, DBUS_TYPE_STRING, &key);
+
+	dbus_gsm_append_array_variant(&entry, type, val);
+
+	dbus_message_iter_close_container(dict, &entry);
+}
+
+int dbus_gsm_signal_property_changed(DBusConnection *conn,
+					const char *path,
+					const char *interface,
+					const char *name,
+					int type, void *value)
+{
+	DBusMessage *signal;
+	DBusMessageIter iter;
+
+	signal = dbus_message_new_signal(path, interface, "PropertyChanged");
+
+	if (!signal) {
+		ofono_error("Unable to allocate new %s.PropertyChanged signal",
+				interface);
+		return -1;
+	}
+
+	dbus_message_iter_init_append(signal, &iter);
+
+	dbus_message_iter_append_basic(&iter, DBUS_TYPE_STRING, &name);
+
+	dbus_gsm_append_variant(&iter, type, value);
+
+	return g_dbus_send_message(conn, signal);
+}
+
+int dbus_gsm_signal_array_property_changed(DBusConnection *conn,
+						const char *path,
+						const char *interface,
+						const char *name,
+						int type, void *value)
+
+{
+	DBusMessage *signal;
+	DBusMessageIter iter;
+
+	signal = dbus_message_new_signal(path, interface, "PropertyChanged");
+
+	if (!signal) {
+		ofono_error("Unable to allocate new %s.PropertyChanged signal",
+				interface);
+		return -1;
+	}
+
+	dbus_message_iter_init_append(signal, &iter);
+
+	dbus_message_iter_append_basic(&iter, DBUS_TYPE_STRING, &name);
+
+	dbus_gsm_append_array_variant(&iter, type, value);
+
+	return g_dbus_send_message(conn, signal);
+}
+
+DBusConnection *dbus_gsm_connection()
+{
+	return g_connection;
+}
+
+void dbus_gsm_set_connection(DBusConnection *conn)
+{
+	if (conn && g_connection != NULL)
+		ofono_error("Setting a connection when it is not NULL");
+
+	g_connection = conn;
+}
+
+static gboolean system_bus_reconnect(void *user_data)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (!conn && (dbus_gsm_init() < 0))
+		return TRUE;
+
+	conn = dbus_gsm_connection();
+
+	if (conn && dbus_connection_get_is_connected(conn))
+		return FALSE;
+
+	ofono_error("While attempting to reconnect, conn != NULL,"
+			" but not connected");
+
+	return TRUE;
+}
+
+static void system_bus_disconnected(DBusConnection *conn, void *user_data)
+{
+	ofono_error("System bus has disconnected!");
+
+	dbus_gsm_set_connection(NULL);
+
+	g_timeout_add(RECONNECT_RETRY_TIMEOUT,
+				system_bus_reconnect, NULL);
+}
+
+int dbus_gsm_init()
+{
+	DBusConnection *conn;
+	DBusError error;
+
+	dbus_error_init(&error);
+
+	conn = g_dbus_setup_bus(DBUS_BUS_SYSTEM, SERVICE_NAME, &error);
+	if (!conn) {
+		ofono_error("Unable to hop onto D-Bus: %s", error.message);
+		return -1;
+	}
+
+	if (g_dbus_set_disconnect_function(conn, system_bus_disconnected,
+						NULL, NULL) == FALSE) {
+		dbus_connection_unref(conn);
+		return -1;
+	}
+
+	dbus_gsm_set_connection(conn);
+
+	return 0;
+}
+
+void dbus_gsm_exit()
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (!conn || !dbus_connection_get_is_connected(conn))
+		return;
+
+	dbus_gsm_set_connection(NULL);
+
+	dbus_connection_unref(conn);
+}
diff --git a/src/dbus-gsm.h b/src/dbus-gsm.h
new file mode 100644
index 00000000..8396c4b4
--- /dev/null
+++ b/src/dbus-gsm.h
@@ -0,0 +1,131 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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
+ *
+ */
+
+#include <dbus/dbus.h>
+#include <gdbus.h>
+
+DBusConnection *dbus_gsm_connection();
+void dbus_gsm_set_connection(DBusConnection *conn);
+
+int dbus_gsm_init();
+void dbus_gsm_exit();
+
+#define MAX_DBUS_PATH_LEN 64
+
+void dbus_gsm_free_string_array(char **array);
+
+/* Essentially a{sv} */
+#define PROPERTIES_ARRAY_SIGNATURE DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING \
+					DBUS_TYPE_STRING_AS_STRING \
+					DBUS_TYPE_VARIANT_AS_STRING \
+					DBUS_DICT_ENTRY_END_CHAR_AS_STRING
+
+void dbus_gsm_dict_append(DBusMessageIter *dict, const char *key, int type,
+				void *value);
+
+void dbus_gsm_append_variant(DBusMessageIter *iter, int type, void *value);
+
+void dbus_gsm_append_array_variant(DBusMessageIter *iter, int type, void *val);
+
+void dbus_gsm_dict_append_array(DBusMessageIter *dict, const char *key,
+				int type, void *val);
+
+
+int dbus_gsm_signal_property_changed(DBusConnection *conn, const char *path,
+					const char *interface, const char *name,
+					int type, void *value);
+
+int dbus_gsm_signal_array_property_changed(DBusConnection *conn,
+						const char *path,
+						const char *interface,
+						const char *name, int type,
+						void *value);
+
+#define DBUS_GSM_ERROR_INTERFACE "org.ofono.Error"
+
+static inline DBusMessage *dbus_gsm_invalid_args(DBusMessage *msg)
+{
+	return g_dbus_create_error(msg, DBUS_GSM_ERROR_INTERFACE
+					".InvalidArguments",
+					"Invalid arguments in method call");
+}
+
+static inline DBusMessage *dbus_gsm_invalid_format(DBusMessage *msg)
+{
+	return g_dbus_create_error(msg, DBUS_GSM_ERROR_INTERFACE
+					".InvalidFormat",
+					"Argument format is not recognized");
+}
+
+static inline DBusMessage *dbus_gsm_not_implemented(DBusMessage *msg)
+{
+	return g_dbus_create_error(msg, DBUS_GSM_ERROR_INTERFACE
+					".NotImplemented",
+					"Implementation not provided");
+}
+
+static inline DBusMessage *dbus_gsm_failed(DBusMessage *msg)
+{
+	return g_dbus_create_error(msg, DBUS_GSM_ERROR_INTERFACE ".Failed",
+					"Operation failed");
+}
+
+static inline DBusMessage *dbus_gsm_busy(DBusMessage *msg)
+{
+	return g_dbus_create_error(msg, DBUS_GSM_ERROR_INTERFACE ".InProgress",
+					"Operation already in progress");
+}
+
+static inline DBusMessage *dbus_gsm_not_found(DBusMessage *msg)
+{
+	return g_dbus_create_error(msg, DBUS_GSM_ERROR_INTERFACE ".NotFound",
+			"Object is not found or not valid for this operation");
+}
+
+static inline DBusMessage *dbus_gsm_not_active(DBusMessage *msg)
+{
+	return g_dbus_create_error(msg, DBUS_GSM_ERROR_INTERFACE ".NotActive",
+			"Operation is not active or in progress");
+}
+
+static inline DBusMessage *dbus_gsm_not_supported(DBusMessage *msg)
+{
+	return g_dbus_create_error(msg, DBUS_GSM_ERROR_INTERFACE
+					".NotSupported",
+					"Operation is not supported by the"
+					" network / modem");
+}
+
+static inline DBusMessage *dbus_gsm_timed_out(DBusMessage *msg)
+{
+	return g_dbus_create_error(msg, DBUS_GSM_ERROR_INTERFACE ".Timedout",
+			"Operation failure due to timeout");
+}
+
+static inline void dbus_gsm_pending_reply(DBusMessage **msg, DBusMessage *reply)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	g_dbus_send_message(conn, reply);
+
+	dbus_message_unref(*msg);
+	*msg = NULL;
+}
diff --git a/src/driver.h b/src/driver.h
new file mode 100644
index 00000000..543fc771
--- /dev/null
+++ b/src/driver.h
@@ -0,0 +1,332 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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
+ *
+ */
+
+struct ofono_modem;
+
+/* 27.007 Section 6.2 */
+enum ofono_clir_option {
+	OFONO_CLIR_OPTION_DEFAULT = 0,
+	OFONO_CLIR_OPTION_INVOCATION,
+	OFONO_CLIR_OPTION_SUPPRESSION
+};
+
+/* 27.007 Section 6.2 */
+enum ofono_cug_option {
+	OFONO_CUG_OPTION_DEFAULT = 0,
+	OFONO_CUG_OPTION_INVOCATION = 1,
+};
+
+enum ofono_error_type {
+	OFONO_ERROR_TYPE_NO_ERROR = 0,
+	OFONO_ERROR_TYPE_CME,
+	OFONO_ERROR_TYPE_CMS,
+	OFONO_ERROR_TYPE_CEER,
+	OFONO_ERROR_TYPE_FAILURE
+};
+
+struct ofono_error {
+	enum ofono_error_type type;
+	int error;
+};
+
+enum ofono_disconnect_reason {
+	OFONO_DISCONNECT_REASON_UNKNOWN = 0,
+	OFONO_DISCONNECT_REASON_LOCAL_HANGUP,
+	OFONO_DISCONNECT_REASON_REMOTE_HANGUP,
+	OFONO_DISCONNECT_REASON_ERROR,
+};
+
+#define OFONO_MAX_PHONE_NUMBER_LENGTH 20
+
+struct ofono_call {
+	unsigned id;
+	int type;
+	int direction;
+	int status;
+	char phone_number[OFONO_MAX_PHONE_NUMBER_LENGTH + 1];
+	int number_type;
+	int clip_validity;
+};
+
+/* Theoretical limit is 16, but each GSM char can be encoded into
+ *  * 3 UTF8 characters resulting in 16*3=48 chars
+ *   */
+#define OFONO_MAX_OPERATOR_NAME_LENGTH 63
+
+struct ofono_network_operator {
+	char name[OFONO_MAX_OPERATOR_NAME_LENGTH + 1];
+	short mcc;
+	short mnc;
+	int status;
+	int tech;
+};
+
+/* 27.007 Section 7.11 Call Forwarding */
+struct ofono_cf_condition {
+	int status;
+	int cls;
+	char phone_number[OFONO_MAX_PHONE_NUMBER_LENGTH + 1];
+	int number_type;
+	int time;
+};
+
+/* 27.007 Section 7.12 Call Waiting */
+struct ofono_cw_condition {
+	int status;
+	int cls;
+};
+
+/* Notification functions, the integer values here should map to
+ * values obtained from the modem.  The enumerations are the same
+ * as the values for the fields found in 3GPP TS 27.007
+ *
+ * Pass in the integer value -1 if the value is not known
+ * Pass in NULL string value if the value is not known
+ */
+typedef void (*ofono_generic_cb_t)(const struct ofono_error *error,
+					void *data);
+
+typedef void (*ofono_call_list_cb_t)(const struct ofono_error *error,
+					int numcalls,
+					const struct ofono_call *call_list,
+					void *data);
+
+typedef void (*ofono_current_operator_cb_t)(const struct ofono_error *error,
+					const struct ofono_network_operator *op,
+					void *data);
+
+typedef void (*ofono_operator_list_cb_t)(const struct ofono_error *error,
+					int total,
+					const struct ofono_network_operator *list,
+					void *data);
+
+typedef void (*ofono_registration_status_cb_t)(const struct ofono_error *error,
+					int status, int lac, int ci, int tech,
+					void *data);
+
+typedef void (*ofono_signal_strength_cb_t)(const struct ofono_error *error,
+					int strength, void *data);
+
+typedef void (*ofono_call_forwarding_query_cb_t)(const struct ofono_error *error,
+					int total,
+					const struct ofono_cf_condition *list,
+					void *data);
+
+typedef void (*ofono_modem_attribute_query_cb_t)(const struct ofono_error *error,
+					const char *attribute, void *data);
+
+typedef void (*ofono_call_setting_status_cb_t)(const struct ofono_error *error,
+						int status, void *data);
+
+typedef void (*ofono_clir_setting_cb_t)(const struct ofono_error *error,
+					int override, int network, void *data);
+
+typedef void (*ofono_call_waiting_status_cb_t)(const struct ofono_error *error,
+					int num, struct ofono_cw_condition *cond,
+					void *data);
+
+typedef void (*ofono_call_meter_query_cb_t)(const struct ofono_error *error,
+					int value, void *data);
+
+typedef void (*ofono_call_meter_puct_query_cb_t)(const struct ofono_error *error,
+					const char *currency, double ppu,
+					void *data);
+
+struct ofono_modem_attribute_ops {
+	void (*query_manufacturer)(struct ofono_modem *modem,
+			ofono_modem_attribute_query_cb_t cb, void *data);
+	void (*query_serial)(struct ofono_modem *modem,
+			ofono_modem_attribute_query_cb_t cb, void *data);
+	void (*query_model)(struct ofono_modem *modem,
+			ofono_modem_attribute_query_cb_t cb, void *data);
+	void (*query_revision)(struct ofono_modem *modem,
+			ofono_modem_attribute_query_cb_t cb, void *data);
+};
+
+struct ofono_modem *ofono_modem_register(struct ofono_modem_attribute_ops *ops);
+int ofono_modem_unregister(struct ofono_modem *modem);
+
+void ofono_modem_set_userdata(struct ofono_modem *modem, void *data);
+void *ofono_modem_userdata(struct ofono_modem *modem);
+
+/* Network related functions, including registration status, operator selection
+ * and signal strength indicators.
+ *
+ * It is up to the plugin to implement CSQ polling if the modem does not support
+ * vendor extensions for signal strength notification.
+ */
+struct ofono_network_registration_ops {
+	void (*registration_status)(struct ofono_modem *modem,
+			ofono_registration_status_cb_t cb, void *data);
+	void (*current_operator)(struct ofono_modem *modem,
+			ofono_current_operator_cb_t cb, void *data);
+	void (*list_operators)(struct ofono_modem *modem,
+			ofono_operator_list_cb_t cb, void *data);
+	void (*register_auto)(struct ofono_modem *modem,
+			ofono_generic_cb_t cb, void *data);
+	void (*register_manual)(struct ofono_modem *modem,
+			const struct ofono_network_operator *oper,
+			ofono_generic_cb_t cb, void *data);
+	void (*deregister)(struct ofono_modem *modem,
+			ofono_generic_cb_t cb, void *data);
+	void (*signal_strength)(struct ofono_modem *modem,
+			ofono_signal_strength_cb_t, void *data);
+};
+
+void ofono_signal_strength_notify(struct ofono_modem *modem, int strength);
+void ofono_network_registration_notify(struct ofono_modem *modem, int status,
+			int lac, int ci, int tech);
+int ofono_network_registration_register(struct ofono_modem *modem,
+				struct ofono_network_registration_ops *ops);
+void ofono_network_registration_unregister(struct ofono_modem *modem);
+
+/* Voice call related functionality, including ATD, ATA, +CHLD, CTFR, CLCC,
+ * SSN notifications (CSSI and CSSU) and VTS.
+ *
+ * It is up to the plugin to implement polling of CLCC if the modem does
+ * not support vendor extensions for call progress indication.
+ */
+struct ofono_voicecall_ops {
+	void (*dial)(struct ofono_modem *modem, const char *number,
+			int number_type, enum ofono_clir_option clir,
+			enum ofono_cug_option cug, ofono_generic_cb_t cb,
+			void *data);
+	void (*answer)(struct ofono_modem *modem,
+			ofono_generic_cb_t cb, void *data);
+	void (*hangup)(struct ofono_modem *modem,
+			ofono_generic_cb_t cb, void *data);
+	void (*list_calls)(struct ofono_modem *modem,
+			ofono_call_list_cb_t cb, void *data);
+	void (*hold_all_active)(struct ofono_modem *modem,
+			ofono_generic_cb_t cb, void *data);
+	void (*release_all_held)(struct ofono_modem *modem,
+			ofono_generic_cb_t cb, void *data);
+	void (*set_udub)(struct ofono_modem *modem,
+			ofono_generic_cb_t cb, void *data);
+	void (*release_all_active)(struct ofono_modem *modem,
+			ofono_generic_cb_t cb, void *data);
+	void (*release_specific)(struct ofono_modem *modem, int id,
+			ofono_generic_cb_t cb, void *data);
+	void (*private_chat)(struct ofono_modem *modem, int id,
+			ofono_generic_cb_t cb, void *data);
+	void (*create_multiparty)(struct ofono_modem *modem,
+			ofono_generic_cb_t cb, void *data);
+	void (*transfer)(struct ofono_modem *modem,
+			ofono_generic_cb_t cb, void *data);
+	void (*deflect)(struct ofono_modem *modem, const char *number,
+			int number_type, ofono_generic_cb_t cb, void *data);
+	void (*swap_without_accept)(struct ofono_modem *modem,
+			ofono_generic_cb_t cb, void *data);
+	void (*send_tones)(struct ofono_modem *modem, const char *tones,
+			ofono_generic_cb_t cb, void *data);
+};
+
+void ofono_voicecall_notify(struct ofono_modem *modem, const struct ofono_call *call);
+void ofono_voicecall_disconnected(struct ofono_modem *modem, int id,
+				enum ofono_disconnect_reason reason,
+				const struct ofono_error *error);
+void ofono_voicecall_cssi(struct ofono_modem *modem, int code, int index);
+void ofono_voicecall_cssu(struct ofono_modem *modem, int code, int index,
+				const char *number, int number_type);
+
+int ofono_voicecall_register(struct ofono_modem *modem, struct ofono_voicecall_ops *ops);
+void ofono_voicecall_unregister(struct ofono_modem *modem);
+
+struct ofono_call_forwarding_ops {
+	void (*activation)(struct ofono_modem *modem, int type, int cls,
+				ofono_generic_cb_t cb, void *data);
+	void (*registration)(struct ofono_modem *modem, int type, int cls,
+				const char *number, int number_type, int time,
+				ofono_generic_cb_t cb, void *data);
+	void (*deactivation)(struct ofono_modem *modem, int type, int cls,
+				ofono_generic_cb_t cb, void *data);
+	void (*erasure)(struct ofono_modem *modem, int type, int cls,
+				ofono_generic_cb_t cb, void *data);
+	void (*query)(struct ofono_modem *modem, int type, int cls,
+				ofono_call_forwarding_query_cb_t cb, void *data);
+};
+
+int ofono_call_forwarding_register(struct ofono_modem *modem,
+				struct ofono_call_forwarding_ops *ops);
+void ofono_call_forwarding_unregister(struct ofono_modem *modem);
+
+struct ofono_ussd_ops {
+	void (*request)(struct ofono_modem *modem, const char *str,
+				ofono_generic_cb_t cb, void *data);
+	void (*cancel)(struct ofono_modem *modem,
+				ofono_generic_cb_t cb, void *data);
+};
+
+void ofono_ussd_notify(struct ofono_modem *modem, int status, const char *str);
+int ofono_ussd_register(struct ofono_modem *modem, struct ofono_ussd_ops *ops);
+void ofono_ussd_unregister(struct ofono_modem *modem);
+
+struct ofono_call_settings_ops {
+	void (*clip_query)(struct ofono_modem *modem,
+				ofono_call_setting_status_cb_t cb, void *data);
+	void (*colp_query)(struct ofono_modem *modem,
+				ofono_call_setting_status_cb_t cb, void *data);
+	void (*clir_query)(struct ofono_modem *modem, ofono_clir_setting_cb_t cb,
+				void *data);
+	void (*colr_query)(struct ofono_modem *modem,
+				ofono_call_setting_status_cb_t cb, void *data);
+	void (*clir_set)(struct ofono_modem *modem, int mode, ofono_generic_cb_t cb,
+				void *data);
+};
+
+int ofono_call_settings_register(struct ofono_modem *modem,
+				struct ofono_call_settings_ops *ops);
+void ofono_call_settings_unregister(struct ofono_modem *modem);
+
+struct ofono_call_waiting_ops {
+	void (*query)(struct ofono_modem *modem, int cls,
+			ofono_call_waiting_status_cb_t cb, void *data);
+	void (*set)(struct ofono_modem *modem, int mode, int cls,
+			ofono_generic_cb_t cb, void *data);
+};
+
+int ofono_call_waiting_register(struct ofono_modem *modem,
+				struct ofono_call_waiting_ops *ops);
+void ofono_call_waiting_unregister(struct ofono_modem *modem);
+
+struct ofono_call_meter_ops {
+	void (*call_meter_query)(struct ofono_modem *modem,
+			ofono_call_meter_query_cb_t cb, void *data);
+	void (*acm_query)(struct ofono_modem *modem,
+			ofono_call_meter_query_cb_t cb, void *data);
+	void (*acm_reset)(struct ofono_modem *modem, const char *sim_pin2,
+			ofono_generic_cb_t cb, void *data);
+	void (*acm_max_query)(struct ofono_modem *modem,
+			ofono_call_meter_query_cb_t cb, void *data);
+	void (*acm_max_set)(struct ofono_modem *modem, int new_value,
+			const char *sim_pin2, ofono_generic_cb_t cb, void *data);
+	void (*puct_query)(struct ofono_modem *modem,
+			ofono_call_meter_puct_query_cb_t cb, void *data);
+	void (*puct_set)(struct ofono_modem *modem, const char *currency,
+			double ppu, const char *sim_pin2,
+			ofono_generic_cb_t cb, void *data);
+};
+
+int ofono_call_meter_register(struct ofono_modem *modem,
+				struct ofono_call_meter_ops *ops);
+void ofono_call_meter_unregister(struct ofono_modem *modem);
+void ofono_call_meter_maximum_notify(struct ofono_modem *modem);
+void ofono_call_meter_changed_notify(struct ofono_modem *modem, int new_value);
diff --git a/src/main.c b/src/main.c
index 37deecb2..03d294de 100644
--- a/src/main.c
+++ b/src/main.c
@@ -31,6 +31,8 @@
 
 #include "ofono.h"
 
+#include "dbus-gsm.h"
+
 static GMainLoop *event_loop;
 
 static void sig_debug(int sig)
@@ -100,6 +102,12 @@ int main(int argc, char **argv)
 
 	__ofono_log_init(option_detach, option_debug);
 
+	if (dbus_gsm_init() != 0)
+		goto cleanup;
+
+	if (__ofono_manager_init() < 0)
+		goto cleanup;
+
 	__ofono_plugin_init(NULL, NULL);
 
 	memset(&sa, 0, sizeof(sa));
@@ -118,6 +126,11 @@ int main(int argc, char **argv)
 
 	__ofono_plugin_cleanup();
 
+	__ofono_manager_cleanup();
+
+	dbus_gsm_exit();
+
+cleanup:
 	g_main_loop_unref(event_loop);
 
 	__ofono_log_cleanup();
diff --git a/src/manager.c b/src/manager.c
new file mode 100644
index 00000000..eb5dc496
--- /dev/null
+++ b/src/manager.c
@@ -0,0 +1,210 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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 <string.h>
+#include <dbus/dbus.h>
+#include <glib.h>
+#include <gdbus.h>
+
+#include "ofono.h"
+
+#include "dbus-gsm.h"
+
+#include "modem.h"
+#include "driver.h"
+
+#define MANAGER_INTERFACE "org.ofono.Manager"
+#define MANAGER_PATH "/"
+
+static GSList *g_modem_list = NULL;
+static int g_next_modem_id = 1;
+
+#if 0
+struct ofono_modem *manager_find_modem_by_id(int id)
+{
+	GSList *l;
+	struct ofono_modem *modem;
+
+	for (l = g_modem_list; l; l = l->next) {
+		modem = l->data;
+
+		if (modem->id == id)
+			return modem;
+	}
+
+	return NULL;
+}
+#endif
+
+/* Clients only need to free *modems */
+static int modem_list(char ***modems)
+{
+	GSList *l;
+	int i;
+	struct ofono_modem *modem;
+
+	*modems = g_new0(char *, g_slist_length(g_modem_list) + 1);
+
+	if (!*modems)
+		return -1;
+
+	for (l = g_modem_list, i = 0; l; l = l->next, i++) {
+		modem = l->data;
+
+		(*modems)[i] = modem->path;
+	}
+
+	return 0;
+}
+
+struct ofono_modem *ofono_modem_register(struct ofono_modem_attribute_ops *ops)
+{
+	struct ofono_modem *modem;
+	DBusConnection *conn = dbus_gsm_connection();
+	char **modems;
+
+	modem = modem_create(g_next_modem_id, ops);
+
+	if (modem == NULL)
+		return 0;
+
+	++g_next_modem_id;
+
+	g_modem_list = g_slist_prepend(g_modem_list, modem);
+
+	if (modem_list(&modems) == 0) {
+		dbus_gsm_signal_array_property_changed(conn, MANAGER_PATH,
+				MANAGER_INTERFACE, "Modems",
+				DBUS_TYPE_OBJECT_PATH, &modems);
+
+		g_free(modems);
+	}
+
+	return modem;
+}
+
+int ofono_modem_unregister(struct ofono_modem *m)
+{
+	struct ofono_modem *modem = m;
+	DBusConnection *conn = dbus_gsm_connection();
+	char **modems;
+
+	if (modem == NULL)
+		return -1;
+
+	modem_remove(modem);
+
+	g_modem_list = g_slist_remove(g_modem_list, modem);
+
+	if (modem_list(&modems) == 0) {
+		dbus_gsm_signal_array_property_changed(conn, MANAGER_PATH,
+				MANAGER_INTERFACE, "Modems",
+				DBUS_TYPE_OBJECT_PATH, &modems);
+
+		g_free(modems);
+	}
+
+	return 0;
+}
+
+static DBusMessage *manager_get_properties(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	DBusMessageIter iter;
+	DBusMessageIter dict;
+	DBusMessage *reply;
+	char **modems;
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return NULL;
+
+	if (modem_list(&modems) == -1)
+		return NULL;
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+						PROPERTIES_ARRAY_SIGNATURE,
+						&dict);
+
+	dbus_gsm_dict_append_array(&dict, "Modems", DBUS_TYPE_OBJECT_PATH,
+					&modems);
+
+	g_free(modems);
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+static GDBusMethodTable manager_methods[] = {
+	{ "GetProperties",	"",	"a{sv}",	manager_get_properties },
+	{ }
+};
+
+static GDBusSignalTable manager_signals[] = {
+	{ "PropertyChanged", "sv" },
+	{ }
+};
+
+int __ofono_manager_init()
+{
+	DBusConnection *conn = dbus_gsm_connection();
+	gboolean ret;
+
+	ret = g_dbus_register_interface(conn, "/", MANAGER_INTERFACE,
+					manager_methods, manager_signals,
+					NULL, NULL, NULL);
+
+	if (ret == FALSE)
+		return -1;
+
+	return 0;
+}
+
+void __ofono_manager_cleanup()
+{
+	GSList *l;
+	struct ofono_modem *modem;
+	DBusConnection *conn = dbus_gsm_connection();
+
+	/* Clean up in case plugins didn't unregister the modems */
+	for (l = g_modem_list; l; l = l->next) {
+		modem = l->data;
+
+		if (!modem)
+			continue;
+
+		ofono_debug("plugin owning %s forgot to unregister, cleaning up",
+				modem->path);
+		modem_remove(modem);
+	}
+
+	g_slist_free(g_modem_list);
+	g_modem_list = 0;
+
+	g_dbus_unregister_interface(conn, "/", MANAGER_INTERFACE);
+}
diff --git a/src/modem.c b/src/modem.c
new file mode 100644
index 00000000..0b8b36ef
--- /dev/null
+++ b/src/modem.c
@@ -0,0 +1,432 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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 <string.h>
+#include <stdio.h>
+
+#include <dbus/dbus.h>
+#include <glib.h>
+#include <gdbus.h>
+
+#include "ofono.h"
+
+#include "dbus-gsm.h"
+#include "modem.h"
+#include "driver.h"
+
+#define MODEM_INTERFACE "org.ofono.Modem"
+
+#define MODEM_FLAG_INITIALIZING_ATTRS 1
+
+#define ATTRIBUTE_QUERY_DELAY 0
+
+struct ofono_modem_data {
+	char      			*manufacturer;
+	char      			*model;
+	char      			*revision;
+	char      			*serial;
+	GSList          		*interface_list;
+	int				flags;
+	unsigned int			idlist;
+	struct ofono_modem_attribute_ops	*ops;
+	DBusMessage			*pending;
+	guint				interface_update;
+};
+
+unsigned int modem_alloc_callid(struct ofono_modem *modem)
+{
+	struct ofono_modem_data *d = modem->modem_info;
+	unsigned int i;
+
+	for (i = 1; i < sizeof(d->idlist) * 8; i++) {
+		if (d->idlist & (0x1 << i))
+			continue;
+
+		d->idlist |= (0x1 << i);
+		return i;
+	}
+
+	return 0;
+}
+
+void modem_release_callid(struct ofono_modem *modem, int id)
+{
+	struct ofono_modem_data *d = modem->modem_info;
+
+	d->idlist &= ~(0x1 << id);
+}
+
+void ofono_modem_set_userdata(struct ofono_modem *modem, void *userdata)
+{
+	if (modem)
+		modem->userdata = userdata;
+}
+
+void *ofono_modem_userdata(struct ofono_modem *modem)
+{
+	if (modem)
+		return modem->userdata;
+
+	return NULL;
+}
+
+static void modem_free(gpointer data)
+{
+	struct ofono_modem *modem = data;
+	GSList *l;
+
+	if (modem == NULL)
+		return;
+
+	for (l = modem->modem_info->interface_list; l; l = l->next)
+		g_free(l->data);
+
+	g_slist_free(modem->modem_info->interface_list);
+
+	g_free(modem->modem_info->manufacturer);
+	g_free(modem->modem_info->serial);
+	g_free(modem->modem_info->revision);
+	g_free(modem->modem_info->model);
+
+	if (modem->modem_info->pending)
+		dbus_message_unref(modem->modem_info->pending);
+
+	if (modem->modem_info->interface_update)
+		g_source_remove(modem->modem_info->interface_update);
+
+	g_free(modem->modem_info);
+	g_free(modem->path);
+	g_free(modem);
+}
+
+static DBusMessage *generate_properties_reply(struct ofono_modem *modem,
+				DBusConnection *conn, DBusMessage *msg)
+{
+	struct ofono_modem_data *info = modem->modem_info;
+	DBusMessage *reply;
+	DBusMessageIter iter;
+	DBusMessageIter dict;
+
+	char **interfaces;
+	int i;
+	GSList *l;
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return NULL;
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+						PROPERTIES_ARRAY_SIGNATURE,
+						&dict);
+
+	if (info->manufacturer)
+		dbus_gsm_dict_append(&dict, "Manufacturer", DBUS_TYPE_STRING,
+					&info->manufacturer);
+
+	if (info->model)
+		dbus_gsm_dict_append(&dict, "Model", DBUS_TYPE_STRING,
+					&info->model);
+
+	if (info->revision)
+		dbus_gsm_dict_append(&dict, "Revision", DBUS_TYPE_STRING,
+					&info->revision);
+
+	if (info->serial)
+		dbus_gsm_dict_append(&dict, "Serial", DBUS_TYPE_STRING,
+					&info->serial);
+
+	interfaces = g_new0(char *, g_slist_length(info->interface_list) + 1);
+	for (i = 0, l = info->interface_list; l; l = l->next, i++)
+		interfaces[i] = l->data;
+
+	dbus_gsm_dict_append_array(&dict, "Interfaces", DBUS_TYPE_STRING,
+					&interfaces);
+
+	g_free(interfaces);
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+static DBusMessage *modem_get_properties(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+
+	if (modem->modem_info->flags & MODEM_FLAG_INITIALIZING_ATTRS) {
+		modem->modem_info->pending = dbus_message_ref(msg);
+		return NULL;
+	}
+
+	return generate_properties_reply(modem, conn, msg);
+}
+
+static GDBusMethodTable modem_methods[] = {
+	{ "GetProperties",	"",	"a{sv}",	modem_get_properties,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ }
+};
+
+static GDBusSignalTable modem_signals[] = {
+	{ "PropertyChanged",	"sv" },
+	{ }
+};
+
+static gboolean trigger_interface_update(void *data)
+{
+	struct ofono_modem *modem = data;
+	struct ofono_modem_data *info = modem->modem_info;
+	DBusConnection *conn = dbus_gsm_connection();
+
+	char **interfaces;
+	GSList *l;
+	int i;
+
+	interfaces = g_new0(char *, g_slist_length(info->interface_list) + 1);
+	for (i = 0, l = info->interface_list; l; l = l->next, i++)
+		interfaces[i] = l->data;
+
+	dbus_gsm_signal_array_property_changed(conn, modem->path,
+						MODEM_INTERFACE,
+						"Interfaces", DBUS_TYPE_STRING,
+						&interfaces);
+
+	g_free(interfaces);
+
+	info->interface_update = 0;
+
+	return FALSE;
+}
+
+void modem_add_interface(struct ofono_modem *modem, const char *interface)
+{
+	struct ofono_modem_data *info = modem->modem_info;
+
+	info->interface_list =
+		g_slist_prepend(info->interface_list, g_strdup(interface));
+
+	if (info->interface_update == 0)
+		info->interface_update =
+			g_timeout_add(0, trigger_interface_update, modem);
+}
+
+void modem_remove_interface(struct ofono_modem *modem, const char *interface)
+{
+	struct ofono_modem_data *info = modem->modem_info;
+
+	GSList *found = g_slist_find_custom(info->interface_list,
+						interface,
+						(GCompareFunc) strcmp);
+
+	if (!found) {
+		ofono_error("Interface %s not found on the interface_list",
+				interface);
+		return;
+	}
+
+	g_free(found->data);
+
+	info->interface_list =
+		g_slist_remove(info->interface_list, found->data);
+
+	if (info->interface_update == 0)
+		info->interface_update =
+			g_timeout_add(0, trigger_interface_update, modem);
+}
+
+static void finish_attr_query(struct ofono_modem *modem)
+{
+	//struct ofono_modem_data *info = modem->modem_info;
+	DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *reply;
+
+	modem->modem_info->flags &= ~MODEM_FLAG_INITIALIZING_ATTRS;
+
+	if (!modem->modem_info->pending)
+		return;
+
+	reply = generate_properties_reply(modem, conn,
+						modem->modem_info->pending);
+
+	if (reply)
+		g_dbus_send_message(conn, reply);
+
+	dbus_message_unref(modem->modem_info->pending);
+	modem->modem_info->pending = NULL;
+}
+
+static void query_serial_cb(const struct ofono_error *error,
+				const char *serial, void *user)
+{
+	struct ofono_modem *modem = user;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		modem->modem_info->serial = g_strdup(serial);
+
+	finish_attr_query(modem);
+}
+
+static gboolean query_serial(gpointer user)
+{
+	struct ofono_modem *modem = user;
+
+	if (!modem->modem_info->ops->query_serial) {
+		finish_attr_query(modem);
+		return FALSE;
+	}
+
+	modem->modem_info->ops->query_serial(modem, query_serial_cb, modem);
+
+	return FALSE;
+}
+
+static void query_revision_cb(const struct ofono_error *error,
+				const char *revision, void *user)
+{
+	struct ofono_modem *modem = user;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		modem->modem_info->revision = g_strdup(revision);
+
+	g_timeout_add(0, query_serial, modem);
+}
+
+static gboolean query_revision(gpointer user)
+{
+	struct ofono_modem *modem = user;
+
+	if (!modem->modem_info->ops->query_revision) {
+		g_timeout_add(0, query_serial, modem);
+		return FALSE;
+	}
+
+	modem->modem_info->ops->query_revision(modem, query_revision_cb, modem);
+
+	return FALSE;
+}
+
+static void query_model_cb(const struct ofono_error *error,
+				const char *model, void *user)
+{
+	struct ofono_modem *modem = user;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		modem->modem_info->model = g_strdup(model);
+
+	g_timeout_add(0, query_revision, modem);
+}
+
+static gboolean query_model(gpointer user)
+{
+	struct ofono_modem *modem = user;
+
+	if (!modem->modem_info->ops->query_model) {
+		/* If model is not supported, don't bother querying revision */
+		g_timeout_add(0, query_serial, modem);
+		return FALSE;
+	}
+
+	modem->modem_info->ops->query_model(modem, query_model_cb, modem);
+
+	return FALSE;
+}
+
+static void query_manufacturer_cb(const struct ofono_error *error,
+					const char *manufacturer, void *user)
+{
+	struct ofono_modem *modem = user;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		modem->modem_info->manufacturer = g_strdup(manufacturer);
+
+	g_timeout_add(0, query_model, modem);
+}
+
+static gboolean query_manufacturer(gpointer user)
+{
+	struct ofono_modem *modem = user;
+
+	if (!modem->modem_info->ops->query_manufacturer) {
+		g_timeout_add(0, query_model, modem);
+		return FALSE;
+	}
+
+	modem->modem_info->ops->query_manufacturer(modem, query_manufacturer_cb,
+							modem);
+
+	return FALSE;
+}
+
+struct ofono_modem *modem_create(int id, struct ofono_modem_attribute_ops *ops)
+{
+	char path[MAX_DBUS_PATH_LEN];
+	DBusConnection *conn = dbus_gsm_connection();
+	struct ofono_modem *modem;
+
+	modem = g_try_new0(struct ofono_modem, 1);
+	if (modem == NULL)
+		return modem;
+
+	modem->modem_info = g_try_new0(struct ofono_modem_data, 1);
+	if (modem->modem_info == NULL) {
+		g_free(modem);
+		return NULL;
+	}
+
+	modem->id = id;
+	modem->modem_info->ops = ops;
+
+	snprintf(path, MAX_DBUS_PATH_LEN, "/modem%d", modem->id);
+	modem->path = g_strdup(path);
+
+	if (!g_dbus_register_interface(conn, path, MODEM_INTERFACE,
+			modem_methods, modem_signals, NULL,
+			modem, modem_free)) {
+		ofono_error("Modem interface init failed on path %s", path);
+		modem_free(modem);
+		return NULL;
+	}
+
+	modem->modem_info->flags |= MODEM_FLAG_INITIALIZING_ATTRS;
+	g_timeout_add(ATTRIBUTE_QUERY_DELAY, query_manufacturer, modem);
+
+	return modem;
+}
+
+void modem_remove(struct ofono_modem *modem)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+	/* Need to make a copy to keep gdbus happy */
+	char *path = g_strdup(modem->path);
+
+	ofono_debug("Removing modem: %s", modem->path);
+
+	g_dbus_unregister_interface(conn, path, MODEM_INTERFACE);
+
+	g_free(path);
+}
diff --git a/src/modem.h b/src/modem.h
new file mode 100644
index 00000000..ea23d6c5
--- /dev/null
+++ b/src/modem.h
@@ -0,0 +1,49 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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
+ *
+ */
+
+struct ofono_modem_attribute_ops;
+
+struct ofono_modem {
+	int		id;
+	char		*path;
+
+	void 		*userdata;
+
+	GSList *ss_control_list;
+
+	struct ofono_modem_data *modem_info;
+	struct network_registration_data *network_registration;
+	struct voicecalls_data *voicecalls;
+	struct call_forwarding_data *call_forwarding;
+	struct ussd_data *ussd;
+	struct call_settings_data *call_settings;
+	struct call_waiting_data *call_waiting;
+	struct call_meter_data *call_meter;
+};
+
+struct ofono_modem *modem_create(int id, struct ofono_modem_attribute_ops *ops);
+void modem_remove(struct ofono_modem *modem);
+
+void modem_add_interface(struct ofono_modem *modem, const char *interface);
+void modem_remove_interface(struct ofono_modem *modem, const char *interface);
+
+unsigned int modem_alloc_callid(struct ofono_modem *modem);
+void modem_release_callid(struct ofono_modem *modem, int id);
diff --git a/src/network.c b/src/network.c
new file mode 100644
index 00000000..bebc0c15
--- /dev/null
+++ b/src/network.c
@@ -0,0 +1,1062 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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 <string.h>
+#include <stdio.h>
+
+#include <dbus/dbus.h>
+#include <glib.h>
+#include <gdbus.h>
+
+#include "ofono.h"
+
+#include "dbus-gsm.h"
+#include "modem.h"
+#include "driver.h"
+#include "common.h"
+
+#define NETWORK_REGISTRATION_INTERFACE "org.ofono.NetworkRegistration"
+#define NETWORK_OPERATOR_INTERFACE "org.ofono.NetworkOperator"
+
+#define NETWORK_REGISTRATION_FLAG_REQUESTING_OPLIST 0x1
+#define NETWORK_REGISTRATION_FLAG_PENDING 0x2
+
+#define AUTO_REGISTER 1
+
+/* How often we update the operator list, in seconds */
+#define OPERATOR_LIST_UPDATE_TIME 300
+
+struct network_registration_data {
+	int status;
+	int location;
+	int cellid;
+	int technology;
+	struct ofono_network_operator *current_operator;
+	GSList *operator_list;
+	struct ofono_network_registration_ops *ops;
+	int flags;
+	DBusMessage *pending;
+	int signal_strength;
+};
+
+static void operator_list_callback(const struct ofono_error *error, int total,
+				const struct ofono_network_operator *list,
+				void *data);
+
+static void current_operator_callback(const struct ofono_error *error,
+				const struct ofono_network_operator *current,
+				void *data);
+
+static void signal_strength_callback(const struct ofono_error *error,
+					int strength, void *data);
+
+static void registration_status_callback(const struct ofono_error *error,
+					int status, int lac, int ci, int tech,
+					void *data);
+
+struct ofono_network_operator_data {
+	struct ofono_network_operator *operator;
+	struct ofono_modem *modem;
+};
+
+static inline const char *network_operator_status_to_string(int status)
+{
+	switch (status) {
+	case OPERATOR_STATUS_AVAILABLE:
+		return "available";
+	case OPERATOR_STATUS_CURRENT:
+		return "current";
+	case OPERATOR_STATUS_FORBIDDEN:
+		return "forbidden";
+	}
+
+	return "unknown";
+}
+
+static inline const char *registration_status_to_string(int status)
+{
+	switch (status) {
+	case NETWORK_REGISTRATION_STATUS_NOT_REGISTERED:
+		return "unregistered";
+	case NETWORK_REGISTRATION_STATUS_REGISTERED:
+		return "registered";
+	case NETWORK_REGISTRATION_STATUS_SEARCHING:
+		return "searching";
+	case NETWORK_REGISTRATION_STATUS_DENIED:
+		return "denied";
+	case NETWORK_REGISTRATION_STATUS_UNKNOWN:
+		return "unknown";
+	case NETWORK_REGISTRATION_STATUS_ROAMING:
+		return "roaming";
+	}
+
+	return "";
+}
+
+static inline const char *registration_tech_to_string(int tech)
+{
+	switch (tech) {
+	case ACCESS_TECHNOLOGY_GSM:
+		return "GSM";
+	case ACCESS_TECHNOLOGY_GSM_COMPACT:
+		return "GSMCompact";
+	case ACCESS_TECHNOLOGY_UTRAN:
+		return "UTRAN";
+	case ACCESS_TECHNOLOGY_GSM_EGPRS:
+		return "GSM+EGPS";
+	case ACCESS_TECHNOLOGY_UTRAN_HSDPA:
+		return "UTRAN+HSDPA";
+	case ACCESS_TECHNOLOGY_UTRAN_HSUPA:
+		return "UTRAN+HSUPA";
+	case ACCESS_TECHNOLOGY_UTRAN_HSDPA_HSUPA:
+		return "UTRAN+HSDPA+HSUPA";
+	default:
+		return "";
+	}
+}
+
+static void register_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct network_registration_data *netreg = modem->network_registration;
+	DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *reply;
+
+	if (!netreg->pending)
+		goto out;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		reply = dbus_message_new_method_return(netreg->pending);
+	else
+		reply = dbus_gsm_failed(netreg->pending);
+
+	g_dbus_send_message(conn, reply);
+
+	dbus_message_unref(netreg->pending);
+	netreg->pending = NULL;
+
+out:
+	netreg->flags &= ~NETWORK_REGISTRATION_FLAG_PENDING;
+
+	if (netreg->ops->registration_status)
+		netreg->ops->registration_status(modem,
+					registration_status_callback, modem);
+}
+
+/* Must use dbus_gsm_free_string_array on network_operators */
+static void network_operator_populate_registered(struct ofono_modem *modem,
+						char ***network_operators)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+	char **children;
+	int i;
+	int modem_len;
+	int num_children;
+	GSList *l;
+	int *mccmnc;
+	char path[MAX_DBUS_PATH_LEN];
+
+	modem_len = snprintf(path, MAX_DBUS_PATH_LEN, "%s/operator",
+				modem->path);
+
+	if (!dbus_connection_list_registered(conn, path, &children)) {
+		ofono_debug("Unable to obtain registered NetworkOperator(s)");
+		*network_operators = g_try_new0(char *, 1);
+		return;
+	}
+
+	for (i = 0; children[i]; i++)
+		;
+
+	num_children = i;
+
+	*network_operators = g_try_new0(char *, num_children + 1);
+
+	mccmnc = g_try_new0(int, num_children * 2);
+	for (i = 0; i < num_children; i++)
+		sscanf(children[i], "%3d%3d", &mccmnc[i*2], &mccmnc[i*2+1]);
+
+	/* Quoting 27.007: "The list of operators shall be in order: home
+	 * network, networks referenced in SIM or active application in the
+	 * UICC (GSM or USIM) in the following order: HPLMN selector, User
+	 * controlled PLMN selector, Operator controlled PLMN selector and
+	 * PLMN selector (in the SIM or GSM application), and other networks."
+	 * Thus we must make sure we return the list in the same order,
+	 * if possible.  Luckily the operator_list is stored in order already
+	 */
+	i = 0;
+	for (l = modem->network_registration->operator_list; l; l = l->next) {
+		struct ofono_network_operator *op = l->data;
+		int j;
+
+		for (j = 0; children[j]; j++) {
+			if (op->mcc == mccmnc[j*2] && op->mnc == mccmnc[j*2+1]) {
+				/* Enough to store '/' + 3 char wide MCC + 3 char wide MNC + null */
+				(*network_operators)[i] =  g_try_new(char, modem_len + 8);
+				snprintf((*network_operators)[i], modem_len + 8, "%s/%s",
+					path, children[j]);
+				++i;
+			}
+		}
+	}
+
+	g_free(mccmnc);
+
+	dbus_free_string_array(children);
+}
+
+static void network_operator_destroy(gpointer userdata)
+{
+	struct ofono_network_operator_data *op = userdata;
+
+	g_free(op);
+}
+
+static gint network_operator_compare(gconstpointer a, gconstpointer b)
+{
+	const struct ofono_network_operator *opa = a;
+	const struct ofono_network_operator *opb = b;
+
+	if (opa->mcc < opb->mcc)
+		return -1;
+
+	if (opa->mcc > opb->mcc)
+		return 1;
+
+	if (opa->mnc < opb->mnc)
+		return -1;
+
+	if (opa->mnc > opb->mnc)
+		return 1;
+
+	return 0;
+}
+
+static inline const char *network_operator_build_path(struct ofono_modem *modem,
+							struct ofono_network_operator *oper)
+{
+	static char path[MAX_DBUS_PATH_LEN];
+
+	snprintf(path, MAX_DBUS_PATH_LEN, "%s/operator/%03d%03d",
+			modem->path, oper->mcc, oper->mnc);
+
+	return path;
+}
+
+static void network_operator_emit_available_operators(struct ofono_modem *modem)
+{
+	//struct network_registration_data *netreg = modem->network_registration;
+	DBusConnection *conn = dbus_gsm_connection();
+	char **network_operators;
+
+	network_operator_populate_registered(modem, &network_operators);
+
+	dbus_gsm_signal_array_property_changed(conn, modem->path,
+						NETWORK_REGISTRATION_INTERFACE,
+						"AvailableOperators",
+						DBUS_TYPE_OBJECT_PATH,
+						&network_operators);
+
+	dbus_gsm_free_string_array(network_operators);
+}
+
+static void set_network_operator_status(struct ofono_modem *modem,
+					struct ofono_network_operator *op,
+					int status)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+	//struct network_registration_data *netreg = modem->network_registration;
+	const char *status_str;
+	const char *path;
+
+	if (op->status == status)
+		return;
+
+	op->status = status;
+
+	status_str = network_operator_status_to_string(status);
+	path = network_operator_build_path(modem, op);
+
+	dbus_gsm_signal_property_changed(conn, path, NETWORK_OPERATOR_INTERFACE,
+						"Status", DBUS_TYPE_STRING,
+						&status_str);
+}
+
+static void set_network_operator_technology(struct ofono_modem *modem,
+						struct ofono_network_operator *op,
+						int tech)
+{
+	//struct network_registration_data *netreg = modem->network_registration;
+	DBusConnection *conn = dbus_gsm_connection();
+	const char *tech_str;
+	const char *path;
+
+	if (op->tech == tech)
+		return;
+
+	op->tech = tech;
+	tech_str = registration_tech_to_string(tech);
+	path = network_operator_build_path(modem, op);
+
+	dbus_gsm_signal_property_changed(conn, path, NETWORK_OPERATOR_INTERFACE,
+						"Technology", DBUS_TYPE_STRING,
+						&tech_str);
+}
+
+static void set_network_operator_name(struct ofono_modem *modem,
+					struct ofono_network_operator *op,
+					const char *name)
+{
+	struct network_registration_data *netreg = modem->network_registration;
+	DBusConnection *conn = dbus_gsm_connection();
+	const char *path;
+
+	if (!strncmp(op->name, name, OFONO_MAX_OPERATOR_NAME_LENGTH))
+		return;
+
+	strncpy(op->name, name, OFONO_MAX_OPERATOR_NAME_LENGTH);
+	op->name[OFONO_MAX_OPERATOR_NAME_LENGTH] = '\0';
+
+	path = network_operator_build_path(modem, op);
+
+	dbus_gsm_signal_property_changed(conn, path,
+				NETWORK_OPERATOR_INTERFACE,
+				"Name", DBUS_TYPE_STRING,
+				&name);
+
+	if (op == netreg->current_operator)
+		dbus_gsm_signal_property_changed(conn, modem->path,
+					NETWORK_REGISTRATION_INTERFACE,
+					"Operator", DBUS_TYPE_STRING,
+					&name);
+}
+
+static DBusMessage *network_operator_get_properties(DBusConnection *conn,
+							DBusMessage *msg,
+							void *data)
+{
+	struct ofono_network_operator_data *op = data;
+	DBusMessage *reply;
+	DBusMessageIter iter;
+	DBusMessageIter dict;
+
+	const char *name = op->operator->name;
+	const char *status =
+		network_operator_status_to_string(op->operator->status);
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return NULL;
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+						PROPERTIES_ARRAY_SIGNATURE,
+						&dict);
+
+	dbus_gsm_dict_append(&dict, "Name", DBUS_TYPE_STRING, &name);
+
+	dbus_gsm_dict_append(&dict, "Status", DBUS_TYPE_STRING, &status);
+
+	if (op->operator->mcc != -1) {
+		dbus_uint16_t mcc = op->operator->mcc;
+		dbus_gsm_dict_append(&dict, "MobileCountryCode",
+					DBUS_TYPE_UINT16, &mcc);
+	}
+
+	if (op->operator->mnc != -1) {
+		dbus_uint16_t mnc = op->operator->mnc;
+		dbus_gsm_dict_append(&dict, "MobileNetworkCode",
+					DBUS_TYPE_UINT16, &mnc);
+	}
+
+	if (op->operator->tech != -1) {
+		const char *technology =
+			registration_tech_to_string(op->operator->tech);
+
+		dbus_gsm_dict_append(&dict, "Technology", DBUS_TYPE_STRING,
+					&technology);
+	}
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+static DBusMessage *network_operator_register(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	struct ofono_network_operator_data *op = data;
+	struct network_registration_data *netreg = op->modem->network_registration;
+
+	if (netreg->flags & NETWORK_REGISTRATION_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (netreg->ops->register_manual == NULL)
+		return dbus_gsm_not_implemented(msg);
+
+	netreg->flags |= NETWORK_REGISTRATION_FLAG_PENDING;
+	netreg->pending = dbus_message_ref(msg);
+
+	netreg->ops->register_manual(op->modem, op->operator,
+					register_callback, op->modem);
+
+	return NULL;
+}
+
+static GDBusMethodTable network_operator_methods[] = {
+	{ "GetProperties",	"",	"a{sv}",	network_operator_get_properties },
+	{ "Register",		"",	"",		network_operator_register,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ }
+};
+
+static GDBusSignalTable network_operator_signals[] = {
+	{ "PropertyChanged",	"sv" },
+	{ }
+};
+
+static gboolean network_operator_dbus_register(struct ofono_modem *modem,
+						struct ofono_network_operator *op)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+	const char *path;
+
+	struct ofono_network_operator_data *opd =
+		g_try_new(struct ofono_network_operator_data, 1);
+
+	if (!opd)
+		return FALSE;
+
+	opd->operator = op;
+	opd->modem = modem;
+
+	path = network_operator_build_path(modem, op);
+
+	if (!g_dbus_register_interface(conn, path, NETWORK_OPERATOR_INTERFACE,
+					network_operator_methods,
+					network_operator_signals,
+					NULL, opd,
+					network_operator_destroy)) {
+		ofono_error("Could not register NetworkOperator %s", path);
+		network_operator_destroy(opd);
+
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+static gboolean network_operator_dbus_unregister(struct ofono_modem *modem,
+						struct ofono_network_operator *op)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+	const char *path = network_operator_build_path(modem, op);
+
+	return g_dbus_unregister_interface(conn, path,
+					NETWORK_OPERATOR_INTERFACE);
+}
+
+static struct network_registration_data *network_registration_create()
+{
+	struct network_registration_data *data;
+
+	data = g_try_new0(struct network_registration_data, 1);
+	if (data == NULL)
+		return data;
+
+	data->status = NETWORK_REGISTRATION_STATUS_UNKNOWN;
+	data->location = -1;
+	data->cellid = -1;
+	data->technology = -1;
+	data->signal_strength = -1;
+
+	return data;
+}
+
+static void network_registration_destroy(gpointer userdata)
+{
+	struct ofono_modem *modem = userdata;
+	struct network_registration_data *data = modem->network_registration;
+	GSList *l;
+
+	for (l = data->operator_list; l; l = l->next) {
+		network_operator_dbus_unregister(modem, l->data);
+		g_free(l->data);
+	}
+
+	g_slist_free(data->operator_list);
+
+	g_free(data);
+
+	modem->network_registration = 0;
+}
+
+static DBusMessage *network_get_properties(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct network_registration_data *netreg = modem->network_registration;
+	DBusMessage *reply;
+	DBusMessageIter iter;
+	DBusMessageIter dict;
+
+	const char *status = registration_status_to_string(netreg->status);
+	const char *operator =
+		netreg->current_operator ? netreg->current_operator->name : "";
+
+	char **network_operators;
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return NULL;
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+						PROPERTIES_ARRAY_SIGNATURE,
+						&dict);
+
+	dbus_gsm_dict_append(&dict, "Status", DBUS_TYPE_STRING, &status);
+
+	if (netreg->location != -1) {
+		dbus_uint16_t location = netreg->location;
+		dbus_gsm_dict_append(&dict, "LocationAreaCode",
+					DBUS_TYPE_UINT16, &location);
+	}
+
+	if (netreg->cellid != -1) {
+		dbus_uint32_t cellid = netreg->cellid;
+		dbus_gsm_dict_append(&dict, "CellId",
+					DBUS_TYPE_UINT32, &cellid);
+	}
+
+	if (netreg->technology != -1) {
+		const char *technology =
+			registration_tech_to_string(netreg->technology);
+
+		dbus_gsm_dict_append(&dict, "Technology", DBUS_TYPE_STRING,
+					&technology);
+	}
+
+	dbus_gsm_dict_append(&dict, "Operator", DBUS_TYPE_STRING, &operator);
+
+	network_operator_populate_registered(modem, &network_operators);
+
+	dbus_gsm_dict_append_array(&dict, "AvailableOperators",
+					DBUS_TYPE_OBJECT_PATH,
+					&network_operators);
+
+	dbus_gsm_free_string_array(network_operators);
+
+	if (netreg->signal_strength != -1) {
+		dbus_uint16_t strength = netreg->signal_strength;
+		dbus_gsm_dict_append(&dict, "Strength", DBUS_TYPE_UINT16,
+					&strength);
+	}
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+static DBusMessage *network_register(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct network_registration_data *netreg = modem->network_registration;
+
+	if (netreg->flags & NETWORK_REGISTRATION_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (netreg->ops->register_auto == NULL)
+		return dbus_gsm_not_implemented(msg);
+
+	netreg->flags |= NETWORK_REGISTRATION_FLAG_PENDING;
+	netreg->pending = dbus_message_ref(msg);
+
+	netreg->ops->register_auto(modem, register_callback, modem);
+
+	return NULL;
+}
+
+static DBusMessage *network_deregister(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct network_registration_data *netreg = modem->network_registration;
+
+	if (netreg->flags & NETWORK_REGISTRATION_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (netreg->ops->deregister == NULL)
+		return dbus_gsm_not_implemented(msg);
+
+	netreg->flags |= NETWORK_REGISTRATION_FLAG_PENDING;
+	netreg->pending = dbus_message_ref(msg);
+
+	netreg->ops->deregister(modem, register_callback, modem);
+
+	return NULL;
+}
+
+static GDBusMethodTable network_registration_methods[] = {
+	{ "GetProperties",	"",	"a{sv}",	network_get_properties	},
+	{ "Register",		"",	"",		network_register,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "Deregister",		"",	"",		network_deregister,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ }
+};
+
+static GDBusSignalTable network_registration_signals[] = {
+	{ "PropertyChanged",	"sv" },
+	{ }
+};
+
+static void update_network_operator_list(struct ofono_modem *modem)
+{
+	struct network_registration_data *netreg = modem->network_registration;
+
+	if (netreg->flags & NETWORK_REGISTRATION_FLAG_REQUESTING_OPLIST)
+		return;
+
+	if (!netreg->ops->list_operators)
+		return;
+
+	netreg->flags |= NETWORK_REGISTRATION_FLAG_REQUESTING_OPLIST;
+	netreg->ops->list_operators(modem, operator_list_callback, modem);
+}
+
+static gboolean update_network_operator_list_cb(void *user_data)
+{
+	struct ofono_modem *modem = user_data;
+
+	update_network_operator_list(modem);
+
+	return TRUE;
+}
+
+static gboolean update_network_operator_list_init(void *user_data)
+{
+	struct ofono_modem *modem = user_data;
+
+	update_network_operator_list(modem);
+
+	return FALSE;
+}
+
+static void set_registration_status(struct ofono_modem *modem, int status)
+{
+	const char *str_status = registration_status_to_string(status);
+	struct network_registration_data *netreg = modem->network_registration;
+	DBusConnection *conn = dbus_gsm_connection();
+
+	netreg->status = status;
+
+	dbus_gsm_signal_property_changed(conn, modem->path,
+					NETWORK_REGISTRATION_INTERFACE,
+					"Status", DBUS_TYPE_STRING,
+					&str_status);
+}
+
+static void set_registration_location(struct ofono_modem *modem, int lac)
+{
+	struct network_registration_data *netreg = modem->network_registration;
+	DBusConnection *conn = dbus_gsm_connection();
+	dbus_uint16_t dbus_lac = lac;
+
+	if (lac > 0xffff)
+		return;
+
+	netreg->location = lac;
+
+	if (netreg->location == -1)
+		return;
+
+	dbus_gsm_signal_property_changed(conn, modem->path,
+						NETWORK_REGISTRATION_INTERFACE,
+						"LocationAreaCode",
+						DBUS_TYPE_UINT16, &dbus_lac);
+}
+
+static void set_registration_cellid(struct ofono_modem *modem, int ci)
+{
+	struct network_registration_data *netreg = modem->network_registration;
+	DBusConnection *conn = dbus_gsm_connection();
+	dbus_uint16_t dbus_ci = ci;
+
+	netreg->cellid = ci;
+
+	if (netreg->cellid == -1)
+		return;
+
+	dbus_gsm_signal_property_changed(conn, modem->path,
+						NETWORK_REGISTRATION_INTERFACE,
+						"CellId", DBUS_TYPE_UINT32,
+						&dbus_ci);
+}
+
+static void set_registration_technology(struct ofono_modem *modem, int tech)
+{
+	struct network_registration_data *netreg = modem->network_registration;
+	const char *tech_str = registration_tech_to_string(tech);
+	DBusConnection *conn = dbus_gsm_connection();
+
+	netreg->technology = tech;
+
+	if (netreg->technology == -1)
+		return;
+
+	dbus_gsm_signal_property_changed(conn, modem->path,
+						NETWORK_REGISTRATION_INTERFACE,
+						"Technology", DBUS_TYPE_STRING,
+						&tech_str);
+}
+
+static void initialize_network_registration(struct ofono_modem *modem)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (!g_dbus_register_interface(conn, modem->path,
+					NETWORK_REGISTRATION_INTERFACE,
+					network_registration_methods,
+					network_registration_signals,
+					NULL, modem,
+					network_registration_destroy)) {
+		ofono_error("Could not register NetworkRegistration interface");
+		network_registration_destroy(modem);
+
+		return;
+	}
+
+	ofono_debug("NetworkRegistration interface for modem: %s created",
+			modem->path);
+
+	modem_add_interface(modem, NETWORK_REGISTRATION_INTERFACE);
+
+	if (modem->network_registration->ops->list_operators) {
+		g_timeout_add_seconds(OPERATOR_LIST_UPDATE_TIME,
+					update_network_operator_list_cb, modem);
+
+		g_timeout_add_seconds(5, update_network_operator_list_init,
+					modem);
+	}
+}
+
+void ofono_network_registration_notify(struct ofono_modem *modem, int status,
+			int lac, int ci, int tech)
+{
+	struct network_registration_data *netreg = modem->network_registration;
+
+	if (!netreg)
+		return;
+
+	if (netreg->status != status)
+		set_registration_status(modem, status);
+
+	if (netreg->location != lac)
+		set_registration_location(modem, lac);
+
+	if (netreg->cellid != ci)
+		set_registration_cellid(modem, ci);
+
+	if (netreg->technology != tech)
+		set_registration_technology(modem, tech);
+
+	if (netreg->status == 1 || netreg->status == 5) {
+		if (netreg->ops->current_operator)
+			netreg->ops->current_operator(modem,
+					current_operator_callback, modem);
+	} else {
+		struct ofono_error error;
+
+		error.type = OFONO_ERROR_TYPE_NO_ERROR;
+		error.error = 0;
+
+		current_operator_callback(&error, NULL, modem);
+
+		netreg->signal_strength = -1;
+	}
+}
+
+static void operator_list_callback(const struct ofono_error *error, int total,
+					const struct ofono_network_operator *list,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct network_registration_data *netreg = modem->network_registration;
+	GSList *n = NULL;
+	GSList *o;
+	int i;
+	gboolean need_to_emit = FALSE;
+
+	netreg->flags &= ~NETWORK_REGISTRATION_FLAG_REQUESTING_OPLIST;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error occurred during operator list");
+		return;
+	}
+
+	for (i = 0; i < total; i++) {
+		o = g_slist_find_custom(netreg->operator_list, &list[i],
+					network_operator_compare);
+
+		if (o) { /* Update and move to a new list */
+			set_network_operator_status(modem, o->data,
+							list[i].status);
+
+			set_network_operator_technology(modem, o->data,
+							list[i].tech);
+
+			set_network_operator_name(modem, o->data,
+							list[i].name);
+
+			n = g_slist_prepend(n, o->data);
+			netreg->operator_list =
+				g_slist_remove(netreg->operator_list, o->data);
+		} else {
+			/* New operator */
+			struct ofono_network_operator *op =
+				g_try_new0(struct ofono_network_operator, 1);
+			if (!op)
+				continue;
+
+			memcpy(op, &list[i], sizeof(struct ofono_network_operator));
+
+			n = g_slist_prepend(n, op);
+
+			network_operator_dbus_register(modem, op);
+
+			need_to_emit = TRUE;
+		}
+	}
+
+	if (n)
+		n = g_slist_reverse(n);
+
+	if (netreg->operator_list)
+		need_to_emit = TRUE;
+
+	for (o = netreg->operator_list; o; o = o->next) {
+		network_operator_dbus_unregister(modem, o->data);
+		g_free(o->data);
+	}
+
+	g_slist_free(netreg->operator_list);
+
+	netreg->operator_list = n;
+
+	if (need_to_emit)
+		network_operator_emit_available_operators(modem);
+}
+
+static void current_operator_callback(const struct ofono_error *error,
+				const struct ofono_network_operator *current,
+				void *data)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+	struct ofono_modem *modem = data;
+	struct network_registration_data *netreg = modem->network_registration;
+	GSList *op = NULL;
+	const char *operator;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error during current operator");
+		return;
+	}
+
+	if (!netreg->current_operator && !current)
+		return;
+
+	/* We got a new network operator, reset the previous one's status */
+	/* It will be updated properly later */
+	if (netreg->current_operator &&
+		(!current ||
+			network_operator_compare(current, netreg->current_operator)))
+		set_network_operator_status(modem, netreg->current_operator,
+						OPERATOR_STATUS_AVAILABLE);
+
+	if (current)
+		op = g_slist_find_custom(netreg->operator_list, current,
+					network_operator_compare);
+
+	if (op) {
+		netreg->current_operator = op->data;
+		set_network_operator_status(modem, op->data,
+						OPERATOR_STATUS_CURRENT);
+		set_network_operator_technology(modem, op->data,
+						current->tech);
+		set_network_operator_name(modem, op->data, current->name);
+
+		return;
+	}
+
+	if (current) {
+		netreg->current_operator =
+			g_try_new0(struct ofono_network_operator, 1);
+
+		if (!netreg->current_operator) {
+			ofono_error("Unable to allocate current operator");
+			return;
+		}
+
+		memcpy(netreg->current_operator, current,
+			sizeof(struct ofono_network_operator));
+
+		netreg->operator_list = g_slist_append(netreg->operator_list,
+						netreg->current_operator);
+
+		network_operator_dbus_register(modem, netreg->current_operator);
+		network_operator_emit_available_operators(modem);
+	} else {
+		/* We don't free this here because operator is registered */
+		/* Taken care of elsewhere */
+		netreg->current_operator = NULL;
+	}
+
+	operator =
+		netreg->current_operator ? netreg->current_operator->name : "";
+
+	dbus_gsm_signal_property_changed(conn, modem->path,
+					NETWORK_REGISTRATION_INTERFACE,
+					"Operator", DBUS_TYPE_STRING,
+					&operator);
+}
+
+static void registration_status_callback(const struct ofono_error *error,
+					int status, int lac, int ci, int tech,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error during registration status query");
+		return;
+	}
+
+	ofono_network_registration_notify(modem, status, lac, ci, tech);
+}
+
+static void init_registration_status(const struct ofono_error *error,
+					int status, int lac, int ci, int tech,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct network_registration_data *netreg = modem->network_registration;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error during registration status query");
+		return;
+	}
+
+	ofono_network_registration_notify(modem, status, lac, ci, tech);
+
+	/* Bootstrap our signal strength value without waiting for the
+	 * stack to report it
+	 */
+	if (netreg->status == 1 || netreg->status == 5) {
+		if (netreg->ops->signal_strength)
+			netreg->ops->signal_strength(modem,
+					signal_strength_callback, modem);
+	}
+
+	if (AUTO_REGISTER && (status == 0 || status == 3))
+		netreg->ops->register_auto(modem, register_callback, modem);
+}
+
+void ofono_signal_strength_notify(struct ofono_modem *modem, int strength)
+{
+	struct network_registration_data *netreg = modem->network_registration;
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (netreg->signal_strength == strength)
+		return;
+
+	/* Theoretically we can get signal strength even when not registered
+	 * to any network.  However, what do we do with it in that case?
+	 */
+	if (netreg->status != NETWORK_REGISTRATION_STATUS_REGISTERED &&
+		netreg->status != NETWORK_REGISTRATION_STATUS_ROAMING)
+		return;
+
+	netreg->signal_strength = strength;
+
+	if (strength != -1) {
+		dbus_uint16_t strength = netreg->signal_strength;
+
+		dbus_gsm_signal_property_changed(conn, modem->path,
+						NETWORK_REGISTRATION_INTERFACE,
+						"Strength", DBUS_TYPE_UINT16,
+						&strength);
+	}
+}
+
+static void signal_strength_callback(const struct ofono_error *error,
+					int strength, void *data)
+{
+	struct ofono_modem *modem = data;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		ofono_debug("Error during signal strength query");
+		return;
+	}
+
+	ofono_signal_strength_notify(modem, strength);
+}
+
+int ofono_network_registration_register(struct ofono_modem *modem,
+					struct ofono_network_registration_ops *ops)
+{
+	if (modem == NULL)
+		return -1;
+
+	if (ops == NULL)
+		return -1;
+
+	modem->network_registration = network_registration_create();
+	if (modem->network_registration == NULL)
+		return -1;
+
+	modem->network_registration->ops = ops;
+
+	initialize_network_registration(modem);
+
+	if (ops->registration_status)
+		ops->registration_status(modem, init_registration_status,
+						modem);
+
+	return 0;
+}
+
+void ofono_network_registration_unregister(struct ofono_modem *modem)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	g_dbus_unregister_interface(conn, modem->path,
+					NETWORK_REGISTRATION_INTERFACE);
+	modem_remove_interface(modem, NETWORK_REGISTRATION_INTERFACE);
+}
+
diff --git a/src/ofono.h b/src/ofono.h
index bf59de3f..0a7d32a8 100644
--- a/src/ofono.h
+++ b/src/ofono.h
@@ -23,6 +23,9 @@
 
 #define OFONO_API_SUBJECT_TO_CHANGE
 
+int __ofono_manager_init();
+void __ofono_manager_cleanup();
+
 #include <ofono/log.h>
 
 int __ofono_log_init(gboolean detach, gboolean debug);
diff --git a/src/ussd.c b/src/ussd.c
new file mode 100644
index 00000000..2429507a
--- /dev/null
+++ b/src/ussd.c
@@ -0,0 +1,426 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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
+
+#define _GNU_SOURCE
+#include <string.h>
+#include <stdio.h>
+
+#include <dbus/dbus.h>
+#include <glib.h>
+#include <gdbus.h>
+
+#include "ofono.h"
+
+#include "dbus-gsm.h"
+#include "modem.h"
+#include "driver.h"
+#include "common.h"
+#include "ussd.h"
+
+#define SUPPLEMENTARY_SERVICES_INTERFACE "org.ofono.SupplementaryServices"
+
+#define USSD_FLAG_PENDING 0x1
+
+enum ussd_state {
+	USSD_STATE_IDLE = 0,
+	USSD_STATE_ACTIVE = 1,
+	USSD_STATE_USER_ACTION = 2
+};
+
+static struct ussd_data *ussd_create()
+{
+	struct ussd_data *r;
+
+	r = g_try_new0(struct ussd_data, 1);
+
+	return r;
+}
+
+static void ussd_destroy(gpointer data)
+{
+	struct ofono_modem *modem = data;
+	struct ussd_data *ussd = modem->ussd;
+
+	g_free(ussd);
+}
+
+struct ss_control_entry {
+	char *service;
+	ss_control_cb_t cb;
+};
+
+static struct ss_control_entry *ss_control_entry_create(const char *service,
+							ss_control_cb_t cb)
+{
+	struct ss_control_entry *r;
+
+	r = g_try_new0(struct ss_control_entry, 1);
+
+	if (!r)
+		return r;
+
+	r->service = g_strdup(service);
+	r->cb = cb;
+
+	return r;
+}
+
+static void ss_control_entry_destroy(struct ss_control_entry *ca)
+{
+	g_free(ca->service);
+	g_free(ca);
+}
+
+static gint ss_control_entry_compare(gconstpointer a, gconstpointer b)
+{
+	const struct ss_control_entry *ca = a;
+	const struct ss_control_entry *cb = b;
+	int ret;
+
+	ret = strcmp(ca->service, cb->service);
+
+	if (ret)
+		return ret;
+
+	if (ca->cb < cb->cb)
+		return -1;
+
+	if (ca->cb > cb->cb)
+		return 1;
+
+	return 0;
+}
+
+static gint ss_control_entry_find_by_service(gconstpointer a, gconstpointer b)
+{
+	const struct ss_control_entry *ca = a;
+	//const char *cb = b;
+
+	return strcmp(ca->service, b);
+}
+
+gboolean ss_control_register(struct ofono_modem *modem, const char *str,
+				ss_control_cb_t cb)
+{
+	//struct ussd_data *ussd = modem->ussd;
+	struct ss_control_entry *entry;
+
+	if (!modem)
+		return FALSE;
+
+	entry = ss_control_entry_create(str, cb);
+
+	if (!entry)
+		return FALSE;
+
+	modem->ss_control_list = g_slist_append(modem->ss_control_list, entry);
+
+	return TRUE;
+}
+
+void ss_control_unregister(struct ofono_modem *modem, const char *str,
+				ss_control_cb_t cb)
+{
+	//struct ussd_data *ussd = modem->ussd;
+	const struct ss_control_entry entry = { (char *)str, cb };
+	GSList *l;
+
+	if (!modem)
+		return;
+
+	l = g_slist_find_custom(modem->ss_control_list, &entry,
+				ss_control_entry_compare);
+
+	if (!l)
+		return;
+
+	ss_control_entry_destroy(l->data);
+	modem->ss_control_list = g_slist_remove(modem->ss_control_list,
+						l->data);
+}
+
+static gboolean recognized_control_string(struct ofono_modem *modem,
+						const char *ss_str,
+						DBusMessage *msg)
+{
+	//struct ussd_data *ussd = modem->ussd;
+	char *str = g_strdup(ss_str);
+	char *sc, *sia, *sib, *sic, *dn;
+	int type;
+	gboolean ret = FALSE;
+
+	ofono_debug("parsing control string");
+
+	if (parse_ss_control_string(str, &type, &sc, &sia, &sib, &sic, &dn)) {
+		GSList *l = modem->ss_control_list;
+
+		ofono_debug("Got parse result: %d, %s, %s, %s, %s, %s",
+				type, sc, sia, sib, sic, dn);
+
+		while ((l = g_slist_find_custom(l, sc,
+				ss_control_entry_find_by_service)) != NULL) {
+			struct ss_control_entry *entry = l->data;
+
+			if (entry->cb(modem, type, sc, sia, sib, sic, dn, msg)) {
+				ret = TRUE;
+				goto out;
+			}
+
+			l = l->next;
+		}
+	}
+
+	/* TODO: Handle all strings that control voice calls */
+
+	/* TODO: Handle Multiple subscriber profile DN*59#SEND and *59#SEND
+	 */
+
+	/* Note: SIM PIN/PIN2 change and unblock and IMEI presentation
+	 * procedures are not handled by the daemon since they are not followed
+	 * by SEND and are not valid USSD requests.
+	 */
+
+	/* TODO: Handle Password registration according to 22.030 Section 6.5.4
+	 */
+
+out:
+	g_free(str);
+
+	return ret;
+}
+
+void ofono_ussd_notify(struct ofono_modem *modem, int status, const char *str)
+{
+	struct ussd_data *ussd = modem->ussd;
+	DBusConnection *conn = dbus_gsm_connection();
+	const char *ussdstr = "USSD";
+	const char sig[] = { DBUS_TYPE_STRING, 0 };
+	DBusMessage *reply;
+	DBusMessageIter iter;
+	DBusMessageIter variant;
+
+	if (status == USSD_STATUS_NOT_SUPPORTED) {
+		ussd->state = USSD_STATE_IDLE;
+		reply = dbus_gsm_not_supported(ussd->pending);
+		goto out;
+	}
+
+	if (status == USSD_STATUS_TIMED_OUT) {
+		ussd->state = USSD_STATE_IDLE;
+		reply = dbus_gsm_timed_out(ussd->pending);
+		goto out;
+	}
+
+	/* TODO: Rework this in the Agent framework */
+	if (ussd->state == USSD_STATE_ACTIVE) {
+		if (status == USSD_STATUS_ACTION_REQUIRED) {
+			ofono_error("Unable to handle action required ussd");
+			return;
+		}
+
+		reply = dbus_message_new_method_return(ussd->pending);
+
+		dbus_message_iter_init_append(reply, &iter);
+
+		dbus_message_iter_append_basic(&iter, DBUS_TYPE_STRING,
+						&ussdstr);
+
+		dbus_message_iter_open_container(&iter, DBUS_TYPE_VARIANT, sig,
+							&variant);
+
+		dbus_message_iter_append_basic(&variant, DBUS_TYPE_STRING,
+						&str);
+
+		dbus_message_iter_close_container(&iter, &variant);
+
+		ussd->state = USSD_STATE_IDLE;
+	} else {
+		ofono_error("Received an unsolicited USSD, ignoring for now...");
+		ofono_debug("USSD is: status: %d, %s", status, str);
+
+		return;
+	}
+
+out:
+	g_dbus_send_message(conn, reply);
+
+	dbus_message_unref(ussd->pending);
+	ussd->pending = NULL;
+}
+
+static void ussd_callback(const struct ofono_error *error, void *data)
+{
+	struct ussd_data *ussd = data;
+	DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *reply;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR)
+		ofono_debug("ussd request failed with error: %s",
+				telephony_error_to_str(error));
+
+	ussd->flags &= ~USSD_FLAG_PENDING;
+
+	if (!ussd->pending)
+		return;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR) {
+		ussd->state = USSD_STATE_ACTIVE;
+		return;
+	}
+
+	reply = dbus_gsm_failed(ussd->pending);
+
+	g_dbus_send_message(conn, reply);
+
+	dbus_message_unref(ussd->pending);
+	ussd->pending = NULL;
+}
+
+static DBusMessage *ussd_initiate(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct ussd_data *ussd = modem->ussd;
+	const char *str;
+
+	if (ussd->flags & USSD_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (ussd->state == USSD_STATE_ACTIVE)
+		return dbus_gsm_busy(msg);
+
+	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_STRING, &str,
+					DBUS_TYPE_INVALID) == FALSE)
+		return dbus_gsm_invalid_args(msg);
+
+	if (strlen(str) == 0)
+		return dbus_gsm_invalid_format(msg);
+
+	ofono_debug("checking if this is a recognized control string");
+	if (recognized_control_string(modem, str, msg))
+		return NULL;
+
+	ofono_debug("No.., checking if this is a USSD string");
+	if (!valid_ussd_string(str))
+		return dbus_gsm_invalid_format(msg);
+
+	ofono_debug("OK, running USSD request");
+
+	if (!ussd->ops->request)
+		return dbus_gsm_not_implemented(msg);
+
+	ussd->flags |= USSD_FLAG_PENDING;
+	ussd->pending = dbus_message_ref(msg);
+
+	ussd->ops->request(modem, str, ussd_callback, ussd);
+
+	return NULL;
+}
+
+static void ussd_cancel_callback(const struct ofono_error *err, void *data)
+{
+	//struct ussd_data *ussd = data;
+}
+
+static DBusMessage *ussd_cancel(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	struct ofono_modem *modem = data;
+	struct ussd_data *ussd = modem->ussd;
+
+	if (ussd->flags & USSD_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (ussd->state == USSD_STATE_IDLE)
+		return dbus_gsm_not_active(msg);
+
+	if (!ussd->ops->cancel)
+		return dbus_gsm_not_implemented(msg);
+
+	ussd->flags |= USSD_FLAG_PENDING;
+	ussd->pending = dbus_message_ref(msg);
+
+	ussd->ops->cancel(modem, ussd_cancel_callback, ussd);
+
+	return NULL;
+}
+
+static GDBusMethodTable ussd_methods[] = {
+	{ "Initiate",	"s",	"sv",	ussd_initiate,
+					G_DBUS_METHOD_FLAG_ASYNC },
+	{ "Cancel",	"",	"",	ussd_cancel,
+					G_DBUS_METHOD_FLAG_ASYNC },
+	{ }
+};
+
+static GDBusSignalTable ussd_signals[] = {
+	{ }
+};
+
+int ofono_ussd_register(struct ofono_modem *modem, struct ofono_ussd_ops *ops)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (modem == NULL)
+		return -1;
+
+	if (ops == NULL)
+		return -1;
+
+	modem->ussd = ussd_create();
+
+	if (modem->ussd == NULL)
+		return -1;
+
+	modem->ussd->ops = ops;
+
+	if (!g_dbus_register_interface(conn, modem->path,
+					SUPPLEMENTARY_SERVICES_INTERFACE,
+					ussd_methods, ussd_signals, NULL,
+					modem, ussd_destroy)) {
+		ofono_error("Could not create %s interface",
+				SUPPLEMENTARY_SERVICES_INTERFACE);
+
+		ussd_destroy(modem->ussd);
+
+		return -1;
+	}
+
+	modem_add_interface(modem, SUPPLEMENTARY_SERVICES_INTERFACE);
+
+	return 0;
+}
+
+void ofono_ussd_unregister(struct ofono_modem *modem)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (modem->ussd == NULL)
+		return;
+
+	modem_remove_interface(modem, SUPPLEMENTARY_SERVICES_INTERFACE);
+	g_dbus_unregister_interface(conn, modem->path,
+					SUPPLEMENTARY_SERVICES_INTERFACE);
+}
diff --git a/src/ussd.h b/src/ussd.h
new file mode 100644
index 00000000..7d8c6d9d
--- /dev/null
+++ b/src/ussd.h
@@ -0,0 +1,39 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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
+ *
+ */
+
+struct ussd_data {
+	struct ofono_ussd_ops *ops;
+	int state;
+	DBusMessage *pending;
+	int flags;
+};
+
+typedef gboolean (*ss_control_cb_t)(struct ofono_modem *modem, int type,
+					const char *sc,
+					const char *sia, const char *sib,
+					const char *sic, const char *dn,
+					DBusMessage *msg);
+
+gboolean ss_control_register(struct ofono_modem *modem, const char *str,
+				ss_control_cb_t cb);
+
+void ss_control_unregister(struct ofono_modem *modem, const char *str,
+				ss_control_cb_t cb);
diff --git a/src/util.c b/src/util.c
new file mode 100644
index 00000000..90c36cd9
--- /dev/null
+++ b/src/util.c
@@ -0,0 +1,693 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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 <string.h>
+#include <ctype.h>
+
+#include <glib.h>
+
+#include "util.h"
+
+/*
+	Name:			GSM 03.38 to Unicode
+	Unicode version:	3.0
+	Table version:		1.1
+	Table format:		Format A
+	Date:			2000 May 30
+	Authors:		Ken Whistler
+				Kent Karlsson
+				Markus Kuhn
+
+	Copyright (c) 2000 Unicode, Inc.  All Rights reserved.
+
+	This file is provided as-is by Unicode, Inc. (The Unicode Consortium).
+	No claims are made as to fitness for any particular purpose.  No
+	warranties of any kind are expressed or implied.  The recipient
+	agrees to determine applicability of information provided.  If this
+	file has been provided on optical media by Unicode, Inc., the sole
+	remedy for any claim will be exchange of defective media within 90
+	days of receipt.
+
+	Unicode, Inc. hereby grants the right to freely use the information
+	supplied in this file in the creation of products supporting the
+	Unicode Standard, and to make copies of this file in any form for
+	internal or external distribution as long as this notice remains
+	attached.
+*/
+
+/* GSM to Unicode extension table, for GSM sequences starting with 0x1B */
+static unsigned short gsm_extension[] =
+{
+	0x0A, 0x000C,		/* See NOTE 3 in 23.038 */
+	0x14, 0x005E,
+	0x1B, 0x0020,		/* See NOTE 1 in 23.038 */
+	0x28, 0x007B,
+	0x29, 0x007D,
+	0x2F, 0x005C,
+	0x3C, 0x005B,
+	0x3D, 0x007E,
+	0x3E, 0x005D,
+	0x40, 0x007C,
+	0x65, 0x20AC
+};
+
+/* Used for conversion of GSM to Unicode */
+static unsigned short gsm_table[] =
+{
+	0x0040, 0x00A3, 0x0024, 0x00A5, 0x00E8, 0x00E9, 0x00F9, 0x00EC, /* 0x07 */
+	0x00F2, 0x00E7, 0x000A, 0x00D8, 0x00F8, 0x000D, 0x00C5, 0x00E5, /* 0x0F */
+	0x0394, 0x005F, 0x03A6, 0x0393, 0x039B, 0x03A9, 0x03A0, 0x03A8, /* 0x17 */
+	0x03A3, 0x0398, 0x039E, 0x00A0, 0x00C6, 0x00E6, 0x00DF, 0x00C9, /* 0x1F */
+	0x0020, 0x0021, 0x0022, 0x0023, 0x00A4, 0x0025, 0x0026, 0x0027, /* 0x27 */
+	0x0028, 0x0029, 0x002A, 0x002B, 0x002C, 0x002D, 0x002E, 0x002F, /* 0x2F */
+	0x0030, 0x0031, 0x0032, 0x0033, 0x0034, 0x0035, 0x0036, 0x0037, /* 0x37 */
+	0x0038, 0x0039, 0x003A, 0x003B, 0x003C, 0x003D, 0x003E, 0x003F, /* 0x3F */
+	0x00A1, 0x0041, 0x0042, 0x0043, 0x0044, 0x0045, 0x0046, 0x0047, /* 0x47 */
+	0x0048, 0x0049, 0x004A, 0x004B, 0x004C, 0x004D, 0x004E, 0x004F, /* 0x4F */
+	0x0050, 0x0051, 0x0052, 0x0053, 0x0054, 0x0055, 0x0056, 0x0057, /* 0x57 */
+	0x0058, 0x0059, 0x005A, 0x00C4, 0x00D6, 0x00D1, 0x00DC, 0x00A7, /* 0x5F */
+	0x00BF, 0x0061, 0x0062, 0x0063, 0x0064, 0x0065, 0x0066, 0x0067, /* 0x67 */
+	0x0068, 0x0069, 0x006A, 0x006B, 0x006C, 0x006D, 0x006E, 0x006F, /* 0x6F */
+	0x0070, 0x0071, 0x0072, 0x0073, 0x0074, 0x0075, 0x0076, 0x0077, /* 0x77 */
+	0x0078, 0x0079, 0x007A, 0x00E4, 0x00F6, 0x00F1, 0x00FC, 0x00E0  /* 0x7F */
+};
+
+#define GUND 0xFFFF
+
+/* 3GPP 27.005 Annex A */
+static unsigned short unicode_256_table[] =
+{
+	GUND, GUND, GUND, GUND, GUND, GUND, GUND, GUND, /* 0x07 */
+	GUND, GUND, 0x0A, GUND, 0x1B0A, 0x0D, GUND, GUND, /* 0x0F */
+	GUND, GUND, GUND, GUND, GUND, GUND, GUND, GUND, /* 0x17 */
+	GUND, GUND, GUND, GUND, GUND, GUND, GUND, GUND, /* 0x1F */
+	0x20, 0x21, 0x22, 0x23, 0x02, 0x25, 0x26, 0x27, /* 0x27 */
+	0x28, 0x29, 0x2A, 0x2B, 0x2C, 0x2D, 0x2E, 0x2F, /* 0x2F */
+	0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, /* 0x37 */
+	0x38, 0x39, 0x3A, 0x3B, 0x3C, 0x3D, 0x3E, 0x3F, /* 0x3F */
+	0x00, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, /* 0x47 */
+	0x48, 0x49, 0x4A, 0x4B, 0x4C, 0x4D, 0x4E, 0x4F, /* 0x4F */
+	0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, /* 0x57 */
+	0x58, 0x59, 0x5A, 0x1B3C, 0x1B2F, 0x1B3E, 0x1B14, 0x11, /* 0x5F */
+	GUND, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67, /* 0x67 */
+	0x68, 0x69, 0x6A, 0x6B, 0x6C, 0x6D, 0x6E, 0x6F, /* 0x6F */
+	0x70, 0x71, 0x72, 0x73, 0x74, 0x75, 0x76, 0x77, /* 0x77 */
+	0x78, 0x79, 0x7A, 0x1B28, 0x1B40, 0x1B29, 0x1B3D, GUND, /* 0x7F */
+	GUND, GUND, GUND, GUND, GUND, GUND, GUND, GUND, /* 0x87 */
+	GUND, GUND, GUND, GUND, GUND, GUND, GUND, GUND, /* 0x8F */
+	GUND, GUND, GUND, GUND, GUND, GUND, GUND, GUND, /* 0x97 */
+	GUND, GUND, GUND, GUND, GUND, GUND, GUND, GUND, /* 0x9F */
+	GUND, 0x40, GUND, 0x01, 0x24, 0x03, GUND, 0x5f, /* 0xA7 */
+	GUND, GUND, GUND, GUND, GUND, GUND, GUND, GUND, /* 0xAF */
+	GUND, GUND, GUND, GUND, GUND, GUND, GUND, GUND, /* 0xB7 */
+	GUND, GUND, GUND, GUND, GUND, GUND, GUND, 0x60, /* 0xBF */
+	0x41, 0x41, 0x41, 0x41, 0x5B, 0x0E, 0x1C, 0x09, /* 0xC7 */
+	0x45, 0x1F, 0x45, 0x45, 0x49, 0x49, 0x49, 0x49, /* 0xCF */
+	GUND, 0x5D, 0x4F, 0x4F, 0x4F, 0x4F, 0x5C, GUND, /* 0xD7 */
+	0x0B, 0x55, 0x55, 0x55, 0x5E, 0x59, GUND, 0x1E, /* 0xDF */
+	0x7F, 0x61, 0x61, 0x61, 0x7B, 0x0F, 0x1D, 0x09, /* 0xE7 */
+	0x04, 0x05, 0x65, 0x65, 0x07, 0x69, 0x69, 0x69, /* 0xEF */
+	GUND, 0x7D, 0x08, 0x6F, 0x6F, 0x6F, 0x7C, GUND, /* 0xF7 */
+	0x0C, 0x06, 0x75, 0x75, 0x7E, 0x79, GUND, 0x79  /* 0xFF */
+};
+
+/* Starts at 0x0390 */
+static unsigned short greek_unicode_offset = 0x0390;
+
+static unsigned short greek_unicode_table[] =
+{
+	GUND, GUND, GUND, 0x13, 0x10, GUND, GUND, GUND, /* 0x07 */
+	0x19, GUND, GUND, 0x14, GUND, GUND, 0x1A, GUND, /* 0x0F */
+	0x16, GUND, GUND, 0x18, GUND, GUND, 0x12, GUND, /* 0x17 */
+	0x17, 0x15, GUND, GUND, GUND, GUND, GUND, GUND, /* 0x1F */
+};
+
+#define UTF8_LENGTH(c) \
+	((c) < 0x80 ? 1 : \
+	 ((c) < 0x800 ? 2 : 3))
+
+static unsigned short gsm_extension_table_lookup(unsigned char k)
+{
+	static unsigned int ext_table_len =
+		(sizeof(gsm_extension) / sizeof(unsigned short)) >> 1;
+	unsigned int i;
+	unsigned short *t;
+
+	for (i = 0, t = gsm_extension; i < ext_table_len; i++) {
+		if (t[0] == k)
+			return t[1];
+		t += 2;
+	}
+
+	return 0;
+}
+
+/*!
+ * Converts text coded using GSM codec into UTF8 encoded text.  If len
+ * is less than 0, and terminator character is given, the length is
+ * computed automatically.
+ *
+ * Returns newly-allocated UTF8 encoded string or NULL if the conversion
+ * could not be performed.  Returns the number of bytes read from the
+ * GSM encoded string in items_read (if not NULL), not including the
+ * terminator character. Returns the number of bytes written into the UTF8
+ * encoded string in items_written (if not NULL) not including the terminal
+ * '\0' character.  The caller is reponsible for freeing the returned value.
+ */
+char *convert_gsm_to_utf8(const unsigned char *text, long len,
+				long *items_read, long *items_written,
+				unsigned char terminator)
+{
+	char *res = NULL;
+	char *out;
+	long i = 0;
+	long res_length;
+
+	if (len == 0 || (len < 0 && !terminator))
+		goto err_out;
+
+	if (len < 0) {
+		i = 0;
+
+		while (text[i] != terminator)
+			i++;
+
+		len = i;
+	}
+
+	for (i = 0, res_length = 0; i < len; i++) {
+		unsigned short c;
+
+		if (text[i] > 0x7f)
+			goto err_out;
+
+		if (text[i] == 0x1b) {
+			++i;
+			if (i >= len)
+				goto err_out;
+
+			c = gsm_extension_table_lookup(text[i]);
+
+			if (c == 0)
+				goto err_out;
+		} else {
+			c = gsm_table[text[i]];
+		}
+
+		res_length += UTF8_LENGTH(c);
+	}
+
+	res = g_malloc(res_length + 1);
+
+	if (!res)
+		goto err_out;
+
+	out = res;
+
+	i = 0;
+	while (out < res + res_length) {
+		unsigned short c;
+
+		if (text[i] == 0x1b)
+			c = gsm_extension_table_lookup(text[++i]);
+		else
+			c = gsm_table[text[i]];
+
+		out += g_unichar_to_utf8(c, out);
+
+		++i;
+	}
+
+	*out = '\0';
+
+	if (items_written)
+		*items_written = out - res;
+
+err_out:
+	if (items_read)
+		*items_read = i;
+
+	return res;
+}
+
+static unsigned short unicode_to_gsm(unsigned short c)
+{
+	static int greek_unicode_size = sizeof(greek_unicode_table) /
+					sizeof(unsigned short);
+	unsigned short converted = GUND;
+
+	if (c == 0x20AC)
+		converted = 0x1B65;
+	else if (c < 256)
+		converted = unicode_256_table[c];
+	else if ((c >= greek_unicode_offset) &&
+			(c < (greek_unicode_offset + greek_unicode_size))) {
+		converted = greek_unicode_table[c-greek_unicode_offset];
+	}
+
+	return converted;
+}
+
+/*!
+ * Converts UTF-8 encoded text to GSM alphabet.  The result is unpacked,
+ * with the 7th bit always 0.  If terminator is not 0, a terminator character
+ * is appended to the result.  This should be in the range 0x80-0xf0
+ *
+ * Returns the encoded data or NULL if the data could not be encoded.  The
+ * data must be freed by the caller.  If items_read is not NULL, it contains
+ * the actual number of bytes read.  If items_written is not NULL, contains
+ * the number of bytes written.
+ */
+unsigned char *convert_utf8_to_gsm(const char *text, long len,
+					long *items_read, long *items_written,
+					unsigned char terminator)
+{
+	long nchars = 0;
+	const char *in;
+	unsigned char *out;
+	unsigned char *res = NULL;
+	long res_len;
+	long i;
+
+	in = text;
+	res_len = 0;
+
+	while ((len < 0 || text + len - in > 0) && *in) {
+		long max = len < 0 ? 6 : text + len - in;
+		gunichar c = g_utf8_get_char_validated(in, max);
+		unsigned short converted = GUND;
+
+		if (c & 0x80000000)
+			goto err_out;
+
+		if (c > 0xffff)
+			goto err_out;
+
+		converted = unicode_to_gsm(c);
+
+		if (converted == GUND)
+			goto err_out;
+
+		if (converted & 0x1b00)
+			res_len += 2;
+		else
+			res_len += 1;
+
+		in = g_utf8_next_char(in);
+		nchars += 1;
+	}
+
+	res = g_malloc(res_len + (terminator ? 1 : 0));
+
+	if (!res)
+		goto err_out;
+
+	in = text;
+	out = res;
+	for (i = 0; i < nchars; i++) {
+		unsigned short converted;
+
+		gunichar c = g_utf8_get_char(in);
+
+		converted = unicode_to_gsm(c);
+		if (converted & 0x1b00) {
+			*out = 0x1b;
+			++out;
+		}
+
+		*out = converted;
+		++out;
+
+		in = g_utf8_next_char(in);
+	}
+
+	if (terminator)
+		*out = terminator;
+
+	if (items_written)
+		*items_written = out - res;
+
+err_out:
+	if (items_read)
+		*items_read = in - text;
+
+	return res;
+}
+
+/*!
+ * Decodes the hex encoded data and converts to a byte array.  If terminator
+ * is not 0, the terminator character is appended to the end of the result.
+ * This might be useful for converting GSM encoded data if the CSCS is set
+ * to HEX.
+ *
+ * Please note that this since GSM does allow embedded null characeters, use
+ * of the terminator or the items_writen is encouraged to find the real size
+ * of the result.
+ */
+unsigned char *decode_hex_own_buf(const char *in, long len, long *items_written,
+					unsigned char terminator,
+					unsigned char *buf)
+{
+	long i, j;
+	char c;
+	unsigned char b;
+
+	if (len < 0)
+		len = strlen(in);
+
+	len &= ~0x1;
+
+	for (i = 0, j = 0; i < len; i++, j++) {
+		c = toupper(in[i]);
+	
+		if (c >= '0' && c <= '9')
+			b = c - '0';
+		else if (c >= 'A' && c <= 'F')
+			b = 10 + c - 'A';
+		else
+			return NULL;
+
+		i += 1;
+
+		c = toupper(in[i]);
+
+		if (c >= '0' && c <= '9')
+			b = b*16 + c - '0';
+		else if (c >= 'A' && c <= 'F')
+			b = b*16 + 10 + c - 'A';
+		else
+			return NULL;
+
+		buf[j] = b;
+	}
+
+	if (terminator)
+		buf[j] = terminator;
+
+	if (items_written)
+		*items_written = j;
+
+	return buf;
+}
+
+unsigned char *decode_hex(const char *in, long len, long *items_written,
+				unsigned char terminator)
+{
+	long i;
+	char c;
+	unsigned char *buf;
+
+	if (len < 0)
+		len = strlen(in);
+
+	len &= ~0x1;
+
+	for (i = 0; i < len; i++) {
+		c = toupper(in[i]);
+
+		if ((c >= '0' && c <= '9') || (c >= 'A' && c <= 'F'))
+			continue;
+
+		return NULL;
+	}
+
+	buf = g_new(unsigned char, (len >> 1) + (terminator ? 1 : 0));
+
+	return decode_hex_own_buf(in, len, items_written, terminator, buf);
+}
+
+/*!
+ * Encodes the data using hexadecimal characters.  len can be negative,
+ * in that case the terminator is used to find the last character.  This is
+ * useful for handling GSM-encoded strings which allow ASCII NULL character
+ * in the stream.
+ */
+char *encode_hex_own_buf(const unsigned char *in, long len,
+				unsigned char terminator, char *buf)
+{
+	long i, j;
+	char c;
+
+	if (len < 0) {
+		i = 0;
+
+		while (in[i] != terminator)
+			i++;
+
+		len = i;
+	}
+
+	for (i = 0, j = 0; i < len; i++, j++) {
+		c = (in[i] >> 4) & 0xf;
+
+		if (c <= 9)
+			buf[j] = '0' + c;
+		else
+			buf[j] = 'A' + c - 10;
+
+		j += 1;
+
+		c = (in[i]) & 0xf;
+
+		if (c <= 9)
+			buf[j] = '0' + c;
+		else
+			buf[j] = 'A' + c - 10;
+	}
+
+	buf[j] = '\0';
+
+	return buf;
+}
+
+char *encode_hex(const unsigned char *in, long len, unsigned char terminator)
+{
+	char *buf;
+	int i;
+
+	if (len < 0) {
+		i = 0;
+
+		while (in[i] != terminator)
+			i++;
+
+		len = i;
+	}
+
+	buf = g_new(char, len * 2 + 1);
+
+	return encode_hex_own_buf(in, len, terminator, buf);
+}
+
+unsigned char *unpack_7bit_own_buf(const unsigned char *in, long len,
+					int byte_offset, gboolean cb,
+					long max_to_unpack, long *items_written,
+					unsigned char terminator,
+					unsigned char *buf)
+{
+	unsigned char rest = 0;
+	unsigned char *out = buf;
+	int bits = 7 - (byte_offset % 7);
+	long i;
+
+	if (len <= 0)
+		return NULL;
+
+	/* In the case of CB, unpack as much as possible */
+	if (cb == TRUE)
+		max_to_unpack = len * 8 / 7;
+
+	for (i = 0; (i < len) && ((out-buf) < max_to_unpack); i++) {
+		/* Grab what we have in the current octet */
+		*out = (in[i] & ((1 << bits) - 1)) << (7 - bits);
+
+		/* Append what we have from the previous octet, if any */
+		*out |= rest;
+
+		/* Figure out the remainder */
+		rest = (in[i] >> bits) & ((1 << (8-bits)) - 1);
+
+		/* We have the entire character, here we don't increate
+		 * out if this is we started at an offset.  Instead
+		 * we effectively populate variable rest */
+		if (i != 0 || bits == 7)
+			out++;
+
+		if ((out-buf) == max_to_unpack)
+			break;
+
+		/* We expected only 1 bit from this octet, means there's 7
+		 * left, take care of them here */
+		if (bits == 1) {
+			*out = rest;
+			out++;
+			bits = 7;
+			rest = 0;
+		} else
+			bits = bits - 1;
+	}
+
+	/* According to 23.038 6.1.2.3.1, last paragraph:
+	 * "If the total number of characters to be sent equals (8n-1)
+	 * where n=1,2,3 etc. then there are 7 spare bits at the end
+	 * of the message. To avoid the situation where the receiving
+	 * entity confuses 7 binary zero pad bits as the @ character,
+	 * the carriage return or <CR> character shall be used for
+	 * padding in this situation, just as for Cell Broadcast."
+	 *
+	 * "The receiving entity shall remove the final <CR> character where
+	 * the message ends on an octet boundary with <CR> as the last
+	 * character.
+	 */
+	if (cb && (((out - buf) % 8) == 0) && (*(out-1) == '\r'))
+			out = out - 1;
+
+	if (terminator)
+		*out = terminator;
+
+	if (items_written)
+		*items_written = out - buf;
+
+	return buf;
+}
+
+unsigned char *unpack_7bit(const unsigned char *in, long len, int byte_offset,
+				gboolean cb, long max_to_unpack,
+				long *items_written, unsigned char terminator)
+{
+	unsigned char *buf = g_new(unsigned char, 
+					len * 8 / 7 + (terminator ? 1 : 0));
+	
+	return unpack_7bit_own_buf(in, len, byte_offset, cb, max_to_unpack,
+				items_written, terminator, buf);
+}
+
+unsigned char *pack_7bit_own_buf(const unsigned char *in, long len,
+					int byte_offset, gboolean cb,
+					long *items_written,
+					unsigned char terminator,
+					unsigned char *buf)
+{
+	int bits = 7 - (byte_offset % 7);
+	unsigned char *out = buf;
+	long i;
+	long total_bits;
+
+	if (len == 0 || !items_written)
+		return NULL;
+
+	if (len < 0) {
+		i = 0;
+
+		while (in[i] != terminator)
+			i++;
+
+		len = i;
+	}
+	
+	total_bits = len * 7;
+
+	if (bits != 7) {
+		total_bits += bits;
+		bits = bits - 1;
+		*out = 0;
+	}
+
+	for (i = 0; i < len; i++) {
+		if (bits != 7) {
+			*out |= (in[i] & ((1 << (7 - bits)) - 1)) <<
+					(bits + 1);
+			out++;
+		}
+
+		/* This is a no op when bits == 0, lets keep valgrind happy */
+		if (bits != 0)
+			*out = in[i] >> (7 - bits);
+
+		if (bits == 0)
+			bits = 7;
+		else
+			bits = bits - 1;
+	}
+	
+	/* If <CR> is intended to be the last character and the message
+	 * (including the wanted <CR>) ends on an octet boundary, then
+	 * another <CR> must be added together with a padding bit 0. The
+	 * receiving entity will perform the carriage return function twice,
+	 * but this will not result in misoperation as the definition of
+	 * <CR> in clause 6.1.1 is identical to the definition of <CR><CR>.
+	 */
+	if (cb && ((total_bits % 8) == 1))
+		*out |= '\r' << 1;
+
+	if (bits != 7)
+		out++;
+
+	if (cb && ((total_bits % 8) == 0) && (in[len-1] == '\r')) {
+		*out = '\r';
+		out++;
+	}
+
+	*items_written = out - buf;
+
+	return buf;
+}
+
+unsigned char *pack_7bit(const unsigned char *in, long len, int byte_offset,
+				gboolean cb, long *items_written,
+				unsigned char terminator)
+{
+	int bits = 7 - (byte_offset % 7);
+	long i;
+	long total_bits;
+	unsigned char *buf;
+
+	if (len == 0 || !items_written)
+		return NULL;
+
+	if (len < 0) {
+		i = 0;
+
+		while (in[i] != terminator)
+			i++;
+
+		len = i;
+	}
+	
+	total_bits = len * 7;
+
+	if (bits != 7)
+		total_bits += bits;
+	
+	/* Round up number of bytes, must append <cr> if true */
+	if (cb && ((total_bits % 8) == 0) && (in[len-1] == '\r'))
+		buf = g_new(unsigned char, (total_bits + 14) / 8);
+	else
+		buf = g_new(unsigned char, (total_bits + 7) / 8);
+
+	return pack_7bit_own_buf(in, len, byte_offset, cb, items_written,
+					terminator, buf);
+}
diff --git a/src/util.h b/src/util.h
new file mode 100644
index 00000000..9d90d315
--- /dev/null
+++ b/src/util.h
@@ -0,0 +1,58 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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
+ *
+ */
+
+char *convert_gsm_to_utf8(const unsigned char *text, long len, long *items_read,
+				long *items_written, unsigned char terminator);
+unsigned char *convert_utf8_to_gsm(const char *text, long len, long *items_read,
+				long *items_written, unsigned char terminator);
+
+unsigned char *decode_hex_own_buf(const char *in, long len, long *items_written,
+					unsigned char terminator,
+					unsigned char *buf);
+
+unsigned char *decode_hex(const char *in, long len, long *items_written,
+				unsigned char terminator);
+
+char *encode_hex_own_buf(const unsigned char *in, long len,
+				unsigned char terminator, char *buf);
+
+char *encode_hex(const unsigned char *in, long len,
+			unsigned char terminator);
+
+unsigned char *unpack_7bit_own_buf(const unsigned char *in, long len,
+					int byte_offset, gboolean cb,
+					long max_to_unpack, long *items_written,
+					unsigned char terminator,
+					unsigned char *buf);
+
+unsigned char *unpack_7bit(const unsigned char *in, long len, int byte_offset,
+				gboolean cb, long max_to_unpack,
+				long *items_written, unsigned char terminator);
+
+unsigned char *pack_7bit_own_buf(const unsigned char *in, long len,
+					int byte_offset, gboolean cb,
+					long *items_written,
+					unsigned char terminator,
+					unsigned char *buf);
+
+unsigned char *pack_7bit(const unsigned char *in, long len, int byte_offset,
+				gboolean cb_or_ussd,
+				long *items_written, unsigned char terminator);
diff --git a/src/voicecall.c b/src/voicecall.c
new file mode 100644
index 00000000..7a5a4b9d
--- /dev/null
+++ b/src/voicecall.c
@@ -0,0 +1,1684 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2009  Intel Corporation. All rights reserved.
+ *
+ *  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 <string.h>
+#include <stdio.h>
+#include <time.h>
+
+#include <dbus/dbus.h>
+#include <glib.h>
+#include <gdbus.h>
+
+#include "ofono.h"
+
+#include "driver.h"
+#include "common.h"
+#include "dbus-gsm.h"
+#include "modem.h"
+
+#define VOICECALL_MANAGER_INTERFACE "org.ofono.VoiceCallManager"
+#define VOICECALL_INTERFACE "org.ofono.VoiceCall"
+
+#define VOICECALLS_FLAG_PENDING 0x1
+#define VOICECALLS_FLAG_MULTI_RELEASE 0x2
+#define VOICECALLS_FLAG_UPDATING_CALL_LIST 0x4
+#define VOICECALLS_FLAG_UPDATING_MPTY_CALL_LIST 0x8
+
+#define MAX_VOICE_CALLS 16
+
+struct voicecalls_data {
+	GSList *call_list;
+	GSList *release_list;
+	GSList *multiparty_list;
+	struct ofono_voicecall_ops *ops;
+	int flags;
+	DBusMessage *pending;
+};
+
+struct voicecall {
+	struct ofono_call *call;
+	struct ofono_modem *modem;
+	time_t start_time;
+};
+
+static void generic_callback(const struct ofono_error *error, void *data);
+static void dial_callback(const struct ofono_error *error, void *data);
+static void multirelease_callback(const struct ofono_error *err, void *data);
+static void multiparty_create_callback(const struct ofono_error *error,
+					void *data);
+static void private_chat_callback(const struct ofono_error *error, void *data);
+
+static gint call_compare_by_id(gconstpointer a, gconstpointer b)
+{
+	const struct ofono_call *call = ((struct voicecall *)a)->call;
+	unsigned int id = GPOINTER_TO_UINT(b);
+
+	if (id < call->id)
+		return -1;
+
+	if (id > call->id)
+		return 1;
+
+	return 0;
+}
+
+static gint call_compare(gconstpointer a, gconstpointer b)
+{
+	const struct voicecall *ca = a;
+	const struct voicecall *cb = b;
+
+	if (ca->call->id < cb->call->id)
+		return -1;
+
+	if (ca->call->id > cb->call->id)
+		return 1;
+
+	return 0;
+}
+
+static const char *call_status_to_string(int status)
+{
+	switch (status) {
+	case CALL_STATUS_ACTIVE:
+		return "active";
+	case CALL_STATUS_HELD:
+		return "held";
+	case CALL_STATUS_DIALING:
+		return "dialing";
+	case CALL_STATUS_ALERTING:
+	return "alerting";
+	case CALL_STATUS_INCOMING:
+		return "incoming";
+	case CALL_STATUS_WAITING:
+		return "waiting";
+	default:
+		return "disconnected";
+	}
+}
+
+static const char *phone_and_clip_to_string(const char *number, int type,
+						int clip_validity)
+{
+	if (clip_validity == CLIP_VALIDITY_WITHHELD && !strlen(number))
+		return "withheld";
+
+	if (clip_validity == CLIP_VALIDITY_NOT_AVAILABLE)
+		return "";
+
+	return phone_number_to_string(number, type);
+}
+
+static const char *time_to_str(const time_t *t)
+{
+	static char buf[128];
+
+	strftime(buf, 127, "%a, %d %b %Y %H:%M:%S %z", localtime(t));
+	buf[127] = '\0';
+
+	return buf;
+}
+
+static DBusMessage *voicecall_get_properties(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	struct voicecall *v = data;
+	struct ofono_call *call = v->call;
+	DBusMessage *reply;
+	DBusMessageIter iter;
+	DBusMessageIter dict;
+	const char *status;
+	const char *callerid;
+	//char timebuf[512];
+	const char *timestr = "";
+
+	reply = dbus_message_new_method_return(msg);
+
+	if (!reply)
+		return NULL;
+
+	status = call_status_to_string(call->status);
+	callerid = phone_number_to_string(call->phone_number,
+						call->number_type);
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+						PROPERTIES_ARRAY_SIGNATURE,
+						&dict);
+
+	dbus_gsm_dict_append(&dict, "State", DBUS_TYPE_STRING, &status);
+
+	dbus_gsm_dict_append(&dict, "LineIdentification",
+				DBUS_TYPE_STRING, &callerid);
+
+	if (call->status == CALL_STATUS_ACTIVE ||
+		(call->status == CALL_STATUS_DISCONNECTED && v->start_time != 0) ||
+		call->status == CALL_STATUS_HELD) {
+		timestr = time_to_str(&v->start_time);
+
+		dbus_gsm_dict_append(&dict, "StartTime", DBUS_TYPE_STRING,
+					&timestr);
+	}
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+static DBusMessage *voicecall_busy(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct voicecall *v = data;
+	struct ofono_modem *modem = v->modem;
+	struct voicecalls_data *voicecalls = modem->voicecalls;
+	struct ofono_call *call = v->call;
+
+	if (call->status != CALL_STATUS_INCOMING &&
+		call->status != CALL_STATUS_WAITING)
+		return dbus_gsm_failed(msg);
+
+	if (!voicecalls->ops->release_specific)
+		return dbus_gsm_not_implemented(msg);
+
+	if (voicecalls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	voicecalls->flags |= VOICECALLS_FLAG_PENDING;
+	voicecalls->pending = dbus_message_ref(msg);
+
+	voicecalls->ops->set_udub(modem, generic_callback, voicecalls);
+
+	return NULL;
+}
+
+static DBusMessage *voicecall_deflect(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct voicecall *v = data;
+	struct ofono_modem *modem = v->modem;
+	struct voicecalls_data *voicecalls = modem->voicecalls;
+	struct ofono_call *call = v->call;
+
+	const char *number;
+	int number_type;
+
+	if (call->status != CALL_STATUS_INCOMING &&
+		call->status != CALL_STATUS_WAITING)
+		return dbus_gsm_failed(msg);
+
+	if (!voicecalls->ops->deflect)
+		return dbus_gsm_not_implemented(msg);
+
+	if (voicecalls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_STRING, &number,
+					DBUS_TYPE_INVALID) == FALSE)
+		return dbus_gsm_invalid_args(msg);
+
+	if (!valid_phone_number_format(number))
+		return dbus_gsm_invalid_format(msg);
+
+	voicecalls->flags |= VOICECALLS_FLAG_PENDING;
+	voicecalls->pending = dbus_message_ref(msg);
+
+	string_to_phone_number(number, &number_type, &number);
+
+	voicecalls->ops->deflect(modem, number, number_type,
+					generic_callback, voicecalls);
+
+	return NULL;
+}
+
+static DBusMessage *voicecall_hangup(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct voicecall *v = data;
+	struct ofono_modem *modem = v->modem;
+	struct voicecalls_data *voicecalls = modem->voicecalls;
+	struct ofono_call *call = v->call;
+
+	if (call->status == CALL_STATUS_DISCONNECTED)
+		return dbus_gsm_failed(msg);
+
+	if (!voicecalls->ops->release_specific)
+		return dbus_gsm_not_implemented(msg);
+
+	if (voicecalls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	voicecalls->flags |= VOICECALLS_FLAG_PENDING;
+	voicecalls->pending = dbus_message_ref(msg);
+
+	voicecalls->ops->release_specific(modem, call->id,
+						generic_callback, voicecalls);
+
+	return NULL;
+}
+
+static DBusMessage *voicecall_answer(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct voicecall *v = data;
+	struct ofono_modem *modem = v->modem;
+	struct voicecalls_data *voicecalls = modem->voicecalls;
+	struct ofono_call *call = v->call;
+
+	if (call->status != CALL_STATUS_INCOMING)
+		return dbus_gsm_failed(msg);
+
+	if (!voicecalls->ops->answer)
+		return dbus_gsm_not_implemented(msg);
+
+	if (voicecalls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	voicecalls->flags |= VOICECALLS_FLAG_PENDING;
+	voicecalls->pending = dbus_message_ref(msg);
+
+	voicecalls->ops->answer(modem, generic_callback, voicecalls);
+
+	return NULL;
+}
+
+static GDBusMethodTable voicecall_methods[] = {
+	{ "GetProperties",	"",	"a{sv}",	voicecall_get_properties },
+	{ "Busy",		"",	"",		voicecall_busy,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "Deflect",		"s",	"",		voicecall_deflect,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "Hangup",		"",	"",		voicecall_hangup,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "Answer",		"",	"",		voicecall_answer,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ }
+};
+
+static GDBusSignalTable voicecall_signals[] = {
+	{ "PropertyChanged",	"sv" },
+	{ "DisconnectReason",	"s" },
+	{ }
+};
+
+static struct voicecall *voicecall_create(struct ofono_modem *modem,
+						struct ofono_call *call)
+{
+	struct voicecall *v;
+
+	v = g_try_new0(struct voicecall, 1);
+
+	if (!v)
+		return NULL;
+
+	v->call = call;
+	v->modem = modem;
+
+	return v;
+}
+
+static void voicecall_destroy(gpointer userdata)
+{
+	struct voicecall *voicecall = (struct voicecall *)userdata;
+
+	g_free(voicecall->call);
+
+	g_free(voicecall);
+}
+
+static const char *voicecall_build_path(struct ofono_modem *modem,
+					const struct ofono_call *call)
+{
+	static char path[MAX_DBUS_PATH_LEN];
+
+	snprintf(path, MAX_DBUS_PATH_LEN, "%s/voicecall%02d",
+			modem->path, call->id);
+
+	return path;
+}
+
+static void voicecall_set_call_status(struct ofono_modem *modem,
+					struct voicecall *call,
+					int status)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+	const char *path;
+	const char *status_str;
+	int old_status;
+
+	if (call->call->status == status)
+		return;
+
+	old_status = call->call->status;
+
+	call->call->status = status;
+
+	status_str = call_status_to_string(status);
+	path = voicecall_build_path(modem, call->call);
+
+	dbus_gsm_signal_property_changed(conn, path, VOICECALL_INTERFACE,
+						"State", DBUS_TYPE_STRING,
+						&status_str);
+
+	if (status == CALL_STATUS_ACTIVE &&
+		(old_status == CALL_STATUS_INCOMING ||
+			old_status == CALL_STATUS_DIALING ||
+			old_status == CALL_STATUS_ALERTING ||
+			old_status == CALL_STATUS_WAITING)) {
+		const char *timestr;
+
+		call->start_time = time(NULL);
+		timestr = time_to_str(&call->start_time);
+
+		dbus_gsm_signal_property_changed(conn, path,
+							VOICECALL_INTERFACE,
+							"StartTime",
+							DBUS_TYPE_STRING,
+							&timestr);
+	}
+}
+
+static void voicecall_set_call_lineid(struct ofono_modem *modem,
+					struct voicecall *v,
+					const char *number, int number_type,
+					int clip_validity)
+{
+	struct ofono_call *call = v->call;
+	DBusConnection *conn = dbus_gsm_connection();
+	const char *path;
+	const char *lineid_str;
+
+	if (!strcmp(call->phone_number, number) &&
+		call->number_type == number_type &&
+		call->clip_validity == clip_validity)
+		return;
+
+	/* Two cases: We get an incoming call with CLIP factored in, or
+	 * CLIP comes in later as a separate event
+	 * For COLP only the phone number should be checked, it can come
+	 * in with the initial call event or later as a separate event */
+
+	/* For plugins that don't keep state, ignore */
+	if (call->clip_validity == CLIP_VALIDITY_VALID &&
+		clip_validity == CLIP_VALIDITY_NOT_AVAILABLE)
+		return;
+
+	strcpy(call->phone_number, number);
+	call->clip_validity = clip_validity;
+	call->number_type = number_type;
+
+	path = voicecall_build_path(modem, call);
+
+	if (call->direction == CALL_DIRECTION_MOBILE_TERMINATED)
+		lineid_str = phone_and_clip_to_string(number, number_type,
+							clip_validity);
+	else
+		lineid_str = phone_number_to_string(number, number_type);
+
+	dbus_gsm_signal_property_changed(conn, path, VOICECALL_INTERFACE,
+						"LineIdentification",
+						DBUS_TYPE_STRING, &lineid_str);
+}
+
+static gboolean voicecall_dbus_register(struct voicecall *voicecall)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+	const char *path;
+
+	if (!voicecall)
+		return FALSE;
+
+	path = voicecall_build_path(voicecall->modem, voicecall->call);
+
+	if (!g_dbus_register_interface(conn, path, VOICECALL_INTERFACE,
+					voicecall_methods,
+					voicecall_signals,
+					NULL, voicecall,
+					voicecall_destroy)) {
+		ofono_error("Could not register VoiceCall %s", path);
+		voicecall_destroy(voicecall);
+
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+static gboolean voicecall_dbus_unregister(struct ofono_modem *modem,
+						struct voicecall *call)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+	const char *path = voicecall_build_path(modem, call->call);
+
+	return g_dbus_unregister_interface(conn, path,
+					VOICECALL_INTERFACE);
+}
+
+static struct voicecalls_data *voicecalls_create()
+{
+	struct voicecalls_data *calls;
+
+	calls = g_try_new0(struct voicecalls_data, 1);
+
+	return calls;
+}
+
+static void voicecalls_destroy(gpointer userdata)
+{
+	struct ofono_modem *modem = userdata;
+	struct voicecalls_data *calls = modem->voicecalls;
+	GSList *l;
+
+	for (l = calls->call_list; l; l = l->next)
+		voicecall_dbus_unregister(modem, l->data);
+
+	g_slist_free(calls->call_list);
+
+	g_free(calls);
+
+	modem->voicecalls = 0;
+}
+
+static int voicecalls_path_list(struct ofono_modem *modem, GSList *call_list,
+				char ***objlist)
+{
+	GSList *l;
+	int i;
+	struct voicecall *v;
+
+	*objlist = g_new0(char *, g_slist_length(call_list) + 1);
+
+	if (*objlist == NULL)
+		return -1;
+
+	for (i = 0, l = call_list; l; l = l->next, i++) {
+		v = l->data;
+		(*objlist)[i] = g_strdup(voicecall_build_path(modem, v->call));
+	}
+
+	return 0;
+}
+
+static gboolean voicecalls_have_active(struct voicecalls_data *calls)
+{
+	GSList *l;
+	struct voicecall *v;
+
+	for (l = calls->call_list; l; l = l->next) {
+		v = l->data;
+
+		if (v->call->status == CALL_STATUS_ACTIVE ||
+			v->call->status == CALL_STATUS_INCOMING ||
+			v->call->status == CALL_STATUS_DIALING ||
+			v->call->status == CALL_STATUS_ALERTING)
+			return TRUE;
+	}
+
+	return FALSE;
+}
+
+static gboolean voicecalls_have_connected(struct voicecalls_data *calls)
+{
+	GSList *l;
+	struct voicecall *v;
+
+	for (l = calls->call_list; l; l = l->next) {
+		v = l->data;
+
+		if (v->call->status == CALL_STATUS_ACTIVE)
+			return TRUE;
+	}
+
+	return FALSE;
+}
+
+static gboolean voicecalls_have_held(struct voicecalls_data *calls)
+{
+	GSList *l;
+	struct voicecall *v;
+
+	for (l = calls->call_list; l; l = l->next) {
+		v = l->data;
+
+		if (v->call->status == CALL_STATUS_HELD)
+			return TRUE;
+	}
+
+	return FALSE;
+}
+
+static int voicecalls_num_with_status(struct voicecalls_data *calls,
+					int status)
+{
+	GSList *l;
+	struct voicecall *v;
+	int num = 0;
+
+	for (l = calls->call_list; l; l = l->next) {
+		v = l->data;
+
+		if (v->call->status == status)
+			num += 1;
+	}
+
+	return num;
+}
+
+static int voicecalls_num_active(struct voicecalls_data *calls)
+{
+	return voicecalls_num_with_status(calls, CALL_STATUS_ACTIVE);
+}
+
+static int voicecalls_num_held(struct voicecalls_data *calls)
+{
+	return voicecalls_num_with_status(calls, CALL_STATUS_HELD);
+}
+
+static int voicecalls_num_connecting(struct voicecalls_data *calls)
+{
+	int r = 0;
+
+	r += voicecalls_num_with_status(calls, CALL_STATUS_DIALING);
+	r += voicecalls_num_with_status(calls, CALL_STATUS_ALERTING);
+
+	return r;
+}
+
+static GSList *voicecalls_held_list(struct voicecalls_data *calls)
+{
+	GSList *l;
+	GSList *r = NULL;
+	struct voicecall *v;
+
+	for (l = calls->call_list; l; l = l->next) {
+		v = l->data;
+
+		if (v->call->status == CALL_STATUS_HELD)
+			r = g_slist_prepend(r, v);
+	}
+
+	if (r)
+		r = g_slist_reverse(r);
+
+	return r;
+}
+
+/* Intended to be used for multiparty, which cannot be incoming,
+ * alerting or dialing */
+static GSList *voicecalls_active_list(struct voicecalls_data *calls)
+{
+	GSList *l;
+	GSList *r = NULL;
+	struct voicecall *v;
+
+	for (l = calls->call_list; l; l = l->next) {
+		v = l->data;
+
+		if (v->call->status == CALL_STATUS_ACTIVE)
+			r = g_slist_prepend(r, v);
+	}
+
+	if (r)
+		r = g_slist_reverse(r);
+
+	return r;
+}
+
+static gboolean voicecalls_have_waiting(struct voicecalls_data *calls)
+{
+	GSList *l;
+	struct voicecall *v;
+
+	for (l = calls->call_list; l; l = l->next) {
+		v = l->data;
+
+		if (v->call->status == CALL_STATUS_WAITING)
+			return TRUE;
+	}
+
+	return FALSE;
+}
+
+static void voicecalls_release_queue(struct ofono_modem *modem, GSList *calls)
+{
+	struct voicecalls_data *voicecalls = modem->voicecalls;
+	GSList *l;
+
+	g_slist_free(voicecalls->release_list);
+	voicecalls->release_list = NULL;
+
+	for (l = calls; l; l = l->next) {
+		voicecalls->release_list =
+			g_slist_prepend(voicecalls->release_list, l->data);
+	}
+}
+
+static void voicecalls_release_next(struct ofono_modem *modem)
+{
+	struct voicecalls_data *voicecalls = modem->voicecalls;
+	struct voicecall *call;
+
+	if (!voicecalls->release_list)
+		return;
+
+	call = voicecalls->release_list->data;
+
+	voicecalls->release_list = g_slist_remove(voicecalls->release_list,
+							call);
+
+	voicecalls->ops->release_specific(modem, call->call->id,
+						multirelease_callback, modem);
+}
+
+static DBusMessage *manager_get_properties(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+	DBusMessage *reply;
+	DBusMessageIter iter;
+	DBusMessageIter dict;
+
+	char **callobj_list;
+
+	reply = dbus_message_new_method_return(msg);
+
+	if (!reply)
+		return NULL;
+
+	dbus_message_iter_init_append(reply, &iter);
+
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+					PROPERTIES_ARRAY_SIGNATURE, &dict);
+
+	voicecalls_path_list(modem, calls->call_list, &callobj_list);
+
+	dbus_gsm_dict_append_array(&dict, "Calls", DBUS_TYPE_OBJECT_PATH,
+				&callobj_list);
+
+	dbus_gsm_free_string_array(callobj_list);
+
+	voicecalls_path_list(modem, calls->multiparty_list, &callobj_list);
+
+	dbus_gsm_dict_append_array(&dict, "MultipartyCalls",
+					DBUS_TYPE_OBJECT_PATH, &callobj_list);
+
+	dbus_gsm_free_string_array(callobj_list);
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+static DBusMessage *manager_dial(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+	const char *number;
+	int number_type;
+	const char *clirstr;
+	enum ofono_clir_option clir;
+
+	if (calls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (g_slist_length(calls->call_list) >= MAX_VOICE_CALLS)
+		return dbus_gsm_failed(msg);
+
+	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_STRING, &number,
+					DBUS_TYPE_STRING, &clirstr,
+					DBUS_TYPE_INVALID) == FALSE)
+		return dbus_gsm_invalid_args(msg);
+
+	if (!valid_phone_number_format(number))
+		return dbus_gsm_invalid_format(msg);
+
+	if (strlen(clirstr) == 0 || !strcmp(clirstr, "default"))
+		clir = OFONO_CLIR_OPTION_DEFAULT;
+	else if (!strcmp(clirstr, "disabled"))
+		clir = OFONO_CLIR_OPTION_SUPPRESSION;
+	else if (!strcmp(clirstr, "enabled"))
+		clir = OFONO_CLIR_OPTION_INVOCATION;
+	else
+		return dbus_gsm_invalid_format(msg);
+
+	if (!calls->ops->dial)
+		return dbus_gsm_not_implemented(msg);
+
+	if (voicecalls_have_active(calls) &&
+		voicecalls_have_held(calls))
+		return dbus_gsm_failed(msg);
+
+	calls->flags |= VOICECALLS_FLAG_PENDING;
+	calls->pending = dbus_message_ref(msg);
+
+	string_to_phone_number(number, &number_type, &number);
+
+	calls->ops->dial(modem, number, number_type, clir,
+				OFONO_CUG_OPTION_DEFAULT,
+				dial_callback, modem);
+
+	return NULL;
+}
+
+static DBusMessage *manager_transfer(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+	int numactive;
+	int numheld;
+
+	if (calls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	numactive = voicecalls_num_active(calls);
+
+	/* According to 22.091 section 5.8, the network has the option of
+	 * implementing the call transfer operation for a call that is
+	 * still dialing/alerting.
+	 */
+	numactive += voicecalls_num_connecting(calls);
+
+	numheld = voicecalls_num_held(calls);
+
+	if ((numactive != 1) && (numheld != 1))
+		return dbus_gsm_failed(msg);
+
+	if (!calls->ops->transfer)
+		return dbus_gsm_not_implemented(msg);
+
+	calls->flags |= VOICECALLS_FLAG_PENDING;
+	calls->pending = dbus_message_ref(msg);
+
+	calls->ops->transfer(modem, generic_callback, calls);
+
+	return NULL;
+}
+
+static DBusMessage *manager_swap_calls(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+
+	if (calls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (voicecalls_have_waiting(calls))
+		return dbus_gsm_failed(msg);
+
+	if (!calls->ops->hold_all_active)
+		return dbus_gsm_not_implemented(msg);
+
+	calls->flags |= VOICECALLS_FLAG_PENDING;
+	calls->pending = dbus_message_ref(msg);
+
+	calls->ops->hold_all_active(modem, generic_callback, calls);
+
+	return NULL;
+}
+
+static DBusMessage *manager_release_and_answer(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+
+	if (calls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (!voicecalls_have_active(calls) || !voicecalls_have_waiting(calls))
+		return dbus_gsm_failed(msg);
+
+	if (!calls->ops->release_all_active)
+		return dbus_gsm_not_implemented(msg);
+
+	calls->flags |= VOICECALLS_FLAG_PENDING;
+	calls->pending = dbus_message_ref(msg);
+
+	calls->ops->release_all_active(modem, generic_callback, calls);
+
+	return NULL;
+}
+
+static DBusMessage *manager_hold_and_answer(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+
+	if (calls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (voicecalls_have_active(calls) && voicecalls_have_held(calls) &&
+		voicecalls_have_waiting(calls))
+		return dbus_gsm_failed(msg);
+
+	if (!calls->ops->hold_all_active)
+		return dbus_gsm_not_implemented(msg);
+
+	calls->flags |= VOICECALLS_FLAG_PENDING;
+	calls->pending = dbus_message_ref(msg);
+
+	calls->ops->hold_all_active(modem, generic_callback, calls);
+
+	return NULL;
+}
+
+static DBusMessage *manager_hangup_all(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+
+	if (calls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (!calls->ops->release_specific)
+		return dbus_gsm_not_implemented(msg);
+
+	if (g_slist_length(calls->call_list) == 0) {
+		DBusMessage *reply = dbus_message_new_method_return(msg);
+		return reply;
+	}
+
+	calls->flags |= VOICECALLS_FLAG_PENDING;
+	calls->flags |= VOICECALLS_FLAG_MULTI_RELEASE;
+
+	calls->pending = dbus_message_ref(msg);
+
+	voicecalls_release_queue(modem, calls->call_list);
+	voicecalls_release_next(modem);
+
+	return NULL;
+}
+
+static DBusMessage *multiparty_private_chat(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+	const char *callpath;
+	const char *c;
+	unsigned int id;
+	GSList *l;
+
+	if (calls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &callpath,
+					DBUS_TYPE_INVALID) == FALSE)
+		return dbus_gsm_invalid_args(msg);
+
+	if (strlen(callpath) == 0 || strlen(callpath) > MAX_DBUS_PATH_LEN)
+		return dbus_gsm_invalid_format(msg);
+
+	c = strrchr(callpath, '/');
+
+	if (!c || strncmp(modem->path, callpath, c-callpath))
+		return dbus_gsm_not_found(msg);
+
+	if (!sscanf(c, "/voicecall%2u", &id))
+		return dbus_gsm_not_found(msg);
+
+	for (l = calls->multiparty_list; l; l = l->next) {
+		struct voicecall *v = l->data;
+		if (v->call->id == id)
+			break;
+	}
+
+	if (!l)
+		return dbus_gsm_not_found(msg);
+
+	/* If we found id on the list of multiparty calls, then by definition
+	 * the multiparty call exists.	Only thing to check is whether we have
+	 * held calls
+	 */
+	if (voicecalls_have_held(calls))
+		return dbus_gsm_failed(msg);
+
+	if (!calls->ops->private_chat)
+		return dbus_gsm_not_implemented(msg);
+
+	calls->flags |= VOICECALLS_FLAG_PENDING;
+	calls->pending = dbus_message_ref(msg);
+
+	calls->ops->private_chat(modem, id, private_chat_callback, modem);
+
+	return NULL;
+}
+
+static DBusMessage *multiparty_create(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+
+	if (calls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (!voicecalls_have_held(calls) || !voicecalls_have_active(calls))
+		return dbus_gsm_failed(msg);
+
+	if (!calls->ops->create_multiparty)
+		return dbus_gsm_not_implemented(msg);
+
+	calls->flags |= VOICECALLS_FLAG_PENDING;
+	calls->pending = dbus_message_ref(msg);
+
+	calls->ops->create_multiparty(modem, multiparty_create_callback, modem);
+
+	return NULL;
+}
+
+static DBusMessage *multiparty_hangup(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+
+	if (calls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (!calls->ops->release_specific)
+		return dbus_gsm_not_implemented(msg);
+
+	if (g_slist_length(calls->multiparty_list) == 0) {
+		DBusMessage *reply = dbus_message_new_method_return(msg);
+		return reply;
+	}
+
+	calls->flags |= VOICECALLS_FLAG_PENDING;
+	calls->pending = dbus_message_ref(msg);
+
+	/* We have waiting calls, can't use +CHLD to release */
+	if (voicecalls_have_waiting(calls)) {
+		calls->flags |= VOICECALLS_FLAG_MULTI_RELEASE;
+		voicecalls_release_queue(modem, calls->multiparty_list);
+		voicecalls_release_next(modem);
+	} else {
+		struct ofono_call *v = calls->multiparty_list->data;
+
+		if (v->status == CALL_STATUS_HELD)
+			calls->ops->release_all_held(modem, generic_callback,
+							calls);
+		else
+			calls->ops->release_all_active(modem, generic_callback,
+							calls);
+	}
+
+	return NULL;
+}
+
+static DBusMessage *manager_tone(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+	const char *in_tones;
+	char *tones;
+	int i, len;
+
+	if (calls->flags & VOICECALLS_FLAG_PENDING)
+		return dbus_gsm_busy(msg);
+
+	if (!calls->ops->send_tones)
+		return dbus_gsm_not_implemented(msg);
+
+	/* Send DTMFs only if we have at least one connected call */
+	if (!voicecalls_have_connected(calls))
+		return dbus_gsm_failed(msg);
+
+	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_STRING, &in_tones,
+					DBUS_TYPE_INVALID) == FALSE)
+		return dbus_gsm_invalid_args(msg);
+
+	len = strlen(in_tones);
+
+	if (len == 0)
+		return dbus_gsm_invalid_format(msg);
+
+	tones = g_ascii_strup(in_tones, len);
+
+	/* Tones can be 0-9, *, #, A-D according to 27.007 C.2.11 */
+	for (i = 0; i < len; i++) {
+		if (g_ascii_isdigit(tones[i]) ||
+			tones[i] == '*' || tones[i] == '#' ||
+				(tones[i] >= 'A' && tones[i] <= 'D'))
+			continue;
+
+		g_free(tones);
+		return dbus_gsm_invalid_format(msg);
+	}
+
+	calls->flags |= VOICECALLS_FLAG_PENDING;
+	calls->pending = dbus_message_ref(msg);
+
+	calls->ops->send_tones(modem, tones, generic_callback, calls);
+
+	g_free(tones);
+
+	return NULL;
+}
+
+static GDBusMethodTable manager_methods[] = {
+	{ "GetProperties",	"",	"a{sv}",	manager_get_properties },
+	{ "Dial",		"ss",	"o",		manager_dial,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "Transfer",		"",	"",		manager_transfer,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "SwapCalls",		"",	"",		manager_swap_calls,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "ReleaseAndAnswer",	"",	"",		manager_release_and_answer,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "HoldAndAnswer",	"",	"",		manager_hold_and_answer,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "HangupAll",		"",	"",		manager_hangup_all,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "PrivateChat",	"o",	"ao",		multiparty_private_chat,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "CreateMultiparty",	"",	"ao",		multiparty_create,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "HangupMultiparty",	"",	"",		multiparty_hangup,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "SendTones",		"s",	"",		manager_tone,
+							G_DBUS_METHOD_FLAG_ASYNC },
+	{ }
+};
+
+static GDBusSignalTable manager_signals[] = {
+	{ "PropertyChanged",	"sv" },
+	{ }
+};
+
+static gboolean real_emit_call_list_changed(void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *voicecalls = modem->voicecalls;
+	DBusConnection *conn = dbus_gsm_connection();
+	char **objpath_list;
+
+	voicecalls_path_list(modem, voicecalls->call_list, &objpath_list);
+
+	dbus_gsm_signal_array_property_changed(conn, modem->path,
+				VOICECALL_MANAGER_INTERFACE,
+				"Calls",
+				DBUS_TYPE_OBJECT_PATH,
+				&objpath_list);
+
+	dbus_gsm_free_string_array(objpath_list);
+
+	ofono_debug("Resetting updating flag");
+	voicecalls->flags &= ~VOICECALLS_FLAG_UPDATING_CALL_LIST;
+
+	return FALSE;
+}
+
+static void emit_call_list_changed(struct ofono_modem *modem)
+{
+	//struct voicecalls_data *calls = modem->voicecalls;
+
+#ifdef DELAY_EMIT
+	if (!(calls->flags & VOICECALLS_FLAG_UPDATING_CALL_LIST)) {
+		calls->flags |= VOICECALLS_FLAG_UPDATING_CALL_LIST;
+		g_timeout_add(0, real_emit_call_list_changed, modem);
+	}
+#else
+	real_emit_call_list_changed(modem);
+#endif
+}
+
+static gboolean real_emit_multiparty_call_list_changed(void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *voicecalls = modem->voicecalls;
+	DBusConnection *conn = dbus_gsm_connection();
+	char **objpath_list;
+
+	voicecalls_path_list(modem, voicecalls->multiparty_list, &objpath_list);
+
+	dbus_gsm_signal_array_property_changed(conn, modem->path,
+				VOICECALL_MANAGER_INTERFACE, "MultipartyCalls",
+				DBUS_TYPE_OBJECT_PATH,
+				&objpath_list);
+
+	dbus_gsm_free_string_array(objpath_list);
+
+	voicecalls->flags &= ~VOICECALLS_FLAG_UPDATING_MPTY_CALL_LIST;
+
+	return FALSE;
+}
+
+static void emit_multiparty_call_list_changed(struct ofono_modem *modem)
+{
+	//struct voicecalls_data *calls = modem->voicecalls;
+
+#ifdef DELAY_EMIT
+	if (!(calls->flags & VOICECALLS_FLAG_UPDATING_MPTY_CALL_LIST)) {
+		calls->flags |= VOICECALLS_FLAG_UPDATING_MPTY_CALL_LIST;
+		g_timeout_add(0, real_emit_multiparty_call_list_changed, modem);
+	}
+#else
+	real_emit_multiparty_call_list_changed(modem);
+#endif
+}
+
+void ofono_voicecall_disconnected(struct ofono_modem *modem, int id,
+				enum ofono_disconnect_reason reason,
+				const struct ofono_error *error)
+{
+	GSList *l;
+	struct voicecalls_data *calls = modem->voicecalls;
+	struct voicecall *call;
+
+	ofono_debug("Got disconnection event for id: %d, reason: %d", id, reason);
+
+	l = g_slist_find_custom(calls->call_list, GINT_TO_POINTER(id),
+				call_compare_by_id);
+
+	if (!l) {
+		ofono_error("Plugin notified us of call disconnect for"
+				" unknown call");
+		return;
+	}
+
+	call = l->data;
+
+	l = g_slist_find_custom(calls->multiparty_list, GINT_TO_POINTER(id),
+				call_compare_by_id);
+
+	if (l) {
+		calls->multiparty_list =
+			g_slist_remove(calls->multiparty_list, call);
+
+		if (calls->multiparty_list->next == NULL) { /* Size == 1 */
+			g_slist_free(calls->multiparty_list);
+			calls->multiparty_list = 0;
+		}
+
+		emit_multiparty_call_list_changed(modem);
+	}
+
+	calls->release_list = g_slist_remove(calls->release_list, call);
+
+	modem_release_callid(modem, id);
+
+	/* TODO: Emit disconnect reason */
+	voicecall_set_call_status(modem, call, CALL_STATUS_DISCONNECTED);
+
+	voicecall_dbus_unregister(modem, call);
+
+	calls->call_list = g_slist_remove(calls->call_list, call);
+
+	emit_call_list_changed(modem);
+}
+
+void ofono_voicecall_notify(struct ofono_modem *modem, const struct ofono_call *call)
+{
+	GSList *l;
+	struct voicecalls_data *calls = modem->voicecalls;
+	struct voicecall *v;
+	struct ofono_call *newcall = NULL;
+	//const char *member;
+
+	ofono_debug("Got a voicecall event, status: %d, id: %u, number: %s",
+			call->status, call->id, call->phone_number);
+
+	l = g_slist_find_custom(calls->call_list, GINT_TO_POINTER(call->id),
+				call_compare_by_id);
+
+	if (l) {
+		ofono_debug("Found call with id: %d\n", call->id);
+		voicecall_set_call_status(modem, l->data, call->status);
+		voicecall_set_call_lineid(modem, l->data, call->phone_number,
+					call->number_type, call->clip_validity);
+
+		return;
+	}
+
+	ofono_debug("Did not find a call with id: %d\n", call->id);
+
+	newcall = g_try_new0(struct ofono_call, 1);
+
+	if (!call) {
+		ofono_error("Unable to allocate call");
+		goto err;
+	}
+
+	memcpy(newcall, call, sizeof(struct ofono_call));
+
+	if (modem_alloc_callid(modem) != call->id) {
+		ofono_error("Warning: Call id and internally tracked id"
+				" do not correspond");
+		goto err;
+	}
+
+	v = voicecall_create(modem, newcall);
+
+	if (!v) {
+		ofono_error("Unable to allocate voicecall_data");
+		goto err;
+	}
+
+	if (!voicecall_dbus_register(v)) {
+		ofono_error("Unable to register voice call");
+		goto err;
+	}
+
+	calls->call_list = g_slist_insert_sorted(calls->call_list, v,
+							call_compare);
+
+	emit_call_list_changed(modem);
+
+	return;
+
+err:
+	if (newcall)
+		g_free(newcall);
+
+	if (v)
+		g_free(v);
+}
+
+void ofono_voicecall_cssi(struct ofono_modem *modem, int code, int index)
+{
+
+}
+
+void ofono_voicecall_cssu(struct ofono_modem *modem, int code, int index,
+			const char *number, int number_type)
+{
+
+}
+
+static void generic_callback(const struct ofono_error *error, void *data)
+{
+	struct voicecalls_data *calls = data;
+	DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *reply;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR)
+		ofono_debug("command failed with error: %s",
+				telephony_error_to_str(error));
+
+	calls->flags &= ~VOICECALLS_FLAG_PENDING;
+
+	if (!calls->pending)
+		return;
+
+	if (error->type == OFONO_ERROR_TYPE_NO_ERROR)
+		reply = dbus_message_new_method_return(calls->pending);
+	else
+		reply = dbus_gsm_failed(calls->pending);
+
+	g_dbus_send_message(conn, reply);
+
+	dbus_message_unref(calls->pending);
+	calls->pending = NULL;
+}
+
+static void multirelease_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+	DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *reply;
+
+	if (g_slist_length(calls->release_list)) {
+		voicecalls_release_next(modem);
+		return;
+	}
+
+	calls->flags &= ~VOICECALLS_FLAG_MULTI_RELEASE;
+	calls->flags &= ~VOICECALLS_FLAG_PENDING;
+
+	if (!calls->pending)
+		return;
+
+	reply = dbus_message_new_method_return(calls->pending);
+
+	g_dbus_send_message(conn, reply);
+
+	dbus_message_unref(calls->pending);
+	calls->pending = NULL;
+}
+
+static struct ofono_call *synthesize_outgoing_call(struct ofono_modem *modem,
+						DBusMessage *msg)
+{
+	const char *number;
+	int number_type;
+	struct ofono_call *call;
+
+	call = g_try_new0(struct ofono_call, 1);
+
+	if (!call)
+		return call;
+
+	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_STRING, &number,
+				DBUS_TYPE_INVALID) == FALSE)
+		number = "";
+	else
+		string_to_phone_number(number, &number_type, &number);
+
+	call->id = modem_alloc_callid(modem);
+
+	if (call->id == 0) {
+		ofono_error("Failed to alloc callid, too many calls");
+		g_free(call);
+		return NULL;
+	}
+
+	call->direction = CALL_DIRECTION_MOBILE_ORIGINATED;
+	call->status = CALL_STATUS_DIALING;
+	strcpy(call->phone_number, number);
+	call->number_type = number_type;
+	call->clip_validity = CLIP_VALIDITY_VALID;
+
+	return call;
+}
+
+static void dial_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+	DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *reply;
+	GSList *l;
+	struct ofono_call *call;
+	const char *path;
+	gboolean need_to_emit = FALSE;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR)
+		ofono_debug("Dial callback returned error: %s",
+			telephony_error_to_str(error));
+
+	calls->flags &= ~VOICECALLS_FLAG_PENDING;
+
+	if (!calls->pending)
+		return;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		reply = dbus_gsm_failed(calls->pending);
+		g_dbus_send_message(conn, reply);
+
+		goto out;
+	}
+
+	reply = dbus_message_new_method_return(calls->pending);
+	if (!reply)
+		goto out;
+
+	/* Two things can happen, the call notification arrived before dial
+	 * callback or dial callback was first.	Handle here */
+	for (l = calls->call_list; l; l = l->next) {
+		struct voicecall *v = l->data;
+
+		if (v->call->status == CALL_STATUS_DIALING ||
+			v->call->status == CALL_STATUS_ALERTING)
+			break;
+	}
+
+	if (!l) {
+		struct voicecall *v;
+		call = synthesize_outgoing_call(modem, calls->pending);
+
+		if (!call) {
+			reply = dbus_gsm_failed(calls->pending);
+			g_dbus_send_message(conn, reply);
+
+			goto out;
+		}
+
+		v = voicecall_create(modem, call);
+
+		if (!v) {
+			reply = dbus_gsm_failed(calls->pending);
+			g_dbus_send_message(conn, reply);
+
+			goto out;
+		}
+
+		ofono_debug("Registering new call: %d", call->id);
+		voicecall_dbus_register(voicecall_create(modem, call));
+
+		calls->call_list = g_slist_insert_sorted(calls->call_list, v,
+					call_compare);
+
+		need_to_emit = TRUE;
+	} else {
+		struct voicecall *v = l->data;
+
+		call = v->call;
+	}
+
+	path = voicecall_build_path(modem, call);
+
+	dbus_message_append_args(reply, DBUS_TYPE_OBJECT_PATH, &path,
+					DBUS_TYPE_INVALID);
+
+	g_dbus_send_message(conn, reply);
+
+	if (need_to_emit)
+		emit_call_list_changed(modem);
+
+out:
+	dbus_message_unref(calls->pending);
+	calls->pending = NULL;
+}
+
+
+static void multiparty_callback_common(struct ofono_modem *modem,
+					DBusMessage *reply)
+{
+	struct voicecalls_data *voicecalls = modem->voicecalls;
+	DBusMessageIter iter;
+	DBusMessageIter array_iter;
+	char **objpath_list;
+	int i;
+
+	voicecalls_path_list(modem, voicecalls->multiparty_list, &objpath_list);
+
+	dbus_message_iter_init_append(reply, &iter);
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+		DBUS_TYPE_OBJECT_PATH_AS_STRING, &array_iter);
+
+	for (i = 0; objpath_list[i]; i++)
+		dbus_message_iter_append_basic(&array_iter,
+			DBUS_TYPE_OBJECT_PATH, &objpath_list[i]);
+
+	dbus_message_iter_close_container(&iter, &array_iter);
+}
+
+static void multiparty_create_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+	DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *reply;
+	gboolean need_to_emit = FALSE;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR)
+		ofono_debug("command failed with error: %s",
+				telephony_error_to_str(error));
+
+	calls->flags &= ~VOICECALLS_FLAG_PENDING;
+
+	if (!calls->pending)
+		return;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		reply = dbus_gsm_failed(calls->pending);
+		goto out;
+	}
+
+	/* We just created a multiparty call, gather all held
+	 * active calls and add them to the multiparty list
+	 */
+	if (calls->multiparty_list) {
+		g_slist_free(calls->multiparty_list);
+		calls->multiparty_list = 0;
+	}
+
+	calls->multiparty_list = g_slist_concat(calls->multiparty_list,
+						voicecalls_held_list(calls));
+
+	calls->multiparty_list = g_slist_concat(calls->multiparty_list,
+						voicecalls_active_list(calls));
+
+	calls->multiparty_list = g_slist_sort(calls->multiparty_list,
+						call_compare);
+
+	if (g_slist_length(calls->multiparty_list) < 2) {
+		ofono_error("Created multiparty call, but size is less than 2"
+				" panic!");
+
+		reply = dbus_gsm_failed(calls->pending);
+	} else {
+		reply = dbus_message_new_method_return(calls->pending);
+
+		multiparty_callback_common(modem, reply);
+		need_to_emit = TRUE;
+	}
+
+out:
+	g_dbus_send_message(conn, reply);
+
+	if (need_to_emit)
+		emit_multiparty_call_list_changed(modem);
+
+	dbus_message_unref(calls->pending);
+	calls->pending = NULL;
+}
+
+static void private_chat_callback(const struct ofono_error *error, void *data)
+{
+	struct ofono_modem *modem = data;
+	struct voicecalls_data *calls = modem->voicecalls;
+	DBusConnection *conn = dbus_gsm_connection();
+	DBusMessage *reply;
+	gboolean need_to_emit = FALSE;
+	const char *callpath;
+	const char *c;
+	int id;
+	GSList *l;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR)
+		ofono_debug("command failed with error: %s",
+				telephony_error_to_str(error));
+
+	calls->flags &= ~VOICECALLS_FLAG_PENDING;
+
+	if (!calls->pending)
+		return;
+
+	if (error->type != OFONO_ERROR_TYPE_NO_ERROR) {
+		reply = dbus_gsm_failed(calls->pending);
+		goto out;
+	}
+
+	dbus_message_get_args(calls->pending, NULL,
+				DBUS_TYPE_OBJECT_PATH, &callpath,
+				DBUS_TYPE_INVALID);
+
+	c = strrchr(callpath, '/');
+	sscanf(c, "/voicecall%2u", &id);
+
+	l = g_slist_find_custom(calls->multiparty_list, GINT_TO_POINTER(id),
+				call_compare_by_id);
+
+	if (l) {
+		calls->multiparty_list =
+			g_slist_remove(calls->multiparty_list, l->data);
+
+		if (g_slist_length(calls->multiparty_list) < 2) {
+			g_slist_free(calls->multiparty_list);
+			calls->multiparty_list = 0;
+		}
+	}
+
+	reply = dbus_message_new_method_return(calls->pending);
+
+	multiparty_callback_common(modem, reply);
+	need_to_emit = TRUE;
+
+out:
+	g_dbus_send_message(conn, reply);
+
+	if (need_to_emit)
+		emit_multiparty_call_list_changed(modem);
+
+	dbus_message_unref(calls->pending);
+	calls->pending = NULL;
+}
+
+int ofono_voicecall_register(struct ofono_modem *modem, struct ofono_voicecall_ops *ops)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (modem == NULL)
+		return -1;
+
+	if (ops == NULL)
+		return -1;
+
+	modem->voicecalls = voicecalls_create();
+
+	if (modem->voicecalls == NULL)
+		return -1;
+
+	modem->voicecalls->ops = ops;
+
+	if (!g_dbus_register_interface(conn, modem->path,
+					VOICECALL_MANAGER_INTERFACE,
+					manager_methods, manager_signals, NULL,
+					modem, voicecalls_destroy)) {
+		ofono_error("Could not create %s interface",
+				VOICECALL_MANAGER_INTERFACE);
+
+		voicecalls_destroy(modem->voicecalls);
+
+		return -1;
+	}
+
+	modem_add_interface(modem, VOICECALL_MANAGER_INTERFACE);
+
+	return 0;
+}
+
+void ofono_voicecall_unregister(struct ofono_modem *modem)
+{
+	DBusConnection *conn = dbus_gsm_connection();
+
+	if (!modem->voicecalls)
+		return;
+
+	modem_remove_interface(modem, VOICECALL_MANAGER_INTERFACE);
+	g_dbus_unregister_interface(conn, modem->path,
+					VOICECALL_MANAGER_INTERFACE);
+
+	modem->voicecalls = NULL;
+}