Re: Sim Access profile server implementation

Re: Sim Access profile server implementation

[Suraj Sumangala]

Hi Waldek,

On 10/6/2010 3:02 PM, Waldemar.Rymarkiewicz@tieto.com wrote:
>
> Hi Marcel,
>
>>>
>>> Can you share that code with us. And also hardware if you. We
>> are still
>>> having hard time to find proper hardware to test this on.
>>
>> Will send a patch soon. I use
>> http://www.stericsson.com/platforms/U8500.jsp hw, but need to
>> check if I can share one with you.
>>
>
> Here you are the patch. Just note that it is based on an old bluez 4.4x
> If you consider to accept the design I will update the patch upon the latest release.
>
> Will appreciate any comments.
>
> Thanks,
> /Waldek


Which part of this is the driver and which is the agent here?

If I am not mistaken, you are sending the responses directly from code 
here, how can we extend this to another hardware/agent without source 
code change?

I don't see D-bus being used other than for set/get property.

Correct me if my understanding is wrong.

Regards
Suraj

Sim Access profile server implementation

[suraj]

Hi,

Please find the Git tree for the Sim access profile server role
implementation I have been working on at

git://gitorious.org/sap-server/sap-server.git

Not sure it could be directly used with Bluez. Nevertheless, it has the
parser implementation of Bluetooth SAP packet. Someone could possibly
reuse it in their implementation.

Regards
Suraj

Re: Sim Access profile server implementation

[Marcel Holtmann]

Hi Suraj,

> Please find the Git tree for the Sim access profile server role
> implementation I have been working on at
> 
> git://gitorious.org/sap-server/sap-server.git
> 
> Not sure it could be directly used with Bluez. Nevertheless, it has the
> parser implementation of Bluetooth SAP packet. Someone could possibly
> reuse it in their implementation.

have you actually tested this with real SAP server capable hardware? I
am still not convinced at all to push APDU over D-Bus. First, there is
potential security issue here and second the latency that D-Bus
introduces seems not be acceptable. So what I am getting it, can this be
actually used in a product that seriously wants to support SAP server?

Regards

Marcel

Re: Sim Access profile server implementation

[Suraj Sumangala]

Hi Marcel,

On 10/5/2010 1:32 PM, Marcel Holtmann wrote:
> Hi Suraj,
>
>> Please find the Git tree for the Sim access profile server role
>> implementation I have been working on at
>>
>> git://gitorious.org/sap-server/sap-server.git
>>
>> Not sure it could be directly used with Bluez. Nevertheless, it has the
>> parser implementation of Bluetooth SAP packet. Someone could possibly
>> reuse it in their implementation.
>
> have you actually tested this with real SAP server capable hardware? I
> am still not convinced at all to push APDU over D-Bus. First, there is
> potential security issue here and second the latency that D-Bus
> introduces seems not be acceptable. So what I am getting it, can this be
> actually used in a product that seriously wants to support SAP server?

No, This was tested with a dummy Agent and PTS. I am yet to get hold of 
a hardware.

The reason for sharing the source code was because any later SAP server 
implementation can re-use the SAP packet parser implementation I have 
already done.

I think we should be able to change the interface later depending on the 
actual hardware/system requirement. But, I believe the internal 
implementation could stay the same.

>
> Regards
>
> Marcel
>
>

Regards
Suraj

RE: Sim Access profile server implementation

[Waldemar.Rymarkiewicz]

Hi Marcel, 

I understand you concerns about dbus reliability and I agree with you , but I guess we should find a compromise solution as ofono is not used widely in Linux mobile platforms nowadays.

I my view, combination of a bluez plugin for sap and a platform dependend sim driver is a reliable and most available solution so far. The SAP plugin implements BT SAP spec and require simple API (connect, disconnect, apdu, atr, status) which is implemented in SIM driver for a certain platform. This way it's relatively easy to support SAP in different platforms. Ofono can have its own driver as well.

What's you view on such design?
Can we also have a double solution one in ofono for ofono aware platforms and the second as a plugin for others? 

BTW, I already have this implemented and tested with real hardware (STEricsson). Bluez SAP works fine with Nokia 616 carkit :)

Regards,
/Waldek

>-----Original Message-----
>From: linux-bluetooth-owner@vger.kernel.org 
>[mailto:linux-bluetooth-owner@vger.kernel.org] On Behalf Of 
>Marcel Holtmann
>Sent: Tuesday, October 05, 2010 10:02 AM
>To: suraj
>Cc: linux-bluetooth@vger.kernel.org; Jothikumar Mothilal
>Subject: Re: Sim Access profile server implementation
>
>Hi Suraj,
>
>> Please find the Git tree for the Sim access profile server role 
>> implementation I have been working on at
>> 
>> git://gitorious.org/sap-server/sap-server.git
>> 
>> Not sure it could be directly used with Bluez. Nevertheless, it has 
>> the parser implementation of Bluetooth SAP packet. Someone could 
>> possibly reuse it in their implementation.
>
>have you actually tested this with real SAP server capable 
>hardware? I am still not convinced at all to push APDU over 
>D-Bus. First, there is potential security issue here and 
>second the latency that D-Bus introduces seems not be 
>acceptable. So what I am getting it, can this be actually used 
>in a product that seriously wants to support SAP server?
>
>Regards
>
>Marcel
>
>
>--
>To unsubscribe from this list: send the line "unsubscribe 
>linux-bluetooth" in the body of a message to 
>majordomo@vger.kernel.org More majordomo info at  
>http://vger.kernel.org/majordomo-info.html
>

RE: Sim Access profile server implementation

[Marcel Holtmann]

Hi Waldemar,

please no top posting on this mailing list.

> I understand you concerns about dbus reliability and I agree with you , but I guess we should find a compromise solution as ofono is not used widely in Linux mobile platforms nowadays.
> 
> I my view, combination of a bluez plugin for sap and a platform dependend sim driver is a reliable and most available solution so far. The SAP plugin implements BT SAP spec and require simple API (connect, disconnect, apdu, atr, status) which is implemented in SIM driver for a certain platform. This way it's relatively easy to support SAP in different platforms. Ofono can have its own driver as well.
> 
> What's you view on such design?
> Can we also have a double solution one in ofono for ofono aware platforms and the second as a plugin for others? 

I am fine with starting this in BlueZ and see how far we get. And yes,
plugin based is a must from my point of view. It is most likely similar
to our different telephony drivers for Handsfree support.

We will move strongly in the direction of oFono being the main telephony
stack. So I do care mostly that this work in conjunction with oFono.
Everything else is just a nice benefit if it doesn't clutter the
overhaul implementation and requires pointless abstraction everywhere.

> BTW, I already have this implemented and tested with real hardware (STEricsson). Bluez SAP works fine with Nokia 616 carkit :)

Can you share that code with us. And also hardware if you. We are still
having hard time to find proper hardware to test this on.

Regards

Marcel

Re: Sim Access profile server implementation

[Suraj Sumangala]

Hi Waldemar,

On 10/5/2010 5:42 PM, Marcel Holtmann wrote:
> Hi Waldemar,
>
> please no top posting on this mailing list.
>
>> I understand you concerns about dbus reliability and I agree with you , but I guess we should find a compromise solution as ofono is not used widely in Linux mobile platforms nowadays.
>>
>> I my view, combination of a bluez plugin for sap and a platform dependend sim driver is a reliable and most available solution so far. The SAP plugin implements BT SAP spec and require simple API (connect, disconnect, apdu, atr, status) which is implemented in SIM driver for a certain platform. This way it's relatively easy to support SAP in different platforms. Ofono can have its own driver as well.
>>
>> What's you view on such design?
>> Can we also have a double solution one in ofono for ofono aware platforms and the second as a plugin for others?
>
> I am fine with starting this in BlueZ and see how far we get. And yes,
> plugin based is a must from my point of view. It is most likely similar
> to our different telephony drivers for Handsfree support.
>
> We will move strongly in the direction of oFono being the main telephony
> stack. So I do care mostly that this work in conjunction with oFono.
> Everything else is just a nice benefit if it doesn't clutter the
> overhaul implementation and requires pointless abstraction everywhere.
>
>> BTW, I already have this implemented and tested with real hardware (STEricsson). Bluez SAP works fine with Nokia 616 carkit :)
>
> Can you share that code with us. And also hardware if you. We are still
> having hard time to find proper hardware to test this on.
>
> Regards
>
> Marcel
>
>

Do let me know if you had to make some changes in the SAP implementation 
for it to work.

It will be great if you can share the code in that case. I can update my 
implementation accordingly.

Regards
Suraj

RE: Sim Access profile server implementation

[Waldemar.Rymarkiewicz]

Marcel,

>
>please no top posting on this mailing list.
>

Sorry, didn't know that.  Thanks for pointing this out.

>> I understand you concerns about dbus reliability and I agree=20
>with you , but I guess we should find a compromise solution as=20
>ofono is not used widely in Linux mobile platforms nowadays.
>>=20
>> I my view, combination of a bluez plugin for sap and a=20
>platform dependend sim driver is a reliable and most available=20
>solution so far. The SAP plugin implements BT SAP spec and=20
>require simple API (connect, disconnect, apdu, atr, status)=20
>which is implemented in SIM driver for a certain platform.=20
>This way it's relatively easy to support SAP in different=20
>platforms. Ofono can have its own driver as well.
>>=20
>> What's you view on such design?
>> Can we also have a double solution one in ofono for ofono=20
>aware platforms and the second as a plugin for others?=20
>
>I am fine with starting this in BlueZ and see how far we get.=20
>And yes, plugin based is a must from my point of view. It is=20
>most likely similar to our different telephony drivers for=20
>Handsfree support.

It's nice to here that.

>We will move strongly in the direction of oFono being the main=20
>telephony stack. So I do care mostly that this work in=20
>conjunction with oFono.
>Everything else is just a nice benefit if it doesn't clutter=20
>the overhaul implementation and requires pointless abstraction=20
>everywhere.
>> BTW, I already have this implemented and tested with real hardware=20
>> (STEricsson). Bluez SAP works fine with Nokia 616 carkit :)
>
>Can you share that code with us. And also hardware if you. We=20
>are still having hard time to find proper hardware to test this on.

Will send a patch soon. I use http://www.stericsson.com/platforms/U8500.jsp=
 hw, but need to check if I can share one with you.

Regards,
/Waldek=

RE: Sim Access profile server implementation

[Waldemar.Rymarkiewicz]

