sap: move it to profiles folder

8 files changed, 2961 insertions, 0 deletions

diff --git a/profiles/sap/main.c b/profiles/sap/main.c
new file mode 100644
index 000000000..c9c90bd23
--- /dev/null
+++ b/profiles/sap/main.c
@@ -0,0 +1,55 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 Instituto Nokia de Tecnologia - INdT
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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 <errno.h>
+#include <gdbus.h>
+#include "plugin.h"
+#include "manager.h"
+
+static DBusConnection *connection;
+
+static int sap_init(void)
+{
+	connection = dbus_bus_get(DBUS_BUS_SYSTEM, NULL);
+
+	if (!connection)
+		return -EIO;
+
+	if (sap_manager_init(connection) < 0) {
+		dbus_connection_unref(connection);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static void sap_exit(void)
+{
+	sap_manager_exit();
+
+	dbus_connection_unref(connection);
+}
+
+BLUETOOTH_PLUGIN_DEFINE(sap, VERSION,
+		BLUETOOTH_PLUGIN_PRIORITY_DEFAULT, sap_init, sap_exit)
diff --git a/profiles/sap/manager.c b/profiles/sap/manager.c
new file mode 100644
index 000000000..9fa9c56e5
--- /dev/null
+++ b/profiles/sap/manager.c
@@ -0,0 +1,83 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 Instituto Nokia de Tecnologia - INdT
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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 "log.h"
+#include "adapter.h"
+
+#include "manager.h"
+#include "server.h"
+
+static DBusConnection *connection = NULL;
+
+static int sap_server_probe(struct btd_adapter *adapter)
+{
+	const char *path = adapter_get_path(adapter);
+	bdaddr_t src;
+
+	DBG("path %s", path);
+
+	adapter_get_address(adapter, &src);
+
+	return sap_server_register(path, &src);
+}
+
+static void sap_server_remove(struct btd_adapter *adapter)
+{
+	const char *path = adapter_get_path(adapter);
+
+	DBG("path %s", path);
+
+	sap_server_unregister(path);
+}
+
+static struct btd_adapter_driver sap_server_driver = {
+	.name	= "sap-server",
+	.probe	= sap_server_probe,
+	.remove	= sap_server_remove,
+};
+
+int sap_manager_init(DBusConnection *conn)
+{
+	connection = dbus_connection_ref(conn);
+
+	if (sap_server_init(connection) < 0) {
+		error("Can't init SAP server");
+		dbus_connection_unref(conn);
+		return -1;
+	}
+
+	btd_register_adapter_driver(&sap_server_driver);
+
+	return 0;
+}
+
+void sap_manager_exit(void)
+{
+	btd_unregister_adapter_driver(&sap_server_driver);
+
+	dbus_connection_unref(connection);
+	connection = NULL;
+
+	sap_server_exit();
+}
diff --git a/profiles/sap/manager.h b/profiles/sap/manager.h
new file mode 100644
index 000000000..e08c882c4
--- /dev/null
+++ b/profiles/sap/manager.h
@@ -0,0 +1,22 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 Instituto Nokia de Tecnologia - INdT
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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
+ */
+
+int sap_manager_init(DBusConnection *conn);
+void sap_manager_exit(void);
diff --git a/profiles/sap/sap-dummy.c b/profiles/sap/sap-dummy.c
new file mode 100644
index 000000000..dab5acc02
--- /dev/null
+++ b/profiles/sap/sap-dummy.c
@@ -0,0 +1,390 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 ST-Ericsson SA
+ *  Copyright (C) 2011 Tieto Poland
+ *
+ *  Author: Waldemar Rymarkiewicz <waldemar.rymarkiewicz@tieto.com>
+ *          for ST-Ericsson
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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 <gdbus.h>
+
+#include "log.h"
+#include "sap.h"
+
+#define SAP_DUMMY_IFACE "org.bluez.SimAccessTest"
+#define SAP_DUMMY_PATH "/org/bluez/test"
+
+enum {
+	SIM_DISCONNECTED = 0x00,
+	SIM_CONNECTED	 = 0x01,
+	SIM_POWERED_OFF	 = 0x02,
+	SIM_MISSING	 = 0x03
+};
+
+static DBusConnection *connection = NULL;
+static unsigned int init_cnt = 0;
+
+static int sim_card_conn_status = SIM_DISCONNECTED;
+static void *sap_data = NULL; /* SAP server private data. */
+static gboolean ongoing_call_status = FALSE;
+static int max_msg_size_supported = 512;
+
+void sap_connect_req(void *sap_device, uint16_t maxmsgsize)
+{
+	DBG("status: %d", sim_card_conn_status);
+
+	if (sim_card_conn_status != SIM_DISCONNECTED) {
+		sap_connect_rsp(sap_device, SAP_STATUS_CONNECTION_FAILED);
+		return;
+	}
+
+	if (max_msg_size_supported > maxmsgsize) {
+		sap_connect_rsp(sap_device, SAP_STATUS_MAX_MSG_SIZE_TOO_SMALL);
+		return;
+	}
+
+	if (max_msg_size_supported < maxmsgsize) {
+		sap_connect_rsp(sap_device,
+					SAP_STATUS_MAX_MSG_SIZE_NOT_SUPPORTED);
+		return;
+	}
+
+	if (ongoing_call_status) {
+		sap_connect_rsp(sap_device, SAP_STATUS_OK_ONGOING_CALL);
+		return;
+	}
+
+	sim_card_conn_status = SIM_CONNECTED;
+	sap_data = sap_device;
+
+	sap_connect_rsp(sap_device, SAP_STATUS_OK);
+	sap_status_ind(sap_device, SAP_STATUS_CHANGE_CARD_RESET);
+}
+
+void sap_disconnect_req(void *sap_device, uint8_t linkloss)
+{
+	sim_card_conn_status = SIM_DISCONNECTED;
+	sap_data = NULL;
+	ongoing_call_status = FALSE;
+
+	DBG("status: %d", sim_card_conn_status);
+
+	if (linkloss)
+		return;
+
+	sap_disconnect_rsp(sap_device);
+}
+
+void sap_transfer_apdu_req(void *sap_device, struct sap_parameter *param)
+{
+	char apdu[] = "APDU response!";
+
+	DBG("status: %d", sim_card_conn_status);
+
+	switch (sim_card_conn_status) {
+	case SIM_MISSING:
+		sap_transfer_apdu_rsp(sap_device,
+				SAP_RESULT_ERROR_CARD_REMOVED, NULL, 0);
+		break;
+	case SIM_POWERED_OFF:
+		sap_transfer_apdu_rsp(sap_device, SAP_RESULT_ERROR_POWERED_OFF,
+								NULL, 0);
+		break;
+	case SIM_DISCONNECTED:
+		sap_transfer_apdu_rsp(sap_device,
+				SAP_RESULT_ERROR_NOT_ACCESSIBLE, NULL, 0);
+		break;
+	case SIM_CONNECTED:
+		sap_transfer_apdu_rsp(sap_device, SAP_RESULT_OK,
+						(uint8_t *)apdu, sizeof(apdu));
+		break;
+	}
+}
+
+void sap_transfer_atr_req(void *sap_device)
+{
+	char atr[] = "ATR response!";
+
+	DBG("status: %d", sim_card_conn_status);
+
+	switch (sim_card_conn_status) {
+	case SIM_MISSING:
+		sap_transfer_atr_rsp(sap_device, SAP_RESULT_ERROR_CARD_REMOVED,
+								NULL, 0);
+		break;
+	case SIM_POWERED_OFF:
+		sap_transfer_atr_rsp(sap_device, SAP_RESULT_ERROR_POWERED_OFF,
+								NULL, 0);
+		break;
+	case SIM_DISCONNECTED:
+		sap_transfer_atr_rsp(sap_device, SAP_RESULT_ERROR_NO_REASON,
+								NULL, 0);
+		break;
+	case SIM_CONNECTED:
+		sap_transfer_atr_rsp(sap_device, SAP_RESULT_OK,
+						(uint8_t *)atr, sizeof(atr));
+		break;
+	}
+}
+
+void sap_power_sim_off_req(void *sap_device)
+{
+	DBG("status: %d", sim_card_conn_status);
+
+	switch (sim_card_conn_status) {
+	case SIM_MISSING:
+		sap_power_sim_off_rsp(sap_device,
+					SAP_RESULT_ERROR_CARD_REMOVED);
+		break;
+	case SIM_POWERED_OFF:
+		sap_power_sim_off_rsp(sap_device,
+					SAP_RESULT_ERROR_POWERED_OFF);
+		break;
+	case SIM_DISCONNECTED:
+		sap_power_sim_off_rsp(sap_device, SAP_RESULT_ERROR_NO_REASON);
+		break;
+	case SIM_CONNECTED:
+		sap_power_sim_off_rsp(sap_device, SAP_RESULT_OK);
+		sim_card_conn_status = SIM_POWERED_OFF;
+		break;
+	}
+}
+
+void sap_power_sim_on_req(void *sap_device)
+{
+	DBG("status: %d", sim_card_conn_status);
+
+	switch (sim_card_conn_status) {
+	case SIM_MISSING:
+		sap_power_sim_on_rsp(sap_device,
+					SAP_RESULT_ERROR_CARD_REMOVED);
+		break;
+	case SIM_POWERED_OFF:
+		sap_power_sim_on_rsp(sap_device, SAP_RESULT_OK);
+		sim_card_conn_status = SIM_CONNECTED;
+		break;
+	case SIM_DISCONNECTED:
+		sap_power_sim_on_rsp(sap_device,
+					SAP_RESULT_ERROR_NOT_ACCESSIBLE);
+		break;
+	case SIM_CONNECTED:
+		sap_power_sim_on_rsp(sap_device, SAP_RESULT_ERROR_NO_REASON);
+		break;
+	}
+}
+
+void sap_reset_sim_req(void *sap_device)
+{
+	DBG("status: %d", sim_card_conn_status);
+
+	switch (sim_card_conn_status) {
+	case SIM_MISSING:
+		sap_reset_sim_rsp(sap_device, SAP_RESULT_ERROR_CARD_REMOVED);
+		break;
+	case SIM_POWERED_OFF:
+		sap_reset_sim_rsp(sap_device, SAP_RESULT_ERROR_POWERED_OFF);
+		break;
+	case SIM_DISCONNECTED:
+		sap_reset_sim_rsp(sap_device, SAP_RESULT_ERROR_NO_REASON);
+		break;
+	case SIM_CONNECTED:
+		sap_reset_sim_rsp(sap_device, SAP_RESULT_OK);
+		break;
+	}
+}
+
+void sap_transfer_card_reader_status_req(void *sap_device)
+{
+	DBG("status: %d", sim_card_conn_status);
+
+	if (sim_card_conn_status != SIM_CONNECTED) {
+		sap_transfer_card_reader_status_rsp(sap_device,
+					SAP_RESULT_ERROR_NO_REASON, 0xF1);
+		return;
+	}
+
+	sap_transfer_card_reader_status_rsp(sap_device, SAP_RESULT_OK, 0xF1);
+}
+
+void sap_set_transport_protocol_req(void *sap_device,
+					struct sap_parameter *param)
+{
+	sap_transport_protocol_rsp(sap_device, SAP_RESULT_NOT_SUPPORTED);
+}
+
+static inline DBusMessage *invalid_args(DBusMessage *msg)
+{
+	return g_dbus_create_error(msg, "org.bluez.Error.InvalidArguments",
+					"Invalid arguments in method call");
+}
+
+static DBusMessage *ongoing_call(DBusConnection *conn, DBusMessage *msg,
+						void *data)
+{
+	dbus_bool_t ongoing;
+
+	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_BOOLEAN, &ongoing,
+						DBUS_TYPE_INVALID))
+		return invalid_args(msg);
+
+	if (ongoing_call_status && !ongoing) {
+		/* An ongoing call has finished. Continue connection.*/
+		sap_status_ind(sap_data, SAP_STATUS_CHANGE_CARD_RESET);
+		ongoing_call_status = FALSE;
+	} else if (!ongoing_call_status && ongoing) {
+		/* An ongoing call has started.*/
+		ongoing_call_status = TRUE;
+	}
+
+	DBG("OngoingCall status set to %d", ongoing_call_status);
+
+	return dbus_message_new_method_return(msg);
+}
+
+static DBusMessage *max_msg_size(DBusConnection *conn, DBusMessage *msg,
+						void *data)
+{
+	dbus_uint32_t size;
+
+	if (sim_card_conn_status == SIM_CONNECTED)
+		return g_dbus_create_error(msg, "org.bluez.Error.Failed",
+				"Can't change msg size when connected.");
+
+	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_UINT32, &size,
+							DBUS_TYPE_INVALID))
+		return invalid_args(msg);
+
+	max_msg_size_supported = size;
+
+	DBG("MaxMessageSize set to %d", max_msg_size_supported);
+
+	return dbus_message_new_method_return(msg);
+}
+
+static DBusMessage *disconnect_immediate(DBusConnection *conn, DBusMessage *msg,
+						void *data)
+{
+	if (sim_card_conn_status == SIM_DISCONNECTED)
+		return g_dbus_create_error(msg, "org.bluez.Error.Failed",
+				"Already disconnected.");
+
+	sim_card_conn_status = SIM_DISCONNECTED;
+	sap_disconnect_ind(sap_data, SAP_DISCONNECTION_TYPE_IMMEDIATE);
+
+	return dbus_message_new_method_return(msg);
+}
+
+static DBusMessage *card_status(DBusConnection *conn, DBusMessage *msg,
+								void *data)
+{
+	dbus_uint32_t status;
+
+	DBG("status %d", sim_card_conn_status);
+
+	if (sim_card_conn_status != SIM_CONNECTED)
+		return g_dbus_create_error(msg, "org.bluez.Error.Failed",
+				"Can't change msg size when not connected.");
+
+	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_UINT32, &status,
+							DBUS_TYPE_INVALID))
+		return invalid_args(msg);
+
+	switch (status) {
+	case 0: /* card removed */
+		sim_card_conn_status = SIM_MISSING;
+		sap_status_ind(sap_data, SAP_STATUS_CHANGE_CARD_REMOVED);
+		break;
+
+	case 1: /* card inserted */
+		if (sim_card_conn_status == SIM_MISSING) {
+			sim_card_conn_status = SIM_CONNECTED;
+			sap_status_ind(sap_data,
+					SAP_STATUS_CHANGE_CARD_INSERTED);
+		}
+		break;
+
+	case 2: /* card not longer available*/
+		sim_card_conn_status = SIM_POWERED_OFF;
+		sap_status_ind(sap_data, SAP_STATUS_CHANGE_CARD_NOT_ACCESSIBLE);
+		break;
+
+	default:
+		return g_dbus_create_error(msg, "org.bluez.Error.Failed",
+				"Unknown card status. Use 0, 1 or 2.");
+	}
+
+	DBG("Card status changed to %d", status);
+
+	return dbus_message_new_method_return(msg);
+}
+
+static const GDBusMethodTable dummy_methods[] = {
+	{ GDBUS_METHOD("OngoingCall",
+				GDBUS_ARGS({ "ongoing", "b" }), NULL,
+				ongoing_call) },
+	{ GDBUS_METHOD("MaxMessageSize",
+				GDBUS_ARGS({ "size", "u" }), NULL,
+				max_msg_size) },
+	{ GDBUS_METHOD("DisconnectImmediate", NULL, NULL,
+				disconnect_immediate) },
+	{ GDBUS_METHOD("CardStatus",
+				GDBUS_ARGS({ "status", "" }), NULL,
+				card_status) },
+	{ }
+};
+
+int sap_init(void)
+{
+	if (init_cnt++)
+		return 0;
+
+	connection = dbus_bus_get(DBUS_BUS_SYSTEM, NULL);
+
+	if (g_dbus_register_interface(connection, SAP_DUMMY_PATH,
+				SAP_DUMMY_IFACE, dummy_methods, NULL, NULL,
+				NULL, NULL) == FALSE) {
+		error("sap-dummy interface %s init failed on path %s",
+					SAP_DUMMY_IFACE, SAP_DUMMY_PATH);
+
+		if (init_cnt--) {
+			dbus_connection_unref(connection);
+			connection = NULL;
+		}
+		return -1;
+	}
+
+	return 0;
+}
+
+void sap_exit(void)
+{
+	if (--init_cnt)
+		return;
+
+	g_dbus_unregister_interface(connection, SAP_DUMMY_PATH,
+							SAP_DUMMY_IFACE);
+
+	dbus_connection_unref(connection);
+	connection = NULL;
+}
diff --git a/profiles/sap/sap-u8500.c b/profiles/sap/sap-u8500.c
new file mode 100644
index 000000000..f07209dbb
--- /dev/null
+++ b/profiles/sap/sap-u8500.c
@@ -0,0 +1,750 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  SAP Driver for ST-Ericsson U8500 platform
+ *
+ *  Copyright (C) 2010-2011 ST-Ericsson SA
+ *
+ *  Author: Waldemar Rymarkiewicz <waldemar.rymarkiewicz@tieto.com> for
+ *  ST-Ericsson.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; version 2 of the License.
+ *
+ *  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 <stdio.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <glib.h>
+
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#include "log.h"
+#include "sap.h"
+
+#define STE_SIMD_SOCK  "/dev/socket/catd_a"
+#define STE_CLIENT_TAG 0x0000
+
+#define sap_error(fmt, arg...) do { \
+	error("STE U8500 SAP: " fmt, ## arg); \
+	} while (0)
+
+#define sap_info(fmt, arg...) do { \
+	info("STE U8500 SAP: " fmt, ## arg); \
+	} while (0)
+
+struct ste_message {
+	uint16_t len;
+	uint16_t id;
+	uint32_t client_tag;
+	uint8_t payload[0];
+} __attribute__((packed));
+
+#define STE_MSG_PAYLOAD_SIZE(msg) (msg->len - sizeof(*msg) + sizeof(msg->len))
+
+enum ste_protocol {
+	STE_START_SAP_REQ	= 0x2D01,
+	STE_START_SAP_RSP	= 0x2E01,
+	STE_END_SAP_REQ		= 0x2D02,
+	STE_END_SAP_RSP		= 0x2E02,
+	STE_POWER_OFF_REQ	= 0x2D03,
+	STE_POWER_OFF_RSP	= 0x2E03,
+	STE_POWER_ON_REQ	= 0x2D04,
+	STE_POWER_ON_RSP	= 0x2E04,
+	STE_RESET_REQ		= 0x2D05,
+	STE_RESET_RSP		= 0x2E05,
+	STE_SEND_APDU_REQ	= 0x2D06,
+	STE_SEND_APDU_RSP	= 0x2E06,
+	STE_GET_ATR_REQ		= 0x2D07,
+	STE_GET_ATR_RSP		= 0x2E07,
+	STE_GET_STATUS_REQ	= 0x2D08,
+	STE_GET_STATUS_RSP	= 0x2E08,
+	STE_STATUS_IND		= 0x2F02,
+	STE_SIM_READY_IND	= 0x2F03,
+};
+
+enum ste_msg {
+	STE_SEND_APDU_MSG,
+	STE_GET_ATR_MSG,
+	STE_POWER_OFF_MSG,
+	STE_POWER_ON_MSG,
+	STE_RESET_MSG,
+	STE_GET_STATUS_MSG,
+	STE_MSG_MAX,
+};
+
+enum ste_status {
+	STE_STATUS_OK		= 0x00000000,
+	STE_STATUS_FAILURE	= 0x00000001,
+	STE_STATUS_BUSY_CALL	= 0x00000002,
+};
+
+enum ste_card_status {
+	STE_CARD_STATUS_UNKNOWN		= 0x00,
+	STE_CARD_STATUS_ACTIVE		= 0x01,
+	STE_CARD_STATUS_NOT_ACTIVE	= 0x02,
+	STE_CARD_STATUS_MISSING		= 0x03,
+	STE_CARD_STATUS_INVALID		= 0x04,
+	STE_CARD_STATUS_DISCONNECTED	= 0x05,
+};
+
+/* Card reader status bits as described in GSM 11.14, Section 12.33
+ * Bits 0-2 are for card reader identity and always zeros. */
+#define ICC_READER_REMOVABLE	(1 << 3)
+#define ICC_READER_PRESENT	(1 << 4)
+#define ICC_READER_ID1		(1 << 5)
+#define ICC_READER_CARD_PRESENT	(1 << 6)
+#define ICC_READER_CARD_POWERED	(1 << 7)
+
+enum ste_state {
+	STE_DISABLED,		/* Reader not present or removed */
+	STE_POWERED_OFF,	/* Card in the reader but powered off */
+	STE_NO_CARD,		/* No card in the reader */
+	STE_ENABLED,		/* Card in the reader and powered on */
+	STE_SIM_BUSY,		/* Modem is busy with ongoing call.*/
+	STE_STATE_MAX
+};
+
+struct ste_u8500 {
+	GIOChannel *io;
+	enum ste_state state;
+	void *sap_data;
+};
+
+typedef int(*recv_state_change_cb)(void *sap, uint8_t result);
+typedef int(*recv_pdu_cb)(void *sap, uint8_t result, uint8_t *data,
+								uint16_t len);
+
+static struct ste_u8500 u8500;
+
+static const uint8_t sim2sap_result[STE_MSG_MAX][STE_STATE_MAX] = {
+	/* SAP results for SEND APDU message */
+	{
+		SAP_RESULT_ERROR_NOT_ACCESSIBLE,	/* STE_DISABLED */
+		SAP_RESULT_ERROR_POWERED_OFF,		/* STE_POWERED_OFF */
+		SAP_RESULT_ERROR_CARD_REMOVED,		/* STE_NO_CARD */
+		SAP_RESULT_ERROR_NO_REASON		/* STE_ENABLED */
+	},
+
+	/* SAP results for GET_ATR message */
+	{
+		SAP_RESULT_ERROR_NO_REASON,
+		SAP_RESULT_ERROR_POWERED_OFF,
+		SAP_RESULT_ERROR_CARD_REMOVED,
+		SAP_RESULT_ERROR_NO_REASON
+	},
+
+	/* SAP results POWER OFF message */
+	{
+		SAP_RESULT_ERROR_NO_REASON,
+		SAP_RESULT_ERROR_POWERED_OFF,
+		SAP_RESULT_ERROR_CARD_REMOVED,
+		SAP_RESULT_ERROR_NO_REASON
+	},
+
+	/* SAP results POWER ON message */
+	{
+		SAP_RESULT_ERROR_NO_REASON,
+		SAP_RESULT_ERROR_NOT_ACCESSIBLE,
+		SAP_RESULT_ERROR_CARD_REMOVED,
+		SAP_RESULT_ERROR_POWERED_ON
+	},
+
+	/* SAP results SIM RESET message */
+	{
+		SAP_RESULT_ERROR_NO_REASON,
+		SAP_RESULT_ERROR_POWERED_OFF,
+		SAP_RESULT_ERROR_CARD_REMOVED,
+		SAP_RESULT_ERROR_NOT_ACCESSIBLE
+	},
+
+	/* SAP results GET STATUS message */
+	{
+		SAP_RESULT_ERROR_NO_REASON,
+		SAP_RESULT_ERROR_NO_REASON,
+		SAP_RESULT_ERROR_NO_REASON,
+		SAP_RESULT_ERROR_NO_REASON
+	}
+};
+
+static uint8_t get_sap_result(enum ste_msg msg, uint32_t status)
+{
+	if (!u8500.io)
+		return SAP_RESULT_ERROR_NO_REASON;
+
+	switch (status) {
+	case STE_STATUS_OK:
+		return SAP_RESULT_OK;
+
+	case STE_STATUS_FAILURE:
+		return sim2sap_result[msg][u8500.state];
+
+	default:
+		DBG("Can't convert a result (status %u)", status);
+		return SAP_RESULT_ERROR_NO_REASON;
+	}
+}
+
+static int get_sap_reader_status(uint32_t card_status, uint8_t *icc_status)
+{
+	/* Card reader is present, not removable and not ID-1 size. */
+	*icc_status = ICC_READER_PRESENT;
+
+	switch (card_status) {
+	case STE_CARD_STATUS_ACTIVE:
+		*icc_status |= ICC_READER_CARD_POWERED;
+
+	case STE_CARD_STATUS_NOT_ACTIVE:
+	case STE_CARD_STATUS_INVALID:
+		*icc_status |= ICC_READER_CARD_PRESENT;
+
+	case STE_CARD_STATUS_MISSING:
+	case STE_CARD_STATUS_DISCONNECTED:
+		return 0;
+
+	default:
+		DBG("Can't convert reader status %u", card_status);
+
+	case STE_CARD_STATUS_UNKNOWN:
+		return -1;
+	}
+}
+
+static uint8_t get_sap_status_change(uint32_t card_status)
+{
+	if (!u8500.io)
+		return SAP_STATUS_CHANGE_UNKNOWN_ERROR;
+
+	switch (card_status) {
+	case STE_CARD_STATUS_UNKNOWN:
+		return SAP_STATUS_CHANGE_UNKNOWN_ERROR;
+
+	case STE_CARD_STATUS_ACTIVE:
+		u8500.state = STE_ENABLED;
+		return SAP_STATUS_CHANGE_CARD_RESET;
+
+	case STE_CARD_STATUS_NOT_ACTIVE:
+		u8500.state = STE_DISABLED;
+		return SAP_STATUS_CHANGE_CARD_NOT_ACCESSIBLE;
+
+	case STE_CARD_STATUS_MISSING:
+		u8500.state = STE_DISABLED;
+		return SAP_STATUS_CHANGE_CARD_REMOVED;
+
+	case STE_CARD_STATUS_INVALID:
+		u8500.state = STE_DISABLED;
+		return SAP_STATUS_CHANGE_CARD_NOT_ACCESSIBLE;
+
+	default:
+		DBG("Can't convert status change %u", card_status);
+		return SAP_STATUS_CHANGE_UNKNOWN_ERROR;
+	}
+}
+
+static int send_message(GIOChannel *io, void *buf, size_t size)
+{
+	gsize written;
+
+	SAP_VDBG("io %p, size %zu", io, size);
+
+	if (g_io_channel_write_chars(io, buf, size, &written, NULL) !=
+							G_IO_STATUS_NORMAL)
+		return -EIO;
+
+	return written;
+}
+
+static int send_request(GIOChannel *io, uint16_t id,
+						struct sap_parameter *param)
+{
+	int ret;
+	struct ste_message *msg;
+	size_t size = sizeof(*msg);
+
+	SAP_VDBG("io %p", io);
+
+	if (param)
+		size += param->len;
+
+	msg = g_try_malloc0(size);
+	if (!msg) {
+		sap_error("sending request failed: %s", strerror(ENOMEM));
+		return -ENOMEM;
+	}
+
+	msg->len = size - sizeof(msg->len);
+	msg->id = id;
+	msg->client_tag = STE_CLIENT_TAG;
+
+	if (param)
+		memcpy(msg->payload, param->val, param->len);
+
+	ret = send_message(io, msg, size);
+	if (ret < 0) {
+		sap_error("sending request failed: %s", strerror(-ret));
+	} else if (ret != (int) size) {
+		sap_error("sending request failed: %d out of %zu bytes sent",
+								ret, size);
+		ret = -EIO;
+	}
+
+	g_free(msg);
+
+	return ret;
+}
+
+static void recv_status(uint32_t status)
+{
+	sap_status_ind(u8500.sap_data, get_sap_status_change(status));
+}
+
+static void recv_card_status(uint32_t status, uint8_t *param)
+{
+	uint32_t *card_status;
+	uint8_t result;
+	uint8_t iccrs;
+
+	if (status != STE_STATUS_OK)
+		return;
+
+	card_status = (uint32_t *)param;
+
+	if (get_sap_reader_status(*card_status, &iccrs) < 0)
+		result = SAP_RESULT_ERROR_NO_REASON;
+	else
+		result = get_sap_result(STE_GET_STATUS_MSG, status);
+
+	sap_transfer_card_reader_status_rsp(u8500.sap_data, result, iccrs);
+}
+
+static void recv_state_change(uint32_t ste_msg, uint32_t status,
+			uint32_t new_state, recv_state_change_cb callback)
+{
+	if (status != STE_STATUS_OK)
+		return;
+
+	u8500.state = new_state;
+
+	if (callback)
+		callback(u8500.sap_data, get_sap_result(ste_msg, status));
+}
+
+static void recv_pdu(uint32_t ste_msg, struct ste_message *msg, uint32_t status,
+					uint8_t *param, recv_pdu_cb callback)
+{
+	uint8_t *data = NULL;
+	uint8_t result;
+	int size = 0;
+
+	if (status == STE_STATUS_OK) {
+		data = param;
+		size = STE_MSG_PAYLOAD_SIZE(msg) - sizeof(status);
+	}
+
+	result = get_sap_result(ste_msg, status);
+
+	if (callback)
+		callback(u8500.sap_data, result, data, size);
+}
+
+static void simd_close(void)
+{
+	DBG("io %p", u8500.io);
+
+	if (u8500.io) {
+		g_io_channel_shutdown(u8500.io, TRUE, NULL);
+		g_io_channel_unref(u8500.io);
+	}
+
+	u8500.state = STE_DISABLED;
+	u8500.io = NULL;
+	u8500.sap_data = NULL;
+}
+
+static void recv_sim_ready(void)
+{
+	sap_info("sim is ready. Try to connect again");
+
+	if (send_request(u8500.io, STE_START_SAP_REQ, NULL) < 0) {
+		sap_connect_rsp(u8500.sap_data, SAP_STATUS_CONNECTION_FAILED);
+		simd_close();
+	}
+}
+
+static void recv_connect_rsp(uint32_t status)
+{
+	switch (status) {
+	case STE_STATUS_OK:
+		if (u8500.state != STE_SIM_BUSY)
+			sap_connect_rsp(u8500.sap_data, SAP_STATUS_OK);
+		break;
+	case STE_STATUS_BUSY_CALL:
+		if (u8500.state != STE_SIM_BUSY) {
+			sap_connect_rsp(u8500.sap_data,
+						SAP_STATUS_OK_ONGOING_CALL);
+
+			u8500.state = STE_SIM_BUSY;
+		}
+		break;
+	default:
+		sap_connect_rsp(u8500.sap_data, SAP_STATUS_CONNECTION_FAILED);
+		simd_close();
+		break;
+	}
+}
+
+static void recv_response(struct ste_message *msg)
+{
+	uint32_t status;
+	uint8_t *param;
+
+	SAP_VDBG("msg_id 0x%x", msg->id);
+
+	if (msg->id == STE_END_SAP_RSP) {
+		sap_disconnect_rsp(u8500.sap_data);
+		simd_close();
+		return;
+	}
+
+	param = msg->payload;
+	status = *(uint32_t *)param;
+	param += sizeof(status);
+
+	SAP_VDBG("status 0x%x", status);
+
+	switch (msg->id) {
+	case STE_START_SAP_RSP:
+		recv_connect_rsp(status);
+		break;
+	case STE_SEND_APDU_RSP:
+		recv_pdu(STE_SEND_APDU_MSG, msg, status, param,
+							sap_transfer_apdu_rsp);
+		break;
+
+	case STE_GET_ATR_RSP:
+		recv_pdu(STE_GET_ATR_MSG, msg, status, param,
+							sap_transfer_atr_rsp);
+		break;
+
+	case STE_POWER_OFF_RSP:
+		recv_state_change(STE_POWER_OFF_MSG, status, STE_POWERED_OFF,
+							sap_power_sim_off_rsp);
+		break;
+
+	case STE_POWER_ON_RSP:
+		recv_state_change(STE_POWER_ON_MSG, status, STE_ENABLED,
+							sap_power_sim_on_rsp);
+		break;
+
+	case STE_RESET_RSP:
+		recv_state_change(STE_RESET_MSG, status, STE_ENABLED,
+							sap_reset_sim_rsp);
+		break;
+
+	case STE_GET_STATUS_RSP:
+		recv_card_status(status, param);
+		break;
+
+	case STE_STATUS_IND:
+		recv_status(status);
+		break;
+
+	case STE_SIM_READY_IND:
+		recv_sim_ready();
+		break;
+
+	default:
+		sap_error("unsupported message received (id 0x%x)", msg->id);
+	}
+}
+
+static int recv_message(void *buf, size_t size)
+{
+	uint8_t *iter = buf;
+	struct ste_message *msg = buf;
+
+	do {
+		SAP_VDBG("size %zu msg->len %u.", size, msg->len);
+
+		if (size < sizeof(*msg)) {
+			sap_error("invalid message received (%zu bytes)", size);
+			return -EBADMSG;
+		}
+
+		/* Message must be complete. */
+		if (size < (msg->len + sizeof(msg->len))) {
+			sap_error("incomplete message received (%zu bytes)",
+									size);
+			return -EBADMSG;
+		}
+
+		recv_response(msg);
+
+		/* Reduce total buffer size by just handled frame size. */
+		size -= msg->len + sizeof(msg->len);
+
+		/* Move msg pointer to the next message if any. */
+		iter += msg->len + sizeof(msg->len);
+		msg = (struct ste_message *)iter;
+	} while (size > 0);
+
+	return 0;
+}
+
+static gboolean simd_data_cb(GIOChannel *io, GIOCondition cond, gpointer data)
+{
+	char buf[SAP_BUF_SIZE];
+	gsize bytes_read;
+	GIOStatus gstatus;
+
+	if (cond & (G_IO_NVAL | G_IO_HUP | G_IO_ERR)) {
+		DBG("Error condition on sim socket (0x%x)", cond);
+		return FALSE;
+	}
+
+	gstatus = g_io_channel_read_chars(io, buf, sizeof(buf), &bytes_read,
+									NULL);
+	if (gstatus != G_IO_STATUS_NORMAL) {
+		sap_error("error while reading from channel (%d)", gstatus);
+		return TRUE;
+	}
+
+	if (recv_message(buf, bytes_read) < 0)
+		sap_error("error while parsing STE Sim message");
+
+	return TRUE;
+}
+
+static void simd_watch(int sock, void *sap_data)
+{
+	GIOChannel *io;
+
+	DBG("sock %d, sap_data %p ", sock, sap_data);
+
+	io = g_io_channel_unix_new(sock);
+
+	g_io_channel_set_close_on_unref(io, TRUE);
+	g_io_channel_set_encoding(io, NULL, NULL);
+	g_io_channel_set_buffered(io, FALSE);
+
+	u8500.io = io;
+	u8500.sap_data = sap_data;
+	u8500.state = STE_DISABLED;
+
+	g_io_add_watch_full(io, G_PRIORITY_DEFAULT,
+			G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+			simd_data_cb, NULL, NULL);
+}
+
+static int simd_connect(void *sap_data)
+{
+	struct sockaddr_un addr;
+	int sock;
+	int err;
+
+	/* Already connected to simd */
+	if (u8500.io)
+		return -EALREADY;
+
+	sock = socket(PF_UNIX, SOCK_STREAM, 0);
+	if (sock < 0) {
+		err = -errno;
+		sap_error("creating socket failed: %s", strerror(-err));
+		return err;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sun_family = AF_UNIX;
+	memcpy(addr.sun_path, STE_SIMD_SOCK, sizeof(STE_SIMD_SOCK) - 1);
+
+	if (connect(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		err = -errno;
+		sap_error("connect to the socket failed: %s", strerror(-err));
+		goto failed;
+	}
+
+	if (fcntl(sock, F_SETFL, O_NONBLOCK) > 0) {
+		err = -errno;
+		sap_error("setting up socket failed: %s", strerror(-err));
+		goto failed;
+	}
+
+	simd_watch(sock, sap_data);
+
+	return 0;
+
+failed:
+	close(sock);
+	return err;
+}
+
+void sap_connect_req(void *sap_device, uint16_t maxmsgsize)
+{
+	DBG("sap_device %p maxmsgsize %u", sap_device, maxmsgsize);
+
+	sap_info("connect request");
+
+	if (simd_connect(sap_device) < 0) {
+		sap_connect_rsp(sap_device, SAP_STATUS_CONNECTION_FAILED);
+		return;
+	}
+
+	if (send_request(u8500.io, STE_START_SAP_REQ, NULL) < 0) {
+		sap_connect_rsp(sap_device, SAP_STATUS_CONNECTION_FAILED);
+		simd_close();
+	}
+}
+
+void sap_disconnect_req(void *sap_device, uint8_t linkloss)
+{
+	DBG("sap_device %p linkloss %u", sap_device, linkloss);
+
+	sap_info("disconnect request %s", linkloss ? "by link loss" : "");
+
+	if (u8500.state == STE_DISABLED) {
+		sap_disconnect_rsp(sap_device);
+		simd_close();
+		return;
+	}
+
+	if (linkloss) {
+		simd_close();
+		return;
+	}
+
+	if (send_request(u8500.io, STE_END_SAP_REQ, NULL) < 0) {
+		sap_disconnect_rsp(sap_device);
+		return;
+	}
+}
+
+void sap_transfer_apdu_req(void *sap_device, struct sap_parameter *param)
+{
+	uint8_t result;
+
+	SAP_VDBG("sap_device %p param %p", sap_device, param);
+
+	if (u8500.state != STE_ENABLED) {
+		result = get_sap_result(STE_SEND_APDU_MSG, STE_STATUS_FAILURE);
+		sap_transfer_apdu_rsp(sap_device, result, NULL, 0);
+		return;
+	}
+
+	if (send_request(u8500.io, STE_SEND_APDU_REQ, param) < 0)
+		sap_transfer_apdu_rsp(sap_device, SAP_RESULT_ERROR_NO_DATA,
+								NULL, 0);
+}
+
+void sap_transfer_atr_req(void *sap_device)
+{
+	uint8_t result;
+
+	DBG("sap_device %p", sap_device);
+
+	if (u8500.state != STE_ENABLED) {
+		result = get_sap_result(STE_GET_ATR_MSG, STE_STATUS_FAILURE);
+		sap_transfer_atr_rsp(sap_device, result, NULL, 0);
+		return;
+	}
+
+	if (send_request(u8500.io, STE_GET_ATR_REQ, NULL) < 0)
+		sap_transfer_atr_rsp(sap_device, SAP_RESULT_ERROR_NO_DATA, NULL,
+									0);
+}
+
+void sap_power_sim_off_req(void *sap_device)
+{
+	uint8_t result;
+
+	DBG("sap_device %p", sap_device);
+
+	if (u8500.state != STE_ENABLED) {
+		result = get_sap_result(STE_POWER_OFF_MSG, STE_STATUS_FAILURE);
+		sap_power_sim_off_rsp(sap_device, result);
+		return;
+	}
+
+	if (send_request(u8500.io, STE_POWER_OFF_REQ, NULL) < 0)
+		sap_power_sim_off_rsp(sap_device, SAP_RESULT_ERROR_NO_REASON);
+}
+
+void sap_power_sim_on_req(void *sap_device)
+{
+	uint8_t result;
+
+	DBG("sap_device %p", sap_device);
+
+	if (u8500.state != STE_POWERED_OFF) {
+		result = get_sap_result(STE_POWER_ON_MSG, STE_STATUS_FAILURE);
+		sap_power_sim_on_rsp(sap_device, result);
+		return;
+	}
+
+	if (send_request(u8500.io, STE_POWER_ON_REQ, NULL) < 0)
+		sap_power_sim_on_rsp(sap_device, SAP_RESULT_ERROR_NO_REASON);
+}
+
+void sap_reset_sim_req(void *sap_device)
+{
+	uint8_t result;
+
+	DBG("sap_device %p", sap_device);
+
+	if (u8500.state != STE_ENABLED) {
+		result = get_sap_result(STE_RESET_MSG, STE_STATUS_FAILURE);
+		sap_reset_sim_rsp(sap_device, result);
+		return;
+	}
+
+	if (send_request(u8500.io, STE_RESET_REQ, NULL) < 0)
+		sap_reset_sim_rsp(sap_device, SAP_RESULT_ERROR_NO_REASON);
+}
+
+void sap_transfer_card_reader_status_req(void *sap_device)
+{
+	uint8_t result;
+
+	DBG("sap_device %p", sap_device);
+
+	if (u8500.state == STE_DISABLED) {
+		result = get_sap_result(STE_GET_STATUS_MSG, STE_STATUS_FAILURE);
+		sap_transfer_card_reader_status_rsp(sap_device, result, 0);
+		return;
+	}
+
+	if (send_request(u8500.io, STE_GET_STATUS_REQ, NULL) < 0)
+		sap_transfer_card_reader_status_rsp(sap_device,
+						SAP_RESULT_ERROR_NO_DATA, 0);
+}
+
+void sap_set_transport_protocol_req(void *sap_device,
+						struct sap_parameter *param)
+{
+	DBG("sap_device %p", sap_device);
+
+	sap_transport_protocol_rsp(sap_device, SAP_RESULT_NOT_SUPPORTED);
+}
+
+int sap_init(void)
+{
+	u8500.state = STE_DISABLED;
+	info("STE U8500 SAP driver initialized");
+	return 0;
+}
+
+void sap_exit(void)
+{
+}
diff --git a/profiles/sap/sap.h b/profiles/sap/sap.h
new file mode 100644
index 000000000..16c333a9b
--- /dev/null
+++ b/profiles/sap/sap.h
@@ -0,0 +1,180 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 Instituto Nokia de Tecnologia - INdT
+ *  Copyright (C) 2010 ST-Ericsson SA
+ *
+ *  Author: Marek Skowron <marek.skowron@tieto.com> for ST-Ericsson.
+ *  Author: Waldemar Rymarkiewicz <waldemar.rymarkiewicz@tieto.com>
+ *          for ST-Ericsson.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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 <stdint.h>
+#include <glib.h>
+
+#ifdef SAP_DEBUG
+#define SAP_VDBG(fmt, arg...) DBG(fmt, arg)
+#else
+#define SAP_VDBG(fmt...)
+#endif
+
+#define SAP_VERSION 0x0101
+
+/* Connection Status - SAP v1.1 section 5.2.2 */
+enum sap_status {
+	SAP_STATUS_OK				= 0x00,
+	SAP_STATUS_CONNECTION_FAILED		= 0x01,
+	SAP_STATUS_MAX_MSG_SIZE_NOT_SUPPORTED	= 0x02,
+	SAP_STATUS_MAX_MSG_SIZE_TOO_SMALL	= 0x03,
+	SAP_STATUS_OK_ONGOING_CALL		= 0x04
+};
+
+/* Disconnection Type - SAP v1.1 section 5.2.3 */
+enum sap_disconnection_type {
+	SAP_DISCONNECTION_TYPE_GRACEFUL		= 0x00,
+	SAP_DISCONNECTION_TYPE_IMMEDIATE	= 0x01
+};
+
+/* Result codes - SAP v1.1 section 5.2.4 */
+enum sap_result {
+	SAP_RESULT_OK			= 0x00,
+	SAP_RESULT_ERROR_NO_REASON	= 0x01,
+	SAP_RESULT_ERROR_NOT_ACCESSIBLE	= 0x02,
+	SAP_RESULT_ERROR_POWERED_OFF	= 0x03,
+	SAP_RESULT_ERROR_CARD_REMOVED	= 0x04,
+	SAP_RESULT_ERROR_POWERED_ON	= 0x05,
+	SAP_RESULT_ERROR_NO_DATA	= 0x06,
+	SAP_RESULT_NOT_SUPPORTED	= 0x07
+};
+
+/* Status Change - SAP v1.1 section 5.2.8 */
+enum sap_status_change {
+	SAP_STATUS_CHANGE_UNKNOWN_ERROR		= 0x00,
+	SAP_STATUS_CHANGE_CARD_RESET		= 0x01,
+	SAP_STATUS_CHANGE_CARD_NOT_ACCESSIBLE	= 0x02,
+	SAP_STATUS_CHANGE_CARD_REMOVED		= 0x03,
+	SAP_STATUS_CHANGE_CARD_INSERTED		= 0x04,
+	SAP_STATUS_CHANGE_CARD_RECOVERED	= 0x05
+};
+
+/* Message format - SAP v1.1 section 5.1 */
+struct sap_parameter {
+	uint8_t id;
+	uint8_t reserved;
+	uint16_t len;
+	uint8_t val[0];
+	/*
+	 * Padding bytes 0-3 bytes
+	 */
+} __attribute__((packed));
+
+struct sap_message {
+	uint8_t id;
+	uint8_t nparam;
+	uint16_t reserved;
+	struct sap_parameter param[0];
+} __attribute__((packed));
+
+#define SAP_BUF_SIZE		512
+#define SAP_MSG_HEADER_SIZE	4
+
+enum sap_protocol {
+	SAP_CONNECT_REQ		= 0x00,
+	SAP_CONNECT_RESP	= 0x01,
+	SAP_DISCONNECT_REQ	= 0x02,
+	SAP_DISCONNECT_RESP	= 0x03,
+	SAP_DISCONNECT_IND	= 0x04,
+	SAP_TRANSFER_APDU_REQ	= 0x05,
+	SAP_TRANSFER_APDU_RESP	= 0x06,
+	SAP_TRANSFER_ATR_REQ	= 0x07,
+	SAP_TRANSFER_ATR_RESP	= 0x08,
+	SAP_POWER_SIM_OFF_REQ	= 0x09,
+	SAP_POWER_SIM_OFF_RESP	= 0x0A,
+	SAP_POWER_SIM_ON_REQ	= 0x0B,
+	SAP_POWER_SIM_ON_RESP	= 0x0C,
+	SAP_RESET_SIM_REQ	= 0x0D,
+	SAP_RESET_SIM_RESP	= 0x0E,
+	SAP_TRANSFER_CARD_READER_STATUS_REQ	= 0x0F,
+	SAP_TRANSFER_CARD_READER_STATUS_RESP	= 0x10,
+	SAP_STATUS_IND	= 0x11,
+	SAP_ERROR_RESP	= 0x12,
+	SAP_SET_TRANSPORT_PROTOCOL_REQ	= 0x13,
+	SAP_SET_TRANSPORT_PROTOCOL_RESP	= 0x14
+};
+
+/* Parameters Ids - SAP 1.1 section 5.2 */
+enum sap_param_id {
+	SAP_PARAM_ID_MAX_MSG_SIZE	= 0x00,
+	SAP_PARAM_ID_CONN_STATUS	= 0x01,
+	SAP_PARAM_ID_RESULT_CODE	= 0x02,
+	SAP_PARAM_ID_DISCONNECT_IND	= 0x03,
+	SAP_PARAM_ID_COMMAND_APDU	= 0x04,
+	SAP_PARAM_ID_COMMAND_APDU7816	= 0x10,
+	SAP_PARAM_ID_RESPONSE_APDU	= 0x05,
+	SAP_PARAM_ID_ATR		= 0x06,
+	SAP_PARAM_ID_CARD_READER_STATUS	= 0x07,
+	SAP_PARAM_ID_STATUS_CHANGE	= 0x08,
+	SAP_PARAM_ID_TRANSPORT_PROTOCOL	= 0x09
+};
+
+#define SAP_PARAM_ID_MAX_MSG_SIZE_LEN		0x02
+#define SAP_PARAM_ID_CONN_STATUS_LEN		0x01
+#define SAP_PARAM_ID_RESULT_CODE_LEN		0x01
+#define SAP_PARAM_ID_DISCONNECT_IND_LEN		0x01
+#define SAP_PARAM_ID_CARD_READER_STATUS_LEN	0x01
+#define SAP_PARAM_ID_STATUS_CHANGE_LEN		0x01
+#define SAP_PARAM_ID_TRANSPORT_PROTO_LEN	0x01
+
+/* Transport Protocol - SAP v1.1 section 5.2.9 */
+enum sap_transport_protocol {
+	SAP_TRANSPORT_PROTOCOL_T0 = 0x00,
+	SAP_TRANSPORT_PROTOCOL_T1 = 0x01
+};
+
+/*SAP driver init and exit routines. Implemented by sap-*.c */
+int sap_init(void);
+void sap_exit(void);
+
+/* SAP requests implemented by sap-*.c */
+void sap_connect_req(void *sap_device, uint16_t maxmsgsize);
+void sap_disconnect_req(void *sap_device, uint8_t linkloss);
+void sap_transfer_apdu_req(void *sap_device, struct sap_parameter *param);
+void sap_transfer_atr_req(void *sap_device);
+void sap_power_sim_off_req(void *sap_device);
+void sap_power_sim_on_req(void *sap_device);
+void sap_reset_sim_req(void *sap_device);
+void sap_transfer_card_reader_status_req(void *sap_device);
+void sap_set_transport_protocol_req(void *sap_device,
+					struct sap_parameter *param);
+
+/*SAP responses to SAP requests. Implemented by server.c */
+int sap_connect_rsp(void *sap_device, uint8_t status);
+int sap_disconnect_rsp(void *sap_device);
+int sap_transfer_apdu_rsp(void *sap_device, uint8_t result,
+				uint8_t *sap_apdu_resp, uint16_t length);
+int sap_transfer_atr_rsp(void *sap_device, uint8_t result,
+				uint8_t *sap_atr, uint16_t length);
+int sap_power_sim_off_rsp(void *sap_device, uint8_t result);
+int sap_power_sim_on_rsp(void *sap_device, uint8_t result);
+int sap_reset_sim_rsp(void *sap_device, uint8_t result);
+int sap_transfer_card_reader_status_rsp(void *sap_device, uint8_t result,
+						uint8_t status);
+int sap_transport_protocol_rsp(void *sap_device, uint8_t result);
+
+/* Event indication. Implemented by server.c*/
+int sap_status_ind(void *sap_device, uint8_t status_change);
+int sap_disconnect_ind(void *sap_device, uint8_t disc_type);
diff --git a/profiles/sap/server.c b/profiles/sap/server.c
new file mode 100644
index 000000000..ee2624dfc
--- /dev/null
+++ b/profiles/sap/server.c
@@ -0,0 +1,1455 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 Instituto Nokia de Tecnologia - INdT
+ *  Copyright (C) 2010 ST-Ericsson SA
+ *  Copyright (C) 2011 Tieto Poland
+ *
+ *  Author: Marek Skowron <marek.skowron@tieto.com> for ST-Ericsson.
+ *  Author: Waldemar Rymarkiewicz <waldemar.rymarkiewicz@tieto.com>
+ *          for ST-Ericsson.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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 <errno.h>
+#include <glib.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+#include <bluetooth/uuid.h>
+
+#include "adapter.h"
+#include "btio.h"
+#include "sdpd.h"
+#include "log.h"
+#include "error.h"
+#include "dbus-common.h"
+#include "sap.h"
+#include "server.h"
+
+#define SAP_SERVER_INTERFACE	"org.bluez.SimAccess"
+#define SAP_SERVER_CHANNEL	8
+
+#define PADDING4(x) ((4 - ((x) & 0x03)) & 0x03)
+#define PARAMETER_SIZE(x) (sizeof(struct sap_parameter) + x + PADDING4(x))
+
+#define SAP_NO_REQ 0xFF
+#define SAP_DISCONNECTION_TYPE_CLIENT 0xFF
+
+#define SAP_TIMER_GRACEFUL_DISCONNECT 30
+#define SAP_TIMER_NO_ACTIVITY 30
+
+enum {
+	SAP_STATE_DISCONNECTED,
+	SAP_STATE_CONNECT_IN_PROGRESS,
+	SAP_STATE_CONNECT_MODEM_BUSY,
+	SAP_STATE_CONNECTED,
+	SAP_STATE_GRACEFUL_DISCONNECT,
+	SAP_STATE_IMMEDIATE_DISCONNECT,
+	SAP_STATE_CLIENT_DISCONNECT
+};
+
+struct sap_connection {
+	GIOChannel *io;
+	uint32_t state;
+	uint8_t processing_req;
+	guint timer_id;
+};
+
+struct sap_server {
+	char *path;
+	uint32_t record_id;
+	GIOChannel *listen_io;
+	struct sap_connection *conn;
+};
+
+static DBusConnection *connection;
+
+static void start_guard_timer(struct sap_server *server, guint interval);
+static void stop_guard_timer(struct sap_server *server);
+static gboolean guard_timeout(gpointer data);
+
+static size_t add_result_parameter(uint8_t result,
+					struct sap_parameter *param)
+{
+	param->id = SAP_PARAM_ID_RESULT_CODE;
+	param->len = htons(SAP_PARAM_ID_RESULT_CODE_LEN);
+	*param->val = result;
+
+	return PARAMETER_SIZE(SAP_PARAM_ID_RESULT_CODE_LEN);
+}
+
+static int is_power_sim_off_req_allowed(uint8_t processing_req)
+{
+	switch (processing_req) {
+	case SAP_NO_REQ:
+	case SAP_TRANSFER_APDU_REQ:
+	case SAP_TRANSFER_ATR_REQ:
+	case SAP_POWER_SIM_ON_REQ:
+	case SAP_RESET_SIM_REQ:
+	case SAP_TRANSFER_CARD_READER_STATUS_REQ:
+		return 1;
+	default:
+		return 0;
+	}
+}
+
+static int is_reset_sim_req_allowed(uint8_t processing_req)
+{
+	switch (processing_req) {
+	case SAP_NO_REQ:
+	case SAP_TRANSFER_APDU_REQ:
+	case SAP_TRANSFER_ATR_REQ:
+	case SAP_TRANSFER_CARD_READER_STATUS_REQ:
+		return 1;
+	default:
+		return 0;
+	}
+}
+
+static int check_msg(struct sap_message *msg)
+{
+	switch (msg->id) {
+	case SAP_CONNECT_REQ:
+		if (msg->nparam != 0x01)
+			return -EBADMSG;
+
+		if (msg->param->id != SAP_PARAM_ID_MAX_MSG_SIZE)
+			return -EBADMSG;
+
+		if (ntohs(msg->param->len) != SAP_PARAM_ID_MAX_MSG_SIZE_LEN)
+			return -EBADMSG;
+
+		break;
+
+	case SAP_TRANSFER_APDU_REQ:
+		if (msg->nparam != 0x01)
+			return -EBADMSG;
+
+		if (msg->param->id != SAP_PARAM_ID_COMMAND_APDU)
+			if (msg->param->id != SAP_PARAM_ID_COMMAND_APDU7816)
+				return -EBADMSG;
+
+		if (msg->param->len == 0x00)
+			return -EBADMSG;
+
+		break;
+
+	case SAP_SET_TRANSPORT_PROTOCOL_REQ:
+		if (msg->nparam != 0x01)
+			return -EBADMSG;
+
+		if (msg->param->id != SAP_PARAM_ID_TRANSPORT_PROTOCOL)
+			return -EBADMSG;
+
+		if (ntohs(msg->param->len) != SAP_PARAM_ID_TRANSPORT_PROTO_LEN)
+			return -EBADMSG;
+
+		if (*msg->param->val != SAP_TRANSPORT_PROTOCOL_T0)
+			if (*msg->param->val != SAP_TRANSPORT_PROTOCOL_T1)
+				return -EBADMSG;
+
+		break;
+
+	case SAP_DISCONNECT_REQ:
+	case SAP_TRANSFER_ATR_REQ:
+	case SAP_POWER_SIM_OFF_REQ:
+	case SAP_POWER_SIM_ON_REQ:
+	case SAP_RESET_SIM_REQ:
+	case SAP_TRANSFER_CARD_READER_STATUS_REQ:
+		if (msg->nparam != 0x00)
+			return -EBADMSG;
+
+		break;
+	}
+
+	return 0;
+}
+
+static sdp_record_t *create_sap_record(uint8_t channel)
+{
+	sdp_list_t *apseq, *aproto, *profiles, *proto[2], *root, *svclass_id;
+	uuid_t sap_uuid, gt_uuid, root_uuid, l2cap, rfcomm;
+	sdp_profile_desc_t profile;
+	sdp_record_t *record;
+	sdp_data_t *ch;
+
+	record = sdp_record_alloc();
+	if (!record)
+		return NULL;
+
+	sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP);
+	root = sdp_list_append(NULL, &root_uuid);
+	sdp_set_browse_groups(record, root);
+	sdp_list_free(root, NULL);
+
+	sdp_uuid16_create(&sap_uuid, SAP_SVCLASS_ID);
+	svclass_id = sdp_list_append(NULL, &sap_uuid);
+	sdp_uuid16_create(&gt_uuid, GENERIC_TELEPHONY_SVCLASS_ID);
+	svclass_id = sdp_list_append(svclass_id, &gt_uuid);
+
+	sdp_set_service_classes(record, svclass_id);
+	sdp_list_free(svclass_id, NULL);
+
+	sdp_uuid16_create(&profile.uuid, SAP_PROFILE_ID);
+	profile.version = SAP_VERSION;
+	profiles = sdp_list_append(NULL, &profile);
+	sdp_set_profile_descs(record, profiles);
+	sdp_list_free(profiles, NULL);
+
+	sdp_uuid16_create(&l2cap, L2CAP_UUID);
+	proto[0] = sdp_list_append(NULL, &l2cap);
+	apseq = sdp_list_append(NULL, proto[0]);
+
+	sdp_uuid16_create(&rfcomm, RFCOMM_UUID);
+	proto[1] = sdp_list_append(NULL, &rfcomm);
+	ch = sdp_data_alloc(SDP_UINT8, &channel);
+	proto[1] = sdp_list_append(proto[1], ch);
+	apseq = sdp_list_append(apseq, proto[1]);
+
+	aproto = sdp_list_append(NULL, apseq);
+	sdp_set_access_protos(record, aproto);
+
+	sdp_set_info_attr(record, "SIM Access Server", NULL, NULL);
+
+	sdp_data_free(ch);
+	sdp_list_free(proto[0], NULL);
+	sdp_list_free(proto[1], NULL);
+	sdp_list_free(apseq, NULL);
+	sdp_list_free(aproto, NULL);
+
+	return record;
+}
+
+static int send_message(struct sap_connection *conn, void *buf, size_t size)
+{
+	size_t written = 0;
+	GError *gerr = NULL;
+	GIOStatus gstatus;
+
+	SAP_VDBG("conn %p, size %zu", conn, size);
+
+	gstatus = g_io_channel_write_chars(conn->io, buf, size, &written,
+									&gerr);
+	if (gstatus != G_IO_STATUS_NORMAL) {
+		if (gerr)
+			g_error_free(gerr);
+
+		error("write error (0x%02x).", gstatus);
+		return -EIO;
+	}
+
+	if (written != size) {
+		error("written %zu bytes out of %zu", written, size);
+		return -EIO;
+	}
+
+	return written;
+}
+
+static int disconnect_ind(struct sap_connection *conn, uint8_t disc_type)
+{
+	char buf[SAP_BUF_SIZE];
+	struct sap_message *msg = (struct sap_message *) buf;
+	struct sap_parameter *param = (struct sap_parameter *) msg->param;
+	size_t size = sizeof(struct sap_message);
+
+	DBG("data %p state %d disc_type 0x%02x", conn, conn->state, disc_type);
+
+	memset(buf, 0, sizeof(buf));
+	msg->id = SAP_DISCONNECT_IND;
+	msg->nparam = 0x01;
+
+	/* Add disconnection type param. */
+	param->id  = SAP_PARAM_ID_DISCONNECT_IND;
+	param->len = htons(SAP_PARAM_ID_DISCONNECT_IND_LEN);
+	*param->val = disc_type;
+	size += PARAMETER_SIZE(SAP_PARAM_ID_DISCONNECT_IND_LEN);
+
+	return send_message(conn, buf, size);
+}
+
+static int sap_error_rsp(struct sap_connection *conn)
+{
+	struct sap_message msg;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.id = SAP_ERROR_RESP;
+
+	error("SAP error (state %d pr 0x%02x).", conn->state,
+							conn->processing_req);
+
+	return send_message(conn, &msg, sizeof(msg));
+}
+
+static void connect_req(struct sap_server *server,
+				struct sap_parameter *param)
+{
+	struct sap_connection *conn = server->conn;
+	uint16_t maxmsgsize, *val;
+
+	DBG("conn %p state %d", conn, conn->state);
+
+	if (!param)
+		goto error_rsp;
+
+	if (conn->state != SAP_STATE_DISCONNECTED)
+		goto error_rsp;
+
+	stop_guard_timer(server);
+
+	val = (uint16_t *) &param->val;
+	maxmsgsize = ntohs(*val);
+
+	DBG("Connect MaxMsgSize: 0x%04x", maxmsgsize);
+
+	conn->state = SAP_STATE_CONNECT_IN_PROGRESS;
+
+	if (maxmsgsize <= SAP_BUF_SIZE) {
+		conn->processing_req = SAP_CONNECT_REQ;
+		sap_connect_req(server, maxmsgsize);
+	} else {
+		sap_connect_rsp(server, SAP_STATUS_MAX_MSG_SIZE_NOT_SUPPORTED);
+	}
+
+	return;
+
+error_rsp:
+	sap_error_rsp(conn);
+}
+
+static int disconnect_req(struct sap_server *server, uint8_t disc_type)
+{
+	struct sap_connection *conn = server->conn;
+
+	DBG("conn %p state %d disc_type 0x%02x", conn, conn->state, disc_type);
+
+	switch (disc_type) {
+	case SAP_DISCONNECTION_TYPE_GRACEFUL:
+		if (conn->state == SAP_STATE_DISCONNECTED ||
+				conn->state == SAP_STATE_CONNECT_IN_PROGRESS ||
+				conn->state == SAP_STATE_CONNECT_MODEM_BUSY)
+			return -EPERM;
+
+		if (conn->state == SAP_STATE_CONNECTED) {
+			conn->state = SAP_STATE_GRACEFUL_DISCONNECT;
+			conn->processing_req = SAP_NO_REQ;
+
+			disconnect_ind(conn, disc_type);
+			/* Timer will disconnect if client won't do.*/
+			start_guard_timer(server,
+					SAP_TIMER_GRACEFUL_DISCONNECT);
+		}
+
+		return 0;
+
+	case SAP_DISCONNECTION_TYPE_IMMEDIATE:
+		if (conn->state == SAP_STATE_DISCONNECTED ||
+				conn->state == SAP_STATE_CONNECT_IN_PROGRESS ||
+				conn->state == SAP_STATE_CONNECT_MODEM_BUSY)
+			return -EPERM;
+
+		if (conn->state == SAP_STATE_CONNECTED ||
+				conn->state == SAP_STATE_GRACEFUL_DISCONNECT) {
+			conn->state = SAP_STATE_IMMEDIATE_DISCONNECT;
+			conn->processing_req = SAP_NO_REQ;
+
+			stop_guard_timer(server);
+			disconnect_ind(conn, disc_type);
+			sap_disconnect_req(server, 0);
+		}
+
+		return 0;
+
+	case SAP_DISCONNECTION_TYPE_CLIENT:
+		if (conn->state != SAP_STATE_CONNECTED &&
+				conn->state != SAP_STATE_GRACEFUL_DISCONNECT) {
+			sap_error_rsp(conn);
+			return -EPERM;
+		}
+
+		conn->state = SAP_STATE_CLIENT_DISCONNECT;
+		conn->processing_req = SAP_NO_REQ;
+
+		stop_guard_timer(server);
+		sap_disconnect_req(server, 0);
+
+		return 0;
+
+	default:
+		error("Unknown disconnection type (0x%02x).", disc_type);
+		return -EINVAL;
+	}
+}
+
+static void transfer_apdu_req(struct sap_server *server,
+					struct sap_parameter *param)
+{
+	struct sap_connection *conn = server->conn;
+
+	SAP_VDBG("conn %p state %d", conn, conn->state);
+
+	if (!param)
+		goto error_rsp;
+
+	param->len = ntohs(param->len);
+
+	if (conn->state != SAP_STATE_CONNECTED &&
+			conn->state != SAP_STATE_GRACEFUL_DISCONNECT)
+		goto error_rsp;
+
+	if (conn->processing_req != SAP_NO_REQ)
+		goto error_rsp;
+
+	conn->processing_req = SAP_TRANSFER_APDU_REQ;
+	sap_transfer_apdu_req(server, param);
+
+	return;
+
+error_rsp:
+	sap_error_rsp(conn);
+}
+
+static void transfer_atr_req(struct sap_server *server)
+{
+	struct sap_connection *conn = server->conn;
+
+	DBG("conn %p state %d", conn, conn->state);
+
+	if (conn->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (conn->processing_req != SAP_NO_REQ)
+		goto error_rsp;
+
+	conn->processing_req = SAP_TRANSFER_ATR_REQ;
+	sap_transfer_atr_req(server);
+
+	return;
+
+error_rsp:
+	sap_error_rsp(conn);
+}
+
+static void power_sim_off_req(struct sap_server *server)
+{
+	struct sap_connection *conn = server->conn;
+
+	DBG("conn %p state %d", conn, conn->state);
+
+	if (conn->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (!is_power_sim_off_req_allowed(conn->processing_req))
+		goto error_rsp;
+
+	conn->processing_req = SAP_POWER_SIM_OFF_REQ;
+	sap_power_sim_off_req(server);
+
+	return;
+
+error_rsp:
+	sap_error_rsp(conn);
+}
+
+static void power_sim_on_req(struct sap_server *server)
+{
+	struct sap_connection *conn = server->conn;
+
+	DBG("conn %p state %d", conn, conn->state);
+
+	if (conn->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (conn->processing_req != SAP_NO_REQ)
+		goto error_rsp;
+
+	conn->processing_req = SAP_POWER_SIM_ON_REQ;
+	sap_power_sim_on_req(server);
+
+	return;
+
+error_rsp:
+	sap_error_rsp(conn);
+}
+
+static void reset_sim_req(struct sap_server *server)
+{
+	struct sap_connection *conn = server->conn;
+
+	DBG("conn %p state %d", conn, conn->state);
+
+	if (conn->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (!is_reset_sim_req_allowed(conn->processing_req))
+		goto error_rsp;
+
+	conn->processing_req = SAP_RESET_SIM_REQ;
+	sap_reset_sim_req(server);
+
+	return;
+
+error_rsp:
+	sap_error_rsp(conn);
+}
+
+static void transfer_card_reader_status_req(struct sap_server *server)
+{
+	struct sap_connection *conn = server->conn;
+
+	DBG("conn %p state %d", conn, conn->state);
+
+	if (conn->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (conn->processing_req != SAP_NO_REQ)
+		goto error_rsp;
+
+	conn->processing_req = SAP_TRANSFER_CARD_READER_STATUS_REQ;
+	sap_transfer_card_reader_status_req(server);
+
+	return;
+
+error_rsp:
+	sap_error_rsp(conn);
+}
+
+static void set_transport_protocol_req(struct sap_server *server,
+					struct sap_parameter *param)
+{
+	struct sap_connection *conn = server->conn;
+
+	if (!param)
+		goto error_rsp;
+
+	DBG("conn %p state %d param %p", conn, conn->state, param);
+
+	if (conn->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (conn->processing_req != SAP_NO_REQ)
+		goto error_rsp;
+
+	conn->processing_req = SAP_SET_TRANSPORT_PROTOCOL_REQ;
+	sap_set_transport_protocol_req(server, param);
+
+	return;
+
+error_rsp:
+	sap_error_rsp(conn);
+}
+
+static void start_guard_timer(struct sap_server *server, guint interval)
+{
+	struct sap_connection *conn = server->conn;
+
+	if (!conn)
+		return;
+
+	if (!conn->timer_id)
+		conn->timer_id = g_timeout_add_seconds(interval, guard_timeout,
+								server);
+	else
+		error("Timer is already active.");
+}
+
+static void stop_guard_timer(struct sap_server *server)
+{
+	struct sap_connection *conn = server->conn;
+
+	if (conn  && conn->timer_id) {
+		g_source_remove(conn->timer_id);
+		conn->timer_id = 0;
+	}
+}
+
+static gboolean guard_timeout(gpointer data)
+{
+	struct sap_server *server = data;
+	struct sap_connection *conn = server->conn;
+
+	if (!conn)
+		return FALSE;
+
+	DBG("conn %p state %d pr 0x%02x", conn, conn->state,
+						conn->processing_req);
+
+	conn->timer_id = 0;
+
+	switch (conn->state) {
+	case SAP_STATE_DISCONNECTED:
+		/* Client opened RFCOMM channel but didn't send CONNECT_REQ,
+		 * in fixed time or client disconnected SAP connection but
+		 * didn't closed RFCOMM channel in fixed time.*/
+		if (conn->io) {
+			g_io_channel_shutdown(conn->io, TRUE, NULL);
+			g_io_channel_unref(conn->io);
+			conn->io = NULL;
+		}
+		break;
+
+	case SAP_STATE_GRACEFUL_DISCONNECT:
+		/* Client didn't disconnect SAP connection in fixed time,
+		 * so close SAP connection immediately. */
+		disconnect_req(server, SAP_DISCONNECTION_TYPE_IMMEDIATE);
+		break;
+
+	default:
+		error("Unexpected state (%d).", conn->state);
+		break;
+	}
+
+	return FALSE;
+}
+
+static void sap_set_connected(struct sap_server *server)
+{
+	gboolean connected = TRUE;
+
+	emit_property_changed(connection, server->path,
+					SAP_SERVER_INTERFACE,
+		"Connected", DBUS_TYPE_BOOLEAN, &connected);
+
+	server->conn->state = SAP_STATE_CONNECTED;
+}
+
+int sap_connect_rsp(void *sap_device, uint8_t status)
+{
+	struct sap_server *server = sap_device;
+	struct sap_connection *conn = server->conn;
+	char buf[SAP_BUF_SIZE];
+	struct sap_message *msg = (struct sap_message *) buf;
+	struct sap_parameter *param = (struct sap_parameter *) msg->param;
+	size_t size = sizeof(struct sap_message);
+	uint16_t *maxmsgsize;
+
+	if (!conn)
+		return -EINVAL;
+
+	DBG("state %d pr 0x%02x status 0x%02x", conn->state,
+						conn->processing_req, status);
+
+	if (conn->state != SAP_STATE_CONNECT_IN_PROGRESS)
+		return -EPERM;
+
+	memset(buf, 0, sizeof(buf));
+	msg->id = SAP_CONNECT_RESP;
+	msg->nparam = 0x01;
+
+	/* Add connection status */
+	param->id = SAP_PARAM_ID_CONN_STATUS;
+	param->len = htons(SAP_PARAM_ID_CONN_STATUS_LEN);
+	*param->val = status;
+	size += PARAMETER_SIZE(SAP_PARAM_ID_CONN_STATUS_LEN);
+
+
+	switch (status) {
+	case SAP_STATUS_OK:
+		sap_set_connected(server);
+		break;
+	case SAP_STATUS_OK_ONGOING_CALL:
+		DBG("ongoing call. Wait for reset indication!");
+		conn->state = SAP_STATE_CONNECT_MODEM_BUSY;
+		break;
+	case SAP_STATUS_MAX_MSG_SIZE_NOT_SUPPORTED: /* Add MaxMsgSize */
+		msg->nparam++;
+		param = (struct sap_parameter *) &buf[size];
+		param->id = SAP_PARAM_ID_MAX_MSG_SIZE;
+		param->len = htons(SAP_PARAM_ID_MAX_MSG_SIZE_LEN);
+		maxmsgsize = (uint16_t *) &param->val;
+		*maxmsgsize = htons(SAP_BUF_SIZE);
+		size += PARAMETER_SIZE(SAP_PARAM_ID_MAX_MSG_SIZE_LEN);
+
+		/* fall */
+	default:
+		conn->state = SAP_STATE_DISCONNECTED;
+
+		/* Timer will shutdown channel if client doesn't send
+		 * CONNECT_REQ or doesn't shutdown channel itself.*/
+		start_guard_timer(server, SAP_TIMER_NO_ACTIVITY);
+		break;
+	}
+
+	conn->processing_req = SAP_NO_REQ;
+
+	return send_message(conn, buf, size);
+}
+
+int sap_disconnect_rsp(void *sap_device)
+{
+	struct sap_server *server = sap_device;
+	struct sap_connection *conn = server->conn;
+	struct sap_message msg;
+
+	if (!conn)
+		return -EINVAL;
+
+	DBG("state %d pr 0x%02x", conn->state, conn->processing_req);
+
+	switch (conn->state) {
+	case SAP_STATE_CLIENT_DISCONNECT:
+		memset(&msg, 0, sizeof(msg));
+		msg.id = SAP_DISCONNECT_RESP;
+
+		conn->state = SAP_STATE_DISCONNECTED;
+		conn->processing_req = SAP_NO_REQ;
+
+		/* Timer will close channel if client doesn't do it.*/
+		start_guard_timer(server, SAP_TIMER_NO_ACTIVITY);
+
+		return send_message(conn, &msg, sizeof(msg));
+
+	case SAP_STATE_IMMEDIATE_DISCONNECT:
+		conn->state = SAP_STATE_DISCONNECTED;
+		conn->processing_req = SAP_NO_REQ;
+
+		if (conn->io) {
+			g_io_channel_shutdown(conn->io, TRUE, NULL);
+			g_io_channel_unref(conn->io);
+			conn->io = NULL;
+		}
+
+		return 0;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+int sap_transfer_apdu_rsp(void *sap_device, uint8_t result, uint8_t *apdu,
+					uint16_t length)
+{
+	struct sap_server *server = sap_device;
+	struct sap_connection *conn = server->conn;
+	char buf[SAP_BUF_SIZE];
+	struct sap_message *msg = (struct sap_message *) buf;
+	struct sap_parameter *param = (struct sap_parameter *) msg->param;
+	size_t size = sizeof(struct sap_message);
+
+	if (!conn)
+		return -EINVAL;
+
+	SAP_VDBG("state %d pr 0x%02x", conn->state, conn->processing_req);
+
+	if (conn->processing_req != SAP_TRANSFER_APDU_REQ)
+		return 0;
+
+	if (result == SAP_RESULT_OK && (!apdu || (apdu && length == 0x00)))
+		return -EINVAL;
+
+	memset(buf, 0, sizeof(buf));
+	msg->id = SAP_TRANSFER_APDU_RESP;
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, param);
+
+	/* Add APDU response. */
+	if (result == SAP_RESULT_OK) {
+		msg->nparam++;
+		param = (struct sap_parameter *) &buf[size];
+		param->id = SAP_PARAM_ID_RESPONSE_APDU;
+		param->len = htons(length);
+
+		size += PARAMETER_SIZE(length);
+
+		if (size > SAP_BUF_SIZE)
+			return -EOVERFLOW;
+
+		memcpy(param->val, apdu, length);
+	}
+
+	conn->processing_req = SAP_NO_REQ;
+
+	return send_message(conn, buf, size);
+}
+
+int sap_transfer_atr_rsp(void *sap_device, uint8_t result, uint8_t *atr,
+					uint16_t length)
+{
+	struct sap_server *server = sap_device;
+	struct sap_connection *conn = server->conn;
+	char buf[SAP_BUF_SIZE];
+	struct sap_message *msg = (struct sap_message *) buf;
+	struct sap_parameter *param = (struct sap_parameter *) msg->param;
+	size_t size = sizeof(struct sap_message);
+
+	if (!conn)
+		return -EINVAL;
+
+	DBG("result 0x%02x state %d pr 0x%02x len %d", result, conn->state,
+			conn->processing_req, length);
+
+	if (conn->processing_req != SAP_TRANSFER_ATR_REQ)
+		return 0;
+
+	if (result == SAP_RESULT_OK && (!atr || (atr && length == 0x00)))
+		return -EINVAL;
+
+	memset(buf, 0, sizeof(buf));
+	msg->id = SAP_TRANSFER_ATR_RESP;
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, param);
+
+	/* Add ATR response */
+	if (result == SAP_RESULT_OK) {
+		msg->nparam++;
+		param = (struct sap_parameter *) &buf[size];
+		param->id = SAP_PARAM_ID_ATR;
+		param->len = htons(length);
+		size += PARAMETER_SIZE(length);
+
+		if (size > SAP_BUF_SIZE)
+			return -EOVERFLOW;
+
+		memcpy(param->val, atr, length);
+	}
+
+	conn->processing_req = SAP_NO_REQ;
+
+	return send_message(conn, buf, size);
+}
+
+int sap_power_sim_off_rsp(void *sap_device, uint8_t result)
+{
+	struct sap_server *server = sap_device;
+	struct sap_connection *conn = server->conn;
+	char buf[SAP_BUF_SIZE];
+	struct sap_message *msg = (struct sap_message *) buf;
+	size_t size = sizeof(struct sap_message);
+
+	if (!conn)
+		return -EINVAL;
+
+	DBG("state %d pr 0x%02x", conn->state, conn->processing_req);
+
+	if (conn->processing_req != SAP_POWER_SIM_OFF_REQ)
+		return 0;
+
+	memset(buf, 0, sizeof(buf));
+	msg->id = SAP_POWER_SIM_OFF_RESP;
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, msg->param);
+
+	conn->processing_req = SAP_NO_REQ;
+
+	return send_message(conn, buf, size);
+}
+
+int sap_power_sim_on_rsp(void *sap_device, uint8_t result)
+{
+	struct sap_server *server = sap_device;
+	struct sap_connection *conn = server->conn;
+	char buf[SAP_BUF_SIZE];
+	struct sap_message *msg = (struct sap_message *) buf;
+	size_t size = sizeof(struct sap_message);
+
+	if (!conn)
+		return -EINVAL;
+
+	DBG("state %d pr 0x%02x", conn->state, conn->processing_req);
+
+	if (conn->processing_req != SAP_POWER_SIM_ON_REQ)
+		return 0;
+
+	memset(buf, 0, sizeof(buf));
+	msg->id = SAP_POWER_SIM_ON_RESP;
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, msg->param);
+
+	conn->processing_req = SAP_NO_REQ;
+
+	return send_message(conn, buf, size);
+}
+
+int sap_reset_sim_rsp(void *sap_device, uint8_t result)
+{
+	struct sap_server *server = sap_device;
+	struct sap_connection *conn = server->conn;
+	char buf[SAP_BUF_SIZE];
+	struct sap_message *msg = (struct sap_message *) buf;
+	size_t size = sizeof(struct sap_message);
+
+	if (!conn)
+		return -EINVAL;
+
+	DBG("state %d pr 0x%02x result 0x%02x", conn->state,
+						conn->processing_req, result);
+
+	if (conn->processing_req != SAP_RESET_SIM_REQ)
+		return 0;
+
+	memset(buf, 0, sizeof(buf));
+	msg->id = SAP_RESET_SIM_RESP;
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, msg->param);
+
+	conn->processing_req = SAP_NO_REQ;
+
+	return send_message(conn, buf, size);
+}
+
+int sap_transfer_card_reader_status_rsp(void *sap_device, uint8_t result,
+						uint8_t status)
+{
+	struct sap_server *server = sap_device;
+	struct sap_connection *conn = server->conn;
+	char buf[SAP_BUF_SIZE];
+	struct sap_message *msg = (struct sap_message *) buf;
+	struct sap_parameter *param = (struct sap_parameter *) msg->param;
+	size_t size = sizeof(struct sap_message);
+
+	if (!conn)
+		return -EINVAL;
+
+	DBG("state %d pr 0x%02x result 0x%02x", conn->state,
+						conn->processing_req, result);
+
+	if (conn->processing_req != SAP_TRANSFER_CARD_READER_STATUS_REQ)
+		return 0;
+
+	memset(buf, 0, sizeof(buf));
+	msg->id = SAP_TRANSFER_CARD_READER_STATUS_RESP;
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, param);
+
+	/* Add card reader status. */
+	if (result == SAP_RESULT_OK) {
+		msg->nparam++;
+		param = (struct sap_parameter *) &buf[size];
+		param->id = SAP_PARAM_ID_CARD_READER_STATUS;
+		param->len = htons(SAP_PARAM_ID_CARD_READER_STATUS_LEN);
+		*param->val = status;
+		size += PARAMETER_SIZE(SAP_PARAM_ID_CARD_READER_STATUS_LEN);
+	}
+
+	conn->processing_req = SAP_NO_REQ;
+
+	return send_message(conn, buf, size);
+}
+
+int sap_transport_protocol_rsp(void *sap_device, uint8_t result)
+{
+	struct sap_server *server = sap_device;
+	struct sap_connection *conn = server->conn;
+	char buf[SAP_BUF_SIZE];
+	struct sap_message *msg = (struct sap_message *) buf;
+	size_t size = sizeof(struct sap_message);
+
+	if (!conn)
+		return -EINVAL;
+
+	DBG("state %d pr 0x%02x result 0x%02x", conn->state,
+						conn->processing_req, result);
+
+	if (conn->processing_req != SAP_SET_TRANSPORT_PROTOCOL_REQ)
+		return 0;
+
+	memset(buf, 0, sizeof(buf));
+	msg->id = SAP_SET_TRANSPORT_PROTOCOL_RESP;
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, msg->param);
+
+	conn->processing_req = SAP_NO_REQ;
+
+	return send_message(conn, buf, size);
+}
+
+int sap_status_ind(void *sap_device, uint8_t status_change)
+{
+	struct sap_server *server = sap_device;
+	struct sap_connection *conn = server->conn;
+	char buf[SAP_BUF_SIZE];
+	struct sap_message *msg = (struct sap_message *) buf;
+	struct sap_parameter *param = (struct sap_parameter *) msg->param;
+	size_t size = sizeof(struct sap_message);
+
+	if (!conn)
+		return -EINVAL;
+
+	DBG("state %d pr 0x%02x sc 0x%02x", conn->state, conn->processing_req,
+								status_change);
+
+	switch (conn->state) {
+	case SAP_STATE_CONNECT_MODEM_BUSY:
+		if (status_change != SAP_STATUS_CHANGE_CARD_RESET)
+			break;
+
+		/* Change state to connected after ongoing call ended */
+		sap_set_connected(server);
+		/* fall */
+	case SAP_STATE_CONNECTED:
+	case SAP_STATE_GRACEFUL_DISCONNECT:
+		memset(buf, 0, sizeof(buf));
+		msg->id = SAP_STATUS_IND;
+		msg->nparam = 0x01;
+
+		/* Add status change. */
+		param->id  = SAP_PARAM_ID_STATUS_CHANGE;
+		param->len = htons(SAP_PARAM_ID_STATUS_CHANGE_LEN);
+		*param->val = status_change;
+		size += PARAMETER_SIZE(SAP_PARAM_ID_STATUS_CHANGE_LEN);
+
+		return send_message(conn, buf, size);
+	case SAP_STATE_DISCONNECTED:
+	case SAP_STATE_CONNECT_IN_PROGRESS:
+	case SAP_STATE_IMMEDIATE_DISCONNECT:
+	case SAP_STATE_CLIENT_DISCONNECT:
+		break;
+	}
+
+	return 0;
+}
+
+int sap_disconnect_ind(void *sap_device, uint8_t disc_type)
+{
+	return disconnect_req(sap_device, SAP_DISCONNECTION_TYPE_IMMEDIATE);
+}
+
+static int handle_cmd(struct sap_server *server, void *buf, size_t size)
+{
+	struct sap_connection *conn = server->conn;
+	struct sap_message *msg = buf;
+
+	if (!conn)
+		return -EINVAL;
+
+	if (size < sizeof(struct sap_message))
+		goto error_rsp;
+
+	if (msg->nparam != 0 && size < (sizeof(struct sap_message) +
+					sizeof(struct sap_parameter) + 4))
+		goto error_rsp;
+
+	if (check_msg(msg) < 0)
+		goto error_rsp;
+
+	switch (msg->id) {
+	case SAP_CONNECT_REQ:
+		connect_req(server, msg->param);
+		return 0;
+	case SAP_DISCONNECT_REQ:
+		disconnect_req(server, SAP_DISCONNECTION_TYPE_CLIENT);
+		return 0;
+	case SAP_TRANSFER_APDU_REQ:
+		transfer_apdu_req(server, msg->param);
+		return 0;
+	case SAP_TRANSFER_ATR_REQ:
+		transfer_atr_req(server);
+		return 0;
+	case SAP_POWER_SIM_OFF_REQ:
+		power_sim_off_req(server);
+		return 0;
+	case SAP_POWER_SIM_ON_REQ:
+		power_sim_on_req(server);
+		return 0;
+	case SAP_RESET_SIM_REQ:
+		reset_sim_req(server);
+		return 0;
+	case SAP_TRANSFER_CARD_READER_STATUS_REQ:
+		transfer_card_reader_status_req(server);
+		return 0;
+	case SAP_SET_TRANSPORT_PROTOCOL_REQ:
+		set_transport_protocol_req(server, msg->param);
+		return 0;
+	default:
+		DBG("Unknown SAP message id 0x%02x.", msg->id);
+		break;
+	}
+
+error_rsp:
+	DBG("Invalid SAP message format.");
+	sap_error_rsp(conn);
+	return -EBADMSG;
+}
+
+static void sap_server_remove_conn(struct sap_server *server)
+{
+	struct sap_connection *conn = server->conn;
+
+	DBG("conn %p", conn);
+
+	if (!conn)
+		return;
+
+	if (conn->io) {
+		g_io_channel_shutdown(conn->io, TRUE, NULL);
+		g_io_channel_unref(conn->io);
+	}
+
+	g_free(conn);
+	server->conn = NULL;
+}
+
+static gboolean sap_io_cb(GIOChannel *io, GIOCondition cond, gpointer data)
+{
+	char buf[SAP_BUF_SIZE];
+	size_t bytes_read = 0;
+	GError *gerr = NULL;
+	GIOStatus gstatus;
+
+	SAP_VDBG("conn %p io %p", conn, io);
+
+	if (cond & G_IO_NVAL) {
+		DBG("ERR (G_IO_NVAL) on rfcomm socket.");
+		return FALSE;
+	}
+
+	if (cond & G_IO_ERR) {
+		DBG("ERR (G_IO_ERR) on rfcomm socket.");
+		return FALSE;
+	}
+
+	if (cond & G_IO_HUP) {
+		DBG("HUP on rfcomm socket.");
+		return FALSE;
+	}
+
+	gstatus = g_io_channel_read_chars(io, buf, sizeof(buf) - 1,
+							&bytes_read, &gerr);
+	if (gstatus != G_IO_STATUS_NORMAL) {
+		if (gerr)
+			g_error_free(gerr);
+
+		return TRUE;
+	}
+
+	if (handle_cmd(data, buf, bytes_read) < 0)
+		error("SAP protocol processing failure.");
+
+	return TRUE;
+}
+
+static void sap_io_destroy(void *data)
+{
+	struct sap_server *server = data;
+	struct sap_connection *conn = server->conn;
+	gboolean connected = FALSE;
+
+	DBG("conn %p", conn);
+
+	if (!conn || !conn->io)
+		return;
+
+	stop_guard_timer(server);
+
+	if (conn->state != SAP_STATE_CONNECT_IN_PROGRESS &&
+			conn->state != SAP_STATE_CONNECT_MODEM_BUSY)
+		emit_property_changed(connection, server->path,
+					SAP_SERVER_INTERFACE, "Connected",
+					DBUS_TYPE_BOOLEAN, &connected);
+
+	if (conn->state == SAP_STATE_CONNECT_IN_PROGRESS ||
+			conn->state == SAP_STATE_CONNECT_MODEM_BUSY ||
+			conn->state == SAP_STATE_CONNECTED ||
+			conn->state == SAP_STATE_GRACEFUL_DISCONNECT)
+		sap_disconnect_req(server, 1);
+
+	sap_server_remove_conn(server);
+}
+
+static void sap_connect_cb(GIOChannel *io, GError *gerr, gpointer data)
+{
+	struct sap_server *server = data;
+	struct sap_connection *conn = server->conn;
+
+	DBG("conn %p, io %p", conn, io);
+
+	if (!conn)
+		return;
+
+	/* Timer will shutdown the channel in case of lack of client
+	   activity */
+	start_guard_timer(server, SAP_TIMER_NO_ACTIVITY);
+
+	g_io_add_watch_full(io, G_PRIORITY_DEFAULT,
+			G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+			sap_io_cb, server, sap_io_destroy);
+}
+
+static void connect_auth_cb(DBusError *derr, void *data)
+{
+	struct sap_server *server = data;
+	struct sap_connection *conn = server->conn;
+	GError *gerr = NULL;
+
+	DBG("conn %p", conn);
+
+	if (!conn)
+		return;
+
+	if (derr && dbus_error_is_set(derr)) {
+		error("Access has been denied (%s)", derr->message);
+		sap_server_remove_conn(server);
+		return;
+	}
+
+	if (!bt_io_accept(conn->io, sap_connect_cb, server, NULL, &gerr)) {
+		error("bt_io_accept: %s", gerr->message);
+		g_error_free(gerr);
+		sap_server_remove_conn(server);
+		return;
+	}
+
+	DBG("Access has been granted.");
+}
+
+static void connect_confirm_cb(GIOChannel *io, gpointer data)
+{
+	struct sap_server *server = data;
+	struct sap_connection *conn = server->conn;
+	GError *gerr = NULL;
+	bdaddr_t src, dst;
+	char dstaddr[18];
+	int err;
+
+	DBG("conn %p io %p", conn, io);
+
+	if (!io)
+		return;
+
+	if (conn) {
+		DBG("Another SAP connection already exists.");
+		g_io_channel_shutdown(io, TRUE, NULL);
+		return;
+	}
+
+	conn = g_try_new0(struct sap_connection, 1);
+	if (!conn) {
+		error("Can't allocate memory for incoming SAP connection.");
+		g_io_channel_shutdown(io, TRUE, NULL);
+		return;
+	}
+
+	g_io_channel_set_encoding(io, NULL, NULL);
+	g_io_channel_set_buffered(io, FALSE);
+
+	server->conn = conn;
+	conn->io = g_io_channel_ref(io);
+	conn->state = SAP_STATE_DISCONNECTED;
+
+	bt_io_get(io, BT_IO_RFCOMM, &gerr,
+			BT_IO_OPT_SOURCE_BDADDR, &src,
+			BT_IO_OPT_DEST_BDADDR, &dst,
+			BT_IO_OPT_INVALID);
+	if (gerr) {
+		error("%s", gerr->message);
+		g_error_free(gerr);
+		sap_server_remove_conn(server);
+		return;
+	}
+
+	ba2str(&dst, dstaddr);
+
+	err = btd_request_authorization(&src, &dst, SAP_UUID, connect_auth_cb,
+								server);
+	if (err < 0) {
+		error("Authorization failure (err %d)", err);
+		sap_server_remove_conn(server);
+		return;
+	}
+
+	DBG("Authorizing incoming SAP connection from %s", dstaddr);
+}
+
+static inline DBusMessage *message_failed(DBusMessage *msg,
+					const char *description)
+{
+	return g_dbus_create_error(msg, ERROR_INTERFACE ".Failed", "%s",
+								description);
+}
+
+static DBusMessage *disconnect(DBusConnection *conn, DBusMessage *msg,
+								void *data)
+{
+	struct sap_server *server = data;
+
+	if (!server)
+		return message_failed(msg, "Server internal error.");
+
+	DBG("conn %p", server->conn);
+
+	if (!server->conn)
+		return message_failed(msg, "Client already disconnected");
+
+	if (disconnect_req(server, SAP_DISCONNECTION_TYPE_GRACEFUL) < 0)
+		return g_dbus_create_error(msg, ERROR_INTERFACE ".Failed",
+					"There is no active connection");
+
+	return dbus_message_new_method_return(msg);
+}
+
+static DBusMessage *get_properties(DBusConnection *c,
+				DBusMessage *msg, void *data)
+{
+	struct sap_connection *conn = data;
+	DBusMessage *reply;
+	DBusMessageIter iter;
+	DBusMessageIter dict;
+	dbus_bool_t connected;
+
+	if (!conn)
+		return message_failed(msg, "Server internal error.");
+
+	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,
+			DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING
+			DBUS_TYPE_STRING_AS_STRING DBUS_TYPE_VARIANT_AS_STRING
+			DBUS_DICT_ENTRY_END_CHAR_AS_STRING, &dict);
+
+	connected = (conn->state == SAP_STATE_CONNECTED ||
+				conn->state == SAP_STATE_GRACEFUL_DISCONNECT);
+	dict_append_entry(&dict, "Connected", DBUS_TYPE_BOOLEAN, &connected);
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+static const GDBusMethodTable server_methods[] = {
+	{ GDBUS_METHOD("GetProperties",
+			NULL, GDBUS_ARGS({ "properties", "a{sv}" }),
+			get_properties) },
+	{ GDBUS_METHOD("Disconnect", NULL, NULL, disconnect) },
+	{ }
+};
+
+static const GDBusSignalTable server_signals[] = {
+	{ GDBUS_SIGNAL("PropertyChanged",
+			GDBUS_ARGS({ "name", "s" }, { "value", "v" })) },
+	{ }
+};
+
+static void server_remove(struct sap_server *server)
+{
+	if (!server)
+		return;
+
+	sap_server_remove_conn(server);
+
+	remove_record_from_server(server->record_id);
+
+	if (server->listen_io) {
+		g_io_channel_shutdown(server->listen_io, TRUE, NULL);
+		g_io_channel_unref(server->listen_io);
+		server->listen_io = NULL;
+	}
+
+	g_free(server->path);
+	g_free(server);
+}
+
+static void destroy_sap_interface(void *data)
+{
+	struct sap_server *server = data;
+
+	DBG("Unregistered interface %s on path %s", SAP_SERVER_INTERFACE,
+								server->path);
+
+	server_remove(server);
+}
+
+int sap_server_register(const char *path, bdaddr_t *src)
+{
+	sdp_record_t *record = NULL;
+	GError *gerr = NULL;
+	GIOChannel *io;
+	struct sap_server *server;
+
+	if (sap_init() < 0) {
+		error("Sap driver initialization failed.");
+		return -1;
+	}
+
+	record = create_sap_record(SAP_SERVER_CHANNEL);
+	if (!record) {
+		error("Creating SAP SDP record failed.");
+		goto sdp_err;
+	}
+
+	if (add_record_to_server(src, record) < 0) {
+		error("Adding SAP SDP record to the SDP server failed.");
+		sdp_record_free(record);
+		goto sdp_err;
+	}
+
+	server = g_new0(struct sap_server, 1);
+	server->path = g_strdup(path);
+	server->record_id = record->handle;
+
+	io = bt_io_listen(BT_IO_RFCOMM, NULL, connect_confirm_cb, server,
+			NULL, &gerr,
+			BT_IO_OPT_SOURCE_BDADDR, src,
+			BT_IO_OPT_CHANNEL, SAP_SERVER_CHANNEL,
+			BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_HIGH,
+			BT_IO_OPT_MASTER, TRUE,
+			BT_IO_OPT_INVALID);
+	if (!io) {
+		error("Can't listen at channel %d.", SAP_SERVER_CHANNEL);
+		g_error_free(gerr);
+		goto server_err;
+	}
+	server->listen_io = io;
+
+	if (!g_dbus_register_interface(connection, server->path,
+					SAP_SERVER_INTERFACE,
+					server_methods, server_signals, NULL,
+					server, destroy_sap_interface)) {
+		error("D-Bus failed to register %s interface",
+							SAP_SERVER_INTERFACE);
+		goto server_err;
+	}
+
+	DBG("server %p, listen socket 0x%02x", server,
+						g_io_channel_unix_get_fd(io));
+
+	return 0;
+
+server_err:
+	server_remove(server);
+sdp_err:
+	sap_exit();
+
+	return -1;
+}
+
+void sap_server_unregister(const char *path)
+{
+	g_dbus_unregister_interface(connection, path, SAP_SERVER_INTERFACE);
+
+	sap_exit();
+}
+
+int sap_server_init(DBusConnection *conn)
+{
+	connection = dbus_connection_ref(conn);
+	return 0;
+}
+
+void sap_server_exit(void)
+{
+	dbus_connection_unref(connection);
+	connection = NULL;
+}
diff --git a/profiles/sap/server.h b/profiles/sap/server.h
new file mode 100644
index 000000000..6d2f5e9eb
--- /dev/null
+++ b/profiles/sap/server.h
@@ -0,0 +1,26 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 ST-Ericsson SA
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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 <gdbus.h>
+
+int sap_server_init(DBusConnection *conn);
+void sap_server_exit(void);
+int sap_server_register(const char *path, bdaddr_t *src);
+void sap_server_unregister(const char *path);