[-- Attachment #1: Type: text/plain, Size: 516 bytes --]


Hi Marcel,

>>
>>Can you share that code with us. And also hardware if you. We 
>are still 
>>having hard time to find proper hardware to test this on.
>
>Will send a patch soon. I use 
>http://www.stericsson.com/platforms/U8500.jsp hw, but need to 
>check if I can share one with you.
>

Here you are the patch. Just note that it is based on an old bluez 4.4x
If you consider to accept the design I will update the patch upon the latest release. 

Will appreciate any comments.

Thanks,
/Waldek 

[-- Attachment #2: 0001-Add-Sim-Access-plugin.patch --]
[-- Type: application/octet-stream, Size: 95321 bytes --]

From 12f560c28fd3fe926d10639156e10d089dd26506 Mon Sep 17 00:00:00 2001
From: Waldemar Rymarkiewicz <waldemar.rymarkiewicz@tieto.com>
Date: Tue, 28 Sep 2010 17:24:19 +0200
Subject: [PATCH] Add Sim Access plugin

Sim Access plugin implements Sim Access Profile server role
according to the Bluetooth Sim Access Profile v1.1 specification.

Change-Id: I55e86dda969ae18339a134686ebf0b31a74ff335
---
 Makefile.am     |    2 +-
 acinclude.m4    |   13 +
 configure.ac    |    1 +
 sap/Android.mk  |   33 ++
 sap/Makefile.am |   25 +
 sap/main.c      |   57 +++
 sap/manager.c   |   98 ++++
 sap/manager.h   |   23 +
 sap/sap-dummy.c |   76 +++
 sap/sap-ste.c   | 1309 +++++++++++++++++++++++++++++++++++++++++++++++++
 sap/sap.h       |  200 ++++++++
 sap/server.c    | 1459 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 sap/server.h    |   25 +
 13 files changed, 3320 insertions(+), 1 deletions(-)
 mode change 100644 => 100755 Makefile.am
 mode change 100644 => 100755 acinclude.m4
 mode change 100644 => 100755 configure.ac
 create mode 100644 sap/Android.mk
 create mode 100644 sap/Makefile.am
 create mode 100644 sap/main.c
 create mode 100644 sap/manager.c
 create mode 100644 sap/manager.h
 create mode 100644 sap/sap-dummy.c
 create mode 100644 sap/sap-ste.c
 create mode 100644 sap/sap.h
 create mode 100644 sap/server.c
 create mode 100644 sap/server.h

diff --git a/Makefile.am b/Makefile.am
old mode 100644
new mode 100755
index c4c6322..acea6bb
--- a/Makefile.am
+++ b/Makefile.am
@@ -1,6 +1,6 @@
 
 SUBDIRS = include lib sbc gdbus common plugins src client\
-			network serial input audio tools \
+			network serial input audio tools sap\
 			rfcomm compat cups test scripts doc
 
 EXTRA_DIST = bluez.m4
diff --git a/acinclude.m4 b/acinclude.m4
old mode 100644
new mode 100755
index 248fed3..68962bc
--- a/acinclude.m4
+++ b/acinclude.m4
@@ -197,6 +197,7 @@ AC_DEFUN([AC_ARG_BLUEZ], [
 	input_enable=yes
 	serial_enable=yes
 	network_enable=yes
+	sap_enable=yes
 	service_enable=yes
 	tools_enable=yes
 	hidd_enable=no
@@ -212,6 +213,7 @@ AC_DEFUN([AC_ARG_BLUEZ], [
 	udevrules_enable=yes
 	configfiles_enable=yes
 	telephony_driver=dummy
+	sap_driver=dummy
 
 	AC_ARG_ENABLE(optimization, AC_HELP_STRING([--disable-optimization], [disable code optimization]), [
 		optimization_enable=${enableval}
@@ -241,6 +243,10 @@ AC_DEFUN([AC_ARG_BLUEZ], [
 		audio_enable=${enableval}
 	])
 
+	AC_ARG_ENABLE(sap, AC_HELP_STRING([--disable-sap], [disable sap plugin]), [
+		sap_enable=${enableval}
+	])
+
 	AC_ARG_ENABLE(service, AC_HELP_STRING([--disable-service], [disable service plugin]), [
 		service_enable=${enableval}
 	])
@@ -323,6 +329,12 @@ AC_DEFUN([AC_ARG_BLUEZ], [
 
 	AC_SUBST([TELEPHONY_DRIVER], [telephony-${telephony_driver}.c])
 
+	AC_ARG_WITH(sap, AC_HELP_STRING([--with-sap=DRIVER], [select SIM Access driver]), [
+		sap_driver=${withval}
+	])
+
+	AC_SUBST([SAP_DRIVER], [sap-${sap_driver}.c])
+
 	if (test "${fortify_enable}" = "yes"); then
 		CFLAGS="$CFLAGS -D_FORTIFY_SOURCE=2"
 	fi
@@ -363,6 +375,7 @@ AC_DEFUN([AC_ARG_BLUEZ], [
 	AM_CONDITIONAL(INPUTPLUGIN, test "${input_enable}" = "yes")
 	AM_CONDITIONAL(SERIALPLUGIN, test "${serial_enable}" = "yes")
 	AM_CONDITIONAL(NETWORKPLUGIN, test "${network_enable}" = "yes")
+	AM_CONDITIONAL(SAPPLUGIN, test "${sap_enable}" = "yes")
 	AM_CONDITIONAL(SERVICEPLUGIN, test "${service_enable}" = "yes")
 	AM_CONDITIONAL(HIDD, test "${hidd_enable}" = "yes")
 	AM_CONDITIONAL(PAND, test "${pand_enable}" = "yes")
diff --git a/configure.ac b/configure.ac
old mode 100644
new mode 100755
index 85e1dae..512d032
--- a/configure.ac
+++ b/configure.ac
@@ -71,4 +71,5 @@ AC_OUTPUT([
 	src/bluetoothd.8
 	src/hcid.conf.5
 	bluez.pc
+	sap/Makefile
 ])
diff --git a/sap/Android.mk b/sap/Android.mk
new file mode 100644
index 0000000..d46837a
--- /dev/null
+++ b/sap/Android.mk
@@ -0,0 +1,33 @@
+LOCAL_PATH:= $(call my-dir)
+
+include $(CLEAR_VARS)
+
+LOCAL_SRC_FILES:= \
+	manager.c \
+	server.c \
+	main.c \
+	sap-ste.c
+
+LOCAL_CFLAGS:= \
+	-DVERSION=\"4.47\"
+
+LOCAL_C_INCLUDES:= \
+	$(LOCAL_PATH)/../include \
+	$(LOCAL_PATH)/../common \
+	$(LOCAL_PATH)/../gdbus \
+	$(LOCAL_PATH)/../src \
+	$(call include-path-for, glib) \
+	$(call include-path-for, dbus)
+
+LOCAL_SHARED_LIBRARIES := \
+	libbluetoothd \
+	libbluetooth \
+	libdbus
+
+LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/bluez-plugin
+LOCAL_UNSTRIPPED_PATH := $(TARGET_OUT_SHARED_LIBRARIES_UNSTRIPPED)/bluez-plugin
+LOCAL_MODULE := sap
+LOCAL_PRELINK_MODULE := false
+
+include $(BUILD_SHARED_LIBRARY)
+
diff --git a/sap/Makefile.am b/sap/Makefile.am
new file mode 100644
index 0000000..2166cfc
--- /dev/null
+++ b/sap/Makefile.am
@@ -0,0 +1,25 @@
+if SAPPLUGIN
+plugindir = $(libdir)/bluetooth/plugins
+
+plugin_LTLIBRARIES = sap.la
+
+sap_la_SOURCES = server.c manager.h sap.c \
+		server.h manager.c \
+		sap.h main.c
+
+LDADD = $(top_builddir)/common/libhelper.a \
+		@GDBUS_LIBS@ @GLIB_LIBS@ @DBUS_LIBS@ @BLUEZ_LIBS@
+endif
+
+AM_LDFLAGS = -module -avoid-version -no-undefined
+
+AM_CFLAGS = -fvisibility=hidden \
+		@BLUEZ_CFLAGS@ @DBUS_CFLAGS@ @GLIB_CFLAGS@ @GDBUS_CFLAGS@
+
+INCLUDES = -I$(top_srcdir)/common -I$(top_srcdir)/src
+
+MAINTAINERCLEANFILES = Makefile.in
+
+sap.c: @SAP_DRIVER@
+	@if [ ! -e $@ ] ; then $(LN_S) $< $@ ; fi
+
diff --git a/sap/main.c b/sap/main.c
new file mode 100644
index 0000000..541951b
--- /dev/null
+++ b/sap/main.c
@@ -0,0 +1,57 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 ST-Ericsson SA
+ *  Copyright (C) 2010 Claudio Takahasi<claudio.takahasi@openbossa.org>
+ *
+ *  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.
+ *
+ *  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 == NULL)
+		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/sap/manager.c b/sap/manager.c
new file mode 100644
index 0000000..4a249a9
--- /dev/null
+++ b/sap/manager.c
@@ -0,0 +1,98 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 ST-Ericsson SA
+ *  Copyright (C) 2010 Claudio Takahasi<claudio.takahasi@openbossa.org>
+ *
+ *  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.
+ *
+ *  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 <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+
+#include <gdbus.h>
+
+#include "logging.h"
+#include "adapter.h"
+#include "device.h"
+
+#include "manager.h"
+
+#include "server.h"
+
+#define SAP_UUID_STR		"0000112D-0000-1000-8000-00805F9B34FB"
+
+static DBusConnection *connection = NULL;
+
+
+static int sap_server_probe(struct btd_adapter *adapter)
+{
+	const gchar *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 gchar *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/sap/manager.h b/sap/manager.h
new file mode 100644
index 0000000..5c1c02a
--- /dev/null
+++ b/sap/manager.h
@@ -0,0 +1,23 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 ST-Ericsson SA
+ *  Copyright (C) 2010 Claudio Takahasi<claudio.takahasi@openbossa.org>
+ *
+ *  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.
+ *
+ *  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/sap/sap-dummy.c b/sap/sap-dummy.c
new file mode 100644
index 0000000..6a93900
--- /dev/null
+++ b/sap/sap-dummy.c
@@ -0,0 +1,76 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 ST-Ericsson SA
+ *  Copyright (C) 2010 Claudio Takahasi<claudio.takahasi@openbossa.org>
+ *
+ *  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.
+ *
+ *  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 <unistd.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#include "sap.h"
+
+void sap_connect_req(void *sap_device, uint16_t maxmsgsize)
+{
+	sap_connect_rsp(sap_device, SAP_STATUS_OK, maxmsgsize);
+	sap_status_ind(sap_device, SAP_STATUS_CHANGE_CARD_RESET);
+}
+
+void sap_disconnect_req(void *sap_device)
+{
+	sap_disconnect_rsp(sap_device, SAP_RESULT_OK);
+}
+
+void sap_transfer_apdu_req(void *sap_device, sap_parameter *param)
+{
+	sap_transfer_apdu_rsp(sap_device, SAP_RESULT_OK, NULL, 0);
+}
+
+void sap_transfer_atr_req(void * sap_device)
+{
+	uint8_t atr[] = {0x3b, 0x9a, 0x96, 0x00, 0x92, 0x01, 0x98, 0x93, 0x17, 0x00, 0x02, 0x28, 0x03, 0x00};
+	sap_transfer_atr_rsp(sap_device, SAP_RESULT_OK, atr, 0x0E);
+}
+
+void sap_power_sim_off_req(void *sap_device)
+{
+	sap_power_sim_off_rsp(sap_device, SAP_RESULT_OK);
+}
+
+void sap_power_sim_on_req(void *sap_device)
+{
+	sap_power_sim_on_rsp(sap_device, SAP_RESULT_OK);
+}
+
+void sap_reset_sim_req(void *sap_device)
+{
+	sap_reset_sim_rsp(sap_device, SAP_RESULT_OK);
+}
+
+void sap_transfer_card_reader_status_req(void * sap_device)
+{
+	sap_transfer_card_reader_status_rsp(sap_device, SAP_RESULT_OK, 0xF1);
+}
+
+void sap_set_transport_protocol_req(void * sap_device,sap_parameter * param)
+{
+
+}
diff --git a/sap/sap-ste.c b/sap/sap-ste.c
new file mode 100644
index 0000000..2298e61
--- /dev/null
+++ b/sap/sap-ste.c
@@ -0,0 +1,1309 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 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; either 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
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <sys/socket.h>
+#include <fcntl.h>
+#include <sys/un.h>
+#include <netinet/in.h>
+#include <string.h>
+#include <errno.h>
+#include <sys/stat.h>
+
+#include <glib.h>
+#include <assert.h>
+#include "logging.h"
+
+#include "sap.h"
+
+/** STE_SIM_MAX_MSG_SIZE - Maximum size of Sim Access Profile message
+ * supported by the server.
+ */
+#define STE_SIM_MAX_MSG_SIZE 	1024
+
+/** STE_SIM_MIN_MSG_SIZE - Minimal size of  Sim Access Profile message
+ * supported by the server.
+ */
+#define STE_SIM_MIN_MSG_SIZE 	512
+
+/** STE_SIM_UNIX_SOCKET_NAME - Unix socket name used to communicate with
+ * SIM daemon.
+ */
+#define STE_SIM_UNIX_SOCKET_NAME "/dev/socket/catd_a"
+
+/*Sizes of SIM message fields.*/
+#define STE_SIM_MSG_LEN_SIZE 		0x0002
+#define STE_SIM_MSG_TYPE_SIZE 		0x0002
+#define STE_SIM_MSG_CLIENT_TAG_SIZE 	0x0004
+#define STE_SIM_MSG_STATUS_SIZE 	0x0004
+
+/*Header size of SIM message */
+#define STE_SIM_MSG_HEADER_SIZE ((STE_SIM_MSG_LEN_SIZE) + \
+		(STE_SIM_MSG_TYPE_SIZE) + (STE_SIM_MSG_CLIENT_TAG_SIZE))
+
+#define STE_SIM_CLIENT_TAG 0x0000
+
+#define STE_UICC_RANGE_REQ_SAP	(0x2D00)
+#define STE_UICC_RANGE_RSP_SAP  (0x2E00)
+#define STE_UICC_RANGE_IND_SAP  (0x2F00)
+
+/** ste_sim_protocol_t - protocol used in communication with SIM daemon.*/
+typedef enum {
+	STE_SIM_START_SAP_REQ 	= (STE_UICC_RANGE_REQ_SAP | 0x0001),
+	STE_SIM_START_SAP_RSP 	= (STE_UICC_RANGE_RSP_SAP | 0x0001),
+
+	STE_SIM_END_SAP_REQ	= (STE_UICC_RANGE_REQ_SAP | 0x0002),
+	STE_SIM_END_SAP_RSP 	= (STE_UICC_RANGE_RSP_SAP | 0x0002),
+
+	STE_SIM_POWER_ON_REQ 	= (STE_UICC_RANGE_REQ_SAP | 0x0004),
+	STE_SIM_POWER_ON_RSP	= (STE_UICC_RANGE_RSP_SAP | 0x0004),
+
+	STE_SIM_POWER_OFF_REQ	= (STE_UICC_RANGE_REQ_SAP | 0x0003),
+	STE_SIM_POWER_OFF_RSP	= (STE_UICC_RANGE_RSP_SAP | 0x0003),
+
+	STE_SIM_RESET_REQ	= (STE_UICC_RANGE_REQ_SAP | 0x0005),
+	STE_SIM_RESET_RSP	= (STE_UICC_RANGE_RSP_SAP | 0x0005),
+
+	STE_SIM_GET_ATR_REQ	= (STE_UICC_RANGE_REQ_SAP | 0x0007),
+	STE_SIM_GET_ATR_RSP	= (STE_UICC_RANGE_RSP_SAP | 0x0007),
+
+	STE_SIM_SEND_APDU_REQ	= (STE_UICC_RANGE_REQ_SAP | 0x0006),
+	STE_SIM_SEND_APDU_RSP	= (STE_UICC_RANGE_RSP_SAP | 0x0006),
+
+	STE_SIM_GET_STATUS_REQ	= (STE_UICC_RANGE_REQ_SAP | 0x0008),
+	STE_SIM_GET_STATUS_RSP	= (STE_UICC_RANGE_RSP_SAP | 0x0008),
+
+	STE_SIM_STATUS_IND	= (STE_UICC_RANGE_IND_SAP | 0x0002)
+} ste_sim_protocol_t;
+
+/** ste_sim_msg_t - Type of SIM message.*/
+typedef enum {
+	STE_SIM_SEND_APDU_MSG = 0,
+	STE_SIM_GET_ATR_MSG,
+	STE_SIM_POWER_OFF_MSG,
+	STE_SIM_POWER_ON_MSG,
+	STE_SIM_RESET_MSG,
+	STE_SIM_GET_STATUS_MSG,
+	STE_SIM_MSG_MAX,
+} ste_sim_msg_t;
+
+/** ste_sim_status_t - Status of a request.*/
+typedef enum {
+	STE_SIM_STATUS_OK 			= 0x00000000,
+	STE_SIM_STATUS_UNDEFINED_FAILURE 	= 0xFFFFFFFF,
+} ste_sim_status_t;
+
+/** ste_sim_card_status_t - Sim card status.*/
+typedef enum {
+	STE_SIM_CARD_STATUS_UNKNOWN = 0x00,
+	STE_SIM_CARD_STATUS_READY = 0x01,
+	STE_SIM_CARD_STATUS_NOT_READY = 0x02,
+	STE_SIM_CARD_STATUS_MISSING = 0x03,
+	STE_SIM_CARD_STATUS_INVALID = 0x04
+} ste_sim_card_status_t;
+
+/** ste_sim_reader_status_t - Sim card reader status.*/
+typedef enum {
+	STE_SIM_READER_UNSPECIFIED_ERROR 	= 0x0000,
+	STE_SIM_READER_NOT_PRESENT		= 0x0001,
+	STE_SIM_READER_BUSY			= 0x0002,
+	STE_SIM_READER_CARD_POWERED_ON		= 0x0003,
+	STE_SIM_READER_DEACTIVATED		= 0x0004,
+	STE_SIM_READER_CARD_POWERED_OFF		= 0x0005,
+	STE_SIM_READER_NO_CARD			= 0x0006
+}ste_sim_reader_status_t;
+
+/** ste_sim_state_t - Sim connection state.*/
+typedef enum {
+		STE_SIM_DISABLED,	/* Reader not presend or removed */
+		STE_SIM_POWERED_OFF,	/* Card in the reader but powered off */
+		STE_SIM_NO_CARD,	/* No card in the reader */
+		STE_SIM_ENABLED,	/* Card in the reader and powered on */
+		STE_SIM_STATE_MAX
+} ste_sim_state_t;
+
+/** ste_sim_message - Sim message format.
+ * @len;	Length of the message minus sizeof(len).
+ * @id;		Request type id.
+ * @client_tag;	SAP server cleint id.
+ * @payload;	Data.
+ */
+typedef struct {
+	uint16_t len;
+	uint16_t id;
+	uint32_t client_tag;
+	uint8_t payload[0];
+} __attribute__ ((packed)) ste_sim_message;
+
+/** ste_sim_connection - main admin structure that keeps data about connection
+ * with Sim daemon.
+ */
+struct ste_sim_connection {
+	GIOChannel  *io;
+	ste_sim_state_t state;
+	void *sap_data;
+};
+
+static struct ste_sim_connection *simd_conn = NULL;
+
+/** sim2sap_result - Conversion table of sap result which varies with
+ * connection state and the message type.
+ */
+static sap_result_t sim2sap_result[STE_SIM_MSG_MAX][STE_SIM_STATE_MAX] = {
+	/* SAP results for SEND APDU message */
+	{SAP_RESULT_ERROR_NOT_ACCESSIBLE,/*for STE_SIM_DISABLED state */
+	 SAP_RESULT_ERROR_POWERED_OFF,	/*for STE_SIM_POWERED_OFF state */
+	 SAP_RESULT_ERROR_CARD_REMOVED,	/*for STE_SIM_NO_CARD state */
+	 SAP_RESULT_ERROR_NO_REASON},	/*for STE_SIM_ENABLED state */
+	 /* 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 void simd_conn_uninit(struct ste_sim_connection *c);
+static int simd_conn_watch(int sock, void *sap_data);
+
+/**
+ * get_sap_result - Convert STE sim status to sap result.
+ * @c;		Connection info structure.
+ * @msg;	Sim message type.
+ * @status;	Sim status.
+ *
+ * Returns:
+ *	SAP result.
+ */
+static sap_result_t get_sap_result(struct ste_sim_connection *c,
+				ste_sim_msg_t msg, ste_sim_status_t status)
+{
+	if (!c)
+		return SAP_RESULT_ERROR_NO_REASON;
+
+	switch (status){
+		case STE_SIM_STATUS_OK:
+			return SAP_RESULT_OK;
+
+		case STE_SIM_STATUS_UNDEFINED_FAILURE:
+			return sim2sap_result[msg][c->state];
+
+		default:
+			error("Cannot convert ste_sim_status [status: %d] \
+						to sap_result.", status);
+			return SAP_RESULT_ERROR_NO_REASON;
+	}
+}
+
+/**
+ * get_sap_reader_status - Convert STE sim reader status to sap reader result.
+ * @status;	Sim card reader status.
+ *
+ * Returns:
+ *	SAP reader status.
+ */
+static icc_reader_status_t get_sap_reader_status(ste_sim_reader_status_t status)
+{
+	switch (status){
+		case STE_SIM_READER_UNSPECIFIED_ERROR:
+			return ICC_READER_UNSPECIFIED_ERROR;
+
+		case STE_SIM_READER_NOT_PRESENT:
+			return ICC_READER_NOT_PRESENT;
+
+		case STE_SIM_READER_BUSY:
+			return ICC_READER_BUSY;
+
+		case STE_SIM_READER_CARD_POWERED_ON:
+			return ICC_READER_CARD_POWERED_ON;
+
+		case STE_SIM_READER_DEACTIVATED:
+			return ICC_READER_DEACTIVATED;
+
+		case STE_SIM_READER_CARD_POWERED_OFF:
+			return ICC_READER_CARD_POWERED_OFF;
+
+		case STE_SIM_READER_NO_CARD:
+			return ICC_READER_NO_CARD;
+
+		default:
+			error("Cannot convert ste_sim_reader_status [status: %d]\
+					to icc_sap_reader_status.", status);
+			return ICC_READER_UNSPECIFIED_ERROR;
+	}
+}
+
+/**
+ * sap_status_change - Convert STE sim card status to sap change event.
+ * @c;		Connection info structure.
+ * @status;	Sim card status.
+ *
+ * This also updates Sim connection state.
+ *
+ * Returns:
+ *	SAP change event.
+ */
+static sap_status_change_t sap_status_change(struct ste_sim_connection *c,
+					ste_sim_card_status_t status)
+{
+	if (!c)
+		return SAP_STATUS_CHANGE_UNKNOWN_ERROR;
+
+	switch (status){
+		case STE_SIM_CARD_STATUS_UNKNOWN:
+			return SAP_STATUS_CHANGE_UNKNOWN_ERROR;
+
+		case STE_SIM_CARD_STATUS_READY:
+			c->state = STE_SIM_ENABLED;
+			return SAP_STATUS_CHANGE_CARD_RESET;
+
+		case STE_SIM_CARD_STATUS_NOT_READY:
+			c->state = STE_SIM_DISABLED;
+			return SAP_STATUS_CHANGE_CARD_NOT_ACCESSIBLE;
+
+		case STE_SIM_CARD_STATUS_MISSING:
+			c->state = STE_SIM_DISABLED;
+			return SAP_STATUS_CHANGE_CARD_REMOVED;
+
+		case STE_SIM_CARD_STATUS_INVALID:
+			c->state = STE_SIM_DISABLED;
+			return SAP_STATUS_CHANGE_CARD_NOT_ACCESSIBLE;
+
+		default:
+			error("Cannot convert ste_sim_status_change to \
+							sap_status_change.");
+			return SAP_STATUS_CHANGE_UNKNOWN_ERROR;
+	}
+}
+/**
+ * send_message - Send Sim message to Sim daemon.
+ * @c;		Connection info structure.
+ * @buf;	Sim message.
+ * @size;	Size of Sim message
+ *
+ * Returns:
+ *	O if success or negaive integer if error occured.
+ */
+static int send_message(struct ste_sim_connection *c,
+					ste_sim_message *buf, gsize size)
+{
+	gsize written = 0;
+	GIOError gerr;
+
+	debug("[STE_DRV] send_message: c %p, buf %p size %d", c, buf,
+								(int) size);
+
+	gerr = g_io_channel_write(c->io, (const gchar *) buf, size, &written);
+
+	if (written != size) {
+		error("[STE_DRV] send_message: written only %d bytes out of %d"
+						, (int)written, (int)size);
+		return -1;
+	}
+
+	if (gerr != G_IO_ERROR_NONE) {
+		int err = errno;
+		error("write error: %s(%d)", strerror(err), err);
+		return -err;
+	}
+
+	return 0;
+}
+
+/**
+ * ste_sim_start_sap_req - Create and send start SAP request message.
+ * @c;	Connection info structure.
+ *
+ * Returns:
+ *	O if success or negaive integer in case of error.
+ */
+static int ste_sim_start_sap_req(struct ste_sim_connection *c)
+{
+	int err;
+	ste_sim_message *msg = NULL;
+	gsize msg_size = STE_SIM_MSG_HEADER_SIZE;
+
+	debug("[STE_DRV] ste_sim_start_sap_req: c %p", c);
+
+	msg = (ste_sim_message *)g_malloc0(msg_size);
+	if (msg == NULL)
+		return -1;
+
+	msg->len = msg_size - STE_SIM_MSG_LEN_SIZE;
+	msg->id = STE_SIM_START_SAP_REQ;
+	msg->client_tag = STE_SIM_CLIENT_TAG;
+
+	err = send_message(c, msg, msg_size);
+	if (err < 0)
+		return err;
+
+	g_free(msg);
+	return 0;
+}
+
+/**
+ * ste_sim_end_sap_req - Create and send end SAP request message.
+ * @c;	Connection info structure.
+ *
+ * Returns:
+ *	O if success or negaive integer in case of error.
+ */
+static int ste_sim_end_sap_req(struct ste_sim_connection *c)
+{
+	int err;
+	ste_sim_message *msg = NULL;
+	gsize msg_size = STE_SIM_MSG_HEADER_SIZE;
+
+	debug("[STE_DRV] ste_sim_end_sap_req: c %p", c);
+
+	msg = (ste_sim_message *)g_malloc0(msg_size);
+	if (msg == NULL)
+		return -1;
+
+	msg->len = msg_size - STE_SIM_MSG_LEN_SIZE;
+	msg->id = STE_SIM_END_SAP_REQ;
+	msg->client_tag = STE_SIM_CLIENT_TAG;
+
+	err = send_message(c, msg, msg_size);
+	if (err < 0)
+		return err;
+
+	g_free(msg);
+	return 0;
+}
+
+/**
+ * ste_sim_end_sap_req - Create and send transfer apdu request message.
+ * @c;	Connection info structure.
+ *
+ * Returns:
+ *	O if success or negaive integer in case of error.
+ */
+static int ste_sim_send_apdu_req(struct ste_sim_connection *c,
+						sap_parameter *param)
+{
+	int err;
+	ste_sim_message *msg = NULL;
+	gsize msg_size = STE_SIM_MSG_HEADER_SIZE + param->len;
+
+	debug("[STE_DRV] ste_sim_send_apdu_req: c %p param %p len %d",
+							c, param, param->len);
+
+	msg = (ste_sim_message *)g_malloc0(msg_size);
+	if (msg == NULL)
+		return -1;
+
+	msg->len = msg_size - STE_SIM_MSG_LEN_SIZE;
+	msg->id = STE_SIM_SEND_APDU_REQ;
+	msg->client_tag = STE_SIM_CLIENT_TAG;
+	memcpy(msg->payload, param->val, param->len);
+
+	err = send_message(c, msg, msg_size);
+	if (err < 0)
+		return err;
+
+	g_free(msg);
+	return 0;
+}
+
+/**
+ * ste_sim_get_atr_req - Create and send get ATR request message.
+ * @c;	Connection info structure.
+ *
+ * Returns:
+ *	O if success or negaive integer in case of error.
+ */
+static int ste_sim_get_atr_req(struct ste_sim_connection *c)
+{
+	int err;
+	ste_sim_message *msg = NULL;
+	gsize msg_size = STE_SIM_MSG_HEADER_SIZE;
+
+	debug("[STE_DRV] ste_sim_get_atr_req: c %p", c);
+
+	msg = (ste_sim_message *)g_malloc0(msg_size);
+	if (msg == NULL)
+		return -1;
+
+	msg->len = msg_size - STE_SIM_MSG_LEN_SIZE;
+	msg->id = STE_SIM_GET_ATR_REQ;
+	msg->client_tag = STE_SIM_CLIENT_TAG;
+
+	err = send_message(c, msg, msg_size);
+	if (err < 0)
+		return err;
+
+	g_free(msg);
+	return 0;
+}
+
+/**
+ * ste_sim_power_off_req - Create and send power off request message.
+ * @c;	Connection info structure.
+ *
+ * Returns:
+ *	O if success or negaive integer in case of error.
+ */
+static int ste_sim_power_off_req(struct ste_sim_connection *c)
+{
+	int err;
+	ste_sim_message *msg = NULL;
+	gsize msg_size = STE_SIM_MSG_HEADER_SIZE;
+
+	debug("[STE_DRV] ste_sim_power_off_req: c %p", c);
+
+	msg = (ste_sim_message *)g_malloc0(msg_size);
+	if (msg == NULL)
+		return -1;
+
+	msg->len = msg_size - STE_SIM_MSG_LEN_SIZE;
+	msg->id = STE_SIM_POWER_OFF_REQ;
+	msg->client_tag = STE_SIM_CLIENT_TAG;
+
+	err = send_message(c, msg, msg_size);
+	if (err < 0)
+		return err;
+
+	g_free(msg);
+	return 0;
+}
+
+/**
+ * ste_sim_power_on_req - Create and send power on request message.
+ * @c;	Connection info structure.
+ *
+ * Returns:
+ *	O if success or negaive integer in case of error.
+ */
+static int ste_sim_power_on_req(struct ste_sim_connection *c)
+{
+	int err;
+	ste_sim_message *msg = NULL;
+	gsize msg_size = STE_SIM_MSG_HEADER_SIZE;
+
+	debug("[STE_DRV] ste_sim_power_on_req: c %p", c);
+
+	msg = (ste_sim_message *)g_malloc0(msg_size);
+	if (msg == NULL)
+		return -1;
+
+	msg->len = msg_size - STE_SIM_MSG_LEN_SIZE;
+	msg->id = STE_SIM_POWER_ON_REQ;
+	msg->client_tag = STE_SIM_CLIENT_TAG;
+
+	err = send_message(c, msg, msg_size);
+	if (err < 0)
+		return err;
+
+	g_free(msg);
+	return 0;
+}
+
+/**
+ * ste_sim_reset_req - Create and send card reset request message.
+ * @c;	Connection info structure.
+ *
+ * Returns:
+ *	O if success or negaive integer in case of error.
+ */
+static int ste_sim_reset_req(struct ste_sim_connection *c)
+{
+	int err;
+	ste_sim_message *msg = NULL;
+	gsize msg_size = STE_SIM_MSG_HEADER_SIZE;
+
+	debug("[STE_DRV] ste_sim_reset_req: c %p", c);
+
+	msg = (ste_sim_message *)g_malloc0(msg_size);
+	if (msg == NULL)
+		return -1;
+
+	msg->len = msg_size - STE_SIM_MSG_LEN_SIZE;
+	msg->id = STE_SIM_RESET_REQ;
+	msg->client_tag = STE_SIM_CLIENT_TAG;
+
+	err = send_message(c, msg, msg_size);
+	if (err < 0)
+		return err;
+
+	g_free(msg);
+	return 0;
+}
+
+/**
+ * ste_sim_get_status_req - Create and send card status request message.
+ * @c;	Connection info structure.
+ *
+ * Returns:
+ *	O if success or negaive integer in case of error.
+ */
+static int ste_sim_get_status_req(struct ste_sim_connection *c)
+{
+	int err;
+	ste_sim_message *msg = NULL;
+	gsize msg_size = STE_SIM_MSG_HEADER_SIZE;
+
+	debug("[STE_DRV] ste_sim_get_status_req: c %p", c);
+
+	msg = (ste_sim_message *)g_malloc0(msg_size);
+	if (msg == NULL)
+		return -1;
+
+	msg->len = msg_size - STE_SIM_MSG_LEN_SIZE;
+	msg->id = STE_SIM_GET_STATUS_REQ;
+	msg->client_tag = STE_SIM_CLIENT_TAG;
+
+	err = send_message(c, msg, msg_size);
+	if (err < 0)
+		return err;
+
+	g_free(msg);
+	return 0;
+}
+
+/**
+ * ste_sim_start_sap_rsp - Handle start SAP response message.
+ * @c;		Connection info structure.
+ * @msg;	Response message.
+ */
+static void ste_sim_start_sap_rsp(struct ste_sim_connection *c,
+						ste_sim_message * msg)
+{
+	uint32_t *status;
+	sap_status_t ss;
+	sap_status_change_t sc;
+
+	assert(msg && c);
+
+	debug("[STE_DRV] ste_sim_start_sap_rsp: c %p (conn %p) msg %p",
+							c, simd_conn, msg );
+
+	status = (uint32_t*) msg->payload;
+	if (*status == STE_SIM_STATUS_OK)
+		sap_connect_rsp(c->sap_data, SAP_STATUS_OK, 0);
+	else {
+		sap_connect_rsp(c->sap_data, SAP_STATUS_CONNECTION_FAILED, 0);
+		simd_conn_uninit(c);
+	}
+}
+
+/**
+ * ste_sim_end_sap_rsp - Handle end SAP response message.
+ * @c;		Connection info structure.
+ * @msg;	Response message.
+ */
+static void ste_sim_end_sap_rsp(struct ste_sim_connection *c,
+						ste_sim_message * msg)
+{
+	debug("[STE_DRV] ste_sim_end_sap_rsp: c %p (conn %p) msg %p",
+							c, simd_conn, msg );
+	assert(msg && c);
+
+	sap_disconnect_rsp(c->sap_data);
+	simd_conn_uninit(c);
+}
+
+/**
+ * ste_sim_send_apdu_rsp - Handle transfer apdu response message.
+ * @c;		Connection info structure.
+ * @msg;	Response message.
+ */
+static void  ste_sim_send_apdu_rsp(struct ste_sim_connection *c,
+						ste_sim_message * msg)
+{
+	uint32_t *status;
+	sap_result_t sr;
+	uint8_t *apdu = NULL;
+	uint16_t len = 0;
+
+	debug("[STE_DRV] ste_sim_send_apdu_rsp: c %p (conn %p) msg %p",
+							c, simd_conn, msg );
+
+	assert(msg && c);
+
+	status = (uint32_t*) msg->payload;
+	if (*status == STE_SIM_STATUS_OK) {
+		apdu = (uint8_t *)(msg->payload + STE_SIM_MSG_STATUS_SIZE);
+		len = msg->len - (STE_SIM_MSG_HEADER_SIZE) +
+				STE_SIM_MSG_LEN_SIZE - STE_SIM_MSG_STATUS_SIZE;
+	}
+
+	sr = get_sap_result(c, STE_SIM_SEND_APDU_MSG, *status);
+	sap_transfer_apdu_rsp(c->sap_data, sr, apdu, len);
+}
+
+/**
+ * ste_sim_get_atr_rsp - Handle get ATR response message.
+ * @c;		Connection info structure.
+ * @msg;	Response message.
+ */
+static void ste_sim_get_atr_rsp(struct ste_sim_connection *c,
+						ste_sim_message * msg)
+{
+	uint32_t *status;
+	sap_result_t sr;
+	uint8_t *atr = NULL;
+	uint16_t len = 0;
+
+	debug("[STE_DRV] ste_sim_get_atr_rsp: c %p (conn %p) msg %p",
+							c, simd_conn, msg );
+
+	assert(msg && c);
+
+	status = (uint32_t*) msg->payload;
+	if (*status == STE_SIM_STATUS_OK) {
+		atr = (uint8_t *)(msg->payload + STE_SIM_MSG_STATUS_SIZE);
+		len = msg->len - (STE_SIM_MSG_HEADER_SIZE) +
+						STE_SIM_MSG_LEN_SIZE;
+	}
+
+	sr = get_sap_result(c, STE_SIM_GET_ATR_MSG, *status);
+	sap_transfer_atr_rsp(c->sap_data, sr, atr, len);
+}
+
+/**
+ * ste_sim_power_off_rsp - Handle power off response message.
+ * @c;		Connection info structure.
+ * @msg;	Response message.
+ */
+static void ste_sim_power_off_rsp(struct ste_sim_connection *c,
+							ste_sim_message * msg)
+{
+	uint32_t *status;
+	sap_result_t sr;
+
+	debug("[STE_DRV] ste_sim_power_off_rsp: c %p (conn %p) msg %p",
+							c, simd_conn, msg );
+
+	assert(msg && c);
+
+	status = (uint32_t *) msg->payload;
+	if (*status == STE_SIM_STATUS_OK)
+		c->state = STE_SIM_POWERED_OFF;
+
+	sr = get_sap_result(c, STE_SIM_POWER_OFF_MSG, *status);
+	sap_power_sim_off_rsp(c->sap_data, sr);
+}
+
+/**
+ * ste_sim_power_on_rsp - Handle power on response message.
+ * @c;		Connection info structure.
+ * @msg;	Response message.
+ */
+static void ste_sim_power_on_rsp(struct ste_sim_connection *c,
+						ste_sim_message * msg)
+{
+	uint32_t *status;
+	sap_result_t sr;
+
+	debug("[STE_DRV] ste_sim_power_on_rsp: c %p (conn %p) msg %p",
+							c, simd_conn, msg );
+
+	assert(msg && c);
+
+	status = (uint32_t *) msg->payload;
+	if (*status == STE_SIM_STATUS_OK)
+		c->state = STE_SIM_ENABLED;
+
+	sr = get_sap_result(c, STE_SIM_POWER_ON_MSG, *status);
+	sap_power_sim_on_rsp(c->sap_data, sr);
+}
+
+/**
+ * ste_sim_reset_rsp - Handle reset response message.
+ * @c;		Connection info structure.
+ * @msg;	Response message.
+ */
+static void ste_sim_reset_rsp(struct ste_sim_connection *c,
+					ste_sim_message * msg)
+{
+	uint32_t *status;
+	sap_result_t sr;
+
+	debug("[STE_DRV] ste_sim_reset_rsp: c %p (conn %p) msg %p",
+							c, simd_conn, msg );
+
+	assert(msg && c);
+
+	status = (uint32_t *) msg->payload;
+	if (*status == STE_SIM_STATUS_OK)
+		c->state = STE_SIM_ENABLED;
+
+	sr = get_sap_result(c, STE_SIM_RESET_MSG, *status);
+	sap_reset_sim_rsp(c->sap_data, sr);
+}
+
+/**
+ * ste_sim_get_status_rsp - Handle card status response message.
+ * @c;		Connection info structure.
+ * @msg;	Response message.
+ */
+static void ste_sim_get_status_rsp(struct ste_sim_connection *c,
+						ste_sim_message * msg)
+{
+	uint8_t *p = NULL;
+	uint32_t *status;
+	sap_result_t sr;
+	ste_sim_reader_status_t rs;
+	icc_reader_status_t iccrs;
+
+	debug("[STE_DRV] ste_sim_get_status_rsp: c %p (conn %p) msg %p",
+							c, simd_conn, msg );
+
+	assert(msg && c);
+
+	p = msg->payload;
+
+	status = (uint32_t *)p; p += sizeof(uint32_t);
+	rs = *((uint16_t *)p);
+
+	iccrs = get_sap_reader_status(rs);
+	sr = get_sap_result(c, STE_SIM_GET_STATUS_MSG, *status);
+	sap_transfer_card_reader_status_rsp(c->sap_data, sr, iccrs);
+}
+
+/**
+ * ste_sim_status_ind - Handle status indication message.
+ * @c;		Connection info structure.
+ * @msg;	Response message.
+ */
+static void ste_sim_status_ind(struct ste_sim_connection *c,
+							ste_sim_message * msg)
+{
+	sap_status_change_t sc;
+	ste_sim_card_status_t cs;
+
+	debug("[STE_DRV] ste_sim_status_ind: c %p (conn %p) msg %p",
+							c, simd_conn, msg );
+
+	assert(msg && c);
+
+	cs = *((uint32_t *) msg->payload);
+	sc = sap_status_change(c, cs);
+
+	sap_status_ind(c->sap_data, sc);
+}
+
+/**
+ * handle_msg - Handle messages from Sim daemon.
+ * @c;		Connection info structure.
+ * @buf;	Data buffer.
+ * @size;	Size of data buffer.
+ *
+ * Returns:
+ *    0 if success or EBADMSG in case of bad message format.
+ */
+static int handle_msg(struct ste_sim_connection *c, unsigned char *buf,
+								gsize size)
+{
+	unsigned char *iter = buf;
+	ste_sim_message * msg = (ste_sim_message *) buf;
+	gsize msize = size;
+
+	assert(c);
+
+	do {
+		debug("[STE_DRV] handle_msg: msize %d msg->len %d.",
+							(int)msize, msg->len);
+
+		/* Message must be at least size of header len */
+		if (msize < STE_SIM_MSG_HEADER_SIZE) {
+			error("[STE_DRV] Invalid message size.");
+			return -EBADMSG;
+		}
+
+		/* Message must be completed. */
+		if (msize < (STE_SIM_MSG_LEN_SIZE + msg->len)) {
+			error("[STE_DRV] Not complete message.");
+			return -EBADMSG;
+		}
+
+		switch (msg->id){
+			case STE_SIM_START_SAP_RSP:
+				ste_sim_start_sap_rsp(c, msg);
+				break;
+
+			case STE_SIM_END_SAP_RSP:
+				ste_sim_end_sap_rsp(c, msg);
+				break;
+
+			case STE_SIM_SEND_APDU_RSP:
+				ste_sim_send_apdu_rsp(c, msg);
+				break;
+
+			case STE_SIM_GET_ATR_RSP:
+				ste_sim_get_atr_rsp(c, msg);
+				break;
+
+			case STE_SIM_POWER_OFF_RSP:
+				ste_sim_power_off_rsp(c, msg);
+				break;
+
+			case STE_SIM_POWER_ON_RSP:
+				ste_sim_power_on_rsp(c, msg);
+				break;
+
+			case STE_SIM_RESET_RSP:
+				ste_sim_reset_rsp(c, msg);
+				break;
+
+			case STE_SIM_GET_STATUS_RSP:
+				ste_sim_get_status_rsp(c, msg);
+				break;
+
+			case STE_SIM_STATUS_IND:
+				ste_sim_status_ind(c, msg);
+				break;
+
+			default:
+				error("[STE_DRV] Invalid or not supported \
+						frame [0x%02x].", msg->id);
+		}
+
+		/* Reduce total buffer size of just handled frame size*/
+		msize -= (STE_SIM_MSG_HEADER_SIZE +
+				(msg->len - (STE_SIM_MSG_HEADER_SIZE) +
+				 STE_SIM_MSG_LEN_SIZE));
+
+		/* Move msg ponter to then next message if any */
+		iter += (msg->len + STE_SIM_MSG_LEN_SIZE);
+		msg = (ste_sim_message *)iter;
+
+	} while (msg && (msize > 0));
+
+	return 0;
+}
+
+/**
+ * simd_io_data_cb - Handle data on socket to Sim daemon.
+ * @io;		Connection info structure.
+ * @cond;	Conndition that triggered this callback.
+ * @data;	Data buffor.
+ *
+ * Returns:
+ *    True if @data was handled, False otherwise.
+ */
+static gboolean simd_io_data_cb(GIOChannel *io, GIOCondition cond,
+							gpointer data)
+{
+	GIOError err = G_IO_ERROR_NONE;
+	unsigned char buf[STE_SIM_MAX_MSG_SIZE];
+	gsize read_bytes = 0;
+
+	if (cond & G_IO_NVAL) {
+		debug("[STE_DRV] NVAL on sim socket");
+		return FALSE;
+	}
+
+	if (cond & G_IO_HUP) {
+		debug("[STE_DRV] HUP on sim socket");
+		return FALSE;
+	}
+
+	if (cond & G_IO_ERR) {
+		debug("[STE_DRV] ERR on sim socket");
+		return FALSE;
+	}
+
+	/* TODO: consider to read bytes in a while loop just in case if one read
+			operation won't read all data. while(read_data!=0)
+	*/
+
+	err = g_io_channel_read(io, (gchar *)buf, sizeof(buf), &read_bytes);
+	if (err != G_IO_ERROR_NONE) {
+		error("[STE_DRV] Error while reading from channel \
+						[err 0x%x io %p].", err, io);
+		return FALSE;
+	}
+
+	if (handle_msg(data, buf, read_bytes) < 0)
+		error("[STE_DRV] Invalid STE Sim message.");
+
+	return TRUE;
+}
+
+/**
+ * simd_io_destroy_cb - Clean up the SAP serer in case of socket closure.
+ * @data;	Private data.
+ *
+ * The callback is run if the connection with Sim daemon has been lost.
+ */
+static void simd_io_destroy_cb(void *data)
+{
+	struct ste_sim_connection *c = (struct ste_sim_connection *)data;
+
+	debug("[STE_DRV] simd_io_destroy_cb: c %p conn %p io %p", c,
+							simd_conn, c->io);
+
+	if (c && simd_conn) {
+		g_io_channel_unref(c->io);
+		g_io_channel_shutdown(c->io, TRUE, NULL);
+		g_free(c);
+		simd_conn = NULL;
+	}
+}
+
+/**
+ * simd_conn_init - Initialize connection with Sim Daemon.
+ * @sap_data;	Private data of SAP Server.
+ *
+ * Returns:
+ *    0 if success, negative number in case of an error.
+ */
+static int simd_conn_init(void * sap_data)
+{
+	int sock;
+	int len = 0;
+	struct sockaddr_un addr;
+	ssize_t addr_len;
+
+	/* Create a socket to communicate with SIMD*/
+	sock = socket(PF_UNIX, SOCK_STREAM, 0);
+	if (sock < 0) {
+		error("[STE_DRV] Create socket failed: %s", strerror(errno));
+		return -errno;
+	}
+
+	/*Connect to SIMD*/
+	memset(&addr, 0, sizeof(addr));
+	addr.sun_family = AF_UNIX;
+	len = sprintf(addr.sun_path, STE_SIM_UNIX_SOCKET_NAME);
+	addr_len = sizeof(addr.sun_family) + len;
+	if (connect(sock, (struct sockaddr *) &addr, addr_len) < 0) {
+		error("[STE_DRV] Connect to the socket failed:  %s",
+							strerror(errno));
+		goto drop;
+	}
+
+	if (fcntl(sock, F_SETFL, O_NONBLOCK) > 0) {
+		error("[STE_DRV] fcntl() failed: %s", strerror(errno));
+		goto drop;
+	}
+
+	debug("[STE_DRV] simd_conn_init: sock %d ",sock);
+
+	/* Start watching incoming data */
+	if (simd_conn_watch(sock, sap_data) < 0)
+		goto drop;
+
+	return 0;
+
+drop:
+	debug("[STE_DRV] simd_conn_init: drop!");
+	close(sock);
+	return -errno;
+}
+
+/**
+ * simd_conn_uninit - Disconnect from Sim daemon.
+ * @c;	Connection info structure.
+ */
+static void simd_conn_uninit(struct ste_sim_connection *c)
+{
+	debug("[STE_DRV] simd_conn_uninit: c %p simd_conn %p", c, simd_conn);
+
+	if (!c)
+		return;
+
+	g_io_channel_shutdown(c->io, TRUE, NULL);
+	g_io_channel_unref(c->io);
+	g_free(c);
+
+	simd_conn = NULL;
+}
+
+
+/**
+ * simd_conn_watch - Start watching Sim daemon connection.
+ * @sap_data;	Private data of SAP Server.
+ *
+ * Returns:
+ *    0 if success, negative number in case of an error.
+ */
+static int simd_conn_watch(int sock, void *sap_data)
+{
+	GIOChannel *io = NULL;
+
+	debug("[STE_DRV] simd_conn_watch: sock %d, sap_data %p ", sock,
+								sap_data);
+
+	if (sock < 0)
+		return -1;
+
+	io = g_io_channel_unix_new(sock);
+	g_io_channel_set_close_on_unref(io, TRUE);
+
+
+	simd_conn = g_new0(struct ste_sim_connection, 1);
+	if (!simd_conn) {
+		error("[STE_DRV] simd_conn_watch: simd_conn %p io %p",
+								simd_conn, io);
+		g_io_channel_shutdown(io, TRUE, NULL);
+		g_io_channel_unref(io);
+		return -1;
+	}
+
+	simd_conn->io = io;
+	simd_conn->sap_data = sap_data;
+	simd_conn->state = STE_SIM_DISABLED;
+
+	g_io_add_watch_full(io, G_PRIORITY_DEFAULT,
+			G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+			simd_io_data_cb, simd_conn, simd_io_destroy_cb);
+
+	return 0;
+}
+
+/**
+ * sap_connect_req - Handle SAP connect request.
+ * @sap_data;	Private data of SAP Server.
+ * @maxmsgsize;	Message size supported by the SAP client.
+ */
+void sap_connect_req(void *sap_device, uint16_t maxmsgsize)
+{
+
+	debug("[STE_DRV] sap_connect_req: conn %p sap_device %p maxmsgsize %d",
+			simd_conn, sap_device, maxmsgsize);
+
+	if (maxmsgsize < STE_SIM_MIN_MSG_SIZE) {
+		sap_connect_rsp(sap_device, SAP_STATUS_MAX_MSG_SIZE_TOO_SMALL,
+							STE_SIM_MAX_MSG_SIZE);
+		return;
+	}
+
+	if (maxmsgsize > STE_SIM_MAX_MSG_SIZE) {
+		sap_connect_rsp(sap_device, SAP_STATUS_MAX_MSG_SIZE_NOT_SUPPORTED,
+							STE_SIM_MAX_MSG_SIZE);
+		return;
+	}
+
+	if (simd_conn_init(sap_device) < 0) {
+		debug("[STE_DRV] simd_conn_init: failed!");
+		sap_connect_rsp(sap_device, SAP_STATUS_CONNECTION_FAILED, 0);
+		return;
+	}
+
+	if (simd_conn && simd_conn->state == STE_SIM_DISABLED) {
+		if (ste_sim_start_sap_req(simd_conn) < 0) {
+			sap_connect_rsp(sap_device, SAP_STATUS_CONNECTION_FAILED,
+							STE_SIM_MAX_MSG_SIZE);
+			simd_conn_uninit(simd_conn);
+		}
+	} else {
+		debug("[STE_DRV] Connection failed! (simd_conn %p) ", simd_conn);
+		sap_connect_rsp(sap_device, SAP_STATUS_CONNECTION_FAILED, 0);
+	}
+}
+
+/**
+ * sap_disconnect_req - Handle SAP disconnect request.
+ * @sap_data;	Private data of SAP Server.
+ * @linkloss;	Indicates thet link is being disconnected due to link loss.
+ */
+void sap_disconnect_req(void *sap_device, uint8_t linkloss)
+{
+	debug("[STE_DRV] sap_disconnect_req: conn %p sap_device %p linkloss %d",
+					simd_conn, sap_device, linkloss);
+	if (simd_conn && linkloss) {
+		simd_conn_uninit(simd_conn);
+		return;
+	}
+
+	if (simd_conn && simd_conn->state != STE_SIM_DISABLED) {
+		if (ste_sim_end_sap_req(simd_conn) < 0) {
+			sap_disconnect_rsp(sap_device);
+		}
+	} else {
+		sap_disconnect_rsp(sap_device);
+	}
+}
+
+/**
+ * sap_transfer_apdu_req - Handle SAP transfer apdu request.
+ * @sap_data;	Private data of SAP Server.
+ * @param;	SAP parameter of the request.
+ */
+void sap_transfer_apdu_req(void *sap_device, sap_parameter *param)
+{
+	sap_result_t sr;
+
+	debug("[STE_DRV] sap_transfer_apdu_req: conn %p sap_device %p param %p\
+		param len %d", simd_conn, sap_device, param, param->len);
+
+	if (simd_conn && simd_conn->state == STE_SIM_ENABLED) {
+		if (ste_sim_send_apdu_req(simd_conn, param) < 0)
+			sap_transfer_apdu_rsp(sap_device,
+					SAP_RESULT_ERROR_NO_REASON, NULL, 0);
+	} else {
+		sr = get_sap_result(simd_conn, STE_SIM_SEND_APDU_MSG,
+					STE_SIM_STATUS_UNDEFINED_FAILURE);
+		sap_transfer_apdu_rsp(sap_device, sr, NULL, 0);
+	}
+}
+
+/**
+ * sap_transfer_atr_req - Handle SAP ATR request.
+ * @sap_data;	Private data of SAP Server.
+ */
+void sap_transfer_atr_req(void * sap_device)
+{
+	sap_result_t sr;
+
+	debug("[STE_DRV] sap_transfer_atr_req: conn %p, sap_device %p",
+							simd_conn, sap_device);
+
+	if (simd_conn && simd_conn->state == STE_SIM_ENABLED) {
+		if (ste_sim_get_atr_req(simd_conn) < 0)
+			sap_transfer_atr_rsp(sap_device,
+				SAP_RESULT_ERROR_NO_DATA, NULL, 0);
+	} else {
+		sr = get_sap_result(simd_conn, STE_SIM_GET_ATR_MSG,
+					STE_SIM_STATUS_UNDEFINED_FAILURE);
+		sap_transfer_atr_rsp(sap_device, sr, NULL, 0);
+	}
+}
+
+/**
+ * sap_power_sim_off_req - Handle SAP power off request.
+ * @sap_data;	Private data of SAP Server.
+ */
+void sap_power_sim_off_req(void *sap_device)
+{
+	sap_result_t sr;
+
+	debug("[STE_DRV] sap_power_sim_off_req: conn %p, sap_device %p",
+							simd_conn, sap_device);
+
+	if (simd_conn && simd_conn->state == STE_SIM_ENABLED) {
+		if (ste_sim_power_off_req(simd_conn) < 0)
+			sap_power_sim_off_rsp(sap_device,
+					SAP_RESULT_ERROR_NO_REASON);
+	} else {
+		sr = get_sap_result(simd_conn, STE_SIM_POWER_OFF_MSG,
+					STE_SIM_STATUS_UNDEFINED_FAILURE);
+		sap_power_sim_off_rsp(sap_device, sr);
+	}
+}
+
+/**
+ * sap_power_sim_on_req - Handle SAP power on request.
+ * @sap_data;	Private data of SAP Server.
+ */
+void sap_power_sim_on_req(void *sap_device)
+{
+	sap_result_t sr;
+
+	debug("[STE_DRV] sap_power_sim_on_req: conn %p, sap_device %p",
+							simd_conn, sap_device);
+
+	if (simd_conn && simd_conn->state == STE_SIM_POWERED_OFF) {
+		if (ste_sim_power_on_req(simd_conn) < 0)
+			sap_power_sim_on_rsp(sap_device,
+					SAP_RESULT_ERROR_NO_REASON);
+	} else {
+		sr = get_sap_result(simd_conn, STE_SIM_POWER_ON_MSG,
+					STE_SIM_STATUS_UNDEFINED_FAILURE);
+		sap_power_sim_on_rsp(sap_device, sr);
+	}
+}
+
+/**
+ * sap_reset_sim_req - Handle SAP reset request.
+ * @sap_data;	Private data of SAP Server.
+ */
+void sap_reset_sim_req(void *sap_device)
+{
+	sap_result_t sr;
+
+	debug("[STE_DRV] sap_reset_sim_req: conn %p, sap_device %p",
+							simd_conn, sap_device);
+
+	if (simd_conn && simd_conn->state == STE_SIM_ENABLED) {
+		if (ste_sim_reset_req(simd_conn) < 0)
+			sap_reset_sim_rsp(sap_device,
+						SAP_RESULT_ERROR_NO_REASON);
+	} else {
+		sr = get_sap_result(simd_conn, STE_SIM_RESET_MSG,
+					STE_SIM_STATUS_UNDEFINED_FAILURE);
+		sap_reset_sim_rsp(sap_device, sr);
+	}
+}
+
+/**
+ * sap_transfer_card_reader_status_req - Handle get card reader status request.
+ * @sap_data;	Private data of SAP Server.
+ */
+void sap_transfer_card_reader_status_req(void * sap_device)
+{
+	sap_result_t sr;
+
+	debug("[STE_DRV] sap_transfer_card_reader_status_req: conn %p, \
+					sap_device %p", simd_conn, sap_device);
+
+	if (simd_conn && simd_conn->state != STE_SIM_DISABLED) {
+		if (ste_sim_get_status_req(simd_conn) < 0)
+			sap_transfer_card_reader_status_rsp(sap_device,
+						SAP_RESULT_ERROR_NO_DATA,
+						ICC_READER_UNSPECIFIED_ERROR);
+	} else {
+		sr = get_sap_result(simd_conn, STE_SIM_GET_STATUS_MSG,
+					STE_SIM_STATUS_UNDEFINED_FAILURE);
+		sap_transfer_card_reader_status_rsp(sap_device, sr,
+						ICC_READER_UNSPECIFIED_ERROR);
+	}
+}
+
+/**
+ * sap_set_transport_protocol_req - Handle set transport protocol request.
+ * @sap_data;	Private data of SAP Server.
+ */
+void sap_set_transport_protocol_req(void * sap_device,sap_parameter * param)
+{
+	debug("[STE_DRV] Not supported.");
+}
+
diff --git a/sap/sap.h b/sap/sap.h
new file mode 100644
index 0000000..d0a1b99
--- /dev/null
+++ b/sap/sap.h
@@ -0,0 +1,200 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 ST-Ericsson SA
+ *  Copyright (C) 2010 Claudio Takahasi<claudio.takahasi@openbossa.org>
+ *
+ *  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.
+ *
+ *  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
+ */
+
+/* SAP 1.1 section 5.2.2 */
+typedef enum {
+	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
+} sap_status_t;
+
+/* SAP 1.1 section 5.2.3 */
+typedef enum {
+	SAP_DISCONNECTION_TYPE_GRACEFUL  = 0x00,
+	SAP_DISCONNECTION_TYPE_IMMEDIATE = 0x01,
+	SAP_DISCONNECTION_TYPE_CLIENT    = 0xFF
+} sap_disconnection_type_t;
+
+/* SAP 1.1 section 5.2.4 */
+typedef enum {
+	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
+} sap_result_t;
+
+/* SAP 1.1 section 5.2.8 Status Change */
+typedef enum {
+	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
+} sap_status_change_t;
+
+
+/* SAP 1.1 section 5.1 Message formats */
+typedef struct {
+	uint8_t id;
+	uint8_t reserved;
+	uint16_t len;
+	uint8_t val[0];
+	/*
+	 * Padding bytes 0-3 bytes
+	 */
+} __attribute__ ((packed)) sap_parameter;
+
+typedef struct {
+	uint8_t id;
+	uint8_t nparam;
+	uint16_t reserved;
+	sap_parameter param[0];
+} __attribute__ ((packed)) sap_message;
+
+
+typedef enum {
+	ICC_READER_UNSPECIFIED_ERROR, /* No further information available */
+	ICC_READER_NOT_PRESENT,       /* Card Reader removed or not present */
+	ICC_READER_BUSY,              /* Card Reader in use */
+	ICC_READER_CARD_POWERED_ON,   /* Card in reader and is powered on */
+	ICC_READER_DEACTIVATED,       /* Card Reader deactivated */
+	ICC_READER_CARD_POWERED_OFF,  /* Card in reader, but powered off */
+	ICC_READER_NO_CARD,           /* No card in reader */
+	ICC_READER_LAST
+} icc_reader_status_t;
+
+
+#define SAP_BUF_SIZE		512
+
+#define SAP_MSG_HEADER_SIZE	4
+
+typedef enum {
+	SAP_CONNECT_REQ                        = 0x00,	/* Client -> Server */
+	SAP_CONNECT_RESP                       = 0x01,	/* Server -> Client */
+
+	SAP_DISCONNECT_REQ                     = 0x02,	/* Client -> Server */
+	SAP_DISCONNECT_RESP                    = 0x03,	/* Server -> Client */
+	SAP_DISCONNECT_IND                     = 0x04,	/* Server -> Client */
+
+	SAP_TRANSFER_APDU_REQ                  = 0x05,	/* Client -> Server */
+	SAP_TRANSFER_APDU_RESP                 = 0x06,	/* Server -> Client */
+
+	SAP_TRANSFER_ATR_REQ                   = 0x07,	/* Client -> Server */
+	SAP_TRANSFER_ATR_RESP                  = 0x08,	/* Server -> Client */
+
+	SAP_POWER_SIM_OFF_REQ                  = 0x09,	/* Client -> Server */
+	SAP_POWER_SIM_OFF_RESP                 = 0x0A,	/* Server -> Client */
+
+	SAP_POWER_SIM_ON_REQ                   = 0x0B,	/* Client -> Server */
+	SAP_POWER_SIM_ON_RESP                  = 0x0C,	/* Server -> Client */
+
+	SAP_RESET_SIM_REQ                      = 0x0D,	/* Client -> Server */
+	SAP_RESET_SIM_RESP                     = 0x0E,	/* Server -> Client */
+
+	SAP_TRANSFER_CARD_READER_STATUS_REQ    = 0x0F,	/* Client -> Server */
+	SAP_TRANSFER_CARD_READER_STATUS_RESP   = 0x10,	/* Server -> Client */
+
+	SAP_STATUS_IND                         = 0x11,	/* Server -> Client */
+	SAP_ERROR_RESP                         = 0x12,	/* Server -> Client */
+
+	SAP_SET_TRANSPORT_PROTOCOL_REQ         = 0x13,	/* Client -> Server */
+	SAP_SET_TRANSPORT_PROTOCOL_RESP        = 0x14,	/* Server -> Client */
+} sap_protocol;
+
+/* SAP 1.1 section 5.2 Parameters IDs and Coding */
+#define SAP_PARAM_ID_MAXMSGSIZE             0x00
+#define SAP_PARAM_ID_MAXMSGSIZE_LEN         0x02
+
+#define SAP_PARAM_ID_CONN_STATUS            0x01
+#define SAP_PARAM_ID_CONN_STATUS_LEN        0x01
+
+#define SAP_PARAM_ID_RESULT_CODE            0x02
+#define SAP_PARAM_ID_RESULT_CODE_LEN        0x01
+
+#define SAP_PARAM_ID_DISCONNECT_IND         0x03
+#define SAP_PARAM_ID_DISCONNECT_IND_LEN     0x01
+
+#define SAP_PARAM_ID_COMMAND_APDU           0x04
+
+#define SAP_PARAM_ID_RESPONSE_APDU          0x05
+
+#define SAP_PARAM_ID_ATR                    0x06
+
+#define SAP_PARAM_ID_CARD_READER_STATUS     0x07
+#define SAP_PARAM_ID_CARD_READER_STATUS_LEN 0x01
+
+#define SAP_PARAM_ID_STATUS_CHANGE          0x08
+#define SAP_PARAM_ID_STATUS_CHANGE_LEN      0x01
+
+#define SAP_PARAM_ID_TRANSPORT_PROTOCOL     0x09
+#define SAP_PARAM_ID_TRANSPORT_PROTOCOL_LEN 0x01
+
+#define SAP_PARAM_ID_COMMAND_APDU7816       0x10
+
+/* SAP 1.1 section 5.2.9 Possible Values for TransportProtocol */
+#define SAP_TRANSPORT_PROTOCOL_T0           0x00
+#define SAP_TRANSPORT_PROTOCOL_T1           0x01
+
+
+/*
+ * Drivers must implement the following functions.
+ * Blocking operations are not allowed, results must be
+ * reported using the functions defined in the next section.
+ * Functions implemented on 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, 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, sap_parameter *param);
+
+
+/*
+ * Drivers must use the following functions to report operation results.
+ * Functions implemented on server.c
+ */
+int sap_connect_rsp(void *sap_device, sap_status_t status, uint16_t maxmsgsize);
+int sap_disconnect_rsp(void *sap_device);
+int sap_transfer_apdu_rsp(void *sap_device, sap_result_t result, uint8_t *sap_apdu_resp, uint16_t length);
+int sap_transfer_atr_rsp(void *sap_device, sap_result_t result, uint8_t *sap_atr, uint16_t length);
+int sap_power_sim_off_rsp(void *sap_device, sap_result_t result);
+int sap_power_sim_on_rsp(void *sap_device, sap_result_t result);
+int sap_reset_sim_rsp(void *sap_device, sap_result_t result);
+int sap_transfer_card_reader_status_rsp(void *sap_device, sap_result_t result, icc_reader_status_t status);
+int sap_error_rsp(void *sap_device);
+int sap_transport_protocol_resp(void *sap_device, sap_result_t resul);
+
+/*
+ * Asynchronous Driver events notification.
+ * Functions implemented on server.c
+ */
+int sap_status_ind(void *sap_device, sap_status_change_t status_change);
+
diff --git a/sap/server.c b/sap/server.c
new file mode 100644
index 0000000..a8d098a
--- /dev/null
+++ b/sap/server.c
@@ -0,0 +1,1459 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 ST-Ericsson SA
+ *  Copyright (C) 2010 Claudio Takahasi<claudio.takahasi@openbossa.org>
+ *
+ *  Author: Marek Skowron <marek.skowron@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.
+ *
+ *  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 <unistd.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <arpa/inet.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+#include <assert.h>
+#include <glib.h>
+#include <gdbus.h>
+
+#include "adapter.h"
+#include "dbus-common.h"
+#include "btio.h"
+#include "sdpd.h"
+
+#include "logging.h"
+#include "error.h"
+
+#include "sap.h"
+#include "server.h"
+
+#define SAP_PROFILE_VERSION 0x0101
+
+#define SAP_SERVER_INTERFACE		"org.bluez.SimAccess"
+#define SAP_SERVER_CHANNEL		8
+#define SAP_UUID			"0000112D-0000-1000-8000-00805F9B34FB"
+#define SAP_BUF_SIZE			512
+#define PADDING4(x) (4 - (x & 0x03))
+#define PARAMETER_SIZE(x) (sizeof(sap_parameter) + x + PADDING4(x))
+
+#define SAP_NO_REQ 0xFF
+
+#define SAP_TIMER_GRACEFUL_DISCONNECT 30
+#define SAP_TIMER_NO_ACTIVITY 30
+
+typedef enum {
+	SAP_STATE_DISCONNECTED,
+	SAP_STATE_CONNECT_IN_PROGRESS,
+	SAP_STATE_CONNECTED,
+	SAP_STATE_GRACEFUL_DISCONNECT,
+	SAP_STATE_IMMEDIATE_DISCONNECT,
+	SAP_STATE_CLIENT_DISCONNECT
+} sap_state_t;
+
+struct sap_server {
+	bdaddr_t	src;
+	char		*path;
+	gboolean	enable;		/* Enable flag */
+	uint32_t	record_id;	/* Service record id */
+	GIOChannel	*listen_io;
+	GIOChannel	*io;		/* Active client channel */
+	sap_state_t	state;		/* State of the SAP server */
+	uint8_t		processing_req; /* Processing request in Connected state */
+	guint timer_id;			/* Guard timer */
+};
+
+static DBusConnection *connection = NULL;
+
+static void connect_req(void *data, sap_parameter *param);
+static int disconnect_req(void *data, sap_disconnection_type_t disc_type);
+static void transfer_apdu_req(void *data, sap_parameter *param);
+static void transfer_atr_req(void *data);
+static void power_sim_off_req(void *data);
+static void power_sim_on_req(void *data);
+static void reset_sim_req(void *data);
+static void transfer_card_reader_status_req(void *data);
+static void set_transport_protocol_req(void *data, sap_parameter *param);
+static int disconnect_ind(void *sap_device, sap_disconnection_type_t type);
+
+static gsize add_result_parameter(sap_result_t result, sap_parameter *param);
+
+static int is_power_sim_off_req_allowed(uint8_t processing_req);
+static int is_reset_sim_req_allowed(uint8_t processing_req);
+
+static int check_msg(sap_message *msg);
+
+static void start_guard_timer(struct sap_server *ss, guint interval);
+static void stop_guard_timer(struct sap_server *ss);
+static gboolean guard_timeout(gpointer data);
+
+static gsize add_result_parameter(sap_result_t result, sap_parameter *param)
+{
+	param->id = SAP_PARAM_ID_RESULT_CODE;
+	param->len = htons(SAP_PARAM_ID_RESULT_CODE_LEN);
+	*param->val = (uint8_t) result;
+	return PARAMETER_SIZE(SAP_PARAM_ID_RESULT_CODE_LEN);
+}
+
+
+static inline 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 inline 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(sap_message *msg)
+{
+	assert(msg);
+
+	switch(msg->id) {
+	case SAP_CONNECT_REQ:
+		if (msg->nparam == 0x01 &&
+				msg->param->id == SAP_PARAM_ID_MAXMSGSIZE &&
+				ntohs(msg->param->len) == SAP_PARAM_ID_MAXMSGSIZE_LEN)
+			return 0;
+		break;
+	case SAP_TRANSFER_APDU_REQ:
+		if (msg->nparam == 0x01 &&
+				(msg->param->id == SAP_PARAM_ID_COMMAND_APDU ||
+				msg->param->id == SAP_PARAM_ID_COMMAND_APDU7816) &&
+				msg->param->len != 0x00)
+			return 0;
+		break;
+	case SAP_SET_TRANSPORT_PROTOCOL_REQ:
+		if (msg->nparam == 0x01 &&
+				msg->param->id == SAP_PARAM_ID_TRANSPORT_PROTOCOL &&
+				ntohs(msg->param->len) == SAP_PARAM_ID_TRANSPORT_PROTOCOL_LEN &&
+				(*msg->param->val  == SAP_TRANSPORT_PROTOCOL_T0 ||
+				*msg->param->val == SAP_TRANSPORT_PROTOCOL_T1))
+			return 0;
+		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 0;
+		break;
+	}
+
+	error("Invalid message");
+	return -EBADMSG;
+}
+
+
+static void start_guard_timer(struct sap_server *ss, guint interval)
+{
+	assert(ss);
+
+	if (!ss->timer_id)
+		ss->timer_id = g_timeout_add_seconds(interval, guard_timeout, ss);
+	else
+		error("[%s::%s] Timer is already active", __FILE__, __FUNCTION__);
+}
+
+static void stop_guard_timer(struct sap_server *ss)
+{
+	if (ss && ss->timer_id) {
+		g_source_remove(ss->timer_id);
+		ss->timer_id = 0x00;
+	}
+}
+
+static gboolean guard_timeout(gpointer data)
+{
+	struct sap_server *ss = data;
+
+	assert(ss);
+
+	debug("[%s::%s] state: %#x processing_req: %#x",
+		__FILE__, __FUNCTION__, ss->state, ss->processing_req);
+
+	/* Set timer ID to zero because timer will be destroyed. */
+	ss->timer_id = 0;
+
+	switch(ss->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. Shutdown RFCOMM channel immediately. */
+		if (ss->io)
+			g_io_channel_shutdown(ss->io, TRUE, NULL);
+		break;
+	case SAP_STATE_GRACEFUL_DISCONNECT:
+		/* Client didn't disconnect SAP connection in fixed time,
+		 * so close SAP connection immediately. */
+		disconnect_req(ss, SAP_DISCONNECTION_TYPE_IMMEDIATE);
+		break;
+	default:
+		error("[%s::%s] Unexpected state: %#x processing_req: %#x",
+					__FILE__, __FUNCTION__, ss->state, ss->processing_req);
+		break;
+	}
+
+	return FALSE;
+}
+
+static void server_free(struct sap_server *ss)
+{
+	if (!ss)
+		return;
+
+	g_free(ss->path);
+	g_free(ss);
+	debug("[%s::%s]", __FILE__, __FUNCTION__);
+}
+
+static sdp_record_t *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;
+
+	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_PROFILE_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_server *ss,
+						unsigned char *buf, gsize size)
+{
+	gsize written = 0;
+	GIOError gerr;
+
+	assert(ss && buf);
+
+	debug("[%s::%s] size=%#x", __FILE__, __FUNCTION__, (unsigned int)size);
+	gerr = g_io_channel_write(ss->io, (const gchar *) buf, size, &written);
+	debug("[%s::%s] written=%#x", __FILE__, __FUNCTION__, (unsigned int)written);
+	if (gerr != G_IO_ERROR_NONE) {
+		int err = errno;
+		error("write error: %s(%d)", strerror(err), err);
+		return -err;
+	}
+
+	return 0;
+}
+
+static void connect_req(void *data, sap_parameter *param)
+{
+	struct sap_server *ss = data;
+	uint16_t maxmsgsize, *val;
+
+	assert(ss && param);
+
+	if (ss->state != SAP_STATE_DISCONNECTED)
+		goto error_rsp;
+
+	stop_guard_timer(ss);
+
+	val = (uint16_t *) &param->val;
+	maxmsgsize = ntohs(*val);
+
+	debug("Connect MaxMsgSize: 0x%04X(%d)", maxmsgsize, maxmsgsize);
+
+	ss->state = SAP_STATE_CONNECT_IN_PROGRESS;
+
+	if (maxmsgsize <= SAP_BUF_SIZE){
+		ss->processing_req = SAP_CONNECT_REQ;
+		sap_connect_req(ss, maxmsgsize);
+	}else{
+		sap_connect_rsp(ss, SAP_STATUS_MAX_MSG_SIZE_NOT_SUPPORTED, htons(SAP_BUF_SIZE));
+	}
+
+	return;
+
+error_rsp:
+	error("Unexpected request state: %#x processing_req: %#x", ss->state, ss->processing_req);
+	sap_error_rsp(data);
+}
+
+static int disconnect_req(void *data, sap_disconnection_type_t disc_type)
+{
+	struct sap_server *ss = data;
+
+	assert(ss);
+	debug("disconnect_req: type 0x%x state %d", disc_type, ss->state);
+
+	switch(disc_type) {
+	case SAP_DISCONNECTION_TYPE_GRACEFUL:
+
+		if (ss->state == SAP_STATE_DISCONNECTED || ss->state == SAP_STATE_CONNECT_IN_PROGRESS)
+			goto error_req;
+
+		if (ss->state == SAP_STATE_CONNECTED) {
+			ss->state = SAP_STATE_GRACEFUL_DISCONNECT;
+			ss->processing_req = SAP_NO_REQ;
+			disconnect_ind(ss, disc_type);
+
+			/* Start guard timer - timer will disconnect connection if client doesn't do it. */
+			start_guard_timer(ss, SAP_TIMER_GRACEFUL_DISCONNECT);
+
+			return 0;
+		}
+
+		/* Disconnection is ongoing - do nothing. */
+		return 0;
+
+	case SAP_DISCONNECTION_TYPE_IMMEDIATE:
+
+		if (ss->state == SAP_STATE_DISCONNECTED || ss->state == SAP_STATE_CONNECT_IN_PROGRESS)
+			goto error_req;
+
+		if (ss->state == SAP_STATE_CONNECTED ||
+				ss->state == SAP_STATE_GRACEFUL_DISCONNECT) {
+			ss->state = SAP_STATE_IMMEDIATE_DISCONNECT;
+			ss->processing_req = SAP_NO_REQ;
+
+			/* Stop timer if it is run. */
+			stop_guard_timer(ss);
+
+			disconnect_ind(ss, disc_type);
+			sap_disconnect_req(ss, 0);
+
+			return 0;
+		}
+
+		/* Disconnection is ongoing - do nothing. */
+		return 0;
+
+	case SAP_DISCONNECTION_TYPE_CLIENT:
+
+		if (ss->state != SAP_STATE_CONNECTED && ss->state != SAP_STATE_GRACEFUL_DISCONNECT)
+			goto error_rsp;
+
+		ss->state = SAP_STATE_CLIENT_DISCONNECT;
+		ss->processing_req = SAP_NO_REQ;
+
+		/* Stop timer if it is run. */
+		stop_guard_timer(ss);
+
+		sap_disconnect_req(ss, 0);
+
+		return 0;
+
+	default:
+		error("Unexpected disconnection type: %#x", disc_type);
+		return -EINVAL;
+	}
+
+error_rsp:
+	sap_error_rsp(ss);
+error_req:
+	error("Unexpected request state: %#x processing_req: %#x", ss->state, ss->processing_req);
+	return -EPERM;
+}
+
+static void transfer_apdu_req(void * data, sap_parameter * param)
+{
+	struct sap_server *ss = data;
+
+	assert(ss && param);
+	param->len = ntohs(param->len);
+
+	debug("transfer_apdu_req: data %p state %d", data, ss->state);
+	debug("transfer_apdu_req: apdu param id %d  val %s len %d ", param->id, param->val, param->len);
+
+	if (ss->state != SAP_STATE_CONNECTED && ss->state != SAP_STATE_GRACEFUL_DISCONNECT)
+		goto error_rsp;
+
+	if (ss->processing_req != SAP_NO_REQ)
+		goto error_rsp;
+
+	ss->processing_req = SAP_TRANSFER_APDU_REQ;
+	sap_transfer_apdu_req(ss, param);
+
+	return;
+
+error_rsp:
+	error("Unexpected request state: %#x processing_req: %#x", ss->state, ss->processing_req);
+	sap_error_rsp(ss);
+}
+
+
+static void transfer_atr_req(void * data)
+{
+	struct sap_server *ss = data;
+
+	assert(ss);
+	debug("transfer_atr_req: data %p state %d", data, ss->state);
+
+	if (ss->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (ss->processing_req != SAP_NO_REQ)
+		goto error_rsp;
+
+	ss->processing_req = SAP_TRANSFER_ATR_REQ;
+	sap_transfer_atr_req(ss);
+
+	return;
+
+error_rsp:
+	error("Unexpected request state: %#x processing_req: %#x", ss->state, ss->processing_req);
+	sap_error_rsp(ss);
+}
+
+
+static void power_sim_off_req(void *data)
+{
+	struct sap_server *ss = data;
+
+	assert(ss);
+	debug("power_sim_off_req: data %p state %d", data, ss->state);
+
+	if (ss->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (!is_power_sim_off_req_allowed(ss->processing_req))
+		goto error_rsp;
+
+	ss->processing_req = SAP_POWER_SIM_OFF_REQ;
+	sap_power_sim_off_req(ss);
+
+	return;
+
+error_rsp:
+	error("Unexpected request state: %#x processing_req: %#x", ss->state, ss->processing_req);
+	sap_error_rsp(ss);
+}
+
+static void power_sim_on_req(void *data)
+{
+	struct sap_server *ss = data;
+
+	assert(ss);
+	debug("power_sim_on_req: data %p state %d", data, ss->state);
+
+	if (ss->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (ss->processing_req != SAP_NO_REQ)
+		goto error_rsp;
+
+	ss->processing_req = SAP_POWER_SIM_ON_REQ;
+	sap_power_sim_on_req(ss);
+
+	return;
+
+error_rsp:
+	error("Unexpected request state: %#x processing_req: %#x", ss->state, ss->processing_req);
+	sap_error_rsp(ss);
+}
+
+static void reset_sim_req(void *data)
+{
+	struct sap_server *ss = data;
+
+	assert(ss);
+	debug("reset_sim_req: data %p state %d", data, ss->state);
+
+	if (ss->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (!is_reset_sim_req_allowed(ss->processing_req))
+		goto error_rsp;
+
+	ss->processing_req = SAP_RESET_SIM_REQ;
+	sap_reset_sim_req(ss);
+
+	return;
+
+error_rsp:
+	error("Unexpected request state: %#x processing_req: %#x", ss->state, ss->processing_req);
+	sap_error_rsp(ss);
+}
+
+static void transfer_card_reader_status_req(void * data)
+{
+	struct sap_server *ss = data;
+
+	assert(ss);
+	debug("transfer_card_reader_status_req: data %p state %d", data, ss->state);
+
+
+	if (ss->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (ss->processing_req != SAP_NO_REQ)
+		goto error_rsp;
+
+	ss->processing_req = SAP_TRANSFER_CARD_READER_STATUS_REQ;
+	sap_transfer_card_reader_status_req(ss);
+
+	return;
+
+error_rsp:
+	error("Unexpected request state: %#x processing_req: %#x", ss->state, ss->processing_req);
+	sap_error_rsp(ss);
+}
+
+static void set_transport_protocol_req(void *data, sap_parameter *param)
+{
+	struct sap_server *ss = data;
+
+	assert(ss && param);
+	debug("set_transport_protocol_req: data %p state %d param %p", data, ss->state, param);
+
+	if (ss->state != SAP_STATE_CONNECTED)
+		goto error_rsp;
+
+	if (ss->processing_req != SAP_NO_REQ)
+		goto error_rsp;
+
+	ss->processing_req = SAP_SET_TRANSPORT_PROTOCOL_REQ;
+	sap_set_transport_protocol_req(ss, param);
+
+	return;
+
+error_rsp:
+	error("Unexpected request state: %#x processing_req: %#x", ss->state, ss->processing_req);
+	sap_error_rsp(ss);
+}
+
+static int disconnect_ind(void *sap_device, sap_disconnection_type_t type)
+{
+	struct sap_server *ss = sap_device;
+	unsigned char buf[SAP_BUF_SIZE];
+	sap_message *msg = (sap_message *) buf;
+	sap_parameter *param = (sap_parameter *) msg->param;
+	gsize size = sizeof(sap_message);
+
+	assert(ss);
+	debug("disconnect_ind: data %p state %d disc_type %d", ss, ss->state, type);
+
+
+	if (ss->state != SAP_STATE_CONNECTED && ss->state != SAP_STATE_GRACEFUL_DISCONNECT)
+		goto error_ind;
+
+	if (ss->state == SAP_STATE_GRACEFUL_DISCONNECT && type == SAP_DISCONNECTION_TYPE_GRACEFUL)
+		goto error_ind;
+
+	memset(buf, 0, sizeof(buf));
+	msg->id = SAP_DISCONNECT_IND;
+	msg->nparam = 0x01;
+
+	/* Adding disconnection type. */
+	param->id  = SAP_PARAM_ID_DISCONNECT_IND;
+	param->len = htons(SAP_PARAM_ID_DISCONNECT_IND_LEN);
+	*param->val = (uint8_t) type;
+	size += PARAMETER_SIZE(SAP_PARAM_ID_DISCONNECT_IND_LEN);
+
+	debug("[%s::%s] buf=%s size=%#x", __FILE__, __FUNCTION__, buf, (unsigned int)size);
+	return send_message(sap_device, buf, size);
+
+error_ind:
+	error("Unexpected indication state: %#x processing_req: %#x", ss->state, ss->processing_req);
+	return -EPERM;
+}
+
+static int handle_cmd(void *data, unsigned char *buf, gsize size)
+{
+	sap_message *msg = (sap_message *) buf;
+
+	/* Avoiding invalid memory access */
+	if (size < sizeof(sap_message))
+		goto error_rsp;
+
+	/* When number of parameters != 0 make sure that it has the min size */
+	if (msg->nparam != 0 &&
+			size < (sizeof(sap_message) + sizeof(sap_parameter) + 4))
+		goto error_rsp;
+
+	if (check_msg(msg) < 0)
+		goto error_rsp;
+
+	switch(msg->id) {
+	case SAP_CONNECT_REQ:
+		debug("SAP Connect");
+		connect_req(data, msg->param);
+		return 0;
+	case SAP_DISCONNECT_REQ:
+		debug("SAP Disconnect");
+		disconnect_req(data, SAP_DISCONNECTION_TYPE_CLIENT);
+		return 0;
+	case SAP_TRANSFER_APDU_REQ:
+		debug("SAP Transfer APDU");
+		transfer_apdu_req(data, msg->param);
+		return 0;
+	case SAP_TRANSFER_ATR_REQ:
+		debug("SAP Transfer ATR");
+		transfer_atr_req(data);
+		return 0;
+	case SAP_POWER_SIM_OFF_REQ:
+		debug("SAP SIM off");
+		power_sim_off_req(data);
+		return 0;
+	case SAP_POWER_SIM_ON_REQ:
+		debug("SAP SIM on");
+		power_sim_on_req(data);
+		return 0;
+	case SAP_RESET_SIM_REQ:
+		debug("SAP SIM reset");
+		reset_sim_req(data);
+		return 0;
+	case SAP_TRANSFER_CARD_READER_STATUS_REQ:
+		debug("SAP reader status");
+		transfer_card_reader_status_req(data);
+		return 0;
+	case SAP_SET_TRANSPORT_PROTOCOL_REQ:
+		debug("SAP set proto request");
+		set_transport_protocol_req(data, msg->param);
+		return 0;
+	}
+
+error_rsp:
+	debug("SAP ERROR RSP");
+	sap_error_rsp(data);
+	return -EBADMSG;
+}
+
+static gboolean sap_io_cb(GIOChannel *chan, GIOCondition cond, void *data)
+{
+	unsigned char buf[SAP_BUF_SIZE];
+	gsize bytes_read = 0;
+
+	debug("[%s::%s]", __FILE__, __FUNCTION__);
+
+	if (cond & G_IO_NVAL)
+		return FALSE;
+
+	if (cond & G_IO_ERR) {
+		debug("ERR on RFCOMM socket");
+		goto failed;
+	}
+
+	if (cond & G_IO_HUP ) {
+		debug("HUP on RFCOMM socket");
+		goto failed;
+	}
+
+	if (g_io_channel_read(chan, (gchar *) buf, sizeof(buf) - 1,
+				&bytes_read) != G_IO_ERROR_NONE)
+		return TRUE;
+
+	if (handle_cmd(data, buf, bytes_read) < 0) {
+		debug("Invalid SAP message");
+	}
+
+	return TRUE;
+
+failed:
+	return FALSE;
+}
+
+static void sap_io_destroy(void *data)
+{
+	struct sap_server *ss = data;
+
+	assert(ss);
+
+	if (ss && ss->io) {
+		gboolean connected = FALSE;
+
+		/* Stop timer if it is run. */
+		stop_guard_timer(ss);
+
+		g_io_channel_shutdown(ss->io, TRUE, NULL);
+		g_io_channel_unref(ss->io);
+
+		ss->io = NULL;
+
+		if (ss->state != SAP_STATE_CONNECT_IN_PROGRESS)
+			emit_property_changed(connection, ss->path, SAP_SERVER_INTERFACE,
+									"Connected", DBUS_TYPE_BOOLEAN, &connected);
+
+		if (ss->state == SAP_STATE_CONNECT_IN_PROGRESS ||
+					ss->state == SAP_STATE_CONNECTED ||
+					ss->state == SAP_STATE_GRACEFUL_DISCONNECT ){
+				sap_disconnect_req(NULL, 1);
+			}
+
+		ss->state = SAP_STATE_DISCONNECTED;
+	}
+
+	debug("[%s::%s]", __FILE__, __FUNCTION__);
+}
+
+static void sap_connect_cb(GIOChannel *chan, GError *gerr, gpointer data)
+{
+	struct sap_server *ss = data;
+
+	assert(ss);
+
+	debug("[%s::%s]", __FILE__, __FUNCTION__);
+
+	/* Start timer which will shutdown channel if client doesn't send CONNECT_REQ. */
+	start_guard_timer(ss, SAP_TIMER_NO_ACTIVITY);
+
+	g_io_add_watch_full(chan, G_PRIORITY_DEFAULT,
+			G_IO_IN | G_IO_ERR | G_IO_HUP| G_IO_NVAL,
+			(GIOFunc) sap_io_cb, ss, sap_io_destroy);
+}
+
+static void sap_auth_cb(DBusError *derr, void *data)
+{
+	GError *gerr = NULL;
+	struct sap_server *ss = data;
+
+	assert(ss && ss->io);
+
+	debug("[%s::%s]", __FILE__, __FUNCTION__);
+
+	if (derr && dbus_error_is_set(derr)) {
+		error("Access denied: %s", derr->message);
+		goto drop;
+	}
+
+	if (!bt_io_accept(ss->io, sap_connect_cb, ss, NULL, &gerr)) {
+		error("bt_io_accept: %s", gerr->message);
+		g_error_free(gerr);
+		goto drop;
+	}
+	return;
+
+drop:
+	g_io_channel_shutdown(ss->io, TRUE, NULL);
+	g_io_channel_unref(ss->io);
+	ss->io = NULL;
+}
+
+static void connect_confirm(GIOChannel *chan, gpointer data)
+{
+	GError *gerr = NULL;
+	bdaddr_t src, dst;
+	int err;
+	struct sap_server *ss = data;
+
+	assert(chan && ss);
+
+	if (ss->io)
+		goto drop;
+
+	bt_io_get(chan, 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);
+		goto drop;
+	}
+
+	ss->io = g_io_channel_ref(chan);
+
+	err = btd_request_authorization(&src, &dst, SAP_UUID, sap_auth_cb, ss);
+
+	if (err < 0) {
+		debug("Authorization denied: %s", strerror(-err));
+		goto drop;
+	}
+
+	debug("[%s::%s] RFCOMM SAP accept socket fd: %#x", __FILE__, __FUNCTION__, g_io_channel_unix_get_fd(chan));
+
+	return;
+
+drop:
+	g_io_channel_shutdown(chan, TRUE, NULL);
+	if (ss->io && ss->io == chan) {
+		g_io_channel_unref(ss->io);
+		ss->io = NULL;
+	}
+}
+
+static void path_unregister(void *data)
+{
+	struct sap_server *ss = data;
+
+	debug("Unregistered interface %s on path %s",
+			SAP_SERVER_INTERFACE, ss->path);
+
+	server_free(ss);
+}
+
+int sap_server_init(DBusConnection *conn)
+{
+	connection = dbus_connection_ref(conn);
+
+	return 0;
+}
+
+void sap_server_exit(void)
+{
+	dbus_connection_unref(connection);
+	connection = NULL;
+}
+
+static inline DBusMessage *invalid_arguments(DBusMessage *msg,
+					const char *description)
+{
+	return g_dbus_create_error(msg, ERROR_INTERFACE ".InvalidArguments",
+				description);
+}
+
+static DBusMessage *enable(DBusConnection *conn,
+			DBusMessage *msg, void *data)
+{
+	struct sap_server *ss = data;
+	sdp_record_t *record;
+	GIOChannel *io;
+	GError *gerr = NULL;
+	gboolean master = TRUE;
+	uint8_t chan = SAP_SERVER_CHANNEL;
+
+	if (ss->enable == TRUE)
+		return g_dbus_create_error(msg, ERROR_INTERFACE
+				".Failed",
+				"Server already enabled");
+
+	record = sap_record(SAP_SERVER_CHANNEL);
+	if (add_record_to_server(&ss->src, record) < 0) {
+		error("Failed to register service record");
+		sdp_record_free(record);
+		return g_dbus_create_error(msg, ERROR_INTERFACE
+				".Failed",
+				"Record registration failed");
+	}
+
+	io = bt_io_listen(BT_IO_RFCOMM, NULL, connect_confirm,
+			ss, NULL, &gerr,
+			BT_IO_OPT_SOURCE_BDADDR, &ss->src,
+			BT_IO_OPT_CHANNEL, chan,
+			BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_HIGH,
+			BT_IO_OPT_MASTER, master,
+			BT_IO_OPT_INVALID);
+
+	if (!io) {
+		error("bt_io_listen: %s", gerr->message);
+		g_error_free(gerr);
+		sdp_record_free(record);
+		return g_dbus_create_error(msg, ERROR_INTERFACE
+				".Failed",
+				"RFCOMM SAP channel listen failed");
+	}
+
+	debug("RFCOMM SAP listen socket fd %#x", g_io_channel_unix_get_fd(io));
+
+	ss->enable = TRUE;
+	ss->record_id = record->handle;
+
+	emit_property_changed(connection, ss->path, SAP_SERVER_INTERFACE,
+				"Enabled", DBUS_TYPE_BOOLEAN, &ss->enable);
+
+	return dbus_message_new_method_return(msg);
+}
+
+static DBusMessage *disable(DBusConnection *conn,
+				DBusMessage *msg, void *data)
+{
+	struct sap_server *ss = data;
+	DBusMessage *reply;
+
+	if (ss->enable == FALSE)
+		return g_dbus_create_error(msg, ERROR_INTERFACE
+				".Failed",
+				"Server already disabled");
+
+	if (ss->state != SAP_STATE_DISCONNECTED)
+		return g_dbus_create_error(msg, ERROR_INTERFACE
+				".Failed",
+				"There is ongoing connection");
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return NULL;
+
+	ss->enable = FALSE;
+	remove_record_from_server(ss->record_id);
+
+	if (ss->listen_io) {
+		debug("[%s::%s] Stopping RFCOMM SAP listen socket fd: %#x", __FILE__, __FUNCTION__, g_io_channel_unix_get_fd(ss->listen_io));
+		g_io_channel_shutdown(ss->listen_io, TRUE, NULL);
+		g_io_channel_unref(ss->listen_io);
+		ss->listen_io = NULL;
+	}
+
+	emit_property_changed(connection, ss->path, SAP_SERVER_INTERFACE,
+				"Enabled", DBUS_TYPE_BOOLEAN, &ss->enable);
+
+	return reply;
+}
+
+static DBusMessage *get_properties(DBusConnection *conn,
+				DBusMessage *msg, void *data)
+{
+	struct sap_server *ss = data;
+	DBusMessage *reply;
+	DBusMessageIter iter;
+	DBusMessageIter dict;
+	dbus_bool_t connected;
+
+	assert(ss && conn && msg);
+
+	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);
+
+	/* Enabled */
+	dict_append_entry(&dict, "Enabled", DBUS_TYPE_BOOLEAN, &ss->enable);
+
+	/* Connected */
+	connected = (ss->state == SAP_STATE_CONNECTED || ss->state == SAP_STATE_GRACEFUL_DISCONNECT);
+	dict_append_entry(&dict, "Connected", DBUS_TYPE_BOOLEAN, &connected);
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+static DBusMessage *set_property(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	DBusMessageIter iter;
+	DBusMessageIter sub;
+	const char *property;
+
+	if (!dbus_message_iter_init(msg, &iter))
+		return invalid_arguments(msg, "Not a dict");
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_STRING)
+		return invalid_arguments(msg, "Key not a string");
+
+	dbus_message_iter_get_basic(&iter, &property);
+	dbus_message_iter_next(&iter);
+
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_VARIANT)
+		return invalid_arguments(msg, "Value not a variant");
+	dbus_message_iter_recurse(&iter, &sub);
+
+	if (g_str_equal("Enabled", property)) {
+		gboolean enabled;
+
+		if (dbus_message_iter_get_arg_type(&sub) != DBUS_TYPE_BOOLEAN)
+			return invalid_arguments(msg, "Value not boolean");
+		dbus_message_iter_get_basic(&sub, &enabled);
+
+		return enabled ? enable(conn, msg, data) :
+				disable(conn, msg, data);
+	}
+
+	return invalid_arguments(msg, "Property does not exist");
+}
+
+
+static DBusMessage *disconnect(DBusConnection *conn, DBusMessage *msg, void *data)
+{
+	struct sap_server *ss = data;
+	dbus_bool_t disc_type;
+	const char *sender;
+	sap_disconnection_type_t sap_disc_type;
+
+	assert(ss);
+
+	if (!ss->enable)
+		return g_dbus_create_error(msg, ERROR_INTERFACE
+				".Failed",
+				"Server already disabled");
+
+	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_BOOLEAN, &disc_type, DBUS_TYPE_INVALID) == FALSE)
+		return NULL;
+
+	sap_disc_type = disc_type ? SAP_DISCONNECTION_TYPE_GRACEFUL : SAP_DISCONNECTION_TYPE_IMMEDIATE;
+	if (disconnect_req(ss, sap_disc_type) < 0)
+		return g_dbus_create_error(msg, ERROR_INTERFACE
+				".Failed",
+				"There is no active connection");
+
+	return dbus_message_new_method_return(msg);
+}
+
+static GDBusMethodTable server_methods[] = {
+	{ "SetProperty",	"sv",	"",	set_property },
+	{ "GetProperties",	"",	"a{sv}",get_properties },
+	{ "Disconnect",	"b",	"", disconnect },
+	{ }
+};
+
+static GDBusSignalTable server_signals[] = {
+	{ "PropertyChanged",		"sv"		},
+	{ }
+};
+
+int sap_server_register(const char *path, bdaddr_t *src)
+{
+	struct sap_server *ss;
+
+	ss = g_new0(struct sap_server, 1);
+	if (!g_dbus_register_interface(connection, path, SAP_SERVER_INTERFACE,
+				server_methods, server_signals, NULL,
+				ss, path_unregister)) {
+		error("D-Bus failed to register %s interface", SAP_SERVER_INTERFACE);
+		server_free(ss);
+		return -1;
+	}
+
+	ss->path = g_strdup(path);
+	bacpy(&ss->src, src);
+
+	return 0;
+}
+
+int sap_server_unregister(const char *path)
+{
+	g_dbus_unregister_interface(connection, path, SAP_SERVER_INTERFACE);
+
+	return 0;
+}
+
+int sap_connect_rsp(void *sap_device, sap_status_t status, uint16_t maxmsgsize)
+{
+	struct sap_server *ss = sap_device;
+	unsigned char buf[SAP_BUF_SIZE];
+	sap_message *msg = (sap_message *) buf;
+	sap_parameter *param = (sap_parameter *) msg->param;
+	gsize size = sizeof(sap_message);
+
+	assert(ss);
+
+	debug("[%s::%s] state: %#x processing_req: %#x status 0%x",
+				__FILE__, __FUNCTION__, ss->state, ss->processing_req, status);
+
+	if (ss->state != SAP_STATE_CONNECT_IN_PROGRESS)
+		goto failed;
+
+	memset(buf, 0, sizeof(buf));
+	msg->id = SAP_CONNECT_RESP;
+	msg->nparam = 0x01;
+
+	/* Adding connection status */
+	param->id = SAP_PARAM_ID_CONN_STATUS;
+	param->len = htons(SAP_PARAM_ID_CONN_STATUS_LEN);
+	*param->val = (uint8_t)status;
+	size += PARAMETER_SIZE(SAP_PARAM_ID_CONN_STATUS_LEN);
+
+	/* Adding MaxMsgSize */
+	if (maxmsgsize && (status == SAP_STATUS_MAX_MSG_SIZE_NOT_SUPPORTED ||
+			status == SAP_STATUS_MAX_MSG_SIZE_TOO_SMALL)) {
+		uint16_t *len;
+		msg->nparam++;
+		/* Skipping the first param */
+		param = (sap_parameter *) &buf[size];
+		param->id = SAP_PARAM_ID_MAXMSGSIZE;
+		param->len = htons(SAP_PARAM_ID_MAXMSGSIZE_LEN);
+		len = (uint16_t *) &param->val;
+		*len = htons(maxmsgsize);
+		size += PARAMETER_SIZE(SAP_PARAM_ID_MAXMSGSIZE_LEN);
+	}
+
+	if (status == SAP_STATUS_OK) {
+		gboolean connected = TRUE;
+		emit_property_changed(connection, ss->path, SAP_SERVER_INTERFACE,
+			"Connected", DBUS_TYPE_BOOLEAN, &connected);
+
+		ss->state = SAP_STATE_CONNECTED;
+	}
+	else {
+		ss->state = SAP_STATE_DISCONNECTED;
+
+		/* Start timer which will shutdown channel if client
+		 * doesn't send CONNECT_REQ or doesn't shutdown channel itself. */
+		start_guard_timer(ss, SAP_TIMER_NO_ACTIVITY);
+	}
+
+	ss->processing_req = SAP_NO_REQ;
+	return send_message(sap_device, buf, size);
+
+failed:
+	error("Unexpected response");
+	return -EPERM;
+}
+
+int sap_disconnect_rsp(void *sap_device)
+{
+	struct sap_server *ss = sap_device;
+	sap_message msg = {0};
+
+	assert(ss);
+	debug("sap_disconnect_rsp: sap_device %p", sap_device);
+	debug("[%s::%s] state: %#x processing_req: %#x",
+				__FILE__, __FUNCTION__, ss->state, ss->processing_req);
+
+	switch(ss->state) {
+	case SAP_STATE_CLIENT_DISCONNECT:
+		msg.id = SAP_DISCONNECT_RESP;
+
+		ss->state = SAP_STATE_DISCONNECTED;
+		ss->processing_req = SAP_NO_REQ;
+
+		/* Start timer which will close channel if client doesn't do it. */
+		start_guard_timer(ss, SAP_TIMER_NO_ACTIVITY);
+
+		return send_message(sap_device, (unsigned char *) &msg, sizeof(msg));
+
+	case SAP_STATE_IMMEDIATE_DISCONNECT:
+		ss->state = SAP_STATE_DISCONNECTED;
+		ss->processing_req = SAP_NO_REQ;
+
+		if (ss->io)
+			g_io_channel_shutdown(ss->io, TRUE, NULL);
+		return 0;
+	default:
+		break;
+	}
+
+failed:
+	error("Unexpected response");
+	return -EPERM;
+}
+
+int sap_transfer_apdu_rsp(void *sap_device, sap_result_t result, uint8_t *apdu, uint16_t length)
+{
+	struct sap_server *ss = sap_device;
+	unsigned char buf[SAP_BUF_SIZE] = {0};
+	sap_message *msg = (sap_message *) buf;
+	sap_parameter *param = (sap_parameter *) msg->param;
+	gsize size = sizeof(sap_message);
+
+	assert(ss);
+
+	debug("[%s::%s] state: %#x processing_req: %#x",
+				__FILE__, __FUNCTION__, ss->state, ss->processing_req);
+
+	if (ss->processing_req != SAP_TRANSFER_APDU_REQ)
+		/* Ignore this response because processing (state) request has been changed.
+		 * This situation can happen e.g. when client sends disconnect request
+		 * while server is processing another request.*/
+		return 0;
+
+	if (result == SAP_RESULT_OK && (!apdu || (apdu && length == 0x00)))
+		return -EINVAL;
+
+	msg->id = SAP_TRANSFER_APDU_RESP;
+
+	/* Adding result code */
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, param);
+
+	/* Adding APDU response. */
+	if (result == SAP_RESULT_OK) {
+		msg->nparam++;
+		/* Skipping the first param. */
+		param = (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);
+	}
+
+	ss->processing_req = SAP_NO_REQ;
+	return send_message(sap_device, buf, size);
+}
+
+int sap_transfer_atr_rsp(void *sap_device, sap_result_t result, uint8_t *atr, uint16_t length)
+{
+	struct sap_server *ss = sap_device;
+	unsigned char buf[SAP_BUF_SIZE]= {0};
+	sap_message *msg = (sap_message *) buf;
+	sap_parameter *param = (sap_parameter *) msg->param;
+	gsize size = sizeof(sap_message);
+
+	assert(ss);
+
+	debug("[%s::%s] state: %#x processing_req: %#x",
+				__FILE__, __FUNCTION__, ss->state, ss->processing_req);
+
+	if (ss->processing_req != SAP_TRANSFER_ATR_REQ)
+		/* Ignore this response because processing (state) request has been changed.
+		 * This situation can happen e.g. when client sends disconnect request
+		 * while server is processing another request.*/
+		return 0;
+
+	if (result == SAP_RESULT_OK && (!atr || (atr && length == 0x00)))
+		return -EINVAL;
+
+	msg->id = SAP_TRANSFER_ATR_RESP;
+
+	/* Adding result code */
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, param);
+
+	/* Adding ATR response */
+	if (result == SAP_RESULT_OK) {
+		msg->nparam++;
+		/* Skipping the first param */
+		param = (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);
+	}
+
+	ss->processing_req = SAP_NO_REQ;
+	return send_message(sap_device, buf, size);
+}
+
+int sap_power_sim_off_rsp(void *sap_device, sap_result_t result)
+{
+	struct sap_server *ss = sap_device;
+	unsigned char buf[SAP_BUF_SIZE] = {0};
+	sap_message *msg = (sap_message *) buf;
+	gsize size = sizeof(sap_message);
+
+	assert(ss);
+
+	debug("[%s::%s] state: %#x processing_req: %#x",
+				__FILE__, __FUNCTION__, ss->state, ss->processing_req);
+
+	if (ss->processing_req != SAP_POWER_SIM_OFF_REQ)
+		/* Ignore this response because processing (state) request has been changed.
+		 * This situation can happen e.g. when client sends disconnect request
+		 * while server is processing another request. */
+		return 0;
+
+	msg->id = SAP_POWER_SIM_OFF_RESP;
+
+	/* Adding result code */
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, msg->param);
+
+	ss->processing_req = SAP_NO_REQ;
+	return send_message(sap_device, buf, size);
+}
+
+int sap_power_sim_on_rsp(void *sap_device, sap_result_t result)
+{
+	struct sap_server *ss = sap_device;
+	unsigned char buf[SAP_BUF_SIZE] = {0};
+	sap_message *msg = (sap_message *) buf;
+	gsize size = sizeof(sap_message);
+
+	assert(ss);
+
+	debug("[%s::%s] state: %#x processing_req: %#x",
+				__FILE__, __FUNCTION__, ss->state, ss->processing_req);
+
+	if (ss->processing_req != SAP_POWER_SIM_ON_REQ)
+		/* Ignore this response because processing (state) request has been changed.
+		 * This situation can happen e.g. when client sends disconnect request
+		 * while server is processing another request. */
+		return 0;
+
+	msg->id = SAP_POWER_SIM_ON_RESP;
+
+	/* Adding result code */
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, msg->param);
+
+	ss->processing_req = SAP_NO_REQ;
+	return send_message(sap_device, buf, size);
+}
+
+int sap_reset_sim_rsp(void *sap_device, sap_result_t result)
+{
+	struct sap_server *ss = sap_device;
+	unsigned char buf[SAP_BUF_SIZE] = {0};
+	sap_message *msg = (sap_message *) buf;
+	gsize size = sizeof(sap_message);
+
+	assert(ss);
+
+	debug("[%s::%s] state: %#x processing_req: %#x",
+				__FILE__, __FUNCTION__, ss->state, ss->processing_req);
+
+	if (ss->processing_req != SAP_RESET_SIM_REQ)
+		/* Ignore this response because processing (state) request has been changed.
+		 * This situation can happen e.g. when client sends disconnect request
+		 * while server is processing another request. */
+		return 0;
+
+	msg->id = SAP_RESET_SIM_RESP;
+
+	/* Adding result code */
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, msg->param);
+
+	ss->processing_req = SAP_NO_REQ;
+	return send_message(sap_device, buf, size);
+}
+
+int sap_transfer_card_reader_status_rsp(void *sap_device, sap_result_t result, icc_reader_status_t status)
+{
+	struct sap_server *ss = sap_device;
+	unsigned char buf[SAP_BUF_SIZE] = {0};
+	sap_message *msg = (sap_message *) buf;
+	sap_parameter *param = (sap_parameter *) msg->param;
+	gsize size = sizeof(sap_message);
+
+	assert(ss);
+
+	debug("[%s::%s] state: %#x processing_req: %#x",
+				__FILE__, __FUNCTION__, ss->state, ss->processing_req);
+
+	if (ss->processing_req != SAP_TRANSFER_CARD_READER_STATUS_REQ)
+		/* Ignore this response because processing (state) request has been changed.
+		 * This situation can happen e.g. when client sends disconnect request
+		 * while server is processing another request. */
+		return 0;
+
+	msg->id = SAP_TRANSFER_CARD_READER_STATUS_RESP;
+
+	/* Adding result code */
+	msg->nparam = 0x01;
+	size += add_result_parameter(result, param);
+
+	/* Adding card reader status. */
+	if (result == SAP_RESULT_OK) {
+		msg->nparam++;
+		/* Skipping the first param. */
+		param = (sap_parameter *) &buf[size];
+		param->id = SAP_PARAM_ID_CARD_READER_STATUS;
+		param->len = htons(SAP_PARAM_ID_CARD_READER_STATUS_LEN);
+		*param->val = (uint8_t) status;
+		size += PARAMETER_SIZE(SAP_PARAM_ID_CARD_READER_STATUS_LEN);
+	}
+
+	ss->processing_req = SAP_NO_REQ;
+	return send_message(sap_device, buf, size);
+}
+
+int sap_error_rsp(void *sap_device)
+{
+	sap_message msg;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.id = SAP_ERROR_RESP;
+
+	return send_message(sap_device, (unsigned char *) &msg, sizeof(msg));
+}
+
+int sap_status_ind(void *sap_device, sap_status_change_t status_change)
+{
+	struct sap_server *ss = sap_device;
+	unsigned char buf[SAP_BUF_SIZE] = {0};
+	sap_message *msg = (sap_message *) buf;
+	sap_parameter *param = (sap_parameter *) msg->param;
+	gsize size = sizeof(sap_message);
+
+	assert(ss);
+
+	debug("[%s::%s] state: %#x processing_req: %#x",
+					__FILE__, __FUNCTION__, ss->state, ss->processing_req);
+
+	if (ss->state != SAP_STATE_CONNECTED && ss->state != SAP_STATE_GRACEFUL_DISCONNECT)
+		/* Ignore this indication when server is not connected. */
+		return 0;
+
+	msg->id = SAP_STATUS_IND;
+	msg->nparam = 0x01;
+
+	/* Adding status change. */
+	param->id  = SAP_PARAM_ID_STATUS_CHANGE;
+	param->len = htons(SAP_PARAM_ID_STATUS_CHANGE_LEN);
+	*param->val = (uint8_t) status_change;
+	size += PARAMETER_SIZE(SAP_PARAM_ID_STATUS_CHANGE_LEN);
+
+	return send_message(sap_device, buf, size);
+}
diff --git a/sap/server.h b/sap/server.h
new file mode 100644
index 0000000..4dca525
--- /dev/null
+++ b/sap/server.h
@@ -0,0 +1,25 @@
+/*
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 ST-Ericsson SA
+ *  Copyright (C) 2010 Claudio Takahasi<claudio.takahasi@openbossa.org>
+ *
+ *  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.
+ *
+ *  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_server_init(DBusConnection *conn);
+void sap_server_exit(void);
+int sap_server_register(const char *path, bdaddr_t *src);
+int sap_server_unregister(const char *path);
+
-- 
1.7.0.4

Re: Sim Access profile server implementation

[Suraj Sumangala]

Hi Waldek,

On 10/6/2010 3:02 PM, Waldemar.Rymarkiewicz@tieto.com wrote:
>
> Hi Marcel,
>
>>>
>>> Can you share that code with us. And also hardware if you. We
>> are still
>>> having hard time to find proper hardware to test this on.
>>
>> Will send a patch soon. I use
>> http://www.stericsson.com/platforms/U8500.jsp hw, but need to
>> check if I can share one with you.
>>
>
> Here you are the patch. Just note that it is based on an old bluez 4.4x
> If you consider to accept the design I will update the patch upon the latest release.
>
> Will appreciate any comments.
>
> Thanks,
> /Waldek

Which part of this is the driver and which is the agent here?

If I am not mistaken, you are sending the responses directly from code
here, how can we extend this to another hardware/agent without source
code change?

I don't see D-bus being used other than for set/get property.

Correct me if my understanding is wrong.

Regards
Suraj

RE: Sim Access profile server implementation

[Waldemar.Rymarkiewicz]

Hi suraj,=20

>Which part of this is the driver and which is the agent here?

sap-ste.c is a driver for u8500 platform. The agent is not involved in this=
 design.

>If I am not mistaken, you are sending the responses directly=20
>from code here, how can we extend this to another=20
>hardware/agent without source code change?

You are right. You can add a new driver that implements interfaces in sap.h=
 then configure with ./configure --enable-sap=3Dyes --with-sap=3DDRIVER
You don't have to modify the code, but you have to rebuild with your driver=
. Usually, there will be one driver per platform so it shoudl not be a prob=
lem.

>I don't see D-bus being used other than for set/get property.

We use D-bus only for enable/disable sap server. Data flow to sim is done o=
ver unix sockets (for u8500) as I have doubts to dbus reliability. However,=
 it's good option to do tests with dbus oriented design as discussed before=
.

>Correct me if my understanding is wrong.
>

Regards,
/Waldek=

Re: Sim Access profile server implementation

[Suraj Sumangala]

Hi Waldek,

On 10/6/2010 3:43 PM, Waldemar.Rymarkiewicz@tieto.com wrote:
> Hi suraj,
>
>> Which part of this is the driver and which is the agent here?
>
> sap-ste.c is a driver for u8500 platform. The agent is not involved in this design.
>
>> If I am not mistaken, you are sending the responses directly
>>from code here, how can we extend this to another
>> hardware/agent without source code change?
>
> You are right. You can add a new driver that implements interfaces in sap.h then configure with ./configure --enable-sap=yes --with-sap=DRIVER
> You don't have to modify the code, but you have to rebuild with your driver. Usually, there will be one driver per platform so it shoudl not be a problem.
>
>> I don't see D-bus being used other than for set/get property.
>
> We use D-bus only for enable/disable sap server. Data flow to sim is done over unix sockets (for u8500) as I have doubts to dbus reliability. However, it's good option to do tests with dbus oriented design as discussed before.

So basically your implementation does not use d-bus for basic SAP 
operations and the SIM access driver is compiled as part of bluetoothd 
right?

>
>> Correct me if my understanding is wrong.
>>
>
> Regards,
> /Waldek

Regards
Suraj

RE: Sim Access profile server implementation

[Waldemar.Rymarkiewicz]

Hi Suraj,
=20
>
>So basically your implementation does not use d-bus for basic=20
>SAP operations and the SIM access driver is compiled as part=20
>of bluetoothd right?
>
>>
>>> Correct me if my understanding is wrong.
>>>

Exactly.

Regards,
/Waldek

RE: Sim Access profile server implementation

[Marcel Holtmann]

Hi Waldemar,

> >>Can you share that code with us. And also hardware if you. We 
> >are still 
> >>having hard time to find proper hardware to test this on.
> >
> >Will send a patch soon. I use 
> >http://www.stericsson.com/platforms/U8500.jsp hw, but need to 
> >check if I can share one with you.
> >
> 
> Here you are the patch. Just note that it is based on an old bluez 4.4x
> If you consider to accept the design I will update the patch upon the latest release. 

in general I am fine if we do this similar to what we do with the
different telephony backend. So yes, go ahead with this.

However, please clean this up. The patch needs a lot of work before it
would be ready upstream. And of course it requires a dummy SAP plugin as
well. Same as we do have for telephony.

Regards

Marcel

RE: Sim Access profile server implementation

[Waldemar.Rymarkiewicz]

Hi Marcel,=20

>in general I am fine if we do this similar to what we do with=20
>the different telephony backend. So yes, go ahead with this.
>
>However, please clean this up. The patch needs a lot of work=20
>before it would be ready upstream. And of course it requires a=20
>dummy SAP plugin as well. Same as we do have for telephony.
>

Then, I will prepare it for upstream.=20

Regards,
/Waldek